Kernel as received from Digi for their Wi-Mx51 SoC running on their

CCWMX51JS carrier board.

638 files changed, 287148 insertions, 1275 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index 2f557f570ade..fbd497190e31 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -106,5 +106,7 @@ source "drivers/uio/Kconfig"
 
 source "drivers/xen/Kconfig"
 
+source "drivers/fims/Kconfig"
+
 source "drivers/staging/Kconfig"
 endmenu
diff --git a/drivers/Makefile b/drivers/Makefile
index fceb71a741c3..259f3d31cc9b 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -67,6 +67,7 @@ obj-$(CONFIG_INPUT)		+= input/
 obj-$(CONFIG_I2O)		+= message/
 obj-$(CONFIG_RTC_LIB)		+= rtc/
 obj-y				+= i2c/
+obj-$(CONFIG_I2C_SLAVE) += i2c-slave/
 obj-$(CONFIG_W1)		+= w1/
 obj-$(CONFIG_POWER_SUPPLY)	+= power/
 obj-$(CONFIG_HWMON)		+= hwmon/
@@ -84,6 +85,7 @@ obj-y				+= lguest/
 obj-$(CONFIG_CPU_FREQ)		+= cpufreq/
 obj-$(CONFIG_CPU_IDLE)		+= cpuidle/
 obj-y				+= idle/
+obj-$(CONFIG_ARCH_MXC)		+= mxc/
 obj-$(CONFIG_MMC)		+= mmc/
 obj-$(CONFIG_MEMSTICK)		+= memstick/
 obj-$(CONFIG_NEW_LEDS)		+= leds/
@@ -97,8 +99,10 @@ obj-$(CONFIG_DMA_ENGINE)	+= dma/
 obj-$(CONFIG_DCA)		+= dca/
 obj-$(CONFIG_HID)		+= hid/
 obj-$(CONFIG_PPC_PS3)		+= ps3/
+obj-$(CONFIG_GENERIC_PWM)	+= pwm/
 obj-$(CONFIG_OF)		+= of/
 obj-$(CONFIG_SSB)		+= ssb/
 obj-$(CONFIG_VIRTIO)		+= virtio/
+obj-$(CONFIG_FIM_CORE)		+= fims/
 obj-$(CONFIG_REGULATOR)		+= regulator/
 obj-$(CONFIG_STAGING)		+= staging/
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index 421b7c71e72d..f7f4c9d3e774 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -724,5 +724,14 @@ config PATA_BF54X
 
 	  If unsure, say N.
 
+config PATA_FSL
+       tristate "Freescale on-chip PATA support"
+       depends on (ARCH_MX51 || ARCH_MX37 || ARCH_MX35 || ARCH_MX3 || ARCH_MX27)
+       help
+         On Freescale processors, say Y here if you wish to use the on-chip
+         ATA interface.
+         If you are unsure, say N to this.
+
 endif # ATA_SFF
+
 endif # ATA
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index 674965fa326d..a2405a4bc7c4 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -72,6 +72,7 @@ obj-$(CONFIG_PATA_BF54X)	+= pata_bf54x.o
 obj-$(CONFIG_PATA_PLATFORM)	+= pata_platform.o
 obj-$(CONFIG_PATA_OF_PLATFORM)	+= pata_of_platform.o
 obj-$(CONFIG_PATA_ICSIDE)	+= pata_icside.o
+obj-$(CONFIG_PATA_FSL)		+= pata_fsl.o
 # Should be last but two libata driver
 obj-$(CONFIG_PATA_ACPI)		+= pata_acpi.o
 # Should be last but one libata driver
diff --git a/drivers/ata/pata_fsl.c b/drivers/ata/pata_fsl.c
new file mode 100644
index 000000000000..379fc2deb830
--- /dev/null
+++ b/drivers/ata/pata_fsl.c
@@ -0,0 +1,1042 @@
+/*
+ * Freescale integrated PATA driver
+ */
+
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/blkdev.h>
+#include <scsi/scsi_host.h>
+#include <linux/ata.h>
+#include <linux/libata.h>
+#include <linux/platform_device.h>
+#include <linux/fsl_devices.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+#include <asm/dma.h>
+
+#define DRV_NAME "pata_fsl"
+
+struct pata_fsl_priv {
+	int ultra;
+	u8 *fsl_ata_regs;
+	struct clk *clk;
+	int dma_rchan;
+	int dma_wchan;
+	int dma_done;
+	int dma_dir;
+	unsigned int adma_des_paddr;
+	unsigned int *adma_des_tp;
+};
+
+struct adma_bd {
+	unsigned char *sg_buf;
+	unsigned char *work_buf;
+	unsigned int dma_address;
+	int length;
+};
+
+struct adma_bulk {
+	struct adma_bd adma_bd_table[MXC_IDE_DMA_BD_NR];
+	struct ata_queued_cmd *qc;
+	int sg_ents;
+	int reserved[2];
+};
+
+enum {
+	/* various constants */
+	FSL_ATA_MAX_SG_LEN = ATA_DMA_BOUNDARY << 1,
+
+	/* offsets to registers */
+	FSL_ATA_TIMING_REGS = 0x00,
+	FSL_ATA_FIFO_FILL = 0x20,
+	FSL_ATA_CONTROL = 0x24,
+	FSL_ATA_INT_PEND = 0x28,
+	FSL_ATA_INT_EN = 0x2C,
+	FSL_ATA_INT_CLEAR = 0x30,
+	FSL_ATA_FIFO_ALARM = 0x34,
+	FSL_ATA_ADMA_ERROR_STATUS = 0x38,
+	FSL_ATA_SYS_DMA_BADDR = 0x3C,
+	FSL_ATA_ADMA_SYS_ADDR = 0x40,
+	FSL_ATA_BLOCK_COUNT = 0x48,
+	FSL_ATA_BURST_LENGTH = 0x4C,
+	FSL_ATA_SECTOR_SIZE = 0x50,
+	FSL_ATA_DRIVE_DATA = 0xA0,
+	FSL_ATA_DRIVE_CONTROL = 0xD8,
+
+	/* bits within FSL_ATA_CONTROL */
+	FSL_ATA_CTRL_DMA_SRST = 0x1000,
+	FSL_ATA_CTRL_DMA_64ADMA = 0x800,
+	FSL_ATA_CTRL_DMA_32ADMA = 0x400,
+	FSL_ATA_CTRL_DMA_STAT_STOP = 0x200,
+	FSL_ATA_CTRL_DMA_ENABLE = 0x100,
+	FSL_ATA_CTRL_FIFO_RST_B = 0x80,
+	FSL_ATA_CTRL_ATA_RST_B = 0x40,
+	FSL_ATA_CTRL_FIFO_TX_EN = 0x20,
+	FSL_ATA_CTRL_FIFO_RCV_EN = 0x10,
+	FSL_ATA_CTRL_DMA_PENDING = 0x08,
+	FSL_ATA_CTRL_DMA_ULTRA = 0x04,
+	FSL_ATA_CTRL_DMA_WRITE = 0x02,
+	FSL_ATA_CTRL_IORDY_EN = 0x01,
+
+	/* bits within the interrupt control registers */
+	FSL_ATA_INTR_ATA_INTRQ1 = 0x80,
+	FSL_ATA_INTR_FIFO_UNDERFLOW = 0x40,
+	FSL_ATA_INTR_FIFO_OVERFLOW = 0x20,
+	FSL_ATA_INTR_CTRL_IDLE = 0x10,
+	FSL_ATA_INTR_ATA_INTRQ2 = 0x08,
+	FSL_ATA_INTR_DMA_ERR = 0x04,
+	FSL_ATA_INTR_DMA_TRANS_OVER = 0x02,
+
+	/* ADMA Addr Descriptor Attribute Filed */
+	FSL_ADMA_DES_ATTR_VALID = 0x01,
+	FSL_ADMA_DES_ATTR_END = 0x02,
+	FSL_ADMA_DES_ATTR_INT = 0x04,
+	FSL_ADMA_DES_ATTR_SET = 0x10,
+	FSL_ADMA_DES_ATTR_TRAN = 0x20,
+	FSL_ADMA_DES_ATTR_LINK = 0x30,
+};
+
+/*
+ * This structure contains the timing parameters for
+ * ATA bus timing in the 5 PIO modes.  The timings
+ * are in nanoseconds, and are converted to clock
+ * cycles before being stored in the ATA controller
+ * timing registers.
+ */
+static struct {
+	short t0, t1, t2_8, t2_16, t2i, t4, t9, tA;
+} pio_specs[] = {
+	[0] = {
+	.t0 = 600, .t1 = 70, .t2_8 = 290, .t2_16 = 165, .t2i = 0, .t4 =
+		30, .t9 = 20, .tA = 50,},
+	[1] = {
+	.t0 = 383, .t1 = 50, .t2_8 = 290, .t2_16 = 125, .t2i = 0, .t4 =
+		20, .t9 = 15, .tA = 50,},
+	[2] = {
+	.t0 = 240, .t1 = 30, .t2_8 = 290, .t2_16 = 100, .t2i = 0, .t4 =
+		15, .t9 = 10, .tA = 50,},
+	[3] = {
+	.t0 = 180, .t1 = 30, .t2_8 = 80, .t2_16 = 80, .t2i = 0, .t4 =
+		10, .t9 = 10, .tA = 50,},
+	[4] = {
+	.t0 = 120, .t1 = 25, .t2_8 = 70, .t2_16 = 70, .t2i = 0, .t4 =
+		10, .t9 = 10, .tA = 50,},
+	};
+
+#define NR_PIO_SPECS (sizeof pio_specs / sizeof pio_specs[0])
+
+/*
+ * This structure contains the timing parameters for
+ * ATA bus timing in the 3 MDMA modes.  The timings
+ * are in nanoseconds, and are converted to clock
+ * cycles before being stored in the ATA controller
+ * timing registers.
+ */
+static struct {
+	short t0M, tD, tH, tJ, tKW, tM, tN, tJNH;
+} mdma_specs[] = {
+	[0] = {
+	.t0M = 480, .tD = 215, .tH = 20, .tJ = 20, .tKW = 215, .tM = 50, .tN =
+		15, .tJNH = 20,},
+	[1] = {
+	.t0M = 150, .tD = 80, .tH = 15, .tJ = 5, .tKW = 50, .tM = 30, .tN =
+		10, .tJNH = 15,},
+	[2] = {
+	.t0M = 120, .tD = 70, .tH = 10, .tJ = 5, .tKW = 25, .tM = 25, .tN =
+		10, .tJNH = 10,},
+	};
+
+#define NR_MDMA_SPECS (sizeof mdma_specs / sizeof mdma_specs[0])
+
+/*
+ * This structure contains the timing parameters for
+ * ATA bus timing in the 6 UDMA modes.  The timings
+ * are in nanoseconds, and are converted to clock
+ * cycles before being stored in the ATA controller
+ * timing registers.
+ */
+static struct {
+	short t2CYC, tCYC, tDS, tDH, tDVS, tDVH, tCVS, tCVH, tFS_min, tLI_max,
+	    tMLI, tAZ, tZAH, tENV_min, tSR, tRFS, tRP, tACK, tSS, tDZFS;
+} udma_specs[] = {
+	[0] = {
+	.t2CYC = 235, .tCYC = 114, .tDS = 15, .tDH = 5, .tDVS = 70, .tDVH =
+		6, .tCVS = 70, .tCVH = 6, .tFS_min = 0, .tLI_max =
+		100, .tMLI = 20, .tAZ = 10, .tZAH = 20, .tENV_min =
+		20, .tSR = 50, .tRFS = 75, .tRP = 160, .tACK = 20, .tSS =
+		50, .tDZFS = 80,},
+	[1] = {
+	.t2CYC = 156, .tCYC = 75, .tDS = 10, .tDH = 5, .tDVS = 48, .tDVH =
+		6, .tCVS = 48, .tCVH = 6, .tFS_min = 0, .tLI_max =
+		100, .tMLI = 20, .tAZ = 10, .tZAH = 20, .tENV_min =
+		20, .tSR = 30, .tRFS = 70, .tRP = 125, .tACK = 20, .tSS =
+		50, .tDZFS = 63,},
+	[2] = {
+	.t2CYC = 117, .tCYC = 55, .tDS = 7, .tDH = 5, .tDVS = 34, .tDVH =
+		6, .tCVS = 34, .tCVH = 6, .tFS_min = 0, .tLI_max =
+		100, .tMLI = 20, .tAZ = 10, .tZAH = 20, .tENV_min =
+		20, .tSR = 20, .tRFS = 60, .tRP = 100, .tACK = 20, .tSS =
+		50, .tDZFS = 47,},
+	[3] = {
+	.t2CYC = 86, .tCYC = 39, .tDS = 7, .tDH = 5, .tDVS = 20, .tDVH =
+		6, .tCVS = 20, .tCVH = 6, .tFS_min = 0, .tLI_max =
+		100, .tMLI = 20, .tAZ = 10, .tZAH = 20, .tENV_min =
+		20, .tSR = 20, .tRFS = 60, .tRP = 100, .tACK = 20, .tSS =
+		50, .tDZFS = 35,},
+	[4] = {
+	.t2CYC = 57, .tCYC = 25, .tDS = 5, .tDH = 5, .tDVS = 7, .tDVH =
+		6, .tCVS = 7, .tCVH = 6, .tFS_min = 0, .tLI_max =
+		100, .tMLI = 20, .tAZ = 10, .tZAH = 20, .tENV_min =
+		20, .tSR = 50, .tRFS = 60, .tRP = 100, .tACK = 20, .tSS =
+		50, .tDZFS = 25,},
+	[5] = {
+	.t2CYC = 38, .tCYC = 17, .tDS = 4, .tDH = 5, .tDVS = 5, .tDVH =
+		6, .tCVS = 10, .tCVH = 10, .tFS_min =
+		0, .tLI_max = 75, .tMLI = 20, .tAZ = 10, .tZAH =
+		20, .tENV_min = 20, .tSR = 20, .tRFS =
+		50, .tRP = 85, .tACK = 20, .tSS = 50, .tDZFS = 40,},
+};
+
+#define NR_UDMA_SPECS (sizeof udma_specs / sizeof udma_specs[0])
+
+struct fsl_ata_time_regs {
+	u8 time_off, time_on, time_1, time_2w;
+	u8 time_2r, time_ax, time_pio_rdx, time_4;
+	u8 time_9, time_m, time_jn, time_d;
+	u8 time_k, time_ack, time_env, time_rpx;
+	u8 time_zah, time_mlix, time_dvh, time_dzfs;
+	u8 time_dvs, time_cvh, time_ss, time_cyc;
+};
+
+static struct regulator *io_reg;
+static struct regulator *core_reg;
+static struct adma_bulk adma_info;
+
+static void
+update_timing_config(struct fsl_ata_time_regs *tp, struct ata_host *host)
+{
+	u32 *lp = (u32 *) tp;
+	struct pata_fsl_priv *priv = host->private_data;
+	u32 *ctlp = (u32 *) priv->fsl_ata_regs;
+	int i;
+
+	for (i = 0; i < 5; i++) {
+		__raw_writel(*lp, ctlp);
+		lp++;
+		ctlp++;
+	}
+}
+
+/*!
+ * Calculate values for the ATA bus timing registers and store
+ * them into the hardware.
+ *
+ * @param       xfer_mode   specifies XFER xfer_mode
+ * @param       pdev        specifies platform_device
+ *
+ * @return      EINVAL      speed out of range, or illegal mode
+ */
+static int set_ata_bus_timing(u8 xfer_mode, struct platform_device *pdev)
+{
+	struct ata_host *host = dev_get_drvdata(&pdev->dev);
+	struct pata_fsl_priv *priv = host->private_data;
+
+	/* get the bus clock cycle time, in ns */
+	int T = 1 * 1000 * 1000 * 1000 / clk_get_rate(priv->clk);
+	struct fsl_ata_time_regs tr = { 0 };
+
+	/*
+	 * every mode gets the same t_off and t_on
+	 */
+	tr.time_off = 3;
+	tr.time_on = 3;
+
+	if (xfer_mode >= XFER_UDMA_0) {
+		int speed = xfer_mode - XFER_UDMA_0;
+		if (speed >= NR_UDMA_SPECS)
+			return -EINVAL;
+
+		tr.time_ack = (udma_specs[speed].tACK + T) / T;
+		tr.time_env = (udma_specs[speed].tENV_min + T) / T;
+		tr.time_rpx = (udma_specs[speed].tRP + T) / T + 2;
+		tr.time_zah = (udma_specs[speed].tZAH + T) / T;
+		tr.time_mlix = (udma_specs[speed].tMLI + T) / T;
+		tr.time_dvh = (udma_specs[speed].tDVH + T) / T + 1;
+		tr.time_dzfs = (udma_specs[speed].tDZFS + T) / T;
+
+		tr.time_dvs = (udma_specs[speed].tDVS + T) / T;
+		tr.time_cvh = (udma_specs[speed].tCVH + T) / T;
+		tr.time_ss = (udma_specs[speed].tSS + T) / T;
+		tr.time_cyc = (udma_specs[speed].tCYC + T) / T;
+	} else if (xfer_mode >= XFER_MW_DMA_0) {
+		int speed = xfer_mode - XFER_MW_DMA_0;
+		if (speed >= NR_MDMA_SPECS)
+			return -EINVAL;
+
+		tr.time_m = (mdma_specs[speed].tM + T) / T;
+		tr.time_jn = (mdma_specs[speed].tJNH + T) / T;
+		tr.time_d = (mdma_specs[speed].tD + T) / T;
+
+		tr.time_k = (mdma_specs[speed].tKW + T) / T;
+	} else {
+		int speed = xfer_mode - XFER_PIO_0;
+		if (speed >= NR_PIO_SPECS)
+			return -EINVAL;
+
+		tr.time_1 = (pio_specs[speed].t1 + T) / T;
+		tr.time_2w = (pio_specs[speed].t2_8 + T) / T;
+
+		tr.time_2r = (pio_specs[speed].t2_8 + T) / T;
+		tr.time_ax = (pio_specs[speed].tA + T) / T + 2;
+		tr.time_pio_rdx = 1;
+		tr.time_4 = (pio_specs[speed].t4 + T) / T;
+
+		tr.time_9 = (pio_specs[speed].t9 + T) / T;
+	}
+
+	update_timing_config(&tr, host);
+
+	return 0;
+}
+
+static void pata_fsl_set_piomode(struct ata_port *ap, struct ata_device *adev)
+{
+	set_ata_bus_timing(adev->pio_mode, to_platform_device(ap->dev));
+}
+
+static void pata_fsl_set_dmamode(struct ata_port *ap, struct ata_device *adev)
+{
+	struct pata_fsl_priv *priv = ap->host->private_data;
+
+	priv->ultra = adev->dma_mode >= XFER_UDMA_0;
+
+	set_ata_bus_timing(adev->dma_mode, to_platform_device(ap->dev));
+}
+
+static int pata_fsl_port_start(struct ata_port *ap)
+{
+	return 0;
+}
+
+static void pata_adma_bulk_unmap(struct ata_queued_cmd *qc)
+{
+	int i;
+	struct adma_bd *bdp = adma_info.adma_bd_table;
+	if (adma_info.qc == NULL)
+		return;
+	BUG_ON(adma_info.qc != qc);
+
+	adma_info.qc = NULL;
+
+	for (i = 0; i < adma_info.sg_ents; i++) {
+		if (bdp->work_buf != bdp->sg_buf) {
+			if (qc->dma_dir == DMA_FROM_DEVICE) {
+				memcpy(bdp->sg_buf, bdp->work_buf, bdp->length);
+				dma_cache_maint(bdp->sg_buf, bdp->length,
+						DMA_FROM_DEVICE);
+			}
+			dma_free_coherent(qc->ap->dev, bdp->length,
+					  bdp->work_buf, bdp->dma_address);
+		}
+		bdp->work_buf = bdp->sg_buf = NULL;
+		bdp++;
+	}
+}
+
+static int pata_adma_bulk_map(struct ata_queued_cmd *qc)
+{
+	unsigned int si;
+	struct scatterlist *sg;
+	struct adma_bd *bdp = adma_info.adma_bd_table;
+
+	BUG_ON(adma_info.qc);
+
+	adma_info.qc = qc;
+	adma_info.sg_ents = 0;
+
+	for_each_sg(qc->sg, sg, qc->n_elem, si) {
+		/*
+		 * The ADMA mode is used setup the ADMA descriptor table
+		 */
+		bdp->sg_buf = sg_virt(sg);
+		bdp->length = sg->length;
+		if (sg->dma_address & 0xFFF) {
+			bdp->work_buf =
+			    dma_alloc_coherent(qc->ap->dev, bdp->length,
+					       &bdp->dma_address, GFP_KERNEL);
+			if (!bdp->work_buf) {
+				printk(KERN_WARNING
+				       "can not allocate aligned buffer\n");
+				goto fail;
+			}
+			if (qc->dma_dir == DMA_TO_DEVICE)
+				memcpy(bdp->work_buf, bdp->sg_buf, bdp->length);
+		} else {
+			bdp->work_buf = bdp->sg_buf;
+			bdp->dma_address = sg->dma_address;
+		}
+
+		adma_info.sg_ents++;
+		bdp++;
+	}
+	return 0;
+      fail:
+	pata_adma_bulk_unmap(qc);
+	return -1;
+}
+
+static void dma_callback(void *arg, int error_status, unsigned int count)
+{
+	struct ata_port *ap = arg;
+	struct pata_fsl_priv *priv = ap->host->private_data;
+	u8 *ata_regs = priv->fsl_ata_regs;
+
+	priv->dma_done = 1;
+	/*
+	 * DMA is finished, so unmask INTRQ from the drive to allow the
+	 * normal ISR to fire.
+	 */
+	__raw_writel(FSL_ATA_INTR_ATA_INTRQ2, ata_regs + FSL_ATA_INT_EN);
+}
+
+static irqreturn_t pata_fsl_adma_intr(int irq, void *dev_instance)
+{
+	struct ata_host *host = dev_instance;
+	struct pata_fsl_priv *priv = host->private_data;
+	u8 *ata_regs = priv->fsl_ata_regs;
+	unsigned int handled = 0;
+	unsigned int i;
+	unsigned long flags;
+	unsigned int pending = __raw_readl(ata_regs + FSL_ATA_INT_PEND);
+
+	if (FSL_ATA_INTR_DMA_TRANS_OVER & pending) {
+		priv->dma_done = 1;
+		__raw_writel(pending, ata_regs + FSL_ATA_INT_CLEAR);
+		handled = 1;
+	} else if (FSL_ATA_INTR_DMA_ERR & pending) {
+		printk(KERN_ERR "dma err status 0x%x ...\n",
+		       __raw_readl(ata_regs + FSL_ATA_ADMA_ERROR_STATUS));
+		__raw_writel(pending, ata_regs + FSL_ATA_INT_CLEAR);
+		handled = 1;
+		i = __raw_readl(ata_regs + FSL_ATA_CONTROL) && 0xFF;
+		i |= FSL_ATA_CTRL_DMA_SRST | FSL_ATA_CTRL_DMA_32ADMA |
+		    FSL_ATA_CTRL_DMA_ENABLE;
+		__raw_writel(i, ata_regs + FSL_ATA_CONTROL);
+	}
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	for (i = 0; i < host->n_ports; i++) {
+		struct ata_port *ap;
+
+		ap = host->ports[i];
+		if (ap && !(ap->flags & ATA_FLAG_DISABLED)) {
+			struct ata_queued_cmd *qc;
+
+			qc = ata_qc_from_tag(ap, ap->link.active_tag);
+			raw_local_irq_restore(flags);
+			pata_adma_bulk_unmap(qc);
+			raw_local_irq_save(flags);
+			if (qc && (!(qc->tf.flags & ATA_TFLAG_POLLING)) &&
+			    (qc->flags & ATA_QCFLAG_ACTIVE))
+				handled |= ata_sff_host_intr(ap, qc);
+		}
+	}
+
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	return IRQ_RETVAL(handled);
+}
+
+static int pata_fsl_check_atapi_dma(struct ata_queued_cmd *qc)
+{
+	return 1;		/* ATAPI DMA not yet supported */
+}
+
+unsigned long pata_fsl_bmdma_mode_filter(struct ata_device *adev,
+					unsigned long xfer_mask)
+{
+	/* Capability of the controller has been specified in the
+	 * platform data. Do not filter any modes, just return
+	 * the xfer_mask */
+	return xfer_mask;
+}
+
+static void pata_fsl_bmdma_setup(struct ata_queued_cmd *qc)
+{
+	int chan, i;
+	int dma_mode = 0, dma_ultra;
+	u32 ata_control;
+	struct ata_port *ap = qc->ap;
+	struct pata_fsl_priv *priv = ap->host->private_data;
+	u8 *ata_regs = priv->fsl_ata_regs;
+	struct fsl_ata_platform_data *plat = ap->dev->platform_data;
+	int err;
+	unsigned int si;
+
+	priv->dma_dir = qc->dma_dir;
+
+	/*
+	 * Configure the on-chip ATA interface hardware.
+	 */
+	dma_ultra = priv->ultra ? FSL_ATA_CTRL_DMA_ULTRA : 0;
+
+	ata_control = FSL_ATA_CTRL_FIFO_RST_B |
+	    FSL_ATA_CTRL_ATA_RST_B | FSL_ATA_CTRL_DMA_PENDING | dma_ultra;
+	if (plat->adma_flag)
+		ata_control |= FSL_ATA_CTRL_DMA_32ADMA |
+		    FSL_ATA_CTRL_DMA_ENABLE;
+
+	if (qc->dma_dir == DMA_TO_DEVICE) {
+		chan = priv->dma_wchan;
+		ata_control |= FSL_ATA_CTRL_FIFO_TX_EN | FSL_ATA_CTRL_DMA_WRITE;
+		dma_mode = DMA_MODE_WRITE;
+	} else {
+		chan = priv->dma_rchan;
+		ata_control |= FSL_ATA_CTRL_FIFO_RCV_EN;
+		dma_mode = DMA_MODE_READ;
+	}
+
+	__raw_writel(ata_control, ata_regs + FSL_ATA_CONTROL);
+	__raw_writel(plat->fifo_alarm, ata_regs + FSL_ATA_FIFO_ALARM);
+
+	if (plat->adma_flag) {
+		i = FSL_ATA_INTR_DMA_TRANS_OVER | FSL_ATA_INTR_DMA_ERR;
+		__raw_writel(FSL_ATA_INTR_ATA_INTRQ2 | i,
+			     ata_regs + FSL_ATA_INT_EN);
+	} else {
+		__raw_writel(FSL_ATA_INTR_ATA_INTRQ1,
+			     ata_regs + FSL_ATA_INT_EN);
+		/*
+		 * Set up the DMA completion callback.
+		 */
+		mxc_dma_callback_set(chan, dma_callback, (void *)ap);
+	}
+
+	/*
+	 * Copy the sg list to an array.
+	 */
+	if (plat->adma_flag) {
+		struct adma_bd *bdp = adma_info.adma_bd_table;
+		pata_adma_bulk_map(qc);
+		for (i = 0; i < adma_info.sg_ents; i++) {
+			priv->adma_des_tp[i << 1] = bdp->length << 12;
+			priv->adma_des_tp[i << 1] |= FSL_ADMA_DES_ATTR_SET;
+			priv->adma_des_tp[i << 1] |= FSL_ADMA_DES_ATTR_VALID;
+			priv->adma_des_tp[(i << 1) + 1] = bdp->dma_address;
+			priv->adma_des_tp[(i << 1) + 1] |=
+			    FSL_ADMA_DES_ATTR_TRAN;
+			priv->adma_des_tp[(i << 1) + 1] |=
+			    FSL_ADMA_DES_ATTR_VALID;
+			if (adma_info.sg_ents == (i + 1))
+				priv->adma_des_tp[(i << 1) + 1] |=
+				    FSL_ADMA_DES_ATTR_END;
+			bdp++;
+		}
+		__raw_writel((qc->nbytes / qc->sect_size), ata_regs +
+			     FSL_ATA_BLOCK_COUNT);
+		__raw_writel(plat->fifo_alarm, ata_regs + FSL_ATA_BURST_LENGTH);
+		__raw_writel(priv->adma_des_paddr,
+			     ata_regs + FSL_ATA_ADMA_SYS_ADDR);
+	} else {
+		int nr_sg = 0;
+		struct scatterlist tmp[MXC_IDE_DMA_BD_NR], *tsg, *sg;
+		tsg = tmp;
+		for_each_sg(qc->sg, sg, qc->n_elem, si) {
+			memcpy(tsg, sg, sizeof(*sg));
+			tsg++;
+			nr_sg++;
+		}
+		err = mxc_dma_sg_config(chan, tmp, nr_sg, 0, dma_mode);
+		if (err)
+			printk(KERN_ERR "pata_fsl_bmdma_setup: error %d\n",
+			       err);
+	}
+}
+
+static void pata_fsl_bmdma_start(struct ata_queued_cmd *qc)
+{
+	struct ata_port *ap = qc->ap;
+	struct pata_fsl_priv *priv = ap->host->private_data;
+	u8 *ata_regs = priv->fsl_ata_regs;
+	struct fsl_ata_platform_data *plat = ap->dev->platform_data;
+	int chan;
+	int err;
+	unsigned i;
+
+	if (1 == plat->adma_flag) {
+		i = FSL_ATA_CTRL_DMA_32ADMA | FSL_ATA_CTRL_DMA_ENABLE;
+		/* The adma mode is used, set dma_start_stop to 1 */
+		__raw_writel(i | __raw_readl(ata_regs + FSL_ATA_CONTROL) |
+			     FSL_ATA_CTRL_DMA_STAT_STOP,
+			     ata_regs + FSL_ATA_CONTROL);
+	} else {
+		/*
+		 * Start the channel.
+		 */
+		chan = qc->dma_dir == DMA_TO_DEVICE ? priv->dma_wchan :
+		    priv->dma_rchan;
+
+		err = mxc_dma_enable(chan);
+		if (err)
+			printk(KERN_ERR "%s: : error %d\n", __func__, err);
+	}
+
+	priv->dma_done = 0;
+
+	ata_sff_exec_command(ap, &qc->tf);
+}
+
+static void pata_fsl_bmdma_stop(struct ata_queued_cmd *qc)
+{
+	struct ata_port *ap = qc->ap;
+	struct pata_fsl_priv *priv = ap->host->private_data;
+	u8 *ata_regs = priv->fsl_ata_regs;
+	struct fsl_ata_platform_data *plat = ap->dev->platform_data;
+	unsigned i;
+
+	if (plat->adma_flag) {
+		/* The adma mode is used, set dma_start_stop to 0 */
+		i = FSL_ATA_CTRL_DMA_32ADMA | FSL_ATA_CTRL_DMA_ENABLE;
+		__raw_writel((i | __raw_readl(ata_regs + FSL_ATA_CONTROL)) &
+			     (~FSL_ATA_CTRL_DMA_STAT_STOP),
+			     ata_regs + FSL_ATA_CONTROL);
+	}
+
+	/* do a dummy read as in ata_bmdma_stop */
+#if 0
+	ata_sff_dma_pause(ap);
+#endif
+}
+
+static u8 pata_fsl_bmdma_status(struct ata_port *ap)
+{
+	struct pata_fsl_priv *priv = ap->host->private_data;
+
+	return priv->dma_done ? ATA_DMA_INTR : 0;
+}
+
+static void pata_fsl_dma_init(struct ata_port *ap)
+{
+	struct pata_fsl_priv *priv = ap->host->private_data;
+
+	priv->dma_rchan = -1;
+	priv->dma_wchan = -1;
+
+	priv->dma_rchan = mxc_dma_request(MXC_DMA_ATA_RX, "MXC ATA RX");
+	if (priv->dma_rchan < 0) {
+		dev_printk(KERN_ERR, ap->dev, "couldn't get RX DMA channel\n");
+		goto err_out;
+	}
+
+	priv->dma_wchan = mxc_dma_request(MXC_DMA_ATA_TX, "MXC ATA TX");
+	if (priv->dma_wchan < 0) {
+		dev_printk(KERN_ERR, ap->dev, "couldn't get TX DMA channel\n");
+		goto err_out;
+	}
+
+	dev_printk(KERN_ERR, ap->dev, "rchan=%d wchan=%d\n", priv->dma_rchan,
+		   priv->dma_wchan);
+	return;
+
+      err_out:
+	ap->mwdma_mask = 0;
+	ap->udma_mask = 0;
+	mxc_dma_free(priv->dma_rchan);
+	mxc_dma_free(priv->dma_wchan);
+	kfree(priv);
+}
+
+#if 0
+static u8 pata_fsl_irq_ack(struct ata_port *ap, unsigned int chk_drq)
+{
+	unsigned int bits = chk_drq ? ATA_BUSY | ATA_DRQ : ATA_BUSY;
+	u8 status;
+
+	status = ata_sff_busy_wait(ap, bits, 1000);
+	if (status & bits)
+		if (ata_msg_err(ap))
+			printk(KERN_ERR "abnormal status 0x%X\n", status);
+
+	return status;
+}
+#endif
+
+static void ata_dummy_noret(struct ata_port *ap)
+{
+	return;
+}
+
+static struct scsi_host_template pata_fsl_sht = {
+	.module = THIS_MODULE,
+	.name = DRV_NAME,
+	.ioctl = ata_scsi_ioctl,
+	.queuecommand = ata_scsi_queuecmd,
+	.can_queue = ATA_DEF_QUEUE,
+	.this_id = ATA_SHT_THIS_ID,
+	.sg_tablesize = LIBATA_MAX_PRD,
+	.cmd_per_lun = ATA_SHT_CMD_PER_LUN,
+	.emulated = ATA_SHT_EMULATED,
+	.use_clustering = ATA_SHT_USE_CLUSTERING,
+	.proc_name = DRV_NAME,
+	.dma_boundary = FSL_ATA_MAX_SG_LEN,
+	.slave_configure = ata_scsi_slave_config,
+	.slave_destroy = ata_scsi_slave_destroy,
+	.bios_param = ata_std_bios_param,
+};
+
+static struct ata_port_operations pata_fsl_port_ops = {
+	.inherits = &ata_bmdma_port_ops,
+	.set_piomode = pata_fsl_set_piomode,
+	.set_dmamode = pata_fsl_set_dmamode,
+
+	.check_atapi_dma = pata_fsl_check_atapi_dma,
+	.cable_detect = ata_cable_unknown,
+	.mode_filter = pata_fsl_bmdma_mode_filter,
+
+	.bmdma_setup = pata_fsl_bmdma_setup,
+	.bmdma_start = pata_fsl_bmdma_start,
+	.bmdma_stop = pata_fsl_bmdma_stop,
+	.bmdma_status = pata_fsl_bmdma_status,
+
+	.qc_prep = ata_noop_qc_prep,
+
+	.sff_data_xfer = ata_sff_data_xfer_noirq,
+	.sff_irq_clear = ata_dummy_noret,
+	.sff_irq_on = ata_sff_irq_on,
+
+	.port_start = pata_fsl_port_start,
+};
+
+static void fsl_setup_port(struct ata_ioports *ioaddr)
+{
+	unsigned int shift = 2;
+
+	ioaddr->data_addr = ioaddr->cmd_addr + (ATA_REG_DATA << shift);
+	ioaddr->error_addr = ioaddr->cmd_addr + (ATA_REG_ERR << shift);
+	ioaddr->feature_addr = ioaddr->cmd_addr + (ATA_REG_FEATURE << shift);
+	ioaddr->nsect_addr = ioaddr->cmd_addr + (ATA_REG_NSECT << shift);
+	ioaddr->lbal_addr = ioaddr->cmd_addr + (ATA_REG_LBAL << shift);
+	ioaddr->lbam_addr = ioaddr->cmd_addr + (ATA_REG_LBAM << shift);
+	ioaddr->lbah_addr = ioaddr->cmd_addr + (ATA_REG_LBAH << shift);
+	ioaddr->device_addr = ioaddr->cmd_addr + (ATA_REG_DEVICE << shift);
+	ioaddr->status_addr = ioaddr->cmd_addr + (ATA_REG_STATUS << shift);
+	ioaddr->command_addr = ioaddr->cmd_addr + (ATA_REG_CMD << shift);
+}
+
+/**
+ *     pata_fsl_probe          -       attach a platform interface
+ *     @pdev: platform device
+ *
+ *     Register a platform bus integrated ATA host controller
+ *
+ *     The 3 platform device resources are used as follows:
+ *
+ *             - I/O Base (IORESOURCE_MEM) virt. addr. of ATA controller regs
+ *             - CTL Base (IORESOURCE_MEM) unused
+ *             - IRQ      (IORESOURCE_IRQ) platform IRQ assigned to ATA
+ *
+ */
+static int __devinit pata_fsl_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct resource *io_res;
+	struct ata_host *host;
+	struct ata_port *ap;
+	struct fsl_ata_platform_data *plat = (struct fsl_ata_platform_data *)
+	    pdev->dev.platform_data;
+	struct pata_fsl_priv *priv;
+	u8 *ata_regs;
+	unsigned int int_enable;
+
+	/*
+	 * Set up resources
+	 */
+	if (unlikely(pdev->num_resources != 3)) {
+		dev_err(&pdev->dev, "invalid number of resources\n");
+		return -EINVAL;
+	}
+	/*
+	 * Get an ata_host structure for this device
+	 */
+	host = ata_host_alloc(&pdev->dev, 1);
+	if (!host)
+		return -ENOMEM;
+	ap = host->ports[0];
+
+	/*
+	 * Allocate private data
+	 */
+	priv = kzalloc(sizeof(struct pata_fsl_priv), GFP_KERNEL);
+	if (priv == NULL) {
+		ret = -ENOMEM;
+		goto err0;
+	}
+	host->private_data = priv;
+
+	/*
+	 * Set up resources
+	 */
+	io_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	ata_regs =
+	    (u8 *) ioremap(io_res->start, io_res->end - io_res->start + 1);
+	priv->fsl_ata_regs = ata_regs;
+	ap->ioaddr.cmd_addr = (void *)(ata_regs + FSL_ATA_DRIVE_DATA);
+	ap->ioaddr.ctl_addr = (void *)(ata_regs + FSL_ATA_DRIVE_CONTROL);
+	ap->ioaddr.altstatus_addr = ap->ioaddr.ctl_addr;
+	ap->ops = &pata_fsl_port_ops;
+	ap->pio_mask = plat->pio_mask;	/* support pio 0~4 */
+	ap->mwdma_mask = plat->mwdma_mask;	/* support mdma 0~2 */
+	ap->udma_mask = plat->udma_mask;
+	pata_fsl_sht.sg_tablesize = plat->max_sg;
+	fsl_setup_port(&ap->ioaddr);
+
+	if (plat->adma_flag) {
+		priv->adma_des_tp =
+		    dma_alloc_coherent(&(pdev->dev),
+				       (2 * MXC_IDE_DMA_BD_NR) *
+				       sizeof(unsigned int),
+				       &(priv->adma_des_paddr), GFP_DMA);
+		if (priv->adma_des_tp == NULL) {
+			ret = -ENOMEM;
+			goto err1;
+		}
+	}
+	/*
+	 * Do platform-specific initialization (e.g. allocate pins,
+	 * turn on clock).  After this call it is assumed that
+	 * plat->get_clk_rate() can be called to calculate
+	 * timing.
+	 */
+	if (plat->init && plat->init(pdev)) {
+		ret = -ENODEV;
+		goto err2;
+	}
+
+	priv->clk = clk_get(&pdev->dev, "ata_clk");
+	clk_enable(priv->clk);
+
+	/* Deassert the reset bit to enable the interface */
+	__raw_writel(FSL_ATA_CTRL_ATA_RST_B, ata_regs + FSL_ATA_CONTROL);
+
+	/* Enable Core regulator & IO Regulator */
+	if (plat->core_reg != NULL) {
+		core_reg = regulator_get(&pdev->dev, plat->core_reg);
+		if (regulator_enable(core_reg))
+			printk(KERN_INFO "enable core regulator error.\n");
+		msleep(100);
+
+	} else
+		core_reg = NULL;
+
+	if (plat->io_reg != NULL) {
+		io_reg = regulator_get(&pdev->dev, plat->io_reg);
+		if (regulator_enable(io_reg))
+			printk(KERN_INFO "enable io regulator error.\n");
+		msleep(100);
+
+	} else
+		io_reg = NULL;
+
+	/* Set initial timing and mode */
+	set_ata_bus_timing(XFER_PIO_4, pdev);
+
+	/* get DMA ready */
+	if (plat->adma_flag == 0)
+		pata_fsl_dma_init(ap);
+
+	/*
+	 * Enable the ATA INTRQ interrupt from the bus, but
+	 * only allow the CPU to see it (INTRQ2) at this point.
+	 * INTRQ1, which goes to the DMA, will be enabled later.
+	 */
+	int_enable = FSL_ATA_INTR_DMA_TRANS_OVER | FSL_ATA_INTR_DMA_ERR |
+	    FSL_ATA_INTR_ATA_INTRQ2;
+	if (plat->adma_flag)
+		__raw_writel(int_enable, ata_regs + FSL_ATA_INT_EN);
+	else
+		__raw_writel(FSL_ATA_INTR_ATA_INTRQ2,
+			     ata_regs + FSL_ATA_INT_EN);
+
+	/* activate */
+	if (plat->adma_flag)
+		ret = ata_host_activate(host, platform_get_irq(pdev, 0),
+					pata_fsl_adma_intr, 0, &pata_fsl_sht);
+	else
+		ret = ata_host_activate(host, platform_get_irq(pdev, 0),
+					ata_sff_interrupt, 0, &pata_fsl_sht);
+
+	if (!ret)
+		return ret;
+
+	clk_disable(priv->clk);
+	regulator_disable(core_reg);
+	regulator_disable(io_reg);
+      err2:
+	if (plat->adma_flag && priv->adma_des_tp)
+		dma_free_coherent(&(pdev->dev),
+				  (2 * MXC_IDE_DMA_BD_NR +
+				   1) * sizeof(unsigned int), priv->adma_des_tp,
+				  priv->adma_des_paddr);
+      err1:
+	iounmap(ata_regs);
+	kfree(priv);
+      err0:
+	ata_host_detach(host);
+	return ret;
+
+}
+
+/**
+ *     pata_fsl_remove -       unplug a platform interface
+ *     @pdev: platform device
+ *
+ *     A platform bus ATA device has been unplugged. Perform the needed
+ *     cleanup. Also called on module unload for any active devices.
+ */
+static int __devexit pata_fsl_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct ata_host *host = dev_get_drvdata(dev);
+	struct pata_fsl_priv *priv = host->private_data;
+	struct fsl_ata_platform_data *plat = (struct fsl_ata_platform_data *)
+	    pdev->dev.platform_data;
+	u8 *ata_regs = priv->fsl_ata_regs;
+
+	__raw_writel(0, ata_regs + FSL_ATA_INT_EN);	/* Disable interrupts */
+
+	ata_host_detach(host);
+
+	clk_disable(priv->clk);
+	clk_put(priv->clk);
+	priv->clk = NULL;
+
+	/* Disable Core regulator & IO Regulator */
+	if (plat->core_reg != NULL) {
+		regulator_disable(core_reg);
+		regulator_put(core_reg);
+	}
+	if (plat->io_reg != NULL) {
+		regulator_disable(io_reg);
+		regulator_put(io_reg);
+	}
+
+	if (plat->exit)
+		plat->exit();
+
+	if (plat->adma_flag && priv->adma_des_tp)
+		dma_free_coherent(&(pdev->dev),
+				  (2 * MXC_IDE_DMA_BD_NR) *
+				  sizeof(unsigned int), priv->adma_des_tp,
+				  priv->adma_des_paddr);
+	iounmap(ata_regs);
+
+	kfree(priv);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int pata_fsl_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct ata_host *host = dev_get_drvdata(&pdev->dev);
+	struct pata_fsl_priv *priv = host->private_data;
+	struct fsl_ata_platform_data *plat = (struct fsl_ata_platform_data *)
+	    pdev->dev.platform_data;
+	u8 *ata_regs = priv->fsl_ata_regs;
+
+	ata_host_suspend(host, state);
+
+	/* Disable interrupts. */
+	__raw_writel(0, ata_regs + FSL_ATA_INT_EN);
+
+	clk_disable(priv->clk);
+
+	if (plat->exit)
+		plat->exit();
+
+	return 0;
+}
+
+static int pata_fsl_resume(struct platform_device *pdev)
+{
+	struct ata_host *host = dev_get_drvdata(&pdev->dev);
+	struct pata_fsl_priv *priv = host->private_data;
+	struct fsl_ata_platform_data *plat = (struct fsl_ata_platform_data *)
+	    pdev->dev.platform_data;
+	u8 *ata_regs = priv->fsl_ata_regs;
+	unsigned char int_enable;
+
+	if (plat->init && plat->init(pdev))
+		return -ENODEV;
+
+	clk_enable(priv->clk);
+
+	/* Deassert the reset bit to enable the interface */
+	__raw_writel(FSL_ATA_CTRL_ATA_RST_B, ata_regs + FSL_ATA_CONTROL);
+
+	/* Set initial timing and mode */
+	set_ata_bus_timing(XFER_PIO_4, pdev);
+
+	/*
+	 * Enable hardware interrupts.
+	 */
+	int_enable = FSL_ATA_INTR_DMA_TRANS_OVER | FSL_ATA_INTR_DMA_ERR |
+	    FSL_ATA_INTR_ATA_INTRQ2;
+	if (1 == plat->adma_flag)
+		__raw_writel(int_enable, ata_regs + FSL_ATA_INT_EN);
+	else
+		__raw_writel(FSL_ATA_INTR_ATA_INTRQ2,
+			     ata_regs + FSL_ATA_INT_EN);
+
+	ata_host_resume(host);
+
+	return 0;
+}
+#endif
+
+static struct platform_driver pata_fsl_driver = {
+	.probe = pata_fsl_probe,
+	.remove = __devexit_p(pata_fsl_remove),
+#ifdef CONFIG_PM
+	.suspend = pata_fsl_suspend,
+	.resume = pata_fsl_resume,
+#endif
+	.driver = {
+		   .name = DRV_NAME,
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static int __init pata_fsl_init(void)
+{
+	return platform_driver_register(&pata_fsl_driver);
+
+	return 0;
+}
+
+static void __exit pata_fsl_exit(void)
+{
+	platform_driver_unregister(&pata_fsl_driver);
+}
+
+module_init(pata_fsl_init);
+module_exit(pata_fsl_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("low-level driver for Freescale ATA");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index 43d6ba83a191..21beac3c04aa 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -418,6 +418,29 @@ config SGI_MBCS
          If you have an SGI Altix with an attached SABrick
          say Y or M here, otherwise say N.
 
+config FM_SI4702
+	tristate "SI4702 FM device driver"
+	depends on (MACH_MX31_3DS || MACH_MX35_3DS || MACH_MX37_3DS || MACH_MX51_3DS)
+	default n
+
+config MXC_IIM
+	tristate "MXC IIM device driver"
+	depends on ARCH_MXC
+	help
+	  Support for access to MXC IIM device, most people should say N here.
+
+config MXS_VIIM
+	tristate "MXS Virtual IIM device driver"
+	depends on ARCH_STMP3XXX
+	help
+	  Support for access to MXS Virtual IIM device, most people should say N here.
+
+config IMX_SIM
+	tristate "IMX SIM support"
+	depends on (ARCH_MX51 || MACH_MX25_3DS)
+	---help---
+         Say Y to enable the SIM driver support.
+
 source "drivers/serial/Kconfig"
 
 config UNIX98_PTYS
@@ -1073,5 +1096,25 @@ config DEVPORT
 
 source "drivers/s390/char/Kconfig"
 
+config ADC_NS9215
+	tristate "Digi ns9215 On-Chip ADC"
+	depends on PROCESSOR_NS9215
+	help
+	  Driver for the internal ADC (Analog Digital Converter) module
+	  found on Digi ns9215 CPUs.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called adc-ns9215.
+
+config ADC_S3C2443
+	tristate "ADC driver for S3C2443 SoC, internal ADC"
+	depends on CPU_S3C2443
+	help
+	  Driver for the internal ADC (Analog Digital Converter) module
+	  found on S3C2443 CPU.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called adc-s3c2443.
+
 endmenu
 
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index 438f71317c5c..ce2067b792b0 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -9,6 +9,11 @@ FONTMAPFILE = cp437.uni
 
 obj-y	 += mem.o random.o tty_io.o n_tty.o tty_ioctl.o tty_ldisc.o tty_buffer.o tty_port.o
 
+obj-$(CONFIG_FM_SI4702)		+= mxc_si4702.o
+obj-$(CONFIG_MXC_IIM)		+= mxc_iim.o
+obj-$(CONFIG_MXS_VIIM)		+= mxs_viim.o
+obj-$(CONFIG_IMX_SIM) 		+= imx_sim.o
+
 obj-$(CONFIG_LEGACY_PTYS)	+= pty.o
 obj-$(CONFIG_UNIX98_PTYS)	+= pty.o
 obj-y				+= misc.o
@@ -95,7 +100,6 @@ obj-$(CONFIG_CS5535_GPIO)	+= cs5535_gpio.o
 obj-$(CONFIG_GPIO_VR41XX)	+= vr41xx_giu.o
 obj-$(CONFIG_GPIO_TB0219)	+= tb0219.o
 obj-$(CONFIG_TELCLOCK)		+= tlclk.o
-
 obj-$(CONFIG_MWAVE)		+= mwave/
 obj-$(CONFIG_AGP)		+= agp/
 obj-$(CONFIG_PCMCIA)		+= pcmcia/
@@ -109,6 +113,9 @@ obj-$(CONFIG_PS3_FLASH)		+= ps3flash.o
 obj-$(CONFIG_JS_RTC)		+= js-rtc.o
 js-rtc-y = rtc.o
 
+obj-$(CONFIG_ADC_NS9215)	+= adc-ns9215.o
+obj-$(CONFIG_ADC_S3C2443)	+= adc-s3c2443.o
+
 # Files generated that shall be removed upon make clean
 clean-files := consolemap_deftbl.c defkeymap.c
 
diff --git a/drivers/char/adc-ns9215.c b/drivers/char/adc-ns9215.c
new file mode 100644
index 000000000000..3c2b12c39f4d
--- /dev/null
+++ b/drivers/char/adc-ns9215.c
@@ -0,0 +1,257 @@
+/*
+ * linux/drivers/char/adc-ns9215.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/cdev.h>
+#include <linux/clk.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+
+/* registers */
+#define ADC_CONFIG			(0x0)
+#define ADC_CLOCK			(0x4)
+#define ADC_OUTPUT			(0x8)
+#define ADC_OUTPUT_OFFS			(0x4)
+
+/* configuration bit fields */
+#define ADC_CONFIG_DISABLE		(0 << 31)
+#define ADC_CONFIG_ENABLE		(1 << 31)
+#define ADC_CONFIG_ENABLECHANNELS	(7)
+
+#define DRIVER_NAME	"adc-ns9215"
+#define CHANNELS	8
+#define MAXCLOCK	14000000	/* max 14MHz */
+#define MAXCLOCKN	0x3FF
+
+struct ns9215_adc_channel {
+	struct semaphore	sem;
+	int			pos;
+	unsigned char		lb;
+};
+
+struct ns9215_adc_pdata {
+	void __iomem			*ioaddr;
+	struct resource			*mem;
+	struct clk			*clk;
+	struct cdev			cdev;
+	struct ns9215_adc_channel	channel[CHANNELS];
+	dev_t				dev_id;
+};
+
+static struct ns9215_adc_pdata pdata;
+
+ssize_t ns9215_adc_read(struct file *filep, char __user *buff,
+		size_t count, loff_t *offp)
+{
+	unsigned int val;
+	unsigned char buf[2];
+	int channel = (int)filep->private_data;
+
+	if (pdata.channel[channel].pos == 1) {
+		copy_to_user(buff, &pdata.channel[channel].lb, 1);
+		pdata.channel[channel].pos = 0;
+		return 1;
+	}
+
+	val = ioread32(pdata.ioaddr + ADC_OUTPUT + (channel * ADC_OUTPUT_OFFS));
+	buf[1] = (val & 0xf00) >> 8;
+	buf[0] = val & 0xff;
+
+	if (count == 1) {
+		copy_to_user(buff, buf , 1);
+		pdata.channel[channel].pos = 1;
+		pdata.channel[channel].lb = buf[1];
+		return 1;
+	}
+
+	copy_to_user(buff, buf, 2);
+	return 2;
+}
+
+int ns9215_adc_open(struct inode *inode, struct file *filep)
+{
+	int channel = iminor(inode);
+
+	if (down_trylock(&pdata.channel[channel].sem) < 0)
+		return -EBUSY;
+
+	filep->private_data = (void *)channel;
+	pdata.channel[channel].pos = 0;
+
+	return 0;
+}
+
+int ns9215_adc_release(struct inode *inode, struct file *filep)
+{
+	int channel = iminor(inode);
+
+	up(&pdata.channel[channel].sem);
+
+	return 0;
+}
+
+struct file_operations ns9215_adc_fops = {
+	.owner = THIS_MODULE,
+	.read = ns9215_adc_read,
+	.open = ns9215_adc_open,
+	.release = ns9215_adc_release,
+};
+
+static int __init ns9215_adc_probe(struct platform_device *pdev)
+{
+	int i, ret;
+	unsigned int clockn;
+
+	pdata.mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!pdata.mem) {
+		pr_err(DRIVER_NAME ": memory not available\n");
+		return -ENOENT;
+	}
+
+	if (!request_mem_region(pdata.mem->start,
+			pdata.mem->end - pdata.mem->start + 1,
+			DRIVER_NAME)) {
+		pr_err(DRIVER_NAME ": memory already mapped\n");
+		goto ns9215_err_mem;
+	}
+
+	pdata.ioaddr = ioremap(pdata.mem->start,
+			pdata.mem->end - pdata.mem->start + 1);
+	if (pdata.ioaddr <= 0) {
+		pr_err(DRIVER_NAME ": unable to remap IO memory\n");
+		goto ns9215_err_remap;
+	}
+	pr_debug(DRIVER_NAME ": mapped ADC to virtual address 0x%x\n",
+			(int)pdata.ioaddr);
+
+	if (alloc_chrdev_region(&pdata.dev_id, 0, CHANNELS, DRIVER_NAME) < 0) {
+		pr_err(DRIVER_NAME ": requesting chrdev_region fails\n");
+		goto ns9215_err_cdev_reg;
+	}
+
+	pdata.clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(pdata.clk)) {
+		ret = PTR_ERR(pdata.clk);
+		goto ns9215_err_clk_get;
+	}
+
+	ret = clk_enable(pdata.clk);
+	if (ret) {
+		pr_err(DRIVER_NAME ": cannot enable clock\n");
+		goto ns9215_err_clk_enable;
+	}
+
+	/* set clock */
+	clockn = ((clk_get_rate(pdata.clk)) - 2 * MAXCLOCK)
+		/ (2 * MAXCLOCK) + 1;
+	if (clockn > MAXCLOCKN) {
+		pr_warning(DRIVER_NAME ": cannot set a proper ADC clock,"
+				" ADC will not work correctly!\n");
+		clockn = MAXCLOCKN;
+	}
+
+	iowrite32(clockn, pdata.ioaddr + ADC_CLOCK);
+
+	/* initialize mutexes for every channel */
+	for (i = 0; i < CHANNELS; i++) {
+		init_MUTEX(&pdata.channel[i].sem);
+		pdata.channel[i].pos = 0;
+	}
+
+	/* enable adc & all channels */
+	iowrite32(ADC_CONFIG_ENABLE | ADC_CONFIG_ENABLECHANNELS,
+			pdata.ioaddr + ADC_CONFIG);
+
+	cdev_init(&pdata.cdev, &ns9215_adc_fops);
+	if (cdev_add(&pdata.cdev, pdata.dev_id, CHANNELS) < 0) {
+		pr_err(DRIVER_NAME ": cannot add character device\n");
+		goto ns9215_err_cdev_add;
+	}
+
+	pr_info(DRIVER_NAME ": ADC available on MAJOR %i\n",
+			MAJOR(pdata.dev_id));
+
+	return 0;
+
+ns9215_err_cdev_add:
+	clk_disable(pdata.clk);
+ns9215_err_clk_enable:
+	clk_put(pdata.clk);
+ns9215_err_clk_get:
+	unregister_chrdev_region(pdata.dev_id, CHANNELS);
+ns9215_err_cdev_reg:
+	iounmap(pdata.ioaddr);
+ns9215_err_remap:
+	release_mem_region(pdata.mem->start,
+			pdata.mem->end - pdata.mem->start + 1);
+ns9215_err_mem:
+
+	return -EIO;
+}
+
+static int ns9215_adc_remove(struct platform_device *pdev)
+{
+	int i;
+
+	/* lock/check all channels */
+	for (i = 0; i < CHANNELS; i++)
+		if (down_trylock(&pdata.channel[i].sem))
+			/* channel still locked! */
+			goto ns9215_adc_remove_unlock;
+
+	/* disable ADC */
+	iowrite32(ADC_CONFIG_DISABLE,  pdata.ioaddr + ADC_CONFIG);
+
+	cdev_del(&pdata.cdev);
+	clk_disable(pdata.clk);
+	clk_put(pdata.clk);
+	unregister_chrdev_region(pdata.dev_id, CHANNELS);
+	iounmap(pdata.ioaddr);
+	release_mem_region(pdata.mem->start,
+			pdata.mem->end - pdata.mem->start + 1);
+
+	return 0;
+
+ns9215_adc_remove_unlock:
+	for (i--; i >= 0; i--)
+		up(&pdata.channel[i].sem);
+	return -EBUSY;
+}
+
+static struct platform_driver ns9215_adc_driver = {
+	.probe		= ns9215_adc_probe,
+	.remove		= ns9215_adc_remove,
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+	/* TODO: power management */
+};
+
+static int __init ns9215_adc_init(void)
+{
+	return platform_driver_register(&ns9215_adc_driver);
+}
+
+static void __exit ns9215_adc_exit(void)
+{
+	platform_driver_unregister(&ns9215_adc_driver);
+}
+
+module_init(ns9215_adc_init);
+module_exit(ns9215_adc_exit);
+
+MODULE_AUTHOR("Matthias Ludwig");
+MODULE_DESCRIPTION("ADC driver for Digi NS9215");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/char/adc-s3c2443.c b/drivers/char/adc-s3c2443.c
new file mode 100644
index 000000000000..34de79e20045
--- /dev/null
+++ b/drivers/char/adc-s3c2443.c
@@ -0,0 +1,323 @@
+/*
+ * linux/drivers/char/adc-s3c2443.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+/*
+ * In future, this driver should be replaced by the general adc core support 
+ * for the Samsung platforms, plus the corresponding hwmon driver and the touch
+ * driver that uses that common adc support.
+ */
+
+/*
+ * TODO, split from this driver and from the touch driver the common stuff and
+ * move it to a core adc module.
+ */
+
+
+#include <linux/cdev.h>
+#include <linux/clk.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+
+#include <plat/regs-adc.h>
+#include <plat/ts.h>
+
+#define DRIVER_NAME	"s3c2443-adc"
+#ifndef NUM_CHANNELS
+# define NUM_CHANNELS	10
+#endif
+
+struct s3c2443_adc_channel {
+	struct semaphore	sem;
+};
+
+struct s3c2443_adc_dev {
+	void __iomem			*ioaddr;
+	struct resource			*res_iomem;
+	struct clk			*clk;
+	struct cdev			cdev;
+	struct s3c2443_adc_channel	channel[NUM_CHANNELS];
+	dev_t				dev_id;
+	spinlock_t			lock;
+	u32				adccon;
+	u32				adctsc;
+  	u32				adcdly;
+};
+
+static struct s3c2443_adc_dev *adc;
+
+static u16 adc_read_sample(struct s3c2443_adc_dev *adcdev, int channel)
+{
+	u32 adccon, timeout = 5000;
+
+	/* Select channel, start conversion and wait until its completed */
+	writel(channel, adcdev->ioaddr + S3C2410_ADCMUX);
+	adccon = readl(adcdev->ioaddr + S3C2410_ADCCON);
+	writel(adccon | S3C2410_ADCCON_ENABLE_START, adcdev->ioaddr + S3C2410_ADCCON);
+	
+	while (timeout--) {
+		adccon = readl(adcdev->ioaddr + S3C2410_ADCCON);
+		if (adccon & S3C2410_ADCCON_ECFLG)
+			break;
+		ndelay(100);
+	}
+
+	return readl(adcdev->ioaddr + S3C2410_ADCDAT0) & S3C2410_ADCDAT0_XPDATA_MASK;
+}
+
+
+static ssize_t s3c2443_adc_read(struct file *filep, char __user *buff,
+				size_t count, loff_t *offp)
+{
+	struct s3c2443_adc_dev *adcdev = (struct s3c2443_adc_dev *)filep->private_data;
+	int ch = iminor(filep->f_dentry->d_inode);
+	u16 val12;
+	u8 val8;
+
+	val12 = adc_read_sample(adcdev, ch);
+	val8 = val12 >> 4;
+
+	count = (count > 2) ? 2 : count;
+	
+	if (count == 1)
+		copy_to_user(buff, &val8, count);
+	else
+		copy_to_user(buff, &val12, count);
+
+	return count;
+}
+
+static int s3c2443_adc_open(struct inode *inode, struct file *filep)
+{
+	struct s3c2443_adc_dev *adcdev = container_of(inode->i_cdev,
+						      struct s3c2443_adc_dev, cdev);
+	int ch = iminor(inode);
+
+	if (ch >= NUM_CHANNELS)
+		return -EINVAL;
+
+	if (down_trylock(&adcdev->channel[ch].sem) < 0)
+		return -EBUSY;
+
+	filep->private_data = (void *)adcdev;
+
+	return 0;
+}
+
+static int s3c2443_adc_release(struct inode *inode, struct file *filep)
+{
+	struct s3c2443_adc_dev *adcdev = container_of(inode->i_cdev,
+						      struct s3c2443_adc_dev, cdev);
+	int ch = iminor(inode);
+
+	up(&adcdev->channel[ch].sem);
+
+	return 0;
+}
+
+struct file_operations s3c2443_adc_fops = {
+	.owner		= THIS_MODULE,
+	.read		= s3c2443_adc_read,
+	.open		= s3c2443_adc_open,
+	.release	= s3c2443_adc_release,
+};
+
+static int __devinit s3c2443_adc_probe(struct platform_device *pdev)
+{
+	int ret, i;
+	struct s3c_ts_mach_info *info;
+
+	info = pdev->dev.platform_data;
+	if (!info) {
+		pr_err(DRIVER_NAME ": no platform data provided\n");
+		return -EINVAL;
+	}
+
+	adc = kzalloc(sizeof(*adc), GFP_KERNEL);
+	if (!adc) {
+		pr_err(DRIVER_NAME ": mem alloc error\n");
+		return -ENOMEM;
+	}
+
+	adc->res_iomem = platform_get_resource_byname(pdev, IORESOURCE_MEM, DRIVER_NAME);
+	if (!adc->res_iomem) {
+		pr_err(DRIVER_NAME ": unable to find iomem resource\n");
+		ret = -ENOENT;
+		goto error_get_res; 
+	}
+	/* 
+	 * Dont request mem region becasue the address space is shared with the touch
+         * driver. This will be resolved in future when both drivers use the same core
+	 * adc funcionality 
+	 */
+	adc->ioaddr = ioremap(adc->res_iomem->start, 
+			      adc->res_iomem->end - adc->res_iomem->start + 1);
+	if (!adc->ioaddr) {
+		pr_err(DRIVER_NAME ": unable to remap IO memory\n");
+		ret = -EBUSY;
+		goto error_remap;
+	}
+	pr_debug(DRIVER_NAME ": mapped ADC to virtual address 0x%x\n", (u32)adc->ioaddr);
+
+	ret = alloc_chrdev_region(&adc->dev_id, 0, NUM_CHANNELS, DRIVER_NAME);
+	if (ret < 0) {
+		pr_err(DRIVER_NAME ": error requesting chrdev_region for %d\n", adc->dev_id);
+		goto error_cdev_reg;
+	}
+
+	/* Get the required source clock */
+	adc->clk = clk_get(&pdev->dev, "adc");
+	if (IS_ERR(adc->clk)) {
+		pr_err(DRIVER_NAME ": error getting adc clock source\n");
+		ret = PTR_ERR(adc->clk);
+		goto error_get_clk;
+	}
+	clk_enable(adc->clk);
+	
+	/* initialize mutexes for every channel */
+	for (i = 0; i < NUM_CHANNELS; i++)
+		init_MUTEX(&adc->channel[i].sem);
+
+	cdev_init(&adc->cdev, &s3c2443_adc_fops);
+	ret = cdev_add(&adc->cdev, adc->dev_id, NUM_CHANNELS);
+	if (ret < 0) {
+		pr_err(DRIVER_NAME ": cannot add character device\n");
+ 		goto error_cdev_add;
+	}
+
+	/* Prepare the passed user configuration values */
+	if ((info->presc & 0xff) > 0)
+		writel(S3C2410_ADCCON_PRSCEN |
+		       S3C2410_ADCCON_PRSCVL(info->presc & 0xFF),
+		       adc->ioaddr + S3C2410_ADCCON);
+	else
+		writel(0, adc->ioaddr + S3C2410_ADCCON);
+
+
+	/* Initialise the start delay register (Manual: Do not use zero value) */
+	if ((info->delay & 0xffff) > 0)
+		writel(info->delay & 0xffff, adc->ioaddr + S3C2410_ADCDLY);
+	else
+		writel(10000,  adc->ioaddr + S3C2410_ADCDLY);
+
+	platform_set_drvdata(pdev, adc);
+
+	pr_info(DRIVER_NAME ": ADC available on MAJOR %i\n", MAJOR(adc->dev_id));
+
+	return 0;
+
+error_cdev_add:
+	clk_put(adc->clk);
+error_get_clk:
+	unregister_chrdev_region(adc->dev_id, NUM_CHANNELS);
+error_cdev_reg:
+	iounmap(adc->ioaddr);
+error_remap:
+error_get_res:
+	kfree(adc);
+	return ret;
+}
+
+int s3c2443_adc_remove(struct platform_device *pdev)
+{
+	int i;
+
+	if (!adc)
+		return -EIO;
+  
+	/* lock/check all channels */
+	for (i = 0; i < NUM_CHANNELS; i++) {
+		if (down_trylock(&adc->channel[i].sem))
+			/* channel still locked! */
+			goto adc_remove_unlock;
+	}
+
+	cdev_del(&adc->cdev);
+	clk_disable(adc->clk);
+	clk_put(adc->clk);
+ 	unregister_chrdev_region(adc->dev_id, NUM_CHANNELS);
+	iounmap(adc->ioaddr);
+	kfree(adc);
+	platform_set_drvdata(pdev, NULL);
+      
+	return 0;
+
+adc_remove_unlock:
+	for (i--; i >= 0; i--)
+		up(&adc->channel[i].sem);
+	return -EBUSY;
+}
+
+#ifdef CONFIG_PM
+static int s3c2443_adc_suspend(struct platform_device *dev, pm_message_t state)
+{
+	struct s3c2443_adc_dev *adc;
+
+	adc = platform_get_drvdata(dev);
+
+	/* save registers and disable the clock */
+	adc->adccon = readl(adc->ioaddr + S3C2410_ADCCON);
+	adc->adctsc = readl(adc->ioaddr + S3C2410_ADCTSC);
+	adc->adcdly = readl(adc->ioaddr + S3C2410_ADCDLY);
+	clk_disable(adc->clk);
+
+	return 0;
+}
+
+static int s3c2443_adc_resume(struct platform_device *pdev)
+{
+	struct s3c2443_adc_dev *adc;
+
+	adc = platform_get_drvdata(pdev);
+
+	/* restore registers and enable the clock */
+	clk_enable(adc->clk);
+	writel(adc->adccon, adc->ioaddr + S3C2410_ADCCON);
+	writel(adc->adctsc, adc->ioaddr + S3C2410_ADCTSC);
+	writel(adc->adcdly, adc->ioaddr + S3C2410_ADCDLY);
+		
+	return 0;
+}
+#else
+#define s3c2443_adc_suspend	NULL
+#define s3c2443_adc_resume	NULL
+#endif
+
+static struct platform_driver s3c2443_adc_driver = {
+	.probe		= s3c2443_adc_probe,
+	.remove		= __devexit_p(s3c2443_adc_remove),
+	.suspend	= s3c2443_adc_suspend,
+	.resume		= s3c2443_adc_resume,
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init s3c2443_adc_init(void)
+{
+	return platform_driver_register(&s3c2443_adc_driver);
+}
+
+static void __exit s3c2443_adc_exit(void)
+{
+	platform_driver_unregister(&s3c2443_adc_driver);
+}
+
+module_init(s3c2443_adc_init);
+module_exit(s3c2443_adc_exit);
+
+MODULE_AUTHOR("Digi International");
+MODULE_DESCRIPTION("ADC driver for S3C2443");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/char/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index 8822eca58ffa..c6703b94324c 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -46,6 +46,30 @@ config HW_RANDOM_AMD
 
 	  If unsure, say Y.
 
+config HW_RANDOM_FSL_RNGA
+	tristate "Freescale RNGA Random Number Generator"
+	depends on HW_RANDOM && ARCH_HAS_RNGA && !MXC_SECURITY_RNG
+	---help---
+	  This driver provides kernel-side support for the Random Number
+	  Generator hardware found on Freescale i.MX processors.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called fsl-rnga.
+
+	  If unsure, say Y.
+
+config HW_RANDOM_FSL_RNGC
+	tristate "Freescale RNGC Random Number Generator"
+	depends on HW_RANDOM && ARCH_HAS_RNGC && !MXC_SECURITY_RNG
+	---help---
+	  This driver provides kernel-side support for the Random Number
+	  Generator hardware found on Freescale i.MX processors.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called fsl-rngc.
+
+	  If unsure, say Y.
+
 config HW_RANDOM_GEODE
 	tristate "AMD Geode HW Random Number Generator support"
 	depends on HW_RANDOM && X86_32 && PCI
diff --git a/drivers/char/hw_random/Makefile b/drivers/char/hw_random/Makefile
index b6effb7522c2..bea544b26e53 100644
--- a/drivers/char/hw_random/Makefile
+++ b/drivers/char/hw_random/Makefile
@@ -6,6 +6,8 @@ obj-$(CONFIG_HW_RANDOM) += rng-core.o
 rng-core-y := core.o
 obj-$(CONFIG_HW_RANDOM_INTEL) += intel-rng.o
 obj-$(CONFIG_HW_RANDOM_AMD) += amd-rng.o
+obj-$(CONFIG_HW_RANDOM_FSL_RNGA) += fsl-rnga.o
+obj-$(CONFIG_HW_RANDOM_FSL_RNGC) += fsl-rngc.o
 obj-$(CONFIG_HW_RANDOM_GEODE) += geode-rng.o
 obj-$(CONFIG_HW_RANDOM_N2RNG) += n2-rng.o
 n2-rng-y := n2-drv.o n2-asm.o
diff --git a/drivers/char/hw_random/fsl-rnga.c b/drivers/char/hw_random/fsl-rnga.c
new file mode 100644
index 000000000000..a5c8065604d4
--- /dev/null
+++ b/drivers/char/hw_random/fsl-rnga.c
@@ -0,0 +1,238 @@
+/*
+ * RNG driver for Freescale RNGA
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * Hardware driver for the Intel/AMD/VIA Random Number Generators (RNG)
+ * (c) Copyright 2003 Red Hat Inc <jgarzik@redhat.com>
+ *
+ * derived from
+ *
+ * Hardware driver for the AMD 768 Random Number Generator (RNG)
+ * (c) Copyright 2001 Red Hat Inc <alan@redhat.com>
+ *
+ * derived from
+ *
+ * Hardware driver for Intel i810 Random Number Generator (RNG)
+ * Copyright 2000,2001 Jeff Garzik <jgarzik@pobox.com>
+ * Copyright 2000,2001 Philipp Rumpf <prumpf@mandrakesoft.com>
+ *
+ * This file is licensed under  the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/hw_random.h>
+#include <linux/io.h>
+
+/* RNGA Registers */
+#define RNGA_CONTROL			0x00
+#define RNGA_STATUS			0x04
+#define RNGA_ENTROPY			0x08
+#define RNGA_OUTPUT_FIFO		0x0c
+#define RNGA_MODE			0x10
+#define RNGA_VERIFICATION_CONTROL	0x14
+#define RNGA_OSC_CONTROL_COUNTER	0x18
+#define RNGA_OSC1_COUNTER		0x1c
+#define RNGA_OSC2_COUNTER		0x20
+#define RNGA_OSC_COUNTER_STATUS		0x24
+
+/* RNGA Registers Range */
+#define RNG_ADDR_RANGE			0x28
+
+/* RNGA Control Register */
+#define RNGA_CONTROL_SLEEP		0x00000010
+#define RNGA_CONTROL_CLEAR_INT		0x00000008
+#define RNGA_CONTROL_MASK_INTS		0x00000004
+#define RNGA_CONTROL_HIGH_ASSURANCE	0x00000002
+#define RNGA_CONTROL_GO			0x00000001
+
+#define RNGA_STATUS_LEVEL_MASK		0x0000ff00
+
+/* RNGA Status Register */
+#define RNGA_STATUS_OSC_DEAD		0x80000000
+#define RNGA_STATUS_SLEEP		0x00000010
+#define RNGA_STATUS_ERROR_INT		0x00000008
+#define RNGA_STATUS_FIFO_UNDERFLOW	0x00000004
+#define RNGA_STATUS_LAST_READ_STATUS	0x00000002
+#define RNGA_STATUS_SECURITY_VIOLATION	0x00000001
+
+static struct platform_device *rng_dev;
+
+static int fsl_rnga_data_present(struct hwrng *rng)
+{
+	int level;
+	u32 rng_base = (u32) rng->priv;
+
+	/* how many random numbers is in FIFO? [0-16] */
+	level = ((__raw_readl(rng_base + RNGA_STATUS) &
+			RNGA_STATUS_LEVEL_MASK) >> 8);
+
+	return level > 0 ? 1 : 0;
+}
+
+static int fsl_rnga_data_read(struct hwrng *rng, u32 * data)
+{
+	int err;
+	u32 ctrl, rng_base = (u32) rng->priv;
+
+	/* retrieve a random number from FIFO */
+	*data = __raw_readl(rng_base + RNGA_OUTPUT_FIFO);
+
+	/* some error while reading this random number? */
+	err = __raw_readl(rng_base + RNGA_STATUS) & RNGA_STATUS_ERROR_INT;
+
+	/* if error: clear error interrupt, but doesn't return random number */
+	if (err) {
+		dev_dbg(&rng_dev->dev, "Error while reading random number!\n");
+		ctrl = __raw_readl(rng_base + RNGA_CONTROL);
+		__raw_writel(ctrl | RNGA_CONTROL_CLEAR_INT,
+					rng_base + RNGA_CONTROL);
+		return 0;
+	} else
+		return 4;
+}
+
+static int fsl_rnga_init(struct hwrng *rng)
+{
+	u32 ctrl, osc, rng_base = (u32) rng->priv;
+
+	/* wake up */
+	ctrl = __raw_readl(rng_base + RNGA_CONTROL);
+	__raw_writel(ctrl & ~RNGA_CONTROL_SLEEP, rng_base + RNGA_CONTROL);
+
+	/* verify if oscillator is working */
+	osc = __raw_readl(rng_base + RNGA_STATUS);
+	if (osc & RNGA_STATUS_OSC_DEAD) {
+		dev_err(&rng_dev->dev, "RNGA Oscillator is dead!\n");
+		return -ENODEV;
+	}
+
+	/* go running */
+	ctrl = __raw_readl(rng_base + RNGA_CONTROL);
+	__raw_writel(ctrl | RNGA_CONTROL_GO, rng_base + RNGA_CONTROL);
+
+	return 0;
+}
+
+static void fsl_rnga_cleanup(struct hwrng *rng)
+{
+	u32 ctrl, rng_base = (u32) rng->priv;
+
+	ctrl = __raw_readl(rng_base + RNGA_CONTROL);
+
+	/* stop rnga */
+	__raw_writel(ctrl & ~RNGA_CONTROL_GO, rng_base + RNGA_CONTROL);
+}
+
+static struct hwrng fsl_rnga = {
+	.name = "fsl-rnga",
+	.init = fsl_rnga_init,
+	.cleanup = fsl_rnga_cleanup,
+	.data_present = fsl_rnga_data_present,
+	.data_read = fsl_rnga_data_read
+};
+
+static int __init fsl_rnga_probe(struct platform_device *pdev)
+{
+	int err = -ENODEV;
+	struct clk *clk;
+	struct resource *res, *mem;
+	void __iomem *rng_base = NULL;
+
+	if (rng_dev)
+		return -EBUSY;
+
+	clk = clk_get(NULL, "rng_clk");
+
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "Could not get rng_clk!\n");
+		err = PTR_ERR(clk);
+		return err;
+	}
+
+	clk_enable(clk);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (!res)
+		return -ENOENT;
+
+	mem = request_mem_region(res->start, res->end - res->start, pdev->name);
+
+	if (mem == NULL)
+		return -EBUSY;
+
+	dev_set_drvdata(&pdev->dev, mem);
+	rng_base = ioremap(res->start, res->end - res->start);
+
+	fsl_rnga.priv = (unsigned long)rng_base;
+
+	err = hwrng_register(&fsl_rnga);
+	if (err) {
+		dev_err(&pdev->dev, "FSL RNGA registering failed (%d)\n", err);
+		return err;
+	}
+
+	rng_dev = pdev;
+
+	dev_info(&pdev->dev, "FSL RNGA Registered.\n");
+
+	return 0;
+}
+
+static int __exit fsl_rnga_remove(struct platform_device *pdev)
+{
+	struct resource *mem = dev_get_drvdata(&pdev->dev);
+	void __iomem *rng_base = (void __iomem *)fsl_rnga.priv;
+
+	hwrng_unregister(&fsl_rnga);
+
+	release_resource(mem);
+
+	iounmap(rng_base);
+
+	return 0;
+}
+
+static struct platform_driver fsl_rnga_driver = {
+	.driver = {
+		   .name = "fsl_rnga",
+		   .owner = THIS_MODULE,
+		   },
+	.remove = __exit_p(fsl_rnga_remove),
+};
+
+static int __init mod_init(void)
+{
+	return platform_driver_probe(&fsl_rnga_driver, fsl_rnga_probe);
+}
+
+static void __exit mod_exit(void)
+{
+	platform_driver_unregister(&fsl_rnga_driver);
+}
+
+module_init(mod_init);
+module_exit(mod_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("H/W RNGA driver for i.MX");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/hw_random/fsl-rngc.c b/drivers/char/hw_random/fsl-rngc.c
new file mode 100644
index 000000000000..9bf78e846fa0
--- /dev/null
+++ b/drivers/char/hw_random/fsl-rngc.c
@@ -0,0 +1,372 @@
+/*
+ * RNG driver for Freescale RNGC
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * Hardware driver for the Intel/AMD/VIA Random Number Generators (RNG)
+ * (c) Copyright 2003 Red Hat Inc <jgarzik@redhat.com>
+ *
+ * derived from
+ *
+ * Hardware driver for the AMD 768 Random Number Generator (RNG)
+ * (c) Copyright 2001 Red Hat Inc <alan@redhat.com>
+ *
+ * derived from
+ *
+ * Hardware driver for Intel i810 Random Number Generator (RNG)
+ * Copyright 2000,2001 Jeff Garzik <jgarzik@pobox.com>
+ * Copyright 2000,2001 Philipp Rumpf <prumpf@mandrakesoft.com>
+ *
+ * This file is licensed under  the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/hw_random.h>
+#include <linux/io.h>
+#include <asm/hardware.h>
+
+#define RNGC_VERSION_MAJOR3 3
+
+#define RNGC_VERSION_ID				0x0000
+#define RNGC_COMMAND				0x0004
+#define RNGC_CONTROL				0x0008
+#define RNGC_STATUS				0x000C
+#define RNGC_ERROR				0x0010
+#define RNGC_FIFO				0x0014
+#define RNGC_VERIF_CTRL				0x0020
+#define RNGC_OSC_CTRL_COUNT			0x0028
+#define RNGC_OSC_COUNT				0x002C
+#define RNGC_OSC_COUNT_STATUS			0x0030
+
+#define RNGC_VERID_ZEROS_MASK			0x0f000000
+#define RNGC_VERID_RNG_TYPE_MASK		0xf0000000
+#define RNGC_VERID_RNG_TYPE_SHIFT		28
+#define RNGC_VERID_CHIP_VERSION_MASK		0x00ff0000
+#define RNGC_VERID_CHIP_VERSION_SHIFT		16
+#define RNGC_VERID_VERSION_MAJOR_MASK		0x0000ff00
+#define RNGC_VERID_VERSION_MAJOR_SHIFT		8
+#define RNGC_VERID_VERSION_MINOR_MASK		0x000000ff
+#define RNGC_VERID_VERSION_MINOR_SHIFT		0
+
+#define RNGC_CMD_ZEROS_MASK			0xffffff8c
+#define RNGC_CMD_SW_RST				0x00000040
+#define RNGC_CMD_CLR_ERR			0x00000020
+#define RNGC_CMD_CLR_INT			0x00000010
+#define RNGC_CMD_SEED				0x00000002
+#define RNGC_CMD_SELF_TEST			0x00000001
+
+#define RNGC_CTRL_ZEROS_MASK			0xfffffc8c
+#define RNGC_CTRL_CTL_ACC			0x00000200
+#define RNGC_CTRL_VERIF_MODE			0x00000100
+#define RNGC_CTRL_MASK_ERROR			0x00000040
+
+#define RNGC_CTRL_MASK_DONE			0x00000020
+#define RNGC_CTRL_AUTO_SEED			0x00000010
+#define RNGC_CTRL_FIFO_UFLOW_MASK		0x00000003
+#define RNGC_CTRL_FIFO_UFLOW_SHIFT		0
+
+#define RNGC_CTRL_FIFO_UFLOW_ZEROS_ERROR	0
+#define RNGC_CTRL_FIFO_UFLOW_ZEROS_ERROR2	1
+#define RNGC_CTRL_FIFO_UFLOW_BUS_XFR		2
+#define RNGC_CTRL_FIFO_UFLOW_ZEROS_INTR		3
+
+#define RNGC_STATUS_ST_PF_MASK			0x00c00000
+#define RNGC_STATUS_ST_PF_SHIFT			22
+#define RNGC_STATUS_ST_PF_TRNG			0x00800000
+#define RNGC_STATUS_ST_PF_PRNG			0x00400000
+#define RNGC_STATUS_ERROR			0x00010000
+#define RNGC_STATUS_FIFO_SIZE_MASK		0x0000f000
+#define RNGC_STATUS_FIFO_SIZE_SHIFT		12
+#define RNGC_STATUS_FIFO_LEVEL_MASK		0x00000f00
+#define RNGC_STATUS_FIFO_LEVEL_SHIFT		8
+#define RNGC_STATUS_NEXT_SEED_DONE		0x00000040
+#define RNGC_STATUS_SEED_DONE			0x00000020
+#define RNGC_STATUS_ST_DONE			0x00000010
+#define RNGC_STATUS_RESEED			0x00000008
+#define RNGC_STATUS_SLEEP			0x00000004
+#define RNGC_STATUS_BUSY			0x00000002
+#define RNGC_STATUS_SEC_STATE			0x00000001
+
+#define RNGC_ERROR_STATUS_ZEROS_MASK		0xffffffc0
+#define RNGC_ERROR_STATUS_BAD_KEY		0x00000040
+#define RNGC_ERROR_STATUS_RAND_ERR		0x00000020
+#define RNGC_ERROR_STATUS_FIFO_ERR		0x00000010
+#define RNGC_ERROR_STATUS_STAT_ERR		0x00000008
+#define RNGC_ERROR_STATUS_ST_ERR		0x00000004
+#define RNGC_ERROR_STATUS_OSC_ERR		0x00000002
+#define RNGC_ERROR_STATUS_LFSR_ERR		0x00000001
+
+#define RNG_ADDR_RANGE				0x34
+
+static DECLARE_COMPLETION(rng_self_testing);
+static DECLARE_COMPLETION(rng_seed_done);
+
+static struct platform_device *rng_dev;
+
+int irq_rng;
+
+static int fsl_rngc_data_present(struct hwrng *rng)
+{
+	int level;
+	u32 rngc_base = (u32) rng->priv;
+
+	/* how many random numbers are in FIFO? [0-16] */
+	level = (__raw_readl(rngc_base + RNGC_STATUS) &
+	    RNGC_STATUS_FIFO_LEVEL_MASK) >> RNGC_STATUS_FIFO_LEVEL_SHIFT;
+
+	return level > 0 ? 1 : 0;
+}
+
+static int fsl_rngc_data_read(struct hwrng *rng, u32 * data)
+{
+	int err;
+	u32 rngc_base = (u32) rng->priv;
+
+	/* retrieve a random number from FIFO */
+	*data = __raw_readl(rngc_base + RNGC_FIFO);
+
+	/* is there some error while reading this random number? */
+	err = __raw_readl(rngc_base + RNGC_STATUS) & RNGC_STATUS_ERROR;
+
+	/* if error happened doesn't return random number */
+	return err ? 0 : 4;
+}
+
+static irqreturn_t rngc_irq(int irq, void *dev)
+{
+	int handled = 0;
+	u32 rngc_base = (u32) dev;
+
+	/* is the seed creation done? */
+	if (__raw_readl(rngc_base + RNGC_STATUS) & RNGC_STATUS_SEED_DONE) {
+		complete(&rng_seed_done);
+		__raw_writel(RNGC_CMD_CLR_INT | RNGC_CMD_CLR_ERR,
+		       rngc_base + RNGC_COMMAND);
+		handled = 1;
+	}
+
+	/* is the self test done? */
+	if (__raw_readl(rngc_base + RNGC_STATUS) & RNGC_STATUS_ST_DONE) {
+		complete(&rng_self_testing);
+		__raw_writel(RNGC_CMD_CLR_INT | RNGC_CMD_CLR_ERR,
+			rngc_base + RNGC_COMMAND);
+		handled = 1;
+	}
+
+	/* is there any error? */
+	if (__raw_readl(rngc_base + RNGC_STATUS) & RNGC_STATUS_ERROR) {
+		/* clear interrupt */
+		__raw_writel(RNGC_CMD_CLR_INT | RNGC_CMD_CLR_ERR,
+		       rngc_base + RNGC_COMMAND);
+		handled = 1;
+	}
+
+	return handled;
+}
+
+static int fsl_rngc_init(struct hwrng *rng)
+{
+	int err;
+	u32 cmd, ctrl, osc, rngc_base = (u32) rng->priv;
+
+	INIT_COMPLETION(rng_self_testing);
+	INIT_COMPLETION(rng_seed_done);
+
+	err = __raw_readl(rngc_base + RNGC_STATUS) & RNGC_STATUS_ERROR;
+	if (err) {
+		/* is this a bad keys error ? */
+		if (__raw_readl(rngc_base + RNGC_ERROR) &
+		   RNGC_ERROR_STATUS_BAD_KEY) {
+			dev_err(&rng_dev->dev, "Can't start, Bad Keys.\n");
+			return -EIO;
+		}
+	}
+
+	/* mask all interrupts, will be unmasked soon */
+	ctrl = __raw_readl(rngc_base + RNGC_CONTROL);
+	__raw_writel(ctrl | RNGC_CTRL_MASK_DONE | RNGC_CTRL_MASK_ERROR,
+	       rngc_base + RNGC_CONTROL);
+
+	/* verify if oscillator is working */
+	osc = __raw_readl(rngc_base + RNGC_ERROR);
+	if (osc & RNGC_ERROR_STATUS_OSC_ERR) {
+		dev_err(&rng_dev->dev, "RNGC Oscillator is dead!\n");
+		return -EIO;
+	}
+
+	err = request_irq(irq_rng, rngc_irq, 0, "fsl_rngc", (void *)rng->priv);
+	if (err) {
+		dev_err(&rng_dev->dev, "Can't get interrupt working.\n");
+		return -EIO;
+	}
+
+	/* do self test, repeat until get success */
+	do {
+		/* clear error */
+		cmd = __raw_readl(rngc_base + RNGC_COMMAND);
+		__raw_writel(cmd | RNGC_CMD_CLR_ERR, rngc_base + RNGC_COMMAND);
+
+		/* unmask all interrupt */
+		ctrl = __raw_readl(rngc_base + RNGC_CONTROL);
+		__raw_writel(ctrl & ~(RNGC_CTRL_MASK_DONE |
+			RNGC_CTRL_MASK_ERROR), rngc_base + RNGC_CONTROL);
+
+		/* run self test */
+		cmd = __raw_readl(rngc_base + RNGC_COMMAND);
+		__raw_writel(cmd | RNGC_CMD_SELF_TEST,
+			rngc_base + RNGC_COMMAND);
+
+		wait_for_completion(&rng_self_testing);
+
+	} while (__raw_readl(rngc_base + RNGC_ERROR) &
+		RNGC_ERROR_STATUS_ST_ERR);
+
+	/* clear interrupt. Is it really necessary here? */
+	__raw_writel(RNGC_CMD_CLR_INT | RNGC_CMD_CLR_ERR,
+		rngc_base + RNGC_COMMAND);
+
+	/* create seed, repeat while there is some statistical error */
+	do {
+		/* clear error */
+		cmd = __raw_readl(rngc_base + RNGC_COMMAND);
+		__raw_writel(cmd | RNGC_CMD_CLR_ERR, rngc_base + RNGC_COMMAND);
+
+		/* seed creation */
+		cmd = __raw_readl(rngc_base + RNGC_COMMAND);
+		__raw_writel(cmd | RNGC_CMD_SEED, rngc_base + RNGC_COMMAND);
+
+		wait_for_completion(&rng_seed_done);
+
+	} while (__raw_readl(rngc_base + RNGC_ERROR) &
+		RNGC_ERROR_STATUS_STAT_ERR);
+
+	err = __raw_readl(rngc_base + RNGC_ERROR) &
+					(RNGC_ERROR_STATUS_STAT_ERR |
+					RNGC_ERROR_STATUS_RAND_ERR |
+					RNGC_ERROR_STATUS_FIFO_ERR |
+					RNGC_ERROR_STATUS_ST_ERR |
+					RNGC_ERROR_STATUS_OSC_ERR |
+					RNGC_ERROR_STATUS_LFSR_ERR);
+
+	if (err) {
+		dev_err(&rng_dev->dev, "FSL RNGC appears inoperable.\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static struct hwrng fsl_rngc = {
+	.name = "fsl-rngc",
+	.init = fsl_rngc_init,
+	.data_present = fsl_rngc_data_present,
+	.data_read = fsl_rngc_data_read
+};
+
+static int __init fsl_rngc_probe(struct platform_device *pdev)
+{
+	int err = -ENODEV;
+	struct clk *clk;
+	struct resource *res, *mem;
+	void __iomem *rngc_base = NULL;
+
+	if (rng_dev)
+		return -EBUSY;
+
+	clk = clk_get(NULL, "rng_clk");
+
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "Can not get rng_clk\n");
+		err = PTR_ERR(clk);
+		return err;
+	}
+
+	clk_enable(clk);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (!res)
+		return -ENOENT;
+
+	mem = request_mem_region(res->start, res->end - res->start, pdev->name);
+
+	if (mem == NULL)
+		return -EBUSY;
+
+	dev_set_drvdata(&pdev->dev, mem);
+	rngc_base = ioremap(res->start, res->end - res->start);
+
+	fsl_rngc.priv = (unsigned long)rngc_base;
+
+	irq_rng = platform_get_irq(pdev, 0);
+
+	err = hwrng_register(&fsl_rngc);
+	if (err) {
+		dev_err(&pdev->dev, "FSL RNGC registering failed (%d)\n", err);
+		return err;
+	}
+
+	rng_dev = pdev;
+
+	dev_info(&pdev->dev, "FSL RNGC Registered.\n");
+
+	return 0;
+}
+
+static int __exit fsl_rngc_remove(struct platform_device *pdev)
+{
+	struct resource *mem = dev_get_drvdata(&pdev->dev);
+	void __iomem *rngc_base = (void __iomem *)fsl_rngc.priv;
+
+	hwrng_unregister(&fsl_rngc);
+
+	release_resource(mem);
+
+	iounmap(rngc_base);
+
+	return 0;
+}
+
+static struct platform_driver fsl_rngc_driver = {
+	.driver = {
+		   .name = "fsl_rngc",
+		   .owner = THIS_MODULE,
+		   },
+	.remove = __exit_p(fsl_rngc_remove),
+};
+
+static int __init mod_init(void)
+{
+	return platform_driver_probe(&fsl_rngc_driver, fsl_rngc_probe);
+}
+
+static void __exit mod_exit(void)
+{
+	platform_driver_unregister(&fsl_rngc_driver);
+}
+
+module_init(mod_init);
+module_exit(mod_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("H/W RNGC driver for i.MX");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/imx_sim.c b/drivers/char/imx_sim.c
new file mode 100644
index 000000000000..41bee5d7cdcb
--- /dev/null
+++ b/drivers/char/imx_sim.c
@@ -0,0 +1,1495 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_sim.c
+ *
+ * @brief Driver for Freescale IMX SIM interface
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/ioport.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/poll.h>
+#include <linux/miscdevice.h>
+#include <linux/clk.h>
+#include <linux/types.h>
+#include <linux/platform_device.h>
+#include <linux/mxc_sim_interface.h>
+
+#include <asm/io.h>
+
+#define SIM_INTERNAL_CLK  0
+#define SIM_RFU          -1
+
+/* Default communication parameters: FI=372, DI=1, PI1=5V, II=50mA, WWT=10 */
+#define SIM_PARAM_DEFAULT { 0, 1, 1, 5, 1, 0, 0, 0, 10 }
+
+/* Transmit and receive buffer sizes */
+#define SIM_XMT_BUFFER_SIZE 256
+#define SIM_RCV_BUFFER_SIZE 256
+
+/* Interface character references */
+#define SIM_IFC_TXI(letter, number) (letter + number * 4)
+#define SIM_IFC_TA1   SIM_IFC_TXI(0, 0)
+#define SIM_IFC_TB1   SIM_IFC_TXI(0, 1)
+#define SIM_IFC_TC1   SIM_IFC_TXI(0, 2)
+#define SIM_IFC_TD1   SIM_IFC_TXI(0, 3)
+#define SIM_IFC_TA2   SIM_IFC_TXI(1, 0)
+#define SIM_IFC_TB2   SIM_IFC_TXI(1, 1)
+#define SIM_IFC_TC2   SIM_IFC_TXI(1, 2)
+#define SIM_IFC_TD2   SIM_IFC_TXI(1, 3)
+#define SIM_IFC_TA3   SIM_IFC_TXI(2, 0)
+#define SIM_IFC_TB3   SIM_IFC_TXI(2, 1)
+#define SIM_IFC_TC3   SIM_IFC_TXI(2, 2)
+#define SIM_IFC_TD3   SIM_IFC_TXI(2, 3)
+#define SIM_IFC_TA4   SIM_IFC_TXI(3, 0)
+#define SIM_IFC_TB4   SIM_IFC_TXI(3, 1)
+#define SIM_IFC_TC4   SIM_IFC_TXI(3, 2)
+#define SIM_IFC_TD4   SIM_IFC_TXI(3, 3)
+
+/* ATR and OPS states */
+#define SIM_STATE_REMOVED              0
+#define SIM_STATE_OPERATIONAL_IDLE     1
+#define SIM_STATE_OPERATIONAL_COMMAND  2
+#define SIM_STATE_OPERATIONAL_RESPONSE 3
+#define SIM_STATE_OPERATIONAL_STATUS1  4
+#define SIM_STATE_OPERATIONAL_STATUS2  5
+#define SIM_STATE_OPERATIONAL_PTS      6
+#define SIM_STATE_DETECTED_ATR_T0       7
+#define SIM_STATE_DETECTED_ATR_TS       8
+#define SIM_STATE_DETECTED_ATR_TXI      9
+#define SIM_STATE_DETECTED_ATR_THB      10
+#define SIM_STATE_DETECTED_ATR_TCK      11
+
+/* Definitions of the offset of the SIM hardware registers */
+#define PORT1_CNTL 	0x00	/* 00 */
+#define SETUP 		0x04	/* 04 */
+#define PORT1_DETECT 	0x08	/* 08 */
+#define PORT1_XMT_BUF 	0x0C	/* 0c */
+#define PORT1_RCV_BUF 	0x10	/* 10 */
+#define PORT0_CNTL 	0x14	/* 14 */
+#define CNTL 		0x18	/* 18 */
+#define CLK_PRESCALER 	0x1C	/* 1c */
+#define RCV_THRESHOLD 	0x20	/* 20 */
+#define ENABLE 		0x24	/* 24 */
+#define XMT_STATUS 	0x28	/* 28 */
+#define RCV_STATUS 	0x2C	/* 2c */
+#define INT_MASK 	0x30	/* 30 */
+#define PORTO_XMT_BUF 	0x34	/* 34 */
+#define PORT0_RCV_BUF 	0x38	/* 38 */
+#define PORT0_DETECT 	0x3C	/* 3c */
+#define DATA_FORMAT 	0x40	/* 40 */
+#define XMT_THRESHOLD 	0x44	/* 44 */
+#define GUARD_CNTL 	0x48	/* 48 */
+#define OD_CONFIG 	0x4C	/* 4c */
+#define RESET_CNTL 	0x50	/* 50 */
+#define CHAR_WAIT 	0x54	/* 54 */
+#define GPCNT 		0x58	/* 58 */
+#define DIVISOR 	0x5C	/* 5c */
+#define BWT 		0x60	/* 60 */
+#define BGT 		0x64	/* 64 */
+#define BWT_H 		0x68	/* 68 */
+#define XMT_FIFO_STAT 	0x6C	/* 6c */
+#define RCV_FIFO_CNT 	0x70	/* 70 */
+#define RCV_FIFO_WPTR 	0x74	/* 74 */
+#define RCV_FIFO_RPTR 	0x78	/* 78 */
+
+/* SIM port[0|1]_cntl register bits */
+#define SIM_PORT_CNTL_SFPD   (1<<7)
+#define SIM_PORT_CNTL_3VOLT  (1<<6)
+#define SIM_PORT_CNTL_SCSP   (1<<5)
+#define SIM_PORT_CNTL_SCEN   (1<<4)
+#define SIM_PORT_CNTL_SRST   (1<<3)
+#define SIM_PORT_CNTL_STEN   (1<<2)
+#define SIM_PORT_CNTL_SVEN   (1<<1)
+#define SIM_PORT_CNTL_SAPD   (1<<0)
+
+/* SIM od_config register bits */
+#define SIM_OD_CONFIG_OD_P1  (1<<1)
+#define SIM_OD_CONFIG_OD_P0  (1<<0)
+
+/* SIM enable register bits */
+#define SIM_ENABLE_XMTEN     (1<<1)
+#define SIM_ENABLE_RCVEN     (1<<0)
+
+/* SIM int_mask register bits */
+#define SIM_INT_MASK_RFEM    (1<<13)
+#define SIM_INT_MASK_BGTM    (1<<12)
+#define SIM_INT_MASK_BWTM    (1<<11)
+#define SIM_INT_MASK_RTM     (1<<10)
+#define SIM_INT_MASK_CWTM    (1<<9)
+#define SIM_INT_MASK_GPCM    (1<<8)
+#define SIM_INT_MASK_TDTFM   (1<<7)
+#define SIM_INT_MASK_TFOM    (1<<6)
+#define SIM_INT_MASK_XTM     (1<<5)
+#define SIM_INT_MASK_TFEIM   (1<<4)
+#define SIM_INT_MASK_ETCIM   (1<<3)
+#define SIM_INT_MASK_OIM     (1<<2)
+#define SIM_INT_MASK_TCIM    (1<<1)
+#define SIM_INT_MASK_RIM     (1<<0)
+
+/* SIM xmt_status register bits */
+#define SIM_XMT_STATUS_GPCNT (1<<8)
+#define SIM_XMT_STATUS_TDTF  (1<<7)
+#define SIM_XMT_STATUS_TFO   (1<<6)
+#define SIM_XMT_STATUS_TC    (1<<5)
+#define SIM_XMT_STATUS_ETC   (1<<4)
+#define SIM_XMT_STATUS_TFE   (1<<3)
+#define SIM_XMT_STATUS_XTE   (1<<0)
+
+/* SIM rcv_status register bits */
+#define SIM_RCV_STATUS_BGT   (1<<11)
+#define SIM_RCV_STATUS_BWT   (1<<10)
+#define SIM_RCV_STATUS_RTE   (1<<9)
+#define SIM_RCV_STATUS_CWT   (1<<8)
+#define SIM_RCV_STATUS_CRCOK (1<<7)
+#define SIM_RCV_STATUS_LRCOK (1<<6)
+#define SIM_RCV_STATUS_RDRF  (1<<5)
+#define SIM_RCV_STATUS_RFD   (1<<4)
+#define SIM_RCV_STATUS_RFE   (1<<1)
+#define SIM_RCV_STATUS_OEF   (1<<0)
+
+/* SIM cntl register bits */
+#define SIM_CNTL_BWTEN       (1<<15)
+#define SIM_CNTL_XMT_CRC_LRC (1<<14)
+#define SIM_CNTL_CRCEN       (1<<13)
+#define SIM_CNTL_LRCEN       (1<<12)
+#define SIM_CNTL_CWTEN       (1<<11)
+#define SIM_CNTL_SAMPLE12    (1<<4)
+#define SIM_CNTL_ONACK       (1<<3)
+#define SIM_CNTL_ANACK       (1<<2)
+#define SIM_CNTL_ICM         (1<<1)
+#define SIM_CNTL_GPCNT_CLK_SEL(x)   ((x&0x03)<<9)
+#define SIM_CNTL_GPCNT_CLK_SEL_MASK     (0x03<<9)
+#define SIM_CNTL_BAUD_SEL(x)        ((x&0x07)<<6)
+#define SIM_CNTL_BAUD_SEL_MASK          (0x07<<6)
+
+/* SIM rcv_threshold register bits */
+#define SIM_RCV_THRESHOLD_RTH(x)    ((x&0x0f)<<9)
+#define SIM_RCV_THRESHOLD_RTH_MASK      (0x0f<<9)
+#define SIM_RCV_THRESHOLD_RDT(x)   ((x&0x1ff)<<0)
+#define SIM_RCV_THRESHOLD_RDT_MASK     (0x1ff<<0)
+
+/* SIM xmt_threshold register bits */
+#define SIM_XMT_THRESHOLD_XTH(x)    ((x&0x0f)<<4)
+#define SIM_XMT_THRESHOLD_XTH_MASK      (0x0f<<4)
+#define SIM_XMT_THRESHOLD_TDT(x)    ((x&0x0f)<<0)
+#define SIM_XMT_THRESHOLD_TDT_MASK      (0x0f<<0)
+
+/* SIM guard_cntl register bits */
+#define SIM_GUARD_CNTL_RCVR11              (1<<8)
+#define SIM_GIARD_CNTL_GETU(x)           (x&0xff)
+#define SIM_GIARD_CNTL_GETU_MASK           (0xff)
+
+/* SIM port[0|]_detect register bits */
+#define SIM_PORT_DETECT_SPDS  (1<<3)
+#define SIM_PORT_DETECT_SPDP  (1<<2)
+#define SIM_PORT_DETECT_SDI   (1<<1)
+#define SIM_PORT_DETECT_SDIM  (1<<0)
+
+/* END of REGS definitions */
+
+/* ATR parser data (the parser state is stored in the main device structure) */
+typedef struct {
+	uint8_t T0;		/* ATR T0 */
+	uint8_t TS;		/* ATR TS */
+	/* ATR TA1, TB1, TC1, TD1, TB1, ... , TD4 */
+	uint8_t TXI[16];
+	uint8_t THB[15];	/* ATR historical bytes */
+	uint8_t TCK;		/* ATR checksum */
+	uint16_t ifc_valid;	/* valid interface characters */
+	uint8_t ifc_current_valid;	/* calid ifcs in the current batch */
+	uint8_t cnt;		/* number of current batch */
+	uint8_t num_hb;		/* number of historical bytes */
+} sim_atrparser_t;
+
+/* Main SIM driver structure */
+typedef struct {
+	/* card inserted = 1, ATR received = 2, card removed = 0 */
+	int present;
+	/* current ATR or OPS state */
+	int state;
+	/* current power state */
+	int power;
+	/* error code occured during transfer */
+	int errval;
+	struct clk *clk;	/* Clock id */
+	uint8_t clk_flag;
+	struct resource *res;	/* IO map memory */
+	void __iomem *ioaddr;	/* Mapped address */
+	int ipb_irq;		/* sim ipb IRQ num */
+	int dat_irq;		/* sim dat IRQ num */
+	/* parser for incoming ATR stream */
+	sim_atrparser_t atrparser;
+	/* raw ATR stream received */
+	sim_atr_t atr;
+	/* communication parameters according to ATR */
+	sim_param_t param_atr;
+	/* current communication parameters */
+	sim_param_t param;
+	/* current TPDU or PTS transfer */
+	sim_xfer_t xfer;
+	/* transfer is on the way = 1, idle = 2 */
+	int xfer_ongoing;
+	/* remaining bytes to transmit for the current transfer */
+	int xmt_remaining;
+	/* transmit position */
+	int xmt_pos;
+	/* receive position / number of bytes received */
+	int rcv_count;
+	uint8_t rcv_buffer[SIM_RCV_BUFFER_SIZE];
+	uint8_t xmt_buffer[SIM_XMT_BUFFER_SIZE];
+	/* transfer completion notifier */
+	struct completion xfer_done;
+	/* async notifier for card and ATR detection */
+	struct fasync_struct *fasync;
+	/* Platform specific data */
+	struct mxc_sim_platform_data *plat_data;
+} sim_t;
+
+static int sim_param_F[] = {
+	SIM_INTERNAL_CLK, 372, 558, 744, 1116, 1488, 1860, SIM_RFU,
+	SIM_RFU, 512, 768, 1024, 1536, 2048, SIM_RFU, SIM_RFU
+};
+
+static int sim_param_D[] = {
+	SIM_RFU, 64 * 1, 64 * 2, 64 * 4, 64 * 8, 64 * 16, SIM_RFU, SIM_RFU,
+	SIM_RFU, SIM_RFU, 64 * 1 / 2, 64 * 1 / 4, 64 * 1 / 8, 64 * 1 / 16,
+	64 * 1 / 32, 64 * 1 / 64
+};
+
+static struct miscdevice sim_dev;
+
+/* Function: sim_calc_param
+ *
+ * Description: determine register values depending on communication parameters
+ *
+ * Parameters:
+ * uint32_t fi             ATR frequency multiplier index
+ * uint32_t di             ATR frequency divider index
+ * uint32_t* ptr_divisor   location to store divisor result
+ * uint32_t* ptr_sample12  location to store sample12 result
+ *
+ * Return Values:
+ *  SIM_OK                          calculation finished without errors
+ * -SIM_E_PARAM_DIVISOR_RANGE       calculated divisor > 255
+ * -SIM_E_PARAM_FBYD_NOTDIVBY8OR12  F/D not divisable by 12 (as required)
+ * -SIM_E_PARAM_FBYD_WITHFRACTION   F/D has a remainder
+ * -SIM_E_PARAM_DI_INVALID          frequency multiplyer index not supported
+ * -SIM_E_PARAM_FI_INVALID          frequency divider index not supported
+ */
+
+static int sim_calc_param(uint32_t fi, uint32_t di, uint32_t *ptr_divisor,
+			  uint32_t *ptr_sample12)
+{
+	int32_t errval = SIM_OK;
+	int32_t f = sim_param_F[fi];
+	int32_t d = sim_param_D[di];
+	int32_t stage2_fra = (64 * f) % d;
+	int32_t stage2_div = (64 * f) / d;
+	uint32_t sample12 = 1;
+	uint32_t divisor = 31;
+
+	pr_debug("%s entering.\n", __func__);
+	if ((f > 0) || (d > 0)) {
+		if (stage2_fra == 0) {
+			if ((stage2_div % 12) == 0) {
+				sample12 = 1;
+				divisor = stage2_div / 12;
+			} else if ((stage2_div % 8) == 0) {
+				sample12 = 0;
+				divisor = stage2_div / 8;
+			} else
+				sample12 = -1;
+			if (sample12 >= 0) {
+				if (divisor < 256) {
+					pr_debug("fi=%i", fi);
+					pr_debug("di=%i", di);
+					pr_debug("f=%i", f);
+					pr_debug("d=%i/64", d);
+					pr_debug("div=%i", stage2_div);
+					pr_debug("divisor=%i", divisor);
+					pr_debug("sample12=%i\n", sample12);
+
+					*ptr_divisor = divisor;
+					*ptr_sample12 = sample12;
+					errval = SIM_OK;
+				} else
+					errval = -SIM_E_PARAM_DIVISOR_RANGE;
+			} else
+				errval = -SIM_E_PARAM_FBYD_NOTDIVBY8OR12;
+		} else
+			errval = -SIM_E_PARAM_FBYD_WITHFRACTION;
+	} else
+		errval = -SIM_E_PARAM_FI_INVALID;
+
+	return errval;
+};
+
+/* Function: sim_set_param
+ *
+ * Description: apply communication parameters (setup devisor and sample12)
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ * sim_param_t* param      pointer to communication parameters
+ *
+ * Return Values:
+ * see function sim_calc_param
+ */
+
+static int sim_set_param(sim_t *sim, sim_param_t *param)
+{
+	uint32_t divisor, sample12, reg_data;
+	int errval;
+
+	pr_debug("%s entering.\n", __func__);
+	errval = sim_calc_param(param->FI, param->DI, &divisor, &sample12);
+	if (errval == SIM_OK) {
+		__raw_writel(divisor, sim->ioaddr + DIVISOR);
+		if (sample12) {
+			reg_data = __raw_readl(sim->ioaddr + CNTL);
+			reg_data |= SIM_CNTL_SAMPLE12;
+			__raw_writel(reg_data, sim->ioaddr + CNTL);
+		} else {
+			reg_data = __raw_readl(sim->ioaddr + CNTL);
+			reg_data &= ~SIM_CNTL_SAMPLE12;
+			__raw_writel(reg_data, sim->ioaddr + CNTL);
+		}
+	}
+
+	return errval;
+};
+
+/* Function: sim_atr_received
+ *
+ * Description: this function is called whenever a valid ATR has been received.
+ * It determines the communication parameters from the ATR received and notifies
+ * the user space application with SIGIO.
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static void sim_atr_received(sim_t *sim)
+{
+	sim_param_t param_default = SIM_PARAM_DEFAULT;
+	sim->param_atr = param_default;
+
+	pr_debug("%s entering.\n", __func__);
+	if (sim->atrparser.ifc_valid & (1 << (SIM_IFC_TA1))) {
+		sim->param_atr.FI = sim->atrparser.TXI[SIM_IFC_TA1] >> 4;
+		sim->param_atr.DI = sim->atrparser.TXI[SIM_IFC_TA1] & 0x0f;
+	}
+	if (sim->atrparser.ifc_valid & (1 << (SIM_IFC_TB1))) {
+		sim->param_atr.PI1 = (sim->atrparser.TXI[SIM_IFC_TB1] >> 4)
+		    & 0x07;
+		sim->param_atr.II = sim->atrparser.TXI[SIM_IFC_TB1] & 0x07f;
+	}
+	if (sim->atrparser.ifc_valid & (1 << (SIM_IFC_TC1)))
+		sim->param_atr.N = sim->atrparser.TXI[SIM_IFC_TC1];
+
+	if (sim->atrparser.ifc_valid & (1 << (SIM_IFC_TD1)))
+		sim->param_atr.T = sim->atrparser.TXI[SIM_IFC_TD1] & 0x0f;
+
+	if (sim->atrparser.ifc_valid & (1 << (SIM_IFC_TB2)))
+		sim->param_atr.PI2 = sim->atrparser.TXI[SIM_IFC_TB2];
+
+	if (sim->atrparser.ifc_valid & (1 << (SIM_IFC_TC2)))
+		sim->param_atr.WWT = sim->atrparser.TXI[SIM_IFC_TC2];
+
+	if (sim->fasync)
+		kill_fasync(&sim->fasync, SIGIO, POLL_IN);
+
+};
+
+/* Function: sim_xmt_fill
+ *
+ * Description: fill the transmit FIFO until the FIFO is full or
+ * the end of the transmission has been reached.
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static void sim_xmt_fill(sim_t *sim)
+{
+	uint32_t reg_data;
+	int bytesleft;
+
+	reg_data = __raw_readl(sim->ioaddr + XMT_FIFO_STAT);
+	bytesleft = 16 - ((reg_data >> 8) & 0x0f);
+
+	pr_debug("txfill: remaining=%i bytesleft=%i\n",
+		 sim->xmt_remaining, bytesleft);
+	if (bytesleft > sim->xmt_remaining)
+		bytesleft = sim->xmt_remaining;
+
+	sim->xmt_remaining -= bytesleft;
+	for (; bytesleft > 0; bytesleft--) {
+		__raw_writel(sim->xmt_buffer[sim->xmt_pos],
+			     sim->ioaddr + PORT1_XMT_BUF);
+		sim->xmt_pos++;
+	};
+/* FIXME: optimization - keep filling until fifo full */
+};
+
+/* Function: sim_xmt_start
+ *
+ * Description: initiate a transfer
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ * int pos                 position in the xfer transmit buffer
+ * int count               number of bytes to be transmitted
+ */
+
+static void sim_xmt_start(sim_t *sim, int pos, int count)
+{
+	uint32_t reg_data;
+
+	pr_debug("tx\n");
+	sim->xmt_remaining = count;
+	sim->xmt_pos = pos;
+	sim_xmt_fill(sim);
+
+	if (sim->xmt_remaining) {
+		reg_data = __raw_readl(sim->ioaddr + INT_MASK);
+		reg_data &= ~SIM_INT_MASK_TDTFM;
+		__raw_writel(reg_data, sim->ioaddr + INT_MASK);
+	} else {
+		reg_data = __raw_readl(sim->ioaddr + INT_MASK);
+		reg_data &= ~SIM_INT_MASK_TCIM;
+		__raw_writel(reg_data, sim->ioaddr + INT_MASK);
+		__raw_writel(SIM_XMT_STATUS_TC | SIM_XMT_STATUS_TDTF,
+			     sim->ioaddr + XMT_STATUS);
+		reg_data = __raw_readl(sim->ioaddr + ENABLE);
+		reg_data |= SIM_ENABLE_XMTEN;
+		__raw_writel(reg_data, sim->ioaddr + ENABLE);
+	}
+};
+
+/* Function: sim_atr_add
+ *
+ * Description: add a byte to the raw ATR string
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ * uint8_t data            byte to be added
+ */
+
+static void sim_atr_add(sim_t *sim, uint8_t data)
+{
+	pr_debug("%s entering.\n", __func__);
+	if (sim->atr.size < SIM_ATR_LENGTH_MAX)
+		sim->atr.t[sim->atr.size++] = data;
+	else
+		printk(KERN_ERR "sim.c: ATR received is too big!\n");
+};
+
+/* Function: sim_fsm
+ *
+ * Description: main finite state machine running in ISR context.
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ * uint8_t data            byte received
+ */
+
+static void sim_fsm(sim_t *sim, uint16_t data)
+{
+	uint32_t temp, i = 0;
+	switch (sim->state) {
+
+		pr_debug("%s stat is %d \n", __func__, sim->state);
+		/* OPS FSM */
+
+	case SIM_STATE_OPERATIONAL_IDLE:
+		printk(KERN_INFO "data received unexpectidly (%04x)\n", data);
+		break;
+
+	case SIM_STATE_OPERATIONAL_COMMAND:
+		if (data == sim->xmt_buffer[1]) {
+			if (sim->xfer.rcv_length) {
+				sim->state = SIM_STATE_OPERATIONAL_RESPONSE;
+			} else {
+				sim->state = SIM_STATE_OPERATIONAL_STATUS1;
+				if (sim->xfer.xmt_length > 5)
+					sim_xmt_start(sim, 5,
+						      sim->xfer.xmt_length - 5);
+			};
+		} else if (((data & 0xf0) == 0x60) | ((data & 0xf0) == 0x90)) {
+			sim->xfer.sw1 = data;
+			sim->state = SIM_STATE_OPERATIONAL_STATUS2;
+		} else {
+			sim->errval = -SIM_E_NACK;
+			complete(&sim->xfer_done);
+		};
+		break;
+
+	case SIM_STATE_OPERATIONAL_RESPONSE:
+		sim->rcv_buffer[sim->rcv_count] = data;
+		sim->rcv_count++;
+		if (sim->rcv_count == sim->xfer.rcv_length)
+			sim->state = SIM_STATE_OPERATIONAL_STATUS1;
+		break;
+
+	case SIM_STATE_OPERATIONAL_STATUS1:
+		sim->xfer.sw1 = data;
+		sim->state = SIM_STATE_OPERATIONAL_STATUS2;
+		break;
+
+	case SIM_STATE_OPERATIONAL_STATUS2:
+		sim->xfer.sw2 = data;
+		sim->state = SIM_STATE_OPERATIONAL_IDLE;
+		complete(&sim->xfer_done);
+		break;
+
+	case SIM_STATE_OPERATIONAL_PTS:
+		sim->rcv_buffer[sim->rcv_count] = data;
+		sim->rcv_count++;
+		if (sim->rcv_count == sim->xfer.rcv_length)
+			sim->state = SIM_STATE_OPERATIONAL_IDLE;
+		break;
+
+		/* ATR FSM */
+
+	case SIM_STATE_DETECTED_ATR_T0:
+		sim_atr_add(sim, data);
+		pr_debug("T0 %02x\n", data);
+		sim->atrparser.T0 = data;
+		sim->state = SIM_STATE_DETECTED_ATR_TS;
+		break;
+
+	case SIM_STATE_DETECTED_ATR_TS:
+		sim_atr_add(sim, data);
+		pr_debug("TS %02x\n", data);
+		sim->atrparser.TS = data;
+		if (data & 0xf0) {
+			sim->atrparser.ifc_current_valid = (data >> 4) & 0x0f;
+			sim->atrparser.num_hb = data & 0x0f;
+			sim->atrparser.ifc_valid = 0;
+			sim->state = SIM_STATE_DETECTED_ATR_TXI;
+			sim->atrparser.cnt = 0;
+		} else {
+			goto sim_fsm_atr_thb;
+		};
+		break;
+
+	case SIM_STATE_DETECTED_ATR_TXI:
+		sim_atr_add(sim, data);
+		i = ffs(sim->atrparser.ifc_current_valid) - 1;
+		pr_debug("T%c%i %02x\n", 'A' + i, sim->atrparser.cnt + 1, data);
+		sim->atrparser.TXI[SIM_IFC_TXI(i, sim->atrparser.cnt)] = data;
+		sim->atrparser.ifc_valid |= 1 << SIM_IFC_TXI(i,
+							     sim->atrparser.
+							     cnt);
+		sim->atrparser.ifc_current_valid &= ~(1 << i);
+
+		if (sim->atrparser.ifc_current_valid == 0) {
+			if (i == 3) {
+				sim->atrparser.ifc_current_valid = (data >> 4)
+				    & 0x0f;
+				sim->atrparser.cnt++;
+
+				if (sim->atrparser.cnt >= 4) {
+					/* error */
+					printk(KERN_ERR "ERROR !\n");
+					break;
+				};
+
+				if (sim->atrparser.ifc_current_valid == 0)
+					goto sim_fsm_atr_thb;
+			} else {
+sim_fsm_atr_thb:
+				if (sim->atrparser.num_hb) {
+					sim->state = SIM_STATE_DETECTED_ATR_THB;
+					sim->atrparser.cnt = 0;
+				} else {
+					goto sim_fsm_atr_tck;
+				};
+			};
+		};
+		break;
+
+	case SIM_STATE_DETECTED_ATR_THB:
+		sim_atr_add(sim, data);
+		pr_debug("THB%i %02x\n", i, data);
+		sim->atrparser.THB[sim->atrparser.cnt] = data;
+		sim->atrparser.cnt++;
+
+		if (sim->atrparser.cnt == sim->atrparser.num_hb) {
+sim_fsm_atr_tck:
+			i = sim->atrparser.ifc_valid & (1 << (SIM_IFC_TD1));
+			temp = sim->atrparser.TXI[SIM_IFC_TD1] & 0x0f;
+			if ((i && temp) == SIM_PROTOCOL_T1)
+				sim->state = SIM_STATE_DETECTED_ATR_TCK;
+			else
+				goto sim_fsm_atr_received;
+		};
+		break;
+
+	case SIM_STATE_DETECTED_ATR_TCK:
+		sim_atr_add(sim, data);
+		/* checksum not required for T=0 */
+		sim->atrparser.TCK = data;
+sim_fsm_atr_received:
+		sim->state = SIM_STATE_OPERATIONAL_IDLE;
+		sim->present = SIM_PRESENT_OPERATIONAL;
+		sim_atr_received(sim);
+		break;
+	};
+};
+
+/* Function: sim_irq_handler
+ *
+ * Description: interrupt service routine.
+ *
+ * Parameters:
+ * int irq                 interrupt number
+ * void *dev_id            pointer to SIM device handler
+ *
+ * Return values:
+ * IRQ_HANDLED             OS specific
+ */
+
+static irqreturn_t sim_irq_handler(int irq, void *dev_id)
+{
+	uint32_t reg_data, reg_data0, reg_data1;
+
+	sim_t *sim = (sim_t *) dev_id;
+
+	pr_debug("%s entering\n", __func__);
+
+	reg_data0 = __raw_readl(sim->ioaddr + XMT_STATUS);
+	reg_data1 = __raw_readl(sim->ioaddr + INT_MASK);
+	if ((reg_data0 & SIM_XMT_STATUS_TC)
+	    && (!(reg_data1 & SIM_INT_MASK_TCIM))) {
+		pr_debug("TC_IRQ\n");
+		__raw_writel(SIM_XMT_STATUS_TC, sim->ioaddr + XMT_STATUS);
+		reg_data = __raw_readl(sim->ioaddr + INT_MASK);
+		reg_data |= SIM_INT_MASK_TCIM;
+		__raw_writel(reg_data, sim->ioaddr + INT_MASK);
+		reg_data = __raw_readl(sim->ioaddr + ENABLE);
+		reg_data &= ~SIM_ENABLE_XMTEN;
+		__raw_writel(reg_data, sim->ioaddr + ENABLE);
+	};
+
+	reg_data0 = __raw_readl(sim->ioaddr + XMT_STATUS);
+	reg_data1 = __raw_readl(sim->ioaddr + INT_MASK);
+	if ((reg_data0 & SIM_XMT_STATUS_TDTF)
+	    && (!(reg_data1 & SIM_INT_MASK_TDTFM))) {
+		pr_debug("TDTF_IRQ\n");
+		__raw_writel(SIM_XMT_STATUS_TDTF, sim->ioaddr + XMT_STATUS);
+		sim_xmt_fill(sim);
+
+		if (sim->xmt_remaining == 0) {
+			__raw_writel(SIM_XMT_STATUS_TC,
+				     sim->ioaddr + XMT_STATUS);
+			reg_data = __raw_readl(sim->ioaddr + INT_MASK);
+			reg_data &= ~SIM_INT_MASK_TCIM;
+			reg_data |= SIM_INT_MASK_TDTFM;
+			__raw_writel(reg_data, sim->ioaddr + INT_MASK);
+		};
+	};
+
+	reg_data0 = __raw_readl(sim->ioaddr + RCV_STATUS);
+	reg_data1 = __raw_readl(sim->ioaddr + INT_MASK);
+	if ((reg_data0 & SIM_RCV_STATUS_RDRF)
+	    && (!(reg_data1 & SIM_INT_MASK_RIM))) {
+		pr_debug("%s RDRF_IRQ\n", __func__);
+		__raw_writel(SIM_RCV_STATUS_RDRF, sim->ioaddr + RCV_STATUS);
+
+		while (__raw_readl(sim->ioaddr + RCV_FIFO_CNT)) {
+			uint32_t data;
+			data = __raw_readl(sim->ioaddr + PORT1_RCV_BUF);
+			pr_debug("RX = %02x state = %i\n", data, sim->state);
+			if (data & 0x700) {
+				if (sim->xfer_ongoing) {
+					/* error */
+					printk(KERN_ERR "ERROR !\n");
+					return IRQ_HANDLED;
+				};
+			} else
+				sim_fsm(sim, data);
+		};
+	};
+
+	reg_data0 = __raw_readl(sim->ioaddr + PORT0_DETECT);
+	if (reg_data0 & SIM_PORT_DETECT_SDI) {
+		pr_debug("%s PD_IRQ\n", __func__);
+		reg_data = __raw_readl(sim->ioaddr + PORT0_DETECT);
+		reg_data |= SIM_PORT_DETECT_SDI;
+		__raw_writel(reg_data, sim->ioaddr + PORT0_DETECT);
+
+		reg_data0 = __raw_readl(sim->ioaddr + PORT0_DETECT);
+		if (reg_data0 & SIM_PORT_DETECT_SPDP) {
+			reg_data = __raw_readl(sim->ioaddr + PORT0_DETECT);
+			reg_data &= ~SIM_PORT_DETECT_SPDS;
+			__raw_writel(reg_data, sim->ioaddr + PORT0_DETECT);
+
+			if (sim->present != SIM_PRESENT_REMOVED) {
+				pr_debug("Removed sim card\n");
+				sim->present = SIM_PRESENT_REMOVED;
+				sim->state = SIM_STATE_REMOVED;
+
+				if (sim->fasync)
+					kill_fasync(&sim->fasync,
+						    SIGIO, POLL_IN);
+			};
+		} else {
+			reg_data = __raw_readl(sim->ioaddr + PORT0_DETECT);
+			reg_data |= SIM_PORT_DETECT_SPDS;
+			__raw_writel(reg_data, sim->ioaddr + PORT0_DETECT);
+
+			if (sim->present == SIM_PRESENT_REMOVED) {
+				pr_debug("Inserted sim card\n");
+				sim->state = SIM_STATE_DETECTED_ATR_T0;
+				sim->present = SIM_PRESENT_DETECTED;
+
+				if (sim->fasync)
+					kill_fasync(&sim->fasync,
+						    SIGIO, POLL_IN);
+			};
+		};
+	};
+
+	return IRQ_HANDLED;
+};
+
+/* Function: sim_power_on
+ *
+ * Description: run the power on sequence
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static void sim_power_on(sim_t *sim)
+{
+	uint32_t reg_data;
+
+	/* power on sequence */
+	pr_debug("%s Powering on the sim port.\n", __func__);
+	reg_data = __raw_readl(sim->ioaddr + PORT0_CNTL);
+	reg_data |= SIM_PORT_CNTL_SVEN;
+	__raw_writel(reg_data, sim->ioaddr + PORT0_CNTL);
+	msleep(10);
+	reg_data = __raw_readl(sim->ioaddr + PORT0_CNTL);
+	reg_data |= SIM_PORT_CNTL_SCEN;
+	__raw_writel(reg_data, sim->ioaddr + PORT0_CNTL);
+	msleep(10);
+	reg_data = SIM_RCV_THRESHOLD_RTH(0) | SIM_RCV_THRESHOLD_RDT(1);
+	__raw_writel(reg_data, sim->ioaddr + RCV_THRESHOLD);
+	__raw_writel(SIM_RCV_STATUS_RDRF, sim->ioaddr + RCV_STATUS);
+	reg_data = __raw_readl(sim->ioaddr + INT_MASK);
+	reg_data &= ~SIM_INT_MASK_RIM;
+	__raw_writel(reg_data, sim->ioaddr + INT_MASK);
+	__raw_writel(31, sim->ioaddr + DIVISOR);
+	reg_data = __raw_readl(sim->ioaddr + CNTL);
+	reg_data |= SIM_CNTL_SAMPLE12;
+	__raw_writel(reg_data, sim->ioaddr + CNTL);
+	reg_data = __raw_readl(sim->ioaddr + ENABLE);
+	reg_data |= SIM_ENABLE_RCVEN;
+	__raw_writel(reg_data, sim->ioaddr + ENABLE);
+	reg_data = __raw_readl(sim->ioaddr + PORT0_CNTL);
+	reg_data |= SIM_PORT_CNTL_SRST;
+	__raw_writel(reg_data, sim->ioaddr + PORT0_CNTL);
+	pr_debug("%s port0_ctl is 0x%x.\n", __func__,
+		 __raw_readl(sim->ioaddr + PORT0_CNTL));
+	sim->power = SIM_POWER_ON;
+};
+
+/* Function: sim_power_off
+ *
+ * Description: run the power off sequence
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static void sim_power_off(sim_t *sim)
+{
+	uint32_t reg_data;
+
+	pr_debug("%s entering.\n", __func__);
+	/* sim_power_off sequence */
+	reg_data = __raw_readl(sim->ioaddr + PORT0_CNTL);
+	reg_data &= ~SIM_PORT_CNTL_SCEN;
+	__raw_writel(reg_data, sim->ioaddr + PORT0_CNTL);
+	reg_data = __raw_readl(sim->ioaddr + ENABLE);
+	reg_data &= ~SIM_ENABLE_RCVEN;
+	__raw_writel(reg_data, sim->ioaddr + ENABLE);
+	reg_data = __raw_readl(sim->ioaddr + INT_MASK);
+	reg_data |= SIM_INT_MASK_RIM;
+	__raw_writel(reg_data, sim->ioaddr + INT_MASK);
+	__raw_writel(0, sim->ioaddr + RCV_THRESHOLD);
+	reg_data = __raw_readl(sim->ioaddr + PORT0_CNTL);
+	reg_data &= ~SIM_PORT_CNTL_SRST;
+	__raw_writel(reg_data, sim->ioaddr + PORT0_CNTL);
+	reg_data = __raw_readl(sim->ioaddr + PORT0_CNTL);
+	reg_data &= ~SIM_PORT_CNTL_SVEN;
+	__raw_writel(reg_data, sim->ioaddr + PORT0_CNTL);
+	sim->power = SIM_POWER_OFF;
+};
+
+/* Function: sim_start
+ *
+ * Description: ramp up the SIM interface
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static void sim_start(sim_t *sim)
+{
+	uint32_t reg_data, clk_rate, clk_div = 0;
+
+	pr_debug("%s entering.\n", __func__);
+	/* Configuring SIM for Operation */
+	reg_data = SIM_XMT_THRESHOLD_XTH(0) | SIM_XMT_THRESHOLD_TDT(4);
+	__raw_writel(reg_data, sim->ioaddr + XMT_THRESHOLD);
+	__raw_writel(0, sim->ioaddr + SETUP);
+	/* ~ 4 MHz */
+	clk_rate = clk_get_rate(sim->clk);
+	clk_div = clk_rate / sim->plat_data->clk_rate;
+	if (clk_rate % sim->plat_data->clk_rate)
+		clk_div++;
+	pr_debug("%s prescaler is 0x%x.\n", __func__, clk_div);
+	__raw_writel(clk_div, sim->ioaddr + CLK_PRESCALER);
+
+	reg_data = SIM_CNTL_GPCNT_CLK_SEL(0) | SIM_CNTL_BAUD_SEL(7)
+	    | SIM_CNTL_SAMPLE12 | SIM_CNTL_ANACK | SIM_CNTL_ICM;
+	__raw_writel(reg_data, sim->ioaddr + CNTL);
+	__raw_writel(31, sim->ioaddr + DIVISOR);
+	reg_data = __raw_readl(sim->ioaddr + OD_CONFIG);
+	reg_data |= SIM_OD_CONFIG_OD_P0;
+	__raw_writel(reg_data, sim->ioaddr + OD_CONFIG);
+	reg_data = __raw_readl(sim->ioaddr + PORT0_CNTL);
+	reg_data |= SIM_PORT_CNTL_3VOLT | SIM_PORT_CNTL_STEN;
+	__raw_writel(reg_data, sim->ioaddr + PORT0_CNTL);
+
+	/* presense detect */
+	pr_debug("%s p0_det is 0x%x \n", __func__,
+		 __raw_readl(sim->ioaddr + PORT0_DETECT));
+	if (__raw_readl(sim->ioaddr + PORT0_DETECT) & SIM_PORT_DETECT_SPDP) {
+		pr_debug("%s card removed \n", __func__);
+		reg_data = __raw_readl(sim->ioaddr + PORT0_DETECT);
+		reg_data &= ~SIM_PORT_DETECT_SPDS;
+		__raw_writel(reg_data, sim->ioaddr + PORT0_DETECT);
+		sim->present = SIM_PRESENT_REMOVED;
+		sim->state = SIM_STATE_REMOVED;
+	} else {
+		pr_debug("%s card inserted \n", __func__);
+		reg_data = __raw_readl(sim->ioaddr + PORT0_DETECT);
+		reg_data |= SIM_PORT_DETECT_SPDS;
+		__raw_writel(reg_data, sim->ioaddr + PORT0_DETECT);
+		sim->present = SIM_PRESENT_DETECTED;
+		sim->state = SIM_STATE_DETECTED_ATR_T0;
+	};
+	reg_data = __raw_readl(sim->ioaddr + PORT0_DETECT);
+	reg_data |= SIM_PORT_DETECT_SDI;
+	reg_data &= ~SIM_PORT_DETECT_SDIM;
+	__raw_writel(reg_data, sim->ioaddr + PORT0_DETECT);
+
+	/*
+	 * Since there is no PD0 layout on MX51, assume
+	 * that there is a SIM card in slot defaulty.
+	 * */
+	if (0 == (sim->plat_data->detect)) {
+		reg_data = __raw_readl(sim->ioaddr + PORT0_DETECT);
+		reg_data |= SIM_PORT_DETECT_SPDS;
+		__raw_writel(reg_data, sim->ioaddr + PORT0_DETECT);
+		sim->present = SIM_PRESENT_DETECTED;
+		sim->state = SIM_STATE_DETECTED_ATR_T0;
+	}
+
+	if (sim->present == SIM_PRESENT_DETECTED)
+		sim_power_on(sim);
+
+};
+
+/* Function: sim_stop
+ *
+ * Description: shut down the SIM interface
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static void sim_stop(sim_t *sim)
+{
+	pr_debug("%s entering.\n", __func__);
+	__raw_writel(0, sim->ioaddr + SETUP);
+	__raw_writel(0, sim->ioaddr + ENABLE);
+	__raw_writel(0, sim->ioaddr + PORT0_CNTL);
+	__raw_writel(0x06, sim->ioaddr + CNTL);
+	__raw_writel(0, sim->ioaddr + CLK_PRESCALER);
+	__raw_writel(0, sim->ioaddr + SETUP);
+	__raw_writel(0, sim->ioaddr + OD_CONFIG);
+	__raw_writel(0, sim->ioaddr + XMT_THRESHOLD);
+	__raw_writel(0xb8, sim->ioaddr + XMT_STATUS);
+	__raw_writel(4, sim->ioaddr + RESET_CNTL);
+	mdelay(1);
+};
+
+/* Function: sim_data_reset
+ *
+ * Description: reset a SIM structure to default values
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static void sim_data_reset(sim_t *sim)
+{
+	sim_param_t param_default = SIM_PARAM_DEFAULT;
+	sim->present = SIM_PRESENT_REMOVED;
+	sim->state = SIM_STATE_REMOVED;
+	sim->power = SIM_POWER_OFF;
+	sim->errval = SIM_OK;
+	memset(&sim->atrparser, 0, sizeof(sim->atrparser));
+	memset(&sim->atr, 0, sizeof(sim->atr));
+	sim->param_atr = param_default;
+	memset(&sim->param, 0, sizeof(sim->param));
+	memset(&sim->xfer, 0, sizeof(sim->xfer));
+	sim->xfer_ongoing = 0;
+	sim->xmt_remaining = 0;
+	sim->xmt_pos = 0;
+	sim->rcv_count = 0;
+	memset(sim->rcv_buffer, 0, SIM_RCV_BUFFER_SIZE);
+	memset(sim->xmt_buffer, 0, SIM_XMT_BUFFER_SIZE);
+};
+
+/* Function: sim_cold_reset
+ *
+ * Description: cold reset the SIM interface, including card
+ * power down and interface hardware reset.
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static void sim_cold_reset(sim_t *sim)
+{
+	pr_debug("%s entering.\n", __func__);
+	if (sim->present != SIM_PRESENT_REMOVED) {
+		sim_power_off(sim);
+		sim_stop(sim);
+		sim_data_reset(sim);
+		sim->state = SIM_STATE_DETECTED_ATR_T0;
+		sim->present = SIM_PRESENT_DETECTED;
+		msleep(50);
+		sim_start(sim);
+		sim_power_on(sim);
+	};
+};
+
+/* Function: sim_warm_reset
+ *
+ * Description: warm reset the SIM interface: just invoke the
+ * reset signal and reset the SIM structure for the interface.
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static void sim_warm_reset(sim_t *sim)
+{
+	uint32_t reg_data;
+
+	pr_debug("%s entering.\n", __func__);
+	if (sim->present != SIM_PRESENT_REMOVED) {
+		reg_data = __raw_readl(sim->ioaddr + PORT0_CNTL);
+		reg_data |= SIM_PORT_CNTL_SRST;
+		__raw_writel(reg_data, sim->ioaddr + PORT0_CNTL);
+		sim_data_reset(sim);
+		msleep(50);
+		reg_data = __raw_readl(sim->ioaddr + PORT0_CNTL);
+		reg_data &= ~SIM_PORT_CNTL_SRST;
+		__raw_writel(reg_data, sim->ioaddr + PORT0_CNTL);
+	};
+};
+
+/* Function: sim_card_lock
+ *
+ * Description: physically lock the SIM card.
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static int sim_card_lock(sim_t *sim)
+{
+	int errval;
+
+	pr_debug("%s entering.\n", __func__);
+	/* place holder for true physcial locking */
+	if (sim->present != SIM_PRESENT_REMOVED)
+		errval = SIM_OK;
+	else
+		errval = -SIM_E_NOCARD;
+	return errval;
+};
+
+/* Function: sim_card_eject
+ *
+ * Description: physically unlock and eject the SIM card.
+ *
+ * Parameters:
+ * sim_t* sim              pointer to SIM device handler
+ */
+
+static int sim_card_eject(sim_t *sim)
+{
+	int errval;
+
+	pr_debug("%s entering.\n", __func__);
+	/* place holder for true physcial locking */
+	if (sim->present != SIM_PRESENT_REMOVED)
+		errval = SIM_OK;
+	else
+		errval = -SIM_E_NOCARD;
+	return errval;
+};
+
+/* Function: sim_ioctl
+ *
+ * Description: handle ioctl calls
+ *
+ * Parameters: OS specific
+ */
+
+static int sim_ioctl(struct inode *inode, struct file *file,
+		     unsigned int cmd, unsigned long arg)
+{
+	int ret, errval = SIM_OK;
+	unsigned long timeout;
+
+	sim_t *sim = (sim_t *) file->private_data;
+
+	pr_debug("%s entering.\n", __func__);
+	switch (cmd) {
+		pr_debug("ioctl cmd %d is issued...\n", cmd);
+
+	case SIM_IOCTL_GET_ATR:
+		if (sim->present != SIM_PRESENT_OPERATIONAL) {
+			errval = -SIM_E_NOCARD;
+			break;
+		};
+		ret = copy_to_user((sim_atr_t *) arg, &sim->atr,
+				   sizeof(sim_atr_t));
+		if (ret)
+			errval = -SIM_E_ACCESS;
+		break;
+
+	case SIM_IOCTL_GET_PARAM_ATR:
+		if (sim->present != SIM_PRESENT_OPERATIONAL) {
+			errval = -SIM_E_NOCARD;
+			break;
+		};
+		ret = copy_to_user((sim_param_t *) arg, &sim->param_atr,
+				   sizeof(sim_param_t));
+		if (ret)
+			errval = -SIM_E_ACCESS;
+		break;
+
+	case SIM_IOCTL_GET_PARAM:
+		ret = copy_to_user((sim_param_t *) arg, &sim->param,
+				   sizeof(sim_param_t));
+		if (ret)
+			errval = -SIM_E_ACCESS;
+		break;
+
+	case SIM_IOCTL_SET_PARAM:
+		ret = copy_from_user(&sim->param, (sim_param_t *) arg,
+				     sizeof(sim_param_t));
+		if (ret)
+			errval = -SIM_E_ACCESS;
+		else
+			errval = sim_set_param(sim, &sim->param);
+		break;
+
+	case SIM_IOCTL_POWER_ON:
+		if (sim->power == SIM_POWER_ON) {
+			errval = -SIM_E_POWERED_ON;
+			break;
+		};
+		sim_power_on(sim);
+		break;
+
+	case SIM_IOCTL_POWER_OFF:
+		if (sim->power == SIM_POWER_OFF) {
+			errval = -SIM_E_POWERED_OFF;
+			break;
+		};
+		sim_power_off(sim);
+		break;
+
+	case SIM_IOCTL_COLD_RESET:
+		if (sim->power == SIM_POWER_OFF) {
+			errval = -SIM_E_POWERED_OFF;
+			break;
+		};
+		sim_cold_reset(sim);
+		break;
+
+	case SIM_IOCTL_WARM_RESET:
+		sim_warm_reset(sim);
+		if (sim->power == SIM_POWER_OFF) {
+			errval = -SIM_E_POWERED_OFF;
+			break;
+		};
+		break;
+
+	case SIM_IOCTL_XFER:
+		if (sim->present != SIM_PRESENT_OPERATIONAL) {
+			errval = -SIM_E_NOCARD;
+			break;
+		};
+
+		ret = copy_from_user(&sim->xfer, (sim_xfer_t *) arg,
+				     sizeof(sim_xfer_t));
+		if (ret) {
+			errval = -SIM_E_ACCESS;
+			break;
+		};
+
+		ret = copy_from_user(sim->xmt_buffer, sim->xfer.xmt_buffer,
+				     sim->xfer.xmt_length);
+		if (ret) {
+			errval = -SIM_E_ACCESS;
+			break;
+		};
+
+		sim->rcv_count = 0;
+		sim->xfer.sw1 = 0;
+		sim->xfer.sw2 = 0;
+
+		if (sim->xfer.type == SIM_XFER_TYPE_TPDU) {
+			if (sim->xfer.xmt_length < 5) {
+				errval = -SIM_E_TPDUSHORT;
+				break;
+			}
+			sim->state = SIM_STATE_OPERATIONAL_COMMAND;
+		} else if (sim->xfer.type == SIM_XFER_TYPE_PTS) {
+			if (sim->xfer.xmt_length == 0) {
+				errval = -SIM_E_PTSEMPTY;
+				break;
+			}
+			sim->state = SIM_STATE_OPERATIONAL_PTS;
+		} else {
+			errval = -SIM_E_INVALIDXFERTYPE;
+			break;
+		};
+
+		if (sim->xfer.xmt_length > SIM_XMT_BUFFER_SIZE) {
+			errval = -SIM_E_INVALIDXMTLENGTH;
+			break;
+		};
+
+		if (sim->xfer.rcv_length > SIM_XMT_BUFFER_SIZE) {
+			errval = -SIM_E_INVALIDRCVLENGTH;
+			break;
+		};
+
+		sim->errval = 0;
+		sim->xfer_ongoing = 1;
+		init_completion(&sim->xfer_done);
+		sim_xmt_start(sim, 0, 5);
+		timeout =
+		    wait_for_completion_interruptible_timeout(&sim->xfer_done,
+							      sim->xfer.
+							      timeout);
+		sim->xfer_ongoing = 0;
+
+		if (sim->errval) {
+			errval = sim->errval;
+			break;
+		};
+
+		if (timeout == 0) {
+			errval = -SIM_E_TIMEOUT;
+			break;
+		}
+
+		ret = copy_to_user(sim->xfer.rcv_buffer, sim->rcv_buffer,
+				   sim->xfer.rcv_length);
+		if (ret) {
+			errval = -SIM_E_ACCESS;
+			break;
+		};
+
+		ret = copy_to_user((sim_xfer_t *) arg, &sim->xfer,
+				   sizeof(sim_xfer_t));
+		if (ret)
+			errval = -SIM_E_ACCESS;
+		break;
+
+	case SIM_IOCTL_GET_PRESENSE:
+		if (put_user(sim->present, (int *)arg))
+			errval = -SIM_E_ACCESS;
+		break;
+
+	case SIM_IOCTL_CARD_LOCK:
+		errval = sim_card_lock(sim);
+		break;
+
+	case SIM_IOCTL_CARD_EJECT:
+		errval = sim_card_eject(sim);
+		break;
+
+	};
+
+	return errval;
+};
+
+/* Function: sim_fasync
+ *
+ * Description: async handler
+ *
+ * Parameters: OS specific
+ */
+
+static int sim_fasync(int fd, struct file *file, int mode)
+{
+	sim_t *sim = (sim_t *) file->private_data;
+	pr_debug("%s entering.\n", __func__);
+	return fasync_helper(fd, file, mode, &sim->fasync);
+}
+
+/* Function: sim_open
+ *
+ * Description: ramp up interface when being opened
+ *
+ * Parameters: OS specific
+ */
+
+static int sim_open(struct inode *inode, struct file *file)
+{
+	int errval = SIM_OK;
+
+	sim_t *sim = dev_get_drvdata(sim_dev.parent);
+	file->private_data = sim;
+
+	pr_debug("%s entering.\n", __func__);
+	if (!sim->ioaddr) {
+		errval = -ENOMEM;
+		return errval;
+	}
+
+	if (!(sim->clk_flag)) {
+		pr_debug("\n%s enable the clock\n", __func__);
+		clk_enable(sim->clk);
+		sim->clk_flag = 1;
+	}
+
+	sim_start(sim);
+
+	return errval;
+};
+
+/* Function: sim_release
+ *
+ * Description: shut down interface when being closed
+ *
+ * Parameters: OS specific
+ */
+
+static int sim_release(struct inode *inode, struct file *file)
+{
+	uint32_t reg_data;
+
+	sim_t *sim = (sim_t *) file->private_data;
+
+	pr_debug("%s entering.\n", __func__);
+	if (sim->clk_flag) {
+		pr_debug("\n%s disable the clock\n", __func__);
+		clk_disable(sim->clk);
+		sim->clk_flag = 0;
+	}
+
+	/* disable presense detection */
+	reg_data = __raw_readl(sim->ioaddr + PORT0_DETECT);
+	__raw_writel(reg_data | SIM_PORT_DETECT_SDIM,
+		     sim->ioaddr + PORT0_DETECT);
+
+	if (sim->present != SIM_PRESENT_REMOVED) {
+		sim_power_off(sim);
+		if (sim->fasync)
+			kill_fasync(&sim->fasync, SIGIO, POLL_IN);
+	};
+
+	sim_stop(sim);
+
+	sim_fasync(-1, file, 0);
+
+	pr_debug("exit\n");
+	return 0;
+};
+
+static const struct file_operations sim_fops = {
+	.open = sim_open,
+	.ioctl = sim_ioctl,
+	.fasync = sim_fasync,
+	.release = sim_release
+};
+
+static struct miscdevice sim_dev = {
+	MISC_DYNAMIC_MINOR,
+	"mxc_sim",
+	&sim_fops
+};
+
+/*****************************************************************************\
+ *                                                                           *
+ * Driver init/exit                                                          *
+ *                                                                           *
+\*****************************************************************************/
+
+static int sim_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct mxc_sim_platform_data *sim_plat = pdev->dev.platform_data;
+
+	sim_t *sim = kzalloc(sizeof(sim_t), GFP_KERNEL);
+
+	if (sim == 0) {
+		ret = -ENOMEM;
+		printk(KERN_ERR "Can't get the MEMORY\n");
+		return ret;
+	};
+
+	BUG_ON(pdev == NULL);
+
+	sim->plat_data = sim_plat;
+	sim->clk_flag = 0;
+
+	sim->res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!sim->res) {
+		ret = -ENOMEM;
+		printk(KERN_ERR "Can't get the MEMORY\n");
+		goto out;
+	}
+
+	/* request the sim clk and sim_serial_clk */
+	sim->clk = clk_get(NULL, sim->plat_data->clock_sim);
+	if (IS_ERR(sim->clk)) {
+		ret = PTR_ERR(sim->clk);
+		printk(KERN_ERR "Get CLK ERROR !\n");
+		goto out;
+	}
+	pr_debug("sim clock:%lu\n", clk_get_rate(sim->clk));
+
+	sim->ipb_irq = platform_get_irq(pdev, 0);
+	sim->dat_irq = platform_get_irq(pdev, 1);
+	if (!(sim->ipb_irq | sim->dat_irq)) {
+		ret = -ENOMEM;
+		goto out1;
+	}
+
+	if (!request_mem_region(sim->res->start,
+				sim->res->end -
+				sim->res->start + 1, pdev->name)) {
+		printk(KERN_ERR "request_mem_region failed\n");
+		ret = -ENOMEM;
+		goto out1;
+	}
+
+	sim->ioaddr = (void *)ioremap(sim->res->start, sim->res->end -
+				      sim->res->start + 1);
+	if (sim->ipb_irq)
+		ret = request_irq(sim->ipb_irq, sim_irq_handler,
+				  0, "mxc_sim_ipb", sim);
+	if (sim->dat_irq)
+		ret |= request_irq(sim->dat_irq, sim_irq_handler,
+				   0, "mxc_sim_dat", sim);
+
+	if (ret) {
+		printk(KERN_ERR "Can't get the irq\n");
+		goto out2;
+	};
+
+	platform_set_drvdata(pdev, sim);
+	sim_dev.parent = &(pdev->dev);
+
+	misc_register(&sim_dev);
+
+	return ret;
+out2:
+	if (sim->ipb_irq)
+		free_irq(sim->ipb_irq, sim);
+	if (sim->dat_irq)
+		free_irq(sim->dat_irq, sim);
+	release_mem_region(sim->res->start,
+			   sim->res->end - sim->res->start + 1);
+out1:
+	clk_put(sim->clk);
+out:
+	kfree(sim);
+	return ret;
+}
+
+static int sim_remove(struct platform_device *pdev)
+{
+	sim_t *sim = platform_get_drvdata(pdev);
+
+	clk_put(sim->clk);
+
+	if (sim->ipb_irq)
+		free_irq(sim->ipb_irq, sim);
+	if (sim->dat_irq)
+		free_irq(sim->dat_irq, sim);
+
+	iounmap(sim->ioaddr);
+
+	kfree(sim);
+	release_mem_region(sim->res->start,
+			   sim->res->end - sim->res->start + 1);
+
+	misc_deregister(&sim_dev);
+	return 0;
+}
+
+static struct platform_driver sim_driver = {
+	.driver = {
+		   .name = "mxc_sim",
+		   },
+	.probe = sim_probe,
+	.remove = sim_remove,
+	.suspend = NULL,
+	.resume = NULL,
+};
+
+static int __init sim_drv_init(void)
+{
+	return platform_driver_register(&sim_driver);
+}
+
+static void __exit sim_drv_exit(void)
+{
+	platform_driver_unregister(&sim_driver);
+}
+
+module_init(sim_drv_init);
+module_exit(sim_drv_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC SIM Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/keyboard.c b/drivers/char/keyboard.c
index de26a978fbdd..5e34577309b7 100644
--- a/drivers/char/keyboard.c
+++ b/drivers/char/keyboard.c
@@ -1123,7 +1123,9 @@ static int emulate_raw(struct vc_data *vc, unsigned int keycode,
 
 #define HW_RAW(dev)	0
 
+#if defined CONFIG_INPUT_KEYBOARD
 #warning "Cannot generate rawmode keyboard for your architecture yet."
+#endif
 
 static int emulate_raw(struct vc_data *vc, unsigned int keycode, unsigned char up_flag)
 {
diff --git a/drivers/char/mxc_iim.c b/drivers/char/mxc_iim.c
new file mode 100755
index 000000000000..b407d34759a9
--- /dev/null
+++ b/drivers/char/mxc_iim.c
@@ -0,0 +1,161 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/mm.h>
+#include <linux/clk.h>
+#include <linux/miscdevice.h>
+
+static unsigned long iim_reg_base, iim_reg_end, iim_reg_size;
+static struct clk *iim_clk;
+static struct device *iim_dev;
+
+/*!
+ * MXC IIM interface - memory map function
+ * This function maps 4KB IIM registers from IIM base address.
+ *
+ * @param file	     struct file *
+ * @param vma	     structure vm_area_struct *
+ *
+ * @return	     Return 0 on success or negative error code on error
+ */
+static int mxc_iim_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	/* Remap-pfn-range will mark the range VM_IO and VM_RESERVED */
+	if (remap_pfn_range(vma,
+			    vma->vm_start,
+			    iim_reg_base >> PAGE_SHIFT,
+			    iim_reg_size,
+			    vma->vm_page_prot))
+		return -EAGAIN;
+
+	return 0;
+}
+
+/*!
+ * MXC IIM interface - open function
+ *
+ * @param inode	     struct inode *
+ * @param filp	     struct file *
+ *
+ * @return	     Return 0 on success or negative error code on error
+ */
+static int mxc_iim_open(struct inode *inode, struct file *filp)
+{
+	iim_clk = clk_get(NULL, "iim_clk");
+	if (IS_ERR(iim_clk)) {
+		dev_err(iim_dev, "No IIM clock defined\n");
+		return -ENODEV;
+	}
+	clk_enable(iim_clk);
+
+	return 0;
+}
+
+/*!
+ * MXC IIM interface - release function
+ *
+ * @param inode	     struct inode *
+ * @param filp	     struct file *
+ *
+ * @return	     Return 0 on success or negative error code on error
+ */
+static int mxc_iim_release(struct inode *inode, struct file *filp)
+{
+	clk_disable(iim_clk);
+	clk_put(iim_clk);
+	return 0;
+}
+
+static const struct file_operations mxc_iim_fops = {
+	.mmap = mxc_iim_mmap,
+	.open = mxc_iim_open,
+	.release = mxc_iim_release,
+};
+
+static struct miscdevice mxc_iim_miscdev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "mxc_iim",
+	.fops = &mxc_iim_fops,
+};
+
+/*!
+ * This function is called by the driver framework to get iim base/end address
+ * and register iim misc device.
+ *
+ * @param	dev	The device structure for IIM passed in by the driver
+ *			framework.
+ *
+ * @return      Returns 0 on success or negative error code on error
+ */
+static int mxc_iim_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	int ret;
+
+	iim_dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (IS_ERR(res)) {
+		dev_err(iim_dev, "Unable to get IIM resource\n");
+		return -ENODEV;
+	}
+
+	iim_reg_base = res->start;
+	iim_reg_end = res->end;
+	iim_reg_size = iim_reg_end - iim_reg_base + 1;
+
+	ret = misc_register(&mxc_iim_miscdev);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int mxc_iim_remove(struct platform_device *pdev)
+{
+	misc_deregister(&mxc_iim_miscdev);
+	return 0;
+}
+
+static struct platform_driver mxc_iim_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "mxc_iim",
+		   },
+	.probe = mxc_iim_probe,
+	.remove = mxc_iim_remove,
+};
+
+static int __init mxc_iim_dev_init(void)
+{
+	return platform_driver_register(&mxc_iim_driver);
+}
+
+static void __exit mxc_iim_dev_cleanup(void)
+{
+	platform_driver_unregister(&mxc_iim_driver);
+}
+
+module_init(mxc_iim_dev_init);
+module_exit(mxc_iim_dev_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC IIM driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(MISC_DYNAMIC_MINOR);
diff --git a/drivers/char/mxc_si4702.c b/drivers/char/mxc_si4702.c
new file mode 100644
index 000000000000..b4b0809e458f
--- /dev/null
+++ b/drivers/char/mxc_si4702.c
@@ -0,0 +1,1220 @@
+/*
+ * linux/drivers/char/mxc_si4702.c
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/cdev.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/regulator/consumer.h>
+#include <asm/uaccess.h>
+#include <linux/err.h>
+#include <linux/mxc_si4702.h>
+
+#define SI4702_DEV_NAME		"si4702"
+#define DEV_MAJOR		0	/* this could be module param */
+#define DEV_MINOR		0
+#define DEV_BASE_MINOR		0
+#define DEV_MINOR_COUNT		256
+#define SI4702_I2C_ADDR	0x10	/* 7bits I2C address */
+#define DELAY_WAIT	0xffff	/* loop_counter max value */
+/* register define */
+#define SI4702_DEVICEID		0x00
+#define SI4702_CHIPID			0x01
+#define SI4702_POWERCFG		0x02
+#define SI4702_CHANNEL		0x03
+#define SI4702_SYSCONFIG1		0x04
+#define SI4702_SYSCONFIG2		0x05
+#define SI4702_SYSCONFIG3		0x06
+#define SI4702_TEST1			0x07
+#define SI4702_TEST2			0x08
+#define SI4702_B00TCONFIG		0x09
+#define SI4702_STATUSRSSI		0x0A
+#define SI4702_READCHAN		0x0B
+#define SI4702_REG_NUM		0x10
+#define SI4702_REG_BYTE		(SI4702_REG_NUM * 2)
+#define SI4702_DEVICE_ID		0x1242
+#define SI4702_RW_REG_NUM	(SI4702_STATUSRSSI - SI4702_POWERCFG)
+#define SI4702_RW_OFFSET	\
+	(SI4702_REG_NUM - SI4702_STATUSRSSI + SI4702_POWERCFG)
+
+#define SI4702_SPACE_MASK	0x0030
+#define SI4702_SPACE_200K	0x0
+#define SI4702_SPACE_100K	0x10
+#define SI4702_SPACE_50K	0x20
+
+#define SI4702_BAND_MASK	0x00c0
+#define SI4702_BAND_LSB		6
+
+#define SI4702_SEEKTH_MASK	0xff00
+#define SI4702_SEEKTH_LSB	8
+
+#define SI4702_SNR_MASK		0x00f0
+#define SI4702_SNR_LSB		4
+
+#define SI4702_CNT_MASK		0x000f
+#define SI4702_CNT_LSB		0
+
+#define SI4702_VOL_MASK		0x000f
+#define SI4702_VOL_LSB		0
+
+#define SI4702_CHAN_MASK	0x03ff
+#define SI4702_TUNE_BIT		0x8000
+#define SI4702_STC_BIT		0x4000
+#define SI4702_DMUTE_BIT	0x4000
+#define SI4702_SEEKUP_BIT	0x0200
+#define SI4702_SEEK_BIT		0x0100
+#define SI4702_SF_BIT		0x2000
+#define SI4702_ENABLE_BIT	0x0001
+#define SI4702_DISABLE_BIT	0x0040
+
+enum {
+	BAND_USA = 0,
+	BAND_JAP_W,
+	BAND_JAP
+};
+
+struct si4702_info {
+	int min_band;
+	int max_band;
+	int space;
+	int volume;
+	int channel;
+	int mute;
+};
+
+struct si4702_drvdata {
+	struct regulator *vio;
+	struct regulator *vdd;
+	struct class *radio_class;
+	struct si4702_info info;
+	/*by default, dev major is zero, and it's alloc dynamicaly. */
+	int major;
+	int minor;
+	struct cdev *cdev;
+	int count;		/* open count */
+	struct i2c_client *client;
+	unsigned char reg_rw_buf[SI4702_REG_BYTE];
+	struct mxc_fm_platform_data *plat_data;
+};
+
+static struct si4702_drvdata *si4702_drvdata;
+
+DEFINE_SPINLOCK(count_lock);
+
+#ifdef DEBUG
+static void si4702_dump_reg(void)
+{
+	int i, j;
+	unsigned char *reg_rw_buf;
+
+	if (NULL == si4702_drvdata)
+		return;
+
+	reg_rw_buf = si4702_drvdata->reg_rw_buf;
+
+	for (i = 0; i < 10; i++) {
+		j = i * 2 + 12;
+		pr_debug("reg[%02d] = %04x\n", i,
+			 ((reg_rw_buf[j] << 8) & 0xFF00) +
+			 (reg_rw_buf[j + 1] & 0x00FF));
+	}
+	for (; i < 16; i++) {
+		j = (i - 10) * 2;
+		pr_debug("reg[%02d] = %04x\n", i,
+			 ((reg_rw_buf[j] << 8) & 0xFF00) +
+			 (reg_rw_buf[j + 1] & 0x00FF));
+	}
+}
+#else
+static void si4702_dump_reg(void)
+{
+}
+#endif				/* DEBUG */
+
+/*
+ *check the si4702 spec for the read/write concequence.
+ *
+ *0 2       A    F0         A    F
+ *-------------------------------
+ *      buf:0 2      A     F
+ */
+#define REG_to_BUF(reg) (((reg >= 0) && (reg < SI4702_STATUSRSSI))?\
+		(reg - SI4702_STATUSRSSI + SI4702_REG_NUM):\
+		((reg >= SI4702_STATUSRSSI) && (reg < SI4702_REG_NUM))?\
+		(reg - SI4702_STATUSRSSI) : -1)
+
+static int si4702_read_reg(const int reg, u16 *value)
+{
+	int ret, index;
+	unsigned char *reg_rw_buf;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	reg_rw_buf = si4702_drvdata->reg_rw_buf;
+
+	index = REG_to_BUF(reg);
+
+	if (-1 == index)
+		return -1;
+
+	ret =
+	    i2c_master_recv(si4702_drvdata->client, reg_rw_buf,
+			    SI4702_REG_BYTE);
+
+	*value = (reg_rw_buf[index * 2] << 8) & 0xFF00;
+	*value |= reg_rw_buf[index * 2 + 1] & 0x00FF;
+
+	return ret < 0 ? ret : 0;
+}
+
+static int si4702_write_reg(const int reg, const u16 value)
+{
+	int index, ret;
+	unsigned char *reg_rw_buf;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	reg_rw_buf = si4702_drvdata->reg_rw_buf;
+
+	index = REG_to_BUF(reg);
+
+	if (-1 == index)
+		return -1;
+
+	reg_rw_buf[index * 2] = (value & 0xFF00) >> 8;
+	reg_rw_buf[index * 2 + 1] = value & 0x00FF;
+
+	ret = i2c_master_send(si4702_drvdata->client,
+			      &reg_rw_buf[SI4702_RW_OFFSET * 2],
+			      (SI4702_STATUSRSSI - SI4702_POWERCFG) * 2);
+	return ret < 0 ? ret : 0;
+}
+
+static void si4702_gpio_get(void)
+{
+	if (NULL == si4702_drvdata)
+		return;
+
+	si4702_drvdata->plat_data->gpio_get();
+}
+
+static void si4702_gpio_put(void)
+{
+	if (NULL == si4702_drvdata)
+		return;
+
+	si4702_drvdata->plat_data->gpio_put();
+}
+
+static void si4702_reset(void)
+{
+	if (NULL == si4702_drvdata)
+		return;
+
+	si4702_drvdata->plat_data->reset();
+}
+
+static void si4702_clock_en(int flag)
+{
+	if (NULL == si4702_drvdata)
+		return;
+
+	si4702_drvdata->plat_data->clock_ctl(flag);
+}
+
+static int si4702_id_detect(struct i2c_client *client)
+{
+	int ret, index;
+	unsigned int ID = 0;
+	unsigned char reg_rw_buf[SI4702_REG_BYTE];
+
+	si4702_gpio_get();
+	si4702_reset();
+	si4702_clock_en(1);
+
+	ret = i2c_master_recv(client, (char *)reg_rw_buf, SI4702_REG_BYTE);
+
+	si4702_gpio_put();
+
+	if (ret < 0)
+		return ret;
+
+	index = REG_to_BUF(SI4702_DEVICEID);
+	if (index < 0)
+		return index;
+
+	ID = (reg_rw_buf[index * 2] << 8) & 0xFF00;
+	ID |= reg_rw_buf[index * 2 + 1] & 0x00FF;
+
+	return ID;
+}
+
+/* valid args 50/100/200 */
+static int si4702_set_space(int space)
+{
+	u16 reg;
+	int ret;
+	struct si4702_info *info;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	ret = si4702_read_reg(SI4702_SYSCONFIG2, &reg);
+	if (ret == -1)
+		return ret;
+
+	reg &= ~SI4702_SPACE_MASK;
+	switch (space) {
+	case 50:
+		reg |= SI4702_SPACE_50K;
+		break;
+	case 100:
+		reg |= SI4702_SPACE_100K;
+		break;
+	case 200:
+		ret |= SI4702_SPACE_200K;
+		break;
+	default:
+		return -1;
+	}
+
+	ret = si4702_write_reg(SI4702_SYSCONFIG2, reg);
+	if (ret == -1)
+		return ret;
+
+	info = &si4702_drvdata->info;
+	info->space = space;
+	return 0;
+}
+
+static int si4702_set_band_range(int band)
+{
+	u16 reg;
+	int ret, band_min, band_max;
+	struct si4702_info *info;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	switch (band) {
+	case BAND_USA:
+		band_min = 87500;
+		band_max = 108000;
+		break;
+	case BAND_JAP_W:
+		band_min = 76000;
+		band_max = 108000;
+		break;
+	case BAND_JAP:
+		band_min = 76000;
+		band_max = 90000;
+		break;
+	default:
+		return -1;
+	}
+
+	ret = si4702_read_reg(SI4702_SYSCONFIG2, &reg);
+	if (ret == -1)
+		return ret;
+
+	reg = (reg & ~SI4702_BAND_MASK)
+	    | ((band << SI4702_BAND_LSB) & SI4702_BAND_MASK);
+	ret = si4702_write_reg(SI4702_SYSCONFIG2, reg);
+	if (ret == -1)
+		return ret;
+
+	info = &si4702_drvdata->info;
+	info->min_band = band_min;
+	info->max_band = band_max;
+	return 0;
+}
+
+static int si4702_set_seekth(u8 seekth)
+{
+	u16 reg;
+	int ret;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	ret = si4702_read_reg(SI4702_SYSCONFIG2, &reg);
+	if (ret == -1)
+		return ret;
+
+	reg =
+	    (reg & ~SI4702_SEEKTH_MASK) | ((seekth << SI4702_SEEKTH_LSB) &
+					   SI4702_SEEKTH_MASK);
+	ret = si4702_write_reg(SI4702_SYSCONFIG2, reg);
+	if (ret == -1)
+		return ret;
+
+	return 0;
+}
+
+static int si4702_set_sksnr(u8 sksnr)
+{
+	u16 reg;
+	int ret;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	ret = si4702_read_reg(SI4702_SYSCONFIG3, &reg);
+	if (ret == -1)
+		return ret;
+
+	reg =
+	    (reg & ~SI4702_SNR_MASK) | ((sksnr << SI4702_SNR_LSB) &
+					SI4702_SNR_MASK);
+	ret = si4702_write_reg(SI4702_SYSCONFIG3, reg);
+	if (ret == -1)
+		return ret;
+
+	return 0;
+}
+
+static int si4702_set_skcnt(u8 skcnt)
+{
+	u16 reg;
+	int ret;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	ret = si4702_read_reg(SI4702_SYSCONFIG3, &reg);
+	if (ret == -1)
+		return ret;
+
+	reg = (reg & ~SI4702_CNT_MASK) | (skcnt & SI4702_CNT_MASK);
+	ret = si4702_write_reg(SI4702_SYSCONFIG3, reg);
+	if (ret == -1)
+		return ret;
+
+	return 0;
+}
+
+static int si4702_set_vol(int vol)
+{
+	u16 reg;
+	int ret;
+	struct si4702_info *info;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	ret = si4702_read_reg(SI4702_SYSCONFIG2, &reg);
+	if (ret == -1)
+		return ret;
+
+	reg = (reg & ~SI4702_VOL_MASK) | (vol & SI4702_VOL_MASK);
+	ret = si4702_write_reg(SI4702_SYSCONFIG2, reg);
+	if (ret == -1)
+		return ret;
+
+	info = &si4702_drvdata->info;
+	info->volume = vol;
+
+	return 0;
+}
+
+static u8 si4702_channel_select(u32 freq)
+{
+	u16 loop_counter = 0;
+	s16 channel;
+	u16 si4702_reg_data;
+	u8 error_ind = 0;
+	struct i2c_client *client;
+	struct si4702_info *info;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	info = &si4702_drvdata->info;
+	client = si4702_drvdata->client;
+
+	dev_info(&client->dev, "Input frequnce is %d\n", freq);
+	if (freq < 76000 || freq > 108000) {
+		dev_err(&client->dev, "Input frequnce is invalid\n");
+		return -1;
+	}
+	/* convert freq to channel */
+	channel = (freq - info->min_band) / info->space;
+
+	si4702_reg_data = SI4702_TUNE_BIT | (channel & SI4702_CHAN_MASK);
+	/* set channel */
+	error_ind = si4702_write_reg(SI4702_CHANNEL, si4702_reg_data);
+	if (error_ind) {
+		dev_err(&client->dev, "Failed to set channel\n");
+		return -1;
+	}
+	dev_info(&client->dev, "Set channel to %d\n", channel);
+
+	/* wait for STC == 1 */
+	do {
+		error_ind =
+		    si4702_read_reg(SI4702_STATUSRSSI, &si4702_reg_data);
+
+		if (error_ind) {
+			dev_err(&client->dev, "Failed to read setted STC\n");
+			return -1;
+		}
+		if ((si4702_reg_data & SI4702_STC_BIT) != 0)
+			break;
+	} while (++loop_counter < DELAY_WAIT);
+
+	/* check loop_counter */
+	if (loop_counter >= DELAY_WAIT) {
+		dev_err(&client->dev, "Can't wait for STC bit set");
+		return -1;
+	}
+	dev_info(&client->dev, "loop counter %d\n", loop_counter);
+
+	loop_counter = 0;
+	/* clear tune bit */
+	error_ind = si4702_write_reg(SI4702_CHANNEL, 0);
+
+	if (error_ind) {
+		dev_err(&client->dev, "Failed to set stop tune\n");
+		return -1;
+	}
+
+	/* wait for STC == 0 */
+	do {
+		error_ind =
+		    si4702_read_reg(SI4702_STATUSRSSI, &si4702_reg_data);
+
+		if (error_ind) {
+			dev_err(&client->dev, "Failed to set read STC\n");
+			return -1;
+		}
+		if ((si4702_reg_data & SI4702_STC_BIT) == 0)
+			break;
+	} while (++loop_counter < DELAY_WAIT);
+
+	/* check loop_counter */
+	if (loop_counter >= DELAY_WAIT) {
+		dev_err(&client->dev, "Can't wait for STC bit clean");
+		return -1;
+	}
+	dev_info(&client->dev, "loop counter %d\n", loop_counter);
+
+	/* read RSSI */
+	error_ind = si4702_read_reg(SI4702_READCHAN, &si4702_reg_data);
+
+	if (error_ind) {
+		dev_err(&client->dev, "Failed to read RSSI\n");
+		return -1;
+	}
+
+	channel = si4702_reg_data & SI4702_CHAN_MASK;
+	dev_info(&client->dev, "seek finish: channel(%d)\n", channel);
+
+	return 0;
+}
+
+static s32 si4702_channel_seek(s16 dir)
+{
+	u16 loop_counter = 0;
+	u16 si4702_reg_data, reg_power_cfg;
+	u8 error_ind = 0;
+	u32 channel, freq;
+	struct i2c_client *client;
+	struct si4702_info *info;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	info = &si4702_drvdata->info;
+	client = si4702_drvdata->client;
+
+	error_ind = si4702_read_reg(SI4702_POWERCFG, &reg_power_cfg);
+
+	if (info->mute) {
+		/* check disable mute */
+		reg_power_cfg &= ~SI4702_DMUTE_BIT;
+	} else {
+		reg_power_cfg |= SI4702_DMUTE_BIT;
+	}
+
+	if (dir) {
+		reg_power_cfg |= SI4702_SEEKUP_BIT;
+	} else {
+		reg_power_cfg &= ~SI4702_SEEKUP_BIT;
+	}
+	/* start seek */
+	reg_power_cfg |= SI4702_SEEK_BIT;
+	error_ind = si4702_write_reg(SI4702_POWERCFG, reg_power_cfg);
+
+	if (error_ind) {
+		dev_err(&client->dev, "Failed to set seek start bit\n");
+		return -1;
+	}
+
+	/* wait STC == 1 */
+	do {
+		error_ind =
+		    si4702_read_reg(SI4702_STATUSRSSI, &si4702_reg_data);
+		if (error_ind) {
+			dev_err(&client->dev, "Failed to read STC bit\n");
+			return -1;
+		}
+
+		if ((si4702_reg_data & SI4702_STC_BIT) != 0)
+			break;
+	} while (++loop_counter < DELAY_WAIT);
+
+	/* clear seek bit */
+	reg_power_cfg &= ~SI4702_SEEK_BIT;
+	error_ind = si4702_write_reg(SI4702_POWERCFG, reg_power_cfg);
+	if (error_ind) {
+		dev_err(&client->dev, "Failed to stop seek\n");
+		return -1;
+	}
+
+	if (loop_counter >= DELAY_WAIT) {
+		dev_err(&client->dev, "Can't wait for STC bit set\n");
+		return -1;
+	}
+
+	/* check whether SF==1 (seek failed bit) */
+	if ((si4702_reg_data & SI4702_SF_BIT) != 0) {
+		dev_err(&client->dev, "Failed to seek any channel\n");
+		return -1;
+	}
+
+	loop_counter = 0;
+	/* wait STC == 0 */
+	do {
+		error_ind =
+		    si4702_read_reg(SI4702_STATUSRSSI, &si4702_reg_data);
+
+		if (error_ind) {
+			dev_err(&client->dev,
+				"Failed to wait STC bit to clear\n");
+			return -1;
+		}
+		if ((si4702_reg_data & SI4702_STC_BIT) == 0)
+			break;
+	} while (++loop_counter < DELAY_WAIT);
+
+	/* check loop_counter */
+	if (loop_counter >= DELAY_WAIT) {
+		dev_err(&client->dev, "Can't wait for STC bit clean");
+		return -1;
+	}
+
+	error_ind = si4702_read_reg(SI4702_READCHAN, &si4702_reg_data);
+
+	if (error_ind) {
+		dev_err(&client->dev, "I2C simulate failed\n");
+		return -1;
+	}
+
+	channel = si4702_reg_data & SI4702_CHAN_MASK;
+	freq = channel * info->space + info->min_band;
+	dev_err(&client->dev,
+		"seek finish: channel(%d), freq(%dKHz)\n", channel, freq);
+
+	return 0;
+}
+
+static int si4702_startup(void)
+{
+	u16 magic = 0, id;
+	struct i2c_client *client;
+	struct mxc_fm_platform_data *data;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	if (si4702_drvdata->vio)
+		regulator_enable(si4702_drvdata->vio);
+	if (si4702_drvdata->vdd)
+		regulator_enable(si4702_drvdata->vdd);
+	data = si4702_drvdata->plat_data;
+	client = si4702_drvdata->client;
+
+	/* read prior to write, otherwise write op will fail */
+	si4702_read_reg(SI4702_DEVICEID, &id);
+	dev_err(&client->dev, "si4702: DEVICEID: 0x%x\n", id);
+
+	si4702_clock_en(1);
+	msleep(100);
+
+	/* disable mute, stereo, seek down, powerup */
+	si4702_write_reg(SI4702_POWERCFG, SI4702_DMUTE_BIT | SI4702_ENABLE_BIT);
+	msleep(500);
+	si4702_read_reg(SI4702_TEST1, &magic);
+	if (magic != 0x3C04)
+		dev_err(&client->dev, "magic number 0x%x.\n", magic);
+	/* close tune, set channel to 0 */
+	si4702_write_reg(SI4702_CHANNEL, 0);
+	/* disable interrupt, disable GPIO */
+	si4702_write_reg(SI4702_SYSCONFIG1, 0);
+	/* set volume to middle level */
+	si4702_set_vol(0xf);
+
+	si4702_set_space(data->space);
+	si4702_set_band_range(data->band);
+	si4702_set_seekth(data->seekth);
+	si4702_set_skcnt(data->skcnt);
+	si4702_set_sksnr(data->sksnr);
+
+	return 0;
+}
+
+static void si4702_shutdown(void)
+{
+	if (NULL == si4702_drvdata)
+		return;
+
+	si4702_write_reg(SI4702_POWERCFG, SI4702_DMUTE_BIT |
+			 SI4702_ENABLE_BIT | SI4702_DISABLE_BIT);
+	msleep(100);
+	si4702_clock_en(0);
+
+	if (si4702_drvdata->vdd)
+		regulator_disable(si4702_drvdata->vdd);
+	if (si4702_drvdata->vio)
+		regulator_disable(si4702_drvdata->vio);
+}
+
+enum {
+	FM_STARTUP = 0,
+	FM_SHUTDOWN,
+	FM_RESET,
+	FM_VOLUP,
+	FM_VOLDOWN,
+	FM_SEEK_UP,
+	FM_SEEK_DOWN,
+	FM_MUTEON,
+	FM_MUTEDIS,
+	FM_SEL,
+	FM_SEEKTH,
+	FM_DL,
+	FM_CTL_MAX
+};
+
+static const char *const fm_control[FM_CTL_MAX] = {
+	[FM_STARTUP] = "start",
+	[FM_SHUTDOWN] = "halt",
+	[FM_RESET] = "reset",
+	[FM_VOLUP] = "volup",
+	[FM_VOLDOWN] = "voldown",
+	[FM_SEEK_UP] = "seeku",
+	[FM_SEEK_DOWN] = "seekd",
+	[FM_MUTEON] = "mute",
+	[FM_MUTEDIS] = "muted",
+	[FM_SEL] = "select",
+	[FM_SEEKTH] = "seekth",
+	[FM_DL] = "delay"
+};
+
+static int cmd(unsigned int index, int arg)
+{
+	struct i2c_client *client;
+	struct mxc_fm_platform_data *plat_data;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	client = si4702_drvdata->client;
+	plat_data = si4702_drvdata->plat_data;
+
+	switch (index) {
+	case FM_SHUTDOWN:
+		dev_err(&client->dev, "FM_SHUTDOWN\n");
+		si4702_shutdown();
+		break;
+	case FM_STARTUP:
+		dev_err(&client->dev, "FM_STARTUP\n");
+		si4702_reset();
+		si4702_startup();
+		break;
+	case FM_RESET:
+		dev_err(&client->dev, "FM_RESET\n");
+		si4702_reset();
+		break;
+	case FM_SEEK_DOWN:
+		dev_err(&client->dev, "SEEK DOWN\n");
+		si4702_channel_seek(0);
+		break;
+	case FM_SEEK_UP:
+		dev_err(&client->dev, "SEEK UP\n");
+		si4702_channel_seek(1);
+		break;
+	case FM_SEL:
+		dev_err(&client->dev, "select %d\n", arg * 100);
+		si4702_channel_select(arg * 100);
+		break;
+	case FM_SEEKTH:
+		dev_err(&client->dev, "seekth = %d\n", arg);
+		si4702_set_seekth(arg);
+		break;
+	case FM_DL:
+		dev_err(&client->dev, "delay = %d\n", arg);
+		break;
+	default:
+		dev_err(&client->dev, "error command\n");
+		break;
+	}
+	return 0;
+}
+
+static ssize_t si4702_show(struct device *dev,
+			   struct device_attribute *attr, char *buf)
+{
+	struct si4702_drvdata *drv_data = dev_get_drvdata(dev);
+	u16 device_id;
+
+	dev_err(&(drv_data->client->dev), "si4702 show\n");
+	si4702_read_reg(SI4702_DEVICEID, &device_id);
+	pr_info("device id %x\n", device_id);
+	si4702_dump_reg();
+	return 0;
+}
+
+static ssize_t si4702_store(struct device *dev,
+			    struct device_attribute *attr, const char *buf,
+			    size_t count)
+{
+	int state = 0;
+	const char *const *s;
+	char *p = NULL;
+	int error;
+	int len, arg = 0;
+	struct si4702_drvdata *drv_data = dev_get_drvdata(dev);
+	struct i2c_client *client = drv_data->client;
+
+	dev_err(&client->dev, "si4702 store %d\n", count);
+
+	p = memchr(buf, ' ', count);
+	if (p) {
+		len = p - buf;
+		*p = '\0';
+	} else
+		len = count;
+
+	len -= 1;
+	dev_err(&client->dev, "cmd %s\n", buf);
+
+	for (s = &fm_control[state]; state < FM_CTL_MAX; s++, state++) {
+		if (*s && !strncmp(buf, *s, len)) {
+			break;
+		}
+	}
+	if (state < FM_CTL_MAX && *s) {
+		if (p)
+			arg = simple_strtoul(p + 1, NULL, 0);
+		dev_err(&client->dev, "arg = %d\n", arg);
+		error = cmd(state, arg);
+	} else {
+		dev_err(&client->dev, "error cmd\n");
+		error = -EINVAL;
+	}
+
+	return error ? error : count;
+}
+
+static int ioctl_si4702(struct inode *inode, struct file *file,
+			unsigned int cmd, unsigned long arg)
+{
+	void __user *argp = (void __user *)arg;
+	int mute = 0;
+	u16 data;
+	int error;
+	u8 volume;
+	unsigned int freq;
+	int dir;
+	struct i2c_client *client;
+	struct si4702_info *info;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	info = &si4702_drvdata->info;
+	client = si4702_drvdata->client;
+
+	dev_err(&client->dev, "ioctl, cmd: 0x%x, arg: 0x%lx\n", cmd, arg);
+
+	switch (cmd) {
+	case SI4702_SETVOLUME:
+		/* get volume from user */
+		if (copy_from_user(&volume, argp, sizeof(u8))) {
+
+			dev_err(&client->dev,
+				"ioctl, copy volume value from user failed\n");
+			return -EFAULT;
+		}
+		dev_err(&client->dev, "volume %d\n", volume);
+		/* refill the register value */
+		volume &= 0x0f;
+		if (info->mute)
+			error = si4702_write_reg(SI4702_POWERCFG, 0x0001);
+		else
+			error = si4702_write_reg(SI4702_POWERCFG, 0x4001);
+
+		error = si4702_write_reg(SI4702_CHANNEL, 0);
+		error = si4702_write_reg(SI4702_SYSCONFIG1, 0);
+		error = si4702_write_reg(SI4702_SYSCONFIG2, 0x0f10 | volume);
+		if (error) {
+			dev_err(&client->dev, "ioctl, set volume failed\n");
+			return -EFAULT;
+		}
+		/* renew the device info */
+		info->volume = volume;
+
+		break;
+	case SI4702_GETVOLUME:
+		/* just copy volume value to user */
+		if (copy_to_user(argp, &(info->volume), sizeof(unsigned int))) {
+			dev_err(&client->dev, "ioctl, copy to user failed\n");
+			return -EFAULT;
+		}
+		break;
+	case SI4702_MUTEON:
+		mute = 1;
+	case SI4702_MUTEOFF:
+		if (mute) {
+			/* enable mute */
+			si4702_read_reg(SI4702_POWERCFG, &data);
+			data &= 0x00FF;
+			error = si4702_write_reg(SI4702_POWERCFG, data);
+		} else {
+			si4702_read_reg(SI4702_POWERCFG, &data);
+			data &= 0x00FF;
+			data |= 0x4000;
+			error = si4702_write_reg(SI4702_POWERCFG, data);
+		}
+		if (error) {
+			dev_err(&client->dev, "ioctl, set mute failed\n");
+			return -EFAULT;
+		}
+		break;
+	case SI4702_SELECT:
+		if (copy_from_user(&freq, argp, sizeof(unsigned int))) {
+
+			dev_err(&client->dev,
+				"ioctl, copy frequence from user failed\n");
+			return -EFAULT;
+		}
+		/* check frequence */
+		if (freq > info->max_band || freq < info->min_band) {
+			dev_err(&client->dev,
+				"the frequence select is out of band\n");
+			return -EINVAL;
+		}
+		if (si4702_channel_select(freq)) {
+			dev_err(&client->dev,
+				"ioctl, failed to select channel\n");
+			return -EFAULT;
+		}
+		break;
+	case SI4702_SEEK:
+		if (copy_from_user(&dir, argp, sizeof(int))) {
+
+			dev_err(&client->dev, "ioctl, copy from user failed\n");
+			return -EFAULT;
+		}
+		/* get seeked channel */
+		dir = si4702_channel_seek(dir);
+		if (dir == -1) {
+			return -EAGAIN;
+		} else if (dir == -2) {
+			return -EFAULT;
+		}
+		if (copy_to_user(argp, &dir, sizeof(int))) {
+
+			dev_err(&client->dev,
+				"ioctl, copy seek frequnce to user failed\n");
+			return -EFAULT;
+		}
+		break;
+	default:
+		dev_err(&client->dev, "SI4702: Invalid ioctl command\n");
+		return -EINVAL;
+
+	}
+	return 0;
+}
+
+static int open_si4702(struct inode *inode, struct file *file)
+{
+	struct i2c_client *client;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	client = si4702_drvdata->client;
+
+	spin_lock(&count_lock);
+	if (si4702_drvdata->count != 0) {
+		dev_err(&client->dev, "device has been open already\n");
+		spin_unlock(&count_lock);
+		return -EBUSY;
+	}
+	si4702_drvdata->count++;
+	spin_unlock(&count_lock);
+
+	/* request and active GPIO */
+	si4702_gpio_get();
+	/* reset the si4702 from it's reset pin */
+	si4702_reset();
+
+	/* startup si4702 */
+	if (si4702_startup()) {
+		spin_lock(&count_lock);
+		si4702_drvdata->count--;
+		spin_unlock(&count_lock);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int release_si4702(struct inode *inode, struct file *file)
+{
+	struct i2c_client *client;
+
+	if (NULL == si4702_drvdata)
+		return -1;
+
+	client = si4702_drvdata->client;
+
+	dev_err(&client->dev, "release\n");
+	/* software shutdown */
+	si4702_shutdown();
+	/* inactive, free GPIO, cut power */
+	si4702_gpio_put();
+
+	spin_lock(&count_lock);
+	si4702_drvdata->count--;
+	spin_unlock(&count_lock);
+
+	return 0;
+}
+
+static int si4702_suspend(struct i2c_client *client, pm_message_t state)
+{
+	return 0;
+}
+
+static int si4702_resume(struct i2c_client *client)
+{
+	return 0;
+}
+
+static struct device_attribute si4702_dev_attr = {
+	.attr = {
+		 .name = "si4702_ctl",
+		 .mode = S_IRUSR | S_IWUSR,
+		 },
+	.show = si4702_show,
+	.store = si4702_store,
+};
+
+static struct file_operations si4702_fops = {
+	.owner = THIS_MODULE,
+	.open = open_si4702,
+	.release = release_si4702,
+	.ioctl = ioctl_si4702,
+};
+
+static int __devinit si4702_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	int ret = 0;
+	struct mxc_fm_platform_data *plat_data;
+	struct si4702_drvdata *drv_data;
+	struct device *dev;
+
+	dev_info(&client->dev, "si4702 device probe process start.\n");
+
+	plat_data = (struct mxc_fm_platform_data *)client->dev.platform_data;
+	if (plat_data == NULL) {
+		dev_err(&client->dev, "lack of platform data!\n");
+		return -ENODEV;
+	}
+
+	drv_data = kmalloc(sizeof(struct si4702_drvdata), GFP_KERNEL);
+	if (drv_data == NULL) {
+		dev_err(&client->dev, "lack of kernel memory!\n");
+		return -ENOMEM;
+	}
+	memset(drv_data, 0, sizeof(struct si4702_drvdata));
+	drv_data->plat_data = plat_data;
+	drv_data->major = DEV_MAJOR;
+	drv_data->minor = DEV_MINOR;
+	drv_data->count = 0;
+
+	/*enable power supply */
+	if (plat_data->reg_vio != NULL) {
+		drv_data->vio = regulator_get(&client->dev, plat_data->reg_vio);
+		if (drv_data->vio == ERR_PTR(-ENOENT))
+			goto free_drv_data;
+		regulator_enable(drv_data->vio);
+	}
+
+	/* here, we assume that vio and vdd are not the same */
+	if (plat_data->reg_vdd != NULL) {
+		drv_data->vdd = regulator_get(&client->dev, plat_data->reg_vdd);
+		if (drv_data->vdd == ERR_PTR(-ENOENT))
+			goto disable_vio;
+		regulator_enable(drv_data->vdd);
+	}
+
+	/*attach client and check device id */
+	if (SI4702_DEVICE_ID != si4702_id_detect(client)) {
+		dev_err(&client->dev, "id wrong.\n");
+		goto disable_vdd;
+	}
+	dev_info(&client->dev, "chip id %x detect.\n", SI4702_DEVICE_ID);
+	drv_data->client = client;
+
+	/*user interface begain */
+	/*create device file in sysfs as a user interface,
+	 * also for debug support */
+	ret = device_create_file(&client->dev, &si4702_dev_attr);
+	if (ret) {
+		dev_err(&client->dev, "create device file failed!\n");
+		goto gpio_put;	/* shall i use some meanful error code? */
+	}
+
+	/*create a char dev for application code access */
+	if (drv_data->major) {
+		ret = register_chrdev(drv_data->major, "si4702", &si4702_fops);;
+	} else {
+		ret = register_chrdev(0, "si4702", &si4702_fops);
+	}
+
+	if (drv_data->major == 0)
+		drv_data->major = ret;
+
+	/* create class and device for udev information */
+	drv_data->radio_class = class_create(THIS_MODULE, "radio");
+	if (IS_ERR(drv_data->radio_class)) {
+		dev_err(&client->dev, "SI4702: failed to create radio class\n");
+		goto char_dev_remove;
+	}
+
+	dev = device_create(drv_data->radio_class, NULL,
+		    MKDEV(drv_data->major, drv_data->minor), NULL, "si4702");
+	if (IS_ERR(dev)) {
+		dev_err(&client->dev,
+			"SI4702: failed to create radio class device\n");
+		goto class_remove;
+	}
+	/*User interface end */
+	dev_set_drvdata(&client->dev, drv_data);
+	si4702_drvdata = drv_data;
+
+	si4702_gpio_get();
+	dev_info(&client->dev, "si4702 device probe successfully.\n");
+	si4702_shutdown();
+
+	return 0;
+
+class_remove:
+	class_destroy(drv_data->radio_class);
+char_dev_remove:
+	unregister_chrdev(drv_data->major, "si4702");
+	device_remove_file(&client->dev, &si4702_dev_attr);
+gpio_put:
+	si4702_gpio_put();
+disable_vdd:
+	if (plat_data->reg_vdd) {
+		regulator_disable(drv_data->vdd);
+		regulator_put(drv_data->vdd);
+	}
+disable_vio:
+	if (plat_data->reg_vio) {
+		regulator_disable(drv_data->vio);
+		regulator_put(drv_data->vio);
+	}
+
+free_drv_data:
+	kfree(drv_data);
+
+	return -ENODEV;
+}
+
+static int __devexit si4702_remove(struct i2c_client *client)
+{
+	struct mxc_fm_platform_data *plat_data;
+	struct si4702_drvdata *drv_data = dev_get_drvdata(&client->dev);
+
+	plat_data = (struct mxc_fm_platform_data *)client->dev.platform_data;
+
+	device_destroy(drv_data->radio_class,
+		       MKDEV(drv_data->major, drv_data->minor));
+	class_destroy(drv_data->radio_class);
+
+	unregister_chrdev(drv_data->major, "si4702");
+	device_remove_file(&client->dev, &si4702_dev_attr);
+	si4702_gpio_put();
+
+	if (plat_data->reg_vdd)
+		regulator_put(drv_data->vdd);
+
+	if (plat_data->reg_vio)
+		regulator_put(drv_data->vio);
+
+	kfree(si4702_drvdata);
+	si4702_drvdata = NULL;
+
+	return 0;
+}
+
+static const struct i2c_device_id si4702_id[] = {
+	{"si4702", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, si4702_id);
+
+static struct i2c_driver i2c_si4702_driver = {
+	.driver = {
+		   .name = "si4702",
+		   },
+	.probe = si4702_probe,
+	.remove = si4702_remove,
+	.suspend = si4702_suspend,
+	.resume = si4702_resume,
+	.id_table = si4702_id,
+};
+
+static int __init init_si4702(void)
+{
+	/*add to i2c driver */
+	pr_info("add si4702 i2c driver\n");
+	return i2c_add_driver(&i2c_si4702_driver);
+}
+
+static void __exit exit_si4702(void)
+{
+	i2c_del_driver(&i2c_si4702_driver);
+}
+
+module_init(init_si4702);
+module_exit(exit_si4702);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("SI4702 FM driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/mxs_viim.c b/drivers/char/mxs_viim.c
new file mode 100755
index 000000000000..2510afa6c13e
--- /dev/null
+++ b/drivers/char/mxs_viim.c
@@ -0,0 +1,175 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/mm.h>
+#include <linux/miscdevice.h>
+
+static unsigned long iim_reg_base0, iim_reg_end0, iim_reg_size0;
+static unsigned long iim_reg_base1, iim_reg_end1, iim_reg_size1;
+static struct device *iim_dev;
+
+/*!
+ * MXS Virtual IIM interface - memory map function
+ * This function maps one page size VIIM registers from VIIM base address0
+ * if the size of the required virtual memory space is less than or equal to
+ * one page size, otherwise this function will also map one page size VIIM
+ * registers from VIIM base address1.
+ *
+ * @param file	     struct file *
+ * @param vma	     structure vm_area_struct *
+ *
+ * @return	     Return 0 on success or negative error code on error
+ */
+static int mxs_viim_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	size_t size = vma->vm_end - vma->vm_start;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	/* Remap-pfn-range will mark the range VM_IO and VM_RESERVED */
+	if (remap_pfn_range(vma,
+			    vma->vm_start,
+			    iim_reg_base0 >> PAGE_SHIFT,
+			    iim_reg_size0,
+			    vma->vm_page_prot))
+		return -EAGAIN;
+
+	if (size > iim_reg_size0) {
+		if (remap_pfn_range(vma,
+				    vma->vm_start + iim_reg_size0,
+				    iim_reg_base1 >> PAGE_SHIFT,
+				    iim_reg_size1,
+				    vma->vm_page_prot))
+			return -EAGAIN;
+	}
+
+	return 0;
+}
+
+/*!
+ * MXS Virtual IIM interface - open function
+ *
+ * @param inode	     struct inode *
+ * @param filp	     struct file *
+ *
+ * @return	     Return 0 on success or negative error code on error
+ */
+static int mxs_viim_open(struct inode *inode, struct file *filp)
+{
+	return 0;
+}
+
+/*!
+ * MXS Virtual IIM interface - release function
+ *
+ * @param inode	     struct inode *
+ * @param filp	     struct file *
+ *
+ * @return	     Return 0 on success or negative error code on error
+ */
+static int mxs_viim_release(struct inode *inode, struct file *filp)
+{
+	return 0;
+}
+
+static const struct file_operations mxs_viim_fops = {
+	.mmap = mxs_viim_mmap,
+	.open = mxs_viim_open,
+	.release = mxs_viim_release,
+};
+
+static struct miscdevice mxs_viim_miscdev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "mxs_viim",
+	.fops = &mxs_viim_fops,
+};
+
+/*!
+ * This function is called by the driver framework to get virtual iim base/end
+ * address and register iim misc device.
+ *
+ * @param	dev	The device structure for Virtual IIM passed in by the
+ *			driver framework.
+ *
+ * @return      Returns 0 on success or negative error code on error
+ */
+static int mxs_viim_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	int ret;
+
+	iim_dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (IS_ERR(res)) {
+		dev_err(iim_dev, "Unable to get Virtual IIM resource 0\n");
+		return -ENODEV;
+	}
+
+	iim_reg_base0 = res->start;
+	iim_reg_end0 = res->end;
+	iim_reg_size0 = iim_reg_end0 - iim_reg_base0 + 1;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (IS_ERR(res)) {
+		dev_err(iim_dev, "Unable to get Virtual IIM resource 1\n");
+		return -ENODEV;
+	}
+
+	iim_reg_base1 = res->start;
+	iim_reg_end1 = res->end;
+	iim_reg_size1 = iim_reg_end1 - iim_reg_base1 + 1;
+
+	ret = misc_register(&mxs_viim_miscdev);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int mxs_viim_remove(struct platform_device *pdev)
+{
+	misc_deregister(&mxs_viim_miscdev);
+	return 0;
+}
+
+static struct platform_driver mxs_viim_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "mxs_viim",
+		   },
+	.probe = mxs_viim_probe,
+	.remove = mxs_viim_remove,
+};
+
+static int __init mxs_viim_dev_init(void)
+{
+	return platform_driver_register(&mxs_viim_driver);
+}
+
+static void __exit mxs_viim_dev_cleanup(void)
+{
+	platform_driver_unregister(&mxs_viim_driver);
+}
+
+module_init(mxs_viim_dev_init);
+module_exit(mxs_viim_dev_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXS Virtual IIM driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(MISC_DYNAMIC_MINOR);
diff --git a/drivers/char/random.c b/drivers/char/random.c
index 675076f5fca8..626e95dab938 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -129,6 +129,9 @@
  *                                unsigned int value);
  * 	void add_interrupt_randomness(int irq);
  *
+ *      void random_input_words(__u32 *buf, size_t wordcount, int ent_count)
+ *      int random_input_wait(void);
+ *
  * add_input_randomness() uses the input layer interrupt timing, as well as
  * the event type information from the hardware.
  *
@@ -140,6 +143,13 @@
  * a better measure, since the timing of the disk interrupts are more
  * unpredictable.
  *
+ * random_input_words() just provides a raw block of entropy to the input
+ * pool, such as from a hardware entropy generator.
+ *
+ * random_input_wait() suspends the caller until such time as the
+ * entropy pool falls below the write threshold, and returns a count of how
+ * much entropy (in bits) is needed to sustain the pool.
+ *
  * All of these routines try to estimate how many bits of randomness a
  * particular randomness source.  They do this by keeping track of the
  * first and second order deltas of the event timings.
@@ -689,6 +699,61 @@ void add_disk_randomness(struct gendisk *disk)
 }
 #endif
 
+/*
+ * random_input_words - add bulk entropy to pool
+ *
+ * @buf: buffer to add
+ * @wordcount: number of __u32 words to add
+ * @ent_count: total amount of entropy (in bits) to credit
+ *
+ * this provides bulk input of entropy to the input pool
+ *
+ */
+void random_input_words(__u32 *buf, size_t wordcount, int ent_count)
+{
+	mix_pool_bytes(&input_pool, buf, wordcount*4);
+
+	credit_entropy_bits(&input_pool, ent_count);
+
+	DEBUG_ENT("crediting %d bits => %d\n",
+		  ent_count, input_pool.entropy_count);
+	/*
+	 * Wake up waiting processes if we have enough
+	 * entropy.
+	 */
+	if (input_pool.entropy_count >= random_read_wakeup_thresh)
+		wake_up_interruptible(&random_read_wait);
+}
+EXPORT_SYMBOL(random_input_words);
+
+/*
+ * random_input_wait - wait until random needs entropy
+ *
+ * this function sleeps until the /dev/random subsystem actually
+ * needs more entropy, and then return the amount of entropy
+ * that it would be nice to have added to the system.
+ */
+int random_input_wait(void)
+{
+	int count;
+
+	wait_event_interruptible(random_write_wait, 
+			 input_pool.entropy_count < random_write_wakeup_thresh);
+
+	count = random_write_wakeup_thresh - input_pool.entropy_count;
+
+        /* likely we got woken up due to a signal */
+	if (count <= 0) count = random_read_wakeup_thresh; 
+
+	DEBUG_ENT("requesting %d bits from input_wait()er %d<%d\n",
+		  count,
+		  input_pool.entropy_count, random_write_wakeup_thresh);
+
+	return count;
+}
+EXPORT_SYMBOL(random_input_wait);
+
+
 #define EXTRACT_SIZE 10
 
 /*********************************************************************
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index e522144cba3a..c5de66aba591 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -83,6 +83,14 @@ config ZCRYPT_MONOLITHIC
 	  that contains all parts of the crypto device driver (ap bus,
 	  request router and all the card drivers).
 
+config CRYPTO_DEV_NS921X_AES
+	tristate "Driver for Digi ns921x AES algorithm"
+	depends on PROCESSOR_NS921X
+	select CRYPTO_BLKCIPHER
+	help
+	  Use Digi hardware acceleration for AES algorithm
+	  available in Digi ns921x CPUs.
+
 config CRYPTO_SHA1_S390
 	tristate "SHA1 digest algorithm"
 	depends on S390
@@ -200,4 +208,16 @@ config CRYPTO_DEV_IXP4XX
 	help
 	  Driver for the IXP4xx NPE crypto engine.
 
+config CRYPTO_DEV_STMP3XXX_DCP
+	tristate "Support for the STMP3xxx DCP engine"
+	depends on ARCH_STMP3XXX
+	select CRYPTO_ALGAPI
+	select CRYPTO_BLKCIPHER
+	help
+	  Say 'Y' here to use the STMP3XXX DCP AES
+	  engine for the CryptoAPI AES algorithm.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called geode-aes.
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index 73557b2968d3..6c9b0b4c62d1 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -2,5 +2,7 @@ obj-$(CONFIG_CRYPTO_DEV_PADLOCK_AES) += padlock-aes.o
 obj-$(CONFIG_CRYPTO_DEV_PADLOCK_SHA) += padlock-sha.o
 obj-$(CONFIG_CRYPTO_DEV_GEODE) += geode-aes.o
 obj-$(CONFIG_CRYPTO_DEV_HIFN_795X) += hifn_795x.o
+obj-$(CONFIG_CRYPTO_DEV_NS921X_AES) += ns921x-aes.o
 obj-$(CONFIG_CRYPTO_DEV_TALITOS) += talitos.o
 obj-$(CONFIG_CRYPTO_DEV_IXP4XX) += ixp4xx_crypto.o
+obj-$(CONFIG_CRYPTO_DEV_STMP3XXX_DCP) += stmp3xxx_dcp.o
diff --git a/drivers/crypto/ns921x-aes.c b/drivers/crypto/ns921x-aes.c
new file mode 100644
index 000000000000..e26b86572405
--- /dev/null
+++ b/drivers/crypto/ns921x-aes.c
@@ -0,0 +1,636 @@
+/*
+ * drivers/crypto/ns921x-aes.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <crypto/algapi.h>
+#include <crypto/aes.h>
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <mach/dma-ns921x.h>
+
+#define DMA_BDPOINTER	0x0
+
+#define DMA_CONTROL	0x4
+#define DMA_CONTROL_SW32	(1 << 28)
+#define DMA_CONTROL_DW32	(1 << 26)
+#define DMA_CONTROL_RST		(1 << 16)
+
+#define DMA_STATUS		0x8
+#define DMA_MAX_BUFFERS		256
+#define DRIVER_NAME "ns921x-aes"
+
+union ns9xxx_dma_desc {
+	struct {
+		u32 source;
+		u16 source_len;
+		u16 dest_len;
+		u32 dest;
+
+#define ENCMODE_CBC	0
+#define ENCMODE_CFB	1
+#define ENCMODE_OFB	2
+#define ENCMODE_CTR	3
+#define ENCMODE_ECB	4
+#define ENCMODE_CCM	5
+#define ENCMODE_KEYEXP	7
+
+#define	ENCACTION_ENC	(0 << 3)
+#define	ENCACTION_DEC	(1 << 3)
+#define ECCKEYSIZE_128	(0 << 4)
+#define ENCKEYSIZE_192	(1 << 4)
+#define ENCKEYSIZE_256	(2 << 4)
+#define ENCKEYSIZE(i)	((i == 16) ? ECCKEYSIZE_128 : \
+		(i == 24) ? ENCKEYSIZE_192 : ENCKEYSIZE_256)
+		u16 control;
+#define ENCOP_NONAES	0
+#define ENCOP_KEY	1
+#define ENCOP_IV	2
+#define ENCOP_NONCE	3
+#define ENCOP_ADD	4
+#define ENCOP_DATA	5
+		u16 flags;
+	};
+	u32 data[4];
+};
+
+#define KEYEXP_SIZE128	0x176
+#define KEYEXP_SIZE192	0x208
+#define KEYEXP_SIZE256	0x240
+#define KEYEXP_SIZE(i)	((i == 16) ? KEYEXP_SIZE128 : \
+		(i == 24) ? KEYEXP_SIZE192 : KEYEXP_SIZE256)
+
+struct ns921x_aes_data {
+	struct resource		*mem;
+	struct clk		*clk;
+
+	struct platform_device	*pdev;
+
+	wait_queue_head_t	waitq;
+
+	void __iomem		*membase;
+	dma_addr_t		p_dma_descr;
+	union ns9xxx_dma_desc	*v_dma_descr;
+
+	dma_addr_t		p_key;
+	u8			*v_key;
+	int			keylen;
+
+	int			irq;
+	int			irq_pending;
+};
+
+struct ns921x_aes_data dev_data;
+
+static inline void cpu_to_be(u32 *ptr, unsigned num)
+{
+	unsigned i;
+
+	for (i = 0; i < num; i++)
+		ptr[i] = cpu_to_be32(ptr[i]);
+}
+
+static inline void be_to_cpu(u32 *ptr, unsigned num)
+{
+	unsigned i;
+
+	for (i = 0; i < num; i++)
+		ptr[i] = be32_to_cpu(ptr[i]);
+}
+
+static irqreturn_t ns921x_aes_int(int irq, void *dev_id)
+{
+	u32 status;
+
+	status = ioread32(dev_data.membase + DMA_STATUS);
+	if (!(status & NS921X_DMA_STIE_NCIP) ) {
+		/* check dma errors */
+		if ( status & (NS921X_DMA_STIE_ECIP |
+			NS921X_DMA_STIE_NRIP | NS921X_DMA_STIE_CAIP |
+			NS921X_DMA_STIE_PCIP) )
+			dev_dbg(&dev_data.pdev->dev,
+				"DMA error. Status = 0x%08x\n", status);
+	}
+	/* ack interrupt */
+	iowrite32(status, dev_data.membase + DMA_STATUS);
+
+	/* disable dma channel*/
+	iowrite32(0, dev_data.membase + DMA_CONTROL);
+
+	/* wake up caller*/
+	dev_data.irq_pending = 0;
+	wake_up(&dev_data.waitq);
+
+	return IRQ_HANDLED;
+}
+
+static int ns921x_aes_cra_init(struct crypto_tfm *tfm)
+{
+	if (dev_data.irq_pending)
+		return -EBUSY;
+
+	memset(dev_data.v_dma_descr, 0,
+		DMA_MAX_BUFFERS * sizeof(*dev_data.v_dma_descr));
+
+	return 0;
+}
+
+static int ns921x_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+		unsigned int keylen)
+{
+	int ret;
+
+	if (!(keylen == 16 || keylen == 24 || keylen == 32)) {
+		ret = -EINVAL;
+		dev_dbg(&dev_data.pdev->dev, "err_keylen\n");
+		goto err_keylen;
+	}
+
+	/* Free DMA allocation, just in case this function was
+	 * called but there was not a call to crypt or decrypt */
+	if (NULL != dev_data.v_key) {
+		dma_free_coherent(&dev_data.pdev->dev, dev_data.keylen,
+				dev_data.v_key, dev_data.p_key);
+		dev_data.v_key = NULL;
+	}
+
+	dev_data.v_key = dma_alloc_coherent(&dev_data.pdev->dev,
+			keylen, &dev_data.p_key, GFP_KERNEL);
+	if (!dev_data.v_key) {
+		ret = -ENOMEM;
+		dev_dbg(&dev_data.pdev->dev, "err_alloc\n");
+		goto err_alloc;
+	}
+
+	memcpy(dev_data.v_key, key, keylen);
+	dev_data.keylen = keylen;
+
+	cpu_to_be((u32 *)dev_data.v_key, dev_data.keylen / 4);
+
+	/* setup dma descr. for key buffer, .control is set in
+	 * de-/encryption routine as it depends on the mode */
+	dev_data.v_dma_descr[0].source = (u32)dev_data.p_key;
+	dev_data.v_dma_descr[0].source_len = dev_data.keylen;
+	dev_data.v_dma_descr[0].flags =
+		ENCOP_KEY | EXT_DMA_DESC_CTRL_FULL;
+
+	return 0;
+
+err_alloc:
+err_keylen:
+	return ret;
+}
+
+int ns921x_aes_dma(unsigned timeout)
+{
+	u32 ctrl;
+
+	/* fire up dma */
+	iowrite32(NS921X_DMA_STIE_NCIE | NS921X_DMA_STIE_ECIE |
+		NS921X_DMA_STIE_NRIE | NS921X_DMA_STIE_NRIE |
+		NS921X_DMA_STIE_CAIE |NS921X_DMA_STIE_PCIE,
+		dev_data.membase + DMA_STATUS);
+	ctrl = DMA_CONTROL_SW32 | NS921X_DMA_CR_SW_32b |
+		NS921X_DMA_CR_CE | NS921X_DMA_CR_CG;
+	iowrite32(ctrl, dev_data.membase + DMA_CONTROL);
+
+	/* wait for isr */
+	dev_data.irq_pending = 1;
+	if (!wait_event_timeout(dev_data.waitq,
+				(dev_data.irq_pending == 0), timeout)) {
+		dev_err(&dev_data.pdev->dev, "interrupt timed out! Retrying\n" );
+		dev_dbg(&dev_data.pdev->dev, "DMA_STATUS = 0x%x\n",
+				ioread32(dev_data.membase + DMA_STATUS) );
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+
+int ns921x_aes_crypt(struct ablkcipher_request *req, int iv)
+{
+	int ret = 0, i, n;
+	int v_buffers, p_buffers;
+
+	/* get number of used pages */
+	for (v_buffers = 0, n = 0; n < req->nbytes; v_buffers++) {
+		/* ensure big endian mode of input data */
+		cpu_to_be((u32 *)page_address(sg_page(&req->src[v_buffers]))
+				+ req->src[v_buffers].offset / 4,
+				req->src[v_buffers].length / 4);
+
+		n += req->src[v_buffers].length;
+	}
+
+	/* src must be same page as dst */
+	if (req->src->page_link != req->dst->page_link) {
+		ret = -EINVAL;
+		goto err_mismatch;
+	}
+
+
+	/* map pages */
+map:	p_buffers = dma_map_sg(&dev_data.pdev->dev, req->dst,
+			v_buffers, DMA_BIDIRECTIONAL);
+
+	/* create dma descriptors for every used page */
+	for (i = 0; i < p_buffers; i++) {
+		/* setup dma descriptors */
+		dev_data.v_dma_descr[1 + iv + i].source =
+			(u32)sg_dma_address(req->src + i);
+		dev_data.v_dma_descr[1 + iv + i].source_len =
+			(u16)sg_dma_len(req->src + i);
+		dev_data.v_dma_descr[1 + iv + i].dest =
+			(u32)sg_dma_address(req->src + i);
+		dev_data.v_dma_descr[1 + iv + i].flags =
+			ENCOP_DATA | EXT_DMA_DESC_CTRL_FULL;
+	}
+
+	/* add additional dma flags to last descriptor */
+	dev_data.v_dma_descr[iv + p_buffers].flags |=
+		EXT_DMA_DESC_CTRL_WRAP | EXT_DMA_DESC_CTRL_LAST;
+
+	/* let hardware do its work */
+	ret = ns921x_aes_dma(HZ * req->src->length / 100);
+
+	/* release dma mappings */
+	dma_sync_sg_for_cpu(&dev_data.pdev->dev, req->src,
+			v_buffers, DMA_BIDIRECTIONAL);
+	dma_unmap_sg(&dev_data.pdev->dev, req->src, i, DMA_BIDIRECTIONAL);
+	/* If DMA transmission did not complete, try again */
+	if (-EAGAIN == ret )
+		goto map;
+
+err_mismatch:
+	/* overwrite key buffer to avoid security breakage! */
+	memset(dev_data.v_key, 0, dev_data.keylen);
+	dma_free_coherent(&dev_data.pdev->dev, dev_data.keylen,
+			dev_data.v_key, dev_data.p_key);
+	dev_data.v_key = NULL;
+
+	/* convert output back to cpu endianes */
+	for (i = 0; i < v_buffers; i++)
+		be_to_cpu((u32 *)page_address(sg_page(&req->dst[i])) +
+				req->dst[i].offset / 4, req->dst[i].length / 4);
+
+	return ret;
+}
+
+static int ns921x_aes_keyexpander(void)
+{
+	int ret;
+	dma_addr_t p_expkey;
+	u8 *v_expkey;
+
+	v_expkey = dma_alloc_coherent(&dev_data.pdev->dev,
+			KEYEXP_SIZE(dev_data.keylen),
+			&p_expkey, GFP_KERNEL);
+	if (!v_expkey)
+		return -ENOMEM;
+
+buff:	dev_data.v_dma_descr[0].control =
+		ENCMODE_KEYEXP | ENCKEYSIZE(dev_data.keylen);
+
+	dev_data.v_dma_descr[1].source_len = KEYEXP_SIZE(dev_data.keylen);
+	dev_data.v_dma_descr[1].dest = p_expkey;
+	dev_data.v_dma_descr[1].flags =
+		ENCOP_DATA | EXT_DMA_DESC_CTRL_WRAP |
+		EXT_DMA_DESC_CTRL_FULL | EXT_DMA_DESC_CTRL_LAST;
+
+	ret = ns921x_aes_dma(HZ/10);
+	if (!ret) {
+		switch (dev_data.keylen) {
+		case 16:
+			memcpy(dev_data.v_key, (u8 *)v_expkey + 160, 16);
+			break;
+		case 24:
+			memcpy(dev_data.v_key, (u8 *)v_expkey + 192, 16);
+			memcpy(dev_data.v_key + 16, (u8 *)v_expkey + 184, 8);
+			break;
+		case 32:
+			memcpy(dev_data.v_key, (u8 *)v_expkey + 224, 16);
+			memcpy(dev_data.v_key + 16, (u8 *)v_expkey + 208, 16);
+			break;
+		}
+
+		/* reset used dma_descriptors */
+		memset(dev_data.v_dma_descr, 0,
+				2 * sizeof(*dev_data.v_dma_descr));
+		dev_data.v_dma_descr[0].source = (u32)dev_data.p_key;
+		dev_data.v_dma_descr[0].source_len = dev_data.keylen;
+		dev_data.v_dma_descr[0].flags =
+			ENCOP_KEY | EXT_DMA_DESC_CTRL_FULL ;
+	}
+	else if (-EAGAIN == ret)
+		goto buff; /* retry dma */
+
+	/* destroy expanded key */
+	memset(v_expkey, 0, KEYEXP_SIZE(dev_data.keylen));
+	dma_free_coherent(&dev_data.pdev->dev, KEYEXP_SIZE(dev_data.keylen),
+			v_expkey, p_expkey);
+	
+	/*
+	 * We need the key for the next operations, or? Fixed Vantive 31365
+	 * (Luis Galdos)
+	 */
+	/* dev_data.v_key = NULL; */
+
+	return ret;
+}
+
+static int ns921x_aes_ecb_encrypt(struct ablkcipher_request *req)
+{
+	dev_data.v_dma_descr[0].control =
+		ENCMODE_ECB | ENCKEYSIZE(dev_data.keylen) | ENCACTION_ENC;
+	return ns921x_aes_crypt(req, 0);
+}
+
+static int ns921x_aes_ecb_decrypt(struct ablkcipher_request *req)
+{
+	int ret;
+
+	ret = ns921x_aes_keyexpander();
+	if (!ret) {
+		dev_data.v_dma_descr[0].control = ENCMODE_ECB |
+			ENCKEYSIZE(dev_data.keylen) | ENCACTION_DEC;
+		ret = ns921x_aes_crypt(req, 0);
+	}
+
+	return ret;
+}
+
+static int ns921x_aes_cbc_encrypt(struct ablkcipher_request *req)
+{
+	int ret, ivsize;
+	u8 *v_iv;
+	dma_addr_t p_iv;
+
+	dev_data.v_dma_descr[0].control =
+		ENCMODE_CBC | ENCKEYSIZE(dev_data.keylen) | ENCACTION_ENC;
+
+	ivsize = 16;
+	/* XXX: crypto_ablkcipher_ivsize(crypto_ablkcipher_reqtfm(req)); */
+	v_iv = dma_alloc_coherent(&dev_data.pdev->dev, ivsize,
+			&p_iv, GFP_KERNEL);
+
+	memcpy(v_iv, req->info, ivsize);
+	cpu_to_be((u32 *)v_iv, ivsize);
+
+	dev_data.v_dma_descr[1].source = (u32)p_iv;
+	dev_data.v_dma_descr[1].source_len = ivsize;
+	dev_data.v_dma_descr[1].flags =
+		EXT_DMA_DESC_CTRL_FULL | ENCOP_IV;
+
+	ret = ns921x_aes_crypt(req, 1);
+
+	dma_free_coherent(&dev_data.pdev->dev, ivsize, v_iv, p_iv);
+
+	return ret;
+return -ENOMEM;
+}
+
+static int ns921x_aes_cbc_decrypt(struct ablkcipher_request *req)
+{
+	int ret, ivsize;
+	u8 *v_iv;
+	dma_addr_t p_iv;
+
+	ret = ns921x_aes_keyexpander();
+	if (!ret) {
+		dev_data.v_dma_descr[0].control = ENCMODE_CBC |
+			ENCKEYSIZE(dev_data.keylen) | ENCACTION_DEC;
+
+		ivsize = 16;
+		/* XXX: crypto_ablkcipher_ivsize(crypto_ablkcipher_reqtfm(req)); */
+		v_iv = dma_alloc_coherent(&dev_data.pdev->dev, ivsize,
+				&p_iv, GFP_KERNEL);
+
+		memcpy(v_iv, req->info, ivsize);
+		cpu_to_be((u32 *)v_iv, ivsize);
+
+		dev_data.v_dma_descr[1].source = (u32)p_iv;
+		dev_data.v_dma_descr[1].source_len = ivsize;
+		dev_data.v_dma_descr[1].flags =
+			 EXT_DMA_DESC_CTRL_FULL | ENCOP_IV;
+
+		ret = ns921x_aes_crypt(req, 1);
+
+		dma_free_coherent(&dev_data.pdev->dev, ivsize, v_iv, p_iv);
+	}
+
+	return ret;
+return -ENOMEM;
+}
+
+static struct ablkcipher_alg ciphers[] = {
+	{
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.setkey		= ns921x_aes_setkey,
+		.encrypt	= ns921x_aes_ecb_encrypt,
+		.decrypt	= ns921x_aes_ecb_decrypt,
+	}, {
+
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.setkey		= ns921x_aes_setkey,
+		.encrypt	= ns921x_aes_cbc_encrypt,
+		.decrypt	= ns921x_aes_cbc_decrypt,
+	},
+};
+
+static struct crypto_alg ns921x_aes_algs[] = {
+	{
+		.cra_name = "ecb(aes)",
+		.cra_driver_name = DRIVER_NAME,
+		.cra_priority = 400,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+		.cra_blocksize = 16,
+		.cra_alignmask = 15,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = ns921x_aes_cra_init,
+		.cra_list = LIST_HEAD_INIT(ns921x_aes_algs[0].cra_list),
+	}, {
+		.cra_name = "cbc(aes)",
+		.cra_driver_name = DRIVER_NAME,
+		.cra_priority = 0,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+		.cra_blocksize = 16,
+		.cra_alignmask = 15,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = ns921x_aes_cra_init,
+		.cra_list = LIST_HEAD_INIT(ns921x_aes_algs[1].cra_list),
+	},
+};
+
+static int __init ns921x_aes_probe(struct platform_device *pdev)
+{
+	int i, ret = -ENOMEM;
+
+	memset(&dev_data, 0, sizeof(dev_data));
+	dev_data.pdev = pdev;
+
+	dev_data.mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!dev_data.mem) {
+		ret = -ENODEV;
+		dev_info(&pdev->dev, "err_get_mem\n");
+		goto err_get_mem;
+	}
+
+	if (!request_mem_region(dev_data.mem->start,
+				dev_data.mem->end - dev_data.mem->start + 1,
+				DRIVER_NAME)) {
+		ret = -EBUSY;
+		dev_info(&pdev->dev, "err_request_mem\n");
+		goto err_request_mem;
+	}
+
+	dev_data.membase = ioremap(dev_data.mem->start,
+			dev_data.mem->end - dev_data.mem->start + 1);
+	if (!dev_data.membase) {
+		ret = -EBUSY;
+		dev_info(&pdev->dev, "err_ioremap\n");
+		goto err_ioremap;
+	}
+
+	dev_data.irq = platform_get_irq(pdev, 0);
+	if (dev_data.irq <= 0) {
+		ret = -EINVAL;
+		dev_info(&pdev->dev, "err_get_irq\n");
+		goto err_get_irq;
+	}
+
+	ret = request_irq(dev_data.irq, ns921x_aes_int, 0, DRIVER_NAME, pdev);
+	if (ret) {
+		dev_info(&pdev->dev, "err_request_irq\n");
+		goto err_request_irq;
+	}
+
+	dev_data.clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(dev_data.clk)) {
+		ret = PTR_ERR(dev_data.clk);
+		dev_info(&pdev->dev, "err_clk_get\n");
+		goto err_clk_get;
+	}
+
+	ret = clk_enable(dev_data.clk);
+	if (ret) {
+		ret = -EBUSY;
+		dev_info(&pdev->dev, "err_clk_enable\n");
+		goto err_clk_enable;
+	}
+
+	/* register memory for maximal number of dma descriptors */
+	pdev->dev.coherent_dma_mask = (u32)-1;
+	dev_data.v_dma_descr = dma_alloc_coherent(&pdev->dev,
+			sizeof(*dev_data.v_dma_descr) * DMA_MAX_BUFFERS,
+			&dev_data.p_dma_descr, GFP_KERNEL);
+	if (!dev_data.v_dma_descr) {
+		ret = -ENOMEM;
+		goto err_dma_descr;
+	}
+
+	iowrite32(DMA_CONTROL_RST, dev_data.membase + DMA_CONTROL);
+	iowrite32((u32)dev_data.p_dma_descr, dev_data.membase + DMA_BDPOINTER);
+
+	for (i = 0; i < ARRAY_SIZE(ns921x_aes_algs); i++) {
+		ns921x_aes_algs[i].cra_u.ablkcipher = ciphers[i];
+		ret = crypto_register_alg(&ns921x_aes_algs[i]);
+		if (ret) {
+			dev_info(&pdev->dev, "err_register\n");
+			goto err_register;
+		}
+	}
+
+	init_waitqueue_head(&dev_data.waitq);
+	dev_info(&pdev->dev, "NS921x AES encryption/decryption module at 0x%p (irq: %d)\n",
+			dev_data.membase, dev_data.irq);
+
+	return 0;
+
+err_register:
+	while (--i >= 0)
+		crypto_unregister_alg(&ns921x_aes_algs[i]);
+	dma_free_coherent(&pdev->dev,
+			sizeof(*dev_data.v_dma_descr) * DMA_MAX_BUFFERS,
+			dev_data.v_dma_descr, dev_data.p_dma_descr);
+err_dma_descr:
+	clk_disable(dev_data.clk);
+err_clk_enable:
+	clk_put(dev_data.clk);
+err_clk_get:
+	free_irq(dev_data.irq, pdev);
+err_request_irq:
+err_get_irq:
+	iounmap(dev_data.membase);
+err_ioremap:
+	release_mem_region(dev_data.mem->start,
+			dev_data.mem->end - dev_data.mem->start + 1);
+err_request_mem:
+err_get_mem:
+
+	return ret;
+}
+
+static int __exit ns921x_aes_remove(struct platform_device *pdev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ns921x_aes_algs); i++)
+		crypto_unregister_alg(&ns921x_aes_algs[i]);
+
+	dma_free_coherent(&pdev->dev,
+			sizeof(*dev_data.v_dma_descr) * DMA_MAX_BUFFERS,
+			dev_data.v_dma_descr, dev_data.p_dma_descr);
+
+	clk_disable(dev_data.clk);
+	clk_put(dev_data.clk);
+
+	free_irq(dev_data.irq, pdev);
+
+	iounmap(dev_data.membase);
+	release_mem_region(dev_data.mem->start,
+			dev_data.mem->end - dev_data.mem->start + 1);
+
+	return 0;
+}
+
+static struct platform_driver ns921x_aes_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ns921x_aes_probe,
+	.remove		= __exit_p(ns921x_aes_remove),
+};
+
+static int __init ns921x_aes_init(void)
+{
+	return platform_driver_register(&ns921x_aes_driver);
+}
+
+static void __exit ns921x_aes_exit(void)
+{
+	platform_driver_unregister(&ns921x_aes_driver);
+}
+
+module_init(ns921x_aes_init);
+module_exit(ns921x_aes_exit);
+
+MODULE_DESCRIPTION("Digi ns921x AES algorithm support");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Digi International Inc.");
+
+MODULE_ALIAS("aes");
diff --git a/drivers/crypto/stmp3xxx_dcp.c b/drivers/crypto/stmp3xxx_dcp.c
new file mode 100644
index 000000000000..ff02ec807ec0
--- /dev/null
+++ b/drivers/crypto/stmp3xxx_dcp.c
@@ -0,0 +1,1401 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/* 
+ * Based on geode-aes.c 
+ * Copyright (C) 2004-2006, Advanced Micro Devices, Inc.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/crypto.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <crypto/algapi.h>
+#include <crypto/aes.h>
+#include <crypto/sha.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include <linux/io.h>
+#include <linux/delay.h>
+
+#include <asm/cacheflush.h>
+
+#include "stmp3xxx_dcp.h"
+
+/* SHA1 is busted */
+#define DISABLE_SHA1
+
+struct stmp3xxx_dcp {
+	struct device *dev;
+	spinlock_t lock;
+	struct mutex op_mutex[STMP3XXX_DCP_NUM_CHANNELS];
+	struct completion op_wait[STMP3XXX_DCP_NUM_CHANNELS];
+	int wait[STMP3XXX_DCP_NUM_CHANNELS];
+	int dcp_vmi_irq;
+	int dcp_irq;
+};
+
+/* cipher flags */
+#define STMP3XXX_DCP_ENC	0x0001
+#define STMP3XXX_DCP_DEC	0x0002
+#define STMP3XXX_DCP_ECB	0x0004
+#define STMP3XXX_DCP_CBC	0x0008
+#define STMP3XXX_DCP_CBC_INIT	0x0010
+#define STMP3XXX_DCP_OTPKEY	0x0020
+
+/* hash flags */
+#define STMP3XXX_DCP_INIT	0x0001
+#define STMP3XXX_DCP_UPDATE	0x0002
+#define STMP3XXX_DCP_FINAL	0x0004
+
+#define STMP3XXX_DCP_AES	0x1000
+#define STMP3XXX_DCP_SHA1	0x2000
+#define STMP3XXX_DCP_CRC32	0x3000
+#define STMP3XXX_DCP_COPY	0x4000
+#define STMP3XXX_DCP_FILL	0x5000
+#define STMP3XXX_DCP_MODE_MASK	0xf000
+
+struct stmp3xxx_dcp_op {
+
+	unsigned int flags;
+
+	void *src;
+	dma_addr_t src_phys;
+
+	void *dst;
+	dma_addr_t dst_phys;
+
+	int len;
+
+	/* the key contains the IV for block modes */
+	union {
+		struct {
+			u8 key[2 * AES_KEYSIZE_128]
+				__attribute__ ((__aligned__(32)));
+			dma_addr_t key_phys;
+			int keylen;
+		} cipher;
+		struct {
+			u8 digest[SHA1_DIGEST_SIZE]
+				__attribute__ ((__aligned__(32)));
+			dma_addr_t digest_phys;
+			int digestlen;
+			int init;
+		} hash;
+	};
+
+	union {
+		struct crypto_blkcipher *blk;
+		struct crypto_cipher *cip;
+		struct crypto_hash *hash;
+	} fallback;
+
+	struct stmp3xxx_dcp_hw_packet pkt
+		__attribute__ ((__aligned__(32)));
+};
+
+struct stmp3xxx_dcp_hash_coherent_block {
+	struct stmp3xxx_dcp_hw_packet pkt[2]
+		__attribute__ ((__aligned__(32)));
+	u8 digest[SHA1_DIGEST_SIZE]
+		__attribute__ ((__aligned__(32)));
+	u8 rembuf[32];
+};
+
+struct stmp3xxx_dcp_hash_op {
+
+	unsigned int flags;
+
+	void *src;
+	dma_addr_t src_phys;
+
+	void *dst;
+	dma_addr_t dst_phys;
+
+	int len;
+
+	/* the key contains the IV for block modes */
+	union {
+		struct {
+			u8 key[2 * AES_KEYSIZE_128]
+				__attribute__ ((__aligned__(32)));
+			dma_addr_t key_phys;
+			int keylen;
+		} cipher;
+		struct {
+			u8 digest[SHA1_DIGEST_SIZE]
+				__attribute__ ((__aligned__(32)));
+			dma_addr_t digest_phys;
+			int digestlen;
+			int init;
+		} hash;
+	};
+
+	union {
+		struct crypto_blkcipher *blk;
+		struct crypto_cipher *cip;
+		struct crypto_hash *hash;
+	} fallback;
+
+	struct stmp3xxx_dcp_hash_coherent_block *hw;
+	dma_addr_t hw_phys;
+};
+
+/* only one */
+static struct stmp3xxx_dcp *global_sdcp;
+
+static void dcp_perform_op(struct stmp3xxx_dcp_op *op)
+{
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+	struct mutex *mutex;
+	struct stmp3xxx_dcp_hw_packet *pkt;
+	int chan;
+	u32 pkt1, pkt2;
+	unsigned long timeout;
+	dma_addr_t pkt_phys;
+	u32 stat;
+
+	pkt1 = BM_DCP_PACKET1_DECR_SEMAPHORE | BM_DCP_PACKET1_INTERRUPT;
+
+	switch (op->flags & STMP3XXX_DCP_MODE_MASK) {
+
+	case STMP3XXX_DCP_AES:
+
+		chan = CIPHER_CHAN;
+
+		/* key is at the payload */
+		pkt1 |= BM_DCP_PACKET1_ENABLE_CIPHER;
+		if ((op->flags & STMP3XXX_DCP_OTPKEY) == 0)
+			pkt1 |= BM_DCP_PACKET1_PAYLOAD_KEY;
+		if (op->flags & STMP3XXX_DCP_ENC)
+			pkt1 |= BM_DCP_PACKET1_CIPHER_ENCRYPT;
+		if (op->flags & STMP3XXX_DCP_CBC_INIT)
+			pkt1 |= BM_DCP_PACKET1_CIPHER_INIT;
+
+		pkt2 = BF_DCP_PACKET2_CIPHER_CFG(0) |
+		       BF_DCP_PACKET2_KEY_SELECT(0) |
+		       BF_DCP_PACKET2_CIPHER_SELECT(
+			BV_DCP_PACKET2_CIPHER_SELECT__AES128);
+
+		if (op->flags & STMP3XXX_DCP_ECB)
+			pkt2 |= BF_DCP_PACKET2_CIPHER_MODE(
+				BV_DCP_PACKET2_CIPHER_MODE__ECB);
+		else if (op->flags & STMP3XXX_DCP_CBC)
+			pkt2 |= BF_DCP_PACKET2_CIPHER_MODE(
+				BV_DCP_PACKET2_CIPHER_MODE__CBC);
+
+		break;
+
+	case STMP3XXX_DCP_SHA1:
+
+		chan = HASH_CHAN;
+
+		pkt1 |= BM_DCP_PACKET1_ENABLE_HASH;
+		if (op->flags & STMP3XXX_DCP_INIT)
+			pkt1 |= BM_DCP_PACKET1_HASH_INIT;
+		if (op->flags & STMP3XXX_DCP_FINAL) {
+			pkt1 |= BM_DCP_PACKET1_HASH_TERM;
+			BUG_ON(op->hash.digest == NULL);
+		}
+
+		pkt2 = BF_DCP_PACKET2_HASH_SELECT(
+			BV_DCP_PACKET2_HASH_SELECT__SHA1);
+		break;
+
+	default:
+		dev_err(sdcp->dev, "Unsupported mode\n");
+		return;
+	}
+
+	mutex = &sdcp->op_mutex[chan];
+	pkt = &op->pkt;
+
+	pkt->pNext = 0;
+	pkt->pkt1 = pkt1;
+	pkt->pkt2 = pkt2;
+	pkt->pSrc = (u32)op->src_phys;
+	pkt->pDst = (u32)op->dst_phys;
+	pkt->size = op->len;
+	pkt->pPayload = chan == CIPHER_CHAN ?
+		(u32)op->cipher.key_phys : (u32)op->hash.digest_phys;
+	pkt->stat = 0;
+
+	pkt_phys = dma_map_single(sdcp->dev, pkt, sizeof(*pkt),
+			DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(sdcp->dev, pkt_phys)) {
+		dev_err(sdcp->dev, "Unable to map packet descriptor\n");
+		return;
+	}
+
+	/* submit the work */
+	mutex_lock(mutex);
+
+	HW_DCP_CHnSTAT_CLR(chan, -1);
+
+	/* Load the work packet pointer and bump the channel semaphore */
+	HW_DCP_CHnCMDPTR_WR(chan, (u32)pkt_phys);
+
+	/* XXX wake from interrupt instead of looping */
+	timeout = jiffies + msecs_to_jiffies(1000);
+
+	sdcp->wait[chan] = 0;
+	HW_DCP_CHnSEMA_WR(chan, BF_DCP_CHnSEMA_INCREMENT(1));
+	while (time_before(jiffies, timeout) && sdcp->wait[chan] == 0)
+		cpu_relax();
+
+	if (!time_before(jiffies, timeout)) {
+		dev_err(sdcp->dev, "Timeout while waiting STAT 0x%08x\n",
+				HW_DCP_STAT_RD());
+		goto out;
+	}
+
+	stat = HW_DCP_CHnSTAT_RD(chan);
+	if ((stat & 0xff) != 0)
+		dev_err(sdcp->dev, "Channel stat error 0x%02x\n",
+				HW_DCP_CHnSTAT_RD(chan) & 0xff);
+out:
+	mutex_unlock(mutex);
+
+	dma_unmap_single(sdcp->dev, pkt_phys, sizeof(*pkt), DMA_TO_DEVICE);
+}
+
+static int dcp_aes_setkey_cip(struct crypto_tfm *tfm, const u8 *key,
+		unsigned int len)
+{
+	struct stmp3xxx_dcp_op *op = crypto_tfm_ctx(tfm);
+	unsigned int ret;
+
+	op->cipher.keylen = len;
+
+	if (len == AES_KEYSIZE_128) {
+		memcpy(op->cipher.key, key, len);
+		return 0;
+	}
+
+	if (len != AES_KEYSIZE_192 && len != AES_KEYSIZE_256) {
+		/* not supported at all */
+		tfm->crt_flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		return -EINVAL;
+	}
+
+	/*
+	 * The requested key size is not supported by HW, do a fallback
+	 */
+	op->fallback.blk->base.crt_flags &= ~CRYPTO_TFM_REQ_MASK;
+	op->fallback.blk->base.crt_flags |= (tfm->crt_flags &
+						CRYPTO_TFM_REQ_MASK);
+
+	ret = crypto_cipher_setkey(op->fallback.cip, key, len);
+	if (ret) {
+		tfm->crt_flags &= ~CRYPTO_TFM_RES_MASK;
+		tfm->crt_flags |= (op->fallback.blk->base.crt_flags &
+					CRYPTO_TFM_RES_MASK);
+	}
+	return ret;
+}
+
+static void dcp_aes_encrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+	struct stmp3xxx_dcp_op *op = crypto_tfm_ctx(tfm);
+
+	if (unlikely(op->cipher.keylen != AES_KEYSIZE_128)) {
+		crypto_cipher_encrypt_one(op->fallback.cip, out, in);
+		return;
+	}
+
+	op->src = (void *) in;
+	op->dst = (void *) out;
+	op->flags = STMP3XXX_DCP_AES | STMP3XXX_DCP_ENC | STMP3XXX_DCP_ECB;
+	op->len = AES_KEYSIZE_128;
+
+	/* map the data */
+	op->src_phys = dma_map_single(sdcp->dev, (void *)in, AES_KEYSIZE_128,
+					DMA_TO_DEVICE);
+	if (dma_mapping_error(sdcp->dev, op->src_phys)) {
+		dev_err(sdcp->dev, "Unable to map source\n");
+		return;
+	}
+
+	op->dst_phys = dma_map_single(sdcp->dev, out, AES_KEYSIZE_128,
+					DMA_FROM_DEVICE);
+	if (dma_mapping_error(sdcp->dev, op->dst_phys)) {
+		dev_err(sdcp->dev, "Unable to map dest\n");
+		goto err_unmap_src;
+	}
+
+	op->cipher.key_phys = dma_map_single(sdcp->dev, op->cipher.key,
+					AES_KEYSIZE_128, DMA_TO_DEVICE);
+	if (dma_mapping_error(sdcp->dev, op->cipher.key_phys)) {
+		dev_err(sdcp->dev, "Unable to map key\n");
+		goto err_unmap_dst;
+	}
+
+	/* perform the operation */
+	dcp_perform_op(op);
+
+	dma_unmap_single(sdcp->dev, op->cipher.key_phys, AES_KEYSIZE_128,
+			DMA_TO_DEVICE);
+err_unmap_dst:
+	dma_unmap_single(sdcp->dev, op->dst_phys, op->len, DMA_FROM_DEVICE);
+err_unmap_src:
+	dma_unmap_single(sdcp->dev, op->src_phys, op->len, DMA_TO_DEVICE);
+}
+
+static void dcp_aes_decrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+	struct stmp3xxx_dcp_op *op = crypto_tfm_ctx(tfm);
+
+	if (unlikely(op->cipher.keylen != AES_KEYSIZE_128)) {
+		crypto_cipher_decrypt_one(op->fallback.cip, out, in);
+		return;
+	}
+
+	op->src = (void *) in;
+	op->dst = (void *) out;
+	op->flags = STMP3XXX_DCP_AES | STMP3XXX_DCP_DEC | STMP3XXX_DCP_ECB;
+	op->len = AES_KEYSIZE_128;
+
+	/* map the data */
+	op->src_phys = dma_map_single(sdcp->dev, (void *)in, AES_KEYSIZE_128,
+					DMA_TO_DEVICE);
+	if (dma_mapping_error(sdcp->dev, op->src_phys)) {
+		dev_err(sdcp->dev, "Unable to map source\n");
+		return;
+	}
+
+	op->dst_phys = dma_map_single(sdcp->dev, out, AES_KEYSIZE_128,
+					DMA_FROM_DEVICE);
+	if (dma_mapping_error(sdcp->dev, op->dst_phys)) {
+		dev_err(sdcp->dev, "Unable to map dest\n");
+		goto err_unmap_src;
+	}
+
+	op->cipher.key_phys = dma_map_single(sdcp->dev, op->cipher.key,
+					AES_KEYSIZE_128, DMA_TO_DEVICE);
+	if (dma_mapping_error(sdcp->dev, op->cipher.key_phys)) {
+		dev_err(sdcp->dev, "Unable to map key\n");
+		goto err_unmap_dst;
+	}
+
+	/* perform the operation */
+	dcp_perform_op(op);
+
+	dma_unmap_single(sdcp->dev, op->cipher.key_phys, AES_KEYSIZE_128,
+			DMA_TO_DEVICE);
+err_unmap_dst:
+	dma_unmap_single(sdcp->dev, op->dst_phys, op->len, DMA_FROM_DEVICE);
+err_unmap_src:
+	dma_unmap_single(sdcp->dev, op->src_phys, op->len, DMA_TO_DEVICE);
+}
+
+static int fallback_init_cip(struct crypto_tfm *tfm)
+{
+	const char *name = tfm->__crt_alg->cra_name;
+	struct stmp3xxx_dcp_op *op = crypto_tfm_ctx(tfm);
+
+	op->fallback.cip = crypto_alloc_cipher(name, 0,
+				CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK);
+
+	if (IS_ERR(op->fallback.cip)) {
+		printk(KERN_ERR "Error allocating fallback algo %s\n", name);
+		return PTR_ERR(op->fallback.cip);
+	}
+
+	return 0;
+}
+
+static void fallback_exit_cip(struct crypto_tfm *tfm)
+{
+	struct stmp3xxx_dcp_op *op = crypto_tfm_ctx(tfm);
+
+	crypto_free_cipher(op->fallback.cip);
+	op->fallback.cip = NULL;
+}
+
+static struct crypto_alg dcp_aes_alg = {
+	.cra_name		= "aes",
+	.cra_driver_name	= "dcp-aes",
+	.cra_priority		= 300,
+	.cra_alignmask		= 15,
+	.cra_flags		= CRYPTO_ALG_TYPE_CIPHER |
+				  CRYPTO_ALG_NEED_FALLBACK,
+	.cra_init		= fallback_init_cip,
+	.cra_exit		= fallback_exit_cip,
+	.cra_blocksize		= AES_KEYSIZE_128,
+	.cra_ctxsize		= sizeof(struct stmp3xxx_dcp_op),
+	.cra_module		= THIS_MODULE,
+	.cra_list		= LIST_HEAD_INIT(dcp_aes_alg.cra_list),
+	.cra_u			= {
+		.cipher	= {
+			.cia_min_keysize	= AES_MIN_KEY_SIZE,
+			.cia_max_keysize	= AES_MAX_KEY_SIZE,
+			.cia_setkey		= dcp_aes_setkey_cip,
+			.cia_encrypt		= dcp_aes_encrypt,
+			.cia_decrypt		= dcp_aes_decrypt
+		}
+	}
+};
+
+static int dcp_aes_setkey_blk(struct crypto_tfm *tfm, const u8 *key,
+		unsigned int len)
+{
+	struct stmp3xxx_dcp_op *op = crypto_tfm_ctx(tfm);
+	unsigned int ret;
+
+	op->cipher.keylen = len;
+
+	if (len == AES_KEYSIZE_128) {
+		memcpy(op->cipher.key, key, len);
+		return 0;
+	}
+
+	if (len != AES_KEYSIZE_192 && len != AES_KEYSIZE_256) {
+		/* not supported at all */
+		tfm->crt_flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		return -EINVAL;
+	}
+
+	/*
+	 * The requested key size is not supported by HW, do a fallback
+	 */
+	op->fallback.blk->base.crt_flags &= ~CRYPTO_TFM_REQ_MASK;
+	op->fallback.blk->base.crt_flags |= (tfm->crt_flags &
+						CRYPTO_TFM_REQ_MASK);
+
+	ret = crypto_blkcipher_setkey(op->fallback.blk, key, len);
+	if (ret) {
+		tfm->crt_flags &= ~CRYPTO_TFM_RES_MASK;
+		tfm->crt_flags |= (op->fallback.blk->base.crt_flags &
+					CRYPTO_TFM_RES_MASK);
+	}
+	return ret;
+}
+
+static int fallback_blk_dec(struct blkcipher_desc *desc,
+		struct scatterlist *dst, struct scatterlist *src,
+		unsigned int nbytes)
+{
+	unsigned int ret;
+	struct crypto_blkcipher *tfm;
+	struct stmp3xxx_dcp_op *op = crypto_blkcipher_ctx(desc->tfm);
+
+	tfm = desc->tfm;
+	desc->tfm = op->fallback.blk;
+
+	ret = crypto_blkcipher_decrypt_iv(desc, dst, src, nbytes);
+
+	desc->tfm = tfm;
+	return ret;
+}
+
+static int fallback_blk_enc(struct blkcipher_desc *desc,
+		struct scatterlist *dst, struct scatterlist *src,
+		unsigned int nbytes)
+{
+	unsigned int ret;
+	struct crypto_blkcipher *tfm;
+	struct stmp3xxx_dcp_op *op = crypto_blkcipher_ctx(desc->tfm);
+
+	tfm = desc->tfm;
+	desc->tfm = op->fallback.blk;
+
+	ret = crypto_blkcipher_encrypt_iv(desc, dst, src, nbytes);
+
+	desc->tfm = tfm;
+	return ret;
+}
+
+static int fallback_init_blk(struct crypto_tfm *tfm)
+{
+	const char *name = tfm->__crt_alg->cra_name;
+	struct stmp3xxx_dcp_op *op = crypto_tfm_ctx(tfm);
+
+	op->fallback.blk = crypto_alloc_blkcipher(name, 0,
+			CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK);
+
+	if (IS_ERR(op->fallback.blk)) {
+		printk(KERN_ERR "Error allocating fallback algo %s\n", name);
+		return PTR_ERR(op->fallback.blk);
+	}
+
+	return 0;
+}
+
+static void fallback_exit_blk(struct crypto_tfm *tfm)
+{
+	struct stmp3xxx_dcp_op *op = crypto_tfm_ctx(tfm);
+
+	crypto_free_blkcipher(op->fallback.blk);
+	op->fallback.blk = NULL;
+}
+
+static int
+dcp_aes_ecb_decrypt(struct blkcipher_desc *desc,
+		  struct scatterlist *dst, struct scatterlist *src,
+		  unsigned int nbytes)
+{
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+	struct stmp3xxx_dcp_op *op = crypto_blkcipher_ctx(desc->tfm);
+	struct blkcipher_walk walk;
+	int err;
+
+	if (unlikely(op->cipher.keylen != AES_KEYSIZE_128))
+		return fallback_blk_dec(desc, dst, src, nbytes);
+
+	blkcipher_walk_init(&walk, dst, src, nbytes);
+
+	/* key needs to be mapped only once */
+	op->cipher.key_phys = dma_map_single(sdcp->dev, op->cipher.key,
+				AES_KEYSIZE_128, DMA_TO_DEVICE);
+	if (dma_mapping_error(sdcp->dev, op->cipher.key_phys)) {
+		dev_err(sdcp->dev, "Unable to map key\n");
+		return -ENOMEM;
+	}
+
+	err = blkcipher_walk_virt(desc, &walk);
+	while (err == 0 && (nbytes = walk.nbytes) > 0) {
+		op->src = walk.src.virt.addr,
+		op->dst = walk.dst.virt.addr;
+		op->flags = STMP3XXX_DCP_AES | STMP3XXX_DCP_DEC |
+				STMP3XXX_DCP_ECB;
+		op->len = nbytes - (nbytes % AES_KEYSIZE_128);
+
+		/* map the data */
+		op->src_phys = dma_map_single(sdcp->dev, op->src, op->len,
+						DMA_TO_DEVICE);
+		if (dma_mapping_error(sdcp->dev, op->src_phys)) {
+			dev_err(sdcp->dev, "Unable to map source\n");
+			err = -ENOMEM;
+			break;
+		}
+
+		op->dst_phys = dma_map_single(sdcp->dev, op->dst, op->len,
+						DMA_FROM_DEVICE);
+		if (dma_mapping_error(sdcp->dev, op->dst_phys)) {
+			dma_unmap_single(sdcp->dev, op->src_phys, op->len,
+						DMA_TO_DEVICE);
+			dev_err(sdcp->dev, "Unable to map dest\n");
+			err = -ENOMEM;
+			break;
+		}
+
+		/* perform! */
+		dcp_perform_op(op);
+
+		dma_unmap_single(sdcp->dev, op->dst_phys, op->len,
+					DMA_FROM_DEVICE);
+		dma_unmap_single(sdcp->dev, op->src_phys, op->len,
+					DMA_TO_DEVICE);
+
+		nbytes -= op->len;
+		err = blkcipher_walk_done(desc, &walk, nbytes);
+	}
+
+	dma_unmap_single(sdcp->dev, op->cipher.key_phys, AES_KEYSIZE_128,
+				DMA_TO_DEVICE);
+
+	return err;
+}
+
+static int
+dcp_aes_ecb_encrypt(struct blkcipher_desc *desc,
+		  struct scatterlist *dst, struct scatterlist *src,
+		  unsigned int nbytes)
+{
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+	struct stmp3xxx_dcp_op *op = crypto_blkcipher_ctx(desc->tfm);
+	struct blkcipher_walk walk;
+	int err, ret;
+
+	if (unlikely(op->cipher.keylen != AES_KEYSIZE_128))
+		return fallback_blk_enc(desc, dst, src, nbytes);
+
+	blkcipher_walk_init(&walk, dst, src, nbytes);
+
+	/* key needs to be mapped only once */
+	op->cipher.key_phys = dma_map_single(sdcp->dev, op->cipher.key,
+				AES_KEYSIZE_128, DMA_TO_DEVICE);
+	if (dma_mapping_error(sdcp->dev, op->cipher.key_phys)) {
+		dev_err(sdcp->dev, "Unable to map key\n");
+		return -ENOMEM;
+	}
+
+	err = blkcipher_walk_virt(desc, &walk);
+
+	err = 0;
+	while (err == 0 && (nbytes = walk.nbytes) > 0) {
+		op->src = walk.src.virt.addr,
+		op->dst = walk.dst.virt.addr;
+		op->flags = STMP3XXX_DCP_AES | STMP3XXX_DCP_ENC |
+			    STMP3XXX_DCP_ECB;
+		op->len = nbytes - (nbytes % AES_KEYSIZE_128);
+
+		/* map the data */
+		op->src_phys = dma_map_single(sdcp->dev, op->src, op->len,
+				DMA_TO_DEVICE);
+		if (dma_mapping_error(sdcp->dev, op->src_phys)) {
+			dev_err(sdcp->dev, "Unable to map source\n");
+			err = -ENOMEM;
+			break;
+		}
+
+		op->dst_phys = dma_map_single(sdcp->dev, op->dst, op->len,
+				DMA_FROM_DEVICE);
+		if (dma_mapping_error(sdcp->dev, op->dst_phys)) {
+			dma_unmap_single(sdcp->dev, op->src_phys, op->len,
+					DMA_TO_DEVICE);
+			dev_err(sdcp->dev, "Unable to map dest\n");
+			err = -ENOMEM;
+			break;
+		}
+
+		/* perform! */
+		dcp_perform_op(op);
+
+		dma_unmap_single(sdcp->dev, op->dst_phys, op->len,
+				DMA_FROM_DEVICE);
+		dma_unmap_single(sdcp->dev, op->src_phys, op->len,
+				DMA_TO_DEVICE);
+
+		nbytes -= op->len;
+		ret =  blkcipher_walk_done(desc, &walk, nbytes);
+	}
+
+	dma_unmap_single(sdcp->dev, op->cipher.key_phys, AES_KEYSIZE_128,
+			DMA_TO_DEVICE);
+
+	return err;
+}
+
+
+static struct crypto_alg dcp_aes_ecb_alg = {
+	.cra_name		= "ecb(aes)",
+	.cra_driver_name	= "dcp-ecb-aes",
+	.cra_priority		= 400,
+	.cra_alignmask		= 15,
+	.cra_flags		= CRYPTO_ALG_TYPE_BLKCIPHER |
+				  CRYPTO_ALG_NEED_FALLBACK,
+	.cra_init		= fallback_init_blk,
+	.cra_exit		= fallback_exit_blk,
+	.cra_blocksize		= AES_KEYSIZE_128,
+	.cra_ctxsize		= sizeof(struct stmp3xxx_dcp_op),
+	.cra_type		= &crypto_blkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_list		= LIST_HEAD_INIT(dcp_aes_ecb_alg.cra_list),
+	.cra_u			= {
+		.blkcipher 	= {
+			.min_keysize	= AES_MIN_KEY_SIZE,
+			.max_keysize	= AES_MAX_KEY_SIZE,
+			.setkey		= dcp_aes_setkey_blk,
+			.encrypt	= dcp_aes_ecb_encrypt,
+			.decrypt	= dcp_aes_ecb_decrypt
+		}
+	}
+};
+
+static int
+dcp_aes_cbc_decrypt(struct blkcipher_desc *desc,
+		  struct scatterlist *dst, struct scatterlist *src,
+		  unsigned int nbytes)
+{
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+	struct stmp3xxx_dcp_op *op = crypto_blkcipher_ctx(desc->tfm);
+	struct blkcipher_walk walk;
+	int err, blockno;
+
+	if (unlikely(op->cipher.keylen != AES_KEYSIZE_128))
+		return fallback_blk_dec(desc, dst, src, nbytes);
+
+	blkcipher_walk_init(&walk, dst, src, nbytes);
+
+	blockno = 0;
+	err = blkcipher_walk_virt(desc, &walk);
+	while (err == 0 && (nbytes = walk.nbytes) > 0) {
+		op->src = walk.src.virt.addr,
+		op->dst = walk.dst.virt.addr;
+		op->flags = STMP3XXX_DCP_AES | STMP3XXX_DCP_DEC |
+			    STMP3XXX_DCP_CBC;
+		if (blockno == 0) {
+			op->flags |= STMP3XXX_DCP_CBC_INIT;
+			memcpy(op->cipher.key + AES_KEYSIZE_128, walk.iv,
+				AES_KEYSIZE_128);
+		}
+		op->len = nbytes - (nbytes % AES_KEYSIZE_128);
+
+		/* key (+iv) needs to be mapped only once */
+		op->cipher.key_phys = dma_map_single(sdcp->dev, op->cipher.key,
+					AES_KEYSIZE_128 * 2, DMA_BIDIRECTIONAL);
+		if (dma_mapping_error(sdcp->dev, op->cipher.key_phys)) {
+			dev_err(sdcp->dev, "Unable to map key\n");
+			err = -ENOMEM;
+			break;
+		}
+
+		/* map the data */
+		op->src_phys = dma_map_single(sdcp->dev, op->src, op->len,
+					DMA_TO_DEVICE);
+		if (dma_mapping_error(sdcp->dev, op->src_phys)) {
+			dma_unmap_single(sdcp->dev, op->cipher.key_phys,
+					AES_KEYSIZE_128 * 2, DMA_BIDIRECTIONAL);
+			dev_err(sdcp->dev, "Unable to map source\n");
+			err = -ENOMEM;
+			break;
+		}
+
+		op->dst_phys = dma_map_single(sdcp->dev, op->dst, op->len,
+						DMA_FROM_DEVICE);
+		if (dma_mapping_error(sdcp->dev, op->dst_phys)) {
+			dma_unmap_single(sdcp->dev, op->cipher.key_phys,
+					AES_KEYSIZE_128 * 2, DMA_BIDIRECTIONAL);
+			dma_unmap_single(sdcp->dev, op->src_phys, op->len,
+					DMA_TO_DEVICE);
+			dev_err(sdcp->dev, "Unable to map dest\n");
+			err = -ENOMEM;
+			break;
+		}
+
+		/* perform! */
+		dcp_perform_op(op);
+
+		dma_unmap_single(sdcp->dev, op->cipher.key_phys,
+					AES_KEYSIZE_128 * 2, DMA_BIDIRECTIONAL);
+		dma_unmap_single(sdcp->dev, op->dst_phys, op->len,
+					DMA_FROM_DEVICE);
+		dma_unmap_single(sdcp->dev, op->src_phys, op->len,
+					DMA_TO_DEVICE);
+
+		nbytes -= op->len;
+		err = blkcipher_walk_done(desc, &walk, nbytes);
+
+		blockno++;
+	}
+
+	return err;
+}
+
+static int
+dcp_aes_cbc_encrypt(struct blkcipher_desc *desc,
+		  struct scatterlist *dst, struct scatterlist *src,
+		  unsigned int nbytes)
+{
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+	struct stmp3xxx_dcp_op *op = crypto_blkcipher_ctx(desc->tfm);
+	struct blkcipher_walk walk;
+	int err, ret, blockno;
+
+	if (unlikely(op->cipher.keylen != AES_KEYSIZE_128))
+		return fallback_blk_enc(desc, dst, src, nbytes);
+
+	blkcipher_walk_init(&walk, dst, src, nbytes);
+
+	blockno = 0;
+
+	err = blkcipher_walk_virt(desc, &walk);
+	while (err == 0 && (nbytes = walk.nbytes) > 0) {
+		op->src = walk.src.virt.addr,
+		op->dst = walk.dst.virt.addr;
+		op->flags = STMP3XXX_DCP_AES | STMP3XXX_DCP_ENC |
+			    STMP3XXX_DCP_CBC;
+		if (blockno == 0) {
+			op->flags |= STMP3XXX_DCP_CBC_INIT;
+			memcpy(op->cipher.key + AES_KEYSIZE_128, walk.iv,
+				AES_KEYSIZE_128);
+		}
+		op->len = nbytes - (nbytes % AES_KEYSIZE_128);
+
+		/* key needs to be mapped only once */
+		op->cipher.key_phys = dma_map_single(sdcp->dev, op->cipher.key,
+					AES_KEYSIZE_128 * 2, DMA_BIDIRECTIONAL);
+		if (dma_mapping_error(sdcp->dev, op->cipher.key_phys)) {
+			dev_err(sdcp->dev, "Unable to map key\n");
+			return -ENOMEM;
+		}
+
+		/* map the data */
+		op->src_phys = dma_map_single(sdcp->dev, op->src, op->len,
+				DMA_TO_DEVICE);
+		if (dma_mapping_error(sdcp->dev, op->src_phys)) {
+			dma_unmap_single(sdcp->dev, op->cipher.key_phys,
+				AES_KEYSIZE_128 * 2, DMA_BIDIRECTIONAL);
+			dev_err(sdcp->dev, "Unable to map source\n");
+			err = -ENOMEM;
+			break;
+		}
+
+		op->dst_phys = dma_map_single(sdcp->dev, op->dst, op->len,
+				DMA_FROM_DEVICE);
+		if (dma_mapping_error(sdcp->dev, op->dst_phys)) {
+			dma_unmap_single(sdcp->dev, op->cipher.key_phys,
+					AES_KEYSIZE_128 * 2, DMA_BIDIRECTIONAL);
+			dma_unmap_single(sdcp->dev, op->src_phys, op->len,
+					DMA_TO_DEVICE);
+			dev_err(sdcp->dev, "Unable to map dest\n");
+			err = -ENOMEM;
+			break;
+		}
+
+		/* perform! */
+		dcp_perform_op(op);
+
+		dma_unmap_single(sdcp->dev, op->cipher.key_phys,
+				AES_KEYSIZE_128 * 2, DMA_BIDIRECTIONAL);
+		dma_unmap_single(sdcp->dev, op->dst_phys, op->len,
+				DMA_FROM_DEVICE);
+		dma_unmap_single(sdcp->dev, op->src_phys, op->len,
+				DMA_TO_DEVICE);
+
+		nbytes -= op->len;
+		ret =  blkcipher_walk_done(desc, &walk, nbytes);
+
+		blockno++;
+	}
+
+	return err;
+}
+
+static struct crypto_alg dcp_aes_cbc_alg = {
+	.cra_name		= "cbc(aes)",
+	.cra_driver_name	= "dcp-cbc-aes",
+	.cra_priority		= 400,
+	.cra_alignmask		= 15,
+	.cra_flags		= CRYPTO_ALG_TYPE_BLKCIPHER |
+				  CRYPTO_ALG_NEED_FALLBACK,
+	.cra_init		= fallback_init_blk,
+	.cra_exit		= fallback_exit_blk,
+	.cra_blocksize		= AES_KEYSIZE_128,
+	.cra_ctxsize		= sizeof(struct stmp3xxx_dcp_op),
+	.cra_type		= &crypto_blkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_list		= LIST_HEAD_INIT(dcp_aes_cbc_alg.cra_list),
+	.cra_u			= {
+		.blkcipher 	= {
+			.min_keysize	= AES_MIN_KEY_SIZE,
+			.max_keysize	= AES_MAX_KEY_SIZE,
+			.setkey		= dcp_aes_setkey_blk,
+			.encrypt	= dcp_aes_cbc_encrypt,
+			.decrypt	= dcp_aes_cbc_decrypt,
+			.ivsize		= AES_KEYSIZE_128,
+		}
+	}
+};
+
+#ifndef DISABLE_SHA1
+
+static void dcp_perform_hash_op(struct stmp3xxx_dcp_hash_op *op)
+{
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+	struct mutex *mutex;
+	int chan;
+	struct stmp3xxx_dcp_hw_packet *pkt;
+	unsigned long timeout;
+	u32 stat;
+	int size1, descno;
+
+	BUG_ON((op->flags & STMP3XXX_DCP_MODE_MASK) != STMP3XXX_DCP_SHA1);
+
+	chan = HASH_CHAN;
+
+	switch (op->flags & (STMP3XXX_DCP_INIT | STMP3XXX_DCP_UPDATE |
+			     STMP3XXX_DCP_FINAL)) {
+
+	case STMP3XXX_DCP_INIT | STMP3XXX_DCP_UPDATE:
+
+		BUG_ON(op->len <= 1);
+
+		pkt = &op->hw->pkt[0];
+		pkt->pNext = 0;
+		pkt->pkt1 = BM_DCP_PACKET1_HASH_INIT |
+			    BM_DCP_PACKET1_DECR_SEMAPHORE |
+			    BM_DCP_PACKET1_INTERRUPT |
+			    BM_DCP_PACKET1_ENABLE_HASH;
+		pkt->pkt2 = BF_DCP_PACKET2_HASH_SELECT(
+				BV_DCP_PACKET2_HASH_SELECT__SHA1);
+		pkt->pSrc = (u32)op->src_phys;
+		pkt->pDst = 0;
+		pkt->size = op->len - 1;
+		pkt->pPayload = 0;
+		pkt->stat = 0;
+
+		/* save last byte */
+		op->hw->rembuf[1] = ((u8 *)op->src)[op->len - 1];
+
+		descno = 1;
+		break;
+
+	case STMP3XXX_DCP_UPDATE:
+
+		BUG_ON(op->len <= 1);
+
+		op->hw->rembuf[0] = op->hw->rembuf[1];
+
+		pkt = &op->hw->pkt[0];
+		pkt->pNext = 0;
+		pkt->pkt1 = BM_DCP_PACKET1_CHAIN_CONTIGUOUS |
+			    BM_DCP_PACKET1_ENABLE_HASH;
+		pkt->pkt2 = BF_DCP_PACKET2_HASH_SELECT(
+				BV_DCP_PACKET2_HASH_SELECT__SHA1);
+		pkt->pSrc = (u32)op->hw_phys + offsetof(
+			struct stmp3xxx_dcp_hash_coherent_block, rembuf);
+		pkt->pDst = 0;
+		pkt->size = 1;
+		pkt->pPayload = 0;
+		pkt->stat = 0;
+
+		pkt = &op->hw->pkt[1];
+		pkt->pNext = 0;
+		pkt->pkt1 = BM_DCP_PACKET1_DECR_SEMAPHORE |
+			    BM_DCP_PACKET1_INTERRUPT |
+			    BM_DCP_PACKET1_ENABLE_HASH;
+		pkt->pkt2 = BF_DCP_PACKET2_HASH_SELECT(
+				BV_DCP_PACKET2_HASH_SELECT__SHA1);
+		pkt->pSrc = (u32)op->src_phys;
+		pkt->pDst = 0;
+		pkt->size = op->len - 1;
+		pkt->pPayload = 0;
+		pkt->stat = 0;
+
+		/* save last byte */
+		op->hw->rembuf[1] = ((u8 *)op->src)[op->len - 1];
+
+		descno = 2;
+
+		break;
+
+	case STMP3XXX_DCP_FINAL:
+
+		op->hw->rembuf[0] = op->hw->rembuf[1];
+
+		pkt = &op->hw->pkt[0];
+		pkt->pNext = 0;
+		pkt->pkt1 = BM_DCP_PACKET1_HASH_TERM |
+			    BM_DCP_PACKET1_DECR_SEMAPHORE |
+			    BM_DCP_PACKET1_INTERRUPT |
+			    BM_DCP_PACKET1_ENABLE_HASH;
+		pkt->pkt2 = BF_DCP_PACKET2_HASH_SELECT(
+				BV_DCP_PACKET2_HASH_SELECT__SHA1);
+		pkt->pSrc = (u32)op->hw_phys + offsetof(
+			struct stmp3xxx_dcp_hash_coherent_block, rembuf);
+		pkt->pDst = 0;
+		pkt->size = 1;
+		pkt->pPayload = (u32)op->hw_phys + offsetof(
+			struct stmp3xxx_dcp_hash_coherent_block, digest);
+		pkt->stat = 0;
+
+		descno = 1;
+
+		break;
+
+		/* all others BUG */
+	default:
+		BUG();
+		return;
+	}
+
+	mutex = &sdcp->op_mutex[chan];
+
+	/* submit the work */
+	mutex_lock(mutex);
+
+	HW_DCP_CHnSTAT_CLR(chan, -1);
+
+	mb();
+	/* Load the work packet pointer and bump the channel semaphore */
+	HW_DCP_CHnCMDPTR_WR(chan, (u32)op->hw_phys +
+		offsetof(struct stmp3xxx_dcp_hash_coherent_block, pkt[0]));
+
+	/* XXX wake from interrupt instead of looping */
+	timeout = jiffies + msecs_to_jiffies(1000);
+
+	HW_DCP_CHnSEMA_WR(chan, BF_DCP_CHnSEMA_INCREMENT(descno));
+
+	while (time_before(jiffies, timeout) &&
+		((HW_DCP_CHnSEMA_RD(chan) >> 16) & 0xff) != 0)
+		cpu_relax();
+
+	if (!time_before(jiffies, timeout)) {
+		dev_err(sdcp->dev, "Timeout while waiting STAT 0x%08x\n",
+				HW_DCP_STAT_RD());
+		goto out;
+	}
+
+	stat = HW_DCP_CHnSTAT_RD(chan);
+	if ((stat & 0xff) != 0)
+		dev_err(sdcp->dev, "Channel stat error 0x%02x\n",
+				HW_DCP_CHnSTAT_RD(chan) & 0xff);
+
+out:
+	mutex_unlock(mutex);
+
+	mdelay(500);
+}
+
+static int fallback_init_hash(struct crypto_tfm *tfm)
+{
+	const char *name = tfm->__crt_alg->cra_name;
+	struct stmp3xxx_dcp_hash_op *op = crypto_tfm_ctx(tfm);
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+
+	op->fallback.hash = crypto_alloc_hash(name, 0,
+			CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(op->fallback.hash)) {
+		printk(KERN_ERR "Error allocating fallback algo %s\n", name);
+		return PTR_ERR(op->fallback.hash);
+	}
+
+	op->hw = dma_alloc_coherent(sdcp->dev, sizeof(*op->hw), &op->hw_phys,
+			GFP_KERNEL);
+	if (op->hw == NULL) {
+		printk(KERN_ERR "Error allocating coherent block for %s\n",
+			name);
+		crypto_free_hash(op->fallback.hash);
+		op->fallback.hash = NULL;
+		return -ENOMEM;
+	}
+	memset(op->hw, 0, sizeof(*op->hw));
+
+	return 0;
+}
+
+static void fallback_exit_hash(struct crypto_tfm *tfm)
+{
+	struct stmp3xxx_dcp_hash_op *op = crypto_tfm_ctx(tfm);
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+
+	dma_free_coherent(sdcp->dev, sizeof(*op->hw), op->hw, op->hw_phys);
+	crypto_free_hash(op->fallback.hash);
+	op->fallback.hash = NULL;
+}
+
+static void dcp_sha1_init(struct crypto_tfm *tfm)
+{
+	struct stmp3xxx_dcp_hash_op *op = crypto_tfm_ctx(tfm);
+
+	op->hash.init = 1;
+	memset(op->hw->rembuf, 0, sizeof(op->hw->rembuf));
+	memset(op->hw->digest, 0, sizeof(op->hw->digest));
+}
+
+static void dcp_sha1_update(struct crypto_tfm *tfm,
+			const uint8_t *data, unsigned int length)
+{
+	struct stmp3xxx_dcp *sdcp = global_sdcp;
+	struct stmp3xxx_dcp_hash_op *op = crypto_tfm_ctx(tfm);
+
+	op->src = (void *)data;
+	op->dst = NULL;
+	op->flags = STMP3XXX_DCP_SHA1 | STMP3XXX_DCP_UPDATE;
+	if (op->hash.init) {
+		op->hash.init = 0;
+		op->flags |= STMP3XXX_DCP_INIT;
+	}
+	op->len = length;
+
+	/* map the data */
+	op->src_phys = dma_map_single(sdcp->dev, op->src, op->len,
+			DMA_TO_DEVICE);
+	if (dma_mapping_error(sdcp->dev, op->src_phys)) {
+		dev_err(sdcp->dev, "Unable to map source\n");
+		return;
+	}
+	op->dst_phys = 0;
+
+	/* perform! */
+	dcp_perform_hash_op(op);
+
+	dma_unmap_single(sdcp->dev, op->src_phys, op->len, DMA_TO_DEVICE);
+}
+
+static void dcp_sha1_final(struct crypto_tfm *tfm, uint8_t *out)
+{
+	struct stmp3xxx_dcp_hash_op *op = crypto_tfm_ctx(tfm);
+	int i;
+	const uint8_t *digest;
+
+	op->src = NULL;
+	op->dst = NULL;
+	op->flags = STMP3XXX_DCP_SHA1 | STMP3XXX_DCP_FINAL;
+	op->len = 0;
+
+	/* perform! */
+	dcp_perform_hash_op(op);
+
+	/* hardware reverses the digest (for some unexplicable reason) */
+	digest = op->hw->digest + SHA1_DIGEST_SIZE;
+	for (i = 0; i < SHA1_DIGEST_SIZE; i++)
+		*out++ = *--digest;
+	memcpy(out, op->hw->digest, SHA1_DIGEST_SIZE);
+}
+
+static struct crypto_alg dcp_sha1_alg = {
+	.cra_name		= "sha1",
+	.cra_driver_name	= "dcp-sha1",
+	.cra_priority		= 300,
+	.cra_flags		= CRYPTO_ALG_TYPE_DIGEST |
+				  CRYPTO_ALG_NEED_FALLBACK,
+	.cra_blocksize		= SHA1_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct stmp3xxx_dcp_hash_op),
+	.cra_module		= THIS_MODULE,
+	.cra_list		= LIST_HEAD_INIT(dcp_sha1_alg.cra_list),
+	.cra_init		= fallback_init_hash,
+	.cra_exit		= fallback_exit_hash,
+	.cra_u			= {
+		.digest = {
+			.dia_digestsize	= SHA1_DIGEST_SIZE,
+			.dia_init   	= dcp_sha1_init,
+			.dia_update 	= dcp_sha1_update,
+			.dia_final  	= dcp_sha1_final,
+		}
+	}
+};
+#endif
+
+static irqreturn_t dcp_common_irq(int irq, void *context)
+{
+	struct stmp3xxx_dcp *sdcp = context;
+	u32 msk;
+
+	/* check */
+	msk = HW_DCP_STAT_RD() & BF_DCP_STAT_IRQ(0x0f);
+	if (msk == 0)
+		return IRQ_NONE;
+
+	/* clear this channel */
+	HW_DCP_STAT_CLR(msk);
+	if (msk & BF_DCP_STAT_IRQ(0x01))
+		sdcp->wait[0]++;
+	if (msk & BF_DCP_STAT_IRQ(0x02))
+		sdcp->wait[1]++;
+	if (msk & BF_DCP_STAT_IRQ(0x04))
+		sdcp->wait[2]++;
+	if (msk & BF_DCP_STAT_IRQ(0x08))
+		sdcp->wait[3]++;
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t dcp_vmi_irq(int irq, void *context)
+{
+	return dcp_common_irq(irq, context);
+}
+
+static irqreturn_t dcp_irq(int irq, void *context)
+{
+	return dcp_common_irq(irq, context);
+}
+
+static int stmp3xxx_dcp_probe(struct platform_device *pdev)
+{
+	struct stmp3xxx_dcp *sdcp = NULL;
+	struct resource *r;
+	int i, ret;
+
+	if (global_sdcp != NULL) {
+		dev_err(&pdev->dev, "Only one instance allowed\n");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	/* allocate memory */
+	sdcp = kzalloc(sizeof(*sdcp), GFP_KERNEL);
+	if (sdcp == NULL) {
+		dev_err(&pdev->dev, "Failed to allocate structure\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	sdcp->dev = &pdev->dev;
+	spin_lock_init(&sdcp->lock);
+
+	for (i = 0; i < STMP3XXX_DCP_NUM_CHANNELS; i++) {
+		mutex_init(&sdcp->op_mutex[i]);
+		init_completion(&sdcp->op_wait[i]);
+	}
+
+	platform_set_drvdata(pdev, sdcp);
+
+	/* Soft reset and remove the clock gate */
+	HW_DCP_CTRL_SET(BM_DCP_CTRL_SFTRST);
+
+	/* At 24Mhz, it takes no more than 4 clocks (160 ns) Maximum for
+	 * the part to reset, reading the register twice should
+	 * be sufficient to get 4 clks delay.
+	 */
+	HW_DCP_CTRL_RD();
+	HW_DCP_CTRL_RD();
+
+	HW_DCP_CTRL_CLR(BM_DCP_CTRL_SFTRST | BM_DCP_CTRL_CLKGATE);
+
+	/* Initialize control registers */
+	HW_DCP_CTRL_WR(STMP3XXX_DCP_CTRL_INIT);
+	HW_DCP_CHANNELCTRL_WR(STMP3XXX_DCP_CHANNELCTRL_INIT);
+
+	/* We do not enable context switching. Give the context
+	 * buffer pointer an illegal address so if context switching is
+	 * inadvertantly enabled, the dcp will return an error instead of
+	 * trashing good memory. The dcp dma cannot access rom, so any rom
+	 * address will do.
+	 */
+	HW_DCP_CONTEXT_WR(0xFFFF0000);
+
+	for (i = 0; i < STMP3XXX_DCP_NUM_CHANNELS; i++)
+		HW_DCP_CHnSTAT_CLR(i, -1);
+	HW_DCP_STAT_CLR(-1);
+
+	r = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "can't get IRQ resource (0)\n");
+		ret = -EIO;
+		goto err_unregister_sha1;
+	}
+	sdcp->dcp_vmi_irq = r->start;
+	ret = request_irq(sdcp->dcp_vmi_irq, dcp_vmi_irq, 0, "stmp3xxx-dcp",
+				sdcp);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "can't request_irq (0)\n");
+		goto err_unregister_sha1;
+	}
+
+	r = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+	if (!r) {
+		dev_err(&pdev->dev, "can't get IRQ resource (1)\n");
+		ret = -EIO;
+		goto err_free_irq0;
+	}
+	sdcp->dcp_irq = r->start;
+	ret = request_irq(sdcp->dcp_irq, dcp_irq, 0, "stmp3xxx-dcp", sdcp);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "can't request_irq (1)\n");
+		goto err_free_irq0;
+	}
+
+	global_sdcp = sdcp;
+
+	ret = crypto_register_alg(&dcp_aes_alg);
+	if (ret != 0)  {
+		dev_err(&pdev->dev, "Failed to register aes crypto\n");
+		goto err_kfree;
+	}
+
+	ret = crypto_register_alg(&dcp_aes_ecb_alg);
+	if (ret != 0)  {
+		dev_err(&pdev->dev, "Failed to register aes ecb crypto\n");
+		goto err_unregister_aes;
+	}
+
+	ret = crypto_register_alg(&dcp_aes_cbc_alg);
+	if (ret != 0)  {
+		dev_err(&pdev->dev, "Failed to register aes cbc crypto\n");
+		goto err_unregister_aes_ecb;
+	}
+
+#ifndef DISABLE_SHA1
+	ret = crypto_register_alg(&dcp_sha1_alg);
+	if (ret != 0)  {
+		dev_err(&pdev->dev, "Failed to register aes cbc crypto\n");
+		goto err_unregister_aes_cbc;
+	}
+#endif
+
+	dev_notice(&pdev->dev, "DCP crypto enabled.!\n");
+	return 0;
+
+err_free_irq0:
+	free_irq(sdcp->dcp_vmi_irq, sdcp);
+err_unregister_sha1:
+#ifndef DISABLE_SHA1
+	crypto_unregister_alg(&dcp_sha1_alg);
+#endif
+err_unregister_aes_cbc:
+	crypto_unregister_alg(&dcp_aes_cbc_alg);
+err_unregister_aes_ecb:
+	crypto_unregister_alg(&dcp_aes_ecb_alg);
+err_unregister_aes:
+	crypto_unregister_alg(&dcp_aes_alg);
+err_kfree:
+	kfree(sdcp);
+err:
+
+	return ret;
+}
+
+static int stmp3xxx_dcp_remove(struct platform_device *pdev)
+{
+	struct stmp3xxx_dcp *sdcp;
+
+	sdcp = platform_get_drvdata(pdev);
+	platform_set_drvdata(pdev, NULL);
+
+	free_irq(sdcp->dcp_irq, sdcp);
+	free_irq(sdcp->dcp_vmi_irq, sdcp);
+#ifndef DISABLE_SHA1
+	crypto_unregister_alg(&dcp_sha1_alg);
+#endif
+	crypto_unregister_alg(&dcp_aes_cbc_alg);
+	crypto_unregister_alg(&dcp_aes_ecb_alg);
+	crypto_unregister_alg(&dcp_aes_alg);
+	kfree(sdcp);
+	global_sdcp = NULL;
+
+	return 0;
+}
+
+
+#ifdef CONFIG_PM
+static int stmp3xxx_dcp_suspend(struct platform_device *pdev,
+		pm_message_t state)
+{
+	return 0;
+}
+
+static int stmp3xxx_dcp_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+#else
+#define stmp3xxx_dcp_suspend	NULL
+#define	stmp3xxx_dcp_resume	NULL
+#endif
+
+static struct platform_driver stmp3xxx_dcp_driver = {
+	.probe		= stmp3xxx_dcp_probe,
+	.remove		= stmp3xxx_dcp_remove,
+	.suspend	= stmp3xxx_dcp_suspend,
+	.resume		= stmp3xxx_dcp_resume,
+	.driver		= {
+		.name   = "stmp3xxx-dcp",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init
+stmp3xxx_dcp_init(void)
+{
+	return platform_driver_register(&stmp3xxx_dcp_driver);
+}
+
+static void __exit
+stmp3xxx_dcp_exit(void)
+{
+	platform_driver_unregister(&stmp3xxx_dcp_driver);
+}
+
+MODULE_AUTHOR("Pantelis Antoniou <pantelis@embeddedalley.com>");
+MODULE_DESCRIPTION("STMP3XXX DCP Crypto Driver");
+MODULE_LICENSE("GPL");
+
+module_init(stmp3xxx_dcp_init);
+module_exit(stmp3xxx_dcp_exit);
diff --git a/drivers/crypto/stmp3xxx_dcp.h b/drivers/crypto/stmp3xxx_dcp.h
new file mode 100644
index 000000000000..32ae0c704daf
--- /dev/null
+++ b/drivers/crypto/stmp3xxx_dcp.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef _STMP3XXX_DCP_H_
+#define _STMP3XXX_DCP_H_
+
+#include <mach/stmp3xxx_regs.h>
+#include <mach/stmp3xxx.h>
+#include <mach/regs-dcp.h>
+
+#define CIPHER_CHAN	1
+#define CIPHER_MASK	(1 << CIPHER_CHAN)
+
+#define HASH_CHAN	0
+#define HASH_MASK	(1 << HASH_CHAN)
+
+#define ALL_MASK	(CIPHER_MASK | HASH_MASK)
+
+/* Defines the initialization value for the dcp control register */
+#define STMP3XXX_DCP_CTRL_INIT \
+   (BM_DCP_CTRL_GATHER_RESIDUAL_WRITES | \
+    BM_DCP_CTRL_ENABLE_CONTEXT_CACHING | \
+    BV_DCP_CTRL_CHANNEL_INTERRUPT_ENABLE__CH0 | \
+    BV_DCP_CTRL_CHANNEL_INTERRUPT_ENABLE__CH1 | \
+    BV_DCP_CTRL_CHANNEL_INTERRUPT_ENABLE__CH2 | \
+    BV_DCP_CTRL_CHANNEL_INTERRUPT_ENABLE__CH3)
+
+/* Defines the initialization value for the dcp channel control register */
+#define STMP3XXX_DCP_CHANNELCTRL_INIT \
+    BF_DCP_CHANNELCTRL_ENABLE_CHANNEL(ALL_MASK)
+
+/* DCP work packet 1 value for encryption */
+#define STMP3XXX_DCP_PKT1_ENCRYPT \
+   (BM_DCP_PACKET1_DECR_SEMAPHORE | \
+    BM_DCP_PACKET1_ENABLE_CIPHER | \
+    BM_DCP_PACKET1_CIPHER_ENCRYPT | \
+    BM_DCP_PACKET1_CIPHER_INIT)
+
+/* DCP work packet 1 value for decryption */
+#define DCP_PKT1_DECRYPT \
+   (BM_DCP_PACKET1_DECR_SEMAPHORE | \
+    BM_DCP_PACKET1_ENABLE_CIPHER | \
+    BM_DCP_PACKET1_CIPHER_INIT)
+
+/* DCP (decryption) work packet definition */
+struct stmp3xxx_dcp_hw_packet {
+	uint32_t pNext;     /* next dcp work packet address */
+	uint32_t pkt1;      /* dcp work packet 1 (control 0) */
+	uint32_t pkt2;      /* dcp work packet 2 (control 1) */
+	uint32_t pSrc;      /* source buffer address */
+	uint32_t pDst;      /* destination buffer address */
+	uint32_t size;      /* buffer size in bytes */
+	uint32_t pPayload;  /* payload buffer address */
+	uint32_t stat;      /* dcp status (written by dcp) */
+};
+
+#define STMP3XXX_DCP_NUM_CHANNELS 4
+
+#endif
diff --git a/drivers/fims/Kconfig b/drivers/fims/Kconfig
new file mode 100644
index 000000000000..f2462f7096cd
--- /dev/null
+++ b/drivers/fims/Kconfig
@@ -0,0 +1,251 @@
+#
+# FIM devices configuration
+#
+
+menuconfig FIM_CORE
+	tristate "FIM drivers support"
+	depends on PROCESSOR_NS921X
+        select FW_LOADER
+        select HOTPLUG 
+	default n
+	help
+	  This option adds the support for the Flexible Interfaces Modules (FIM).
+	  Currently only the NS921X processors are supporting the FIM-devices. 
+          The number of available FIMs is two for NS9215 and one for NS9210.
+
+	  For installing the specific FIM-firmware, the FIM core driver depends on
+	  Hotplug and the firmware-subsystem too. 
+
+##           If a FIM-driver is selected as loadable module, then is required
+##           to have installed the user-space application 'mdev' (Busybox).
+
+
+if FIM_CORE
+
+config FIM_ZERO
+	bool "FIM 0"
+	depends on PROCESSOR_NS9210 || PROCESSOR_NS9215 && FIM_CORE=y
+	help
+	  Enables FIM 0
+
+choice
+	prompt "Choose FIM 0 driver"
+	depends on FIM_ZERO
+	help
+	  Only one of the following drivers can run in the FIM. Please pick one.
+
+config FIM_ZERO_SDIO_SELECTED
+	bool "SDIO driver"
+	depends on MMC && MMC_BLOCK && !(CME9210JS_SERIAL_PORTA_FULL || CME9210JS_SERIAL_PORTA_CTSRTSRXTX)
+	select FIM_ZERO_SDIO
+	help
+	  Enables the FIM-SDIO driver.
+	  
+          For testing the SDIO functionality is required to choose an additional 
+          SDIO-driver (GPS/UART) from the MMC-core.
+
+config FIM_ZERO_SERIAL_RXTX
+	bool "Serial driver 2-wires (RX/TX)"
+	depends on !(CME9210JS_SERIAL_PORTA_FULL || CME9210JS_SERIAL_PORTA_CTSRTSRXTX)
+	select FIM_ZERO_SERIAL_SELECTED
+	help
+	  Enables the FIM-serial driver without the HW flow control (CTS and RTS
+	  lines are not requested by the driver).
+
+config FIM_ZERO_SERIAL_CTSRTS
+	bool "Serial driver 4-wires (RX/TX/CTS/RTS)"
+	depends on !(CME9210JS_SERIAL_PORTA || CME9210JS_SERIAL_PORTA_FULL || CME9210JS_SERIAL_PORTA_CTSRTSRXTX)
+	select FIM_ZERO_SERIAL_SELECTED
+	help
+	  Enables the FIM-serial driver with HW flow control support. 
+
+config FIM_ZERO_CAN_SELECTED
+	bool "CAN-bus driver"
+	depends on CAN && CAN_DEV && (!MACH_CME9210 && !MACH_CME9210JS)
+	select FIM_ZERO_CAN
+	help
+	  Enables the FIM-CAN bus driver.
+
+config FIM_ZERO_W1_SELECTED
+	bool "1-wire driver"
+	depends on !(CME9210JS_SERIAL_PORTA_FULL || CME9210JS_SERIAL_PORTA_CTSRTSRXTX)
+	select FIM_ZERO_W1
+	select W1
+	select W1_MASTER_GPIO
+	help
+	  Enables the 1-wire GPIO driver on the FIM-board. For the selection of the
+	  connected slaves you need to enter the config menu of the 1-wire subsystem.
+
+config FIM_ZERO_USB_SELECTED
+        tristate "USB serial port driver"
+        select FIM_ZERO_USB
+	select SERIAL_CORE
+        help
+          Enables the USB serial port driver (low speed).
+
+endchoice # FIM_ZERO
+
+config FIM_ONE
+	bool "FIM 1"
+	depends on PROCESSOR_NS9210 || PROCESSOR_NS9215 && FIM_CORE=y
+	help
+	  Enables FIM 1
+
+choice
+	prompt "Choose FIM 1 driver"
+	depends on FIM_ONE
+	help
+	  Only one of the following drivers can run in the FIM. Please pick one.
+
+config FIM_ONE_SDIO_SELECTED
+	bool "SDIO driver"
+	depends on MMC && MMC_BLOCK && (!MACH_CME9210 && !MACH_CME9210JS) && !FIM_ZERO_SDIO_SELECTED
+	select FIM_ONE_SDIO
+	help
+	  Enables the FIM-SDIO serial driver. 
+	  
+          For testing the SDIO functionality is required to choose an additional 
+          SDIO-driver (GPS/UART) from the MMC-core.
+
+config FIM_ONE_SERIAL_RXTX
+	bool "Serial driver 2-wires (RX/TX)"
+	depends on !MACH_CME9210 && !MACH_CME9210JS
+	select FIM_ONE_SERIAL_SELECTED
+	help
+	  Enables the FIM-serial driver without the HW flow control (CTS and RTS
+          lines are not requested by the driver).
+
+config FIM_ONE_SERIAL_CTSRTS
+	bool "Serial driver 4-wires (RX/TX/CTS/RTS)"
+	depends on !MACH_CME9210 && !MACH_CME9210JS
+	select FIM_ONE_SERIAL_SELECTED
+	help
+	  Enables the FIM-serial driver with HW flow control support. 
+	  
+config FIM_ONE_CAN_SELECTED
+	bool "CAN-bus driver"
+	depends on CAN && CAN_DEV && !((MACH_CME9210 || MACH_CME9210JS) && (CME9210JS_SERIAL_PORTA_FULL || FIM_ZERO_SERIAL_SELECTED || FIM_ZERO_SDIO_SELECTED))
+	select FIM_ONE_CAN
+	help
+	  Enables the FIM-CAN bus driver.
+
+config FIM_ONE_W1_SELECTED
+	bool "1-wire driver"
+	depends on !MACH_CME9210 && !MACH_CME9210JS && !FIM_ZERO_W1_SELECTED
+	select FIM_ONE_W1
+	select W1_MASTER_GPIO
+	help
+	  Enables the 1-wire GPIO driver on the FIM-board. For the selection of the
+	  connected slaves you need to enter the config menu of the 1-wire subsystem.
+
+config FIM_ONE_USB_SELECTED
+        tristate "USB serial port driver"
+        depends on !MACH_CME9210JS
+        select FIM_ONE_USB
+	select SERIAL_CORE
+        help
+          Enables the USB serial port driver (low speed).
+
+endchoice # FIM_ONE
+
+# -- Special configuration options for some FIM-drivers
+config SERIAL_FIM_CONSOLE
+        bool "Enable the FIM Serial Console"
+        depends on FIM_ZERO_SERIAL_SELECTED || FIM_ONE_SERIAL_SELECTED
+	select SERIAL_CORE_CONSOLE
+        help
+          Enables the console functionality on the FIM serial ports.
+
+config FIM_MODULES
+	bool "Enable FIM drivers as loadable modules"
+	depends on !FIM_ZERO || !FIM_ONE
+	help
+	  This option allows to build the FIM drivers as loadable modules
+	  rather than as built-in features.
+
+if FIM_MODULES
+
+# -- These are the options for enabling the FIM-drivers as loadable modules
+config FIM_SDIO_MODULE
+	bool "SDIO as loadable module"
+	depends on MMC && MMC_BLOCK && !FIM_ZERO_SDIO_SELECTED && !FIM_ONE_SDIO_SELECTED && (!FIM_ZERO || !FIM_ONE)
+	select FIM_ZERO_SDIO
+	select FIM_ONE_SDIO if !(MACH_CME9210JS || MACH_CME9210)
+	help
+          Enables the FIM SDIO as loadable module. The number of FIMs to be
+	  handled by this driver can be passed with the module parameter 'fims'
+
+config FIM_SERIAL_MODULE
+	bool "Serial as loadable module"
+	depends on !FIM_ZERO_SERIAL_SELECTED && !FIM_ONE_SERIAL_SELECTED && (!FIM_ZERO || !FIM_ONE)
+	select FIM_ZERO_SERIAL
+	select FIM_ONE_SERIAL if !(MACH_CME9210JS || MACH_CME9210)
+	select SERIAL_CORE
+	help
+          Enables the FIM Serial loadable module. The number of FIMs to be
+	  handled by this driver can be passed with the module parameter 'fims'
+
+config FIM_CAN_MODULE
+	bool "CAN as loadable module"
+	depends on CAN && CAN_DEV && !FIM_ZERO_CAN_SELECTED && !FIM_ONE_CAN_SELECTED && (!FIM_ZERO || !FIM_ONE)
+	select FIM_ZERO_CAN if !(MACH_CME9210JS || MACH_CME9210)
+	select FIM_ONE_CAN
+	help
+          Enables the FIM CAN as loadable module. The number of FIMs to be
+	  handled by this driver can be passed with the module parameter 'fims'
+
+config FIM_USB_MODULE
+        bool "USB serial port driver as loadable module"
+        depends on SERIAL_CORE && !FIM_ZERO_USB_SELECTED && !FIM_ONE_USB_SELECTED
+        select FIM_ZERO_USB
+        select FIM_ONE_USB if !(MACH_CME9210JS || MACH_CME9210)
+        help
+          Enables the FIM USB Device Controller as loadable module.
+          The number of FIMs to be handled by this driver can be passed with the
+          module parameter 'fims'
+
+endif # FIM_MODULES
+
+# -- Variables for selection the platform devices
+config FIM_ZERO_SDIO
+	bool
+
+config FIM_ONE_SDIO
+	bool
+
+config FIM_ZERO_SERIAL
+	bool
+
+config FIM_ZERO_SERIAL_SELECTED
+        select FIM_ZERO_SERIAL
+	select SERIAL_CORE
+	bool
+	
+config FIM_ONE_SERIAL
+	bool
+
+config FIM_ONE_SERIAL_SELECTED
+        select FIM_ONE_SERIAL
+	select SERIAL_CORE
+	bool
+
+config FIM_ZERO_CAN
+	bool
+
+config FIM_ONE_CAN
+	bool
+
+config FIM_ZERO_W1
+        bool
+
+config FIM_ONE_W1
+        bool
+
+config FIM_ZERO_USB
+        bool
+
+config FIM_ONE_USB
+        bool
+
+endif # FIM_CORE
+
diff --git a/drivers/fims/Makefile b/drivers/fims/Makefile
new file mode 100644
index 000000000000..a6db651aa225
--- /dev/null
+++ b/drivers/fims/Makefile
@@ -0,0 +1,13 @@
+#
+# Makefile for the kernel FIM device drivers
+#
+
+obj-$(CONFIG_FIM_CORE)		+= fim.o
+obj-$(CONFIG_FIM_ZERO_SERIAL)	+= serial/
+obj-$(CONFIG_FIM_ZERO_SDIO)	+= sdio/
+obj-$(CONFIG_FIM_ZERO_CAN)	+= can/
+obj-$(CONFIG_FIM_ZERO_USB)	+= usb/
+obj-$(CONFIG_FIM_ONE_SERIAL)	+= serial/
+obj-$(CONFIG_FIM_ONE_SDIO)	+= sdio/
+obj-$(CONFIG_FIM_ONE_CAN)	+= can/
+obj-$(CONFIG_FIM_ONE_USB)	+= usb/
\ No newline at end of file
diff --git a/drivers/fims/can/Makefile b/drivers/fims/can/Makefile
new file mode 100644
index 000000000000..b48226c93b9f
--- /dev/null
+++ b/drivers/fims/can/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for the kernel FIM device drivers
+#
+
+ifneq ($(CONFIG_FIM_CAN_MODULE),)
+	obj-m				+= fim_can.o
+else
+	obj-$(CONFIG_FIM_ZERO_CAN)	+= fim_can.o
+	obj-$(CONFIG_FIM_ONE_CAN)	+= fim_can.o
+endif
diff --git a/drivers/fims/can/README b/drivers/fims/can/README
new file mode 100644
index 000000000000..265258be3183
--- /dev/null
+++ b/drivers/fims/can/README
@@ -0,0 +1,3 @@
+You must license the CAN firmware driver from Digi before you can use it.
+Please register at http://www.digi.com/canbus to receive the required 
+CAN bus support files.
diff --git a/drivers/fims/can/fim_can.c b/drivers/fims/can/fim_can.c
new file mode 100644
index 000000000000..f71eae26357b
--- /dev/null
+++ b/drivers/fims/can/fim_can.c
@@ -0,0 +1,1483 @@
+/* -*- linux-c -*-
+ *
+ * drivers/fims/fim_can.c
+ *
+ * Copyright (C) 2008-2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ *  !Revision:   $Revision: 1.2 $
+ *  !Author:     Luis Galdos, Hector Oron
+ *  !Descr:      
+ *  !References: Based on the virtual CAN driver (Copyright (c) 2002-2007
+ *               Volkswagen Group Electronic Research)
+ *               All rights reserved.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/fcntl.h>
+#include <linux/interrupt.h>
+#include <linux/ptrace.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/skbuff.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+
+/* Header files for the CAN-stack */
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/can/error.h>
+
+/* For registering the FIM-driver */
+#include <mach/fim-ns921x.h>
+#include <mach/gpio.h>
+
+/* If the driver is being to be loaded as a built-in driver, then include the header */
+#if !defined(MODULE)
+#include "fim_can.h"
+extern const unsigned char fim_can_firmware[];
+#define FIM_CAN_FIRMWARE_FILE			(NULL)
+#define FIM_CAN_FIRMWARE_CODE			fim_can_firmware
+#else
+const unsigned char *fim_can_firmware = NULL;
+#define FIM_CAN_FIRMWARE_FILE			"fim_can.bin"
+#define FIM_CAN_FIRMWARE_CODE			(NULL)
+#endif
+
+/* Driver informations */
+#define DRIVER_VERSION				"1.2"
+#define DRIVER_AUTHOR				"Luis Galdos, Hector Oron"
+#define DRIVER_DESC				"FIM CAN bus driver"
+#define FIM_DRIVER_NAME				"fim-can"
+#define FIM_DRIVER_FIRMWARE_NAME		"fim_can.bin"
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRIVER_VERSION);
+
+/* Module parameter for selection the FIMs */
+NS921X_FIM_NUMBERS_PARAM(fims_number);
+
+#define printk_err(fmt, args...)		printk(KERN_ERR "[ ERROR ] fim-can: " fmt, ## args)
+#define printk_info(fmt, args...)		printk(KERN_INFO "fim-can: " fmt, ## args)
+#define printk_dbg(fmt, args...)		printk(KERN_DEBUG "fim-can: " fmt, ## args)
+
+#if 0
+#define FIM_CAN_DEBUG
+#endif
+
+#ifdef FIM_CAN_DEBUG
+#  define printk_debug(fmt, args...)		printk(KERN_DEBUG "fim-can: " fmt, ## args)
+#else
+#  define printk_debug(fmt, args...)		do { } while (0)
+#endif
+
+#define FIM_PRIVATE_BUFFER			((void *)0xffffffff)
+
+/* Available commands for the PIC */
+#define FIM_CAN_CMD_TX				0x00
+#define FIM_CAN_CMD_SHUTDOWN			0x40
+#define FIM_CAN_CMD_CONFIG			0x41
+#define FIM_CAN_CMD_FILTER(i)			(0x80 + i)
+#define FIM_CAN_ERR_FILTER(i)			(i > 0x87 || i < 0x80)
+#define FIM_CAN_NR_FILTERS			(8)
+
+/* Default timing values (see Net+OS driver: nacbus.c) */
+#define FIM_CAN_DEFAULT_SJW			(7)
+#define FIM_CAN_DEFAULT_PROPSEG			(8)
+#define FIM_CAN_DEFAULT_PHASE1			(8)
+#define FIM_CAN_DEFAULT_PHASE2			(8)
+#define FIM_CAN_DEFAULT_BITRATE			CAN_BITRATE_DEFAULT
+
+/* Maximal values from the Net+OS driver */
+#define FIM_CAN_MAX_BITRATE	                (1000000)
+
+#define CAN_BUS_RECESSIVE			(0xffffffff)
+#define CAN_BUS_DOMINANT			(0x00000000)
+
+/* Status register according to the FIM-firmware specification (page 9) */
+#define FIM_CAN_BUSSTATE_REG			0
+#define FIM_CAN_TXERRCNT_REG			1
+#define FIM_CAN_RXERRCNT_REG			2
+#define FIM_CAN_FATALERR_REG			3
+
+/* Control bits of the bus state register */
+#define FIM_CAN_BUSSTATE_ERRPAS			0x01
+#define FIM_CAN_BUSSTATE_OFF			0x02
+
+/* Control bits of the fatal error register */
+#define FIM_CAN_FATAL_CONTROLLER		0x01
+#define FIM_CAN_FATAL_CMDERR			0x40
+#define FIM_CAN_FATAL_HW			0x41
+#define FIM_CAN_FATAL_FILTER			0x42
+#define FIM_CAN_FATAL_CONFIG			0x43
+
+/* Interrupts from the FIM to the ARM (see can_defs.inc) */
+#define FIM_CAN_INT_RESET			(0x01)
+#define FIM_CAN_INT_CMD_ERR			(0x45)
+#define FIM_CAN_INT_HW_FAILURE			(0x41)
+#define FIM_CAN_INT_FILTER_ERR			(0x42)
+#define FIM_CAN_INT_CFG_ERR			(0x43)
+
+/*
+ * Macros for the configuration of the DMA-channel
+ */
+#define FIM_CAN_DMA_BUFFER_SIZE			(256)
+#define FIM_CAN_DMA_BUFFERS			(60)
+
+/*
+ * @XXX: Remove this ugly macro and use a structure for the PIC-message according
+ * to the specification, page 8 (Transmit message)
+ */
+#define FIRST_DATA_BYTE_IN_MESSAGE		(3)
+
+/*
+ * GPIO configuration 
+ */
+#define FIM_CAN_MAX_GPIOS			(2)
+
+/* Structure for setting the ID-filters */
+#define FIM_CAN_FILTER_SIZE			(8)
+struct fim_can_filter_t {
+	u8 code;
+	u8 filter[FIM_CAN_FILTER_SIZE];
+};
+
+/*
+ * Initializing some needed variables. This is important step
+ * as CAN driver might not work upon this values
+ */
+static struct can_bittiming_const fim_bittiming_const = {
+	.tseg1_min = 1,
+	.tseg1_max = 200,
+	.tseg2_min = 1,
+	.tseg2_max = 200,
+	.sjw_max = 40,
+	.brp_min = 1,
+	.brp_max = 2048,
+	.brp_inc = 1,	
+};
+
+/*
+ * This structure provides the interface for configuring the timing parameters of
+ * the CAN-controller.
+ */
+struct fim_can_timing_t {
+	u8 code;
+	u16 sjw;
+	u16 cspl;
+	u16 cp2pl;
+	u16 csi;
+	u16 cp2i;
+	u16 csp;
+} __attribute__ ((packed));
+
+/*
+ * Structure for sending the shutdown command to the FIM
+ * This command is required before stopping the FIM, otherwise a transmitted
+ * frame can be corrupted
+ */
+struct fim_can_shutdown_t {
+	u8 code;
+};
+
+/*
+ * Structure for sending user-data to the controller
+ * @XXX: The maximal frame length is just an arbitrary value (can be modified)
+ */
+#define FIM_CAN_FRAME_LENGTH			128
+struct fim_can_txframe {
+	u16 crc;
+	u32 bytepos;
+	u32 bitcnt;
+	u32 bitstuff;
+	u32 last5;
+	u32 stuffed;
+	u32 len;
+	u32 result;
+	u8 buffer[FIM_CAN_FRAME_LENGTH];
+};
+
+/* @FIXME: Use the generic structure for the CAN-frames from <include/linux/can.h> */
+struct fim_can_rxframe {
+	u8 id[4];
+	u8 ctrl;
+	u8 data[8];
+} __attribute__ ((packed));
+
+struct fim_can_t {
+	struct can_priv can;
+	int index;
+	struct fim_driver fim;
+	struct semaphore sem;
+	struct net_device *dev;
+	int reg;
+ 	struct fim_gpio_t gpios[FIM_CAN_MAX_GPIOS];
+	struct clk *cpu_clk;
+	int opened;
+	spinlock_t lock;
+};
+
+/* Function prototypes */
+static int fim_can_send_skb(struct fim_can_t *port, unsigned char *data, int len,
+			    struct sk_buff *skb);
+static int fim_can_send_buffer(struct fim_can_t *port, unsigned char *data, int len);
+static int fim_can_restart_fim(struct fim_can_t *port);
+static int fim_can_start_fim(struct fim_can_t *port);
+static int fim_can_stop_fim(struct fim_can_t *port);
+
+/* Debug function for dumping the CAN-frames for the FIM-firmware */
+#if defined(FIM_CAN_DEBUG)
+#define fim_can_dump_frame(f, m)		fim_can_do_dump_frame(f, m)
+#else
+#define fim_can_dump_frame(f, m)		do { } while (0)
+#endif
+inline static void fim_can_do_dump_frame(struct fim_can_txframe *frame, char *marke)
+{
+	int cnt;
+	char buffer[512];
+	unsigned int pos;
+	int len;
+	unsigned char *data;
+
+	len = 3 + (frame->stuffed + 7) / 8;
+	data = (unsigned char *)(frame->buffer);
+	pos = 0;
+	for (cnt = 0; cnt < len; cnt++)
+		pos += snprintf(buffer + pos, sizeof(buffer), "0x%02x ", *(data + cnt));
+
+	printk_debug("%s buffer : %s\n", marke, buffer);
+	printk_debug("%s infos  :\n\
+\tCRC           : 0x%x\n\
+\tbytePos       : 0x%x\n\
+\tbitCount      : 0x%x\n\
+\tbitStuff      : 0x%x\n\
+\tlast 5        : 0x%x\n\
+\tstuffed count : 0x%x\n",
+		     marke,
+		     frame->crc,
+		     frame->bytepos,
+		     frame->bitcnt,
+		     frame->bitstuff,
+		     frame->last5,
+		     frame->stuffed);
+}
+
+/* Debug function for dumping the CAN-timing parameters */
+#if defined(FIM_CAN_DEBUG)
+#define fim_can_dump_timing(cfg, s, p, y, b)	fim_can_do_dump_timing(cfg, s, p, y, b)
+#else
+#define fim_can_dump_timing(cfg, s, p, y, b)	do { } while (0)
+#endif
+
+/* Print the timing values for the controller */
+inline static void fim_can_do_dump_timing(struct fim_can_timing_t *cfg,
+					  unsigned int sjw, unsigned int sample_point,
+					  unsigned int sync_period, unsigned int bitrate)
+{
+	printk_debug("SJW %u | Sample point %u | Sync period %u | Bitrate %u\n\
+\tSJW   : 0x%04x\n\
+\tCSPL  : 0x%04x\n\
+\tCP2PL : 0x%04x\n\
+\tCSI   : 0x%04x\n\
+\tCP2I  : 0x%04x\n\
+\tCSP   : 0x%04x\n\
+",
+		     sjw,
+		     sample_point,
+		     sync_period,
+		     bitrate,
+		     cfg->sjw,
+		     cfg->cspl,
+		     cfg->cp2pl,
+		     cfg->csi,
+		     cfg->cp2i,
+		     cfg->csp);
+}
+
+inline static void fim_can_parse_filter(struct fim_can_filter_t *flt, canid_t id,
+					canid_t mask)
+{
+	flt->filter[0] = (id >> 24);
+	flt->filter[1] = (mask >> 24);
+	flt->filter[2] = (id >> 16);
+	flt->filter[3] = (mask >> 16);
+	flt->filter[4] = (id >> 8);
+	flt->filter[5] = (mask >> 8);
+	flt->filter[6] = id;
+	flt->filter[7] = mask;
+}
+
+/* Calculate the latency times for the passed bitrate */
+static int fim_can_calculate_latency(u32 bitrate, u16 *rx, u16 *tx)
+{
+	int ret;
+
+	if (bitrate <= 0 || bitrate > FIM_CAN_MAX_BITRATE)
+		return -ERANGE;
+	
+	/*
+	 * In the future we will probably need different latency times for the
+	 * bitrates. Let us keep this function for this purpose.
+	 */
+#define FIM_CAN_RXIRQ_LATENCY		(1)
+#define FIM_CAN_TXIRQ_LATENCY		(100)
+	*rx = FIM_CAN_RXIRQ_LATENCY;
+	*tx = FIM_CAN_TXIRQ_LATENCY;
+	ret = 0;
+	
+	return ret;
+}
+
+static int fim_can_shutdown(struct fim_can_t *port)
+{
+	struct fim_buffer_t buf;
+	struct fim_can_shutdown_t cmd;
+
+	cmd.code = FIM_CAN_CMD_SHUTDOWN;
+
+	buf.private = FIM_PRIVATE_BUFFER;
+	buf.length = sizeof(struct fim_can_shutdown_t);
+	buf.data = (unsigned char *)&cmd;
+
+	return 0;
+}
+
+/* Configures all timing values */
+static void fim_can_fill_timing(struct fim_can_t *port, struct fim_can_timing_t *tim,
+				u16 sjw, u16 sample_point, u16 sync_period)
+{
+	unsigned int ten_per;
+	u16 larx, latx;
+	struct can_bittiming *bt;
+
+	bt = &port->can.bittiming;
+	printk_debug("New timing: %u bps | sjw %u | sample %u | sync %u | clock %u\n",
+		     bt->bitrate,
+		     sjw,
+		     sample_point,
+		     sync_period,
+		     bt->clock);
+
+	if (fim_can_calculate_latency(bt->bitrate, &larx, &latx)) {
+		printk_err("Failed latency calculation for rate %u\n", bt->bitrate);
+		return;
+	}
+
+	/* Calculate how many clocks are in the ten percent of the bit rate */
+	ten_per = (clk_get_rate(port->cpu_clk) + (5 * bt->bitrate)) / (10 * bt->bitrate);
+	printk_debug("Ten percent by %i Bps is aprox. %i\n", bt->bitrate, ten_per);
+
+	tim->sjw = (sjw * ten_per + 5) / 10;
+	tim->cspl = (sample_point * ten_per + 5) / 10;
+	tim->cp2pl = (((100 - sample_point) * ten_per) + 5) / 10;
+	tim->csi = larx;
+	tim->cp2i = latx;
+	tim->csp = ((sync_period * ten_per) + 5) / 10;
+
+	printk_debug("sjw: %d \tcspl: %d \tcp2pl: %d \tcsi: %d \tcp2i: %d \tcsp: %d\n",
+		     tim->sjw, tim->cspl, tim->cp2pl,
+		     tim->csi, tim->cp2i, tim->csp);
+}
+
+/* Configure the filter for accepting all the incoming messages */
+inline static void fim_can_filter_accept(struct fim_can_filter_t *filter)
+{
+	memset(filter->filter, 0x00, FIM_CAN_FILTER_SIZE);
+}
+
+/* Configure the filter for rejecting all the incoming messages */
+inline static void fim_can_filter_reject(struct fim_can_filter_t *filter)
+{
+	memset(filter->filter, 0x20, FIM_CAN_FILTER_SIZE);
+}
+
+/*
+ * This function inits the CAN-controller according to its specification.
+ * Unfortunately the firmware expects the configuration data and the filters in
+ * only ONE message. For this reason we need a separated function only for the
+ * init-configuration
+ */
+static int fim_can_set_init_config(struct fim_can_t *port)
+{
+	struct fim_can_timing_t cfg;
+	struct fim_can_filter_t filters[FIM_CAN_NR_FILTERS];
+	int cnt, retval;
+	unsigned char *ptr;
+	unsigned char data[sizeof(struct fim_can_timing_t) + (FIM_CAN_NR_FILTERS * 8)];
+	struct can_bittiming *bt;
+
+	/* Init the timing parameters */
+	cfg.code = FIM_CAN_CMD_CONFIG;
+	bt = &port->can.bittiming;
+
+	if (bt->bitrate == 0) {
+		printk("Sorry, bitrate required for\n");
+		return -EINVAL;
+	}
+
+	/* We must use the default bit rates for our FIM-firmware */
+	if (!bt->sjw)
+		bt->sjw = FIM_CAN_DEFAULT_SJW;
+	if (!bt->prop_seg)
+		bt->prop_seg = FIM_CAN_DEFAULT_PROPSEG;
+
+	if (!bt->phase_seg1)
+		bt->phase_seg1 = FIM_CAN_DEFAULT_PHASE1;
+	
+	if (!bt->phase_seg2)
+		bt->phase_seg2 = FIM_CAN_DEFAULT_PHASE2;
+			
+	fim_can_fill_timing(port, &cfg,
+			    bt->sjw,
+			    bt->prop_seg + bt->phase_seg1 + 1,
+			    bt->phase_seg2);
+
+	fim_can_do_dump_timing(&cfg,
+			       bt->sjw,
+			       bt->prop_seg + bt->phase_seg1 + 1,
+			       bt->phase_seg2,
+			       bt->bitrate);
+
+	/* Not init the filter with the default values */
+	for (cnt = 0; cnt < FIM_CAN_NR_FILTERS; cnt++)
+		fim_can_filter_accept(&filters[cnt]);
+
+	/* Now setup the init message */
+	memcpy(data, &cfg, sizeof(struct fim_can_timing_t));
+	ptr = data + sizeof(struct fim_can_timing_t);
+	for (cnt = 0; cnt < FIM_CAN_NR_FILTERS; cnt++)
+		memcpy(ptr, filters[cnt].filter, FIM_CAN_FILTER_SIZE);
+
+	retval = fim_can_send_buffer(port, data, sizeof(data));
+	if (retval)
+		printk_err("Couldn't send the init config.\n");
+
+	return retval;
+}
+
+/*
+ * Implement the interface for chaning the state of the CAN-controller. The user
+ * have access to this function over the sysfs attributes:
+ * echo 2 > sys/class/net/can0/can_restart : mode = 1 (CAN_MODE_START)
+ * 
+ */
+static int fim_can_set_mode(struct net_device *dev, enum can_mode mode)
+{
+	struct fim_can_t *port;
+	struct fim_driver *fim;
+	int retval;
+
+	port = netdev_priv(dev);
+	fim = &port->fim;
+
+	printk_debug("Set mode command %x called (FIM %i)\n", mode, fim->picnr);
+
+	if (!port->opened) {
+		printk_err("CAN-port %i was not opened up now\n", fim->picnr);
+		return -ERESTARTSYS;
+	}
+
+	/*
+	 * The different modes can be set using the sysfs attributes:
+	 * start : /sys/class/net/can0/can_restart
+	 */
+	switch (mode) {
+	case CAN_MODE_START:
+		printk_dbg("FIM%i: Starting the controller\n", fim->picnr);
+		retval = fim_can_restart_fim(port);
+		break;
+	case CAN_MODE_STOP:
+		printk_dbg("FIM%i: Stopping the controller\n", fim->picnr);
+		retval = fim_can_stop_fim(port);
+		break;
+	default:
+		retval = -EOPNOTSUPP;
+		break;
+	}
+
+	return retval;
+}
+
+/*
+ * Read the status registers of the FIM for obtaining the status information.
+ */
+static int fim_can_get_state(struct net_device *dev, enum can_state *state)
+{
+	struct fim_can_t *port;
+	struct fim_driver *fim;
+	int retval;
+	unsigned int bus, fatal;
+
+	port = netdev_priv(dev);
+	fim = &port->fim;
+	printk_debug("Get state called (FIM %i)\n", fim->picnr);
+
+	retval = fim_get_exp_reg(fim, FIM_CAN_BUSSTATE_REG, &bus);
+	if (retval) {
+		printk_err("Reading the bus state register (%i)\n", retval);
+		goto exit_all;
+	}
+
+	retval = fim_get_exp_reg(fim, FIM_CAN_FATALERR_REG, &fatal);
+	if (retval) {
+		printk_err("Reading the fatal error register (%i)\n", retval);
+		goto exit_all;
+	}
+
+	/* @XXX: Are we using the correct flag for informing about the error? */
+	if (fatal) {
+		*state = CAN_STATE_BUS_OFF;
+		goto exit_all;
+	}
+
+	/* Check the current status of the BUS */
+	if (port->can.state == CAN_STATE_STOPPED) {
+		printk_debug("Controller was stopped\n");
+		*state = CAN_STATE_STOPPED;
+	} else if (bus & FIM_CAN_BUSSTATE_OFF) {
+		printk_debug("Current Bus state seems to be OFF\n");
+		*state = CAN_STATE_BUS_OFF;
+	} else if (bus & FIM_CAN_BUSSTATE_ERRPAS) {
+		printk_debug("Bus seems to be in the passive error state\n");
+		*state = CAN_STATE_BUS_PASSIVE;
+	} else
+		*state = CAN_STATE_ACTIVE;
+
+	retval = 0;
+
+exit_all:
+	return retval;
+}
+
+/* Return zero if the FIM was successful stopped */
+static int fim_can_stop_fim(struct fim_can_t *port)
+{
+	int retval;
+	struct fim_driver *fim;
+
+	if (!port)
+		return -ENODEV;
+
+	/*
+	 * We will disable the IRQ anyway (otherwise is too dangerous having the
+	 * active interrupt)
+	 */
+	retval = 0;
+	fim = &port->fim;
+	if (fim_is_running(fim)) {
+		retval = fim_send_stop(fim);
+		printk_debug("%s: Disabling interrupt\n",__func__);
+		fim_disable_irq(fim);
+	}
+
+	return retval;
+}
+
+/*
+ * This function will download the firmware, start and reconfigure the CAN-controller
+ * For starting the FIM we assume that it was already stopped!
+ */
+static int fim_can_start_fim(struct fim_can_t *port)
+{
+	struct fim_driver *fim;
+	int retval;
+
+	if (!port)
+		return -ENODEV;
+
+	fim = &port->fim;
+	if (fim_is_running(fim)) {
+		printk_debug("The FIM %i is already running\n", fim->picnr);
+		return 0;
+	}
+
+	/*
+	 * The function for downloading the firmware will stop the FIM if
+	 * its still running
+	 */
+	retval = fim_download_firmware(fim);
+	if (retval)
+		return -EAGAIN;
+
+	retval = fim_send_start(fim);
+	if (retval)
+		return -EAGAIN;
+	
+	printk_debug("Enable interrupt\n");
+	fim_enable_irq(fim);
+
+	/*
+	 * Disable the interrupt if an error is detected, otherwise the system
+	 * can hang up
+	 */
+	retval = fim_can_set_init_config(port);
+	if (retval) {
+		printk_debug("%s: Disabling interrupt\n",__func__);
+		fim_disable_irq(fim);
+		return retval;
+	}
+
+	return retval;
+}
+
+/*
+ * This function restart the FIM
+ * It includes the reconfiguration of the CAN-port too (it will send the init
+ * configuration to the port too)
+ */
+static int fim_can_restart_fim(struct fim_can_t *port)
+{
+	int retval;
+
+	if (!port)
+		return -ENODEV;
+
+	/* First try to stop the FIM */
+	retval = fim_can_stop_fim(port);
+	if (retval)
+		return retval;
+
+	return fim_can_start_fim(port);
+}
+
+/*
+ * This function is called when the interface is going up (ifconfig <iface> up)
+ * Only enable the interrupt and restart the netdev queue.
+ */
+static int fim_can_open(struct net_device *dev)
+{
+	struct fim_can_t *port;
+	struct fim_driver *fim;
+	int retval;
+
+	/* So, lets start the PIC with our default data */
+	port = netdev_priv(dev);
+	fim = &port->fim;
+	printk_debug("Open function called (FIM %i)\n", fim->picnr);
+
+	/* Always set the CAN-interface to the active state! */
+	retval = fim_can_restart_fim(port);
+	if (!retval) {
+		netif_start_queue(dev);
+		port->can.state = CAN_STATE_ACTIVE;
+		port->opened = 1;
+	}
+
+	return retval;
+}
+
+/*
+ * This function is called when the interface is going down (ifconfig <iface> down)
+ * Disable the IRQ and the netif queue.
+ */
+static int fim_can_stop(struct net_device *dev)
+{
+	struct fim_can_t *port;
+	struct fim_driver *fim;
+	int retval = 0;
+
+	port = netdev_priv(dev);
+	fim = &port->fim;
+	printk_debug("Close function called (FIM %i)\n", fim->picnr);
+
+	/* Paranoic sanity check */
+	if (!port->opened)
+		return 0;
+
+	netif_stop_queue(dev);
+	can_close_cleanup(dev);
+	port->can.state = CAN_STATE_STOPPED;
+	port->opened = 0;
+
+	/* Transmit a shutdown command to the FIM before stopping it */
+	if (fim_is_running(fim)) {
+		retval = fim_can_shutdown(port);
+		if (retval) {
+			printk_err("Unable to shutdown the FIM %i\n", fim->picnr);
+			goto err_exit;
+		}
+
+		/*
+		 * The below delay is arbitrary and should give the FIM some time
+		 * for processing the last CAN-frame on the bus
+		 */
+		udelay(1000);
+
+		/* Check if need to stop the FIM */
+		printk_debug("%s: Disabling interrupt\n", __func__);
+		fim_disable_irq(fim);
+		fim_send_stop(fim);
+	}
+
+err_exit:
+	return retval;
+}
+
+/*
+ * This function will save the SKB-pointer inside the FIM-buffer. By this way
+ * is possible to free the SKB inside the TX-callback
+ */
+static int fim_can_send_skb(struct fim_can_t *port, unsigned char *data, int len,
+			    struct sk_buff *skb)
+{
+	struct fim_buffer_t buf;
+
+	buf.private = skb;
+	buf.length = len;
+	buf.data = data;
+	return fim_send_buffer(&port->fim, &buf);
+}
+
+/*
+ * This function will mask the FIM-buffer so that in the TX-callback we can
+ * know that it doesn't contain any important information for us
+ */
+static int fim_can_send_buffer(struct fim_can_t *port, unsigned char *data, int len)
+{
+	struct fim_buffer_t buf;
+
+	buf.private = FIM_PRIVATE_BUFFER;
+	buf.length = len;
+	buf.data = data;
+	return fim_send_buffer(&port->fim, &buf);
+}
+
+/*
+ * Write one bit to the frame, update the CRC, and bit stuff if we have
+ * 5 consecutive bits all of the same value.
+ */
+#define LOWEST_FIVE_BITS_MASK			(0x1f)
+static void fim_can_write_stuffbit(struct fim_can_txframe *frame, unsigned bit)
+{
+	bit &= 1;
+
+	frame->buffer[frame->bytepos] = (frame->buffer[frame->bytepos] << 1) | bit;
+	frame->last5 = ((frame->last5 << 1) | bit) & LOWEST_FIVE_BITS_MASK;
+	frame->bitcnt++;
+	frame->stuffed++;
+
+	if (frame->bitcnt == 8) {
+		frame->bytepos += 1;
+		frame->bitcnt = 0;
+	}
+
+	if (frame->bitstuff && ((frame->last5 == LOWEST_FIVE_BITS_MASK) ||
+				(frame->last5 == 0)))
+		fim_can_write_stuffbit(frame, ~frame->last5);
+}
+
+/*
+ * Real CAN polynomial is 0xc599, but this value used used
+ * do to the way the algorithm works.  The high bit is
+ * xor'ed in the first line.
+ */
+#define CAN_CRC_POLYNOMIAL_LESS_HIGH_BIT	(0x4599)
+inline static void fim_can_add_crc(struct fim_can_txframe *frame, unsigned short bit)
+{
+	unsigned short crcnxt;
+
+	frame->crc <<= 1;
+	crcnxt = ((frame->crc & 0x8000) != 0) ^ (bit);
+	frame->crc &= 0x7ffe;
+	if (crcnxt)
+		frame->crc ^= CAN_CRC_POLYNOMIAL_LESS_HIGH_BIT;
+}
+
+inline static void fim_can_frame_writeone(struct fim_can_txframe *frame, u32 bit)
+{
+	fim_can_add_crc(frame, bit);
+	fim_can_write_stuffbit(frame, bit);
+}
+
+/*
+ * Write one or more bits to the frame.
+ */
+inline static void fim_can_frame_write(struct fim_can_txframe *frame, unsigned int data,
+				       int bits)
+{
+	int idx;
+	for (idx = bits - 1; idx >= 0; idx--)
+		fim_can_frame_writeone(frame, (data & (1 << idx)) != 0);
+}
+
+inline static void fim_can_frame_stop_stuff(struct fim_can_txframe *frame)
+{
+	frame->bitstuff = 0;
+}
+
+inline static void fim_can_frame_start(struct fim_can_txframe *frame)
+{
+	frame->crc = 0;
+	frame->bytepos = FIRST_DATA_BYTE_IN_MESSAGE;
+
+	frame->last5 = CAN_BUS_RECESSIVE;
+	frame->bitcnt = 0;
+	frame->bitstuff = 1;
+	frame->stuffed = 0;
+	fim_can_frame_writeone(frame, CAN_BUS_DOMINANT);
+}
+
+inline static void fim_can_frame_stop(struct fim_can_txframe *frame)
+{
+	frame->buffer[frame->bytepos] <<= (8 - frame->bitcnt);
+	frame->buffer[frame->bytepos] |= (0xFF >> frame->bitcnt);
+}
+
+/*
+ * Function that checks if the CAN-controller registered an error
+ * If an error is detected, then a error frame will be passed to the socket-layer
+ * Return zero if no error was detected, otherwise an value different than zero
+ */
+static int fim_can_check_error(struct net_device *dev)
+{
+	struct sk_buff *skb;
+	struct fim_can_t *port;
+	struct fim_driver *fim;
+	struct can_frame *cf;
+	unsigned int bus, fatal;
+	struct net_device_stats *stats;
+
+	port = netdev_priv(dev);
+	fim = &port->fim;
+	fim_get_exp_reg(fim, FIM_CAN_BUSSTATE_REG, &bus);
+	fim_get_exp_reg(fim, FIM_CAN_FATALERR_REG, &fatal);
+
+	if ((!bus) && (!fatal))
+		return 0;
+
+	printk_debug("Calling the error handling function (Bus 0x%x)\n", bus);
+
+	/* Stop the FIM if it's running */
+	if (fim_is_running(fim)) {
+		printk_debug("%s: Disabling interrupt\n",__func__);
+		fim_disable_irq(fim);
+		fim_send_stop(fim);
+	}
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (!skb) {
+		printk_err("Couldn't create the ERROR frame\n");
+		goto end_exit;
+	}
+
+	skb->dev = dev;
+	skb->protocol = htons(ETH_P_CAN);
+	cf = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
+	memset(cf, 0x00, sizeof(struct can_frame));
+	cf->can_id = CAN_ERR_FLAG;
+	cf->can_dlc = CAN_ERR_DLC;
+
+	/*
+	 * These are the possible errors defined in the FIM-CAN specification
+	 * The corresponding error flags are under: include/linux/can/error.h
+	 */
+	if (bus & FIM_CAN_BUSSTATE_OFF)
+		cf->can_id |= CAN_ERR_BUSOFF;
+
+	/* @XXX: This seems to be the correct flags for our controller, or? */
+	if (bus & FIM_CAN_BUSSTATE_ERRPAS) {
+		cf->can_id |= CAN_ERR_BUSERROR;
+		cf->data[1] |= CAN_ERR_CRTL_TX_PASSIVE;
+		port->can.can_stats.error_passive++;
+	}
+
+	/* Pass the error frame to the socket layer */
+	netif_rx(skb);
+
+	dev->last_rx = jiffies;
+	stats = dev->get_stats(dev);
+	stats->rx_packets++;
+	stats->rx_bytes += cf->can_dlc;
+
+end_exit:
+	return 1;
+}
+
+static int fim_can_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct fim_can_t *port;
+	struct net_device_stats *stats;
+	struct can_frame *cf;
+	struct fim_can_txframe frame;
+	u8 dlc, cnt;
+	int retval;
+	canid_t id;
+
+	port = netdev_priv(dev);
+	spin_lock(&port->lock);
+	netif_stop_queue(dev);
+	spin_unlock(&port->lock);
+
+	stats = dev->get_stats(dev);
+	cf = (struct can_frame *)skb->data;
+
+	id = cf->can_id;
+	dlc = cf->can_dlc;
+
+	/*
+	 * Check for the bus state before sending the data
+	 * @FIXME: The controller fails if its state is the error passive
+	 */
+	if (fim_can_check_error(dev)) {
+		printk_debug("Bus OFF or unknown error. Aborting Xmit %p\n", skb);
+		stats->tx_dropped++;
+		spin_lock(&port->lock);
+		dev_kfree_skb(skb);
+		netif_wake_queue(dev);
+		spin_unlock(&port->lock);
+		return NETDEV_TX_OK;
+	}
+
+	/* Reset our frame first */
+	memset(&frame, 0x0, sizeof(struct fim_can_txframe));
+
+	fim_can_frame_start(&frame);
+	fim_can_dump_frame(&frame, "START");
+
+	/* Check for the extended ID (18 bits length) */
+	if (id & CAN_EFF_FLAG)
+		fim_can_frame_write(&frame, id, 18);
+	else
+		fim_can_frame_write(&frame, id, 11);
+
+	fim_can_dump_frame(&frame, "ID");
+
+	/* Check if we have a RTR message */
+	if (id & CAN_RTR_FLAG)
+		fim_can_frame_write(&frame, 1, 1);
+	else
+		fim_can_frame_write(&frame, 0, 1);
+	fim_can_dump_frame(&frame, "RTR");
+
+	/* Write the identifier bit which is dominant in the standard frame */
+	fim_can_frame_write(&frame, 0, 1);
+	fim_can_dump_frame(&frame, "IDE");
+
+	/* Write the R0 bit */
+	fim_can_frame_write(&frame, 0, 1);
+	fim_can_dump_frame(&frame, "RESERVE");
+
+	/* Write the control field (data length passed by the user) */
+	fim_can_frame_write(&frame, dlc, 4);
+	fim_can_dump_frame(&frame, "CONTROL");
+
+	/*
+	 * Now write the data bytes (maximal eight bytes)
+	 * According to the CAN-specification, don't add the data bytes to the frame
+	 * although the data length code is greter than zero
+	 */
+	for (cnt = 0; cnt < (dlc & 0x0F) && !(id & CAN_RTR_FLAG); cnt++)
+		fim_can_frame_write(&frame, cf->data[cnt], 8);
+
+	fim_can_dump_frame(&frame, "DATA");
+
+	/* Write the CRC */
+	fim_can_frame_write(&frame, frame.crc, 15);
+	fim_can_dump_frame(&frame, "CRC");
+
+	/* Stop the bit stuffing for this frame */
+	fim_can_frame_stop_stuff(&frame);
+
+	/* Write the CRC delimiter, CRC ACK and ACK delimiter */
+	fim_can_frame_write(&frame, CAN_BUS_RECESSIVE, 3);
+	fim_can_dump_frame(&frame, "CRC/ACK");
+
+	fim_can_frame_stop(&frame);
+
+	/*
+	 * Set the first three byte of the frame as described in the specification
+	 * (see page 8, transmit message structure)
+	 */
+	frame.buffer[0] = FIM_CAN_CMD_TX;
+	frame.buffer[1] = ((id & CAN_EFF_FLAG) ? 18 : 11) + 1;
+	frame.buffer[2] = frame.stuffed;
+	fim_can_dump_frame(&frame, "END");
+
+	printk_debug("Sending a new SKB %p\n", skb);
+	retval = fim_can_send_skb(port, frame.buffer, 3 + (frame.stuffed + 7) / 8, skb);
+	if (retval) {
+		stats->tx_fifo_errors++;
+		stats->tx_dropped++;
+		printk_err("Couldn't send a frame with the ID 0x%X\n", id);
+		goto exit_free_skb;
+	}
+
+	stats->tx_bytes += dlc;
+	stats->tx_packets++;
+	dev->trans_start = jiffies;
+
+exit_free_skb:
+	return NETDEV_TX_OK;
+}
+
+/*
+ * Only errors are being generated by the firmware
+ * If an error ocurrs the FIM must be resetted (it hangs up in a endless loop)
+ */
+static void fim_can_isr(struct fim_driver *driver, int irq, unsigned char code,
+			unsigned int rx_fifo)
+{
+	struct net_device *dev;
+	struct fim_can_t *port;
+
+	printk_debug("FIM IRQ %x | %08x\n", code, rx_fifo);
+
+	port = driver->driver_data;
+	dev = port->dev;
+
+	if (code && code != FIM_CAN_INT_RESET) {
+		printk_debug("%s: Disabling interrupt\n",__func__);
+		fim_disable_irq(driver);
+		fim_send_stop(driver);
+	}
+
+	switch (code) {
+	case FIM_CAN_INT_CMD_ERR:
+		printk_err("Command error (FIM %i)\n", driver->picnr);
+		break;
+	case FIM_CAN_INT_HW_FAILURE:
+		printk_err("HW failure (FIM %i). Please restart the controller.\n",
+			   driver->picnr);
+		break;
+	case FIM_CAN_INT_FILTER_ERR:
+		printk_err("Filter error (FIM %i)\n", driver->picnr);
+		break;
+	case FIM_CAN_INT_CFG_ERR:
+		printk_err("Config error (FIM %i)\n", driver->picnr);
+		break;
+	default:
+		break;
+	}
+
+	/*
+	 * Check if we have a stop condition of the CAN-controller
+	 * If we need to stop the controller, restart the netif-queue anyway, then
+	 * otherwise we will have problems when freeing the SKBs
+	 */
+	if (code && code != FIM_CAN_INT_RESET) {
+		if (netif_queue_stopped(dev))
+			netif_wake_queue(dev);
+	}
+}
+
+static void fim_can_tx_isr(struct fim_driver *driver, int irq,
+			   struct fim_buffer_t *pdata)
+{
+	struct fim_can_t *port;
+	struct fim_buffer_t *buf;
+	struct net_device *dev;
+	unsigned long flags;
+	struct sk_buff *skb;
+
+	port = driver->driver_data;
+	buf = pdata->private;
+	dev = port->dev;
+	skb = pdata->private;
+
+	spin_lock_irqsave(&port->lock, flags);
+	printk_debug("TX callback | SKB %p | dev %p\n", skb, dev);
+
+	/*
+	 * @FIXME: If we remove this delay, then we will not catch the errors
+	 * that can ocurrs during the frame transmission
+	 */
+	udelay(200);
+
+	/* Restart the TX-queue if it was stopped for some reason */
+	if (fim_can_check_error(dev))
+		printk_debug("The udelay is working correctly\n");
+
+	/*
+	 * Check if we have a SKB to free
+	 * IMPORTANT: Allways free the SKB, otherwise we will have a memory leak,
+	 * although in the template CAN-drivers they are not freeing the SKB
+	 */
+	if (skb && skb != FIM_PRIVATE_BUFFER) {
+		printk_debug("Freeing the   SKB %p\n", skb);
+		dev_kfree_skb_irq(skb);
+	}
+
+	/*
+	 * Although we had a failure, we must restart the TX-queue, otherwise
+	 * we will have a kernel panic when freeing the SKBs
+	 */
+	if (dev && netif_queue_stopped(dev)) {
+		printk_debug("Restarting the netif-queue\n");
+		netif_wake_queue(dev);
+	}
+
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+/*
+ * This function is called when the FIM has a new CAN-frame for us
+ * According to the configured filters we will receive only the filtered messages
+ */
+static void fim_can_rx_isr(struct fim_driver *driver, int irq,
+			   struct fim_buffer_t *pdata)
+{
+	struct sk_buff *skb;
+	struct fim_can_t *port;
+	struct net_device *dev;
+	struct can_frame *cf;
+	struct net_device_stats *stats;
+	struct fim_can_rxframe *rx;
+
+	/* Set our internal data */
+	rx = (struct fim_can_rxframe *)pdata->data;
+	port = driver->driver_data;
+	dev = port->dev;
+
+	printk_debug("New CAN-frame (%i bytes) | ID 0x%02x%02x%02x%02x\n",
+		     pdata->length, rx->id[3], rx->id[2], rx->id[1], rx->id[0]);
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (!skb) {
+		printk_err("No memory available? Dropping a CAN-frame!\n");
+		return;
+	}
+
+	skb->dev = dev;
+	skb->protocol = htons(ETH_P_CAN);
+
+	cf = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
+	memset(cf, 0x00, sizeof(struct can_frame));
+
+	/* Start processing the arrived frame */
+
+	/* Set the CAN-ID corresponding to the FIM-controller specification */
+	if (rx->id[0] & 0x80)
+		cf->can_id |= CAN_RTR_FLAG;
+
+	if (rx->id[0] & 0x40)
+		cf->can_id |= CAN_EFF_FLAG;
+
+	/* Remove the control bits and set to the correct ID */
+	rx->id[0] &= 0x1F;
+	if (cf->can_id & CAN_EFF_FLAG)
+		cf->can_id |= rx->id[3] + (rx->id[2] << 8) + (rx->id[1] << 16) +
+		    (rx->id[0] << 24);
+	else
+		cf->can_id |= (rx->id[1] >> 2) + (rx->id[0] << 6);
+
+	/* Copy the data */
+	cf->can_dlc = rx->ctrl & 0x0F;
+	memcpy(cf->data, rx->data, cf->can_dlc);
+
+	/* Pass the data to the sockets-layer */
+	netif_rx(skb);
+
+	/* Update the device statics */
+	stats = dev->get_stats(dev);
+	dev->last_rx = jiffies;
+	stats->rx_packets++;
+	stats->rx_bytes += cf->can_dlc;
+}
+
+static int unregister_fim_can(struct fim_can_t *port)
+{
+	struct fim_gpio_t gpios[FIM_CAN_MAX_GPIOS];
+	int cnt;
+	struct fim_driver *fim;
+
+	if (!port || !port->reg)
+		return -ENODEV;
+
+	//unregister_netdev(port->dev);
+	unregister_candev(port->dev);
+	free_candev(port->dev);
+
+	/* Activate the interrupt (@BUG in the IRQ-subsystem?) */
+	fim = &port->fim;
+
+	printk_debug("%s: Disabling interrupt\n", __func__);
+	fim_disable_irq(fim);
+
+	/* Stop the FIM first */
+	if (!fim_is_running(fim))
+		fim_send_stop(fim);
+
+	printk_debug("Going to unregister the FIM %i (running %i)\n",
+		    fim->picnr, fim_is_running(fim));
+	fim_unregister_driver(fim);
+
+	/* And free the GPIOs */
+	memcpy(gpios, port->gpios, sizeof(struct fim_gpio_t) * FIM_CAN_MAX_GPIOS);
+	for (cnt = 0; cnt < FIM_CAN_MAX_GPIOS; cnt++) {
+		
+		if (gpios[cnt].nr != FIM_GPIO_DONT_USE) {
+			printk_debug("Freeing the GPIO %i\n", gpios[cnt].nr);
+			gpio_free(gpios[cnt].nr);
+		}
+	}
+
+	/* Free the obtained clock */
+	if (!IS_ERR(port->cpu_clk))
+		clk_put(port->cpu_clk);
+	
+	port->reg = 0;
+
+	return 0;
+}
+
+/*
+ * IMPORTANT: First register the FIM-driver, and at last the CAN-device, then
+ * it will automatically start with the bit time configuration.
+ */
+static int register_fim_can(struct device *devi, int picnr, struct fim_gpio_t gpios[],
+			    int bitrate)
+{
+	int retval, cnt;
+	int func;
+	struct net_device *dev;
+	struct fim_can_t *port;
+	struct fim_dma_cfg_t dma_cfg;
+
+	/* Now create the net device */
+	dev = alloc_candev(sizeof(struct fim_can_t));
+	if (!dev) {
+		printk_err("Couldn't alloc a new CAN device\n");
+		return -ENOMEM;
+	}
+
+	/* Set our port structure as private data */
+	port = netdev_priv(dev);
+	port->dev = dev;
+
+	/* Get a reference to the SYS clock for setting the baudrate */
+	if (IS_ERR(port->cpu_clk = clk_get(&dev->dev, "systemclock"))) {
+		printk_err("Couldn't get the SYS clock.\n");
+		goto err_free_candev;
+	}
+
+	/* Request the corresponding GPIOs (@XXX: Check the returned values) */
+	for (cnt=0; cnt < FIM_CAN_MAX_GPIOS; cnt++) {
+		if (gpios[cnt].nr == FIM_LAST_GPIO)
+			break;
+		if (gpios[cnt].nr == FIM_GPIO_DONT_USE)
+			continue;
+		printk_debug("Requesting the GPIO %i (Function %i)\n",
+			     gpios[cnt].nr, gpios[cnt].func);
+		retval = gpio_request(gpios[cnt].nr, FIM_DRIVER_NAME);
+		if (!retval) {
+			func = gpios[cnt].func;
+			gpio_configure_ns921x_unlocked(gpios[cnt].nr,
+						       NS921X_GPIO_INPUT,
+						       NS921X_GPIO_DONT_INVERT,
+						       func,
+						       NS921X_GPIO_ENABLE_PULLUP);
+		} else {
+			/* Free the already requested GPIOs */
+			printk_err("Couldn't request the GPIO %i\n", gpios[cnt].nr);
+			while (cnt)
+				gpio_free(gpios[--cnt].nr);
+			goto err_free_candev;
+		}
+	}
+
+	/* Now try to register the FIM-driver */
+	port->fim.picnr = picnr;
+	port->fim.driver.name = FIM_DRIVER_NAME;
+	port->fim.fim_isr = fim_can_isr;
+	port->fim.dma_tx_isr = fim_can_tx_isr;
+	port->fim.dma_rx_isr = fim_can_rx_isr;
+	port->fim.driver_data = port;
+
+	/* Specific DMA configuration for the CAN-controller */
+	dma_cfg.rxnr = FIM_CAN_DMA_BUFFERS;
+	dma_cfg.txnr = FIM_CAN_DMA_BUFFERS;
+	dma_cfg.rxsz = FIM_CAN_DMA_BUFFER_SIZE;
+	dma_cfg.txsz = FIM_CAN_DMA_BUFFER_SIZE;
+	port->fim.dma_cfg = &dma_cfg;
+
+	/* Check if have a firmware code for using to */
+	port->fim.fw_name = FIM_CAN_FIRMWARE_FILE;
+	port->fim.fw_code = FIM_CAN_FIRMWARE_CODE;
+	retval = fim_register_driver(&port->fim);
+	if (retval) {
+		printk_err("Couldn't register the FIM %i CAN driver.\n", picnr);
+		goto err_free_gpios;
+	}
+
+	/* Stop the FIM then it will be started in the open-function */
+	fim_send_stop(&port->fim);
+
+	/* Configure the net device with the default values */
+	dev->flags |= IFF_ECHO;
+	dev->open = fim_can_open;
+	dev->stop = fim_can_stop;
+	dev->hard_start_xmit = fim_can_xmit;
+
+	/* Special attributes for the CAN-stack */
+	port->can.do_get_state = fim_can_get_state;
+	port->can.do_set_mode = fim_can_set_mode;
+
+ 	port->can.bittiming.clock = clk_get_rate(port->cpu_clk);
+ 	printk_debug("CPU clock is %luHz\n", clk_get_rate(port->cpu_clk));
+
+	/*
+	 * @TODO: Set the correct maximal BRP for the controller.
+	 *   DEFAULT_MAX_BRP 64
+	 *   DEFAULT_MAX_SJW 4
+	 */
+ 	port->can.bittiming_const = &fim_bittiming_const;
+ 	if (!port->can.bittiming_const) {
+ 		printk_debug("bittiming_const is not initialized.\n");
+ 		goto err_unreg_fim;
+ 	}
+
+	/*
+	 * Only store the passed bitrate. The corresponding timing parameters
+	 * will be setup in the function that sends the configuration to the FIM.
+	 * (Luis Galdos)
+	 */
+	port->can.bittiming.bitrate    = bitrate;
+
+	/* Now register the new CAN-net device */
+	retval = register_candev(dev);
+	if (retval) {
+		printk_err("Registering the net device for the FIM %i\n", picnr);
+		goto err_unreg_fim;
+	}
+
+	spin_lock_init(&port->lock);
+	port->dev = dev;
+	memcpy(port->gpios, gpios, sizeof(struct fim_gpio_t) * FIM_CAN_MAX_GPIOS);
+	port->reg = 1;
+	dev_set_drvdata(devi, port);
+
+ 	return 0;
+
+err_unreg_fim:
+	fim_unregister_driver(&port->fim);
+
+err_free_gpios:
+	for (cnt = 0; cnt < FIM_CAN_MAX_GPIOS; cnt++) {
+		
+                if (gpios[cnt].nr != FIM_GPIO_DONT_USE) {
+			printk_debug("Freeing the GPIO %i\n", gpios[cnt].nr);
+			gpio_free(gpios[cnt].nr);
+		}
+	}
+	
+err_free_candev:
+	free_candev(dev);
+
+ 	return -EINVAL;
+}
+
+static __init int fim_can_probe(struct platform_device *pdev)
+{
+	int nrpics;
+	int retval;
+	struct fim_gpio_t gpios[FIM_CAN_MAX_GPIOS];
+	struct fim_can_platform_data *pdata;
+	int bitrate;
+
+	printk_debug("Starting the FIM CAN bus driver.\n");
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata)
+		return -ENODEV;
+
+	/* If no bitrate passed, then use the default bitrate */
+	bitrate = pdata->fim_can_bitrate ? pdata->fim_can_bitrate :
+		FIM_CAN_DEFAULT_BITRATE;
+
+	/* Sanity check for the passed bit rate */
+	if (bitrate <= 0 || bitrate > FIM_CAN_MAX_BITRATE) {
+		printk_err("Invalid bit rate %i (max. rate is %i)\n",
+			   bitrate, FIM_CAN_MAX_BITRATE);
+		return -EINVAL;
+	}
+
+	/* Get the number of available PICs from the FIM-core */
+	nrpics = fim_number_pics();
+
+	if (fim_check_device_id(fims_number, pdata->fim_nr)) {
+#if defined(MODULE)
+		printk_dbg("Skipping FIM%i (not selected)\n", pdata->fim_nr);
+#else
+		printk_err("Invalid FIM number '%i' in platform data\n", pdata->fim_nr);
+#endif
+		return -ENODEV;
+	}
+
+	/*
+	 * Start with the registration of the CAN-ports
+	 */
+	gpios[0].nr	= pdata->rx_gpio_nr;
+	gpios[0].func	= pdata->rx_gpio_func;
+	gpios[1].nr	= pdata->tx_gpio_nr;
+	gpios[1].func	= pdata->tx_gpio_func;
+	printk_debug("%s: GPIO RX: %d | GPIO TX: %d \n", __func__,
+		     gpios[0].nr, gpios[1].nr );
+	
+	/* XXX SDIO code approach */
+	retval = register_fim_can(&pdev->dev, pdata->fim_nr, gpios, bitrate);
+
+	return retval;
+}
+
+static __devexit int fim_can_remove(struct platform_device *pdev)
+{
+	struct fim_can_t *port;
+
+	port = dev_get_drvdata(&pdev->dev);
+	if (!port) {
+		printk_err("remove: NULL pointer found!\n");
+		return -ENODEV;
+	}
+
+	unregister_fim_can(port);
+	
+	return 0;
+}
+
+static struct platform_driver fim_can_platform_driver = {
+	.probe	= fim_can_probe,
+	.remove	= __devexit_p(fim_can_remove),
+	.driver	= {
+		   .owner = THIS_MODULE,
+		   .name  = FIM_DRIVER_NAME,
+	},
+};
+
+/*
+ * This is the function that will be called when the module is loaded
+ * into the kernel space
+ */
+static __init int fim_can_init(void)
+{
+	printk_info(DRIVER_DESC " " DRIVER_VERSION "\n");
+
+        /* Check for the passed number parameter */
+	if (fim_check_numbers_param(fims_number)) {
+		printk_err("Invalid number '%i' of FIMs to handle\n", fims_number);
+		return -EINVAL;
+	}
+
+	return platform_driver_register(&fim_can_platform_driver);
+}
+
+static __exit void fim_can_exit(void)
+{
+	printk_info("Removing the FIM CAN driver\n");
+	platform_driver_unregister(&fim_can_platform_driver);
+}
+
+module_init(fim_can_init);
+module_exit(fim_can_exit);
diff --git a/drivers/fims/can/fim_can.h b/drivers/fims/can/fim_can.h
new file mode 100644
index 000000000000..f954330205be
--- /dev/null
+++ b/drivers/fims/can/fim_can.h
@@ -0,0 +1,28 @@
+/* -*- linux-c -*-
+ *
+ * drivers/fims/can/fim_can.h
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ * Dummy header file for building the kernel with the FIM-CAN driver.
+ * For obtaining a valid fimrware please refer to the FIMs documentation of the
+ * Digi ESP.
+ *
+ */
+
+#warning "Using a dummy FIM CAN firmware"
+
+static const unsigned char fim_can_firmware[] = { 
+	0xff, 0xff, 0xff, 0xff,  0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff,  0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff,  0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff,  0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff,  0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff,  0xff, 0xff, 0xff, 0xff,
+};
+
diff --git a/drivers/fims/dma.h b/drivers/fims/dma.h
new file mode 100644
index 000000000000..9806e53f66e7
--- /dev/null
+++ b/drivers/fims/dma.h
@@ -0,0 +1,177 @@
+/* -*- linux-c -*-
+ *
+ * drivers/fims/dma.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ *  !Revision:   $Revision: 1.3 $
+ *  !Author:     Luis Galdos
+ *  !Descr:      
+ *  !References:
+ */
+
+
+#ifndef __FIM_API_DMA_H
+#define __FIM_API_DMA_H
+
+
+#include <linux/module.h>
+#include <linux/errno.h>
+
+#include <mach/dma-ns921x.h>
+
+
+/*
+ * The access to the DMA-channels of the FIMs is only provided by the FIM-API
+ * The FIM-drivers should not use the functions of this source code file
+ */
+
+
+inline static void fim_dma_reset_fifo(struct iohub_dma_fifo_t *fifo)
+{
+	if (!fifo)
+		return;
+	
+	fifo->next_free = fifo->first;
+	fifo->dma_first = fifo->first;
+	fifo->dma_last = fifo->dma_next = NULL;
+}
+
+
+inline int fim_dma_init_fifo(struct iohub_dma_fifo_t *fifo, int length,
+			     struct iohub_dma_desc_t **descs)
+{
+	if (!fifo || !length || !descs)
+		return -EINVAL;
+
+	/* @XXX: These are the start values for the API */
+	fifo->length = length;
+	fifo->first = (struct iohub_dma_desc_t *)descs;
+	fifo->last = fifo->first + fifo->length - 1;
+	
+	fifo->next_free = fifo->first;
+	fifo->dma_first = fifo->first;
+	fifo->dma_last = fifo->dma_next = NULL;
+	return 0;
+}
+
+
+inline static struct iohub_dma_desc_t *fim_dma_get_next(struct iohub_dma_fifo_t *fifo, 
+							struct iohub_dma_desc_t *ptr)
+{
+	if (ptr == fifo->last)
+		return fifo->first;
+	else
+		return ptr + 1;
+}
+
+
+/*
+ * Return the index of a passed DMA-descriptor
+ */
+inline static struct iohub_dma_desc_t *fim_dma_get_by_index(struct iohub_dma_fifo_t *fifo,
+							    int index)
+{
+	if (index < 0)
+		return fifo->first;
+	else if (index >= fifo->length)
+		return fifo->last;
+	else
+		return fifo->first + index;
+}
+
+
+inline static int fim_dma_length(struct iohub_dma_fifo_t *fifo)
+{
+	return fifo->length;
+}
+
+
+/*
+ * Returns the number of free DMA-descriptors
+ */
+inline static int fim_dma_frees(struct iohub_dma_fifo_t *fifo)
+{
+	int retval;
+
+	/* The first is the INIT state of the FIFO */
+	if (!fifo->dma_last && !fifo->dma_next)
+		retval = fifo->length;
+	else if (fifo->dma_last == fifo->dma_next)
+		retval = fifo->length;
+	else if (!fifo->dma_last)
+		retval = fifo->last - fifo->dma_next;
+	else if (fifo->dma_next == fifo->dma_first)
+		retval = (fifo->last - fifo->first);    
+	else if (fifo->dma_next > fifo->dma_first)
+		retval = (fifo->last - fifo->dma_next) +
+			(fifo->dma_first - fifo->first);
+	else
+		retval = (fifo->dma_first - fifo->dma_next);
+
+	return retval;
+}
+
+
+/*
+ * Check if the FIFO contains descriptors or is empty
+ */
+inline static int fim_dma_is_empty(struct iohub_dma_fifo_t *fifo)
+{
+	/* @XXX: That's only for FIFOs with one pointer */
+	if (fifo->length == 1 && fifo->dma_next)
+		return 0;
+
+	if (fim_dma_frees(fifo) == fifo->length)
+		return 1;
+	else
+		return 0;
+}
+
+
+inline static int fim_dma_get_index(struct iohub_dma_fifo_t *fifo,
+				    struct iohub_dma_desc_t *desc)
+{
+	return desc - fifo->first;
+}
+
+
+/*
+ * Check if there is enough space for a specific number of buffer descriptors.
+ * Return a pointer to the first buffer descriptor is the FIFO has enough space
+ */
+inline static struct iohub_dma_desc_t *fim_dma_alloc(struct iohub_dma_fifo_t *fifo,
+						     int count)
+{
+	int len, diff;
+	struct iohub_dma_desc_t *tmp, *retval;
+
+	len = fim_dma_frees(fifo);
+	if (len < count)
+		return NULL;
+
+	if (!fifo->dma_next) {
+		retval = fifo->first;
+		fifo->dma_next = fifo->first + count - 1;
+	} else {
+		retval = fim_dma_get_next(fifo, fifo->dma_next);
+		tmp = fifo->dma_next + count;
+		if (tmp <= fifo->last)
+			fifo->dma_next = tmp;
+		else {
+			diff = tmp - fifo->last - 1;
+			fifo->dma_next = fifo->first + diff;
+		}
+	}
+
+	return retval;
+}
+
+
+
+#endif /* ifndef __FIM_API_DMA_H */
diff --git a/drivers/fims/fim.c b/drivers/fims/fim.c
new file mode 100644
index 000000000000..8df1db902a88
--- /dev/null
+++ b/drivers/fims/fim.c
@@ -0,0 +1,2216 @@
+/* -*- linux-c -*-
+ *
+ * drivers/fim.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ *  !Revision:   $Revision: 1.45 $
+ *  !Author:     Silvano Najera, Luis Galdos
+ *  !Descr:      
+ *  !References:
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/blkdev.h>
+#include <linux/spinlock.h>
+#include <linux/moduleparam.h>
+#include <linux/firmware.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/signal.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+#include <asm/irq.h>
+#include <asm/signal.h>
+#include <linux/kfifo.h>
+#include <linux/sysfs.h>
+
+/* Arch-dependent header files */
+#include <mach/regs-sys-ns921x.h>
+#include <mach/regs-iohub-ns921x.h>
+#include <mach/irqs.h>
+
+/* Include the structure for the FIM-firmware */
+#include <mach/fim-ns921x.h>
+#include <mach/fim-firmware.h>
+
+#include "fim_reg.h"
+#include "dma.h"
+
+#define DRIVER_VERSION			"v0.2"
+#define DRIVER_AUTHOR			"Silvano Najera, Luis Galdos"
+#define DRIVER_DESC			"FIMs driver"
+
+/* Module information */
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.1");
+
+#define DRIVER_NAME			"fim"
+#define FIM_DRIVER_NAME			"fims"
+#define FIM_BUS_TYPE_NAME		"fim-bus"
+
+#define IRQ_NS921X_PIC			IRQ_NS921X_PIC0
+#define	FIM0_SHIFT			6
+
+#if 0
+#define FIM_CORE_DEBUG
+#endif
+
+#define printk_err(fmt, args...)	printk(KERN_ERR "[ ERROR ] fims: " fmt, ## args)
+#define printk_info(fmt, args...)	printk(KERN_INFO "fims: " fmt, ## args)
+#define printk_warn(fmt, args...)	printk(KERN_DEBUG "fims: " fmt, ## args)
+
+#ifdef FIM_CORE_DEBUG
+#  define printk_debug(fmt, args...)	printk(KERN_DEBUG "fims: %s() " fmt, __FUNCTION__ , ## args)
+#else
+#  define printk_debug(fmt, args...)
+#endif
+
+
+#define PIC_IS_LOCKED(pic)		(atomic_read(&pic->requested))
+#define PIC_LOCK_REQUEST(pic)		do { atomic_set(&pic->requested, 1); } while (0)
+#define PIC_FREE_REQUEST(pic)		do { atomic_set(&pic->requested, 0); } while (0)
+
+#define PIC_TX_LOCK(pic)		do { atomic_set(&pic->tx_tasked, 1); } while (0)
+#define PIC_TX_FREE(pic)		do { atomic_set(&pic->tx_tasked, 0); } while (0)
+#define PIC_TX_IS_FREE(pic)		(!atomic_read(&pic->tx_tasked))
+
+
+/* Use the same function for all the binary attributes */
+#define FIM_SYSFS_ATTR(_name) \
+{ \
+    .attr =  { \
+	    .name = __stringify(_name), \
+	    .mode = S_IRUGO, \
+	    .owner = THIS_MODULE, \
+    }, \
+    .read =  fim_sysfs_attr_read, \
+}
+
+
+struct fims_t {
+	struct pic_t pics[FIM_NR_PICS];
+	struct bus_type *bus_type;
+	struct device *bus_dev;
+	struct device_driver *driver;
+};
+
+static struct fims_t *the_fims;
+
+
+static int pic_config_output_clock_divisor(struct pic_t *pic,
+					   struct fim_program_t *program);
+static int pic_send_interrupt(struct pic_t *pic, unsigned int code);
+
+
+static int pic_download_firmware(struct pic_t *pic, const unsigned char *buffer);
+static int pic_stop_and_reset(struct pic_t *pic);
+static int pic_start_at_zero(struct pic_t *pic);
+
+
+static int fim_remove(struct device *dev);
+static int fim_probe(struct device *dev);
+static irqreturn_t pic_irq(int irq, void *tpic);
+static int pic_dma_init(struct pic_t *pic, struct fim_dma_cfg_t *cfg);
+static void pic_dma_stop(struct pic_t *pic);
+static int pic_is_running(struct pic_t *pic);
+
+
+/* Interrupt handlers */
+static void isr_from_pic(struct pic_t *pic, int irqnr);
+static void isr_dma_tx(struct pic_t *pic, int irqnr);
+static void isr_dma_rx(struct pic_t *pic, int irqnr);
+static int fim_start_tx_dma(struct pic_t *pic);
+
+
+/*
+ * Be aware with this function, we have max. one PAGE available for writing into
+ * the return buffer
+ * When the module is unregistered it's NOT required to reset the attributes
+ */
+static ssize_t fim_sysfs_attr_read(struct kobject *kobj, struct bin_attribute *attr,
+				   char *buffer, loff_t off, size_t count)
+{
+	int size, cnt;
+	struct device *dev;
+	struct pic_t *pic;
+	struct attribute *bin;
+	struct iohub_dma_desc_t *desc;
+	unsigned int ctrlreg;
+
+	dev = container_of(kobj, struct device, kobj);
+	bin = &attr->attr;
+	pic = (struct pic_t *)dev->driver_data;
+	if (!pic)
+		return 0;
+
+	/* Check for the correct attribute */
+	if (!strcmp(bin->name, "irq"))
+		size = snprintf(buffer, PAGE_SIZE, "%i", pic->irq);
+
+	else if (pic->driver && !strcmp(bin->name, "module"))
+		size = snprintf(buffer, PAGE_SIZE, "%s", pic->driver->driver.name);
+	
+	else if (!strcmp(bin->name, "running"))
+		size = snprintf(buffer, PAGE_SIZE, "%i", pic_is_running(pic));
+
+	else if (pic->driver && !strcmp(bin->name, "version"))
+		size = snprintf(buffer, PAGE_SIZE, "@TODO: Version of the firmware");
+
+	else if (pic->driver && !strcmp(bin->name, "fwname"))
+		size = snprintf(buffer, PAGE_SIZE, "%s", pic->fw_name);
+
+	else if (pic->driver && !strcmp(bin->name, "fwlength"))
+		size = snprintf(buffer, PAGE_SIZE, "%i", pic->fw_length);
+
+	else if (pic->driver && !strcmp(bin->name, "txdma")) {
+		ctrlreg = readl(pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+		size = snprintf(buffer, PAGE_SIZE, "CTRL 0x%08x\n", ctrlreg);
+		ctrlreg = readl(pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+		for (cnt=0; cnt < pic->tx_fifo.length; cnt++) {
+			desc = fim_dma_get_by_index(&pic->tx_fifo, cnt);
+			size += snprintf(buffer + size,
+					PAGE_SIZE - size,
+					"[ TX %02i ] 0x%04x | %i\n",
+					cnt, desc->control, desc->length);
+		}
+	}
+
+	else if (pic->driver && !strcmp(bin->name, "rxdma")) {
+		ctrlreg = readl(pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+		size = snprintf(buffer, PAGE_SIZE, "CTRL 0x%08x\n", ctrlreg);
+		for (cnt=0; cnt < pic->rx_fifo.length; cnt++) {
+			desc = fim_dma_get_by_index(&pic->rx_fifo, cnt);
+			size += snprintf(buffer + size,
+					PAGE_SIZE - size,
+					"[ RX %02i ] 0x%04x | %i\n",
+					cnt, desc->control, desc->length);
+		}
+	}
+
+	else
+		size = 0;
+	
+	/* Only allows a single read for this binary attributes */
+	if (off || count < size)
+		return 0;
+
+	return size;
+}
+
+
+/* These are the non-default binary attributes for the FIMs */
+static struct bin_attribute fim_sysfs_attrs[] = {
+        FIM_SYSFS_ATTR(irq),
+	FIM_SYSFS_ATTR(running),
+	FIM_SYSFS_ATTR(module),
+	FIM_SYSFS_ATTR(version),
+	FIM_SYSFS_ATTR(fwname),
+	FIM_SYSFS_ATTR(fwlength),
+	FIM_SYSFS_ATTR(txdma),
+	FIM_SYSFS_ATTR(rxdma),
+};
+
+
+
+/*
+ * This tasklet will search for full receive DMA-buffers
+ * Only if the PIC has an associated driver the corresponding callback will be started
+ */
+static void pic_rx_tasklet_func(unsigned long data)
+{
+	struct pic_t *pic;
+	struct iohub_dma_desc_t *desc;
+	struct fim_driver *driver;
+	struct fim_buffer_t buffer;
+	struct iohub_dma_fifo_t *fifo;
+
+	/* If no driver is attached, then the DMA-channel is normally disabled */
+	pic = (struct pic_t *)data;
+	if (!pic || !pic->driver)
+		return;
+		
+	driver = pic->driver;
+	fifo = &pic->rx_fifo;
+
+	/* The pointer to DMA-first will be updated in the locked segment! */
+	spin_lock(&pic->rx_lock);
+	desc = fifo->dma_first;
+
+	/*
+	 * If a driver is waiting for the data then create a callback buffer,
+	 * otherwise only restore the DMA-buffer descriptor
+	 */
+	while (desc->control & IOHUB_DMA_DESC_CTRL_FULL) {
+		/*
+		 * If the driver doesn't have a RX-callback then only free
+		 * the DMA-descriptors
+		 */
+		if (driver->dma_rx_isr) {
+			buffer.data = phys_to_virt(desc->src);
+			buffer.length = desc->length;
+			pic->driver->dma_rx_isr(driver, pic->irq, &buffer);
+		}
+		
+		/* And free the DMA-descriptor for a next transfer */
+		desc->length = pic->dma_cfg.rxsz;
+		desc->control &= ~(IOHUB_DMA_DESC_CTRL_FULL |
+				   IOHUB_DMA_DESC_CTRL_LAST);
+		desc = fim_dma_get_next(fifo, desc);
+	}
+
+	/* Update the buffer index for the next tasklet */
+	fifo->dma_first = desc;
+
+	/* Check for previously detected errors */
+	if (fifo->rx_error) {
+
+		if (pic->driver->dma_error_isr)
+			pic->driver->dma_error_isr(pic->driver, fifo->rx_error, 0);
+		
+		/* fim_dma_reset_fifo(fifo); */
+		fifo->rx_error = 0;
+	}
+	
+	spin_unlock(&pic->rx_lock);
+}
+
+
+
+inline static struct pic_t *get_pic_by_index(int index)
+{
+	if (index < FIM_MIN_PIC_INDEX || index > FIM_MAX_PIC_INDEX)
+		return NULL;
+
+	
+	return &the_fims->pics[index];
+}
+
+
+static struct pic_t *get_pic_from_driver(struct fim_driver *driver)
+{
+	if (!driver)
+		return NULL;
+
+	return get_pic_by_index(driver->picnr);
+}
+
+
+static int pic_get_ctrl_reg(struct pic_t *pic, int reg, unsigned int *val)
+{
+	if (NS92XX_FIM_CTRL_REG_CHECK(reg))
+		return -EINVAL;
+	
+	*val = readl(pic->reg_addr + NS92XX_FIM_CTRL_REG(reg));
+	return 0;
+}
+
+
+static void pic_set_ctrl_reg(struct pic_t *pic, int reg, unsigned int val)
+{
+	if (NS92XX_FIM_CTRL_REG_CHECK(reg))
+		return;
+	
+	writel(val, pic->reg_addr + NS92XX_FIM_CTRL_REG(reg));
+}
+
+
+static int pic_is_running(struct pic_t *pic)
+{
+	unsigned int regval;
+	
+	regval = readl(pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+	if (regval & NS92XX_FIM_GEN_CTRL_PROGMEM)
+		return 1;
+	else
+		return 0;
+}
+
+int fim_is_running(struct fim_driver *driver)
+{
+	struct pic_t *pic;
+	
+	if (!(pic = get_pic_from_driver(driver)))
+		return -ENODEV;
+
+	return pic_is_running(pic);
+}
+
+
+static void fim_pic_release(struct device *dev)
+{
+	/* @TODO: Nothing to do here? */
+}
+
+
+/*
+ * Returns the number of available bytes that can be theoretically requested
+ * for a TX-transfer with more than one buffer descriptor
+ */
+int fim_tx_buffers_room(struct fim_driver *driver)
+{
+	int retval, cnt;
+	struct pic_dma_desc_t *pic_desc;
+	struct pic_t *pic;
+	
+	if (!(pic = get_pic_by_index(driver->picnr)))
+		return -ENODEV;
+
+	retval = 0;
+	for (cnt=0; cnt < pic->tx_fifo.length; cnt++) {
+		pic_desc = pic->tx_desc + cnt;
+		if (atomic_read(&pic_desc->tasked))
+			continue;
+
+		retval += pic_desc->length;
+	}
+
+	return retval;
+}
+
+
+
+int fim_tx_buffers_level(struct fim_driver *driver)
+{
+	int retval, cnt;
+	struct pic_dma_desc_t *pic_desc;
+	struct pic_t *pic;
+
+	if (!(pic = get_pic_by_index(driver->picnr)))
+		return -ENODEV;
+
+	retval = 0;
+	for (cnt=0; cnt < pic->tx_fifo.length; cnt++) {
+		pic_desc = pic->tx_desc + cnt;
+		if (atomic_read(&pic_desc->tasked))
+			retval += pic_desc->length;
+	}
+
+	return retval;
+}
+
+
+int fim_number_pics(void)
+{
+	return FIM_NR_PICS;
+}
+
+
+
+/*
+ * Allocate a new FIM-buffer
+ * @TODO: Bind this buffer to an internal list of the driver, so that we can free
+ * all the buffers if the driver is being unloaded
+ */
+struct fim_buffer_t *fim_alloc_buffer(struct fim_driver *driver, int length,
+				      unsigned int gfp_flags)
+{
+	struct fim_buffer_t *retval;
+
+	if (!driver || length < 0)
+		return NULL;
+	
+	if (!(retval = kmalloc(sizeof(struct fim_buffer_t) + length , gfp_flags)))
+		return NULL;
+
+	retval->sent = 0;
+	retval->length = length;
+	retval->data = (void *)retval + sizeof(struct fim_buffer_t);
+	retval->private = NULL;
+	return retval;
+}
+
+
+/*
+ * Free an already requested FIM-buffer
+ * @TODO: Use the internal list for removing this buffer
+ */
+void fim_free_buffer(struct fim_driver *driver, struct fim_buffer_t *buffer)
+{
+	if (!driver || !buffer)
+		return;
+	
+	kfree(buffer);	
+}
+
+
+/*
+ * Be sure that you protect this function correctly.
+ * Use the spinlock tx_lock for this purpose.
+ */
+inline static void pic_reset_tx_fifo(struct pic_t *pic)
+{
+	int cnt;
+	struct iohub_dma_desc_t *desc;
+	struct pic_dma_desc_t *pic_desc;
+
+	fim_dma_reset_fifo(&pic->tx_fifo);
+	for(cnt=0; cnt < pic->tx_fifo.length; cnt++) {
+		desc = fim_dma_get_by_index(&pic->tx_fifo, cnt);
+		desc->control &= ~IOHUB_DMA_DESC_CTRL_FULL;
+		desc->length = 0;
+		pic_desc = pic->tx_desc + cnt;
+		atomic_set(&pic_desc->tasked, 0);
+	}
+}
+
+
+/*
+ * Check if there are enough buffers for the requested memory size
+ * In success then will try to restart the DMA-channel
+ * The return value is different then ZERO by errors.
+ * This function can be called from an interrupt context too.
+ */
+int fim_send_buffer(struct fim_driver *driver, const struct fim_buffer_t *bufdesc)
+{
+	struct pic_t *pic;
+	int cnt, len, last_len, pos;
+	struct pic_dma_desc_t *pic_desc;
+	struct pic_dma_desc_t *pic_descs[IOHUB_MAX_DMA_BUFFERS] = { NULL }; /* @XXX */
+	struct iohub_dma_desc_t *desc;
+	int backup_len;
+	int count, retval;
+	unsigned char *buffer;
+	
+	if (!bufdesc || !driver || (bufdesc->length <= 0) || !bufdesc->data ||
+	    !bufdesc->private)
+		return -EINVAL;
+
+	if (!(pic = get_pic_from_driver(driver)))
+		return -ENODEV;
+
+	/*
+	 * Lock this section so that the PIC can be unregistered now
+	 */
+	spin_lock(&pic->tx_lock);
+	if (!pic->driver) {
+		retval = -ENODEV;
+		goto exit_ret;
+	}
+
+	/* Check for the available buffers */
+	len = count = bufdesc->length;
+	buffer = bufdesc->data;
+	pos = 0;
+	for (cnt=0; cnt < pic->tx_fifo.length; cnt++) {
+		pic_desc = pic->tx_desc + cnt;
+		if (atomic_read(&pic_desc->tasked))
+			continue;
+
+		atomic_set(&pic_desc->tasked, 1);
+		pic_descs[pos++] = pic_desc;
+		
+		last_len = len;
+		len -= (int)pic_desc->length;
+		if (len <= 0) {
+			/* Try to get the correct number of FIFO descriptors */
+			if(!(desc = fim_dma_alloc(&pic->tx_fifo, pos))) {
+				printk_err(" Couldn't get the FIFO descriptors.\n");
+				break;
+			}
+
+			/* Copy the transfer data to the corresponding DMA-buffers */
+			backup_len = pic_desc->length;
+			pic_desc->length = last_len;
+			for (cnt=0; pic_descs[cnt] && cnt < pos; cnt++) {
+				len = pic_descs[cnt]->length;
+				memcpy(phys_to_virt(pic_descs[cnt]->src), buffer, len);
+				desc->src = pic_descs[cnt]->src;
+				desc->reserved = 0;
+				desc->length = len;
+				buffer += len;
+				desc = fim_dma_get_next(&pic->tx_fifo, desc);
+			}
+			pic_desc->length = backup_len;
+
+			/*
+			 * Save the private data into the first PIC-DMA descriptor
+			 * This private data will be used inside the TX-callback function
+			 * @BUG: We must save the requested length for the TX-callback,
+			 * then the DMA-controller set the length to zero.
+			 */
+			pic_descs[0]->private = bufdesc->private;
+			pic_descs[0]->total_length = bufdesc->length;
+			fim_start_tx_dma(pic);
+			retval = 0;
+			goto exit_ret;
+		}
+	}
+
+	printk_debug("No free TX-buffers available (%i). Aborting.\n", bufdesc->length);
+	for (cnt=0; pic_descs[cnt] && cnt < pic->tx_fifo.length; cnt++)
+		atomic_dec(&pic_descs[cnt]->tasked);
+	retval = -ENOMEM;
+	
+ exit_ret:
+	spin_unlock(&pic->tx_lock);
+	return retval;
+}
+
+
+/* @TODO: Please test this function before using it */
+void fim_flush_rx(struct fim_driver *driver)
+{
+	unsigned int cnt;
+	struct pic_t *pic;
+	struct iohub_dma_desc_t *desc;
+	struct iohub_dma_fifo_t *fifo;
+	
+	if (!(pic = get_pic_by_index(driver->picnr)))
+		return;
+	
+	writel(0x00, pic->iohub_addr + IOHUB_RX_DMA_ICTRL_REG);
+	writel(IOHUB_RX_DMA_CTRL_CA, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+	for (cnt=0; cnt < pic->rx_fifo.length; cnt++) {
+		desc = fim_dma_get_by_index(&pic->rx_fifo, cnt);
+		desc->control = IOHUB_DMA_DESC_CTRL_INT;
+	}
+
+	writel(IOHUB_ICTRL_RXNCIE | IOHUB_ICTRL_RXNRIE | IOHUB_ICTRL_RXPCIE,
+	       pic->iohub_addr + IOHUB_RX_DMA_ICTRL_REG);
+	writel(pic->rx_fifo.phys_descs, pic->iohub_addr + IOHUB_RX_DMA_BUFPTR_REG);
+	writel(IOHUB_RX_DMA_CTRL_CE, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+
+	/* Reset the internal fifo too */
+	fifo = &pic->rx_fifo;
+	fim_dma_reset_fifo(fifo);
+}
+
+
+/*
+ * Drain the TX-buffers by first aborting the DMA-channel. The DMA-descriptors
+ * will be reseted inside the interrupt routine
+ */
+void fim_flush_tx(struct fim_driver *driver)
+{
+	struct pic_t *pic;
+	
+	if (!(pic = get_pic_by_index(driver->picnr)))
+		return;
+
+	pic_reset_tx_fifo(pic);
+	
+	writel(IOHUB_RX_DMA_CTRL_CA, pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+}
+
+
+
+/*
+ * This function is responsible for restarting the DMA-channel with the correct index
+ * If the DMA-channel is transferring data it will returns inmediately, but the
+ * callback of the DMA-TX will recall this function and send the requested data buffers
+ */
+static int fim_start_tx_dma(struct pic_t *pic)
+{
+	unsigned long txctrl;
+	struct iohub_dma_desc_t *desc = NULL;
+	struct iohub_dma_fifo_t *fifo;
+	int retval = 0;
+	unsigned int channel_index, fifo_index;
+	
+	if (fim_dma_is_empty(&pic->tx_fifo)) {
+		printk(KERN_DEBUG "Nothing to do, the TX-FIFO is empty.\n");
+		return 0;
+	}
+		
+	/* @XXX: Need to protect this section */
+	if (atomic_read(&pic->tx_tasked)) {
+		printk_debug("The DMA-Controller seems to be tasked.\n");
+		return 0;
+	}
+
+	/* Lock the next DMA-transfer and get the internal data */
+	atomic_set(&pic->tx_tasked, 1);
+	
+	spin_lock(&pic->tx_lock);
+	fifo = &pic->tx_fifo;
+	fifo->dma_last = fifo->dma_next;
+
+	/*
+	 * Write the control bit fields for the new TX-transfer
+	 * The close-buffer interrupt will be enabled only for the last descriptor!
+	 */
+	for (desc=fifo->dma_first; ; desc=fim_dma_get_next(fifo, desc)) {
+		if (desc->length <= 0) {
+			printk_err("Invalid FIFO descriptor 0x%p length, %u\n",
+			       desc, desc->length);
+			retval = -EINVAL;
+			goto exit_unlock;
+		}
+
+		/* IMPORTANT: Enable the interrupt only for the last buffer! */
+		desc->control = IOHUB_DMA_DESC_CTRL_FULL;
+		if (desc == fifo->last)
+			desc->control |= IOHUB_DMA_DESC_CTRL_WRAP;		
+		if (desc == fifo->dma_last) {
+			desc->control |= (IOHUB_DMA_DESC_CTRL_INT |
+					  IOHUB_DMA_DESC_CTRL_LAST);
+			break;
+		}
+	}
+		
+	/* Get the index of the first FIFO descriptor for the DMA-controller */
+	fifo_index = fim_dma_get_index(&pic->tx_fifo, fifo->dma_first);
+	txctrl = readl(pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+	txctrl |= IOHUB_TX_DMA_CTRL_INDEXEN | IOHUB_TX_DMA_CTRL_INDEX(fifo_index);
+	writel(txctrl, pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+
+	/*
+	 * If the TX-FIFO and the DMA-Channel have different index then we have
+	 * an error, then normally they must have the same value.
+	 * @FIXME: Workaround for this problem, or remove the paranoic sanity check
+	 */
+	channel_index = readl(pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG) & 0x3FF; 
+	if (fifo_index && (fifo_index != channel_index)) {
+		printk_warn("FIFO index 0x%X mismatch DMA index 0x%X\n",
+		       fifo_index, channel_index);
+	} else {
+		printk_debug("DMA index %i for next DMA buffer\n", channel_index);
+	}
+
+	/*
+	 * Check if an abort interrupt was executed in the time we have
+	 * configured the DMA-descriptors. In that skip the channel restart
+	 */
+	if (!atomic_read(&pic->tx_aborted)) {
+		writel(txctrl & ~IOHUB_TX_DMA_CTRL_CE,
+		       pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+		writel(txctrl | IOHUB_TX_DMA_CTRL_CE,
+		       pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+	} else
+		atomic_set(&pic->tx_tasked, 0);
+
+ exit_unlock:
+	spin_unlock(&pic->tx_lock);	
+	return retval;
+}
+
+
+
+
+/* DONT poll with this function. We need another function for this purpose. */
+int fim_get_exp_reg(struct fim_driver *driver, int nr, unsigned int *value)
+{
+	struct pic_t *pic;
+
+	if (!driver)
+		return -EINVAL;
+
+	if (!(pic = get_pic_by_index(driver->picnr)))
+		return -EINVAL;
+
+	*value = readl(pic->reg_addr + NS92XX_FIM_EXP_REG(nr));
+	
+	return 0;
+}
+
+
+
+/* Called when the PIC interrupts the ARM-processor */
+static void isr_from_pic(struct pic_t *pic, int irqnr)
+{
+	unsigned int status;
+	struct fim_driver *driver;
+	unsigned int rx_fifo;	
+	unsigned int timeout;
+	
+	status = readl(pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+	rx_fifo = readl(pic->iohub_addr + IOHUB_RX_DIR_FIFO_REG);
+	
+	driver = pic->driver;
+	if (driver && driver->fim_isr)
+		driver->fim_isr(driver, pic->irq, status & 0xFF, rx_fifo);
+
+	/* @TEST */
+	writel(status, pic->reg_addr + NS92XX_FIM_CTRL7_REG);
+
+	writel(status | NS92XX_FIM_GEN_CTRL_INTACKWR,
+	       pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+
+	timeout = 0xFFFF;
+	do {
+		timeout--;
+		status = readl(pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+	} while (timeout && (status & NS92XX_FIM_GEN_CTRL_INTFROMPIC));
+
+	/* @XXX: Should we stop the PIC for avoiding more timeout errors? */
+	if (!timeout) {
+		printk_err("FIM %i interrupt handshake timeout.\n", pic->index);
+		return;
+	}
+	
+	writel(status & ~NS92XX_FIM_GEN_CTRL_INTACKWR, pic->reg_addr +
+	       NS92XX_FIM_GEN_CTRL_REG);
+}
+
+
+
+static void isr_dma_tx(struct pic_t *pic, int irqnr)
+{
+	struct iohub_dma_fifo_t *fifo;
+	unsigned int ifs, txctrl, ictrl;
+	int cnt;
+	struct iohub_dma_desc_t *desc;
+	struct pic_dma_desc_t *pic_desc;
+	struct fim_driver *driver;
+	struct fim_buffer_t buffer= { 0, NULL, NULL };
+	
+	fifo = &pic->tx_fifo;
+	ifs = readl(pic->iohub_addr + IOHUB_IFS_REG);
+	if (ifs & IOHUB_IFS_TXNRIP) {
+		/*
+		 * From the Net+OS driver seems to be that the DMA controller is
+		 * setting the TXNRIP status bit although no error happened. But for
+		 * being sure that no error happened, we check the full control bit
+		 * of the last buffer descriptor: If this bit is high then the
+		 * last buffer was not transferred, ERROR!
+		 */
+		txctrl = readl(pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG) - 1;
+		desc = fifo->dma_last;
+		if(desc->control & IOHUB_DMA_DESC_CTRL_FULL) {
+			ictrl = readl(pic->iohub_addr + IOHUB_TX_ICTRL_REG);
+			printk_err("TXNRIP: ICTRL 0x%08x | CTRL 0x%08x | DESC 0x%p\n",
+			       ictrl, txctrl, desc);
+		}
+	}
+
+	/*
+	 * The channel abort is normally being called for flushing the TX-buffers.
+	 */
+	if (ifs & IOHUB_IFS_TXCAIP) {
+
+		atomic_set(&pic->tx_aborted, 1);
+		spin_lock(&pic->tx_lock);
+		printk_debug("TXCAIP: Freeing the TX-buffers\n");
+		pic_reset_tx_fifo(pic);
+
+		/* Reset the index of the TX-channel to zero */
+		writel(IOHUB_RX_DMA_CTRL_CA | IOHUB_TX_DMA_CTRL_INDEXEN,
+		       pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);		
+		writel(0x00, pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+
+		/* Reset the atomic variables for enabling the TX-requests */
+		atomic_set(&pic->tx_aborted, 0);
+		atomic_set(&pic->tx_tasked, 0);
+		spin_unlock(&pic->tx_lock);		
+		return;
+	}
+
+	
+	/*
+	 * If no NC interrupt pending then return immediately.
+	 * @FIXME: Is possible that a driver is waiting for the TXNCIP on this case?
+	 */
+	if(!(ifs & IOHUB_IFS_TXNCIP))
+		return;
+	
+	/* Now free the allocated PIC-DMA descriptors */
+	desc = fifo->dma_first;
+	driver = pic->driver;
+	do {
+		for(cnt=0; cnt < pic->tx_fifo.length; cnt++) {
+			pic_desc = pic->tx_desc + cnt;
+			if(pic_desc->src == desc->src) {
+				if(!buffer.data) {
+					buffer.data = phys_to_virt(desc->src);
+					buffer.private = pic_desc->private;
+				}
+
+				/*
+				 * @BUG: The DMA-channel seems to reset the buffer length
+				 * otherwise use: buffer.length += desc->length;
+				 */
+				buffer.length = pic_desc->total_length;
+				desc->src = 0;
+				atomic_set(&pic_desc->tasked, 0);
+				break;
+			}
+		}
+		desc = fim_dma_get_next(fifo, desc);
+	} while (desc != fim_dma_get_next(fifo, fifo->dma_last));
+
+
+	/* Now give the control to the driver's callback function */
+	if (driver->dma_tx_isr)
+		driver->dma_tx_isr(driver, pic->irq, &buffer);
+	
+	/* Free the DMA-controller */
+	fifo->dma_first = fim_dma_get_next(fifo, fifo->dma_last);
+	atomic_set(&pic->tx_tasked, 0);
+
+	/* Check if another descriptors are waiting in the FIFO */
+	if (fifo->dma_next != fifo->dma_last)
+		fim_start_tx_dma(pic);
+	
+}
+
+
+
+/* Only check that no errors ocurred by the last buffer descriptor */
+static void isr_dma_rx(struct pic_t *pic, int irqnr)
+{
+	unsigned int ifs;
+	struct iohub_dma_fifo_t *fifo;
+	unsigned int rxctrl, ictrl;
+	unsigned long error;
+	int verbose;
+
+	/* Get the selected verbosity */
+	verbose = (pic->driver) ? (pic->driver->verbose) : (0);
+	
+	/* If we have an error, then always restart the DMA-channel */
+	ifs = readl(pic->iohub_addr + IOHUB_IFS_REG);
+	error = 0;
+	
+	if (ifs & IOHUB_IFS_RXNRIP) {
+		ictrl = readl(pic->iohub_addr + IOHUB_RX_DMA_ICTRL_REG);
+		rxctrl = readl(pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+
+		if (verbose && printk_ratelimit())
+			printk_warn("RXNRIP: ictrl 0x%08x | rxctrl 0x%08x\n", ictrl, rxctrl);
+		error |= IOHUB_IFS_RXNRIP;
+	}
+
+	if (!(ifs & IOHUB_IFS_RXNCIP)) {
+
+		if (verbose && printk_ratelimit())
+			printk_warn("RXNCIP: Unexpected state (ifs: %x)\n", ifs);
+		error |= IOHUB_IFS_RXNCIP;
+	}
+
+	/* If we have a failure, then reset the DMA-controller and -FIFO */
+	if (error) {
+		fifo = &pic->rx_fifo;
+		writel(fifo->phys_descs, pic->iohub_addr + IOHUB_RX_DMA_BUFPTR_REG);
+		writel(0x00, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+		writel(ifs, pic->iohub_addr + IOHUB_IFS_REG);
+		writel(IOHUB_RX_DMA_CTRL_CE, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+
+		/* This is for the RX tasklet that will inform the FIM-driver about this failure */
+		fifo->rx_error = error;
+	}
+
+	/*
+	 * @IMPORTANT: The API of Net+OS is restarting the RX-channel after each
+	 * transfer, but we don't know exactly why. Since our API is working
+	 * without the restart, we don't touch the RX-channel.
+	 */
+#if 0
+	rxctrl = readl(pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+	rxctrl &= ~IOHUB_RX_DMA_CTRL_CE;
+	writel(rxctrl, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+	writel(rxctrl | IOHUB_RX_DMA_CTRL_CE, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+#endif
+	
+	/* Schedule the task which will free the RX-DMA buffer descriptors */
+	tasklet_hi_schedule(&pic->rx_tasklet);
+}
+
+
+/* This is the main ISR for the PIC-interrupts */
+static irqreturn_t pic_irq(int irq, void *tpic)
+{
+	unsigned int ifs;
+	struct pic_t *pic = (struct pic_t *)tpic;
+	unsigned long flags;
+
+	spin_lock_irqsave(&pic->lock, flags);
+	ifs = readl(pic->iohub_addr + IOHUB_IFS_REG);
+
+	if (ifs & IOHUB_IFS_MODIP)
+		isr_from_pic(pic, irq);
+	if (ifs & IOHUB_IFS_DMA_TX)
+		isr_dma_tx(pic, irq);
+	if (ifs & IOHUB_IFS_DMA_RX)
+		isr_dma_rx(pic, irq);
+	
+	writel(ifs, pic->iohub_addr + IOHUB_IFS_REG);
+	spin_unlock_irqrestore(&pic->lock, flags);
+	return IRQ_HANDLED;
+}
+
+
+
+int fim_enable_irq(struct fim_driver *driver)
+{
+	struct pic_t *pic;
+	
+	if (!driver)
+		return -EINVAL;
+
+	if (!(pic = get_pic_from_driver(driver)))
+		return -EINVAL;
+
+	/* IRQ enable if required */
+	if (!atomic_read(&pic->irq_enabled)) {
+		printk_debug("Enabling the PIC IRQ %i\n", pic->irq);
+		enable_irq(pic->irq);
+		atomic_set(&pic->irq_enabled, 1);
+	}
+	return 0;
+}
+
+
+int fim_disable_irq(struct fim_driver *driver)
+{
+	struct pic_t *pic;
+	
+	if (!driver)
+		return -EINVAL;
+
+	if (!(pic = get_pic_from_driver(driver)))
+		return -EINVAL;
+
+	/* Disable the IRQ */
+	if (atomic_read(&pic->irq_enabled)) {
+		printk_debug("Disabling the PIC IRQ %i\n", pic->irq);
+		disable_irq(pic->irq);
+		atomic_set(&pic->irq_enabled, 0);
+	}
+	return 0;
+}
+
+/*
+ * Function for downloading the FIM-firmware
+ * IMPORTANT: This function will automatically stop the FIM if it's running,
+ * additionally it will (try) to restore the current state of the DMA-channels
+ */
+int fim_download_firmware(struct fim_driver *driver)
+{
+	struct pic_t *pic;
+	int retval;
+	const struct firmware *fw;
+	const unsigned char *fwbuf;
+	unsigned int txctrl, rxctrl;
+	
+	if (!(pic = get_pic_from_driver(driver)))
+		return -EINVAL;
+
+	/* Stop the FIM first */
+	if (pic_is_running(pic)) {
+		retval = pic_stop_and_reset(pic);
+		if (retval) {
+			printk_err("Couldn't stop the PIC %i\n", pic->index);
+			return retval;
+		}
+	}
+
+	/*
+	 * Now download the firmware using the firmware-subsystem or from
+	 * the passed array with the firmware code
+	 */
+	if (driver->fw_name) {
+		printk_debug("Requesting the firmware '%s'\n", driver->fw_name);
+		snprintf(pic->fw_name, FIM_MAX_FIRMWARE_NAME, "%s", driver->fw_name);
+		retval = request_firmware(&fw, driver->fw_name, pic->dev);
+		if (retval) {
+			printk_err("request_firmware() failed, %i\n", retval);
+			goto exit_download;
+		}
+		fwbuf = fw->data;
+	} else if (driver->fw_code) {
+		printk_debug("Using the built-in firmware code\n");
+		fwbuf = driver->fw_code;
+	} else {
+		printk_err("No code and no firmware name passed? Aborting\n");
+		retval = -EINVAL;
+		goto exit_download;
+	}
+
+	/*
+	 * Since the firmware download should be executed on the fly, disable
+	 * the DMA-channels first, otherwise the channels will run like a
+	 * spree killer
+	 */
+	spin_lock(&pic->tx_lock);
+	txctrl = readl(pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+	rxctrl = readl(pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+	writel(txctrl & ~IOHUB_TX_DMA_CTRL_CE, pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+	writel(rxctrl & ~IOHUB_RX_DMA_CTRL_CE, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+	retval = pic_download_firmware(pic, fwbuf);
+	writel(txctrl, pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+	writel(rxctrl, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+	spin_unlock(&pic->tx_lock);
+	
+	/* Release the obtained data from the firmware-subsystem */
+	if (driver->fw_name)
+		release_firmware(fw);
+
+	if (retval)
+		printk_err("Firmware install in the PIC %i failed.\n", pic->index);
+
+ exit_download:
+	return retval;
+}
+
+
+int fim_register_driver(struct fim_driver *driver)
+{
+	struct pic_t *pic;
+	int retval;
+	const struct firmware *fw;
+	const unsigned char *fwbuf;
+
+	/* Sanity checks */
+	if (!driver || !driver->driver.name)
+		return -EINVAL;
+
+	if (!(pic = get_pic_by_index(driver->picnr)))
+		return -ENODEV;
+
+	if (pic->driver || PIC_IS_LOCKED(pic)) {
+		printk_err("PIC %i already requested.\n", pic->index);
+		return -EBUSY;
+	}
+	
+        /* Lock the PIC request */
+	PIC_LOCK_REQUEST(pic);
+
+	/* Stop the PIC before any other action */
+	retval = pic_stop_and_reset(pic);
+	if (retval) {
+		printk_err("Couldn't stop and reset the FIM.\n");
+		goto exit_free_pic;
+	}
+
+	/* Try to get the firmware from the user space */
+	if (driver->fw_name) {
+		printk_debug("Requesting the firmware '%s'\n", driver->fw_name);
+		snprintf(pic->fw_name, FIM_MAX_FIRMWARE_NAME, "%s", driver->fw_name);
+		retval = request_firmware(&fw, driver->fw_name, pic->dev);
+		if (retval) {
+			printk_err("request_firmware() failed, %i\n", retval);
+			goto exit_free_pic;
+		}
+		fwbuf = fw->data;
+	} else if (driver->fw_code) {
+		printk_debug("Using the built-in firmware code\n");
+		fwbuf = driver->fw_code;
+	} else {
+		printk_err("%s: Neither code nor firmware passed. Aborting request.\n",
+			   driver->driver.name);
+		goto exit_free_pic;
+	}
+
+	retval = pic_download_firmware(pic, fwbuf);
+
+	/* Release the obtained data from the firmware-subsystem */
+	if (driver->fw_name)
+		release_firmware(fw);
+
+	if (retval) {
+		printk_err("%s: Firmware install by the FIM%i failed.\n",
+			   driver->driver.name, pic->index);
+		goto exit_free_pic;
+	}
+
+	/* Start the PIC at zero */
+	retval = pic_start_at_zero(pic);
+	if (retval) {
+		printk_err("%s: FIM%i couldn't be started at zero.\n",
+			   driver->driver.name, pic->index);
+		goto exit_free_pic;
+	}
+
+	/*
+	 * The DMA channels will be controlled by the CPU. Pass the driver-config
+	 * values to the init-function
+	 */
+	retval = pic_dma_init(pic, driver->dma_cfg);
+	if (retval) {
+		printk_err("%s: DMA channel init failed for the FIM%i\n",
+			   driver->driver.name, pic->index);
+		goto goto_stop_pic;
+	}
+	
+	/*
+	 * Enable the tasklet before requesting the IRQ, then in some cases the
+	 * FIM sends an interrupt before its initialization through the FIM-driver.
+	 */
+	pic->driver = driver;
+	driver->dev = pic->dev;
+	tasklet_init(&pic->rx_tasklet, pic_rx_tasklet_func, (unsigned long)pic);
+
+	/* Request the IRQ for this FIM */
+	retval = request_irq(pic->irq, pic_irq, 0, driver->driver.name, pic);
+	if (retval) {
+		printk_err("%s: Couldn't request the IRQ %i\n",
+			   driver->driver.name, pic->irq);
+		goto exit_kill_tasklet;
+	}
+
+	/* Disable the IRQ and done */
+	disable_irq(pic->irq);
+	atomic_set(&pic->irq_enabled, 0);
+	printk_info("FIM%i registered for driver '%s' [IRQ %i]\n",
+		    pic->index, driver->driver.name, pic->irq);
+	
+	return 0;
+
+ exit_kill_tasklet:
+	tasklet_kill(&pic->rx_tasklet);
+	
+ goto_stop_pic:
+	pic_stop_and_reset(pic);
+
+ exit_free_pic:
+	PIC_FREE_REQUEST(pic);
+	
+	return retval;
+}
+
+
+int fim_unregister_driver(struct fim_driver *driver)
+{
+	int ret;
+	struct pic_t *pic;
+	
+	if (!driver)
+		return -EINVAL;
+
+	if (!(pic = get_pic_from_driver(driver)))
+		return -EINVAL;
+
+	if (!pic->driver || !PIC_IS_LOCKED(pic))
+		return -EINVAL;
+
+	/* Stop the PIC but wait for another running operations first */
+	spin_lock(&pic->tx_lock);
+	ret = pic_stop_and_reset(pic);
+	if (ret)
+		printk_err("Couldn't stop the PIC %i\n", pic->index);
+	
+	fim_disable_irq(pic->driver);
+
+	/* Free the requested IRQ */
+	printk_debug("Freeing the IRQ %i\n", pic->irq);
+	free_irq(pic->irq, pic);
+
+	/* Free the DMA-resources */
+	tasklet_kill(&pic->rx_tasklet);
+	pic_dma_stop(pic);
+	
+	/* And free the driver field */
+	pic->driver = NULL;
+	PIC_FREE_REQUEST(pic);
+	spin_unlock(&pic->tx_lock);
+	return 0;
+}
+
+
+/*
+ * Return a PIC-pointer for low level operations
+ * The driver structure contains the number of the pointer
+ */
+struct pic_t *fim_request_pic(int picnr)
+{
+	struct pic_t *pic;
+
+	if (!(pic = get_pic_by_index(picnr)))
+		return NULL;
+
+	if (pic->driver || PIC_IS_LOCKED(pic))
+		return NULL;
+
+	PIC_LOCK_REQUEST(pic);
+	writel(0x00, pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+	printk_info("PIC %i successful requested\n", pic->index);
+	return pic;
+}
+
+
+/*
+ * Free the PIC that was requested with the above function fim_request_pic()
+ */
+void fim_free_pic(struct pic_t *pic)
+{
+	if (!pic)
+		return;
+
+	if (!(get_pic_by_index(pic->index)))
+		return;
+
+	PIC_FREE_REQUEST(pic);
+}
+	
+
+/*
+ * This function can be used for downloading a firmware to the PIC.
+ * Please note that this function will reset the complete IOHUB-module, included the
+ * DMA-configuration registers. That's important when the FIM-driver
+ * is using the index of the DMA-channel, then this will have the index zero after
+ * the download.
+ */
+static int pic_download_firmware(struct pic_t *pic, const unsigned char *buffer)
+{
+	int mode, ret;
+	unsigned int status;
+	struct fim_program_t *program = (struct fim_program_t *)buffer;
+	int offset;
+	
+	if (!program || !pic)
+		return -EINVAL;
+	
+	if (!(FORMAT_TYPE_VALID(program->format)))
+		return -EINVAL;
+
+	printk_debug("Starting the download of the PIC firmware.\n");
+	
+	/* Check if the PIC is running, before starting the firmware update */
+	if (pic_is_running(pic)) {
+		printk_err("The PIC %i is running. Aborting the download.\n",
+		       pic->index);
+		return -EBUSY;
+	}
+
+	/* Check if the firmware has the correct header */
+	if (!(PROCESSOR_TYPE_VALID(program->processor))) {
+		printk_err("Invalid processor type. Aborting firmware download.\n");
+		return -EINVAL;
+	}
+
+	/* Enable the clock to IO processor and reset the module */
+	status = readl(SYS_CLOCK);
+	writel(status | (1 << (pic->index + FIM0_SHIFT)), SYS_CLOCK);
+	status = readl(SYS_RESET);
+	writel(status & ~(1 << (pic->index + FIM0_SHIFT)), SYS_RESET);
+	writel(status | (1 << (pic->index + FIM0_SHIFT)), SYS_RESET);
+
+	/* Configure the output clock */
+	ret = pic_config_output_clock_divisor(pic, program);
+	if (ret) {
+		printk_err("Couldn't set the clock output divisor.\n");
+		return ret;
+	}
+
+	switch (program->hw_mode) {
+	case FIM_HW_ASSIST_MODE_NONE:
+		mode = 0x00;
+		break;
+	case FIM_HW_ASSIST_MODE_GENERIC:
+		mode = 0x01;
+		break;
+	case FIM_HW_ASSIST_MODE_CAN:
+		mode = 0x02;
+		break;
+	default:
+		printk_err("Invalid HWA mode %i\n", program->hw_mode);
+		return -EINVAL;
+	}
+
+	status = readl(pic->hwa_addr + NS92XX_FIM_HWA_GEN_CONF_REG);
+	writel(mode | status, pic->hwa_addr + NS92XX_FIM_HWA_GEN_CONF_REG);
+	
+	/* Update the HW assist config registers */
+	for (offset = 0; offset < FIM_NUM_HWA_CONF_REGS; offset++) {
+		status = program->hwa_cfg[offset];
+		writel(status, pic->hwa_addr + NS92XX_FIM_HWA_SIGNAL(offset));
+	}
+
+	/* Check for the maximal supported number of instructions */
+	if (program->length > FIM_NS9215_MAX_INSTRUCTIONS) {
+		printk_err("Invalid firmware length %i (too long)\n", program->length);
+		return -EINVAL;
+	}
+
+	/* Start programming the PIC (the program size is in 16bit-words) */
+	printk_debug("Starting to program the firmware (%i Bytes)\n",
+		     program->length);
+	for (offset = 0; offset < program->length; offset++)
+		writel(program->data[offset] & NS92XX_FIM_INSTRUCTION_MASK,
+		       pic->instr_addr + 4*offset);
+
+	/* Save the firmware length into the internal structure for the sysfs */
+	pic->fw_length = program->length;
+	return 0;
+}
+
+
+
+static int pic_start_at_zero(struct pic_t *pic)
+{
+	unsigned int regval;
+	
+	if (!pic)
+		return -EINVAL;
+	if (pic->index < 0)
+		return -ENODEV;
+
+	printk_debug("Starting the PIC %i at zero.\n", pic->index);
+	
+	regval = readl(pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+	writel(regval | NS92XX_FIM_GEN_CTRL_START_PIC, pic->reg_addr +
+	       NS92XX_FIM_GEN_CTRL_REG);
+
+	/* Check if the PIC is really running */
+	if (!pic_is_running(pic)) {
+		printk_err("Unable to start the PIC %i\n", pic->index);
+		return -EAGAIN;
+	}
+	
+	return 0;
+}
+
+
+static int pic_stop_and_reset(struct pic_t *pic)
+{
+	unsigned int regval;
+	int retval;
+	
+	if (!pic)
+		return -EINVAL;
+
+	if (pic_is_running(pic))
+		printk_debug("The PIC %i is running, need to stop it.\n", pic->index);
+	
+	regval = readl(pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+
+	/* Clear the interrupt flags too! */
+	regval &= ~NS92XX_FIM_INT_MASK(0xff);
+	
+	writel(regval & NS92XX_FIM_GEN_CTRL_STOP_PIC,
+	       pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+
+	retval = 0;
+	if (pic_is_running(pic)) {
+		printk_err("Unabled to stop the PIC %i\n", pic->index);
+		retval = -EAGAIN;
+	}
+
+	/* Reset the HWA generial register too */
+	writel(0x00, pic->hwa_addr + NS92XX_FIM_HWA_GEN_CONF_REG);
+	
+	return retval;
+}
+
+/*
+ * This function stops the FIM and check if the DMA-channel still has a buffer
+ * for the FIM.
+ */
+int fim_send_reset(struct fim_driver *driver)
+{
+	struct pic_t *pic;
+	int retval, cnt, fifo_reset;
+	struct iohub_dma_fifo_t *fifo;
+	struct iohub_dma_desc_t *desc;
+
+	if (!(pic = get_pic_by_index(driver->picnr)))
+		return -EINVAL;
+
+	retval = pic_stop_and_reset(pic);
+	if (retval)
+		return retval;
+
+	/*
+	 * For avoiding errors by the restart of the FIM, check if a DMA buffer
+	 * is waiting for being transmitted. In that case, reset the buffer and
+	 * reset the index of the DMA-controller.
+	 */
+	fifo = &pic->tx_fifo;
+	fifo_reset = 0;
+	for (desc = fifo->first, cnt = 0; cnt < fifo->length; cnt++) {
+
+		/*
+		 * If we are stopping the FIM but have some DMA-buffers pending, then
+		 * we need to reset the DMA-channel before the next restart.
+		 */
+		if (!fifo_reset && (desc->control & IOHUB_DMA_DESC_CTRL_FULL))
+			fifo_reset = 1;
+
+		desc->control = 0;
+		desc = fim_dma_get_next(fifo, desc);
+	}
+
+	if (fifo_reset) {
+		unsigned long txctrl;
+
+		printk_warn("Stopping FIM %i with a full DMA buffer!\n",
+			    pic->index);
+		fim_dma_reset_fifo(fifo);			
+		txctrl = IOHUB_TX_DMA_CTRL_INDEXEN | IOHUB_TX_DMA_CTRL_INDEX(0);
+		writel(txctrl, pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+	}
+
+	/*
+	 * Reset the RX-channel too.
+	 * IMPORTANT: Set the wrap control bit by the last DMA-buffer, otherwise
+	 * the FIFO will overflow!
+	 */
+	fifo = &pic->rx_fifo;
+	for (desc = fifo->first, cnt = 0; cnt < fifo->length; cnt++) {
+		desc->control = IOHUB_DMA_DESC_CTRL_INT;
+		desc = fim_dma_get_next(fifo, desc);
+	}
+	fifo->last->control |= IOHUB_DMA_DESC_CTRL_WRAP;
+	fim_dma_reset_fifo(fifo);
+
+	/* Free the internal resources */
+	atomic_set(&pic->tx_tasked, 0);
+	atomic_set(&pic->tx_aborted, 0);
+	
+	return retval;
+}
+
+
+int fim_send_start(struct fim_driver *driver)
+{
+	ulong reg;
+	
+	struct pic_t *pic;
+	if (!(pic = get_pic_by_index(driver->picnr)))
+		return -EINVAL;
+
+	/* Connect the clock to the FIM */
+	reg = readl(SYS_CLOCK);
+	writel(reg | (1 << (pic->index + FIM0_SHIFT)), SYS_CLOCK);
+
+	reg = readl(SYS_RESET);
+	writel(reg | (1 << (pic->index + FIM0_SHIFT)), SYS_RESET);
+	
+	return pic_start_at_zero(pic);
+}
+
+int fim_send_stop(struct fim_driver *driver)
+{
+	struct pic_t *pic;
+	int ret;
+	ulong reg;
+
+	if (!(pic = get_pic_by_index(driver->picnr)))
+		return -EINVAL;
+
+	reg = readl(pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+
+	/* Clear the interrupt flags too! */
+	reg &= ~NS92XX_FIM_INT_MASK(0xff);
+	
+	writel(reg & ~NS92XX_FIM_GEN_CTRL_START_PIC,
+	       pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+
+	ret = 0;
+	if (pic_is_running(pic)) {
+		printk_err("Unabled to stop the PIC %i\n", pic->index);
+		ret = -EAGAIN;
+	}
+
+	/* Disable the clock and reset the module */
+	reg = readl(SYS_CLOCK);
+	writel(reg & ~(1 << (pic->index + FIM0_SHIFT)), SYS_CLOCK);
+	reg = readl(SYS_RESET);
+	writel(reg & ~(1 << (pic->index + FIM0_SHIFT)), SYS_RESET);
+	
+	return ret;
+}
+
+int fim_send_interrupt2(struct fim_driver *driver, unsigned int code)
+{
+	struct pic_t *pic;
+	if (!driver || !(pic = get_pic_by_index(driver->picnr)))
+		return -EINVAL;
+
+	return pic_send_interrupt(pic, code);
+}
+
+
+/*
+ * This function provides the access to the control registers of the PICs
+ * reg : Number of the control register (from 0 to 15)
+ * val : Value to write into the control register
+ */
+void fim_set_ctrl_reg(struct fim_driver *driver, int reg, unsigned int val)
+{
+	struct pic_t *pic;
+
+	if (!(pic = get_pic_from_driver(driver)))
+		return;
+
+	writel(val, pic->reg_addr + NS92XX_FIM_CTRL_REG(reg));
+}
+
+
+/* Provides the read access to the control registers of the PICs */
+int fim_get_ctrl_reg(struct fim_driver *driver, int reg, unsigned int *val)
+{
+	struct pic_t *pic;
+	
+	if (!(pic = get_pic_from_driver(driver)) || !val)
+		return -EINVAL;
+
+	if (NS92XX_FIM_CTRL_REG_CHECK(reg))
+		return -EINVAL;
+	
+	*val = readl(pic->reg_addr + NS92XX_FIM_CTRL_REG(reg));
+	return 0;
+}
+
+
+
+int fim_get_stat_reg(struct fim_driver *driver, int reg, unsigned int *val)
+{
+	struct pic_t *pic;
+	
+	if (!(pic = get_pic_from_driver(driver)) || !val)
+		return -EINVAL;
+
+	if (NS92XX_FIM_STAT_REG_CHECK(reg))
+		return -EINVAL;
+	
+	*val = readl(pic->reg_addr + NS92XX_FIM_STAT_REG(reg));
+	return 0;
+}
+
+
+
+/* Interrupt the PIC sending the interrput with the number `code' */
+static int pic_send_interrupt(struct pic_t *pic, u32 code)
+{
+	unsigned int stopcnt;
+	u32 status;
+	
+	if (!pic || !code || (code & ~0x7f))
+		return -EINVAL;
+
+	if (!pic_is_running(pic)) {
+		printk_err("The PIC %i isn't running. Aborting the IRQ request.\n",
+			   pic->index);
+		return -EAGAIN;
+	}
+
+	code = NS92XX_FIM_INT_MASK(code);
+	status = readl(pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+	writel(status | code, pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+	
+	/* This loop is perhaps problematic, exit with a timeout */
+	stopcnt = 0xFFFF;
+	do {
+		status = readl(pic->reg_addr  + NS92XX_FIM_GEN_CTRL_REG);
+		stopcnt--;
+	} while (!(status & NS92XX_FIM_GEN_CTRL_INTACKRD) && stopcnt);
+
+	if (!stopcnt) {
+		printk_err("Timeout waiting for RDACK from the PIC %i\n", pic->index);
+		return -EINVAL;
+	}
+
+	/* Reset the interrupt bits for the PIC acknowledge */
+	status &= ~NS92XX_FIM_GEN_CTRL_INTTOPIC;
+	writel(status, pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+
+	stopcnt = 0xFFFF;
+	do {
+		status = readl(pic->reg_addr + NS92XX_FIM_GEN_CTRL_REG);
+		stopcnt--;
+	} while ((status & NS92XX_FIM_GEN_CTRL_INTACKRD) && stopcnt);
+
+	if (!stopcnt) {
+		printk_err("Timeout by the second RDACK from the PIC %i\n", pic->index);
+		return -EINVAL;
+	}
+	
+	return 0;
+}
+
+
+/* Set the HWA PIC clock (see PIC module specification, page 19) */
+static int pic_config_output_clock_divisor(struct pic_t *pic,
+					   struct fim_program_t *program)
+{
+	int div;
+	int clkd;
+	unsigned int val;
+	
+	if (!pic || !program) return -EINVAL;
+	
+	div = program->clkdiv;
+	switch (div) {
+	case FIM_CLK_DIV_2:
+		clkd = FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_2;
+		break;
+
+	case FIM_CLK_DIV_4:
+		clkd = FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_4;
+		break;
+
+	case FIM_CLK_DIV_8:
+		clkd = FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_8;
+		break;
+
+	case FIM_CLK_DIV_16:
+		clkd = FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_16;
+		break;
+
+	case FIM_CLK_DIV_32:
+		clkd = FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_32;
+		break;
+
+	case FIM_CLK_DIV_64:
+		clkd = FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_64;
+		break;
+
+	case FIM_CLK_DIV_128:
+		clkd = FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_128;
+		break;
+
+	case FIM_CLK_DIV_256:
+		clkd = FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_256;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	val = readl(pic->hwa_addr + NS92XX_FIM_HWA_GEN_CONF_REG);
+	writel(val | clkd, pic->hwa_addr + NS92XX_FIM_HWA_GEN_CONF_REG);
+	return 0;
+}
+
+
+static int fim_probe(struct device *dev)
+{
+	int i, cnt, retval;
+	struct pic_t *pic;
+	
+	if (!dev)
+		return -EINVAL;
+
+	printk_debug("Starting the FIM-probe of the device `%s'\n", dev->bus_id);
+
+	/* Use the get_pic_by_index function for verifying that we have a FIM attached */
+	pic = (struct pic_t *)dev->driver_data;
+	pic = get_pic_by_index(pic->index);
+	if (!pic) {
+		printk_err("Invalid PIC index %i\n", pic->index);
+		return -ENODEV;
+	}
+
+	i = pic->index;
+	pic->dev = dev;	
+	pic->reg_addr = ioremap(NS92XX_FIM_REG_BASE_PA + i*NS92XX_FIM_REG_OFFSET,
+				NS92XX_FIM_REG_SIZE);
+	pic->instr_addr = ioremap(NS92XX_FIM_INSTR_BASE_PA + i*NS92XX_FIM_INSTR_OFFSET,
+				  NS92XX_FIM_INSTR_SIZE);
+	pic->hwa_addr = ioremap(NS92XX_FIM_HWA_BASE_PA + i*NS92XX_FIM_HWA_OFFSET,
+				NS92XX_FIM_HWA_SIZE);
+	pic->iohub_addr = ioremap(NS92XX_FIM_IOHUB_BASE_PA + i*NS92XX_FIM_IOHUB_OFFSET,
+				  NS92XX_FIM_IOHUB_SIZE);
+
+	/* @FIXME: Unmap the already registered memory addresses */
+	if (!pic->reg_addr || !pic->instr_addr || !pic->hwa_addr || !pic->iohub_addr) {
+		printk_err("ioremap() failed by the PIC %i\n", pic->index);
+		retval = -EAGAIN;
+		goto exit_unmap;
+	}
+	
+	spin_lock_init(&pic->lock);
+	
+	/* Create the file attributes under the sysfs */
+	retval = 0;
+	for (cnt=0; cnt < ARRAY_SIZE(fim_sysfs_attrs); cnt++) {
+		retval = sysfs_create_bin_file(&dev->kobj, &fim_sysfs_attrs[cnt]);
+		if (retval) {
+			while (cnt)
+				sysfs_remove_bin_file(&dev->kobj,
+						      &fim_sysfs_attrs[--cnt]);
+			goto exit_unmap;
+		}
+	}
+
+	/* Set the function pointers for providing low level access to the PICs */
+	pic->is_running = pic_is_running;
+	pic->start_at_zero = pic_start_at_zero;
+	pic->stop_and_reset = pic_stop_and_reset;
+	pic->download_firmware = pic_download_firmware;
+	pic->get_ctrl_reg = pic_get_ctrl_reg;
+	pic->set_ctrl_reg = pic_set_ctrl_reg;
+	pic->send_interrupt = pic_send_interrupt;
+	pic->ack_interrupt = isr_from_pic;
+	return 0;
+	
+ exit_unmap:
+	if (pic->reg_addr) iounmap(pic->reg_addr);
+	if (pic->instr_addr) iounmap(pic->instr_addr);
+	if (pic->hwa_addr) iounmap(pic->hwa_addr);
+	if (pic->iohub_addr) iounmap(pic->iohub_addr);
+	pic->reg_addr = pic->instr_addr = pic->hwa_addr = pic->iohub_addr = NULL;
+	
+	
+	return retval;
+}
+
+
+/* This function will be called for each PIC-device (but not for the parent device) */
+static int fim_remove(struct device *dev)
+{
+	int retval, cnt;
+	struct pic_t *pic = (struct pic_t *)dev->driver_data;
+	if (!pic)
+		return -EINVAL;
+
+	printk_info("Starting to remove the PIC %s\n", dev->bus_id);
+	
+	if (pic->driver) {
+		retval = fim_unregister_driver(pic->driver);
+		if (retval) {
+			printk_err("Can unregister the PIC %i, %i\n", pic->index,
+				   retval);
+		}
+		pic->driver = NULL;
+	}
+
+	/* Remove the created sysfs attributes */
+	for (cnt=0; cnt < ARRAY_SIZE(fim_sysfs_attrs); cnt++)
+		sysfs_remove_bin_file(&dev->kobj, &fim_sysfs_attrs[cnt]);
+	
+	/* Free the mapped pages of the IO memory */
+	if (pic->reg_addr) iounmap(pic->reg_addr);
+	if (pic->instr_addr) iounmap(pic->instr_addr);
+	if (pic->hwa_addr) iounmap(pic->hwa_addr);
+	if (pic->iohub_addr) iounmap(pic->iohub_addr);		
+	pic->dev = NULL;
+	return 0;
+}
+
+
+/* This function returns a non-zero value if the device correspond to our fim-bus */
+static int fim_bus_match(struct device *dev, struct device_driver *driver)
+{
+	printk_debug("@TODO: Create a match mechanism for the FIMs.\n");
+	return 1;
+}
+
+
+struct bus_type fim_bus_type = {
+	.name = FIM_BUS_TYPE_NAME,
+	.match = fim_bus_match,
+};
+
+
+/*
+ * Bus parent that will be registered without calling the probe function
+ * the sys file is under: /sys/devices/fims/
+ */
+struct device fim_bus_dev = {
+	.bus_id = "fims",
+	.release = fim_pic_release
+};
+
+
+static struct device_driver fims_driver = {
+	.probe =  fim_probe,
+	.remove = fim_remove,
+	.name = FIM_DRIVER_NAME,
+	.owner = THIS_MODULE,
+	.bus = &fim_bus_type,
+};
+
+
+
+/*
+ * These are the two PIC devices that we currently have in the NS9215
+ * With the help of the `bus type' the probe function will be called when
+ * the devices are registered
+ * Sys path: /sys/bus/fim-bus/devices/fim[01..]
+ */
+static struct device fim_pics[] = {
+	{
+		.bus_id = "fim0",
+		.release = fim_pic_release,
+		.bus = &fim_bus_type,
+		.parent = &fim_bus_dev,
+	},
+	{
+		.bus_id = "fim1",
+		.release = fim_pic_release,
+		.bus = &fim_bus_type,
+		.parent = &fim_bus_dev,
+	}
+};
+
+
+/*
+ * If no configuration for the DMA-buffer descriptors is passed (cfg = NULL),
+ * then use the default values. These are defined under the header file 'fim.h'
+ */
+inline static int pic_dma_check_config(struct pic_t *pic, struct fim_dma_cfg_t *cfg)
+{
+	if (cfg) {
+		if (cfg->rxnr <= 0 || cfg->rxnr > IOHUB_MAX_DMA_BUFFERS ||
+		    cfg->txnr <= 0 || cfg->txnr > IOHUB_MAX_DMA_BUFFERS) {
+			printk_err("Invalid number of RX/TX-DMA buffers (%i | %i)\n",
+				   cfg->rxnr, cfg->txnr);
+			return -EINVAL;
+		}
+
+		if (cfg->rxsz > IOHUB_MAX_DMA_LENGTH || cfg->rxsz <= 0 ||
+		    cfg->txsz > IOHUB_MAX_DMA_LENGTH || cfg->txsz <= 0) {
+			printk_err("Invalid DMA-buffer length (%i | %i)\n",
+				   cfg->rxsz, cfg->txsz);
+			return -EINVAL;
+		}
+	}
+
+	pic->dma_cfg.rxnr = (!cfg) ? PIC_DMA_RX_BUFFERS : cfg->rxnr;
+	pic->dma_cfg.txnr = (!cfg) ? PIC_DMA_TX_BUFFERS : cfg->txnr;
+	pic->dma_cfg.rxsz = (!cfg) ? PIC_DMA_BUFFER_SIZE : cfg->rxsz;
+	pic->dma_cfg.txsz = (!cfg) ? PIC_DMA_BUFFER_SIZE : cfg->txsz;
+	return 0;
+}
+
+
+/*
+ * This function starts the DMA-buffers and -fifos for the passed PIC
+ * rxs   : Number of RX-buffer descriptors to use
+ * txs   : Number of TX-buffer descriptors to create
+ * rxlen : Length of each RX-DMA buffer
+ * txlen : Length of each TX-DMA buffer
+ * @IMPORTANT: No sanity check will be executed in this function
+ */
+static int pic_dma_init(struct pic_t *pic, struct fim_dma_cfg_t *cfg)
+{
+	int retval;
+	struct iohub_dma_desc_t *desc;
+	struct pic_dma_desc_t *pic_desc;
+	int cnt;
+	dma_addr_t phys_buffers;
+	
+	if (!pic || !pic->dev)
+		return -EINVAL;
+
+	/* Sanity checks for the passed DMA-descriptors configuration */
+ 	if ((retval = pic_dma_check_config(pic, cfg)))
+		return retval;
+	
+	/* Get the DMA-memory for the RX- and TX-buffers */
+	cfg = &pic->dma_cfg;
+	pic->dma_size = (cfg->rxnr * cfg->rxsz) + (cfg->txnr * cfg->txsz);
+	pic->dma_virt = dma_alloc_coherent(NULL, pic->dma_size,
+					   &pic->dma_phys, GFP_KERNEL);
+	if (!pic->dma_virt)
+		return -ENOMEM;
+
+	/*
+	 * Get the DMA-memory for the RX-descriptors (FIFOs)
+	 */
+	pic->rx_fifo.descs = dma_alloc_coherent(NULL,
+						cfg->rxnr * IOHUB_DMA_DESC_LENGTH,
+						&pic->rx_fifo.phys_descs,
+						GFP_KERNEL);
+	if (!pic->rx_fifo.descs) {
+		retval = -ENOMEM;
+		goto free_dma_bufs;
+	}
+	
+	fim_dma_init_fifo(&pic->rx_fifo, cfg->rxnr, pic->rx_fifo.descs);
+	printk_debug("RX FIFO descriptors range is 0x%p to 0x%p [0x%08x]\n",
+		     pic->rx_fifo.first, pic->rx_fifo.last, pic->rx_fifo.phys_descs);
+
+	/* Setup the RX-descriptors with the corresponding DMA-buffers */
+	phys_buffers = pic->dma_phys;
+	desc = pic->rx_fifo.first;
+	do {
+		desc->length = cfg->rxsz;
+		desc->src = phys_buffers;
+		desc->control = IOHUB_DMA_DESC_CTRL_INT;
+		printk_debug("[ RX %p ] 0x%p (0x%08x) | 0x%08x | 0x%04x\n",
+			     desc, phys_to_virt(desc->src), desc->src, desc->length,
+			     desc->control);
+		phys_buffers += cfg->rxsz;
+		desc = fim_dma_get_next(&pic->rx_fifo, desc);
+	} while (desc != pic->rx_fifo.first);
+	pic->rx_fifo.last->control |= IOHUB_DMA_DESC_CTRL_WRAP;
+
+	/*
+	 * Now create the DMA-memory for the TX-buffer descriptors
+	 */
+	pic->tx_fifo.descs = dma_alloc_coherent(NULL,
+						cfg->txnr * IOHUB_DMA_DESC_LENGTH,
+						&pic->tx_fifo.phys_descs,
+						GFP_KERNEL);
+	if (!pic->tx_fifo.descs) {
+		retval = -ENOMEM;
+		goto free_dma_rxfifo;
+	}
+	
+	fim_dma_init_fifo(&pic->tx_fifo, cfg->txnr, pic->tx_fifo.descs);
+	printk_debug("TX FIFO descriptors range is 0x%p to 0x%p [0x%08x]\n",
+		     pic->tx_fifo.first, pic->tx_fifo.last, pic->tx_fifo.phys_descs);
+	if(!(pic->tx_desc = kzalloc(cfg->txsz *
+				    sizeof(struct pic_dma_desc_t), GFP_KERNEL))) {
+		printk_err("Couldn't create the TX-dma descriptors\n");
+		retval = -ENOMEM;
+		goto free_dma_rxfifo;
+	}
+	
+	/* Setup the TX-buffers and -descriptors */
+	desc = pic->tx_fifo.first;
+	cnt = 0;
+	do {
+		desc->length = cfg->txsz;
+		desc->src = phys_buffers;
+		desc->control = 0;
+		pic_desc = pic->tx_desc + cnt++;
+		pic_desc->src = phys_buffers;
+		pic_desc->length = cfg->txsz;
+		printk_debug("[ TX %p ] 0x%p (0x%08x) | 0x%08x | 0x%04x\n",
+			     desc, phys_to_virt(desc->src), desc->src, desc->length,
+			     desc->control);
+
+		phys_buffers += cfg->txsz;
+		desc = fim_dma_get_next(&pic->tx_fifo, desc);
+	} while (desc != pic->tx_fifo.first);
+
+	/* Set the last buffer descriptor with the WRAP bit */
+	pic->tx_fifo.last->control = IOHUB_DMA_DESC_CTRL_WRAP;
+
+	/*
+	 * Enable the interrupts for the TX-DMA channel and set the buf desc pointer
+	 * TXCAIP : used for flushing the TX-buffers
+	 */
+	spin_lock_init(&pic->tx_lock);
+	writel(0x00, pic->iohub_addr + IOHUB_TX_ICTRL_REG);
+	writel(IOHUB_TX_DMA_CTRL_CA, pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+	writel(0x00, pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+	writel(IOHUB_ICTRL_TXNCIE | IOHUB_ICTRL_TXNRIE | IOHUB_ICTRL_TXECIE |
+	       IOHUB_IFS_TXCAIP, pic->iohub_addr + IOHUB_TX_ICTRL_REG);
+	writel(pic->tx_fifo.phys_descs, pic->iohub_addr + IOHUB_TX_DMA_BUFPTR_REG);
+	
+	/* Enable the interrupts for the RX-DMA channel */
+	spin_lock_init(&pic->rx_lock);
+	writel(0x00, pic->iohub_addr + IOHUB_RX_DMA_ICTRL_REG);
+	writel(IOHUB_RX_DMA_CTRL_CA, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+	writel(IOHUB_ICTRL_RXNCIE | IOHUB_ICTRL_RXNRIE | IOHUB_ICTRL_RXPCIE,
+	       pic->iohub_addr + IOHUB_RX_DMA_ICTRL_REG);
+	writel(pic->rx_fifo.phys_descs, pic->iohub_addr + IOHUB_RX_DMA_BUFPTR_REG);
+	writel(IOHUB_RX_DMA_CTRL_CE, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+	
+	return 0;
+
+ free_dma_rxfifo:
+	dma_free_coherent(NULL, pic->rx_fifo.length, pic->rx_fifo.descs,
+			  pic->rx_fifo.phys_descs);
+	pic->rx_fifo.descs = NULL;
+	pic->rx_fifo.phys_descs = 0;
+	
+ free_dma_bufs:
+	dma_free_coherent(NULL, pic->dma_size, pic->dma_virt, pic->dma_phys);
+	pic->dma_virt = NULL;
+	pic->dma_phys = 0;
+	
+	return retval;
+}
+
+
+static void pic_dma_stop(struct pic_t *pic)
+{
+	int cnt;
+	struct pic_dma_desc_t *pic_desc;
+	
+	if (!pic || !pic->dev)
+		return;
+
+	printk_debug("Freeing the DMA resources of the PIC %i\n", pic->index);
+	
+	/* Disable all the interrupts and abort all the DMA-transfers */
+	writel(0x00, pic->iohub_addr + IOHUB_TX_ICTRL_REG);
+	writel(IOHUB_TX_DMA_CTRL_CA, pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+	writel(0x00, pic->iohub_addr + IOHUB_TX_DMA_CTRL_REG);
+
+	writel(0x00, pic->iohub_addr + IOHUB_RX_DMA_ICTRL_REG);
+	writel(IOHUB_RX_DMA_CTRL_CA, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+	writel(0x00, pic->iohub_addr + IOHUB_RX_DMA_CTRL_REG);
+	
+	/* Reset the DMA-channel data */
+	atomic_set(&pic->tx_tasked, 0);
+	for (cnt=0; cnt < pic->tx_fifo.length; cnt++) {
+		pic_desc = pic->tx_desc + cnt;
+		atomic_set(&pic_desc->tasked, 0);
+	}
+
+	/* Free the internal TX-descriptors */
+	if (pic->tx_desc) {
+		kfree(pic->tx_desc);
+		pic->tx_desc = NULL;
+	}
+	
+	/* Free the FIFOs */
+	if (pic->tx_fifo.descs && pic->tx_fifo.phys_descs) {
+		dma_free_coherent(NULL, pic->tx_fifo.length, pic->tx_fifo.descs,
+				  pic->tx_fifo.phys_descs);
+		pic->tx_fifo.descs = NULL;
+		pic->tx_fifo.phys_descs = 0;
+	}
+
+	if (pic->rx_fifo.descs && pic->rx_fifo.phys_descs) {
+		dma_free_coherent(NULL, pic->rx_fifo.length, pic->rx_fifo.descs,
+				  pic->rx_fifo.phys_descs);
+		pic->rx_fifo.descs = NULL;
+		pic->rx_fifo.phys_descs = 0;
+	}
+	
+	/* Free the DMA-buffers */
+	if (pic->dma_virt && pic->dma_phys) {
+		dma_free_coherent(NULL, pic->dma_size, pic->dma_virt, pic->dma_phys);
+		pic->dma_virt = NULL;
+		pic->dma_phys = 0;
+	}
+}
+
+int fim_dma_stop(struct fim_driver *fim)
+{
+	struct pic_t *pic;
+
+	if (!(pic = get_pic_from_driver(fim)))
+                return -ENODEV;
+	
+	pic_dma_stop(pic);
+	return 0;
+}
+
+int fim_dma_start(struct fim_driver *fim, struct fim_dma_cfg_t *cfg)
+{
+	struct pic_t *pic;
+
+	if (!(pic = get_pic_from_driver(fim)))
+                return -ENODEV;
+
+	/* Per default Use the already configured DMA configuration */
+	if (!cfg)
+		cfg = &pic->dma_cfg;
+	
+	return pic_dma_init(pic, cfg);
+}
+
+/*
+ * @TODO: We wanted originally to separate the PICs and the virtual devices
+ * initialization, but now we have a part of the PICs-init in this init-function
+ */
+static int __devinit fim_init_module(void)
+{
+	int ret, i;
+	struct pic_t *pic;	
+
+	printk_info("Starting to register the FIMs module.\n");
+	
+	if (the_fims)
+		return -EINVAL;
+	if (!(the_fims = kzalloc(sizeof(struct fims_t), GFP_KERNEL))) {
+		printk_err("kmalloc() failed.\n");
+		return -ENOMEM;
+	}
+
+	/* Register the internal bus type */
+	ret = bus_register(&fim_bus_type);
+	if (ret) {
+		printk_err("Registering the PICs bus, %i\n", ret);
+		goto goto_free_mem;
+	}
+
+	/* Register the bus parent */
+	ret = device_register(&fim_bus_dev);
+	if (ret) {
+		printk_err( "Couldn't register the parent PIC device, %i\n", ret);
+		goto goto_driver_unreg;
+	}
+
+	ret = driver_register(&fims_driver);
+	if (ret) {
+		printk_err("Couldn't register the FIMs driver, %i\n", ret);
+		goto goto_bus_unreg;
+	}
+
+	/* Start registering the PIC devices */
+	for (i=0; i <= FIM_MAX_PIC_INDEX; i++) {
+		pic = &the_fims->pics[i];
+		pic->index = i;
+		pic->irq = IRQ_NS921X_PIC0 + i;
+		fim_pics[i].driver_data = pic;
+		ret = device_register(&fim_pics[i]);
+		if (ret) {
+			printk_err("Registering the PIC device %i, %i\n", i, ret);
+			while(i) device_unregister(&fim_pics[--i]);
+			goto goto_parent_unreg;
+		}
+	}
+
+	the_fims->driver = &fims_driver;
+	the_fims->bus_dev = &fim_bus_dev;
+	the_fims->bus_type = &fim_bus_type;
+	printk(KERN_INFO DRIVER_DESC " " DRIVER_VERSION "\n");
+	return 0;
+
+ goto_parent_unreg:
+	device_unregister(&fim_bus_dev);
+	
+ goto_driver_unreg:
+	driver_unregister(&fims_driver);
+	
+ goto_bus_unreg:
+	bus_unregister(&fim_bus_type);
+	
+ goto_free_mem:
+	if(the_fims) kfree(the_fims);
+	
+	return ret;
+}
+
+
+static void __devexit fim_exit_module(void)
+{
+	int i;
+
+	printk_info("Unregistering the FIMs module.\n");
+	for (i=0; i <= FIM_MAX_PIC_INDEX; i++) {
+		device_unregister(&fim_pics[i]);
+	}
+
+	device_unregister(&fim_bus_dev);
+	driver_unregister(&fims_driver);
+
+	bus_unregister(the_fims->bus_type);
+	kfree(the_fims);
+	the_fims = NULL;
+}
+
+
+module_init(fim_init_module);
+module_exit(fim_exit_module);
+
+
+EXPORT_SYMBOL(fim_register_driver);
+EXPORT_SYMBOL(fim_unregister_driver);
+EXPORT_SYMBOL(fim_get_exp_reg);
+EXPORT_SYMBOL(fim_send_interrupt2);
+EXPORT_SYMBOL(fim_enable_irq);
+EXPORT_SYMBOL(fim_disable_irq);
+EXPORT_SYMBOL(fim_send_buffer);
+EXPORT_SYMBOL(fim_tx_buffers_room);
+EXPORT_SYMBOL(fim_tx_buffers_level);
+EXPORT_SYMBOL(fim_number_pics);
+EXPORT_SYMBOL(fim_send_reset);
+EXPORT_SYMBOL(fim_send_start);
+EXPORT_SYMBOL(fim_send_stop);
+EXPORT_SYMBOL(fim_flush_rx);
+EXPORT_SYMBOL(fim_flush_tx);
+EXPORT_SYMBOL(fim_alloc_buffer);
+EXPORT_SYMBOL(fim_free_buffer);
+EXPORT_SYMBOL(fim_set_ctrl_reg);
+EXPORT_SYMBOL(fim_get_ctrl_reg);
+EXPORT_SYMBOL(fim_get_stat_reg);
+EXPORT_SYMBOL(fim_request_pic);
+EXPORT_SYMBOL(fim_free_pic);
+EXPORT_SYMBOL(fim_download_firmware);
+EXPORT_SYMBOL(fim_is_running);
+EXPORT_SYMBOL(fim_dma_stop);
+EXPORT_SYMBOL(fim_dma_start);
diff --git a/drivers/fims/fim_reg.h b/drivers/fims/fim_reg.h
new file mode 100644
index 000000000000..b70d434150d3
--- /dev/null
+++ b/drivers/fims/fim_reg.h
@@ -0,0 +1,279 @@
+/*
+ * include/asm-arm/arch-ns9xxx/fim_reg.h
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ *  !Revision:   $Revision: 1.8 $
+ *  !Author:     Silvano Najera, Luis Galdos
+ *  !Descr:      
+ *  !References:
+ */
+
+
+#ifndef __FIM_REG_H
+#define __FIM_REG_H
+
+
+/* @FIXME: We must use the IOHUB base address*/
+#define NS92XX_FIM_IOHUB_BASE_PA		(0x90000000)
+#define NS92XX_FIM_IOHUB_SIZE			(0x1000)
+#define NS92XX_FIM_IOHUB_OFFSET			(0x8000)
+
+
+/* Base addresses of all memory regions */
+#define NS92XX_FIM_REG_BASE_PA			(0x90001000)
+#define NS92XX_FIM_REG_OFFSET			(0x8000)
+#define NS92XX_FIM_REG_SIZE                     (0x3000)
+
+#define NS92XX_FIM_INSTR_BASE_PA		(0x90004000)
+#define NS92XX_FIM_INSTR_OFFSET			(0x8000)
+#define NS92XX_FIM_INSTR_SIZE                   (0x5000)
+
+#define NS92XX_FIM_HWA_BASE_PA			(0x90068000)
+#define NS92XX_FIM_HWA_OFFSET			(0x8000)
+#define NS92XX_FIM_HWA_SIZE                     (0x8000)
+
+#define NS92XX_FIM_IO_SPACE_BASE_PA		(0x90078000)
+#define NS92XX_FIM_IO_SPACE_OFFSET		(0x8000)
+#define NS92XX_FIM_IO_SPACE_SIZE                (0x8000)
+
+
+/* FIM register offsets */
+#define NS92XX_FIM_GEN_CTRL_REG			(0x00)
+	
+#define NS92XX_FIM_CTRL0_REG			(0x10)
+#define NS92XX_FIM_CTRL1_REG			(0x14)
+#define NS92XX_FIM_CTRL2_REG			(0x18)
+#define NS92XX_FIM_CTRL3_REG			(0x1c)
+#define NS92XX_FIM_CTRL4_REG			(0x20)
+#define NS92XX_FIM_CTRL5_REG			(0x24)
+#define NS92XX_FIM_CTRL6_REG			(0x28)
+#define NS92XX_FIM_CTRL7_REG			(0x2c)
+#define NS92XX_FIM_CTRL8_REG			(0x30)
+#define NS92XX_FIM_CTRL9_REG			(0x34)
+#define NS92XX_FIM_CTRL10_REG			(0x38)
+#define NS92XX_FIM_CTRL11_REG			(0x3c)
+#define NS92XX_FIM_CTRL12_REG			(0x40)
+#define NS92XX_FIM_CTRL13_REG			(0x44)
+#define NS92XX_FIM_CTRL14_REG			(0x48)
+#define NS92XX_FIM_CTRL15_REG			(0x4c)
+#define NS92XX_FIM_CTRL_REG(i)			(NS92XX_FIM_CTRL0_REG + 4*i)
+#define NS92XX_FIM_CTRL_REG_CHECK(i)		((i<0 || i>15)?1:0)
+
+
+/*
+ * According to the HW-reference manual of the FIMs (page 16)
+ * The macros have as offset the address 0x90001000 (NS92XX_FIM_REG_BASE_PA)
+ */
+#define NS92XX_FIM_STAT0_REG			(0x50)
+#define NS92XX_FIM_STAT1_REG			(0x54)
+#define NS92XX_FIM_STAT2_REG			(0x58)
+#define NS92XX_FIM_STAT3_REG			(0x5c)
+#define NS92XX_FIM_STAT4_REG			(0x60)
+#define NS92XX_FIM_STAT5_REG			(0x64)
+#define NS92XX_FIM_STAT6_REG			(0x68)
+#define NS92XX_FIM_STAT7_REG			(0x6c)
+#define NS92XX_FIM_STAT8_REG			(0x70)
+#define NS92XX_FIM_STAT9_REG			(0x74)
+#define NS92XX_FIM_STAT10_REG			(0x78)
+#define NS92XX_FIM_STAT11_REG			(0x7c)
+#define NS92XX_FIM_STAT12_REG			(0x80)
+#define NS92XX_FIM_STAT13_REG			(0x84)
+#define NS92XX_FIM_STAT14_REG			(0x88)
+#define NS92XX_FIM_STAT_REG(i)			(NS92XX_FIM_STAT0_REG + 4*i)
+#define NS92XX_FIM_STAT_REG_CHECK(i)		((i<0 || i>14)?1:0)
+
+
+/*
+ * According to the HW-reference manual of the FIMs (page 16)
+ * The macros have as offset the address 0x90001000 (NS92XX_FIM_REG_BASE_PA)
+ */
+#define NS92XX_FIM_EXP0_REG			(0x50)
+#define NS92XX_FIM_EXP1_REG			(0x54)
+#define NS92XX_FIM_EXP2_REG			(0x58)
+#define NS92XX_FIM_EXP3_REG			(0x5c)
+#define NS92XX_FIM_EXP4_REG			(0x60)
+#define NS92XX_FIM_EXP5_REG			(0x64)
+#define NS92XX_FIM_EXP6_REG			(0x68)
+#define NS92XX_FIM_EXP7_REG			(0x6c)
+#define NS92XX_FIM_EXP8_REG			(0x70)
+#define NS92XX_FIM_EXP9_REG			(0x74)
+#define NS92XX_FIM_EXP10_REG			(0x78)
+#define NS92XX_FIM_EXP11_REG			(0x7c)
+#define NS92XX_FIM_EXP12_REG			(0x80)
+#define NS92XX_FIM_EXP13_REG			(0x84)
+#define NS92XX_FIM_EXP14_REG			(0x88)
+#define NS92XX_FIM_EXP15_REG			(0x8c)
+#define NS92XX_FIM_EXP_REG(i)			(NS92XX_FIM_EXP0_REG + 4*i)
+#define NS92XX_FIM_EXP_REG_CHECK(i)		((i<0 || i>15)?1:0)
+
+
+/* FIM intruction memory offset */
+#define NS92XX_FIM_INSTRUCTION_ADDR		(0x00)
+
+/* Hardware assist register offsets */
+#define NS92XX_FIM_HWA_GEN_CONF_REG		(0x00)
+
+#define NS92XX_FIM_HWA_SIGNAL0			(0x04)
+#define NS92XX_FIM_HWA_SIGNAL1			(0x08)
+#define NS92XX_FIM_HWA_SIGNAL2			(0x0c)
+#define NS92XX_FIM_HWA_SIGNAL3			(0x10)
+#define NS92XX_FIM_HWA_SIGNAL4			(0x14)
+#define NS92XX_FIM_HWA_SIGNAL5			(0x18)
+#define NS92XX_FIM_HWA_SIGNAL6			(0x1c)
+#define NS92XX_FIM_HWA_SIGNAL7			(0x20)
+#define NS92XX_FIM_HWA_CONTROL0			(0x24)
+#define NS92XX_FIM_HWA_CONTROL1			(0x28)
+#define NS92XX_FIM_HWA_CONTROL2			(0x2c)
+#define NS92XX_FIM_HWA_CONTROL3			(0x30)
+#define NS92XX_FIM_HWA_16_BIT_BUS		(0x34)
+#define NS92XX_FIM_HWA_32_BIT_BUS		(0x38)
+#define NS92XX_FIM_HWA_SIGNAL(index)		(NS92XX_FIM_HWA_GEN_CONF_REG + NS92XX_FIM_HWA_SIGNAL0 + 4*index)
+
+
+/* Fim register bit fields */
+#define NS92XX_FIM_GEN_CTRL_PROGMEM		(0x80000000)
+#define NS92XX_FIM_GEN_CTRL_RESERVED1		(0x7fff0000)
+#define NS92XX_FIM_GEN_CTRL_INTACKWR		(0x00008000)
+#define NS92XX_FIM_GEN_CTRL_INTTOPIC		(0x00007f00)
+#define NS92XX_FIM_GEN_CTRL_INTACKRD		(0x00000080)
+#define NS92XX_FIM_GEN_CTRL_INTFROMPIC		(0x0000007f)
+
+/* Mask used for the interrupt codes from the ARM to the PICs */
+#define NS92XX_FIM_INT_MASK(code)		(code<<8)
+
+
+/* FIM intructions bit fields*/
+#define NS92XX_FIM_INSTRUCTION_MASK		(0x00003fff)
+
+/* Hardware assist register bit fields */
+#define NS92XX_FIM_HWA_GEN_CONF_RESERVED1	(0xffffffe0)
+#define NS92XX_FIM_HWA_GEN_CONF_CLKSEL		(0x0000001c)
+#define NS92XX_FIM_HWA_GEN_CONF_ENABLE		(0x00000003)
+
+#define NS92XX_FIM_HWA_SIGNAL0_RESERVED		(0xfffffc00)
+#define NS92XX_FIM_HWA_SIGNAL0_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_SIGNAL0_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_SIGNAL0_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_SIGNAL0_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_SIGNAL0_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_SIGNAL1_RESERVED		(0xfffffc00)
+#define NS92XX_FIM_HWA_SIGNAL1_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_SIGNAL1_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_SIGNAL1_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_SIGNAL1_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_SIGNAL1_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_SIGNAL2_RESERVED		(0xfffffc00)
+#define NS92XX_FIM_HWA_SIGNAL2_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_SIGNAL2_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_SIGNAL2_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_SIGNAL2_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_SIGNAL2_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_SIGNAL3_RESERVED		(0xfffffc00)
+#define NS92XX_FIM_HWA_SIGNAL3_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_SIGNAL3_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_SIGNAL3_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_SIGNAL3_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_SIGNAL3_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_SIGNAL4_RESERVED		(0xfffffc00)
+#define NS92XX_FIM_HWA_SIGNAL4_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_SIGNAL4_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_SIGNAL4_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_SIGNAL4_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_SIGNAL4_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_SIGNAL5_RESERVED		(0xfffffc00)
+#define NS92XX_FIM_HWA_SIGNAL5_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_SIGNAL5_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_SIGNAL5_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_SIGNAL5_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_SIGNAL5_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_SIGNAL6_RESERVED		(0xfffffc00)
+#define NS92XX_FIM_HWA_SIGNAL6_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_SIGNAL6_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_SIGNAL6_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_SIGNAL6_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_SIGNAL6_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_SIGNAL7_RESERVED		(0xfffffc00)
+#define NS92XX_FIM_HWA_SIGNAL7_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_SIGNAL7_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_SIGNAL7_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_SIGNAL7_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_SIGNAL7_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_CONTROL0_RESERVED	(0xfffffc00)
+#define NS92XX_FIM_HWA_CONTROL0_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_CONTROL0_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_CONTROL0_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_CONTROL0_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_CONTROL0_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_CONTROL1_RESERVED	(0xfffffc00)
+#define NS92XX_FIM_HWA_CONTROL1_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_CONTROL1_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_CONTROL1_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_CONTROL1_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_CONTROL1_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_CONTROL2_RESERVED	(0xfffffc00)
+#define NS92XX_FIM_HWA_CONTROL2_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_CONTROL2_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_CONTROL2_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_CONTROL2_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_CONTROL2_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_CONTROL3_RESERVED	(0xfffffc00)
+#define NS92XX_FIM_HWA_CONTROL3_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_CONTROL3_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_CONTROL3_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_CONTROL3_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_CONTROL3_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_16_BIT_BUS_RESERVED	(0xfffffc00)
+#define NS92XX_FIM_HWA_16_BIT_BUS_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_16_BIT_BUS_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_16_BIT_BUS_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_16_BIT_BUS_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_16_BIT_BUS_CLKS		(0x00000003)
+
+#define NS92XX_FIM_HWA_32_BIT_BUS_RESERVED	(0xfffffc00)
+#define NS92XX_FIM_HWA_32_BIT_BUS_OMODE		(0x00000300)
+#define NS92XX_FIM_HWA_32_BIT_BUS_IMODE		(0x000000e0)
+#define NS92XX_FIM_HWA_32_BIT_BUS_CLKE		(0x00000010)
+#define NS92XX_FIM_HWA_32_BIT_BUS_CNTRL		(0x0000000c)
+#define NS92XX_FIM_HWA_32_BIT_BUS_CLKS		(0x00000003)
+
+
+
+/* H/W Assist General Configuration Register Codes */
+#define FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_2	(0)
+#define FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_4	(1<<2)
+#define FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_8	(2<<2)
+#define FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_16	(3<<2)
+#define FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_32	(4<<2)
+#define FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_64	(5<<2)
+#define FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_128	(6<<2)
+#define FIM_HWA_GEN_CFG_CLKSEL_DIVIDE_BY_256	(7<<2)
+
+
+
+#endif	/* __FIM_REG_H */
+
+
+
+
+
+
+
+
diff --git a/drivers/fims/sdio/Makefile b/drivers/fims/sdio/Makefile
new file mode 100644
index 000000000000..cd2f1149fde2
--- /dev/null
+++ b/drivers/fims/sdio/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for the kernel FIM device drivers
+#
+
+ifneq ($(CONFIG_FIM_SDIO_MODULE),)
+	obj-m					+= fim_sdio.o
+else
+	obj-$(CONFIG_FIM_ZERO_SDIO)		+= fim_sdio.o
+	obj-$(CONFIG_FIM_ONE_SDIO)		+= fim_sdio.o
+endif
diff --git a/drivers/fims/sdio/fim_sdio.c b/drivers/fims/sdio/fim_sdio.c
new file mode 100644
index 000000000000..3b618dbeea98
--- /dev/null
+++ b/drivers/fims/sdio/fim_sdio.c
@@ -0,0 +1,1616 @@
+/* -*- linux-c -*-
+ *
+ * drivers/fims/fim_sdio.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ *  !Revision:   $Revision: 1.20 $
+ *  !Author:     Luis Galdos
+ *  !Descr:      
+ *  !References:
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/slab.h>
+#include <linux/wait.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
+#include <linux/mmc/card.h>
+#include <linux/platform_device.h>
+#include <linux/wait.h>
+#include <linux/timer.h>
+#include <linux/scatterlist.h>
+#include <linux/mmc/sd.h>
+#include <linux/mmc/sdio.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/irq.h>
+
+#include <asm/scatterlist.h>
+#include <asm/uaccess.h>
+#include <asm/io.h>
+#include <mach/gpio.h>
+#include <mach/regs-sys-ns921x.h>
+#include <mach/regs-iohub-ns921x.h>
+#include <mach/regs-io-ns921x.h>
+#include <asm/delay.h>
+
+
+/* For registering the FIM-driver */
+#include <mach/fim-ns921x.h>
+
+
+/*
+ * If the driver is going to be loaded as a built-in driver, then include the header
+ * file with the firmware, otherwise set the name of the binary file that should
+ * be read with the help of the firmware-subsystem
+ */
+#if !defined(MODULE)
+# if defined(CONFIG_PROCESSOR_NS9215)
+#  include "fim_sdio0.h"
+#  include "fim_sdio1.h"
+# elif defined(CONFIG_PROCESSOR_NS9210)
+#  include "fim_sdio0_9210.h"
+#  include "fim_sdio1_9210.h"
+# else
+#  error "FIM-SDIO: Unsupported processor type."
+# endif /* CONFIG_PROCESSOR_NS9215 */
+extern const unsigned char fim_sdio_firmware[];
+#define FIM_SDIO_FW_FILE			(NULL)
+#define FIM_SDIO_FW_CODE0			fim_sdio_firmware0
+#define FIM_SDIO_FW_CODE1			fim_sdio_firmware1
+#else
+const unsigned char *fim_sdio_firmware = NULL;
+# if defined(CONFIG_PROCESSOR_NS9215)
+#  define FIM_SDIO_FW_FILE_PAT			"fim_sdio%i.bin"
+# elif defined(CONFIG_PROCESSOR_NS9210)
+#  define FIM_SDIO_FW_FILE_PAT			"fim_sdio%i_9210.bin"
+# else
+#  error "FIM-SDIO: Unsupported processor type."
+# endif /* CONFIG_PROCESSOR_NS9215 */
+#define FIM_SDIO_FW_LEN				sizeof(FIM_SDIO_FW_FILE_PAT) + 10
+#define FIM_SDIO_FW_CODE			(NULL)
+#endif
+
+/* Driver informations */
+#define DRIVER_VERSION				"0.2"
+#define DRIVER_AUTHOR				"Luis Galdos"
+#define DRIVER_DESC				"FIM SDIO driver"
+#define FIM_SDIO_DRIVER_NAME			"fim-sdio"
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRIVER_VERSION);
+
+/* Module parameter for selection the FIMs */
+NS921X_FIM_NUMBERS_PARAM(fims_number);
+
+/* Registers with status information */
+#define FIM_SDIO_GPIOS_REG			0x02
+#define FIM_SDIO_GPIOS_REG_CD			0x01
+#define FIM_SDIO_GPIOS_REG_WP			0x02
+#define FIM_SDIO_CARD_STATREG			0x00
+
+/* Interrupts from the FIM to the driver */
+#define FIM_SDIO_INTARM_CARD_DAT1			0x01
+#define FIM_SDIO_INTARM_CARD_DETECTED		0x02
+
+
+/* Macros for the SDIO-interface to the FIM-firmware */
+#define SDIO_HOST_TX_HDR			0x40
+#define SDIO_HOST_CMD_MASK			0x3f
+#define SDIO_FIFO_TX_48RSP			0x01
+#define	SDIO_FIFO_TX_136RSP			0x02
+#define SDIO_FIFO_TX_BW4			0x04
+#define SDIO_FIFO_TX_BLKWR			0x08
+#define SDIO_FIFO_TX_BLKRD			0x10
+#define SDIO_FIFO_TX_DISCRC			0x20
+
+/* User specified macros */
+#define FIM_SDIO_TIMEOUT_MS			500
+#define FIM_SDIO_TX_CMD_LEN			5
+#define FIM_SDIO_MAX_RESP_LENGTH			17
+
+/* Status bits from the PIC-firmware */
+#define FIM_SDIO_RX_RSP				0x01
+#define FIM_SDIO_RX_BLKRD				0x02
+#define FIM_SDIO_RX_TIMEOUT			0x04
+
+/*
+ * Firmware specific control registers
+ * IMPORTANT: The control bit for the card detect is enabled wih the main control!
+ */
+#define FIM_SDIO_CLKDIV_REG			0
+#define FIM_SDIO_INTCFG_REG			1
+#define FIM_SDIO_INTCFG_MAIN			(1 << 0)
+#define FIM_SDIO_INTCFG_CARD			(1 << 1)
+#define FIM_SDIO_INTCFG_SDIO			(1 << 2)
+#define FIM_SDIO_BLOCKS_REG                     2
+#define FIM_SDIO_BLKSZ_LSB_REG			3
+#define FIM_SDIO_BLKSZ_MSB_REG			4
+#define FIM_SDIO_MAIN_REG			5
+#define FIM_SDIO_MAIN_START			(1 << 0)
+
+/* Firmware specific status registers */
+#define FIM_SDIO_VERSION_SREG			0
+
+/* Internal flags for the request function */
+#define FIM_SDIO_REQUEST_NEW			0x00
+#define FIM_SDIO_REQUEST_CMD			0x01
+#define FIM_SDIO_REQUEST_STOP			0x02
+#define FIM_SDIO_SET_BUS_WIDTH			0x04
+
+/* Macros for the DMA-configuraton */
+#define FIM_SDIO_DMA_BUFFER_SIZE			PAGE_SIZE
+#define FIM_SDIO_DMA_RX_BUFFERS			21
+#define FIM_SDIO_DMA_TX_BUFFERS			10
+
+/* Used for the Card Detect timer */
+#define FIM_SDIO_CD_POLLING_TIMER			(HZ / 2)
+
+/*
+ * The below macro force the use of the timer for polling the state of the card
+ * detection line
+ */
+#if 0
+#define FIM_SDIO_FORCE_CD_POLLING
+#endif
+
+/* Enable the multipple block transfer (in development) */
+#if 0
+# define FIM_SDIO_MULTI_BLOCK
+# define FIM_SDIO_MAX_BLOCKS			(FIM_SDIO_DMA_RX_BUFFERS - 10)
+# define FIM_SDIO_SKIP_CRC_CHECK
+#else
+# define FIM_SDIO_MAX_BLOCKS			1
+#endif
+
+#define printk_err(fmt, args...)                printk(KERN_ERR "[ ERROR ] fim-sdio: " fmt, ## args)
+#define printk_info(fmt, args...)               printk(KERN_INFO "fim-sdio: " fmt, ## args)
+#define printk_dbg(fmt, args...)                printk(KERN_DEBUG "fim-sdio: " fmt, ## args)
+
+#if 0
+#define FIM_SDIO_DEBUG
+#define FIM_SDIO_DEBUG_CRC
+#endif
+
+#ifdef FIM_SDIO_DEBUG
+#  define printk_debug(fmt, args...)		printk(KERN_DEBUG "fim-sdio: %s() " fmt, __FUNCTION__ , ## args)
+#else
+#  define printk_debug(fmt, args...)
+#endif
+
+/* If enabled will generate an CRC error in the function that checks it */
+#if 0
+#define FIM_SDIO_FORCE_CRC
+#endif
+
+/*
+ * GPIO configuration
+ * Please note that the write protect GPIO must be under the index [4], then
+ * the driver will read its status (the firmware doesn't support it)
+ */
+#define FIM_SDIO_D0_GPIO			0
+#define FIM_SDIO_D1_GPIO			1
+#define FIM_SDIO_D2_GPIO			2
+#define FIM_SDIO_D3_GPIO			3
+#define FIM_SDIO_WP_GPIO			4
+#define FIM_SDIO_CD_GPIO			5
+#define FIM_SDIO_CLK_GPIO			6
+#define FIM_SDIO_CMD_GPIO			7
+#define FIM_SDIO_MAX_GPIOS			8
+
+/* Values for the block read state machine */
+enum fim_blkrd_state {
+	BLKRD_STATE_IDLE		= 0,
+	BLKRD_STATE_WAIT_ACK		= 1, /* Waiting for the block read ACK */
+	BLKRD_STATE_WAIT_DATA		= 2, /* Waiting for the block read data */
+	BLKRD_STATE_WAIT_CRC		= 3, /* Waiting for the CRC */
+	BLKRD_STATE_HAVE_DATA		= 4, /* Have block read data with the CRC */
+	BLKRD_STATE_TIMEOUTED		= 5, /* Timeout response from the PIC */
+	BLKRD_STATE_CRC_ERR		= 6, /* Compared CRC (PIC and card) differs */
+};
+
+
+/* Values for the command state machine */
+enum fim_cmd_state {
+	CMD_STATE_IDLE			= 0,
+	CMD_STATE_WAIT_ACK		= 1, /* Waiting for the response ACK */
+	CMD_STATE_WAIT_DATA		= 2, /* Waiting for the response data */
+	CMD_STATE_HAVE_RSP		= 3, /* Have response data */
+	CMD_STATE_TIMEOUTED		= 4, /* Timeout response from the PIC */
+	CMD_STATE_CRC_ERR		= 5, /* Compared CRC (PIC and card) differs */
+};
+
+/*
+ * Internal port structure for the FIM-SDIO host controller
+ * wp_gpio : GPIO to use for reading the write protect line
+ */
+struct fim_sdio_t {
+	struct fim_driver fim;
+	unsigned int flags;
+	struct device *device;
+	int index;
+
+	struct fim_gpio_t gpios[FIM_SDIO_MAX_GPIOS];
+	struct fim_buffer_t *buf;
+	struct mmc_command *mmc_cmd;
+	struct timer_list mmc_timer;
+	struct mmc_host *mmc;
+	struct mmc_request *mmc_req;
+
+	enum fim_cmd_state cmd_state;
+	enum fim_blkrd_state blkrd_state;
+
+	int trans_blocks;
+	int trans_sg;
+	int reg;
+
+	int wp_gpio;
+	struct clk *sys_clk;
+	int cd_gpio;
+	int cd_irq;
+	int cd_value;
+	struct timer_list cd_timer;
+
+	/* struct scatterlist *sg; */
+	u8 *sg_virt;
+
+	/* Members used for restarting the FIM */
+	atomic_t fim_is_down;
+	struct work_struct restart_work;
+};
+
+/*
+ * Transfer command structure for the card
+ * opctl : Control byte for the PIC
+ * blksz : Block size
+ * cmd   : Command to send to the card
+ */
+struct fim_sd_tx_cmd_t {
+	unsigned char opctl;
+	unsigned char blksz_msb;
+        unsigned char blksz_lsb;
+        unsigned char cmd[FIM_SDIO_TX_CMD_LEN];
+}__attribute__((__packed__));
+
+
+/*
+ * Response receive structure from the Card
+ * resp  : Card response, with a length of 5 or 17 as appropriate
+ * stat  : Opcode of the executed command
+ * crc   : CRC
+ */
+struct fim_sd_rx_resp_t {
+        unsigned char stat;
+        unsigned char resp[FIM_SDIO_MAX_RESP_LENGTH];
+        unsigned char crc;
+}__attribute__((__packed__));
+
+
+/* Main structure for the available ports */
+struct fim_sdios_t {
+	int fims;
+	struct fim_sdio_t *ports;
+};
+
+
+static struct fim_sdios_t *fim_sdios;
+
+/* Internal functions */
+static void fim_sd_process_next(struct fim_sdio_t *port);
+inline static struct fim_sdio_t *get_port_from_mmc(struct mmc_host *mmc);
+inline static void *fim_sd_dma_to_sg(struct fim_sdio_t *port, struct mmc_data *data,
+				     unsigned char *dma_buf, int dma_len);
+static void fim_sd_set_clock(struct fim_sdio_t *port, long int clockrate);
+static struct fim_buffer_t *fim_sd_alloc_cmd(struct fim_sdio_t *port);
+static int fim_sd_send_command(struct fim_sdio_t *port, struct mmc_command *cmd);
+
+
+/* Return the corresponding port structure */
+inline static struct fim_sdio_t *get_port_from_mmc(struct mmc_host *mmc)
+{
+	if (!mmc)
+		return NULL;
+
+	return (struct fim_sdio_t *)mmc->private[0];
+}
+
+/*
+ * This function is called when the FIM didn't respond to our requests. This normally
+ * happens when the card was unplugged during a data transfer or by another similar
+ * errors. So, for avoiding some additional failure we stop any further transfers to
+ * the FIM.
+ */
+static void fim_sd_cmd_timeout(unsigned long data)
+{
+	struct fim_sdio_t *port;
+
+	port = (struct fim_sdio_t *)data;
+
+	/* If the command pointer isn't NULL then a timeout has ocurred */
+	if (port->mmc_cmd) {
+		atomic_set(&port->fim_is_down, 1);
+		port->mmc_cmd->error = -ENOMEDIUM;
+		fim_sd_process_next(port);
+	}
+}
+
+/* Workqueue for restarting a FIM */
+static void fim_sd_restart_work_func(struct work_struct *work)
+{
+	struct fim_sdio_t *port;
+	struct fim_driver *fim;
+	int ret;
+       
+	port = container_of(work, struct fim_sdio_t, restart_work);
+	fim = &port->fim;
+
+	printk_dbg("Going to restart the FIM%i\n", fim->picnr);
+	fim_disable_irq(&port->fim);
+       
+	ret = fim_send_stop(&port->fim);
+	if (ret) {
+		printk_err("Couldn't stop the FIM%i\n", fim->picnr);
+		return;
+	}
+
+	ret = fim_download_firmware(&port->fim);
+	if (ret) {
+		printk_err("FIM%i download failed\n", fim->picnr);
+		return;
+	}
+
+        ret = fim_send_start(&port->fim);
+        if (ret) {
+		printk_err("FIM%i start failed\n", fim->picnr);
+		return;
+        }
+
+	/* Reset the internal values */
+	printk_dbg("Re-enabling the IRQ of FIM%u\n", fim->picnr);
+	atomic_set(&port->fim_is_down, 0);
+	fim_enable_irq(&port->fim);
+	port->mmc_cmd = NULL;
+	mmc_detect_change(port->mmc, msecs_to_jiffies(100));
+}
+
+/* Check the status of the card detect line if no external IRQ supported */
+static void fim_sd_cd_timer_func(unsigned long _port)
+{
+	struct fim_sdio_t *port;
+	int val;
+	
+	port = (struct fim_sdio_t *)_port;
+	val = gpio_get_value_ns921x(port->cd_gpio);
+
+	if (val != port->cd_value) {
+		port->cd_value = val;
+
+		if (atomic_read(&port->fim_is_down))
+			schedule_work(&port->restart_work);
+		else
+			mmc_detect_change(port->mmc, msecs_to_jiffies(100));
+	}
+
+	mod_timer(&port->cd_timer, jiffies + FIM_SDIO_CD_POLLING_TIMER);
+}
+
+/*
+ * Configure the trigger edge of the CD interrupt depending on the current
+ * state of the GPIO
+ */
+static inline int fim_sd_prepare_cd_irq(struct fim_sdio_t *port)
+{
+	int val;
+	unsigned int type;
+ 
+	val = gpio_get_value_ns921x(port->cd_gpio);
+	printk_debug("CD interrupt received (val %i)\n", val);
+
+	if (!val)
+		type = IRQF_TRIGGER_RISING;
+	else
+		type = IRQF_TRIGGER_FALLING;
+	
+	return set_irq_type(port->cd_irq, type);
+}
+
+/*
+ * The external interrupt line only supports one edge detection! That means, we must
+ * check the status of the line and reconfigure the interrupt trigger type.
+ */
+static irqreturn_t fim_sd_cd_irq(int irq, void *_port)
+{
+	struct fim_sdio_t *port;
+	int ret;
+
+	port = (struct fim_sdio_t *)_port;
+	if ((ret = fim_sd_prepare_cd_irq(port)))
+		printk_err("Failed CD IRQ reconfiguration (%i)\n", ret);
+
+	/*
+	 * If the FIM was stopped, then only schedule the workqueue for restarting it
+	 * again. This worker will call the detect change function.
+	 */
+	if (atomic_read(&port->fim_is_down))
+		schedule_work(&port->restart_work);
+	else
+		mmc_detect_change(port->mmc, msecs_to_jiffies(100));
+	
+	return IRQ_HANDLED;
+}
+
+inline static int fim_sd_card_plugged(struct fim_sdio_t *port)
+{
+	struct fim_driver *fim;
+	unsigned int val;
+	
+	fim = &port->fim;
+	fim_get_stat_reg(fim, FIM_SDIO_CARD_STATREG, &val);
+	return !val;
+}
+
+/*
+ * Handler for the incoming FIM-interrupts. Available interrupts:
+ * - Card detection
+ * - SDIO interrupts from the cards
+ */
+static void fim_sd_isr(struct fim_driver *driver, int irq, unsigned char code,
+		       unsigned int rx_fifo)
+{
+	struct fim_sdio_t *port;
+	
+	port = driver->driver_data;
+	if (!port || !port->mmc) {
+		printk_dbg("Null pointer by unexpected IRQ 0x%02x\n", code);
+		return;
+	}
+	
+	switch (code) {
+
+	case FIM_SDIO_INTARM_CARD_DETECTED:
+		if (fim_sd_card_plugged(port)) {
+			fim_set_ctrl_reg(&port->fim, FIM_SDIO_INTCFG_REG, 0);
+			printk_debug("SD card detected\n");
+		} else {
+			printk_debug("SD card removed\n");
+		}
+		mmc_detect_change(port->mmc, msecs_to_jiffies(100));
+		break;
+
+	case FIM_SDIO_INTARM_CARD_DAT1:
+		printk_debug("SDIO IRQ\n");
+
+		/*
+		 * Normally we don't need this sanity check but the FIM is reporting
+		 * some SDIO-interrupts where no SDIO-card was initialized.
+		 */
+		if (port->mmc->sdio_irq_thread)
+			mmc_signal_sdio_irq(port->mmc);
+		else
+			printk_dbg("Premature SDIO IRQ received!\n");
+		break;
+	default:
+		printk_err("Unknown IRQ %i | FIM %i | %x\n",
+			   code, port->fim.picnr, rx_fifo);
+		break;
+	}
+}
+
+/*
+ * This is the TX-callback that the API call after a DMA-package was closed
+ * The fim buffer structure contains our internal private data
+ * Free the allocated FIM-buffer that was used for sending the DMA-data
+ */
+static void fim_sd_tx_isr(struct fim_driver *driver, int irq,
+			  struct fim_buffer_t *pdata)
+{
+	struct fim_buffer_t *buf;
+	struct fim_sdio_t *port;
+
+	port = (struct fim_sdio_t *)driver->driver_data;
+	if (pdata->private) {
+		buf = pdata->private;
+		fim_free_buffer(&port->fim, buf);
+	}
+}
+
+/* @XXX: Remove this ugly code! */
+inline static void fim_sd_parse_resp(struct mmc_command *cmd,
+				     struct fim_sd_rx_resp_t *resp)
+{
+	unsigned char *ptr;
+	ptr = (unsigned char *)cmd->resp;
+	if (cmd->flags & MMC_RSP_136) {
+		*ptr++ = resp->resp[3];
+		*ptr++ = resp->resp[2];
+		*ptr++ = resp->resp[1];
+		*ptr++ = resp->resp[0];
+		*ptr++ = resp->resp[7];
+		*ptr++ = resp->resp[6];
+		*ptr++ = resp->resp[5];
+		*ptr++ = resp->resp[4];
+		*ptr++ = resp->resp[11];
+		*ptr++ = resp->resp[10];
+		*ptr++ = resp->resp[9];
+		*ptr++ = resp->resp[8];
+		*ptr++ = resp->resp[15];
+		*ptr++ = resp->resp[14];
+		*ptr++ = resp->resp[13];
+		*ptr++ = resp->resp[12];
+	} else {
+		*ptr++ = resp->resp[3];
+		*ptr++ = resp->resp[2];
+		*ptr++ = resp->resp[1];
+		*ptr++ = resp->resp[0];
+		*ptr++ = resp->resp[4];
+		*ptr++ = resp->stat;
+	}
+}
+
+/*
+ * This function checks the CRC by block read transfer
+ * The information about the length and content of the CRC was obtained
+ * from the firmware-source code (sd.asm)
+ */
+inline static int fim_sd_check_blkrd_crc(struct fim_sdio_t *port, unsigned char *data,
+					 int length)
+{
+	int crc_len;
+	unsigned char *pic_crc;	
+	
+	/*
+	 * The CRC length depends on the bus width (see sd.asm)
+	 * No CRC enabled : One byte (0x00)
+	 * One bit bus    : Four bytes
+	 * Four bit bus   : Eight bytes 
+	 */
+	if (!(port->mmc_cmd->flags & MMC_RSP_CRC)) {
+		crc_len = 1;
+		pic_crc = data;
+	} else if (port->mmc->ios.bus_width == MMC_BUS_WIDTH_1) {		
+		crc_len = 4;
+		pic_crc = data + 2;
+	} else {
+		crc_len = 16;
+		pic_crc = data + 8;
+	}
+
+	if (crc_len != length) {
+		printk_err("Unexpected CRC length %i (expected %i)\n",
+		       length, crc_len);
+		return -EINVAL;
+	}
+
+        /*
+	 * Code for forcing a CRC-error and the behavior of the MMC-layer
+	 * crc_error = 10 : Error reading the partition table
+	 * crc_error = 40 : Error by a block read transfer
+	 */
+#ifdef FIM_SDIO_FORCE_CRC
+	static int crc_error = 0;
+	if (crc_error == 40) {
+		crc_error++;
+		return 1;
+	} else
+		crc_error++;
+#endif
+
+	/* If the CRC is disabled, the PIC only appended a dummy Byte */
+	if (crc_len == 1)
+		return 0;
+
+#if defined(FIM_SDIO_DEBUG_CRC)
+#define FIM_SDIO_CRC_PATTERN                   "%02x %02x %02x %02x %02x %02x %02x %02x"
+	{
+		int retval, len;
+		
+		len = crc_len >> 1;
+		retval = memcmp(data, pic_crc, len);
+		if (retval) {
+
+			printk_dbg("Data len %i | CRC len %i\n", length, len);
+			
+			printk_dbg("CRC FIM : " FIM_SDIO_CRC_PATTERN "\n",
+				   *pic_crc, *(pic_crc + 1),
+				   *(pic_crc + 2), *(pic_crc + 3),
+				   *(pic_crc + 4), *(pic_crc + 5),
+				   *(pic_crc + 6), *(pic_crc + 7));
+			printk_dbg("CRC MMC : " FIM_SDIO_CRC_PATTERN "\n",
+				   *data, *(data + 1),
+				   *(data + 2), *(data + 3),
+				   *(data + 4), *(data + 5),
+				   *(data + 6), *(data + 7));
+		}
+
+		return retval;
+	}
+#else
+	return memcmp(data, pic_crc, crc_len >> 1);
+#endif
+}
+
+inline static void fim_sd_print_crc(int length, unsigned char *crc)
+{	
+	/* Only four and six as length supported */
+	if (length == 4)
+		printk_info("CRC: %u\n", *((unsigned int *)crc));
+	else
+		printk_info("CRC: %lu\n", *((unsigned long *)crc));
+}
+
+/*
+ * Called when a receive DMA-buffer was closed.
+ * Unfortunately the data received from the PIC has different formats. Sometimes it
+ * contains a response, sometimes data of a block read request and sometimes the CRC
+ * of the read data. In the case of a read transfer it is really amazing, then
+ * the transfer consists in four DMA-buffers.
+ */
+static void fim_sd_rx_isr(struct fim_driver *driver, int irq,
+			  struct fim_buffer_t *pdata)
+{
+	struct fim_sdio_t *port;
+	struct mmc_command *mmc_cmd;
+	struct fim_sd_rx_resp_t *resp;
+	int len, crc_len;
+	unsigned char *crc_ptr;
+	int is_ack;
+	
+	/* Get the correct port from the FIM-driver structure */
+	len = pdata->length;
+	port = (struct fim_sdio_t *)driver->driver_data;
+
+	/*
+	 * The timeout function can set the command structure to NULL, for this reason
+	 * check here is we can handle the response correctly
+	 */
+	if ((mmc_cmd = port->mmc_cmd) == NULL) {
+		printk_err("Timeouted command response?\n");
+		goto exit_unlock;
+	}
+		
+	/*
+	 * Check the current state of the command and update it if required
+	 * IMPORTANT: The buffer can contain response data or the data from a block
+	 * read too, for this reason was implemented the state machine
+	 */
+	resp = (struct fim_sd_rx_resp_t *)pdata->data;
+	is_ack = (pdata->length == 1) ? 1 : 0;
+	
+	printk_debug("CMD%i | RESP stat %x | CMD stat %i | BLKRD stat %i | Len %i\n",
+		     mmc_cmd->opcode, resp->stat, port->cmd_state,
+		     port->blkrd_state, pdata->length);
+
+	/*
+	 * By the ACKs the PIC will NOT send a timeout. Timeouts are only
+	 * set by the response and and block read data
+	 */
+	if (is_ack && resp->stat & FIM_SDIO_RX_TIMEOUT) {
+		mmc_cmd->error = -ETIMEDOUT;
+		port->blkrd_state = BLKRD_STATE_HAVE_DATA;
+		port->cmd_state = CMD_STATE_HAVE_RSP;
+
+		/* Check the conditions for the BLOCK READ state machine */
+	} else if (port->blkrd_state == BLKRD_STATE_WAIT_ACK && is_ack &&
+		   resp->stat & FIM_SDIO_RX_BLKRD) {
+		port->blkrd_state = BLKRD_STATE_WAIT_DATA;
+		
+		/* Check if the block read data has arrived */
+	} else if (port->blkrd_state == BLKRD_STATE_WAIT_DATA && !is_ack) {
+		crc_len = len - mmc_cmd->data->blksz;
+		crc_ptr = pdata->data + mmc_cmd->data->blksz;
+		port->blkrd_state = BLKRD_STATE_HAVE_DATA;
+
+#if !defined(FIM_SDIO_SKIP_CRC_CHECK)
+#define FIM_SDIO_RDBLK_PATTERN "%02x %02x %02x %02x %02x %02x " \
+				"%02x %02x %02x %02x %02x %02x"
+		if (fim_sd_check_blkrd_crc(port, crc_ptr, crc_len)) {
+			printk_err("CRC failure | Data %i | CRC %i | Retries %i\n",
+				   len, crc_len, mmc_cmd->retries);
+
+			printk_dbg(FIM_SDIO_RDBLK_PATTERN "\n",
+				   *pdata->data      , *(pdata->data + 1),
+				   *(pdata->data + 2), *(pdata->data + 3),
+				   *(pdata->data + 4), *(pdata->data + 5),
+				   *(pdata->data + 6), *(pdata->data + 7),
+				   *(pdata->data + 8), *(pdata->data + 9),
+				   *(pdata->data + 10), *(pdata->data + 11));
+
+			mmc_cmd->error = -EILSEQ;
+		} else {
+			fim_sd_dma_to_sg(port, mmc_cmd->data,
+					 pdata->data, pdata->length - crc_len);
+ 		}
+#else
+		fim_sd_dma_to_sg(port, mmc_cmd->data,
+				 pdata->data, pdata->length - crc_len);
+#endif
+
+		/* Check if we have a multiple transfer read */
+		port->trans_blocks -= 1;
+		if (port->trans_blocks > 0) {
+			printk_debug("Wait for next block %i\n", port->trans_blocks);
+			port->blkrd_state = BLKRD_STATE_WAIT_ACK;
+		}
+		
+		/* Check the conditions for the COMMAND state machine */
+	} else if (is_ack && port->cmd_state == CMD_STATE_WAIT_ACK &&
+		   resp->stat & FIM_SDIO_RX_RSP) {
+		port->cmd_state = CMD_STATE_WAIT_DATA;
+	} else if (!is_ack && port->cmd_state == CMD_STATE_WAIT_DATA) {
+		fim_sd_parse_resp(mmc_cmd, resp);
+		port->cmd_state = CMD_STATE_HAVE_RSP;
+
+		/* Check for unexpected acks or opcodes */
+	} else {
+		
+		/* @FIXME: Need a correct errror handling for this condition */
+		if (mmc_cmd->data && mmc_cmd->data->flags & MMC_DATA_READ)
+			printk_err("Failed multi RX (CMD%u | PIC stat %x | State %x)\n",
+				   mmc_cmd->opcode, resp->stat, port->blkrd_state);
+		else
+			printk_err("Unexpected RX stat (CMD%i | PIC stat %x | Leng %i)\n",
+				   mmc_cmd->opcode, resp->stat, pdata->length);
+	}
+
+	/*
+	 * By errors set the two states machines to the end position for sending
+	 * the error to the MMC-layer
+	 */
+	if (mmc_cmd->error) {
+		port->cmd_state = CMD_STATE_HAVE_RSP;
+		port->blkrd_state = BLKRD_STATE_HAVE_DATA;
+	}
+	
+	/*
+	 * Now evaluate if need to wait for another RX-interrupt or
+	 * can send the request done to the MMC-layer
+	 */
+	if (port->cmd_state == CMD_STATE_HAVE_RSP &&
+	    port->blkrd_state == BLKRD_STATE_HAVE_DATA) {
+
+#if defined(FIM_SDIO_MULTI_BLOCK)
+		if (mmc_cmd->data) {
+			uint blocks;
+			
+			fim_get_ctrl_reg(&port->fim, FIM_SDIO_BLOCKS_REG, &blocks);
+			printk_debug("CMD%u: End code %i | Blocks %u\n",
+				   mmc_cmd->opcode, mmc_cmd->error, blocks);
+		}
+#endif /* FIM_SDIO_MULTI_BLOCK */
+		
+		fim_sd_process_next(port);
+	}
+
+ exit_unlock:
+	return;
+}
+
+/* Send a buffer over the FIM-API */
+static int fim_sd_send_buffer(struct fim_sdio_t *port, struct fim_buffer_t *buf)
+{
+	struct fim_driver *fim;
+
+	if (!buf || !port)
+		return -EINVAL;
+	
+	fim = &port->fim;
+	buf->private = buf;
+	return fim_send_buffer(fim, buf);
+}
+
+/* Returns a command buffer allocated from the FIM-API */
+static struct fim_buffer_t *fim_sd_alloc_cmd(struct fim_sdio_t *port)
+{
+	struct fim_driver *fim;
+	int length;
+
+	if(!port)
+		return NULL;
+	
+	fim = &port->fim;
+	length = sizeof(struct fim_sd_tx_cmd_t);
+	
+	return fim_alloc_buffer(fim, length, GFP_KERNEL);
+}
+
+/* Returns a buffer allocated from the FIM-API */
+static struct fim_buffer_t *fim_sd_alloc_buffer(struct fim_sdio_t *port, int length)
+{
+	struct fim_driver *fim;
+
+	if (!port || length <= 0)
+		return NULL;
+	
+	fim = &port->fim;
+	return fim_alloc_buffer(fim, length, GFP_KERNEL);
+}
+
+static void fim_sd_free_buffer(struct fim_sdio_t *port, struct fim_buffer_t *buf)
+{
+	struct fim_driver *fim;
+
+	if (!port || !buf)
+		return;
+	
+	fim = &port->fim;
+	fim_free_buffer(fim, buf);
+}
+
+/*
+ * Copy the data from the MMC-layer (scatter list) to the DMA-buffer for the FIM-API
+ * Since we have only support for single block reads, some sanity checks
+ * are not implemented
+ */
+inline static void fim_sd_sg_to_dma(struct fim_sdio_t *port, struct mmc_data *data,
+				   struct fim_buffer_t *buf)
+{
+	unsigned int len, cnt, dma_len;
+	struct scatterlist *sg;
+	unsigned char *sg_buf;
+	unsigned char *dma_buf;
+	int process;
+	
+	sg = data->sg;
+	len = data->sg_len;
+	dma_buf = buf->data;
+	dma_len = 0;
+
+	/* Need a correct error handling */
+	if (len > 1) {
+		printk_err("The FIM-SD host only supports single block\n");
+		len = 1;
+	}
+	
+	/* @XXX: Check the correctness of the memcpy operation */
+	for (cnt = 0; cnt < len && dma_len <= buf->length; cnt++) {
+                sg_buf = sg_virt(&sg[cnt]);
+		process = (buf->length >= sg[cnt].length) ? sg[cnt].length : buf->length;
+                memcpy(dma_buf, sg_buf, process);
+		dma_buf += process;
+		dma_len += process;
+		data->bytes_xfered += process;
+	}
+}
+
+/*
+ * Function called when RD data has arrived
+ * Return value is the pointer of the last byte copied to the scatterlist, it can
+ * be used for appending more data (e.g. in multiple block read transfers)
+ */
+inline static void *fim_sd_dma_to_sg(struct fim_sdio_t *port, struct mmc_data *data,
+				     unsigned char *dma_buf, int dma_len)
+{
+	char *sg_buf;
+	
+	/* This loop was tested only with single block transfers */
+	sg_buf = port->sg_virt;
+	printk_debug("RX: %i bytes from %p to %p\n", dma_len, dma_buf, sg_buf);
+	memcpy(sg_buf, dma_buf, dma_len);
+	data->bytes_xfered += dma_len;
+
+	/* Update the pointer inside the SG buffer for the next transfer */
+	port->sg_virt += dma_len;
+	
+#if 0
+	for (cnt = port->trans_sg; cnt < len && dma_len > 0; cnt++) {
+
+		if (sg[cnt].length != 512)
+			printk_dbg("Unexpected block length %u\n", sg[cnt].length);
+
+		process = dma_len > sg[cnt].length ? sg[cnt].length : dma_len;
+		sg_buf = sg_virt(&sg[cnt]);
+                memcpy(sg_buf, dma_buf, process);
+		dma_buf += process;
+		dma_len -= process;
+		data->bytes_xfered += process;
+		sg_buf += process;
+		port->trans_sg += 1;
+	}
+
+	port->sg = &sg[cnt];
+#endif
+	
+	return sg_buf;
+}
+
+/* This function will send the command to the PIC using the TX-DMA buffers */
+static int fim_sd_send_command(struct fim_sdio_t *port, struct mmc_command *cmd)
+{
+	struct mmc_data *data;
+	struct fim_buffer_t *buf;
+	struct fim_sd_tx_cmd_t *txcmd;
+	unsigned int block_length, blocks;
+	int retval, length;
+	
+	/* @TODO: Send an error response to the MMC-core */
+	if (!(buf = fim_sd_alloc_cmd(port))) {
+		printk_err("No memory available for a new CMD?\n");
+		return -ENOMEM;
+	}
+
+	/* Use the buffer data for the TX-command */
+	txcmd = (struct fim_sd_tx_cmd_t *)buf->data;
+	txcmd->opctl = 0;
+	
+	/*
+	 * Set the internal flags for the next response sequences
+	 * Assume that we will wait for a command response (not block read).
+	 * By block reads the flag will be modified inside the if-condition
+	 */
+	port->cmd_state = CMD_STATE_WAIT_ACK;
+	port->blkrd_state = BLKRD_STATE_HAVE_DATA;
+	if ((data = cmd->data) != NULL) {
+		block_length = data->blksz;
+		blocks = data->blocks;
+
+#if !defined(FIM_SDIO_MULTI_BLOCK)
+		if (blocks != 1) {
+			printk_err("Only supports single block transfer (%i)\n", blocks);
+			cmd->error = -EILSEQ;
+			fim_sd_process_next(port);
+			return -EILSEQ;
+		}
+#endif
+
+		printk_debug("CMD%u: %s %i blks | %i blksz | SG len %u\n",
+			     cmd->opcode,
+			     (data->flags & MMC_DATA_READ) ? "RX" : "TX",
+			     data->blocks, data->blksz,
+			     data->sg_len);
+
+		/* Reset the scatter list position */
+		port->trans_sg     = 0;
+		port->sg_virt      = sg_virt(&data->sg[0]);
+		port->trans_blocks = blocks;
+
+		/* Setup the FIM registers (number of blocks and block size) */
+#if defined(FIM_SDIO_MULTI_BLOCK)
+		fim_set_ctrl_reg(&port->fim, FIM_SDIO_BLOCKS_REG, blocks);
+		fim_set_ctrl_reg(&port->fim, FIM_SDIO_BLKSZ_LSB_REG, block_length);
+		fim_set_ctrl_reg(&port->fim, FIM_SDIO_BLKSZ_MSB_REG,
+				 (block_length >> 8));
+#endif
+
+		/* Check if the transfer request is for reading or writing */
+		if (cmd->data->flags & MMC_DATA_READ) {
+			txcmd->opctl |= SDIO_FIFO_TX_BLKRD;
+			port->blkrd_state = BLKRD_STATE_WAIT_ACK;
+		} else
+			txcmd->opctl |= SDIO_FIFO_TX_BLKWR;
+	} else {
+		block_length = 0;
+		blocks = 0;
+	}
+
+	/* Set the correct expected response length */
+	if (cmd->flags & MMC_RSP_136)
+		txcmd->opctl |= SDIO_FIFO_TX_136RSP;
+	else
+		txcmd->opctl |= SDIO_FIFO_TX_48RSP;
+
+	/* Set the correct CRC configuration */
+	if (!(cmd->flags & MMC_RSP_CRC)) {
+		printk_debug("CRC is disabled\n");
+		txcmd->opctl |= SDIO_FIFO_TX_DISCRC;		
+	}
+
+	/* Set the correct bus width */
+	if (port->mmc->ios.bus_width == MMC_BUS_WIDTH_4) {
+		printk_debug("Bus width has four bits\n");
+		txcmd->opctl |= SDIO_FIFO_TX_BW4;
+	}
+
+	txcmd->blksz_msb = (block_length >> 8);
+	txcmd->blksz_lsb =  block_length;
+	txcmd->cmd[0] = SDIO_HOST_TX_HDR | (cmd->opcode & SDIO_HOST_CMD_MASK);
+	txcmd->cmd[1] = cmd->arg >> 24;
+	txcmd->cmd[2] = cmd->arg >> 16;
+	txcmd->cmd[3] = cmd->arg >> 8;
+	txcmd->cmd[4] = cmd->arg;
+
+	/*
+	 * Store the private data for the callback function
+	 * If an error ocurrs when sending the buffer, the timeout function will
+	 * send the error to the MMC-layer
+	 */
+	port->buf = buf;
+	port->mmc_cmd = cmd;
+	buf->private = port;
+	if ((retval = fim_sd_send_buffer(port, buf))) {
+/* 		printk_err("MMC command %i (err %i)\n", cmd->opcode, */
+/* 			   retval); */
+		fim_sd_free_buffer(port, buf);
+		mod_timer(&port->mmc_timer,
+			  jiffies + msecs_to_jiffies(1/* FIM_SDIO_TIMEOUT_MS */));
+		goto exit_ok;
+	} else
+		mod_timer(&port->mmc_timer,
+			  jiffies + msecs_to_jiffies(FIM_SDIO_TIMEOUT_MS));
+
+	/*
+	 * If we have a write command then fill a next buffer and send it
+	 * @TODO: We need here an error handling, then otherwise we have started a
+	 * WR-transfer but have no transfer data (perhaps not too critical?)
+	 */
+	if (data && data->flags & MMC_DATA_WRITE) {
+		length = data->blksz * data->blocks;
+		if (!(buf = fim_sd_alloc_buffer(port, length))) {
+			printk_err("Buffer alloc BLKWR failed, %i\n", length);
+			goto exit_ok;
+		}
+
+		buf->private = port;
+		fim_sd_sg_to_dma(port, data, buf);
+		if ((retval = fim_sd_send_buffer(port, buf))) {
+			printk_err("Send BLKWR-buffer failed, %i\n", retval);
+			fim_sd_free_buffer(port, buf);
+		}
+	}
+	
+ exit_ok:
+	return 0;
+}
+
+/*
+ * This function will be called from the request function and from the ISR callback
+ * when a command was executed
+ * In some cases we don't need to pass the command response to the Linux-layer (e.g.
+ * by the configuration of the bus width)
+ */
+static void fim_sd_process_next(struct fim_sdio_t *port)
+{
+	if (port->flags == FIM_SDIO_REQUEST_NEW) {
+		port->flags = FIM_SDIO_REQUEST_CMD;
+		fim_sd_send_command(port, port->mmc_req->cmd);
+	} else if ((!(port->flags & FIM_SDIO_REQUEST_STOP)) && port->mmc_req->stop) {
+		port->flags = FIM_SDIO_REQUEST_STOP;
+		fim_sd_send_command(port, port->mmc_req->stop);
+	} else {
+		/* By timeouts the core might retry sending another command */
+		port->mmc_cmd = NULL;
+		mmc_request_done(port->mmc, port->mmc_req);
+	}
+}
+
+/*
+ * Called for processing three main request types:
+ * command : Submit the command to the PIC and returns inmediately
+ * stop    : Request for stopping an already started request?
+ * data    : For data transfer
+ */
+static void fim_sd_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct fim_sdio_t *port;
+
+	if (!(port = get_port_from_mmc(mmc)))
+		return;
+
+	/* In the case that the FIM was shutdown return at this point */
+	if (atomic_read(&port->fim_is_down)) {
+		mrq->cmd->error = -ENOMEDIUM;
+		mmc_request_done(mmc, mrq);
+		return;
+	}
+	
+	/*
+	 * Wait if the timeout function is running or the RX-callback is active
+	 */
+	port->mmc_req = mrq;
+	port->flags = FIM_SDIO_REQUEST_NEW;	
+	fim_sd_process_next(port);
+}
+
+/* Set the transfer clock using the pre-defined values */
+static void fim_sd_set_clock(struct fim_sdio_t *port, long int clockrate)
+{
+	unsigned long clkdiv;
+	unsigned long clk;
+
+	if (clockrate) {
+		clk = clk_get_rate(port->sys_clk) / 4;
+		clkdiv  = clk /clockrate + 0x02;
+
+		/* @XXX: If the configuration failed disable the clock */
+		if (clkdiv < 0x4 || clkdiv > 0xff) {
+			printk_err("Unsupported clock %luHz (div out of range 0x%4lx)\n",
+				   clockrate, clkdiv);
+			clockrate = -EINVAL;
+		}
+		else
+			printk_debug("Calculated divisor is 0x%02lx for %lu\n",
+				     clkdiv, clockrate);
+	} else {
+		printk_debug("@TODO: Disable the clock (set to 0Hz)\n");
+		clkdiv = 0;
+		clockrate = -EINVAL;
+	}
+
+	/* Now write the value to the corresponding FIM-register */
+	if (clockrate >= 0) {
+		printk_debug("Setting the clock to %ld (%lx)\n",
+			     clockrate, clkdiv);
+		fim_set_ctrl_reg(&port->fim, FIM_SDIO_CLKDIV_REG, clkdiv);
+	}
+}
+
+/*
+ * Called by the core system for setting the available modes and clock speed
+ * 
+ */
+static void fim_sd_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct fim_sdio_t *port;
+	unsigned long ireg;
+	
+	if (!(port = get_port_from_mmc(mmc)))
+		return;
+
+	/*
+	 * The FIM-board doesn't have a power switch for the card, but probably the
+	 * next revision will include it, so that we can control the switch from here.
+	 */
+	ireg = 0;
+	switch (ios->power_mode) {
+	case MMC_POWER_OFF:
+	case MMC_POWER_UP:
+	case MMC_POWER_ON:
+		ireg |= FIM_SDIO_INTCFG_MAIN | FIM_SDIO_INTCFG_CARD;
+		break;
+	}
+
+	fim_set_ctrl_reg(&port->fim, FIM_SDIO_INTCFG_REG, ireg);
+	
+	fim_sd_set_clock(port, ios->clock);
+}
+
+/*
+ * Return the read only status of the plugged card
+ * Since the FIM-firmware doesn't include this GPIO, we will read it at this point
+ */
+static int fim_sd_get_ro(struct mmc_host *mmc)
+{
+	struct fim_sdio_t *port;
+
+	if (!(port = get_port_from_mmc(mmc))) {
+		printk_err("No FIM-port by a registered MMC-host?\n");
+		return -ENODEV;
+	}
+
+	return gpio_get_value_ns921x(port->wp_gpio);
+}
+
+
+static void fim_sd_enable_sdio_irq(struct mmc_host *mmc, int enable)
+{
+	struct fim_sdio_t *port;
+	unsigned int ireg;
+	
+	printk_debug("%s the SDIO IRQ\n", enable ? "Enabling" : "Disabling");
+	if (!(port = get_port_from_mmc(mmc))) {
+		printk_err("NULL port pointer found!\n");
+		return;
+	}
+
+	fim_get_ctrl_reg(&port->fim, FIM_SDIO_INTCFG_REG, &ireg);		
+	if (enable)
+		ireg |= FIM_SDIO_INTCFG_SDIO;
+	else
+		ireg &= ~FIM_SDIO_INTCFG_SDIO;
+		
+	fim_set_ctrl_reg(&port->fim, FIM_SDIO_INTCFG_REG, ireg);
+}
+
+
+/* Available driver host operations */
+static const struct mmc_host_ops fim_sd_ops = {
+	.request = fim_sd_request,
+	.set_ios = fim_sd_set_ios,
+	.get_ro  = fim_sd_get_ro,
+	.enable_sdio_irq = fim_sd_enable_sdio_irq,
+};
+
+
+/* Called normally only when exiting the driver */
+static int fim_sdio_unregister_port(struct fim_sdio_t *port)
+{
+	int cnt;
+	
+	if (!port || !port->reg)
+		return -ENODEV;
+
+	printk_info("Removing the FIM MMC host %i\n", port->index);
+	mmc_remove_host(port->mmc);
+	mmc_free_host(port->mmc);
+
+	del_timer_sync(&port->mmc_timer);
+
+	/* Reset the main control register */
+	fim_set_ctrl_reg(&port->fim, FIM_SDIO_MAIN_REG, 0);
+	
+	fim_unregister_driver(&port->fim);
+
+	for (cnt=0; cnt < FIM_SDIO_MAX_GPIOS; cnt++) {
+		printk_debug("Freeing GPIO %i\n", port->gpios[cnt].nr);
+		if (port->gpios[cnt].nr == FIM_LAST_GPIO)
+			break;		
+		else if (port->gpios[cnt].nr == FIM_GPIO_DONT_USE)
+			continue;
+		else
+			gpio_free(port->gpios[cnt].nr);
+	}
+
+	if (port->sys_clk && !IS_ERR(port->sys_clk)) {
+		clk_put(port->sys_clk);
+		port->sys_clk = NULL;
+	}
+
+	if (port->cd_irq > 0) {
+		free_irq(port->cd_irq, port);
+		port->cd_irq = -1;
+	}
+	
+	port->reg = 0;
+	return 0;
+}
+
+
+
+/* Register the new FIM driver by the FIM-API */
+static int fim_sdio_register_port(struct device *dev, struct fim_sdio_t *port,
+				  struct fim_sdio_platform_data *pdata,
+				  struct fim_gpio_t gpios[])
+{
+	int retval;
+	int cnt;
+	struct fim_dma_cfg_t dma_cfg;
+	int picnr = pdata->fim_nr;
+	unsigned long hcaps = pdata->host_caps;
+	unsigned int fwver;
+
+#if defined(MODULE)
+	const char *fwcode = NULL;
+	char fwname[FIM_SDIO_FW_LEN];
+	snprintf(fwname, FIM_SDIO_FW_LEN, FIM_SDIO_FW_FILE_PAT, picnr);
+#else
+	char *fwname = NULL;
+	const char *fwcode = (picnr == 0) ? FIM_SDIO_FW_CODE0 : FIM_SDIO_FW_CODE1;
+#endif
+	
+	/* Specific DMA configuration for the SD-host driver */
+	dma_cfg.rxnr = FIM_SDIO_DMA_RX_BUFFERS;
+	dma_cfg.txnr = FIM_SDIO_DMA_TX_BUFFERS;
+	dma_cfg.rxsz = FIM_SDIO_DMA_BUFFER_SIZE;
+	dma_cfg.txsz = FIM_SDIO_DMA_BUFFER_SIZE;
+
+	port->index = picnr;
+	port->fim.picnr = picnr;
+	port->fim.driver.name = FIM_SDIO_DRIVER_NAME;
+	port->fim.driver_data = port;
+	port->fim.fim_isr = fim_sd_isr;
+	port->fim.dma_tx_isr = fim_sd_tx_isr;
+	port->fim.dma_rx_isr = fim_sd_rx_isr;
+	port->fim.driver_data = port;
+	port->fim.dma_cfg = &dma_cfg;
+
+	/* Check if have a firmware code for using to */
+	port->fim.fw_name = fwname;
+	port->fim.fw_code = fwcode;
+	retval = fim_register_driver(&port->fim);
+	if (retval) {
+		printk_err("Couldn't register the FIM driver.\n");
+		return retval;
+	}
+
+	/* Request the corresponding GPIOs (@XXX: Check the returned values) */
+	for (cnt=0; cnt < FIM_SDIO_MAX_GPIOS; cnt++) {
+
+		if (gpios[cnt].nr == FIM_LAST_GPIO)
+			break;
+		
+		if (gpios[cnt].nr == FIM_GPIO_DONT_USE)
+			continue;
+
+		printk_debug("Requesting the GPIO %i (Function %i)\n",
+			     gpios[cnt].nr, gpios[cnt].func);
+		retval = gpio_request(gpios[cnt].nr, FIM_SDIO_DRIVER_NAME);
+		if (!retval) {
+			gpio_configure_ns921x_unlocked(gpios[cnt].nr,
+						       NS921X_GPIO_INPUT,
+						       NS921X_GPIO_DONT_INVERT,
+						       gpios[cnt].func,
+						       NS921X_GPIO_DISABLE_PULLUP);
+		} else {
+			/* Free the already requested GPIOs */
+			printk_err("Couldn't request the GPIO %i\n", gpios[cnt].nr);
+			while (cnt)
+				gpio_free(gpios[--cnt].nr);
+			
+			goto exit_unreg_fim;
+		}
+	}
+	
+	/* Configure and init the timer for the command timeouts */
+	init_timer(&port->mmc_timer);
+	port->mmc_timer.function = fim_sd_cmd_timeout;
+	port->mmc_timer.data = (unsigned long)port;
+
+	/* This is the timer for checking the state of the card detect line */
+	init_timer(&port->cd_timer);
+	port->cd_timer.function = fim_sd_cd_timer_func;
+	port->cd_timer.data = (unsigned long)port;
+	
+	port->mmc = mmc_alloc_host(sizeof(struct fim_sdio_t *), port->fim.dev);
+	if (!port->mmc) {
+		printk_err("Alloc MMC host by the FIM %i failed.\n", picnr);
+		retval = -ENOMEM;
+		goto exit_free_gpios;
+	}
+
+        /* Get a reference to the SYS clock for setting the clock */
+        if (IS_ERR(port->sys_clk = clk_get(port->fim.dev, "systemclock"))) {
+                printk_err("Couldn't get the SYS clock.\n");
+                goto exit_free_host;
+        }
+	
+	/* These are the default values for this SD-host */
+	port->mmc->ops = &fim_sd_ops;
+
+	/* Supported physical properties of the FIM-host (see the PIC-firmware code) */
+	port->mmc->f_min = 320000;
+	port->mmc->f_max = 25000000;
+	port->mmc->ocr_avail = MMC_VDD_33_34 | MMC_VDD_32_33;
+	port->mmc->caps = hcaps;
+
+	/* Maximum number of blocks in one req */
+	port->mmc->max_blk_count = FIM_SDIO_MAX_BLOCKS;
+	port->mmc->max_blk_size  = FIM_SDIO_DMA_BUFFER_SIZE;
+	/* The maximum per SG entry depends on the buffer size */
+	port->mmc->max_seg_size  = FIM_SDIO_DMA_BUFFER_SIZE;
+
+	port->mmc->max_req_size       = 4095 * FIM_SDIO_MAX_BLOCKS;
+	port->mmc->max_seg_size       = port->mmc->max_req_size;
+	port->mmc->max_phys_segs      = FIM_SDIO_MAX_BLOCKS;
+	port->mmc->max_hw_segs        = FIM_SDIO_MAX_BLOCKS;
+	
+	/* Save our port structure into the private pointer */
+	port->mmc->private[0] = (unsigned long)port;
+	retval = mmc_add_host(port->mmc);
+	if (retval) {
+		printk_err("Couldn't add the MMC host\n");
+		goto exit_put_clk;
+	}
+
+	memcpy(port->gpios, gpios, sizeof(struct fim_gpio_t) * FIM_SDIO_MAX_GPIOS);
+	port->reg = 1;
+	dev_set_drvdata(dev, port);
+
+	/* Fist disable the SDIO-IRQ */
+	fim_sd_enable_sdio_irq(port->mmc, 0);
+	
+	/*
+	 * If no interrupt line is available for the card detection, then use the timer
+	 * for the polling.
+	 */
+	port->cd_irq = gpio_to_irq(port->cd_gpio);
+
+#if defined(FIM_SDIO_FORCE_CD_POLLING)
+	port->cd_irq = -1;
+#endif
+	
+	if (port->cd_irq > 0) {
+		int iret;
+		
+		/* First check if we have an IRQ at this line */
+		printk_info("Got IRQ %i for GPIO %i\n", port->cd_irq,
+			    port->cd_gpio);
+
+		iret = request_irq(port->cd_irq, fim_sd_cd_irq,
+				   IRQF_DISABLED | IRQF_TRIGGER_FALLING,
+				   "fim-sdio-cd", port);
+		if (iret) {
+			printk_err("Failed IRQ %i request (%i)\n", port->cd_irq, iret);
+			port->cd_irq = -1;
+		}
+	}
+
+	/*
+	 * By errors (e.g. in the case that we couldn't request the IRQ) use the
+	 * polling function
+	 */
+	if (port->cd_irq < 0) {
+		printk(KERN_DEBUG "Polling the Card Detect line (no IRQ)\n");
+		port->cd_value = 1; /* @XXX: Use an invert value for this purpose */
+		mod_timer(&port->cd_timer, jiffies + HZ / 2);
+	} else {
+		/*
+		 * Setup the card detect interrupt at this point, otherwise the first
+		 * event detection will not work when the system is booted with a
+		 * plugged card.
+		 */
+		if ((retval = fim_sd_prepare_cd_irq(port))) {
+			printk_err("Failed card detect IRQ setup\n");
+			goto exit_free_cdirq;
+		}
+	}
+
+	INIT_WORK(&port->restart_work, fim_sd_restart_work_func);
+
+	/* And enable the FIM-interrupt */
+	fim_enable_irq(&port->fim);
+	fim_set_ctrl_reg(&port->fim, FIM_SDIO_MAIN_REG, FIM_SDIO_MAIN_START);
+
+	/* Print the firmware version */
+	fim_get_stat_reg(&port->fim, FIM_SDIO_VERSION_SREG, &fwver);
+	printk_dbg("FIM%d running [fw rev 0x%02x]\n", port->fim.picnr, fwver);
+	return 0;
+
+ exit_free_cdirq:
+	free_irq(port->cd_irq, port);
+	
+ exit_put_clk:
+	clk_put(port->sys_clk);
+	
+ exit_free_host:
+	mmc_free_host(port->mmc);
+
+ exit_free_gpios:
+	for (cnt=0; gpios[cnt].nr < FIM_SDIO_MAX_GPIOS; cnt++) {
+		if (gpios[cnt].nr == FIM_LAST_GPIO)
+			break;
+		if (gpios[cnt].nr != FIM_GPIO_DONT_USE)
+			gpio_free(gpios[cnt].nr);
+	}
+	
+ exit_unreg_fim:
+	fim_unregister_driver(&port->fim);
+
+	return retval;
+}
+
+
+
+static int __devinit fim_sdio_probe(struct platform_device *pdev)
+{
+	struct fim_sdio_t *port;
+	struct fim_sdio_platform_data *pdata;
+	struct fim_gpio_t gpios[FIM_SDIO_MAX_GPIOS];
+	int retval;
+
+	printk_debug("Probing a new device with ID %i\n", pdev->id);
+	
+	pdata = pdev->dev.platform_data;
+	if (!pdata)
+		return -ENXIO;
+
+	if (fim_check_device_id(fims_number, pdata->fim_nr)) {
+#if defined(MODULE)
+		printk_dbg("Skipping FIM%i (not selected)\n", pdata->fim_nr);
+#else
+		printk_err("Invalid FIM number '%i' in platform data\n", pdata->fim_nr);
+#endif
+		return -ENODEV;
+	}
+	
+	port = fim_sdios->ports + pdata->fim_nr;
+
+	/* Get the GPIOs-table from the platform data structure */
+	gpios[FIM_SDIO_D0_GPIO].nr   = pdata->d0_gpio_nr;
+	gpios[FIM_SDIO_D0_GPIO].func = pdata->d0_gpio_func;
+	gpios[FIM_SDIO_D1_GPIO].nr   = pdata->d1_gpio_nr;
+	gpios[FIM_SDIO_D1_GPIO].func = pdata->d1_gpio_func;
+	gpios[FIM_SDIO_D2_GPIO].nr   = pdata->d2_gpio_nr;
+	gpios[FIM_SDIO_D2_GPIO].func = pdata->d2_gpio_func;
+	gpios[FIM_SDIO_D3_GPIO].nr   = pdata->d3_gpio_nr;
+	gpios[FIM_SDIO_D3_GPIO].func = pdata->d3_gpio_func;
+	gpios[FIM_SDIO_WP_GPIO].nr   = pdata->wp_gpio_nr;
+	gpios[FIM_SDIO_WP_GPIO].func = pdata->wp_gpio_func;
+	gpios[FIM_SDIO_CD_GPIO].nr   = pdata->cd_gpio_nr;
+	gpios[FIM_SDIO_CD_GPIO].func = pdata->cd_gpio_func;
+	gpios[FIM_SDIO_CLK_GPIO].nr   = pdata->clk_gpio_nr;
+	gpios[FIM_SDIO_CLK_GPIO].func = pdata->clk_gpio_func;
+	gpios[FIM_SDIO_CMD_GPIO].nr   = pdata->cmd_gpio_nr;
+	gpios[FIM_SDIO_CMD_GPIO].func = pdata->cmd_gpio_func;
+	port->wp_gpio = pdata->wp_gpio_nr;
+	port->cd_gpio = pdata->cd_gpio_nr;
+	
+	retval = fim_sdio_register_port(&pdev->dev, port, pdata, gpios);
+
+	return retval;
+}
+
+static int __devexit fim_sdio_remove(struct platform_device *pdev)
+{
+	struct fim_sdio_t *port;
+	int retval;
+	
+	port = dev_get_drvdata(&pdev->dev);
+
+	retval = fim_sdio_unregister_port(port);
+	if (!retval)
+		dev_set_drvdata(&pdev->dev, NULL);
+
+	return retval;
+}
+
+static struct platform_driver fim_sdio_platform_driver = {
+	.probe	= fim_sdio_probe,
+	.remove	= __devexit_p(fim_sdio_remove),
+	.driver	= {
+		   .owner = THIS_MODULE,
+		   .name  = FIM_SDIO_DRIVER_NAME,
+	},
+};
+
+/*
+ * This is the function that will be called when the module is loaded
+ * into the kernel space
+ */
+static int __init fim_sdio_init(void)
+{
+	int retval;
+	int nrpics;
+	
+	printk_debug("Starting the FIM SDIO driver.\n");	
+
+	/* Get the number of available FIMs */
+	nrpics = fim_number_pics();
+
+        /* Check for the passed number parameter */
+	if (fim_check_numbers_param(fims_number)) {
+		printk_err("Invalid number '%i' of FIMs to handle\n", fims_number);
+		return -EINVAL;
+	}
+	
+	fim_sdios = kzalloc(sizeof(struct fim_sdios_t) +
+			    (nrpics * sizeof(struct fim_sdio_t)), GFP_KERNEL);
+	if (!fim_sdios)
+		return -ENOMEM;
+
+	fim_sdios->fims = nrpics;
+	fim_sdios->ports = (void *)fim_sdios + sizeof(struct fim_sdios_t);	
+
+	retval = platform_driver_register(&fim_sdio_platform_driver);
+	if (retval)
+		goto exit_free_ports;
+	
+	printk_info(DRIVER_DESC " v" DRIVER_VERSION "\n");
+	return 0;
+
+ exit_free_ports:
+	kfree(fim_sdios);
+
+	return retval;
+}
+
+
+
+/*
+ * Free the requested resources (GPIOs, memory, drivers, etc.)
+ * The following steps MUST be followed when unregistering the driver:
+ * - First remove and free the MMC-host
+ * - Unregister the FIM-driver (will free the DMA-channel)
+ * - Free the GPIOs at last
+ */
+static void __exit fim_sdio_exit(void)
+{
+	printk_info("Removing the FIM SDIO driver\n");
+	platform_driver_unregister(&fim_sdio_platform_driver);		
+	kfree(fim_sdios);
+}
+
+
+module_init(fim_sdio_init);
+module_exit(fim_sdio_exit);
+
diff --git a/drivers/fims/sdio/fim_sdio0.h b/drivers/fims/sdio/fim_sdio0.h
new file mode 100644
index 000000000000..12c99a074c5d
--- /dev/null
+++ b/drivers/fims/sdio/fim_sdio0.h
@@ -0,0 +1,225 @@
+
+/*
+ * Automatic generated header file with the firmware code for the FIM
+ * Input binary  : fim_firmware.bin
+ * Output header : fim_sdio.h
+ * Structure     : fim_sdio_firmware0
+ */
+
+static const unsigned char fim_sdio_firmware0[] = { 
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x01, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x2f, 0x03, 0x00, 0x00,  0x0e, 0x28, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0xa0, 0x00, 0x03, 0x08,  
+	0x83, 0x12, 0xa1, 0x00,  0x8b, 0x10, 0x21, 0x08,  
+	0x83, 0x00, 0xa0, 0x0e,  0x20, 0x0e, 0x09, 0x00,  
+	0x1d, 0x23, 0x83, 0x12,  0x9f, 0x01, 0x9f, 0x14,  
+	0x83, 0x16, 0xff, 0x30,  0x99, 0x00, 0x83, 0x12,  
+	0x83, 0x12, 0x15, 0x1c,  0x16, 0x28, 0x00, 0x30,  
+	0xa6, 0x00, 0x1c, 0x28,  0xa4, 0x01, 0xbb, 0x01,  
+	0xbc, 0x01, 0xbd, 0x01,  0xbe, 0x01, 0xbf, 0x01,  
+	0xc0, 0x01, 0xc1, 0x01,  0xc2, 0x01, 0x83, 0x16,  
+	0xa1, 0x01, 0x83, 0x12,  0xb9, 0x01, 0x3d, 0x20,  
+	0x8c, 0x1d, 0x54, 0x28,  0x2e, 0x20, 0x1c, 0x28,  
+	0x83, 0x16, 0x1a, 0x14,  0x83, 0x12, 0x10, 0x08,  
+	0xac, 0x00, 0xac, 0x0b,  0x33, 0x28, 0x83, 0x16,  
+	0x1a, 0x10, 0x83, 0x12,  0x10, 0x08, 0xac, 0x00,  
+	0xac, 0x0b, 0x3a, 0x28,  0x08, 0x00, 0x22, 0x1c,  
+	0x48, 0x28, 0x83, 0x16,  0x89, 0x1f, 0x46, 0x28,  
+	0x83, 0x12, 0x89, 0x01,  0x22, 0x10, 0x08, 0x00,  
+	0x83, 0x12, 0x08, 0x00,  0x11, 0x1c, 0x08, 0x00,  
+	0x83, 0x16, 0x90, 0x1c,  0x4f, 0x28, 0x83, 0x12,  
+	0x08, 0x00, 0x83, 0x12,  0x22, 0x14, 0x01, 0x30,  
+	0x89, 0x00, 0x08, 0x00,  0x9f, 0x10, 0xa7, 0x01,  
+	0x0f, 0x23, 0xa3, 0x00,  0x0f, 0x23, 0xb5, 0x00,  
+	0x0f, 0x23, 0xb4, 0x00,  0xff, 0x39, 0x03, 0x19,  
+	0x62, 0x28, 0x35, 0x08,  0x01, 0x3e, 0xb5, 0x00,  
+	0x0f, 0x23, 0x75, 0x20,  0x0f, 0x23, 0x75, 0x20,  
+	0x0f, 0x23, 0x75, 0x20,  0x0f, 0x23, 0x75, 0x20,  
+	0x0f, 0x23, 0x75, 0x20,  0x8e, 0x20, 0x27, 0x08,  
+	0x75, 0x20, 0x9f, 0x14,  0x23, 0x18, 0x9b, 0x28,  
+	0xa3, 0x18, 0x9b, 0x28,  0x46, 0x29, 0xaa, 0x00,  
+	0x08, 0x30, 0xad, 0x00,  0xaa, 0x0d, 0x83, 0x16,  
+	0x03, 0x18, 0x7e, 0x28,  0x9a, 0x10, 0x7f, 0x28,  
+	0x9a, 0x14, 0x83, 0x12,  0x85, 0x20, 0x2e, 0x20,  
+	0xad, 0x0b, 0x78, 0x28,  0x08, 0x00, 0xa7, 0x13,  
+	0x03, 0x18, 0xa7, 0x17,  0xa7, 0x0d, 0xa7, 0x1f,  
+	0x08, 0x00, 0x09, 0x30,  0xa7, 0x06, 0x08, 0x00,  
+	0x07, 0x30, 0xa9, 0x00,  0x03, 0x10, 0xa7, 0x0d,  
+	0xa7, 0x1f, 0x96, 0x28,  0x09, 0x30, 0xa7, 0x06,  
+	0xa9, 0x0b, 0x90, 0x28,  0x03, 0x14, 0xa7, 0x0d,  
+	0x08, 0x00, 0x64, 0x30,  0xab, 0x00, 0xb0, 0x01,  
+	0xa7, 0x01, 0xab, 0x0b,  0xa2, 0x28, 0x1c, 0x29,  
+	0x1f, 0x21, 0x03, 0x18,  0x9f, 0x28, 0x23, 0x1a,  
+	0xe0, 0x28, 0x01, 0x30,  0x18, 0x23, 0x03, 0x10,  
+	0x2d, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0xa3, 0x18, 0xc2, 0x28,  
+	0x8e, 0x20, 0x27, 0x08,  0xa8, 0x00, 0x2c, 0x21,  
+	0x13, 0x23, 0x28, 0x08,  0xa3, 0x19, 0xc0, 0x28,  
+	0x18, 0x23, 0x46, 0x29,  0x13, 0x23, 0x46, 0x29,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x8e, 0x20, 0x27, 0x08,  
+	0xa8, 0x00, 0x2c, 0x21,  0x13, 0x23, 0x28, 0x08,  
+	0x18, 0x23, 0x46, 0x29,  0x50, 0x30, 0x84, 0x00,  
+	0x2d, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x2c, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x2c, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x2c, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x2c, 0x21, 0x80, 0x00,  0x84, 0x0a, 0xa3, 0x1c,  
+	0x14, 0x29, 0x2c, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x2c, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x2c, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x2c, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x2c, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x2c, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x2c, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x2c, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x2c, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x2c, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x2c, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x8e, 0x20, 0x27, 0x08,  
+	0xa8, 0x00, 0x2c, 0x21,  0x80, 0x00, 0x24, 0x18,  
+	0xfc, 0x29, 0x0b, 0x2a,  0x04, 0x30, 0x18, 0x23,  
+	0x1c, 0x28, 0x23, 0x1a,  0x21, 0x22, 0x83, 0x16,  
+	0x92, 0x18, 0x28, 0x29,  0x83, 0x12, 0x2e, 0x20,  
+	0x03, 0x10, 0x08, 0x00,  0x83, 0x12, 0x2e, 0x20,  
+	0x03, 0x14, 0x08, 0x00,  0x1f, 0x21, 0x85, 0x20,  
+	0xb0, 0x0d, 0x1f, 0x21,  0x85, 0x20, 0xb0, 0x0d,  
+	0x1f, 0x21, 0x85, 0x20,  0xb0, 0x0d, 0x1f, 0x21,  
+	0x85, 0x20, 0xb0, 0x0d,  0x1f, 0x21, 0x85, 0x20,  
+	0xb0, 0x0d, 0x1f, 0x21,  0x85, 0x20, 0xb0, 0x0d,  
+	0x1f, 0x21, 0x85, 0x20,  0xb0, 0x0d, 0x1f, 0x21,  
+	0x85, 0x20, 0xb0, 0x0d,  0x30, 0x08, 0x08, 0x00,  
+	0x23, 0x1a, 0xfc, 0x29,  0xa3, 0x19, 0x4f, 0x29,  
+	0x2e, 0x20, 0x2e, 0x20,  0x2e, 0x20, 0x3d, 0x20,  
+	0x1c, 0x28, 0x2e, 0x20,  0x2e, 0x20, 0xff, 0x30,  
+	0x23, 0x1d, 0x11, 0x30,  0x83, 0x16, 0x98, 0x00,  
+	0xff, 0x3a, 0x99, 0x00,  0x83, 0x12, 0x2e, 0x20,  
+	0x83, 0x16, 0x98, 0x01,  0x83, 0x12, 0x2e, 0x20,  
+	0x34, 0x08, 0xb6, 0x00,  0x35, 0x08, 0xb7, 0x00,  
+	0x0f, 0x23, 0xb8, 0x00,  0x23, 0x19, 0x79, 0x29,  
+	0x08, 0x30, 0xba, 0x00,  0xb8, 0x1b, 0x6f, 0x29,  
+	0x83, 0x16, 0x18, 0x10,  0x83, 0x12, 0x00, 0x30,  
+	0x73, 0x29, 0x83, 0x16,  0x18, 0x14, 0x83, 0x12,  
+	0x01, 0x30, 0xe7, 0x22,  0x2e, 0x20, 0xb8, 0x0d,  
+	0xba, 0x0b, 0x68, 0x29,  0x9e, 0x29, 0x26, 0x18,  
+	0x8c, 0x29, 0xb8, 0x0e,  0x38, 0x08, 0x83, 0x16,  
+	0x98, 0x00, 0x83, 0x12,  0xa3, 0x1e, 0xe7, 0x22,  
+	0x2e, 0x20, 0xb8, 0x0e,  0x38, 0x08, 0x83, 0x16,  
+	0x98, 0x00, 0x83, 0x12,  0xa3, 0x1e, 0xe7, 0x22,  
+	0x2e, 0x20, 0x9e, 0x29,  0x38, 0x08, 0x83, 0x16,  
+	0x98, 0x00, 0x83, 0x12,  0xb8, 0x0e, 0x38, 0x08,  
+	0xa3, 0x1e, 0xe7, 0x22,  0x2e, 0x20, 0x38, 0x08,  
+	0x83, 0x16, 0x98, 0x00,  0x83, 0x12, 0xb8, 0x0e,  
+	0x38, 0x08, 0xa3, 0x1e,  0xe7, 0x22, 0x2e, 0x20,  
+	0xb6, 0x0b, 0x62, 0x29,  0xb7, 0x0b, 0x62, 0x29,  
+	0xa3, 0x1a, 0xaa, 0x29,  0x10, 0x30, 0xc4, 0x00,  
+	0x03, 0x01, 0xe7, 0x22,  0xc4, 0x0b, 0xa6, 0x29,  
+	0x10, 0x30, 0xc4, 0x00,  0xc5, 0x01, 0xbb, 0x0d,  
+	0xbc, 0x0d, 0x03, 0x18,  0x45, 0x14, 0xbd, 0x0d,  
+	0xbe, 0x0d, 0x03, 0x18,  0xc5, 0x14, 0xbf, 0x0d,  
+	0xc0, 0x0d, 0x03, 0x18,  0x45, 0x15, 0xc1, 0x0d,  
+	0xc2, 0x0d, 0x03, 0x18,  0xc5, 0x15, 0x26, 0x18,  
+	0xc5, 0x0e, 0x45, 0x08,  0x83, 0x16, 0x98, 0x00,  
+	0x83, 0x12, 0x2e, 0x20,  0xc4, 0x0b, 0xac, 0x29,  
+	0x83, 0x16, 0xff, 0x30,  0x98, 0x00, 0x83, 0x12,  
+	0x2e, 0x20, 0x83, 0x16,  0xff, 0x30, 0x99, 0x00,  
+	0x83, 0x12, 0x2e, 0x20,  0x2e, 0x20, 0xc6, 0x01,  
+	0x83, 0x16, 0x10, 0x1c,  0xd7, 0x29, 0x83, 0x12,  
+	0xc6, 0x15, 0x83, 0x12,  0x2e, 0x20, 0x83, 0x16,  
+	0x10, 0x1c, 0xde, 0x29,  0x83, 0x12, 0x46, 0x15,  
+	0x83, 0x12, 0x2e, 0x20,  0x83, 0x16, 0x10, 0x1c,  
+	0xe5, 0x29, 0x83, 0x12,  0xc6, 0x14, 0x83, 0x12,  
+	0x2e, 0x20, 0x83, 0x16,  0x10, 0x1c, 0xec, 0x29,  
+	0x83, 0x12, 0x46, 0x14,  0x83, 0x12, 0x46, 0x08,  
+	0x18, 0x23, 0x2e, 0x20,  0x2e, 0x20, 0x2e, 0x20,  
+	0x83, 0x12, 0x2e, 0x20,  0x83, 0x16, 0x10, 0x1c,  
+	0xf2, 0x29, 0x83, 0x12,  0x2e, 0x20, 0x2e, 0x20,  
+	0x2e, 0x20, 0x1c, 0x28,  0x24, 0x18, 0x01, 0x2a,  
+	0x21, 0x22, 0x2e, 0x20,  0xfc, 0x29, 0x21, 0x22,  
+	0x2e, 0x20, 0x24, 0x18,  0x01, 0x2a, 0x24, 0x1d,  
+	0x0b, 0x2a, 0x2e, 0x20,  0x2e, 0x20, 0x2e, 0x20,  
+	0x1c, 0x28, 0x24, 0x15,  0x01, 0x30, 0x18, 0x23,  
+	0x50, 0x30, 0x84, 0x00,  0x06, 0x30, 0xa3, 0x18,  
+	0x11, 0x30, 0xcf, 0x00,  0x00, 0x08, 0x13, 0x23,  
+	0x84, 0x0a, 0xcf, 0x0b,  0x14, 0x2a, 0x28, 0x08,  
+	0x18, 0x23, 0x23, 0x1a,  0xfc, 0x29, 0x2e, 0x20,  
+	0x2e, 0x20, 0x2e, 0x20,  0x1c, 0x28, 0x24, 0x18,  
+	0x32, 0x2a, 0x83, 0x16,  0x10, 0x18, 0x30, 0x2a,  
+	0x83, 0x12, 0x24, 0x14,  0xa4, 0x14, 0x02, 0x30,  
+	0x18, 0x23, 0x34, 0x08,  0xb6, 0x00, 0x35, 0x08,  
+	0xb7, 0x00, 0x08, 0x00,  0x83, 0x12, 0x08, 0x00,  
+	0x23, 0x19, 0x4d, 0x2a,  0x83, 0x16, 0x03, 0x10,  
+	0x10, 0x18, 0x03, 0x14,  0xa1, 0x0d, 0x03, 0x01,  
+	0x10, 0x18, 0x01, 0x30,  0x83, 0x12, 0xa3, 0x1a,  
+	0x41, 0x2a, 0xa4, 0x18,  0xe7, 0x22, 0xb9, 0x0a,  
+	0xb9, 0x1d, 0x08, 0x00,  0x83, 0x16, 0x21, 0x08,  
+	0x83, 0x12, 0x13, 0x23,  0xb9, 0x01, 0x83, 0x16,  
+	0xa1, 0x01, 0x83, 0x12,  0x73, 0x2a, 0x39, 0x19,  
+	0x62, 0x2a, 0x83, 0x16,  0xa1, 0x01, 0x90, 0x19,  
+	0xa1, 0x15, 0x10, 0x19,  0x21, 0x15, 0x90, 0x18,  
+	0xa1, 0x14, 0x10, 0x18,  0x21, 0x14, 0x21, 0x08,  
+	0xa1, 0x0e, 0x83, 0x12,  0x39, 0x15, 0xa3, 0x1a,  
+	0x08, 0x00, 0xa4, 0x18,  0xe7, 0x22, 0x08, 0x00,  
+	0x83, 0x16, 0x90, 0x19,  0xa1, 0x15, 0x10, 0x19,  
+	0x21, 0x15, 0x90, 0x18,  0xa1, 0x14, 0x10, 0x18,  
+	0x21, 0x14, 0x21, 0x08,  0x83, 0x12, 0x13, 0x23,  
+	0xa3, 0x1a, 0x73, 0x2a,  0xa4, 0x18, 0xe7, 0x22,  
+	0xb9, 0x01, 0xb6, 0x0b,  0x08, 0x00, 0xb7, 0x0b,  
+	0x08, 0x00, 0xa4, 0x1c,  0x81, 0x2a, 0xa4, 0x10,  
+	0x08, 0x30, 0x23, 0x1d,  0x02, 0x30, 0xb6, 0x00,  
+	0x01, 0x30, 0xb7, 0x00,  0x08, 0x00, 0x24, 0x10,  
+	0x23, 0x12, 0xa3, 0x1a,  0xe4, 0x2a, 0x10, 0x30,  
+	0xc4, 0x00, 0x03, 0x01,  0xe7, 0x22, 0xc4, 0x0b,  
+	0x87, 0x2a, 0x23, 0x19,  0x92, 0x2a, 0x3c, 0x08,  
+	0x13, 0x23, 0x3b, 0x08,  0x18, 0x23, 0x08, 0x00,  
+	0x04, 0x08, 0xe7, 0x00,  0x47, 0x30, 0x84, 0x00,  
+	0x08, 0x30, 0xc4, 0x00,  0x80, 0x01, 0x84, 0x0a,  
+	0xc4, 0x0b, 0x98, 0x2a,  0x47, 0x30, 0x84, 0x00,  
+	0x04, 0x30, 0xc4, 0x00,  0xc2, 0x0d, 0x03, 0x18,  
+	0x80, 0x17, 0xc2, 0x0d,  0x03, 0x18, 0x80, 0x15,  
+	0xc0, 0x0d, 0x03, 0x18,  0x00, 0x17, 0xc0, 0x0d,  
+	0x03, 0x18, 0x00, 0x15,  0xbe, 0x0d, 0x03, 0x18,  
+	0x80, 0x16, 0xbe, 0x0d,  0x03, 0x18, 0x80, 0x14,  
+	0xbc, 0x0d, 0x03, 0x18,  0x00, 0x16, 0xbc, 0x0d,  
+	0x03, 0x18, 0x00, 0x14,  0x84, 0x0a, 0xc4, 0x0b,  
+	0xa0, 0x2a, 0x04, 0x30,  0xc4, 0x00, 0xc1, 0x0d,  
+	0x03, 0x18, 0x80, 0x17,  0xc1, 0x0d, 0x03, 0x18,  
+	0x80, 0x15, 0xbf, 0x0d,  0x03, 0x18, 0x00, 0x17,  
+	0xbf, 0x0d, 0x03, 0x18,  0x00, 0x15, 0xbd, 0x0d,  
+	0x03, 0x18, 0x80, 0x16,  0xbd, 0x0d, 0x03, 0x18,  
+	0x80, 0x14, 0xbb, 0x0d,  0x03, 0x18, 0x00, 0x16,  
+	0xbb, 0x0d, 0x03, 0x18,  0x00, 0x14, 0x84, 0x0a,  
+	0xc4, 0x0b, 0xbd, 0x2a,  0x47, 0x30, 0x84, 0x00,  
+	0x07, 0x30, 0xc4, 0x00,  0x00, 0x08, 0x13, 0x23,  
+	0x84, 0x0a, 0xc4, 0x0b,  0xdc, 0x2a, 0x00, 0x08,  
+	0x18, 0x23, 0x08, 0x00,  0x03, 0x01, 0x18, 0x23,  
+	0x08, 0x00, 0xc3, 0x00,  0xc3, 0x0c, 0xbb, 0x0d,  
+	0xbc, 0x0d, 0x03, 0x1c,  0xf1, 0x2a, 0x21, 0x30,  
+	0xbb, 0x06, 0x10, 0x30,  0xbc, 0x06, 0x23, 0x1d,  
+	0x08, 0x00, 0xc3, 0x0c,  0xbd, 0x0d, 0xbe, 0x0d,  
+	0x03, 0x1c, 0xfc, 0x2a,  0x21, 0x30, 0xbd, 0x06,  
+	0x10, 0x30, 0xbe, 0x06,  0xc3, 0x0c, 0xbf, 0x0d,  
+	0xc0, 0x0d, 0x03, 0x1c,  0x05, 0x2b, 0x21, 0x30,  
+	0xbf, 0x06, 0x10, 0x30,  0xc0, 0x06, 0xc3, 0x0c,  
+	0xc1, 0x0d, 0xc2, 0x0d,  0x03, 0x1c, 0x08, 0x00,  
+	0x21, 0x30, 0xc1, 0x06,  0x10, 0x30, 0xc2, 0x06,  
+	0x08, 0x00, 0x8c, 0x19,  0x0f, 0x2b, 0x0f, 0x08,  
+	0x08, 0x00, 0x8c, 0x1b,  0x13, 0x2b, 0x8e, 0x01,  
+	0x8d, 0x00, 0x08, 0x00,  0x8c, 0x1b, 0x18, 0x2b,  
+	0x8e, 0x0a, 0x8d, 0x00,  0x08, 0x00, 0x83, 0x12,  
+	0x20, 0x30, 0x84, 0x00,  0x80, 0x01, 0x84, 0x0a,  
+	0x04, 0x08, 0x80, 0x3c,  0x03, 0x1d, 0x20, 0x2b,  
+	0xa0, 0x30, 0x84, 0x00,  0x80, 0x01, 0x84, 0x0a,  
+	0x04, 0x08, 0xff, 0x3c,  0x03, 0x1d, 0x28, 0x2b,  
+	0x08, 0x00
+};
+
diff --git a/drivers/fims/sdio/fim_sdio0_9210.h b/drivers/fims/sdio/fim_sdio0_9210.h
new file mode 100644
index 000000000000..540583e9f999
--- /dev/null
+++ b/drivers/fims/sdio/fim_sdio0_9210.h
@@ -0,0 +1,197 @@
+
+/*
+ * Automatic generated header file with the firmware code for the FIM
+ * Input binary  : fim_firmware.bin
+ * Output header : fim_sdio_9210.h
+ * Structure     : fim_sdio_firmware0
+ */
+
+static const unsigned char fim_sdio_firmware0[] = { 
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x01, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0xbf, 0x02, 0x00, 0x00,  0x0e, 0x28, 0x00, 0x00,  
+	0x01, 0x00, 0x00, 0x00,  0xa0, 0x00, 0x03, 0x08,  
+	0x83, 0x12, 0xa1, 0x00,  0x8b, 0x10, 0x21, 0x08,  
+	0x83, 0x00, 0xa0, 0x0e,  0x20, 0x0e, 0x09, 0x00,  
+	0x01, 0x30, 0x98, 0x00,  0xad, 0x22, 0x83, 0x16,  
+	0x99, 0x01, 0x19, 0x15,  0x19, 0x14, 0x83, 0x12,  
+	0x17, 0x28, 0xa4, 0x01,  0xbb, 0x01, 0xbc, 0x01,  
+	0xbd, 0x01, 0xbe, 0x01,  0xbf, 0x01, 0xc0, 0x01,  
+	0xc1, 0x01, 0xc2, 0x01,  0x83, 0x16, 0xa1, 0x01,  
+	0x83, 0x12, 0xb9, 0x01,  0x8c, 0x1d, 0x37, 0x28,  
+	0x28, 0x20, 0x17, 0x28,  0x83, 0x16, 0x98, 0x14,  
+	0x83, 0x12, 0x10, 0x08,  0xac, 0x00, 0xac, 0x0b,  
+	0x2d, 0x28, 0x83, 0x16,  0x98, 0x10, 0x83, 0x12,  
+	0x10, 0x08, 0xac, 0x00,  0xac, 0x0b, 0x34, 0x28,  
+	0x08, 0x00, 0x83, 0x16,  0x19, 0x11, 0x83, 0x12,  
+	0xa7, 0x01, 0x93, 0x22,  0xa3, 0x00, 0x93, 0x22,  
+	0xb5, 0x00, 0x93, 0x22,  0xb4, 0x00, 0xff, 0x39,  
+	0x03, 0x19, 0x47, 0x28,  0x35, 0x08, 0x01, 0x3e,  
+	0xb5, 0x00, 0x93, 0x22,  0x5c, 0x20, 0x93, 0x22,  
+	0x5c, 0x20, 0x93, 0x22,  0x5c, 0x20, 0x93, 0x22,  
+	0x5c, 0x20, 0x93, 0x22,  0x5c, 0x20, 0x75, 0x20,  
+	0x27, 0x08, 0x5c, 0x20,  0x83, 0x16, 0x19, 0x15,  
+	0x83, 0x12, 0x23, 0x18,  0x82, 0x28, 0xa3, 0x18,  
+	0x82, 0x28, 0x2d, 0x29,  0xaa, 0x00, 0x08, 0x30,  
+	0xad, 0x00, 0xaa, 0x0d,  0x83, 0x16, 0x03, 0x18,  
+	0x65, 0x28, 0x18, 0x11,  0x66, 0x28, 0x18, 0x15,  
+	0x83, 0x12, 0x6c, 0x20,  0x28, 0x20, 0xad, 0x0b,  
+	0x5f, 0x28, 0x08, 0x00,  0xa7, 0x13, 0x03, 0x18,  
+	0xa7, 0x17, 0xa7, 0x0d,  0xa7, 0x1f, 0x08, 0x00,  
+	0x09, 0x30, 0xa7, 0x06,  0x08, 0x00, 0x07, 0x30,  
+	0xa9, 0x00, 0x03, 0x10,  0xa7, 0x0d, 0xa7, 0x1f,  
+	0x7d, 0x28, 0x09, 0x30,  0xa7, 0x06, 0xa9, 0x0b,  
+	0x77, 0x28, 0x03, 0x14,  0xa7, 0x0d, 0x08, 0x00,  
+	0x64, 0x30, 0xab, 0x00,  0xb0, 0x01, 0xa7, 0x01,  
+	0xab, 0x0b, 0x89, 0x28,  0x03, 0x29, 0x06, 0x21,  
+	0x03, 0x18, 0x86, 0x28,  0x23, 0x1a, 0xc7, 0x28,  
+	0x01, 0x30, 0xa4, 0x22,  0x03, 0x10, 0x14, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0xa3, 0x18,  0xa9, 0x28, 0x75, 0x20,  
+	0x27, 0x08, 0xa8, 0x00,  0x13, 0x21, 0x9b, 0x22,  
+	0x28, 0x08, 0xa3, 0x19,  0xa7, 0x28, 0xa4, 0x22,  
+	0x2d, 0x29, 0x9b, 0x22,  0x2d, 0x29, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x75, 0x20,  0x27, 0x08, 0xa8, 0x00,  
+	0x13, 0x21, 0x9b, 0x22,  0x28, 0x08, 0xa4, 0x22,  
+	0x2d, 0x29, 0x50, 0x30,  0x84, 0x00, 0x14, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x13, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x13, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x13, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x13, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0xa3, 0x1c, 0xfb, 0x28,  
+	0x13, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x13, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x13, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x13, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x13, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x13, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x13, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x13, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x13, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x13, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x13, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x75, 0x20,  0x27, 0x08, 0xa8, 0x00,  
+	0x13, 0x21, 0x80, 0x00,  0x24, 0x18, 0xac, 0x29,  
+	0xbb, 0x29, 0x04, 0x30,  0xa4, 0x22, 0x17, 0x28,  
+	0x23, 0x1a, 0xd1, 0x21,  0x83, 0x16, 0x10, 0x19,  
+	0x0f, 0x29, 0x83, 0x12,  0x28, 0x20, 0x03, 0x10,  
+	0x08, 0x00, 0x83, 0x12,  0x28, 0x20, 0x03, 0x14,  
+	0x08, 0x00, 0x06, 0x21,  0x6c, 0x20, 0xb0, 0x0d,  
+	0x06, 0x21, 0x6c, 0x20,  0xb0, 0x0d, 0x06, 0x21,  
+	0x6c, 0x20, 0xb0, 0x0d,  0x06, 0x21, 0x6c, 0x20,  
+	0xb0, 0x0d, 0x06, 0x21,  0x6c, 0x20, 0xb0, 0x0d,  
+	0x06, 0x21, 0x6c, 0x20,  0xb0, 0x0d, 0x06, 0x21,  
+	0x6c, 0x20, 0xb0, 0x0d,  0x06, 0x21, 0x6c, 0x20,  
+	0xb0, 0x0d, 0x30, 0x08,  0x08, 0x00, 0x23, 0x1a,  
+	0xac, 0x29, 0xa3, 0x19,  0x35, 0x29, 0x28, 0x20,  
+	0x28, 0x20, 0x28, 0x20,  0x17, 0x28, 0x28, 0x20,  
+	0x28, 0x20, 0xff, 0x30,  0x01, 0x30, 0x83, 0x16,  
+	0x18, 0x14, 0x19, 0x10,  0x83, 0x12, 0x28, 0x20,  
+	0x83, 0x16, 0x18, 0x10,  0x83, 0x12, 0x28, 0x20,  
+	0x34, 0x08, 0xb6, 0x00,  0x35, 0x08, 0xb7, 0x00,  
+	0x93, 0x22, 0xb8, 0x00,  0x08, 0x30, 0xba, 0x00,  
+	0xb8, 0x1b, 0x51, 0x29,  0x83, 0x16, 0x18, 0x10,  
+	0x83, 0x12, 0x00, 0x30,  0x55, 0x29, 0x83, 0x16,  
+	0x18, 0x14, 0x83, 0x12,  0x01, 0x30, 0x6c, 0x22,  
+	0x28, 0x20, 0xb8, 0x0d,  0xba, 0x0b, 0x4a, 0x29,  
+	0x5b, 0x29, 0xb6, 0x0b,  0x46, 0x29, 0xb7, 0x0b,  
+	0x46, 0x29, 0xa3, 0x1a,  0x67, 0x29, 0x10, 0x30,  
+	0xc4, 0x00, 0x03, 0x01,  0x6c, 0x22, 0xc4, 0x0b,  
+	0x63, 0x29, 0x10, 0x30,  0xc4, 0x00, 0xc5, 0x01,  
+	0xbb, 0x0d, 0xbc, 0x0d,  0x03, 0x18, 0x72, 0x29,  
+	0x83, 0x16, 0x18, 0x10,  0x83, 0x12, 0x75, 0x29,  
+	0x83, 0x16, 0x18, 0x14,  0x83, 0x12, 0x28, 0x20,  
+	0xc4, 0x0b, 0x69, 0x29,  0x83, 0x16, 0x18, 0x14,  
+	0x83, 0x12, 0x28, 0x20,  0x83, 0x16, 0x19, 0x14,  
+	0x83, 0x12, 0x28, 0x20,  0x28, 0x20, 0xc6, 0x01,  
+	0x83, 0x16, 0x10, 0x1c,  0x87, 0x29, 0x83, 0x12,  
+	0xc6, 0x15, 0x83, 0x12,  0x28, 0x20, 0x83, 0x16,  
+	0x10, 0x1c, 0x8e, 0x29,  0x83, 0x12, 0x46, 0x15,  
+	0x83, 0x12, 0x28, 0x20,  0x83, 0x16, 0x10, 0x1c,  
+	0x95, 0x29, 0x83, 0x12,  0xc6, 0x14, 0x83, 0x12,  
+	0x28, 0x20, 0x83, 0x16,  0x10, 0x1c, 0x9c, 0x29,  
+	0x83, 0x12, 0x46, 0x14,  0x83, 0x12, 0x46, 0x08,  
+	0xa4, 0x22, 0x28, 0x20,  0x28, 0x20, 0x28, 0x20,  
+	0x83, 0x12, 0x28, 0x20,  0x83, 0x16, 0x10, 0x1c,  
+	0xa2, 0x29, 0x83, 0x12,  0x28, 0x20, 0x28, 0x20,  
+	0x28, 0x20, 0x17, 0x28,  0x24, 0x18, 0xb1, 0x29,  
+	0xd1, 0x21, 0x28, 0x20,  0xac, 0x29, 0xd1, 0x21,  
+	0x28, 0x20, 0x24, 0x18,  0xb1, 0x29, 0x24, 0x1d,  
+	0xbb, 0x29, 0x28, 0x20,  0x28, 0x20, 0x28, 0x20,  
+	0x17, 0x28, 0x24, 0x15,  0x01, 0x30, 0xa4, 0x22,  
+	0x50, 0x30, 0x84, 0x00,  0x06, 0x30, 0xa3, 0x18,  
+	0x11, 0x30, 0xcf, 0x00,  0x00, 0x08, 0x9b, 0x22,  
+	0x84, 0x0a, 0xcf, 0x0b,  0xc4, 0x29, 0x28, 0x08,  
+	0xa4, 0x22, 0x23, 0x1a,  0xac, 0x29, 0x28, 0x20,  
+	0x28, 0x20, 0x28, 0x20,  0x17, 0x28, 0x24, 0x18,  
+	0xe2, 0x29, 0x83, 0x16,  0x10, 0x18, 0xe0, 0x29,  
+	0x83, 0x12, 0x24, 0x14,  0xa4, 0x14, 0x02, 0x30,  
+	0xa4, 0x22, 0x34, 0x08,  0xb6, 0x00, 0x35, 0x08,  
+	0xb7, 0x00, 0x08, 0x00,  0x83, 0x12, 0x08, 0x00,  
+	0x83, 0x16, 0x03, 0x10,  0x10, 0x18, 0x03, 0x14,  
+	0xa1, 0x0d, 0x03, 0x01,  0x10, 0x18, 0x01, 0x30,  
+	0x83, 0x12, 0xa3, 0x1a,  0xef, 0x29, 0xa4, 0x18,  
+	0x6c, 0x22, 0xb9, 0x0a,  0xb9, 0x1d, 0x08, 0x00,  
+	0x83, 0x16, 0x21, 0x08,  0x83, 0x12, 0x9b, 0x22,  
+	0xb9, 0x01, 0x83, 0x16,  0xa1, 0x01, 0x83, 0x12,  
+	0xfb, 0x29, 0xb6, 0x0b,  0x08, 0x00, 0xb7, 0x0b,  
+	0x08, 0x00, 0xa4, 0x1c,  0x08, 0x2a, 0xa4, 0x10,  
+	0x08, 0x30, 0x02, 0x30,  0xb6, 0x00, 0x01, 0x30,  
+	0xb7, 0x00, 0x08, 0x00,  0x24, 0x10, 0x23, 0x12,  
+	0xa3, 0x1a, 0x69, 0x2a,  0x10, 0x30, 0xc4, 0x00,  
+	0x03, 0x01, 0x6c, 0x22,  0xc4, 0x0b, 0x0e, 0x2a,  
+	0x3c, 0x08, 0x9b, 0x22,  0x3b, 0x08, 0xa4, 0x22,  
+	0x08, 0x00, 0x04, 0x08,  0xe7, 0x00, 0x47, 0x30,  
+	0x84, 0x00, 0x08, 0x30,  0xc4, 0x00, 0x80, 0x01,  
+	0x84, 0x0a, 0xc4, 0x0b,  0x1d, 0x2a, 0x47, 0x30,  
+	0x84, 0x00, 0x04, 0x30,  0xc4, 0x00, 0xc2, 0x0d,  
+	0x03, 0x18, 0x80, 0x17,  0xc2, 0x0d, 0x03, 0x18,  
+	0x80, 0x15, 0xc0, 0x0d,  0x03, 0x18, 0x00, 0x17,  
+	0xc0, 0x0d, 0x03, 0x18,  0x00, 0x15, 0xbe, 0x0d,  
+	0x03, 0x18, 0x80, 0x16,  0xbe, 0x0d, 0x03, 0x18,  
+	0x80, 0x14, 0xbc, 0x0d,  0x03, 0x18, 0x00, 0x16,  
+	0xbc, 0x0d, 0x03, 0x18,  0x00, 0x14, 0x84, 0x0a,  
+	0xc4, 0x0b, 0x25, 0x2a,  0x04, 0x30, 0xc4, 0x00,  
+	0xc1, 0x0d, 0x03, 0x18,  0x80, 0x17, 0xc1, 0x0d,  
+	0x03, 0x18, 0x80, 0x15,  0xbf, 0x0d, 0x03, 0x18,  
+	0x00, 0x17, 0xbf, 0x0d,  0x03, 0x18, 0x00, 0x15,  
+	0xbd, 0x0d, 0x03, 0x18,  0x80, 0x16, 0xbd, 0x0d,  
+	0x03, 0x18, 0x80, 0x14,  0xbb, 0x0d, 0x03, 0x18,  
+	0x00, 0x16, 0xbb, 0x0d,  0x03, 0x18, 0x00, 0x14,  
+	0x84, 0x0a, 0xc4, 0x0b,  0x42, 0x2a, 0x47, 0x30,  
+	0x84, 0x00, 0x07, 0x30,  0xc4, 0x00, 0x00, 0x08,  
+	0x9b, 0x22, 0x84, 0x0a,  0xc4, 0x0b, 0x61, 0x2a,  
+	0x00, 0x08, 0xa4, 0x22,  0x08, 0x00, 0x03, 0x01,  
+	0xa4, 0x22, 0x08, 0x00,  0xc3, 0x00, 0xc3, 0x0c,  
+	0xbb, 0x0d, 0xbc, 0x0d,  0x03, 0x1c, 0x76, 0x2a,  
+	0x21, 0x30, 0xbb, 0x06,  0x10, 0x30, 0xbc, 0x06,  
+	0x08, 0x00, 0xc3, 0x0c,  0xbd, 0x0d, 0xbe, 0x0d,  
+	0x03, 0x1c, 0x80, 0x2a,  0x21, 0x30, 0xbd, 0x06,  
+	0x10, 0x30, 0xbe, 0x06,  0xc3, 0x0c, 0xbf, 0x0d,  
+	0xc0, 0x0d, 0x03, 0x1c,  0x89, 0x2a, 0x21, 0x30,  
+	0xbf, 0x06, 0x10, 0x30,  0xc0, 0x06, 0xc3, 0x0c,  
+	0xc1, 0x0d, 0xc2, 0x0d,  0x03, 0x1c, 0x08, 0x00,  
+	0x21, 0x30, 0xc1, 0x06,  0x10, 0x30, 0xc2, 0x06,  
+	0x08, 0x00, 0x8c, 0x19,  0x93, 0x2a, 0x8b, 0x13,  
+	0x00, 0x00, 0x0f, 0x08,  0x00, 0x00, 0x8b, 0x17,  
+	0x08, 0x00, 0x8c, 0x1b,  0x9b, 0x2a, 0x8b, 0x13,  
+	0x00, 0x00, 0x8e, 0x01,  0x8d, 0x00, 0x00, 0x00,  
+	0x8b, 0x17, 0x08, 0x00,  0x8c, 0x1b, 0xa4, 0x2a,  
+	0x8b, 0x13, 0x00, 0x00,  0x8e, 0x0a, 0x8d, 0x00,  
+	0x00, 0x00, 0x8b, 0x17,  0x08, 0x00, 0x83, 0x12,  
+	0x20, 0x30, 0x84, 0x00,  0x80, 0x01, 0x84, 0x0a,  
+	0x04, 0x08, 0x80, 0x3c,  0x03, 0x1d, 0xb0, 0x2a,  
+	0xa0, 0x30, 0x84, 0x00,  0x80, 0x01, 0x84, 0x0a,  
+	0x04, 0x08, 0xff, 0x3c,  0x03, 0x1d, 0xb8, 0x2a,  
+	0x08, 0x00
+};
+
diff --git a/drivers/fims/sdio/fim_sdio1.h b/drivers/fims/sdio/fim_sdio1.h
new file mode 100644
index 000000000000..7f73a0c5fc3e
--- /dev/null
+++ b/drivers/fims/sdio/fim_sdio1.h
@@ -0,0 +1,225 @@
+
+/*
+ * Automatic generated header file with the firmware code for the FIM
+ * Input binary  : fim_firmware.bin
+ * Output header : fim_sdio.h
+ * Structure     : fim_sdio_firmware1
+ */
+
+static const unsigned char fim_sdio_firmware1[] = { 
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x01, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x2f, 0x03, 0x00, 0x00,  0x0e, 0x28, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0xa0, 0x00, 0x03, 0x08,  
+	0x83, 0x12, 0xa1, 0x00,  0x8b, 0x10, 0x21, 0x08,  
+	0x83, 0x00, 0xa0, 0x0e,  0x20, 0x0e, 0x09, 0x00,  
+	0x1d, 0x23, 0x83, 0x12,  0x9f, 0x01, 0x9f, 0x15,  
+	0x83, 0x16, 0xff, 0x30,  0x99, 0x00, 0x83, 0x12,  
+	0x83, 0x12, 0x15, 0x1c,  0x16, 0x28, 0x01, 0x30,  
+	0xa6, 0x00, 0x1c, 0x28,  0xa4, 0x01, 0xbb, 0x01,  
+	0xbc, 0x01, 0xbd, 0x01,  0xbe, 0x01, 0xbf, 0x01,  
+	0xc0, 0x01, 0xc1, 0x01,  0xc2, 0x01, 0x83, 0x16,  
+	0xa1, 0x01, 0x83, 0x12,  0xb9, 0x01, 0x3d, 0x20,  
+	0x8c, 0x1d, 0x54, 0x28,  0x2e, 0x20, 0x1c, 0x28,  
+	0x83, 0x16, 0x1a, 0x15,  0x83, 0x12, 0x10, 0x08,  
+	0xac, 0x00, 0xac, 0x0b,  0x33, 0x28, 0x83, 0x16,  
+	0x1a, 0x11, 0x83, 0x12,  0x10, 0x08, 0xac, 0x00,  
+	0xac, 0x0b, 0x3a, 0x28,  0x08, 0x00, 0x22, 0x1c,  
+	0x48, 0x28, 0x83, 0x16,  0x89, 0x1f, 0x46, 0x28,  
+	0x83, 0x12, 0x89, 0x01,  0x22, 0x10, 0x08, 0x00,  
+	0x83, 0x12, 0x08, 0x00,  0x11, 0x1c, 0x08, 0x00,  
+	0x83, 0x16, 0x90, 0x1e,  0x4f, 0x28, 0x83, 0x12,  
+	0x08, 0x00, 0x83, 0x12,  0x22, 0x14, 0x01, 0x30,  
+	0x89, 0x00, 0x08, 0x00,  0x9f, 0x11, 0xa7, 0x01,  
+	0x0f, 0x23, 0xa3, 0x00,  0x0f, 0x23, 0xb5, 0x00,  
+	0x0f, 0x23, 0xb4, 0x00,  0xff, 0x39, 0x03, 0x19,  
+	0x62, 0x28, 0x35, 0x08,  0x01, 0x3e, 0xb5, 0x00,  
+	0x0f, 0x23, 0x75, 0x20,  0x0f, 0x23, 0x75, 0x20,  
+	0x0f, 0x23, 0x75, 0x20,  0x0f, 0x23, 0x75, 0x20,  
+	0x0f, 0x23, 0x75, 0x20,  0x8e, 0x20, 0x27, 0x08,  
+	0x75, 0x20, 0x9f, 0x15,  0x23, 0x18, 0x9b, 0x28,  
+	0xa3, 0x18, 0x9b, 0x28,  0x46, 0x29, 0xaa, 0x00,  
+	0x08, 0x30, 0xad, 0x00,  0xaa, 0x0d, 0x83, 0x16,  
+	0x03, 0x18, 0x7e, 0x28,  0x9a, 0x11, 0x7f, 0x28,  
+	0x9a, 0x15, 0x83, 0x12,  0x85, 0x20, 0x2e, 0x20,  
+	0xad, 0x0b, 0x78, 0x28,  0x08, 0x00, 0xa7, 0x13,  
+	0x03, 0x18, 0xa7, 0x17,  0xa7, 0x0d, 0xa7, 0x1f,  
+	0x08, 0x00, 0x09, 0x30,  0xa7, 0x06, 0x08, 0x00,  
+	0x07, 0x30, 0xa9, 0x00,  0x03, 0x10, 0xa7, 0x0d,  
+	0xa7, 0x1f, 0x96, 0x28,  0x09, 0x30, 0xa7, 0x06,  
+	0xa9, 0x0b, 0x90, 0x28,  0x03, 0x14, 0xa7, 0x0d,  
+	0x08, 0x00, 0x64, 0x30,  0xab, 0x00, 0xb0, 0x01,  
+	0xa7, 0x01, 0xab, 0x0b,  0xa2, 0x28, 0x1c, 0x29,  
+	0x1f, 0x21, 0x03, 0x18,  0x9f, 0x28, 0x23, 0x1a,  
+	0xe0, 0x28, 0x01, 0x30,  0x18, 0x23, 0x03, 0x10,  
+	0x2d, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0xa3, 0x18, 0xc2, 0x28,  
+	0x8e, 0x20, 0x27, 0x08,  0xa8, 0x00, 0x2c, 0x21,  
+	0x13, 0x23, 0x28, 0x08,  0xa3, 0x19, 0xc0, 0x28,  
+	0x18, 0x23, 0x46, 0x29,  0x13, 0x23, 0x46, 0x29,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x2c, 0x21, 0x13, 0x23,  
+	0x2c, 0x21, 0x13, 0x23,  0x8e, 0x20, 0x27, 0x08,  
+	0xa8, 0x00, 0x2c, 0x21,  0x13, 0x23, 0x28, 0x08,  
+	0x18, 0x23, 0x46, 0x29,  0x50, 0x30, 0x84, 0x00,  
+	0x2d, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x2c, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x2c, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x2c, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x2c, 0x21, 0x80, 0x00,  0x84, 0x0a, 0xa3, 0x1c,  
+	0x14, 0x29, 0x2c, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x2c, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x2c, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x2c, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x2c, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x2c, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x2c, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x2c, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x2c, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x2c, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x2c, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x8e, 0x20, 0x27, 0x08,  
+	0xa8, 0x00, 0x2c, 0x21,  0x80, 0x00, 0x24, 0x18,  
+	0xfc, 0x29, 0x0b, 0x2a,  0x04, 0x30, 0x18, 0x23,  
+	0x1c, 0x28, 0x23, 0x1a,  0x21, 0x22, 0x83, 0x16,  
+	0x92, 0x19, 0x28, 0x29,  0x83, 0x12, 0x2e, 0x20,  
+	0x03, 0x10, 0x08, 0x00,  0x83, 0x12, 0x2e, 0x20,  
+	0x03, 0x14, 0x08, 0x00,  0x1f, 0x21, 0x85, 0x20,  
+	0xb0, 0x0d, 0x1f, 0x21,  0x85, 0x20, 0xb0, 0x0d,  
+	0x1f, 0x21, 0x85, 0x20,  0xb0, 0x0d, 0x1f, 0x21,  
+	0x85, 0x20, 0xb0, 0x0d,  0x1f, 0x21, 0x85, 0x20,  
+	0xb0, 0x0d, 0x1f, 0x21,  0x85, 0x20, 0xb0, 0x0d,  
+	0x1f, 0x21, 0x85, 0x20,  0xb0, 0x0d, 0x1f, 0x21,  
+	0x85, 0x20, 0xb0, 0x0d,  0x30, 0x08, 0x08, 0x00,  
+	0x23, 0x1a, 0xfc, 0x29,  0xa3, 0x19, 0x4f, 0x29,  
+	0x2e, 0x20, 0x2e, 0x20,  0x2e, 0x20, 0x3d, 0x20,  
+	0x1c, 0x28, 0x2e, 0x20,  0x2e, 0x20, 0xff, 0x30,  
+	0x23, 0x1d, 0x11, 0x30,  0x83, 0x16, 0x98, 0x00,  
+	0xff, 0x3a, 0x99, 0x00,  0x83, 0x12, 0x2e, 0x20,  
+	0x83, 0x16, 0x98, 0x01,  0x83, 0x12, 0x2e, 0x20,  
+	0x34, 0x08, 0xb6, 0x00,  0x35, 0x08, 0xb7, 0x00,  
+	0x0f, 0x23, 0xb8, 0x00,  0x23, 0x19, 0x79, 0x29,  
+	0x08, 0x30, 0xba, 0x00,  0xb8, 0x1b, 0x6f, 0x29,  
+	0x83, 0x16, 0x18, 0x12,  0x83, 0x12, 0x00, 0x30,  
+	0x73, 0x29, 0x83, 0x16,  0x18, 0x16, 0x83, 0x12,  
+	0x01, 0x30, 0xe7, 0x22,  0x2e, 0x20, 0xb8, 0x0d,  
+	0xba, 0x0b, 0x68, 0x29,  0x9e, 0x29, 0x26, 0x18,  
+	0x8c, 0x29, 0xb8, 0x0e,  0x38, 0x08, 0x83, 0x16,  
+	0x98, 0x00, 0x83, 0x12,  0xa3, 0x1e, 0xe7, 0x22,  
+	0x2e, 0x20, 0xb8, 0x0e,  0x38, 0x08, 0x83, 0x16,  
+	0x98, 0x00, 0x83, 0x12,  0xa3, 0x1e, 0xe7, 0x22,  
+	0x2e, 0x20, 0x9e, 0x29,  0x38, 0x08, 0x83, 0x16,  
+	0x98, 0x00, 0x83, 0x12,  0xb8, 0x0e, 0x38, 0x08,  
+	0xa3, 0x1e, 0xe7, 0x22,  0x2e, 0x20, 0x38, 0x08,  
+	0x83, 0x16, 0x98, 0x00,  0x83, 0x12, 0xb8, 0x0e,  
+	0x38, 0x08, 0xa3, 0x1e,  0xe7, 0x22, 0x2e, 0x20,  
+	0xb6, 0x0b, 0x62, 0x29,  0xb7, 0x0b, 0x62, 0x29,  
+	0xa3, 0x1a, 0xaa, 0x29,  0x10, 0x30, 0xc4, 0x00,  
+	0x03, 0x01, 0xe7, 0x22,  0xc4, 0x0b, 0xa6, 0x29,  
+	0x10, 0x30, 0xc4, 0x00,  0xc5, 0x01, 0xbb, 0x0d,  
+	0xbc, 0x0d, 0x03, 0x18,  0x45, 0x14, 0xbd, 0x0d,  
+	0xbe, 0x0d, 0x03, 0x18,  0xc5, 0x14, 0xbf, 0x0d,  
+	0xc0, 0x0d, 0x03, 0x18,  0x45, 0x15, 0xc1, 0x0d,  
+	0xc2, 0x0d, 0x03, 0x18,  0xc5, 0x15, 0x26, 0x18,  
+	0xc5, 0x0e, 0x45, 0x08,  0x83, 0x16, 0x98, 0x00,  
+	0x83, 0x12, 0x2e, 0x20,  0xc4, 0x0b, 0xac, 0x29,  
+	0x83, 0x16, 0xff, 0x30,  0x98, 0x00, 0x83, 0x12,  
+	0x2e, 0x20, 0x83, 0x16,  0xff, 0x30, 0x99, 0x00,  
+	0x83, 0x12, 0x2e, 0x20,  0x2e, 0x20, 0xc6, 0x01,  
+	0x83, 0x16, 0x10, 0x1e,  0xd7, 0x29, 0x83, 0x12,  
+	0xc6, 0x15, 0x83, 0x12,  0x2e, 0x20, 0x83, 0x16,  
+	0x10, 0x1e, 0xde, 0x29,  0x83, 0x12, 0x46, 0x15,  
+	0x83, 0x12, 0x2e, 0x20,  0x83, 0x16, 0x10, 0x1e,  
+	0xe5, 0x29, 0x83, 0x12,  0xc6, 0x14, 0x83, 0x12,  
+	0x2e, 0x20, 0x83, 0x16,  0x10, 0x1e, 0xec, 0x29,  
+	0x83, 0x12, 0x46, 0x14,  0x83, 0x12, 0x46, 0x08,  
+	0x18, 0x23, 0x2e, 0x20,  0x2e, 0x20, 0x2e, 0x20,  
+	0x83, 0x12, 0x2e, 0x20,  0x83, 0x16, 0x10, 0x1e,  
+	0xf2, 0x29, 0x83, 0x12,  0x2e, 0x20, 0x2e, 0x20,  
+	0x2e, 0x20, 0x1c, 0x28,  0x24, 0x18, 0x01, 0x2a,  
+	0x21, 0x22, 0x2e, 0x20,  0xfc, 0x29, 0x21, 0x22,  
+	0x2e, 0x20, 0x24, 0x18,  0x01, 0x2a, 0x24, 0x1d,  
+	0x0b, 0x2a, 0x2e, 0x20,  0x2e, 0x20, 0x2e, 0x20,  
+	0x1c, 0x28, 0x24, 0x15,  0x01, 0x30, 0x18, 0x23,  
+	0x50, 0x30, 0x84, 0x00,  0x06, 0x30, 0xa3, 0x18,  
+	0x11, 0x30, 0xcf, 0x00,  0x00, 0x08, 0x13, 0x23,  
+	0x84, 0x0a, 0xcf, 0x0b,  0x14, 0x2a, 0x28, 0x08,  
+	0x18, 0x23, 0x23, 0x1a,  0xfc, 0x29, 0x2e, 0x20,  
+	0x2e, 0x20, 0x2e, 0x20,  0x1c, 0x28, 0x24, 0x18,  
+	0x32, 0x2a, 0x83, 0x16,  0x10, 0x1a, 0x30, 0x2a,  
+	0x83, 0x12, 0x24, 0x14,  0xa4, 0x14, 0x02, 0x30,  
+	0x18, 0x23, 0x34, 0x08,  0xb6, 0x00, 0x35, 0x08,  
+	0xb7, 0x00, 0x08, 0x00,  0x83, 0x12, 0x08, 0x00,  
+	0x23, 0x19, 0x4d, 0x2a,  0x83, 0x16, 0x03, 0x10,  
+	0x10, 0x1a, 0x03, 0x14,  0xa1, 0x0d, 0x03, 0x01,  
+	0x10, 0x1a, 0x01, 0x30,  0x83, 0x12, 0xa3, 0x1a,  
+	0x41, 0x2a, 0xa4, 0x18,  0xe7, 0x22, 0xb9, 0x0a,  
+	0xb9, 0x1d, 0x08, 0x00,  0x83, 0x16, 0x21, 0x08,  
+	0x83, 0x12, 0x13, 0x23,  0xb9, 0x01, 0x83, 0x16,  
+	0xa1, 0x01, 0x83, 0x12,  0x73, 0x2a, 0x39, 0x19,  
+	0x62, 0x2a, 0x83, 0x16,  0xa1, 0x01, 0x90, 0x1b,  
+	0xa1, 0x15, 0x10, 0x1b,  0x21, 0x15, 0x90, 0x1a,  
+	0xa1, 0x14, 0x10, 0x1a,  0x21, 0x14, 0x21, 0x08,  
+	0xa1, 0x0e, 0x83, 0x12,  0x39, 0x15, 0xa3, 0x1a,  
+	0x08, 0x00, 0xa4, 0x18,  0xe7, 0x22, 0x08, 0x00,  
+	0x83, 0x16, 0x90, 0x1b,  0xa1, 0x15, 0x10, 0x1b,  
+	0x21, 0x15, 0x90, 0x1a,  0xa1, 0x14, 0x10, 0x1a,  
+	0x21, 0x14, 0x21, 0x08,  0x83, 0x12, 0x13, 0x23,  
+	0xa3, 0x1a, 0x73, 0x2a,  0xa4, 0x18, 0xe7, 0x22,  
+	0xb9, 0x01, 0xb6, 0x0b,  0x08, 0x00, 0xb7, 0x0b,  
+	0x08, 0x00, 0xa4, 0x1c,  0x81, 0x2a, 0xa4, 0x10,  
+	0x08, 0x30, 0x23, 0x1d,  0x02, 0x30, 0xb6, 0x00,  
+	0x01, 0x30, 0xb7, 0x00,  0x08, 0x00, 0x24, 0x10,  
+	0x23, 0x12, 0xa3, 0x1a,  0xe4, 0x2a, 0x10, 0x30,  
+	0xc4, 0x00, 0x03, 0x01,  0xe7, 0x22, 0xc4, 0x0b,  
+	0x87, 0x2a, 0x23, 0x19,  0x92, 0x2a, 0x3c, 0x08,  
+	0x13, 0x23, 0x3b, 0x08,  0x18, 0x23, 0x08, 0x00,  
+	0x04, 0x08, 0xe7, 0x00,  0x47, 0x30, 0x84, 0x00,  
+	0x08, 0x30, 0xc4, 0x00,  0x80, 0x01, 0x84, 0x0a,  
+	0xc4, 0x0b, 0x98, 0x2a,  0x47, 0x30, 0x84, 0x00,  
+	0x04, 0x30, 0xc4, 0x00,  0xc2, 0x0d, 0x03, 0x18,  
+	0x80, 0x17, 0xc2, 0x0d,  0x03, 0x18, 0x80, 0x15,  
+	0xc0, 0x0d, 0x03, 0x18,  0x00, 0x17, 0xc0, 0x0d,  
+	0x03, 0x18, 0x00, 0x15,  0xbe, 0x0d, 0x03, 0x18,  
+	0x80, 0x16, 0xbe, 0x0d,  0x03, 0x18, 0x80, 0x14,  
+	0xbc, 0x0d, 0x03, 0x18,  0x00, 0x16, 0xbc, 0x0d,  
+	0x03, 0x18, 0x00, 0x14,  0x84, 0x0a, 0xc4, 0x0b,  
+	0xa0, 0x2a, 0x04, 0x30,  0xc4, 0x00, 0xc1, 0x0d,  
+	0x03, 0x18, 0x80, 0x17,  0xc1, 0x0d, 0x03, 0x18,  
+	0x80, 0x15, 0xbf, 0x0d,  0x03, 0x18, 0x00, 0x17,  
+	0xbf, 0x0d, 0x03, 0x18,  0x00, 0x15, 0xbd, 0x0d,  
+	0x03, 0x18, 0x80, 0x16,  0xbd, 0x0d, 0x03, 0x18,  
+	0x80, 0x14, 0xbb, 0x0d,  0x03, 0x18, 0x00, 0x16,  
+	0xbb, 0x0d, 0x03, 0x18,  0x00, 0x14, 0x84, 0x0a,  
+	0xc4, 0x0b, 0xbd, 0x2a,  0x47, 0x30, 0x84, 0x00,  
+	0x07, 0x30, 0xc4, 0x00,  0x00, 0x08, 0x13, 0x23,  
+	0x84, 0x0a, 0xc4, 0x0b,  0xdc, 0x2a, 0x00, 0x08,  
+	0x18, 0x23, 0x08, 0x00,  0x03, 0x01, 0x18, 0x23,  
+	0x08, 0x00, 0xc3, 0x00,  0xc3, 0x0c, 0xbb, 0x0d,  
+	0xbc, 0x0d, 0x03, 0x1c,  0xf1, 0x2a, 0x21, 0x30,  
+	0xbb, 0x06, 0x10, 0x30,  0xbc, 0x06, 0x23, 0x1d,  
+	0x08, 0x00, 0xc3, 0x0c,  0xbd, 0x0d, 0xbe, 0x0d,  
+	0x03, 0x1c, 0xfc, 0x2a,  0x21, 0x30, 0xbd, 0x06,  
+	0x10, 0x30, 0xbe, 0x06,  0xc3, 0x0c, 0xbf, 0x0d,  
+	0xc0, 0x0d, 0x03, 0x1c,  0x05, 0x2b, 0x21, 0x30,  
+	0xbf, 0x06, 0x10, 0x30,  0xc0, 0x06, 0xc3, 0x0c,  
+	0xc1, 0x0d, 0xc2, 0x0d,  0x03, 0x1c, 0x08, 0x00,  
+	0x21, 0x30, 0xc1, 0x06,  0x10, 0x30, 0xc2, 0x06,  
+	0x08, 0x00, 0x8c, 0x19,  0x0f, 0x2b, 0x0f, 0x08,  
+	0x08, 0x00, 0x8c, 0x1b,  0x13, 0x2b, 0x8e, 0x01,  
+	0x8d, 0x00, 0x08, 0x00,  0x8c, 0x1b, 0x18, 0x2b,  
+	0x8e, 0x0a, 0x8d, 0x00,  0x08, 0x00, 0x83, 0x12,  
+	0x20, 0x30, 0x84, 0x00,  0x80, 0x01, 0x84, 0x0a,  
+	0x04, 0x08, 0x80, 0x3c,  0x03, 0x1d, 0x20, 0x2b,  
+	0xa0, 0x30, 0x84, 0x00,  0x80, 0x01, 0x84, 0x0a,  
+	0x04, 0x08, 0xff, 0x3c,  0x03, 0x1d, 0x28, 0x2b,  
+	0x08, 0x00
+};
+
diff --git a/drivers/fims/sdio/fim_sdio1_9210.h b/drivers/fims/sdio/fim_sdio1_9210.h
new file mode 100644
index 000000000000..cfb920660d69
--- /dev/null
+++ b/drivers/fims/sdio/fim_sdio1_9210.h
@@ -0,0 +1,197 @@
+
+/*
+ * Automatic generated header file with the firmware code for the FIM
+ * Input binary  : fim_firmware.bin
+ * Output header : fim_sdio_9210.h
+ * Structure     : fim_sdio_firmware1
+ */
+
+static const unsigned char fim_sdio_firmware1[] = { 
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x01, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0xbf, 0x02, 0x00, 0x00,  0x0e, 0x28, 0x00, 0x00,  
+	0x01, 0x00, 0x00, 0x00,  0xa0, 0x00, 0x03, 0x08,  
+	0x83, 0x12, 0xa1, 0x00,  0x8b, 0x10, 0x21, 0x08,  
+	0x83, 0x00, 0xa0, 0x0e,  0x20, 0x0e, 0x09, 0x00,  
+	0x01, 0x30, 0x98, 0x00,  0xad, 0x22, 0x83, 0x16,  
+	0x99, 0x01, 0x19, 0x15,  0x19, 0x14, 0x83, 0x12,  
+	0x17, 0x28, 0xa4, 0x01,  0xbb, 0x01, 0xbc, 0x01,  
+	0xbd, 0x01, 0xbe, 0x01,  0xbf, 0x01, 0xc0, 0x01,  
+	0xc1, 0x01, 0xc2, 0x01,  0x83, 0x16, 0xa1, 0x01,  
+	0x83, 0x12, 0xb9, 0x01,  0x8c, 0x1d, 0x37, 0x28,  
+	0x28, 0x20, 0x17, 0x28,  0x83, 0x16, 0x98, 0x14,  
+	0x83, 0x12, 0x10, 0x08,  0xac, 0x00, 0xac, 0x0b,  
+	0x2d, 0x28, 0x83, 0x16,  0x98, 0x10, 0x83, 0x12,  
+	0x10, 0x08, 0xac, 0x00,  0xac, 0x0b, 0x34, 0x28,  
+	0x08, 0x00, 0x83, 0x16,  0x19, 0x11, 0x83, 0x12,  
+	0xa7, 0x01, 0x93, 0x22,  0xa3, 0x00, 0x93, 0x22,  
+	0xb5, 0x00, 0x93, 0x22,  0xb4, 0x00, 0xff, 0x39,  
+	0x03, 0x19, 0x47, 0x28,  0x35, 0x08, 0x01, 0x3e,  
+	0xb5, 0x00, 0x93, 0x22,  0x5c, 0x20, 0x93, 0x22,  
+	0x5c, 0x20, 0x93, 0x22,  0x5c, 0x20, 0x93, 0x22,  
+	0x5c, 0x20, 0x93, 0x22,  0x5c, 0x20, 0x75, 0x20,  
+	0x27, 0x08, 0x5c, 0x20,  0x83, 0x16, 0x19, 0x15,  
+	0x83, 0x12, 0x23, 0x18,  0x82, 0x28, 0xa3, 0x18,  
+	0x82, 0x28, 0x2d, 0x29,  0xaa, 0x00, 0x08, 0x30,  
+	0xad, 0x00, 0xaa, 0x0d,  0x83, 0x16, 0x03, 0x18,  
+	0x65, 0x28, 0x18, 0x11,  0x66, 0x28, 0x18, 0x15,  
+	0x83, 0x12, 0x6c, 0x20,  0x28, 0x20, 0xad, 0x0b,  
+	0x5f, 0x28, 0x08, 0x00,  0xa7, 0x13, 0x03, 0x18,  
+	0xa7, 0x17, 0xa7, 0x0d,  0xa7, 0x1f, 0x08, 0x00,  
+	0x09, 0x30, 0xa7, 0x06,  0x08, 0x00, 0x07, 0x30,  
+	0xa9, 0x00, 0x03, 0x10,  0xa7, 0x0d, 0xa7, 0x1f,  
+	0x7d, 0x28, 0x09, 0x30,  0xa7, 0x06, 0xa9, 0x0b,  
+	0x77, 0x28, 0x03, 0x14,  0xa7, 0x0d, 0x08, 0x00,  
+	0x64, 0x30, 0xab, 0x00,  0xb0, 0x01, 0xa7, 0x01,  
+	0xab, 0x0b, 0x89, 0x28,  0x03, 0x29, 0x06, 0x21,  
+	0x03, 0x18, 0x86, 0x28,  0x23, 0x1a, 0xc7, 0x28,  
+	0x01, 0x30, 0xa4, 0x22,  0x03, 0x10, 0x14, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0xa3, 0x18,  0xa9, 0x28, 0x75, 0x20,  
+	0x27, 0x08, 0xa8, 0x00,  0x13, 0x21, 0x9b, 0x22,  
+	0x28, 0x08, 0xa3, 0x19,  0xa7, 0x28, 0xa4, 0x22,  
+	0x2d, 0x29, 0x9b, 0x22,  0x2d, 0x29, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x13, 0x21,  0x9b, 0x22, 0x13, 0x21,  
+	0x9b, 0x22, 0x75, 0x20,  0x27, 0x08, 0xa8, 0x00,  
+	0x13, 0x21, 0x9b, 0x22,  0x28, 0x08, 0xa4, 0x22,  
+	0x2d, 0x29, 0x50, 0x30,  0x84, 0x00, 0x14, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x13, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x13, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x13, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x13, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0xa3, 0x1c, 0xfb, 0x28,  
+	0x13, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x13, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x13, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x13, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x13, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x13, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x13, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x13, 0x21,  0x80, 0x00, 0x84, 0x0a,  
+	0x13, 0x21, 0x80, 0x00,  0x84, 0x0a, 0x13, 0x21,  
+	0x80, 0x00, 0x84, 0x0a,  0x13, 0x21, 0x80, 0x00,  
+	0x84, 0x0a, 0x75, 0x20,  0x27, 0x08, 0xa8, 0x00,  
+	0x13, 0x21, 0x80, 0x00,  0x24, 0x18, 0xac, 0x29,  
+	0xbb, 0x29, 0x04, 0x30,  0xa4, 0x22, 0x17, 0x28,  
+	0x23, 0x1a, 0xd1, 0x21,  0x83, 0x16, 0x10, 0x19,  
+	0x0f, 0x29, 0x83, 0x12,  0x28, 0x20, 0x03, 0x10,  
+	0x08, 0x00, 0x83, 0x12,  0x28, 0x20, 0x03, 0x14,  
+	0x08, 0x00, 0x06, 0x21,  0x6c, 0x20, 0xb0, 0x0d,  
+	0x06, 0x21, 0x6c, 0x20,  0xb0, 0x0d, 0x06, 0x21,  
+	0x6c, 0x20, 0xb0, 0x0d,  0x06, 0x21, 0x6c, 0x20,  
+	0xb0, 0x0d, 0x06, 0x21,  0x6c, 0x20, 0xb0, 0x0d,  
+	0x06, 0x21, 0x6c, 0x20,  0xb0, 0x0d, 0x06, 0x21,  
+	0x6c, 0x20, 0xb0, 0x0d,  0x06, 0x21, 0x6c, 0x20,  
+	0xb0, 0x0d, 0x30, 0x08,  0x08, 0x00, 0x23, 0x1a,  
+	0xac, 0x29, 0xa3, 0x19,  0x35, 0x29, 0x28, 0x20,  
+	0x28, 0x20, 0x28, 0x20,  0x17, 0x28, 0x28, 0x20,  
+	0x28, 0x20, 0xff, 0x30,  0x01, 0x30, 0x83, 0x16,  
+	0x18, 0x14, 0x19, 0x10,  0x83, 0x12, 0x28, 0x20,  
+	0x83, 0x16, 0x18, 0x10,  0x83, 0x12, 0x28, 0x20,  
+	0x34, 0x08, 0xb6, 0x00,  0x35, 0x08, 0xb7, 0x00,  
+	0x93, 0x22, 0xb8, 0x00,  0x08, 0x30, 0xba, 0x00,  
+	0xb8, 0x1b, 0x51, 0x29,  0x83, 0x16, 0x18, 0x10,  
+	0x83, 0x12, 0x00, 0x30,  0x55, 0x29, 0x83, 0x16,  
+	0x18, 0x14, 0x83, 0x12,  0x01, 0x30, 0x6c, 0x22,  
+	0x28, 0x20, 0xb8, 0x0d,  0xba, 0x0b, 0x4a, 0x29,  
+	0x5b, 0x29, 0xb6, 0x0b,  0x46, 0x29, 0xb7, 0x0b,  
+	0x46, 0x29, 0xa3, 0x1a,  0x67, 0x29, 0x10, 0x30,  
+	0xc4, 0x00, 0x03, 0x01,  0x6c, 0x22, 0xc4, 0x0b,  
+	0x63, 0x29, 0x10, 0x30,  0xc4, 0x00, 0xc5, 0x01,  
+	0xbb, 0x0d, 0xbc, 0x0d,  0x03, 0x18, 0x72, 0x29,  
+	0x83, 0x16, 0x18, 0x10,  0x83, 0x12, 0x75, 0x29,  
+	0x83, 0x16, 0x18, 0x14,  0x83, 0x12, 0x28, 0x20,  
+	0xc4, 0x0b, 0x69, 0x29,  0x83, 0x16, 0x18, 0x14,  
+	0x83, 0x12, 0x28, 0x20,  0x83, 0x16, 0x19, 0x14,  
+	0x83, 0x12, 0x28, 0x20,  0x28, 0x20, 0xc6, 0x01,  
+	0x83, 0x16, 0x10, 0x1c,  0x87, 0x29, 0x83, 0x12,  
+	0xc6, 0x15, 0x83, 0x12,  0x28, 0x20, 0x83, 0x16,  
+	0x10, 0x1c, 0x8e, 0x29,  0x83, 0x12, 0x46, 0x15,  
+	0x83, 0x12, 0x28, 0x20,  0x83, 0x16, 0x10, 0x1c,  
+	0x95, 0x29, 0x83, 0x12,  0xc6, 0x14, 0x83, 0x12,  
+	0x28, 0x20, 0x83, 0x16,  0x10, 0x1c, 0x9c, 0x29,  
+	0x83, 0x12, 0x46, 0x14,  0x83, 0x12, 0x46, 0x08,  
+	0xa4, 0x22, 0x28, 0x20,  0x28, 0x20, 0x28, 0x20,  
+	0x83, 0x12, 0x28, 0x20,  0x83, 0x16, 0x10, 0x1c,  
+	0xa2, 0x29, 0x83, 0x12,  0x28, 0x20, 0x28, 0x20,  
+	0x28, 0x20, 0x17, 0x28,  0x24, 0x18, 0xb1, 0x29,  
+	0xd1, 0x21, 0x28, 0x20,  0xac, 0x29, 0xd1, 0x21,  
+	0x28, 0x20, 0x24, 0x18,  0xb1, 0x29, 0x24, 0x1d,  
+	0xbb, 0x29, 0x28, 0x20,  0x28, 0x20, 0x28, 0x20,  
+	0x17, 0x28, 0x24, 0x15,  0x01, 0x30, 0xa4, 0x22,  
+	0x50, 0x30, 0x84, 0x00,  0x06, 0x30, 0xa3, 0x18,  
+	0x11, 0x30, 0xcf, 0x00,  0x00, 0x08, 0x9b, 0x22,  
+	0x84, 0x0a, 0xcf, 0x0b,  0xc4, 0x29, 0x28, 0x08,  
+	0xa4, 0x22, 0x23, 0x1a,  0xac, 0x29, 0x28, 0x20,  
+	0x28, 0x20, 0x28, 0x20,  0x17, 0x28, 0x24, 0x18,  
+	0xe2, 0x29, 0x83, 0x16,  0x10, 0x18, 0xe0, 0x29,  
+	0x83, 0x12, 0x24, 0x14,  0xa4, 0x14, 0x02, 0x30,  
+	0xa4, 0x22, 0x34, 0x08,  0xb6, 0x00, 0x35, 0x08,  
+	0xb7, 0x00, 0x08, 0x00,  0x83, 0x12, 0x08, 0x00,  
+	0x83, 0x16, 0x03, 0x10,  0x10, 0x18, 0x03, 0x14,  
+	0xa1, 0x0d, 0x03, 0x01,  0x10, 0x18, 0x01, 0x30,  
+	0x83, 0x12, 0xa3, 0x1a,  0xef, 0x29, 0xa4, 0x18,  
+	0x6c, 0x22, 0xb9, 0x0a,  0xb9, 0x1d, 0x08, 0x00,  
+	0x83, 0x16, 0x21, 0x08,  0x83, 0x12, 0x9b, 0x22,  
+	0xb9, 0x01, 0x83, 0x16,  0xa1, 0x01, 0x83, 0x12,  
+	0xfb, 0x29, 0xb6, 0x0b,  0x08, 0x00, 0xb7, 0x0b,  
+	0x08, 0x00, 0xa4, 0x1c,  0x08, 0x2a, 0xa4, 0x10,  
+	0x08, 0x30, 0x02, 0x30,  0xb6, 0x00, 0x01, 0x30,  
+	0xb7, 0x00, 0x08, 0x00,  0x24, 0x10, 0x23, 0x12,  
+	0xa3, 0x1a, 0x69, 0x2a,  0x10, 0x30, 0xc4, 0x00,  
+	0x03, 0x01, 0x6c, 0x22,  0xc4, 0x0b, 0x0e, 0x2a,  
+	0x3c, 0x08, 0x9b, 0x22,  0x3b, 0x08, 0xa4, 0x22,  
+	0x08, 0x00, 0x04, 0x08,  0xe7, 0x00, 0x47, 0x30,  
+	0x84, 0x00, 0x08, 0x30,  0xc4, 0x00, 0x80, 0x01,  
+	0x84, 0x0a, 0xc4, 0x0b,  0x1d, 0x2a, 0x47, 0x30,  
+	0x84, 0x00, 0x04, 0x30,  0xc4, 0x00, 0xc2, 0x0d,  
+	0x03, 0x18, 0x80, 0x17,  0xc2, 0x0d, 0x03, 0x18,  
+	0x80, 0x15, 0xc0, 0x0d,  0x03, 0x18, 0x00, 0x17,  
+	0xc0, 0x0d, 0x03, 0x18,  0x00, 0x15, 0xbe, 0x0d,  
+	0x03, 0x18, 0x80, 0x16,  0xbe, 0x0d, 0x03, 0x18,  
+	0x80, 0x14, 0xbc, 0x0d,  0x03, 0x18, 0x00, 0x16,  
+	0xbc, 0x0d, 0x03, 0x18,  0x00, 0x14, 0x84, 0x0a,  
+	0xc4, 0x0b, 0x25, 0x2a,  0x04, 0x30, 0xc4, 0x00,  
+	0xc1, 0x0d, 0x03, 0x18,  0x80, 0x17, 0xc1, 0x0d,  
+	0x03, 0x18, 0x80, 0x15,  0xbf, 0x0d, 0x03, 0x18,  
+	0x00, 0x17, 0xbf, 0x0d,  0x03, 0x18, 0x00, 0x15,  
+	0xbd, 0x0d, 0x03, 0x18,  0x80, 0x16, 0xbd, 0x0d,  
+	0x03, 0x18, 0x80, 0x14,  0xbb, 0x0d, 0x03, 0x18,  
+	0x00, 0x16, 0xbb, 0x0d,  0x03, 0x18, 0x00, 0x14,  
+	0x84, 0x0a, 0xc4, 0x0b,  0x42, 0x2a, 0x47, 0x30,  
+	0x84, 0x00, 0x07, 0x30,  0xc4, 0x00, 0x00, 0x08,  
+	0x9b, 0x22, 0x84, 0x0a,  0xc4, 0x0b, 0x61, 0x2a,  
+	0x00, 0x08, 0xa4, 0x22,  0x08, 0x00, 0x03, 0x01,  
+	0xa4, 0x22, 0x08, 0x00,  0xc3, 0x00, 0xc3, 0x0c,  
+	0xbb, 0x0d, 0xbc, 0x0d,  0x03, 0x1c, 0x76, 0x2a,  
+	0x21, 0x30, 0xbb, 0x06,  0x10, 0x30, 0xbc, 0x06,  
+	0x08, 0x00, 0xc3, 0x0c,  0xbd, 0x0d, 0xbe, 0x0d,  
+	0x03, 0x1c, 0x80, 0x2a,  0x21, 0x30, 0xbd, 0x06,  
+	0x10, 0x30, 0xbe, 0x06,  0xc3, 0x0c, 0xbf, 0x0d,  
+	0xc0, 0x0d, 0x03, 0x1c,  0x89, 0x2a, 0x21, 0x30,  
+	0xbf, 0x06, 0x10, 0x30,  0xc0, 0x06, 0xc3, 0x0c,  
+	0xc1, 0x0d, 0xc2, 0x0d,  0x03, 0x1c, 0x08, 0x00,  
+	0x21, 0x30, 0xc1, 0x06,  0x10, 0x30, 0xc2, 0x06,  
+	0x08, 0x00, 0x8c, 0x19,  0x93, 0x2a, 0x8b, 0x13,  
+	0x00, 0x00, 0x0f, 0x08,  0x00, 0x00, 0x8b, 0x17,  
+	0x08, 0x00, 0x8c, 0x1b,  0x9b, 0x2a, 0x8b, 0x13,  
+	0x00, 0x00, 0x8e, 0x01,  0x8d, 0x00, 0x00, 0x00,  
+	0x8b, 0x17, 0x08, 0x00,  0x8c, 0x1b, 0xa4, 0x2a,  
+	0x8b, 0x13, 0x00, 0x00,  0x8e, 0x0a, 0x8d, 0x00,  
+	0x00, 0x00, 0x8b, 0x17,  0x08, 0x00, 0x83, 0x12,  
+	0x20, 0x30, 0x84, 0x00,  0x80, 0x01, 0x84, 0x0a,  
+	0x04, 0x08, 0x80, 0x3c,  0x03, 0x1d, 0xb0, 0x2a,  
+	0xa0, 0x30, 0x84, 0x00,  0x80, 0x01, 0x84, 0x0a,  
+	0x04, 0x08, 0xff, 0x3c,  0x03, 0x1d, 0xb8, 0x2a,  
+	0x08, 0x00
+};
+
diff --git a/drivers/fims/serial/Makefile b/drivers/fims/serial/Makefile
new file mode 100644
index 000000000000..936afd535a80
--- /dev/null
+++ b/drivers/fims/serial/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for the kernel FIM device drivers
+#
+
+ifneq ($(CONFIG_FIM_SERIAL_MODULE),)
+	obj-m				+= fim_serial.o
+else
+	obj-$(CONFIG_FIM_ZERO_SERIAL)	+= fim_serial.o
+	obj-$(CONFIG_FIM_ONE_SERIAL)	+= fim_serial.o
+endif
\ No newline at end of file
diff --git a/drivers/fims/serial/fim_serial.c b/drivers/fims/serial/fim_serial.c
new file mode 100644
index 000000000000..8795e19020c6
--- /dev/null
+++ b/drivers/fims/serial/fim_serial.c
@@ -0,0 +1,1654 @@
+/* -*- linux-c -*-
+ *
+ * drivers/fims/serial/fim_serial.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ *  !Revision:   $Revision: 1.1.1.1 $
+ *  !Author:     Luis Galdos
+ *  !Descr:      
+ *  !References:
+ */
+
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/console.h>
+#include <linux/serial.h>
+#include <linux/serial_core.h>
+#include <linux/tty.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/init.h>
+#include <asm/io.h>
+#include <linux/clk.h>
+#include <asm/delay.h>
+
+#include <mach/gpio.h>
+#include <mach/regs-sys-ns921x.h>
+#include <mach/regs-iohub-ns921x.h>
+
+/* For registering the FIM-driver */
+#include <mach/fim-ns921x.h>
+
+/*
+ * If the driver is being compiled as a built-in driver, then include the header file
+ * which contains the firmware for this FIM-device
+ */
+#if !defined(MODULE)
+#include "fim_serial.h"
+extern const unsigned char fim_serial_firmware[];
+#define FIM_SERIAL_FIRMWARE_FILE		(NULL)
+#define FIM_SERIAL_FIRMWARE_CODE		fim_serial_firmware
+#else
+const unsigned char *fim_serial_firmware = NULL;
+#define FIM_SERIAL_FIRMWARE_FILE		"fim_serial.bin"
+#define FIM_SERIAL_FIRMWARE_CODE		(NULL)
+#endif
+
+/* Driver informations */
+#define DRIVER_VERSION				"0.2"
+#define DRIVER_AUTHOR				"Silvano Najera, Luis Galdos"
+#define DRIVER_DESC				"FIM serial driver"
+#define FIM_DRIVER_NAME				"fim-serial"
+#define FIM_PLATFORM_DRIVER_NAME		"platform:fim-serial"
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRIVER_VERSION);
+
+/* Module parameter for selection the FIMs */
+NS921X_FIM_NUMBERS_PARAM(fims_number);
+
+/* Firmware-dependent interrupts from the ARM to the FIM */
+#define FIM_SERIAL_INT_INSERT_CHAR		0x01
+#define FIM_SERIAL_INT_MATCH_CHAR		0x02
+#define FIM_SERIAL_INT_BITS_CHAR		0x04
+#define FIM_SERIAL_INT_CHAR_GAP 		0x08
+#define FIM_SERIAL_INT_BIT_POS			0x10
+#define FIM_SERIAL_INT_PRESCALE			0x20
+#define FIM_SERIAL_INT_BIT_TIME 		0x40
+
+/* Interrupts from the FIM to the driver */
+#define FIM_INT_FROM_MATCH_CHAR1		0x01
+#define FIM_INT_FROM_MATCH_CHAR2		0x02
+#define FIM_INT_FROM_RX_OVERFLOW		0x04
+
+/* FIM status */
+#define FIM_SERIAL_STAT_COMPLETE		0x01
+#define FIM_SERIAL_STAT_TX_ENABLE		0x02
+#define FIM_SERIAL_STAT_HW_FLOW 		0x04
+#define FIM_SERIAL_STAT_MATCH_CHAR1		0x08
+#define FIM_SERIAL_STAT_MATCH_CHAR2		0x10
+
+/* Special control registers */
+#define FIM_SERIAL_TXIO_REG			0
+#define FIM_SERIAL_RXIO_REG			1
+#define FIM_SERIAL_RTSIO_REG			2
+#define FIM_SERIAL_CTSIO_REG			3
+#define FIM_SERIAL_CTRL_REG			6
+
+/* This is used only for the internal FIM-buffers */
+#define FIM_SERIAL_BUF_PRIVATE			(void *)(0xffffffff)
+
+/*
+ * IMPORTANT: The number of chars per DMA-transfer must be low, otherwise
+ * the latency times by user-interrupts (CTRL-C) will be too high (some seconds!)
+ * The problem is that we can't stop the DMA-transfer in an easy way and the
+ * FIM-firmware can't be stopped on the fly. 
+ */
+#define FIM_SERIAL_TX_CHARS_PER_DMA		32
+#define FIM_SERIAL_TX_DMA_BUFFER_SIZE		(FIM_SERIAL_TX_CHARS_PER_DMA * 2)
+#define FIM_SERIAL_RX_DMA_BUFFER_SIZE		(PAGE_SIZE / 8)
+#define FIM_SERIAL_TX_DMA_BUFFERS		64
+#define FIM_SERIAL_RX_DMA_BUFFERS		32
+
+#define FIM_SERIAL_GPIO_RX			(0)
+#define FIM_SERIAL_GPIO_TX			(1)
+#define FIM_SERIAL_GPIO_CTS			(2)
+#define FIM_SERIAL_GPIO_RTS			(3)
+#define FIM_SERIAL_GPIO_LAST			(4)
+
+#define printk_err(fmt, args...)                printk(KERN_DEBUG "[ ERROR ] fim-serial: " fmt, ## args)
+#define printk_info(fmt, args...)               printk(KERN_DEBUG "fim-serial: " fmt, ## args)
+#define printk_dbg(fmt, args...)                printk(KERN_DEBUG "fim-serial: " fmt, ## args)
+
+#define print_parse_err(parse, port, raw, pos)	printk_debug(parse " | Byte %i | Raw %x | Data %i | Total %i | Mask %x\n", pos, raw, port->numbits, port->totalbits, port->bitsmask)
+
+#if 0
+#define FIM_SERIAL_DEBUG
+#endif
+
+#ifdef FIM_SERIAL_DEBUG
+#  define printk_debug(fmt, args...)		printk(KERN_DEBUG "fim-serial: " fmt, ## args)
+#else
+#  define printk_debug(fmt, args...)
+#endif
+
+#define FIM_CONSOLE_NAME			"ttyFIM"
+#define FIM_SERIAL_DEV_NAME			"ttyFIM"
+
+/* @XXX: Remove this macro (currently used for the aximal number of GPIOs) */
+#define FIM_SERIAL_MAX_GPIOS			(8)
+
+/*
+ * Masks for the incoming PIC-data. The definition are coming from the Net+OS driver
+ * and are in some cases unknown, then there is no exact documentation in the
+ * PIC-code that defines the bit fields of the data bytes
+ */
+#define FIM_DATA_PARENB(data, port)             ((data >> port->numbits) & 0x01)
+#define FIM_DATA_STOPB(data, port)              ((data >> (port->totalbits - 2)) & 0x03)
+#define FIM_DATA_BITS(data, port)               ((data >> (port->totalbits - 1)) & 0x01)
+
+
+/* Due to the fact that we can handle only one port per FIM */
+struct fim_serial_t {
+	dev_t devnr;
+	struct fim_driver fim;
+	struct uart_port uart;
+	struct uart_driver *driver;
+	struct device *device;
+	int minor;
+	int index;
+	int open_count;
+	int configured;
+	unsigned int totalbits;
+	unsigned short bitsmask;
+	unsigned short numbits;
+	unsigned int cflag;
+	unsigned int baud;
+	unsigned int last_totalbits;
+	struct fim_gpio_t gpios[FIM_SERIAL_MAX_GPIOS];
+	struct clk *sys_clk;
+	spinlock_t tx_lock;
+	struct tasklet_struct tasklet;
+	int reg;
+	struct ktermios *termios;
+};
+
+/* Main structure for the port-handling */
+struct fim_serials_t {
+	struct uart_driver driver;
+	struct fim_serial_t *ports;
+	int fims;
+};
+
+static struct fim_serials_t *fim_serials;
+
+/* Firmware-dependent interrupts from the ARM to the FIM */
+#define FIM_SERIAL_INT_INSERT_CHAR		0x01
+#define FIM_SERIAL_INT_MATCH_CHAR		0x02
+#define FIM_SERIAL_INT_BITS_CHAR		0x04
+#define FIM_SERIAL_INT_CHAR_GAP 		0x08
+#define FIM_SERIAL_INT_BIT_POS			0x10
+#define FIM_SERIAL_INT_PRESCALE			0x20
+#define FIM_SERIAL_INT_BIT_TIME 		0x40
+
+/* Interrupts from the FIM to the driver */
+#define FIM_INT_FROM_MATCH_CHAR1		0x01
+#define FIM_INT_FROM_MATCH_CHAR2		0x02
+#define FIM_INT_FROM_RX_OVERFLOW		0x04
+
+
+/* FIM status */
+#define FIM_SERIAL_STAT_COMPLETE		0x01
+#define FIM_SERIAL_STAT_TX_ENABLE		0x02
+#define FIM_SERIAL_STAT_HW_FLOW 		0x04
+#define FIM_SERIAL_STAT_MATCH_CHAR1		0x08
+#define FIM_SERIAL_STAT_MATCH_CHAR2		0x10
+
+inline static struct fim_serial_t *get_port_from_uart(struct uart_port *uart)
+{
+	return dev_get_drvdata(uart->dev);
+}
+
+
+inline static struct uart_port *get_uart_from_port(struct fim_serial_t *port)
+{
+	return &port->uart;
+}
+
+inline static struct tty_struct *get_tty_from_port(struct fim_serial_t *port)
+{
+	struct uart_info *info;
+
+	info = port->uart.info;
+	return (info) ? info->port.tty : NULL;
+}
+
+inline static struct fim_serial_t *get_port_by_index(int index)
+{
+	if (index < 0 || index > fim_serials->fims)
+		return NULL;
+	
+	return fim_serials->ports + index;
+}
+
+inline static int fim_serial_reset_matchs(struct fim_driver *fim)
+{
+	fim_set_ctrl_reg(fim, 0, 0xFF);
+	fim_set_ctrl_reg(fim, 1, 0xFF);
+	fim_set_ctrl_reg(fim, 2, 0xFF);
+	fim_set_ctrl_reg(fim, 3, 0xFF);
+	fim_set_ctrl_reg(fim, 4, 0x00);
+	fim_set_ctrl_reg(fim, 5, 0x00);
+	return fim_send_interrupt2(fim, FIM_SERIAL_INT_MATCH_CHAR);
+}
+
+inline static int fim_serial_reset_all(struct fim_driver *fim)
+{
+	int retval;
+	
+	fim_set_ctrl_reg(fim, 0, 0);
+	fim_set_ctrl_reg(fim, 1, 0);
+	fim_set_ctrl_reg(fim, 2, 0);
+	fim_set_ctrl_reg(fim, 3, 0);
+	retval = fim_send_interrupt2(fim, FIM_SERIAL_INT_BIT_POS);
+	if (!retval)
+		retval = fim_serial_reset_matchs(fim);
+
+	return retval;
+}
+
+
+static unsigned char get_parity(unsigned char data, unsigned char mask, int odd) 
+{ 
+	unsigned char ones = 0;
+	mask = (mask >> 1) + 1;
+	do {
+		ones += (data & mask) ? 1 : 0;
+		mask >>= 1;
+	} while (mask);
+	if (odd)
+		return ((ones & 1) ^ 1);
+	else
+		return (ones & 1);
+}
+
+/*
+ * Send a char over an interrupt
+ * Only implemented for sending the XOFF/XON chars. Otherwise use the send buffer
+ * function, which uses the DMA-channel for a performanter data transfer
+ */
+static int fim_serial_send_char(struct fim_serial_t *port, unsigned char ch)
+{
+	unsigned int status;
+	int retval;
+	struct fim_driver *fim;
+	unsigned char data = 1;
+	unsigned int timeout;
+	struct tty_struct *tty;
+
+	fim = &port->fim;
+	if (!(tty = get_tty_from_port(port))) {
+		printk_err("No TTY-port found for the FIM %i\n", fim->picnr);
+		return -ENODEV;
+	}
+	
+	/* First check if the FIM is tasked with another send-char request */
+	timeout = 0xFFFF;
+	do {
+		timeout--;
+		fim_get_exp_reg(fim, 0, &status);
+	} while (timeout && (status & FIM_SERIAL_INT_INSERT_CHAR));
+	
+	if (!timeout) {
+		printk_err("Timeout by sending a char over the FIM %i\n", fim->picnr);
+		return -EAGAIN;
+	}
+	
+	if (C_CSTOPB(tty))
+		data = (data << 1) | 0x01;
+	
+	if (C_PARENB(tty))
+		data = (data << 1) | get_parity(ch, port->bitsmask, C_PARODD(tty));
+
+	data = (data << port->numbits) | (ch & port->bitsmask);
+
+	/* And send the char using the interrupt function */
+	fim_set_ctrl_reg(fim, 0, data & 0xFF);
+	fim_set_ctrl_reg(fim, 1, (data >> 8) & 0xFF);
+	retval = fim_send_interrupt2(fim, FIM_SERIAL_INT_INSERT_CHAR);
+	if (retval)
+		printk_err("Send char by the FIM %i failed, %i\n", fim->picnr, retval);
+
+	return retval;
+}
+
+/*
+ * Return value of this function is the number of copied bytes from `src' to `dest'
+ */
+static int fim_serial_parse_data(struct tty_struct *tty,
+				 struct fim_serial_t *port,
+				 unsigned char *src, int length,
+				 unsigned char *dest)
+{
+	unsigned short data;
+	int copied;
+	struct uart_port *uart;
+
+	uart = get_uart_from_port(port);
+	for (copied=0; length > 1; length -= 2) {
+		if (port->totalbits < 8) {
+			data = *src >> (8 - port->totalbits);
+			src++;
+		} else {
+			data = *src;
+			src++;
+			data |= (((unsigned short)*src >> (16 - port->totalbits)) << 8);
+		}
+
+		/* Ignore VSTART and VSTOP characters if hadshake is enabled */
+		if (I_IXOFF(tty)) {
+			if ((data & port->bitsmask) == START_CHAR(tty) ||
+			    (data & port->bitsmask) == STOP_CHAR(tty)) {
+				src++;
+				length -= 2;
+				continue;
+			}
+		}
+
+		*dest = data & port->bitsmask;
+
+		if (C_PARENB(tty)) {
+			if (FIM_DATA_PARENB(data, port) != get_parity(*dest,
+								      port->bitsmask,
+								      C_PARODD(tty))) {
+				uart->icount.parity++;
+ 				print_parse_err("Parity", port, data, copied);
+				continue;
+			}
+		}
+
+		if (C_CSTOPB(tty)) {
+			if (FIM_DATA_STOPB(data, port) != 0x03) {
+
+				/* @BUG: With 5N2 the stop bit flag is incorrect! */
+				if (port->totalbits == 8) {
+					uart->icount.frame++;
+					print_parse_err("Stop  ", port, data, copied);
+					continue;
+				}
+			}
+		} else {
+			if (!FIM_DATA_BITS(data, port)) {
+				uart->icount.frame++;
+ 				print_parse_err("Data  ", port, data, copied);
+				continue;
+			}
+		}
+
+		src++;
+		dest++;
+		copied++;
+	}	
+
+	return copied;
+}
+
+/*
+ * The following function is coming from the Net+OS driver
+ * This functions depends on the number of total bits too. For this reason is
+ * required to call it if the number of bits changes
+ */
+static int fim_serial_baudrate(struct fim_serial_t *port, struct ktermios *termios)
+{
+	unsigned int div, prescale = 1;
+	unsigned long clock;
+	int baud, cnt, retval;
+	struct fim_driver *fim;
+	unsigned int gap;
+	unsigned int bit_time;
+	unsigned int cflag;
+
+	fim = &port->fim;
+	cflag = termios->c_cflag;
+	baud = tty_termios_baud_rate(termios);
+	
+	/* Check if really need to change something */	
+	if (port->baud == baud) {
+		if (port->last_totalbits != port->totalbits) {
+			printk_debug("Setting only the gap timer (%iBps | %i | %i)\n",
+				     baud, port->totalbits, port->last_totalbits);
+			clock = clk_get_rate(port->sys_clk) / baud;
+			goto set_char_gap;
+		}
+		return 0;
+	}
+	
+	clock = clk_get_rate(port->sys_clk);
+	printk_debug("Obtained SYS clock is %luHz\n", clock);
+	clock = clock / baud;
+	div = (clock / 256) + 1;
+
+	/* Must round up to next power of 2 (see NET+OS driver) */
+	for (cnt = 1; cnt <= 8; cnt++) {
+		if (div < (unsigned int)(1 << cnt)) {
+			div = 1 << cnt;
+			prescale = cnt - 1;
+			break;
+		}
+	}
+
+	/* This sanity check is coming from the Net+OS driver */
+	if (cnt > 8) {
+		printk_debug("Wrong prescale value %i by the baud %i\n", cnt, baud);
+		return -EINVAL;
+	}
+
+	/* The Net+OS driver has another calculation of the bit time */
+	bit_time = (clock / div) - 1;
+	
+	printk_debug("%iB | Pre %i | Btime %u | Tbits %i | Dbits %i | S %i | Pa %i\n",
+		     baud, prescale, bit_time, port->totalbits, port->numbits,
+		     cflag & CSTOPB, cflag & PARENB);
+
+	/* Set the bit time */
+	fim_set_ctrl_reg(fim, 0, bit_time);
+	retval = fim_send_interrupt2(fim, FIM_SERIAL_INT_BIT_TIME);
+	if (retval) {
+		printk_err("Couldn't set the bit time of the FIM %i\n", fim->picnr);
+		return -EAGAIN;
+	}
+
+	/* Set the prescale value */
+	fim_set_ctrl_reg(fim, 0, prescale);
+	retval = fim_send_interrupt2(fim, FIM_SERIAL_INT_PRESCALE);
+	if (retval) {
+		printk_err("Prescale configuration of the FIM %i failed.\n",
+			   fim->picnr);
+		return retval;
+	}
+	
+ set_char_gap:
+	gap = (2 * clock * port->totalbits)/30;
+	fim_set_ctrl_reg(fim, 0, (gap & 0xFF) + 1);
+	fim_set_ctrl_reg(fim, 1, ((gap >> 8) & 0xFF) + 1);
+	retval = fim_send_interrupt2(fim, FIM_SERIAL_INT_CHAR_GAP);
+	if (retval) {
+		printk_err("GAP timer by the FIM %i (Total bytes %i)\n",
+			   fim->picnr, port->totalbits);
+		return retval;
+	}
+	
+	port->baud = baud;
+	port->last_totalbits = port->totalbits;
+	return 0;
+}
+
+static int fim_sw_flowctrl(struct fim_serial_t *port, struct ktermios *termios)
+{
+	struct fim_driver *fim;
+	int retval;
+	unsigned short start_match, stop_match;	
+	unsigned short mask;
+	unsigned int cflag, iflag;
+	
+	cflag = termios->c_cflag;
+	iflag = termios->c_iflag;
+	
+	fim = &port->fim;
+	start_match = 1;
+	stop_match = 1;
+
+	printk_debug("Calling %s\n", __func__);
+	
+	/* Check if need to set the SW flow control, otherwise reset it */
+	if (iflag & IXON || iflag & IXOFF) {
+		printk_debug("Active SW flow control? @XXX: Test it first.\n");
+		if (cflag & CSTOPB) {
+			start_match = (start_match << 1) | 1;
+			stop_match = (stop_match << 1) | 1;
+		}
+			
+		if (cflag & PARENB) {
+
+			start_match = (start_match << 1) |
+				get_parity(termios->c_cc[VSTART],
+					   port->bitsmask,
+					   cflag & PARENB & PARODD);
+			
+			stop_match = (stop_match << 1) |
+				get_parity(termios->c_cc[VSTOP],
+					   port->bitsmask,
+					   cflag & PARENB & PARODD);
+		}
+		
+		mask = start_match | port->bitsmask;
+
+		if (port->totalbits < 8) {
+			start_match = start_match << (8 - port->totalbits);
+			stop_match = stop_match << (8 - port->totalbits);
+			mask = port->bitsmask << (8 - port->totalbits);
+		}
+
+		fim_set_ctrl_reg(fim, 0, start_match & 0xff);
+		fim_set_ctrl_reg(fim, 1, 0x00);
+		fim_set_ctrl_reg(fim, 2, stop_match & 0xff);
+		fim_set_ctrl_reg(fim, 3, 0x00);
+		fim_set_ctrl_reg(fim, 4, mask & 0xff);
+		fim_set_ctrl_reg(fim, 5, 0x00);
+
+	} else {
+		fim_serial_reset_matchs(fim);
+	}
+	
+	/* Now send the interrupt for the SW flow control */
+	retval = fim_send_interrupt2(fim, FIM_SERIAL_INT_MATCH_CHAR);
+	if (retval) {
+		printk_err("Couldn't set the bits match\n");
+		return retval;
+	}
+	
+	return 0;
+}
+
+/* Function for the setup of the bit positions in the FIM-firmware */
+static int fim_serial_setup_bitpos(struct fim_serial_t *port, ulong hwflow)
+{
+	struct fim_driver *fim;
+	struct fim_gpio_t *gpios;
+	unsigned int cts, rts;
+	int offset;
+
+	/* Depending on the processor we have different offset */
+	if (processor_is_ns9215())
+		offset = 68;
+	else if (processor_is_ns9210())
+		offset = 0;
+	else
+		return -EINVAL;
+
+	fim = &port->fim;
+	gpios = port->gpios;
+
+	/* Set the TX- and RX-line */
+	printk_debug("TX %i | RX %i | RTS %i | CTS %i | Offset %i\n",
+		     gpios[FIM_SERIAL_GPIO_TX].nr, gpios[FIM_SERIAL_GPIO_RX].nr,
+		     gpios[FIM_SERIAL_GPIO_RTS].nr, gpios[FIM_SERIAL_GPIO_CTS].nr,
+		     offset);
+		
+	fim_set_ctrl_reg(fim, FIM_SERIAL_TXIO_REG,
+			 1 << (gpios[FIM_SERIAL_GPIO_TX].nr - offset));
+	
+	fim_set_ctrl_reg(fim, FIM_SERIAL_RXIO_REG,
+			 1 << (gpios[FIM_SERIAL_GPIO_RX].nr - offset));
+
+	/* Check if the GPIOs are available for the HW flow control */
+	if (hwflow && (gpios[FIM_SERIAL_GPIO_CTS].nr == FIM_GPIO_DONT_USE ||
+		       gpios[FIM_SERIAL_GPIO_RTS].nr == FIM_GPIO_DONT_USE)) {
+		printk_dbg("HW flow control not supported (GPIOs not defined)\n");
+		return -EOPNOTSUPP;
+	}
+	
+	/* Setup the CTS and RTS if HW flow controls is requested */
+	cts = hwflow ? 1 << (gpios[FIM_SERIAL_GPIO_CTS].nr - offset) : 0;
+	rts = hwflow ? 1 << (gpios[FIM_SERIAL_GPIO_RTS].nr - offset) : 0;
+	fim_set_ctrl_reg(fim, FIM_SERIAL_CTSIO_REG, cts);
+	fim_set_ctrl_reg(fim, FIM_SERIAL_RTSIO_REG, rts);
+
+	return fim_send_interrupt2(fim, FIM_SERIAL_INT_BIT_POS);
+}
+
+/*
+ * The ktermios structure is declared under: include/asm-arm/termbits.h
+ */
+static int fim_serial_configure_port(struct fim_serial_t *port,
+				   struct ktermios *termios,
+				   struct ktermios *old_termios)
+{
+	struct fim_driver *fim;
+	unsigned int cflag, old_cflag;
+	int databits, stopbits, parbit;
+	int retval;
+	unsigned int regval;
+	int timeout;
+	unsigned long flags;
+
+	/* That's really bad but it can happen */
+	if (!termios)
+		return -EINVAL;
+	
+	/* @BUG: The info pointer is NULL when the console is started! */
+	fim = &port->fim;
+	cflag = termios->c_cflag;
+	old_cflag = 0;
+	if (old_termios)
+		old_cflag = old_termios->c_cflag;
+
+	/* Check the number of bits to set */
+	switch (cflag & CSIZE) {
+	case CS5:
+		databits = 5;
+		break;
+	case CS6:
+		databits = 6;
+		break;
+	case CS7:
+		databits = 7;
+		break;
+	case CS8:
+		databits = 8;
+		break;
+	default:
+		printk_err("Invalid number of data bits flag 0x%04X\n", cflag & CSIZE);
+		return -EINVAL;
+	}
+
+	/* Check if the STOPB flag is set, in that case use two stop bits */
+	stopbits = 1;
+	if (cflag & CSTOPB)
+		stopbits = 2;
+	
+	parbit = 0;
+	if (cflag & PARENB)
+		parbit = 1;
+
+	/* Lock the below code segment first */
+	spin_lock_irqsave(&port->tx_lock, flags);
+
+	/* Now, we must wait for the empty DMA-buffers */
+	timeout = 1000;
+	while (timeout && fim_tx_buffers_level(&port->fim)) {
+		udelay(1000);
+		cpu_relax();
+		timeout--;
+	}
+
+	if (!timeout) {
+		printk_err("Timeout waiting for an empty TX-DMA buffer!\n");
+		retval = -ETIME;
+		goto exit_unlock;
+	}
+	
+	/* Ok, we are save, we can try to configure the FIM with the valid byte length */
+	if (port->totalbits != databits + stopbits + parbit) {
+		fim_set_ctrl_reg(fim, 0, databits + stopbits + parbit);
+		retval = fim_send_interrupt2(fim, FIM_SERIAL_INT_BITS_CHAR);
+		if (retval) {
+			printk_err("Failed config of the data bits by the FIM %i\n",
+				   fim->picnr);
+			goto exit_unlock;
+		}
+	}
+	
+	/* Save the number of bits for the future operations */
+	port->numbits = databits;
+	port->totalbits = databits + stopbits + parbit;
+	port->bitsmask = (1 << databits) - 1;
+	port->cflag = cflag;
+
+	/* Configure the software flow control */
+	retval = fim_sw_flowctrl(port, termios);
+	if (retval) {
+		printk_err("SW flow control of the FIM %i\n", fim->picnr);
+		goto exit_unlock;
+	}
+
+	/* Now configure the baudrate and bit timing parameters */
+	retval = fim_serial_baudrate(port, termios);
+	if (retval) {
+		printk_err("Couldn't set the baud rate by the FIM %i\n", fim->picnr);
+		goto exit_unlock;
+	}
+
+	/*
+	 * Check if need to enable the CRTSCTS-lines. In that case first configure
+	 * the bits position and then enable the HW-flow control in the config register
+	 */
+	if ((cflag & CRTSCTS) != (old_cflag & CRTSCTS)) {
+				
+		retval = fim_serial_setup_bitpos(port, cflag & CRTSCTS);
+		if (retval) {
+
+			/*
+			 * This return value means that the HW flow control could not
+			 * be enabled. Reset this flag so that the higher TTY layer
+			 * knows that a requested operation failed and informs the user
+			 * space about this.
+			 */
+			if (retval == -EOPNOTSUPP)
+				termios->c_cflag &= ~CRTSCTS;
+			
+			printk_err("Couldn't setup the FIM port %i\n",
+				   fim->picnr);
+			goto exit_unlock;
+		}
+
+		/* Now set the control status register to the correct value */
+		fim_get_ctrl_reg(fim, FIM_SERIAL_CTRL_REG, &regval);
+		if (cflag & CRTSCTS)
+			regval |= FIM_SERIAL_STAT_HW_FLOW;
+		else
+			regval &= ~FIM_SERIAL_STAT_HW_FLOW;
+		fim_set_ctrl_reg(fim, FIM_SERIAL_CTRL_REG, regval);
+	}
+
+	/* After each reconfiguration we need to re-init the FIM-firmware */
+	fim_get_ctrl_reg(fim, FIM_SERIAL_CTRL_REG, &regval);
+	fim_set_ctrl_reg(fim, FIM_SERIAL_CTRL_REG,
+			 regval | FIM_SERIAL_STAT_TX_ENABLE | FIM_SERIAL_STAT_COMPLETE);
+
+	retval = 0;
+	port->termios = termios;
+	
+ exit_unlock:
+	spin_unlock_irqrestore(&port->tx_lock, flags);
+	return retval;
+}
+
+/*
+ * Send a buffer over the FIM-core and the DMA-controller
+ * For being faster we use an internal private FIM-buffer for sending the data
+ * to the DMA-channel
+ */
+static int fim_serial_send_buffer(struct fim_serial_t *port,
+				  struct uart_port *uart)
+{
+	struct fim_buffer_t buf;
+	unsigned char *pdest;
+	unsigned short data;
+	unsigned char ch;
+	struct fim_driver *fim;
+	int bytes_per_char, stop_bits;
+	int len, chars_to_send;
+	struct circ_buf *xmit;
+	unsigned char buf_data[FIM_SERIAL_TX_DMA_BUFFER_SIZE];
+	int retval;
+
+	if (!port || !uart) {
+		printk_err("NULL pointer passed (port %p | uart %p)\n", port, uart);
+		return -EINVAL;
+	}
+
+	spin_lock(&port->tx_lock);
+	
+	xmit  = &uart->info->xmit;
+	len = uart_circ_chars_pending(xmit);
+	data = 1;
+	if (port->cflag & CSTOPB)
+		data = (data << 1) | 0x01;
+	stop_bits = data;
+	
+	if (port->cflag & PARENB)
+		data = (data << 1);
+	data = (data << port->numbits);
+
+	bytes_per_char = 1;
+	if (data & 0xFF00)
+		bytes_per_char = 2;
+	
+	fim = &port->fim;
+	len = (len > FIM_SERIAL_TX_CHARS_PER_DMA) ? FIM_SERIAL_TX_CHARS_PER_DMA : len;
+
+	/* Now copy the data into the FIM-buffer */
+	pdest = buf_data;
+	chars_to_send = 0;
+	while (!uart_circ_empty(xmit) && chars_to_send < len) {
+		ch = xmit->buf[xmit->tail];
+		data = stop_bits;
+		if (port->cflag & PARENB)
+			data = (data << 1) | get_parity(ch, port->bitsmask,
+							port->cflag & PARODD);
+
+		data = (data << port->numbits) | (ch & port->bitsmask);
+
+		/* For char length greater than 8 bits use two bytes for the TX-data */
+		*pdest++ = data & 0xFF;
+		if(bytes_per_char == 2)
+			*pdest++ = (data >> 8) & 0xFF;
+
+		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		uart->icount.tx++;
+		chars_to_send += 1;
+	}
+
+	
+        /* Check a last time if we need to send the data */
+	if (uart_tx_stopped(uart) || !chars_to_send) {
+		printk_debug("Aborting TX due a stopped queue!\n");
+		goto exit_unlock;
+	}
+
+        /*
+	 * Setup the FIM-buffer with the information that we have a private
+	 * buffer and don't want to free it inside the TX-callback
+	 */
+	retval = 0;
+	buf.private = FIM_SERIAL_BUF_PRIVATE;
+	buf.length = chars_to_send * bytes_per_char;
+	buf.data = buf_data;
+	if (fim_send_buffer(fim, &buf)) {
+		printk_err("FIM send buffer request failed (len %i)\n", buf.length);
+		retval = -ENOMEM;
+	}
+	
+
+ exit_unlock:
+	spin_unlock(&port->tx_lock);
+	
+	return retval;
+}
+
+static int fim_serial_transmit(struct uart_port *uart)
+{
+	struct circ_buf *xmit;
+	struct fim_serial_t *port;
+	struct fim_driver *fim;
+
+	port = get_port_from_uart(uart);
+	fim = &port->fim;
+	xmit  = &uart->info->xmit;
+
+	/*
+	 * Check if we have enough space for the request, if not then the tasklet
+	 * will be responsible for recalling this function and for resending the data
+	 * of the UART buffer
+	 */
+	if (fim_tx_buffers_room(fim) < uart_circ_chars_pending(xmit)) {
+		printk_debug("Skipping transmit request with length %lu\n",
+			     uart_circ_chars_pending(xmit));
+		return 0;
+	}
+	
+	printk_debug("Request to send %lu chars | stopped %i | empty %i\n",
+		     uart_circ_chars_pending(xmit),
+		     uart_tx_stopped(uart), uart_circ_empty(xmit));
+
+	/* Check if need to send only one char */
+	if (uart->x_char) {
+		printk_debug("Sending only one char\n");
+		fim_serial_send_char(port, uart->x_char);
+		uart->x_char = 0;
+		uart->icount.tx++;
+		return 1;
+	}
+
+	/* Check if have something to send */
+	if (uart_circ_empty(xmit) || uart_tx_stopped(uart)) {
+		printk_debug("Circular buffer empty or TX stopped\n");
+		return 0;
+	}
+
+	/* IMPORTANT: Always use the "tail" of the xmit-buffer */
+	fim_serial_send_buffer(port, uart);
+
+	/* Tell the higher layer that we can "probably" send more data */
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(uart);
+	
+	return 0;
+}
+
+/* Function for transmitting data to the FIM-port */
+static void fim_serial_start_tx(struct uart_port *uart)
+{
+	printk_debug("Calling %s\n", __func__);
+	fim_serial_transmit(uart);
+}
+
+/* Return zero if the TX-DMA buffers are NOT empty */
+static unsigned int fim_serial_tx_empty(struct uart_port *uart)
+{
+	struct fim_driver *fim;
+	unsigned int level;
+	struct fim_serial_t *port;
+	
+	port = get_port_from_uart(uart);
+	fim = &port->fim;
+	level = fim_tx_buffers_level(fim);
+
+	printk_debug("TX empty called (Level %i)\n", level);
+	
+	return (level) ? 0 : TIOCSER_TEMT;
+}
+
+/* Handler for the incoming FIM-interrupts */
+static void fim_serial_isr(struct fim_driver *driver, int irq, unsigned char code,
+			   unsigned int rx_fifo)
+{
+	struct uart_port *uart;
+	struct fim_serial_t *port;
+	
+	if(!code)
+		return;
+
+	port = (struct fim_serial_t *)driver->driver_data;
+	uart = get_uart_from_port(port);
+	
+	switch (code) {
+	case FIM_INT_FROM_MATCH_CHAR1:
+		printk_err("@TODO: Match char1 interrupt received.\n");
+		break;
+	case FIM_INT_FROM_MATCH_CHAR2:
+		printk_err("@TODO: Match char2 interrupt received.\n");
+		break;
+	case FIM_INT_FROM_RX_OVERFLOW:
+		uart->icount.overrun++;
+		break;
+	default:
+		break;
+	}
+
+	return;
+}
+
+/* This tasklet will restart the function for transmitting data to the FIM */
+static void fim_serial_tasklet_func(unsigned long data)
+{
+	struct fim_serial_t *port;
+	struct circ_buf *xmit;
+	struct uart_port *uart;
+
+	port = (struct fim_serial_t *)data;
+	printk_debug("Tasklet for port %p\n", port);
+	
+	if (!port)
+		return;
+	
+	uart = get_uart_from_port(port);
+	if (!uart)
+		return;
+
+	xmit = &uart->info->xmit;
+	if (uart_circ_chars_pending(xmit))
+		fim_serial_transmit(uart);
+}
+
+/*
+ * This is the TX-callback that the FIM-core call after a DMA-buffer was closed
+ * The fim buffer structure contains our internal private data
+ */
+static void fim_serial_tx_isr(struct fim_driver *driver, int irq,
+			      struct fim_buffer_t *pdata)
+{	
+	struct fim_buffer_t *buf;
+	struct fim_serial_t *port;
+	
+	port = (struct fim_serial_t *)driver->driver_data;
+	buf = (struct fim_buffer_t *)pdata->private;
+	printk_debug("TX-callback | FIM %i\n", driver->picnr);
+
+	/*
+	 * Free the allocated FIM-buffer
+	 */
+	if (buf && buf != FIM_SERIAL_BUF_PRIVATE)
+		fim_free_buffer(&port->fim, buf);
+
+	/* Schedule the tasklet for continuing with the data transmission */
+	tasklet_schedule(&port->tasklet);
+	
+	return;
+}
+
+/* Called when a receive DMA-buffer was closed */
+static void fim_serial_rx_isr(struct fim_driver *driver, int irq,
+			      struct fim_buffer_t *pdata)
+{
+	struct fim_serial_t *port;
+	struct uart_port *uart;
+	struct tty_struct *tty;
+	int length;
+	
+	/* Get the correct port from the FIM-driver structure */
+	port = (struct fim_serial_t *)driver->driver_data;
+	uart = &port->uart;
+	tty = get_tty_from_port(port);
+
+	/* If the port is closed then the tty structure will be NULL */
+	if (!port || !tty) {
+		printk_debug("uart %p | tty %p | port %p\n", uart, tty, port);
+		return;
+	}
+
+	/* By errors pass nothing to the TTY-layer */
+	if ((length = fim_serial_parse_data(tty, port, pdata->data, pdata->length,
+					   pdata->data)) <= 0) {
+		printk_err("Parsing the RX-DMA data (len %i)\n", length);
+		return;
+	}
+
+	tty_insert_flip_string(tty, pdata->data, length);
+	tty_flip_buffer_push(tty);
+	return;
+}
+
+static unsigned int fim_serial_get_mctrl(struct uart_port *port)
+{
+	return TIOCM_CTS | TIOCM_DSR | TIOCM_CAR;
+}
+
+static void fim_serial_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	/* @TODO? */
+}
+
+/* @TODO: This function isn't called for stopping the TX */
+static void fim_serial_stop_tx(struct uart_port *port)
+{
+	printk_debug("Calling %s\n", __func__);
+}
+
+static void fim_serial_stop_rx(struct uart_port *port)
+{
+	printk_debug("Calling %s\n", __func__);
+	/* @TODO? */
+}
+
+static void fim_serial_enable_ms(struct uart_port *port)
+{
+	/* @TODO? */
+}
+
+static void fim_serial_break_ctl(struct uart_port *port, int ctl)
+{
+	/* @TODO? */
+}
+
+/*
+ * Called when the port is opened. By the boot-console it will be called before
+ * the port configuration using set_termios
+ */
+static int fim_serial_startup(struct uart_port *uart)
+{
+	struct fim_serial_t *port;
+	unsigned int regval;
+	
+	printk_debug("Calling %s\n", __func__);
+	port = get_port_from_uart(uart);
+
+	fim_enable_irq(&port->fim);
+	
+        fim_get_ctrl_reg(&port->fim, FIM_SERIAL_CTRL_REG, &regval);
+        fim_set_ctrl_reg(&port->fim,
+                         FIM_SERIAL_CTRL_REG,
+                         regval | FIM_SERIAL_STAT_TX_ENABLE | FIM_SERIAL_STAT_COMPLETE);
+	
+	return 0;
+}
+
+/* Normally we must free the IRQ here, but the current FIM-firmware doesn't allow it */
+static void fim_serial_shutdown(struct uart_port *port)
+{
+	printk_debug("@TODO: Extend the firmware for %s\n", __func__);
+}
+
+/* Called for setting the termios config */
+static void fim_serial_set_termios(struct uart_port *uart, struct ktermios *termios,
+				   struct ktermios *old)
+{
+	struct fim_serial_t *port;
+	unsigned int cflag;
+	
+	printk_debug("Calling %s | termios %p | old %p\n", __func__, termios, old);
+
+	cflag = (old) ? old->c_cflag : 0;
+	if (cflag == termios->c_cflag) {
+		printk_debug("Skipping the termios configuration\n");
+		return;
+	}	
+	
+	port = get_port_from_uart(uart);
+	fim_serial_configure_port(port, termios, old);	
+}
+
+/* Return a string describing the type of the port */
+static const char *fim_serial_type(struct uart_port *port)
+{
+	return FIM_DRIVER_NAME;
+}
+
+static void fim_serial_release_port(struct uart_port *port)
+{
+	printk_debug("Calling %s\n", __func__);
+}
+
+static int fim_serial_request_port(struct uart_port *port)
+{
+	printk_debug("Calling %s\n", __func__);
+	return 0;
+}
+
+/* @TODO: Get more infos about the UART configuration */
+static void fim_serial_config_port(struct uart_port *port, int flags)
+{
+	printk_debug("Calling %s\n", __func__);
+	port->type = UPIO_MEM;
+}
+
+static int fim_serial_verify_port(struct uart_port *port, struct serial_struct *ser)
+{
+	printk_debug("Verify port called\n");
+	return 0;
+}
+
+static struct uart_ops fim_serial_ops = {
+	.tx_empty	= fim_serial_tx_empty,
+	.set_mctrl	= fim_serial_set_mctrl,
+	.get_mctrl	= fim_serial_get_mctrl,
+	.stop_tx	= fim_serial_stop_tx,
+	.start_tx	= fim_serial_start_tx,
+	.stop_rx	= fim_serial_stop_rx,
+	.enable_ms	= fim_serial_enable_ms,
+	.break_ctl	= fim_serial_break_ctl,
+	.startup	= fim_serial_startup,
+	.shutdown	= fim_serial_shutdown,
+	.set_termios	= fim_serial_set_termios,
+	.type		= fim_serial_type,
+	.release_port	= fim_serial_release_port,
+	.request_port	= fim_serial_request_port,
+	.config_port	= fim_serial_config_port,
+	.verify_port	= fim_serial_verify_port
+};
+
+#ifdef CONFIG_SERIAL_FIM_CONSOLE
+static void fim_serial_wait_tx(struct fim_driver *fim)
+{
+	unsigned int timeout;
+	unsigned int status;
+	
+	/* First check if the FIM is tasked with another send-char request */
+	timeout = 0xFFFF;
+	do {
+		timeout--;
+		fim_get_exp_reg(fim, 0, &status);
+		cpu_relax();
+	} while (timeout && (status & FIM_SERIAL_INT_INSERT_CHAR));
+}
+
+static void fim_serial_console_putchar(struct uart_port *uart, int ch)
+{
+	struct fim_driver *fim;
+	struct fim_serial_t *port;
+	unsigned short data = 1;
+	unsigned int cflag;
+
+	port = get_port_from_uart(uart);
+	fim = &port->fim;
+	cflag = port->cflag;
+
+	/* First wait for a free TX-FIFO */
+	fim_serial_wait_tx(fim);
+
+	if (cflag & CSTOPB)
+		data = (data << 1) | 0x01;
+	
+	if (cflag & PARENB)
+		data = (data << 1) | get_parity(ch, port->bitsmask,
+						cflag & PARENB & PARODD);
+
+	data = (data << port->numbits) | (ch & port->bitsmask);
+
+	/* And send the char using the interrupt function */
+	fim_set_ctrl_reg(fim, 0, data & 0xFF);
+	fim_set_ctrl_reg(fim, 1, (data >> 8) & 0xFF);
+	fim_send_interrupt2(fim, FIM_SERIAL_INT_INSERT_CHAR);
+}
+
+static void fim_serial_console_write(struct console *co, const char *str,
+				   unsigned int count)
+{
+	struct fim_serial_t *port;
+	struct uart_port *uart;
+	unsigned long flags;
+
+	port = get_port_by_index(co->index);
+	uart = get_uart_from_port(port);
+	
+/* 	if (oops_in_progress) @TODO? */
+	spin_lock_irqsave(&port->tx_lock, flags);
+
+	/* @TODO: Test this function more intensive! */
+
+	uart_console_write(uart, str, count, fim_serial_console_putchar);
+
+	fim_serial_wait_tx(&port->fim);
+	
+	spin_unlock_irqrestore(&port->tx_lock, flags);
+}
+
+static int __init fim_serial_console_setup(struct console *co, char *options)
+{
+	struct fim_serial_t *port;
+	struct uart_port *uart;
+	int baud = 9600;
+	int bits = 8;
+	int parity = 'n';
+	int flow = 'n';
+
+	printk_debug("Calling %s for index %i\n", __func__, co->index);
+
+	/* @FIXME: Sanity checks required */
+	port = get_port_by_index(co->index);
+	if (!port->reg)
+		return -ENODEV;
+
+	uart = get_uart_from_port(port);
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+
+	printk_debug("Console %p: B%i | P%i | B%i\n", uart, baud, parity, bits);
+	fim_serial_startup(uart);
+	return uart_set_options(uart, co, baud, parity, bits, flow);
+}
+
+/* The data of the console is being initialized inside the init-function */
+static struct console fim_console = {
+	.name	= FIM_SERIAL_DEV_NAME,
+	.write	= fim_serial_console_write,
+	.device	= uart_console_device,
+	.setup	= fim_serial_console_setup,
+	.flags	= CON_PRINTBUFFER,
+	.index	= -1,
+};
+#define fim_console_ptr (&fim_console)
+#else
+#define fim_console_ptr	(NULL)
+#endif /* CONFIG_SERIAL_FIM_CONSOLE */
+
+static int fim_serial_unregister_port(struct fim_serial_t *port)
+{
+	int cnt;
+	struct fim_driver *fim;
+	struct uart_port *uart;
+	
+	if (!port || !port->reg)
+		return -ENODEV;
+
+	tasklet_kill(&port->tasklet);
+	
+	uart = &port->uart;
+	fim = &port->fim;
+	fim_set_ctrl_reg(fim, FIM_SERIAL_CTRL_REG, 0x00);
+		
+	/* Reset all the control register */
+	fim_serial_reset_all(fim);
+	
+	fim_unregister_driver(fim);
+	uart_remove_one_port(&fim_serials->driver, uart);
+		
+	/* Free all the requested GPIOs */
+	for (cnt=0; cnt < FIM_SERIAL_MAX_GPIOS; cnt++) {
+		printk_debug("Freeing GPIO %i\n", port->gpios[cnt].nr);
+		if (port->gpios[cnt].nr == FIM_LAST_GPIO)
+			break;		
+		else if (port->gpios[cnt].nr == FIM_GPIO_DONT_USE)
+			continue;
+		else
+			gpio_free(port->gpios[cnt].nr);
+	}
+
+	/* Free the clock */
+        clk_put(port->sys_clk);
+
+	port->reg = 0;
+	return 0;
+}
+
+static int fim_serial_register_port(struct device *dev,
+				    struct fim_serial_t *port, int minor, int picnr,
+				    struct fim_gpio_t gpios[])
+{
+	int retval;
+	int cnt, func;
+	struct fim_driver *fim;
+	struct uart_port *uart;
+	struct fim_dma_cfg_t dma_cfg;
+
+	fim = &port->fim;
+	uart = &port->uart;
+	
+	port->index = minor;
+	fim->picnr = picnr;
+	fim->driver.name = FIM_DRIVER_NAME;
+	fim->fim_isr = fim_serial_isr;
+	fim->dma_tx_isr = fim_serial_tx_isr;
+	fim->dma_rx_isr = fim_serial_rx_isr;
+	fim->driver_data = port;
+
+	/* Set our desired DMA-channel configuration */
+	dma_cfg.rxnr = FIM_SERIAL_RX_DMA_BUFFERS;
+	dma_cfg.txnr = FIM_SERIAL_TX_DMA_BUFFERS;
+	dma_cfg.rxsz = FIM_SERIAL_RX_DMA_BUFFER_SIZE;
+	dma_cfg.txsz = FIM_SERIAL_TX_DMA_BUFFER_SIZE;
+	fim->dma_cfg = &dma_cfg;
+	
+	/* Check if have a firmware code for using to */
+	fim->fw_name = FIM_SERIAL_FIRMWARE_FILE;
+	fim->fw_code = FIM_SERIAL_FIRMWARE_CODE;
+	retval = fim_register_driver(fim);
+	if (retval) {
+		printk_err("Couldn't register the FIM driver.\n");
+		return retval;
+	}
+
+	/* This is required for the FIM-firmware */
+	fim_set_ctrl_reg(fim, FIM_SERIAL_CTRL_REG, 0x00);
+
+	/* Setup the bit positions but disable the HW flow control first */
+	memcpy(port->gpios, gpios, FIM_SERIAL_MAX_GPIOS * sizeof(struct fim_gpio_t));
+	retval = fim_serial_setup_bitpos(port, 0);
+	if (retval) {
+		printk_err("Setting the default GPIOs\n");
+		goto err_unreg_fim;
+	}
+	
+	retval = fim_serial_reset_matchs(fim);
+	if (retval) {
+		printk_err("Resetting the match registers\n");
+		goto err_unreg_fim;
+	}
+
+	/* Request the corresponding GPIOs (@XXX: Check the returned values) */
+	printk_debug("Requesting and configuring the GPIO's\n");
+	for (cnt=0; gpios[cnt].nr != FIM_LAST_GPIO; cnt++) {
+
+		if (gpios[cnt].nr == FIM_GPIO_DONT_USE)
+			continue;
+		
+		printk_debug("Going to request the GPIO %i\n", gpios[cnt].nr);
+		retval = gpio_request(gpios[cnt].nr, FIM_DRIVER_NAME);
+		if (!retval) {
+		  
+			func = gpios[cnt].func;
+			
+			gpio_configure_ns921x_unlocked(gpios[cnt].nr,
+						       NS921X_GPIO_INPUT,
+						       NS921X_GPIO_DONT_INVERT,
+						       func,
+						       NS921X_GPIO_ENABLE_PULLUP);
+		} else {
+			/* Free the already requested GPIOs */
+			printk_err("Couldn't request the GPIO %i\n", gpios[cnt].nr);
+			while (cnt) gpio_free(gpios[--cnt].nr);
+			goto err_free_gpios;
+		}
+	}
+
+	/* Get a reference to the SYS clock for setting the baudrate */
+	if (IS_ERR(port->sys_clk = clk_get(port->fim.dev, "systemclock"))) {
+		printk_err("Couldn't get the SYS clock.\n");
+		goto err_free_gpios;
+	}
+
+	/* Init the internal data before registering the UART port */
+        tasklet_init(&port->tasklet, fim_serial_tasklet_func, (unsigned long)port);
+
+	spin_lock_init(&port->tx_lock);
+	port->minor = minor;
+	port->reg = 1;
+	port->driver = &fim_serials->driver;
+
+	/*
+	 * Register with the corresponding minor number
+	 * IMPORTANT: Set the internal fim-serial structure as driver-data before
+	 * calling the function for adding a new uart port, then in some functions
+	 * the pointer to the FIM-port is obtained from the driver data
+	 */
+	uart->ops = &fim_serial_ops;
+	uart->flags = UPF_BOOT_AUTOCONF;
+	uart->dev = dev;
+	uart->type = PORT_UNKNOWN;
+	uart->line = minor;
+	uart->iotype = UPIO_PORT;
+	uart->mapbase = 0x1234;
+	dev_set_drvdata(uart->dev, port);
+	if ((retval = uart_add_one_port(&fim_serials->driver, uart))) {
+		dev_set_drvdata(uart->dev, NULL);
+		printk_err("Couldn't register the UART port %i\n", minor);
+		goto err_put_clk;
+	}
+
+	return 0;
+
+ err_put_clk:
+	clk_put(port->sys_clk);
+	
+ err_free_gpios:
+	for (cnt=0; gpios[cnt].nr < FIM_SERIAL_MAX_GPIOS; cnt++) {
+		if (gpios[cnt].nr == FIM_LAST_GPIO)
+			break;
+
+		if (gpios[cnt].nr != FIM_GPIO_DONT_USE)
+			gpio_free(gpios[cnt].nr);
+	}
+	
+ err_unreg_fim:
+	fim_unregister_driver(&port->fim);
+
+	return retval;
+}
+
+/*
+ * Probe function
+ */
+static int __devinit fim_serial_probe(struct platform_device *pdev)
+{
+	struct fim_serial_t *port;
+	struct fim_gpio_t gpios[FIM_SERIAL_MAX_GPIOS];
+	int retval;
+	struct fim_serial_platform_data *pdata;
+	
+	printk_info("Probe function called for device ID %i\n", pdev->id);
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata)
+		return -ENXIO;
+
+        if (fim_check_device_id(fims_number, pdata->fim_nr)) {
+#if defined(MODULE)
+		printk_dbg("Skipping FIM%i (not selected)\n", pdata->fim_nr);
+#else
+		printk_err("Invalid FIM number '%i' in platform data\n", pdata->fim_nr);
+#endif
+		return -ENODEV;
+        }
+	
+	port = fim_serials->ports + pdata->fim_nr;
+
+	/* @XXX: The below code is really ugly, remove it! */
+	gpios[FIM_SERIAL_GPIO_RX].nr   = pdata->rx_gpio_nr;
+	gpios[FIM_SERIAL_GPIO_RX].func = pdata->rx_gpio_func;
+	gpios[FIM_SERIAL_GPIO_TX].nr   = pdata->tx_gpio_nr;
+	gpios[FIM_SERIAL_GPIO_TX].func = pdata->tx_gpio_func;
+	gpios[FIM_SERIAL_GPIO_CTS].nr   = pdata->cts_gpio_nr;
+	gpios[FIM_SERIAL_GPIO_CTS].func = pdata->cts_gpio_func;
+	gpios[FIM_SERIAL_GPIO_RTS].nr   = pdata->rts_gpio_nr;
+	gpios[FIM_SERIAL_GPIO_RTS].func = pdata->rts_gpio_func;
+	gpios[FIM_SERIAL_GPIO_LAST].nr   = FIM_LAST_GPIO;
+
+	/* And try to register the FIM */
+	printk_debug("FIM %i | Port %p | Uart %p)\n", pdata->fim_nr, port, &port->uart);
+	retval = fim_serial_register_port(&pdev->dev, port,
+					  pdata->fim_nr, pdata->fim_nr,
+					  gpios);
+
+	return retval;
+}
+
+static int __devexit fim_serial_remove(struct platform_device *pdev)
+{
+	struct fim_serial_t *port;
+	int retval;
+	
+	port = dev_get_drvdata(&pdev->dev);
+
+	retval = fim_serial_unregister_port(port);
+	if (!retval)
+		dev_set_drvdata(&pdev->dev, NULL);
+
+	return retval;
+}
+
+#if defined(CONFIG_PM)
+
+static int fim_serial_suspend(struct platform_device *pdev,
+			      pm_message_t state)
+{
+	int ret;
+	struct fim_serial_t *port;
+
+	/* Only stop the FIM before entering the suspend mode */
+	port = dev_get_drvdata(&pdev->dev);
+	ret = fim_send_stop(&port->fim);
+	if (ret)
+		printk_err("Couldn't stop the FIM\n");
+
+	return ret;
+}
+
+static int fim_serial_resume(struct platform_device *pdev)
+{
+	struct fim_serial_t *port;
+	int retval;
+	struct fim_driver *fim;
+
+	port = dev_get_drvdata(&pdev->dev);
+	fim = &port->fim;
+
+	/*
+	 * Reset the internal values for being able to restore the port settings,
+	 * otherwise the port will not work correctly after the wakeup reset.
+	 */
+	port->totalbits = 0;
+	port->numbits = 0;
+	port->last_totalbits = 0;
+	port->baud = 0;
+	
+	retval = fim_download_firmware(&port->fim);
+	if (retval) {
+		printk_err("FIM download failed\n");
+		goto exit_resume;
+	}
+
+	/* This is required for the FIM-firmware */
+        fim_set_ctrl_reg(fim, FIM_SERIAL_CTRL_REG, 0xff);
+	
+	retval = fim_send_start(&port->fim);
+	if (retval) {
+		printk_err("FIM start failed\n");
+		goto exit_resume;
+	}
+
+        fim_set_ctrl_reg(fim, FIM_SERIAL_CTRL_REG, 0x00);
+	
+        retval = fim_serial_setup_bitpos(port, 0);
+        if (retval) {
+                printk_err("Setting the default GPIOs\n");
+                goto exit_resume;
+        }
+
+        retval = fim_serial_reset_matchs(fim);
+        if (retval) {
+                printk_err("Resetting the match registers\n");
+                goto exit_resume;
+        }
+
+	/*
+	 * If the port was not opened yet, then no termios will be available, so
+	 * only return at this point.
+	 */
+	if (port->termios) {
+		retval = fim_serial_configure_port(port, port->termios, NULL);
+		if (retval)
+			printk_err("Port configuration failed\n");
+	}
+	
+ exit_resume:
+	return retval;
+}
+#else
+#define fim_serial_suspend			NULL
+#define fim_serial_resume			NULL
+#endif /* CONFIG_PM */
+
+/* @XXX: Work with hotplug and coldplug? */
+MODULE_ALIAS(FIM_PLATFORM_DRIVER_NAME);
+
+static struct platform_driver fim_serial_platform_driver = {
+	.probe	 = fim_serial_probe,
+	.remove	 = __devexit_p(fim_serial_remove),
+	.suspend = fim_serial_suspend,
+	.resume  = fim_serial_resume,
+	.driver	 = {
+		.owner = THIS_MODULE,
+		.name  = FIM_DRIVER_NAME,
+	},
+};
+
+static int __init fim_serial_init(void)
+{
+	int retval;
+	int nrpics;
+
+	printk_debug("Starting the FIM serial driver.\n");
+
+	/* Get the number of maximal available FIMs */
+	nrpics = fim_number_pics();
+
+	/* Check for the passed number parameter */
+	if (fim_check_numbers_param(fims_number)) {
+		printk_err("Invalid number '%i' of FIMs to handle\n", fims_number);
+		return -EINVAL;
+	}
+
+	fim_serials = kzalloc(sizeof(struct fim_serials_t) +
+			      (nrpics * sizeof(struct fim_serial_t)), GFP_KERNEL);
+	if (!fim_serials)
+		return -ENOMEM;
+
+	fim_serials->fims = nrpics;
+	fim_serials->ports = (void *)fim_serials + sizeof(struct fim_serials_t);
+
+	/* Init the UART driver */
+	fim_serials->driver.owner = THIS_MODULE;
+	fim_serials->driver.driver_name	= FIM_DRIVER_NAME;
+	fim_serials->driver.dev_name = FIM_SERIAL_DEV_NAME;
+	fim_serials->driver.nr = nrpics;
+
+	/* @FIXME: This is ugly! */
+#if defined(CONFIG_SERIAL_FIM_CONSOLE)
+	fim_console_ptr->data = &fim_serials->driver;
+#endif
+	
+	fim_serials->driver.cons = fim_console_ptr;
+	
+	printk_debug("Going to register the driver\n");
+	retval = uart_register_driver(&fim_serials->driver);
+	if (retval)
+		goto err_free_mem;
+
+	printk_debug("Trying to register the platform driver\n");
+	retval = platform_driver_register(&fim_serial_platform_driver);
+	if (retval)
+		goto err_unreg_uart;
+
+	printk_info(DRIVER_DESC " v" DRIVER_VERSION "\n");
+	return 0;
+
+ err_unreg_uart:
+	uart_unregister_driver(&fim_serials->driver);
+	
+ err_free_mem:
+	kfree(fim_serials);
+	
+	return retval;
+}
+
+static void __exit fim_serial_exit(void)
+{
+	printk_info("Removing the FIM serial driver\n");
+
+	platform_driver_unregister(&fim_serial_platform_driver);
+	uart_unregister_driver(&fim_serials->driver);
+
+	kfree(fim_serials);
+}
+
+module_init(fim_serial_init);
+module_exit(fim_serial_exit);
+
diff --git a/drivers/fims/serial/fim_serial.h b/drivers/fims/serial/fim_serial.h
new file mode 100644
index 000000000000..7ddf3f243b30
--- /dev/null
+++ b/drivers/fims/serial/fim_serial.h
@@ -0,0 +1,155 @@
+
+/*
+ * Automatic generated header file with the firmware code for the FIM
+ * Input binary  : fim_firmware.bin
+ * Output header : fim_uart.h
+ * Structure     : fim_serial_firmware
+ */
+
+static const unsigned char fim_serial_firmware[] = { 
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x01, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x16, 0x02, 0x00, 0x00,  0x62, 0x28, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0xa0, 0x00, 0x03, 0x08,  
+	0x83, 0x12, 0xa1, 0x00,  0x8b, 0x10, 0x83, 0x16,  
+	0x09, 0x18, 0x19, 0x28,  0x89, 0x18, 0x21, 0x28,  
+	0x09, 0x19, 0x2f, 0x28,  0x89, 0x19, 0x33, 0x28,  
+	0x09, 0x1a, 0x39, 0x28,  0x89, 0x1a, 0x4f, 0x28,  
+	0x09, 0x1b, 0x57, 0x28,  0x18, 0x28, 0x83, 0x12,  
+	0x18, 0x14, 0x10, 0x08,  0xc0, 0x00, 0x11, 0x08,  
+	0xc1, 0x00, 0x25, 0x14,  0x5b, 0x28, 0x83, 0x12,  
+	0x10, 0x08, 0xc2, 0x00,  0x11, 0x08, 0xc3, 0x00,  
+	0x12, 0x08, 0xc4, 0x00,  0x13, 0x08, 0xc5, 0x00,  
+	0x14, 0x08, 0xc6, 0x00,  0x15, 0x08, 0xc7, 0x00,  
+	0x5b, 0x28, 0x83, 0x12,  0x10, 0x08, 0xc8, 0x00,  
+	0x5b, 0x28, 0x83, 0x12,  0x10, 0x08, 0xb5, 0x00,  
+	0x11, 0x08, 0xb6, 0x00,  0x5b, 0x28, 0x83, 0x12,  
+	0x10, 0x08, 0xbc, 0x00,  0x11, 0x08, 0xbd, 0x00,  
+	0x12, 0x08, 0xbe, 0x00,  0x13, 0x08, 0xbf, 0x00,  
+	0x3c, 0x08, 0x83, 0x16,  0x98, 0x00, 0x83, 0x12,  
+	0xa9, 0x00, 0x3e, 0x08,  0xa9, 0x04, 0x29, 0x08,  
+	0xff, 0x3a, 0x83, 0x16,  0x99, 0x00, 0x83, 0x12,  
+	0x5b, 0x28, 0x83, 0x12,  0x10, 0x08, 0x07, 0x39,  
+	0x83, 0x16, 0x81, 0x00,  0x01, 0x17, 0x83, 0x12,  
+	0x5b, 0x28, 0x83, 0x12,  0x10, 0x08, 0xc9, 0x00,  
+	0x5b, 0x28, 0x89, 0x17,  0xa4, 0x14, 0x21, 0x08,  
+	0x83, 0x00, 0xa0, 0x0e,  0x20, 0x0e, 0x09, 0x00,  
+	0x8b, 0x17, 0x0b, 0x16,  0x83, 0x16, 0x01, 0x17,  
+	0x83, 0x12, 0x04, 0x22,  0x98, 0x01, 0xb0, 0x30,  
+	0xb8, 0x00, 0xb9, 0x00,  0xa4, 0x18, 0xf1, 0x21,  
+	0x16, 0x1c, 0x6c, 0x28,  0x71, 0x28, 0x7e, 0x20,  
+	0x86, 0x28, 0x4e, 0x21,  0x7e, 0x20, 0x4e, 0x21,  
+	0x7e, 0x20, 0x4e, 0x21,  0x24, 0x18, 0xe8, 0x21,  
+	0xa4, 0x18, 0xf1, 0x21,  0x7e, 0x20, 0x08, 0x00,  
+	0x22, 0x1c, 0x82, 0x28,  0xa2, 0x20, 0x08, 0x00,  
+	0xa2, 0x19, 0x21, 0x21,  0xfd, 0x20, 0x08, 0x00,  
+	0x23, 0x1c, 0x8a, 0x28,  0x79, 0x21, 0x71, 0x28,  
+	0x73, 0x20, 0x16, 0x1d,  0x93, 0x28, 0x3f, 0x08,  
+	0x83, 0x16, 0x10, 0x05,  0x83, 0x12, 0x03, 0x1d,  
+	0x71, 0x28, 0x25, 0x1c,  0x9d, 0x28, 0x40, 0x08,  
+	0xaa, 0x00, 0x41, 0x08,  0xb7, 0x00, 0x25, 0x10,  
+	0x18, 0x10, 0xc7, 0x21,  0x71, 0x28, 0x96, 0x1c,  
+	0x71, 0x28, 0x8c, 0x1d,  0xbf, 0x21, 0x71, 0x28,  
+	0x01, 0x08, 0xa8, 0x00,  0x22, 0x1d, 0xab, 0x28,  
+	0xa8, 0x1b, 0xab, 0x28,  0xa2, 0x1c, 0xb5, 0x28,  
+	0xa2, 0x10, 0x22, 0x11,  0xa8, 0x1b, 0x22, 0x15,  
+	0xa2, 0x18, 0x08, 0x00,  0x31, 0x08, 0x28, 0x02,  
+	0x03, 0x18, 0xb5, 0x28,  0x08, 0x00, 0x22, 0x11,  
+	0xb1, 0x1b, 0x22, 0x15,  0x49, 0x08, 0xb1, 0x07,  
+	0x03, 0x1c, 0xbd, 0x28,  0xa2, 0x14, 0x22, 0x1e,  
+	0xc1, 0x28, 0x22, 0x12,  0x08, 0x00, 0x03, 0x10,  
+	0x3d, 0x08, 0x83, 0x16,  0x10, 0x05, 0x83, 0x12,  
+	0x03, 0x19, 0xc9, 0x28,  0x03, 0x14, 0xab, 0x0c,  
+	0xad, 0x0b, 0xe1, 0x28,  0x23, 0x18, 0x79, 0x21,  
+	0x2b, 0x08, 0xae, 0x00,  0x96, 0x1d, 0xd7, 0x28,  
+	0x2b, 0x08, 0x46, 0x05,  0x42, 0x02, 0x03, 0x19,  
+	0x26, 0x14, 0x16, 0x1e,  0xde, 0x28, 0x2b, 0x08,  
+	0x46, 0x05, 0x44, 0x02,  0x03, 0x19, 0xa6, 0x14,  
+	0xab, 0x01, 0x08, 0x30,  0xad, 0x00, 0xac, 0x0b,  
+	0x08, 0x00, 0x23, 0x18,  0x79, 0x21, 0xa2, 0x15,  
+	0x35, 0x08, 0xb3, 0x00,  0x36, 0x08, 0xb4, 0x00,  
+	0x22, 0x10, 0x26, 0x1c,  0xf4, 0x28, 0x26, 0x10,  
+	0x2b, 0x08, 0x47, 0x05,  0x43, 0x02, 0x03, 0x19,  
+	0xe2, 0x21, 0x08, 0x00,  0xa6, 0x1c, 0x08, 0x00,  
+	0xa6, 0x10, 0x2b, 0x08,  0x47, 0x05, 0x45, 0x02,  
+	0x03, 0x19, 0xe5, 0x21,  0x08, 0x00, 0x3d, 0x08,  
+	0x83, 0x16, 0x10, 0x05,  0x83, 0x12, 0x03, 0x1d,  
+	0x20, 0x29, 0x01, 0x08,  0xa8, 0x00, 0x23, 0x18,  
+	0x79, 0x21, 0xa2, 0x1d,  0x0a, 0x29, 0x29, 0x21,  
+	0xa2, 0x01, 0x22, 0x14,  0x22, 0x16, 0xa8, 0x1b,  
+	0x22, 0x15, 0x48, 0x08,  0xac, 0x00, 0xac, 0x19,  
+	0x08, 0x30, 0xad, 0x00,  0xab, 0x01, 0x03, 0x10,  
+	0x49, 0x0c, 0xa9, 0x00,  0x0a, 0x30, 0x29, 0x02,  
+	0x28, 0x07, 0xb1, 0x00,  0x03, 0x1c, 0x1f, 0x29,  
+	0xa2, 0x14, 0x08, 0x00,  0x08, 0x00, 0xb3, 0x0b,  
+	0x08, 0x00, 0xb4, 0x0b,  0x08, 0x00, 0xa2, 0x11,  
+	0x2b, 0x14, 0x29, 0x21,  0x08, 0x00, 0x38, 0x08,  
+	0xba, 0x00, 0xba, 0x0a,  0x03, 0x1d, 0x30, 0x29,  
+	0xb0, 0x30, 0xba, 0x00,  0x3a, 0x08, 0x39, 0x02,  
+	0x03, 0x1d, 0x36, 0x29,  0xdd, 0x21, 0x08, 0x00,  
+	0x3a, 0x08, 0xbb, 0x00,  0xbb, 0x0a, 0x03, 0x1d,  
+	0x3d, 0x29, 0xb0, 0x30,  0xbb, 0x00, 0x3b, 0x08,  
+	0x39, 0x02, 0x03, 0x1d,  0x43, 0x29, 0xdd, 0x21,  
+	0x08, 0x00, 0x38, 0x08,  0x84, 0x00, 0x2e, 0x08,  
+	0x80, 0x00, 0x3a, 0x08,  0x84, 0x00, 0x2b, 0x08,  
+	0x80, 0x00, 0x3b, 0x08,  0xb8, 0x00, 0x08, 0x00,  
+	0x8c, 0x1b, 0x74, 0x29,  0x3e, 0x08, 0xff, 0x3a,  
+	0x83, 0x16, 0x98, 0x05,  0x83, 0x12, 0x39, 0x08,  
+	0x38, 0x02, 0x03, 0x19,  0x08, 0x00, 0x39, 0x08,  
+	0x84, 0x00, 0x00, 0x08,  0x39, 0x1c, 0x67, 0x29,  
+	0x00, 0x1c, 0x67, 0x29,  0x8b, 0x13, 0x00, 0x00,  
+	0x8e, 0x0a, 0x8d, 0x00,  0x00, 0x00, 0x8b, 0x17,  
+	0x6e, 0x29, 0x8b, 0x13,  0x00, 0x00, 0x8e, 0x01,  
+	0x8d, 0x00, 0x00, 0x00,  0x8b, 0x17, 0x6e, 0x29,  
+	0xb9, 0x0a, 0x03, 0x1d,  0x08, 0x00, 0xb0, 0x30,  
+	0xb9, 0x00, 0x08, 0x00,  0x3e, 0x08, 0x83, 0x16,  
+	0x98, 0x04, 0x83, 0x12,  0x08, 0x00, 0x01, 0x08,  
+	0xa7, 0x00, 0x23, 0x1d,  0x82, 0x29, 0xa7, 0x1b,  
+	0x82, 0x29, 0xa3, 0x1c,  0x8c, 0x29, 0xa3, 0x10,  
+	0x23, 0x11, 0xa7, 0x1b,  0x23, 0x15, 0xa3, 0x18,  
+	0x08, 0x00, 0x27, 0x08,  0x32, 0x02, 0x03, 0x1c,  
+	0x8c, 0x29, 0x08, 0x00,  0xa3, 0x1d, 0xa0, 0x29,  
+	0x3c, 0x08, 0x83, 0x16,  0x98, 0x04, 0x83, 0x12,  
+	0x23, 0x1a, 0x9e, 0x29,  0x23, 0x16, 0x23, 0x11,  
+	0xb2, 0x1b, 0x23, 0x15,  0x49, 0x08, 0xb2, 0x07,  
+	0x03, 0x1c, 0x9d, 0x29,  0xa3, 0x14, 0x08, 0x00,  
+	0x23, 0x10, 0x08, 0x00,  0x3c, 0x08, 0x2a, 0x1c,  
+	0xa7, 0x29, 0x83, 0x16,  0x98, 0x04, 0x83, 0x12,  
+	0xab, 0x29, 0xff, 0x3a,  0x83, 0x16, 0x98, 0x05,  
+	0x83, 0x12, 0xaa, 0x0c,  0x23, 0x11, 0xb2, 0x1b,  
+	0x23, 0x15, 0x49, 0x08,  0xb2, 0x07, 0x03, 0x1c,  
+	0xb4, 0x29, 0xa3, 0x14,  0xaf, 0x0b, 0xb8, 0x29,  
+	0xa3, 0x15, 0x08, 0x00,  0xb0, 0x0b, 0x08, 0x00,  
+	0x37, 0x08, 0xaa, 0x00,  0x08, 0x30, 0xb0, 0x00,  
+	0x08, 0x00, 0xfc, 0x21,  0xaa, 0x00, 0x09, 0x30,  
+	0x48, 0x02, 0x03, 0x1c,  0xc7, 0x29, 0xfc, 0x21,  
+	0xb7, 0x00, 0x48, 0x08,  0xaf, 0x00, 0x08, 0x30,  
+	0xb0, 0x00, 0xa3, 0x01,  0x23, 0x14, 0x01, 0x08,  
+	0xb2, 0x00, 0x23, 0x11,  0xb2, 0x1b, 0x23, 0x15,  
+	0x49, 0x08, 0xb2, 0x07,  0x03, 0x1c, 0xd7, 0x29,  
+	0xa3, 0x14, 0x3c, 0x08,  0xff, 0x3a, 0x83, 0x16,  
+	0x98, 0x05, 0x83, 0x12,  0x08, 0x00, 0x24, 0x18,  
+	0x08, 0x00, 0x09, 0x15,  0x24, 0x14, 0x08, 0x00,  
+	0x09, 0x14, 0x24, 0x14,  0x08, 0x00, 0x89, 0x14,  
+	0x24, 0x14, 0x08, 0x00,  0x83, 0x16, 0x89, 0x1f,  
+	0xfa, 0x29, 0x83, 0x12,  0x17, 0x08, 0xff, 0x3a,  
+	0x89, 0x05, 0x24, 0x10,  0x08, 0x00, 0x83, 0x16,  
+	0x09, 0x08, 0x7f, 0x39,  0x03, 0x1d, 0xfa, 0x29,  
+	0x83, 0x12, 0x89, 0x13,  0xa4, 0x10, 0x08, 0x00,  
+	0x83, 0x12, 0x08, 0x00,  0x8c, 0x19, 0xfc, 0x29,  
+	0x8b, 0x13, 0x00, 0x00,  0x0f, 0x08, 0x00, 0x00,  
+	0x8b, 0x17, 0x08, 0x00,  0x83, 0x12, 0x20, 0x30,  
+	0x84, 0x00, 0x80, 0x01,  0x84, 0x0a, 0x04, 0x08,  
+	0x80, 0x3c, 0x03, 0x1d,  0x07, 0x2a, 0xa0, 0x30,  
+	0x84, 0x00, 0x80, 0x01,  0x84, 0x0a, 0x04, 0x08,  
+	0xff, 0x3c, 0x03, 0x1d,  0x0f, 0x2a, 0x08, 0x00 
+	
+};
+
diff --git a/drivers/fims/usb/Makefile b/drivers/fims/usb/Makefile
new file mode 100644
index 000000000000..b9b97fa4d6b1
--- /dev/null
+++ b/drivers/fims/usb/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for the kernel FIM device drivers
+#
+
+ifneq ($(CONFIG_FIM_USB_MODULE),)
+	obj-m					+= fim_usb.o
+else
+	obj-$(CONFIG_FIM_ZERO_USB)		+= fim_usb.o
+	obj-$(CONFIG_FIM_ONE_USB)		+= fim_usb.o
+endif
diff --git a/drivers/fims/usb/README b/drivers/fims/usb/README
new file mode 100644
index 000000000000..55d7091ab467
--- /dev/null
+++ b/drivers/fims/usb/README
@@ -0,0 +1,12 @@
+
+README
+------
+
+[HOST]
+* Need to add a rule to udev for changing the write access to the
+  different device nodes (per default only root has write access)
+* Use the python script for writing to the EP by using libusb
+
+[TARGET]
+* Add a hotplug rull for starting an application that reads the
+  data from the serial port
diff --git a/drivers/fims/usb/fim_usb.c b/drivers/fims/usb/fim_usb.c
new file mode 100644
index 000000000000..2292c4c406a9
--- /dev/null
+++ b/drivers/fims/usb/fim_usb.c
@@ -0,0 +1,2030 @@
+/* -*- linux-c -*-
+ *
+ * driver/fims/usb/fim_usb.c
+ *
+ * Copyright (C) 2009 Digi International Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/timer.h>
+#include <linux/tty.h>
+#include <linux/serial.h>
+#include <linux/serial_core.h>
+#include <linux/reboot.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <linux/completion.h>
+
+#include <mach/fim-ns921x.h>
+#include <mach/hardware.h>
+#include <mach/gpio.h>
+#include <mach/regs-iohub-ns921x.h>
+
+/* #include "fim_usb.h" */
+/*
+ * If the driver is being compiled as a built-in driver, then include the header file
+ * which contains the firmware for this FIM-device
+ */
+#if !defined(MODULE)
+#include "fim_usb.h"
+extern const unsigned char fim_usb_firmware[];
+#define FIM_USB_FIRMWARE_FILE			(NULL)
+#define FIM_USB_FIRMWARE_CODE			fim_usb_firmware
+#else
+const unsigned char *fim_usb_firmware = NULL;
+#define FIM_USB_FIRMWARE_FILE			"fim_usb.bin"
+#define FIM_USB_FIRMWARE_CODE			fim_usb_firmware
+#endif
+
+/* Driver informations */
+#define DRIVER_VERSION				"1.0"
+#define FIM_DRIVER_AUTHOR			"Hector Oron, Luis Galdos"
+#define FIM_DRIVER_DESC				"FIM USB serial device driver"
+#define FIM_DRIVER_NAME				"fim-usb"
+#define FIM_USB_UART_DEV_NAME			"ttyFUSB"
+
+/* Module parameters */
+NS921X_FIM_NUMBERS_PARAM(fims_number);
+
+/* Macros used for the USB descriptors and resources */
+#define FIM_USB_MANU				"Digi"
+#define FIM_USB_PROD_DESC			"FIM-USBx" /* @FIXME: Cant change to an odd length */
+#define FIM_USB_SERIALNR			"NS921X"
+#define FIM_USB_IN_EPNR				(USB_DIR_IN | FIM_USB_INT_EP_NR)
+#define FIM_USB_OUT_EPNR			(USB_DIR_OUT | FIM_USB_INT_EP_NR)
+
+/* MAX GPIO NUMBER */
+#define FIM_USB_MAX_GPIOS			(6)
+
+#define FIM_USB_GPIO_DP				(0)
+#define FIM_USB_GPIO_DM				(1)
+#define FIM_USB_GPIO_RCV			(2)
+#define FIM_USB_GPIO_OE				(3)
+#define FIM_USB_GPIO_ENUM			(4)
+#define FIM_USB_GPIO_SPND			(5)
+
+/* Macros for controlling the enumeration GPIO of the MAX */
+#define FIM_USB_GPIO_ENUM_ENABLE		(1)
+#define FIM_USB_GPIO_ENUM_DISABLE		(0)
+
+
+#define FIM_USB_MAX_PACK_SIZE			(8)
+#define FIM_USB_INT_EP_NR			(1)
+
+
+/* Firmware dependent status/error flags */
+#define FIM_USB_BIT_STUFF_ERROR                 (1 << 0)
+#define FIM_USB_CRC_ERROR                       (1 << 1)
+#define FIM_USB_RX_OVERFLOW                     (1 << 2)
+#define FIM_USB_DEV_ADDR_NO_MATCH               (1 << 3)
+#define FIM_USB_SYNC_ERROR                      (1 << 4)
+#define FIM_USB_IN_DATA_SENT_MARKER             (1 << 5)
+#define FIM_USB_BUS_RESET                       (1 << 6)
+#define FIM_USB_KEEP_ALIVE                      (1 << 7)
+
+/* When a data packet is received it includes the sync, token, CRC, etc. */
+#define FIM_USB_DATA_OVERHEAD			(5)
+#define FIM_USB_DATA_HEAD			(2)
+#define FIM_USB_DATA_TAIL			(3)
+
+/*
+ * Since the FIMs doesn't support the CRC calculation and other additional features
+ * (like the setup of the different tokens), we must create the complete USB frame
+ * before putting it into the DMA-buffer (sucks!). So, we need the below token values
+ * for each frame.
+ *
+ * (Luis Galdos)
+ */
+#define USB_TOKEN_IN				(0x69)
+#define USB_TOKEN_OUT				(0xe1)
+#define USB_TOKEN_DATA0				(0xc3)
+#define USB_TOKEN_DATA1				(0x4b)
+#define USB_TOKEN_SETUP				(0x2d)
+#define USB_TOKEN_SYNC				(0x80)
+
+/* Configuration status registers of the FIM firmware */
+#define FIM_USB_ADDR_REG                        2
+#define FIM_USB_CONTROL3                        3
+#define FIM_USB_CONTROL4                        4
+#define FIM_USB_CONTROL5                        5
+#define FIM_USB_CONTROL6                        6
+
+#define FIM_USB_CTRL10_REG			(10)
+#define FIM_USB_STAT10_REG			(10)
+#define FIM_USB_CTRL3_REG			(3)
+#define FIM_USB_STAT3_REG			(3)
+#define FIM_USB_ALIVE_COUNT_REG			FIM_USB_CTRL10_REG
+#define FIM_USB_ALIVE_COUNT_STAT		FIM_USB_STAT10_REG
+
+/* Main control register */
+#define FIM_USB_MAIN_REG			(7)
+#define FIM_USB_MAIN_START			(1 << 0)
+#define FIM_USB_MAIN_NOKEEPALIVE		(1 << 1)
+#define FIM_USB_MAIN_NOINIRQ			(1 << 2)
+#define FIM_USB_MAIN_ALIVE_COUNT		(1 << 3)
+
+#define FIM_USB_REVISION_REG			(0)
+
+/* Macros for the configuration of the DMA-channel */
+#define FIM_USB_DMA_BUFFER_SIZE			(512)
+#define FIM_USB_DMA_RX_BUFFERS			(61)
+#define FIM_USB_DMA_TX_BUFFERS			(4)
+
+/* For printing messages (errors, warning, etc.) */
+#define pk_err(fmt, args...)			printk(KERN_ERR "[ ERROR ] fim-usb: " fmt, ## args)
+#define pk_info(fmt, args...)			printk(KERN_INFO "fim-usb: " fmt, ## args)
+#define pk_dbg(fmt, args...)			printk(KERN_DEBUG "fim-usb: " fmt, ## args)
+#define pk_dump_ctrl_packet(p)			printk(KERN_DEBUG "bRequestType 0x%02x | bRequest 0x%02x | " \
+							"wValue 0x%04x | wIndex 0x%04x | wLength %u\n", \
+							p->bRequestType, p->bRequest, \
+							p->wValue, p->wIndex, p->wLength);
+
+#if 0
+#define FIM_USB_DEBUG
+#endif
+
+/* Macros for printing debug messages */
+#ifdef FIM_USB_DEBUG
+#define dbg_func(fmt, args...)			printk(KERN_DEBUG "fim-usb: %s() " fmt, __func__, ## args)
+#define dbg_naked(fmt, args...)			printk(KERN_DEBUG "fim-usb: " fmt, ## args)
+#define dbg_pr(fmt, args...)			printk(KERN_DEBUG "fim-usb: " fmt, ## args)
+#else
+#define dbg_pr(fmt, args...)			do { } while (0)
+#define dbg_naked(fmt, args...)			do { } while (0)
+#define dbg_func(fmt, args...)			do { } while (0)
+#endif
+
+/* Enable the debug messages for the UART operations/functions */
+#if 0
+#define FIM_USB_UART_DEBUG
+#endif
+
+#ifdef FIM_USB_UART_DEBUG
+#define dbg_uart(fmt, args...)			printk(KERN_DEBUG "fim-usb UART: " fmt, ## args)
+#else
+#define dbg_uart(fmt, args...)			do { } while (0)
+#endif
+
+/* Enable the debug messages for the enumeration sequence */
+#if 0
+#define FIM_USB_ENUM_DEBUG
+#endif
+
+#ifdef FIM_USB_ENUM_DEBUG
+#define dbg_enum(fmt, args...)			printk(KERN_DEBUG "fim-usb ENUM: " fmt, ## args)
+#else
+#define dbg_enum(fmt, args...)			do { } while (0)
+#endif
+
+/* Enable the debug messages for the received tokens */
+#if 0
+#define FIM_USB_TOKEN_DEBUG
+#endif
+
+#ifdef FIM_USB_TOKEN_DEBUG
+#define dbg_token(fmt, args...)			printk(KERN_DEBUG "fim-usb TN: " fmt, ## args)
+#else
+#define dbg_token(fmt, args...)			do { } while (0)
+#endif
+
+/* Enable the debug messages for the received tokens */
+#if 0
+#define FIM_USB_WRITE_DEBUG
+#endif
+
+#ifdef FIM_USB_WRITE_DEBUG
+#define dbg_write(fmt, args...)			printk(KERN_DEBUG "fim-usb WRITE: " fmt, ## args)
+#else
+#define dbg_write(fmt, args...)			do { } while (0)
+#endif
+
+#if 0
+#define FIM_USB_FORCE_ENUM_TEST
+#endif
+
+/* Data structure used for passing a data buffer to the FIM core */
+struct fim_usb_in_frame {
+	u8 epnr;
+	u8 bytes;
+	u8 token;
+	u8 data[32];
+	u16 *crc;
+}__attribute__((__packed__));
+
+/*
+ * Let's use this structure for handling the received FIM-buffers
+ * 
+ */
+struct fim_usb_token {
+	unsigned long type:8;
+	unsigned long addr:7;
+	unsigned long ep:4;
+} __attribute__((packed));
+
+/* Structure for the data received after the OUT tokens */
+struct fim_usb_data {
+	u8 sync;     /* Sync token 0x80 */
+	u8 token;    /* DATA0 or DATA1 */
+	u8 data[32]; /* @XXX: Quick and dirty! Need a macro for this value */ 
+} __attribute__((packed));
+
+struct fim_usb_ep {
+	u8 nr;
+	u8 addr;
+
+	u16 bytes;
+	u16 requested;
+	u8 *data;
+	
+	u8 zlp;
+	u8 last_data_token;
+
+	struct fim_usb_port *port;
+	
+	struct work_struct tx_work; /* Worker for the IN transfers */
+	struct uart_port *tx_uart;
+	struct semaphore tx_sem;
+};
+
+struct fim_usb_port {
+	int					index;
+	struct fim_driver			fim;
+	struct device				*dev;
+ 	struct fim_gpio_t 			gpios[FIM_USB_MAX_GPIOS];
+	struct clk				*clk;
+	int 					reg;
+
+	/* We have three endpoints: control and two interrupts */
+#define FIM_USB_NR_EP_CTRL			(0)
+#define FIM_USB_NR_EP_OUT			(1)
+#define FIM_USB_NR_EP_IN			(2)
+#define FIM_USB_NR_EPS				(3)
+	struct fim_usb_ep			eps[FIM_USB_NR_EPS];
+	int					ep_next_data;
+
+	/* Data used for the emulation of the serial port */
+	struct uart_port uart;
+	struct work_struct uart_work;
+	int uart_created;
+	struct semaphore uart_sem;
+
+	/* Used for restarting the FIM after heavy failures */
+	struct work_struct restart_work;
+	struct completion restart_completed;
+
+	/* Timer used for detecting the hotplug events */
+	struct delayed_work hotplug_work;
+	int unplug_detected;
+#define FIM_USB_HOTPLUG_ALIVE_TIMER			(HZ / 5)
+#define FIM_USB_HOTPLUG_ALIVE_COUNTER			(150)
+};
+
+#define FIM_USB_STRING_MAX_LEN				(32)
+
+struct fim_usb_string {
+        u8 id;
+        const char *s;
+
+	/* This data will be send to the host */
+	struct usb_descriptor_header desc;
+	u16 data[FIM_USB_STRING_MAX_LEN];
+} __attribute__((packed));
+
+/* The default endpoint descriptors include the members for the audio extension (see include/linux/usb/ch9.h) */
+struct usb_endpoint_descriptor2 {
+        __u8  bLength;
+        __u8  bDescriptorType;
+
+        __u8  bEndpointAddress;
+        __u8  bmAttributes;
+        __le16 wMaxPacketSize;
+        __u8  bInterval;
+} __attribute__ ((packed));
+
+/* Let's keep it simple by using a static configuration descriptor */
+struct fim_usb_config_descriptor {
+	struct usb_config_descriptor config;
+	struct usb_interface_descriptor interface;
+	struct usb_endpoint_descriptor2 ep_in;
+	struct usb_endpoint_descriptor2 ep_out;
+} __attribute__((__packed__));
+
+
+/* Prototypes */
+static void fim_usb_regs_reset(struct fim_usb_port *port);
+static void fim_usb_uart_tx_work_func(struct work_struct *work);
+static int fim_usb_reboot_notifier_func(struct notifier_block *this, unsigned long code,
+					void *unused);
+static int fim_usb_restart_fim(struct fim_usb_port *port);
+
+static struct uart_driver fim_usb_uart_driver;
+static struct fim_usb_port **fim_usb_ports;
+static int fim_usb_ports_max;
+static struct notifier_block fim_usb_reboot_notifier = {
+	.notifier_call	= fim_usb_reboot_notifier_func,
+	.next		= NULL,
+	.priority	= 0
+};
+
+static inline struct fim_usb_port *port_from_fim(struct fim_driver *fim)
+{
+	return fim->driver_data;
+}
+
+static inline struct tty_struct *tty_from_port(struct fim_usb_port *port)
+{
+        struct uart_info *info;
+
+        info = port->uart.info;
+        return (info) ? info->port.tty : NULL;
+}
+
+static inline struct fim_usb_port *port_from_uart(struct uart_port *uart)
+{
+        return dev_get_drvdata(uart->dev);
+}
+
+static void fim_usb_enable_pullup(struct fim_usb_port *port)
+{	
+	dbg_func("FIM %i\n", port->index);
+
+	/*
+	 * Enable the pulldown for the enumeration process and remove the suspend
+	 * control for starting the externa PHY. The other GPIOs are controller by
+	 * the FIM, specially the Output Enable.
+	 */
+	gpio_set_value(port->gpios[FIM_USB_GPIO_SPND].nr, 0);
+	gpio_set_value(port->gpios[FIM_USB_GPIO_ENUM].nr, 1);
+}
+
+static void fim_usb_disable_pullup(struct fim_usb_port *port)
+{	
+	dbg_func("FIM %i\n", port->index);
+
+	/* Disable the pulldown and set the PHY in the suspend mode */
+	gpio_set_value(port->gpios[FIM_USB_GPIO_ENUM].nr, 0);
+	gpio_set_value(port->gpios[FIM_USB_GPIO_SPND].nr, 1);
+}
+
+static void fim_usb_regs_reset(struct fim_usb_port *port)
+{
+        /* FIM control registers configuration */
+        fim_set_ctrl_reg(&port->fim, 0, 0);
+        fim_set_ctrl_reg(&port->fim, 1, 0);
+        fim_set_ctrl_reg(&port->fim, 2, 0);
+        fim_set_ctrl_reg(&port->fim, 3, 0);
+        fim_set_ctrl_reg(&port->fim, 4, 0);
+        fim_set_ctrl_reg(&port->fim, 5, 0);
+        fim_set_ctrl_reg(&port->fim, 6, 0);
+	fim_set_ctrl_reg(&port->fim, FIM_USB_MAIN_REG, 0);
+}
+
+/*
+ * According to the function[1] of the NetOS driver, we must first configure the
+ * dedicated GPIOs
+ *
+ * [1] iop_config.c:naIopUsbInit()
+ */
+static int fim_usb_config(struct fim_usb_port *port)
+{
+        unsigned long clks_per_bit;
+	unsigned long timer0_init_value;
+	struct fim_driver *fim;
+	int offset;
+	struct fim_gpio_t *gpios;
+	
+	fim = &port->fim;
+	gpios = port->gpios;
+ 
+        /* Depending on the processor we have different offset */
+        if (processor_is_ns9215())
+                offset = 68;
+        else if (processor_is_ns9210())
+                offset = 0;
+        else
+                return -EINVAL;
+	
+        /* FIM control registers configuration */
+	fim_set_ctrl_reg(fim, 3, 1 << (gpios[FIM_USB_GPIO_DP].nr - offset));
+	fim_set_ctrl_reg(fim, 4, 1 << (gpios[FIM_USB_GPIO_DM].nr - offset));
+	fim_set_ctrl_reg(fim, 5, 1 << (gpios[FIM_USB_GPIO_RCV].nr - offset));
+	fim_set_ctrl_reg(fim, 6, 1 << (gpios[FIM_USB_GPIO_OE].nr - offset));
+	
+        /* Calculate timer setting */
+        /* FIM init Configuration. XXX port->clk must be MHz */
+        clks_per_bit = clk_get_rate(port->clk) / 1000000;
+        clks_per_bit = (clks_per_bit * 2) / 3;
+        dbg_pr("Clocks Per Bit: 0x%lx [Sysclock %lu]\n", clks_per_bit,
+	       clk_get_rate(port->clk)); 
+
+        /* Sanity check for clock */
+        if ((clks_per_bit > 270) || (clks_per_bit < 0)) {
+                pk_err("Invalid calculated clocks per bit %lu\n", clks_per_bit); 
+		return -EINVAL;
+        }
+
+        /* Load values according to the NetOS driver */
+        fim_set_ctrl_reg(&port->fim, 0, 231);
+
+	timer0_init_value = 256 - (clks_per_bit - 11);
+        fim_set_ctrl_reg(&port->fim, 1, timer0_init_value);
+
+        /* Set the initial device address to zero */
+        fim_set_ctrl_reg(&port->fim, FIM_USB_ADDR_REG, 0);
+
+	return 0;
+}
+
+/* We only provide two interrupt endpoints and one control EP */
+static void fim_usb_init_eps(struct fim_usb_port *port)
+{
+	struct fim_usb_ep *ep;
+
+	memset(&port->eps, 0, sizeof(port->eps));
+	
+	/* Setup the control EP0 */
+	ep = &port->eps[FIM_USB_NR_EP_CTRL];
+	ep->addr = 0;
+	ep->port = port;
+	ep->nr = FIM_USB_NR_EP_CTRL;
+	
+	/* Setup the OUT EP1 */
+	ep = &port->eps[FIM_USB_NR_EP_OUT];
+	ep->addr = FIM_USB_INT_EP_NR;
+	ep->port = port;
+	ep->nr = FIM_USB_NR_EP_OUT;
+	
+	/* Setup the IN EP1. Init the worker for the IN transfers too! */
+	ep = &port->eps[FIM_USB_NR_EP_IN];
+	ep->addr = FIM_USB_INT_EP_NR;
+	INIT_WORK(&ep->tx_work, fim_usb_uart_tx_work_func);
+	ep->port = port;
+	ep->nr = FIM_USB_NR_EP_IN;
+
+	/* @XXX: Define a correct reset value for this member */
+	port->ep_next_data = 0;
+}
+
+/* Be sure that you have disconnected the pull-up first */
+static void fim_usb_init_port(struct fim_usb_port *port)
+{
+	/* Reset the registers */
+	fim_usb_regs_reset(port);
+
+	/* Download config to enpoint buffer */
+	fim_usb_config(port);
+	fim_usb_init_eps(port);
+}
+
+static int fim_usb_reboot_notifier_func(struct notifier_block *this, unsigned long code,
+					void *unused)
+{
+	struct fim_usb_port *port;
+	int cnt;
+
+	for (cnt = 0; cnt < fim_usb_ports_max; cnt++) {
+		port = fim_usb_ports[cnt];
+		if (!port)
+			continue;
+		
+		fim_usb_disable_pullup(port);
+	}
+
+	return NOTIFY_DONE;
+}
+
+static void fim_usb_hotplug_work_func(struct work_struct *work)
+{
+	struct fim_driver *fim;
+	int reg;
+	int cnt_curr, cnt_init;
+	struct fim_usb_port *port;
+
+	port = container_of(work, struct fim_usb_port, hotplug_work.work);
+	fim = &port->fim;
+
+	/* Get the status of the main register and the current countdown value */
+	fim_get_ctrl_reg(fim, FIM_USB_MAIN_REG, &reg);
+	fim_get_stat_reg(fim, FIM_USB_ALIVE_COUNT_STAT, &cnt_init);
+	
+	/* Set the initial value for the countdown register */
+	fim_set_ctrl_reg(fim, FIM_USB_ALIVE_COUNT_REG, FIM_USB_HOTPLUG_ALIVE_COUNTER);
+
+	/* Inform the FIM about the new reloaded value */
+	fim_set_ctrl_reg(fim, FIM_USB_MAIN_REG, reg | FIM_USB_MAIN_ALIVE_COUNT);
+
+	/* We need to wait for the status register */
+	msleep_interruptible(50);
+	fim_get_stat_reg(fim, FIM_USB_ALIVE_COUNT_STAT, &cnt_curr);
+
+	/* Reset the flag of the control register */
+	fim_set_ctrl_reg(fim, FIM_USB_MAIN_REG, reg);
+	
+	/* Re-trigger the timer function if all looks OK */
+	if (cnt_init != cnt_curr) {
+		port->unplug_detected = 0;
+		schedule_delayed_work(&port->hotplug_work, FIM_USB_HOTPLUG_ALIVE_TIMER);
+	} else {
+		/* In the case that the timer has expired we need to wait for the RXNRIP */
+		port->unplug_detected = 1;
+
+		/* Remove the serial port for canceling the UART operations */
+		if (port->uart_created) {
+			struct uart_port *uart;
+			
+			uart = &port->uart;
+			uart_remove_one_port(&fim_usb_uart_driver, uart);
+			port->uart_created = 0;
+		}
+
+		fim_usb_restart_fim(port);
+		
+		pk_dbg("Keep alive counter expired (init 0x%x | curr 0x%x). Port %i removed\n",
+		       cnt_init, cnt_curr, fim->picnr);
+	}
+}
+
+/* Function used for creating or removing the UART port of this FIM */
+static struct uart_ops fim_usb_uart_ops;
+static void fim_usb_uart_work_func(struct work_struct *work)
+{
+	struct fim_usb_port *port;
+	int ret;
+	struct uart_port *uart;
+	struct fim_driver *fim;
+
+	port = container_of(work, struct fim_usb_port, uart_work);
+	uart = &port->uart;
+	fim = &port->fim;
+
+	/*
+	 * This can happen when the device entered the suspend mode and restarts the
+	 * enumeration sequence
+	 */
+	if (port->uart_created) {
+		pk_dbg("UART port %i already created.\n", fim->picnr);
+		goto exit_sched;
+	}
+
+	/* We really need to restart the structure at this place */
+	uart->ops = &fim_usb_uart_ops;
+	uart->flags = UPF_BOOT_AUTOCONF;
+	uart->dev = port->dev;
+	uart->type = UPIO_MEM;
+	uart->line = fim->picnr; /* Use the FIM number as minor */
+	uart->iotype = UPIO_PORT;
+	dev_set_drvdata(uart->dev, port);
+
+	/*
+	 * @FIXME: Lock this section, otherwise we will have some problems by the plug-unplug
+	 * tests of the SALAB.
+	 */
+	pk_dbg("Adding the new port %i\n", fim->picnr);
+	ret = uart_add_one_port(&fim_usb_uart_driver, uart);
+	if (ret) {
+                dev_set_drvdata(uart->dev, NULL);
+                pk_err("Couldn't register the UART port %i\n", fim->picnr);
+                goto err_add;
+        }
+
+	port->uart_created = 1;
+
+ exit_sched:
+	/* @FIXME: Is this the correct place for our FIM timer loader? */
+	schedule_delayed_work(&port->hotplug_work, 0);
+	
+ err_add:
+	return;
+}
+
+/*
+ * Send a buffer over the FIM-API. We are using static buffers for sending the data
+ * to the FIMs, we don't need to have a new allocated buffer, since the FIM only accepts
+ * frames with a maximal sizo of 13 bytes (8 with the EP data)
+ * (Luis Galdos)
+ */
+static int __fim_usb_ep_send_buffer(struct fim_usb_port *port, void *buffer, int len, void *priv)
+{
+        struct fim_driver *fim;
+        int retval;
+	struct fim_buffer_t buf;
+
+        if (!port || !len)
+                return -EINVAL;
+
+        fim = &port->fim;
+	
+        buf.private = priv;
+	buf.data = buffer;
+	buf.length = len;
+
+	retval = fim_send_buffer(fim, &buf);
+        if (retval) {
+		int level;
+		
+		level = fim_tx_buffers_level(&port->fim);
+		pk_dbg("FIM%i send %i bytes failed (err %i, level %i)\n",
+		       fim->picnr, buf.length, retval, level);
+		schedule_work(&port->restart_work);
+	}
+	
+        return retval;
+}
+
+/* Calculate the CRC16 for the passed buffer */
+static inline u16 fim_usb_calc_buffer_crc(const u8 *buffer, unsigned long length)
+{
+	u16 retval, bx;
+	unsigned char crc16;
+	int i;
+	ulong cnt;
+
+	retval = 0xffff;
+	for (cnt = 0; cnt <length; cnt++) {
+		for (i=0; i < 8; i++) {
+			crc16 = retval >> (15 - i);
+			bx = crc16 ^ buffer[cnt];
+			retval <<= 1;
+			
+			if (bx & (0x1 << i))
+				retval ^= 0x8005;
+		}
+	}
+
+	bx = ~retval;
+	retval = 0;
+	for (i=0; i < 8; i++)
+		retval |= ((bx & (0x1 << i)) << (15 - (i * 2)) |
+			   (bx & (0x100 << i)) >> (1 + (i * 2)));
+	
+	return retval;
+}
+
+#define fim_usb_ep_next_data_token(ep)			((ep->last_data_token == USB_TOKEN_DATA0) ? \
+								USB_TOKEN_DATA1 : USB_TOKEN_DATA0)
+
+/*
+ * Call this function for sending a ZLP by the passed EP
+ * If force is true, the function will ignore the current TX fifo level and will queue
+ * the new IN data packet.
+ */
+static int fim_usb_write_zlp(struct fim_usb_port *port, struct fim_usb_ep *ep, int force)
+{
+	struct fim_usb_in_frame fin;
+	int level;
+	
+	dbg_write("EP%u: Sending ZLP\n", ep->nr);
+
+	level = fim_tx_buffers_level(&port->fim);
+	if (level && !force) {
+		pk_dbg("ZLP: TX level > 0 (%i). FIM restart.\n", level);
+		fim_usb_restart_fim(port);
+		return -ERESTART;
+	}
+
+	memset(&fin, 0, sizeof(fin));
+	fin.epnr = ep->addr & ~USB_DIR_IN;
+	fin.bytes = FIM_USB_DATA_TAIL;
+	fin.token = fim_usb_ep_next_data_token(ep);
+
+	/* @XXX: Reset this flag depending on the return value of the send buffer function */
+	ep->zlp = 0;
+	
+	return __fim_usb_ep_send_buffer(port, &fin, FIM_USB_DATA_OVERHEAD, ep);
+}
+
+/* IMPORTANT: This function returns the number of transferred bytes! */
+static int fim_usb_write_packet(struct fim_usb_port *port, struct fim_usb_ep *ep, u8 *buf, u16 len)
+{
+	u16 crc, val;
+	struct fim_usb_in_frame fin;
+	int ret, level;
+
+	/* @FIXME: Use a correct sanity check! */
+	level = fim_tx_buffers_level(&port->fim);
+	if (level) {
+		pk_dbg("write packet: TX level = %i. FIM restart.\n", level);
+		fim_usb_restart_fim(port);
+		return -EINVAL;
+	}
+		
+	val = min(len, (u16)FIM_USB_MAX_PACK_SIZE);
+	ep->bytes = len - val;
+	ep->data = buf + val;
+	ep->requested -= val;
+	dbg_enum("EP%u: Sending %u bytes [%p], pending %u [%p], requested %u\n",
+		 ep->nr, val, buf, ep->bytes, ep->data, ep->requested);
+		
+	/*
+	 * The FIM expects that the IN-data is placed in the TX-FIFO.
+	 *
+	 */
+	crc = fim_usb_calc_buffer_crc(buf, val);
+#if defined(FIM_USB_DEBUG_SEND_BUFFER)
+	dbg_func("Calculated CRC 0x%04x\n", crc);
+#endif
+
+	/* Format: Endpoint number | Number of bytes | Token | Data | CRC16 */
+	fin.epnr = ep->addr & ~USB_DIR_IN;
+	fin.bytes = val + FIM_USB_DATA_TAIL; /* Token and CRC16 */
+
+	fin.token = fim_usb_ep_next_data_token(ep);
+	ep->last_data_token = fin.token;
+
+	fin.crc = (u16 *)(fin.data + val);
+	memcpy(fin.data, buf, val);
+
+	*fin.crc = crc;
+	ret = __fim_usb_ep_send_buffer(port, &fin, val + FIM_USB_DATA_OVERHEAD, ep);
+	return ret ? ret : val;
+}
+
+#define FIM_USB_VENDOR_ID			(0x0210)
+#define FIM_USB_PRODUCT_ID			(0xbe13)
+
+#define STRING_MANUFACTURER			1
+#define STRING_PRODUCT				2
+#define STRING_SERIALNUM			3
+
+#define DEV_CONFIG_VALUE			1
+#define FIM_USB_INTERFACE			0
+
+static struct usb_device_descriptor fim_usb_device_desc = {
+        .bLength =              sizeof(fim_usb_device_desc),
+        .bDescriptorType =      USB_DT_DEVICE,
+        .bcdUSB =               __constant_cpu_to_le16(0x0110),
+        .bDeviceClass =         0,
+        .bDeviceSubClass =      0,
+        .bDeviceProtocol =      0,
+	.bMaxPacketSize0 =	8,
+        .idVendor =             __constant_cpu_to_le16(FIM_USB_VENDOR_ID),
+        .idProduct =            __constant_cpu_to_le16(FIM_USB_PRODUCT_ID),
+        .iManufacturer =        STRING_MANUFACTURER,
+        .iProduct =             STRING_PRODUCT,
+        .iSerialNumber =        STRING_SERIALNUM,
+        .bNumConfigurations =   1
+};
+
+static struct fim_usb_config_descriptor fim_usb_config_desc2 = {
+	.config = {
+		.bLength =              USB_DT_CONFIG_SIZE,
+		.bDescriptorType =      USB_DT_CONFIG,
+		
+		/* compute wTotalLength on the fly */
+		.bNumInterfaces =       1,
+		.bConfigurationValue =  DEV_CONFIG_VALUE,
+		.iConfiguration =       0,
+		.bmAttributes =         USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
+		.bMaxPower =            0
+	},
+	.interface = {
+		.bLength =              USB_DT_INTERFACE_SIZE,
+		.bDescriptorType =      USB_DT_INTERFACE,
+		.bInterfaceNumber =     FIM_USB_INTERFACE,
+		.bNumEndpoints =        2,
+		.bInterfaceClass =      0,
+		.bInterfaceSubClass =   0,
+		.bInterfaceProtocol =   0,
+		.iInterface =           0
+	},
+	.ep_out = {
+		.bLength =              USB_DT_ENDPOINT_SIZE,
+		.bDescriptorType =      USB_DT_ENDPOINT,
+		.bEndpointAddress =     FIM_USB_OUT_EPNR,
+		.bmAttributes =         USB_ENDPOINT_XFER_INT,
+		.wMaxPacketSize =	__constant_cpu_to_le16(FIM_USB_MAX_PACK_SIZE),
+		.bInterval =		1,
+	},
+	.ep_in = {
+		.bLength =              USB_DT_ENDPOINT_SIZE,
+		.bDescriptorType =      USB_DT_ENDPOINT,
+		.bEndpointAddress =     FIM_USB_IN_EPNR,
+		.bmAttributes =         USB_ENDPOINT_XFER_INT,
+		.wMaxPacketSize =	__constant_cpu_to_le16(FIM_USB_MAX_PACK_SIZE),
+		.bInterval =		1,
+	}
+};
+
+static struct fim_usb_string fim_usb_strings [] = {
+        {
+		.id = STRING_MANUFACTURER,
+		.s  = FIM_USB_MANU
+	}, {
+		.id = STRING_PRODUCT,
+		.s  = FIM_USB_PROD_DESC
+	}, {
+		.id = STRING_SERIALNUM,
+		.s  = FIM_USB_SERIALNR
+	}
+};
+
+/* Send the requested string descriptor to the host */
+static int fim_usb_send_string_descriptor(struct fim_usb_port *port, struct fim_usb_ep *ep,
+					  struct usb_ctrlrequest *ctrl)
+{
+	int ret, cnt;
+	u8 value;
+
+	ret = -EINVAL;
+	value = le16_to_cpu(ctrl->wValue);
+	
+	if (value == 0) {
+		struct usb_string_descriptor desc;
+		
+		desc.bLength = sizeof(desc);
+		desc.bDescriptorType = USB_DT_STRING;
+		desc.wData[0] = __constant_cpu_to_le16(0x0409);
+		ret = fim_usb_write_packet(port, ep, (u8 *)&desc, sizeof(desc));
+
+	} else {
+		struct fim_usb_string *pstr;
+
+		pstr = NULL;
+		for (cnt = 0; cnt < ARRAY_SIZE(fim_usb_strings); cnt++) {
+			if (fim_usb_strings[cnt].id == value) {
+				pstr = &fim_usb_strings[cnt];
+				break;
+			}
+		}
+
+		if (!pstr) {
+			pk_dbg("Unavailable string descriptor 0x%04x request!\n", value);
+			ret = -EINVAL;
+			goto exit_str_desc;
+		}
+
+		/* OK, we have a string descriptor, send it now! */
+		ret = fim_usb_write_packet(port, ep, (u8 *)&pstr->desc, pstr->desc.bLength);
+	}
+
+ exit_str_desc:
+	return ret;
+}
+
+static int fim_usb_handle_ep0_get_descriptor(struct fim_usb_port *port, struct usb_ctrlrequest *ctrl)
+{
+	u8 desc;
+	u16 wvalue, len, total, val;
+	struct fim_usb_ep *ep;
+	int ret;
+
+	len = le16_to_cpu(ctrl->wLength);
+	wvalue = le16_to_cpu(ctrl->wValue);
+	desc = wvalue >> 8;
+	ep = &port->eps[FIM_USB_NR_EP_CTRL];
+
+	ep->requested = len;
+	dbg_enum("Handling get descriptor 0x%02x (len %u)\n", desc, len);
+
+	ret = 0;
+	switch (desc) {
+
+		/* Descriptor type device */
+	case USB_DT_DEVICE:
+		ep->last_data_token = USB_TOKEN_DATA0;
+		ret = fim_usb_write_packet(port, ep, (u8 *)&fim_usb_device_desc, sizeof(fim_usb_device_desc));
+		break;
+
+		/* Descriptor type configuration */
+	case USB_DT_CONFIG:
+		ep->last_data_token = USB_TOKEN_DATA0;
+
+		/* Setup the total length of the configuration descriptor */
+		total = sizeof(fim_usb_config_desc2);
+		val = min(len, total);
+		fim_usb_config_desc2.config.wTotalLength = cpu_to_le16(total);
+
+		ret = fim_usb_write_packet(port, ep, (u8 *)&fim_usb_config_desc2, val);
+		dbg_enum("Configuration desc. size = %u | sent %u | requested %u\n",
+			 total, ret, ep->requested);
+		break;
+
+		/* Descriptor type string */
+	case USB_DT_STRING:
+
+		ep->last_data_token = USB_TOKEN_DATA0;
+		
+		/* According to the spec: by index zero return the table with the supported languages */
+		ret = fim_usb_send_string_descriptor(port, ep, ctrl);
+		break;
+
+	default:
+		pk_err("Unhandled GET descriptor request 0x%02x\n", desc);
+		break;
+	}
+
+	return ret;
+}
+
+static int fim_usb_handle_ep0_standard(struct fim_usb_port *port, struct usb_ctrlrequest *ctrl)
+{
+	int ret = 0;
+	struct fim_usb_ep *ep;
+	u8 addr;
+	u16 value, idx, type;
+	int level;
+
+	ep = &port->eps[FIM_USB_NR_EP_CTRL];
+
+	switch (ctrl->bRequest) {
+	case USB_REQ_GET_DESCRIPTOR:
+		if (ctrl->bRequestType != USB_DIR_IN)
+			break;
+
+		dbg_enum("Get descriptor 0x%02x\n", le16_to_cpu(ctrl->wValue));
+		ret = fim_usb_handle_ep0_get_descriptor(port, ctrl);
+		break;
+
+		/* We must responde with a zero packet length */
+	case USB_REQ_SET_ADDRESS:
+		addr = le16_to_cpu(ctrl->wValue);
+		ep->last_data_token = USB_TOKEN_DATA0;
+		fim_usb_write_zlp(port, ep, 0);
+
+		/* @FIXME: Arrgghhh...! Give the FIM some time for sending the ZLP */
+		udelay(1500);
+		
+		fim_set_ctrl_reg(&port->fim, FIM_USB_ADDR_REG, addr);
+		dbg_enum("Set address request (%d)\n", addr);
+		break;
+
+		/* @FIXME: Check for the configuration number */
+	case USB_REQ_SET_CONFIGURATION:
+		value = le16_to_cpu(ctrl->wValue);
+		fim_usb_write_zlp(port, ep, 0);
+		dbg_enum("Set configuration %u request\n", value);
+
+		/* So, let's create the serial device port */
+		schedule_work(&port->uart_work);
+		break;
+
+		/* @FIXME: Check for the interface number */
+	case USB_REQ_SET_INTERFACE:
+
+		level = fim_tx_buffers_level(&port->fim);
+		if (level) {
+
+			/*
+			 * That's really bad, cause the host wants to start a new data transfer but
+			 * the TX FIFO is not free!
+			 */
+			pk_dbg("TX FIFO restart for SET interface (level = %i)\n", level);
+			schedule_work(&port->restart_work);
+			return -ERESTART;
+		}
+		
+		value = le16_to_cpu(ctrl->wValue);
+		fim_usb_write_zlp(port, ep, 0);
+		dbg_enum("Set configuration %u request\n", value);
+		break;
+
+	case USB_REQ_CLEAR_FEATURE:
+		idx = le16_to_cpu(ctrl->wIndex);
+		value = le16_to_cpu(ctrl->wValue);
+		type = le16_to_cpu(ctrl->bRequest);
+		dbg_enum("Clear feature type 0x%x | index 0x%x | value 0x%0x\n",
+			 type, idx, value);
+
+		/* Only EPs supported now */
+		if (type != 2 && (!idx || idx == FIM_USB_IN_EPNR || idx == FIM_USB_OUT_EPNR))
+			fim_usb_write_zlp(port, ep, 0);
+		else {
+			/* @FIXME: Send a STALL at this point */
+			pk_err("Invalid clear feature request (idx 0x%x | type 0x%x)\n", idx, type);
+			ret = -EINVAL;
+		}
+		
+		break;
+		
+	default:
+		pk_err("Unhandled standard request 0x%02x\n", ctrl->bRequest);
+		pk_dump_ctrl_packet(ctrl);
+		/* @FIXME: Send a STALL to the host! */
+		break;
+	}
+	
+	return ret;
+}
+
+static int fim_usb_handle_ep1_out(struct fim_usb_port *port, u8 *data, int len)
+{
+	struct tty_struct *tty;
+	int count;
+
+	dbg_uart("FIM%i: Writing %i bytes into UART fifo\n", port->index, len);
+	
+	tty = tty_from_port(port);
+	if (!port->uart_created || !tty) {
+
+		/* This happens when the serial port was not opened yet */
+		dbg_uart("Found an uninitialized port %p | tty %p\n", port, tty);
+		return -ENODEV;
+	}
+
+	/* Don't copy more bytes than there is room for in the buffer */
+	count = tty_buffer_request_room(tty, len);
+	if (count < len) {
+		pk_dbg("Dropping %i chars [ len %i > %i count ]\n", len - count, len, count);
+		len = count;
+
+		if (len == 0xfffe)
+			return 0;
+	}
+	
+	tty_insert_flip_string(tty, data, len);
+        tty_flip_buffer_push(tty);
+
+	return 0;
+}
+
+/* Handle the requests arrived for the EP0 (control EP) */
+static int fim_usb_handle_ep0(struct fim_usb_port *port,
+			      struct usb_ctrlrequest *pctrl)
+{
+	int ret = 0;
+	u16 windex;
+
+	if (!pctrl || !port)
+		return -ENODEV;
+
+	windex = le16_to_cpu(pctrl->wIndex);
+	
+	switch (pctrl->bRequestType & USB_TYPE_MASK) {
+
+	case USB_TYPE_STANDARD:
+		ret = fim_usb_handle_ep0_standard(port, pctrl);
+		break;
+
+	default:
+		pk_err("bRequestType 0x%02x | bRequest 0x%02x | "
+		       "wValue 0x%04x | wIndex 0x%04x | wLength %u\n",
+		       pctrl->bRequestType, pctrl->bRequest,
+		       pctrl->wValue, pctrl->wIndex, pctrl->wLength);
+		break;
+	}
+	
+	return ret;
+}
+
+static void fim_usb_error_isr(struct fim_driver *driver, ulong rx_err, ulong tx_err)
+{
+	struct fim_usb_port *port;
+
+	port = driver->driver_data;
+
+	if ((rx_err & IOHUB_IFS_RXNRIP) || port->unplug_detected) {
+
+		if (!work_pending(&port->restart_work)) {
+			/* pk_dbg("Scheduling restart worker\n"); */
+			schedule_work(&port->restart_work);
+		}
+	} else
+		pk_dbg("Unhandled FIM error (rx 0x%lx | tx 0x%lx)\n", rx_err, tx_err);
+}
+
+/*
+ * Handler for the incoming FIM-interrupts. Available interrupts:
+ * -
+ */
+static void fim_usb_isr(struct fim_driver *driver, int irq, unsigned char code,
+		       unsigned int rx_fifo)
+{
+	struct fim_usb_port *port;
+	
+        dbg_func("FIM IRQ %i\n", irq);
+	port = port_from_fim(driver);
+
+	switch (code) {
+	default:
+		pk_err("Unknown IRQ %i | FIM %i | %x\n",
+			   code, port->fim.picnr, rx_fifo);
+		break;
+	}
+}
+
+/*
+ * This is the TX-callback that the API call after a DMA-package was closed
+ * The fim buffer structure contains our internal private data
+ * Free the allocated FIM-buffer that was used for sending the DMA-data
+ */
+static void fim_usb_tx_isr(struct fim_driver *fim, int irq,
+			  struct fim_buffer_t *pdata)
+{
+	struct fim_usb_port *port;
+	struct fim_usb_ep *ep;
+	struct uart_port *uart;
+	struct circ_buf *xmit;
+
+	dbg_write("TX IRQ\n");
+
+	/* Reset the internal buffer descriptor of the EP */
+	port = fim->driver_data;
+
+	/* Here we can start the next data transfer if there is something pending */
+	ep = &port->eps[FIM_USB_NR_EP_CTRL];
+	if (!ep->addr && ep->bytes) {
+		int sent;
+		
+		sent = fim_usb_write_packet(port, ep, ep->data, ep->bytes);
+		if (sent == FIM_USB_MAX_PACK_SIZE && !ep->bytes && ep->requested) {
+			fim_usb_write_zlp(port, ep, 1);
+			dbg_enum("ZLP for big packet (request pending %u)\n", ep->requested);
+		}
+		
+		/* @XXX: Should we return at this point? */
+		/* return; */
+	}
+
+	/* Schedule the work queue at this point if there is some remaining data */
+	ep = &port->eps[FIM_USB_NR_EP_IN];
+	uart = ep->tx_uart;
+	if (uart) {
+
+		/* The worker will wakeup the device if the circular buffer is empty */
+		xmit = &uart->info->xmit;
+		if (xmit && !uart_circ_empty(xmit) && !uart_tx_stopped(uart)) {
+			dbg_uart("EP%02x Scheduling TX work\n", ep->addr);
+			schedule_work(&ep->tx_work);
+		}
+	}
+}
+
+static int fim_usb_restart_fim(struct fim_usb_port *port)
+{
+        int ret;
+	struct fim_driver *fim;
+
+        if (!port)
+                return -ENODEV;
+
+	fim = &port->fim;
+
+	fim_usb_disable_pullup(port);
+
+	/* Reset the main register */
+	fim_usb_regs_reset(port);
+	
+	/* First try to stop the FIM */
+	fim_disable_irq(fim);
+	ret = fim_send_stop(fim);
+        if (ret) {
+		pk_err("Couldn't stop the FIM%i\n", fim->picnr);
+                return ret;
+	}
+
+	fim_dma_stop(&port->fim);
+	fim_dma_start(&port->fim, NULL);
+	
+        ret = fim_send_start(fim);
+        if (ret) {
+		pk_err("Couldn't start the FIM%i\n", fim->picnr);
+                return -EAGAIN;
+	}
+	
+	/* Restart the DMA channel when we know that we are safe! */
+        fim_enable_irq(fim);
+	
+	/* This function resets the address number to zero */
+	fim_usb_init_port(port);
+
+	/* This will enable the start of the FIM in the firmware */
+	fim_set_ctrl_reg(&port->fim, FIM_USB_MAIN_REG,
+			 FIM_USB_MAIN_START |  FIM_USB_MAIN_NOKEEPALIVE | FIM_USB_MAIN_NOINIRQ);
+
+	fim_usb_enable_pullup(port);
+	
+	return ret;
+}
+
+static void fim_usb_restart_work_func(struct work_struct *work)
+{
+	struct fim_usb_port *port;
+
+	port = container_of(work, struct fim_usb_port, restart_work);
+	fim_usb_restart_fim(port);
+	complete(&port->restart_completed);
+}
+
+/* Handle the bus resets reported by the FIM */
+static void fim_usb_bus_reset(struct fim_usb_port *port)
+{
+	unsigned int addr;
+	int level;
+
+	dbg_enum("Bus reset (addr = 0)\n");
+
+	/* We need to restore the TX buffers at this point */
+	fim_get_ctrl_reg(&port->fim, FIM_USB_ADDR_REG, &addr);
+	level = fim_tx_buffers_level(&port->fim);
+	if (level && addr) {
+		dbg_enum("Need to restart the FIM (level %i, curr addr. %u)\n",
+			 level, addr);
+		schedule_work(&port->restart_work);
+	} else {
+		addr = 0;
+		fim_set_ctrl_reg(&port->fim, FIM_USB_ADDR_REG, addr);
+	}
+}
+
+/*
+ * Called when a receive DMA-buffer was closed.
+ * The first byte contains the status of the closed buffer.
+ */
+static void fim_usb_rx_isr(struct fim_driver *fim, int irq,
+			   struct fim_buffer_t *pdata)
+{
+	u8 status, err;
+	struct usb_ctrlrequest *pctrl;
+	struct fim_usb_port *port;
+	struct fim_usb_ep *ep;
+	struct fim_usb_token *token;
+
+	port = fim->driver_data;
+	status = *(pdata->data);
+	err = *(pdata->data + pdata->length);
+	token = (struct fim_usb_token *)(pdata->data + 1);
+
+	if (pdata->length == 1) {
+
+		switch (status) {
+		
+		case FIM_USB_BUS_RESET:
+			dbg_pr("Bus reset received!\n");
+			fim_usb_bus_reset(port);
+			break;
+
+		case FIM_USB_KEEP_ALIVE:
+			dbg_pr("Alive\n");
+			break;
+
+		case FIM_USB_IN_DATA_SENT_MARKER:
+			dbg_write("FIM sending IN frame\n");
+			break;
+
+		default:
+			dbg_pr("Unexpected status 0x%x\n", status);
+			break;
+		}
+		
+	} else {
+		
+		switch (token->type) {
+
+		case USB_TOKEN_DATA0:
+		case USB_TOKEN_DATA1:
+			dbg_token("DATA%c %u bytes for EP%u\n",
+			       (token->type == USB_TOKEN_DATA0) ? '0' : '1',
+			       pdata->length, port->ep_next_data);
+			
+			/* The complete frame contains the sync byte, the CRC and the error */
+			if (port->ep_next_data == 0) {
+
+				/* @XXX: Stupid sanity check for let it be for now */
+				if (pdata->length != FIM_USB_DATA_OVERHEAD && pdata->length != FIM_USB_DATA_TAIL) { 
+					pctrl = (struct usb_ctrlrequest *)(pdata->data + 2);
+					fim_usb_handle_ep0(port, pctrl);
+				}
+				
+			} else {
+				struct fim_usb_data *fdata;
+				u16 bytes;
+
+				/* Remove the overhead from the data frame */
+				bytes = pdata->length - FIM_USB_DATA_OVERHEAD;
+				fdata = (struct fim_usb_data *)pdata->data;
+
+				/* We have only one EP, so let us call it directly from here */
+				fim_usb_handle_ep1_out(port, pdata->data + FIM_USB_DATA_HEAD, bytes);
+			}
+			break;
+
+			/* Get the endpoint number for the incoming data */
+		case USB_TOKEN_OUT:
+			dbg_token("OUT token for EP %x\n", token->ep);
+			port->ep_next_data = token->ep;
+
+			/* Reset the data token of the IN EP81 */
+			ep = &port->eps[FIM_USB_NR_EP_IN];
+			ep->last_data_token = USB_TOKEN_DATA1;
+			break;
+
+			/* DONT print the IN tokens cause they are generated continously */
+		case USB_TOKEN_IN:
+			/* dbg_token("IN token for EP %x\n", token->ep); */
+			port->ep_next_data = token->ep;
+			break;
+
+		case USB_TOKEN_SETUP:
+			port->ep_next_data = 0;
+			/* dbg_token("SETUP token for EP %x\n", token->ep); */
+
+			/* Reset the data token of the IN EP81 */
+			ep = &port->eps[FIM_USB_NR_EP_IN];
+			ep->last_data_token = USB_TOKEN_DATA1;			
+			break;
+
+			/* Here the tokens that only generate noise */
+		case USB_TOKEN_SYNC:
+		default:
+			break;
+		}
+	}
+}
+
+static int fim_usb_unregister_port(struct fim_usb_port *port)
+{
+	int cnt, ret;
+	struct fim_driver *fim;
+	struct uart_port *uart;
+	
+	if (!port || !port->reg)
+		return -ENODEV;
+
+	fim = &port->fim;
+	pk_dbg("Going to unregister the FIM USB port %i\n", fim->picnr);
+
+	flush_work(&port->uart_work);
+	cancel_work_sync(&port->restart_work);
+	cancel_delayed_work_sync(&port->hotplug_work);
+
+	/* Disable the FIM from the connected host */
+	fim_usb_disable_pullup(port);
+	
+	ret = fim_unregister_driver(fim);
+	if (ret)
+		goto exit_unreg;
+
+	/* Unregister the UART port */
+	if (port->uart_created) {
+		dbg_uart("Removing the port %i\n", fim->picnr);
+		uart = &port->uart;
+		uart_remove_one_port(&fim_usb_uart_driver, uart);
+		port->uart_created = 0;
+	}
+	
+	/* Free the requested GPIO */
+	for (cnt = 0; cnt < FIM_USB_MAX_GPIOS; cnt++) {
+		dbg_pr("Freeing GPIO %i\n", port->gpios[cnt].nr);
+                if (port->gpios[cnt].nr == FIM_LAST_GPIO)
+                        break;
+                else if (port->gpios[cnt].nr == FIM_GPIO_DONT_USE)
+                        continue;
+                else
+                        gpio_free(port->gpios[cnt].nr);
+	}
+
+	/* Free the requested clock */
+	if (!IS_ERR(port->clk))
+	        clk_put(port->clk);
+
+	fim_usb_ports[fim->picnr] = NULL;
+	kfree(port);
+	ret = 0;
+	
+ exit_unreg:
+	return ret;
+}
+
+/*
+ * Called when the port is opened. By the boot-console it will be called before
+ * the port configuration using set_termios
+ */
+static int fim_usb_uart_startup(struct uart_port *uart)
+{
+        struct fim_usb_port *port;
+	int ret = 0;
+
+	dbg_uart("Calling %s()\n", __func__);
+        port = port_from_uart(uart);
+	if (!port)
+		return -ENODEV;
+
+#if defined(FIM_USB_FORCE_ENUM_TEST)
+	/* This will restart the enumeration sequence */
+	pk_info("Forcing the enumeration test!\n");
+	schedule_work(&port->restart_work);
+	ret = -ENODEV;
+#else
+	/*
+	 * We must flush the TX FIFO when there is some data pending.
+	 * @XXX: Use a waitqueue with timeout for returning a failure,
+	 * otherwise the user must reopen the port (annoying)
+	 */
+	if (fim_tx_buffers_level(&port->fim)) {
+		pk_dbg("TX FIFO contains data, FIM restart required.\n");
+		schedule_work(&port->restart_work);
+		wait_for_completion(&port->restart_completed);
+
+		pk_dbg("Restart completed. Re-checking TX FIFO state.\n");
+		if (fim_tx_buffers_level(&port->fim)) {
+			pk_dbg("TX FIFO invalid state. Retry required.\n");
+			ret = -EAGAIN;
+		}
+	}
+#endif /* FIM_USB_FORCE_ENUM_TEST */
+
+	return ret;
+}
+
+/* Return a string describing the type of the port */
+static const char *fim_usb_uart_type(struct uart_port *port)
+{
+	dbg_uart("Calling %s\n", __func__);
+        return FIM_DRIVER_NAME;
+}
+
+static void fim_usb_uart_release_port(struct uart_port *port)
+{
+        dbg_uart("Calling %s\n", __func__);
+}
+
+static int fim_usb_uart_request_port(struct uart_port *port)
+{
+        dbg_uart("Calling %s\n", __func__);
+        return 0;
+}
+
+/* @TODO: Get more infos about the UART configuration */
+static void fim_usb_uart_config_port(struct uart_port *port, int flags)
+{
+        dbg_uart("Calling %s\n", __func__);
+        port->type = UPIO_MEM;
+}
+
+static int fim_usb_uart_verify_port(struct uart_port *port, struct serial_struct *ser)
+{
+        dbg_uart("Verify port called\n");
+        return 0;
+}
+
+/* Called for setting the termios config */
+static void fim_usb_uart_set_termios(struct uart_port *uart, struct ktermios *termios,
+				     struct ktermios *old)
+{
+        dbg_uart("Calling %s\n", __func__);
+}
+
+static unsigned int fim_usb_uart_tx_empty(struct uart_port *uart)
+{
+        return TIOCSER_TEMT;
+}
+
+/* Handle the data transfer from the UART circular buffer to the USB host (IN transfers) */
+static void fim_usb_uart_tx_work_func(struct work_struct *work)
+{
+	struct fim_usb_ep *ep;
+	struct circ_buf *xmit;
+	u8 buf[FIM_USB_MAX_PACK_SIZE];
+	struct uart_port *uart;
+	ulong pending, pos;
+	int ret, tail;
+	struct fim_usb_port *port;
+	__u32 tx;
+
+	ep = container_of(work, struct fim_usb_ep, tx_work);
+	port = ep->port;
+
+	if (down_interruptible(&port->uart_sem))
+		return;
+
+	uart = ep->tx_uart;
+	xmit  = &uart->info->xmit;
+	tail = xmit->tail;
+	tx = uart->icount.tx;	
+	pending = uart_circ_chars_pending(xmit);
+	pos = ret = 0;
+
+	while (pending--) {
+
+		buf[pos++] = xmit->buf[tail];
+		
+		if (pos == FIM_USB_MAX_PACK_SIZE || pending == 0) {
+
+			/*
+			 * Check if there is enough space in the DMA buffers for the next
+			 * transfer request. Otherwise we must wait for the completion of
+			 * some TX transfer and continue sending the data
+			 */
+			while (fim_tx_buffers_level(&port->fim)) {
+				schedule_timeout_interruptible(HZ / 1000);
+				if (signal_pending(current))
+					goto exit_tx_worker;				
+			}
+			
+			/* Send the data to the FIM */
+			dbg_uart("EP%02x Sending %lu bytes | remaining %lu\n", ep->addr, pos, pending);
+			ret = fim_usb_write_packet(port, ep, buf, pos);
+			if (ret <= 0) {
+				pr_err("Couldn't send the buffer (err %i)\n", ret);
+				goto exit_tx_worker;
+			}
+		}
+		
+		tail = (tail + 1) & (UART_XMIT_SIZE - 1);
+		tx++;
+
+		/* Only one messages per call! */
+		if (pos == FIM_USB_MAX_PACK_SIZE || pending == 0)
+			break;
+	}
+
+	/* OK, it looks we have sent some data, so update the UART structure */
+	xmit->tail = tail;
+	uart->icount.tx = tx;
+
+	/* Check for the ZLP condition on this EP */
+	if (uart_circ_empty(xmit) && pos == FIM_USB_MAX_PACK_SIZE) {
+		dbg_uart("Need to send a ZLP\n");
+
+		/* @XXX: Quick and dirty! */
+		/* Wait for an empty TX FIFO cause we can't queue messages */
+		while (fim_tx_buffers_level(&port->fim)) {
+			schedule_timeout_interruptible(HZ / 1000);
+			if (signal_pending(current))
+				goto exit_tx_worker;			
+		}
+		
+		fim_usb_write_zlp(port, ep, 0);
+	}
+	
+	/*
+	 * Tell the higher layer that we can send more data
+	 * IMPORTANT: We need to check if TX was stopped before calling the wakeup cause
+	 * the bove ZLP write function seems to take a lot of time and we have probably
+	 * received a signal during this time
+	 */
+        if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS && !uart_tx_stopped(uart)) {
+		uart_write_wakeup(uart);
+	}
+
+ exit_tx_worker:
+	up(&port->uart_sem);
+}
+
+/* This function is called when the TTY layer has some data to be sent */
+static void fim_usb_uart_start_tx(struct uart_port *uart)
+{
+	ulong pending;
+	struct fim_usb_ep *ep;
+	struct fim_usb_port *port;
+	struct circ_buf *xmit;
+
+	port = port_from_uart(uart);
+
+	/*
+	 * IMPORTANT: The data token is modified in the function that puts the data
+	 * buffer to the FIM-core.
+	 */
+	ep = &port->eps[FIM_USB_NR_EP_IN];
+	ep->tx_uart = uart;
+	
+	xmit  = &uart->info->xmit;
+	pending = uart_circ_chars_pending(xmit);
+	
+	dbg_uart("Scheduling TX worker | %lu bytes\n", pending);
+	schedule_work(&ep->tx_work);
+}
+
+static unsigned int fim_usb_uart_get_mctrl(struct uart_port *port)
+{
+	return TIOCM_CTS | TIOCM_DSR | TIOCM_CAR;
+}
+
+static void fim_usb_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+        /* @TODO? */
+	dbg_uart("Calling %s\n", __func__);
+}
+
+static void fim_usb_uart_stop_tx(struct uart_port *port)
+{
+	/* flush_work(&port->uart_work); */
+	dbg_uart("Calling %s\n", __func__);
+}
+
+static void fim_usb_uart_stop_rx(struct uart_port *port)
+{
+	dbg_uart("Calling %s\n", __func__);
+}
+
+static void fim_usb_uart_enable_ms(struct uart_port *port)
+{
+        /* @TODO? */
+	dbg_uart("Calling %s\n", __func__);
+}
+
+static void fim_usb_uart_break_ctl(struct uart_port *port, int ctl)
+{
+        /* @TODO? */
+	dbg_uart("Calling %s\n", __func__);
+}
+
+static void fim_usb_uart_shutdown(struct uart_port *uart)
+{
+	struct fim_usb_port *port;
+	struct fim_usb_ep *ep;
+	
+	port = port_from_uart(uart);
+	ep = &port->eps[FIM_USB_NR_EP_IN];
+
+	flush_work(&ep->tx_work);
+	dbg_uart("Calling %s\n", __func__);
+}
+
+static struct uart_ops fim_usb_uart_ops = {
+        .tx_empty       = fim_usb_uart_tx_empty,
+        .set_mctrl      = fim_usb_uart_set_mctrl,
+        .get_mctrl      = fim_usb_uart_get_mctrl,
+        .stop_tx        = fim_usb_uart_stop_tx,
+        .start_tx       = fim_usb_uart_start_tx,
+        .stop_rx        = fim_usb_uart_stop_rx,
+        .enable_ms      = fim_usb_uart_enable_ms,
+        .break_ctl      = fim_usb_uart_break_ctl,
+	.startup        = fim_usb_uart_startup,
+        .shutdown       = fim_usb_uart_shutdown,
+        .set_termios    = fim_usb_uart_set_termios,
+	.type           = fim_usb_uart_type,
+        .release_port   = fim_usb_uart_release_port,
+        .request_port   = fim_usb_uart_request_port,
+	.config_port    = fim_usb_uart_config_port,
+        .verify_port    = fim_usb_uart_verify_port
+};
+
+static int fim_usb_register_port(struct device *dev, int picnr, struct fim_gpio_t gpios[])
+{
+	int ret, cnt;
+	struct fim_dma_cfg_t dma_cfg;
+	struct fim_usb_port *port;
+	unsigned int fwver;
+
+	port = kzalloc(sizeof(* port), GFP_KERNEL);
+	if (!port)
+		return -ENOMEM;
+	
+	/* Get a reference to the SYS clock for setting CLK rate */
+	ret = IS_ERR(port->clk = clk_get(port->fim.dev, "systemclock"));
+	if (ret) {
+		pk_err("Couldn't get the SYS clock.\n");
+		goto err_free_port;
+	}
+
+	/* Request GPIO */
+	for (cnt = 0; cnt < FIM_USB_MAX_GPIOS; cnt++) {
+
+		if (gpios[cnt].nr == FIM_LAST_GPIO)
+			break;
+		if (gpios[cnt].nr == FIM_GPIO_DONT_USE)
+			continue;
+		
+		dbg_pr("Requesting the GPIO %i (Function %i)\n",
+		       gpios[cnt].nr, gpios[cnt].func);
+		ret = gpio_request(gpios[cnt].nr, FIM_DRIVER_NAME);
+		if (!ret) {
+			int dir, pullup;
+			
+			/*
+			 * According to the documentation, the pins ENUM and SUSPEND
+			 * must be controlled by the driver (they are not by the
+			 * FIM-firmware).
+			 */
+			if (cnt == FIM_USB_GPIO_ENUM || cnt == FIM_USB_GPIO_SPND) {
+				dir = NS921X_GPIO_OUTPUT;
+				pullup = NS921X_GPIO_DISABLE_PULLUP;
+			} else {
+				dir = NS921X_GPIO_INPUT;
+				pullup = NS921X_GPIO_ENABLE_PULLUP;
+			}
+			
+			gpio_configure_ns921x_unlocked(gpios[cnt].nr,
+						       dir,
+						       NS921X_GPIO_DONT_INVERT,
+						       gpios[cnt].func,
+						       pullup);
+		} else {
+			/* Free the already requested GPIOs */
+			pk_err("Couldn't request the GPIO %i\n", gpios[cnt].nr);
+			while (cnt)
+				gpio_free(gpios[--cnt].nr);
+			goto err_free_clk;
+		}
+	}
+
+	/* Try to register FIM driver */
+	port->index		= picnr;
+	port->fim.picnr		= picnr;
+	port->fim.driver.name	= FIM_DRIVER_NAME;
+	port->fim.driver_data	= port;
+	port->fim.fim_isr	= fim_usb_isr;
+	port->fim.dma_tx_isr	= fim_usb_tx_isr;
+	port->fim.dma_rx_isr	= fim_usb_rx_isr;
+	port->fim.dma_error_isr	= fim_usb_error_isr;
+	port->fim.driver_data	= port;
+
+	/* Specific DMA config */
+	dma_cfg.rxnr = FIM_USB_DMA_RX_BUFFERS;
+	dma_cfg.txnr = FIM_USB_DMA_TX_BUFFERS;
+	dma_cfg.rxsz = FIM_USB_DMA_BUFFER_SIZE;
+	dma_cfg.txsz = FIM_USB_DMA_BUFFER_SIZE;
+	port->fim.dma_cfg = &dma_cfg;
+
+	/* Check if have a firmware code for using to */
+	port->fim.fw_name = FIM_USB_FIRMWARE_FILE;
+	port->fim.fw_code = FIM_USB_FIRMWARE_CODE;
+	ret = fim_register_driver(&port->fim);
+	if (ret) {
+		pk_err("Couldn't register the FIM %i USB driver.\n", picnr);
+		goto err_free_gpios;
+	}
+
+	port->dev = dev;
+	memcpy(port->gpios, gpios, sizeof(port->gpios));
+	
+	port->reg = 1;
+	dev_set_drvdata(dev, port);
+
+	INIT_WORK(&port->uart_work, fim_usb_uart_work_func);
+	INIT_WORK(&port->restart_work, fim_usb_restart_work_func);
+	init_completion(&port->restart_completed);
+	init_MUTEX(&port->uart_sem);
+
+        /* This is the timer for checking the state of the card detect line */
+	INIT_DELAYED_WORK(&port->hotplug_work, fim_usb_hotplug_work_func);
+
+	/* And enable the FIM-interrupt */
+	ret = fim_enable_irq(&port->fim);
+	if (ret) {
+		pk_err("Couldn't enable the FIM IRQ\n");
+		goto err_unreg_fim;
+	}
+
+	/* First do it at this point! */
+	fim_usb_disable_pullup(port);
+	fim_usb_init_port(port);
+
+	/* Print the firmware version */
+        fim_get_stat_reg(&port->fim, FIM_USB_REVISION_REG, &fwver);
+        pk_dbg("FIM%d running [fw rev 0x%02x]\n", picnr, fwver);
+
+	fim_usb_ports[picnr] = port;
+	
+	/* This will enable the start of the FIM in the firmware */
+	fim_set_ctrl_reg(&port->fim, FIM_USB_MAIN_REG,
+			 FIM_USB_MAIN_START |  FIM_USB_MAIN_NOKEEPALIVE | FIM_USB_MAIN_NOINIRQ);
+	fim_usb_enable_pullup(port);	
+	return 0;
+
+ err_unreg_fim:
+	fim_unregister_driver(&port->fim);
+	
+ err_free_gpios:
+	for (cnt = 0; cnt < FIM_USB_MAX_GPIOS; cnt++) {
+
+		if (gpios[cnt].nr == FIM_GPIO_DONT_USE)
+			continue;
+
+		if (gpios[cnt].nr == FIM_LAST_GPIO)
+			break;
+		
+		gpio_free(gpios[cnt].nr);
+	}
+
+ err_free_clk:
+	clk_put(port->clk);
+
+ err_free_port:
+	kfree(port);
+
+	return ret;
+}
+
+static int __init fim_usb_probe(struct platform_device *pdev)
+{
+        int nrpics;
+	struct fim_usb_platform_data *pdata;
+	struct fim_gpio_t gpios[FIM_USB_MAX_GPIOS];
+
+	dbg_func("New device with ID %i\n", pdev->id);
+	
+	pdata = pdev->dev.platform_data;
+	if (!pdata)
+		return -ENXIO;
+
+        /* Get the number of available PICs from the FIM-core */
+        nrpics = fim_number_pics();
+
+	if (fim_check_device_id(fims_number, pdata->fim_nr)) {
+#if defined(MODULE)
+                pk_dbg("Skipping FIM%i (not selected)\n", pdata->fim_nr);
+#else
+                pk_err("Invalid FIM number '%i' in platform data\n", pdata->fim_nr);
+#endif
+		
+                return -ENODEV;
+        }
+
+	/*
+	 * Start with the registration of the pdata ports
+	 */
+	gpios[FIM_USB_GPIO_DP].nr	= pdata->vp_gpio_nr;
+	gpios[FIM_USB_GPIO_DP].func	= pdata->vp_gpio_func;
+	gpios[FIM_USB_GPIO_DM].nr	= pdata->vm_gpio_nr;
+	gpios[FIM_USB_GPIO_DM].func	= pdata->vm_gpio_func;
+	gpios[FIM_USB_GPIO_RCV].nr	= pdata->rcv_gpio_nr;
+	gpios[FIM_USB_GPIO_RCV].func	= pdata->rcv_gpio_func;
+	gpios[FIM_USB_GPIO_OE].nr	= pdata->oe_l_gpio_nr;
+	gpios[FIM_USB_GPIO_OE].func	= pdata->oe_l_gpio_func;
+	gpios[FIM_USB_GPIO_ENUM].nr	= pdata->enum_gpio_nr;
+	gpios[FIM_USB_GPIO_ENUM].func	= pdata->enum_gpio_func;
+	gpios[FIM_USB_GPIO_SPND].nr	= pdata->spnd_gpio_nr;
+	gpios[FIM_USB_GPIO_SPND].func	= pdata->spnd_gpio_func;
+
+	dbg_pr("Pins: DP %d | DM %d | RCV %d | OE %d | ENUM %d | SPND %d\n",
+	       gpios[0].nr, gpios[1].nr, gpios[2].nr, 
+	       gpios[3].nr, gpios[4].nr, gpios[5].nr );
+
+	return fim_usb_register_port(&pdev->dev, pdata->fim_nr, gpios);
+}
+
+static int __exit fim_usb_remove(struct platform_device *pdev)
+{
+	struct fim_usb_port *port;
+	struct fim_usb_platform_data *pdata;
+	int retval;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata)
+		return -ENXIO;
+
+	port = dev_get_drvdata(&pdev->dev);
+	if (!port)
+		return -ENXIO;
+	
+	retval = fim_usb_unregister_port(port);
+	if (retval)
+		goto exit_remove;
+
+	platform_set_drvdata(pdev, NULL);
+
+ exit_remove:
+	return retval;
+}
+
+#ifdef	CONFIG_PM
+/* Here we need to desconnect from the host and stop the FIM */
+static int fim_usb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct fim_usb_port *port;
+	int ret;
+	struct fim_driver *fim;
+
+	port = dev_get_drvdata(&pdev->dev);
+	if (!port)
+		return -ENXIO;
+
+	cancel_delayed_work_sync(&port->hotplug_work);
+	
+	/* Disable the device and stop the FIM */
+	fim_usb_disable_pullup(port);
+
+	/* Stop the FIM now */
+	fim = &port->fim;
+	ret = fim_send_stop(fim);
+	if (ret)
+		pk_err("Couldn't stop the FIM %i\n", fim->picnr);
+	
+	return ret;
+}
+
+static int fim_usb_resume(struct platform_device *pdev)
+{
+	struct fim_usb_port *port;
+
+	port = dev_get_drvdata(&pdev->dev);
+	if (!port)
+		return -ENXIO;
+
+	return fim_usb_restart_fim(port);
+}
+#else
+#define	fim_usb_suspend	NULL
+#define	fim_usb_resume	NULL
+#endif
+
+static struct platform_driver fim_usb_driver = {
+	.driver		= {
+		.name	= FIM_DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe          = fim_usb_probe,
+	.remove		= __exit_p(fim_usb_remove),
+	.suspend	= fim_usb_suspend,
+	.resume		= fim_usb_resume,
+};
+
+static int __init fim_usb_init(void)
+{
+	int cnt, pos, ret, pics;
+	struct fim_usb_string *pstr;
+	u16 *pdata;
+	
+        /* Check for the passed number parameter */
+        if (fim_check_numbers_param(fims_number)) {
+                pk_err("Invalid number '%i' of FIMs to handle\n", fims_number);
+                return -EINVAL;
+        }
+
+	pk_info("%s: version %s [%s | %s]\n", FIM_DRIVER_NAME, DRIVER_VERSION,
+		__DATE__, __TIME__);
+
+	/* For having an easier descriptors handling we only want to use static data buffers */
+	for (cnt = 0; cnt < ARRAY_SIZE(fim_usb_strings); cnt++) {
+		pstr = &fim_usb_strings[cnt];
+		pdata = pstr->data;
+
+		for (pos = 0; pos < strlen(pstr->s); pos++)
+			*(pdata + pos) = cpu_to_le16(*(pstr->s + pos));
+
+		pstr->desc.bLength = sizeof(pstr->desc) + (2 * pos);
+	}
+
+	pics = fim_number_pics();
+	fim_usb_ports_max = pics;
+	fim_usb_ports = kzalloc(pics, sizeof(*fim_usb_ports));
+	if (!fim_usb_ports)
+		return -ENOMEM;
+
+	/* We need a reboot notifier for disabling the pullup */
+	ret = register_reboot_notifier(&fim_usb_reboot_notifier);
+	if (ret) {
+		pk_err("Reboot notifier register failed, %i\n", ret);
+		goto err_exit_init;
+	}
+		
+	/* Register the internal UART driver */
+        /* Get the number of maximal available FIMs */
+        fim_usb_uart_driver.owner = THIS_MODULE;
+        fim_usb_uart_driver.driver_name = FIM_DRIVER_NAME;
+        fim_usb_uart_driver.dev_name = FIM_USB_UART_DEV_NAME;
+        fim_usb_uart_driver.nr = pics;
+        ret = uart_register_driver(&fim_usb_uart_driver);
+        if (ret)
+		goto err_unreg_noti;
+
+	ret = platform_driver_register(&fim_usb_driver);
+	if (ret)
+		goto err_unreg_uart;
+
+	return 0;
+	
+ err_unreg_uart:
+	uart_unregister_driver(&fim_usb_uart_driver);
+
+ err_unreg_noti:
+	unregister_reboot_notifier(&fim_usb_reboot_notifier);
+	
+ err_exit_init:
+	kfree(fim_usb_ports);
+	return ret;
+}
+
+static void __exit fim_usb_exit(void)
+{
+	pk_info("Removing the FIM USB driver [%s | %s]\n", __DATE__, __TIME__);
+	platform_driver_unregister(&fim_usb_driver);
+	uart_unregister_driver(&fim_usb_uart_driver);
+
+	/* Free the resources for the reboot notifier */
+	unregister_reboot_notifier(&fim_usb_reboot_notifier);
+	kfree(fim_usb_ports);
+}
+
+module_init(fim_usb_init);
+module_exit(fim_usb_exit);
+
+MODULE_DESCRIPTION(FIM_DRIVER_DESC);
+MODULE_AUTHOR(FIM_DRIVER_AUTHOR);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:fim_usb");
+
diff --git a/drivers/fims/usb/fim_usb.h b/drivers/fims/usb/fim_usb.h
new file mode 100644
index 000000000000..a650ebcf9d10
--- /dev/null
+++ b/drivers/fims/usb/fim_usb.h
@@ -0,0 +1,185 @@
+
+/*
+ * Automatic generated header file with the firmware code for the FIM
+ * Input binary  : fim_firmware.bin
+ * Output header : fim_usb.h
+ * Structure     : fim_usb_firmware
+ */
+
+static const unsigned char fim_usb_firmware[] = { 
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x01, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,  
+	0x91, 0x02, 0x00, 0x00,  0x4a, 0x2a, 0x00, 0x00,  
+	0x03, 0x00, 0x00, 0x00,  0xa0, 0x00, 0x03, 0x08,  
+	0x83, 0x12, 0xa1, 0x00,  0x11, 0x08, 0x81, 0x00,  
+	0x0b, 0x11, 0x8b, 0x17,  0x27, 0x19, 0x4e, 0x28,  
+	0xa4, 0x1b, 0x55, 0x29,  0x83, 0x16, 0x22, 0x08,  
+	0x10, 0x05, 0x03, 0x1d,  0x2d, 0x28, 0x23, 0x08,  
+	0x10, 0x05, 0x03, 0x1d,  0x3e, 0x28, 0x83, 0x12,  
+	0x25, 0x1c, 0x51, 0x29,  0x83, 0x16, 0x81, 0x16,  
+	0x83, 0x12, 0x25, 0x16,  0x83, 0x16, 0xa1, 0x14,  
+	0x83, 0x16, 0x22, 0x08,  0x10, 0x05, 0x03, 0x1d,  
+	0x2c, 0x28, 0x23, 0x08,  0x10, 0x05, 0x03, 0x1d,  
+	0x2c, 0x28, 0x22, 0x28,  0x76, 0x28, 0x23, 0x08,  
+	0x10, 0x05, 0x03, 0x1d,  0x74, 0x2a, 0x83, 0x12,  
+	0xab, 0x14, 0x2b, 0x18,  0x45, 0x28, 0x06, 0x30,  
+	0x2e, 0x02, 0x03, 0x19,  0x43, 0x28, 0xae, 0x01,  
+	0x83, 0x16, 0x21, 0x10,  0xa1, 0x14, 0x76, 0x28,  
+	0x83, 0x12, 0xab, 0x10,  0x2b, 0x1c, 0x45, 0x28,  
+	0x35, 0x28, 0xae, 0x01,  0x76, 0x28, 0xae, 0x0a,  
+	0x07, 0x30, 0x2e, 0x02,  0x03, 0x19, 0x24, 0x14,  
+	0x83, 0x16, 0x21, 0x14,  0xa1, 0x14, 0x76, 0x28,  
+	0xa7, 0x10, 0xa7, 0x19,  0x65, 0x28, 0x27, 0x18,  
+	0x64, 0x28, 0x83, 0x16,  0x22, 0x08, 0x10, 0x05,  
+	0x03, 0x19, 0x5e, 0x28,  0x22, 0x08, 0xff, 0x3a,  
+	0x98, 0x05, 0x23, 0x08,  0x98, 0x04, 0x76, 0x28,  
+	0x22, 0x08, 0x98, 0x04,  0x23, 0x08, 0xff, 0x3a,  
+	0x98, 0x05, 0x76, 0x28,  0x76, 0x28, 0x27, 0x1a,  
+	0x6f, 0x28, 0x83, 0x16,  0x22, 0x08, 0xff, 0x3a,  
+	0x98, 0x05, 0x23, 0x08,  0xff, 0x3a, 0x98, 0x05,  
+	0x76, 0x28, 0x83, 0x16,  0x22, 0x08, 0xff, 0x3a,  
+	0x98, 0x05, 0x23, 0x08,  0x98, 0x04, 0x76, 0x28,  
+	0x83, 0x12, 0xab, 0x0c,  0x21, 0x08, 0x83, 0x00,  
+	0xa0, 0x0e, 0x20, 0x0e,  0x09, 0x00, 0x83, 0x16,  
+	0xa1, 0x01, 0x83, 0x12,  0xae, 0x01, 0xab, 0x01,  
+	0xa6, 0x01, 0xa5, 0x01,  0xa4, 0x01, 0x97, 0x1d,  
+	0x8b, 0x28, 0x83, 0x16,  0x12, 0x08, 0x9a, 0x00,  
+	0x83, 0x12, 0x39, 0x21,  0x80, 0x30, 0x30, 0x02,  
+	0x03, 0x19, 0x93, 0x28,  0x24, 0x16, 0x24, 0x08,  
+	0x36, 0x22, 0x30, 0x08,  0x2b, 0x22, 0x25, 0x14,  
+	0x39, 0x21, 0x30, 0x08,  0x2b, 0x22, 0x69, 0x30,  
+	0x30, 0x02, 0x03, 0x1d,  0x9f, 0x28, 0xa6, 0x14,  
+	0xef, 0x28, 0xe1, 0x30,  0x30, 0x02, 0x03, 0x1d,  
+	0xa5, 0x28, 0x26, 0x15,  0xef, 0x28, 0x2d, 0x30,  
+	0x30, 0x02, 0x03, 0x1d,  0xab, 0x28, 0x26, 0x14,  
+	0xef, 0x28, 0xc3, 0x30,  0x30, 0x02, 0x03, 0x1d,  
+	0xb1, 0x28, 0xa6, 0x15,  0xbc, 0x28, 0x4b, 0x30,  
+	0x30, 0x02, 0x03, 0x1d,  0xb7, 0x28, 0xa6, 0x15,  
+	0xbc, 0x28, 0x34, 0x21,  0x24, 0x08, 0x36, 0x22,  
+	0xb6, 0x01, 0x7d, 0x28,  0xc2, 0x01, 0x43, 0x30,  
+	0x84, 0x00, 0x39, 0x21,  0x25, 0x1a, 0xc8, 0x28,  
+	0x30, 0x08, 0x80, 0x00,  0x84, 0x0a, 0xc2, 0x0a,  
+	0x2b, 0x22, 0xbf, 0x28,  0xff, 0x30, 0xbd, 0x00,  
+	0xbe, 0x00, 0xc2, 0x03,  0xc2, 0x03, 0x03, 0x19,  
+	0xdb, 0x28, 0x43, 0x30,  0x84, 0x00, 0x00, 0x08,  
+	0x08, 0x30, 0xbf, 0x00,  0x06, 0x22, 0x80, 0x0c,  
+	0xbf, 0x0b, 0xd4, 0x28,  0x84, 0x0a, 0xc2, 0x0b,  
+	0xd1, 0x28, 0x3e, 0x08,  0xff, 0x3a, 0x41, 0x02,  
+	0x03, 0x1d, 0xe6, 0x28,  0x3d, 0x08, 0xff, 0x3a,  
+	0x40, 0x02, 0x03, 0x1d,  0xe6, 0x28, 0xe7, 0x28,  
+	0xa4, 0x14, 0x24, 0x08,  0x36, 0x22, 0xb6, 0x1c,  
+	0x7d, 0x28, 0xa4, 0x1c,  0xad, 0x29, 0xb6, 0x01,  
+	0x7d, 0x28, 0x1f, 0x30,  0xbb, 0x00, 0xa5, 0x16,  
+	0xbc, 0x01, 0x39, 0x21,  0x30, 0x08, 0x2b, 0x22,  
+	0xba, 0x00, 0x7f, 0x39,  0x12, 0x02, 0x03, 0x1d,  
+	0xfd, 0x28, 0xa5, 0x17,  0xfd, 0x28, 0x39, 0x21,  
+	0x30, 0x08, 0x2b, 0x22,  0xb9, 0x00, 0x34, 0x21,  
+	0x3b, 0x08, 0x1f, 0x3a,  0x1f, 0x39, 0xbb, 0x00,  
+	0x40, 0x08, 0x1f, 0x39,  0x3b, 0x02, 0x03, 0x1d,  
+	0xa4, 0x14, 0xa5, 0x1f,  0xa4, 0x15, 0x24, 0x08,  
+	0x36, 0x22, 0xa5, 0x1f,  0x7d, 0x28, 0xa4, 0x18,  
+	0x7d, 0x28, 0xa6, 0x18,  0x17, 0x29, 0xb6, 0x14,  
+	0x7d, 0x28, 0x8c, 0x1d,  0x1a, 0x29, 0xb4, 0x29,  
+	0x28, 0x18, 0x1f, 0x29,  0x25, 0x22, 0xa9, 0x00,  
+	0x28, 0x14, 0xba, 0x0d,  0xb9, 0x0d, 0x0f, 0x30,  
+	0x39, 0x05, 0x29, 0x02,  0x03, 0x1d, 0xb4, 0x29,  
+	0xa4, 0x16, 0x24, 0x08,  0x17, 0x1d, 0x36, 0x22,  
+	0x28, 0x10, 0xd6, 0x21,  0x25, 0x22, 0xb5, 0x00,  
+	0x25, 0x22, 0xaf, 0x00,  0xbb, 0x21, 0xb5, 0x0b,  
+	0x2e, 0x29, 0xee, 0x29,  0x83, 0x21, 0x83, 0x12,  
+	0x25, 0x1e, 0xa4, 0x14,  0x08, 0x00, 0x08, 0x30,  
+	0xb2, 0x00, 0x83, 0x21,  0x83, 0x12, 0x25, 0x1a,  
+	0x08, 0x00, 0xb0, 0x0c,  0xb0, 0x13, 0xc0, 0x0d,  
+	0xc1, 0x0d, 0x40, 0x10,  0x83, 0x16, 0xa1, 0x10,  
+	0x21, 0x1c, 0x4b, 0x29,  0x83, 0x12, 0xb0, 0x17,  
+	0x40, 0x14, 0x83, 0x12,  0xa5, 0x1a, 0x14, 0x22,  
+	0xb2, 0x0b, 0x3b, 0x29,  0x08, 0x00, 0x83, 0x12,  
+	0xa4, 0x17, 0xb7, 0x01,  0x76, 0x28, 0x83, 0x12,  
+	0xb7, 0x0a, 0x37, 0x19,  0x72, 0x29, 0x22, 0x08,  
+	0x83, 0x16, 0x10, 0x05,  0x03, 0x1d, 0x64, 0x29,  
+	0x23, 0x08, 0x10, 0x05,  0x03, 0x1d, 0x64, 0x29,  
+	0x83, 0x12, 0x76, 0x28,  0x83, 0x16, 0x81, 0x16,  
+	0x83, 0x12, 0x24, 0x08,  0x83, 0x16, 0x00, 0x30,  
+	0x1a, 0x04, 0x03, 0x19,  0x6e, 0x29, 0x9a, 0x03,  
+	0x83, 0x12, 0x97, 0x1c,  0x36, 0x22, 0x7d, 0x28,  
+	0x83, 0x16, 0x81, 0x16,  0x83, 0x12, 0xa4, 0x13,  
+	0x24, 0x17, 0x24, 0x08,  0x36, 0x22, 0x83, 0x16,  
+	0x22, 0x08, 0x10, 0x05,  0x03, 0x1d, 0x7d, 0x28,  
+	0x23, 0x08, 0x10, 0x05,  0x03, 0x1d, 0x7d, 0x28,  
+	0x79, 0x29, 0x83, 0x16,  0x24, 0x08, 0x10, 0x05,  
+	0x03, 0x19, 0x90, 0x29,  0x89, 0x29, 0x24, 0x08,  
+	0x10, 0x05, 0x03, 0x19,  0x9b, 0x29, 0xa1, 0x1c,  
+	0x89, 0x29, 0x08, 0x00,  0x24, 0x08, 0x10, 0x05,  
+	0x03, 0x1d, 0x9b, 0x29,  0x23, 0x08, 0x10, 0x05,  
+	0x03, 0x19, 0x9b, 0x29,  0xa1, 0x1c, 0x90, 0x29,  
+	0x08, 0x00, 0x83, 0x12,  0x10, 0x08, 0x81, 0x00,  
+	0x83, 0x16, 0x81, 0x12,  0xa1, 0x1c, 0xa0, 0x29,  
+	0x08, 0x00, 0x83, 0x16,  0x22, 0x08, 0x10, 0x05,  
+	0x03, 0x19, 0x08, 0x00,  0x23, 0x08, 0x10, 0x05,  
+	0x03, 0x19, 0x08, 0x00,  0x74, 0x2a, 0x83, 0x12,  
+	0xd6, 0x21, 0xd2, 0x30,  0xaf, 0x00, 0xbb, 0x21,  
+	0xb6, 0x01, 0xee, 0x29,  0x83, 0x12, 0xd6, 0x21,  
+	0x5a, 0x30, 0xaf, 0x00,  0xbb, 0x21, 0xb6, 0x01,  
+	0xee, 0x29, 0x08, 0x30,  0xb4, 0x00, 0xaf, 0x0c,  
+	0x03, 0x1c, 0xc6, 0x29,  0xce, 0x21, 0x06, 0x30,  
+	0x2e, 0x02, 0x03, 0x19,  0xca, 0x21, 0xc7, 0x29,  
+	0xca, 0x21, 0xb4, 0x0b,  0xbd, 0x29, 0x08, 0x00,  
+	0xae, 0x01, 0x27, 0x10,  0xd2, 0x21, 0x08, 0x00,  
+	0x27, 0x14, 0xd2, 0x21,  0xae, 0x0a, 0x08, 0x00,  
+	0xa7, 0x14, 0xa7, 0x18,  0xd3, 0x29, 0x08, 0x00,  
+	0xae, 0x01, 0x27, 0x15,  0x11, 0x08, 0x81, 0x00,  
+	0x83, 0x16, 0x81, 0x12,  0x83, 0x12, 0xce, 0x21,  
+	0xce, 0x21, 0x83, 0x16,  0x22, 0x08, 0xff, 0x3a,  
+	0x99, 0x05, 0x23, 0x08,  0xff, 0x3a, 0x99, 0x05,  
+	0x25, 0x08, 0xff, 0x3a,  0x98, 0x05, 0x83, 0x12,  
+	0x80, 0x30, 0xaf, 0x00,  0xbb, 0x21, 0x08, 0x00,  
+	0xa7, 0x15, 0xca, 0x21,  0xca, 0x21, 0x27, 0x16,  
+	0xca, 0x21, 0x0a, 0x30,  0xaa, 0x00, 0x00, 0x00,  
+	0xaa, 0x0b, 0xf5, 0x29,  0x83, 0x16, 0x25, 0x08,  
+	0x98, 0x04, 0x83, 0x12,  0xca, 0x21, 0x83, 0x16,  
+	0x81, 0x16, 0x22, 0x08,  0x99, 0x04, 0x23, 0x08,  
+	0x99, 0x04, 0x83, 0x12,  0xa7, 0x01, 0x7d, 0x28,  
+	0x03, 0x10, 0xbd, 0x0d,  0xbe, 0x0d, 0xaa, 0x0d,  
+	0x2a, 0x08, 0x00, 0x06,  0x01, 0x39, 0x03, 0x19,  
+	0x13, 0x2a, 0x80, 0x30,  0xbe, 0x06, 0x05, 0x30,  
+	0xbd, 0x06, 0x08, 0x00,  0x03, 0x10, 0xbb, 0x0d,  
+	0x03, 0x01, 0xb0, 0x1b,  0x20, 0x30, 0xbb, 0x06,  
+	0xbb, 0x1e, 0x1e, 0x2a,  0x05, 0x30, 0xbb, 0x06,  
+	0xbc, 0x0a, 0x3c, 0x08,  0x0b, 0x3c, 0x03, 0x19,  
+	0xa5, 0x12, 0x08, 0x00,  0x24, 0x2a, 0x8c, 0x19,  
+	0x25, 0x2a, 0x00, 0x00,  0x0f, 0x08, 0x00, 0x00,  
+	0x08, 0x00, 0x8c, 0x1b,  0x24, 0x15, 0x8c, 0x1b,  
+	0x2b, 0x2a, 0x8b, 0x13,  0x00, 0x00, 0x8e, 0x01,  
+	0x8d, 0x00, 0x00, 0x00,  0x8b, 0x17, 0x08, 0x00,  
+	0x8c, 0x1b, 0x24, 0x15,  0x8c, 0x1b, 0x36, 0x2a,  
+	0x8b, 0x13, 0x00, 0x00,  0x8e, 0x0a, 0x8d, 0x00,  
+	0x00, 0x00, 0x8b, 0x17,  0x08, 0x00, 0x24, 0x15,  
+	0x09, 0x14, 0x83, 0x16,  0x89, 0x1f, 0x44, 0x2a,  
+	0x83, 0x12, 0x03, 0x01,  0x89, 0x00, 0x49, 0x2a,  
+	0x7f, 0x22, 0x03, 0x30,  0x98, 0x00, 0x17, 0x1c,  
+	0x4d, 0x2a, 0x64, 0x00,  0x08, 0x30, 0xb2, 0x00,  
+	0x83, 0x12, 0x13, 0x08,  0x83, 0x16, 0xa2, 0x00,  
+	0x83, 0x12, 0x14, 0x08,  0x83, 0x16, 0xa3, 0x00,  
+	0x83, 0x12, 0x15, 0x08,  0x83, 0x16, 0xa4, 0x00,  
+	0x83, 0x12, 0x16, 0x08,  0x83, 0x16, 0xa5, 0x00,  
+	0x83, 0x16, 0x98, 0x01,  0x23, 0x08, 0x98, 0x04,  
+	0x25, 0x08, 0x98, 0x04,  0x99, 0x01, 0x22, 0x08,  
+	0x99, 0x04, 0x23, 0x08,  0x99, 0x04, 0x24, 0x08,  
+	0x99, 0x04, 0x83, 0x12,  0x8b, 0x17, 0x8b, 0x16,  
+	0x83, 0x12, 0x7d, 0x28,  0x83, 0x16, 0x81, 0x16,  
+	0x22, 0x08, 0x10, 0x05,  0x03, 0x19, 0x7d, 0x28,  
+	0x23, 0x08, 0x10, 0x05,  0x03, 0x19, 0x7d, 0x28,  
+	0x74, 0x2a, 0x83, 0x12,  0x20, 0x30, 0x84, 0x00,  
+	0x80, 0x01, 0x84, 0x0a,  0x04, 0x08, 0x80, 0x3c,  
+	0x03, 0x1d, 0x82, 0x2a,  0xa0, 0x30, 0x84, 0x00,  
+	0x80, 0x01, 0x84, 0x0a,  0x04, 0x08, 0xff, 0x3c,  
+	0x03, 0x1d, 0x8a, 0x2a,  0x08, 0x00
+};
+
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 82020abc329e..4ec50128aa50 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -49,6 +49,7 @@ struct gpio_desc {
 #define FLAG_RESERVED	2
 #define FLAG_EXPORT	3	/* protected by sysfs_lock */
 #define FLAG_SYSFS	4	/* exported via /sys/class/gpio/control */
+#define FLAG_IS_WAKEUP	5
 
 #ifdef CONFIG_DEBUG_FS
 	const char		*label;
@@ -236,6 +237,52 @@ static ssize_t gpio_direction_store(struct device *dev,
 static const DEVICE_ATTR(direction, 0644,
 		gpio_direction_show, gpio_direction_store);
 
+static ssize_t gpio_wakeup_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	const struct gpio_desc	*desc = dev_get_drvdata(dev);
+	ssize_t			status;
+
+	mutex_lock(&sysfs_lock);
+
+	if (!test_bit(FLAG_EXPORT, &desc->flags))
+		status = -EIO;
+	else {
+		status = sprintf(buf, "%s\n",
+			test_bit(FLAG_IS_WAKEUP, &desc->flags)
+				? "[enabled] disabled" : "enabled [disabled]");
+	}
+
+	mutex_unlock(&sysfs_lock);
+	return status;
+}
+
+/* Enables the wakeup-capability on one GPIO */
+static ssize_t gpio_wakeup_store(struct device *dev, struct device_attribute *attr,
+				 const char *buf, size_t size)
+{
+	const struct gpio_desc	*desc = dev_get_drvdata(dev);
+	unsigned		gpio = desc - gpio_desc;
+	ssize_t			status;
+
+	mutex_lock(&sysfs_lock);
+
+	if (!test_bit(FLAG_EXPORT, &desc->flags))
+		status = -EIO;
+	else if (sysfs_streq(buf, "enabled"))
+		status = gpio_wakeup_configure(gpio, 1);
+	else if (sysfs_streq(buf, "disabled"))
+		status = gpio_wakeup_configure(gpio, 0);
+	else
+		status = -EINVAL;
+
+	mutex_unlock(&sysfs_lock);
+	return status ? : size;
+}
+
+static const DEVICE_ATTR(wakeup, 0644,
+			 gpio_wakeup_show, gpio_wakeup_store);
+
 static ssize_t gpio_value_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -287,6 +334,7 @@ static /*const*/ DEVICE_ATTR(value, 0644,
 static const struct attribute *gpio_attrs[] = {
 	&dev_attr_direction.attr,
 	&dev_attr_value.attr,
+	&dev_attr_wakeup.attr,
 	NULL,
 };
 
@@ -995,6 +1043,62 @@ fail:
 }
 EXPORT_SYMBOL_GPL(gpio_direction_output);
 
+int gpio_wakeup_configure(unsigned gpio, int value)
+{
+	unsigned long		flags;
+	struct gpio_chip	*chip;
+	struct gpio_desc	*desc = &gpio_desc[gpio];
+	int			status = -EINVAL;
+
+	spin_lock_irqsave(&gpio_lock, flags);
+
+	if (!gpio_is_valid(gpio))
+		goto fail;
+	chip = desc->chip;
+	if (!chip || !chip->set || !chip->wakeup_configure)
+		goto fail;
+	gpio -= chip->base;
+	if (gpio >= chip->ngpio)
+		goto fail;
+	status = gpio_ensure_requested(desc, gpio);
+	if (status < 0)
+		goto fail;
+
+	/* now we know the gpio is valid and chip won't vanish */
+
+	spin_unlock_irqrestore(&gpio_lock, flags);
+
+	might_sleep_if(extra_checks && chip->can_sleep);
+
+	if (status) {
+		status = chip->request(chip, gpio);
+		if (status < 0) {
+			pr_debug("GPIO-%d: chip request fail, %d\n",
+				chip->base + gpio, status);
+			/* and it's not available to anyone else ...
+			 * gpio_request() is the fully clean solution.
+			 */
+			goto lose;
+		}
+	}
+
+	status = chip->wakeup_configure(chip, gpio, value);
+	if (status == 0) {
+		if (value)
+			set_bit(FLAG_IS_WAKEUP, &desc->flags);
+		else
+			clear_bit(FLAG_IS_WAKEUP, &desc->flags);
+	}
+lose:
+	return status;
+fail:
+	spin_unlock_irqrestore(&gpio_lock, flags);
+	if (status)
+		pr_debug("%s: gpio-%d status %d\n",
+			__func__, gpio, status);
+	return status;
+}
+EXPORT_SYMBOL_GPL(gpio_wakeup_configure);
 
 /* I/O calls are only valid after configuration completed; the relevant
  * "is this a valid GPIO" error checks should already have been done.
@@ -1049,7 +1153,6 @@ EXPORT_SYMBOL_GPL(__gpio_get_value);
 void __gpio_set_value(unsigned gpio, int value)
 {
 	struct gpio_chip	*chip;
-
 	chip = gpio_to_chip(gpio);
 	WARN_ON(extra_checks && chip->can_sleep);
 	chip->set(chip, gpio - chip->base, value);
@@ -1093,7 +1196,21 @@ int __gpio_to_irq(unsigned gpio)
 }
 EXPORT_SYMBOL_GPL(__gpio_to_irq);
 
+/**
+ * __gpio_set_pullupdown() - set pull up/down resistor
+ * Context: any
+ *
+ * This is used, where available, to set the internal
+ * GPIO pull up/down resistor.
+ */
+void __gpio_set_pullupdown(unsigned gpio, int value)
+{
+	struct gpio_chip	*chip;
 
+	chip = gpio_to_chip(gpio);
+	chip->pullupdown(chip, gpio - chip->base, value);
+}
+EXPORT_SYMBOL_GPL(__gpio_set_pullupdown);
 
 /* There's no value in making it easy to inline GPIO calls that may sleep.
  * Common examples include ones connected to I2C or SPI chips.
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index c709e821f04b..a92dc21ddf2c 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -888,4 +888,14 @@ config HWMON_DEBUG_CHIP
 	  a problem with I2C support and want to see more of what is going
 	  on.
 
+config MXC_MMA7450
+	tristate "MMA7450 device driver"
+	depends on MACH_MX31_3DS
+	default n
+
+config SENSORS_ISL29003
+	tristate "ISL29003 Light Sensor"
+	depends on MACH_MX37_3DS || MACH_MX51_3DS
+	default y
+
 endif # HWMON
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 58fc5be5355d..f2d69799c424 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -77,7 +77,9 @@ obj-$(CONFIG_SENSORS_VT1211)	+= vt1211.o
 obj-$(CONFIG_SENSORS_VT8231)	+= vt8231.o
 obj-$(CONFIG_SENSORS_W83627EHF)	+= w83627ehf.o
 obj-$(CONFIG_SENSORS_W83L785TS)	+= w83l785ts.o
+obj-$(CONFIG_MXC_MMA7450)	+= mxc_mma7450.o
 obj-$(CONFIG_SENSORS_W83L786NG)	+= w83l786ng.o
+obj-$(CONFIG_SENSORS_ISL29003)	+= isl29003.o
 
 ifeq ($(CONFIG_HWMON_DEBUG_CHIP),y)
 EXTRA_CFLAGS += -DDEBUG
diff --git a/drivers/hwmon/isl29003.c b/drivers/hwmon/isl29003.c
new file mode 100644
index 000000000000..be3f9b90a183
--- /dev/null
+++ b/drivers/hwmon/isl29003.c
@@ -0,0 +1,441 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/hwmon/isl29003.c
+ *
+ * @brief ISL29003 light sensor Driver
+ *
+ * @ingroup
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/regulator/consumer.h>
+
+enum isl29003_width {
+	ISL29003_WIDTH_16 = 0,
+	ISL29003_WIDTH_12,
+	ISL29003_WIDTH_8,
+	ISL29003_WIDTH_4,
+};
+
+enum isl29003_gain {
+	ISL29003_GAIN_1000 = 0,
+	ISL29003_GAIN_4000,
+	ISL29003_GAIN_16000,
+	ISL29003_GAIN_64000,
+};
+
+enum isl29003_mode {
+	ISL29003_MODE_DIODE1 = 0,
+	ISL29003_MODE_DIODE2,
+	ISL29003_MODE_DIODE1_2,
+};
+
+struct isl29003_param {
+	enum isl29003_width width;
+	enum isl29003_gain gain;
+	enum isl29003_mode mode;
+};
+
+/* bit definition for ISL29003_CMD reg */
+#define ENABLE 7
+#define ADCPD 6
+#define TIMEING_MODE 5
+#define MODE 2
+#define WIDTH 0
+
+/* bit definition for ISL29003_CTRL reg */
+#define INT_FLAG 5
+#define GAIN 2
+#define INT_PERSIST 0
+
+enum isl29003_reg {
+	ISL29003_CMD = 0,
+	ISL29003_CTRL,
+	ISL29003_THRS_HI,
+	ISL29003_THRS_LO,
+	ISL29003_LSB_S,
+	ISL29003_MSB_S,
+	ISL29003_LSB_T,
+	ISL29003_MSB_T,
+	ISL29003_SYNC_IIC = 0x80,
+	ISL29003_CLAR_INT = 0x40
+};
+
+/* default configure for ISL29003 */
+#define ISL29003_WIDTH_DEFAULT ISL29003_WIDTH_16
+#define ISL29003_GAIN_DEFAULT ISL29003_GAIN_16000
+#define ISL29003_MODE_DEFAULT ISL29003_MODE_DIODE1
+
+/* range table for different GAIN settings */
+int range[4] = { 973, 3892, 15568, 62272 };
+
+/* width table for different WIDTH settings */
+int width[4] = { 16, 1, 256, 16 };
+
+struct isl29003_data {
+	struct i2c_client *client;
+	struct device *hwmon_dev;
+	struct regulator *vdd_reg;
+	struct isl29003_param param;
+	int lux_coeff;
+	unsigned char enable;
+};
+
+static struct i2c_client *isl29003_client;
+
+/*!
+ * This function do the isl29003 register read.
+ */
+int isl29003_read(struct i2c_client *client, u8 reg)
+{
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+/*!
+ * This function do the isl29003 register write.
+ */
+int isl29003_write(struct i2c_client *client, u8 reg, char value)
+{
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+/*!
+ * This function do the isl29003 config and enable.
+ */
+static int isl29003_on(void)
+{
+	unsigned char cmd;
+	int err = 0;
+	struct mxc_lightsensor_platform_data *ls_data;
+	struct isl29003_data *data = i2c_get_clientdata(isl29003_client);
+
+	if (data->enable)
+		goto exit;
+
+	ls_data = (struct mxc_lightsensor_platform_data *)
+	    (isl29003_client->dev).platform_data;
+
+	/* coeff=range*100k/rext/2^n */
+	data->lux_coeff = range[data->param.gain] * 100 /
+	    ls_data->rext / width[data->param.width];
+
+	if (data->vdd_reg)
+		regulator_enable(data->vdd_reg);
+	msleep(100);
+
+	cmd = data->param.gain << GAIN;
+	if (isl29003_write(isl29003_client, ISL29003_CTRL, cmd)) {
+		err = -ENODEV;
+		goto exit;
+	}
+
+	cmd = (data->param.width << WIDTH) | (data->param.mode << MODE) |
+	    (1 << ENABLE);
+	if (isl29003_write(isl29003_client, ISL29003_CMD, cmd)) {
+		err = -ENODEV;
+		goto exit;
+	}
+
+	data->enable = 1;
+
+	pr_info("isl29003 on\n");
+	return 0;
+exit:
+	return err;
+}
+
+/*!
+ * This function shut down the isl29003.
+ */
+static int isl29003_off(void)
+{
+	struct isl29003_data *data = i2c_get_clientdata(isl29003_client);
+	int cmd;
+
+	if (!data->enable)
+		return 0;
+
+	cmd = isl29003_read(isl29003_client, ISL29003_CMD);
+	if (cmd < 0)
+		return -ENODEV;
+
+	cmd = ((cmd | (1 << ADCPD)) & (~(1 << ENABLE)));
+	if (isl29003_write(isl29003_client, ISL29003_CMD, (char)cmd))
+		return -ENODEV;
+
+	if (data->vdd_reg)
+		regulator_disable(data->vdd_reg);
+
+	data->enable = 0;
+
+	pr_info("isl29003 off\n");
+	return 0;
+}
+
+/*!
+ * This function read the isl29003 lux registers and convert them to the lux
+ * value.
+ *
+ * @output buffer	this param holds the lux value, when =-1, read fail
+ *
+ * @return 0
+ */
+static int isl29003_read_lux(void)
+{
+	int d;
+	int lux;
+	struct isl29003_data *data = i2c_get_clientdata(isl29003_client);
+
+	d = isl29003_read(isl29003_client, ISL29003_MSB_S);
+	if (d < 0)
+		goto err;
+
+	lux = d;
+	d = isl29003_read(isl29003_client, ISL29003_LSB_S);
+	if (d < 0)
+		goto err;
+
+	lux = (lux << 8) + d;
+
+	if (data->param.width < ISL29003_WIDTH_8)
+		lux = (data->lux_coeff * lux) >> 12;
+	else
+		lux = data->lux_coeff * lux;
+
+	return lux;
+err:
+	return -1;
+}
+
+static ssize_t ls_enable(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	char *endp;
+	int enable = simple_strtoul(buf, &endp, 0);
+	size_t size = endp - buf;
+
+	if (*endp && isspace(*endp))
+		size++;
+	if (size != count)
+		return -EINVAL;
+
+	if (enable == 1) {
+		if (isl29003_on())
+			pr_info("device open fail\n");
+	}
+	if (enable == 0) {
+		if (isl29003_off())
+			pr_info("device powerdown fail\n");
+	}
+
+	return count;
+}
+
+static SENSOR_DEVICE_ATTR(enable, S_IWUGO, NULL, ls_enable, 0);
+
+static ssize_t show_lux(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", isl29003_read_lux());
+}
+
+static SENSOR_DEVICE_ATTR(lux, S_IRUGO, show_lux, NULL, 0);
+
+static int isl29003_i2c_probe(struct i2c_client *client,
+			      const struct i2c_device_id *did)
+{
+	int err = 0;
+	struct isl29003_data *data;
+	struct regulator *vdd_reg;
+	struct mxc_lightsensor_platform_data *ls_data;
+
+	ls_data = (struct mxc_lightsensor_platform_data *)
+	    (client->dev).platform_data;
+
+	if (ls_data && ls_data->vdd_reg)
+		vdd_reg = regulator_get(&client->dev, ls_data->vdd_reg);
+	else
+		vdd_reg = NULL;
+
+	/* check the existence of the device */
+	if (vdd_reg)
+		regulator_enable(vdd_reg);
+	msleep(100);
+
+	if (isl29003_write(client, ISL29003_CMD, 0))
+		err = -ENODEV;
+
+	if (!err)
+		if (isl29003_read(client, ISL29003_CMD))
+			err = -ENODEV;
+
+	if (vdd_reg)
+		regulator_disable(vdd_reg);
+	if (err < 0)
+		goto exit1;
+
+	isl29003_client = client;
+	data = kzalloc(sizeof(struct isl29003_data), GFP_KERNEL);
+	if (data == NULL) {
+		err = -ENOMEM;
+		goto exit1;
+	}
+
+	i2c_set_clientdata(client, data);
+	data->client = client;
+
+	data->param.width = ISL29003_WIDTH_DEFAULT;
+	data->param.gain = ISL29003_GAIN_DEFAULT;
+	data->param.mode = ISL29003_MODE_DEFAULT;
+
+	data->enable = 0;
+
+	err = device_create_file(&client->dev,
+				 &sensor_dev_attr_enable.dev_attr);
+	if (err)
+		goto exit2;
+
+	err = device_create_file(&client->dev, &sensor_dev_attr_lux.dev_attr);
+	if (err)
+		goto exit_remove1;
+
+	/* Register sysfs hooks */
+	data->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		goto exit_remove2;
+	}
+
+	data->vdd_reg = vdd_reg;
+	return 0;
+
+exit_remove2:
+	device_remove_file(&client->dev, &sensor_dev_attr_lux.dev_attr);
+exit_remove1:
+	device_remove_file(&client->dev, &sensor_dev_attr_enable.dev_attr);
+exit2:
+	kfree(data);
+exit1:
+	if (vdd_reg) {
+		regulator_put(vdd_reg);
+		vdd_reg = NULL;
+	}
+	isl29003_client = NULL;
+	return err;
+}
+
+static int isl29003_i2c_remove(struct i2c_client *client)
+{
+	int err;
+	struct isl29003_data *data = i2c_get_clientdata(client);
+
+	if (data->vdd_reg) {
+		regulator_put(data->vdd_reg);
+		data->vdd_reg = NULL;
+	}
+	hwmon_device_unregister(data->hwmon_dev);
+	device_remove_file(&client->dev, &sensor_dev_attr_enable.dev_attr);
+	device_remove_file(&client->dev, &sensor_dev_attr_lux.dev_attr);
+	err = i2c_detach_client(client);
+	if (err)
+		return err;
+	kfree(client);
+	return 0;
+}
+
+static int isl29003_suspend(struct i2c_client *client, pm_message_t message)
+{
+	int cmd;
+
+	struct isl29003_data *data = i2c_get_clientdata(client);
+
+	if (!data->enable)
+		goto exit;
+
+	cmd = isl29003_read(client, ISL29003_CMD);
+	if (cmd < 0)
+		goto err;
+
+	cmd = (cmd | (1 << ADCPD));
+	if (isl29003_write(client, ISL29003_CMD, (char)cmd))
+		goto err;
+exit:
+	return 0;
+err:
+	return -ENODEV;
+}
+
+static int isl29003_resume(struct i2c_client *client)
+{
+	int cmd;
+
+	struct isl29003_data *data = i2c_get_clientdata(client);
+
+	if (!data->enable)
+		goto exit;
+
+	cmd = isl29003_read(client, ISL29003_CMD);
+	if (cmd < 0)
+		goto err;
+
+	cmd = (cmd & (~(1 << ADCPD)));
+	if (isl29003_write(client, ISL29003_CMD, (char)cmd))
+		goto err;
+exit:
+	return 0;
+err:
+	return -ENODEV;
+}
+
+static const struct i2c_device_id isl29003_id[] = {
+	{"isl29003", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, isl29003_id);
+
+static struct i2c_driver isl29003_driver = {
+	.driver = {
+		   .name = "isl29003",
+		   },
+	.probe = isl29003_i2c_probe,
+	.remove = isl29003_i2c_remove,
+	.suspend = isl29003_suspend,
+	.resume = isl29003_resume,
+	.id_table = isl29003_id,
+};
+
+static int __init isl29003_init(void)
+{
+	return i2c_add_driver(&isl29003_driver);;
+}
+
+static void __exit isl29003_cleanup(void)
+{
+	i2c_del_driver(&isl29003_driver);
+}
+
+module_init(isl29003_init);
+module_exit(isl29003_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("ISL29003 light sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/mxc_mma7450.c b/drivers/hwmon/mxc_mma7450.c
new file mode 100644
index 000000000000..1c78631044b9
--- /dev/null
+++ b/drivers/hwmon/mxc_mma7450.c
@@ -0,0 +1,788 @@
+/*
+ * linux/drivers/hwmon/mma7450.c
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*include file*/
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/input-polldev.h>
+#include <linux/hwmon.h>
+#include <linux/regulator/consumer.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <mach/mxc.h>
+
+/*macro define*/
+#define MMA7450_I2C_ADDR	0x1D
+#define DEVICE_NAME		"mma7450"
+#define POLL_INTERVAL		100
+#define DEBUG
+
+#define INPUT_FUZZ	4
+#define INPUT_FLAT	4
+
+enum {
+	REG_XOUTL = 0x00,
+	REG_XOUTH,
+	REG_YOUTL,
+	REG_YOUTH,
+	REG_ZOUTL,
+	REG_ZOUTH,
+	REG_XOUT8,
+	REG_YOUT8,
+	REG_ZOUT8,
+	REG_STATUS,
+	REG_DETSRC,
+	REG_TOUT,
+	REG_RESERVED_0,
+	REG_I2CAD,
+	REG_USRINF,
+	REG_WHOAMI,
+	REG_XOFFL,
+	REG_XOFFH,
+	REG_YOFFL,
+	REG_YOFFH,
+	REG_ZOFFL,
+	REG_ZOFFH,
+	REG_MCTL,
+	REG_INTRST,
+	REG_CTL1,
+	REG_CTL2,
+	REG_LDTH,
+	REG_PDTH,
+	REG_PD,
+	REG_LT,
+	REG_TW,
+	REG_REVERVED_1,
+};
+
+enum {
+	MOD_STANDBY = 0,
+	MOD_MEASURE,
+	MOD_LEVEL_D,
+	MOD_PULSE_D,
+};
+
+enum {
+	INT_1L_2P = 0,
+	INT_1P_2L,
+	INT_1SP_2P,
+};
+
+struct mma7450_status {
+	u8 mod;
+	u8 ctl1;
+	u8 ctl2;
+};
+
+/*forward declear*/
+static ssize_t mma7450_show(struct device *dev,
+			    struct device_attribute *attr, char *buf);
+static ssize_t mma7450_store(struct device *dev,
+			     struct device_attribute *attr, const char *buf,
+			     size_t count);
+static int mma7450_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id);
+static int mma7450_remove(struct i2c_client *client);
+static int mma7450_suspend(struct i2c_client *client, pm_message_t state);
+static int mma7450_resume(struct i2c_client *client);
+static void mma_bh_handler(struct work_struct *work);
+
+/*definition*/
+static struct regulator *reg_dvdd_io;
+static struct regulator *reg_avdd;
+static struct i2c_client *mma7450_client;
+static struct device *hwmon_dev;
+static struct input_polled_dev *mma7450_idev;
+static struct mxc_mma7450_platform_data *plat_data;
+static u8 mma7450_mode;
+static struct device_attribute mma7450_dev_attr = {
+	.attr = {
+		 .name = "mma7450_ctl",
+		 .mode = S_IRUSR | S_IWUSR,
+		 },
+	.show = mma7450_show,
+	.store = mma7450_store,
+};
+
+static const struct i2c_device_id mma7450_id[] = {
+	{"mma7450", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, mma7450_id);
+
+static struct i2c_driver i2c_mma7450_driver = {
+	.driver = {
+		   .name = "mma7450",
+		   },
+	.probe = mma7450_probe,
+	.remove = mma7450_remove,
+	.suspend = mma7450_suspend,
+	.resume = mma7450_resume,
+	.id_table = mma7450_id,
+};
+
+static struct mma7450_status mma_status = {
+	.mod = 0,
+	.ctl1 = 0,
+	.ctl2 = 0,
+};
+
+DECLARE_WORK(mma_work, mma_bh_handler);
+
+#ifdef DEBUG
+enum {
+	MMA_REG_R = 0,
+	MMA_REG_W,
+	MMA_SET_MOD,
+	MMA_SET_L_THR,
+	MMA_SET_P_THR,
+	MMA_SET_INTP,
+	MMA_SET_INTB,
+	MMA_SET_G,
+	MMA_I2C_EABLE,
+	MMA_OFF_X,
+	MMA_OFF_Y,
+	MMA_OFF_Z,
+	MMA_SELF_TEST,
+	MMA_SET_LDPL,
+	MMA_SET_PDPL,
+	MMA_SET_PDV,
+	MMA_SET_LTV,
+	MMA_SET_TW,
+	MMA_CMD_MAX
+};
+
+static char *command[MMA_CMD_MAX] = {
+	[MMA_REG_R] = "readreg",
+	[MMA_REG_W] = "writereg",
+	[MMA_SET_MOD] = "setmod",
+	[MMA_SET_L_THR] = "setlt",
+	[MMA_SET_P_THR] = "setpt",
+	[MMA_SET_INTP] = "setintp",
+	[MMA_SET_INTB] = "setintb",
+	[MMA_SET_G] = "setg",
+	[MMA_I2C_EABLE] = "setie",
+	[MMA_OFF_X] = "setxo",
+	[MMA_OFF_Y] = "setyo",
+	[MMA_OFF_Z] = "setzo",
+	[MMA_SELF_TEST] = "selft",
+	[MMA_SET_LDPL] = "setldp",
+	[MMA_SET_PDPL] = "setpdp",
+	[MMA_SET_PDV] = "setpdv",
+	[MMA_SET_LTV] = "setltv",
+	[MMA_SET_TW] = "settw",
+};
+
+static void set_mod(u8 mode)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(mma7450_client, REG_MCTL);
+	/* shall I test the ret value? */
+	ret = (ret & ~0x3) | (mode & 0x3);
+	mma_status.mod = ret;
+	i2c_smbus_write_byte_data(mma7450_client, REG_MCTL, ret);
+}
+
+static void set_level_thr(u8 lth)
+{
+	i2c_smbus_write_byte_data(mma7450_client, REG_LDTH, lth);
+}
+
+static void set_pulse_thr(u8 pth)
+{
+	i2c_smbus_write_byte_data(mma7450_client, REG_PDTH, pth);
+}
+
+static void set_int_pin(u8 pin)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(mma7450_client, REG_CTL1);
+	ret = (ret & ~0x1) | (pin & 0x1);
+	mma_status.ctl1 = ret;
+	i2c_smbus_write_byte_data(mma7450_client, REG_CTL1, ret);
+}
+
+static void set_int_bit(u8 bit)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(mma7450_client, REG_CTL1);
+	ret = (ret & ~0x6) | ((bit << 1) & 0x6);
+	mma_status.ctl1 = ret;
+	i2c_smbus_write_byte_data(mma7450_client, REG_CTL1, ret);
+}
+
+static void set_g_level(u8 gl)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(mma7450_client, REG_MCTL);
+	ret = (ret & ~0xC) | ((gl << 2) & 0xC);
+	i2c_smbus_write_byte_data(mma7450_client, REG_MCTL, ret);
+}
+
+static void set_i2c_enable(u8 i2c_e)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(mma7450_client, REG_I2CAD);
+	ret = (ret & ~0x80) | ((i2c_e << 7) & 0x80);
+	i2c_smbus_write_byte_data(mma7450_client, REG_I2CAD, ret);
+}
+
+static void set_x_offset(u16 xo)
+{
+	u8 data;
+
+	data = (xo & 0xFF);
+	i2c_smbus_write_byte_data(mma7450_client, REG_XOFFL, data);
+	data = (xo & 0xFF00) >> 8;
+	i2c_smbus_write_byte_data(mma7450_client, REG_XOFFH, data);
+}
+
+static void set_y_offset(u16 yo)
+{
+	u8 data;
+
+	data = (yo & 0xFF);
+	i2c_smbus_write_byte_data(mma7450_client, REG_YOFFL, data);
+	data = (yo & 0xFF00) >> 8;
+	i2c_smbus_write_byte_data(mma7450_client, REG_YOFFH, data);
+}
+
+static void set_z_offset(u16 zo)
+{
+	u8 data;
+
+	data = (zo & 0xFF);
+	i2c_smbus_write_byte_data(mma7450_client, REG_ZOFFL, data);
+	data = (zo & 0xFF00) >> 8;
+	i2c_smbus_write_byte_data(mma7450_client, REG_ZOFFH, data);
+}
+
+static void selftest(u8 st)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(mma7450_client, REG_MCTL);
+	ret = (ret & ~0x10) | ((st << 4) & 0x10);
+	i2c_smbus_write_byte_data(mma7450_client, REG_MCTL, ret);
+}
+
+static void set_level_det_p(u8 ldp)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(mma7450_client, REG_CTL2);
+	ret = (ret & ~0x1) | ((ldp << 0) & 0x1);
+	mma_status.ctl2 = ret;
+	i2c_smbus_write_byte_data(mma7450_client, REG_CTL2, ret);
+}
+
+static void set_pulse_det_p(u8 pdp)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(mma7450_client, REG_CTL2);
+	ret = (ret & ~0x2) | ((pdp << 1) & 0x2);
+	mma_status.ctl2 = ret;
+	i2c_smbus_write_byte_data(mma7450_client, REG_CTL2, ret);
+}
+
+static void set_pulse_duration(u8 pd)
+{
+	i2c_smbus_write_byte_data(mma7450_client, REG_PD, pd);
+}
+
+static void set_latency_time(u8 lt)
+{
+	i2c_smbus_write_byte_data(mma7450_client, REG_LT, lt);
+}
+
+static void set_time_window(u8 tw)
+{
+	i2c_smbus_write_byte_data(mma7450_client, REG_TW, tw);
+}
+
+static void parse_arg(const char *arg, int *reg, int *value)
+{
+	const char *p;
+
+	for (p = arg;; p++) {
+		if (*p == ' ' || *p == '\0')
+			break;
+	}
+
+	p++;
+
+	*reg = simple_strtoul(arg, NULL, 16);
+	*value = simple_strtoul(p, NULL, 16);
+}
+
+static void cmd_read_reg(const char *arg)
+{
+	int reg, value, ret;
+
+	parse_arg(arg, &reg, &value);
+	ret = i2c_smbus_read_byte_data(mma7450_client, reg);
+	dev_info(&mma7450_client->dev, "read reg0x%x = %x\n", reg, ret);
+}
+
+static void cmd_write_reg(const char *arg)
+{
+	int reg, value, ret;
+
+	parse_arg(arg, &reg, &value);
+	ret = i2c_smbus_write_byte_data(mma7450_client, reg, value);
+	dev_info(&mma7450_client->dev, "write reg result %s\n",
+		 ret ? "failed" : "success");
+}
+
+static int exec_command(const char *buf, size_t count)
+{
+	const char *p, *s;
+	const char *arg;
+	int i, value = 0;
+
+	for (p = buf;; p++) {
+		if (*p == ' ' || *p == '\0' || p - buf >= count)
+			break;
+	}
+	arg = p + 1;
+
+	for (i = MMA_REG_R; i < MMA_CMD_MAX; i++) {
+		s = command[i];
+		if (s && !strncmp(buf, s, p - buf)) {
+			dev_info(&mma7450_client->dev, "command %s\n", s);
+			goto mma_exec_command;
+		}
+	}
+
+	dev_err(&mma7450_client->dev, "command is not found\n");
+	return -1;
+
+      mma_exec_command:
+	if (i != MMA_REG_R && i != MMA_REG_W)
+		value = simple_strtoul(arg, NULL, 16);
+
+	switch (i) {
+	case MMA_REG_R:
+		cmd_read_reg(arg);
+		break;
+	case MMA_REG_W:
+		cmd_write_reg(arg);
+		break;
+	case MMA_SET_MOD:
+		set_mod(value);
+		break;
+	case MMA_SET_L_THR:
+		set_level_thr(value);
+		break;
+	case MMA_SET_P_THR:
+		set_pulse_thr(value);
+		break;
+	case MMA_SET_INTP:
+		set_int_pin(value);
+		break;
+	case MMA_SET_INTB:
+		set_int_bit(value);
+		break;
+	case MMA_SET_G:
+		set_g_level(value);
+		break;
+	case MMA_I2C_EABLE:
+		set_i2c_enable(value);
+		break;
+	case MMA_OFF_X:
+		set_x_offset(value);
+		break;
+	case MMA_OFF_Y:
+		set_y_offset(value);
+		break;
+	case MMA_OFF_Z:
+		set_z_offset(value);
+		break;
+	case MMA_SELF_TEST:
+		selftest(value);
+		break;
+	case MMA_SET_LDPL:
+		set_level_det_p(value);
+		break;
+	case MMA_SET_PDPL:
+		set_pulse_det_p(value);
+		break;
+	case MMA_SET_PDV:
+		set_pulse_duration(value);
+		break;
+	case MMA_SET_LTV:
+		set_latency_time(value);
+		break;
+	case MMA_SET_TW:
+		set_time_window(value);
+		break;
+	default:
+		dev_err(&mma7450_client->dev, "command is not found\n");
+		break;
+	}
+
+	return 0;
+}
+
+static ssize_t mma7450_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	int ret, reg;
+
+	for (reg = REG_XOUTL; reg < REG_REVERVED_1; reg++) {
+		ret = i2c_smbus_read_byte_data(mma7450_client, reg);
+		dev_info(&mma7450_client->dev, "reg0x%02x:\t%03d\t0x%02x\n",
+			 reg, (s8) ret, ret);
+	}
+
+	return 0;
+}
+
+static ssize_t mma7450_store(struct device *dev,
+			     struct device_attribute *attr, const char *buf,
+			     size_t count)
+{
+	exec_command(buf, count);
+
+	return count;
+}
+
+#else
+
+static ssize_t mma7450_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	return 0;
+}
+
+static ssize_t mma7450_store(struct device *dev,
+			     struct device_attribute *attr, const char *buf,
+			     size_t count)
+{
+	return count;
+}
+
+#endif
+
+static void report_abs(void)
+{
+	u8 status, mod = mma_status.mod;
+	s16 x, y, z, tmp;
+
+	status = i2c_smbus_read_byte_data(mma7450_client, REG_STATUS);
+	if (!(status & 0x01)) {	/* data ready in measurement mode? */
+		return;
+	}
+	if ((mod & 0x0c) == 0) {	/* 8g range */
+		x = 0xFF & i2c_smbus_read_byte_data(mma7450_client, REG_XOUTL);
+		x |= 0xFF00 &
+		    (i2c_smbus_read_byte_data(mma7450_client, REG_XOUTH) << 8);
+		y = 0xFF & i2c_smbus_read_byte_data(mma7450_client, REG_YOUTL);
+		y |= 0xFF00 &
+		    (i2c_smbus_read_byte_data(mma7450_client, REG_YOUTH) << 8);
+		z = 0xFF & i2c_smbus_read_byte_data(mma7450_client, REG_ZOUTL);
+		z |= 0xFF00 &
+		    (i2c_smbus_read_byte_data(mma7450_client, REG_ZOUTH) << 8);
+	} else {		/* 2g/4g range */
+		x = 0xFF & i2c_smbus_read_byte_data(mma7450_client, REG_XOUT8);
+		y = 0xFF & i2c_smbus_read_byte_data(mma7450_client, REG_YOUT8);
+		z = 0xFF & i2c_smbus_read_byte_data(mma7450_client, REG_ZOUT8);
+	}
+
+	status = i2c_smbus_read_byte_data(mma7450_client, REG_STATUS);
+	if (status & 0x02) {	/* data is overwrite */
+		return;
+	}
+
+	/* convert signed 10bits to signed 16bits */
+	x = (short)(x << 6) >> 6;
+	y = (short)(y << 6) >> 6;
+	z = (short)(z << 6) >> 6;
+
+	input_report_abs(mma7450_idev->input, ABS_X, x);
+	input_report_abs(mma7450_idev->input, ABS_Y, y);
+	input_report_abs(mma7450_idev->input, ABS_Z, z);
+	input_sync(mma7450_idev->input);
+}
+
+static void mma_bh_handler(struct work_struct *work)
+{
+}
+
+static void mma7450_dev_poll(struct input_polled_dev *dev)
+{
+	report_abs();
+}
+
+static irqreturn_t mma7450_interrupt(int irq, void *dev_id)
+{
+	struct input_dev *input_dev = dev_id;
+	u8 int_bit, int_pin;
+
+	int_bit = mma_status.ctl1 & 0x6;
+	int_pin = mma_status.ctl1 & 0x1;
+
+	switch (mma_status.mod & 0x03) {
+	case 1:
+		/*only int1 report data ready int */
+		if (plat_data->int1 != irq)
+			goto error_bad_int;
+		schedule_work(&mma_work);
+		break;
+	case 2:
+		/* for level and pulse detection mode,
+		 * choice tasklet to handle interrupt quickly.
+		 * Currently, leave it doing nothing*/
+		if (plat_data->int1 == irq) {
+			if ((int_bit == 0) && (int_pin != 0))
+				goto error_bad_int;
+			if ((int_bit == 0x2) && (int_pin != 0x1))
+				goto error_bad_int;
+			if (int_bit == 0x4)
+				goto error_bad_int;
+		}
+		if (plat_data->int2 == irq) {
+			if ((int_bit == 0) && (int_pin != 0x1))
+				goto error_bad_int;
+			if ((int_bit == 0x2) && (int_pin != 0))
+				goto error_bad_int;
+			if (int_bit == 0x4)
+				goto error_bad_int;
+		}
+
+		dev_info(&input_dev->dev, "motion detected in level mod\n");
+
+		break;
+	case 3:
+		if (plat_data->int1 == irq) {
+			if ((int_bit == 0) && (int_pin != 0x1))
+				goto error_bad_int;
+			if ((int_bit == 0x2) && (int_pin != 0))
+				goto error_bad_int;
+			if ((int_bit == 0x4) && (int_pin != 0x1))
+				goto error_bad_int;
+		}
+		if (plat_data->int2 == irq) {
+			if ((int_bit == 0) && (int_pin != 0))
+				goto error_bad_int;
+			if ((int_bit == 0x2) && (int_pin != 0x1))
+				goto error_bad_int;
+			if ((int_bit == 0x4) && (int_pin != 0))
+				goto error_bad_int;
+		}
+
+		if (mma_status.ctl2 & 0x02)
+			dev_info(&input_dev->dev,
+				 "freefall detected in pulse mod\n");
+		else
+			dev_info(&input_dev->dev,
+				 "motion detected in pulse mod\n");
+
+		break;
+	case 0:
+	default:
+		break;
+	}
+      error_bad_int:
+	return IRQ_RETVAL(1);
+}
+
+static int mma7450_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	int ret;
+	struct input_dev *idev;
+
+	plat_data =
+	    (struct mxc_mma7450_platform_data *)client->dev.platform_data;
+	if (plat_data == NULL) {
+		dev_err(&client->dev, "lack of platform data!\n");
+		return -ENODEV;
+	}
+
+	/*enable power supply */
+	/*when to power on/off the power is to be considered later */
+	/*shall I check the return value */
+	reg_dvdd_io = regulator_get(&client->dev, plat_data->reg_dvdd_io);
+	if (reg_dvdd_io != ERR_PTR(-ENOENT))
+		regulator_enable(reg_dvdd_io);
+	else
+		return -EINVAL;
+
+	reg_avdd = regulator_get(&client->dev, plat_data->reg_avdd);
+	if (reg_avdd != ERR_PTR(-ENOENT))
+		regulator_enable(reg_avdd);
+	else {
+		regulator_put(reg_dvdd_io);
+		return -EINVAL;
+	}
+
+	/*bind the right device to the driver */
+	ret = i2c_smbus_read_byte_data(client, REG_I2CAD);
+	if (MMA7450_I2C_ADDR != (0x7F & ret)) {	/*compare the address value */
+		dev_err(&client->dev,
+			"read chip ID 0x%x is not equal to 0x%x!\n", ret,
+			MMA7450_I2C_ADDR);
+		goto error_disable_power;
+	}
+	mma7450_client = client;
+
+	/*interrupt register */
+	/*when to register interrupt is to be considered later */
+
+	/*create device file in sysfs as user interface */
+	ret = device_create_file(&client->dev, &mma7450_dev_attr);
+	if (ret) {
+		dev_err(&client->dev, "create device file failed!\n");
+		goto error_disable_power;
+	}
+
+	/*register to hwmon device */
+	hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(hwmon_dev)) {
+		dev_err(&client->dev, "hwmon register failed!\n");
+		ret = PTR_ERR(hwmon_dev);
+		goto error_rm_dev_file;
+	}
+
+	/*input poll device register */
+	mma7450_idev = input_allocate_polled_device();
+	if (!mma7450_idev) {
+		dev_err(&client->dev, "alloc poll device failed!\n");
+		ret = -ENOMEM;
+		goto error_rm_hwmon_dev;
+	}
+	mma7450_idev->poll = mma7450_dev_poll;
+	mma7450_idev->poll_interval = POLL_INTERVAL;
+	idev = mma7450_idev->input;
+	idev->name = DEVICE_NAME;
+	idev->id.bustype = BUS_I2C;
+	idev->dev.parent = &client->dev;
+	idev->evbit[0] = BIT_MASK(EV_ABS);
+
+	input_set_abs_params(idev, ABS_X, -512, 512, INPUT_FUZZ, INPUT_FLAT);
+	input_set_abs_params(idev, ABS_Y, -512, 512, INPUT_FUZZ, INPUT_FLAT);
+	input_set_abs_params(idev, ABS_Z, -512, 512, INPUT_FUZZ, INPUT_FLAT);
+	ret = input_register_polled_device(mma7450_idev);
+	if (ret) {
+		dev_err(&client->dev, "register poll device failed!\n");
+		goto error_free_poll_dev;
+	}
+
+	/* configure gpio as input for interrupt monitor */
+	plat_data->gpio_pin_get();
+
+	set_irq_type(plat_data->int1, IRQF_TRIGGER_RISING);
+	/* register interrupt handle */
+	ret = request_irq(plat_data->int1, mma7450_interrupt,
+			  IRQF_TRIGGER_RISING, DEVICE_NAME, idev);
+
+	if (ret) {
+		dev_err(&client->dev, "request_irq(%d) returned error %d\n",
+			plat_data->int1, ret);
+		goto error_rm_poll_dev;
+	}
+
+	set_irq_type(plat_data->int2, IRQF_TRIGGER_RISING);
+	ret = request_irq(plat_data->int2, mma7450_interrupt,
+			  IRQF_TRIGGER_RISING, DEVICE_NAME, idev);
+	if (ret) {
+		dev_err(&client->dev, "request_irq(%d) returned error %d\n",
+			plat_data->int2, ret);
+		goto error_free_irq1;
+	}
+
+	dev_info(&client->dev, "mma7450 device is probed successfully.\n");
+
+	set_mod(1);
+	return 0;		/*what value shall be return */
+
+	/*error handle */
+      error_free_irq1:
+	free_irq(plat_data->int1, 0);
+      error_rm_poll_dev:
+	input_unregister_polled_device(mma7450_idev);
+      error_free_poll_dev:
+	input_free_polled_device(mma7450_idev);
+      error_rm_hwmon_dev:
+	hwmon_device_unregister(hwmon_dev);
+      error_rm_dev_file:
+	device_remove_file(&client->dev, &mma7450_dev_attr);
+      error_disable_power:
+	regulator_disable(reg_dvdd_io);	/*shall I check the return value */
+	regulator_disable(reg_avdd);
+	regulator_put(reg_dvdd_io);
+	regulator_put(reg_avdd);
+
+	return ret;
+}
+
+static int mma7450_remove(struct i2c_client *client)
+{
+	free_irq(plat_data->int2, mma7450_idev->input);
+	free_irq(plat_data->int1, mma7450_idev->input);
+	plat_data->gpio_pin_put();
+	input_unregister_polled_device(mma7450_idev);
+	input_free_polled_device(mma7450_idev);
+	hwmon_device_unregister(hwmon_dev);
+	device_remove_file(&client->dev, &mma7450_dev_attr);
+	regulator_disable(reg_dvdd_io);	/*shall I check the return value */
+	regulator_disable(reg_avdd);
+	regulator_put(reg_dvdd_io);
+	regulator_put(reg_avdd);
+	return 0;
+}
+
+static int mma7450_suspend(struct i2c_client *client, pm_message_t state)
+{
+	mma7450_mode = i2c_smbus_read_byte_data(mma7450_client, REG_MCTL);
+	i2c_smbus_write_byte_data(mma7450_client, REG_MCTL,
+				  mma7450_mode & ~0x3);
+	return 0;
+}
+
+static int mma7450_resume(struct i2c_client *client)
+{
+	i2c_smbus_write_byte_data(mma7450_client, REG_MCTL, mma7450_mode);
+	return 0;
+}
+
+static int __init init_mma7450(void)
+{
+	/*register driver */
+	printk(KERN_INFO "add mma i2c driver\n");
+	return i2c_add_driver(&i2c_mma7450_driver);
+}
+
+static void __exit exit_mma7450(void)
+{
+	printk(KERN_INFO "del mma i2c driver.\n");
+	return i2c_del_driver(&i2c_mma7450_driver);
+}
+
+module_init(init_mma7450);
+module_exit(exit_mma7450);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MMA7450 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c-slave/Kconfig b/drivers/i2c-slave/Kconfig
new file mode 100644
index 000000000000..6907a2ab6688
--- /dev/null
+++ b/drivers/i2c-slave/Kconfig
@@ -0,0 +1,40 @@
+#
+# I2C slave subsystem configuration
+#
+
+menuconfig I2C_SLAVE
+	bool "I2C Slave support"
+	depends on HAS_IOMEM
+	---help---
+	  I2C (pronounce: I-square-C) is a slow serial bus protocol used in
+	  many micro controller applications and developed by Philips. 
+
+	  If you want I2C Slave support, you should say Y here.
+
+	  This I2C Slave support can also be built as a module.  If so, the module
+	  will be called i2c-slave.
+
+if I2C_SLAVE
+
+config I2C_SLAVE_CORE
+	tristate "I2C SLAVE CORE"
+	default y
+	---help---
+		i2c slave core.
+
+config MXC_I2C_SLAVE
+	tristate "MXC I2C SLAVE"
+	depends on I2C_SLAVE_CORE
+	default y
+	---help---
+		mxc i2c slave support.
+
+config I2C_SLAVE_CLIENT
+	tristate "I2C SLAVE CLIENT"
+	default y
+	---help---
+		i2c slave client that used when the master is on the same board.
+		this is for i2c master which is on the same board with the slave.
+
+endif # I2C_SLAVE
+
diff --git a/drivers/i2c-slave/Makefile b/drivers/i2c-slave/Makefile
new file mode 100644
index 000000000000..675407f405cf
--- /dev/null
+++ b/drivers/i2c-slave/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for the i2c slave.
+#
+
+i2c_slave-objs := i2c_slave_ring_buffer.o i2c_slave_device.o i2c_slave_core.o
+obj-$(CONFIG_I2C_SLAVE_CORE)	+= i2c_slave.o
+obj-$(CONFIG_MXC_I2C_SLAVE)		+= mxc_i2c_slave.o
+obj-$(CONFIG_I2C_SLAVE_CLIENT)		+= i2c_slave_client.o
+
+
diff --git a/drivers/i2c-slave/i2c_slave_client.c b/drivers/i2c-slave/i2c_slave_client.c
new file mode 100644
index 000000000000..4068fe172b83
--- /dev/null
+++ b/drivers/i2c-slave/i2c_slave_client.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright 2007-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+
+struct i2c_client *i2c_slave_client;
+static int i2c_slave_client_probe(struct i2c_client *adapter);
+static int i2c_slave_client_remove(struct i2c_client *client);
+static struct i2c_driver i2c_slave_client_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "i2c-slave-client",
+		   },
+	.probe = i2c_slave_client_probe,
+	.remove = i2c_slave_client_remove,
+};
+
+/*!
+ * ov2640 I2C attach function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  Error code indicating success or failure
+ */
+static int i2c_slave_client_probe(struct i2c_client *client)
+{
+	i2c_slave_client = client;
+	return 0;
+}
+
+/*!
+ * ov2640 I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int i2c_slave_client_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+/*!
+ * ov2640 init function
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int i2c_slave_client_init(void)
+{
+	return i2c_add_driver(&i2c_slave_client_driver);
+}
+
+/*!
+ * OV2640 cleanup function
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit i2c_slave_client_clean(void)
+{
+	i2c_del_driver(&i2c_slave_client_driver);
+}
+
+module_init(i2c_slave_client_init);
+module_exit(i2c_slave_client_clean);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("I2c Slave Client Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c-slave/i2c_slave_core.c b/drivers/i2c-slave/i2c_slave_core.c
new file mode 100644
index 000000000000..0592d53a6fad
--- /dev/null
+++ b/drivers/i2c-slave/i2c_slave_core.c
@@ -0,0 +1,358 @@
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/fs.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <asm/uaccess.h>
+#include "i2c_slave_device.h"
+
+int i2c_slave_major;
+
+static ssize_t i2c_slave_read(struct file *fd, char __user *buf, size_t len,
+			      loff_t *ptr)
+{
+	i2c_slave_device_t *dev;
+	void *kbuf;
+	int ret;
+
+	if (len == 0)
+		return 0;
+
+	kbuf = kmalloc(len, GFP_KERNEL);
+	if (!kbuf) {
+		ret = -ENOMEM;
+		goto error0;
+	}
+
+	dev = (i2c_slave_device_t *) fd->private_data;
+	if (!dev) {
+		ret = -ENODEV;
+		goto error1;
+	}
+
+	ret = i2c_slave_device_read(dev, len, kbuf);
+	if (ret <= 0 || copy_to_user(buf, kbuf, len)) {
+		ret = -EFAULT;
+	}
+
+      error1:
+	kfree(kbuf);
+      error0:
+	return ret;
+}
+
+static ssize_t i2c_slave_write(struct file *fd, const char __user *buf,
+			       size_t len, loff_t *ptr)
+{
+	i2c_slave_device_t *dev;
+	void *kbuf;
+	int ret;
+
+	if (len == 0)
+		return 0;
+
+	kbuf = kmalloc(len, GFP_KERNEL);
+	if (!kbuf) {
+		ret = -ENOMEM;
+		goto error0;
+	}
+	if (copy_from_user(kbuf, buf, len)) {
+		ret = -EFAULT;
+		goto error1;
+	}
+
+	dev = (i2c_slave_device_t *) fd->private_data;
+	if (!dev) {
+		ret = -ENODEV;
+		goto error1;
+	}
+
+	ret = i2c_slave_device_write(dev, len, (u8 *) kbuf);
+
+      error1:
+	kfree(kbuf);
+      error0:
+	return ret;
+}
+
+static int i2c_slave_ioctl(struct inode *inode, struct file *fd,
+			   unsigned code, unsigned long value)
+{
+	/*todo */
+	return 0;
+}
+
+static int i2c_slave_open(struct inode *inode, struct file *fd)
+{
+	int ret;
+	unsigned int id;
+	i2c_slave_device_t *dev;
+	id = iminor(inode);
+
+	if (id >= I2C_SLAVE_DEVICE_MAX) {
+		ret = -ENODEV;
+		goto error;
+	}
+
+	dev = i2c_slave_device_find(id);
+	if (!dev) {
+		ret = -ENODEV;
+		goto error;
+	}
+
+	i2c_slave_rb_clear(dev->receive_buffer);
+	i2c_slave_rb_clear(dev->send_buffer);
+
+	if (i2c_slave_device_start(dev)) {
+		ret = -EBUSY;
+		goto error;
+	}
+
+	fd->private_data = (void *)dev;
+	ret = 0;
+
+      error:
+	return ret;
+}
+
+static int i2c_slave_release(struct inode *inode, struct file *fd)
+{
+	int ret;
+	unsigned int id;
+	i2c_slave_device_t *dev;
+	id = iminor(inode);
+
+	if (id >= I2C_SLAVE_DEVICE_MAX) {
+		ret = -ENODEV;
+		goto error;
+	}
+
+	dev = i2c_slave_device_find(id);
+	if (!dev) {
+		ret = -ENODEV;
+		goto error;
+	}
+
+	if (i2c_slave_device_stop(dev)) {
+		ret = -EBUSY;
+		goto error;
+	}
+
+	ret = 0;
+
+      error:
+	return ret;
+}
+
+static const struct file_operations i2c_slave_fops = {
+	.owner = THIS_MODULE,
+	.llseek = no_llseek,
+	.read = i2c_slave_read,
+	.write = i2c_slave_write,
+	.ioctl = i2c_slave_ioctl,
+	.open = i2c_slave_open,
+	.release = i2c_slave_release,
+};
+
+static int i2c_slave_bus_match(struct device *dev, struct device_driver *drv)
+{
+	return 0;
+}
+
+/*
+static int i2c_slave_bus_probe(struct device *dev)
+{
+	struct device_driver *driver = dev->driver;
+
+	if (driver) {
+		return driver->probe(dev);
+	} else {
+		dev_err(dev, "%s: no driver\n", __func__);
+		return -ENODEV;
+	}
+}
+*/
+
+static int i2c_slave_bus_remove(struct device *dev)
+{
+	struct device_driver *driver = dev->driver;
+
+	if (driver) {
+		if (!driver->remove) {
+			return 0;
+		} else {
+			return driver->remove(dev);
+		}
+	} else {
+
+		dev_err(dev, "%s: no driver\n", __func__);
+		return -ENODEV;
+	}
+}
+
+static void i2c_slave_bus_shutdown(struct device *dev)
+{
+	struct device_driver *driver = dev->driver;
+
+	if (driver) {
+
+		driver->shutdown(dev);
+	} else {
+
+		dev_err(dev, "%s: no driver\n", __func__);
+		return;
+	}
+}
+static int i2c_slave_bus_suspend(struct device *dev, pm_message_t state)
+{
+	struct device_driver *driver = dev->driver;
+
+	if (driver) {
+
+		if (!driver->suspend) {
+			return 0;
+		} else {
+			return driver->suspend(dev, state);
+		}
+	} else {
+
+		dev_err(dev, "%s: no driver\n", __func__);
+		return -ENODEV;
+	}
+}
+
+static int i2c_slave_bus_resume(struct device *dev)
+{
+	struct device_driver *driver = dev->driver;
+
+	if (driver) {
+
+		if (!driver->resume) {
+			return 0;
+		} else {
+			return driver->resume(dev);
+		}
+	} else {
+
+		dev_err(dev, "%s: no driver\n", __func__);
+		return -ENODEV;
+	}
+}
+
+struct bus_type i2c_slave_bus_type = {
+	.name = "i2c-slave",
+	.match = i2c_slave_bus_match,
+	.remove = i2c_slave_bus_remove,
+	.shutdown = i2c_slave_bus_shutdown,
+	.suspend = i2c_slave_bus_suspend,
+	.resume = i2c_slave_bus_resume,
+};
+
+EXPORT_SYMBOL_GPL(i2c_slave_bus_type);
+
+static int i2c_slave_driver_probe(struct device *dev)
+{
+	return 0;
+}
+
+static int i2c_slave_driver_remove(struct device *dev)
+{
+	return 0;
+}
+
+static int i2c_slave_driver_shutdown(struct device *dev)
+{
+	return 0;
+}
+
+static int i2c_slave_driver_suspend(struct device *dev, pm_message_t state)
+{
+	return 0;
+}
+
+static int i2c_slave_driver_resume(struct device *dev)
+{
+	return 0;
+}
+
+extern struct class *i2c_slave_class;
+
+static struct device_driver i2c_slave_driver = {
+	.name = "i2c-slave",
+	.owner = THIS_MODULE,
+	.bus = &i2c_slave_bus_type,
+	.probe = i2c_slave_driver_probe,
+	.remove = i2c_slave_driver_remove,
+	.shutdown = i2c_slave_driver_shutdown,
+	.suspend = i2c_slave_driver_suspend,
+	.resume = i2c_slave_driver_resume,
+};
+
+static int __init i2c_slave_dev_init(void)
+{
+	int ret;
+
+	printk(KERN_INFO "i2c slave /dev entries driver\n");
+
+	ret = bus_register(&i2c_slave_bus_type);
+	if (ret) {
+		printk(KERN_ERR "%s: bus_register error\n", __func__);
+		goto out;
+	}
+
+	i2c_slave_class = class_create(THIS_MODULE, "i2c-slave");
+	if (IS_ERR(i2c_slave_class)) {
+		pr_err("%s: class_create error\n", __func__);
+		goto out_unreg_bus;
+	}
+
+	i2c_slave_major = register_chrdev(0, "i2c-slave", &i2c_slave_fops);
+	if (i2c_slave_major <= 0) {
+		pr_err("%s: register_chrdev error\n", __func__);
+		goto out_unreg_class;
+	}
+
+	ret = driver_register(&i2c_slave_driver);
+	if (ret) {
+		pr_err("%s: driver_register error\n", __func__);
+		goto out_unreg_chrdev;
+	}
+
+	return 0;
+
+      out_unreg_chrdev:
+	unregister_chrdev(i2c_slave_major, "i2c-slave");
+      out_unreg_class:
+	class_destroy(i2c_slave_class);
+      out_unreg_bus:
+	bus_unregister(&i2c_slave_bus_type);
+      out:
+	pr_err("%s: init error\n", __func__);
+	return ret;
+}
+
+static void __exit i2c_dev_exit(void)
+{
+	driver_unregister(&i2c_slave_driver);
+	class_destroy(i2c_slave_class);
+	unregister_chrdev(i2c_slave_major, "i2c-slave");
+	bus_unregister(&i2c_slave_bus_type);
+}
+
+module_init(i2c_slave_dev_init);
+module_exit(i2c_dev_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("I2C Slave Driver Core");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c-slave/i2c_slave_device.c b/drivers/i2c-slave/i2c_slave_device.c
new file mode 100644
index 000000000000..bfe39809928e
--- /dev/null
+++ b/drivers/i2c-slave/i2c_slave_device.c
@@ -0,0 +1,270 @@
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/major.h>
+#include <linux/mm.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/kdev_t.h>
+#include "i2c_slave_device.h"
+static i2c_slave_device_t *i2c_slave_devices[I2C_SLAVE_DEVICE_MAX];
+struct class *i2c_slave_class;
+static int i2c_slave_device_get_id(void)
+{
+	int i;
+	for (i = 0; i < I2C_SLAVE_DEVICE_MAX; i++) {
+		if (!i2c_slave_devices[i])
+			return i;
+	}
+	return -1;
+}
+
+i2c_slave_device_t *i2c_slave_device_find(int id)
+{
+	if (id >= 0 && id < I2C_SLAVE_DEVICE_MAX)
+		return i2c_slave_devices[id];
+
+	else
+		return NULL;
+}
+void i2c_slave_device_set_name(i2c_slave_device_t *device, char *name)
+{
+	device->name = name;
+}
+
+void i2c_slave_device_set_address(i2c_slave_device_t *device, u8 address)
+{
+	device->address = address;
+}
+
+u8 i2c_slave_device_get_addr(i2c_slave_device_t *device)
+{
+	return device->address;
+}
+
+int i2c_slave_device_set_freq(i2c_slave_device_t *device, u32 freq)
+{
+	/*TODO: freq check */
+	device->scl_freq = freq;
+	return 0;
+}
+
+u32 i2c_slave_device_get_freq(i2c_slave_device_t *device)
+{
+	return device->scl_freq;
+}
+
+/*used by the specific i2c device to register itself to the core.*/
+i2c_slave_device_t *i2c_slave_device_alloc(void)
+{
+	int id;
+	i2c_slave_device_t *device;
+	id = i2c_slave_device_get_id();
+	if (id < 0) {
+		goto error;
+	}
+	device =
+	    (i2c_slave_device_t *) kzalloc(sizeof(i2c_slave_device_t),
+					   GFP_KERNEL);
+	if (!device) {
+		printk(KERN_ERR "%s: alloc device error\n", __func__);
+		goto error_device;
+	}
+	device->receive_buffer = i2c_slave_rb_alloc(PAGE_SIZE);
+	if (!device->receive_buffer) {
+		printk(KERN_ERR "%s: alloc receive buffer error\n", __func__);
+		goto error_receive_buffer;
+	}
+	device->send_buffer = i2c_slave_rb_alloc(PAGE_SIZE);
+	if (!device->send_buffer) {
+		printk(KERN_ERR "%s: alloc send buffer error\n", __func__);
+		goto error_send_buffer;
+	}
+	device->id = id;
+	return device;
+
+      error_send_buffer:
+	kfree(device->receive_buffer);
+      error_receive_buffer:
+	kfree((void *)device);
+      error_device:
+	pr_debug(KERN_ERR "%s: no memory\n", __func__);
+      error:
+	return 0;
+}
+
+void i2c_slave_device_free(i2c_slave_device_t *dev)
+{
+	i2c_slave_rb_release(dev->receive_buffer);
+	i2c_slave_rb_release(dev->send_buffer);
+	kfree(dev);
+}
+
+int i2c_slave_device_register(i2c_slave_device_t *device)
+{
+	device->dev = device_create(i2c_slave_class, NULL,
+				    MKDEV(i2c_slave_major, device->id),
+				    NULL, "slave-i2c-%d", device->id);
+	if (!device->dev) {
+		return -1;
+	}
+	i2c_slave_devices[device->id] = device;
+	return 0;
+}
+
+void i2c_slave_device_unregister(i2c_slave_device_t *device)
+{
+	device_destroy(i2c_slave_class, MKDEV(i2c_slave_major, device->id));
+	i2c_slave_devices[device->id] = 0;
+	i2c_slave_device_free(device);
+}
+
+/*
+	this two functions are used by i2c slave core to start or stop the specific i2c device.
+*/
+int i2c_slave_device_start(i2c_slave_device_t *device)
+{
+	return device->start(device);
+}
+
+int i2c_slave_device_stop(i2c_slave_device_t *device)
+{
+	return device->stop(device);
+}
+
+/*
+	this two functions are used by i2c slave core to get data by the specific i2c slave device
+	or send data to it to feed i2c master's need.
+
+	@mod: async(1) or sync(0) mode.
+*/
+int i2c_slave_device_read(i2c_slave_device_t *device, int num, u8 *data)
+{
+	int read_num, read_total = 0;
+	int step = 1000;
+	u8 *read_buf = data;
+	printk(KERN_INFO "%s: device id=%d, num=%d\n", __func__, device->id,
+	       num);
+	read_num = i2c_slave_rb_consume(device->receive_buffer, num, read_buf);
+	read_total += read_num;
+	read_buf += read_num;
+	step--;
+	while ((read_total < num) && step) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(HZ / 10);
+		if (!signal_pending(current)) {
+		} else {
+			 /*TODO*/ break;
+		}
+		read_num =
+		    i2c_slave_rb_consume(device->receive_buffer,
+					 num - read_total, read_buf);
+		num -= read_num;
+		read_buf += read_num;
+		step--;
+	}
+	return read_total;
+}
+int i2c_slave_device_write(i2c_slave_device_t *device, int num, u8 *data)
+{
+	int write_num, write_total = 0;
+	int step = 1000;
+	u8 *buf_index = data;
+	write_num = i2c_slave_rb_produce(device->send_buffer, num, buf_index);
+	write_total += write_num;
+	buf_index += write_num;
+	step--;
+	while (write_total < num && step) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(HZ / 10);
+		if (!signal_pending(current)) {
+		} else {
+			 /*TODO*/ step = 0;
+			break;
+		}
+		write_num =
+		    i2c_slave_rb_produce(device->send_buffer, num - write_total,
+					 buf_index);
+		write_total += write_num;
+		buf_index += write_num;
+		step--;
+	}
+	while (step && i2c_slave_rb_num(device->send_buffer)) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(HZ / 10);
+		if (!signal_pending(current)) {
+			step--;
+		} else {
+			 /*TODO*/ step = 0;
+			break;
+		}
+	}
+	if (!step) {
+		write_total -= i2c_slave_rb_num(device->send_buffer);
+		i2c_slave_rb_clear(device->send_buffer);
+	}
+	return write_total;
+}
+
+/*
+ * this 2 functions used by the specific i2c slave device when they got data from master(produce),
+ * or is request by master(consume).
+ */
+int i2c_slave_device_produce(i2c_slave_device_t *device, int num, u8 *data)
+{
+	int ret;
+	ret = i2c_slave_rb_produce(device->receive_buffer, num, data);
+	return ret;
+}
+int i2c_slave_device_consume(i2c_slave_device_t *device, int num, u8 *data)
+{
+	return i2c_slave_rb_consume(device->send_buffer, num, data);
+}
+
+EXPORT_SYMBOL(i2c_slave_device_set_name);
+EXPORT_SYMBOL(i2c_slave_device_set_address);
+EXPORT_SYMBOL(i2c_slave_device_get_addr);
+EXPORT_SYMBOL(i2c_slave_device_find);
+EXPORT_SYMBOL(i2c_slave_device_set_freq);
+EXPORT_SYMBOL(i2c_slave_device_get_freq);
+
+/*
+* used by the specific i2c device to register itself to the core.
+*/
+EXPORT_SYMBOL(i2c_slave_device_alloc);
+EXPORT_SYMBOL(i2c_slave_device_free);
+EXPORT_SYMBOL(i2c_slave_device_register);
+EXPORT_SYMBOL(i2c_slave_device_unregister);
+
+/*
+	this two functions are used by i2c slave core to start or stop the specific i2c device.
+*/
+EXPORT_SYMBOL(i2c_slave_device_start);
+EXPORT_SYMBOL(i2c_slave_device_stop);
+
+/*
+	this two functions are used by i2c slave core to get data by the specific i2c slave device
+	or send data to it for it to feed i2c master's need.
+
+	@mod: async(1) or sync(0) mode.
+*/
+EXPORT_SYMBOL(i2c_slave_device_read);
+EXPORT_SYMBOL(i2c_slave_device_write);
+
+/*
+* this 2 functions used by the specific i2c slave device when they got data from master,
+* or is request by master.
+*/
+EXPORT_SYMBOL(i2c_slave_device_produce);
+EXPORT_SYMBOL(i2c_slave_device_consume);
diff --git a/drivers/i2c-slave/i2c_slave_device.h b/drivers/i2c-slave/i2c_slave_device.h
new file mode 100644
index 000000000000..e08ea569e030
--- /dev/null
+++ b/drivers/i2c-slave/i2c_slave_device.h
@@ -0,0 +1,79 @@
+/*
+ * Copyright 2007-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef __I2C_SLAVE_H__
+#define __I2C_SLAVE_H__
+
+#include <linux/list.h>
+#include "i2c_slave_ring_buffer.h"
+
+#define I2C_SLAVE_DEVICE_MAX 256
+extern int i2c_slave_major;
+
+typedef struct i2c_slave_device {
+	struct list_head list;
+	u8 address;
+	u32 scl_freq;
+	char *name;
+	i2c_slave_ring_buffer_t *receive_buffer;
+	i2c_slave_ring_buffer_t *send_buffer;
+	int (*start) (struct i2c_slave_device *);
+	int (*stop) (struct i2c_slave_device *);
+	/*int (*set_freq)(struct i2c_slave_device*);
+	int (*set_addr)(struct i2c_slave_device*);*/
+	void *private_data;
+	struct device *dev;
+	int id;
+} i2c_slave_device_t;
+
+/*
+	used by the specific device to set some infomations.
+*/
+void i2c_slave_device_set_name(i2c_slave_device_t *device, char *name);
+void i2c_slave_device_set_address(i2c_slave_device_t *device, u8 address);
+i2c_slave_device_t *i2c_slave_device_find(int id);
+u8 i2c_slave_device_get_addr(i2c_slave_device_t *device);
+int i2c_slave_device_set_freq(i2c_slave_device_t *device, u32 freq);
+u32 i2c_slave_device_get_freq(i2c_slave_device_t *device);
+
+/*
+**used by the specific i2c device to register itself to the core.
+*/
+i2c_slave_device_t *i2c_slave_device_alloc(void);
+void i2c_slave_device_free(i2c_slave_device_t *);
+int i2c_slave_device_register(i2c_slave_device_t *device);
+void i2c_slave_device_unregister(i2c_slave_device_t *device);
+
+/*
+	this two functions are used by i2c slave core to start or stop the specific i2c device.
+*/
+int i2c_slave_device_start(i2c_slave_device_t *device);
+int i2c_slave_device_stop(i2c_slave_device_t *device);
+
+/*
+	this two functions are used by i2c slave core to get data by the specific i2c slave device
+	or send data to it for it to feed i2c master's need.
+
+	@mod: async(1) or sync(0) mode.
+*/
+int i2c_slave_device_read(i2c_slave_device_t *device, int num, u8 *data);
+int i2c_slave_device_write(i2c_slave_device_t *device, int num, u8 *data);
+
+/*
+*this 2 functions used by the specific i2c slave device when they got data from master,
+*or is requested by master.
+*/
+int i2c_slave_device_produce(i2c_slave_device_t *device, int num, u8 *data);
+int i2c_slave_device_consume(i2c_slave_device_t *device, int num, u8 *data);
+
+#endif
diff --git a/drivers/i2c-slave/i2c_slave_ring_buffer.c b/drivers/i2c-slave/i2c_slave_ring_buffer.c
new file mode 100644
index 000000000000..4a55e099235f
--- /dev/null
+++ b/drivers/i2c-slave/i2c_slave_ring_buffer.c
@@ -0,0 +1,185 @@
+/*
+ * Copyright 2007-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/slab.h>
+#include <linux/hardirq.h>
+#include "i2c_slave_ring_buffer.h"
+
+i2c_slave_ring_buffer_t *i2c_slave_rb_alloc(int size)
+{
+	i2c_slave_ring_buffer_t *ring_buf;
+
+	ring_buf =
+	    (i2c_slave_ring_buffer_t *) kzalloc(sizeof(i2c_slave_ring_buffer_t),
+						GFP_KERNEL);
+	if (!ring_buf) {
+		pr_debug("%s: alloc ring_buf error\n", __func__);
+		goto error;
+	}
+
+	ring_buf->buffer = kmalloc(size, GFP_KERNEL);
+	if (!ring_buf->buffer) {
+		pr_debug("%s: alloc buffer error\n", __func__);
+		goto error1;
+	}
+
+	ring_buf->total = size;
+
+	ring_buf->lock = __SPIN_LOCK_UNLOCKED(ring_buf->lock);
+	return ring_buf;
+
+      error1:
+	kfree(ring_buf);
+      error:
+	return NULL;
+}
+
+void i2c_slave_rb_release(i2c_slave_ring_buffer_t *ring)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(ring->lock, flags);
+	kfree(ring->buffer);
+	spin_unlock_irqrestore(ring->lock, flags);
+	kfree(ring);
+}
+
+int i2c_slave_rb_produce(i2c_slave_ring_buffer_t *ring, int num, char *data)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(ring->lock, flags);
+
+	if (ring->start < ring->end) {
+		if ((ring->start + num) < ring->end) {	/*have enough space */
+			memcpy(&ring->buffer[ring->start], data, num);
+			ring->start += num;
+			ret = num;
+		} else {	/*space not enough, just copy part of it. */
+			ret = ring->end - ring->start;
+			memcpy(&ring->buffer[ring->start], data, ret);
+			ring->start += ret;
+			ring->full = 1;
+		}
+	} else if (ring->start >= ring->end && !ring->full) {
+		if (ring->start + num <= ring->total) {	/*space enough */
+			ret = num;
+			memcpy(&ring->buffer[ring->start], data, ret);
+			ring->start += ret;
+		} else {	/*turn ring->start around */
+			ret = ring->total - ring->start;
+			memcpy(&ring->buffer[ring->start], data, ret);
+			ring->start = 0;
+			num -= ret;
+			data += ret;
+			if (num < ring->end) {	/*space enough */
+				ret += num;
+				memcpy(&ring->buffer[ring->start], data, num);
+			} else {	/*space not enough, just copy part of it. */
+				ret += ring->end;
+				memcpy(&ring->buffer[ring->start], data,
+				       ring->end);
+				ring->start = ring->end;
+				ring->full = 1;
+			}
+		}
+	} else {		/*(ring->data == ring->end) && ring->full ) : full */
+		ret = 0;
+	}
+
+	spin_unlock_irqrestore(ring->lock, flags);
+	return ret;
+}
+
+int i2c_slave_rb_consume(i2c_slave_ring_buffer_t *ring, int num, char *data)
+{
+	int ret;
+	unsigned long flags;
+	spin_lock_irqsave(ring->lock, flags);
+	if (num <= 0) {
+		ret = 0;
+		goto out;
+	}
+
+	if (ring->start > ring->end) {
+		if (num <= ring->start - ring->end) {	/*enough */
+			ret = num;
+			memcpy(data, &ring->buffer[ring->end], ret);
+			ring->end += ret;
+		} else {	/*not enough */
+			ret = ring->start - ring->end;
+			memcpy(data, &ring->buffer[ring->end], ret);
+			ring->end += ret;
+		}
+	} else if (ring->start < ring->end || ring->full) {
+		if (num <= ring->total - ring->end) {
+			ret = ring->total - ring->end;
+			memcpy(data, &ring->buffer[ring->end], ret);
+			ring->end += ret;
+		} else if (num <= ring->total - ring->end + ring->start) {
+			ret = ring->total - ring->end;
+			memcpy(data, &ring->buffer[ring->end], ret);
+			ring->end = 0;
+			data += ret;
+			num -= ret;
+			memcpy(data, &ring->buffer[ring->end], num);
+			ring->end = num;
+			ret += num;
+		} else {
+			ret = ring->total - ring->end;
+			memcpy(data, &ring->buffer[ring->end], ret);
+			ring->end = 0;
+			data += ret;
+			num -= ret;
+			memcpy(data, &ring->buffer[ring->end], ring->start);
+			ring->end = ring->start;
+			ret += ring->start;
+		}
+		ring->full = 0;
+	} else {		/*empty */
+		ret = 0;
+	}
+
+      out:
+	spin_unlock_irqrestore(ring->lock, flags);
+
+	return ret;
+}
+
+int i2c_slave_rb_num(i2c_slave_ring_buffer_t *ring)
+{
+	int ret;
+	unsigned long flags;
+	spin_lock_irqsave(ring->lock, flags);
+	if (ring->start > ring->end) {
+		ret = ring->start - ring->end;
+	} else if (ring->start < ring->end) {
+		ret = ring->total - ring->end + ring->start;
+	} else if (ring->full) {
+		ret = ring->total;
+	} else {
+		ret = 0;
+	}
+	spin_unlock_irqrestore(ring->lock, flags);
+	return ret;
+}
+
+void i2c_slave_rb_clear(i2c_slave_ring_buffer_t *ring)
+{
+	unsigned long flags;
+	spin_lock_irqsave(ring->lock, flags);
+
+	ring->start = ring->end = 0;
+	ring->full = 0;
+	spin_unlock_irqrestore(ring->lock, flags);
+}
diff --git a/drivers/i2c-slave/i2c_slave_ring_buffer.h b/drivers/i2c-slave/i2c_slave_ring_buffer.h
new file mode 100644
index 000000000000..1068e5f1b527
--- /dev/null
+++ b/drivers/i2c-slave/i2c_slave_ring_buffer.h
@@ -0,0 +1,39 @@
+#ifndef __BUFFER_MANAGER_H__
+#define __BUFFER_MANAGER_H__
+/*
+ * Copyright 2007-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/types.h>
+#include <linux/spinlock.h>
+
+typedef struct i2c_slave_ring_buffer {
+	int start;
+	int end;
+	int total;
+	bool full;
+	char *buffer;
+	spinlock_t lock;
+} i2c_slave_ring_buffer_t;
+
+i2c_slave_ring_buffer_t *i2c_slave_rb_alloc(int size);
+
+void i2c_slave_rb_release(i2c_slave_ring_buffer_t *ring);
+
+int i2c_slave_rb_produce(i2c_slave_ring_buffer_t *ring, int num, char *data);
+
+int i2c_slave_rb_consume(i2c_slave_ring_buffer_t *ring, int num, char *data);
+
+int i2c_slave_rb_num(i2c_slave_ring_buffer_t *ring);
+
+void i2c_slave_rb_clear(i2c_slave_ring_buffer_t *ring);
+
+#endif
diff --git a/drivers/i2c-slave/mxc_i2c_slave.c b/drivers/i2c-slave/mxc_i2c_slave.c
new file mode 100644
index 000000000000..39e067303f6b
--- /dev/null
+++ b/drivers/i2c-slave/mxc_i2c_slave.c
@@ -0,0 +1,329 @@
+/*
+ * Copyright 2007-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <asm/io.h>
+#include <linux/pm.h>
+#include <linux/interrupt.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include "i2c_slave_device.h"
+#include "mxc_i2c_slave.h"
+#include "mxc_i2c_slave_reg.h"
+
+struct mxc_i2c_slave_clk {
+	int reg_value;
+	int div;
+};
+
+static const struct mxc_i2c_slave_clk i2c_clk_table[] = {
+	{0x20, 22}, {0x21, 24}, {0x22, 26}, {0x23, 28},
+	{0, 30}, {1, 32}, {0x24, 32}, {2, 36},
+	{0x25, 36}, {0x26, 40}, {3, 42}, {0x27, 44},
+	{4, 48}, {0x28, 48}, {5, 52}, {0x29, 56},
+	{6, 60}, {0x2A, 64}, {7, 72}, {0x2B, 72},
+	{8, 80}, {0x2C, 80}, {9, 88}, {0x2D, 96},
+	{0xA, 104}, {0x2E, 112}, {0xB, 128}, {0x2F, 128},
+	{0xC, 144}, {0xD, 160}, {0x30, 160}, {0xE, 192},
+	{0x31, 192}, {0x32, 224}, {0xF, 240}, {0x33, 256},
+	{0x10, 288}, {0x11, 320}, {0x34, 320}, {0x12, 384},
+	{0x35, 384}, {0x36, 448}, {0x13, 480}, {0x37, 512},
+	{0x14, 576}, {0x15, 640}, {0x38, 640}, {0x16, 768},
+	{0x39, 768}, {0x3A, 896}, {0x17, 960}, {0x3B, 1024},
+	{0x18, 1152}, {0x19, 1280}, {0x3C, 1280}, {0x1A, 1536},
+	{0x3D, 1536}, {0x3E, 1792}, {0x1B, 1920}, {0x3F, 2048},
+	{0x1C, 2304}, {0x1D, 2560}, {0x1E, 3072}, {0x1F, 3840},
+	{0, 0}
+};
+
+extern void gpio_i2c_active(int i2c_num);
+extern void gpio_i2c_inactive(int i2c_num);
+
+static irqreturn_t interrupt_handler(int irq, void *dev_id)
+{
+	u16 status, ctl;
+	int num;
+	u16 data;
+	struct mxc_i2c_slave_device *mxc_i2c =
+	    (struct mxc_i2c_slave_device *)dev_id;
+
+	status = readw(mxc_i2c->reg_base + MXC_I2SR);
+	ctl = readw(mxc_i2c->reg_base + MXC_I2CR);
+
+	dev_dbg(mxc_i2c->dev->dev, "status=%x, ctl=%x\n", status, ctl);
+
+	if (status & MXC_I2SR_IAAS) {
+		if (status & MXC_I2SR_SRW) {
+			/*slave send */
+			num =
+			    i2c_slave_device_consume(mxc_i2c->dev, 1,
+						     (u8 *) &data);
+			if (num < 1) {
+				/*FIXME:not ready to send data */
+				printk(KERN_ERR
+				       " i2c-slave:%s:data not ready\n",
+				       __func__);
+			} else {
+				ctl |= MXC_I2CR_MTX;
+				writew(ctl, mxc_i2c->reg_base + MXC_I2CR);
+				/*send data */
+				writew(data, mxc_i2c->reg_base + MXC_I2DR);
+			}
+
+		} else {
+			/*slave receive */
+			ctl &= ~MXC_I2CR_MTX;
+			writew(ctl, mxc_i2c->reg_base + MXC_I2CR);
+
+			/*dummy read */
+			data = readw(mxc_i2c->reg_base + MXC_I2DR);
+		}
+	} else {
+		/*slave send */
+		if (ctl & MXC_I2CR_MTX) {
+			if (!(status & MXC_I2SR_RXAK)) {	/*ACK received */
+				num =
+				    i2c_slave_device_consume(mxc_i2c->dev, 1,
+							     (u8 *) &data);
+				if (num < 1) {
+					/*FIXME:not ready to send data */
+					printk(KERN_ERR
+					       " i2c-slave:%s:data not ready\n",
+					       __func__);
+				} else {
+					ctl |= MXC_I2CR_MTX;
+					writew(ctl,
+					       mxc_i2c->reg_base + MXC_I2CR);
+					writew(data,
+					       mxc_i2c->reg_base + MXC_I2DR);
+				}
+			} else {
+				/*no ACK. */
+				/*dummy read */
+				ctl &= ~MXC_I2CR_MTX;
+				writew(ctl, mxc_i2c->reg_base + MXC_I2CR);
+				data = readw(mxc_i2c->reg_base + MXC_I2DR);
+			}
+		} else {	/*read */
+			ctl &= ~MXC_I2CR_MTX;
+			writew(ctl, mxc_i2c->reg_base + MXC_I2CR);
+
+			/*read */
+			data = readw(mxc_i2c->reg_base + MXC_I2DR);
+			i2c_slave_device_produce(mxc_i2c->dev, 1,
+						 (u8 *) &data);
+		}
+
+	}
+
+	writew(0x0, mxc_i2c->reg_base + MXC_I2SR);
+
+	return IRQ_HANDLED;
+}
+
+static int start(i2c_slave_device_t *device)
+{
+	volatile unsigned int cr;
+	unsigned int addr;
+	struct mxc_i2c_slave_device *mxc_dev;
+
+	mxc_dev = (struct mxc_i2c_slave_device *)device->private_data;
+	if (!mxc_dev) {
+		dev_err(device->dev, "%s: get mxc_dev error\n", __func__);
+		return -ENODEV;
+	}
+
+	clk_enable(mxc_dev->clk);
+	/* Set the frequency divider */
+	writew(mxc_dev->clkdiv, mxc_dev->reg_base + MXC_IFDR);
+
+	/* Set the Slave bit */
+	cr = readw(mxc_dev->reg_base + MXC_I2CR);
+	cr &= (!MXC_I2CR_MSTA);
+	writew(cr, mxc_dev->reg_base + MXC_I2CR);
+
+	/*Set Slave Address */
+	addr = mxc_dev->dev->address << 1;
+	writew(addr, mxc_dev->reg_base + MXC_IADR);
+
+	/* Clear the status register */
+	writew(0x0, mxc_dev->reg_base + MXC_I2SR);
+
+	/* Enable I2C and its interrupts */
+	writew(MXC_I2CR_IEN, mxc_dev->reg_base + MXC_I2CR);
+	writew(MXC_I2CR_IEN | MXC_I2CR_IIEN, mxc_dev->reg_base + MXC_I2CR);
+
+	return 0;
+
+}
+
+static int stop(i2c_slave_device_t *device)
+{
+	struct mxc_i2c_slave_device *mxc_dev;
+
+	mxc_dev = (struct mxc_i2c_slave_device *)device->private_data;
+
+	writew(0x0, mxc_dev->reg_base + MXC_I2CR);
+	clk_disable(mxc_dev->clk);
+
+	return 0;
+}
+
+static int mxc_i2c_slave_probe(struct platform_device *pdev)
+{
+	int i;
+	u32 clk_freq;
+	struct resource *res;
+	struct mxc_i2c_slave_device *mxc_dev;
+
+	mxc_dev = kzalloc(sizeof(struct mxc_i2c_slave_device), GFP_KERNEL);
+	if (!mxc_dev) {
+		goto error0;
+	}
+	mxc_dev->dev = i2c_slave_device_alloc();
+	if (mxc_dev->dev == 0) {
+		dev_err(&pdev->dev, "%s: i2c_slave_device_alloc error\n",
+			__func__);
+		goto error1;
+	}
+
+	i2c_slave_device_set_address(mxc_dev->dev, MXC_I2C_SLAVE_ADDRESS);
+	i2c_slave_device_set_freq(mxc_dev->dev, MXC_I2C_SLAVE_FREQ);
+	i2c_slave_device_set_name(mxc_dev->dev, MXC_I2C_SLAVE_NAME);
+	mxc_dev->dev->start = start;
+	mxc_dev->dev->stop = stop;
+
+	mxc_dev->dev->private_data = mxc_dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "%s: get resource error\n", __func__);
+		goto error2;
+	}
+	mxc_dev->reg_base = IO_ADDRESS(res->start);
+
+	mxc_dev->irq = platform_get_irq(pdev, 0);
+	if (mxc_dev->irq < 0) {
+		dev_err(&pdev->dev, "%s: get irq error\n", __func__);
+		goto error2;
+	}
+	if (request_irq(mxc_dev->irq, interrupt_handler,
+			0, mxc_dev->dev->name, mxc_dev) < 0) {
+		dev_err(&pdev->dev, "%s: request_irq error\n", __func__);
+		goto error2;
+	}
+
+	/*i2c id on soc */
+	mxc_dev->id = pdev->id;
+
+	gpio_i2c_active(mxc_dev->id);
+
+	/*clock */
+	mxc_dev->clk = clk_get(&pdev->dev, "i2c_clk");
+	clk_freq = clk_get_rate(mxc_dev->clk);
+	mxc_dev->clkdiv = -1;
+	if (mxc_dev->dev->scl_freq) {
+		/* Calculate divider and round up any fractional part */
+		int div = (clk_freq + mxc_dev->dev->scl_freq - 1) /
+		    mxc_dev->dev->scl_freq;
+		for (i = 0; i2c_clk_table[i].div != 0; i++) {
+			if (i2c_clk_table[i].div >= div) {
+				mxc_dev->clkdiv = i2c_clk_table[i].reg_value;
+				break;
+			}
+		}
+	}
+	if (mxc_dev->clkdiv == -1) {
+		i--;
+		mxc_dev->clkdiv = 0x1F;	/* Use max divider */
+	}
+	dev_dbg(&pdev->dev,
+		"i2c slave speed is %d/%d = %d bps, reg val = 0x%02X\n",
+		clk_freq, i2c_clk_table[i].div, clk_freq / i2c_clk_table[i].div,
+		mxc_dev->clkdiv);
+
+	if (i2c_slave_device_register(mxc_dev->dev) < 0) {
+		dev_err(&pdev->dev, "%s: i2c_slave_device_register error\n",
+			__func__);
+		goto error3;
+	}
+
+	platform_set_drvdata(pdev, (void *)mxc_dev);
+
+	/*start(mxc_dev->dev); */
+	return 0;
+
+      error3:
+      error2:
+	i2c_slave_device_free(mxc_dev->dev);
+      error1:
+	kfree(mxc_dev);
+      error0:
+	return -ENODEV;
+}
+
+static int mxc_i2c_slave_remove(struct platform_device *pdev)
+{
+	struct mxc_i2c_slave_device *mxc_dev;
+	mxc_dev = (struct mxc_i2c_slave_device *)platform_get_drvdata(pdev);
+
+	i2c_slave_device_unregister(mxc_dev->dev);
+	kfree((void *)mxc_dev);
+
+	return 0;
+}
+
+static int mxc_i2c_slave_suspend(struct platform_device *pdev,
+				 pm_message_t state)
+{
+	return 0;
+}
+
+static int mxc_i2c_slave_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver mxci2c_slave_driver = {
+	.driver = {
+		   .name = "mxc_i2c_slave",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = mxc_i2c_slave_probe,
+	.remove = mxc_i2c_slave_remove,
+	.suspend = mxc_i2c_slave_suspend,
+	.resume = mxc_i2c_slave_resume,
+};
+
+static int __init mxc_i2c_slave_init(void)
+{
+	/* Register the device driver structure. */
+	return platform_driver_register(&mxci2c_slave_driver);
+}
+
+/*!
+ * This function is used to cleanup all resources before the driver exits.
+ */
+static void __exit mxc_i2c_slave_exit(void)
+{
+	platform_driver_unregister(&mxci2c_slave_driver);
+}
+
+module_init(mxc_i2c_slave_init);
+module_exit(mxc_i2c_slave_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC I2C Slave Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c-slave/mxc_i2c_slave.h b/drivers/i2c-slave/mxc_i2c_slave.h
new file mode 100644
index 000000000000..7b8d5143588e
--- /dev/null
+++ b/drivers/i2c-slave/mxc_i2c_slave.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright 2007-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef __MXC_I2C_SLAVE_H__
+#define __MXC_I2C_SLAVE_H__
+
+#include <linux/clk.h>
+#include "i2c_slave_device.h"
+
+#define MXC_I2C_SLAVE_NAME "MXC_I2C_SLAVE"
+#define MXC_I2C_SLAVE_ADDRESS 0x55
+#define MXC_I2C_SLAVE_FREQ 1000*100
+
+struct mxc_i2c_slave_device {
+	/*!
+	 * The default clock divider value to be used.
+	 */
+	unsigned int clkdiv;
+
+	/*!
+	 * The clock source for the device.
+	 */
+	struct clk *clk;
+
+	/*i2c id on soc */
+	int id;
+
+	int irq;
+	unsigned long reg_base;
+	bool state;		/*0:stop, 1:start */
+	i2c_slave_device_t *dev;
+};
+
+#endif
diff --git a/drivers/i2c-slave/mxc_i2c_slave_reg.h b/drivers/i2c-slave/mxc_i2c_slave_reg.h
new file mode 100644
index 000000000000..6450ad6e3db9
--- /dev/null
+++ b/drivers/i2c-slave/mxc_i2c_slave_reg.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright 2007-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef __MXC_I2C_SLAVE_REG_H__
+#define __MXC_I2C_SLAVE_REG_H__
+
+/* Address offsets of the I2C registers */
+#define MXC_IADR                0x00	/* Address Register */
+#define MXC_IFDR                0x04	/* Freq div register */
+#define MXC_I2CR                0x08	/* Control regsiter */
+#define MXC_I2SR                0x0C	/* Status register */
+#define MXC_I2DR                0x10	/* Data I/O register */
+
+/* Bit definitions of I2CR */
+#define MXC_I2CR_IEN            0x0080
+#define MXC_I2CR_IIEN           0x0040
+#define MXC_I2CR_MSTA           0x0020
+#define MXC_I2CR_MTX            0x0010
+#define MXC_I2CR_TXAK           0x0008
+#define MXC_I2CR_RSTA           0x0004
+
+/* Bit definitions of I2SR */
+#define MXC_I2SR_ICF            0x0080
+#define MXC_I2SR_IAAS           0x0040
+#define MXC_I2SR_IBB            0x0020
+#define MXC_I2SR_IAL            0x0010
+#define MXC_I2SR_SRW            0x0004
+#define MXC_I2SR_IIF            0x0002
+#define MXC_I2SR_RXAK           0x0001
+
+#endif /* __MXC_I2C_REG_H__ */
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index 7f95905bbb9d..0d9ad8e73026 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -160,6 +160,16 @@ config I2C_NFORCE2_S4985
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-nforce2-s4985.
 
+config I2C_NS9XXX
+	tristate "Digi ns9360, ns921x"
+	depends on PROCESSOR_NS9360 || PROCESSOR_NS921X
+	help
+	  If you say yes to this option, support will be included for the
+	  I2C interface included in Digi ns9360 and ns921x CPUs.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-ns9xxx.
+
 config I2C_SIS5595
 	tristate "SiS 5595"
 	depends on PCI
@@ -328,6 +338,10 @@ config I2C_DAVINCI
 config I2C_GPIO
 	tristate "GPIO-based bitbanging I2C"
 	depends on GENERIC_GPIO
+
+config I2C_PARPORT
+	tristate "Parallel port adapter"
+	depends on PARPORT
 	select I2C_ALGOBIT
 	help
 	  This is a very simple bitbanging I2C driver utilizing the
@@ -391,9 +405,34 @@ config I2C_MPC
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-mpc.
 
+config I2C_MXC
+	tristate "MXC I2C support"
+	depends on I2C && ARCH_MXC
+	help
+	  If you say yes to this option, support will be included for Freescale
+	  MXC I2C modules.
+
+	  This driver can also be built as a module.
+
+config I2C_MXC_HS
+	tristate "MXC HIGH SPEED I2C support"
+	depends on I2C && ARCH_MXC
+	help
+	  If you say yes to this option, support will be included for Freescale
+	  MXC HIGH SPEED I2C modules.
+
+	  This driver can also be built as a module.
+
+config I2C_STMP378X
+	tristate "STMP378x I2C adapter"
+	depends on MACH_STMP378X
+	help
+	  TBD
+
 config I2C_MV64XXX
 	tristate "Marvell mv64xxx I2C Controller"
 	depends on (MV64X60 || PLAT_ORION) && EXPERIMENTAL
+
 	help
 	  If you say yes to this option, support will be included for the
 	  built-in I2C interface on the Marvell 64xxx line of host bridges.
@@ -461,6 +500,19 @@ config I2C_S3C2410
 	  Say Y here to include support for I2C controller in the
 	  Samsung S3C2410 based System-on-Chip devices.
 
+config I2C_S3C2410_ADAPTER_NR
+        int "S3C bus number for the I2C-adapter"
+        depends on I2C_S3C2410
+        default 1
+        help
+          This is the adapter/bus number for the I2C-adapter. This number can
+          be used by pre-been declared I2C-devices that will be probed
+          first when the I2C-bus driver is loaded.
+          For declaring this kind of static I2C-devices use the function
+          i2c_register_board_info (see drivers/i2c/i2c-core.c).
+
+
+
 config I2C_SH7760
 	tristate "Renesas SH7760 I2C Controller"
 	depends on CPU_SUBTYPE_SH7760
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index 0c2c4b26cdf1..2e0c0302c40c 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_I2C_I801)		+= i2c-i801.o
 obj-$(CONFIG_I2C_ISCH)		+= i2c-isch.o
 obj-$(CONFIG_I2C_NFORCE2)	+= i2c-nforce2.o
 obj-$(CONFIG_I2C_NFORCE2_S4985)	+= i2c-nforce2-s4985.o
+obj-$(CONFIG_I2C_NS9XXX)	+= i2c-ns9xxx.o
 obj-$(CONFIG_I2C_PIIX4)		+= i2c-piix4.o
 obj-$(CONFIG_I2C_SIS5595)	+= i2c-sis5595.o
 obj-$(CONFIG_I2C_SIS630)	+= i2c-sis630.o
@@ -67,6 +68,9 @@ obj-$(CONFIG_I2C_SIBYTE)	+= i2c-sibyte.o
 obj-$(CONFIG_I2C_STUB)		+= i2c-stub.o
 obj-$(CONFIG_SCx200_ACB)	+= scx200_acb.o
 obj-$(CONFIG_SCx200_I2C)	+= scx200_i2c.o
+obj-$(CONFIG_I2C_MXC)		+= mxc_i2c.o
+obj-$(CONFIG_I2C_MXC_HS)	+= mxc_i2c_hs.o
+obj-$(CONFIG_I2C_STMP378X)	+= i2c-stmp378x.o
 
 ifeq ($(CONFIG_I2C_DEBUG_BUS),y)
 EXTRA_CFLAGS += -DDEBUG
diff --git a/drivers/i2c/busses/i2c-ns9xxx.c b/drivers/i2c/busses/i2c-ns9xxx.c
new file mode 100644
index 000000000000..0b6d1eb3239b
--- /dev/null
+++ b/drivers/i2c/busses/i2c-ns9xxx.c
@@ -0,0 +1,558 @@
+/*
+ * linux/drivers/i2c/busses/i2c-ns9xxx.c
+ *
+ * based on old i2c-ns9xxx.c by Digi International Inc.
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/i2c-ns9xxx.h>
+#include <linux/platform_device.h>
+
+#include <asm/gpio.h>
+#include <asm/io.h>
+
+/* registers */
+#define I2C_CMD				0x00
+#define I2C_STATUS			0x00
+#define I2C_MASTERADDR			0x04
+#define I2C_SLAVEADDR			0x08
+#define I2C_CONFIG			0x0c
+
+/* command bit fields */
+#define I2C_CMD_TXVAL			(1 << 13)
+#define I2C_MASTERADDR_7BIT		0
+#define I2C_MASTERADDR_10BIT		1
+
+/* configuration masks */
+#define I2C_CONFIG_CLREFMASK		0x000001ff
+#define I2C_MASTERADDR_ADDRMASK		0x000007ff
+
+/* configuration shifts */
+#define I2C_MASTERADDR_ADDRSHIFT	1
+
+/* shifted i2c commands */
+#define I2C_CMD_NOP			0
+#define I2C_CMD_READ			(4 << 8)
+#define I2C_CMD_WRITE			(5 << 8)
+#define I2C_CMD_STOP			(6 << 8)
+
+/* interrupt causes */
+#define I2C_IRQ_MASK			(0xf << 8)
+#define I2C_IRQ_ARBITLOST		(1 << 8)
+#define I2C_IRQ_NOACK			(2 << 8)
+#define I2C_IRQ_TXDATA			(3 << 8)
+#define I2C_IRQ_RXDATA			(4 << 8)
+#define I2C_IRQ_CMDACK			(5 << 8)
+
+#define I2C_NORMALSPEED			100000
+#define I2C_HIGHSPEED			400000
+
+#define DRIVER_NAME			"i2c-ns9xxx"
+
+enum i2c_int_state {
+	I2C_INT_AWAITING,
+	I2C_INT_OK,
+	I2C_INT_RETRY,
+	I2C_INT_ERROR,
+	I2C_INT_ABORT
+};
+
+struct ns9xxx_i2c {
+	struct i2c_adapter	adap;
+	struct resource		*mem;
+	struct clk		*clk;
+
+	void __iomem		*ioaddr;
+
+	spinlock_t		lock;
+	wait_queue_head_t	wait_q;
+
+	struct plat_ns9xxx_i2c	*pdata;
+
+	char			*buf;
+	int			irq;
+	enum i2c_int_state	state;
+};
+
+static int ns9xxx_i2c_xfer(struct i2c_adapter *adap,
+		struct i2c_msg msgs[], int num);
+
+static u32 ns9xxx_i2c_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_10BIT_ADDR
+		| I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE
+		| I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA;
+}
+
+static struct i2c_algorithm ns9xxx_i2c_algo = {
+	.master_xfer	= ns9xxx_i2c_xfer,
+	.functionality	= ns9xxx_i2c_func,
+};
+
+static irqreturn_t ns9xxx_i2c_irq(int irqnr, void *dev_id)
+{
+	struct ns9xxx_i2c *dev_data = (struct ns9xxx_i2c *)dev_id;
+	unsigned int status;
+
+	/* acknowledge by reading */
+	status = readl(dev_data->ioaddr + I2C_CMD);
+
+	if (dev_data->state != I2C_INT_AWAITING)
+		return IRQ_HANDLED;
+
+	switch (status & I2C_IRQ_MASK) {
+	case I2C_IRQ_RXDATA:
+		spin_lock(&dev_data->lock);
+		if (dev_data->buf)
+			*dev_data->buf = status & 0xff;
+		spin_unlock(&dev_data->lock);
+	case I2C_IRQ_CMDACK:
+	case I2C_IRQ_TXDATA:
+		dev_data->state = I2C_INT_OK;
+		break;
+	case I2C_IRQ_NOACK:
+		writel(I2C_CMD_STOP, dev_data->ioaddr + I2C_CMD);
+		dev_data->state = I2C_INT_ABORT;
+		break;
+	case I2C_IRQ_ARBITLOST:
+		dev_data->state = I2C_INT_RETRY;
+		break;
+	default:
+		dev_data->state = I2C_INT_ERROR;
+	}
+
+	wake_up_interruptible(&dev_data->wait_q);
+
+	return IRQ_HANDLED;
+}
+
+static int ns9xxx_i2c_send_cmd(struct ns9xxx_i2c *dev_data, unsigned int cmd)
+{
+	dev_data->state = I2C_INT_AWAITING;
+	do {
+		writel(cmd, dev_data->ioaddr + I2C_CMD);
+		if (!wait_event_interruptible_timeout(dev_data->wait_q,
+					dev_data->state != I2C_INT_AWAITING,
+					dev_data->adap.timeout))
+			return -ETIMEDOUT;
+	} while (dev_data->state == I2C_INT_AWAITING);
+
+	if (dev_data->state != I2C_INT_OK)
+		return -EIO;
+
+	return 0;
+}
+
+static int ns9xxx_i2c_read(struct ns9xxx_i2c *dev_data, int count)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	while (count-- > 1) {
+		spin_lock_irqsave(&dev_data->lock, flags);
+		dev_data->buf++;
+		spin_unlock_irqrestore(&dev_data->lock, flags);
+
+		ret = ns9xxx_i2c_send_cmd(dev_data, I2C_CMD_NOP);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+static int ns9xxx_i2c_write(struct ns9xxx_i2c *dev_data,
+		const char *buf, int count)
+{
+	int ret = 0;
+
+	while (count--) {
+		ret = ns9xxx_i2c_send_cmd(dev_data,
+				I2C_CMD_NOP | I2C_CMD_TXVAL | *buf);
+		if (ret)
+			break;
+		buf++;
+	}
+
+	return ret;
+}
+
+static int ns9xxx_i2c_bitbang(struct ns9xxx_i2c *dev_data, struct i2c_msg *msg)
+{
+	int i, nr_bits, ret;
+
+	gpio_direction_output(dev_data->pdata->gpio_sda, 1);
+	gpio_direction_output(dev_data->pdata->gpio_scl, 1);
+	mdelay(10);
+
+	/* start */
+	gpio_set_value(dev_data->pdata->gpio_sda, 0);
+	mdelay(1);
+	gpio_set_value(dev_data->pdata->gpio_scl, 0);
+	mdelay(1);
+
+	nr_bits = (msg->flags & I2C_M_TEN) ? 10 : 7;
+	for (i = 0; i < nr_bits; i++) {
+		/* set data */
+		if (msg->addr & (1 << (nr_bits - i - 1)))
+			gpio_set_value(dev_data->pdata->gpio_sda, 1);
+		else
+			gpio_set_value(dev_data->pdata->gpio_sda, 1);
+		mdelay(1);
+
+		/* toggle clock */
+		gpio_set_value(dev_data->pdata->gpio_scl, 1);
+		mdelay(1);
+		gpio_set_value(dev_data->pdata->gpio_scl, 0);
+		mdelay(1);
+	}
+
+	/* read ack */
+	gpio_direction_input(dev_data->pdata->gpio_sda);
+	gpio_set_value(dev_data->pdata->gpio_scl, 1);
+	mdelay(1);
+	ret = gpio_get_value(dev_data->pdata->gpio_sda);
+
+	/* stop */
+	gpio_direction_output(dev_data->pdata->gpio_sda, 1);
+
+	return ret ? 0 : -ENODEV;
+}
+
+static int ns9xxx_i2c_xfer(struct i2c_adapter *adap,
+		struct i2c_msg msgs[], int num)
+{
+	struct ns9xxx_i2c *dev_data = (struct ns9xxx_i2c *)adap->algo_data;
+	int len, i, ret = 0, retry = 10;
+	unsigned long flags = 0;
+	unsigned int cmd, reg;
+	char *buf = NULL;
+
+	dev_data->state = I2C_INT_OK;
+
+	for (i = 0; i < num; i++) {
+		if (dev_data->state == I2C_INT_RETRY) {
+			ret = ns9xxx_i2c_send_cmd(dev_data, I2C_CMD_STOP);
+			--retry;
+			if (ret || !retry)
+				return -EIO;
+		}
+
+		len = msgs[i].len;
+		buf = msgs[i].buf;
+
+		spin_lock_irqsave(&dev_data->lock, flags);
+		dev_data->buf = buf;
+		spin_unlock_irqrestore(&dev_data->lock, flags);
+
+		if (msgs[i].len == 0) {
+			/* send using use bitbang mode */
+			ret = ns9xxx_i2c_bitbang(dev_data, &msgs[i]);
+			/* reset gpios to hardware i2c */
+			dev_data->pdata->gpio_configuration_func();
+		} else {
+			if (!(msgs[i].flags & I2C_M_NOSTART)) {
+				/* set device address */
+				reg = ((msgs[i].addr & I2C_MASTERADDR_ADDRMASK)
+						<< I2C_MASTERADDR_ADDRSHIFT);
+
+				if (msgs[i].flags & I2C_M_TEN)
+					reg |= I2C_MASTERADDR_10BIT;
+				else
+					reg |= I2C_MASTERADDR_7BIT;
+
+				writel(reg, dev_data->ioaddr +
+						I2C_MASTERADDR);
+
+				if (msgs[i].flags & I2C_M_RD)
+					cmd = I2C_CMD_READ;
+				else {
+					cmd = I2C_CMD_WRITE | I2C_CMD_TXVAL;
+					cmd |= *buf;
+					len--;
+					buf++;
+				}
+
+				ret = ns9xxx_i2c_send_cmd(dev_data, cmd);
+				if (ret) {
+					if (dev_data->state == I2C_INT_RETRY) {
+						i = 0;
+						continue;
+					}
+					break;
+				}
+			}
+
+			if (msgs[i].flags & I2C_M_RD)
+				ret = ns9xxx_i2c_read(dev_data, len);
+			else
+				ret = ns9xxx_i2c_write(dev_data, buf, len);
+			if (ret) {
+				if (dev_data->state == I2C_INT_RETRY) {
+					i = 0;
+					continue;
+				}
+				break;
+			}
+		}
+	}
+
+	if (ns9xxx_i2c_send_cmd(dev_data, I2C_CMD_STOP)) {
+		/* sometimes interface gets stucked
+		 * try to fix this by send "start, nop, start" */
+		ns9xxx_i2c_send_cmd(dev_data, I2C_CMD_NOP);
+		ns9xxx_i2c_send_cmd(dev_data, I2C_CMD_STOP);
+	}
+
+	spin_lock_irqsave(&dev_data->lock, flags);
+	dev_data->buf = NULL;
+	spin_unlock_irqrestore(&dev_data->lock, flags);
+
+	/* return ERROR or number of transmits */
+	return ((ret < 0) ? ret : i);
+}
+
+static int ns9xxx_i2c_set_clock(struct ns9xxx_i2c *dev_data, unsigned int freq)
+{
+	u32 config;
+
+	config = readl(dev_data->ioaddr + I2C_CONFIG) & ~I2C_CONFIG_CLREFMASK;
+
+	switch (freq) {
+	case I2C_NORMALSPEED:
+		config |= (((clk_get_rate(dev_data->clk) / (4 * freq))
+				- 4 - 3) / 2) & I2C_CONFIG_CLREFMASK;
+		break;
+	case I2C_HIGHSPEED:
+		config |= (((clk_get_rate(dev_data->clk) / (4 * freq))
+				- 4 - 24) * 2 / 3) & I2C_CONFIG_CLREFMASK;
+		break;
+	default:
+		pr_warning(DRIVER_NAME ": wrong clock configuration,"
+				" i2c won't work!\n");
+		return -EINVAL;
+	}
+
+	writel(config, dev_data->ioaddr + I2C_CONFIG);
+
+	return 0;
+}
+
+static int __devinit ns9xxx_i2c_probe(struct platform_device *pdev)
+{
+	struct ns9xxx_i2c *dev_data;
+	int ret;
+
+	dev_data = kzalloc(sizeof(*dev_data), GFP_KERNEL);
+	if (!dev_data) {
+		dev_dbg(&pdev->dev, "%s: err_alloc_dd\n", __func__);
+		ret = -ENOMEM;
+		goto err_alloc_dd;
+	}
+	platform_set_drvdata(pdev, dev_data);
+
+	dev_data->pdata = pdev->dev.platform_data;
+	if (!dev_data->pdata) {
+		dev_dbg(&pdev->dev, "%s: err_pdata\n", __func__);
+		ret = -ENOENT;
+		goto err_pdata;
+	}
+
+	snprintf(dev_data->adap.name, ARRAY_SIZE(dev_data->adap.name),
+			DRIVER_NAME);
+	dev_data->adap.owner = THIS_MODULE;
+	dev_data->adap.algo = &ns9xxx_i2c_algo;
+	dev_data->adap.algo_data = dev_data;
+	dev_data->adap.retries = 1;
+	dev_data->adap.timeout = HZ / 10;
+	dev_data->adap.class = I2C_CLASS_HWMON;
+	dev_data->buf = NULL;
+
+	spin_lock_init(&dev_data->lock);
+	init_waitqueue_head(&dev_data->wait_q);
+
+	dev_data->irq = platform_get_irq(pdev, 0);
+	if (dev_data->irq <= 0) {
+		dev_dbg(&pdev->dev, "%s: err_irq\n", __func__);
+		ret = -ENOENT;
+		goto err_irq;
+	}
+
+	dev_data->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!dev_data->mem) {
+		dev_dbg(&pdev->dev, "%s: err_mem\n", __func__);
+		ret = -ENOENT;
+		goto err_mem;
+	}
+
+	if (!request_mem_region(dev_data->mem->start,
+				dev_data->mem->end - dev_data->mem->start + 1,
+				DRIVER_NAME)) {
+		dev_dbg(&pdev->dev, "%s: err_req_mem\n", __func__);
+		ret = -EBUSY;
+		goto err_req_mem;
+	}
+
+	dev_data->ioaddr = ioremap(dev_data->mem->start,
+			dev_data->mem->end - dev_data->mem->start + 1);
+	if (dev_data->ioaddr <= 0) {
+		dev_dbg(&pdev->dev, "%s: err_map_mem\n", __func__);
+		ret = -EBUSY;
+		goto err_map_mem;
+	} else
+		dev_dbg(&pdev->dev, "mapped I2C interface to virtual address"
+				"0x%x\n", (int)dev_data->ioaddr);
+
+	if (gpio_request(dev_data->pdata->gpio_scl, DRIVER_NAME)) {
+		dev_dbg(&pdev->dev, "%s: err_gpio_scl\n", __func__);
+		ret = -EBUSY;
+		goto err_gpio_scl;
+	}
+
+	if (gpio_request(dev_data->pdata->gpio_sda, DRIVER_NAME)) {
+		dev_dbg(&pdev->dev, "%s: err_gpio_sda\n", __func__);
+		ret = -EBUSY;
+		goto err_gpio_sda;
+	}
+
+	dev_data->clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(dev_data->clk)) {
+		dev_dbg(&pdev->dev, "%s: err_clk_get\n", __func__);
+		ret = PTR_ERR(dev_data->clk);
+		goto err_clk_get;
+	}
+
+	ret = clk_enable(dev_data->clk);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_clk_enable\n", __func__);
+		goto err_clk_enable;
+	}
+
+	/* configure i2c interface */
+	if (!dev_data->pdata->gpio_configuration_func) {
+		dev_dbg(&pdev->dev, "%s: err_cfg_gpio\n", __func__);
+		ret = -ENOENT;
+		goto err_cfg_gpio;
+	}
+	dev_data->pdata->gpio_configuration_func();
+
+	/* Set spike filter width to maximum value to workaround communication
+	 * problems on the cc9p9360 module */
+	writel(readl(dev_data->ioaddr + I2C_CONFIG) | (0xf << 9),
+	       dev_data->ioaddr + I2C_CONFIG);
+
+	if (dev_data->pdata->speed)
+		ret = ns9xxx_i2c_set_clock(dev_data, dev_data->pdata->speed);
+	else
+		ret = ns9xxx_i2c_set_clock(dev_data, I2C_NORMALSPEED);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_set_clk\n", __func__);
+		goto err_set_clk;
+	}
+
+	ret = request_irq(dev_data->irq, ns9xxx_i2c_irq, 0,
+			DRIVER_NAME, dev_data);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_req_irq\n", __func__);
+		ret = -EBUSY;
+		goto err_req_irq;
+	}
+
+	ret = i2c_add_numbered_adapter(&dev_data->adap);
+	if (ret < 0) {
+		dev_dbg(&pdev->dev, "%s: err_add_adap\n", __func__);
+		goto err_add_adap;
+	}
+
+	dev_info(&pdev->dev, "NS9XXX I2C adapter\n");
+
+	return 0;
+
+err_add_adap:
+	free_irq(dev_data->irq, dev_data);
+err_req_irq:
+err_set_clk:
+err_cfg_gpio:
+	clk_disable(dev_data->clk);
+err_clk_enable:
+	clk_put(dev_data->clk);
+err_clk_get:
+	gpio_free(dev_data->pdata->gpio_sda);
+err_gpio_sda:
+	gpio_free(dev_data->pdata->gpio_scl);
+err_gpio_scl:
+	iounmap(dev_data->ioaddr);
+err_map_mem:
+	release_mem_region(dev_data->mem->start,
+			dev_data->mem->end - dev_data->mem->start + 1);
+err_req_mem:
+err_mem:
+err_irq:
+err_pdata:
+err_alloc_dd:
+	kfree(dev_data);
+
+	return ret;
+}
+
+static int __devexit ns9xxx_i2c_remove(struct platform_device *pdev)
+{
+	struct ns9xxx_i2c *dev_data = platform_get_drvdata(pdev);
+
+	i2c_del_adapter(&dev_data->adap);
+
+	free_irq(dev_data->irq, dev_data);
+
+	clk_disable(dev_data->clk);
+	clk_put(dev_data->clk);
+
+	gpio_free(dev_data->pdata->gpio_sda);
+	gpio_free(dev_data->pdata->gpio_scl);
+
+	iounmap(dev_data->ioaddr);
+	release_mem_region(dev_data->mem->start,
+			dev_data->mem->end - dev_data->mem->start + 1);
+
+	kfree(dev_data);
+
+	return 0;
+}
+
+static struct platform_driver ns9xxx_i2c_driver = {
+	.probe		= ns9xxx_i2c_probe,
+	.remove		= __devexit_p(ns9xxx_i2c_remove),
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	}
+};
+
+static int __init ns9xxx_i2c_init(void)
+{
+	return platform_driver_register(&ns9xxx_i2c_driver);
+}
+
+static void __exit ns9xxx_i2c_exit(void)
+{
+	platform_driver_unregister(&ns9xxx_i2c_driver);
+}
+
+module_init(ns9xxx_i2c_init);
+module_exit(ns9xxx_i2c_exit);
+
+MODULE_AUTHOR("Matthias Ludwig");
+MODULE_DESCRIPTION("Digi NS9xxx I2C Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index b7434d24904e..622caefd6f50 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -666,7 +666,7 @@ static int s3c24xx_i2c_clockrate(struct s3c24xx_i2c *i2c, unsigned int *got)
 
 	pdata = s3c24xx_i2c_get_platformdata(i2c->adap.dev.parent);
 	clkin /= 1000;		/* clkin now in KHz */
-     
+
 	dev_dbg(i2c->dev, "pdata %p, freq %lu %lu..%lu\n",
 		 pdata, pdata->bus_freq, pdata->min_freq, pdata->max_freq);
 
@@ -689,6 +689,13 @@ static int s3c24xx_i2c_clockrate(struct s3c24xx_i2c *i2c, unsigned int *got)
 
 	for (; start > end; start--) {
 		freq = s3c24xx_i2c_calcdivisor(clkin, start, &div1, &divs);
+		/*
+		 * If the clock is higher than the requested, continue searching
+		 * (Luis Galdos)
+		 */
+		if (pdata->bus_freq && (freq * 1000 > pdata->bus_freq))
+			continue;
+
 		if (freq_acceptable(freq, start))
 			goto found;
 	}
diff --git a/drivers/i2c/busses/i2c-stmp378x.c b/drivers/i2c/busses/i2c-stmp378x.c
new file mode 100644
index 000000000000..4a01b55f5f07
--- /dev/null
+++ b/drivers/i2c/busses/i2c-stmp378x.c
@@ -0,0 +1,337 @@
+/*
+ * Freescale STMP378X I2C bus driver
+ *
+ * Author: Dmitrij Frasenyak <sed@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/* #define DEBUG */
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/completion.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+
+#include <mach/regs-i2c.h>
+#include <mach/regs-apbx.h>
+#include <mach/i2c.h>
+
+static void reset_i2c_module(void)
+{
+	int count;
+	count = 1000;
+	HW_I2C_CTRL0_SET(BM_I2C_CTRL0_SFTRST);
+	udelay(10); /* Reseting the module can take multiple clocks.*/
+	while (--count && (!(HW_I2C_CTRL0_RD() & BM_I2C_CTRL0_CLKGATE)))
+		udelay(1);
+
+	if (!count) {
+		printk(KERN_ERR "timeout reseting the module\n");
+		BUG();
+	}
+
+	/* take controller out of reset */
+	HW_I2C_CTRL0_CLR(BM_I2C_CTRL0_SFTRST | BM_I2C_CTRL0_CLKGATE);
+	udelay(10);
+	HW_I2C_CTRL1_SET(0x0000FF00); /* Wil catch all error (IRQ mask) */
+
+}
+
+/*
+ * Low level master read/write transaction.
+ */
+static int stmp378x_i2c_xfer_msg(struct i2c_adapter *adap,
+				 struct i2c_msg *msg, int stop)
+{
+	struct stmp378x_i2c_dev *dev = i2c_get_adapdata(adap);
+	int err;
+
+	init_completion(&dev->cmd_complete);
+	dev->cmd_err = 0;
+
+	dev_dbg(dev->dev, " Start XFER ===>\n");
+	dev_dbg(dev->dev, "addr: 0x%04x, len: %d, flags: 0x%x, stop: %d\n",
+		msg->addr, msg->len, msg->flags, stop);
+
+	if ((msg->len == 0) || (msg->len > (PAGE_SIZE - 1)))
+		return -EINVAL;
+
+	if (msg->flags & I2C_M_RD) {
+		hw_i2c_setup_read(msg->addr ,
+				  msg->buf ,
+				  msg->len,
+				  stop ? BM_I2C_CTRL0_POST_SEND_STOP : 0);
+
+		hw_i2c_run(1); /* read */
+	} else {
+		hw_i2c_setup_write(msg->addr ,
+				   msg->buf ,
+				   msg->len,
+				   stop ? BM_I2C_CTRL0_POST_SEND_STOP : 0);
+
+		hw_i2c_run(0); /* write */
+	}
+
+	err = wait_for_completion_interruptible_timeout(
+		&dev->cmd_complete,
+		msecs_to_jiffies(1000)
+		);
+
+	if (err < 0) {
+		dev_dbg(dev->dev, "controler is timed out\n");
+		return -ETIMEDOUT;
+	}
+	if ((!dev->cmd_err) && (msg->flags & I2C_M_RD))
+		hw_i2c_finish_read(msg->buf, msg->len);
+
+	dev_dbg(dev->dev, "<============= Done with err=%d\n", dev->cmd_err);
+
+
+	return dev->cmd_err;
+}
+
+
+static int
+stmp378x_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
+{
+	int i;
+	int err;
+
+	if (!msgs->len)
+		return -EINVAL;
+
+	for (i = 0; i < num; i++) {
+		err = stmp378x_i2c_xfer_msg(adap, &msgs[i], (i == (num - 1)));
+		if (err)
+			break;
+	}
+
+	if (err == 0)
+		err = num;
+
+	return err;
+}
+
+static u32
+stmp378x_i2c_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK);
+}
+
+/*
+ * Debug. Don't need dma_irq for the final version
+ */
+
+static irqreturn_t
+stmp378x_i2c_dma_isr(int this_irq, void *dev_id)
+{
+	hw_i2c_clear_dma_interrupt();
+	return IRQ_HANDLED;
+
+}
+
+#define I2C_IRQ_MASK 0x000000FF
+
+static irqreturn_t
+stmp378x_i2c_isr(int this_irq, void *dev_id)
+{
+	struct stmp378x_i2c_dev *dev = dev_id;
+	u32 stat;
+	u32 done_mask =
+		BM_I2C_CTRL1_DATA_ENGINE_CMPLT_IRQ |
+		BM_I2C_CTRL1_BUS_FREE_IRQ ;
+
+	stat = HW_I2C_CTRL1_RD() & I2C_IRQ_MASK;
+	if (!stat)
+		return IRQ_NONE;
+
+	if (stat & BM_I2C_CTRL1_NO_SLAVE_ACK_IRQ) {
+		dev->cmd_err = -EREMOTEIO;
+
+		/*
+		 * Stop DMA
+		 * Clear NAK
+		 */
+		HW_I2C_CTRL1_SET(BM_I2C_CTRL1_CLR_GOT_A_NAK);
+		hw_i2c_reset_dma();
+		reset_i2c_module();
+
+		complete(&dev->cmd_complete);
+
+		goto done;
+	}
+
+/* Don't care about  BM_I2C_CTRL1_OVERSIZE_XFER_TERM_IRQ */
+	if (stat & (
+		    BM_I2C_CTRL1_EARLY_TERM_IRQ |
+		    BM_I2C_CTRL1_MASTER_LOSS_IRQ |
+		    BM_I2C_CTRL1_SLAVE_STOP_IRQ |
+		    BM_I2C_CTRL1_SLAVE_IRQ
+		    )) {
+		dev->cmd_err = -EIO;
+		complete(&dev->cmd_complete);
+		goto done;
+	}
+	if ((stat & done_mask)  == done_mask)
+		complete(&dev->cmd_complete);
+
+
+done:
+	HW_I2C_CTRL1_CLR(stat);
+	return IRQ_HANDLED;
+}
+
+static const struct i2c_algorithm stmp378x_i2c_algo = {
+	.master_xfer	= stmp378x_i2c_xfer,
+	.functionality	= stmp378x_i2c_func,
+};
+
+
+static int
+stmp378x_i2c_probe(struct platform_device *pdev)
+{
+	struct stmp378x_i2c_dev	*dev;
+	struct i2c_adapter	*adap;
+	struct resource		*irq;
+	int err = 0;
+
+	/* NOTE: driver uses the static register mapping */
+	dev = kzalloc(sizeof(struct stmp378x_i2c_dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&pdev->dev, "no mem \n");
+		return -ENOMEM;
+	}
+
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0); /* Error */
+	if (!irq) {
+		dev_err(&pdev->dev, "no err_irq resource\n");
+		err = -ENODEV;
+		goto nores;
+	}
+	dev->irq_err = irq->start;
+
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 1); /* DMA */
+	if (!irq) {
+		dev_err(&pdev->dev, "no dma_irq resource\n");
+		err = -ENODEV;
+		goto nores;
+	}
+
+	dev->irq_dma = irq->start;
+	dev->dev = &pdev->dev;
+
+	err = request_irq(dev->irq_err, stmp378x_i2c_isr, 0, pdev->name, dev);
+	if (err) {
+		dev_err(&pdev->dev, "Can't get IRQ\n");
+		goto no_err_irq;
+	}
+
+	err = request_irq(dev->irq_dma,
+			  stmp378x_i2c_dma_isr,
+			  0, pdev->name, dev);
+	if (err) {
+		dev_err(&pdev->dev, "Can't get IRQ\n");
+		goto no_dma_irq;
+	}
+
+	err = hw_i2c_init(&pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "HW Init failed\n");
+		goto init_failed;
+	}
+
+	HW_I2C_CTRL1_SET(0x0000FF00); /* Will catch all error (IRQ mask) */
+
+	adap = &dev->adapter;
+	i2c_set_adapdata(adap, dev);
+	adap->owner = THIS_MODULE;
+	adap->class = I2C_CLASS_HWMON;
+	strncpy(adap->name, "378x I2C adapter", sizeof(adap->name));
+	adap->algo = &stmp378x_i2c_algo;
+	adap->dev.parent = &pdev->dev;
+
+	adap->nr = pdev->id;
+	err = i2c_add_numbered_adapter(adap);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to add adapter\n");
+		goto no_i2c_adapter;
+
+	}
+
+	return 0;
+
+no_i2c_adapter:
+	hw_i2c_stop(dev->dev);
+init_failed:
+	free_irq(dev->irq_dma, dev);
+no_dma_irq:
+	free_irq(dev->irq_err, dev);
+no_err_irq:
+nores:
+	kfree(dev);
+	return err;
+}
+
+static int
+stmp378x_i2c_remove(struct platform_device *pdev)
+{
+	struct stmp378x_i2c_dev	*dev = platform_get_drvdata(pdev);
+	int res;
+
+	res = i2c_del_adapter(&dev->adapter);
+	if (res)
+		return -EBUSY;
+
+	hw_i2c_stop(dev->dev);
+
+	platform_set_drvdata(pdev, NULL);
+
+	free_irq(dev->irq_err, dev);
+	free_irq(dev->irq_dma, dev);
+
+	kfree(dev);
+	return 0;
+}
+
+static struct platform_driver stmp378x_i2c_driver = {
+	.probe		= stmp378x_i2c_probe,
+	.remove		= __devexit_p(stmp378x_i2c_remove),
+	.driver		= {
+		.name	= "i2c_stmp",
+		.owner	= THIS_MODULE,
+	},
+};
+
+/* I2C may be needed to bring up other drivers */
+
+static int __init stmp378x_i2c_init_driver(void)
+{
+	return platform_driver_register(&stmp378x_i2c_driver);
+}
+subsys_initcall(stmp378x_i2c_init_driver);
+
+static void __exit stmp378x_i2c_exit_driver(void)
+{
+	platform_driver_unregister(&stmp378x_i2c_driver);
+}
+module_exit(stmp378x_i2c_exit_driver);
+
+MODULE_AUTHOR("old_chap@embeddedalley.com");
+MODULE_DESCRIPTION("IIC for Freescale STMP378x");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/busses/mxc_i2c.c b/drivers/i2c/busses/mxc_i2c.c
new file mode 100644
index 000000000000..f051419f02bc
--- /dev/null
+++ b/drivers/i2c/busses/mxc_i2c.c
@@ -0,0 +1,799 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_i2c.c
+ *
+ * @brief Driver for the Freescale Semiconductor MXC I2C buses.
+ *
+ * Based on i2c driver algorithm for PCF8584 adapters
+ *
+ * @ingroup MXCI2C
+ */
+
+/*
+ * Include Files
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/i2c.h>
+#include <linux/clk.h>
+#include <asm/irq.h>
+#include <asm/io.h>
+#include "mxc_i2c_reg.h"
+
+/*!
+ * In case the MXC device has multiple I2C modules, this structure is used to
+ * store information specific to each I2C module.
+ */
+typedef struct {
+	/*!
+	 * This structure is used to identify the physical i2c bus along with
+	 * the access algorithms necessary to access it.
+	 */
+	struct i2c_adapter adap;
+
+	/*!
+	 * This waitqueue is used to wait for the data transfer to complete.
+	 */
+	wait_queue_head_t wq;
+
+	/*!
+	 * The base address of the I2C device.
+	 */
+	unsigned long membase;
+
+	/*!
+	 * The interrupt number used by the I2C device.
+	 */
+	int irq;
+
+	/*!
+	 * The default clock divider value to be used.
+	 */
+	unsigned int clkdiv;
+
+	/*!
+	 * The clock source for the device.
+	 */
+	struct clk *clk;
+
+	/*!
+	 * The current power state of the device
+	 */
+	bool low_power;
+
+	/*!
+	 * Boolean to indicate if data was transferred
+	 */
+	bool transfer_done;
+
+	/*!
+	 * Boolean to indicate if we received an ACK for the data transmitted
+	 */
+	bool tx_success;
+} mxc_i2c_device;
+
+struct clk_div_table {
+	int reg_value;
+	int div;
+};
+
+static const struct clk_div_table i2c_clk_table[] = {
+	{0x20, 22}, {0x21, 24}, {0x22, 26}, {0x23, 28},
+	{0, 30}, {1, 32}, {0x24, 32}, {2, 36},
+	{0x25, 36}, {0x26, 40}, {3, 42}, {0x27, 44},
+	{4, 48}, {0x28, 48}, {5, 52}, {0x29, 56},
+	{6, 60}, {0x2A, 64}, {7, 72}, {0x2B, 72},
+	{8, 80}, {0x2C, 80}, {9, 88}, {0x2D, 96},
+	{0xA, 104}, {0x2E, 112}, {0xB, 128}, {0x2F, 128},
+	{0xC, 144}, {0xD, 160}, {0x30, 160}, {0xE, 192},
+	{0x31, 192}, {0x32, 224}, {0xF, 240}, {0x33, 256},
+	{0x10, 288}, {0x11, 320}, {0x34, 320}, {0x12, 384},
+	{0x35, 384}, {0x36, 448}, {0x13, 480}, {0x37, 512},
+	{0x14, 576}, {0x15, 640}, {0x38, 640}, {0x16, 768},
+	{0x39, 768}, {0x3A, 896}, {0x17, 960}, {0x3B, 1024},
+	{0x18, 1152}, {0x19, 1280}, {0x3C, 1280}, {0x1A, 1536},
+	{0x3D, 1536}, {0x3E, 1792}, {0x1B, 1920}, {0x3F, 2048},
+	{0x1C, 2304}, {0x1D, 2560}, {0x1E, 3072}, {0x1F, 3840},
+	{0, 0}
+};
+
+extern void gpio_i2c_active(int i2c_num);
+extern void gpio_i2c_inactive(int i2c_num);
+
+/*!
+ * Transmit a \b STOP signal to the slave device.
+ *
+ * @param   dev   the mxc i2c structure used to get to the right i2c device
+ */
+static void mxc_i2c_stop(mxc_i2c_device * dev)
+{
+	unsigned int cr, sr;
+	int retry = 16;
+
+	cr = readw(dev->membase + MXC_I2CR);
+	cr &= ~(MXC_I2CR_MSTA | MXC_I2CR_MTX);
+	writew(cr, dev->membase + MXC_I2CR);
+
+	/* Wait till the Bus Busy bit is reset */
+	sr = readw(dev->membase + MXC_I2SR);
+	while (retry-- && ((sr & MXC_I2SR_IBB))) {
+		udelay(3);
+		sr = readw(dev->membase + MXC_I2SR);
+	}
+	if (retry <= 0)
+		dev_err(&dev->adap.dev, "Could not set I2C Bus Busy bit"
+			" to zero.\n");
+}
+
+/*!
+ * Wait for the transmission of the data byte to complete. This function waits
+ * till we get a signal from the interrupt service routine indicating completion
+ * of the address cycle or we time out.
+ *
+ * @param   dev         the mxc i2c structure used to get to the right i2c device
+ * @param   trans_flag  transfer flag
+ *
+ *
+ * @return  The function returns 0 on success or -1 if an ack was not received
+ */
+
+static int mxc_i2c_wait_for_tc(mxc_i2c_device * dev, int trans_flag)
+{
+	int retry = 16;
+
+	while (retry-- && !dev->transfer_done) {
+		wait_event_interruptible_timeout(dev->wq,
+						 dev->transfer_done,
+						 dev->adap.timeout);
+	}
+	dev->transfer_done = false;
+
+	if (retry <= 0) {
+		/* Unable to send data */
+		dev_err(&dev->adap.dev, "Data not transmitted\n");
+		return -1;
+	}
+
+	if (!dev->tx_success) {
+		/* An ACK was not received for transmitted byte */
+		dev_err(&dev->adap.dev, "ACK not received \n");
+		return -1;
+	}
+
+	return 0;
+}
+
+/*!
+ * Transmit a \b START signal to the slave device.
+ *
+ * @param   dev   the mxc i2c structure used to get to the right i2c device
+ * @param   *msg  pointer to a message structure that contains the slave
+ *                address
+ *
+ * @return  The function returns EBUSY on failure, 0 on success.
+ */
+static int mxc_i2c_start(mxc_i2c_device *dev, struct i2c_msg *msg)
+{
+	volatile unsigned int cr, sr;
+	unsigned int addr_trans;
+	int retry = 16;
+
+	/*
+	 * Set the slave address and the requested transfer mode
+	 * in the data register
+	 */
+	addr_trans = msg->addr << 1;
+	if (msg->flags & I2C_M_RD) {
+		addr_trans |= 0x01;
+	}
+
+	/* Set the Master bit */
+	cr = readw(dev->membase + MXC_I2CR);
+	cr |= MXC_I2CR_MSTA;
+	writew(cr, dev->membase + MXC_I2CR);
+
+	/* Wait till the Bus Busy bit is set */
+	sr = readw(dev->membase + MXC_I2SR);
+	while (retry-- && (!(sr & MXC_I2SR_IBB))) {
+		udelay(3);
+		sr = readw(dev->membase + MXC_I2SR);
+	}
+	if (retry <= 0) {
+		dev_err(&dev->adap.dev, "Could not grab Bus ownership\n");
+		return -EBUSY;
+	}
+
+	/* Set the Transmit bit */
+	cr = readw(dev->membase + MXC_I2CR);
+	cr |= MXC_I2CR_MTX;
+	writew(cr, dev->membase + MXC_I2CR);
+
+	writew(addr_trans, dev->membase + MXC_I2DR);
+	return 0;
+}
+
+/*!
+ * Transmit a \b REPEAT START to the slave device
+ *
+ * @param   dev   the mxc i2c structure used to get to the right i2c device
+ * @param   *msg  pointer to a message structure that contains the slave
+ *                address
+ */
+static void mxc_i2c_repstart(mxc_i2c_device *dev, struct i2c_msg *msg)
+{
+	volatile unsigned int cr, sr;
+	unsigned int addr_trans;
+	int retry = 16;
+
+	/*
+	 * Set the slave address and the requested transfer mode
+	 * in the data register
+	 */
+	addr_trans = msg->addr << 1;
+	if (msg->flags & I2C_M_RD) {
+		addr_trans |= 0x01;
+	}
+	cr = readw(dev->membase + MXC_I2CR);
+	cr |= MXC_I2CR_RSTA;
+	writew(cr, dev->membase + MXC_I2CR);
+	/* Wait till the Bus Busy bit is set */
+	sr = readw(dev->membase + MXC_I2SR);
+	while (retry-- && (!(sr & MXC_I2SR_IBB))) {
+		udelay(3);
+		sr = readw(dev->membase + MXC_I2SR);
+	}
+	if (retry <= 0) {
+		dev_err(&dev->adap.dev, "Could not grab Bus ownership\n");
+		return;
+	}
+	writew(addr_trans, dev->membase + MXC_I2DR);
+}
+
+/*!
+ * Read the received data. The function waits till data is available or times
+ * out. Generates a stop signal if this is the last message to be received.
+ * Sends an ack for all the bytes received except the last byte.
+ *
+ * @param  dev       the mxc i2c structure used to get to the right i2c device
+ * @param  *msg      pointer to a message structure that contains the slave
+ *                   address and a pointer to the receive buffer
+ * @param  last      indicates that this is the last message to be received
+ * @param  addr_comp flag indicates that we just finished the address cycle
+ *
+ * @return  The function returns the number of bytes read or -1 on time out.
+ */
+static int mxc_i2c_readbytes(mxc_i2c_device * dev, struct i2c_msg *msg,
+			     int last, int addr_comp)
+{
+	int i;
+	char *buf = msg->buf;
+	int len = msg->len;
+	volatile unsigned int cr;
+
+	cr = readw(dev->membase + MXC_I2CR);
+	/*
+	 * Clear MTX to switch to receive mode.
+	 */
+	cr &= ~MXC_I2CR_MTX;
+	/*
+	 * Clear the TXAK bit to gen an ack when receiving only one byte.
+	 */
+	if (len == 1) {
+		cr |= MXC_I2CR_TXAK;
+	} else {
+		cr &= ~MXC_I2CR_TXAK;
+	}
+	writew(cr, dev->membase + MXC_I2CR);
+	/*
+	 * Dummy read only at the end of an address cycle
+	 */
+	if (addr_comp > 0) {
+		readw(dev->membase + MXC_I2DR);
+	}
+
+	for (i = 0; i < len; i++) {
+		/*
+		 * Wait for data transmission to complete
+		 */
+		if (mxc_i2c_wait_for_tc(dev, msg->flags)) {
+			mxc_i2c_stop(dev);
+			return -1;
+		}
+		/*
+		 * Do not generate an ACK for the last byte
+		 */
+		if (i == (len - 2)) {
+			cr = readw(dev->membase + MXC_I2CR);
+			cr |= MXC_I2CR_TXAK;
+			writew(cr, dev->membase + MXC_I2CR);
+		} else if (i == (len - 1)) {
+			if (last) {
+				mxc_i2c_stop(dev);
+			}
+		}
+		/* Read the data */
+		*buf++ = readw(dev->membase + MXC_I2DR);
+	}
+
+	return i;
+}
+
+/*!
+ * Write the data to the data register. Generates a stop signal if this is
+ * the last message to be sent or if no ack was received for the data sent.
+ *
+ * @param   dev   the mxc i2c structure used to get to the right i2c device
+ * @param   *msg  pointer to a message structure that contains the slave
+ *                address and data to be sent
+ * @param   last  indicates that this is the last message to be received
+ *
+ * @return  The function returns the number of bytes written or -1 on time out
+ *          or if no ack was received for the data that was sent.
+ */
+static int mxc_i2c_writebytes(mxc_i2c_device * dev, struct i2c_msg *msg,
+			      int last)
+{
+	int i;
+	char *buf = msg->buf;
+	int len = msg->len;
+	volatile unsigned int cr;
+
+	cr = readw(dev->membase + MXC_I2CR);
+	/* Set MTX to switch to transmit mode */
+	cr |= MXC_I2CR_MTX;
+	writew(cr, dev->membase + MXC_I2CR);
+
+	for (i = 0; i < len; i++) {
+		/*
+		 * Write the data
+		 */
+		writew(*buf++, dev->membase + MXC_I2DR);
+		if (mxc_i2c_wait_for_tc(dev, msg->flags)) {
+			mxc_i2c_stop(dev);
+			return -1;
+		}
+	}
+	if (last > 0) {
+		mxc_i2c_stop(dev);
+	}
+
+	return i;
+}
+
+/*!
+ * Function enables the I2C module and initializes the registers.
+ *
+ * @param   dev   the mxc i2c structure used to get to the right i2c device
+ * @param   trans_flag  transfer flag
+ */
+static void mxc_i2c_module_en(mxc_i2c_device * dev, int trans_flag)
+{
+	clk_enable(dev->clk);
+	/* Set the frequency divider */
+	writew(dev->clkdiv, dev->membase + MXC_IFDR);
+	/* Clear the status register */
+	writew(0x0, dev->membase + MXC_I2SR);
+	/* Enable I2C and its interrupts */
+	writew(MXC_I2CR_IEN, dev->membase + MXC_I2CR);
+	writew(MXC_I2CR_IEN | MXC_I2CR_IIEN, dev->membase + MXC_I2CR);
+}
+
+/*!
+ * Disables the I2C module.
+ *
+ * @param   dev   the mxc i2c structure used to get to the right i2c device
+ */
+static void mxc_i2c_module_dis(mxc_i2c_device * dev)
+{
+	writew(0x0, dev->membase + MXC_I2CR);
+	clk_disable(dev->clk);
+}
+
+/*!
+ * The function is registered in the adapter structure. It is called when an MXC
+ * driver wishes to transfer data to a device connected to the I2C device.
+ *
+ * @param   adap   adapter structure for the MXC i2c device
+ * @param   msgs[] array of messages to be transferred to the device
+ * @param   num    number of messages to be transferred to the device
+ *
+ * @return  The function returns the number of messages transferred,
+ *          \b -EREMOTEIO on I2C failure and a 0 if the num argument is
+ *          less than 0.
+ */
+static int mxc_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
+			int num)
+{
+	mxc_i2c_device *dev = (mxc_i2c_device *) (i2c_get_adapdata(adap));
+	int i, ret = 0, addr_comp = 0;
+	volatile unsigned int sr;
+	int retry = 5;
+
+	if (dev->low_power) {
+		dev_err(&dev->adap.dev, "I2C Device in low power mode\n");
+		return -EREMOTEIO;
+	}
+
+	if (num < 1) {
+		return 0;
+	}
+
+	mxc_i2c_module_en(dev, msgs[0].flags);
+	sr = readw(dev->membase + MXC_I2SR);
+	/*
+	 * Check bus state
+	 */
+
+	while ((sr & MXC_I2SR_IBB) && retry--) {
+		udelay(5);
+		sr = readw(dev->membase + MXC_I2SR);
+	}
+
+	if ((sr & MXC_I2SR_IBB) && retry < 0) {
+		mxc_i2c_module_dis(dev);
+		dev_err(&dev->adap.dev, "Bus busy\n");
+		return -EREMOTEIO;
+	}
+
+	//gpio_i2c_active(dev->adap.id);
+	dev->transfer_done = false;
+	dev->tx_success = false;
+	for (i = 0; i < num && ret >= 0; i++) {
+		addr_comp = 0;
+		/*
+		 * Send the slave address and transfer direction in the
+		 * address cycle
+		 */
+		if (i == 0) {
+			/*
+			 * Send a start or repeat start signal
+			 */
+			if (mxc_i2c_start(dev, &msgs[0]))
+				return -EREMOTEIO;
+			/* Wait for the address cycle to complete */
+			if (mxc_i2c_wait_for_tc(dev, msgs[0].flags)) {
+				mxc_i2c_stop(dev);
+				//gpio_i2c_inactive(dev->adap.id);
+				mxc_i2c_module_dis(dev);
+				return -EREMOTEIO;
+			}
+			addr_comp = 1;
+		} else {
+			/*
+			 * Generate repeat start only if required i.e the address
+			 * changed or the transfer direction changed
+			 */
+			if ((msgs[i].addr != msgs[i - 1].addr) ||
+			    ((msgs[i].flags & I2C_M_RD) !=
+			     (msgs[i - 1].flags & I2C_M_RD))) {
+				mxc_i2c_repstart(dev, &msgs[i]);
+				/* Wait for the address cycle to complete */
+				if (mxc_i2c_wait_for_tc(dev, msgs[i].flags)) {
+					mxc_i2c_stop(dev);
+					//gpio_i2c_inactive(dev->adap.id);
+					mxc_i2c_module_dis(dev);
+					return -EREMOTEIO;
+				}
+				addr_comp = 1;
+			}
+		}
+
+		/* Transfer the data */
+		if (msgs[i].flags & I2C_M_RD) {
+			/* Read the data */
+			ret = mxc_i2c_readbytes(dev, &msgs[i], (i + 1 == num),
+						addr_comp);
+			if (ret < 0) {
+				dev_err(&dev->adap.dev, "mxc_i2c_readbytes:"
+					" fail.\n");
+				break;
+			}
+		} else {
+			/* Write the data */
+			ret = mxc_i2c_writebytes(dev, &msgs[i], (i + 1 == num));
+			if (ret < 0) {
+				dev_err(&dev->adap.dev, "mxc_i2c_writebytes:"
+					" fail.\n");
+				break;
+			}
+		}
+	}
+
+	//gpio_i2c_inactive(dev->adap.id);
+	mxc_i2c_module_dis(dev);
+	/*
+	 * Decrease by 1 as we do not want Start message to be included in
+	 * the count
+	 */
+	return (i < 0 ? ret : i);
+}
+
+/*!
+ * Returns the i2c functionality supported by this driver.
+ *
+ * @param   adap adapter structure for this i2c device
+ *
+ * @return Returns the functionality that is supported.
+ */
+static u32 mxc_i2c_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+/*!
+ * Stores the pointers for the i2c algorithm functions. The algorithm functions
+ * is used by the i2c bus driver to talk to the i2c bus
+ */
+static struct i2c_algorithm mxc_i2c_algorithm = {
+	.master_xfer = mxc_i2c_xfer,
+	.functionality = mxc_i2c_func
+};
+
+/*!
+ * Interrupt Service Routine. It signals to the process about the data transfer
+ * completion. Also sets a flag if bus arbitration is lost.
+ * @param   irq    the interrupt number
+ * @param   dev_id driver private data
+ *
+ * @return  The function returns \b IRQ_HANDLED.
+ */
+static irqreturn_t mxc_i2c_handler(int irq, void *dev_id)
+{
+	mxc_i2c_device *dev = dev_id;
+	volatile unsigned int sr, cr;
+
+	sr = readw(dev->membase + MXC_I2SR);
+	cr = readw(dev->membase + MXC_I2CR);
+
+	/*
+	 * Clear the interrupt bit
+	 */
+	writew(0x0, dev->membase + MXC_I2SR);
+
+	if (sr & MXC_I2SR_IAL) {
+		dev_err(&dev->adap.dev, "Bus Arbitration lost\n");
+	} else {
+		/* Interrupt due byte transfer completion */
+		dev->tx_success = true;
+		/* Check if RXAK is received in Transmit mode */
+		if ((cr & MXC_I2CR_MTX) && (sr & MXC_I2SR_RXAK)) {
+			dev->tx_success = false;
+		}
+		dev->transfer_done = true;
+		wake_up_interruptible(&dev->wq);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * This function is called to put the I2C adapter in a low power state. Refer to the
+ * document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure used to give information on which I2C
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function returns 0 on success and -1 on failure.
+ */
+static int mxci2c_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	mxc_i2c_device *mxcdev = platform_get_drvdata(pdev);
+	volatile unsigned int sr = 0;
+
+	if (mxcdev == NULL) {
+		return -1;
+	}
+
+	/* Prevent further calls to be processed */
+	mxcdev->low_power = true;
+	/* Wait till we finish the current transfer */
+	sr = readw(mxcdev->membase + MXC_I2SR);
+	while (sr & MXC_I2SR_IBB) {
+		msleep(10);
+		sr = readw(mxcdev->membase + MXC_I2SR);
+	}
+	gpio_i2c_inactive(mxcdev->adap.id);
+
+	return 0;
+}
+
+/*!
+ * This function is called to bring the I2C adapter back from a low power state. Refer
+ * to the document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure used to give information on which I2C
+ *                to resume
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int mxci2c_resume(struct platform_device *pdev)
+{
+	mxc_i2c_device *mxcdev = platform_get_drvdata(pdev);
+
+	if (mxcdev == NULL)
+		return -1;
+
+	mxcdev->low_power = false;
+	gpio_i2c_active(mxcdev->adap.id);
+
+	return 0;
+}
+
+/*!
+ * This function is called during the driver binding process.
+ *
+ * @param   pdev  the device structure used to store device specific
+ *                information that is used by the suspend, resume and remove
+ *                functions
+ *
+ * @return  The function always returns 0.
+ */
+static int mxci2c_probe(struct platform_device *pdev)
+{
+	mxc_i2c_device *mxc_i2c;
+	struct mxc_i2c_platform_data *i2c_plat_data = pdev->dev.platform_data;
+	struct resource *res;
+	int id = pdev->id;
+	u32 clk_freq;
+	int ret = 0;
+	int i;
+
+	mxc_i2c = kzalloc(sizeof(mxc_i2c_device), GFP_KERNEL);
+	if (!mxc_i2c) {
+		return -ENOMEM;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		ret = -ENODEV;
+		goto err1;
+	}
+	mxc_i2c->membase = IO_ADDRESS(res->start);
+
+	/*
+	 * Request the I2C interrupt
+	 */
+	mxc_i2c->irq = platform_get_irq(pdev, 0);
+	if (mxc_i2c->irq < 0) {
+		ret = mxc_i2c->irq;
+		goto err1;
+	}
+
+	ret = request_irq(mxc_i2c->irq, mxc_i2c_handler,
+			  0, pdev->name, mxc_i2c);
+	if (ret < 0) {
+		goto err1;
+	}
+
+	init_waitqueue_head(&mxc_i2c->wq);
+
+	mxc_i2c->low_power = false;
+
+	gpio_i2c_active(id);
+
+	mxc_i2c->clk = clk_get(&pdev->dev, "i2c_clk");
+	clk_freq = clk_get_rate(mxc_i2c->clk);
+	mxc_i2c->clkdiv = -1;
+	if (i2c_plat_data->i2c_clk) {
+		/* Calculate divider and round up any fractional part */
+		int div = (clk_freq + i2c_plat_data->i2c_clk - 1) /
+		    i2c_plat_data->i2c_clk;
+		for (i = 0; i2c_clk_table[i].div != 0; i++) {
+			if (i2c_clk_table[i].div >= div) {
+				mxc_i2c->clkdiv = i2c_clk_table[i].reg_value;
+				break;
+			}
+		}
+	}
+	if (mxc_i2c->clkdiv == -1) {
+		i--;
+		mxc_i2c->clkdiv = 0x1F;	/* Use max divider */
+	}
+	dev_dbg(&pdev->dev, "i2c speed is %d/%d = %d bps, reg val = 0x%02X\n",
+		clk_freq, i2c_clk_table[i].div,
+		clk_freq / i2c_clk_table[i].div, mxc_i2c->clkdiv);
+
+	/*
+	 * Set the adapter information
+	 */
+	strlcpy(mxc_i2c->adap.name, pdev->name, 48);
+	mxc_i2c->adap.id = mxc_i2c->adap.nr = id;
+	mxc_i2c->adap.algo = &mxc_i2c_algorithm;
+	mxc_i2c->adap.timeout = 1;
+	platform_set_drvdata(pdev, mxc_i2c);
+	i2c_set_adapdata(&mxc_i2c->adap, mxc_i2c);
+	if ((ret = i2c_add_numbered_adapter(&mxc_i2c->adap)) < 0) {
+		goto err2;
+	}
+
+	printk(KERN_INFO "MXC I2C driver\n");
+	return 0;
+
+      err2:
+	free_irq(mxc_i2c->irq, mxc_i2c);
+	gpio_i2c_inactive(id);
+      err1:
+	dev_err(&pdev->dev, "failed to probe i2c adapter\n");
+	kfree(mxc_i2c);
+	return ret;
+}
+
+/*!
+ * Dissociates the driver from the I2C device.
+ *
+ * @param   pdev   the device structure used to give information on which I2C
+ *                to remove
+ *
+ * @return  The function always returns 0.
+ */
+static int mxci2c_remove(struct platform_device *pdev)
+{
+	mxc_i2c_device *mxc_i2c = platform_get_drvdata(pdev);
+	int id = pdev->id;
+
+	free_irq(mxc_i2c->irq, mxc_i2c);
+	i2c_del_adapter(&mxc_i2c->adap);
+	gpio_i2c_inactive(id);
+	clk_put(mxc_i2c->clk);
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxci2c_driver = {
+	.driver = {
+		   .name = "mxc_i2c",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = mxci2c_probe,
+	.remove = mxci2c_remove,
+	.suspend_late = mxci2c_suspend,
+	.resume_early = mxci2c_resume,
+};
+
+/*!
+ * Function requests the interrupts and registers the i2c adapter structures.
+ *
+ * @return The function returns 0 on success and a non-zero value on failure.
+ */
+static int __init mxc_i2c_init(void)
+{
+	/* Register the device driver structure. */
+	return platform_driver_register(&mxci2c_driver);
+}
+
+/*!
+ * This function is used to cleanup all resources before the driver exits.
+ */
+static void __exit mxc_i2c_exit(void)
+{
+	platform_driver_unregister(&mxci2c_driver);
+}
+
+subsys_initcall(mxc_i2c_init);
+module_exit(mxc_i2c_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC I2C driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/busses/mxc_i2c_hs.c b/drivers/i2c/busses/mxc_i2c_hs.c
new file mode 100644
index 000000000000..b9132351359e
--- /dev/null
+++ b/drivers/i2c/busses/mxc_i2c_hs.c
@@ -0,0 +1,549 @@
+/*
+ * Copyright 2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/i2c.h>
+#include <linux/clk.h>
+#include <asm/irq.h>
+#include <asm/io.h>
+#include "mxc_i2c_hs_reg.h"
+
+typedef struct {
+	struct device *dev;
+
+	unsigned long reg_base_virt;
+	unsigned long reg_base_phy;
+	int irq;
+	unsigned int speed;
+	struct clk *ipg_clk;
+	struct clk *serial_clk;
+	bool low_power;
+
+	struct i2c_msg *msg;
+	int index;
+} mxc_i2c_hs;
+
+struct clk_div_table {
+	int reg_value;
+	int div;
+};
+
+static const struct clk_div_table i2c_clk_table[] = {
+	{0x0, 16}, {0x1, 18}, {0x2, 20}, {0x3, 22},
+	{0x20, 24}, {0x21, 26}, {0x22, 28}, {0x23, 30},
+	{0x4, 32}, {0x5, 36}, {0x6, 40}, {0x7, 44},
+	{0x24, 48}, {0x25, 52}, {0x26, 56}, {0x27, 60},
+	{0x8, 64}, {0x9, 72}, {0xa, 80}, {0xb, 88},
+	{0x28, 96}, {0x29, 104}, {0x2a, 112}, {0x2b, 120},
+	{0xc, 128}, {0xd, 144}, {0xe, 160}, {0xf, 176},
+	{0x2c, 192}, {0x2d, 208}, {0x2e, 224}, {0x2f, 240},
+	{0x10, 256}, {0x11, 288}, {0x12, 320}, {0x13, 352},
+	{0x30, 384}, {0x31, 416}, {0x32, 448}, {0x33, 480},
+	{0x14, 512}, {0x15, 576}, {0x16, 640}, {0x17, 704},
+	{0x34, 768}, {0x35, 832}, {0x36, 896}, {0x37, 960},
+	{0x18, 1024}, {0x19, 1152}, {0x1a, 1280}, {0x1b, 1408},
+	{0x38, 1536}, {0x39, 1664}, {0x3a, 1792}, {0x3b, 1920},
+	{0x1c, 2048}, {0x1d, 2304}, {0x1e, 2560}, {0x1f, 2816},
+	{0x3c, 3072}, {0x3d, 3328}, {0x3E, 3584}, {0x3F, 3840},
+	{-1, -1}
+};
+
+static struct i2c_adapter *adap;
+
+extern void gpio_i2c_hs_inactive(void);
+extern void gpio_i2c_hs_active(void);
+
+static u16 reg_read(mxc_i2c_hs *i2c_hs, u32 reg_offset)
+{
+	return __raw_readw(i2c_hs->reg_base_virt + reg_offset);
+}
+
+static void reg_write(mxc_i2c_hs *i2c_hs, u32 reg_offset, u16 data)
+{
+	__raw_writew(data, i2c_hs->reg_base_virt + reg_offset);
+}
+
+static void reg_set_mask(mxc_i2c_hs *i2c_hs, u32 reg_offset, u16 mask)
+{
+	u16 value;
+
+	value = reg_read(i2c_hs, reg_offset);
+	value |= mask;
+	reg_write(i2c_hs, reg_offset, value);
+}
+static void reg_clear_mask(mxc_i2c_hs *i2c_hs, u32 reg_offset, u16 mask)
+{
+	u16 value;
+
+	value = reg_read(i2c_hs, reg_offset);
+	value &= ~mask;
+	reg_write(i2c_hs, reg_offset, value);
+}
+
+static void mxci2c_hs_set_div(mxc_i2c_hs *i2c_hs)
+{
+	unsigned long clk_freq;
+	int i;
+	int div = -1;;
+
+	clk_freq = clk_get_rate(i2c_hs->serial_clk);
+	if (i2c_hs->speed) {
+		div = (clk_freq + i2c_hs->speed - 1) / i2c_hs->speed;
+		for (i = 0; i2c_clk_table[i].div >= 0; i++) {
+			if (i2c_clk_table[i].div >= div) {
+				div = i2c_clk_table[i].reg_value;
+				reg_write(i2c_hs, HIFSFDR, div);
+				break;
+			}
+		}
+	}
+}
+
+static int mxci2c_hs_enable(mxc_i2c_hs *i2c_hs)
+{
+	gpio_i2c_hs_active();
+	clk_enable(i2c_hs->ipg_clk);
+	clk_enable(i2c_hs->serial_clk);
+	mxci2c_hs_set_div(i2c_hs);
+	reg_write(i2c_hs, HICR, reg_read(i2c_hs, HICR) | HICR_HIEN);
+
+	return 0;
+}
+
+static int mxci2c_hs_disable(mxc_i2c_hs *i2c_hs)
+{
+	reg_write(i2c_hs, HICR, reg_read(i2c_hs, HICR) & (~HICR_HIEN));
+	clk_disable(i2c_hs->ipg_clk);
+	clk_disable(i2c_hs->serial_clk);
+
+	return 0;
+}
+
+static int mxci2c_hs_bus_busy(mxc_i2c_hs *i2c_hs)
+{
+	u16 value;
+	int retry = 1000;
+
+	while (retry--) {
+		value = reg_read(i2c_hs, HISR);
+		if (value & HISR_HIBB) {
+			udelay(1);
+		} else {
+			break;
+		}
+	}
+
+	if (retry <= 0) {
+		dev_dbg(NULL, "%s: Bus Busy!\n", __func__);
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+static int mxci2c_hs_start(mxc_i2c_hs *i2c_hs, int repeat_start, u16 address)
+{
+	u16 mask;
+	int ret = 0;
+
+	mxci2c_hs_bus_busy(i2c_hs);
+
+	/*7 bit address */
+	reg_clear_mask(i2c_hs, HICR, HICR_ADDR_MODE);
+
+	/*send start */
+	if (repeat_start)
+		mask = HICR_RSTA;
+	else
+		mask = HICR_MSTA;
+	reg_set_mask(i2c_hs, HICR, mask);
+
+	return ret;
+}
+
+static int mxci2c_hs_stop(mxc_i2c_hs *i2c_hs)
+{
+	reg_clear_mask(i2c_hs, HICR, HICR_MSTA);
+	reg_clear_mask(i2c_hs, HICR, HICR_HIIEN);
+
+	return 0;
+}
+
+static int mxci2c_wait_writefifo(mxc_i2c_hs *i2c_hs)
+{
+	int i, num, left;
+	int retry, ret = 0;
+
+	retry = 10000;
+	while (retry--) {
+		udelay(10);
+		if (reg_read(i2c_hs, HISR) & (HISR_TDE | HISR_TDC_ZERO)) {
+			if (i2c_hs->index < i2c_hs->msg->len) {
+				left = i2c_hs->msg->len - i2c_hs->index;
+				num =
+				    (left >
+				     HITFR_MAX_COUNT) ? HITFR_MAX_COUNT : left;
+				for (i = 0; i < num; i++) {
+					reg_write(i2c_hs, HITDR,
+						  i2c_hs->msg->buf[i2c_hs->
+								   index + i]);
+				}
+				i2c_hs->index += num;
+			} else {
+				if (reg_read(i2c_hs, HISR) & HISR_TDC_ZERO) {
+					msleep(1);
+					break;
+				}
+			}
+		}
+	}
+
+	if (retry <= 0) {
+		printk(KERN_ERR "%s:wait error\n", __func__);
+		ret = -1;
+	}
+
+	return ret;
+}
+
+static int mxci2c_wait_readfifo(mxc_i2c_hs *i2c_hs)
+{
+	int i, num, left;
+	int retry, ret = 0;
+	u16 value;
+
+	retry = 10000;
+	while (retry--) {
+		udelay(10);
+		value = reg_read(i2c_hs, HISR);
+		if (value & (HISR_RDF | HISR_RDC_ZERO)) {
+			if (i2c_hs->index < i2c_hs->msg->len) {
+				left = i2c_hs->msg->len - i2c_hs->index;
+				num =
+				    (left >
+				     HITFR_MAX_COUNT) ? HITFR_MAX_COUNT : left;
+				for (i = 0; i < num; i++) {
+					i2c_hs->msg->buf[i2c_hs->index + i] =
+					    reg_read(i2c_hs, HIRDR);
+				}
+				i2c_hs->index += num;
+			} else {
+				if (value & HISR_RDC_ZERO) {
+					break;
+				}
+			}
+		}
+	}
+
+	if (retry <= 0) {
+		printk(KERN_ERR "%s:wait error\n", __func__);
+		ret = -1;
+	}
+
+	return ret;
+}
+
+static int mxci2c_hs_read(mxc_i2c_hs *i2c_hs, int repeat_start,
+			  struct i2c_msg *msg)
+{
+	int ret;
+
+	if (msg->len > HIRDCR_MAX_COUNT) {
+		printk(KERN_ERR "%s: error: msg too long, max longth 256\n",
+		       __func__);
+		return -1;
+	}
+
+	ret = 0;
+	i2c_hs->msg = msg;
+	i2c_hs->index = 0;
+
+	/*set address */
+	reg_write(i2c_hs, HIMADR, HIMADR_LSB_ADR(msg->addr));
+
+	/*receive mode */
+	reg_clear_mask(i2c_hs, HICR, HICR_MTX);
+
+	reg_clear_mask(i2c_hs, HICR, HICR_HIIEN);
+
+	 /*FIFO*/ reg_set_mask(i2c_hs, HIRFR, HIRFR_RFEN | HIRFR_RFWM(7));
+	reg_set_mask(i2c_hs, HIRFR, HIRFR_RFLSH);
+
+	 /*TDCR*/
+	    reg_write(i2c_hs, HIRDCR, HIRDCR_RDC_EN | HIRDCR_RDC(msg->len));
+
+	mxci2c_hs_start(i2c_hs, repeat_start, msg->addr);
+
+	ret = mxci2c_wait_readfifo(i2c_hs);
+
+	if (ret < 0)
+		return ret;
+	else
+		return msg->len;
+}
+
+static int mxci2c_hs_write(mxc_i2c_hs *i2c_hs, int repeat_start,
+			   struct i2c_msg *msg)
+{
+	int ret, i;
+
+	if (msg->len > HITDCR_MAX_COUNT) {
+		printk(KERN_ERR "%s: error: msg too long, max longth 256\n",
+		       __func__);
+		return -1;
+	}
+
+	ret = 0;
+	i2c_hs->msg = msg;
+	i2c_hs->index = 0;
+
+	/*set address */
+	reg_write(i2c_hs, HIMADR, HIMADR_LSB_ADR(msg->addr));
+
+	/*transmit mode */
+	reg_set_mask(i2c_hs, HICR, HICR_MTX);
+
+	reg_clear_mask(i2c_hs, HICR, HICR_HIIEN);
+
+	/* TDCR */
+	reg_write(i2c_hs, HITDCR, HITDCR_TDC_EN | HITDCR_TDC(msg->len));
+
+	/* FIFO */
+	reg_set_mask(i2c_hs, HITFR, HITFR_TFEN);
+	reg_set_mask(i2c_hs, HITFR, HITFR_TFLSH);
+
+	if (msg->len > HITFR_MAX_COUNT)
+		i2c_hs->index = HITFR_MAX_COUNT;
+	else {
+		i2c_hs->index = msg->len;
+	}
+
+	for (i = 0; i < i2c_hs->index; i++) {
+		reg_write(i2c_hs, HITDR, msg->buf[i]);
+	}
+
+	mxci2c_hs_start(i2c_hs, repeat_start, msg->addr);
+
+	ret = mxci2c_wait_writefifo(i2c_hs);
+
+	if (ret < 0)
+		return ret;
+	else
+		return msg->len;
+}
+
+static int mxci2c_hs_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
+			  int num)
+{
+	int i;
+	int ret = -EIO;
+
+	mxc_i2c_hs *i2c_hs = (mxc_i2c_hs *) (i2c_get_adapdata(adap));
+
+	if (i2c_hs->low_power) {
+		dev_err(&adap->dev, "I2C Device in low power mode\n");
+		return -EREMOTEIO;
+	}
+
+	if (num < 1) {
+		return 0;
+	}
+
+	mxci2c_hs_enable(i2c_hs);
+
+	for (i = 0; i < num; i++) {
+		if (msgs[i].flags & I2C_M_RD) {
+			ret = mxci2c_hs_read(i2c_hs, 0, &msgs[i]);
+			if (ret < 0)
+				break;
+		} else {
+			ret = mxci2c_hs_write(i2c_hs, 0, &msgs[i]);
+			if (ret < 0)
+				break;
+		}
+		mxci2c_hs_stop(i2c_hs);
+	}
+	mxci2c_hs_stop(i2c_hs);
+
+	mxci2c_hs_disable(i2c_hs);
+
+	if (ret < 0)
+		return ret;
+
+	return i;
+}
+
+static u32 mxci2c_hs_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+/*!
+ * Stores the pointers for the i2c algorithm functions. The algorithm functions
+ * is used by the i2c bus driver to talk to the i2c bus
+ */
+static struct i2c_algorithm mxci2c_hs_algorithm = {
+	.master_xfer = mxci2c_hs_xfer,
+	.functionality = mxci2c_hs_func
+};
+
+static int mxci2c_hs_probe(struct platform_device *pdev)
+{
+	mxc_i2c_hs *i2c_hs;
+	struct mxc_i2c_platform_data *i2c_plat_data = pdev->dev.platform_data;
+	struct resource *res;
+	int id = pdev->id;
+	int ret = 0;
+
+	i2c_hs = kzalloc(sizeof(mxc_i2c_hs), GFP_KERNEL);
+	if (!i2c_hs) {
+		return -ENOMEM;
+	}
+
+	i2c_hs->dev = &pdev->dev;
+
+	i2c_hs->speed = i2c_plat_data->i2c_clk;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		ret = -ENODEV;
+		goto err1;
+	}
+	i2c_hs->reg_base_virt = IO_ADDRESS(res->start);
+	i2c_hs->reg_base_phy = res->start;
+
+	i2c_hs->ipg_clk = clk_get(&pdev->dev, "hsi2c_clk");
+	i2c_hs->serial_clk = clk_get(&pdev->dev, "hsi2c_serial_clk");
+
+	/*
+	 * Request the I2C interrupt
+	 */
+	i2c_hs->irq = platform_get_irq(pdev, 0);
+	if (i2c_hs->irq < 0) {
+		ret = i2c_hs->irq;
+		goto err1;
+	}
+
+	i2c_hs->low_power = false;
+
+	/*
+	 * Set the adapter information
+	 */
+	adap = kzalloc(sizeof(struct i2c_adapter), GFP_KERNEL);
+	if (!adap) {
+		ret = -ENODEV;
+		goto err1;
+	}
+	strlcpy(adap->name, pdev->name, 48);
+	adap->id = adap->nr = id;
+	adap->algo = &mxci2c_hs_algorithm;
+	adap->timeout = 1;
+	platform_set_drvdata(pdev, i2c_hs);
+	i2c_set_adapdata(adap, i2c_hs);
+	ret = i2c_add_numbered_adapter(adap);
+	if (ret < 0) {
+		goto err2;
+	}
+
+	printk(KERN_INFO "MXC HS I2C driver\n");
+	return 0;
+
+      err2:
+	kfree(adap);
+      err1:
+	dev_err(&pdev->dev, "failed to probe high speed i2c adapter\n");
+	kfree(i2c_hs);
+	return ret;
+}
+
+static int mxci2c_hs_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	mxc_i2c_hs *i2c_hs = platform_get_drvdata(pdev);
+
+	if (i2c_hs == NULL) {
+		return -1;
+	}
+
+	/* Prevent further calls to be processed */
+	i2c_hs->low_power = true;
+
+	gpio_i2c_hs_inactive();
+
+	return 0;
+}
+
+static int mxci2c_hs_resume(struct platform_device *pdev)
+{
+	mxc_i2c_hs *i2c_hs = platform_get_drvdata(pdev);
+
+	if (i2c_hs == NULL)
+		return -1;
+
+	i2c_hs->low_power = false;
+	gpio_i2c_hs_active();
+
+	return 0;
+}
+
+static int mxci2c_hs_remove(struct platform_device *pdev)
+{
+	mxc_i2c_hs *i2c_hs = platform_get_drvdata(pdev);
+
+	i2c_del_adapter(adap);
+	gpio_i2c_hs_inactive();
+	platform_set_drvdata(pdev, NULL);
+	kfree(i2c_hs);
+	return 0;
+}
+
+static struct platform_driver mxci2c_hs_driver = {
+	.driver = {
+		   .name = "mxc_i2c_hs",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = mxci2c_hs_probe,
+	.remove = mxci2c_hs_remove,
+	.suspend = mxci2c_hs_suspend,
+	.resume = mxci2c_hs_resume,
+};
+
+/*!
+ * Function requests the interrupts and registers the i2c adapter structures.
+ *
+ * @return The function returns 0 on success and a non-zero value on failure.
+ */
+static int __init mxci2c_hs_init(void)
+{
+	/* Register the device driver structure. */
+	return platform_driver_register(&mxci2c_hs_driver);
+}
+
+/*!
+ * This function is used to cleanup all resources before the driver exits.
+ */
+static void __exit mxci2c_hs_exit(void)
+{
+	platform_driver_unregister(&mxci2c_hs_driver);
+}
+
+subsys_initcall(mxci2c_hs_init);
+module_exit(mxci2c_hs_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC HIGH SPEED I2C driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/busses/mxc_i2c_hs_reg.h b/drivers/i2c/busses/mxc_i2c_hs_reg.h
new file mode 100644
index 000000000000..fe6bb9a8f1ff
--- /dev/null
+++ b/drivers/i2c/busses/mxc_i2c_hs_reg.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright 2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __MXC_I2C_HS_REG_H__
+#define __MXC_I2C_HS_REG_H__
+
+#define	HISADR        0x00
+
+#define	HIMADR        0x04
+#define	HIMADR_LSB_ADR(x) ((x) << 1)
+#define	HIMADR_MSB_ADR(x) (((x) & 0x7) << 8)
+
+#define	HICR          0x08
+#define	HICR_HIEN	0x1
+#define	HICR_DMA_EN_RX 0x2
+#define	HICR_DMA_EN_TR 0x4
+#define	HICR_RSTA 0x8
+#define	HICR_TXAK 0x10
+#define	HICR_MTX 0x20
+#define	HICR_MSTA 0x40
+#define	HICR_HIIEN 0x80
+#define	HICR_ADDR_MODE 0x100
+#define	HICR_MST_CODE(x) (((x)&0x7) << 9)
+#define	HICR_HSM_EN 0x1000
+#define	HICR_SAMC(x) (((x)&0x3) << 13)
+#define 	SAMC_7_10 	0
+#define	SMAC_7		1
+#define	SMAC_10		2
+
+#define  HISR          0x0c
+#define 	HISR_RDF		0x1
+#define 	HISR_TDE		0x2
+#define 	HISR_HIAAS	0x4
+#define 	HISR_HIAL	0x8
+#define 	HISR_BTD		0x10
+#define 	HISR_RDC_ZERO	0x20
+#define 	HISR_TDC_ZERO	0x40
+#define 	HISR_RXAK		0x80
+#define 	HISR_HIBB		0x100
+#define 	HISR_SRW		0x200
+#define 	HISR_SADDR_MODE	0x400
+#define 	HISR_SHS_MODE	0x800
+
+#define  HIIMR         0x10
+#define  	HIIMR_RDF	0x1
+#define  	HIIMR_TDE	0x2
+#define  	HIIMR_AAS	0x4
+#define  	HIIMR_AL	0x8
+#define  	HIIMR_BTD	0x10
+#define  	HIIMR_RDC	0x20
+#define  	HIIMR_TDC	0x40
+#define  	HIIMR_RXAK	0x80
+
+#define  HITDR         0x14
+
+#define  HIRDR         0x18
+
+#define  HIFSFDR       0x1c
+
+#define  HIHSFDR       0x20
+
+#define  HITFR         0x24
+#define	HITFR_TFEN		0x1
+#define	HITFR_TFLSH	0x2
+#define	HITFR_TFWM(x) (((x) & 0x7) << 2)
+#define	HITFR_TFC(x)	(((x) >> 8) & 0xF)
+#define	HITFR_MAX_COUNT 8
+
+#define  HIRFR         0x28
+#define  	HIRFR_RFEN			0x1
+#define  	HIRFR_RFLSH		0x2
+#define  	HIRFR_RFWM(x)	 	(((x) & 0x7) << 2)
+#define  	HIRFR_RFC(x) 		(((x) >> 8) & 0xF)
+#define	HIRFR_MAX_COUNT 8
+
+#define  HITDCR        0x2c
+#define	HITDCR_TDC(x)		((x) & 0xFF)
+#define	HITDCR_TDC_EN	0x100
+#define	HITDCR_TDC_RSTA	0x200
+#define	HITDCR_MAX_COUNT 0xFF
+
+#define  HIRDCR        0x30
+#define	HIRDCR_RDC(x)		((x) & 0xFF)
+#define	HIRDCR_RDC_EN	0x100
+#define	HIRDCR_RDC_RSTA	0x200
+#define	HIRDCR_MAX_COUNT 0xFF
+
+#endif
diff --git a/drivers/i2c/busses/mxc_i2c_reg.h b/drivers/i2c/busses/mxc_i2c_reg.h
new file mode 100644
index 000000000000..4fc76c8aa811
--- /dev/null
+++ b/drivers/i2c/busses/mxc_i2c_reg.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __MXC_I2C_REG_H__
+#define __MXC_I2C_REG_H__
+
+/* Address offsets of the I2C registers */
+#define MXC_IADR                0x00	/* Address Register */
+#define MXC_IFDR                0x04	/* Freq div register */
+#define MXC_I2CR                0x08	/* Control regsiter */
+#define MXC_I2SR                0x0C	/* Status register */
+#define MXC_I2DR                0x10	/* Data I/O register */
+
+/* Bit definitions of I2CR */
+#define MXC_I2CR_IEN            0x0080
+#define MXC_I2CR_IIEN           0x0040
+#define MXC_I2CR_MSTA           0x0020
+#define MXC_I2CR_MTX            0x0010
+#define MXC_I2CR_TXAK           0x0008
+#define MXC_I2CR_RSTA           0x0004
+
+/* Bit definitions of I2SR */
+#define MXC_I2SR_ICF            0x0080
+#define MXC_I2SR_IAAS           0x0040
+#define MXC_I2SR_IBB            0x0020
+#define MXC_I2SR_IAL            0x0010
+#define MXC_I2SR_SRW            0x0004
+#define MXC_I2SR_IIF            0x0002
+#define MXC_I2SR_RXAK           0x0001
+
+#endif				/* __MXC_I2C_REG_H__ */
diff --git a/drivers/ide/Kconfig b/drivers/ide/Kconfig
index e6857e01d1ba..79d216fa3919 100644
--- a/drivers/ide/Kconfig
+++ b/drivers/ide/Kconfig
@@ -749,6 +749,13 @@ config BLK_DEV_IDE_RAPIDE
 	  Say Y here if you want to support the Yellowstone RapIDE controller
 	  manufactured for use with Acorn computers.
 
+config BLK_DEV_IDE_S3C2443
+	tristate "S3C2443 ATA driver support"
+	depends on ARM && CPU_S3C2443
+	help
+	  Say Y here if you want to support the onboard IDE interface on 
+	  S3C2443 boards
+
 config IDE_H8300
 	tristate "H8300 IDE support"
 	depends on H8300
diff --git a/drivers/ide/Makefile b/drivers/ide/Makefile
index 7818d402b188..67be799e6c80 100644
--- a/drivers/ide/Makefile
+++ b/drivers/ide/Makefile
@@ -107,6 +107,7 @@ obj-$(CONFIG_BLK_DEV_PLATFORM)		+= ide_platform.o
 
 obj-$(CONFIG_BLK_DEV_IDE_ICSIDE)	+= icside.o
 obj-$(CONFIG_BLK_DEV_IDE_RAPIDE)	+= rapide.o
+obj-$(CONFIG_BLK_DEV_IDE_S3C2443)	+= s3c2443-ide.o
 obj-$(CONFIG_BLK_DEV_PALMCHIP_BK3710)	+= palm_bk3710.o
 
 obj-$(CONFIG_BLK_DEV_IDE_AU1XXX)	+= au1xxx-ide.o
diff --git a/drivers/ide/s3c2443-ide.c b/drivers/ide/s3c2443-ide.c
new file mode 100644
index 000000000000..4de1a537fb4e
--- /dev/null
+++ b/drivers/ide/s3c2443-ide.c
@@ -0,0 +1,477 @@
+/* linux/drivers/ide/s3c2443-ide.c
+ *
+ * Copyright (c) 2009 Digi Internationa Inc.
+ * http://www.digi.com
+ * 
+ * Partially based on previous driver by Seung-Chull, Suh <sc.suh@samsung.com>
+ * Copyright(C) Samsung Electronics 2006
+ *
+ * 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/errno.h>
+#include <linux/ide.h>
+#include <linux/blkdev.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+
+#include <mach/map.h>
+#include <mach/gpio.h>
+#include <mach/regs-gpio.h>
+#include <mach/regs-gpioj.h>
+#include <mach/regs-bus.h>
+#include <mach/regs-cfata.h>
+#include <mach/gpio.h>
+
+#define DRV_NAME "s3c2443-ide"
+
+MODULE_AUTHOR("Pedro Perez de Heredia <pedro.perez@digi.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("S3C2443 IDE driver");
+
+struct  _s3c2443_ide_hwif {
+	ide_hwif_t		*hwif;
+	int			irq;
+	void __iomem		*membase;
+	struct platform_device	*dev;
+	struct clk		*clk;
+};
+
+static struct _s3c2443_ide_hwif s3c2443_ide_hwif;
+
+static inline void s3c2443_wait_until_ready(void)
+{
+	u32 i, reg;
+
+	for (i = 0; i < 100000; i++) {
+		reg = readl(s3c2443_ide_hwif.membase + S3C2443_ATA_FIFO_STATUS);
+		if ((reg >> 28) == 0)
+			return;
+	}
+}
+
+static __inline__ u8 s3c2443_ide_readb(unsigned long port)
+{
+	s3c2443_wait_until_ready();
+	(void)readb(port);
+	s3c2443_wait_until_ready();
+	return readb(s3c2443_ide_hwif.membase + S3C2443_ATA_PIO_RDATA);
+}
+
+static __inline__ u16 s3c2443_ide_readw(unsigned long port)
+{
+	s3c2443_wait_until_ready();
+	(void)readw(port);
+	s3c2443_wait_until_ready();
+	return readw(s3c2443_ide_hwif.membase + S3C2443_ATA_PIO_RDATA);
+}
+
+static __inline__ void s3c2443_ide_writeb(u8 val, unsigned long port)
+{
+	s3c2443_wait_until_ready();
+	writeb(val, port);
+}
+
+static __inline__ void s3c2443_ide_writew(u16 val, unsigned long port)
+{
+	s3c2443_wait_until_ready();
+	writew(val, port);
+}
+
+static void s3c2443_ide_exec_command(ide_hwif_t *hwif, u8 cmd)
+{
+	s3c2443_ide_writeb(cmd, hwif->io_ports.command_addr);
+}
+
+static u8 s3c2443_read_status(ide_hwif_t *hwif)
+{
+	return s3c2443_ide_readb(hwif->io_ports.status_addr);
+}
+
+static u8 s3c2443_read_altstatus(ide_hwif_t *hwif)
+{
+	return s3c2443_ide_readb(hwif->io_ports.ctl_addr);
+}
+
+static void s3c2443_set_irq(ide_hwif_t *hwif, int on)
+{
+	u8 ctl = ATA_DEVCTL_OBS;
+
+	if (on == 4) { /* hack for SRST */
+		ctl |= 4;
+		on &= ~4;
+	}
+
+	ctl |= on ? 0 : 2;
+	s3c2443_ide_writeb(ctl, hwif->io_ports.ctl_addr);
+}
+
+static void s3c2443_tf_load(ide_drive_t *drive, ide_task_t *task)
+{
+	ide_hwif_t *hwif = drive->hwif;
+	struct ide_io_ports *io_ports = &hwif->io_ports;
+	struct ide_taskfile *tf = &task->tf;
+	u8 HIHI = (task->tf_flags & IDE_TFLAG_LBA48) ? 0xE0 : 0xEF;
+
+	if (task->tf_flags & IDE_TFLAG_FLAGGED)
+		HIHI = 0xFF;
+
+	if (task->tf_flags & IDE_TFLAG_OUT_DATA) {
+		u16 data = (tf->hob_data << 8) | tf->data;
+		s3c2443_ide_writew (data, io_ports->data_addr);
+	}
+
+	if (task->tf_flags & IDE_TFLAG_OUT_HOB_FEATURE)
+		s3c2443_ide_writeb(tf->hob_feature, io_ports->feature_addr);
+	if (task->tf_flags & IDE_TFLAG_OUT_HOB_NSECT)
+		s3c2443_ide_writeb(tf->hob_nsect, io_ports->nsect_addr);
+	if (task->tf_flags & IDE_TFLAG_OUT_HOB_LBAL)
+		s3c2443_ide_writeb(tf->hob_lbal, io_ports->lbal_addr);
+	if (task->tf_flags & IDE_TFLAG_OUT_HOB_LBAM)
+		s3c2443_ide_writeb(tf->hob_lbam, io_ports->lbam_addr);
+	if (task->tf_flags & IDE_TFLAG_OUT_HOB_LBAH)
+		s3c2443_ide_writeb(tf->hob_lbah, io_ports->lbah_addr);
+
+	if (task->tf_flags & IDE_TFLAG_OUT_FEATURE)
+		s3c2443_ide_writeb(tf->feature, io_ports->feature_addr);
+	if (task->tf_flags & IDE_TFLAG_OUT_NSECT)
+		s3c2443_ide_writeb(tf->nsect, io_ports->nsect_addr);
+	if (task->tf_flags & IDE_TFLAG_OUT_LBAL)
+		s3c2443_ide_writeb(tf->lbal, io_ports->lbal_addr);
+	if (task->tf_flags & IDE_TFLAG_OUT_LBAM)
+		s3c2443_ide_writeb(tf->lbam, io_ports->lbam_addr);
+	if (task->tf_flags & IDE_TFLAG_OUT_LBAH)
+		s3c2443_ide_writeb(tf->lbah, io_ports->lbah_addr);
+
+	if (task->tf_flags & IDE_TFLAG_OUT_DEVICE)
+		s3c2443_ide_writeb((tf->device & HIHI) | drive->select,
+			 io_ports->device_addr);
+}
+
+static void s3c2443_tf_read(ide_drive_t *drive, ide_task_t *task)
+{
+	ide_hwif_t *hwif = drive->hwif;
+	struct ide_io_ports *io_ports = &hwif->io_ports;
+	struct ide_taskfile *tf = &task->tf;
+
+	if (task->tf_flags & IDE_TFLAG_IN_DATA) {
+		u16 data;
+
+		data = s3c2443_ide_readw(io_ports->data_addr);
+
+		tf->data = data & 0xff;
+		tf->hob_data = (data >> 8) & 0xff;
+	}
+
+	/* be sure we're looking at the low order bits */
+	s3c2443_ide_writeb(ATA_DEVCTL_OBS & ~0x80, io_ports->ctl_addr);
+
+	if (task->tf_flags & IDE_TFLAG_IN_FEATURE)
+		tf->feature = s3c2443_ide_readb(io_ports->feature_addr);
+	if (task->tf_flags & IDE_TFLAG_IN_NSECT)
+		tf->nsect  = s3c2443_ide_readb(io_ports->nsect_addr);
+	if (task->tf_flags & IDE_TFLAG_IN_LBAL)
+		tf->lbal   = s3c2443_ide_readb(io_ports->lbal_addr);
+	if (task->tf_flags & IDE_TFLAG_IN_LBAM)
+		tf->lbam   = s3c2443_ide_readb(io_ports->lbam_addr);
+	if (task->tf_flags & IDE_TFLAG_IN_LBAH)
+		tf->lbah   = s3c2443_ide_readb(io_ports->lbah_addr);
+	if (task->tf_flags & IDE_TFLAG_IN_DEVICE)
+		tf->device = s3c2443_ide_readb(io_ports->device_addr);
+
+	if (task->tf_flags & IDE_TFLAG_LBA48) {
+		s3c2443_ide_writeb(ATA_DEVCTL_OBS | 0x80, io_ports->ctl_addr);
+
+		if (task->tf_flags & IDE_TFLAG_IN_HOB_FEATURE)
+			tf->hob_feature = s3c2443_ide_readb(io_ports->feature_addr);
+		if (task->tf_flags & IDE_TFLAG_IN_HOB_NSECT)
+			tf->hob_nsect   = s3c2443_ide_readb(io_ports->nsect_addr);
+		if (task->tf_flags & IDE_TFLAG_IN_HOB_LBAL)
+			tf->hob_lbal    = s3c2443_ide_readb(io_ports->lbal_addr);
+		if (task->tf_flags & IDE_TFLAG_IN_HOB_LBAM)
+			tf->hob_lbam    = s3c2443_ide_readb(io_ports->lbam_addr);
+		if (task->tf_flags & IDE_TFLAG_IN_HOB_LBAH)
+			tf->hob_lbah    = s3c2443_ide_readb(io_ports->lbah_addr);
+	}
+}
+
+static void s3c2443_input_data(ide_drive_t *drive, struct request *rq, void *buf,
+		    unsigned int len)
+{
+	ide_hwif_t *hwif = drive->hwif;
+	struct ide_io_ports *io_ports = &hwif->io_ports;
+	u16 *buffer = (u16 *)buf;
+
+	/* round up, it should be power of 2 */
+	len = (len + 1) >> 1;
+
+	while (len--) {
+		*buffer++ = s3c2443_ide_readw(io_ports->data_addr);
+	}
+}
+
+static void s3c2443_output_data(ide_drive_t *drive, struct request *rq, void *buf,
+		     unsigned int len)
+{
+	ide_hwif_t *hwif = drive->hwif;
+	struct ide_io_ports *io_ports = &hwif->io_ports;
+	u16 *buffer = (u16 *)buf;
+
+	len >>= 1;
+
+	while (len--) {
+		s3c2443_ide_writew(*buffer++, io_ports->data_addr);
+	}
+}
+
+static const struct ide_tp_ops s3c2443_tp_ops = {
+	.exec_command		= s3c2443_ide_exec_command,
+	.read_status		= s3c2443_read_status,
+	.read_altstatus		= s3c2443_read_altstatus,
+ 	.set_irq		= s3c2443_set_irq,
+ 	.tf_load		= s3c2443_tf_load,
+ 	.tf_read		= s3c2443_tf_read,
+	.input_data		= s3c2443_input_data,
+	.output_data		= s3c2443_output_data,
+};
+
+static const struct ide_port_info s3c2443_port_info = {
+	.tp_ops 		= &s3c2443_tp_ops,
+	.host_flags		= IDE_HFLAG_MMIO | IDE_HFLAG_NO_DMA | \
+				  IDE_HFLAG_NO_IO_32BIT,
+};
+
+static int s3c2443_ide_ack_intr(ide_hwif_t *hwif)
+{
+	u32 reg = 0, tout = 100;
+	
+	while (tout--) {
+		reg = readl(s3c2443_ide_hwif.membase + S3C2443_ATA_IRQ);
+		if (reg) {
+			writel(reg, s3c2443_ide_hwif.membase + S3C2443_ATA_IRQ);
+			break;
+		}
+	}
+
+	return (int)reg;
+}
+
+static int s3c2443_ide_hw_config(struct _s3c2443_ide_hwif *hw)
+{
+	int ret = 0;
+	u32 reg;
+	void __iomem *ebicon;
+
+	ebicon = ioremap(S3C2443_PA_EBI + 0x8, 0x4);
+	if (ebicon == NULL) {
+		ret =-EBUSY;
+		goto err_unmap;
+	}
+
+	/* Configure EBI bank 2 and 3 for the CF interface */
+	reg = readl(ebicon);
+	reg |= S3C2443_EBICON_BANK3_CFG | S3C2443_EBICON_BANK2_CFG;
+	writel(reg, ebicon);
+
+	/* Configure the IO lines*/
+	s3c2443_gpio_cfgpin(S3C2410_GPG15, S3C2443_GPG15_CF_PWR);
+	s3c2443_gpio_cfgpin(S3C2410_GPG14, S3C2443_GPG14_CF_RESET);
+	s3c2443_gpio_cfgpin(S3C2410_GPG13, S3C2443_GPG13_CF_nREG);
+	s3c2443_gpio_cfgpin(S3C2410_GPG12, S3C2443_GPG12_nINPACK);
+	s3c2443_gpio_cfgpin(S3C2410_GPG11, S3C2443_GPG11_CF_nIREQ);
+
+ 	s3c2443_gpio_cfgpin(S3C2410_GPA10, 0);
+ 	s3c2443_gpio_cfgpin(S3C2410_GPA11, 1);
+ 	s3c2443_gpio_cfgpin(S3C2410_GPA12, 1);
+ 	s3c2443_gpio_cfgpin(S3C2410_GPA15, 1);
+
+	/* Clear the card detect condition */
+	reg = readl(S3C24XX_MISCCR) & ~S3C2443_MISCCR_nCD_CF;
+	writel(reg, S3C24XX_MISCCR);
+
+	/* Output Port disabled, Card power off, ATA mode */
+	writel(0x07, hw->membase + S3C2443_MUX_REG);
+	mdelay(10);
+	/* Output Port enable, Card power off, ATA mode */
+	writel(0x03, hw->membase + S3C2443_MUX_REG);
+	mdelay(10);
+	/* Output Port enable, Card power on, ATA mode */
+	writel(0x01, hw->membase + S3C2443_MUX_REG);
+	mdelay(500);	/* wait for 500ms */
+
+	writel(0x1C238, hw->membase + S3C2443_ATA_PIO_TIME);
+	writel(0x20B1362, hw->membase + S3C2443_ATA_UDMA_TIME);
+
+	/* Enable ATA */
+	reg = readl(hw->membase + S3C2443_ATA_CONTROL);
+	writel(reg | 0x1, hw->membase + S3C2443_ATA_CONTROL);
+
+	mdelay(200);
+	/* remove IRQ Status and enable only ATA device interrupt */
+	writel(0x1f, hw->membase + S3C2443_ATA_IRQ);
+	writel(0x1b, hw->membase + S3C2443_ATA_IRQ_MASK);
+
+	mdelay(200);
+
+err_unmap:
+	iounmap(ebicon);
+	return ret;
+}
+
+
+static int __devinit s3c2443_ide_probe(struct platform_device *pdev)
+{
+	int ret = 0, i;
+	struct _s3c2443_ide_hwif *shwif = &s3c2443_ide_hwif;
+	hw_regs_t hw, *hws[] = {&hw, NULL, NULL, NULL};
+	struct resource *res;
+	struct ide_host *host;
+
+	memset(&s3c2443_ide_hwif, 0, sizeof(struct _s3c2443_ide_hwif));
+
+	shwif->dev = pdev;
+	
+	/* find and map our resources */
+	shwif->irq = platform_get_irq(pdev, 0);
+	if (shwif->irq < 0) {
+		pr_debug("%s: %s, can not get IORESOURCE_IRQ\n", DRV_NAME, __func__);
+		ret =-ENOENT;
+		goto error;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		pr_debug("%s: %s, can not get IORESOURCE_MEM\n", DRV_NAME, __func__);
+		ret = -ENOENT;
+		goto error;
+	}
+
+	if (!request_mem_region(res->start, res->end - res->start + 1,
+				pdev->name)) {
+		pr_debug("%s: %s,request_mem_region failed\n", DRV_NAME, __func__);
+		ret = -EBUSY;
+		goto error;
+	}
+
+	shwif->membase = ioremap(res->start, res->end - res->start + 1);
+	if (shwif->membase == NULL) {
+		pr_debug("%s: %s, can not map IO space at 0x%08x\n", DRV_NAME, __func__, res->start);
+		ret = -ENOMEM;
+		goto error_map;
+	}
+
+	shwif->clk = clk_get(&pdev->dev, "cfc");
+	if (IS_ERR(shwif->clk)) {
+		pr_debug("%s: %s, failed to find clock source\n", DRV_NAME, __func__);
+		ret = PTR_ERR(shwif->clk);
+		goto error_clock;
+	}
+
+	if ((ret = clk_enable(shwif->clk))) {
+		pr_debug("%s: %s, failed to enable clock\n", DRV_NAME, __func__);
+		goto error_clk_en;
+	}
+
+	ret = s3c2443_ide_hw_config(shwif);
+	if (ret) {
+		pr_debug("%s: %s, failed to configure IDE hardware\n", DRV_NAME, __func__);
+		goto error_hwcfg;
+	}
+
+	memset(&hw, 0, sizeof(hw));
+
+	for (i = 0; i < 9; i++)
+		hw.io_ports_array[i] = (unsigned long)(shwif->membase + S3C2443_ATA_PIO_DTR + (i * 4));
+	hw.io_ports.ctl_addr = (unsigned long)(shwif->membase + S3C2443_ATA_PIO_DTR + 8 * 4);
+	hw.irq = shwif->irq;
+	hw.ack_intr = s3c2443_ide_ack_intr;
+	hw.chipset = ide_generic;
+	hw.dev = &pdev->dev;
+
+	ret = ide_host_add(&s3c2443_port_info, hws, &host);
+	if (ret) {
+		pr_debug("%s: %s, failed add IDE host device\n", DRV_NAME, __func__);
+		goto error_add;
+	}
+
+	shwif->hwif = host->ports[0];
+	platform_set_drvdata(pdev, host);
+
+	printk(KERN_INFO "S3C2443 IDE driver\n");
+	return 0;
+
+error_add:
+	/* XXX unconfig hardware here ? */
+error_hwcfg:
+	clk_disable(shwif->clk);
+error_clk_en:
+	clk_put(shwif->clk);
+error_clock:
+	iounmap(shwif->membase);
+error_map:
+	release_mem_region(res->start, res->end - res->start + 1);
+error:
+	return ret;
+}
+
+static int __devexit s3c2443_ide_remove(struct platform_device *pdev)
+{
+	struct ide_host *host = dev_get_drvdata(&pdev->dev);
+	struct _s3c2443_ide_hwif *shwif = &s3c2443_ide_hwif;
+	struct resource *res;
+
+	ide_host_remove(host);
+
+	clk_disable(shwif->clk);
+	clk_put(shwif->clk);
+
+	iounmap(shwif->membase);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res != NULL) 
+		release_mem_region(res->start, res->end - res->start + 1);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int s3c2443_ide_resume(struct platform_device *dev)
+{
+	struct _s3c2443_ide_hwif *shwif = &s3c2443_ide_hwif;
+
+	return s3c2443_ide_hw_config(shwif);
+}
+#else
+# define s3c2443_ide_suspend NULL
+# define s3c2443_ide_resume  NULL
+#endif
+
+
+static struct platform_driver s3c2443_ide_driver = {
+	.probe		= s3c2443_ide_probe,
+	.remove		= __devexit_p(s3c2443_ide_remove),
+	.resume		= s3c2443_ide_resume,
+	.driver = {
+		.name	= "s3c2443-ide",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init s3c2443_ide_init(void)
+{
+	return platform_driver_register(&s3c2443_ide_driver);
+}
+
+static void __exit s3c2443_ide_exit(void)
+{
+	platform_driver_unregister(&s3c2443_ide_driver);
+}
+
+module_init(s3c2443_ide_init);
+module_exit(s3c2443_ide_exit);
+
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index efd70a974591..1db764a066ee 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -268,6 +268,13 @@ config KEYBOARD_PXA27x
 	  To compile this driver as a module, choose M here: the
 	  module will be called pxa27x_keypad.
 
+config KEYBOARD_MXC
+	tristate "MXC Keypad Driver"
+	depends on ARCH_MXC
+	help
+	  This is the Keypad driver for the Freescale MXC application
+	  processors.
+
 config KEYBOARD_AAED2000
 	tristate "AAED-2000 keyboard"
 	depends on MACH_AAED2000
@@ -314,6 +321,13 @@ config KEYBOARD_BFIN
 	  To compile this driver as a module, choose M here: the
 	  module will be called bf54x-keys.
 
+config KEYBOARD_MPR084
+	tristate "Freescale MPR084 Touch Keypad Driver"
+	depends on ARCH_MX37
+	help
+	  This is the Keypad driver for the Freescale Proximity Capacitive 
+	  Touch Sensor controller chip.
+
 config KEYBOARD_SH_KEYSC
 	tristate "SuperH KEYSC keypad support"
 	depends on SUPERH
@@ -321,6 +335,21 @@ config KEYBOARD_SH_KEYSC
 	  Say Y here if you want to use a keypad attached to the KEYSC block
 	  on SuperH processors such as sh7722 and sh7343.
 
+
 	  To compile this driver as a module, choose M here: the
 	  module will be called sh_keysc.
+
+config KEYBOARD_STMP3XXX
+	tristate "STMP3xxx keyboard"
+	depends on ARCH_STMP3XXX
+	help
+	 -to be written-
+
+
+config KEYBOARD_MC9S08DZ60
+	tristate "mc9s08dz60 keyboard"
+	depends on MXC_PMIC_MC9S08DZ60
+	help
+	 -to be written-
+
 endif
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index 0edc8f285d1c..c877b8bcfbea 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -18,6 +18,9 @@ obj-$(CONFIG_KEYBOARD_SPITZ)		+= spitzkbd.o
 obj-$(CONFIG_KEYBOARD_TOSA)		+= tosakbd.o
 obj-$(CONFIG_KEYBOARD_HIL)		+= hil_kbd.o
 obj-$(CONFIG_KEYBOARD_HIL_OLD)		+= hilkbd.o
+obj-$(CONFIG_KEYBOARD_MXC)		+= mxc_keyb.o
+obj-$(CONFIG_KEYBOARD_MPR084)		+= mpr084.o
+obj-$(CONFIG_KEYBOARD_STMP3XXX)		+= stmp3xxx-kbd.o
 obj-$(CONFIG_KEYBOARD_OMAP)		+= omap-keypad.o
 obj-$(CONFIG_KEYBOARD_PXA27x)		+= pxa27x_keypad.o
 obj-$(CONFIG_KEYBOARD_AAED2000)		+= aaed2000_kbd.o
@@ -27,3 +30,4 @@ obj-$(CONFIG_KEYBOARD_HP7XX)		+= jornada720_kbd.o
 obj-$(CONFIG_KEYBOARD_MAPLE)		+= maple_keyb.o
 obj-$(CONFIG_KEYBOARD_BFIN)		+= bf54x-keys.o
 obj-$(CONFIG_KEYBOARD_SH_KEYSC)		+= sh_keysc.o
+obj-$(CONFIG_KEYBOARD_MC9S08DZ60)	+= mc9s08dz60_keyb.o
diff --git a/drivers/input/keyboard/mc9s08dz60_keyb.c b/drivers/input/keyboard/mc9s08dz60_keyb.c
new file mode 100644
index 000000000000..8ec9f646c7c7
--- /dev/null
+++ b/drivers/input/keyboard/mc9s08dz60_keyb.c
@@ -0,0 +1,248 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc9s08dz60keyb.c
+ *
+ * @brief Driver for the Freescale Semiconductor MXC keypad port.
+ *
+ * The keypad driver is designed as a standard Input driver which interacts
+ * with low level keypad port hardware. Upon opening, the Keypad driver
+ * initializes the keypad port. When the keypad interrupt happens the driver
+ * calles keypad polling timer and scans the keypad matrix for key
+ * press/release. If all key press/release happened it comes out of timer and
+ * waits for key press interrupt. The scancode for key press and release events
+ * are passed to Input subsytem.
+ *
+ * @ingroup keypad
+ */
+
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/mm.h>
+#include <linux/init.h>
+#include <mach/hardware.h>
+#include <linux/kd.h>
+#include <linux/fs.h>
+#include <linux/kbd_kern.h>
+#include <linux/ioctl.h>
+#include <linux/poll.h>
+#include <linux/interrupt.h>
+#include <linux/timer.h>
+#include <linux/input.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/mfd/mc9s08dz60/pmic.h>
+#include <asm/mach/keypad.h>
+
+#define MOD_NAME "mc9s08dz60-keyb"
+/*
+ * Module header file
+ */
+
+/*! Input device structure. */
+static struct input_dev *mc9s08dz60kbd_dev;
+static unsigned int key_status;
+static int keypad_irq;
+static unsigned int key_code_map[8] = {
+	KEY_LEFT,
+	KEY_DOWN,
+	0,
+	0,
+	KEY_UP,
+	KEY_RIGHT,
+	0,
+	0,
+};
+static unsigned int keycodes_size = 8;
+
+static void read_key_handler(struct work_struct *work);
+static DECLARE_WORK(key_pad_event, read_key_handler);
+
+
+static void read_key_handler(struct work_struct *work)
+{
+	unsigned int val1, val2;
+	int pre_val, curr_val, i;
+	val1 = val2 = 0xff;
+	mcu_pmic_read_reg(REG_MCU_KPD_1, &val1, 0xff);
+	mcu_pmic_read_reg(REG_MCU_KPD_2, &val2, 0xff);
+	pr_debug("key pressed, 0x%02x%02x\n", val2, val1);
+	for (i = 0; i < 8; i++) {
+		curr_val = (val1 >> i) & 0x1;
+		if (curr_val > 0)
+			input_event(mc9s08dz60kbd_dev, EV_KEY,
+			key_code_map[i], 1);
+		else {
+			pre_val = (key_status >> i) & 0x1;
+			if (pre_val > 0)
+				input_event(mc9s08dz60kbd_dev, EV_KEY,
+				key_code_map[i], 0);
+		}
+	}
+	key_status = val1;
+
+}
+
+static irqreturn_t mc9s08dz60kpp_interrupt(int irq, void *dev_id)
+{
+	schedule_work(&key_pad_event);
+	return IRQ_RETVAL(1);
+}
+
+/*!
+ * This function is called when the keypad driver is opened.
+ * Since keypad initialization is done in __init, nothing is done in open.
+ *
+ * @param    dev    Pointer to device inode
+ *
+ * @result    The function always return 0
+ */
+static int mc9s08dz60kpp_open(struct input_dev *dev)
+{
+	return 0;
+}
+
+/*!
+ * This function is called close the keypad device.
+ * Nothing is done in this function, since every thing is taken care in
+ * __exit function.
+ *
+ * @param    dev    Pointer to device inode
+ *
+ */
+static void mc9s08dz60kpp_close(struct input_dev *dev)
+{
+}
+
+
+/*!
+ * This function is called during the driver binding process.
+ *
+ * @param   pdev  the device structure used to store device specific
+ *                information that is used by the suspend, resume and remove
+ *                functions.
+ *
+ * @return  The function returns 0 on successful registration. Otherwise returns
+ *          specific error code.
+ */
+static int mc9s08dz60kpp_probe(struct platform_device *pdev)
+{
+	int i, irq;
+	int retval;
+
+	retval = mcu_pmic_write_reg(REG_MCU_KPD_CONTROL, 0x1, 0x1);
+	if (retval != 0) {
+		pr_info("mc9s08dz60 keypad: mcu not detected!\n");
+		return retval;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	retval = request_irq(irq, mc9s08dz60kpp_interrupt,
+			0, MOD_NAME, MOD_NAME);
+	if (retval) {
+		pr_debug("KPP: request_irq(%d) returned error %d\n",
+			 irq, retval);
+		return retval;
+	}
+	keypad_irq = irq;
+
+	mc9s08dz60kbd_dev = input_allocate_device();
+	if (!mc9s08dz60kbd_dev) {
+		pr_info(KERN_ERR "mc9s08dz60kbd_dev: \
+		not enough memory for input device\n");
+		retval = -ENOMEM;
+		goto err1;
+	}
+
+	mc9s08dz60kbd_dev->name = "mc9s08dz60kpd";
+	mc9s08dz60kbd_dev->id.bustype = BUS_HOST;
+	mc9s08dz60kbd_dev->open = mc9s08dz60kpp_open;
+	mc9s08dz60kbd_dev->close = mc9s08dz60kpp_close;
+
+	retval = input_register_device(mc9s08dz60kbd_dev);
+	if (retval < 0) {
+		pr_info(KERN_ERR
+		       "mc9s08dz60kbd_dev: failed to register input device\n");
+		goto err2;
+	}
+
+	__set_bit(EV_KEY, mc9s08dz60kbd_dev->evbit);
+
+	for (i = 0; i < keycodes_size; i++)
+		__set_bit(key_code_map[i], mc9s08dz60kbd_dev->keybit);
+
+	device_init_wakeup(&pdev->dev, 1);
+
+	pr_info("mc9s08dz60 keypad probed\n");
+
+	return 0;
+
+err2:
+	input_free_device(mc9s08dz60kbd_dev);
+err1:
+	free_irq(irq, MOD_NAME);
+	return retval;
+}
+
+/*!
+ * Dissociates the driver from the kpp device.
+ *
+ * @param   pdev  the device structure used to give information on which SDHC
+ *                to remove
+ *
+ * @return  The function always returns 0.
+ */
+static int mc9s08dz60kpp_remove(struct platform_device *pdev)
+{
+	free_irq(keypad_irq, MOD_NAME);
+	input_unregister_device(mc9s08dz60kbd_dev);
+
+	if (mc9s08dz60kbd_dev)
+		input_free_device(mc9s08dz60kbd_dev);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mc9s08dz60kpd_driver = {
+	.driver = {
+		   .name = "mc9s08dz60keypad",
+		   .bus = &platform_bus_type,
+		   },
+	.probe = mc9s08dz60kpp_probe,
+	.remove = mc9s08dz60kpp_remove
+};
+
+static int __init mc9s08dz60kpp_init(void)
+{
+	pr_info(KERN_INFO "mc9s08dz60 keypad loaded\n");
+	platform_driver_register(&mc9s08dz60kpd_driver);
+	return 0;
+}
+
+static void __exit mc9s08dz60kpp_cleanup(void)
+{
+	platform_driver_unregister(&mc9s08dz60kpd_driver);
+}
+
+module_init(mc9s08dz60kpp_init);
+module_exit(mc9s08dz60kpp_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC Keypad Controller Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/keyboard/mpr084.c b/drivers/input/keyboard/mpr084.c
new file mode 100644
index 000000000000..0c0c5d8709e7
--- /dev/null
+++ b/drivers/input/keyboard/mpr084.c
@@ -0,0 +1,508 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file  linux/drivers/input/keyboard/mpr084.c
+ *
+ * @brief Driver for the Freescale MPR084 I2C Touch Sensor KeyPad module.
+ *
+ *
+ *
+ * @ingroup Keypad
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/string.h>
+#include <linux/bcd.h>
+#include <linux/list.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/kthread.h>
+#include <linux/input.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+#include <asm/mach/irq.h>
+#include <mach/gpio.h>
+
+/*
+ * Definitions
+ */
+#define DEBUG 0
+
+#define KEY_COUNT         8
+
+/*
+  *Registers in MPR084
+  */
+#define MPR084_FIFO_ADDR                        0x00
+#define MPR084_FAULT_ADDR                       0x01
+#define MPR084_TPS_ADDR                         0x02
+#define MPR084_TPC_ADDR                         0x03
+#define MPR084_STR1_ADDR                        0x04
+#define MPR084_STR2_ADDR                        0x05
+#define MPR084_STR3_ADDR                        0x06
+#define MPR084_STR4_ADDR                        0x07
+#define MPR084_STR5_ADDR                        0x08
+#define MPR084_STR6_ADDR                        0x09
+#define MPR084_STR7_ADDR                        0x0A
+#define MPR084_STR8_ADDR                        0x0B
+#define MPR084_ECEM_ADDR                        0x0C
+#define MPR084_MNTP_ADDR                        0x0D
+#define MPR084_MTC_ADDR                         0x0E
+#define MPR084_TASP_ADDR                        0x0F
+#define MPR084_SC_ADDR                          0x10
+#define MPR084_LPC_ADDR                         0x11
+#define MPR084_SKT_ADDR                         0x12
+#define MPR084_CONFIG_ADDR                      0x13
+#define MPR084_SI_ADDR                          0x14
+#define MPR084_ADDR_MINI                        MPR084_FIFO_ADDR
+#define MPR084_ADDR_MAX                         MPR084_SI_ADDR
+
+/* FIFO registers */
+#define MPR084_FIFO_MORE_DATA_FLAG              0x80
+#define MPR084_FIFO_NO_DATA_FLAG                0x40
+#define MPR084_FIFO_OVERFLOW_FLAG               0x20
+#define MPR084_FIFO_PAD_IS_TOUCHED              0x10
+#define MPR084_FIFO_POSITION_MASK               0x0F
+
+#define DRIVER_NAME "mpr084"
+
+struct mpr084_data {
+	struct i2c_client *client;
+	struct device_driver driver;
+	struct input_dev *idev;
+	struct task_struct *tstask;
+	struct completion kpirq_completion;
+	int kpirq;
+	int kp_thread_cnt;
+	int opened;
+};
+
+static int kpstatus[KEY_COUNT];
+static struct mxc_keyp_platform_data *keypad;
+static const unsigned short *mxckpd_keycodes;
+static struct regulator *vdd_reg;
+
+static int mpr084_read_register(struct mpr084_data *data,
+				unsigned char regaddr, int *value)
+{
+	int ret = 0;
+	unsigned char regvalue;
+
+	ret = i2c_master_send(data->client, &regaddr, 1);
+	if (ret < 0)
+		goto err;
+	udelay(20);
+	ret = i2c_master_recv(data->client, &regvalue, 1);
+	if (ret < 0)
+		goto err;
+	*value = regvalue;
+
+	return ret;
+err:
+	return -ENODEV;
+}
+
+static int mpr084_write_register(struct mpr084_data *data,
+				 u8 regaddr, u8 regvalue)
+{
+	int ret = 0;
+	unsigned char msgbuf[2];
+
+	msgbuf[0] = regaddr;
+	msgbuf[1] = regvalue;
+	ret = i2c_master_send(data->client, msgbuf, 2);
+	if (ret < 0) {
+		printk(KERN_ERR "%s - Error in writing to I2C Register %d \n",
+		       __func__, regaddr);
+		return ret;
+	}
+
+	return ret;
+}
+
+
+static irqreturn_t mpr084_keypadirq(int irq, void *v)
+{
+	struct mpr084_data *d = v;
+
+	disable_irq(d->kpirq);
+	complete(&d->kpirq_completion);
+	return IRQ_HANDLED;
+}
+
+static int mpr084ts_thread(void *v)
+{
+	struct mpr084_data *d = v;
+	int ret = 0, fifo = 0;
+	int index = 0, currentstatus = 0;
+
+	if (d->kp_thread_cnt)
+		return -EINVAL;
+	d->kp_thread_cnt = 1;
+	while (1) {
+
+		if (kthread_should_stop())
+			break;
+		/* Wait for keypad interrupt */
+		if (wait_for_completion_interruptible_timeout
+		    (&d->kpirq_completion, HZ) <= 0)
+			continue;
+
+		ret = mpr084_read_register(d, MPR084_FIFO_ADDR, &fifo);
+		if (ret < 0) {
+			printk(KERN_ERR
+			       "%s: Err in reading keypad FIFO register \n\n",
+			       __func__);
+		} else {
+			if (fifo & MPR084_FIFO_OVERFLOW_FLAG)
+				printk(KERN_ERR
+				       "%s: FIFO overflow \n\n", __func__);
+			while (!(fifo & MPR084_FIFO_NO_DATA_FLAG)) {
+				index = fifo & MPR084_FIFO_POSITION_MASK;
+				currentstatus =
+				    fifo & MPR084_FIFO_PAD_IS_TOUCHED;
+				/*Scan key map for changes */
+				if ((currentstatus) ^ (kpstatus[index])) {
+					if (!(currentstatus)) {
+						/*Key released. */
+						input_event(d->idev, EV_KEY,
+							    mxckpd_keycodes
+							    [index], 0);
+					} else {
+						/* Key pressed. */
+						input_event(d->idev, EV_KEY,
+							    mxckpd_keycodes
+							    [index], 1);
+					}
+					/*Store current keypad status */
+					kpstatus[index] = currentstatus;
+				}
+				mpr084_read_register(d, MPR084_FIFO_ADDR,
+						     &fifo);
+				if (fifo & MPR084_FIFO_OVERFLOW_FLAG)
+					printk(KERN_ERR
+					       "%s: FIFO overflow \n\n",
+					       __func__);
+			}
+		}
+		/* Re-enable interrupts */
+		enable_irq(d->kpirq);
+	}
+
+	d->kp_thread_cnt = 0;
+	return 0;
+}
+
+/*!
+ * This function puts the Keypad controller in low-power mode/state.
+ *
+ * @param   pdev  the device structure used to give information on Keypad
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function always returns 0.
+ */
+static int mpr084_suspend(struct i2c_client *client, pm_message_t state)
+{
+	struct mpr084_data *d = i2c_get_clientdata(client);
+
+	if (!IS_ERR(d->tstask) && d->opened)
+		kthread_stop(d->tstask);
+
+	return 0;
+}
+
+/*!
+ * This function brings the Keypad controller back from low-power state.
+ *
+ * @param   pdev  the device structure used to give information on Keypad
+ *                to resume
+ *
+ * @return  The function always returns 0.
+ */
+static int mpr084_resume(struct i2c_client *client)
+{
+	struct mpr084_data *d = i2c_get_clientdata(client);
+
+	if (d->opened)
+		d->tstask = kthread_run(mpr084ts_thread, d, DRIVER_NAME "kpd");
+
+	return 0;
+}
+
+static int mpr084_idev_open(struct input_dev *idev)
+{
+	struct mpr084_data *d = input_get_drvdata(idev);
+	int ret = 0;
+
+	d->tstask = kthread_run(mpr084ts_thread, d, DRIVER_NAME "kpd");
+	if (IS_ERR(d->tstask))
+		ret = PTR_ERR(d->tstask);
+	else
+		d->opened++;
+	return ret;
+}
+
+static void mpr084_idev_close(struct input_dev *idev)
+{
+	struct mpr084_data *d = input_get_drvdata(idev);
+
+	if (!IS_ERR(d->tstask))
+		kthread_stop(d->tstask);
+	if (d->opened > 0)
+		d->opened--;
+}
+
+static int mpr084_driver_register(struct mpr084_data *data)
+{
+	struct input_dev *idev;
+	int ret = 0;
+
+	if (data->kpirq) {
+		ret =
+		    request_irq(data->kpirq, mpr084_keypadirq,
+				IRQF_TRIGGER_FALLING, DRIVER_NAME, data);
+		if (!ret) {
+			init_completion(&data->kpirq_completion);
+			set_irq_wake(data->kpirq, 1);
+		} else {
+			printk(KERN_ERR "%s: cannot grab irq %d\n",
+			       __func__, data->kpirq);
+		}
+
+	}
+	idev = input_allocate_device();
+	data->idev = idev;
+	input_set_drvdata(idev, data);
+	idev->name = DRIVER_NAME;
+	idev->open = mpr084_idev_open;
+	idev->close = mpr084_idev_close;
+	if (!ret)
+		ret = input_register_device(idev);
+
+	return ret;
+}
+
+static int mpr084_i2c_remove(struct i2c_client *client)
+{
+	int err;
+	struct mpr084_data *d = i2c_get_clientdata(client);
+
+	free_irq(d->kpirq, d);
+	input_unregister_device(d->idev);
+	if (keypad->inactive)
+		keypad->inactive();
+
+	/*Disable the Regulator*/
+	if (keypad->vdd_reg) {
+		regulator_disable(vdd_reg);
+		regulator_put(vdd_reg);
+	}
+
+	err = i2c_detach_client(client);
+	if (err) {
+		dev_err(&client->dev, "Client deregistration failed, "
+			"client not detached.\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int mpr084_configure(struct mpr084_data *data)
+{
+	int ret = 0, regValue = 0;
+
+	ret = mpr084_write_register(data, MPR084_TPC_ADDR, 0x1d);
+	if (ret < 0)
+		goto err;
+	ret = mpr084_write_register(data, MPR084_STR1_ADDR, 0x10);
+	if (ret < 0)
+		goto err;
+	ret = mpr084_write_register(data, MPR084_STR2_ADDR, 0x10);
+	if (ret < 0)
+		goto err;
+	ret = mpr084_write_register(data, MPR084_STR3_ADDR, 0x10);
+	if (ret < 0)
+		goto err;
+	ret = mpr084_write_register(data, MPR084_STR4_ADDR, 0x10);
+	if (ret < 0)
+		goto err;
+	ret = mpr084_write_register(data, MPR084_STR5_ADDR, 0x10);
+	if (ret < 0)
+		goto err;
+	ret = mpr084_write_register(data, MPR084_STR6_ADDR, 0x10);
+	if (ret < 0)
+		goto err;
+	ret = mpr084_write_register(data, MPR084_STR7_ADDR, 0x10);
+	if (ret < 0)
+		goto err;
+	ret = mpr084_write_register(data, MPR084_STR8_ADDR, 0x10);
+	if (ret < 0)
+		goto err;
+	/* channel enable mask: enable all */
+	ret = mpr084_write_register(data, MPR084_ECEM_ADDR, 0xff);
+	if (ret < 0)
+		goto err;
+	/*two conccurrent touch position allowed */
+	ret = mpr084_write_register(data, MPR084_MNTP_ADDR, 0x02);
+	if (ret < 0)
+		goto err;
+
+	/* master tick period*/
+	ret = mpr084_write_register(data, MPR084_MTC_ADDR, 0x05);
+	if (ret < 0)
+		goto err;
+
+
+	/*Sample period */
+	ret = mpr084_write_register(data, MPR084_TASP_ADDR, 0x02);
+	if (ret < 0)
+		goto err;
+
+
+	/* disable sournder*/
+	ret = mpr084_write_register(data, MPR084_SC_ADDR, 0x00);
+	if (ret < 0)
+		goto err;
+
+	/* stuck key timeout */
+	ret = mpr084_write_register(data, MPR084_SKT_ADDR, 0x01);
+	if (ret < 0)
+		goto err;
+
+	/*enabled IRQEN, RUNE, IRQR */
+	ret = mpr084_read_register(data, MPR084_CONFIG_ADDR, &regValue);
+	if (ret < 0) {
+		printk(KERN_ERR
+		  "%s: Err in reading keypad CONFIGADDR register \n\n",
+		   __func__);
+		goto err;
+	}
+	regValue |= 0x03;
+	ret = mpr084_write_register(data, MPR084_CONFIG_ADDR, regValue);
+	if (ret < 0)
+		goto err;
+	return ret;
+err:
+	return -ENODEV;
+}
+
+static int mpr084_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct mpr084_data *data;
+	int err = 0, i = 0;
+#if DEBUG
+	int regValue = 0;
+#endif
+	data = kzalloc(sizeof(struct mpr084_data), GFP_KERNEL);
+	if (data == NULL)
+		return -ENOMEM;
+	i2c_set_clientdata(client, data);
+	data->client = client;
+	data->kpirq = client->irq;
+	err = mpr084_driver_register(data);
+	if (err < 0)
+		goto exit_free;
+	keypad = (struct mxc_keyp_platform_data *)(client->dev).platform_data;
+	if (keypad->active)
+		keypad->active();
+
+	/*Enable the Regulator*/
+	if (keypad && keypad->vdd_reg) {
+		vdd_reg = regulator_get(&client->dev, keypad->vdd_reg);
+		if (!IS_ERR(vdd_reg))
+			regulator_enable(vdd_reg);
+		else
+			vdd_reg = NULL;
+	} else
+		vdd_reg = NULL;
+
+	mxckpd_keycodes = keypad->matrix;
+	data->idev->keycode = &mxckpd_keycodes;
+	data->idev->keycodesize = sizeof(unsigned char);
+	data->idev->keycodemax = KEY_COUNT;
+	data->idev->id.bustype = BUS_I2C;
+	__set_bit(EV_KEY, data->idev->evbit);
+	for (i = 0; i < 8; i++)
+		__set_bit(mxckpd_keycodes[i], data->idev->keybit);
+	err = mpr084_configure(data);
+	if (err == -ENODEV) {
+		free_irq(data->kpirq, data);
+		input_unregister_device(data->idev);
+		goto exit_free;
+	}
+
+#if DEBUG
+	for (i = MPR084_ADDR_MINI; i <= MPR084_ADDR_MAX; i++) {
+		err = mpr084_read_register(data, i, &regValue);
+		if (err < 0) {
+		   printk(KERN_ERR
+		  "%s: Err in reading keypad CONFIGADDR register \n\n",
+		   __func__);
+		   goto exit_free;
+	    }
+	    printk("MPR084 Register id: %d, Value:%d \n", i, regValue);
+
+	}
+#endif
+	memset(kpstatus, 0, sizeof(kpstatus));
+	printk(KERN_INFO "%s: Device Attached\n", __func__);
+	return 0;
+exit_free:
+	/*disable the Regulator*/
+	if (vdd_reg) {
+		regulator_disable(vdd_reg);
+		regulator_put(vdd_reg);
+		vdd_reg = NULL;
+	}
+	kfree(data);
+	return err;
+}
+
+static const struct i2c_device_id mpr084_id[] = {
+	{ "mpr084", 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, mpr084_id);
+
+static struct i2c_driver mpr084_driver = {
+	.driver = {
+		   .name = DRIVER_NAME,
+		   },
+	.probe = mpr084_i2c_probe,
+	.remove = mpr084_i2c_remove,
+	.suspend = mpr084_suspend,
+	.resume = mpr084_resume,
+	.command = NULL,
+	.id_table = mpr084_id,
+};
+static int __init mpr084_init(void)
+{
+	return i2c_add_driver(&mpr084_driver);
+}
+
+static void __exit mpr084_exit(void)
+{
+	i2c_del_driver(&mpr084_driver);
+}
+
+MODULE_AUTHOR("Freescale Semiconductor Inc");
+MODULE_DESCRIPTION("MPR084 Touch KeyPad Controller driver");
+MODULE_LICENSE("GPL");
+module_init(mpr084_init);
+module_exit(mpr084_exit);
diff --git a/drivers/input/keyboard/mxc_keyb.c b/drivers/input/keyboard/mxc_keyb.c
new file mode 100644
index 000000000000..248b8e560903
--- /dev/null
+++ b/drivers/input/keyboard/mxc_keyb.c
@@ -0,0 +1,1036 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_keyb.c
+ *
+ * @brief Driver for the Freescale Semiconductor MXC keypad port.
+ *
+ * The keypad driver is designed as a standard Input driver which interacts
+ * with low level keypad port hardware. Upon opening, the Keypad driver
+ * initializes the keypad port. When the keypad interrupt happens the driver
+ * calles keypad polling timer and scans the keypad matrix for key
+ * press/release. If all key press/release happened it comes out of timer and
+ * waits for key press interrupt. The scancode for key press and release events
+ * are passed to Input subsytem.
+ *
+ * @ingroup keypad
+ */
+
+/*!
+ * Comment KPP_DEBUG to disable debug messages
+ */
+#define KPP_DEBUG        0
+
+#if KPP_DEBUG
+#define	DEBUG
+#include <linux/kernel.h>
+#endif
+
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/mm.h>
+#include <linux/init.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <mach/hardware.h>
+#include <linux/kd.h>
+#include <linux/fs.h>
+#include <linux/kbd_kern.h>
+#include <linux/ioctl.h>
+#include <linux/poll.h>
+#include <linux/interrupt.h>
+#include <linux/timer.h>
+#include <linux/input.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <asm/mach/keypad.h>
+
+/*
+ * Module header file
+ */
+#include "mxc_keyb.h"
+
+/*!
+ * This structure holds the keypad private data structure.
+ */
+static struct keypad_priv kpp_dev;
+
+/*! Indicates if the key pad device is enabled. */
+static unsigned int key_pad_enabled;
+
+/*! Input device structure. */
+static struct input_dev *mxckbd_dev = NULL;
+
+/*! KPP clock handle. */
+static struct clk *kpp_clk;
+
+/*! This static variable indicates whether a key event is pressed/released. */
+static unsigned short KPress;
+
+/*! cur_rcmap and prev_rcmap array is used to detect key press and release. */
+static unsigned short *cur_rcmap;	/* max 64 bits (8x8 matrix) */
+static unsigned short *prev_rcmap;
+
+/*!
+ * Debounce polling period(10ms) in system ticks.
+ */
+static unsigned short KScanRate = (10 * HZ) / 1000;
+
+static struct keypad_data *keypad;
+
+static int has_leaning_key;
+/*!
+ * These arrays are used to store press and release scancodes.
+ */
+static short **press_scancode;
+static short **release_scancode;
+
+static const unsigned short *mxckpd_keycodes;
+static unsigned short mxckpd_keycodes_size;
+
+#define press_left_code     30
+#define press_right_code    29
+#define press_up_code       28
+#define press_down_code     27
+
+#define rel_left_code       158
+#define rel_right_code      157
+#define rel_up_code         156
+#define rel_down_code       155
+/*!
+ * These functions are used to configure and the GPIO pins for keypad to
+ * activate and deactivate it.
+ */
+extern void gpio_keypad_active(void);
+extern void gpio_keypad_inactive(void);
+
+/*!
+ * This function is called for generating scancodes for key press and
+ * release on keypad for the board.
+ *
+ *  @param   row        Keypad row pressed on the keypad matrix.
+ *  @param   col        Keypad col pressed on the keypad matrix.
+ *  @param   press      Indicated key press/release.
+ *
+ *  @return     Key press/release Scancode.
+ */
+static signed short mxc_scan_matrix_leaning_key(int row, int col, int press)
+{
+	static unsigned first_row;
+	static unsigned first_set = 0, flag = 0;
+	signed short scancode = -1;
+
+	if (press) {
+		if ((3 == col) && ((3 == row) ||
+				   (4 == row) || (5 == row) || (6 == row))) {
+			if (first_set == 0) {
+				first_set = 1;
+				first_row = row;
+			} else {
+				first_set = 0;
+				if (((first_row == 6) || (first_row == 3))
+				    && ((row == 6) || (row == 3)))
+					scancode = press_down_code;
+				else if (((first_row == 3) || (first_row == 5))
+					 && ((row == 3) || (row == 5)))
+					scancode = press_left_code;
+				else if (((first_row == 6) || (first_row == 4))
+					 && ((row == 6) || (row == 4)))
+					scancode = press_right_code;
+				else if (((first_row == 4) || (first_row == 5))
+					 && ((row == 4) || (row == 5)))
+					scancode = press_up_code;
+				KPress = 1;
+				kpp_dev.iKeyState = KStateUp;
+				pr_debug("Press (%d, %d) scan=%d Kpress=%d\n",
+					 row, col, scancode, KPress);
+			}
+		} else {
+			/*
+			 * check for other keys only
+			 * if the cursor key presses
+			 * are not detected may be
+			 * this needs better logic
+			 */
+			if ((0 == (cur_rcmap[3] & BITSET(0, 3))) &&
+			    (0 == (cur_rcmap[4] & BITSET(0, 3))) &&
+			    (0 == (cur_rcmap[5] & BITSET(0, 3))) &&
+			    (0 == (cur_rcmap[6] & BITSET(0, 3)))) {
+				scancode = ((col * kpp_dev.kpp_rows) + row);
+				KPress = 1;
+				kpp_dev.iKeyState = KStateUp;
+				flag = 1;
+				pr_debug("Press (%d, %d) scan=%d Kpress=%d\n",
+					 row, col, scancode, KPress);
+			}
+		}
+	} else {
+		if ((flag == 0) && (3 == col)
+		    && ((3 == row) || (4 == row) || (5 == row)
+			|| (6 == row))) {
+			if (first_set == 0) {
+				first_set = 1;
+				first_row = row;
+			} else {
+				first_set = 0;
+				if (((first_row == 6) || (first_row == 3))
+				    && ((row == 6) || (row == 3)))
+					scancode = rel_down_code;
+				else if (((first_row == 3) || (first_row == 5))
+					 && ((row == 3) || (row == 5)))
+					scancode = rel_left_code;
+				else if (((first_row == 6) || (first_row == 4))
+					 && ((row == 6) || (row == 4)))
+					scancode = rel_right_code;
+				else if (((first_row == 4) || (first_row == 5))
+					 && ((row == 4) || (row == 5)))
+					scancode = rel_up_code;
+				KPress = 0;
+				kpp_dev.iKeyState = KStateDown;
+				pr_debug("Release (%d, %d) scan=%d Kpress=%d\n",
+					 row, col, scancode, KPress);
+			}
+		} else {
+			/*
+			 * check for other keys only
+			 * if the cursor key presses
+			 * are not detected may be
+			 * this needs better logic
+			 */
+			if ((0 == (prev_rcmap[3] & BITSET(0, 3))) &&
+			    (0 == (prev_rcmap[4] & BITSET(0, 3))) &&
+			    (0 == (cur_rcmap[5] & BITSET(0, 3))) &&
+			    (0 == (cur_rcmap[6] & BITSET(0, 3)))) {
+				scancode = ((col * kpp_dev.kpp_rows) + row) +
+				    MXC_KEYRELEASE;
+				KPress = 0;
+				flag = 0;
+				kpp_dev.iKeyState = KStateDown;
+				pr_debug("Release (%d, %d) scan=%d Kpress=%d\n",
+					 row, col, scancode, KPress);
+			}
+		}
+	}
+	return scancode;
+}
+
+/*!
+ * This function is called to scan the keypad matrix to find out the key press
+ * and key release events. Make scancode and break scancode are generated for
+ * key press and key release events.
+ *
+ * The following scanning sequence are done for
+ * keypad row and column scanning,
+ * -# Write 1's to KPDR[15:8], setting column data to 1's
+ * -# Configure columns as totem pole outputs(for quick discharging of keypad
+ * capacitance)
+ * -# Configure columns as open-drain
+ * -# Write a single column to 0, others to 1.
+ * -# Sample row inputs and save data. Multiple key presses can be detected on
+ * a single column.
+ * -# Repeat steps the above steps for remaining columns.
+ * -# Return all columns to 0 in preparation for standby mode.
+ * -# Clear KPKD and KPKR status bit(s) by writing to a 1,
+ *    Set the KPKR synchronizer chain by writing "1" to KRSS register,
+ *    Clear the KPKD synchronizer chain by writing "1" to KDSC register
+ *
+ * @result    Number of key pressed/released.
+ */
+static int mxc_kpp_scan_matrix(void)
+{
+	unsigned short reg_val;
+	int col, row;
+	short scancode = 0;
+	int keycnt = 0;		/* How many keys are still pressed */
+
+	/*
+	 * wmb() linux kernel function which guarantees orderings in write
+	 * operations
+	 */
+	wmb();
+
+	/* save cur keypad matrix to prev */
+
+	memcpy(prev_rcmap, cur_rcmap, kpp_dev.kpp_rows * sizeof(prev_rcmap[0]));
+	memset(cur_rcmap, 0, kpp_dev.kpp_rows * sizeof(cur_rcmap[0]));
+
+	for (col = 0; col < kpp_dev.kpp_cols; col++) {	/* Col */
+		/* 2. Write 1.s to KPDR[15:8] setting column data to 1.s */
+		reg_val = __raw_readw(KPDR);
+		reg_val |= 0xff00;
+		__raw_writew(reg_val, KPDR);
+
+		/*
+		 * 3. Configure columns as totem pole outputs(for quick
+		 * discharging of keypad capacitance)
+		 */
+		reg_val = __raw_readw(KPCR);
+		reg_val &= 0x00ff;
+		__raw_writew(reg_val, KPCR);
+
+		udelay(2);
+
+		/*
+		 * 4. Configure columns as open-drain
+		 */
+		reg_val = __raw_readw(KPCR);
+		reg_val |= ((1 << kpp_dev.kpp_cols) - 1) << 8;
+		__raw_writew(reg_val, KPCR);
+
+		/*
+		 * 5. Write a single column to 0, others to 1.
+		 * 6. Sample row inputs and save data. Multiple key presses
+		 * can be detected on a single column.
+		 * 7. Repeat steps 2 - 6 for remaining columns.
+		 */
+
+		/* Col bit starts at 8th bit in KPDR */
+		reg_val = __raw_readw(KPDR);
+		reg_val &= ~(1 << (8 + col));
+		__raw_writew(reg_val, KPDR);
+
+		/* Delay added to avoid propagating the 0 from column to row
+		 * when scanning. */
+
+		udelay(5);
+
+		/* Read row input */
+		reg_val = __raw_readw(KPDR);
+		for (row = 0; row < kpp_dev.kpp_rows; row++) {	/* sample row */
+			if (TEST_BIT(reg_val, row) == 0) {
+				cur_rcmap[row] = BITSET(cur_rcmap[row], col);
+				keycnt++;
+			}
+		}
+	}
+
+	/*
+	 * 8. Return all columns to 0 in preparation for standby mode.
+	 * 9. Clear KPKD and KPKR status bit(s) by writing to a .1.,
+	 * set the KPKR synchronizer chain by writing "1" to KRSS register,
+	 * clear the KPKD synchronizer chain by writing "1" to KDSC register
+	 */
+	reg_val = 0x00;
+	__raw_writew(reg_val, KPDR);
+	reg_val = __raw_readw(KPDR);
+	reg_val = __raw_readw(KPSR);
+	reg_val |= KBD_STAT_KPKD | KBD_STAT_KPKR | KBD_STAT_KRSS |
+	    KBD_STAT_KDSC;
+	__raw_writew(reg_val, KPSR);
+
+	/* Check key press status change */
+
+	/*
+	 * prev_rcmap array will contain the previous status of the keypad
+	 * matrix.  cur_rcmap array will contains the present status of the
+	 * keypad matrix. If a bit is set in the array, that (row, col) bit is
+	 * pressed, else it is not pressed.
+	 *
+	 * XORing these two variables will give us the change in bit for
+	 * particular row and column.  If a bit is set in XOR output, then that
+	 * (row, col) has a change of status from the previous state.  From
+	 * the diff variable the key press and key release of row and column
+	 * are found out.
+	 *
+	 * If the key press is determined then scancode for key pressed
+	 * can be generated using the following statement:
+	 *    scancode = ((row * 8) + col);
+	 *
+	 * If the key release is determined then scancode for key release
+	 * can be generated using the following statement:
+	 *    scancode = ((row * 8) + col) + MXC_KEYRELEASE;
+	 */
+	for (row = 0; row < kpp_dev.kpp_rows; row++) {
+		unsigned char diff;
+
+		/*
+		 * Calculate the change in the keypad row status
+		 */
+		diff = prev_rcmap[row] ^ cur_rcmap[row];
+
+		for (col = 0; col < kpp_dev.kpp_cols; col++) {
+			if ((diff >> col) & 0x1) {
+				/* There is a status change on col */
+				if ((prev_rcmap[row] & BITSET(0, col)) == 0) {
+					/*
+					 * Previous state is 0, so now
+					 * a key is pressed
+					 */
+					if (has_leaning_key) {
+						scancode =
+						    mxc_scan_matrix_leaning_key
+						    (row, col, 1);
+					} else {
+						scancode =
+						    ((row * kpp_dev.kpp_cols) +
+						     col);
+						KPress = 1;
+						kpp_dev.iKeyState = KStateUp;
+					}
+					pr_debug("Press   (%d, %d) scan=%d "
+						 "Kpress=%d\n",
+						 row, col, scancode, KPress);
+					press_scancode[row][col] =
+					    (short)scancode;
+				} else {
+					/*
+					 * Previous state is not 0, so
+					 * now a key is released
+					 */
+					if (has_leaning_key) {
+						scancode =
+						    mxc_scan_matrix_leaning_key
+						    (row, col, 0);
+					} else {
+						scancode =
+						    (row * kpp_dev.kpp_cols) +
+						    col + MXC_KEYRELEASE;
+						KPress = 0;
+						kpp_dev.iKeyState = KStateDown;
+					}
+
+					pr_debug
+					    ("Release (%d, %d) scan=%d Kpress=%d\n",
+					     row, col, scancode, KPress);
+					release_scancode[row][col] =
+					    (short)scancode;
+					keycnt++;
+				}
+			}
+		}
+	}
+
+	/*
+	 * This switch case statement is the
+	 * implementation of state machine of debounce
+	 * logic for key press/release.
+	 * The explaination of state machine is as
+	 * follows:
+	 *
+	 * KStateUp State:
+	 * This is in intial state of the state machine
+	 * this state it checks for any key presses.
+	 * The key press can be checked using the
+	 * variable KPress. If KPress is set, then key
+	 * press is identified and switches the to
+	 * KStateFirstDown state for key press to
+	 * debounce.
+	 *
+	 * KStateFirstDown:
+	 * After debounce delay(10ms), if the KPress is
+	 * still set then pass scancode generated to
+	 * input device and change the state to
+	 * KStateDown, else key press debounce is not
+	 * satisfied so change the state to KStateUp.
+	 *
+	 * KStateDown:
+	 * In this state it checks for any key release.
+	 * If KPress variable is cleared, then key
+	 * release is indicated and so, switch the
+	 * state to KStateFirstUp else to state
+	 * KStateDown.
+	 *
+	 * KStateFirstUp:
+	 * After debounce delay(10ms), if the KPress is
+	 * still reset then pass the key release
+	 * scancode to input device and change
+	 * the state to KStateUp else key release is
+	 * not satisfied so change the state to
+	 * KStateDown.
+	 */
+	switch (kpp_dev.iKeyState) {
+	case KStateUp:
+		if (KPress) {
+			/* First Down (must debounce). */
+			kpp_dev.iKeyState = KStateFirstDown;
+		} else {
+			/* Still UP.(NO Changes) */
+			kpp_dev.iKeyState = KStateUp;
+		}
+		break;
+
+	case KStateFirstDown:
+		if (KPress) {
+			for (row = 0; row < kpp_dev.kpp_rows; row++) {
+				for (col = 0; col < kpp_dev.kpp_cols; col++) {
+					if ((press_scancode[row][col] != -1)) {
+						/* Still Down, so add scancode */
+						scancode =
+						    press_scancode[row][col];
+						input_event(mxckbd_dev, EV_KEY,
+							    mxckpd_keycodes
+							    [scancode], 1);
+						if (mxckpd_keycodes[scancode] ==
+						    KEY_LEFTSHIFT) {
+							input_event(mxckbd_dev,
+								    EV_KEY,
+								    KEY_3, 1);
+						}
+						kpp_dev.iKeyState = KStateDown;
+						press_scancode[row][col] = -1;
+					}
+				}
+			}
+		} else {
+			/* Just a bounce */
+			kpp_dev.iKeyState = KStateUp;
+		}
+		break;
+
+	case KStateDown:
+		if (KPress) {
+			/* Still down (no change) */
+			kpp_dev.iKeyState = KStateDown;
+		} else {
+			/* First Up. Must debounce */
+			kpp_dev.iKeyState = KStateFirstUp;
+		}
+		break;
+
+	case KStateFirstUp:
+		if (KPress) {
+			/* Just a bounce */
+			kpp_dev.iKeyState = KStateDown;
+		} else {
+			for (row = 0; row < kpp_dev.kpp_rows; row++) {
+				for (col = 0; col < kpp_dev.kpp_cols; col++) {
+					if ((release_scancode[row][col] != -1)) {
+						scancode =
+						    release_scancode[row][col];
+						scancode =
+						    scancode - MXC_KEYRELEASE;
+						input_event(mxckbd_dev, EV_KEY,
+							    mxckpd_keycodes
+							    [scancode], 0);
+						if (mxckpd_keycodes[scancode] ==
+						    KEY_LEFTSHIFT) {
+							input_event(mxckbd_dev,
+								    EV_KEY,
+								    KEY_3, 0);
+						}
+						kpp_dev.iKeyState = KStateUp;
+						release_scancode[row][col] = -1;
+					}
+				}
+			}
+		}
+		break;
+
+	default:
+		return -EBADRQC;
+		break;
+	}
+
+	return keycnt;
+}
+
+/*!
+ * This function is called to start the timer for scanning the keypad if there
+ * is any key press. Currently this interval is  set to 10 ms. When there are
+ * no keys pressed on the keypad we return back, waiting for a keypad key
+ * press interrupt.
+ *
+ * @param data  Opaque data passed back by kernel. Not used.
+ */
+static void mxc_kpp_handle_timer(unsigned long data)
+{
+	unsigned short reg_val;
+	int i;
+
+	if (key_pad_enabled == 0) {
+		return;
+	}
+	if (mxc_kpp_scan_matrix() == 0) {
+		/*
+		 * Stop scanning and wait for interrupt.
+		 * Enable press interrupt and disable release interrupt.
+		 */
+		__raw_writew(0x00FF, KPDR);
+		reg_val = __raw_readw(KPSR);
+		reg_val |= (KBD_STAT_KPKR | KBD_STAT_KPKD);
+		reg_val |= KBD_STAT_KRSS | KBD_STAT_KDSC;
+		__raw_writew(reg_val, KPSR);
+		reg_val |= KBD_STAT_KDIE;
+		reg_val &= ~KBD_STAT_KRIE;
+		__raw_writew(reg_val, KPSR);
+
+		/*
+		 * No more keys pressed... make sure unwanted key codes are
+		 * not given upstairs
+		 */
+		for (i = 0; i < kpp_dev.kpp_rows; i++) {
+			memset(press_scancode[i], -1,
+			       sizeof(press_scancode[0][0]) * kpp_dev.kpp_cols);
+			memset(release_scancode[i], -1,
+			       sizeof(release_scancode[0][0]) *
+			       kpp_dev.kpp_cols);
+		}
+		return;
+	}
+
+	/*
+	 * There are still some keys pressed, continue to scan.
+	 * We shall scan again in 10 ms. This has to be tuned according
+	 * to the requirement.
+	 */
+	kpp_dev.poll_timer.expires = jiffies + KScanRate;
+	kpp_dev.poll_timer.function = mxc_kpp_handle_timer;
+	add_timer(&kpp_dev.poll_timer);
+}
+
+/*!
+ * This function is the keypad Interrupt handler.
+ * This function checks for keypad status register (KPSR) for key press
+ * and interrupt. If key press interrupt has occurred, then the key
+ * press interrupt in the KPSR are disabled.
+ * It then calls mxc_kpp_scan_matrix to check for any key pressed/released.
+ * If any key is found to be pressed, then a timer is set to call
+ * mxc_kpp_scan_matrix function for every 10 ms.
+ *
+ * @param   irq      The Interrupt number
+ * @param   dev_id   Driver private data
+ *
+ * @result    The function returns \b IRQ_RETVAL(1) if interrupt was handled,
+ *            returns \b IRQ_RETVAL(0) if the interrupt was not handled.
+ *            \b IRQ_RETVAL is defined in include/linux/interrupt.h.
+ */
+static irqreturn_t mxc_kpp_interrupt(int irq, void *dev_id)
+{
+	unsigned short reg_val;
+
+	/* Delete the polling timer */
+	del_timer(&kpp_dev.poll_timer);
+	reg_val = __raw_readw(KPSR);
+
+	/* Check if it is key press interrupt */
+	if (reg_val & KBD_STAT_KPKD) {
+		/*
+		 * Disable key press(KDIE status bit) interrupt
+		 */
+		reg_val &= ~KBD_STAT_KDIE;
+		__raw_writew(reg_val, KPSR);
+	} else {
+		/* spurious interrupt */
+		return IRQ_RETVAL(0);
+	}
+	/*
+	 * Check if any keys are pressed, if so start polling.
+	 */
+	mxc_kpp_handle_timer(0);
+
+	return IRQ_RETVAL(1);
+}
+
+/*!
+ * This function is called when the keypad driver is opened.
+ * Since keypad initialization is done in __init, nothing is done in open.
+ *
+ * @param    dev    Pointer to device inode
+ *
+ * @result    The function always return 0
+ */
+static int mxc_kpp_open(struct input_dev *dev)
+{
+	return 0;
+}
+
+/*!
+ * This function is called close the keypad device.
+ * Nothing is done in this function, since every thing is taken care in
+ * __exit function.
+ *
+ * @param    dev    Pointer to device inode
+ *
+ */
+static void mxc_kpp_close(struct input_dev *dev)
+{
+}
+
+#ifdef CONFIG_PM
+/*!
+ * This function puts the Keypad controller in low-power mode/state.
+ * If Keypad is enabled as a wake source(i.e. it can resume the system
+ * from suspend mode), the Keypad controller doesn't enter low-power state.
+ *
+ * @param   pdev  the device structure used to give information on Keypad
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  return -1 when the keypad is pressed. Otherwise, return 0
+ */
+static int mxc_kpp_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	/* When the keypad is still pressed, clean up registers and timers */
+	if (timer_pending(&kpp_dev.poll_timer))
+		return -1;
+
+	if (device_may_wakeup(&pdev->dev)) {
+		enable_irq_wake(keypad->irq);
+	} else {
+		disable_irq(keypad->irq);
+		key_pad_enabled = 0;
+		clk_disable(kpp_clk);
+		gpio_keypad_inactive();
+	}
+
+	return 0;
+}
+
+/*!
+ * This function brings the Keypad controller back from low-power state.
+ * If Keypad is enabled as a wake source(i.e. it can resume the system
+ * from suspend mode), the Keypad controller doesn't enter low-power state.
+ *
+ * @param   pdev  the device structure used to give information on Keypad
+ *                to resume
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_kpp_resume(struct platform_device *pdev)
+{
+	if (device_may_wakeup(&pdev->dev)) {
+		disable_irq_wake(keypad->irq);
+	} else {
+		gpio_keypad_active();
+		clk_enable(kpp_clk);
+		key_pad_enabled = 1;
+		enable_irq(keypad->irq);
+	}
+
+	return 0;
+}
+
+#else
+#define mxc_kpp_suspend  NULL
+#define mxc_kpp_resume   NULL
+#endif				/* CONFIG_PM */
+
+/*!
+ * This function is called to free the allocated memory for local arrays
+ */
+static void mxc_kpp_free_allocated(void)
+{
+
+	int i;
+
+	if (press_scancode) {
+		for (i = 0; i < kpp_dev.kpp_rows; i++) {
+			if (press_scancode[i])
+				kfree(press_scancode[i]);
+		}
+		kfree(press_scancode);
+	}
+
+	if (release_scancode) {
+		for (i = 0; i < kpp_dev.kpp_rows; i++) {
+			if (release_scancode[i])
+				kfree(release_scancode[i]);
+		}
+		kfree(release_scancode);
+	}
+
+	if (cur_rcmap)
+		kfree(cur_rcmap);
+
+	if (prev_rcmap)
+		kfree(prev_rcmap);
+
+	if (mxckbd_dev)
+		input_free_device(mxckbd_dev);
+}
+
+/*!
+ * This function is called during the driver binding process.
+ *
+ * @param   pdev  the device structure used to store device specific
+ *                information that is used by the suspend, resume and remove
+ *                functions.
+ *
+ * @return  The function returns 0 on successful registration. Otherwise returns
+ *          specific error code.
+ */
+static int mxc_kpp_probe(struct platform_device *pdev)
+{
+	int i, irq;
+	int retval;
+	unsigned int reg_val;
+
+	keypad = (struct keypad_data *)pdev->dev.platform_data;
+
+	kpp_dev.kpp_cols = keypad->colmax;
+	kpp_dev.kpp_rows = keypad->rowmax;
+	key_pad_enabled = 0;
+
+	/*
+	 * Request for IRQ number for keypad port. The Interrupt handler
+	 * function (mxc_kpp_interrupt) is called when ever interrupt occurs on
+	 * keypad port.
+	 */
+	irq = platform_get_irq(pdev, 0);
+	keypad->irq = irq;
+	retval = request_irq(irq, mxc_kpp_interrupt, 0, MOD_NAME, MOD_NAME);
+	if (retval) {
+		pr_debug("KPP: request_irq(%d) returned error %d\n",
+			 MXC_INT_KPP, retval);
+		return retval;
+	}
+
+	/* Enable keypad clock */
+	kpp_clk = clk_get(&pdev->dev, "kpp_clk");
+	clk_enable(kpp_clk);
+
+	/* IOMUX configuration for keypad */
+	gpio_keypad_active();
+
+	/* Configure keypad */
+
+	/* Enable number of rows in keypad (KPCR[7:0])
+	 * Configure keypad columns as open-drain (KPCR[15:8])
+	 *
+	 * Configure the rows/cols in KPP
+	 * LSB nibble in KPP is for 8 rows
+	 * MSB nibble in KPP is for 8 cols
+	 */
+	reg_val = __raw_readw(KPCR);
+	reg_val |= (1 << keypad->rowmax) - 1;	/* LSB */
+	reg_val |= ((1 << keypad->colmax) - 1) << 8;	/* MSB */
+	__raw_writew(reg_val, KPCR);
+
+	/* Write 0's to KPDR[15:8] */
+	reg_val = __raw_readw(KPDR);
+	reg_val &= 0x00ff;
+	__raw_writew(reg_val, KPDR);
+
+	/* Configure columns as output, rows as input (KDDR[15:0]) */
+	reg_val = __raw_readw(KDDR);
+	reg_val |= 0xff00;
+	reg_val &= 0xff00;
+	__raw_writew(reg_val, KDDR);
+
+	reg_val = __raw_readw(KPSR);
+	reg_val &= ~(KBD_STAT_KPKR | KBD_STAT_KPKD);
+	reg_val |= KBD_STAT_KPKD;
+	reg_val |= KBD_STAT_KRSS | KBD_STAT_KDSC;
+	__raw_writew(reg_val, KPSR);
+	reg_val |= KBD_STAT_KDIE;
+	reg_val &= ~KBD_STAT_KRIE;
+	__raw_writew(reg_val, KPSR);
+
+	has_leaning_key = keypad->learning;
+	mxckpd_keycodes = keypad->matrix;
+	mxckpd_keycodes_size = keypad->rowmax * keypad->colmax;
+
+	if ((keypad->matrix == (void *)0)
+	    || (mxckpd_keycodes_size == 0)) {
+		retval = -ENODEV;
+		goto err1;
+	}
+
+	mxckbd_dev = input_allocate_device();
+	if (!mxckbd_dev) {
+		printk(KERN_ERR
+		       "mxckbd_dev: not enough memory for input device\n");
+		retval = -ENOMEM;
+		goto err1;
+	}
+
+	mxckbd_dev->keycode = (void *)mxckpd_keycodes;
+	mxckbd_dev->keycodesize = sizeof(mxckpd_keycodes[0]);
+	mxckbd_dev->keycodemax = mxckpd_keycodes_size;
+	mxckbd_dev->name = "mxckpd";
+	mxckbd_dev->id.bustype = BUS_HOST;
+	mxckbd_dev->open = mxc_kpp_open;
+	mxckbd_dev->close = mxc_kpp_close;
+
+	retval = input_register_device(mxckbd_dev);
+	if (retval < 0) {
+		printk(KERN_ERR
+		       "mxckbd_dev: failed to register input device\n");
+		goto err2;
+	}
+
+	/* allocate required memory */
+	press_scancode = kmalloc(kpp_dev.kpp_rows * sizeof(press_scancode[0]),
+				 GFP_KERNEL);
+	release_scancode =
+	    kmalloc(kpp_dev.kpp_rows * sizeof(release_scancode[0]), GFP_KERNEL);
+
+	if (!press_scancode || !release_scancode) {
+		retval = -ENOMEM;
+		goto err3;
+	}
+
+	for (i = 0; i < kpp_dev.kpp_rows; i++) {
+		press_scancode[i] = kmalloc(kpp_dev.kpp_cols
+					    * sizeof(press_scancode[0][0]),
+					    GFP_KERNEL);
+		release_scancode[i] =
+		    kmalloc(kpp_dev.kpp_cols * sizeof(release_scancode[0][0]),
+			    GFP_KERNEL);
+
+		if (!press_scancode[i] || !release_scancode[i]) {
+			retval = -ENOMEM;
+			goto err3;
+		}
+	}
+
+	cur_rcmap =
+	    kmalloc(kpp_dev.kpp_rows * sizeof(cur_rcmap[0]), GFP_KERNEL);
+	prev_rcmap =
+	    kmalloc(kpp_dev.kpp_rows * sizeof(prev_rcmap[0]), GFP_KERNEL);
+
+	if (!cur_rcmap || !prev_rcmap) {
+		retval = -ENOMEM;
+		goto err3;
+	}
+
+	__set_bit(EV_KEY, mxckbd_dev->evbit);
+
+	for (i = 0; i < mxckpd_keycodes_size; i++)
+		__set_bit(mxckpd_keycodes[i], mxckbd_dev->keybit);
+
+	for (i = 0; i < kpp_dev.kpp_rows; i++) {
+		memset(press_scancode[i], -1,
+		       sizeof(press_scancode[0][0]) * kpp_dev.kpp_cols);
+		memset(release_scancode[i], -1,
+		       sizeof(release_scancode[0][0]) * kpp_dev.kpp_cols);
+	}
+	memset(cur_rcmap, 0, kpp_dev.kpp_rows * sizeof(cur_rcmap[0]));
+	memset(prev_rcmap, 0, kpp_dev.kpp_rows * sizeof(prev_rcmap[0]));
+
+	key_pad_enabled = 1;
+	/* Initialize the polling timer */
+	init_timer(&kpp_dev.poll_timer);
+
+	/* By default, devices should wakeup if they can */
+	/* So keypad is set as "should wakeup" as it can */
+	device_init_wakeup(&pdev->dev, 1);
+
+	return 0;
+
+      err3:
+	mxc_kpp_free_allocated();
+      err2:
+	input_free_device(mxckbd_dev);
+      err1:
+	free_irq(irq, MOD_NAME);
+	clk_disable(kpp_clk);
+	clk_put(kpp_clk);
+	return retval;
+}
+
+/*!
+ * Dissociates the driver from the kpp device.
+ *
+ * @param   pdev  the device structure used to give information on which SDHC
+ *                to remove
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_kpp_remove(struct platform_device *pdev)
+{
+	unsigned short reg_val;
+
+	/*
+	 * Clear the KPKD status flag (write 1 to it) and synchronizer chain.
+	 * Set KDIE control bit, clear KRIE control bit (avoid false release
+	 * events. Disable the keypad GPIO pins.
+	 */
+	__raw_writew(0x00, KPCR);
+	__raw_writew(0x00, KPDR);
+	__raw_writew(0x00, KDDR);
+
+	reg_val = __raw_readw(KPSR);
+	reg_val |= KBD_STAT_KPKD;
+	reg_val &= ~KBD_STAT_KRSS;
+	reg_val |= KBD_STAT_KDIE;
+	reg_val &= ~KBD_STAT_KRIE;
+	__raw_writew(reg_val, KPSR);
+
+	gpio_keypad_inactive();
+	clk_disable(kpp_clk);
+	clk_put(kpp_clk);
+
+	KPress = 0;
+
+	del_timer(&kpp_dev.poll_timer);
+
+	free_irq(keypad->irq, MOD_NAME);
+	input_unregister_device(mxckbd_dev);
+
+	mxc_kpp_free_allocated();
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_kpd_driver = {
+	.driver = {
+		   .name = "mxc_keypad",
+		   .bus = &platform_bus_type,
+		   },
+	.suspend = mxc_kpp_suspend,
+	.resume = mxc_kpp_resume,
+	.probe = mxc_kpp_probe,
+	.remove = mxc_kpp_remove
+};
+
+/*!
+ * This function is called for module initialization.
+ * It registers keypad char driver and requests for KPP irq number. This
+ * function does the initialization of the keypad device.
+ *
+ * The following steps are used for keypad configuration,\n
+ * -# Enable number of rows in the keypad control register (KPCR[7:0}).\n
+ * -# Write 0's to KPDR[15:8]\n
+ * -# Configure keypad columns as open-drain (KPCR[15:8])\n
+ * -# Configure columns as output, rows as input (KDDR[15:0])\n
+ * -# Clear the KPKD status flag (write 1 to it) and synchronizer chain\n
+ * -# Set KDIE control bit, clear KRIE control bit\n
+ * In this function the keypad queue initialization is done.
+ * The keypad IOMUX configuration are done here.*
+
+ *
+ * @return      0 on success and a non-zero value on failure.
+ */
+static int __init mxc_kpp_init(void)
+{
+	printk(KERN_INFO "MXC keypad loaded\n");
+	platform_driver_register(&mxc_kpd_driver);
+	return 0;
+}
+
+/*!
+ * This function is called whenever the module is removed from the kernel. It
+ * unregisters the keypad driver from kernel and frees the irq number.
+ * This function puts the keypad to standby mode. The keypad interrupts are
+ * disabled. It calls gpio_keypad_inactive function to switch gpio
+ * configuration into default state.
+ *
+ */
+static void __exit mxc_kpp_cleanup(void)
+{
+	platform_driver_unregister(&mxc_kpd_driver);
+}
+
+module_init(mxc_kpp_init);
+module_exit(mxc_kpp_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC Keypad Controller Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/keyboard/mxc_keyb.h b/drivers/input/keyboard/mxc_keyb.h
new file mode 100644
index 000000000000..d231eef50088
--- /dev/null
+++ b/drivers/input/keyboard/mxc_keyb.h
@@ -0,0 +1,191 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup keypad Keypad Driver
+ */
+
+/*!
+ * @file mxc_keyb.h
+ *
+ * @brief MXC keypad header file.
+ *
+ * @ingroup keypad
+ */
+#ifndef __MXC_KEYB_H__
+#define __MXC_KEYB_H__
+
+/*!
+ * Keypad Module Name
+ */
+#define MOD_NAME  "mxckpd"
+
+/*!
+ * Keypad irq number
+ */
+#define KPP_IRQ  MXC_INT_KPP
+
+/*!
+ * XLATE mode selection
+ */
+#define KEYPAD_XLATE        0
+
+/*!
+ * RAW mode selection
+ */
+#define KEYPAD_RAW          1
+
+/*!
+ * Maximum number of keys.
+ */
+#define MAXROW			8
+#define MAXCOL			8
+#define MXC_MAXKEY		(MAXROW * MAXCOL)
+
+/*!
+ * This define indicates break scancode for every key release. A constant
+ * of 128 is added to the key press scancode.
+ */
+#define  MXC_KEYRELEASE   128
+
+/*
+ * _reg_KPP_KPCR   _reg_KPP_KPSR _reg_KPP_KDDR _reg_KPP_KPDR
+ * Keypad Control Register Address
+ */
+#define KPCR    IO_ADDRESS(KPP_BASE_ADDR + 0x00)
+
+/*
+ * Keypad Status Register Address
+ */
+#define KPSR    IO_ADDRESS(KPP_BASE_ADDR + 0x02)
+
+/*
+ * Keypad Data Direction Address
+ */
+#define KDDR    IO_ADDRESS(KPP_BASE_ADDR + 0x04)
+
+/*
+ * Keypad Data Register
+ */
+#define KPDR    IO_ADDRESS(KPP_BASE_ADDR + 0x06)
+
+/*
+ * Key Press Interrupt Status bit
+ */
+#define KBD_STAT_KPKD        0x01
+
+/*
+ * Key Release Interrupt Status bit
+ */
+#define KBD_STAT_KPKR        0x02
+
+/*
+ * Key Depress Synchronizer Chain Status bit
+ */
+#define KBD_STAT_KDSC        0x04
+
+/*
+ * Key Release Synchronizer Status bit
+ */
+#define KBD_STAT_KRSS        0x08
+
+/*
+ * Key Depress Interrupt Enable Status bit
+ */
+#define KBD_STAT_KDIE        0x100
+
+/*
+ * Key Release Interrupt Enable
+ */
+#define KBD_STAT_KRIE        0x200
+
+/*
+ * Keypad Clock Enable
+ */
+#define KBD_STAT_KPPEN       0x400
+
+/*!
+ * Buffer size of keypad queue. Should be a power of 2.
+ */
+#define KPP_BUF_SIZE    128
+
+/*!
+ * Test whether bit is set for integer c
+ */
+#define TEST_BIT(c, n) ((c) & (0x1 << (n)))
+
+/*!
+ * Set nth bit in the integer c
+ */
+#define BITSET(c, n)   ((c) | (1 << (n)))
+
+/*!
+ * Reset nth bit in the integer c
+ */
+#define BITRESET(c, n) ((c) & ~(1 << (n)))
+
+/*!
+ * This enum represents the keypad state machine to maintain debounce logic
+ * for key press/release.
+ */
+enum KeyState {
+
+	/*!
+	 * Key press state.
+	 */
+	KStateUp,
+
+	/*!
+	 * Key press debounce state.
+	 */
+	KStateFirstDown,
+
+	/*!
+	 * Key release state.
+	 */
+	KStateDown,
+
+	/*!
+	 * Key release debounce state.
+	 */
+	KStateFirstUp
+};
+
+/*!
+ * Keypad Private Data Structure
+ */
+typedef struct keypad_priv {
+
+	/*!
+	 * Keypad state machine.
+	 */
+	enum KeyState iKeyState;
+
+	/*!
+	 * Number of rows configured in the keypad matrix
+	 */
+	unsigned long kpp_rows;
+
+	/*!
+	 * Number of Columns configured in the keypad matrix
+	 */
+	unsigned long kpp_cols;
+
+	/*!
+	 * Timer used for Keypad polling.
+	 */
+	struct timer_list poll_timer;
+
+} keypad_priv;
+
+#endif				/* __MXC_KEYB_H__ */
diff --git a/drivers/input/keyboard/stmp3xxx-kbd.c b/drivers/input/keyboard/stmp3xxx-kbd.c
new file mode 100644
index 000000000000..f70c2f024401
--- /dev/null
+++ b/drivers/input/keyboard/stmp3xxx-kbd.c
@@ -0,0 +1,300 @@
+/*
+ * Keypad ladder driver for Freescale STMP37XX/STMP378X boards
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/input.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <mach/regs-lradc.h>
+#include <mach/lradc.h>
+#include <mach/stmp3xxx.h>
+
+#define BUTTON_PRESS_THRESHOLD  3300
+#define LRADC_NOISE_MARGIN      100
+
+/* this value represents the the lradc value at 3.3V ( 3.3V / 0.000879 V/b ) */
+#define TARGET_VDDIO_LRADC_VALUE 3754
+
+struct stmpkbd_data {
+	struct input_dev *input;
+	int last_button;
+	int irq;
+	struct stmpkbd_keypair *keycodes;
+};
+
+static int delay1 = 500;
+static int delay2 = 200;
+
+static int stmpkbd_open(struct input_dev *dev);
+static void stmpkbd_close(struct input_dev *dev);
+
+static struct stmpkbd_data *stmpkbd_data_alloc(struct platform_device *pdev,
+		struct stmpkbd_keypair *keys)
+{
+	struct stmpkbd_data *d = kzalloc(sizeof(*d), GFP_KERNEL);
+
+	if (!d)
+		return NULL;
+
+	if (!keys) {
+		dev_err(&pdev->dev,
+			"No keycodes in platform_data, bailing out.\n");
+		kfree(d);
+		return NULL;
+	}
+	d->keycodes = keys;
+
+	d->input = input_allocate_device();
+	if (!d->input) {
+		kfree(d);
+		return NULL;
+	}
+
+	d->input->phys = "onboard";
+	d->input->uniq = "0000'0000";
+	d->input->name = pdev->name;
+	d->input->id.bustype = BUS_HOST;
+	d->input->open = stmpkbd_open;
+	d->input->close = stmpkbd_close;
+	d->input->dev.parent = &pdev->dev;
+
+	set_bit(EV_KEY, d->input->evbit);
+	set_bit(EV_REL, d->input->evbit);
+	set_bit(EV_REP, d->input->evbit);
+
+
+	d->last_button = -1;
+
+	while (keys->raw >= 0) {
+		set_bit(keys->kcode, d->input->keybit);
+		keys++;
+	}
+
+	return d;
+}
+
+static inline struct input_dev *GET_INPUT_DEV(struct stmpkbd_data *d)
+{
+	BUG_ON(!d);
+	return d->input;
+}
+
+static void stmpkbd_data_free(struct stmpkbd_data *d)
+{
+	if (!d)
+		return;
+	if (d->input)
+		input_free_device(d->input);
+	kfree(d);
+}
+
+static unsigned stmpkbd_decode_button(struct stmpkbd_keypair *codes,
+			int raw_button)
+
+{
+	pr_debug("Decoding %d\n", raw_button);
+	while (codes->raw != -1) {
+		if ((raw_button > (codes->raw - LRADC_NOISE_MARGIN)) &&
+		    (raw_button < (codes->raw + LRADC_NOISE_MARGIN))) {
+			pr_debug("matches code 0x%x = %d\n",
+				codes->kcode, codes->kcode);
+			return codes->kcode;
+		}
+		codes++;
+	}
+	return (unsigned)-1; /* invalid key */
+}
+
+
+static irqreturn_t stmpkbd_irq_handler(int irq, void *dev_id)
+{
+	struct platform_device *pdev = dev_id;
+	struct stmpkbd_data *devdata = platform_get_drvdata(pdev);
+	u16 raw_button, normalized_button, vddio;
+	unsigned btn;
+
+	raw_button = HW_LRADC_CHn_RD(LRADC_CH0) & BM_LRADC_CHn_VALUE;
+	vddio = hw_lradc_vddio();
+	BUG_ON(vddio == 0);
+
+	normalized_button = (raw_button * TARGET_VDDIO_LRADC_VALUE) /
+		vddio;
+
+	if (normalized_button < BUTTON_PRESS_THRESHOLD &&
+	    devdata->last_button < 0) {
+
+		btn = stmpkbd_decode_button(devdata->keycodes,
+				normalized_button);
+
+		if (btn < KEY_MAX) {
+			devdata->last_button = btn;
+			input_report_key(GET_INPUT_DEV(devdata),
+				devdata->last_button, !0);
+		} else
+			dev_err(&pdev->dev, "Invalid button: raw = %d, "
+				"normalized = %d, vddio = %d\n",
+				raw_button, normalized_button, vddio);
+	} else if (devdata->last_button > 0 &&
+		normalized_button >= BUTTON_PRESS_THRESHOLD) {
+
+		input_report_key(GET_INPUT_DEV(devdata),
+				devdata->last_button, 0);
+		devdata->last_button = -1;
+
+	}
+
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC0_IRQ);
+	return IRQ_HANDLED;
+}
+
+static int stmpkbd_open(struct input_dev *dev)
+{
+	/* enable clock */
+	return 0;
+}
+
+static void stmpkbd_close(struct input_dev *dev)
+{
+	/* disable clock */
+}
+
+static void stmpkbd_hwinit(struct platform_device *pdev)
+{
+	hw_lradc_init_ladder(LRADC_CH0, LRADC_DELAY_TRIGGER_BUTTON, 200);
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC0_IRQ);
+	HW_LRADC_CTRL1_SET(BM_LRADC_CTRL1_LRADC0_IRQ_EN);
+	hw_lradc_set_delay_trigger_kick(LRADC_DELAY_TRIGGER_BUTTON, !0);
+}
+
+static int stmpkbd_suspend(struct platform_device *pdev, pm_message_t state)
+{
+#ifdef CONFIG_PM
+	struct input_dev *idev = platform_get_drvdata(pdev);
+
+	hw_lradc_stop_ladder(LRADC_CH0, LRADC_DELAY_TRIGGER_BUTTON);
+	hw_lradc_set_delay_trigger_kick(LRADC_DELAY_TRIGGER_BUTTON, 0);
+	hw_lradc_unuse_channel(LRADC_CH0);
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC0_IRQ_EN);
+	stmpkbd_close(idev);
+#endif
+	return 0;
+}
+
+static int stmpkbd_resume(struct platform_device *pdev)
+{
+#ifdef CONFIG_PM
+	struct input_dev *idev = platform_get_drvdata(pdev);
+
+	HW_LRADC_CTRL1_SET(BM_LRADC_CTRL1_LRADC0_IRQ_EN);
+	stmpkbd_open(idev);
+	hw_lradc_use_channel(LRADC_CH0);
+	stmpkbd_hwinit(pdev);
+#endif
+	return 0;
+}
+
+static int __devinit stmpkbd_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	int irq = platform_get_irq(pdev, 0);
+	struct stmpkbd_data *d;
+
+	/* Create and register the input driver. */
+	d = stmpkbd_data_alloc(pdev,
+		(struct stmpkbd_keypair *)pdev->dev.platform_data);
+	if (!d) {
+		dev_err(&pdev->dev, "Cannot allocate driver structures\n");
+		err = -ENOMEM;
+		goto err_out;
+	}
+
+	d->irq = irq;
+	err = request_irq(irq, stmpkbd_irq_handler,
+		IRQF_DISABLED, pdev->name, pdev);
+	if (err) {
+		dev_err(&pdev->dev, "Cannot request keypad IRQ\n");
+		goto err_free_dev;
+	}
+
+	platform_set_drvdata(pdev, d);
+
+	/* Register the input device */
+	err = input_register_device(GET_INPUT_DEV(d));
+	if (err)
+		goto err_free_irq;
+
+	/* these two have to be set after registering the input device */
+	d->input->rep[REP_DELAY] = delay1;
+	d->input->rep[REP_PERIOD] = delay2;
+
+	hw_lradc_use_channel(LRADC_CH0);
+	stmpkbd_hwinit(pdev);
+
+	return 0;
+
+err_free_irq:
+	platform_set_drvdata(pdev, NULL);
+	free_irq(irq, pdev);
+err_free_dev:
+	stmpkbd_data_free(d);
+err_out:
+	return err;
+}
+
+static int __devexit stmpkbd_remove(struct platform_device *pdev)
+{
+	struct stmpkbd_data *d = platform_get_drvdata(pdev);
+
+	hw_lradc_unuse_channel(LRADC_CH0);
+	input_unregister_device(GET_INPUT_DEV(d));
+	free_irq(d->irq, pdev);
+	stmpkbd_data_free(d);
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver stmpkbd_driver = {
+	.probe		= stmpkbd_probe,
+	.remove		= __devexit_p(stmpkbd_remove),
+	.suspend	= stmpkbd_suspend,
+	.resume		= stmpkbd_resume,
+	.driver		= {
+		.name	= "stmp3xxx-keyboard",
+	},
+};
+
+static int __init stmpkbd_init(void)
+{
+	return platform_driver_register(&stmpkbd_driver);
+}
+
+static void __exit stmpkbd_exit(void)
+{
+	platform_driver_unregister(&stmpkbd_driver);
+}
+
+module_init(stmpkbd_init);
+module_exit(stmpkbd_exit);
+MODULE_DESCRIPTION("Freescale STMP3xxxx keyboard driver");
+MODULE_AUTHOR("dmitry pervushin <dimka@embeddedalley.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 199055db5082..16eaf1408090 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -40,6 +40,15 @@ config INPUT_M68K_BEEP
 	tristate "M68k Beeper support"
 	depends on M68K
 
+config INPUT_STMP3XXX_ROTDEC
+	tristate "STMP3xxx Rotary Decoder support"
+	depends on MACH_STMP378X
+	select INPUT_POLLDEV
+	help
+	  Say Y here for support for the rotary decoder on STMP3xxx
+
+	  If compiled as a module, it will be called stmp3xxx_rotdec
+
 config INPUT_APANEL
 	tristate "Fujitsu Lifebook Application Panel buttons"
 	depends on X86 && I2C && LEDS_CLASS
@@ -220,4 +229,13 @@ config HP_SDC_RTC
 	  Say Y here if you want to support the built-in real time clock
 	  of the HP SDC controller.
 
+config INPUT_MMA7455L
+	tristate "Freescale MMA7455L 3-axis accelerometer"
+	depends on I2C
+	help
+	  I2C driver for the Freescale MMA7455L 3-axis accelerometer.
+
+	  The userspace interface is a 3-axis (X/Y/Z) relative movement
+	  Linux input device, reporting REL_[XYZ] events.
+
 endif
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index d7db2aeb8a98..c3bd6d5c5f11 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -7,6 +7,7 @@
 obj-$(CONFIG_INPUT_SPARCSPKR)		+= sparcspkr.o
 obj-$(CONFIG_INPUT_PCSPKR)		+= pcspkr.o
 obj-$(CONFIG_INPUT_M68K_BEEP)		+= m68kspkr.o
+obj-$(CONFIG_INPUT_STMP3XXX_ROTDEC)	+= stmp3xxx_rotdec.o
 obj-$(CONFIG_INPUT_IXP4XX_BEEPER)	+= ixp4xx-beeper.o
 obj-$(CONFIG_INPUT_COBALT_BTNS)		+= cobalt_btns.o
 obj-$(CONFIG_INPUT_WISTRON_BTNS)	+= wistron_btns.o
@@ -21,3 +22,4 @@ obj-$(CONFIG_HP_SDC_RTC)		+= hp_sdc_rtc.o
 obj-$(CONFIG_INPUT_UINPUT)		+= uinput.o
 obj-$(CONFIG_INPUT_APANEL)		+= apanel.o
 obj-$(CONFIG_INPUT_SGI_BTNS)		+= sgi_btns.o
+obj-$(CONFIG_INPUT_MMA7455L)		+= mma7455l.o
diff --git a/drivers/input/misc/mma7455l.c b/drivers/input/misc/mma7455l.c
new file mode 100644
index 000000000000..f5113139e528
--- /dev/null
+++ b/drivers/input/misc/mma7455l.c
@@ -0,0 +1,647 @@
+/* Linux kernel driver for the Freescale MMA7455L 3-axis accelerometer
+ *
+ * Copyright (C) 2009 by Always Innovating, Inc.
+ * Copyright 2009 Digi International, Inc. All Rights Reserved.
+ * Author: Gregoire Gentil <gregoire@gentil.com>
+ * Author: Tim Yamin <plasm@roo.me.uk>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ *
+ */
+
+/*
+ * What this driver doesn't yet support:
+ *
+ * - INT2 handling
+ * - Pulse detection (and the sysctls to control it)
+ * - 10-bit measurement
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/sysfs.h>
+#include <linux/gpio.h>
+
+#include <linux/mma7455l.h>
+#include <linux/i2c.h>
+
+#define MMA7455L_WHOAMI_MAGIC		0x55
+
+enum mma7455l_reg {
+	MMA7455L_REG_XOUTL		= 0x00,
+	MMA7455L_REG_XOUTH		= 0x01,
+	MMA7455L_REG_YOUTL		= 0x02,
+	MMA7455L_REG_YOUTH		= 0x03,
+	MMA7455L_REG_ZOUTL 		= 0x04,
+	MMA7455L_REG_ZOUTH		= 0x05,
+	MMA7455L_REG_XOUT8		= 0x06,
+	MMA7455L_REG_YOUT8		= 0x07,
+	MMA7455L_REG_ZOUT8		= 0x08,
+	MMA7455L_REG_STATUS		= 0x09,
+	MMA7455L_REG_DETSRC		= 0x0a,
+	MMA7455L_REG_TOUT		= 0x0b,
+	MMA7455L_REG_RESERVED1		= 0x0c,
+	MMA7455L_REG_I2CAD		= 0x0d,
+	MMA7455L_REG_USRINF		= 0x0e,
+	MMA7455L_REG_WHOAMI		= 0x0f,
+	MMA7455L_REG_XOFFL		= 0x10,
+	MMA7455L_REG_XOFFH		= 0x11,
+	MMA7455L_REG_YOFFL		= 0x12,
+	MMA7455L_REG_YOFFH		= 0x13,
+	MMA7455L_REG_ZOFFL		= 0x14,
+	MMA7455L_REG_ZOFFH		= 0x15,
+	MMA7455L_REG_MCTL		= 0x16,
+	MMA7455L_REG_INTRST		= 0x17,
+	MMA7455L_REG_CTL1		= 0x18,
+	MMA7455L_REG_CTL2		= 0x19,
+	MMA7455L_REG_LDTH		= 0x1a,
+	MMA7455L_REG_PDTH		= 0x1b,
+	MMA7455L_REG_PW			= 0x1c,
+	MMA7455L_REG_LT			= 0x1d,
+	MMA7455L_REG_TW			= 0x1e,
+	MMA7455L_REG_RESERVED2		= 0x1f,
+};
+
+enum mma7455l_reg_status {
+	MMA7455L_STATUS_XDA		= 0x08,
+	MMA7455L_STATUS_YDA		= 0x10,
+	MMA7455L_STATUS_ZDA		= 0x20,
+};
+
+enum mma7455l_mode {
+	MMA7455L_MODE_STANDBY		= 0,
+	MMA7455L_MODE_MEASUREMENT	= 1,
+	MMA7455L_MODE_LEVELDETECTION	= 0x42, /* Set DRPD to on */
+	MMA7455L_MODE_PULSEDETECTION	= 0x43, /* Set DRPD to on */
+	MMA7455L_MODE_MASK		= 0x43,
+};
+
+enum mma7455l_gselect {
+	MMA7455L_GSELECT_8		= 0x0,
+	MMA7455L_GSELECT_2		= 0x4,
+	MMA7455L_GSELECT_4		= 0x8,
+	MMA7455L_GSELECT_MASK		= 0xC,
+};
+
+/* FIXME */
+#define MMA7455L_F_FS			0x0020 	/* ADC full scale */
+
+
+struct mma7455l_info {
+	/* Calibration data */
+	s16 calibration_x;
+	s16 calibration_y;
+	s16 calibration_z;
+
+	/* Defaults to I2c access */
+	struct i2c_client *client;
+	struct input_dev *input_dev;
+	struct mutex lock;		/* Struct mutex lock */
+	struct delayed_work work;
+
+	u8 mode;
+	u8 gSelect;
+
+	u8 flags;
+	u8 working;
+};
+
+/* lowlevel register access functions */
+
+#define WRITE_BIT	(1 << 7)
+#define ADDR_SHIFT	1
+
+/* Defaults to I2c access */
+static inline u_int8_t __reg_read(struct mma7455l_info *mma, u_int8_t reg)
+{
+	unsigned char buf;
+
+	struct i2c_msg msgs[] = {
+		{mma->client->addr, 0, 1, &reg},	/* setup read ptr */
+		{mma->client->addr, I2C_M_RD, 1, &buf},	/* read date */
+	};
+
+	/* read register */
+	if ((i2c_transfer(mma->client->adapter, &msgs[0], 2)) != 2) {
+		dev_err(&mma->client->dev, "%s: read error\n", __func__);
+		return -EIO;
+	}
+	return buf;
+}
+
+static inline int __reg_write(struct mma7455l_info *mma,
+				u_int8_t reg, u_int8_t val)
+{
+
+	struct i2c_msg msgs[] = {
+		{mma->client->addr, 0, 1, &reg},	/* setup read ptr */
+		{mma->client->addr, 0, 1, &val},	/* write value */
+	};
+
+	/* Write register */
+	if ((i2c_transfer(mma->client->adapter, &msgs[0], 2)) != 2) {
+		dev_err(&mma->client->dev, "%s: read error\n", __func__);
+		return -EIO;
+	}
+	return 0;
+}
+
+static u_int8_t reg_read(struct mma7455l_info *mma, u_int8_t reg)
+{
+	u_int8_t ret;
+
+	mutex_lock(&mma->lock);
+	ret = __reg_read(mma, reg);
+	mutex_unlock(&mma->lock);
+
+	return ret;
+}
+
+static s16 __reg_read_10(struct mma7455l_info *mma, u8 reg1, u8 reg2)
+{
+	u8 v1, v2;
+
+	v1 = __reg_read(mma, reg1);
+	v2 = __reg_read(mma, reg2);
+
+	return (v2 & 0x4) << 13 | (v2 & 0x3) << 8 | v1;
+}
+
+static int reg_write(struct mma7455l_info *mma, u_int8_t reg, u_int8_t val)
+{
+	int ret;
+
+	mutex_lock(&mma->lock);
+	ret = __reg_write(mma, reg, val);
+	mutex_unlock(&mma->lock);
+
+	return ret;
+}
+
+static s16 __reg_write_10(struct mma7455l_info *mma,
+				u8 reg1, u8 reg2, s16 value)
+{
+	int ret;
+	u8 v1, v2;
+
+	v1 = value & 0xFF;
+	if (value < 0)
+		v2 = ((value >> 8) & 0x3) | 0x4;
+	else
+		v2 = 0;
+
+	ret = __reg_write(mma, reg1, v1);
+	ret = __reg_write(mma, reg2, v2);
+	return ret;
+}
+
+static void mma7455l_work(struct work_struct *work)
+{
+	struct mma7455l_info *mma =
+			container_of(work, struct mma7455l_info, work.work);
+
+	s8 val;
+	mma->working = 1;
+
+	/* FIXME: 10 bit accuracy? */
+	if (!(mma->flags & MMA7455L_STATUS_XDA)) {
+		val = reg_read(mma, MMA7455L_REG_XOUT8);
+		input_report_abs(mma->input_dev, ABS_X, val);
+	}
+	if (!(mma->flags & MMA7455L_STATUS_YDA)) {
+		val = reg_read(mma, MMA7455L_REG_YOUT8);
+		input_report_abs(mma->input_dev, ABS_Y, val);
+	}
+	if (!(mma->flags & MMA7455L_STATUS_ZDA)) {
+		val = reg_read(mma, MMA7455L_REG_ZOUT8);
+		input_report_abs(mma->input_dev, ABS_Z, val);
+	}
+
+	mma->working = 0;
+	input_sync(mma->input_dev);
+	put_device(&mma->client->dev);
+
+	/* Enable IRQ and clear out interrupt */
+	reg_write(mma, MMA7455L_REG_INTRST, 0x3);
+	reg_write(mma, MMA7455L_REG_INTRST, 0x0);
+
+	enable_irq(mma->client->irq);
+}
+
+static void mma7455l_schedule_work(struct mma7455l_info *mma)
+{
+	int status;
+
+	get_device(&mma->client->dev);
+	status = schedule_delayed_work(&mma->work, HZ / 10);
+}
+
+static irqreturn_t mma7455l_interrupt(int irq, void *_mma)
+{
+	struct mma7455l_info *mma = _mma;
+	mma7455l_schedule_work(mma);
+
+	/* Disable any further interrupts until we have processed
+	 * the current one */
+	disable_irq(mma->client->irq);
+	return IRQ_HANDLED;
+}
+
+/* sysfs */
+
+static void get_mode(struct mma7455l_info *mma, u8 *mode, u8 *gSelect)
+{
+	u8 tmp = reg_read(mma, MMA7455L_REG_MCTL);
+
+	*mode = tmp & MMA7455L_MODE_MASK;
+	*gSelect = tmp & MMA7455L_GSELECT_MASK;
+}
+
+static void set_mode(struct mma7455l_info *mma, u8 mode, u8 gSelect)
+{
+	reg_write(mma, MMA7455L_REG_MCTL, mode | gSelect);
+}
+
+static void update_mode(struct mma7455l_info *mma, u8 mode, u8 gSelect)
+{
+	mma->mode = mode;
+	mma->gSelect = gSelect;
+
+	reg_write(mma, MMA7455L_REG_MCTL, mma->mode | mma->gSelect);
+}
+
+static ssize_t show_measure(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct mma7455l_info *mma = dev_get_drvdata(dev);
+	s8 x, y, z;
+	u8 old_Mode, old_gSelect;
+
+	get_mode(mma, &old_Mode, &old_gSelect);
+	set_mode(mma, MMA7455L_MODE_MEASUREMENT, MMA7455L_GSELECT_2);
+
+	while (reg_read(mma, MMA7455L_REG_STATUS) == 0)
+		msleep(10);
+
+	x = reg_read(mma, MMA7455L_REG_XOUT8);
+	y = reg_read(mma, MMA7455L_REG_YOUT8);
+	z = reg_read(mma, MMA7455L_REG_ZOUT8);
+
+	set_mode(mma, old_Mode, old_gSelect);
+	return sprintf(buf, "%d %d %d\n", x, y, z);
+}
+
+static ssize_t show_mode(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct mma7455l_info *mma = dev_get_drvdata(dev);
+
+	switch (mma->mode) {
+	case MMA7455L_MODE_STANDBY:
+		return sprintf(buf, "Standby\n");
+		break;
+	case MMA7455L_MODE_MEASUREMENT:
+		return sprintf(buf, "Measurement\n");
+		break;
+	case MMA7455L_MODE_LEVELDETECTION:
+		return sprintf(buf, "Level Detection\n");
+		break;
+	case MMA7455L_MODE_PULSEDETECTION:
+		return sprintf(buf, "Pulse Detection\n");
+		break;
+	}
+
+	return sprintf(buf, "Unknown mode!\n");
+}
+
+static ssize_t show_gSelect(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct mma7455l_info *mma = dev_get_drvdata(dev);
+
+	switch (mma->gSelect) {
+	case MMA7455L_GSELECT_8:
+		return sprintf(buf, "8\n");
+		break;
+	case MMA7455L_GSELECT_4:
+		return sprintf(buf, "4\n");
+		break;
+	case MMA7455L_GSELECT_2:
+		return sprintf(buf, "2\n");
+		break;
+	}
+
+	return sprintf(buf, "Unknown gSelect!\n");
+}
+
+static ssize_t show_level_threshold(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct mma7455l_info *mma = dev_get_drvdata(dev);
+	return sprintf(buf, "%u\n", reg_read(mma, MMA7455L_REG_LDTH));
+}
+
+static ssize_t show_calibration(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	s16 x, y, z;
+	struct mma7455l_info *mma = dev_get_drvdata(dev);
+
+	mutex_lock(&mma->lock);
+	x = __reg_read_10(mma, MMA7455L_REG_XOFFL, MMA7455L_REG_XOFFH);
+	y = __reg_read_10(mma, MMA7455L_REG_YOFFL, MMA7455L_REG_YOFFH);
+	z = __reg_read_10(mma, MMA7455L_REG_ZOFFL, MMA7455L_REG_ZOFFH);
+	mutex_unlock(&mma->lock);
+
+	return sprintf(buf, "%d %d %d\n", x, y, z);
+}
+
+static ssize_t write_mode(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mma7455l_info *mma = dev_get_drvdata(dev);
+
+	if (!strncmp(buf, "Standby", count))
+		update_mode(mma, MMA7455L_MODE_STANDBY, mma->gSelect);
+	else if (!strncmp(buf, "Measurement", count))
+		update_mode(mma, MMA7455L_MODE_MEASUREMENT, mma->gSelect);
+	else if (!strncmp(buf, "Level Detection", count))
+		update_mode(mma, MMA7455L_MODE_LEVELDETECTION, mma->gSelect);
+	else if (!strncmp(buf, "Pulse Detection", count))
+		update_mode(mma, MMA7455L_MODE_PULSEDETECTION, mma->gSelect);
+
+	return count;
+}
+
+static ssize_t write_gSelect(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned long v;
+	struct mma7455l_info *mma = dev_get_drvdata(dev);
+
+	if (strict_strtoul(buf, 10, &v) == 0) {
+		switch (v) {
+		case 8:
+			update_mode(mma, mma->mode, MMA7455L_GSELECT_8);
+			break;
+		case 4:
+			update_mode(mma, mma->mode, MMA7455L_GSELECT_4);
+			break;
+		case 2:
+			update_mode(mma, mma->mode, MMA7455L_GSELECT_2);
+			break;
+		default:
+			return -EINVAL;
+			break;
+		}
+		return count;
+	}
+
+	return -EINVAL;
+}
+
+static ssize_t write_level_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned long v;
+	struct mma7455l_info *mma = dev_get_drvdata(dev);
+
+	if (strict_strtoul(buf, 10, &v) == 0) {
+		if (v <= 0xFF) {
+			reg_write(mma, MMA7455L_REG_LDTH, v);
+			return count;
+		} else
+			return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static ssize_t write_calibration(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int x, y, z;
+	struct mma7455l_info *mma = dev_get_drvdata(dev);
+
+	if (sscanf(buf, "%d %d %d", &x, &y, &z) == 3) {
+		mutex_lock(&mma->lock);
+		__reg_write_10(mma, MMA7455L_REG_XOFFL, MMA7455L_REG_XOFFH, x);
+		__reg_write_10(mma, MMA7455L_REG_YOFFL, MMA7455L_REG_YOFFH, y);
+		__reg_write_10(mma, MMA7455L_REG_ZOFFL, MMA7455L_REG_ZOFFH, z);
+		mutex_unlock(&mma->lock);
+
+		return count;
+	}
+
+	return -EINVAL;
+}
+
+static DEVICE_ATTR(measure, S_IRUGO, show_measure, NULL);
+static DEVICE_ATTR(mode, S_IRUGO | S_IWUGO, show_mode, write_mode);
+static DEVICE_ATTR(gSelect, S_IRUGO | S_IWUGO, show_gSelect, write_gSelect);
+static DEVICE_ATTR(level_threshold, S_IRUGO | S_IWUGO, show_level_threshold,
+			write_level_threshold);
+static DEVICE_ATTR(calibration, S_IRUGO | S_IWUGO, show_calibration,
+			write_calibration);
+
+static struct attribute *mma7455l_sysfs_entries[] = {
+       &dev_attr_measure.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_gSelect.attr,
+	&dev_attr_level_threshold.attr,
+	&dev_attr_calibration.attr,
+	NULL
+};
+
+static struct attribute_group mma7455l_attr_group = {
+       .attrs  = mma7455l_sysfs_entries,
+};
+
+/* input device handling and driver core interaction */
+static int mma7455l_input_open(struct input_dev *inp)
+{
+	struct mma7455l_info *mma = input_get_drvdata(inp);
+	if (mma->mode == MMA7455L_MODE_STANDBY)
+		update_mode(mma, MMA7455L_MODE_MEASUREMENT, mma->gSelect);
+
+	return 0;
+}
+
+static void mma7455l_input_close(struct input_dev *inp)
+{
+	struct mma7455l_info *mma = input_get_drvdata(inp);
+	update_mode(mma, MMA7455L_MODE_STANDBY, MMA7455L_GSELECT_2);
+}
+
+static int __devinit mma7455l_probe(struct i2c_client * client , const struct i2c_device_id * id )
+{
+	int rc;
+	struct mma7455l_info *mma;
+	u_int8_t wai;
+
+	mma = kzalloc(sizeof(*mma), GFP_KERNEL);
+	if (!mma)
+		return -ENOMEM;
+
+	mutex_init(&mma->lock);
+	INIT_DELAYED_WORK(&mma->work, mma7455l_work);
+	mma->client = client;
+	mma->flags = 0;
+	mma->working = 0;
+
+	/* Set initial calibration data */
+	mma->calibration_x = 0;
+	mma->calibration_y = 0;
+	mma->calibration_z = 0;
+
+	i2c_set_clientdata(client, mma);
+
+	wai = reg_read(mma, MMA7455L_REG_WHOAMI);
+	if (wai != MMA7455L_WHOAMI_MAGIC) {
+		printk(KERN_ERR
+			"mma7455l unknown whoami signature 0x%02x\n", wai);
+		dev_set_drvdata(&client->dev, NULL);
+		kfree(mma);
+		return -ENODEV;
+	}
+
+	rc = request_irq(client->irq, mma7455l_interrupt,
+			IRQF_TRIGGER_HIGH, "mma7455l", mma);
+	if (rc < 0) {
+		dev_err(&client->dev, "mma7455l error requesting IRQ %d\n",
+				client->irq);
+		/* FIXME */
+		return rc;
+	}
+
+	rc = sysfs_create_group(&client->dev.kobj, &mma7455l_attr_group);
+	if (rc) {
+		dev_err(&client->dev, "error creating sysfs group\n");
+		return rc;
+	}
+
+	/* initialize input layer details */
+	mma->input_dev = input_allocate_device();
+	if (!mma->input_dev) {
+		dev_err(&client->dev,
+			"mma7455l Unable to allocate input device\n");
+		/* FIXME */
+	}
+
+	set_bit(EV_ABS, mma->input_dev->evbit);
+	set_bit(ABS_X, mma->input_dev->absbit);
+	set_bit(ABS_Y, mma->input_dev->absbit);
+	set_bit(ABS_Z, mma->input_dev->absbit);
+
+	input_set_drvdata(mma->input_dev, mma);
+	mma->input_dev->name = "MMA7455L";
+	mma->input_dev->open = mma7455l_input_open;
+	mma->input_dev->close = mma7455l_input_close;
+
+	rc = input_register_device(mma->input_dev);
+	if (!rc) {
+		update_mode(mma, MMA7455L_MODE_STANDBY, MMA7455L_GSELECT_2);
+
+		mutex_lock(&mma->lock);
+		__reg_write_10(mma, MMA7455L_REG_XOFFL,
+				MMA7455L_REG_XOFFH, mma->calibration_x);
+		__reg_write_10(mma, MMA7455L_REG_YOFFL,
+				MMA7455L_REG_YOFFH, mma->calibration_y);
+		__reg_write_10(mma, MMA7455L_REG_ZOFFL,
+				MMA7455L_REG_ZOFFH, mma->calibration_z);
+		mutex_unlock(&mma->lock);
+
+		return 0;
+	}
+
+	input_free_device(mma->input_dev);
+	return rc;
+}
+
+static int __devexit mma7455l_remove(struct i2c_client *client)
+{
+	struct mma7455l_info *mma = dev_get_drvdata(&client->dev);
+
+	sysfs_remove_group(&client->dev.kobj, &mma7455l_attr_group);
+	input_unregister_device(mma->input_dev);
+	dev_set_drvdata(&client->dev, NULL);
+	kfree(mma);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int mma7455l_suspend(struct i2c_client *client, pm_message_t message)
+{
+	struct mma7455l_info *mma = dev_get_drvdata(&client->dev);
+	get_mode(mma, &mma->mode, &mma->gSelect);
+	set_mode(mma, MMA7455L_MODE_STANDBY, MMA7455L_GSELECT_2);
+
+	return 0;
+}
+
+static int mma7455l_resume(struct i2c_client *client)
+{
+	struct mma7455l_info *mma = dev_get_drvdata(&client->dev);
+	update_mode(mma, mma->mode, mma->gSelect);
+
+	return 0;
+}
+#else
+#define mma7455l_suspend NULL
+#define mma7455l_resume  NULL
+#endif
+
+static const struct i2c_device_id mma7455l_id[] = {
+	{ "mma7455l", 1 },
+	{ },
+};
+
+static struct i2c_driver mma7455l_driver = {
+	.driver = {
+		.name	= "mma7455l",
+		.owner	= THIS_MODULE,
+	},
+
+	.probe	 	= mma7455l_probe,
+	.remove	 	= __devexit_p(mma7455l_remove),
+	.suspend 	= mma7455l_suspend,
+	.resume	 	= mma7455l_resume,
+	.id_table	= mma7455l_id,
+};
+
+static int __init mma7455l_init(void)
+{
+	return i2c_add_driver(&mma7455l_driver);
+}
+
+static void __exit mma7455l_exit(void)
+{
+	i2c_del_driver(&mma7455l_driver);
+}
+
+MODULE_AUTHOR("Gregoire Gentil <gregoire@gentil.com>");
+MODULE_LICENSE("GPL");
+
+module_init(mma7455l_init);
diff --git a/drivers/input/misc/stmp3xxx_rotdec.c b/drivers/input/misc/stmp3xxx_rotdec.c
new file mode 100644
index 000000000000..505a79b25769
--- /dev/null
+++ b/drivers/input/misc/stmp3xxx_rotdec.c
@@ -0,0 +1,171 @@
+/*
+ * Freescale STMP3XXX Rotary Encoder Driver
+ *
+ * Author: Drew Benedetti <drewb@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/input-polldev.h>
+#include <mach/regs-timrot.h>
+#include <mach/rotdec.h>
+
+static int relative;
+static unsigned int poll_interval = 500;
+
+void stmp3xxx_rotdec_flush(struct input_polled_dev *dev)
+{
+	/* in relative mode, reading the counter resets it */
+	if (relative)
+		HW_TIMROT_ROTCOUNT_RD();
+}
+
+void stmp3xxx_rotdec_poll(struct input_polled_dev *dev)
+{
+	s16 cnt = HW_TIMROT_ROTCOUNT_RD() & BM_TIMROT_ROTCOUNT_UPDOWN;
+	if (relative)
+		input_report_rel(dev->input, REL_WHEEL, cnt);
+	else
+		input_report_abs(dev->input, ABS_WHEEL, cnt);
+}
+
+struct input_polled_dev *rotdec;
+static u32 rotctrl;
+
+static int stmp3xxx_rotdec_probe(struct platform_device *pdev)
+{
+	int rc = 0;
+
+	/* save original state of HW_TIMROT_ROTCTRL */
+	rotctrl = HW_TIMROT_ROTCTRL_RD();
+
+	if (!(rotctrl & BM_TIMROT_ROTCTRL_ROTARY_PRESENT)) {
+		dev_info(&pdev->dev, "No rotary decoder present\n");
+		rc = -ENODEV;
+		goto err_rotdec_present;
+	} else {
+		/* I had to add some extra line breaks in here
+		 * to avoid lines >80 chars wide
+		 */
+		HW_TIMROT_ROTCTRL_WR(
+		 BF_TIMROT_ROTCTRL_DIVIDER(0x0) | /* 32kHz divider - 1 */
+		 BF_TIMROT_ROTCTRL_OVERSAMPLE(
+			BV_TIMROT_ROTCTRL_OVERSAMPLE__2X) |
+		 BF_TIMROT_ROTCTRL_SELECT_B(
+			BV_TIMROT_ROTCTRL_SELECT_B__ROTARYB) |
+		 BF_TIMROT_ROTCTRL_SELECT_A(
+			BV_TIMROT_ROTCTRL_SELECT_A__ROTARYA)
+		);
+		HW_TIMROT_ROTCTRL_CLR(
+		 BM_TIMROT_ROTCTRL_POLARITY_B |
+		 BM_TIMROT_ROTCTRL_POLARITY_A
+		);
+
+		if (relative)
+			HW_TIMROT_ROTCTRL_SET(BM_TIMROT_ROTCTRL_RELATIVE);
+		else
+			HW_TIMROT_ROTCTRL_CLR(BM_TIMROT_ROTCTRL_RELATIVE);
+
+		rc = rotdec_pinmux_request();
+		if (rc) {
+			dev_err(&pdev->dev,
+				"Pin request failed (err=%d)\n", rc);
+			goto err_pinmux;
+		}
+
+		/* set up input_polled_dev */
+		rotdec = input_allocate_polled_device();
+		if (!rotdec) {
+			dev_err(&pdev->dev,
+				"Unable to allocate polled device\n");
+			rc = -ENOMEM;
+			goto err_alloc_polldev;
+		}
+		rotdec->flush = stmp3xxx_rotdec_flush;
+		rotdec->poll = stmp3xxx_rotdec_poll;
+		rotdec->poll_interval = poll_interval; /* msec */
+
+		rotdec->input->name = "stmp3xxx-rotdec";
+		if (relative)
+			input_set_capability(rotdec->input, EV_REL, REL_WHEEL);
+		else {
+			input_set_capability(rotdec->input, EV_ABS, ABS_WHEEL);
+			input_set_abs_params(rotdec->input, ABS_WHEEL,
+					-32768, 32767, 0, 0);
+		}
+
+		rc = input_register_polled_device(rotdec);
+		if (rc) {
+			dev_err(&pdev->dev,
+				"Unable to register rotary decoder (err=%d)\n",
+				rc);
+			goto err_reg_polldev;
+		}
+	}
+
+	return 0;
+
+err_reg_polldev:
+	input_free_polled_device(rotdec);
+err_alloc_polldev:
+	rotdec_pinmux_free();
+err_pinmux:
+	/* restore original register state */
+	HW_TIMROT_ROTCTRL_WR(rotctrl);
+
+err_rotdec_present:
+	return rc;
+}
+
+static int stmp3xxx_rotdec_remove(struct platform_device *pdev)
+{
+	input_unregister_polled_device(rotdec);
+	input_free_polled_device(rotdec);
+
+	rotdec_pinmux_free();
+
+	/* restore original register state */
+	HW_TIMROT_ROTCTRL_WR(rotctrl);
+
+	return 0;
+}
+
+static struct platform_driver stmp3xxx_rotdec_driver = {
+	.probe	= stmp3xxx_rotdec_probe,
+	.remove	= stmp3xxx_rotdec_remove,
+	.driver	= {
+		.name	= "stmp3xxx-rotdec",
+	},
+};
+
+static int __init stmp3xxx_rotdec_init(void)
+{
+	return platform_driver_register(&stmp3xxx_rotdec_driver);
+}
+
+static void __exit stmp3xxx_rotdec_exit(void)
+{
+	platform_driver_unregister(&stmp3xxx_rotdec_driver);
+}
+
+module_init(stmp3xxx_rotdec_init);
+module_exit(stmp3xxx_rotdec_exit);
+
+module_param(relative, bool, 0600);
+module_param(poll_interval, uint, 0600);
+
+MODULE_AUTHOR("Drew Benedetti <drewb@embeddedalley.com>");
+MODULE_DESCRIPTION("STMP3xxx rotary decoder driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 3d1ab8fa9acc..98c57a981139 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -71,6 +71,18 @@ config TOUCHSCREEN_FUJITSU
 	  To compile this driver as a module, choose M here: the
 	  module will be called fujitsu-ts.
 
+config TOUCHSCREEN_S3C
+        tristate "S3C touchscreen driver"
+        depends on ARCH_S3C2410
+        help
+          Say Y here to enable the driver for the touchscreen on the
+          S3C SMDK board.
+
+          If unsure, say N.
+
+          To compile this driver as a module, choose M here: the
+          module will be called s3c24xx_ts.
+	  
 config TOUCHSCREEN_GUNZE
 	tristate "Gunze AHL-51S touchscreen"
 	select SERIO
@@ -150,6 +162,47 @@ config TOUCHSCREEN_HP7XX
 	  To compile this driver as a module, choose M here: the
 	  module will be called jornada720_ts.
 
+config TOUCHSCREEN_MXC
+	tristate "MXC touchscreen input driver"
+	depends on MXC_MC13783_ADC || MXC_MC13892_ADC
+	help
+	  Say Y here if you have an MXC based board with touchscreen
+	  attached to it.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mxc_ts.
+
+config TOUCHSCREEN_IMX_ADC
+	tristate "Freescale i.MX ADC touchscreen input driver"
+	depends on IMX_ADC
+	help
+	  Say Y here if you have a Freescale i.MX based board with a
+	  touchscreen interfaced to the processor's integrated ADC.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx_adc_ts.
+
+config TOUCHSCREEN_TSC2007
+	tristate "TI Touch Screen Controller Chip TSC2007"
+	depends on ARCH_MX51 || ARCH_MX37 || MACH_MX35_3DS
+	help
+	  If you say yes here you get support for TSC2007 touch screen controller chip.
+	 
+	  This driver can also be built as a module.  If so, the module
+	  will be called tsc2007.
+
+config TOUCHSCREEN_STMP3XXX
+	tristate "STMP3XXX LRADC-based touchscreen"
+	depends on ARCH_STMP3XXX
+	select SERIO
+	help
+	  Say Y here if you want to enable TMP3XXX LRADC-based touchscreen.
+	  module will be called stmp3xxx_ts.
+
 config TOUCHSCREEN_HTCPEN
 	tristate "HTC Shift X9500 touchscreen"
 	depends on ISA
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 15cf29079489..44febf8308bc 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -20,14 +20,19 @@ obj-$(CONFIG_TOUCHSCREEN_MK712)		+= mk712.o
 obj-$(CONFIG_TOUCHSCREEN_HP600)		+= hp680_ts_input.o
 obj-$(CONFIG_TOUCHSCREEN_HP7XX)		+= jornada720_ts.o
 obj-$(CONFIG_TOUCHSCREEN_HTCPEN)	+= htcpen.o
+obj-$(CONFIG_TOUCHSCREEN_MXC)		+= mxc_ts.o
+obj-$(CONFIG_TOUCHSCREEN_IMX_ADC)	+= imx_adc_ts.o
 obj-$(CONFIG_TOUCHSCREEN_USB_COMPOSITE)	+= usbtouchscreen.o
 obj-$(CONFIG_TOUCHSCREEN_PENMOUNT)	+= penmount.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHIT213)	+= touchit213.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHRIGHT)	+= touchright.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHWIN)	+= touchwin.o
 obj-$(CONFIG_TOUCHSCREEN_UCB1400)	+= ucb1400_ts.o
+obj-$(CONFIG_TOUCHSCREEN_TSC2007)	+= tsc2007.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX)	+= wm97xx-ts.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9705)	+= wm9705.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9712)	+= wm9712.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9713)	+= wm9713.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX_MAINSTONE)	+= mainstone-wm97xx.o
+obj-$(CONFIG_TOUCHSCREEN_S3C)           += s3c24xx_ts.o
+obj-$(CONFIG_TOUCHSCREEN_STMP3XXX)	+= stmp3xxx_ts.o
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index b9b7fc6ff1eb..5ebfda209fb4 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -17,14 +17,11 @@
  *  it under the terms of the GNU General Public License version 2 as
  *  published by the Free Software Foundation.
  */
-#include <linux/hwmon.h>
+#include <linux/device.h>
 #include <linux/init.h>
-#include <linux/err.h>
 #include <linux/delay.h>
 #include <linux/input.h>
 #include <linux/interrupt.h>
-#include <linux/slab.h>
-#include <linux/gpio.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/ads7846.h>
 #include <asm/irq.h>
@@ -33,9 +30,7 @@
 /*
  * This code has been heavily tested on a Nokia 770, and lightly
  * tested on other ads7846 devices (OSK/Mistral, Lubbock).
- * TSC2046 is just newer ads7846 silicon.
- * Support for ads7843 tested on Atmel at91sam926x-EK.
- * Support for ads7845 has only been stubbed in.
+ * Support for ads7843 and ads7845 has only been stubbed in.
  *
  * IRQ handling needs a workaround because of a shortcoming in handling
  * edge triggered IRQs on some platforms like the OMAP1/2. These
@@ -51,8 +46,7 @@
  * files.
  */
 
-#define TS_POLL_DELAY	(1 * 1000000)	/* ns delay before the first sample */
-#define TS_POLL_PERIOD	(5 * 1000000)	/* ns delay between samples */
+#define	TS_POLL_PERIOD	msecs_to_jiffies(10)
 
 /* this driver doesn't aim at the peak continuous sample rate */
 #define	SAMPLE_BITS	(8 /*cmd*/ + 16 /*sample*/ + 2 /* before, after */)
@@ -61,72 +55,57 @@ struct ts_event {
 	/* For portability, we can't read 12 bit values using SPI (which
 	 * would make the controller deliver them as native byteorder u16
 	 * with msbs zeroed).  Instead, we read them as two 8-bit values,
-	 * *** WHICH NEED BYTESWAPPING *** and range adjustment.
+	 * which need byteswapping then range adjustment.
 	 */
-	u16	x;
-	u16	y;
-	u16	z1, z2;
-	int	ignore;
-};
-
-/*
- * We allocate this separately to avoid cache line sharing issues when
- * driver is used with DMA-based SPI controllers (like atmel_spi) on
- * systems where main memory is not DMA-coherent (most non-x86 boards).
- */
-struct ads7846_packet {
-	u8			read_x, read_y, read_z1, read_z2, pwrdown;
-	u16			dummy;		/* for the pwrdown read */
-	struct ts_event		tc;
+	__be16 x;
+	__be16 y;
+	__be16 z1, z2;
+	int    ignore;
 };
 
 struct ads7846 {
 	struct input_dev	*input;
 	char			phys[32];
 
-	struct spi_device	*spi;
-
-#if defined(CONFIG_HWMON) || defined(CONFIG_HWMON_MODULE)
-	struct attribute_group	*attr_group;
-	struct device		*hwmon;
-#endif
+	u32			*txbuf;
+	u32			*rxbuf;
+	u8			buflen;
+	u8			skip_samples;
+	u16			rotate;
 
+	struct spi_device	*spi;
 	u16			model;
-	u16			vref_mv;
 	u16			vref_delay_usecs;
 	u16			x_plate_ohms;
 	u16			pressure_max;
 
-	struct ads7846_packet	*packet;
+	u8			read_x, read_y, read_z1, read_z2, pwrdown;
+	u16                     zerro;          /* to send zerros while receiving */
+	u16			dummy;		/* for the pwrdown read */
+	struct ts_event		tc;
 
-	struct spi_transfer	xfer[18];
+	struct spi_transfer	xfer[10];
 	struct spi_message	msg[5];
 	struct spi_message	*last_msg;
 	int			msg_idx;
 	int			read_cnt;
 	int			read_rep;
 	int			last_read;
+	int			skip_this_sample;
 
 	u16			debounce_max;
 	u16			debounce_tol;
 	u16			debounce_rep;
 
-	u16			penirq_recheck_delay_usecs;
-
 	spinlock_t		lock;
-	struct hrtimer		timer;
+	struct timer_list	timer;		/* P: lock */
 	unsigned		pendown:1;	/* P: lock */
 	unsigned		pending:1;	/* P: lock */
 // FIXME remove "irq_disabled"
 	unsigned		irq_disabled:1;	/* P: lock */
 	unsigned		disabled:1;
-	unsigned		is_suspended:1;
 
-	int			(*filter)(void *data, int data_idx, int *val);
-	void			*filter_data;
-	void			(*filter_cleanup)(void *data);
 	int			(*get_pendown_state)(void);
-	int			gpio_pendown;
 };
 
 /* leave chip selected when we're done, for quicker re-select? */
@@ -136,6 +115,7 @@ struct ads7846 {
 #define	CS_CHANGE(xfer)	((xfer).cs_change = 0)
 #endif
 
+
 /*--------------------------------------------------------------------------*/
 
 /* The ADS7846 has touchscreen and other sensors.
@@ -162,16 +142,19 @@ struct ads7846 {
 #define	MAX_12BIT	((1<<12)-1)
 
 /* leave ADC powered up (disables penirq) between differential samples */
-#define	READ_12BIT_DFR(x, adc, vref) (ADS_START | ADS_A2A1A0_d_ ## x \
-	| ADS_12_BIT | ADS_DFR | \
-	(adc ? ADS_PD10_ADC_ON : 0) | (vref ? ADS_PD10_REF_ON : 0))
+#define	READ_12BIT_DFR(x) (ADS_START | ADS_A2A1A0_d_ ## x \
+	| ADS_12_BIT | ADS_DFR)
+
+#define	READ_Y	(READ_12BIT_DFR(y)  | ADS_PD10_ADC_ON)
+#define	READ_Z1	(READ_12BIT_DFR(z1) | ADS_PD10_ADC_ON)
+#define	READ_Z2	(READ_12BIT_DFR(z2) | ADS_PD10_ADC_ON)
 
-#define	READ_Y(vref)	(READ_12BIT_DFR(y,  1, vref))
-#define	READ_Z1(vref)	(READ_12BIT_DFR(z1, 1, vref))
-#define	READ_Z2(vref)	(READ_12BIT_DFR(z2, 1, vref))
+#define	READ_X	(READ_12BIT_DFR(x)  | ADS_PD10_ADC_ON)
+#define	PWRDOWN	(READ_12BIT_DFR(y)  | ADS_PD10_PDOWN)	/* LAST */
 
-#define	READ_X(vref)	(READ_12BIT_DFR(x,  1, vref))
-#define	PWRDOWN		(READ_12BIT_DFR(y,  0, 0))	/* LAST */
+/* alternate ads7843 commands */
+#define	ALT_READ_Y      (READ_12BIT_DFR(y)  | ADS_PD10_ALL_ON)
+#define	ALT_READ_X      (READ_12BIT_DFR(x)  | ADS_PD10_ALL_ON)
 
 /* single-ended samples need to first power up reference voltage;
  * we leave both ADC and VREF powered
@@ -179,15 +162,20 @@ struct ads7846 {
 #define	READ_12BIT_SER(x) (ADS_START | ADS_A2A1A0_ ## x \
 	| ADS_12_BIT | ADS_SER)
 
-#define	REF_ON	(READ_12BIT_DFR(x, 1, 1))
-#define	REF_OFF	(READ_12BIT_DFR(y, 0, 0))
+#define	REF_ON	(READ_12BIT_DFR(x) | ADS_PD10_ALL_ON)
+#define	REF_OFF	(READ_12BIT_DFR(y) | ADS_PD10_PDOWN)
+
+#define	MAX_BUF_SAMPLE_LEN	(20)
+/* Following configuration should be done in the platform configuration */
+#define	SCREEN_LANDSCAPE	1
+#undef	SCREEN_PORTRAIT
+#define MAX_DIFF_BETWEEN_SAMPLES	100
+
 
 /*--------------------------------------------------------------------------*/
 
 /*
  * Non-touchscreen sensors only use single-ended conversions.
- * The range is GND..vREF. The ads7843 and ads7835 must use external vREF;
- * ads7846 lets that pin be unconnected, to use internal vREF.
  */
 
 struct ser_req {
@@ -195,6 +183,7 @@ struct ser_req {
 	u8			command;
 	u8			ref_off;
 	u16			scratch;
+	u16                     zerro;
 	__be16			sample;
 	struct spi_message	msg;
 	struct spi_transfer	xfer[6];
@@ -206,62 +195,94 @@ static void ads7846_disable(struct ads7846 *ts);
 static int device_suspended(struct device *dev)
 {
 	struct ads7846 *ts = dev_get_drvdata(dev);
-	return ts->is_suspended || ts->disabled;
+	return dev->power.power_state.event != PM_EVENT_ON || ts->disabled;
+}
+
+static int ads7843_setup_buffers(struct device *dev)
+{
+	struct ads7846		*ts = dev_get_drvdata(dev);
+	int i;
+
+	ts->txbuf = kzalloc(sizeof(u32) * ts->buflen * 3, GFP_KERNEL);
+	if (!ts->txbuf)
+		return -ENOMEM;
+
+	ts->rxbuf = kzalloc(sizeof(u32) * ts->buflen * 3, GFP_KERNEL);
+	if (!ts->rxbuf) {
+		kfree(ts->txbuf);
+		return -ENOMEM;
+	}
+	for (i = 0; i < ((ts->buflen * 3) / 2); i++)
+#if defined( SCREEN_LANDSCAPE )
+		ts->txbuf[i] = (READ_12BIT_DFR(x) | ADS_PD10_PDOWN) << 8;
+#else
+		ts->txbuf[i] = (READ_12BIT_DFR(y) | ADS_PD10_PDOWN) << 8;
+#endif
+	for (; i < ts->buflen * 3; i++)
+#if defined( SCREEN_LANDSCAPE )
+		ts->txbuf[i] = (READ_12BIT_DFR(y) | ADS_PD10_PDOWN) << 8;
+#else
+		ts->txbuf[i] = (READ_12BIT_DFR(x) | ADS_PD10_PDOWN) << 8;
+#endif
+	return 0;
 }
 
+
 static int ads7846_read12_ser(struct device *dev, unsigned command)
 {
 	struct spi_device	*spi = to_spi_device(dev);
 	struct ads7846		*ts = dev_get_drvdata(dev);
 	struct ser_req		*req = kzalloc(sizeof *req, GFP_KERNEL);
 	int			status;
-	int			use_internal;
+	int			sample;
+	int			i;
 
 	if (!req)
 		return -ENOMEM;
 
 	spi_message_init(&req->msg);
 
-	/* FIXME boards with ads7846 might use external vref instead ... */
-	use_internal = (ts->model == 7846);
-
-	/* maybe turn on internal vREF, and let it settle */
-	if (use_internal) {
-		req->ref_on = REF_ON;
-		req->xfer[0].tx_buf = &req->ref_on;
-		req->xfer[0].len = 1;
-		spi_message_add_tail(&req->xfer[0], &req->msg);
-
-		req->xfer[1].rx_buf = &req->scratch;
-		req->xfer[1].len = 2;
-
-		/* for 1uF, settle for 800 usec; no cap, 100 usec.  */
-		req->xfer[1].delay_usecs = ts->vref_delay_usecs;
-		spi_message_add_tail(&req->xfer[1], &req->msg);
-	}
+	/* activate reference, so it has time to settle; */
+	req->ref_on = REF_ON;
+	req->xfer[0].tx_buf = &req->ref_on;
+	req->xfer[0].len = 1;
+	req->xfer[1].tx_buf = &req->zerro;
+	req->xfer[1].rx_buf = &req->scratch;
+	req->xfer[1].len = 2;
+
+	/*
+	 * for external VREF, 0 usec (and assume it's always on);
+	 * for 1uF, use 800 usec;
+	 * no cap, 100 usec.
+	 */
+	req->xfer[1].delay_usecs = ts->vref_delay_usecs;
 
 	/* take sample */
 	req->command = (u8) command;
 	req->xfer[2].tx_buf = &req->command;
 	req->xfer[2].len = 1;
-	spi_message_add_tail(&req->xfer[2], &req->msg);
-
+	req->xfer[3].tx_buf = &req->zerro;
 	req->xfer[3].rx_buf = &req->sample;
 	req->xfer[3].len = 2;
-	spi_message_add_tail(&req->xfer[3], &req->msg);
 
 	/* REVISIT:  take a few more samples, and compare ... */
 
-	/* converter in low power mode & enable PENIRQ */
-	req->ref_off = PWRDOWN;
+	/* turn off reference */
+	req->ref_off = REF_OFF;
 	req->xfer[4].tx_buf = &req->ref_off;
 	req->xfer[4].len = 1;
-	spi_message_add_tail(&req->xfer[4], &req->msg);
-
+	// TODO req->xfer[3].tx_buf = &req->zerro;
+	req->xfer[5].tx_buf = &req->zerro;
 	req->xfer[5].rx_buf = &req->scratch;
 	req->xfer[5].len = 2;
+
 	CS_CHANGE(req->xfer[5]);
-	spi_message_add_tail(&req->xfer[5], &req->msg);
+
+	/* group all the transfers together, so we can't interfere with
+	 * reading touchscreen state; disable penirq while sampling
+	 */
+	for (i = 0; i < 6; i++)
+		spi_message_add_tail(&req->xfer[i], &req->msg);
 
 	ts->irq_disabled = 1;
 	disable_irq(spi->irq);
@@ -269,184 +290,37 @@ static int ads7846_read12_ser(struct device *dev, unsigned command)
 	ts->irq_disabled = 0;
 	enable_irq(spi->irq);
 
-	if (status == 0) {
-		/* on-wire is a must-ignore bit, a BE12 value, then padding */
-		status = be16_to_cpu(req->sample);
-		status = status >> 3;
-		status &= 0x0fff;
-	}
+	if (req->msg.status)
+		status = req->msg.status;
+
+	/* on-wire is a must-ignore bit, a BE12 value, then padding */
+	sample = be16_to_cpu(req->sample);
+	sample = sample >> 3;
+	sample &= 0x0fff;
 
 	kfree(req);
-	return status;
+	return status ? status : sample;
 }
 
-#if defined(CONFIG_HWMON) || defined(CONFIG_HWMON_MODULE)
-
-#define SHOW(name, var, adjust) static ssize_t \
+#define SHOW(name) static ssize_t \
 name ## _show(struct device *dev, struct device_attribute *attr, char *buf) \
 { \
-	struct ads7846 *ts = dev_get_drvdata(dev); \
 	ssize_t v = ads7846_read12_ser(dev, \
-			READ_12BIT_SER(var) | ADS_PD10_ALL_ON); \
+			READ_12BIT_SER(name) | ADS_PD10_ALL_ON); \
 	if (v < 0) \
 		return v; \
-	return sprintf(buf, "%u\n", adjust(ts, v)); \
+	return sprintf(buf, "%u\n", (unsigned) v); \
 } \
 static DEVICE_ATTR(name, S_IRUGO, name ## _show, NULL);
 
-
-/* Sysfs conventions report temperatures in millidegrees Celcius.
- * ADS7846 could use the low-accuracy two-sample scheme, but can't do the high
- * accuracy scheme without calibration data.  For now we won't try either;
- * userspace sees raw sensor values, and must scale/calibrate appropriately.
- */
-static inline unsigned null_adjust(struct ads7846 *ts, ssize_t v)
-{
-	return v;
-}
-
-SHOW(temp0, temp0, null_adjust)		/* temp1_input */
-SHOW(temp1, temp1, null_adjust)		/* temp2_input */
-
-
-/* sysfs conventions report voltages in millivolts.  We can convert voltages
- * if we know vREF.  userspace may need to scale vAUX to match the board's
- * external resistors; we assume that vBATT only uses the internal ones.
- */
-static inline unsigned vaux_adjust(struct ads7846 *ts, ssize_t v)
-{
-	unsigned retval = v;
-
-	/* external resistors may scale vAUX into 0..vREF */
-	retval *= ts->vref_mv;
-	retval = retval >> 12;
-	return retval;
-}
-
-static inline unsigned vbatt_adjust(struct ads7846 *ts, ssize_t v)
-{
-	unsigned retval = vaux_adjust(ts, v);
-
-	/* ads7846 has a resistor ladder to scale this signal down */
-	if (ts->model == 7846)
-		retval *= 4;
-	return retval;
-}
-
-SHOW(in0_input, vaux, vaux_adjust)
-SHOW(in1_input, vbatt, vbatt_adjust)
-
-
-static struct attribute *ads7846_attributes[] = {
-	&dev_attr_temp0.attr,
-	&dev_attr_temp1.attr,
-	&dev_attr_in0_input.attr,
-	&dev_attr_in1_input.attr,
-	NULL,
-};
-
-static struct attribute_group ads7846_attr_group = {
-	.attrs = ads7846_attributes,
-};
-
-static struct attribute *ads7843_attributes[] = {
-	&dev_attr_in0_input.attr,
-	&dev_attr_in1_input.attr,
-	NULL,
-};
-
-static struct attribute_group ads7843_attr_group = {
-	.attrs = ads7843_attributes,
-};
-
-static struct attribute *ads7845_attributes[] = {
-	&dev_attr_in0_input.attr,
-	NULL,
-};
-
-static struct attribute_group ads7845_attr_group = {
-	.attrs = ads7845_attributes,
-};
-
-static int ads784x_hwmon_register(struct spi_device *spi, struct ads7846 *ts)
-{
-	struct device *hwmon;
-	int err;
-
-	/* hwmon sensors need a reference voltage */
-	switch (ts->model) {
-	case 7846:
-		if (!ts->vref_mv) {
-			dev_dbg(&spi->dev, "assuming 2.5V internal vREF\n");
-			ts->vref_mv = 2500;
-		}
-		break;
-	case 7845:
-	case 7843:
-		if (!ts->vref_mv) {
-			dev_warn(&spi->dev,
-				"external vREF for ADS%d not specified\n",
-				ts->model);
-			return 0;
-		}
-		break;
-	}
-
-	/* different chips have different sensor groups */
-	switch (ts->model) {
-	case 7846:
-		ts->attr_group = &ads7846_attr_group;
-		break;
-	case 7845:
-		ts->attr_group = &ads7845_attr_group;
-		break;
-	case 7843:
-		ts->attr_group = &ads7843_attr_group;
-		break;
-	default:
-		dev_dbg(&spi->dev, "ADS%d not recognized\n", ts->model);
-		return 0;
-	}
-
-	err = sysfs_create_group(&spi->dev.kobj, ts->attr_group);
-	if (err)
-		return err;
-
-	hwmon = hwmon_device_register(&spi->dev);
-	if (IS_ERR(hwmon)) {
-		sysfs_remove_group(&spi->dev.kobj, ts->attr_group);
-		return PTR_ERR(hwmon);
-	}
-
-	ts->hwmon = hwmon;
-	return 0;
-}
-
-static void ads784x_hwmon_unregister(struct spi_device *spi,
-				     struct ads7846 *ts)
-{
-	if (ts->hwmon) {
-		sysfs_remove_group(&spi->dev.kobj, ts->attr_group);
-		hwmon_device_unregister(ts->hwmon);
-	}
-}
-
-#else
-static inline int ads784x_hwmon_register(struct spi_device *spi,
-					 struct ads7846 *ts)
-{
-	return 0;
-}
-
-static inline void ads784x_hwmon_unregister(struct spi_device *spi,
-					    struct ads7846 *ts)
-{
-}
-#endif
+SHOW(temp0)
+SHOW(temp1)
+SHOW(vaux)
+SHOW(vbatt)
 
 static int is_pen_down(struct device *dev)
 {
-	struct ads7846	*ts = dev_get_drvdata(dev);
+	struct ads7846		*ts = dev_get_drvdata(dev);
 
 	return ts->pendown;
 }
@@ -472,11 +346,10 @@ static ssize_t ads7846_disable_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct ads7846 *ts = dev_get_drvdata(dev);
-	long i;
-
-	if (strict_strtoul(buf, 10, &i))
-		return -EINVAL;
+	char *endp;
+	int i;
 
+	i = simple_strtoul(buf, &endp, 10);
 	spin_lock_irq(&ts->lock);
 
 	if (i)
@@ -491,26 +364,8 @@ static ssize_t ads7846_disable_store(struct device *dev,
 
 static DEVICE_ATTR(disable, 0664, ads7846_disable_show, ads7846_disable_store);
 
-static struct attribute *ads784x_attributes[] = {
-	&dev_attr_pen_down.attr,
-	&dev_attr_disable.attr,
-	NULL,
-};
-
-static struct attribute_group ads784x_attr_group = {
-	.attrs = ads784x_attributes,
-};
-
 /*--------------------------------------------------------------------------*/
 
-static int get_pendown_state(struct ads7846 *ts)
-{
-	if (ts->get_pendown_state)
-		return ts->get_pendown_state();
-
-	return !gpio_get_value(ts->gpio_pendown);
-}
-
 /*
  * PENIRQ only kicks the timer.  The timer only reissues the SPI transfer,
  * to retrieve touchscreen status.
@@ -522,25 +377,25 @@ static int get_pendown_state(struct ads7846 *ts)
 static void ads7846_rx(void *ads)
 {
 	struct ads7846		*ts = ads;
-	struct ads7846_packet	*packet = ts->packet;
+	struct input_dev	*input_dev = ts->input;
 	unsigned		Rt;
+	unsigned		sync = 0;
 	u16			x, y, z1, z2;
+	unsigned long		flags;
 
-	/* ads7846_rx_val() did in-place conversion (including byteswap) from
-	 * on-the-wire format as part of debouncing to get stable readings.
+	/* adjust:  on-wire is a must-ignore bit, a BE12 value, then padding;
+	 * built from two 8 bit values written msb-first.
 	 */
-	x = packet->tc.x;
-	y = packet->tc.y;
-	z1 = packet->tc.z1;
-	z2 = packet->tc.z2;
+	x = (be16_to_cpu(ts->tc.x) >> 3) & 0x0fff;
+	y = (be16_to_cpu(ts->tc.y) >> 3) & 0x0fff;
+	z1 = (be16_to_cpu(ts->tc.z1) >> 3) & 0x0fff;
+	z2 = (be16_to_cpu(ts->tc.z2) >> 3) & 0x0fff;
 
 	/* range filtering */
 	if (x == MAX_12BIT)
 		x = 0;
 
-	if (ts->model == 7843) {
-		Rt = ts->pressure_max / 2;
-	} else if (likely(x && z1)) {
+	if (likely(x && z1 && !device_suspended(&ts->spi->dev))) {
 		/* compute touch pressure resistance using equation #2 */
 		Rt = z2;
 		Rt -= z1;
@@ -548,143 +403,224 @@ static void ads7846_rx(void *ads)
 		Rt *= ts->x_plate_ohms;
 		Rt /= z1;
 		Rt = (Rt + 2047) >> 12;
-	} else {
+	} else
 		Rt = 0;
-	}
 
 	/* Sample found inconsistent by debouncing or pressure is beyond
-	 * the maximum. Don't report it to user space, repeat at least
-	 * once more the measurement
-	 */
-	if (packet->tc.ignore || Rt > ts->pressure_max) {
-#ifdef VERBOSE
-		pr_debug("%s: ignored %d pressure %d\n",
-			ts->spi->dev.bus_id, packet->tc.ignore, Rt);
-#endif
-		hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD),
-			      HRTIMER_MODE_REL);
+	* the maximum. Don't report it to user space, repeat at least
+	* once more the measurement */
+	if (ts->tc.ignore || Rt > ts->pressure_max) {
+		mod_timer(&ts->timer, jiffies + TS_POLL_PERIOD);
 		return;
 	}
 
-	/* Maybe check the pendown state before reporting. This discards
-	 * false readings when the pen is lifted.
+	/* NOTE:  "pendown" is inferred from pressure; we don't rely on
+	 * being able to check nPENIRQ status, or "friendly" trigger modes
+	 * (both-edges is much better than just-falling or low-level).
+	 *
+	 * REVISIT:  some boards may require reading nPENIRQ; it's
+	 * needed on 7843.  and 7845 reads pressure differently...
+	 *
+	 * REVISIT:  the touchscreen might not be connected; this code
+	 * won't notice that, even if nPENIRQ never fires ...
 	 */
-	if (ts->penirq_recheck_delay_usecs) {
-		udelay(ts->penirq_recheck_delay_usecs);
-		if (!get_pendown_state(ts))
-			Rt = 0;
+	if (!ts->pendown && Rt != 0) {
+		input_report_key(input_dev, BTN_TOUCH, 1);
+		sync = 1;
+	} else if (ts->pendown && Rt == 0) {
+		input_report_key(input_dev, BTN_TOUCH, 0);
+		sync = 1;
 	}
 
-	/* NOTE: We can't rely on the pressure to determine the pen down
-	 * state, even this controller has a pressure sensor.  The pressure
-	 * value can fluctuate for quite a while after lifting the pen and
-	 * in some cases may not even settle at the expected value.
-	 *
-	 * The only safe way to check for the pen up condition is in the
-	 * timer by reading the pen signal state (it's a GPIO _and_ IRQ).
-	 */
 	if (Rt) {
-		struct input_dev *input = ts->input;
+		input_report_abs(input_dev, ABS_X, x);
+		input_report_abs(input_dev, ABS_Y, y);
+		sync = 1;
+	}
 
-		if (!ts->pendown) {
-			input_report_key(input, BTN_TOUCH, 1);
-			ts->pendown = 1;
-#ifdef VERBOSE
-			dev_dbg(&ts->spi->dev, "DOWN\n");
+	if (sync) {
+		input_report_abs(input_dev, ABS_PRESSURE, Rt);
+		input_sync(input_dev);
+	}
+
+#ifdef	VERBOSE
+	if (Rt || ts->pendown)
+		pr_debug("%s: %d/%d/%d%s\n", ts->spi->dev.bus_id,
+			x, y, Rt, Rt ? "" : " UP");
 #endif
+
+	spin_lock_irqsave(&ts->lock, flags);
+
+	ts->pendown = (Rt != 0);
+	mod_timer(&ts->timer, jiffies + TS_POLL_PERIOD);
+
+	spin_unlock_irqrestore(&ts->lock, flags);
+}
+
+static inline u16 ad7843_get_sample_val(u32 sample)
+{
+	return (((((sample & 0x00ff0000) >> 8) | (sample >> 24)) >> 3) & 0x0fff);
+}
+
+static u32 ad7843_get_better_values(struct ads7846 *ts, int index)
+{
+	u32 diff12, diff23, diff31;
+	u32 vals[3];
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		vals[i] = ad7843_get_sample_val(ts->rxbuf[index+i]);
+		if (vals[i] == 0x0fff) {
+			ts->skip_this_sample = 1;
+			return 0;
 		}
-		input_report_abs(input, ABS_X, x);
-		input_report_abs(input, ABS_Y, y);
-		input_report_abs(input, ABS_PRESSURE, Rt);
+	}
 
-		input_sync(input);
-#ifdef VERBOSE
-		dev_dbg(&ts->spi->dev, "%4d/%4d/%4d\n", x, y, Rt);
-#endif
+	diff12 = (vals[0] > vals[1]) ? vals[0] - vals[1] : vals[1] - vals[0];
+	if (diff12 > MAX_DIFF_BETWEEN_SAMPLES) {
+		ts->skip_this_sample = 1;
+		return 0;
+	}
+
+	diff23 = (vals[1] > vals[2]) ? vals[1] - vals[2] : vals[2] - vals[1];
+	if (diff23 > MAX_DIFF_BETWEEN_SAMPLES) {
+		ts->skip_this_sample = 1;
+		return 0;
 	}
 
-	hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD),
-			HRTIMER_MODE_REL);
+	diff31 = (vals[2] > vals[0]) ? vals[2] - vals[0] : vals[0] - vals[2];
+	if (diff31 > MAX_DIFF_BETWEEN_SAMPLES) {
+		ts->skip_this_sample = 1;
+		return 0;
+	}
+
+	if (diff12 < diff23 && diff12 < diff31)
+		return (vals[0] + vals[1]) / 2;
+	if (diff23 < diff12 && diff23 < diff31)
+		return (vals[1] + vals[2]) / 2;
+
+	return (vals[0] + vals[2]) / 2;
 }
 
-static int ads7846_debounce(void *ads, int data_idx, int *val)
+static void ads7843_rx_average(void *ads)
+{
+	struct ads7846          *ts = ads;
+	struct input_dev        *input_dev = ts->input;
+	u16                     x, y, temp;
+	unsigned long           flags;
+	int i, sample_count;
+
+	dev_dbg(&ts->spi->dev, "%s\n", __FUNCTION__);
+
+	for (i = 0, y = 0, sample_count = 0; i < (ts->buflen * 3 / 2); i+=3) {
+		if (i >= ts->skip_samples*3) {
+			temp = ad7843_get_better_values(ts, i);
+			if (!ts->skip_this_sample) {
+			if (ts->rotate == 180) {
+					y += MAX_12BIT - temp;
+			} else if (ts->rotate == 0) {
+					y += temp;
+			} else {
+				dev_info(&ts->spi->dev, "Rotate mode %d, not implemented yet\n", ts->rotate);
+			}
+				sample_count++;
+		}
+	}
+		ts->skip_this_sample = 0;
+	}
+
+	if (!sample_count)
+		goto sample_taken;
+
+	y /= sample_count;
+
+	for (x = 0, sample_count = 0; i < (ts->buflen * 3); i+=3) {
+		if (i >= (ts->skip_samples + ts->buflen / 2)*3) {
+			temp = ad7843_get_better_values(ts, i);
+			if (!ts->skip_this_sample) {
+			if (ts->rotate == 180) {
+					x += MAX_12BIT - temp;
+			} else if (ts->rotate == 0) {
+					x += temp;
+			} else {
+				dev_info(&ts->spi->dev, "Rotate mode %d, not implemented yet\n", ts->rotate);
+			}
+				sample_count++;
+		}
+	}
+		ts->skip_this_sample = 0;
+	}
+
+	if (!sample_count)
+		goto sample_taken;
+
+	x /= sample_count;
+
+       if (ts->pendown) {
+               input_report_key(input_dev, BTN_TOUCH, 1);
+               input_report_abs(input_dev, ABS_PRESSURE, ts->pressure_max / 2);
+               input_report_abs(input_dev, ABS_X, x);
+               input_report_abs(input_dev, ABS_Y, y);
+       } else {
+               input_report_key(input_dev, BTN_TOUCH, 0);
+               input_report_abs(input_dev, ABS_PRESSURE, 0);
+       }
+
+sample_taken:
+       input_sync(input_dev);
+	dev_dbg(&ts->spi->dev, "%d/%d %s\n", x, y, ts->pendown ? "" : " UP");
+
+       if (ts->pendown) {
+               spin_lock_irqsave(&ts->lock, flags);
+               mod_timer(&ts->timer, jiffies + TS_POLL_PERIOD);
+               spin_unlock_irqrestore(&ts->lock, flags);
+       }
+}
+
+static void ads7846_debounce(void *ads)
 {
 	struct ads7846		*ts = ads;
+	struct spi_message	*m;
+	struct spi_transfer	*t;
+	int			val;
+	int			status;
 
-	if (!ts->read_cnt || (abs(ts->last_read - *val) > ts->debounce_tol)) {
-		/* Start over collecting consistent readings. */
-		ts->read_rep = 0;
+	m = &ts->msg[ts->msg_idx];
+	t = list_entry(m->transfers.prev, struct spi_transfer, transfer_list);
+	val = (be16_to_cpu(*(__be16 *)t->rx_buf) >> 3) & 0x0fff;
+	if (!ts->read_cnt || (abs(ts->last_read - val) > ts->debounce_tol)) {
 		/* Repeat it, if this was the first read or the read
 		 * wasn't consistent enough. */
 		if (ts->read_cnt < ts->debounce_max) {
-			ts->last_read = *val;
+			ts->last_read = val;
 			ts->read_cnt++;
-			return ADS7846_FILTER_REPEAT;
 		} else {
 			/* Maximum number of debouncing reached and still
 			 * not enough number of consistent readings. Abort
 			 * the whole sample, repeat it in the next sampling
 			 * period.
 			 */
+			ts->tc.ignore = 1;
 			ts->read_cnt = 0;
-			return ADS7846_FILTER_IGNORE;
+			/* Last message will contain ads7846_rx() as the
+			 * completion function.
+			 */
+			m = ts->last_msg;
 		}
+		/* Start over collecting consistent readings. */
+		ts->read_rep = 0;
 	} else {
 		if (++ts->read_rep > ts->debounce_rep) {
 			/* Got a good reading for this coordinate,
 			 * go for the next one. */
+			ts->tc.ignore = 0;
+			ts->msg_idx++;
 			ts->read_cnt = 0;
 			ts->read_rep = 0;
-			return ADS7846_FILTER_OK;
-		} else {
+			m++;
+		} else
 			/* Read more values that are consistent. */
 			ts->read_cnt++;
-			return ADS7846_FILTER_REPEAT;
-		}
-	}
-}
-
-static int ads7846_no_filter(void *ads, int data_idx, int *val)
-{
-	return ADS7846_FILTER_OK;
-}
-
-static void ads7846_rx_val(void *ads)
-{
-	struct ads7846 *ts = ads;
-	struct ads7846_packet *packet = ts->packet;
-	struct spi_message *m;
-	struct spi_transfer *t;
-	int val;
-	int action;
-	int status;
-
-	m = &ts->msg[ts->msg_idx];
-	t = list_entry(m->transfers.prev, struct spi_transfer, transfer_list);
-
-	/* adjust:  on-wire is a must-ignore bit, a BE12 value, then padding;
-	 * built from two 8 bit values written msb-first.
-	 */
-	val = be16_to_cpup((__be16 *)t->rx_buf) >> 3;
-
-	action = ts->filter(ts->filter_data, ts->msg_idx, &val);
-	switch (action) {
-	case ADS7846_FILTER_REPEAT:
-		break;
-	case ADS7846_FILTER_IGNORE:
-		packet->tc.ignore = 1;
-		/* Last message will contain ads7846_rx() as the
-		 * completion function.
-		 */
-		m = ts->last_msg;
-		break;
-	case ADS7846_FILTER_OK:
-		*(u16 *)t->rx_buf = val;
-		packet->tc.ignore = 0;
-		m = &ts->msg[++ts->msg_idx];
-		break;
-	default:
-		BUG();
 	}
 	status = spi_async(ts->spi, m);
 	if (status)
@@ -692,44 +628,51 @@ static void ads7846_rx_val(void *ads)
 				status);
 }
 
-static enum hrtimer_restart ads7846_timer(struct hrtimer *handle)
+static void ads7846_timer(unsigned long handle)
 {
-	struct ads7846	*ts = container_of(handle, struct ads7846, timer);
-	int		status = 0;
+	struct ads7846		*ts = (void *)handle;
+	struct input_dev        *input_dev = ts->input;
+	int			status = 0;
 
+	/* get sample */
+	ts->pendown = ts->get_pendown_state();
 	spin_lock_irq(&ts->lock);
+	if (ts->model == 7843) {
+		ts->pending = 0;
+		if (unlikely(!ts->pendown)) {
 
-	if (unlikely(!get_pendown_state(ts) ||
-		     device_suspended(&ts->spi->dev))) {
-		if (ts->pendown) {
-			struct input_dev *input = ts->input;
-
-			input_report_key(input, BTN_TOUCH, 0);
-			input_report_abs(input, ABS_PRESSURE, 0);
-			input_sync(input);
-
-			ts->pendown = 0;
-#ifdef VERBOSE
-			dev_dbg(&ts->spi->dev, "UP\n");
-#endif
-		}
+			input_report_key(input_dev, BTN_TOUCH, 0);
+			input_report_abs(input_dev, ABS_PRESSURE, 0);
+			input_sync(input_dev);
 
-		/* measurement cycle ended */
-		if (!device_suspended(&ts->spi->dev)) {
-			ts->irq_disabled = 0;
-			enable_irq(ts->spi->irq);
+			if (!device_suspended(&ts->spi->dev)) {
+				ts->irq_disabled = 0;
+				enable_irq(ts->spi->irq);
+			}
+		} else {
+			/* pen is still down, continue with the measurement */
+			status = spi_async(ts->spi, &ts->msg[0]);
+			if (status)
+				dev_err(&ts->spi->dev, "spi_async --> %d\n", status);
 		}
-		ts->pending = 0;
 	} else {
-		/* pen is still down, continue with the measurement */
-		ts->msg_idx = 0;
-		status = spi_async(ts->spi, &ts->msg[0]);
-		if (status)
-			dev_err(&ts->spi->dev, "spi_async --> %d\n", status);
+		if (unlikely(ts->msg_idx && !ts->pendown)) {
+			/* measurement cycle ended */
+			if (!device_suspended(&ts->spi->dev)) {
+				ts->irq_disabled = 0;
+				enable_irq(ts->spi->irq);
+			}
+			ts->pending = 0;
+			ts->msg_idx = 0;
+		} else {
+			/* pen is still down, continue with the measurement */
+			ts->msg_idx = 0;
+			status = spi_async(ts->spi, &ts->msg[0]);
+			if (status)
+				dev_err(&ts->spi->dev, "spi_async --> %d\n", status);
+		}
 	}
-
 	spin_unlock_irq(&ts->lock);
-	return HRTIMER_NORESTART;
 }
 
 static irqreturn_t ads7846_irq(int irq, void *handle)
@@ -738,7 +681,8 @@ static irqreturn_t ads7846_irq(int irq, void *handle)
 	unsigned long flags;
 
 	spin_lock_irqsave(&ts->lock, flags);
-	if (likely(get_pendown_state(ts))) {
+
+	if (likely(ts->get_pendown_state())) {
 		if (!ts->irq_disabled) {
 			/* The ARM do_simple_IRQ() dispatcher doesn't act
 			 * like the other dispatchers:  it will report IRQs
@@ -748,8 +692,7 @@ static irqreturn_t ads7846_irq(int irq, void *handle)
 			ts->irq_disabled = 1;
 			disable_irq(ts->spi->irq);
 			ts->pending = 1;
-			hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_DELAY),
-					HRTIMER_MODE_REL);
+			mod_timer(&ts->timer, jiffies);
 		}
 	}
 	spin_unlock_irqrestore(&ts->lock, flags);
@@ -785,6 +728,7 @@ static void ads7846_disable(struct ads7846 *ts)
 	/* we know the chip's in lowpower mode since we always
 	 * leave it that way after every request
 	 */
+
 }
 
 /* Must be called with ts->lock held */
@@ -804,7 +748,7 @@ static int ads7846_suspend(struct spi_device *spi, pm_message_t message)
 
 	spin_lock_irq(&ts->lock);
 
-	ts->is_suspended = 1;
+	spi->dev.power.power_state = message;
 	ads7846_disable(ts);
 
 	spin_unlock_irq(&ts->lock);
@@ -819,7 +763,7 @@ static int ads7846_resume(struct spi_device *spi)
 
 	spin_lock_irq(&ts->lock);
 
-	ts->is_suspended = 0;
+	spi->dev.power.power_state = PMSG_ON;
 	ads7846_enable(ts);
 
 	spin_unlock_irq(&ts->lock);
@@ -827,45 +771,13 @@ static int ads7846_resume(struct spi_device *spi)
 	return 0;
 }
 
-static int __devinit setup_pendown(struct spi_device *spi, struct ads7846 *ts)
-{
-	struct ads7846_platform_data *pdata = spi->dev.platform_data;
-	int err;
-
-	/* REVISIT when the irq can be triggered active-low, or if for some
-	 * reason the touchscreen isn't hooked up, we don't need to access
-	 * the pendown state.
-	 */
-	if (!pdata->get_pendown_state && !gpio_is_valid(pdata->gpio_pendown)) {
-		dev_err(&spi->dev, "no get_pendown_state nor gpio_pendown?\n");
-		return -EINVAL;
-	}
-
-	if (pdata->get_pendown_state) {
-		ts->get_pendown_state = pdata->get_pendown_state;
-		return 0;
-	}
-
-	err = gpio_request(pdata->gpio_pendown, "ads7846_pendown");
-	if (err) {
-		dev_err(&spi->dev, "failed to request pendown GPIO%d\n",
-				pdata->gpio_pendown);
-		return err;
-	}
-
-	ts->gpio_pendown = pdata->gpio_pendown;
-	return 0;
-}
-
 static int __devinit ads7846_probe(struct spi_device *spi)
 {
 	struct ads7846			*ts;
-	struct ads7846_packet		*packet;
 	struct input_dev		*input_dev;
 	struct ads7846_platform_data	*pdata = spi->dev.platform_data;
 	struct spi_message		*m;
 	struct spi_transfer		*x;
-	int				vref;
 	int				err;
 
 	if (!spi->irq) {
@@ -885,32 +797,46 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 		return -EINVAL;
 	}
 
+	/* REVISIT when the irq can be triggered active-low, or if for some
+	 * reason the touchscreen isn't hooked up, we don't need to access
+	 * the pendown state.
+	 */
+	if (pdata->get_pendown_state == NULL) {
+		dev_dbg(&spi->dev, "no get_pendown_state function?\n");
+		return -EINVAL;
+	}
+
 	/* We'd set TX wordsize 8 bits and RX wordsize to 13 bits ... except
 	 * that even if the hardware can do that, the SPI controller driver
 	 * may not.  So we stick to very-portable 8 bit words, both RX and TX.
 	 */
 	spi->bits_per_word = 8;
-	spi->mode = SPI_MODE_0;
-	err = spi_setup(spi);
-	if (err < 0)
-		return err;
 
 	ts = kzalloc(sizeof(struct ads7846), GFP_KERNEL);
-	packet = kzalloc(sizeof(struct ads7846_packet), GFP_KERNEL);
 	input_dev = input_allocate_device();
-	if (!ts || !packet || !input_dev) {
+	if (!ts || !input_dev) {
 		err = -ENOMEM;
 		goto err_free_mem;
 	}
 
 	dev_set_drvdata(&spi->dev, ts);
 
-	ts->packet = packet;
+	spi->dev.power.power_state = PMSG_ON;
+
 	ts->spi = spi;
 	ts->input = input_dev;
-	ts->vref_mv = pdata->vref_mv;
 
-	hrtimer_init(&ts->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	ts->buflen = pdata->buflen ? : MAX_BUF_SAMPLE_LEN;
+	ts->buflen = ts->buflen & ~0x1;	/* must be even */
+
+	if (ads7843_setup_buffers(&spi->dev)) {
+		dev_dbg(&spi->dev, "error allocating memory for sample buffers\n");
+		err = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	init_timer(&ts->timer);
+	ts->timer.data = (unsigned long) ts;
 	ts->timer.function = ads7846_timer;
 
 	spin_lock_init(&ts->lock);
@@ -919,33 +845,18 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 	ts->vref_delay_usecs = pdata->vref_delay_usecs ? : 100;
 	ts->x_plate_ohms = pdata->x_plate_ohms ? : 400;
 	ts->pressure_max = pdata->pressure_max ? : ~0;
+	ts->skip_samples = pdata->skip_samples ? : 0;
+	ts->rotate = pdata->rotate ? : 0;
 
-	if (pdata->filter != NULL) {
-		if (pdata->filter_init != NULL) {
-			err = pdata->filter_init(pdata, &ts->filter_data);
-			if (err < 0)
-				goto err_free_mem;
-		}
-		ts->filter = pdata->filter;
-		ts->filter_cleanup = pdata->filter_cleanup;
-	} else if (pdata->debounce_max) {
+	if (pdata->debounce_max) {
 		ts->debounce_max = pdata->debounce_max;
-		if (ts->debounce_max < 2)
-			ts->debounce_max = 2;
 		ts->debounce_tol = pdata->debounce_tol;
 		ts->debounce_rep = pdata->debounce_rep;
-		ts->filter = ads7846_debounce;
-		ts->filter_data = ts;
+		if (ts->debounce_rep > ts->debounce_max + 1)
+			ts->debounce_rep = ts->debounce_max - 1;
 	} else
-		ts->filter = ads7846_no_filter;
-
-	err = setup_pendown(spi, ts);
-	if (err)
-		goto err_cleanup_filter;
-
-	if (pdata->penirq_recheck_delay_usecs)
-		ts->penirq_recheck_delay_usecs =
-				pdata->penirq_recheck_delay_usecs;
+		ts->debounce_tol = ~0;
+	ts->get_pendown_state = pdata->get_pendown_state;
 
 	snprintf(ts->phys, sizeof(ts->phys), "%s/input0", spi->dev.bus_id);
 
@@ -964,9 +875,8 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 			pdata->y_max ? : MAX_12BIT,
 			0, 0);
 	input_set_abs_params(input_dev, ABS_PRESSURE,
-			pdata->pressure_min, pdata->pressure_max, 0, 0);
-
-	vref = pdata->keep_vref_on;
+			pdata->pressure_min ? : 0,
+			pdata->pressure_max ? : 1, 0, 0);
 
 	/* set up the transfers to read touchscreen state; this assumes we
 	 * use formula #2 for pressure, not #3.
@@ -976,203 +886,176 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 
 	spi_message_init(m);
 
-	/* y- still on; turn on only y+ (and ADC) */
-	packet->read_y = READ_Y(vref);
-	x->tx_buf = &packet->read_y;
-	x->len = 1;
-	spi_message_add_tail(x, m);
-
-	x++;
-	x->rx_buf = &packet->tc.y;
-	x->len = 2;
-	spi_message_add_tail(x, m);
-
-	/* the first sample after switching drivers can be low quality;
-	 * optionally discard it, using a second one after the signals
-	 * have had enough time to stabilize.
-	 */
-	if (pdata->settle_delay_usecs) {
-		x->delay_usecs = pdata->settle_delay_usecs;
-
-		x++;
-		x->tx_buf = &packet->read_y;
-		x->len = 1;
-		spi_message_add_tail(x, m);
-
-		x++;
-		x->rx_buf = &packet->tc.y;
-		x->len = 2;
+	if (ts->model == 7843) {
+		x->tx_buf = ts->txbuf;
+		x->rx_buf = ts->rxbuf;
+		x->len = ts->buflen * sizeof(u32) * 3; /* For every sample we take 3 samples and choose the better 2 */
 		spi_message_add_tail(x, m);
-	}
 
-	m->complete = ads7846_rx_val;
-	m->context = ts;
-
-	m++;
-	spi_message_init(m);
-
-	/* turn y- off, x+ on, then leave in lowpower */
-	x++;
-	packet->read_x = READ_X(vref);
-	x->tx_buf = &packet->read_x;
-	x->len = 1;
-	spi_message_add_tail(x, m);
-
-	x++;
-	x->rx_buf = &packet->tc.x;
-	x->len = 2;
-	spi_message_add_tail(x, m);
-
-	/* ... maybe discard first sample ... */
-	if (pdata->settle_delay_usecs) {
-		x->delay_usecs = pdata->settle_delay_usecs;
+		m->complete = ads7843_rx_average;
+		m->context = ts;
 
-		x++;
-		x->tx_buf = &packet->read_x;
+		ts->last_msg = m;
+	} else {
+		/* y- still on; turn on only y+ (and ADC) */
+		ts->read_y = READ_Y;
+		x->tx_buf = &ts->read_y;
 		x->len = 1;
 		spi_message_add_tail(x, m);
 
 		x++;
-		x->rx_buf = &packet->tc.x;
+		x->rx_buf = &ts->tc.y;
 		x->len = 2;
 		spi_message_add_tail(x, m);
-	}
 
-	m->complete = ads7846_rx_val;
-	m->context = ts;
+		m->complete = ads7846_debounce;
+		m->context = ts;
 
-	/* turn y+ off, x- on; we'll use formula #2 */
-	if (ts->model == 7846) {
 		m++;
 		spi_message_init(m);
 
+		/* turn y- off, x+ on, then leave in lowpower */
 		x++;
-		packet->read_z1 = READ_Z1(vref);
-		x->tx_buf = &packet->read_z1;
+		ts->read_x = READ_X;
+		x->tx_buf = &ts->read_x;
 		x->len = 1;
 		spi_message_add_tail(x, m);
 
 		x++;
-		x->rx_buf = &packet->tc.z1;
+		x->rx_buf = &ts->tc.x;
 		x->len = 2;
 		spi_message_add_tail(x, m);
 
-		/* ... maybe discard first sample ... */
-		if (pdata->settle_delay_usecs) {
-			x->delay_usecs = pdata->settle_delay_usecs;
+		m->complete = ads7846_debounce;
+		m->context = ts;
+
+		/* turn y+ off, x- on; we'll use formula #2 */
+		if (ts->model == 7846) {
+			m++;
+			spi_message_init(m);
 
 			x++;
-			x->tx_buf = &packet->read_z1;
+			ts->read_z1 = READ_Z1;
+			x->tx_buf = &ts->read_z1;
 			x->len = 1;
 			spi_message_add_tail(x, m);
 
 			x++;
-			x->rx_buf = &packet->tc.z1;
+			x->rx_buf = &ts->tc.z1;
 			x->len = 2;
 			spi_message_add_tail(x, m);
-		}
-
-		m->complete = ads7846_rx_val;
-		m->context = ts;
-
-		m++;
-		spi_message_init(m);
-
-		x++;
-		packet->read_z2 = READ_Z2(vref);
-		x->tx_buf = &packet->read_z2;
-		x->len = 1;
-		spi_message_add_tail(x, m);
 
-		x++;
-		x->rx_buf = &packet->tc.z2;
-		x->len = 2;
-		spi_message_add_tail(x, m);
+			m->complete = ads7846_debounce;
+			m->context = ts;
 
-		/* ... maybe discard first sample ... */
-		if (pdata->settle_delay_usecs) {
-			x->delay_usecs = pdata->settle_delay_usecs;
+			m++;
+			spi_message_init(m);
 
 			x++;
-			x->tx_buf = &packet->read_z2;
+			ts->read_z2 = READ_Z2;
+			x->tx_buf = &ts->read_z2;
 			x->len = 1;
 			spi_message_add_tail(x, m);
 
 			x++;
-			x->rx_buf = &packet->tc.z2;
+			x->rx_buf = &ts->tc.z2;
 			x->len = 2;
 			spi_message_add_tail(x, m);
-		}
 
-		m->complete = ads7846_rx_val;
-		m->context = ts;
-	}
+			m->complete = ads7846_debounce;
+			m->context = ts;
+		}
 
-	/* power down */
-	m++;
-	spi_message_init(m);
+		/* power down */
+		m++;
+		spi_message_init(m);
 
-	x++;
-	packet->pwrdown = PWRDOWN;
-	x->tx_buf = &packet->pwrdown;
-	x->len = 1;
-	spi_message_add_tail(x, m);
+		x++;
+		ts->pwrdown = PWRDOWN;
+		x->tx_buf = &ts->pwrdown;
+		x->len = 1;
+		spi_message_add_tail(x, m);
 
-	x++;
-	x->rx_buf = &packet->dummy;
-	x->len = 2;
-	CS_CHANGE(*x);
-	spi_message_add_tail(x, m);
+		x++;
+		x->rx_buf = &ts->dummy;
+		x->len = 2;
+		CS_CHANGE(*x);
+		spi_message_add_tail(x, m);
 
-	m->complete = ads7846_rx;
-	m->context = ts;
+		m->complete = ads7846_rx;
+		m->context = ts;
 
-	ts->last_msg = m;
+		ts->last_msg = m;
+	}
 
 	if (request_irq(spi->irq, ads7846_irq, IRQF_TRIGGER_FALLING,
 			spi->dev.driver->name, ts)) {
 		dev_dbg(&spi->dev, "irq %d busy?\n", spi->irq);
 		err = -EBUSY;
-		goto err_free_gpio;
+		goto err_free_buf;
 	}
 
-	err = ads784x_hwmon_register(spi, ts);
-	if (err)
-		goto err_free_irq;
-
 	dev_info(&spi->dev, "touchscreen, irq %d\n", spi->irq);
 
-	/* take a first sample, leaving nPENIRQ active and vREF off; avoid
+	/* take a first sample, leaving nPENIRQ active; avoid
 	 * the touchscreen, in case it's not connected.
 	 */
-	(void) ads7846_read12_ser(&spi->dev,
-			  READ_12BIT_SER(vaux) | ADS_PD10_ALL_ON);
+	if (ts->model != 7843) {
+		/* take a first sample, leaving nPENIRQ active; avoid
+		 * the touchscreen, in case it's not connected.
+		 */
+		(void) ads7846_read12_ser(&spi->dev,
+				READ_12BIT_SER(vaux) | ADS_PD10_ALL_ON);
+	}
+
+	/* ads7843/7845 don't have temperature sensors, and
+	 * use the other sensors a bit differently too
+	 */
+	if (ts->model == 7846) {
+		device_create_file(&spi->dev, &dev_attr_temp0);
+		device_create_file(&spi->dev, &dev_attr_temp1);
+	}
 
-	err = sysfs_create_group(&spi->dev.kobj, &ads784x_attr_group);
-	if (err)
-		goto err_remove_hwmon;
+	if (ts->model != 7845 && ts->model != 7843)
+		device_create_file(&spi->dev, &dev_attr_vbatt);
+
+	if (ts->model != 7843) {
+		device_create_file(&spi->dev, &dev_attr_vaux);
+	}
+
+	device_create_file(&spi->dev, &dev_attr_pen_down);
+	device_create_file(&spi->dev, &dev_attr_disable);
 
 	err = input_register_device(input_dev);
 	if (err)
-		goto err_remove_attr_group;
+		goto err_remove_attr;
 
 	return 0;
 
- err_remove_attr_group:
-	sysfs_remove_group(&spi->dev.kobj, &ads784x_attr_group);
- err_remove_hwmon:
-	ads784x_hwmon_unregister(spi, ts);
- err_free_irq:
+ err_remove_attr:
+	device_remove_file(&spi->dev, &dev_attr_disable);
+	device_remove_file(&spi->dev, &dev_attr_pen_down);
+	if (ts->model == 7846) {
+		device_remove_file(&spi->dev, &dev_attr_temp1);
+		device_remove_file(&spi->dev, &dev_attr_temp0);
+	}
+
+	if (ts->model != 7845 && ts->model != 7843)
+		device_remove_file(&spi->dev, &dev_attr_vbatt);
+
+	if (ts->model != 7843) {
+		device_remove_file(&spi->dev, &dev_attr_vaux);
+	}
+
 	free_irq(spi->irq, ts);
- err_free_gpio:
-	if (ts->gpio_pendown != -1)
-		gpio_free(ts->gpio_pendown);
- err_cleanup_filter:
-	if (ts->filter_cleanup)
-		ts->filter_cleanup(ts->filter_data);
+
+ err_free_buf:
+	if (ts->txbuf)
+		kfree(ts->txbuf);
+	if (ts->rxbuf)
+		kfree(ts->rxbuf);
  err_free_mem:
 	input_free_device(input_dev);
-	kfree(packet);
 	kfree(ts);
 	return err;
 }
@@ -1181,24 +1064,33 @@ static int __devexit ads7846_remove(struct spi_device *spi)
 {
 	struct ads7846		*ts = dev_get_drvdata(&spi->dev);
 
-	ads784x_hwmon_unregister(spi, ts);
 	input_unregister_device(ts->input);
 
 	ads7846_suspend(spi, PMSG_SUSPEND);
 
-	sysfs_remove_group(&spi->dev.kobj, &ads784x_attr_group);
+	if (ts->txbuf)
+		kfree(ts->txbuf);
+	if (ts->rxbuf)
+		kfree(ts->rxbuf);
 
-	free_irq(ts->spi->irq, ts);
-	/* suspend left the IRQ disabled */
-	enable_irq(ts->spi->irq);
+	device_remove_file(&spi->dev, &dev_attr_disable);
+	device_remove_file(&spi->dev, &dev_attr_pen_down);
+	if (ts->model == 7846) {
+		device_remove_file(&spi->dev, &dev_attr_temp1);
+		device_remove_file(&spi->dev, &dev_attr_temp0);
+	}
 
-	if (ts->gpio_pendown != -1)
-		gpio_free(ts->gpio_pendown);
+	if (ts->model != 7845 && ts->model != 7843)
+		device_remove_file(&spi->dev, &dev_attr_vbatt);
 
-	if (ts->filter_cleanup)
-		ts->filter_cleanup(ts->filter_data);
+	if (ts->model != 7843) {
+		device_remove_file(&spi->dev, &dev_attr_vaux);
+	}
+
+	free_irq(ts->spi->irq, ts);
+	/* suspend left the IRQ disabled */
+	disable_irq(ts->spi->irq);
 
-	kfree(ts->packet);
 	kfree(ts);
 
 	dev_dbg(&spi->dev, "unregistered touchscreen\n");
diff --git a/drivers/input/touchscreen/imx_adc_ts.c b/drivers/input/touchscreen/imx_adc_ts.c
new file mode 100644
index 000000000000..ec43a16213ae
--- /dev/null
+++ b/drivers/input/touchscreen/imx_adc_ts.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file imx_adc_ts.c
+ *
+ * @brief Driver for the Freescale Semiconductor i.MX ADC touchscreen.
+ *
+ * This touchscreen driver is designed as a standard input driver.  It is a
+ * wrapper around the low level ADC driver. Much of the hardware configuration
+ * and touchscreen functionality is implemented in the low level ADC driver.
+ * During initialization, this driver creates a kernel thread.  This thread
+ * then calls the ADC driver to obtain touchscreen values continously. These
+ * values are then passed to the input susbsystem.
+ *
+ * @ingroup touchscreen
+ */
+
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/input.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/freezer.h>
+#include <linux/imx_adc.h>
+
+#define IMX_ADC_TS_NAME	"imx_adc_ts"
+
+static struct input_dev *imx_inputdev;
+static u32 input_ts_installed;
+
+static int ts_thread(void *arg)
+{
+	struct t_touch_screen ts_sample;
+	int wait = 0;
+	daemonize("imx_adc_ts");
+	while (input_ts_installed) {
+		try_to_freeze();
+
+		memset(&ts_sample, 0, sizeof(ts_sample));
+		if (0 != imx_adc_get_touch_sample(&ts_sample, !wait))
+			continue;
+
+		input_report_abs(imx_inputdev, ABS_X, ts_sample.x_position);
+		input_report_abs(imx_inputdev, ABS_Y, ts_sample.y_position);
+		input_report_abs(imx_inputdev, ABS_PRESSURE,
+				 ts_sample.contact_resistance);
+		input_sync(imx_inputdev);
+		wait = ts_sample.contact_resistance;
+		msleep(10);
+	}
+
+	return 0;
+}
+
+static int __init imx_adc_ts_init(void)
+{
+	int retval;
+
+	if (!is_imx_adc_ready())
+		return -ENODEV;
+
+	imx_inputdev = input_allocate_device();
+	if (!imx_inputdev) {
+		pr_err("imx_ts_init: not enough memory for input device\n");
+		return -ENOMEM;
+	}
+
+	imx_inputdev->name = IMX_ADC_TS_NAME;
+	imx_inputdev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+	imx_inputdev->keybit[BIT_WORD(BTN_TOUCH)] |= BIT_MASK(BTN_TOUCH);
+	imx_inputdev->absbit[0] =
+	    BIT_MASK(ABS_X) | BIT_MASK(ABS_Y) | BIT_MASK(ABS_PRESSURE);
+	retval = input_register_device(imx_inputdev);
+	if (retval < 0) {
+		input_free_device(imx_inputdev);
+		return retval;
+	}
+
+	input_ts_installed = 1;
+	kthread_run(ts_thread, NULL, "ts_thread");
+	pr_info("i.MX ADC input touchscreen loaded.\n");
+	return 0;
+}
+
+static void __exit imx_adc_ts_exit(void)
+{
+	input_ts_installed = 0;
+	input_unregister_device(imx_inputdev);
+
+	if (imx_inputdev) {
+		input_free_device(imx_inputdev);
+		imx_inputdev = NULL;
+	}
+}
+
+late_initcall(imx_adc_ts_init);
+module_exit(imx_adc_ts_exit);
+
+MODULE_DESCRIPTION("i.MX ADC input touchscreen driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/mxc_ts.c b/drivers/input/touchscreen/mxc_ts.c
new file mode 100644
index 000000000000..b354d81c5465
--- /dev/null
+++ b/drivers/input/touchscreen/mxc_ts.c
@@ -0,0 +1,118 @@
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_ts.c
+ *
+ * @brief Driver for the Freescale Semiconductor MXC touchscreen.
+ *
+ * The touchscreen driver is designed as a standard input driver which is a
+ * wrapper over low level PMIC driver. Most of the hardware configuration and
+ * touchscreen functionality is implemented in the low level PMIC driver. During
+ * initialization, this driver creates a kernel thread. This thread then calls
+ * PMIC driver to obtain touchscreen values continously. These values are then
+ * passed to the input susbsystem.
+ *
+ * @ingroup touchscreen
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/input.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/freezer.h>
+#include <linux/pmic_external.h>
+#include <linux/pmic_adc.h>
+
+#define MXC_TS_NAME	"mxc_ts"
+
+static struct input_dev *mxc_inputdev = NULL;
+static u32 input_ts_installed;
+
+static int ts_thread(void *arg)
+{
+	t_touch_screen ts_sample;
+	s32 wait = 0;
+
+	daemonize("mxc_ts");
+	while (input_ts_installed) {
+		try_to_freeze();
+		memset(&ts_sample, 0, sizeof(t_touch_screen));
+		if (0 != pmic_adc_get_touch_sample(&ts_sample, !wait))
+			continue;
+		if (!(ts_sample.contact_resistance || wait))
+			continue;
+
+		input_report_abs(mxc_inputdev, ABS_X, ts_sample.x_position);
+		input_report_abs(mxc_inputdev, ABS_Y, ts_sample.y_position);
+		input_report_abs(mxc_inputdev, ABS_PRESSURE,
+				 ts_sample.contact_resistance);
+		input_sync(mxc_inputdev);
+
+		wait = ts_sample.contact_resistance;
+		msleep(20);
+	}
+
+	return 0;
+}
+
+static int __init mxc_ts_init(void)
+{
+	int retval;
+
+	if (!is_pmic_adc_ready())
+		return -ENODEV;
+
+	mxc_inputdev = input_allocate_device();
+	if (!mxc_inputdev) {
+		printk(KERN_ERR
+		       "mxc_ts_init: not enough memory for input device\n");
+		return -ENOMEM;
+	}
+
+	mxc_inputdev->name = MXC_TS_NAME;
+	mxc_inputdev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+	mxc_inputdev->keybit[BIT_WORD(BTN_TOUCH)] |= BIT_MASK(BTN_TOUCH);
+	mxc_inputdev->absbit[0] =
+	    BIT_MASK(ABS_X) | BIT_MASK(ABS_Y) | BIT_MASK(ABS_PRESSURE);
+	retval = input_register_device(mxc_inputdev);
+	if (retval < 0) {
+		input_free_device(mxc_inputdev);
+		return retval;
+	}
+
+	input_ts_installed = 1;
+	kernel_thread(ts_thread, NULL, CLONE_VM | CLONE_FS);
+	printk("mxc input touchscreen loaded\n");
+	return 0;
+}
+
+static void __exit mxc_ts_exit(void)
+{
+	input_ts_installed = 0;
+	input_unregister_device(mxc_inputdev);
+
+	if (mxc_inputdev) {
+		input_free_device(mxc_inputdev);
+		mxc_inputdev = NULL;
+	}
+}
+
+late_initcall(mxc_ts_init);
+module_exit(mxc_ts_exit);
+
+MODULE_DESCRIPTION("MXC input touchscreen driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/s3c24xx_ts.c b/drivers/input/touchscreen/s3c24xx_ts.c
new file mode 100644
index 000000000000..8d4cb5fa34ef
--- /dev/null
+++ b/drivers/input/touchscreen/s3c24xx_ts.c
@@ -0,0 +1,627 @@
+/* linux/drivers/input/touchscreen/s3c-ts.c
+ *
+ * $Id: s3c-ts.c,v 1.7 2007/02/01 05:08:41 yongkal Exp $
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Copyright (c) 2004 Arnaud Patard <arnaud.patard@rtp-net.org>
+ * iPAQ H1940 touchscreen support
+ *
+ * ChangeLog
+ *
+ * 2004-09-05: Herbert Potzl <herbert@13thfloor.at>
+ *	- added clock (de-)allocation code
+ *
+ * 2005-03-06: Arnaud Patard <arnaud.patard@rtp-net.org>
+ *      - h1940_ -> s3c24xx (this driver is now also used on the n30
+ *        machines :P)
+ *      - Debug messages are now enabled with the config option
+ *        TOUCHSCREEN_S3C_DEBUG
+ *      - Changed the way the value are read
+ *      - Input subsystem should now work
+ *      - Use ioremap and readl/writel
+ *
+ * 2005-03-23: Arnaud Patard <arnaud.patard@rtp-net.org>
+ *      - Make use of some undocumented features of the touchscreen
+ *        controller
+ *
+ * 2006-09-05: Ryu Euiyoul <ryu.real@gmail.com>
+ *      - added power management suspend and resume code
+ *
+ * 2008-10-20: Luis Galdos <luis.galdos@digi.com>
+ *      - Modifications for using a platform device
+ *      - Close and open function implemented
+ *
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/init.h>
+#include <linux/serio.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/signal.h>
+
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <mach/hardware.h>
+
+#include <mach/irqs.h>
+
+#include <mach/map.h>
+#include <plat/ts.h>
+#include <plat/regs-adc.h>
+
+
+#define printk_err(fmt, args...)                printk(KERN_ERR "[ ERROR ] s3c24xx-ts: " fmt, ## args)
+#define printk_info(fmt, args...)               printk(KERN_INFO "s3c24xx-ts: " fmt, ## args)
+
+#if 0
+#define S3C24XX_TS_DEBUG
+#endif
+
+#ifdef S3C24XX_TS_DEBUG
+#  define printk_debug(fmt, args...)		printk(KERN_DEBUG "s3c24xx-ts: %s() " fmt, __FUNCTION__ , ## args)
+#else
+#  define printk_debug(fmt, args...)
+#endif
+
+
+
+/* @TODO: Remove the below macro */
+#define S3C24XX_SAMPLE_NUM_MAX			(20)
+
+
+/* For ts->dev.id.version */
+#define S3C_TSVERSION				0x0101
+
+#define WAIT4INT(x)				(((x)<<8) | \
+						S3C2410_ADCTSC_YM_SEN | \
+						S3C2410_ADCTSC_YP_SEN | \
+						S3C2410_ADCTSC_XP_SEN | \
+						S3C2410_ADCTSC_XY_PST(3))
+
+#define AUTOPST					(S3C2410_ADCTSC_YM_SEN | \
+						S3C2410_ADCTSC_YP_SEN | \
+						S3C2410_ADCTSC_XP_SEN | \
+						S3C2410_ADCTSC_AUTO_PST | \
+						S3C2410_ADCTSC_XY_PST(0))
+
+/*
+ * Definitions & global arrays.
+ */
+static char *s3c_ts_name = "s3c TouchScreen";
+
+
+/*
+ * Per-touchscreen data.
+ */
+struct s3c_ts {
+	ulong xp;
+	ulong yp;
+	char phys[32];
+	struct input_dev *indev;
+	struct clk *clk;
+	void __iomem *base;
+	struct resource *mem;
+	struct timer_list timer;
+	int timer_enabled;
+	int irq_adc;
+	int irq_tc;
+	atomic_t closed;
+	unsigned int adccon;
+	unsigned int adctsc;
+	unsigned int adcdly;
+
+	ulong xps[S3C24XX_SAMPLE_NUM_MAX];
+	ulong yps[S3C24XX_SAMPLE_NUM_MAX];
+	int sample;
+	struct s3c_ts_mach_info *info;
+	int skip_this_sample;
+};
+
+
+#define MAX_DIFF_BETWEEN_SAMPLES        200
+static ulong filter_raw_values(struct s3c_ts *ts, ulong *samples)
+{
+        ulong diff12, diff23, diff31;
+        ulong vals[3];
+        int cnt;
+
+	/* First reset the internal flag */
+	ts->skip_this_sample = 0;
+
+        for (cnt = 0; cnt < 3; cnt++)
+                vals[cnt] = samples[cnt];
+
+        diff12 = (vals[0] > vals[1]) ? vals[0] - vals[1] : vals[1] - vals[0];
+        if (diff12 > MAX_DIFF_BETWEEN_SAMPLES) {
+                ts->skip_this_sample = 1;
+                return 0;
+        }
+
+        diff23 = (vals[1] > vals[2]) ? vals[1] - vals[2] : vals[2] - vals[1];
+        if (diff23 > MAX_DIFF_BETWEEN_SAMPLES) {
+                ts->skip_this_sample = 1;
+                return 0;
+        }
+
+        diff31 = (vals[2] > vals[0]) ? vals[2] - vals[0] : vals[0] - vals[2];
+        if (diff31 > MAX_DIFF_BETWEEN_SAMPLES) {
+                ts->skip_this_sample = 1;
+                return 0;
+        }
+
+        if (diff12 < diff23 && diff12 < diff31)
+                return (vals[0] + vals[1]) / 2;
+        if (diff23 < diff12 && diff23 < diff31)
+                return (vals[1] + vals[2]) / 2;
+
+        return (vals[0] + vals[2]) / 2;
+}
+
+static inline int s3c_ts_calc_sample(struct s3c_ts *ts)
+{
+	int offset;
+	struct s3c_ts_mach_info *info;
+	unsigned long px, py, tx, ty;
+	int cnt;
+
+	info = ts->info;
+	px = py = 0;
+	for (cnt = 0, offset = 0; offset < info->probes / 3; offset++) {
+		
+		tx = filter_raw_values(ts, &ts->xps[offset * 3]);		
+		if (ts->skip_this_sample)
+			continue;
+
+		ty = filter_raw_values(ts, &ts->yps[offset * 3]);
+		if (ts->skip_this_sample)
+			continue;
+		
+		cnt++;
+		px += tx;
+		py += ty;
+	}
+
+	if (cnt) {
+		ts->xp = px / cnt;
+		ts->yp = py / cnt;
+	}
+
+	return cnt;
+}
+
+/* static struct s3c_ts *ts; */
+
+static void touch_timer_fire(unsigned long data)
+{
+	unsigned long data0;
+	unsigned long data1;
+	int updown;
+	struct s3c_ts *ts;
+	unsigned int adccon;
+
+	ts = (struct s3c_ts *)data;
+
+	data0 = readl(ts->base + S3C2410_ADCDAT0);
+	data1 = readl(ts->base + S3C2410_ADCDAT1);
+
+	updown = (!(data0 & S3C2410_ADCDAT0_UPDOWN)) &&
+		(!(data1 & S3C2410_ADCDAT1_UPDOWN));
+
+	if (updown) {
+		if (ts->timer_enabled) {
+			struct s3c_ts_mach_info *info;
+			info = ts->info;
+
+			ts->xps[ts->sample] = ts->xp;
+			ts->yps[ts->sample] = ts->yp;
+			ts->sample += 1;
+			
+			if (ts->sample == info->probes) {
+				if (s3c_ts_calc_sample(ts)) {
+					printk_debug("XP %lu | YP %lu\n",
+						     ts->xp, ts->yp);
+					input_report_abs(ts->indev, ABS_X, ts->xp);
+					input_report_abs(ts->indev, ABS_Y, ts->yp);
+					
+					input_report_key(ts->indev, BTN_TOUCH, 1);
+					input_report_abs(ts->indev, ABS_PRESSURE, 1);
+					input_sync(ts->indev);
+				}
+
+				/* Reset the number of samples */
+				ts->sample = 0;
+			}
+		}
+
+		ts->xp = 0;
+		ts->yp = 0;
+		writel(S3C2410_ADCTSC_PULL_UP_DISABLE | AUTOPST,
+		       ts->base + S3C2410_ADCTSC);
+		adccon = readl(ts->base + S3C2410_ADCCON);
+		writel(adccon | S3C2410_ADCCON_ENABLE_START, ts->base + S3C2410_ADCCON);
+	} else {
+		input_report_key(ts->indev, BTN_TOUCH, 0);
+		input_report_abs(ts->indev, ABS_PRESSURE, 0);
+		input_sync(ts->indev);
+		writel(WAIT4INT(0), ts->base + S3C2410_ADCTSC);
+	}
+
+	ts->timer_enabled = 0;
+}
+
+
+static irqreturn_t stylus_updown(int irq, void *dev_id)
+{
+	unsigned long data0;
+	unsigned long data1;
+	int updown;
+	struct s3c_ts *ts;
+
+	ts = (struct s3c_ts *)dev_id;
+
+	disable_irq(ts->irq_adc);
+	data0 = readl(ts->base + S3C2410_ADCDAT0);
+	data1 = readl(ts->base + S3C2410_ADCDAT1);
+
+	updown = (!(data0 & S3C2410_ADCDAT0_UPDOWN)) &&
+		(!(data1 & S3C2410_ADCDAT1_UPDOWN));
+
+	printk_debug("   %c\n",	updown ? 'D' : 'U');
+
+	/* TODO we should never get an interrupt with updown set while
+	 * the timer is running, but maybe we ought to verify that the
+	 * timer isn't running anyways. */
+
+       if (updown) {
+	       ts->sample = 0;
+	       touch_timer_fire((unsigned long)ts);
+       }
+
+#ifdef CONFIG_ARCH_S3C6400
+        __raw_writel(0x0, ts->base + S3C_ADCCLRWK);
+        __raw_writel(0x0, ts->base + S3C_ADCCLRINT);
+#endif
+
+	if (!atomic_read(&ts->closed))
+		enable_irq(ts->irq_adc);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t stylus_action(int irq, void *dev_id)
+{
+	unsigned long data0;
+	unsigned long data1;
+	struct s3c_ts *ts;
+	unsigned int adccon;
+	struct s3c_ts_mach_info *info;
+	
+	ts = (struct s3c_ts *)dev_id;
+	info = ts->info;
+	
+	disable_irq(ts->irq_tc);
+
+	data0 = readl(ts->base + S3C2410_ADCDAT0);
+	data1 = readl(ts->base + S3C2410_ADCDAT1);
+	adccon = readl(ts->base + S3C2410_ADCCON);
+
+	ts->xp = (data0 & S3C2410_ADCDAT0_XPDATA_MASK);
+	ts->yp = (data1 & S3C2410_ADCDAT1_YPDATA_MASK);
+	
+	ts->timer_enabled = 1;
+
+	/* Use the correct values for updating the timer */
+	mod_timer(&ts->timer, jiffies + msecs_to_jiffies(info->trigger_ms));
+	writel(WAIT4INT(1), ts->base + S3C2410_ADCTSC);
+
+#ifdef CONFIG_ARCH_S3C6400
+        __raw_writel(0x0, ts->base + S3C_ADCCLRWK);
+        __raw_writel(0x0, ts->base + S3C_ADCCLRINT);
+#endif
+
+	if (!atomic_read(&ts->closed))
+		enable_irq(ts->irq_tc);
+	
+	return IRQ_HANDLED;
+}
+
+
+static void s3c_ts_close(struct input_dev *dev)
+{
+	struct s3c_ts *ts;
+
+	printk_debug("Close function called\n");
+	
+	ts = input_get_drvdata(dev);
+	atomic_set(&ts->closed, 1);
+	disable_irq(ts->irq_adc);
+	disable_irq(ts->irq_tc);
+}
+
+
+
+/* Only disable the interrupts */
+static int s3c_ts_open(struct input_dev *dev)
+{
+	struct s3c_ts *ts;
+
+	printk_debug("Open function called\n");
+	
+	ts = input_get_drvdata(dev);
+	atomic_set(&ts->closed, 0);
+	enable_irq(ts->irq_adc);
+	enable_irq(ts->irq_tc);
+	return 0;
+}
+
+
+/*
+ * The functions for inserting/removing us as a module.
+ */
+static int __devinit s3c_ts_probe(struct platform_device *pdev)
+{
+	struct s3c_ts_mach_info *info;
+	struct input_dev *indev;
+	int err;
+	struct s3c_ts *ts;
+
+	info = pdev->dev.platform_data;
+	if (!info) {
+		printk_err("Too bad: no platform data attached. Aborting.\n");
+		return -EINVAL;
+	}
+
+	/* Sanity checks */
+	if (!info->trigger_ms || info->probes > S3C24XX_SAMPLE_NUM_MAX) {
+		printk_err("Found an invalid configuration.\n");
+		return -EINVAL;
+	}
+	
+	
+	ts = kzalloc(sizeof(struct s3c_ts), GFP_KERNEL);
+	ts->info = info;
+	indev = input_allocate_device();
+	if (!ts || !indev) {
+		if (ts)
+			kfree(ts);
+		if (indev)
+			kfree(indev);				
+		return -ENOMEM;
+	} else
+		ts->indev = indev;
+
+	/* Get the required source clock */
+	ts->clk = clk_get(&pdev->dev, "adc");
+	if (!ts->clk) {
+		printk_err("Failed to get adc clock source\n");
+		err = -ENOENT;
+		goto err_free_ts;
+	}
+	clk_enable(ts->clk);
+
+	/* @XXX: Get the memory area from the configuration structure */
+	ts->base = ioremap(S3C2410_PA_ADC, 0x20);
+	if (!ts->base) {
+		printk_err("Failed to ioremap register block\n");
+		err = -EINVAL;
+		goto err_put_clock;
+	}
+
+	/* Prepare the passed user configuration values */
+	if ((info->presc & 0xff) > 0)
+		writel(S3C2410_ADCCON_PRSCEN |
+		       S3C2410_ADCCON_PRSCVL(info->presc & 0xFF),
+		       ts->base + S3C2410_ADCCON);
+	else
+		writel(0, ts->base + S3C2410_ADCCON);
+
+
+	/* Initialise the start delay register (Manual: Do not use zero value) */
+	if ((info->delay & 0xffff) > 0)
+		writel(info->delay & 0xffff,  ts->base + S3C2410_ADCDLY);
+	else
+		writel(10000,  ts->base + S3C2410_ADCDLY);
+
+	/* Configure the touch screen controller for "Waiting for Interrupt Mode" */
+	writel(WAIT4INT(0), ts->base + S3C2410_ADCTSC);
+
+	ts->indev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+	ts->indev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+
+	/*
+	 * The data values for the position conversion are coming from the data
+	 * sheet of the touch controller (S3C2443: 24-10)
+	 */
+	input_set_abs_params(ts->indev, ABS_X, info->xmin, info->xmax, 0, 0);
+	input_set_abs_params(ts->indev, ABS_Y, info->ymin, info->ymax, 0, 0);
+	input_set_abs_params(ts->indev, ABS_PRESSURE, 0, 10, 0, 0);
+
+	sprintf(ts->phys, "ts0");
+
+	indev->open = s3c_ts_open;
+	indev->close = s3c_ts_close;
+	indev->name = s3c_ts_name;
+	indev->phys = ts->phys;
+	indev->id.bustype = BUS_RS232;
+	indev->id.vendor = 0xDEAD;
+	indev->id.product = 0xBEEF;
+	indev->id.version = S3C_TSVERSION;
+
+	/* Get the irqs from the platform device data */
+	ts->irq_adc = platform_get_irq(pdev, 0);
+	err = request_irq(ts->irq_adc, stylus_action, 0, "s3c_action", ts);
+	if (err) {
+		printk_err("Requesting the ADC IRQ %i\n", ts->irq_adc);
+		goto err_put_clock;
+	}
+ 	printk_debug("Got ADC IRQ %i\n", ts->irq_adc);
+	
+	ts->irq_tc = platform_get_irq(pdev, 1);
+	err = request_irq(ts->irq_tc, stylus_updown, 0, "s3c_updown", ts);
+	if (err) {
+		printk_err("Requesting the touch screen IRQ %i\n", ts->irq_tc);
+		goto err_free_adcirq;
+	}
+ 	printk_debug("Got TC IRQ %i\n", ts->irq_tc);
+
+	
+	/* Disable the interrupts first */
+	disable_irq(ts->irq_adc);
+	disable_irq(ts->irq_tc);
+	
+	printk_info("%s successfully loaded\n", s3c_ts_name);
+
+	/* All went ok, so register to the input system */
+	err = input_register_device(ts->indev);
+	if (err)
+		goto err_free_tcirq;
+
+
+        /* Configure and init the timer for the command timeouts */
+        init_timer(&ts->timer);
+        ts->timer.function = touch_timer_fire;
+        ts->timer.data = (unsigned long)ts;
+
+	input_set_drvdata(indev, ts);
+	platform_set_drvdata(pdev, ts);
+	
+	return 0;
+	
+ err_free_tcirq:
+	free_irq(ts->irq_tc, ts);
+
+ err_free_adcirq:
+	free_irq(ts->irq_adc, ts);
+	
+ err_put_clock:
+	clk_disable(ts->clk);
+	clk_put(ts->clk);
+	
+ err_free_ts:
+	input_free_device(ts->indev);
+	kfree(ts);
+	
+	return err;
+}
+
+static int __devexit s3c_ts_remove(struct platform_device *pdev)
+{
+	struct s3c_ts *ts;
+
+	ts = platform_get_drvdata(pdev);
+	
+	printk_debug("Removing the device with the ID %i\n", pdev->id);
+
+	if (!atomic_read(&ts->closed)) {
+		disable_irq(ts->irq_adc);
+		disable_irq(ts->irq_tc);
+	}
+	
+	free_irq(ts->irq_adc, ts);
+	free_irq(ts->irq_tc, ts);
+
+	if (ts->clk) {
+		clk_disable(ts->clk);
+		clk_put(ts->clk);
+		ts->clk = NULL;
+	}
+
+	input_unregister_device(ts->indev);
+	iounmap(ts->base);
+
+	input_free_device(ts->indev);
+	kfree(ts);
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+
+#ifdef CONFIG_PM
+static int s3c_ts_suspend(struct platform_device *dev, pm_message_t state)
+{
+	struct s3c_ts *ts;
+
+	ts = platform_get_drvdata(dev);
+
+	ts->adccon = readl(ts->base + S3C2410_ADCCON);
+	ts->adctsc = readl(ts->base + S3C2410_ADCTSC);
+	ts->adcdly = readl(ts->base + S3C2410_ADCDLY);
+
+	/* Don't forget to re-enable the interrupts! */
+	disable_irq(ts->irq_adc);
+	disable_irq(ts->irq_tc);
+	clk_disable(ts->clk);
+	return 0;
+}
+
+static int s3c_ts_resume(struct platform_device *pdev)
+{
+	struct s3c_ts *ts;
+
+	ts = platform_get_drvdata(pdev);
+	
+	clk_enable(ts->clk);
+	writel(ts->adccon, ts->base + S3C2410_ADCCON);
+	writel(ts->adctsc, ts->base + S3C2410_ADCTSC);
+	writel(ts->adcdly, ts->base + S3C2410_ADCDLY);
+	writel(WAIT4INT(0), ts->base + S3C2410_ADCTSC);
+
+	enable_irq(ts->irq_adc);
+	enable_irq(ts->irq_tc);
+
+	return 0;
+}
+#else
+#define s3c_ts_suspend NULL
+#define s3c_ts_resume  NULL
+#endif
+
+static struct platform_driver s3c_ts_driver = {
+       .probe		= s3c_ts_probe,
+       .remove		= __devexit_p(s3c_ts_remove),
+       .suspend		= s3c_ts_suspend,
+       .resume		= s3c_ts_resume,
+       .driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "s3c24xx-ts",
+	},
+};
+
+static char banner[] __initdata = KERN_INFO "S3C Touchscreen driver, (c) 2006 Samsung Electronics\n";
+
+static int __init s3c_ts_init(void)
+{
+	printk(banner);
+	return platform_driver_register(&s3c_ts_driver);
+}
+
+static void __exit s3c_ts_exit(void)
+{
+	platform_driver_unregister(&s3c_ts_driver);
+}
+
+module_init(s3c_ts_init);
+module_exit(s3c_ts_exit);
+
+MODULE_AUTHOR("Samsung AP");
+MODULE_DESCRIPTION("s3c touchscreen driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/stmp3xxx_ts.c b/drivers/input/touchscreen/stmp3xxx_ts.c
new file mode 100644
index 000000000000..05f787b9d9d4
--- /dev/null
+++ b/drivers/input/touchscreen/stmp3xxx_ts.c
@@ -0,0 +1,386 @@
+/*
+ * Freesclae STMP37XX/STMP378X Touchscreen driver
+ *
+ * Author: Vitaly Wool <vital@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/* #define DEBUG*/
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+
+#include <mach/lradc.h>
+#include <mach/hardware.h>
+#include <mach/regs-lradc.h>
+
+#define TOUCH_DEBOUNCE_TOLERANCE	100
+
+struct stmp3xxx_ts_info {
+	int touch_irq;
+	int device_irq;
+	struct input_dev *idev;
+	enum {
+		TS_STATE_DISABLED,
+		TS_STATE_TOUCH_DETECT,
+		TS_STATE_TOUCH_VERIFY,
+		TS_STATE_X_PLANE,
+		TS_STATE_Y_PLANE,
+	} state;
+	u16 x;
+	u16 y;
+	int sample_count;
+};
+
+static inline void enter_state_touch_detect(struct stmp3xxx_ts_info *info)
+{
+	HW_LRADC_CHn_CLR(2, 0xFFFFFFFF);
+	HW_LRADC_CHn_CLR(3, 0xFFFFFFFF);
+	HW_LRADC_CHn_CLR(4, 0xFFFFFFFF);
+	HW_LRADC_CHn_CLR(5, 0xFFFFFFFF);
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC5_IRQ);
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_TOUCH_DETECT_IRQ);
+	/*
+	 * turn off the yplus and yminus pullup and pulldown, and turn off touch
+	 * detect (enables yminus, and xplus through a resistor.On a press,
+	 * xplus is pulled down)
+	 */
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_YMINUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_YPLUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_XMINUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_XPLUS_ENABLE);
+	HW_LRADC_CTRL0_SET(BM_LRADC_CTRL0_TOUCH_DETECT_ENABLE);
+
+	hw_lradc_set_delay_trigger_kick(LRADC_DELAY_TRIGGER_TOUCHSCREEN, 0);
+	info->state = TS_STATE_TOUCH_DETECT;
+	info->sample_count = 0;
+}
+
+static inline void enter_state_disabled(struct stmp3xxx_ts_info *info)
+{
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_YMINUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_YPLUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_XMINUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_XPLUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_TOUCH_DETECT_ENABLE);
+
+	hw_lradc_set_delay_trigger_kick(LRADC_DELAY_TRIGGER_TOUCHSCREEN, 0);
+	info->state = TS_STATE_DISABLED;
+	info->sample_count = 0;
+}
+
+
+static inline void enter_state_x_plane(struct stmp3xxx_ts_info *info)
+{
+	HW_LRADC_CTRL0_SET(BM_LRADC_CTRL0_YMINUS_ENABLE);
+	HW_LRADC_CTRL0_SET(BM_LRADC_CTRL0_YPLUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_XMINUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_XPLUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_TOUCH_DETECT_ENABLE);
+
+	hw_lradc_set_delay_trigger_kick(LRADC_DELAY_TRIGGER_TOUCHSCREEN, 1);
+
+	info->state = TS_STATE_X_PLANE;
+	info->sample_count = 0;
+}
+
+static inline void enter_state_y_plane(struct stmp3xxx_ts_info *info)
+{
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_YMINUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_YPLUS_ENABLE);
+	HW_LRADC_CTRL0_SET(BM_LRADC_CTRL0_XMINUS_ENABLE);
+	HW_LRADC_CTRL0_SET(BM_LRADC_CTRL0_XPLUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_TOUCH_DETECT_ENABLE);
+
+	hw_lradc_set_delay_trigger_kick(LRADC_DELAY_TRIGGER_TOUCHSCREEN, 1);
+	info->state = TS_STATE_Y_PLANE;
+	info->sample_count = 0;
+}
+
+static inline void enter_state_touch_verify(struct stmp3xxx_ts_info *info)
+{
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_YMINUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_YPLUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_XMINUS_ENABLE);
+	HW_LRADC_CTRL0_CLR(BM_LRADC_CTRL0_XPLUS_ENABLE);
+	HW_LRADC_CTRL0_SET(BM_LRADC_CTRL0_TOUCH_DETECT_ENABLE);
+
+	info->state = TS_STATE_TOUCH_VERIFY;
+	hw_lradc_set_delay_trigger_kick(LRADC_DELAY_TRIGGER_TOUCHSCREEN, 1);
+	info->sample_count = 0;
+}
+
+static void process_lradc(struct stmp3xxx_ts_info *info, u16 x, u16 y,
+			int pressure)
+{
+	switch (info->state) {
+	case TS_STATE_X_PLANE:
+		pr_debug("%s: x plane state, sample_count %d\n", __func__,
+				info->sample_count);
+		if (info->sample_count < 2) {
+			info->x = x;
+			info->sample_count++;
+		} else {
+			if (abs(info->x - x) > TOUCH_DEBOUNCE_TOLERANCE)
+				info->sample_count = 1;
+			else {
+				u16 x_c = info->x * (info->sample_count - 1);
+				info->x = (x_c + x) / info->sample_count;
+				info->sample_count++;
+			}
+		}
+		if (info->sample_count > 4)
+			enter_state_y_plane(info);
+		else
+			hw_lradc_set_delay_trigger_kick(
+					LRADC_DELAY_TRIGGER_TOUCHSCREEN, 1);
+		break;
+
+	case TS_STATE_Y_PLANE:
+		pr_debug("%s: y plane state, sample_count %d\n", __func__,
+				info->sample_count);
+		if (info->sample_count < 2) {
+			info->y = y;
+			info->sample_count++;
+		} else {
+			if (abs(info->y - y) > TOUCH_DEBOUNCE_TOLERANCE)
+				info->sample_count = 1;
+			else {
+				u16 y_c = info->y * (info->sample_count - 1);
+				info->y = (y_c + y) / info->sample_count;
+				info->sample_count++;
+			}
+		}
+		if (info->sample_count > 4)
+			enter_state_touch_verify(info);
+		else
+			hw_lradc_set_delay_trigger_kick(
+					LRADC_DELAY_TRIGGER_TOUCHSCREEN, 1);
+		break;
+
+	case TS_STATE_TOUCH_VERIFY:
+		pr_debug("%s: touch verify state, sample_count %d\n", __func__,
+				info->sample_count);
+		pr_debug("%s: x %d, y %d\n", __func__, info->x, info->y);
+		input_report_abs(info->idev, ABS_X, info->x);
+		input_report_abs(info->idev, ABS_Y, info->y);
+		input_report_abs(info->idev, ABS_PRESSURE, pressure);
+		input_sync(info->idev);
+		/* fall through */
+	case TS_STATE_TOUCH_DETECT:
+		pr_debug("%s: touch detect state, sample_count %d\n", __func__,
+				info->sample_count);
+		if (pressure) {
+			input_report_abs(info->idev, ABS_PRESSURE, pressure);
+			enter_state_x_plane(info);
+			hw_lradc_set_delay_trigger_kick(
+					LRADC_DELAY_TRIGGER_TOUCHSCREEN, 1);
+		} else
+			enter_state_touch_detect(info);
+		break;
+
+	default:
+		printk(KERN_ERR "%s: unknown touchscreen state %d\n", __func__,
+				info->state);
+	}
+}
+
+static irqreturn_t ts_handler(int irq, void *dev_id)
+{
+	struct stmp3xxx_ts_info *info = dev_id;
+	u16 x_plus, y_plus;
+	int pressure = 0;
+
+	if (irq == info->touch_irq)
+		HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_TOUCH_DETECT_IRQ);
+	else if (irq == info->device_irq)
+		HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC5_IRQ);
+
+	/* get x, y values */
+	x_plus = HW_LRADC_CHn_RD(LRADC_TOUCH_X_PLUS) & BM_LRADC_CHn_VALUE;
+	y_plus = HW_LRADC_CHn_RD(LRADC_TOUCH_Y_PLUS) & BM_LRADC_CHn_VALUE;
+
+	/* pressed? */
+	if (HW_LRADC_STATUS_RD() & BM_LRADC_STATUS_TOUCH_DETECT_RAW)
+		pressure = 1;
+
+	pr_debug("%s: irq %d, x_plus %d, y_plus %d, pressure %d\n",
+			__func__, irq, x_plus, y_plus, pressure);
+
+	process_lradc(info, x_plus, y_plus, pressure);
+
+	return IRQ_HANDLED;
+}
+
+static int stmp3xxx_ts_probe(struct platform_device *pdev)
+{
+	struct input_dev *idev;
+	struct stmp3xxx_ts_info *info;
+	int ret = 0;
+	struct resource *res;
+
+	idev = input_allocate_device();
+	info = kzalloc(sizeof(struct stmp3xxx_ts_info), GFP_KERNEL);
+	if (idev == NULL || info == NULL) {
+		ret = -ENOMEM;
+		goto out_nomem;
+	}
+
+	idev->name = "STMP3XXX touchscreen";
+	idev->evbit[0] = BIT(EV_ABS);
+	input_set_abs_params(idev, ABS_X, 0, 0xFFF, 0, 0);
+	input_set_abs_params(idev, ABS_Y, 0, 0xFFF, 0, 0);
+	input_set_abs_params(idev, ABS_PRESSURE, 0, 1, 0, 0);
+
+	ret = input_register_device(idev);
+	if (ret)
+		goto out_nomem;
+
+	info->idev = idev;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		printk(KERN_ERR "%s: couldn't get IRQ resource\n", __func__);
+		ret = -ENODEV;
+		goto out_nodev;
+	}
+	info->touch_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+	if (!res) {
+		printk(KERN_ERR "%s: couldn't get IRQ resource\n", __func__);
+		ret = -ENODEV;
+		goto out_nodev;
+	}
+	info->device_irq = res->start;
+
+	ret = request_irq(info->touch_irq, ts_handler, IRQF_DISABLED,
+				"stmp3xxx_ts_touch", info);
+	if (ret)
+		goto out_nodev;
+
+	ret = request_irq(info->device_irq, ts_handler, IRQF_DISABLED,
+				"stmp3xxx_ts_dev", info);
+	if (ret) {
+		free_irq(info->touch_irq, info);
+		goto out_nodev;
+	}
+	enter_state_touch_detect(info);
+
+	hw_lradc_use_channel(LRADC_CH2);
+	hw_lradc_use_channel(LRADC_CH3);
+	hw_lradc_use_channel(LRADC_CH5);
+	hw_lradc_configure_channel(LRADC_CH2, 0, 0, 0);
+	hw_lradc_configure_channel(LRADC_CH3, 0, 0, 0);
+	hw_lradc_configure_channel(LRADC_CH5, 0, 0, 0);
+
+	/* Clear the accumulator & NUM_SAMPLES for the channels */
+	HW_LRADC_CHn_CLR(LRADC_CH2, 0xFFFFFFFF);
+	HW_LRADC_CHn_CLR(LRADC_CH3, 0xFFFFFFFF);
+	HW_LRADC_CHn_CLR(LRADC_CH5, 0xFFFFFFFF);
+
+	hw_lradc_set_delay_trigger(LRADC_DELAY_TRIGGER_TOUCHSCREEN,
+			0x3c, 0, 0, 8);
+
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC5_IRQ);
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_TOUCH_DETECT_IRQ);
+
+	HW_LRADC_CTRL1_SET(BM_LRADC_CTRL1_LRADC5_IRQ_EN);
+	HW_LRADC_CTRL1_SET(BM_LRADC_CTRL1_TOUCH_DETECT_IRQ_EN);
+
+	platform_set_drvdata(pdev, info);
+	device_init_wakeup(&pdev->dev, 1);
+	goto out;
+
+out_nodev:
+	input_free_device(idev);
+out_nomem:
+	kfree(idev);
+	kfree(info);
+out:
+	return ret;
+}
+
+static int stmp3xxx_ts_remove(struct platform_device *pdev)
+{
+	struct stmp3xxx_ts_info *info = platform_get_drvdata(pdev);
+
+	platform_set_drvdata(pdev, NULL);
+
+	hw_lradc_unuse_channel(LRADC_CH2);
+	hw_lradc_unuse_channel(LRADC_CH3);
+	hw_lradc_unuse_channel(LRADC_CH5);
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC5_IRQ_EN);
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_TOUCH_DETECT_IRQ_EN);
+
+	free_irq(info->device_irq, info);
+	free_irq(info->touch_irq, info);
+	input_free_device(info->idev);
+
+	enter_state_disabled(info);
+	kfree(info->idev);
+	kfree(info);
+	return 0;
+}
+
+static int stmp3xxx_ts_suspend(struct platform_device *pdev,
+				pm_message_t state)
+{
+#ifdef CONFIG_PM
+	if (!device_may_wakeup(&pdev->dev)) {
+		hw_lradc_unuse_channel(LRADC_CH2);
+		hw_lradc_unuse_channel(LRADC_CH3);
+		hw_lradc_unuse_channel(LRADC_CH5);
+	}
+#endif
+	return 0;
+}
+
+static int stmp3xxx_ts_resume(struct platform_device *pdev)
+{
+#ifdef CONFIG_PM
+	if (!device_may_wakeup(&pdev->dev)) {
+		hw_lradc_use_channel(LRADC_CH2);
+		hw_lradc_use_channel(LRADC_CH3);
+		hw_lradc_use_channel(LRADC_CH5);
+	}
+#endif
+	return 0;
+}
+
+static struct platform_driver stmp3xxx_ts_driver = {
+	.probe		= stmp3xxx_ts_probe,
+	.remove		= stmp3xxx_ts_remove,
+	.suspend	= stmp3xxx_ts_suspend,
+	.resume		= stmp3xxx_ts_resume,
+	.driver		= {
+		.name		= "stmp3xxx_ts",
+	},
+};
+
+static int __init stmp3xxx_ts_init(void)
+{
+	return platform_driver_register(&stmp3xxx_ts_driver);
+}
+
+static void __exit stmp3xxx_ts_exit(void)
+{
+	platform_driver_unregister(&stmp3xxx_ts_driver);
+}
+
+module_init(stmp3xxx_ts_init);
+module_exit(stmp3xxx_ts_exit);
diff --git a/drivers/input/touchscreen/tsc2007.c b/drivers/input/touchscreen/tsc2007.c
new file mode 100644
index 000000000000..6e18ffbebd47
--- /dev/null
+++ b/drivers/input/touchscreen/tsc2007.c
@@ -0,0 +1,443 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file tsc2007.c
+ *
+ * @brief Driver for TI's tsc2007 I2C Touch Screen Controller.
+ *
+ * This driver is based on the driver written by Bill Gatliff
+ *  Copyright (C) 2005 Bill Gatliff <bgat at billgatliff.com>
+ *  Changes for 2.6.20 kernel by Nicholas Chen <nchen at cs.umd.edu>
+ *
+ * @ingroup touchscreen
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/string.h>
+#include <linux/bcd.h>
+#include <linux/list.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/kthread.h>
+#include <linux/input.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+#include <asm/mach/irq.h>
+
+#define DRIVER_NAME "tsc2007"
+
+enum tsc2007_pd {
+	PD_POWERDOWN = 0,	/* penirq */
+	PD_IREFOFF_ADCON = 1,	/* no penirq */
+	PD_IREFON_ADCOFF = 2,	/* penirq */
+	PD_IREFON_ADCON = 3,	/* no penirq */
+	PD_PENIRQ_ARM = PD_IREFON_ADCOFF,
+	PD_PENIRQ_DISARM = PD_IREFON_ADCON,
+};
+
+enum tsc2007_m {
+	M_12BIT = 0,
+	M_8BIT = 1
+};
+
+enum tsc2007_cmd {
+	MEAS_TEMP0 = 0,
+	MEAS_IN1 = 2,
+	MEAS_XPOS = 12,
+	MEAS_YPOS = 13,
+	MEAS_Z1POS = 14,
+	MEAS_Z2POS = 15
+};
+
+#define tsc2007_CMD(cn, pdn, m) (((cn) << 4) | ((pdn) << 2) | ((m) << 1))
+
+#define ADC_MAX ((1 << 12) - 1)
+
+struct tsc2007_data {
+	struct i2c_client *client;
+	struct input_dev *idev;
+	struct timer_list penirq_timer;
+	struct task_struct *tstask;
+	u32 ts_thread_cnt;
+	struct completion penirq_completion;
+	struct completion penup_completion;
+	enum tsc2007_m m;
+	int penirq;
+	int penup_threshold;
+	struct regulator *vdd_reg;
+	int opened;
+};
+
+static int tsc2007_read(struct tsc2007_data *data,
+			enum tsc2007_cmd cmd, enum tsc2007_pd pd, int *val)
+{
+	unsigned char c;
+	unsigned char d[2];
+	int ret;
+
+	c = tsc2007_CMD(cmd, pd, data->m);
+
+	ret = i2c_master_send(data->client, &c, 1);
+
+	if (ret < 0)
+		goto err;
+
+	udelay(20);
+	ret = i2c_master_recv(data->client, d, data->m == M_12BIT ? 2 : 1);
+	if (ret < 0)
+		goto err;
+
+	if (val) {
+		*val = d[0];
+		*val <<= 4;
+		if (data->m == M_12BIT)
+			*val += (d[1] >> 4);
+	}
+
+	return 0;
+      err:
+	return -ENODEV;
+}
+
+static inline int tsc2007_read_xpos(struct tsc2007_data *d, enum
+				    tsc2007_pd pd, int *x)
+{
+	return tsc2007_read(d, MEAS_XPOS, pd, x);
+}
+
+static inline int tsc2007_read_ypos(struct tsc2007_data *d, enum
+				    tsc2007_pd pd, int *y)
+{
+	return tsc2007_read(d, MEAS_YPOS, pd, y);
+}
+
+static inline int tsc2007_read_pressure(struct tsc2007_data *d, enum
+					tsc2007_pd pd, int *p)
+{
+	return tsc2007_read(d, MEAS_Z1POS, pd, p);
+}
+
+static inline int tsc2007_powerdown(struct tsc2007_data *d)
+{
+	/* we don't have a distinct powerdown command,
+	   so do a benign read with the PD bits cleared */
+	return tsc2007_read(d, MEAS_IN1, PD_POWERDOWN, 0);
+}
+
+#define PENUP_TIMEOUT		10
+
+static irqreturn_t tsc2007_penirq(int irq, void *v)
+{
+	struct tsc2007_data *d = v;
+
+	disable_irq(d->penirq);
+	complete(&d->penirq_completion);
+	return IRQ_HANDLED;
+}
+
+static void tsc2007_pen_up(unsigned long v)
+{
+	struct tsc2007_data *d = (struct tsc2007_data *)v;
+
+	complete(&d->penup_completion);
+	return;
+}
+
+static inline void tsc2007_restart_pen_up_timer(struct tsc2007_data *d)
+{
+	mod_timer(&d->penirq_timer, jiffies + (PENUP_TIMEOUT * HZ) / 1000);
+}
+
+static int tsc2007ts_thread(void *v)
+{
+	struct tsc2007_data *d = v;
+
+	if (d->ts_thread_cnt)
+		return -EINVAL;
+	d->ts_thread_cnt = 1;
+
+	while (1) {
+		unsigned int x = 0, y = 0, p = 0;
+
+		if (kthread_should_stop())
+			break;
+		/* Wait for an Pen down interrupt */
+		if (wait_for_completion_interruptible_timeout
+		    (&d->penirq_completion, HZ) <= 0)
+			continue;
+
+		tsc2007_read_xpos(d, PD_PENIRQ_DISARM, &x);
+		tsc2007_read_ypos(d, PD_PENIRQ_DISARM, &y);
+		tsc2007_read_pressure(d, PD_PENIRQ_DISARM, &p);
+		input_report_abs(d->idev, ABS_X, 4096 - x);
+		input_report_abs(d->idev, ABS_Y, 4096 - y);
+		input_report_abs(d->idev, ABS_PRESSURE, p);
+		input_sync(d->idev);
+
+		while (p > d->penup_threshold) {
+			tsc2007_restart_pen_up_timer(d);
+			wait_for_completion_interruptible(&d->penup_completion);
+			/* Pen Down */
+			tsc2007_read_xpos(d, PD_PENIRQ_DISARM, &x);
+			tsc2007_read_ypos(d, PD_PENIRQ_DISARM, &y);
+			tsc2007_read_pressure(d, PD_PENIRQ_DISARM, &p);
+			if (p <= d->penup_threshold)
+				break;
+
+			input_report_abs(d->idev, ABS_X, 4096 - x);
+			input_report_abs(d->idev, ABS_Y, 4096 - y);
+			input_report_abs(d->idev, ABS_PRESSURE, p);
+			input_sync(d->idev);
+		};
+
+		/* Pen Up */
+		input_report_abs(d->idev, ABS_X, 4096 - x);
+		input_report_abs(d->idev, ABS_Y, 4096 - y);
+		input_report_abs(d->idev, ABS_PRESSURE, 0);
+		input_sync(d->idev);
+
+		tsc2007_read(d, MEAS_TEMP0, PD_PENIRQ_ARM, 0);
+		enable_irq(d->penirq);
+
+	}
+
+	d->ts_thread_cnt = 0;
+	return 0;
+}
+
+/*!
+ * This function puts the touch screen controller in low-power mode/state.
+ *
+ * @param   pdev  the device structure used to give information on touch screen
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function always returns 0.
+ */
+static int tsc2007_suspend(struct i2c_client *client, pm_message_t state)
+{
+	struct tsc2007_data *d = i2c_get_clientdata(client);
+
+	if (!IS_ERR(d->tstask) && d->opened)
+		kthread_stop(d->tstask);
+
+	return 0;
+}
+
+/*!
+ * This function brings the touch screen controller back from low-power state.
+ *
+ * @param   pdev  the device structure used to give information on touch screen
+ *                to resume
+ *
+ * @return  The function always returns 0.
+ */
+static int tsc2007_resume(struct i2c_client *client)
+{
+	struct tsc2007_data *d = i2c_get_clientdata(client);
+
+	if (d->opened)
+		d->tstask = kthread_run(tsc2007ts_thread, d, DRIVER_NAME "tsd");
+
+	return 0;
+}
+
+static int tsc2007_idev_open(struct input_dev *idev)
+{
+	struct tsc2007_data *d = input_get_drvdata(idev);
+	int ret = 0;
+
+	d->penirq_timer.data = (unsigned long)d;
+	d->penirq_timer.function = tsc2007_pen_up;
+
+	init_completion(&d->penup_completion);
+
+	d->tstask = kthread_run(tsc2007ts_thread, d, DRIVER_NAME "tsd");
+	if (IS_ERR(d->tstask))
+		ret = PTR_ERR(d->tstask);
+	else
+		d->opened++;
+
+	return ret;
+}
+
+static void tsc2007_idev_close(struct input_dev *idev)
+{
+	struct tsc2007_data *d = input_get_drvdata(idev);
+	if (!IS_ERR(d->tstask))
+		kthread_stop(d->tstask);
+
+	del_timer_sync(&d->penirq_timer);
+
+	if (d->opened > 0)
+		d->opened--;
+}
+
+static int tsc2007_driver_register(struct tsc2007_data *data)
+{
+	struct input_dev *idev;
+	int ret = 0;
+
+	init_timer(&data->penirq_timer);
+	data->penirq_timer.data = (unsigned long)data;
+	data->penirq_timer.function = tsc2007_pen_up;
+
+	init_completion(&data->penirq_completion);
+
+	if (data->penirq) {
+		ret =
+		    request_irq(data->penirq, tsc2007_penirq, IRQF_TRIGGER_LOW,
+				DRIVER_NAME, data);
+		if (!ret) {
+			printk(KERN_INFO "%s: Registering Touchscreen device\n",
+			       __func__);
+			set_irq_wake(data->penirq, 1);
+		} else {
+			printk(KERN_ERR "%s: Cannot grab irq %d\n",
+			       __func__, data->penirq);
+		}
+	}
+	idev = input_allocate_device();
+	data->idev = idev;
+	input_set_drvdata(idev, data);
+	idev->name = DRIVER_NAME;
+	idev->evbit[0] = BIT(EV_ABS);
+	idev->open = tsc2007_idev_open;
+	idev->close = tsc2007_idev_close;
+	idev->absbit[0] = BIT(ABS_X) | BIT(ABS_Y) | BIT(ABS_PRESSURE);
+	input_set_abs_params(idev, ABS_X, 0, ADC_MAX, 0, 0);
+	input_set_abs_params(idev, ABS_Y, 0, ADC_MAX, 0, 0);
+	input_set_abs_params(idev, ABS_PRESSURE, 0, 0, 0, 0);
+
+	if (!ret)
+		ret = input_register_device(idev);
+
+	return ret;
+}
+
+static int tsc2007_i2c_remove(struct i2c_client *client)
+{
+	int err;
+	struct tsc2007_data *d = i2c_get_clientdata(client);
+	struct mxc_tsc_platform_data *tsc_data;
+
+	free_irq(d->penirq, d);
+	input_unregister_device(d->idev);
+
+	err = i2c_detach_client(client);
+	if (err) {
+		dev_err(&client->dev, "Client deregistration failed, "
+			"client not detached.\n");
+		return err;
+	}
+
+	tsc_data = (struct mxc_tsc_platform_data *)(client->dev).platform_data;
+	if (tsc_data && tsc_data->inactive)
+		tsc_data->inactive();
+
+	if (d->vdd_reg) {
+		regulator_disable(d->vdd_reg);
+		regulator_put(d->vdd_reg);
+		d->vdd_reg = NULL;
+	}
+	return 0;
+}
+
+static int tsc2007_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct tsc2007_data *data;
+	struct mxc_tsc_platform_data *tsc_data;
+	int err = 0;
+
+	data = kzalloc(sizeof(struct tsc2007_data), GFP_KERNEL);
+	if (data == NULL)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, data);
+	data->client = client;
+	data->penirq = client->irq;
+
+	tsc_data = (struct mxc_tsc_platform_data *)(client->dev).platform_data;
+	if (tsc_data && tsc_data->vdd_reg) {
+		if (tsc_data->penup_threshold > (ADC_MAX >> 3))
+			data->penup_threshold = (ADC_MAX >> 3);
+		else if (tsc_data->penup_threshold > 0)
+			data->penup_threshold = tsc_data->penup_threshold;
+		else
+			data->penup_threshold = 10;
+
+		data->vdd_reg = regulator_get(&client->dev, tsc_data->vdd_reg);
+		if (!IS_ERR(data->vdd_reg))
+			regulator_enable(data->vdd_reg);
+		else
+			data->vdd_reg = NULL;
+		if (tsc_data->active)
+			tsc_data->active();
+	} else {
+		data->vdd_reg = NULL;
+		data->penup_threshold = 10;
+	}
+
+	err = tsc2007_powerdown(data);
+	if (err >= 0) {
+		data->m = M_12BIT;
+
+		err = tsc2007_driver_register(data);
+		if (err < 0)
+			goto exit;
+
+		return 0;
+	}
+
+      exit:
+	return err;
+}
+
+static const struct i2c_device_id tsc2007_id[] = {
+	{ "tsc2007", 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, tsc2007_id);
+
+static struct i2c_driver tsc2007_driver = {
+	.driver = {
+		   .name = DRIVER_NAME,
+		   },
+	.probe = tsc2007_i2c_probe,
+	.remove = tsc2007_i2c_remove,
+	.suspend = tsc2007_suspend,
+	.resume = tsc2007_resume,
+	.command = NULL,
+	.id_table = tsc2007_id,
+};
+
+static int __init tsc2007_init(void)
+{
+	return i2c_add_driver(&tsc2007_driver);
+}
+
+static void __exit tsc2007_exit(void)
+{
+	i2c_del_driver(&tsc2007_driver);
+}
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("tsc2007 Touch Screen Controller driver");
+MODULE_LICENSE("GPL");
+
+module_init(tsc2007_init);
+module_exit(tsc2007_exit);
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index e7fb7d2fcbfc..f7aa808bdc37 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -17,6 +17,13 @@ config LEDS_CLASS
 
 comment "LED drivers"
 
+config LEDS_STMP378X
+	tristate "Support for PWM LEDs on STMP378X"
+	depends on LEDS_CLASS && MACH_STMP378X
+	help
+	  This option enables support for the LEDs connected to PWM
+	  outputs on the Freescale STMP378X.
+
 config LEDS_ATMEL_PWM
 	tristate "LED Support using Atmel PWM outputs"
 	depends on LEDS_CLASS && ATMEL_PWM
@@ -24,6 +31,10 @@ config LEDS_ATMEL_PWM
 	  This option enables support for LEDs driven using outputs
 	  of the dedicated PWM controller found on newer Atmel SOCs.
 
+config LEDS_MC13892
+	tristate "LED Support for mc13892 pmic"
+	depends on LEDS_CLASS && MXC_MC13892_LIGHT
+
 config LEDS_LOCOMO
 	tristate "LED Support for Locomo device"
 	depends on LEDS_CLASS && SHARP_LOCOMO
@@ -113,6 +124,12 @@ config LEDS_GPIO
 	  outputs. To be useful the particular board must have LEDs
 	  and they must be connected to the GPIO lines.
 
+config LEDS_PWM
+       tristate "LED Support for PWM connected LEDs"
+       depends on LEDS_CLASS && GENERIC_PWM
+       help
+         Enables support for LEDs connected to PWM outputs.
+
 config LEDS_HP_DISK
 	tristate "LED Support for disk protection LED on HP notebooks"
 	depends on LEDS_CLASS && ACPI
@@ -192,6 +209,14 @@ config LEDS_TRIGGER_IDE_DISK
 	  This allows LEDs to be controlled by IDE disk activity.
 	  If unsure, say Y.
 
+config LEDS_TRIGGER_DIM
+	tristate "LED Dimmer Trigger"
+	depends on LEDS_TRIGGERS
+	help
+	  Regulates the brightness of an LED based on the 1-minute CPU
+	  load average.  Ideal for PWM-driven LEDs.
+	  If unsure, say Y.
+
 config LEDS_TRIGGER_HEARTBEAT
 	tristate "LED Heartbeat Trigger"
 	depends on LEDS_TRIGGERS
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index e1967a29850e..18a8a10bb8c2 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -5,7 +5,9 @@ obj-$(CONFIG_LEDS_CLASS)		+= led-class.o
 obj-$(CONFIG_LEDS_TRIGGERS)		+= led-triggers.o
 
 # LED Platform Drivers
+obj-$(CONFIG_LEDS_STMP378X)		+= leds-stmp378x-pwm.o
 obj-$(CONFIG_LEDS_ATMEL_PWM)		+= leds-atmel-pwm.o
+obj-$(CONFIG_LEDS_MC13892)		+= leds-mc13892.o
 obj-$(CONFIG_LEDS_LOCOMO)		+= leds-locomo.o
 obj-$(CONFIG_LEDS_S3C24XX)		+= leds-s3c24xx.o
 obj-$(CONFIG_LEDS_AMS_DELTA)		+= leds-ams-delta.o
@@ -17,6 +19,7 @@ obj-$(CONFIG_LEDS_COBALT_RAQ)		+= leds-cobalt-raq.o
 obj-$(CONFIG_LEDS_SUNFIRE)		+= leds-sunfire.o
 obj-$(CONFIG_LEDS_PCA9532)		+= leds-pca9532.o
 obj-$(CONFIG_LEDS_GPIO)			+= leds-gpio.o
+obj-$(CONFIG_LEDS_PWM)			+= leds-pwm.o
 obj-$(CONFIG_LEDS_CLEVO_MAIL)		+= leds-clevo-mail.o
 obj-$(CONFIG_LEDS_HP6XX)		+= leds-hp6xx.o
 obj-$(CONFIG_LEDS_FSG)			+= leds-fsg.o
@@ -27,6 +30,7 @@ obj-$(CONFIG_LEDS_HP_DISK)		+= leds-hp-disk.o
 # LED Triggers
 obj-$(CONFIG_LEDS_TRIGGER_TIMER)	+= ledtrig-timer.o
 obj-$(CONFIG_LEDS_TRIGGER_IDE_DISK)	+= ledtrig-ide-disk.o
+obj-$(CONFIG_LEDS_TRIGGER_DIM)		+= ledtrig-dim.o
 obj-$(CONFIG_LEDS_TRIGGER_HEARTBEAT)	+= ledtrig-heartbeat.o
 obj-$(CONFIG_LEDS_TRIGGER_BACKLIGHT)	+= ledtrig-backlight.o
 obj-$(CONFIG_LEDS_TRIGGER_DEFAULT_ON)	+= ledtrig-default-on.o
diff --git a/drivers/leds/leds-mc13892.c b/drivers/leds/leds-mc13892.c
new file mode 100644
index 000000000000..9edd20446235
--- /dev/null
+++ b/drivers/leds/leds-mc13892.c
@@ -0,0 +1,152 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/leds.h>
+#include <linux/pmic_light.h>
+
+static void mc13892_led_set(struct led_classdev *led_cdev,
+			    enum led_brightness value)
+{
+	struct platform_device *dev = to_platform_device(led_cdev->dev->parent);
+	int led_ch;
+
+	switch (dev->id) {
+	case 'r':
+		led_ch = LIT_RED;
+		break;
+	case 'g':
+		led_ch = LIT_GREEN;
+		break;
+	case 'b':
+		led_ch = LIT_BLUE;
+		break;
+	default:
+		return;
+	}
+
+	/* set current with medium value, in case current is too large */
+	mc13892_bklit_set_current(led_ch, LIT_CURR_12);
+	/* max duty cycle is 63, brightness needs to be divided by 4 */
+	mc13892_bklit_set_dutycycle(led_ch, value / 4);
+
+}
+
+static int mc13892_led_remove(struct platform_device *dev)
+{
+	struct led_classdev *led_cdev = platform_get_drvdata(dev);
+
+	led_classdev_unregister(led_cdev);
+	kfree(led_cdev->name);
+	kfree(led_cdev);
+
+	return 0;
+}
+
+#define LED_NAME_LEN	16
+
+static int mc13892_led_probe(struct platform_device *dev)
+{
+	int ret;
+	struct led_classdev *led_cdev;
+	char *name;
+
+	led_cdev = kzalloc(sizeof(struct led_classdev), GFP_KERNEL);
+	if (led_cdev == NULL) {
+		dev_err(&dev->dev, "No memory for device\n");
+		return -ENOMEM;
+	}
+	name = kzalloc(LED_NAME_LEN, GFP_KERNEL);
+	if (name == NULL) {
+		dev_err(&dev->dev, "No memory for device\n");
+		ret = -ENOMEM;
+		goto exit_err;
+	}
+
+	strcpy(name, dev->name);
+	ret = strlen(dev->name);
+	if (ret > LED_NAME_LEN - 2) {
+		dev_err(&dev->dev, "led name is too long\n");
+		goto exit_err1;
+	}
+	name[ret] = dev->id;
+	name[ret + 1] = '\0';
+	led_cdev->name = name;
+	led_cdev->brightness_set = mc13892_led_set;
+
+	ret = led_classdev_register(&dev->dev, led_cdev);
+	if (ret < 0) {
+		dev_err(&dev->dev, "led_classdev_register failed\n");
+		goto exit_err1;
+	}
+
+	platform_set_drvdata(dev, led_cdev);
+
+	return 0;
+      exit_err1:
+	kfree(led_cdev->name);
+      exit_err:
+	kfree(led_cdev);
+	return ret;
+}
+
+#ifdef CONFIG_PM
+static int mc13892_led_suspend(struct platform_device *dev, pm_message_t state)
+{
+	struct led_classdev *led_cdev = platform_get_drvdata(dev);
+
+	led_classdev_suspend(led_cdev);
+	return 0;
+}
+
+static int mc13892_led_resume(struct platform_device *dev)
+{
+	struct led_classdev *led_cdev = platform_get_drvdata(dev);
+
+	led_classdev_resume(led_cdev);
+	return 0;
+}
+#else
+#define mc13892_led_suspend NULL
+#define mc13892_led_resume NULL
+#endif
+
+static struct platform_driver mc13892_led_driver = {
+	.probe = mc13892_led_probe,
+	.remove = mc13892_led_remove,
+	.suspend = mc13892_led_suspend,
+	.resume = mc13892_led_resume,
+	.driver = {
+		   .name = "pmic_leds",
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static int __init mc13892_led_init(void)
+{
+	return platform_driver_register(&mc13892_led_driver);
+}
+
+static void __exit mc13892_led_exit(void)
+{
+	platform_driver_unregister(&mc13892_led_driver);
+}
+
+module_init(mc13892_led_init);
+module_exit(mc13892_led_exit);
+
+MODULE_DESCRIPTION("Led driver for PMIC mc13892");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/leds/leds-pwm.c b/drivers/leds/leds-pwm.c
new file mode 100644
index 000000000000..fe2477e777e2
--- /dev/null
+++ b/drivers/leds/leds-pwm.c
@@ -0,0 +1,189 @@
+/*
+ * drivers/leds/leds-pwm.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * All rights reserved.
+ *
+ * Code rebased on original leds-pwm.c from Bill Gatliff
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/leds.h>
+#include <linux/io.h>
+#include <linux/pwm.h>
+#include <linux/pwm-led.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+
+
+struct led_pwm {
+	struct led_classdev	led;
+	struct pwm_channel	*pwm;
+	unsigned long period;
+};
+
+static void
+led_pwm_brightness_set(struct led_classdev *c,
+		       enum led_brightness b)
+{
+	struct led_pwm *led;
+	unsigned long period;
+
+	period = 1000000000UL;
+	led = container_of(c, struct led_pwm, led);
+	led->period = period;
+	pwm_set_period_ns(led->pwm, period);
+}
+
+
+static enum led_brightness
+led_pwm_brightness_get(struct led_classdev *c)
+{
+	struct led_pwm *led;
+	led = container_of(c, struct led_pwm, led);
+	return led->period;
+}
+
+static int
+led_pwm_blink_set(struct led_classdev *c,
+		  unsigned long *on_ms,
+		  unsigned long *off_ms)
+{
+	struct led_pwm *led;
+	struct pwm_channel_config cfg;
+
+	led = container_of(c, struct led_pwm, led);
+
+	if (*on_ms == 0 && *off_ms == 0) {
+		*on_ms = 1000UL;
+		*off_ms = 1000UL;
+	}
+
+	cfg.config_mask = PWM_CONFIG_DUTY_NS
+		| PWM_CONFIG_PERIOD_NS;
+
+	cfg.duty_ns = *on_ms * 1000000000UL;
+	cfg.period_ns = (*on_ms + *off_ms) * 1000000000UL;
+
+	return pwm_config(led->pwm, &cfg);
+}
+
+
+static int __init
+led_pwm_probe(struct platform_device *pdev)
+{
+	struct pwm_led_platform_data *pdata = pdev->dev.platform_data;
+	struct led_pwm *led;
+	int ret;
+
+	if (!pdata || !pdata->led_info) {
+		return -EINVAL;
+	}
+
+	led = kzalloc(sizeof(*led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+	
+	led->pwm = pwm_request(pdata->bus_id, pdata->chan, 
+				pdata->led_info->name);
+
+	if (!led->pwm) {
+		ret = -EINVAL;
+		goto err_pwm_request;
+	}
+
+	platform_set_drvdata(pdev, led);
+
+	led->led.name = pdata->led_info->name;
+	led->led.default_trigger = pdata->led_info->default_trigger;
+	led->led.brightness_set = led_pwm_brightness_set;
+	led->led.brightness_get = led_pwm_brightness_get;
+	led->led.blink_set = led_pwm_blink_set;
+	led->led.brightness = LED_OFF;
+ 
+	ret = pwm_config(led->pwm, pdata->config);
+	if (ret)
+		goto err_pwm_config;
+	
+	pwm_start(led->pwm);
+
+	ret = led_classdev_register(&pdev->dev, &led->led);
+	if (ret < 0)
+		goto err_classdev_register;
+
+	return 0;
+
+err_classdev_register:
+	pwm_stop(led->pwm);
+err_pwm_config:
+	pwm_free(led->pwm);
+err_pwm_request:
+	kfree(led);
+
+	return ret;
+}
+
+static int
+led_pwm_remove(struct platform_device *pdev)
+{
+	struct led_pwm *led = platform_get_drvdata(pdev);
+
+	led_classdev_unregister(&led->led);
+
+ 	if (led->pwm) {
+		pwm_stop(led->pwm);
+		pwm_free(led->pwm);
+ 	}
+
+	kfree(led);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+
+static struct platform_driver led_pwm_driver = {
+	.driver = {
+		.name =		"leds-pwm",
+		.owner =	THIS_MODULE,
+	},
+	.probe = led_pwm_probe,
+	.remove = led_pwm_remove,
+};
+
+
+static int __init led_pwm_modinit(void)
+{
+	return platform_driver_register(&led_pwm_driver);
+}
+late_initcall(led_pwm_modinit);
+
+
+static void __exit led_pwm_modexit(void)
+{
+	platform_driver_unregister(&led_pwm_driver);
+}
+module_exit(led_pwm_modexit);
+
+
+MODULE_AUTHOR("Hector Oron <Hector.Oron <at> digi.com>");
+MODULE_DESCRIPTION("Driver for LEDs with PWM-controlled brightness");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:leds-pwm");
+
diff --git a/drivers/leds/leds-stmp378x-pwm.c b/drivers/leds/leds-stmp378x-pwm.c
new file mode 100644
index 000000000000..f0865db4eb90
--- /dev/null
+++ b/drivers/leds/leds-stmp378x-pwm.c
@@ -0,0 +1,190 @@
+/*
+ * Freescale STMP378X PWM LED driver
+ *
+ * Author: Drew Benedetti <drewb@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/leds.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <mach/hardware.h>
+#include <mach/regs-pwm.h>
+#include <mach/regs-clkctrl.h>
+#include <mach/pwm-led.h>
+#include <mach/stmp3xxx.h>
+
+/* Up to 5 PWM lines are available. */
+#define PWM_MAX 5
+
+/* PWM enables are the lowest PWM_MAX bits of HW_PWM_CTRL register */
+#define BM_PWM_CTRL_PWM_ENABLE(n)	((1<<(n)) & ((1<<(PWM_MAX))-1))
+#define BF_PWM_PERIODn_SETTINGS					\
+		(BF_PWM_PERIODn_CDIV(5) | /* divide by 64 */ 	\
+		BF_PWM_PERIODn_INACTIVE_STATE(2) | /* low */ 	\
+		BF_PWM_PERIODn_ACTIVE_STATE(3) | /* high */ 	\
+		BF_PWM_PERIODn_PERIOD(LED_FULL)) /* 255 cycles */
+
+struct stmp378x_led {
+	struct led_classdev led_dev;
+	int in_use;
+};
+
+static struct stmp378x_led leds[PWM_MAX];
+
+static struct clk *pwm_clk;
+
+static void stmp378x_pwm_led_brightness_set(struct led_classdev *pled,
+					    enum led_brightness value)
+{
+	unsigned int pwmn;
+
+	pwmn = container_of(pled, struct stmp378x_led, led_dev) - leds;
+
+	if (pwmn < PWM_MAX && leds[pwmn].in_use) {
+		HW_PWM_CTRL_CLR(BM_PWM_CTRL_PWM_ENABLE(pwmn));
+		HW_PWM_ACTIVEn_WR(pwmn, BF_PWM_ACTIVEn_INACTIVE(value) |
+				BF_PWM_ACTIVEn_ACTIVE(0));
+		HW_PWM_PERIODn_WR(pwmn, BF_PWM_PERIODn_SETTINGS);
+		HW_PWM_CTRL_SET(BM_PWM_CTRL_PWM_ENABLE(pwmn));
+	}
+}
+
+static int stmp378x_pwm_led_probe(struct platform_device *pdev)
+{
+	struct led_classdev *led;
+	unsigned int pwmn;
+	int leds_in_use = 0, rc = 0;
+	int i;
+
+	stmp3xxx_reset_block(REGS_PWM_BASE, 1);
+
+	pwm_clk = clk_get(&pdev->dev, "pwm");
+	if (IS_ERR(pwm_clk)) {
+		rc = PTR_ERR(pwm_clk);
+		return rc;
+	}
+
+	clk_enable(pwm_clk);
+
+	for (i = 0; i < pdev->num_resources; i++) {
+
+		if (pdev->resource[i].flags & IORESOURCE_DISABLED)
+			continue;
+
+		pwmn = pdev->resource[i].start;
+		if (pwmn >= PWM_MAX) {
+			dev_err(&pdev->dev, "PWM %d doesn't exist\n", pwmn);
+			continue;
+		}
+
+		rc = pwm_led_pinmux_request(pwmn, "stmp378x_pwm_led");
+		if (rc) {
+			dev_err(&pdev->dev,
+				"PWM %d is not available (err=%d)\n",
+				pwmn, rc);
+			continue;
+		}
+
+		led = &leds[pwmn].led_dev;
+
+		led->flags = pdev->resource[i].flags;
+		led->name = pdev->resource[i].name;
+		led->brightness = LED_HALF;
+		led->flags = 0;
+		led->brightness_set = stmp378x_pwm_led_brightness_set;
+		led->default_trigger = 0;
+
+		rc = led_classdev_register(&pdev->dev, led);
+		if (rc < 0) {
+			dev_err(&pdev->dev,
+				"Unable to register LED device %d (err=%d)\n",
+				pwmn, rc);
+			pwm_led_pinmux_free(pwmn, "stmp378x_pwm_led");
+			continue;
+		}
+
+		/* PWM LED is available now */
+		leds[pwmn].in_use = !0;
+		leds_in_use++;
+
+		/* Set default brightness */
+		stmp378x_pwm_led_brightness_set(led, LED_HALF);
+	}
+
+	if (leds_in_use == 0) {
+		dev_info(&pdev->dev, "No PWM LEDs available\n");
+		clk_disable(pwm_clk);
+		clk_put(pwm_clk);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int stmp378x_pwm_led_remove(struct platform_device *pdev)
+{
+	unsigned int pwmn;
+
+	for (pwmn = 0; pwmn < PWM_MAX; pwmn++) {
+
+		if (!leds[pwmn].in_use)
+			continue;
+
+		/* Disable LED */
+		HW_PWM_CTRL_CLR(BM_PWM_CTRL_PWM_ENABLE(pwmn));
+		HW_PWM_ACTIVEn_WR(pwmn, BF_PWM_ACTIVEn_INACTIVE(0) |
+				BF_PWM_ACTIVEn_ACTIVE(0));
+		HW_PWM_PERIODn_WR(pwmn, BF_PWM_PERIODn_SETTINGS);
+
+		led_classdev_unregister(&leds[pwmn].led_dev);
+		pwm_led_pinmux_free(pwmn, "stmp378x_pwm_led");
+
+		leds[pwmn].led_dev.name = 0;
+		leds[pwmn].in_use = 0;
+	}
+
+	clk_disable(pwm_clk);
+	clk_put(pwm_clk);
+
+	return 0;
+}
+
+
+static struct platform_driver stmp378x_pwm_led_driver = {
+	.probe   = stmp378x_pwm_led_probe,
+	.remove  = stmp378x_pwm_led_remove,
+	.driver  = {
+		.name = "stmp378x-pwm-led",
+	},
+};
+
+static int __init stmp378x_pwm_led_init(void)
+{
+	return platform_driver_register(&stmp378x_pwm_led_driver);
+}
+
+static void __exit stmp378x_pwm_led_exit(void)
+{
+	platform_driver_unregister(&stmp378x_pwm_led_driver);
+}
+
+module_init(stmp378x_pwm_led_init);
+module_exit(stmp378x_pwm_led_exit);
+
+MODULE_AUTHOR("Drew Benedetti <drewb@embeddedalley.com>");
+MODULE_DESCRIPTION("STMP378X PWM LED driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/leds/ledtrig-dim.c b/drivers/leds/ledtrig-dim.c
new file mode 100644
index 000000000000..04dbaf66a49e
--- /dev/null
+++ b/drivers/leds/ledtrig-dim.c
@@ -0,0 +1,95 @@
+/*
+ * LED Dim Trigger
+ *
+ * Copyright (C) 2008 Bill Gatliff <bgat <at> billgatliff.com>
+ *
+ * "Dims" an LED based on system load.  Derived from Atsushi Nemoto's
+ * ledtrig-heartbeat.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.
+ *
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/timer.h>
+#include <linux/sched.h>
+#include <linux/leds.h>
+
+#include "leds.h"
+
+struct dim_trig_data {
+	struct timer_list timer;
+};
+
+
+static void
+led_dim_function(unsigned long data)
+{
+	struct led_classdev *led_cdev = (struct led_classdev *)data;
+	struct dim_trig_data *dim_data = led_cdev->trigger_data;
+	unsigned int brightness;
+
+	brightness = ((LED_FULL - LED_OFF) * avenrun[0]) / EXP_1;
+	if (brightness > LED_FULL)
+		brightness = LED_FULL;
+
+	led_set_brightness(led_cdev, brightness);
+	mod_timer(&dim_data->timer, jiffies + msecs_to_jiffies(500));
+}
+
+
+static void
+dim_trig_activate(struct led_classdev *led_cdev)
+{
+	struct dim_trig_data *dim_data;
+
+	dim_data = kzalloc(sizeof(*dim_data), GFP_KERNEL);
+	if (!dim_data)
+		return;
+
+	led_cdev->trigger_data = dim_data;
+	setup_timer(&dim_data->timer,
+		    led_dim_function, (unsigned long)led_cdev);
+	led_dim_function(dim_data->timer.data);
+}
+
+
+static void
+dim_trig_deactivate(struct led_classdev *led_cdev)
+{
+	struct dim_trig_data *dim_data = led_cdev->trigger_data;
+
+	if (dim_data) {
+		del_timer_sync(&dim_data->timer);
+		kfree(dim_data);
+	}
+}
+
+
+static struct led_trigger dim_led_trigger = {
+	.name     = "dim",
+	.activate = dim_trig_activate,
+	.deactivate = dim_trig_deactivate,
+};
+
+
+static int __init dim_trig_init(void)
+{
+	return led_trigger_register(&dim_led_trigger);
+}
+module_init(dim_trig_init);
+
+
+static void __exit dim_trig_exit(void)
+{
+	led_trigger_unregister(&dim_led_trigger);
+}
+module_exit(dim_trig_exit);
+
+
+MODULE_AUTHOR("Bill Gatliff <bgat <at> billgatliff.com>");
+MODULE_DESCRIPTION("Dim LED trigger");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index 5189c4eb439f..92990ac87b39 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -387,4 +387,6 @@ config USB_MR800
 	  To compile this driver as a module, choose M here: the
 	  module will be called radio-mr800.
 
+source "drivers/media/radio/stfm1000/Kconfig"
+
 endif # RADIO_ADAPTERS
diff --git a/drivers/media/radio/Makefile b/drivers/media/radio/Makefile
index 240ec63cdafc..00cf526d8e19 100644
--- a/drivers/media/radio/Makefile
+++ b/drivers/media/radio/Makefile
@@ -20,4 +20,6 @@ obj-$(CONFIG_USB_DSBR) += dsbr100.o
 obj-$(CONFIG_USB_SI470X) += radio-si470x.o
 obj-$(CONFIG_USB_MR800) += radio-mr800.o
 
+obj-$(CONFIG_RADIO_STFM1000) += stfm1000/
+
 EXTRA_CFLAGS += -Isound
diff --git a/drivers/media/radio/stfm1000/Kconfig b/drivers/media/radio/stfm1000/Kconfig
new file mode 100644
index 000000000000..ef30bf87de0b
--- /dev/null
+++ b/drivers/media/radio/stfm1000/Kconfig
@@ -0,0 +1,26 @@
+config RADIO_STFM1000
+	tristate "STFM1000 support"
+	depends on I2C && VIDEO_V4L2 && ARCH_STMP3XXX
+	select I2C_ALGOBIT
+	---help---
+	  Choose Y here if you have this FM radio card, and then fill in the
+	  port address below.
+
+	  In order to control your radio card, you will need to use programs
+	  that are compatible with the Video For Linux API.  Information on
+	  this API and pointers to "v4l" programs may be found at
+	  <file:Documentation/video4linux/API.html>.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called stfm1000.
+
+config RADIO_STFM1000_ALSA
+	tristate "STFM1000 audio support"
+	depends on RADIO_STFM1000 && SND
+	select SND_PCM
+	---help---
+	  This is a video4linux driver for direct (DMA) audio in
+	  STFM1000 using ALSA
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called stfm1000-alsa.
diff --git a/drivers/media/radio/stfm1000/Makefile b/drivers/media/radio/stfm1000/Makefile
new file mode 100644
index 000000000000..01f354001a64
--- /dev/null
+++ b/drivers/media/radio/stfm1000/Makefile
@@ -0,0 +1,14 @@
+stfm1000-objs	:= stfm1000-core.o stfm1000-i2c.o stfm1000-precalc.o stfm1000-filter.o stfm1000-rds.o
+
+clean-files += stfm1000-precalc.o
+
+obj-$(CONFIG_RADIO_STFM1000) += stfm1000.o
+obj-$(CONFIG_RADIO_STFM1000_ALSA) += stfm1000-alsa.o
+
+stfm1000-core.o: $(obj)/stfm1000-precalc.h
+
+hostprogs-$(CONFIG_RADIO_STFM1000) := gen-precalc
+$(obj)/stfm1000-precalc.c: $(obj)/gen-precalc $(src)/stfm1000-regs.h
+	$(obj)/gen-precalc >$@
+
+EXTRA_CFLAGS += -Idrivers/media/radio
diff --git a/drivers/media/radio/stfm1000/gen-precalc.c b/drivers/media/radio/stfm1000/gen-precalc.c
new file mode 100644
index 000000000000..d3797dbef815
--- /dev/null
+++ b/drivers/media/radio/stfm1000/gen-precalc.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/* generate precalculated tables */
+#include <stddef.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "stfm1000-regs.h"
+
+static void generate_tune1(void)
+{
+	int start, end;
+	int ndiv;           // N Divider in PLL
+	int incr;           // Increment in PLL
+	int cicosr;         // CIC oversampling ratio
+	int sdnominal;      // value to serve pilot/interpolator loop in SD
+	int i, temp;        // used in tuning table construction
+
+	start = STFM1000_FREQUENCY_100KHZ_MIN;
+	end = start + STFM1000_FREQUENCY_100KHZ_RANGE;
+
+	printf("const struct stfm1000_tune1\n"
+		"stfm1000_tune1_table[STFM1000_FREQUENCY_100KHZ_RANGE] = {\n");
+
+	for (i = start; i < end; i++) {
+
+		ndiv = (int)((i+14)/15) - 48;
+		incr = i - (int)(i/15)*15;
+		cicosr = (int)(i*2/3.0/16.0 + 0.5);
+		sdnominal = (int)(i*100.0e3/1.5/(double)cicosr/2.0/2.0*2.0*8.0*256.0/228.0e3*65536);
+
+		temp = 0x00000000;                                    // clear
+		temp = temp | ((cicosr<<9) & STFM1000_TUNE1_CICOSR);  // bits[14:9] 0x00007E00
+		temp = temp | ((ndiv<<4)   & STFM1000_TUNE1_PLL_DIV); // bits[8:4]  0x000001F0
+		temp = temp | ((incr)      & STFM1000_TUNE1_PLL_DIV); // bits[3:0]  0x0000000F
+
+		printf("\t[%d - STFM1000_FREQUENCY_100KHZ_MIN] = "
+			"{ .tune1 = 0x%08x, .sdnom = 0x%08x },\n",
+			i, temp, sdnominal);
+	}
+	printf("};\n");
+
+}
+
+int main(int argc, char *argv[])
+{
+	printf("#include \"stfm1000-regs.h\"\n\n");
+
+	generate_tune1();
+
+	return 0;
+}
diff --git a/drivers/media/radio/stfm1000/stfm1000-alsa.c b/drivers/media/radio/stfm1000/stfm1000-alsa.c
new file mode 100644
index 000000000000..d1da4475bc07
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-alsa.c
@@ -0,0 +1,660 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/math64.h>
+
+#include <sound/control.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <linux/version.h>      /* for KERNEL_VERSION MACRO     */
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <mach/regs-dri.h>
+#include <mach/regs-apbx.h>
+#include <mach/regs-clkctrl.h>
+
+#include "stfm1000.h"
+
+#define STFM1000_PERIODS	16
+
+static int stfm1000_snd_volume_info(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_info *uinfo)
+{
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
+	uinfo->count = 2;			/* two channels */
+	uinfo->value.integer.min = 0;
+	uinfo->value.integer.max = 20;
+	return 0;
+}
+
+static int stfm1000_snd_volume_get(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *ucontrol)
+{
+	struct stfm1000 *stfm1000 = snd_kcontrol_chip(kcontrol);
+
+	(void)stfm1000;
+	ucontrol->value.integer.value[0] = 0;	/* left */
+	ucontrol->value.integer.value[1] = 0;	/* right */
+	return 0;
+}
+
+static int stfm1000_snd_volume_put(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *ucontrol)
+{
+	struct stfm1000 *stfm1000 = snd_kcontrol_chip(kcontrol);
+	int change;
+	int left, right;
+
+	(void)stfm1000;
+
+	left = ucontrol->value.integer.value[0];
+	if (left < 0)
+		left = 0;
+	if (left > 20)
+		left = 20;
+	right = ucontrol->value.integer.value[1];
+	if (right < 0)
+		right = 0;
+	if (right > 20)
+		right = 20;
+
+	change = 1;
+	return change;
+}
+
+static struct snd_kcontrol_new stfm1000_snd_controls[] = {
+	{
+		.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+		.name = "Radio Volume",
+		.index = 0,
+		.info = stfm1000_snd_volume_info,
+		.get = stfm1000_snd_volume_get,
+		.put = stfm1000_snd_volume_put,
+		.private_value = 0,
+	},
+};
+
+static struct snd_pcm_hardware stfm1000_snd_capture = {
+
+	.info =                 SNDRV_PCM_INFO_MMAP |
+				SNDRV_PCM_INFO_MMAP_VALID |
+				SNDRV_PCM_INFO_INTERLEAVED |
+				SNDRV_PCM_INFO_BLOCK_TRANSFER,
+	.formats =		SNDRV_PCM_FMTBIT_S16_LE,
+	.rates =		SNDRV_PCM_RATE_44100 | SNDRV_PCM_RATE_48000,
+	.rate_min =		44100,
+	.rate_max =		48000,
+	.channels_min =		2,
+	.channels_max =		2,
+	.buffer_bytes_max =	SZ_256K,
+	.period_bytes_min =	SZ_4K,
+	.period_bytes_max =	SZ_4K,
+	.periods_min =		STFM1000_PERIODS,
+	.periods_max =		STFM1000_PERIODS,
+};
+
+static int stfm1000_snd_capture_open(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct stfm1000 *stfm1000 = snd_pcm_substream_chip(substream);
+	int err;
+
+	/* should never happen, just a sanity check */
+	BUG_ON(stfm1000 == NULL);
+
+	mutex_lock(&stfm1000->deffered_work_lock);
+	stfm1000->read_count  = 0;
+	stfm1000->read_offset = 0;
+
+	stfm1000->substream = substream;
+	runtime->private_data = stfm1000;
+	runtime->hw = stfm1000_snd_capture;
+
+	mutex_unlock(&stfm1000->deffered_work_lock);
+
+	err = snd_pcm_hw_constraint_integer(runtime,
+		SNDRV_PCM_HW_PARAM_PERIODS);
+	if (err < 0) {
+		printk(KERN_ERR "%s: snd_pcm_hw_constraint_integer "
+			"SNDRV_PCM_HW_PARAM_PERIODS failed\n", __func__);
+		return err;
+	}
+
+	err = snd_pcm_hw_constraint_step(runtime, 0,
+		SNDRV_PCM_HW_PARAM_PERIODS, 2);
+	if (err < 0) {
+		printk(KERN_ERR "%s: snd_pcm_hw_constraint_integer "
+			"SNDRV_PCM_HW_PARAM_PERIODS failed\n", __func__);
+		return err;
+	}
+
+	return 0;
+}
+
+static int stfm1000_snd_capture_close(struct snd_pcm_substream *substream)
+{
+	struct stfm1000 *stfm1000 = snd_pcm_substream_chip(substream);
+
+	(void)stfm1000;	/* nothing */
+	return 0;
+}
+
+static int stfm1000_snd_hw_params(struct snd_pcm_substream *substream,
+	struct snd_pcm_hw_params *hw_params)
+{
+	struct stfm1000 *stfm1000 = snd_pcm_substream_chip(substream);
+	unsigned int period_size, periods;
+	int ret;
+
+	periods = params_periods(hw_params);
+	period_size = params_period_bytes(hw_params);
+
+	if (period_size < 0x100 || period_size > 0x10000)
+		return -EINVAL;
+	if (periods < STFM1000_PERIODS)
+		return -EINVAL;
+	if (period_size * periods > 1024 * 1024)
+		return -EINVAL;
+
+	stfm1000->blocks  = periods;
+	stfm1000->blksize = period_size;
+	stfm1000->bufsize = params_buffer_bytes(hw_params);
+
+	ret = snd_pcm_lib_malloc_pages(substream, stfm1000->bufsize);
+	if (ret < 0) {	/* 0 & 1 are valid returns */
+		printk(KERN_ERR "%s: snd_pcm_lib_malloc_pages() failed\n",
+			__func__);
+		return ret;
+	}
+
+	/* the dri buffer is twice as large as the audio buffer */
+	stfm1000->dri_bufsz = (stfm1000->bufsize / 4) *
+		sizeof(struct stfm1000_dri_sample);
+	stfm1000->dri_buf = dma_alloc_coherent(&stfm1000->radio.dev,
+			stfm1000->dri_bufsz, &stfm1000->dri_phys, GFP_KERNEL);
+	if (stfm1000->dri_buf == NULL) {
+		printk(KERN_ERR "%s: dma_alloc_coherent() failed\n", __func__);
+		snd_pcm_lib_free_pages(substream);
+		return -ENOMEM;
+	}
+
+	return ret;
+}
+
+static int stfm1000_snd_hw_free(struct snd_pcm_substream *substream)
+{
+	struct stfm1000 *stfm1000 = snd_pcm_substream_chip(substream);
+
+	if (stfm1000->dri_buf) {
+		dma_free_coherent(&stfm1000->radio.dev,
+			(stfm1000->bufsize / 4) *
+			sizeof(struct stfm1000_dri_sample),
+			stfm1000->dri_buf, stfm1000->dri_phys);
+		stfm1000->dri_buf = NULL;
+		stfm1000->dri_phys = 0;
+	}
+	snd_pcm_lib_free_pages(substream);
+	return 0;
+}
+
+
+static int stfm1000_snd_capture_prepare(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct stfm1000 *stfm1000 = snd_pcm_substream_chip(substream);
+
+	stfm1000->substream = substream;
+
+	if (snd_pcm_format_width(runtime->format) != 16 ||
+		!snd_pcm_format_signed(runtime->format) ||
+			snd_pcm_format_big_endian(runtime->format)) {
+		printk(KERN_INFO "STFM1000: ALSA capture_prepare illegal format\n");
+		return -EINVAL;
+	}
+
+	/* really shouldn't happen */
+	BUG_ON(stfm1000->blocks > stfm1000->desc_num);
+
+	mutex_lock(&stfm1000->deffered_work_lock);
+
+	if (stfm1000->now_recording != 0) {
+		printk(KERN_INFO "STFM1000: ALSA capture_prepare still running\n");
+		mutex_unlock(&stfm1000->deffered_work_lock);
+		return -EBUSY;
+	}
+	stfm1000->now_recording = 1;
+
+	mutex_unlock(&stfm1000->deffered_work_lock);
+
+	return 0;
+
+}
+
+static void stfm1000_snd_capture_trigger_start(struct work_struct *work)
+{
+	struct stfm1000 *stfm1000;
+
+	stfm1000 = container_of(work, struct stfm1000,
+		snd_capture_start_work.work);
+
+	mutex_lock(&stfm1000->deffered_work_lock);
+
+	BUG_ON(stfm1000->now_recording != 1);
+
+	stfm1000_bring_up(stfm1000);
+
+	mutex_unlock(&stfm1000->deffered_work_lock);
+}
+
+static void stfm1000_snd_capture_trigger_stop(struct work_struct *work)
+{
+	struct stfm1000 *stfm1000;
+
+	stfm1000 = container_of(work, struct stfm1000,
+		snd_capture_stop_work.work);
+
+	mutex_lock(&stfm1000->deffered_work_lock);
+
+	stfm1000->stopping_recording = 1;
+
+	stfm1000_take_down(stfm1000);
+
+	BUG_ON(stfm1000->now_recording != 1);
+	stfm1000->now_recording = 0;
+
+	stfm1000->stopping_recording = 0;
+
+	mutex_unlock(&stfm1000->deffered_work_lock);
+}
+
+static int execute_non_atomic(work_func_t fn, struct execute_work *ew)
+{
+	if (!in_atomic() && !in_interrupt()) {
+		fn(&ew->work);
+		return 0;
+	}
+
+	INIT_WORK(&ew->work, fn);
+	schedule_work(&ew->work);
+
+	return 1;
+}
+
+static int stfm1000_snd_capture_trigger(struct snd_pcm_substream *substream,
+	int cmd)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct stfm1000 *stfm1000 = runtime->private_data;
+	int err = 0;
+
+	(void)stfm1000;
+
+	switch (cmd) {
+
+	case SNDRV_PCM_TRIGGER_START:
+		execute_non_atomic(stfm1000_snd_capture_trigger_start,
+			&stfm1000->snd_capture_start_work);
+		break;
+
+	case SNDRV_PCM_TRIGGER_STOP:
+		execute_non_atomic(stfm1000_snd_capture_trigger_stop,
+			&stfm1000->snd_capture_stop_work);
+		break;
+
+	case SNDRV_PCM_TRIGGER_RESUME:
+	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
+		stmp3xxx_dma_unfreeze(stfm1000->dma_ch);
+		break;
+
+	case SNDRV_PCM_TRIGGER_SUSPEND:
+	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
+		stmp3xxx_dma_freeze(stfm1000->dma_ch);
+		break;
+
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	return err;
+}
+
+static snd_pcm_uframes_t
+stfm1000_snd_capture_pointer(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct stfm1000 *stfm1000 = runtime->private_data;
+
+	if (stfm1000->read_count) {
+		stfm1000->read_count  -= snd_pcm_lib_period_bytes(substream);
+		stfm1000->read_offset += snd_pcm_lib_period_bytes(substream);
+		if (stfm1000->read_offset == substream->runtime->dma_bytes)
+			stfm1000->read_offset = 0;
+	}
+
+	return bytes_to_frames(runtime, stfm1000->read_offset);
+}
+
+static struct snd_pcm_ops stfm1000_snd_capture_ops = {
+	.open = stfm1000_snd_capture_open,
+	.close = stfm1000_snd_capture_close,
+	.ioctl = snd_pcm_lib_ioctl,
+	.hw_params = stfm1000_snd_hw_params,
+	.hw_free = stfm1000_snd_hw_free,
+	.prepare = stfm1000_snd_capture_prepare,
+	.trigger = stfm1000_snd_capture_trigger,
+	.pointer = stfm1000_snd_capture_pointer,
+};
+
+static void stfm1000_snd_free(struct snd_card *card)
+{
+	struct stfm1000 *stfm1000 = card->private_data;
+
+	free_irq(IRQ_DRI_ATTENTION, stfm1000);
+	free_irq(IRQ_DRI_DMA, stfm1000);
+}
+
+static int stfm1000_alsa_instance_init(struct stfm1000 *stfm1000)
+{
+	int ret, i;
+	struct snd_card *card;
+	struct snd_pcm *pcm;
+	struct snd_kcontrol *ctl;
+
+	mutex_init(&stfm1000->deffered_work_lock);
+
+	/* request dma channel */
+	stfm1000->desc_num = STFM1000_PERIODS;
+	stfm1000->dma_ch = STMP3xxx_DMA(5, STMP3XXX_BUS_APBX);
+	ret = stmp3xxx_dma_request(stfm1000->dma_ch, &stfm1000->radio.dev,
+			"stmp3xxx dri");
+	if (ret != 0) {
+		printk(KERN_ERR "%s: stmp3xxx_dma_request failed\n", __func__);
+		goto err;
+	}
+
+	stfm1000->dma = kzalloc(sizeof(*stfm1000->dma) * stfm1000->desc_num,
+			GFP_KERNEL);
+	if (stfm1000->dma == NULL) {
+		printk(KERN_ERR "%s: stmp3xxx_dma_request failed\n", __func__);
+		ret = -ENOMEM;
+		goto err_rel_dma;
+	}
+
+	for (i = 0; i < stfm1000->desc_num; i++) {
+		ret = stmp3xxx_dma_allocate_command(stfm1000->dma_ch,
+				&stfm1000->dma[i]);
+		if (ret != 0) {
+			printk(KERN_ERR "%s: stmp3xxx_dma_allocate_command "
+				"failed\n", __func__);
+			goto err_free_dma;
+		}
+	}
+
+	/* allocate ALSA card structure (we only need an extra pointer
+	 * back to stfm1000) */
+	card = snd_card_new(-1, NULL, THIS_MODULE, 0);
+	if (card == NULL) {
+		ret = -ENOMEM;
+		printk(KERN_ERR "%s: snd_card_new failed\n", __func__);
+		goto err_free_dma;
+	}
+	stfm1000->card = card;
+	card->private_data = stfm1000;	/* point back */
+
+	/* mixer controls */
+	strcpy(card->driver, "stfm1000");
+	card->private_free = stfm1000_snd_free;
+
+	strcpy(card->mixername, "stfm1000 mixer");
+	for (i = 0; i < ARRAY_SIZE(stfm1000_snd_controls); i++) {
+		ctl = snd_ctl_new1(&stfm1000_snd_controls[i], stfm1000);
+		if (ctl == NULL) {
+			printk(KERN_ERR "%s: snd_ctl_new1 failed\n", __func__);
+			goto err_free_controls;
+		}
+		ret = snd_ctl_add(card, ctl);
+		if (ret != 0) {
+			printk(KERN_ERR "%s: snd_ctl_add failed\n", __func__);
+			goto err_free_controls;
+		}
+	}
+
+	/* PCM */
+	ret = snd_pcm_new(card, "STFM1000 PCM", 0, 0, 1, &pcm);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: snd_ctl_add failed\n", __func__);
+		goto err_free_controls;
+	}
+	stfm1000->pcm = pcm;
+	pcm->private_data = stfm1000;	/* point back */
+
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE,
+		&stfm1000_snd_capture_ops);
+	pcm->info_flags = 0;
+	strcpy(pcm->name, "STFM1000 PCM");
+
+	snd_card_set_dev(card, &stfm1000->radio.dev);
+	strcpy(card->shortname, "STFM1000");
+
+	ret = snd_pcm_lib_preallocate_pages_for_all(stfm1000->pcm,
+		SNDRV_DMA_TYPE_CONTINUOUS, card->dev, SZ_256K, SZ_256K);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: snd_pcm_lib_preallocate_pages_for_all "
+			"failed\n", __func__);
+		goto err_free_pcm;
+	}
+
+	ret = request_irq(IRQ_DRI_DMA, stfm1000_dri_dma_irq, 0, "stfm1000",
+		stfm1000);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: request_irq failed\n", __func__);
+		goto err_free_prealloc;
+	}
+
+	ret = request_irq(IRQ_DRI_ATTENTION, stfm1000_dri_attn_irq, 0,
+		"stfm1000", stfm1000);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: request_irq failed\n", __func__);
+		goto err_rel_irq;
+	}
+
+	ret = snd_card_register(stfm1000->card);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: snd_card_register failed\n", __func__);
+		goto err_rel_irq2;
+	}
+
+	/* Enable completion interrupt */
+	stmp3xxx_dma_clear_interrupt(stfm1000->dma_ch);
+	stmp3xxx_dma_enable_interrupt(stfm1000->dma_ch);
+
+	printk(KERN_INFO "%s/alsa: %s registered\n", "STFM1000",
+		card->longname);
+
+	return 0;
+
+err_rel_irq2:
+	free_irq(IRQ_DRI_ATTENTION, stfm1000);
+
+err_rel_irq:
+	free_irq(IRQ_DRI_DMA, stfm1000);
+
+err_free_prealloc:
+	snd_pcm_lib_preallocate_free_for_all(stfm1000->pcm);
+
+err_free_pcm:
+	/* XXX TODO */
+
+err_free_controls:
+	/* XXX TODO */
+
+/* err_free_card: */
+	snd_card_free(stfm1000->card);
+
+err_free_dma:
+	for (i = stfm1000->desc_num - 1; i >= 0; i--) {
+		if (stfm1000->dma[i].command != NULL)
+			stmp3xxx_dma_free_command(stfm1000->dma_ch,
+				&stfm1000->dma[i]);
+	}
+
+err_rel_dma:
+	stmp3xxx_dma_release(stfm1000->dma_ch);
+err:
+	return ret;
+}
+
+static void stfm1000_alsa_instance_release(struct stfm1000 *stfm1000)
+{
+	int i;
+
+	stmp3xxx_dma_clear_interrupt(stfm1000->dma_ch);
+	stmp3xxx_arch_dma_reset_channel(stfm1000->dma_ch);
+
+	snd_card_free(stfm1000->card);
+
+	for (i = stfm1000->desc_num - 1; i >= 0; i--)
+		stmp3xxx_dma_free_command(stfm1000->dma_ch, &stfm1000->dma[i]);
+
+	kfree(stfm1000->dma);
+
+	stmp3xxx_dma_release(stfm1000->dma_ch);
+}
+
+static void stfm1000_alsa_dma_irq(struct stfm1000 *stfm1000)
+{
+	struct snd_pcm_runtime *runtime;
+	int desc;
+	s16 *src, *dst;
+
+	if (stfm1000->stopping_recording)
+		return;
+
+	if (stfm1000->read_count >= stfm1000->blksize *
+			(stfm1000->blocks - 2)) {
+		printk(KERN_ERR "irq: overrun %d - Blocks in %d\n",
+			stfm1000->read_count, stfm1000->blocks);
+		return;
+	}
+
+	/* someone has brutally killed user-space */
+	if (stfm1000->substream == NULL ||
+			stfm1000->substream->runtime == NULL)
+		return;
+
+	BUG_ON(stfm1000->substream == NULL);
+	BUG_ON(stfm1000->substream->runtime == NULL);
+
+	desc = stfm1000->read_offset / stfm1000->blksize;
+	runtime = stfm1000->substream->runtime;
+
+	if (runtime->dma_area == NULL)
+		printk(KERN_INFO "runtime->dma_area = NULL\n");
+	BUG_ON(runtime->dma_area == NULL);
+	if (stfm1000->dri_buf == NULL)
+		printk(KERN_INFO "stfm1000->dri_buf = NULL\n");
+	BUG_ON(stfm1000->dri_buf == NULL);
+
+	if (desc >= stfm1000->blocks) {
+		printk(KERN_INFO "desc=%d ->blocks=%d\n",
+				desc, stfm1000->blocks);
+		printk(KERN_INFO "->read_offset=%x ->blksize=%x\n",
+				stfm1000->read_offset, stfm1000->blksize);
+	}
+	BUG_ON(desc >= stfm1000->blocks);
+
+	src = stfm1000->dri_buf + desc * (stfm1000->blksize * 2);
+	dst = (void *)runtime->dma_area + desc * stfm1000->blksize;
+
+	/* perform filtering */
+	stfm1000_decode_block(stfm1000, src, dst, stfm1000->blksize / 4);
+
+	stfm1000->read_count += stfm1000->blksize;
+
+	if (stfm1000->read_count >=
+		snd_pcm_lib_period_bytes(stfm1000->substream))
+		snd_pcm_period_elapsed(stfm1000->substream);
+}
+
+static void stfm1000_alsa_attn_irq(struct stfm1000 *stfm1000)
+{
+	/* nothing */
+}
+
+struct stfm1000_alsa_ops stfm1000_default_alsa_ops = {
+	.init = stfm1000_alsa_instance_init,
+	.release = stfm1000_alsa_instance_release,
+	.dma_irq = stfm1000_alsa_dma_irq,
+	.attn_irq = stfm1000_alsa_attn_irq,
+};
+
+static int stfm1000_alsa_init(void)
+{
+	struct stfm1000 *stfm1000 = NULL;
+	struct list_head *list;
+	int ret;
+
+	stfm1000_alsa_ops = &stfm1000_default_alsa_ops;
+
+	list_for_each(list, &stfm1000_devlist) {
+		stfm1000 = list_entry(list, struct stfm1000, devlist);
+		ret = (*stfm1000_alsa_ops->init)(stfm1000);
+		if (ret != 0) {
+			printk(KERN_ERR "stfm1000 ALSA driver for DMA sound "
+				"failed init.\n");
+			return ret;
+		}
+		stfm1000->alsa_initialized = 1;
+	}
+
+	printk(KERN_INFO "stfm1000 ALSA driver for DMA sound loaded\n");
+
+	return 0;
+}
+
+static void stfm1000_alsa_exit(void)
+{
+	struct stfm1000 *stfm1000 = NULL;
+	struct list_head *list;
+
+	list_for_each(list, &stfm1000_devlist) {
+		stfm1000 = list_entry(list, struct stfm1000, devlist);
+
+		if (!stfm1000->alsa_initialized)
+			continue;
+
+		stfm1000_take_down(stfm1000);
+		(*stfm1000_alsa_ops->release)(stfm1000);
+		stfm1000->alsa_initialized = 0;
+	}
+
+	printk(KERN_INFO "stfm1000 ALSA driver for DMA sound unloaded\n");
+}
+
+/* We initialize this late, to make sure the sound system is up and running */
+late_initcall(stfm1000_alsa_init);
+module_exit(stfm1000_alsa_exit);
+
+MODULE_AUTHOR("Pantelis Antoniou");
+MODULE_DESCRIPTION("An ALSA PCM driver for the STFM1000 chip.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/radio/stfm1000/stfm1000-core.c b/drivers/media/radio/stfm1000/stfm1000-core.c
new file mode 100644
index 000000000000..5086100bb480
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-core.c
@@ -0,0 +1,2459 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/math64.h>
+
+#include <sound/control.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <linux/version.h>	/* for KERNEL_VERSION MACRO */
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/device.h>
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+#include <mach/regs-dri.h>
+#include <mach/regs-apbx.h>
+#include <mach/regs-clkctrl.h>
+
+#include "stfm1000.h"
+
+static DEFINE_MUTEX(devlist_lock);
+static unsigned int stfm1000_devcount;
+
+LIST_HEAD(stfm1000_devlist);
+EXPORT_SYMBOL(stfm1000_devlist);
+
+/* alsa interface */
+struct stfm1000_alsa_ops *stfm1000_alsa_ops;
+EXPORT_SYMBOL(stfm1000_alsa_ops);
+
+/* region, 0=US, 1=europe */
+static int georegion = 1;	/* default is europe */
+static int rds_enable = 1;	/* default is enabled */
+
+static int sw_tune(struct stfm1000 *stfm1000, u32 freq);
+
+static const const char *stfm1000_get_rev_txt(u32 id)
+{
+	switch (id) {
+	case 0x01: return "TA1";
+	case 0x02: return "TA2";
+	case 0x11: return "TB1";
+	case 0x12: return "TB2";
+	}
+	return NULL;
+}
+
+static const struct stfm1000_reg stfm1000_tb2_powerup[] = {
+	STFM1000_REG(REF, 0x00200000),
+	STFM1000_DELAY(20),
+	STFM1000_REG(DATAPATH, 0x00010210),
+	STFM1000_REG(TUNE1, 0x0004CF01),
+	STFM1000_REG(SDNOMINAL, 0x1C5EBCF0),
+	STFM1000_REG(PILOTTRACKING, 0x000001B6),
+	STFM1000_REG(INITIALIZATION1, 0x9fb80008),
+	STFM1000_REG(INITIALIZATION2, 0x8516e444 | STFM1000_DEEMPH_50_75B),
+	STFM1000_REG(INITIALIZATION3, 0x1402190b),
+	STFM1000_REG(INITIALIZATION4, 0x525bf052),
+	STFM1000_REG(INITIALIZATION5, 0x1000d106),
+	STFM1000_REG(INITIALIZATION6, 0x000062cb),
+	STFM1000_REG(AGC_CONTROL1, 0x1BCB2202),
+	STFM1000_REG(AGC_CONTROL2, 0x000020F0),
+	STFM1000_REG(CLK1, 0x10000000),
+	STFM1000_REG(CLK1, 0x20000000),
+	STFM1000_REG(CLK1, 0x00000000),
+	STFM1000_REG(CLK2, 0x7f000000),
+	STFM1000_REG(REF, 0x00B8222D),
+	STFM1000_REG(CLK1, 0x30000000),
+	STFM1000_REG(CLK1, 0x30002000),
+	STFM1000_REG(CLK1, 0x10002000),
+	STFM1000_REG(LNA, 0x0D080009),
+	STFM1000_DELAY(10),
+	STFM1000_REG(MIXFILT, 0x00008000),
+	STFM1000_REG(MIXFILT, 0x00000000),
+	STFM1000_REG(MIXFILT, 0x00007205),
+	STFM1000_REG(ADC, 0x001B3282),
+	STFM1000_REG(ATTENTION, 0x0000003F),
+	STFM1000_END,
+};
+
+static const struct stfm1000_reg stfm1000_ta2_powerup[] = {
+	STFM1000_REG(REF, 0x00200000),
+	STFM1000_DELAY(20),
+	STFM1000_REG(DATAPATH, 0x00010210),
+	STFM1000_REG(TUNE1, 0x00044F01),
+	STFM1000_REG(SDNOMINAL, 0x1C5EBCF0),
+	STFM1000_REG(PILOTTRACKING, 0x000001B6),
+	STFM1000_REG(INITIALIZATION1, 0x9fb80008),
+	STFM1000_REG(INITIALIZATION2, 0x8506e444),
+	STFM1000_REG(INITIALIZATION3, 0x1402190b),
+	STFM1000_REG(INITIALIZATION4, 0x525bf052),
+	STFM1000_REG(INITIALIZATION5, 0x7000d106),
+	STFM1000_REG(INITIALIZATION6, 0x0000c2cb),
+	STFM1000_REG(AGC_CONTROL1, 0x002c8402),
+	STFM1000_REG(AGC_CONTROL2, 0x00140050),
+	STFM1000_REG(CLK1, 0x10000000),
+	STFM1000_REG(CLK1, 0x20000000),
+	STFM1000_REG(CLK1, 0x00000000),
+	STFM1000_REG(CLK2, 0x7f000000),
+	STFM1000_REG(REF, 0x0030222D),
+	STFM1000_REG(CLK1, 0x30000000),
+	STFM1000_REG(CLK1, 0x30002000),
+	STFM1000_REG(CLK1, 0x10002000),
+	STFM1000_REG(LNA, 0x05080009),
+	STFM1000_REG(MIXFILT, 0x00008000),
+	STFM1000_REG(MIXFILT, 0x00000000),
+	STFM1000_REG(MIXFILT, 0x00007200),
+	STFM1000_REG(ADC, 0x00033000),
+	STFM1000_REG(ATTENTION, 0x0000003F),
+	STFM1000_END,
+};
+
+static const struct stfm1000_reg stfm1000_powerdown[] = {
+	STFM1000_REG(DATAPATH, 0x00010210),
+	STFM1000_REG(REF, 0),
+	STFM1000_REG(LNA, 0),
+	STFM1000_REG(MIXFILT, 0),
+	STFM1000_REG(CLK1, 0x20000000),
+	STFM1000_REG(CLK1, 0),
+	STFM1000_REG(CLK2, 0),
+	STFM1000_REG(ADC, 0),
+	STFM1000_REG(TUNE1, 0),
+	STFM1000_REG(SDNOMINAL, 0),
+	STFM1000_REG(PILOTTRACKING, 0),
+	STFM1000_REG(INITIALIZATION1, 0),
+	STFM1000_REG(INITIALIZATION2, 0),
+	STFM1000_REG(INITIALIZATION3, 0),
+	STFM1000_REG(INITIALIZATION4, 0),
+	STFM1000_REG(INITIALIZATION5, 0),
+	STFM1000_REG(INITIALIZATION6, 0x00007E00),
+	STFM1000_REG(AGC_CONTROL1, 0),
+	STFM1000_REG(AGC_CONTROL2, 0),
+	STFM1000_REG(DATAPATH, 0x00000200),
+};
+
+struct stfm1000_tuner_pmi {
+	u32 min;
+	u32 max;
+	u32 freq;
+	u32 pll_xtal;	/* 1 = pll, 0 = xtal */
+};
+
+#define PLL 1
+#define XTAL 0
+
+static const struct stfm1000_tuner_pmi stfm1000_pmi_lookup[] = {
+	{ .min =  76100, .max =  76500, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  79700, .max =  79900, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  80800, .max =  81200, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  82100, .max =  82600, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  86800, .max =  87200, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  88100, .max =  88600, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  89800, .max =  90500, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  91400, .max =  91900, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  92800, .max =  93300, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  97400, .max =  97900, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  98800, .max =  99200, .freq = 19200, .pll_xtal = PLL },
+	{ .min = 100200, .max = 100400, .freq = 19200, .pll_xtal = PLL },
+	{ .min = 103500, .max = 103900, .freq = 19200, .pll_xtal = PLL },
+	{ .min = 104800, .max = 105200, .freq = 19200, .pll_xtal = PLL },
+	{ .min = 106100, .max = 106500, .freq = 19200, .pll_xtal = PLL },
+
+	{ .min =  76600, .max =  77000, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  77800, .max =  78300, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  79200, .max =  79600, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  80600, .max =  80700, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  83900, .max =  84400, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  85300, .max =  85800, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  94200, .max =  94700, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  95600, .max =  96100, .freq = 20000, .pll_xtal = PLL },
+	{ .min = 100500, .max = 100800, .freq = 20000, .pll_xtal = PLL },
+	{ .min = 101800, .max = 102200, .freq = 20000, .pll_xtal = PLL },
+	{ .min = 103100, .max = 103400, .freq = 20000, .pll_xtal = PLL },
+	{ .min = 106600, .max = 106900, .freq = 20000, .pll_xtal = PLL },
+	{ .min = 107800, .max = 108000, .freq = 20000, .pll_xtal = PLL },
+
+	{ .min =      0, .max =      0, .freq = 24000, .pll_xtal = XTAL }
+};
+
+int stfm1000_power_up(struct stfm1000 *stfm1000)
+{
+	struct stfm1000_reg *reg, *pwrup_reg;
+	const struct stfm1000_reg *orig_reg, *treg;
+	int ret, size;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	/* Enable DRI clock for 24Mhz. */
+	HW_CLKCTRL_XTAL_CLR(BM_CLKCTRL_XTAL_DRI_CLK24M_GATE);
+
+	orig_reg = stfm1000->revid == STFM1000_CHIP_REV_TA2 ?
+			stfm1000_ta2_powerup : stfm1000_tb2_powerup;
+
+	/* find size of the set */
+	for (treg = orig_reg; treg->regno != STFM1000_REG_END; treg++)
+		;
+	size = (treg + 1 - orig_reg) * sizeof(*treg);
+
+	/* allocate copy */
+	pwrup_reg = kmalloc(size, GFP_KERNEL);
+	if (pwrup_reg == NULL) {
+		printk(KERN_ERR "%s: out of memory\n", __func__);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* copy it */
+	memcpy(pwrup_reg, orig_reg, size);
+
+	/* fixup region of INITILIZATION2 */
+	for (reg = pwrup_reg; reg->regno != STFM1000_REG_END; reg++) {
+
+		/* we only care for INITIALIZATION2 register */
+		if (reg->regno != STFM1000_INITIALIZATION2)
+			continue;
+
+		/* geographic region select */
+		if (stfm1000->georegion == 0)	/* USA */
+			reg->value &= ~STFM1000_DEEMPH_50_75B;
+		else			/* Europe */
+			reg->value |=  STFM1000_DEEMPH_50_75B;
+
+		/* RDS enabled */
+		if (stfm1000->revid == STFM1000_CHIP_REV_TB2) {
+			if (stfm1000->rds_enable)
+				reg->value |=  STFM1000_RDS_ENABLE;
+			else
+				reg->value &= ~STFM1000_RDS_ENABLE;
+		}
+	}
+
+	ret = stfm1000_write_regs(stfm1000, pwrup_reg);
+
+	kfree(pwrup_reg);
+out:
+	mutex_unlock(&stfm1000->state_lock);
+
+	return ret;
+}
+
+int stfm1000_power_down(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	/* Disable DRI clock for 24Mhz. */
+	HW_CLKCTRL_XTAL_CLR(BM_CLKCTRL_XTAL_DRI_CLK24M_GATE);
+
+	ret = stfm1000_write_regs(stfm1000, stfm1000_powerdown);
+
+	/* Disable DRI clock for 24Mhz. */
+	/* XXX bug warning, disabling the DRI clock is bad news */
+	/* doing so causes noise to be received from the DRI */
+	/* interface. Leave it on for now */
+	/* HW_CLKCTRL_XTAL_CLR(BM_CLKCTRL_XTAL_DRI_CLK24M_GATE); */
+
+	mutex_unlock(&stfm1000->state_lock);
+
+	return ret;
+}
+
+int stfm1000_dcdc_update(struct stfm1000 *stfm1000, u32 freq)
+{
+	const struct stfm1000_tuner_pmi *pmi;
+	int i;
+
+	/* search for DCDC frequency */
+	pmi = stfm1000_pmi_lookup;
+	for (i = 0;  i < ARRAY_SIZE(stfm1000_pmi_lookup); i++, pmi++) {
+		if (freq >= pmi->min && freq <= pmi->max)
+			break;
+	}
+	if (i >= ARRAY_SIZE(stfm1000_pmi_lookup))
+		return -1;
+
+	/* adjust DCDC frequency so that it is out of Tuner PLL range */
+	/* XXX there is no adjustment API (os_pmi_SetDcdcFreq)*/
+	return 0;
+}
+
+static void Mute_Audio(struct stfm1000 *stfm1000)
+{
+	stfm1000->mute = 1;
+}
+
+static void Unmute_Audio(struct stfm1000 *stfm1000)
+{
+	stfm1000->mute = 0;
+}
+
+static const struct stfm1000_reg sd_dp_on_regs[] = {
+	STFM1000_REG_SETBITS(DATAPATH, STFM1000_DP_EN),
+	STFM1000_DELAY(3),
+	STFM1000_REG_SETBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_REG_CLRBITS(AGC_CONTROL1, STFM1000_B2_BYPASS_AGC_CTL),
+	STFM1000_REG_CLRBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_END,
+};
+
+static int SD_DP_On(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	ret = stfm1000_write_regs(stfm1000, sd_dp_on_regs);
+	if (ret != 0)
+		return ret;
+
+	return 0;
+}
+
+static const struct stfm1000_reg sd_dp_off_regs[] = {
+	STFM1000_REG_SETBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_REG_CLRBITS(DATAPATH, STFM1000_DP_EN),
+	STFM1000_REG_SETBITS(AGC_CONTROL1, STFM1000_B2_BYPASS_AGC_CTL),
+	STFM1000_REG_CLRBITS(PILOTTRACKING, STFM1000_B2_PILOTTRACKING_EN),
+	STFM1000_REG_CLRBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_END,
+};
+
+static int SD_DP_Off(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	ret = stfm1000_write_regs(stfm1000, sd_dp_off_regs);
+	if (ret != 0)
+		return ret;
+
+	return 0;
+}
+
+static int DRI_Start_Stream(struct stfm1000 *stfm1000)
+{
+	dma_addr_t dma_buffer_phys;
+	int i, next;
+	u32 cmd;
+
+	/* we must not be gated */
+	BUG_ON(HW_CLKCTRL_XTAL_RD() & BM_CLKCTRL_XTAL_DRI_CLK24M_GATE);
+
+	/* hw_dri_SetReset */
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_SFTRST | BM_DRI_CTRL_CLKGATE);
+	HW_DRI_CTRL_SET(BM_DRI_CTRL_SFTRST);
+	while ((HW_DRI_CTRL_RD() & BM_DRI_CTRL_CLKGATE) == 0)
+		cpu_relax();
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_SFTRST | BM_DRI_CTRL_CLKGATE);
+
+	/* DRI enable/config */
+	HW_DRI_TIMING_WR(BF_DRI_TIMING_GAP_DETECTION_INTERVAL(0x10) |
+			 BF_DRI_TIMING_PILOT_REP_RATE(0x08));
+
+	/* XXX SDK bug */
+	/* While the SDK enables the gate here, everytime the stream */
+	/* is started, doing so, causes the DRI to input audio noise */
+	/* at any subsequent starts */
+	/* Enable DRI clock for 24Mhz. */
+	/* HW_CLKCTRL_XTAL_CLR(BM_CLKCTRL_XTAL_DRI_CLK24M_GATE); */
+
+	stmp3xxx_arch_dma_reset_channel(stfm1000->dma_ch);
+
+	dma_buffer_phys = stfm1000->dri_phys;
+
+	for (i = 0; i < stfm1000->blocks; i++) {
+		next = (i + 1) % stfm1000->blocks;
+
+		/* link */
+		stfm1000->dma[i].command->next = stfm1000->dma[next].handle;
+		stfm1000->dma[i].next_descr = &stfm1000->dma[next];
+
+		/* receive DRI is 8 bytes per 4 samples */
+		cmd = BF_APBX_CHn_CMD_XFER_COUNT(stfm1000->blksize * 2) |
+		      BM_APBX_CHn_CMD_IRQONCMPLT |
+		      BM_APBX_CHn_CMD_CHAIN |
+		      BF_APBX_CHn_CMD_COMMAND(
+			BV_APBX_CHn_CMD_COMMAND__DMA_WRITE);
+
+		stfm1000->dma[i].command->cmd = cmd;
+		stfm1000->dma[i].command->buf_ptr = dma_buffer_phys;
+		stfm1000->dma[i].command->pio_words[0] =
+			BM_DRI_CTRL_OVERFLOW_IRQ_EN |
+			BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ_EN |
+			BM_DRI_CTRL_ATTENTION_IRQ_EN |
+			/* BM_DRI_CTRL_STOP_ON_OFLOW_ERROR | */
+			/* BM_DRI_CTRL_STOP_ON_PILOT_ERROR | */
+			BM_DRI_CTRL_ENABLE_INPUTS;
+
+		dma_buffer_phys += stfm1000->blksize * 2;
+
+	}
+
+	/* Enable completion interrupt */
+	stmp3xxx_dma_clear_interrupt(stfm1000->dma_ch);
+	stmp3xxx_dma_enable_interrupt(stfm1000->dma_ch);
+
+	/* clear DRI interrupts pending */
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_OVERFLOW_IRQ |
+			BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ |
+			BM_DRI_CTRL_ATTENTION_IRQ);
+
+	/* Stop DRI on error */
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_STOP_ON_OFLOW_ERROR |
+			BM_DRI_CTRL_STOP_ON_PILOT_ERROR);
+
+	/* Reacquire data stream */
+	HW_DRI_CTRL_SET(BM_DRI_CTRL_REACQUIRE_PHASE |
+		BM_DRI_CTRL_OVERFLOW_IRQ_EN |
+		BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ_EN |
+		BM_DRI_CTRL_ATTENTION_IRQ_EN |
+		BM_DRI_CTRL_ENABLE_INPUTS);
+
+	stmp3xxx_dma_go(stfm1000->dma_ch, stfm1000->dma, 1);
+
+	/* Turn on DRI hardware (don't forget to leave RUN bit ON) */
+	HW_DRI_CTRL_SET(BM_DRI_CTRL_RUN);
+
+	return 0;
+}
+
+static int DRI_Stop_Stream(struct stfm1000 *stfm1000)
+{
+	int desc;
+
+	/* disable interrupts */
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_OVERFLOW_IRQ_EN |
+			BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ_EN |
+			BM_DRI_CTRL_ATTENTION_IRQ_EN);
+
+	/* Freeze DMA channel for a moment */
+	stmp3xxx_dma_freeze(stfm1000->dma_ch);
+
+	/* all descriptors, set sema bit */
+	for (desc = 0; desc < stfm1000->blocks; desc++)
+		stfm1000->dma[desc].command->cmd |= BM_APBX_CHn_CMD_SEMAPHORE;
+
+	/* Let the current DMA transaction finish */
+	stmp3xxx_dma_unfreeze(stfm1000->dma_ch);
+	msleep(5);
+
+	/* dma shutdown */
+	stmp3xxx_arch_dma_reset_channel(stfm1000->dma_ch);
+
+	/* Turn OFF data lines and stop controller */
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_ENABLE_INPUTS | BM_DRI_CTRL_RUN);
+
+	/* hw_dri_SetReset */
+	HW_DRI_CTRL_SET(BM_DRI_CTRL_SFTRST | BM_DRI_CTRL_CLKGATE);
+
+	/* XXX SDK bug */
+	/* While the SDK enables the gate here, everytime the stream */
+	/* is started, doing so, causes the DRI to input audio noise */
+	/* at any subsequent starts */
+	/* Enable DRI clock for 24Mhz. */
+	/* Disable DRI clock for 24Mhz. */
+	/* HW_CLKCTRL_XTAL_SET(BM_CLKCTRL_XTAL_DRI_CLK24M_GATE); */
+
+	return 0;
+}
+
+static int DRI_On(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	if (stfm1000->active)
+		DRI_Start_Stream(stfm1000);
+
+	ret = stfm1000_set_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_SAI_EN);
+	return ret;
+}
+
+static int DRI_Off(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	if (stfm1000->active)
+		DRI_Stop_Stream(stfm1000);
+
+	ret = stfm1000_clear_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_SAI_EN);
+
+	return 0;
+}
+
+static int SD_Set_Channel_Filter(struct stfm1000 *stfm1000)
+{
+	int bypass_setting;
+	int sig_qual;
+	u32 tmp;
+	int ret;
+
+	/*
+	 * set channel filter
+	 *
+	 * B2_NEAR_CHAN_MIX_REG_MASK values from T-Spec
+	 * 000 : 0 kHz mix.
+	 * 001 : +100 kHz mix.
+	 * 010 : +200 kHz mix.
+	 * 011 : +300 kHz mix.
+	 * 100 : -400 kHz mix.
+	 * 101 : -300 kHz mix.
+	 * 110 : -200 kHz mix.
+	 * 111 : -100 kHz mix.
+	 */
+
+	/* get near channel amplitude */
+	ret = stfm1000_write_masked(stfm1000, STFM1000_INITIALIZATION3,
+		STFM1000_B2_NEAR_CHAN_MIX(0x01),
+		STFM1000_B2_NEAR_CHAN_MIX_MASK);
+	if (ret != 0)
+		return ret;
+
+	msleep(10);	/* wait for the signal quality to settle */
+
+	ret = stfm1000_read(stfm1000, STFM1000_SIGNALQUALITY, &tmp);
+	if (ret != 0)
+		return ret;
+
+	sig_qual = (tmp & STFM1000_NEAR_CHAN_AMPLITUDE_MASK) >>
+		STFM1000_NEAR_CHAN_AMPLITUDE_SHIFT;
+
+	bypass_setting = 0;
+
+	/* check near channel amplitude vs threshold */
+	if (sig_qual < stfm1000->adj_chan_th) {
+		/* get near channel amplitude again */
+		ret = stfm1000_write_masked(stfm1000, STFM1000_INITIALIZATION3,
+			STFM1000_B2_NEAR_CHAN_MIX(0x05),
+			STFM1000_B2_NEAR_CHAN_MIX_MASK);
+		if (ret != 0)
+			return ret;
+
+		msleep(10);	/* wait for the signal quality to settle */
+
+		ret = stfm1000_read(stfm1000, STFM1000_SIGNALQUALITY, &tmp);
+		if (ret != 0)
+			return ret;
+
+		sig_qual = (tmp & STFM1000_NEAR_CHAN_AMPLITUDE_MASK) >>
+			STFM1000_NEAR_CHAN_AMPLITUDE_SHIFT;
+
+		if (sig_qual < stfm1000->adj_chan_th)
+			bypass_setting = 2;
+	}
+
+	/* set filter settings */
+	ret = stfm1000_write_masked(stfm1000, STFM1000_INITIALIZATION1,
+		STFM1000_B2_BYPASS_FILT(bypass_setting),
+		STFM1000_B2_BYPASS_FILT_MASK);
+	if (ret != 0)
+		return ret;
+
+	return 0;
+}
+
+static int SD_Look_For_Pilot_TA2(struct stfm1000 *stfm1000)
+{
+	int i;
+	u32 pilot;
+	int ret;
+
+	/* assume pilot */
+	stfm1000->pilot_present = 1;
+
+	for (i = 0; i < 3; i++) {
+
+		ret = stfm1000_read(stfm1000, STFM1000_PILOTCORRECTION,
+			&pilot);
+		if (ret != 0)
+			return ret;
+
+		pilot &= STFM1000_PILOTEST_TA2_MASK;
+		pilot >>= STFM1000_PILOTEST_TA2_SHIFT;
+
+		/* out of range? */
+		if (pilot < 0xe2 || pilot >= 0xb5) {
+			stfm1000->pilot_present = 0;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+
+static int SD_Look_For_Pilot_TB2(struct stfm1000 *stfm1000)
+{
+	int i;
+	u32 pilot;
+	int ret;
+
+	/* assume pilot */
+	stfm1000->pilot_present = 1;
+
+	for (i = 0; i < 3; i++) {
+
+		ret = stfm1000_read(stfm1000, STFM1000_PILOTCORRECTION,
+			&pilot);
+		if (ret != 0)
+			return ret;
+
+		pilot &= STFM1000_PILOTEST_TB2_MASK;
+		pilot >>= STFM1000_PILOTEST_TB2_SHIFT;
+
+		/* out of range? */
+		if (pilot < 0x1e || pilot >= 0x7f) {
+			stfm1000->pilot_present = 0;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int SD_Look_For_Pilot(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	if (stfm1000->revid == STFM1000_CHIP_REV_TA2)
+		ret = SD_Look_For_Pilot_TA2(stfm1000);
+	else
+		ret = SD_Look_For_Pilot_TB2(stfm1000);
+
+	if (ret != 0)
+		return ret;
+
+	if (!stfm1000->pilot_present) {
+		ret = stfm1000_clear_bits(stfm1000, STFM1000_PILOTTRACKING,
+			STFM1000_B2_PILOTTRACKING_EN);
+		if (ret != 0)
+			return ret;
+
+		/* set force mono parameters for the filter */
+		stfm1000->filter_parms.pCoefForcedMono = 1;
+
+		/* yeah, I know, it's stupid */
+		stfm1000->rds_state.demod.pCoefForcedMono =
+			stfm1000->filter_parms.pCoefForcedMono;
+	}
+
+	return 0;
+}
+
+static int SD_Gear_Shift_Pilot_Tracking(struct stfm1000 *stfm1000)
+{
+	static const struct {
+		int delay;
+		u32 value;
+	} track_table[] = {
+		{ .delay = 10, .value = 0x81b6 },
+		{ .delay =  6, .value = 0x82a5 },
+		{ .delay =  6, .value = 0x8395 },
+		{ .delay =  8, .value = 0x8474 },
+		{ .delay = 20, .value = 0x8535 },
+		{ .delay = 50, .value = 0x8632 },
+		{ .delay =  0, .value = 0x8810 },
+	};
+	int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(track_table); i++) {
+		ret = stfm1000_write(stfm1000, STFM1000_PILOTTRACKING,
+			track_table[i].value);
+		if (ret != 0)
+			return ret;
+
+		if (i < ARRAY_SIZE(track_table) - 1)	/* last one no delay */
+			msleep(track_table[i].delay);
+	}
+
+	return 0;
+}
+
+static int SD_Optimize_Channel(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	ret = stfm1000_set_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		return ret;
+
+	ret = stfm1000_write(stfm1000, STFM1000_PILOTTRACKING,
+		STFM1000_B2_PILOTTRACKING_EN |
+		STFM1000_B2_PILOTLPF_TIMECONSTANT(0x01) |
+		STFM1000_B2_PFDSCALE(0x0B) |
+		STFM1000_B2_PFDFILTER_SPEEDUP(0x06));	/* 0x000081B6 */
+	if (ret != 0)
+		return ret;
+
+	ret = SD_Set_Channel_Filter(stfm1000);
+	if (ret != 0)
+		return ret;
+
+	ret = SD_Look_For_Pilot(stfm1000);
+	if (ret != 0)
+		return ret;
+
+	if (stfm1000->pilot_present) {
+		ret = SD_Gear_Shift_Pilot_Tracking(stfm1000);
+		if (ret != 0)
+			return ret;
+	}
+
+	ret = stfm1000_clear_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		return ret;
+
+	return 0;
+}
+
+static int Monitor_STFM_Quality(struct stfm1000 *stfm1000)
+{
+	u32 tmp, rssi_dc_est, tone_data;
+	u32 lna_rms, bias, agc_out, lna_th, lna, ref;
+	u16 rssi_mantissa, rssi_exponent, rssi_decoded;
+	u16 prssi;
+	s16 mpx_dc;
+	int rssi_log;
+	int bypass_filter;
+	int ret;
+
+	ret = stfm1000_set_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		return ret;
+
+	/* Get Rssi register readings from STFM1000 */
+	stfm1000_read(stfm1000, STFM1000_RSSI_TONE, &tmp);
+	rssi_dc_est = tmp & 0xffff;
+	tone_data = (tmp >> 16) & 0x0fff;
+
+	rssi_mantissa = (rssi_dc_est & 0xffe0) >> 5;	/* 11Msb */
+	rssi_exponent = rssi_dc_est & 0x001f;	/* 5 lsb */
+	rssi_decoded = (u32)rssi_mantissa << rssi_exponent;
+
+	/* Convert Rsst to 10log(Rssi) */
+	for (prssi = 20; prssi > 0; prssi--)
+		if (rssi_decoded >= (1 << prssi))
+			break;
+
+	rssi_log = (3 * rssi_decoded >> prssi) + (3 * prssi - 3);
+	/* clamp to positive */
+	if (rssi_log < 0)
+		rssi_log = 0;
+	/* Compensate for errors in truncation/approximation by adding 1 */
+	rssi_log++;
+
+	stfm1000->rssi_dc_est_log = rssi_log;
+	stfm1000->signal_strength = stfm1000->rssi_dc_est_log;
+
+	/* determine absolute value */
+	if (tmp & 0x0800)
+		mpx_dc = ((tmp >> 16) & 0x0fff) | 0xf000;
+	else
+		mpx_dc = (tmp >> 16) & 0x0fff;
+	stfm1000->mpx_dc = mpx_dc;
+	mpx_dc = mpx_dc < 0 ? -mpx_dc : mpx_dc;
+
+	if (stfm1000->tuning_grid_50KHz)
+		stfm1000->is_station = rssi_log > stfm1000->tune_rssi_th;
+	else
+		stfm1000->is_station = rssi_log > stfm1000->tune_rssi_th &&
+			mpx_dc > stfm1000->tune_mpx_dc_th;
+
+	/* weak signal? */
+	if (stfm1000->rssi_dc_est_log <
+		(stfm1000->filter_parms.pCoefLmrGaTh - 20)) {
+
+		if (stfm1000->pilot_present)
+			bypass_filter = 1;	/* Filter settings #2 */
+		else
+			bypass_filter = 0;
+
+		/* configure filter for narrow band */
+		ret = stfm1000_write_masked(stfm1000, STFM1000_AGC_CONTROL1,
+				STFM1000_B2_BYPASS_FILT(bypass_filter),
+				STFM1000_B2_BYPASS_FILT_MASK);
+		if (ret != 0)
+			return ret;
+
+		/* Turn off pilot tracking */
+		ret = stfm1000_clear_bits(stfm1000, STFM1000_PILOTTRACKING,
+				STFM1000_B2_PILOTTRACKING_EN);
+		if (ret != 0)
+			return ret;
+
+		/* enable "forced mono" in black box */
+		stfm1000->filter_parms.pCoefForcedMono = 1;
+
+		/* yeah, I know, it's stupid */
+		stfm1000->rds_state.demod.pCoefForcedMono =
+			stfm1000->filter_parms.pCoefForcedMono;
+
+		/* Set weak signal flag */
+		stfm1000->weak_signal = 1;
+
+		if (stfm1000->revid == STFM1000_CHIP_REV_TA2) {
+
+			/* read AGC_STAT register */
+			ret = stfm1000_read(stfm1000, STFM1000_AGC_STAT, &tmp);
+			if (ret != 0)
+				return ret;
+
+			lna_rms = (tmp & STFM1000_LNA_RMS_MASK) >>
+				STFM1000_LNA_RMS_SHIFT;
+
+			/* Check  the energy level from LNA Power Meter A/D */
+			if (lna_rms == 0)
+				bias = STFM1000_IBIAS2_DN | STFM1000_IBIAS1_UP;
+			else
+				bias = STFM1000_IBIAS2_UP | STFM1000_IBIAS1_DN;
+
+			if (lna_rms == 0 || lna_rms > 2) {
+				ret = stfm1000_write_masked(stfm1000,
+					STFM1000_LNA, bias,
+					STFM1000_IBIAS2_UP |
+					STFM1000_IBIAS2_DN |
+					STFM1000_IBIAS1_UP |
+					STFM1000_IBIAS1_DN);
+				if (ret != 0)
+					return ret;
+			}
+
+		} else {
+
+			/* Set LNA bias */
+
+			/* read AGC_STAT register */
+			ret = stfm1000_read(stfm1000, STFM1000_AGC_STAT, &tmp);
+			if (ret != 0)
+				return ret;
+
+			agc_out = (tmp & STFM1000_AGCOUT_STAT_MASK) >>
+					STFM1000_AGCOUT_STAT_SHIFT;
+
+			/* read LNA register (this is a cached register) */
+			ret = stfm1000_read(stfm1000, STFM1000_LNA, &lna);
+			if (ret != 0)
+				return ret;
+
+			/* read REF register (this is a cached register) */
+			ret = stfm1000_read(stfm1000, STFM1000_REF, &ref);
+			if (ret != 0)
+				return ret;
+
+/* work around the 80 line width problem */
+#undef LNADEF
+#define LNADEF STFM1000_LNA_AMP1_IMPROVE_DISTORTION
+			if (agc_out == 31) {
+				if (rssi_log <= 16) {
+					if (lna & STFM1000_IBIAS1_DN)
+						lna &= ~STFM1000_IBIAS1_DN;
+					else {
+						lna |=  STFM1000_IBIAS1_UP;
+						ref &= ~LNADEF;
+					}
+				}
+				if (rssi_log >= 26) {
+					if (lna & STFM1000_IBIAS1_UP) {
+						lna &= ~STFM1000_IBIAS1_UP;
+						ref |=  LNADEF;
+					} else
+						lna |=  STFM1000_IBIAS1_DN;
+				}
+			} else {
+				lna &= ~STFM1000_IBIAS1_UP;
+				lna |=  STFM1000_IBIAS1_DN;
+				ref |=  LNADEF;
+			}
+#undef LNADEF
+
+			ret = stfm1000_write_masked(stfm1000, STFM1000_LNA,
+				lna, STFM1000_IBIAS1_UP | STFM1000_IBIAS1_DN);
+			if (ret != 0)
+				return ret;
+
+			ret = stfm1000_write_masked(stfm1000, STFM1000_REF,
+				ref, STFM1000_LNA_AMP1_IMPROVE_DISTORTION);
+			if (ret != 0)
+				return ret;
+		}
+
+	} else if (stfm1000->rssi_dc_est_log >
+		(stfm1000->filter_parms.pCoefLmrGaTh - 17)) {
+
+			bias = STFM1000_IBIAS2_UP | STFM1000_IBIAS1_DN;
+
+			ret = stfm1000_write_masked(stfm1000, STFM1000_LNA,
+				bias, STFM1000_IBIAS2_UP | STFM1000_IBIAS2_DN |
+				STFM1000_IBIAS1_UP | STFM1000_IBIAS1_DN);
+			if (ret != 0)
+				return ret;
+
+			ret = SD_Set_Channel_Filter(stfm1000);
+			if (ret != 0)
+				return ret;
+
+			ret = SD_Look_For_Pilot(stfm1000);
+			if (ret != 0)
+				return ret;
+
+			if (stfm1000->pilot_present) {
+				if (stfm1000->prev_pilot_present ||
+					stfm1000->weak_signal) {
+
+					/* gear shift pilot tracking */
+					ret = SD_Gear_Shift_Pilot_Tracking(
+						stfm1000);
+					if (ret != 0)
+						return ret;
+
+					/* set force mono parameters for the
+					 * filter */
+					stfm1000->filter_parms.
+						pCoefForcedMono = stfm1000->
+							force_mono;
+
+					/* yeah, I know, it's stupid */
+					stfm1000->rds_state.demod.
+						pCoefForcedMono = stfm1000->
+							filter_parms.
+							pCoefForcedMono;
+				}
+			} else {
+				ret = stfm1000_clear_bits(stfm1000,
+					STFM1000_PILOTTRACKING,
+					STFM1000_B2_PILOTTRACKING_EN);
+				if (ret != 0)
+					return ret;
+
+				/* set force mono parameters for the filter */
+				stfm1000->filter_parms.pCoefForcedMono = 1;
+
+				/* yeah, I know, it's stupid */
+				stfm1000->rds_state.demod.pCoefForcedMono =
+					stfm1000->filter_parms.pCoefForcedMono;
+			}
+
+			/* Reset weak signal flag */
+			stfm1000->weak_signal = 0;
+			stfm1000->prev_pilot_present = stfm1000->pilot_present;
+
+	} else {
+
+		ret = SD_Look_For_Pilot(stfm1000);
+		if (ret != 0)
+			return ret;
+
+		if (!stfm1000->pilot_present) {
+			ret = stfm1000_clear_bits(stfm1000,
+				STFM1000_PILOTTRACKING,
+				STFM1000_B2_PILOTTRACKING_EN);
+			if (ret != 0)
+				return ret;
+
+			/* set force mono parameters for the filter */
+			stfm1000->filter_parms.pCoefForcedMono = 1;
+
+			/* yeah, I know, it's stupid */
+			stfm1000->rds_state.demod.pCoefForcedMono =
+				stfm1000->filter_parms.pCoefForcedMono;
+
+			/* Reset weak signal flag */
+			stfm1000->weak_signal = 0;
+			stfm1000->prev_pilot_present = stfm1000->pilot_present;
+		}
+
+	}
+
+	if (stfm1000->revid == STFM1000_CHIP_REV_TA2) {
+
+		/* read AGC_STAT register */
+		ret = stfm1000_read(stfm1000, STFM1000_AGC_STAT, &tmp);
+		if (ret != 0)
+			return ret;
+
+		agc_out = (tmp & STFM1000_AGCOUT_STAT_MASK) >>
+			STFM1000_AGCOUT_STAT_SHIFT;
+		lna_rms = (tmp & STFM1000_LNA_RMS_MASK) >>
+			STFM1000_LNA_RMS_SHIFT;
+
+		ret = stfm1000_read(stfm1000, STFM1000_AGC_CONTROL1, &tmp);
+		if (ret != 0)
+			return ret;
+
+		/* extract LNATH */
+		lna_th = (tmp & STFM1000_B2_LNATH_MASK) >>
+			STFM1000_B2_LNATH_SHIFT;
+
+		if (lna_rms > lna_th && agc_out <= 1) {
+
+			ret = stfm1000_write_masked(stfm1000, STFM1000_LNA,
+				STFM1000_USEATTEN(1), STFM1000_USEATTEN_MASK);
+			if (ret != 0)
+				return ret;
+
+		} else if (agc_out > 15) {
+
+			ret = stfm1000_write_masked(stfm1000, STFM1000_LNA,
+				STFM1000_USEATTEN(0), STFM1000_USEATTEN_MASK);
+			if (ret != 0)
+				return ret;
+		}
+	}
+
+	/* disable buffered writes */
+	ret = stfm1000_clear_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		return ret;
+
+	return ret;
+}
+
+static int Is_Station(struct stfm1000 *stfm1000)
+{
+	u32 tmp, rssi_dc_est, tone_data;
+	u16 rssi_mantissa, rssi_exponent, rssi_decoded;
+	u16 prssi;
+	s16 mpx_dc;
+	int rssi_log;
+
+	/* Get Rssi register readings from STFM1000 */
+	stfm1000_read(stfm1000, STFM1000_RSSI_TONE, &tmp);
+	rssi_dc_est = tmp & 0xffff;
+	tone_data = (tmp >> 16) & 0x0fff;
+
+	rssi_mantissa = (rssi_dc_est & 0xffe0) >> 5;	/* 11Msb */
+	rssi_exponent = rssi_dc_est & 0x001f;	/* 5 lsb */
+	rssi_decoded = (u32)rssi_mantissa << rssi_exponent;
+
+	/* Convert Rsst to 10log(Rssi) */
+	for (prssi = 20; prssi > 0; prssi--)
+		if (rssi_decoded >= (1 << prssi))
+			break;
+
+	rssi_log = (3 * rssi_decoded >> prssi) + (3 * prssi - 3);
+	/* clamp to positive */
+	if (rssi_log < 0)
+		rssi_log = 0;
+	/* Compensate for errors in truncation/approximation by adding 1 */
+	rssi_log++;
+
+	stfm1000->rssi_dc_est_log = rssi_log;
+	stfm1000->signal_strength = stfm1000->rssi_dc_est_log;
+
+	/* determine absolute value */
+	if (tmp & 0x0800)
+		mpx_dc = ((tmp >> 16) & 0x0fff) | 0xf000;
+	else
+		mpx_dc = (tmp >> 16) & 0x0fff;
+	stfm1000->mpx_dc = mpx_dc;
+	mpx_dc = mpx_dc < 0 ? -mpx_dc : mpx_dc;
+
+	if (stfm1000->tuning_grid_50KHz)
+		stfm1000->is_station = rssi_log > stfm1000->tune_rssi_th;
+	else
+		stfm1000->is_station = rssi_log > stfm1000->tune_rssi_th &&
+			mpx_dc > stfm1000->tune_mpx_dc_th;
+
+	return 0;
+}
+
+int Monitor_STFM_AGC(struct stfm1000 *stfm1000)
+{
+	/* we don't do any AGC for now */
+	return 0;
+}
+
+static int Take_Down(struct stfm1000 *stfm1000)
+{
+	Mute_Audio(stfm1000);
+
+	DRI_Off(stfm1000);
+
+	SD_DP_Off(stfm1000);
+
+	return 0;
+}
+
+static int Bring_Up(struct stfm1000 *stfm1000)
+{
+	SD_DP_On(stfm1000);
+
+	SD_Optimize_Channel(stfm1000);
+
+	DRI_On(stfm1000);
+
+	Unmute_Audio(stfm1000);
+
+	if (stfm1000->rds_enable)
+		stfm1000_rds_reset(&stfm1000->rds_state);
+
+	stfm1000->rds_sync = stfm1000->rds_enable; /* force sync (if RDS) */
+	stfm1000->rds_demod_running = 0;
+	stfm1000->rssi_dc_est_log = 0;
+	stfm1000->signal_strength = 0;
+
+	stfm1000->next_quality_monitor = jiffies + msecs_to_jiffies(
+		stfm1000->quality_monitor_period);
+	stfm1000->next_agc_monitor = jiffies + msecs_to_jiffies(
+		stfm1000->agc_monitor_period);
+	stfm1000->rds_pkt_bad = 0;
+	stfm1000->rds_pkt_good = 0;
+	stfm1000->rds_pkt_recovered = 0;
+	stfm1000->rds_pkt_lost_sync = 0;
+	stfm1000->rds_bit_overruns = 0;
+
+	return 0;
+}
+
+/* These are not used yet */
+
+static int Lock_Station(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	ret = SD_Optimize_Channel(stfm1000);
+	if (ret != 0)
+		return ret;
+
+	/* AGC monitor start? */
+
+	return ret;
+}
+
+static const struct stfm1000_reg sd_unlock_regs[] = {
+	STFM1000_REG_SETBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_REG_CLRBITS(PILOTTRACKING, STFM1000_B2_PILOTTRACKING_EN),
+	STFM1000_REG_CLRBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_END,
+};
+
+static int Unlock_Station(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	ret = stfm1000_write_regs(stfm1000, sd_unlock_regs);
+	return ret;
+}
+
+irqreturn_t stfm1000_dri_dma_irq(int irq, void *dev_id)
+{
+	struct stfm1000 *stfm1000 = dev_id;
+	u32 err_mask, irq_mask;
+	u32 ctrl;
+	int handled = 0;
+
+#ifdef CONFIG_ARCH_STMP37XX
+	err_mask = 1 << (16 + stfm1000->dma_ch);
+#endif
+#ifdef CONFIG_ARCH_STMP378X
+	err_mask = 1 << stfm1000->dma_ch;
+#endif
+	irq_mask = 1 << stfm1000->dma_ch;
+
+#ifdef CONFIG_ARCH_STMP37XX
+	ctrl = HW_APBX_CTRL1_RD();
+#endif
+#ifdef CONFIG_ARCH_STMP378X
+	ctrl = HW_APBX_CTRL2_RD();
+#endif
+
+	if (ctrl & err_mask) {
+		handled = 1;
+		printk(KERN_WARNING "%s: DMA audio channel %d error\n",
+			__func__, stfm1000->dma_ch);
+#ifdef CONFIG_ARCH_STMP37XX
+		HW_APBX_CTRL1_CLR(err_mask);
+#endif
+#ifdef CONFIG_ARCH_STMP378X
+		HW_APBX_CTRL2_CLR(err_mask);
+#endif
+	}
+
+	if (HW_APBX_CTRL1_RD() & irq_mask) {
+		handled = 1;
+		stmp3xxx_dma_clear_interrupt(stfm1000->dma_ch);
+
+		if (stfm1000->alsa_initialized) {
+			BUG_ON(stfm1000_alsa_ops->dma_irq == NULL);
+			(*stfm1000_alsa_ops->dma_irq)(stfm1000);
+		}
+	}
+
+	return handled ? IRQ_HANDLED : IRQ_NONE;
+}
+EXPORT_SYMBOL(stfm1000_dri_dma_irq);
+
+irqreturn_t stfm1000_dri_attn_irq(int irq, void *dev_id)
+{
+	struct stfm1000 *stfm1000 = dev_id;
+	int handled = 1;
+	u32 mask;
+
+	(void)stfm1000;
+	mask = HW_DRI_CTRL_RD();
+	mask &= BM_DRI_CTRL_OVERFLOW_IRQ | BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ |
+		BM_DRI_CTRL_ATTENTION_IRQ;
+
+	HW_DRI_CTRL_CLR(mask);
+
+	printk(KERN_INFO "DRI_ATTN:%s%s%s\n",
+			(mask & BM_DRI_CTRL_OVERFLOW_IRQ) ? " OV" : "",
+			(mask & BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ) ? " SL" : "",
+			(mask & BM_DRI_CTRL_ATTENTION_IRQ) ? " AT" : "");
+
+	if (stfm1000->alsa_initialized) {
+		BUG_ON(stfm1000_alsa_ops->attn_irq == NULL);
+		(*stfm1000_alsa_ops->attn_irq)(stfm1000);
+	}
+
+	return handled ? IRQ_HANDLED : IRQ_NONE;
+}
+EXPORT_SYMBOL(stfm1000_dri_attn_irq);
+
+void stfm1000_decode_block(struct stfm1000 *stfm1000, const s16 *src, s16 *dst,
+	int count)
+{
+	int i;
+
+	if (stfm1000->mute) {
+		memset(dst, 0, count * sizeof(s16) * 2);
+		return;
+
+	}
+
+	for (i = 0; i < count; i++, dst += 2, src += 4) {
+
+		stfm1000_filter_decode(&stfm1000->filter_parms,
+			src[0],	src[1], src[2]);
+
+		dst[0] = stfm1000_filter_value_left(&stfm1000->filter_parms);
+		dst[1] = stfm1000_filter_value_right(&stfm1000->filter_parms);
+	}
+
+	stfm1000->rssi = stfm1000->filter_parms.RssiDecoded;
+	stfm1000->stereo = stfm1000->pilot_present &&
+		!stfm1000->filter_parms.pCoefForcedMono;
+
+	/* RDS processing */
+	if (stfm1000->rds_demod_running) {
+		/* rewind */
+		src -= count * 4;
+		stfm1000_rds_demod(&stfm1000->rds_state, src, count);
+	}
+
+}
+EXPORT_SYMBOL(stfm1000_decode_block);
+
+void stfm1000_take_down(struct stfm1000 *stfm1000)
+{
+	mutex_lock(&stfm1000->state_lock);
+	stfm1000->active = 0;
+	Take_Down(stfm1000);
+	mutex_unlock(&stfm1000->state_lock);
+}
+EXPORT_SYMBOL(stfm1000_take_down);
+
+void stfm1000_bring_up(struct stfm1000 *stfm1000)
+{
+	mutex_lock(&stfm1000->state_lock);
+
+	stfm1000->active = 1;
+
+	stfm1000_filter_reset(&stfm1000->filter_parms);
+
+	Bring_Up(stfm1000);
+
+	mutex_unlock(&stfm1000->state_lock);
+}
+EXPORT_SYMBOL(stfm1000_bring_up);
+
+void stfm1000_tune_current(struct stfm1000 *stfm1000)
+{
+	mutex_lock(&stfm1000->state_lock);
+	sw_tune(stfm1000, stfm1000->freq);
+	mutex_unlock(&stfm1000->state_lock);
+}
+EXPORT_SYMBOL(stfm1000_tune_current);
+
+/* Alternate ZIF Tunings to avoid EMI */
+const struct stfm1000_tune1
+stfm1000_board_emi_tuneups[STFM1000_FREQUENCY_100KHZ_RANGE] = {
+#undef TUNE_ENTRY
+#define TUNE_ENTRY(f, t1, sd) \
+	[(f) - STFM1000_FREQUENCY_100KHZ_MIN] = \
+		{ .tune1 = (t1), .sdnom = (sd) }
+	TUNE_ENTRY(765, 0x84030, 0x1BF5E50D),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(780, 0x84240, 0x1BA5162F),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(795, 0x84250, 0x1C2D2F39),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(810, 0x84460, 0x1BDD207E),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(825, 0x84470, 0x1C6138CD),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(839, 0xC4680, 0x1C11F704),	/* 061215 Jon, IF +100kHz */
+	TUNE_ENTRY(840, 0x84680, 0x1c11f704),
+	TUNE_ENTRY(855, 0x84890, 0x1BC71C71),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(870, 0x848A0, 0x1C43DE10),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(885, 0x84AB0, 0x1BF9B021),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(899, 0xC4CC0, 0x1BB369A9),	/* 061025 Arthur, IF +100kHz */
+	TUNE_ENTRY(900, 0x84CC0, 0x1BB369A9),	/* 061025 Arthur, IF 0kHz */
+	TUNE_ENTRY(915, 0x84CD0, 0x1C299A5B),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(930, 0x84ee0, 0x1be3e6aa),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(945, 0x84ef0, 0x1c570f8b),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(959, 0xC5100, 0x1c11f704),
+	TUNE_ENTRY(960, 0x85100, 0x1c11f704),
+	TUNE_ENTRY(975, 0x85310, 0x1bd03d57),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(990, 0x85320, 0x1c3dc822),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(1005, 0x85530, 0x1bfc93ff),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(1019, 0xC5740, 0x1BBE683C),	/* 061025 Arthur, IF +100kHz */
+	TUNE_ENTRY(1020, 0x85740, 0x1bbe683c),	/* 061025 Arthur, IF +0kHz */
+	TUNE_ENTRY(1035, 0x85750, 0x1c26dab6),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(1050, 0x85960, 0x1be922b4),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(1065, 0x85970, 0x1c4f357c),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(1079, 0xC5B80, 0x1c11f704),
+	TUNE_ENTRY(1080, 0x85B80, 0x1c11f704),
+#undef TUNE_ENTRY
+};
+
+static const struct stfm1000_tune1 *stfm1000_board_emi_tune(int freq100)
+{
+	const struct stfm1000_tune1 *tune1;
+
+	if ((unsigned int)(freq100 - STFM1000_FREQUENCY_100KHZ_MIN) >=
+			STFM1000_FREQUENCY_100KHZ_RANGE)
+		return NULL;
+
+	tune1 = &stfm1000_board_emi_tuneups[freq100 -
+		STFM1000_FREQUENCY_100KHZ_MIN];
+	if (tune1->tune1 == 0 && tune1->sdnom == 0)
+		return NULL;
+	return tune1;
+}
+
+/* freq in kHz */
+static int sw_tune(struct stfm1000 *stfm1000, u32 freq)
+{
+	u32 freq100 = freq / 100;
+	int tune_cap;
+	int i2s_clock;
+	int mix_reg;
+	int if_freq, fe_freq;
+	u32 tune1, sdnom, agc1;
+	const struct stfm1000_tune1 *tp;
+	int ret;
+
+	if_freq = 0;
+	mix_reg = 1;
+	switch (mix_reg) {
+	case 0: if_freq = -2; break;
+	case 1: if_freq = -1; break;
+	case 2: if_freq =  0; break;
+	case 3: if_freq =  1; break;
+	case 4: if_freq =  2; break;
+	}
+
+	/* handle board specific EMI tuning */
+	tp = stfm1000_board_emi_tune(freq100);
+	if (tp != NULL) {
+		tune1 = tp->tune1;
+		sdnom = tp->sdnom;
+	} else {
+		fe_freq = freq100 + if_freq;
+
+		/* clamp into range */
+		if (fe_freq < STFM1000_FREQUENCY_100KHZ_MIN)
+			fe_freq = STFM1000_FREQUENCY_100KHZ_MIN;
+		else if (fe_freq > STFM1000_FREQUENCY_100KHZ_MAX)
+			fe_freq = STFM1000_FREQUENCY_100KHZ_MAX;
+
+		tp = &stfm1000_tune1_table[fe_freq -
+			STFM1000_FREQUENCY_100KHZ_MIN];
+
+		/* bits [14:0], [20:18] */
+		tune1 = (tp->tune1 & 0x7fff) | (mix_reg << 18);
+		sdnom = tp->sdnom;
+	}
+
+	agc1 = stfm1000->revid == STFM1000_CHIP_REV_TA2 ? 0x0400 : 0x2200;
+
+	ret = stfm1000_write_masked(stfm1000, STFM1000_AGC_CONTROL1,
+			agc1, 0x3f00);
+	if (ret != 0)
+		goto err;
+
+	ret = stfm1000_write_masked(stfm1000, STFM1000_TUNE1, tune1,
+		0xFFFF7FFF);	/* do not set bit-15 */
+	if (ret != 0)
+		goto err;
+
+	/* keep this around */
+	stfm1000->sdnominal_pivot = sdnom;
+
+	ret = stfm1000_write(stfm1000, STFM1000_SDNOMINAL, sdnom);
+	if (ret != 0)
+		goto err;
+
+	/* fix for seek-not-stopping on alternate tunings */
+	ret = stfm1000_set_bits(stfm1000, STFM1000_DATAPATH,
+			STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		goto err;
+
+	ret = stfm1000_clear_bits(stfm1000, STFM1000_DATAPATH,
+			STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		goto err;
+
+	ret = stfm1000_set_bits(stfm1000, STFM1000_INITIALIZATION2,
+			STFM1000_DRI_CLK_EN);
+	if (ret != 0)
+		goto err;
+
+	/* 6MHz spur fix */
+	if ((freq100 >=  778 && freq100 <=  782) ||
+	    (freq100 >=  838 && freq100 <=  842) ||
+	    (freq100 >=  898 && freq100 <=  902) ||
+	    (freq100 >=  958 && freq100 <=  962) ||
+	    (freq100 >= 1018 && freq100 <= 1022) ||
+	    (freq100 >= 1078 && freq100 <= 1080))
+		i2s_clock = 5;  /* 4.8MHz */
+	else
+		i2s_clock = 4;
+
+	ret = stfm1000_write_masked(stfm1000, STFM1000_DATAPATH,
+		STFM1000_SAI_CLK_DIV(i2s_clock), STFM1000_SAI_CLK_DIV_MASK);
+	if (ret != 0)
+		goto err;
+
+	ret = stfm1000_set_bits(stfm1000, STFM1000_INITIALIZATION2,
+		STFM1000_DRI_CLK_EN);
+	if (ret != 0)
+		goto err;
+
+	if (tune1 & 0xf)
+		ret = stfm1000_set_bits(stfm1000, STFM1000_CLK1,
+			STFM1000_ENABLE_TAPDELAYFIX);
+	else
+		ret = stfm1000_clear_bits(stfm1000, STFM1000_CLK1,
+			STFM1000_ENABLE_TAPDELAYFIX);
+
+	if (ret != 0)
+		goto err;
+
+	tune_cap = (int)(stfm1000->tune_cap_a_f -
+		stfm1000->tune_cap_b_f * freq100);
+	if (tune_cap < 4)
+		tune_cap = 4;
+	ret = stfm1000_write_masked(stfm1000, STFM1000_LNA,
+		STFM1000_ANTENNA_TUNECAP(tune_cap),
+		STFM1000_ANTENNA_TUNECAP_MASK);
+	if (ret != 0)
+		goto err;
+
+	/* set signal strenth to 0 */
+	/* stfm1000_dcdc_update(); */
+
+	/* cmp_rds_setRdsStatus(0) */
+	/* cmp_rds_ResetGroupCallbacks(); */
+	stfm1000->freq = freq;
+
+	return 0;
+err:
+	return -1;
+}
+
+static const struct v4l2_queryctrl radio_qctrl[] = {
+	{
+		.id		= V4L2_CID_AUDIO_MUTE,
+		.name		= "Mute",
+		.minimum	= 0,
+		.maximum	= 1,
+		.default_value	= 1,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+	},
+};
+
+static int vidioc_querycap(struct file *file, void *priv,
+				struct v4l2_capability *v)
+{
+	strlcpy(v->driver, "radio-stfm1000", sizeof(v->driver));
+	strlcpy(v->card, "STFM1000 Radio", sizeof(v->card));
+	sprintf(v->bus_info, "i2c");
+	v->version = KERNEL_VERSION(0, 0, 1);
+	v->capabilities = V4L2_CAP_TUNER;
+	return 0;
+}
+
+static int vidioc_g_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *v)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+	u32 tmp, rssi_dc_est, tone_data;
+	u16 rssi_mantissa, rssi_exponent, rssi_decoded;
+	u16 prssi;
+	s16 mpx_dc;
+	int rssi_log;
+	int ret;
+
+	if (v->index > 0)
+		return -EINVAL;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	strcpy(v->name, "FM");
+	v->type = V4L2_TUNER_RADIO;
+	v->rangelow = (u32)(87.5 * 16000);
+	v->rangehigh = (u32)(108 * 16000);
+	v->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO;
+	v->capability = V4L2_TUNER_CAP_LOW;
+	v->audmode = V4L2_TUNER_MODE_STEREO;
+	v->signal = 0; /* tr_getsigstr(); */
+
+	msleep(50);
+
+	ret = stfm1000_read(stfm1000, STFM1000_RSSI_TONE, &tmp);
+	if (ret != 0)
+		goto out;
+
+	rssi_dc_est = tmp & 0xffff;
+	tone_data = (tmp >> 16) & 0x0fff;
+
+	rssi_mantissa = (rssi_dc_est & 0xffe0) >> 5;	/* 11Msb */
+	rssi_exponent = rssi_dc_est & 0x001f;	/* 5 lsb */
+	rssi_decoded = (u32)rssi_mantissa << rssi_exponent;
+
+	/* Convert Rsst to 10log(Rssi) */
+	for (prssi = 20; prssi > 0; prssi--)
+		if (rssi_decoded >= (1 << prssi))
+			break;
+
+	rssi_log = (3 * rssi_decoded >> prssi) + (3 * prssi - 3);
+	/* clamp to positive */
+	if (rssi_log < 0)
+		rssi_log = 0;
+	/* Compensate for errors in truncation/approximation by adding 1 */
+	rssi_log++;
+
+	stfm1000->rssi_dc_est_log = rssi_log;
+	stfm1000->signal_strength = stfm1000->rssi_dc_est_log;
+
+	/* determine absolute value */
+	if (tmp & 0x0800)
+		mpx_dc = ((tmp >> 16) & 0x0fff) | 0xf000;
+	else
+		mpx_dc = (tmp >> 16) & 0x0fff;
+	stfm1000->mpx_dc = mpx_dc;
+	mpx_dc = mpx_dc < 0 ? -mpx_dc : mpx_dc;
+
+	v->signal = rssi_decoded & 0xffff;
+
+out:
+	mutex_unlock(&stfm1000->state_lock);
+
+	return ret;
+}
+
+static int vidioc_s_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *v)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	(void)stfm1000;
+
+	if (v->index > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int vidioc_s_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	mutex_lock(&stfm1000->state_lock);
+
+	/* convert from the crazy linux value to our decimal based values */
+	stfm1000->freq = (u32)div_u64((u64)(125 * (u64)f->frequency), 2000);
+
+	if (stfm1000->active)
+		Take_Down(stfm1000);
+
+	sw_tune(stfm1000, stfm1000->freq);
+
+	if (stfm1000->active)
+		Bring_Up(stfm1000);
+
+	mutex_unlock(&stfm1000->state_lock);
+
+	return 0;
+}
+
+static int vidioc_g_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	f->type = V4L2_TUNER_RADIO;
+	f->frequency = stfm1000->freq * 16;
+
+	return 0;
+}
+
+static int vidioc_queryctrl(struct file *file, void *priv,
+				struct v4l2_queryctrl *qc)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+	int i;
+
+	(void)stfm1000;
+
+	for (i = 0; i < ARRAY_SIZE(radio_qctrl); i++) {
+		if (qc->id && qc->id == radio_qctrl[i].id) {
+			memcpy(qc, &radio_qctrl[i], sizeof(*qc));
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static int vidioc_g_ctrl(struct file *file, void *priv,
+				struct v4l2_control *ctrl)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	switch (ctrl->id) {
+
+	case V4L2_CID_AUDIO_MUTE:
+		ctrl->value = stfm1000->mute;
+		return 0;
+
+	}
+	return -EINVAL;
+}
+
+static int vidioc_s_ctrl(struct file *file, void *priv,
+				struct v4l2_control *ctrl)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+	int ret;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	ret = -EINVAL;
+
+	switch (ctrl->id) {
+
+	case V4L2_CID_AUDIO_MUTE:
+		stfm1000->mute = ctrl->value;
+		ret = 0;
+		break;
+	}
+
+	mutex_unlock(&stfm1000->state_lock);
+
+	return ret;
+}
+
+static int vidioc_g_audio(struct file *file, void *priv,
+				struct v4l2_audio *a)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	(void)stfm1000;
+
+	if (a->index > 1)
+		return -EINVAL;
+
+	strcpy(a->name, "Radio");
+	a->capability = V4L2_AUDCAP_STEREO;
+	return 0;
+}
+
+static int vidioc_s_audio(struct file *file, void *priv,
+					struct v4l2_audio *a)
+{
+	if (a->index > 1)
+		return -EINVAL;
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	(void)stfm1000;
+
+	*i = 0;
+
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	(void)stfm1000;
+
+	if (i != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+const struct v4l2_ioctl_ops stfm_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+	.vidioc_g_tuner		= vidioc_g_tuner,
+	.vidioc_s_tuner		= vidioc_s_tuner,
+	.vidioc_g_frequency	= vidioc_g_frequency,
+	.vidioc_s_frequency	= vidioc_s_frequency,
+	.vidioc_queryctrl	= vidioc_queryctrl,
+	.vidioc_g_ctrl		= vidioc_g_ctrl,
+	.vidioc_s_ctrl		= vidioc_s_ctrl,
+	.vidioc_g_audio		= vidioc_g_audio,
+	.vidioc_s_audio		= vidioc_s_audio,
+	.vidioc_g_input		= vidioc_g_input,
+	.vidioc_s_input		= vidioc_s_input,
+};
+
+static int stfm1000_open(struct inode *inode, struct file *file)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	mutex_lock(&stfm1000->state_lock);
+	stfm1000->users = 1;
+	mutex_unlock(&stfm1000->state_lock);
+
+	return 0;
+}
+static int stfm1000_close(struct inode *inode, struct file *file)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	if (!stfm1000)
+		return -ENODEV;
+
+	stfm1000->users = 0;
+	if (stfm1000->removed)
+		kfree(stfm1000);
+	return 0;
+}
+
+static const struct file_operations stfm1000_fops = {
+	.owner		= THIS_MODULE,
+	.open		= stfm1000_open,
+	.release	= stfm1000_close,
+	.ioctl		= video_ioctl2,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl	= v4l_compat_ioctl32,
+#endif
+	.llseek		= no_llseek,
+};
+
+/* sysfs */
+
+#define STFM1000_RO_ATTR(var) \
+static ssize_t stfm1000_show_ ## var(struct device *d, \
+	struct device_attribute *attr, char *buf) \
+{ \
+	struct i2c_client *client = to_i2c_client(d); \
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client); \
+	return sprintf(buf, "%d\n", stfm1000->var); \
+} \
+static DEVICE_ATTR(var, 0444, stfm1000_show_ ##var, NULL)
+
+#define STFM1000_RW_ATTR(var) \
+static ssize_t stfm1000_show_ ## var(struct device *d, \
+	struct device_attribute *attr, char *buf) \
+{ \
+	struct i2c_client *client = to_i2c_client(d); \
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client); \
+	return sprintf(buf, "%u\n", stfm1000->var); \
+} \
+static ssize_t stfm1000_store_ ## var(struct device *d, \
+	struct device_attribute *attr, const char *buf, size_t size) \
+{ \
+	struct i2c_client *client = to_i2c_client(d); \
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client); \
+	unsigned long v; \
+	\
+	strict_strtoul(buf, 0, &v); \
+	stfm1000_commit_ ## var(stfm1000, v); \
+	return size; \
+} \
+static DEVICE_ATTR(var, 0644, stfm1000_show_ ##var, stfm1000_store_ ##var)
+
+#define STFM1000_RW_ATTR_SIMPLE(var) \
+static void stfm1000_commit_ ## var(struct stfm1000 *stfm1000, \
+	unsigned long value) \
+{ \
+	stfm1000->var = value; \
+} \
+STFM1000_RW_ATTR(var)
+
+STFM1000_RO_ATTR(weak_signal);
+STFM1000_RO_ATTR(pilot_present);
+STFM1000_RO_ATTR(stereo);
+STFM1000_RO_ATTR(rssi);
+STFM1000_RO_ATTR(mpx_dc);
+STFM1000_RO_ATTR(signal_strength);
+STFM1000_RW_ATTR_SIMPLE(rds_signal_th);
+STFM1000_RO_ATTR(rds_present);
+STFM1000_RO_ATTR(is_station);
+
+static void stfm1000_commit_georegion(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	/* don't do anything for illegal region */
+	if (value != 0 && value != 1)
+		return;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	stfm1000->georegion = value;
+	if (stfm1000->georegion == 0)
+		stfm1000_clear_bits(stfm1000, STFM1000_INITIALIZATION2,
+			STFM1000_DEEMPH_50_75B);
+	else
+		stfm1000_set_bits(stfm1000, STFM1000_INITIALIZATION2,
+			STFM1000_DEEMPH_50_75B);
+
+	mutex_unlock(&stfm1000->state_lock);
+}
+STFM1000_RW_ATTR(georegion);
+
+static void stfm1000_commit_freq(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	mutex_lock(&stfm1000->state_lock);
+
+	/* clamp */
+	if (value < STFM1000_FREQUENCY_100KHZ_MIN * 100)
+		value = STFM1000_FREQUENCY_100KHZ_MIN * 100;
+	else if (value > STFM1000_FREQUENCY_100KHZ_MAX * 100)
+		value = STFM1000_FREQUENCY_100KHZ_MAX * 100;
+
+	stfm1000->freq = value;
+
+	if (stfm1000->active)
+		Take_Down(stfm1000);
+
+	sw_tune(stfm1000, stfm1000->freq);
+
+	if (stfm1000->active)
+		Bring_Up(stfm1000);
+
+	mutex_unlock(&stfm1000->state_lock);
+}
+STFM1000_RW_ATTR(freq);
+
+static void stfm1000_commit_mute(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	stfm1000->mute = !!value;
+}
+STFM1000_RW_ATTR(mute);
+
+static void stfm1000_commit_force_mono(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	stfm1000->force_mono = !!value;
+	/* set force mono parameters for the filter */
+	stfm1000->filter_parms.pCoefForcedMono = stfm1000->force_mono;
+
+	/* yeah, I know, it's stupid */
+	stfm1000->rds_state.demod.pCoefForcedMono =
+		stfm1000->filter_parms.pCoefForcedMono;
+}
+STFM1000_RW_ATTR(force_mono);
+
+STFM1000_RW_ATTR_SIMPLE(monitor_period);
+STFM1000_RW_ATTR_SIMPLE(quality_monitor);
+STFM1000_RW_ATTR_SIMPLE(quality_monitor_period);
+STFM1000_RW_ATTR_SIMPLE(agc_monitor_period);
+STFM1000_RW_ATTR_SIMPLE(tune_rssi_th);
+STFM1000_RW_ATTR_SIMPLE(tune_mpx_dc_th);
+
+static void stfm1000_commit_rds_enable(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	/* don't do anything for illegal values (or for not TB2) */
+	if ((value != 0 && value != 1) ||
+		stfm1000->revid == STFM1000_CHIP_REV_TA2)
+		return;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	stfm1000->rds_enable = value;
+	if (stfm1000->rds_enable == 0)
+		stfm1000_clear_bits(stfm1000, STFM1000_INITIALIZATION2,
+			STFM1000_RDS_ENABLE);
+	else
+		stfm1000_set_bits(stfm1000, STFM1000_INITIALIZATION2,
+			STFM1000_RDS_ENABLE);
+
+	mutex_unlock(&stfm1000->state_lock);
+}
+STFM1000_RW_ATTR(rds_enable);
+
+static void stfm1000_commit_rds_sync(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	stfm1000->rds_sync = stfm1000->rds_enable && !!value;
+}
+STFM1000_RW_ATTR(rds_sync);
+
+STFM1000_RW_ATTR_SIMPLE(rds_pkt_good);
+STFM1000_RW_ATTR_SIMPLE(rds_pkt_bad);
+STFM1000_RW_ATTR_SIMPLE(rds_pkt_recovered);
+STFM1000_RW_ATTR_SIMPLE(rds_pkt_lost_sync);
+STFM1000_RW_ATTR_SIMPLE(rds_bit_overruns);
+STFM1000_RW_ATTR_SIMPLE(rds_info);
+
+static void stfm1000_commit_rds_sdnominal_adapt(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	stfm1000->rds_sdnominal_adapt = !!value;
+	stfm1000->rds_state.demod.sdnom_adapt = stfm1000->rds_sdnominal_adapt;
+}
+STFM1000_RW_ATTR(rds_sdnominal_adapt);
+
+static void stfm1000_commit_rds_phase_pop(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	stfm1000->rds_phase_pop = !!value;
+	stfm1000->rds_state.demod.PhasePoppingEnabled =
+		stfm1000->rds_phase_pop;
+}
+STFM1000_RW_ATTR(rds_phase_pop);
+
+STFM1000_RW_ATTR_SIMPLE(tuning_grid_50KHz);
+
+static ssize_t stfm1000_show_rds_ps(struct device *d,
+	struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(d);
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client);
+	char ps[9];
+
+	if (stfm1000_rds_get_ps(&stfm1000->rds_state, ps, sizeof(ps)) <= 0)
+		ps[0] = '\0';
+
+	return sprintf(buf, "%s\n", ps);
+}
+static DEVICE_ATTR(rds_ps, 0444, stfm1000_show_rds_ps, NULL);
+
+static ssize_t stfm1000_show_rds_text(struct device *d,
+	struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(d);
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client);
+	char text[65];
+
+	if (stfm1000_rds_get_text(&stfm1000->rds_state, text,
+		sizeof(text)) <= 0)
+		text[0] = '\0';
+
+	return sprintf(buf, "%s\n", text);
+}
+static DEVICE_ATTR(rds_text, 0444, stfm1000_show_rds_text, NULL);
+
+static struct device_attribute *stfm1000_attrs[] = {
+	&dev_attr_agc_monitor_period,
+	&dev_attr_force_mono,
+	&dev_attr_freq,
+	&dev_attr_georegion,
+	&dev_attr_is_station,
+	&dev_attr_monitor_period,
+	&dev_attr_mpx_dc,
+	&dev_attr_mute,
+	&dev_attr_pilot_present,
+	&dev_attr_quality_monitor,
+	&dev_attr_quality_monitor_period,
+	&dev_attr_rds_bit_overruns,
+	&dev_attr_rds_enable,
+	&dev_attr_rds_info,
+	&dev_attr_rds_phase_pop,
+	&dev_attr_rds_pkt_bad,
+	&dev_attr_rds_pkt_good,
+	&dev_attr_rds_pkt_lost_sync,
+	&dev_attr_rds_pkt_recovered,
+	&dev_attr_rds_present,
+	&dev_attr_rds_ps,
+	&dev_attr_rds_sdnominal_adapt,
+	&dev_attr_rds_signal_th,
+	&dev_attr_rds_sync,
+	&dev_attr_rds_text,
+	&dev_attr_rssi,
+	&dev_attr_signal_strength,
+	&dev_attr_stereo,
+	&dev_attr_tune_mpx_dc_th,
+	&dev_attr_tune_rssi_th,
+	&dev_attr_tuning_grid_50KHz,
+	&dev_attr_weak_signal,
+	NULL,
+};
+
+/* monitor thread */
+
+static void rds_process(struct stfm1000 *stfm1000)
+{
+	int count, bit;
+	int mix_reg, sdnominal_reg;
+	u32 sdnom, sdnom_new, limit;
+	u8 buf[8];
+
+	if (!stfm1000->rds_enable)
+		return;
+
+	if (stfm1000->rds_sync &&
+		stfm1000->rssi_dc_est_log > stfm1000->rds_signal_th) {
+		if (stfm1000->rds_info)
+			printk(KERN_INFO "RDS: sync\n");
+		stfm1000_rds_reset(&stfm1000->rds_state);
+		stfm1000->rds_demod_running = 1;
+		stfm1000->rds_sync = 0;
+	}
+
+	if (!stfm1000->rds_demod_running)
+		return;
+
+	/* process mix reg requests */
+	spin_lock_irq(&stfm1000->rds_lock);
+	mix_reg = stfm1000_rds_mix_msg_get(&stfm1000->rds_state);
+	spin_unlock_irq(&stfm1000->rds_lock);
+
+	if (mix_reg != -1) {
+
+		if (stfm1000->rds_info)
+			printk(KERN_INFO "RDS: new RDS_MIXOFFSET %d\n",
+				mix_reg & 1);
+
+		/* update register */
+		if (mix_reg & 1)
+			stfm1000_set_bits(stfm1000, STFM1000_INITIALIZATION2,
+				STFM1000_RDS_MIXOFFSET);
+		else
+			stfm1000_clear_bits(stfm1000, STFM1000_INITIALIZATION2,
+				STFM1000_RDS_MIXOFFSET);
+
+		/* signal it's processed */
+		spin_lock_irq(&stfm1000->rds_lock);
+		stfm1000_rds_mix_msg_processed(&stfm1000->rds_state, mix_reg);
+		spin_unlock_irq(&stfm1000->rds_lock);
+	}
+
+	/* process sdnominal reg requests */
+	spin_lock_irq(&stfm1000->rds_lock);
+	sdnominal_reg = stfm1000_rds_sdnominal_msg_get(&stfm1000->rds_state);
+	spin_unlock_irq(&stfm1000->rds_lock);
+
+	/* any change? */
+	if (sdnominal_reg != 0) {
+
+		stfm1000_read(stfm1000, STFM1000_SDNOMINAL, &sdnom);
+
+		sdnom_new = sdnom + sdnominal_reg;
+
+		/* Limit SDNOMINAL to within 244 ppm of its ideal value */
+		limit = stfm1000->sdnominal_pivot +
+			(stfm1000->sdnominal_pivot >> 12);
+		if (sdnom_new > limit)
+			sdnom_new = limit;
+
+		limit = stfm1000->sdnominal_pivot -
+			(stfm1000->sdnominal_pivot >> 12);
+		if (sdnom_new < limit)
+			sdnom_new = limit;
+
+		/* write the register */
+		stfm1000_write(stfm1000, STFM1000_SDNOMINAL, sdnom_new);
+
+		/* signal it's processed */
+		spin_lock_irq(&stfm1000->rds_lock);
+		stfm1000_rds_sdnominal_msg_processed(&stfm1000->rds_state,
+			sdnominal_reg);
+		spin_unlock_irq(&stfm1000->rds_lock);
+	}
+
+	/* pump bits out & pass them to the process function */
+	spin_lock_irq(&stfm1000->rds_lock);
+	while (stfm1000_rds_bits_available(&stfm1000->rds_state) > 128) {
+		count = 0;
+		while (count++ < 128 &&
+			(bit = stmf1000_rds_get_bit(
+				&stfm1000->rds_state)) >= 0) {
+			spin_unlock_irq(&stfm1000->rds_lock);
+
+			/* push bit for packet processing */
+			stfm1000_rds_packet_bit(&stfm1000->rds_state, bit);
+
+			spin_lock_irq(&stfm1000->rds_lock);
+		}
+	}
+	spin_unlock_irq(&stfm1000->rds_lock);
+
+	/* now we're free to process non-interrupt related work */
+	while (stfm1000_rds_packet_dequeue(&stfm1000->rds_state, buf) == 0) {
+
+		if (stfm1000->rds_info)
+			printk(KERN_INFO "RDS-PKT: %02x %02x %02x %02x "
+					"%02x %02x %02x %02x\n",
+				buf[0], buf[1], buf[2], buf[3],
+				buf[4], buf[5], buf[6], buf[7]);
+
+		stfm1000_rds_process_packet(&stfm1000->rds_state, buf);
+	}
+
+	/* update our own counters */
+	stfm1000->rds_pkt_good += stfm1000->rds_state.pkt.good_packets;
+	stfm1000->rds_pkt_bad += stfm1000->rds_state.pkt.bad_packets;
+	stfm1000->rds_pkt_recovered +=
+		stfm1000->rds_state.pkt.recovered_packets;
+	stfm1000->rds_pkt_lost_sync +=
+		stfm1000->rds_state.pkt.sync_lost_packets;
+	stfm1000->rds_bit_overruns +=
+		stfm1000->rds_state.demod.RdsDemodSkippedBitCnt;
+
+	/* zero them now */
+	stfm1000->rds_state.pkt.good_packets = 0;
+	stfm1000->rds_state.pkt.bad_packets = 0;
+	stfm1000->rds_state.pkt.recovered_packets = 0;
+	stfm1000->rds_state.pkt.sync_lost_packets = 0;
+	stfm1000->rds_state.demod.RdsDemodSkippedBitCnt = 0;
+
+	/* reset requested from RDS handler? */
+	if (stfm1000_rds_get_reset_req(&stfm1000->rds_state)) {
+		if (stfm1000->rds_info)
+			printk(KERN_INFO "RDS: reset requested\n");
+		stfm1000_rds_reset(&stfm1000->rds_state);
+
+		stfm1000->rds_sync = stfm1000->rds_enable; /* force sync (if RDS) */
+		stfm1000->rds_demod_running = 0;
+		stfm1000->rssi_dc_est_log = 0;
+		stfm1000->signal_strength = 0;
+	}
+}
+
+void stfm1000_monitor_signal(struct stfm1000 *stfm1000, int bit)
+{
+	set_bit(bit, &stfm1000->thread_events);
+	return wake_up_interruptible(&stfm1000->thread_wait);
+}
+
+static int stfm1000_monitor_thread(void *data)
+{
+	struct stfm1000 *stfm1000 = data;
+	int ret;
+
+	printk(KERN_INFO "stfm1000: monitor thread started\n");
+
+	set_freezable();
+
+	/* Hmm, linux becomes *very* unhappy without this ... */
+	while (!kthread_should_stop()) {
+
+		ret = wait_event_interruptible_timeout(stfm1000->thread_wait,
+				stfm1000->thread_events == 0,
+				msecs_to_jiffies(stfm1000->monitor_period));
+
+		stfm1000->thread_events = 0;
+
+		if (kthread_should_stop())
+			break;
+
+		try_to_freeze();
+
+		mutex_lock(&stfm1000->state_lock);
+
+		/* we must be active */
+		if (!stfm1000->active)
+			goto next;
+
+		if (stfm1000->rds_enable)
+			rds_process(stfm1000);
+
+		/* perform quality monitor */
+		if (time_after_eq(jiffies, stfm1000->next_quality_monitor)) {
+
+			/* full quality monitor? */
+			if (stfm1000->quality_monitor)
+				Monitor_STFM_Quality(stfm1000);
+			else	/* simple */
+				Is_Station(stfm1000);
+
+			while (time_after_eq(jiffies,
+					stfm1000->next_quality_monitor))
+				stfm1000->next_quality_monitor +=
+					msecs_to_jiffies(
+					stfm1000->quality_monitor_period);
+		}
+
+		/* perform AGC monitor (if enabled) */
+		if (stfm1000->agc_monitor && time_after_eq(jiffies,
+				stfm1000->next_agc_monitor)) {
+			Monitor_STFM_AGC(stfm1000);
+			while (time_after_eq(jiffies,
+					stfm1000->next_agc_monitor))
+				stfm1000->next_agc_monitor +=
+					msecs_to_jiffies(
+						stfm1000->agc_monitor_period);
+		}
+next:
+		mutex_unlock(&stfm1000->state_lock);
+	}
+
+	printk(KERN_INFO "stfm1000: monitor thread stopped\n");
+
+	return 0;
+}
+
+static u64 stfm1000_dma_mask = DMA_32BIT_MASK;
+
+static int stfm1000_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct device *dev;
+	struct stfm1000 *stfm1000;
+	struct video_device *vd;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	int ret;
+	u32 id;
+	const char *idtxt;
+	int i;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_warn(&adapter->dev,
+			"I2C doesn't support I2C_FUNC_SMBUS_BYTE_DATA\n");
+		return -EIO;
+	}
+
+	/* make sure the dma masks are set correctly */
+	dev = &client->dev;
+	if (!dev->dma_mask)
+		dev->dma_mask = &stfm1000_dma_mask;
+	if (!dev->coherent_dma_mask)
+		dev->coherent_dma_mask = DMA_32BIT_MASK;
+
+	stfm1000 = kzalloc(sizeof(*stfm1000), GFP_KERNEL);
+	if (!stfm1000)
+		return -ENOMEM;
+
+	stfm1000->client = client;
+	i2c_set_clientdata(client, stfm1000);
+
+	mutex_init(&stfm1000->xfer_lock);
+	mutex_init(&stfm1000->state_lock);
+
+	vd = &stfm1000->radio;
+
+	strcpy(vd->name, "stfm1000");
+	vd->vfl_type		= VID_TYPE_TUNER;
+	vd->fops		= &stfm1000_fops;
+	vd->ioctl_ops		= &stfm_ioctl_ops;
+
+	/* vd->debug = V4L2_DEBUG_IOCTL | V4L2_DEBUG_IOCTL_ARG; */
+
+	vd->parent = &client->dev;
+
+	ret = video_register_device(vd, VFL_TYPE_RADIO, -1);
+	if (ret != 0) {
+		dev_warn(&adapter->dev,
+			 "Cannot register radio device\n");
+		goto out;
+	}
+
+	spin_lock_init(&stfm1000->rds_lock);
+
+	stfm1000_setup_reg_set(stfm1000);
+
+	/* stfm1000->dbgflg |= STFM1000_DBGFLG_I2C; */
+
+	ret = stfm1000_read(stfm1000, STFM1000_CHIPID, &id);
+	if (ret < 0) {
+		dev_warn(&adapter->dev,
+			 "Cannot read ID register\n");
+		goto out;
+	}
+	stfm1000->revid = id & 0xff;
+
+	/* NOTE: the tables are precalculated */
+	stfm1000->tune_rssi_th = 28;
+	stfm1000->tune_mpx_dc_th = 300;
+	stfm1000->adj_chan_th = 100;
+	stfm1000->pilot_est_th = 25;
+	stfm1000->agc_monitor = 0;	/* AGC monitor disabled */
+	stfm1000->quality_monitor = 1;
+	stfm1000->weak_signal = 0;
+	stfm1000->prev_pilot_present = 0;
+	stfm1000->tune_cap_a_f = (u32)(72.4 * 65536);
+	stfm1000->tune_cap_b_f = (u32)(0.07 * 65536);
+
+	/* only TB2 supports RDS */
+	stfm1000->rds_enable = stfm1000->revid == STFM1000_CHIP_REV_TB2 &&
+		rds_enable;
+	stfm1000->rds_present = 0;
+	stfm1000->rds_signal_th = 33;
+
+	stfm1000->freq = 92600;
+
+	stfm1000->georegion = georegion;
+	stfm1000->rssi = 0;
+	stfm1000->stereo = 0;
+	stfm1000->force_mono = 0;
+	stfm1000->monitor_period = 100;
+	stfm1000->quality_monitor_period = 1000;
+	stfm1000->agc_monitor_period = 200;
+
+	stfm1000->rds_sdnominal_adapt = 0;
+	stfm1000->rds_phase_pop = 1;
+
+	/* enable info about RDS */
+	stfm1000->rds_info = 0;
+
+	ret = stfm1000_power_up(stfm1000);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: stfm1000_power_up failed\n",
+				__func__);
+		goto out;
+	}
+
+	if (stfm1000_alsa_ops && stfm1000_alsa_ops->init) {
+		ret = (*stfm1000_alsa_ops->init)(stfm1000);
+		if (ret != 0)
+			goto out;
+		stfm1000->alsa_initialized = 1;
+	}
+
+	ret = 0;
+	for (i = 0; stfm1000_attrs[i]; i++) {
+		ret = device_create_file(dev, stfm1000_attrs[i]);
+		if (ret)
+			break;
+	}
+	if (ret) {
+		while (--i >= 0)
+			device_remove_file(dev, stfm1000_attrs[i]);
+		goto out;
+	}
+
+	/* add it to the list */
+	mutex_lock(&devlist_lock);
+	stfm1000->idx = stfm1000_devcount++;
+	list_add_tail(&stfm1000->devlist, &stfm1000_devlist);
+	mutex_unlock(&devlist_lock);
+
+	init_waitqueue_head(&stfm1000->thread_wait);
+	stfm1000->thread = kthread_run(stfm1000_monitor_thread, stfm1000,
+			"stfm1000-%d", stfm1000->idx);
+	if (stfm1000->thread == NULL) {
+		printk(KERN_ERR "stfm1000: kthread_run failed\n");
+		goto out;
+	}
+
+	idtxt = stfm1000_get_rev_txt(stfm1000->revid);
+	if (idtxt == NULL)
+		printk(KERN_INFO "STFM1000: Loaded for unknown revision id "
+			"0x%02x\n", stfm1000->revid);
+	else
+		printk(KERN_INFO "STFM1000: Loaded for revision %s\n", idtxt);
+
+	return 0;
+
+out:
+	kfree(stfm1000);
+	return ret;
+}
+
+static int stfm1000_remove(struct i2c_client *client)
+{
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client);
+	struct device *dev = &client->dev;
+	int i;
+
+	kthread_stop(stfm1000->thread);
+
+	for (i = 0; stfm1000_attrs[i]; i++)
+		device_remove_file(dev, stfm1000_attrs[i]);
+
+	if (stfm1000->alsa_initialized) {
+		BUG_ON(stfm1000_alsa_ops->release == NULL);
+		(*stfm1000_alsa_ops->release)(stfm1000);
+		stfm1000->alsa_initialized = 0;
+	}
+
+	stfm1000_power_down(stfm1000);
+
+	video_unregister_device(&stfm1000->radio);
+
+	mutex_lock(&devlist_lock);
+	list_del(&stfm1000->devlist);
+	mutex_unlock(&devlist_lock);
+
+	kfree(stfm1000);
+	return 0;
+}
+
+static const struct i2c_device_id stfm1000_id[] = {
+	{ "stfm1000", 0xC0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, stfm1000_id);
+
+static struct i2c_driver stfm1000_i2c_driver = {
+	.driver = {
+		.name = "stfm1000",
+	},
+	.probe		= stfm1000_probe,
+	.remove		= stfm1000_remove,
+	.id_table	= stfm1000_id,
+};
+
+static int __init
+stfm1000_init(void)
+{
+	/* pull those in */
+	(void)Lock_Station;
+	(void)Unlock_Station;
+	return i2c_add_driver(&stfm1000_i2c_driver);
+}
+
+static void __exit
+stfm1000_exit(void)
+{
+	i2c_del_driver(&stfm1000_i2c_driver);
+
+	stfm1000_alsa_ops = NULL;
+}
+
+module_init(stfm1000_init);
+module_exit(stfm1000_exit);
+
+MODULE_AUTHOR("Pantelis Antoniou");
+MODULE_DESCRIPTION("A driver for the STFM1000 chip.");
+MODULE_LICENSE("GPL");
+
+module_param(georegion, int, 0400);
+module_param(rds_enable, int, 0400);
diff --git a/drivers/media/radio/stfm1000/stfm1000-filter.c b/drivers/media/radio/stfm1000/stfm1000-filter.c
new file mode 100644
index 000000000000..df42524a5da7
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-filter.c
@@ -0,0 +1,860 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/init.h>
+
+#include "stfm1000.h"
+
+void stfm1000_filter_reset(struct stfm1000_filter_parms *sdf)
+{
+	sdf->Left = 0;
+	sdf->Right = 0;
+	sdf->RssiDecoded = 0;
+	sdf->RssiMant = 0;
+	sdf->RssiExp = 0;
+	sdf->RssiLb = 0;
+	sdf->TrueRssi = 0;
+	sdf->Prssi = 0;
+	sdf->RssiLog = 0;
+	sdf->ScaledTrueRssi = 0;
+	sdf->FilteredRssi = 0;
+	sdf->PrevFilteredRssi = 0;
+	sdf->DecRssi = 0;
+	sdf->ScaledRssiDecoded = 0;
+	sdf->ScaledRssiDecodedZ = 0;
+	sdf->ScaledRssiDecodedZz = 0;
+	sdf->Echo = 0;
+	sdf->EchoLb = 0;
+	sdf->TrueEcho = 0;
+	sdf->FilteredEchoLpr = 0;
+	sdf->PrevFilteredEchoLpr = 0;
+	sdf->FilteredEchoLmr = 0;
+	sdf->PrevFilteredEchoLmr = 0;
+	sdf->GatedEcho = 0;
+	sdf->ControlLpr = 0;
+	sdf->ControlLmr = 0;
+	sdf->LprBw = 0;
+	sdf->LmrBw = 0;
+
+	sdf->LprXz = 0;
+	sdf->LprXzz = 0;
+	sdf->LprYz = 0;
+	sdf->LprYzz = 0;
+	sdf->LmrXz = 0;
+	sdf->LmrXzz = 0;
+	sdf->LmrYz = 0;
+	sdf->LmrYzz = 0;
+	sdf->FilteredLpr = 0;
+	sdf->FilteredLmr = 0;
+
+	sdf->B0B = 0;
+	sdf->B0S = 0;
+	sdf->B0M = 0;
+	sdf->B1over2B = 0;
+	sdf->B1over2S = 0;
+	sdf->B1over2M = 0;
+	sdf->A1over2B = 0;
+	sdf->A1over2S = 0;
+	sdf->A1over2M = 0;
+	sdf->A2B = 0;
+	sdf->A2S = 0;
+	sdf->A2M = 0;
+
+	sdf->AdjBw = 0;
+
+	sdf->pCoefLprBwThLo = 20 << 8;
+	sdf->pCoefLprBwThHi = 30 << 8;
+	sdf->pCoefLmrBwThLo = 40 << 8;
+	sdf->pCoefLmrBwThHi = 50 << 8;
+	sdf->pCoefLprBwSlSc = 4800;	/* SDK-2287 */
+	sdf->pCoefLprBwSlSh = 10;	/* SDK-2287 */
+	sdf->pCoefLmrBwSlSc = 4800;	/* SDK-2287 */
+	sdf->pCoefLmrBwSlSh = 10;	/* SDK-2287 */
+	sdf->pCoefLprGaSlSc = 0;
+	sdf->pCoefLprGaSlSh = 0;
+
+	sdf->ScaledControlLmr = 0;
+
+	sdf->LprGa = 32767;
+	sdf->LmrGa = 32767;
+
+	sdf->pCoefLprGaTh = 20;		/* 25 */
+	sdf->pCoefLmrGaTh = 55;		/* 60 50 */
+
+	sdf->MuteAudio = 0;
+	sdf->PrevMuteAudio = 0;
+	sdf->MuteActionFlag = 0;
+	sdf->ScaleAudio = 0;
+
+	/* *** Programmable initial setup for stereo path filters */
+	sdf->LprB0 = 18806;		/* -3dB cutoff = 17 kHz */
+	sdf->LprB1over2 = 18812;	/* -3dB cutoff = 17 kHz */
+	sdf->LprA1over2 = -16079;	/* -3dB cutoff = 17 kHz */
+	sdf->LprA2 = -11125;		/* -3dB cutoff = 17 kHz */
+	sdf->LmrB0 = 18806;		/* -3dB cutoff = 17 kHz */
+	sdf->LmrB1over2 = 18812;	/* -3dB cutoff = 17 kHz */
+	sdf->LmrA1over2 = -16079;	/* -3dB cutoff = 17 kHz */
+	sdf->LmrA2 = -11125;		/* -3dB cutoff = 17 kHz */
+
+	sdf->pCoefForceLockLmrBw = 0;	/* Force Lock LMR BW = LPR BW
+					 * XXX BUG WARNING -
+					 * This control doesn't work! */
+
+	sdf->pCoefForcedMono = 0;	/* Do not set this =
+					 * Quality Monitor will overwrite it */
+	sdf->pCoefBypassBlend = 0;	/* BUG WARNING -
+					 * This control doesn't work! */
+	sdf->pCoefBypassSoftmute = 0;	/* BUG WARNING -
+					 * This control doesn't work! */
+	sdf->pCoefBypassBwCtl = 0;	/* BUG WARNING -
+					 * This control doesn't work! */
+
+	/* There's a bug or something in the attack/decay section b/c
+	 * setting these coef's to anything */
+	/* higher than 100ms or so causes the RSSI to be artificially low -
+	 * Needs investigation! 15DEC06 */
+	sdf->pCoefRssiAttack = 65386;		/* changed to 100ms to avoid
+						 * stereo crackling
+						 * 60764 corresponds to 3 */
+	sdf->pCoefRssiDecay = 65386;		/* changed to 100ms to avoid
+						 * stereo crackling
+						 * 65530 corresponds to 10 */
+	sdf->pCoefEchoLprAttack = 52239;	/* corresponds to 1 */
+	sdf->pCoefEchoLprDecay = 64796;		/* corresponds to 20 */
+	sdf->pCoefEchoLmrAttack = 52239;	/* corresponds to 1 */
+	sdf->pCoefEchoLmrDecay = 65520;		/* corresponds to 20 */
+	sdf->pCoefEchoTh = 100;
+	sdf->pCoefEchoScLpr = (u16) (0.9999 * 32767.0);
+	sdf->pCoefEchoScLmr = (u16) (0.9999 * 32767.0);
+}
+
+void stfm1000_filter_decode(struct stfm1000_filter_parms *sdf, s16 Lpr,
+	s16 Lmr, u16 Rssi)
+{
+	s16 temp1_reg;		/* mimics 16 bit register */
+	s16 temp2_reg;		/* mimics 16 bit register */
+	s16 temp3_reg;		/* mimics 16 bit register */
+	s16 temp4_reg;		/* mimics 16 bit register */
+#ifndef _TUNER_STFM_MUTE
+	s16 temp5_reg;		/* mimics 16 bit register */
+#endif
+	s32 temp2_reg_32;	/*eI 108 27th Feb 06 temp variables. */
+
+	/* **************************************************************** */
+	/* *** Stereo Processing ****************************************** */
+	/* **************************************************************** */
+	/* *** This block operates at Fs = 44.1kHz */
+	/* ******** */
+	/* *** LPR path filter (2nd order IIR) */
+
+	sdf->Acc_signed = sdf->LprB0 * Lpr + 2 * (sdf->LprB1over2 * sdf->LprXz)
+	    + sdf->LprB0 * sdf->LprXzz + 2 * (sdf->LprA1over2 * sdf->LprYz)
+	    + sdf->LprA2 * sdf->LprYzz;
+
+	sdf->FilteredLpr = sdf->Acc_signed >> 15;
+
+	sdf->LprXzz = sdf->LprXz;	/* update taps */
+	sdf->LprXz = Lpr;
+	sdf->LprYzz = sdf->LprYz;
+	sdf->LprYz = sdf->FilteredLpr;
+
+	/* *** LMR path filter (2nd order IIR) */
+	sdf->Acc_signed = sdf->LmrB0 * Lmr + 2 * (sdf->LmrB1over2 * sdf->LmrXz)
+	    + sdf->LmrB0 * sdf->LmrXzz + 2 * (sdf->LmrA1over2 * sdf->LmrYz)
+	    + sdf->LmrA2 * sdf->LmrYzz;
+
+	sdf->FilteredLmr = sdf->Acc_signed >> 15;
+
+	sdf->LmrXzz = sdf->LmrXz;	/* update taps */
+	sdf->LmrXz = Lmr;
+	sdf->LmrYzz = sdf->LmrYz;
+	sdf->LmrYz = sdf->FilteredLmr;
+
+	/* *** Stereo Matrix */
+	if (0 == sdf->pCoefBypassBlend)
+		temp1_reg = sdf->LmrGa * sdf->FilteredLmr >> 15; /* Blend */
+	else
+		temp1_reg = sdf->FilteredLmr;
+
+	if (sdf->pCoefForcedMono)	/* Forced Mono */
+		temp1_reg = 0;
+
+	if (0 == sdf->pCoefBypassSoftmute) {
+		temp2_reg = sdf->LprGa * sdf->FilteredLpr >> 15; /* LPR */
+		temp3_reg = sdf->LprGa * temp1_reg >> 15;	 /* LMR */
+	} else {
+		temp2_reg = sdf->FilteredLpr;
+		temp3_reg = temp1_reg;
+	}
+
+	temp4_reg = (temp2_reg + temp3_reg) / 2;	/* Matrix */
+
+#ifndef _TUNER_STFM_MUTE
+	temp5_reg = (temp2_reg - temp3_reg) / 2;
+#endif
+
+#if 0
+	/* *** DC Cut Filter (leaky bucket estimate) */
+	if (0 == sdf->pCoefBypassDcCut) {
+		sdf->LeftLb_i32 =
+		    sdf->LeftLb_i32 + temp4_reg - (sdf->LeftLb_i32 >> 8);
+		temp2_reg = temp4_reg - (sdf->LeftLb_i32 >> 8);	/* signal -
+								dc_estimate */
+
+		sdf->RightLb_i32 =
+		    sdf->RightLb_i32 + temp5_reg - (sdf->RightLb_i32 >> 8);
+		temp3_reg = temp5_reg - (sdf->RightLb_i32 >> 8); /* signal -
+								dc_estimate */
+	} else {
+		temp2_reg = temp4_reg;
+		temp3_reg = temp5_reg;
+	}
+#endif
+#ifdef _TUNER_STFM_MUTE
+	/* *** Mute Audio */
+	if (sdf->MuteAudio != sdf->PrevMuteAudio)	/* Mute transition */
+		sdf->MuteActionFlag = 1;	/* set flag */
+	sdf->PrevMuteAudio = sdf->MuteAudio;	/* update history */
+
+	if (sdf->MuteActionFlag) {
+		if (0 == sdf->MuteAudio) {	/* Mute to zero */
+			/* gradual mute down */
+			sdf->ScaleAudio = sdf->ScaleAudio - sdf->pCoefMuteStep;
+
+			/* eI-117:Oct28:as per C++ code */
+			/* if (0 < sdf->ScaleAudio) */
+			if (0 > sdf->ScaleAudio) {
+				sdf->ScaleAudio = 0;	 /* Minimum scale
+							  * factor */
+				sdf->MuteActionFlag = 0; /* End Mute Action */
+			}
+		} else {	/* Un-Mute to one */
+				/* gradual mute up */
+			sdf->ScaleAudio = sdf->ScaleAudio + sdf->pCoefMuteStep;
+			if (0 > sdf->ScaleAudio) {	/* look for rollover
+							 * beyong 32767 */
+				sdf->ScaleAudio = 32767; /* Maximum scale
+							  * factor */
+				sdf->MuteActionFlag = 0; /* End Mute Action */
+			}
+		}		/* end else */
+	}			/* end if (sdf->MuteActionFlag) */
+
+/*! Output Processed Sample */
+
+	sdf->Left = (temp2_reg * sdf->ScaleAudio) >> 15;	/* Scale */
+	sdf->Right = (temp3_reg * sdf->ScaleAudio) >> 15;	/* Scale */
+
+#else /* !_TUNER_STFM_MUTE */
+
+	sdf->Left = temp4_reg;
+	sdf->Right = temp5_reg;
+
+#endif /* !_TUNER_STFM_MUTE */
+
+	/* *** End Stereo Processing ************************************** */
+	/* **************************************************************** */
+
+	/* **************************************************************** */
+	/* *** Signal Quality Indicators ********************************** */
+	/* **************************************************************** */
+	/* *** This block operates at Fs = 44.1kHz */
+	/* ******** */
+	/* *** RSSI */
+	/* ******** */
+	/* Decode Floating Point RSSI data */
+	/*! Input RSSI sample */
+	sdf->RssiMant = (Rssi & 0xFFE0) >> 5;	/* 11 msb's */
+	sdf->RssiExp = Rssi & 0x001F;	/* 5 lsb's */
+	sdf->RssiDecoded = sdf->RssiMant << sdf->RssiExp;
+
+	/* *** Convert RSSI to 10*Log10(RSSI) */
+	/* This is easily accomplished in DSP code using the CLZ instruction */
+	/* rather than using all these comparisons. */
+	/* The basic idea is this: */
+	/* if x >= 2^P */
+	/*   f(x) = 3*x>>P + (3*P-3) */
+	/* Approx. is valid over the range of sdf->RssiDecoded in [0, 2^21] */
+	/* *** */
+	if (sdf->RssiDecoded >= 1048576)
+		sdf->Prssi = 20;
+	else if (sdf->RssiDecoded >= 524288)
+		sdf->Prssi = 19;
+	else if (sdf->RssiDecoded >= 262144)
+		sdf->Prssi = 18;
+	else if (sdf->RssiDecoded >= 131072)
+		sdf->Prssi = 17;
+	else if (sdf->RssiDecoded >= 65536)
+		sdf->Prssi = 16;
+	else if (sdf->RssiDecoded >= 32768)
+		sdf->Prssi = 15;
+	else if (sdf->RssiDecoded >= 16384)
+		sdf->Prssi = 14;
+	else if (sdf->RssiDecoded >= 8192)
+		sdf->Prssi = 13;
+	else if (sdf->RssiDecoded >= 4096)
+		sdf->Prssi = 12;
+	else if (sdf->RssiDecoded >= 2048)
+		sdf->Prssi = 11;
+	else if (sdf->RssiDecoded >= 1024)
+		sdf->Prssi = 10;
+	else if (sdf->RssiDecoded >= 512)
+		sdf->Prssi = 9;
+	else if (sdf->RssiDecoded >= 256)
+		sdf->Prssi = 8;
+	else if (sdf->RssiDecoded >= 128)
+		sdf->Prssi = 7;
+	else if (sdf->RssiDecoded >= 64)
+		sdf->Prssi = 6;
+	else if (sdf->RssiDecoded >= 32)
+		sdf->Prssi = 5;
+	else if (sdf->RssiDecoded >= 16)
+		sdf->Prssi = 4;
+	else if (sdf->RssiDecoded >= 8)
+		sdf->Prssi = 3;
+	else if (sdf->RssiDecoded >= 4)
+		sdf->Prssi = 2;
+	else if (sdf->RssiDecoded >= 2)
+		sdf->Prssi = 1;
+	else
+		sdf->Prssi = 0;
+	sdf->RssiLog =
+	    (3 * sdf->RssiDecoded >> sdf->Prssi) + (3 * sdf->Prssi - 3);
+
+	if (0 > sdf->RssiLog)	/* Clamp to positive */
+		sdf->RssiLog = 0;
+
+	/* Compensate for errors in truncation/approximation by adding 1 */
+	sdf->RssiLog = sdf->RssiLog + 1;
+
+	/* Leaky Bucket Filter DC estimate of RSSI */
+	sdf->RssiLb = sdf->RssiLb + sdf->RssiLog - (sdf->RssiLb >> 3);
+	sdf->TrueRssi = sdf->RssiLb >> 3;
+
+	/* Scale up so we have some room for precision */
+	sdf->ScaledTrueRssi = sdf->TrueRssi << 8;
+	/* ************ */
+	/* *** end RSSI */
+	/* ************ */
+
+	/* ******** */
+	/* *** Echo */
+	/* ******** */
+	/* *** Isolate Echo information as higher frequency info */
+	/* using [1 -2 1] highpass FIR */
+	sdf->ScaledRssiDecoded = sdf->RssiDecoded >> 4;
+	sdf->Echo =
+	    (s16) ((sdf->ScaledRssiDecoded -
+		    2 * sdf->ScaledRssiDecodedZ + sdf->ScaledRssiDecodedZz));
+	sdf->ScaledRssiDecodedZz = sdf->ScaledRssiDecodedZ;
+	sdf->ScaledRssiDecodedZ = sdf->ScaledRssiDecoded;
+	/* ************ */
+	/* *** end Echo */
+	/* ************ */
+	/* *** End Signal Quality Indicators ******************************* */
+	/* ***************************************************************** */
+
+	/* ***************************************************************** */
+	/* *** Weak Signal Processing ************************************** */
+	/* ***************************************************************** */
+	/* *** This block operates at Fs = 44.1/16 = 2.75 Khz
+	 * *eI 108 28th Feb 06 WSP and SM executes at 2.75Khz */
+	/* decimate by 16                   STFM_FILTER_BLOCK_MULTIPLE is 16 */
+	if (0 == sdf->DecRssi) {
+		/* *** Filter RSSI via attack/decay structure */
+		if (sdf->ScaledTrueRssi > sdf->PrevFilteredRssi)
+			sdf->Acc =
+			    sdf->pCoefRssiAttack *
+			    sdf->PrevFilteredRssi + (65535 -
+						     sdf->pCoefRssiAttack)
+			    * sdf->ScaledTrueRssi;
+		else
+			sdf->Acc =
+			    sdf->pCoefRssiDecay *
+			    sdf->PrevFilteredRssi + (65535 -
+						     sdf->pCoefRssiDecay)
+			    * sdf->ScaledTrueRssi;
+
+		sdf->FilteredRssi = sdf->Acc >> 16;
+		sdf->PrevFilteredRssi = sdf->FilteredRssi;
+
+		/* *** Form Echo "energy" representation */
+		if (0 > sdf->Echo)
+			sdf->Echo = -sdf->Echo;	/* ABS() */
+
+		/* Threshold compare */
+		sdf->GatedEcho = (s16) (sdf->Echo - sdf->pCoefEchoTh);
+		if (0 > sdf->GatedEcho)	/* Clamp to (+)ve */
+			sdf->GatedEcho = 0;
+
+		/* *** Leaky bucket DC estimate of Echo energy */
+		sdf->EchoLb = sdf->EchoLb + sdf->GatedEcho -
+				(sdf->EchoLb >> 3);
+		sdf->TrueEcho = sdf->EchoLb >> 3;
+
+		/* *** Filter Echo via attack/decay structure for LPR */
+		if (sdf->TrueEcho > sdf->PrevFilteredEchoLpr)
+			sdf->Acc =
+			    sdf->pCoefEchoLprAttack *
+			    sdf->PrevFilteredEchoLpr +
+				(65535 - sdf->pCoefEchoLprAttack) *
+					sdf->TrueEcho;
+		else
+			sdf->Acc =
+			    sdf->pCoefEchoLprDecay *
+			    sdf->PrevFilteredEchoLpr +
+				(65535 - sdf->pCoefEchoLprDecay) *
+					sdf->TrueEcho;
+
+		sdf->FilteredEchoLpr = sdf->Acc >> 16;
+		sdf->PrevFilteredEchoLpr = sdf->FilteredEchoLpr;
+
+		/* *** Filter Echo via attack/decay structure for LMR */
+		if (sdf->TrueEcho > sdf->PrevFilteredEchoLmr)
+			sdf->Acc = sdf->pCoefEchoLmrAttack *
+				sdf->PrevFilteredEchoLmr +
+					(65535 - sdf->pCoefEchoLmrAttack)
+						* sdf->TrueEcho;
+		else
+			sdf->Acc =
+			    sdf->pCoefEchoLmrDecay *
+			    sdf->PrevFilteredEchoLmr + (65535 -
+							sdf->pCoefEchoLmrDecay)
+			    * sdf->TrueEcho;
+
+		sdf->FilteredEchoLmr = sdf->Acc >> 16;
+		sdf->PrevFilteredEchoLmr = sdf->FilteredEchoLmr;
+
+		/* *** Form control variables */
+		/* Generically speaking, ctl = f(RSSI, Echo) =
+		 * RSSI - (a*Echo)<<b, where a,b are programmable */
+		sdf->ControlLpr = sdf->FilteredRssi -
+		    ((sdf->pCoefEchoScLpr *
+		      sdf->FilteredEchoLpr << sdf->pCoefEchoShLpr) >> 15);
+		if (0 > sdf->ControlLpr)
+			sdf->ControlLpr = 0;	/* Clamp to positive */
+
+		sdf->ControlLmr = sdf->FilteredRssi -
+		    ((sdf->pCoefEchoScLmr *
+		      sdf->FilteredEchoLmr << sdf->pCoefEchoShLmr) >> 15);
+		if (0 > sdf->ControlLmr)
+			sdf->ControlLmr = 0;	/* Clamp to positive */
+
+		/* *** Define LPR_BW = f(control LPR) */
+		/* Assume that 5 kHz and 17 kHz are limits of LPR_BW control */
+		if (sdf->ControlLpr <= sdf->pCoefLprBwThLo)
+			sdf->LprBw = 5000;	/* lower limit is 5 kHz */
+		else if (sdf->ControlLpr >= sdf->pCoefLprBwThHi)
+			sdf->LprBw = 17000;	/* upper limit is 17 kHz */
+		else
+			sdf->LprBw = 17000 -
+			    ((sdf->pCoefLprBwSlSc *
+			      (sdf->pCoefLprBwThHi -
+			       sdf->ControlLpr)) >> sdf->pCoefLprBwSlSh);
+
+		/* *** Define LMR_BW = f(control LMR) */
+		/* Assume that 5 kHz and 17 kHz are limits of LPR_BW control */
+		if (0 == sdf->pCoefForceLockLmrBw) {	/* only do these calc's
+							 * if LMR BW not
+							 * ForceLocked */
+			if (sdf->ControlLmr <= sdf->pCoefLmrBwThLo)
+				sdf->LmrBw = 5000;	/* lower limit is
+							 * 5 kHz */
+			else if (sdf->ControlLmr >= sdf->pCoefLmrBwThHi)
+				sdf->LmrBw = 17000;	/* upper limit is
+							 * 17 kHz */
+			else
+				sdf->LmrBw = 17000 -
+				    ((sdf->pCoefLmrBwSlSc *
+				      (sdf->pCoefLmrBwThHi -
+				       sdf->ControlLmr)) >>
+				     sdf->pCoefLmrBwSlSh);
+		}
+		/* *** Define LMR_Gain = f(control LMR)
+		 * Assume that Blending occurs across 20 dB range of
+		 * control LMR. For sake of listenability, approximate
+		 * antilog blending curve
+		 * To simplify antilog approx, scale control LMR back into
+		 * "RSSI in dB range" [0,60] */
+		sdf->ScaledControlLmr = sdf->ControlLmr >> 8;
+
+		/* how far below blend threshold are we? */
+		temp1_reg = sdf->pCoefLmrGaTh - sdf->ScaledControlLmr;
+		if (0 > temp1_reg)	/* We're not below threshold,
+					 * so no blending needed */
+			temp1_reg = 0;
+		temp2_reg = 20 - temp1_reg;	/* Blend range = 20 dB */
+		if (0 > temp2_reg)
+			temp2_reg = 0;	/* if beyond that range,
+					 * then clamp to 0 */
+
+		/* We want stereo separation (n dB) to rolloff linearly over
+		 * the 20 dB wide blend region.
+		 * this necessitates a particular rolloff for the blend
+		 * parameter, which is not obvious.
+		 * See sw_audio/log_approx.m for calculation of this rolloff,
+		 * implemented below...
+		 * Note that stereo_separation (in dB) = 20*log10((1+a)/(1-a)),
+		 * where a = blend scaler
+		 * appropriately scaled for 2^15.  This relationship sits at
+		 * the heart of why this curve is needed. */
+		if (15 <= temp2_reg)
+			temp3_reg = 264 * temp2_reg + 27487;
+		else if (10 <= temp2_reg)
+			temp3_reg = 650 * temp2_reg + 21692;
+		else if (5 <= temp2_reg)
+			temp3_reg = 1903 * temp2_reg + 9166;
+		else
+			temp3_reg = 3736 * temp2_reg;
+
+		sdf->LmrGa = temp3_reg;
+
+		if (32767 < sdf->LmrGa)
+			sdf->LmrGa = 32767;	/* Clamp to '1' */
+
+		/* *** Define LPR_Gain = f(control LPR)
+		 * Assume that SoftMuting occurs across 20 dB range of
+		 * control LPR
+		 * For sake of listenability, approximate antilog softmute
+		 * curve To simplify antilog approx, scale control LPR back
+		 * into "RSSI in dB range" [0,60] */
+		sdf->ScaledControlLpr = sdf->ControlLpr >> 8;
+		/* how far below softmute threshold are we? */
+		temp1_reg = sdf->pCoefLprGaTh - sdf->ScaledControlLpr;
+		if (0 > temp1_reg)	/* We're not below threshold,
+					 * so no softmute needed */
+			temp1_reg = 0;
+		temp2_reg = 20 - temp1_reg;	/* SoftmMute range = 20 dB */
+		if (0 > temp2_reg)
+			temp2_reg = 0;	/* if beyond that range,
+					 * then clamp to 0 */
+		/* Form 100*10^((temp2_reg-20)/20) approximation (antilog)
+		 * over range [0,20] dB
+		 * approximation in range [0,100], but we only want to
+		 * softmute down to -20 dB, no further */
+		if (16 < temp2_reg)
+			temp3_reg = 9 * temp2_reg - 80;
+		else if (12 < temp2_reg)
+			temp3_reg = 6 * temp2_reg - 33;
+		else if (8 < temp2_reg)
+			temp3_reg = 4 * temp2_reg - 8;
+		else
+			temp3_reg = 2 * temp2_reg + 9;
+
+		sdf->LprGa = 328 * temp3_reg;	/* close to 32767*(1/100) */
+
+		if (32767 < sdf->LprGa)
+			sdf->LprGa = 32767;	/* Clamp to '1' */
+
+		if (3277 > sdf->LprGa)
+			sdf->LprGa = 3277;	/* Clamp to 0.1*32767 =
+						 * -20 dB min gain */
+
+		/* *************** Bandwidth Sweep Algorithm ************ */
+		/* *** Calculate 2nd order filter coefficients as function
+		 * of desired BW. We do this by constructing piece-wise
+		 * linear filter coef's as f(BW), which is why we break the
+		 * calc's into different BW regions below.
+		 * coef(BW) = S*(M*BW + B)
+		 * For more info, see sw_audio/ws_filter.m checked into CVS */
+		if (0 == sdf->pCoefBypassBwCtl)	{ /* if ==1, then we just go
+						   * with default coef set */
+			/* determine if we run thru loop once or twice... */
+			if (1 == sdf->pCoefForceLockLmrBw)
+				temp4_reg = 1;	/* run thru once only to calc.
+						 * LPR coef's */
+			else
+				temp4_reg = 2;	/* run thru twice to calc.
+						 * LPR and LMR coef's */
+
+			/* Here's the big coef. calc. loop */
+			for (temp1_reg = 0; temp1_reg < temp4_reg;
+				temp1_reg++) {
+
+				if (0 == temp1_reg)
+					temp2_reg = (s16) sdf->LprBw;
+				else
+					temp2_reg = (s16) sdf->LmrBw;
+
+
+				if (6000 > temp2_reg) {
+					/* interval = [4.4kHz, 6.0kHz) */
+					sdf->B0M = 22102;
+					sdf->B0B = -2209;
+					sdf->B0S = 1;
+
+					sdf->B1over2M = 22089;
+					sdf->B1over2B = -2205;
+					sdf->B1over2S = 1;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = -24664;
+					sdf->A2B = 11698;
+					sdf->A2S = 2;
+				} else if (8000 > temp2_reg) {
+					/* interval = [6.0kHz, 8.0kHz) */
+					sdf->B0M = 22102;
+					sdf->B0B = -2209;
+					sdf->B0S = 1;
+
+					sdf->B1over2M = 22089;
+					sdf->B1over2B = -2205;
+					sdf->B1over2S = 1;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = -31231;
+					sdf->A2B = 18468;
+					sdf->A2S = 1;
+				} else if (10000 > temp2_reg) {
+					/* interval = [8.0kHz, 10.0kHz) */
+					sdf->B0M = 28433;
+					sdf->B0B = -4506;
+					sdf->B0S = 1;
+
+					sdf->B1over2M = 28462;
+					sdf->B1over2B = -4584;
+					sdf->B1over2S = 1;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = -14811;
+					sdf->A2B = 12511;
+					sdf->A2S = 1;
+				} else if (12000 > temp2_reg) {
+					/* interval = [10.0kHz, 12.0kHz) */
+					sdf->B0M = 28433;
+					sdf->B0B = -4506;
+					sdf->B0S = 1;
+
+					sdf->B1over2M = 28462;
+					sdf->B1over2B = -4584;
+					sdf->B1over2S = 1;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = -181;
+					sdf->A2B = 5875;
+					sdf->A2S = 1;
+				} else if (14000 > temp2_reg) {
+					/* interval = [12.0kHz, 14.0kHz) */
+					sdf->B0M = 18291;
+					sdf->B0B = -4470;
+					sdf->B0S = 2;
+
+					sdf->B1over2M = 18461;
+					sdf->B1over2B = -4597;
+					sdf->B1over2S = 2;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = 14379;
+					sdf->A2B = -2068;
+					sdf->A2S = 1;
+				} else if (16000 > temp2_reg) {
+					/* interval = [14.0kHz, 16.0kHz) */
+					sdf->B0M = 18291;
+					sdf->B0B = -4470;
+					sdf->B0S = 2;
+
+					sdf->B1over2M = 18461;
+					sdf->B1over2B = -4597;
+					sdf->B1over2S = 2;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = 30815;
+					sdf->A2B = -12481;
+					sdf->A2S = 1;
+				} else if (18000 > temp2_reg) {
+					/* interval = [16.0kHz, 18.0kHz) */
+					sdf->B0M = 24740;
+					sdf->B0B = -9152;
+					sdf->B0S = 2;
+
+					sdf->B1over2M = 24730;
+					sdf->B1over2B = -9142;
+					sdf->B1over2S = 2;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = 25631;
+					sdf->A2B = -13661;
+					sdf->A2S = 2;
+				} else {
+					/* interval = [18.0kHz, 19.845kHz) */
+					sdf->B0M = 24740;
+					sdf->B0B = -9152;
+					sdf->B0S = 2;
+
+					sdf->B1over2M = 24730;
+					sdf->B1over2B = -9142;
+					sdf->B1over2S = 2;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = 19382;
+					sdf->A2B = -12183;
+					sdf->A2S = 4;
+				}
+
+				if (0 == temp1_reg) {
+					/* The piece-wise linear eq's are
+					 * based on a scaled version
+					 * (32768/22050) of BW */
+
+					/* Note 32768/22050 <-> 2*(16384/22050)
+					 * <-> 2*((16384/22050)*32768)>>15 */
+					sdf->AdjBw = ((temp2_reg << 1) *
+						24348) >> 15;
+
+					/* temp = mx */
+					temp3_reg = (sdf->B0M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LprB0 = sdf->B0S *
+						(temp3_reg + sdf->B0B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->B1over2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LprB1over2 = sdf->B1over2S *
+						(temp3_reg + sdf->B1over2B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->A1over2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LprA1over2 = -sdf->A1over2S *
+						(temp3_reg + sdf->A1over2B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->A2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LprA2 = -sdf->A2S *
+						(temp3_reg + sdf->A2B);
+					/* *** end LPR channel --
+					 * LPR coefficients now ready for
+					 * Stereo Path next time */
+				} else {
+					/* The piece-wise linear eq's are
+					 * based on a scaled version
+					 * (32768/22050) of BW */
+
+					/* Note 32768/22050 <-> 2*(16384/22050)
+					 * <-> 2*((16384/22050)*32768)>>15 */
+					sdf->AdjBw = ((temp2_reg << 1) *
+						24348) >> 15;
+
+					/* temp = mx */
+					temp3_reg = (sdf->B0M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LmrB0 = sdf->B0S *
+						(temp3_reg + sdf->B0B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->B1over2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LmrB1over2 = sdf->B1over2S *
+						(temp3_reg + sdf->B1over2B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->A1over2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LmrA1over2 = -sdf->A1over2S *
+						(temp3_reg + sdf->A1over2B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->A2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LmrA2 = -sdf->A2S *
+						(temp3_reg + sdf->A2B);
+					/* *** end LMR channel -- LMR
+					 * coefficients now ready for Stereo
+					 * Path next time */
+				}
+			}	/* end for (temp1_reg=0... */
+			if (1 == sdf->pCoefForceLockLmrBw) {
+				/* if Force Lock LMR BW = LPR BW */
+				/* then set LMR coef's = LPR coef's */
+				sdf->LmrB0 = sdf->LprB0;
+				sdf->LmrB1over2 = sdf->LprB1over2;
+				sdf->LmrA1over2 = sdf->LprA1over2;
+				sdf->LmrA2 = sdf->LprA2;
+			}
+
+		}		/* end if (0 == sdf->pCoef_BypassBwCtl) */
+		/* eI 108 24th Feb 06 Streo Matrix part moved after
+		 * weak signal processing. */
+		if (0 == sdf->pCoefBypassBlend)
+			temp1_reg = sdf->LmrGa;	/* Blend */
+		else
+			temp1_reg = 1;
+
+		if (sdf->pCoefForcedMono)	/* Forced Mono */
+			temp1_reg = 0;
+
+		if (0 == sdf->pCoefBypassSoftmute) {
+
+			/* SoftMute applied to LPR */
+			sdf->temp2_reg_sm = sdf->LprGa;
+
+			temp2_reg_32 = sdf->LprGa * temp1_reg;
+
+			/* SoftMute applied to LMR */
+			sdf->temp3_reg_sm = (temp2_reg_32) >> 15;
+		} else {
+			sdf->temp2_reg_sm = 1;	/* eI 108 24th Feb 06 update
+						 * global variable for IIR
+						 * filter. */
+			sdf->temp3_reg_sm = temp1_reg;
+		}
+
+	}			/* end if (0 == sdf->DecRssi) */
+
+	sdf->DecRssi = ((sdf->DecRssi + 1) % 16);	/* end decimation
+							 * by 16 */
+
+	/* *** End Weak Signal Processing ********************************** */
+	/* ***************************************************************** */
+}
diff --git a/drivers/media/radio/stfm1000/stfm1000-filter.h b/drivers/media/radio/stfm1000/stfm1000-filter.h
new file mode 100644
index 000000000000..d24e3e9244b8
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-filter.h
@@ -0,0 +1,185 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef STFM1000_FILTER_H
+#define STFM1000_FILTER_H
+
+/* STFM1000 Black Box Filter parameters */
+struct stfm1000_filter_parms {
+	s16 LprXzz;		/* LPR x(n-2) stereo filter */
+	s16 LmrXzz;		/* LMR x(n-2) stereo filter */
+	s16 LprYzz;		/* LPR y(n-2) stereo filter */
+	s16 LmrYzz;		/* LMR y(n-2) stereo filter */
+
+	s16 LprXz;		/* LPR x(n-1) stereo filter */
+	s16 LmrXz;		/* LMR x(n-1) stereo filter */
+	s16 FilteredLpr;	/* LPR filter output */
+	s16 FilteredLmr;	/* LMR filter output */
+
+	s16 LprB0;		/* LPR stereo filter coef */
+	s16 LprB1over2;		/* LPR stereo filter coef */
+	s16 LprA1over2;		/* LPR stereo filter coef */
+	s16 LprA2;		/* LPR stereo filter coef */
+
+	s16 LmrB0;		/* LMR stereo filter coef */
+	s16 LmrB1over2;		/* LMR stereo filter coef */
+	s16 LmrA1over2;		/* LMR stereo filter coef */
+	s16 LmrA2;		/* LMR stereo filter coef */
+
+	s16 LprYz;		/* LPR y(n-1) stereo filter */
+	s16 LmrYz;		/* LMR y(n-1) stereo filter */
+
+	s16 Left;		/* left channel audio out */
+	s16 Right;		/* right channel audio out */
+	s32 LeftLb;		/* left channel dc estimate */
+	s32 RightLb;		/* right channel dc estimate */
+
+	u32 RssiDecoded;	/* integer decoded RSSI */
+
+	u16 RssiMant;		/* mantissa of float-coded RSSI */
+	s16 RssiLog;		/* 10log10(decoded RSSI) */
+
+	u16 RssiExp;		/* exponent of float-coded RSSI */
+	u16 RssiLb;		/* leaky bucket dc of rssi */
+
+	u16 Prssi;		/* power of 2 for RSSI */
+	u16 TrueRssi;		/* DC estimate of log RSSI */
+
+	u16 ScaledRssiDecoded;	/* scaled log RSSI */
+	s16 Echo;		/* Echo info from HiPass(RSSI) */
+	u16 ScaledRssiDecodedZ;	/* history buffer for above */
+	u16 ScaledRssiDecodedZz;/* ditto */
+
+	u16 ScaledTrueRssi;	/* scaled version for precision */
+	u16 FilteredRssi;	/* Attack/Decay filtered RSSI */
+	u16 PrevFilteredRssi;	/* previous version of above */
+
+	u16 EchoLb;		/* DC estimate of Echo energy */
+	u16 TrueEcho;		/* scaled version of above */
+	u16 FilteredEchoLpr;	/* Attack/Decay filt. Echo */
+	u16 PrevFilteredEchoLpr;/* previous version of above */
+	u16 FilteredEchoLmr;	/* Attack/Decay filt. Echo */
+	u16 PrevFilteredEchoLmr;/* previous version of above */
+	s16 GatedEcho;		/* Echo gated by threshold */
+
+	s16 ControlLpr;		/* master control for LPR */
+	s16 ControlLmr;		/* master control for LMR */
+	u16 LprBw;		/* LPR Bandwidth desired */
+	u16 LmrBw;		/* LMR Bandwidth desired */
+	u16 LprGa;		/* LPR Gain (SoftMute) desired */
+	u16 LmrGa;		/* LMR Gain (Blend) desired */
+	u16 ScaledControlLmr;	/* Scaled down version Ctl LMR */
+	u16 ScaledControlLpr;	/* Scaled down version Ctl LPR */
+
+	s16 B0M;		/* BW ctl B0 coef slope */
+	s16 B0B;		/* BW ctl B0 coef y-intercept */
+
+	u16 B0S;		/* BW ctl B0 coef scale */
+	s16 B1over2M;		/* BW ctl B1/2 coef slope */
+
+	s16 B1over2B;		/* BW ctl B1/2 coef y-intercept */
+	s16 A1over2B;		/* BW ctl A1/2 coef y-intercept */
+
+	u16 B1over2S;		/* BW ctl B1/2 coef scale */
+	u16 A1over2S;		/* BW ctl A1/2 coef scale */
+
+	s16 A1over2M;		/* BW ctl A1/2 coef slope */
+	u16 A2S;		/* BW ctl A2 coef scale */
+
+	s16 A2M;		/* BW ctl A2 coef slope */
+	s16 A2B;		/* BW ctl A2 coef y-intercept */
+
+	u16 AdjBw;		/* Desired Filter BW scaled into range */
+
+	u16 DecRssi;		/*! Decimation modulo counter */
+
+	s16 ScaleAudio;		/*! Scale factor for Audio Mute */
+	u8 MuteAudio;		/*! Control for muting audio */
+	u8 PrevMuteAudio;	/*! History of control for muting audio */
+	u8 MuteActionFlag;	/*! Indicator of when mute ramping occurs */
+
+	u32 Acc;		/* mimics H/W accumulator */
+	s32 Acc_signed;
+	s16 temp1_reg;		/* mimics 16 bit register */
+	s16 temp2_reg;		/* mimics 16 bit register */
+	s16 temp3_reg;		/* mimics 16 bit register */
+	s16 temp4_reg;		/* mimics 16 bit register */
+	s16 temp5_reg;		/* mimics 16 bit register */
+
+	/* *** Programmable Coefficients */
+	u16 pCoefRssiAttack;	/* prog coef RSSI attack */
+	u16 pCoefRssiDecay;	/* prog coef RSSI decay */
+	u16 pCoefEchoLprAttack;	/* prog coef Echo LPR attack */
+	u16 pCoefEchoLprDecay;	/* prog coef Echo LPR decay */
+	u16 pCoefEchoLmrAttack;	/* prog coef Echo LMR attack */
+	u16 pCoefEchoLmrDecay;	/* prog coef Echo LMR decay */
+
+	u16 pCoefEchoTh;	/* prog coef Echo threshold */
+
+	u16 pCoefEchoScLpr;	/* prog coef scale Echo LPR infl. */
+	u16 pCoefEchoScLmr;	/* prog coef scale Echo LMR infl. */
+	u16 pCoefEchoShLpr;	/* prog coef shift Echo LPR infl. */
+	u16 pCoefEchoShLmr;	/* prog coef shift Echo LMR infl. */
+
+	u16 pCoefLprBwThLo;	/* prog coef Low Th LPR BW */
+	u16 pCoefLprBwThHi;	/* prog coef High Th LPR BW */
+	u16 pCoefLmrBwThLo;	/* prog coef Low Th LMR BW */
+	u16 pCoefLmrBwThHi;	/* prog coef High Th LMR BW */
+
+	u16 pCoefLprGaTh;	/* prog coef Th LPR Gain (SoftMute) */
+	u16 pCoefLmrGaTh;	/* prog coef Th LMR Gain (Blend) */
+
+	u16 pCoefLprBwSlSc;	/* prog coef Slope scale LPR BW */
+	u16 pCoefLprBwSlSh;	/* prog coef Slope shift LPR BW */
+	u16 pCoefLmrBwSlSc;	/* prog coef Slope scale LMR BW */
+	u16 pCoefLmrBwSlSh;	/* prog coef Slope shift LMR BW */
+	u16 pCoefLprGaSlSc;	/* prog coef Slope scale LPR Gain */
+	u16 pCoefLprGaSlSh;	/* prog coef Slope shift LPR Gain */
+
+	u8 pCoefForcedMono;	/* Forced Mono control bit */
+	u8 pCoefBypassBlend;	/* Forced bypass of stereo blend */
+	u8 pCoefBypassSoftmute;	/* Forced bypass of softmute */
+	u8 pCoefBypassDcCut;	/* Forced bypass of audio DC Cut filter */
+
+	u8 pCoefBypassBwCtl;	/* Forced bypass of bandwidth control */
+	u8 pCoefForceLockLmrBw;	/* prog flag to force LMR BW=LPR BW */
+
+	/* XXX added here, they were global */
+	s16 temp2_reg_sm;
+	s16 temp3_reg_sm;
+
+};
+
+/* STFM1000 Black Box Filter Function prototypes */
+void stfm1000_filter_reset(struct stfm1000_filter_parms *sdf);
+void stfm1000_filter_decode(struct stfm1000_filter_parms *sdf, s16 Lpr,
+	s16 Lmr, u16 Rssi);
+
+static inline s16
+stfm1000_filter_value_left(struct stfm1000_filter_parms *sdf)
+{
+	return sdf->Left;
+}
+
+static inline s16
+stfm1000_filter_value_right(struct stfm1000_filter_parms *sdf)
+{
+	return sdf->Right;
+}
+
+static inline u32
+stfm1000_filter_value_rssi(struct stfm1000_filter_parms *sdf)
+{
+	return sdf->RssiDecoded;
+}
+
+#endif
diff --git a/drivers/media/radio/stfm1000/stfm1000-i2c.c b/drivers/media/radio/stfm1000/stfm1000-i2c.c
new file mode 100644
index 000000000000..50a047ecfda6
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-i2c.c
@@ -0,0 +1,453 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/io.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+
+#include <linux/version.h>      /* for KERNEL_VERSION MACRO     */
+
+#include "stfm1000.h"
+
+#define stfm1000_i2c_debug(p, fmt, arg...) \
+	do { \
+		if ((p)->dbgflg & STFM1000_DBGFLG_I2C) \
+			printk(KERN_INFO "stfm1000: " fmt, ##arg); \
+	} while (0)
+
+static const char *reg_names[STFM1000_NUM_REGS] = {
+#undef REGNAME
+#define REGNAME(x) \
+	[STFM1000_ ## x / 4] = #x ""
+
+	REGNAME(TUNE1),
+	REGNAME(SDNOMINAL),
+	REGNAME(PILOTTRACKING),
+	REGNAME(INITIALIZATION1),
+	REGNAME(INITIALIZATION2),
+	REGNAME(INITIALIZATION3),
+	REGNAME(INITIALIZATION4),
+	REGNAME(INITIALIZATION5),
+	REGNAME(INITIALIZATION6),
+	REGNAME(REF),
+	REGNAME(LNA),
+	REGNAME(MIXFILT),
+	REGNAME(CLK1),
+	REGNAME(CLK2),
+	REGNAME(ADC),
+	REGNAME(AGC_CONTROL1),
+	REGNAME(AGC_CONTROL2),
+	REGNAME(DATAPATH),
+	REGNAME(RMS),
+	REGNAME(AGC_STAT),
+	REGNAME(SIGNALQUALITY),
+	REGNAME(DCEST),
+	REGNAME(RSSI_TONE),
+	REGNAME(PILOTCORRECTION),
+	REGNAME(ATTENTION),
+	REGNAME(CLK3),
+	REGNAME(CHIPID),
+#undef REGNAME
+};
+
+static const int stfm1000_rw_regs[] = {
+	STFM1000_TUNE1,
+	STFM1000_SDNOMINAL,
+	STFM1000_PILOTTRACKING,
+	STFM1000_INITIALIZATION1,
+	STFM1000_INITIALIZATION2,
+	STFM1000_INITIALIZATION3,
+	STFM1000_INITIALIZATION4,
+	STFM1000_INITIALIZATION5,
+	STFM1000_INITIALIZATION6,
+	STFM1000_REF,
+	STFM1000_LNA,
+	STFM1000_MIXFILT,
+	STFM1000_CLK1,
+	STFM1000_CLK2,
+	STFM1000_ADC,
+	STFM1000_AGC_CONTROL1,
+	STFM1000_AGC_CONTROL2,
+	STFM1000_DATAPATH,
+	STFM1000_ATTENTION,	/* it's both WR/RD */
+};
+
+static const int stfm1000_ra_regs[] = {
+	STFM1000_RMS,
+	STFM1000_AGC_STAT,
+	STFM1000_SIGNALQUALITY,
+	STFM1000_DCEST,
+	STFM1000_RSSI_TONE,
+	STFM1000_PILOTCORRECTION,
+	STFM1000_ATTENTION,	/* it's both WR/RD - always read */
+	STFM1000_CLK3,
+	STFM1000_CHIPID
+};
+
+static int verify_writes;
+
+void stfm1000_setup_reg_set(struct stfm1000 *stfm1000)
+{
+	int i, reg;
+
+	/* set up register sets (read/write) */
+	for (i = 0; i < ARRAY_SIZE(stfm1000_rw_regs); i++) {
+		reg = stfm1000_rw_regs[i] / 4;
+		stfm1000->reg_rw_set[reg / 32] |= 1U << (reg & 31);
+		/* printk(KERN_INFO "STFM1000: rw <= %d\n", reg); */
+	}
+
+	/* for (i = 0; i < ARRAY_SIZE(stfm1000->reg_rw_set); i++)
+		printk("RW[%d] = 0x%08x\n", i, stfm1000->reg_rw_set[i]); */
+
+	/* set up register sets (read only) */
+	for (i = 0; i < ARRAY_SIZE(stfm1000_ra_regs); i++) {
+		reg = stfm1000_ra_regs[i] / 4;
+		stfm1000->reg_ra_set[reg / 32] |= 1U << (reg & 31);
+		/* printk(KERN_INFO "STFM1000: rw <= %d\n", reg); */
+	}
+	/* for (i = 0; i < ARRAY_SIZE(stfm1000->reg_ra_set); i++)
+		printk("RO[%d] = 0x%08x\n", i, stfm1000->reg_ra_set[i]); */
+
+	/* clear dirty */
+	memset(stfm1000->reg_dirty_set, 0, sizeof(stfm1000->reg_dirty_set));
+}
+
+static int stfm1000_reg_is_rw(struct stfm1000 *stfm1000, int reg)
+{
+	reg >>= 2;
+	return !!(stfm1000->reg_rw_set[reg / 32] & (1 << (reg & 31)));
+}
+
+static int stfm1000_reg_is_ra(struct stfm1000 *stfm1000, int reg)
+{
+	reg >>= 2;
+	return !!(stfm1000->reg_ra_set[reg / 32] & (1 << (reg & 31)));
+}
+
+static int stfm1000_reg_is_dirty(struct stfm1000 *stfm1000, int reg)
+{
+	reg >>= 2;
+	return !!(stfm1000->reg_dirty_set[reg / 32] & (1 << (reg & 31)));
+}
+
+static void stfm1000_reg_set_dirty(struct stfm1000 *stfm1000, int reg)
+{
+	reg >>= 2;
+	stfm1000->reg_dirty_set[reg / 32] |= 1 << (reg & 31);
+}
+
+static inline int stfm1000_reg_is_writeable(struct stfm1000 *stfm1000, int reg)
+{
+	return stfm1000_reg_is_rw(stfm1000, reg);
+}
+
+static inline int stfm1000_reg_is_readable(struct stfm1000 *stfm1000, int reg)
+{
+	return stfm1000_reg_is_rw(stfm1000, reg) ||
+	       stfm1000_reg_is_ra(stfm1000, reg);
+}
+
+/********************************************************/
+
+static int write_reg_internal(struct stfm1000 *stfm1000, int reg, u32 value)
+{
+	u8 values[5];
+	int ret;
+
+	stfm1000_i2c_debug(stfm1000, "%s(%s - 0x%02x, 0x%08x)\n", __func__,
+		reg_names[reg / 4], reg, value);
+
+	values[0] = (u8)reg;
+	values[1] = (u8)value;
+	values[2] = (u8)(value >> 8);
+	values[3] = (u8)(value >> 16);
+	values[4] = (u8)(value >> 24);
+	ret = i2c_master_send(stfm1000->client, values, 5);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int read_reg_internal(struct stfm1000 *stfm1000, int reg, u32 *value)
+{
+	u8 regb = reg;
+	u8 values[4];
+	int ret;
+
+	ret = i2c_master_send(stfm1000->client, &regb, 1);
+	if (ret < 0)
+		goto out;
+	ret = i2c_master_recv(stfm1000->client, values, 4);
+	if (ret < 0)
+		goto out;
+	*value = (u32)values[0] | ((u32)values[1] << 8) |
+		 ((u32)values[2] << 16) | ((u32)values[3] << 24);
+	ret = 0;
+
+	stfm1000_i2c_debug(stfm1000, "%s(%s - 0x%02x, 0x%08x)\n", __func__,
+		reg_names[reg / 4], reg, *value);
+out:
+	return ret;
+}
+
+int stfm1000_raw_write(struct stfm1000 *stfm1000, int reg, u32 value)
+{
+	int ret;
+
+	mutex_lock(&stfm1000->xfer_lock);
+	ret = write_reg_internal(stfm1000, reg, value);
+	mutex_unlock(&stfm1000->xfer_lock);
+
+	if (ret < 0)
+		dev_err(&stfm1000->client->dev, "%s: failed", __func__);
+
+	return ret;
+}
+
+int stfm1000_raw_read(struct stfm1000 *stfm1000, int reg, u32 *value)
+{
+	int ret;
+
+	mutex_lock(&stfm1000->xfer_lock);
+	ret = read_reg_internal(stfm1000, reg, value);
+	mutex_unlock(&stfm1000->xfer_lock);
+
+	if (ret < 0)
+		dev_err(&stfm1000->client->dev, "%s: failed", __func__);
+
+	return ret;
+}
+
+static inline void stfm1000_set_shadow_reg(struct stfm1000 *stfm1000,
+	int reg, u32 val)
+{
+	stfm1000->shadow_regs[reg / 4] = val;
+}
+
+static inline u32 stfm1000_get_shadow_reg(struct stfm1000 *stfm1000, int reg)
+{
+	return stfm1000->shadow_regs[reg / 4];
+}
+
+int stfm1000_write(struct stfm1000 *stfm1000, int reg, u32 value)
+{
+	int ret;
+
+	if (!stfm1000_reg_is_writeable(stfm1000, reg)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	mutex_lock(&stfm1000->xfer_lock);
+
+	/* same value as last one written? */
+	if (stfm1000_reg_is_dirty(stfm1000, reg) &&
+		stfm1000_get_shadow_reg(stfm1000, reg) == value) {
+		ret = 0;
+
+		stfm1000_i2c_debug(stfm1000, "%s - HIT "
+			"(%s - 0x%02x, 0x%08x)\n", __func__,
+			reg_names[reg / 4], reg, value);
+
+		goto out_unlock;
+	}
+
+	/* actually write the register */
+	ret = write_reg_internal(stfm1000, reg, value);
+	if (ret < 0)
+		goto out_unlock;
+
+	/* update shadow register & mark it as dirty */
+	/* only if register is not read always */
+	if (!stfm1000_reg_is_ra(stfm1000, reg)) {
+		stfm1000_set_shadow_reg(stfm1000, reg, value);
+		stfm1000_reg_set_dirty(stfm1000, reg);
+	}
+
+out_unlock:
+	mutex_unlock(&stfm1000->xfer_lock);
+
+out:
+	if (ret < 0)
+		dev_err(&stfm1000->client->dev, "%s: failed", __func__);
+
+	if (verify_writes) {
+		u32 value2 = ~0;
+
+		stfm1000_raw_read(stfm1000, reg, &value2);
+
+		stfm1000_i2c_debug(stfm1000, "%s - VER "
+			"(%s - 0x%02x, W=0x%08x V=0x%08x) %s\n", __func__,
+			reg_names[reg / 4], reg, value, value2,
+			value == value2 ? "OK" : "** differs **");
+	}
+
+	return ret;
+}
+
+int stfm1000_read(struct stfm1000 *stfm1000, int reg, u32 *value)
+{
+	int ret = 0;
+
+	if (!stfm1000_reg_is_readable(stfm1000, reg)) {
+		ret = -EINVAL;
+		printk(KERN_INFO "%s: !readable(%d)\n", __func__, reg);
+		goto out;
+	}
+
+	mutex_lock(&stfm1000->xfer_lock);
+
+	/* if the register can be written & is dirty, use the shadow */
+	if (stfm1000_reg_is_writeable(stfm1000, reg) &&
+		stfm1000_reg_is_dirty(stfm1000, reg)) {
+
+		*value = stfm1000_get_shadow_reg(stfm1000, reg);
+		ret = 0;
+
+		stfm1000_i2c_debug(stfm1000, "%s - HIT "
+			"(%s - 0x%02x, 0x%08x)\n", __func__,
+			reg_names[reg / 4], reg, *value);
+
+		goto out_unlock;
+	}
+
+	/* register must be read */
+	ret = read_reg_internal(stfm1000, reg, value);
+	if (ret < 0)
+		goto out;
+
+	/* if the register is writeable, update shadow */
+	if (stfm1000_reg_is_writeable(stfm1000, reg)) {
+		stfm1000_set_shadow_reg(stfm1000, reg, *value);
+		stfm1000_reg_set_dirty(stfm1000, reg);
+	}
+
+out_unlock:
+	mutex_unlock(&stfm1000->xfer_lock);
+
+out:
+	if (ret < 0)
+		dev_err(&stfm1000->client->dev, "%s: failed", __func__);
+
+	return ret;
+}
+
+int stfm1000_write_masked(struct stfm1000 *stfm1000, int reg, u32 value,
+	u32 mask)
+{
+	int ret = 0;
+	u32 old_value;
+
+	if (!stfm1000_reg_is_writeable(stfm1000, reg)) {
+		ret = -EINVAL;
+		printk(KERN_ERR "%s: !writeable(%d)\n", __func__, reg);
+		goto out;
+	}
+
+	mutex_lock(&stfm1000->xfer_lock);
+
+	/* if the register wasn't written before, read it */
+	if (!stfm1000_reg_is_dirty(stfm1000, reg)) {
+		ret = read_reg_internal(stfm1000, reg, &old_value);
+		if (ret != 0)
+			goto out_unlock;
+	} else	/* register was written, use the last value */
+		old_value = stfm1000_get_shadow_reg(stfm1000, reg);
+
+	/* perform masking */
+	value = (old_value & ~mask) | (value & mask);
+
+	/* if we write the same value, don't bother */
+	if (stfm1000_reg_is_dirty(stfm1000, reg) && value == old_value) {
+		ret = 0;
+
+		stfm1000_i2c_debug(stfm1000, "%s - HIT "
+			"(%s - 0x%02x, 0x%08x)\n", __func__,
+			reg_names[reg / 4], reg, value);
+
+		goto out_unlock;
+	}
+
+	/* actually write the register to the chip */
+	ret = write_reg_internal(stfm1000, reg, value);
+	if (ret < 0)
+		goto out_unlock;
+
+	/* if no error, update the shadow register and mark it as dirty */
+	stfm1000_set_shadow_reg(stfm1000, reg, value);
+	stfm1000_reg_set_dirty(stfm1000, reg);
+
+out_unlock:
+	mutex_unlock(&stfm1000->xfer_lock);
+
+out:
+	if (ret < 0)
+		dev_err(&stfm1000->client->dev, "%s: failed", __func__);
+
+	if (verify_writes) {
+		u32 value2 = ~0;
+
+		stfm1000_raw_read(stfm1000, reg, &value2);
+
+		stfm1000_i2c_debug(stfm1000, "%s - VER "
+			"(%s - 0x%02x, W=0x%08x V=0x%08x) %s\n", __func__,
+			reg_names[reg / 4], reg, value, value2,
+			value == value2 ? "OK" : "** differs **");
+	}
+
+	return ret;
+}
+
+int stfm1000_set_bits(struct stfm1000 *stfm1000, int reg, u32 value)
+{
+	return stfm1000_write_masked(stfm1000, reg, value, value);
+}
+
+int stfm1000_clear_bits(struct stfm1000 *stfm1000, int reg, u32 value)
+{
+	return stfm1000_write_masked(stfm1000, reg, ~value, value);
+}
+
+int stfm1000_write_regs(struct stfm1000 *stfm1000,
+	const struct stfm1000_reg *reg)
+{
+	int ret;
+
+	for (; reg && reg->regno != STFM1000_REG_END; reg++) {
+
+		if (reg->regno == STFM1000_REG_DELAY) {
+			msleep(reg->value);
+			continue;
+		}
+
+		if (reg->regno & STFM1000_REG_SET_BITS_MASK)
+			ret = stfm1000_set_bits(stfm1000, reg->regno & 0xff,
+				reg->value);
+		else if (reg->regno & STFM1000_REG_CLEAR_BITS_MASK)
+			ret = stfm1000_clear_bits(stfm1000, reg->regno & 0xff,
+				reg->value);
+		else
+			ret = stfm1000_write(stfm1000, reg->regno, reg->value);
+
+		if (ret != 0) {
+			printk(KERN_ERR "%s: failed to write reg 0x%x "
+				"with 0x%08x\n",
+				__func__, reg->regno, reg->value);
+			return ret;
+		}
+	}
+	return 0;
+}
+
diff --git a/drivers/media/radio/stfm1000/stfm1000-rds.c b/drivers/media/radio/stfm1000/stfm1000-rds.c
new file mode 100644
index 000000000000..5f63052d00c2
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-rds.c
@@ -0,0 +1,1529 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/init.h>
+
+#include "stfm1000.h"
+
+#include "stfm1000-rds.h"
+
+#define bitstream_to_rds_state(b)	\
+	container_of(b, struct stfm1000_rds_state, bitstream)
+#define demod_to_rds_state(d)		\
+	container_of(d, struct stfm1000_rds_state, demod)
+#define pkt_to_rds_state(p)		\
+	container_of(p, struct stfm1000_rds_state, pkt)
+#define text_to_rds_state(t)		\
+	container_of(t, struct stfm1000_rds_state, text)
+#define rds_state_to_stfm1000(r)	\
+	container_of(r, struct stfm1000, rds_state)
+
+#define TADJSH 8 /*Shifts used in bitslice loop filter */
+
+/* Reverse of Matlab's Fquant (see MatchedFilterDecomposition.m), so that */
+/* mixandsum code is easy;   Used by rds_bitstream_stfmdemod.arm */
+const s16 u16_rds_basis[2*RDS_BASISLENGTH+8] = {
+	14, 24, 34, 43, 50, 56, 60, 62, 62,
+	60, 55, 49, 41, 32, 22, 11, 14, 24,
+	34, 43, 50, 56, 60, 62, 62, 60, 55,
+	49, 41, 32, 22, 11, 14, 24, 34, 43,
+	50, 56, 60, 62
+};
+
+static int bits_free(struct stfm1000_rds_bitstream *rdsb)
+{
+	/* Do not show the last one word free. */
+	int FreeSpace = rdsb->TailBitCount - rdsb->HeadBitCount - 32;
+
+	if (FreeSpace < 0)
+		FreeSpace = (RDS_BITBUFSIZE * 32) + FreeSpace;
+	return FreeSpace;
+}
+
+static void put1bit(struct stfm1000_rds_bitstream *rdsb, int bit)
+{
+	int index = (rdsb->HeadBitCount >> 5);
+	u32 CurBit = (rdsb->HeadBitCount & 0x1f);
+	u32 CurWord = rdsb->buf[index];
+
+	if (CurBit == 0)
+		CurWord = 0;
+
+	CurWord = CurWord | (((u32)bit & 1) << CurBit);
+	rdsb->buf[index] = CurWord;
+	rdsb->HeadBitCount++;
+	if (rdsb->HeadBitCount >= RDS_BITBUFSIZE * 32)
+		rdsb->HeadBitCount = 0;
+}
+
+static int get1bit(struct stfm1000_rds_bitstream *rdsb)
+{
+	int Bit = 0;
+	int index = (rdsb->TailBitCount >> 5);
+	int CurBit = (rdsb->TailBitCount & 0x1f);
+	u32 CurWord = rdsb->buf[index];
+
+	Bit = (CurWord >> CurBit) & 1;
+	rdsb->TailBitCount++;
+	if (rdsb->TailBitCount == RDS_BITBUFSIZE*32)
+		rdsb->TailBitCount = 0;
+
+	return Bit;
+}
+
+static int bits_filled(struct stfm1000_rds_bitstream *rdsb)
+{
+	int FilledSpace = rdsb->HeadBitCount - rdsb->TailBitCount;
+
+	if (FilledSpace < 0)
+		FilledSpace = (RDS_BITBUFSIZE * 32) + FilledSpace;
+	return FilledSpace;
+}
+
+static void rds_mix_msg(struct stfm1000_rds_demod *rdsd, u8 MixSetting)
+{
+	if (rdsd->mix_msg_pending)
+		rdsd->mix_msg_overrun++;
+	rdsd->mix_msg = MixSetting;
+	rdsd->mix_msg_pending = 1;
+
+	/* signal monitor thread */
+	stfm1000_monitor_signal(
+		rds_state_to_stfm1000(demod_to_rds_state(rdsd)),
+		EVENT_RDS_MIXFILT);
+}
+
+/* call with interrupts disabled please */
+int stfm1000_rds_mix_msg_get(struct stfm1000_rds_state *rds)
+{
+	struct stfm1000_rds_demod *rdsd = &rds->demod;
+
+	if (!rdsd->mix_msg_pending)
+		return -1;
+
+	return rdsd->mix_msg;
+}
+
+/* call with interrupts disabled please */
+int stfm1000_rds_mix_msg_processed(struct stfm1000_rds_state *rds, int mix_msg)
+{
+	struct stfm1000_rds_demod *rdsd = &rds->demod;
+
+	if (!rdsd->mix_msg_pending)
+		return -1;
+
+	rdsd->mix_msg_pending = 0;
+
+	/* update the completion indication bit */
+	if ((mix_msg & 0x8) == 0)
+		rdsd->MixPopDone = 1;
+
+	/* this is reflected off the hardware register */
+	rdsd->rds_mix_offset = mix_msg & 1;
+
+	if (rdsd->mix_msg != mix_msg) {
+		rdsd->mix_msg_processed_changed++;
+		return -1;
+	}
+	return 0;
+}
+
+static void rds_sdnominal_msg(struct stfm1000_rds_demod *rdsd, int sdnominal)
+{
+	if (rdsd->sdnominal_msg_pending)
+		rdsd->sdnominal_msg_overrun++;
+	rdsd->sdnominal_msg = sdnominal;
+	rdsd->sdnominal_msg_pending = 1;
+
+	/* signal monitor thread */
+	stfm1000_monitor_signal(
+		rds_state_to_stfm1000(demod_to_rds_state(rdsd)),
+		EVENT_RDS_SDNOMINAL);
+}
+
+/* call with interrupts disabled please */
+int stfm1000_rds_sdnominal_msg_get(struct stfm1000_rds_state *rds)
+{
+	struct stfm1000_rds_demod *rdsd = &rds->demod;
+
+	if (!rdsd->sdnominal_msg_pending)
+		return 0;
+
+	return rdsd->sdnominal_msg;
+}
+
+/* call with interrupts disabled please */
+int stfm1000_rds_sdnominal_msg_processed(struct stfm1000_rds_state *rds,
+	int sdnominal_msg)
+{
+	struct stfm1000_rds_demod *rdsd = &rds->demod;
+
+	if (!rdsd->sdnominal_msg_pending)
+		return -1;
+
+	rdsd->sdnominal_msg_pending = 0;
+	return 0;
+}
+
+void demod_loop(struct stfm1000_rds_bitstream *rdsb,
+	struct stfm1000_rds_demod *rdsd)
+{
+	s32 filter_out;
+	u32 freeSpace;
+	s32 decomp_hist_pp;
+	u8 phase;
+
+	/* Check if we're at a half-basis point */
+	if ((rdsd->i & (RDS_BASISLENGTH/2 - 1)) != 0)
+		return;	/* Nope, return */
+
+	/* Yes, time to do our work */
+	/* Rotate the length 3 history buffer */
+	decomp_hist_pp = rdsd->decomp_hist_p;
+	rdsd->decomp_hist_p = rdsd->decomp_hist;
+	if ((rdsd->i & (RDS_BASISLENGTH-1)) == 0) {
+		rdsd->decomp_hist = rdsd->mixandsum1>>9; /* Grab output of
+							  * mixandsum1/512 */
+		rdsd->mixandsum1 = 0;	 /* Reset mixandsum #1 */
+	} else {
+		rdsd->decomp_hist = rdsd->mixandsum2>>9; /*Grab output of
+							  * mixandsum2/512 */
+		rdsd->mixandsum2 = 0;	/* Reset mixandsum #2 */
+	}
+
+	/* Form correlator/decimator output by convolving with the
+	 * decomposition coefficients, DecompQuant from Matlab work. */
+	filter_out = (-58*rdsd->decomp_hist + 59*decomp_hist_pp)>>7;
+
+	/*Figure out which half-basis we are in (out of a bit-length cycle) */
+	phase = rdsd->i*2/RDS_BASISLENGTH;
+	/*Now what we do depends on the phase variable */
+	/*Phase 0:      Bitslice and do timing alignment */
+	/*others (1-3): Keep value for timing alignment */
+
+	if (phase == 0) {   /*Main processing (bitslice) */
+		u32 Ph;
+		u8 OldBit = rdsd->sliced_data;	/* Save the previous value */
+
+		rdsd->return_num = 1;
+		if (filter_out >= 0) { /*This bit is "1" */
+			/*return value is XOR of previous bit (still in
+			 * sliced_data) w/ this */
+			/*  bit (1), which equals (NOT of the previous bit) */
+			rdsd->return_rdsdemod = !OldBit;
+			rdsd->sliced_data = 1;	/*Newest bit value is 1 */
+		} else { /*This bit is "0" */
+			/*return value is XOR of previous bit (still in
+			 * sliced_data) w/ this */
+			/*  bit (0), which equals the previous bit */
+			rdsd->return_rdsdemod = OldBit;
+			rdsd->sliced_data = 0;	/*Newest bit value is 0 */
+		}
+
+		freeSpace = bits_free(rdsb);
+
+		if (freeSpace > 0)
+			put1bit(rdsb, rdsd->return_rdsdemod);
+		else
+			rdsd->RdsDemodSkippedBitCnt++;
+
+		/*Increment bits received counter */
+		rdsd->BitAlignmentCounter++;
+		/*If mixer phase determination hasn't been done, start it */
+		if ((rdsd->MixPhaseState == 0) && (!rdsd->MixPhaseDetInProg)) {
+			rdsd->MixPhaseDetInProg = 1;
+			/*Go to first mixer setting (0) */
+			rds_mix_msg(rdsd, 0);
+		}
+
+		/* Do bit-slicing time adaption after the mixer phase
+		 * determination */
+		if (!(rdsd->MixPhaseDetInProg) && !(rdsd->Synchronous)) {
+
+			/* Bitslice Timing Adjust Code (runs after
+			 * MixPhaseDetInProg and if RDS is not synchronous to
+			 * the FM pilot. */
+
+			u8 BigPh2;	/* Expecting a large value in
+					 * PhaseValue[2] */
+			u32 MaxRMS = 0;	/*Largest phase RMS */
+			s8 MaxPh = 0;	/*Index of largest phase RMS */
+			s32 zerocross;
+
+			/* Locate the largest phase RMS
+			 * (should be at phase zero) */
+			for (Ph = 0; Ph < 4; Ph++)
+				if (rdsd->Ph_RMS[Ph] > MaxRMS) {
+					MaxRMS = rdsd->Ph_RMS[Ph];
+					MaxPh = Ph;
+				}
+
+			/* During each bit time we expect the four phases to
+			 * take one of the following patterns, where 1
+			 * corresponds to maximum modulation:
+			 *   1,    0,  -1,    0			Case I
+			 *  -1,    0,   1,    0			Case II
+			 *   1,  1/2,   0, -1/2			Case III
+			 *  -1, -1/2,   0,  1/2			Case IV
+			 * We need to distinguish between cases in order to do
+			 * the timing adjustment. Below we compare the
+			 * correlation of the samples with Case I and Case III
+			 * to see which has a bigger abs(correlation).  Thus
+			 * BigPh2, if set, means that we decided on Case I or
+			 * Case II; if BigPh2 clear, we decided Case III or IV.
+			 */
+			BigPh2 = abs(rdsd->PhaseValue[0]-rdsd->PhaseValue[2]) >
+				 abs(rdsd->PhaseValue[0] +
+				 ((rdsd->PhaseValue[1]-
+					rdsd->PhaseValue[3])>>1));
+			/* If BigPh2, use the difference between phase 1 value
+			 * (downgoing for Case I, upgoing for Case II) and
+			 * phase 3 value (upgoing for Case I, downgoing for
+			 * Case II, thus the subtraction) to indicate timing
+			 * error.  If not BigPh2, use the sum of the phase 1
+			 * value (downgoing for Case III, upgoing for Case IV)
+			 * and phase 3 value (downgoing for Case III, upgoing
+			 * for Case IV, thus the addition) to indicate timing
+			 * error.  If BigPh2, the slopes at phase 1 & phase 3
+			 * are approximately double that if not BigPh2.
+			 * Since we are trying to measure timing, scale
+			 * by 1/2 in the BigPh2 case. */
+			if (BigPh2)
+				zerocross = (rdsd->PhaseValue[1]-
+					rdsd->PhaseValue[3])>>1;
+			else
+				zerocross = rdsd->PhaseValue[1]+
+					rdsd->PhaseValue[3];
+			/* Now if the prev bit was a "1", then the first zero
+			 * crossing (phase 1 if BigPh2, phase 2 if !BigPh2)
+			 * was a falling one, and if we were late then
+			 * zerocross should be negative. If the prev bit was a
+			 * "0", then the first zero crossing was a rising one,
+			 * and if we were late then zerocross would be
+			 * positive. If we are "late" it means that we need to
+			 * do a shorter cycle of, say, 15 samples instead of
+			 * 16, to "catch up" so that in the future we will be
+			 * sampling earlier.  We shorten the cycle by adding
+			 * to i, so "late" is going to mean "increment i".
+			 * Therefore "late" should be positive, which is done
+			 * here by inverting zerocross if the previous bit was
+			 * 1.  You could say that this step reflects cases I
+			 * and III into II and IV, respectively. */
+			if (OldBit)
+				zerocross = -zerocross;
+			if (!rdsd->DisablePushing) {
+				/*The algorithm so far has a stable operating
+				 * point 17 phases away from the correct one.
+				 * The following code is experimental and may
+				 * be deleterious in low SNR conditions, but is
+				 * an attempt to move off of the incorrect
+				 * operating point. */
+
+				if (MaxPh != 0) {
+					/* If it isn't the same MaxPh as the
+					 * last non-zero one, clear the counter
+					 */
+					if (MaxPh != rdsd->PushLastMaxPh) {
+						/*Reset the counter */
+						rdsd->PushCounter = 0;
+						/*Record which phase we're now
+						 * counting */
+						rdsd->PushLastMaxPh = MaxPh;
+					}
+					/* If the Max RMS is on the same
+					 * non-zero phase, count up */
+					rdsd->PushCounter++;
+				}
+				/* Once every 128 bits, check and then reset
+				 * PushCounter */
+				if (!(rdsd->BitAlignmentCounter & 0x0FF)) {
+					/*If 90% of the time the max phase has
+					 * been from the same non-zero phase,
+					 * decide that we are latched onto a 0
+					 * lock point. Do a large push of the
+					 * timing. */
+					if (rdsd->PushCounter > 230) {
+						s32 pshiph;
+						/*Convert from phase number to
+						 * the number of filter
+						 *  output samples that we need
+						 *  to shift */
+						if (rdsd->PushLastMaxPh >= 2)
+							pshiph =
+								4 - (s8)rdsd->
+								PushLastMaxPh;
+						else
+							pshiph =
+								-(s8)rdsd->
+								PushLastMaxPh;
+						/* Scale by the number of i-
+						 * phases per output sample */
+						pshiph <<=
+							RDS_BASISSHIFTS-1;
+						/* Perform big pop to get near
+						 * correct timing */
+						rdsd->i += (RDS_BASISLENGTH<<1)
+							+ pshiph;
+						/* Set status indicating big
+						 * pop was needed.  Reset all
+						 * leaky-bucket and summation
+						 * variables because the big
+						 * timing shift has invalidated
+						 * them. Ph_RMS values don't
+						 * need to be reset because
+						 * they will shift over to
+						 * reasonable values again
+						 * before their erroneous
+						 * values could have effect. */
+						rdsd->rds_big_timeshift = 1;
+						/*rdsd->Ph_RMS[0] = 0; */
+						/*rdsd->Ph_RMS[1] = 0; */
+						/*rdsd->Ph_RMS[2] = 0; */
+						/*rdsd->Ph_RMS[3] = 0; */
+						rdsd->mixandsum1 = 0;
+						rdsd->mixandsum2 = 0;
+						rdsd->SkipsAccum +=
+							pshiph;
+
+						/* Make adjustments in other
+						 * values because of the push
+						 * (they wouldn't otherwise be
+						 * able to use the information
+						 * that a push was needed in
+						 * their future control
+						 * decisions). */
+						if (rdsd->PushLastMaxPh != 2) {
+							/* If we weren't
+							 * pushing from phase
+							 * two, accumulate (for
+							 * use in adapting
+							 * SDNOMINAL) the
+							 * phases moved by
+							 * pushing. Phase two
+							 * pushes are not used;
+							 * the push direction
+							 * is arbitrary since
+							 * Phase 2 is 180
+							 * degrees out.  Also,
+							 * phase 2 pushes don't
+							 * result from
+							 * reasonable slippage.
+							 * */
+
+							if (rdsd->sdnom_adapt)
+								rdsd->SdnomSk
+								+= pshiph;
+
+							/* Modify timing_adj to
+							 * account for half of
+							 * the DC response that
+							 * would have occurred
+							 * in timing_adj if
+							 * that control loop
+							 * had seen the push
+							 * happen. (Why half?
+							 * Because the loop has
+							 * already seen a
+							 * history of zerocross
+							 * values that heads it
+							 * in the same
+							 * direction as this
+							 * adjustment, but may
+							 * have seen as few as
+							 * half of what it
+							 * should have.) */
+							rdsd->timing_adj +=
+								pshiph <<
+								(TADJSH+1);
+						}
+						/*Set countdown timer that will
+						 * prevent any mixer popping
+						 * until the Ph_RMS variables
+						 * have had enough time to
+						 * stabilize */
+
+						/* 2.5 time constants */
+						rdsd->PushSafetyZone = 5;
+					}
+					/*Reset the push counter */
+					rdsd->PushCounter = 0;
+				}  /*end once every 128 bits */
+			}  /*end if !DisablePushing */
+
+			/* Further possible additions:
+			 *
+			 * 1. Pushes modify timing_adj to decrease convergence
+			 *    time.
+			 * 2. Separate timing_adj into pilottracking and non-pt
+			 *    cases (avoids convergence time after stereo/mono
+			 *    transitions)
+			 *
+			 * Old loop filter was a leaky bucket integrator, and
+			 * it always lagged behind if the FM station had RDS
+			 * asynchronous to the pilot, because the control loop
+			 * needs another integrator to converge on a frequency
+			 * error.
+			 * New loop filter = 1/(1-1/z) * (a-1/z) * k,
+			 * where a = 1+1/256 and k = 1/1024.
+			 * You can narrow the loop bandwidth by making "a"
+			 * twice as close to 1 and halving k, e.g. a = 1+1/512
+			 * and k = 1/2048.
+			 * (The way implemented, that narrowing loop BW by
+			 * a factor of 2 can be done by incrementing TADJSH.)
+			 *
+			 * TGR 8/31/2007 */
+
+			/*Integrator, 1/(1-1/z) */
+			rdsd->timing_adj += zerocross;
+			/*Limit to 1 phase every 8 samples */
+			if (rdsd->SkipSafetyZone) {
+				rdsd->SkipSafetyZone--;
+				rdsd->sampskip = 0;
+			} else {
+				/*sampskip of non-zero is allowed,
+				 * calculate what it really is */
+
+				/*Saturate timing_adj to 2's comp
+				 * (2*TADJSH+4)-bit range. */
+				if (rdsd->timing_adj > (1<<(2*TADJSH+3))-1)
+					rdsd->timing_adj = (1<<(2*TADJSH+3))-1;
+				if (rdsd->timing_adj < -(1<<(2*TADJSH+3)))
+					rdsd->timing_adj = -(1<<(2*TADJSH+3));
+
+				/* Zero, implemented after the integrator
+				 * output.
+				 * (a-1/z) = (1+1/256) - 1/z = (1-1/z) + 1/256.
+				 * But (1 - 1/z) is timing_adj-
+				 * prev_timing_adj = zerocross. */
+				rdsd->sampskip = zerocross	/* 1 - 1/z */
+					/* 1/256 (with rounding) */
+					+ ((rdsd->timing_adj
+						+ (1<<(TADJSH-1)))>>TADJSH);
+				/*Round and apply k */
+				rdsd->sampskip += (1<<(TADJSH+1));
+				rdsd->sampskip >>= (TADJSH+2);
+				/*Limit to [-1,+1] inclusive */
+				if (rdsd->sampskip > 1)
+					rdsd->sampskip = 1;
+				if (rdsd->sampskip < -1)
+					rdsd->sampskip = -1;
+				/* If non-zero, start the skip safety zone,
+				 * which excludes more sample skipping for a
+				 * while.  Note that the safety zone only
+				 * applies to the skips -- pushes can still
+				 * happen inside a SkipSafetyZone. */
+				if (rdsd->sampskip)
+					rdsd->SkipSafetyZone = 8-1;
+			}
+			/**********************************************
+			* End Timing Adjust Code
+			**********************************************/
+
+			/**********************************************
+			* Begin Phase Popper Code
+			**********************************************/
+			/* If Phase Popping is enabled and 1/2 of a
+			 * time constant has gone by... */
+			if (rdsd->PhasePoppingEnabled &&
+				!(rdsd->BitAlignmentCounter &
+					((1<<(RMSALPHASHIFTS-1))-1))) {
+
+				u8 ForcePop = 0; /* Used to force a pop */
+
+				/*Record the maximum of the envelope */
+				if (MaxRMS > rdsd->PhasePopMaxRMS)
+					rdsd->PhasePopMaxRMS = MaxRMS;
+				/* Also track MaxRMS into MixPhase0/1Mag, so
+				 * that we can see what the largest RMS on each
+				 * of those phases is.  On synchronous stations
+				 * (meaning the RDS carrier and bit rate are
+				 * synchronized with the pilot), the right mix
+				 * phase will always be big and the wrong phase
+				 * small. On asynchronous stations (and
+				 * stations without RDS), both phases will at
+				 * some time or other have about the
+				 * same amplitude on each of the phases. */
+				if (rdsd->rds_mix_offset) {
+					if (MaxRMS > rdsd->MixPhase1Mag)
+						rdsd->MixPhase1Mag = MaxRMS;
+				} else {
+					if (MaxRMS > rdsd->MixPhase0Mag)
+						rdsd->MixPhase0Mag = MaxRMS;
+				}
+				/* Update PopSafetyZone and PushSafetyZone
+				 * counters.  With RMSALPHASHIFTS = 5, each
+				 * tick is 16/1187.5 =~ 13.5 ms. */
+				if (rdsd->PopSafetyZone) {
+					rdsd->PopSafetyZone--;
+					/* If safety zone just ended and this
+					 * mix phase is giving smaller RMS than
+					 * before the pop, then the pop was a
+					 * mistake.  Go back to previous mixer
+					 * phase */
+					if (!(rdsd->PopSafetyZone)
+						&& (rdsd->PhasePopMaxRMS <
+							rdsd->PrePopRMS))
+						ForcePop = 1;
+				}
+				/* If there is no recent push, and Phase 0 has
+				 * the maximum RMS, and at least 1/7th of a
+				 * second has passed since the last phase pop,
+				 * and ((the RMS is less than 1/2 of
+				 * PhasePopMaxRMS) or (the RMS is less than
+				 * 100)), then try a phase pop. */
+				if (/* (rdsd->Ph_RMS[0] == MaxRMS) &&
+						* Phase 0 has maximum RMS  */
+					!(rdsd->PopSafetyZone))	{
+					 /* and Long enough since last
+					  * phase pop */
+
+					/* Eligible for a pop, see if one of
+					 * the pop conditions is met */
+					if ((MaxRMS<<1) <
+							rdsd->PhasePopMaxRMS) {
+						/*RMS decline from its peak */
+						ForcePop = 1;
+					} else if ((MaxRMS>>RMSALPHASHIFTS)
+						< 50) {
+						/*RMS too small to receive,
+						 * either there's no RDS or
+						 * this is the wrong phase */
+						ForcePop = 1;
+					}
+				}
+				if (ForcePop) {
+
+					/*Pop to opposite setting */
+					rds_mix_msg(rdsd, 0x8 |
+						!rdsd->rds_mix_offset);
+
+					/*Save the pre-pop RMS so that later we
+					 * can see if the pop was actually
+					 * effective */
+					rdsd->PrePopRMS = MaxRMS;
+					/*Reset the PhasePopMaxRMS.  We rely on
+					 * the PopSafetyZone to give time to
+					 * get a new valid max RMS before we're
+					 * eligible for the next phase pop.  If
+					 * there were no reset we'd be forever
+					 * incrementing PhasePopMaxRMS due
+					 * to just happenstance large-noise
+					 * samples and it might eventually get
+					 * some freakish large value causing
+					 * frequent erroneous pops. */
+					rdsd->PhasePopMaxRMS = 0;
+					/* Pop Safety zone length is decided by
+					 * how much of an asynchronous
+					 * frequency can be supported. Allowing
+					* 50 ppm of transmitter error (error
+					* between their own pilot, that we
+					* should be locked to, and their RDS
+					* carrier (which by RDS spec should be
+					* locked to their pilot, but we've
+					* recently found frequently isn't).
+					* 50ppm * 57kHz = 2.85Hz.
+					* (2.85 cycles/sec)(4 pops/cycle)
+					* = 11.4 pops/second.
+					* Safety zone = (1/11.4) seconds =~ 104
+					* bits, round down to 96 bits which
+					* yields 6 ticks if RMSALPHASHIFTS = 5.
+					* */
+					rdsd->PopSafetyZone = 96>>
+						(RMSALPHASHIFTS-1);
+				}
+			}
+			/******************************************************
+			* End Phase Popper Code
+			******************************************************/
+
+			/* SDNOMINAL adaption */
+			if (rdsd->sdnom_adapt) {
+				rdsd->SdnomSk += rdsd->sampskip;
+				if (rdsd->pCoefForcedMono &&
+					(rdsd->BitAlignmentCounter & 0xFFF) ==
+						0x800) {
+
+					rds_sdnominal_msg(rdsd,
+						-(rdsd->SdnomSk<<9));
+
+					/*Reset skips counter */
+					rdsd->SdnomSk = 0;
+				}
+			}
+
+			rdsd->SkipsAccum += rdsd->sampskip;
+			/* Once per 3.45 seconds, print out signal strength,
+			 * skips and pops. Then reset the variables totalling
+			 * those occurrences */
+			if (!(rdsd->BitAlignmentCounter & 0xFFF)) {
+				/* During very noisy input (or if no RDS, or no
+				 * station present), timing_adj can go crazy,
+				 * since it is the integral of noise.  Although
+				 * it is a saturated value (earlier, in the
+				 * timing adjust code), the level at which we
+				 * can saturate still leaves room for
+				 * timing_adj to get too big.  A large value of
+				 * timing_adj is a persistent pathology because
+				 * the phase is shifting so quickly that the
+				 * push detector (which relies on stable
+				 * phase-RMS values) never triggers, thus there
+				 * is no implemented rescue besides this
+				 * clearing that restores proper function. */
+				if (abs(rdsd->SkipsAccum) > 300)
+					rdsd->timing_adj = 0;
+				/*Reset the accumulations. */
+				rdsd->SkipsAccum = 0;
+			}
+		} /*End of bit timing adaption */
+
+		/* If mixer phase determination in progress,
+		 * perform actions at certain times */
+		if (rdsd->MixPhaseDetInProg) {
+			/*~10ms settling time after mixer phase change */
+			#define MIXPHASE_STARTMEAS 12
+			/*~20ms measurement window */
+			#define MIXPHASE_ENDMEAS (MIXPHASE_STARTMEAS+24)
+			if (rdsd->BitAlignmentCounter == MIXPHASE_STARTMEAS) {
+				/*Reset the RMS variables */
+				rdsd->Ph_RMS[0] = 0;
+				rdsd->Ph_RMS[1] = 0;
+				rdsd->Ph_RMS[2] = 0;
+				rdsd->Ph_RMS[3] = 0;
+				/* Don't reset mixandsum values because at
+				 * least they have filtered continuously.  All
+				 * we really need for the mixer phase decision
+				 * is a constant measurement window. */
+			} else if (rdsd->BitAlignmentCounter ==
+					MIXPHASE_ENDMEAS) {
+				/*Measurement = mean of RMS values */
+				u32 Ndx, MeasVal = 0;
+				for (Ndx = 0; Ndx < 4;
+					MeasVal += rdsd->Ph_RMS[Ndx++]>>2);
+				/*Store measurement in correct place */
+				if (rdsd->MixPhaseState == 1) {
+					rdsd->MixPhase0Mag = MeasVal;
+					/*Go to next mixer setting */
+					rds_mix_msg(rdsd, 1);
+				} else if (rdsd->MixPhaseState == 2) {
+					u8 NextMixSetting;
+					rdsd->MixPhase1Mag = MeasVal;
+					/* Both measurements done now, see what
+					 * mixer setting we need to use.
+					 * 0 if MixPhase0Mag > MixPhase1Mag,
+					 * 1 otherwise. */
+					NextMixSetting = (rdsd->MixPhase0Mag
+						<= rdsd->MixPhase1Mag);
+					/* If the mixer setting needed is 1,
+					 * that is already the current setting.
+					 * Terminate mixer phase determination.
+					 * Otherwise send message to switch the
+					 * mixer phase setting. */
+					if (NextMixSetting) {
+						rdsd->MixPhaseState = 3;
+						rdsd->MixPhaseDetInProg = 0;
+					} else
+						rds_mix_msg(rdsd, 0);
+				}
+			}
+			/* Reset BitAlignmentCounter if the Mixer just popped
+			 * Change state, if required.  States are:
+			 * 0: Initial state, send msg causing RDS_MIXOFFSET=>0
+			 * 1: Measure with RDS_MIXOFFSET = 0.
+			 * 	Lasts just over 30 ms.
+			 * 2: Measure with RDS_MIXOFFSET = 1.
+			 * 	Lasts just over 30 ms.
+			 * 3: At final RDS_MIXOFFSET value.
+			 * 	Lasts as long as RDS continues. */
+			if (rdsd->MixPopDone) {
+				rdsd->MixPopDone = 0;
+				rdsd->BitAlignmentCounter = 0;
+				rdsd->MixPhaseState++;	/*Go to next state */
+				/* If we got to state 3, turn off mixer phase
+				 * determination code */
+				if (rdsd->MixPhaseState == 3)
+					rdsd->MixPhaseDetInProg = 0;
+			}
+		}
+
+		/* Update status variables */
+		rdsd->RDS_BIT_AMP_STAT_REG9 = rdsd->Ph_RMS[0]>>RMSALPHASHIFTS;
+		/*Saturate */
+		if (rdsd->RDS_BIT_AMP_STAT_REG9 > 511)
+			rdsd->RDS_BIT_AMP_STAT_REG9 = 511;
+	}  /*End phase 0 code */
+
+	/***************************************************
+	* Actions common to all phases
+	***************************************************/
+
+	/* Save the output of each phase for possible
+	 * calculations during phase 0 */
+	rdsd->PhaseValue[phase] = filter_out;
+
+	/*So that we can measure signal amplitude and/or determine what (if */
+	/*  any) big jump is needed, maintain the RMS of each phase.  Phase */
+	/*  0 RMS is already in Ph_RMS[0] (see bitslicing code, earlier). */
+	rdsd->Ph_RMS[phase] += abs(filter_out) -
+		(rdsd->Ph_RMS[phase]>>RMSALPHASHIFTS);
+}
+
+#if defined(CONFIG_ARM)
+
+/* assembly version for ARM */
+#define RDS_MAC(_acc, _x, _y) \
+	__asm__ __volatile__ (			 \
+	"smlabb	%0, %1, %2, %0\n"		 \
+	: "=&r" (_acc)				 \
+	: "r" (_x), "r" (_y)			 \
+	: "cc")
+
+#else
+
+/* all others, use standard C */
+#define RDS_MAC(_acc, _x, _y) 			 \
+	do {					 \
+		(_acc) += (s16)(_x) * (s16)(_y); \
+	} while (0)
+
+#endif
+
+static void rds_demod(const u16 *data, struct stfm1000_rds_demod *rdsd,
+	struct stfm1000_rds_bitstream *rbit, int total)
+{
+	register const s16 *basis0;
+	register const s16 *basis1;
+	register s16 val;
+	register int i;
+	register int sampskip;
+	register s32 acc1;
+	register s32 acc2;
+
+	/* point to the table */
+	basis0 = u16_rds_basis;
+	basis1 = basis0 + 8;
+
+	rdsd->return_num = 0;
+
+	/* restore state */
+	i = rdsd->i;
+	acc1 = rdsd->mixandsum1;
+	acc2 = rdsd->mixandsum2;	/* 64 bit */
+	sampskip = rdsd->sampskip;
+
+	while (total-- > 0) {
+
+		val = data[3];	/* load RDS data */
+		data += 4;
+		if (val == 0x7fff)	/* illegal RDS sample */
+			continue;
+
+		RDS_MAC(acc1, val, basis0[i]);
+		RDS_MAC(acc2, val, basis1[i]);
+
+		if (i == 4) {
+			i += sampskip;
+			sampskip = 0;
+		}
+
+		if ((i & (RDS_BASISLENGTH / 2 - 1)) == 0) {
+
+			/* save state */
+			rdsd->mixandsum1 = acc1;
+			rdsd->mixandsum2 = acc2;
+			rdsd->i = i;
+			rdsd->sampskip = sampskip;
+
+			demod_loop(rbit, rdsd);
+
+			/* restore state */
+			acc1 = rdsd->mixandsum1;
+			acc2 = rdsd->mixandsum2;
+			i = rdsd->i;
+			sampskip = rdsd->sampskip;
+		}
+		i = (i + 1) & 31;
+	}
+
+	/* save state */
+	rdsd->mixandsum1 = acc1;
+	rdsd->mixandsum2 = acc2;
+	rdsd->i = i;
+	rdsd->sampskip = sampskip;
+}
+
+void stfm1000_rds_demod(struct stfm1000_rds_state *rds, const u16 *dri_data,
+	int total)
+{
+	rds_demod(dri_data, &rds->demod, &rds->bitstream, total);
+
+	/* signal only when we have enough */
+	if (bits_filled(&rds->bitstream) > 128)
+		stfm1000_monitor_signal(rds_state_to_stfm1000(rds),
+				EVENT_RDS_BITS);
+}
+
+static void bitstream_reset(struct stfm1000_rds_bitstream *rdsb)
+{
+	memset(rdsb, 0, sizeof(*rdsb));
+}
+
+static void demod_reset(struct stfm1000_rds_demod *rdsd)
+{
+	memset(rdsd, 0, sizeof(*rdsd));
+	rdsd->sdnom_adapt = 0;	/* XXX this doesn't really work right */
+					/* it causes underruns at ALSA */
+	rdsd->PhasePoppingEnabled = 1;	/* does this? */
+}
+
+static void packet_reset(struct stfm1000_rds_pkt *rdsp)
+{
+	memset(rdsp, 0, sizeof(*rdsp));
+	rdsp->state = SYNC_OFFSET_A;
+}
+
+static void text_reset(struct stfm1000_rds_text *rdst)
+{
+	memset(rdst, 0, sizeof(*rdst));
+}
+
+void stfm1000_rds_reset(struct stfm1000_rds_state *rds)
+{
+	bitstream_reset(&rds->bitstream);
+	demod_reset(&rds->demod);
+	packet_reset(&rds->pkt);
+	text_reset(&rds->text);
+	rds->reset_req = 0;
+}
+
+int stfm1000_rds_bits_available(struct stfm1000_rds_state *rds)
+{
+	return bits_filled(&rds->bitstream);
+}
+
+int stmf1000_rds_get_bit(struct stfm1000_rds_state *rds)
+{
+	if (bits_filled(&rds->bitstream) == 0)
+		return -1;
+	return get1bit(&rds->bitstream);
+}
+
+int stmf1000_rds_avail_bits(struct stfm1000_rds_state *rds)
+{
+	return bits_filled(&rds->bitstream);
+}
+
+static const u32 rds_ParityCheck[] = {
+	0x31B, 0x38F, 0x2A7, 0x0F7, 0x1EE,
+	0x3DC, 0x201, 0x1BB, 0x376, 0x355,
+	0x313, 0x39F, 0x287, 0x0B7, 0x16E,
+	0x2DC, 0x001, 0x002, 0x004, 0x008,
+	0x010, 0x020, 0x040, 0x080, 0x100,
+	0x200
+};
+
+static int calc_syndrome(u32 rdscrc)
+{
+	int i;
+	u32 syndrome = 0;
+	int word = 0x1;
+
+	for (i = 0; i < 26; i++) {
+		if (rdscrc & word)
+			syndrome ^= rds_ParityCheck[i];
+		word <<= 1;
+	}
+	return syndrome;
+}
+
+static u32 ecc_table[1024];
+static int ecc_table_generated;
+
+static void generate_ecc_table(void)
+{
+	int i, j, size;
+	u32 syndrome, word;
+
+	for (i = 0; i < ECC_TBL_SIZE; i++)
+		ecc_table[i] = 0xFFFFFFFF;
+	ecc_table[0] = 0x0;
+
+	for (j = 0; j < 5; j++) {
+		word = (1 << (j + 1)) - 1;	/* 0x01 0x03 0x07 0x0f 0x1f */
+		size = 26 - j; 			/* 26, 25, 24, 23, 22 */
+		syndrome = 0;
+		for (i = 0; i < size; i++) {
+			syndrome = calc_syndrome(word);
+			ecc_table[syndrome] = word;
+			word <<= 1;
+		}
+	}
+}
+
+static u32 ecc_correct(u32 rdsBits, int *recovered)
+{
+	u32 syndrome;
+	u32 errorBits;
+
+	if (recovered)
+		*recovered = 0;
+
+	/* Calculate Syndrome on Received Packet */
+	syndrome = calc_syndrome(rdsBits);
+
+	if (syndrome == 0)
+		return rdsBits; /* block is clean */
+
+	/* generate table first time we get here */
+	if (!ecc_table_generated) {
+		generate_ecc_table();
+		ecc_table_generated = 1;
+	}
+
+	/* Attempt to recover block */
+	errorBits = ecc_table[syndrome];
+	if (errorBits == UNRECOVERABLE_RDS_BLOCK)
+		return UNRECOVERABLE_RDS_BLOCK;	/* Block can not be recovered.
+						 * it is bad packet */
+
+	rdsBits = rdsBits ^ errorBits;
+	if (recovered)
+		(*recovered)++;
+	return rdsBits;                 /* ECC correct */
+}
+
+/* The following table lists the RDS and RBDS Program Type codes
+ * and their meanings:
+ * PTY code RDS Program type RBDS Program type */
+static const struct stfm1000_rds_pty stc_tss_pty_tab[] = {
+	{  0, "No program type",    "No program type"},
+	{  1, "News",               "News"},
+	{  2, "Current affairs",    "Information"},
+	{  3, "Information",        "Sports"},
+	{  4, "Sports",             "Talk"},
+	{  5, "Education",          "Rock"},
+	{  6, "Drama",              "Classic Rock"},
+	{  7, "Culture",            "Adult Hits"},
+	{  8, "Science",            "Soft Rock"},
+	{  9, "Varied",             "Top 40"},
+	{ 10, "Pop",                "Music Country"},
+	{ 11, "Rock",               "Music Oldies"},
+	{ 12, "M.O.R.",             "Music Soft"},
+	{ 13, "Light classical",    "Nostalgia"},
+	{ 14, "Serious",            "Classical Jazz"},
+	{ 15, "Other Music",        "Classical"},
+	{ 16, "Weather",            "Rhythm and Blues"},
+	{ 17, "Finance",            "Soft Rhythm and Blues"},
+	{ 18, "Children's programs", "Language"},
+	{ 19, "Social Affairs",     "Religious Music"},
+	{ 20, "Religion",           "Religious Talk"},
+	{ 21, "Phone In",           "Personality"},
+	{ 22, "Travel",             "Public"},
+	{ 23, "Leisure",            "College"},
+	{ 24, "Jazz Music",         "Unassigned"},
+	{ 25, "Country Music",      "Unassigned"},
+	{ 26, "National Music",     "Unassigned"},
+	{ 27, "Oldies Music",       "Unassigned"},
+	{ 28, "Folk Music",         "Unassigned"},
+	{ 29, "Documentary",        "Weather"},
+	{ 30, "Alarm Test",         "Emergency Test"},
+	{ 31, "Alarm",              "Emergency"},
+};
+
+#if 0
+static const char *rds_group_txt[] = {
+	[RDS_GROUP_TYPE_0A] = "Basic tuning and switching information (0A)",
+	[RDS_GROUP_TYPE_0B] = "Basic tuning and switching information (0B)",
+	[RDS_GROUP_TYPE_1A] = "Program item number and slow labeling codes",
+	[RDS_GROUP_TYPE_1B] = "Program item number",
+	[RDS_GROUP_TYPE_2A] = "Radio Text (2A)",
+	[RDS_GROUP_TYPE_2B] = "Radio Text (2B)",
+	[RDS_GROUP_TYPE_3A] = "Application identification for ODA only",
+	[RDS_GROUP_TYPE_3B] = "Open data applications",
+	[RDS_GROUP_TYPE_4A] = "Clock-time and date",
+	[RDS_GROUP_TYPE_4B] = "Open data applications",
+	[RDS_GROUP_TYPE_5A] = "Transparent Data Channels (32 ch.) or ODA (5A)",
+	[RDS_GROUP_TYPE_5B] = "Transparent Data Channels (32 ch.) or ODA (5B)",
+	[RDS_GROUP_TYPE_6A] = "In House Applications or ODA (6A)",
+	[RDS_GROUP_TYPE_6B] = "In House Applications or ODA (6B)",
+	[RDS_GROUP_TYPE_7A] = "Radio Paging or ODA",
+	[RDS_GROUP_TYPE_7B] = "Open Data Applications",
+	[RDS_GROUP_TYPE_8A] = "Traffic Message Channel or ODA",
+	[RDS_GROUP_TYPE_8B] = "Open Data Applications",
+	[RDS_GROUP_TYPE_9A] = "Emergency warning system or ODA",
+	[RDS_GROUP_TYPE_9B] = "Open Data Applications",
+	[RDS_GROUP_TYPE_10A] = "Program Type Name",
+	[RDS_GROUP_TYPE_10B] = "Open Data Applications (10B)",
+	[RDS_GROUP_TYPE_11A] = "Open Data Applications (11A)",
+	[RDS_GROUP_TYPE_11B] = "Open Data Applications (11B)",
+	[RDS_GROUP_TYPE_12A] = "Open Data Applications (12A)",
+	[RDS_GROUP_TYPE_12B] = "Open Data Applications (12B)",
+	[RDS_GROUP_TYPE_13A] = "Enhanced Radio Paging or ODA",
+	[RDS_GROUP_TYPE_13B] = "Open Data Applications",
+	[RDS_GROUP_TYPE_14A] = "Enhanced Other Networks information (14A)",
+	[RDS_GROUP_TYPE_14B] = "Enhanced Other Networks information (14B)",
+	[RDS_GROUP_TYPE_15A] = "Defined in RBDS",
+	[RDS_GROUP_TYPE_15B] = "Fast switching information",
+};
+#endif
+
+static void dump_rds_packet(u8 *buf)
+{
+	u16 pi, offb;
+
+	pi   = (u16)(buf[0] << 8) | buf[1];
+	offb = (u16)(buf[1] << 8) | buf[2];
+
+	printk(KERN_INFO "GRP: "
+		"PI=0x%04x "
+		"GT=%2d VER=%d TP=%d PTY=%2d "
+		"PS_SEG=%2d RT_AB=%2d RT_SEG=%2d\n", pi,
+		RDS_GROUP_TYPE(offb), RDS_VERSION(offb), RDS_TP(offb),
+		RDS_PTY(offb),
+		RDS_PS_SEG(offb), RDS_RT_AB(offb), RDS_RT_SEG(offb));
+}
+
+void stfm1000_rds_process_packet(struct stfm1000_rds_state *rds, u8 *buffer)
+{
+	struct stfm1000_rds_text *rdst = &rds->text;
+	/* char tempCallLetters[5] = {0}; */
+	struct rds_group_data grp;
+	int grpno;
+	u32 offset;
+	char tps[9];
+	int i, seg, idx;
+
+	grp.piCode  = ((u16)buffer[0] << 8) | buffer[1];
+	grp.offsetB = ((u16)buffer[2] << 8) | buffer[3];
+	grp.offsetC = ((u16)buffer[4] << 8) | buffer[5];
+	grp.offsetD = ((u16)buffer[6] << 8) | buffer[7];
+
+	grpno = (grp.offsetB >> (8 + 3)) & 0x1f;
+
+	if (rds_state_to_stfm1000(rds)->rds_info)
+		dump_rds_packet(buffer);
+
+	/* Is this the first time through? */
+	if (!rdst->bRds_detected) {
+		rdst->pi            = grp.piCode;
+		rdst->tp            = RDS_TP(grp.offsetB);
+		rdst->version       = RDS_VERSION(grp.offsetB);
+		rdst->pty.id        = RDS_PTY(grp.offsetB);
+		rdst->pty.pRds      = stc_tss_pty_tab[rdst->pty.id].pRds;
+		rdst->pty.pRdbs     = stc_tss_pty_tab[rdst->pty.id].pRdbs;
+		rdst->bRds_detected = 1;
+	}
+
+	/* Have we process too many PI errors? */
+	if (grp.piCode != rdst->pi) {
+		if (rdst->mismatch++ > 10) {
+
+			/* requested reset of RDS */
+			rds->reset_req = 1;
+
+			/* signal monitor thread */
+			stfm1000_monitor_signal(rds_state_to_stfm1000(rds),
+				EVENT_RDS_RESET);
+
+			if (rds_state_to_stfm1000(rds)->rds_info)
+				printk(KERN_INFO "RDS: RESET!!!\n");
+
+			text_reset(rdst);
+		}
+		rdst->consecutiveGood = 0;
+		return;
+	}
+
+	if (rdst->consecutiveGood++ > 10)
+		rdst->mismatch = 0;            /* reset bad count */
+
+	if (rdst->consecutiveGood > rdst->consecutiveGoodMax)
+		rdst->consecutiveGoodMax = rdst->consecutiveGood;
+
+	switch (grpno) {
+	case RDS_GROUP_TYPE_0A:
+	case RDS_GROUP_TYPE_0B:
+		/* Extract Service Name information */
+		offset = RDS_PS_SEG(grp.offsetB) * 2;
+		rdst->wk_ps[offset]     = buffer[6];	/* better */
+		rdst->wk_ps[offset + 1] = buffer[7];
+		rdst->wk_ps_mask |= 1 << RDS_PS_SEG(grp.offsetB);
+
+		if (rds_state_to_stfm1000(rds)->rds_info) {
+			for (i = 0; i < 8; i++) {
+				if (rdst->wk_ps_mask & (1 << i)) {
+					tps[i * 2] =
+						rdst->wk_ps[i * 2];
+					tps[i * 2 + 1] =
+						rdst->wk_ps[i * 2 + 1];
+				} else {
+					tps[i * 2] = '_';
+					tps[i * 2 + 1] = '_';
+				}
+			}
+			tps[ARRAY_SIZE(tps) - 1] = '\0';
+			if (rds_state_to_stfm1000(rds)->rds_info)
+				printk(KERN_INFO "RDS-PS (curr): %s\n", tps);
+		}
+
+		if (rdst->wk_ps_mask != ALL_SEGMENT_BITS)
+			break;
+
+		if (rdst->ps_valid) {
+			if (memcmp(rdst->ps, rdst->wk_ps, 8) != 0) {
+				memset(rdst->cp_ps, 0, 8);
+				memset(rdst->wk_ps, 0, 8);
+				rdst->wk_ps_mask = 0;
+			}
+
+			memset(rdst->ps, 0, 8);
+			rdst->ps_valid = 0;
+			break;
+		}
+
+		/* does working buffer == compare buffer */
+		if (memcmp(rdst->cp_ps, rdst->wk_ps, 8) != 0) {
+			/* just copy from working to compare buffer */
+			memcpy(rdst->cp_ps, rdst->wk_ps, 8);
+			rdst->wk_ps_mask = 0;
+			break;
+		}
+
+		/* working buffer matches compare buffer, send to UI */
+		memcpy(rdst->ps, rdst->cp_ps, 8);
+		rdst->ps_valid = 1;
+
+		if (rds_state_to_stfm1000(rds)->rds_info)
+			printk(KERN_INFO "RDS: PS '%s'\n", rdst->ps);
+
+		/* clear working mask-only */
+		rdst->wk_ps_mask = 0;
+		break;
+
+	case RDS_GROUP_TYPE_2A:
+
+		/* Clear buffer */
+		if (rdst->textAB_flag != RDS_RT_AB(grp.offsetB)) {
+			memset(rdst->wk_text, 0, 64);
+			rdst->wk_text_mask = 0;
+			rdst->textAB_flag  = RDS_RT_AB(grp.offsetB);
+		}
+
+		/* Extract Text */
+		seg = RDS_RT_SEG(grp.offsetB);
+		idx = seg * 4;
+
+		#define CNVT_EOT(x) ((x) != RDS_EOT ? (x) : 0)
+		rdst->wk_text[idx++] = CNVT_EOT(buffer[4]);
+		rdst->wk_text[idx++] = CNVT_EOT(buffer[5]);
+		rdst->wk_text[idx++] = CNVT_EOT(buffer[6]);
+		rdst->wk_text[idx++] = CNVT_EOT(buffer[7]);
+
+		rdst->wk_text_mask |= 1 << seg;
+		/* scan msg data for EOT.  If found set all higher
+		 * mask bits */
+		for (idx = 0; idx < 4; idx++) {
+			if (rdst->text[idx] == RDS_EOT)
+				break;
+		}
+		if (idx < 4) {
+			/* set current and all higher bits  */
+			for (idx = RDS_RT_SEG(grp.offsetB); idx < 16;
+				idx++)
+				rdst->wk_text_mask |= 1 << idx;
+		}
+
+		/* Process buffer when filled */
+		if (rdst->wk_text_mask != ALL_TEXT_BITS)
+			break;
+
+		if (!rdst->text_valid)
+			rdst->text_valid = 1;
+		else if (memcmp(rdst->text, rdst->wk_text, 64) == 0)
+			break;
+
+		memcpy(rdst->text, rdst->wk_text, 64);
+
+		if (rds_state_to_stfm1000(rds)->rds_info)
+			printk(KERN_INFO "RDS: TEXT '%s'\n", rdst->text);
+
+		memset(rdst->wk_text, 0, 64);
+		rdst->wk_text_mask = 0;
+		break;
+
+	default:
+		break;
+	}
+}
+
+int stfm1000_rds_packet_dequeue(struct stfm1000_rds_state *rds, u8 *buf)
+{
+	struct stfm1000_rds_pkt *pkt = &rds->pkt;
+
+	if (pkt->buf_cnt == 0)
+		return -1;
+
+	memcpy(buf, &pkt->buf_queue[pkt->buf_tail][0], 8);
+	if (++pkt->buf_tail >= RDS_PKT_QUEUE)
+		pkt->buf_tail = 0;
+	pkt->buf_cnt--;
+
+	return 0;
+}
+
+void stfm1000_rds_packet_bit(struct stfm1000_rds_state *rds, int bit)
+{
+	struct stfm1000_rds_pkt *pkt = &rds->pkt;
+	u32 rdsdata, rdscrc, rdscrc_c, rdscrc_cp;
+	int correct, correct2, recovered, recovered2;
+	int RetVal;
+
+	/* Stick into shift register */
+	pkt->rdsstream = ((pkt->rdsstream << 1) | bit) & 0x03ffffff;
+	pkt->bitsinfifo++;
+	pkt->bitcount++;
+
+	/* wait for 26 bits of block */
+	if (pkt->bitsinfifo < 26)
+		return;
+
+	rdsdata = pkt->rdsstream & 0x03fffc00;	/* 16 bits of Info. word */
+	rdscrc = pkt->rdsstream & 0x3ff;	/* 10 bits of Checkword */
+
+	switch (pkt->state) {
+	case SYNC_OFFSET_A:
+
+		RetVal = calc_syndrome(pkt->rdsstream);
+
+		switch (RetVal) {
+		case RDS_SYNDROME_OFFSETA:
+			pkt->state = OFFSET_B;
+			break;
+		case RDS_SYNDROME_OFFSETB:
+			pkt->state = OFFSET_C_CP;
+			break;
+		case RDS_SYNDROME_OFFSETC:
+			pkt->state = OFFSET_D;
+			break;
+		case RDS_SYNDROME_OFFSETCP:
+			pkt->state = OFFSET_D;
+			break;
+		case RDS_SYNDROME_OFFSETD:
+			pkt->state = OFFSET_A;
+			break;
+		default:
+			pkt->state = SYNC_OFFSET_A;
+			break;
+		}
+		if (pkt->state == SYNC_OFFSET_A) {
+			pkt->sync_lost_packets++;
+			/* XXX send info? */
+			break;
+		}
+
+		pkt->good_packets++;
+
+		rdsdata = pkt->rdsstream & 0x03fffc00;
+
+		/* Save type A packet in buffer */
+		rdsdata >>= 10;
+		pkt->buffer[0] = (rdsdata >> 8);
+		pkt->buffer[1] = (rdsdata & 0xff);
+		pkt->bitsinfifo = 0;
+
+		/* We found a block with zero errors, but it is not at the
+		 * start of the group. */
+		if (pkt->state == OFFSET_B)
+			pkt->discardpacket = 0;
+		else
+			pkt->discardpacket = 1;
+		break;
+
+	case OFFSET_A:		/* Type A: we are in sync now */
+		rdscrc ^= RDS_OFFSETA;
+		correct = ecc_correct(rdsdata | rdscrc, &recovered);
+		if (correct == UNRECOVERABLE_RDS_BLOCK) {
+			pkt->bad_packets++;
+			pkt->discardpacket++;
+			pkt->state++;
+			pkt->bitsinfifo = 0;
+			break;
+		}
+
+		if (recovered)
+			pkt->recovered_packets++;
+		pkt->good_packets++;
+
+		/* Attempt to see, if we can get the entire group.
+		 * Don't discard. */
+		pkt->discardpacket = 0;
+		rdsdata = correct & 0x03fffc00;
+
+		/* Save type A packet in buffer */
+		rdsdata >>= 10;
+		pkt->buffer[0] = (rdsdata >> 8);
+		pkt->buffer[1] = (rdsdata & 0xff);
+		pkt->bitsinfifo = 0;
+		pkt->state++;
+		break;
+
+	case OFFSET_B:		/* Waiting for type B */
+		rdscrc ^= RDS_OFFSETB;
+		correct = ecc_correct(rdsdata | rdscrc, &recovered);
+		if (correct == UNRECOVERABLE_RDS_BLOCK) {
+			pkt->bad_packets++;
+			pkt->discardpacket++;
+			pkt->state++;
+			pkt->bitsinfifo = 0;
+			break;
+		}
+		if (recovered)
+			pkt->recovered_packets++;
+		pkt->good_packets++;
+
+		rdsdata = correct & 0x03fffc00;
+
+		/* Save type B packet in buffer */
+		rdsdata >>= 10;
+		pkt->buffer[2] = (rdsdata >> 8);
+		pkt->buffer[3] = (rdsdata & 0xff);
+		pkt->bitsinfifo = 0;
+		pkt->state++;
+		break;
+
+	case OFFSET_C_CP:	/* Waiting for type C or C' */
+		rdscrc_c = rdscrc ^ RDS_OFFSETC;
+		rdscrc_cp = rdscrc ^ RDS_OFFSETCP;
+		correct = ecc_correct(rdsdata | rdscrc_c, &recovered);
+		correct2 = ecc_correct(rdsdata | rdscrc_cp, &recovered2);
+		if (correct == UNRECOVERABLE_RDS_BLOCK
+		    && correct2 == UNRECOVERABLE_RDS_BLOCK) {
+			pkt->bad_packets++;
+			pkt->discardpacket++;
+			pkt->state++;
+			pkt->bitsinfifo = 0;
+			break;
+		}
+
+		if (recovered || recovered2)
+			pkt->recovered_packets++;
+		pkt->good_packets++;
+
+		if (correct == UNRECOVERABLE_RDS_BLOCK)
+			correct = correct2;
+
+		rdsdata = correct & 0x03fffc00;
+
+		/* Save type C packet in buffer */
+		rdsdata >>= 10;
+		pkt->buffer[4] = (rdsdata >> 8);
+		pkt->buffer[5] = (rdsdata & 0xff);
+		pkt->bitsinfifo = 0;
+		pkt->state++;
+		break;
+
+	case OFFSET_D:		/* Waiting for type D */
+		rdscrc ^= RDS_OFFSETD;
+		correct = ecc_correct(rdsdata | rdscrc, &recovered);
+		if (correct == UNRECOVERABLE_RDS_BLOCK) {
+			pkt->bad_packets++;
+			pkt->discardpacket++;
+			pkt->state = OFFSET_A;
+			pkt->bitsinfifo = 0;
+			break;
+		}
+
+		if (recovered)
+			pkt->recovered_packets++;
+		pkt->good_packets++;
+
+		rdsdata = correct & 0x03fffc00;
+
+		/* Save type D packet in buffer */
+		rdsdata >>= 10;
+		pkt->buffer[6] = (rdsdata >> 8);
+		pkt->buffer[7] = (rdsdata & 0xff);
+
+		/* buffer it if all segments were ok */
+		if (pkt->discardpacket) {
+			/* We're still in sync, so back to state 1 */
+			pkt->state = OFFSET_A;
+			pkt->bitsinfifo = 0;
+			pkt->discardpacket = 0;
+			break;
+		}
+
+		pkt->state++;
+		/* fall-through */
+
+	case PACKET_OUT:
+		pkt->GroupDropOnce = 1;
+
+		/* queue packet */
+		if (pkt->buf_cnt < RDS_PKT_QUEUE) {
+			memcpy(&pkt->buf_queue[pkt->buf_head][0],
+				pkt->buffer, 8);
+			if (++pkt->buf_head >= RDS_PKT_QUEUE)
+				pkt->buf_head = 0;
+			pkt->buf_cnt++;
+		} else
+			pkt->buf_overruns++;
+
+		/* We're still in sync, so back to state 1 */
+		pkt->state = OFFSET_A;
+		pkt->bitsinfifo = 0;
+		pkt->discardpacket = 0;
+		break;
+
+	}
+
+	/* Lots of errors? If so, go back to resync mode */
+	if (pkt->discardpacket >= 10) {
+		pkt->state = SYNC_OFFSET_A;	/* reset sync state */
+		pkt->bitsinfifo = 26;	/* resync a bit faster */
+	}
+}
+
+/* GROUP_TYPE 0A-0B (buffer must have enough space for 9 bytes) */
+int stfm1000_rds_get_ps(struct stfm1000_rds_state *rds, u8 *buffer,
+	int bufsize)
+{
+	struct stfm1000_rds_text *rdst = &rds->text;
+
+	if (bufsize < 9)
+		return -1;
+
+	if (!rdst->ps_valid)
+		return -1;
+
+	memcpy(buffer, rdst->ps, 8);
+	buffer[8] = '\0';
+
+	return 8;
+}
+
+/* GROUP_TYPE 2A (buffer must have enough space for 65 bytes) */
+int stfm1000_rds_get_text(struct stfm1000_rds_state *rds, u8 *buffer,
+	int bufsize)
+{
+	struct stfm1000_rds_text *rdst = &rds->text;
+
+	if (bufsize < 9)
+		return -1;
+
+	if (!rdst->text_valid)
+		return -1;
+
+	memcpy(buffer, rdst->text, 64);
+	buffer[64] = '\0';
+
+	return 64;
+}
diff --git a/drivers/media/radio/stfm1000/stfm1000-rds.h b/drivers/media/radio/stfm1000/stfm1000-rds.h
new file mode 100644
index 000000000000..44b9a610f86e
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-rds.h
@@ -0,0 +1,364 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef STFM1000_RDS_H
+#define STFM1000_RDS_H
+
+#include <linux/types.h>
+
+/* log2(number of samples in a filter basis) */
+#define RDS_BASISSHIFTS		4
+
+/* number of samples in a filter basis */
+#define RDS_BASISLENGTH		(1 << RDS_BASISSHIFTS)
+
+#define TIME_ADAPT_OVER		100
+
+/* 2^(-this) is the RMS leaky bucket time constant */
+#define RMSALPHASHIFTS		5
+
+#define PROCESS_RDS_BITS	128
+
+#define RDS_BITBUFSIZE		1024	/* was 128 */
+struct stfm1000_rds_bitstream {
+	u32 buf[RDS_BITBUFSIZE];	/* bit buffer */
+	int HeadBitCount;		/* bit buffer head counter */
+	int TailBitCount;		/* bit buffer tail counter */
+};
+
+struct stfm1000_rds_demod {
+	u32 mixandsum1;			/* Accumulator for first
+					 * basis filter */
+	u32 mixandsum2;			/* Accumulator for 2nd
+					 * basis filter */
+	u32 i;				/* Phase Index, 32 phases per
+					 * RDS bit */
+	u32 return_num;			/* Set if there is a new RDS bit */
+	u32 BitAlignmentCounter;	/* Counts bits for timing purposes */
+	int sampskip;			/* Requested timing shift (on i) */
+
+	int DisablePushing;		/* Disables phase push algorithm
+					 * (phase push happens when Ph_RMS[x],
+					 * x != 0, is consistently the maximum
+					 * Ph_RMS) */
+	int MixPopDone;			/* Last mixer phase set request is
+					 * done */
+	u8 rds_big_timeshift;		/* If set, indicates a push or large
+					 * timing shift occurred */
+	int return_rdsdemod;		/* Output, (most recent bit) XOR
+					 * (prev bit) */
+	u32 RDS_BIT_AMP_STAT_REG9;	/* Size of bit (RMS of RDS signal at
+					 * bitslicing instant, typically 220
+					 * to 270) */
+	s32 decomp_hist;		/* Most recent basis filter output */
+	s32 decomp_hist_p;		/* Previous basis filter output */
+	s32 PhaseValue[4];		/* Half-basis phase samples over the
+					 * most recent bit */
+	u32 Ph_RMS[4];			/* RMS of the four half-basis phases */
+	s32 timing_adj;			/* Timing loop leaky-bucket
+					 * accumulator */
+	u32 MixPhase0Mag;		/* Magnitude of RDS signal with RDS
+					 * mixer phase 0 (from mixer phase
+					 * determination) */
+	u32 MixPhase1Mag;		/* Magnitude of RDS signal with RDS
+					 * mixer phase 1 (from mixer phase
+					 * determination) */
+	u32 PhasePopMaxRMS;		/* Maximum RMS observed since last
+					 * phase pop */
+	u32 PrePopRMS;			/* Max of Ph_RMS array right before the
+					 * most recent phase pop */
+	u8 MixPhaseState;		/* State of RDS mixer phase
+					 * determination state machine */
+	int MixPhaseDetInProg;		/* Set if RDS mix phase determination
+					 * is in progress */
+	int sliced_data;		/* The most recent bit decision */
+	u8 PopSafetyZone;		/* Countdown timer, holds off next
+					 * phase pop after a recent one */
+	u8 PushSafetyZone;		/* Countdown timer, holds off next
+					 * phase pop after a timing push (b/c
+					 * timing push resets Ph_RMS vars) */
+	u8 SkipSafetyZone;		/* Countdown timer, holds off next
+					 * phase skip (small timing adj) */
+	int Synchronous;		/* RDS has been determined to be
+					 * synchronous to pilot */
+	u8 PushLastMaxPh;		/* The index at which Ph_RMS is
+					 * maximum ("x" in the above two
+					 * comments) */
+	s32 PushCounter;		/* Counts instances of Ph_RMS[x], x!=0,
+					 * being the maximum Ph_RMS */
+	s32 SkipsAccum;			/* Accumulation of all timing skips
+					 * since RDS demod started */
+	s32 SdnomSk;			/* Skips counter used for SDNOMINAL
+					 * adaption */
+
+	/* update this everytime it's changed & put it here */
+	unsigned int rds_mix_offset : 1;
+
+	unsigned int sdnom_adapt : 1;
+	unsigned int pCoefForcedMono : 1;	/* copy of filter parameter */
+	unsigned int PhasePoppingEnabled : 1;
+
+	unsigned int mix_msg_pending : 1;
+	u8 mix_msg;
+	unsigned int mix_msg_overrun;
+	unsigned int mix_msg_processed_changed;
+
+	unsigned int sdnominal_msg_pending : 1;
+	int sdnominal_msg;
+	unsigned int sdnominal_msg_overrun;
+
+	u32 RdsDemodSkippedBitCnt;	/* bit skipped by RDS demodulator due
+					 * to unavailable space in buf[]
+					 * (bit buffer) */
+};
+
+#define RDS_OFFSETA	0x0fc
+#define RDS_OFFSETB	0x198
+#define RDS_OFFSETC	0x168
+#define RDS_OFFSETCP	0x350
+#define RDS_OFFSETD	0x1b4
+
+#define RDS_SYNDROME_OFFSETA    0x3d8
+#define RDS_SYNDROME_OFFSETB    0x3d4
+#define RDS_SYNDROME_OFFSETC    0x25c
+#define RDS_SYNDROME_OFFSETCP   0x3cc
+#define RDS_SYNDROME_OFFSETD    0x258
+
+#define SYNC_OFFSET_A 0 /* default state */
+#define OFFSET_A      1
+#define OFFSET_B      2
+#define OFFSET_C_CP   3
+#define OFFSET_D      4
+#define PACKET_OUT    5
+
+#define ECC_TBL_SIZE   		    1024
+#define UNRECOVERABLE_RDS_BLOCK 0xffffffff
+
+#define RDS_PKT_QUEUE	16
+
+struct stfm1000_rds_pkt {
+	int state;		/* Current state */
+	u32 rdsstream;		/* Current RDS data */
+	u8 buffer[8];		/* temporary storage of RDS data */
+	int discardpacket;	/* discard packet count */
+	int sync_lost_packets;	/* sync lost */
+	int good_packets;	/* good packet */
+	int bad_packets;	/* bad packet */
+	int recovered_packets;	/* recovered packet */
+	int bitsinfifo;		/* bits count */
+	int GroupDropOnce;	/* Send Group Drop Message once */
+	int bitcount;		/* Counter for Number of Bits read */
+
+	/* queue the packets here */
+	int buf_overruns;
+	int buf_head;
+	int buf_tail;
+	int buf_cnt;
+	int buf_queue[RDS_PKT_QUEUE][8];
+};
+
+#define AUDIT            0
+#define ALL_SEGMENT_BITS 0xF
+#define ALL_TEXT_BITS    0xFFFF
+
+struct stfm1000_rds_pty {
+	u8  id;         /* Program Type ID */
+	u8 *pRds;       /* RDS description */
+	u8 *pRdbs;      /* RDBS description */
+};
+
+struct stfm1000_rds_text {
+	u8 bRds_detected;		/* Has the first packet come in yet? */
+	u16 pi;				/* Program Identification Code (PI) */
+	struct stfm1000_rds_pty pty;	/* Program Type (PTY)) */
+	u8 tp;				/* Traffic Program (TP) identification
+					 * code */
+	u8 ps[9];			/* Program Service Name Sent to UI */
+	u8 altFreq[2];			/* Alternate frequency (AF) */
+	u8 callLetters[5];		/* For US, stations call letters */
+
+	u8 text[65];			/* Radio Text A */
+
+	unsigned int version : 1;	/* Is station broadcasting version
+					 * A or B (B0) */
+	unsigned int ps_valid : 1;	/* station name is valid */
+	unsigned int text_valid : 1;	/* Text is valid */
+	unsigned int textAB_flag : 1;	/* Current flag setting, reset if flag
+					 * changes */
+
+	/*------------------Working area--------------------------- */
+	u8 cp_ps[8];			/* Compare buffer for PS */
+	u8 wk_ps[8];			/* Program Service buffer */
+	u8 wk_ps_mask;			/* lower 4 bits must be set
+					 * before copy */
+	u8 wk_text[64];			/* Radio Text buffer */
+	u16 wk_text_mask;		/* all bits must be set before copy */
+
+	/*-------------------Counters------------------------------ */
+	u32 messages;			/* total number of messages recieved */
+	u32 unsupported;		/* call to unsupported group type */
+	u32 mismatch;			/* Mismatched values */
+	u32 consecutiveGood;		/* Consecutive good will clear bad  */
+	u32 consecutiveGoodMax;		/* Max counter for paramaters */
+};
+
+/* Maximum number of RDS groups described in the U.S. RBDS Standard. */
+#define MAX_RDS_GROUPS_SUPPORTED 32
+
+/* Common Constants */
+#define RDS_LINE_FEED   0xA
+#define RDS_EOT         0xD
+
+/* Offsets into OFFSETB  */
+#define RDS_GROUP_TYPE(x)         (((x) >> 12) & 0xF)
+#define RDS_VERSION(x)            (((x) >> 11) & 0x1)
+#define RDS_TP(x)                 (((x) >> 10) & 0x1)
+#define RDS_PTY(x)                (((x) >>  5) & 0x1F)
+#define RDS_PS_SEG(x)              ((x) & 0x3)
+#define RDS_RT_AB(x)              (((x) >>  4) & 0x1)
+#define RDS_RT_SEG(x)              ((x) & 0xF)
+
+/* This values corresond to the Group Types defined */
+/* In the U.S. RBDS standard. */
+#define RDS_GROUP_TYPE_0A	 0 /* Basic tuning and switching information */
+#define RDS_GROUP_TYPE_0B	 1 /* Basic tuning and switching information */
+#define RDS_GROUP_TYPE_1A	 2 /* Program item number and slow labeling
+				    * codes */
+#define RDS_GROUP_TYPE_1B	 3 /* Program item number */
+#define RDS_GROUP_TYPE_2A	 4 /* Radio Text */
+#define RDS_GROUP_TYPE_2B	 5 /* Radio Text */
+#define RDS_GROUP_TYPE_3A	 6 /* Application identification for ODA
+				    * only */
+#define RDS_GROUP_TYPE_3B	 7 /* Open data applications */
+#define RDS_GROUP_TYPE_4A	 8 /* Clock-time and date */
+#define RDS_GROUP_TYPE_4B	 9 /* Open data applications */
+#define RDS_GROUP_TYPE_5A	10 /* Transparent Data Channels (32 channels)
+				    * or ODA */
+#define RDS_GROUP_TYPE_5B	11 /* Transparent Data Channels (32 channels)
+				    * or ODA */
+#define RDS_GROUP_TYPE_6A	12 /* In House Applications or ODA */
+#define RDS_GROUP_TYPE_6B	13 /* In House Applications or ODA */
+#define RDS_GROUP_TYPE_7A	14 /* Radio Paging or ODA */
+#define RDS_GROUP_TYPE_7B	15 /* Open Data Applications */
+#define RDS_GROUP_TYPE_8A	16 /* Traffic Message Channel or ODA */
+#define RDS_GROUP_TYPE_8B	17 /* Open Data Applications */
+#define RDS_GROUP_TYPE_9A	18 /* Emergency warning system or ODA */
+#define RDS_GROUP_TYPE_9B	19 /* Open Data Applications */
+#define RDS_GROUP_TYPE_10A	20 /* Program Type Name */
+#define RDS_GROUP_TYPE_10B	21 /* Open Data Applications */
+#define RDS_GROUP_TYPE_11A	22 /* Open Data Applications */
+#define RDS_GROUP_TYPE_11B	23 /* Open Data Applications */
+#define RDS_GROUP_TYPE_12A	24 /* Open Data Applications */
+#define RDS_GROUP_TYPE_12B	25 /* Open Data Applications */
+#define RDS_GROUP_TYPE_13A	26 /* Enhanced Radio Paging or ODA */
+#define RDS_GROUP_TYPE_13B	27 /* Open Data Applications */
+#define RDS_GROUP_TYPE_14A	28 /* Enhanced Other Networks information */
+#define RDS_GROUP_TYPE_14B	29 /* Enhanced Other Networks information */
+#define RDS_GROUP_TYPE_15A	30 /* Defined in RBDS */
+#define RDS_GROUP_TYPE_15B	31 /* Fast switching information */
+#define NUM_DEFINED_RDS_GROUPS	32 /* Number of groups defined in RBDS
+				    * standard */
+
+/* Structure representing Generic packet of 64 bits. */
+struct rds_group_data {
+	u16 piCode;	/* * Program ID */
+	u16 offsetB;	/* subject to group type */
+	u16 offsetC;	/* subject to group type */
+	u16 offsetD;	/* subject to group type */
+};
+
+/* Structure representing Group 0A (Service Name) */
+struct rds_group0A {
+	u16 piCode;	/* * Program ID */
+	u16 offsetB;	/* subject to group type */
+	u8 freq[2];	/* alt frequency 0=1 */
+	u8 text[2];	/* Name segment */
+};
+
+/* Structure representing Group 0B (Service Name) */
+struct rds_group0B {
+	u16 piCode;	/* * Program ID */
+	u16 offsetB;	/* subject to group type */
+	u16 piCode_dup;	/* Duplicate PI Code */
+	u8 text[2];	/* station text */
+};
+
+/* Structure representing Group 2A (Radio Text) (64 char) */
+struct rds_group2A {
+	u16 piCode;	/* * Program ID */
+	u16 offsetB;	/* subject to group type */
+	u8 text[4];
+};
+
+/* Structure representing Group 2B (Radio Text) (32 char) */
+struct rds_group2B {
+	u16 piCode;	/* * Program ID */
+	u16 offsetB;	/* subject to group type */
+	u16 piCode_dup;	/* Duplicate PI Code */
+	u8 text[2];
+};
+
+/* Structure representing all groups */
+union rds_msg {
+	struct rds_group2B gt2B;
+	struct rds_group2A gt2A;
+	struct rds_group0B gt0B;
+	struct rds_group0A gt0A;
+	struct rds_group_data gt00;
+};
+
+struct stfm1000_rds_state {
+	struct stfm1000_rds_bitstream bitstream;
+	struct stfm1000_rds_demod demod;
+	struct stfm1000_rds_pkt pkt;
+	struct stfm1000_rds_text text;
+	unsigned int reset_req : 1;
+};
+
+/* callback from rds etc. */
+void stfm1000_rds_reset(struct stfm1000_rds_state *rds);
+void stfm1000_rds_start(struct stfm1000_rds_state *rds);
+void stfm1000_rds_stop(struct stfm1000_rds_state *rds);
+
+/* call these from the monitor thread, but with interrupts disabled */
+int stfm1000_rds_mix_msg_get(struct stfm1000_rds_state *rds);
+int stfm1000_rds_mix_msg_processed(struct stfm1000_rds_state *rds,
+	int mix_msg);
+int stfm1000_rds_sdnominal_msg_get(struct stfm1000_rds_state *rds);
+int stfm1000_rds_sdnominal_msg_processed(struct stfm1000_rds_state *rds,
+	int sdnominal_msg);
+int stfm1000_rds_bits_available(struct stfm1000_rds_state *rds);
+int stmf1000_rds_get_bit(struct stfm1000_rds_state *rds);
+
+/* called from audio handler (interrupt) */
+void stfm1000_rds_demod(struct stfm1000_rds_state *rds, const u16 *dri_data,
+	int total);
+
+/* call these from monitor thread, interrupts enabled */
+void stfm1000_rds_packet_bit(struct stfm1000_rds_state *rds, int bit);
+int stfm1000_rds_packet_dequeue(struct stfm1000_rds_state *rds, u8 *buf);
+void stfm1000_rds_process_packet(struct stfm1000_rds_state *rds, u8 *buffer);
+
+static inline int stfm1000_rds_get_reset_req(struct stfm1000_rds_state *rds)
+{
+	return rds->reset_req;
+}
+
+/* GROUP_TYPE 0A-0B */
+int stfm1000_rds_get_ps(struct stfm1000_rds_state *rds, u8 *buffer,
+	int bufsize);
+
+/* GROUP_TYPE 2A */
+int stfm1000_rds_get_text(struct stfm1000_rds_state *rds, u8 *buffer,
+	int bufsize);
+
+#endif
diff --git a/drivers/media/radio/stfm1000/stfm1000-regs.h b/drivers/media/radio/stfm1000/stfm1000-regs.h
new file mode 100644
index 000000000000..c9476b7d67e1
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-regs.h
@@ -0,0 +1,165 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef STFM1000_REGS_H
+#define STFM1000_REGS_H
+
+/* registers */
+#define STFM1000_TUNE1			0x00
+#define STFM1000_SDNOMINAL		0x04
+#define STFM1000_PILOTTRACKING		0x08
+#define STFM1000_INITIALIZATION1	0x10
+#define STFM1000_INITIALIZATION2	0x14
+#define STFM1000_INITIALIZATION3	0x18
+#define STFM1000_INITIALIZATION4	0x1C
+#define STFM1000_INITIALIZATION5	0x20
+#define STFM1000_INITIALIZATION6	0x24
+#define STFM1000_REF			0x28
+#define STFM1000_LNA			0x2C
+#define STFM1000_MIXFILT		0x30
+#define STFM1000_CLK1			0x34
+#define STFM1000_CLK2			0x38
+#define STFM1000_ADC			0x3C
+#define STFM1000_AGC_CONTROL1		0x44
+#define STFM1000_AGC_CONTROL2		0x48
+#define STFM1000_DATAPATH		0x5C
+#define STFM1000_RMS			0x60
+#define STFM1000_AGC_STAT		0x64
+#define STFM1000_SIGNALQUALITY		0x68
+#define STFM1000_DCEST			0x6C
+#define STFM1000_RSSI_TONE		0x70
+#define STFM1000_PILOTCORRECTION	0x74
+#define STFM1000_ATTENTION		0x78
+#define STFM1000_CLK3			0x7C
+#define STFM1000_CHIPID			0x80
+
+/* number of registers */
+#define STFM1000_NUM_REGS		((0x80 + 4) / 4)
+
+#define STFM1000_FREQUENCY_100KHZ_MIN	758
+#define STFM1000_FREQUENCY_100KHZ_RANGE	325
+#define STFM1000_FREQUENCY_100KHZ_MAX	(STFM1000_FREQUENCY_100KHZ_MIN + \
+					STFM1000_FREQUENCY_100KHZ_RANGE)
+
+#define STFM1000_TUNE1_B2_MIX		0x001C0000
+#define STFM1000_TUNE1_CICOSR		0x00007E00
+#define STFM1000_TUNE1_PLL_DIV		0x000001FF
+
+#define STFM1000_CHIP_REV_TA1		0x00000001
+#define STFM1000_CHIP_REV_TA2		0x00000002
+#define STFM1000_CHIP_REV_TB1		0x00000011
+#define STFM1000_CHIP_REV_TB2		0x00000012
+
+/* DATAPATH bits we use */
+#define STFM1000_DP_EN			0x01000000
+#define STFM1000_DB_ACCEPT		0x00010000
+#define STFM1000_SAI_CLK_DIV_MASK	0x7c
+#define STFM1000_SAI_CLK_DIV_SHIFT	2
+#define STFM1000_SAI_CLK_DIV(x) \
+	(((x) << STFM1000_SAI_CLK_DIV_SHIFT) & STFM1000_SAI_CLK_DIV_MASK)
+#define STFM1000_SAI_EN			0x00000001
+
+/* AGC_CONTROL1 bits we use */
+#define STFM1000_B2_BYPASS_AGC_CTL	0x00004000
+#define STFM1000_B2_BYPASS_FILT_MASK	0x0000000C
+#define STFM1000_B2_BYPASS_FILT_SHIFT	2
+#define STFM1000_B2_BYPASS_FILT(x) \
+	(((x) << STFM1000_B2_BYPASS_FILT_SHIFT) & STFM1000_B2_BYPASS_FILT_MASK)
+#define STFM1000_B2_LNATH_MASK		0x001F0000
+#define STFM1000_B2_LNATH_SHIFT		16
+#define STFM1000_B2_LNATH(x) \
+	(((x) << STFM1000_B2_LNATH_SHIFT) & STFM1000_B2_LNATH_MASK)
+
+/* AGC_STAT bits we use */
+#define STFM1000_AGCOUT_STAT_MASK	0x1F000000
+#define STFM1000_AGCOUT_STAT_SHIFT	24
+#define STFM1000_LNA_RMS_MASK		0x00001F00
+#define STFM1000_LNA_RMS_SHIFT		8
+
+/* PILOTTRACKING bits we use */
+#define STFM1000_B2_PILOTTRACKING_EN		0x00008000
+#define STFM1000_B2_PILOTLPF_TIMECONSTANT_MASK	0x00000f00
+#define STFM1000_B2_PILOTLPF_TIMECONSTANT_SHIFT	8
+#define STFM1000_B2_PILOTLPF_TIMECONSTANT(x)	\
+	(((x) << STFM1000_B2_PILOTLPF_TIMECONSTANT_SHIFT) & \
+		STFM1000_B2_PILOTLPF_TIMECONSTANT_MASK)
+#define STFM1000_B2_PFDSCALE_MASK		0x000000f0
+#define STFM1000_B2_PFDSCALE_SHIFT		4
+#define STFM1000_B2_PFDSCALE(x)	\
+	(((x) << STFM1000_B2_PFDSCALE_SHIFT) & STFM1000_B2_PFDSCALE_MASK)
+#define STFM1000_B2_PFDFILTER_SPEEDUP_MASK	0x0000000f
+#define STFM1000_B2_PFDFILTER_SPEEDUP_SHIFT	0
+#define STFM1000_B2_PFDFILTER_SPEEDUP(x)	\
+	(((x) << STFM1000_B2_PFDFILTER_SPEEDUP_SHIFT) & \
+		STFM1000_B2_PFDFILTER_SPEEDUP_MASK)
+
+/* PILOTCORRECTION bits we use */
+#define STFM1000_PILOTEST_TA2_MASK	0xff000000
+#define STFM1000_PILOTEST_TA2_SHIFT	24
+#define STFM1000_PILOTEST_TB2_MASK	0xfe000000
+#define STFM1000_PILOTEST_TB2_SHIFT	25
+
+/* INITIALIZATION1 bits we use */
+#define STFM1000_B2_BYPASS_FILT_MASK	0x0000000C
+#define STFM1000_B2_BYPASS_FILT_SHIFT	2
+#define STFM1000_B2_BYPASS_FILT(x)	\
+	(((x) << STFM1000_B2_BYPASS_FILT_SHIFT) & STFM1000_B2_BYPASS_FILT_MASK)
+
+/* INITIALIZATION2 bits we use */
+#define STFM1000_DRI_CLK_EN		0x80000000
+#define STFM1000_DEEMPH_50_75B		0x00000100
+#define STFM1000_RDS_ENABLE		0x00100000
+#define STFM1000_RDS_MIXOFFSET		0x00200000
+
+/* INITIALIZATION3 bits we use */
+#define STFM1000_B2_NEAR_CHAN_MIX_MASK	0x1c000000
+#define STFM1000_B2_NEAR_CHAN_MIX_SHIFT	26
+#define STFM1000_B2_NEAR_CHAN_MIX(x)	\
+	(((x) << STFM1000_B2_NEAR_CHAN_MIX_SHIFT) & \
+		STFM1000_B2_NEAR_CHAN_MIX_MASK)
+
+/* CLK1 bits we use */
+#define STFM1000_ENABLE_TAPDELAYFIX	0x00000020
+
+/* REF bits we use */
+#define STFM1000_LNA_AMP1_IMPROVE_DISTORTION 0x08000000
+
+/* LNA bits we use */
+#define STFM1000_AMP2_IMPROVE_DISTORTION 0x08000000
+#define STFM1000_ANTENNA_TUNECAP_MASK	0x001F0000
+#define STFM1000_ANTENNA_TUNECAP_SHIFT	16
+#define STFM1000_ANTENNA_TUNECAP(x)	\
+	(((x) << STFM1000_ANTENNA_TUNECAP_SHIFT) & \
+		STFM1000_ANTENNA_TUNECAP_MASK)
+#define STFM1000_IBIAS2_UP		0x00000008
+#define STFM1000_IBIAS2_DN		0x00000004
+#define STFM1000_IBIAS1_UP		0x00000002
+#define STFM1000_IBIAS1_DN		0x00000001
+#define STFM1000_USEATTEN_MASK		0x00600000
+#define STFM1000_USEATTEN_SHIFT		21
+#define STFM1000_USEATTEN(x)	\
+	(((x) << STFM1000_USEATTEN_SHIFT) & STFM1000_USEATTEN_MASK)
+
+/* SIGNALQUALITY bits we use */
+#define STFM1000_NEAR_CHAN_AMPLITUDE_MASK	0x0000007F
+#define STFM1000_NEAR_CHAN_AMPLITUDE_SHIFT	0
+#define STFM1000_NEAR_CHAN_AMPLITUDE(x)	\
+	(((x) << STFM1000_NEAR_CHAN_AMPLITUDE_SHIFT) & \
+		STFM1000_NEAR_CHAN_AMPLITUDE_MASK)
+
+/* precalc tables elements */
+struct stfm1000_tune1 {
+	unsigned int tune1;	/* at least 32 bit */
+	unsigned int sdnom;
+};
+
+#endif
diff --git a/drivers/media/radio/stfm1000/stfm1000.h b/drivers/media/radio/stfm1000/stfm1000.h
new file mode 100644
index 000000000000..5ae6f38db3b0
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000.h
@@ -0,0 +1,254 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef STFM1000_H
+#define STFM1000_H
+
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <linux/i2c.h>
+#include <linux/list.h>
+#include <linux/wait.h>
+#include <linux/irq.h>
+#include <media/videobuf-dma-sg.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <mach/dma.h>
+
+#include "stfm1000-regs.h"
+
+#include "stfm1000-filter.h"
+#include "stfm1000-rds.h"
+
+struct stfm1000 {
+	struct list_head devlist;
+	int idx;
+
+	struct i2c_client *client;
+	struct video_device radio;
+
+	/* alsa */
+	struct snd_card *card;
+	struct snd_pcm *pcm;
+	struct snd_pcm_substream *substream;
+	struct stmp3xxx_dma_descriptor *dma;
+	int desc_num;
+	int dma_ch;
+	int dma_irq;
+	int attn_irq;
+
+	struct mutex state_lock;
+	int read_count;
+	int read_offset;
+	int blocks;
+	int blksize;
+	int bufsize;
+
+	struct mutex deffered_work_lock;
+	struct execute_work snd_capture_start_work;
+	struct execute_work snd_capture_stop_work;
+
+	int now_recording;
+	int alsa_initialized;
+	int stopping_recording;
+
+	/* actual DRI buffer */
+	dma_addr_t dri_phys;
+	void *dri_buf;
+	int dri_bufsz;
+
+	/* various */
+	u16 curvol;
+	int users;
+	int removed;
+	struct mutex xfer_lock;
+	u8 revid;
+
+	unsigned int dbgflg;
+
+	/* shadow registers */
+	u32 shadow_regs[STFM1000_NUM_REGS];
+	u32 reg_rw_set[(STFM1000_NUM_REGS + 31) / 32];
+	u32 reg_ra_set[(STFM1000_NUM_REGS + 31) / 32];
+	u32 reg_dirty_set[(STFM1000_NUM_REGS + 31) / 32];
+
+	/* tuning parameters (not everything is used for now) */
+	u16 tune_rssi_th;		/* sd_ctl_TuneRssiTh_u16 */
+	u16 tune_mpx_dc_th;		/* sd_ctl_TuneMpxDcTh_u16 */
+	u16 adj_chan_th;		/* sd_ctl_AdjChanTh_u16 */
+	u16 pilot_est_th;		/* sd_ctl_PilotEstTh_u16 */
+	u16 coef_lna_turn_off_th;	/* sd_ctl_pCoefLnaTurnOffTh_u16 */
+	u16 coef_lna_turn_on_th;	/* sd_ctl_pCoefLnaTurnOnTh_u16 */
+	u16 reg_agc_ref_lna_off;	/* sd_ctl_pRegAgcRefLnaOff_u16 */
+	u16 reg_agc_ref_lna_on;		/* sd_ctl_pRegAgcRefLnaOn_u16 */
+
+	u32 sdnominal_pivot;		/* sd_ctl_SdnominalData_u32 */
+
+	/* jiffies of the next monitor cycle */
+	unsigned long next_quality_monitor;
+	unsigned long next_agc_monitor;
+
+	unsigned int mute : 1;			/* XXX */
+	unsigned int lna_driving : 1;		/* sd_ctl_LnaDriving_u1 */
+	unsigned int weak_signal : 1;		/* sd_ctl_WeakSignal_u1 */
+	unsigned int is_station : 1;		/* XXX */
+	unsigned int force_mono : 1;		/* XXX */
+	unsigned int signal_indicator : 1;	/* XXX */
+	unsigned int stereo_indicator : 1;	/* XXX */
+	unsigned int agc_monitor : 1;		/* XXX */
+	unsigned int quality_monitor : 1;	/* XXX */
+	unsigned int pilot_present : 1;		/* sd_ctl_PilotPresent_u1 */
+	unsigned int prev_pilot_present : 1;	/* XXX */
+	unsigned int stereo : 1;
+	unsigned int active : 1;		/* set when audio enabled */
+	unsigned int rds_enable : 1;		/* set when rds is enabled */
+	unsigned int rds_present : 1;		/* RDS info present */
+	unsigned int rds_sync : 1;		/* RDS force sync */
+	unsigned int rds_demod_running : 1;	/* RDS demod is running ATM */
+	unsigned int rds_sdnominal_adapt : 1;	/* adapt for better recept. */
+	unsigned int rds_phase_pop : 1;		/* enable phase pop */
+	unsigned int rds_info : 1;		/* print debugging info RDS */
+	unsigned int tuning_grid_50KHz : 1;	/* tuning grid of 50Khz */
+	u32 rssi;				/* rssi last decoded frame */
+	u16 rssi_dc_est_log;
+	u16 signal_strength;			/* is rssi_dc_est_log */
+	u16 rds_signal_th;			/* RDS threshold */
+	s16 mpx_dc;				/* sd_ctl_ShadowToneData_i16 */
+
+	u32 tune_cap_a_f;			/* float! sd_ctl_TuneCapA_f */
+	u32 tune_cap_b_f;			/* float! sd_ctl_TuneCapB_f */
+
+	int monitor_period;			/* period of the monitor */
+	int quality_monitor_period;		/* update period in ms */
+	int agc_monitor_period;			/* update period in ms */
+
+	int georegion;				/* current graphical region */
+
+	/* last tuned frequency */
+	int freq;				/* 88.0 = 8800 */
+
+	/* weak signal processing filter state */
+	struct stfm1000_filter_parms filter_parms;
+
+	/* state of rds */
+	spinlock_t rds_lock;
+	struct stfm1000_rds_state rds_state;
+	unsigned int rds_pkt_bad;
+	unsigned int rds_pkt_good;
+	unsigned int rds_pkt_recovered;
+	unsigned int rds_pkt_lost_sync;
+	unsigned int rds_bit_overruns;
+
+	/* monitor thread */
+	wait_queue_head_t thread_wait;
+	unsigned long thread_events;
+	struct task_struct *thread;
+};
+
+#define EVENT_RDS_BITS		0
+#define EVENT_RDS_MIXFILT	1
+#define EVENT_RDS_SDNOMINAL	2
+#define EVENT_RDS_RESET		3
+
+#define STFM1000_DBGFLG_I2C	(1 << 0)
+
+static inline struct stfm1000 *stfm1000_from_file(struct file *file)
+{
+	return container_of(video_devdata(file), struct stfm1000, radio);
+}
+
+/* in stfm1000-i2c.c */
+
+/* setup reg set */
+void stfm1000_setup_reg_set(struct stfm1000 *stfm1000);
+
+/* direct access to registers bypassing the shadow register set */
+int stfm1000_raw_read(struct stfm1000 *stfm1000, int reg, u32 *value);
+int stfm1000_raw_write(struct stfm1000 *stfm1000, int reg, u32 value);
+
+/* access using the shadow register set */
+int stfm1000_write(struct stfm1000 *stfm1000, int reg, u32 value);
+int stfm1000_read(struct stfm1000 *stfm1000, int reg, u32 *value);
+int stfm1000_write_masked(struct stfm1000 *stfm1000, int reg, u32 value,
+		u32 mask);
+int stfm1000_set_bits(struct stfm1000 *stfm1000, int reg, u32 value);
+int stfm1000_clear_bits(struct stfm1000 *stfm1000, int reg, u32 value);
+
+struct stfm1000_reg {
+	unsigned int regno;
+	u32 value;
+};
+
+#define STFM1000_REG_END	-1
+#define STFM1000_REG_DELAY	-2
+
+#define STFM1000_REG_SET_BITS_MASK	0x1000
+#define STFM1000_REG_CLEAR_BITS_MASK	0x2000
+
+#define STFM1000_REG(r, v) \
+	{ .regno = STFM1000_ ## r , .value = (v) }
+
+#define STFM1000_END \
+	{ .regno = STFM1000_REG_END }
+
+#define STFM1000_DELAY(x) \
+	{ .regno = STFM1000_REG_DELAY, .value = (x) }
+
+#define STFM1000_REG_SETBITS(r, v) \
+	{ .regno = STFM1000_ ## r | STFM1000_REG_SET_BITS_MASK, \
+		.value = (v) }
+
+#define STFM1000_REG_CLRBITS(r, v) \
+	{ .regno = STFM1000_ ## r | STFM1000_REG_CLEAR_BITS_MASK, \
+		.value = (v) }
+
+int stfm1000_write_regs(struct stfm1000 *stfm1000,
+		const struct stfm1000_reg *reg);
+
+/* in stfm1000-precalc.c */
+extern const struct stfm1000_tune1
+stfm1000_tune1_table[STFM1000_FREQUENCY_100KHZ_RANGE];
+
+/* exported for use by alsa driver */
+
+struct stfm1000_dri_sample {
+	/* L+R */
+	u16 l_plus_r;
+	/* L-R */
+	u16 l_minus_r;
+	/* Rx signal strength channel */
+	u16 rssi;
+	/* Radio data service channel */
+	u16 rds;
+};
+
+struct stfm1000_alsa_ops {
+	int  (*init)(struct stfm1000 *stfm1000);
+	void  (*release)(struct stfm1000 *stfm1000);
+	void (*dma_irq)(struct stfm1000 *stfm1000);
+	void (*attn_irq)(struct stfm1000 *stfm1000);
+};
+
+extern struct list_head stfm1000_devlist;
+extern struct stfm1000_alsa_ops *stfm1000_alsa_ops;
+
+/* needed for setting the interrupt handlers from alsa */
+irqreturn_t stfm1000_dri_attn_irq(int irq, void *dev_id);
+irqreturn_t stfm1000_dri_dma_irq(int irq, void *dev_id);
+void stfm1000_decode_block(struct stfm1000 *stfm1000, const s16 *src, s16 *dst, int count);
+void stfm1000_take_down(struct stfm1000 *stfm1000);
+void stfm1000_bring_up(struct stfm1000 *stfm1000);
+void stfm1000_tune_current(struct stfm1000 *stfm1000);
+
+void stfm1000_monitor_signal(struct stfm1000 *stfm1000, int bit);
+
+#endif
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index 47102c2c8250..3e1feb550e4b 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -531,6 +531,45 @@ config VIDEO_W9966
 	  Check out <file:Documentation/video4linux/w9966.txt> for more
 	  information.
 
+config VIDEO_MXC_CAMERA
+	tristate "MXC Video For Linux Camera"
+	depends on VIDEO_DEV && ARCH_MXC
+	default y
+	---help---
+	  This is the video4linux2 capture driver based on MXC IPU/eMMA module.
+
+source "drivers/media/video/mxc/capture/Kconfig"
+
+config VIDEO_MXC_OUTPUT
+	tristate "MXC Video For Linux Video Output"
+	depends on VIDEO_DEV && ARCH_MXC
+	default y
+	---help---
+	  This is the video4linux2 output driver based on MXC IPU/eMMA module.
+
+source "drivers/media/video/mxc/output/Kconfig"
+
+config VIDEO_PXP
+	tristate "STMP3xxx PxP"
+	depends on VIDEO_DEV && VIDEO_V4L2 && ARCH_STMP3XXX
+	select VIDEOBUF_DMA_CONTIG
+	---help---
+	  This is a video4linux driver for the Freescale PxP
+	  (Pixel Pipeline). This module supports output overlay of
+	  the STMP3xxx framebuffer on a video stream.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called pxp.
+
+config VIDEO_MXC_OPL
+	tristate
+	depends on VIDEO_DEV && ARCH_MXC
+	default n
+	---help---
+	  This is the ARM9-optimized OPL (Open Primitives Library) software
+	  rotation/mirroring implementation. It may be used by eMMA video
+	  capture or output device.
+
 config VIDEO_CPIA
 	tristate "CPiA Video For Linux"
 	depends on VIDEO_V4L1
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index 16962f3aa157..fe2152b79743 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -57,6 +57,14 @@ obj-$(CONFIG_VIDEO_ZORAN) += zoran/
 obj-$(CONFIG_VIDEO_PMS) += pms.o
 obj-$(CONFIG_VIDEO_VINO) += vino.o indycam.o
 obj-$(CONFIG_VIDEO_STRADIS) += stradis.o
+obj-$(CONFIG_VIDEO_MXC_IPU_CAMERA) += mxc/capture/
+obj-$(CONFIG_VIDEO_MXC_EMMA_CAMERA) += mxc/capture/
+obj-$(CONFIG_VIDEO_MXC_CSI_CAMERA) += mxc/capture/
+obj-$(CONFIG_VIDEO_MXC_IPU_OUTPUT) += mxc/output/
+obj-$(CONFIG_VIDEO_MXC_IPUV1_WVGA_OUTPUT) += mxc/output/
+obj-$(CONFIG_VIDEO_MXC_EMMA_OUTPUT) += mxc/output/
+obj-$(CONFIG_VIDEO_MXC_OPL) += mxc/opl/
+obj-$(CONFIG_VIDEO_PXP)	+= pxp.o
 obj-$(CONFIG_VIDEO_CPIA) += cpia.o
 obj-$(CONFIG_VIDEO_CPIA_PP) += cpia_pp.o
 obj-$(CONFIG_VIDEO_CPIA_USB) += cpia_usb.o
diff --git a/drivers/media/video/mxc/capture/Kconfig b/drivers/media/video/mxc/capture/Kconfig
new file mode 100644
index 000000000000..adab0a886f36
--- /dev/null
+++ b/drivers/media/video/mxc/capture/Kconfig
@@ -0,0 +1,123 @@
+if VIDEO_MXC_CAMERA
+
+menu "MXC Camera/V4L2 PRP Features support"
+config VIDEO_MXC_IPU_CAMERA
+	bool
+	depends on VIDEO_MXC_CAMERA && MXC_IPU
+	default y
+
+config VIDEO_MXC_EMMA_CAMERA
+	tristate "MX27 eMMA support"
+	depends on VIDEO_MXC_CAMERA && MXC_EMMA && FB_MXC_SYNC_PANEL
+	select VIDEO_MXC_OPL
+	default y
+
+config VIDEO_MXC_CSI_CAMERA
+	tristate "MX25 CSI camera support"
+	depends on !VIDEO_MXC_EMMA_CAMERA
+
+config VIDEO_MXC_CSI_DMA
+	bool "CSI-DMA Still Image Capture support"
+	depends on VIDEO_MXC_EMMA_CAMERA
+	default n
+	---help---
+	  Use CSI-DMA method instead of CSI-PrP link to capture still image. This allows
+	  to use less physical contiguous memory to capture big resolution still image. But
+	  with this method the CSC (Color Space Conversion) and resize are not supported.
+	  If unsure, say N.
+
+choice
+	prompt "Select Camera/TV Decoder"
+	default MXC_CAMERA_OV3640
+	depends on VIDEO_MXC_CAMERA
+
+config MXC_CAMERA_MC521DA
+	tristate "Magnachip mc521da camera support"
+	select I2C_MXC
+	depends on VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the mc521da Camera with your MXC system, say Y here.
+
+config MXC_EMMA_CAMERA_MICRON111
+	tristate "Micron mt9v111 camera support with eMMA"
+	select I2C_MXC
+	depends on VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the mt9v111 Camera with your MXC system, say Y here.
+
+config MXC_CAMERA_OV2640_EMMA
+	tristate "OmniVision ov2640 camera support with eMMA"
+	depends on VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the ov2640 Camera with your MXC system, say Y here.
+
+config MXC_CAMERA_MICRON111
+	tristate "Micron mt9v111 camera support"
+	select I2C_MXC
+	depends on ! VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the mt9v111 Camera with your MXC system, say Y here.
+
+config MXC_CAMERA_OV2640
+	tristate "OmniVision ov2640 camera support"
+	depends on !VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the ov2640 Camera with your MXC system, say Y here.
+
+config MXC_CAMERA_OV3640
+	tristate "OmniVision ov3640 camera support"
+	depends on !VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the ov3640 Camera with your MXC system, say Y here.
+
+config MXC_TVIN_ADV7180
+	tristate "Analog Device adv7180 TV Decoder Input support"
+	depends on MACH_MX35_3DS
+	---help---
+	  If you plan to use the adv7180 video decoder with your MXC system, say Y here.
+
+endchoice
+
+config MXC_IPU_PRP_VF_SDC
+	tristate "Pre-Processor VF SDC library"
+	depends on VIDEO_MXC_IPU_CAMERA && FB_MXC_SYNC_PANEL
+	default y
+	---help---
+	  Use case PRP_VF_SDC:
+		Preprocessing image from smart sensor for viewfinder and
+		displaying it on synchronous display with SDC use case.
+		If SDC BG is selected, Rotation will not be supported.
+		CSI -> IC (PRP VF) -> MEM
+		MEM -> IC (ROT) -> MEM
+		MEM -> SDC (FG/BG)
+
+config MXC_IPU_PRP_VF_ADC
+	tristate "Pre-Processor VF ADC library"
+	depends on VIDEO_MXC_IPU_CAMERA && FB_MXC_ASYNC_PANEL
+	default y
+	---help---
+	  Use case PRP_VF_ADC:
+		Preprocessing image from smart sensor for viewfinder and
+		displaying it on asynchronous display.
+		CSI -> IC (PRP VF) -> ADC2
+
+config MXC_IPU_PRP_ENC
+	tristate "Pre-processor Encoder library"
+	depends on VIDEO_MXC_IPU_CAMERA
+	default y
+	---help---
+	  Use case PRP_ENC:
+		Preprocessing image from smart sensor for encoder.
+		CSI -> IC (PRP ENC) -> MEM
+
+config MXC_IPU_CSI_ENC
+	tristate "IPU CSI Encoder library"
+	depends on VIDEO_MXC_IPU_CAMERA
+	default y
+	---help---
+	  Use case IPU_CSI_ENC:
+		Get raw image with CSI from smart sensor for encoder.
+		CSI -> MEM
+endmenu
+
+endif
diff --git a/drivers/media/video/mxc/capture/Makefile b/drivers/media/video/mxc/capture/Makefile
new file mode 100644
index 000000000000..112923c8fc8f
--- /dev/null
+++ b/drivers/media/video/mxc/capture/Makefile
@@ -0,0 +1,39 @@
+ifeq ($(CONFIG_VIDEO_MXC_IPU_CAMERA),y)
+	obj-$(CONFIG_VIDEO_MXC_CAMERA) += mxc_v4l2_capture.o
+	obj-$(CONFIG_MXC_IPU_PRP_VF_ADC) += ipu_prp_vf_adc.o
+	obj-$(CONFIG_MXC_IPU_PRP_VF_SDC) += ipu_prp_vf_sdc.o ipu_prp_vf_sdc_bg.o
+	obj-$(CONFIG_MXC_IPU_PRP_ENC) += ipu_prp_enc.o ipu_still.o
+	obj-$(CONFIG_MXC_IPU_CSI_ENC) += ipu_csi_enc.o ipu_still.o
+endif
+
+obj-$(CONFIG_VIDEO_MXC_CSI_CAMERA) += fsl_csi.o csi_v4l2_capture.o
+
+mx27_capture-objs := mx27_prphw.o mx27_prpsw.o emma_v4l2_capture.o
+obj-$(CONFIG_VIDEO_MXC_EMMA_CAMERA) += mx27_csi.o mx27_capture.o
+
+mc521da_camera-objs := mc521da.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_MC521DA) += mc521da_camera.o
+
+emma_mt9v111_camera-objs := emma_mt9v111.o sensor_clock.o
+obj-$(CONFIG_MXC_EMMA_CAMERA_MICRON111) += emma_mt9v111_camera.o
+
+mt9v111_camera-objs := mt9v111.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_MICRON111) += mt9v111_camera.o
+
+hv7161_camera-objs := hv7161.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_HV7161) += hv7161_camera.o
+
+s5k3aaex_camera-objs := s5k3aaex.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_S5K3AAEX) += s5k3aaex_camera.o
+
+emma_ov2640_camera-objs := emma_ov2640.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_OV2640_EMMA) += emma_ov2640_camera.o
+
+ov2640_camera-objs := ov2640.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_OV2640) += ov2640_camera.o
+
+ov3640_camera-objs := ov3640.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_OV3640) += ov3640_camera.o
+
+adv7180_tvin-objs := adv7180.o sensor_clock.o
+obj-$(CONFIG_MXC_TVIN_ADV7180) += adv7180_tvin.o
diff --git a/drivers/media/video/mxc/capture/adv7180.c b/drivers/media/video/mxc/capture/adv7180.c
new file mode 100644
index 000000000000..07a68ecaa0a5
--- /dev/null
+++ b/drivers/media/video/mxc/capture/adv7180.c
@@ -0,0 +1,981 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file adv7180.c
+ *
+ * @brief Analog Device ADV7180 video decoder functions
+ *
+ * @ingroup Camera
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/wait.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-int-device.h>
+#include "mxc_v4l2_capture.h"
+
+static struct regulator *dvddio_regulator;
+static struct regulator *dvdd_regulator;
+static struct regulator *avdd_regulator;
+static struct regulator *pvdd_regulator;
+
+extern void gpio_sensor_active(void);
+extern void gpio_sensor_inactive(void);
+
+static int adv7180_probe(struct i2c_client *adapter,
+			 const struct i2c_device_id *id);
+static int adv7180_detach(struct i2c_client *client);
+
+static const struct i2c_device_id adv7180_id[] = {
+	{"adv7180", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, adv7180_id);
+
+static struct i2c_driver adv7180_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "adv7180",
+		   },
+	.probe = adv7180_probe,
+	.remove = adv7180_detach,
+	.id_table = adv7180_id,
+};
+
+/*!
+ * Maintains the information on the current state of the sesor.
+ */
+struct sensor {
+	struct v4l2_int_device *v4l2_int_device;
+	struct i2c_client *i2c_client;
+	struct v4l2_pix_format pix;
+	struct v4l2_captureparm streamcap;
+	bool on;
+
+	/* control settings */
+	int brightness;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+	v4l2_std_id std_id;
+} adv7180_data;
+
+/*! List of input video formats supported. The video formats is corresponding
+ * with v4l2 id in video_fmt_t
+ */
+typedef enum {
+	ADV7180_NTSC = 0,	/*!< Locked on (M) NTSC video signal. */
+	ADV7180_PAL,		/*!< (B, G, H, I, N)PAL video signal. */
+	ADV7180_NOT_LOCKED,	/*!< Not locked on a signal. */
+} video_fmt_idx;
+
+/*! Number of video standards supported (including 'not locked' signal). */
+#define ADV7180_STD_MAX		(ADV7180_PAL + 1)
+
+/*! Video format structure. */
+typedef struct {
+	int v4l2_id;		/*!< Video for linux ID. */
+	char name[16];		/*!< Name (e.g., "NTSC", "PAL", etc.) */
+	u16 raw_width;		/*!< Raw width. */
+	u16 raw_height;		/*!< Raw height. */
+	u16 active_width;	/*!< Active width. */
+	u16 active_height;	/*!< Active height. */
+} video_fmt_t;
+
+/*! Description of video formats supported.
+ *
+ *  PAL: raw=720x625, active=720x576.
+ *  NTSC: raw=720x525, active=720x480.
+ */
+static video_fmt_t video_fmts[] = {
+	{			/*! NTSC */
+	 .v4l2_id = V4L2_STD_NTSC,
+	 .name = "NTSC",
+	 .raw_width = 720 - 1,	/* SENS_FRM_WIDTH */
+	 .raw_height = 288 - 1,	/* SENS_FRM_HEIGHT */
+	 .active_width = 720,	/* ACT_FRM_WIDTH plus 1 */
+	 .active_height = (480 / 2),	/* ACT_FRM_WIDTH plus 1 */
+	 },
+	{			/*! (B, G, H, I, N) PAL */
+	 .v4l2_id = V4L2_STD_PAL,
+	 .name = "PAL",
+	 .raw_width = 720 - 1,
+	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .active_width = 720,
+	 .active_height = (576 / 2),
+	 },
+	{			/*! Unlocked standard */
+	 .v4l2_id = V4L2_STD_ALL,
+	 .name = "Autodetect",
+	 .raw_width = 720 - 1,
+	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .active_width = 720,
+	 .active_height = (576 / 2),
+	 },
+};
+
+/*!* Standard index of ADV7180. */
+static video_fmt_idx video_idx = ADV7180_PAL;
+
+/*! @brief This mutex is used to provide mutual exclusion.
+ *
+ *  Create a mutex that can be used to provide mutually exclusive
+ *  read/write access to the globally accessible data structures
+ *  and variables that were defined above.
+ */
+static DECLARE_MUTEX(mutex);
+
+#define IF_NAME                    "adv7180"
+#define ADV7180_INPUT_CTL              0x00	/* Input Control */
+#define ADV7180_STATUS_1               0x10	/* Status #1 */
+#define ADV7180_BRIGHTNESS             0x0a	/* Brightness */
+#define ADV7180_IDENT                  0x11	/* IDENT */
+#define ADV7180_VSYNC_FIELD_CTL_1      0x31	/* VSYNC Field Control #1 */
+#define ADV7180_MANUAL_WIN_CTL         0x3d	/* Manual Window Control */
+#define ADV7180_SD_SATURATION_CB       0xe3	/* SD Saturation Cb */
+#define ADV7180_SD_SATURATION_CR       0xe4	/* SD Saturation Cr */
+#define ADV7180_PWR_MNG                0x0f     /* Power Management */
+
+/* supported controls */
+/* This hasn't been fully implemented yet.
+ * This is how it should work, though. */
+static struct v4l2_queryctrl adv7180_qctrl[] = {
+	{
+	.id = V4L2_CID_BRIGHTNESS,
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.name = "Brightness",
+	.minimum = 0,		/* check this value */
+	.maximum = 255,		/* check this value */
+	.step = 1,		/* check this value */
+	.default_value = 127,	/* check this value */
+	.flags = 0,
+	}, {
+	.id = V4L2_CID_SATURATION,
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.name = "Saturation",
+	.minimum = 0,		/* check this value */
+	.maximum = 255,		/* check this value */
+	.step = 0x1,		/* check this value */
+	.default_value = 127,	/* check this value */
+	.flags = 0,
+	}
+};
+
+/***********************************************************************
+ * I2C transfert.
+ ***********************************************************************/
+
+/*! Read one register from a ADV7180 i2c slave device.
+ *
+ *  @param *reg		register in the device we wish to access.
+ *
+ *  @return		       0 if success, an error code otherwise.
+ */
+static inline int adv7180_read(u8 reg)
+{
+	int val;
+	val = i2c_smbus_read_byte_data(adv7180_data.i2c_client, reg);
+	if (val < 0) {
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"%s:read reg error: reg=%2x \n", __func__, reg);
+		return -1;
+	}
+	return val;
+}
+
+/*! Write one register of a ADV7180 i2c slave device.
+ *
+ *  @param *reg		register in the device we wish to access.
+ *
+ *  @return		       0 if success, an error code otherwise.
+ */
+static int adv7180_write_reg(u8 reg, u8 val)
+{
+	if (i2c_smbus_write_byte_data(adv7180_data.i2c_client, reg, val) < 0) {
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"%s:write reg error:reg=%2x,val=%2x\n", __func__,
+			reg, val);
+		return -1;
+	}
+	return 0;
+}
+
+/***********************************************************************
+ * mxc_v4l2_capture interface.
+ ***********************************************************************/
+
+/*!
+ * Return attributes of current video standard.
+ * Since this device autodetects the current standard, this function also
+ * sets the values that need to be changed if the standard changes.
+ * There is no set std equivalent function.
+ *
+ *  @return		None.
+ */
+static void adv7180_get_std(v4l2_std_id *std)
+{
+	int tmp;
+	int idx;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_get_std\n");
+
+	/* Read the AD_RESULT to get the detect output video standard */
+	tmp = adv7180_read(ADV7180_STATUS_1) & 0x70;
+
+	down(&mutex);
+	if (tmp == 0x40) {
+		/* PAL */
+		*std = V4L2_STD_PAL;
+		idx = ADV7180_PAL;
+	} else if (tmp == 0) {
+		/*NTSC*/
+		*std = V4L2_STD_NTSC;
+		idx = ADV7180_NTSC;
+	} else {
+		*std = V4L2_STD_ALL;
+		idx = ADV7180_NOT_LOCKED;
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"Got invalid video standard! \n");
+	}
+	up(&mutex);
+
+	/* This assumes autodetect which this device uses. */
+	if (*std != adv7180_data.std_id) {
+		video_idx = idx;
+		adv7180_data.std_id = *std;
+		adv7180_data.pix.width = video_fmts[video_idx].raw_width;
+		adv7180_data.pix.height = video_fmts[video_idx].raw_height;
+	}
+}
+
+/***********************************************************************
+ * IOCTL Functions from v4l2_int_ioctl_desc.
+ ***********************************************************************/
+
+/*!
+ * ioctl_g_ifparm - V4L2 sensor interface handler for vidioc_int_g_ifparm_num
+ * s: pointer to standard V4L2 device structure
+ * p: pointer to standard V4L2 vidioc_int_g_ifparm_num ioctl structure
+ *
+ * Gets slave interface parameters.
+ * Calculates the required xclk value to support the requested
+ * clock parameters in p.  This value is returned in the p
+ * parameter.
+ *
+ * vidioc_int_g_ifparm returns platform-specific information about the
+ * interface settings used by the sensor.
+ *
+ * Called on open.
+ */
+static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_g_ifparm\n");
+
+	if (s == NULL) {
+		pr_err("   ERROR!! no slave device set!\n");
+		return -1;
+	}
+
+	/* Initialize structure to 0s then set any non-0 values. */
+	memset(p, 0, sizeof(*p));
+	p->if_type = V4L2_IF_TYPE_BT656; /* This is the only possibility. */
+	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.nobt_hs_inv = 1;
+
+	/* ADV7180 has a dedicated clock so no clock settings needed. */
+
+	return 0;
+}
+
+/*!
+ * Sets the camera power.
+ *
+ * s  pointer to the camera device
+ * on if 1, power is to be turned on.  0 means power is to be turned off
+ *
+ * ioctl_s_power - V4L2 sensor interface handler for vidioc_int_s_power_num
+ * @s: pointer to standard V4L2 device structure
+ * @on: power state to which device is to be set
+ *
+ * Sets devices power state to requrested state, if possible.
+ * This is called on open, close, suspend and resume.
+ */
+static int ioctl_s_power(struct v4l2_int_device *s, int on)
+{
+	struct sensor *sensor = s->priv;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_s_power\n");
+
+	if (on && !sensor->on) {
+		gpio_sensor_active();
+		if (adv7180_write_reg(ADV7180_PWR_MNG, 0) != 0)
+			return -EIO;
+	} else if (!on && sensor->on) {
+		if (adv7180_write_reg(ADV7180_PWR_MNG, 0x24) != 0)
+			return -EIO;
+		gpio_sensor_inactive();
+	}
+
+	sensor->on = on;
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_parm - V4L2 sensor interface handler for VIDIOC_G_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the sensor's video CAPTURE parameters.
+ */
+static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	struct sensor *sensor = s->priv;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_g_parm\n");
+
+	switch (a->type) {
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		memset(a, 0, sizeof(*a));
+		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cparm->capability = sensor->streamcap.capability;
+		cparm->timeperframe = sensor->streamcap.timeperframe;
+		cparm->capturemode = sensor->streamcap.capturemode;
+		break;
+
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		break;
+
+	default:
+		pr_debug("ioctl_g_parm:type is unknown %d\n", a->type);
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * ioctl_s_parm - V4L2 sensor interface handler for VIDIOC_S_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the sensor to use the input parameters, if possible.  If
+ * not possible, reverts to the old parameters and returns the
+ * appropriate error code.
+ *
+ * This driver cannot change these settings.
+ */
+static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_s_parm\n");
+
+	switch (a->type) {
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		break;
+
+	default:
+		pr_debug("   type is unknown - %d\n", a->type);
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_fmt_cap - V4L2 sensor interface handler for ioctl_g_fmt_cap
+ * @s: pointer to standard V4L2 device structure
+ * @f: pointer to standard V4L2 v4l2_format structure
+ *
+ * Returns the sensor's current pixel format in the v4l2_format
+ * parameter.
+ */
+static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	struct sensor *sensor = s->priv;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_g_fmt_cap\n");
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   Returning size of %dx%d\n",
+			 sensor->pix.width, sensor->pix.height);
+		f->fmt.pix = sensor->pix;
+		break;
+
+	case V4L2_BUF_TYPE_PRIVATE: {
+		v4l2_std_id std;
+		adv7180_get_std(&std);
+		f->fmt.pix.pixelformat = (u32)std;
+		}
+		break;
+
+	default:
+		f->fmt.pix = sensor->pix;
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * ioctl_queryctrl - V4L2 sensor interface handler for VIDIOC_QUERYCTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @qc: standard V4L2 VIDIOC_QUERYCTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control information
+ * from the video_control[] array.  Otherwise, returns -EINVAL if the
+ * control is not supported.
+ */
+static int ioctl_queryctrl(struct v4l2_int_device *s,
+			   struct v4l2_queryctrl *qc)
+{
+	int i;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_queryctrl\n");
+
+	for (i = 0; i < ARRAY_SIZE(adv7180_qctrl); i++)
+		if (qc->id && qc->id == adv7180_qctrl[i].id) {
+			memcpy(qc, &(adv7180_qctrl[i]),
+				sizeof(*qc));
+			return (0);
+		}
+
+	return -EINVAL;
+}
+
+/*!
+ * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_G_CTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control's current
+ * value from the video_control[] array.  Otherwise, returns -EINVAL
+ * if the control is not supported.
+ */
+static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int ret = 0;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_g_ctrl\n");
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_BRIGHTNESS\n");
+		adv7180_data.brightness = adv7180_read(ADV7180_BRIGHTNESS);
+		vc->value = adv7180_data.brightness;
+		break;
+	case V4L2_CID_CONTRAST:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_CONTRAST\n");
+		vc->value = adv7180_data.contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_SATURATION\n");
+		adv7180_data.saturation = adv7180_read(ADV7180_SD_SATURATION_CB);
+		vc->value = adv7180_data.saturation;
+		break;
+	case V4L2_CID_HUE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_HUE\n");
+		vc->value = adv7180_data.hue;
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_DO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_RED_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_RED_BALANCE\n");
+		vc->value = adv7180_data.red;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_BLUE_BALANCE\n");
+		vc->value = adv7180_data.blue;
+		break;
+	case V4L2_CID_GAMMA:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_GAMMA\n");
+		break;
+	case V4L2_CID_EXPOSURE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_EXPOSURE\n");
+		vc->value = adv7180_data.ae_mode;
+		break;
+	case V4L2_CID_AUTOGAIN:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_AUTOGAIN\n");
+		break;
+	case V4L2_CID_GAIN:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_GAIN\n");
+		break;
+	case V4L2_CID_HFLIP:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_HFLIP\n");
+		break;
+	case V4L2_CID_VFLIP:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_VFLIP\n");
+		break;
+	default:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   Default case\n");
+		vc->value = 0;
+		ret = -EPERM;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_ctrl - V4L2 sensor interface handler for VIDIOC_S_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_S_CTRL ioctl structure
+ *
+ * If the requested control is supported, sets the control's current
+ * value in HW (and updates the video_control[] array).  Otherwise,
+ * returns -EINVAL if the control is not supported.
+ */
+static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int retval = 0;
+	u8 tmp;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_s_ctrl\n");
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_BRIGHTNESS\n");
+		tmp = vc->value;
+		adv7180_write_reg(ADV7180_BRIGHTNESS, tmp);
+		adv7180_data.brightness = vc->value;
+		break;
+	case V4L2_CID_CONTRAST:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_CONTRAST\n");
+		break;
+	case V4L2_CID_SATURATION:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_SATURATION\n");
+		tmp = vc->value;
+		adv7180_write_reg(ADV7180_SD_SATURATION_CB, tmp);
+		adv7180_write_reg(ADV7180_SD_SATURATION_CR, tmp);
+		adv7180_data.saturation = vc->value;
+		break;
+	case V4L2_CID_HUE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_HUE\n");
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_DO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_RED_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_RED_BALANCE\n");
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_BLUE_BALANCE\n");
+		break;
+	case V4L2_CID_GAMMA:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_GAMMA\n");
+		break;
+	case V4L2_CID_EXPOSURE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_EXPOSURE\n");
+		break;
+	case V4L2_CID_AUTOGAIN:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_AUTOGAIN\n");
+		break;
+	case V4L2_CID_GAIN:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_GAIN\n");
+		break;
+	case V4L2_CID_HFLIP:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_HFLIP\n");
+		break;
+	case V4L2_CID_VFLIP:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_VFLIP\n");
+		break;
+	default:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   Default case\n");
+		retval = -EPERM;
+		break;
+	}
+
+	return retval;
+}
+
+/*!
+ * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
+ * @s: pointer to standard V4L2 device structure
+ */
+static int ioctl_init(struct v4l2_int_device *s)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_init\n");
+	return 0;
+}
+
+/*!
+ * ioctl_dev_init - V4L2 sensor interface handler for vidioc_int_dev_init_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Initialise the device when slave attaches to the master.
+ */
+static int ioctl_dev_init(struct v4l2_int_device *s)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_dev_init\n");
+	return 0;
+}
+
+/*!
+ * This structure defines all the ioctls for this module.
+ */
+static struct v4l2_int_ioctl_desc adv7180_ioctl_desc[] = {
+
+	{vidioc_int_dev_init_num, (v4l2_int_ioctl_func *)ioctl_dev_init},
+
+	/*!
+	 * Delinitialise the dev. at slave detach.
+	 * The complement of ioctl_dev_init.
+	 */
+/*	{vidioc_int_dev_exit_num, (v4l2_int_ioctl_func *)ioctl_dev_exit}, */
+
+	{vidioc_int_s_power_num, (v4l2_int_ioctl_func *)ioctl_s_power},
+	{vidioc_int_g_ifparm_num, (v4l2_int_ioctl_func *)ioctl_g_ifparm},
+/*	{vidioc_int_g_needs_reset_num,
+				(v4l2_int_ioctl_func *)ioctl_g_needs_reset}, */
+/*	{vidioc_int_reset_num, (v4l2_int_ioctl_func *)ioctl_reset}, */
+	{vidioc_int_init_num, (v4l2_int_ioctl_func *)ioctl_init},
+
+	/*!
+	 * VIDIOC_ENUM_FMT ioctl for the CAPTURE buffer type.
+	 */
+/*	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_enum_fmt_cap}, */
+
+	/*!
+	 * VIDIOC_TRY_FMT ioctl for the CAPTURE buffer type.
+	 * This ioctl is used to negotiate the image capture size and
+	 * pixel format without actually making it take effect.
+	 */
+/*	{vidioc_int_try_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_try_fmt_cap}, */
+
+	{vidioc_int_g_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_g_fmt_cap},
+
+	/*!
+	 * If the requested format is supported, configures the HW to use that
+	 * format, returns error code if format not supported or HW can't be
+	 * correctly configured.
+	 */
+/*	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_fmt_cap}, */
+
+	{vidioc_int_g_parm_num, (v4l2_int_ioctl_func *)ioctl_g_parm},
+	{vidioc_int_s_parm_num, (v4l2_int_ioctl_func *)ioctl_s_parm},
+	{vidioc_int_queryctrl_num, (v4l2_int_ioctl_func *)ioctl_queryctrl},
+	{vidioc_int_g_ctrl_num, (v4l2_int_ioctl_func *)ioctl_g_ctrl},
+	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *)ioctl_s_ctrl},
+};
+
+static struct v4l2_int_slave adv7180_slave = {
+	.ioctls = adv7180_ioctl_desc,
+	.num_ioctls = ARRAY_SIZE(adv7180_ioctl_desc),
+};
+
+static struct v4l2_int_device adv7180_int_device = {
+	.module = THIS_MODULE,
+	.name = "adv7180",
+	.type = v4l2_int_type_slave,
+	.u = {
+		.slave = &adv7180_slave,
+	},
+};
+
+
+/***********************************************************************
+ * I2C client and driver.
+ ***********************************************************************/
+
+/*! ADV7180 Reset function.
+ *
+ *  @return		None.
+ */
+static void adv7180_hard_reset(void)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev,
+		"In adv7180:adv7180_hard_reset\n");
+
+	/*! Driver works fine without explicit register
+	 * initialization. Furthermore, initializations takes about 2 seconds
+	 * at startup...
+	 */
+
+	/*! Set YPbPr input on AIN1,4,5 and normal
+	 * operations(autodection of all stds).
+	 */
+	adv7180_write_reg(ADV7180_INPUT_CTL, 0x09);
+
+	/*! Datasheet recommends: */
+	adv7180_write_reg(ADV7180_VSYNC_FIELD_CTL_1, 0x02);
+	adv7180_write_reg(ADV7180_MANUAL_WIN_CTL, 0xa2);
+}
+
+/*! ADV7180 I2C attach function.
+ *
+ *  @param *adapter	struct i2c_adapter *.
+ *
+ *  @return		Error code indicating success or failure.
+ */
+
+/*!
+ * ADV7180 I2C probe function.
+ * Function set in i2c_driver struct.
+ * Called by insmod.
+ *
+ *  @param *adapter	I2C adapter descriptor.
+ *
+ *  @return		Error code indicating success or failure.
+ */
+static int adv7180_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	int rev_id;
+	int ret = 0;
+	struct mxc_tvin_platform_data *plat_data = client->dev.platform_data;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_probe\n");
+
+	if (plat_data->dvddio_reg) {
+		dvddio_regulator =
+		    regulator_get(&client->dev, plat_data->dvddio_reg);
+		if (!IS_ERR_VALUE((unsigned long)dvddio_regulator)) {
+			regulator_set_voltage(dvddio_regulator, 3300000, 3300000);
+			if (regulator_enable(dvddio_regulator) != 0)
+				return -ENODEV;
+		}
+	}
+
+	if (plat_data->dvdd_reg) {
+		dvdd_regulator =
+		    regulator_get(&client->dev, plat_data->dvdd_reg);
+		if (!IS_ERR_VALUE((unsigned long)dvdd_regulator)) {
+			regulator_set_voltage(dvdd_regulator, 1800000, 1800000);
+			if (regulator_enable(dvdd_regulator) != 0)
+				return -ENODEV;
+		}
+	}
+
+	if (plat_data->avdd_reg) {
+		avdd_regulator =
+		    regulator_get(&client->dev, plat_data->avdd_reg);
+		if (!IS_ERR_VALUE((unsigned long)avdd_regulator)) {
+			regulator_set_voltage(avdd_regulator, 1800000, 1800000);
+			if (regulator_enable(avdd_regulator) != 0)
+				return -ENODEV;
+		}
+	}
+
+	if (plat_data->pvdd_reg) {
+		pvdd_regulator =
+		    regulator_get(&client->dev, plat_data->pvdd_reg);
+		if (!IS_ERR_VALUE((unsigned long)pvdd_regulator)) {
+			regulator_set_voltage(pvdd_regulator, 1800000, 1800000);
+			if (regulator_enable(pvdd_regulator) != 0)
+				return -ENODEV;
+		}
+	}
+
+	if (plat_data->reset)
+		plat_data->reset();
+
+	if (plat_data->pwdn)
+		plat_data->pwdn(1);
+
+	msleep(1);
+
+	/* Set initial values for the sensor struct. */
+	memset(&adv7180_data, 0, sizeof(adv7180_data));
+	adv7180_data.i2c_client = client;
+	adv7180_data.streamcap.timeperframe.denominator = 30;
+	adv7180_data.streamcap.timeperframe.numerator = 1;
+	adv7180_data.std_id = V4L2_STD_ALL;
+	video_idx = ADV7180_NOT_LOCKED;
+	adv7180_data.pix.width = video_fmts[video_idx].raw_width;
+	adv7180_data.pix.height = video_fmts[video_idx].raw_height;
+	adv7180_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;  /* YUV422 */
+	adv7180_data.pix.priv = 1;  /* 1 is used to indicate TV in */
+	adv7180_data.on = true;
+
+	gpio_sensor_active();
+
+	dev_dbg(&adv7180_data.i2c_client->dev,
+		"%s:adv7180 probe i2c address is 0x%02X \n",
+		__func__, adv7180_data.i2c_client->addr);
+
+	/*! Read the revision ID of the tvin chip */
+	rev_id = adv7180_read(ADV7180_IDENT);
+	dev_dbg(&adv7180_data.i2c_client->dev,
+		"%s:Analog Device adv7%2X0 detected! \n", __func__,
+		rev_id);
+
+	/*! ADV7180 initialization. */
+	adv7180_hard_reset();
+
+	pr_debug("   type is %d (expect %d)\n",
+		 adv7180_int_device.type, v4l2_int_type_slave);
+	pr_debug("   num ioctls is %d\n",
+		 adv7180_int_device.u.slave->num_ioctls);
+
+	/* This function attaches this structure to the /dev/video0 device.
+	 * The pointer in priv points to the mt9v111_data structure here.*/
+	adv7180_int_device.priv = &adv7180_data;
+	ret = v4l2_int_device_register(&adv7180_int_device);
+
+	return ret;
+}
+
+/*!
+ * ADV7180 I2C detach function.
+ * Called on rmmod.
+ *
+ *  @param *client	struct i2c_client*.
+ *
+ *  @return		Error code indicating success or failure.
+ */
+static int adv7180_detach(struct i2c_client *client)
+{
+	struct mxc_tvin_platform_data *plat_data = client->dev.platform_data;
+
+	dev_dbg(&adv7180_data.i2c_client->dev,
+		"%s:Removing %s video decoder @ 0x%02X from adapter %s \n",
+		__func__, IF_NAME, client->addr << 1, client->adapter->name);
+
+	if (plat_data->pwdn)
+		plat_data->pwdn(0);
+
+	if (dvddio_regulator) {
+		regulator_disable(dvddio_regulator);
+		regulator_put(dvddio_regulator);
+	}
+
+	if (dvdd_regulator) {
+		regulator_disable(dvdd_regulator);
+		regulator_put(dvdd_regulator);
+	}
+
+	if (avdd_regulator) {
+		regulator_disable(avdd_regulator);
+		regulator_put(avdd_regulator);
+	}
+
+	if (pvdd_regulator) {
+		regulator_disable(pvdd_regulator);
+		regulator_put(pvdd_regulator);
+	}
+
+	v4l2_int_device_unregister(&adv7180_int_device);
+
+	return 0;
+}
+
+/*!
+ * ADV7180 init function.
+ * Called on insmod.
+ *
+ * @return    Error code indicating success or failure.
+ */
+static __init int adv7180_init(void)
+{
+	u8 err = 0;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_init\n");
+
+	/* Tells the i2c driver what functions to call for this driver. */
+	err = i2c_add_driver(&adv7180_i2c_driver);
+	if (err != 0)
+		pr_err("%s:driver registration failed, error=%d \n",
+			__func__, err);
+
+	return err;
+}
+
+/*!
+ * ADV7180 cleanup function.
+ * Called on rmmod.
+ *
+ * @return   Error code indicating success or failure.
+ */
+static void __exit adv7180_clean(void)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_clean\n");
+	i2c_del_driver(&adv7180_i2c_driver);
+	gpio_sensor_inactive();
+}
+
+module_init(adv7180_init);
+module_exit(adv7180_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor");
+MODULE_DESCRIPTION("Anolog Device ADV7180 video decoder driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/csi_v4l2_capture.c b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
new file mode 100644
index 000000000000..3266d2500081
--- /dev/null
+++ b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
@@ -0,0 +1,1024 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/media/video/mxc/capture/csi_v4l2_capture.c
+ * This file is derived from mxc_v4l2_capture.c
+ *
+ * @brief MX25 Video For Linux 2 driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/io.h>
+#include <linux/semaphore.h>
+#include <linux/pagemap.h>
+#include <linux/vmalloc.h>
+#include <linux/types.h>
+#include <linux/fb.h>
+#include <linux/dma-mapping.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-int-device.h>
+#include <linux/mxcfb.h>
+#include "mxc_v4l2_capture.h"
+#include "fsl_csi.h"
+
+static int video_nr = -1;
+static cam_data *g_cam;
+
+static int csi_v4l2_master_attach(struct v4l2_int_device *slave);
+static void csi_v4l2_master_detach(struct v4l2_int_device *slave);
+static u8 camera_power(cam_data *cam, bool cameraOn);
+
+/*! Information about this driver. */
+static struct v4l2_int_master csi_v4l2_master = {
+	.attach = csi_v4l2_master_attach,
+	.detach = csi_v4l2_master_detach,
+};
+
+static struct v4l2_int_device csi_v4l2_int_device = {
+	.module = THIS_MODULE,
+	.name = "csi_v4l2_cap",
+	.type = v4l2_int_type_master,
+	.u = {
+	      .master = &csi_v4l2_master,
+	      },
+};
+
+/*!
+ * Indicates whether the palette is supported.
+ *
+ * @param palette V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_UYVY or V4L2_PIX_FMT_YUV420
+ *
+ * @return 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	return (palette == V4L2_PIX_FMT_RGB565) ||
+	    (palette == V4L2_PIX_FMT_UYVY) || (palette == V4L2_PIX_FMT_YUV420);
+}
+
+/*!
+ * start the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int start_preview(cam_data *cam)
+{
+	unsigned long fb_addr = (unsigned long)cam->v4l2_fb.base;
+
+	__raw_writel(fb_addr, CSI_CSIDMASA_FB1);
+	__raw_writel(fb_addr, CSI_CSIDMASA_FB2);
+	__raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+
+	csi_enable_int(0);
+
+	return 0;
+}
+
+/*!
+ * shut down the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int stop_preview(cam_data *cam)
+{
+	csi_disable_int();
+
+	/* set CSI_CSIDMASA_FB1 and CSI_CSIDMASA_FB2 to default value */
+	__raw_writel(0, CSI_CSIDMASA_FB1);
+	__raw_writel(0, CSI_CSIDMASA_FB2);
+	__raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+
+	return 0;
+}
+
+/***************************************************************************
+ * VIDIOC Functions.
+ **************************************************************************/
+
+/*!
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int csi_v4l2_g_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		f->fmt.pix = cam->v2f.fmt.pix;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_OVERLAY\n");
+		f->fmt.win = cam->win;
+		break;
+	default:
+		pr_debug("   type is invalid\n");
+		retval = -EINVAL;
+	}
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__, cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+
+	return retval;
+}
+
+/*!
+ * V4L2 - csi_v4l2_s_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int csi_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+	int size = 0;
+	int bytesperline = 0;
+	int *width, *height;
+
+	pr_debug("In MVC: %s\n", __func__);
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type=V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		if (!valid_mode(f->fmt.pix.pixelformat)) {
+			pr_err("ERROR: v4l2 capture: %s: format "
+			       "not supported\n", __func__);
+			return -EINVAL;
+		}
+
+		/* Handle case where size requested is larger than cuurent
+		 * camera setting. */
+		if ((f->fmt.pix.width > cam->crop_bounds.width)
+		    || (f->fmt.pix.height > cam->crop_bounds.height)) {
+			/* Need the logic here, calling vidioc_s_param if
+			 * camera can change. */
+			pr_debug("csi_v4l2_s_fmt size changed\n");
+		}
+		if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+			height = &f->fmt.pix.width;
+			width = &f->fmt.pix.height;
+		} else {
+			width = &f->fmt.pix.width;
+			height = &f->fmt.pix.height;
+		}
+
+		if ((cam->crop_bounds.width / *width > 8) ||
+		    ((cam->crop_bounds.width / *width == 8) &&
+		     (cam->crop_bounds.width % *width))) {
+			*width = cam->crop_bounds.width / 8;
+			if (*width % 8)
+				*width += 8 - *width % 8;
+			pr_err("ERROR: v4l2 capture: width exceeds limit "
+			       "resize to %d.\n", *width);
+		}
+
+		if ((cam->crop_bounds.height / *height > 8) ||
+		    ((cam->crop_bounds.height / *height == 8) &&
+		     (cam->crop_bounds.height % *height))) {
+			*height = cam->crop_bounds.height / 8;
+			if (*height % 8)
+				*height += 8 - *height % 8;
+			pr_err("ERROR: v4l2 capture: height exceeds limit "
+			       "resize to %d.\n", *height);
+		}
+
+		switch (f->fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_RGB565:
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			csi_set_16bit_imagpara(f->fmt.pix.width,
+					       f->fmt.pix.height);
+			bytesperline = f->fmt.pix.width * 2;
+			break;
+		case V4L2_PIX_FMT_UYVY:
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			csi_set_16bit_imagpara(f->fmt.pix.width,
+					       f->fmt.pix.height);
+			bytesperline = f->fmt.pix.width * 2;
+			break;
+		case V4L2_PIX_FMT_YUV420:
+			size = f->fmt.pix.width * f->fmt.pix.height * 3 / 2;
+			csi_set_12bit_imagpara(f->fmt.pix.width,
+					       f->fmt.pix.height);
+			bytesperline = f->fmt.pix.width;
+			break;
+		case V4L2_PIX_FMT_YUV422P:
+		case V4L2_PIX_FMT_RGB24:
+		case V4L2_PIX_FMT_BGR24:
+		case V4L2_PIX_FMT_BGR32:
+		case V4L2_PIX_FMT_RGB32:
+		case V4L2_PIX_FMT_NV12:
+		default:
+			pr_debug("   case not supported\n");
+			break;
+		}
+
+		if (f->fmt.pix.bytesperline < bytesperline)
+			f->fmt.pix.bytesperline = bytesperline;
+		else
+			bytesperline = f->fmt.pix.bytesperline;
+
+		if (f->fmt.pix.sizeimage < size)
+			f->fmt.pix.sizeimage = size;
+		else
+			size = f->fmt.pix.sizeimage;
+
+		cam->v2f.fmt.pix = f->fmt.pix;
+
+		if (cam->v2f.fmt.pix.priv != 0) {
+			if (copy_from_user(&cam->offset,
+					   (void *)cam->v2f.fmt.pix.priv,
+					   sizeof(cam->offset))) {
+				retval = -EFAULT;
+				break;
+			}
+		}
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		pr_debug("   type=V4L2_BUF_TYPE_VIDEO_OVERLAY\n");
+		cam->win = f->fmt.win;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__, cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+
+	return retval;
+}
+
+/*!
+ * V4L2 - csi_v4l2_s_param function
+ * Allows setting of capturemode and frame rate.
+ *
+ * @param cam         structure cam_data *
+ * @param parm        structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int csi_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
+{
+	struct v4l2_ifparm ifparm;
+	struct v4l2_format cam_fmt;
+	struct v4l2_streamparm currentparm;
+	int err = 0;
+
+	pr_debug("In %s\n", __func__);
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pr_err(KERN_ERR "%s invalid type\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Stop the viewfinder */
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+
+	currentparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* First check that this device can support the changes requested. */
+	err = vidioc_int_g_parm(cam->sensor, &currentparm);
+	if (err) {
+		pr_err("%s: vidioc_int_g_parm returned an error %d\n",
+		       __func__, err);
+		goto exit;
+	}
+
+	pr_debug("   Current capabilities are %x\n",
+		 currentparm.parm.capture.capability);
+	pr_debug("   Current capturemode is %d  change to %d\n",
+		 currentparm.parm.capture.capturemode,
+		 parm->parm.capture.capturemode);
+	pr_debug("   Current framerate is %d  change to %d\n",
+		 currentparm.parm.capture.timeperframe.denominator,
+		 parm->parm.capture.timeperframe.denominator);
+
+	err = vidioc_int_s_parm(cam->sensor, parm);
+	if (err) {
+		pr_err("%s: vidioc_int_s_parm returned an error %d\n",
+		       __func__, err);
+		goto exit;
+	}
+
+	vidioc_int_g_ifparm(cam->sensor, &ifparm);
+	cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	pr_debug("   g_fmt_cap returns widthxheight of input as %d x %d\n",
+		 cam_fmt.fmt.pix.width, cam_fmt.fmt.pix.height);
+
+exit:
+	return err;
+}
+
+/*!
+ * V4L interface - open function
+ *
+ * @param inode        structure inode *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int csi_v4l_open(struct inode *inode, struct file *file)
+{
+	struct v4l2_ifparm ifparm;
+	struct v4l2_format cam_fmt;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int err = 0;
+
+	pr_debug("   device name is %s\n", dev->name);
+
+	if (!cam) {
+		pr_err("ERROR: v4l2 capture: Internal error, "
+		       "cam_data not found!\n");
+		return -EBADF;
+	}
+
+	down(&cam->busy_lock);
+	err = 0;
+	if (signal_pending(current))
+		goto oops;
+
+	if (cam->open_count++ == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+
+		vidioc_int_g_ifparm(cam->sensor, &ifparm);
+
+		cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		csi_enable_mclk(CSI_MCLK_I2C, true, true);
+		vidioc_int_init(cam->sensor);
+	}
+
+	file->private_data = dev;
+
+oops:
+	up(&cam->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L interface - close function
+ *
+ * @param inode    struct inode *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int csi_v4l_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	int err = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	pr_debug("In MVC:%s\n", __func__);
+
+	if (!cam) {
+		pr_err("ERROR: v4l2 capture: Internal error, "
+		       "cam_data not found!\n");
+		return -EBADF;
+	}
+
+	/* for the case somebody hit the ctrl C */
+	if (cam->overlay_pid == current->pid) {
+		err = stop_preview(cam);
+		cam->overlay_on = false;
+	}
+
+	if (--cam->open_count == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+		file->private_data = NULL;
+		csi_enable_mclk(CSI_MCLK_I2C, false, false);
+	}
+
+	return err;
+}
+
+/*
+ * V4L interface - read function
+ *
+ * @param file       struct file *
+ * @param read buf   char *
+ * @param count      size_t
+ * @param ppos       structure loff_t *
+ *
+ * @return           bytes read
+ */
+static ssize_t csi_v4l_read(struct file *file, char *buf, size_t count,
+			    loff_t *ppos)
+{
+	int err = 0;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	/* Stop the viewfinder */
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+
+	if (cam->still_buf_vaddr == NULL) {
+		cam->still_buf_vaddr = dma_alloc_coherent(0,
+							  PAGE_ALIGN
+							  (cam->v2f.fmt.
+							   pix.sizeimage),
+							  &cam->
+							  still_buf,
+							  GFP_DMA | GFP_KERNEL);
+		if (cam->still_buf_vaddr == NULL) {
+			pr_err("alloc dma memory failed\n");
+			return -ENOMEM;
+		}
+		cam->still_counter = 0;
+		__raw_writel(cam->still_buf, CSI_CSIDMASA_FB2);
+		__raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF,
+			     CSI_CSICR3);
+		__raw_writel(__raw_readl(CSI_CSISR), CSI_CSISR);
+		__raw_writel(__raw_readl(CSI_CSICR3) | BIT_FRMCNT_RST,
+			     CSI_CSICR3);
+		csi_enable_int(1);
+	}
+
+	wait_event_interruptible(cam->still_queue, cam->still_counter);
+	csi_disable_int();
+	err = copy_to_user(buf, cam->still_buf_vaddr,
+			   cam->v2f.fmt.pix.sizeimage);
+
+	if (cam->still_buf_vaddr != NULL) {
+		dma_free_coherent(0, PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+				  cam->still_buf_vaddr, cam->still_buf);
+		cam->still_buf = 0;
+		cam->still_buf_vaddr = NULL;
+	}
+
+	if (cam->overlay_on == true)
+		start_preview(cam);
+
+	up(&cam->busy_lock);
+	if (err < 0)
+		return err;
+
+	return cam->v2f.fmt.pix.sizeimage - err;
+}
+
+/*!
+ * V4L interface - ioctl function
+ *
+ * @param inode      struct inode*
+ *
+ * @param file       struct file*
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void*
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int csi_v4l_do_ioctl(struct inode *inode, struct file *file,
+			    unsigned int ioctlnr, void *arg)
+{
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int retval = 0;
+
+	pr_debug("In MVC: %s, %x\n", __func__, ioctlnr);
+	wait_event_interruptible(cam->power_queue, cam->low_power == false);
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+		/*!
+		 * V4l2 VIDIOC_G_FMT ioctl
+		 */
+	case VIDIOC_G_FMT:{
+			struct v4l2_format *gf = arg;
+			pr_debug("   case VIDIOC_G_FMT\n");
+			retval = csi_v4l2_g_fmt(cam, gf);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_FMT ioctl
+		 */
+	case VIDIOC_S_FMT:{
+			struct v4l2_format *sf = arg;
+			pr_debug("   case VIDIOC_S_FMT\n");
+			retval = csi_v4l2_s_fmt(cam, sf);
+			vidioc_int_s_fmt_cap(cam->sensor, sf);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_OVERLAY ioctl
+		 */
+	case VIDIOC_OVERLAY:{
+			int *on = arg;
+			pr_debug("   case VIDIOC_OVERLAY\n");
+			if (*on) {
+				cam->overlay_on = true;
+				cam->overlay_pid = current->pid;
+				start_preview(cam);
+			}
+			if (!*on) {
+				stop_preview(cam);
+				cam->overlay_on = false;
+			}
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_G_FBUF ioctl
+		 */
+	case VIDIOC_G_FBUF:{
+			struct v4l2_framebuffer *fb = arg;
+			*fb = cam->v4l2_fb;
+			fb->capability = V4L2_FBUF_CAP_EXTERNOVERLAY;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_FBUF ioctl
+		 */
+	case VIDIOC_S_FBUF:{
+			struct v4l2_framebuffer *fb = arg;
+			cam->v4l2_fb = *fb;
+			break;
+		}
+
+	case VIDIOC_G_PARM:{
+			struct v4l2_streamparm *parm = arg;
+			pr_debug("   case VIDIOC_G_PARM\n");
+			vidioc_int_g_parm(cam->sensor, parm);
+			break;
+		}
+
+	case VIDIOC_S_PARM:{
+			struct v4l2_streamparm *parm = arg;
+			pr_debug("   case VIDIOC_S_PARM\n");
+			retval = csi_v4l2_s_param(cam, parm);
+			break;
+		}
+
+	case VIDIOC_QUERYCAP:{
+			struct v4l2_capability *cap = arg;
+			pr_debug("   case VIDIOC_QUERYCAP\n");
+			strcpy(cap->driver, "csi_v4l2");
+			cap->version = KERNEL_VERSION(0, 1, 11);
+			cap->capabilities = V4L2_CAP_VIDEO_OVERLAY |
+			    V4L2_CAP_VIDEO_OUTPUT_OVERLAY | V4L2_CAP_READWRITE;
+			cap->card[0] = '\0';
+			cap->bus_info[0] = '\0';
+			break;
+		}
+
+	case VIDIOC_S_CROP:
+		pr_debug("   case not supported\n");
+		break;
+
+	case VIDIOC_S_CTRL:
+	case VIDIOC_G_STD:
+	case VIDIOC_QBUF:
+	case VIDIOC_QUERYBUF:
+	case VIDIOC_REQBUFS:
+	case VIDIOC_DQBUF:
+	case VIDIOC_G_OUTPUT:
+	case VIDIOC_S_OUTPUT:
+	case VIDIOC_ENUMSTD:
+	case VIDIOC_G_CROP:
+	case VIDIOC_CROPCAP:
+	case VIDIOC_S_STD:
+	case VIDIOC_G_CTRL:
+	case VIDIOC_STREAMOFF:
+	case VIDIOC_STREAMON:
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_ENUMINPUT:
+	case VIDIOC_G_INPUT:
+	case VIDIOC_S_INPUT:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	case VIDIOC_ENUMOUTPUT:
+	default:
+		pr_debug("   case not supported\n");
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&cam->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L interface - ioctl function
+ *
+ * @return  None
+ */
+static int csi_v4l_ioctl(struct inode *inode, struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	return video_usercopy(inode, file, cmd, arg, csi_v4l_do_ioctl);
+}
+
+/*!
+ * V4L interface - mmap function
+ *
+ * @param file        structure file *
+ *
+ * @param vma         structure vm_area_struct *
+ *
+ * @return status     0 Success, EINTR busy lock error, ENOBUFS remap_page error
+ */
+static int csi_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *dev = video_devdata(file);
+	unsigned long size;
+	int res = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	pr_debug("%s\n", __func__);
+	pr_debug("\npgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		 vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	size = vma->vm_end - vma->vm_start;
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		pr_err("ERROR: v4l2 capture: %s : "
+		       "remap_pfn_range failed\n", __func__);
+		res = -ENOBUFS;
+		goto csi_mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+csi_mmap_exit:
+	up(&cam->busy_lock);
+	return res;
+}
+
+/*!
+ * This structure defines the functions to be called in this driver.
+ */
+static struct file_operations csi_v4l_fops = {
+	.owner = THIS_MODULE,
+	.open = csi_v4l_open,
+	.release = csi_v4l_close,
+	.read = csi_v4l_read,
+	.ioctl = csi_v4l_ioctl,
+	.mmap = csi_mmap,
+};
+
+static struct video_device csi_v4l_template = {
+	.name = "Mx25 Camera",
+	.vfl_type = VID_TYPE_CAPTURE,
+	.fops = &csi_v4l_fops,
+	.release = video_device_release,
+};
+
+/*!
+ * This function can be used to release any platform data on closing.
+ */
+static void camera_platform_release(struct device *device)
+{
+}
+
+/*! Device Definition for csi v4l2 device */
+static struct platform_device csi_v4l2_devices = {
+	.name = "csi_v4l2",
+	.dev = {
+		.release = camera_platform_release,
+		},
+	.id = 0,
+};
+
+/*!
+ * initialize cam_data structure
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static void init_camera_struct(cam_data *cam)
+{
+	pr_debug("In MVC: %s\n", __func__);
+
+	/* Default everything to 0 */
+	memset(cam, 0, sizeof(cam_data));
+
+	init_MUTEX(&cam->param_lock);
+	init_MUTEX(&cam->busy_lock);
+
+	cam->video_dev = video_device_alloc();
+	if (cam->video_dev == NULL)
+		return;
+
+	*(cam->video_dev) = csi_v4l_template;
+
+	video_set_drvdata(cam->video_dev, cam);
+	dev_set_drvdata(&csi_v4l2_devices.dev, (void *)cam);
+	cam->video_dev->minor = -1;
+
+	init_waitqueue_head(&cam->still_queue);
+
+	cam->streamparm.parm.capture.capturemode = 0;
+
+	cam->standard.index = 0;
+	cam->standard.id = V4L2_STD_UNKNOWN;
+	cam->standard.frameperiod.denominator = 30;
+	cam->standard.frameperiod.numerator = 1;
+	cam->standard.framelines = 480;
+	cam->standard_autodetect = true;
+	cam->streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	cam->streamparm.parm.capture.timeperframe = cam->standard.frameperiod;
+	cam->streamparm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	cam->overlay_on = false;
+	cam->capture_on = false;
+	cam->skip_frame = 0;
+	cam->v4l2_fb.flags = V4L2_FBUF_FLAG_OVERLAY;
+
+	cam->v2f.fmt.pix.sizeimage = 480 * 640 * 2;
+	cam->v2f.fmt.pix.bytesperline = 640 * 2;
+	cam->v2f.fmt.pix.width = 640;
+	cam->v2f.fmt.pix.height = 480;
+	cam->v2f.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	cam->win.w.width = 160;
+	cam->win.w.height = 160;
+	cam->win.w.left = 0;
+	cam->win.w.top = 0;
+	cam->still_counter = 0;
+
+	csi_start_callback(cam);
+	init_waitqueue_head(&cam->power_queue);
+	spin_lock_init(&cam->int_lock);
+}
+
+/*!
+ * camera_power function
+ *    Turns Sensor power On/Off
+ *
+ * @param       cam           cam data struct
+ * @param       cameraOn      true to turn camera on, false to turn off power.
+ *
+ * @return status
+ */
+static u8 camera_power(cam_data *cam, bool cameraOn)
+{
+	pr_debug("In MVC: %s on=%d\n", __func__, cameraOn);
+
+	if (cameraOn == true) {
+		csi_enable_mclk(CSI_MCLK_I2C, true, true);
+		vidioc_int_s_power(cam->sensor, 1);
+	} else {
+		csi_enable_mclk(CSI_MCLK_I2C, false, false);
+		vidioc_int_s_power(cam->sensor, 0);
+	}
+	return 0;
+}
+
+/*!
+ * This function is called to put the sensor in a low power state.
+ * Refer to the document driver-model/driver.txt in the kernel source tree
+ * for more information.
+ *
+ * @param   pdev  the device structure used to give information on which I2C
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function returns 0 on success and -1 on failure.
+ */
+static int csi_v4l2_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	pr_debug("In MVC: %s\n", __func__);
+
+	if (cam == NULL)
+		return -1;
+
+	cam->low_power = true;
+
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+
+	camera_power(cam, false);
+
+	return 0;
+}
+
+/*!
+ * This function is called to bring the sensor back from a low power state.
+ * Refer to the document driver-model/driver.txt in the kernel source tree
+ * for more information.
+ *
+ * @param   pdev   the device structure
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int csi_v4l2_resume(struct platform_device *pdev)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	pr_debug("In MVC: %s\n", __func__);
+
+	if (cam == NULL)
+		return -1;
+
+	cam->low_power = false;
+	wake_up_interruptible(&cam->power_queue);
+	camera_power(cam, true);
+
+	if (cam->overlay_on == true)
+		start_preview(cam);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver csi_v4l2_driver = {
+	.driver = {
+		   .name = "csi_v4l2",
+		   },
+	.probe = NULL,
+	.remove = NULL,
+#ifdef CONFIG_PM
+	.suspend = csi_v4l2_suspend,
+	.resume = csi_v4l2_resume,
+#endif
+	.shutdown = NULL,
+};
+
+/*!
+ * Initializes the camera driver.
+ */
+static int csi_v4l2_master_attach(struct v4l2_int_device *slave)
+{
+	cam_data *cam = slave->u.slave->master->priv;
+	struct v4l2_format cam_fmt;
+
+	pr_debug("In MVC: %s\n", __func__);
+	pr_debug("   slave.name = %s\n", slave->name);
+	pr_debug("   master.name = %s\n", slave->u.slave->master->name);
+
+	cam->sensor = slave;
+	if (slave == NULL) {
+		pr_err("ERROR: v4l2 capture: slave parameter not valid.\n");
+		return -1;
+	}
+
+	csi_enable_mclk(CSI_MCLK_I2C, true, true);
+	vidioc_int_dev_init(slave);
+	csi_enable_mclk(CSI_MCLK_I2C, false, false);
+	cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* Used to detect TV in (type 1) vs. camera (type 0) */
+	cam->device_type = cam_fmt.fmt.pix.priv;
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__, cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+
+	return 0;
+}
+
+/*!
+ * Disconnects the camera driver.
+ */
+static void csi_v4l2_master_detach(struct v4l2_int_device *slave)
+{
+	pr_debug("In MVC: %s\n", __func__);
+
+	vidioc_int_dev_exit(slave);
+}
+
+/*!
+ * Entry point for the V4L2
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int camera_init(void)
+{
+	u8 err = 0;
+
+	/* Register the device driver structure. */
+	err = platform_driver_register(&csi_v4l2_driver);
+	if (err != 0) {
+		pr_err("ERROR: v4l2 capture:camera_init: "
+		       "platform_driver_register failed.\n");
+		return err;
+	}
+
+	/* Create g_cam and initialize it. */
+	g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL);
+	if (g_cam == NULL) {
+		pr_err("ERROR: v4l2 capture: failed to register camera\n");
+		platform_driver_unregister(&csi_v4l2_driver);
+		return -1;
+	}
+	init_camera_struct(g_cam);
+
+	/* Set up the v4l2 device and register it */
+	csi_v4l2_int_device.priv = g_cam;
+	/* This function contains a bug that won't let this be rmmod'd. */
+	v4l2_int_device_register(&csi_v4l2_int_device);
+
+	/* Register the platform device */
+	err = platform_device_register(&csi_v4l2_devices);
+	if (err != 0) {
+		pr_err("ERROR: v4l2 capture: camera_init: "
+		       "platform_device_register failed.\n");
+		platform_driver_unregister(&csi_v4l2_driver);
+		kfree(g_cam);
+		g_cam = NULL;
+		return err;
+	}
+
+	/* register v4l video device */
+	if (video_register_device(g_cam->video_dev, VFL_TYPE_GRABBER, video_nr)
+	    == -1) {
+		platform_device_unregister(&csi_v4l2_devices);
+		platform_driver_unregister(&csi_v4l2_driver);
+		kfree(g_cam);
+		g_cam = NULL;
+		pr_err("ERROR: v4l2 capture: video_register_device failed\n");
+		return -1;
+	}
+	pr_debug("   Video device registered: %s #%d\n",
+		 g_cam->video_dev->name, g_cam->video_dev->minor);
+
+	return err;
+}
+
+/*!
+ * Exit and cleanup for the V4L2
+ */
+static void __exit camera_exit(void)
+{
+	pr_debug("In MVC: %s\n", __func__);
+
+	if (g_cam->open_count) {
+		pr_err("ERROR: v4l2 capture:camera open "
+		       "-- setting ops to NULL\n");
+	} else {
+		pr_info("V4L2 freeing image input device\n");
+		v4l2_int_device_unregister(&csi_v4l2_int_device);
+		csi_stop_callback(g_cam);
+		video_unregister_device(g_cam->video_dev);
+		platform_driver_unregister(&csi_v4l2_driver);
+		platform_device_unregister(&csi_v4l2_devices);
+
+		kfree(g_cam);
+		g_cam = NULL;
+	}
+}
+
+module_init(camera_init);
+module_exit(camera_exit);
+
+module_param(video_nr, int, 0444);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2 capture driver for Mx25 based cameras");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/capture/emma_mt9v111.c b/drivers/media/video/mxc/capture/emma_mt9v111.c
new file mode 100644
index 000000000000..73e9bba36d1e
--- /dev/null
+++ b/drivers/media/video/mxc/capture/emma_mt9v111.c
@@ -0,0 +1,679 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mt9v111.c
+ *
+ * @brief mt9v111 camera driver functions
+ *
+ * @ingroup Camera
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/clk.h>
+#include "mxc_v4l2_capture.h"
+#include "mt9v111.h"
+
+#ifdef MT9V111_DEBUG
+static u16 testpattern;
+#endif
+
+static sensor_interface *interface_param;
+static mt9v111_conf mt9v111_device;
+static int reset_frame_rate = 30;
+
+#define MT9V111_FRAME_RATE_NUM    20
+
+static mt9v111_image_format format[2] = {
+	{
+	 .index = 0,
+	 .width = 640,
+	 .height = 480,
+	 },
+	{
+	 .index = 1,
+	 .width = 352,
+	 .height = 288,
+	 },
+};
+
+static int mt9v111_attach(struct i2c_adapter *adapter);
+static int mt9v111_detach(struct i2c_client *client);
+
+static struct i2c_driver mt9v111_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "MT9V111 Client",
+		   },
+	.attach_adapter = mt9v111_attach,
+	.detach_client = mt9v111_detach,
+};
+
+static struct i2c_client mt9v111_i2c_client = {
+	.name = "mt9v111 I2C dev",
+	.addr = MT9V111_I2C_ADDRESS,
+	.driver = &mt9v111_i2c_driver,
+};
+
+/*
+ * Function definitions
+ */
+
+#ifdef MT9V111_DEBUG
+static inline int mt9v111_read_reg(u8 reg)
+{
+	int val = i2c_smbus_read_word_data(&mt9v111_i2c_client, reg);
+	if (val != -1)
+		val = cpu_to_be16(val);
+	return val;
+}
+#endif
+
+static inline int mt9v111_write_reg(u8 reg, u16 val)
+{
+	pr_debug("write reg %x val %x.\n", reg, val);
+	return i2c_smbus_write_word_data(&mt9v111_i2c_client, reg,
+					 cpu_to_be16(val));
+}
+
+/*!
+ * Initialize mt9v111_sensor_lib
+ * Libarary for Sensor configuration through I2C
+ *
+ * @param       coreReg       Core Registers
+ * @param       ifpReg        IFP Register
+ *
+ * @return status
+ */
+static u8 mt9v111_sensor_lib(mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
+{
+	u8 reg;
+	u16 data;
+	u8 error = 0;
+
+	/*
+	 * setup to IFP registers
+	 */
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_MODE_CONTROL;
+	data = ifpReg->modeControl;
+	mt9v111_write_reg(reg, data);
+
+	/* Output format */
+	reg = MT9V111I_FORMAT_CONTROL;
+	data = ifpReg->formatControl;	/* Set bit 12 */
+	mt9v111_write_reg(reg, data);
+
+	/* AE limit 4 */
+	reg = MT9V111I_SHUTTER_WIDTH_LIMIT_AE;
+	data = ifpReg->gainLimitAE;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_OUTPUT_FORMAT_CTRL2;
+	data = ifpReg->outputFormatCtrl2;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_AE_SPEED;
+	data = ifpReg->AESpeed;
+	mt9v111_write_reg(reg, data);
+
+	/* output image size */
+	reg = MT9V111i_H_PAN;
+	data = 0x8000 | ifpReg->HPan;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_ZOOM;
+	data = 0x8000 | ifpReg->HZoom;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_SIZE;
+	data = 0x8000 | ifpReg->HSize;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_PAN;
+	data = 0x8000 | ifpReg->VPan;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_ZOOM;
+	data = 0x8000 | ifpReg->VZoom;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_SIZE;
+	data = 0x8000 | ifpReg->VSize;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_PAN;
+	data = ~0x8000 & ifpReg->HPan;
+	mt9v111_write_reg(reg, data);
+#if 0
+	reg = MT9V111I_UPPER_SHUTTER_DELAY_LIM;
+	data = ifpReg->upperShutterDelayLi;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_SHUTTER_60;
+	data = ifpReg->shutter_width_60;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_SEARCH_FLICK_60;
+	data = ifpReg->search_flicker_60;
+	mt9v111_write_reg(reg, data);
+#endif
+
+	/*
+	 * setup to sensor core registers
+	 */
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = coreReg->addressSelect;
+	mt9v111_write_reg(reg, data);
+
+	/* enable changes and put the Sync bit on */
+	reg = MT9V111S_OUTPUT_CTRL;
+	data = MT9V111S_OUTCTRL_SYNC | MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
+	mt9v111_write_reg(reg, data);
+
+	/* min PIXCLK - Default */
+	reg = MT9V111S_PIXEL_CLOCK_SPEED;
+	data = coreReg->pixelClockSpeed;
+	mt9v111_write_reg(reg, data);
+
+	/* Setup image flipping / Dark rows / row/column skip */
+	reg = MT9V111S_READ_MODE;
+	data = coreReg->readMode;
+	mt9v111_write_reg(reg, data);
+
+	/*zoom 0 */
+	reg = MT9V111S_DIGITAL_ZOOM;
+	data = coreReg->digitalZoom;
+	mt9v111_write_reg(reg, data);
+
+	/* min H-blank */
+	reg = MT9V111S_HOR_BLANKING;
+	data = coreReg->horizontalBlanking;
+	mt9v111_write_reg(reg, data);
+
+	/* min V-blank */
+	reg = MT9V111S_VER_BLANKING;
+	data = coreReg->verticalBlanking;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111S_SHUTTER_WIDTH;
+	data = coreReg->shutterWidth;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111S_SHUTTER_DELAY;
+	data = ifpReg->upperShutterDelayLi;
+	mt9v111_write_reg(reg, data);
+
+	/* changes become effective */
+	reg = MT9V111S_OUTPUT_CTRL;
+	data = MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
+	mt9v111_write_reg(reg, data);
+
+	return error;
+}
+
+/*!
+ * mt9v111 sensor interface Initialization
+ * @param param            sensor_interface *
+ * @param width            u32
+ * @param height           u32
+ * @return  None
+ */
+static void mt9v111_interface(sensor_interface *param, u32 width, u32 height)
+{
+	param->Vsync_pol = 0x0;
+	param->clk_mode = 0x0;	/*gated */
+	param->pixclk_pol = 0x0;
+	param->data_width = 0x1;
+	param->data_pol = 0x0;
+	param->ext_vsync = 0x0;
+	param->Vsync_pol = 0x0;
+	param->Hsync_pol = 0x0;
+	param->width = width - 1;
+	param->height = height - 1;
+	param->active_width = width;
+	param->active_height = height;
+	param->pixel_fmt = IPU_PIX_FMT_UYVY;
+	param->mclk = 27000000;
+}
+
+/*!
+ * MT9V111 frame rate calculate
+ *
+ * @param frame_rate       int *
+ * @param mclk             int
+ * @return  None
+ */
+static void mt9v111_rate_cal(int *frame_rate, int mclk)
+{
+	int num_clock_per_row;
+	int max_rate = 0;
+
+	mt9v111_device.coreReg->horizontalBlanking = MT9V111_HORZBLANK_MIN;
+
+	num_clock_per_row = (format[0].width + 114 + MT9V111_HORZBLANK_MIN) * 2;
+	max_rate = mclk / (num_clock_per_row *
+			   (format[0].height + MT9V111_VERTBLANK_DEFAULT));
+
+	if ((*frame_rate > max_rate) || (*frame_rate == 0)) {
+		*frame_rate = max_rate;
+	}
+
+	mt9v111_device.coreReg->verticalBlanking
+	    = mclk / (*frame_rate * num_clock_per_row) - format[0].height;
+
+	reset_frame_rate = *frame_rate;
+}
+
+/*!
+ * MT9V111 sensor configuration
+ *
+ * @param frame_rate       int *
+ * @param high_quality     int
+ * @return  sensor_interface *
+ */
+sensor_interface *mt9v111_config(int *frame_rate, int high_quality)
+{
+	u32 out_width, out_height;
+
+	if (interface_param == NULL)
+		return NULL;
+
+	mt9v111_device.coreReg->addressSelect = MT9V111I_SEL_SCA;
+	mt9v111_device.ifpReg->addrSpaceSel = MT9V111I_SEL_IFP;
+
+	mt9v111_device.coreReg->windowHeight = MT9V111_WINHEIGHT;
+	mt9v111_device.coreReg->windowWidth = MT9V111_WINWIDTH;
+	mt9v111_device.coreReg->zoomColStart = 0;
+	mt9v111_device.coreReg->zomRowStart = 0;
+	mt9v111_device.coreReg->digitalZoom = 0x0;
+
+	mt9v111_device.coreReg->verticalBlanking = MT9V111_VERTBLANK_DEFAULT;
+	mt9v111_device.coreReg->horizontalBlanking = MT9V111_HORZBLANK_MIN;
+	mt9v111_device.coreReg->pixelClockSpeed = 0;
+	mt9v111_device.coreReg->readMode = 0xd0a1;
+
+	mt9v111_device.ifpReg->outputFormatCtrl2 = 0;
+	mt9v111_device.ifpReg->gainLimitAE = 0x300;
+	mt9v111_device.ifpReg->AESpeed = 0x80;
+
+	/* here is the default value */
+	mt9v111_device.ifpReg->formatControl = 0xc800;
+	mt9v111_device.ifpReg->modeControl = 0x708e;
+	mt9v111_device.ifpReg->awbSpeed = 0x4514;
+	mt9v111_device.coreReg->shutterWidth = 0xf8;
+
+	out_width = 640;
+	out_height = 480;
+
+	/*output size */
+	mt9v111_device.ifpReg->HPan = 0;
+	mt9v111_device.ifpReg->HZoom = 640;
+	mt9v111_device.ifpReg->HSize = out_width;
+	mt9v111_device.ifpReg->VPan = 0;
+	mt9v111_device.ifpReg->VZoom = 480;
+	mt9v111_device.ifpReg->VSize = out_height;
+
+	mt9v111_interface(interface_param, out_width, out_height);
+	set_mclk_rate(&interface_param->mclk);
+	mt9v111_rate_cal(frame_rate, interface_param->mclk);
+	mt9v111_sensor_lib(mt9v111_device.coreReg, mt9v111_device.ifpReg);
+
+	return interface_param;
+}
+
+/*!
+ * mt9v111 sensor set color configuration
+ *
+ * @param bright       int
+ * @param saturation   int
+ * @param red          int
+ * @param green        int
+ * @param blue         int
+ * @return  None
+ */
+static void
+mt9v111_set_color(int bright, int saturation, int red, int green, int blue)
+{
+	u8 reg;
+	u16 data;
+
+	switch (saturation) {
+	case 100:
+		mt9v111_device.ifpReg->awbSpeed = 0x4514;
+		break;
+	case 150:
+		mt9v111_device.ifpReg->awbSpeed = 0x6D14;
+		break;
+	case 75:
+		mt9v111_device.ifpReg->awbSpeed = 0x4D14;
+		break;
+	case 50:
+		mt9v111_device.ifpReg->awbSpeed = 0x5514;
+		break;
+	case 37:
+		mt9v111_device.ifpReg->awbSpeed = 0x5D14;
+		break;
+	case 25:
+		mt9v111_device.ifpReg->awbSpeed = 0x6514;
+		break;
+	default:
+		mt9v111_device.ifpReg->awbSpeed = 0x4514;
+		break;
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_AWB_SPEED;
+	data = mt9v111_device.ifpReg->awbSpeed;
+	mt9v111_write_reg(reg, data);
+}
+
+/*!
+ * mt9v111 sensor get color configuration
+ *
+ * @param bright       int *
+ * @param saturation   int *
+ * @param red          int *
+ * @param green        int *
+ * @param blue         int *
+ * @return  None
+ */
+static void
+mt9v111_get_color(int *bright, int *saturation, int *red, int *green, int *blue)
+{
+	*saturation = (mt9v111_device.ifpReg->awbSpeed & 0x3800) >> 11;
+	switch (*saturation) {
+	case 0:
+		*saturation = 100;
+		break;
+	case 1:
+		*saturation = 75;
+		break;
+	case 2:
+		*saturation = 50;
+		break;
+	case 3:
+		*saturation = 37;
+		break;
+	case 4:
+		*saturation = 25;
+		break;
+	case 5:
+		*saturation = 150;
+		break;
+	case 6:
+		*saturation = 0;
+		break;
+	default:
+		*saturation = 0;
+		break;
+	}
+}
+
+/*!
+ * mt9v111 sensor set AE measurement window mode configuration
+ *
+ * @param ae_mode      int
+ * @return  None
+ */
+static void mt9v111_set_ae_mode(int ae_mode)
+{
+	u8 reg;
+	u16 data;
+
+	mt9v111_device.ifpReg->modeControl &= 0xfff3;
+	mt9v111_device.ifpReg->modeControl |= (ae_mode & 0x03) << 2;
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_MODE_CONTROL;
+	data = mt9v111_device.ifpReg->modeControl;
+	mt9v111_write_reg(reg, data);
+}
+
+/*!
+ * mt9v111 sensor get AE measurement window mode configuration
+ *
+ * @param ae_mode      int *
+ * @return  None
+ */
+static void mt9v111_get_ae_mode(int *ae_mode)
+{
+	if (ae_mode != NULL) {
+		*ae_mode = (mt9v111_device.ifpReg->modeControl & 0xc) >> 2;
+	}
+}
+
+/*!
+ * mt9v111 Reset function
+ *
+ * @return  None
+ */
+static sensor_interface *mt9v111_reset(void)
+{
+	return mt9v111_config(&reset_frame_rate, 0);
+}
+
+struct camera_sensor camera_sensor_if = {
+	.set_color = mt9v111_set_color,
+	.get_color = mt9v111_get_color,
+	.set_ae_mode = mt9v111_set_ae_mode,
+	.get_ae_mode = mt9v111_get_ae_mode,
+	.config = mt9v111_config,
+	.reset = mt9v111_reset,
+};
+
+#ifdef MT9V111_DEBUG
+/*!
+ * Set sensor to test mode, which will generate test pattern.
+ *
+ * @return none
+ */
+static void mt9v111_test_pattern(bool flag)
+{
+	u16 data;
+
+	/* switch to sensor registers */
+	mt9v111_write_reg(MT9V111I_ADDR_SPACE_SEL, MT9V111I_SEL_SCA);
+
+	if (flag == true) {
+		testpattern = MT9V111S_OUTCTRL_TEST_MODE;
+
+		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) & 0xBF;
+		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+
+		mt9v111_write_reg(MT9V111S_TEST_DATA, 0);
+
+		/* changes take effect */
+		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
+		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+	} else {
+		testpattern = 0;
+
+		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) | 0x40;
+		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+
+		/* changes take effect */
+		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
+		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+	}
+}
+#endif
+
+/*!
+ * mt9v111 I2C detect_client function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @param address            int
+ * @param kind               int
+ *
+ * @return  Error code indicating success or failure
+ */
+static int mt9v111_detect_client(struct i2c_adapter *adapter, int address,
+				 int kind)
+{
+	mt9v111_i2c_client.adapter = adapter;
+	if (i2c_attach_client(&mt9v111_i2c_client)) {
+		mt9v111_i2c_client.adapter = NULL;
+		printk(KERN_ERR "mt9v111_attach: i2c_attach_client failed\n");
+		return -1;
+	}
+
+	interface_param = (sensor_interface *)
+	    kmalloc(sizeof(sensor_interface), GFP_KERNEL);
+	if (!interface_param) {
+		printk(KERN_ERR "mt9v111_attach: kmalloc failed \n");
+		return -1;
+	}
+
+	printk(KERN_INFO "MT9V111 Detected\n");
+
+	return 0;
+}
+
+static unsigned short normal_i2c[] = { MT9V111_I2C_ADDRESS, I2C_CLIENT_END };
+
+/* Magic definition of all other variables and things */
+I2C_CLIENT_INSMOD;
+
+/*!
+ * mt9v111 I2C attach function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  Error code indicating success or failure
+ */
+static int mt9v111_attach(struct i2c_adapter *adap)
+{
+	uint32_t mclk = 27000000;
+	struct clk *clk;
+	int err;
+
+	clk = clk_get(NULL, "csi_clk");
+	clk_enable(clk);
+	set_mclk_rate(&mclk);
+
+	err = i2c_probe(adap, &addr_data, &mt9v111_detect_client);
+
+	clk_disable(clk);
+	clk_put(clk);
+
+	return err;
+}
+
+/*!
+ * mt9v111 I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int mt9v111_detach(struct i2c_client *client)
+{
+	int err;
+
+	if (!mt9v111_i2c_client.adapter)
+		return -1;
+
+	err = i2c_detach_client(&mt9v111_i2c_client);
+	mt9v111_i2c_client.adapter = NULL;
+
+	if (interface_param)
+		kfree(interface_param);
+	interface_param = NULL;
+
+	return err;
+}
+
+extern void gpio_sensor_active(void);
+
+/*!
+ * MT9V111 init function
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int mt9v111_init(void)
+{
+	u8 err;
+
+	gpio_sensor_active();
+
+	mt9v111_device.coreReg = (mt9v111_coreReg *)
+	    kmalloc(sizeof(mt9v111_coreReg), GFP_KERNEL);
+	if (!mt9v111_device.coreReg)
+		return -1;
+
+	memset(mt9v111_device.coreReg, 0, sizeof(mt9v111_coreReg));
+
+	mt9v111_device.ifpReg = (mt9v111_IFPReg *)
+	    kmalloc(sizeof(mt9v111_IFPReg), GFP_KERNEL);
+	if (!mt9v111_device.ifpReg) {
+		kfree(mt9v111_device.coreReg);
+		mt9v111_device.coreReg = NULL;
+		return -1;
+	}
+
+	memset(mt9v111_device.ifpReg, 0, sizeof(mt9v111_IFPReg));
+
+	err = i2c_add_driver(&mt9v111_i2c_driver);
+
+	return err;
+}
+
+extern void gpio_sensor_inactive(void);
+/*!
+ * MT9V111 cleanup function
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit mt9v111_clean(void)
+{
+	if (mt9v111_device.coreReg) {
+		kfree(mt9v111_device.coreReg);
+		mt9v111_device.coreReg = NULL;
+	}
+
+	if (mt9v111_device.ifpReg) {
+		kfree(mt9v111_device.ifpReg);
+		mt9v111_device.ifpReg = NULL;
+	}
+
+	i2c_del_driver(&mt9v111_i2c_driver);
+
+	gpio_sensor_inactive();
+}
+
+module_init(mt9v111_init);
+module_exit(mt9v111_clean);
+
+/* Exported symbols for modules. */
+EXPORT_SYMBOL(camera_sensor_if);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Mt9v111 Camera Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/emma_ov2640.c b/drivers/media/video/mxc/capture/emma_ov2640.c
new file mode 100644
index 000000000000..ceffea4d52a9
--- /dev/null
+++ b/drivers/media/video/mxc/capture/emma_ov2640.c
@@ -0,0 +1,444 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/regulator/consumer.h>
+
+#include "mxc_v4l2_capture.h"
+
+enum ov2640_mode {
+	ov2640_mode_1600_1120,
+	ov2640_mode_800_600
+};
+
+struct reg_value {
+	u8 reg;
+	u8 value;
+	int delay_ms;
+};
+
+static struct reg_value ov2640_setting_1600_1120[] = {
+	{0xff, 0x1, 0}, {0x12, 0x80, 1}, {0xff, 0, 0}, {0x2c, 0xff, 0},
+	{0x2e, 0xdf, 0}, {0xff, 0x1, 0}, {0x3c, 0x32, 0}, {0x11, 0x01, 0},
+	{0x09, 0x00, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0}, {0x14, 0x48, 0},
+	{0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0}, {0x3b, 0xfb, 0},
+	{0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0}, {0x39, 0x82, 0},
+	{0x35, 0x88, 0}, {0x22, 0x0a, 0}, {0x37, 0x40, 0}, {0x23, 0x00, 0},
+	{0x34, 0xa0, 0}, {0x36, 0x1a, 0}, {0x06, 0x02, 0}, {0x07, 0xc0, 0},
+	{0x0d, 0xb7, 0}, {0x0e, 0x01, 0}, {0x4c, 0x00, 0}, {0x4a, 0x81, 0},
+	{0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0}, {0x26, 0x82, 0},
+	{0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x3f, 0}, {0x0c, 0x3c, 0},
+	{0x5d, 0x55, 0}, {0x5e, 0x7d, 0}, {0x5f, 0x7d, 0}, {0x60, 0x55, 0},
+	{0x61, 0x70, 0}, {0x62, 0x80, 0}, {0x7c, 0x05, 0}, {0x20, 0x80, 0},
+	{0x28, 0x30, 0}, {0x6c, 0x00, 0}, {0x6d, 0x80, 0}, {0x6e, 00, 0},
+	{0x70, 0x02, 0}, {0x71, 0x94, 0}, {0x73, 0xc1, 0}, {0x3d, 0x34, 0},
+	{0x5a, 0x57, 0}, {0x4f, 0xbb, 0}, {0x50, 0x9c, 0}, {0xff, 0x00, 0},
+	{0xe5, 0x7f, 0}, {0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0x44, 0x06, 0},
+	{0xe0, 0x14, 0}, {0x76, 0xff, 0}, {0x33, 0xa0, 0}, {0x42, 0x20, 0},
+	{0x43, 0x18, 0}, {0x4c, 0x00, 0}, {0x87, 0xd0, 0}, {0xd7, 0x03, 0},
+	{0xd9, 0x10, 0}, {0xd3, 0x82, 0}, {0xc8, 0x08, 0}, {0xc9, 0x80, 0},
+	{0x7c, 0x00, 0}, {0x7d, 0x00, 0}, {0x7c, 0x03, 0}, {0x7d, 0x48, 0},
+	{0x7d, 0x48, 0}, {0x7c, 0x08, 0}, {0x7d, 0x20, 0}, {0x7d, 0x10, 0},
+	{0x7d, 0x0e, 0}, {0x90, 0x00, 0}, {0x91, 0x0e, 0}, {0x91, 0x1a, 0},
+	{0x91, 0x31, 0}, {0x91, 0x5a, 0}, {0x91, 0x69, 0}, {0x91, 0x75, 0},
+	{0x91, 0x7e, 0}, {0x91, 0x88, 0}, {0x91, 0x8f, 0}, {0x91, 0x96, 0},
+	{0x91, 0xa3, 0}, {0x91, 0xaf, 0}, {0x91, 0xc4, 0}, {0x91, 0xd7, 0},
+	{0x91, 0xe8, 0}, {0x91, 0x20, 0}, {0x92, 0x00, 0}, {0x93, 0x06, 0},
+	{0x93, 0xe3, 0}, {0x93, 0x03, 0}, {0x93, 0x03, 0}, {0x93, 0x00, 0},
+	{0x93, 0x02, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x96, 0x00, 0}, {0x97, 0x08, 0}, {0x97, 0x19, 0}, {0x97, 0x02, 0},
+	{0x97, 0x0c, 0}, {0x97, 0x24, 0}, {0x97, 0x30, 0}, {0x97, 0x28, 0},
+	{0x97, 0x26, 0}, {0x97, 0x02, 0}, {0x97, 0x98, 0}, {0x97, 0x80, 0},
+	{0x97, 0x00, 0}, {0x97, 0x00, 0}, {0xa4, 0x00, 0}, {0xa8, 0x00, 0},
+	{0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0}, {0xc7, 0x10, 0},
+	{0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0}, {0xb9, 0x7c, 0},
+	{0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0}, {0xb0, 0xc5, 0},
+	{0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0}, {0xa6, 0x00, 0},
+	{0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x1b, 0}, {0xa7, 0x31, 0},
+	{0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0},
+	{0xa7, 0x19, 0}, {0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0},
+	{0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x19, 0}, {0xa7, 0x31, 0},
+	{0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xc0, 0xc8, 0}, {0xc1, 0x96, 0},
+	{0x86, 0x3d, 0}, {0x50, 0x00, 0}, {0x51, 0x90, 0}, {0x52, 0x18, 0},
+	{0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x88, 0}, {0x57, 0x00, 0},
+	{0x5a, 0x90, 0}, {0x5b, 0x18, 0}, {0x5c, 0x05, 0}, {0xc3, 0xef, 0},
+	{0x7f, 0x00, 0}, {0xda, 0x01, 0}, {0xe5, 0x1f, 0}, {0xe1, 0x67, 0},
+	{0xe0, 0x00, 0}, {0xdd, 0x7f, 0}, {0x05, 0x00, 0}
+};
+
+static struct reg_value ov2640_setting_800_600[] = {
+	{0xff, 0, 0}, {0xff, 1, 0}, {0x12, 0x80, 1}, {0xff, 00, 0},
+	{0x2c, 0xff, 0}, {0x2e, 0xdf, 0}, {0xff, 0x1, 0}, {0x3c, 0x32, 0},
+	{0x11, 0x01, 0}, {0x09, 0x00, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0},
+	{0x14, 0x48, 0}, {0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0},
+	{0x3b, 0xfb, 0}, {0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0},
+	{0x39, 0x92, 0}, {0x35, 0xda, 0}, {0x22, 0x1a, 0}, {0x37, 0xc3, 0},
+	{0x23, 0x00, 0}, {0x34, 0xc0, 0}, {0x36, 0x1a, 0}, {0x06, 0x88, 0},
+	{0x07, 0xc0, 0}, {0x0d, 0x87, 0}, {0x0e, 0x41, 0}, {0x4c, 0x00, 0},
+	{0x4a, 0x81, 0}, {0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0},
+	{0x26, 0x82, 0}, {0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x22, 0},
+	{0x0c, 0x3c, 0}, {0x5d, 0x55, 0}, {0x5e, 0x7d, 0}, {0x5f, 0x7d, 0},
+	{0x60, 0x55, 0}, {0x61, 0x70, 0}, {0x62, 0x80, 0}, {0x7c, 0x05, 0},
+	{0x20, 0x80, 0}, {0x28, 0x30, 0}, {0x6c, 0x00, 0}, {0x6d, 0x80, 0},
+	{0x6e, 00, 0}, {0x70, 0x02, 0}, {0x71, 0x94, 0}, {0x73, 0xc1, 0},
+	{0x12, 0x40, 0}, {0x17, 0x11, 0}, {0x18, 0x43, 0}, {0x19, 0x00, 0},
+	{0x1a, 0x4b, 0}, {0x32, 0x09, 0}, {0x37, 0xc0, 0}, {0x4f, 0xca, 0},
+	{0x50, 0xa8, 0}, {0x6d, 0x00, 0}, {0x3d, 0x38, 0}, {0xff, 0x00, 0},
+	{0xe5, 0x7f, 0}, {0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0x44, 0x06, 0},
+	{0xe0, 0x14, 0}, {0x76, 0xff, 0}, {0x33, 0xa0, 0}, {0x42, 0x20, 0},
+	{0x43, 0x18, 0}, {0x4c, 0x00, 0}, {0x87, 0xd0, 0}, {0x88, 0x3f, 0},
+	{0xd7, 0x03, 0}, {0xd9, 0x10, 0}, {0xd3, 0x82, 0}, {0xc8, 0x08, 0},
+	{0xc9, 0x80, 0}, {0x7c, 0x00, 0}, {0x7d, 0x00, 0}, {0x7c, 0x03, 0},
+	{0x7d, 0x48, 0}, {0x7d, 0x48, 0}, {0x7c, 0x08, 0}, {0x7d, 0x20, 0},
+	{0x7d, 0x10, 0}, {0x7d, 0x0e, 0}, {0x90, 0x00, 0}, {0x91, 0x0e, 0},
+	{0x91, 0x1a, 0}, {0x91, 0x31, 0}, {0x91, 0x5a, 0}, {0x91, 0x69, 0},
+	{0x91, 0x75, 0}, {0x91, 0x7e, 0}, {0x91, 0x88, 0}, {0x91, 0x8f, 0},
+	{0x91, 0x96, 0}, {0x91, 0xa3, 0}, {0x91, 0xaf, 0}, {0x91, 0xc4, 0},
+	{0x91, 0xd7, 0}, {0x91, 0xe8, 0}, {0x91, 0x20, 0}, {0x92, 0x00, 0},
+	{0x93, 0x06, 0}, {0x93, 0xe3, 0}, {0x93, 0x03, 0}, {0x93, 0x03, 0},
+	{0x93, 0x00, 0}, {0x93, 0x02, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x96, 0x00, 0}, {0x97, 0x08, 0}, {0x97, 0x19, 0},
+	{0x97, 0x02, 0}, {0x97, 0x0c, 0}, {0x97, 0x24, 0}, {0x97, 0x30, 0},
+	{0x97, 0x28, 0}, {0x97, 0x26, 0}, {0x97, 0x02, 0}, {0x97, 0x98, 0},
+	{0x97, 0x80, 0}, {0x97, 0x00, 0}, {0x97, 0x00, 0}, {0xa4, 0x00, 0},
+	{0xa8, 0x00, 0}, {0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0},
+	{0xc7, 0x10, 0}, {0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0},
+	{0xb9, 0x7c, 0}, {0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0},
+	{0xb0, 0xc5, 0}, {0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0},
+	{0xa6, 0x00, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x1b, 0},
+	{0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xa7, 0x20, 0},
+	{0xa7, 0xd8, 0}, {0xa7, 0x19, 0}, {0xa7, 0x31, 0}, {0xa7, 0x00, 0},
+	{0xa7, 0x18, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x19, 0},
+	{0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xc0, 0x64, 0},
+	{0xc1, 0x4b, 0}, {0x86, 0x1d, 0}, {0x50, 0x00, 0}, {0x51, 0xc8, 0},
+	{0x52, 0x96, 0}, {0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x00, 0},
+	{0x57, 0x00, 0}, {0x5a, 0xc8, 0}, {0x5b, 0x96, 0}, {0x5c, 0x00, 0},
+	{0xc3, 0xef, 0}, {0x7f, 0x00, 0}, {0xda, 0x01, 0}, {0xe5, 0x1f, 0},
+	{0xe1, 0x67, 0}, {0xe0, 0x00, 0}, {0xdd, 0x7f, 0}, {0x05, 0x00, 0}
+};
+
+static struct regulator *io_regulator;
+static struct regulator *core_regulator;
+static struct regulator *analog_regulator;
+static struct regulator *gpo_regulator;
+u32 mclk = 24000000;
+
+struct i2c_client *ov2640_i2c_client;
+
+static sensor_interface *interface_param;
+static int reset_frame_rate = 30;
+static int ov2640_probe(struct i2c_client *adapter,
+			const struct i2c_device_id *id);
+static int ov2640_remove(struct i2c_client *client);
+
+static const struct i2c_device_id ov2640_id[] = {
+	{"ov2640", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ov2640_id);
+
+static struct i2c_driver ov2640_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "ov2640",
+		   },
+	.probe = ov2640_probe,
+	.remove = ov2640_remove,
+	.id_table = ov2640_id,
+};
+
+/*!
+ * ov2640 I2C attach function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  Error code indicating success or failure
+ */
+static int ov2640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct mxc_camera_platform_data *plat_data = client->dev.platform_data;
+
+	ov2640_i2c_client = client;
+	mclk = plat_data->mclk;
+
+	io_regulator = regulator_get(&client->dev, plat_data->io_regulator);
+	core_regulator = regulator_get(&client->dev, plat_data->core_regulator);
+	analog_regulator =
+	    regulator_get(&client->dev, plat_data->analog_regulator);
+	gpo_regulator = regulator_get(&client->dev, plat_data->gpo_regulator);
+
+	interface_param = (sensor_interface *)
+	    kmalloc(sizeof(sensor_interface), GFP_KERNEL);
+	if (!interface_param) {
+		dev_dbg(&ov2640_i2c_client->dev,
+			"ov2640_probe: kmalloc failed \n");
+		return -1;
+	}
+
+	return 0;
+}
+
+/*!
+ * ov2640 I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int ov2640_remove(struct i2c_client *client)
+{
+	kfree(interface_param);
+	interface_param = NULL;
+
+	if (!IS_ERR_VALUE((unsigned long)io_regulator)) {
+		regulator_disable(io_regulator);
+		regulator_put(io_regulator);
+	}
+
+	if (!IS_ERR_VALUE((unsigned long)core_regulator)) {
+		regulator_disable(core_regulator);
+		regulator_put(core_regulator);
+	}
+
+	if (!IS_ERR_VALUE((unsigned long)gpo_regulator)) {
+		regulator_disable(gpo_regulator);
+		regulator_put(gpo_regulator);
+	}
+
+	if (!IS_ERR_VALUE((unsigned long)analog_regulator)) {
+		regulator_disable(analog_regulator);
+		regulator_put(analog_regulator);
+	}
+
+	return 0;
+}
+
+static int ov2640_write_reg(u8 reg, u8 val)
+{
+	if (i2c_smbus_write_byte_data(ov2640_i2c_client, reg, val) < 0) {
+		dev_dbg(&ov2640_i2c_client->dev,
+			"%s:write reg errorr:reg=%x,val=%x\n", __func__, reg,
+			val);
+		return -1;
+	}
+	return 0;
+}
+
+static int ov2640_init_mode(enum ov2640_mode mode)
+{
+	struct reg_value *setting;
+	int i, num;
+
+	switch (mode) {
+	case ov2640_mode_1600_1120:
+		setting = ov2640_setting_1600_1120;
+		num = ARRAY_SIZE(ov2640_setting_1600_1120);
+		break;
+	case ov2640_mode_800_600:
+		setting = ov2640_setting_800_600;
+		num = ARRAY_SIZE(ov2640_setting_800_600);
+		break;
+	default:
+		return 0;
+	}
+
+	for (i = 0; i < num; i++) {
+		ov2640_write_reg(setting[i].reg, setting[i].value);
+		if (setting[i].delay_ms > 0)
+			msleep(setting[i].delay_ms);
+	}
+
+	return 0;
+}
+
+/*!
+ * ov2640 sensor interface Initialization
+ * @param param            sensor_interface *
+ * @param width            u32
+ * @param height           u32
+ * @return  None
+ */
+static void ov2640_interface(sensor_interface *param, u32 width, u32 height)
+{
+	param->Vsync_pol = 0x0;
+	param->clk_mode = 0x0;	/*gated */
+	param->pixclk_pol = 0x0;
+	param->data_width = 0x1;
+	param->data_pol = 0x0;
+	param->ext_vsync = 0x0;
+	param->Vsync_pol = 0x0;
+	param->Hsync_pol = 0x0;
+	param->width = width - 1;
+	param->height = height - 1;
+	param->active_width = width;
+	param->active_height = height;
+	param->pixel_fmt = IPU_PIX_FMT_UYVY;
+	param->mclk = mclk;
+}
+
+static void ov2640_set_color(int bright, int saturation, int red, int green,
+			     int blue)
+{
+
+}
+
+static void ov2640_get_color(int *bright, int *saturation, int *red, int *green,
+			     int *blue)
+{
+
+}
+static void ov2640_set_ae_mode(int ae_mode)
+{
+
+}
+static void ov2640_get_ae_mode(int *ae_mode)
+{
+
+}
+
+extern void gpio_sensor_active(void);
+
+static sensor_interface *ov2640_config(int *frame_rate, int high_quality)
+{
+
+	u32 out_width, out_height;
+
+	/*set io votage */
+	if (!IS_ERR_VALUE((unsigned long)io_regulator)) {
+		regulator_set_voltage(io_regulator, 2800000, 2800000);
+		if (regulator_enable(io_regulator) != 0) {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:io set voltage error\n", __func__);
+			return NULL;
+		} else {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:io set voltage ok\n", __func__);
+		}
+	}
+
+	/*core votage */
+	if (!IS_ERR_VALUE((unsigned long)core_regulator)) {
+		regulator_set_voltage(core_regulator, 1300000, 1300000);
+		if (regulator_enable(core_regulator) != 0) {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:core set voltage error\n", __func__);
+			return NULL;
+		} else {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:core set voltage ok\n", __func__);
+		}
+	}
+
+	/*GPO 3 */
+	if (!IS_ERR_VALUE((unsigned long)gpo_regulator)) {
+		if (regulator_enable(gpo_regulator) != 0) {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:gpo3 enable error\n", __func__);
+			return NULL;
+		} else {
+			dev_dbg(&ov2640_i2c_client->dev, "%s:gpo3 enable ok\n",
+				__func__);
+		}
+	}
+
+	if (!IS_ERR_VALUE((unsigned long)analog_regulator)) {
+		regulator_set_voltage(analog_regulator, 2000000, 2000000);
+		if (regulator_enable(analog_regulator) != 0) {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:analog set voltage error\n", __func__);
+			return NULL;
+		} else {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:analog set voltage ok\n", __func__);
+		}
+	}
+
+	gpio_sensor_active();
+
+	if (high_quality) {
+		out_width = 1600;
+		out_height = 1120;
+	} else {
+		out_width = 800;
+		out_height = 600;
+	}
+	ov2640_interface(interface_param, out_width, out_height);
+	set_mclk_rate(&interface_param->mclk);
+
+	if (high_quality)
+		ov2640_init_mode(ov2640_mode_1600_1120);
+	else
+		ov2640_init_mode(ov2640_mode_800_600);
+
+	msleep(300);
+
+	return interface_param;
+}
+
+static sensor_interface *ov2640_reset(void)
+{
+	return ov2640_config(&reset_frame_rate, 0);
+}
+
+struct camera_sensor camera_sensor_if = {
+	.set_color = ov2640_set_color,
+	.get_color = ov2640_get_color,
+	.set_ae_mode = ov2640_set_ae_mode,
+	.get_ae_mode = ov2640_get_ae_mode,
+	.config = ov2640_config,
+	.reset = ov2640_reset,
+};
+
+EXPORT_SYMBOL(camera_sensor_if);
+
+/*!
+ * ov2640 init function
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int ov2640_init(void)
+{
+	u8 err;
+
+	err = i2c_add_driver(&ov2640_i2c_driver);
+
+	return err;
+}
+
+extern void gpio_sensor_inactive(void);
+/*!
+ * OV2640 cleanup function
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit ov2640_clean(void)
+{
+	i2c_del_driver(&ov2640_i2c_driver);
+
+	gpio_sensor_inactive();
+}
+
+module_init(ov2640_init);
+module_exit(ov2640_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("OV2640 Camera Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/emma_v4l2_capture.c b/drivers/media/video/mxc/capture/emma_v4l2_capture.c
new file mode 100644
index 000000000000..9cb08b26f1cd
--- /dev/null
+++ b/drivers/media/video/mxc/capture/emma_v4l2_capture.c
@@ -0,0 +1,2074 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_v4l2_capture.c
+ *
+ * @brief MX27 Video For Linux 2 driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/pagemap.h>
+#include <linux/vmalloc.h>
+#include <linux/types.h>
+#include <linux/fb.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/semaphore.h>
+#include <linux/version.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+
+#include "mxc_v4l2_capture.h"
+#include "mx27_prp.h"
+#include "mx27_csi.h"
+
+static int csi_mclk_flag_backup;
+static int video_nr = -1;
+static cam_data *g_cam;
+
+/*!
+ * Free frame buffers
+ *
+ * @param cam      Structure cam_data *
+ *
+ * @return status  0 success.
+ */
+static int mxc_free_frame_buf(cam_data *cam)
+{
+	int i;
+
+	for (i = 0; i < FRAME_NUM; i++) {
+		if (cam->frame[i].vaddress != 0) {
+			dma_free_coherent(0,
+					  cam->frame[i].buffer.length,
+					  cam->frame[i].vaddress,
+					  cam->frame[i].paddress);
+			cam->frame[i].vaddress = 0;
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * Allocate frame buffers
+ *
+ * @param cam      Structure cam_data *
+ *
+ * @param count    int number of buffer need to allocated
+ *
+ * @return status  -0 Successfully allocated a buffer, -ENOBUFS	failed.
+ */
+static int mxc_allocate_frame_buf(cam_data *cam, int count)
+{
+	int i;
+
+	for (i = 0; i < count; i++) {
+		cam->frame[i].vaddress =
+			dma_alloc_coherent(0,
+					PAGE_ALIGN(cam->v2f. fmt.pix.sizeimage),
+					   &cam->frame[i].paddress,
+					   GFP_DMA | GFP_KERNEL);
+		if (cam->frame[i].vaddress == 0) {
+			pr_debug("mxc_allocate_frame_buf failed.\n");
+			mxc_free_frame_buf(cam);
+			return -ENOBUFS;
+		}
+		cam->frame[i].buffer.index = i;
+		cam->frame[i].buffer.flags = V4L2_BUF_FLAG_MAPPED;
+		cam->frame[i].buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cam->frame[i].buffer.length =
+		    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
+		cam->frame[i].buffer.memory = V4L2_MEMORY_MMAP;
+		cam->frame[i].buffer.m.offset = cam->frame[i].paddress;
+		cam->frame[i].index = i;
+	}
+
+	return 0;
+}
+
+/*!
+ * Free frame buffers status
+ *
+ * @param cam    Structure cam_data *
+ *
+ * @return none
+ */
+static void mxc_free_frames(cam_data *cam)
+{
+	int i;
+
+	for (i = 0; i < FRAME_NUM; i++) {
+		cam->frame[i].buffer.flags = V4L2_BUF_FLAG_MAPPED;
+	}
+
+	cam->enc_counter = 0;
+	cam->skip_frame = 0;
+	INIT_LIST_HEAD(&cam->ready_q);
+	INIT_LIST_HEAD(&cam->working_q);
+	INIT_LIST_HEAD(&cam->done_q);
+}
+
+/*!
+ * Return the buffer status
+ *
+ * @param cam 	   Structure cam_data *
+ * @param buf      Structure v4l2_buffer *
+ *
+ * @return status  0 success, EINVAL failed.
+ */
+static int mxc_v4l2_buffer_status(cam_data *cam, struct v4l2_buffer *buf)
+{
+	/* check range */
+	if (buf->index < 0 || buf->index >= FRAME_NUM) {
+		pr_debug("mxc_v4l2_buffer_status buffers not allocated\n");
+		return -EINVAL;
+	}
+
+	memcpy(buf, &(cam->frame[buf->index].buffer), sizeof(*buf));
+	return 0;
+}
+
+/*!
+ * start the encoder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_streamon(cam_data *cam)
+{
+	struct mxc_v4l_frame *frame;
+	int err = 0;
+
+	if (!cam)
+		return -EIO;
+
+	if (list_empty(&cam->ready_q)) {
+		printk(KERN_ERR "mxc_streamon buffer not been queued yet\n");
+		return -EINVAL;
+	}
+
+	cam->capture_pid = current->pid;
+
+	if (cam->enc_enable) {
+		err = cam->enc_enable(cam);
+		if (err != 0) {
+			return err;
+		}
+	}
+
+	cam->ping_pong_csi = 0;
+	if (cam->enc_update_eba) {
+		frame =
+		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
+		list_del(cam->ready_q.next);
+		list_add_tail(&frame->queue, &cam->working_q);
+		err = cam->enc_update_eba(frame->paddress, &cam->ping_pong_csi);
+
+		frame =
+		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
+		list_del(cam->ready_q.next);
+		list_add_tail(&frame->queue, &cam->working_q);
+		err |=
+		    cam->enc_update_eba(frame->paddress, &cam->ping_pong_csi);
+	} else {
+		return -EINVAL;
+	}
+
+	return err;
+}
+
+/*!
+ * Shut down the encoder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_streamoff(cam_data *cam)
+{
+	int err = 0;
+
+	if (!cam)
+		return -EIO;
+
+	if (cam->enc_disable) {
+		err = cam->enc_disable(cam);
+	}
+	mxc_free_frames(cam);
+	return err;
+}
+
+/*!
+ * Valid whether the palette is supported
+ *
+ * @param palette pixel format
+ *
+ * @return 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	/*
+	 * MX27 PrP channel 2 supports YUV444, but YUV444 is not
+	 * defined by V4L2 :(
+	 */
+	return ((palette == V4L2_PIX_FMT_YUYV) ||
+		(palette == V4L2_PIX_FMT_YUV420));
+}
+
+/*!
+ * Valid and adjust the overlay window size, position
+ *
+ * @param cam      structure cam_data *
+ * @param win      struct v4l2_window  *
+ *
+ * @return 0
+ */
+static int verify_preview(cam_data *cam, struct v4l2_window *win)
+{
+	if (cam->output >= num_registered_fb) {
+		pr_debug("verify_preview No matched.\n");
+		return -1;
+	}
+	cam->overlay_fb = (struct fb_info *)registered_fb[cam->output];
+
+	/* TODO: suppose 16bpp, 4 bytes alignment */
+	win->w.left &= ~0x1;
+
+	if (win->w.width + win->w.left > cam->overlay_fb->var.xres)
+		win->w.width = cam->overlay_fb->var.xres - win->w.left;
+	if (win->w.height + win->w.top > cam->overlay_fb->var.yres)
+		win->w.height = cam->overlay_fb->var.yres - win->w.top;
+
+	/*
+	 * TODO: suppose 16bpp. Rounded down to a multiple of 2 pixels for
+	 * width according to PrP limitations.
+	 */
+	if ((cam->rotation == V4L2_MXC_ROTATE_90_RIGHT)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_VFLIP)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_HFLIP)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_LEFT))
+		win->w.height &= ~0x1;
+	else
+		win->w.width &= ~0x1;
+
+	return 0;
+}
+
+/*!
+ * start the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int start_preview(cam_data *cam)
+{
+	int err = 0;
+
+	err = prp_vf_select(cam);
+	if (err != 0)
+		return err;
+
+	cam->overlay_pid = current->pid;
+	err = cam->vf_start_sdc(cam);
+
+	return err;
+}
+
+/*!
+ * shut down the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int stop_preview(cam_data *cam)
+{
+	int err = 0;
+
+	err = prp_vf_deselect(cam);
+	return err;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_g_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_g_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		f->fmt.pix.width = cam->v2f.fmt.pix.width;
+		f->fmt.pix.height = cam->v2f.fmt.pix.height;
+		f->fmt.pix.sizeimage = cam->v2f.fmt.pix.sizeimage;
+		f->fmt.pix.pixelformat = cam->v2f.fmt.pix.pixelformat;
+		f->fmt.pix.bytesperline = cam->v2f.fmt.pix.bytesperline;
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+		retval = 0;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		f->fmt.win = cam->win;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_s_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+	int size = 0;
+	int bytesperline = 0;
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		if (!valid_mode(f->fmt.pix.pixelformat)) {
+			pr_debug("mxc_v4l2_s_fmt: format not supported\n");
+			retval = -EINVAL;
+		}
+
+		if (cam->rotation != V4L2_MXC_ROTATE_NONE)
+			pr_debug("mxc_v4l2_s_fmt: capture rotation ignored\n");
+
+		switch (f->fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_YUYV:
+			f->fmt.pix.width &= ~0x1;	/* Multiple of 2 */
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			bytesperline = f->fmt.pix.width * 2;
+			break;
+		case V4L2_PIX_FMT_YUV420:
+			f->fmt.pix.width &= ~0x7;	/* Multiple of 8 */
+			f->fmt.pix.height &= ~0x1;	/* Multiple of 2 */
+			size = f->fmt.pix.width * f->fmt.pix.height * 3 / 2;
+			bytesperline = f->fmt.pix.width * 3 / 2;
+			break;
+		default:
+			/* Suppose it's YUV444 or 32bpp */
+			size = f->fmt.pix.width * f->fmt.pix.height * 4;
+			bytesperline = f->fmt.pix.width * 4;
+			pr_info("mxc_v4l2_s_fmt: default assume"
+				" to be YUV444 interleaved.\n");
+			break;
+		}
+
+		if (f->fmt.pix.bytesperline < bytesperline) {
+			f->fmt.pix.bytesperline = bytesperline;
+		} else {
+			bytesperline = f->fmt.pix.bytesperline;
+		}
+
+		if (f->fmt.pix.sizeimage > size) {
+			pr_debug("mxc_v4l2_s_fmt: sizeimage bigger than"
+				 " needed.\n");
+			size = f->fmt.pix.sizeimage;
+		}
+		f->fmt.pix.sizeimage = size;
+
+		cam->v2f.fmt.pix.sizeimage = size;
+		cam->v2f.fmt.pix.bytesperline = bytesperline;
+		cam->v2f.fmt.pix.width = f->fmt.pix.width;
+		cam->v2f.fmt.pix.height = f->fmt.pix.height;
+		cam->v2f.fmt.pix.pixelformat = f->fmt.pix.pixelformat;
+		retval = 0;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		retval = verify_preview(cam, &f->fmt.win);
+		cam->win = f->fmt.win;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/*!
+ * get control param
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_get_v42l_control(cam_data *cam, struct v4l2_control *c)
+{
+	int status = 0;
+
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		c->value = cam->rotation;
+		break;
+	case V4L2_CID_VFLIP:
+		c->value = cam->rotation;
+		break;
+	case V4L2_CID_MXC_ROT:
+		c->value = cam->rotation;
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		c->value = cam->bright;
+		break;
+	case V4L2_CID_HUE:
+		c->value = cam->hue;
+		break;
+	case V4L2_CID_CONTRAST:
+		c->value = cam->contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		c->value = cam->saturation;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		c->value = cam->red;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		c->value = cam->blue;
+		break;
+	case V4L2_CID_BLACK_LEVEL:
+		c->value = cam->ae_mode;
+		break;
+	default:
+		status = -EINVAL;
+	}
+	return status;
+}
+
+/*!
+ * V4L2 - set_control function
+ * V4L2_CID_MXC_ROT is the extention for rotation/mirroring.
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_set_v42l_control(cam_data *cam, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		if (c->value == 1) {
+			if ((cam->rotation != V4L2_MXC_ROTATE_VERT_FLIP) &&
+			    (cam->rotation != V4L2_MXC_ROTATE_180))
+				cam->rotation = V4L2_MXC_ROTATE_HORIZ_FLIP;
+			else
+				cam->rotation = V4L2_MXC_ROTATE_180;
+		} else {
+			if (cam->rotation == V4L2_MXC_ROTATE_HORIZ_FLIP)
+				cam->rotation = V4L2_MXC_ROTATE_NONE;
+			else if (cam->rotation == V4L2_MXC_ROTATE_180)
+				cam->rotation = V4L2_MXC_ROTATE_VERT_FLIP;
+		}
+		break;
+	case V4L2_CID_VFLIP:
+		if (c->value == 1) {
+			if ((cam->rotation != V4L2_MXC_ROTATE_HORIZ_FLIP) &&
+			    (cam->rotation != V4L2_MXC_ROTATE_180))
+				cam->rotation = V4L2_MXC_ROTATE_VERT_FLIP;
+			else
+				cam->rotation = V4L2_MXC_ROTATE_180;
+		} else {
+			if (cam->rotation == V4L2_MXC_ROTATE_VERT_FLIP)
+				cam->rotation = V4L2_MXC_ROTATE_NONE;
+			if (cam->rotation == V4L2_MXC_ROTATE_180)
+				cam->rotation = V4L2_MXC_ROTATE_HORIZ_FLIP;
+		}
+		break;
+	case V4L2_CID_MXC_ROT:
+		switch (c->value) {
+		case V4L2_MXC_ROTATE_NONE:
+		case V4L2_MXC_ROTATE_VERT_FLIP:
+		case V4L2_MXC_ROTATE_HORIZ_FLIP:
+		case V4L2_MXC_ROTATE_180:
+		case V4L2_MXC_ROTATE_90_RIGHT:
+		case V4L2_MXC_ROTATE_90_RIGHT_VFLIP:
+		case V4L2_MXC_ROTATE_90_RIGHT_HFLIP:
+		case V4L2_MXC_ROTATE_90_LEFT:
+			cam->rotation = c->value;
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case V4L2_CID_HUE:
+		cam->hue = c->value;
+		break;
+	case V4L2_CID_CONTRAST:
+		cam->contrast = c->value;
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		cam->bright = c->value;
+	case V4L2_CID_SATURATION:
+		cam->saturation = c->value;
+	case V4L2_CID_RED_BALANCE:
+		cam->red = c->value;
+	case V4L2_CID_BLUE_BALANCE:
+		cam->blue = c->value;
+		csi_enable_mclk(CSI_MCLK_I2C, true, true);
+		cam->cam_sensor->set_color(cam->bright, cam->saturation,
+					   cam->red, cam->green, cam->blue);
+		csi_enable_mclk(CSI_MCLK_I2C, false, false);
+		break;
+	case V4L2_CID_BLACK_LEVEL:
+		cam->ae_mode = c->value & 0x03;
+		csi_enable_mclk(CSI_MCLK_I2C, true, true);
+		if (cam->cam_sensor->set_ae_mode)
+			cam->cam_sensor->set_ae_mode(cam->ae_mode);
+		csi_enable_mclk(CSI_MCLK_I2C, false, false);
+		break;
+	case V4L2_CID_MXC_FLASH:
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_s_param function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param parm        structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
+{
+	sensor_interface *param;
+	csi_signal_cfg_t csi_param;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pr_debug("mxc_v4l2_s_param invalid type\n");
+		return -EINVAL;
+	}
+
+	if (parm->parm.capture.timeperframe.denominator >
+	    cam->standard.frameperiod.denominator) {
+		pr_debug("mxc_v4l2_s_param frame rate %d larger "
+			 "than standard supported %d\n",
+			 parm->parm.capture.timeperframe.denominator,
+			 cam->standard.frameperiod.denominator);
+		return -EINVAL;
+	}
+
+	cam->streamparm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+
+	csi_enable_mclk(CSI_MCLK_I2C, true, true);
+	param = cam->cam_sensor->config
+	    (&parm->parm.capture.timeperframe.denominator,
+	     parm->parm.capture.capturemode);
+	csi_enable_mclk(CSI_MCLK_I2C, false, false);
+
+	cam->streamparm.parm.capture.timeperframe =
+	    parm->parm.capture.timeperframe;
+
+	if ((parm->parm.capture.capturemode != 0) &&
+	    (parm->parm.capture.capturemode != V4L2_MODE_HIGHQUALITY)) {
+		pr_debug("mxc_v4l2_s_param frame un-supported capture mode\n");
+		return -EINVAL;
+	}
+
+	if (parm->parm.capture.capturemode ==
+	    cam->streamparm.parm.capture.capturemode) {
+		return 0;
+	}
+
+	/* resolution changed, so need to re-program the CSI */
+	csi_param.sens_clksrc = 0;
+	csi_param.clk_mode = param->clk_mode;
+	csi_param.pixclk_pol = param->pixclk_pol;
+	csi_param.data_width = param->data_width;
+	csi_param.data_pol = param->data_pol;
+	csi_param.ext_vsync = param->ext_vsync;
+	csi_param.Vsync_pol = param->Vsync_pol;
+	csi_param.Hsync_pol = param->Hsync_pol;
+	csi_init_interface(param->width, param->height, param->pixel_fmt,
+			   csi_param);
+
+	if (parm->parm.capture.capturemode != V4L2_MODE_HIGHQUALITY) {
+		cam->streamparm.parm.capture.capturemode = 0;
+	} else {
+		cam->streamparm.parm.capture.capturemode =
+		    V4L2_MODE_HIGHQUALITY;
+		cam->streamparm.parm.capture.extendedmode =
+		    parm->parm.capture.extendedmode;
+		cam->streamparm.parm.capture.readbuffers = 1;
+	}
+	return 0;
+}
+
+/*!
+ * Dequeue one V4L capture buffer
+ *
+ * @param cam         structure cam_data *
+ * @param buf         structure v4l2_buffer *
+ *
+ * @return  status    0 success, EINVAL invalid frame number,
+ *                    ETIME timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l_dqueue(cam_data *cam, struct v4l2_buffer *buf)
+{
+	int retval = 0;
+	struct mxc_v4l_frame *frame;
+
+	if (!wait_event_interruptible_timeout(cam->enc_queue,
+					      cam->enc_counter != 0, 10 * HZ)) {
+		printk(KERN_ERR "mxc_v4l_dqueue timeout enc_counter %x\n",
+		       cam->enc_counter);
+		return -ETIME;
+	} else if (signal_pending(current)) {
+		printk(KERN_ERR "mxc_v4l_dqueue() interrupt received\n");
+		return -ERESTARTSYS;
+	}
+
+	cam->enc_counter--;
+
+	frame = list_entry(cam->done_q.next, struct mxc_v4l_frame, queue);
+	list_del(cam->done_q.next);
+	if (frame->buffer.flags & V4L2_BUF_FLAG_DONE) {
+		frame->buffer.flags &= ~V4L2_BUF_FLAG_DONE;
+	} else if (frame->buffer.flags & V4L2_BUF_FLAG_QUEUED) {
+		printk(KERN_ERR "VIDIOC_DQBUF: Buffer not filled.\n");
+		frame->buffer.flags &= ~V4L2_BUF_FLAG_QUEUED;
+		retval = -EINVAL;
+	} else if ((frame->buffer.flags & 0x7) == V4L2_BUF_FLAG_MAPPED) {
+		printk(KERN_ERR "VIDIOC_DQBUF: Buffer not queued.\n");
+		retval = -EINVAL;
+	}
+
+	buf->bytesused = cam->v2f.fmt.pix.sizeimage;
+	buf->index = frame->index;
+	buf->flags = frame->buffer.flags;
+
+	return retval;
+}
+
+/*!
+ * V4L interface - open function
+ *
+ * @param inode        structure inode *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l_open(struct inode *inode, struct file *file)
+{
+	sensor_interface *param;
+	csi_signal_cfg_t csi_param;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int err = 0;
+
+	if (!cam) {
+		pr_info("Internal error, cam_data not found!\n");
+		return -ENODEV;
+	}
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	if (signal_pending(current))
+		goto oops;
+
+	if (cam->open_count++ == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+
+		err = prp_enc_select(cam);
+
+		cam->enc_counter = 0;
+		cam->skip_frame = 0;
+		INIT_LIST_HEAD(&cam->ready_q);
+		INIT_LIST_HEAD(&cam->working_q);
+		INIT_LIST_HEAD(&cam->done_q);
+
+		csi_enable_mclk(CSI_MCLK_I2C, true, true);
+		param = cam->cam_sensor->reset();
+		if (param == NULL) {
+			cam->open_count--;
+			csi_enable_mclk(CSI_MCLK_I2C, false, false);
+			err = -ENODEV;
+			goto oops;
+		}
+		csi_param.sens_clksrc = 0;
+		csi_param.clk_mode = param->clk_mode;
+		csi_param.pixclk_pol = param->pixclk_pol;
+		csi_param.data_width = param->data_width;
+		csi_param.data_pol = param->data_pol;
+		csi_param.ext_vsync = param->ext_vsync;
+		csi_param.Vsync_pol = param->Vsync_pol;
+		csi_param.Hsync_pol = param->Hsync_pol;
+		csi_init_interface(param->width, param->height,
+				   param->pixel_fmt, csi_param);
+		cam->cam_sensor->get_color(&cam->bright, &cam->saturation,
+					   &cam->red, &cam->green, &cam->blue);
+		if (cam->cam_sensor->get_ae_mode)
+			cam->cam_sensor->get_ae_mode(&cam->ae_mode);
+		csi_enable_mclk(CSI_MCLK_I2C, false, false);
+		prp_init(cam);
+
+	}
+
+	file->private_data = dev;
+      oops:
+	up(&cam->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L interface - close function
+ *
+ * @param inode    struct inode *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int mxc_v4l_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	int err = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	/* for the case somebody hit the ctrl C */
+	if (cam->overlay_pid == current->pid) {
+		err = stop_preview(cam);
+		cam->overlay_on = false;
+	}
+	if (cam->capture_pid == current->pid) {
+		err |= mxc_streamoff(cam);
+		cam->capture_on = false;
+		wake_up_interruptible(&cam->enc_queue);
+	}
+
+	if (--cam->open_count == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+		pr_debug("mxc_v4l_close: release resource\n");
+
+		err |= prp_enc_deselect(cam);
+
+		mxc_free_frame_buf(cam);
+		file->private_data = NULL;
+
+		/* capture off */
+		wake_up_interruptible(&cam->enc_queue);
+		mxc_free_frames(cam);
+		cam->enc_counter++;
+		prp_exit(cam);
+	}
+
+	return err;
+}
+
+#ifdef CONFIG_VIDEO_MXC_CSI_DMA
+#include <mach/dma.h>
+
+#define CSI_DMA_STATUS_IDLE	0	/* DMA is not started */
+#define CSI_DMA_STATUS_WORKING	1	/* DMA is transfering the data */
+#define CSI_DMA_STATUS_DONE	2	/* One frame completes successfully */
+#define CSI_DMA_STATUS_ERROR	3	/* Error occurs during the DMA */
+
+/*
+ * Sometimes the start of the DMA is not synchronized with the CSI
+ * SOF (Start of Frame) interrupt which will lead to incorrect
+ * captured image. In this case the driver will re-try capturing
+ * another frame. The following macro defines the maximum re-try
+ * times.
+ */
+#define CSI_DMA_RETRY		8
+
+/*
+ * Size of the physical contiguous memory area used to hold image data
+ * transfered by DMA. It can be less than the size of the image data.
+ */
+#define CSI_MEM_SIZE		(1024 * 600)
+
+/* Number of bytes for one DMA transfer */
+#define CSI_DMA_LENGTH		(1024 * 200)
+
+static int g_dma_channel;
+static int g_dma_status = CSI_DMA_STATUS_DONE;
+static volatile int g_dma_completed;	/* number of completed DMA transfers */
+static volatile int g_dma_copied;	/* number of copied DMA transfers */
+static struct tasklet_struct g_dma_tasklet;
+static char *g_user_buf;	/* represents the buf passed by read() */
+static int g_user_count;	/* represents the count passed by read() */
+
+/*!
+ * @brief setup the DMA to transfer data
+ *	  There may be more than one DMA to transfer the whole image. Those
+ *	  DMAs work like chain. This function is used to setup the DMA in
+ *	  case there is enough space to hold the data.
+ * @param	data	pointer to the cam structure
+ */
+static void mxc_csi_dma_chaining(void *data)
+{
+	cam_data *cam = (cam_data *) data;
+	int count, chained = 0;
+	int max_dma = CSI_MEM_SIZE / CSI_DMA_LENGTH;
+	mxc_dma_requestbuf_t dma_request;
+
+	while (chained * CSI_DMA_LENGTH < g_user_count) {
+		/*
+		 * Calculate how many bytes the DMA should transfer. It may
+		 * be less than CSI_DMA_LENGTH if the DMA is the last one.
+		 */
+		if ((chained + 1) * CSI_DMA_LENGTH > g_user_count)
+			count = g_user_count - chained * CSI_DMA_LENGTH;
+		else
+			count = CSI_DMA_LENGTH;
+		pr_debug("%s() DMA chained count = %d\n", __FUNCTION__, count);
+
+		/* Config DMA */
+		memset(&dma_request, 0, sizeof(mxc_dma_requestbuf_t));
+		dma_request.dst_addr = cam->still_buf
+		    + (chained % max_dma) * CSI_DMA_LENGTH;
+		dma_request.src_addr = (dma_addr_t) CSI_CSIRXFIFO_PHYADDR;
+		dma_request.num_of_bytes = count;
+		mxc_dma_config(g_dma_channel, &dma_request, 1,
+			       MXC_DMA_MODE_READ);
+
+		chained++;
+	}
+}
+
+/*!
+ * @brief Copy image data from physical contiguous memory to user space buffer
+ *	  Once the data are copied, there will be more spare space in the
+ *	  physical contiguous memory to receive data from DMA.
+ * @param	data	pointer to the cam structure
+ */
+static void mxc_csi_dma_task(unsigned long data)
+{
+	cam_data *cam = (cam_data *) data;
+	int count;
+	int max_dma = CSI_MEM_SIZE / CSI_DMA_LENGTH;
+
+	while (g_dma_copied < g_dma_completed) {
+		/*
+		 * Calculate how many bytes the DMA has transfered. It may
+		 * be less than CSI_DMA_LENGTH if the DMA is the last one.
+		 */
+		if ((g_dma_copied + 1) * CSI_DMA_LENGTH > g_user_count)
+			count = g_user_count - g_dma_copied * CSI_DMA_LENGTH;
+		else
+			count = CSI_DMA_LENGTH;
+		if (copy_to_user(g_user_buf + g_dma_copied * CSI_DMA_LENGTH,
+				 cam->still_buf_vaddr + (g_dma_copied % max_dma)
+				 * CSI_DMA_LENGTH, count))
+			pr_debug("Warning: some bytes not copied\n");
+
+		g_dma_copied++;
+	}
+
+	/* If the whole image has been captured */
+	if (g_dma_copied * CSI_DMA_LENGTH >= g_user_count) {
+		cam->still_counter++;
+		wake_up_interruptible(&cam->still_queue);
+	}
+
+	pr_debug("%s() DMA completed = %d copied = %d\n",
+		 __FUNCTION__, g_dma_completed, g_dma_copied);
+}
+
+/*!
+ * @brief DMA interrupt callback function
+ * @param	data	pointer to the cam structure
+ * @param	error	DMA error flag
+ * @param	count	number of bytes transfered by the DMA
+ */
+static void mxc_csi_dma_callback(void *data, int error, unsigned int count)
+{
+	cam_data *cam = (cam_data *) data;
+	int max_dma = CSI_MEM_SIZE / CSI_DMA_LENGTH;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&cam->int_lock, lock_flags);
+
+	g_dma_completed++;
+
+	if (error != MXC_DMA_DONE) {
+		g_dma_status = CSI_DMA_STATUS_ERROR;
+		pr_debug("%s() DMA error\n", __FUNCTION__);
+	}
+
+	/* If the whole image has been captured */
+	if ((g_dma_status != CSI_DMA_STATUS_ERROR)
+	    && (g_dma_completed * CSI_DMA_LENGTH >= g_user_count))
+		g_dma_status = CSI_DMA_STATUS_DONE;
+
+	if ((g_dma_status == CSI_DMA_STATUS_WORKING) &&
+	    (g_dma_completed >= g_dma_copied + max_dma)) {
+		g_dma_status = CSI_DMA_STATUS_ERROR;
+		pr_debug("%s() Previous buffer over written\n", __FUNCTION__);
+	}
+
+	/* Schedule the tasklet */
+	tasklet_schedule(&g_dma_tasklet);
+
+	spin_unlock_irqrestore(&cam->int_lock, lock_flags);
+
+	pr_debug("%s() count = %d bytes\n", __FUNCTION__, count);
+}
+
+/*!
+ * @brief CSI interrupt callback function
+ * @param	data	pointer to the cam structure
+ * @param	status	CSI interrupt status
+ */
+static void mxc_csi_irq_callback(void *data, unsigned long status)
+{
+	cam_data *cam = (cam_data *) data;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&cam->int_lock, lock_flags);
+
+	/* Wait for SOF (Start of Frame) interrupt to sync the image */
+	if (status & BIT_SOF_INT) {
+		if (g_dma_status == CSI_DMA_STATUS_IDLE) {
+			/* Start DMA transfer to capture image */
+			mxc_dma_enable(g_dma_channel);
+			g_dma_status = CSI_DMA_STATUS_WORKING;
+			pr_debug("%s() DMA started.\n", __FUNCTION__);
+		} else if (g_dma_status == CSI_DMA_STATUS_WORKING) {
+			/*
+			 * Another SOF occurs during DMA transfer. In this
+			 * case the image is not synchronized so need to
+			 * report error and probably try again.
+			 */
+			g_dma_status = CSI_DMA_STATUS_ERROR;
+			pr_debug("%s() Image is not synchronized with DMA - "
+				 "SOF before DMA completes\n", __FUNCTION__);
+		}
+	}
+
+	spin_unlock_irqrestore(&cam->int_lock, lock_flags);
+
+	pr_debug("%s() g_dma_status = %d\n", __FUNCTION__, g_dma_status);
+}
+
+/*!
+ * V4L interface - read function
+ *
+ * @param file       struct file *
+ * @param read buf   char *
+ * @param count      size_t
+ * @param ppos       structure loff_t *
+ *
+ * @return           bytes read
+ */
+static ssize_t
+mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
+{
+	int err = 0;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int retry = CSI_DMA_RETRY;
+
+	g_user_buf = buf;
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	/* Video capture and still image capture are exclusive */
+	if (cam->capture_on == true) {
+		err = -EBUSY;
+		goto exit0;
+	}
+
+	/* The CSI-DMA can not do CSC */
+	if (cam->v2f.fmt.pix.pixelformat != V4L2_PIX_FMT_YUYV) {
+		pr_info("mxc_v4l_read support YUYV pixel format only\n");
+		err = -EINVAL;
+		goto exit0;
+	}
+
+	/* The CSI-DMA can not do resize or crop */
+	if ((cam->v2f.fmt.pix.width != cam->crop_bounds.width)
+	    || (cam->v2f.fmt.pix.height != cam->crop_bounds.height)) {
+		pr_info("mxc_v4l_read resize is not supported\n");
+		pr_info("supported image size width = %d height = %d\n",
+			cam->crop_bounds.width, cam->crop_bounds.height);
+		err = -EINVAL;
+		goto exit0;
+	}
+	if ((cam->crop_current.left != cam->crop_bounds.left)
+	    || (cam->crop_current.width != cam->crop_bounds.width)
+	    || (cam->crop_current.top != cam->crop_bounds.top)
+	    || (cam->crop_current.height != cam->crop_bounds.height)) {
+		pr_info("mxc_v4l_read cropping is not supported\n");
+		err = -EINVAL;
+		goto exit0;
+	}
+
+	cam->still_buf_vaddr = dma_alloc_coherent(0,
+						  PAGE_ALIGN(CSI_MEM_SIZE),
+						  &cam->still_buf,
+						  GFP_DMA | GFP_KERNEL);
+
+	if (!cam->still_buf_vaddr) {
+		pr_info("mxc_v4l_read failed at allocate still_buf\n");
+		err = -ENOBUFS;
+		goto exit0;
+	}
+
+	/* Initialize DMA */
+	g_dma_channel = mxc_dma_request(MXC_DMA_CSI_RX, "CSI RX DMA");
+	if (g_dma_channel < 0) {
+		pr_debug("mxc_v4l_read failed to request DMA channel\n");
+		err = -EIO;
+		goto exit1;
+	}
+
+	err = mxc_dma_callback_set(g_dma_channel,
+				   (mxc_dma_callback_t) mxc_csi_dma_callback,
+				   (void *)cam);
+	if (err != 0) {
+		pr_debug("mxc_v4l_read failed to set DMA callback\n");
+		err = -EIO;
+		goto exit2;
+	}
+
+	g_user_buf = buf;
+	if (cam->v2f.fmt.pix.sizeimage < count)
+		g_user_count = cam->v2f.fmt.pix.sizeimage;
+	else
+		g_user_count = count & ~0x3;
+
+	tasklet_init(&g_dma_tasklet, mxc_csi_dma_task, (unsigned long)cam);
+	g_dma_status = CSI_DMA_STATUS_DONE;
+	csi_set_callback(mxc_csi_irq_callback, cam);
+	csi_enable_prpif(0);
+
+	/* clear current SOF first */
+	csi_clear_status(BIT_SOF_INT);
+	csi_enable_mclk(CSI_MCLK_RAW, true, true);
+
+	do {
+		g_dma_completed = g_dma_copied = 0;
+		mxc_csi_dma_chaining(cam);
+		cam->still_counter = 0;
+		g_dma_status = CSI_DMA_STATUS_IDLE;
+
+		if (!wait_event_interruptible_timeout(cam->still_queue,
+						      cam->still_counter != 0,
+						      10 * HZ)) {
+			pr_info("mxc_v4l_read timeout counter %x\n",
+				cam->still_counter);
+			err = -ETIME;
+			goto exit3;
+		}
+
+		if (g_dma_status == CSI_DMA_STATUS_DONE)
+			break;
+
+		if (retry-- == 0)
+			break;
+
+		pr_debug("Now retry image capture\n");
+	} while (1);
+
+	if (g_dma_status != CSI_DMA_STATUS_DONE)
+		err = -EIO;
+
+      exit3:
+	csi_enable_prpif(1);
+	g_dma_status = CSI_DMA_STATUS_DONE;
+	csi_set_callback(0, 0);
+	csi_enable_mclk(CSI_MCLK_RAW, false, false);
+	tasklet_kill(&g_dma_tasklet);
+
+      exit2:
+	mxc_dma_free(g_dma_channel);
+
+      exit1:
+	dma_free_coherent(0, PAGE_ALIGN(CSI_MEM_SIZE),
+			  cam->still_buf_vaddr, cam->still_buf);
+	cam->still_buf = 0;
+
+      exit0:
+	up(&cam->busy_lock);
+	if (err < 0)
+		return err;
+	else
+		return g_user_count;
+}
+#else
+/*!
+ * V4L interface - read function
+ *
+ * @param file       struct file *
+ * @param read buf   char *
+ * @param count      size_t
+ * @param ppos       structure loff_t *
+ *
+ * @return           bytes read
+ */
+static ssize_t
+mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
+{
+	int err = 0;
+	u8 *v_address;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	/* Video capture and still image capture are exclusive */
+	if (cam->capture_on == true) {
+		err = -EBUSY;
+		goto exit0;
+	}
+
+	v_address = dma_alloc_coherent(0,
+				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+				       &cam->still_buf, GFP_DMA | GFP_KERNEL);
+
+	if (!v_address) {
+		pr_info("mxc_v4l_read failed at allocate still_buf\n");
+		err = -ENOBUFS;
+		goto exit0;
+	}
+
+	if (prp_still_select(cam)) {
+		err = -EIO;
+		goto exit1;
+	}
+
+	cam->still_counter = 0;
+	if (cam->csi_start(cam)) {
+		err = -EIO;
+		goto exit2;
+	}
+
+	if (!wait_event_interruptible_timeout(cam->still_queue,
+					      cam->still_counter != 0,
+					      10 * HZ)) {
+		pr_info("mxc_v4l_read timeout counter %x\n",
+			cam->still_counter);
+		err = -ETIME;
+		goto exit2;
+	}
+	err = copy_to_user(buf, v_address, cam->v2f.fmt.pix.sizeimage);
+
+      exit2:
+	prp_still_deselect(cam);
+
+      exit1:
+	dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address,
+			  cam->still_buf);
+	cam->still_buf = 0;
+
+      exit0:
+	up(&cam->busy_lock);
+	if (err < 0)
+		return err;
+	else
+		return (cam->v2f.fmt.pix.sizeimage - err);
+}
+#endif				/* CONFIG_VIDEO_MXC_CSI_DMA */
+
+/*!
+ * V4L interface - ioctl function
+ *
+ * @param inode      struct inode *
+ *
+ * @param file       struct file *
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void *
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int
+mxc_v4l_do_ioctl(struct inode *inode, struct file *file,
+		 unsigned int ioctlnr, void *arg)
+{
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int retval = 0;
+	unsigned long lock_flags;
+
+	if (!cam)
+		return -EBADF;
+
+	wait_event_interruptible(cam->power_queue, cam->low_power == false);
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+		/*!
+		 * V4l2 VIDIOC_QUERYCAP ioctl
+		 */
+	case VIDIOC_QUERYCAP:{
+			struct v4l2_capability *cap = arg;
+			strcpy(cap->driver, "mxc_v4l2");
+			cap->version = KERNEL_VERSION(0, 1, 11);
+			cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+			    V4L2_CAP_VIDEO_OVERLAY | V4L2_CAP_STREAMING
+			    | V4L2_CAP_READWRITE;
+			cap->card[0] = '\0';
+			cap->bus_info[0] = '\0';
+			retval = 0;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_G_FMT ioctl
+		 */
+	case VIDIOC_G_FMT:{
+			struct v4l2_format *gf = arg;
+			retval = mxc_v4l2_g_fmt(cam, gf);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_FMT ioctl
+		 */
+	case VIDIOC_S_FMT:{
+			struct v4l2_format *sf = arg;
+			retval = mxc_v4l2_s_fmt(cam, sf);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_REQBUFS ioctl
+		 */
+	case VIDIOC_REQBUFS:{
+			struct v4l2_requestbuffers *req = arg;
+			if (req->count > FRAME_NUM) {
+				pr_info("VIDIOC_REQBUFS: not enough buffer\n");
+				req->count = FRAME_NUM;
+			}
+
+			if ((req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+			    (req->memory != V4L2_MEMORY_MMAP)) {
+				pr_debug("VIDIOC_REQBUFS: wrong buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			mxc_streamoff(cam);
+			mxc_free_frame_buf(cam);
+
+			retval = mxc_allocate_frame_buf(cam, req->count);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_QUERYBUF ioctl
+		 */
+	case VIDIOC_QUERYBUF:{
+			struct v4l2_buffer *buf = arg;
+			int index = buf->index;
+
+			if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+				pr_debug
+				    ("VIDIOC_QUERYBUFS: wrong buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			memset(buf, 0, sizeof(buf));
+			buf->index = index;
+
+			down(&cam->param_lock);
+			retval = mxc_v4l2_buffer_status(cam, buf);
+			up(&cam->param_lock);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_QBUF ioctl
+		 */
+	case VIDIOC_QBUF:{
+			struct v4l2_buffer *buf = arg;
+			int index = buf->index;
+
+			pr_debug("VIDIOC_QBUF: %d\n", buf->index);
+
+			spin_lock_irqsave(&cam->int_lock, lock_flags);
+			if ((cam->frame[index].buffer.flags & 0x7) ==
+			    V4L2_BUF_FLAG_MAPPED) {
+				cam->frame[index].buffer.flags |=
+				    V4L2_BUF_FLAG_QUEUED;
+				if (cam->skip_frame > 0) {
+					list_add_tail(&cam->frame[index].queue,
+						      &cam->working_q);
+					retval =
+					    cam->enc_update_eba(cam->
+								frame[index].
+								paddress,
+								&cam->
+								ping_pong_csi);
+					cam->skip_frame = 0;
+				} else {
+					list_add_tail(&cam->frame[index].queue,
+						      &cam->ready_q);
+				}
+			} else if (cam->frame[index].buffer.flags &
+				   V4L2_BUF_FLAG_QUEUED) {
+				pr_debug
+				    ("VIDIOC_QBUF: buffer already queued\n");
+			} else if (cam->frame[index].buffer.
+				   flags & V4L2_BUF_FLAG_DONE) {
+				pr_debug
+				    ("VIDIOC_QBUF: overwrite done buffer.\n");
+				cam->frame[index].buffer.flags &=
+				    ~V4L2_BUF_FLAG_DONE;
+				cam->frame[index].buffer.flags |=
+				    V4L2_BUF_FLAG_QUEUED;
+			}
+			buf->flags = cam->frame[index].buffer.flags;
+			spin_unlock_irqrestore(&cam->int_lock, lock_flags);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_DQBUF ioctl
+		 */
+	case VIDIOC_DQBUF:{
+			struct v4l2_buffer *buf = arg;
+
+			retval = mxc_v4l_dqueue(cam, buf);
+
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_STREAMON ioctl
+		 */
+	case VIDIOC_STREAMON:{
+			cam->capture_on = true;
+			retval = mxc_streamon(cam);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_STREAMOFF ioctl
+		 */
+	case VIDIOC_STREAMOFF:{
+			retval = mxc_streamoff(cam);
+			cam->capture_on = false;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_G_CTRL ioctl
+		 */
+	case VIDIOC_G_CTRL:{
+			retval = mxc_get_v42l_control(cam, arg);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_CTRL ioctl
+		 */
+	case VIDIOC_S_CTRL:{
+			retval = mxc_set_v42l_control(cam, arg);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_CROPCAP ioctl
+		 */
+	case VIDIOC_CROPCAP:{
+			struct v4l2_cropcap *cap = arg;
+
+			if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+			    cap->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+				retval = -EINVAL;
+				break;
+			}
+			cap->bounds = cam->crop_bounds;
+			cap->defrect = cam->crop_defrect;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_G_CROP ioctl
+		 */
+	case VIDIOC_G_CROP:{
+			struct v4l2_crop *crop = arg;
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+			    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+				retval = -EINVAL;
+				break;
+			}
+			crop->c = cam->crop_current;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_CROP ioctl
+		 */
+	case VIDIOC_S_CROP:{
+			struct v4l2_crop *crop = arg;
+			struct v4l2_rect *b = &cam->crop_bounds;
+			int i;
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+			    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+				retval = -EINVAL;
+				break;
+			}
+
+			crop->c.top = (crop->c.top < b->top) ? b->top
+			    : crop->c.top;
+			if (crop->c.top > b->top + b->height)
+				crop->c.top = b->top + b->height - 1;
+			if (crop->c.height > b->top + b->height - crop->c.top)
+				crop->c.height =
+				    b->top + b->height - crop->c.top;
+
+			crop->c.left = (crop->c.left < b->left) ? b->left
+			    : crop->c.left;
+			if (crop->c.left > b->left + b->width)
+				crop->c.left = b->left + b->width - 1;
+			if (crop->c.width > b->left - crop->c.left + b->width)
+				crop->c.width =
+				    b->left - crop->c.left + b->width;
+
+			crop->c.width &= ~0x1;
+
+			/*
+			 * MX27 PrP limitation:
+			 * The right spare space (CSI_FRAME_X_SIZE
+			 *  - SOURCE_LINE_STRIDE - PICTURE_X_SIZE)) must be
+			 * multiple of 32.
+			 * So we tune the crop->c.left value to the closest
+			 * desired cropping value and meet the PrP requirement.
+			 */
+			i = ((b->left + b->width)
+			     - (crop->c.left + crop->c.width)) % 32;
+			if (i <= 16) {
+				if (crop->c.left + crop->c.width + i
+				    <= b->left + b->width)
+					crop->c.left += i;
+				else if (crop->c.left - (32 - i) >= b->left)
+					crop->c.left -= 32 - i;
+				else {
+					retval = -EINVAL;
+					break;
+				}
+			} else {
+				if (crop->c.left - (32 - i) >= b->left)
+					crop->c.left -= 32 - i;
+				else if (crop->c.left + crop->c.width + i
+					 <= b->left + b->width)
+					crop->c.left += i;
+				else {
+					retval = -EINVAL;
+					break;
+				}
+			}
+
+			cam->crop_current = crop->c;
+
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_OVERLAY ioctl
+		 */
+	case VIDIOC_OVERLAY:{
+			int *on = arg;
+			if (*on) {
+				cam->overlay_on = true;
+				retval = start_preview(cam);
+			}
+			if (!*on) {
+				retval = stop_preview(cam);
+				cam->overlay_on = false;
+			}
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_G_FBUF ioctl
+		 */
+	case VIDIOC_G_FBUF:{
+			struct v4l2_framebuffer *fb = arg;
+			struct fb_var_screeninfo *var;
+
+			if (cam->output >= num_registered_fb) {
+				retval = -EINVAL;
+				break;
+			}
+
+			var = &registered_fb[cam->output]->var;
+			cam->v4l2_fb.fmt.width = var->xres;
+			cam->v4l2_fb.fmt.height = var->yres;
+			cam->v4l2_fb.fmt.bytesperline =
+			    var->xres_virtual * var->bits_per_pixel;
+			cam->v4l2_fb.fmt.colorspace = V4L2_COLORSPACE_SRGB;
+			*fb = cam->v4l2_fb;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_FBUF ioctl
+		 */
+	case VIDIOC_S_FBUF:{
+			struct v4l2_framebuffer *fb = arg;
+			cam->v4l2_fb.flags = fb->flags;
+			cam->v4l2_fb.fmt.pixelformat = fb->fmt.pixelformat;
+			break;
+		}
+
+	case VIDIOC_G_PARM:{
+			struct v4l2_streamparm *parm = arg;
+			if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+				pr_debug("VIDIOC_G_PARM invalid type\n");
+				retval = -EINVAL;
+				break;
+			}
+			parm->parm.capture = cam->streamparm.parm.capture;
+			break;
+		}
+	case VIDIOC_S_PARM:{
+			struct v4l2_streamparm *parm = arg;
+			retval = mxc_v4l2_s_param(cam, parm);
+			break;
+		}
+
+		/* linux v4l2 bug, kernel c0485619 user c0405619 */
+	case VIDIOC_ENUMSTD:{
+			struct v4l2_standard *e = arg;
+			*e = cam->standard;
+			pr_debug("VIDIOC_ENUMSTD call\n");
+			retval = 0;
+			break;
+		}
+
+	case VIDIOC_G_STD:{
+			v4l2_std_id *e = arg;
+			*e = cam->standard.id;
+			break;
+		}
+
+	case VIDIOC_S_STD:{
+			break;
+		}
+
+	case VIDIOC_ENUMOUTPUT:
+		{
+			struct v4l2_output *output = arg;
+
+			if (output->index >= num_registered_fb) {
+				retval = -EINVAL;
+				break;
+			}
+
+			strncpy(output->name,
+				registered_fb[output->index]->fix.id, 31);
+			output->type = V4L2_OUTPUT_TYPE_ANALOG;
+			output->audioset = 0;
+			output->modulator = 0;
+			output->std = V4L2_STD_UNKNOWN;
+
+			break;
+		}
+	case VIDIOC_G_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			*p_output_num = cam->output;
+			break;
+		}
+	case VIDIOC_S_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			if (*p_output_num >= num_registered_fb) {
+				retval = -EINVAL;
+				break;
+			}
+
+			cam->output = *p_output_num;
+			break;
+		}
+
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_ENUMINPUT:
+	case VIDIOC_G_INPUT:
+	case VIDIOC_S_INPUT:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	default:
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&cam->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L interface - ioctl function
+ *
+ * @return  None
+ */
+static int
+mxc_v4l_ioctl(struct inode *inode, struct file *file,
+	      unsigned int cmd, unsigned long arg)
+{
+	return video_usercopy(inode, file, cmd, arg, mxc_v4l_do_ioctl);
+}
+
+/*!
+ * V4L interface - mmap function
+ *
+ * @param file        structure file *
+ *
+ * @param vma         structure vm_area_struct *
+ *
+ * @return status     0 Success, EINTR busy lock error, ENOBUFS remap_page error
+ */
+static int mxc_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *dev = video_devdata(file);
+	unsigned long size;
+	int res = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	pr_debug("pgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		 vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	size = vma->vm_end - vma->vm_start;
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		pr_debug("mxc_mmap: remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+      mxc_mmap_exit:
+	up(&cam->busy_lock);
+	return res;
+}
+
+/*!
+ * V4L interface - poll function
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_poll(struct file *file, poll_table * wait)
+{
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	queue = &cam->enc_queue;
+	poll_wait(file, queue, wait);
+
+	up(&cam->busy_lock);
+	return res;
+}
+
+static struct
+file_operations mxc_v4l_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_v4l_open,
+	.release = mxc_v4l_close,
+	.read = mxc_v4l_read,
+	.ioctl = mxc_v4l_ioctl,
+	.mmap = mxc_mmap,
+	.poll = mxc_poll,
+};
+
+static struct video_device mxc_v4l_template = {
+	.name = "Mxc Camera",
+	.vfl_type = VID_TYPE_CAPTURE,
+	.fops = &mxc_v4l_fops,
+	.release = video_device_release,
+};
+
+static void camera_platform_release(struct device *device)
+{
+}
+
+/*! Device Definition for Mt9v111 devices */
+static struct platform_device mxc_v4l2_devices = {
+	.name = "mxc_v4l2",
+	.dev = {
+		.release = camera_platform_release,
+		},
+	.id = 0,
+};
+
+extern struct camera_sensor camera_sensor_if;
+
+/*!
+* Camera V4l2 callback function.
+*
+* @return status
+*/
+static void camera_callback(u32 mask, void *dev)
+{
+	struct mxc_v4l_frame *done_frame;
+	struct mxc_v4l_frame *ready_frame;
+
+	cam_data *cam = (cam_data *) dev;
+	if (cam == NULL)
+		return;
+
+	if (list_empty(&cam->working_q)) {
+		printk(KERN_ERR "camera_callback: working queue empty\n");
+		return;
+	}
+
+	done_frame =
+	    list_entry(cam->working_q.next, struct mxc_v4l_frame, queue);
+	if (done_frame->buffer.flags & V4L2_BUF_FLAG_QUEUED) {
+		done_frame->buffer.flags |= V4L2_BUF_FLAG_DONE;
+		done_frame->buffer.flags &= ~V4L2_BUF_FLAG_QUEUED;
+
+		if (list_empty(&cam->ready_q)) {
+			cam->skip_frame++;
+		} else {
+			ready_frame =
+			    list_entry(cam->ready_q.next, struct mxc_v4l_frame,
+				       queue);
+			list_del(cam->ready_q.next);
+			list_add_tail(&ready_frame->queue, &cam->working_q);
+			cam->enc_update_eba(ready_frame->paddress,
+					    &cam->ping_pong_csi);
+		}
+
+		/* Added to the done queue */
+		list_del(cam->working_q.next);
+		list_add_tail(&done_frame->queue, &cam->done_q);
+
+		/* Wake up the queue */
+		cam->enc_counter++;
+		wake_up_interruptible(&cam->enc_queue);
+	} else {
+		printk(KERN_ERR "camera_callback :buffer not queued\n");
+	}
+}
+
+/*!
+ * initialize cam_data structure
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static void init_camera_struct(cam_data *cam)
+{
+	int i;
+
+	/* Default everything to 0 */
+	memset(cam, 0, sizeof(cam_data));
+
+	init_MUTEX(&cam->param_lock);
+	init_MUTEX(&cam->busy_lock);
+
+	cam->video_dev = video_device_alloc();
+	if (cam->video_dev == NULL)
+		return;
+
+	*(cam->video_dev) = mxc_v4l_template;
+
+	video_set_drvdata(cam->video_dev, cam);
+	dev_set_drvdata(&mxc_v4l2_devices.dev, (void *)cam);
+	cam->video_dev->minor = -1;
+
+	for (i = 0; i < FRAME_NUM; i++) {
+		cam->frame[i].width = 0;
+		cam->frame[i].height = 0;
+		cam->frame[i].paddress = 0;
+	}
+
+	init_waitqueue_head(&cam->enc_queue);
+	init_waitqueue_head(&cam->still_queue);
+
+	/* setup cropping */
+	cam->crop_bounds.left = 0;
+	cam->crop_bounds.width = 640;
+	cam->crop_bounds.top = 0;
+	cam->crop_bounds.height = 480;
+	cam->crop_current = cam->crop_defrect = cam->crop_bounds;
+	cam->streamparm.parm.capture.capturemode = 0;
+
+	cam->standard.index = 0;
+	cam->standard.id = V4L2_STD_UNKNOWN;
+	cam->standard.frameperiod.denominator = 30;
+	cam->standard.frameperiod.numerator = 1;
+	cam->standard.framelines = 480;
+	cam->streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	cam->streamparm.parm.capture.timeperframe = cam->standard.frameperiod;
+	cam->streamparm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	cam->overlay_on = false;
+	cam->capture_on = false;
+	cam->skip_frame = 0;
+	cam->v4l2_fb.capability = V4L2_FBUF_CAP_EXTERNOVERLAY;
+	cam->v4l2_fb.flags = V4L2_FBUF_FLAG_PRIMARY;
+
+	cam->v2f.fmt.pix.sizeimage = 352 * 288 * 3 / 2;
+	cam->v2f.fmt.pix.bytesperline = 288 * 3 / 2;
+	cam->v2f.fmt.pix.width = 288;
+	cam->v2f.fmt.pix.height = 352;
+	cam->v2f.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
+	cam->win.w.width = 160;
+	cam->win.w.height = 160;
+	cam->win.w.left = 0;
+	cam->win.w.top = 0;
+
+	cam->cam_sensor = &camera_sensor_if;
+	cam->enc_callback = camera_callback;
+
+	init_waitqueue_head(&cam->power_queue);
+	cam->int_lock = __SPIN_LOCK_UNLOCKED(cam->int_lock);
+	spin_lock_init(&cam->int_lock);
+}
+
+extern void gpio_sensor_active(void);
+extern void gpio_sensor_inactive(void);
+
+/*!
+ * camera_power function
+ *    Turn Sensor power On/Off
+ *
+ * @param       cameraOn      true to turn camera on, otherwise shut down
+ *
+ * @return status
+ */
+static u8 camera_power(bool cameraOn)
+{
+	if (cameraOn == true) {
+		gpio_sensor_active();
+		csi_enable_mclk(csi_mclk_flag_backup, true, true);
+	} else {
+		csi_mclk_flag_backup = csi_read_mclk_flag();
+		csi_enable_mclk(csi_mclk_flag_backup, false, false);
+		gpio_sensor_inactive();
+	}
+	return 0;
+}
+
+/*!
+ * This function is called to put the sensor in a low power state. Refer to the
+ * document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure used to give information on which I2C
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function returns 0 on success and -1 on failure.
+ */
+static int mxc_v4l2_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	if (cam == NULL) {
+		return -1;
+	}
+
+	cam->low_power = true;
+
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+	if ((cam->capture_on == true) && cam->enc_disable) {
+		cam->enc_disable(cam);
+	}
+	camera_power(false);
+
+	return 0;
+}
+
+/*!
+ * This function is called to bring the sensor back from a low power state.Refer
+ * to the document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int mxc_v4l2_resume(struct platform_device *pdev)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	if (cam == NULL) {
+		return -1;
+	}
+
+	cam->low_power = false;
+	wake_up_interruptible(&cam->power_queue);
+
+	if (cam->overlay_on == true)
+		start_preview(cam);
+	if (cam->capture_on == true)
+		mxc_streamon(cam);
+	camera_power(true);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_v4l2_driver = {
+	.driver = {
+		   .name = "mxc_v4l2",
+		   .owner = THIS_MODULE,
+		   .bus = &platform_bus_type,
+		   },
+	.probe = NULL,
+	.remove = NULL,
+	.suspend = mxc_v4l2_suspend,
+	.resume = mxc_v4l2_resume,
+	.shutdown = NULL,
+};
+
+/*!
+ * Entry point for the V4L2
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int camera_init(void)
+{
+	u8 err = 0;
+	cam_data *cam;
+
+	g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL);
+	if (g_cam == NULL) {
+		pr_debug("failed to mxc_v4l_register_camera\n");
+		return -1;
+	}
+
+	cam = g_cam;
+	init_camera_struct(cam);
+
+	/* Register the I2C device */
+	err = platform_device_register(&mxc_v4l2_devices);
+	if (err != 0) {
+		pr_debug("camera_init: platform_device_register failed.\n");
+		video_device_release(cam->video_dev);
+		kfree(cam);
+		g_cam = NULL;
+	}
+
+	/* Register the device driver structure. */
+	err = platform_driver_register(&mxc_v4l2_driver);
+	if (err != 0) {
+		platform_device_unregister(&mxc_v4l2_devices);
+		pr_debug("camera_init: driver_register failed.\n");
+		video_device_release(cam->video_dev);
+		kfree(cam);
+		g_cam = NULL;
+		return err;
+	}
+
+	/* register v4l device */
+	if (video_register_device(cam->video_dev, VFL_TYPE_GRABBER, video_nr)
+	    == -1) {
+		platform_driver_unregister(&mxc_v4l2_driver);
+		platform_device_unregister(&mxc_v4l2_devices);
+		video_device_release(cam->video_dev);
+		kfree(cam);
+		g_cam = NULL;
+		pr_debug("video_register_device failed\n");
+		return -1;
+	}
+
+	return err;
+}
+
+/*!
+ * Exit and cleanup for the V4L2
+ *
+ */
+static void __exit camera_exit(void)
+{
+	pr_debug("unregistering video\n");
+
+	video_unregister_device(g_cam->video_dev);
+
+	platform_driver_unregister(&mxc_v4l2_driver);
+	platform_device_unregister(&mxc_v4l2_devices);
+
+	if (g_cam->open_count) {
+		pr_debug("camera open -- setting ops to NULL\n");
+	} else {
+		pr_debug("freeing camera\n");
+		mxc_free_frame_buf(g_cam);
+		kfree(g_cam);
+		g_cam = NULL;
+	}
+}
+
+module_init(camera_init);
+module_exit(camera_exit);
+
+module_param(video_nr, int, 0444);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2 capture driver for Mxc based cameras");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/capture/fsl_csi.c b/drivers/media/video/mxc/capture/fsl_csi.c
new file mode 100644
index 000000000000..4f14f26eede1
--- /dev/null
+++ b/drivers/media/video/mxc/capture/fsl_csi.c
@@ -0,0 +1,276 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_csi.c, this file is derived from mx27_csi.c
+ *
+ * @brief mx25 CMOS Sensor interface functions
+ *
+ * @ingroup CSI
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <mach/clock.h>
+#include <mach/hardware.h>
+
+#include "mxc_v4l2_capture.h"
+#include "fsl_csi.h"
+
+static bool g_csi_mclk_on;
+static csi_irq_callback_t g_callback;
+static void *g_callback_data;
+static struct clk csi_mclk;
+
+static irqreturn_t csi_irq_handler(int irq, void *data)
+{
+	cam_data *cam = (cam_data *) data;
+	unsigned long status = __raw_readl(CSI_CSISR);
+	unsigned long cr3 = __raw_readl(CSI_CSICR3);
+	unsigned int frame_count = (cr3 >> 16) & 0xFFFF;
+
+	__raw_writel(status, CSI_CSISR);
+
+	if (status & BIT_SOF_INT) {
+		/* reflash the embeded DMA controller */
+		if (frame_count % 2 == 1)
+			__raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+	}
+
+	if (status & BIT_DMA_TSF_DONE_FB2) {
+		if (frame_count == 2) {
+			cam->still_counter++;
+			wake_up_interruptible(&cam->still_queue);
+		}
+	}
+
+	if (g_callback)
+		g_callback(g_callback_data, status);
+
+	pr_debug("CSI status = 0x%08lX\n", status);
+
+	return IRQ_HANDLED;
+}
+
+static void csihw_reset_frame_count(void)
+{
+	__raw_writel(__raw_readl(CSI_CSICR3) | BIT_FRMCNT_RST, CSI_CSICR3);
+}
+
+static void csihw_reset(void)
+{
+	csihw_reset_frame_count();
+	__raw_writel(CSICR1_RESET_VAL, CSI_CSICR1);
+	__raw_writel(CSICR2_RESET_VAL, CSI_CSICR2);
+	__raw_writel(CSICR3_RESET_VAL, CSI_CSICR3);
+}
+
+/*!
+ * csi_init_interface
+ *    Init csi interface
+ */
+void csi_init_interface(void)
+{
+	unsigned int val = 0;
+	unsigned int imag_para;
+
+	val |= BIT_SOF_POL;
+	val |= BIT_REDGE;
+	val |= BIT_GCLK_MODE;
+	val |= BIT_HSYNC_POL;
+	val |= BIT_PACK_DIR;
+	val |= BIT_FCC;
+	val |= BIT_SWAP16_EN;
+	val |= 1 << SHIFT_MCLKDIV;
+	__raw_writel(val, CSI_CSICR1);
+
+	imag_para = (640 << 16) | 960;
+	__raw_writel(imag_para, CSI_CSIIMAG_PARA);
+
+	val = 0x1010;
+	val |= BIT_DMA_REFLASH_RFF;
+	__raw_writel(val, CSI_CSICR3);
+}
+EXPORT_SYMBOL(csi_init_interface);
+
+/*!
+ * csi_enable_mclk
+ *
+ * @param       src         enum define which source to control the clk
+ *                          CSI_MCLK_VF CSI_MCLK_ENC CSI_MCLK_RAW CSI_MCLK_I2C
+ * @param       flag        true to enable mclk, false to disable mclk
+ * @param       wait        true to wait 100ms make clock stable, false not wait
+ *
+ * @return      0 for success
+ */
+int32_t csi_enable_mclk(int src, bool flag, bool wait)
+{
+	if (flag == true) {
+		csi_mclk_enable();
+		if (wait == true)
+			msleep(10);
+		pr_debug("Enable csi clock from source %d\n", src);
+		g_csi_mclk_on = true;
+	} else {
+		csi_mclk_disable();
+		pr_debug("Disable csi clock from source %d\n", src);
+		g_csi_mclk_on = false;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(csi_enable_mclk);
+
+/*!
+ * csi_read_mclk_flag
+ *
+ * @return  gcsi_mclk_source
+ */
+int csi_read_mclk_flag(void)
+{
+	return 0;
+}
+EXPORT_SYMBOL(csi_read_mclk_flag);
+
+void csi_start_callback(void *data)
+{
+	cam_data *cam = (cam_data *) data;
+
+	if (request_irq(MXC_INT_CSI, csi_irq_handler, 0, "csi", cam) < 0)
+		pr_debug("CSI error: irq request fail\n");
+
+}
+EXPORT_SYMBOL(csi_start_callback);
+
+void csi_stop_callback(void *data)
+{
+	cam_data *cam = (cam_data *) data;
+
+	free_irq(MXC_INT_CSI, cam);
+}
+EXPORT_SYMBOL(csi_stop_callback);
+
+void csi_enable_int(int arg)
+{
+	unsigned long cr1 = __raw_readl(CSI_CSICR1);
+
+	cr1 |= BIT_SOF_INTEN;
+	if (arg == 1) {
+		/* still capture needs DMA intterrupt */
+		cr1 |= BIT_FB1_DMA_DONE_INTEN;
+		cr1 |= BIT_FB2_DMA_DONE_INTEN;
+	}
+	__raw_writel(cr1, CSI_CSICR1);
+}
+EXPORT_SYMBOL(csi_enable_int);
+
+void csi_disable_int(void)
+{
+	unsigned long cr1 = __raw_readl(CSI_CSICR1);
+
+	cr1 &= ~BIT_SOF_INTEN;
+	cr1 &= ~BIT_FB1_DMA_DONE_INTEN;
+	cr1 &= ~BIT_FB2_DMA_DONE_INTEN;
+	__raw_writel(cr1, CSI_CSICR1);
+}
+EXPORT_SYMBOL(csi_disable_int);
+
+void csi_set_16bit_imagpara(int width, int height)
+{
+	int imag_para = 0;
+	unsigned long cr3 = __raw_readl(CSI_CSICR3);
+
+	imag_para = (width << 16) | (height * 2);
+	__raw_writel(imag_para, CSI_CSIIMAG_PARA);
+
+	/* reflash the embeded DMA controller */
+	__raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+}
+EXPORT_SYMBOL(csi_set_16bit_imagpara);
+
+void csi_set_12bit_imagpara(int width, int height)
+{
+	int imag_para = 0;
+	unsigned long cr3 = __raw_readl(CSI_CSICR3);
+
+	imag_para = (width << 16) | (height * 3 / 2);
+	__raw_writel(imag_para, CSI_CSIIMAG_PARA);
+
+	/* reflash the embeded DMA controller */
+	__raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+}
+EXPORT_SYMBOL(csi_set_12bit_imagpara);
+
+static void csi_mclk_recalc(struct clk *clk)
+{
+	u32 div;
+
+	div = (__raw_readl(CSI_CSICR1) & BIT_MCLKDIV) >> SHIFT_MCLKDIV;
+	if (div == 0)
+		div = 1;
+	else
+		div = div * 2;
+
+	clk->rate = clk->parent->rate / div;
+}
+
+void csi_mclk_enable(void)
+{
+	__raw_writel(__raw_readl(CSI_CSICR1) | BIT_MCLKEN, CSI_CSICR1);
+}
+
+void csi_mclk_disable(void)
+{
+	__raw_writel(__raw_readl(CSI_CSICR1) & ~BIT_MCLKEN, CSI_CSICR1);
+}
+
+int32_t __init csi_init_module(void)
+{
+	int ret = 0;
+	struct clk *per_clk;
+
+	csihw_reset();
+	csi_init_interface();
+
+	per_clk = clk_get(NULL, "csi_clk");
+	if (IS_ERR(per_clk))
+		return PTR_ERR(per_clk);
+
+	clk_put(per_clk);
+	csi_mclk.name = "csi_mclk";
+	csi_mclk.parent = per_clk;
+	clk_register(&csi_mclk);
+	clk_enable(per_clk);
+	csi_mclk_recalc(&csi_mclk);
+
+	return ret;
+}
+
+void __exit csi_cleanup_module(void)
+{
+	clk_disable(&csi_mclk);
+	clk_unregister(&csi_mclk);
+}
+
+module_init(csi_init_module);
+module_exit(csi_cleanup_module);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("fsl CSI driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/fsl_csi.h b/drivers/media/video/mxc/capture/fsl_csi.h
new file mode 100644
index 000000000000..41bfff03f07e
--- /dev/null
+++ b/drivers/media/video/mxc/capture/fsl_csi.h
@@ -0,0 +1,197 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_csi.h
+ *
+ * @brief mx25 CMOS Sensor interface functions
+ *
+ * @ingroup CSI
+ */
+
+#ifndef MX25_CSI_H
+#define MX25_CSI_H
+
+#include <linux/io.h>
+
+/* reset values */
+#define CSICR1_RESET_VAL	0x40000800
+#define CSICR2_RESET_VAL	0x0
+#define CSICR3_RESET_VAL	0x0
+
+/* csi control reg 1 */
+#define BIT_SWAP16_EN		(0x1 << 31)
+#define BIT_EXT_VSYNC		(0x1 << 30)
+#define BIT_EOF_INT_EN		(0x1 << 29)
+#define BIT_PRP_IF_EN		(0x1 << 28)
+#define BIT_CCIR_MODE		(0x1 << 27)
+#define BIT_COF_INT_EN		(0x1 << 26)
+#define BIT_SF_OR_INTEN		(0x1 << 25)
+#define BIT_RF_OR_INTEN		(0x1 << 24)
+#define BIT_SFF_DMA_DONE_INTEN  (0x1 << 22)
+#define BIT_STATFF_INTEN	(0x1 << 21)
+#define BIT_FB2_DMA_DONE_INTEN  (0x1 << 20)
+#define BIT_FB1_DMA_DONE_INTEN  (0x1 << 19)
+#define BIT_RXFF_INTEN		(0x1 << 18)
+#define BIT_SOF_POL		(0x1 << 17)
+#define BIT_SOF_INTEN		(0x1 << 16)
+#define BIT_MCLKDIV		(0xF << 12)
+#define BIT_HSYNC_POL		(0x1 << 11)
+#define BIT_CCIR_EN		(0x1 << 10)
+#define BIT_MCLKEN		(0x1 << 9)
+#define BIT_FCC			(0x1 << 8)
+#define BIT_PACK_DIR		(0x1 << 7)
+#define BIT_CLR_STATFIFO	(0x1 << 6)
+#define BIT_CLR_RXFIFO		(0x1 << 5)
+#define BIT_GCLK_MODE		(0x1 << 4)
+#define BIT_INV_DATA		(0x1 << 3)
+#define BIT_INV_PCLK		(0x1 << 2)
+#define BIT_REDGE		(0x1 << 1)
+#define BIT_PIXEL_BIT		(0x1 << 0)
+
+#define SHIFT_MCLKDIV		12
+
+/* control reg 3 */
+#define BIT_FRMCNT		(0xFFFF << 16)
+#define BIT_FRMCNT_RST		(0x1 << 15)
+#define BIT_DMA_REFLASH_RFF	(0x1 << 14)
+#define BIT_DMA_REFLASH_SFF	(0x1 << 13)
+#define BIT_DMA_REQ_EN_RFF	(0x1 << 12)
+#define BIT_DMA_REQ_EN_SFF	(0x1 << 11)
+#define BIT_STATFF_LEVEL	(0x7 << 8)
+#define BIT_HRESP_ERR_EN	(0x1 << 7)
+#define BIT_RXFF_LEVEL		(0x7 << 4)
+#define BIT_TWO_8BIT_SENSOR	(0x1 << 3)
+#define BIT_ZERO_PACK_EN	(0x1 << 2)
+#define BIT_ECC_INT_EN		(0x1 << 1)
+#define BIT_ECC_AUTO_EN		(0x1 << 0)
+
+#define SHIFT_FRMCNT		16
+
+/* csi status reg */
+#define BIT_SFF_OR_INT		(0x1 << 25)
+#define BIT_RFF_OR_INT		(0x1 << 24)
+#define BIT_DMA_TSF_DONE_SFF	(0x1 << 22)
+#define BIT_STATFF_INT		(0x1 << 21)
+#define BIT_DMA_TSF_DONE_FB2	(0x1 << 20)
+#define BIT_DMA_TSF_DONE_FB1	(0x1 << 19)
+#define BIT_RXFF_INT		(0x1 << 18)
+#define BIT_EOF_INT		(0x1 << 17)
+#define BIT_SOF_INT		(0x1 << 16)
+#define BIT_F2_INT		(0x1 << 15)
+#define BIT_F1_INT		(0x1 << 14)
+#define BIT_COF_INT		(0x1 << 13)
+#define BIT_HRESP_ERR_INT	(0x1 << 7)
+#define BIT_ECC_INT		(0x1 << 1)
+#define BIT_DRDY		(0x1 << 0)
+
+#define CSI_MCLK_VF		1
+#define CSI_MCLK_ENC		2
+#define CSI_MCLK_RAW		4
+#define CSI_MCLK_I2C		8
+#endif
+
+#define CSI_CSICR1		(IO_ADDRESS(CSI_BASE_ADDR))
+#define CSI_CSICR2		(IO_ADDRESS(CSI_BASE_ADDR + 0x4))
+#define CSI_CSICR3		(IO_ADDRESS(CSI_BASE_ADDR + 0x8))
+#define CSI_STATFIFO		(IO_ADDRESS(CSI_BASE_ADDR + 0xC))
+#define CSI_CSIRXFIFO		(IO_ADDRESS(CSI_BASE_ADDR + 0x10))
+#define CSI_CSIRXCNT		(IO_ADDRESS(CSI_BASE_ADDR + 0x14))
+#define CSI_CSISR		(IO_ADDRESS(CSI_BASE_ADDR + 0x18))
+
+#define CSI_CSIDBG		(IO_ADDRESS(CSI_BASE_ADDR + 0x1C))
+#define CSI_CSIDMASA_STATFIFO	(IO_ADDRESS(CSI_BASE_ADDR + 0x20))
+#define CSI_CSIDMATS_STATFIFO	(IO_ADDRESS(CSI_BASE_ADDR + 0x24))
+#define CSI_CSIDMASA_FB1	(IO_ADDRESS(CSI_BASE_ADDR + 0x28))
+#define CSI_CSIDMASA_FB2	(IO_ADDRESS(CSI_BASE_ADDR + 0x2C))
+#define CSI_CSIFBUF_PARA	(IO_ADDRESS(CSI_BASE_ADDR + 0x30))
+#define CSI_CSIIMAG_PARA	(IO_ADDRESS(CSI_BASE_ADDR + 0x34))
+
+#define CSI_CSIRXFIFO_PHYADDR	(CSI_BASE_ADDR + 0x10)
+
+static inline void csi_clear_status(unsigned long status)
+{
+	__raw_writel(status, CSI_CSISR);
+}
+
+struct csi_signal_cfg_t {
+	unsigned data_width:3;
+	unsigned clk_mode:2;
+	unsigned ext_vsync:1;
+	unsigned Vsync_pol:1;
+	unsigned Hsync_pol:1;
+	unsigned pixclk_pol:1;
+	unsigned data_pol:1;
+	unsigned sens_clksrc:1;
+};
+
+struct csi_config_t {
+	/* control reg 1 */
+	unsigned int swap16_en:1;
+	unsigned int ext_vsync:1;
+	unsigned int eof_int_en:1;
+	unsigned int prp_if_en:1;
+	unsigned int ccir_mode:1;
+	unsigned int cof_int_en:1;
+	unsigned int sf_or_inten:1;
+	unsigned int rf_or_inten:1;
+	unsigned int sff_dma_done_inten:1;
+	unsigned int statff_inten:1;
+	unsigned int fb2_dma_done_inten:1;
+	unsigned int fb1_dma_done_inten:1;
+	unsigned int rxff_inten:1;
+	unsigned int sof_pol:1;
+	unsigned int sof_inten:1;
+	unsigned int mclkdiv:4;
+	unsigned int hsync_pol:1;
+	unsigned int ccir_en:1;
+	unsigned int mclken:1;
+	unsigned int fcc:1;
+	unsigned int pack_dir:1;
+	unsigned int gclk_mode:1;
+	unsigned int inv_data:1;
+	unsigned int inv_pclk:1;
+	unsigned int redge:1;
+	unsigned int pixel_bit:1;
+
+	/* control reg 3 */
+	unsigned int frmcnt:16;
+	unsigned int frame_reset:1;
+	unsigned int dma_reflash_rff:1;
+	unsigned int dma_reflash_sff:1;
+	unsigned int dma_req_en_rff:1;
+	unsigned int dma_req_en_sff:1;
+	unsigned int statff_level:3;
+	unsigned int hresp_err_en:1;
+	unsigned int rxff_level:3;
+	unsigned int two_8bit_sensor:1;
+	unsigned int zero_pack_en:1;
+	unsigned int ecc_int_en:1;
+	unsigned int ecc_auto_en:1;
+	/* fifo counter */
+	unsigned int rxcnt;
+};
+
+typedef void (*csi_irq_callback_t) (void *data, unsigned long status);
+
+int32_t csi_enable_mclk(int src, bool flag, bool wait);
+void csi_init_interface(void);
+void csi_set_16bit_imagpara(int width, int height);
+void csi_set_12bit_imagpara(int width, int height);
+int csi_read_mclk_flag(void);
+void csi_start_callback(void *data);
+void csi_stop_callback(void *data);
+void csi_enable_int(int arg);
+void csi_disable_int(void);
+void csi_mclk_enable(void);
+void csi_mclk_disable(void);
diff --git a/drivers/media/video/mxc/capture/ipu_csi_enc.c b/drivers/media/video/mxc/capture/ipu_csi_enc.c
new file mode 100644
index 000000000000..fb3d0d7d5ec6
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_csi_enc.c
@@ -0,0 +1,277 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file csi_enc.c
+ *
+ * @brief CSI Use case for video capture
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/ipu.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+#ifdef CAMERA_DBG
+	#define CAMERA_TRACE(x) (printk)x
+#else
+	#define CAMERA_TRACE(x)
+#endif
+
+/*
+ * Function definitions
+ */
+
+/*!
+ * csi ENC callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t csi_enc_callback(int irq, void *dev_id)
+{
+	cam_data *cam = (cam_data *) dev_id;
+
+	if (cam->enc_callback == NULL)
+		return IRQ_HANDLED;
+
+	cam->enc_callback(irq, dev_id);
+	return IRQ_HANDLED;
+}
+
+/*!
+ * CSI ENC enable channel setup function
+ *
+ * @param cam       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int csi_enc_setup(cam_data *cam)
+{
+	ipu_channel_params_t params;
+	u32 pixel_fmt;
+	int err = 0;
+	dma_addr_t dummy = 0xdeadbeaf;
+
+	CAMERA_TRACE("In csi_enc_setup\n");
+	if (!cam) {
+		printk(KERN_ERR "cam private is NULL\n");
+		return -ENXIO;
+	}
+
+	memset(&params, 0, sizeof(ipu_channel_params_t));
+	params.csi_mem.csi = cam->csi;
+
+	if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
+		pixel_fmt = IPU_PIX_FMT_YUV420P;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P)
+		pixel_fmt = IPU_PIX_FMT_YUV422P;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY)
+		pixel_fmt = IPU_PIX_FMT_UYVY;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_NV12)
+		pixel_fmt = IPU_PIX_FMT_NV12;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR24)
+		pixel_fmt = IPU_PIX_FMT_BGR24;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24)
+		pixel_fmt = IPU_PIX_FMT_RGB24;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB565)
+		pixel_fmt = IPU_PIX_FMT_RGB565;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR32)
+		pixel_fmt = IPU_PIX_FMT_BGR32;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB32)
+		pixel_fmt = IPU_PIX_FMT_RGB32;
+	else {
+		printk(KERN_ERR "format not supported\n");
+		return -EINVAL;
+	}
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_ENC, cam->csi, true, true);
+
+	err = ipu_init_channel(CSI_MEM, &params);
+	if (err != 0) {
+		printk(KERN_ERR "ipu_init_channel %d\n", err);
+		return err;
+	}
+
+	err = ipu_init_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+				      pixel_fmt, cam->v2f.fmt.pix.width,
+				      cam->v2f.fmt.pix.height,
+				      cam->v2f.fmt.pix.width, IPU_ROTATE_NONE,
+				      dummy, dummy,
+				      cam->offset.u_offset,
+				      cam->offset.v_offset);
+	if (err != 0) {
+		printk(KERN_ERR "CSI_MEM output buffer\n");
+		return err;
+	}
+	err = ipu_enable_channel(CSI_MEM);
+	if (err < 0) {
+		printk(KERN_ERR "ipu_enable_channel CSI_MEM\n");
+		return err;
+	}
+
+	return err;
+}
+
+/*!
+ * function to update physical buffer address for encorder IDMA channel
+ *
+ * @param eba         physical buffer address for encorder IDMA channel
+ * @param buffer_num  int buffer 0 or buffer 1
+ *
+ * @return  status
+ */
+static int csi_enc_eba_update(dma_addr_t eba, int *buffer_num)
+{
+	int err = 0;
+
+	pr_debug("eba %x\n", eba);
+	err = ipu_update_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+					*buffer_num, eba);
+	if (err != 0) {
+		printk(KERN_ERR "err %d buffer_num %d\n", err, *buffer_num);
+		return err;
+	}
+
+	ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, *buffer_num);
+
+	*buffer_num = (*buffer_num == 0) ? 1 : 0;
+
+	return 0;
+}
+
+/*!
+ * Enable encoder task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int csi_enc_enabling_tasks(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+	CAMERA_TRACE("IPU:In csi_enc_enabling_tasks\n");
+
+	err = ipu_request_irq(IPU_IRQ_CSI0_OUT_EOF,
+			      csi_enc_callback, 0, "Mxc Camera", cam);
+	if (err != 0) {
+		printk(KERN_ERR "Error registering rot irq\n");
+		return err;
+	}
+
+	err = csi_enc_setup(cam);
+	if (err != 0) {
+		printk(KERN_ERR "csi_enc_setup %d\n", err);
+		return err;
+	}
+
+	return err;
+}
+
+/*!
+ * Disable encoder task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+static int csi_enc_disabling_tasks(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	ipu_free_irq(IPU_IRQ_CSI0_OUT_EOF, cam);
+
+	err = ipu_disable_channel(CSI_MEM, true);
+
+	ipu_uninit_channel(CSI_MEM);
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_ENC, cam->csi, false, false);
+
+	return err;
+}
+
+/*!
+ * function to select CSI ENC as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+int csi_enc_select(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	if (cam) {
+		cam->enc_update_eba = csi_enc_eba_update;
+		cam->enc_enable = csi_enc_enabling_tasks;
+		cam->enc_disable = csi_enc_disabling_tasks;
+	} else {
+		err = -EIO;
+	}
+
+	return err;
+}
+
+/*!
+ * function to de-select CSI ENC as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+int csi_enc_deselect(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	if (cam) {
+		cam->enc_update_eba = NULL;
+		cam->enc_enable = NULL;
+		cam->enc_disable = NULL;
+	}
+
+	return err;
+}
+
+/*!
+ * Init the Encorder channels
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int csi_enc_init(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit the Encorder channels
+ *
+ */
+void __exit csi_enc_exit(void)
+{
+}
+
+module_init(csi_enc_init);
+module_exit(csi_enc_exit);
+
+EXPORT_SYMBOL(csi_enc_select);
+EXPORT_SYMBOL(csi_enc_deselect);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("CSI ENC Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ipu_prp_enc.c b/drivers/media/video/mxc/capture/ipu_prp_enc.c
new file mode 100644
index 000000000000..79eb8a615ee8
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_prp_enc.c
@@ -0,0 +1,455 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_prp_enc.c
+ *
+ * @brief IPU Use case for PRP-ENC
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/ipu.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+#ifdef CAMERA_DBG
+	#define CAMERA_TRACE(x) (printk)x
+#else
+	#define CAMERA_TRACE(x)
+#endif
+
+static ipu_rotate_mode_t grotation = IPU_ROTATE_NONE;
+
+/*
+ * Function definitions
+ */
+
+/*!
+ * IPU ENC callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t prp_enc_callback(int irq, void *dev_id)
+{
+	cam_data *cam = (cam_data *) dev_id;
+
+	if (cam->enc_callback == NULL)
+		return IRQ_HANDLED;
+
+	cam->enc_callback(irq, dev_id);
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * PrpENC enable channel setup function
+ *
+ * @param cam       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int prp_enc_setup(cam_data * cam)
+{
+	ipu_channel_params_t enc;
+	int err = 0;
+	dma_addr_t dummy = 0xdeadbeaf;
+
+	CAMERA_TRACE("In prp_enc_setup\n");
+	if (!cam) {
+		printk(KERN_ERR "cam private is NULL\n");
+		return -ENXIO;
+	}
+	memset(&enc, 0, sizeof(ipu_channel_params_t));
+
+	ipu_csi_get_window_size(&enc.csi_prp_enc_mem.in_width,
+				&enc.csi_prp_enc_mem.in_height, cam->csi);
+
+	enc.csi_prp_enc_mem.in_pixel_fmt = IPU_PIX_FMT_UYVY;
+	enc.csi_prp_enc_mem.out_width = cam->v2f.fmt.pix.width;
+	enc.csi_prp_enc_mem.out_height = cam->v2f.fmt.pix.height;
+	enc.csi_prp_enc_mem.csi = cam->csi;
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		enc.csi_prp_enc_mem.out_width = cam->v2f.fmt.pix.height;
+		enc.csi_prp_enc_mem.out_height = cam->v2f.fmt.pix.width;
+	}
+
+	if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_YUV420P;
+		pr_info("YUV420\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_YUV422P;
+		pr_info("YUV422P\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_NV12) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_NV12;
+		pr_info("NV12\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR24) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_BGR24;
+		pr_info("BGR24\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_RGB24;
+		pr_info("RGB24\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB565) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_RGB565;
+		pr_info("RGB565\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR32) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_BGR32;
+		pr_info("BGR32\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB32) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_RGB32;
+		pr_info("RGB32\n");
+	} else {
+		printk(KERN_ERR "format not supported\n");
+		return -EINVAL;
+	}
+
+	err = ipu_init_channel(CSI_PRP_ENC_MEM, &enc);
+	if (err != 0) {
+		printk(KERN_ERR "ipu_init_channel %d\n", err);
+		return err;
+	}
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_ENC, cam->csi, true, true);
+
+	grotation = cam->rotation;
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		if (cam->rot_enc_bufs_vaddr[0]) {
+			dma_free_coherent(0, cam->rot_enc_buf_size[0],
+					  cam->rot_enc_bufs_vaddr[0],
+					  cam->rot_enc_bufs[0]);
+		}
+		if (cam->rot_enc_bufs_vaddr[1]) {
+			dma_free_coherent(0, cam->rot_enc_buf_size[1],
+					  cam->rot_enc_bufs_vaddr[1],
+					  cam->rot_enc_bufs[1]);
+		}
+		cam->rot_enc_buf_size[0] =
+		    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
+		cam->rot_enc_bufs_vaddr[0] =
+		    (void *)dma_alloc_coherent(0, cam->rot_enc_buf_size[0],
+					       &cam->rot_enc_bufs[0],
+					       GFP_DMA | GFP_KERNEL);
+		if (!cam->rot_enc_bufs_vaddr[0]) {
+			printk(KERN_ERR "alloc enc_bufs0\n");
+			return -ENOMEM;
+		}
+		cam->rot_enc_buf_size[1] =
+		    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
+		cam->rot_enc_bufs_vaddr[1] =
+		    (void *)dma_alloc_coherent(0, cam->rot_enc_buf_size[1],
+					       &cam->rot_enc_bufs[1],
+					       GFP_DMA | GFP_KERNEL);
+		if (!cam->rot_enc_bufs_vaddr[1]) {
+			dma_free_coherent(0, cam->rot_enc_buf_size[0],
+					  cam->rot_enc_bufs_vaddr[0],
+					  cam->rot_enc_bufs[0]);
+			cam->rot_enc_bufs_vaddr[0] = NULL;
+			cam->rot_enc_bufs[0] = 0;
+			printk(KERN_ERR "alloc enc_bufs1\n");
+			return -ENOMEM;
+		}
+
+		err = ipu_init_channel_buffer(CSI_PRP_ENC_MEM,
+					      IPU_OUTPUT_BUFFER,
+					      enc.csi_prp_enc_mem.out_pixel_fmt,
+					      enc.csi_prp_enc_mem.out_width,
+					      enc.csi_prp_enc_mem.out_height,
+					      enc.csi_prp_enc_mem.out_width,
+					      IPU_ROTATE_NONE,
+					      cam->rot_enc_bufs[0],
+					      cam->rot_enc_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "CSI_PRP_ENC_MEM err\n");
+			return err;
+		}
+
+		err = ipu_init_channel(MEM_ROT_ENC_MEM, NULL);
+		if (err != 0) {
+			printk(KERN_ERR "MEM_ROT_ENC_MEM channel err\n");
+			return err;
+		}
+
+		err = ipu_init_channel_buffer(MEM_ROT_ENC_MEM, IPU_INPUT_BUFFER,
+					      enc.csi_prp_enc_mem.out_pixel_fmt,
+					      enc.csi_prp_enc_mem.out_width,
+					      enc.csi_prp_enc_mem.out_height,
+					      enc.csi_prp_enc_mem.out_width,
+					      cam->rotation,
+					      cam->rot_enc_bufs[0],
+					      cam->rot_enc_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "MEM_ROT_ENC_MEM input buffer\n");
+			return err;
+		}
+
+		err =
+		    ipu_init_channel_buffer(MEM_ROT_ENC_MEM, IPU_OUTPUT_BUFFER,
+					    enc.csi_prp_enc_mem.out_pixel_fmt,
+					    enc.csi_prp_enc_mem.out_height,
+					    enc.csi_prp_enc_mem.out_width,
+					    cam->v2f.fmt.pix.bytesperline /
+					    bytes_per_pixel(enc.csi_prp_enc_mem.
+							    out_pixel_fmt),
+					    IPU_ROTATE_NONE, dummy, dummy,
+					    cam->offset.u_offset,
+					    cam->offset.v_offset);
+		if (err != 0) {
+			printk(KERN_ERR "MEM_ROT_ENC_MEM output buffer\n");
+			return err;
+		}
+
+		err = ipu_link_channels(CSI_PRP_ENC_MEM, MEM_ROT_ENC_MEM);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "link CSI_PRP_ENC_MEM-MEM_ROT_ENC_MEM\n");
+			return err;
+		}
+
+		err = ipu_enable_channel(CSI_PRP_ENC_MEM);
+		if (err < 0) {
+			printk(KERN_ERR "ipu_enable_channel CSI_PRP_ENC_MEM\n");
+			return err;
+		}
+		err = ipu_enable_channel(MEM_ROT_ENC_MEM);
+		if (err < 0) {
+			printk(KERN_ERR "ipu_enable_channel MEM_ROT_ENC_MEM\n");
+			return err;
+		}
+
+		ipu_select_buffer(CSI_PRP_ENC_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(CSI_PRP_ENC_MEM, IPU_OUTPUT_BUFFER, 1);
+	} else {
+		err =
+		    ipu_init_channel_buffer(CSI_PRP_ENC_MEM, IPU_OUTPUT_BUFFER,
+					    enc.csi_prp_enc_mem.out_pixel_fmt,
+					    enc.csi_prp_enc_mem.out_width,
+					    enc.csi_prp_enc_mem.out_height,
+					    cam->v2f.fmt.pix.bytesperline /
+					    bytes_per_pixel(enc.csi_prp_enc_mem.
+							    out_pixel_fmt),
+					    cam->rotation, dummy, dummy,
+					    cam->offset.u_offset,
+					    cam->offset.v_offset);
+		if (err != 0) {
+			printk(KERN_ERR "CSI_PRP_ENC_MEM output buffer\n");
+			return err;
+		}
+		err = ipu_enable_channel(CSI_PRP_ENC_MEM);
+		if (err < 0) {
+			printk(KERN_ERR "ipu_enable_channel CSI_PRP_ENC_MEM\n");
+			return err;
+		}
+	}
+
+	return err;
+}
+
+/*!
+ * function to update physical buffer address for encorder IDMA channel
+ *
+ * @param eba         physical buffer address for encorder IDMA channel
+ * @param buffer_num  int buffer 0 or buffer 1
+ *
+ * @return  status
+ */
+static int prp_enc_eba_update(dma_addr_t eba, int *buffer_num)
+{
+	int err = 0;
+
+	pr_debug("eba %x\n", eba);
+	if (grotation >= IPU_ROTATE_90_RIGHT) {
+		err = ipu_update_channel_buffer(MEM_ROT_ENC_MEM,
+						IPU_OUTPUT_BUFFER, *buffer_num,
+						eba);
+	} else {
+		err = ipu_update_channel_buffer(CSI_PRP_ENC_MEM,
+						IPU_OUTPUT_BUFFER, *buffer_num,
+						eba);
+	}
+	if (err != 0) {
+		printk(KERN_ERR "err %d buffer_num %d\n", err, *buffer_num);
+		return err;
+	}
+
+	if (grotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_select_buffer(MEM_ROT_ENC_MEM, IPU_OUTPUT_BUFFER,
+				  *buffer_num);
+	} else {
+		ipu_select_buffer(CSI_PRP_ENC_MEM, IPU_OUTPUT_BUFFER,
+				  *buffer_num);
+	}
+
+	*buffer_num = (*buffer_num == 0) ? 1 : 0;
+	return 0;
+}
+
+/*!
+ * Enable encoder task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int prp_enc_enabling_tasks(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+	CAMERA_TRACE("IPU:In prp_enc_enabling_tasks\n");
+
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		err = ipu_request_irq(IPU_IRQ_PRP_ENC_ROT_OUT_EOF,
+				      prp_enc_callback, 0, "Mxc Camera", cam);
+	} else {
+		err = ipu_request_irq(IPU_IRQ_PRP_ENC_OUT_EOF,
+				      prp_enc_callback, 0, "Mxc Camera", cam);
+	}
+	if (err != 0) {
+		printk(KERN_ERR "Error registering rot irq\n");
+		return err;
+	}
+
+	err = prp_enc_setup(cam);
+	if (err != 0) {
+		printk(KERN_ERR "prp_enc_setup %d\n", err);
+		return err;
+	}
+
+	return err;
+}
+
+/*!
+ * Disable encoder task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+static int prp_enc_disabling_tasks(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_free_irq(IPU_IRQ_PRP_ENC_ROT_OUT_EOF, cam);
+	} else {
+		ipu_free_irq(IPU_IRQ_PRP_ENC_OUT_EOF, cam);
+	}
+
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_unlink_channels(CSI_PRP_ENC_MEM, MEM_ROT_ENC_MEM);
+	}
+
+	err = ipu_disable_channel(CSI_PRP_ENC_MEM, true);
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		err |= ipu_disable_channel(MEM_ROT_ENC_MEM, true);
+	}
+
+	ipu_uninit_channel(CSI_PRP_ENC_MEM);
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_uninit_channel(MEM_ROT_ENC_MEM);
+	}
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_ENC, cam->csi, false, false);
+
+	return err;
+}
+
+/*!
+ * function to select PRP-ENC as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+int prp_enc_select(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	if (cam) {
+		cam->enc_update_eba = prp_enc_eba_update;
+		cam->enc_enable = prp_enc_enabling_tasks;
+		cam->enc_disable = prp_enc_disabling_tasks;
+	} else {
+		err = -EIO;
+	}
+
+	return err;
+}
+
+/*!
+ * function to de-select PRP-ENC as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+int prp_enc_deselect(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	//err = prp_enc_disabling_tasks(cam);
+
+	if (cam) {
+		cam->enc_update_eba = NULL;
+		cam->enc_enable = NULL;
+		cam->enc_disable = NULL;
+		if (cam->rot_enc_bufs_vaddr[0]) {
+			dma_free_coherent(0, cam->rot_enc_buf_size[0],
+					  cam->rot_enc_bufs_vaddr[0],
+					  cam->rot_enc_bufs[0]);
+			cam->rot_enc_bufs_vaddr[0] = NULL;
+			cam->rot_enc_bufs[0] = 0;
+		}
+		if (cam->rot_enc_bufs_vaddr[1]) {
+			dma_free_coherent(0, cam->rot_enc_buf_size[1],
+					  cam->rot_enc_bufs_vaddr[1],
+					  cam->rot_enc_bufs[1]);
+			cam->rot_enc_bufs_vaddr[1] = NULL;
+			cam->rot_enc_bufs[1] = 0;
+		}
+	}
+
+	return err;
+}
+
+/*!
+ * Init the Encorder channels
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int prp_enc_init(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit the Encorder channels
+ *
+ */
+void __exit prp_enc_exit(void)
+{
+}
+
+module_init(prp_enc_init);
+module_exit(prp_enc_exit);
+
+EXPORT_SYMBOL(prp_enc_select);
+EXPORT_SYMBOL(prp_enc_deselect);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IPU PRP ENC Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ipu_prp_sw.h b/drivers/media/video/mxc/capture/ipu_prp_sw.h
new file mode 100644
index 000000000000..0d51e2ae8670
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_prp_sw.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_prp_sw.h
+ *
+ * @brief This file contains the IPU PRP use case driver header.
+ *
+ * @ingroup IPU
+ */
+
+#ifndef _INCLUDE_IPU__PRP_SW_H_
+#define _INCLUDE_IPU__PRP_SW_H_
+
+int csi_enc_select(void *private);
+int csi_enc_deselect(void *private);
+int prp_enc_select(void *private);
+int prp_enc_deselect(void *private);
+int prp_vf_adc_select(void *private);
+int prp_vf_sdc_select(void *private);
+int prp_vf_sdc_select_bg(void *private);
+int prp_vf_adc_deselect(void *private);
+int prp_vf_sdc_deselect(void *private);
+int prp_vf_sdc_deselect_bg(void *private);
+int prp_still_select(void *private);
+int prp_still_deselect(void *private);
+
+#endif
diff --git a/drivers/media/video/mxc/capture/ipu_prp_vf_adc.c b/drivers/media/video/mxc/capture/ipu_prp_vf_adc.c
new file mode 100644
index 000000000000..a7ac09ae87d4
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_prp_vf_adc.c
@@ -0,0 +1,601 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_prp_vf_adc.c
+ *
+ * @brief IPU Use case for PRP-VF
+ *
+ * @ingroup IPU
+ */
+
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+#include <mach/mxcfb.h>
+#include <mach/ipu.h>
+#include <linux/dma-mapping.h>
+
+/*
+ * Function definitions
+ */
+
+/*!
+ * prpvf_start - start the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_start(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	ipu_channel_params_t vf;
+	ipu_channel_params_t params;
+	u32 format = IPU_PIX_FMT_RGB565;
+	u32 size = 2;
+	int err = 0;
+
+	if (!cam) {
+		printk(KERN_ERR "prpvf_start private is NULL\n");
+		return -ENXIO;
+	}
+
+	if (cam->overlay_active == true) {
+		printk(KERN_ERR "prpvf_start already start.\n");
+		return 0;
+	}
+
+	mxcfb_set_refresh_mode(cam->overlay_fb, MXCFB_REFRESH_OFF, 0);
+
+	memset(&vf, 0, sizeof(ipu_channel_params_t));
+	ipu_csi_get_window_size(&vf.csi_prp_vf_adc.in_width,
+				&vf.csi_prp_vf_adc.in_height);
+	vf.csi_prp_vf_adc.in_pixel_fmt = IPU_PIX_FMT_UYVY;
+	vf.csi_prp_vf_adc.out_width = cam->win.w.width;
+	vf.csi_prp_vf_adc.out_height = cam->win.w.height;
+	vf.csi_prp_vf_adc.graphics_combine_en = 0;
+	vf.csi_prp_vf_adc.out_left = cam->win.w.left;
+
+	/* hope to be removed when those offset taken cared by adc driver. */
+#ifdef CONFIG_FB_MXC_EPSON_QVGA_PANEL
+	vf.csi_prp_vf_adc.out_left += 12;
+#endif
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+	vf.csi_prp_vf_adc.out_left += 2;
+#endif
+
+	vf.csi_prp_vf_adc.out_top = cam->win.w.top;
+
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		vf.csi_prp_vf_adc.out_width = cam->win.w.height;
+		vf.csi_prp_vf_adc.out_height = cam->win.w.width;
+
+		size = cam->win.w.width * cam->win.w.height * size;
+		vf.csi_prp_vf_adc.out_pixel_fmt = format;
+		err = ipu_init_channel(CSI_PRP_VF_MEM, &vf);
+		if (err != 0)
+			return err;
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, true, true);
+
+		if (cam->vf_bufs_vaddr[0]) {
+			dma_free_coherent(0, cam->vf_bufs_size[0],
+					  cam->vf_bufs_vaddr[0],
+					  cam->vf_bufs[0]);
+		}
+		if (cam->vf_bufs_vaddr[1]) {
+			dma_free_coherent(0, cam->vf_bufs_size[1],
+					  cam->vf_bufs_vaddr[1],
+					  cam->vf_bufs[1]);
+		}
+		cam->vf_bufs_size[0] = size;
+		cam->vf_bufs_vaddr[0] = (void *)dma_alloc_coherent(0,
+								   cam->
+								   vf_bufs_size
+								   [0],
+								   &cam->
+								   vf_bufs[0],
+								   GFP_DMA |
+								   GFP_KERNEL);
+		if (cam->vf_bufs_vaddr[0] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate vf buffer\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+		cam->vf_bufs_size[1] = size;
+		cam->vf_bufs_vaddr[1] = (void *)dma_alloc_coherent(0,
+								   cam->
+								   vf_bufs_size
+								   [1],
+								   &cam->
+								   vf_bufs[1],
+								   GFP_DMA |
+								   GFP_KERNEL);
+		if (cam->vf_bufs_vaddr[1] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate vf buffer\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+
+		err = ipu_init_channel_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      IPU_ROTATE_NONE,
+					      cam->vf_bufs[0], cam->vf_bufs[1],
+					      0, 0);
+		if (err != 0)
+			goto out_3;
+
+		if (cam->rot_vf_bufs[0]) {
+			dma_free_coherent(0, cam->rot_vf_buf_size[0],
+					  cam->rot_vf_bufs_vaddr[0],
+					  cam->rot_vf_bufs[0]);
+		}
+		if (cam->rot_vf_bufs[1]) {
+			dma_free_coherent(0, cam->rot_vf_buf_size[1],
+					  cam->rot_vf_bufs_vaddr[1],
+					  cam->rot_vf_bufs[1]);
+		}
+		cam->rot_vf_buf_size[0] = PAGE_ALIGN(size);
+		cam->rot_vf_bufs_vaddr[0] = (void *)dma_alloc_coherent(0,
+								       cam->
+								       rot_vf_buf_size
+								       [0],
+								       &cam->
+								       rot_vf_bufs
+								       [0],
+								       GFP_DMA |
+								       GFP_KERNEL);
+		if (cam->rot_vf_bufs_vaddr[0] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate rot_vf_bufs\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+		cam->rot_vf_buf_size[1] = PAGE_ALIGN(size);
+		cam->rot_vf_bufs_vaddr[1] = (void *)dma_alloc_coherent(0,
+								       cam->
+								       rot_vf_buf_size
+								       [1],
+								       &cam->
+								       rot_vf_bufs
+								       [1],
+								       GFP_DMA |
+								       GFP_KERNEL);
+		if (cam->rot_vf_bufs_vaddr[1] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate rot_vf_bufs\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+		err = ipu_init_channel(MEM_ROT_VF_MEM, NULL);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start :Error "
+			       "MEM_ROT_VF_MEM channel\n");
+			goto out_3;
+		}
+
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_INPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      cam->vf_rotation, cam->vf_bufs[0],
+					      cam->vf_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "MEM_ROT_VF_MEM input buffer\n");
+			goto out_2;
+		}
+
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      IPU_ROTATE_NONE,
+					      cam->rot_vf_bufs[0],
+					      cam->rot_vf_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "MEM_ROT_VF_MEM output buffer\n");
+			goto out_2;
+		}
+
+		err = ipu_link_channels(CSI_PRP_VF_MEM, MEM_ROT_VF_MEM);
+		if (err < 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "linking CSI_PRP_VF_MEM-MEM_ROT_VF_MEM\n");
+			goto out_2;
+		}
+
+		ipu_disable_channel(ADC_SYS2, false);
+		ipu_uninit_channel(ADC_SYS2);
+
+		params.adc_sys2.disp = DISP0;
+		params.adc_sys2.ch_mode = WriteTemplateNonSeq;
+		params.adc_sys2.out_left = cam->win.w.left;
+		/* going to be removed when those offset taken cared by adc driver. */
+#ifdef CONFIG_FB_MXC_EPSON_QVGA_PANEL
+		params.adc_sys2.out_left += 12;
+#endif
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+		params.adc_sys2.out_left += 2;
+#endif
+		params.adc_sys2.out_top = cam->win.w.top;
+		err = ipu_init_channel(ADC_SYS2, &params);
+		if (err != 0) {
+			printk(KERN_ERR
+			       "prpvf_start: Error initializing ADC SYS1\n");
+			goto out_2;
+		}
+
+		err = ipu_init_channel_buffer(ADC_SYS2, IPU_INPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      IPU_ROTATE_NONE,
+					      cam->rot_vf_bufs[0],
+					      cam->rot_vf_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "Error initializing ADC SYS1 buffer\n");
+			goto out_1;
+		}
+
+		err = ipu_link_channels(MEM_ROT_VF_MEM, ADC_SYS2);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "Error linking MEM_ROT_VF_MEM-ADC_SYS2\n");
+			goto out_1;
+		}
+
+		ipu_enable_channel(CSI_PRP_VF_MEM);
+		ipu_enable_channel(MEM_ROT_VF_MEM);
+		ipu_enable_channel(ADC_SYS2);
+
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+	}
+#ifndef CONFIG_MXC_IPU_PRP_VF_SDC
+	else if (cam->vf_rotation == IPU_ROTATE_NONE) {
+		vf.csi_prp_vf_adc.out_pixel_fmt = IPU_PIX_FMT_BGR32;
+		err = ipu_init_channel(CSI_PRP_VF_ADC, &vf);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing CSI_PRP_VF_ADC\n");
+			return err;
+		}
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, true, true);
+		err = ipu_init_channel_buffer(CSI_PRP_VF_ADC, IPU_OUTPUT_BUFFER,
+					      format, cam->win.w.width,
+					      cam->win.w.height,
+					      cam->win.w.width, IPU_ROTATE_NONE,
+					      0, 0, 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing CSI_PRP_VF_MEM\n");
+			return err;
+		}
+		ipu_enable_channel(CSI_PRP_VF_ADC);
+	}
+#endif
+	else {
+		size = cam->win.w.width * cam->win.w.height * size;
+		vf.csi_prp_vf_adc.out_pixel_fmt = format;
+		err = ipu_init_channel(CSI_PRP_VF_MEM, &vf);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing CSI_PRP_VF_MEM\n");
+			return err;
+		}
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, true, true);
+
+		if (cam->vf_bufs[0]) {
+			dma_free_coherent(0, cam->vf_bufs_size[0],
+					  cam->vf_bufs_vaddr[0],
+					  cam->vf_bufs[0]);
+		}
+		if (cam->vf_bufs[1]) {
+			dma_free_coherent(0, cam->vf_bufs_size[1],
+					  cam->vf_bufs_vaddr[1],
+					  cam->vf_bufs[1]);
+		}
+		cam->vf_bufs_size[0] = PAGE_ALIGN(size);
+		cam->vf_bufs_vaddr[0] = (void *)dma_alloc_coherent(0,
+								   cam->
+								   vf_bufs_size
+								   [0],
+								   &cam->
+								   vf_bufs[0],
+								   GFP_DMA |
+								   GFP_KERNEL);
+		if (cam->vf_bufs_vaddr[0] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate vf_bufs\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+		cam->vf_bufs_size[1] = PAGE_ALIGN(size);
+		cam->vf_bufs_vaddr[1] = (void *)dma_alloc_coherent(0,
+								   cam->
+								   vf_bufs_size
+								   [1],
+								   &cam->
+								   vf_bufs[1],
+								   GFP_DMA |
+								   GFP_KERNEL);
+		if (cam->vf_bufs_vaddr[1] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate vf_bufs\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+		err = ipu_init_channel_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      cam->vf_rotation,
+					      cam->vf_bufs[0], cam->vf_bufs[1],
+					      0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing CSI_PRP_VF_MEM\n");
+			goto out_3;
+		}
+
+		ipu_disable_channel(ADC_SYS2, false);
+		ipu_uninit_channel(ADC_SYS2);
+
+		params.adc_sys2.disp = DISP0;
+		params.adc_sys2.ch_mode = WriteTemplateNonSeq;
+		params.adc_sys2.out_left = cam->win.w.left;
+		// going to be removed when those offset taken cared by adc driver.
+#ifdef CONFIG_FB_MXC_EPSON_QVGA_PANEL
+		params.adc_sys2.out_left += 12;
+#endif
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+		params.adc_sys2.out_left += 2;
+#endif
+		params.adc_sys2.out_top = cam->win.w.top;
+		err = ipu_init_channel(ADC_SYS2, &params);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing ADC_SYS2\n");
+			goto out_3;
+		}
+
+		err = ipu_init_channel_buffer(ADC_SYS2, IPU_INPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      IPU_ROTATE_NONE, cam->vf_bufs[0],
+					      cam->vf_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing ADC SYS1 buffer\n");
+			goto out_1;
+		}
+
+		err = ipu_link_channels(CSI_PRP_VF_MEM, ADC_SYS2);
+		if (err < 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "linking MEM_ROT_VF_MEM-ADC_SYS2\n");
+			goto out_1;
+		}
+
+		ipu_enable_channel(CSI_PRP_VF_MEM);
+		ipu_enable_channel(ADC_SYS2);
+
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+	}
+
+	cam->overlay_active = true;
+	return err;
+
+      out_1:
+	ipu_uninit_channel(ADC_SYS2);
+      out_2:
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_uninit_channel(MEM_ROT_VF_MEM);
+	}
+      out_3:
+	ipu_uninit_channel(CSI_PRP_VF_MEM);
+	if (cam->rot_vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[0],
+				  cam->rot_vf_bufs_vaddr[0],
+				  cam->rot_vf_bufs[0]);
+		cam->rot_vf_bufs_vaddr[0] = NULL;
+		cam->rot_vf_bufs[0] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[1],
+				  cam->rot_vf_bufs_vaddr[1],
+				  cam->rot_vf_bufs[1]);
+		cam->rot_vf_bufs_vaddr[1] = NULL;
+		cam->rot_vf_bufs[1] = 0;
+	}
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0], cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1], cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+	return err;
+}
+
+/*!
+ * prpvf_stop - stop the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_stop(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	if (cam->overlay_active == false)
+		return 0;
+
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_unlink_channels(CSI_PRP_VF_MEM, MEM_ROT_VF_MEM);
+		ipu_unlink_channels(MEM_ROT_VF_MEM, ADC_SYS2);
+
+		ipu_disable_channel(CSI_PRP_VF_MEM, true);
+		ipu_disable_channel(MEM_ROT_VF_MEM, true);
+		ipu_disable_channel(ADC_SYS2, true);
+
+		ipu_uninit_channel(CSI_PRP_VF_MEM);
+		ipu_uninit_channel(MEM_ROT_VF_MEM);
+		ipu_uninit_channel(ADC_SYS2);
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, false, false);
+	}
+#ifndef CONFIG_MXC_IPU_PRP_VF_SDC
+	else if (cam->vf_rotation == IPU_ROTATE_NONE) {
+		ipu_disable_channel(CSI_PRP_VF_ADC, false);
+		ipu_uninit_channel(CSI_PRP_VF_ADC);
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, false, false);
+	}
+#endif
+	else {
+		ipu_unlink_channels(CSI_PRP_VF_MEM, ADC_SYS2);
+
+		ipu_disable_channel(CSI_PRP_VF_MEM, true);
+		ipu_disable_channel(ADC_SYS2, true);
+
+		ipu_uninit_channel(CSI_PRP_VF_MEM);
+		ipu_uninit_channel(ADC_SYS2);
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, false, false);
+	}
+
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0], cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1], cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[0],
+				  cam->rot_vf_bufs_vaddr[0],
+				  cam->rot_vf_bufs[0]);
+		cam->rot_vf_bufs_vaddr[0] = NULL;
+		cam->rot_vf_bufs[0] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[1],
+				  cam->rot_vf_bufs_vaddr[1],
+				  cam->rot_vf_bufs[1]);
+		cam->rot_vf_bufs_vaddr[1] = NULL;
+		cam->rot_vf_bufs[1] = 0;
+	}
+
+	cam->overlay_active = false;
+
+	mxcfb_set_refresh_mode(cam->overlay_fb, MXCFB_REFRESH_PARTIAL, 0);
+	return err;
+}
+
+/*!
+ * function to select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  status
+ */
+int prp_vf_adc_select(void *private)
+{
+	cam_data *cam;
+	if (private) {
+		cam = (cam_data *) private;
+		cam->vf_start_adc = prpvf_start;
+		cam->vf_stop_adc = prpvf_stop;
+		cam->overlay_active = false;
+	} else {
+		return -EIO;
+	}
+	return 0;
+}
+
+/*!
+ * function to de-select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  status
+ */
+int prp_vf_adc_deselect(void *private)
+{
+	cam_data *cam;
+	int err = 0;
+	err = prpvf_stop(private);
+
+	if (private) {
+		cam = (cam_data *) private;
+		cam->vf_start_adc = NULL;
+		cam->vf_stop_adc = NULL;
+	}
+	return err;
+}
+
+/*!
+ * Init viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int prp_vf_adc_init(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+void __exit prp_vf_adc_exit(void)
+{
+}
+
+module_init(prp_vf_adc_init);
+module_exit(prp_vf_adc_exit);
+
+EXPORT_SYMBOL(prp_vf_adc_select);
+EXPORT_SYMBOL(prp_vf_adc_deselect);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IPU PRP VF ADC Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ipu_prp_vf_sdc.c b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc.c
new file mode 100644
index 000000000000..369facdf29c2
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc.c
@@ -0,0 +1,411 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_prp_vf_sdc.c
+ *
+ * @brief IPU Use case for PRP-VF
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/console.h>
+#include <linux/ipu.h>
+#include <linux/mxcfb.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+/*
+ * Function definitions
+ */
+
+/*!
+ * prpvf_start - start the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_start(void *private)
+{
+	struct fb_var_screeninfo fbvar;
+	struct fb_info *fbi = NULL;
+	cam_data *cam = (cam_data *) private;
+	ipu_channel_params_t vf;
+	u32 format = IPU_PIX_FMT_RGB565;
+	u32 size = 2, temp = 0;
+	int err = 0, i = 0;
+
+	if (!cam) {
+		printk(KERN_ERR "private is NULL\n");
+		return -EIO;
+	}
+
+	if (cam->overlay_active == true) {
+		pr_debug("already started.\n");
+		return 0;
+	}
+
+	for (i = 0; i < num_registered_fb; i++) {
+		char *idstr = registered_fb[i]->fix.id;
+		if (strcmp(idstr, "DISP3 FG") == 0)
+			fbi = registered_fb[i];
+	}
+
+	if (fbi == NULL) {
+		printk(KERN_ERR "DISP3 FG fb not found\n");
+		return -EPERM;
+	}
+
+	fbvar = fbi->var;
+	fbvar.bits_per_pixel = 16;
+	fbvar.nonstd = 0;
+	fbvar.xres = fbvar.xres_virtual = cam->win.w.width;
+	fbvar.yres = cam->win.w.height;
+	fbvar.yres_virtual = cam->win.w.height * 2;
+	fbvar.activate |= FB_ACTIVATE_FORCE;
+	fb_set_var(fbi, &fbvar);
+
+	ipu_disp_set_window_pos(MEM_FG_SYNC, cam->win.w.left,
+			cam->win.w.top);
+
+	memset(&vf, 0, sizeof(ipu_channel_params_t));
+	ipu_csi_get_window_size(&vf.csi_prp_vf_mem.in_width,
+				&vf.csi_prp_vf_mem.in_height, cam->csi);
+	vf.csi_prp_vf_mem.in_pixel_fmt = IPU_PIX_FMT_UYVY;
+	vf.csi_prp_vf_mem.out_width = cam->win.w.width;
+	vf.csi_prp_vf_mem.out_height = cam->win.w.height;
+	vf.csi_prp_vf_mem.csi = cam->csi;
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		vf.csi_prp_vf_mem.out_width = cam->win.w.height;
+		vf.csi_prp_vf_mem.out_height = cam->win.w.width;
+	}
+	vf.csi_prp_vf_mem.out_pixel_fmt = format;
+	size = cam->win.w.width * cam->win.w.height * size;
+
+	err = ipu_init_channel(CSI_PRP_VF_MEM, &vf);
+	if (err != 0)
+		goto out_5;
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, true, true);
+
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0],
+				  (dma_addr_t) cam->vf_bufs[0]);
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1],
+				  (dma_addr_t) cam->vf_bufs[1]);
+	}
+	cam->vf_bufs_size[0] = PAGE_ALIGN(size);
+	cam->vf_bufs_vaddr[0] = (void *)dma_alloc_coherent(0,
+							   cam->vf_bufs_size[0],
+							   (dma_addr_t *) &
+							   cam->vf_bufs[0],
+							   GFP_DMA |
+							   GFP_KERNEL);
+	if (cam->vf_bufs_vaddr[0] == NULL) {
+		printk(KERN_ERR "Error to allocate vf buffer\n");
+		err = -ENOMEM;
+		goto out_4;
+	}
+	cam->vf_bufs_size[1] = PAGE_ALIGN(size);
+	cam->vf_bufs_vaddr[1] = (void *)dma_alloc_coherent(0,
+							   cam->vf_bufs_size[1],
+							   (dma_addr_t *) &
+							   cam->vf_bufs[1],
+							   GFP_DMA |
+							   GFP_KERNEL);
+	if (cam->vf_bufs_vaddr[1] == NULL) {
+		printk(KERN_ERR "Error to allocate vf buffer\n");
+		err = -ENOMEM;
+		goto out_3;
+	}
+	pr_debug("vf_bufs %x %x\n", cam->vf_bufs[0], cam->vf_bufs[1]);
+
+	if (cam->vf_rotation >= IPU_ROTATE_VERT_FLIP) {
+		err = ipu_init_channel_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      IPU_ROTATE_NONE, cam->vf_bufs[0],
+					      cam->vf_bufs[1], 0, 0);
+		if (err != 0) {
+			goto out_3;
+		}
+
+		err = ipu_init_channel(MEM_ROT_VF_MEM, NULL);
+		if (err != 0) {
+			printk(KERN_ERR "Error MEM_ROT_VF_MEM channel\n");
+			goto out_3;
+		}
+
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_INPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      cam->vf_rotation, cam->vf_bufs[0],
+					      cam->vf_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "Error MEM_ROT_VF_MEM input buffer\n");
+			goto out_2;
+		}
+
+		if (cam->vf_rotation < IPU_ROTATE_90_RIGHT) {
+			temp = vf.csi_prp_vf_mem.out_width;
+			vf.csi_prp_vf_mem.out_width =
+						vf.csi_prp_vf_mem.out_height;
+			vf.csi_prp_vf_mem.out_height = temp;
+		}
+
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      IPU_ROTATE_NONE,
+					      fbi->fix.smem_start +
+					      (fbi->fix.line_length *
+					       fbi->var.yres),
+					      fbi->fix.smem_start, 0, 0);
+
+		if (err != 0) {
+			printk(KERN_ERR "Error MEM_ROT_VF_MEM output buffer\n");
+			goto out_2;
+		}
+
+		err = ipu_link_channels(CSI_PRP_VF_MEM, MEM_ROT_VF_MEM);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "Error link CSI_PRP_VF_MEM-MEM_ROT_VF_MEM\n");
+			goto out_2;
+		}
+
+		err = ipu_link_channels(MEM_ROT_VF_MEM, MEM_FG_SYNC);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "Error link MEM_ROT_VF_MEM-MEM_FG_SYNC\n");
+			goto out_1;
+		}
+
+		ipu_enable_channel(CSI_PRP_VF_MEM);
+		ipu_enable_channel(MEM_ROT_VF_MEM);
+
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+	} else {
+		err = ipu_init_channel_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format, cam->win.w.width,
+					      cam->win.w.height,
+					      cam->win.w.width,
+					      cam->vf_rotation,
+					      fbi->fix.smem_start +
+					      (fbi->fix.line_length *
+					       fbi->var.yres),
+					      fbi->fix.smem_start, 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "Error initializing CSI_PRP_VF_MEM\n");
+			goto out_4;
+		}
+
+		err = ipu_link_channels(CSI_PRP_VF_MEM, MEM_FG_SYNC);
+		if (err < 0) {
+			printk(KERN_ERR "Error linking ipu channels\n");
+			goto out_4;
+		}
+
+		ipu_enable_channel(CSI_PRP_VF_MEM);
+
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+	}
+
+	acquire_console_sem();
+	fb_blank(fbi, FB_BLANK_UNBLANK);
+	release_console_sem();
+
+	cam->overlay_active = true;
+	return err;
+
+out_1:
+	ipu_unlink_channels(CSI_PRP_VF_MEM, MEM_ROT_VF_MEM);
+out_2:
+	if (cam->vf_rotation >= IPU_ROTATE_VERT_FLIP) {
+		ipu_uninit_channel(MEM_ROT_VF_MEM);
+	}
+out_3:
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0],
+				  (dma_addr_t) cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1],
+				  (dma_addr_t) cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+out_4:
+	ipu_uninit_channel(CSI_PRP_VF_MEM);
+out_5:
+	return err;
+}
+
+/*!
+ * prpvf_stop - stop the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_stop(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0, i = 0;
+	struct fb_info *fbi = NULL;
+
+	if (cam->overlay_active == false)
+		return 0;
+
+	for (i = 0; i < num_registered_fb; i++) {
+		char *idstr = registered_fb[i]->fix.id;
+		if (strcmp(idstr, "DISP3 FG") == 0)
+			fbi = registered_fb[i];
+	}
+
+	if (fbi == NULL) {
+		printk(KERN_ERR "DISP3 FG fb not found\n");
+		return -EPERM;
+	}
+
+	ipu_disp_set_window_pos(MEM_FG_SYNC, 0, 0);
+
+	if (cam->vf_rotation >= IPU_ROTATE_VERT_FLIP) {
+		ipu_unlink_channels(CSI_PRP_VF_MEM, MEM_ROT_VF_MEM);
+		ipu_unlink_channels(MEM_ROT_VF_MEM, MEM_FG_SYNC);
+	} else {
+		ipu_unlink_channels(CSI_PRP_VF_MEM, MEM_FG_SYNC);
+	}
+
+	ipu_disable_channel(CSI_PRP_VF_MEM, true);
+
+	if (cam->vf_rotation >= IPU_ROTATE_VERT_FLIP) {
+		ipu_disable_channel(MEM_ROT_VF_MEM, true);
+		ipu_uninit_channel(MEM_ROT_VF_MEM);
+	}
+	ipu_uninit_channel(CSI_PRP_VF_MEM);
+
+	acquire_console_sem();
+	fb_blank(fbi, FB_BLANK_POWERDOWN);
+	release_console_sem();
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, false, false);
+
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0],
+				  (dma_addr_t) cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1],
+				  (dma_addr_t) cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+
+	cam->overlay_active = false;
+	return err;
+}
+
+/*!
+ * function to select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  status
+ */
+int prp_vf_sdc_select(void *private)
+{
+	cam_data *cam;
+	int err = 0;
+	if (private) {
+		cam = (cam_data *) private;
+		cam->vf_start_sdc = prpvf_start;
+		cam->vf_stop_sdc = prpvf_stop;
+		cam->overlay_active = false;
+	} else
+		err = -EIO;
+
+	return err;
+}
+
+/*!
+ * function to de-select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  int
+ */
+int prp_vf_sdc_deselect(void *private)
+{
+	cam_data *cam;
+	int err = 0;
+	err = prpvf_stop(private);
+
+	if (private) {
+		cam = (cam_data *) private;
+		cam->vf_start_sdc = NULL;
+		cam->vf_stop_sdc = NULL;
+	}
+	return err;
+}
+
+/*!
+ * Init viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int prp_vf_sdc_init(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+void __exit prp_vf_sdc_exit(void)
+{
+}
+
+module_init(prp_vf_sdc_init);
+module_exit(prp_vf_sdc_exit);
+
+EXPORT_SYMBOL(prp_vf_sdc_select);
+EXPORT_SYMBOL(prp_vf_sdc_deselect);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IPU PRP VF SDC Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c
new file mode 100644
index 000000000000..4035c7664022
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c
@@ -0,0 +1,413 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_prp_vf_sdc_bg.c
+ *
+ * @brief IPU Use case for PRP-VF back-ground
+ *
+ * @ingroup IPU
+ */
+#include <linux/dma-mapping.h>
+#include <linux/fb.h>
+#include <linux/ipu.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+static int buffer_num = 0;
+static int buffer_ready = 0;
+
+/*
+ * Function definitions
+ */
+
+/*!
+ * SDC V-Sync callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t prpvf_sdc_vsync_callback(int irq, void *dev_id)
+{
+	pr_debug("buffer_ready %d buffer_num %d\n", buffer_ready, buffer_num);
+	if (buffer_ready > 0) {
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		buffer_ready--;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * VF EOF callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t prpvf_vf_eof_callback(int irq, void *dev_id)
+{
+	pr_debug("buffer_ready %d buffer_num %d\n", buffer_ready, buffer_num);
+
+	ipu_select_buffer(MEM_ROT_VF_MEM, IPU_INPUT_BUFFER, buffer_num);
+
+	buffer_num = (buffer_num == 0) ? 1 : 0;
+
+	ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, buffer_num);
+	buffer_ready++;
+	return IRQ_HANDLED;
+}
+
+/*!
+ * prpvf_start - start the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_start(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	ipu_channel_params_t vf;
+	u32 format;
+	u32 offset;
+	u32 bpp, size = 3;
+	int err = 0;
+
+	if (!cam) {
+		printk(KERN_ERR "private is NULL\n");
+		return -EIO;
+	}
+
+	if (cam->overlay_active == true) {
+		pr_debug("already start.\n");
+		return 0;
+	}
+
+	format = cam->v4l2_fb.fmt.pixelformat;
+	if (cam->v4l2_fb.fmt.pixelformat == IPU_PIX_FMT_BGR24) {
+		bpp = 3, size = 3;
+		pr_info("BGR24\n");
+	} else if (cam->v4l2_fb.fmt.pixelformat == IPU_PIX_FMT_RGB565) {
+		bpp = 2, size = 2;
+		pr_info("RGB565\n");
+	} else if (cam->v4l2_fb.fmt.pixelformat == IPU_PIX_FMT_BGR32) {
+		bpp = 4, size = 4;
+		pr_info("BGR32\n");
+	} else {
+		printk(KERN_ERR
+		       "unsupported fix format from the framebuffer.\n");
+		return -EINVAL;
+	}
+
+	offset = cam->v4l2_fb.fmt.bytesperline * cam->win.w.top +
+	    size * cam->win.w.left;
+
+	if (cam->v4l2_fb.base == 0) {
+		printk(KERN_ERR "invalid frame buffer address.\n");
+	} else {
+		offset += (u32) cam->v4l2_fb.base;
+	}
+
+	memset(&vf, 0, sizeof(ipu_channel_params_t));
+	ipu_csi_get_window_size(&vf.csi_prp_vf_mem.in_width,
+				&vf.csi_prp_vf_mem.in_height, cam->csi);
+	vf.csi_prp_vf_mem.in_pixel_fmt = IPU_PIX_FMT_UYVY;
+	vf.csi_prp_vf_mem.out_width = cam->win.w.width;
+	vf.csi_prp_vf_mem.out_height = cam->win.w.height;
+	vf.csi_prp_vf_mem.csi = cam->csi;
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		vf.csi_prp_vf_mem.out_width = cam->win.w.height;
+		vf.csi_prp_vf_mem.out_height = cam->win.w.width;
+	}
+	vf.csi_prp_vf_mem.out_pixel_fmt = format;
+	size = cam->win.w.width * cam->win.w.height * size;
+
+	err = ipu_init_channel(CSI_PRP_VF_MEM, &vf);
+	if (err != 0)
+		goto out_4;
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, true, true);
+
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0], cam->vf_bufs[0]);
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1], cam->vf_bufs[1]);
+	}
+	cam->vf_bufs_size[0] = PAGE_ALIGN(size);
+	cam->vf_bufs_vaddr[0] = (void *)dma_alloc_coherent(0,
+							   cam->vf_bufs_size[0],
+							   &cam->vf_bufs[0],
+							   GFP_DMA |
+							   GFP_KERNEL);
+	if (cam->vf_bufs_vaddr[0] == NULL) {
+		printk(KERN_ERR "Error to allocate vf buffer\n");
+		err = -ENOMEM;
+		goto out_3;
+	}
+	cam->vf_bufs_size[1] = PAGE_ALIGN(size);
+	cam->vf_bufs_vaddr[1] = (void *)dma_alloc_coherent(0,
+							   cam->vf_bufs_size[1],
+							   &cam->vf_bufs[1],
+							   GFP_DMA |
+							   GFP_KERNEL);
+	if (cam->vf_bufs_vaddr[1] == NULL) {
+		printk(KERN_ERR "Error to allocate vf buffer\n");
+		err = -ENOMEM;
+		goto out_3;
+	}
+
+	err = ipu_init_channel_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER,
+				      format, vf.csi_prp_vf_mem.out_width,
+				      vf.csi_prp_vf_mem.out_height,
+				      vf.csi_prp_vf_mem.out_width,
+				      IPU_ROTATE_NONE, cam->vf_bufs[0],
+				      cam->vf_bufs[1], 0, 0);
+	if (err != 0) {
+		printk(KERN_ERR "Error initializing CSI_PRP_VF_MEM\n");
+		goto out_3;
+	}
+	err = ipu_init_channel(MEM_ROT_VF_MEM, NULL);
+	if (err != 0) {
+		printk(KERN_ERR "Error MEM_ROT_VF_MEM channel\n");
+		goto out_3;
+	}
+
+	err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_INPUT_BUFFER,
+				      format, vf.csi_prp_vf_mem.out_width,
+				      vf.csi_prp_vf_mem.out_height,
+				      vf.csi_prp_vf_mem.out_width,
+				      cam->vf_rotation, cam->vf_bufs[0],
+				      cam->vf_bufs[1], 0, 0);
+	if (err != 0) {
+		printk(KERN_ERR "Error MEM_ROT_VF_MEM input buffer\n");
+		goto out_2;
+	}
+
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      cam->overlay_fb->var.xres * bpp,
+					      IPU_ROTATE_NONE, offset, 0, 0, 0);
+
+		if (err != 0) {
+			printk(KERN_ERR "Error MEM_ROT_VF_MEM output buffer\n");
+			goto out_2;
+		}
+	} else {
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      cam->overlay_fb->var.xres * bpp,
+					      IPU_ROTATE_NONE, offset, 0, 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "Error MEM_ROT_VF_MEM output buffer\n");
+			goto out_2;
+		}
+	}
+
+	ipu_clear_irq(IPU_IRQ_PRP_VF_OUT_EOF);
+	err = ipu_request_irq(IPU_IRQ_PRP_VF_OUT_EOF, prpvf_vf_eof_callback,
+			      0, "Mxc Camera", cam);
+	if (err != 0) {
+		printk(KERN_ERR
+		       "Error registering IPU_IRQ_PRP_VF_OUT_EOF irq.\n");
+		goto out_2;
+	}
+
+	ipu_clear_irq(IPU_IRQ_BG_SF_END);
+	err = ipu_request_irq(IPU_IRQ_BG_SF_END, prpvf_sdc_vsync_callback,
+			      0, "Mxc Camera", NULL);
+	if (err != 0) {
+		printk(KERN_ERR "Error registering IPU_IRQ_BG_SF_END irq.\n");
+		goto out_1;
+	}
+
+	ipu_enable_channel(CSI_PRP_VF_MEM);
+	ipu_enable_channel(MEM_ROT_VF_MEM);
+
+	buffer_num = 0;
+	buffer_ready = 0;
+	ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+
+	cam->overlay_active = true;
+	return err;
+
+      out_1:
+	ipu_free_irq(IPU_IRQ_PRP_VF_OUT_EOF, NULL);
+      out_2:
+	ipu_uninit_channel(MEM_ROT_VF_MEM);
+      out_3:
+	ipu_uninit_channel(CSI_PRP_VF_MEM);
+      out_4:
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0], cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1], cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[0],
+				  cam->rot_vf_bufs_vaddr[0],
+				  cam->rot_vf_bufs[0]);
+		cam->rot_vf_bufs_vaddr[0] = NULL;
+		cam->rot_vf_bufs[0] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[1],
+				  cam->rot_vf_bufs_vaddr[1],
+				  cam->rot_vf_bufs[1]);
+		cam->rot_vf_bufs_vaddr[1] = NULL;
+		cam->rot_vf_bufs[1] = 0;
+	}
+	return err;
+}
+
+/*!
+ * prpvf_stop - stop the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_stop(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (cam->overlay_active == false)
+		return 0;
+
+	ipu_free_irq(IPU_IRQ_BG_SF_END, NULL);
+
+	ipu_free_irq(IPU_IRQ_PRP_VF_OUT_EOF, cam);
+
+	ipu_disable_channel(CSI_PRP_VF_MEM, true);
+	ipu_disable_channel(MEM_ROT_VF_MEM, true);
+	ipu_uninit_channel(CSI_PRP_VF_MEM);
+	ipu_uninit_channel(MEM_ROT_VF_MEM);
+	ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, false, false);
+
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0], cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1], cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[0],
+				  cam->rot_vf_bufs_vaddr[0],
+				  cam->rot_vf_bufs[0]);
+		cam->rot_vf_bufs_vaddr[0] = NULL;
+		cam->rot_vf_bufs[0] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[1],
+				  cam->rot_vf_bufs_vaddr[1],
+				  cam->rot_vf_bufs[1]);
+		cam->rot_vf_bufs_vaddr[1] = NULL;
+		cam->rot_vf_bufs[1] = 0;
+	}
+
+	buffer_num = 0;
+	buffer_ready = 0;
+	cam->overlay_active = false;
+	return 0;
+}
+
+/*!
+ * function to select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  status
+ */
+int prp_vf_sdc_select_bg(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (cam) {
+		cam->vf_start_sdc = prpvf_start;
+		cam->vf_stop_sdc = prpvf_stop;
+		cam->overlay_active = false;
+	}
+
+	return 0;
+}
+
+/*!
+ * function to de-select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  status
+ */
+int prp_vf_sdc_deselect_bg(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+	err = prpvf_stop(private);
+
+	if (cam) {
+		cam->vf_start_sdc = NULL;
+		cam->vf_stop_sdc = NULL;
+	}
+	return err;
+}
+
+/*!
+ * Init viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int prp_vf_sdc_init_bg(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+void __exit prp_vf_sdc_exit_bg(void)
+{
+}
+
+module_init(prp_vf_sdc_init_bg);
+module_exit(prp_vf_sdc_exit_bg);
+
+EXPORT_SYMBOL(prp_vf_sdc_select_bg);
+EXPORT_SYMBOL(prp_vf_sdc_deselect_bg);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IPU PRP VF SDC Backgroud Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ipu_still.c b/drivers/media/video/mxc/capture/ipu_still.c
new file mode 100644
index 000000000000..34cea8609c95
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_still.c
@@ -0,0 +1,252 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_still.c
+ *
+ * @brief IPU Use case for still image capture
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/ipu.h>
+#include <linux/semaphore.h>
+#include <linux/ipu.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+static int callback_eof_flag;
+
+#ifdef CONFIG_MXC_IPU_V1
+static int callback_flag;
+/*
+ * Function definitions
+ */
+/*!
+ * CSI EOF callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t prp_csi_eof_callback(int irq, void *dev_id)
+{
+	if (callback_flag == 2) {
+		ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_enable_channel(CSI_MEM);
+	}
+
+	callback_flag++;
+	return IRQ_HANDLED;
+}
+#endif
+
+/*!
+ * CSI callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t prp_still_callback(int irq, void *dev_id)
+{
+	cam_data *cam = (cam_data *) dev_id;
+
+	callback_eof_flag++;
+	if (callback_eof_flag < 5)
+		ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
+	else {
+		cam->still_counter++;
+		wake_up_interruptible(&cam->still_queue);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * start csi->mem task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int prp_still_start(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	u32 pixel_fmt;
+	int err;
+	ipu_channel_params_t params;
+
+	if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
+		pixel_fmt = IPU_PIX_FMT_YUV420P;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P)
+		pixel_fmt = IPU_PIX_FMT_YUV422P;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY)
+		pixel_fmt = IPU_PIX_FMT_UYVY;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR24)
+		pixel_fmt = IPU_PIX_FMT_BGR24;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24)
+		pixel_fmt = IPU_PIX_FMT_RGB24;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB565)
+		pixel_fmt = IPU_PIX_FMT_RGB565;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR32)
+		pixel_fmt = IPU_PIX_FMT_BGR32;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB32)
+		pixel_fmt = IPU_PIX_FMT_RGB32;
+	else {
+		printk(KERN_ERR "format not supported\n");
+		return -EINVAL;
+	}
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_RAW, cam->csi, true, true);
+
+	memset(&params, 0, sizeof(params));
+	err = ipu_init_channel(CSI_MEM, &params);
+	if (err != 0)
+		return err;
+
+	err = ipu_init_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+				      pixel_fmt, cam->v2f.fmt.pix.width,
+				      cam->v2f.fmt.pix.height,
+				      cam->v2f.fmt.pix.width, IPU_ROTATE_NONE,
+				      cam->still_buf, 0, 0, 0);
+	if (err != 0)
+		return err;
+
+#ifdef CONFIG_MXC_IPU_V1
+	err = ipu_request_irq(IPU_IRQ_SENSOR_OUT_EOF, prp_still_callback,
+			      0, "Mxc Camera", cam);
+	if (err != 0) {
+		printk(KERN_ERR "Error registering irq.\n");
+		return err;
+	}
+	callback_flag = 0;
+	callback_eof_flag = 0;
+	err = ipu_request_irq(IPU_IRQ_SENSOR_EOF, prp_csi_eof_callback,
+			      0, "Mxc Camera", NULL);
+	if (err != 0) {
+		printk(KERN_ERR "Error IPU_IRQ_SENSOR_EOF \n");
+		return err;
+	}
+#else
+	ipu_clear_irq(IPU_IRQ_CSI0_OUT_EOF);
+	err = ipu_request_irq(IPU_IRQ_CSI0_OUT_EOF, prp_still_callback,
+			      0, "Mxc Camera", cam);
+	if (err != 0) {
+		printk(KERN_ERR "Error registering irq.\n");
+		return err;
+	}
+
+	callback_eof_flag = 0;
+
+	ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
+	ipu_enable_channel(CSI_MEM);
+#endif
+
+	return err;
+}
+
+/*!
+ * stop csi->mem encoder task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int prp_still_stop(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+#ifdef CONFIG_MXC_IPU_V1
+	ipu_free_irq(IPU_IRQ_SENSOR_EOF, NULL);
+	ipu_free_irq(IPU_IRQ_SENSOR_OUT_EOF, cam);
+#else
+	ipu_free_irq(IPU_IRQ_CSI0_OUT_EOF, cam);
+#endif
+
+	ipu_disable_channel(CSI_MEM, true);
+	ipu_uninit_channel(CSI_MEM);
+	ipu_csi_enable_mclk_if(CSI_MCLK_RAW, cam->csi, false, false);
+
+	return err;
+}
+
+/*!
+ * function to select CSI_MEM as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+int prp_still_select(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (cam) {
+		cam->csi_start = prp_still_start;
+		cam->csi_stop = prp_still_stop;
+	}
+
+	return 0;
+}
+
+/*!
+ * function to de-select CSI_MEM as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+int prp_still_deselect(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	err = prp_still_stop(cam);
+
+	if (cam) {
+		cam->csi_start = NULL;
+		cam->csi_stop = NULL;
+	}
+
+	return err;
+}
+
+/*!
+ * Init the Encorder channels
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int prp_still_init(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit the Encorder channels
+ *
+ */
+void __exit prp_still_exit(void)
+{
+}
+
+module_init(prp_still_init);
+module_exit(prp_still_exit);
+
+EXPORT_SYMBOL(prp_still_select);
+EXPORT_SYMBOL(prp_still_deselect);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IPU PRP STILL IMAGE Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/mc521da.c b/drivers/media/video/mxc/capture/mc521da.c
new file mode 100644
index 000000000000..a8ff84fb4c84
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mc521da.c
@@ -0,0 +1,648 @@
+/*
+ * Copyright 2006-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc521da.c
+ *
+ * @brief MC521DA camera driver functions
+ *
+ * @ingroup Camera
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/clk.h>
+#include "mxc_v4l2_capture.h"
+
+#define MC521DA_I2C_ADDRESS	0x22
+#define MC521DA_TERM		0xFF
+
+typedef struct {
+	u16 width;
+	u16 height;
+} mc521da_image_format;
+
+struct mc521da_reg {
+	u8 reg;
+	u8 val;
+};
+
+static sensor_interface *interface_param = NULL;
+
+static mc521da_image_format format[2] = {
+	{
+	 .width = 1600,
+	 .height = 1200,
+	 },
+	{
+	 .width = 640,
+	 .height = 480,
+	 },
+};
+
+const static struct mc521da_reg mc521da_initial[] = {
+	/*----------------------------------------------------------
+	 * Sensor Setting Start
+	 *----------------------------------------------------------
+	 */
+	{0xff, 0x01},		/* Sensor setting start */
+	{0x01, 0x10},		/* Wavetable script, generated by waveman */
+	{0x10, 0x64},
+	{0x03, 0x00}, {0x04, 0x06}, {0x05, 0x30}, {0x06, 0x02}, {0x08, 0x00},
+	{0x03, 0x01}, {0x04, 0x41}, {0x05, 0x70}, {0x06, 0x03}, {0x08, 0x00},
+	{0x03, 0x02}, {0x04, 0x55}, {0x05, 0x30}, {0x06, 0x03}, {0x08, 0x00},
+	{0x03, 0x03}, {0x04, 0x5A}, {0x05, 0x30}, {0x06, 0x02}, {0x08, 0x00},
+	{0x03, 0x04}, {0x04, 0x7A}, {0x05, 0x30}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x05}, {0x04, 0x9C}, {0x05, 0x30}, {0x06, 0x0F}, {0x08, 0x00},
+	{0x03, 0x06}, {0x04, 0x73}, {0x05, 0x31}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x07}, {0x04, 0x2D}, {0x05, 0x3B}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x08}, {0x04, 0x32}, {0x05, 0x33}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x09}, {0x04, 0x67}, {0x05, 0x63}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x0a}, {0x04, 0x6C}, {0x05, 0x23}, {0x06, 0x0E}, {0x08, 0x00},
+	{0x03, 0x0b}, {0x04, 0x71}, {0x05, 0x23}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x0c}, {0x04, 0x30}, {0x05, 0x2F}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x0d}, {0x04, 0x00}, {0x05, 0x00}, {0x06, 0x06}, {0x08, 0x00},
+	{0x07, 0x0e},
+
+	/* Start Address */
+	{0x10, 0x64}, {0x14, 0x10}, {0x15, 0x00},
+
+	/* SYNC */
+	{0x18, 0x40}, {0x19, 0x00}, {0x1A, 0x03}, {0x1B, 0x00},
+
+	/* X-Y Mirror */
+	{0x11, 0x00}, {0xda, 0x00},	/* X mirror OFF, Y Mirror OFF */
+
+	/* Frame height */
+	{0x1c, 0x13}, {0x1d, 0x04}, {0x0e, 0x4b}, {0x0f, 0x05},
+	{0x9e, 0x04}, {0x9d, 0xc6}, {0xcc, 0x14}, {0xcd, 0x05},
+
+	/* Frame width */
+	{0x0c, 0x35}, {0x0d, 0x07}, {0x9b, 0x10}, {0x9c, 0x07},
+	{0x93, 0x21},
+
+	{0x01, 0x01}, {0x40, 0x00}, {0x41, 0x00}, {0x42, 0xf0},
+	{0x43, 0x03}, {0x44, 0x0a}, {0x45, 0x00}, {0x3b, 0x40},
+	{0x38, 0x18}, {0x3c, 0x00}, {0x20, 0x00}, {0x21, 0x01},
+	{0x22, 0x00}, {0x23, 0x01}, {0x24, 0x00}, {0x25, 0x01},
+	{0x26, 0x00}, {0x27, 0x01}, {0xb9, 0x04}, {0xb8, 0xc3},
+	{0xbb, 0x04}, {0xba, 0xc3}, {0xbf, 0x04}, {0xbe, 0xc3},
+
+	/* Ramp */
+	{0x57, 0x07}, {0x56, 0xd6}, {0x55, 0x03}, {0x54, 0x74},
+	{0x9f, 0x99}, {0x94, 0x80}, {0x91, 0x78}, {0x92, 0x8b},
+
+	/* Output Mode */
+	{0x52, 0x10}, {0x51, 0x00},
+
+	/* Analog Gain and Output driver */
+	{0x28, 0x00}, {0xdd, 0x82}, {0xdb, 0x00}, {0xdc, 0x00},
+
+	/* Update */
+	{0x00, 0x84},
+
+	/* PLL  ADC clock = 75 MHz */
+	{0xb5, 0x60}, {0xb4, 0x02}, {0xb5, 0x20},
+
+	/*----------------------------------------------*/
+	/*      ISP Setting Start                       */
+	/*----------------------------------------------*/
+	{0xff, 0x02},
+	{0x01, 0xbd}, {0x02, 0xf8}, {0x03, 0x3a}, {0x04, 0x00}, {0x0e, 0x00},
+
+	/* Output mode */
+	{0x88, 0x00}, {0x87, 0x11},
+
+	/* Threshold */
+	{0xb6, 0x1b}, {0x0d, 0xc0}, {0x24, 0x00}, {0x25, 0x00}, {0x26, 0x00},
+
+	/* Image Effect */
+	{0x3f, 0x80}, {0x40, 0x00}, {0x41, 0x00}, {0x42, 0x80}, {0x43, 0x00},
+	{0x44, 0x00}, {0x45, 0x00}, {0x46, 0x00}, {0x56, 0x80}, {0x57, 0x20},
+	{0x58, 0x20}, {0x59, 0x02}, {0x5a, 0x00}, {0x5b, 0x78}, {0x5c, 0x7c},
+	{0x5d, 0x84}, {0x5e, 0x85}, {0x5f, 0x78}, {0x60, 0x7e}, {0x61, 0x82},
+	{0x62, 0x85}, {0x63, 0x00}, {0x64, 0x80}, {0x65, 0x00}, {0x66, 0x80},
+	{0x67, 0x80}, {0x68, 0x80},
+
+	/* Auto Focus */
+	{0x6e, 0x02}, {0x6f, 0xe5}, {0x70, 0x08}, {0x71, 0x01}, {0x72, 0x00},
+
+	/* Decimator */
+	{0x78, 0xff}, {0x79, 0xff}, {0x7a, 0x70}, {0x7b, 0x00}, {0x7c, 0x00},
+	{0x7d, 0x00}, {0x7e, 0xc8}, {0x7f, 0xc8}, {0x80, 0x96}, {0x81, 0x96},
+	{0x82, 0x00}, {0x83, 0x00}, {0x84, 0x00}, {0x85, 0x00}, {0x86, 0x00},
+
+	/* Luminance Info */
+	{0xf9, 0x20}, {0xb7, 0x7f}, {0xb8, 0x28}, {0xb9, 0x08},
+	{0xf9, 0xa0}, {0xb7, 0x10}, {0xb9, 0x00},
+	{0xf9, 0x40}, {0xb7, 0x7f}, {0xb8, 0x28}, {0xb9, 0x08},
+	{0xf9, 0xc0}, {0xb7, 0x08}, {0xb9, 0x00},
+	{0xf9, 0x60}, {0xb7, 0x7f}, {0xb8, 0x28}, {0xb9, 0x08},
+	{0xf9, 0xe0}, {0xb7, 0x05}, {0xb9, 0x00},
+	{0xf9, 0x00}, {0xb7, 0x03}, {0xb8, 0x2d}, {0xb9, 0xcd},
+	{0xf9, 0x80}, {0xb7, 0x02}, {0xb9, 0x00},
+
+	/* AE */
+	{0x8a, 0x00}, {0x89, 0xc0}, {0x8c, 0x32}, {0x8d, 0x96}, {0x8e, 0x25},
+	{0x8f, 0x70}, {0x90, 0x12}, {0x91, 0x41}, {0x9e, 0x2e}, {0x9f, 0x2e},
+	{0xa0, 0x0b}, {0xa1, 0x71}, {0xa2, 0xb0}, {0xa3, 0x09}, {0xa4, 0x89},
+	{0xa5, 0x68}, {0xa6, 0x1a}, {0xa7, 0xb3}, {0xa8, 0xf0}, {0xa9, 0x19},
+	{0xaa, 0x6a}, {0xab, 0x6b}, {0xac, 0x01}, {0xad, 0xe8}, {0xae, 0x48},
+	{0xaf, 0x01}, {0xb0, 0x96}, {0xb1, 0xe6}, {0xb2, 0x03}, {0xb3, 0x00},
+	{0xb4, 0x10}, {0xb5, 0x00}, {0xb6, 0x04}, {0xba, 0x44}, {0xbb, 0x3a},
+	{0xbc, 0x01}, {0xbd, 0x08}, {0xbe, 0xa0}, {0xbf, 0x01}, {0xc0, 0x82},
+	{0x8a, 0xe1}, {0x8b, 0x8c},
+
+	/* AWB */
+	{0xc8, 0x00}, {0xc9, 0x00}, {0xca, 0x40}, {0xcb, 0xB0}, {0xcc, 0x40},
+	{0xcd, 0xff}, {0xce, 0x19}, {0xcf, 0x40}, {0xd0, 0x01}, {0xd1, 0x43},
+	{0xd2, 0x80}, {0xd3, 0x80}, {0xd4, 0xf1}, {0xdf, 0x00}, {0xe0, 0x8f},
+	{0xe1, 0x8f}, {0xe2, 0x53}, {0xe3, 0x97}, {0xe4, 0x1f}, {0xe5, 0x3b},
+	{0xe6, 0x9c}, {0xe7, 0x2e}, {0xe8, 0x03}, {0xe9, 0x02},
+
+	/* Neutral CCM */
+	{0xfa, 0x00}, {0xd5, 0x3f}, {0xd6, 0x8c}, {0xd7, 0x43}, {0xd8, 0x08},
+	{0xd9, 0x27}, {0xda, 0x7e}, {0xdb, 0x17}, {0xdc, 0x1a}, {0xdd, 0x47},
+	{0xde, 0xa1},
+
+	/* Blue CCM */
+	{0xfa, 0x01}, {0xd5, 0x3f}, {0xd6, 0x77}, {0xd7, 0x34}, {0xd8, 0x03},
+	{0xd9, 0x18}, {0xda, 0x6e}, {0xdb, 0x16}, {0xdc, 0x0f}, {0xdd, 0x29},
+	{0xde, 0x77},
+
+	/* Red CCM */
+	{0xfa, 0x02}, {0xd5, 0x3f}, {0xd6, 0x7d}, {0xd7, 0x2f}, {0xd8, 0x0e},
+	{0xd9, 0x1e}, {0xda, 0x76}, {0xdb, 0x18}, {0xdc, 0x29}, {0xdd, 0x51},
+	{0xde, 0xba},
+
+	/* AWB */
+	{0xea, 0x00}, {0xeb, 0x1a}, {0xc8, 0x33}, {0xc9, 0xc2},
+
+	{0xed, 0x02}, {0xee, 0x02},
+
+	/* AFD */
+	{0xf0, 0x11}, {0xf1, 0x03}, {0xf2, 0x05}, {0xf5, 0x05}, {0xf6, 0x32},
+	{0xf7, 0x32},
+
+	/* Lens Shading */
+	{0xf9, 0x00}, {0x05, 0x04}, {0x06, 0xff}, {0x07, 0xf2}, {0x08, 0x00},
+	{0x09, 0x00}, {0x0a, 0xf2}, {0x0b, 0xff}, {0x0c, 0xff},
+	{0xf9, 0x01}, {0x05, 0x04}, {0x06, 0xff}, {0x07, 0x8b}, {0x08, 0x16},
+	{0x09, 0x16}, {0x0a, 0x8b}, {0x0b, 0xff}, {0x0c, 0xe0},
+	{0xf9, 0x02}, {0x05, 0x04}, {0x06, 0xff}, {0x07, 0x8b}, {0x08, 0x16},
+	{0x09, 0x16}, {0x0a, 0x8b}, {0x0b, 0xff}, {0x0c, 0xe0},
+	{0xf9, 0x03}, {0x05, 0x04}, {0x06, 0xff}, {0x07, 0x7c}, {0x08, 0x26},
+	{0x09, 0x26}, {0x0a, 0x7c}, {0x0b, 0xd0}, {0x0c, 0xe0},
+	{0xf9, 0x04}, {0x05, 0x0d}, {0x06, 0x40}, {0x07, 0xa0}, {0x08, 0x00},
+	{0x09, 0x00}, {0x0a, 0xa0}, {0x0b, 0x40}, {0x0c, 0xe0},
+	{0xf9, 0x05}, {0x05, 0x0d}, {0x06, 0x40}, {0x07, 0xa0}, {0x08, 0x00},
+	{0x09, 0x00}, {0x0a, 0xa0}, {0x0b, 0x40}, {0x0c, 0xa0},
+	{0xf9, 0x06}, {0x05, 0x0d}, {0x06, 0x40}, {0x07, 0xa0}, {0x08, 0x00},
+	{0x09, 0x00}, {0x0a, 0xa0}, {0x0b, 0x40}, {0x0c, 0xa0},
+	{0xf9, 0x07}, {0x05, 0x0d}, {0x06, 0x40}, {0x07, 0xa0}, {0x08, 0x00},
+	{0x09, 0x00}, {0x0a, 0xa0}, {0x0b, 0x40}, {0x0c, 0xa0},
+
+	/* Edge setting */
+	{0x73, 0x68}, {0x74, 0x40}, {0x75, 0x00}, {0x76, 0xff}, {0x77, 0x80},
+	{0x4f, 0x80}, {0x50, 0x82}, {0x51, 0x82}, {0x52, 0x08},
+
+	/* Interpolation Setting */
+	{0x23, 0x7f}, {0x22, 0x08}, {0x18, 0xff}, {0x19, 0x00},
+	{0x40, 0x00}, {0x53, 0xff}, {0x54, 0x0a}, {0x55, 0xc2},
+	{0x1b, 0x18},
+
+	{0xfa, 0x00}, {0x15, 0x0c}, {0x22, 0x00}, {0x0e, 0xef}, {0x1f, 0x1d},
+	{0x20, 0x2d}, {0x1c, 0x01}, {0x1d, 0x02}, {0x1e, 0x03}, {0x0e, 0xee},
+	{0x12, 0x10}, {0x16, 0x10}, {0x17, 0x02}, {0x1a, 0x01},
+	{0xfa, 0x04}, {0x0e, 0xef}, {0x1c, 0x01}, {0x1d, 0x02}, {0x1e, 0x03},
+	{0x1f, 0x11}, {0x20, 0x11}, {0x0e, 0xee}, {0x12, 0x03}, {0x16, 0x10},
+	{0x17, 0x02}, {0x1a, 0xee},
+	{0xfa, 0x08}, {0x0e, 0xef}, {0x1c, 0x01}, {0x1d, 0x02}, {0x1e, 0x03},
+	{0x1f, 0x00}, {0x20, 0x00}, {0x0e, 0xee}, {0x12, 0x03}, {0x16, 0x10},
+	{0x17, 0x02}, {0x1a, 0x22},
+
+	/* Gamma Correction */
+	{0x27, 0x62}, {0x28, 0x00}, {0x27, 0x62}, {0x28, 0x00}, {0x29, 0x00},
+	{0x2a, 0x00}, {0x2f, 0x03}, {0x30, 0x10}, {0x31, 0x2b}, {0x32, 0x50},
+	{0x33, 0x70}, {0x34, 0x90}, {0x35, 0xB0}, {0x36, 0xD0}, {0x37, 0x00},
+	{0x38, 0x18}, {0x39, 0x57}, {0x3a, 0x89}, {0x3b, 0xac}, {0x3c, 0xc9},
+	{0x3d, 0xde}, {0x3e, 0xef}, {0x2b, 0x00}, {0x2c, 0x00}, {0x2d, 0x40},
+	{0x2e, 0xab},
+
+	/* Contrast */
+	{0x47, 0x10}, {0x48, 0x1f}, {0x49, 0xe3}, {0x4a, 0xf0}, {0x4b, 0x08},
+	{0x4c, 0x14}, {0x4d, 0xe9}, {0x4e, 0xf5}, {0x98, 0x8a},
+
+	{0xfa, 0x00},
+	{MC521DA_TERM, MC521DA_TERM}
+};
+
+static int mc521da_attach(struct i2c_adapter *adapter);
+static int mc521da_detach(struct i2c_client *client);
+
+static struct i2c_driver mc521da_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "MC521DA Client",
+		   },
+	.attach_adapter = mc521da_attach,
+	.detach_client = mc521da_detach,
+};
+
+static struct i2c_client mc521da_i2c_client = {
+	.name = "MC521DA I2C dev",
+	.addr = MC521DA_I2C_ADDRESS,
+	.driver = &mc521da_i2c_driver,
+};
+
+static inline int mc521da_read_reg(u8 reg)
+{
+	return i2c_smbus_read_byte_data(&mc521da_i2c_client, reg);
+}
+
+static inline int mc521da_write_reg(u8 reg, u8 val)
+{
+	return i2c_smbus_write_byte_data(&mc521da_i2c_client, reg, val);
+}
+
+static int mc521da_write_regs(const struct mc521da_reg reglist[])
+{
+	int err;
+	const struct mc521da_reg *next = reglist;
+
+	while (!((next->reg == MC521DA_TERM) && (next->val == MC521DA_TERM))) {
+		err = mc521da_write_reg(next->reg, next->val);
+		if (err) {
+			return err;
+		}
+		next++;
+	}
+	return 0;
+}
+
+/*!
+ * mc521da sensor downscale function
+ * @param downscale            bool
+ * @return  Error code indicating success or failure
+ */
+static u8 mc521da_sensor_downscale(bool downscale)
+{
+	u8 data;
+	u32 i = 0;
+
+	if (downscale == true) {
+		// VGA
+		mc521da_write_reg(0xff, 0x01);
+
+		mc521da_write_reg(0x52, 0x30);
+		mc521da_write_reg(0x51, 0x00);
+
+		mc521da_write_reg(0xda, 0x01);
+		mc521da_write_reg(0x00, 0x8C);
+
+		/* Wait for changes to take effect */
+		while (i < 256) {
+			i++;
+			data = mc521da_read_reg(0x00);
+			if ((data & 0x80) == 0)
+				break;
+			msleep(5);
+		}
+
+		/* ISP */
+		mc521da_write_reg(0xff, 0x02);
+
+		mc521da_write_reg(0x03, 0x3b);	/* Enable Decimator */
+
+		mc521da_write_reg(0x7a, 0x74);
+		mc521da_write_reg(0x7b, 0x01);
+		mc521da_write_reg(0x7e, 0x50);
+		mc521da_write_reg(0x7f, 0x50);
+		mc521da_write_reg(0x80, 0x3c);
+		mc521da_write_reg(0x81, 0x3c);
+	} else {
+		//UXGA
+		mc521da_write_reg(0xff, 0x01);
+		mc521da_write_reg(0x52, 0x10);
+		mc521da_write_reg(0x51, 0x00);
+		mc521da_write_reg(0xda, 0x00);
+
+		/* update */
+		mc521da_write_reg(0x00, 0x84);
+
+		/* Wait for changes to take effect */
+		while (i < 256) {
+			i++;
+			data = mc521da_read_reg(0x00);
+			if ((data & 0x80) == 0)
+				break;
+			msleep(5);
+		}
+
+		/* ISP */
+		mc521da_write_reg(0xff, 0x02);
+
+		mc521da_write_reg(0x03, 0x3a);
+
+		mc521da_write_reg(0x7a, 0x70);
+		mc521da_write_reg(0x7b, 0x00);
+		mc521da_write_reg(0x7e, 0xc8);
+		mc521da_write_reg(0x7f, 0xc8);
+		mc521da_write_reg(0x80, 0x96);
+		mc521da_write_reg(0x81, 0x96);
+	}
+
+	return 0;
+}
+
+/*!
+ * mc521da sensor interface Initialization
+ * @param param            sensor_interface *
+ * @param width            u32
+ * @param height           u32
+ * @return  None
+ */
+static void mc521da_interface(sensor_interface * param, u32 width, u32 height)
+{
+	param->clk_mode = 0x0;	//gated
+	param->pixclk_pol = 0x0;
+	param->data_width = 0x1;
+	param->data_pol = 0x0;
+	param->ext_vsync = 0x0;
+	param->Vsync_pol = 0x0;
+	param->Hsync_pol = 0x0;
+	param->width = width - 1;
+	param->height = height - 1;
+	param->active_width = width;
+	param->active_height = height;
+	param->pixel_fmt = IPU_PIX_FMT_UYVY;
+}
+
+extern void gpio_sensor_reset(bool flag);
+
+/*!
+ * mc521da Reset function
+ *
+ * @return  None
+ */
+static sensor_interface *mc521da_reset(void)
+{
+	if (interface_param == NULL)
+		return NULL;
+
+	mc521da_interface(interface_param, format[1].width, format[1].height);
+	set_mclk_rate(&interface_param->mclk);
+
+	gpio_sensor_reset(true);
+	msleep(10);
+	gpio_sensor_reset(false);
+	msleep(50);
+
+	return interface_param;
+}
+
+/*!
+ * mc521da sensor configuration
+ *
+ * @param frame_rate       int 	*
+ * @param high_quality     int
+ * @return  sensor_interface *
+ */
+static sensor_interface *mc521da_config(int *frame_rate, int high_quality)
+{
+	int num_clock_per_row, err;
+	int max_rate = 0;
+	int index = 1;
+	u16 frame_height;
+
+	if (high_quality == 1)
+		index = 0;
+
+	err = mc521da_write_regs(mc521da_initial);
+	if (err) {
+		/* Reduce the MCLK */
+		interface_param->mclk = 20000000;
+		mc521da_reset();
+
+		printk(KERN_INFO "mc521da: mclk reduced\n");
+		mc521da_write_regs(mc521da_initial);
+	}
+
+	mc521da_interface(interface_param, format[index].width,
+			  format[index].height);
+
+	if (index == 0) {
+		mc521da_sensor_downscale(false);
+	} else {
+		mc521da_sensor_downscale(true);
+	}
+
+	num_clock_per_row = 1845;
+	max_rate = interface_param->mclk * 3 * (index + 1)
+	    / (2 * num_clock_per_row * 1300);
+
+	if ((*frame_rate > max_rate) || (*frame_rate == 0)) {
+		*frame_rate = max_rate;
+	}
+
+	frame_height = 1300 * max_rate / (*frame_rate);
+
+	*frame_rate = interface_param->mclk * 3 * (index + 1)
+	    / (2 * num_clock_per_row * frame_height);
+
+	mc521da_write_reg(0xff, 0x01);
+	mc521da_write_reg(0xE, frame_height & 0xFF);
+	mc521da_write_reg(0xF, (frame_height & 0xFF00) >> 8);
+	mc521da_write_reg(0xCC, frame_height & 0xFF);
+	mc521da_write_reg(0xCD, (frame_height & 0xFF00) >> 8);
+
+	return interface_param;
+}
+
+/*!
+ * mc521da sensor set color configuration
+ *
+ * @param bright       int
+ * @param saturation   int
+ * @param red          int
+ * @param green        int
+ * @param blue         int
+ * @return  None
+ */
+static void
+mc521da_set_color(int bright, int saturation, int red, int green, int blue)
+{
+	/* Select ISP */
+	mc521da_write_reg(0xff, 0x02);
+
+	mc521da_write_reg(0x41, bright);
+	mc521da_write_reg(0xca, red);
+	mc521da_write_reg(0xcb, green);
+	mc521da_write_reg(0xcc, blue);
+}
+
+/*!
+ * mc521da sensor get color configuration
+ *
+ * @param bright       int *
+ * @param saturation   int *
+ * @param red          int *
+ * @param green        int *
+ * @param blue         int *
+ * @return  None
+ */
+static void
+mc521da_get_color(int *bright, int *saturation, int *red, int *green, int *blue)
+{
+	*saturation = 0;
+
+	/* Select ISP */
+	mc521da_write_reg(0xff, 0x02);
+
+	*bright = mc521da_read_reg(0x41);
+	*red = mc521da_read_reg(0xCA);
+	*green = mc521da_read_reg(0xCB);
+	*blue = mc521da_read_reg(0xCC);
+}
+
+struct camera_sensor camera_sensor_if = {
+      set_color:mc521da_set_color,
+      get_color:mc521da_get_color,
+      config:mc521da_config,
+      reset:mc521da_reset,
+};
+
+/*!
+ * mc521da I2C detect_client function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @param address            int
+ * @param kind               int
+ *
+ * @return  Error code indicating success or failure
+ */
+static int mc521da_detect_client(struct i2c_adapter *adapter, int address,
+				 int kind)
+{
+	mc521da_i2c_client.adapter = adapter;
+	if (i2c_attach_client(&mc521da_i2c_client)) {
+		mc521da_i2c_client.adapter = NULL;
+		printk(KERN_ERR "mc521da_attach: i2c_attach_client failed\n");
+		return -1;
+	}
+
+	interface_param = (sensor_interface *)
+	    kmalloc(sizeof(sensor_interface), GFP_KERNEL);
+	if (!interface_param) {
+		printk(KERN_ERR "mc521da_attach: kmalloc failed \n");
+		return -1;
+	}
+
+	interface_param->mclk = 25000000;
+
+	printk(KERN_INFO "mc521da Detected\n");
+
+	return 0;
+}
+
+static unsigned short normal_i2c[] = { MC521DA_I2C_ADDRESS, I2C_CLIENT_END };
+
+/* Magic definition of all other variables and things */
+I2C_CLIENT_INSMOD;
+
+static int mc521da_attach(struct i2c_adapter *adap)
+{
+	uint32_t mclk = 25000000;
+	struct clk *clk;
+	int err;
+
+	clk = clk_get(NULL, "csi_clk");
+	clk_enable(clk);
+	set_mclk_rate(&mclk);
+
+	gpio_sensor_reset(true);
+	msleep(10);
+	gpio_sensor_reset(false);
+	msleep(100);
+
+	err = i2c_probe(adap, &addr_data, &mc521da_detect_client);
+
+	clk_disable(clk);
+	clk_put(clk);
+
+	return err;
+}
+
+/*!
+ * mc521da I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int mc521da_detach(struct i2c_client *client)
+{
+	int err;
+
+	if (!mc521da_i2c_client.adapter)
+		return -1;
+
+	err = i2c_detach_client(&mc521da_i2c_client);
+	mc521da_i2c_client.adapter = NULL;
+
+	if (interface_param)
+		kfree(interface_param);
+	interface_param = NULL;
+
+	return err;
+}
+
+extern void gpio_sensor_active(void);
+extern void gpio_sensor_inactive(void);
+
+/*!
+ * mc521da init function
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int mc521da_init(void)
+{
+	gpio_sensor_active();
+	return i2c_add_driver(&mc521da_i2c_driver);
+}
+
+/*!
+ * mc521da cleanup function
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit mc521da_clean(void)
+{
+	i2c_del_driver(&mc521da_i2c_driver);
+	gpio_sensor_inactive();
+}
+
+module_init(mc521da_init);
+module_exit(mc521da_clean);
+
+/* Exported symbols for modules. */
+EXPORT_SYMBOL(camera_sensor_if);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MC521DA Camera Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/mt9v111.c b/drivers/media/video/mxc/capture/mt9v111.c
new file mode 100644
index 000000000000..c95a20683924
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mt9v111.c
@@ -0,0 +1,1076 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mt9v111.c
+ *
+ * @brief mt9v111 camera driver functions
+ *
+ * @ingroup Camera
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/clk.h>
+#include <media/v4l2-int-device.h>
+#include "mxc_v4l2_capture.h"
+#include "mt9v111.h"
+
+#ifdef MT9V111_DEBUG
+static u16 testpattern = 0;
+#endif
+
+static mt9v111_conf mt9v111_device;
+
+/*!
+ * Holds the current frame rate.
+ */
+static int reset_frame_rate = MT9V111_FRAME_RATE;
+
+struct sensor {
+	const struct mt9v111_platform_data *platform_data;
+	struct v4l2_int_device *v4l2_int_device;
+	struct i2c_client *i2c_client;
+	struct v4l2_pix_format pix;
+	struct v4l2_captureparm streamcap;
+	bool on;
+
+	/* control settings */
+	int brightness;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+} mt9v111_data;
+
+extern void gpio_sensor_active(void);
+extern void gpio_sensor_inactive(void);
+
+static int mt9v111_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id);
+static int mt9v111_remove(struct i2c_client *client);
+
+static const struct i2c_device_id mt9v111_id[] = {
+	{"mt9v111", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, mt9v111_id);
+
+static struct i2c_driver mt9v111_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "mt9v111",
+		   },
+	.probe = mt9v111_probe,
+	.remove = mt9v111_remove,
+	.id_table = mt9v111_id,
+/* To add power management add .suspend and .resume functions */
+};
+
+/*
+ * Function definitions
+ */
+
+#ifdef MT9V111_DEBUG
+static inline int mt9v111_read_reg(u8 reg)
+{
+	int val = i2c_smbus_read_word_data(mt9v111_data.i2c_client, reg);
+	if (val != -1)
+		val = cpu_to_be16(val);
+	return val;
+}
+#endif
+
+/*!
+ * Writes to the register via I2C.
+ */
+static inline int mt9v111_write_reg(u8 reg, u16 val)
+{
+	pr_debug("In mt9v111_write_reg (0x%x, 0x%x)\n", reg, val);
+	pr_debug("   write reg %x val %x.\n", reg, val);
+
+	return i2c_smbus_write_word_data(mt9v111_data.i2c_client,
+					 reg, cpu_to_be16(val));
+}
+
+/*!
+ * Initialize mt9v111_sensor_lib
+ * Libarary for Sensor configuration through I2C
+ *
+ * @param       coreReg       Core Registers
+ * @param       ifpReg        IFP Register
+ *
+ * @return status
+ */
+static u8 mt9v111_sensor_lib(mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
+{
+	u8 reg;
+	u16 data;
+	u8 error = 0;
+
+	pr_debug("In mt9v111_sensor_lib\n");
+
+	/*
+	 * setup to IFP registers
+	 */
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_MODE_CONTROL;
+	data = ifpReg->modeControl;
+	mt9v111_write_reg(reg, data);
+
+	/* Output format */
+	reg = MT9V111I_FORMAT_CONTROL;
+	data = ifpReg->formatControl;	/* Set bit 12 */
+	mt9v111_write_reg(reg, data);
+
+	/* AE limit 4 */
+	reg = MT9V111I_SHUTTER_WIDTH_LIMIT_AE;
+	data = ifpReg->gainLimitAE;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_OUTPUT_FORMAT_CTRL2;
+	data = ifpReg->outputFormatCtrl2;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_AE_SPEED;
+	data = ifpReg->AESpeed;
+	mt9v111_write_reg(reg, data);
+
+	/* output image size */
+	reg = MT9V111i_H_PAN;
+	data = 0x8000 | ifpReg->HPan;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_ZOOM;
+	data = 0x8000 | ifpReg->HZoom;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_SIZE;
+	data = 0x8000 | ifpReg->HSize;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_PAN;
+	data = 0x8000 | ifpReg->VPan;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_ZOOM;
+	data = 0x8000 | ifpReg->VZoom;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_SIZE;
+	data = 0x8000 | ifpReg->VSize;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_PAN;
+	data = ~0x8000 & ifpReg->HPan;
+	mt9v111_write_reg(reg, data);
+#if 0
+	reg = MT9V111I_UPPER_SHUTTER_DELAY_LIM;
+	data = ifpReg->upperShutterDelayLi;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_SHUTTER_60;
+	data = ifpReg->shutter_width_60;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_SEARCH_FLICK_60;
+	data = ifpReg->search_flicker_60;
+	mt9v111_write_reg(reg, data);
+#endif
+
+	/*
+	 * setup to sensor core registers
+	 */
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = coreReg->addressSelect;
+	mt9v111_write_reg(reg, data);
+
+	/* enable changes and put the Sync bit on */
+	reg = MT9V111S_OUTPUT_CTRL;
+	data = MT9V111S_OUTCTRL_SYNC | MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
+	mt9v111_write_reg(reg, data);
+
+	/* min PIXCLK - Default */
+	reg = MT9V111S_PIXEL_CLOCK_SPEED;
+	data = coreReg->pixelClockSpeed;
+	mt9v111_write_reg(reg, data);
+
+	/* Setup image flipping / Dark rows / row/column skip */
+	reg = MT9V111S_READ_MODE;
+	data = coreReg->readMode;
+	mt9v111_write_reg(reg, data);
+
+	/* zoom 0 */
+	reg = MT9V111S_DIGITAL_ZOOM;
+	data = coreReg->digitalZoom;
+	mt9v111_write_reg(reg, data);
+
+	/* min H-blank */
+	reg = MT9V111S_HOR_BLANKING;
+	data = coreReg->horizontalBlanking;
+	mt9v111_write_reg(reg, data);
+
+	/* min V-blank */
+	reg = MT9V111S_VER_BLANKING;
+	data = coreReg->verticalBlanking;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111S_SHUTTER_WIDTH;
+	data = coreReg->shutterWidth;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111S_SHUTTER_DELAY;
+	data = ifpReg->upperShutterDelayLi;
+	mt9v111_write_reg(reg, data);
+
+	/* changes become effective */
+	reg = MT9V111S_OUTPUT_CTRL;
+	data = MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
+	mt9v111_write_reg(reg, data);
+
+	return error;
+}
+
+/*!
+ * MT9V111 frame rate calculate
+ *
+ * @param frame_rate       int *
+ * @param mclk             int
+ * @return  None
+ */
+static void mt9v111_rate_cal(int *frame_rate, int mclk)
+{
+	int num_clock_per_row;
+	int max_rate = 0;
+
+	pr_debug("In mt9v111_rate_cal\n");
+
+	num_clock_per_row = (MT9V111_MAX_WIDTH + 114 + MT9V111_HORZBLANK_MIN)
+			* 2;
+	max_rate = mclk / (num_clock_per_row *
+			   (MT9V111_MAX_HEIGHT + MT9V111_VERTBLANK_DEFAULT));
+
+	if ((*frame_rate > max_rate) || (*frame_rate == 0)) {
+		*frame_rate = max_rate;
+	}
+
+	mt9v111_device.coreReg->verticalBlanking
+	    = mclk / (*frame_rate * num_clock_per_row) - MT9V111_MAX_HEIGHT;
+
+	reset_frame_rate = *frame_rate;
+}
+
+/*!
+ * MT9V111 sensor configuration
+ */
+void mt9v111_config(void)
+{
+	pr_debug("In mt9v111_config\n");
+
+	mt9v111_device.coreReg->addressSelect = MT9V111I_SEL_SCA;
+	mt9v111_device.ifpReg->addrSpaceSel = MT9V111I_SEL_IFP;
+
+	mt9v111_device.coreReg->windowHeight = MT9V111_WINHEIGHT;
+	mt9v111_device.coreReg->windowWidth = MT9V111_WINWIDTH;
+	mt9v111_device.coreReg->zoomColStart = 0;
+	mt9v111_device.coreReg->zomRowStart = 0;
+	mt9v111_device.coreReg->digitalZoom = 0x0;
+
+	mt9v111_device.coreReg->verticalBlanking = MT9V111_VERTBLANK_DEFAULT;
+	mt9v111_device.coreReg->horizontalBlanking = MT9V111_HORZBLANK_MIN;
+	mt9v111_device.coreReg->pixelClockSpeed = 0;
+	mt9v111_device.coreReg->readMode = 0xd0a1;
+
+	mt9v111_device.ifpReg->outputFormatCtrl2 = 0;
+	mt9v111_device.ifpReg->gainLimitAE = 0x300;
+	mt9v111_device.ifpReg->AESpeed = 0x80;
+
+	/* here is the default value */
+	mt9v111_device.ifpReg->formatControl = 0xc800;
+	mt9v111_device.ifpReg->modeControl = 0x708e;
+	mt9v111_device.ifpReg->awbSpeed = 0x4514;
+	mt9v111_device.coreReg->shutterWidth = 0xf8;
+
+	/* output size */
+	mt9v111_device.ifpReg->HPan = 0;
+	mt9v111_device.ifpReg->HZoom = MT9V111_MAX_WIDTH;
+	mt9v111_device.ifpReg->HSize = MT9V111_MAX_WIDTH;
+	mt9v111_device.ifpReg->VPan = 0;
+	mt9v111_device.ifpReg->VZoom = MT9V111_MAX_HEIGHT;
+	mt9v111_device.ifpReg->VSize = MT9V111_MAX_HEIGHT;
+}
+
+/*!
+ * mt9v111 sensor set saturtionn
+ *
+ * @param saturation   int
+
+ * @return  Error code of 0.
+ */
+static int mt9v111_set_saturation(int saturation)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_saturation(%d)\n",
+		saturation);
+
+	switch (saturation) {
+	case 150:
+		mt9v111_device.ifpReg->awbSpeed = 0x6D14;
+		break;
+	case 100:
+		mt9v111_device.ifpReg->awbSpeed = 0x4514;
+		break;
+	case 75:
+		mt9v111_device.ifpReg->awbSpeed = 0x4D14;
+		break;
+	case 50:
+		mt9v111_device.ifpReg->awbSpeed = 0x5514;
+		break;
+	case 37:
+		mt9v111_device.ifpReg->awbSpeed = 0x5D14;
+		break;
+	case 25:
+		mt9v111_device.ifpReg->awbSpeed = 0x6514;
+		break;
+	default:
+		mt9v111_device.ifpReg->awbSpeed = 0x4514;
+		break;
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_AWB_SPEED;
+	data = mt9v111_device.ifpReg->awbSpeed;
+	mt9v111_write_reg(reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set Auto Exposure measurement window mode configuration
+ *
+ * @param ae_mode      int
+ * @return  Error code of 0 (no Error)
+ */
+static int mt9v111_set_ae_mode(int ae_mode)
+{
+	u8 reg;
+	u16 data;
+
+	pr_debug("In mt9v111_set_ae_mode(%d)\n",
+		ae_mode);
+
+	/* Currently this driver only supports auto and manual exposure
+	 * modes. */
+	if ((ae_mode > 1) || (ae_mode << 0))
+		return -EPERM;
+
+	/*
+	 * The auto exposure is set in bit 14.
+	 * Other values are set for:
+	 *  -on the fly defect correction is on (bit 13).
+	 *  -aperature correction knee enabled (bit 12).
+	 *  -ITU_R BT656 synchronization codes are embedded in the image (bit 7)
+	 *  -AE measurement window is weighted sum of large and center windows
+	 *     (bits 2-3).
+	 *  -auto white balance is on (bit 1).
+	 *  -normal color processing (bit 4 = 0).
+	 */
+	/* V4L2_EXPOSURE_AUTO = 0; needs register setting of 0x708E */
+	/* V4L2_EXPOSURE_MANUAL = 1 needs register setting of 0x308E */
+	mt9v111_device.ifpReg->modeControl &= 0x3fff;
+	mt9v111_device.ifpReg->modeControl |= (ae_mode & 0x03) << 14;
+	mt9v111_data.ae_mode = ae_mode;
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_MODE_CONTROL;
+	data = mt9v111_device.ifpReg->modeControl;
+	mt9v111_write_reg(reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor get AE measurement window mode configuration
+ *
+ * @param ae_mode      int *
+ * @return  None
+ */
+static void mt9v111_get_ae_mode(int *ae_mode)
+{
+	pr_debug("In mt9v111_get_ae_mode(%d)\n", *ae_mode);
+
+	if (ae_mode != NULL) {
+		*ae_mode = (mt9v111_device.ifpReg->modeControl & 0xc) >> 2;
+	}
+}
+
+#ifdef MT9V111_DEBUG
+/*!
+ * Set sensor to test mode, which will generate test pattern.
+ *
+ * @return none
+ */
+static void mt9v111_test_pattern(bool flag)
+{
+	u16 data;
+
+	/* switch to sensor registers */
+	mt9v111_write_reg(MT9V111I_ADDR_SPACE_SEL, MT9V111I_SEL_SCA);
+
+	if (flag == true) {
+		testpattern = MT9V111S_OUTCTRL_TEST_MODE;
+
+		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) & 0xBF;
+		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+
+		mt9v111_write_reg(MT9V111S_TEST_DATA, 0);
+
+		/* changes take effect */
+		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
+		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+	} else {
+		testpattern = 0;
+
+		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) | 0x40;
+		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+
+		/* changes take effect */
+		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
+		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+	}
+}
+#endif
+
+
+/* --------------- IOCTL functions from v4l2_int_ioctl_desc --------------- */
+
+/*!
+ * ioctl_g_ifparm - V4L2 sensor interface handler for vidioc_int_g_ifparm_num
+ * s: pointer to standard V4L2 device structure
+ * p: pointer to standard V4L2 vidioc_int_g_ifparm_num ioctl structure
+ *
+ * Gets slave interface parameters.
+ * Calculates the required xclk value to support the requested
+ * clock parameters in p.  This value is returned in the p
+ * parameter.
+ *
+ * vidioc_int_g_ifparm returns platform-specific information about the
+ * interface settings used by the sensor.
+ *
+ * Given the image capture format in pix, the nominal frame period in
+ * timeperframe, calculate the required xclk frequency.
+ *
+ * Called on open.
+ */
+static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
+{
+	pr_debug("In mt9v111:ioctl_g_ifparm\n");
+
+	if (s == NULL) {
+		pr_err("   ERROR!! no slave device set!\n");
+		return -1;
+	}
+
+	memset(p, 0, sizeof(*p));
+	p->u.bt656.clock_curr = MT9V111_MCLK;
+	p->if_type = V4L2_IF_TYPE_BT656;
+	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.clock_min = MT9V111_CLK_MIN;
+	p->u.bt656.clock_max = MT9V111_CLK_MAX;
+
+	return 0;
+}
+
+/*!
+ * Sets the camera power.
+ *
+ * s  pointer to the camera device
+ * on if 1, power is to be turned on.  0 means power is to be turned off
+ *
+ * ioctl_s_power - V4L2 sensor interface handler for vidioc_int_s_power_num
+ * @s: pointer to standard V4L2 device structure
+ * @on: power state to which device is to be set
+ *
+ * Sets devices power state to requrested state, if possible.
+ * This is called on suspend and resume.
+ */
+static int ioctl_s_power(struct v4l2_int_device *s, int on)
+{
+	struct sensor *sensor = s->priv;
+
+	pr_debug("In mt9v111:ioctl_s_power\n");
+
+	sensor->on = on;
+
+	if (on)
+		gpio_sensor_active();
+	else
+		gpio_sensor_inactive();
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_parm - V4L2 sensor interface handler for VIDIOC_G_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the sensor's video CAPTURE parameters.
+ */
+static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	int ret = 0;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+	/* s->priv points to mt9v111_data */
+
+	pr_debug("In mt9v111:ioctl_g_parm\n");
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		memset(a, 0, sizeof(*a));
+		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cparm->capability = mt9v111_data.streamcap.capability;
+		cparm->timeperframe =
+				mt9v111_data.streamcap.timeperframe;
+		cparm->capturemode = mt9v111_data.streamcap.capturemode;
+		ret = 0;
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_err("   type is not V4L2_BUF_TYPE_VIDEO_CAPTURE " \
+			"but %d\n", a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_err("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_parm - V4L2 sensor interface handler for VIDIOC_S_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the sensor to use the input parameters, if possible.  If
+ * not possible, reverts to the old parameters and returns the
+ * appropriate error code.
+ */
+static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	int ret = 0;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+	/* s->priv points to mt9v111_data */
+
+	pr_debug("In mt9v111:ioctl_s_parm\n");
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+
+		/* Check that the new frame rate is allowed.
+		 * Changing the frame rate is not allowed on this
+		 *camera. */
+		if (cparm->timeperframe.denominator !=
+		    mt9v111_data.streamcap.timeperframe.denominator) {
+			pr_err("ERROR: mt9v111: ioctl_s_parm: " \
+			       "This camera does not allow frame rate "
+			       "changes.\n");
+			ret = -EINVAL;
+		} else {
+			mt9v111_data.streamcap.timeperframe =
+						cparm->timeperframe;
+		      /* Call any camera functions to match settings. */
+		}
+
+		/* Check that new capture mode is supported. */
+		if ((cparm->capturemode != 0) &&
+		    !(cparm->capturemode & V4L2_MODE_HIGHQUALITY)) {
+			pr_err("ERROR: mt9v111: ioctl_s_parm: " \
+				"unsupported capture mode\n");
+			ret  = -EINVAL;
+		} else {
+			mt9v111_data.streamcap.capturemode =
+						cparm->capturemode;
+		      /* Call any camera functions to match settings. */
+		      /* Right now this camera only supports 1 mode. */
+		}
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_err("   type is not V4L2_BUF_TYPE_VIDEO_CAPTURE " \
+			"but %d\n", a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_err("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_fmt_cap - V4L2 sensor interface handler for ioctl_g_fmt_cap
+ * @s: pointer to standard V4L2 device structure
+ * @f: pointer to standard V4L2 v4l2_format structure
+ *
+ * Returns the sensor's current pixel format in the v4l2_format
+ * parameter.
+ */
+static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	struct sensor *sensor = s->priv;
+	/* s->priv points to mt9v111_data */
+
+	pr_debug("In mt9v111:ioctl_g_fmt_cap.\n");
+	pr_debug("   Returning size of %dx%d\n",
+		sensor->pix.width, sensor->pix.height);
+
+	f->fmt.pix = sensor->pix;
+
+	return 0;
+}
+
+/*!
+ * ioctl_queryctrl - V4L2 sensor interface handler for VIDIOC_QUERYCTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @qc: standard V4L2 VIDIOC_QUERYCTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control information
+ * from the video_control[] array.  Otherwise, returns -EINVAL if the
+ * control is not supported.
+ */
+static int ioctl_queryctrl(struct v4l2_int_device *s, struct v4l2_queryctrl *qc)
+{
+	pr_debug("In mt9v111:ioctl_queryctrl\n");
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_G_CTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control's current
+ * value from the video_control[] array.  Otherwise, returns -EINVAL
+ * if the control is not supported.
+ */
+static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	pr_debug("In mt9v111:ioctl_g_ctrl\n");
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		pr_debug("   V4L2_CID_BRIGHTNESS\n");
+		vc->value = mt9v111_data.brightness;
+		break;
+	case V4L2_CID_CONTRAST:
+		pr_debug("   V4L2_CID_CONTRAST\n");
+		vc->value = mt9v111_data.contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		pr_debug("   V4L2_CID_SATURATION\n");
+		vc->value = mt9v111_data.saturation;
+		break;
+	case V4L2_CID_HUE:
+		pr_debug("   V4L2_CID_HUE\n");
+		vc->value = mt9v111_data.hue;
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_DO_WHITE_BALANCE\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		pr_debug("   V4L2_CID_RED_BALANCE\n");
+		vc->value = mt9v111_data.red;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
+		vc->value = mt9v111_data.blue;
+		break;
+	case V4L2_CID_GAMMA:
+		pr_debug("   V4L2_CID_GAMMA\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_EXPOSURE:
+		pr_debug("   V4L2_CID_EXPOSURE\n");
+		vc->value = mt9v111_data.ae_mode;
+		break;
+	case V4L2_CID_AUTOGAIN:
+		pr_debug("   V4L2_CID_AUTOGAIN\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_GAIN:
+		pr_debug("   V4L2_CID_GAIN\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_HFLIP:
+		pr_debug("   V4L2_CID_HFLIP\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_VFLIP:
+		pr_debug("   V4L2_CID_VFLIP\n");
+		vc->value = 0;
+		break;
+	default:
+		pr_debug("   Default case\n");
+		return -EPERM;
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * ioctl_s_ctrl - V4L2 sensor interface handler for VIDIOC_S_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_S_CTRL ioctl structure
+ *
+ * If the requested control is supported, sets the control's current
+ * value in HW (and updates the video_control[] array).  Otherwise,
+ * returns -EINVAL if the control is not supported.
+ */
+static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int retval = 0;
+
+	pr_debug("In mt9v111:ioctl_s_ctrl %d\n",
+		vc->id);
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		pr_debug("   V4L2_CID_BRIGHTNESS\n");
+		break;
+	case V4L2_CID_CONTRAST:
+		pr_debug("   V4L2_CID_CONTRAST\n");
+		break;
+	case V4L2_CID_SATURATION:
+		pr_debug("   V4L2_CID_SATURATION\n");
+		retval = mt9v111_set_saturation(vc->value);
+		break;
+	case V4L2_CID_HUE:
+		pr_debug("   V4L2_CID_HUE\n");
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_DO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_RED_BALANCE:
+		pr_debug("   V4L2_CID_RED_BALANCE\n");
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
+		break;
+	case V4L2_CID_GAMMA:
+		pr_debug("   V4L2_CID_GAMMA\n");
+		break;
+	case V4L2_CID_EXPOSURE:
+		pr_debug("   V4L2_CID_EXPOSURE\n");
+		retval = mt9v111_set_ae_mode(vc->value);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		pr_debug("   V4L2_CID_AUTOGAIN\n");
+		break;
+	case V4L2_CID_GAIN:
+		pr_debug("   V4L2_CID_GAIN\n");
+		break;
+	case V4L2_CID_HFLIP:
+		pr_debug("   V4L2_CID_HFLIP\n");
+		break;
+	case V4L2_CID_VFLIP:
+		pr_debug("   V4L2_CID_VFLIP\n");
+		break;
+	default:
+		pr_debug("   Default case\n");
+		retval = -EPERM;
+		break;
+	}
+
+	return retval;
+}
+
+/*!
+ * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
+ * @s: pointer to standard V4L2 device structure
+ */
+static int ioctl_init(struct v4l2_int_device *s)
+{
+	pr_debug("In mt9v111:ioctl_init\n");
+
+	return 0;
+}
+
+/*!
+ * ioctl_dev_init - V4L2 sensor interface handler for vidioc_int_dev_init_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Initialise the device when slave attaches to the master.
+ */
+static int ioctl_dev_init(struct v4l2_int_device *s)
+{
+	uint32_t clock_rate = MT9V111_MCLK;
+
+	pr_debug("In mt9v111:ioctl_dev_init\n");
+
+	gpio_sensor_active();
+
+	set_mclk_rate(&clock_rate);
+	mt9v111_rate_cal(&reset_frame_rate, clock_rate);
+	mt9v111_sensor_lib(mt9v111_device.coreReg, mt9v111_device.ifpReg);
+
+	return 0;
+}
+
+/*!
+ * This structure defines all the ioctls for this module and links them to the
+ * enumeration.
+ */
+static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
+
+	{vidioc_int_dev_init_num, (v4l2_int_ioctl_func *)ioctl_dev_init},
+
+	/*!
+	 * Delinitialise the dev. at slave detach.
+	 * The complement of ioctl_dev_init.
+	 */
+/*	{vidioc_int_dev_exit_num, (v4l2_int_ioctl_func *) ioctl_dev_exit}, */
+
+	{vidioc_int_s_power_num, (v4l2_int_ioctl_func *) ioctl_s_power},
+	{vidioc_int_g_ifparm_num, (v4l2_int_ioctl_func *) ioctl_g_ifparm},
+/*	{vidioc_int_g_needs_reset_num,
+				(v4l2_int_ioctl_func *) ioctl_g_needs_reset}, */
+/*	{vidioc_int_reset_num, (v4l2_int_ioctl_func *) ioctl_reset}, */
+	{vidioc_int_init_num, (v4l2_int_ioctl_func *) ioctl_init},
+
+	/*!
+	 * VIDIOC_ENUM_FMT ioctl for the CAPTURE buffer type.
+	 */
+/*	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *) ioctl_enum_fmt_cap}, */
+
+	/*!
+	 * VIDIOC_TRY_FMT ioctl for the CAPTURE buffer type.
+	 * This ioctl is used to negotiate the image capture size and
+	 * pixel format without actually making it take effect.
+	 */
+/*	{vidioc_int_try_fmt_cap_num,
+				(v4l2_int_ioctl_func *) ioctl_try_fmt_cap}, */
+
+	{vidioc_int_g_fmt_cap_num, (v4l2_int_ioctl_func *) ioctl_g_fmt_cap},
+
+	/*!
+	 * If the requested format is supported, configures the HW to use that
+	 * format, returns error code if format not supported or HW can't be
+	 * correctly configured.
+	 */
+/*	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_fmt_cap}, */
+
+	{vidioc_int_g_parm_num, (v4l2_int_ioctl_func *) ioctl_g_parm},
+	{vidioc_int_s_parm_num, (v4l2_int_ioctl_func *) ioctl_s_parm},
+/*	{vidioc_int_queryctrl_num, (v4l2_int_ioctl_func *) ioctl_queryctrl}, */
+	{vidioc_int_g_ctrl_num, (v4l2_int_ioctl_func *) ioctl_g_ctrl},
+	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *) ioctl_s_ctrl},
+};
+
+static struct v4l2_int_slave mt9v111_slave = {
+	.ioctls = mt9v111_ioctl_desc,
+	.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+};
+
+static struct v4l2_int_device mt9v111_int_device = {
+	.module = THIS_MODULE,
+	.name = "mt9v111",
+	.type = v4l2_int_type_slave,
+	.u = {
+		.slave = &mt9v111_slave,
+		},
+};
+
+/*!
+ * mt9v111 I2C probe function
+ * Function set in i2c_driver struct.
+ * Called by insmod mt9v111_camera.ko.
+ *
+ * @return  Error code indicating success or failure
+ */
+static int mt9v111_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	int retval;
+
+	pr_debug("In mt9v111_probe  device id is %s\n", id->name);
+
+	/* Set initial values for the sensor struct. */
+	memset(&mt9v111_data, 0, sizeof(mt9v111_data));
+	mt9v111_data.i2c_client = client;
+	pr_debug("   client name is %s\n", client->name);
+	mt9v111_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	mt9v111_data.pix.width = MT9V111_MAX_WIDTH;
+	mt9v111_data.pix.height = MT9V111_MAX_HEIGHT;
+	mt9v111_data.streamcap.capability = 0; /* No higher resolution or frame
+						* frame rate changes supported.
+						*/
+	mt9v111_data.streamcap.timeperframe.denominator = MT9V111_FRAME_RATE;
+	mt9v111_data.streamcap.timeperframe.numerator = 1;
+
+	mt9v111_int_device.priv = &mt9v111_data;
+
+	pr_debug("   type is %d (expect %d)\n",
+		mt9v111_int_device.type, v4l2_int_type_slave);
+	pr_debug("   num ioctls is %d\n",
+		mt9v111_int_device.u.slave->num_ioctls);
+
+	/* This function attaches this structure to the /dev/video0 device.
+	 * The pointer in priv points to the mt9v111_data structure here.*/
+	retval = v4l2_int_device_register(&mt9v111_int_device);
+
+	return retval;
+}
+
+/*!
+ * Function set in i2c_driver struct.
+ * Called on rmmod mt9v111_camera.ko
+ */
+static int mt9v111_remove(struct i2c_client *client)
+{
+	pr_debug("In mt9v111_remove\n");
+
+	v4l2_int_device_unregister(&mt9v111_int_device);
+	return 0;
+}
+
+/*!
+ * MT9V111 init function.
+ * Called by insmod mt9v111_camera.ko.
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int mt9v111_init(void)
+{
+	u8 err;
+
+	pr_debug("In mt9v111_init\n");
+
+	/* Allocate memory for state structures. */
+	mt9v111_device.coreReg = (mt9v111_coreReg *)
+				kmalloc(sizeof(mt9v111_coreReg), GFP_KERNEL);
+	if (!mt9v111_device.coreReg)
+		return -1;
+	memset(mt9v111_device.coreReg, 0, sizeof(mt9v111_coreReg));
+
+	mt9v111_device.ifpReg = (mt9v111_IFPReg *)
+				kmalloc(sizeof(mt9v111_IFPReg), GFP_KERNEL);
+	if (!mt9v111_device.ifpReg) {
+		kfree(mt9v111_device.coreReg);
+		mt9v111_device.coreReg = NULL;
+		return -1;
+	}
+	memset(mt9v111_device.ifpReg, 0, sizeof(mt9v111_IFPReg));
+
+	/* Set contents of the just created structures. */
+	mt9v111_config();
+
+	/* Tells the i2c driver what functions to call for this driver. */
+	err = i2c_add_driver(&mt9v111_i2c_driver);
+	if (err != 0)
+		pr_err("%s:driver registration failed, error=%d \n",
+		       __func__, err);
+
+	return err;
+}
+
+/*!
+ * MT9V111 cleanup function.
+ * Called on rmmod mt9v111_camera.ko
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit mt9v111_clean(void)
+{
+	pr_debug("In mt9v111_clean()\n");
+
+	i2c_del_driver(&mt9v111_i2c_driver);
+	gpio_sensor_inactive();
+
+	if (mt9v111_device.coreReg) {
+		kfree(mt9v111_device.coreReg);
+		mt9v111_device.coreReg = NULL;
+	}
+
+	if (mt9v111_device.ifpReg) {
+		kfree(mt9v111_device.ifpReg);
+		mt9v111_device.ifpReg = NULL;
+	}
+}
+
+module_init(mt9v111_init);
+module_exit(mt9v111_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Mt9v111 Camera Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/mt9v111.h b/drivers/media/video/mxc/capture/mt9v111.h
new file mode 100644
index 000000000000..cf38cec4757c
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mt9v111.h
@@ -0,0 +1,431 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup Camera Sensor Drivers
+ */
+
+/*!
+ * @file mt9v111.h
+ *
+ * @brief MT9V111 Camera Header file
+ *
+ * This header file contains defines and structures for the iMagic mi8012
+ * aka the Micron mt9v111 camera.
+ *
+ * @ingroup Camera
+ */
+
+#ifndef MT9V111_H_
+#define MT9V111_H_
+
+/*!
+ * Basic camera values
+ */
+#define MT9V111_FRAME_RATE        30
+#define MT9V111_MCLK              27000000 /* Desired clock rate */
+#define MT9V111_CLK_MIN           12000000 /* This clock rate yields 15 fps */
+#define MT9V111_CLK_MAX           27000000
+#define MT9V111_MAX_WIDTH         640      /* Max width for this camera */
+#define MT9V111_MAX_HEIGHT        480      /* Max height for this camera */
+
+/*!
+ * mt9v111 IFP REGISTER BANK MAP
+ */
+#define MT9V111I_ADDR_SPACE_SEL           0x1
+#define MT9V111I_BASE_MAXTRIX_SIGN        0x2
+#define MT9V111I_BASE_MAXTRIX_SCALE15     0x3
+#define MT9V111I_BASE_MAXTRIX_SCALE69     0x4
+#define MT9V111I_APERTURE_GAIN            0x5
+#define MT9V111I_MODE_CONTROL             0x6
+#define MT9V111I_SOFT_RESET               0x7
+#define MT9V111I_FORMAT_CONTROL           0x8
+#define MT9V111I_BASE_MATRIX_CFK1         0x9
+#define MT9V111I_BASE_MATRIX_CFK2         0xa
+#define MT9V111I_BASE_MATRIX_CFK3         0xb
+#define MT9V111I_BASE_MATRIX_CFK4         0xc
+#define MT9V111I_BASE_MATRIX_CFK5         0xd
+#define MT9V111I_BASE_MATRIX_CFK6         0xe
+#define MT9V111I_BASE_MATRIX_CFK7         0xf
+#define MT9V111I_BASE_MATRIX_CFK8         0x10
+#define MT9V111I_BASE_MATRIX_CFK9         0x11
+#define MT9V111I_AWB_POSITION             0x12
+#define MT9V111I_AWB_RED_GAIN             0x13
+#define MT9V111I_AWB_BLUE_GAIN            0x14
+#define MT9V111I_DELTA_MATRIX_CF_SIGN     0x15
+#define MT9V111I_DELTA_MATRIX_CF_D1       0x16
+#define MT9V111I_DELTA_MATRIX_CF_D2       0x17
+#define MT9V111I_DELTA_MATRIX_CF_D3       0x18
+#define MT9V111I_DELTA_MATRIX_CF_D4       0x19
+#define MT9V111I_DELTA_MATRIX_CF_D5       0x1a
+#define MT9V111I_DELTA_MATRIX_CF_D6       0x1b
+#define MT9V111I_DELTA_MATRIX_CF_D7       0x1c
+#define MT9V111I_DELTA_MATRIX_CF_D8       0x1d
+#define MT9V111I_DELTA_MATRIX_CF_D9       0x1e
+#define MT9V111I_LUMINANCE_LIMIT_WB       0x20
+#define MT9V111I_RBG_MANUUAL_WB           0x21
+#define MT9V111I_AWB_RED_LIMIT            0x22
+#define MT9V111I_AWB_BLUE_LIMIT           0x23
+#define MT9V111I_MATRIX_ADJUST_LIMIT      0x24
+#define MT9V111I_AWB_SPEED                0x25
+#define MT9V111I_H_BOUND_AE               0x26
+#define MT9V111I_V_BOUND_AE               0x27
+#define MT9V111I_H_BOUND_AE_CEN_WIN       0x2b
+#define MT9V111I_V_BOUND_AE_CEN_WIN       0x2c
+#define MT9V111I_BOUND_AWB_WIN            0x2d
+#define MT9V111I_AE_PRECISION_TARGET      0x2e
+#define MT9V111I_AE_SPEED                 0x2f
+#define MT9V111I_RED_AWB_MEASURE          0x30
+#define MT9V111I_LUMA_AWB_MEASURE         0x31
+#define MT9V111I_BLUE_AWB_MEASURE         0x32
+#define MT9V111I_LIMIT_SHARP_SATU_CTRL    0x33
+#define MT9V111I_LUMA_OFFSET              0x34
+#define MT9V111I_CLIP_LIMIT_OUTPUT_LUMI   0x35
+#define MT9V111I_GAIN_LIMIT_AE            0x36
+#define MT9V111I_SHUTTER_WIDTH_LIMIT_AE   0x37
+#define MT9V111I_UPPER_SHUTTER_DELAY_LIM  0x39
+#define MT9V111I_OUTPUT_FORMAT_CTRL2      0x3a
+#define MT9V111I_IPF_BLACK_LEVEL_SUB      0x3b
+#define MT9V111I_IPF_BLACK_LEVEL_ADD      0x3c
+#define MT9V111I_ADC_LIMIT_AE_ADJ         0x3d
+#define MT9V111I_GAIN_THRE_CCAM_ADJ       0x3e
+#define MT9V111I_LINEAR_AE                0x3f
+#define MT9V111I_THRESHOLD_EDGE_DEFECT    0x47
+#define MT9V111I_LUMA_SUM_MEASURE         0x4c
+#define MT9V111I_TIME_ADV_SUM_LUMA        0x4d
+#define MT9V111I_MOTION                   0x52
+#define MT9V111I_GAMMA_KNEE_Y12           0x53
+#define MT9V111I_GAMMA_KNEE_Y34           0x54
+#define MT9V111I_GAMMA_KNEE_Y56           0x55
+#define MT9V111I_GAMMA_KNEE_Y78           0x56
+#define MT9V111I_GAMMA_KNEE_Y90           0x57
+#define MT9V111I_GAMMA_VALUE_Y0           0x58
+#define MT9V111I_SHUTTER_60               0x59
+#define MT9V111I_SEARCH_FLICK_60          0x5c
+#define MT9V111I_RATIO_IMAGE_GAIN_BASE    0x5e
+#define MT9V111I_RATIO_IMAGE_GAIN_DELTA   0x5f
+#define MT9V111I_SIGN_VALUE_REG5F         0x60
+#define MT9V111I_AE_GAIN                  0x62
+#define MT9V111I_MAX_GAIN_AE              0x67
+#define MT9V111I_LENS_CORRECT_CTRL        0x80
+#define MT9V111I_SHADING_PARAMETER1       0x81
+#define MT9V111I_SHADING_PARAMETER2       0x82
+#define MT9V111I_SHADING_PARAMETER3       0x83
+#define MT9V111I_SHADING_PARAMETER4       0x84
+#define MT9V111I_SHADING_PARAMETER5       0x85
+#define MT9V111I_SHADING_PARAMETER6       0x86
+#define MT9V111I_SHADING_PARAMETER7       0x87
+#define MT9V111I_SHADING_PARAMETER8       0x88
+#define MT9V111I_SHADING_PARAMETER9       0x89
+#define MT9V111I_SHADING_PARAMETER10      0x8A
+#define MT9V111I_SHADING_PARAMETER11      0x8B
+#define MT9V111I_SHADING_PARAMETER12      0x8C
+#define MT9V111I_SHADING_PARAMETER13      0x8D
+#define MT9V111I_SHADING_PARAMETER14      0x8E
+#define MT9V111I_SHADING_PARAMETER15      0x8F
+#define MT9V111I_SHADING_PARAMETER16      0x90
+#define MT9V111I_SHADING_PARAMETER17      0x91
+#define MT9V111I_SHADING_PARAMETER18      0x92
+#define MT9V111I_SHADING_PARAMETER19      0x93
+#define MT9V111I_SHADING_PARAMETER20      0x94
+#define MT9V111I_SHADING_PARAMETER21      0x95
+#define MT9V111i_FLASH_CTRL               0x98
+#define MT9V111i_LINE_COUNTER             0x99
+#define MT9V111i_FRAME_COUNTER            0x9A
+#define MT9V111i_H_PAN                    0xA5
+#define MT9V111i_H_ZOOM                   0xA6
+#define MT9V111i_H_SIZE                   0xA7
+#define MT9V111i_V_PAN                    0xA8
+#define MT9V111i_V_ZOOM                   0xA9
+#define MT9V111i_V_SIZE                   0xAA
+
+#define MT9V111I_SEL_IFP                  0x1
+#define MT9V111I_SEL_SCA                  0x4
+#define MT9V111I_FC_RGB_OR_YUV            0x1000
+
+/*!
+ * Mt9v111 SENSOR CORE REGISTER BANK MAP
+ */
+#define MT9V111S_ADDR_SPACE_SEL           0x1
+#define MT9V111S_COLUMN_START             0x2
+#define MT9V111S_WIN_HEIGHT               0x3
+#define MT9V111S_WIN_WIDTH                0x4
+#define MT9V111S_HOR_BLANKING             0x5
+#define MT9V111S_VER_BLANKING             0x6
+#define MT9V111S_OUTPUT_CTRL              0x7
+#define MT9V111S_ROW_START                0x8
+#define MT9V111S_SHUTTER_WIDTH            0x9
+#define MT9V111S_PIXEL_CLOCK_SPEED        0xa
+#define MT9V111S_RESTART                  0xb
+#define MT9V111S_SHUTTER_DELAY            0xc
+#define MT9V111S_RESET                    0xd
+#define MT9V111S_COLUMN_START_IN_ZOOM     0x12
+#define MT9V111S_ROW_START_IN_ZOOM        0x13
+#define MT9V111S_DIGITAL_ZOOM             0x1e
+#define MT9V111S_READ_MODE                0x20
+#define MT9V111S_DAC_CTRL                 0x27
+#define MT9V111S_GREEN1_GAIN              0x2b
+#define MT9V111S_BLUE_GAIN                0x2c
+#define MT9V111S_READ_GAIN                0x2d
+#define MT9V111S_GREEN2_GAIN              0x2e
+#define MT9V111S_ROW_NOISE_CTRL           0x30
+#define MT9V111S_DARK_TARGET_W            0x31
+#define MT9V111S_TEST_DATA                0x32
+#define MT9V111S_GLOBAL_GAIN              0x35
+#define MT9V111S_SENSOR_CORE_VERSION      0x36
+#define MT9V111S_DARK_TARGET_WO           0x37
+#define MT9V111S_VERF_DAC                 0x41
+#define MT9V111S_VCM_VCL                  0x42
+#define MT9V111S_DISABLE_BYPASS           0x58
+#define MT9V111S_CALIB_MEAN_TEST          0x59
+#define MT9V111S_DARK_G1_AVE              0x5B
+#define MT9V111S_DARK_G2_AVE              0x5C
+#define MT9V111S_DARK_R_AVE               0x5D
+#define MT9V111S_DARK_B_AVE               0x5E
+#define MT9V111S_CAL_THRESHOLD            0x5f
+#define MT9V111S_CAL_G1                   0x60
+#define MT9V111S_CAL_G2                   0x61
+#define MT9V111S_CAL_CTRL                 0x62
+#define MT9V111S_CAL_R                    0x63
+#define MT9V111S_CAL_B                    0x64
+#define MT9V111S_CHIP_ENABLE              0xF1
+#define MT9V111S_CHIP_VERSION             0xFF
+
+/* OUTPUT_CTRL */
+#define MT9V111S_OUTCTRL_SYNC             0x1
+#define MT9V111S_OUTCTRL_CHIP_ENABLE      0x2
+#define MT9V111S_OUTCTRL_TEST_MODE        0x40
+
+/* READ_MODE */
+#define MT9V111S_RM_NOBADFRAME            0x1
+#define MT9V111S_RM_NODESTRUCT            0x2
+#define MT9V111S_RM_COLUMNSKIP            0x4
+#define MT9V111S_RM_ROWSKIP               0x8
+#define MT9V111S_RM_BOOSTEDRESET          0x1000
+#define MT9V111S_RM_COLUMN_LATE           0x10
+#define MT9V111S_RM_ROW_LATE              0x80
+#define MT9V111S_RM_RIGTH_TO_LEFT         0x4000
+#define MT9V111S_RM_BOTTOM_TO_TOP         0x8000
+
+/*! I2C Slave Address */
+#define MT9V111_I2C_ADDRESS	0x48
+
+/*!
+ * The image resolution enum for the mt9v111 sensor
+ */
+typedef enum {
+	MT9V111_OutputResolution_VGA = 0,	/*!< VGA size */
+	MT9V111_OutputResolution_QVGA,	/*!< QVGA size */
+	MT9V111_OutputResolution_CIF,	/*!< CIF size */
+	MT9V111_OutputResolution_QCIF,	/*!< QCIF size */
+	MT9V111_OutputResolution_QQVGA,	/*!< QQVGA size */
+	MT9V111_OutputResolution_SXGA	/*!< SXGA size */
+} MT9V111_OutputResolution;
+
+enum {
+	MT9V111_WINWIDTH = 0x287,
+	MT9V111_WINWIDTH_DEFAULT = 0x287,
+	MT9V111_WINWIDTH_MIN = 0x9,
+
+	MT9V111_WINHEIGHT = 0x1E7,
+	MT9V111_WINHEIGHT_DEFAULT = 0x1E7,
+
+	MT9V111_HORZBLANK_DEFAULT = 0x26,
+	MT9V111_HORZBLANK_MIN = 0x9,
+	MT9V111_HORZBLANK_MAX = 0x3FF,
+
+	MT9V111_VERTBLANK_DEFAULT = 0x4,
+	MT9V111_VERTBLANK_MIN = 0x3,
+	MT9V111_VERTBLANK_MAX = 0xFFF,
+};
+
+/*!
+ * Mt9v111 Core Register structure.
+ */
+typedef struct {
+	u32 addressSelect;	/*!< select address bank for Core Register 0x4 */
+	u32 columnStart;	/*!< Starting Column */
+	u32 windowHeight;	/*!< Window Height */
+	u32 windowWidth;	/*!< Window Width */
+	u32 horizontalBlanking;	/*!< Horizontal Blank time, in pixels */
+	u32 verticalBlanking;	/*!< Vertical Blank time, in pixels */
+	u32 outputControl;	/*!< Register to control sensor output */
+	u32 rowStart;		/*!< Starting Row */
+	u32 shutterWidth;
+	u32 pixelClockSpeed;	/*!< pixel date rate */
+	u32 restart;		/*!< Abandon the readout of current frame */
+	u32 shutterDelay;
+	u32 reset;		/*!< reset the sensor to the default mode */
+	u32 zoomColStart;	/*!< Column start in the Zoom mode */
+	u32 zomRowStart;	/*!< Row start in the Zoom mode */
+	u32 digitalZoom;	/*!< 1 means zoom by 2 */
+	u32 readMode;		/*!< Readmode: aspects of the readout of the sensor */
+	u32 dACStandbyControl;
+	u32 green1Gain;		/*!< Gain Settings */
+	u32 blueGain;
+	u32 redGain;
+	u32 green2Gain;
+	u32 rowNoiseControl;
+	u32 darkTargetwNC;
+	u32 testData;		/*!< test mode */
+	u32 globalGain;
+	u32 chipVersion;
+	u32 darkTargetwoNC;
+	u32 vREFDACs;
+	u32 vCMandVCL;
+	u32 disableBypass;
+	u32 calibMeanTest;
+	u32 darkG1average;
+	u32 darkG2average;
+	u32 darkRaverage;
+	u32 darkBaverage;
+	u32 calibThreshold;
+	u32 calibGreen1;
+	u32 calibGreen2;
+	u32 calibControl;
+	u32 calibRed;
+	u32 calibBlue;
+	u32 chipEnable;		/*!< Image core Registers written by image flow processor */
+} mt9v111_coreReg;
+
+/*!
+ * Mt9v111 IFP Register structure.
+ */
+typedef struct {
+	u32 addrSpaceSel;	/*!< select address bank for Core Register 0x1 */
+	u32 baseMaxtrixSign;	/*!< sign of coefficient for base color correction matrix */
+	u32 baseMaxtrixScale15;	/*!< scaling of color correction coefficient K1-5 */
+	u32 baseMaxtrixScale69;	/*!< scaling of color correction coefficient K6-9 */
+	u32 apertureGain;	/*!< sharpening */
+	u32 modeControl;	/*!< bit 7 CCIR656 sync codes are embedded in the image */
+	u32 softReset;		/*!< Image processing mode: 1 reset mode, 0 operational mode */
+	u32 formatControl;	/*!< bit12 1 for RGB565, 0 for YcrCb */
+	u32 baseMatrixCfk1;	/*!< K1 Color correction coefficient */
+	u32 baseMatrixCfk2;	/*!< K2 Color correction coefficient */
+	u32 baseMatrixCfk3;	/*!< K3 Color correction coefficient */
+	u32 baseMatrixCfk4;	/*!< K4 Color correction coefficient */
+	u32 baseMatrixCfk5;	/*!< K5 Color correction coefficient */
+	u32 baseMatrixCfk6;	/*!< K6 Color correction coefficient */
+	u32 baseMatrixCfk7;	/*!< K7 Color correction coefficient */
+	u32 baseMatrixCfk8;	/*!< K8 Color correction coefficient */
+	u32 baseMatrixCfk9;	/*!< K9 Color correction coefficient */
+	u32 awbPosition;	/*!< Current position of AWB color correction matrix */
+	u32 awbRedGain;		/*!< Current value of AWB red channel gain */
+	u32 awbBlueGain;	/*!< Current value of AWB blue channel gain */
+	u32 deltaMatrixCFSign;	/*!< Sign of coefficients of delta color correction matrix register */
+	u32 deltaMatrixCFD1;	/*!< D1 Delta coefficient */
+	u32 deltaMatrixCFD2;	/*!< D2 Delta coefficient */
+	u32 deltaMatrixCFD3;	/*!< D3 Delta coefficient */
+	u32 deltaMatrixCFD4;	/*!< D4 Delta coefficient */
+	u32 deltaMatrixCFD5;	/*!< D5 Delta coefficient */
+	u32 deltaMatrixCFD6;	/*!< D6 Delta coefficient */
+	u32 deltaMatrixCFD7;	/*!< D7 Delta coefficient */
+	u32 deltaMatrixCFD8;	/*!< D8 Delta coefficient */
+	u32 deltaMatrixCFD9;	/*!< D9 Delta coefficient */
+	u32 lumLimitWB;		/*!< Luminance range of pixels considered in WB statistics */
+	u32 RBGManualWB;	/*!< Red and Blue color channel gains for manual white balance */
+	u32 awbRedLimit;	/*!< Limits on Red channel gain adjustment through AWB */
+	u32 awbBlueLimit;	/*!< Limits on Blue channel gain adjustment through AWB */
+	u32 matrixAdjLimit;	/*!< Limits on color correction matrix adjustment through AWB */
+	u32 awbSpeed;		/*!< AWB speed and color saturation control */
+	u32 HBoundAE;		/*!< Horizontal boundaries of AWB measurement window */
+	u32 VBoundAE;		/*!< Vertical boundaries of AWB measurement window */
+	u32 HBoundAECenWin;	/*!< Horizontal boundaries of AE measurement window for backlight compensation */
+	u32 VBoundAECenWin;	/*!< Vertical boundaries of AE measurement window for backlight compensation */
+	u32 boundAwbWin;	/*!< Boundaries of AWB measurement window */
+	u32 AEPrecisionTarget;	/*!< Auto exposure target and precision control */
+	u32 AESpeed;		/*!< AE speed and sensitivity control register */
+	u32 redAWBMeasure;	/*!< Measure of the red channel value used by AWB */
+	u32 lumaAWBMeasure;	/*!< Measure of the luminance channel value used by AWB */
+	u32 blueAWBMeasure;	/*!< Measure of the blue channel value used by AWB */
+	u32 limitSharpSatuCtrl;	/*!< Automatic control of sharpness and color saturation */
+	u32 lumaOffset;		/*!< Luminance offset control (brightness control) */
+	u32 clipLimitOutputLumi;	/*!< Clipping limits for output luminance */
+	u32 gainLimitAE;	/*!< Imager gain limits for AE adjustment */
+	u32 shutterWidthLimitAE;	/*!< Shutter width (exposure time) limits for AE adjustment */
+	u32 upperShutterDelayLi;	/*!< Upper Shutter Delay Limit */
+	u32 outputFormatCtrl2;	/*!< Output Format Control 2
+				   00 = 16-bit RGB565.
+				   01 = 15-bit RGB555.
+				   10 = 12-bit RGB444x.
+				   11 = 12-bit RGBx444.       */
+	u32 ipfBlackLevelSub;	/*!< IFP black level subtraction */
+	u32 ipfBlackLevelAdd;	/*!< IFP black level addition */
+	u32 adcLimitAEAdj;	/*!< ADC limits for AE adjustment */
+	u32 agimnThreCamAdj;	/*!< Gain threshold for CCM adjustment */
+	u32 linearAE;
+	u32 thresholdEdgeDefect;	/*!< Edge threshold for interpolation and defect correction */
+	u32 lumaSumMeasure;	/*!< Luma measured by AE engine */
+	u32 timeAdvSumLuma;	/*!< Time-averaged luminance value tracked by auto exposure */
+	u32 motion;		/*!< 1 when motion is detected */
+	u32 gammaKneeY12;	/*!< Gamma knee points Y1 and Y2 */
+	u32 gammaKneeY34;	/*!< Gamma knee points Y3 and Y4 */
+	u32 gammaKneeY56;	/*!< Gamma knee points Y5 and Y6 */
+	u32 gammaKneeY78;	/*!< Gamma knee points Y7 and Y8 */
+	u32 gammaKneeY90;	/*!< Gamma knee points Y9 and Y10 */
+	u32 gammaKneeY0;	/*!< Gamma knee point Y0 */
+	u32 shutter_width_60;
+	u32 search_flicker_60;
+	u32 ratioImageGainBase;
+	u32 ratioImageGainDelta;
+	u32 signValueReg5F;
+	u32 aeGain;
+	u32 maxGainAE;
+	u32 lensCorrectCtrl;
+	u32 shadingParameter1;	/*!< Shade Parameters */
+	u32 shadingParameter2;
+	u32 shadingParameter3;
+	u32 shadingParameter4;
+	u32 shadingParameter5;
+	u32 shadingParameter6;
+	u32 shadingParameter7;
+	u32 shadingParameter8;
+	u32 shadingParameter9;
+	u32 shadingParameter10;
+	u32 shadingParameter11;
+	u32 shadingParameter12;
+	u32 shadingParameter13;
+	u32 shadingParameter14;
+	u32 shadingParameter15;
+	u32 shadingParameter16;
+	u32 shadingParameter17;
+	u32 shadingParameter18;
+	u32 shadingParameter19;
+	u32 shadingParameter20;
+	u32 shadingParameter21;
+	u32 flashCtrl;		/*!< Flash control */
+	u32 lineCounter;	/*!< Line counter */
+	u32 frameCounter;	/*!< Frame counter */
+	u32 HPan;		/*!< Horizontal pan in decimation */
+	u32 HZoom;		/*!< Horizontal zoom in decimation */
+	u32 HSize;		/*!< Horizontal output size iIn decimation */
+	u32 VPan;		/*!< Vertical pan in decimation */
+	u32 VZoom;		/*!< Vertical zoom in decimation */
+	u32 VSize;		/*!< Vertical output size in decimation */
+} mt9v111_IFPReg;
+
+/*!
+ * mt9v111 Config structure
+ */
+typedef struct {
+	mt9v111_coreReg *coreReg;	/*!< Sensor Core Register Bank */
+	mt9v111_IFPReg *ifpReg;	/*!< IFP Register Bank */
+} mt9v111_conf;
+
+typedef struct {
+	u8 index;
+	u16 width;
+	u16 height;
+} mt9v111_image_format;
+
+#endif				/* MT9V111_H_  */
diff --git a/drivers/media/video/mxc/capture/mx27_csi.c b/drivers/media/video/mxc/capture/mx27_csi.c
new file mode 100644
index 000000000000..24fce05be110
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mx27_csi.c
@@ -0,0 +1,333 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_csi.c
+ *
+ * @brief CMOS Sensor interface functions
+ *
+ * @ingroup CSI
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <mach/clock.h>
+#include <mach/hardware.h>
+
+#include "mx27_csi.h"
+
+static csi_config_t g_csi_cfg;	/* csi hardware configuration */
+static bool gcsi_mclk_on = false;
+static csi_irq_callback_t g_callback = 0;
+static void *g_callback_data = 0;
+static struct clk csi_mclk;
+
+static irqreturn_t csi_irq_handler(int irq, void *data)
+{
+	unsigned long status = __raw_readl(CSI_CSISR);
+
+	__raw_writel(status, CSI_CSISR);
+	if (g_callback)
+		g_callback(g_callback_data, status);
+
+	pr_debug("CSI status = 0x%08lX\n", status);
+
+	return IRQ_HANDLED;
+}
+
+static void csihw_set_config(csi_config_t * cfg)
+{
+	unsigned val = 0;
+
+	/* control reg 1 */
+	val |= cfg->swap16_en ? BIT_SWAP16_EN : 0;
+	val |= cfg->ext_vsync ? BIT_EXT_VSYNC : 0;
+	val |= cfg->eof_int_en ? BIT_EOF_INT_EN : 0;
+	val |= cfg->prp_if_en ? BIT_PRP_IF_EN : 0;
+	val |= cfg->ccir_mode ? BIT_CCIR_MODE : 0;
+	val |= cfg->cof_int_en ? BIT_COF_INT_EN : 0;
+	val |= cfg->sf_or_inten ? BIT_SF_OR_INTEN : 0;
+	val |= cfg->rf_or_inten ? BIT_RF_OR_INTEN : 0;
+	val |= cfg->statff_level << SHIFT_STATFF_LEVEL;
+	val |= cfg->staff_inten ? BIT_STATFF_INTEN : 0;
+	val |= cfg->rxff_level << SHIFT_RXFF_LEVEL;
+	val |= cfg->rxff_inten ? BIT_RXFF_INTEN : 0;
+	val |= cfg->sof_pol ? BIT_SOF_POL : 0;
+	val |= cfg->sof_inten ? BIT_SOF_INTEN : 0;
+	val |= cfg->mclkdiv << SHIFT_MCLKDIV;
+	val |= cfg->hsync_pol ? BIT_HSYNC_POL : 0;
+	val |= cfg->ccir_en ? BIT_CCIR_EN : 0;
+	val |= cfg->mclken ? BIT_MCLKEN : 0;
+	val |= cfg->fcc ? BIT_FCC : 0;
+	val |= cfg->pack_dir ? BIT_PACK_DIR : 0;
+	val |= cfg->gclk_mode ? BIT_GCLK_MODE : 0;
+	val |= cfg->inv_data ? BIT_INV_DATA : 0;
+	val |= cfg->inv_pclk ? BIT_INV_PCLK : 0;
+	val |= cfg->redge ? BIT_REDGE : 0;
+
+	__raw_writel(val, CSI_CSICR1);
+
+	/* control reg 3 */
+	val = 0x0;
+	val |= cfg->csi_sup ? BIT_CSI_SUP : 0;
+	val |= cfg->zero_pack_en ? BIT_ZERO_PACK_EN : 0;
+	val |= cfg->ecc_int_en ? BIT_ECC_INT_EN : 0;
+	val |= cfg->ecc_auto_en ? BIT_ECC_AUTO_EN : 0;
+
+	__raw_writel(val, CSI_CSICR3);
+
+	/* rxfifo counter */
+	__raw_writel(cfg->rxcnt, CSI_CSIRXCNT);
+
+	/* update global config */
+	memcpy(&g_csi_cfg, cfg, sizeof(csi_config_t));
+}
+
+static void csihw_reset_frame_count(void)
+{
+	__raw_writel(__raw_readl(CSI_CSICR3) | BIT_FRMCNT_RST, CSI_CSICR3);
+}
+
+static void csihw_reset(void)
+{
+	csihw_reset_frame_count();
+	__raw_writel(CSICR1_RESET_VAL, CSI_CSICR1);
+	__raw_writel(CSICR2_RESET_VAL, CSI_CSICR2);
+	__raw_writel(CSICR3_RESET_VAL, CSI_CSICR3);
+}
+
+/*!
+ * csi_init_interface
+ *    Sets initial values for the CSI registers.
+ *    The width and height of the sensor and the actual frame size will be
+ *    set to the same values.
+ * @param       width        Sensor width
+ * @param       height       Sensor height
+ * @param       pixel_fmt    pixel format
+ * @param       sig          csi_signal_cfg_t
+ *
+ * @return      0 for success, -EINVAL for error
+ */
+int32_t csi_init_interface(uint16_t width, uint16_t height,
+			   uint32_t pixel_fmt, csi_signal_cfg_t sig)
+{
+	csi_config_t cfg;
+
+	/* Set the CSI_SENS_CONF register remaining fields */
+	cfg.swap16_en = 1;
+	cfg.ext_vsync = sig.ext_vsync;
+	cfg.eof_int_en = 0;
+	cfg.prp_if_en = 1;
+	cfg.ccir_mode = 0;
+	cfg.cof_int_en = 0;
+	cfg.sf_or_inten = 0;
+	cfg.rf_or_inten = 0;
+	cfg.statff_level = 0;
+	cfg.staff_inten = 0;
+	cfg.rxff_level = 2;
+	cfg.rxff_inten = 0;
+	cfg.sof_pol = 1;
+	cfg.sof_inten = 0;
+	cfg.mclkdiv = 0;
+	cfg.hsync_pol = 1;
+	cfg.ccir_en = 0;
+	cfg.mclken = gcsi_mclk_on ? 1 : 0;
+	cfg.fcc = 1;
+	cfg.pack_dir = 0;
+	cfg.gclk_mode = 1;
+	cfg.inv_data = sig.data_pol;
+	cfg.inv_pclk = sig.pixclk_pol;
+	cfg.redge = 1;
+	cfg.csicnt1_rsv = 0;
+
+	/* control reg 3 */
+	cfg.frmcnt = 0;
+	cfg.frame_reset = 0;
+	cfg.csi_sup = 0;
+	cfg.zero_pack_en = 0;
+	cfg.ecc_int_en = 0;
+	cfg.ecc_auto_en = 0;
+
+	csihw_set_config(&cfg);
+
+	return 0;
+}
+
+/*!
+ * csi_enable_prpif
+ *    Enable or disable CSI-PrP interface
+ * @param       enable        Non-zero to enable, zero to disable
+ */
+void csi_enable_prpif(uint32_t enable)
+{
+	if (enable) {
+		g_csi_cfg.prp_if_en = 1;
+		g_csi_cfg.sof_inten = 0;
+		g_csi_cfg.pack_dir = 0;
+	} else {
+		g_csi_cfg.prp_if_en = 0;
+		g_csi_cfg.sof_inten = 1;
+		g_csi_cfg.pack_dir = 1;
+	}
+
+	csihw_set_config(&g_csi_cfg);
+}
+
+/*!
+ * csi_enable_mclk
+ *
+ * @param       src         enum define which source to control the clk
+ *                          CSI_MCLK_VF CSI_MCLK_ENC CSI_MCLK_RAW CSI_MCLK_I2C
+ * @param       flag        true to enable mclk, false to disable mclk
+ * @param       wait        true to wait 100ms make clock stable, false not wait
+ *
+ * @return      0 for success
+ */
+int32_t csi_enable_mclk(int src, bool flag, bool wait)
+{
+	if (flag == true) {
+		clk_enable(&csi_mclk);
+		if (wait == true)
+			msleep(10);
+		pr_debug("Enable csi clock from source %d\n", src);
+		gcsi_mclk_on = true;
+	} else {
+		clk_disable(&csi_mclk);
+		pr_debug("Disable csi clock from source %d\n", src);
+		gcsi_mclk_on = false;
+	}
+
+	return 0;
+}
+
+/*!
+ * csi_read_mclk_flag
+ *
+ * @return  gcsi_mclk_source
+ */
+int csi_read_mclk_flag(void)
+{
+	return 0;
+}
+
+void csi_set_callback(csi_irq_callback_t callback, void *data)
+{
+	g_callback = callback;
+	g_callback_data = data;
+}
+
+static void _mclk_recalc(struct clk *clk)
+{
+	u32 div;
+
+	div = (__raw_readl(CSI_CSICR1) & BIT_MCLKDIV) >> SHIFT_MCLKDIV;
+	div = (div + 1) * 2;
+
+	clk->rate = clk->parent->rate / div;
+}
+
+static unsigned long _mclk_round_rate(struct clk *clk, unsigned long rate)
+{
+	/* Keep CSI divider and change parent clock */
+	if (clk->parent->round_rate) {
+		return clk->parent->round_rate(clk->parent, rate * 2);
+	}
+	return 0;
+}
+
+static int _mclk_set_rate(struct clk *clk, unsigned long rate)
+{
+	int ret = -EINVAL;
+
+	/* Keep CSI divider and change parent clock */
+	if (clk->parent->set_rate) {
+		ret = clk->parent->set_rate(clk->parent, rate * 2);
+		if (ret == 0) {
+			clk->rate = clk->parent->rate / 2;
+		}
+	}
+
+	return ret;
+}
+
+static int _mclk_enable(struct clk *clk)
+{
+	__raw_writel(__raw_readl(CSI_CSICR1) | BIT_MCLKEN, CSI_CSICR1);
+	return 0;
+}
+
+static void _mclk_disable(struct clk *clk)
+{
+	__raw_writel(__raw_readl(CSI_CSICR1) & ~BIT_MCLKEN, CSI_CSICR1);
+}
+
+static struct clk csi_mclk = {
+	.name = "csi_clk",
+	.recalc = _mclk_recalc,
+	.round_rate = _mclk_round_rate,
+	.set_rate = _mclk_set_rate,
+	.enable = _mclk_enable,
+	.disable = _mclk_disable,
+};
+
+int32_t __init csi_init_module(void)
+{
+	int ret = 0;
+	struct clk *per_clk;
+
+	per_clk = clk_get(NULL, "csi_perclk");
+	if (IS_ERR(per_clk))
+		return PTR_ERR(per_clk);
+	clk_put(per_clk);
+	csi_mclk.parent = per_clk;
+	clk_register(&csi_mclk);
+	clk_enable(per_clk);
+	csi_mclk.recalc(&csi_mclk);
+
+	csihw_reset();
+
+	/* interrupt enable */
+	ret = request_irq(MXC_INT_CSI, csi_irq_handler, 0, "csi", 0);
+	if (ret)
+		pr_debug("CSI error: irq request fail\n");
+
+	return ret;
+}
+
+void __exit csi_cleanup_module(void)
+{
+	/* free irq */
+	free_irq(MXC_INT_CSI, 0);
+
+	clk_disable(&csi_mclk);
+}
+
+module_init(csi_init_module);
+module_exit(csi_cleanup_module);
+
+EXPORT_SYMBOL(csi_init_interface);
+EXPORT_SYMBOL(csi_enable_mclk);
+EXPORT_SYMBOL(csi_read_mclk_flag);
+EXPORT_SYMBOL(csi_set_callback);
+EXPORT_SYMBOL(csi_enable_prpif);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MX27 CSI driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/mx27_csi.h b/drivers/media/video/mxc/capture/mx27_csi.h
new file mode 100644
index 000000000000..9bd99781e626
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mx27_csi.h
@@ -0,0 +1,167 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_csi.h
+ *
+ * @brief CMOS Sensor interface functions
+ *
+ * @ingroup CSI
+ */
+
+#ifndef MX27_CSI_H
+#define MX27_CSI_H
+
+#include <linux/io.h>
+
+/* reset values */
+#define CSICR1_RESET_VAL	0x40000800
+#define CSICR2_RESET_VAL	0x0
+#define CSICR3_RESET_VAL	0x0
+
+/* csi control reg 1 */
+#define BIT_SWAP16_EN		(0x1 << 31)
+#define BIT_EXT_VSYNC		(0x1 << 30)
+#define BIT_EOF_INT_EN		(0x1 << 29)
+#define BIT_PRP_IF_EN		(0x1 << 28)
+#define BIT_CCIR_MODE		(0x1 << 27)
+#define BIT_COF_INT_EN		(0x1 << 26)
+#define BIT_SF_OR_INTEN		(0x1 << 25)
+#define BIT_RF_OR_INTEN		(0x1 << 24)
+#define BIT_STATFF_LEVEL	(0x3 << 22)
+#define BIT_STATFF_INTEN	(0x1 << 21)
+#define BIT_RXFF_LEVEL		(0x3 << 19)
+#define BIT_RXFF_INTEN		(0x1 << 18)
+#define BIT_SOF_POL		(0x1 << 17)
+#define BIT_SOF_INTEN		(0x1 << 16)
+#define BIT_MCLKDIV		(0xF << 12)
+#define BIT_HSYNC_POL		(0x1 << 11)
+#define BIT_CCIR_EN		(0x1 << 10)
+#define BIT_MCLKEN		(0x1 << 9)
+#define BIT_FCC			(0x1 << 8)
+#define BIT_PACK_DIR		(0x1 << 7)
+#define BIT_CLR_STATFIFO	(0x1 << 6)
+#define BIT_CLR_RXFIFO		(0x1 << 5)
+#define BIT_GCLK_MODE		(0x1 << 4)
+#define BIT_INV_DATA		(0x1 << 3)
+#define BIT_INV_PCLK		(0x1 << 2)
+#define BIT_REDGE		(0x1 << 1)
+
+#define SHIFT_STATFF_LEVEL	22
+#define SHIFT_RXFF_LEVEL	19
+#define SHIFT_MCLKDIV		12
+
+/* control reg 3 */
+#define BIT_FRMCNT		(0xFFFF << 16)
+#define BIT_FRMCNT_RST		(0x1 << 15)
+#define BIT_CSI_SUP		(0x1 << 3)
+#define BIT_ZERO_PACK_EN	(0x1 << 2)
+#define BIT_ECC_INT_EN		(0x1 << 1)
+#define BIT_ECC_AUTO_EN		(0x1)
+
+#define SHIFT_FRMCNT		16
+
+/* csi status reg */
+#define BIT_SFF_OR_INT		(0x1 << 25)
+#define BIT_RFF_OR_INT		(0x1 << 24)
+#define BIT_STATFF_INT		(0x1 << 21)
+#define BIT_RXFF_INT		(0x1 << 18)
+#define BIT_EOF_INT		(0x1 << 17)
+#define BIT_SOF_INT		(0x1 << 16)
+#define BIT_F2_INT		(0x1 << 15)
+#define BIT_F1_INT		(0x1 << 14)
+#define BIT_COF_INT		(0x1 << 13)
+#define BIT_ECC_INT		(0x1 << 1)
+#define BIT_DRDY		(0x1 << 0)
+
+#define CSI_MCLK_VF		1
+#define CSI_MCLK_ENC		2
+#define CSI_MCLK_RAW		4
+#define CSI_MCLK_I2C		8
+
+#define CSI_CSICR1		(IO_ADDRESS(CSI_BASE_ADDR))
+#define CSI_CSICR2		(IO_ADDRESS(CSI_BASE_ADDR + 0x4))
+#define CSI_CSISR		(IO_ADDRESS(CSI_BASE_ADDR + 0x8))
+#define CSI_STATFIFO		(IO_ADDRESS(CSI_BASE_ADDR + 0xC))
+#define CSI_CSIRXFIFO		(IO_ADDRESS(CSI_BASE_ADDR + 0x10))
+#define CSI_CSIRXCNT		(IO_ADDRESS(CSI_BASE_ADDR + 0x14))
+#define CSI_CSICR3		(IO_ADDRESS(CSI_BASE_ADDR + 0x1C))
+
+#define CSI_CSIRXFIFO_PHYADDR	(CSI_BASE_ADDR + 0x10)
+
+static __inline void csi_clear_status(unsigned long status)
+{
+	__raw_writel(status, CSI_CSISR);
+}
+
+typedef struct {
+	unsigned data_width:3;
+	unsigned clk_mode:2;
+	unsigned ext_vsync:1;
+	unsigned Vsync_pol:1;
+	unsigned Hsync_pol:1;
+	unsigned pixclk_pol:1;
+	unsigned data_pol:1;
+	unsigned sens_clksrc:1;
+} csi_signal_cfg_t;
+
+typedef struct {
+	/* control reg 1 */
+	unsigned int swap16_en:1;
+	unsigned int ext_vsync:1;
+	unsigned int eof_int_en:1;
+	unsigned int prp_if_en:1;
+	unsigned int ccir_mode:1;
+	unsigned int cof_int_en:1;
+	unsigned int sf_or_inten:1;
+	unsigned int rf_or_inten:1;
+	unsigned int statff_level:2;
+	unsigned int staff_inten:1;
+	unsigned int rxff_level:2;
+	unsigned int rxff_inten:1;
+	unsigned int sof_pol:1;
+	unsigned int sof_inten:1;
+	unsigned int mclkdiv:4;
+	unsigned int hsync_pol:1;
+	unsigned int ccir_en:1;
+	unsigned int mclken:1;
+	unsigned int fcc:1;
+	unsigned int pack_dir:1;
+	unsigned int gclk_mode:1;
+	unsigned int inv_data:1;
+	unsigned int inv_pclk:1;
+	unsigned int redge:1;
+	unsigned int csicnt1_rsv:1;
+
+	/* control reg 3 */
+	unsigned int frmcnt:16;
+	unsigned int frame_reset:1;
+	unsigned int csi_sup:1;
+	unsigned int zero_pack_en:1;
+	unsigned int ecc_int_en:1;
+	unsigned int ecc_auto_en:1;
+
+	/* fifo counter */
+	unsigned int rxcnt;
+} csi_config_t;
+
+typedef void (*csi_irq_callback_t) (void *data, unsigned long status);
+
+int32_t csi_enable_mclk(int src, bool flag, bool wait);
+int32_t csi_init_interface(uint16_t width, uint16_t height,
+			   uint32_t pixel_fmt, csi_signal_cfg_t sig);
+int csi_read_mclk_flag(void);
+void csi_set_callback(csi_irq_callback_t callback, void *data);
+void csi_enable_prpif(uint32_t enable);
+
+#endif
diff --git a/drivers/media/video/mxc/capture/mx27_prp.h b/drivers/media/video/mxc/capture/mx27_prp.h
new file mode 100644
index 000000000000..e32e9029daff
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mx27_prp.h
@@ -0,0 +1,310 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_prp.h
+ *
+ * @brief Header file for MX27 V4L2 capture driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#ifndef __MX27_PRP_H__
+#define __MX27_PRP_H__
+
+#define PRP_REG(ofs)	(IO_ADDRESS(EMMA_BASE_ADDR) + ofs)
+
+/* Register definitions of PrP */
+#define PRP_CNTL			PRP_REG(0x00)
+#define PRP_INTRCNTL	 		PRP_REG(0x04)
+#define PRP_INTRSTATUS			PRP_REG(0x08)
+#define PRP_SOURCE_Y_PTR		PRP_REG(0x0C)
+#define PRP_SOURCE_CB_PTR		PRP_REG(0x10)
+#define PRP_SOURCE_CR_PTR		PRP_REG(0x14)
+#define PRP_DEST_RGB1_PTR		PRP_REG(0x18)
+#define PRP_DEST_RGB2_PTR		PRP_REG(0x1C)
+#define PRP_DEST_Y_PTR			PRP_REG(0x20)
+#define PRP_DEST_CB_PTR			PRP_REG(0x24)
+#define PRP_DEST_CR_PTR			PRP_REG(0x28)
+#define PRP_SOURCE_FRAME_SIZE  		PRP_REG(0x2C)
+#define PRP_CH1_LINE_STRIDE		PRP_REG(0x30)
+#define PRP_SRC_PIXEL_FORMAT_CNTL	PRP_REG(0x34)
+#define PRP_CH1_PIXEL_FORMAT_CNTL	PRP_REG(0x38)
+#define PRP_CH1_OUT_IMAGE_SIZE		PRP_REG(0x3C)
+#define PRP_CH2_OUT_IMAGE_SIZE		PRP_REG(0x40)
+#define PRP_SOURCE_LINE_STRIDE		PRP_REG(0x44)
+#define PRP_CSC_COEF_012		PRP_REG(0x48)
+#define PRP_CSC_COEF_345		PRP_REG(0x4C)
+#define PRP_CSC_COEF_678		PRP_REG(0x50)
+#define PRP_CH1_RZ_HORI_COEF1		PRP_REG(0x54)
+#define PRP_CH1_RZ_HORI_COEF2		PRP_REG(0x58)
+#define PRP_CH1_RZ_HORI_VALID		PRP_REG(0x5C)
+#define PRP_CH1_RZ_VERT_COEF1		PRP_REG(0x60)
+#define PRP_CH1_RZ_VERT_COEF2		PRP_REG(0x64)
+#define PRP_CH1_RZ_VERT_VALID		PRP_REG(0x68)
+#define PRP_CH2_RZ_HORI_COEF1		PRP_REG(0x6C)
+#define PRP_CH2_RZ_HORI_COEF2		PRP_REG(0x70)
+#define PRP_CH2_RZ_HORI_VALID		PRP_REG(0x74)
+#define PRP_CH2_RZ_VERT_COEF1		PRP_REG(0x78)
+#define PRP_CH2_RZ_VERT_COEF2		PRP_REG(0x7C)
+#define PRP_CH2_RZ_VERT_VALID		PRP_REG(0x80)
+
+#define B_SET(b)			(1 << (b))
+
+/* Bit definitions for PrP control register */
+#define PRP_CNTL_RSTVAL			0x28
+#define PRP_CNTL_CH1EN			B_SET(0)
+#define PRP_CNTL_CH2EN			B_SET(1)
+#define PRP_CNTL_CSI			B_SET(2)
+#define PRP_CNTL_IN_32			B_SET(3)
+#define PRP_CNTL_IN_RGB			B_SET(4)
+#define PRP_CNTL_IN_YUV420		0
+#define PRP_CNTL_IN_YUV422		PRP_CNTL_IN_32
+#define PRP_CNTL_IN_RGB16		PRP_CNTL_IN_RGB
+#define PRP_CNTL_IN_RGB32		(PRP_CNTL_IN_RGB | PRP_CNTL_IN_32)
+#define PRP_CNTL_CH1_RGB8		0
+#define PRP_CNTL_CH1_RGB16		B_SET(5)
+#define PRP_CNTL_CH1_RGB32		B_SET(6)
+#define PRP_CNTL_CH1_YUV422		(B_SET(5) | B_SET(6))
+#define PRP_CNTL_CH2_YUV420		0
+#define PRP_CNTL_CH2_YUV422		B_SET(7)
+#define PRP_CNTL_CH2_YUV444		B_SET(8)
+#define PRP_CNTL_CH1_LOOP		B_SET(9)
+#define PRP_CNTL_CH2_LOOP		B_SET(10)
+#define PRP_CNTL_AUTODROP		B_SET(11)
+#define PRP_CNTL_RST			B_SET(12)
+#define PRP_CNTL_CNTREN			B_SET(13)
+#define PRP_CNTL_WINEN			B_SET(14)
+#define PRP_CNTL_UNCHAIN		B_SET(15)
+#define PRP_CNTL_IN_SKIP_NONE		0
+#define PRP_CNTL_IN_SKIP_1_2		B_SET(16)
+#define PRP_CNTL_IN_SKIP_1_3		B_SET(17)
+#define PRP_CNTL_IN_SKIP_2_3		(B_SET(16) | B_SET(17))
+#define PRP_CNTL_IN_SKIP_1_4		B_SET(18)
+#define PRP_CNTL_IN_SKIP_3_4		(B_SET(16) | B_SET(18))
+#define PRP_CNTL_IN_SKIP_2_5		(B_SET(17) | B_SET(18))
+#define PRP_CNTL_IN_SKIP_3_5		(B_SET(16) | B_SET(17) | B_SET(18))
+#define PRP_CNTL_CH1_SKIP_NONE		0
+#define PRP_CNTL_CH1_SKIP_1_2		B_SET(19)
+#define PRP_CNTL_CH1_SKIP_1_3		B_SET(20)
+#define PRP_CNTL_CH1_SKIP_2_3		(B_SET(19) | B_SET(20))
+#define PRP_CNTL_CH1_SKIP_1_4		B_SET(21)
+#define PRP_CNTL_CH1_SKIP_3_4		(B_SET(19) | B_SET(21))
+#define PRP_CNTL_CH1_SKIP_2_5		(B_SET(20) | B_SET(21))
+#define PRP_CNTL_CH1_SKIP_3_5		(B_SET(19) | B_SET(20) | B_SET(21))
+#define PRP_CNTL_CH2_SKIP_NONE		0
+#define PRP_CNTL_CH2_SKIP_1_2		B_SET(22)
+#define PRP_CNTL_CH2_SKIP_1_3		B_SET(23)
+#define PRP_CNTL_CH2_SKIP_2_3		(B_SET(22) | B_SET(23))
+#define PRP_CNTL_CH2_SKIP_1_4		B_SET(24)
+#define PRP_CNTL_CH2_SKIP_3_4		(B_SET(22) | B_SET(24))
+#define PRP_CNTL_CH2_SKIP_2_5		(B_SET(23) | B_SET(24))
+#define PRP_CNTL_CH2_SKIP_3_5		(B_SET(22) | B_SET(23) | B_SET(24))
+#define PRP_CNTL_FIFO_I128		0
+#define PRP_CNTL_FIFO_I96		B_SET(25)
+#define PRP_CNTL_FIFO_I64		B_SET(26)
+#define PRP_CNTL_FIFO_I32		(B_SET(25) | B_SET(26))
+#define PRP_CNTL_FIFO_O64		0
+#define PRP_CNTL_FIFO_O48		B_SET(27)
+#define PRP_CNTL_FIFO_O32		B_SET(28)
+#define PRP_CNTL_FIFO_O16		(B_SET(27) | B_SET(28))
+#define PRP_CNTL_CH2B1			B_SET(29)
+#define PRP_CNTL_CH2B2			B_SET(30)
+#define PRP_CNTL_CH2_FLOWEN		B_SET(31)
+
+/* Bit definitions for PrP interrupt control register */
+#define PRP_INTRCNTL_RDERR		B_SET(0)
+#define PRP_INTRCNTL_CH1WERR		B_SET(1)
+#define PRP_INTRCNTL_CH2WERR		B_SET(2)
+#define PRP_INTRCNTL_CH1FC		B_SET(3)
+#define PRP_INTRCNTL_CH2FC		B_SET(5)
+#define PRP_INTRCNTL_LBOVF		B_SET(7)
+#define PRP_INTRCNTL_CH2OVF		B_SET(8)
+
+/* Bit definitions for PrP interrupt status register */
+#define PRP_INTRSTAT_RDERR		B_SET(0)
+#define PRP_INTRSTAT_CH1WERR		B_SET(1)
+#define PRP_INTRSTAT_CH2WERR		B_SET(2)
+#define PRP_INTRSTAT_CH2BUF2		B_SET(3)
+#define PRP_INTRSTAT_CH2BUF1		B_SET(4)
+#define PRP_INTRSTAT_CH1BUF2		B_SET(5)
+#define PRP_INTRSTAT_CH1BUF1		B_SET(6)
+#define PRP_INTRSTAT_LBOVF		B_SET(7)
+#define PRP_INTRSTAT_CH2OVF		B_SET(8)
+
+#define PRP_CHANNEL_1		0x1
+#define PRP_CHANNEL_2		0x2
+
+/* PRP-CSI config */
+#define PRP_CSI_EN		0x80
+#define PRP_CSI_LOOP		(0x40 | PRP_CSI_EN)
+#define PRP_CSI_IRQ_FRM		(0x08 | PRP_CSI_LOOP)
+#define PRP_CSI_IRQ_CH1ERR	(0x10 | PRP_CSI_LOOP)
+#define PRP_CSI_IRQ_CH2ERR	(0x20 | PRP_CSI_LOOP)
+#define PRP_CSI_IRQ_ALL		(0x38 | PRP_CSI_LOOP)
+#define PRP_CSI_SKIP_NONE	0
+#define PRP_CSI_SKIP_1OF2	1
+#define PRP_CSI_SKIP_1OF3	2
+#define PRP_CSI_SKIP_2OF3	3
+#define PRP_CSI_SKIP_1OF4	4
+#define PRP_CSI_SKIP_3OF4	5
+#define PRP_CSI_SKIP_2OF5	6
+#define PRP_CSI_SKIP_4OF5	7
+
+#define PRP_PIXIN_RGB565	0x2CA00565
+#define PRP_PIXIN_RGB888	0x41000888
+#define PRP_PIXIN_YUV420	0
+#define PRP_PIXIN_YUYV		0x22000888
+#define PRP_PIXIN_YVYU		0x20100888
+#define PRP_PIXIN_UYVY		0x03080888
+#define PRP_PIXIN_VYUY		0x01180888
+#define PRP_PIXIN_YUV422	0x62080888
+
+#define PRP_PIX1_RGB332		0x14400322
+#define PRP_PIX1_RGB565		0x2CA00565
+#define PRP_PIX1_RGB888		0x41000888
+#define PRP_PIX1_YUYV		0x62000888
+#define PRP_PIX1_YVYU		0x60100888
+#define PRP_PIX1_UYVY		0x43080888
+#define PRP_PIX1_VYUY		0x41180888
+#define PRP_PIX1_UNUSED		0
+
+#define PRP_PIX2_YUV420		0
+#define PRP_PIX2_YUV422		1
+#define PRP_PIX2_YUV444		4
+#define PRP_PIX2_UNUSED		8
+
+#define PRP_ALGO_WIDTH_ANY	0
+#define PRP_ALGO_HEIGHT_ANY	0
+#define PRP_ALGO_WIDTH_BIL	1
+#define PRP_ALGO_WIDTH_AVG	2
+#define PRP_ALGO_HEIGHT_BIL	4
+#define PRP_ALGO_HEIGHT_AVG	8
+#define PRP_ALGO_BYPASS		0x10
+
+typedef struct _emma_prp_ratio {
+	unsigned short num;
+	unsigned short den;
+} emma_prp_ratio;
+
+/*
+ * The following definitions are for resizing. Definition values must not
+ * be changed otherwise decision logic will be wrong.
+ */
+#define SCALE_RETRY	16	/* retry times if ratio is not supported */
+
+#define BC_COEF		3
+#define MAX_TBL		20
+#define SZ_COEF		(1 << BC_COEF)
+
+#define ALGO_AUTO	0
+#define ALGO_BIL	1
+#define ALGO_AVG	2
+
+typedef struct {
+	char tbl[20];		/* table entries */
+	char len;		/* table length used */
+	char algo;		/* ALGO_xxx */
+	char ratio[20];		/* ratios used */
+} scale_t;
+
+/*
+ * structure for prp scaling.
+ * algorithm - bilinear or averaging for each axis
+ * PRP_ALGO_WIDTH_x | PRP_ALGO_HEIGHT_x | PRP_ALGO_BYPASS
+ * PRP_ALGO_BYPASS - Ch1 will not use Ch2 scaling with this flag
+ */
+typedef struct _emma_prp_scale {
+	unsigned char algo;
+	emma_prp_ratio width;
+	emma_prp_ratio height;
+} emma_prp_scale;
+
+typedef struct emma_prp_cfg {
+	unsigned int in_pix;	/* PRP_PIXIN_xxx */
+	unsigned short in_width;	/* image width, 32 - 2044 */
+	unsigned short in_height;	/* image height, 32 - 2044 */
+	unsigned char in_csi;	/* PRP_CSI_SKIP_x | PRP_CSI_LOOP */
+	unsigned short in_line_stride;	/* in_line_stride and in_line_skip */
+	unsigned short in_line_skip;	/* allow cropping from CSI */
+	unsigned int in_ptr;	/* bus address */
+	/*
+	 * in_csc[9] = 1 -> Y-16
+	 * if in_csc[1..9] == 0
+	 *      in_csc[0] represents YUV range 0-3 = A0,A1,B0,B1;
+	 * else
+	 *      in_csc[0..9] represents either format
+	 */
+	unsigned short in_csc[10];
+
+	unsigned char ch2_pix;	/* PRP_PIX2_xxx */
+	emma_prp_scale ch2_scale;	/* resizing paramters */
+	unsigned short ch2_width;	/* 4-2044, 0 = scaled */
+	unsigned short ch2_height;	/* 4-2044, 0 = scaled */
+	unsigned int ch2_ptr;	/* bus addr */
+	unsigned int ch2_ptr2;	/* bus addr for 2nd buf (loop mode) */
+	unsigned char ch2_csi;	/* PRP_CSI_SKIP_x | PRP_CSI_LOOP */
+
+	unsigned int ch1_pix;	/* PRP_PIX1_xxx */
+	emma_prp_scale ch1_scale;	/* resizing parameters */
+	unsigned short ch1_width;	/* 4-2044, 0 = scaled */
+	unsigned short ch1_height;	/* 4-2044, 0 = scaled */
+	unsigned short ch1_stride;	/* 4-4088, 0 = ch1_width */
+	unsigned int ch1_ptr;	/* bus addr */
+	unsigned int ch1_ptr2;	/* bus addr for 2nd buf (loop mode) */
+	unsigned char ch1_csi;	/* PRP_CSI_SKIP_x | PRP_CSI_LOOP */
+
+	/*
+	 * channel resizing coefficients
+	 * scale[0] for channel 1 width
+	 * scale[1] for channel 1 height
+	 * scale[2] for channel 2 width
+	 * scale[3] for channel 2 height
+	 */
+	scale_t scale[4];
+} emma_prp_cfg;
+
+int prphw_reset(void);
+int prphw_enable(int channel);
+int prphw_disable(int channel);
+int prphw_inptr(emma_prp_cfg *);
+int prphw_ch1ptr(emma_prp_cfg *);
+int prphw_ch1ptr2(emma_prp_cfg *);
+int prphw_ch2ptr(emma_prp_cfg *);
+int prphw_ch2ptr2(emma_prp_cfg *);
+int prphw_cfg(emma_prp_cfg *);
+int prphw_isr(void);
+void prphw_init(void);
+void prphw_exit(void);
+
+/*
+ * scale	out	coefficient table
+ * din		in	scale numerator
+ * dout		in	scale denominator
+ * inv		in	pre-scale dimension
+ * vout		in/out	post-scale output dimension
+ * pout		out	post-scale internal dimension [opt]
+ * retry	in	retry times (round the output length) when need
+ */
+int prp_scale(scale_t * pscale, int din, int dout, int inv,
+	      unsigned short *vout, unsigned short *pout, int retry);
+
+int prp_init(void *dev_id);
+void prp_exit(void *dev_id);
+int prp_enc_select(void *data);
+int prp_enc_deselect(void *data);
+int prp_vf_select(void *data);
+int prp_vf_deselect(void *data);
+int prp_still_select(void *data);
+int prp_still_deselect(void *data);
+
+#endif				/* __MX27_PRP_H__ */
diff --git a/drivers/media/video/mxc/capture/mx27_prphw.c b/drivers/media/video/mxc/capture/mx27_prphw.c
new file mode 100644
index 000000000000..c56a6df1716e
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mx27_prphw.c
@@ -0,0 +1,1099 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_prphw.c
+ *
+ * @brief MX27 Video For Linux 2 capture driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/clk.h>
+#include <asm/io.h>
+#include <linux/delay.h>
+
+#include "mx27_prp.h"
+
+#define PRP_MIN_IN_WIDTH	32
+#define PRP_MAX_IN_WIDTH	2044
+#define PRP_MIN_IN_HEIGHT	32
+#define PRP_MAX_IN_HEIGHT	2044
+
+typedef struct _coeff_t {
+	unsigned long coeff[2];
+	unsigned long cntl;
+} coeff_t[2][2];
+
+static coeff_t *PRP_RSZ_COEFF = (coeff_t *) PRP_CH1_RZ_HORI_COEF1;
+
+static unsigned char scale_get(scale_t * t,
+			       unsigned char *i, unsigned char *out);
+static int gcd(int x, int y);
+static int ratio(int x, int y, int *den);
+static int prp_scale_bilinear(scale_t * t, int coeff, int base, int nxt);
+static int prp_scale_ave(scale_t * t, unsigned char base);
+static int ave_scale(scale_t * t, int inv, int outv);
+static int scale(scale_t * t, int inv, int outv);
+
+/*!
+ * @param t	table
+ * @param i	table index
+ * @param out	bilinear	# input pixels to advance
+ *		average		whether result is ready for output
+ * @return	coefficient
+*/
+static unsigned char scale_get(scale_t * t, unsigned char *i,
+			       unsigned char *out)
+{
+	unsigned char c;
+
+	c = t->tbl[*i];
+	(*i)++;
+	*i %= t->len;
+
+	if (out) {
+		if (t->algo == ALGO_BIL) {
+			for ((*out) = 1;
+			     (*i) && ((*i) < t->len) && !t->tbl[(*i)]; (*i)++) {
+				(*out)++;
+			}
+			if ((*i) == t->len)
+				(*i) = 0;
+		} else
+			*out = c >> BC_COEF;
+	}
+
+	c &= SZ_COEF - 1;
+
+	if (c == SZ_COEF - 1)
+		c = SZ_COEF;
+
+	return c;
+}
+
+/*!
+ * @brief Get maximum common divisor.
+ * @param x	First input value
+ * @param y	Second input value
+ * @return	Maximum common divisor of x and y
+ */
+static int gcd(int x, int y)
+{
+	int k;
+
+	if (x < y) {
+		k = x;
+		x = y;
+		y = k;
+	}
+
+	while ((k = x % y)) {
+		x = y;
+		y = k;
+	}
+
+	return y;
+}
+
+/*!
+ * @brief Get ratio.
+ * @param x	First input value
+ * @param y	Second input value
+ * @param den	Denominator of the ratio (corresponding to y)
+ * @return	Numerator of the ratio (corresponding to x)
+ */
+static int ratio(int x, int y, int *den)
+{
+	int g;
+
+	if (!x || !y)
+		return 0;
+
+	g = gcd(x, y);
+	*den = y / g;
+
+	return x / g;
+}
+
+/*!
+ * @brief Build PrP coefficient entry based on bilinear algorithm
+ *
+ * @param t	The pointer to scale_t structure
+ * @param coeff	The weighting coefficient
+ * @param base	The base of the coefficient
+ * @param nxt	Number of pixels to be read
+ *
+ * @return	The length of current coefficient table on success
+ *		-1 on failure
+ */
+static int prp_scale_bilinear(scale_t * t, int coeff, int base, int nxt)
+{
+	int i;
+
+	if (t->len >= sizeof(t->tbl))
+		return -1;
+
+	coeff = ((coeff << BC_COEF) + (base >> 1)) / base;
+	if (coeff >= SZ_COEF - 1)
+		coeff--;
+
+	coeff |= SZ_COEF;
+	t->tbl[(int)t->len++] = (unsigned char)coeff;
+
+	for (i = 1; i < nxt; i++) {
+		if (t->len >= MAX_TBL)
+			return -1;
+
+		t->tbl[(int)t->len++] = 0;
+	}
+
+	return t->len;
+}
+
+#define _bary(name)	static const unsigned char name[]
+
+_bary(c1) = {
+7};
+
+_bary(c2) = {
+4, 4};
+
+_bary(c3) = {
+2, 4, 2};
+
+_bary(c4) = {
+2, 2, 2, 2};
+
+_bary(c5) = {
+1, 2, 2, 2, 1};
+
+_bary(c6) = {
+1, 1, 2, 2, 1, 1};
+
+_bary(c7) = {
+1, 1, 1, 2, 1, 1, 1};
+
+_bary(c8) = {
+1, 1, 1, 1, 1, 1, 1, 1};
+
+_bary(c9) = {
+1, 1, 1, 1, 1, 1, 1, 1, 0};
+
+_bary(c10) = {
+0, 1, 1, 1, 1, 1, 1, 1, 1, 0};
+
+_bary(c11) = {
+0, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0};
+
+_bary(c12) = {
+0, 1, 1, 0, 1, 1, 1, 1, 0, 1, 1, 0};
+
+_bary(c13) = {
+0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0};
+
+_bary(c14) = {
+0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 0, 1, 0};
+
+_bary(c15) = {
+0, 1, 0, 1, 0, 1, 1, 0, 1, 1, 0, 1, 0, 1, 0};
+
+_bary(c16) = {
+1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0};
+
+_bary(c17) = {
+0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0};
+
+_bary(c18) = {
+0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0};
+
+_bary(c19) = {
+0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0};
+
+_bary(c20) = {
+0, 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0};
+
+static const unsigned char *ave_coeff[] = {
+	c1, c2, c3, c4, c5, c6, c7, c8, c9, c10,
+	c11, c12, c13, c14, c15, c16, c17, c18, c19, c20
+};
+
+/*!
+ * @brief Build PrP coefficient table based on average algorithm
+ *
+ * @param t	The pointer to scale_t structure
+ * @param base	The base of the coefficient
+ *
+ * @return	The length of current coefficient table on success
+ *		-1 on failure
+ */
+static int prp_scale_ave(scale_t * t, unsigned char base)
+{
+	if (t->len + base > sizeof(t->tbl))
+		return -1;
+
+	memcpy(&t->tbl[(int)t->len], ave_coeff[(int)base - 1], base);
+	t->len = (unsigned char)(t->len + base);
+	t->tbl[t->len - 1] |= SZ_COEF;
+
+	return t->len;
+}
+
+/*!
+ * @brief Build PrP coefficient table based on average algorithm
+ *
+ * @param t	The pointer to scale_t structure
+ * @param inv	Input resolution
+ * @param outv	Output resolution
+ *
+ * @return	The length of current coefficient table on success
+ *		-1 on failure
+ */
+static int ave_scale(scale_t * t, int inv, int outv)
+{
+	int ratio_count;
+
+	ratio_count = 0;
+	if (outv != 1) {
+		unsigned char a[20];
+		int v;
+
+		/* split n:m into multiple n[i]:1 */
+		for (v = 0; v < outv; v++)
+			a[v] = (unsigned char)(inv / outv);
+
+		inv %= outv;
+		if (inv) {
+			/* find start of next layer */
+			v = (outv - inv) >> 1;
+			inv += v;
+			for (; v < inv; v++)
+				a[v]++;
+		}
+
+		for (v = 0; v < outv; v++) {
+			if (prp_scale_ave(t, a[v]) < 0)
+				return -1;
+
+			t->ratio[ratio_count] = a[v];
+			ratio_count++;
+		}
+	} else if (prp_scale_ave(t, inv) < 0) {
+		return -1;
+	} else {
+		t->ratio[ratio_count++] = (char)inv;
+		ratio_count++;
+	}
+
+	return t->len;
+}
+
+/*!
+ * @brief Build PrP coefficient table
+ *
+ * @param t	The pointer to scale_t structure
+ * @param inv	input resolution reduced ratio
+ * @param outv	output resolution reduced ratio
+ *
+ * @return	The length of current coefficient table on success
+ *		-1 on failure
+ */
+static int scale(scale_t * t, int inv, int outv)
+{
+	int v;			/* overflow counter */
+	int coeff, nxt;		/* table output */
+
+	t->len = 0;
+	if (t->algo == ALGO_AUTO) {
+		/* automatic choice - bilinear for shrinking less than 2:1 */
+		t->algo = ((outv != inv) && ((2 * outv) > inv)) ?
+		    ALGO_BIL : ALGO_AVG;
+	}
+
+	/* 1:1 resize must use averaging, bilinear will hang */
+	if ((inv == outv) && (t->algo == ALGO_BIL)) {
+		pr_debug("Warning: 1:1 resize must use averaging algo\n");
+		t->algo = ALGO_AVG;
+	}
+
+	memset(t->tbl, 0, sizeof(t->tbl));
+	if (t->algo == ALGO_BIL) {
+		t->ratio[0] = (char)inv;
+		t->ratio[1] = (char)outv;
+	} else
+		memset(t->ratio, 0, sizeof(t->ratio));
+
+	if (inv == outv) {
+		/* force scaling */
+		t->ratio[0] = 1;
+		if (t->algo == ALGO_BIL)
+			t->ratio[1] = 1;
+
+		return prp_scale_ave(t, 1);
+	}
+
+	if (inv < outv) {
+		pr_debug("Upscaling not supported %d:%d\n", inv, outv);
+		return -1;
+	}
+
+	if (t->algo != ALGO_BIL)
+		return ave_scale(t, inv, outv);
+
+	v = 0;
+	if (inv >= 2 * outv) {
+		/* downscale: >=2:1 bilinear approximation */
+		coeff = inv - 2 * outv;
+		v = 0;
+		nxt = 0;
+		do {
+			v += coeff;
+			nxt = 2;
+			while (v >= outv) {
+				v -= outv;
+				nxt++;
+			}
+
+			if (prp_scale_bilinear(t, 1, 2, nxt) < 0)
+				return -1;
+		} while (v);
+	} else {
+		/* downscale: bilinear */
+		int in_pos_inc = 2 * outv;
+		int out_pos = inv;
+		int out_pos_inc = 2 * inv;
+		int init_carry = inv - outv;
+		int carry = init_carry;
+
+		v = outv + in_pos_inc;
+		do {
+			coeff = v - out_pos;
+			out_pos += out_pos_inc;
+			carry += out_pos_inc;
+			for (nxt = 0; v < out_pos; nxt++) {
+				v += in_pos_inc;
+				carry -= in_pos_inc;
+			}
+			if (prp_scale_bilinear(t, coeff, in_pos_inc, nxt) < 0)
+				return -1;
+		} while (carry != init_carry);
+	}
+	return t->len;
+}
+
+/*!
+ * @brief Build PrP coefficient table
+ *
+ * @param pscale	The pointer to scale_t structure which holdes
+ * 			coefficient tables
+ * @param din		Scale ratio numerator
+ * @param dout		Scale ratio denominator
+ * @param inv		Input resolution
+ * @param vout		Output resolution
+ * @param pout		Internal output resolution
+ * @param retry		Retry times (round the output length) when need
+ *
+ * @return		Zero on success, others on failure
+ */
+int prp_scale(scale_t * pscale, int din, int dout, int inv,
+	      unsigned short *vout, unsigned short *pout, int retry)
+{
+	int num;
+	int den;
+	unsigned short outv;
+
+	/* auto-generation of values */
+	if (!(dout && din)) {
+		if (!*vout)
+			dout = din = 1;
+		else {
+			din = inv;
+			dout = *vout;
+		}
+	}
+
+	if (din < dout) {
+		pr_debug("Scale err, unsupported ratio %d : %d\n", din, dout);
+		return -1;
+	}
+
+      lp_retry:
+	num = ratio(din, dout, &den);
+	if (!num) {
+		pr_debug("Scale err, unsupported ratio %d : %d\n", din, dout);
+		return -1;
+	}
+
+	if (num > MAX_TBL || scale(pscale, num, den) < 0) {
+		dout++;
+		if (retry--)
+			goto lp_retry;
+
+		pr_debug("Scale err, unsupported ratio %d : %d\n", num, den);
+		return -1;
+	}
+
+	if (pscale->algo == ALGO_BIL) {
+		unsigned char i, j, k;
+
+		outv =
+		    (unsigned short)(inv / pscale->ratio[0] * pscale->ratio[1]);
+		inv %= pscale->ratio[0];
+		for (i = j = 0; inv > 0; j++) {
+			unsigned char nxt;
+
+			k = scale_get(pscale, &i, &nxt);
+			if (inv == 1 && k < SZ_COEF) {
+				/* needs 2 pixels for this output */
+				break;
+			}
+			inv -= nxt;
+		}
+		outv = outv + j;
+	} else {
+		unsigned char i, tot;
+
+		for (tot = i = 0; pscale->ratio[i]; i++)
+			tot = tot + pscale->ratio[i];
+
+		outv = (unsigned short)(inv / tot) * i;
+		inv %= tot;
+		for (i = 0; inv > 0; i++, outv++)
+			inv -= pscale->ratio[i];
+	}
+
+	if (!(*vout) || ((*vout) > outv))
+		*vout = outv;
+
+	if (pout)
+		*pout = outv;
+
+	return 0;
+}
+
+/*!
+ * @brief Reset PrP block
+ */
+int prphw_reset(void)
+{
+	unsigned long val;
+	unsigned long flag;
+	int i;
+
+	flag = PRP_CNTL_RST;
+	val = PRP_CNTL_RSTVAL;
+
+	__raw_writel(flag, PRP_CNTL);
+
+	/* timeout */
+	for (i = 0; i < 1000; i++) {
+		if (!(__raw_readl(PRP_CNTL) & flag)) {
+			pr_debug("PrP reset over\n");
+			break;
+		}
+		msleep(1);
+	}
+
+	/* verify reset value */
+	if (__raw_readl(PRP_CNTL) != val) {
+		pr_info("PrP reset err, val = 0x%08X\n", __raw_readl(PRP_CNTL));
+		return -1;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Enable PrP channel.
+ * @param channel	Channel number to be enabled
+ * @return		Zero on success, others on failure
+ */
+int prphw_enable(int channel)
+{
+	unsigned long val;
+
+	val = __raw_readl(PRP_CNTL);
+	if (channel & PRP_CHANNEL_1)
+		val |= PRP_CNTL_CH1EN;
+	if (channel & PRP_CHANNEL_2)
+		val |= (PRP_CNTL_CH2EN | PRP_CNTL_CH2_FLOWEN);
+
+	__raw_writel(val, PRP_CNTL);
+
+	return 0;
+}
+
+/*!
+ * @brief Disable PrP channel.
+ * @param channel	Channel number to be disable
+ * @return		Zero on success, others on failure
+ */
+int prphw_disable(int channel)
+{
+	unsigned long val;
+
+	val = __raw_readl(PRP_CNTL);
+	if (channel & PRP_CHANNEL_1)
+		val &= ~PRP_CNTL_CH1EN;
+	if (channel & PRP_CHANNEL_2)
+		val &= ~(PRP_CNTL_CH2EN | PRP_CNTL_CH2_FLOWEN);
+
+	__raw_writel(val, PRP_CNTL);
+
+	return 0;
+}
+
+/*!
+ * @brief Set PrP input buffer address.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_inptr(emma_prp_cfg * cfg)
+{
+	if (cfg->in_csi & PRP_CSI_EN)
+		return -1;
+
+	__raw_writel(cfg->in_ptr, PRP_SOURCE_Y_PTR);
+	if (cfg->in_pix == PRP_PIXIN_YUV420) {
+		u32 size;
+
+		size = cfg->in_line_stride * cfg->in_height;
+		__raw_writel(cfg->in_ptr + size, PRP_SOURCE_CB_PTR);
+		__raw_writel(cfg->in_ptr + size + (size >> 2),
+			     PRP_SOURCE_CR_PTR);
+	}
+	return 0;
+}
+
+/*!
+ * @brief Set PrP channel 1 output buffer 1 address.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_ch1ptr(emma_prp_cfg * cfg)
+{
+	if (cfg->ch1_pix == PRP_PIX1_UNUSED)
+		return -1;
+
+	__raw_writel(cfg->ch1_ptr, PRP_DEST_RGB1_PTR);
+
+	/* support double buffer in loop mode only */
+	if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP) {
+		if (cfg->ch1_ptr2)
+			__raw_writel(cfg->ch1_ptr2, PRP_DEST_RGB2_PTR);
+		else
+			__raw_writel(cfg->ch1_ptr, PRP_DEST_RGB2_PTR);
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Set PrP channel 1 output buffer 2 address.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_ch1ptr2(emma_prp_cfg * cfg)
+{
+	if (cfg->ch1_pix == PRP_PIX1_UNUSED ||
+	    (cfg->in_csi & PRP_CSI_LOOP) != PRP_CSI_LOOP)
+		return -1;
+
+	if (cfg->ch1_ptr2)
+		__raw_writel(cfg->ch1_ptr2, PRP_DEST_RGB2_PTR);
+	else
+		return -1;
+
+	return 0;
+}
+
+/*!
+ * @brief Set PrP channel 2 output buffer 1 address.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_ch2ptr(emma_prp_cfg * cfg)
+{
+	u32 size;
+
+	if (cfg->ch2_pix == PRP_PIX2_UNUSED)
+		return -1;
+
+	__raw_writel(cfg->ch2_ptr, PRP_DEST_Y_PTR);
+
+	if (cfg->ch2_pix == PRP_PIX2_YUV420) {
+		size = cfg->ch2_width * cfg->ch2_height;
+		__raw_writel(cfg->ch2_ptr + size, PRP_DEST_CB_PTR);
+		__raw_writel(cfg->ch2_ptr + size + (size >> 2),
+			     PRP_DEST_CR_PTR);
+	}
+
+	__raw_writel(__raw_readl(PRP_CNTL) | PRP_CNTL_CH2B1, PRP_CNTL);
+	return 0;
+}
+
+/*!
+ * @brief Set PrP channel 2 output buffer 2 address.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_ch2ptr2(emma_prp_cfg * cfg)
+{
+	u32 size;
+
+	if (cfg->ch2_pix == PRP_PIX2_UNUSED ||
+	    (cfg->in_csi & PRP_CSI_LOOP) != PRP_CSI_LOOP)
+		return -1;
+
+	__raw_writel(cfg->ch2_ptr2, PRP_SOURCE_Y_PTR);
+	if (cfg->ch2_pix == PRP_PIX2_YUV420) {
+		size = cfg->ch2_width * cfg->ch2_height;
+		__raw_writel(cfg->ch2_ptr2 + size, PRP_SOURCE_CB_PTR);
+		__raw_writel(cfg->ch2_ptr2 + size + (size >> 2),
+			     PRP_SOURCE_CR_PTR);
+	}
+
+	__raw_writel(__raw_readl(PRP_CNTL) | PRP_CNTL_CH2B2, PRP_CNTL);
+	return 0;
+}
+
+/*!
+ * @brief Build CSC table
+ * @param csc	CSC table
+ *		in	csc[0]=index		0..3 : A.1 A.0 B.1 B.0
+ *			csc[1]=direction	0 : YUV2RGB  1 : RGB2YUV
+ *		out	csc[0..4] are coefficients c[9] is offset
+ *			csc[0..8] are coefficients c[9] is offset
+ */
+void csc_tbl(short csc[10])
+{
+	static const unsigned short _r2y[][9] = {
+		{0x4D, 0x4B, 0x3A, 0x57, 0x55, 0x40, 0x40, 0x6B, 0x29},
+		{0x42, 0x41, 0x32, 0x4C, 0x4A, 0x38, 0x38, 0x5E, 0x24},
+		{0x36, 0x5C, 0x25, 0x3B, 0x63, 0x40, 0x40, 0x74, 0x18},
+		{0x2F, 0x4F, 0x20, 0x34, 0x57, 0x38, 0x38, 0x66, 0x15},
+	};
+	static const unsigned short _y2r[][5] = {
+		{0x80, 0xb4, 0x2c, 0x5b, 0x0e4},
+		{0x95, 0xcc, 0x32, 0x68, 0x104},
+		{0x80, 0xca, 0x18, 0x3c, 0x0ec},
+		{0x95, 0xe5, 0x1b, 0x44, 0x1e0},
+	};
+	unsigned short *_csc;
+	int _csclen;
+
+	csc[9] = csc[0] & 1;
+	_csclen = csc[0] & 3;
+
+	if (csc[1]) {
+		_csc = (unsigned short *)_r2y[_csclen];
+		_csclen = sizeof(_r2y[0]);
+	} else {
+		_csc = (unsigned short *)_y2r[_csclen];
+		_csclen = sizeof(_y2r[0]);
+		memset(csc + 5, 0, sizeof(short) * 4);
+	}
+	memcpy(csc, _csc, _csclen);
+}
+
+/*!
+ * @brief Setup PrP resize coefficient registers
+ *
+ * @param ch	PrP channel number
+ * @param dir	Direction, 0 - horizontal, 1 - vertical
+ * @param scale	The pointer to scale_t structure
+ */
+static void prp_set_scaler(int ch, int dir, scale_t * scale)
+{
+	int i;
+	unsigned int coeff[2];
+	unsigned int valid;
+
+	for (coeff[0] = coeff[1] = valid = 0, i = 19; i >= 0; i--) {
+		int j;
+
+		j = i > 9 ? 1 : 0;
+		coeff[j] = (coeff[j] << BC_COEF) |
+		    (scale->tbl[i] & (SZ_COEF - 1));
+
+		if (i == 5 || i == 15)
+			coeff[j] <<= 1;
+
+		valid = (valid << 1) | (scale->tbl[i] >> BC_COEF);
+	}
+
+	valid |= (scale->len << 24) | ((2 - scale->algo) << 31);
+
+	for (i = 0; i < 2; i++)
+		(*PRP_RSZ_COEFF)[1 - ch][dir].coeff[i] = coeff[i];
+
+	(*PRP_RSZ_COEFF)[1 - ch][dir].cntl = valid;
+}
+
+/*!
+ * @brief Setup PrP registers relevant to input.
+ * @param cfg		Pointer to PrP configuration parameter
+ * @param prp_cntl	Holds the value for PrP control register
+ * @return		Zero on success, others on failure
+ */
+static int prphw_input_cfg(emma_prp_cfg * cfg, unsigned long *prp_cntl)
+{
+	unsigned long mask;
+
+	switch (cfg->in_pix) {
+	case PRP_PIXIN_YUV420:
+		*prp_cntl |= PRP_CNTL_IN_YUV420;
+		mask = 0x7;
+		break;
+	case PRP_PIXIN_YUYV:
+	case PRP_PIXIN_YVYU:
+	case PRP_PIXIN_UYVY:
+	case PRP_PIXIN_VYUY:
+		*prp_cntl |= PRP_CNTL_IN_YUV422;
+		mask = 0x1;
+		break;
+	case PRP_PIXIN_RGB565:
+		*prp_cntl |= PRP_CNTL_IN_RGB16;
+		mask = 0x1;
+		break;
+	case PRP_PIXIN_RGB888:
+		*prp_cntl |= PRP_CNTL_IN_RGB32;
+		mask = 0;
+		break;
+	default:
+		pr_debug("Unsupported input pix format 0x%08X\n", cfg->in_pix);
+		return -1;
+	}
+
+	/* align the input image width */
+	if (cfg->in_width & mask) {
+		pr_debug("in_width misaligned. in_width=%d\n", cfg->in_width);
+		return -1;
+	}
+
+	if ((cfg->in_width < PRP_MIN_IN_WIDTH)
+	    || (cfg->in_width > PRP_MAX_IN_WIDTH)) {
+		pr_debug("Unsupported input width %d\n", cfg->in_width);
+		return -1;
+	}
+
+	cfg->in_height &= ~1;	/* truncate to make even */
+
+	if ((cfg->in_height < PRP_MIN_IN_HEIGHT)
+	    || (cfg->in_height > PRP_MAX_IN_HEIGHT)) {
+		pr_debug("Unsupported input height %d\n", cfg->in_height);
+		return -1;
+	}
+
+	if (!(cfg->in_csi & PRP_CSI_EN))
+		if (!cfg->in_line_stride)
+			cfg->in_line_stride = cfg->in_width;
+
+	__raw_writel(cfg->in_pix, PRP_SRC_PIXEL_FORMAT_CNTL);
+	__raw_writel((cfg->in_width << 16) | cfg->in_height,
+		     PRP_SOURCE_FRAME_SIZE);
+	__raw_writel((cfg->in_line_skip << 16) | cfg->in_line_stride,
+		     PRP_SOURCE_LINE_STRIDE);
+
+	if (!(cfg->in_csi & PRP_CSI_EN)) {
+		__raw_writel(cfg->in_ptr, PRP_SOURCE_Y_PTR);
+		if (cfg->in_pix == PRP_PIXIN_YUV420) {
+			unsigned int size;
+
+			size = cfg->in_line_stride * cfg->in_height;
+			__raw_writel(cfg->in_ptr + size, PRP_SOURCE_CB_PTR);
+			__raw_writel(cfg->in_ptr + size + (size >> 2),
+				     PRP_SOURCE_CR_PTR);
+		}
+	}
+
+	/* always cropping */
+	*prp_cntl |= PRP_CNTL_WINEN;
+
+	/* color space conversion */
+	if (!cfg->in_csc[1]) {
+		if (cfg->in_csc[0] > 3) {
+			pr_debug("in_csc invalid 0x%X\n", cfg->in_csc[0]);
+			return -1;
+		}
+		if ((cfg->in_pix == PRP_PIXIN_RGB565)
+		    || (cfg->in_pix == PRP_PIXIN_RGB888))
+			cfg->in_csc[1] = 1;
+		else
+			cfg->in_csc[0] = 0;
+		csc_tbl(cfg->in_csc);
+	}
+
+	__raw_writel((cfg->in_csc[0] << 21) | (cfg->in_csc[1] << 11)
+		     | cfg->in_csc[2], PRP_CSC_COEF_012);
+	__raw_writel((cfg->in_csc[3] << 21) | (cfg->in_csc[4] << 11)
+		     | cfg->in_csc[5], PRP_CSC_COEF_345);
+	__raw_writel((cfg->in_csc[6] << 21) | (cfg->in_csc[7] << 11)
+		     | cfg->in_csc[8] | (cfg->in_csc[9] << 31),
+		     PRP_CSC_COEF_678);
+
+	if (cfg->in_csi & PRP_CSI_EN) {
+		*prp_cntl |= PRP_CNTL_CSI;
+
+		/* loop mode enable, ch1 ch2 together */
+		if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP)
+			*prp_cntl |= (PRP_CNTL_CH1_LOOP | PRP_CNTL_CH2_LOOP);
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Setup PrP registers relevant to channel 2.
+ * @param cfg		Pointer to PrP configuration parameter
+ * @param prp_cntl	Holds the value for PrP control register
+ * @return		Zero on success, others on failure
+ */
+static int prphw_ch2_cfg(emma_prp_cfg * cfg, unsigned long *prp_cntl)
+{
+	switch (cfg->ch2_pix) {
+	case PRP_PIX2_YUV420:
+		*prp_cntl |= PRP_CNTL_CH2_YUV420;
+		break;
+	case PRP_PIX2_YUV422:
+		*prp_cntl |= PRP_CNTL_CH2_YUV422;
+		break;
+	case PRP_PIX2_YUV444:
+		*prp_cntl |= PRP_CNTL_CH2_YUV444;
+		break;
+	case PRP_PIX2_UNUSED:
+		return 0;
+	default:
+		pr_debug("Unsupported channel 2 pix format 0x%08X\n",
+			 cfg->ch2_pix);
+		return -1;
+	}
+
+	if (cfg->ch2_pix == PRP_PIX2_YUV420) {
+		cfg->ch2_height &= ~1;	/* ensure U/V presence */
+		cfg->ch2_width &= ~7;	/* ensure U/V word aligned */
+	} else if (cfg->ch2_pix == PRP_PIX2_YUV422) {
+		cfg->ch2_width &= ~1;	/* word aligned */
+	}
+
+	__raw_writel((cfg->ch2_width << 16) | cfg->ch2_height,
+		     PRP_CH2_OUT_IMAGE_SIZE);
+
+	if (cfg->ch2_pix == PRP_PIX2_YUV420) {
+		u32 size;
+
+		/* Luminanance band start address */
+		__raw_writel(cfg->ch2_ptr, PRP_DEST_Y_PTR);
+
+		if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP) {
+			if (!cfg->ch2_ptr2)
+				__raw_writel(cfg->ch2_ptr, PRP_SOURCE_Y_PTR);
+			else
+				__raw_writel(cfg->ch2_ptr2, PRP_SOURCE_Y_PTR);
+		}
+
+		/* Cb and Cr band start address */
+		size = cfg->ch2_width * cfg->ch2_height;
+		__raw_writel(cfg->ch2_ptr + size, PRP_DEST_CB_PTR);
+		__raw_writel(cfg->ch2_ptr + size + (size >> 2),
+			     PRP_DEST_CR_PTR);
+
+		if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP) {
+			if (!cfg->ch2_ptr2) {
+				__raw_writel(cfg->ch2_ptr + size,
+					     PRP_SOURCE_CB_PTR);
+				__raw_writel(cfg->ch2_ptr + size + (size >> 2),
+					     PRP_SOURCE_CR_PTR);
+			} else {
+				__raw_writel(cfg->ch2_ptr2 + size,
+					     PRP_SOURCE_CB_PTR);
+				__raw_writel(cfg->ch2_ptr2 + size + (size >> 2),
+					     PRP_SOURCE_CR_PTR);
+			}
+		}
+	} else {		/* Pixel interleaved YUV422 or YUV444 */
+		__raw_writel(cfg->ch2_ptr, PRP_DEST_Y_PTR);
+
+		if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP) {
+			if (!cfg->ch2_ptr2)
+				__raw_writel(cfg->ch2_ptr, PRP_SOURCE_Y_PTR);
+			else
+				__raw_writel(cfg->ch2_ptr2, PRP_SOURCE_Y_PTR);
+		}
+	}
+	*prp_cntl |= PRP_CNTL_CH2B1 | PRP_CNTL_CH2B2;
+
+	return 0;
+}
+
+/*!
+ * @brief Setup PrP registers relevant to channel 1.
+ * @param cfg		Pointer to PrP configuration parameter
+ * @param prp_cntl	Holds the value for PrP control register
+ * @return		Zero on success, others on failure
+ */
+static int prphw_ch1_cfg(emma_prp_cfg * cfg, unsigned long *prp_cntl)
+{
+	int ch1_bpp = 0;
+
+	switch (cfg->ch1_pix) {
+	case PRP_PIX1_RGB332:
+		*prp_cntl |= PRP_CNTL_CH1_RGB8;
+		ch1_bpp = 1;
+		break;
+	case PRP_PIX1_RGB565:
+		*prp_cntl |= PRP_CNTL_CH1_RGB16;
+		ch1_bpp = 2;
+		break;
+	case PRP_PIX1_RGB888:
+		*prp_cntl |= PRP_CNTL_CH1_RGB32;
+		ch1_bpp = 4;
+		break;
+	case PRP_PIX1_YUYV:
+	case PRP_PIX1_YVYU:
+	case PRP_PIX1_UYVY:
+	case PRP_PIX1_VYUY:
+		*prp_cntl |= PRP_CNTL_CH1_YUV422;
+		ch1_bpp = 2;
+		break;
+	case PRP_PIX1_UNUSED:
+		return 0;
+	default:
+		pr_debug("Unsupported channel 1 pix format 0x%08X\n",
+			 cfg->ch1_pix);
+		return -1;
+	}
+
+	/* parallel or cascade resize */
+	if (cfg->ch1_scale.algo & PRP_ALGO_BYPASS)
+		*prp_cntl |= PRP_CNTL_UNCHAIN;
+
+	/* word align */
+	if (ch1_bpp == 2)
+		cfg->ch1_width &= ~1;
+	else if (ch1_bpp == 1)
+		cfg->ch1_width &= ~3;
+
+	if (!cfg->ch1_stride)
+		cfg->ch1_stride = cfg->ch1_width;
+
+	__raw_writel(cfg->ch1_pix, PRP_CH1_PIXEL_FORMAT_CNTL);
+	__raw_writel((cfg->ch1_width << 16) | cfg->ch1_height,
+		     PRP_CH1_OUT_IMAGE_SIZE);
+	__raw_writel(cfg->ch1_stride * ch1_bpp, PRP_CH1_LINE_STRIDE);
+	__raw_writel(cfg->ch1_ptr, PRP_DEST_RGB1_PTR);
+
+	/* double buffer for loop mode */
+	if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP) {
+		if (cfg->ch1_ptr2)
+			__raw_writel(cfg->ch1_ptr2, PRP_DEST_RGB2_PTR);
+		else
+			__raw_writel(cfg->ch1_ptr, PRP_DEST_RGB2_PTR);
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Setup PrP registers.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_cfg(emma_prp_cfg * cfg)
+{
+	unsigned long prp_cntl = 0;
+	unsigned long val;
+
+	/* input pixel format checking */
+	if (prphw_input_cfg(cfg, &prp_cntl))
+		return -1;
+
+	if (prphw_ch2_cfg(cfg, &prp_cntl))
+		return -1;
+
+	if (prphw_ch1_cfg(cfg, &prp_cntl))
+		return -1;
+
+	/* register setting */
+	__raw_writel(prp_cntl, PRP_CNTL);
+
+	/* interrupt configuration */
+	val = PRP_INTRCNTL_RDERR | PRP_INTRCNTL_LBOVF;
+	if (cfg->ch1_pix != PRP_PIX1_UNUSED)
+		val |= PRP_INTRCNTL_CH1FC | PRP_INTRCNTL_CH1WERR;
+	if (cfg->ch2_pix != PRP_PIX2_UNUSED)
+		val |=
+		    PRP_INTRCNTL_CH2FC | PRP_INTRCNTL_CH2WERR |
+		    PRP_INTRCNTL_CH2OVF;
+	__raw_writel(val, PRP_INTRCNTL);
+
+	prp_set_scaler(1, 0, &cfg->scale[0]);	/* Channel 1 width */
+	prp_set_scaler(1, 1, &cfg->scale[1]);	/* Channel 1 height */
+	prp_set_scaler(0, 0, &cfg->scale[2]);	/* Channel 2 width */
+	prp_set_scaler(0, 1, &cfg->scale[3]);	/* Channel 2 height */
+
+	return 0;
+}
+
+/*!
+ * @brief Check PrP interrupt status.
+ * @return	PrP interrupt status
+ */
+int prphw_isr(void)
+{
+	int status;
+
+	status = __raw_readl(PRP_INTRSTATUS) & 0x1FF;
+
+	if (status & (PRP_INTRSTAT_RDERR | PRP_INTRSTAT_CH1WERR |
+		      PRP_INTRSTAT_CH2WERR))
+		pr_debug("isr bus error. status= 0x%08X\n", status);
+	else if (status & PRP_INTRSTAT_CH2OVF)
+		pr_debug("isr ch 2 buffer overflow. status= 0x%08X\n", status);
+	else if (status & PRP_INTRSTAT_LBOVF)
+		pr_debug("isr line buffer overflow. status= 0x%08X\n", status);
+
+	/* silicon bug?? enable bit does not self clear? */
+	if (!(__raw_readl(PRP_CNTL) & PRP_CNTL_CH1_LOOP))
+		__raw_writel(__raw_readl(PRP_CNTL) & (~PRP_CNTL_CH1EN),
+			     PRP_CNTL);
+	if (!(__raw_readl(PRP_CNTL) & PRP_CNTL_CH2_LOOP))
+		__raw_writel(__raw_readl(PRP_CNTL) & (~PRP_CNTL_CH2EN),
+			     PRP_CNTL);
+
+	__raw_writel(status, PRP_INTRSTATUS);	/* clr irq */
+
+	return status;
+}
+
+static struct clk *emma_clk;
+
+/*!
+ * @brief  PrP module clock enable
+ */
+void prphw_init(void)
+{
+	emma_clk = clk_get(NULL, "emma_clk");
+	clk_enable(emma_clk);
+}
+
+/*!
+ * @brief PrP module clock disable
+ */
+void prphw_exit(void)
+{
+	clk_disable(emma_clk);
+	clk_put(emma_clk);
+}
diff --git a/drivers/media/video/mxc/capture/mx27_prpsw.c b/drivers/media/video/mxc/capture/mx27_prpsw.c
new file mode 100644
index 000000000000..ce7db16913ec
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mx27_prpsw.c
@@ -0,0 +1,1042 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_prpsw.c
+ *
+ * @brief MX27 Video For Linux 2 capture driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/fb.h>
+#include <linux/pci.h>
+#include <asm/cacheflush.h>
+#include <asm/io.h>
+#include <asm/irq.h>
+
+#include "mxc_v4l2_capture.h"
+#include "mx27_prp.h"
+#include "mx27_csi.h"
+#include "../drivers/video/mxc/mx2fb.h"
+#include "../opl/opl.h"
+
+#define MEAN_COEF	(SZ_COEF >> 1)
+
+static char prp_dev[] = "emma_prp";
+static int g_still_on = 0;
+static emma_prp_cfg g_prp_cfg;
+static int g_vfbuf, g_rotbuf;
+static struct tasklet_struct prp_vf_tasklet;
+
+/*
+ * The following variables represents the virtual address for the cacheable
+ * buffers accessed by SW rotation/mirroring. The rotation/mirroring in
+ * cacheable buffers has significant performance improvement than it in
+ * non-cacheable buffers.
+ */
+static char *g_vaddr_vfbuf[2] = { 0, 0 };
+static char *g_vaddr_rotbuf[2] = { 0, 0 };
+static char *g_vaddr_fb = 0;
+
+static int set_ch1_addr(emma_prp_cfg * cfg, cam_data * cam);
+static int prp_v4l2_cfg(emma_prp_cfg * cfg, cam_data * cam);
+static int prp_vf_mem_alloc(cam_data * cam);
+static void prp_vf_mem_free(cam_data * cam);
+static int prp_rot_mem_alloc(cam_data * cam);
+static void prp_rot_mem_free(cam_data * cam);
+static int prp_enc_update_eba(u32 eba, int *buffer_num);
+static int prp_enc_enable(void *private);
+static int prp_enc_disable(void *private);
+static int prp_vf_start(void *private);
+static int prp_vf_stop(void *private);
+static int prp_still_start(void *private);
+static int prp_still_stop(void *private);
+static irqreturn_t prp_isr(int irq, void *dev_id);
+static void rotation(unsigned long private);
+static int prp_resize_check_ch1(emma_prp_cfg * cfg);
+static int prp_resize_check_ch2(emma_prp_cfg * cfg);
+
+#define PRP_DUMP(val)	pr_debug("%s\t = 0x%08X\t%d\n", #val, val, val)
+
+/*!
+ * @brief Dump PrP configuration parameters.
+ * @param cfg	The pointer to PrP configuration parameter
+ */
+static void prp_cfg_dump(emma_prp_cfg * cfg)
+{
+	PRP_DUMP(cfg->in_pix);
+	PRP_DUMP(cfg->in_width);
+	PRP_DUMP(cfg->in_height);
+	PRP_DUMP(cfg->in_csi);
+	PRP_DUMP(cfg->in_line_stride);
+	PRP_DUMP(cfg->in_line_skip);
+	PRP_DUMP(cfg->in_ptr);
+
+	PRP_DUMP(cfg->ch1_pix);
+	PRP_DUMP(cfg->ch1_width);
+	PRP_DUMP(cfg->ch1_height);
+	PRP_DUMP(cfg->ch1_scale.algo);
+	PRP_DUMP(cfg->ch1_scale.width.num);
+	PRP_DUMP(cfg->ch1_scale.width.den);
+	PRP_DUMP(cfg->ch1_scale.height.num);
+	PRP_DUMP(cfg->ch1_scale.height.den);
+	PRP_DUMP(cfg->ch1_stride);
+	PRP_DUMP(cfg->ch1_ptr);
+	PRP_DUMP(cfg->ch1_ptr2);
+	PRP_DUMP(cfg->ch1_csi);
+
+	PRP_DUMP(cfg->ch2_pix);
+	PRP_DUMP(cfg->ch2_width);
+	PRP_DUMP(cfg->ch2_height);
+	PRP_DUMP(cfg->ch2_scale.algo);
+	PRP_DUMP(cfg->ch2_scale.width.num);
+	PRP_DUMP(cfg->ch2_scale.width.den);
+	PRP_DUMP(cfg->ch2_scale.height.num);
+	PRP_DUMP(cfg->ch2_scale.height.den);
+	PRP_DUMP(cfg->ch2_ptr);
+	PRP_DUMP(cfg->ch2_ptr2);
+	PRP_DUMP(cfg->ch2_csi);
+}
+
+/*!
+ * @brief Set PrP channel 1 output address.
+ * @param cfg	Pointer to emma_prp_cfg structure
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static int set_ch1_addr(emma_prp_cfg * cfg, cam_data * cam)
+{
+	if (cam->rotation != V4L2_MXC_ROTATE_NONE) {
+		cfg->ch1_ptr = (unsigned int)cam->rot_vf_bufs[0];
+		cfg->ch1_ptr2 = (unsigned int)cam->rot_vf_bufs[1];
+		if ((cam->rotation == V4L2_MXC_ROTATE_90_RIGHT)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_VFLIP)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_HFLIP)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_LEFT))
+			cfg->ch1_stride = cam->win.w.height;
+		else
+			cfg->ch1_stride = cam->win.w.width;
+
+		if (cam->v4l2_fb.flags != V4L2_FBUF_FLAG_OVERLAY) {
+			struct fb_info *fb = cam->overlay_fb;
+			if (!fb)
+				return -1;
+			if (g_vaddr_fb)
+				iounmap(g_vaddr_fb);
+			g_vaddr_fb = ioremap_cached(fb->fix.smem_start,
+						    fb->fix.smem_len);
+			if (!g_vaddr_fb)
+				return -1;
+		}
+	} else if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		cfg->ch1_ptr = (unsigned int)cam->vf_bufs[0];
+		cfg->ch1_ptr2 = (unsigned int)cam->vf_bufs[1];
+		cfg->ch1_stride = cam->win.w.width;
+	} else {
+		struct fb_info *fb = cam->overlay_fb;
+
+		if (!fb)
+			return -1;
+
+		cfg->ch1_ptr = fb->fix.smem_start;
+		cfg->ch1_ptr += cam->win.w.top * fb->var.xres_virtual
+		    * (fb->var.bits_per_pixel >> 3)
+		    + cam->win.w.left * (fb->var.bits_per_pixel >> 3);
+		cfg->ch1_ptr2 = cfg->ch1_ptr;
+		cfg->ch1_stride = fb->var.xres_virtual;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Setup PrP configuration parameters.
+ * @param cfg	Pointer to emma_prp_cfg structure
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static int prp_v4l2_cfg(emma_prp_cfg * cfg, cam_data * cam)
+{
+	cfg->in_pix = PRP_PIXIN_YUYV;
+	cfg->in_width = cam->crop_current.width;
+	cfg->in_height = cam->crop_current.height;
+	cfg->in_line_stride = cam->crop_current.left;
+	cfg->in_line_skip = cam->crop_current.top;
+	cfg->in_ptr = 0;
+	cfg->in_csi = PRP_CSI_LOOP;
+	memset(cfg->in_csc, 0, sizeof(cfg->in_csc));
+
+	if (cam->overlay_on) {
+		/* Convert V4L2 pixel format to PrP pixel format */
+		switch (cam->v4l2_fb.fmt.pixelformat) {
+		case V4L2_PIX_FMT_RGB332:
+			cfg->ch1_pix = PRP_PIX1_RGB332;
+			break;
+		case V4L2_PIX_FMT_RGB32:
+		case V4L2_PIX_FMT_BGR32:
+			cfg->ch1_pix = PRP_PIX1_RGB888;
+			break;
+		case V4L2_PIX_FMT_YUYV:
+			cfg->ch1_pix = PRP_PIX1_YUYV;
+			break;
+		case V4L2_PIX_FMT_UYVY:
+			cfg->ch1_pix = PRP_PIX1_UYVY;
+			break;
+		case V4L2_PIX_FMT_RGB565:
+		default:
+			cfg->ch1_pix = PRP_PIX1_RGB565;
+			break;
+		}
+		if ((cam->rotation == V4L2_MXC_ROTATE_90_RIGHT)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_VFLIP)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_HFLIP)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_LEFT)) {
+			cfg->ch1_width = cam->win.w.height;
+			cfg->ch1_height = cam->win.w.width;
+		} else {
+			cfg->ch1_width = cam->win.w.width;
+			cfg->ch1_height = cam->win.w.height;
+		}
+
+		if (set_ch1_addr(cfg, cam))
+			return -1;
+	} else {
+		cfg->ch1_pix = PRP_PIX1_UNUSED;
+		cfg->ch1_width = cfg->in_width;
+		cfg->ch1_height = cfg->in_height;
+	}
+	cfg->ch1_scale.algo = 0;
+	cfg->ch1_scale.width.num = cfg->in_width;
+	cfg->ch1_scale.width.den = cfg->ch1_width;
+	cfg->ch1_scale.height.num = cfg->in_height;
+	cfg->ch1_scale.height.den = cfg->ch1_height;
+	cfg->ch1_csi = PRP_CSI_EN;
+
+	if (cam->capture_on || g_still_on) {
+		switch (cam->v2f.fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_YUYV:
+			cfg->ch2_pix = PRP_PIX2_YUV422;
+			break;
+		case V4L2_PIX_FMT_YUV420:
+			cfg->ch2_pix = PRP_PIX2_YUV420;
+			break;
+			/*
+			 * YUV444 is not defined by V4L2.
+			 * We support it in default case.
+			 */
+		default:
+			cfg->ch2_pix = PRP_PIX2_YUV444;
+			break;
+		}
+		cfg->ch2_width = cam->v2f.fmt.pix.width;
+		cfg->ch2_height = cam->v2f.fmt.pix.height;
+	} else {
+		cfg->ch2_pix = PRP_PIX2_UNUSED;
+		cfg->ch2_width = cfg->in_width;
+		cfg->ch2_height = cfg->in_height;
+	}
+	cfg->ch2_scale.algo = 0;
+	cfg->ch2_scale.width.num = cfg->in_width;
+	cfg->ch2_scale.width.den = cfg->ch2_width;
+	cfg->ch2_scale.height.num = cfg->in_height;
+	cfg->ch2_scale.height.den = cfg->ch2_height;
+	cfg->ch2_csi = PRP_CSI_EN;
+
+	memset(cfg->scale, 0, sizeof(cfg->scale));
+	cfg->scale[0].algo = cfg->ch1_scale.algo & 3;
+	cfg->scale[1].algo = (cfg->ch1_scale.algo >> 2) & 3;
+	cfg->scale[2].algo = cfg->ch2_scale.algo & 3;
+	cfg->scale[3].algo = (cfg->ch2_scale.algo >> 2) & 3;
+
+	prp_cfg_dump(cfg);
+
+	if (prp_resize_check_ch2(cfg))
+		return -1;
+
+	if (prp_resize_check_ch1(cfg))
+		return -1;
+
+	return 0;
+}
+
+/*!
+ * @brief PrP interrupt handler
+ */
+static irqreturn_t prp_isr(int irq, void *dev_id)
+{
+	int status;
+	cam_data *cam = (cam_data *) dev_id;
+
+	status = prphw_isr();
+
+	if (g_still_on && (status & PRP_INTRSTAT_CH2BUF1)) {
+		prp_still_stop(cam);
+		cam->still_counter++;
+		wake_up_interruptible(&cam->still_queue);
+		/*
+		 * Still & video capture use the same PrP channel 2.
+		 * They are execlusive.
+		 */
+	} else if (cam->capture_on) {
+		if (status & (PRP_INTRSTAT_CH2BUF1 | PRP_INTRSTAT_CH2BUF2)) {
+			cam->enc_callback(0, cam);
+		}
+	}
+	if (cam->overlay_on
+	    && (status & (PRP_INTRSTAT_CH1BUF1 | PRP_INTRSTAT_CH1BUF2))) {
+		if (cam->rotation != V4L2_MXC_ROTATE_NONE) {
+			g_rotbuf = (status & PRP_INTRSTAT_CH1BUF1) ? 0 : 1;
+			tasklet_schedule(&prp_vf_tasklet);
+		} else if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+			struct fb_gwinfo gwinfo;
+
+			gwinfo.enabled = 1;
+			gwinfo.alpha_value = 255;
+			gwinfo.ck_enabled = 0;
+			gwinfo.xpos = cam->win.w.left;
+			gwinfo.ypos = cam->win.w.top;
+			gwinfo.xres = cam->win.w.width;
+			gwinfo.yres = cam->win.w.height;
+			gwinfo.xres_virtual = cam->win.w.width;
+			gwinfo.vs_reversed = 0;
+			if (status & PRP_INTRSTAT_CH1BUF1)
+				gwinfo.base = (unsigned long)cam->vf_bufs[0];
+			else
+				gwinfo.base = (unsigned long)cam->vf_bufs[1];
+
+			mx2_gw_set(&gwinfo);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * @brief PrP initialization.
+ * @param dev_id	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_init(void *dev_id)
+{
+	enable_irq(MXC_INT_EMMAPRP);
+	if (request_irq(MXC_INT_EMMAPRP, prp_isr, 0, prp_dev, dev_id))
+		return -1;
+	prphw_init();
+
+	return 0;
+}
+
+/*!
+ * @brief PrP initialization.
+ * @param dev_id	Pointer to cam_data structure
+ */
+void prp_exit(void *dev_id)
+{
+	prphw_exit();
+	disable_irq(MXC_INT_EMMAPRP);
+	free_irq(MXC_INT_EMMAPRP, dev_id);
+}
+
+/*!
+ * @brief Update PrP channel 2 output buffer address.
+ * @param eba		Physical address for PrP output buffer
+ * @param buffer_num	The PrP channel 2 buffer number to be updated
+ * @return		Zero on success, others on failure
+ */
+static int prp_enc_update_eba(u32 eba, int *buffer_num)
+{
+	if (*buffer_num) {
+		g_prp_cfg.ch2_ptr2 = eba;
+		prphw_ch2ptr2(&g_prp_cfg);
+		*buffer_num = 0;
+	} else {
+		g_prp_cfg.ch2_ptr = eba;
+		prphw_ch2ptr(&g_prp_cfg);
+		*buffer_num = 1;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Enable PrP for encoding.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_enc_enable(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (prp_v4l2_cfg(&g_prp_cfg, cam))
+		return -1;
+
+	csi_enable_mclk(CSI_MCLK_ENC, true, true);
+	prphw_reset();
+
+	if (prphw_cfg(&g_prp_cfg))
+		return -1;
+
+	prphw_enable(cam->overlay_on ? (PRP_CHANNEL_1 | PRP_CHANNEL_2)
+		     : PRP_CHANNEL_2);
+
+	return 0;
+}
+
+/*!
+ * @brief Disable PrP for encoding.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_enc_disable(void *private)
+{
+	prphw_disable(PRP_CHANNEL_2);
+	csi_enable_mclk(CSI_MCLK_ENC, false, false);
+
+	return 0;
+}
+
+/*!
+ * @brief Setup encoding functions.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_enc_select(void *private)
+{
+	int ret = 0;
+	cam_data *cam = (cam_data *) private;
+
+	if (cam) {
+		cam->enc_update_eba = prp_enc_update_eba;
+		cam->enc_enable = prp_enc_enable;
+		cam->enc_disable = prp_enc_disable;
+	} else
+		ret = -EIO;
+
+	return ret;
+}
+
+/*!
+ * @brief Uninstall encoding functions.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_enc_deselect(void *private)
+{
+	int ret = 0;
+	cam_data *cam = (cam_data *) private;
+
+	ret = prp_enc_disable(private);
+
+	if (cam) {
+		cam->enc_update_eba = NULL;
+		cam->enc_enable = NULL;
+		cam->enc_disable = NULL;
+	}
+
+	return ret;
+}
+
+/*!
+ * @brief Allocate memory for overlay.
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static int prp_vf_mem_alloc(cam_data * cam)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		cam->vf_bufs_size[i] = cam->win.w.width * cam->win.w.height * 2;
+		cam->vf_bufs_vaddr[i] = dma_alloc_coherent(0,
+							   cam->vf_bufs_size[i],
+							   &cam->vf_bufs[i],
+							   GFP_DMA |
+							   GFP_KERNEL);
+		if (!cam->vf_bufs_vaddr[i]) {
+			pr_debug("Failed to alloc memory for vf.\n");
+			prp_vf_mem_free(cam);
+			return -1;
+		}
+
+		g_vaddr_vfbuf[i] =
+		    ioremap_cached(cam->vf_bufs[i], cam->vf_bufs_size[i]);
+		if (!g_vaddr_vfbuf[i]) {
+			pr_debug("Failed to ioremap_cached() for vf.\n");
+			prp_vf_mem_free(cam);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Free memory for overlay.
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static void prp_vf_mem_free(cam_data * cam)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		if (cam->vf_bufs_vaddr[i]) {
+			dma_free_coherent(0,
+					  cam->vf_bufs_size[i],
+					  cam->vf_bufs_vaddr[i],
+					  cam->vf_bufs[i]);
+		}
+		cam->vf_bufs[i] = 0;
+		cam->vf_bufs_vaddr[i] = 0;
+		cam->vf_bufs_size[i] = 0;
+		if (g_vaddr_vfbuf[i]) {
+			iounmap(g_vaddr_vfbuf[i]);
+			g_vaddr_vfbuf[i] = 0;
+		}
+	}
+}
+
+/*!
+ * @brief Allocate intermediate memory for overlay rotation/mirroring.
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static int prp_rot_mem_alloc(cam_data * cam)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		cam->rot_vf_buf_size[i] =
+		    cam->win.w.width * cam->win.w.height * 2;
+		cam->rot_vf_bufs_vaddr[i] =
+		    dma_alloc_coherent(0, cam->rot_vf_buf_size[i],
+				       &cam->rot_vf_bufs[i],
+				       GFP_DMA | GFP_KERNEL);
+		if (!cam->rot_vf_bufs_vaddr[i]) {
+			pr_debug("Failed to alloc memory for vf rotation.\n");
+			prp_rot_mem_free(cam);
+			return -1;
+		}
+
+		g_vaddr_rotbuf[i] =
+		    ioremap_cached(cam->rot_vf_bufs[i],
+				   cam->rot_vf_buf_size[i]);
+		if (!g_vaddr_rotbuf[i]) {
+			pr_debug
+			    ("Failed to ioremap_cached() for rotation buffer.\n");
+			prp_rot_mem_free(cam);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Free intermedaite memory for overlay rotation/mirroring.
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static void prp_rot_mem_free(cam_data * cam)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		if (cam->rot_vf_bufs_vaddr[i]) {
+			dma_free_coherent(0,
+					  cam->rot_vf_buf_size[i],
+					  cam->rot_vf_bufs_vaddr[i],
+					  cam->rot_vf_bufs[i]);
+		}
+		cam->rot_vf_bufs[i] = 0;
+		cam->rot_vf_bufs_vaddr[i] = 0;
+		cam->rot_vf_buf_size[i] = 0;
+		if (g_vaddr_rotbuf[i]) {
+			iounmap(g_vaddr_rotbuf[i]);
+			g_vaddr_rotbuf[i] = 0;
+		}
+	}
+}
+
+/*!
+ * @brief Start overlay (view finder).
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_vf_start(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		prp_vf_mem_free(cam);
+		if (prp_vf_mem_alloc(cam)) {
+			pr_info("Error to allocate vf buffer\n");
+			return -ENOMEM;
+		}
+	}
+
+	if (cam->rotation != V4L2_MXC_ROTATE_NONE) {
+		prp_rot_mem_free(cam);
+		if (prp_rot_mem_alloc(cam)) {
+			pr_info("Error to allocate rotation buffer\n");
+			prp_vf_mem_free(cam);
+			return -ENOMEM;
+		}
+	}
+
+	if (prp_v4l2_cfg(&g_prp_cfg, cam)) {
+		prp_vf_mem_free(cam);
+		prp_rot_mem_free(cam);
+		return -1;
+	}
+
+	csi_enable_mclk(CSI_MCLK_VF, true, true);
+	prphw_reset();
+
+	if (prphw_cfg(&g_prp_cfg)) {
+		prp_vf_mem_free(cam);
+		prp_rot_mem_free(cam);
+		return -1;
+	}
+	g_vfbuf = g_rotbuf = 0;
+	tasklet_init(&prp_vf_tasklet, rotation, (unsigned long)private);
+
+	prphw_enable(cam->capture_on ? (PRP_CHANNEL_1 | PRP_CHANNEL_2)
+		     : PRP_CHANNEL_1);
+
+	return 0;
+}
+
+/*!
+ * @brief Stop overlay (view finder).
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_vf_stop(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	prphw_disable(PRP_CHANNEL_1);
+
+	csi_enable_mclk(CSI_MCLK_VF, false, false);
+	tasklet_kill(&prp_vf_tasklet);
+
+	if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		struct fb_gwinfo gwinfo;
+
+		/* Disable graphic window */
+		gwinfo.enabled = 0;
+		mx2_gw_set(&gwinfo);
+
+		prp_vf_mem_free(cam);
+	}
+	prp_rot_mem_free(cam);
+	if (g_vaddr_fb) {
+		iounmap(g_vaddr_fb);
+		g_vaddr_fb = 0;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Setup overlay functions.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_vf_select(void *private)
+{
+	int ret = 0;
+	cam_data *cam = (cam_data *) private;
+
+	if (cam) {
+		cam->vf_start_sdc = prp_vf_start;
+		cam->vf_stop_sdc = prp_vf_stop;
+		cam->overlay_active = false;
+	} else
+		ret = -EIO;
+
+	return ret;
+}
+
+/*!
+ * @brief Uninstall overlay functions.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_vf_deselect(void *private)
+{
+	int ret = 0;
+	cam_data *cam = (cam_data *) private;
+
+	ret = prp_vf_stop(private);
+
+	if (cam) {
+		cam->vf_start_sdc = NULL;
+		cam->vf_stop_sdc = NULL;
+	}
+
+	return ret;
+}
+
+/*!
+ * @brief Start still picture capture.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_still_start(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	g_still_on = 1;
+	g_prp_cfg.ch2_ptr = (unsigned int)cam->still_buf;
+	g_prp_cfg.ch2_ptr2 = 0;
+
+	if (prp_v4l2_cfg(&g_prp_cfg, cam))
+		return -1;
+
+	csi_enable_mclk(CSI_MCLK_RAW, true, true);
+	prphw_reset();
+
+	if (prphw_cfg(&g_prp_cfg)) {
+		g_still_on = 0;
+		return -1;
+	}
+
+	prphw_enable(cam->overlay_on ? (PRP_CHANNEL_1 | PRP_CHANNEL_2)
+		     : PRP_CHANNEL_2);
+
+	return 0;
+}
+
+/*!
+ * @brief Stop still picture capture.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_still_stop(void *private)
+{
+	prphw_disable(PRP_CHANNEL_2);
+
+	csi_enable_mclk(CSI_MCLK_RAW, false, false);
+
+	g_still_on = 0;
+
+	return 0;
+}
+
+/*!
+ * @brief Setup functions for still picture capture.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_still_select(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (cam) {
+		cam->csi_start = prp_still_start;
+		cam->csi_stop = prp_still_stop;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Uninstall functions for still picture capture.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_still_deselect(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	err = prp_still_stop(cam);
+
+	if (cam) {
+		cam->csi_start = NULL;
+		cam->csi_stop = NULL;
+	}
+
+	return err;
+}
+
+/*!
+ * @brief Perform software rotation or mirroring
+ * @param private	Argument passed to the tasklet
+ */
+static void rotation(unsigned long private)
+{
+	char *src, *dst;
+	int width, height, s_stride, d_stride;
+	int size;
+	cam_data *cam = (cam_data *) private;
+
+	src = g_vaddr_rotbuf[g_rotbuf];
+	size = cam->rot_vf_buf_size[g_rotbuf];
+
+	if ((cam->rotation == V4L2_MXC_ROTATE_90_RIGHT)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_VFLIP)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_HFLIP)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_LEFT)) {
+		width = cam->win.w.height;
+		height = cam->win.w.width;
+		s_stride = cam->win.w.height << 1;
+	} else {
+		width = cam->win.w.width;
+		height = cam->win.w.height;
+		s_stride = cam->win.w.width << 1;
+	}
+
+	if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		dst = g_vaddr_vfbuf[g_vfbuf];
+		d_stride = cam->win.w.width << 1;
+	} else {		/* The destination is the framebuffer */
+		struct fb_info *fb = cam->overlay_fb;
+		if (!fb)
+			return;
+		dst = g_vaddr_fb;
+		dst += cam->win.w.top * fb->var.xres_virtual
+		    * (fb->var.bits_per_pixel >> 3)
+		    + cam->win.w.left * (fb->var.bits_per_pixel >> 3);
+		d_stride = fb->var.xres_virtual << 1;
+	}
+
+	/*
+	 * Invalidate the data in cache before performing the SW rotaion
+	 * or mirroring in case the image size is less than QVGA. For image
+	 * larger than QVGA it is not invalidated becase the invalidation
+	 * will consume much time while we don't see any artifacts on the
+	 * output if we don't perform invalidation for them.
+	 * Similarly we don't flush the data after SW rotation/mirroring.
+	 */
+	if (size < 320 * 240 * 2)
+		dmac_inv_range(src, src + size);
+	switch (cam->rotation) {
+	case V4L2_MXC_ROTATE_VERT_FLIP:
+		opl_vmirror_u16(src, s_stride, width, height, dst, d_stride);
+		break;
+	case V4L2_MXC_ROTATE_HORIZ_FLIP:
+		opl_hmirror_u16(src, s_stride, width, height, dst, d_stride);
+		break;
+	case V4L2_MXC_ROTATE_180:
+		opl_rotate180_u16(src, s_stride, width, height, dst, d_stride);
+		break;
+	case V4L2_MXC_ROTATE_90_RIGHT:
+		opl_rotate90_u16(src, s_stride, width, height, dst, d_stride);
+		break;
+	case V4L2_MXC_ROTATE_90_RIGHT_VFLIP:
+		opl_rotate90_vmirror_u16(src, s_stride, width, height, dst,
+					 d_stride);
+		break;
+	case V4L2_MXC_ROTATE_90_RIGHT_HFLIP:
+		/* ROTATE_90_RIGHT_HFLIP = ROTATE_270_RIGHT_VFLIP */
+		opl_rotate270_vmirror_u16(src, s_stride, width, height, dst,
+					  d_stride);
+		break;
+	case V4L2_MXC_ROTATE_90_LEFT:
+		opl_rotate270_u16(src, s_stride, width, height, dst, d_stride);
+		break;
+	default:
+		return;
+	}
+
+	/* Config and display the graphic window */
+	if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		struct fb_gwinfo gwinfo;
+
+		gwinfo.enabled = 1;
+		gwinfo.alpha_value = 255;
+		gwinfo.ck_enabled = 0;
+		gwinfo.xpos = cam->win.w.left;
+		gwinfo.ypos = cam->win.w.top;
+		gwinfo.xres = cam->win.w.width;
+		gwinfo.yres = cam->win.w.height;
+		gwinfo.xres_virtual = cam->win.w.width;
+		gwinfo.vs_reversed = 0;
+		gwinfo.base = (unsigned long)cam->vf_bufs[g_vfbuf];
+		mx2_gw_set(&gwinfo);
+
+		g_vfbuf = g_vfbuf ? 0 : 1;
+	}
+}
+
+/*
+ * @brief Check if the resize ratio is supported based on the input and output
+ *        dimension
+ * @param	input	input dimension
+ * @param	output	output dimension
+ * @return	output dimension (should equal the parameter *output*)
+ * 		-1 on failure
+ */
+static int check_simple(scale_t * scale, int input, int output)
+{
+	unsigned short int_out;	/* PrP internel width or height */
+	unsigned short orig_out = output;
+
+	if (prp_scale(scale, input, output, input, &orig_out, &int_out, 0))
+		return -1;	/* resize failed */
+	else
+		return int_out;
+}
+
+/*
+ * @brief Check if the resize ratio is supported based on the input and output
+ *        dimension
+ * @param	input	input dimension
+ * @param	output	output dimension
+ * @return	output dimension, may be rounded.
+ * 		-1 on failure
+ */
+static int check_simple_retry(scale_t * scale, int input, int output)
+{
+	unsigned short int_out;	/* PrP internel width or height */
+	unsigned short orig_out = output;
+
+	if (prp_scale(scale, input, output, input, &orig_out, &int_out,
+		      SCALE_RETRY))
+		return -1;	/* resize failed */
+	else
+		return int_out;
+}
+
+/*!
+ * @brief Check if the resize ratio is supported by PrP channel 1
+ * @param cfg	Pointer to emma_prp_cfg structure
+ * @return	Zero on success, others on failure
+ */
+static int prp_resize_check_ch1(emma_prp_cfg * cfg)
+{
+	int in_w, in_h, ch1_w, ch1_h, ch2_w, ch2_h, w, h;
+	scale_t *pscale = &cfg->scale[0];	/* Ch1 width resize coeff */
+
+	if (cfg->ch1_pix == PRP_PIX1_UNUSED)
+		return 0;
+
+	in_w = cfg->in_width;
+	in_h = cfg->in_height;
+	ch1_w = cfg->ch1_width;
+	ch1_h = cfg->ch1_height;
+	ch2_w = cfg->ch2_width;
+	ch2_h = cfg->ch2_height;
+
+	/*
+	 * For channel 1, try parallel resize first. If the resize
+	 * ratio is not exactly supported, try cascade resize. If it
+	 * still fails, use parallel resize but with rounded value.
+	 */
+	w = check_simple(pscale, in_w, ch1_w);
+	h = check_simple(pscale + 1, in_h, ch1_h);
+	if ((w == ch1_w) && (h == ch1_h))
+		goto exit_parallel;
+
+	if (cfg->ch2_pix != PRP_PIX2_UNUSED) {
+		/*
+		 * Channel 2 is already used. The pscale is still pointing
+		 * to ch1 resize coeff for temporary use.
+		 */
+		w = check_simple(pscale, in_w, ch2_w);
+		h = check_simple(pscale + 1, in_h, ch2_h);
+		if ((w == ch2_w) && (h == ch2_h)) {
+			/* Try cascade resize now */
+			w = check_simple(pscale, ch2_w, ch1_w);
+			h = check_simple(pscale + 1, ch2_h, ch1_h);
+			if ((w == ch1_w) && (h == ch1_h))
+				goto exit_cascade;
+		}
+	} else {
+		/*
+		 * Try cascade resize for width, width is multiple of 2.
+		 * Channel 2 is not used. So we have more values to pick
+		 * for channel 2 resize.
+		 */
+		for (w = in_w - 2; w > ch1_w; w -= 2) {
+			/* Ch2 width resize */
+			if (check_simple(pscale + 2, in_w, w) != w)
+				continue;
+			/* Ch1 width resize */
+			if (check_simple(pscale, w, ch1_w) != ch1_w)
+				continue;
+			break;
+		}
+		if ((ch2_w = w) > ch1_w) {
+			/* try cascade resize for height */
+			for (h = in_h - 1; h > ch1_h; h--) {
+				/* Ch2 height resize */
+				if (check_simple(pscale + 3, in_h, h) != h)
+					continue;
+				/* Ch1 height resize */
+				if (check_simple(pscale + 1, h, ch1_h) != ch1_h)
+					continue;
+				break;
+			}
+			if ((ch2_h = h) > ch1_h)
+				goto exit_cascade;
+		}
+	}
+
+	/* Have to try parallel resize again and round the dimensions */
+	w = check_simple_retry(pscale, in_w, ch1_w);
+	h = check_simple_retry(pscale + 1, in_h, ch1_h);
+	if ((w != -1) && (h != -1))
+		goto exit_parallel;
+
+	pr_debug("Ch1 resize error.\n");
+	return -1;
+
+      exit_parallel:
+	cfg->ch1_scale.algo |= PRP_ALGO_BYPASS;
+	pr_debug("ch1 parallel resize.\n");
+	pr_debug("original width = %d internel width = %d\n", ch1_w, w);
+	pr_debug("original height = %d internel height = %d\n", ch1_h, h);
+	return 0;
+
+      exit_cascade:
+	cfg->ch1_scale.algo &= ~PRP_ALGO_BYPASS;
+	pr_debug("ch1 cascade resize.\n");
+	pr_debug("[width] in : ch2 : ch1=%d : %d : %d\n", in_w, ch2_w, ch1_w);
+	pr_debug("[height] in : ch2 : ch1=%d : %d : %d\n", in_h, ch2_h, ch1_h);
+	return 0;
+}
+
+/*!
+ * @brief Check if the resize ratio is supported by PrP channel 2
+ * @param cfg	Pointer to emma_prp_cfg structure
+ * @return	Zero on success, others on failure
+ */
+static int prp_resize_check_ch2(emma_prp_cfg * cfg)
+{
+	int w, h;
+	scale_t *pscale = &cfg->scale[2];	/* Ch2 width resize coeff */
+
+	if (cfg->ch2_pix == PRP_PIX2_UNUSED)
+		return 0;
+
+	w = check_simple_retry(pscale, cfg->in_width, cfg->ch2_width);
+	h = check_simple_retry(pscale + 1, cfg->in_height, cfg->ch2_height);
+	if ((w != -1) && (h != -1)) {
+		pr_debug("Ch2 resize.\n");
+		pr_debug("Original width = %d internel width = %d\n",
+			 cfg->ch2_width, w);
+		pr_debug("Original height = %d internel height = %d\n",
+			 cfg->ch2_height, h);
+		return 0;
+	} else {
+		pr_debug("Ch2 resize error.\n");
+		return -1;
+	}
+}
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.c b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
new file mode 100644
index 000000000000..1852d702e505
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
@@ -0,0 +1,2593 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/media/video/mxc/capture/mxc_v4l2_capture.c
+ *
+ * @brief Mxc Video For Linux 2 driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/io.h>
+#include <linux/semaphore.h>
+#include <linux/pagemap.h>
+#include <linux/vmalloc.h>
+#include <linux/types.h>
+#include <linux/fb.h>
+#include <linux/dma-mapping.h>
+#include <linux/mxcfb.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-int-device.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+static int video_nr = -1;
+static cam_data *g_cam;
+
+/*! This data is used for the output to the display. */
+#define MXC_V4L2_CAPTURE_NUM_OUTPUTS	3
+#define MXC_V4L2_CAPTURE_NUM_INPUTS	2
+static struct v4l2_output mxc_capture_outputs[MXC_V4L2_CAPTURE_NUM_OUTPUTS] = {
+	{
+	 .index = 0,
+	 .name = "DISP3 BG",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN,
+	 },
+	{
+	 .index = 1,
+	 .name = "DISP0",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN,
+	 },
+	{
+	 .index = 2,
+	 .name = "DISP3 FG",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN,
+	 },
+};
+
+static struct v4l2_input mxc_capture_inputs[MXC_V4L2_CAPTURE_NUM_INPUTS] = {
+	{
+	 .index = 0,
+	 .name = "CSI IC MEM",
+	 .type = V4L2_INPUT_TYPE_CAMERA,
+	 .audioset = 0,
+	 .tuner = 0,
+	 .std = V4L2_STD_UNKNOWN,
+	 .status = 0,
+	 },
+	{
+	 .index = 1,
+	 .name = "CSI MEM",
+	 .type = V4L2_INPUT_TYPE_CAMERA,
+	 .audioset = 0,
+	 .tuner = 0,
+	 .std = V4L2_STD_UNKNOWN,
+	 .status = V4L2_IN_ST_NO_POWER,
+	 },
+};
+
+/*! List of TV input video formats supported. The video formats is corresponding
+ * to the v4l2_id in video_fmt_t.
+ * Currently, only PAL and NTSC is supported. Needs to be expanded in the
+ * future.
+ */
+typedef enum {
+	TV_NTSC = 0,		/*!< Locked on (M) NTSC video signal. */
+	TV_PAL,			/*!< (B, G, H, I, N)PAL video signal. */
+	TV_NOT_LOCKED,		/*!< Not locked on a signal. */
+} video_fmt_idx;
+
+/*! Number of video standards supported (including 'not locked' signal). */
+#define TV_STD_MAX		(TV_NOT_LOCKED + 1)
+
+/*! Video format structure. */
+typedef struct {
+	int v4l2_id;		/*!< Video for linux ID. */
+	char name[16];		/*!< Name (e.g., "NTSC", "PAL", etc.) */
+	u16 raw_width;		/*!< Raw width. */
+	u16 raw_height;		/*!< Raw height. */
+	u16 active_width;	/*!< Active width. */
+	u16 active_height;	/*!< Active height. */
+	u16 active_top;		/*!< Active top. */
+	u16 active_left;	/*!< Active left. */
+} video_fmt_t;
+
+/*!
+ * Description of video formats supported.
+ *
+ *  PAL: raw=720x625, active=720x576.
+ *  NTSC: raw=720x525, active=720x480.
+ */
+static video_fmt_t video_fmts[] = {
+	{			/*! NTSC */
+	 .v4l2_id = V4L2_STD_NTSC,
+	 .name = "NTSC",
+	 .raw_width = 720 - 1,		/* SENS_FRM_WIDTH */
+	 .raw_height = 288 - 1,		/* SENS_FRM_HEIGHT */
+	 .active_width = 720,		/* ACT_FRM_WIDTH plus 1 */
+	 .active_height = (480 / 2),	/* ACT_FRM_HEIGHT plus 1 */
+	 .active_top = 12,
+	 .active_left = 0,
+	 },
+	{			/*! (B, G, H, I, N) PAL */
+	 .v4l2_id = V4L2_STD_PAL,
+	 .name = "PAL",
+	 .raw_width = 720 - 1,
+	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .active_width = 720,
+	 .active_height = (576 / 2),
+	 .active_top = 0,
+	 .active_left = 0,
+	 },
+	{			/*! Unlocked standard */
+	 .v4l2_id = V4L2_STD_ALL,
+	 .name = "Autodetect",
+	 .raw_width = 720 - 1,
+	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .active_width = 720,
+	 .active_height = (576 / 2),
+	 .active_top = 0,
+	 .active_left = 0,
+	 },
+};
+
+/*!* Standard index of TV. */
+static video_fmt_idx video_index = TV_NOT_LOCKED;
+
+static int mxc_v4l2_master_attach(struct v4l2_int_device *slave);
+static void mxc_v4l2_master_detach(struct v4l2_int_device *slave);
+static u8 camera_power(cam_data *cam, bool cameraOn);
+
+/*! Information about this driver. */
+static struct v4l2_int_master mxc_v4l2_master = {
+	.attach = mxc_v4l2_master_attach,
+	.detach = mxc_v4l2_master_detach,
+};
+
+static struct v4l2_int_device mxc_v4l2_int_device = {
+	.module = THIS_MODULE,
+	.name = "mxc_v4l2_cap",
+	.type = v4l2_int_type_master,
+	.u = {
+		.master = &mxc_v4l2_master,
+		},
+};
+
+/***************************************************************************
+ * Functions for handling Frame buffers.
+ **************************************************************************/
+
+/*!
+ * Free frame buffers
+ *
+ * @param cam      Structure cam_data *
+ *
+ * @return status  0 success.
+ */
+static int mxc_free_frame_buf(cam_data *cam)
+{
+	int i;
+
+	pr_debug("MVC: In mxc_free_frame_buf\n");
+
+	for (i = 0; i < FRAME_NUM; i++) {
+		if (cam->frame[i].vaddress != 0) {
+			dma_free_coherent(0, cam->frame[i].buffer.length,
+					  cam->frame[i].vaddress,
+					  cam->frame[i].paddress);
+			cam->frame[i].vaddress = 0;
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * Allocate frame buffers
+ *
+ * @param cam      Structure cam_data*
+ * @param count    int number of buffer need to allocated
+ *
+ * @return status  -0 Successfully allocated a buffer, -ENOBUFS	failed.
+ */
+static int mxc_allocate_frame_buf(cam_data *cam, int count)
+{
+	int i;
+
+	pr_debug("In MVC:mxc_allocate_frame_buf - size=%d\n",
+		cam->v2f.fmt.pix.sizeimage);
+
+	for (i = 0; i < count; i++) {
+		cam->frame[i].vaddress =
+		    dma_alloc_coherent(0,
+				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+				       &cam->frame[i].paddress,
+				       GFP_DMA | GFP_KERNEL);
+		if (cam->frame[i].vaddress == 0) {
+			pr_err("ERROR: v4l2 capture: "
+				"mxc_allocate_frame_buf failed.\n");
+			mxc_free_frame_buf(cam);
+			return -ENOBUFS;
+		}
+		cam->frame[i].buffer.index = i;
+		cam->frame[i].buffer.flags = V4L2_BUF_FLAG_MAPPED;
+		cam->frame[i].buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cam->frame[i].buffer.length =
+		    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
+		cam->frame[i].buffer.memory = V4L2_MEMORY_MMAP;
+		cam->frame[i].buffer.m.offset = cam->frame[i].paddress;
+		cam->frame[i].index = i;
+	}
+
+	return 0;
+}
+
+/*!
+ * Free frame buffers status
+ *
+ * @param cam    Structure cam_data *
+ *
+ * @return none
+ */
+static void mxc_free_frames(cam_data *cam)
+{
+	int i;
+
+	pr_debug("In MVC:mxc_free_frames\n");
+
+	for (i = 0; i < FRAME_NUM; i++) {
+		cam->frame[i].buffer.flags = V4L2_BUF_FLAG_MAPPED;
+	}
+
+	cam->enc_counter = 0;
+	cam->skip_frame = 0;
+	INIT_LIST_HEAD(&cam->ready_q);
+	INIT_LIST_HEAD(&cam->working_q);
+	INIT_LIST_HEAD(&cam->done_q);
+}
+
+/*!
+ * Return the buffer status
+ *
+ * @param cam 	   Structure cam_data *
+ * @param buf      Structure v4l2_buffer *
+ *
+ * @return status  0 success, EINVAL failed.
+ */
+static int mxc_v4l2_buffer_status(cam_data *cam, struct v4l2_buffer *buf)
+{
+	pr_debug("In MVC:mxc_v4l2_buffer_status\n");
+
+	if (buf->index < 0 || buf->index >= FRAME_NUM) {
+		pr_err("ERROR: v4l2 capture: mxc_v4l2_buffer_status buffers "
+		       "not allocated\n");
+		return -EINVAL;
+	}
+
+	memcpy(buf, &(cam->frame[buf->index].buffer), sizeof(*buf));
+	return 0;
+}
+
+/***************************************************************************
+ * Functions for handling the video stream.
+ **************************************************************************/
+
+/*!
+ * Indicates whether the palette is supported.
+ *
+ * @param palette V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	return ((palette == V4L2_PIX_FMT_RGB565) ||
+		(palette == V4L2_PIX_FMT_BGR24) ||
+		(palette == V4L2_PIX_FMT_RGB24) ||
+		(palette == V4L2_PIX_FMT_BGR32) ||
+		(palette == V4L2_PIX_FMT_RGB32) ||
+		(palette == V4L2_PIX_FMT_YUV422P) ||
+		(palette == V4L2_PIX_FMT_UYVY) ||
+		(palette == V4L2_PIX_FMT_YUV420) ||
+		(palette == V4L2_PIX_FMT_NV12));
+}
+
+/*!
+ * Start the encoder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_streamon(cam_data *cam)
+{
+	struct mxc_v4l_frame *frame;
+	int err = 0;
+
+	pr_debug("In MVC:mxc_streamon\n");
+
+	if (NULL == cam) {
+		pr_err("ERROR! cam parameter is NULL\n");
+		return -1;
+	}
+
+	if (list_empty(&cam->ready_q)) {
+		pr_err("ERROR: v4l2 capture: mxc_streamon buffer has not been "
+			"queued yet\n");
+		return -EINVAL;
+	}
+
+	cam->capture_pid = current->pid;
+
+	if (cam->enc_enable) {
+		err = cam->enc_enable(cam);
+		if (err != 0) {
+			return err;
+		}
+	}
+
+	cam->ping_pong_csi = 0;
+	if (cam->enc_update_eba) {
+		frame =
+		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
+		list_del(cam->ready_q.next);
+		list_add_tail(&frame->queue, &cam->working_q);
+		err = cam->enc_update_eba(frame->buffer.m.offset,
+					  &cam->ping_pong_csi);
+
+		frame =
+		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
+		list_del(cam->ready_q.next);
+		list_add_tail(&frame->queue, &cam->working_q);
+		err |= cam->enc_update_eba(frame->buffer.m.offset,
+					   &cam->ping_pong_csi);
+	} else {
+		return -EINVAL;
+	}
+
+	cam->capture_on = true;
+
+	return err;
+}
+
+/*!
+ * Shut down the encoder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_streamoff(cam_data *cam)
+{
+	int err = 0;
+
+	pr_debug("In MVC:mxc_streamoff\n");
+
+	if (cam->capture_on == false)
+		return 0;
+
+	if (cam->enc_disable) {
+		err = cam->enc_disable(cam);
+	}
+	mxc_free_frames(cam);
+	mxc_capture_inputs[cam->current_input].status |= V4L2_IN_ST_NO_POWER;
+	cam->capture_on = false;
+	return err;
+}
+
+/*!
+ * Valid and adjust the overlay window size, position
+ *
+ * @param cam      structure cam_data *
+ * @param win      struct v4l2_window  *
+ *
+ * @return 0
+ */
+static int verify_preview(cam_data *cam, struct v4l2_window *win)
+{
+	int i = 0, width_bound = 0, height_bound = 0;
+	int *width, *height;
+	struct fb_info *bg_fbi = NULL;
+	bool foregound_fb;
+
+	pr_debug("In MVC: verify_preview\n");
+
+	do {
+		cam->overlay_fb = (struct fb_info *)registered_fb[i];
+		if (cam->overlay_fb == NULL) {
+			pr_err("ERROR: verify_preview frame buffer NULL.\n");
+			return -1;
+		}
+		if (strcmp(cam->overlay_fb->fix.id, "DISP3 BG") == 0)
+			bg_fbi = cam->overlay_fb;
+		if (strcmp(cam->overlay_fb->fix.id,
+			    mxc_capture_outputs[cam->output].name) == 0) {
+			if (strcmp(cam->overlay_fb->fix.id, "DISP3 FG") == 0)
+				foregound_fb = true;
+			break;
+		}
+	} while (++i < FB_MAX);
+
+	if (foregound_fb) {
+		width_bound = bg_fbi->var.xres;
+		height_bound = bg_fbi->var.yres;
+
+		if (win->w.width + win->w.left > bg_fbi->var.xres ||
+		    win->w.height + win->w.top > bg_fbi->var.yres) {
+			pr_err("ERROR: FG window position exceeds.\n");
+			return -1;
+		}
+	} else {
+		/* 4 bytes alignment for BG */
+		width_bound = cam->overlay_fb->var.xres;
+		height_bound = cam->overlay_fb->var.yres;
+
+		if (cam->overlay_fb->var.bits_per_pixel == 24) {
+			win->w.left -= win->w.left % 4;
+		} else if (cam->overlay_fb->var.bits_per_pixel == 16) {
+			win->w.left -= win->w.left % 2;
+		}
+
+		if (win->w.width + win->w.left > cam->overlay_fb->var.xres)
+			win->w.width = cam->overlay_fb->var.xres - win->w.left;
+		if (win->w.height + win->w.top > cam->overlay_fb->var.yres)
+			win->w.height = cam->overlay_fb->var.yres - win->w.top;
+	}
+
+	/* stride line limitation */
+	win->w.height -= win->w.height % 8;
+	win->w.width -= win->w.width % 8;
+
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		height = &win->w.width;
+		width = &win->w.height;
+	} else {
+		width = &win->w.width;
+		height = &win->w.height;
+	}
+
+	if ((cam->crop_bounds.width / *width > 8) ||
+	    ((cam->crop_bounds.width / *width == 8) &&
+	     (cam->crop_bounds.width % *width))) {
+		*width = cam->crop_bounds.width / 8;
+		if (*width % 8)
+			*width += 8 - *width % 8;
+		if (*width + win->w.left > width_bound) {
+			pr_err("ERROR: v4l2 capture: width exceeds "
+				"resize limit.\n");
+			return -1;
+		}
+		pr_err("ERROR: v4l2 capture: width exceeds limit. "
+			"Resize to %d.\n",
+			*width);
+	}
+
+	if ((cam->crop_bounds.height / *height > 8) ||
+	    ((cam->crop_bounds.height / *height == 8) &&
+	     (cam->crop_bounds.height % *height))) {
+		*height = cam->crop_bounds.height / 8;
+		if (*height % 8)
+			*height += 8 - *height % 8;
+		if (*height + win->w.top > height_bound) {
+			pr_err("ERROR: v4l2 capture: height exceeds "
+				"resize limit.\n");
+			return -1;
+		}
+		pr_err("ERROR: v4l2 capture: height exceeds limit "
+			"resize to %d.\n",
+			*height);
+	}
+
+	return 0;
+}
+
+/*!
+ * start the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int start_preview(cam_data *cam)
+{
+	int err = 0;
+
+	pr_debug("MVC: start_preview\n");
+
+#if defined(CONFIG_MXC_IPU_PRP_VF_SDC) || defined(CONFIG_MXC_IPU_PRP_VF_SDC_MODULE)
+	pr_debug("   This is an SDC display\n");
+	if (cam->output == 0 || cam->output == 2) {
+		if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY)
+			err = prp_vf_sdc_select(cam);
+		else if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_PRIMARY)
+			err = prp_vf_sdc_select_bg(cam);
+		if (err != 0)
+			return err;
+
+		err = cam->vf_start_sdc(cam);
+	}
+#endif
+
+#if defined(CONFIG_MXC_IPU_PRP_VF_ADC) || defined(CONFIG_MXC_IPU_PRP_VF_ADC_MODULE)
+	pr_debug("   This is an ADC display\n");
+	if (cam->output == 1) {
+		err = prp_vf_adc_select(cam);
+		if (err != 0)
+			return err;
+
+		err = cam->vf_start_adc(cam);
+	}
+#endif
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__,
+		 cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+	pr_debug("End of %s: crop_bounds widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_bounds.width, cam->crop_bounds.height);
+	pr_debug("End of %s: crop_defrect widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_defrect.width, cam->crop_defrect.height);
+	pr_debug("End of %s: crop_current widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_current.width, cam->crop_current.height);
+
+	return err;
+}
+
+/*!
+ * shut down the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int stop_preview(cam_data *cam)
+{
+	int err = 0;
+
+	pr_debug("MVC: stop preview\n");
+
+#if defined(CONFIG_MXC_IPU_PRP_VF_ADC) || defined(CONFIG_MXC_IPU_PRP_VF_ADC_MODULE)
+	if (cam->output == 1) {
+		err = prp_vf_adc_deselect(cam);
+	}
+#endif
+
+#if defined(CONFIG_MXC_IPU_PRP_VF_SDC) || defined(CONFIG_MXC_IPU_PRP_VF_SDC_MODULE)
+	if (cam->output == 0 || cam->output == 2) {
+		if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY)
+			err = prp_vf_sdc_deselect(cam);
+		else if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_PRIMARY)
+			err = prp_vf_sdc_deselect_bg(cam);
+	}
+#endif
+
+	return err;
+}
+
+/***************************************************************************
+ * VIDIOC Functions.
+ **************************************************************************/
+
+/*!
+ * V4L2 - mxc_v4l2_g_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_g_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+
+	pr_debug("In MVC: mxc_v4l2_g_fmt type=%d\n", f->type);
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		f->fmt.pix = cam->v2f.fmt.pix;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_OVERLAY\n");
+		f->fmt.win = cam->win;
+		break;
+	default:
+		pr_debug("   type is invalid\n");
+		retval = -EINVAL;
+	}
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__,
+		 cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+	pr_debug("End of %s: crop_bounds widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_bounds.width, cam->crop_bounds.height);
+	pr_debug("End of %s: crop_defrect widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_defrect.width, cam->crop_defrect.height);
+	pr_debug("End of %s: crop_current widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_current.width, cam->crop_current.height);
+
+	return retval;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_s_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+	int size = 0;
+	int bytesperline = 0;
+	int *width, *height;
+
+	pr_debug("In MVC: mxc_v4l2_s_fmt\n");
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type=V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		if (!valid_mode(f->fmt.pix.pixelformat)) {
+			pr_err("ERROR: v4l2 capture: mxc_v4l2_s_fmt: format "
+			       "not supported\n");
+			return -EINVAL;
+		}
+
+		/*
+		 * Force the capture window resolution to be crop bounds
+		 * for CSI MEM input mode.
+		 */
+		if (strcmp(mxc_capture_inputs[cam->current_input].name,
+			   "CSI MEM") == 0) {
+			f->fmt.pix.width = cam->crop_bounds.width;
+			f->fmt.pix.height = cam->crop_bounds.height;
+		}
+
+		/* Handle case where size requested is larger than cuurent
+		 * camera setting. */
+		if ((f->fmt.pix.width > cam->crop_bounds.width)
+			|| (f->fmt.pix.height > cam->crop_bounds.height)) {
+			/* Need the logic here, calling vidioc_s_param if
+			 * camera can change. */
+			/* For the moment, just return an error. */
+			return -EINVAL;
+		}
+
+		if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+			height = &f->fmt.pix.width;
+			width = &f->fmt.pix.height;
+		} else {
+			width = &f->fmt.pix.width;
+			height = &f->fmt.pix.height;
+		}
+
+		/* stride line limitation */
+		*width -= *width % 8;
+		*height -= *height % 8;
+
+		if ((cam->crop_bounds.width / *width > 8) ||
+		    ((cam->crop_bounds.width / *width == 8) &&
+		     (cam->crop_bounds.width % *width))) {
+			*width = cam->crop_bounds.width / 8;
+			if (*width % 8)
+				*width += 8 - *width % 8;
+			pr_err("ERROR: v4l2 capture: width exceeds limit "
+				"resize to %d.\n",
+			       *width);
+		}
+
+		if ((cam->crop_bounds.height / *height > 8) ||
+		    ((cam->crop_bounds.height / *height == 8) &&
+		     (cam->crop_bounds.height % *height))) {
+			*height = cam->crop_bounds.height / 8;
+			if (*height % 8)
+				*height += 8 - *height % 8;
+			pr_err("ERROR: v4l2 capture: height exceeds limit "
+			       "resize to %d.\n",
+			       *height);
+		}
+
+		switch (f->fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_RGB565:
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			bytesperline = f->fmt.pix.width * 2;
+			break;
+		case V4L2_PIX_FMT_BGR24:
+			size = f->fmt.pix.width * f->fmt.pix.height * 3;
+			bytesperline = f->fmt.pix.width * 3;
+			break;
+		case V4L2_PIX_FMT_RGB24:
+			size = f->fmt.pix.width * f->fmt.pix.height * 3;
+			bytesperline = f->fmt.pix.width * 3;
+			break;
+		case V4L2_PIX_FMT_BGR32:
+			size = f->fmt.pix.width * f->fmt.pix.height * 4;
+			bytesperline = f->fmt.pix.width * 4;
+			break;
+		case V4L2_PIX_FMT_RGB32:
+			size = f->fmt.pix.width * f->fmt.pix.height * 4;
+			bytesperline = f->fmt.pix.width * 4;
+			break;
+		case V4L2_PIX_FMT_YUV422P:
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			bytesperline = f->fmt.pix.width;
+			break;
+		case V4L2_PIX_FMT_UYVY:
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			bytesperline = f->fmt.pix.width * 2;
+			break;
+		case V4L2_PIX_FMT_YUV420:
+			size = f->fmt.pix.width * f->fmt.pix.height * 3 / 2;
+			bytesperline = f->fmt.pix.width;
+			break;
+		case V4L2_PIX_FMT_NV12:
+			size = f->fmt.pix.width * f->fmt.pix.height * 3 / 2;
+			bytesperline = f->fmt.pix.width;
+			break;
+		default:
+			break;
+		}
+
+		if (f->fmt.pix.bytesperline < bytesperline) {
+			f->fmt.pix.bytesperline = bytesperline;
+		} else {
+			bytesperline = f->fmt.pix.bytesperline;
+		}
+
+		if (f->fmt.pix.sizeimage < size) {
+			f->fmt.pix.sizeimage = size;
+		} else {
+			size = f->fmt.pix.sizeimage;
+		}
+
+		cam->v2f.fmt.pix = f->fmt.pix;
+
+		if (cam->v2f.fmt.pix.priv != 0) {
+			if (copy_from_user(&cam->offset,
+					   (void *)cam->v2f.fmt.pix.priv,
+					   sizeof(cam->offset))) {
+				retval = -EFAULT;
+				break;
+			}
+		}
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		pr_debug("   type=V4L2_BUF_TYPE_VIDEO_OVERLAY\n");
+		retval = verify_preview(cam, &f->fmt.win);
+		cam->win = f->fmt.win;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__,
+		 cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+	pr_debug("End of %s: crop_bounds widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_bounds.width, cam->crop_bounds.height);
+	pr_debug("End of %s: crop_defrect widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_defrect.width, cam->crop_defrect.height);
+	pr_debug("End of %s: crop_current widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_current.width, cam->crop_current.height);
+
+	return retval;
+}
+
+/*!
+ * get control param
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_g_ctrl(cam_data *cam, struct v4l2_control *c)
+{
+	int status = 0;
+
+	pr_debug("In MVC:mxc_v4l2_g_ctrl\n");
+
+	/* probably don't need to store the values that can be retrieved,
+	 * locally, but they are for now. */
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		/* This is handled in the ipu. */
+		if (cam->rotation == IPU_ROTATE_HORIZ_FLIP)
+			c->value = 1;
+		break;
+	case V4L2_CID_VFLIP:
+		/* This is handled in the ipu. */
+		if (cam->rotation == IPU_ROTATE_VERT_FLIP)
+			c->value = 1;
+		break;
+	case V4L2_CID_MXC_ROT:
+		/* This is handled in the ipu. */
+		c->value = cam->rotation;
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		c->value = cam->bright;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->bright = c->value;
+		break;
+	case V4L2_CID_HUE:
+		c->value = cam->hue;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->hue = c->value;
+		break;
+	case V4L2_CID_CONTRAST:
+		c->value = cam->contrast;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->contrast = c->value;
+		break;
+	case V4L2_CID_SATURATION:
+		c->value = cam->saturation;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->saturation = c->value;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		c->value = cam->red;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->red = c->value;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		c->value = cam->blue;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->blue = c->value;
+		break;
+	case V4L2_CID_BLACK_LEVEL:
+		c->value = cam->ae_mode;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->ae_mode = c->value;
+		break;
+	default:
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+	}
+
+	return status;
+}
+
+/*!
+ * V4L2 - set_control function
+ *          V4L2_CID_PRIVATE_BASE is the extention for IPU preprocessing.
+ *          0 for normal operation
+ *          1 for vertical flip
+ *          2 for horizontal flip
+ *          3 for horizontal and vertical flip
+ *          4 for 90 degree rotation
+ * @param cam         structure cam_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_ctrl(cam_data *cam, struct v4l2_control *c)
+{
+	int ret = 0;
+	int tmp_rotation = IPU_ROTATE_NONE;
+
+	pr_debug("In MVC:mxc_v4l2_s_ctrl\n");
+
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		/* This is done by the IPU */
+		if (c->value == 1) {
+			if ((cam->rotation != IPU_ROTATE_VERT_FLIP) &&
+			    (cam->rotation != IPU_ROTATE_180))
+				cam->rotation = IPU_ROTATE_HORIZ_FLIP;
+			else
+				cam->rotation = IPU_ROTATE_180;
+		} else {
+			if (cam->rotation == IPU_ROTATE_HORIZ_FLIP)
+				cam->rotation = IPU_ROTATE_NONE;
+			if (cam->rotation == IPU_ROTATE_180)
+				cam->rotation = IPU_ROTATE_VERT_FLIP;
+		}
+		break;
+	case V4L2_CID_VFLIP:
+		/* This is done by the IPU */
+		if (c->value == 1) {
+			if ((cam->rotation != IPU_ROTATE_HORIZ_FLIP) &&
+			    (cam->rotation != IPU_ROTATE_180))
+				cam->rotation = IPU_ROTATE_VERT_FLIP;
+			else
+				cam->rotation = IPU_ROTATE_180;
+		} else {
+			if (cam->rotation == IPU_ROTATE_VERT_FLIP)
+				cam->rotation = IPU_ROTATE_NONE;
+			if (cam->rotation == IPU_ROTATE_180)
+				cam->rotation = IPU_ROTATE_HORIZ_FLIP;
+		}
+		break;
+	case V4L2_CID_MXC_ROT:
+	case V4L2_CID_MXC_VF_ROT:
+		/* This is done by the IPU */
+		switch (c->value) {
+		case V4L2_MXC_ROTATE_NONE:
+			tmp_rotation = IPU_ROTATE_NONE;
+			break;
+		case V4L2_MXC_ROTATE_VERT_FLIP:
+			tmp_rotation = IPU_ROTATE_VERT_FLIP;
+			break;
+		case V4L2_MXC_ROTATE_HORIZ_FLIP:
+			tmp_rotation = IPU_ROTATE_HORIZ_FLIP;
+			break;
+		case V4L2_MXC_ROTATE_180:
+			tmp_rotation = IPU_ROTATE_180;
+			break;
+		case V4L2_MXC_ROTATE_90_RIGHT:
+			tmp_rotation = IPU_ROTATE_90_RIGHT;
+			break;
+		case V4L2_MXC_ROTATE_90_RIGHT_VFLIP:
+			tmp_rotation = IPU_ROTATE_90_RIGHT_VFLIP;
+			break;
+		case V4L2_MXC_ROTATE_90_RIGHT_HFLIP:
+			tmp_rotation = IPU_ROTATE_90_RIGHT_HFLIP;
+			break;
+		case V4L2_MXC_ROTATE_90_LEFT:
+			tmp_rotation = IPU_ROTATE_90_LEFT;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+
+		if (c->id == V4L2_CID_MXC_VF_ROT)
+			cam->vf_rotation = tmp_rotation;
+		else
+			cam->rotation = tmp_rotation;
+
+		break;
+	case V4L2_CID_HUE:
+		cam->hue = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_CONTRAST:
+		cam->contrast = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		cam->bright = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_SATURATION:
+		cam->saturation = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		cam->red = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		cam->blue = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_EXPOSURE:
+		cam->ae_mode = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_MXC_FLASH:
+#ifdef CONFIG_MXC_IPU_V1
+		ipu_csi_flash_strobe(true);
+#endif
+		break;
+	default:
+		pr_debug("   default case\n");
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_s_param function
+ * Allows setting of capturemode and frame rate.
+ *
+ * @param cam         structure cam_data *
+ * @param parm        structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
+{
+	struct v4l2_ifparm ifparm;
+	struct v4l2_format cam_fmt;
+	struct v4l2_streamparm currentparm;
+	ipu_csi_signal_cfg_t csi_param;
+	int err = 0;
+
+	pr_debug("In mxc_v4l2_s_param\n");
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pr_err(KERN_ERR "mxc_v4l2_s_param invalid type\n");
+		return -EINVAL;
+	}
+
+	/* Stop the viewfinder */
+	if (cam->overlay_on == true) {
+		stop_preview(cam);
+	}
+
+	currentparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* First check that this device can support the changes requested. */
+	err = vidioc_int_g_parm(cam->sensor, &currentparm);
+	if (err) {
+		pr_err("%s: vidioc_int_g_parm returned an error %d\n",
+			__func__, err);
+		goto exit;
+	}
+
+	pr_debug("   Current capabilities are %x\n",
+			currentparm.parm.capture.capability);
+	pr_debug("   Current capturemode is %d  change to %d\n",
+			currentparm.parm.capture.capturemode,
+			parm->parm.capture.capturemode);
+	pr_debug("   Current framerate is %d  change to %d\n",
+			currentparm.parm.capture.timeperframe.denominator,
+			parm->parm.capture.timeperframe.denominator);
+
+	/* This will change any camera settings needed. */
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+	err = vidioc_int_s_parm(cam->sensor, parm);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+	if (err) {
+		pr_err("%s: vidioc_int_s_parm returned an error %d\n",
+			__func__, err);
+		goto exit;
+	}
+
+	/* If resolution changed, need to re-program the CSI */
+	/* Get new values. */
+	vidioc_int_g_ifparm(cam->sensor, &ifparm);
+
+	csi_param.data_width = 0;
+	csi_param.clk_mode = 0;
+	csi_param.ext_vsync = 0;
+	csi_param.Vsync_pol = 0;
+	csi_param.Hsync_pol = 0;
+	csi_param.pixclk_pol = 0;
+	csi_param.data_pol = 0;
+	csi_param.sens_clksrc = 0;
+	csi_param.pack_tight = 0;
+	csi_param.force_eof = 0;
+	csi_param.data_en_pol = 0;
+	csi_param.data_fmt = 0;
+	csi_param.csi = 0;
+	csi_param.mclk = 0;
+
+	/* This may not work on other platforms. Check when adding a new one.*/
+	pr_debug("   clock_curr=mclk=%d\n", ifparm.u.bt656.clock_curr);
+	if (ifparm.u.bt656.clock_curr == 0) {
+		csi_param.clk_mode = IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE;
+	} else {
+		csi_param.clk_mode = IPU_CSI_CLK_MODE_GATED_CLK;
+	}
+
+	csi_param.pixclk_pol = ifparm.u.bt656.latch_clk_inv;
+
+	if (ifparm.u.bt656.mode == V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT) {
+		csi_param.data_width = IPU_CSI_DATA_WIDTH_8;
+	} else if (ifparm.u.bt656.mode
+				== V4L2_IF_TYPE_BT656_MODE_NOBT_10BIT) {
+		csi_param.data_width = IPU_CSI_DATA_WIDTH_10;
+	} else {
+		csi_param.data_width = IPU_CSI_DATA_WIDTH_8;
+	}
+
+	csi_param.Vsync_pol = ifparm.u.bt656.nobt_vs_inv;
+	csi_param.Hsync_pol = ifparm.u.bt656.nobt_hs_inv;
+	csi_param.ext_vsync = ifparm.u.bt656.bt_sync_correct;
+
+	/* if the capturemode changed, the size bounds will have changed. */
+	cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vidioc_int_g_fmt_cap(cam->sensor, &cam_fmt);
+	pr_debug("   g_fmt_cap returns widthxheight of input as %d x %d\n",
+			cam_fmt.fmt.pix.width, cam_fmt.fmt.pix.height);
+
+	csi_param.data_fmt = cam_fmt.fmt.pix.pixelformat;
+
+	cam->crop_bounds.top = cam->crop_bounds.left = 0;
+	cam->crop_bounds.width = cam_fmt.fmt.pix.width;
+	cam->crop_bounds.height = cam_fmt.fmt.pix.height;
+
+	/* This essentially loses the data at the left and bottom of the image
+	 * giving a digital zoom image, if crop_current is less than the full
+	 * size of the image. */
+	ipu_csi_set_window_size(cam->crop_current.width,
+				cam->crop_current.height, cam->csi);
+	ipu_csi_set_window_pos(cam->crop_current.left,
+			       cam->crop_current.top,
+			       cam->csi);
+	ipu_csi_init_interface(cam->crop_bounds.width,
+			       cam->crop_bounds.height,
+			       cam_fmt.fmt.pix.pixelformat, csi_param);
+
+
+exit:
+	if (cam->overlay_on == true)
+		start_preview(cam);
+
+	return err;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_s_std function
+ *
+ * Sets the TV standard to be used.
+ *
+ * @param cam	      structure cam_data *
+ * @param parm	      structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_std(cam_data *cam, v4l2_std_id e)
+{
+	bool change = false;
+
+	if (e != cam->standard.id) {
+		change = true;
+	}
+
+	pr_debug("In mxc_v4l2_s_std %Lx\n", e);
+	if (e == V4L2_STD_PAL) {
+		pr_debug("   Setting standard to PAL %Lx\n", V4L2_STD_PAL);
+		cam->standard.id = V4L2_STD_PAL;
+		video_index = TV_PAL;
+		cam->crop_current.top = 0;
+	} else if (e == V4L2_STD_NTSC) {
+		pr_debug("   Setting standard to NTSC %Lx\n",
+				V4L2_STD_NTSC);
+		/* Get rid of the white dot line in NTSC signal input */
+		cam->standard.id = V4L2_STD_NTSC;
+		video_index = TV_NTSC;
+		cam->crop_current.top = 12;
+	} else {
+		cam->standard.id = V4L2_STD_ALL;
+		video_index = TV_NOT_LOCKED;
+		cam->crop_current.top = 0;
+		pr_err("ERROR: unrecognized std! %Lx (PAL=%Lx, NTSC=%Lx\n",
+			e, V4L2_STD_PAL, V4L2_STD_NTSC);
+	}
+
+	cam->standard.index = video_index;
+	strcpy(cam->standard.name, video_fmts[video_index].name);
+	cam->crop_bounds.width = video_fmts[video_index].raw_width;
+	cam->crop_bounds.height = video_fmts[video_index].raw_height;
+	cam->crop_current.width = video_fmts[video_index].active_width;
+	cam->crop_current.height = video_fmts[video_index].active_height;
+	cam->crop_current.left = 0;
+
+	return 0;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_g_std function
+ *
+ * Gets the TV standard from the TV input device.
+ *
+ * @param cam	      structure cam_data *
+ *
+ * @param e	      structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_g_std(cam_data *cam, v4l2_std_id *e)
+{
+	struct v4l2_format tv_fmt;
+
+	pr_debug("In mxc_v4l2_g_std\n");
+
+	if (cam->device_type == 1) {
+		/* Use this function to get what the TV-In device detects the
+		 * format to be. pixelformat is used to return the std value
+		 * since the interface has no vidioc_g_std.*/
+		tv_fmt.type = V4L2_BUF_TYPE_PRIVATE;
+		vidioc_int_g_fmt_cap(cam->sensor, &tv_fmt);
+
+		/* If the TV-in automatically detects the standard, then if it
+		 * changes, the settings need to change. */
+		if (cam->standard_autodetect) {
+			if (cam->standard.id != tv_fmt.fmt.pix.pixelformat) {
+				pr_debug("MVC: mxc_v4l2_g_std: "
+					"Changing standard\n");
+				mxc_v4l2_s_std(cam, tv_fmt.fmt.pix.pixelformat);
+			}
+		}
+
+		*e = tv_fmt.fmt.pix.pixelformat;
+	}
+
+	return 0;
+}
+
+/*!
+ * Dequeue one V4L capture buffer
+ *
+ * @param cam         structure cam_data *
+ * @param buf         structure v4l2_buffer *
+ *
+ * @return  status    0 success, EINVAL invalid frame number,
+ *                    ETIME timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l_dqueue(cam_data *cam, struct v4l2_buffer *buf)
+{
+	int retval = 0;
+	struct mxc_v4l_frame *frame;
+
+	pr_debug("In MVC:mxc_v4l_dqueue\n");
+
+	if (!wait_event_interruptible_timeout(cam->enc_queue,
+					      cam->enc_counter != 0, 10 * HZ)) {
+		pr_err("ERROR: v4l2 capture: mxc_v4l_dqueue timeout "
+			"enc_counter %x\n",
+		       cam->enc_counter);
+		return -ETIME;
+	} else if (signal_pending(current)) {
+		pr_err("ERROR: v4l2 capture: mxc_v4l_dqueue() "
+			"interrupt received\n");
+		return -ERESTARTSYS;
+	}
+
+	cam->enc_counter--;
+
+	frame = list_entry(cam->done_q.next, struct mxc_v4l_frame, queue);
+	list_del(cam->done_q.next);
+	if (frame->buffer.flags & V4L2_BUF_FLAG_DONE) {
+		frame->buffer.flags &= ~V4L2_BUF_FLAG_DONE;
+	} else if (frame->buffer.flags & V4L2_BUF_FLAG_QUEUED) {
+		pr_err("ERROR: v4l2 capture: VIDIOC_DQBUF: "
+			"Buffer not filled.\n");
+		frame->buffer.flags &= ~V4L2_BUF_FLAG_QUEUED;
+		retval = -EINVAL;
+	} else if ((frame->buffer.flags & 0x7) == V4L2_BUF_FLAG_MAPPED) {
+		pr_err("ERROR: v4l2 capture: VIDIOC_DQBUF: "
+			"Buffer not queued.\n");
+		retval = -EINVAL;
+	}
+
+	buf->bytesused = cam->v2f.fmt.pix.sizeimage;
+	buf->index = frame->index;
+	buf->flags = frame->buffer.flags;
+	buf->m = cam->frame[frame->index].buffer.m;
+
+	return retval;
+}
+
+/*!
+ * V4L interface - open function
+ *
+ * @param inode        structure inode *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l_open(struct inode *inode, struct file *file)
+{
+	struct v4l2_ifparm ifparm;
+	struct v4l2_format cam_fmt;
+	ipu_csi_signal_cfg_t csi_param;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int err = 0;
+
+	pr_debug("\nIn MVC: mxc_v4l_open\n");
+	pr_debug("   device name is %s\n", dev->name);
+
+	if (!cam) {
+		pr_err("ERROR: v4l2 capture: Internal error, "
+			"cam_data not found!\n");
+		return -EBADF;
+	}
+
+	down(&cam->busy_lock);
+	err = 0;
+	if (signal_pending(current))
+		goto oops;
+
+	if (cam->open_count++ == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+
+		if (strcmp(mxc_capture_inputs[cam->current_input].name,
+			   "CSI MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_CSI_ENC) || defined(CONFIG_MXC_IPU_CSI_ENC_MODULE)
+			err = csi_enc_select(cam);
+#endif
+		} else if (strcmp(mxc_capture_inputs[cam->current_input].name,
+				  "CSI IC MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_PRP_ENC) || defined(CONFIG_MXC_IPU_PRP_ENC_MODULE)
+			err = prp_enc_select(cam);
+#endif
+		}
+
+		cam->enc_counter = 0;
+		cam->skip_frame = 0;
+		INIT_LIST_HEAD(&cam->ready_q);
+		INIT_LIST_HEAD(&cam->working_q);
+		INIT_LIST_HEAD(&cam->done_q);
+
+		vidioc_int_g_ifparm(cam->sensor, &ifparm);
+
+		csi_param.sens_clksrc = 0;
+
+		csi_param.clk_mode = 0;
+		csi_param.data_pol = 0;
+		csi_param.ext_vsync = 0;
+
+		csi_param.pack_tight = 0;
+		csi_param.force_eof = 0;
+		csi_param.data_en_pol = 0;
+		csi_param.mclk = ifparm.u.bt656.clock_curr;
+
+		csi_param.pixclk_pol = ifparm.u.bt656.latch_clk_inv;
+
+		/* Once we handle multiple inputs this will need to change. */
+		csi_param.csi = 0;
+
+		if (ifparm.u.bt656.mode
+				== V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT)
+			csi_param.data_width = IPU_CSI_DATA_WIDTH_8;
+		else if (ifparm.u.bt656.mode
+				== V4L2_IF_TYPE_BT656_MODE_NOBT_10BIT)
+			csi_param.data_width = IPU_CSI_DATA_WIDTH_10;
+		else
+			csi_param.data_width = IPU_CSI_DATA_WIDTH_8;
+
+
+		csi_param.Vsync_pol = ifparm.u.bt656.nobt_vs_inv;
+		csi_param.Hsync_pol = ifparm.u.bt656.nobt_hs_inv;
+
+		csi_param.csi = cam->csi;
+
+		cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		vidioc_int_g_fmt_cap(cam->sensor, &cam_fmt);
+
+		/* Reset the sizes.  Needed to prevent carryover of last
+		 * operation.*/
+		cam->crop_bounds.top = cam->crop_bounds.left = 0;
+		cam->crop_bounds.width = cam_fmt.fmt.pix.width;
+		cam->crop_bounds.height = cam_fmt.fmt.pix.height;
+
+		/* This also is the max crop size for this device. */
+		cam->crop_defrect.top = cam->crop_defrect.left = 0;
+		cam->crop_defrect.width = cam_fmt.fmt.pix.width;
+		cam->crop_defrect.height = cam_fmt.fmt.pix.height;
+
+		/* At this point, this is also the current image size. */
+		cam->crop_current.top = cam->crop_current.left = 0;
+		cam->crop_current.width = cam_fmt.fmt.pix.width;
+		cam->crop_current.height = cam_fmt.fmt.pix.height;
+
+		pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+			__func__,
+			cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+		pr_debug("End of %s: crop_bounds widthxheight %d x %d\n",
+			__func__,
+			cam->crop_bounds.width, cam->crop_bounds.height);
+		pr_debug("End of %s: crop_defrect widthxheight %d x %d\n",
+			__func__,
+			cam->crop_defrect.width, cam->crop_defrect.height);
+		pr_debug("End of %s: crop_current widthxheight %d x %d\n",
+			__func__,
+			cam->crop_current.width, cam->crop_current.height);
+
+		csi_param.data_fmt = cam_fmt.fmt.pix.pixelformat;
+		pr_debug("On Open: Input to ipu size is %d x %d\n",
+				cam_fmt.fmt.pix.width, cam_fmt.fmt.pix.height);
+		ipu_csi_set_window_size(cam->crop_current.width,
+					cam->crop_current.width,
+					cam->csi);
+		ipu_csi_set_window_pos(cam->crop_current.left,
+					cam->crop_current.top,
+					cam->csi);
+		ipu_csi_init_interface(cam->crop_bounds.width,
+					cam->crop_bounds.height,
+					cam_fmt.fmt.pix.pixelformat,
+					csi_param);
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
+				       true, true);
+		vidioc_int_init(cam->sensor);
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
+				       false, false);
+}
+
+	file->private_data = dev;
+
+      oops:
+	up(&cam->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L interface - close function
+ *
+ * @param inode    struct inode *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int mxc_v4l_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	int err = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	pr_debug("In MVC:mxc_v4l_close\n");
+
+	if (!cam) {
+		pr_err("ERROR: v4l2 capture: Internal error, "
+			"cam_data not found!\n");
+		return -EBADF;
+	}
+
+	/* for the case somebody hit the ctrl C */
+	if (cam->overlay_pid == current->pid) {
+		err = stop_preview(cam);
+		cam->overlay_on = false;
+	}
+	if (cam->capture_pid == current->pid) {
+		err |= mxc_streamoff(cam);
+		wake_up_interruptible(&cam->enc_queue);
+	}
+
+	if (--cam->open_count == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+		pr_info("mxc_v4l_close: release resource\n");
+
+		if (strcmp(mxc_capture_inputs[cam->current_input].name,
+			   "CSI MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_CSI_ENC) || defined(CONFIG_MXC_IPU_CSI_ENC_MODULE)
+			err |= csi_enc_deselect(cam);
+#endif
+		} else if (strcmp(mxc_capture_inputs[cam->current_input].name,
+				  "CSI IC MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_PRP_ENC) || defined(CONFIG_MXC_IPU_PRP_ENC_MODULE)
+			err |= prp_enc_deselect(cam);
+#endif
+		}
+
+		mxc_free_frame_buf(cam);
+		file->private_data = NULL;
+
+		/* capture off */
+		wake_up_interruptible(&cam->enc_queue);
+		mxc_free_frames(cam);
+		cam->enc_counter++;
+	}
+
+	return err;
+}
+
+#if defined(CONFIG_MXC_IPU_PRP_ENC) || defined(CONFIG_MXC_IPU_CSI_ENC) || \
+    defined(CONFIG_MXC_IPU_PRP_ENC_MODULE) || \
+    defined(CONFIG_MXC_IPU_CSI_ENC_MODULE)
+/*
+ * V4L interface - read function
+ *
+ * @param file       struct file *
+ * @param read buf   char *
+ * @param count      size_t
+ * @param ppos       structure loff_t *
+ *
+ * @return           bytes read
+ */
+static ssize_t mxc_v4l_read(struct file *file, char *buf, size_t count,
+			    loff_t *ppos)
+{
+	int err = 0;
+	u8 *v_address;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	/* Stop the viewfinder */
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+
+	v_address = dma_alloc_coherent(0,
+				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+				       &cam->still_buf, GFP_DMA | GFP_KERNEL);
+
+	if (!v_address) {
+		err = -ENOBUFS;
+		goto exit0;
+	}
+
+	err = prp_still_select(cam);
+	if (err != 0) {
+		err = -EIO;
+		goto exit1;
+	}
+
+	cam->still_counter = 0;
+	err = cam->csi_start(cam);
+	if (err != 0) {
+		err = -EIO;
+		goto exit2;
+	}
+
+	if (!wait_event_interruptible_timeout(cam->still_queue,
+					      cam->still_counter != 0,
+					      10 * HZ)) {
+		pr_err("ERROR: v4l2 capture: mxc_v4l_read timeout counter %x\n",
+		       cam->still_counter);
+		err = -ETIME;
+		goto exit2;
+	}
+	err = copy_to_user(buf, v_address, cam->v2f.fmt.pix.sizeimage);
+
+      exit2:
+	prp_still_deselect(cam);
+
+      exit1:
+	dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address,
+			  cam->still_buf);
+	cam->still_buf = 0;
+
+      exit0:
+	if (cam->overlay_on == true) {
+		start_preview(cam);
+	}
+
+	up(&cam->busy_lock);
+	if (err < 0)
+		return err;
+
+	return (cam->v2f.fmt.pix.sizeimage - err);
+}
+#endif
+
+/*!
+ * V4L interface - ioctl function
+ *
+ * @param inode      struct inode*
+ *
+ * @param file       struct file*
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void*
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int mxc_v4l_do_ioctl(struct inode *inode, struct file *file,
+			    unsigned int ioctlnr, void *arg)
+{
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int retval = 0;
+	unsigned long lock_flags;
+
+	pr_debug("In MVC: mxc_v4l_do_ioctl %x\n", ioctlnr);
+	wait_event_interruptible(cam->power_queue, cam->low_power == false);
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+	/*!
+	 * V4l2 VIDIOC_QUERYCAP ioctl
+	 */
+	case VIDIOC_QUERYCAP: {
+		struct v4l2_capability *cap = arg;
+		pr_debug("   case VIDIOC_QUERYCAP\n");
+		strcpy(cap->driver, "mxc_v4l2");
+		cap->version = KERNEL_VERSION(0, 1, 11);
+		cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+				    V4L2_CAP_VIDEO_OVERLAY |
+				    V4L2_CAP_STREAMING |
+				    V4L2_CAP_READWRITE;
+		cap->card[0] = '\0';
+		cap->bus_info[0] = '\0';
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_G_FMT ioctl
+	 */
+	case VIDIOC_G_FMT: {
+		struct v4l2_format *gf = arg;
+		pr_debug("   case VIDIOC_G_FMT\n");
+		retval = mxc_v4l2_g_fmt(cam, gf);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_S_FMT ioctl
+	 */
+	case VIDIOC_S_FMT: {
+		struct v4l2_format *sf = arg;
+		pr_debug("   case VIDIOC_S_FMT\n");
+		retval = mxc_v4l2_s_fmt(cam, sf);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_REQBUFS ioctl
+	 */
+	case VIDIOC_REQBUFS: {
+		struct v4l2_requestbuffers *req = arg;
+		pr_debug("   case VIDIOC_REQBUFS\n");
+
+		if (req->count > FRAME_NUM) {
+			pr_err("ERROR: v4l2 capture: VIDIOC_REQBUFS: "
+			       "not enough buffers\n");
+			req->count = FRAME_NUM;
+		}
+
+		if ((req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+		    (req->memory != V4L2_MEMORY_MMAP)) {
+			pr_err("ERROR: v4l2 capture: VIDIOC_REQBUFS: "
+			       "wrong buffer type\n");
+			retval = -EINVAL;
+			break;
+		}
+
+		mxc_streamoff(cam);
+		mxc_free_frame_buf(cam);
+		cam->enc_counter = 0;
+		cam->skip_frame = 0;
+		INIT_LIST_HEAD(&cam->ready_q);
+		INIT_LIST_HEAD(&cam->working_q);
+		INIT_LIST_HEAD(&cam->done_q);
+
+		retval = mxc_allocate_frame_buf(cam, req->count);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_QUERYBUF ioctl
+	 */
+	case VIDIOC_QUERYBUF: {
+		struct v4l2_buffer *buf = arg;
+		int index = buf->index;
+		pr_debug("   case VIDIOC_QUERYBUF\n");
+
+		if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+			pr_err("ERROR: v4l2 capture: "
+			       "VIDIOC_QUERYBUFS: "
+			       "wrong buffer type\n");
+			retval = -EINVAL;
+			break;
+		}
+
+		memset(buf, 0, sizeof(buf));
+		buf->index = index;
+
+		down(&cam->param_lock);
+		retval = mxc_v4l2_buffer_status(cam, buf);
+		up(&cam->param_lock);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_QBUF ioctl
+	 */
+	case VIDIOC_QBUF: {
+		struct v4l2_buffer *buf = arg;
+		int index = buf->index;
+		pr_debug("   case VIDIOC_QBUF\n");
+
+		spin_lock_irqsave(&cam->int_lock, lock_flags);
+		cam->frame[index].buffer.m.offset = buf->m.offset;
+		if ((cam->frame[index].buffer.flags & 0x7) ==
+		    V4L2_BUF_FLAG_MAPPED) {
+			cam->frame[index].buffer.flags |=
+			    V4L2_BUF_FLAG_QUEUED;
+			if (cam->skip_frame > 0) {
+				list_add_tail(&cam->frame[index].queue,
+					      &cam->working_q);
+				retval =
+				    cam->enc_update_eba(cam->
+							frame[index].
+							buffer.m.offset,
+							&cam->
+							ping_pong_csi);
+				cam->skip_frame = 0;
+			} else {
+				list_add_tail(&cam->frame[index].queue,
+					      &cam->ready_q);
+			}
+		} else if (cam->frame[index].buffer.
+			   flags & V4L2_BUF_FLAG_QUEUED) {
+			pr_err("ERROR: v4l2 capture: VIDIOC_QBUF: "
+			       "buffer already queued\n");
+			retval = -EINVAL;
+		} else if (cam->frame[index].buffer.
+			   flags & V4L2_BUF_FLAG_DONE) {
+			pr_err("ERROR: v4l2 capture: VIDIOC_QBUF: "
+			       "overwrite done buffer.\n");
+			cam->frame[index].buffer.flags &=
+			    ~V4L2_BUF_FLAG_DONE;
+			cam->frame[index].buffer.flags |=
+			    V4L2_BUF_FLAG_QUEUED;
+			retval = -EINVAL;
+		}
+
+		buf->flags = cam->frame[index].buffer.flags;
+		spin_unlock_irqrestore(&cam->int_lock, lock_flags);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_DQBUF ioctl
+	 */
+	case VIDIOC_DQBUF: {
+		struct v4l2_buffer *buf = arg;
+		pr_debug("   case VIDIOC_DQBUF\n");
+
+		if ((cam->enc_counter == 0) &&
+			(file->f_flags & O_NONBLOCK)) {
+			retval = -EAGAIN;
+			break;
+		}
+
+		retval = mxc_v4l_dqueue(cam, buf);
+
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_STREAMON ioctl
+	 */
+	case VIDIOC_STREAMON: {
+		pr_debug("   case VIDIOC_STREAMON\n");
+		retval = mxc_streamon(cam);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_STREAMOFF ioctl
+	 */
+	case VIDIOC_STREAMOFF: {
+		pr_debug("   case VIDIOC_STREAMOFF\n");
+		retval = mxc_streamoff(cam);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_G_CTRL ioctl
+	 */
+	case VIDIOC_G_CTRL: {
+		pr_debug("   case VIDIOC_G_CTRL\n");
+		retval = mxc_v4l2_g_ctrl(cam, arg);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_S_CTRL ioctl
+	 */
+	case VIDIOC_S_CTRL: {
+		pr_debug("   case VIDIOC_S_CTRL\n");
+		retval = mxc_v4l2_s_ctrl(cam, arg);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_CROPCAP ioctl
+	 */
+	case VIDIOC_CROPCAP: {
+		struct v4l2_cropcap *cap = arg;
+		pr_debug("   case VIDIOC_CROPCAP\n");
+		if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+		    cap->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+			retval = -EINVAL;
+			break;
+		}
+		cap->bounds = cam->crop_bounds;
+		cap->defrect = cam->crop_defrect;
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_G_CROP ioctl
+	 */
+	case VIDIOC_G_CROP: {
+		struct v4l2_crop *crop = arg;
+		pr_debug("   case VIDIOC_G_CROP\n");
+
+		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+		    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+			retval = -EINVAL;
+			break;
+		}
+		crop->c = cam->crop_current;
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_S_CROP ioctl
+	 */
+	case VIDIOC_S_CROP: {
+		struct v4l2_crop *crop = arg;
+		struct v4l2_rect *b = &cam->crop_bounds;
+		pr_debug("   case VIDIOC_S_CROP\n");
+
+		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+		    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+			retval = -EINVAL;
+			break;
+		}
+
+		crop->c.top = (crop->c.top < b->top) ? b->top
+			      : crop->c.top;
+		if (crop->c.top > b->top + b->height)
+			crop->c.top = b->top + b->height - 1;
+		if (crop->c.height > b->top + b->height - crop->c.top)
+			crop->c.height =
+				b->top + b->height - crop->c.top;
+
+		crop->c.left = (crop->c.left < b->left) ? b->left
+		    : crop->c.left;
+		if (crop->c.left > b->left + b->width)
+			crop->c.left = b->left + b->width - 1;
+		if (crop->c.width > b->left - crop->c.left + b->width)
+			crop->c.width =
+				b->left - crop->c.left + b->width;
+
+		crop->c.width -= crop->c.width % 8;
+		crop->c.left -= crop->c.left % 4;
+		cam->crop_current = crop->c;
+
+		pr_debug("   Cropping Input to ipu size %d x %d\n",
+				cam->crop_current.width,
+				cam->crop_current.height);
+		ipu_csi_set_window_size(cam->crop_current.width,
+					cam->crop_current.height,
+					cam->csi);
+		ipu_csi_set_window_pos(cam->crop_current.left,
+				       cam->crop_current.top,
+				       cam->csi);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_OVERLAY ioctl
+	 */
+	case VIDIOC_OVERLAY: {
+		int *on = arg;
+		pr_debug("   VIDIOC_OVERLAY on=%d\n", *on);
+		if (*on) {
+			cam->overlay_on = true;
+			cam->overlay_pid = current->pid;
+			retval = start_preview(cam);
+		}
+		if (!*on) {
+			retval = stop_preview(cam);
+			cam->overlay_on = false;
+		}
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_G_FBUF ioctl
+	 */
+	case VIDIOC_G_FBUF: {
+		struct v4l2_framebuffer *fb = arg;
+		pr_debug("   case VIDIOC_G_FBUF\n");
+		*fb = cam->v4l2_fb;
+		fb->capability = V4L2_FBUF_CAP_EXTERNOVERLAY;
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_S_FBUF ioctl
+	 */
+	case VIDIOC_S_FBUF: {
+		struct v4l2_framebuffer *fb = arg;
+		pr_debug("   case VIDIOC_S_FBUF\n");
+		cam->v4l2_fb = *fb;
+		break;
+	}
+
+	case VIDIOC_G_PARM: {
+		struct v4l2_streamparm *parm = arg;
+		pr_debug("   case VIDIOC_G_PARM\n");
+		vidioc_int_g_parm(cam->sensor, parm);
+		break;
+	}
+
+	case VIDIOC_S_PARM:  {
+		struct v4l2_streamparm *parm = arg;
+		pr_debug("   case VIDIOC_S_PARM\n");
+		retval = mxc_v4l2_s_param(cam, parm);
+		break;
+	}
+
+	/* linux v4l2 bug, kernel c0485619 user c0405619 */
+	case VIDIOC_ENUMSTD: {
+		struct v4l2_standard *e = arg;
+		pr_debug("   case VIDIOC_ENUMSTD\n");
+		*e = cam->standard;
+		break;
+	}
+
+	case VIDIOC_G_STD: {
+		v4l2_std_id *e = arg;
+		pr_debug("   case VIDIOC_G_STD\n");
+		retval = mxc_v4l2_g_std(cam, e);
+		break;
+	}
+
+	case VIDIOC_S_STD: {
+		v4l2_std_id *e = arg;
+		pr_debug("   case VIDIOC_S_STD\n");
+		retval = mxc_v4l2_s_std(cam, *e);
+
+		break;
+	}
+
+	case VIDIOC_ENUMOUTPUT: {
+		struct v4l2_output *output = arg;
+		pr_debug("   case VIDIOC_ENUMOUTPUT\n");
+		if (output->index >= MXC_V4L2_CAPTURE_NUM_OUTPUTS) {
+			retval = -EINVAL;
+			break;
+		}
+		*output = mxc_capture_outputs[output->index];
+
+		break;
+	}
+	case VIDIOC_G_OUTPUT: {
+		int *p_output_num = arg;
+		pr_debug("   case VIDIOC_G_OUTPUT\n");
+		*p_output_num = cam->output;
+		break;
+	}
+
+	case VIDIOC_S_OUTPUT: {
+		int *p_output_num = arg;
+		pr_debug("   case VIDIOC_S_OUTPUT\n");
+		if (*p_output_num >= MXC_V4L2_CAPTURE_NUM_OUTPUTS) {
+			retval = -EINVAL;
+			break;
+		}
+		cam->output = *p_output_num;
+		break;
+	}
+
+	case VIDIOC_ENUMINPUT: {
+		struct v4l2_input *input = arg;
+		pr_debug("   case VIDIOC_ENUMINPUT\n");
+		if (input->index >= MXC_V4L2_CAPTURE_NUM_INPUTS) {
+			retval = -EINVAL;
+			break;
+		}
+		*input = mxc_capture_inputs[input->index];
+		break;
+	}
+
+	case VIDIOC_G_INPUT: {
+		int *index = arg;
+		pr_debug("   case VIDIOC_G_INPUT\n");
+		*index = cam->current_input;
+		break;
+	}
+
+	case VIDIOC_S_INPUT: {
+		int *index = arg;
+		pr_debug("   case VIDIOC_S_INPUT\n");
+		if (*index >= MXC_V4L2_CAPTURE_NUM_INPUTS) {
+			retval = -EINVAL;
+			break;
+		}
+
+		if (*index == cam->current_input)
+			break;
+
+		if ((mxc_capture_inputs[cam->current_input].status &
+		    V4L2_IN_ST_NO_POWER) == 0) {
+			retval = mxc_streamoff(cam);
+			if (retval)
+				break;
+			mxc_capture_inputs[cam->current_input].status |=
+							V4L2_IN_ST_NO_POWER;
+		}
+
+		if (strcmp(mxc_capture_inputs[*index].name, "CSI MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_CSI_ENC) || defined(CONFIG_MXC_IPU_CSI_ENC_MODULE)
+			retval = csi_enc_select(cam);
+			if (retval)
+				break;
+#endif
+		} else if (strcmp(mxc_capture_inputs[*index].name,
+				  "CSI IC MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_PRP_ENC) || defined(CONFIG_MXC_IPU_PRP_ENC_MODULE)
+			retval = prp_enc_select(cam);
+			if (retval)
+				break;
+#endif
+		}
+
+		mxc_capture_inputs[*index].status &= ~V4L2_IN_ST_NO_POWER;
+		cam->current_input = *index;
+		break;
+	}
+
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	default:
+		pr_debug("   case default or not supported\n");
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&cam->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L interface - ioctl function
+ *
+ * @return  None
+ */
+static int mxc_v4l_ioctl(struct inode *inode, struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	pr_debug("In MVC:mxc_v4l_ioctl\n");
+	return video_usercopy(inode, file, cmd, arg, mxc_v4l_do_ioctl);
+}
+
+/*!
+ * V4L interface - mmap function
+ *
+ * @param file        structure file *
+ *
+ * @param vma         structure vm_area_struct *
+ *
+ * @return status     0 Success, EINTR busy lock error, ENOBUFS remap_page error
+ */
+static int mxc_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *dev = video_devdata(file);
+	unsigned long size;
+	int res = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	pr_debug("In MVC:mxc_mmap\n");
+	pr_debug("   pgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		 vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	size = vma->vm_end - vma->vm_start;
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		pr_err("ERROR: v4l2 capture: mxc_mmap: "
+			"remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+      mxc_mmap_exit:
+	up(&cam->busy_lock);
+	return res;
+}
+
+/*!
+ * V4L interface - poll function
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_poll(struct file *file, poll_table *wait)
+{
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	pr_debug("In MVC:mxc_poll\n");
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	queue = &cam->enc_queue;
+	poll_wait(file, queue, wait);
+
+	up(&cam->busy_lock);
+
+	return res;
+}
+
+/*!
+ * This structure defines the functions to be called in this driver.
+ */
+static struct file_operations mxc_v4l_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_v4l_open,
+	.release = mxc_v4l_close,
+	.read = mxc_v4l_read,
+	.ioctl = mxc_v4l_ioctl,
+	.mmap = mxc_mmap,
+	.poll = mxc_poll,
+};
+
+static struct video_device mxc_v4l_template = {
+	.name = "Mxc Camera",
+	.vfl_type = VID_TYPE_CAPTURE,
+	.fops = &mxc_v4l_fops,
+	.release = video_device_release,
+};
+
+/*!
+ * This function can be used to release any platform data on closing.
+ */
+static void camera_platform_release(struct device *device)
+{
+}
+
+/*! Device Definition for Mt9v111 devices */
+static struct platform_device mxc_v4l2_devices = {
+	.name = "mxc_v4l2",
+	.dev = {
+		.release = camera_platform_release,
+		},
+	.id = 0,
+};
+
+/*!
+ * Camera V4l2 callback function.
+ *
+ * @param mask      u32
+ *
+ * @param dev       void device structure
+ *
+ * @return status
+ */
+static void camera_callback(u32 mask, void *dev)
+{
+	struct mxc_v4l_frame *done_frame;
+	struct mxc_v4l_frame *ready_frame;
+
+	cam_data *cam = (cam_data *) dev;
+	if (cam == NULL)
+		return;
+
+	pr_debug("In MVC:camera_callback\n");
+
+	if (list_empty(&cam->working_q)) {
+		pr_err("ERROR: v4l2 capture: camera_callback: "
+			"working queue empty\n");
+		return;
+	}
+
+	done_frame =
+		list_entry(cam->working_q.next, struct mxc_v4l_frame, queue);
+	if (done_frame->buffer.flags & V4L2_BUF_FLAG_QUEUED) {
+		done_frame->buffer.flags |= V4L2_BUF_FLAG_DONE;
+		done_frame->buffer.flags &= ~V4L2_BUF_FLAG_QUEUED;
+
+		if (list_empty(&cam->ready_q)) {
+			cam->skip_frame++;
+		} else {
+			ready_frame = list_entry(cam->ready_q.next,
+						 struct mxc_v4l_frame,
+						 queue);
+			list_del(cam->ready_q.next);
+			list_add_tail(&ready_frame->queue, &cam->working_q);
+			cam->enc_update_eba(ready_frame->buffer.m.offset,
+					    &cam->ping_pong_csi);
+		}
+
+		/* Added to the done queue */
+		list_del(cam->working_q.next);
+		list_add_tail(&done_frame->queue, &cam->done_q);
+
+		/* Wake up the queue */
+		cam->enc_counter++;
+		wake_up_interruptible(&cam->enc_queue);
+	} else {
+		pr_err("ERROR: v4l2 capture: camera_callback: "
+			"buffer not queued\n");
+	}
+}
+
+/*!
+ * initialize cam_data structure
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static void init_camera_struct(cam_data *cam)
+{
+	pr_debug("In MVC: init_camera_struct\n");
+
+	/* Default everything to 0 */
+	memset(cam, 0, sizeof(cam_data));
+
+	init_MUTEX(&cam->param_lock);
+	init_MUTEX(&cam->busy_lock);
+
+	cam->video_dev = video_device_alloc();
+	if (cam->video_dev == NULL)
+		return;
+
+	*(cam->video_dev) = mxc_v4l_template;
+
+	video_set_drvdata(cam->video_dev, cam);
+	dev_set_drvdata(&mxc_v4l2_devices.dev, (void *)cam);
+	cam->video_dev->minor = -1;
+
+	init_waitqueue_head(&cam->enc_queue);
+	init_waitqueue_head(&cam->still_queue);
+
+	/* setup cropping */
+	cam->crop_bounds.left = 0;
+	cam->crop_bounds.width = 640;
+	cam->crop_bounds.top = 0;
+	cam->crop_bounds.height = 480;
+	cam->crop_current = cam->crop_defrect = cam->crop_bounds;
+	ipu_csi_set_window_size(cam->crop_current.width,
+				cam->crop_current.height, cam->csi);
+	ipu_csi_set_window_pos(cam->crop_current.left,
+				cam->crop_current.top, cam->csi);
+	cam->streamparm.parm.capture.capturemode = 0;
+
+	cam->standard.index = 0;
+	cam->standard.id = V4L2_STD_UNKNOWN;
+	cam->standard.frameperiod.denominator = 30;
+	cam->standard.frameperiod.numerator = 1;
+	cam->standard.framelines = 480;
+	cam->standard_autodetect = true;
+	cam->streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	cam->streamparm.parm.capture.timeperframe = cam->standard.frameperiod;
+	cam->streamparm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	cam->overlay_on = false;
+	cam->capture_on = false;
+	cam->skip_frame = 0;
+	cam->v4l2_fb.flags = V4L2_FBUF_FLAG_OVERLAY;
+
+	cam->v2f.fmt.pix.sizeimage = 352 * 288 * 3 / 2;
+	cam->v2f.fmt.pix.bytesperline = 288 * 3 / 2;
+	cam->v2f.fmt.pix.width = 288;
+	cam->v2f.fmt.pix.height = 352;
+	cam->v2f.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
+	cam->win.w.width = 160;
+	cam->win.w.height = 160;
+	cam->win.w.left = 0;
+	cam->win.w.top = 0;
+
+	cam->csi = 0;  /* Need to determine how to set this correctly with
+			* multiple video input devices. */
+
+	cam->enc_callback = camera_callback;
+	init_waitqueue_head(&cam->power_queue);
+	spin_lock_init(&cam->int_lock);
+}
+
+/*!
+ * camera_power function
+ *    Turns Sensor power On/Off
+ *
+ * @param       cam           cam data struct
+ * @param       cameraOn      true to turn camera on, false to turn off power.
+ *
+ * @return status
+ */
+static u8 camera_power(cam_data *cam, bool cameraOn)
+{
+	pr_debug("In MVC:camera_power on=%d\n", cameraOn);
+
+	if (cameraOn == true) {
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		vidioc_int_s_power(cam->sensor, 1);
+	} else {
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		vidioc_int_s_power(cam->sensor, 0);
+	}
+	return 0;
+}
+
+/*!
+ * This function is called to put the sensor in a low power state.
+ * Refer to the document driver-model/driver.txt in the kernel source tree
+ * for more information.
+ *
+ * @param   pdev  the device structure used to give information on which I2C
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function returns 0 on success and -1 on failure.
+ */
+static int mxc_v4l2_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	pr_debug("In MVC:mxc_v4l2_suspend\n");
+
+	if (cam == NULL) {
+		return -1;
+	}
+
+	cam->low_power = true;
+
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+	if ((cam->capture_on == true) && cam->enc_disable) {
+		cam->enc_disable(cam);
+	}
+	camera_power(cam, false);
+
+	return 0;
+}
+
+/*!
+ * This function is called to bring the sensor back from a low power state.
+ * Refer to the document driver-model/driver.txt in the kernel source tree
+ * for more information.
+ *
+ * @param   pdev   the device structure
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int mxc_v4l2_resume(struct platform_device *pdev)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	pr_debug("In MVC:mxc_v4l2_resume\n");
+
+	if (cam == NULL) {
+		return -1;
+	}
+
+	cam->low_power = false;
+	wake_up_interruptible(&cam->power_queue);
+	camera_power(cam, true);
+
+	if (cam->overlay_on == true)
+		start_preview(cam);
+	if (cam->capture_on == true)
+		mxc_streamon(cam);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_v4l2_driver = {
+	.driver = {
+		   .name = "mxc_v4l2",
+		   },
+	.probe = NULL,
+	.remove = NULL,
+	.suspend = mxc_v4l2_suspend,
+	.resume = mxc_v4l2_resume,
+	.shutdown = NULL,
+};
+
+/*!
+ * Initializes the camera driver.
+ */
+static int mxc_v4l2_master_attach(struct v4l2_int_device *slave)
+{
+	cam_data *cam = slave->u.slave->master->priv;
+	struct v4l2_format cam_fmt;
+
+	pr_debug("In MVC: mxc_v4l2_master_attach\n");
+	pr_debug("   slave.name = %s\n", slave->name);
+	pr_debug("   master.name = %s\n", slave->u.slave->master->name);
+
+	cam->sensor = slave;
+	if (slave == NULL) {
+		pr_err("ERROR: v4l2 capture: slave parameter not valid.\n");
+		return -1;
+	}
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+	vidioc_int_dev_init(slave);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+	cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vidioc_int_g_fmt_cap(cam->sensor, &cam_fmt);
+
+	/* Used to detect TV in (type 1) vs. camera (type 0)*/
+	cam->device_type = cam_fmt.fmt.pix.priv;
+
+	/* Set the input size to the ipu for this device */
+	cam->crop_bounds.top = cam->crop_bounds.left = 0;
+	cam->crop_bounds.width = cam_fmt.fmt.pix.width;
+	cam->crop_bounds.height = cam_fmt.fmt.pix.height;
+
+	/* This also is the max crop size for this device. */
+	cam->crop_defrect.top = cam->crop_defrect.left = 0;
+	cam->crop_defrect.width = cam_fmt.fmt.pix.width;
+	cam->crop_defrect.height = cam_fmt.fmt.pix.height;
+
+	/* At this point, this is also the current image size. */
+	cam->crop_current.top = cam->crop_current.left = 0;
+	cam->crop_current.width = cam_fmt.fmt.pix.width;
+	cam->crop_current.height = cam_fmt.fmt.pix.height;
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__,
+		 cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+	pr_debug("End of %s: crop_bounds widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_bounds.width, cam->crop_bounds.height);
+	pr_debug("End of %s: crop_defrect widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_defrect.width, cam->crop_defrect.height);
+	pr_debug("End of %s: crop_current widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_current.width, cam->crop_current.height);
+
+	return 0;
+}
+
+/*!
+ * Disconnects the camera driver.
+ */
+static void mxc_v4l2_master_detach(struct v4l2_int_device *slave)
+{
+	pr_debug("In MVC:mxc_v4l2_master_detach\n");
+	vidioc_int_dev_exit(slave);
+}
+
+/*!
+ * Entry point for the V4L2
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int camera_init(void)
+{
+	u8 err = 0;
+
+	pr_debug("In MVC:camera_init\n");
+
+	/* Register the device driver structure. */
+	err = platform_driver_register(&mxc_v4l2_driver);
+	if (err != 0) {
+		pr_err("ERROR: v4l2 capture:camera_init: "
+			"platform_driver_register failed.\n");
+		return err;
+	}
+
+	/* Create g_cam and initialize it. */
+	if ((g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL)) == NULL) {
+		pr_err("ERROR: v4l2 capture: failed to register camera\n");
+		platform_driver_unregister(&mxc_v4l2_driver);
+		return -1;
+	}
+	init_camera_struct(g_cam);
+
+	/* Set up the v4l2 device and register it*/
+	mxc_v4l2_int_device.priv = g_cam;
+	/* This function contains a bug that won't let this be rmmod'd. */
+	v4l2_int_device_register(&mxc_v4l2_int_device);
+
+	/* Register the I2C device */
+	err = platform_device_register(&mxc_v4l2_devices);
+	if (err != 0) {
+		pr_err("ERROR: v4l2 capture: camera_init: "
+		       "platform_device_register failed.\n");
+		platform_driver_unregister(&mxc_v4l2_driver);
+		kfree(g_cam);
+		g_cam = NULL;
+		return err;
+	}
+
+	/* register v4l video device */
+	if (video_register_device(g_cam->video_dev, VFL_TYPE_GRABBER, video_nr)
+	    == -1) {
+		platform_device_unregister(&mxc_v4l2_devices);
+		platform_driver_unregister(&mxc_v4l2_driver);
+		kfree(g_cam);
+		g_cam = NULL;
+		pr_err("ERROR: v4l2 capture: video_register_device failed\n");
+		return -1;
+	}
+	pr_debug("   Video device registered: %s #%d\n",
+		 g_cam->video_dev->name, g_cam->video_dev->minor);
+
+	return err;
+}
+
+/*!
+ * Exit and cleanup for the V4L2
+ */
+static void __exit camera_exit(void)
+{
+	pr_debug("In MVC: camera_exit\n");
+
+	pr_info("V4L2 unregistering video\n");
+
+	if (g_cam->open_count) {
+		pr_err("ERROR: v4l2 capture:camera open "
+			"-- setting ops to NULL\n");
+	} else {
+		pr_info("V4L2 freeing image input device\n");
+		v4l2_int_device_unregister(&mxc_v4l2_int_device);
+		video_unregister_device(g_cam->video_dev);
+		platform_driver_unregister(&mxc_v4l2_driver);
+		platform_device_unregister(&mxc_v4l2_devices);
+
+		mxc_free_frame_buf(g_cam);
+		kfree(g_cam);
+		g_cam = NULL;
+	}
+}
+
+module_init(camera_init);
+module_exit(camera_exit);
+
+module_param(video_nr, int, 0444);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2 capture driver for Mxc based cameras");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
new file mode 100644
index 000000000000..a9c0c4d159da
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
@@ -0,0 +1,199 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup MXC_V4L2_CAPTURE MXC V4L2 Video Capture Driver
+ */
+/*!
+ * @file mxc_v4l2_capture.h
+ *
+ * @brief mxc V4L2 capture device API  Header file
+ *
+ * It include all the defines for frame operations, also three structure defines
+ * use case ops structure, common v4l2 driver structure and frame structure.
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#ifndef __MXC_V4L2_CAPTURE_H__
+#define __MXC_V4L2_CAPTURE_H__
+
+#include <asm/uaccess.h>
+#include <linux/list.h>
+#include <linux/smp_lock.h>
+#include <linux/ipu.h>
+#include <linux/mxc_v4l2.h>
+
+#include <media/v4l2-dev.h>
+
+#define FRAME_NUM 3
+
+/*!
+ * v4l2 frame structure.
+ */
+struct mxc_v4l_frame {
+	u32 paddress;
+	void *vaddress;
+	int count;
+	int width;
+	int height;
+
+	struct v4l2_buffer buffer;
+	struct list_head queue;
+	int index;
+};
+
+/* Only for old version.  Will go away soon. */
+typedef struct {
+	u8 clk_mode;
+	u8 ext_vsync;
+	u8 Vsync_pol;
+	u8 Hsync_pol;
+	u8 pixclk_pol;
+	u8 data_pol;
+	u8 data_width;
+	u8 pack_tight;
+	u8 force_eof;
+	u8 data_en_pol;
+	u16 width;
+	u16 height;
+	u32 pixel_fmt;
+	u32 mclk;
+	u16 active_width;
+	u16 active_height;
+} sensor_interface;
+
+/* Sensor control function */
+/* Only for old version.  Will go away soon. */
+struct camera_sensor {
+	void (*set_color) (int bright, int saturation, int red, int green,
+			   int blue);
+	void (*get_color) (int *bright, int *saturation, int *red, int *green,
+			   int *blue);
+	void (*set_ae_mode) (int ae_mode);
+	void (*get_ae_mode) (int *ae_mode);
+	sensor_interface *(*config) (int *frame_rate, int high_quality);
+	sensor_interface *(*reset) (void);
+	void (*get_std) (v4l2_std_id *std);
+	void (*set_std) (v4l2_std_id std);
+	unsigned int csi;
+};
+
+/*!
+ * common v4l2 driver structure.
+ */
+typedef struct _cam_data {
+	struct video_device *video_dev;
+	int device_type;
+
+	/* semaphore guard against SMP multithreading */
+	struct semaphore busy_lock;
+
+	int open_count;
+
+	/* params lock for this camera */
+	struct semaphore param_lock;
+
+	/* Encoder */
+	struct list_head ready_q;
+	struct list_head done_q;
+	struct list_head working_q;
+	int ping_pong_csi;
+	spinlock_t int_lock;
+	struct mxc_v4l_frame frame[FRAME_NUM];
+	int skip_frame;
+	wait_queue_head_t enc_queue;
+	int enc_counter;
+	dma_addr_t rot_enc_bufs[2];
+	void *rot_enc_bufs_vaddr[2];
+	int rot_enc_buf_size[2];
+	enum v4l2_buf_type type;
+
+	/* still image capture */
+	wait_queue_head_t still_queue;
+	int still_counter;
+	dma_addr_t still_buf;
+	void *still_buf_vaddr;
+
+	/* overlay */
+	struct v4l2_window win;
+	struct v4l2_framebuffer v4l2_fb;
+	dma_addr_t vf_bufs[2];
+	void *vf_bufs_vaddr[2];
+	int vf_bufs_size[2];
+	dma_addr_t rot_vf_bufs[2];
+	void *rot_vf_bufs_vaddr[2];
+	int rot_vf_buf_size[2];
+	bool overlay_active;
+	int output;
+	struct fb_info *overlay_fb;
+
+	/* v4l2 format */
+	struct v4l2_format v2f;
+	int rotation;	/* for IPUv1 and IPUv3, this means encoder rotation */
+	int vf_rotation; /* viewfinder rotation only for IPUv1 and IPUv3 */
+	struct v4l2_mxc_offset offset;
+
+	/* V4l2 control bit */
+	int bright;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+	/* standard */
+	struct v4l2_streamparm streamparm;
+	struct v4l2_standard standard;
+	bool standard_autodetect;
+
+	/* crop */
+	struct v4l2_rect crop_bounds;
+	struct v4l2_rect crop_defrect;
+	struct v4l2_rect crop_current;
+
+	int (*enc_update_eba) (dma_addr_t eba, int *bufferNum);
+	int (*enc_enable) (void *private);
+	int (*enc_disable) (void *private);
+	void (*enc_callback) (u32 mask, void *dev);
+	int (*vf_start_adc) (void *private);
+	int (*vf_stop_adc) (void *private);
+	int (*vf_start_sdc) (void *private);
+	int (*vf_stop_sdc) (void *private);
+	int (*csi_start) (void *private);
+	int (*csi_stop) (void *private);
+
+	/* misc status flag */
+	bool overlay_on;
+	bool capture_on;
+	int overlay_pid;
+	int capture_pid;
+	bool low_power;
+	wait_queue_head_t power_queue;
+	unsigned int csi;
+	int current_input;
+
+	/* camera sensor interface */
+	struct camera_sensor *cam_sensor; 	/* old version */
+	struct v4l2_int_device *sensor;
+} cam_data;
+
+#if defined(CONFIG_MXC_IPU_V1) || defined(CONFIG_VIDEO_MXC_EMMA_CAMERA) \
+			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA_MODULE) \
+			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA)
+void set_mclk_rate(uint32_t *p_mclk_freq);
+#else
+void set_mclk_rate(uint32_t *p_mclk_freq, uint32_t csi);
+#endif
+#endif				/* __MXC_V4L2_CAPTURE_H__ */
diff --git a/drivers/media/video/mxc/capture/ov2640.c b/drivers/media/video/mxc/capture/ov2640.c
new file mode 100644
index 000000000000..a1329b0b431d
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ov2640.c
@@ -0,0 +1,1080 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ov2640.c
+ *
+ * @brief ov2640 camera driver functions
+ *
+ * @ingroup Camera
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/regulator/consumer.h>
+
+#include <media/v4l2-int-device.h>
+#include "mxc_v4l2_capture.h"
+
+#define MIN_FPS 5
+#define MAX_FPS 30
+#define DEFAULT_FPS 30
+
+#define OV2640_XCLK_MIN 6000000
+#define OV2640_XCLK_MAX 27000000
+
+/*
+enum ov2640_mode {
+	ov2640_mode_1600_1120,
+	ov2640_mode_800_600
+};
+*/
+
+struct reg_value {
+	u8 reg;
+	u8 value;
+	int delay_ms;
+};
+
+static struct reg_value ov2640_setting_1600_1120[] = {
+#ifdef CONFIG_MACH_MX25_3DS
+	{0xff, 0x01, 0}, {0x12, 0x80, 5}, {0xff, 0x00, 0}, {0x2c, 0xff, 0},
+	{0x2e, 0xdf, 0}, {0xff, 0x01, 0}, {0x3c, 0x32, 0}, {0x11, 0x00, 0},
+	{0x09, 0x02, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0}, {0x14, 0x48, 0},
+	{0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0}, {0x3b, 0xfb, 0},
+	{0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0}, {0x39, 0x02, 0},
+	{0x35, 0x58, 0}, {0x22, 0x0a, 0}, {0x37, 0x40, 0}, {0x23, 0x00, 0},
+	{0x34, 0xa0, 0}, {0x36, 0x1a, 0}, {0x06, 0x02, 0}, {0x07, 0xc0, 0},
+	{0x0d, 0xb7, 0}, {0x0e, 0x01, 0}, {0x4c, 0x00, 0}, {0x4a, 0x81, 0},
+	{0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0}, {0x26, 0x82, 0},
+	{0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x3f, 0}, {0x61, 0x70, 0},
+	{0x62, 0x80, 0}, {0x7c, 0x05, 0}, {0x20, 0x80, 0}, {0x28, 0x30, 0},
+	{0x6c, 0x00, 0}, {0x6d, 0x80, 0}, {0x6e, 0x00, 0}, {0x70, 0x02, 0},
+	{0x71, 0x94, 0}, {0x73, 0xc1, 0}, {0x3d, 0x34, 0}, {0x5a, 0x57, 0},
+	{0x4f, 0xbb, 0}, {0x50, 0x9c, 0}, {0xff, 0x00, 0}, {0xe5, 0x7f, 0},
+	{0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0xe0, 0x14, 0}, {0x76, 0xff, 0},
+	{0x33, 0xa0, 0}, {0x42, 0x20, 0}, {0x43, 0x18, 0}, {0x4c, 0x00, 0},
+	{0x87, 0xd0, 0}, {0x88, 0x3f, 0}, {0xd7, 0x01, 0}, {0xd9, 0x10, 0},
+	{0xd3, 0x82, 0}, {0xc8, 0x08, 0}, {0xc9, 0x80, 0}, {0x7c, 0x00, 0},
+	{0x7d, 0x00, 0}, {0x7c, 0x03, 0}, {0x7d, 0x48, 0}, {0x7d, 0x48, 0},
+	{0x7c, 0x08, 0}, {0x7d, 0x20, 0}, {0x7d, 0x10, 0}, {0x7d, 0x0e, 0},
+	{0x90, 0x00, 0}, {0x91, 0x0e, 0}, {0x91, 0x1a, 0}, {0x91, 0x31, 0},
+	{0x91, 0x5a, 0}, {0x91, 0x69, 0}, {0x91, 0x75, 0}, {0x91, 0x7e, 0},
+	{0x91, 0x88, 0}, {0x91, 0x8f, 0}, {0x91, 0x96, 0}, {0x91, 0xa3, 0},
+	{0x91, 0xaf, 0}, {0x91, 0xc4, 0}, {0x91, 0xd7, 0}, {0x91, 0xe8, 0},
+	{0x91, 0x20, 0}, {0x92, 0x00, 0}, {0x93, 0x06, 0}, {0x93, 0xe3, 0},
+	{0x93, 0x05, 0}, {0x93, 0x05, 0}, {0x93, 0x00, 0}, {0x93, 0x04, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x96, 0x00, 0},
+	{0x97, 0x08, 0}, {0x97, 0x19, 0}, {0x97, 0x02, 0}, {0x97, 0x0c, 0},
+	{0x97, 0x24, 0}, {0x97, 0x30, 0}, {0x97, 0x28, 0}, {0x97, 0x26, 0},
+	{0x97, 0x02, 0}, {0x97, 0x98, 0}, {0x97, 0x80, 0}, {0x97, 0x00, 0},
+	{0x97, 0x00, 0}, {0xc3, 0xed, 0}, {0xa4, 0x00, 0}, {0xa8, 0x00, 0},
+	{0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0}, {0xc7, 0x10, 0},
+	{0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0}, {0xb9, 0x7c, 0},
+	{0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0}, {0xb0, 0xc5, 0},
+	{0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0}, {0xc0, 0xc8, 0},
+	{0xc1, 0x96, 0}, {0x86, 0x1d, 0}, {0x50, 0x00, 0}, {0x51, 0x90, 0},
+	{0x52, 0x2c, 0}, {0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x88, 0},
+	{0x57, 0x00, 0}, {0x5a, 0x90, 0}, {0x5b, 0x2c, 0}, {0x5c, 0x05, 0},
+	{0xc3, 0xed, 0}, {0x7f, 0x00, 0}, {0xda, 0x00, 0}, {0xe5, 0x1f, 0},
+	{0xe1, 0x77, 0}, {0xe0, 0x00, 0}, {0xdd, 0x7f, 0}, {0x05, 0x00, 0},
+	{0xff, 0x00, 0}, {0xe0, 0x04, 0}, {0xc0, 0xc8, 0}, {0xc1, 0x96, 0},
+	{0x86, 0x3d, 0}, {0x50, 0x00, 0}, {0x51, 0x90, 0}, {0x52, 0x2c, 0},
+	{0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x88, 0}, {0x57, 0x00, 0},
+	{0x5a, 0x40, 0}, {0x5b, 0xf0, 0}, {0x5c, 0x01, 0}, {0xd3, 0x82, 0},
+	{0xe0, 0x00, 1000}
+#else
+	{0xff, 0x1, 0}, {0x12, 0x80, 1}, {0xff, 0, 0}, {0x2c, 0xff, 0},
+	{0x2e, 0xdf, 0}, {0xff, 0x1, 0}, {0x3c, 0x32, 0}, {0x11, 0x01, 0},
+	{0x09, 0x00, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0}, {0x14, 0x48, 0},
+	{0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0}, {0x3b, 0xfb, 0},
+	{0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0}, {0x39, 0x82, 0},
+	{0x35, 0x88, 0}, {0x22, 0x0a, 0}, {0x37, 0x40, 0}, {0x23, 0x00, 0},
+	{0x34, 0xa0, 0}, {0x36, 0x1a, 0}, {0x06, 0x02, 0}, {0x07, 0xc0, 0},
+	{0x0d, 0xb7, 0}, {0x0e, 0x01, 0}, {0x4c, 0x00, 0}, {0x4a, 0x81, 0},
+	{0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0}, {0x26, 0x82, 0},
+	{0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x3f, 0}, {0x0c, 0x3c, 0},
+	{0x5d, 0x55, 0}, {0x5e, 0x7d, 0}, {0x5f, 0x7d, 0}, {0x60, 0x55, 0},
+	{0x61, 0x70, 0}, {0x62, 0x80, 0}, {0x7c, 0x05, 0}, {0x20, 0x80, 0},
+	{0x28, 0x30, 0}, {0x6c, 0x00, 0}, {0x6d, 0x80, 0}, {0x6e, 00, 0},
+	{0x70, 0x02, 0}, {0x71, 0x94, 0}, {0x73, 0xc1, 0}, {0x3d, 0x34, 0},
+	{0x5a, 0x57, 0}, {0x4f, 0xbb, 0}, {0x50, 0x9c, 0}, {0xff, 0x00, 0},
+	{0xe5, 0x7f, 0}, {0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0x44, 0x06, 0},
+	{0xe0, 0x14, 0}, {0x76, 0xff, 0}, {0x33, 0xa0, 0}, {0x42, 0x20, 0},
+	{0x43, 0x18, 0}, {0x4c, 0x00, 0}, {0x87, 0xd0, 0}, {0xd7, 0x03, 0},
+	{0xd9, 0x10, 0}, {0xd3, 0x82, 0}, {0xc8, 0x08, 0}, {0xc9, 0x80, 0},
+	{0x7c, 0x00, 0}, {0x7d, 0x00, 0}, {0x7c, 0x03, 0}, {0x7d, 0x48, 0},
+	{0x7d, 0x48, 0}, {0x7c, 0x08, 0}, {0x7d, 0x20, 0}, {0x7d, 0x10, 0},
+	{0x7d, 0x0e, 0}, {0x90, 0x00, 0}, {0x91, 0x0e, 0}, {0x91, 0x1a, 0},
+	{0x91, 0x31, 0}, {0x91, 0x5a, 0}, {0x91, 0x69, 0}, {0x91, 0x75, 0},
+	{0x91, 0x7e, 0}, {0x91, 0x88, 0}, {0x91, 0x8f, 0}, {0x91, 0x96, 0},
+	{0x91, 0xa3, 0}, {0x91, 0xaf, 0}, {0x91, 0xc4, 0}, {0x91, 0xd7, 0},
+	{0x91, 0xe8, 0}, {0x91, 0x20, 0}, {0x92, 0x00, 0}, {0x93, 0x06, 0},
+	{0x93, 0xe3, 0}, {0x93, 0x03, 0}, {0x93, 0x03, 0}, {0x93, 0x00, 0},
+	{0x93, 0x02, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x96, 0x00, 0}, {0x97, 0x08, 0}, {0x97, 0x19, 0}, {0x97, 0x02, 0},
+	{0x97, 0x0c, 0}, {0x97, 0x24, 0}, {0x97, 0x30, 0}, {0x97, 0x28, 0},
+	{0x97, 0x26, 0}, {0x97, 0x02, 0}, {0x97, 0x98, 0}, {0x97, 0x80, 0},
+	{0x97, 0x00, 0}, {0x97, 0x00, 0}, {0xa4, 0x00, 0}, {0xa8, 0x00, 0},
+	{0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0}, {0xc7, 0x10, 0},
+	{0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0}, {0xb9, 0x7c, 0},
+	{0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0}, {0xb0, 0xc5, 0},
+	{0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0}, {0xa6, 0x00, 0},
+	{0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x1b, 0}, {0xa7, 0x31, 0},
+	{0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0},
+	{0xa7, 0x19, 0}, {0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0},
+	{0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x19, 0}, {0xa7, 0x31, 0},
+	{0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xc0, 0xc8, 0}, {0xc1, 0x96, 0},
+	{0x86, 0x3d, 0}, {0x50, 0x00, 0}, {0x51, 0x90, 0}, {0x52, 0x18, 0},
+	{0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x88, 0}, {0x57, 0x00, 0},
+	{0x5a, 0x90, 0}, {0x5b, 0x18, 0}, {0x5c, 0x05, 0}, {0xc3, 0xef, 0},
+	{0x7f, 0x00, 0}, {0xda, 0x01, 0}, {0xe5, 0x1f, 0}, {0xe1, 0x67, 0},
+	{0xe0, 0x00, 0}, {0xdd, 0x7f, 0}, {0x05, 0x00, 0}
+#endif
+};
+
+static struct reg_value ov2640_setting_800_600[] = {
+#ifdef CONFIG_MACH_MX25_3DS
+	{0xff, 0x01, 0}, {0x12, 0x80, 5}, {0xff, 0x00, 0}, {0x2c, 0xff, 0},
+	{0x2e, 0xdf, 0}, {0xff, 0x01, 0}, {0x3c, 0x32, 0}, {0x11, 0x00, 0},
+	{0x09, 0x02, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0}, {0x14, 0x48, 0},
+	{0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0}, {0x3b, 0xfb, 0},
+	{0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0}, {0x39, 0x92, 0},
+	{0x35, 0xda, 0}, {0x22, 0x1a, 0}, {0x37, 0xc3, 0}, {0x23, 0x00, 0},
+	{0x34, 0xc0, 0}, {0x36, 0x1a, 0}, {0x06, 0x88, 0}, {0x07, 0xc0, 0},
+	{0x0d, 0x87, 0}, {0x0e, 0x41, 0}, {0x4c, 0x00, 0},
+	{0x48, 0x00, 0}, {0x5b, 0x00, 0}, {0x42, 0x03, 0}, {0x4a, 0x81, 0},
+	{0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0}, {0x26, 0x82, 0},
+	{0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x22, 0}, {0x0c, 0x3c, 0},
+	{0x61, 0x70, 0}, {0x62, 0x80, 0}, {0x7c, 0x05, 0}, {0x20, 0x80, 0},
+	{0x28, 0x30, 0}, {0x6c, 0x00, 0}, {0x6d, 0x80, 0}, {0x6e, 0x00, 0},
+	{0x70, 0x02, 0}, {0x71, 0x94, 0}, {0x73, 0xc1, 0}, {0x12, 0x40, 0},
+	{0x17, 0x11, 0}, {0x18, 0x43, 0}, {0x19, 0x00, 0}, {0x1a, 0x4b, 0},
+	{0x32, 0x09, 0}, {0x37, 0xc0, 0}, {0x4f, 0xca, 0}, {0x50, 0xa8, 0},
+	{0x5a, 0x23, 0}, {0x6d, 0x00, 0}, {0x3d, 0x38, 0}, {0xff, 0x00, 0},
+	{0xe5, 0x7f, 0}, {0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0xe0, 0x14, 0},
+	{0x76, 0xff, 0}, {0x33, 0xa0, 0}, {0x42, 0x20, 0}, {0x43, 0x18, 0},
+	{0x4c, 0x00, 0}, {0x87, 0xd5, 0}, {0x88, 0x3f, 0}, {0xd7, 0x01, 0},
+	{0xd9, 0x10, 0}, {0xd3, 0x82, 0}, {0xc8, 0x08, 0}, {0xc9, 0x80, 0},
+	{0x7c, 0x00, 0}, {0x7d, 0x00, 0}, {0x7c, 0x03, 0}, {0x7d, 0x48, 0},
+	{0x7d, 0x48, 0}, {0x7c, 0x08, 0}, {0x7d, 0x20, 0}, {0x7d, 0x10, 0},
+	{0x7d, 0x0e, 0}, {0x90, 0x00, 0}, {0x91, 0x0e, 0}, {0x91, 0x1a, 0},
+	{0x91, 0x31, 0}, {0x91, 0x5a, 0}, {0x91, 0x69, 0}, {0x91, 0x75, 0},
+	{0x91, 0x7e, 0}, {0x91, 0x88, 0}, {0x91, 0x8f, 0}, {0x91, 0x96, 0},
+	{0x91, 0xa3, 0}, {0x91, 0xaf, 0}, {0x91, 0xc4, 0}, {0x91, 0xd7, 0},
+	{0x91, 0xe8, 0}, {0x91, 0x20, 0}, {0x92, 0x00, 0}, {0x93, 0x06, 0},
+	{0x93, 0xe3, 0}, {0x93, 0x05, 0}, {0x93, 0x05, 0}, {0x93, 0x00, 0},
+	{0x93, 0x04, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x96, 0x00, 0}, {0x97, 0x08, 0}, {0x97, 0x19, 0}, {0x97, 0x02, 0},
+	{0x97, 0x0c, 0}, {0x97, 0x24, 0}, {0x97, 0x30, 0}, {0x97, 0x28, 0},
+	{0x97, 0x26, 0}, {0x97, 0x02, 0}, {0x97, 0x98, 0}, {0x97, 0x80, 0},
+	{0x97, 0x00, 0}, {0x97, 0x00, 0}, {0xc3, 0xed, 0}, {0xa4, 0x00, 0},
+	{0xa8, 0x00, 0}, {0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0},
+	{0xc7, 0x10, 0}, {0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0},
+	{0xb9, 0x7c, 0}, {0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0},
+	{0xb0, 0xc5, 0}, {0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0},
+	{0xc0, 0x64, 0}, {0xc1, 0x4b, 0}, {0x8c, 0x00, 0}, {0x86, 0x3d, 0},
+	{0x50, 0x00, 0}, {0x51, 0xc8, 0}, {0x52, 0x96, 0}, {0x53, 0x00, 0},
+	{0x54, 0x00, 0}, {0x55, 0x00, 0}, {0x5a, 0xc8, 0}, {0x5b, 0x96, 0},
+	{0x5c, 0x00, 0}, {0xd3, 0x82, 0}, {0xc3, 0xed, 0}, {0x7f, 0x00, 0},
+	{0xda, 0x00, 0}, {0xe5, 0x1f, 0}, {0xe1, 0x67, 0}, {0xe0, 0x00, 0},
+	{0xdd, 0x7f, 0}, {0x05, 0x00, 0}, {0xff, 0x00, 0}, {0xe0, 0x04, 0},
+	{0xc0, 0x64, 0}, {0xc1, 0x4b, 0}, {0x8c, 0x00, 0}, {0x86, 0x3d, 0},
+	{0x50, 0x00, 0}, {0x51, 0xc8, 0}, {0x52, 0x96, 0}, {0x53, 0x00, 0},
+	{0x54, 0x00, 0}, {0x55, 0x00, 0}, {0x5a, 0xa0, 0}, {0x5b, 0x78, 0},
+	{0x5c, 0x00, 0}, {0xd3, 0x82, 0}, {0xe0, 0x00, 1000}
+#else
+	{0xff, 0, 0}, {0xff, 1, 0}, {0x12, 0x80, 1}, {0xff, 00, 0},
+	{0x2c, 0xff, 0}, {0x2e, 0xdf, 0}, {0xff, 0x1, 0}, {0x3c, 0x32, 0},
+	{0x11, 0x01, 0}, {0x09, 0x00, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0},
+	{0x14, 0x48, 0}, {0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0},
+	{0x3b, 0xfb, 0}, {0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0},
+	{0x39, 0x92, 0}, {0x35, 0xda, 0}, {0x22, 0x1a, 0}, {0x37, 0xc3, 0},
+	{0x23, 0x00, 0}, {0x34, 0xc0, 0}, {0x36, 0x1a, 0}, {0x06, 0x88, 0},
+	{0x07, 0xc0, 0}, {0x0d, 0x87, 0}, {0x0e, 0x41, 0}, {0x4c, 0x00, 0},
+	{0x4a, 0x81, 0}, {0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0},
+	{0x26, 0x82, 0}, {0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x22, 0},
+	{0x0c, 0x3c, 0}, {0x5d, 0x55, 0}, {0x5e, 0x7d, 0}, {0x5f, 0x7d, 0},
+	{0x60, 0x55, 0}, {0x61, 0x70, 0}, {0x62, 0x80, 0}, {0x7c, 0x05, 0},
+	{0x20, 0x80, 0}, {0x28, 0x30, 0}, {0x6c, 0x00, 0}, {0x6d, 0x80, 0},
+	{0x6e, 00, 0}, {0x70, 0x02, 0}, {0x71, 0x94, 0}, {0x73, 0xc1, 0},
+	{0x12, 0x40, 0}, {0x17, 0x11, 0}, {0x18, 0x43, 0}, {0x19, 0x00, 0},
+	{0x1a, 0x4b, 0}, {0x32, 0x09, 0}, {0x37, 0xc0, 0}, {0x4f, 0xca, 0},
+	{0x50, 0xa8, 0}, {0x6d, 0x00, 0}, {0x3d, 0x38, 0}, {0xff, 0x00, 0},
+	{0xe5, 0x7f, 0}, {0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0x44, 0x06, 0},
+	{0xe0, 0x14, 0}, {0x76, 0xff, 0}, {0x33, 0xa0, 0}, {0x42, 0x20, 0},
+	{0x43, 0x18, 0}, {0x4c, 0x00, 0}, {0x87, 0xd0, 0}, {0x88, 0x3f, 0},
+	{0xd7, 0x03, 0}, {0xd9, 0x10, 0}, {0xd3, 0x82, 0}, {0xc8, 0x08, 0},
+	{0xc9, 0x80, 0}, {0x7c, 0x00, 0}, {0x7d, 0x00, 0}, {0x7c, 0x03, 0},
+	{0x7d, 0x48, 0}, {0x7d, 0x48, 0}, {0x7c, 0x08, 0}, {0x7d, 0x20, 0},
+	{0x7d, 0x10, 0}, {0x7d, 0x0e, 0}, {0x90, 0x00, 0}, {0x91, 0x0e, 0},
+	{0x91, 0x1a, 0}, {0x91, 0x31, 0}, {0x91, 0x5a, 0}, {0x91, 0x69, 0},
+	{0x91, 0x75, 0}, {0x91, 0x7e, 0}, {0x91, 0x88, 0}, {0x91, 0x8f, 0},
+	{0x91, 0x96, 0}, {0x91, 0xa3, 0}, {0x91, 0xaf, 0}, {0x91, 0xc4, 0},
+	{0x91, 0xd7, 0}, {0x91, 0xe8, 0}, {0x91, 0x20, 0}, {0x92, 0x00, 0},
+	{0x93, 0x06, 0}, {0x93, 0xe3, 0}, {0x93, 0x03, 0}, {0x93, 0x03, 0},
+	{0x93, 0x00, 0}, {0x93, 0x02, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x96, 0x00, 0}, {0x97, 0x08, 0}, {0x97, 0x19, 0},
+	{0x97, 0x02, 0}, {0x97, 0x0c, 0}, {0x97, 0x24, 0}, {0x97, 0x30, 0},
+	{0x97, 0x28, 0}, {0x97, 0x26, 0}, {0x97, 0x02, 0}, {0x97, 0x98, 0},
+	{0x97, 0x80, 0}, {0x97, 0x00, 0}, {0x97, 0x00, 0}, {0xa4, 0x00, 0},
+	{0xa8, 0x00, 0}, {0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0},
+	{0xc7, 0x10, 0}, {0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0},
+	{0xb9, 0x7c, 0}, {0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0},
+	{0xb0, 0xc5, 0}, {0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0},
+	{0xa6, 0x00, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x1b, 0},
+	{0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xa7, 0x20, 0},
+	{0xa7, 0xd8, 0}, {0xa7, 0x19, 0}, {0xa7, 0x31, 0}, {0xa7, 0x00, 0},
+	{0xa7, 0x18, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x19, 0},
+	{0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xc0, 0x64, 0},
+	{0xc1, 0x4b, 0}, {0x86, 0x1d, 0}, {0x50, 0x00, 0}, {0x51, 0xc8, 0},
+	{0x52, 0x96, 0}, {0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x00, 0},
+	{0x57, 0x00, 0}, {0x5a, 0xc8, 0}, {0x5b, 0x96, 0}, {0x5c, 0x00, 0},
+	{0xc3, 0xef, 0}, {0x7f, 0x00, 0}, {0xda, 0x01, 0}, {0xe5, 0x1f, 0},
+	{0xe1, 0x67, 0}, {0xe0, 0x00, 0}, {0xdd, 0x7f, 0}, {0x05, 0x00, 0}
+#endif
+};
+
+/*!
+ * Maintains the information on the current state of the sesor.
+ */
+struct sensor {
+	const struct ov2640_platform_data *platform_data;
+	struct v4l2_int_device *v4l2_int_device;
+	struct i2c_client *i2c_client;
+	struct v4l2_pix_format pix;
+	struct v4l2_captureparm streamcap;
+	bool on;
+
+	/* control settings */
+	int brightness;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+	u32 csi;
+	u32 mclk;
+
+} ov2640_data;
+
+static struct regulator *io_regulator;
+static struct regulator *core_regulator;
+static struct regulator *analog_regulator;
+static struct regulator *gpo_regulator;
+
+extern void gpio_sensor_active(void);
+extern void gpio_sensor_inactive(void);
+
+/* list of image formats supported by this sensor */
+/*
+const static struct v4l2_fmtdesc ov2640_formats[] = {
+	{
+		.description = "YUYV (YUV 4:2:2), packed",
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+	},
+};
+ */
+
+static int ov2640_init_mode(struct sensor *s)
+{
+	int ret = -1;
+	struct reg_value *setting;
+	int i, num;
+
+	pr_debug("In ov2640:ov2640_init_mode capturemode is %d\n",
+		s->streamcap.capturemode);
+
+	if (s->streamcap.capturemode & V4L2_MODE_HIGHQUALITY) {
+		s->pix.width = 1600;
+		s->pix.height = 1120;
+		setting = ov2640_setting_1600_1120;
+		num = ARRAY_SIZE(ov2640_setting_1600_1120);
+	} else {
+		s->pix.width = 800;
+		s->pix.height = 600;
+		setting = ov2640_setting_800_600;
+		num = ARRAY_SIZE(ov2640_setting_800_600);
+	}
+
+	for (i = 0; i < num; i++) {
+		ret = i2c_smbus_write_byte_data(s->i2c_client,
+						setting[i].reg,
+						setting[i].value);
+		if (ret < 0) {
+			pr_err("write reg error: reg=%x, val=%x\n",
+			       setting[i].reg, setting[i].value);
+			return ret;
+		}
+		if (setting[i].delay_ms > 0)
+			msleep(setting[i].delay_ms);
+	}
+
+	return ret;
+}
+
+/* At present only support change to 15fps(only for SVGA mode) */
+static int ov2640_set_fps(struct sensor *s, int fps)
+{
+	int ret = 0;
+
+	if (i2c_smbus_write_byte_data(s->i2c_client, 0xff, 0x01) < 0) {
+		pr_err("in %s,change to sensor addr failed\n", __func__);
+		ret = -EPERM;
+	}
+
+	/* change the camera framerate to 15fps(only for SVGA mode) */
+	if (i2c_smbus_write_byte_data(s->i2c_client, 0x11, 0x01) < 0) {
+		pr_err("change camera to 15fps failed\n");
+		ret = -EPERM;
+	}
+
+	return ret;
+}
+
+static int ov2640_set_format(struct sensor *s, int format)
+{
+	int ret = 0;
+
+	if (i2c_smbus_write_byte_data(s->i2c_client, 0xff, 0x00) < 0)
+		ret = -EPERM;
+
+	if (format == V4L2_PIX_FMT_RGB565) {
+		/* set RGB565 format */
+		if (i2c_smbus_write_byte_data(s->i2c_client, 0xda, 0x08) < 0)
+			ret = -EPERM;
+
+		if (i2c_smbus_write_byte_data(s->i2c_client, 0xd7, 0x03) < 0)
+			ret = -EPERM;
+	} else if (format == V4L2_PIX_FMT_YUV420) {
+		/* set YUV420 format */
+		if (i2c_smbus_write_byte_data(s->i2c_client, 0xda, 0x00) < 0)
+			ret = -EPERM;
+
+		if (i2c_smbus_write_byte_data(s->i2c_client, 0xd7, 0x1b) < 0)
+			ret = -EPERM;
+	} else {
+		pr_debug("format not supported\n");
+	}
+
+	return ret;
+}
+
+/* --------------- IOCTL functions from v4l2_int_ioctl_desc --------------- */
+
+/*!
+ * ioctl_g_ifparm - V4L2 sensor interface handler for vidioc_int_g_ifparm_num
+ * s: pointer to standard V4L2 device structure
+ * p: pointer to standard V4L2 vidioc_int_g_ifparm_num ioctl structure
+ *
+ * Gets slave interface parameters.
+ * Calculates the required xclk value to support the requested
+ * clock parameters in p.  This value is returned in the p
+ * parameter.
+ *
+ * vidioc_int_g_ifparm returns platform-specific information about the
+ * interface settings used by the sensor.
+ *
+ * Given the image capture format in pix, the nominal frame period in
+ * timeperframe, calculate the required xclk frequency.
+ *
+ * Called on open.
+ */
+static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
+{
+	pr_debug("In ov2640:ioctl_g_ifparm\n");
+
+	if (s == NULL) {
+		pr_err("   ERROR!! no slave device set!\n");
+		return -1;
+	}
+
+	memset(p, 0, sizeof(*p));
+	p->u.bt656.clock_curr = ov2640_data.mclk;
+	p->if_type = V4L2_IF_TYPE_BT656;
+	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.clock_min = OV2640_XCLK_MIN;
+	p->u.bt656.clock_max = OV2640_XCLK_MAX;
+
+	return 0;
+}
+
+/*!
+ * Sets the camera power.
+ *
+ * s  pointer to the camera device
+ * on if 1, power is to be turned on.  0 means power is to be turned off
+ *
+ * ioctl_s_power - V4L2 sensor interface handler for vidioc_int_s_power_num
+ * @s: pointer to standard V4L2 device structure
+ * @on: power state to which device is to be set
+ *
+ * Sets devices power state to requrested state, if possible.
+ * This is called on open, close, suspend and resume.
+ */
+static int ioctl_s_power(struct v4l2_int_device *s, int on)
+{
+	struct sensor *sensor = s->priv;
+
+	pr_debug("In ov2640:ioctl_s_power\n");
+
+	if (on && !sensor->on) {
+		gpio_sensor_active();
+		if (io_regulator)
+			if (regulator_enable(io_regulator) != 0)
+				return -EIO;
+		if (core_regulator)
+			if (regulator_enable(core_regulator) != 0)
+				return -EIO;
+		if (gpo_regulator)
+			if (regulator_enable(gpo_regulator) != 0)
+				return -EIO;
+		if (analog_regulator)
+			if (regulator_enable(analog_regulator) != 0)
+				return -EIO;
+	} else if (!on && sensor->on) {
+		if (analog_regulator)
+			regulator_disable(analog_regulator);
+		if (core_regulator)
+			regulator_disable(core_regulator);
+		if (io_regulator)
+			regulator_disable(io_regulator);
+		if (gpo_regulator)
+			regulator_disable(gpo_regulator);
+		gpio_sensor_inactive();
+	}
+
+	sensor->on = on;
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_parm - V4L2 sensor interface handler for VIDIOC_G_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the sensor's video CAPTURE parameters.
+ */
+static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	struct sensor *sensor = s->priv;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+	int ret = 0;
+
+	pr_debug("In ov2640:ioctl_g_parm\n");
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		memset(a, 0, sizeof(*a));
+		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cparm->capability = sensor->streamcap.capability;
+		cparm->timeperframe = sensor->streamcap.timeperframe;
+		cparm->capturemode = sensor->streamcap.capturemode;
+		ret = 0;
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_err("   type is not V4L2_BUF_TYPE_VIDEO_CAPTURE " \
+			"but %d\n", a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_err("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_parm - V4L2 sensor interface handler for VIDIOC_S_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the sensor to use the input parameters, if possible.  If
+ * not possible, reverts to the old parameters and returns the
+ * appropriate error code.
+ */
+static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	struct sensor *sensor = s->priv;
+	struct v4l2_fract *timeperframe = &a->parm.capture.timeperframe;
+	u32 tgt_fps;	/* target frames per secound */
+	int ret = 0;
+
+	pr_debug("In ov2640:ioctl_s_parm\n");
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+
+		/* Check that the new frame rate is allowed. */
+		if ((timeperframe->numerator == 0)
+		    || (timeperframe->denominator == 0)) {
+			timeperframe->denominator = DEFAULT_FPS;
+			timeperframe->numerator = 1;
+		}
+		tgt_fps = timeperframe->denominator
+			  / timeperframe->numerator;
+
+		if (tgt_fps > MAX_FPS) {
+			timeperframe->denominator = MAX_FPS;
+			timeperframe->numerator = 1;
+		} else if (tgt_fps < MIN_FPS) {
+			timeperframe->denominator = MIN_FPS;
+			timeperframe->numerator = 1;
+		}
+		sensor->streamcap.timeperframe = *timeperframe;
+		sensor->streamcap.capturemode =
+				(u32)a->parm.capture.capturemode;
+
+		ret = ov2640_init_mode(sensor);
+		if (tgt_fps == 15)
+			ov2640_set_fps(sensor, tgt_fps);
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_err("   type is not V4L2_BUF_TYPE_VIDEO_CAPTURE " \
+			"but %d\n", a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_err("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_fmt_cap - V4L2 sensor interface handler for ioctl_s_fmt_cap
+ * 		     set camera output format and resolution format
+ *
+ * @s: pointer to standard V4L2 device structure
+ * @arg: pointer to parameter, according this to set camera
+ *
+ * Returns 0 if set succeed, else return -1
+ */
+static int ioctl_s_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	struct sensor *sensor = s->priv;
+	u32 format = f->fmt.pix.pixelformat;
+	int size = 0, ret = 0;
+
+	size = f->fmt.pix.width * f->fmt.pix.height;
+	switch (format) {
+	case V4L2_PIX_FMT_RGB565:
+		if (size > 640 * 480)
+			sensor->streamcap.capturemode = V4L2_MODE_HIGHQUALITY;
+		else
+			sensor->streamcap.capturemode = 0;
+		ret = ov2640_init_mode(sensor);
+
+		ret = ov2640_set_format(sensor, V4L2_PIX_FMT_RGB565);
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		if (size > 640 * 480)
+			sensor->streamcap.capturemode = V4L2_MODE_HIGHQUALITY;
+		else
+			sensor->streamcap.capturemode = 0;
+		ret = ov2640_init_mode(sensor);
+		break;
+	case V4L2_PIX_FMT_YUV420:
+		if (size > 640 * 480)
+			sensor->streamcap.capturemode = V4L2_MODE_HIGHQUALITY;
+		else
+			sensor->streamcap.capturemode = 0;
+		ret = ov2640_init_mode(sensor);
+
+		/* YUYV: width * 2, YY: width */
+		ret = ov2640_set_format(sensor, V4L2_PIX_FMT_YUV420);
+		break;
+	default:
+		pr_debug("case not supported\n");
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_g_fmt_cap - V4L2 sensor interface handler for ioctl_g_fmt_cap
+ * @s: pointer to standard V4L2 device structure
+ * @f: pointer to standard V4L2 v4l2_format structure
+ *
+ * Returns the sensor's current pixel format in the v4l2_format
+ * parameter.
+ */
+static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	struct sensor *sensor = s->priv;
+
+	pr_debug("In ov2640:ioctl_g_fmt_cap.\n");
+
+	f->fmt.pix = sensor->pix;
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_G_CTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control's current
+ * value from the video_control[] array.  Otherwise, returns -EINVAL
+ * if the control is not supported.
+ */
+static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int ret = 0;
+
+	pr_debug("In ov2640:ioctl_g_ctrl\n");
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		vc->value = ov2640_data.brightness;
+		break;
+	case V4L2_CID_HUE:
+		vc->value = ov2640_data.hue;
+		break;
+	case V4L2_CID_CONTRAST:
+		vc->value = ov2640_data.contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		vc->value = ov2640_data.saturation;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		vc->value = ov2640_data.red;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		vc->value = ov2640_data.blue;
+		break;
+	case V4L2_CID_EXPOSURE:
+		vc->value = ov2640_data.ae_mode;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_ctrl - V4L2 sensor interface handler for VIDIOC_S_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_S_CTRL ioctl structure
+ *
+ * If the requested control is supported, sets the control's current
+ * value in HW (and updates the video_control[] array).  Otherwise,
+ * returns -EINVAL if the control is not supported.
+ */
+static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int retval = 0;
+
+	pr_debug("In ov2640:ioctl_s_ctrl %d\n", vc->id);
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		pr_debug("   V4L2_CID_BRIGHTNESS\n");
+		break;
+	case V4L2_CID_CONTRAST:
+		pr_debug("   V4L2_CID_CONTRAST\n");
+		break;
+	case V4L2_CID_SATURATION:
+		pr_debug("   V4L2_CID_SATURATION\n");
+		break;
+	case V4L2_CID_HUE:
+		pr_debug("   V4L2_CID_HUE\n");
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_DO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_RED_BALANCE:
+		pr_debug("   V4L2_CID_RED_BALANCE\n");
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
+		break;
+	case V4L2_CID_GAMMA:
+		pr_debug("   V4L2_CID_GAMMA\n");
+		break;
+	case V4L2_CID_EXPOSURE:
+		pr_debug("   V4L2_CID_EXPOSURE\n");
+		break;
+	case V4L2_CID_AUTOGAIN:
+		pr_debug("   V4L2_CID_AUTOGAIN\n");
+		break;
+	case V4L2_CID_GAIN:
+		pr_debug("   V4L2_CID_GAIN\n");
+		break;
+	case V4L2_CID_HFLIP:
+		pr_debug("   V4L2_CID_HFLIP\n");
+		break;
+	case V4L2_CID_VFLIP:
+		pr_debug("   V4L2_CID_VFLIP\n");
+		break;
+	default:
+		pr_debug("   Default case\n");
+		retval = -EPERM;
+		break;
+	}
+
+	return retval;
+}
+
+/*!
+ * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
+ * @s: pointer to standard V4L2 device structure
+ */
+static int ioctl_init(struct v4l2_int_device *s)
+{
+	pr_debug("In ov2640:ioctl_init\n");
+
+	return 0;
+}
+
+/*!
+ * ioctl_dev_init - V4L2 sensor interface handler for vidioc_int_dev_init_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Initialise the device when slave attaches to the master.
+ */
+static int ioctl_dev_init(struct v4l2_int_device *s)
+{
+	struct sensor *sensor = s->priv;
+	u32 tgt_xclk;	/* target xclk */
+
+	pr_debug("In ov2640:ioctl_dev_init\n");
+
+	gpio_sensor_active();
+	ov2640_data.on = true;
+
+	tgt_xclk = ov2640_data.mclk;
+	tgt_xclk = min(tgt_xclk, (u32)OV2640_XCLK_MAX);
+	tgt_xclk = max(tgt_xclk, (u32)OV2640_XCLK_MIN);
+	ov2640_data.mclk = tgt_xclk;
+
+	pr_debug("   Setting mclk to %d MHz\n",
+		tgt_xclk / 1000000);
+	set_mclk_rate(&ov2640_data.mclk);
+
+	return ov2640_init_mode(sensor);
+}
+
+/*!
+ * ioctl_dev_exit - V4L2 sensor interface handler for vidioc_int_dev_exit_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Delinitialise the device when slave detaches to the master.
+ */
+static int ioctl_dev_exit(struct v4l2_int_device *s)
+{
+	pr_debug("In ov2640:ioctl_dev_exit\n");
+
+	gpio_sensor_inactive();
+
+	return 0;
+}
+
+/*!
+ * This structure defines all the ioctls for this module and links them to the
+ * enumeration.
+ */
+static struct v4l2_int_ioctl_desc ov2640_ioctl_desc[] = {
+	{vidioc_int_dev_init_num, (v4l2_int_ioctl_func *)ioctl_dev_init},
+	{vidioc_int_dev_exit_num, (v4l2_int_ioctl_func*)ioctl_dev_exit},
+	{vidioc_int_s_power_num, (v4l2_int_ioctl_func *)ioctl_s_power},
+	{vidioc_int_g_ifparm_num, (v4l2_int_ioctl_func *)ioctl_g_ifparm},
+/*	{vidioc_int_g_needs_reset_num,
+				(v4l2_int_ioctl_func *)ioctl_g_needs_reset}, */
+/*	{vidioc_int_reset_num, (v4l2_int_ioctl_func *)ioctl_reset}, */
+	{vidioc_int_init_num, (v4l2_int_ioctl_func *)ioctl_init},
+/*	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_enum_fmt_cap}, */
+/*	{vidioc_int_try_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_try_fmt_cap}, */
+	{vidioc_int_g_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_g_fmt_cap},
+	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func*)ioctl_s_fmt_cap},
+	{vidioc_int_g_parm_num, (v4l2_int_ioctl_func *)ioctl_g_parm},
+	{vidioc_int_s_parm_num, (v4l2_int_ioctl_func *)ioctl_s_parm},
+/*	{vidioc_int_queryctrl_num, (v4l2_int_ioctl_func *)ioctl_queryctrl}, */
+	{vidioc_int_g_ctrl_num, (v4l2_int_ioctl_func *)ioctl_g_ctrl},
+	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *)ioctl_s_ctrl},
+};
+
+static struct v4l2_int_slave ov2640_slave = {
+	.ioctls = ov2640_ioctl_desc,
+	.num_ioctls = ARRAY_SIZE(ov2640_ioctl_desc),
+};
+
+static struct v4l2_int_device ov2640_int_device = {
+	.module = THIS_MODULE,
+	.name = "ov2640",
+	.type = v4l2_int_type_slave,
+	.u = {
+		.slave = &ov2640_slave,
+		},
+};
+
+/*!
+ * ov2640 I2C attach function
+ * Function set in i2c_driver struct.
+ * Called by insmod ov2640_camera.ko.
+ *
+ * @param client            struct i2c_client*
+ * @return  Error code indicating success or failure
+ */
+static int ov2640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int retval;
+	struct mxc_camera_platform_data *plat_data = client->dev.platform_data;
+
+	pr_debug("In ov2640_probe (RH_BT565)\n");
+
+	/* Set initial values for the sensor struct. */
+	memset(&ov2640_data, 0, sizeof(ov2640_data));
+	ov2640_data.i2c_client = client;
+	ov2640_data.mclk = 24000000;
+	ov2640_data.mclk = plat_data->mclk;
+	ov2640_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	ov2640_data.pix.width = 800;
+	ov2640_data.pix.height = 600;
+	ov2640_data.streamcap.capability = V4L2_MODE_HIGHQUALITY
+					   | V4L2_CAP_TIMEPERFRAME;
+	ov2640_data.streamcap.capturemode = 0;
+	ov2640_data.streamcap.timeperframe.denominator = DEFAULT_FPS;
+	ov2640_data.streamcap.timeperframe.numerator = 1;
+
+	if (plat_data->io_regulator) {
+		io_regulator =
+		    regulator_get(&client->dev, plat_data->io_regulator);
+		if (!IS_ERR(io_regulator)) {
+			regulator_set_voltage(io_regulator, 2800000, 2800000);
+			if (regulator_enable(io_regulator) != 0) {
+				pr_err("%s:io set voltage error\n", __func__);
+				goto err1;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:io set voltage ok\n", __func__);
+			}
+		} else
+			io_regulator = NULL;
+	}
+
+	if (plat_data->core_regulator) {
+		core_regulator =
+		    regulator_get(&client->dev, plat_data->core_regulator);
+		if (!IS_ERR(core_regulator)) {
+			regulator_set_voltage(core_regulator,
+					 1300000, 1300000);
+			if (regulator_enable(core_regulator) != 0) {
+				pr_err("%s:core set voltage error\n", __func__);
+				goto err2;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:core set voltage ok\n", __func__);
+			}
+		} else
+			core_regulator = NULL;
+	}
+
+	if (plat_data->analog_regulator) {
+		analog_regulator =
+		    regulator_get(&client->dev, plat_data->analog_regulator);
+		if (!IS_ERR(analog_regulator)) {
+			regulator_set_voltage(analog_regulator, 2000000, 2000000);
+			if (regulator_enable(analog_regulator) != 0) {
+				pr_err("%s:analog set voltage error\n",
+					 __func__);
+				goto err3;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:analog set voltage ok\n", __func__);
+			}
+		} else
+			analog_regulator = NULL;
+	}
+
+	if (plat_data->gpo_regulator) {
+		gpo_regulator =
+		    regulator_get(&client->dev, plat_data->gpo_regulator);
+		if (!IS_ERR(gpo_regulator)) {
+			if (regulator_enable(gpo_regulator) != 0) {
+				pr_err("%s:gpo3 set voltage error\n", __func__);
+				goto err4;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:gpo3 set voltage ok\n", __func__);
+			}
+		} else
+			gpo_regulator = NULL;
+	}
+
+	/* This function attaches this structure to the /dev/video0 device.
+	 * The pointer in priv points to the ov2640_data structure here.*/
+	ov2640_int_device.priv = &ov2640_data;
+	retval = v4l2_int_device_register(&ov2640_int_device);
+
+	return retval;
+
+err4:
+	if (analog_regulator) {
+		regulator_disable(analog_regulator);
+		regulator_put(analog_regulator);
+	}
+err3:
+	if (core_regulator) {
+		regulator_disable(core_regulator);
+		regulator_put(core_regulator);
+	}
+err2:
+	if (io_regulator) {
+		regulator_disable(io_regulator);
+		regulator_put(io_regulator);
+	}
+err1:
+	return -1;
+}
+
+/*!
+ * ov2640 I2C detach function
+ * Called on rmmod ov2640_camera.ko
+ *
+ * @param client            struct i2c_client*
+ * @return  Error code indicating success or failure
+ */
+static int ov2640_remove(struct i2c_client *client)
+{
+	pr_debug("In ov2640_remove\n");
+
+	v4l2_int_device_unregister(&ov2640_int_device);
+
+	if (gpo_regulator) {
+		regulator_disable(gpo_regulator);
+		regulator_put(gpo_regulator);
+	}
+
+	if (analog_regulator) {
+		regulator_disable(analog_regulator);
+		regulator_put(analog_regulator);
+	}
+
+	if (core_regulator) {
+		regulator_disable(core_regulator);
+		regulator_put(core_regulator);
+	}
+
+	if (io_regulator) {
+		regulator_disable(io_regulator);
+		regulator_put(io_regulator);
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id ov2640_id[] = {
+	{"ov2640", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ov2640_id);
+
+static struct i2c_driver ov2640_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "ov2640",
+		  },
+	.probe = ov2640_probe,
+	.remove = ov2640_remove,
+	.id_table = ov2640_id,
+/* To add power management add .suspend and .resume functions */
+};
+
+/*!
+ * ov2640 init function
+ * Called by insmod ov2640_camera.ko.
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int ov2640_init(void)
+{
+	u8 err;
+
+	pr_debug("In ov2640_init\n");
+
+	err = i2c_add_driver(&ov2640_i2c_driver);
+	if (err != 0)
+		pr_err("%s:driver registration failed, error=%d \n",
+			__func__, err);
+
+	return err;
+}
+
+/*!
+ * OV2640 cleanup function
+ * Called on rmmod ov2640_camera.ko
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit ov2640_clean(void)
+{
+	pr_debug("In ov2640_clean\n");
+	i2c_del_driver(&ov2640_i2c_driver);
+}
+
+module_init(ov2640_init);
+module_exit(ov2640_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("OV2640 Camera Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ov3640.c b/drivers/media/video/mxc/capture/ov3640.c
new file mode 100644
index 000000000000..3ff269dda924
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ov3640.c
@@ -0,0 +1,1086 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-int-device.h>
+#include "mxc_v4l2_capture.h"
+
+#define OV3640_VOLTAGE_ANALOG               2800000
+#define OV3640_VOLTAGE_DIGITAL_CORE         1500000
+#define OV3640_VOLTAGE_DIGITAL_IO           1800000
+
+
+/* Check these values! */
+#define MIN_FPS 15
+#define MAX_FPS 30
+#define DEFAULT_FPS 30
+
+#define OV3640_XCLK_MIN 6000000
+#define OV3640_XCLK_MAX 24000000
+
+enum ov3640_mode {
+	ov3640_mode_MIN = 0,
+	ov3640_mode_VGA_640_480 = 0,
+	ov3640_mode_QVGA_320_240 = 1,
+	ov3640_mode_QXGA_2048_1536 = 2,
+	ov3640_mode_XGA_1024_768 = 3,
+	ov3640_mode_MAX = 3
+};
+
+enum ov3640_frame_rate {
+	ov3640_15_fps,
+	ov3640_30_fps
+};
+
+struct reg_value {
+	u16 u16RegAddr;
+	u8 u8Val;
+	u8 u8Mask;
+	u32 u32Delay_ms;
+};
+
+struct ov3640_mode_info {
+	enum ov3640_mode mode;
+	u32 width;
+	u32 height;
+	struct reg_value *init_data_ptr;
+	u32 init_data_size;
+};
+
+/*!
+ * Maintains the information on the current state of the sesor.
+ */
+struct sensor {
+	const struct ov3640_platform_data *platform_data;
+	struct v4l2_int_device *v4l2_int_device;
+	struct i2c_client *i2c_client;
+	struct v4l2_pix_format pix;
+	struct v4l2_captureparm streamcap;
+	bool on;
+
+	/* control settings */
+	int brightness;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+	u32 mclk;
+	int csi;
+} ov3640_data;
+
+static struct reg_value ov3640_setting_15fps_QXGA_2048_1536[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x00, 0, 0}, {0x304c, 0x81, 0, 0},
+	{0x30d7, 0x10, 0, 0}, {0x30d9, 0x0d, 0, 0}, {0x30db, 0x08, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x04, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0}, {0x30ba, 0x04, 0, 0},
+	{0x30bb, 0x08, 0, 0}, {0x3507, 0x06, 0, 0}, {0x350a, 0x4f, 0, 0},
+	{0x3100, 0x02, 0, 0}, {0x3301, 0xde, 0, 0}, {0x3304, 0x00, 0, 0},
+	{0x3400, 0x00, 0, 0}, {0x3404, 0x02, 0, 0}, {0x3600, 0xc4, 0, 0},
+	{0x3302, 0xef, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
+	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
+	{0x3025, 0x00, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x00, 0, 0},
+	{0x335f, 0x68, 0, 0}, {0x3360, 0x00, 0, 0}, {0x3361, 0x00, 0, 0},
+	{0x3362, 0x68, 0, 0}, {0x3363, 0x00, 0, 0}, {0x3364, 0x00, 0, 0},
+	{0x3403, 0x00, 0, 0}, {0x3088, 0x08, 0, 0}, {0x3089, 0x00, 0, 0},
+	{0x308a, 0x06, 0, 0}, {0x308b, 0x00, 0, 0}, {0x307c, 0x10, 0, 0},
+	{0x3090, 0xc0, 0, 0}, {0x304c, 0x84, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3012, 0x00, 0, 0},
+	{0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0}, {0x3022, 0x00, 0, 0},
+	{0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0}, {0x3025, 0x18, 0, 0},
+	{0x3026, 0x06, 0, 0}, {0x3027, 0x0c, 0, 0}, {0x302a, 0x06, 0, 0},
+	{0x302b, 0x20, 0, 0}, {0x3075, 0x44, 0, 0}, {0x300d, 0x00, 0, 0},
+	{0x30d7, 0x00, 0, 0}, {0x3069, 0x40, 0, 0}, {0x303e, 0x01, 0, 0},
+	{0x303f, 0x80, 0, 0}, {0x3302, 0x20, 0, 0}, {0x335f, 0x68, 0, 0},
+	{0x3360, 0x18, 0, 0}, {0x3361, 0x0c, 0, 0}, {0x3362, 0x68, 0, 0},
+	{0x3363, 0x08, 0, 0}, {0x3364, 0x04, 0, 0}, {0x3403, 0x42, 0, 0},
+	{0x3088, 0x08, 0, 0}, {0x3089, 0x00, 0, 0}, {0x308a, 0x06, 0, 0},
+	{0x308b, 0x00, 0, 0},
+};
+
+static struct reg_value ov3640_setting_15fps_XGA_1024_768[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x00, 0, 0}, {0x304c, 0x81, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x04, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0}, {0x30ba, 0x04, 0, 0},
+	{0x30bb, 0x08, 0, 0}, {0x3507, 0x06, 0, 0}, {0x350a, 0x4f, 0, 0},
+	{0x3100, 0x02, 0, 0}, {0x3301, 0xde, 0, 0}, {0x3304, 0x00, 0, 0},
+	{0x3400, 0x01, 0, 0}, {0x3404, 0x1d, 0, 0}, {0x3600, 0xc4, 0, 0},
+	{0x3302, 0xef, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
+	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
+	{0x3025, 0x00, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x00, 0, 0},
+	{0x335f, 0x68, 0, 0}, {0x3360, 0x00, 0, 0}, {0x3361, 0x00, 0, 0},
+	{0x3362, 0x34, 0, 0}, {0x3363, 0x00, 0, 0}, {0x3364, 0x00, 0, 0},
+	{0x3403, 0x00, 0, 0}, {0x3088, 0x04, 0, 0}, {0x3089, 0x00, 0, 0},
+	{0x308a, 0x03, 0, 0}, {0x308b, 0x00, 0, 0}, {0x307c, 0x10, 0, 0},
+	{0x3090, 0xc0, 0, 0}, {0x304c, 0x84, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3011, 0x01, 0, 0},
+};
+
+static struct reg_value ov3640_setting_30fps_XGA_1024_768[] = {
+	{0x0, 0x0, 0}
+};
+
+static struct reg_value ov3640_setting_15fps_VGA_640_480[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x00, 0, 0}, {0x304c, 0x81, 0, 0},
+	{0x30d7, 0x10, 0, 0}, {0x30d9, 0x0d, 0, 0}, {0x30db, 0x08, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x04, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0}, {0x30ba, 0x04, 0, 0},
+	{0x30bb, 0x08, 0, 0}, {0x3507, 0x06, 0, 0}, {0x350a, 0x4f, 0, 0},
+	{0x3100, 0x02, 0, 0}, {0x3301, 0xde, 0, 0}, {0x3304, 0x00, 0, 0},
+	{0x3400, 0x00, 0, 0}, {0x3404, 0x42, 0, 0}, {0x3600, 0xc4, 0, 0},
+	{0x3302, 0xef, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
+	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
+	{0x3025, 0x00, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x00, 0, 0},
+	{0x335f, 0x68, 0, 0}, {0x3360, 0x00, 0, 0}, {0x3361, 0x00, 0, 0},
+	{0x3362, 0x12, 0, 0}, {0x3363, 0x80, 0, 0}, {0x3364, 0xe0, 0, 0},
+	{0x3403, 0x00, 0, 0}, {0x3088, 0x02, 0, 0}, {0x3089, 0x80, 0, 0},
+	{0x308a, 0x01, 0, 0}, {0x308b, 0xe0, 0, 0}, {0x307c, 0x10, 0, 0},
+	{0x3090, 0xc0, 0, 0}, {0x304c, 0x84, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3011, 0x00, 0, 0},
+};
+
+static struct reg_value ov3640_setting_30fps_VGA_640_480[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x01, 0, 0}, {0x304c, 0x82, 0, 0},
+	{0x30d7, 0x10, 0, 0}, {0x30d9, 0x0d, 0, 0}, {0x30db, 0x08, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x0c, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x3300, 0x13, 0, 0}, {0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0},
+	{0x30ba, 0x04, 0, 0}, {0x30bb, 0x08, 0, 0}, {0x3100, 0x02, 0, 0},
+	{0x3301, 0x10, 0x30, 0}, {0x3304, 0x00, 0x03, 0}, {0x3400, 0x00, 0, 0},
+	{0x3404, 0x02, 0, 0}, {0x3600, 0xc0, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3012, 0x10, 0, 0},
+	{0x3023, 0x06, 0, 0}, {0x3026, 0x03, 0, 0}, {0x3027, 0x04, 0, 0},
+	{0x302a, 0x03, 0, 0}, {0x302b, 0x10, 0, 0}, {0x3075, 0x24, 0, 0},
+	{0x300d, 0x01, 0, 0}, {0x30d7, 0x80, 0x80, 0}, {0x3069, 0x00, 0x40, 0},
+	{0x303e, 0x00, 0, 0}, {0x303f, 0xc0, 0, 0}, {0x3302, 0x20, 0x20, 0},
+	{0x335f, 0x34, 0, 0}, {0x3360, 0x0c, 0, 0}, {0x3361, 0x04, 0, 0},
+	{0x3362, 0x12, 0, 0}, {0x3363, 0x88, 0, 0}, {0x3364, 0xe4, 0, 0},
+	{0x3403, 0x42, 0, 0}, {0x3088, 0x02, 0, 0}, {0x3089, 0x80, 0, 0},
+	{0x308a, 0x01, 0, 0}, {0x308b, 0xe0, 0, 0}, {0x3362, 0x12, 0, 0},
+	{0x3363, 0x88, 0, 0}, {0x3364, 0xe4, 0, 0}, {0x3403, 0x42, 0, 0},
+	{0x3088, 0x02, 0, 0}, {0x3089, 0x80, 0, 0}, {0x308a, 0x01, 0, 0},
+	{0x308b, 0xe0, 0, 0}, {0x300e, 0x37, 0, 0}, {0x300f, 0xe1, 0, 0},
+	{0x3010, 0x22, 0, 0}, {0x3011, 0x01, 0, 0}, {0x304c, 0x84, 0, 0},
+	{0x3014, 0x04, 0, 0}, {0x3015, 0x02, 0, 0}, {0x302e, 0x00, 0, 0},
+	{0x302d, 0x00, 0, 0},
+};
+
+static struct reg_value ov3640_setting_15fps_QVGA_320_240[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x00, 0, 0}, {0x304c, 0x81, 0, 0},
+	{0x30d7, 0x10, 0, 0}, {0x30d9, 0x0d, 0, 0}, {0x30db, 0x08, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x04, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0}, {0x30ba, 0x04, 0, 0},
+	{0x30bb, 0x08, 0, 0}, {0x3507, 0x06, 0, 0}, {0x350a, 0x4f, 0, 0},
+	{0x3100, 0x02, 0, 0}, {0x3301, 0xde, 0, 0}, {0x3304, 0x00, 0, 0},
+	{0x3400, 0x00, 0, 0}, {0x3404, 0x42, 0, 0}, {0x3600, 0xc4, 0, 0},
+	{0x3302, 0xef, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
+	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
+	{0x3025, 0x00, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x00, 0, 0},
+	{0x335f, 0x68, 0, 0}, {0x3360, 0x00, 0, 0}, {0x3361, 0x00, 0, 0},
+	{0x3362, 0x01, 0, 0}, {0x3363, 0x40, 0, 0}, {0x3364, 0xf0, 0, 0},
+	{0x3403, 0x00, 0, 0}, {0x3088, 0x01, 0, 0}, {0x3089, 0x40, 0, 0},
+	{0x308a, 0x00, 0, 0}, {0x308b, 0xf0, 0, 0}, {0x307c, 0x10, 0, 0},
+	{0x3090, 0xc0, 0, 0}, {0x304c, 0x84, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3011, 0x01, 0, 0},
+};
+
+static struct reg_value ov3640_setting_30fps_QVGA_320_240[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x01, 0, 0}, {0x304c, 0x82, 0, 0},
+	{0x30d7, 0x10, 0, 0}, {0x30d9, 0x0d, 0, 0}, {0x30db, 0x08, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x0c, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x3300, 0x13, 0, 0}, {0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0},
+	{0x30ba, 0x04, 0, 0}, {0x30bb, 0x08, 0, 0}, {0x3100, 0x02, 0, 0},
+	{0x3301, 0x10, 0x30, 0}, {0x3304, 0x00, 0x03, 0}, {0x3400, 0x00, 0, 0},
+	{0x3404, 0x02, 0, 0}, {0x3600, 0xc0, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3012, 0x10, 0, 0},
+	{0x3023, 0x06, 0, 0}, {0x3026, 0x03, 0, 0}, {0x3027, 0x04, 0, 0},
+	{0x302a, 0x03, 0, 0}, {0x302b, 0x10, 0, 0}, {0x3075, 0x24, 0, 0},
+	{0x300d, 0x01, 0, 0}, {0x30d7, 0x80, 0x80, 0}, {0x3069, 0x00, 0x40, 0},
+	{0x303e, 0x00, 0, 0}, {0x303f, 0xc0, 0, 0}, {0x3302, 0x20, 0x20, 0},
+	{0x335f, 0x34, 0, 0}, {0x3360, 0x0c, 0, 0}, {0x3361, 0x04, 0, 0},
+	{0x3362, 0x34, 0, 0}, {0x3363, 0x08, 0, 0}, {0x3364, 0x04, 0, 0},
+	{0x3403, 0x42, 0, 0}, {0x3088, 0x04, 0, 0}, {0x3089, 0x00, 0, 0},
+	{0x308a, 0x03, 0, 0}, {0x308b, 0x00, 0, 0}, {0x3362, 0x12, 0, 0},
+	{0x3363, 0x88, 0, 0}, {0x3364, 0xe4, 0, 0}, {0x3403, 0x42, 0, 0},
+	{0x3088, 0x02, 0, 0}, {0x3089, 0x80, 0, 0}, {0x308a, 0x01, 0, 0},
+	{0x308b, 0xe0, 0, 0}, {0x300e, 0x37, 0, 0}, {0x300f, 0xe1, 0, 0},
+	{0x3010, 0x22, 0, 0}, {0x3011, 0x01, 0, 0}, {0x304c, 0x84, 0, 0},
+};
+
+static struct ov3640_mode_info ov3640_mode_info_data[2][ov3640_mode_MAX + 1] = {
+	{
+		{ov3640_mode_VGA_640_480,    640,  480,
+		ov3640_setting_15fps_VGA_640_480,
+		ARRAY_SIZE(ov3640_setting_15fps_VGA_640_480)},
+		{ov3640_mode_QVGA_320_240,   320,  240,
+		ov3640_setting_15fps_QVGA_320_240,
+		ARRAY_SIZE(ov3640_setting_15fps_QVGA_320_240)},
+		{ov3640_mode_XGA_1024_768,   1024, 768,
+		ov3640_setting_15fps_XGA_1024_768,
+		ARRAY_SIZE(ov3640_setting_15fps_XGA_1024_768)},
+		{ov3640_mode_QXGA_2048_1536, 2048, 1536,
+		ov3640_setting_15fps_QXGA_2048_1536,
+		ARRAY_SIZE(ov3640_setting_15fps_QXGA_2048_1536)},
+	},
+	{
+		{ov3640_mode_VGA_640_480,    640,  480,
+		ov3640_setting_30fps_VGA_640_480,
+		ARRAY_SIZE(ov3640_setting_30fps_VGA_640_480)},
+		{ov3640_mode_QVGA_320_240,   320,  240,
+		ov3640_setting_30fps_QVGA_320_240,
+		ARRAY_SIZE(ov3640_setting_30fps_QVGA_320_240)},
+		{ov3640_mode_XGA_1024_768,   1024, 768,
+		ov3640_setting_30fps_XGA_1024_768,
+		ARRAY_SIZE(ov3640_setting_30fps_XGA_1024_768)},
+		{ov3640_mode_QXGA_2048_1536, 0, 0, NULL, 0},
+	},
+};
+
+static struct regulator *io_regulator;
+static struct regulator *core_regulator;
+static struct regulator *analog_regulator;
+static struct regulator *gpo_regulator;
+
+static int ov3640_probe(struct i2c_client *adapter,
+				const struct i2c_device_id *device_id);
+static int ov3640_remove(struct i2c_client *client);
+
+static s32 ov3640_read_reg(u16 reg, u8 *val);
+static s32 ov3640_write_reg(u16 reg, u8 val);
+
+static const struct i2c_device_id ov3640_id[] = {
+	{"ov3640", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ov3640_id);
+
+static struct i2c_driver ov3640_i2c_driver = {
+	.driver = {
+		  .owner = THIS_MODULE,
+		  .name  = "ov3640",
+		  },
+	.probe  = ov3640_probe,
+	.remove = ov3640_remove,
+	.id_table = ov3640_id,
+};
+
+extern void gpio_sensor_active(unsigned int csi_index);
+extern void gpio_sensor_inactive(unsigned int csi);
+
+static s32 ov3640_write_reg(u16 reg, u8 val)
+{
+	u8 au8Buf[3] = {0};
+
+	au8Buf[0] = reg >> 8;
+	au8Buf[1] = reg & 0xff;
+	au8Buf[2] = val;
+
+	if (i2c_master_send(ov3640_data.i2c_client, au8Buf, 3) < 0) {
+		pr_err("%s:write reg error:reg=%x,val=%x\n",
+			__func__, reg, val);
+		return -1;
+	}
+
+	return 0;
+}
+
+static s32 ov3640_read_reg(u16 reg, u8 *val)
+{
+	u8 au8RegBuf[2] = {0};
+	u8 u8RdVal = 0;
+
+	au8RegBuf[0] = reg >> 8;
+	au8RegBuf[1] = reg & 0xff;
+
+	if (2 != i2c_master_send(ov3640_data.i2c_client, au8RegBuf, 2)) {
+		pr_err("%s:write reg error:reg=%x\n",
+				__func__, reg);
+		return -1;
+	}
+
+	if (1 != i2c_master_recv(ov3640_data.i2c_client, &u8RdVal, 1)) {
+		pr_err("%s:read reg error:reg=%x,val=%x\n",
+				__func__, reg, u8RdVal);
+		return -1;
+	}
+
+	*val = u8RdVal;
+
+	return u8RdVal;
+}
+
+static int ov3640_init_mode(enum ov3640_frame_rate frame_rate,
+			    enum ov3640_mode mode)
+{
+	struct reg_value *pModeSetting = NULL;
+	s32 i = 0;
+	s32 iModeSettingArySize = 0;
+	register u32 Delay_ms = 0;
+	register u16 RegAddr = 0;
+	register u8 Mask = 0;
+	register u8 Val = 0;
+	u8 RegVal = 0;
+	int retval = 0;
+
+	if (mode > ov3640_mode_MAX || mode < ov3640_mode_MIN) {
+		pr_err("Wrong ov3640 mode detected!\n");
+		return -1;
+	}
+
+	pModeSetting = ov3640_mode_info_data[frame_rate][mode].init_data_ptr;
+	iModeSettingArySize =
+		ov3640_mode_info_data[frame_rate][mode].init_data_size;
+
+	ov3640_data.pix.width = ov3640_mode_info_data[frame_rate][mode].width;
+	ov3640_data.pix.height = ov3640_mode_info_data[frame_rate][mode].height;
+
+	for (i = 0; i < iModeSettingArySize; ++i, ++pModeSetting) {
+		Delay_ms = pModeSetting->u32Delay_ms;
+		RegAddr = pModeSetting->u16RegAddr;
+		Val = pModeSetting->u8Val;
+		Mask = pModeSetting->u8Mask;
+
+		if (Mask) {
+			retval = ov3640_read_reg(RegAddr, &RegVal);
+			if (retval < 0)
+				goto err;
+
+			RegVal &= ~(u8)Mask;
+			Val &= Mask;
+			Val |= RegVal;
+		}
+
+		retval = ov3640_write_reg(RegAddr, Val);
+		if (retval < 0)
+			goto err;
+
+		if (Delay_ms)
+			msleep(Delay_ms);
+	}
+err:
+	return retval;
+}
+
+/* --------------- IOCTL functions from v4l2_int_ioctl_desc --------------- */
+
+static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
+{
+	if (s == NULL) {
+		pr_err("   ERROR!! no slave device set!\n");
+		return -1;
+	}
+
+	memset(p, 0, sizeof(*p));
+	p->u.bt656.clock_curr = ov3640_data.mclk;
+	pr_debug("   clock_curr=mclk=%d\n", ov3640_data.mclk);
+	p->if_type = V4L2_IF_TYPE_BT656;
+	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.clock_min = OV3640_XCLK_MIN;
+	p->u.bt656.clock_max = OV3640_XCLK_MAX;
+	p->u.bt656.bt_sync_correct = 1;  /* Indicate external vsync */
+
+	return 0;
+}
+
+/*!
+ * ioctl_s_power - V4L2 sensor interface handler for VIDIOC_S_POWER ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @on: indicates power mode (on or off)
+ *
+ * Turns the power on or off, depending on the value of on and returns the
+ * appropriate error code.
+ */
+static int ioctl_s_power(struct v4l2_int_device *s, int on)
+{
+	struct sensor *sensor = s->priv;
+
+	if (on && !sensor->on) {
+		gpio_sensor_active(ov3640_data.csi);
+		if (io_regulator)
+			if (regulator_enable(io_regulator) != 0)
+				return -EIO;
+		if (core_regulator)
+			if (regulator_enable(core_regulator) != 0)
+				return -EIO;
+		if (gpo_regulator)
+			if (regulator_enable(gpo_regulator) != 0)
+				return -EIO;
+		if (analog_regulator)
+			if (regulator_enable(analog_regulator) != 0)
+				return -EIO;
+	} else if (!on && sensor->on) {
+		if (analog_regulator)
+			regulator_disable(analog_regulator);
+		if (core_regulator)
+			regulator_disable(core_regulator);
+		if (io_regulator)
+			regulator_disable(io_regulator);
+		if (gpo_regulator)
+			regulator_disable(gpo_regulator);
+		gpio_sensor_inactive(ov3640_data.csi);
+	}
+
+	sensor->on = on;
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_parm - V4L2 sensor interface handler for VIDIOC_G_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the sensor's video CAPTURE parameters.
+ */
+static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	struct sensor *sensor = s->priv;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+	int ret = 0;
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		memset(a, 0, sizeof(*a));
+		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cparm->capability = sensor->streamcap.capability;
+		cparm->timeperframe = sensor->streamcap.timeperframe;
+		cparm->capturemode = sensor->streamcap.capturemode;
+		ret = 0;
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_debug("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_parm - V4L2 sensor interface handler for VIDIOC_S_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the sensor to use the input parameters, if possible.  If
+ * not possible, reverts to the old parameters and returns the
+ * appropriate error code.
+ */
+static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	struct sensor *sensor = s->priv;
+	struct v4l2_fract *timeperframe = &a->parm.capture.timeperframe;
+	u32 tgt_fps;	/* target frames per secound */
+	enum ov3640_frame_rate frame_rate;
+	int ret = 0;
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		/* Check that the new frame rate is allowed. */
+		if ((timeperframe->numerator == 0) ||
+		    (timeperframe->denominator == 0)) {
+			timeperframe->denominator = DEFAULT_FPS;
+			timeperframe->numerator = 1;
+		}
+
+		tgt_fps = timeperframe->denominator /
+			  timeperframe->numerator;
+
+		if (tgt_fps > MAX_FPS) {
+			timeperframe->denominator = MAX_FPS;
+			timeperframe->numerator = 1;
+		} else if (tgt_fps < MIN_FPS) {
+			timeperframe->denominator = MIN_FPS;
+			timeperframe->numerator = 1;
+		}
+
+		/* Actual frame rate we use */
+		tgt_fps = timeperframe->denominator /
+			  timeperframe->numerator;
+
+		if (tgt_fps == 15)
+			frame_rate = ov3640_15_fps;
+		else if (tgt_fps == 30)
+			frame_rate = ov3640_30_fps;
+		else {
+			pr_err(" The camera frame rate is not supported!\n");
+			return -EINVAL;
+		}
+
+		sensor->streamcap.timeperframe = *timeperframe;
+		sensor->streamcap.capturemode =
+				(u32)a->parm.capture.capturemode;
+
+		ret = ov3640_init_mode(frame_rate,
+				       sensor->streamcap.capturemode);
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_debug("   type is not " \
+			"V4L2_BUF_TYPE_VIDEO_CAPTURE but %d\n",
+			a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_debug("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_g_fmt_cap - V4L2 sensor interface handler for ioctl_g_fmt_cap
+ * @s: pointer to standard V4L2 device structure
+ * @f: pointer to standard V4L2 v4l2_format structure
+ *
+ * Returns the sensor's current pixel format in the v4l2_format
+ * parameter.
+ */
+static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	struct sensor *sensor = s->priv;
+
+	f->fmt.pix = sensor->pix;
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_G_CTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control's current
+ * value from the video_control[] array.  Otherwise, returns -EINVAL
+ * if the control is not supported.
+ */
+static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int ret = 0;
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		vc->value = ov3640_data.brightness;
+		break;
+	case V4L2_CID_HUE:
+		vc->value = ov3640_data.hue;
+		break;
+	case V4L2_CID_CONTRAST:
+		vc->value = ov3640_data.contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		vc->value = ov3640_data.saturation;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		vc->value = ov3640_data.red;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		vc->value = ov3640_data.blue;
+		break;
+	case V4L2_CID_EXPOSURE:
+		vc->value = ov3640_data.ae_mode;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_ctrl - V4L2 sensor interface handler for VIDIOC_S_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_S_CTRL ioctl structure
+ *
+ * If the requested control is supported, sets the control's current
+ * value in HW (and updates the video_control[] array).  Otherwise,
+ * returns -EINVAL if the control is not supported.
+ */
+static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int retval = 0;
+
+	pr_debug("In ov3640:ioctl_s_ctrl %d\n",
+		 vc->id);
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		break;
+	case V4L2_CID_CONTRAST:
+		break;
+	case V4L2_CID_SATURATION:
+		break;
+	case V4L2_CID_HUE:
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		break;
+	case V4L2_CID_RED_BALANCE:
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		break;
+	case V4L2_CID_GAMMA:
+		break;
+	case V4L2_CID_EXPOSURE:
+		break;
+	case V4L2_CID_AUTOGAIN:
+		break;
+	case V4L2_CID_GAIN:
+		break;
+	case V4L2_CID_HFLIP:
+		break;
+	case V4L2_CID_VFLIP:
+		break;
+	default:
+		retval = -EPERM;
+		break;
+	}
+
+	return retval;
+}
+
+/*!
+ * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
+ * @s: pointer to standard V4L2 device structure
+ */
+static int ioctl_init(struct v4l2_int_device *s)
+{
+
+	return 0;
+}
+
+/*!
+ * ioctl_dev_init - V4L2 sensor interface handler for vidioc_int_dev_init_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Initialise the device when slave attaches to the master.
+ */
+static int ioctl_dev_init(struct v4l2_int_device *s)
+{
+	struct sensor *sensor = s->priv;
+	u32 tgt_xclk;	/* target xclk */
+	u32 tgt_fps;	/* target frames per secound */
+	enum ov3640_frame_rate frame_rate;
+
+	gpio_sensor_active(ov3640_data.csi);
+	ov3640_data.on = true;
+
+	/* mclk */
+	tgt_xclk = ov3640_data.mclk;
+	tgt_xclk = min(tgt_xclk, (u32)OV3640_XCLK_MAX);
+	tgt_xclk = max(tgt_xclk, (u32)OV3640_XCLK_MIN);
+	ov3640_data.mclk = tgt_xclk;
+
+	pr_debug("   Setting mclk to %d MHz\n", tgt_xclk / 1000000);
+	set_mclk_rate(&ov3640_data.mclk, ov3640_data.csi);
+
+	/* Default camera frame rate is set in probe */
+	tgt_fps = sensor->streamcap.timeperframe.denominator /
+		  sensor->streamcap.timeperframe.numerator;
+
+	if (tgt_fps == 15)
+		frame_rate = ov3640_15_fps;
+	else if (tgt_fps == 30)
+		frame_rate = ov3640_30_fps;
+	else
+		return -EINVAL; /* Only support 15fps or 30fps now. */
+
+	return ov3640_init_mode(frame_rate,
+				sensor->streamcap.capturemode);
+}
+
+/*!
+ * ioctl_dev_exit - V4L2 sensor interface handler for vidioc_int_dev_exit_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Delinitialise the device when slave detaches to the master.
+ */
+static int ioctl_dev_exit(struct v4l2_int_device *s)
+{
+	gpio_sensor_inactive(ov3640_data.csi);
+
+	return 0;
+}
+
+/*!
+ * This structure defines all the ioctls for this module and links them to the
+ * enumeration.
+ */
+static struct v4l2_int_ioctl_desc ov3640_ioctl_desc[] = {
+	{vidioc_int_dev_init_num, (v4l2_int_ioctl_func *)ioctl_dev_init},
+	{vidioc_int_dev_exit_num, ioctl_dev_exit},
+	{vidioc_int_s_power_num, (v4l2_int_ioctl_func *)ioctl_s_power},
+	{vidioc_int_g_ifparm_num, (v4l2_int_ioctl_func *)ioctl_g_ifparm},
+/*	{vidioc_int_g_needs_reset_num,
+				(v4l2_int_ioctl_func *)ioctl_g_needs_reset}, */
+/*	{vidioc_int_reset_num, (v4l2_int_ioctl_func *)ioctl_reset}, */
+	{vidioc_int_init_num, (v4l2_int_ioctl_func *)ioctl_init},
+/*	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_enum_fmt_cap}, */
+/*	{vidioc_int_try_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_try_fmt_cap}, */
+	{vidioc_int_g_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_g_fmt_cap},
+/*	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_fmt_cap}, */
+	{vidioc_int_g_parm_num, (v4l2_int_ioctl_func *)ioctl_g_parm},
+	{vidioc_int_s_parm_num, (v4l2_int_ioctl_func *)ioctl_s_parm},
+/*	{vidioc_int_queryctrl_num, (v4l2_int_ioctl_func *)ioctl_queryctrl}, */
+	{vidioc_int_g_ctrl_num, (v4l2_int_ioctl_func *)ioctl_g_ctrl},
+	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *)ioctl_s_ctrl},
+};
+
+static struct v4l2_int_slave ov3640_slave = {
+	.ioctls = ov3640_ioctl_desc,
+	.num_ioctls = ARRAY_SIZE(ov3640_ioctl_desc),
+};
+
+static struct v4l2_int_device ov3640_int_device = {
+	.module = THIS_MODULE,
+	.name = "ov3640",
+	.type = v4l2_int_type_slave,
+	.u = {
+		.slave = &ov3640_slave,
+	},
+};
+
+/*!
+ * ov3640 I2C probe function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  Error code indicating success or failure
+ */
+static int ov3640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int retval;
+	struct mxc_camera_platform_data *plat_data = client->dev.platform_data;
+
+	/* Set initial values for the sensor struct. */
+	memset(&ov3640_data, 0, sizeof(ov3640_data));
+	ov3640_data.mclk = 24000000; /* 6 - 54 MHz, typical 24MHz */
+	ov3640_data.mclk = plat_data->mclk;
+	ov3640_data.csi = plat_data->csi;
+
+	ov3640_data.i2c_client = client;
+	ov3640_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	ov3640_data.pix.width = 640;
+	ov3640_data.pix.height = 480;
+	ov3640_data.streamcap.capability = V4L2_MODE_HIGHQUALITY |
+					   V4L2_CAP_TIMEPERFRAME;
+	ov3640_data.streamcap.capturemode = 0;
+	ov3640_data.streamcap.timeperframe.denominator = DEFAULT_FPS;
+	ov3640_data.streamcap.timeperframe.numerator = 1;
+
+	if (plat_data->io_regulator) {
+		io_regulator = regulator_get(&client->dev,
+					     plat_data->io_regulator);
+		if (!IS_ERR(io_regulator)) {
+			regulator_set_voltage(io_regulator,
+					      OV3640_VOLTAGE_DIGITAL_IO,
+					      OV3640_VOLTAGE_DIGITAL_IO);
+			if (regulator_enable(io_regulator) != 0) {
+				pr_err("%s:io set voltage error\n", __func__);
+				goto err1;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:io set voltage ok\n", __func__);
+			}
+		} else
+			io_regulator = NULL;
+	}
+
+	if (plat_data->core_regulator) {
+		core_regulator = regulator_get(&client->dev,
+					       plat_data->core_regulator);
+		if (!IS_ERR(core_regulator)) {
+			regulator_set_voltage(core_regulator,
+					      OV3640_VOLTAGE_DIGITAL_CORE,
+					      OV3640_VOLTAGE_DIGITAL_CORE);
+			if (regulator_enable(core_regulator) != 0) {
+				pr_err("%s:core set voltage error\n", __func__);
+				goto err2;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:core set voltage ok\n", __func__);
+			}
+		} else
+			core_regulator = NULL;
+	}
+
+	if (plat_data->analog_regulator) {
+		analog_regulator = regulator_get(&client->dev,
+						 plat_data->analog_regulator);
+		if (!IS_ERR(analog_regulator)) {
+			regulator_set_voltage(analog_regulator,
+					      OV3640_VOLTAGE_ANALOG,
+					      OV3640_VOLTAGE_ANALOG);
+			if (regulator_enable(analog_regulator) != 0) {
+				pr_err("%s:analog set voltage error\n",
+					__func__);
+				goto err3;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:analog set voltage ok\n", __func__);
+			}
+		} else
+			analog_regulator = NULL;
+	}
+
+	if (plat_data->gpo_regulator) {
+		gpo_regulator = regulator_get(&client->dev,
+					      plat_data->gpo_regulator);
+		if (!IS_ERR(gpo_regulator)) {
+			if (regulator_enable(gpo_regulator) != 0) {
+				pr_err("%s:gpo3 enable error\n", __func__);
+				goto err4;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:gpo3 enable ok\n", __func__);
+			}
+		} else
+			gpo_regulator = NULL;
+	}
+
+	ov3640_int_device.priv = &ov3640_data;
+	retval = v4l2_int_device_register(&ov3640_int_device);
+
+	return retval;
+
+err4:
+	if (analog_regulator) {
+		regulator_disable(analog_regulator);
+		regulator_put(analog_regulator);
+	}
+err3:
+	if (core_regulator) {
+		regulator_disable(core_regulator);
+		regulator_put(core_regulator);
+	}
+err2:
+	if (io_regulator) {
+		regulator_disable(io_regulator);
+		regulator_put(io_regulator);
+	}
+err1:
+	return -1;
+}
+
+/*!
+ * ov3640 I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int ov3640_remove(struct i2c_client *client)
+{
+	v4l2_int_device_unregister(&ov3640_int_device);
+
+	if (gpo_regulator) {
+		regulator_disable(gpo_regulator);
+		regulator_put(gpo_regulator);
+	}
+
+	if (analog_regulator) {
+		regulator_disable(analog_regulator);
+		regulator_put(analog_regulator);
+	}
+
+	if (core_regulator) {
+		regulator_disable(core_regulator);
+		regulator_put(core_regulator);
+	}
+
+	if (io_regulator) {
+		regulator_disable(io_regulator);
+		regulator_put(io_regulator);
+	}
+
+	return 0;
+}
+
+/*!
+ * ov3640 init function
+ * Called by insmod ov3640_camera.ko.
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int ov3640_init(void)
+{
+	u8 err;
+
+	err = i2c_add_driver(&ov3640_i2c_driver);
+	if (err != 0)
+		pr_err("%s:driver registration failed, error=%d \n",
+			__func__, err);
+
+	return err;
+}
+
+/*!
+ * OV3640 cleanup function
+ * Called on rmmod ov3640_camera.ko
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit ov3640_clean(void)
+{
+	i2c_del_driver(&ov3640_i2c_driver);
+}
+
+module_init(ov3640_init);
+module_exit(ov3640_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("OV3640 Camera Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0");
+MODULE_ALIAS("CSI");
diff --git a/drivers/media/video/mxc/capture/sensor_clock.c b/drivers/media/video/mxc/capture/sensor_clock.c
new file mode 100644
index 000000000000..c15cf27c57cd
--- /dev/null
+++ b/drivers/media/video/mxc/capture/sensor_clock.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file sensor_clock.c
+ *
+ * @brief camera clock function
+ *
+ * @ingroup Camera
+ */
+#include <linux/init.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/clk.h>
+
+#if defined(CONFIG_MXC_IPU_V1) || defined(CONFIG_VIDEO_MXC_EMMA_CAMERA) \
+			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA_MODULE) \
+			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA)
+/*
+ * set_mclk_rate
+ *
+ * @param       p_mclk_freq  mclk frequence
+ *
+ */
+void set_mclk_rate(uint32_t * p_mclk_freq)
+{
+	struct clk *clk;
+	uint32_t freq = 0;
+
+	clk = clk_get(NULL, "csi_clk");
+
+	freq = clk_round_rate(clk, *p_mclk_freq);
+	clk_set_rate(clk, freq);
+
+	*p_mclk_freq = freq;
+
+	clk_put(clk);
+	pr_debug("mclk frequency = %d\n", *p_mclk_freq);
+}
+#else
+/*
+ * set_mclk_rate
+ *
+ * @param       p_mclk_freq  mclk frequence
+ * @param	csi         csi 0 or csi 1
+ *
+ */
+void set_mclk_rate(uint32_t *p_mclk_freq, uint32_t csi)
+{
+	struct clk *clk;
+	uint32_t freq = 0;
+	char *mclk;
+
+	if (csi == 0) {
+		mclk = "csi_mclk1";
+	} else if (csi == 1) {
+		mclk = "csi_mclk2";
+	} else {
+		pr_debug("invalid csi num %d\n", csi);
+		return;
+	}
+
+	clk = clk_get(NULL, mclk);
+
+	freq = clk_round_rate(clk, *p_mclk_freq);
+	clk_set_rate(clk, freq);
+
+	*p_mclk_freq = freq;
+
+	clk_put(clk);
+	pr_debug("%s frequency = %d\n", mclk, *p_mclk_freq);
+}
+#endif
+
+/* Exported symbols for modules. */
+EXPORT_SYMBOL(set_mclk_rate);
diff --git a/drivers/media/video/mxc/opl/Makefile b/drivers/media/video/mxc/opl/Makefile
new file mode 100644
index 000000000000..092a62c5ac4a
--- /dev/null
+++ b/drivers/media/video/mxc/opl/Makefile
@@ -0,0 +1,5 @@
+opl-objs	:= opl_mod.o rotate90_u16.o rotate270_u16.o	\
+		   rotate90_u16_qcif.o rotate270_u16_qcif.o	\
+		   vmirror_u16.o hmirror_rotate180_u16.o
+
+obj-$(CONFIG_VIDEO_MXC_OPL)	+= opl.o
diff --git a/drivers/media/video/mxc/opl/hmirror_rotate180_u16.c b/drivers/media/video/mxc/opl/hmirror_rotate180_u16.c
new file mode 100644
index 000000000000..3119a128c1f2
--- /dev/null
+++ b/drivers/media/video/mxc/opl/hmirror_rotate180_u16.c
@@ -0,0 +1,259 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include "opl.h"
+
+static inline u32 rot_left_u16(u16 x, unsigned int n)
+{
+	return (x << n) | (x >> (16 - n));
+}
+
+static inline u32 rot_left_u32(u32 x, unsigned int n)
+{
+	return (x << n) | (x >> (32 - n));
+}
+
+static inline u32 byte_swap_u32(u32 x)
+{
+	u32 t1, t2, t3;
+
+	t1 = x ^ ((x << 16) | x >> 16);
+	t2 = t1 & 0xff00ffff;
+	t3 = (x >> 8) | (x << 24);
+	return t3 ^ (t2 >> 8);
+}
+
+static int opl_hmirror_u16_by1(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror);
+static int opl_hmirror_u16_by2(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror);
+static int opl_hmirror_u16_by4(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror);
+static int opl_hmirror_u16_by8(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror);
+
+int opl_hmirror_u16(const u8 * src, int src_line_stride, int width, int height,
+		    u8 * dst, int dst_line_stride)
+{
+	if (!src || !dst)
+		return OPLERR_NULL_PTR;
+
+	if (width == 0 || height == 0 || src_line_stride == 0
+	    || dst_line_stride == 0)
+		return OPLERR_BAD_ARG;
+
+	if (width % 8 == 0)
+		return opl_hmirror_u16_by8(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 0);
+	else if (width % 4 == 0)
+		return opl_hmirror_u16_by4(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 0);
+	else if (width % 2 == 0)
+		return opl_hmirror_u16_by2(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 0);
+	else			/* (width % 1) */
+		return opl_hmirror_u16_by1(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 0);
+}
+
+int opl_rotate180_u16(const u8 * src, int src_line_stride, int width,
+		      int height, u8 * dst, int dst_line_stride)
+{
+	if (!src || !dst)
+		return OPLERR_NULL_PTR;
+
+	if (width == 0 || height == 0 || src_line_stride == 0
+	    || dst_line_stride == 0)
+		return OPLERR_BAD_ARG;
+
+	if (width % 8 == 0)
+		return opl_hmirror_u16_by8(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 1);
+	else if (width % 4 == 0)
+		return opl_hmirror_u16_by4(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 1);
+	else if (width % 2 == 0)
+		return opl_hmirror_u16_by2(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 1);
+	else			/* (width % 1) */
+		return opl_hmirror_u16_by1(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 1);
+}
+
+static int opl_hmirror_u16_by1(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror)
+{
+	const u8 *src_row_addr;
+	const u8 *psrc;
+	u8 *dst_row_addr, *pdst;
+	int i, j;
+	u16 pixel;
+
+	src_row_addr = src;
+	if (vmirror) {
+		dst_row_addr = dst + dst_line_stride * (height - 1);
+		dst_line_stride = -dst_line_stride;
+	} else
+		dst_row_addr = dst;
+
+	/* Loop over all rows */
+	for (i = 0; i < height; i++) {
+		/* Loop over each pixel */
+		psrc = src_row_addr;
+		pdst = dst_row_addr + (width - 1) * BYTES_PER_PIXEL
+		    - (BYTES_PER_PIXEL - BYTES_PER_PIXEL);
+		for (j = 0; j < width; j++) {
+			pixel = *(u16 *) psrc;
+			*(u16 *) pdst = pixel;
+			psrc += BYTES_PER_PIXEL;
+			pdst -= BYTES_PER_PIXEL;
+		}
+		src_row_addr += src_line_stride;
+		dst_row_addr += dst_line_stride;
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+static int opl_hmirror_u16_by2(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror)
+{
+	const u8 *src_row_addr;
+	const u8 *psrc;
+	u8 *dst_row_addr, *pdst;
+	int i, j;
+	u32 pixelsin, pixelsout;
+
+	src_row_addr = src;
+	if (vmirror) {
+		dst_row_addr = dst + dst_line_stride * (height - 1);
+		dst_line_stride = -dst_line_stride;
+	} else
+		dst_row_addr = dst;
+
+	/* Loop over all rows */
+	for (i = 0; i < height; i++) {
+		/* Loop over each pixel */
+		psrc = src_row_addr;
+		pdst = dst_row_addr + (width - 2) * BYTES_PER_PIXEL;
+		for (j = 0; j < (width >> 1); j++) {
+			pixelsin = *(u32 *) psrc;
+			pixelsout = rot_left_u32(pixelsin, 16);
+			*(u32 *) pdst = pixelsout;
+			psrc += BYTES_PER_2PIXEL;
+			pdst -= BYTES_PER_2PIXEL;
+		}
+		src_row_addr += src_line_stride;
+		dst_row_addr += dst_line_stride;
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+static int opl_hmirror_u16_by4(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror)
+{
+	const u8 *src_row_addr;
+	const u8 *psrc;
+	u8 *dst_row_addr, *pdst;
+	int i, j;
+
+	union doubleword {
+		u64 dw;
+		u32 w[2];
+	};
+
+	union doubleword inbuf;
+	union doubleword outbuf;
+
+	src_row_addr = src;
+	if (vmirror) {
+		dst_row_addr = dst + dst_line_stride * (height - 1);
+		dst_line_stride = -dst_line_stride;
+	} else
+		dst_row_addr = dst;
+
+	/* Loop over all rows */
+	for (i = 0; i < height; i++) {
+		/* Loop over each pixel */
+		psrc = src_row_addr;
+		pdst = dst_row_addr + (width - 4) * BYTES_PER_PIXEL;
+		for (j = 0; j < (width >> 2); j++) {
+			inbuf.dw = *(u64 *) psrc;
+			outbuf.w[0] = rot_left_u32(inbuf.w[1], 16);
+			outbuf.w[1] = rot_left_u32(inbuf.w[0], 16);
+			*(u64 *) pdst = outbuf.dw;
+			psrc += BYTES_PER_4PIXEL;
+			pdst -= BYTES_PER_4PIXEL;
+		}
+		src_row_addr += src_line_stride;
+		dst_row_addr += dst_line_stride;
+	}
+	return OPLERR_SUCCESS;
+}
+
+static int opl_hmirror_u16_by8(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror)
+{
+	const u8 *src_row_addr;
+	const u8 *psrc;
+	u8 *dst_row_addr, *pdst;
+	int i, j;
+
+	src_row_addr = src;
+	if (vmirror) {
+		dst_row_addr = dst + dst_line_stride * (height - 1);
+		dst_line_stride = -dst_line_stride;
+	} else
+		dst_row_addr = dst;
+
+	/* Loop over all rows */
+	for (i = 0; i < height; i++) {
+		/* Loop over each pixel */
+		psrc = src_row_addr;
+		pdst = dst_row_addr + (width - 1) * BYTES_PER_PIXEL - 2;
+		for (j = (width >> 3); j > 0; j--) {
+			__asm__ volatile (
+				"ldmia	%0!,{r2-r5}\n\t"
+				"mov	r6, r2\n\t"
+				"mov	r7, r3\n\t"
+				"mov	r2, r5, ROR #16\n\t"
+				"mov	r3, r4, ROR #16\n\t"
+				"mov	r4, r7, ROR #16\n\t"
+				"mov	r5, r6, ROR #16\n\t"
+				"stmda	%1!,{r2-r5}\n\t"
+
+				:"+r"(psrc), "+r"(pdst)
+				:"0"(psrc), "1"(pdst)
+				:"r2", "r3", "r4", "r5", "r6", "r7",
+				"memory"
+			);
+		}
+		src_row_addr += src_line_stride;
+		dst_row_addr += dst_line_stride;
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+EXPORT_SYMBOL(opl_hmirror_u16);
+EXPORT_SYMBOL(opl_rotate180_u16);
diff --git a/drivers/media/video/mxc/opl/opl.h b/drivers/media/video/mxc/opl/opl.h
new file mode 100644
index 000000000000..24644c8e78fa
--- /dev/null
+++ b/drivers/media/video/mxc/opl/opl.h
@@ -0,0 +1,162 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup OPLIP OPL Image Processing
+ */
+/*!
+ * @file opl.h
+ *
+ * @brief The OPL (Open Primitives Library) Image Processing library defines
+ * efficient functions for rotation and mirroring.
+ *
+ * It includes ARM9-optimized rotation and mirroring functions. It is derived
+ * from the original OPL project which is found at sourceforge.freescale.net.
+ *
+ * @ingroup OPLIP
+ */
+#ifndef __OPL_H__
+#define __OPL_H__
+
+#include <linux/types.h>
+
+#define BYTES_PER_PIXEL			2
+#define CACHE_LINE_WORDS		8
+#define BYTES_PER_WORD			4
+
+#define BYTES_PER_2PIXEL		(BYTES_PER_PIXEL * 2)
+#define BYTES_PER_4PIXEL		(BYTES_PER_PIXEL * 4)
+#define BYTES_PER_8PIXEL		(BYTES_PER_PIXEL * 8)
+
+#define QCIF_Y_WIDTH			176
+#define QCIF_Y_HEIGHT			144
+
+/*! Enumerations of opl error code */
+enum opl_error {
+	OPLERR_SUCCESS = 0,
+	OPLERR_NULL_PTR,
+	OPLERR_BAD_ARG,
+	OPLERR_DIV_BY_ZERO,
+	OPLERR_OVER_FLOW,
+	OPLERR_UNDER_FLOW,
+	OPLERR_MISALIGNED,
+};
+
+/*!
+ * @brief Rotate a 16bbp buffer 90 degrees clockwise.
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_rotate90_u16(const u8 * src, int src_line_stride, int width, int height,
+		     u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Rotate a 16bbp buffer 180 degrees clockwise.
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_rotate180_u16(const u8 * src, int src_line_stride, int width,
+		      int height, u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Rotate a 16bbp buffer 270 degrees clockwise
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_rotate270_u16(const u8 * src, int src_line_stride, int width,
+		      int height, u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Mirror a 16bpp buffer horizontally
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_hmirror_u16(const u8 * src, int src_line_stride, int width, int height,
+		    u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Mirror a 16bpp buffer vertically
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_vmirror_u16(const u8 * src, int src_line_stride, int width, int height,
+		    u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Rotate a 16bbp buffer 90 degrees clockwise and mirror vertically
+ *	  It is equivalent to rotate 270 degree and mirror horizontally
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_rotate90_vmirror_u16(const u8 * src, int src_line_stride, int width,
+			     int height, u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Rotate a 16bbp buffer 270 degrees clockwise and mirror vertically
+ *	  It is equivalent to rotate 90 degree and mirror horizontally
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_rotate270_vmirror_u16(const u8 * src, int src_line_stride, int width,
+			      int height, u8 * dst, int dst_line_stride);
+
+#endif				/* __OPL_H__ */
diff --git a/drivers/media/video/mxc/opl/opl_mod.c b/drivers/media/video/mxc/opl/opl_mod.c
new file mode 100644
index 000000000000..a581aadda252
--- /dev/null
+++ b/drivers/media/video/mxc/opl/opl_mod.c
@@ -0,0 +1,30 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+
+static __init int opl_init(void)
+{
+	return 0;
+}
+
+static void __exit opl_exit(void)
+{
+}
+
+module_init(opl_init);
+module_exit(opl_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("OPL Software Rotation/Mirroring");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/opl/rotate270_u16.c b/drivers/media/video/mxc/opl/rotate270_u16.c
new file mode 100644
index 000000000000..add87f1a9e44
--- /dev/null
+++ b/drivers/media/video/mxc/opl/rotate270_u16.c
@@ -0,0 +1,285 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include "opl.h"
+
+static int opl_rotate270_u16_by16(const u8 * src, int src_line_stride,
+				  int width, int height, u8 * dst,
+				  int dst_line_stride, int vmirror);
+static int opl_rotate270_u16_by4(const u8 * src, int src_line_stride, int width,
+				 int height, u8 * dst, int dst_line_stride,
+				 int vmirror);
+static int opl_rotate270_vmirror_u16_both(const u8 * src, int src_line_stride,
+					  int width, int height, u8 * dst,
+					  int dst_line_stride, int vmirror);
+int opl_rotate270_u16_qcif(const u8 * src, u8 * dst);
+
+int opl_rotate270_u16(const u8 * src, int src_line_stride, int width,
+		      int height, u8 * dst, int dst_line_stride)
+{
+	return opl_rotate270_vmirror_u16_both(src, src_line_stride, width,
+					      height, dst, dst_line_stride, 0);
+}
+
+int opl_rotate270_vmirror_u16(const u8 * src, int src_line_stride, int width,
+			      int height, u8 * dst, int dst_line_stride)
+{
+	return opl_rotate270_vmirror_u16_both(src, src_line_stride, width,
+					      height, dst, dst_line_stride, 1);
+}
+
+static int opl_rotate270_vmirror_u16_both(const u8 * src, int src_line_stride,
+					  int width, int height, u8 * dst,
+					  int dst_line_stride, int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL;
+	const int BLOCK_SIZE_PIXELS_BY4 = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL / 4;
+
+	if (!src || !dst)
+		return OPLERR_NULL_PTR;
+
+	if (width == 0 || height == 0 || src_line_stride == 0
+	    || dst_line_stride == 0)
+		return OPLERR_BAD_ARG;
+
+	/* The QCIF algorithm doesn't support vertical mirroring */
+	if (vmirror == 0 && width == QCIF_Y_WIDTH && height == QCIF_Y_HEIGHT
+	    && src_line_stride == QCIF_Y_WIDTH * 2
+	    && src_line_stride == QCIF_Y_HEIGHT * 2)
+		return opl_rotate270_u16_qcif(src, dst);
+	else if (width % BLOCK_SIZE_PIXELS == 0
+		 && height % BLOCK_SIZE_PIXELS == 0)
+		return opl_rotate270_u16_by16(src, src_line_stride, width,
+					      height, dst, dst_line_stride,
+					      vmirror);
+	else if (width % BLOCK_SIZE_PIXELS_BY4 == 0
+		 && height % BLOCK_SIZE_PIXELS_BY4 == 0)
+		return opl_rotate270_u16_by4(src, src_line_stride, width,
+					     height, dst, dst_line_stride,
+					     vmirror);
+	else
+		return OPLERR_BAD_ARG;
+}
+
+/*
+ * Rotate Counter Clockwise, divide RGB component into 16 row strips, read
+ * non sequentially and write sequentially. This is done in 16 line strips
+ * so that the cache is used better. Cachelines are 8 words = 32 bytes. Pixels
+ * are 2 bytes. The 16 reads will be cache misses, but the next 240 should
+ * be from cache. The writes to the output buffer will be sequential for 16
+ * writes.
+ *
+ * Example:
+ * Input data matrix:	    output matrix
+ *
+ * 0 | 1 | 2 | 3 | 4 |      4 | 0 | 0 | 3 |
+ * 4 | 3 | 2 | 1 | 0 |      3 | 1 | 9 | 6 |
+ * 6 | 7 | 8 | 9 | 0 |      2 | 2 | 8 | 2 |
+ * 5 | 3 | 2 | 6 | 3 |      1 | 3 | 7 | 3 |
+ * ^			    0 | 4 | 6 | 5 | < Write the input data sequentially
+ * Read first column
+ * Start at the bottom
+ * Move to next column and repeat
+ *
+ * Loop over k decreasing (blocks)
+ * in_block_ptr = src + (((RGB_HEIGHT_PIXELS / BLOCK_SIZE_PIXELS) - k)
+ *                * BLOCK_SIZE_PIXELS) * (RGB_WIDTH_BYTES)
+ * out_block_ptr = dst + (((RGB_HEIGHT_PIXELS / BLOCK_SIZE_PIXELS) - k)
+ *                * BLOCK_SIZE_BYTES) + (RGB_WIDTH_PIXELS - 1)
+ *                * RGB_HEIGHT_PIXELS * BYTES_PER_PIXEL
+ *
+ * Loop over i decreasing (width)
+ * Each pix:
+ * in_block_ptr += RGB_WIDTH_BYTES
+ * out_block_ptr += 4
+ *
+ * Each row of block:
+ * in_block_ptr -= RGB_WIDTH_BYTES * BLOCK_SIZE_PIXELS - 2
+ * out_block_ptr -= RGB_HEIGHT_PIXELS * BYTES_PER_PIXEL + 2 * BLOCK_SIZE_PIXELS;
+ *
+ * It may perform vertical mirroring too depending on the vmirror flag.
+ */
+static int opl_rotate270_u16_by16(const u8 * src, int src_line_stride,
+				  int width, int height, u8 * dst,
+				  int dst_line_stride, int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL;
+	const int IN_INDEX = src_line_stride * BLOCK_SIZE_PIXELS
+	    - BYTES_PER_PIXEL;
+	const int OUT_INDEX = vmirror ?
+	    -dst_line_stride + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS
+	    : dst_line_stride + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS;
+	const u8 *in_block_ptr;
+	u8 *out_block_ptr;
+	int i, k;
+
+	for (k = height / BLOCK_SIZE_PIXELS; k > 0; k--) {
+		in_block_ptr = src + (((height / BLOCK_SIZE_PIXELS) - k)
+				      * BLOCK_SIZE_PIXELS) * src_line_stride;
+		out_block_ptr = dst + (((height / BLOCK_SIZE_PIXELS) - k)
+				       * BLOCK_SIZE_PIXELS * BYTES_PER_PIXEL) +
+		    (width - 1) * dst_line_stride;
+
+		/*
+		* For vertical mirroring the writing starts from the
+		* first line
+		*/
+		if (vmirror)
+			out_block_ptr -= dst_line_stride * (width - 1);
+
+		for (i = width; i > 0; i--) {
+			__asm__ volatile (
+				"ldrh	r2, [%0], %4\n\t"
+				"ldrh	r3, [%0], %4\n\t"
+				"ldrh	r4, [%0], %4\n\t"
+				"ldrh	r5, [%0], %4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], %4\n\t"
+				"ldrh	r3, [%0], %4\n\t"
+				"ldrh	r4, [%0], %4\n\t"
+				"ldrh	r5, [%0], %4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], %4\n\t"
+				"ldrh	r3, [%0], %4\n\t"
+				"ldrh	r4, [%0], %4\n\t"
+				"ldrh	r5, [%0], %4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], %4\n\t"
+				"ldrh	r3, [%0], %4\n\t"
+				"ldrh	r4, [%0], %4\n\t"
+				"ldrh	r5, [%0], %4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				:"+r" (in_block_ptr), "+r"(out_block_ptr)	/* output */
+				:"0"(in_block_ptr), "1"(out_block_ptr), "r"(src_line_stride)	/* input  */
+				:"r2", "r3", "r4", "r5", "memory"	/* modify */
+			);
+			in_block_ptr -= IN_INDEX;
+			out_block_ptr -= OUT_INDEX;
+		}
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+/*
+ * Rotate Counter Clockwise, divide RGB component into 4 row strips, read
+ * non sequentially and write sequentially. This is done in 4 line strips
+ * so that the cache is used better. Cachelines are 8 words = 32 bytes. Pixels
+ * are 2 bytes. The 4 reads will be cache misses, but the next 60 should
+ * be from cache. The writes to the output buffer will be sequential for 4
+ * writes.
+ *
+ * Example:
+ * Input data matrix:	    output matrix
+ *
+ * 0 | 1 | 2 | 3 | 4 |      4 | 0 | 0 | 3 |
+ * 4 | 3 | 2 | 1 | 0 |      3 | 1 | 9 | 6 |
+ * 6 | 7 | 8 | 9 | 0 |      2 | 2 | 8 | 2 |
+ * 5 | 3 | 2 | 6 | 3 |      1 | 3 | 7 | 3 |
+ * ^			    0 | 4 | 6 | 5 | < Write the input data sequentially
+ * Read first column
+ * Start at the bottom
+ * Move to next column and repeat
+ *
+ * Loop over k decreasing (blocks)
+ * in_block_ptr = src + (((RGB_HEIGHT_PIXELS / BLOCK_SIZE_PIXELS) - k)
+ *                * BLOCK_SIZE_PIXELS) * (RGB_WIDTH_BYTES)
+ * out_block_ptr = dst + (((RGB_HEIGHT_PIXELS / BLOCK_SIZE_PIXELS) - k)
+ *                * BLOCK_SIZE_BYTES) + (RGB_WIDTH_PIXELS - 1)
+ *                * RGB_HEIGHT_PIXELS * BYTES_PER_PIXEL
+ *
+ * Loop over i decreasing (width)
+ * Each pix:
+ * in_block_ptr += RGB_WIDTH_BYTES
+ * out_block_ptr += 4
+ *
+ * Each row of block:
+ * in_block_ptr -= RGB_WIDTH_BYTES * BLOCK_SIZE_PIXELS - 2
+ * out_block_ptr -= RGB_HEIGHT_PIXELS * BYTES_PER_PIXEL + 2 * BLOCK_SIZE_PIXELS;
+ *
+ * It may perform vertical mirroring too depending on the vmirror flag.
+ */
+static int opl_rotate270_u16_by4(const u8 * src, int src_line_stride, int width,
+				 int height, u8 * dst, int dst_line_stride,
+				 int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL / 4;
+	const int IN_INDEX = src_line_stride * BLOCK_SIZE_PIXELS
+	    - BYTES_PER_PIXEL;
+	const int OUT_INDEX = vmirror ?
+	    -dst_line_stride + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS
+	    : dst_line_stride + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS;
+	const u8 *in_block_ptr;
+	u8 *out_block_ptr;
+	int i, k;
+
+	for (k = height / BLOCK_SIZE_PIXELS; k > 0; k--) {
+		in_block_ptr = src + (((height / BLOCK_SIZE_PIXELS) - k)
+				      * BLOCK_SIZE_PIXELS) * src_line_stride;
+		out_block_ptr = dst + (((height / BLOCK_SIZE_PIXELS) - k)
+				       * BLOCK_SIZE_PIXELS * BYTES_PER_PIXEL)
+		    + (width - 1) * dst_line_stride;
+
+		/*
+		* For vertical mirroring the writing starts from the
+		* first line
+		*/
+		if (vmirror)
+			out_block_ptr -= dst_line_stride * (width - 1);
+
+		for (i = width; i > 0; i--) {
+			__asm__ volatile (
+				"ldrh	r2, [%0], %4\n\t"
+				"ldrh	r3, [%0], %4\n\t"
+				"ldrh	r4, [%0], %4\n\t"
+				"ldrh	r5, [%0], %4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				:"+r" (in_block_ptr), "+r"(out_block_ptr)	/* output */
+				:"0"(in_block_ptr), "1"(out_block_ptr), "r"(src_line_stride)	/* input  */
+				:"r2", "r3", "r4", "r5", "memory"	/* modify */
+			);
+			in_block_ptr -= IN_INDEX;
+			out_block_ptr -= OUT_INDEX;
+		}
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+EXPORT_SYMBOL(opl_rotate270_u16);
+EXPORT_SYMBOL(opl_rotate270_vmirror_u16);
diff --git a/drivers/media/video/mxc/opl/rotate270_u16_qcif.S b/drivers/media/video/mxc/opl/rotate270_u16_qcif.S
new file mode 100644
index 000000000000..4101eaac4554
--- /dev/null
+++ b/drivers/media/video/mxc/opl/rotate270_u16_qcif.S
@@ -0,0 +1,70 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/linkage.h>
+
+	.text
+	.align	2
+ENTRY(opl_rotate270_u16_qcif)
+	STMFD    sp!,{r4-r10}
+        MOV      r12,#0x160
+        MOV      r10,#0x90
+        MOV      r3,r10,LSR #4
+.L1.16:
+        RSB      r2,r3,r10,LSR #4
+        MOV      r5,r2,LSL #5
+        MOV      r4,r12,LSR #1
+        SMULBB   r4,r5,r4
+        ADD      r2,r1,r2,LSL #5
+        ADD      r5,r2,#0xc000
+        ADD      r5,r5,#0x4e0
+        MOV      r2,r12,LSR #1
+        ADD      r4,r0,r4
+.L1.52:
+        LDRH     r6,[r4],r12
+        LDRH     r7,[r4],r12
+        LDRH     r8,[r4],r12
+        LDRH     r9,[r4],r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        SUBS     r2,r2,#1
+        LDRH     r6,[r4],r12
+        LDRH     r7,[r4],r12
+        LDRH     r8,[r4],r12
+        LDRH     r9,[r4],r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        LDRH     r6,[r4],r12
+        LDRH     r7,[r4],r12
+        LDRH     r8,[r4],r12
+        LDRH     r9,[r4],r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        LDRH     r6,[r4],r12
+        LDRH     r7,[r4],r12
+        LDRH     r8,[r4],r12
+        LDRH     r9,[r4],r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        SUB      r4,r4,#0x1500
+        STMIA    r5,{r6,r7}
+        SUB      r5,r5,#0x138
+        SUB      r4,r4,#0xfe
+        BGT      .L1.52
+        SUBS     r3,r3,#1
+        BGT      .L1.16
+        LDMFD    sp!,{r4-r10}
+        BX       lr
+	.size	opl_rotate270_u16_qcif, . - opl_rotate270_u16_qcif
diff --git a/drivers/media/video/mxc/opl/rotate90_u16.c b/drivers/media/video/mxc/opl/rotate90_u16.c
new file mode 100644
index 000000000000..dd7d445aa952
--- /dev/null
+++ b/drivers/media/video/mxc/opl/rotate90_u16.c
@@ -0,0 +1,220 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include "opl.h"
+
+static int opl_rotate90_u16_by16(const u8 * src, int src_line_stride, int width,
+				 int height, u8 * dst, int dst_line_stride,
+				 int vmirror);
+static int opl_rotate90_u16_by4(const u8 * src, int src_line_stride, int width,
+				int height, u8 * dst, int dst_line_stride,
+				int vmirror);
+static int opl_rotate90_vmirror_u16_both(const u8 * src, int src_line_stride,
+					 int width, int height, u8 * dst,
+					 int dst_line_stride, int vmirror);
+int opl_rotate90_u16_qcif(const u8 * src, u8 * dst);
+
+int opl_rotate90_u16(const u8 * src, int src_line_stride, int width, int height,
+		     u8 * dst, int dst_line_stride)
+{
+	return opl_rotate90_vmirror_u16_both(src, src_line_stride, width,
+					    height, dst, dst_line_stride, 0);
+}
+
+int opl_rotate90_vmirror_u16(const u8 * src, int src_line_stride, int width,
+			    int height, u8 * dst, int dst_line_stride)
+{
+	return opl_rotate90_vmirror_u16_both(src, src_line_stride, width,
+					    height, dst, dst_line_stride, 1);
+}
+
+static int opl_rotate90_vmirror_u16_both(const u8 * src, int src_line_stride,
+					 int width, int height, u8 * dst,
+					 int dst_line_stride, int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL;
+	const int BLOCK_SIZE_PIXELS_BY4 = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL / 4;
+
+	if (!src || !dst)
+		return OPLERR_NULL_PTR;
+
+	if (width == 0 || height == 0 || src_line_stride == 0
+	    || dst_line_stride == 0)
+		return OPLERR_BAD_ARG;
+
+	/* The QCIF algorithm doesn't support vertical mirroring */
+	if (vmirror == 0 && width == QCIF_Y_WIDTH && height == QCIF_Y_HEIGHT
+	    && src_line_stride == QCIF_Y_WIDTH * 2
+	    && src_line_stride == QCIF_Y_HEIGHT * 2)
+		return opl_rotate90_u16_qcif(src, dst);
+	else if (width % BLOCK_SIZE_PIXELS == 0
+		 && height % BLOCK_SIZE_PIXELS == 0)
+		return opl_rotate90_u16_by16(src, src_line_stride, width,
+					     height, dst, dst_line_stride,
+					     vmirror);
+	else if (width % BLOCK_SIZE_PIXELS_BY4 == 0
+		 && height % BLOCK_SIZE_PIXELS_BY4 == 0)
+		return opl_rotate90_u16_by4(src, src_line_stride, width, height,
+					    dst, dst_line_stride, vmirror);
+	else
+		return OPLERR_BAD_ARG;
+}
+
+/*
+ * Performs clockwise rotation (and possibly vertical mirroring depending
+ * on the vmirror flag) using block sizes of 16x16
+ * The algorithm is similar to 270 degree clockwise rotation algorithm
+ */
+static int opl_rotate90_u16_by16(const u8 * src, int src_line_stride, int width,
+				 int height, u8 * dst, int dst_line_stride,
+				 int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL;
+	const int BLOCK_SIZE_BYTES = BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS;
+	const int IN_INDEX = src_line_stride * BLOCK_SIZE_PIXELS
+	    + BYTES_PER_PIXEL;
+	const int OUT_INDEX = vmirror ?
+	    -dst_line_stride - BLOCK_SIZE_BYTES
+	    : dst_line_stride - BLOCK_SIZE_BYTES;
+	const u8 *in_block_ptr;
+	u8 *out_block_ptr;
+	int i, k;
+
+	for (k = height / BLOCK_SIZE_PIXELS; k > 0; k--) {
+		in_block_ptr = src + src_line_stride * (height - 1)
+		    - (src_line_stride * BLOCK_SIZE_PIXELS *
+		       (height / BLOCK_SIZE_PIXELS - k));
+		out_block_ptr = dst + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS *
+		    ((height / BLOCK_SIZE_PIXELS) - k);
+
+		/*
+		 * For vertical mirroring the writing starts from the
+		 * bottom line
+		 */
+		if (vmirror)
+			out_block_ptr += dst_line_stride * (width - 1);
+
+		for (i = width; i > 0; i--) {
+			__asm__ volatile (
+				"ldrh	r2, [%0], -%4\n\t"
+				"ldrh	r3, [%0], -%4\n\t"
+				"ldrh	r4, [%0], -%4\n\t"
+				"ldrh	r5, [%0], -%4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], -%4\n\t"
+				"ldrh	r3, [%0], -%4\n\t"
+				"ldrh	r4, [%0], -%4\n\t"
+				"ldrh	r5, [%0], -%4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], -%4\n\t"
+				"ldrh	r3, [%0], -%4\n\t"
+				"ldrh	r4, [%0], -%4\n\t"
+				"ldrh	r5, [%0], -%4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], -%4\n\t"
+				"ldrh	r3, [%0], -%4\n\t"
+				"ldrh	r4, [%0], -%4\n\t"
+				"ldrh	r5, [%0], -%4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				:"+r" (in_block_ptr), "+r"(out_block_ptr)	/* output */
+				:"0"(in_block_ptr), "1"(out_block_ptr), "r"(src_line_stride)	/* input  */
+				:"r2", "r3", "r4", "r5", "memory"	/* modify */
+			);
+			in_block_ptr += IN_INDEX;
+			out_block_ptr += OUT_INDEX;
+		}
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+/*
+ * Performs clockwise rotation (and possibly vertical mirroring depending
+ * on the vmirror flag) using block sizes of 4x4
+ * The algorithm is similar to 270 degree clockwise rotation algorithm
+ */
+static int opl_rotate90_u16_by4(const u8 * src, int src_line_stride, int width,
+				int height, u8 * dst, int dst_line_stride,
+				int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL / 4;
+	const int BLOCK_SIZE_BYTES = BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS;
+	const int IN_INDEX = src_line_stride * BLOCK_SIZE_PIXELS
+	    + BYTES_PER_PIXEL;
+	const int OUT_INDEX = vmirror ?
+	    -dst_line_stride - BLOCK_SIZE_BYTES
+	    : dst_line_stride - BLOCK_SIZE_BYTES;
+	const u8 *in_block_ptr;
+	u8 *out_block_ptr;
+	int i, k;
+
+	for (k = height / BLOCK_SIZE_PIXELS; k > 0; k--) {
+		in_block_ptr = src + src_line_stride * (height - 1)
+		    - (src_line_stride * BLOCK_SIZE_PIXELS *
+		       (height / BLOCK_SIZE_PIXELS - k));
+		out_block_ptr = dst + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS
+		    * ((height / BLOCK_SIZE_PIXELS) - k);
+
+		/*
+		 * For horizontal mirroring the writing starts from the
+		 * bottom line
+		 */
+		if (vmirror)
+			out_block_ptr += dst_line_stride * (width - 1);
+
+		for (i = width; i > 0; i--) {
+			__asm__ volatile (
+				"ldrh	r2, [%0], -%4\n\t"
+				"ldrh	r3, [%0], -%4\n\t"
+				"ldrh	r4, [%0], -%4\n\t"
+				"ldrh	r5, [%0], -%4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				:"+r" (in_block_ptr), "+r"(out_block_ptr)	/* output */
+				:"0"(in_block_ptr), "1"(out_block_ptr), "r"(src_line_stride)	/* input  */
+				:"r2", "r3", "r4", "r5", "memory"	/* modify */
+			);
+			in_block_ptr += IN_INDEX;
+			out_block_ptr += OUT_INDEX;
+		}
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+EXPORT_SYMBOL(opl_rotate90_u16);
+EXPORT_SYMBOL(opl_rotate90_vmirror_u16);
diff --git a/drivers/media/video/mxc/opl/rotate90_u16_qcif.S b/drivers/media/video/mxc/opl/rotate90_u16_qcif.S
new file mode 100644
index 000000000000..8568a9e629e5
--- /dev/null
+++ b/drivers/media/video/mxc/opl/rotate90_u16_qcif.S
@@ -0,0 +1,71 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/linkage.h>
+
+	.text
+	.align	2
+ENTRY(opl_rotate90_u16_qcif)
+        STMFD    sp!,{r4-r10}
+        MOV      r12,#0x160
+        MOV      r10,#0x90
+        MOV      r3,r10,LSR #4
+.L1.216:
+        RSB      r2,r3,r10,LSR #4
+        MOV      r4,#0x20
+        SMULBB   r5,r4,r2
+        MOV      r4,#0x1600
+        SMULBB   r2,r4,r2
+        ADD      r4,r0,#0xc000
+        ADD      r4,r4,#0x4a0
+        SUB      r4,r4,r2
+        MOV      r2,r12,LSR #1
+        ADD      r5,r1,r5
+.L1.256:
+        LDRH     r6,[r4],-r12
+        LDRH     r7,[r4],-r12
+        LDRH     r8,[r4],-r12
+        LDRH     r9,[r4],-r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        SUBS     r2,r2,#1
+        LDRH     r6,[r4],-r12
+        LDRH     r7,[r4],-r12
+        LDRH     r8,[r4],-r12
+        LDRH     r9,[r4],-r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        LDRH     r6,[r4],-r12
+        LDRH     r7,[r4],-r12
+        LDRH     r8,[r4],-r12
+        LDRH     r9,[r4],-r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        LDRH     r6,[r4],-r12
+        LDRH     r7,[r4],-r12
+        LDRH     r8,[r4],-r12
+        LDRH     r9,[r4],-r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        ADD      r4,r4,#0x1600
+        STMIA    r5!,{r6,r7}
+        ADD      r5,r5,#0x100
+        ADD      r4,r4,#2
+        BGT      .L1.256
+        SUBS     r3,r3,#1
+        BGT      .L1.216
+        LDMFD    sp!,{r4-r10}
+        BX       lr
+	.size	opl_rotate90_u16_qcif, . - opl_rotate90_u16_qcif
diff --git a/drivers/media/video/mxc/opl/vmirror_u16.c b/drivers/media/video/mxc/opl/vmirror_u16.c
new file mode 100644
index 000000000000..57f805c08a81
--- /dev/null
+++ b/drivers/media/video/mxc/opl/vmirror_u16.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/string.h>
+#include "opl.h"
+
+int opl_vmirror_u16(const u8 * src, int src_line_stride, int width, int height,
+		    u8 * dst, int dst_line_stride)
+{
+	const u8 *src_row_addr;
+	u8 *dst_row_addr;
+	int i;
+
+	if (!src || !dst)
+		return OPLERR_NULL_PTR;
+
+	if (width == 0 || height == 0 || src_line_stride == 0
+	    || dst_line_stride == 0)
+		return OPLERR_BAD_ARG;
+
+	src_row_addr = src;
+	dst_row_addr = dst + (height - 1) * dst_line_stride;
+
+	/* Loop over all rows */
+	for (i = 0; i < height; i++) {
+		/* memcpy each row */
+		memcpy(dst_row_addr, src_row_addr, BYTES_PER_PIXEL * width);
+		src_row_addr += src_line_stride;
+		dst_row_addr -= dst_line_stride;
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+EXPORT_SYMBOL(opl_vmirror_u16);
diff --git a/drivers/media/video/mxc/output/Kconfig b/drivers/media/video/mxc/output/Kconfig
new file mode 100644
index 000000000000..2153ad248907
--- /dev/null
+++ b/drivers/media/video/mxc/output/Kconfig
@@ -0,0 +1,28 @@
+config VIDEO_MXC_IPU_OUTPUT
+	bool "IPU v4l2 support"
+	depends on VIDEO_MXC_OUTPUT && MXC_IPU
+	default y
+	---help---
+	  This is the video4linux2 driver for IPU post processing video output.
+
+config VIDEO_MXC_IPUV1_WVGA_OUTPUT
+	bool "IPUv1 WVGA v4l2 display support"
+	depends on VIDEO_MXC_OUTPUT && MXC_IPU
+	default n
+	---help---
+	  This is the video4linux2 driver for IPUv1 WVGA post processing video output.
+
+config VIDEO_MXC_EMMA_OUTPUT
+	bool
+	depends on VIDEO_MXC_OUTPUT && MXC_EMMA && FB_MXC_SYNC_PANEL
+	default y
+	---help---
+	  This is the video4linux2 driver for EMMA post processing video output.
+
+config VIDEO_MXC_OUTPUT_FBSYNC
+	bool "Synchronize the output with LCDC refresh"
+	depends on VIDEO_MXC_EMMA_OUTPUT
+	default y
+	---help---
+	  Synchronize the post-processing with LCDC EOF (End of Frame) to
+	  prevent tearing issue. If unsure, say Y.
diff --git a/drivers/media/video/mxc/output/Makefile b/drivers/media/video/mxc/output/Makefile
new file mode 100644
index 000000000000..1713fa3bf3ab
--- /dev/null
+++ b/drivers/media/video/mxc/output/Makefile
@@ -0,0 +1,11 @@
+ifeq ($(CONFIG_VIDEO_MXC_EMMA_OUTPUT),y)
+	mx27_output-objs := mx27_v4l2_output.o mx27_pp.o
+	obj-$(CONFIG_VIDEO_MXC_OUTPUT) += mx27_output.o
+endif
+
+ifeq ($(CONFIG_VIDEO_MXC_IPU_OUTPUT),y)
+	obj-$(CONFIG_VIDEO_MXC_OUTPUT)	+= mxc_v4l2_output.o
+endif
+ifeq ($(CONFIG_VIDEO_MXC_IPUV1_WVGA_OUTPUT),y)
+	obj-$(CONFIG_VIDEO_MXC_OUTPUT)	+= mx31_v4l2_wvga_output.o
+endif
diff --git a/drivers/media/video/mxc/output/mx27_pp.c b/drivers/media/video/mxc/output/mx27_pp.c
new file mode 100644
index 000000000000..a82328015fe2
--- /dev/null
+++ b/drivers/media/video/mxc/output/mx27_pp.c
@@ -0,0 +1,904 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_pp.c
+ *
+ * @brief MX27 V4L2 Video Output Driver
+ *
+ * Video4Linux2 Output Device using MX27 eMMA Post-processing functionality.
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/module.h>
+#include <linux/fb.h>
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <asm/io.h>
+
+#include "mx27_pp.h"
+#include "mxc_v4l2_output.h"
+
+#define SCALE_RETRY	32	/* to be more relax, less precise */
+#define PP_SKIP		1
+#define PP_TBL_MAX	40
+
+static unsigned short scale_tbl[PP_TBL_MAX];
+static int g_hlen, g_vlen;
+
+static emma_pp_cfg g_pp_cfg;
+static int g_disp_num = 0;
+static char pp_dev[] = "emma_pp";
+
+/*!
+ * @brief PP resizing routines
+ */
+static int gcd(int x, int y);
+static int ratio(int x, int y, int *den);
+static int scale_0d(int k, int coeff, int base, int nxt);
+static int scale_1d(int inv, int outv, int k);
+static int scale_1d_smart(int *inv, int *outv, int index);
+static int scale_2d(emma_pp_scale * sz);
+
+static irqreturn_t pp_isr(int irq, void *dev_id);
+static int set_output_addr(emma_pp_cfg * cfg, vout_data * vout);
+static int pphw_reset(void);
+static int pphw_enable(int flag);
+static int pphw_ptr(emma_pp_cfg * cfg);
+static int pphw_outptr(emma_pp_cfg * cfg);
+static int pphw_cfg(emma_pp_cfg * cfg);
+static int pphw_isr(void);
+static void pphw_init(void);
+static void pphw_exit(void);
+
+#define PP_DUMP(reg)	pr_debug("%s\t = 0x%08X\n", #reg, __raw_readl(reg))
+void pp_dump(void)
+{
+	PP_DUMP(PP_CNTL);
+	PP_DUMP(PP_INTRCNTL);
+	PP_DUMP(PP_INTRSTATUS);
+	PP_DUMP(PP_SOURCE_Y_PTR);
+	PP_DUMP(PP_SOURCE_CB_PTR);
+	PP_DUMP(PP_SOURCE_CR_PTR);
+	PP_DUMP(PP_DEST_RGB_PTR);
+	PP_DUMP(PP_QUANTIZER_PTR);
+	PP_DUMP(PP_PROCESS_FRAME_PARA);
+	PP_DUMP(PP_SOURCE_FRAME_WIDTH);
+	PP_DUMP(PP_DEST_DISPLAY_WIDTH);
+	PP_DUMP(PP_DEST_IMAGE_SIZE);
+	PP_DUMP(PP_DEST_FRAME_FMT_CNTL);
+	PP_DUMP(PP_RESIZE_INDEX);
+	PP_DUMP(PP_CSC_COEF_0123);
+	PP_DUMP(PP_CSC_COEF_4);
+}
+
+/*!
+ * @brief Set PP input address.
+ * @param ptr	The pointer to the Y value of input
+ * @return	Zero on success, others on failure
+ */
+int pp_ptr(unsigned long ptr)
+{
+	g_pp_cfg.ptr.y = ptr;
+	g_pp_cfg.ptr.u = g_pp_cfg.ptr.v = g_pp_cfg.ptr.qp = 0;
+
+	return pphw_ptr(&g_pp_cfg);
+}
+
+/*!
+ * @brief Enable or disable PP.
+ * @param flag	Zero to disable PP, others to enable PP
+ * @return	Zero on success, others on failure
+ */
+int pp_enable(int flag)
+{
+	return pphw_enable(flag);
+}
+
+/*!
+ * @brief Get the display No. of last completed PP frame.
+ * @return	The display No. of last completed PP frame.
+ */
+int pp_num_last(void)
+{
+	return (g_disp_num ? 0 : 1);
+}
+
+/*!
+ * @brief Initialize PP.
+ * @param vout	Pointer to _vout_data structure
+ * @return	Zero on success, others on failure
+ */
+int pp_init(vout_data * vout)
+{
+	pphw_init();
+	pphw_enable(0);
+	enable_irq(MXC_INT_EMMAPP);
+	return request_irq(MXC_INT_EMMAPP, pp_isr, 0, pp_dev, vout);
+}
+
+/*!
+ * @brief Deinitialize PP.
+ * @param vout	Pointer to _vout_data structure
+ */
+void pp_exit(vout_data * vout)
+{
+	disable_irq(MXC_INT_EMMAPP);
+	free_irq(MXC_INT_EMMAPP, vout);
+	pphw_enable(0);
+	pphw_exit();
+}
+
+/*!
+ * @brief Configure PP.
+ * @param vout	Pointer to _vout_data structure
+ * @return	Zero on success, others on failure
+ */
+int pp_cfg(vout_data * vout)
+{
+	if (!vout)
+		return -1;
+
+	/* PP accepts YUV420 input only */
+	if (vout->v2f.fmt.pix.pixelformat != V4L2_PIX_FMT_YUV420) {
+		pr_debug("unsupported pixel format.\n");
+		return -1;
+	}
+
+	g_pp_cfg.operation = 0;
+
+	memset(g_pp_cfg.csc_table, 0, sizeof(g_pp_cfg.csc_table));
+
+	/* Convert output pixel format to PP required format */
+	switch (vout->v4l2_fb.fmt.pixelformat) {
+	case V4L2_PIX_FMT_BGR32:
+		g_pp_cfg.red_width = 8;
+		g_pp_cfg.green_width = 8;
+		g_pp_cfg.blue_width = 8;
+		g_pp_cfg.red_offset = 8;
+		g_pp_cfg.green_offset = 16;
+		g_pp_cfg.blue_offset = 24;
+		g_pp_cfg.rgb_resolution = 32;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+		g_pp_cfg.red_width = 8;
+		g_pp_cfg.green_width = 8;
+		g_pp_cfg.blue_width = 8;
+		g_pp_cfg.red_offset = 24;
+		g_pp_cfg.green_offset = 16;
+		g_pp_cfg.blue_offset = 8;
+		g_pp_cfg.rgb_resolution = 32;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		g_pp_cfg.red_width = 0;
+		g_pp_cfg.green_width = 0;
+		g_pp_cfg.blue_width = 0;
+		g_pp_cfg.red_offset = 0;
+		g_pp_cfg.green_offset = 0;
+		g_pp_cfg.blue_offset = PP_PIX_YUYV;
+		g_pp_cfg.rgb_resolution = 16;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		g_pp_cfg.red_width = 0;
+		g_pp_cfg.green_width = 0;
+		g_pp_cfg.blue_width = 0;
+		g_pp_cfg.red_offset = 0;
+		g_pp_cfg.green_offset = 0;
+		g_pp_cfg.blue_offset = PP_PIX_UYVY;
+		g_pp_cfg.rgb_resolution = 16;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+	default:
+		g_pp_cfg.red_width = 5;
+		g_pp_cfg.green_width = 6;
+		g_pp_cfg.blue_width = 5;
+		g_pp_cfg.red_offset = 11;
+		g_pp_cfg.green_offset = 5;
+		g_pp_cfg.blue_offset = 0;
+		g_pp_cfg.rgb_resolution = 16;
+		break;
+	}
+
+	if (vout->ipu_buf[0] != -1)
+		g_pp_cfg.ptr.y =
+		    (unsigned int)vout->queue_buf_paddr[vout->ipu_buf[0]];
+	else
+		g_pp_cfg.ptr.y = 0;
+
+	g_pp_cfg.ptr.u = g_pp_cfg.ptr.v = g_pp_cfg.ptr.qp = 0;
+
+	g_pp_cfg.dim.in.width = vout->v2f.fmt.pix.width;
+	g_pp_cfg.dim.in.height = vout->v2f.fmt.pix.height;
+	g_pp_cfg.dim.out.width = vout->crop_current.width;
+	g_pp_cfg.dim.out.height = vout->crop_current.height;
+	g_pp_cfg.dim.num.width = 0;
+	g_pp_cfg.dim.num.height = 0;
+	g_pp_cfg.dim.den.width = 0;
+	g_pp_cfg.dim.den.height = 0;
+
+	if (scale_2d(&g_pp_cfg.dim)) {
+		pr_debug("unsupported resize ratio.\n");
+		return -1;
+	}
+
+	g_pp_cfg.dim.out.width = vout->crop_current.width;
+	g_pp_cfg.dim.out.height = vout->crop_current.height;
+
+	g_pp_cfg.in_y_stride = 0;
+	if (set_output_addr(&g_pp_cfg, vout)) {
+		pr_debug("failed to set pp output address.\n");
+		return -1;
+	}
+
+	return pphw_cfg(&g_pp_cfg);
+}
+
+irqreturn_t mxc_v4l2out_pp_in_irq_handler(int irq, void *dev_id);
+
+/*!
+ * @brief PP IRQ handler.
+ */
+static irqreturn_t pp_isr(int irq, void *dev_id)
+{
+	int status;
+	vout_data *vout = dev_id;
+
+	status = pphw_isr();
+	if ((status & 0x1) == 0) {	/* Not frame complete interrupt */
+		pr_debug("not pp frame complete interrupt\n");
+		return IRQ_HANDLED;
+	}
+
+	if (vout->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		g_disp_num = g_disp_num ? 0 : 1;
+		g_pp_cfg.outptr = (unsigned int)vout->display_bufs[g_disp_num];
+		pphw_outptr(&g_pp_cfg);
+	}
+
+	return mxc_v4l2out_pp_in_irq_handler(irq, dev_id);
+}
+
+/*!
+ * @brief Set PP output address.
+ * @param cfg	Pointer to emma_pp_cfg structure
+ * @param vout	Pointer to _vout_data structure
+ * @return	Zero on success, others on failure
+ */
+static int set_output_addr(emma_pp_cfg * cfg, vout_data * vout)
+{
+	if (vout->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		g_disp_num = 0;
+		cfg->outptr = (unsigned int)vout->display_bufs[g_disp_num];
+		cfg->out_stride = vout->crop_current.width;
+		return 0;
+	} else {
+		struct fb_info *fb;
+
+		fb = registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+		if (!fb)
+			return -1;
+
+		cfg->outptr = fb->fix.smem_start;
+		cfg->outptr += vout->crop_current.top * fb->var.xres_virtual
+		    * (fb->var.bits_per_pixel >> 3)
+		    + vout->crop_current.left * (fb->var.bits_per_pixel >> 3);
+		cfg->out_stride = fb->var.xres_virtual;
+
+		return 0;
+	}
+}
+
+/*!
+ * @brief Get maximum common divisor.
+ * @param x	First input value
+ * @param y	Second input value
+ * @return	Maximum common divisor of x and y
+ */
+static int gcd(int x, int y)
+{
+	int k;
+
+	if (x < y) {
+		k = x;
+		x = y;
+		y = k;
+	}
+
+	while ((k = x % y)) {
+		x = y;
+		y = k;
+	}
+
+	return y;
+}
+
+/*!
+ * @brief Get ratio.
+ * @param x	First input value
+ * @param y	Second input value
+ * @param den	Denominator of the ratio (corresponding to y)
+ * @return	Numerator of the ratio (corresponding to x)
+ */
+static int ratio(int x, int y, int *den)
+{
+	int g;
+
+	if (!x || !y)
+		return 0;
+
+	g = gcd(x, y);
+	*den = y / g;
+
+	return x / g;
+}
+
+/*!
+ * @brief Build PP coefficient entry
+ * Build one or more coefficient entries for PP coefficient table based
+ * on given coefficient.
+ *
+ * @param k	The index of the coefficient in coefficient table
+ * @param coeff	The weighting coefficient
+ * @param base	The base of the coefficient
+ * @param nxt	Number of pixels to be read
+ *
+ * @return	The index of the next coefficient entry on success
+ *		-1 on failure
+ */
+static int scale_0d(int k, int coeff, int base, int nxt)
+{
+	if (k >= PP_TBL_MAX) {
+		/* no more space in table */
+		pr_debug("no space in scale table, k = %d\n", k);
+		return -1;
+	}
+
+	coeff = ((coeff << BC_COEF) + (base >> 1)) / base;
+
+	/*
+	 * Valid values for weighting coefficient are 0, 2 to 30, and 31.
+	 * A value of 31 is treated as 32 and therefore 31 is an
+	 * invalid co-efficient.
+	 */
+	if (coeff >= SZ_COEF - 1)
+		coeff--;
+	else if (coeff == 1)
+		coeff++;
+	coeff = coeff << BC_NXT;
+
+	if (nxt < SZ_NXT) {
+		coeff |= nxt;
+		coeff <<= 1;
+		coeff |= 1;
+	} else {
+		/*
+		 * src inc field is 2 bit wide, for 4+, use special
+		 * code 0:0:1 to prevent dest inc
+		 */
+		coeff |= PP_SKIP;
+		coeff <<= 1;
+		coeff |= 1;
+		nxt -= PP_SKIP;
+		do {
+			pr_debug("tbl = %03X\n", coeff);
+			scale_tbl[k++] = coeff;
+			coeff = (nxt > PP_SKIP) ? PP_SKIP : nxt;
+			coeff <<= 1;
+		} while ((nxt -= PP_SKIP) > 0);
+	}
+	pr_debug("tbl = %03X\n", coeff);
+	scale_tbl[k++] = coeff;
+
+	return k;
+}
+
+/*
+ * @brief Build PP coefficient table
+ * Build PP coefficient table for one dimension (width or height)
+ * based on given input and output resolution
+ *
+ * @param inv	input resolution
+ * @param outv	output resolution
+ * @param k	index of free table entry
+ *
+ * @return	The index of the next free coefficient entry on success
+ *		-1 on failure
+ */
+static int scale_1d(int inv, int outv, int k)
+{
+	int v;			/* overflow counter */
+	int coeff, nxt;		/* table output */
+
+	if (inv == outv)
+		return scale_0d(k, 1, 1, 1);	/* force scaling */
+
+	if (inv * 4 < outv) {
+		pr_debug("upscale err: ratio should be in range 1:1 to 1:4\n");
+		return -1;
+	}
+
+	v = 0;
+	if (inv < outv) {
+		/* upscale: mix <= 2 input pixels per output pixel */
+		do {
+			coeff = outv - v;
+			v += inv;
+			if (v >= outv) {
+				v -= outv;
+				nxt = 1;
+			} else
+				nxt = 0;
+			pr_debug("upscale: coeff = %d/%d nxt = %d\n", coeff,
+				 outv, nxt);
+			k = scale_0d(k, coeff, outv, nxt);
+			if (k < 0)
+				return -1;
+		} while (v);
+	} else if (inv >= 2 * outv) {
+		/* PP doesn't support resize ratio > 2:1 except 4:1. */
+		if ((inv != 2 * outv) && (inv != 4 * outv))
+			return -1;
+		/* downscale: >=2:1 bilinear approximation */
+		coeff = inv - 2 * outv;
+		v = 0;
+		nxt = 0;
+		do {
+			v += coeff;
+			nxt = 2;
+			while (v >= outv) {
+				v -= outv;
+				nxt++;
+			}
+			pr_debug("downscale: coeff = 1/2 nxt = %d\n", nxt);
+			k = scale_0d(k, 1, 2, nxt);
+			if (k < 0)
+				return -1;
+		} while (v);
+	} else {
+		/* downscale: bilinear */
+		int in_pos_inc = 2 * outv;
+		int out_pos = inv;
+		int out_pos_inc = 2 * inv;
+		int init_carry = inv - outv;
+		int carry = init_carry;
+
+		v = outv + in_pos_inc;
+		do {
+			coeff = v - out_pos;
+			out_pos += out_pos_inc;
+			carry += out_pos_inc;
+			for (nxt = 0; v < out_pos; nxt++) {
+				v += in_pos_inc;
+				carry -= in_pos_inc;
+			}
+			pr_debug("downscale: coeff = %d/%d nxt = %d\n", coeff,
+				 in_pos_inc, nxt);
+			k = scale_0d(k, coeff, in_pos_inc, nxt);
+			if (k < 0)
+				return -1;
+		} while (carry != init_carry);
+	}
+	return k;
+}
+
+/*
+ * @brief Build PP coefficient table
+ * Build PP coefficient table for one dimension (width or height)
+ * based on given input and output resolution. The given input
+ * and output resolution might be not supported due to hardware
+ * limits. In this case this functin rounds the input and output
+ * to closest possible values and return them to caller.
+ *
+ * @param inv	input resolution, might be modified after the call
+ * @param outv	output resolution, might be modified after the call
+ * @param k	index of free table entry
+ *
+ * @return	The index of the next free coefficient entry on success
+ *		-1 on failure
+ */
+static int scale_1d_smart(int *inv, int *outv, int index)
+{
+	int len, num, den, retry;
+	static int num1, den1;
+
+	if (!inv || !outv)
+		return -1;
+
+	/* Both should be non-zero */
+	if (!(*inv) || !(*outv))
+		return -1;
+
+	retry = SCALE_RETRY;
+
+	do {
+		num = ratio(*inv, *outv, &den);
+		pr_debug("num = %d, den = %d\n", num, den);
+		if (!num)
+			continue;
+
+		if (index != 0) {
+			/*
+			 * We are now resizing height. Check to see if the
+			 * resize ratio for width can be reused by height
+			 */
+			if ((num == num1) && (den == den1))
+				return index;
+		}
+
+		if ((len = scale_1d(num, den, index)) < 0)
+			/* increase output dimension to try another ratio */
+			(*outv)++;
+		else {
+			if (index == 0) {
+				/*
+				 * We are now resizing width. The same resize
+				 * ratio may be reused by height, so save the
+				 * ratio.
+				 */
+				num1 = num;
+				den1 = den;
+			}
+			return len;
+		}
+	} while (retry--);
+
+	pr_debug("pp scale err\n");
+	return -1;
+}
+
+/*
+ * @brief Build PP coefficient table for both width and height
+ * Build PP coefficient table for both width and height based on
+ * given resizing ratios.
+ *
+ * @param sz	Structure contains resizing ratio informations
+ *
+ * @return	0 on success, others on failure
+ */
+static int scale_2d(emma_pp_scale * sz)
+{
+	int inv, outv;
+
+	/* horizontal resizing. parameter check - must provide in size */
+	if (!sz->in.width)
+		return -1;
+
+	/* Resizing based on num:den */
+	inv = sz->num.width;
+	outv = sz->den.width;
+
+	if ((g_hlen = scale_1d_smart(&inv, &outv, 0)) > 0) {
+		/* Resizing succeeded */
+		sz->den.width = outv;
+		sz->out.width = (sz->in.width * outv) / inv;
+	} else {
+		/* Resizing based on in:out */
+		inv = sz->in.width;
+		outv = sz->out.width;
+
+		if ((g_hlen = scale_1d_smart(&inv, &outv, 0)) > 0) {
+			/* Resizing succeeded */
+			sz->out.width = outv;
+			sz->num.width = ratio(sz->in.width, sz->out.width,
+					      &sz->den.width);
+		} else
+			return -1;
+	}
+
+	sz->out.width &= ~1;
+
+	/* vertical resizing. parameter check - must provide in size */
+	if (!sz->in.height)
+		return -1;
+
+	/* Resizing based on num:den */
+	inv = sz->num.height;
+	outv = sz->den.height;
+
+	if ((g_vlen = scale_1d_smart(&inv, &outv, g_hlen)) > 0) {
+		/* Resizing succeeded */
+		sz->den.height = outv;
+		sz->out.height = (sz->in.height * outv) / inv;
+	} else {
+		/* Resizing based on in:out */
+		inv = sz->in.height;
+		outv = sz->out.height;
+
+		if ((g_vlen = scale_1d_smart(&inv, &outv, g_hlen)) > 0) {
+			/* Resizing succeeded */
+			sz->out.height = outv;
+			sz->num.height = ratio(sz->in.height, sz->out.height,
+					       &sz->den.height);
+		} else
+			return -1;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Set PP resizing registers.
+ * @param sz	Pointer to pp scaling structure
+ * @return	Zero on success, others on failure
+ */
+static int pphw_scale(emma_pp_scale * sz)
+{
+	__raw_writel((sz->out.width << 16) | sz->out.height,
+		     PP_DEST_IMAGE_SIZE);
+	__raw_writel(((g_hlen - 1) << 16) | (g_vlen ==
+					     g_hlen ? 0 : (g_hlen << 8)) |
+		     (g_vlen - 1), PP_RESIZE_INDEX);
+	for (g_hlen = 0; g_hlen < g_vlen; g_hlen++)
+		__raw_writel(scale_tbl[g_hlen],
+			     PP_RESIZE_COEF_TBL + g_hlen * 4);
+
+	return 0;
+}
+
+/*!
+ * @brief Reset PP.
+ * @return	Zero on success, others on failure
+ */
+static int pphw_reset(void)
+{
+	int i;
+
+	__raw_writel(0x100, PP_CNTL);
+
+	/* timeout */
+	for (i = 0; i < 1000; i++) {
+		if (!(__raw_readl(PP_CNTL) & 0x100)) {
+			pr_debug("pp reset over\n");
+			break;
+		}
+	}
+
+	/* check reset value */
+	if (__raw_readl(PP_CNTL) != 0x876) {
+		pr_debug("pp reset value err = 0x%08X\n", __raw_readl(PP_CNTL));
+		return -1;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Enable or disable PP.
+ * @param flag	Zero to disable PP, others to enable PP
+ * @return	Zero on success, others on failure
+ */
+static int pphw_enable(int flag)
+{
+	int ret = 0;
+
+	if (flag)
+		__raw_writel(__raw_readl(PP_CNTL) | 1, PP_CNTL);
+	else
+		ret = pphw_reset();
+
+	return ret;
+}
+
+/*!
+ * @brief Set PP input address.
+ * @param cfg	The pointer to PP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+static int pphw_ptr(emma_pp_cfg * cfg)
+{
+	if (!cfg->ptr.u) {
+		int size;
+
+		/* yuv - packed */
+		size = PP_CALC_Y_SIZE(cfg);
+		cfg->ptr.u = cfg->ptr.y + size;
+		cfg->ptr.v = cfg->ptr.u + (size >> 2);
+
+		/* yuv packed with qp appended */
+		if (!cfg->ptr.qp)
+			cfg->ptr.qp = cfg->ptr.v + (size >> 2);
+	}
+	__raw_writel(cfg->ptr.y, PP_SOURCE_Y_PTR);
+	__raw_writel(cfg->ptr.u, PP_SOURCE_CB_PTR);
+	__raw_writel(cfg->ptr.v, PP_SOURCE_CR_PTR);
+	__raw_writel(cfg->ptr.qp, PP_QUANTIZER_PTR);
+
+	return 0;
+}
+
+/*!
+ * @brief Set PP output address.
+ * @param cfg	The pointer to PP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+static int pphw_outptr(emma_pp_cfg * cfg)
+{
+	__raw_writel(cfg->outptr, PP_DEST_RGB_PTR);
+	return 0;
+}
+
+/*!
+ * @brief Configuration PP.
+ * @param cfg	The pointer to PP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+static int pphw_cfg(emma_pp_cfg * cfg)
+{
+	int rt;
+	register int r;
+
+	pphw_scale(&cfg->dim);
+
+	if (!cfg->in_y_stride)
+		cfg->in_y_stride = cfg->dim.in.width;
+
+	if (!cfg->out_stride)
+		cfg->out_stride = cfg->dim.out.width;
+
+	r = __raw_readl(PP_CNTL) & ~EN_MASK;
+
+	/* config parms */
+	r |= cfg->operation & EN_MASK;
+	if (cfg->operation & EN_MACROBLOCK) {
+		/* Macroblock Mode */
+		r |= 0x0200;
+		__raw_writel(0x06, PP_INTRCNTL);
+	} else {
+		/* Frame mode */
+		__raw_writel(0x05, PP_INTRCNTL);
+	}
+
+	if (cfg->red_width | cfg->green_width | cfg->blue_width) {
+		/* color conversion to be performed */
+		r |= EN_CSC;
+		if (!(cfg->red_offset | cfg->green_offset)) {
+			/* auto offset B:G:R LSb to Msb */
+			cfg->green_offset = cfg->blue_offset + cfg->blue_width;
+			cfg->red_offset = cfg->green_offset + cfg->green_width;
+		}
+		if (!cfg->rgb_resolution) {
+			/* derive minimum resolution required */
+			int w, w2;
+
+			w = cfg->red_offset + cfg->red_width;
+			w2 = cfg->blue_offset + cfg->blue_width;
+			if (w < w2)
+				w = w2;
+			w2 = cfg->green_offset + cfg->green_width;
+			if (w < w2)
+				w = w2;
+			if (w > 16)
+				w = 24;
+			else if (w > 8)
+				w = 16;
+			else
+				w = 8;
+			cfg->rgb_resolution = w;
+		}
+		/* 00,11 - 32 bpp, 10 - 16 bpp, 01 - 8 bpp */
+		r &= ~0xC00;
+		if (cfg->rgb_resolution < 32)
+			r |= (cfg->rgb_resolution << 7);
+		__raw_writel((cfg->red_offset << 26) |
+			     (cfg->green_offset << 21) |
+			     (cfg->blue_offset << 16) |
+			     (cfg->red_width << 8) |
+			     (cfg->green_width << 4) |
+			     cfg->blue_width, PP_DEST_FRAME_FMT_CNTL);
+	} else {
+		/* add YUV422 formatting */
+		static const unsigned int _422[] = {
+			0x62000888,
+			0x60100888,
+			0x43080888,
+			0x41180888
+		};
+
+		__raw_writel(_422[(cfg->blue_offset >> 3) & 3],
+			     PP_DEST_FRAME_FMT_CNTL);
+		cfg->rgb_resolution = 16;
+		r &= ~0xC00;
+		r |= (cfg->rgb_resolution << 7);
+	}
+
+	/* add csc formatting */
+	if (!cfg->csc_table[1]) {
+		static const unsigned short _csc[][6] = {
+			{0x80, 0xb4, 0x2c, 0x5b, 0x0e4, 0},
+			{0x95, 0xcc, 0x32, 0x68, 0x104, 1},
+			{0x80, 0xca, 0x18, 0x3c, 0x0ec, 0},
+			{0x95, 0xe5, 0x1b, 0x44, 0x10e, 1},
+		};
+		memcpy(cfg->csc_table, _csc[cfg->csc_table[0]],
+		       sizeof(_csc[0]));
+	}
+	__raw_writel((cfg->csc_table[0] << 24) |
+		     (cfg->csc_table[1] << 16) |
+		     (cfg->csc_table[2] << 8) |
+		     cfg->csc_table[3], PP_CSC_COEF_0123);
+	__raw_writel((cfg->csc_table[5] ? (1 << 9) : 0) | cfg->csc_table[4],
+		     PP_CSC_COEF_4);
+
+	__raw_writel(r, PP_CNTL);
+
+	pphw_ptr(cfg);
+	pphw_outptr(cfg);
+
+	/*
+	 * #MB in a row = input_width / 16pix
+	 * 1 byte per QP per MB
+	 * QP must be formatted to be 4-byte aligned
+	 * YUV lines are to be 4-byte aligned as well
+	 * So Y is 8 byte aligned, as U = V = Y/2 for 420
+	 * MPEG MBs are 16x16 anyway
+	 */
+	__raw_writel((cfg->dim.in.width << 16) | cfg->dim.in.height,
+		     PP_PROCESS_FRAME_PARA);
+	__raw_writel(cfg->in_y_stride | (PP_CALC_QP_WIDTH(cfg) << 16),
+		     PP_SOURCE_FRAME_WIDTH);
+
+	/* in bytes */
+	rt = cfg->rgb_resolution >> 3;
+	if (rt == 3)
+		rt = 4;
+	__raw_writel(cfg->out_stride * rt, PP_DEST_DISPLAY_WIDTH);
+
+	pp_dump();
+	return 0;
+}
+
+/*!
+ * @brief Check PP interrupt status.
+ * @return	PP interrupt status
+ */
+static int pphw_isr(void)
+{
+	unsigned long status;
+
+	pr_debug("pp: in isr.\n");
+	status = __raw_readl(PP_INTRSTATUS) & 7;
+	if (!status) {
+		pr_debug("pp: not my isr err.\n");
+		return status;
+	}
+
+	if (status & 4)
+		pr_debug("pp: isr state error.\n");
+
+	/* clear interrupt status */
+	__raw_writel(status, PP_INTRSTATUS);
+
+	return status;
+}
+
+static struct clk *emma_clk;
+
+/*!
+ * @brief PP module clock enable
+ */
+static void pphw_init(void)
+{
+	emma_clk = clk_get(NULL, "emma_clk");
+	clk_enable(emma_clk);
+}
+
+/*!
+ * @brief PP module clock disable
+ */
+static void pphw_exit(void)
+{
+	clk_disable(emma_clk);
+	clk_put(emma_clk);
+}
diff --git a/drivers/media/video/mxc/output/mx27_pp.h b/drivers/media/video/mxc/output/mx27_pp.h
new file mode 100644
index 000000000000..7bc65ddda3a1
--- /dev/null
+++ b/drivers/media/video/mxc/output/mx27_pp.h
@@ -0,0 +1,180 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_pp.h
+ *
+ * @brief Header file for MX27 V4L2 Video Output Driver
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#ifndef __MX27_PP_H__
+#define __MX27_PP_H__
+
+#include "mxc_v4l2_output.h"
+
+/* PP register definitions */
+#define PP_REG(ofs)    (IO_ADDRESS(EMMA_BASE_ADDR) - 0x400 + ofs)
+
+/* Register offsets */
+#define PP_CNTL			PP_REG(0x00)
+#define PP_INTRCNTL	 	PP_REG(0x04)
+#define PP_INTRSTATUS		PP_REG(0x08)
+#define PP_SOURCE_Y_PTR		PP_REG(0x0C)
+#define PP_SOURCE_CB_PTR	PP_REG(0x10)
+#define PP_SOURCE_CR_PTR	PP_REG(0x14)
+#define PP_DEST_RGB_PTR 	PP_REG(0x18)
+#define PP_QUANTIZER_PTR	PP_REG(0x1C)
+#define PP_PROCESS_FRAME_PARA	PP_REG(0x20)
+#define PP_SOURCE_FRAME_WIDTH	PP_REG(0x24)
+#define PP_DEST_DISPLAY_WIDTH	PP_REG(0x28)
+#define PP_DEST_IMAGE_SIZE	PP_REG(0x2C)
+#define PP_DEST_FRAME_FMT_CNTL	PP_REG(0x30)
+#define PP_RESIZE_INDEX		PP_REG(0x34)
+#define	PP_CSC_COEF_0123	PP_REG(0x38)
+#define	PP_CSC_COEF_4		PP_REG(0x3C)
+#define PP_RESIZE_COEF_TBL	PP_REG(0x100)
+
+/* resize table dimensions
+    dest pixel index    left/32    right/32    #src pixels to read
+    0                   [BC_COEF]  [BC_COEF]   [BC_NXT]
+    :
+    pp_tbl_max-1
+*/
+#define BC_NXT		2
+#define BC_COEF		5
+#define SZ_COEF		(1 << BC_COEF)
+#define SZ_NXT		(1 << BC_NXT)
+
+/* PP operations */
+#define EN_DEBLOCK 	0x02
+#define EN_DERING	0x04
+#define EN_CSC		0x10
+#define EN_MACROBLOCK	0x20
+#define EN_DEF		0x16
+#define EN_MASK		0x36
+#define EN_BIGDATA	0x1000
+#define EN_BIGQP	0x2000
+
+/* PP CSC tables */
+#define CSC_TBL_NONE	0x80
+#define CSC_TBL_REUSE	0x81
+#define CSC_TBL_A1	0x00
+#define CSC_TBL_A0	0x20
+#define CSC_TBL_B1	0x40
+#define CSC_TBL_B0	0x60
+/* converts from 4 decimal fixed point to hw setting & vice versa */
+#define PP_CSC_FP4_2_HW(coeff)	((((coeff) << 7) + 5000) / 10000)
+#define PP_CSC_HW_2_FP4(coeff)	((((coeff) * 10000) + 64) >> 7)
+
+#define PP_PIX_YUYV	0
+#define PP_PIX_YVYU	8
+#define PP_PIX_UYVY	16
+#define PP_PIX_VYUY	24
+
+/* PP size & width calculation macros */
+#define PP_CALC_QP_WIDTH(cfg)	\
+	(!((cfg)->operation & (EN_DEBLOCK | EN_DERING)) ? 0 : \
+		(((((cfg)->dim.in.width + 15) >> 4) + 3) & ~3))
+#define PP_CALC_Y_SIZE(cfg)		\
+	((cfg)->in_y_stride * (cfg)->dim.in.height)
+#define PP_CALC_CH_SIZE(cfg)		(PP_CALC_Y_SIZE(cfg) >> 2)
+#define PP_CALC_BPP(cfg) 		\
+	((cfg)->rgb_resolution > 16 ?  4 : ((cfg)->rgb_resolution >> 3))
+#define PP_CALC_YUV_SIZE(cfg)		\
+	((PP_CALC_Y_SIZE(cfg) * 3) >> 1)
+#define PP_CALC_QP_SIZE(cfg)		\
+	(PP_CALC_QP_WIDTH(cfg) * (((cfg)->dim.in.height + 15) >> 4))
+#define PP_CALC_DEST_WIDTH(cfg)	\
+	(((cfg)->out_stride & ~1) * PP_CALC_BPP(cfg))
+#define PP_CALC_DEST_SIZE(cfg)	\
+	((cfg)->dim.out.height * PP_CALC_DEST_WIDTH(cfg))
+
+/*
+ * physical addresses for bus mastering
+ * v=0 -> yuv packed
+ * v=0 & qp=0 -> yuv packed with qp appended
+ */
+typedef struct _emma_pp_ptr {
+	unsigned int y;		/* Y data (line align8) */
+	unsigned int u;		/* U data (line align4) */
+	unsigned int v;		/* V data (line align4) */
+	unsigned int qp;	/* Quantization (line align4) */
+} emma_pp_ptr;
+
+typedef struct _emma_pp_size {
+	int width;
+	int height;
+} emma_pp_size;
+
+/*
+ * if num.width != 0
+ * 	resize ratio = num.width : den.width
+ * else
+ * 	resize ratio = in.width : out.width
+ * same for height
+ */
+typedef struct _emma_pp_scale {
+	emma_pp_size num;
+	emma_pp_size den;
+	emma_pp_size in;	/* clip */
+	emma_pp_size out;	/* 0 -> same as in */
+} emma_pp_scale;
+
+typedef struct _emma_pp_cfg {
+	unsigned char operation;	/* OR of EN_xx defines */
+
+	/*
+	 * input color coeff
+	 * fixed pt 8 bits, steps of 1/128
+	 * csc[5] is 1 or 0 to indicate Y + 16
+	 * csc[0] is matrix id 0-3 while csc[1-5]=0
+	 */
+	unsigned short csc_table[6];
+
+	/*
+	 * Output color (shade width, shade offset, pixel resolution)
+	 * Eg. 16bpp RGB565 resolution, the values could be:
+	 * red_width = 5, green_width = 6, blue_width = 6
+	 * red_offset = 11, green_offset = 5, blue_offset = 0 (defaults)
+	 * rgb_resolution = 16 (default)
+	 * For YUV422: xxx_width=0, blue_offset=PP_PIX_xxx
+	 */
+	unsigned short red_width;
+	unsigned short green_width;
+	unsigned short blue_width;
+	/* if offsets are 0, the offsets are by width LSb to MSb B:G:R */
+	unsigned short red_offset;
+	unsigned short blue_offset;
+	unsigned short green_offset;
+	/* if resolution is 0, the minimum for the sum of widths is chosen */
+	short rgb_resolution;	/* 8,16,24 bpp only */
+
+	emma_pp_ptr ptr;	/* dma buffer pointers */
+	unsigned int outptr;	/* RGB/YUV output */
+	emma_pp_scale dim;	/* in/out dimensions */
+
+	/* pixels between two adjacent input Y rows */
+	unsigned short in_y_stride;	/* 0 = in_width */
+	/* PIXELS between two adjacent output rows */
+	unsigned short out_stride;	/* 0 = out_width */
+} emma_pp_cfg;
+
+int pp_ptr(unsigned long ptr);
+int pp_enable(int flag);
+int pp_cfg(vout_data * vout);
+int pp_init(vout_data * vout);
+int pp_num_last(void);
+void pp_exit(vout_data * vout);
+
+#endif				/* __MX27_PP_H__ */
diff --git a/drivers/media/video/mxc/output/mx27_v4l2_output.c b/drivers/media/video/mxc/output/mx27_v4l2_output.c
new file mode 100644
index 000000000000..a00f92d8e979
--- /dev/null
+++ b/drivers/media/video/mxc/output/mx27_v4l2_output.c
@@ -0,0 +1,1442 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_v4l2_output.c
+ *
+ * @brief MX27 V4L2 Video Output Driver
+ *
+ * Video4Linux2 Output Device using MX27 eMMA Post-processing functionality.
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/semaphore.h>
+#include <linux/poll.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+
+#include "mxc_v4l2_output.h"
+#include "mx27_pp.h"
+#include "../drivers/video/mxc/mx2fb.h"
+
+#define SDC_FG_FB_FORMAT	V4L2_PIX_FMT_RGB565
+
+struct v4l2_output mxc_outputs[1] = {
+	{
+	 .index = 0,
+	 .name = "DISP0 Video Out",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
+						   but no other choice */
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN},
+};
+
+static int video_nr = 16;
+static spinlock_t g_lock = SPIN_LOCK_UNLOCKED;
+vout_data *g_vout;
+
+/* debug counters */
+uint32_t g_irq_cnt;
+uint32_t g_buf_output_cnt;
+uint32_t g_buf_q_cnt;
+uint32_t g_buf_dq_cnt;
+
+#ifdef CONFIG_VIDEO_MXC_OUTPUT_FBSYNC
+static uint32_t g_output_fb = -1;
+static uint32_t g_fb_enabled = 0;
+static uint32_t g_pp_ready = 0;
+
+static int fb_event_notify(struct notifier_block *self,
+			   unsigned long action, void *data)
+{
+	struct fb_event *event = data;
+	struct fb_info *info = event->info;
+	unsigned long lock_flags;
+	int blank, i;
+
+	for (i = 0; i < num_registered_fb; i++)
+		if (registered_fb[i] == info)
+			break;
+
+	/*
+	 * Check if the event is sent by the framebuffer in which
+	 * the video is displayed.
+	 */
+	if (i != g_output_fb)
+		return 0;
+
+	switch (action) {
+	case FB_EVENT_BLANK:
+		blank = *(int *)event->data;
+		spin_lock_irqsave(&g_lock, lock_flags);
+		g_fb_enabled = !blank;
+		if (blank && g_pp_ready) {
+			if (pp_enable(1))
+				pr_debug("unable to enable PP\n");
+			g_pp_ready = 0;
+		}
+		spin_unlock_irqrestore(&g_lock, lock_flags);
+		break;
+	case FB_EVENT_MXC_EOF:
+		spin_lock_irqsave(&g_lock, lock_flags);
+		g_fb_enabled = 1;
+		if (g_pp_ready) {
+			if (pp_enable(1))
+				pr_debug("unable to enable PP\n");
+			g_pp_ready = 0;
+		}
+		spin_unlock_irqrestore(&g_lock, lock_flags);
+		break;
+	}
+
+	return 0;
+}
+
+static struct notifier_block fb_event_notifier = {
+	.notifier_call = fb_event_notify,
+};
+
+static struct notifier_block mx2fb_event_notifier = {
+	.notifier_call = fb_event_notify,
+};
+#endif
+
+#define QUEUE_SIZE (MAX_FRAME_NUM + 1)
+static __inline int queue_size(v4l_queue * q)
+{
+	if (q->tail >= q->head)
+		return (q->tail - q->head);
+	else
+		return ((q->tail + QUEUE_SIZE) - q->head);
+}
+
+static __inline int queue_buf(v4l_queue * q, int idx)
+{
+	if (((q->tail + 1) % QUEUE_SIZE) == q->head)
+		return -1;	/* queue full */
+	q->list[q->tail] = idx;
+	q->tail = (q->tail + 1) % QUEUE_SIZE;
+	return 0;
+}
+
+static __inline int dequeue_buf(v4l_queue * q)
+{
+	int ret;
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	ret = q->list[q->head];
+	q->head = (q->head + 1) % QUEUE_SIZE;
+	return ret;
+}
+
+static __inline int peek_next_buf(v4l_queue * q)
+{
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	return q->list[q->head];
+}
+
+static __inline unsigned long get_jiffies(struct timeval *t)
+{
+	struct timeval cur;
+
+	if (t->tv_usec >= 1000000) {
+		t->tv_sec += t->tv_usec / 1000000;
+		t->tv_usec = t->tv_usec % 1000000;
+	}
+
+	do_gettimeofday(&cur);
+	if ((t->tv_sec < cur.tv_sec)
+	    || ((t->tv_sec == cur.tv_sec) && (t->tv_usec < cur.tv_usec)))
+		return jiffies;
+
+	if (t->tv_usec < cur.tv_usec) {
+		cur.tv_sec = t->tv_sec - cur.tv_sec - 1;
+		cur.tv_usec = t->tv_usec + 1000000 - cur.tv_usec;
+	} else {
+		cur.tv_sec = t->tv_sec - cur.tv_sec;
+		cur.tv_usec = t->tv_usec - cur.tv_usec;
+	}
+
+	return jiffies + timeval_to_jiffies(&cur);
+}
+
+/*!
+ * Private function to free buffers
+ *
+ * @param bufs_paddr	Array of physical address of buffers to be freed
+ *
+ * @param bufs_vaddr	Array of virtual address of buffers to be freed
+ *
+ * @param num_buf	Number of buffers to be freed
+ *
+ * @param size		Size for each buffer to be free
+ *
+ * @return status  0 success.
+ */
+static int mxc_free_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+			    int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		if (bufs_vaddr[i] != 0) {
+			dma_free_coherent(0, size, bufs_vaddr[i],
+					  bufs_paddr[i]);
+			pr_debug("freed @ paddr=0x%08X\n", (u32) bufs_paddr[i]);
+			bufs_paddr[i] = 0;
+			bufs_vaddr[i] = NULL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Private function to allocate buffers
+ *
+ * @param bufs_paddr	Output array of physical address of buffers allocated
+ *
+ * @param bufs_vaddr	Output array of virtual address of buffers allocated
+ *
+ * @param num_buf	Input number of buffers to allocate
+ *
+ * @param size		Input size for each buffer to allocate
+ *
+ * @return status	-0 Successfully allocated a buffer, -ENOBUFS failed.
+ */
+static int mxc_allocate_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+				int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		bufs_vaddr[i] = dma_alloc_coherent(0, size,
+						   &bufs_paddr[i],
+						   GFP_DMA | GFP_KERNEL);
+
+		if (bufs_vaddr[i] == 0) {
+			mxc_free_buffers(bufs_paddr, bufs_vaddr, i, size);
+			pr_debug("dma_alloc_coherent failed.\n");
+			return -ENOBUFS;
+		}
+		pr_debug("allocated @ paddr=0x%08X, size=%d.\n",
+			 (u32) bufs_paddr[i], size);
+	}
+
+	return 0;
+}
+
+static void mxc_v4l2out_timer_handler(unsigned long arg)
+{
+	int index;
+	unsigned long timeout;
+	unsigned long lock_flags;
+	vout_data *vout = (vout_data *) arg;
+
+	pr_debug("timer handler: %lu\n", jiffies);
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	if ((vout->state == STATE_STREAM_OFF)
+	    || (vout->state == STATE_STREAM_STOPPING)) {
+		pr_debug("stream has stopped\n");
+		goto exit0;
+	}
+
+	/*
+	 * If timer occurs before PP h/w is ready, then set the state to
+	 * paused and the timer will be set again when next buffer is queued
+	 * or PP completes.
+	 */
+	if (vout->ipu_buf[0] != -1) {
+		pr_debug("buffer is busy\n");
+		vout->state = STATE_STREAM_PAUSED;
+		goto exit0;
+	}
+
+	/* Dequeue buffer and pass to PP */
+	index = dequeue_buf(&vout->ready_q);
+	if (index == -1) {	/* no buffers ready, should never occur */
+		pr_debug("mxc_v4l2out: timer - no queued buffers ready\n");
+		goto exit0;
+	}
+
+	g_buf_dq_cnt++;
+	vout->frame_count++;
+	vout->ipu_buf[0] = index;
+
+	if (pp_ptr((unsigned int)vout->queue_buf_paddr[index])) {
+		pr_debug("unable to update buffer\n");
+		goto exit0;
+	}
+#ifdef CONFIG_VIDEO_MXC_OUTPUT_FBSYNC
+	if (g_fb_enabled && (vout->v4l2_fb.flags != V4L2_FBUF_FLAG_OVERLAY))
+		g_pp_ready = 1;
+	else if (pp_enable(1)) {
+		pr_debug("unable to enable PP\n");
+		goto exit0;
+	}
+#else
+	if (pp_enable(1)) {
+		pr_debug("unable to enable PP\n");
+		goto exit0;
+	}
+#endif
+	pr_debug("enabled index %d\n", index);
+
+	/* Setup timer for next buffer */
+	index = peek_next_buf(&vout->ready_q);
+	pr_debug("next index %d\n", index);
+	if (index != -1) {
+		/* if timestamp is 0, then default to 30fps */
+		if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+		    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+			timeout =
+			    vout->start_jiffies + vout->frame_count * HZ / 30;
+		else
+			timeout =
+			    get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+		if (jiffies >= timeout) {
+			pr_debug("warning: timer timeout already expired.\n");
+		}
+
+		if (mod_timer(&vout->output_timer, timeout))
+			pr_debug("warning: timer was already set\n");
+
+		pr_debug("timer handler next schedule: %lu\n", timeout);
+	} else {
+		vout->state = STATE_STREAM_PAUSED;
+		pr_debug("timer handler paused\n");
+	}
+
+      exit0:
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+}
+
+irqreturn_t mxc_v4l2out_pp_in_irq_handler(int irq, void *dev_id)
+{
+	int last_buf;
+	int index;
+	unsigned long timeout;
+	unsigned long lock_flags;
+	vout_data *vout = dev_id;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	g_irq_cnt++;
+
+	if ((vout->state == STATE_STREAM_OFF)
+	    || (vout->state == STATE_STREAM_STOPPING)) {
+		spin_unlock_irqrestore(&g_lock, lock_flags);
+		return IRQ_HANDLED;
+	}
+
+	if (vout->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		struct fb_gwinfo gwinfo;
+
+		gwinfo.enabled = 1;
+		gwinfo.alpha_value = 255;
+		gwinfo.ck_enabled = 0;
+		gwinfo.xpos = vout->crop_current.left;
+		gwinfo.ypos = vout->crop_current.top;
+		gwinfo.base = (unsigned long)vout->display_bufs[pp_num_last()];
+		gwinfo.xres = vout->crop_current.width;
+		gwinfo.yres = vout->crop_current.height;
+		gwinfo.xres_virtual = vout->crop_current.width;
+		gwinfo.vs_reversed = 0;
+
+		mx2_gw_set(&gwinfo);
+	}
+
+	/* Process previous buffer */
+	last_buf = vout->ipu_buf[0];
+	pr_debug("last_buf %d g_irq_cnt %d\n", last_buf, g_irq_cnt);
+	if (last_buf != -1) {
+		g_buf_output_cnt++;
+		vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
+		queue_buf(&vout->done_q, last_buf);
+		vout->ipu_buf[0] = -1;
+		wake_up_interruptible(&vout->v4l_bufq);
+	}
+
+	/* Setup timer for next buffer, when stream has been paused */
+	if ((vout->state == STATE_STREAM_PAUSED)
+	    && ((index = peek_next_buf(&vout->ready_q)) != -1)) {
+		pr_debug("next index %d\n", index);
+		/* if timestamp is 0, then default to 30fps */
+		if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+		    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+			timeout =
+			    vout->start_jiffies + vout->frame_count * HZ / 30;
+		else
+			timeout =
+			    get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+		if (jiffies >= timeout) {
+			pr_debug("warning: timer timeout already expired.\n");
+		}
+
+		vout->state = STATE_STREAM_ON;
+
+		if (mod_timer(&vout->output_timer, timeout))
+			pr_debug("warning: timer was already set\n");
+
+		pr_debug("timer handler next schedule: %lu\n", timeout);
+	}
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * Start the output stream
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamon(vout_data * vout)
+{
+	unsigned long timeout;
+	int index;
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state != STATE_STREAM_OFF)
+		return -EBUSY;
+
+	if (queue_size(&vout->ready_q) < 1) {
+		pr_debug("no buffers queued yet!\n");
+		return -EINVAL;
+	}
+
+	vout->ipu_buf[0] = -1;
+
+	if (vout->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		/* Free previously allocated buffer */
+		mxc_free_buffers(vout->display_bufs, vout->display_bufs_vaddr,
+				 2, vout->display_buf_size);
+		/* Allocate buffers for foreground */
+		if (mxc_allocate_buffers(vout->display_bufs,
+					 vout->display_bufs_vaddr, 2,
+					 vout->display_buf_size) < 0) {
+			pr_debug("unable to allocate SDC FG buffers\n");
+			return -ENOMEM;
+		}
+	}
+
+	/* Configure PP */
+	if (pp_cfg(vout)) {
+		/* Free previously allocated buffer */
+		mxc_free_buffers(vout->display_bufs, vout->display_bufs_vaddr,
+				 2, vout->display_buf_size);
+		pr_debug("failed to config PP.\n");
+		return -EINVAL;
+	}
+#ifdef CONFIG_VIDEO_MXC_OUTPUT_FBSYNC
+	g_output_fb = vout->output_fb_num[vout->cur_disp_output];
+	g_fb_enabled = 0;
+	g_pp_ready = 0;
+	fb_register_client(&fb_event_notifier);
+	mx2fb_register_client(&mx2fb_event_notifier);
+#endif
+	vout->frame_count = 0;
+	vout->state = STATE_STREAM_ON;
+	index = peek_next_buf(&vout->ready_q);
+
+	/* if timestamp is 0, then default to 30fps */
+	if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+	    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+		timeout = jiffies;
+	else
+		timeout = get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+	if (jiffies >= timeout) {
+		pr_debug("warning: timer timeout already expired.\n");
+	}
+
+	vout->start_jiffies = vout->output_timer.expires = timeout;
+	pr_debug("STREAMON:Add timer %d timeout @ %lu jiffies, current = %lu\n",
+		 index, timeout, jiffies);
+	add_timer(&vout->output_timer);
+
+	return 0;
+}
+
+/*!
+ * Shut down the voutera
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamoff(vout_data * vout)
+{
+	int i, retval = 0;
+	unsigned long lock_flag = 0;
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state == STATE_STREAM_OFF) {
+		return 0;
+	}
+
+	spin_lock_irqsave(&g_lock, lock_flag);
+
+	del_timer(&vout->output_timer);
+	pp_enable(0);		/* Disable PP */
+
+	if (vout->state == STATE_STREAM_ON) {
+		vout->state = STATE_STREAM_STOPPING;
+	}
+
+	spin_unlock_irqrestore(&g_lock, lock_flag);
+
+	vout->ready_q.head = vout->ready_q.tail = 0;
+	vout->done_q.head = vout->done_q.tail = 0;
+	for (i = 0; i < vout->buffer_cnt; i++) {
+		vout->v4l2_bufs[i].flags = 0;
+		vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+		vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+	}
+
+	vout->state = STATE_STREAM_OFF;
+
+	if (vout->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		struct fb_gwinfo gwinfo;
+
+		/* Disable graphic window */
+		gwinfo.enabled = 0;
+		mx2_gw_set(&gwinfo);
+	}
+#ifdef CONFIG_VIDEO_MXC_OUTPUT_FBSYNC
+	g_output_fb = -1;
+	g_fb_enabled = 0;
+	g_pp_ready = 0;
+	fb_unregister_client(&fb_event_notifier);
+	mx2fb_unregister_client(&mx2fb_event_notifier);
+#endif
+
+	mxc_free_buffers(vout->display_bufs, vout->display_bufs_vaddr,
+			 2, vout->display_buf_size);
+
+	return retval;
+}
+
+/*
+ * Valid whether the palette is supported
+ *
+ * @param palette  V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return 1 if supported, 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	return (palette == V4L2_PIX_FMT_YUV420);
+}
+
+/*
+ * Returns bits per pixel for given pixel format
+ *
+ * @param pixelformat  V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return bits per pixel of pixelformat
+ */
+static u32 fmt_to_bpp(u32 pixelformat)
+{
+	u32 bpp;
+
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_RGB565:
+		bpp = 16;
+		break;
+	case V4L2_PIX_FMT_BGR24:
+	case V4L2_PIX_FMT_RGB24:
+		bpp = 24;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_RGB32:
+		bpp = 32;
+		break;
+	default:
+		bpp = 8;
+		break;
+	}
+	return bpp;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_g_fmt(vout_data * vout, struct v4l2_format *f)
+{
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		return -EINVAL;
+	}
+	*f = vout->v2f;
+	return 0;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_s_fmt(vout_data * vout, struct v4l2_format *f)
+{
+	int retval = 0;
+	u32 size = 0;
+	u32 bytesperline;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		retval = -EINVAL;
+		goto err0;
+	}
+	if (!valid_mode(f->fmt.pix.pixelformat)) {
+		pr_debug("pixel format not supported\n");
+		retval = -EINVAL;
+		goto err0;
+	}
+
+	bytesperline = (f->fmt.pix.width * fmt_to_bpp(f->fmt.pix.pixelformat)) /
+	    8;
+	if (f->fmt.pix.bytesperline < bytesperline) {
+		f->fmt.pix.bytesperline = bytesperline;
+	} else {
+		bytesperline = f->fmt.pix.bytesperline;
+	}
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUV422P:
+		/* byteperline for YUV planar formats is for
+		   Y plane only */
+		size = bytesperline * f->fmt.pix.height * 2;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+		size = (bytesperline * f->fmt.pix.height * 3) / 2;
+		break;
+	default:
+		size = bytesperline * f->fmt.pix.height;
+		break;
+	}
+
+	/* Return the actual size of the image to the app */
+	f->fmt.pix.sizeimage = size;
+
+	vout->v2f.fmt.pix.sizeimage = size;
+	vout->v2f.fmt.pix.width = f->fmt.pix.width;
+	vout->v2f.fmt.pix.height = f->fmt.pix.height;
+	vout->v2f.fmt.pix.pixelformat = f->fmt.pix.pixelformat;
+	vout->v2f.fmt.pix.bytesperline = f->fmt.pix.bytesperline;
+
+	retval = 0;
+      err0:
+	return retval;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_get_v42lout_control(vout_data * vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		return (vout->rotate & IPU_ROTATE_HORIZ_FLIP) ? 1 : 0;
+	case V4L2_CID_VFLIP:
+		return (vout->rotate & IPU_ROTATE_VERT_FLIP) ? 1 : 0;
+	case (V4L2_CID_PRIVATE_BASE + 1):
+		return vout->rotate;
+	default:
+		return -EINVAL;
+	}
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_set_v42lout_control(vout_data * vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+	case V4L2_CID_MXC_ROT:
+		return 0;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*!
+ * V4L2 interface - open function
+ *
+ * @param inode        structure inode *
+ *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l2out_open(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+	int err;
+
+	if (!vout) {
+		pr_info("Internal error, vout_data not found!\n");
+		return -ENODEV;
+	}
+
+	down(&vout->busy_lock);
+
+	err = -EINTR;
+	if (signal_pending(current))
+		goto oops;
+
+	if (vout->open_count++ == 0) {
+		pp_init(vout);
+
+		init_waitqueue_head(&vout->v4l_bufq);
+
+		init_timer(&vout->output_timer);
+		vout->output_timer.function = mxc_v4l2out_timer_handler;
+		vout->output_timer.data = (unsigned long)vout;
+
+		vout->state = STATE_STREAM_OFF;
+		g_irq_cnt = g_buf_output_cnt = g_buf_q_cnt = g_buf_dq_cnt = 0;
+
+	}
+
+	file->private_data = dev;
+	up(&vout->busy_lock);
+	return 0;
+
+      oops:
+	up(&vout->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L2 interface - close function
+ *
+ * @param inode    struct inode *
+ *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int mxc_v4l2out_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = file->private_data;
+	vout_data *vout = video_get_drvdata(dev);
+
+	if (--vout->open_count == 0) {
+		pr_debug("release resource\n");
+
+		pp_exit(vout);
+		if (vout->state != STATE_STREAM_OFF)
+			mxc_v4l2out_streamoff(vout);
+
+		file->private_data = NULL;
+
+		mxc_free_buffers(vout->queue_buf_paddr,
+				 vout->queue_buf_vaddr,
+				 vout->buffer_cnt, vout->queue_buf_size);
+		vout->buffer_cnt = 0;
+		mxc_free_buffers(vout->display_bufs,
+				 vout->display_bufs_vaddr,
+				 2, vout->display_buf_size);
+
+		/* capture off */
+		wake_up_interruptible(&vout->v4l_bufq);
+	}
+
+	return 0;
+}
+
+/*!
+ * V4L2 interface - ioctl function
+ *
+ * @param inode      struct inode *
+ *
+ * @param file       struct file *
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void *
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int
+mxc_v4l2out_do_ioctl(struct inode *inode, struct file *file,
+		     unsigned int ioctlnr, void *arg)
+{
+	struct video_device *dev = file->private_data;
+	vout_data *vout = video_get_drvdata(dev);
+	int retval = 0;
+	int i = 0;
+
+	if (!vout)
+		return -EBADF;
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+	case VIDIOC_QUERYCAP:
+		{
+			struct v4l2_capability *cap = arg;
+			strcpy(cap->driver, "mxc_v4l2_output");
+			cap->version = 0;
+			cap->capabilities =
+			    V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+			cap->card[0] = '\0';
+			cap->bus_info[0] = '\0';
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_FMT:
+		{
+			struct v4l2_format *gf = arg;
+			retval = mxc_v4l2out_g_fmt(vout, gf);
+			break;
+		}
+	case VIDIOC_S_FMT:
+		{
+			struct v4l2_format *sf = arg;
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+			retval = mxc_v4l2out_s_fmt(vout, sf);
+			break;
+		}
+	case VIDIOC_REQBUFS:
+		{
+			struct v4l2_requestbuffers *req = arg;
+			if ((req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (req->memory != V4L2_MEMORY_MMAP)) {
+				pr_debug
+				    ("VIDIOC_REQBUFS: incorrect buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			if (req->count == 0)
+				mxc_v4l2out_streamoff(vout);
+
+			if (vout->state == STATE_STREAM_OFF) {
+				if (vout->queue_buf_paddr[0] != 0) {
+					mxc_free_buffers(vout->queue_buf_paddr,
+							 vout->queue_buf_vaddr,
+							 vout->buffer_cnt,
+							 vout->queue_buf_size);
+					pr_debug
+					    ("VIDIOC_REQBUFS: freed buffers\n");
+				}
+				vout->buffer_cnt = 0;
+			} else {
+				pr_debug("VIDIOC_REQBUFS: Buffer is in use\n");
+				retval = -EBUSY;
+				break;
+			}
+
+			if (req->count == 0)
+				break;
+
+			if (req->count < MIN_FRAME_NUM) {
+				req->count = MIN_FRAME_NUM;
+			} else if (req->count > MAX_FRAME_NUM) {
+				req->count = MAX_FRAME_NUM;
+			}
+			vout->buffer_cnt = req->count;
+			vout->queue_buf_size =
+			    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+
+			retval = mxc_allocate_buffers(vout->queue_buf_paddr,
+						      vout->queue_buf_vaddr,
+						      vout->buffer_cnt,
+						      vout->queue_buf_size);
+			if (retval < 0)
+				break;
+
+			/* Init buffer queues */
+			vout->done_q.head = 0;
+			vout->done_q.tail = 0;
+			vout->ready_q.head = 0;
+			vout->ready_q.tail = 0;
+
+			for (i = 0; i < vout->buffer_cnt; i++) {
+				memset(&(vout->v4l2_bufs[i]), 0,
+				       sizeof(vout->v4l2_bufs[i]));
+				vout->v4l2_bufs[i].flags = 0;
+				vout->v4l2_bufs[i].memory = V4L2_MEMORY_MMAP;
+				vout->v4l2_bufs[i].index = i;
+				vout->v4l2_bufs[i].type =
+				    V4L2_BUF_TYPE_VIDEO_OUTPUT;
+				vout->v4l2_bufs[i].length =
+				    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+				vout->v4l2_bufs[i].m.offset =
+				    (unsigned long)vout->queue_buf_paddr[i];
+				vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+				vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+			}
+			break;
+		}
+	case VIDIOC_QUERYBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			u32 type = buf->type;
+			int index = buf->index;
+
+			if ((type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt)) {
+				pr_debug
+				    ("VIDIOC_QUERYBUFS: incorrect buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+			down(&vout->param_lock);
+			memcpy(buf, &(vout->v4l2_bufs[index]), sizeof(*buf));
+			up(&vout->param_lock);
+			break;
+		}
+	case VIDIOC_QBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int index = buf->index;
+			unsigned long lock_flags;
+			unsigned long timeout;
+
+			if ((buf->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt) || (buf->flags != 0)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			pr_debug("VIDIOC_QBUF: %d\n", buf->index);
+
+			spin_lock_irqsave(&g_lock, lock_flags);
+
+			memcpy(&(vout->v4l2_bufs[index]), buf, sizeof(*buf));
+			vout->v4l2_bufs[index].flags |= V4L2_BUF_FLAG_QUEUED;
+
+			g_buf_q_cnt++;
+			queue_buf(&vout->ready_q, index);
+
+			if (vout->state == STATE_STREAM_PAUSED) {
+				index = peek_next_buf(&vout->ready_q);
+
+				/* if timestamp is 0, then default to 30fps */
+				if ((vout->v4l2_bufs[index].timestamp.tv_sec ==
+				     0)
+				    && (vout->v4l2_bufs[index].timestamp.
+					tv_usec == 0))
+					timeout =
+					    vout->start_jiffies +
+					    vout->frame_count * HZ / 30;
+				else
+					timeout =
+					    get_jiffies(&vout->v4l2_bufs[index].
+							timestamp);
+
+				if (jiffies >= timeout) {
+					pr_debug
+					    ("warning: timer timeout already expired.\n");
+				}
+
+				vout->output_timer.expires = timeout;
+				pr_debug
+				    ("QBUF:Add timer %d timeout @ %lu jiffies, "
+				     "current = %lu\n", index, timeout,
+				     jiffies);
+				add_timer(&vout->output_timer);
+				vout->state = STATE_STREAM_ON;
+			}
+
+			spin_unlock_irqrestore(&g_lock, lock_flags);
+			break;
+		}
+	case VIDIOC_DQBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int idx;
+
+			pr_debug("VIDIOC_DQBUF: q size = %d\n",
+				 queue_size(&vout->done_q));
+
+			if ((queue_size(&vout->done_q) == 0) &&
+			    (file->f_flags & O_NONBLOCK)) {
+				retval = -EAGAIN;
+				break;
+			}
+
+			if (!wait_event_interruptible_timeout(vout->v4l_bufq,
+							      queue_size(&vout->
+									 done_q)
+							      != 0, 10 * HZ)) {
+				pr_debug("VIDIOC_DQBUF: timeout\n");
+				retval = -ETIME;
+				break;
+			} else if (signal_pending(current)) {
+				pr_debug("VIDIOC_DQBUF: interrupt received\n");
+				retval = -ERESTARTSYS;
+				break;
+			}
+			idx = dequeue_buf(&vout->done_q);
+			if (idx == -1) {	/* No frame free */
+				pr_debug
+				    ("VIDIOC_DQBUF: no free buffers, returning\n");
+				retval = -EAGAIN;
+				break;
+			}
+			if ((vout->v4l2_bufs[idx].flags & V4L2_BUF_FLAG_DONE) ==
+			    0)
+				pr_debug
+				    ("VIDIOC_DQBUF: buffer in done q, but not "
+				     "flagged as done\n");
+
+			vout->v4l2_bufs[idx].flags = 0;
+			memcpy(buf, &(vout->v4l2_bufs[idx]), sizeof(*buf));
+			pr_debug("VIDIOC_DQBUF: %d\n", buf->index);
+			break;
+		}
+	case VIDIOC_STREAMON:
+		{
+			retval = mxc_v4l2out_streamon(vout);
+			break;
+		}
+	case VIDIOC_STREAMOFF:
+		{
+			retval = mxc_v4l2out_streamoff(vout);
+			break;
+		}
+	case VIDIOC_G_CTRL:
+		{
+			retval = mxc_get_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_S_CTRL:
+		{
+			retval = mxc_set_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_CROPCAP:
+		{
+			struct v4l2_cropcap *cap = arg;
+
+			if (cap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			cap->bounds = vout->crop_bounds[vout->cur_disp_output];
+			cap->defrect = vout->crop_bounds[vout->cur_disp_output];
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			crop->c = vout->crop_current;
+			break;
+		}
+	case VIDIOC_S_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+			struct v4l2_rect *b =
+			    &(vout->crop_bounds[vout->cur_disp_output]);
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.height < 0) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.width < 0) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (crop->c.top < b->top)
+				crop->c.top = b->top;
+			if (crop->c.top > b->top + b->height)
+				crop->c.top = b->top + b->height;
+			if (crop->c.height > b->top - crop->c.top + b->height)
+				crop->c.height =
+				    b->top - crop->c.top + b->height;
+
+			if (crop->c.left < b->left)
+				crop->c.top = b->left;
+			if (crop->c.left > b->left + b->width)
+				crop->c.top = b->left + b->width;
+			if (crop->c.width > b->left - crop->c.left + b->width)
+				crop->c.width =
+				    b->left - crop->c.left + b->width;
+
+			/* stride line limitation */
+			crop->c.height -= crop->c.height % 8;
+			crop->c.width -= crop->c.width % 8;
+
+			vout->crop_current = crop->c;
+
+			vout->display_buf_size = vout->crop_current.width *
+			    vout->crop_current.height;
+			vout->display_buf_size *=
+			    fmt_to_bpp(SDC_FG_FB_FORMAT) / 8;
+			break;
+		}
+	case VIDIOC_ENUMOUTPUT:
+		{
+			struct v4l2_output *output = arg;
+
+			if ((output->index >= 2) ||
+			    (vout->output_enabled[output->index] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			*output = mxc_outputs[0];
+			output->name[4] = '0' + output->index;
+			break;
+		}
+	case VIDIOC_G_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			*p_output_num = vout->cur_disp_output;
+			break;
+		}
+	case VIDIOC_S_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			if ((*p_output_num >= 2) ||
+			    (vout->output_enabled[*p_output_num] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+
+			vout->cur_disp_output = *p_output_num;
+			break;
+		}
+	case VIDIOC_G_FBUF:
+		{
+			struct v4l2_framebuffer *fb = arg;
+			*fb = vout->v4l2_fb;
+			break;
+		}
+	case VIDIOC_S_FBUF:
+		{
+			struct v4l2_framebuffer *fb = arg;
+			vout->v4l2_fb = *fb;
+			vout->v4l2_fb.capability = V4L2_FBUF_CAP_EXTERNOVERLAY;
+			break;
+		}
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_G_PARM:
+	case VIDIOC_ENUMSTD:
+	case VIDIOC_G_STD:
+	case VIDIOC_S_STD:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	default:
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&vout->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L2 interface - ioctl function
+ *
+ * @return  None
+ */
+static int
+mxc_v4l2out_ioctl(struct inode *inode, struct file *file,
+		  unsigned int cmd, unsigned long arg)
+{
+	return video_usercopy(inode, file, cmd, arg, mxc_v4l2out_do_ioctl);
+}
+
+/*!
+ * V4L2 interface - mmap function
+ *
+ * @param file          structure file *
+ *
+ * @param vma           structure vm_area_struct *
+ *
+ * @return status       0 Success, EINTR busy lock error,
+ *                      ENOBUFS remap_page error
+ */
+static int mxc_v4l2out_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *dev = file->private_data;
+	unsigned long start = vma->vm_start;
+	unsigned long size = vma->vm_end - vma->vm_start;
+	int res = 0;
+	vout_data *vout = video_get_drvdata(dev);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	/* make buffers write-thru cacheable */
+	vma->vm_page_prot = __pgprot(pgprot_val(vma->vm_page_prot) &
+				     ~L_PTE_BUFFERABLE);
+
+	if (remap_pfn_range(vma, start, vma->vm_pgoff, size, vma->vm_page_prot)) {
+		pr_debug("mxc_mmap(V4L)i - remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+      mxc_mmap_exit:
+	up(&vout->busy_lock);
+	return res;
+}
+
+/*!
+ * V4L2 interface - poll function
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_v4l2out_poll(struct file *file, poll_table * wait)
+{
+	struct video_device *dev = file->private_data;
+	vout_data *vout = video_get_drvdata(dev);
+
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	queue = &vout->v4l_bufq;
+	poll_wait(file, queue, wait);
+
+	up(&vout->busy_lock);
+	return res;
+}
+
+static struct file_operations mxc_v4l2out_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_v4l2out_open,
+	.release = mxc_v4l2out_close,
+	.ioctl = mxc_v4l2out_ioctl,
+	.mmap = mxc_v4l2out_mmap,
+	.poll = mxc_v4l2out_poll,
+};
+
+static struct video_device mxc_v4l2out_template = {
+	.name = "MXC Video Output",
+	.vfl_type = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING,
+	.fops = &mxc_v4l2out_fops,
+	.release = video_device_release,
+};
+
+/*!
+ * Probe routine for the framebuffer driver. It is called during the
+ * driver binding process.      The following functions are performed in
+ * this routine: Framebuffer initialization, Memory allocation and
+ * mapping, Framebuffer registration, IPU initialization.
+ *
+ * @return      Appropriate error code to the kernel common code
+ */
+static int mxc_v4l2out_probe(struct platform_device *pdev)
+{
+	int i;
+	vout_data *vout;
+
+	/*
+	 * Allocate sufficient memory for the fb structure
+	 */
+	g_vout = vout = kmalloc(sizeof(vout_data), GFP_KERNEL);
+
+	if (!vout)
+		return 0;
+
+	memset(vout, 0, sizeof(vout_data));
+
+	vout->video_dev = video_device_alloc();
+	if (vout->video_dev == NULL)
+		return -1;
+	vout->video_dev->minor = -1;
+
+	*(vout->video_dev) = mxc_v4l2out_template;
+
+	/* register v4l device */
+	if (video_register_device(vout->video_dev,
+				  VFL_TYPE_GRABBER, video_nr) == -1) {
+		pr_debug("video_register_device failed\n");
+		return 0;
+	}
+	pr_debug("mxc_v4l2out: registered device video%d\n",
+		 vout->video_dev->minor & 0x1f);
+
+	video_set_drvdata(vout->video_dev, vout);
+
+	init_MUTEX(&vout->param_lock);
+	init_MUTEX(&vout->busy_lock);
+
+	/* setup outputs and cropping */
+	vout->cur_disp_output = -1;
+	for (i = 0; i < num_registered_fb; i++) {
+		char *idstr = registered_fb[i]->fix.id;
+		if (strncmp(idstr, "DISP", 4) == 0) {
+			int disp_num = i;
+			vout->crop_bounds[disp_num].left = 0;
+			vout->crop_bounds[disp_num].top = 0;
+			vout->crop_bounds[disp_num].width =
+			    registered_fb[i]->var.xres;
+			vout->crop_bounds[disp_num].height =
+			    registered_fb[i]->var.yres;
+			vout->output_enabled[disp_num] = true;
+			vout->output_fb_num[disp_num] = i;
+			if (vout->cur_disp_output == -1)
+				vout->cur_disp_output = disp_num;
+		}
+
+	}
+	vout->crop_current = vout->crop_bounds[vout->cur_disp_output];
+
+	/* Setup framebuffer parameters */
+	vout->v4l2_fb.capability = V4L2_FBUF_CAP_EXTERNOVERLAY;
+	vout->v4l2_fb.flags = V4L2_FBUF_FLAG_PRIMARY;
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_v4l2out_driver = {
+	.driver = {
+		   .name = "MXC Video Output",
+		   .owner = THIS_MODULE,
+		   .bus = &platform_bus_type,
+		   },
+	.probe = mxc_v4l2out_probe,
+	.remove = NULL,
+};
+
+static void camera_platform_release(struct device *device)
+{
+}
+
+static struct platform_device mxc_v4l2out_device = {
+	.name = "MXC Video Output",
+	.dev = {
+		.release = camera_platform_release,
+		},
+	.id = 0,
+};
+
+/*!
+ * mxc v4l2 init function
+ *
+ */
+static int mxc_v4l2out_init(void)
+{
+	u8 err = 0;
+
+	err = platform_driver_register(&mxc_v4l2out_driver);
+	if (err == 0) {
+		platform_device_register(&mxc_v4l2out_device);
+	}
+	return err;
+}
+
+/*!
+ * mxc v4l2 cleanup function
+ *
+ */
+static void mxc_v4l2out_clean(void)
+{
+	pr_debug("unregistering video\n");
+
+	video_unregister_device(g_vout->video_dev);
+
+	platform_driver_unregister(&mxc_v4l2out_driver);
+	platform_device_unregister(&mxc_v4l2out_device);
+	kfree(g_vout);
+	g_vout = NULL;
+}
+
+module_init(mxc_v4l2out_init);
+module_exit(mxc_v4l2out_clean);
+
+module_param(video_nr, int, 0444);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2-driver for MXC video output");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/output/mx31_v4l2_wvga_output.c b/drivers/media/video/mxc/output/mx31_v4l2_wvga_output.c
new file mode 100644
index 000000000000..4a82da2d7d00
--- /dev/null
+++ b/drivers/media/video/mxc/output/mx31_v4l2_wvga_output.c
@@ -0,0 +1,1926 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/media/video/mxc/output/mxc_v4l2_output.c
+ *
+ * @brief MXC V4L2 Video Output Driver
+ *
+ * Video4Linux2 Output Device using MXC IPU Post-processing functionality.
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/console.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <asm/cacheflush.h>
+#include <asm/io.h>
+#include <asm/semaphore.h>
+#include <linux/dma-mapping.h>
+
+#include <mach/mxcfb.h>
+#include <mach/ipu.h>
+
+#include "mxc_v4l2_output.h"
+
+vout_data *g_vout;
+#define SDC_FG_FB_FORMAT        IPU_PIX_FMT_RGB565
+
+struct v4l2_output mxc_outputs[2] = {
+	{
+	 .index = MXC_V4L2_OUT_2_SDC,
+	 .name = "DISP3 Video Out",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
+						   but no other choice */
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN},
+	{
+	 .index = MXC_V4L2_OUT_2_ADC,
+	 .name = "DISPx Video Out",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
+						   but no other choice */
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN}
+};
+
+static int video_nr = 16;
+static DEFINE_SPINLOCK(g_lock);
+static unsigned int g_pp_out_number;
+static unsigned int g_pp_in_number;
+
+/* debug counters */
+uint32_t g_irq_cnt;
+uint32_t g_buf_output_cnt;
+uint32_t g_buf_q_cnt;
+uint32_t g_buf_dq_cnt;
+
+static inline uint32_t channel_2_dma(ipu_channel_t ch, ipu_buffer_t type)
+{
+	return ((type == IPU_INPUT_BUFFER) ? ((uint32_t) ch & 0xFF) :
+		((type == IPU_OUTPUT_BUFFER) ? (((uint32_t) ch >> 8) & 0xFF)
+		 : (((uint32_t) ch >> 16) & 0xFF)));
+};
+
+static inline uint32_t DMAParamAddr(uint32_t dma_ch)
+{
+	return (0x10000 | (dma_ch << 4));
+};
+
+#define QUEUE_SIZE (MAX_FRAME_NUM + 1)
+static inline int queue_size(v4l_queue * q)
+{
+	if (q->tail >= q->head)
+		return (q->tail - q->head);
+	else
+		return ((q->tail + QUEUE_SIZE) - q->head);
+}
+
+static inline int queue_buf(v4l_queue * q, int idx)
+{
+	if (((q->tail + 1) % QUEUE_SIZE) == q->head)
+		return -1;	/* queue full */
+	q->list[q->tail] = idx;
+	q->tail = (q->tail + 1) % QUEUE_SIZE;
+	return 0;
+}
+
+static inline int dequeue_buf(v4l_queue * q)
+{
+	int ret;
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	ret = q->list[q->head];
+	q->head = (q->head + 1) % QUEUE_SIZE;
+	return ret;
+}
+
+static inline int peek_next_buf(v4l_queue * q)
+{
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	return q->list[q->head];
+}
+
+static inline unsigned long get_jiffies(struct timeval *t)
+{
+	struct timeval cur;
+
+	if (t->tv_usec >= 1000000) {
+		t->tv_sec += t->tv_usec / 1000000;
+		t->tv_usec = t->tv_usec % 1000000;
+	}
+
+	do_gettimeofday(&cur);
+	if ((t->tv_sec < cur.tv_sec)
+	    || ((t->tv_sec == cur.tv_sec) && (t->tv_usec < cur.tv_usec)))
+		return jiffies;
+
+	if (t->tv_usec < cur.tv_usec) {
+		cur.tv_sec = t->tv_sec - cur.tv_sec - 1;
+		cur.tv_usec = t->tv_usec + 1000000 - cur.tv_usec;
+	} else {
+		cur.tv_sec = t->tv_sec - cur.tv_sec;
+		cur.tv_usec = t->tv_usec - cur.tv_usec;
+	}
+
+	return jiffies + timeval_to_jiffies(&cur);
+}
+
+/*!
+ * Private function to free buffers
+ *
+ * @param bufs_paddr	Array of physical address of buffers to be freed
+ *
+ * @param bufs_vaddr	Array of virtual address of buffers to be freed
+ *
+ * @param num_buf	Number of buffers to be freed
+ *
+ * @param size		Size for each buffer to be free
+ *
+ * @return status  0 success.
+ */
+static int mxc_free_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+			    int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		if (bufs_vaddr[i] != 0) {
+			dma_free_coherent(0, size, bufs_vaddr[i],
+					  bufs_paddr[i]);
+			pr_debug("freed @ paddr=0x%08X\n", (u32) bufs_paddr[i]);
+			bufs_paddr[i] = 0;
+			bufs_vaddr[i] = NULL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Private function to allocate buffers
+ *
+ * @param bufs_paddr	Output array of physical address of buffers allocated
+ *
+ * @param bufs_vaddr	Output array of virtual address of buffers allocated
+ *
+ * @param num_buf	Input number of buffers to allocate
+ *
+ * @param size		Input size for each buffer to allocate
+ *
+ * @return status	-0 Successfully allocated a buffer, -ENOBUFS failed.
+ */
+static int mxc_allocate_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+				int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		bufs_vaddr[i] = dma_alloc_coherent(0, size,
+						   &bufs_paddr[i],
+						   GFP_DMA | GFP_KERNEL);
+
+		if (bufs_vaddr[i] == 0) {
+			mxc_free_buffers(bufs_paddr, bufs_vaddr, i, size);
+			printk(KERN_ERR "dma_alloc_coherent failed.\n");
+			return -ENOBUFS;
+		}
+		pr_debug("allocated @ paddr=0x%08X, size=%d.\n",
+			 (u32) bufs_paddr[i], size);
+	}
+
+	return 0;
+}
+
+/*
+ * Returns bits per pixel for given pixel format
+ *
+ * @param pixelformat  V4L2_PIX_FMT_RGB565,
+ * 		       V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return bits per pixel of pixelformat
+ */
+static u32 fmt_to_bpp(u32 pixelformat)
+{
+	u32 bpp;
+
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_RGB565:
+		bpp = 16;
+		break;
+	case V4L2_PIX_FMT_BGR24:
+	case V4L2_PIX_FMT_RGB24:
+		bpp = 24;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_RGB32:
+		bpp = 32;
+		break;
+	default:
+		bpp = 8;
+		break;
+	}
+	return bpp;
+}
+
+static u32 bpp_to_fmt(struct fb_info *fbi)
+{
+	if (fbi->var.nonstd)
+		return fbi->var.nonstd;
+
+	if (fbi->var.bits_per_pixel == 24)
+		return V4L2_PIX_FMT_BGR24;
+	else if (fbi->var.bits_per_pixel == 32)
+		return V4L2_PIX_FMT_BGR32;
+	else if (fbi->var.bits_per_pixel == 16)
+		return V4L2_PIX_FMT_RGB565;
+
+	return 0;
+}
+
+static void mxc_v4l2out_timer_handler(unsigned long arg)
+{
+	int index;
+	unsigned long timeout;
+	unsigned long lock_flags = 0;
+	vout_data *vout = (vout_data *) arg;
+
+	dev_dbg(vout->video_dev->dev, "timer handler: %lu\n", jiffies);
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	if ((vout->state == STATE_STREAM_STOPPING)
+	    || (vout->state == STATE_STREAM_OFF))
+		goto exit0;
+	/*
+	 * If timer occurs before IPU h/w is ready, then set the state to
+	 * paused and the timer will be set again when next buffer is queued
+	 * or PP comletes
+	 */
+	if (vout->ipu_buf[0] != -1) {
+		dev_dbg(vout->video_dev->dev, "IPU buffer busy\n");
+		vout->state = STATE_STREAM_PAUSED;
+		goto exit0;
+	}
+
+	/* One frame buffer should be ready here */
+	if (vout->frame_count % 2 == 1) {
+		/* set BUF0 rdy */
+		if (ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 0) <
+		    0)
+			pr_debug("error selecting display buf 0");
+	} else {
+		if (ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 1) <
+		    0)
+			pr_debug("error selecting display buf 1");
+	}
+
+	/* Dequeue buffer and pass to IPU */
+	index = dequeue_buf(&vout->ready_q);
+	if (index == -1) {	/* no buffers ready, should never occur */
+		dev_err(vout->video_dev->dev,
+			"mxc_v4l2out: timer - no queued buffers ready\n");
+		goto exit0;
+	}
+
+	g_buf_dq_cnt++;
+	vout->frame_count++;
+	vout->ipu_buf[1] = vout->ipu_buf[0] = index;
+
+	if (ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+				      0,
+				      vout->v4l2_bufs[vout->ipu_buf[0]].m.
+				      offset) < 0)
+		goto exit0;
+
+	if (ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+				      1,
+				      vout->v4l2_bufs[vout->ipu_buf[0]].m.
+				      offset + vout->v2f.fmt.pix.width / 2) < 0)
+		goto exit0;
+
+	/* All buffer should now ready in IPU out, tranfer to display buf */
+	if (ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
+				      0,
+				      vout->
+				      display_bufs[(vout->frame_count -
+						    1) % 2]) < 0) {
+		dev_err(vout->video_dev->dev,
+			"unable to update buffer %d address\n",
+			vout->next_rdy_ipu_buf);
+		goto exit0;
+	}
+	if (ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
+				      1,
+				      vout->
+				      display_bufs[(vout->frame_count -
+						    1) % 2] +
+				      vout->crop_current.width / 2 *
+				      bytes_per_pixel(SDC_FG_FB_FORMAT)) < 0) {
+		dev_err(vout->video_dev->dev,
+			"unable to update buffer %d address\n",
+			vout->next_rdy_ipu_buf);
+		goto exit0;
+	}
+
+	if (ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0) < 0) {
+		dev_err(vout->video_dev->dev,
+			"unable to set IPU buffer ready\n");
+		goto exit0;
+	}
+
+	if (ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 0) < 0) {
+		dev_err(vout->video_dev->dev,
+			"unable to set IPU buffer ready\n");
+		goto exit0;
+	}
+
+	/* Setup timer for next buffer */
+	index = peek_next_buf(&vout->ready_q);
+	if (index != -1) {
+		/* if timestamp is 0, then default to 30fps */
+		if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+		    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+			timeout =
+			    vout->start_jiffies + vout->frame_count * HZ / 30;
+		else
+			timeout =
+			    get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+		if (jiffies >= timeout) {
+			dev_dbg(vout->video_dev->dev,
+				"warning: timer timeout already expired.\n");
+		}
+		if (mod_timer(&vout->output_timer, timeout))
+			dev_dbg(vout->video_dev->dev,
+				"warning: timer was already set\n");
+
+		dev_dbg(vout->video_dev->dev,
+			"timer handler next schedule: %lu\n", timeout);
+	} else {
+		vout->state = STATE_STREAM_PAUSED;
+	}
+
+exit0:
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+}
+
+extern void _ipu_write_param_mem(uint32_t addr, uint32_t *data,
+				 uint32_t numWords);
+
+static irqreturn_t mxc_v4l2out_pp_in_irq_handler(int irq, void *dev_id)
+{
+	unsigned long lock_flags = 0;
+	vout_data *vout = dev_id;
+	uint32_t u_offset;
+	uint32_t v_offset;
+	uint32_t local_params[4];
+	uint32_t width, height;
+	uint32_t dma_chan;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+	g_irq_cnt++;
+
+	dma_chan = channel_2_dma(vout->post_proc_ch, IPU_INPUT_BUFFER);
+	memset(&local_params, 0, sizeof(local_params));
+
+	if (g_pp_in_number % 2 == 1) {
+		u_offset = vout->offset.u_offset - vout->v2f.fmt.pix.width / 4;
+		v_offset = vout->offset.v_offset - vout->v2f.fmt.pix.width / 4;
+		width = vout->v2f.fmt.pix.width / 2;
+		height = vout->v2f.fmt.pix.height;
+		local_params[3] =
+		    (uint32_t) ((width - 1) << 12) | ((uint32_t) (height -
+								  1) << 24);
+		local_params[1] = (1UL << (46 - 32)) | (u_offset << (53 - 32));
+		local_params[2] = u_offset >> (64 - 53);
+		local_params[2] |= v_offset << (79 - 64);
+		local_params[3] |= v_offset >> (96 - 79);
+		_ipu_write_param_mem(DMAParamAddr(dma_chan), local_params, 4);
+
+		if (ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 1) <
+		    0) {
+			dev_err(vout->video_dev->dev,
+				"unable to set IPU buffer ready\n");
+		}
+	} else {
+		u_offset = vout->offset.u_offset;
+		v_offset = vout->offset.v_offset;
+		width = vout->v2f.fmt.pix.width / 2;
+		height = vout->v2f.fmt.pix.height;
+		local_params[3] =
+		    (uint32_t) ((width - 1) << 12) | ((uint32_t) (height -
+								  1) << 24);
+		local_params[1] = (1UL << (46 - 32)) | (u_offset << (53 - 32));
+		local_params[2] = u_offset >> (64 - 53);
+		local_params[2] |= v_offset << (79 - 64);
+		local_params[3] |= v_offset >> (96 - 79);
+		_ipu_write_param_mem(DMAParamAddr(dma_chan), local_params, 4);
+	}
+	g_pp_in_number++;
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mxc_v4l2out_pp_out_irq_handler(int irq, void *dev_id)
+{
+	vout_data *vout = dev_id;
+	int index;
+	unsigned long timeout;
+	u32 lock_flags = 0;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	if (g_pp_out_number % 2 == 1) {
+		if (ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 1)
+		    < 0) {
+			dev_err(vout->video_dev->dev,
+				"unable to set IPU buffer ready\n");
+		}
+	} else {
+		if (vout->ipu_buf[0] != -1) {
+			vout->v4l2_bufs[vout->ipu_buf[0]].flags =
+			    V4L2_BUF_FLAG_DONE;
+			queue_buf(&vout->done_q, vout->ipu_buf[0]);
+			wake_up_interruptible(&vout->v4l_bufq);
+			vout->ipu_buf[0] = -1;
+		}
+		if (vout->state == STATE_STREAM_STOPPING) {
+			if ((vout->ipu_buf[0] == -1)
+			    && (vout->ipu_buf[1] == -1))
+				vout->state = STATE_STREAM_OFF;
+		} else if ((vout->state == STATE_STREAM_PAUSED)
+			   && ((index = peek_next_buf(&vout->ready_q)) != -1)) {
+			/*!
+			 * Setup timer for next buffer,
+			 * when stream has been paused
+			 */
+			pr_debug("next index %d\n", index);
+
+			/* if timestamp is 0, then default to 30fps */
+			if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+			    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+				timeout =
+				    vout->start_jiffies +
+				    vout->frame_count * HZ / 30;
+			else
+				timeout =
+				    get_jiffies(&vout->v4l2_bufs[index].
+						timestamp);
+
+			if (jiffies >= timeout) {
+				pr_debug
+				    ("warning: timer timeout"
+				     "already expired.\n");
+			}
+
+			vout->state = STATE_STREAM_ON;
+
+			if (mod_timer(&vout->output_timer, timeout))
+				pr_debug("warning: timer was already set\n");
+
+			pr_debug("timer handler next schedule: %lu\n", timeout);
+		}
+	}
+	g_pp_out_number++;
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+	return IRQ_HANDLED;
+}
+
+/*!
+ * Start the output stream
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamon(vout_data *vout)
+{
+	struct device *dev = vout->video_dev->dev;
+	ipu_channel_params_t params;
+	struct mxcfb_pos fb_pos;
+	struct fb_var_screeninfo fbvar;
+	struct fb_info *fbi =
+	    registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	int pp_in_buf[2];
+	u16 out_width;
+	u16 out_height;
+	ipu_channel_t display_input_ch = MEM_PP_MEM;
+	bool use_direct_adc = false;
+	mm_segment_t old_fs;
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state != STATE_STREAM_OFF)
+		return -EBUSY;
+
+	if (queue_size(&vout->ready_q) < 2) {
+		dev_err(dev, "2 buffers not been queued yet!\n");
+		return -EINVAL;
+	}
+
+	out_width = vout->crop_current.width;
+	out_height = vout->crop_current.height;
+
+	vout->next_done_ipu_buf = vout->next_rdy_ipu_buf = 0;
+	vout->ipu_buf[0] = pp_in_buf[0] = dequeue_buf(&vout->ready_q);
+	vout->ipu_buf[1] = pp_in_buf[1] = vout->ipu_buf[0];
+	vout->frame_count = 1;
+	g_pp_out_number = 1;
+	g_pp_in_number = 1;
+
+	ipu_enable_irq(IPU_IRQ_PP_IN_EOF);
+	ipu_enable_irq(IPU_IRQ_PP_OUT_EOF);
+
+	/* Init Display Channel */
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL
+	if (vout->cur_disp_output < DISP3) {
+		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_OFF, 0);
+		fbi = NULL;
+		if (ipu_can_rotate_in_place(vout->rotate)) {
+			dev_dbg(dev, "Using PP direct to ADC channel\n");
+			use_direct_adc = true;
+			vout->display_ch = MEM_PP_ADC;
+			vout->post_proc_ch = MEM_PP_ADC;
+
+			memset(&params, 0, sizeof(params));
+			params.mem_pp_adc.in_width = vout->v2f.fmt.pix.width;
+			params.mem_pp_adc.in_height = vout->v2f.fmt.pix.height;
+			params.mem_pp_adc.in_pixel_fmt =
+			    vout->v2f.fmt.pix.pixelformat;
+			params.mem_pp_adc.out_width = out_width;
+			params.mem_pp_adc.out_height = out_height;
+			params.mem_pp_adc.out_pixel_fmt = SDC_FG_FB_FORMAT;
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+			params.mem_pp_adc.out_left =
+			    2 + vout->crop_current.left;
+#else
+			params.mem_pp_adc.out_left =
+			    12 + vout->crop_current.left;
+#endif
+			params.mem_pp_adc.out_top = vout->crop_current.top;
+			if (ipu_init_channel(
+				vout->post_proc_ch, &params) != 0) {
+				dev_err(dev, "Error initializing PP chan\n");
+				return -EINVAL;
+			}
+
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_INPUT_BUFFER,
+						    params.mem_pp_adc.
+						    in_pixel_fmt,
+						    params.mem_pp_adc.in_width,
+						    params.mem_pp_adc.in_height,
+						    vout->v2f.fmt.pix.
+						    bytesperline /
+						    bytes_per_pixel(params.
+							mem_pp_adc.
+							in_pixel_fmt),
+						    vout->rotate,
+						    vout->
+						    v4l2_bufs[pp_in_buf[0]].m.
+						    offset,
+						    vout->
+						    v4l2_bufs[pp_in_buf[1]].m.
+						    offset,
+						    vout->offset.u_offset,
+						    vout->offset.v_offset) !=
+			    0) {
+				dev_err(dev, "Error initializing PP in buf\n");
+				return -EINVAL;
+			}
+
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_OUTPUT_BUFFER,
+						    params.mem_pp_adc.
+						    out_pixel_fmt, out_width,
+						    out_height, out_width,
+						    vout->rotate, 0, 0, 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing PP"
+					"output buffer\n");
+				return -EINVAL;
+			}
+
+		} else {
+			dev_dbg(dev, "Using ADC SYS2 channel\n");
+			vout->display_ch = ADC_SYS2;
+			vout->post_proc_ch = MEM_PP_MEM;
+
+			if (vout->display_bufs[0]) {
+				mxc_free_buffers(vout->display_bufs,
+						 vout->display_bufs_vaddr,
+						 2, vout->display_buf_size);
+			}
+
+			vout->display_buf_size = vout->crop_current.width *
+			    vout->crop_current.height *
+			    fmt_to_bpp(SDC_FG_FB_FORMAT) / 8;
+			mxc_allocate_buffers(vout->display_bufs,
+					     vout->display_bufs_vaddr,
+					     2, vout->display_buf_size);
+
+			memset(&params, 0, sizeof(params));
+			params.adc_sys2.disp = vout->cur_disp_output;
+			params.adc_sys2.ch_mode = WriteTemplateNonSeq;
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+			params.adc_sys2.out_left = 2 + vout->crop_current.left;
+#else
+			params.adc_sys2.out_left = 12 + vout->crop_current.left;
+#endif
+			params.adc_sys2.out_top = vout->crop_current.top;
+			if (ipu_init_channel(ADC_SYS2, &params) < 0)
+				return -EINVAL;
+
+			if (ipu_init_channel_buffer(vout->display_ch,
+						    IPU_INPUT_BUFFER,
+						    SDC_FG_FB_FORMAT,
+						    out_width, out_height,
+						    out_width, IPU_ROTATE_NONE,
+						    vout->display_bufs[0],
+						    vout->display_bufs[1], 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing SDC FG buffer\n");
+				return -EINVAL;
+			}
+		}
+	} else
+#endif
+	{			/* Use SDC */
+		dev_dbg(dev, "Using SDC channel\n");
+
+		fbvar = fbi->var;
+		if (vout->cur_disp_output == 3) {
+			vout->display_ch = MEM_FG_SYNC;
+			fbvar.bits_per_pixel = 16;
+			fbvar.nonstd = IPU_PIX_FMT_UYVY;
+
+			fbvar.xres = fbvar.xres_virtual = out_width;
+			fbvar.yres = out_height;
+			fbvar.yres_virtual = out_height * 2;
+		} else if (vout->cur_disp_output == 5) {
+			vout->display_ch = MEM_DC_SYNC;
+			fbvar.bits_per_pixel = 16;
+			fbvar.nonstd = IPU_PIX_FMT_UYVY;
+
+			fbvar.xres = fbvar.xres_virtual = out_width;
+			fbvar.yres = out_height;
+			fbvar.yres_virtual = out_height * 2;
+		} else {
+			vout->display_ch = MEM_BG_SYNC;
+		}
+
+		fbvar.activate |= FB_ACTIVATE_FORCE;
+		fb_set_var(fbi, &fbvar);
+
+		fb_pos.x = vout->crop_current.left;
+		fb_pos.y = vout->crop_current.top;
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
+					     (unsigned long)&fb_pos);
+			set_fs(old_fs);
+		}
+
+		vout->display_bufs[1] = fbi->fix.smem_start;
+		vout->display_bufs[0] = fbi->fix.smem_start +
+		    (fbi->fix.line_length * fbi->var.yres);
+		vout->display_buf_size = vout->crop_current.width *
+		    vout->crop_current.height * fbi->var.bits_per_pixel / 8;
+
+		vout->post_proc_ch = MEM_PP_MEM;
+	}
+
+	/* Init PP */
+	if (use_direct_adc == false) {
+		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
+			out_width = vout->crop_current.height;
+			out_height = vout->crop_current.width;
+		}
+		memset(&params, 0, sizeof(params));
+		params.mem_pp_mem.in_width = vout->v2f.fmt.pix.width / 2;
+		params.mem_pp_mem.in_height = vout->v2f.fmt.pix.height;
+		params.mem_pp_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
+		params.mem_pp_mem.out_width = out_width / 2;
+		params.mem_pp_mem.out_height = out_height;
+		if (vout->display_ch == ADC_SYS2)
+			params.mem_pp_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
+		else
+			params.mem_pp_mem.out_pixel_fmt = bpp_to_fmt(fbi);
+		if (ipu_init_channel(vout->post_proc_ch, &params) != 0) {
+			dev_err(dev, "Error initializing PP channel\n");
+			return -EINVAL;
+		}
+
+		if (ipu_init_channel_buffer(vout->post_proc_ch,
+					    IPU_INPUT_BUFFER,
+					    params.mem_pp_mem.in_pixel_fmt,
+					    params.mem_pp_mem.in_width,
+					    params.mem_pp_mem.in_height,
+					    vout->v2f.fmt.pix.bytesperline /
+					    bytes_per_pixel(params.mem_pp_mem.
+							    in_pixel_fmt),
+					    IPU_ROTATE_NONE,
+					    vout->v4l2_bufs[pp_in_buf[0]].m.
+					    offset,
+					    vout->v4l2_bufs[pp_in_buf[0]].m.
+					    offset + params.mem_pp_mem.in_width,
+					    vout->offset.u_offset,
+					    vout->offset.v_offset) != 0) {
+			dev_err(dev, "Error initializing PP input buffer\n");
+			return -EINVAL;
+		}
+
+		if (!ipu_can_rotate_in_place(vout->rotate)) {
+			if (vout->rot_pp_bufs[0]) {
+				mxc_free_buffers(vout->rot_pp_bufs,
+						 vout->rot_pp_bufs_vaddr, 2,
+						 vout->display_buf_size);
+			}
+			if (mxc_allocate_buffers
+			    (vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
+			     vout->display_buf_size) < 0)
+				return -ENOBUFS;
+
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_OUTPUT_BUFFER,
+						    params.mem_pp_mem.
+						    out_pixel_fmt, out_width,
+						    out_height, out_width,
+						    IPU_ROTATE_NONE,
+						    vout->rot_pp_bufs[0],
+						    vout->rot_pp_bufs[1], 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing"
+					"PP output buffer\n");
+				return -EINVAL;
+			}
+
+			if (ipu_init_channel(MEM_ROT_PP_MEM, NULL) != 0) {
+				dev_err(dev,
+					"Error initializing PP ROT channel\n");
+				return -EINVAL;
+			}
+
+			if (ipu_init_channel_buffer(MEM_ROT_PP_MEM,
+						    IPU_INPUT_BUFFER,
+						    params.mem_pp_mem.
+						    out_pixel_fmt, out_width,
+						    out_height, out_width,
+						    vout->rotate,
+						    vout->rot_pp_bufs[0],
+						    vout->rot_pp_bufs[1], 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing PP ROT"
+					"input buffer\n");
+				return -EINVAL;
+			}
+
+			/* swap width and height */
+			if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
+				out_width = vout->crop_current.width;
+				out_height = vout->crop_current.height;
+			}
+
+			if (ipu_init_channel_buffer(MEM_ROT_PP_MEM,
+						    IPU_OUTPUT_BUFFER,
+						    params.mem_pp_mem.
+						    out_pixel_fmt, out_width,
+						    out_height, out_width,
+						    IPU_ROTATE_NONE,
+						    vout->display_bufs[0],
+						    vout->display_bufs[1], 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing PP"
+					"output buffer\n");
+				return -EINVAL;
+			}
+
+			if (ipu_link_channels(vout->post_proc_ch,
+					      MEM_ROT_PP_MEM) < 0)
+				return -EINVAL;
+
+			ipu_select_buffer(MEM_ROT_PP_MEM, IPU_OUTPUT_BUFFER, 0);
+			ipu_select_buffer(MEM_ROT_PP_MEM, IPU_OUTPUT_BUFFER, 1);
+
+			ipu_enable_channel(MEM_ROT_PP_MEM);
+
+			display_input_ch = MEM_ROT_PP_MEM;
+		} else {
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_OUTPUT_BUFFER,
+						    params.mem_pp_mem.
+						    out_pixel_fmt,
+						    out_width / 2,
+						    out_height,
+						    out_width,
+						    vout->rotate,
+						    vout->display_bufs[0],
+						    vout->display_bufs[0]
+						    +
+						    out_width / 2 *
+						    bytes_per_pixel
+						    (SDC_FG_FB_FORMAT), 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing PP"
+					"output buffer\n");
+				return -EINVAL;
+			}
+		}
+		if (ipu_unlink_channels(
+			display_input_ch, vout->display_ch) < 0) {
+			dev_err(dev, "Error linking ipu channels\n");
+			return -EINVAL;
+		}
+	}
+
+	vout->state = STATE_STREAM_PAUSED;
+
+	ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
+
+	if (use_direct_adc == false) {
+		ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 0);
+		ipu_enable_channel(vout->post_proc_ch);
+
+		if (fbi) {
+			acquire_console_sem();
+			fb_blank(fbi, FB_BLANK_UNBLANK);
+			release_console_sem();
+		} else {
+			ipu_enable_channel(vout->display_ch);
+		}
+	} else {
+		ipu_enable_channel(vout->post_proc_ch);
+	}
+
+	vout->start_jiffies = jiffies;
+	dev_dbg(dev,
+		"streamon: start time = %lu jiffies\n", vout->start_jiffies);
+
+	return 0;
+}
+
+/*!
+ * Shut down the voutera
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamoff(vout_data *vout)
+{
+	struct fb_info *fbi =
+	    registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	int i, retval = 0;
+	unsigned long lockflag = 0;
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state == STATE_STREAM_OFF)
+		return 0;
+
+	spin_lock_irqsave(&g_lock, lockflag);
+
+	del_timer(&vout->output_timer);
+
+	if (vout->state == STATE_STREAM_ON)
+		vout->state = STATE_STREAM_STOPPING;
+
+	ipu_disable_irq(IPU_IRQ_PP_IN_EOF);
+	ipu_disable_irq(IPU_IRQ_PP_OUT_EOF);
+
+	spin_unlock_irqrestore(&g_lock, lockflag);
+
+	if (vout->post_proc_ch == MEM_PP_MEM) {	/* SDC or ADC with Rotation */
+		if (!ipu_can_rotate_in_place(vout->rotate)) {
+			ipu_unlink_channels(MEM_PP_MEM, MEM_ROT_PP_MEM);
+			ipu_unlink_channels(MEM_ROT_PP_MEM, vout->display_ch);
+			ipu_disable_channel(MEM_ROT_PP_MEM, true);
+
+			if (vout->rot_pp_bufs[0]) {
+				mxc_free_buffers(vout->rot_pp_bufs,
+						 vout->rot_pp_bufs_vaddr, 2,
+						 vout->display_buf_size);
+			}
+		} else {
+			ipu_unlink_channels(MEM_PP_MEM, vout->display_ch);
+		}
+		ipu_disable_channel(MEM_PP_MEM, true);
+
+		if (vout->display_ch == ADC_SYS2) {
+			ipu_disable_channel(vout->display_ch, true);
+			ipu_uninit_channel(vout->display_ch);
+		} else {
+			fbi->var.activate |= FB_ACTIVATE_FORCE;
+			fb_set_var(fbi, &fbi->var);
+
+			if (vout->display_ch == MEM_FG_SYNC) {
+				acquire_console_sem();
+				fb_blank(fbi, FB_BLANK_POWERDOWN);
+				release_console_sem();
+			}
+
+			vout->display_bufs[0] = 0;
+			vout->display_bufs[1] = 0;
+		}
+
+		ipu_uninit_channel(MEM_PP_MEM);
+		if (!ipu_can_rotate_in_place(vout->rotate))
+			ipu_uninit_channel(MEM_ROT_PP_MEM);
+	} else {		/* ADC Direct */
+		ipu_disable_channel(MEM_PP_ADC, true);
+		ipu_uninit_channel(MEM_PP_ADC);
+	}
+	vout->ready_q.head = vout->ready_q.tail = 0;
+	vout->done_q.head = vout->done_q.tail = 0;
+	for (i = 0; i < vout->buffer_cnt; i++) {
+		vout->v4l2_bufs[i].flags = 0;
+		vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+		vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+	}
+
+	vout->state = STATE_STREAM_OFF;
+
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL
+	if (vout->cur_disp_output < DISP3) {
+		if (vout->display_bufs[0] != 0) {
+			mxc_free_buffers(vout->display_bufs,
+					 vout->display_bufs_vaddr, 2,
+					 vout->display_buf_size);
+		}
+
+		mxcfb_set_refresh_mode(registered_fb
+				       [vout->
+					output_fb_num[vout->cur_disp_output]],
+				       MXCFB_REFRESH_PARTIAL, 0);
+	}
+#endif
+
+	return retval;
+}
+
+/*
+ * Valid whether the palette is supported
+ *
+ * @param palette  V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return 1 if supported, 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	return ((palette == V4L2_PIX_FMT_RGB565) ||
+		(palette == V4L2_PIX_FMT_BGR24) ||
+		(palette == V4L2_PIX_FMT_RGB24) ||
+		(palette == V4L2_PIX_FMT_BGR32) ||
+		(palette == V4L2_PIX_FMT_RGB32) ||
+		(palette == V4L2_PIX_FMT_NV12) ||
+		(palette == V4L2_PIX_FMT_YUV422P) ||
+		(palette == V4L2_PIX_FMT_YUV420));
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_g_fmt(vout_data *vout, struct v4l2_format *f)
+{
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+	*f = vout->v2f;
+	return 0;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_s_fmt(vout_data *vout, struct v4l2_format *f)
+{
+	int retval = 0;
+	u32 size = 0;
+	u32 bytesperline;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		retval = -EINVAL;
+		goto err0;
+	}
+	if (!valid_mode(f->fmt.pix.pixelformat)) {
+		dev_err(vout->video_dev->dev, "pixel format not supported\n");
+		retval = -EINVAL;
+		goto err0;
+	}
+
+	bytesperline = (f->fmt.pix.width * fmt_to_bpp(f->fmt.pix.pixelformat)) /
+	    8;
+	if (f->fmt.pix.bytesperline < bytesperline) {
+		f->fmt.pix.bytesperline = bytesperline;
+	} else {
+		bytesperline = f->fmt.pix.bytesperline;
+	}
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUV422P:
+		/* byteperline for YUV planar formats is for
+		   Y plane only */
+		size = bytesperline * f->fmt.pix.height * 2;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_NV12:
+		size = (bytesperline * f->fmt.pix.height * 3) / 2;
+		break;
+	default:
+		size = bytesperline * f->fmt.pix.height;
+		break;
+	}
+
+	/* Return the actual size of the image to the app */
+	if (f->fmt.pix.sizeimage < size)
+		f->fmt.pix.sizeimage = size;
+	else
+		size = f->fmt.pix.sizeimage;
+
+	vout->v2f.fmt.pix = f->fmt.pix;
+	if (vout->v2f.fmt.pix.priv != 0) {
+		if (copy_from_user(&vout->offset,
+				   (void *)vout->v2f.fmt.pix.priv,
+				   sizeof(vout->offset))) {
+			retval = -EFAULT;
+			goto err0;
+		}
+	} else {
+		vout->offset.u_offset = 0;
+		vout->offset.v_offset = 0;
+	}
+
+	retval = 0;
+err0:
+	return retval;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_get_v42lout_control(vout_data *vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		return (vout->rotate & IPU_ROTATE_HORIZ_FLIP) ? 1 : 0;
+	case V4L2_CID_VFLIP:
+		return (vout->rotate & IPU_ROTATE_VERT_FLIP) ? 1 : 0;
+	case (V4L2_CID_PRIVATE_BASE + 1):
+		return vout->rotate;
+	default:
+		return -EINVAL;
+	}
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_set_v42lout_control(vout_data *vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		vout->rotate |= c->value ? IPU_ROTATE_HORIZ_FLIP :
+		    IPU_ROTATE_NONE;
+		break;
+	case V4L2_CID_VFLIP:
+		vout->rotate |= c->value ? IPU_ROTATE_VERT_FLIP :
+		    IPU_ROTATE_NONE;
+		break;
+	case V4L2_CID_MXC_ROT:
+		vout->rotate = c->value;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*!
+ * V4L2 interface - open function
+ *
+ * @param inode        structure inode *
+ *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l2out_open(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+	int err;
+
+	if (!vout)
+		return -ENODEV;
+
+	down(&vout->busy_lock);
+
+	err = -EINTR;
+	if (signal_pending(current))
+		goto oops;
+
+	if (vout->open_count++ == 0) {
+		ipu_request_irq(IPU_IRQ_PP_IN_EOF,
+				mxc_v4l2out_pp_in_irq_handler,
+				0, dev->name, vout);
+		ipu_request_irq(IPU_IRQ_PP_OUT_EOF,
+				mxc_v4l2out_pp_out_irq_handler,
+				0, dev->name, vout);
+
+		init_waitqueue_head(&vout->v4l_bufq);
+
+		init_timer(&vout->output_timer);
+		vout->output_timer.function = mxc_v4l2out_timer_handler;
+		vout->output_timer.data = (unsigned long)vout;
+
+		vout->state = STATE_STREAM_OFF;
+		vout->rotate = IPU_ROTATE_NONE;
+		g_irq_cnt = g_buf_output_cnt = g_buf_q_cnt = g_buf_dq_cnt = 0;
+
+	}
+
+	file->private_data = dev;
+
+	up(&vout->busy_lock);
+
+	return 0;
+
+oops:
+	up(&vout->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L2 interface - close function
+ *
+ * @param inode    struct inode *
+ *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int mxc_v4l2out_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+
+	if (--vout->open_count == 0) {
+		if (vout->state != STATE_STREAM_OFF)
+			mxc_v4l2out_streamoff(vout);
+
+		ipu_free_irq(IPU_IRQ_PP_IN_EOF, vout);
+		ipu_free_irq(IPU_IRQ_PP_OUT_EOF, vout);
+
+		file->private_data = NULL;
+
+		mxc_free_buffers(vout->queue_buf_paddr, vout->queue_buf_vaddr,
+				 vout->buffer_cnt, vout->queue_buf_size);
+		vout->buffer_cnt = 0;
+		mxc_free_buffers(vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
+				 vout->display_buf_size);
+
+		/* capture off */
+		wake_up_interruptible(&vout->v4l_bufq);
+	}
+
+	return 0;
+}
+
+/*!
+ * V4L2 interface - ioctl function
+ *
+ * @param inode      struct inode *
+ *
+ * @param file       struct file *
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void *
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int
+mxc_v4l2out_do_ioctl(struct inode *inode, struct file *file,
+		     unsigned int ioctlnr, void *arg)
+{
+	struct video_device *vdev = file->private_data;
+	vout_data *vout = video_get_drvdata(vdev);
+	int retval = 0;
+	int i = 0;
+
+	if (!vout)
+		return -EBADF;
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+	case VIDIOC_QUERYCAP:
+		{
+			struct v4l2_capability *cap = arg;
+			strcpy(cap->driver, "mxc_v4l2_output");
+			cap->version = 0;
+			cap->capabilities =
+			    V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+			cap->card[0] = '\0';
+			cap->bus_info[0] = '\0';
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_FMT:
+		{
+			struct v4l2_format *gf = arg;
+			retval = mxc_v4l2out_g_fmt(vout, gf);
+			break;
+		}
+	case VIDIOC_S_FMT:
+		{
+			struct v4l2_format *sf = arg;
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+			retval = mxc_v4l2out_s_fmt(vout, sf);
+			break;
+		}
+	case VIDIOC_REQBUFS:
+		{
+			struct v4l2_requestbuffers *req = arg;
+			if ((req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (req->memory != V4L2_MEMORY_MMAP)) {
+				dev_dbg(vdev->dev,
+					"VIDIOC_REQBUFS: incorrect"
+					"buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			if (req->count == 0)
+				mxc_v4l2out_streamoff(vout);
+
+			if (vout->state == STATE_STREAM_OFF) {
+				if (vout->queue_buf_paddr[0] != 0) {
+					mxc_free_buffers(vout->queue_buf_paddr,
+							 vout->queue_buf_vaddr,
+							 vout->buffer_cnt,
+							 vout->queue_buf_size);
+					dev_dbg(vdev->dev,
+						"VIDIOC_REQBUFS:"
+						"freed buffers\n");
+				}
+				vout->buffer_cnt = 0;
+			} else {
+				dev_dbg(vdev->dev,
+					"VIDIOC_REQBUFS: Buffer is in use\n");
+				retval = -EBUSY;
+				break;
+			}
+
+			if (req->count == 0)
+				break;
+
+			if (req->count < MIN_FRAME_NUM)
+				req->count = MIN_FRAME_NUM;
+			else if (req->count > MAX_FRAME_NUM)
+				req->count = MAX_FRAME_NUM;
+			vout->buffer_cnt = req->count;
+			vout->queue_buf_size =
+			    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+
+			retval = mxc_allocate_buffers(vout->queue_buf_paddr,
+						      vout->queue_buf_vaddr,
+						      vout->buffer_cnt,
+						      vout->queue_buf_size);
+			if (retval < 0)
+				break;
+
+			/* Init buffer queues */
+			vout->done_q.head = 0;
+			vout->done_q.tail = 0;
+			vout->ready_q.head = 0;
+			vout->ready_q.tail = 0;
+
+			for (i = 0; i < vout->buffer_cnt; i++) {
+				memset(&(vout->v4l2_bufs[i]), 0,
+				       sizeof(vout->v4l2_bufs[i]));
+				vout->v4l2_bufs[i].flags = 0;
+				vout->v4l2_bufs[i].memory = V4L2_MEMORY_MMAP;
+				vout->v4l2_bufs[i].index = i;
+				vout->v4l2_bufs[i].type =
+				    V4L2_BUF_TYPE_VIDEO_OUTPUT;
+				vout->v4l2_bufs[i].length =
+				    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+				vout->v4l2_bufs[i].m.offset =
+				    (unsigned long)vout->queue_buf_paddr[i];
+				vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+				vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+			}
+			break;
+		}
+	case VIDIOC_QUERYBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			u32 type = buf->type;
+			int index = buf->index;
+
+			if ((type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt)) {
+				dev_dbg(vdev->dev,
+					"VIDIOC_QUERYBUFS: incorrect"
+					"buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+			down(&vout->param_lock);
+			memcpy(buf, &(vout->v4l2_bufs[index]), sizeof(*buf));
+			up(&vout->param_lock);
+			break;
+		}
+	case VIDIOC_QBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int index = buf->index;
+			unsigned long lock_flags;
+
+			if ((buf->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			dev_dbg(vdev->dev, "VIDIOC_QBUF: %d\n", buf->index);
+
+			/* mmapped buffers are L1 WB cached,
+			 * so we need to clean them */
+			if (buf->flags & V4L2_BUF_FLAG_MAPPED)
+				flush_cache_all();
+
+			spin_lock_irqsave(&g_lock, lock_flags);
+
+			memcpy(&(vout->v4l2_bufs[index]), buf, sizeof(*buf));
+			vout->v4l2_bufs[index].flags |= V4L2_BUF_FLAG_QUEUED;
+
+			g_buf_q_cnt++;
+			queue_buf(&vout->ready_q, index);
+			if (vout->state == STATE_STREAM_PAUSED) {
+				unsigned long timeout;
+
+				index = peek_next_buf(&vout->ready_q);
+
+				/* if timestamp is 0, then default to 30fps */
+				if ((vout->v4l2_bufs[index].timestamp.tv_sec ==
+				     0)
+				    && (vout->v4l2_bufs[index].timestamp.
+					tv_usec == 0))
+					timeout =
+					    vout->start_jiffies +
+					    vout->frame_count * HZ / 30;
+				else
+					timeout =
+					    get_jiffies(&vout->v4l2_bufs[index].
+							timestamp);
+
+				if (jiffies >= timeout) {
+					dev_dbg(vout->video_dev->dev,
+						"warning: timer timeout"
+						"already expired.\n");
+				}
+				vout->output_timer.expires = timeout;
+				dev_dbg(vdev->dev,
+					"QBUF: frame #%u timeout @"
+					" %lu jiffies, current = %lu\n",
+					vout->frame_count, timeout, jiffies);
+				add_timer(&vout->output_timer);
+				vout->state = STATE_STREAM_ON;
+			}
+
+			spin_unlock_irqrestore(&g_lock, lock_flags);
+			break;
+		}
+	case VIDIOC_DQBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int idx;
+
+			if ((queue_size(&vout->done_q) == 0) &&
+			    (file->f_flags & O_NONBLOCK)) {
+				retval = -EAGAIN;
+				break;
+			}
+
+			if (!wait_event_interruptible_timeout(vout->v4l_bufq,
+							      queue_size(&vout->
+									 done_q)
+							      != 0, 10 * HZ)) {
+				dev_dbg(vdev->dev, "VIDIOC_DQBUF: timeout\n");
+				retval = -ETIME;
+				break;
+			} else if (signal_pending(current)) {
+				dev_dbg(vdev->dev,
+					"VIDIOC_DQBUF: interrupt received\n");
+				retval = -ERESTARTSYS;
+				break;
+			}
+			idx = dequeue_buf(&vout->done_q);
+			if (idx == -1) {	/* No frame free */
+				dev_dbg(vdev->dev,
+					"VIDIOC_DQBUF: no free buffers\n");
+				retval = -EAGAIN;
+				break;
+			}
+			if ((vout->v4l2_bufs[idx].flags & V4L2_BUF_FLAG_DONE) ==
+			    0)
+				dev_dbg(vdev->dev,
+					"VIDIOC_DQBUF: buffer in done q, "
+					"but not flagged as done\n");
+
+			vout->v4l2_bufs[idx].flags = 0;
+			memcpy(buf, &(vout->v4l2_bufs[idx]), sizeof(*buf));
+			dev_dbg(vdev->dev, "VIDIOC_DQBUF: %d\n", buf->index);
+			break;
+		}
+	case VIDIOC_STREAMON:
+		{
+			retval = mxc_v4l2out_streamon(vout);
+			break;
+		}
+	case VIDIOC_STREAMOFF:
+		{
+			retval = mxc_v4l2out_streamoff(vout);
+			break;
+		}
+	case VIDIOC_G_CTRL:
+		{
+			retval = mxc_get_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_S_CTRL:
+		{
+			retval = mxc_set_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_CROPCAP:
+		{
+			struct v4l2_cropcap *cap = arg;
+
+			if (cap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+
+			cap->bounds = vout->crop_bounds[vout->cur_disp_output];
+			cap->defrect = vout->crop_bounds[vout->cur_disp_output];
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			crop->c = vout->crop_current;
+			break;
+		}
+	case VIDIOC_S_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+			struct v4l2_rect *b =
+			    &(vout->crop_bounds[vout->cur_disp_output]);
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.height < 0) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.width < 0) {
+				retval = -EINVAL;
+				break;
+			}
+
+			/* only full screen supported for SDC BG */
+			if (vout->cur_disp_output == 4) {
+				crop->c = vout->crop_current;
+				break;
+			}
+
+			if (crop->c.top < b->top)
+				crop->c.top = b->top;
+			if (crop->c.top >= b->top + b->height)
+				crop->c.top = b->top + b->height - 1;
+			if (crop->c.height > b->top - crop->c.top + b->height)
+				crop->c.height =
+				    b->top - crop->c.top + b->height;
+
+			if (crop->c.left < b->left)
+				crop->c.left = b->left;
+			if (crop->c.left >= b->left + b->width)
+				crop->c.left = b->left + b->width - 1;
+			if (crop->c.width > b->left - crop->c.left + b->width)
+				crop->c.width =
+				    b->left - crop->c.left + b->width;
+
+			/* stride line limitation */
+			crop->c.height -= crop->c.height % 8;
+			crop->c.width -= crop->c.width % 8;
+
+			vout->crop_current = crop->c;
+			break;
+		}
+	case VIDIOC_ENUMOUTPUT:
+		{
+			struct v4l2_output *output = arg;
+
+			if ((output->index >= 5) ||
+			    (vout->output_enabled[output->index] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (output->index < 3) {
+				*output = mxc_outputs[MXC_V4L2_OUT_2_ADC];
+				output->name[4] = '0' + output->index;
+			} else {
+				*output = mxc_outputs[MXC_V4L2_OUT_2_SDC];
+			}
+			break;
+		}
+	case VIDIOC_G_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			*p_output_num = vout->cur_disp_output;
+			break;
+		}
+	case VIDIOC_S_OUTPUT:
+		{
+			int *p_output_num = arg;
+			int fbnum;
+			struct v4l2_rect *b;
+
+			if ((*p_output_num >= MXC_V4L2_OUT_NUM_OUTPUTS) ||
+			    (vout->output_enabled[*p_output_num] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+
+			vout->cur_disp_output = *p_output_num;
+
+			/* Update bounds in case they have changed */
+			b = &vout->crop_bounds[vout->cur_disp_output];
+
+			fbnum = vout->output_fb_num[vout->cur_disp_output];
+			if (vout->cur_disp_output == 3)
+				fbnum = vout->output_fb_num[4];
+
+			b->width = registered_fb[fbnum]->var.xres;
+			b->height = registered_fb[fbnum]->var.yres;
+
+			vout->crop_current = *b;
+			break;
+		}
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_G_PARM:
+	case VIDIOC_ENUMSTD:
+	case VIDIOC_G_STD:
+	case VIDIOC_S_STD:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	default:
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&vout->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L2 interface - ioctl function
+ *
+ * @return  None
+ */
+static int
+mxc_v4l2out_ioctl(struct inode *inode, struct file *file,
+		  unsigned int cmd, unsigned long arg)
+{
+	return video_usercopy(inode, file, cmd, arg, mxc_v4l2out_do_ioctl);
+}
+
+/*!
+ * V4L2 interface - mmap function
+ *
+ * @param file          structure file *
+ *
+ * @param vma           structure vm_area_struct *
+ *
+ * @return status       0 Success, EINTR busy lock error,
+ *                      ENOBUFS remap_page error
+ */
+static int mxc_v4l2out_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *vdev = video_devdata(file);
+	unsigned long size = vma->vm_end - vma->vm_start;
+	int res = 0;
+	int i;
+	vout_data *vout = video_get_drvdata(vdev);
+
+	dev_dbg(vdev->dev, "pgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	for (i = 0; i < vout->buffer_cnt; i++) {
+		if ((vout->v4l2_bufs[i].m.offset ==
+		     (vma->vm_pgoff << PAGE_SHIFT)) &&
+		    (vout->v4l2_bufs[i].length >= size)) {
+			vout->v4l2_bufs[i].flags |= V4L2_BUF_FLAG_MAPPED;
+			break;
+		}
+	}
+	if (i == vout->buffer_cnt) {
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	/* make buffers inner write-back, outer write-thru cacheable */
+	vma->vm_page_prot = pgprot_outer_wrthru(vma->vm_page_prot);
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		dev_dbg(vdev->dev, "mmap remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+mxc_mmap_exit:
+	up(&vout->busy_lock);
+	return res;
+}
+
+/*!
+ * V4L2 interface - poll function
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_v4l2out_poll(struct file *file, poll_table * wait)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	queue = &vout->v4l_bufq;
+	poll_wait(file, queue, wait);
+
+	up(&vout->busy_lock);
+	return res;
+}
+
+static struct
+file_operations mxc_v4l2out_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_v4l2out_open,
+	.release = mxc_v4l2out_close,
+	.ioctl = mxc_v4l2out_ioctl,
+	.mmap = mxc_v4l2out_mmap,
+	.poll = mxc_v4l2out_poll,
+};
+
+static struct video_device mxc_v4l2out_template = {
+	.owner = THIS_MODULE,
+	.name = "MXC Video Output",
+	.type = 0,
+	.type2 = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING,
+	.fops = &mxc_v4l2out_fops,
+	.release = video_device_release,
+};
+
+/*!
+ * Probe routine for the framebuffer driver. It is called during the
+ * driver binding process.      The following functions are performed in
+ * this routine: Framebuffer initialization, Memory allocation and
+ * mapping, Framebuffer registration, IPU initialization.
+ *
+ * @return      Appropriate error code to the kernel common code
+ */
+static int mxc_v4l2out_probe(struct platform_device *pdev)
+{
+	int i;
+	vout_data *vout;
+
+	/*
+	 * Allocate sufficient memory for the fb structure
+	 */
+	g_vout = vout = kmalloc(sizeof(vout_data), GFP_KERNEL);
+
+	if (!vout)
+		return 0;
+
+	memset(vout, 0, sizeof(vout_data));
+
+	vout->video_dev = video_device_alloc();
+	if (vout->video_dev == NULL)
+		return -1;
+	vout->video_dev->dev = &pdev->dev;
+	vout->video_dev->minor = -1;
+
+	*(vout->video_dev) = mxc_v4l2out_template;
+
+	/* register v4l device */
+	if (video_register_device(vout->video_dev,
+				  VFL_TYPE_GRABBER, video_nr) == -1) {
+		dev_dbg(&pdev->dev, "video_register_device failed\n");
+		return 0;
+	}
+	dev_info(&pdev->dev, "Registered device video%d\n",
+		 vout->video_dev->minor & 0x1f);
+	vout->video_dev->dev = &pdev->dev;
+
+	video_set_drvdata(vout->video_dev, vout);
+
+	init_MUTEX(&vout->param_lock);
+	init_MUTEX(&vout->busy_lock);
+
+	/* setup outputs and cropping */
+	vout->cur_disp_output = -1;
+	for (i = 0; i < num_registered_fb; i++) {
+		char *idstr = registered_fb[i]->fix.id;
+		if (strncmp(idstr, "DISP", 4) == 0) {
+			int disp_num = idstr[4] - '0';
+			if (disp_num == 3) {
+				if (strcmp(idstr, "DISP3 BG - DI1") == 0)
+					disp_num = 5;
+				else if (strncmp(idstr, "DISP3 BG", 8) == 0)
+					disp_num = 4;
+			}
+			vout->crop_bounds[disp_num].left = 0;
+			vout->crop_bounds[disp_num].top = 0;
+			vout->crop_bounds[disp_num].width =
+			    registered_fb[i]->var.xres;
+			vout->crop_bounds[disp_num].height =
+			    registered_fb[i]->var.yres;
+			vout->output_enabled[disp_num] = true;
+			vout->output_fb_num[disp_num] = i;
+			if (vout->cur_disp_output == -1)
+				vout->cur_disp_output = disp_num;
+		}
+
+	}
+	vout->crop_current = vout->crop_bounds[vout->cur_disp_output];
+
+	platform_set_drvdata(pdev, vout);
+
+	return 0;
+}
+
+static int mxc_v4l2out_remove(struct platform_device *pdev)
+{
+	vout_data *vout = platform_get_drvdata(pdev);
+
+	if (vout->video_dev) {
+		if (-1 != vout->video_dev->minor)
+			video_unregister_device(vout->video_dev);
+		else
+			video_device_release(vout->video_dev);
+		vout->video_dev = NULL;
+	}
+
+	platform_set_drvdata(pdev, NULL);
+
+	kfree(vout);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_v4l2out_driver = {
+	.driver = {
+		   .name = "MXC Video Output",
+		   },
+	.probe = mxc_v4l2out_probe,
+	.remove = mxc_v4l2out_remove,
+};
+
+static struct platform_device mxc_v4l2out_device = {
+	.name = "MXC Video Output",
+	.id = 0,
+};
+
+/*!
+ * mxc v4l2 init function
+ *
+ */
+static int mxc_v4l2out_init(void)
+{
+	u8 err = 0;
+
+	err = platform_driver_register(&mxc_v4l2out_driver);
+	if (err == 0)
+		platform_device_register(&mxc_v4l2out_device);
+	return err;
+}
+
+/*!
+ * mxc v4l2 cleanup function
+ *
+ */
+static void mxc_v4l2out_clean(void)
+{
+	video_unregister_device(g_vout->video_dev);
+
+	platform_driver_unregister(&mxc_v4l2out_driver);
+	platform_device_unregister(&mxc_v4l2out_device);
+	kfree(g_vout);
+	g_vout = NULL;
+}
+
+module_init(mxc_v4l2out_init);
+module_exit(mxc_v4l2out_clean);
+
+module_param(video_nr, int, 0444);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2-driver for MXC video output");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/output/mxc_v4l2_output.c b/drivers/media/video/mxc/output/mxc_v4l2_output.c
new file mode 100644
index 000000000000..9254db5980c2
--- /dev/null
+++ b/drivers/media/video/mxc/output/mxc_v4l2_output.c
@@ -0,0 +1,2602 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/media/video/mxc/output/mxc_v4l2_output.c
+ *
+ * @brief MXC V4L2 Video Output Driver
+ *
+ * Video4Linux2 Output Device using MXC IPU Post-processing functionality.
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/semaphore.h>
+#include <linux/console.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/mxcfb.h>
+#include <media/v4l2-ioctl.h>
+#include <asm/cacheflush.h>
+
+#include "mxc_v4l2_output.h"
+
+vout_data *g_vout;
+#define INTERLACED_CONTENT(vout) ((cpu_is_mx51_rev(CHIP_REV_2_0) >= 1) && \
+				  (((vout)->field_fmt == V4L2_FIELD_INTERLACED_TB) || \
+				   ((vout)->field_fmt == V4L2_FIELD_INTERLACED_BT)))
+#define LOAD_3FIELDS(vout) ((INTERLACED_CONTENT(vout)) && \
+			    ((vout)->motion_sel != HIGH_MOTION))
+
+#define SDC_FG_FB_FORMAT        IPU_PIX_FMT_RGB565
+
+struct v4l2_output mxc_outputs[2] = {
+	{
+	 .index = MXC_V4L2_OUT_2_SDC,
+	 .name = "DISP3 Video Out",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
+						   but no other choice */
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN},
+	{
+	 .index = MXC_V4L2_OUT_2_ADC,
+	 .name = "DISPx Video Out",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
+						   but no other choice */
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN}
+};
+
+static int video_nr = 16;
+static int pending_buffer;
+static int pp_eof;
+static spinlock_t g_lock = SPIN_LOCK_UNLOCKED;
+static int last_index_n;
+static int last_index_c;
+
+/* debug counters */
+uint32_t g_irq_cnt;
+uint32_t g_buf_output_cnt;
+uint32_t g_buf_q_cnt;
+uint32_t g_buf_dq_cnt;
+
+#define QUEUE_SIZE (MAX_FRAME_NUM + 1)
+static __inline int queue_size(v4l_queue * q)
+{
+	if (q->tail >= q->head)
+		return (q->tail - q->head);
+	else
+		return ((q->tail + QUEUE_SIZE) - q->head);
+}
+
+static __inline int queue_buf(v4l_queue * q, int idx)
+{
+	if (((q->tail + 1) % QUEUE_SIZE) == q->head)
+		return -1;	/* queue full */
+	q->list[q->tail] = idx;
+	q->tail = (q->tail + 1) % QUEUE_SIZE;
+	return 0;
+}
+
+static __inline int dequeue_buf(v4l_queue * q)
+{
+	int ret;
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	ret = q->list[q->head];
+	q->head = (q->head + 1) % QUEUE_SIZE;
+	return ret;
+}
+
+static __inline int peek_next_buf(v4l_queue * q)
+{
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	return q->list[q->head];
+}
+
+static __inline unsigned long get_jiffies(struct timeval *t)
+{
+	struct timeval cur;
+
+	if (t->tv_usec >= 1000000) {
+		t->tv_sec += t->tv_usec / 1000000;
+		t->tv_usec = t->tv_usec % 1000000;
+	}
+
+	do_gettimeofday(&cur);
+	if ((t->tv_sec < cur.tv_sec)
+	    || ((t->tv_sec == cur.tv_sec) && (t->tv_usec < cur.tv_usec)))
+		return jiffies;
+
+	if (t->tv_usec < cur.tv_usec) {
+		cur.tv_sec = t->tv_sec - cur.tv_sec - 1;
+		cur.tv_usec = t->tv_usec + 1000000 - cur.tv_usec;
+	} else {
+		cur.tv_sec = t->tv_sec - cur.tv_sec;
+		cur.tv_usec = t->tv_usec - cur.tv_usec;
+	}
+
+	return jiffies + timeval_to_jiffies(&cur);
+}
+
+/*!
+ * Private function to free buffers
+ *
+ * @param bufs_paddr	Array of physical address of buffers to be freed
+ *
+ * @param bufs_vaddr	Array of virtual address of buffers to be freed
+ *
+ * @param num_buf	Number of buffers to be freed
+ *
+ * @param size		Size for each buffer to be free
+ *
+ * @return status  0 success.
+ */
+static int mxc_free_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+			    int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		if (bufs_vaddr[i] != 0) {
+			dma_free_coherent(0, size, bufs_vaddr[i],
+					  bufs_paddr[i]);
+			pr_debug("freed @ paddr=0x%08X\n", (u32) bufs_paddr[i]);
+			bufs_paddr[i] = 0;
+			bufs_vaddr[i] = NULL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Private function to allocate buffers
+ *
+ * @param bufs_paddr	Output array of physical address of buffers allocated
+ *
+ * @param bufs_vaddr	Output array of virtual address of buffers allocated
+ *
+ * @param num_buf	Input number of buffers to allocate
+ *
+ * @param size		Input size for each buffer to allocate
+ *
+ * @return status	-0 Successfully allocated a buffer, -ENOBUFS failed.
+ */
+static int mxc_allocate_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+				int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		bufs_vaddr[i] = dma_alloc_coherent(0, size,
+						   &bufs_paddr[i],
+						   GFP_DMA | GFP_KERNEL);
+
+		if (bufs_vaddr[i] == 0) {
+			mxc_free_buffers(bufs_paddr, bufs_vaddr, i, size);
+			printk(KERN_ERR "dma_alloc_coherent failed.\n");
+			return -ENOBUFS;
+		}
+		pr_debug("allocated @ paddr=0x%08X, size=%d.\n",
+			 (u32) bufs_paddr[i], size);
+	}
+
+	return 0;
+}
+
+/*
+ * Returns bits per pixel for given pixel format
+ *
+ * @param pixelformat  V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return bits per pixel of pixelformat
+ */
+static u32 fmt_to_bpp(u32 pixelformat)
+{
+	u32 bpp;
+
+	bpp = 8 * bytes_per_pixel(pixelformat);
+	return bpp;
+}
+
+
+
+static bool format_is_yuv(u32 pixelformat)
+{
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YUV422P:
+	case V4L2_PIX_FMT_YVU420:
+	case V4L2_PIX_FMT_NV12:
+		return true;
+		break;
+	}
+	return false;
+}
+
+static u32 bpp_to_fmt(struct fb_info *fbi)
+{
+	if (fbi->var.nonstd)
+		return fbi->var.nonstd;
+
+	if (fbi->var.bits_per_pixel == 24)
+		return V4L2_PIX_FMT_BGR24;
+	else if (fbi->var.bits_per_pixel == 32)
+		return V4L2_PIX_FMT_BGR32;
+	else if (fbi->var.bits_per_pixel == 16)
+		return V4L2_PIX_FMT_RGB565;
+
+	return 0;
+}
+
+static irqreturn_t mxc_v4l2out_disp_refresh_irq_handler(int irq, void *dev_id)
+{
+	vout_data *vout = dev_id;
+	int index, last_buf, ret;
+	unsigned long timeout;
+	unsigned long lock_flags = 0;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	g_irq_cnt++;
+
+	if (vout->ic_bypass && (pending_buffer || vout->frame_count < 3)) {
+		last_buf = vout->ipu_buf[vout->next_done_ipu_buf];
+		if (last_buf != -1) {
+			g_buf_output_cnt++;
+			vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
+			queue_buf(&vout->done_q, last_buf);
+			vout->ipu_buf[vout->next_done_ipu_buf] = -1;
+			wake_up_interruptible(&vout->v4l_bufq);
+			vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+		}
+	}
+
+	if ((pending_buffer) && (pp_eof || vout->ic_bypass)) {
+		pp_eof = 0;
+		if (vout->ic_bypass) {
+			ret = ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER,
+					  vout->next_rdy_ipu_buf);
+		} else {
+			if (LOAD_3FIELDS(vout)) {
+				ret = ipu_select_multi_vdi_buffer(vout->next_rdy_ipu_buf);
+			} else {
+				ret = ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+							vout->next_rdy_ipu_buf);
+			}
+		}
+		if (ret < 0) {
+			dev_err(&vout->video_dev->dev,
+				"unable to set IPU buffer ready\n");
+		}
+		/* Non IC split action */
+		if (!vout->pp_split)
+			vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
+
+		pending_buffer = 0;
+
+		/* Setup timer for next buffer */
+		index = peek_next_buf(&vout->ready_q);
+		if (index != -1) {
+			/* if timestamp is 0, then default to 30fps */
+			if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+				&& (vout->v4l2_bufs[index].timestamp.tv_usec == 0)
+				&& vout->start_jiffies)
+				timeout =
+					vout->start_jiffies + vout->frame_count * HZ / 30;
+			else
+				timeout =
+					get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+			if (jiffies >= timeout) {
+				dev_dbg(&vout->video_dev->dev,
+					"warning: timer timeout already expired.\n");
+			}
+			if (mod_timer(&vout->output_timer, timeout))
+				dev_dbg(&vout->video_dev->dev,
+					"warning: timer was already set\n");
+
+			dev_dbg(&vout->video_dev->dev,
+				"timer handler next schedule: %lu\n", timeout);
+		} else {
+			vout->state = STATE_STREAM_PAUSED;
+		}
+	}
+
+	if (vout->state == STATE_STREAM_STOPPING) {
+		if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
+			vout->state = STATE_STREAM_OFF;
+		}
+	}
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+
+	return IRQ_HANDLED;
+}
+
+static int get_display_irq(vout_data *vout)
+{
+
+	int disp_irq = 0;
+
+	switch (vout->display_ch) {
+	case MEM_FG_SYNC:
+	case MEM_BG_SYNC:
+		disp_irq = IPU_IRQ_BG_SF_END;
+		break;
+	case MEM_DC_SYNC:
+		disp_irq = IPU_IRQ_DC_FC_1;
+		break;
+	default:
+		dev_err(&vout->video_dev->dev,
+			"not support display channel\n");
+	}
+
+	return disp_irq;
+}
+
+static void mxc_v4l2out_timer_handler(unsigned long arg)
+{
+	int index, ret;
+	unsigned long lock_flags = 0;
+	vout_data *vout = (vout_data *) arg;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	if ((vout->state == STATE_STREAM_STOPPING)
+	    || (vout->state == STATE_STREAM_OFF))
+		goto exit0;
+	/*
+	 * If timer occurs before IPU h/w is ready, then set the state to
+	 * paused and the timer will be set again when next buffer is queued
+	 * or PP comletes
+	 */
+	if (vout->ipu_buf[vout->next_rdy_ipu_buf] != -1) {
+		dev_dbg(&vout->video_dev->dev, "IPU buffer busy\n");
+		vout->state = STATE_STREAM_PAUSED;
+		goto exit0;
+	}
+
+	/* Dequeue buffer and pass to IPU */
+	unsigned int aid_field_offset, current_field_offset;
+	if (INTERLACED_CONTENT(vout)) {
+		if (((LOAD_3FIELDS(vout)) && (vout->next_rdy_ipu_buf)) ||
+		    ((!LOAD_3FIELDS(vout)) && !(vout->next_rdy_ipu_buf))) {
+			aid_field_offset = vout->bytesperline;
+			current_field_offset = 0;
+			index = last_index_n;
+		} else {
+			aid_field_offset = 0;
+			current_field_offset = vout->bytesperline;
+			index = dequeue_buf(&vout->ready_q);
+			if (index == -1) {	/* no buffers ready, should never occur */
+				dev_err(&vout->video_dev->dev,
+					"mxc_v4l2out: timer - no queued buffers ready\n");
+				goto exit0;
+			}
+			g_buf_dq_cnt++;
+			vout->frame_count++;
+			last_index_n = index;
+		}
+	} else {
+		current_field_offset = 0;
+		index = dequeue_buf(&vout->ready_q);
+		if (index == -1) {	/* no buffers ready, should never occur */
+			dev_err(&vout->video_dev->dev,
+				"mxc_v4l2out: timer - no queued buffers ready\n");
+			goto exit0;
+		}
+		g_buf_dq_cnt++;
+		vout->frame_count++;
+	}
+
+	if (vout->ic_bypass) {
+		vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
+		ret = ipu_update_channel_buffer(vout->display_ch, IPU_INPUT_BUFFER,
+				      vout->next_rdy_ipu_buf,
+				      vout->v4l2_bufs[index].m.offset);
+	} else {
+		if (LOAD_3FIELDS(vout)) {
+			int index_n = index;
+			int index_p = last_index_c;
+			index = last_index_n;
+			vout->ipu_buf_p[vout->next_rdy_ipu_buf] = index_p;
+			vout->ipu_buf[vout->next_rdy_ipu_buf] = last_index_c = index;
+			vout->ipu_buf_n[vout->next_rdy_ipu_buf] = last_index_n = index_n;
+			last_index_n = vout->ipu_buf_n[vout->next_rdy_ipu_buf];
+			last_index_c = vout->ipu_buf[vout->next_rdy_ipu_buf];
+			ret = ipu_update_channel_buffer(vout->post_proc_ch,
+							IPU_INPUT_BUFFER,
+							vout->next_rdy_ipu_buf,
+							vout->v4l2_bufs[index].m.offset+current_field_offset);
+			ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_P,
+							 IPU_INPUT_BUFFER,
+							 vout->next_rdy_ipu_buf,
+							 vout->v4l2_bufs[index_p].m.offset+aid_field_offset);
+			ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_N,
+							 IPU_INPUT_BUFFER,
+							 vout->next_rdy_ipu_buf,
+							 vout->v4l2_bufs[index_n].m.offset+aid_field_offset);
+		} else {
+			vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
+			if (vout->pp_split) {
+				vout->ipu_buf[!vout->next_rdy_ipu_buf] = index;
+				/* always left stripe */
+				ret = ipu_update_channel_buffer(vout->post_proc_ch,
+									IPU_INPUT_BUFFER,
+									0,/* vout->next_rdy_ipu_buf,*/
+									(vout->v4l2_bufs[index].m.offset) +
+									vout->pp_left_stripe.input_column +
+									current_field_offset);
+
+				/* the U/V offset has to be updated inside of IDMAC */
+				/* according to stripe offset */
+				ret += ipu_update_channel_offset(vout->post_proc_ch,
+									IPU_INPUT_BUFFER,
+									vout->v2f.fmt.pix.pixelformat,
+									vout->v2f.fmt.pix.width,
+									vout->v2f.fmt.pix.height,
+									vout->v2f.fmt.pix.width,
+									vout->offset.u_offset,
+									vout->offset.v_offset,
+									0,
+									vout->pp_left_stripe.input_column + current_field_offset);
+
+			} else
+				ret = ipu_update_channel_buffer(vout->post_proc_ch,
+									IPU_INPUT_BUFFER,
+									vout->next_rdy_ipu_buf,
+									vout->v4l2_bufs[index].m.offset +
+									current_field_offset);
+		}
+	}
+	if (ret < 0) {
+		dev_err(&vout->video_dev->dev,
+			"unable to update buffer %d address rc=%d\n",
+			vout->next_rdy_ipu_buf, ret);
+		goto exit0;
+	}
+
+	pending_buffer = 1;
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+
+	return;
+
+      exit0:
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+}
+
+static irqreturn_t mxc_v4l2out_pp_in_irq_handler(int irq, void *dev_id)
+{
+	int last_buf;
+	int index;
+	unsigned long timeout;
+	unsigned long lock_flags = 0;
+	vout_data *vout = dev_id;
+	int pp_out_buf_num = 0;
+	int disp_buf_num = 0;
+	int disp_buf_num_next = 1;
+	int pp_out_buf_offset = 0;
+	int release_buffer = 1;
+	int eba_offset;
+	int ret = -1;
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	g_irq_cnt++;
+
+	/* Process previous buffer */
+	if (LOAD_3FIELDS(vout))
+		last_buf = vout->ipu_buf_p[vout->next_done_ipu_buf];
+	else
+		last_buf = vout->ipu_buf[vout->next_done_ipu_buf];
+
+	/* If IC split mode on, update output buffer number */
+	if (last_buf != -1) {
+		if (vout->pp_split) {
+			pp_out_buf_num = vout->pp_split_buf_num & 1;/* left/right stripe */
+			disp_buf_num = vout->pp_split_buf_num >> 1;
+			disp_buf_num_next = ((vout->pp_split_buf_num+2) & 3) >> 1;
+			if (!pp_out_buf_num) {/* next buffer is right stripe*/
+				eba_offset = vout->pp_right_stripe.input_column;/*always right stripe*/
+				ret = ipu_update_channel_buffer(vout->post_proc_ch,
+						IPU_INPUT_BUFFER,
+						1, /* right stripe */
+						(vout->v4l2_bufs[vout->ipu_buf[disp_buf_num]].m.offset)
+						+ eba_offset);
+
+				ret += ipu_update_channel_offset(vout->post_proc_ch,
+								IPU_INPUT_BUFFER,
+								vout->v2f.fmt.pix.pixelformat,
+								vout->v2f.fmt.pix.width,
+								vout->v2f.fmt.pix.height,
+								vout->v2f.fmt.pix.width,
+								vout->offset.u_offset,
+								vout->offset.v_offset,
+								0,
+								vout->pp_right_stripe.input_column);
+
+				/* select right stripe */
+				ret += ipu_select_buffer(vout->post_proc_ch,
+												IPU_INPUT_BUFFER, 1);
+				if (ret < 0)
+					dev_err(&vout->video_dev->dev,
+					"unable to set IPU buffer ready\n");
+
+				vout->ipu_buf[vout->next_done_ipu_buf] = -1;
+				vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+
+			} else /* right stripe is done, run display refresh */
+				ret = ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER,
+								disp_buf_num);
+
+			vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
+
+			/* offset for next buffer's EBA */
+			pp_out_buf_offset = pp_out_buf_num ? vout->pp_right_stripe.output_column :
+												vout->pp_left_stripe.output_column;
+
+			/* next buffer update */
+			eba_offset = vout->display_bufs[disp_buf_num_next] +
+								pp_out_buf_offset;
+
+			ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
+												pp_out_buf_num, eba_offset);
+
+			/* next buffer ready */
+			ret = ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, pp_out_buf_num);
+
+			/* next stripe_buffer index 0..3 */
+			vout->pp_split_buf_num = (vout->pp_split_buf_num + 1) & 3;
+		}
+
+		/* release buffer if second stripe is done */
+		release_buffer = vout->pp_split ? pp_out_buf_num : 1;
+		if (release_buffer) {
+			if ((!INTERLACED_CONTENT(vout)) || (vout->next_done_ipu_buf)) {
+				g_buf_output_cnt++;
+				vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
+				queue_buf(&vout->done_q, last_buf);
+				wake_up_interruptible(&vout->v4l_bufq);
+			}
+			vout->ipu_buf[vout->next_done_ipu_buf] = -1;
+			if (LOAD_3FIELDS(vout)) {
+				vout->ipu_buf_p[vout->next_done_ipu_buf] = -1;
+				vout->ipu_buf_n[vout->next_done_ipu_buf] = -1;
+			}
+			vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+		}
+	} /* end of last_buf != -1 */
+
+	if (release_buffer)
+		pp_eof = 1;
+
+	if (vout->state == STATE_STREAM_STOPPING) {
+		if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
+			vout->state = STATE_STREAM_OFF;
+		}
+	} else if ((vout->state == STATE_STREAM_PAUSED)
+		   && ((index = peek_next_buf(&vout->ready_q)) != -1)) {
+		/* Setup timer for next buffer, when stream has been paused */
+		pr_debug("next index %d\n", index);
+
+		/* if timestamp is 0, then default to 30fps */
+		if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+		    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+			timeout =
+			    vout->start_jiffies + vout->frame_count * HZ / 30;
+		else
+			timeout =
+			    get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+		if (jiffies >= timeout) {
+			pr_debug("warning: timer timeout already expired.\n");
+		}
+
+		vout->state = STATE_STREAM_ON;
+
+		if (mod_timer(&vout->output_timer, timeout))
+			pr_debug("warning: timer was already set\n");
+
+		pr_debug("timer handler next schedule: %lu\n", timeout);
+	}
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ *  Initialize VDI channels
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int init_VDI_channel(vout_data *vout, ipu_channel_params_t params)
+{
+	struct device *dev = &vout->video_dev->dev;
+
+	if (ipu_init_channel(MEM_VDI_PRP_VF_MEM, &params) != 0) {
+		dev_dbg(dev, "Error initializing VDI current channel\n");
+		return -EINVAL;
+	}
+	if (LOAD_3FIELDS(vout)) {
+		if (ipu_init_channel(MEM_VDI_PRP_VF_MEM_P, &params) != 0) {
+			dev_err(dev, "Error initializing VDI previous channel\n");
+			return -EINVAL;
+		}
+		if (ipu_init_channel(MEM_VDI_PRP_VF_MEM_N, &params) != 0) {
+			dev_err(dev, "Error initializing VDI next channel\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Initialize VDI channel buffers
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int init_VDI_in_channel_buffer(vout_data *vout, uint32_t in_pixel_fmt,
+				   uint16_t in_width, uint16_t in_height,
+				   uint32_t stride,
+				   dma_addr_t phyaddr_0, dma_addr_t phyaddr_1,
+				   uint32_t u_offset, uint32_t v_offset)
+{
+	struct device *dev = &vout->video_dev->dev;
+
+	if (ipu_init_channel_buffer(MEM_VDI_PRP_VF_MEM, IPU_INPUT_BUFFER,
+				    in_pixel_fmt, in_width, in_height, stride,
+				    IPU_ROTATE_NONE,
+				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset+vout->bytesperline,
+				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
+				    u_offset, v_offset) != 0) {
+		dev_err(dev, "Error initializing VDI current input buffer\n");
+		return -EINVAL;
+	}
+	if (LOAD_3FIELDS(vout)) {
+		if (ipu_init_channel_buffer(MEM_VDI_PRP_VF_MEM_P,
+					    IPU_INPUT_BUFFER,
+					    in_pixel_fmt, in_width, in_height,
+					    stride, IPU_ROTATE_NONE,
+					    vout->v4l2_bufs[vout->ipu_buf_p[0]].m.offset,
+					    vout->v4l2_bufs[vout->ipu_buf_p[0]].m.offset+vout->bytesperline,
+					    u_offset, v_offset) != 0) {
+			dev_err(dev, "Error initializing VDI previous input buffer\n");
+			return -EINVAL;
+		}
+		if (ipu_init_channel_buffer(MEM_VDI_PRP_VF_MEM_N,
+					    IPU_INPUT_BUFFER,
+					    in_pixel_fmt, in_width, in_height,
+					    stride, IPU_ROTATE_NONE,
+					    vout->v4l2_bufs[vout->ipu_buf_n[0]].m.offset,
+					    vout->v4l2_bufs[vout->ipu_buf_n[0]].m.offset+vout->bytesperline,
+					    u_offset, v_offset) != 0) {
+			dev_err(dev, "Error initializing VDI next input buffer\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Initialize VDI path
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int init_VDI(ipu_channel_params_t params, vout_data *vout,
+			struct device *dev, struct fb_info *fbi,
+			ipu_channel_t  *display_input_ch, u16 out_width,
+			u16 out_height)
+{
+	params.mem_prp_vf_mem.in_width = vout->v2f.fmt.pix.width;
+	params.mem_prp_vf_mem.in_height = vout->v2f.fmt.pix.height;
+	params.mem_prp_vf_mem.motion_sel = vout->motion_sel;
+	params.mem_prp_vf_mem.field_fmt = vout->field_fmt;
+	params.mem_prp_vf_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
+	params.mem_prp_vf_mem.out_width = out_width;
+	params.mem_prp_vf_mem.out_height = out_height;
+	if (vout->display_ch == ADC_SYS2)
+		params.mem_prp_vf_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
+	else
+		params.mem_prp_vf_mem.out_pixel_fmt = bpp_to_fmt(fbi);
+
+	if (init_VDI_channel(vout, params) != 0) {
+		dev_err(dev, "Error init_VDI_channel channel\n");
+		return -EINVAL;
+	}
+
+
+	if (init_VDI_in_channel_buffer(vout,
+				       params.mem_prp_vf_mem.in_pixel_fmt,
+				       params.mem_prp_vf_mem.in_width,
+				       params.mem_prp_vf_mem.in_height,
+				       bytes_per_pixel(params.mem_prp_vf_mem.
+						       in_pixel_fmt),
+				       vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
+				       vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
+				       vout->offset.u_offset,
+				       vout->offset.v_offset) != 0) {
+		return -EINVAL;
+	}
+
+	if (!ipu_can_rotate_in_place(vout->rotate)) {
+		if (vout->rot_pp_bufs[0]) {
+			mxc_free_buffers(vout->rot_pp_bufs,
+					 vout->rot_pp_bufs_vaddr, 2,
+					 vout->display_buf_size);
+		}
+		if (mxc_allocate_buffers
+		    (vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
+		     vout->display_buf_size) < 0) {
+			return -ENOBUFS;
+		}
+
+		if (ipu_init_channel_buffer(vout->post_proc_ch,
+					    IPU_OUTPUT_BUFFER,
+					    params.mem_prp_vf_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    IPU_ROTATE_NONE,
+					    vout->rot_pp_bufs[0],
+					    vout->rot_pp_bufs[1], 0, 0) != 0) {
+			dev_err(dev, "Error initializing PRP output buffer\n");
+			return -EINVAL;
+		}
+
+		if (ipu_init_channel(MEM_ROT_VF_MEM, NULL) != 0) {
+			dev_err(dev, "Error initializing PP ROT channel\n");
+			return -EINVAL;
+		}
+		if (ipu_init_channel_buffer(MEM_ROT_VF_MEM,
+					    IPU_INPUT_BUFFER,
+					    params.mem_prp_vf_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    vout->rotate,
+					    vout->rot_pp_bufs[0],
+					    vout->rot_pp_bufs[1], 0, 0) != 0) {
+			dev_err(dev,
+				"Error initializing PP ROT input buffer\n");
+			return -EINVAL;
+		}
+
+		/* swap width and height */
+		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
+			out_width = vout->crop_current.width;
+			out_height = vout->crop_current.height;
+		}
+
+		if (ipu_init_channel_buffer(MEM_ROT_VF_MEM,
+					    IPU_OUTPUT_BUFFER,
+					    params.mem_prp_vf_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    IPU_ROTATE_NONE,
+					    vout->display_bufs[0],
+					    vout->display_bufs[1], 0, 0) != 0) {
+			dev_err(dev,
+				"Error initializing PP-VDI output buffer\n");
+			return -EINVAL;
+		}
+
+		if (ipu_link_channels(vout->post_proc_ch, MEM_ROT_VF_MEM) < 0)
+			return -EINVAL;
+
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+
+		ipu_enable_channel(MEM_ROT_VF_MEM);
+		*display_input_ch = MEM_ROT_VF_MEM;
+
+	} else {
+		if (ipu_init_channel_buffer(vout->post_proc_ch,
+					    IPU_OUTPUT_BUFFER,
+					    params.mem_prp_vf_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    vout->rotate,
+					    vout->display_bufs[0],
+					    vout->display_bufs[1], 0, 0) != 0) {
+			dev_err(dev,
+				"Error initializing PP-VDI output buffer\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Initialize PP path
+ *
+ * @param params    structure ipu_channel_params_t
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int init_PP(ipu_channel_params_t *params, vout_data *vout,
+		   struct device *dev, struct fb_info *fbi,
+		   ipu_channel_t *display_input_ch, u16 out_width,
+		   u16 out_height)
+{
+	u16 in_width, out_stride; /* stride of output channel */
+	unsigned int ipu_ic_out_max_width_size;
+
+	if (vout->display_ch == ADC_SYS2)
+		params->mem_pp_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
+	else
+		params->mem_pp_mem.out_pixel_fmt = bpp_to_fmt(fbi);
+
+	out_stride = out_width;
+	in_width = params->mem_pp_mem.in_width = vout->v2f.fmt.pix.width;
+	params->mem_pp_mem.in_height = vout->v2f.fmt.pix.height;
+	params->mem_pp_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
+	params->mem_pp_mem.out_width = out_width;
+	params->mem_pp_mem.out_height = out_height;
+	params->mem_pp_mem.out_resize_ratio = 0; /* 0 means unused */
+
+#ifdef CONFIG_MXC_IPU_V1
+	ipu_ic_out_max_width_size = 800;
+#else
+	ipu_ic_out_max_width_size = 1024;
+#endif
+	/* split IC by two stripes, the by pass is impossible*/
+	if (out_width > ipu_ic_out_max_width_size) {
+		vout->pp_split = 1;
+		out_stride = 2*out_width;
+
+
+		ipu_calc_stripes_sizes(
+					params->mem_pp_mem.in_width, /* input frame width;>1 */
+					params->mem_pp_mem.out_width, /* output frame width; >1 */
+					ipu_ic_out_max_width_size,
+					(((unsigned long long)1) << 32), /* 32bit for fractional*/
+					1,	/* equal stripes */
+					params->mem_pp_mem.in_pixel_fmt,
+					params->mem_pp_mem.out_pixel_fmt,
+					&(vout->pp_left_stripe),
+					&(vout->pp_right_stripe));
+
+
+		vout->pp_left_stripe.input_column = vout->pp_left_stripe.input_column *
+								fmt_to_bpp(vout->v2f.fmt.pix.pixelformat) / 8;
+		vout->pp_left_stripe.output_column = vout->pp_left_stripe.output_column *
+								fmt_to_bpp(params->mem_pp_mem.out_pixel_fmt) / 8;
+		vout->pp_right_stripe.input_column = vout->pp_right_stripe.input_column *
+								fmt_to_bpp(vout->v2f.fmt.pix.pixelformat) / 8;
+
+
+		vout->pp_right_stripe.output_column = vout->pp_right_stripe.output_column *
+								fmt_to_bpp(params->mem_pp_mem.out_pixel_fmt) / 8;
+
+
+
+		/* updare parameters */
+		params->mem_pp_mem.in_width = vout->pp_left_stripe.input_width;
+		params->mem_pp_mem.out_width = vout->pp_left_stripe.output_width;
+		out_width = vout->pp_left_stripe.output_width;
+		/* for using in ic_init*/
+		params->mem_pp_mem.out_resize_ratio = vout->pp_left_stripe.irr;
+
+		vout->pp_split_buf_num = 0;
+	} else
+		vout->pp_split = 0;
+
+
+	if (ipu_init_channel(vout->post_proc_ch, params) != 0) {
+		dev_err(dev, "Error initializing PP channel\n");
+		return -EINVAL;
+	}
+
+	if (ipu_init_channel_buffer(vout->post_proc_ch,
+				    IPU_INPUT_BUFFER,
+				    params->mem_pp_mem.in_pixel_fmt,
+				    params->mem_pp_mem.in_width,
+				    params->mem_pp_mem.in_height,
+				    vout->v2f.fmt.pix.bytesperline /
+				    bytes_per_pixel(params->mem_pp_mem.
+									in_pixel_fmt),
+									IPU_ROTATE_NONE,
+									vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
+									vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
+									vout->offset.u_offset,
+									vout->offset.v_offset) != 0) {
+		dev_err(dev, "Error initializing PP input buffer\n");
+		return -EINVAL;
+	}
+
+	if (!ipu_can_rotate_in_place(vout->rotate)) {
+		if (vout->rot_pp_bufs[0]) {
+			mxc_free_buffers(vout->rot_pp_bufs,
+					 vout->rot_pp_bufs_vaddr, 2,
+					 vout->display_buf_size);
+		}
+		if (mxc_allocate_buffers
+		    (vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
+		     vout->display_buf_size) < 0) {
+			return -ENOBUFS;
+		}
+
+		if (ipu_init_channel_buffer(vout->post_proc_ch,
+					    IPU_OUTPUT_BUFFER,
+					    params->mem_pp_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_stride,
+					    IPU_ROTATE_NONE,
+					    vout->rot_pp_bufs[0],
+					    vout->rot_pp_bufs[1], 0, 0) != 0) {
+			dev_err(dev, "Error initializing PP output buffer\n");
+			return -EINVAL;
+		}
+
+		if (ipu_init_channel(MEM_ROT_PP_MEM, NULL) != 0) {
+			dev_err(dev, "Error initializing PP ROT channel\n");
+			return -EINVAL;
+		}
+		if (ipu_init_channel_buffer(MEM_ROT_PP_MEM,
+					    IPU_INPUT_BUFFER,
+					    params->mem_pp_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_stride,
+					    vout->rotate,
+					    vout->rot_pp_bufs[0],
+					    vout->rot_pp_bufs[1], 0, 0) != 0) {
+			dev_err(dev,
+				"Error initializing PP ROT input buffer\n");
+			return -EINVAL;
+		}
+
+		/* swap width and height */
+		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
+			out_stride = out_width = vout->crop_current.width;
+			out_height = vout->crop_current.height;
+		}
+
+		if (ipu_init_channel_buffer(MEM_ROT_PP_MEM,
+					    IPU_OUTPUT_BUFFER,
+					    params->mem_pp_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_stride,
+					    IPU_ROTATE_NONE,
+					    vout->display_bufs[0],
+					    vout->display_bufs[1], 0, 0) != 0) {
+			dev_err(dev, "Error initializing PP output buffer\n");
+			return -EINVAL;
+		}
+
+		if (ipu_link_channels(vout->post_proc_ch, MEM_ROT_PP_MEM) < 0)
+			return -EINVAL;
+
+		ipu_select_buffer(MEM_ROT_PP_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_ROT_PP_MEM, IPU_OUTPUT_BUFFER, 1);
+
+		ipu_enable_channel(MEM_ROT_PP_MEM);
+		*display_input_ch = MEM_ROT_PP_MEM;
+
+	} else {
+		if (ipu_init_channel_buffer(vout->post_proc_ch,
+					    IPU_OUTPUT_BUFFER,
+					    params->mem_pp_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_stride,
+					    vout->rotate,
+					    vout->display_bufs[0],
+					    vout->display_bufs[1], 0, 0) != 0) {
+			dev_err(dev, "Error initializing PP output buffer\n");
+			return -EINVAL;
+		}
+	}
+
+	/* fix EBAs for IDMAC channels */
+	if (vout->pp_split) {
+		ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+									0,
+									vout->v4l2_bufs[vout->ipu_buf[0]].m.offset +
+									vout->pp_left_stripe.input_column);
+		ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+									1,
+									vout->v4l2_bufs[vout->ipu_buf[0]].m.offset +
+									vout->pp_right_stripe.input_column);
+		ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
+									0,
+									vout->display_bufs[0] +
+									vout->pp_left_stripe.output_column);
+
+		ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
+									1,
+									vout->display_bufs[0] +
+									vout->pp_right_stripe.output_column);
+	}
+
+	return 0;
+}
+
+/*!
+ * Start the output stream
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamon(vout_data * vout)
+{
+	struct device *dev = &vout->video_dev->dev;
+	ipu_channel_params_t params;
+	struct mxcfb_pos fb_pos;
+	struct fb_var_screeninfo fbvar;
+	struct fb_info *fbi =
+	    registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	u16 out_width;
+	u16 out_height;
+	int disp_irq = 0;
+	ipu_channel_t display_input_ch;
+	bool use_direct_adc = false;
+	mm_segment_t old_fs;
+
+	dev_dbg(dev, "mxc_v4l2out_streamon: field format=%d\n",
+		vout->field_fmt);
+	if (INTERLACED_CONTENT(vout)) {
+		ipu_request_irq(IPU_IRQ_PRP_VF_OUT_EOF,
+				mxc_v4l2out_pp_in_irq_handler,
+				0, &vout->video_dev->name, vout);
+		display_input_ch = MEM_VDI_PRP_VF_MEM;
+	} else {
+		ipu_request_irq(IPU_IRQ_PP_IN_EOF,
+				mxc_v4l2out_pp_in_irq_handler,
+				0, &vout->video_dev->name, vout);
+		display_input_ch = MEM_PP_MEM;
+	}
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state != STATE_STREAM_OFF)
+		return -EBUSY;
+
+	if (queue_size(&vout->ready_q) < 2) {
+		dev_err(dev, "2 buffers not been queued yet!\n");
+		return -EINVAL;
+	}
+
+	if ((vout->field_fmt == V4L2_FIELD_BOTTOM) || (vout->field_fmt == V4L2_FIELD_TOP)) {
+		dev_err(dev, "4 queued buffers need, not supported yet!\n");
+		return -EINVAL;
+	}
+
+	pending_buffer = 0;
+
+	out_width = vout->crop_current.width;
+	out_height = vout->crop_current.height;
+	vout->next_done_ipu_buf = 0;
+	vout->next_rdy_ipu_buf = 1;
+
+	if (!INTERLACED_CONTENT(vout)) {
+		vout->next_done_ipu_buf = vout->next_rdy_ipu_buf = 0;
+		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
+		if (out_width != vout->v2f.fmt.pix.width && /*pp_split*/
+			out_height != vout->v2f.fmt.pix.height &&
+			out_width > 1024) {
+			vout->ipu_buf[1] = vout->ipu_buf[0];
+			vout->frame_count = 1;
+	} else {
+			vout->ipu_buf[1] = dequeue_buf(&vout->ready_q);
+			vout->frame_count = 2;
+		}
+	} else if (!LOAD_3FIELDS(vout)) {
+		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf[1] = -1;
+		vout->frame_count = 1;
+		last_index_n = vout->ipu_buf[0];
+	} else {
+		vout->ipu_buf_p[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf[0] = vout->ipu_buf_p[0];
+		vout->ipu_buf_n[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf_p[1] = -1;
+		vout->ipu_buf[1] = -1;
+		vout->ipu_buf_n[1] = -1;
+		last_index_c = vout->ipu_buf[0];
+		last_index_n = vout->ipu_buf_n[0];
+		vout->frame_count = 2;
+	}
+
+	/* Init Display Channel */
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL
+	if (INTERLACED_CONTENT(vout))
+		ipu_enable_irq(IPU_IRQ_PRP_VF_OUT_EOF);
+	else
+		ipu_enable_irq(IPU_IRQ_PP_IN_EOF);
+
+	if (vout->cur_disp_output < DISP3) {
+		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_OFF, 0);
+		fbi = NULL;
+		if (ipu_can_rotate_in_place(vout->rotate)) {
+			dev_dbg(dev, "Using PP direct to ADC channel\n");
+			use_direct_adc = true;
+			vout->display_ch = MEM_PP_ADC;
+			vout->post_proc_ch = MEM_PP_ADC;
+
+			memset(&params, 0, sizeof(params));
+			params.mem_pp_adc.in_width = vout->v2f.fmt.pix.width;
+			params.mem_pp_adc.in_height = vout->v2f.fmt.pix.height;
+			params.mem_pp_adc.in_pixel_fmt =
+			    vout->v2f.fmt.pix.pixelformat;
+			params.mem_pp_adc.out_width = out_width;
+			params.mem_pp_adc.out_height = out_height;
+			params.mem_pp_adc.out_pixel_fmt = SDC_FG_FB_FORMAT;
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+			params.mem_pp_adc.out_left =
+			    2 + vout->crop_current.left;
+#else
+			params.mem_pp_adc.out_left =
+			    12 + vout->crop_current.left;
+#endif
+			params.mem_pp_adc.out_top = vout->crop_current.top;
+			if (ipu_init_channel(vout->post_proc_ch, &params) != 0) {
+				dev_err(dev, "Error initializing PP chan\n");
+				return -EINVAL;
+			}
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_INPUT_BUFFER,
+						    params.mem_pp_adc.
+						    in_pixel_fmt,
+						    params.mem_pp_adc.in_width,
+						    params.mem_pp_adc.in_height,
+						    vout->v2f.fmt.pix.
+						    bytesperline /
+						    bytes_per_pixel(params.
+								    mem_pp_adc.
+								    in_pixel_fmt),
+						    vout->rotate,
+						    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
+						    vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
+						    vout->offset.u_offset,
+						    vout->offset.v_offset) !=
+			    0) {
+				dev_err(dev, "Error initializing PP in buf\n");
+				return -EINVAL;
+			}
+
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_OUTPUT_BUFFER,
+						    params.mem_pp_adc.
+						    out_pixel_fmt, out_width,
+						    out_height, out_width,
+						    vout->rotate, 0, 0, 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing PP output buffer\n");
+				return -EINVAL;
+			}
+
+		} else {
+			dev_dbg(dev, "Using ADC SYS2 channel\n");
+			vout->display_ch = ADC_SYS2;
+			vout->post_proc_ch = MEM_PP_MEM;
+
+			if (vout->display_bufs[0]) {
+				mxc_free_buffers(vout->display_bufs,
+						 vout->display_bufs_vaddr,
+						 2, vout->display_buf_size);
+			}
+
+			vout->display_buf_size = vout->crop_current.width *
+			    vout->crop_current.height *
+			    fmt_to_bpp(SDC_FG_FB_FORMAT) / 8;
+			mxc_allocate_buffers(vout->display_bufs,
+					     vout->display_bufs_vaddr,
+					     2, vout->display_buf_size);
+
+			memset(&params, 0, sizeof(params));
+			params.adc_sys2.disp = vout->cur_disp_output;
+			params.adc_sys2.ch_mode = WriteTemplateNonSeq;
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+			params.adc_sys2.out_left = 2 + vout->crop_current.left;
+#else
+			params.adc_sys2.out_left = 12 + vout->crop_current.left;
+#endif
+			params.adc_sys2.out_top = vout->crop_current.top;
+			if (ipu_init_channel(ADC_SYS2, &params) < 0)
+				return -EINVAL;
+
+			if (ipu_init_channel_buffer(vout->display_ch,
+						    IPU_INPUT_BUFFER,
+						    SDC_FG_FB_FORMAT,
+						    out_width, out_height,
+						    out_width, IPU_ROTATE_NONE,
+						    vout->display_bufs[0],
+						    vout->display_bufs[1], 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing SDC FG buffer\n");
+				return -EINVAL;
+			}
+		}
+	} else
+#endif
+	{			/* Use SDC */
+		unsigned int ipu_ch = CHAN_NONE;
+
+		dev_dbg(dev, "Using SDC channel\n");
+
+		/*
+		 * Bypass IC if resizing and rotation are not needed
+		 * Meanwhile, apply IC bypass to SDC only
+		 */
+      vout->pp_split = 0;/* no pp_split by default */
+		if (out_width == vout->v2f.fmt.pix.width &&
+			out_height == vout->v2f.fmt.pix.height &&
+			ipu_can_rotate_in_place(vout->rotate)) {
+			vout->ic_bypass = 1;
+			ipu_disable_irq(IPU_IRQ_PP_IN_EOF);
+		} else {
+			vout->ic_bypass = 0;
+		}
+
+#ifdef CONFIG_MXC_IPU_V1
+		/* IPUv1 needs IC to do CSC */
+		if (format_is_yuv(vout->v2f.fmt.pix.pixelformat) !=
+		    format_is_yuv(bpp_to_fmt(fbi)))
+			vout->ic_bypass = 0;
+#endif
+
+		/* We are using IC to do input cropping */
+		if (vout->queue_buf_paddr[vout->ipu_buf[0]] !=
+		    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset ||
+		    vout->queue_buf_paddr[vout->ipu_buf[1]] !=
+		    vout->v4l2_bufs[vout->ipu_buf[1]].m.offset)
+			vout->ic_bypass = 0;
+
+		if (vout->ic_bypass)
+			pr_debug("Bypassing IC\n");
+
+		fbvar = fbi->var;
+
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_IPU_CHAN,
+					(unsigned long)&ipu_ch);
+			set_fs(old_fs);
+		}
+
+		if (ipu_ch == CHAN_NONE) {
+			dev_err(dev,
+					"Can not get display ipu channel\n");
+			return -EINVAL;
+		}
+
+		vout->display_ch = ipu_ch;
+
+		if (vout->cur_disp_output == 3 || vout->cur_disp_output == 5) {
+			fbvar.bits_per_pixel = 16;
+			if (format_is_yuv(vout->v2f.fmt.pix.pixelformat))
+				fbvar.nonstd = IPU_PIX_FMT_UYVY;
+			else
+				fbvar.nonstd = 0;
+
+			fbvar.xres = fbvar.xres_virtual = out_width;
+			fbvar.yres = out_height;
+			fbvar.yres_virtual = out_height * 2;
+		}
+
+		if (vout->ic_bypass) {
+			fbvar.bits_per_pixel = 8*
+				bytes_per_pixel(vout->v2f.fmt.pix.pixelformat);
+			fbvar.nonstd = vout->v2f.fmt.pix.pixelformat;
+		}
+
+		fbvar.activate |= FB_ACTIVATE_FORCE;
+		fb_set_var(fbi, &fbvar);
+
+		fb_pos.x = vout->crop_current.left;
+		fb_pos.y = vout->crop_current.top;
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
+					     (unsigned long)&fb_pos);
+			set_fs(old_fs);
+		}
+
+		vout->display_bufs[1] = fbi->fix.smem_start;
+		vout->display_bufs[0] = fbi->fix.smem_start +
+		    (fbi->fix.line_length * fbi->var.yres);
+		vout->display_buf_size = vout->crop_current.width *
+		    vout->crop_current.height * fbi->var.bits_per_pixel / 8;
+		if (INTERLACED_CONTENT(vout))
+			vout->post_proc_ch = MEM_VDI_PRP_VF_MEM;
+		else
+			vout->post_proc_ch = MEM_PP_MEM;
+	}
+
+	/* Init PP */
+	if (use_direct_adc == false && !vout->ic_bypass) {
+		if (INTERLACED_CONTENT(vout)) {
+			vout->post_proc_ch = MEM_VDI_PRP_VF_MEM;
+			ipu_enable_irq(IPU_IRQ_PRP_VF_OUT_EOF);
+		} else {
+			vout->post_proc_ch = MEM_PP_MEM;
+			ipu_enable_irq(IPU_IRQ_PP_IN_EOF);
+		}
+
+		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
+			out_width = vout->crop_current.height;
+			out_height = vout->crop_current.width;
+		}
+		memset(&params, 0, sizeof(params));
+		int rc;
+		if (INTERLACED_CONTENT(vout)) {
+			rc = init_VDI(params, vout, dev, fbi, &display_input_ch,
+				      out_width, out_height);
+		} else {
+			rc = init_PP(&params, vout, dev, fbi, &display_input_ch,
+				     out_width, out_height);
+		}
+		if (rc < 0)
+			return rc;
+		if (!vout->pp_split) { /* display channel link */
+			if (ipu_link_channels(display_input_ch, vout->display_ch) < 0) {
+				dev_err(dev, "Error linking ipu channels\n");
+				return -EINVAL;
+			}
+		}
+	}
+
+	vout->state = STATE_STREAM_PAUSED;
+
+	if (use_direct_adc == false) {
+		if (!vout->ic_bypass) {
+#ifndef CONFIG_MXC_IPU_V1
+			ipu_enable_channel(vout->post_proc_ch);
+#endif
+			if (LOAD_3FIELDS(vout)) {
+				ipu_enable_channel(MEM_VDI_PRP_VF_MEM_P);
+				ipu_enable_channel(MEM_VDI_PRP_VF_MEM_N);
+				ipu_select_multi_vdi_buffer(0);
+			} else if (INTERLACED_CONTENT(vout)) {
+				ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
+			} else {
+				ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
+				if (!vout->pp_split)
+					ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 1);
+			}
+			ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 0);
+			ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 1);
+#ifdef CONFIG_MXC_IPU_V1
+			ipu_enable_channel(vout->post_proc_ch);
+#endif
+		} else {
+			ipu_update_channel_buffer(vout->display_ch,
+				IPU_INPUT_BUFFER,
+				0, vout->v4l2_bufs[vout->ipu_buf[0]].m.offset);
+			ipu_update_channel_buffer(vout->display_ch,
+				IPU_INPUT_BUFFER,
+				1, vout->v4l2_bufs[vout->ipu_buf[1]].m.offset);
+			ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 0);
+			ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 1);
+		}
+		disp_irq = get_display_irq(vout);
+		ipu_request_irq(disp_irq, mxc_v4l2out_disp_refresh_irq_handler,
+				0, NULL, vout);
+
+		if (fbi) {
+			acquire_console_sem();
+			fb_blank(fbi, FB_BLANK_UNBLANK);
+			release_console_sem();
+		} else {
+			ipu_enable_channel(vout->display_ch);
+		}
+	} else {
+		ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
+		ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 1);
+		ipu_enable_channel(vout->post_proc_ch);
+	}
+	vout->start_jiffies = jiffies;
+
+	msleep(1);
+
+	dev_dbg(dev,
+		"streamon: start time = %lu jiffies\n", vout->start_jiffies);
+
+	return 0;
+}
+
+/*!
+ * Shut down the voutera
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamoff(vout_data * vout)
+{
+	struct fb_info *fbi =
+	    registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	int i, retval = 0, disp_irq = 0;
+	unsigned long lockflag = 0;
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state == STATE_STREAM_OFF) {
+		return 0;
+	}
+
+	if (INTERLACED_CONTENT(vout))
+		ipu_free_irq(IPU_IRQ_PRP_VF_OUT_EOF, vout);
+	else
+		ipu_free_irq(IPU_IRQ_PP_IN_EOF, vout);
+
+	spin_lock_irqsave(&g_lock, lockflag);
+
+	del_timer(&vout->output_timer);
+
+	if (vout->state == STATE_STREAM_ON) {
+		vout->state = STATE_STREAM_STOPPING;
+	}
+
+	if (!vout->ic_bypass) {
+		if (INTERLACED_CONTENT(vout))
+			ipu_disable_irq(IPU_IRQ_PRP_VF_OUT_EOF);
+		else
+			ipu_disable_irq(IPU_IRQ_PP_IN_EOF);
+	}
+
+	spin_unlock_irqrestore(&g_lock, lockflag);
+
+	pending_buffer = 0;
+	disp_irq = get_display_irq(vout);
+	ipu_free_irq(disp_irq, vout);
+
+	if (vout->display_ch == MEM_FG_SYNC) {
+		struct mxcfb_pos fb_pos;
+		mm_segment_t old_fs;
+
+		fb_pos.x = 0;
+		fb_pos.y = 0;
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
+					(unsigned long)&fb_pos);
+			set_fs(old_fs);
+		}
+	}
+
+	if (vout->post_proc_ch == MEM_PP_MEM ||
+	    vout->post_proc_ch == MEM_PRP_VF_MEM) {
+		/* SDC or ADC with Rotation */
+		if (!ipu_can_rotate_in_place(vout->rotate)) {
+			ipu_unlink_channels(MEM_PP_MEM, MEM_ROT_PP_MEM);
+			ipu_unlink_channels(MEM_ROT_PP_MEM,
+					    vout->display_ch);
+			ipu_disable_channel(MEM_ROT_PP_MEM, true);
+
+			if (vout->rot_pp_bufs[0]) {
+				mxc_free_buffers(vout->rot_pp_bufs,
+						 vout->rot_pp_bufs_vaddr, 2,
+						 vout->display_buf_size);
+			}
+		} else {
+			ipu_unlink_channels(MEM_PP_MEM, vout->display_ch);
+		}
+		ipu_disable_channel(MEM_PP_MEM, true);
+
+		if (vout->display_ch == ADC_SYS2 ||
+			vout->display_ch == MEM_FG_SYNC) {
+			ipu_disable_channel(vout->display_ch, true);
+			ipu_uninit_channel(vout->display_ch);
+		} else {
+			fbi->var.activate |= FB_ACTIVATE_FORCE;
+			fb_set_var(fbi, &fbi->var);
+
+			if (vout->display_ch == MEM_FG_SYNC) {
+				acquire_console_sem();
+				fb_blank(fbi, FB_BLANK_POWERDOWN);
+				release_console_sem();
+			}
+
+			vout->display_bufs[0] = 0;
+			vout->display_bufs[1] = 0;
+		}
+
+		ipu_uninit_channel(MEM_PP_MEM);
+		if (!ipu_can_rotate_in_place(vout->rotate))
+			ipu_uninit_channel(MEM_ROT_PP_MEM);
+	} else if (INTERLACED_CONTENT(vout) && (vout->post_proc_ch == MEM_VDI_PRP_VF_MEM)) {
+		if (!ipu_can_rotate_in_place(vout->rotate)) {
+			ipu_unlink_channels(MEM_VDI_PRP_VF_MEM,
+					    MEM_ROT_VF_MEM);
+			ipu_unlink_channels(MEM_ROT_VF_MEM,
+					    vout->display_ch);
+			ipu_disable_channel(MEM_ROT_VF_MEM, true);
+
+			if (vout->rot_pp_bufs[0]) {
+				mxc_free_buffers(vout->rot_pp_bufs,
+						 vout->rot_pp_bufs_vaddr, 2,
+						 vout->display_buf_size);
+			}
+		} else {
+			ipu_unlink_channels(MEM_VDI_PRP_VF_MEM,
+					    vout->display_ch);
+		}
+
+		ipu_disable_channel(MEM_VDI_PRP_VF_MEM, true);
+
+		if (vout->display_ch == ADC_SYS2 ||
+			vout->display_ch == MEM_FG_SYNC) {
+			ipu_disable_channel(vout->display_ch, true);
+			ipu_uninit_channel(vout->display_ch);
+		} else {
+			fbi->var.activate |= FB_ACTIVATE_FORCE;
+			fb_set_var(fbi, &fbi->var);
+
+			if (vout->display_ch == MEM_FG_SYNC) {
+				acquire_console_sem();
+				fb_blank(fbi, FB_BLANK_POWERDOWN);
+				release_console_sem();
+			}
+
+			vout->display_bufs[0] = 0;
+			vout->display_bufs[1] = 0;
+		}
+
+		ipu_uninit_channel(MEM_VDI_PRP_VF_MEM);
+		if (!ipu_can_rotate_in_place(vout->rotate))
+			ipu_uninit_channel(MEM_ROT_VF_MEM);
+	} else {		/* ADC Direct */
+		ipu_disable_channel(MEM_PP_ADC, true);
+		ipu_uninit_channel(MEM_PP_ADC);
+	}
+	vout->ready_q.head = vout->ready_q.tail = 0;
+	vout->done_q.head = vout->done_q.tail = 0;
+	for (i = 0; i < vout->buffer_cnt; i++) {
+		vout->v4l2_bufs[i].flags = 0;
+		vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+		vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+	}
+
+	vout->state = STATE_STREAM_OFF;
+
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL
+	if (vout->cur_disp_output < DISP3) {
+		if (vout->display_bufs[0] != 0) {
+			mxc_free_buffers(vout->display_bufs,
+					 vout->display_bufs_vaddr, 2,
+					 vout->display_buf_size);
+		}
+
+		mxcfb_set_refresh_mode(registered_fb
+				       [vout->
+					output_fb_num[vout->cur_disp_output]],
+				       MXCFB_REFRESH_PARTIAL, 0);
+	}
+#endif
+
+	return retval;
+}
+
+/*
+ * Valid whether the palette is supported
+ *
+ * @param palette  V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return 1 if supported, 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	return ((palette == V4L2_PIX_FMT_RGB565) ||
+		(palette == V4L2_PIX_FMT_BGR24) ||
+		(palette == V4L2_PIX_FMT_RGB24) ||
+		(palette == V4L2_PIX_FMT_BGR32) ||
+		(palette == V4L2_PIX_FMT_RGB32) ||
+		(palette == V4L2_PIX_FMT_NV12) ||
+		(palette == V4L2_PIX_FMT_YUV422P) ||
+		(palette == V4L2_PIX_FMT_YUV420));
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_g_fmt(vout_data * vout, struct v4l2_format *f)
+{
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		return -EINVAL;
+	}
+	*f = vout->v2f;
+	return 0;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_s_fmt(vout_data * vout, struct v4l2_format *f)
+{
+	int retval = 0;
+	u32 size = 0;
+	u32 bytesperline;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		retval = -EINVAL;
+		goto err0;
+	}
+	if (!valid_mode(f->fmt.pix.pixelformat)) {
+		dev_err(&vout->video_dev->dev, "pixel format not supported\n");
+		retval = -EINVAL;
+		goto err0;
+	}
+
+	bytesperline = (f->fmt.pix.width * fmt_to_bpp(f->fmt.pix.pixelformat)) /
+	    8;
+	if (f->fmt.pix.bytesperline < bytesperline) {
+		f->fmt.pix.bytesperline = bytesperline;
+	} else {
+		bytesperline = f->fmt.pix.bytesperline;
+	}
+	vout->bytesperline = bytesperline;
+
+	/* Based on http://v4l2spec.bytesex.org/spec/x6386.htm#V4L2-FIELD */
+	vout->field_fmt = f->fmt.pix.field;
+	switch (vout->field_fmt) {
+		/* Images are in progressive format, not interlaced */
+	case V4L2_FIELD_NONE:
+		break;
+		/* The two fields of a frame are passed in separate buffers,
+		   in temporal order, i. e. the older one first. */
+	case V4L2_FIELD_ALTERNATE:
+		dev_err(&vout->video_dev->dev,
+			"V4L2_FIELD_ALTERNATE field format not supported yet!\n");
+		break;
+	case V4L2_FIELD_INTERLACED_TB:
+		if (cpu_is_mx51())
+			break;
+	case V4L2_FIELD_INTERLACED_BT:
+		dev_err(&vout->video_dev->dev,
+			"V4L2_FIELD_INTERLACED_BT field format not supported yet!\n");
+	default:
+		vout->field_fmt = V4L2_FIELD_NONE;
+		break;
+	}
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUV422P:
+		/* byteperline for YUV planar formats is for
+		   Y plane only */
+		size = bytesperline * f->fmt.pix.height * 2;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_NV12:
+		size = (bytesperline * f->fmt.pix.height * 3) / 2;
+		break;
+	default:
+		size = bytesperline * f->fmt.pix.height;
+		break;
+	}
+
+	/* Return the actual size of the image to the app */
+	if (f->fmt.pix.sizeimage < size) {
+		f->fmt.pix.sizeimage = size;
+	} else {
+		size = f->fmt.pix.sizeimage;
+	}
+
+	vout->v2f.fmt.pix = f->fmt.pix;
+	if (vout->v2f.fmt.pix.priv != 0) {
+		if (copy_from_user(&vout->offset,
+				   (void *)vout->v2f.fmt.pix.priv,
+				   sizeof(vout->offset))) {
+			retval = -EFAULT;
+			goto err0;
+		}
+	} else {
+		vout->offset.u_offset = 0;
+		vout->offset.v_offset = 0;
+	}
+
+	retval = 0;
+      err0:
+	return retval;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_get_v42lout_control(vout_data * vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		return (vout->rotate & IPU_ROTATE_HORIZ_FLIP) ? 1 : 0;
+	case V4L2_CID_VFLIP:
+		return (vout->rotate & IPU_ROTATE_VERT_FLIP) ? 1 : 0;
+	case (V4L2_CID_PRIVATE_BASE + 1):
+		return vout->rotate;
+	default:
+		return -EINVAL;
+	}
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_set_v42lout_control(vout_data * vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		vout->rotate |= c->value ? IPU_ROTATE_HORIZ_FLIP :
+		    IPU_ROTATE_NONE;
+		break;
+	case V4L2_CID_VFLIP:
+		vout->rotate |= c->value ? IPU_ROTATE_VERT_FLIP :
+		    IPU_ROTATE_NONE;
+		break;
+	case V4L2_CID_MXC_ROT:
+		vout->rotate = c->value;
+		break;
+	case V4L2_CID_MXC_MOTION:
+		vout->motion_sel = c->value;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*!
+ * V4L2 interface - open function
+ *
+ * @param inode        structure inode *
+ *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l2out_open(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+	int err;
+
+	if (!vout) {
+		return -ENODEV;
+	}
+
+	down(&vout->busy_lock);
+
+	err = -EINTR;
+	if (signal_pending(current))
+		goto oops;
+
+
+	if (vout->open_count++ == 0) {
+		init_waitqueue_head(&vout->v4l_bufq);
+
+		init_timer(&vout->output_timer);
+		vout->output_timer.function = mxc_v4l2out_timer_handler;
+		vout->output_timer.data = (unsigned long)vout;
+
+		vout->state = STATE_STREAM_OFF;
+		vout->rotate = IPU_ROTATE_NONE;
+		g_irq_cnt = g_buf_output_cnt = g_buf_q_cnt = g_buf_dq_cnt = 0;
+
+	}
+
+	file->private_data = dev;
+
+	up(&vout->busy_lock);
+
+	return 0;
+
+      oops:
+	up(&vout->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L2 interface - close function
+ *
+ * @param inode    struct inode *
+ *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int mxc_v4l2out_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+
+	if (--vout->open_count == 0) {
+		if (vout->state != STATE_STREAM_OFF)
+			mxc_v4l2out_streamoff(vout);
+
+		file->private_data = NULL;
+
+		mxc_free_buffers(vout->queue_buf_paddr, vout->queue_buf_vaddr,
+				 vout->buffer_cnt, vout->queue_buf_size);
+		vout->buffer_cnt = 0;
+		mxc_free_buffers(vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
+				 vout->display_buf_size);
+
+		/* capture off */
+		wake_up_interruptible(&vout->v4l_bufq);
+
+	}
+
+	return 0;
+}
+
+/*!
+ * V4L2 interface - ioctl function
+ *
+ * @param inode      struct inode *
+ *
+ * @param file       struct file *
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void *
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int
+mxc_v4l2out_do_ioctl(struct inode *inode, struct file *file,
+		     unsigned int ioctlnr, void *arg)
+{
+	struct video_device *vdev = file->private_data;
+	vout_data *vout = video_get_drvdata(vdev);
+	int retval = 0;
+	int i = 0;
+
+	if (!vout)
+		return -EBADF;
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+	case VIDIOC_QUERYCAP:
+		{
+			struct v4l2_capability *cap = arg;
+			strcpy(cap->driver, "mxc_v4l2_output");
+			cap->version = 0;
+			cap->capabilities =
+			    V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+			cap->card[0] = '\0';
+			cap->bus_info[0] = '\0';
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_FMT:
+		{
+			struct v4l2_format *gf = arg;
+			retval = mxc_v4l2out_g_fmt(vout, gf);
+			break;
+		}
+	case VIDIOC_S_FMT:
+		{
+			struct v4l2_format *sf = arg;
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+			retval = mxc_v4l2out_s_fmt(vout, sf);
+			break;
+		}
+	case VIDIOC_REQBUFS:
+		{
+			struct v4l2_requestbuffers *req = arg;
+			if ((req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (req->memory != V4L2_MEMORY_MMAP)) {
+				dev_dbg(&vdev->dev,
+					"VIDIOC_REQBUFS: incorrect buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			if (req->count == 0)
+				mxc_v4l2out_streamoff(vout);
+
+			if (vout->state == STATE_STREAM_OFF) {
+				if (vout->queue_buf_paddr[0] != 0) {
+					mxc_free_buffers(vout->queue_buf_paddr,
+							 vout->queue_buf_vaddr,
+							 vout->buffer_cnt,
+							 vout->queue_buf_size);
+					dev_dbg(&vdev->dev,
+						"VIDIOC_REQBUFS: freed buffers\n");
+				}
+				vout->buffer_cnt = 0;
+			} else {
+				dev_dbg(&vdev->dev,
+					"VIDIOC_REQBUFS: Buffer is in use\n");
+				retval = -EBUSY;
+				break;
+			}
+
+			if (req->count == 0)
+				break;
+
+			if (req->count < MIN_FRAME_NUM) {
+				req->count = MIN_FRAME_NUM;
+			} else if (req->count > MAX_FRAME_NUM) {
+				req->count = MAX_FRAME_NUM;
+			}
+			vout->buffer_cnt = req->count;
+			vout->queue_buf_size =
+			    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+
+			retval = mxc_allocate_buffers(vout->queue_buf_paddr,
+						      vout->queue_buf_vaddr,
+						      vout->buffer_cnt,
+						      vout->queue_buf_size);
+			if (retval < 0)
+				break;
+
+			/* Init buffer queues */
+			vout->done_q.head = 0;
+			vout->done_q.tail = 0;
+			vout->ready_q.head = 0;
+			vout->ready_q.tail = 0;
+
+			for (i = 0; i < vout->buffer_cnt; i++) {
+				memset(&(vout->v4l2_bufs[i]), 0,
+				       sizeof(vout->v4l2_bufs[i]));
+				vout->v4l2_bufs[i].flags = 0;
+				vout->v4l2_bufs[i].memory = V4L2_MEMORY_MMAP;
+				vout->v4l2_bufs[i].index = i;
+				vout->v4l2_bufs[i].type =
+				    V4L2_BUF_TYPE_VIDEO_OUTPUT;
+				vout->v4l2_bufs[i].length =
+				    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+				vout->v4l2_bufs[i].m.offset =
+				    (unsigned long)vout->queue_buf_paddr[i];
+				vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+				vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+			}
+			break;
+		}
+	case VIDIOC_QUERYBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			u32 type = buf->type;
+			int index = buf->index;
+
+			if ((type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt)) {
+				dev_dbg(&vdev->dev,
+					"VIDIOC_QUERYBUFS: incorrect buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+			down(&vout->param_lock);
+			memcpy(buf, &(vout->v4l2_bufs[index]), sizeof(*buf));
+			up(&vout->param_lock);
+			break;
+		}
+	case VIDIOC_QBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int index = buf->index;
+			unsigned long lock_flags;
+			int param[5][3];
+
+			if ((buf->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			dev_dbg(&vdev->dev, "VIDIOC_QBUF: %d field = %d\n", buf->index, buf->field);
+
+			/* mmapped buffers are L1 WB cached,
+			 * so we need to clean them */
+			if (buf->memory & V4L2_MEMORY_MMAP) {
+				flush_cache_all();
+			}
+
+			spin_lock_irqsave(&g_lock, lock_flags);
+
+			memcpy(&(vout->v4l2_bufs[index]), buf, sizeof(*buf));
+			vout->v4l2_bufs[index].flags |= V4L2_BUF_FLAG_QUEUED;
+
+			g_buf_q_cnt++;
+			if (vout->v4l2_bufs[index].reserved)
+				if (!copy_from_user(&param[0][0],
+						    (void *)vout->
+						    v4l2_bufs[index]
+						    .reserved, sizeof(param)))
+					ipu_set_csc_coefficients(vout->
+								 display_ch,
+								 param);
+			queue_buf(&vout->ready_q, index);
+			if (vout->state == STATE_STREAM_PAUSED) {
+				unsigned long timeout;
+
+				index = peek_next_buf(&vout->ready_q);
+
+				/* if timestamp is 0, then default to 30fps */
+				if ((vout->v4l2_bufs[index].timestamp.tv_sec ==
+				     0)
+				    && (vout->v4l2_bufs[index].timestamp.
+					tv_usec == 0))
+					timeout =
+					    vout->start_jiffies +
+					    vout->frame_count * HZ / 30;
+				else
+					timeout =
+					    get_jiffies(&vout->v4l2_bufs[index].
+							timestamp);
+
+				if (jiffies >= timeout) {
+					dev_dbg(&vout->video_dev->dev,
+						"warning: timer timeout already expired.\n");
+				}
+				vout->output_timer.expires = timeout;
+				dev_dbg(&vdev->dev,
+					"QBUF: frame #%u timeout @ %lu jiffies, current = %lu\n",
+					vout->frame_count, timeout, jiffies);
+				add_timer(&vout->output_timer);
+				vout->state = STATE_STREAM_ON;
+			}
+
+			spin_unlock_irqrestore(&g_lock, lock_flags);
+			break;
+		}
+	case VIDIOC_DQBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int idx;
+
+			if ((queue_size(&vout->done_q) == 0) &&
+			    (file->f_flags & O_NONBLOCK)) {
+				retval = -EAGAIN;
+				break;
+			}
+
+			if (!wait_event_interruptible_timeout(vout->v4l_bufq,
+							      queue_size(&vout->
+									 done_q)
+							      != 0, 10 * HZ)) {
+				dev_dbg(&vdev->dev, "VIDIOC_DQBUF: timeout\n");
+				retval = -ETIME;
+				break;
+			} else if (signal_pending(current)) {
+				dev_dbg(&vdev->dev,
+					"VIDIOC_DQBUF: interrupt received\n");
+				retval = -ERESTARTSYS;
+				break;
+			}
+			idx = dequeue_buf(&vout->done_q);
+			if (idx == -1) {	/* No frame free */
+				dev_dbg(&vdev->dev,
+					"VIDIOC_DQBUF: no free buffers, returning\n");
+				retval = -EAGAIN;
+				break;
+			}
+			if ((vout->v4l2_bufs[idx].flags & V4L2_BUF_FLAG_DONE) ==
+			    0)
+				dev_dbg(&vdev->dev,
+					"VIDIOC_DQBUF: buffer in done q, but not "
+					"flagged as done\n");
+
+			vout->v4l2_bufs[idx].flags = 0;
+			memcpy(buf, &(vout->v4l2_bufs[idx]), sizeof(*buf));
+			dev_dbg(&vdev->dev, "VIDIOC_DQBUF: %d\n", buf->index);
+			break;
+		}
+	case VIDIOC_STREAMON:
+		{
+			retval = mxc_v4l2out_streamon(vout);
+			break;
+		}
+	case VIDIOC_STREAMOFF:
+		{
+			retval = mxc_v4l2out_streamoff(vout);
+			break;
+		}
+	case VIDIOC_G_CTRL:
+		{
+			retval = mxc_get_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_S_CTRL:
+		{
+			retval = mxc_set_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_CROPCAP:
+		{
+			struct v4l2_cropcap *cap = arg;
+
+			if (cap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+
+			cap->bounds = vout->crop_bounds[vout->cur_disp_output];
+			cap->defrect = vout->crop_bounds[vout->cur_disp_output];
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			crop->c = vout->crop_current;
+			break;
+		}
+	case VIDIOC_S_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+			struct v4l2_rect *b =
+			    &(vout->crop_bounds[vout->cur_disp_output]);
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.height < 0) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.width < 0) {
+				retval = -EINVAL;
+				break;
+			}
+
+			/* only full screen supported for SDC BG and SDC DC */
+			if (vout->cur_disp_output == 4 ||
+			    vout->cur_disp_output == 5) {
+				crop->c = vout->crop_current;
+				break;
+			}
+
+			if (crop->c.top < b->top)
+				crop->c.top = b->top;
+			if (crop->c.top >= b->top + b->height)
+				crop->c.top = b->top + b->height - 1;
+			if (crop->c.height > b->top - crop->c.top + b->height)
+				crop->c.height =
+				    b->top - crop->c.top + b->height;
+
+			if (crop->c.left < b->left)
+				crop->c.left = b->left;
+			if (crop->c.left >= b->left + b->width)
+				crop->c.left = b->left + b->width - 1;
+			if (crop->c.width > b->left - crop->c.left + b->width)
+				crop->c.width =
+				    b->left - crop->c.left + b->width;
+
+			/* stride line limitation */
+			crop->c.height -= crop->c.height % 8;
+			crop->c.width -= crop->c.width % 8;
+
+			vout->crop_current = crop->c;
+			break;
+		}
+	case VIDIOC_ENUMOUTPUT:
+		{
+			struct v4l2_output *output = arg;
+
+			if ((output->index >= 5) ||
+			    (vout->output_enabled[output->index] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (output->index < 3) {
+				*output = mxc_outputs[MXC_V4L2_OUT_2_ADC];
+				output->name[4] = '0' + output->index;
+			} else {
+				*output = mxc_outputs[MXC_V4L2_OUT_2_SDC];
+			}
+			break;
+		}
+	case VIDIOC_G_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			*p_output_num = vout->cur_disp_output;
+			break;
+		}
+	case VIDIOC_S_OUTPUT:
+		{
+			int *p_output_num = arg;
+			int fbnum;
+			struct v4l2_rect *b;
+
+			if ((*p_output_num >= MXC_V4L2_OUT_NUM_OUTPUTS) ||
+			    (vout->output_enabled[*p_output_num] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+
+			vout->cur_disp_output = *p_output_num;
+
+			/* Update bounds in case they have changed */
+			b = &vout->crop_bounds[vout->cur_disp_output];
+
+			fbnum = vout->output_fb_num[vout->cur_disp_output];
+
+			/*
+			 * For FG overlay, it uses BG window parameter as
+			 * limitation reference; and BG must be enabled to
+			 * support FG.
+			 */
+			if (vout->cur_disp_output == 3) {
+				unsigned int i, ipu_ch = CHAN_NONE;
+				struct fb_info *fbi;
+				mm_segment_t old_fs;
+
+				for (i = 0; i < num_registered_fb; i++) {
+					fbi = registered_fb[i];
+					if (fbi->fbops->fb_ioctl) {
+						old_fs = get_fs();
+						set_fs(KERNEL_DS);
+						fbi->fbops->fb_ioctl(fbi,
+							MXCFB_GET_FB_IPU_CHAN,
+							(unsigned long)&ipu_ch);
+						set_fs(old_fs);
+					}
+					if (ipu_ch == CHAN_NONE) {
+						dev_err(&vdev->dev,
+						"Can't get disp ipu channel\n");
+						retval = -EINVAL;
+						break;
+					}
+
+					if (ipu_ch == MEM_BG_SYNC) {
+						fbnum = i;
+						break;
+					}
+				}
+			}
+
+			b->width = registered_fb[fbnum]->var.xres;
+			b->height = registered_fb[fbnum]->var.yres;
+
+			vout->crop_current = *b;
+			break;
+		}
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_G_PARM:
+	case VIDIOC_ENUMSTD:
+	case VIDIOC_G_STD:
+	case VIDIOC_S_STD:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	default:
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&vout->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L2 interface - ioctl function
+ *
+ * @return  None
+ */
+static int
+mxc_v4l2out_ioctl(struct inode *inode, struct file *file,
+		  unsigned int cmd, unsigned long arg)
+{
+	return video_usercopy(inode, file, cmd, arg, mxc_v4l2out_do_ioctl);
+}
+
+/*!
+ * V4L2 interface - mmap function
+ *
+ * @param file          structure file *
+ *
+ * @param vma           structure vm_area_struct *
+ *
+ * @return status       0 Success, EINTR busy lock error,
+ *                      ENOBUFS remap_page error
+ */
+static int mxc_v4l2out_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *vdev = video_devdata(file);
+	unsigned long size = vma->vm_end - vma->vm_start;
+	int res = 0;
+	int i;
+	vout_data *vout = video_get_drvdata(vdev);
+
+	dev_dbg(&vdev->dev, "pgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	for (i = 0; i < vout->buffer_cnt; i++) {
+		if ((vout->v4l2_bufs[i].m.offset ==
+		     (vma->vm_pgoff << PAGE_SHIFT)) &&
+		    (vout->v4l2_bufs[i].length >= size)) {
+			vout->v4l2_bufs[i].flags |= V4L2_BUF_FLAG_MAPPED;
+			break;
+		}
+	}
+	if (i == vout->buffer_cnt) {
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	/* make buffers inner write-back, outer write-thru cacheable */
+	/* vma->vm_page_prot = pgprot_outer_wrthru(vma->vm_page_prot);*/
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		dev_dbg(&vdev->dev, "mmap remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+      mxc_mmap_exit:
+	up(&vout->busy_lock);
+	return res;
+}
+
+/*!
+ * V4L2 interface - poll function
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_v4l2out_poll(struct file *file, poll_table * wait)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	queue = &vout->v4l_bufq;
+	poll_wait(file, queue, wait);
+
+	up(&vout->busy_lock);
+	return res;
+}
+
+static struct
+file_operations mxc_v4l2out_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_v4l2out_open,
+	.release = mxc_v4l2out_close,
+	.ioctl = mxc_v4l2out_ioctl,
+	.mmap = mxc_v4l2out_mmap,
+	.poll = mxc_v4l2out_poll,
+};
+
+static struct video_device mxc_v4l2out_template = {
+	.name = "MXC Video Output",
+	.vfl_type = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING,
+	.fops = &mxc_v4l2out_fops,
+	.release = video_device_release,
+};
+
+/*!
+ * Probe routine for the framebuffer driver. It is called during the
+ * driver binding process.      The following functions are performed in
+ * this routine: Framebuffer initialization, Memory allocation and
+ * mapping, Framebuffer registration, IPU initialization.
+ *
+ * @return      Appropriate error code to the kernel common code
+ */
+static int mxc_v4l2out_probe(struct platform_device *pdev)
+{
+	int i;
+	vout_data *vout;
+
+	/*
+	 * Allocate sufficient memory for the fb structure
+	 */
+	g_vout = vout = kmalloc(sizeof(vout_data), GFP_KERNEL);
+
+	if (!vout)
+		return 0;
+
+	memset(vout, 0, sizeof(vout_data));
+
+	vout->video_dev = video_device_alloc();
+	if (vout->video_dev == NULL)
+		return -1;
+	vout->video_dev->minor = -1;
+
+	*(vout->video_dev) = mxc_v4l2out_template;
+
+	/* register v4l device */
+	if (video_register_device(vout->video_dev,
+				  VFL_TYPE_GRABBER, video_nr) == -1) {
+		dev_dbg(&pdev->dev, "video_register_device failed\n");
+		return 0;
+	}
+	dev_info(&pdev->dev, "Registered device video%d\n",
+		 vout->video_dev->minor & 0x1f);
+	/*vout->video_dev->dev = &pdev->dev;*/
+
+	video_set_drvdata(vout->video_dev, vout);
+
+	init_MUTEX(&vout->param_lock);
+	init_MUTEX(&vout->busy_lock);
+
+	/* setup outputs and cropping */
+	vout->cur_disp_output = -1;
+	for (i = 0; i < num_registered_fb; i++) {
+		char *idstr = registered_fb[i]->fix.id;
+		if (strncmp(idstr, "DISP", 4) == 0) {
+			int disp_num = idstr[4] - '0';
+			if (disp_num == 3) {
+				if (strcmp(idstr, "DISP3 BG - DI1") == 0)
+					disp_num = 5;
+				else if (strncmp(idstr, "DISP3 BG", 8) == 0)
+					disp_num = 4;
+			}
+			vout->crop_bounds[disp_num].left = 0;
+			vout->crop_bounds[disp_num].top = 0;
+			vout->crop_bounds[disp_num].width =
+			    registered_fb[i]->var.xres;
+			vout->crop_bounds[disp_num].height =
+			    registered_fb[i]->var.yres;
+			vout->output_enabled[disp_num] = true;
+			vout->output_fb_num[disp_num] = i;
+			if (vout->cur_disp_output == -1) {
+				vout->cur_disp_output = disp_num;
+			}
+		}
+
+	}
+	vout->crop_current = vout->crop_bounds[vout->cur_disp_output];
+
+	platform_set_drvdata(pdev, vout);
+
+	return 0;
+}
+
+static int mxc_v4l2out_remove(struct platform_device *pdev)
+{
+	vout_data *vout = platform_get_drvdata(pdev);
+
+	if (vout->video_dev) {
+		if (-1 != vout->video_dev->minor)
+			video_unregister_device(vout->video_dev);
+		else
+			video_device_release(vout->video_dev);
+		vout->video_dev = NULL;
+	}
+
+	platform_set_drvdata(pdev, NULL);
+
+	kfree(vout);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_v4l2out_driver = {
+	.driver = {
+		   .name = "MXC Video Output",
+		   },
+	.probe = mxc_v4l2out_probe,
+	.remove = mxc_v4l2out_remove,
+};
+
+static struct platform_device mxc_v4l2out_device = {
+	.name = "MXC Video Output",
+	.id = 0,
+};
+
+/*!
+ * mxc v4l2 init function
+ *
+ */
+static int mxc_v4l2out_init(void)
+{
+	u8 err = 0;
+
+	err = platform_driver_register(&mxc_v4l2out_driver);
+	if (err == 0) {
+		platform_device_register(&mxc_v4l2out_device);
+	}
+	return err;
+}
+
+/*!
+ * mxc v4l2 cleanup function
+ *
+ */
+static void mxc_v4l2out_clean(void)
+{
+	video_unregister_device(g_vout->video_dev);
+
+	platform_driver_unregister(&mxc_v4l2out_driver);
+	platform_device_unregister(&mxc_v4l2out_device);
+	kfree(g_vout);
+	g_vout = NULL;
+}
+
+module_init(mxc_v4l2out_init);
+module_exit(mxc_v4l2out_clean);
+
+module_param(video_nr, int, 0444);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2-driver for MXC video output");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/output/mxc_v4l2_output.h b/drivers/media/video/mxc/output/mxc_v4l2_output.h
new file mode 100644
index 000000000000..45d713222d86
--- /dev/null
+++ b/drivers/media/video/mxc/output/mxc_v4l2_output.h
@@ -0,0 +1,146 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup MXC_V4L2_OUTPUT MXC V4L2 Video Output Driver
+ */
+/*!
+ * @file mxc_v4l2_output.h
+ *
+ * @brief MXC V4L2 Video Output Driver Header file
+ *
+ * Video4Linux2 Output Device using MXC IPU Post-processing functionality.
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#ifndef __MXC_V4L2_OUTPUT_H__
+#define __MXC_V4L2_OUTPUT_H__
+
+#include <media/v4l2-dev.h>
+
+#ifdef __KERNEL__
+
+#include <linux/ipu.h>
+#include <linux/mxc_v4l2.h>
+#include <linux/videodev2.h>
+
+#define MIN_FRAME_NUM 2
+#define MAX_FRAME_NUM 30
+
+#define MXC_V4L2_OUT_NUM_OUTPUTS        6
+#define MXC_V4L2_OUT_2_SDC              0
+#define MXC_V4L2_OUT_2_ADC              1
+
+
+typedef struct {
+	int list[MAX_FRAME_NUM + 1];
+	int head;
+	int tail;
+} v4l_queue;
+
+/*!
+ * States for the video stream
+ */
+typedef enum {
+	STATE_STREAM_OFF,
+	STATE_STREAM_ON,
+	STATE_STREAM_PAUSED,
+	STATE_STREAM_STOPPING,
+} v4lout_state;
+
+/*!
+ * common v4l2 driver structure.
+ */
+typedef struct _vout_data {
+	struct video_device *video_dev;
+	/*!
+	 * semaphore guard against SMP multithreading
+	 */
+	struct semaphore busy_lock;
+
+	/*!
+	 * number of process that have device open
+	 */
+	int open_count;
+
+	/*!
+	 * params lock for this camera
+	 */
+	struct semaphore param_lock;
+
+	struct timer_list output_timer;
+	unsigned long start_jiffies;
+	u32 frame_count;
+
+	v4l_queue ready_q;
+	v4l_queue done_q;
+
+	s8 next_rdy_ipu_buf;
+	s8 next_done_ipu_buf;
+	s8 ipu_buf[2];
+	s8 ipu_buf_p[2];
+	s8 ipu_buf_n[2];
+	volatile v4lout_state state;
+
+	int cur_disp_output;
+	int output_fb_num[MXC_V4L2_OUT_NUM_OUTPUTS];
+	int output_enabled[MXC_V4L2_OUT_NUM_OUTPUTS];
+	struct v4l2_framebuffer v4l2_fb;
+	int ic_bypass;
+	ipu_channel_t display_ch;
+	ipu_channel_t post_proc_ch;
+
+	/*!
+	 * FRAME_NUM-buffering, so we need a array
+	 */
+	int buffer_cnt;
+	dma_addr_t queue_buf_paddr[MAX_FRAME_NUM];
+	void *queue_buf_vaddr[MAX_FRAME_NUM];
+	u32 queue_buf_size;
+	struct v4l2_buffer v4l2_bufs[MAX_FRAME_NUM];
+	u32 display_buf_size;
+	dma_addr_t display_bufs[2];
+	void *display_bufs_vaddr[2];
+	dma_addr_t rot_pp_bufs[2];
+	void *rot_pp_bufs_vaddr[2];
+
+	/*!
+	 * Poll wait queue
+	 */
+	wait_queue_head_t v4l_bufq;
+
+	/*!
+	 * v4l2 format
+	 */
+	struct v4l2_format v2f;
+	struct v4l2_mxc_offset offset;
+	ipu_rotate_mode_t rotate;
+
+	/* crop */
+	struct v4l2_rect crop_bounds[MXC_V4L2_OUT_NUM_OUTPUTS];
+	struct v4l2_rect crop_current;
+	u32 bytesperline;
+	enum v4l2_field field_fmt;
+	ipu_motion_sel motion_sel;
+
+	/* PP split fot two stripes*/
+	int pp_split; /* 0,1 */
+	struct stripe_param pp_left_stripe;
+	struct stripe_param pp_right_stripe; /* struct for split parameters */
+	/* IC ouput buffer number. Counting from 0 to 3 */
+	int pp_split_buf_num; /*  0..3 */
+
+} vout_data;
+
+#endif
+#endif				/* __MXC_V4L2_OUTPUT_H__ */
diff --git a/drivers/media/video/pxp.c b/drivers/media/video/pxp.c
new file mode 100644
index 000000000000..26e45bbd0e74
--- /dev/null
+++ b/drivers/media/video/pxp.c
@@ -0,0 +1,1318 @@
+/*
+ * Freescale STMP378X PxP driver
+ *
+ * Author: Matt Porter <mporter@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2009 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/vmalloc.h>
+
+#include <media/videobuf-dma-contig.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+
+#include <mach/stmp3xxx_regs.h>
+#include <mach/regs-pxp.h>
+
+#include "pxp.h"
+
+#define PXP_DRIVER_NAME			"stmp3xxx-pxp"
+#define PXP_DRIVER_MAJOR		1
+#define PXP_DRIVER_MINOR		0
+
+#define PXP_DEF_BUFS			2
+#define PXP_MIN_PIX			8
+
+#define V4L2_OUTPUT_TYPE_INTERNAL	4
+
+#define REG_OFFSET	0x10
+#define REGS1_NUMS	16
+#define REGS2_NUMS	5
+#define REGS3_NUMS	32
+static u32 regs1[REGS1_NUMS];
+static u32 regs2[REGS2_NUMS];
+static u32 regs3[REGS3_NUMS];
+
+static struct pxp_data_format pxp_s0_formats[] = {
+	{
+		.name = "24-bit RGB",
+		.bpp = 4,
+		.fourcc = V4L2_PIX_FMT_RGB24,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ctrl_s0_fmt = BV_PXP_CTRL_S0_FORMAT__RGB888,
+	}, {
+		.name = "16-bit RGB 5:6:5",
+		.bpp = 2,
+		.fourcc = V4L2_PIX_FMT_RGB565,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ctrl_s0_fmt = BV_PXP_CTRL_S0_FORMAT__RGB565,
+	}, {
+		.name = "16-bit RGB 5:5:5",
+		.bpp = 2,
+		.fourcc = V4L2_PIX_FMT_RGB555,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ctrl_s0_fmt = BV_PXP_CTRL_S0_FORMAT__RGB555,
+	}, {
+		.name = "YUV 4:2:0 Planar",
+		.bpp = 2,
+		.fourcc = V4L2_PIX_FMT_YUV420,
+		.colorspace = V4L2_COLORSPACE_JPEG,
+		.ctrl_s0_fmt = BV_PXP_CTRL_S0_FORMAT__YUV420,
+	}, {
+		.name = "YUV 4:2:2 Planar",
+		.bpp = 2,
+		.fourcc = V4L2_PIX_FMT_YUV422P,
+		.colorspace = V4L2_COLORSPACE_JPEG,
+		.ctrl_s0_fmt = BV_PXP_CTRL_S0_FORMAT__YUV422,
+	},
+};
+
+struct v4l2_queryctrl pxp_controls[] = {
+	{
+		.id 		= V4L2_CID_HFLIP,
+		.type 		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name 		= "Horizontal Flip",
+		.minimum 	= 0,
+		.maximum 	= 1,
+		.step 		= 1,
+		.default_value	= 0,
+		.flags		= 0,
+	}, {
+		.id		= V4L2_CID_VFLIP,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "Vertical Flip",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 0,
+		.flags		= 0,
+	}, {
+		.id		= V4L2_CID_PRIVATE_BASE,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Rotation",
+		.minimum	= 0,
+		.maximum	= 270,
+		.step		= 90,
+		.default_value	= 0,
+		.flags		= 0,
+	}, {
+		.id		= V4L2_CID_PRIVATE_BASE + 1,
+		.name		= "Background Color",
+		.minimum	= 0,
+		.maximum	= 0xFFFFFF,
+		.step		= 1,
+		.default_value	= 0,
+		.flags		= 0,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+	}, {
+		.id		= V4L2_CID_PRIVATE_BASE + 2,
+		.name		= "Set S0 Chromakey",
+		.minimum	= -1,
+		.maximum	= 0xFFFFFF,
+		.step		= 1,
+		.default_value	= -1,
+		.flags		= 0,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+	}, {
+		.id		= V4L2_CID_PRIVATE_BASE + 3,
+		.name		= "YUV Colorspace",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 0,
+		.flags		= 0,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+	},
+};
+
+static void pxp_set_ctrl(struct pxps *pxp)
+{
+	u32 ctrl;
+
+	ctrl = BF_PXP_CTRL_S0_FORMAT(pxp->s0_fmt->ctrl_s0_fmt);
+	ctrl |=
+	BF_PXP_CTRL_OUTPUT_RGB_FORMAT(BV_PXP_CTRL_OUTPUT_RGB_FORMAT__RGB888);
+	ctrl |= BM_PXP_CTRL_CROP;
+
+	if (pxp->scaling)
+		ctrl |= BM_PXP_CTRL_SCALE;
+	if (pxp->vflip)
+		ctrl |= BM_PXP_CTRL_VFLIP;
+	if (pxp->hflip)
+		ctrl |= BM_PXP_CTRL_HFLIP;
+	if (pxp->rotate)
+		ctrl |= BF_PXP_CTRL_ROTATE(pxp->rotate/90);
+
+	ctrl |= BM_PXP_CTRL_IRQ_ENABLE;
+	if (pxp->active)
+		ctrl |= BM_PXP_CTRL_ENABLE;
+
+	HW_PXP_CTRL_WR(ctrl);
+}
+
+static void pxp_set_rgbbuf(struct pxps *pxp)
+{
+	HW_PXP_RGBBUF_WR(pxp->outb_phys);
+	/* Always equal to the FB size */
+	HW_PXP_RGBSIZE_WR(BF_PXP_RGBSIZE_WIDTH(pxp->fb.fmt.width) |
+			  BF_PXP_RGBSIZE_HEIGHT(pxp->fb.fmt.height));
+}
+
+static void pxp_set_s0colorkey(struct pxps *pxp)
+{
+	/* Low and high are set equal. V4L does not allow a chromakey range */
+	if (pxp->s0_chromakey == -1) {
+		/* disable color key */
+		HW_PXP_S0COLORKEYLOW_WR(0xFFFFFF);
+		HW_PXP_S0COLORKEYHIGH_WR(0x0);
+	} else {
+		HW_PXP_S0COLORKEYLOW_WR(pxp->s0_chromakey);
+		HW_PXP_S0COLORKEYHIGH_WR(pxp->s0_chromakey);
+	}
+}
+
+static void pxp_set_s1colorkey(struct pxps *pxp)
+{
+	/* Low and high are set equal. V4L does not allow a chromakey range */
+	if (pxp->s1_chromakey_state != 0 && pxp->s1_chromakey != -1) {
+		HW_PXP_OLCOLORKEYLOW_WR(pxp->s1_chromakey);
+		HW_PXP_OLCOLORKEYHIGH_WR(pxp->s1_chromakey);
+	} else {
+		/* disable color key */
+		HW_PXP_OLCOLORKEYLOW_WR(0xFFFFFF);
+		HW_PXP_OLCOLORKEYHIGH_WR(0x0);
+	}
+}
+
+static void pxp_set_oln(struct pxps *pxp)
+{
+	HW_PXP_OLn_WR(0, (u32)pxp->fb.base);
+	HW_PXP_OLnSIZE_WR(0, BF_PXP_OLnSIZE_WIDTH(pxp->fb.fmt.width >> 3) |
+				BF_PXP_OLnSIZE_HEIGHT(pxp->fb.fmt.height >> 3));
+}
+
+static void pxp_set_olparam(struct pxps *pxp)
+{
+	u32 olparam;
+	struct v4l2_pix_format *fmt = &pxp->fb.fmt;
+
+	olparam = BF_PXP_OLnPARAM_ALPHA(pxp->global_alpha);
+	if (fmt->pixelformat == V4L2_PIX_FMT_RGB24)
+		olparam |=
+		BF_PXP_OLnPARAM_FORMAT(BV_PXP_OLnPARAM_FORMAT__RGB888);
+	else
+		olparam |=
+		BF_PXP_OLnPARAM_FORMAT(BV_PXP_OLnPARAM_FORMAT__RGB565);
+	if (pxp->global_alpha_state)
+		olparam |= BF_PXP_OLnPARAM_ALPHA_CNTL(
+				BV_PXP_OLnPARAM_ALPHA_CNTL__Override);
+	if (pxp->s1_chromakey_state)
+		olparam |= BM_PXP_OLnPARAM_ENABLE_COLORKEY;
+	if (pxp->overlay_state)
+		olparam |= BM_PXP_OLnPARAM_ENABLE;
+	HW_PXP_OLnPARAM_WR(0, olparam);
+}
+
+static void pxp_set_s0param(struct pxps *pxp)
+{
+	u32 s0param;
+
+	s0param = BF_PXP_S0PARAM_XBASE(pxp->drect.left >> 3);
+	s0param |= BF_PXP_S0PARAM_YBASE(pxp->drect.top >> 3);
+	s0param |= BF_PXP_S0PARAM_WIDTH(pxp->s0_width >> 3);
+	s0param |= BF_PXP_S0PARAM_HEIGHT(pxp->s0_height >> 3);
+	HW_PXP_S0PARAM_WR(s0param);
+}
+
+static void pxp_set_s0crop(struct pxps *pxp)
+{
+	u32 s0crop;
+
+	s0crop = BF_PXP_S0CROP_XBASE(pxp->srect.left >> 3);
+	s0crop |= BF_PXP_S0CROP_YBASE(pxp->srect.top >> 3);
+	s0crop |= BF_PXP_S0CROP_WIDTH(pxp->drect.width >> 3);
+	s0crop |= BF_PXP_S0CROP_HEIGHT(pxp->drect.height >> 3);
+	HW_PXP_S0CROP_WR(s0crop);
+}
+
+static int pxp_set_scaling(struct pxps *pxp)
+{
+	int ret = 0;
+	u32 xscale, yscale, s0scale;
+
+	if ((pxp->s0_fmt->fourcc != V4L2_PIX_FMT_YUV420) &&
+		(pxp->s0_fmt->fourcc != V4L2_PIX_FMT_YUV422P)) {
+		pxp->scaling = 0;
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if ((pxp->srect.width == pxp->drect.width) &&
+		(pxp->srect.height == pxp->drect.height)) {
+		pxp->scaling = 0;
+		goto out;
+	}
+
+	pxp->scaling = 1;
+	xscale = pxp->srect.width * 0x1000 / pxp->drect.width;
+	yscale = pxp->srect.height * 0x1000 / pxp->drect.height;
+	s0scale = BF_PXP_S0SCALE_YSCALE(yscale) |
+		  BF_PXP_S0SCALE_XSCALE(xscale);
+	HW_PXP_S0SCALE_WR(s0scale);
+
+out:
+	pxp_set_ctrl(pxp);
+
+	return ret;
+}
+
+static int pxp_set_fbinfo(struct pxps *pxp)
+{
+	struct fb_var_screeninfo var;
+	struct fb_fix_screeninfo fix;
+	struct v4l2_framebuffer *fb = &pxp->fb;
+	int err;
+
+	err = stmp3xxxfb_get_info(&var, &fix);
+
+	fb->fmt.width = var.xres;
+	fb->fmt.height = var.yres;
+	if (var.bits_per_pixel == 16)
+		fb->fmt.pixelformat = V4L2_PIX_FMT_RGB565;
+	else
+		fb->fmt.pixelformat = V4L2_PIX_FMT_RGB24;
+	fb->base = (void *)fix.smem_start;
+	return err;
+}
+
+static void pxp_set_s0bg(struct pxps *pxp)
+{
+	HW_PXP_S0BACKGROUND_WR(pxp->s0_bgcolor);
+}
+
+static void pxp_set_csc(struct pxps *pxp)
+{
+	if (pxp->yuv) {
+		/* YUV colorspace */
+		HW_PXP_CSCCOEFF0_WR(0x04030000);
+		HW_PXP_CSCCOEFF1_WR(0x01230208);
+		HW_PXP_CSCCOEFF2_WR(0x076b079c);
+	} else {
+		/* YCrCb colorspace */
+		HW_PXP_CSCCOEFF0_WR(0x84ab01f0);
+		HW_PXP_CSCCOEFF1_WR(0x01230204);
+		HW_PXP_CSCCOEFF2_WR(0x0730079c);
+	}
+}
+
+static int pxp_set_cstate(struct pxps *pxp, struct v4l2_control *vc)
+{
+
+	if (vc->id == V4L2_CID_HFLIP)
+		pxp->hflip = vc->value;
+	else if (vc->id == V4L2_CID_VFLIP)
+		pxp->vflip = vc->value;
+	else if (vc->id == V4L2_CID_PRIVATE_BASE) {
+		if (vc->value % 90)
+			return -ERANGE;
+		pxp->rotate = vc->value;
+	} else if (vc->id == V4L2_CID_PRIVATE_BASE + 1) {
+		pxp->s0_bgcolor = vc->value;
+		pxp_set_s0bg(pxp);
+	} else if (vc->id == V4L2_CID_PRIVATE_BASE + 2) {
+		pxp->s0_chromakey = vc->value;
+		pxp_set_s0colorkey(pxp);
+	} else if (vc->id == V4L2_CID_PRIVATE_BASE + 3) {
+		pxp->yuv = vc->value;
+		pxp_set_csc(pxp);
+	}
+
+	pxp_set_ctrl(pxp);
+
+	return 0;
+}
+
+static int pxp_get_cstate(struct pxps *pxp, struct v4l2_control *vc)
+{
+	if (vc->id == V4L2_CID_HFLIP)
+		vc->value = pxp->hflip;
+	else if (vc->id == V4L2_CID_VFLIP)
+		vc->value = pxp->vflip;
+	else if (vc->id == V4L2_CID_PRIVATE_BASE)
+		vc->value = pxp->rotate;
+	else if (vc->id == V4L2_CID_PRIVATE_BASE + 1)
+		vc->value = pxp->s0_bgcolor;
+	else if (vc->id == V4L2_CID_PRIVATE_BASE + 2)
+		vc->value = pxp->s0_chromakey;
+	else if (vc->id == V4L2_CID_PRIVATE_BASE + 3)
+		vc->value = pxp->yuv;
+
+	return 0;
+}
+
+static int pxp_enumoutput(struct file *file, void *fh,
+			struct v4l2_output *o)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	if ((o->index < 0) || (o->index > 1))
+		return -EINVAL;
+
+	memset(o, 0, sizeof(struct v4l2_output));
+	if (o->index == 0) {
+		strcpy(o->name, "PxP Display Output");
+		pxp->output = 0;
+	} else {
+		strcpy(o->name, "PxP Virtual Output");
+		pxp->output = 1;
+	}
+	o->type = V4L2_OUTPUT_TYPE_INTERNAL;
+	o->std = 0;
+	o->reserved[0] = pxp->outb_phys;
+
+	return 0;
+}
+
+static int pxp_g_output(struct file *file, void *fh,
+			unsigned int *i)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	*i = pxp->output;
+
+	return 0;
+}
+
+static int pxp_s_output(struct file *file, void *fh,
+			unsigned int i)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct v4l2_pix_format *fmt = &pxp->fb.fmt;
+	int bpp;
+
+	if ((i < 0) || (i > 1))
+		return -EINVAL;
+
+	if (pxp->outb)
+		goto out;
+
+	/* Output buffer is same format as fbdev */
+	if (fmt->pixelformat == V4L2_PIX_FMT_RGB24)
+		bpp = 4;
+	else
+		bpp = 2;
+
+	pxp->outb = kmalloc(fmt->width * fmt->height * bpp, GFP_KERNEL);
+	pxp->outb_phys = virt_to_phys(pxp->outb);
+	dma_map_single(NULL, pxp->outb,
+			fmt->width * fmt->height * bpp, DMA_TO_DEVICE);
+
+out:
+	pxp_set_rgbbuf(pxp);
+
+	return 0;
+}
+
+static int pxp_enum_fmt_video_output(struct file *file, void *fh,
+				struct v4l2_fmtdesc *fmt)
+{
+	enum v4l2_buf_type type = fmt->type;
+	int index = fmt->index;
+
+	if ((fmt->index < 0) || (fmt->index >= ARRAY_SIZE(pxp_s0_formats)))
+		return -EINVAL;
+
+	memset(fmt, 0, sizeof(struct v4l2_fmtdesc));
+	fmt->index = index;
+	fmt->type = type;
+	fmt->pixelformat = pxp_s0_formats[index].fourcc;
+	strcpy(fmt->description, pxp_s0_formats[index].name);
+
+	return 0;
+}
+
+static int pxp_g_fmt_video_output(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pf = &f->fmt.pix;
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct pxp_data_format *fmt = pxp->s0_fmt;
+
+	pf->width = pxp->s0_width;
+	pf->height = pxp->s0_height;
+	pf->pixelformat = fmt->fourcc;
+	pf->field = V4L2_FIELD_NONE;
+	pf->bytesperline = fmt->bpp * pf->width;
+	pf->sizeimage = pf->bytesperline * pf->height;
+	pf->colorspace = fmt->colorspace;
+	pf->priv = 0;
+
+	return 0;
+}
+
+static struct pxp_data_format *pxp_get_format(struct v4l2_format *f)
+{
+	struct pxp_data_format *fmt;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pxp_s0_formats); i++) {
+		fmt = &pxp_s0_formats[i];
+		if (fmt->fourcc == f->fmt.pix.pixelformat)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(pxp_s0_formats))
+		return NULL;
+
+	return &pxp_s0_formats[i];
+}
+
+static int pxp_try_fmt_video_output(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	int w = f->fmt.pix.width;
+	int h = f->fmt.pix.height;
+	struct pxp_data_format *fmt = pxp_get_format(f);
+
+	if (!fmt)
+		return -EINVAL;
+
+	w = min(w, 2040);
+	w = max(w, 8);
+	h = min(h, 2040);
+	h = max(h, 8);
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.width = w;
+	f->fmt.pix.height = h;
+	f->fmt.pix.pixelformat = fmt->fourcc;
+
+	return 0;
+}
+
+static int pxp_s_fmt_video_output(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct v4l2_pix_format *pf = &f->fmt.pix;
+	int ret = pxp_try_fmt_video_output(file, fh, f);
+
+	if (ret == 0) {
+		pxp->s0_fmt = pxp_get_format(f);
+		pxp->s0_width = pf->width;
+		pxp->s0_height = pf->height;
+		pxp_set_ctrl(pxp);
+		pxp_set_s0param(pxp);
+	}
+
+	return ret;
+}
+
+static int pxp_g_fmt_output_overlay(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct v4l2_window *wf = &f->fmt.win;
+
+	memset(wf, 0, sizeof(struct v4l2_window));
+	wf->chromakey = pxp->s1_chromakey;
+	wf->global_alpha = pxp->global_alpha;
+	wf->field = V4L2_FIELD_NONE;
+	wf->clips = NULL;
+	wf->clipcount = 0;
+	wf->bitmap = NULL;
+	wf->w.left = pxp->srect.left;
+	wf->w.top = pxp->srect.top;
+	wf->w.width = pxp->srect.width;
+	wf->w.height = pxp->srect.height;
+
+	return 0;
+}
+
+static int pxp_try_fmt_output_overlay(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct v4l2_window *wf = &f->fmt.win;
+	struct v4l2_rect srect;
+	u32 s1_chromakey = wf->chromakey;
+	u8 global_alpha = wf->global_alpha;
+
+	memcpy(&srect, &(wf->w), sizeof(struct v4l2_rect));
+
+	pxp_g_fmt_output_overlay(file, fh, f);
+
+	wf->chromakey = s1_chromakey;
+	wf->global_alpha = global_alpha;
+
+	/* Constrain parameters to the input buffer */
+	wf->w.left = srect.left;
+	wf->w.top = srect.top;
+	wf->w.width = min(srect.width, ((__s32)pxp->s0_width - wf->w.left));
+	wf->w.height = min(srect.height, ((__s32)pxp->s0_height - wf->w.top));
+
+	return 0;
+}
+
+static int pxp_s_fmt_output_overlay(struct file *file, void *fh,
+					struct v4l2_format *f)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct v4l2_window *wf = &f->fmt.win;
+	int ret = pxp_try_fmt_output_overlay(file, fh, f);
+
+	if (ret == 0) {
+		pxp->srect.left = wf->w.left;
+		pxp->srect.top = wf->w.top;
+		pxp->srect.width = wf->w.width;
+		pxp->srect.height = wf->w.height;
+		pxp->global_alpha = wf->global_alpha;
+		pxp->s1_chromakey = wf->chromakey;
+		pxp_set_s0param(pxp);
+		pxp_set_s0crop(pxp);
+		pxp_set_scaling(pxp);
+		pxp_set_olparam(pxp);
+		pxp_set_s1colorkey(pxp);
+	}
+
+	return ret;
+}
+
+static int pxp_reqbufs(struct file *file, void *priv,
+			struct v4l2_requestbuffers *r)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	return videobuf_reqbufs(&pxp->s0_vbq, r);
+}
+
+static int pxp_querybuf(struct file *file, void *priv,
+			struct v4l2_buffer *b)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	return videobuf_querybuf(&pxp->s0_vbq, b);
+}
+
+static int pxp_qbuf(struct file *file, void *priv,
+			struct v4l2_buffer *b)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	return videobuf_qbuf(&pxp->s0_vbq, b);
+}
+
+static int pxp_dqbuf(struct file *file, void *priv,
+			struct v4l2_buffer *b)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	return videobuf_dqbuf(&pxp->s0_vbq, b, file->f_flags & O_NONBLOCK);
+}
+
+static int pxp_streamon(struct file *file, void *priv,
+			enum v4l2_buf_type t)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	int ret = 0;
+
+	if ((t != V4L2_BUF_TYPE_VIDEO_OUTPUT))
+		return -EINVAL;
+
+	ret = videobuf_streamon(&pxp->s0_vbq);
+
+	if (!ret && (pxp->output == 0))
+		stmp3xxxfb_cfg_pxp(1, pxp->outb_phys);
+
+	return ret;
+}
+
+static int pxp_streamoff(struct file *file, void *priv,
+			enum v4l2_buf_type t)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	int ret = 0;
+
+	if ((t != V4L2_BUF_TYPE_VIDEO_OUTPUT))
+		return -EINVAL;
+
+	ret = videobuf_streamoff(&pxp->s0_vbq);
+
+	if (!ret)
+		stmp3xxxfb_cfg_pxp(0, 0);
+
+	return ret;
+}
+
+static int pxp_buf_setup(struct videobuf_queue *q,
+			unsigned int *count, unsigned *size)
+{
+	struct pxps *pxp = q->priv_data;
+
+	*size = pxp->s0_width * pxp->s0_height * pxp->s0_fmt->bpp;
+
+	if (0 == *count)
+		*count = PXP_DEF_BUFS;
+
+	return 0;
+}
+
+static void pxp_buf_free(struct videobuf_queue *q, struct videobuf_buffer *vb)
+{
+	if (in_interrupt())
+		BUG();
+
+	videobuf_dma_contig_free(q, vb);
+
+	vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static int pxp_buf_prepare(struct videobuf_queue *q,
+			struct videobuf_buffer *vb,
+			enum v4l2_field field)
+{
+	struct pxps *pxp = q->priv_data;
+	int ret = 0;
+
+	vb->width = pxp->s0_width;
+	vb->height = pxp->s0_height;
+	vb->size = vb->width * vb->height * pxp->s0_fmt->bpp;
+	vb->field = V4L2_FIELD_NONE;
+	vb->state = VIDEOBUF_NEEDS_INIT;
+
+
+	ret = videobuf_iolock(q, vb, NULL);
+	if (ret)
+		goto fail;
+	vb->state = VIDEOBUF_PREPARED;
+
+	return 0;
+
+fail:
+	pxp_buf_free(q, vb);
+	return ret;
+}
+
+static void pxp_buf_output(struct pxps *pxp)
+{
+	dma_addr_t Y, U, V;
+
+	if (pxp->active) {
+		pxp->active->state = VIDEOBUF_ACTIVE;
+		Y = videobuf_to_dma_contig(pxp->active);
+		HW_PXP_S0BUF_WR(Y);
+		if ((pxp->s0_fmt->fourcc == V4L2_PIX_FMT_YUV420) ||
+		    (pxp->s0_fmt->fourcc == V4L2_PIX_FMT_YUV422P)) {
+			int s = 1;	/* default to YUV 4:2:2 */
+			if (pxp->s0_fmt->fourcc == V4L2_PIX_FMT_YUV420)
+				s = 2;
+			U = Y + (pxp->s0_width * pxp->s0_height);
+			V = U + ((pxp->s0_width * pxp->s0_height) >> s);
+			HW_PXP_S0UBUF_WR(U);
+			HW_PXP_S0VBUF_WR(V);
+		}
+		HW_PXP_CTRL_SET(BM_PXP_CTRL_ENABLE);
+	}
+}
+
+static void pxp_buf_queue(struct videobuf_queue *q,
+			struct videobuf_buffer *vb)
+{
+	struct pxps *pxp = q->priv_data;
+	unsigned long flags;
+
+	spin_lock_irqsave(&pxp->lock, flags);
+
+	list_add_tail(&vb->queue, &pxp->outq);
+	vb->state = VIDEOBUF_QUEUED;
+
+	if (!pxp->active) {
+		pxp->active = vb;
+		pxp_buf_output(pxp);
+	}
+
+	spin_unlock_irqrestore(&pxp->lock, flags);
+}
+
+static void pxp_buf_release(struct videobuf_queue *q,
+			struct videobuf_buffer *vb)
+{
+	pxp_buf_free(q, vb);
+}
+
+static struct videobuf_queue_ops pxp_vbq_ops = {
+	.buf_setup	= pxp_buf_setup,
+	.buf_prepare	= pxp_buf_prepare,
+	.buf_queue	= pxp_buf_queue,
+	.buf_release	= pxp_buf_release,
+};
+
+static int pxp_querycap(struct file *file, void *fh,
+			struct v4l2_capability *cap)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	memset(cap, 0, sizeof(*cap));
+	strcpy(cap->driver, "pxp");
+	strcpy(cap->card, "pxp");
+	strlcpy(cap->bus_info, pxp->pdev->dev.bus_id, sizeof(cap->bus_info));
+
+	cap->version = (PXP_DRIVER_MAJOR << 8) + PXP_DRIVER_MINOR;
+
+	cap->capabilities = V4L2_CAP_VIDEO_OUTPUT |
+				V4L2_CAP_VIDEO_OUTPUT_OVERLAY |
+				V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+static int pxp_g_fbuf(struct file *file, void *priv,
+			struct v4l2_framebuffer *fb)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	memset(fb, 0, sizeof(*fb));
+
+	fb->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
+			 V4L2_FBUF_CAP_CHROMAKEY |
+			 V4L2_FBUF_CAP_LOCAL_ALPHA |
+			 V4L2_FBUF_CAP_GLOBAL_ALPHA;
+
+	if (pxp->global_alpha_state)
+		fb->flags |= V4L2_FBUF_FLAG_GLOBAL_ALPHA;
+	if (pxp->local_alpha_state)
+		fb->flags |= V4L2_FBUF_FLAG_LOCAL_ALPHA;
+	if (pxp->s1_chromakey_state)
+		fb->flags |= V4L2_FBUF_FLAG_CHROMAKEY;
+
+	return 0;
+}
+
+static int pxp_s_fbuf(struct file *file, void *priv,
+			struct v4l2_framebuffer *fb)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	pxp->overlay_state =
+		(fb->flags & V4L2_FBUF_FLAG_OVERLAY) != 0;
+	pxp->global_alpha_state =
+		(fb->flags & V4L2_FBUF_FLAG_GLOBAL_ALPHA) != 0;
+	pxp->local_alpha_state =
+		(fb->flags & V4L2_FBUF_FLAG_LOCAL_ALPHA) != 0;
+	/* Global alpha overrides local alpha if both are requested */
+	if (pxp->global_alpha_state && pxp->local_alpha_state)
+		pxp->local_alpha_state = 0;
+	pxp->s1_chromakey_state =
+		(fb->flags & V4L2_FBUF_FLAG_CHROMAKEY) != 0;
+
+	pxp_set_olparam(pxp);
+	pxp_set_s0crop(pxp);
+	pxp_set_scaling(pxp);
+
+	return 0;
+}
+
+static int pxp_g_crop(struct file *file, void *fh,
+			struct v4l2_crop *c)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	if (c->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY)
+		return -EINVAL;
+
+	c->c.left = pxp->drect.left;
+	c->c.top = pxp->drect.top;
+	c->c.width = pxp->drect.width;
+	c->c.height = pxp->drect.height;
+
+	return 0;
+}
+
+static int pxp_s_crop(struct file *file, void *fh,
+			struct v4l2_crop *c)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	int l = c->c.left;
+	int t = c->c.top;
+	int w = c->c.width;
+	int h = c->c.height;
+	int fbw = pxp->fb.fmt.width;
+	int fbh = pxp->fb.fmt.height;
+
+	if (c->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY)
+		return -EINVAL;
+
+	/* Constrain parameters to FB limits */
+	w = min(w, fbw);
+	w = max(w, PXP_MIN_PIX);
+	h = min(h, fbh);
+	h = max(h, PXP_MIN_PIX);
+	if ((l + w) > fbw)
+		l = 0;
+	if ((t + h) > fbh)
+		t = 0;
+
+	/* Round up values to PxP pixel block */
+	l = roundup(l, PXP_MIN_PIX);
+	t = roundup(t, PXP_MIN_PIX);
+	w = roundup(w, PXP_MIN_PIX);
+	h = roundup(h, PXP_MIN_PIX);
+
+	pxp->drect.left = l;
+	pxp->drect.top = t;
+	pxp->drect.width = w;
+	pxp->drect.height = h;
+
+	pxp_set_s0param(pxp);
+	pxp_set_s0crop(pxp);
+	pxp_set_scaling(pxp);
+
+	return 0;
+}
+
+static int pxp_queryctrl(struct file *file, void *priv,
+			 struct v4l2_queryctrl *qc)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pxp_controls); i++)
+		if (qc->id && qc->id == pxp_controls[i].id) {
+			memcpy(qc, &(pxp_controls[i]), sizeof(*qc));
+			return 0;
+		}
+
+	return -EINVAL;
+}
+
+static int pxp_g_ctrl(struct file *file, void *priv,
+			 struct v4l2_control *vc)
+{
+	int i;
+
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	for (i = 0; i < ARRAY_SIZE(pxp_controls); i++)
+		if (vc->id == pxp_controls[i].id)
+			return pxp_get_cstate(pxp, vc);
+
+	return -EINVAL;
+}
+
+static int pxp_s_ctrl(struct file *file, void *priv,
+			 struct v4l2_control *vc)
+{
+	int i;
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	for (i = 0; i < ARRAY_SIZE(pxp_controls); i++)
+		if (vc->id == pxp_controls[i].id) {
+			if (vc->value < pxp_controls[i].minimum ||
+			    vc->value > pxp_controls[i].maximum)
+				return -ERANGE;
+			return pxp_set_cstate(pxp, vc);
+		}
+
+	return -EINVAL;
+}
+
+void pxp_release(struct video_device *vfd)
+{
+	struct pxps *pxp = video_get_drvdata(vfd);
+
+	spin_lock(&pxp->lock);
+	video_device_release(vfd);
+	spin_unlock(&pxp->lock);
+}
+
+static int pxp_hw_init(struct pxps *pxp)
+{
+	struct fb_var_screeninfo var;
+	struct fb_fix_screeninfo fix;
+	int err;
+
+	err = stmp3xxxfb_get_info(&var, &fix);
+	if (err)
+		return err;
+
+	/* Pull PxP out of reset */
+	HW_PXP_CTRL_WR(0);
+
+	/* Config defaults */
+	pxp->active = NULL;
+
+	pxp->s0_fmt = &pxp_s0_formats[0];
+	pxp->drect.left = pxp->srect.left = 0;
+	pxp->drect.top = pxp->srect.top = 0;
+	pxp->drect.width = pxp->srect.width = pxp->s0_width = var.xres;
+	pxp->drect.height = pxp->srect.height = pxp->s0_height = var.yres;
+	pxp->s0_bgcolor = 0;
+
+	pxp->output = 0;
+	err = pxp_set_fbinfo(pxp);
+	if (err)
+		return err;
+
+	pxp->scaling = 0;
+	pxp->hflip = 0;
+	pxp->vflip = 0;
+	pxp->rotate = 0;
+	pxp->yuv = 0;
+
+	pxp->overlay_state = 0;
+	pxp->global_alpha_state = 0;
+	pxp->global_alpha = 0;
+	pxp->local_alpha_state = 0;
+	pxp->s1_chromakey_state = 0;
+	pxp->s1_chromakey = -1;
+	pxp->s0_chromakey = -1;
+
+	/* Write default h/w config */
+	pxp_set_ctrl(pxp);
+	pxp_set_s0param(pxp);
+	pxp_set_s0crop(pxp);
+	pxp_set_oln(pxp);
+	pxp_set_olparam(pxp);
+	pxp_set_s0colorkey(pxp);
+	pxp_set_s1colorkey(pxp);
+	pxp_set_csc(pxp);
+
+	return 0;
+}
+
+static int pxp_open(struct inode *inode, struct file *file)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	int ret = 0;
+
+	mutex_lock(&pxp->mutex);
+	pxp->users++;
+
+	if (pxp->users > 1) {
+		pxp->users--;
+		ret = -EBUSY;
+		goto out;
+	}
+
+out:
+	mutex_unlock(&pxp->mutex);
+	if (ret)
+		return ret;
+
+	videobuf_queue_dma_contig_init(&pxp->s0_vbq,
+				&pxp_vbq_ops,
+				&pxp->pdev->dev,
+				&pxp->lock,
+				V4L2_BUF_TYPE_VIDEO_OUTPUT,
+				V4L2_FIELD_NONE,
+				sizeof(struct videobuf_buffer),
+				pxp);
+
+	return 0;
+}
+
+static int pxp_close(struct inode *inode, struct file *file)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	videobuf_stop(&pxp->s0_vbq);
+	videobuf_mmap_free(&pxp->s0_vbq);
+
+	mutex_lock(&pxp->mutex);
+	pxp->users--;
+	mutex_unlock(&pxp->mutex);
+
+	return 0;
+}
+
+static int pxp_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	int ret;
+
+	ret = videobuf_mmap_mapper(&pxp->s0_vbq, vma);
+
+	return ret;
+}
+
+static const struct file_operations pxp_fops = {
+	.owner		= THIS_MODULE,
+	.open		= pxp_open,
+	.release	= pxp_close,
+	.ioctl		= video_ioctl2,
+	.mmap		= pxp_mmap,
+};
+
+static const struct v4l2_ioctl_ops pxp_ioctl_ops = {
+	.vidioc_querycap		= pxp_querycap,
+
+	.vidioc_reqbufs			= pxp_reqbufs,
+	.vidioc_querybuf		= pxp_querybuf,
+	.vidioc_qbuf			= pxp_qbuf,
+	.vidioc_dqbuf			= pxp_dqbuf,
+
+	.vidioc_streamon		= pxp_streamon,
+	.vidioc_streamoff		= pxp_streamoff,
+
+	.vidioc_enum_output		= pxp_enumoutput,
+	.vidioc_g_output		= pxp_g_output,
+	.vidioc_s_output		= pxp_s_output,
+
+	.vidioc_enum_fmt_vid_out	= pxp_enum_fmt_video_output,
+	.vidioc_try_fmt_vid_out		= pxp_try_fmt_video_output,
+	.vidioc_g_fmt_vid_out		= pxp_g_fmt_video_output,
+	.vidioc_s_fmt_vid_out		= pxp_s_fmt_video_output,
+
+	.vidioc_try_fmt_vid_out_overlay	= pxp_try_fmt_output_overlay,
+	.vidioc_g_fmt_vid_out_overlay	= pxp_g_fmt_output_overlay,
+	.vidioc_s_fmt_vid_out_overlay	= pxp_s_fmt_output_overlay,
+
+	.vidioc_g_fbuf			= pxp_g_fbuf,
+	.vidioc_s_fbuf			= pxp_s_fbuf,
+
+	.vidioc_g_crop			= pxp_g_crop,
+	.vidioc_s_crop			= pxp_s_crop,
+
+	.vidioc_queryctrl		= pxp_queryctrl,
+	.vidioc_g_ctrl			= pxp_g_ctrl,
+	.vidioc_s_ctrl			= pxp_s_ctrl,
+};
+
+static const struct video_device pxp_template = {
+	.name				= "PxP",
+	.vfl_type			= VID_TYPE_OVERLAY |
+					  VID_TYPE_CLIPPING |
+					  VID_TYPE_SCALES,
+	.fops				= &pxp_fops,
+	.release			= pxp_release,
+	.minor				= -1,
+	.ioctl_ops			= &pxp_ioctl_ops,
+};
+
+static irqreturn_t pxp_irq(int irq, void *dev_id)
+{
+	struct pxps *pxp = (struct pxps *)dev_id;
+	struct videobuf_buffer *vb;
+	unsigned long flags;
+
+	spin_lock_irqsave(&pxp->lock, flags);
+
+	HW_PXP_STAT_CLR(BM_PXP_STAT_IRQ);
+
+	vb = pxp->active;
+	vb->state = VIDEOBUF_DONE;
+	do_gettimeofday(&vb->ts);
+	vb->field_count++;
+
+	list_del_init(&vb->queue);
+
+	if (list_empty(&pxp->outq)) {
+		pxp->active = NULL;
+		goto out;
+	}
+
+	pxp->active = list_entry(pxp->outq.next,
+				struct videobuf_buffer,
+				queue);
+
+	pxp_buf_output(pxp);
+
+out:
+	wake_up(&vb->done);
+
+	spin_unlock_irqrestore(&pxp->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static int pxp_probe(struct platform_device *pdev)
+{
+	struct pxps *pxp;
+	struct resource *res;
+	int irq;
+	int err = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (!res || irq < 0) {
+		err = -ENODEV;
+		goto exit;
+	}
+
+	pxp = kzalloc(sizeof(*pxp), GFP_KERNEL);
+	if (!pxp) {
+		dev_err(&pdev->dev, "failed to allocate control object\n");
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	dev_set_drvdata(&pdev->dev, pxp);
+	pxp->res = res;
+	pxp->irq = irq;
+
+	INIT_LIST_HEAD(&pxp->outq);
+	spin_lock_init(&pxp->lock);
+	mutex_init(&pxp->mutex);
+
+	if (!request_mem_region(res->start, res->end - res->start + 1,
+				PXP_DRIVER_NAME)) {
+		err = -EBUSY;
+		goto freepxp;
+	}
+
+	pxp->regs = (void __iomem *)res->start; /* it is already ioremapped */
+	pxp->pdev = pdev;
+
+	err = request_irq(pxp->irq, pxp_irq, 0, PXP_DRIVER_NAME, pxp);
+
+	if (err) {
+		dev_err(&pdev->dev, "interrupt register failed\n");
+		goto release;
+	}
+
+	pxp->vdev = video_device_alloc();
+	if (!pxp->vdev) {
+		dev_err(&pdev->dev, "video_device_alloc() failed\n");
+		err = -ENOMEM;
+		goto freeirq;
+	}
+
+	memcpy(pxp->vdev, &pxp_template, sizeof(pxp_template));
+	video_set_drvdata(pxp->vdev, pxp);
+
+	err = video_register_device(pxp->vdev, VFL_TYPE_GRABBER, 0);
+	if (err) {
+		dev_err(&pdev->dev, "failed to register video device\n");
+		goto freevdev;
+	}
+
+	err = pxp_hw_init(pxp);
+	if (err) {
+		dev_err(&pdev->dev, "failed to initialize hardware\n");
+		goto freevdev;
+	}
+
+	dev_info(&pdev->dev, "initialized\n");
+
+exit:
+	return err;
+
+freevdev:
+	video_device_release(pxp->vdev);
+
+freeirq:
+	free_irq(pxp->irq, pxp);
+
+release:
+	release_mem_region(res->start, res->end - res->start + 1);
+
+freepxp:
+	kfree(pxp);
+
+	return err;
+}
+
+static int __devexit pxp_remove(struct platform_device *pdev)
+{
+	struct pxps *pxp = platform_get_drvdata(pdev);
+
+	video_unregister_device(pxp->vdev);
+	video_device_release(pxp->vdev);
+
+	kfree(pxp->outb);
+	kfree(pxp);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int pxp_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int i;
+
+	while (HW_PXP_CTRL_RD() & BM_PXP_CTRL_ENABLE)
+		;
+
+	for (i = 0; i < REGS1_NUMS; i++)
+		regs1[i] = __raw_readl(HW_PXP_CTRL_ADDR + REG_OFFSET * i);
+
+	for (i = 0; i < REGS2_NUMS; i++)
+		regs2[i] = __raw_readl(HW_PXP_PAGETABLE_ADDR + REG_OFFSET * i);
+
+	for (i = 0; i < REGS3_NUMS; i++)
+		regs3[i] = __raw_readl(HW_PXP_OL0_ADDR + REG_OFFSET * i);
+
+	HW_PXP_CTRL_SET(BM_PXP_CTRL_SFTRST);
+
+	return 0;
+}
+
+static int pxp_resume(struct platform_device *pdev)
+{
+	int i;
+
+	/* Pull PxP out of reset */
+	HW_PXP_CTRL_WR(0);
+
+	for (i = 0; i < REGS1_NUMS; i++)
+		__raw_writel(regs1[i], HW_PXP_CTRL_ADDR + REG_OFFSET * i);
+
+	for (i = 0; i < REGS2_NUMS; i++)
+		__raw_writel(regs2[i], HW_PXP_PAGETABLE_ADDR + REG_OFFSET * i);
+
+	for (i = 0; i < REGS3_NUMS; i++)
+		__raw_writel(regs3[i], HW_PXP_OL0_ADDR + REG_OFFSET * i);
+
+	return 0;
+}
+#else
+#define	pxp_suspend	NULL
+#define	pxp_resume	NULL
+#endif
+
+static struct platform_driver pxp_driver = {
+	.driver 	= {
+		.name	= PXP_DRIVER_NAME,
+	},
+	.probe		= pxp_probe,
+	.remove		= __exit_p(pxp_remove),
+	.suspend	= pxp_suspend,
+	.resume		= pxp_resume,
+};
+
+
+static int __devinit pxp_init(void)
+{
+	return platform_driver_register(&pxp_driver);
+}
+
+static void __exit pxp_exit(void)
+{
+	platform_driver_unregister(&pxp_driver);
+}
+
+module_init(pxp_init);
+module_exit(pxp_exit);
+
+MODULE_DESCRIPTION("STMP37xx PxP driver");
+MODULE_AUTHOR("Matt Porter <mporter@embeddedalley.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/pxp.h b/drivers/media/video/pxp.h
new file mode 100644
index 000000000000..4ac191fa7d65
--- /dev/null
+++ b/drivers/media/video/pxp.h
@@ -0,0 +1,76 @@
+/*
+ * Freescale STMP378X PxP driver
+ *
+ * Author: Matt Porter <mporter@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2009 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+struct pxps {
+	struct platform_device *pdev;
+	struct resource *res;
+	int irq;
+	void __iomem *regs;
+
+	spinlock_t lock;
+	struct mutex mutex;
+	int users;
+
+	struct video_device *vdev;
+
+	struct videobuf_queue s0_vbq;
+	struct videobuf_buffer *active;
+	struct list_head outq;
+
+	int output;
+	u32 *outb;
+	dma_addr_t outb_phys;
+
+	/* Current S0 configuration */
+	struct pxp_data_format *s0_fmt;
+	u32 s0_width;
+	u32 s0_height;
+	u32 s0_bgcolor;
+	u32 s0_chromakey;
+
+	struct v4l2_framebuffer fb;
+	struct v4l2_rect drect;
+	struct v4l2_rect srect;
+
+	/* Transformation support */
+	int scaling;
+	int hflip;
+	int vflip;
+	int rotate;
+	int yuv;
+
+	/* Output overlay support */
+	int overlay_state;
+	int global_alpha_state;
+	u8  global_alpha;
+	int local_alpha_state;
+	int s1_chromakey_state;
+	u32 s1_chromakey;
+};
+
+struct pxp_data_format {
+	char *name;
+	unsigned int bpp;
+	u32 fourcc;
+	enum v4l2_colorspace colorspace;
+	u32 ctrl_s0_fmt;
+};
+
+extern int stmp3xxxfb_get_info(struct fb_var_screeninfo *var,
+				struct fb_fix_screeninfo *fix);
+extern void stmp3xxxfb_cfg_pxp(int enable, dma_addr_t pxp_phys);
diff --git a/drivers/media/video/videobuf-dma-contig.c b/drivers/media/video/videobuf-dma-contig.c
index 31944b11e6ea..eb02786795b4 100644
--- a/drivers/media/video/videobuf-dma-contig.c
+++ b/drivers/media/video/videobuf-dma-contig.c
@@ -255,7 +255,7 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 
 	mem->size = PAGE_ALIGN(q->bufs[first]->bsize);
 	mem->vaddr = dma_alloc_coherent(q->dev, mem->size,
-					&mem->dma_handle, GFP_KERNEL);
+					&mem->dma_handle, GFP_KERNEL | GFP_DMA);
 	if (!mem->vaddr) {
 		dev_err(q->dev, "dma_alloc_coherent size %ld failed\n",
 			mem->size);
@@ -269,7 +269,7 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	size = vma->vm_end - vma->vm_start;
 	size = (size < mem->size) ? size : mem->size;
 
-	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_page_prot = pgprot_writethru(vma->vm_page_prot);
 	retval = remap_pfn_range(vma, vma->vm_start,
 				 mem->dma_handle >> PAGE_SHIFT,
 				 size, vma->vm_page_prot);
diff --git a/drivers/mfd/wm8350-core.c b/drivers/mfd/wm8350-core.c
index 0d47fb9e4b3b..3a23bcb3c77e 100644
--- a/drivers/mfd/wm8350-core.c
+++ b/drivers/mfd/wm8350-core.c
@@ -80,10 +80,6 @@ static int wm8350_phys_read(struct wm8350 *wm8350, u8 reg, int num_regs,
 		/* Cache is CPU endian */
 		dest[i - reg] = be16_to_cpu(dest[i - reg]);
 
-		/* Satisfy non-volatile bits from cache */
-		dest[i - reg] &= wm8350_reg_io_map[i].vol;
-		dest[i - reg] |= wm8350->reg_cache[i];
-
 		/* Mask out non-readable bits */
 		dest[i - reg] &= wm8350_reg_io_map[i].readable;
 	}
@@ -183,9 +179,6 @@ static int wm8350_write(struct wm8350 *wm8350, u8 reg, int num_regs, u16 *src)
 			(wm8350->reg_cache[i] & ~wm8350_reg_io_map[i].writable)
 			| src[i - reg];
 
-		/* Don't store volatile bits */
-		wm8350->reg_cache[i] &= ~wm8350_reg_io_map[i].vol;
-
 		src[i - reg] = cpu_to_be16(src[i - reg]);
 	}
 
@@ -1123,7 +1116,6 @@ static int wm8350_create_cache(struct wm8350 *wm8350, int mode)
 			}
 			value = be16_to_cpu(value);
 			value &= wm8350_reg_io_map[i].readable;
-			value &= ~wm8350_reg_io_map[i].vol;
 			wm8350->reg_cache[i] = value;
 		} else
 			wm8350->reg_cache[i] = reg_map[i];
@@ -1186,9 +1178,14 @@ int wm8350_device_init(struct wm8350 *wm8350, int irq,
 			dev_info(wm8350->dev, "Found Rev G device\n");
 			wm8350->rev = WM8350_REV_G;
 			break;
+		case WM8350_REV_H:
+			dev_info(wm8350->dev, "Found Rev H device\n");
+			wm8350->rev = WM8350_REV_H;
+			break;
 		default:
 			/* For safety we refuse to run on unknown hardware */
-			dev_info(wm8350->dev, "Found unknown rev\n");
+			dev_info(wm8350->dev, "Found unknown rev %x\n",
+				 (id2 & WM8350_CHIP_REV_MASK) >> 12);
 			ret = -ENODEV;
 			goto err;
 		}
@@ -1233,8 +1230,8 @@ int wm8350_device_init(struct wm8350 *wm8350, int irq,
 		goto err;
 	}
 	wm8350->chip_irq = irq;
-
-	wm8350_reg_write(wm8350, WM8350_SYSTEM_INTERRUPTS_MASK, 0x0);
+	/*mask gpio and rtc interrupt*/
+	wm8350_reg_write(wm8350, WM8350_SYSTEM_INTERRUPTS_MASK, 0x50);
 
 	wm8350_client_dev_register(wm8350, "wm8350-codec",
 				   &(wm8350->codec.pdev));
diff --git a/drivers/mmc/card/Kconfig b/drivers/mmc/card/Kconfig
index 3f2a912659af..10ba9a035a58 100644
--- a/drivers/mmc/card/Kconfig
+++ b/drivers/mmc/card/Kconfig
@@ -50,3 +50,15 @@ config MMC_TEST
 
 	  This driver is only of interest to those developing or
 	  testing a host driver. Most people should say N here.
+
+config SDIO_UNIFI_FS
+	tristate "UniFi SDIO glue for Freescale MMC/SDIO"
+	depends on (MMC_MXC || MMC_IMX_ESDHCI)
+	depends on (MACH_MX31_3DS || MACH_MX35_3DS || MACH_MX37_3DS || MACH_MX51_3DS)
+	help
+	  This provides an interface between the CSR UniFi WiFi
+	  driver and the Freescale MMC/SDIO interface.
+	  If you have a MXC platform with a UniFi WiFi chip,
+	  say M here.
+
+	  If unsure, say N.
diff --git a/drivers/mmc/card/Makefile b/drivers/mmc/card/Makefile
index 0d407514f67d..bbcf742d0ea1 100644
--- a/drivers/mmc/card/Makefile
+++ b/drivers/mmc/card/Makefile
@@ -12,3 +12,4 @@ obj-$(CONFIG_MMC_TEST)		+= mmc_test.o
 
 obj-$(CONFIG_SDIO_UART)		+= sdio_uart.o
 
+obj-$(CONFIG_SDIO_UNIFI_FS)	+= unifi_fs/
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index 3d067c35185d..a2a81f38ec10 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -236,6 +236,19 @@ static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 		brq.stop.flags = MMC_RSP_SPI_R1B | MMC_RSP_R1B | MMC_CMD_AC;
 		brq.data.blocks = req->nr_sectors;
 
+		/*
+		 * @BUG: The block-layer is able to modify the minimum number of
+		 * transfer blocks for one request (see [1]). At this moment, it sets
+		 * this number to eigth, which is a problem for controllers that only
+		 * supports single-transfers.
+		 * 
+		 * [1] block/blk-settings.c:blk_queue_max_sectors()
+		 *
+		 * (Luis Galdos)
+		 */
+		if (brq.data.blocks > card->host->max_blk_count)
+			brq.data.blocks = card->host->max_blk_count;
+		
 		if (brq.data.blocks > 1) {
 			/* SPI multiblock writes terminate using a special
 			 * token, not a STOP_TRANSMISSION request.
@@ -266,6 +279,27 @@ static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 
 		mmc_queue_bounce_pre(mq);
 
+                /*
+                 * Adjust the sg list so it is the same size as the
+                 * request.
+		 * @BUG: See above description (Luis Galdos)
+                 */
+                if (brq.data.blocks != req->nr_sectors) {
+			int data_size, i;
+			struct scatterlist *sg;
+			
+                        data_size = brq.data.blocks * brq.data.blksz;
+                        for_each_sg(brq.data.sg, sg, brq.data.sg_len, i) {
+                                data_size -= sg->length;
+                                if (data_size <= 0) {
+                                        sg->length += data_size;
+                                        i++;
+                                        break;
+                                }
+                        }
+                        brq.data.sg_len = i;
+                }
+		
 		mmc_wait_for_req(card->host, &brq.mrq);
 
 		mmc_queue_bounce_post(mq);
diff --git a/drivers/mmc/card/unifi_fs/Makefile b/drivers/mmc/card/unifi_fs/Makefile
new file mode 100644
index 000000000000..381d4a2d1fd5
--- /dev/null
+++ b/drivers/mmc/card/unifi_fs/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_SDIO_UNIFI_FS) = unifi_fs.o
+unifi_fs-objs = fs_lx.o
diff --git a/drivers/mmc/card/unifi_fs/fs_lx.c b/drivers/mmc/card/unifi_fs/fs_lx.c
new file mode 100644
index 000000000000..fd5657299809
--- /dev/null
+++ b/drivers/mmc/card/unifi_fs/fs_lx.c
@@ -0,0 +1,684 @@
+/*
+ * fs_lx.c - Freescale SDIO glue module for UniFi.
+ *
+ * Copyright (C) 2008 Cambridge Silicon Radio Ltd.
+ *
+ *  Important:
+ *      This module does not support more than one device driver instances.
+ *
+ */
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+
+#include <linux/module.h>
+#include <linux/init.h>
+
+#include <linux/scatterlist.h>
+
+#include <linux/mmc/core.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sdio.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio_ids.h>
+
+#include <linux/clk.h>
+#include <linux/err.h>
+
+#include <mach/mmc.h>
+#include <mach/gpio.h>
+
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+
+#include "fs_sdio_api.h"
+
+struct regulator_unifi {
+	struct regulator *reg_gpo1;
+	struct regulator *reg_gpo2;
+	struct regulator *reg_1v5_ana_bb;
+	struct regulator *reg_vdd_vpa;
+	struct regulator *reg_1v5_dd;
+};
+
+static struct sdio_driver sdio_unifi_driver;
+
+static int fs_sdio_probe(struct sdio_func *func,
+			 const struct sdio_device_id *id);
+static void fs_sdio_remove(struct sdio_func *func);
+static void fs_sdio_irq(struct sdio_func *func);
+static int fs_sdio_suspend(struct device *dev, pm_message_t state);
+static int fs_sdio_resume(struct device *dev);
+static int do_sdio_hard_reset(struct sdio_dev *fdev);
+
+/* Globals to store the context to this module and the device driver */
+static struct sdio_dev *available_sdio_dev;
+static struct fs_driver *available_driver;
+struct mxc_unifi_platform_data *plat_data;
+
+extern void mxc_mmc_force_detect(int id);
+
+enum sdio_cmd_direction {
+	CMD_READ,
+	CMD_WRITE,
+};
+
+static int fsl_io_rw_direct(struct mmc_card *card, int write, unsigned fn,
+	unsigned addr, u8 in, u8 *out)
+{
+	struct mmc_command cmd;
+	int err;
+
+	BUG_ON(!card);
+	BUG_ON(fn > 7);
+
+	memset(&cmd, 0, sizeof(struct mmc_command));
+
+	cmd.opcode = SD_IO_RW_DIRECT;
+	cmd.arg = write ? 0x80000000 : 0x00000000;
+	cmd.arg |= fn << 28;
+	cmd.arg |= (write && out) ? 0x08000000 : 0x00000000;
+	cmd.arg |= addr << 9;
+	cmd.arg |= in;
+	cmd.flags = MMC_RSP_SPI_R5 | MMC_RSP_R5 | MMC_CMD_AC;
+
+	err = mmc_wait_for_cmd(card->host, &cmd, 0);
+	if (err)
+		return err;
+
+	if (mmc_host_is_spi(card->host)) {
+		/* host driver already reported errors */
+	} else {
+		if (cmd.resp[0] & R5_ERROR)
+			return -EIO;
+		if (cmd.resp[0] & R5_FUNCTION_NUMBER)
+			return -EINVAL;
+		if (cmd.resp[0] & R5_OUT_OF_RANGE)
+			return -ERANGE;
+	}
+
+	if (out) {
+		if (mmc_host_is_spi(card->host))
+			*out = (cmd.resp[0] >> 8) & 0xFF;
+		else
+			*out = cmd.resp[0] & 0xFF;
+	}
+
+	return 0;
+}
+
+
+int fs_sdio_readb(struct sdio_dev *fdev, int funcnum, unsigned long addr,
+		  unsigned char *pdata)
+{
+	int err;
+	char val;
+
+	sdio_claim_host(fdev->func);
+	if (funcnum == 0)
+		val = sdio_f0_readb(fdev->func, (unsigned int)addr, &err);
+	else
+		val = sdio_readb(fdev->func, (unsigned int)addr, &err);
+	sdio_release_host(fdev->func);
+	if (!err)
+		*pdata = val;
+	else
+		printk(KERN_ERR "fs_lx: readb error,fun=%d,addr=%d,data=%d,"
+		       "err=%d\n", funcnum, (int)addr, *pdata, err);
+
+	return err;
+}
+EXPORT_SYMBOL(fs_sdio_readb);
+
+int fs_sdio_writeb(struct sdio_dev *fdev, int funcnum, unsigned long addr,
+		   unsigned char data)
+{
+	int err;
+
+	sdio_claim_host(fdev->func);
+	if (funcnum == 0)
+		err = fsl_io_rw_direct(fdev->func->card, 1, 0, addr,
+				       data, NULL);
+	else
+		sdio_writeb(fdev->func, data, (unsigned int)addr, &err);
+	sdio_release_host(fdev->func);
+
+	if (err)
+		printk(KERN_ERR "fs_lx: writeb error,fun=%d,addr=%d,data=%d,"
+		       "err=%d\n", funcnum, (int)addr, data, err);
+	return err;
+}
+EXPORT_SYMBOL(fs_sdio_writeb);
+
+int fs_sdio_block_rw(struct sdio_dev *fdev, int funcnum, unsigned long addr,
+		     unsigned char *pdata, unsigned int count, int direction)
+{
+	int err;
+
+	sdio_claim_host(fdev->func);
+	if (direction == CMD_READ)
+		err = sdio_memcpy_fromio(fdev->func, pdata, addr, count);
+	else
+		err = sdio_memcpy_toio(fdev->func, addr, pdata, count);
+	sdio_release_host(fdev->func);
+
+	return err;
+}
+EXPORT_SYMBOL(fs_sdio_block_rw);
+
+int fs_sdio_enable_interrupt(struct sdio_dev *fdev, int enable)
+{
+	struct mmc_host *host = fdev->func->card->host;
+	unsigned long flags;
+
+	spin_lock_irqsave(&fdev->lock, flags);
+	if (enable) {
+		if (!fdev->int_enabled) {
+		fdev->int_enabled = 1;
+		host->ops->enable_sdio_irq(host, 1);
+		}
+	} else {
+		if (fdev->int_enabled) {
+		host->ops->enable_sdio_irq(host, 0);
+		fdev->int_enabled = 0;
+		}
+	}
+	spin_unlock_irqrestore(&fdev->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(fs_sdio_enable_interrupt);
+
+int fs_sdio_disable(struct sdio_dev *fdev)
+{
+	int err;
+	sdio_claim_host(fdev->func);
+	err = sdio_disable_func(fdev->func);
+	sdio_release_host(fdev->func);
+	if (err)
+		printk(KERN_ERR "fs_lx:fs_sdio_disable error,err=%d\n", err);
+	return err;
+}
+EXPORT_SYMBOL(fs_sdio_disable);
+
+int fs_sdio_enable(struct sdio_dev *fdev)
+{
+	int err = 0;
+
+	sdio_claim_host(fdev->func);
+	err = sdio_disable_func(fdev->func);
+	err = sdio_enable_func(fdev->func);
+	sdio_release_host(fdev->func);
+	if (err)
+		printk(KERN_ERR "fs_lx:fs_sdio_enable error,err=%d\n", err);
+	return err;
+}
+EXPORT_SYMBOL(fs_sdio_enable);
+
+int fs_sdio_set_max_clock_speed(struct sdio_dev *fdev, int max_khz)
+{
+	struct mmc_card *card = fdev->func->card;
+
+	/* Respect the host controller's min-max. */
+	max_khz *= 1000;
+	if (max_khz < card->host->f_min)
+		max_khz = card->host->f_min;
+	if (max_khz > card->host->f_max)
+		max_khz = card->host->f_max;
+
+	card->host->ios.clock = max_khz;
+	card->host->ops->set_ios(card->host, &card->host->ios);
+
+	return max_khz / 1000;
+}
+EXPORT_SYMBOL(fs_sdio_set_max_clock_speed);
+
+int fs_sdio_set_block_size(struct sdio_dev *fdev, int blksz)
+{
+	return 0;
+}
+EXPORT_SYMBOL(fs_sdio_set_block_size);
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ * Turn on the power of WIFI card
+ *
+ * ---------------------------------------------------------------------------
+ */
+static void fs_unifi_power_on(void)
+{
+	struct regulator_unifi *reg_unifi;
+	unsigned int tmp;
+
+	reg_unifi = plat_data->priv;
+
+	if (reg_unifi->reg_gpo1)
+		regulator_enable(reg_unifi->reg_gpo1);
+	if (reg_unifi->reg_gpo2)
+		regulator_enable(reg_unifi->reg_gpo2);
+
+	if (plat_data->enable)
+		plat_data->enable(1);
+
+	if (reg_unifi->reg_1v5_ana_bb) {
+		regulator_set_voltage(reg_unifi->reg_1v5_ana_bb,
+					1500000, 1500000);
+		regulator_enable(reg_unifi->reg_1v5_ana_bb);
+	}
+	if (reg_unifi->reg_vdd_vpa) {
+		tmp = regulator_get_voltage(reg_unifi->reg_vdd_vpa);
+		if (tmp < 3000000 || tmp > 3600000)
+			regulator_set_voltage(reg_unifi->reg_vdd_vpa,
+					      3000000, 3000000);
+		regulator_enable(reg_unifi->reg_vdd_vpa);
+	}
+	/* WL_1V5DD should come on last, 10ms after other supplies */
+	msleep(10);
+	if (reg_unifi->reg_1v5_dd) {
+		regulator_set_voltage(reg_unifi->reg_1v5_dd,
+					1500000, 1500000);
+		regulator_enable(reg_unifi->reg_1v5_dd);
+	}
+	msleep(10);
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ * Turn off the power of WIFI card
+ *
+ * ---------------------------------------------------------------------------
+ */
+static void fs_unifi_power_off(void)
+{
+	struct regulator_unifi *reg_unifi;
+
+	reg_unifi = plat_data->priv;
+	if (reg_unifi->reg_1v5_dd)
+		regulator_disable(reg_unifi->reg_1v5_dd);
+	if (reg_unifi->reg_vdd_vpa)
+		regulator_disable(reg_unifi->reg_vdd_vpa);
+
+	if (reg_unifi->reg_1v5_ana_bb)
+		regulator_disable(reg_unifi->reg_1v5_ana_bb);
+
+	if (plat_data->enable)
+		plat_data->enable(0);
+
+	if (reg_unifi->reg_gpo2)
+		regulator_disable(reg_unifi->reg_gpo2);
+
+	if (reg_unifi->reg_gpo1)
+		regulator_disable(reg_unifi->reg_gpo1);
+}
+
+/* This should be made conditional on being slot 2 too - so we can
+ * use a plug in card in slot 1
+ */
+int fs_sdio_hard_reset(struct sdio_dev *fdev)
+{
+	return 0;
+}
+EXPORT_SYMBOL(fs_sdio_hard_reset);
+
+static const struct sdio_device_id fs_sdio_ids[] = {
+	{SDIO_DEVICE(0x032a, 0x0001)},
+	{ /* end: all zeroes */	},
+};
+
+static struct sdio_driver sdio_unifi_driver = {
+	.name = "fs_unifi",
+	.probe = fs_sdio_probe,
+	.remove = fs_sdio_remove,
+	.id_table = fs_sdio_ids,
+	.drv = {
+		.suspend = fs_sdio_suspend,
+		.resume	= fs_sdio_resume,
+	}
+};
+
+int fs_sdio_register_driver(struct fs_driver *driver)
+{
+	int ret, retry;
+
+	/* Switch us on, sdio device may exist if power is on by default. */
+	plat_data->hardreset(0);
+	if (available_sdio_dev)
+		mxc_mmc_force_detect(plat_data->host_id);
+	/* Wait for card removed */
+	for (retry = 0; retry < 100; retry++) {
+		if (!available_sdio_dev)
+			break;
+		msleep(100);
+	}
+	if (retry == 100)
+		printk(KERN_ERR "fs_sdio_register_driver: sdio device exists, "
+				"timeout for card removed");
+	fs_unifi_power_on();
+	plat_data->hardreset(1);
+	msleep(500);
+	mxc_mmc_force_detect(plat_data->host_id);
+	for (retry = 0; retry < 100; retry++) {
+		if (available_sdio_dev)
+			break;
+		msleep(50);
+	}
+	if (retry == 100)
+		printk(KERN_ERR "fs_sdio_register_driver: Timeout waiting"
+				" for card added\n");
+	/* Store the context to the device driver to the global */
+	available_driver = driver;
+
+	/*
+	 * If available_sdio_dev is not NULL, probe has been called,
+	 * so pass the probe to the registered driver
+	 */
+	if (available_sdio_dev) {
+		/* Store the context to the new device driver */
+		available_sdio_dev->driver = driver;
+
+		printk(KERN_INFO "fs_sdio_register_driver: Glue exists, add "
+		       "device driver and register IRQ\n");
+		driver->probe(available_sdio_dev);
+
+		/* Register the IRQ handler to the SDIO IRQ. */
+		sdio_claim_host(available_sdio_dev->func);
+		ret = sdio_claim_irq(available_sdio_dev->func, fs_sdio_irq);
+		sdio_release_host(available_sdio_dev->func);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(fs_sdio_register_driver);
+
+void fs_sdio_unregister_driver(struct fs_driver *driver)
+{
+	int retry;
+	/*
+	 * If available_sdio_dev is not NULL, probe has been called,
+	 * so pass the remove to the registered driver to clean up.
+	 */
+	if (available_sdio_dev) {
+		struct mmc_host *host = available_sdio_dev->func->card->host;
+
+		printk(KERN_INFO "fs_sdio_unregister_driver: Glue exists, "
+		       "unregister IRQ and remove device driver\n");
+
+		/* Unregister the IRQ handler first. */
+		sdio_claim_host(available_sdio_dev->func);
+		sdio_release_irq(available_sdio_dev->func);
+		sdio_release_host(available_sdio_dev->func);
+
+		driver->remove(available_sdio_dev);
+
+		if (!available_sdio_dev->int_enabled) {
+			available_sdio_dev->int_enabled = 1;
+			host->ops->enable_sdio_irq(host, 1);
+		}
+
+		/* Invalidate the context to the device driver */
+		available_sdio_dev->driver = NULL;
+	}
+
+	/* invalidate the context to the device driver to the global */
+	available_driver = NULL;
+	/* Power down the UniFi */
+	fs_unifi_power_off();
+
+}
+EXPORT_SYMBOL(fs_sdio_unregister_driver);
+
+static void fs_sdio_irq(struct sdio_func *func)
+{
+	struct sdio_dev *fdev = (struct sdio_dev *)sdio_get_drvdata(func);
+	if (fdev->driver) {
+		if (fdev->driver->card_int_handler)
+			fdev->driver->card_int_handler(fdev);
+	}
+}
+
+#ifdef CONFIG_PM
+static int fs_sdio_suspend(struct device *dev, pm_message_t state)
+{
+	struct sdio_dev *fdev = available_sdio_dev;
+
+	/* Pass event to the registered driver. */
+	if (fdev->driver)
+		if (fdev->driver->suspend)
+			fdev->driver->suspend(fdev, state);
+
+	return 0;
+}
+
+static int fs_sdio_resume(struct device *dev)
+{
+	struct sdio_dev *fdev = available_sdio_dev;
+
+	/* Pass event to the registered driver. */
+	if (fdev->driver)
+		if (fdev->driver->resume)
+			fdev->driver->resume(fdev);
+
+	return 0;
+}
+#else
+#define fs_sdio_suspend NULL
+#define fs_sdio_resume  NULL
+#endif
+
+static int fs_sdio_probe(struct sdio_func *func,
+			 const struct sdio_device_id *id)
+{
+	struct sdio_dev *fdev;
+	int ret = 0;
+
+	/* Allocate our private context */
+	fdev = kmalloc(sizeof(struct sdio_dev), GFP_KERNEL);
+	if (!fdev)
+		return -ENOMEM;
+	available_sdio_dev = fdev;
+	memset(fdev, 0, sizeof(struct sdio_dev));
+	fdev->func = func;
+	fdev->vendor_id = id->vendor;
+	fdev->device_id = id->device;
+	fdev->max_blocksize = func->max_blksize;
+	fdev->int_enabled = 1;
+	spin_lock_init(&fdev->lock);
+
+	/* Store our context in the MMC driver */
+	printk(KERN_INFO "fs_sdio_probe: Add glue driver\n");
+	sdio_set_drvdata(func, fdev);
+
+	return 0;
+}
+
+static void fs_sdio_remove(struct sdio_func *func)
+{
+	struct sdio_dev *fdev = (struct sdio_dev *)sdio_get_drvdata(func);
+	struct mmc_host *host = func->card->host;
+
+	/* If there is a registered device driver, pass on the remove */
+	if (fdev->driver) {
+		printk(KERN_INFO "fs_sdio_remove: Free IRQ and remove device "
+		       "driver\n");
+		/* Unregister the IRQ handler first. */
+		sdio_claim_host(fdev->func);
+		sdio_release_irq(func);
+		sdio_release_host(fdev->func);
+
+		fdev->driver->remove(fdev);
+
+		if (!fdev->int_enabled) {
+			fdev->int_enabled = 1;
+			host->ops->enable_sdio_irq(host, 1);
+		}
+	}
+
+	/* Unregister the card context from the MMC driver. */
+	sdio_set_drvdata(func, NULL);
+
+	/* Invalidate the global to our context. */
+	available_sdio_dev = NULL;
+	kfree(fdev);
+}
+
+static int fs_unifi_init(void)
+{
+	struct regulator_unifi *reg_unifi;
+	struct regulator *reg;
+	int err = 0;
+
+	plat_data = get_unifi_plat_data();
+
+	if (!plat_data)
+		return -ENOENT;
+
+	reg_unifi = kzalloc(sizeof(struct regulator_unifi), GFP_KERNEL);
+	if (!reg_unifi)
+		return -ENOMEM;
+
+	if (plat_data->reg_gpo1) {
+		reg = regulator_get(NULL, plat_data->reg_gpo1);
+		if (!IS_ERR(reg))
+			reg_unifi->reg_gpo1 = reg;
+		else {
+			err = -EINVAL;
+			goto err_reg_gpo1;
+		}
+	}
+
+	if (plat_data->reg_gpo2) {
+		reg = regulator_get(NULL, plat_data->reg_gpo2);
+		if (!IS_ERR(reg))
+			reg_unifi->reg_gpo2 = reg;
+		else {
+			err = -EINVAL;
+			goto err_reg_gpo2;
+		}
+	}
+
+	if (plat_data->reg_1v5_ana_bb) {
+		reg = regulator_get(NULL, plat_data->reg_1v5_ana_bb);
+		if (!IS_ERR(reg))
+			reg_unifi->reg_1v5_ana_bb = reg;
+		else {
+			err = -EINVAL;
+			goto err_reg_1v5_ana_bb;
+		}
+	}
+
+	if (plat_data->reg_vdd_vpa) {
+		reg = regulator_get(NULL, plat_data->reg_vdd_vpa);
+		if (!IS_ERR(reg))
+			reg_unifi->reg_vdd_vpa = reg;
+		else {
+			err = -EINVAL;
+			goto err_reg_vdd_vpa;
+		}
+	}
+
+	if (plat_data->reg_1v5_dd) {
+		reg = regulator_get(NULL, plat_data->reg_1v5_dd);
+		if (!IS_ERR(reg))
+			reg_unifi->reg_1v5_dd = reg;
+		else {
+			err = -EINVAL;
+			goto err_reg_1v5_dd;
+		}
+	}
+	plat_data->priv = reg_unifi;
+	return 0;
+
+err_reg_1v5_dd:
+	if (reg_unifi->reg_vdd_vpa)
+		regulator_put(reg_unifi->reg_vdd_vpa);
+err_reg_vdd_vpa:
+	if (reg_unifi->reg_1v5_ana_bb)
+		regulator_put(reg_unifi->reg_1v5_ana_bb);
+err_reg_1v5_ana_bb:
+	if (reg_unifi->reg_gpo2)
+		regulator_put(reg_unifi->reg_gpo2);
+err_reg_gpo2:
+	if (reg_unifi->reg_gpo1)
+		regulator_put(reg_unifi->reg_gpo1);
+err_reg_gpo1:
+	kfree(reg_unifi);
+	return err;
+}
+
+int fs_unifi_remove(void)
+{
+	struct regulator_unifi *reg_unifi;
+
+	reg_unifi = plat_data->priv;
+	plat_data->priv = NULL;
+
+	if (reg_unifi->reg_1v5_dd)
+		regulator_put(reg_unifi->reg_1v5_dd);
+	if (reg_unifi->reg_vdd_vpa)
+		regulator_put(reg_unifi->reg_vdd_vpa);
+
+	if (reg_unifi->reg_1v5_ana_bb)
+		regulator_put(reg_unifi->reg_1v5_ana_bb);
+
+	if (reg_unifi->reg_gpo2)
+		regulator_put(reg_unifi->reg_gpo2);
+
+	if (reg_unifi->reg_gpo1)
+		regulator_put(reg_unifi->reg_gpo1);
+
+	kfree(reg_unifi);
+	return 0;
+}
+
+/* Module init and exit, register and unregister to the SDIO/MMC driver */
+static int __init fs_sdio_init(void)
+{
+	int err;
+
+	printk(KERN_INFO "Freescale: Register to MMC/SDIO driver\n");
+	/* Sleep a bit - otherwise if the mmc subsystem has just started, it
+	 * will allow us to register, then immediatly remove us!
+	 */
+	msleep(10);
+	err = fs_unifi_init();
+	if (err) {
+		printk(KERN_ERR "Error: fs_unifi_init failed!\n");
+		return err;
+	}
+	err = sdio_register_driver(&sdio_unifi_driver);
+	if (err) {
+		printk(KERN_ERR "Error: register sdio_unifi_driver failed!\n");
+		fs_unifi_remove();
+	}
+	return err;
+}
+
+module_init(fs_sdio_init);
+
+static void __exit fs_sdio_exit(void)
+{
+	printk(KERN_INFO "Freescale: Unregister from MMC/SDIO driver\n");
+	sdio_unregister_driver(&sdio_unifi_driver);
+	fs_unifi_remove();
+}
+
+module_exit(fs_sdio_exit);
+
+MODULE_DESCRIPTION("Freescale SDIO glue driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mmc/card/unifi_fs/fs_sdio_api.h b/drivers/mmc/card/unifi_fs/fs_sdio_api.h
new file mode 100644
index 000000000000..ea6ebd765e28
--- /dev/null
+++ b/drivers/mmc/card/unifi_fs/fs_sdio_api.h
@@ -0,0 +1,68 @@
+/*
+ *fs_sdio_api.h - Freescale SDIO glue module API for UniFi.
+ *
+ * Copyright (C) 2008 Cambridge Silicon Radio Ltd.
+ *
+ */
+/*
+ * Copyright 2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef _FS_SDIO_API_H
+#define _FS_SDIO_API_H
+
+struct sdio_dev;
+
+struct fs_driver {
+	const char *name;
+	int (*probe)(struct sdio_dev *fdev);
+	void (*remove)(struct sdio_dev *fdev);
+	void (*card_int_handler)(struct sdio_dev *fdev);
+	void (*suspend)(struct sdio_dev *fdev, pm_message_t state);
+	void (*resume)(struct sdio_dev *fdev);
+};
+
+int fs_sdio_readb(struct sdio_dev *fdev, int funcnum,
+		  unsigned long addr, unsigned char *pdata);
+int fs_sdio_writeb(struct sdio_dev *fdev, int funcnum,
+		   unsigned long addr, unsigned char data);
+int fs_sdio_block_rw(struct sdio_dev *fdev, int funcnum,
+		     unsigned long addr, unsigned char *pdata,
+		     unsigned int count, int direction);
+
+int fs_sdio_register_driver(struct fs_driver *driver);
+void fs_sdio_unregister_driver(struct fs_driver *driver);
+int fs_sdio_set_block_size(struct sdio_dev *fdev, int blksz);
+int fs_sdio_set_max_clock_speed(struct sdio_dev *fdev, int max_khz);
+int fs_sdio_enable_interrupt(struct sdio_dev *fdev, int enable);
+int fs_sdio_enable(struct sdio_dev *fdev);
+int fs_sdio_hard_reset(struct sdio_dev *fdev);
+
+struct sdio_dev {
+	/**< Device driver for this module. */
+	struct fs_driver *driver;
+
+	struct sdio_func *func;
+
+	/**< Data private to the device driver. */
+	void *drv_data;
+
+	int int_enabled;
+	spinlock_t lock;
+
+	uint16_t vendor_id;     /**< Vendor ID of the card. */
+	uint16_t device_id;     /**< Device ID of the card. */
+
+	/**< Maximum block size supported. */
+	int max_blocksize;
+};
+
+
+#endif	/* #ifndef _FS_SDIO_API_H */
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index fdd7c760be8c..7112a25356d5 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -208,7 +208,7 @@ static int mmc_read_ext_csd(struct mmc_card *card)
 	}
 
 	ext_csd_struct = ext_csd[EXT_CSD_REV];
-	if (ext_csd_struct > 2) {
+	if (ext_csd_struct > 3) {
 		printk(KERN_ERR "%s: unrecognised EXT_CSD structure "
 			"version %d\n", mmc_hostname(card->host),
 			ext_csd_struct);
@@ -433,15 +433,26 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	/*
 	 * Activate wide bus (if supported).
 	 */
-	if ((card->csd.mmca_vsn >= CSD_SPEC_VER_4) &&
-		(host->caps & MMC_CAP_4_BIT_DATA)) {
-		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-			EXT_CSD_BUS_WIDTH, EXT_CSD_BUS_WIDTH_4);
-		if (err)
-			goto free_card;
+ 	if ((card->csd.mmca_vsn >= CSD_SPEC_VER_4) &&
+	    (host->caps & (MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA))) {
+		unsigned ext_csd_bit, bus_width;
 
-		mmc_set_bus_width(card->host, MMC_BUS_WIDTH_4);
-	}
+		if (host->caps & MMC_CAP_8_BIT_DATA) {
+			ext_csd_bit = EXT_CSD_BUS_WIDTH_8;
+			bus_width = MMC_BUS_WIDTH_8;
+		} else {
+			ext_csd_bit = EXT_CSD_BUS_WIDTH_4;
+			bus_width = MMC_BUS_WIDTH_4;
+		}
+
+ 		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+				 EXT_CSD_BUS_WIDTH, ext_csd_bit);
+
+ 		if (err)
+ 			goto free_card;
+ 
+		mmc_set_bus_width(card->host, bus_width);
+ 	}
 
 	if (!oldcard)
 		host->card = card;
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index dfa585f7feaf..852a319ddad6 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -165,6 +165,51 @@ config MMC_SPI
 
 	  If unsure, or if your system has no SPI master driver, say N.
 
+config MMC_MXC
+	tristate "Freescale MXC Multimedia Card Interface support"
+	depends on ARCH_MXC && MMC
+	help
+	  This selects the Freescale MXC Multimedia card Interface.
+	  If you have a MXC platform with a Multimedia Card slot,
+	  say Y or M here.
+
+config MMC_IMX_ESDHCI
+	tristate "Freescale i.MX Secure Digital Host Controller Interface support"
+	depends on ARCH_MXC && MMC
+	help
+	  This selects the Freescale i.MX Multimedia card Interface.
+	  If you have a i.MX platform with a Multimedia Card slot,
+	  say Y or M here.
+
+	  If unsure, say N.
+
+config MMC_IMX_ESDHCI_SELECT2
+	bool "Enable second ESDHCI port"
+	depends on MMC_IMX_ESDHCI && ARCH_MX25
+	default n
+	help
+	  Enable the second ESDHC port
+
+config MMC_IMX_ESDHCI_PIO_MODE
+	bool "Freescale i.MX Secure Digital Host Controller Interface PIO mode"
+	depends on MMC_IMX_ESDHC != n
+	default n
+	help
+	  This set the Freescale i.MX Multimedia card Interface to PIO mode.
+	  If you have a i.MX platform with a Multimedia Card slot,
+	  and want test it with PIO mode.
+	  say Y here.
+
+	  If unsure, say N.
+
+config MMC_STMP3XXX
+	tristate "STMP37xx/378x MMC support"
+	depends on MMC && ARCH_STMP3XXX
+	help
+	  Select Y if you would like to access STMP37xx/378x MMC support.
+
+	  If unsure, say N.
+
 config MMC_S3C
 	tristate "Samsung S3C SD/MMC Card Interface support"
 	depends on ARCH_S3C2410
@@ -176,12 +221,24 @@ config MMC_S3C
 
 	  If unsure, say N.
 
+config HSMMC_S3C
+        tristate "Samsung S3C High Speed SD/MMC Card Interface support"
+        depends on ARCH_S3C2410 && MMC
+        help
+          This selects a driver for the High Speed MMC interface found in
+          Samsung's S3C2410, S3C2412, S3C2440, S3C2442 CPUs.
+          If you have a board based on one of those and a MMC/SD
+          slot, say Y or M here.
+
+          If unsure, say N.
+ 
 config MMC_SDRICOH_CS
 	tristate "MMC/SD driver for Ricoh Bay1Controllers (EXPERIMENTAL)"
 	depends on EXPERIMENTAL && PCI && PCMCIA
 	help
 	  Say Y here if your Notebook reports a Ricoh Bay1Controller PCMCIA
 	  card whenever you insert a MMC or SD card into the card slot.
+	  say Y or M here.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called sdricoh_cs.
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index c794cc5ce442..8467f9ac6d40 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -12,6 +12,9 @@ obj-$(CONFIG_MMC_IMX)		+= imxmmc.o
 obj-$(CONFIG_MMC_SDHCI)		+= sdhci.o
 obj-$(CONFIG_MMC_SDHCI_PCI)	+= sdhci-pci.o
 obj-$(CONFIG_MMC_RICOH_MMC)	+= ricoh_mmc.o
+obj-$(CONFIG_MMC_IMX_ESDHCI)	+= mx_sdhci.o
+obj-$(CONFIG_MMC_MXC)		+= mxc_mmc.o
+obj-$(CONFIG_MMC_STMP3XXX)	+= stmp3xxx_mmc.o
 obj-$(CONFIG_MMC_WBSD)		+= wbsd.o
 obj-$(CONFIG_MMC_AU1X)		+= au1xmmc.o
 obj-$(CONFIG_MMC_OMAP)		+= omap.o
@@ -20,6 +23,7 @@ obj-$(CONFIG_MMC_ATMELMCI)	+= atmel-mci.o
 obj-$(CONFIG_MMC_TIFM_SD)	+= tifm_sd.o
 obj-$(CONFIG_MMC_SPI)		+= mmc_spi.o
 obj-$(CONFIG_MMC_S3C)   	+= s3cmci.o
+obj-$(CONFIG_HSMMC_S3C)         += s3c-hsmmc.o
 obj-$(CONFIG_MMC_SDRICOH_CS)	+= sdricoh_cs.o
 obj-$(CONFIG_MMC_TMIO)		+= tmio_mmc.o
 
diff --git a/drivers/mmc/host/mx_sdhci.c b/drivers/mmc/host/mx_sdhci.c
new file mode 100644
index 000000000000..c6352e9edd3b
--- /dev/null
+++ b/drivers/mmc/host/mx_sdhci.c
@@ -0,0 +1,2153 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx_sdhci.c
+ *
+ * @brief Driver for the Freescale Semiconductor MXC eSDHC modules.
+ *
+ * This driver code is based on sdhci.c, by Pierre Ossman <drzeus@drzeus.cx>");
+ * This driver supports Enhanced Secure Digital Host Controller
+ * modules eSDHC of MXC. eSDHC is also referred as enhanced MMC/SD
+ * controller.
+ *
+ * @ingroup MMC_SD
+ */
+
+#include <linux/delay.h>
+#include <linux/highmem.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/scatterlist.h>
+
+#include <linux/leds.h>
+
+#include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
+#include <linux/mmc/card.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+
+#include <asm/dma.h>
+#include <asm/io.h>
+#include <mach/hardware.h>
+#include <asm/irq.h>
+#include <asm/mach/irq.h>
+#include <asm/mach-types.h>
+#include <mach/mmc.h>
+
+#include "mx_sdhci.h"
+
+#define DRIVER_NAME "mxsdhci"
+
+#define DBG(f, x...) \
+	pr_debug(DRIVER_NAME " [%s()]: " f, __func__, ## x)
+
+static unsigned int debug_quirks;
+static int last_op_dir;
+
+/*
+ * Different quirks to handle when the hardware deviates from a strict
+ * interpretation of the SDHCI specification.
+ */
+
+/* Controller doesn't honor resets unless we touch the clock register */
+#define SDHCI_QUIRK_CLOCK_BEFORE_RESET			(1<<0)
+/* Controller has bad caps bits, but really supports DMA */
+#define SDHCI_QUIRK_FORCE_DMA				(1<<1)
+/* Controller doesn't like to be reset when there is no card inserted. */
+#define SDHCI_QUIRK_NO_CARD_NO_RESET			(1<<2)
+/* Controller doesn't like clearing the power reg before a change */
+#define SDHCI_QUIRK_SINGLE_POWER_WRITE			(1<<3)
+/* Controller has flaky internal state so reset it on each ios change */
+#define SDHCI_QUIRK_RESET_CMD_DATA_ON_IOS		(1<<4)
+/* Controller has an unusable DMA engine */
+#define SDHCI_QUIRK_BROKEN_DMA				(1<<5)
+/* Controller can only DMA from 32-bit aligned addresses */
+#define SDHCI_QUIRK_32BIT_DMA_ADDR			(1<<6)
+/* Controller can only DMA chunk sizes that are a multiple of 32 bits */
+#define SDHCI_QUIRK_32BIT_DMA_SIZE			(1<<7)
+/* Controller needs to be reset after each request to stay stable */
+#define SDHCI_QUIRK_RESET_AFTER_REQUEST			(1<<8)
+/* Controller needs voltage and power writes to happen separately */
+#define SDHCI_QUIRK_NO_SIMULT_VDD_AND_POWER		(1<<9)
+/* Controller has an off-by-one issue with timeout value */
+#define SDHCI_QUIRK_INCR_TIMEOUT_CONTROL		(1<<10)
+/* Controller only support the PIO */
+#define SDHCI_QUIRK_ONLY_PIO 				(1<<16)
+/* Controller support the External DMA */
+#define SDHCI_QUIRK_EXTERNAL_DMA_MODE			(1<<17)
+/* Controller support the Internal Simple DMA */
+#define SDHCI_QUIRK_INTERNAL_SIMPLE_DMA			(1<<18)
+/* Controller support the Internal Advanced DMA */
+#define SDHCI_QUIRK_INTERNAL_ADVANCED_DMA 		(1<<19)
+
+/*
+ * defines the mxc flags refer to the special hw pre-conditons and behavior
+ */
+static unsigned int mxc_quirks;
+#ifdef CONFIG_MMC_IMX_ESDHCI_PIO_MODE
+static unsigned int debug_quirks = SDHCI_QUIRK_ONLY_PIO;
+#else
+static unsigned int debug_quirks;
+#endif
+static unsigned int mxc_wml_value = 512;
+static unsigned int *adma_des_table;
+
+#ifndef MXC_SDHCI_NUM
+#define MXC_SDHCI_NUM	4
+#endif
+
+static struct sdhci_chip *mxc_fix_chips[MXC_SDHCI_NUM];
+
+static void sdhci_prepare_data(struct sdhci_host *, struct mmc_data *);
+static void sdhci_finish_data(struct sdhci_host *);
+
+static void sdhci_send_command(struct sdhci_host *, struct mmc_command *);
+static void sdhci_finish_command(struct sdhci_host *);
+
+/* Used to active the SD bus */
+extern void gpio_sdhc_active(int module);
+extern void gpio_sdhc_inactive(int module);
+static void sdhci_dma_irq(void *devid, int error, unsigned int cnt);
+
+void mxc_mmc_force_detect(int id)
+{
+	struct sdhci_host *host;
+	if ((id < 0) || (id >= MXC_SDHCI_NUM))
+		return;
+	if (!mxc_fix_chips[id])
+		return;
+	host = mxc_fix_chips[id]->hosts[0];
+	if (host->detect_irq)
+		return;
+
+	schedule_work(&host->cd_wq);
+	return;
+}
+
+EXPORT_SYMBOL(mxc_mmc_force_detect);
+
+static void sdhci_dumpregs(struct sdhci_host *host)
+{
+	printk(KERN_DEBUG DRIVER_NAME
+	       ": ============== REGISTER DUMP ==============\n");
+
+	printk(KERN_DEBUG DRIVER_NAME ": Sys addr: 0x%08x | Version:  0x%08x\n",
+	       readl(host->ioaddr + SDHCI_DMA_ADDRESS),
+	       readl(host->ioaddr + SDHCI_HOST_VERSION));
+	printk(KERN_DEBUG DRIVER_NAME ": Blk size: 0x%08x | Blk cnt:  0x%08x\n",
+	       (readl(host->ioaddr + SDHCI_BLOCK_SIZE) & 0xFFFF),
+	       (readl(host->ioaddr + SDHCI_BLOCK_COUNT) >> 16));
+	printk(KERN_DEBUG DRIVER_NAME ": Argument: 0x%08x | Trn mode: 0x%08x\n",
+	       readl(host->ioaddr + SDHCI_ARGUMENT),
+	       readl(host->ioaddr + SDHCI_TRANSFER_MODE));
+	printk(KERN_DEBUG DRIVER_NAME ": Present:  0x%08x | Host ctl: 0x%08x\n",
+	       readl(host->ioaddr + SDHCI_PRESENT_STATE),
+	       readl(host->ioaddr + SDHCI_HOST_CONTROL));
+	printk(KERN_DEBUG DRIVER_NAME ": Clock:    0x%08x\n",
+	       readl(host->ioaddr + SDHCI_CLOCK_CONTROL));
+	printk(KERN_DEBUG DRIVER_NAME ": Int stat: 0x%08x\n",
+	       readl(host->ioaddr + SDHCI_INT_STATUS));
+	printk(KERN_DEBUG DRIVER_NAME ": Int enab: 0x%08x | Sig enab: 0x%08x\n",
+	       readl(host->ioaddr + SDHCI_INT_ENABLE),
+	       readl(host->ioaddr + SDHCI_SIGNAL_ENABLE));
+	printk(KERN_DEBUG DRIVER_NAME ": Caps:     0x%08x\n",
+	       readl(host->ioaddr + SDHCI_CAPABILITIES));
+
+	printk(KERN_DEBUG DRIVER_NAME
+	       ": ===========================================\n");
+}
+
+/*****************************************************************************\
+ *                                                                           *
+ * Low level functions                                                       *
+ *                                                                           *
+\*****************************************************************************/
+
+static void sdhci_reset(struct sdhci_host *host, u8 mask)
+{
+	unsigned long tmp;
+	unsigned long mask_u32;
+	unsigned long reg_save = 0;
+
+	if (host->chip->quirks & SDHCI_QUIRK_NO_CARD_NO_RESET) {
+		if (!(readl(host->ioaddr + SDHCI_PRESENT_STATE) &
+		      SDHCI_CARD_PRESENT))
+			return;
+	}
+
+	if (mask & SDHCI_RESET_ALL)
+		host->clock = 0;
+	else if (host->flags & SDHCI_CD_PRESENT)
+		reg_save = readl(host->ioaddr + SDHCI_HOST_CONTROL);
+
+	tmp = readl(host->ioaddr + SDHCI_CLOCK_CONTROL) | (mask << 24);
+	mask_u32 = readl(host->ioaddr + SDHCI_SIGNAL_ENABLE);
+	writel(tmp, host->ioaddr + SDHCI_CLOCK_CONTROL);
+
+	/* Wait max 100 ms */
+	tmp = 5000;
+
+	/* hw clears the bit when it's done */
+	while ((readl(host->ioaddr + SDHCI_CLOCK_CONTROL) >> 24) & mask) {
+		if (tmp == 0) {
+			printk(KERN_ERR "%s: Reset 0x%x never completed.\n",
+			       mmc_hostname(host->mmc), (int)mask);
+			sdhci_dumpregs(host);
+			return;
+		}
+		tmp--;
+		udelay(20);
+	}
+	/*
+	 * The INT_EN SIG_EN regs have been modified after reset.
+	 * re-configure them ag.
+	 */
+	if (!(mask & SDHCI_RESET_ALL) && (host->flags & SDHCI_CD_PRESENT))
+		writel(reg_save, host->ioaddr + SDHCI_HOST_CONTROL);
+	if (host->flags & SDHCI_USE_DMA)
+		mask_u32 &= ~(SDHCI_INT_DATA_AVAIL | SDHCI_INT_SPACE_AVAIL);
+	if (mxc_wml_value == 512)
+		writel(SDHCI_WML_128_WORDS, host->ioaddr + SDHCI_WML);
+	else
+		writel(SDHCI_WML_16_WORDS, host->ioaddr + SDHCI_WML);
+	writel(mask_u32 | SDHCI_INT_CARD_INT, host->ioaddr + SDHCI_INT_ENABLE);
+	writel(mask_u32, host->ioaddr + SDHCI_SIGNAL_ENABLE);
+	last_op_dir = 0;
+}
+
+static void sdhci_init(struct sdhci_host *host)
+{
+	u32 intmask;
+
+	sdhci_reset(host, SDHCI_RESET_ALL);
+
+	intmask = SDHCI_INT_ADMA_ERROR |
+	    SDHCI_INT_DATA_END_BIT | SDHCI_INT_DATA_CRC |
+	    SDHCI_INT_DATA_TIMEOUT | SDHCI_INT_INDEX |
+	    SDHCI_INT_END_BIT | SDHCI_INT_CRC | SDHCI_INT_TIMEOUT |
+	    SDHCI_INT_DATA_AVAIL | SDHCI_INT_SPACE_AVAIL |
+	    SDHCI_INT_DMA_END | SDHCI_INT_DATA_END | SDHCI_INT_RESPONSE;
+
+	if (host->flags & SDHCI_USE_DMA)
+		intmask &= ~(SDHCI_INT_DATA_AVAIL | SDHCI_INT_SPACE_AVAIL);
+	/* Configure the WML rege */
+	if (mxc_wml_value == 512)
+		writel(SDHCI_WML_128_WORDS, host->ioaddr + SDHCI_WML);
+	else
+		writel(SDHCI_WML_16_WORDS, host->ioaddr + SDHCI_WML);
+	writel(intmask | SDHCI_INT_CARD_INT, host->ioaddr + SDHCI_INT_ENABLE);
+	writel(intmask, host->ioaddr + SDHCI_SIGNAL_ENABLE);
+}
+
+static void sdhci_activate_led(struct sdhci_host *host)
+{
+	u32 ctrl;
+
+	ctrl = readl(host->ioaddr + SDHCI_HOST_CONTROL);
+	ctrl |= SDHCI_CTRL_LED;
+	writel(ctrl, host->ioaddr + SDHCI_HOST_CONTROL);
+}
+
+static void sdhci_deactivate_led(struct sdhci_host *host)
+{
+	u32 ctrl;
+
+	ctrl = readl(host->ioaddr + SDHCI_HOST_CONTROL);
+	ctrl &= ~SDHCI_CTRL_LED;
+	writel(ctrl, host->ioaddr + SDHCI_HOST_CONTROL);
+}
+
+/*****************************************************************************\
+ *                                                                           *
+ * Core functions                                                            *
+ *                                                                           *
+\*****************************************************************************/
+
+static inline char *sdhci_sg_to_buffer(struct sdhci_host *host)
+{
+	return sg_virt(host->cur_sg);
+}
+
+static inline int sdhci_next_sg(struct sdhci_host *host)
+{
+	/*
+	 * Skip to next SG entry.
+	 */
+	host->cur_sg++;
+	host->num_sg--;
+
+	/*
+	 * Any entries left?
+	 */
+	if (host->num_sg > 0) {
+		host->offset = 0;
+		host->remain = host->cur_sg->length;
+	}
+
+	return host->num_sg;
+}
+
+static void sdhci_read_block_pio(struct sdhci_host *host)
+{
+	int blksize, chunk_remain;
+	u32 data;
+	char *buffer;
+	int size;
+
+	DBG("PIO reading\n");
+
+	blksize = host->data->blksz;
+	chunk_remain = 0;
+	data = 0;
+
+	buffer = sdhci_sg_to_buffer(host) + host->offset;
+
+	while (blksize) {
+		if (chunk_remain == 0) {
+			data = readl(host->ioaddr + SDHCI_BUFFER);
+			chunk_remain = min(blksize, 4);
+		}
+
+		size = min(host->remain, chunk_remain);
+
+		chunk_remain -= size;
+		blksize -= size;
+		host->offset += size;
+		host->remain -= size;
+
+		while (size) {
+			*buffer = data & 0xFF;
+			buffer++;
+			data >>= 8;
+			size--;
+		}
+
+		if (host->remain == 0) {
+			if (sdhci_next_sg(host) == 0) {
+				BUG_ON(blksize != 0);
+				return;
+			}
+			buffer = sdhci_sg_to_buffer(host);
+		}
+	}
+}
+
+static void sdhci_write_block_pio(struct sdhci_host *host)
+{
+	int blksize, chunk_remain;
+	u32 data;
+	char *buffer;
+	int bytes, size;
+
+	DBG("PIO writing\n");
+
+	blksize = host->data->blksz;
+	chunk_remain = 4;
+	data = 0;
+
+	bytes = 0;
+	buffer = sdhci_sg_to_buffer(host) + host->offset;
+
+	while (blksize) {
+		size = min(host->remain, chunk_remain);
+
+		chunk_remain -= size;
+		blksize -= size;
+		host->offset += size;
+		host->remain -= size;
+
+		while (size) {
+			data >>= 8;
+			data |= (u32) *buffer << 24;
+			buffer++;
+			size--;
+		}
+
+		if (chunk_remain == 0) {
+			writel(data, host->ioaddr + SDHCI_BUFFER);
+			chunk_remain = min(blksize, 4);
+		}
+
+		if (host->remain == 0) {
+			if (sdhci_next_sg(host) == 0) {
+				BUG_ON(blksize != 0);
+				return;
+			}
+			buffer = sdhci_sg_to_buffer(host);
+		}
+	}
+}
+
+static void sdhci_transfer_pio(struct sdhci_host *host)
+{
+	u32 mask;
+
+	BUG_ON(!host->data);
+
+	if (host->num_sg == 0)
+		return;
+
+	if (host->data->flags & MMC_DATA_READ)
+		mask = SDHCI_DATA_AVAILABLE;
+	else
+		mask = SDHCI_SPACE_AVAILABLE;
+
+	while (readl(host->ioaddr + SDHCI_PRESENT_STATE) & mask) {
+		if (host->data->flags & MMC_DATA_READ)
+			sdhci_read_block_pio(host);
+		else
+			sdhci_write_block_pio(host);
+
+		if (host->num_sg == 0)
+			break;
+	}
+
+	DBG("PIO transfer complete.\n");
+}
+
+static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_data *data)
+{
+	u32 count;
+	unsigned target_timeout, current_timeout;
+
+	WARN_ON(host->data);
+
+	if (data == NULL)
+		return;
+
+	/* Sanity checks */
+	BUG_ON(data->blksz * data->blocks > 524288);
+	BUG_ON(data->blksz > host->mmc->max_blk_size);
+	BUG_ON(data->blocks > 65535);
+
+	host->data = data;
+	host->data_early = 0;
+	if (host->data->flags & MMC_DATA_READ)
+		writel(readl(host->ioaddr + SDHCI_CLOCK_CONTROL) |
+		       SDHCI_CLOCK_HLK_EN, host->ioaddr + SDHCI_CLOCK_CONTROL);
+
+	/* timeout in us */
+	target_timeout = data->timeout_ns / 1000 +
+	    data->timeout_clks / host->clock;
+
+	/*
+	 * Figure out needed cycles.
+	 * We do this in steps in order to fit inside a 32 bit int.
+	 * The first step is the minimum timeout, which will have a
+	 * minimum resolution of 6 bits:
+	 * (1) 2^13*1000 > 2^22,
+	 * (2) host->timeout_clk < 2^16
+	 *     =>
+	 *     (1) / (2) > 2^6
+	 */
+	count = 0;
+	current_timeout = (1 << 13) * 1000 / host->timeout_clk;
+	while (current_timeout < target_timeout) {
+		count++;
+		current_timeout <<= 1;
+		if (count >= 0xF)
+			break;
+	}
+
+	/*
+	 * Compensate for an off-by-one error in the CaFe hardware; otherwise,
+	 * a too-small count gives us interrupt timeouts.
+	 */
+	if ((host->chip->quirks & SDHCI_QUIRK_INCR_TIMEOUT_CONTROL))
+		count++;
+
+	if (count >= 0xF) {
+		DBG(KERN_WARNING "%s: Too large timeout requested!\n",
+		    mmc_hostname(host->mmc));
+		count = 0xE;
+	}
+
+	/* Set the max time-out value to level up the compatibility */
+	count = 0xE;
+
+	count =
+	    (count << 16) | (readl(host->ioaddr + SDHCI_CLOCK_CONTROL) &
+			     0xFFF0FFFF);
+	writel(count, host->ioaddr + SDHCI_CLOCK_CONTROL);
+
+	if (host->flags & SDHCI_USE_DMA)
+		host->flags |= SDHCI_REQ_USE_DMA;
+
+	if (unlikely((host->flags & SDHCI_REQ_USE_DMA) &&
+		     (host->chip->quirks & SDHCI_QUIRK_32BIT_DMA_SIZE) &&
+		     ((data->blksz * data->blocks) & 0x3))) {
+		DBG("Reverting to PIO because of transfer size (%d)\n",
+		    data->blksz * data->blocks);
+		host->flags &= ~SDHCI_REQ_USE_DMA;
+	}
+
+	/*
+	 * The assumption here being that alignment is the same after
+	 * translation to device address space.
+	 */
+	if (unlikely((host->flags & SDHCI_REQ_USE_DMA) &&
+		     (host->chip->quirks & SDHCI_QUIRK_32BIT_DMA_ADDR) &&
+		     (data->sg->offset & 0x3))) {
+		DBG("Reverting to PIO because of bad alignment\n");
+		host->flags &= ~SDHCI_REQ_USE_DMA;
+	}
+
+	if (host->flags & SDHCI_REQ_USE_DMA) {
+		int i;
+		struct scatterlist *tsg;
+
+		host->dma_size = data->blocks * data->blksz;
+		count =
+		    dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
+			       (data->
+				flags & MMC_DATA_READ) ? DMA_FROM_DEVICE :
+			       DMA_TO_DEVICE);
+		BUG_ON(count != data->sg_len);
+		DBG("Configure the sg DMA, %s, len is 0x%x, count is %d\n",
+		    (data->flags & MMC_DATA_READ)
+		    ? "DMA_FROM_DEIVCE" : "DMA_TO_DEVICE", host->dma_size,
+		    count);
+
+		/* Make sure the ADMA mode is selected. */
+		i = readl(host->ioaddr + SDHCI_HOST_CONTROL);
+		i |= SDHCI_CTRL_ADMA;
+		writel(i, host->ioaddr + SDHCI_HOST_CONTROL);
+
+		tsg = data->sg;
+		/* ADMA mode is used, create the descriptor table */
+		for (i = 0; i < count; i++) {
+			if (tsg->dma_address & 0xFFF) {
+				DBG(KERN_ERR "ADMA addr isn't 4K aligned.\n");
+				DBG(KERN_ERR "0x%x\n", tsg->dma_address);
+				DBG(KERN_ERR "Changed to Single DMA mode.\n");
+				goto Single_DMA;
+			}
+			adma_des_table[2 * i] = tsg->length << 12;
+			adma_des_table[2 * i] |= FSL_ADMA_DES_ATTR_SET;
+			adma_des_table[2 * i] |= FSL_ADMA_DES_ATTR_VALID;
+			adma_des_table[2 * i + 1] = tsg->dma_address;
+			adma_des_table[2 * i + 1] |= FSL_ADMA_DES_ATTR_TRAN;
+			adma_des_table[2 * i + 1] |= FSL_ADMA_DES_ATTR_VALID;
+			if (count == (i + 1))
+				adma_des_table[2 * i + 1] |=
+				    FSL_ADMA_DES_ATTR_END;
+			tsg++;
+		}
+
+		/* Write the physical address to ADMA address reg */
+		writel(virt_to_phys(adma_des_table),
+		       host->ioaddr + SDHCI_ADMA_ADDRESS);
+	      Single_DMA:
+		/* Rollback to the Single DMA mode */
+		i = readl(host->ioaddr + SDHCI_HOST_CONTROL);
+		i &= ~SDHCI_CTRL_ADMA;
+		writel(i, host->ioaddr + SDHCI_HOST_CONTROL);
+		/* Single DMA mode is used */
+		writel(sg_dma_address(data->sg),
+		       host->ioaddr + SDHCI_DMA_ADDRESS);
+	} else if ((host->flags & SDHCI_USE_EXTERNAL_DMA) &&
+		   (data->blocks * data->blksz >= mxc_wml_value)) {
+		host->dma_size = data->blocks * data->blksz;
+		DBG("Configure the External DMA, %s, len is 0x%x\n",
+		    (data->flags & MMC_DATA_READ)
+		    ? "DMA_FROM_DEIVCE" : "DMA_TO_DEVICE", host->dma_size);
+
+		if (data->blksz & 0x3) {
+			printk(KERN_ERR
+			       "mxc_mci: block size not multiple of 4 bytes\n");
+		}
+
+		if (data->flags & MMC_DATA_READ)
+			host->dma_dir = DMA_FROM_DEVICE;
+		else
+			host->dma_dir = DMA_TO_DEVICE;
+
+		host->dma_len = dma_map_sg(mmc_dev(host->mmc), data->sg,
+					   data->sg_len, host->dma_dir);
+
+		if (data->flags & MMC_DATA_READ) {
+			mxc_dma_sg_config(host->dma, data->sg, data->sg_len,
+					  host->dma_size, MXC_DMA_MODE_READ);
+		} else {
+			mxc_dma_sg_config(host->dma, data->sg, data->sg_len,
+					  host->dma_size, MXC_DMA_MODE_WRITE);
+		}
+	} else {
+		host->cur_sg = data->sg;
+		host->num_sg = data->sg_len;
+
+		host->offset = 0;
+		host->remain = host->cur_sg->length;
+	}
+
+	/* We do not handle DMA boundaries, so set it to max (512 KiB) */
+	writel((data->blocks << 16) | SDHCI_MAKE_BLKSZ(7, data->blksz),
+	       host->ioaddr + SDHCI_BLOCK_SIZE);
+}
+
+static void sdhci_finish_data(struct sdhci_host *host)
+{
+	struct mmc_data *data;
+	u16 blocks;
+
+	BUG_ON(!host->data);
+
+	data = host->data;
+	host->data = NULL;
+
+	if (host->flags & SDHCI_REQ_USE_DMA) {
+		dma_unmap_sg(&(host->chip->pdev)->dev, data->sg, data->sg_len,
+			     (data->flags & MMC_DATA_READ) ? DMA_FROM_DEVICE :
+			     DMA_TO_DEVICE);
+	}
+	if ((host->flags & SDHCI_USE_EXTERNAL_DMA) &&
+	    (host->dma_size >= mxc_wml_value) && (data != NULL)) {
+		dma_unmap_sg(mmc_dev(host->mmc), data->sg,
+			     host->dma_len, host->dma_dir);
+		host->dma_size = 0;
+	}
+
+	/*
+	 * Controller doesn't count down when in single block mode.
+	 */
+	if (data->blocks == 1)
+		blocks = (data->error == 0) ? 0 : 1;
+	else
+		blocks = readl(host->ioaddr + SDHCI_BLOCK_COUNT) >> 16;
+	data->bytes_xfered = data->blksz * data->blocks;
+
+	if (data->stop) {
+		/*
+		 * The controller needs a reset of internal state machines
+		 * upon error conditions.
+		 */
+		if (data->error) {
+			sdhci_reset(host, SDHCI_RESET_CMD);
+			sdhci_reset(host, SDHCI_RESET_DATA);
+		}
+
+		sdhci_send_command(host, data->stop);
+	} else
+		tasklet_schedule(&host->finish_tasklet);
+}
+
+static void sdhci_send_command(struct sdhci_host *host, struct mmc_command *cmd)
+{
+	int flags;
+	u32 mask;
+	u32 mode = 0;
+	unsigned long timeout;
+
+	DBG("sdhci_send_command 0x%x is starting...\n", cmd->opcode);
+	WARN_ON(host->cmd);
+
+	/* Wait max 10 ms */
+	timeout = 5000;
+
+	mask = SDHCI_CMD_INHIBIT;
+	if ((cmd->data != NULL) || (cmd->flags & MMC_RSP_BUSY))
+		mask |= SDHCI_DATA_INHIBIT;
+
+	/* We shouldn't wait for data inihibit for stop commands, even
+	   though they might use busy signaling */
+	if (host->mrq->data && (cmd == host->mrq->data->stop))
+		mask &= ~SDHCI_DATA_INHIBIT;
+
+	while (readl(host->ioaddr + SDHCI_PRESENT_STATE) & mask) {
+		if (timeout == 0) {
+			printk(KERN_ERR "%s: Controller never released "
+			       "inhibit bit(s).\n", mmc_hostname(host->mmc));
+			sdhci_dumpregs(host);
+			cmd->error = -EIO;
+			tasklet_schedule(&host->finish_tasklet);
+			return;
+		}
+		timeout--;
+		udelay(20);
+	}
+
+	mod_timer(&host->timer, jiffies + 1 * HZ);
+
+	host->cmd = cmd;
+
+	sdhci_prepare_data(host, cmd->data);
+
+	writel(cmd->arg, host->ioaddr + SDHCI_ARGUMENT);
+
+	/* Set up the transfer mode */
+	if (cmd->data != NULL) {
+		mode = SDHCI_TRNS_BLK_CNT_EN | SDHCI_TRNS_DPSEL;
+		if (cmd->data->blocks > 1)
+			mode |= SDHCI_TRNS_MULTI;
+		if (cmd->data->flags & MMC_DATA_READ)
+			mode |= SDHCI_TRNS_READ;
+		else
+			mode &= ~SDHCI_TRNS_READ;
+		if (host->flags & SDHCI_USE_DMA)
+			mode |= SDHCI_TRNS_DMA;
+		if (host->flags & SDHCI_USE_EXTERNAL_DMA)
+			DBG("Prepare data completely in %s transfer mode.\n",
+			    "EXTTERNAL DMA");
+	}
+
+	if ((cmd->flags & MMC_RSP_136) && (cmd->flags & MMC_RSP_BUSY)) {
+		printk(KERN_ERR "%s: Unsupported response type!\n",
+		       mmc_hostname(host->mmc));
+		cmd->error = -EINVAL;
+		tasklet_schedule(&host->finish_tasklet);
+		return;
+	}
+
+	if (!(cmd->flags & MMC_RSP_PRESENT))
+		flags = SDHCI_CMD_RESP_NONE;
+	else if (cmd->flags & MMC_RSP_136)
+		flags = SDHCI_CMD_RESP_LONG;
+	else if (cmd->flags & MMC_RSP_BUSY)
+		flags = SDHCI_CMD_RESP_SHORT_BUSY;
+	else
+		flags = SDHCI_CMD_RESP_SHORT;
+
+	if (cmd->flags & MMC_RSP_CRC)
+		flags |= SDHCI_CMD_CRC;
+	if (cmd->flags & MMC_RSP_OPCODE)
+		flags |= SDHCI_CMD_INDEX;
+	if (cmd->data)
+		flags |= SDHCI_CMD_DATA;
+
+	mode |= SDHCI_MAKE_CMD(cmd->opcode, flags);
+	DBG("Complete sending cmd, transfer mode would be 0x%x.\n", mode);
+	writel(mode, host->ioaddr + SDHCI_TRANSFER_MODE);
+}
+
+static void sdhci_finish_command(struct sdhci_host *host)
+{
+	int i;
+
+	BUG_ON(host->cmd == NULL);
+
+	if (host->cmd->flags & MMC_RSP_PRESENT) {
+		if (host->cmd->flags & MMC_RSP_136) {
+			/* CRC is stripped so we need to do some shifting. */
+			for (i = 0; i < 4; i++) {
+				host->cmd->resp[i] = readl(host->ioaddr +
+							   SDHCI_RESPONSE + (3 -
+									     i)
+							   * 4) << 8;
+				if (i != 3)
+					host->cmd->resp[i] |=
+					    readb(host->ioaddr +
+						  SDHCI_RESPONSE + (3 - i) * 4 -
+						  1);
+			}
+		} else {
+			host->cmd->resp[0] =
+			    readl(host->ioaddr + SDHCI_RESPONSE);
+		}
+	}
+
+	host->cmd->error = 0;
+
+	if (host->data && host->data_early)
+		sdhci_finish_data(host);
+
+	if (!host->cmd->data)
+		tasklet_schedule(&host->finish_tasklet);
+
+	host->cmd = NULL;
+}
+
+static void sdhci_set_clock(struct sdhci_host *host, unsigned int clock)
+{
+	/*This variable holds the value of clock divider, prescaler */
+	int div = 0, prescaler = 0;
+	int clk_rate;
+	u32 clk;
+	unsigned long timeout;
+
+	if (clock == 0) {
+		goto out;
+	} else {
+		if (!host->plat_data->clk_flg) {
+			clk_enable(host->clk);
+			host->plat_data->clk_flg = 1;
+		}
+	}
+	if (clock == host->clock)
+		return;
+
+	clk_rate = clk_get_rate(host->clk);
+	clk = readl(host->ioaddr + SDHCI_CLOCK_CONTROL) & ~SDHCI_CLOCK_MASK;
+	writel(clk, host->ioaddr + SDHCI_CLOCK_CONTROL);
+
+	if (clock == host->min_clk)
+		prescaler = 16;
+	else
+		prescaler = 0;
+	while (prescaler <= 0x80) {
+		for (div = 0; div <= 0xF; div++) {
+			int x;
+			if (prescaler != 0)
+				x = (clk_rate / (div + 1)) / (prescaler * 2);
+			else
+				x = clk_rate / (div + 1);
+
+			DBG("x=%d, clock=%d %d\n", x, clock, div);
+			if (x <= clock)
+				break;
+		}
+		if (div < 0x10)
+			break;
+		if (prescaler == 0)
+			prescaler = 1;
+		else
+			prescaler <<= 1;
+	}
+	DBG("prescaler = 0x%x, divider = 0x%x\n", prescaler, div);
+	clk |= (prescaler << 8) | (div << 4);
+
+	/* Configure the clock control register */
+	clk |=
+	    (readl(host->ioaddr + SDHCI_CLOCK_CONTROL) & (~SDHCI_CLOCK_MASK));
+	if (host->plat_data->vendor_ver < ESDHC_VENDOR_V22)
+		writel(clk, host->ioaddr + SDHCI_CLOCK_CONTROL);
+	else
+		writel(clk | SDHCI_CLOCK_SD_EN,
+		       host->ioaddr + SDHCI_CLOCK_CONTROL);
+
+	/* Wait max 10 ms */
+	timeout = 5000;
+	while (timeout > 0) {
+		timeout--;
+		udelay(20);
+	}
+
+      out:
+	host->clock = clock;
+}
+
+static void sdhci_set_power(struct sdhci_host *host, unsigned short power)
+{
+	int voltage = 0;
+
+	/* There is no PWR CTL REG */
+	if (host->power == power)
+		return;
+
+	if (host->regulator_mmc) {
+		if (power == (unsigned short)-1) {
+			regulator_disable(host->regulator_mmc);
+			DBG("mmc power off\n");
+		} else {
+			if (power == 7)
+				voltage = 1800000;
+			else if (power >= 8)
+				voltage = 2000000 + (power - 8) * 100000;
+			regulator_set_voltage(host->regulator_mmc,
+					      voltage, voltage);
+
+			if (regulator_enable(host->regulator_mmc) == 0) {
+				DBG("mmc power on\n");
+				msleep(1);
+			}
+		}
+	}
+
+	host->power = power;
+}
+
+/*****************************************************************************\
+ *                                                                           *
+ * MMC callbacks                                                             *
+ *                                                                           *
+\*****************************************************************************/
+
+static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct sdhci_host *host;
+	unsigned long flags;
+
+	host = mmc_priv(mmc);
+
+	/* Enable the clock */
+	if (!host->plat_data->clk_flg) {
+		clk_enable(host->clk);
+		host->plat_data->clk_flg = 1;
+	}
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	WARN_ON(host->mrq != NULL);
+
+	sdhci_activate_led(host);
+	if (cpu_is_mx35_rev(CHIP_REV_2_0) < 0) {
+		if (mrq->cmd && mrq->data) {
+			if (mrq->data->flags & MMC_DATA_READ)
+				last_op_dir = 1;
+			else {
+				if (last_op_dir)
+					sdhci_reset(host,
+						    SDHCI_RESET_CMD |
+						    SDHCI_RESET_DATA);
+			}
+		}
+	}
+
+	if (host->flags & SDHCI_USE_EXTERNAL_DMA)
+		spin_unlock_irqrestore(&host->lock, flags);
+
+	host->mrq = mrq;
+	if (!(host->flags & SDHCI_CD_PRESENT)) {
+		host->mrq->cmd->error = -ENOMEDIUM;
+		tasklet_schedule(&host->finish_tasklet);
+	} else
+		sdhci_send_command(host, mrq->cmd);
+
+	if (!(host->flags & SDHCI_USE_EXTERNAL_DMA))
+		spin_unlock_irqrestore(&host->lock, flags);
+
+	mmiowb();
+}
+
+static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct sdhci_host *host;
+	unsigned long flags;
+	u32 tmp;
+	mxc_dma_device_t dev_id = 0;
+
+	DBG("%s: clock %u, bus %lu, power %u, vdd %u\n", DRIVER_NAME,
+	    ios->clock, 1UL << ios->bus_width, ios->power_mode, ios->vdd);
+
+	host = mmc_priv(mmc);
+
+	/* Configure the External DMA mode */
+	if (host->flags & SDHCI_USE_EXTERNAL_DMA) {
+		host->dma_dir = DMA_NONE;
+		if (mmc->ios.bus_width != host->mode) {
+			mxc_dma_free(host->dma);
+			if (mmc->ios.bus_width == MMC_BUS_WIDTH_4) {
+				if (host->id == 0)
+					dev_id = MXC_DMA_MMC1_WIDTH_4;
+				else
+					dev_id = MXC_DMA_MMC2_WIDTH_4;
+			} else {
+				if (host->id == 0)
+					dev_id = MXC_DMA_MMC1_WIDTH_1;
+				else
+					dev_id = MXC_DMA_MMC2_WIDTH_1;
+			}
+			host->dma = mxc_dma_request(dev_id, "MXC MMC");
+			if (host->dma < 0)
+				DBG("Cannot allocate MMC DMA channel\n");
+			mxc_dma_callback_set(host->dma, sdhci_dma_irq,
+					     (void *)host);
+			/* Configure the WML rege */
+			if (mxc_wml_value == 512)
+				writel(SDHCI_WML_128_WORDS,
+				       host->ioaddr + SDHCI_WML);
+			else
+				writel(SDHCI_WML_16_WORDS,
+				       host->ioaddr + SDHCI_WML);
+		}
+	}
+
+	host->mode = mmc->ios.bus_width;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	/*
+	 * Reset the chip on each power off.
+	 * Should clear out any weird states.
+	 */
+	if (ios->power_mode == MMC_POWER_OFF) {
+		writel(0, host->ioaddr + SDHCI_SIGNAL_ENABLE);
+		sdhci_init(host);
+	}
+
+	sdhci_set_clock(host, ios->clock);
+
+	if (ios->power_mode == MMC_POWER_OFF)
+		sdhci_set_power(host, -1);
+	else {
+		sdhci_set_power(host, ios->vdd);
+		if (!readl(host->ioaddr + SDHCI_SIGNAL_ENABLE)) {
+			tmp = readl(host->ioaddr + SDHCI_INT_ENABLE);
+			if (host->sdio_enable)
+				writel(tmp, host->ioaddr + SDHCI_SIGNAL_ENABLE);
+			else
+				writel(tmp & ~SDHCI_INT_CARD_INT,
+				       host->ioaddr + SDHCI_SIGNAL_ENABLE);
+		}
+	}
+
+	tmp = readl(host->ioaddr + SDHCI_HOST_CONTROL);
+
+	if (ios->bus_width == MMC_BUS_WIDTH_4) {
+		tmp &= ~SDHCI_CTRL_8BITBUS;
+		tmp |= SDHCI_CTRL_4BITBUS;
+	} else if (ios->bus_width == MMC_BUS_WIDTH_8) {
+		tmp &= ~SDHCI_CTRL_4BITBUS;
+		tmp |= SDHCI_CTRL_8BITBUS;
+	} else if (ios->bus_width == MMC_BUS_WIDTH_1) {
+		tmp &= ~SDHCI_CTRL_4BITBUS;
+		tmp &= ~SDHCI_CTRL_8BITBUS;
+	}
+
+	if (host->flags & SDHCI_USE_DMA)
+		tmp |= SDHCI_CTRL_ADMA;
+
+	writel(tmp, host->ioaddr + SDHCI_HOST_CONTROL);
+
+	/*
+	 * Some (ENE) controllers go apeshit on some ios operation,
+	 * signalling timeout and CRC errors even on CMD0. Resetting
+	 * it on each ios seems to solve the problem.
+	 */
+	if (host->chip->quirks & SDHCI_QUIRK_RESET_CMD_DATA_ON_IOS)
+		sdhci_reset(host, SDHCI_RESET_CMD | SDHCI_RESET_DATA);
+
+	mmiowb();
+	spin_unlock_irqrestore(&host->lock, flags);
+}
+
+static int sdhci_get_ro(struct mmc_host *mmc)
+{
+	struct sdhci_host *host;
+
+	host = mmc_priv(mmc);
+
+	if (host->plat_data->wp_status)
+		return host->plat_data->wp_status(mmc->parent);
+	else
+		return 0;
+}
+
+static void sdhci_enable_sdio_irq(struct mmc_host *mmc, int enable)
+{
+	struct sdhci_host *host;
+	unsigned long flags;
+	u32 ier, prot, clk, present;
+
+	host = mmc_priv(mmc);
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (enable) {
+		if (host->sdio_enable++)
+			goto exit_unlock;
+	} else {
+		if (--(host->sdio_enable))
+			goto exit_unlock;
+	}
+	/* Enable the clock */
+	if (!host->plat_data->clk_flg) {
+		clk_enable(host->clk);
+		host->plat_data->clk_flg = 1;
+	}
+	ier = readl(host->ioaddr + SDHCI_SIGNAL_ENABLE);
+	prot = readl(host->ioaddr + SDHCI_HOST_CONTROL);
+	clk = readl(host->ioaddr + SDHCI_CLOCK_CONTROL);
+
+	if (enable) {
+		ier |= SDHCI_INT_CARD_INT;
+		prot |= SDHCI_CTRL_D3CD;
+		clk |= SDHCI_CLOCK_PER_EN | SDHCI_CLOCK_IPG_EN;
+		present = readl(host->ioaddr + SDHCI_PRESENT_STATE);
+		if ((present & SDHCI_CARD_INT_MASK) != SDHCI_CARD_INT_ID)
+			writel(SDHCI_INT_CARD_INT,
+			       host->ioaddr + SDHCI_INT_STATUS);
+	} else {
+		ier &= ~SDHCI_INT_CARD_INT;
+		prot &= ~SDHCI_CTRL_D3CD;
+		clk &= ~(SDHCI_CLOCK_PER_EN | SDHCI_CLOCK_IPG_EN);
+	}
+
+	writel(prot, host->ioaddr + SDHCI_HOST_CONTROL);
+	writel(ier, host->ioaddr + SDHCI_SIGNAL_ENABLE);
+	writel(clk, host->ioaddr + SDHCI_CLOCK_CONTROL);
+
+	mmiowb();
+      exit_unlock:
+	spin_unlock_irqrestore(&host->lock, flags);
+}
+
+static const struct mmc_host_ops sdhci_ops = {
+	.request = sdhci_request,
+	.set_ios = sdhci_set_ios,
+	.get_ro = sdhci_get_ro,
+	.enable_sdio_irq = sdhci_enable_sdio_irq,
+};
+
+/*****************************************************************************\
+ *                                                                           *
+ * Tasklets                                                                  *
+ *                                                                           *
+\*****************************************************************************/
+
+static void sdhci_tasklet_card(unsigned long param)
+{
+	struct sdhci_host *host;
+	unsigned long flags;
+	unsigned int cd_status = 0;
+
+	host = (struct sdhci_host *)param;
+
+	if (host->flags & SDHCI_CD_PRESENT)
+		host->flags &= ~SDHCI_CD_PRESENT;
+	else
+		host->flags |= SDHCI_CD_PRESENT;
+	/* Detect there is a card in slot or not */
+	DBG("cd_status=%d %s\n", cd_status,
+	    (host->flags & SDHCI_CD_PRESENT) ? "inserted" : "removed");
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (!(host->flags & SDHCI_CD_PRESENT)) {
+		if (host->mrq) {
+			printk(KERN_ERR "%s: Card removed during transfer!\n",
+			       mmc_hostname(host->mmc));
+			printk(KERN_ERR "%s: Resetting controller.\n",
+			       mmc_hostname(host->mmc));
+
+			sdhci_reset(host, SDHCI_RESET_CMD);
+			sdhci_reset(host, SDHCI_RESET_DATA);
+
+			host->mrq->cmd->error = -ENOMEDIUM;
+			tasklet_schedule(&host->finish_tasklet);
+		}
+	}
+
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	mmc_detect_change(host->mmc, msecs_to_jiffies(200));
+}
+
+static void sdhci_tasklet_finish(unsigned long param)
+{
+	struct sdhci_host *host;
+	unsigned long flags;
+	struct mmc_request *mrq;
+
+	host = (struct sdhci_host *)param;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	del_timer(&host->timer);
+
+	mrq = host->mrq;
+
+	/*
+	 * The controller needs a reset of internal state machines
+	 * upon error conditions.
+	 */
+	if (mrq->cmd->error ||
+	    (mrq->data && (mrq->data->error ||
+			   (mrq->data->stop && mrq->data->stop->error))) ||
+	    (host->chip->quirks & SDHCI_QUIRK_RESET_AFTER_REQUEST)) {
+
+		/* Some controllers need this kick or reset won't work here */
+		if (host->chip->quirks & SDHCI_QUIRK_CLOCK_BEFORE_RESET) {
+			unsigned int clock;
+
+			/* This is to force an update */
+			clock = host->clock;
+			host->clock = 0;
+			sdhci_set_clock(host, clock);
+		}
+
+		/* Spec says we should do both at the same time, but Ricoh
+		   controllers do not like that. */
+		sdhci_reset(host, SDHCI_RESET_CMD);
+		sdhci_reset(host, SDHCI_RESET_DATA);
+	}
+
+	host->mrq = NULL;
+	host->cmd = NULL;
+	host->data = NULL;
+
+	sdhci_deactivate_led(host);
+
+	mmiowb();
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	/* Stop the clock when the req is done */
+	flags = SDHCI_DATA_ACTIVE | SDHCI_DOING_WRITE | SDHCI_DOING_READ;
+	if (!(readl(host->ioaddr + SDHCI_PRESENT_STATE) & flags)) {
+		if (host->plat_data->clk_flg) {
+			clk_disable(host->clk);
+			host->plat_data->clk_flg = 0;
+		}
+	}
+
+	mmc_request_done(host->mmc, mrq);
+}
+
+static void sdhci_timeout_timer(unsigned long data)
+{
+	struct sdhci_host *host;
+	unsigned long tmp, flags;
+
+	host = (struct sdhci_host *)data;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (host->mrq) {
+		printk(KERN_ERR "%s: Timeout waiting for hardware "
+		       "interrupt.\n", mmc_hostname(host->mmc));
+		sdhci_dumpregs(host);
+
+		if (host->data) {
+			host->data->error = -ETIMEDOUT;
+			sdhci_finish_data(host);
+		} else {
+			if (host->cmd)
+				host->cmd->error = -ETIMEDOUT;
+			else
+				host->mrq->cmd->error = -ETIMEDOUT;
+
+			tasklet_schedule(&host->finish_tasklet);
+		}
+
+		if (!readl(host->ioaddr + SDHCI_SIGNAL_ENABLE)) {
+			printk(KERN_ERR "%s, ERROR SIG_INT is 0.\n", __func__);
+			tmp = readl(host->ioaddr + SDHCI_INT_ENABLE);
+			if (host->sdio_enable)
+				writel(tmp, host->ioaddr + SDHCI_SIGNAL_ENABLE);
+			else
+				writel(tmp & ~SDHCI_INT_CARD_INT,
+				       host->ioaddr + SDHCI_SIGNAL_ENABLE);
+			if (!host->plat_data->status(host->mmc->parent))
+				schedule_work(&host->cd_wq);
+		}
+	}
+
+	mmiowb();
+	spin_unlock_irqrestore(&host->lock, flags);
+}
+
+static void sdhci_cd_timer(unsigned long data)
+{
+	struct sdhci_host *host;
+
+	host = (struct sdhci_host *)data;
+	schedule_work(&host->cd_wq);
+}
+
+/*****************************************************************************\
+ *                                                                           *
+ * Interrupt handling                                                        *
+ *                                                                           *
+\*****************************************************************************/
+
+static void sdhci_cmd_irq(struct sdhci_host *host, u32 intmask)
+{
+	BUG_ON(intmask == 0);
+
+	if (!host->cmd) {
+		printk(KERN_ERR "%s: Got command interrupt 0x%08x even "
+		       "though no command operation was in progress.\n",
+		       mmc_hostname(host->mmc), (unsigned)intmask);
+		sdhci_dumpregs(host);
+		return;
+	}
+
+	if (intmask & SDHCI_INT_TIMEOUT)
+		host->cmd->error = -ETIMEDOUT;
+	else if (intmask & (SDHCI_INT_CRC | SDHCI_INT_END_BIT |
+			    SDHCI_INT_INDEX))
+		host->cmd->error = -EILSEQ;
+
+	if (host->cmd->error)
+		tasklet_schedule(&host->finish_tasklet);
+	else if (intmask & SDHCI_INT_RESPONSE)
+		sdhci_finish_command(host);
+}
+
+static void sdhci_data_irq(struct sdhci_host *host, u32 intmask)
+{
+	u32 intsave = 0;
+
+	BUG_ON(intmask == 0);
+
+	if (!host->data) {
+		/*
+		 * A data end interrupt is sent together with the response
+		 * for the stop command.
+		 */
+		if (intmask & SDHCI_INT_DATA_END)
+			return;
+
+		printk(KERN_ERR "%s: Got data interrupt 0x%08x even "
+		       "though no data operation was in progress.\n",
+		       mmc_hostname(host->mmc), (unsigned)intmask);
+		sdhci_dumpregs(host);
+		sdhci_reset(host, SDHCI_RESET_CMD);
+		sdhci_reset(host, SDHCI_RESET_DATA);
+		return;
+	}
+
+	/* Mask the INT */
+	intsave = readl(host->ioaddr + SDHCI_INT_ENABLE);
+	writel(intsave & (~(intmask & SDHCI_INT_DATA_RE_MASK)),
+	       host->ioaddr + SDHCI_INT_ENABLE);
+
+	if (intmask & SDHCI_INT_DATA_TIMEOUT)
+		host->data->error = -ETIMEDOUT;
+	else if (intmask & (SDHCI_INT_DATA_CRC | SDHCI_INT_DATA_END_BIT))
+		host->data->error = -EILSEQ;
+
+	if (host->data->error)
+		sdhci_finish_data(host);
+	else {
+		if ((host->flags & SDHCI_USE_EXTERNAL_DMA) &&
+		    (host->dma_size >= mxc_wml_value)) {
+			/* Use DMA if transfer size is greater than fifo size */
+			if (intmask & (SDHCI_INT_DATA_AVAIL |
+				       SDHCI_INT_SPACE_AVAIL)) {
+				intsave &= ~SDHCI_INT_DATA_RE_MASK;
+				if (mxc_dma_enable(host->dma) < 0) {
+					printk(KERN_ERR "ENABLE SDMA ERR.\n");
+					intsave |= SDHCI_INT_DATA_RE_MASK;
+				}
+			}
+		} else {
+			if (intmask & (SDHCI_INT_DATA_AVAIL |
+				       SDHCI_INT_SPACE_AVAIL))
+				sdhci_transfer_pio(host);
+		}
+
+		/*
+		 * We currently don't do anything fancy with DMA
+		 * boundaries, but as we can't disable the feature
+		 * we need to at least restart the transfer.
+		 */
+		if ((intmask & SDHCI_INT_DMA_END) &&
+		    (!(intmask & SDHCI_INT_DATA_END)))
+			writel(readl(host->ioaddr + SDHCI_DMA_ADDRESS),
+			       host->ioaddr + SDHCI_DMA_ADDRESS);
+
+		if (intmask & SDHCI_INT_DATA_END) {
+			if (host->data->flags & MMC_DATA_READ)
+				writel(readl(host->ioaddr + SDHCI_CLOCK_CONTROL)
+				       & ~SDHCI_CLOCK_HLK_EN,
+				       host->ioaddr + SDHCI_CLOCK_CONTROL);
+			if (host->cmd) {
+				/*
+				 * Data managed to finish before the
+				 * command completed. Make sure we do
+				 * things in the proper order.
+				 */
+				host->data_early = 1;
+			} else {
+
+				if (host->plat_data->vendor_ver
+				    < ESDHC_VENDOR_V22) {
+					/*
+					 * There are the DATA END INT when
+					 * writing is not complete. Double
+					 * check on it. TO2 has been fixed it.
+					 */
+					intmask = readl(host->ioaddr +
+							SDHCI_PRESENT_STATE);
+					if (intmask & SDHCI_DATA_ACTIVE)
+						goto data_irq_out;
+				}
+				sdhci_finish_data(host);
+			}
+		}
+	}
+      data_irq_out:
+	/* Enable the INT */
+	writel(intsave, host->ioaddr + SDHCI_INT_ENABLE);
+}
+
+/*!
+* This function is called by DMA Interrupt Service Routine to indicate
+* requested DMA transfer is completed.
+*
+* @param   devid  pointer to device specific structure
+* @param   error any DMA error
+* @param   cnt   amount of data that was transferred
+*/
+static void sdhci_dma_irq(void *devid, int error, unsigned int cnt)
+{
+	u32 intsave = 0;
+	int ret;
+	struct sdhci_host *host = devid;
+
+	DBG("%s: error: %d Transferred bytes:%d\n", DRIVER_NAME, error, cnt);
+	if (host->flags & SDHCI_USE_EXTERNAL_DMA) {
+		/*
+		 * Stop the DMA transfer here, the data_irq would be called
+		 * to process the others
+		 */
+		ret = mxc_dma_disable(host->dma);
+		if (ret < 0)
+			printk(KERN_ERR "Disable dma channel err %d\n", ret);
+
+		if (error) {
+			DBG("Error in DMA transfer\n");
+			return;
+		}
+		intsave = readl(host->ioaddr + SDHCI_INT_ENABLE);
+		intsave |= SDHCI_INT_DATA_RE_MASK;
+		writel(intsave, host->ioaddr + SDHCI_INT_ENABLE);
+	}
+}
+
+/* woke queue handler func */
+static void esdhc_cd_callback(struct work_struct *work)
+{
+	unsigned long flags;
+	unsigned int cd_status = 0;
+	struct sdhci_host *host = container_of(work, struct sdhci_host, cd_wq);
+
+	cd_status = host->plat_data->status(host->mmc->parent);
+	if (cd_status)
+		host->flags &= ~SDHCI_CD_PRESENT;
+	else
+		host->flags |= SDHCI_CD_PRESENT;
+	/* Detect there is a card in slot or not */
+	DBG("cd_status=%d %s\n", cd_status,
+	    (host->flags & SDHCI_CD_PRESENT) ? "inserted" : "removed");
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (!(host->flags & SDHCI_CD_PRESENT)) {
+		printk(KERN_INFO
+		       "%s: Card removed and resetting controller.\n",
+		       mmc_hostname(host->mmc));
+		if (host->mrq) {
+			struct mmc_data *data;
+			data = host->data;
+			host->data = NULL;
+
+			printk(KERN_ERR
+			       "%s: Card removed during transfer!\n",
+			       mmc_hostname(host->mmc));
+			printk(KERN_ERR
+			       "%s: Resetting controller.\n",
+			       mmc_hostname(host->mmc));
+
+			if ((host->flags & SDHCI_USE_EXTERNAL_DMA) &&
+			    (data != NULL)) {
+				dma_unmap_sg(mmc_dev(host->mmc), data->sg,
+					     host->dma_len, host->dma_dir);
+				host->dma_size = 0;
+			}
+			sdhci_reset(host, SDHCI_RESET_CMD);
+			sdhci_reset(host, SDHCI_RESET_DATA);
+
+			host->mrq->cmd->error = -ENOMEDIUM;
+			tasklet_schedule(&host->finish_tasklet);
+		}
+
+		if (host->init_flag > 0)
+			/* The initialization of sdhc controller has been
+			 * done in the resume func */
+			host->init_flag--;
+		else
+			sdhci_init(host);
+	}
+
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	if (host->flags & SDHCI_CD_PRESENT) {
+		del_timer(&host->cd_timer);
+		mmc_detect_change(host->mmc, msecs_to_jiffies(100));
+	} else
+		mmc_detect_change(host->mmc, 0);
+}
+
+/*!
+* Card detection interrupt service routine registered to handle
+* the SDHC interrupts. This interrupt routine handles card
+* insertion and card removal interrupts.
+*
+* @param   irq    the interrupt number
+* @param   devid  driver private data
+*
+* @return  The function returns \b IRQ_RETVAL(1)
+*/
+static irqreturn_t sdhci_cd_irq(int irq, void *dev_id)
+{
+	unsigned int cd_status = 0;
+	struct sdhci_host *host = dev_id;
+
+	do {
+		if (host->detect_irq == 0)
+			break;
+		cd_status = host->plat_data->status(host->mmc->parent);
+		if (cd_status)
+			set_irq_type(host->detect_irq, IRQF_TRIGGER_FALLING);
+		else
+			set_irq_type(host->detect_irq, IRQF_TRIGGER_RISING);
+	} while (cd_status != host->plat_data->status(host->mmc->parent));
+
+	DBG("cd_status=%d %s\n", cd_status, cd_status ? "removed" : "inserted");
+
+	cd_status = host->plat_data->status(host->mmc->parent);
+	if (!cd_status)
+		/* If there is a card in the slot, the timer is start
+		 * to work. Then the card detection would be carried
+		 * after the timer is timeout.
+		 * */
+		mod_timer(&host->cd_timer, jiffies + HZ / 2);
+	else
+		/* If there is no card, call the card detection func
+		 * immediately. */
+		schedule_work(&host->cd_wq);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t sdhci_irq(int irq, void *dev_id)
+{
+	irqreturn_t result;
+	struct sdhci_host *host = dev_id;
+	u32 intmask;
+	int cardint = 0;
+
+	spin_lock(&host->lock);
+
+	intmask = readl(host->ioaddr + SDHCI_INT_STATUS);
+
+	if (!intmask || intmask == 0xffffffff) {
+		result = IRQ_NONE;
+		goto out;
+	}
+
+	DBG("*** %s got interrupt: 0x%08x\n", mmc_hostname(host->mmc), intmask);
+
+	if (intmask & (SDHCI_INT_CARD_INSERT | SDHCI_INT_CARD_REMOVE)) {
+		writel(intmask &
+		       (SDHCI_INT_CARD_INSERT | SDHCI_INT_CARD_REMOVE),
+		       host->ioaddr + SDHCI_INT_STATUS);
+		tasklet_schedule(&host->card_tasklet);
+	}
+
+	intmask &= ~(SDHCI_INT_CARD_INSERT | SDHCI_INT_CARD_REMOVE);
+
+	if (intmask & SDHCI_INT_CMD_MASK) {
+		writel(intmask & SDHCI_INT_CMD_MASK,
+		       host->ioaddr + SDHCI_INT_STATUS);
+		sdhci_cmd_irq(host, intmask & SDHCI_INT_CMD_MASK);
+	}
+
+	if (intmask & SDHCI_INT_DATA_MASK) {
+		writel(intmask & SDHCI_INT_DATA_MASK,
+		       host->ioaddr + SDHCI_INT_STATUS);
+		if (cpu_is_mx35_rev(CHIP_REV_2_0) < 0) {
+			if (!
+			    (readl(host->ioaddr + SDHCI_TRANSFER_MODE) &
+			     SDHCI_TRNS_READ))
+				intmask &= ~SDHCI_INT_DATA_END_BIT;
+		}
+		if (intmask & SDHCI_INT_DATA_MASK)
+			sdhci_data_irq(host, intmask & SDHCI_INT_DATA_MASK);
+	}
+
+	intmask &= ~(SDHCI_INT_CMD_MASK | SDHCI_INT_DATA_MASK);
+
+	intmask &= ~SDHCI_INT_ERROR;
+
+	if (intmask & SDHCI_INT_BUS_POWER) {
+		printk(KERN_ERR "%s: Card is consuming too much power!\n",
+		       mmc_hostname(host->mmc));
+		writel(SDHCI_INT_BUS_POWER, host->ioaddr + SDHCI_INT_STATUS);
+	}
+
+	intmask &= ~SDHCI_INT_BUS_POWER;
+
+	if (intmask & SDHCI_INT_CARD_INT)
+		cardint = readl(host->ioaddr + SDHCI_SIGNAL_ENABLE) &
+		    SDHCI_INT_CARD_INT;
+
+	intmask &= ~SDHCI_INT_CARD_INT;
+
+	if (intmask) {
+		printk(KERN_ERR "%s: Unexpected interrupt 0x%08x.\n",
+		       mmc_hostname(host->mmc), intmask);
+		sdhci_dumpregs(host);
+
+		writel(intmask, host->ioaddr + SDHCI_INT_STATUS);
+	}
+
+	result = IRQ_HANDLED;
+
+	mmiowb();
+      out:
+	spin_unlock(&host->lock);
+
+	/*
+	 * We have to delay this as it calls back into the driver.
+	 */
+	if (cardint)
+		mmc_signal_sdio_irq(host->mmc);
+
+	return result;
+}
+
+/*****************************************************************************\
+ *                                                                           *
+ * Suspend/resume                                                            *
+ *                                                                           *
+\*****************************************************************************/
+
+#ifdef CONFIG_PM
+
+static int sdhci_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct sdhci_chip *chip;
+	int i, ret;
+
+	chip = dev_get_drvdata(&pdev->dev);
+	if (!chip)
+		return 0;
+
+	DBG("Suspending...\n");
+
+	for (i = 0; i < chip->num_slots; i++) {
+		if (!chip->hosts[i])
+			continue;
+		ret = mmc_suspend_host(chip->hosts[i]->mmc, state);
+		if (ret) {
+			for (i--; i >= 0; i--)
+				mmc_resume_host(chip->hosts[i]->mmc);
+			return ret;
+		}
+	}
+
+	for (i = 0; i < chip->num_slots; i++) {
+		if (!chip->hosts[i])
+			continue;
+		free_irq(chip->hosts[i]->irq, chip->hosts[i]);
+	}
+
+	return 0;
+}
+
+static int sdhci_resume(struct platform_device *pdev)
+{
+	struct sdhci_chip *chip;
+	int i, ret;
+
+	chip = dev_get_drvdata(&pdev->dev);
+	if (!chip)
+		return 0;
+
+	DBG("Resuming...\n");
+
+	for (i = 0; i < chip->num_slots; i++) {
+		if (!chip->hosts[i])
+			continue;
+		ret = request_irq(chip->hosts[i]->irq, sdhci_irq,
+				  IRQF_SHARED,
+				  mmc_hostname(chip->hosts[i]->mmc),
+				  chip->hosts[i]);
+		if (ret)
+			return ret;
+		sdhci_init(chip->hosts[i]);
+		chip->hosts[i]->init_flag = 2;
+		mmiowb();
+		ret = mmc_resume_host(chip->hosts[i]->mmc);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+#else				/* CONFIG_PM */
+
+#define sdhci_suspend NULL
+#define sdhci_resume NULL
+
+#endif				/* CONFIG_PM */
+
+/*****************************************************************************\
+ *                                                                           *
+ * Device probing/removal                                                    *
+ *                                                                           *
+\*****************************************************************************/
+
+static int __devinit sdhci_probe_slot(struct platform_device
+				      *pdev, int slot)
+{
+	struct mxc_mmc_platform_data *mmc_plat = pdev->dev.platform_data;
+	int ret = 0;
+	unsigned int version, caps;
+	struct sdhci_chip *chip;
+	struct mmc_host *mmc;
+	struct sdhci_host *host;
+	mxc_dma_device_t dev_id = 0;
+
+	if (!mmc_plat)
+		return -EINVAL;
+
+	chip = dev_get_drvdata(&pdev->dev);
+	BUG_ON(!chip);
+
+	mmc = mmc_alloc_host(sizeof(struct sdhci_host), &pdev->dev);
+	if (!mmc)
+		return -ENOMEM;
+
+	host = mmc_priv(mmc);
+	host->mmc = mmc;
+	host->id = pdev->id;
+	host->dma = -1;
+	host->plat_data = mmc_plat;
+	if (!host->plat_data) {
+		ret = -EINVAL;
+		goto out0;
+	}
+
+	host->chip = chip;
+	chip->hosts[slot] = host;
+
+	/* Get pwr supply for eSDHC */
+	if (NULL != mmc_plat->power_mmc) {
+		host->regulator_mmc =
+		    regulator_get(&pdev->dev, mmc_plat->power_mmc);
+		if (IS_ERR(host->regulator_mmc)) {
+			ret = PTR_ERR(host->regulator_mmc);
+			goto out1;
+		}
+		if (regulator_enable(host->regulator_mmc) == 0) {
+			DBG("mmc power on\n");
+			msleep(1);
+		}
+	}
+
+	/* Active the eSDHC bus */
+	gpio_sdhc_active(pdev->id);
+
+	/* Get the SDHC clock from clock system APIs */
+	host->clk = clk_get(&pdev->dev, mmc_plat->clock_mmc);
+	if (NULL == host->clk)
+		printk(KERN_ERR "MXC MMC can't get clock.\n");
+	DBG("SDHC:%d clock:%lu\n", pdev->id, clk_get_rate(host->clk));
+
+	host->res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!host->res) {
+		ret = -ENOMEM;
+		goto out2;
+	}
+	host->irq = platform_get_irq(pdev, 0);
+	if (!host->irq) {
+		ret = -ENOMEM;
+		goto out2;
+	}
+	host->detect_irq = platform_get_irq(pdev, 1);
+	if (!host->detect_irq) {
+		host->flags &= ~SDHCI_CD_PRESENT;
+		if ((pdev->id >= 0) && (pdev->id < MXC_SDHCI_NUM))
+			mxc_fix_chips[pdev->id] = chip;
+		goto no_detect_irq;
+	}
+
+	do {
+		ret = host->plat_data->status(host->mmc->parent);
+		if (ret)
+			set_irq_type(host->detect_irq, IRQF_TRIGGER_FALLING);
+		else
+			set_irq_type(host->detect_irq, IRQF_TRIGGER_RISING);
+	} while (ret != host->plat_data->status(host->mmc->parent));
+
+	ret = host->plat_data->status(host->mmc->parent);
+	if (ret)
+		host->flags &= ~SDHCI_CD_PRESENT;
+	else
+		host->flags |= SDHCI_CD_PRESENT;
+
+      no_detect_irq:
+	DBG("slot %d at 0x%x, irq %d \n", slot, host->res->start, host->irq);
+	if (!request_mem_region(host->res->start,
+				host->res->end -
+				host->res->start + 1, pdev->name)) {
+		printk(KERN_ERR "request_mem_region failed\n");
+		ret = -ENOMEM;
+		goto out2;
+	}
+	host->ioaddr = (void *)ioremap(host->res->start, host->res->end -
+				       host->res->start + 1);
+	if (!host->ioaddr) {
+		ret = -ENOMEM;
+		goto out3;
+	}
+
+	sdhci_reset(host, SDHCI_RESET_ALL);
+
+	version = readl(host->ioaddr + SDHCI_HOST_VERSION);
+	host->plat_data->vendor_ver = (version & SDHCI_VENDOR_VER_MASK) >>
+	    SDHCI_VENDOR_VER_SHIFT;
+	version = (version & SDHCI_SPEC_VER_MASK) >> SDHCI_SPEC_VER_SHIFT;
+	if (version != 1) {
+		printk(KERN_ERR "%s: Unknown controller version (%d). "
+		       "You may experience problems.\n", mmc_hostname(mmc),
+		       version);
+	}
+
+	caps = readl(host->ioaddr + SDHCI_CAPABILITIES);
+
+	if (chip->quirks & SDHCI_QUIRK_FORCE_DMA)
+		host->flags |= SDHCI_USE_DMA;
+	else if (!(caps & SDHCI_CAN_DO_DMA))
+		DBG("Controller doesn't have DMA capability\n");
+	else if (chip->
+		 quirks & (SDHCI_QUIRK_INTERNAL_ADVANCED_DMA |
+			   SDHCI_QUIRK_INTERNAL_SIMPLE_DMA))
+		host->flags |= SDHCI_USE_DMA;
+	else if (chip->quirks & (SDHCI_QUIRK_EXTERNAL_DMA_MODE))
+		host->flags |= SDHCI_USE_EXTERNAL_DMA;
+	else
+		host->flags &= ~SDHCI_USE_DMA;
+
+	/*
+	 * These definitions of eSDHC are not compatible with the SD Host
+	 * Controller Spec v2.0
+	 */
+	host->min_clk = mmc_plat->min_clk;
+	host->max_clk = mmc_plat->max_clk;
+	host->timeout_clk = 1024 * 1000;	/* Just set the value temply. */
+
+	/*
+	 * Set host parameters.
+	 */
+	mmc->ops = &sdhci_ops;
+	mmc->f_min = host->min_clk;
+	mmc->f_max = host->max_clk;
+	mmc->caps = MMC_CAP_SDIO_IRQ;
+	mmc->caps |= mmc_plat->caps;
+
+	if (caps & SDHCI_CAN_DO_HISPD)
+		mmc->caps |= MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED;
+
+	mmc->ocr_avail = mmc_plat->ocr_mask;
+	if (caps & SDHCI_CAN_VDD_330)
+		mmc->ocr_avail |= MMC_VDD_32_33 | MMC_VDD_33_34;
+	if (caps & SDHCI_CAN_VDD_300)
+		mmc->ocr_avail |= MMC_VDD_29_30 | MMC_VDD_30_31;
+	if (caps & SDHCI_CAN_VDD_180)
+		mmc->ocr_avail |= MMC_VDD_165_195;
+
+	if (mmc->ocr_avail == 0) {
+		printk(KERN_ERR "%s: Hardware doesn't report any "
+		       "support voltages.\n", mmc_hostname(mmc));
+		ret = -ENODEV;
+		goto out3;
+	}
+
+	spin_lock_init(&host->lock);
+
+	/*
+	 * Maximum number of segments. Hardware cannot do scatter lists.
+	 */
+	if (host->flags & SDHCI_USE_DMA)
+		mmc->max_hw_segs = 1;
+	else
+		mmc->max_hw_segs = 16;
+	mmc->max_phys_segs = 16;
+
+	/*
+	 * Maximum number of sectors in one transfer. Limited by DMA boundary
+	 * size (512KiB).
+	 */
+	if (host->flags & SDHCI_USE_EXTERNAL_DMA)
+		mmc->max_req_size = 32 * 1024;
+	else
+		mmc->max_req_size = 524288;
+
+	/*
+	 * Maximum segment size. Could be one segment with the maximum number
+	 * of bytes.
+	 */
+	mmc->max_seg_size = mmc->max_req_size;
+
+	/*
+	 * Maximum block size. This varies from controller to controller and
+	 * is specified in the capabilities register.
+	 */
+	mmc->max_blk_size =
+	    (caps & SDHCI_MAX_BLOCK_MASK) >> SDHCI_MAX_BLOCK_SHIFT;
+	if (mmc->max_blk_size > 3) {
+		printk(KERN_WARNING "%s: Invalid maximum block size, "
+		       "assuming 512 bytes\n", mmc_hostname(mmc));
+		mmc->max_blk_size = 512;
+	} else
+		mmc->max_blk_size = 512 << mmc->max_blk_size;
+
+	/*
+	 * Maximum block count.
+	 */
+	mmc->max_blk_count = 65535;
+
+	/*
+	 * Apply a continous physical memory used for storing the ADMA
+	 * descriptor table.
+	 */
+	if (host->flags & SDHCI_USE_DMA) {
+		adma_des_table = kcalloc((2 * (mmc->max_phys_segs) + 1),
+					 sizeof(unsigned int), GFP_DMA);
+		if (adma_des_table == NULL) {
+			printk(KERN_ERR "Cannot allocate ADMA memory\n");
+			ret = -ENOMEM;
+			goto out3;
+		}
+	}
+
+	/*
+	 * Init tasklets.
+	 */
+	tasklet_init(&host->card_tasklet,
+		     sdhci_tasklet_card, (unsigned long)host);
+	tasklet_init(&host->finish_tasklet,
+		     sdhci_tasklet_finish, (unsigned long)host);
+
+	/* initialize the work queue */
+	INIT_WORK(&host->cd_wq, esdhc_cd_callback);
+
+	setup_timer(&host->timer, sdhci_timeout_timer, (unsigned long)host);
+	setup_timer(&host->cd_timer, sdhci_cd_timer, (unsigned long)host);
+
+	if (host->detect_irq) {
+		ret = request_irq(host->detect_irq, sdhci_cd_irq, 0,
+				  pdev->name, host);
+		if (ret)
+			goto out4;
+	}
+
+	ret = request_irq(host->irq, sdhci_irq, IRQF_SHARED, pdev->name, host);
+	if (ret)
+		goto out5;
+
+	sdhci_init(host);
+
+	if (host->flags & SDHCI_USE_EXTERNAL_DMA) {
+		/* Apply the 1-bit SDMA channel. */
+		if (host->id == 0)
+			dev_id = MXC_DMA_MMC1_WIDTH_1;
+		else
+			dev_id = MXC_DMA_MMC2_WIDTH_1;
+		host->dma = mxc_dma_request(dev_id, "MXC MMC");
+		if (host->dma < 0) {
+			DBG("Cannot allocate MMC DMA channel\n");
+			goto out6;
+		}
+		mxc_dma_callback_set(host->dma, sdhci_dma_irq, (void *)host);
+	}
+#ifdef CONFIG_MMC_DEBUG
+	sdhci_dumpregs(host);
+#endif
+
+	mmiowb();
+
+	if (mmc_add_host(mmc) < 0)
+		goto out6;
+	if (host->flags & SDHCI_USE_EXTERNAL_DMA)
+		printk(KERN_INFO "%s: SDHCI detect irq %d irq %d %s\n",
+		       mmc_hostname(mmc), host->detect_irq, host->irq,
+		       "EXTERNAL DMA");
+	else
+		printk(KERN_INFO "%s: SDHCI detect irq %d irq %d %s\n",
+		       mmc_hostname(mmc), host->detect_irq, host->irq,
+		       (host->flags & SDHCI_USE_DMA) ? "INTERNAL DMA" : "PIO");
+
+	return 0;
+
+      out6:
+	free_irq(host->irq, host);
+      out5:
+	if (host->detect_irq)
+		free_irq(host->detect_irq, host);
+	else {
+		if ((pdev->id >= 0) && (pdev->id < MXC_SDHCI_NUM))
+			mxc_fix_chips[pdev->id] = chip;
+	}
+      out4:
+	del_timer_sync(&host->timer);
+	del_timer_sync(&host->cd_timer);
+	tasklet_kill(&host->card_tasklet);
+	tasklet_kill(&host->finish_tasklet);
+      out3:
+	if (host->flags & SDHCI_USE_DMA)
+		kfree(adma_des_table);
+	release_mem_region(host->res->start,
+			   host->res->end - host->res->start + 1);
+      out2:
+	clk_disable(host->clk);
+	host->plat_data->clk_flg = 0;
+	clk_put(host->clk);
+      out1:
+	gpio_sdhc_inactive(pdev->id);
+      out0:
+	mmc_free_host(mmc);
+	return ret;
+}
+
+static void sdhci_remove_slot(struct platform_device *pdev, int slot)
+{
+	struct sdhci_chip *chip;
+	struct mmc_host *mmc;
+	struct sdhci_host *host;
+
+	chip = dev_get_drvdata(&pdev->dev);
+	host = chip->hosts[slot];
+	mmc = host->mmc;
+
+	chip->hosts[slot] = NULL;
+
+	mmc_remove_host(mmc);
+
+	sdhci_reset(host, SDHCI_RESET_ALL);
+
+	if (host->detect_irq)
+		free_irq(host->detect_irq, host);
+	else {
+		if ((pdev->id >= 0) && (pdev->id < MXC_SDHCI_NUM))
+			mxc_fix_chips[pdev->id] = NULL;
+	}
+	free_irq(host->irq, host);
+	if (chip->quirks & SDHCI_QUIRK_EXTERNAL_DMA_MODE) {
+		host->flags &= ~SDHCI_USE_EXTERNAL_DMA;
+		mxc_dma_free(host->dma);
+	}
+
+	del_timer_sync(&host->timer);
+
+	tasklet_kill(&host->card_tasklet);
+	tasklet_kill(&host->finish_tasklet);
+
+	if (host->flags & SDHCI_USE_DMA)
+		kfree(adma_des_table);
+	release_mem_region(host->res->start,
+			   host->res->end - host->res->start + 1);
+	clk_disable(host->clk);
+	host->plat_data->clk_flg = 0;
+	clk_put(host->clk);
+	mmc_free_host(mmc);
+	gpio_sdhc_inactive(pdev->id);
+}
+
+static int sdhci_probe(struct platform_device *pdev)
+{
+	int ret = 0, i;
+	u8 slots = 1;
+	struct sdhci_chip *chip;
+
+	printk(KERN_INFO DRIVER_NAME ": MXC SDHCI Controller Driver. \n");
+	BUG_ON(pdev == NULL);
+
+	chip = kzalloc(sizeof(struct sdhci_chip) +
+		       sizeof(struct sdhci_host *) * slots, GFP_KERNEL);
+	if (!chip) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	/* Distinguish different platform */
+	if (machine_is_mx37_3ds()) {
+		mxc_quirks = SDHCI_QUIRK_EXTERNAL_DMA_MODE;
+	} else {
+		mxc_quirks = SDHCI_QUIRK_INTERNAL_ADVANCED_DMA |
+		    SDHCI_QUIRK_INTERNAL_SIMPLE_DMA;
+	}
+	chip->pdev = pdev;
+	chip->quirks = mxc_quirks;
+
+	if (debug_quirks)
+		chip->quirks = debug_quirks;
+
+	chip->num_slots = slots;
+	dev_set_drvdata(&pdev->dev, chip);
+
+	for (i = 0; i < slots; i++) {
+		ret = sdhci_probe_slot(pdev, i);
+		if (ret) {
+			for (i--; i >= 0; i--)
+				sdhci_remove_slot(pdev, i);
+			goto free;
+		}
+	}
+
+	return 0;
+
+      free:
+	dev_set_drvdata(&pdev->dev, NULL);
+	kfree(chip);
+
+      err:
+	return ret;
+}
+
+static int sdhci_remove(struct platform_device *pdev)
+{
+	int i;
+	struct sdhci_chip *chip;
+
+	chip = dev_get_drvdata(&pdev->dev);
+
+	if (chip) {
+		for (i = 0; i < chip->num_slots; i++)
+			sdhci_remove_slot(pdev, i);
+
+		dev_set_drvdata(&pdev->dev, NULL);
+
+		kfree(chip);
+	}
+
+	return 0;
+}
+
+static struct platform_driver sdhci_driver = {
+	.driver = {
+		   .name = DRIVER_NAME,
+		   },
+	.probe = sdhci_probe,
+	.remove = sdhci_remove,
+	.suspend = sdhci_suspend,
+	.resume = sdhci_resume,
+};
+
+/*****************************************************************************\
+ *                                                                           *
+ * Driver init/exit                                                          *
+ *                                                                           *
+\*****************************************************************************/
+
+static int __init sdhci_drv_init(void)
+{
+	printk(KERN_INFO DRIVER_NAME
+	       ": MXC Secure Digital Host Controller Interface driver\n");
+	return platform_driver_register(&sdhci_driver);
+}
+
+static void __exit sdhci_drv_exit(void)
+{
+	DBG("Exiting\n");
+
+	platform_driver_unregister(&sdhci_driver);
+}
+
+module_init(sdhci_drv_init);
+module_exit(sdhci_drv_exit);
+
+module_param(debug_quirks, uint, 0444);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC Secure Digital Host Controller Interface driver");
+MODULE_LICENSE("GPL");
+
+MODULE_PARM_DESC(debug_quirks, "Force certain quirks.");
diff --git a/drivers/mmc/host/mx_sdhci.h b/drivers/mmc/host/mx_sdhci.h
new file mode 100644
index 000000000000..509d444a6e81
--- /dev/null
+++ b/drivers/mmc/host/mx_sdhci.h
@@ -0,0 +1,275 @@
+/*
+ *  linux/drivers/mmc/host/mx_sdhci.h - Secure Digital Host
+ *  Controller Interface driver
+ *
+ *  Copyright (C) 2005-2007 Pierre Ossman, All Rights Reserved.
+ *  Copyright 2008-2009 Freescale Semiconductor, Inc. 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.
+ */
+
+/*
+ * Controller registers
+ */
+
+#define SDHCI_DMA_ADDRESS	0x00
+
+#define SDHCI_BLOCK_SIZE	0x04
+#define  SDHCI_MAKE_BLKSZ(dma, blksz) (((dma & 0x7) << 13) | (blksz & 0x1FFF))
+
+#define SDHCI_BLOCK_COUNT	0x04
+
+#define SDHCI_ARGUMENT		0x08
+
+#define SDHCI_TRANSFER_MODE	0x0C
+#define  SDHCI_TRNS_DMA		0x00000001
+#define  SDHCI_TRNS_BLK_CNT_EN	0x00000002
+#define  SDHCI_TRNS_ACMD12	0x00000004
+#define  SDHCI_TRNS_READ	0x00000010
+#define  SDHCI_TRNS_MULTI	0x00000020
+#define  SDHCI_TRNS_DPSEL	0x00200000
+#define  SDHCI_TRNS_MASK	0xFFFF0000
+
+#define SDHCI_COMMAND		0x0E
+#define  SDHCI_CMD_RESP_MASK	0x03
+#define  SDHCI_CMD_CRC		0x08
+#define  SDHCI_CMD_INDEX	0x10
+#define  SDHCI_CMD_DATA		0x20
+
+#define  SDHCI_CMD_RESP_NONE	0x00
+#define  SDHCI_CMD_RESP_LONG	0x01
+#define  SDHCI_CMD_RESP_SHORT	0x02
+#define  SDHCI_CMD_RESP_SHORT_BUSY 0x03
+
+#define SDHCI_MAKE_CMD(c, f) (((c & 0xff) << 8) | (f & 0xff)) << 16
+
+#define SDHCI_RESPONSE		0x10
+
+#define SDHCI_BUFFER		0x20
+
+#define SDHCI_PRESENT_STATE	0x24
+#define  SDHCI_CMD_INHIBIT	0x00000001
+#define  SDHCI_DATA_INHIBIT	0x00000002
+#define  SDHCI_DATA_ACTIVE 	0x00000004
+#define  SDHCI_DOING_WRITE	0x00000100
+#define  SDHCI_DOING_READ	0x00000200
+#define  SDHCI_SPACE_AVAILABLE	0x00000400
+#define  SDHCI_DATA_AVAILABLE	0x00000800
+#define  SDHCI_CARD_PRESENT	0x00010000
+#define  SDHCI_WRITE_PROTECT	0x00080000
+#define  SDHCI_DAT0_IDLE	0x01000000
+#define  SDHCI_CARD_INT_MASK	0x0E000000
+#define  SDHCI_CARD_INT_ID	0x0C000000
+
+#define SDHCI_HOST_CONTROL 	0x28
+#define  SDHCI_CTRL_LED		0x00000001
+#define  SDHCI_CTRL_4BITBUS	0x00000002
+#define  SDHCI_CTRL_8BITBUS	0x00000004
+#define  SDHCI_CTRL_HISPD	0x00000004
+#define  SDHCI_CTRL_DMA_MASK	0x18
+#define   SDHCI_CTRL_SDMA	0x00
+#define   SDHCI_CTRL_ADMA1	0x08
+#define   SDHCI_CTRL_ADMA32	0x10
+#define   SDHCI_CTRL_ADMA64	0x18
+#define  SDHCI_CTRL_D3CD 	0x00000008
+#define  SDHCI_CTRL_ADMA 	0x00000100
+/* wake up control */
+#define  SDHCI_CTRL_WECINS 	0x04000000
+
+#define SDHCI_POWER_CONTROL	0x29
+#define  SDHCI_POWER_ON		0x01
+#define  SDHCI_POWER_180	0x0A
+#define  SDHCI_POWER_300	0x0C
+#define  SDHCI_POWER_330	0x0E
+
+#define SDHCI_BLOCK_GAP_CONTROL	0x2A
+
+#define SDHCI_WAKE_UP_CONTROL	0x2B
+
+#define SDHCI_CLOCK_CONTROL	0x2C
+#define  SDHCI_DIVIDER_SHIFT	8
+#define  SDHCI_CLOCK_SD_EN	0x00000008
+#define  SDHCI_CLOCK_PER_EN	0x00000004
+#define  SDHCI_CLOCK_HLK_EN	0x00000002
+#define  SDHCI_CLOCK_IPG_EN	0x00000001
+#define  SDHCI_CLOCK_MASK 	0x0000FFFF
+
+#define SDHCI_TIMEOUT_CONTROL	0x2E
+
+#define SDHCI_SOFTWARE_RESET	0x2F
+#define  SDHCI_RESET_ALL	0x01
+#define  SDHCI_RESET_CMD	0x02
+#define  SDHCI_RESET_DATA	0x04
+
+#define SDHCI_INT_STATUS	0x30
+#define SDHCI_INT_ENABLE	0x34
+#define SDHCI_SIGNAL_ENABLE	0x38
+#define  SDHCI_INT_RESPONSE	0x00000001
+#define  SDHCI_INT_DATA_END	0x00000002
+#define  SDHCI_INT_DMA_END	0x00000008
+#define  SDHCI_INT_SPACE_AVAIL	0x00000010
+#define  SDHCI_INT_DATA_AVAIL	0x00000020
+#define  SDHCI_INT_CARD_INSERT	0x00000040
+#define  SDHCI_INT_CARD_REMOVE	0x00000080
+#define  SDHCI_INT_CARD_INT	0x00000100
+#define  SDHCI_INT_ERROR	0x00008000
+#define  SDHCI_INT_TIMEOUT	0x00010000
+#define  SDHCI_INT_CRC		0x00020000
+#define  SDHCI_INT_END_BIT	0x00040000
+#define  SDHCI_INT_INDEX	0x00080000
+#define  SDHCI_INT_DATA_TIMEOUT	0x00100000
+#define  SDHCI_INT_DATA_CRC	0x00200000
+#define  SDHCI_INT_DATA_END_BIT	0x00400000
+#define  SDHCI_INT_BUS_POWER	0x00800000
+#define  SDHCI_INT_ACMD12ERR	0x01000000
+#define  SDHCI_INT_ADMA_ERROR	0x10000000
+
+#define  SDHCI_INT_NORMAL_MASK	0x00007FFF
+#define  SDHCI_INT_ERROR_MASK	0xFFFF8000
+
+#define  SDHCI_INT_CMD_MASK	(SDHCI_INT_RESPONSE | SDHCI_INT_TIMEOUT | \
+		SDHCI_INT_CRC | SDHCI_INT_END_BIT | SDHCI_INT_INDEX)
+#define  SDHCI_INT_DATA_MASK	(SDHCI_INT_DATA_END | SDHCI_INT_DMA_END | \
+		SDHCI_INT_DATA_AVAIL | SDHCI_INT_SPACE_AVAIL | \
+		SDHCI_INT_DATA_TIMEOUT | SDHCI_INT_DATA_CRC | \
+		SDHCI_INT_DATA_END_BIT | SDHCI_INT_ADMA_ERROR)
+#define  SDHCI_INT_DATA_RE_MASK	(SDHCI_INT_DMA_END | \
+		SDHCI_INT_DATA_AVAIL | SDHCI_INT_SPACE_AVAIL)
+
+#define SDHCI_ACMD12_ERR	0x3C
+
+/* 3E-3F reserved */
+
+#define SDHCI_CAPABILITIES	0x40
+#define  SDHCI_TIMEOUT_CLK_MASK	0x0000003F
+#define  SDHCI_TIMEOUT_CLK_SHIFT 0
+#define  SDHCI_TIMEOUT_CLK_UNIT	0x00000080
+#define  SDHCI_CLOCK_BASE_MASK	0x00003F00
+#define  SDHCI_CLOCK_BASE_SHIFT	8
+#define  SDHCI_MAX_BLOCK_MASK	0x00030000
+#define  SDHCI_MAX_BLOCK_SHIFT  16
+#define  SDHCI_CAN_DO_ADMA2	0x00080000
+#define  SDHCI_CAN_DO_ADMA1	0x00100000
+#define  SDHCI_CAN_DO_HISPD	0x00200000
+#define  SDHCI_CAN_DO_DMA	0x00400000
+#define  SDHCI_CAN_VDD_330	0x01000000
+#define  SDHCI_CAN_VDD_300	0x02000000
+#define  SDHCI_CAN_VDD_180	0x04000000
+#define  SDHCI_CAN_64BIT	0x10000000
+
+/* 44-47 reserved for more caps */
+#define SDHCI_WML 		0x44
+#define  SDHCI_WML_4_WORDS 	0x00040004
+#define  SDHCI_WML_16_WORDS 	0x00100010
+#define  SDHCI_WML_64_WORDS 	0x00400040
+#define  SDHCI_WML_128_WORDS 	0x00800080
+
+#define SDHCI_MAX_CURRENT	0x48
+
+/* 4C-4F reserved for more max current */
+
+#define SDHCI_SET_ACMD12_ERROR	0x50
+#define SDHCI_SET_INT_ERROR	0x52
+
+#define SDHCI_ADMA_ERROR	0x54
+
+/* 55-57 reserved */
+
+#define SDHCI_ADMA_ADDRESS	0x58
+
+/* 60-FB reserved */
+
+/* ADMA Addr Descriptor Attribute Filed */
+enum {
+	FSL_ADMA_DES_ATTR_VALID = 0x01,
+	FSL_ADMA_DES_ATTR_END = 0x02,
+	FSL_ADMA_DES_ATTR_INT = 0x04,
+	FSL_ADMA_DES_ATTR_SET = 0x10,
+	FSL_ADMA_DES_ATTR_TRAN = 0x20,
+	FSL_ADMA_DES_ATTR_LINK = 0x30,
+};
+
+#define SDHCI_HOST_VERSION	0xFC
+#define  SDHCI_VENDOR_VER_MASK	0xFF00
+#define  SDHCI_VENDOR_VER_SHIFT	8
+#define  SDHCI_SPEC_VER_MASK	0x00FF
+#define  SDHCI_SPEC_VER_SHIFT	0
+#define   SDHCI_SPEC_100	0
+#define   SDHCI_SPEC_200	1
+#define   ESDHC_VENDOR_V22 	0x12
+
+struct sdhci_chip;
+
+struct sdhci_host {
+	struct sdhci_chip *chip;
+	struct mmc_host *mmc;	/* MMC structure */
+
+#ifdef CONFIG_LEDS_CLASS
+	struct led_classdev led;	/* LED control */
+#endif
+
+	spinlock_t lock;	/* Mutex */
+
+	int init_flag;		/* Host has been initialized */
+	int flags;		/* Host attributes */
+#define SDHCI_USE_DMA		(1<<0)	/* Host is DMA capable */
+#define SDHCI_REQ_USE_DMA	(1<<1)	/* Use DMA for this req. */
+#define SDHCI_USE_EXTERNAL_DMA	(1<<2)	/* Use the External DMA */
+#define SDHCI_CD_PRESENT 	(1<<8)	/* CD present */
+#define SDHCI_WP_ENABLED	(1<<9)	/* Write protect */
+
+	unsigned int max_clk;	/* Max possible freq (MHz) */
+	unsigned int min_clk;	/* Min possible freq (MHz) */
+	unsigned int timeout_clk;	/* Timeout freq (KHz) */
+
+	unsigned int clock;	/* Current clock (MHz) */
+	unsigned short power;	/* Current voltage */
+	struct regulator *regulator_mmc;	/*! Regulator */
+
+	struct mmc_request *mrq;	/* Current request */
+	struct mmc_command *cmd;	/* Current command */
+	struct mmc_data *data;	/* Current data request */
+	unsigned int data_early:1;	/* Data finished before cmd */
+
+	unsigned int id;	/* Id for SD/MMC block */
+	int mode;		/* SD/MMC mode */
+	int dma;		/* DMA channel number. */
+	unsigned int dma_size;	/* Number of Bytes in DMA */
+	unsigned int dma_len;	/* Length of the s-g list */
+	unsigned int dma_dir;	/* DMA transfer direction */
+
+	struct scatterlist *cur_sg;	/* We're working on this */
+	int num_sg;		/* Entries left */
+	int offset;		/* Offset into current sg */
+	int remain;		/* Bytes left in current */
+
+	struct resource *res;	/* IO map memory */
+	int irq;		/* Device IRQ */
+	int detect_irq;		/* Card Detect IRQ number. */
+	int sdio_enable;	/* sdio interrupt enable number. */
+	struct clk *clk;	/* Clock id */
+	int bar;		/* PCI BAR index */
+	unsigned long addr;	/* Bus address */
+	void __iomem *ioaddr;	/* Mapped address */
+
+	struct tasklet_struct card_tasklet;	/* Tasklet structures */
+	struct tasklet_struct finish_tasklet;
+	struct work_struct cd_wq;	/* card detection work queue */
+	/* Platform specific data */
+	struct mxc_mmc_platform_data *plat_data;
+
+	struct timer_list timer;	/* Timer for timeouts */
+	struct timer_list cd_timer;	/* Timer for cd */
+};
+
+struct sdhci_chip {
+	struct platform_device *pdev;
+
+	unsigned long quirks;
+
+	int num_slots;		/* Slots on controller */
+	struct sdhci_host *hosts[0];	/* Pointers to hosts */
+};
diff --git a/drivers/mmc/host/mxc_mmc.c b/drivers/mmc/host/mxc_mmc.c
new file mode 100644
index 000000000000..064b3bffb85f
--- /dev/null
+++ b/drivers/mmc/host/mxc_mmc.c
@@ -0,0 +1,1530 @@
+/*
+ *  linux/drivers/mmc/host/mxc_mmc.c - Freescale MXC/i.MX MMC driver
+ *
+ *  based on imxmmc.c
+ *  Copyright (C) 2004 Sascha Hauer, Pengutronix <sascha@saschahauer.de>
+ *
+ *  derived from pxamci.c by Russell King
+ *
+ * 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.
+ *
+ */
+
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_mmc.c
+ *
+ * @brief Driver for the Freescale Semiconductor MXC SDHC modules.
+ *
+ * This driver code is based on imxmmc.c, by Sascha Hauer,
+ * Pengutronix <sascha@saschahauer.de>. This driver supports both Secure Digital
+ * Host Controller modules (SDHC1 and SDHC2) of MXC. SDHC is also referred as
+ * MMC/SD controller. This code is not tested for SD cards.
+ *
+ * @ingroup MMC_SD
+ */
+
+/*
+ * Include Files
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/blkdev.h>
+#include <linux/dma-mapping.h>
+#include <linux/mmc/mmc.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sd.h>
+#include <linux/delay.h>
+#include <linux/timer.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+
+#include <asm/dma.h>
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <asm/sizes.h>
+#include <asm/mach-types.h>
+#include <asm/mach/irq.h>
+#include <mach/mmc.h>
+
+#include "mxc_mmc.h"
+
+#define RSP_TYPE(x)	((x) & ~(MMC_RSP_BUSY|MMC_RSP_OPCODE))
+
+/*
+ * This define is used to test the driver without using DMA
+ */
+#define MXC_MMC_DMA_ENABLE
+
+/*!
+ * Maxumum length of s/g list, only length of 1 is currently supported
+ */
+#define NR_SG   1
+
+#ifdef CONFIG_MMC_DEBUG
+static void dump_cmd(struct mmc_command *cmd)
+{
+	printk(KERN_INFO "%s: CMD: opcode: %d ", DRIVER_NAME, cmd->opcode);
+	printk(KERN_INFO "arg: 0x%08x ", cmd->arg);
+	printk(KERN_INFO "flags: 0x%08x\n", cmd->flags);
+}
+
+static void dump_status(const char *func, int sts)
+{
+	unsigned int bitset;
+	printk(KERN_INFO "%s:status: ", func);
+	while (sts) {
+		/* Find the next bit set */
+		bitset = sts & ~(sts - 1);
+		switch (bitset) {
+		case STATUS_CARD_INSERTION:
+			printk(KERN_INFO "CARD_INSERTION|");
+			break;
+		case STATUS_CARD_REMOVAL:
+			printk(KERN_INFO "CARD_REMOVAL |");
+			break;
+		case STATUS_YBUF_EMPTY:
+			printk(KERN_INFO "YBUF_EMPTY |");
+			break;
+		case STATUS_XBUF_EMPTY:
+			printk(KERN_INFO "XBUF_EMPTY |");
+			break;
+		case STATUS_YBUF_FULL:
+			printk(KERN_INFO "YBUF_FULL |");
+			break;
+		case STATUS_XBUF_FULL:
+			printk(KERN_INFO "XBUF_FULL |");
+			break;
+		case STATUS_BUF_UND_RUN:
+			printk(KERN_INFO "BUF_UND_RUN |");
+			break;
+		case STATUS_BUF_OVFL:
+			printk(KERN_INFO "BUF_OVFL |");
+			break;
+		case STATUS_READ_OP_DONE:
+			printk(KERN_INFO "READ_OP_DONE |");
+			break;
+		case STATUS_WR_CRC_ERROR_CODE_MASK:
+			printk(KERN_INFO "WR_CRC_ERROR_CODE |");
+			break;
+		case STATUS_READ_CRC_ERR:
+			printk(KERN_INFO "READ_CRC_ERR |");
+			break;
+		case STATUS_WRITE_CRC_ERR:
+			printk(KERN_INFO "WRITE_CRC_ERR |");
+			break;
+		case STATUS_SDIO_INT_ACTIVE:
+			printk(KERN_INFO "SDIO_INT_ACTIVE |");
+			break;
+		case STATUS_END_CMD_RESP:
+			printk(KERN_INFO "END_CMD_RESP |");
+			break;
+		case STATUS_WRITE_OP_DONE:
+			printk(KERN_INFO "WRITE_OP_DONE |");
+			break;
+		case STATUS_CARD_BUS_CLK_RUN:
+			printk(KERN_INFO "CARD_BUS_CLK_RUN |");
+			break;
+		case STATUS_BUF_READ_RDY:
+			printk(KERN_INFO "BUF_READ_RDY |");
+			break;
+		case STATUS_BUF_WRITE_RDY:
+			printk(KERN_INFO "BUF_WRITE_RDY |");
+			break;
+		case STATUS_RESP_CRC_ERR:
+			printk(KERN_INFO "RESP_CRC_ERR |");
+			break;
+		case STATUS_TIME_OUT_RESP:
+			printk(KERN_INFO "TIME_OUT_RESP |");
+			break;
+		case STATUS_TIME_OUT_READ:
+			printk(KERN_INFO "TIME_OUT_READ |");
+			break;
+		default:
+			printk(KERN_INFO "Invalid Status Register value0x%x\n",
+			       bitset);
+			break;
+		}
+		sts &= ~bitset;
+	}
+	printk(KERN_INFO "\n");
+}
+#endif
+
+/*!
+ * This structure is a way for the low level driver to define their own
+ * \b mmc_host structure. This structure includes the core \b mmc_host
+ * structure that is provided by Linux MMC/SD Bus protocol driver as an
+ * element and has other elements that are specifically required by this
+ * low-level driver.
+ */
+struct mxcmci_host {
+	/*!
+	 * The mmc structure holds all the information about the device
+	 * structure, current SDHC io bus settings, the current OCR setting,
+	 * devices attached to this host, and so on.
+	 */
+	struct mmc_host *mmc;
+
+	/*!
+	 * This variable is used for locking the host data structure from
+	 * multiple access.
+	 */
+	spinlock_t lock;
+
+	/*!
+	 * Resource structure, which will maintain base addresses and IRQs.
+	 */
+	struct resource *res;
+
+	/*!
+	 * Base address of SDHC, used in readl and writel.
+	 */
+	void *base;
+
+	/*!
+	 * SDHC IRQ number.
+	 */
+	int irq;
+
+	/*!
+	 * Card Detect IRQ number.
+	 */
+	int detect_irq;
+
+	/*!
+	 * Clock id to hold ipg_perclk.
+	 */
+	struct clk *clk;
+	/*!
+	 * MMC mode.
+	 */
+	int mode;
+
+	/*!
+	 * DMA channel number.
+	 */
+	int dma;
+
+	/*!
+	 * Pointer to hold MMC/SD request.
+	 */
+	struct mmc_request *req;
+
+	/*!
+	 * Pointer to hold MMC/SD command.
+	 */
+	struct mmc_command *cmd;
+
+	/*!
+	 * Pointer to hold MMC/SD data.
+	 */
+	struct mmc_data *data;
+
+	/*!
+	 * Holds the number of bytes to transfer using DMA.
+	 */
+	unsigned int dma_size;
+
+	/*!
+	 * Value to store in Command and Data Control Register
+	 * - currently unused
+	 */
+	unsigned int cmdat;
+
+	/*!
+	 * Regulator
+	 */
+	struct regulator *regulator_mmc;
+
+	/*!
+	 * Current vdd settting
+	 */
+	int current_vdd;
+
+	/*!
+	 * Power mode - currently unused
+	 */
+	unsigned int power_mode;
+
+	/*!
+	 * DMA address for scatter-gather transfers
+	 */
+	dma_addr_t sg_dma;
+
+	/*!
+	 * Length of the scatter-gather list
+	 */
+	unsigned int dma_len;
+
+	/*!
+	 * Holds the direction of data transfer.
+	 */
+	unsigned int dma_dir;
+
+	/*!
+	 * Id for MMC block.
+	 */
+	unsigned int id;
+
+	/*!
+	 * Note whether this driver has been suspended.
+	 */
+	unsigned int mxc_mmc_suspend_flag;
+
+	/*!
+	 * sdio_irq enable/disable ref count
+	 */
+	int sdio_irq_cnt;
+
+	/*!
+	 * Platform specific data
+	 */
+	struct mxc_mmc_platform_data *plat_data;
+};
+
+extern void gpio_sdhc_active(int module);
+extern void gpio_sdhc_inactive(int module);
+
+#ifdef MXC_MMC_DMA_ENABLE
+static void mxcmci_dma_irq(void *devid, int error, unsigned int cnt);
+#endif
+static int mxcmci_data_done(struct mxcmci_host *host, unsigned int stat);
+
+/* Wait count to start the clock */
+#define CMD_WAIT_CNT 100
+
+#define MAX_HOST 10
+static struct mmc_host *hosts[MAX_HOST];
+
+void mxc_mmc_force_detect(int id)
+{
+	if (id < MAX_HOST)
+		mmc_detect_change(hosts[id], msecs_to_jiffies(100));
+}
+
+EXPORT_SYMBOL(mxc_mmc_force_detect);
+
+/*!
+  This function sets the SDHC register to stop the clock and waits for the
+ * clock stop indication.
+ */
+static void mxcmci_stop_clock(struct mxcmci_host *host, bool wait)
+{
+	int wait_cnt = 0;
+	while (1) {
+		__raw_writel(STR_STP_CLK_STOP_CLK,
+			     host->base + MMC_STR_STP_CLK);
+
+		if (!wait)
+			break;
+
+		wait_cnt = CMD_WAIT_CNT;
+		while (wait_cnt--) {
+			if (!(__raw_readl(host->base + MMC_STATUS) &
+			      STATUS_CARD_BUS_CLK_RUN))
+				break;
+		}
+
+		if (!(__raw_readl(host->base + MMC_STATUS) &
+		      STATUS_CARD_BUS_CLK_RUN))
+			break;
+	}
+}
+
+/*!
+ * This function sets the SDHC register to start the clock and waits for the
+ * clock start indication. When the clock starts SDHC module starts processing
+ * the command in CMD Register with arguments in ARG Register.
+ *
+ * @param host Pointer to MMC/SD host structure
+ * @param wait Boolean value to indicate whether to wait for the clock to start or come out instantly
+ */
+static void mxcmci_start_clock(struct mxcmci_host *host, bool wait)
+{
+	int wait_cnt;
+
+#ifdef CONFIG_MMC_DEBUG
+	dump_status(__FUNCTION__, __raw_readl(host->base + MMC_STATUS));
+#endif
+
+	while (1) {
+		__raw_writel(STR_STP_CLK_START_CLK,
+			     host->base + MMC_STR_STP_CLK);
+		if (!wait)
+			break;
+
+		wait_cnt = CMD_WAIT_CNT;
+		while (wait_cnt--) {
+			if (__raw_readl(host->base + MMC_STATUS) &
+			    STATUS_CARD_BUS_CLK_RUN) {
+				break;
+			}
+		}
+
+		if (__raw_readl(host->base + MMC_STATUS) &
+		    STATUS_CARD_BUS_CLK_RUN) {
+			break;
+		}
+	}
+#ifdef CONFIG_MMC_DEBUG
+	dump_status(__FUNCTION__, __raw_readl(host->base + MMC_STATUS));
+#endif
+	pr_debug("%s:CLK_RATE: 0x%08x\n", DRIVER_NAME,
+		 __raw_readl(host->base + MMC_CLK_RATE));
+}
+
+/*!
+ * This function resets the SDHC host.
+ *
+ * @param host  Pointer to MMC/SD  host structure
+ */
+static void mxcmci_softreset(struct mxcmci_host *host)
+{
+	/* reset sequence */
+	__raw_writel(0x8, host->base + MMC_STR_STP_CLK);
+	__raw_writel(0x9, host->base + MMC_STR_STP_CLK);
+	__raw_writel(0x1, host->base + MMC_STR_STP_CLK);
+	__raw_writel(0x1, host->base + MMC_STR_STP_CLK);
+	__raw_writel(0x1, host->base + MMC_STR_STP_CLK);
+	__raw_writel(0x1, host->base + MMC_STR_STP_CLK);
+	__raw_writel(0x1, host->base + MMC_STR_STP_CLK);
+	__raw_writel(0x1, host->base + MMC_STR_STP_CLK);
+	__raw_writel(0x1, host->base + MMC_STR_STP_CLK);
+	__raw_writel(0x1, host->base + MMC_STR_STP_CLK);
+	__raw_writel(0x3f, host->base + MMC_CLK_RATE);
+
+	__raw_writel(0xff, host->base + MMC_RES_TO);
+	__raw_writel(512, host->base + MMC_BLK_LEN);
+	__raw_writel(1, host->base + MMC_NOB);
+}
+
+/*!
+ * This function is called to setup SDHC register for data transfer.
+ * The function allocates DMA buffers, configures the DMA channel.
+ * Start the DMA channel to transfer data. When DMA is not enabled this
+ * function set ups only Number of Block and Block Length registers.
+ *
+ * @param host  Pointer to MMC/SD host structure
+ * @param data  Pointer to MMC/SD data structure
+ */
+static void mxcmci_setup_data(struct mxcmci_host *host, struct mmc_data *data)
+{
+	unsigned int nob = data->blocks;
+
+	if (data->flags & MMC_DATA_STREAM) {
+		nob = 0xffff;
+	}
+
+	host->data = data;
+
+	__raw_writel(nob, host->base + MMC_NOB);
+	__raw_writel(data->blksz, host->base + MMC_BLK_LEN);
+
+	host->dma_size = data->blocks * data->blksz;
+	pr_debug("%s:Request bytes to transfer:%d\n", DRIVER_NAME,
+		 host->dma_size);
+
+#ifdef MXC_MMC_DMA_ENABLE
+	if (host->dma_size <= (16 << host->mmc->ios.bus_width)) {
+		return;
+	}
+
+	if (data->blksz & 0x3) {
+		printk(KERN_ERR
+		       "mxc_mci: block size not multiple of 4 bytes\n");
+	}
+
+	if (data->flags & MMC_DATA_READ) {
+		host->dma_dir = DMA_FROM_DEVICE;
+	} else {
+		host->dma_dir = DMA_TO_DEVICE;
+	}
+	host->dma_len = dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
+				   host->dma_dir);
+
+	if (data->flags & MMC_DATA_READ) {
+		mxc_dma_sg_config(host->dma, data->sg, data->sg_len,
+				  host->dma_size, MXC_DMA_MODE_READ);
+	} else {
+		mxc_dma_sg_config(host->dma, data->sg, data->sg_len,
+				  host->dma_size, MXC_DMA_MODE_WRITE);
+	}
+#endif
+}
+
+/*!
+ * This function is called by \b mxcmci_request() function to setup the SDHC
+ * register to issue command. This function disables the card insertion and
+ * removal detection interrupt.
+ *
+ * @param host  Pointer to MMC/SD host structure
+ * @param cmd   Pointer to MMC/SD command structure
+ * @param cmdat Value to store in Command and Data Control Register
+ */
+static void mxcmci_start_cmd(struct mxcmci_host *host, struct mmc_command *cmd,
+			     unsigned int cmdat)
+{
+	WARN_ON(host->cmd != NULL);
+	host->cmd = cmd;
+
+	switch (RSP_TYPE(mmc_resp_type(cmd))) {
+	case RSP_TYPE(MMC_RSP_R1):	/* r1, r1b, r6 */
+		cmdat |= CMD_DAT_CONT_RESPONSE_FORMAT_R1;
+		break;
+	case RSP_TYPE(MMC_RSP_R3):
+		cmdat |= CMD_DAT_CONT_RESPONSE_FORMAT_R3;
+		break;
+	case RSP_TYPE(MMC_RSP_R2):
+		cmdat |= CMD_DAT_CONT_RESPONSE_FORMAT_R2;
+		break;
+	default:
+		/* No Response required */
+		break;
+	}
+
+	if (cmd->opcode == MMC_GO_IDLE_STATE) {
+		cmdat |= CMD_DAT_CONT_INIT;	/* This command needs init */
+	}
+
+	if (host->mmc->ios.bus_width == MMC_BUS_WIDTH_4) {
+		cmdat |= CMD_DAT_CONT_BUS_WIDTH_4;
+	}
+
+	__raw_writel(cmd->opcode, host->base + MMC_CMD);
+	__raw_writel(cmd->arg, host->base + MMC_ARG);
+
+	__raw_writel(cmdat, host->base + MMC_CMD_DAT_CONT);
+
+	if (!(__raw_readl(host->base + MMC_STATUS) & STATUS_CARD_BUS_CLK_RUN))
+		mxcmci_start_clock(host, true);
+}
+
+/*!
+ * This function is called to complete the command request.
+ * This function enables insertion or removal interrupt.
+ *
+ * @param host Pointer to MMC/SD host structure
+ * @param req  Pointer to MMC/SD command request structure
+ */
+static void mxcmci_finish_request(struct mxcmci_host *host,
+				  struct mmc_request *req)
+{
+
+	host->req = NULL;
+	host->cmd = NULL;
+	host->data = NULL;
+
+	mmc_request_done(host->mmc, req);
+}
+
+/*!
+ * This function is called when the requested command is completed.
+ * This function reads the response from the card and data if the command is for
+ * data transfer. This function checks for CRC error in response FIFO or
+ * data FIFO.
+ *
+ * @param host  Pointer to MMC/SD host structure
+ * @param stat  Content of SDHC Status Register
+ *
+ * @return This function returns 0 if there is no pending command, otherwise 1
+ * always.
+ */
+static int mxcmci_cmd_done(struct mxcmci_host *host, unsigned int stat)
+{
+	struct mmc_command *cmd = host->cmd;
+	struct mmc_data *data = host->data;
+	int i;
+	u32 a, b, c;
+	u32 temp_data;
+	unsigned int status;
+	unsigned long *buf;
+	u8 *buf8;
+	int no_of_bytes;
+	int no_of_words;
+
+	if (!cmd) {
+		/* There is no command for completion */
+		return 0;
+	}
+
+	/* As this function finishes the command, initialize cmd to NULL */
+	host->cmd = NULL;
+
+	/* check for Time out errors */
+	if (stat & STATUS_TIME_OUT_RESP) {
+		__raw_writel(STATUS_TIME_OUT_RESP, host->base + MMC_STATUS);
+		pr_debug("%s: CMD %d TIMEOUT\n", DRIVER_NAME, cmd->opcode);
+		cmd->error = -ETIMEDOUT;
+		/*
+		 * Reinitialized the controller to clear the unknown
+		 * error state.
+		 */
+		mxcmci_softreset(host);
+		__raw_writel(READ_TO_VALUE, host->base + MMC_READ_TO);
+		__raw_writel(INT_CNTR_END_CMD_RES, host->base + MMC_INT_CNTR);
+	} else if (stat & STATUS_RESP_CRC_ERR && cmd->flags & MMC_RSP_CRC) {
+		__raw_writel(STATUS_RESP_CRC_ERR, host->base + MMC_STATUS);
+		printk(KERN_ERR "%s: cmd %d CRC error\n", DRIVER_NAME,
+		       cmd->opcode);
+		cmd->error = -EILSEQ;
+		/*
+		 * Reinitialized the controller to clear the unknown
+		 * error state.
+		 */
+		mxcmci_softreset(host);
+		__raw_writel(READ_TO_VALUE, host->base + MMC_READ_TO);
+		__raw_writel(INT_CNTR_END_CMD_RES, host->base + MMC_INT_CNTR);
+	}
+
+	/* Read response from the card */
+	switch (RSP_TYPE(mmc_resp_type(cmd))) {
+	case RSP_TYPE(MMC_RSP_R1):	/* r1, r1b, r6 */
+		a = __raw_readl(host->base + MMC_RES_FIFO) & 0xffff;
+		b = __raw_readl(host->base + MMC_RES_FIFO) & 0xffff;
+		c = __raw_readl(host->base + MMC_RES_FIFO) & 0xffff;
+		cmd->resp[0] = a << 24 | b << 8 | c >> 8;
+		break;
+	case RSP_TYPE(MMC_RSP_R3):	/* r3, r4 */
+		a = __raw_readl(host->base + MMC_RES_FIFO) & 0xffff;
+		b = __raw_readl(host->base + MMC_RES_FIFO) & 0xffff;
+		c = __raw_readl(host->base + MMC_RES_FIFO) & 0xffff;
+		cmd->resp[0] = a << 24 | b << 8 | c >> 8;
+		break;
+	case RSP_TYPE(MMC_RSP_R2):
+		for (i = 0; i < 4; i++) {
+			a = __raw_readl(host->base + MMC_RES_FIFO) & 0xffff;
+			b = __raw_readl(host->base + MMC_RES_FIFO) & 0xffff;
+			cmd->resp[i] = a << 16 | b;
+		}
+		break;
+	default:
+		break;
+	}
+
+	pr_debug("%s: 0x%08x, 0x%08x, 0x%08x, 0x%08x\n", DRIVER_NAME,
+		 cmd->resp[0], cmd->resp[1], cmd->resp[2], cmd->resp[3]);
+
+	if (!host->data || cmd->error) {
+		/* complete the command */
+		mxcmci_finish_request(host, host->req);
+		return 1;
+	}
+
+	/* The command has a data transfer */
+#ifdef MXC_MMC_DMA_ENABLE
+	/* Use DMA if transfer size is greater than fifo size */
+	if (host->dma_size > (16 << host->mmc->ios.bus_width)) {
+		mxc_dma_enable(host->dma);
+		return 1;
+	}
+#endif
+	/* Use PIO tranfer of data */
+	buf = (unsigned long *)sg_virt(data->sg);
+	buf8 = (u8 *) buf;
+
+	/* calculate the number of bytes requested for transfer */
+	no_of_bytes = data->blocks * data->blksz;
+	no_of_words = (no_of_bytes + 3) / 4;
+	pr_debug("no_of_words=%d\n", no_of_words);
+
+	if (data->flags & MMC_DATA_READ) {
+		for (i = 0; i < no_of_words; i++) {
+			/* wait for buffers to be ready for read */
+			while (!(__raw_readl(host->base + MMC_STATUS) &
+				 (STATUS_BUF_READ_RDY | STATUS_READ_OP_DONE))) ;
+
+			pr_debug("status is 0x%x\n",
+				 __raw_readl(host->base + MMC_STATUS));
+			/* read 32 bit data */
+			temp_data = __raw_readl(host->base + MMC_BUFFER_ACCESS);
+			if (SD_APP_SEND_SCR == cmd->opcode) {
+				pr_debug("CMD51 read out 0x%x\n", temp_data);
+				if (temp_data == 0xFFFFFFFF)
+					temp_data = 0;
+			}
+			if (no_of_bytes >= 4) {
+				*buf++ = temp_data;
+				no_of_bytes -= 4;
+			} else {
+				do {
+					*buf8++ = temp_data;
+					temp_data = temp_data >> 8;
+				} while (--no_of_bytes);
+			}
+		}
+
+		/* wait for read operation completion bit */
+		while (!(__raw_readl(host->base + MMC_STATUS) &
+			 STATUS_READ_OP_DONE)) ;
+
+		/* check for time out and CRC errors */
+		status = __raw_readl(host->base + MMC_STATUS);
+		if (status & STATUS_TIME_OUT_READ) {
+			printk(KERN_ERR "%s: Read time out occurred\n",
+			       DRIVER_NAME);
+			data->error = -ETIMEDOUT;
+			__raw_writel(STATUS_TIME_OUT_READ,
+				     host->base + MMC_STATUS);
+			/*
+			 * Reinitialized the controller to clear the unknown
+			 * error state.
+			 */
+			mxcmci_softreset(host);
+			__raw_writel(READ_TO_VALUE, host->base + MMC_READ_TO);
+			__raw_writel(INT_CNTR_END_CMD_RES,
+				     host->base + MMC_INT_CNTR);
+		} else if (status & STATUS_READ_CRC_ERR) {
+			printk(KERN_ERR "%s: Read CRC error occurred\n",
+			       DRIVER_NAME);
+			if (SD_APP_SEND_SCR != cmd->opcode)
+				data->error = -EILSEQ;
+			__raw_writel(STATUS_READ_CRC_ERR,
+				     host->base + MMC_STATUS);
+			/*
+			 * Reinitialized the controller to clear the unknown
+			 * error state.
+			 */
+			mxcmci_softreset(host);
+			__raw_writel(READ_TO_VALUE, host->base + MMC_READ_TO);
+			__raw_writel(INT_CNTR_END_CMD_RES,
+				     host->base + MMC_INT_CNTR);
+		}
+		__raw_writel(STATUS_READ_OP_DONE, host->base + MMC_STATUS);
+
+		pr_debug("%s: Read %u words\n", DRIVER_NAME, i);
+	} else {
+		for (i = 0; i < no_of_words; i++) {
+
+			/* wait for buffers to be ready for write */
+			while (!(__raw_readl(host->base + MMC_STATUS) &
+				 STATUS_BUF_WRITE_RDY)) ;
+
+			/* write 32 bit data */
+			__raw_writel(*buf++, host->base + MMC_BUFFER_ACCESS);
+			if (__raw_readl(host->base + MMC_STATUS) &
+			    STATUS_WRITE_OP_DONE) {
+				break;
+			}
+		}
+
+		/* wait for write operation completion bit */
+		while (!(__raw_readl(host->base + MMC_STATUS) &
+			 STATUS_WRITE_OP_DONE)) ;
+
+		/* check for CRC errors */
+		status = __raw_readl(host->base + MMC_STATUS);
+		if (status & STATUS_WRITE_CRC_ERR) {
+			printk(KERN_ERR "%s: Write CRC error occurred\n",
+			       DRIVER_NAME);
+			data->error = -EILSEQ;
+			__raw_writel(STATUS_WRITE_CRC_ERR,
+				     host->base + MMC_STATUS);
+		}
+		__raw_writel(STATUS_WRITE_OP_DONE, host->base + MMC_STATUS);
+		pr_debug("%s: Written %u words\n", DRIVER_NAME, i);
+	}
+
+	/* complete the data transfer request */
+	mxcmci_data_done(host, status);
+
+	return 1;
+}
+
+/*!
+ * This function is called when the data transfer is completed either by DMA
+ * or by core. This function is called to clean up the DMA buffer and to send
+ * STOP transmission command for commands to transfer data. This function
+ * completes request issued by the MMC/SD core driver.
+ *
+ * @param host   pointer to MMC/SD host structure.
+ * @param stat   content of SDHC Status Register
+ *
+ * @return This function returns 0 if no data transfer otherwise return 1
+ * always.
+ */
+static int mxcmci_data_done(struct mxcmci_host *host, unsigned int stat)
+{
+	struct mmc_data *data = host->data;
+
+	if (!data) {
+		return 0;
+	}
+#ifdef MXC_MMC_DMA_ENABLE
+	if (host->dma_size > (16 << host->mmc->ios.bus_width)) {
+		dma_unmap_sg(mmc_dev(host->mmc), data->sg, host->dma_len,
+			     host->dma_dir);
+	}
+#endif
+	if (__raw_readl(host->base + MMC_STATUS) & STATUS_ERR_MASK) {
+		printk(KERN_ERR "%s: request failed. status: 0x%08x\n",
+		       DRIVER_NAME, __raw_readl(host->base + MMC_STATUS));
+	}
+
+	host->data = NULL;
+	data->bytes_xfered = host->dma_size;
+
+	if (host->req->stop && !(data->error)) {
+		mxcmci_stop_clock(host, true);
+		mxcmci_start_cmd(host, host->req->stop, 0);
+	} else {
+		mxcmci_finish_request(host, host->req);
+	}
+
+	return 1;
+}
+
+/*!
+ * GPIO interrupt service routine registered to handle the SDHC interrupts.
+ * This interrupt routine handles card insertion and card removal interrupts.
+ *
+ * @param   irq    the interrupt number
+ * @param   devid  driver private data
+ * @param   regs   holds a snapshot of the processor's context before the
+ *                 processor entered the interrupt code
+ *
+ * @return  The function returns \b IRQ_RETVAL(1)
+ */
+static irqreturn_t mxcmci_gpio_irq(int irq, void *devid)
+{
+	struct mxcmci_host *host = devid;
+	int card_gpio_status = host->plat_data->status(host->mmc->parent);
+
+	pr_debug("%s: MMC%d status=%d %s\n", DRIVER_NAME, host->id,
+		 card_gpio_status, card_gpio_status ? "removed" : "inserted");
+
+	if (card_gpio_status == host->plat_data->card_inserted_state) {
+		/*
+		 * Reinitialized the controller to clear the unknown
+		 * error state when a card is inserted.
+		 */
+		mxcmci_softreset(host);
+		__raw_writel(READ_TO_VALUE, host->base + MMC_READ_TO);
+		__raw_writel(INT_CNTR_END_CMD_RES, host->base + MMC_INT_CNTR);
+
+		mmc_detect_change(host->mmc, msecs_to_jiffies(100));
+	} else {
+		mxcmci_cmd_done(host, STATUS_TIME_OUT_RESP);
+		mmc_detect_change(host->mmc, msecs_to_jiffies(50));
+	}
+
+	do {
+		card_gpio_status = host->plat_data->status(host->mmc->parent);
+		if (card_gpio_status) {
+			set_irq_type(host->detect_irq, IRQF_TRIGGER_FALLING);
+		} else {
+			set_irq_type(host->detect_irq, IRQF_TRIGGER_RISING);
+		}
+	} while (card_gpio_status !=
+		 host->plat_data->status(host->mmc->parent));
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * Interrupt service routine registered to handle the SDHC interrupts.
+ * This interrupt routine handles end of command, card insertion and
+ * card removal interrupts. If the interrupt is card insertion or removal then
+ * inform the MMC/SD core driver to detect the change in physical connections.
+ * If the command is END_CMD_RESP read the Response FIFO. If DMA is not enabled
+ * and data transfer is associated with the command then read or write the data
+ * from or to the BUFFER_ACCESS FIFO.
+ *
+ * @param   irq    the interrupt number
+ * @param   devid  driver private data
+ * @param   regs   holds a snapshot of the processor's context before the
+ *                 processor entered the interrupt code
+ *
+ * @return  The function returns \b IRQ_RETVAL(1) if interrupt was handled,
+ *          returns \b IRQ_RETVAL(0) if the interrupt was not handled.
+ */
+static irqreturn_t mxcmci_irq(int irq, void *devid)
+{
+	struct mxcmci_host *host = devid;
+	struct mmc_data *data = host->data;
+	unsigned int status = 0;
+	u32 intctrl;
+
+	if (host->mxc_mmc_suspend_flag == 1) {
+		clk_enable(host->clk);
+	}
+
+	status = __raw_readl(host->base + MMC_STATUS);
+	pr_debug("MXC MMC IRQ status is 0x%x.\n", status);
+#ifdef CONFIG_MMC_DEBUG
+	dump_status(__FUNCTION__, status);
+#endif
+	if (status & STATUS_END_CMD_RESP) {
+		__raw_writel(STATUS_END_CMD_RESP, host->base + MMC_STATUS);
+		mxcmci_cmd_done(host, status);
+	}
+#ifdef MXC_MMC_DMA_ENABLE
+	/*
+	 * If read length < fifo length, STATUS_END_CMD_RESP and
+	 * STATUS_READ_OP_DONE may come together. In this case, it's using PIO
+	 * mode, we ignore STATUS_READ_OP_DONE.
+	 */
+	if ((status & (STATUS_WRITE_OP_DONE | STATUS_READ_OP_DONE)) &&
+	    !(status & STATUS_END_CMD_RESP)) {
+		pr_debug(KERN_INFO "MXC MMC IO OP DONE INT.\n");
+		intctrl = __raw_readl(host->base + MMC_INT_CNTR);
+		__raw_writel((~(INT_CNTR_WRITE_OP_DONE | INT_CNTR_READ_OP_DONE)
+			      & intctrl), host->base + MMC_INT_CNTR);
+
+		pr_debug("%s:READ/WRITE OPERATION DONE\n", DRIVER_NAME);
+		/* check for time out and CRC errors */
+		status = __raw_readl(host->base + MMC_STATUS);
+		if (status & STATUS_READ_OP_DONE) {
+			if (status & STATUS_TIME_OUT_READ) {
+				pr_debug("%s: Read time out occurred\n",
+					 DRIVER_NAME);
+				data->error = -ETIMEDOUT;
+				__raw_writel(STATUS_TIME_OUT_READ,
+					     host->base + MMC_STATUS);
+			} else if (status & STATUS_READ_CRC_ERR) {
+				pr_debug("%s: Read CRC error occurred\n",
+					 DRIVER_NAME);
+				data->error = -EILSEQ;
+				__raw_writel(STATUS_READ_CRC_ERR,
+					     host->base + MMC_STATUS);
+			}
+			__raw_writel(STATUS_READ_OP_DONE,
+				     host->base + MMC_STATUS);
+		}
+
+		/* check for CRC errors */
+		if (status & STATUS_WRITE_OP_DONE) {
+			if (status & STATUS_WRITE_CRC_ERR) {
+				printk(KERN_ERR
+				       "%s: Write CRC error occurred\n",
+				       DRIVER_NAME);
+				data->error = -EILSEQ;
+				__raw_writel(STATUS_WRITE_CRC_ERR,
+					     host->base + MMC_STATUS);
+			}
+			__raw_writel(STATUS_WRITE_OP_DONE,
+				     host->base + MMC_STATUS);
+		}
+
+		mxcmci_data_done(host, status);
+	}
+#endif
+	status = __raw_readl(host->base + MMC_STATUS);
+	intctrl = __raw_readl(host->base + MMC_INT_CNTR);
+	if ((status & STATUS_SDIO_INT_ACTIVE)
+	    && (intctrl & INT_CNTR_SDIO_IRQ_EN)) {
+		__raw_writel(STATUS_SDIO_INT_ACTIVE, host->base + MMC_STATUS);
+
+		/*Here we do not handle the sdio interrupt to client driver
+		   if the host is in suspend state */
+		if (host->mxc_mmc_suspend_flag == 0) {
+			mmc_signal_sdio_irq(host->mmc);
+		}
+	}
+	return IRQ_HANDLED;
+}
+
+/*!
+ * This function is called by MMC/SD Bus Protocol driver to issue a MMC
+ * and SD commands to the SDHC.
+ *
+ * @param  mmc  Pointer to MMC/SD host structure
+ * @param  req  Pointer to MMC/SD command request structure
+ */
+static void mxcmci_request(struct mmc_host *mmc, struct mmc_request *req)
+{
+	struct mxcmci_host *host = mmc_priv(mmc);
+	/* Holds the value of Command and Data Control Register */
+	unsigned long cmdat;
+
+	WARN_ON(host->req != NULL);
+
+	host->req = req;
+#ifdef CONFIG_MMC_DEBUG
+	dump_cmd(req->cmd);
+	dump_status(__FUNCTION__, __raw_readl(host->base + MMC_STATUS));
+#endif
+
+	cmdat = 0;
+	if (req->data) {
+		mxcmci_setup_data(host, req->data);
+
+		cmdat |= CMD_DAT_CONT_DATA_ENABLE;
+
+		if (req->data->flags & MMC_DATA_WRITE) {
+			cmdat |= CMD_DAT_CONT_WRITE;
+		}
+		if (req->data->flags & MMC_DATA_STREAM) {
+			printk(KERN_ERR
+			       "MXC MMC does not support stream mode\n");
+		}
+	}
+	mxcmci_start_cmd(host, req->cmd, cmdat);
+}
+
+/*!
+ * This function is called by MMC/SD Bus Protocol driver to change the clock
+ * speed of MMC or SD card
+ *
+ * @param mmc Pointer to MMC/SD host structure
+ * @param ios Pointer to MMC/SD I/O type structure
+ */
+static void mxcmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct mxcmci_host *host = mmc_priv(mmc);
+	/*This variable holds the value of clock prescaler */
+	int prescaler;
+	int clk_rate = clk_get_rate(host->clk);
+	int voltage = 0;
+#ifdef MXC_MMC_DMA_ENABLE
+	mxc_dma_device_t dev_id = 0;
+#endif
+
+	pr_debug("%s: clock %u, bus %lu, power %u, vdd %u\n", DRIVER_NAME,
+		 ios->clock, 1UL << ios->bus_width, ios->power_mode, ios->vdd);
+
+	host->dma_dir = DMA_NONE;
+
+#ifdef MXC_MMC_DMA_ENABLE
+	if (mmc->ios.bus_width != host->mode) {
+		mxc_dma_free(host->dma);
+		if (mmc->ios.bus_width == MMC_BUS_WIDTH_4) {
+			if (host->id == 0) {
+				dev_id = MXC_DMA_MMC1_WIDTH_4;
+			} else {
+				dev_id = MXC_DMA_MMC2_WIDTH_4;
+			}
+		} else {
+			if (host->id == 0) {
+				dev_id = MXC_DMA_MMC1_WIDTH_1;
+			} else {
+				dev_id = MXC_DMA_MMC2_WIDTH_1;
+			}
+		}
+		host->dma = mxc_dma_request(dev_id, "MXC MMC");
+		if (host->dma < 0) {
+			printk(KERN_ERR "Cannot allocate MMC DMA channel\n");
+		}
+		host->mode = mmc->ios.bus_width;
+		mxc_dma_callback_set(host->dma, mxcmci_dma_irq, (void *)host);
+	}
+#endif
+
+	if ((ios->vdd != host->current_vdd) && host->regulator_mmc) {
+		if (ios->vdd == 7)
+			voltage = 1800000;
+		else if (ios->vdd >= 8)
+			voltage = 2000000 + (ios->vdd - 8) * 100000;
+		regulator_set_voltage(host->regulator_mmc, voltage, voltage);
+	}
+	host->current_vdd = ios->vdd;
+
+	if (ios->power_mode != host->power_mode && host->regulator_mmc) {
+		if (ios->power_mode == MMC_POWER_UP) {
+			if (regulator_enable(host->regulator_mmc) == 0) {
+				pr_debug("mmc power on\n");
+				msleep(1);
+			}
+		} else if (ios->power_mode == MMC_POWER_OFF) {
+			regulator_disable(host->regulator_mmc);
+			pr_debug("mmc power off\n");
+		}
+	}
+	host->power_mode = ios->power_mode;
+
+	/*
+	 *  Vary divider first, then prescaler.
+	 **/
+	if (ios->clock) {
+		unsigned int clk_dev = 0;
+
+		/*
+		 * when prescaler = 16, CLK_20M = CLK_DIV / 2
+		 */
+		if (ios->clock == mmc->f_min)
+			prescaler = 16;
+		else
+			prescaler = 0;
+
+		/* clk_dev =1, CLK_DIV = ipg_perclk/2 */
+		while (prescaler <= 0x800) {
+			for (clk_dev = 1; clk_dev <= 0xF; clk_dev++) {
+				int x;
+				if (prescaler != 0) {
+					x = (clk_rate / (clk_dev + 1)) /
+					    (prescaler * 2);
+				} else {
+					x = clk_rate / (clk_dev + 1);
+				}
+
+				pr_debug("x=%d, clock=%d %d\n", x, ios->clock,
+					 clk_dev);
+				if (x <= ios->clock) {
+					break;
+				}
+			}
+			if (clk_dev < 0x10) {
+				break;
+			}
+			if (prescaler == 0)
+				prescaler = 1;
+			else
+				prescaler <<= 1;
+		}
+
+		pr_debug("prescaler = 0x%x, divider = 0x%x\n", prescaler,
+			 clk_dev);
+		mxcmci_stop_clock(host, true);
+		__raw_writel((prescaler << 4) | clk_dev,
+			     host->base + MMC_CLK_RATE);
+		mxcmci_start_clock(host, false);
+	} else {
+		mxcmci_stop_clock(host, true);
+	}
+}
+
+static void mxcmci_enable_sdio_irq(struct mmc_host *mmc, int enable)
+{
+	struct mxcmci_host *host = mmc_priv(mmc);
+	u32 intctrl;
+	unsigned long flags;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (enable)
+		host->sdio_irq_cnt++;
+	else
+		host->sdio_irq_cnt--;
+
+	if (host->sdio_irq_cnt == 1 || host->sdio_irq_cnt == 0) {
+		intctrl = __raw_readl(host->base + MMC_INT_CNTR);
+		intctrl &= ~INT_CNTR_SDIO_IRQ_EN;
+		if (host->sdio_irq_cnt)
+			intctrl |= INT_CNTR_SDIO_IRQ_EN;
+		__raw_writel(intctrl, host->base + MMC_INT_CNTR);
+	}
+
+	spin_unlock_irqrestore(&host->lock, flags);
+}
+
+static int mxcmci_get_ro(struct mmc_host *mmc)
+{
+	struct mxcmci_host *host = mmc_priv(mmc);
+
+	if (host->plat_data->wp_status)
+		return host->plat_data->wp_status(mmc->parent);
+	else
+		return 0;
+}
+
+/*!
+ * MMC/SD host operations structure.
+ * These functions are registered with MMC/SD Bus protocol driver.
+ */
+static struct mmc_host_ops mxcmci_ops = {
+	.request = mxcmci_request,
+	.set_ios = mxcmci_set_ios,
+	.get_ro = mxcmci_get_ro,
+	.enable_sdio_irq = mxcmci_enable_sdio_irq,
+};
+
+#ifdef MXC_MMC_DMA_ENABLE
+/*!
+ * This function is called by DMA Interrupt Service Routine to indicate
+ * requested DMA transfer is completed.
+ *
+ * @param   devid  pointer to device specific structure
+ * @param   error any DMA error
+ * @param   cnt   amount of data that was transferred
+ */
+static void mxcmci_dma_irq(void *devid, int error, unsigned int cnt)
+{
+	struct mxcmci_host *host = devid;
+	u32 status;
+	ulong nob, blk_size, i, blk_len;
+
+	mxc_dma_disable(host->dma);
+
+	if (error) {
+		printk(KERN_ERR "Error in DMA transfer\n");
+		status = __raw_readl(host->base + MMC_STATUS);
+#ifdef CONFIG_MMC_DEBUG
+		dump_status(__FUNCTION__, status);
+#endif
+		mxcmci_data_done(host, status);
+		return;
+	}
+	pr_debug("%s: Transfered bytes:%d\n", DRIVER_NAME, cnt);
+	nob = __raw_readl(host->base + MMC_REM_NOB);
+	blk_size = __raw_readl(host->base + MMC_REM_BLK_SIZE);
+	blk_len = __raw_readl(host->base + MMC_BLK_LEN);
+	pr_debug("%s: REM_NOB:%lu REM_BLK_SIZE:%lu\n", DRIVER_NAME, nob,
+		 blk_size);
+	i = 0;
+
+	/* Enable the WRITE OP Done INT */
+	status = __raw_readl(host->base + MMC_INT_CNTR);
+	__raw_writel((INT_CNTR_READ_OP_DONE | INT_CNTR_WRITE_OP_DONE | status),
+		     host->base + MMC_INT_CNTR);
+}
+#endif
+
+/*!
+ * This function is called during the driver binding process. Based on the SDHC
+ * module that is being probed this function adds the appropriate SDHC module
+ * structure in the core driver.
+ *
+ * @param   pdev  the device structure used to store device specific
+ *                information that is used by the suspend, resume and remove
+ *                functions.
+ *
+ * @return  The function returns 0 on successful registration and initialization
+ *          of SDHC module. Otherwise returns specific error code.
+ */
+static int mxcmci_probe(struct platform_device *pdev)
+{
+	struct mxc_mmc_platform_data *mmc_plat = pdev->dev.platform_data;
+	struct mmc_host *mmc;
+	struct mxcmci_host *host = NULL;
+	int card_gpio_status;
+	int ret = -ENODEV;
+
+	if (!mmc_plat) {
+		return -EINVAL;
+	}
+
+	mmc = mmc_alloc_host(sizeof(struct mxcmci_host), &pdev->dev);
+	if (!mmc) {
+		return -ENOMEM;
+	}
+	host = mmc_priv(mmc);
+	platform_set_drvdata(pdev, mmc);
+
+	mmc->ops = &mxcmci_ops;
+	mmc->ocr_avail = mmc_plat->ocr_mask;
+
+	/* Hack to work with LP1070 */
+	if (mmc->ocr_avail && ~(MMC_VDD_31_32 - 1) == 0)
+		mmc->ocr_avail |= MMC_VDD_31_32;
+
+	mmc->max_phys_segs = NR_SG;
+	mmc->caps = MMC_CAP_4_BIT_DATA | MMC_CAP_SDIO_IRQ;
+
+	mmc->f_min = mmc_plat->min_clk;
+	mmc->f_max = mmc_plat->max_clk;
+	mmc->max_req_size = 32 * 1024;
+	mmc->max_seg_size = mmc->max_req_size;
+	mmc->max_blk_count = 65536;
+
+	spin_lock_init(&host->lock);
+	host->mmc = mmc;
+	host->dma = -1;
+	host->dma_dir = DMA_NONE;
+	host->id = pdev->id;
+	host->mxc_mmc_suspend_flag = 0;
+	host->mode = -1;
+	host->plat_data = mmc_plat;
+	if (!host->plat_data) {
+		ret = -EINVAL;
+		goto out0;
+	}
+
+	/* Get pwr supply for SDHC */
+	if (NULL != mmc_plat->power_mmc) {
+		host->regulator_mmc =
+		    regulator_get(&pdev->dev, mmc_plat->power_mmc);
+		if (IS_ERR(host->regulator_mmc)) {
+			ret = PTR_ERR(host->regulator_mmc);
+			goto out1;
+		}
+		if (!regulator_is_enabled(host->regulator_mmc)) {
+			if (regulator_enable(host->regulator_mmc) == 0) {
+				pr_debug("mmc power on\n");
+				msleep(1);
+			}
+		}
+	}
+
+	gpio_sdhc_active(pdev->id);
+
+	host->clk = clk_get(&pdev->dev, "sdhc_clk");
+	pr_debug("SDHC:%d clock:%lu\n", pdev->id, clk_get_rate(host->clk));
+	clk_enable(host->clk);
+
+	host->res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!host->res) {
+		ret = -ENOMEM;
+		goto out2;
+	}
+
+	if (!request_mem_region(host->res->start,
+				host->res->end -
+				host->res->start + 1, pdev->name)) {
+		printk(KERN_ERR "request_mem_region failed\n");
+		ret = -ENOMEM;
+		goto out2;
+	}
+	host->base = (void *)IO_ADDRESS(host->res->start);
+	if (!host->base) {
+		ret = -ENOMEM;
+		goto out3;
+	}
+
+	host->irq = platform_get_irq(pdev, 0);
+	if (!host->irq) {
+		ret = -ENOMEM;
+		goto out3;
+	}
+
+	if (!host->plat_data->card_fixed) {
+		host->detect_irq = platform_get_irq(pdev, 1);
+		if (!host->detect_irq)
+			goto out3;
+
+		do {
+			card_gpio_status =
+			    host->plat_data->status(host->mmc->parent);
+			if (card_gpio_status)
+				set_irq_type(host->detect_irq,
+					     IRQF_TRIGGER_FALLING);
+			else
+				set_irq_type(host->detect_irq,
+					     IRQF_TRIGGER_RISING);
+
+		} while (card_gpio_status !=
+			 host->plat_data->status(host->mmc->parent));
+
+		ret = request_irq(host->detect_irq, mxcmci_gpio_irq, 0,
+				  pdev->name, host);
+		if (ret)
+			goto out3;
+	}
+
+	mxcmci_softreset(host);
+
+	if (__raw_readl(host->base + MMC_REV_NO) != SDHC_REV_NO) {
+		printk(KERN_ERR "%s: wrong rev.no. 0x%08x. aborting.\n",
+		       pdev->name, MMC_REV_NO);
+		goto out3;
+	}
+	__raw_writel(READ_TO_VALUE, host->base + MMC_READ_TO);
+
+	__raw_writel(INT_CNTR_END_CMD_RES, host->base + MMC_INT_CNTR);
+
+	ret = request_irq(host->irq, mxcmci_irq, 0, pdev->name, host);
+	if (ret) {
+		goto out4;
+	}
+
+	if ((ret = mmc_add_host(mmc)) < 0) {
+		goto out5;
+	}
+
+	printk(KERN_INFO "%s-%d found\n", pdev->name, pdev->id);
+	if (host->id < MAX_HOST)
+		hosts[host->id] = host->mmc;
+
+	return 0;
+
+      out5:
+	free_irq(host->irq, host);
+      out4:
+	free_irq(host->detect_irq, host);
+      out3:
+	release_mem_region(pdev->resource[0].start,
+			   pdev->resource[0].end - pdev->resource[0].start + 1);
+      out2:
+	clk_disable(host->clk);
+	regulator_disable(host->regulator_mmc);
+	regulator_put(host->regulator_mmc);
+      out1:
+	gpio_sdhc_inactive(pdev->id);
+      out0:
+	mmc_free_host(mmc);
+	platform_set_drvdata(pdev, NULL);
+	return ret;
+}
+
+/*!
+ * Dissociates the driver from the SDHC device. Removes the appropriate SDHC
+ * module structure from the core driver.
+ *
+ * @param   pdev  the device structure used to give information on which SDHC
+ *                to remove
+ *
+ * @return  The function always returns 0.
+ */
+static int mxcmci_remove(struct platform_device *pdev)
+{
+	struct mmc_host *mmc = platform_get_drvdata(pdev);
+
+	if (mmc) {
+		struct mxcmci_host *host = mmc_priv(mmc);
+
+		hosts[host->id] = NULL;
+		mmc_remove_host(mmc);
+		free_irq(host->irq, host);
+		free_irq(host->detect_irq, host);
+#ifdef MXC_MMC_DMA_ENABLE
+		mxc_dma_free(host->dma);
+#endif
+		release_mem_region(host->res->start,
+				   host->res->end - host->res->start + 1);
+		mmc_free_host(mmc);
+		if (NULL != host->regulator_mmc)
+			regulator_put(host->regulator_mmc);
+		gpio_sdhc_inactive(pdev->id);
+	}
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+/*!
+ * This function is called to put the SDHC in a low power state. Refer to the
+ * document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure used to give information on which SDHC
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function always returns 0.
+ */
+static int mxcmci_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct mmc_host *mmc = platform_get_drvdata(pdev);
+	struct mxcmci_host *host = mmc_priv(mmc);
+	int ret = 0;
+
+	if (mmc) {
+		host->mxc_mmc_suspend_flag = 1;
+		ret = mmc_suspend_host(mmc, state);
+	}
+
+	clk_disable(host->clk);
+	/*
+	 * The CD INT should be disabled in the suspend
+	 * and enabled in resumed.
+	 * Otherwise, the system would be halt when wake
+	 * up with the situation that there is a card
+	 * insertion during the system is in suspend mode.
+	 */
+	disable_irq(host->detect_irq);
+
+	gpio_sdhc_inactive(pdev->id);
+
+	if (host->regulator_mmc)
+		regulator_disable(host->regulator_mmc);
+
+	return ret;
+}
+
+/*!
+ * This function is called to bring the SDHC back from a low power state. Refer
+ * to the document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure used to give information on which SDHC
+ *                to resume
+ *
+ * @return  The function always returns 0.
+ */
+static int mxcmci_resume(struct platform_device *pdev)
+{
+	struct mmc_host *mmc = platform_get_drvdata(pdev);
+	struct mxcmci_host *host = mmc_priv(mmc);
+	int ret = 0;
+
+	/*
+	 * Note that a card insertion interrupt will cause this
+	 * driver to resume automatically.  In that case we won't
+	 * actually have to do any work here.  Return success.
+	 */
+	if (!host->mxc_mmc_suspend_flag) {
+		return 0;
+	}
+
+	/* enable pwr supply for SDHC */
+	if (host->regulator_mmc && !regulator_is_enabled(host->regulator_mmc)) {
+		regulator_enable(host->regulator_mmc);
+		msleep(1);
+	}
+
+	gpio_sdhc_active(pdev->id);
+
+	clk_enable(host->clk);
+
+	if (mmc) {
+		ret = mmc_resume_host(mmc);
+		host->mxc_mmc_suspend_flag = 0;
+	}
+
+	enable_irq(host->detect_irq);
+
+	return ret;
+}
+#else
+#define mxcmci_suspend  NULL
+#define mxcmci_resume   NULL
+#endif				/* CONFIG_PM */
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcmci_driver = {
+	.driver = {
+		   .name = "mxcmci",
+		   },
+	.probe = mxcmci_probe,
+	.remove = mxcmci_remove,
+	.suspend = mxcmci_suspend,
+	.resume = mxcmci_resume,
+};
+
+/*!
+ * This function is used to initialize the MMC/SD driver module. The function
+ * registers the power management callback functions with the kernel and also
+ * registers the MMC/SD callback functions with the core MMC/SD driver.
+ *
+ * @return  The function returns 0 on success and a non-zero value on failure.
+ */
+static int __init mxcmci_init(void)
+{
+	printk(KERN_INFO "MXC MMC/SD driver\n");
+	return platform_driver_register(&mxcmci_driver);
+}
+
+/*!
+ * This function is used to cleanup all resources before the driver exits.
+ */
+static void __exit mxcmci_exit(void)
+{
+	platform_driver_unregister(&mxcmci_driver);
+}
+
+module_init(mxcmci_init);
+module_exit(mxcmci_exit);
+
+MODULE_DESCRIPTION("MXC Multimedia Card Interface Driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mmc/host/mxc_mmc.h b/drivers/mmc/host/mxc_mmc.h
new file mode 100644
index 000000000000..3ad45377dde6
--- /dev/null
+++ b/drivers/mmc/host/mxc_mmc.h
@@ -0,0 +1,126 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef __MXC_MMC_REG_H__
+#define __MXC_MMC_REG_H__
+
+#include <mach/hardware.h>
+
+/*!
+ * @defgroup MMC_SD MMC/SD Driver
+ */
+
+/*!
+ * @file mxc_mmc.h
+ *
+ * @brief Driver for the Freescale Semiconductor MXC SDHC modules.
+ *
+ * This file defines offsets and bits of SDHC registers. SDHC is also referred as
+ * MMC/SD controller
+ *
+ * @ingroup MMC_SD
+ */
+
+/*!
+ * Number of SDHC modules
+ */
+
+#define SDHC_MMC_WML                    16
+#define SDHC_SD_WML                     64
+#define DRIVER_NAME                     "MXCMMC"
+#define SDHC_MEM_SIZE                   16384
+#define SDHC_REV_NO                     0x400
+#define READ_TO_VALUE                   0x2db4
+
+/* Address offsets of the SDHC registers */
+#define MMC_STR_STP_CLK                 0x00	/* Clock Control Reg */
+#define MMC_STATUS                      0x04	/* Status Reg */
+#define MMC_CLK_RATE                    0x08	/* Clock Rate Reg */
+#define MMC_CMD_DAT_CONT                0x0C	/* Command and Data Control Reg */
+#define MMC_RES_TO                      0x10	/* Response Time-out Reg */
+#define MMC_READ_TO                     0x14	/* Read Time-out Reg */
+#define MMC_BLK_LEN                     0x18	/* Block Length Reg */
+#define MMC_NOB                         0x1C	/* Number of Blocks Reg */
+#define MMC_REV_NO                      0x20	/* Revision Number Reg */
+#define MMC_INT_CNTR                    0x24	/* Interrupt Control Reg */
+#define MMC_CMD                         0x28	/* Command Number Reg */
+#define MMC_ARG                         0x2C	/* Command Argument Reg */
+#define MMC_RES_FIFO                    0x34	/* Command Response Reg */
+#define MMC_BUFFER_ACCESS               0x38	/* Data Buffer Access Reg */
+#define MMC_REM_NOB                     0x40	/* Remaining NOB Reg */
+#define MMC_REM_BLK_SIZE                0x44	/* Remaining Block Size Reg */
+
+/* Bit definitions for STR_STP_CLK */
+#define STR_STP_CLK_RESET               (1<<3)
+#define STR_STP_CLK_START_CLK           (1<<1)
+#define STR_STP_CLK_STOP_CLK            (1<<0)
+
+/* Bit definitions for STATUS */
+#define STATUS_CARD_INSERTION           (1<<31)
+#define STATUS_CARD_REMOVAL             (1<<30)
+#define STATUS_YBUF_EMPTY               (1<<29)
+#define STATUS_XBUF_EMPTY               (1<<28)
+#define STATUS_YBUF_FULL                (1<<27)
+#define STATUS_XBUF_FULL                (1<<26)
+#define STATUS_BUF_UND_RUN              (1<<25)
+#define STATUS_BUF_OVFL                 (1<<24)
+#define STATUS_SDIO_INT_ACTIVE          (1<<14)
+#define STATUS_END_CMD_RESP             (1<<13)
+#define STATUS_WRITE_OP_DONE            (1<<12)
+#define STATUS_READ_OP_DONE             (1<<11)
+#define STATUS_WR_CRC_ERROR_CODE_MASK   (3<<9)
+#define STATUS_CARD_BUS_CLK_RUN         (1<<8)
+#define STATUS_BUF_READ_RDY             (1<<7)
+#define STATUS_BUF_WRITE_RDY            (1<<6)
+#define STATUS_RESP_CRC_ERR             (1<<5)
+#define STATUS_READ_CRC_ERR             (1<<3)
+#define STATUS_WRITE_CRC_ERR            (1<<2)
+#define STATUS_TIME_OUT_RESP            (1<<1)
+#define STATUS_TIME_OUT_READ            (1<<0)
+#define STATUS_ERR_MASK                 0x3f
+
+/* Clock rate definitions */
+#define CLK_RATE_PRESCALER(x)           ((x) & 0xF)
+#define CLK_RATE_CLK_DIVIDER(x)         (((x) & 0xF) << 4)
+
+/* Bit definitions for CMD_DAT_CONT */
+#define CMD_DAT_CONT_CMD_RESP_LONG_OFF  (1<<12)
+#define CMD_DAT_CONT_STOP_READWAIT      (1<<11)
+#define CMD_DAT_CONT_START_READWAIT     (1<<10)
+#define CMD_DAT_CONT_BUS_WIDTH_1        (0<<8)
+#define CMD_DAT_CONT_BUS_WIDTH_4        (2<<8)
+#define CMD_DAT_CONT_INIT               (1<<7)
+#define CMD_DAT_CONT_WRITE              (1<<4)
+#define CMD_DAT_CONT_DATA_ENABLE        (1<<3)
+#define CMD_DAT_CONT_RESPONSE_FORMAT_R1 (1)
+#define CMD_DAT_CONT_RESPONSE_FORMAT_R2 (2)
+#define CMD_DAT_CONT_RESPONSE_FORMAT_R3 (3)
+#define CMD_DAT_CONT_RESPONSE_FORMAT_R4 (4)
+#define CMD_DAT_CONT_RESPONSE_FORMAT_R5 (5)
+#define CMD_DAT_CONT_RESPONSE_FORMAT_R6 (6)
+
+/* Bit definitions for INT_CNTR */
+#define INT_CNTR_SDIO_INT_WKP_EN        (1<<18)
+#define INT_CNTR_CARD_INSERTION_WKP_EN  (1<<17)
+#define INT_CNTR_CARD_REMOVAL_WKP_EN    (1<<16)
+#define INT_CNTR_CARD_INSERTION_EN      (1<<15)
+#define INT_CNTR_CARD_REMOVAL_EN        (1<<14)
+#define INT_CNTR_SDIO_IRQ_EN            (1<<13)
+#define INT_CNTR_DAT0_EN                (1<<12)
+#define INT_CNTR_BUF_READ_EN            (1<<4)
+#define INT_CNTR_BUF_WRITE_EN           (1<<3)
+#define INT_CNTR_END_CMD_RES            (1<<2)
+#define INT_CNTR_WRITE_OP_DONE          (1<<1)
+#define INT_CNTR_READ_OP_DONE           (1<<0)
+
+#endif				/* __MXC_MMC_REG_H__ */
diff --git a/drivers/mmc/host/s3c-hsmmc.c b/drivers/mmc/host/s3c-hsmmc.c
new file mode 100644
index 000000000000..21986a2c72f7
--- /dev/null
+++ b/drivers/mmc/host/s3c-hsmmc.c
@@ -0,0 +1,1457 @@
+/* -*- linux-c -*-
+ *
+ *  linux/drivers/mmc/s3c-hsmmc.c - Samsung S3C24XX HS-MMC driver
+ *
+ * $Id: s3c-hsmmc.c,v 1.18 2007/06/07 07:14:57 scsuh Exp $
+ *
+ *  Copyright (C) 2006 Samsung Electronics, All Rights Reserved.
+ *  by Seung-Chull, Suh <sc.suh@samsung.com>
+ *
+ *  This driver is made for High Speed MMC interface. This interface
+ *  is adopted and implemented since s3c2443 was made.
+ *
+ * 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.
+ *
+ *  Modified by Ryu,Euiyoul <steven.ryu@samsung.com>
+ *  Modified by Seung-chull, Suh to support s3c6400
+ *  Modified by Luis Galdos, Added PM-support
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/highmem.h>
+#include <linux/dma-mapping.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
+#include <linux/err.h>
+#include <linux/proc_fs.h>
+#include <linux/clk.h>
+
+#include <asm/dma.h>
+#include <asm/dma-mapping.h>
+
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <asm/scatterlist.h>
+#include <asm/sizes.h>
+#include <asm/mach/mmc.h>
+
+#include <mach/dma.h>
+#include <plat/hsmmc.h>
+#include <mach/regs-hsmmc.h>
+#include <mach/regs-gpio.h>
+#include <mach/regs-gpioj.h>
+
+
+#define printk_err(fmt, args...)                printk(KERN_ERR "[ ERROR ] hsmmc: " fmt, ## args)
+#define printk_info(fmt, args...)               printk(KERN_INFO "hsmmc: " fmt, ## args)
+
+#if 0
+#define CONFIG_S3CMCI_DEBUG
+#endif
+
+#ifdef CONFIG_S3CMCI_DEBUG
+#  define printk_debug(fmt, args...)		printk(KERN_DEBUG "hsmmc: %s() " fmt, __FUNCTION__ , ## args)
+#else
+#  define printk_debug(fmt, args...)
+#endif
+
+/* Enables the support for the scatterlists */
+#define CONFIG_HSMMC_PSEUDO_SCATTERGATHER       (1)
+
+#define DBG(x, ...)
+
+#include "s3c-hsmmc.h"
+
+#define DRIVER_NAME				"s3c-hsmmc"
+#define PFX					DRIVER_NAME ": "
+
+#define RESSIZE(ressource) (((ressource)->end - (ressource)->start)+1)
+
+#ifdef CONFIG_HSMMC_PROC_DATA
+struct s3c_hsmmc_host *global_host[3];
+#endif
+
+/*****************************************************************************\
+ *                                                                           *
+ * Low level functions                                                       *
+ *                                                                           *
+\*****************************************************************************/
+
+static struct s3c_hsmmc_cfg s3c_hsmmc_platform = {
+	.hwport = 0,
+	.host_caps = (MMC_CAP_4_BIT_DATA | MMC_CAP_MMC_HIGHSPEED),
+	.base = NULL,
+	.ctrl3[0] = 0x80800000,
+	.ctrl3[1] = 0x80800000,
+	.set_gpio = NULL,
+};
+
+/* s3c_hsmmc_get_platdata
+ *
+ * get the platform data associated with the given device, or return
+ * the default if there is none
+ */
+
+static struct s3c_hsmmc_cfg *s3c_hsmmc_get_platdata (struct device *dev)
+{
+	if (dev->platform_data != NULL)
+		return (struct s3c_hsmmc_cfg *)dev->platform_data;
+
+	return &s3c_hsmmc_platform;
+}
+
+static void s3c_hsmmc_reset (struct s3c_hsmmc_host *host, u8 mask)
+{
+	unsigned long timeout;
+
+	s3c_hsmmc_writeb(mask, S3C2410_HSMMC_SWRST);
+
+	if (mask & S3C_HSMMC_RESET_ALL)
+		host->clock = (uint)-1;
+
+	/* Wait max 100 ms */
+	timeout = 100;
+
+	/* hw clears the bit when it's done */
+	while (s3c_hsmmc_readb(S3C2410_HSMMC_SWRST) & mask) {
+		if (timeout == 0) {
+			printk("%s: Reset 0x%x never completed. \n",
+				mmc_hostname(host->mmc), (int)mask);
+			return;
+		}
+		timeout--;
+		mdelay(1);
+	}
+
+}
+
+static void s3c_hsmmc_ios_init (struct s3c_hsmmc_host *host)
+{
+	u32 intmask;
+
+	s3c_hsmmc_reset(host, S3C_HSMMC_RESET_ALL);
+
+	intmask = S3C_HSMMC_INT_BUS_POWER | S3C_HSMMC_INT_DATA_END_BIT |
+		S3C_HSMMC_INT_DATA_CRC | S3C_HSMMC_INT_DATA_TIMEOUT | S3C_HSMMC_INT_INDEX |
+		S3C_HSMMC_INT_END_BIT | S3C_HSMMC_INT_CRC | S3C_HSMMC_INT_TIMEOUT |
+		S3C_HSMMC_INT_CARD_REMOVE | S3C_HSMMC_INT_CARD_INSERT |
+		S3C_HSMMC_INT_DATA_AVAIL | S3C_HSMMC_INT_SPACE_AVAIL |
+		S3C_HSMMC_INT_DATA_END | S3C_HSMMC_INT_RESPONSE;
+
+#ifdef CONFIG_HSMMC_PSEUDO_SCATTERGATHER
+	intmask |= S3C_HSMMC_INT_DMA_END;
+#endif
+	s3c_hsmmc_writel(intmask, S3C2410_HSMMC_NORINTSTSEN);
+	s3c_hsmmc_writel(intmask, S3C2410_HSMMC_NORINTSIGEN);
+}
+
+/*****************************************************************************\
+ *                                                                           *
+ * Tasklets                                                                  *
+ *                                                                           *
+\*****************************************************************************/
+
+static void s3c_hsmmc_tasklet_card (ulong param)
+{
+	struct s3c_hsmmc_host *host;
+	unsigned long iflags;
+
+	host = (struct s3c_hsmmc_host*)param;
+	spin_lock_irqsave( &host->lock, iflags);
+
+	if (!(s3c_hsmmc_readl(S3C2410_HSMMC_PRNSTS) & S3C_HSMMC_CARD_PRESENT)) {
+		if (host->mrq) {
+			printk(KERN_ERR "%s: Card removed during transfer!\n",
+				mmc_hostname(host->mmc));
+			printk(KERN_ERR "%s: Resetting controller.\n",
+				mmc_hostname(host->mmc));
+
+			s3c_hsmmc_reset(host, S3C_HSMMC_RESET_CMD);
+			s3c_hsmmc_reset(host, S3C_HSMMC_RESET_DATA);
+
+			host->mrq->cmd->error = -EILSEQ;
+			tasklet_schedule(&host->finish_tasklet);
+		}
+	}
+
+	spin_unlock_irqrestore( &host->lock, iflags);
+
+	mmc_detect_change(host->mmc, msecs_to_jiffies(500));
+}
+
+static void s3c_hsmmc_activate_led(struct s3c_hsmmc_host *host)
+{
+	unsigned int ctrl;
+	struct s3c_hsmmc_cfg *cfg = host->plat_data;
+
+	if (cfg->gpio_led == S3C2443_GPJ13) {
+		ctrl = s3c_hsmmc_readl(S3C2410_HSMMC_HOSTCTL);
+		ctrl &= ~S3C_HSMMC_CTRL_LED;
+		s3c_hsmmc_writel(ctrl, S3C2410_HSMMC_HOSTCTL);
+	} else if (cfg->gpio_led)
+		s3c2410_gpio_setpin(cfg->gpio_led, 1);
+
+
+#if 0	// for 6400
+	s3c_gpio_cfgpin(S3C_GPJ13, S3C_GPJ13_SD0LED);
+#endif
+}
+
+static void s3c_hsmmc_deactivate_led(struct s3c_hsmmc_host *host)
+{
+	unsigned int ctrl;
+	struct s3c_hsmmc_cfg *cfg = host->plat_data;
+	
+	if (cfg->gpio_led == S3C2443_GPJ13) {
+		ctrl = s3c_hsmmc_readl(S3C2410_HSMMC_HOSTCTL);
+		ctrl |= S3C_HSMMC_CTRL_LED;
+		s3c_hsmmc_writel(ctrl, S3C2410_HSMMC_HOSTCTL);
+	} else if (cfg->gpio_led)
+		s3c2410_gpio_setpin(cfg->gpio_led, 0);
+
+
+#if 0	// for 6400
+	s3c_gpio_cfgpin(S3C_GPJ13, S3C_GPJ13_INP);
+#endif
+}
+
+/*****************************************************************************\
+ *                                                                           *
+ * Core functions                                                            *
+ *                                                                           *
+\*****************************************************************************/
+
+#ifdef CONFIG_HSMMC_PSEUDO_SCATTERGATHER
+static inline uint s3c_hsmmc_build_dma_table (struct s3c_hsmmc_host *host,
+						struct mmc_data *data)
+{
+	uint i, j = 0, sub_num = 0;
+	dma_addr_t addr;
+	uint size, length, end;
+	int boundary, xor_bit;
+	struct scatterlist * sg = data->sg;
+
+	/* build dma table except last one */
+	for (i=0; i<(host->sg_len-1); i++) {
+		addr = sg[i].dma_address;
+		size = sg[i].length;
+		DBG("%d - addr: %08x, size: %08x\n", i, addr, size);
+
+		for (; (j<CONFIG_S3C_HSMMC_MAX_HW_SEGS*4) && size; j++) {
+			end = addr + size;
+			xor_bit = min(7+(2+8+2), fls(addr^end) -1);
+
+			DBG("%08x %08x %08x %d\n", addr, size, end, xor_bit);
+
+			host->dblk[j].dma_address = addr;
+
+			length = (end & ~((1<<xor_bit)-1)) - addr;
+			boundary = xor_bit - (2+8+2);
+			DBG("length: %x, boundary: %d\n", length, boundary);
+
+			if (length < S3C_HSMMC_MALLOC_SIZE) {
+				boundary = 0;
+
+				if ((addr+length) & (S3C_HSMMC_MALLOC_SIZE-1)) {
+					void *dest;
+
+					DBG("#########error fixing: %08x, %x\n", addr, length);
+					dest = host->sub_block[sub_num] + S3C_HSMMC_MALLOC_SIZE - length;
+					if (data->flags & MMC_DATA_WRITE) { /* write */
+						memcpy(dest, phys_to_virt(addr), length);
+					}
+
+					host->dblk[j].original = phys_to_virt(addr);
+					host->dblk[j].dma_address = dma_map_single(NULL, dest, length, host->dma_dir);
+					sub_num++;
+				}
+			}
+
+			host->dblk[j].length = length;
+			host->dblk[j].boundary = boundary;
+			DBG("   %d: %08x, %08x %x\n",
+						j, addr, length, boundary);
+			addr += length;
+			size -= length;
+		}
+	}
+
+	/* the last one */
+	host->dblk[j].dma_address = sg[i].dma_address;
+	host->dblk[j].length = sg[i].length;
+	host->dblk[j].boundary = 0x7;
+
+ 	return (j+1);
+}
+#endif
+
+static inline void s3c_hsmmc_prepare_data(struct s3c_hsmmc_host *host,
+					  struct mmc_command *cmd)
+{
+	u32 reg;
+	struct mmc_data *data = cmd->data;
+
+	/* If no data to send, only enable the CMD complete interrupt */
+	if (data == NULL) {
+		reg = s3c_hsmmc_readl(S3C2410_HSMMC_NORINTSTSEN);
+		reg |= S3C_HSMMC_NIS_CMDCMP;
+		s3c_hsmmc_writel(reg, S3C2410_HSMMC_NORINTSTSEN);
+		return;
+	}
+
+#ifdef CONFIG_HSMMC_PROC_DATA
+	{
+		u32 total_size;
+		int bit;
+
+		total_size = data->blksz * data->blocks;
+		bit = fls(total_size) - 13;
+		if (bit < 0) bit = 0;
+		if (data->flags & MMC_DATA_WRITE) {	/* write */
+			host->tx_pkt[bit]++;
+		} else {	/* read */
+			host->rx_pkt[bit]++;
+		}
+	}
+#endif
+	
+	reg = s3c_hsmmc_readl(S3C2410_HSMMC_NORINTSTSEN) & ~S3C_HSMMC_NIS_CMDCMP;
+	reg |= S3C_HSMMC_NIS_TRSCMP;
+	s3c_hsmmc_writel(reg, S3C2410_HSMMC_NORINTSTSEN);
+
+	host->dma_dir = (data->flags & MMC_DATA_READ)
+		? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+
+
+	
+#ifdef CONFIG_HSMMC_PSEUDO_SCATTERGATHER
+	host->sg_len = dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
+				  host->dma_dir);
+
+	reg = s3c_hsmmc_readl(S3C2410_HSMMC_NORINTSTSEN);
+	if (host->sg_len == 1) {
+		reg &= ~S3C_HSMMC_NIS_DMA;
+	} else {
+		reg |= S3C_HSMMC_NIS_DMA;
+	}
+	s3c_hsmmc_writel(reg, S3C2410_HSMMC_NORINTSTSEN);
+
+	DBG("data->sg_len: %d\n", data->sg_len);
+	host->dma_blk = s3c_hsmmc_build_dma_table(host, data);
+	host->next_blk = 0;
+#else
+	dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
+		   host->dma_dir);
+#endif
+}
+
+static inline void s3c_hsmmc_set_transfer_mode (struct s3c_hsmmc_host *host,
+					struct mmc_data *data)
+{
+	u16 mode;
+
+	mode = S3C_HSMMC_TRNS_DMA;
+
+	if (data->stop)
+		mode |= S3C_HSMMC_TRNS_ACMD12;
+
+	/* Set the configuration for the number of blocks to transfer */
+	if (data->blocks > 1)
+		mode |= (S3C_HSMMC_TRNS_MULTI | S3C_HSMMC_TRNS_BLK_CNT_EN);
+
+	/* Set the transfer direction */
+	if (data->flags & MMC_DATA_READ)
+		mode |= S3C_HSMMC_TRNS_READ;
+
+	printk_debug("Setting the transfer mode to 0x%08x\n", mode);
+	s3c_hsmmc_writew(mode, S3C2410_HSMMC_TRNMOD);
+}
+
+static inline void s3c_hsmmc_send_register (struct s3c_hsmmc_host *host)
+{
+	struct mmc_command *cmd = host->cmd;
+	struct mmc_data *data = cmd->data;
+
+	u32 cmd_val;
+
+	if (data) {
+#ifdef CONFIG_HSMMC_PSEUDO_SCATTERGATHER
+		struct s3c_hsmmc_dma_blk *dblk;
+
+		dblk = &host->dblk[0];
+
+		s3c_hsmmc_writew(S3C_HSMMC_MAKE_BLKSZ(dblk->boundary, data->blksz),
+				 S3C2410_HSMMC_BLKSIZE);
+		
+		s3c_hsmmc_writel(dblk->dma_address, S3C2410_HSMMC_SYSAD);
+#else
+		s3c_hsmmc_writew(S3C_HSMMC_MAKE_BLKSZ(0x7, data->blksz),
+				 S3C2410_HSMMC_BLKSIZE);
+		
+		s3c_hsmmc_writel(sg_dma_address(data->sg), S3C2410_HSMMC_SYSAD);
+#endif
+		
+		s3c_hsmmc_writew(data->blocks, S3C2410_HSMMC_BLKCNT);
+		s3c_hsmmc_set_transfer_mode(host, data);
+	}
+
+	s3c_hsmmc_writel(cmd->arg, S3C2410_HSMMC_ARGUMENT);
+
+	cmd_val = (cmd->opcode << 8);
+	if (cmd_val == (12<<8))
+		cmd_val |= (3 << 6);
+
+	if (cmd->flags & MMC_RSP_136)		/* Long RSP */
+		cmd_val |= S3C_HSMMC_CMD_RESP_LONG;
+	else if (cmd->flags & MMC_RSP_BUSY)	/* R1B */
+		cmd_val |= S3C_HSMMC_CMD_RESP_SHORT_BUSY;
+	else if (cmd->flags & MMC_RSP_PRESENT)	/* Normal RSP */
+		cmd_val |= S3C_HSMMC_CMD_RESP_SHORT;
+
+	if (cmd->flags & MMC_RSP_OPCODE)
+		cmd_val |= S3C_HSMMC_CMD_INDEX;
+
+	if (cmd->flags & MMC_RSP_CRC)
+		cmd_val |= S3C_HSMMC_CMD_CRC;
+
+	if (data)
+		cmd_val |= S3C_HSMMC_CMD_DATA;
+
+	s3c_hsmmc_writew(cmd_val, S3C2410_HSMMC_CMDREG);
+}
+
+
+/* Send a command to the HSMMC-controller */
+static inline void s3c_hsmmc_send_command(struct s3c_hsmmc_host *host,
+					  struct mmc_command *cmd)
+{
+	u32 mask;
+	ulong timeout;
+
+	/* Wait max 10 ms */
+	timeout = 10;
+
+	mask = S3C_HSMMC_CMD_INHIBIT;
+	if ((cmd->data != NULL) || (cmd->flags & MMC_RSP_BUSY))
+		mask |= S3C_HSMMC_DATA_INHIBIT;
+
+	while (s3c_hsmmc_readl(S3C2410_HSMMC_PRNSTS) & mask) {
+		if (timeout == 0) {
+			printk_err("%s: Controller never released "
+				   "inhibit bit(s).\n", mmc_hostname(host->mmc));
+			cmd->error = -EILSEQ;
+			tasklet_schedule(&host->finish_tasklet);
+			return;
+		}
+		timeout--;
+		mdelay(1);
+	}
+
+	mod_timer(&host->timer, jiffies + 4 * HZ);
+
+	host->cmd = cmd;
+
+	s3c_hsmmc_prepare_data(host, cmd);
+	s3c_hsmmc_send_register(host);
+}
+
+
+static void s3c_hsmmc_finish_data(struct s3c_hsmmc_host *host)
+{
+	struct mmc_data *data;
+	u16 blocks;
+
+	BUG_ON(!host->data);
+
+	data = host->data;
+	host->data = NULL;
+
+	dma_unmap_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
+			(data->flags & MMC_DATA_READ)
+			? DMA_FROM_DEVICE : DMA_TO_DEVICE);
+
+	/*
+	 * Controller doesn't count down when in single block mode.
+	 */
+	if ((data->blocks == 1) && (data->error == 0))
+		blocks = 0;
+	else {
+		blocks = s3c_hsmmc_readw(S3C2410_HSMMC_BLKCNT);
+	}
+	data->bytes_xfered = data->blksz * (data->blocks - blocks);
+
+	if ((data->error == 0) && blocks) {
+		printk_err("%s: Controller signalled completion even "
+			   "though there were blocks left. : %d\n",
+			   mmc_hostname(host->mmc), blocks);
+		data->error = -EILSEQ;
+	}
+
+	printk_debug("Ending data transfer (%d bytes)\n", data->bytes_xfered);
+
+	tasklet_schedule(&host->finish_tasklet);
+}
+
+static void s3c_hsmmc_finish_command (struct s3c_hsmmc_host *host)
+{
+	int i;
+	unsigned int resp;
+
+	BUG_ON(host->cmd == NULL);
+
+	if (host->cmd->flags & MMC_RSP_PRESENT) {
+		if (host->cmd->flags & MMC_RSP_136) {
+			/* CRC is stripped so we need to do some shifting. */
+			for (i = 0; i < 4; i++) {
+				resp = s3c_hsmmc_readl(S3C2410_HSMMC_RSPREG0 +
+						       (3 - i) * 4);
+				host->cmd->resp[i] = resp << 8;
+				if (i != 3) {
+					resp = s3c_hsmmc_readb(S3C2410_HSMMC_RSPREG0 +
+							       ((3 - i) * 4) - 1);
+					host->cmd->resp[i] |= resp;
+				}
+			}
+		} else {
+			resp = s3c_hsmmc_readl(S3C2410_HSMMC_RSPREG0);
+			printk_debug("Got a RSP: 0x%08x\n", resp);
+			host->cmd->resp[0] = resp;
+		}
+	}
+
+	host->cmd->error = 0;
+
+	printk_debug("Ending cmd (%u)\n", host->cmd->opcode);
+
+	if (host->cmd->data)
+		host->data = host->cmd->data;
+	else
+		tasklet_schedule(&host->finish_tasklet);
+
+	host->cmd = NULL;
+}
+
+static void s3c_hsmmc_tasklet_finish (unsigned long param)
+{
+	struct s3c_hsmmc_host *host;
+	unsigned long iflags;
+	struct mmc_request *mrq;
+
+	host = (struct s3c_hsmmc_host*)param;
+
+	spin_lock_irqsave(&host->lock, iflags);
+
+	del_timer(&host->timer);
+
+	mrq = host->mrq;
+
+	/*
+	 * The controller needs a reset of internal state machines
+	 * upon error conditions.
+	 */
+	if ((mrq->cmd->error != 0) ||
+		(mrq->data && (mrq->data->error != 0)) ) {
+		s3c_hsmmc_reset(host, S3C_HSMMC_RESET_CMD);
+		s3c_hsmmc_reset(host, S3C_HSMMC_RESET_DATA);
+	}
+
+	host->mrq = NULL;
+	host->cmd = NULL;
+	host->data = NULL;
+
+	s3c_hsmmc_deactivate_led(host);
+
+	mmiowb();
+	spin_unlock_irqrestore(&host->lock, iflags);
+
+	mmc_request_done(host->mmc, mrq);
+}
+
+/*****************************************************************************
+ *                                                                           *
+ * Interrupt handling                                                        *
+ *                                                                           *
+ *****************************************************************************/
+
+static void s3c_hsmmc_cmd_irq (struct s3c_hsmmc_host *host, u32 intmask)
+{
+	if (intmask & S3C_HSMMC_INT_TIMEOUT)
+		host->cmd->error = -ETIMEDOUT;
+	else if (intmask & S3C_HSMMC_INT_CRC)
+		host->cmd->error = -EILSEQ;
+	else if (intmask & (S3C_HSMMC_INT_END_BIT | S3C_HSMMC_INT_INDEX))
+		host->cmd->error = -EILSEQ;
+	else
+		host->cmd->error = -EILSEQ;
+
+	tasklet_schedule(&host->finish_tasklet);
+}
+
+static void s3c_hsmmc_data_irq (struct s3c_hsmmc_host *host, u32 intmask)
+{
+	if (!host->data) {
+		/*
+		 * A data end interrupt is sent together with the response
+		 * for the stop command.
+		 */
+		if (intmask & S3C_HSMMC_INT_DATA_END)
+			return;
+
+		printk_err("%s: Got data interrupt even though no "
+			   "data operation was in progress.\n",
+			   mmc_hostname(host->mmc));
+		return;
+	}
+
+	if (intmask & S3C_HSMMC_INT_DATA_TIMEOUT)
+		host->data->error = -ETIMEDOUT;
+	else if (intmask & S3C_HSMMC_INT_DATA_CRC)
+		host->data->error = -EILSEQ;
+	else if (intmask & S3C_HSMMC_INT_DATA_END_BIT)
+		host->data->error = -EILSEQ;
+
+	if (host->data->error != 0)
+		s3c_hsmmc_finish_data(host);
+}
+
+
+/*****************************************************************************\
+ *                                                                           *
+ * Interrupt handling                                                        *
+ *                                                                           *
+\*****************************************************************************/
+
+/*
+ * ISR for SDI Interface IRQ
+ * Communication between driver and ISR works as follows:
+ *   host->mrq 			points to current request
+ *   host->complete_what	tells the ISR when the request is considered done
+ *     COMPLETION_CMDSENT	  when the command was sent
+ *     COMPLETION_RSPFIN          when a response was received
+ *     COMPLETION_XFERFINISH	  when the data transfer is finished
+ *     COMPLETION_XFERFINISH_RSPFIN both of the above.
+ *   host->complete_request	is the completion-object the driver waits for
+ *
+ * 1) Driver sets up host->mrq and host->complete_what
+ * 2) Driver prepares the transfer
+ * 3) Driver enables interrupts
+ * 4) Driver starts transfer
+ * 5) Driver waits for host->complete_rquest
+ * 6) ISR checks for request status (errors and success)
+ * 6) ISR sets host->mrq->cmd->error and host->mrq->data->error
+ * 7) ISR completes host->complete_request
+ * 8) ISR disables interrupts
+ * 9) Driver wakes up and takes care of the request
+ */
+
+static irqreturn_t s3c_hsmmc_irq (int irq, void *dev_id)
+{
+	irqreturn_t result = 0;
+	struct s3c_hsmmc_host *host = dev_id;
+	struct mmc_request *mrq;
+	u32 intsts;
+#ifdef CONFIG_HSMMC_S3C_IRQ_WORKAROUND
+	uint i, org_irq_sts;
+#endif
+	spin_lock(&host->lock);
+
+	mrq = host->mrq;
+
+	intsts = s3c_hsmmc_readw(S3C2410_HSMMC_NORINTSTS);
+
+	/* Sometimes, hsmmc does not update its status bit immediately
+	 * when it generates irqs. by scsuh.
+	 */
+#ifdef CONFIG_HSMMC_S3C_IRQ_WORKAROUND
+	for (i=0; i<0x1000; i++) {
+		if ((intsts = s3c_hsmmc_readw(S3C2410_HSMMC_NORINTSTS)))
+			break;
+	}
+#endif
+
+	if (unlikely(!intsts)) {
+		result = IRQ_NONE;
+		goto out;
+	}
+	intsts = s3c_hsmmc_readl(S3C2410_HSMMC_NORINTSTS);
+#ifdef CONFIG_HSMMC_S3C_IRQ_WORKAROUND
+	org_irq_sts = intsts;
+#endif
+
+	printk_debug("IRQ : Status 0x%08x\n", intsts);
+#if 0
+	printk(PFX "got interrupt = 0x%08x\n", intsts);
+	printk(PFX "got mask = 0x%08x\n", s3c_hsmmc_readl(S3C2410_HSMMC_NORINTSTSEN));
+	printk(PFX "got signal = 0x%08x\n", s3c_hsmmc_readl(S3C2410_HSMMC_NORINTSIGEN));
+#endif
+
+	if (unlikely(intsts & S3C_HSMMC_INT_CARD_CHANGE)) {
+		u32 reg16;
+
+		if (intsts & S3C_HSMMC_INT_CARD_INSERT)
+			printk_debug("card inserted.\n");
+		else if (intsts & S3C_HSMMC_INT_CARD_REMOVE)
+			printk_debug("card removed.\n");
+
+		reg16 = s3c_hsmmc_readw(S3C2410_HSMMC_NORINTSTSEN);
+		s3c_hsmmc_writew(reg16 & ~S3C_HSMMC_INT_CARD_CHANGE,
+							S3C2410_HSMMC_NORINTSTSEN);
+		s3c_hsmmc_writew(S3C_HSMMC_INT_CARD_CHANGE, S3C2410_HSMMC_NORINTSTS);
+		s3c_hsmmc_writew(reg16, S3C2410_HSMMC_NORINTSTSEN);
+
+		intsts &= ~S3C_HSMMC_INT_CARD_CHANGE;
+
+		tasklet_schedule(&host->card_tasklet);
+		goto insert;
+	}
+
+	if (likely(!(intsts & S3C_HSMMC_NIS_ERR))) {
+		s3c_hsmmc_writel(intsts, S3C2410_HSMMC_NORINTSTS);
+
+		if (intsts & S3C_HSMMC_NIS_CMDCMP) {
+			printk_debug("command done\n");
+			s3c_hsmmc_finish_command(host);
+		}
+
+		if (intsts & S3C_HSMMC_NIS_TRSCMP) {
+			printk_debug("transfer done\n\n");
+			s3c_hsmmc_finish_command(host);
+			s3c_hsmmc_finish_data(host);
+			intsts &= ~S3C_HSMMC_NIS_DMA;
+		}
+
+#ifdef CONFIG_HSMMC_PSEUDO_SCATTERGATHER
+		if (intsts & S3C_HSMMC_NIS_DMA) {
+			struct s3c_hsmmc_dma_blk *dblk;
+
+			dblk = &host->dblk[host->next_blk];
+			if (dblk->original) {
+				/* on read */
+				if (host->dma_dir == DMA_FROM_DEVICE) {
+					memcpy(dblk->original,
+					       phys_to_virt(dblk->dma_address),
+					       dblk->length);
+				}
+				dma_unmap_single(NULL, dblk->dma_address, dblk->length,
+						 host->dma_dir);
+				dblk->original = 0;
+			}
+
+			host->next_blk++;
+			dblk = &host->dblk[host->next_blk];
+
+			if (host->next_blk == (host->dma_blk-1)) {
+				u32 reg;
+
+				reg = s3c_hsmmc_readl(S3C2410_HSMMC_NORINTSTSEN);
+				reg &= ~S3C_HSMMC_NIS_DMA;
+				s3c_hsmmc_writel(reg, S3C2410_HSMMC_NORINTSTSEN);
+			}
+
+			/* We do not handle DMA boundaries, so set it to max (512 KiB) */
+			s3c_hsmmc_writew((dblk->boundary<<12 | 0x200),
+					 S3C2410_HSMMC_BLKSIZE);
+			s3c_hsmmc_writel(dblk->dma_address, S3C2410_HSMMC_SYSAD);
+		}
+#endif
+	} else {
+		unsigned int interr;
+		interr = s3c_hsmmc_readl(S3C2410_HSMMC_ERRINTSTS) & 0xffff;
+
+		/* Timeout errors are printed in the tasklet, or? (Luis Galdos) */
+		if (interr != S3C_HSMMC_EIS_CMDTIMEOUT)
+			printk_err("Error detected : 0x%04x\n", interr);
+		
+		if (intsts & S3C_HSMMC_INT_CMD_MASK) {
+			s3c_hsmmc_writel(intsts & S3C_HSMMC_INT_CMD_MASK,
+					 S3C2410_HSMMC_NORINTSTS);
+			s3c_hsmmc_cmd_irq(host, intsts & S3C_HSMMC_INT_CMD_MASK);
+		}
+
+		if (intsts & S3C_HSMMC_INT_DATA_MASK) {
+			s3c_hsmmc_writel(intsts & S3C_HSMMC_INT_DATA_MASK,
+					 S3C2410_HSMMC_NORINTSTS);
+			s3c_hsmmc_finish_command(host);
+			s3c_hsmmc_data_irq(host, intsts & S3C_HSMMC_INT_DATA_MASK);
+		}
+
+		intsts &= ~(S3C_HSMMC_INT_CMD_MASK | S3C_HSMMC_INT_DATA_MASK);
+	}
+
+	/* XXX: fix later by scsuh */
+#if 0
+	if (intsts & S3C_HSMMC_INT_BUS_POWER) {
+		printk_err("%s: Card is consuming too much power!\n",
+			mmc_hostname(host->mmc));
+		s3c_hsmmc_writel(S3C_HSMMC_INT_BUS_POWER, S3C2410_HSMMC_NORINTSTS);
+	}
+
+	intsts &= S3C_HSMMC_INT_BUS_POWER;
+
+	if (intsts) {
+		printk_err("%s: Unexpected interrupt 0x%08x.\n",
+			mmc_hostname(host->mmc), intsts);
+
+		s3c_hsmmc_writel(intsts, S3C2410_HSMMC_NORINTSTS);
+	}
+#endif
+
+#ifdef CONFIG_HSMMC_S3C_IRQ_WORKAROUND
+	for (i=0; i<0x1000; i++) {
+		if (org_irq_sts != s3c_hsmmc_readl(S3C2410_HSMMC_NORINTSTS))
+			break;
+	}
+#endif
+
+insert:
+	result = IRQ_HANDLED;
+	mmiowb();
+
+out:
+	spin_unlock(&host->lock);
+
+	return result;
+}
+
+static void s3c_hsmmc_check_status (unsigned long data)
+{
+        struct s3c_hsmmc_host *host = (struct s3c_hsmmc_host *)data;
+
+	s3c_hsmmc_irq(0, host);
+}
+
+static void s3c_hsmmc_request (struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct s3c_hsmmc_host *host = mmc_priv(mmc);
+	unsigned long flags;
+
+	printk_debug("[CMD%d] arg:0x%08x flags:0x%02x retries:%u\n",
+		     mrq->cmd->opcode, mrq->cmd->arg,
+		     mrq->cmd->flags, mrq->cmd->retries);
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	WARN_ON(host->mrq != NULL);
+
+	s3c_hsmmc_activate_led(host);
+
+	host->mrq = mrq;
+
+	if (likely(s3c_hsmmc_readl(S3C2410_HSMMC_PRNSTS) & S3C_HSMMC_CARD_PRESENT)) {
+		s3c_hsmmc_send_command(host, mrq->cmd);
+	} else {
+		printk_debug("No card present? Aborting a command request.\n");
+		host->mrq->cmd->error = -ENOMEDIUM;
+		tasklet_schedule(&host->finish_tasklet);
+	}
+
+	mmiowb();
+	spin_unlock_irqrestore(&host->lock, flags);
+}
+
+/* return 0: OK
+ * return -1: error
+ */
+static int set_bus_width (struct s3c_hsmmc_host *host, uint width)
+{
+	u8 reg;
+
+	reg = s3c_hsmmc_readb(S3C2410_HSMMC_HOSTCTL);
+
+	switch (width) {
+	case MMC_BUS_WIDTH_1:
+		reg &= ~(S3C_HSMMC_CTRL_4BIT | S3C_HSMMC_CTRL_8BIT);
+		break;
+	case MMC_BUS_WIDTH_4:
+		reg |= S3C_HSMMC_CTRL_4BIT;
+		break;
+	case MMC_BUS_WIDTH_8:
+		reg |= S3C_HSMMC_CTRL_8BIT;
+		break;
+	default:
+		printk_err("Invalid bus width %u\n", width);
+		return -EINVAL;
+	}
+
+	s3c_hsmmc_writeb(reg, S3C2410_HSMMC_HOSTCTL);
+
+	printk_debug("HOSTCTL 0x%02x\n", s3c_hsmmc_readb(S3C2410_HSMMC_HOSTCTL));
+
+	return 0;
+}
+
+static void hsmmc_set_clock (struct s3c_hsmmc_host *host, ulong clock)
+{
+	uint i, j;
+	u32 val = 0, tmp_clk = 0, clk_src = 0;
+	ulong timeout;
+	u16 div = -1;
+	u8 ctrl;
+
+	/* if we already set, just out. */
+	if (clock == host->clock) {
+		printk("%p:host->clock0 : %d\n", host->base, host->clock);
+		return;
+	}
+
+	/* before setting clock, clkcon must be disabled. */
+	s3c_hsmmc_writew(0, S3C2410_HSMMC_CLKCON);
+
+	s3c_hsmmc_writeb(S3C_HSMMC_TIMEOUT_MAX, S3C2410_HSMMC_TIMEOUTCON);
+
+	/* change the edge type according to frequency */
+	ctrl = s3c_hsmmc_readb(S3C2410_HSMMC_HOSTCTL);
+	if (clock > 25000000)
+		ctrl |= S3C_HSMMC_CTRL_HIGHSPEED;
+	else
+		ctrl &= ~S3C_HSMMC_CTRL_HIGHSPEED;
+	s3c_hsmmc_writeb(ctrl, S3C2410_HSMMC_HOSTCTL);
+
+	if (clock == 0) {
+		return;
+	}
+
+	/* calculate optimal clock. by scsuh */
+	for (i=0; i < 3; i++) {
+		tmp_clk = clk_get_rate(host->clk[i]);
+		if (tmp_clk <= clock) {
+			if (tmp_clk >= val) {
+				val = tmp_clk;
+				div = 0;
+				clk_src = i+1;
+			}
+		}
+
+		for (j=0x1; j<=0x80; j <<= 1) {
+			tmp_clk = clk_get_rate(host->clk[i]) / (j<<1);
+			if ((val < tmp_clk) && (tmp_clk <= clock)) {
+				val = tmp_clk;
+				div = j;
+				clk_src = i+1;
+				break;
+			}
+		}
+	}
+
+	/* clk_src = 0x00; */
+	printk_debug("val: %d, div: %x, clk_src: %d\n", val, div, clk_src);
+
+	s3c_hsmmc_writel(0x0000c100 | (clk_src << 4), S3C2410_HSMMC_CONTROL2);
+	if (clock > 25000000)
+		/* 0x00008080 6400, 0x00800080 2443 */
+		s3c_hsmmc_writel(host->ctrl3[1], S3C2410_HSMMC_CONTROL3);
+	else
+		/* 0x80808080 6400, 0x00800080 2443 */
+		s3c_hsmmc_writel(host->ctrl3[0], S3C2410_HSMMC_CONTROL3);
+
+	s3c_hsmmc_writew(((div<<8) | S3C_HSMMC_CLOCK_INT_EN), S3C2410_HSMMC_CLKCON);
+
+	timeout = 10;
+	while (!((val = s3c_hsmmc_readw(S3C2410_HSMMC_CLKCON))
+				& S3C_HSMMC_CLOCK_INT_STABLE)) {
+		if (!timeout) {
+			printk_err("Clock stabilization: %08x | Div %u\n", val, div);
+			return;
+		}
+		timeout--;
+		mdelay(1);
+	}
+
+	s3c_hsmmc_writew(val | S3C_HSMMC_CLOCK_CARD_EN, S3C2410_HSMMC_CLKCON);
+	timeout = 10;
+	while (!((val = s3c_hsmmc_readw(S3C2410_HSMMC_CLKCON))
+				& S3C_HSMMC_CLOCK_EXT_STABLE)) {
+		if (!timeout) {
+			printk_err("Clock stabilization: %08x | Div %u\n", val, div);
+			return;
+		}
+		timeout--;
+		mdelay(1);
+	}
+
+//out:
+//	host->clock = clock;
+//	printk("%p: host->clock1 : %d\n", host->base, host->clock);
+}
+
+
+static void s3c_hsmmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct s3c_hsmmc_host *host = mmc_priv(mmc);
+	struct s3c_hsmmc_cfg *cfg = host->plat_data;
+
+	unsigned long iflags;
+
+	spin_lock_irqsave(&host->lock, iflags);
+
+
+	switch (ios->power_mode) {
+	case MMC_POWER_ON:
+	case MMC_POWER_UP:
+		s3c2410_gpio_cfgpin(S3C2443_GPL0, S3C2443_GPL0_SD0DAT0);
+
+		if (cfg->host_caps & (MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA)) {
+			s3c2410_gpio_cfgpin(S3C2443_GPL1, S3C2443_GPL1_SD0DAT1);
+			s3c2410_gpio_cfgpin(S3C2443_GPL2, S3C2443_GPL2_SD0DAT2);
+			s3c2410_gpio_cfgpin(S3C2443_GPL3, S3C2443_GPL3_SD0DAT3);
+		}
+
+		if (cfg->host_caps & MMC_CAP_8_BIT_DATA) {
+			s3c2410_gpio_cfgpin(S3C2443_GPL4, S3C2443_GPL4_SD0DAT4);
+			s3c2410_gpio_cfgpin(S3C2443_GPL5, S3C2443_GPL5_SD0DAT5);
+			s3c2410_gpio_cfgpin(S3C2443_GPL6, S3C2443_GPL6_SD0DAT6);
+			s3c2410_gpio_cfgpin(S3C2443_GPL7, S3C2443_GPL7_SD0DAT7);
+		}
+
+		s3c2410_gpio_cfgpin(S3C2443_GPL8, S3C2443_GPL8_SD0CMD);
+		s3c2410_gpio_cfgpin(S3C2443_GPL9, S3C2443_GPL9_SD0CLK);
+
+		/* Check for the dedicated GPIOs of the HSMMC-controller */
+		if (cfg->gpio_led == S3C2443_GPJ13)
+			s3c2410_gpio_cfgpin(S3C2443_GPJ13, S3C2440_GPJ13_SD0LED);
+		
+		if (cfg->gpio_detect == S3C2443_GPJ14)
+			s3c2410_gpio_cfgpin(S3C2443_GPJ14, S3C2440_GPJ14_SD0CD);
+
+		if (cfg->gpio_wprotect == S3C2443_GPJ15)
+			s3c2410_gpio_cfgpin(S3C2443_GPJ15, S3C2440_GPJ15_SD0WP);
+
+		break;
+	default:
+		break;
+	}
+
+	/*
+	 * Reset the chip on each power off.
+	 * Should clear out any weird states.
+	 */
+	if (ios->power_mode == MMC_POWER_OFF) {
+		s3c_hsmmc_writew(0, S3C2410_HSMMC_NORINTSIGEN);
+		s3c_hsmmc_ios_init(host);
+	}
+
+	if (host->plat_data->set_gpio)
+		host->plat_data->set_gpio();
+
+	printk_debug("Clock: %d | Bus width %d\n", ios->clock, ios->bus_width);
+	hsmmc_set_clock(host, ios->clock);
+	set_bus_width(host, ios->bus_width);
+
+	printk_debug("CTRL2 0x%08x | CTRL3 0x%08x | CLKCON 0x%04x\n",
+		     s3c_hsmmc_readl(S3C2410_HSMMC_CONTROL2),
+		     s3c_hsmmc_readl(S3C2410_HSMMC_CONTROL3),
+		     s3c_hsmmc_readw(S3C2410_HSMMC_CLKCON));
+
+	if (ios->power_mode == MMC_POWER_OFF)
+		s3c_hsmmc_writeb(S3C_HSMMC_POWER_OFF, S3C2410_HSMMC_PWRCON);
+	else
+		s3c_hsmmc_writeb(S3C_HSMMC_POWER_ON_ALL, S3C2410_HSMMC_PWRCON);
+
+	udelay(1000);
+	spin_unlock_irqrestore(&host->lock, iflags);
+}
+
+
+
+/*
+ * If no GPIO was configured for the write protect, then assume that the card
+ * is NOT write protected
+ */
+static int s3c_hsmmc_get_ro(struct mmc_host *mmc)
+{
+	struct s3c_hsmmc_host *host = mmc_priv(mmc);
+	struct s3c_hsmmc_cfg *cfg = host->plat_data;
+	unsigned int retval;
+	
+	/* Depending on the configured GPIO get the RO-value */
+	if (cfg->gpio_wprotect == S3C2443_GPJ15) {
+		retval = s3c_hsmmc_readl(S3C2410_HSMMC_PRNSTS);
+		retval &= S3C_HSMMC_WRITE_PROTECT; 
+	} else if (cfg->gpio_wprotect) {
+		retval = s3c2410_gpio_getpin(cfg->gpio_wprotect);
+		retval = cfg->wprotect_invert ? !retval : retval;
+	} else
+		retval = 0;
+			
+	return retval;
+}
+
+
+static struct mmc_host_ops s3c_hsmmc_ops = {
+	.request	= s3c_hsmmc_request,
+	.set_ios	= s3c_hsmmc_set_ios,
+	.get_ro		= s3c_hsmmc_get_ro,
+};
+
+
+
+static int s3c_hsmmc_probe (struct platform_device *pdev)
+{
+	struct mmc_host *mmc;
+	struct s3c_hsmmc_host *host;
+	struct s3c_hsmmc_cfg *plat_data;
+	uint i;
+	int ret;
+
+	mmc = mmc_alloc_host(sizeof(struct s3c_hsmmc_host), &pdev->dev);
+	if (!mmc)
+		ret = -ENOMEM;
+
+	plat_data = s3c_hsmmc_get_platdata(&pdev->dev);
+
+	host = mmc_priv(mmc);
+
+	host->mmc = mmc;
+	host->plat_data = plat_data;
+	platform_set_drvdata(pdev, mmc);
+
+	host->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!host->mem) {
+		printk_err("Failed to get io memory region resouce.\n");
+		ret = -ENOENT;
+		goto probe_free_host;
+	}
+
+	host->mem = request_mem_region(host->mem->start,
+				       RESSIZE(host->mem), pdev->name);
+	if (!host->mem) {
+		printk_err("Failed to request io memory region.\n");
+		ret = -ENOENT;
+		goto probe_free_host;
+	}
+
+	host->base = ioremap(host->mem->start, RESSIZE(host->mem));
+	if (!host->base) {
+		printk_err("Failed to ioremap() the region %p\n", host->mem);
+		ret = -EINVAL;
+		goto err_free_mem_region;
+	}
+	
+	host->irq = platform_get_irq(pdev, 0);
+	if (host->irq == 0) {
+		printk("failed to get interrupt resouce.\n");
+		ret = -EINVAL;
+		goto untasklet;
+	}
+
+	host->flags |= S3C_HSMMC_USE_DMA;
+
+	/* every platform has different ctrl3 value. by scsuh */
+	host->ctrl3[0] = plat_data->ctrl3[0];
+	host->ctrl3[1] = plat_data->ctrl3[1];
+
+#ifdef CONFIG_HSMMC_PSEUDO_SCATTERGATHER
+	for (i=0; i<S3C_HSMMC_MAX_SUB_BUF; i++)
+		host->sub_block[i] = kmalloc(S3C_HSMMC_MALLOC_SIZE, GFP_KERNEL);
+#endif
+
+	s3c_hsmmc_reset(host, S3C_HSMMC_RESET_ALL);
+
+#ifdef CONFIG_ARCH_S3C6400
+	clk_enable(clk_get(&pdev->dev, "sclk_48m"));
+#endif
+
+	/* register 3 clock source if exist */
+	for (i=0; i<3; i++) {
+		host->clk[i] = clk_get(&pdev->dev, plat_data->clk_name[i]);
+		if (IS_ERR(host->clk[i])) {
+			ret = PTR_ERR(host->clk[i]);
+			host->clk[i] = ERR_PTR(-ENOENT);
+		}
+
+		if (clk_enable(host->clk[i])) {
+			host->clk[i] = ERR_PTR(-ENOENT);
+		}
+	}
+
+	mmc->ops = &s3c_hsmmc_ops;
+	mmc->ocr_avail = MMC_VDD_32_33|MMC_VDD_33_34;
+	mmc->f_min = 400 * 1000; /* at least 400kHz */
+
+	/* you must make sure that our hsmmc block can support
+	 * up to 52MHz. by scsuh
+	 */
+	mmc->f_max = 100 * MHZ;
+	mmc->caps = plat_data->host_caps;
+	printk_debug("mmc->caps: %08x\n", (unsigned int)mmc->caps);
+
+	spin_lock_init(&host->lock);
+
+	/*
+	 * Maximum number of segments. Hardware cannot do scatter lists.
+	 * XXX: must modify later. by scsuh
+	 */
+#ifdef CONFIG_HSMMC_PSEUDO_SCATTERGATHER
+	mmc->max_hw_segs = CONFIG_S3C_HSMMC_MAX_HW_SEGS;
+	mmc->max_phys_segs = CONFIG_S3C_HSMMC_MAX_HW_SEGS;
+#else
+	mmc->max_hw_segs = 1;
+#endif
+
+	/*
+	 * Maximum number of sectors in one transfer. Limited by DMA boundary
+	 * size (512KiB), which means (512 KiB/512=) 1024 entries.
+	 */
+	mmc->max_blk_count = 1024;
+
+	/*
+	 * Maximum segment size. Could be one segment with the maximum number
+	 * of sectors.
+	 */
+	mmc->max_seg_size = mmc->max_blk_count * 512;
+	mmc->max_req_size = mmc->max_blk_count * 512;
+
+	init_timer(&host->timer);
+        host->timer.data = (unsigned long)host;
+        host->timer.function = s3c_hsmmc_check_status;
+        host->timer.expires = jiffies + HZ;
+
+	/*
+	 * Init tasklets.
+	 */
+	tasklet_init(&host->card_tasklet,
+		s3c_hsmmc_tasklet_card, (unsigned long)host);
+	tasklet_init(&host->finish_tasklet,
+		s3c_hsmmc_tasklet_finish, (unsigned long)host);
+
+	ret = request_irq(host->irq, s3c_hsmmc_irq, 0, DRIVER_NAME, host);
+	if (ret)
+		goto untasklet;
+
+	s3c_hsmmc_ios_init(host);
+
+	mmc_add_host(mmc);
+
+#ifdef CONFIG_HSMMC_PROC_DATA
+	global_host[plat_data->hwport] = host;
+#endif
+
+	printk(KERN_INFO "%s.%d: at 0x%p with irq %d. clk src:",
+			pdev->name, pdev->id, host->base, host->irq);
+	for (i=0; i<3; i++) {
+		if (!IS_ERR(host->clk[i]))
+			printk(" %s", plat_data->clk_name[i]);
+	}
+	printk("\n");
+
+	return 0;
+
+ untasklet:
+	tasklet_kill(&host->card_tasklet);
+	tasklet_kill(&host->finish_tasklet);
+
+	for (i=0; i<3; i++) {
+		clk_disable(host->clk[i]);
+		clk_put(host->clk[i]);
+	}
+
+ err_free_mem_region:
+	release_mem_region(host->mem->start, RESSIZE(host->mem));
+	
+ probe_free_host:
+	mmc_free_host(mmc);
+	platform_set_drvdata(pdev, NULL);
+	
+	return ret;
+}
+
+static int s3c_hsmmc_remove(struct platform_device *dev)
+{
+	struct mmc_host *mmc  = platform_get_drvdata(dev);
+	struct s3c_hsmmc_host *host = mmc_priv(mmc);
+	int i;
+
+	printk_debug("Removing the MMC %p | S3C host %p\n", mmc, host);
+	
+	mmc_remove_host(mmc);
+
+	/* Reset the MMC controller */
+	s3c_hsmmc_reset(host, S3C_HSMMC_RESET_ALL);
+
+#ifdef CONFIG_ARCH_S3C6400
+	clk_disable(clk_get(&dev->dev, "sclk_48m"));
+#endif
+
+	/* Free only the requested clocks */
+	for (i =0; i< 3; i++) {
+		if (!PTR_ERR(host->clk[i])) {
+			clk_disable(host->clk[i]);
+			clk_put(host->clk[i]);
+		}
+	}
+
+	/* Free the IRQ and mapped memory */
+	free_irq(host->irq, host);
+	iounmap(host->base);
+	release_mem_region(host->mem->start, RESSIZE(host->mem));
+	
+	del_timer_sync(&host->timer);
+
+	tasklet_kill(&host->card_tasklet);
+	tasklet_kill(&host->finish_tasklet);
+
+#ifdef CONFIG_HSMMC_PSEUDO_SCATTERGATHER
+	kfree(host->sub_block);
+#endif
+	mmc_free_host(mmc);
+
+	return 0;
+}
+
+/* This is for the power management support */
+#ifdef CONFIG_PM
+static int s3c_hsmmc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct mmc_host *mmc;
+	struct s3c_hsmmc_host *host;
+	int cnt, retval;
+
+	mmc = platform_get_drvdata(pdev);
+	host = mmc_priv(mmc);
+
+	/* Check for all the three available clocks to disable */
+	for (cnt = 0; cnt < 3; cnt++) {
+		if (!PTR_ERR(host->clk[cnt]))
+			clk_disable(host->clk[cnt]);
+	}
+
+	retval = mmc_suspend_host(mmc, state);
+
+	return retval;
+}
+
+static int s3c_hsmmc_resume(struct platform_device *pdev)
+{
+	struct mmc_host *mmc;
+	struct s3c_hsmmc_host *host;
+	int cnt, retval;
+
+	mmc = platform_get_drvdata(pdev);
+	host = mmc_priv(mmc);
+	
+	/* Reenable the clocks first */
+	for (cnt = 0; cnt < 3; cnt++) {
+		if (!PTR_ERR(host->clk[cnt]))
+			clk_enable(host->clk[cnt]);
+	}
+
+	s3c_hsmmc_ios_init(host);
+
+	 /*
+	  * By unsafe resumes we MUST check the card state at this point, then the
+	  * higher MMC-layer is probably transferring some kind of data to the
+	  * block device that doesn't exist any more.
+	  */
+#if defined(CONFIG_MMC_UNSAFE_RESUME)
+	if (s3c_hsmmc_readl(S3C2410_HSMMC_PRNSTS) & S3C_HSMMC_CARD_PRESENT)
+		retval = mmc_resume_host(mmc);
+	else {
+		retval = 0;
+		mmc_detect_change(mmc, msecs_to_jiffies(500));
+	}
+#else
+	retval = mmc_resume_host(mmc);	
+#endif /* CONFIG_MMC_UNSAFE_RESUME */
+	
+	return retval;
+}
+#else
+#define s3c_hsmmc_suspend				NULL
+#define s3c_hsmmc_resume				NULL
+#endif
+
+static struct platform_driver s3c_hsmmc_driver = {
+	.probe          = s3c_hsmmc_probe,
+	.remove         = s3c_hsmmc_remove,
+	.suspend	= s3c_hsmmc_suspend,
+	.resume		= s3c_hsmmc_resume,
+	.driver		= {
+		.name	= "s3c-hsmmc",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init s3c_hsmmc_drv_init(void)
+{
+	return platform_driver_register(&s3c_hsmmc_driver);
+}
+
+static void __exit s3c_hsmmc_drv_exit(void)
+{
+	platform_driver_unregister(&s3c_hsmmc_driver);
+}
+
+#ifdef CONFIG_HSMMC_PROC_DATA
+static struct proc_dir_entry *evb_resource_dump;
+
+
+static int mem_proc_read (char *buffer, char **buffer_location, off_t offset,
+			  int buffer_length, int *zero, void *ptr)
+{
+	return 0;
+}
+
+
+static int mem_proc_write(struct file *file, const char *buffer,
+			  unsigned long count, void *data)
+{
+	uint i, range = 0;
+	char flag = 'l';
+	uint port = 0;
+
+#if 0
+	printk("buffers: %s\n", buffer);
+#endif
+	sscanf(buffer, "%d %c", &port, &flag);
+
+	switch (flag) {
+	case 'c':
+		printk("clean %d port\n", port);
+		for (i=0; i<7; i++) {
+			global_host[port]->rx_pkt[i] = 0;
+			global_host[port]->tx_pkt[i] = 0;
+		}
+//		break;
+
+	case 'l':
+		printk("\t\t\trx\ttx\n");
+		for (i=0; i<7; i++) {
+			printk("0x%05x ~ 0x%05x\t%-7d\t%-7d\n",
+			       range, 0x1000 << i,
+			       global_host[port]->rx_pkt[i],
+			       global_host[port]->tx_pkt[i]);
+			range = 0x1000 << i;
+
+		}
+		break;
+	}
+
+	return count;
+}
+
+int __init mem_proc_scsuh (void)
+{
+	evb_resource_dump = create_proc_entry("mmc", 0666, &proc_root);
+	evb_resource_dump->read_proc = mem_proc_read;
+	evb_resource_dump->write_proc = mem_proc_write;
+	evb_resource_dump->nlink = 1;
+	return 0;
+}
+
+module_init(mem_proc_scsuh);
+#endif
+
+module_init(s3c_hsmmc_drv_init);
+module_exit(s3c_hsmmc_drv_exit);
+
+
+MODULE_DESCRIPTION("S3C SD HOST I/F 1.0 Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mmc/host/s3c-hsmmc.h b/drivers/mmc/host/s3c-hsmmc.h
new file mode 100644
index 000000000000..ce12f2ebf619
--- /dev/null
+++ b/drivers/mmc/host/s3c-hsmmc.h
@@ -0,0 +1,237 @@
+/*
+ *  linux/drivers/mmc/s3c-hsmmc.h - Samsung S3C SDI Interface driver
+ *
+ * $Id: s3c-hsmmc.h,v 1.9 2007/06/07 07:14:57 scsuh Exp $
+ *
+ *  Copyright (C) 2004 Thomas Kleffel, 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef MHZ
+#define MHZ (1000*1000)
+#endif
+
+#define s3c_hsmmc_readl(x)	readl((host->base)+(x))
+#define s3c_hsmmc_readw(x)	readw((host->base)+(x))
+#define s3c_hsmmc_readb(x)	readb((host->base)+(x))
+
+#define s3c_hsmmc_writel(v,x)	writel((v),(host->base)+(x))
+#define s3c_hsmmc_writew(v,x)	writew((v),(host->base)+(x))
+#define s3c_hsmmc_writeb(v,x)	writeb((v),(host->base)+(x))
+
+#define S3C_HSMMC_CLOCK_ON	1
+#define S3C_HSMMC_CLOCK_OFF	0
+
+#define	SPEED_NORMAL	0
+#define	SPEED_HIGH	1
+
+/*
+ * Controller registers
+ */
+
+//S3C_HSMMC_BLOCK_SIZE:		0x04
+#define S3C_HSMMC_MAKE_BLKSZ(dma, blksz) (((dma & 0x7) << 12) | (blksz & 0xFFF))
+
+//S3C_HSMMC_TRANSFER_MODE:	0x0C
+#define S3C_HSMMC_TRNS_DMA		0x01
+#define S3C_HSMMC_TRNS_BLK_CNT_EN	0x02
+#define S3C_HSMMC_TRNS_ACMD12		0x04
+#define S3C_HSMMC_TRNS_READ		0x10
+#define S3C_HSMMC_TRNS_MULTI		0x20
+
+//S3C_HSMMC_COMMAND		0x0E
+#define S3C_HSMMC_CMD_RESP_MASK		0x03
+#define S3C_HSMMC_CMD_CRC		0x08
+#define S3C_HSMMC_CMD_INDEX		0x10
+#define S3C_HSMMC_CMD_DATA		0x20
+
+#define S3C_HSMMC_CMD_RESP_NONE		0x00
+#define S3C_HSMMC_CMD_RESP_LONG		0x01
+#define S3C_HSMMC_CMD_RESP_SHORT	0x02
+#define S3C_HSMMC_CMD_RESP_SHORT_BUSY	0x03
+
+#define S3C_HSMMC_MAKE_CMD(c, f) (((c & 0xff) << 8) | (f & 0xff))
+
+//S3C_HSMMC_PRESENT_STATE	0x24
+#define S3C_HSMMC_CMD_INHIBIT		0x00000001
+#define S3C_HSMMC_DATA_INHIBIT		0x00000002
+#define S3C_HSMMC_DOING_WRITE		0x00000100
+#define S3C_HSMMC_DOING_READ		0x00000200
+#define S3C_HSMMC_SPACE_AVAILABLE	0x00000400
+#define S3C_HSMMC_DATA_AVAILABLE	0x00000800
+#define S3C_HSMMC_CARD_PRESENT		0x00010000
+#define S3C_HSMMC_WRITE_PROTECT		0x00080000
+
+//S3C_HSMMC_HOST_CONTROL	0x28
+#define S3C_HSMMC_CTRL_LED		(0x01)
+#define S3C_HSMMC_CTRL_4BITBUS		(0x02)
+#define S3C_HSMMC_CTRL_HIGHSPEED	(0x04)
+#define S3C_HSMMC_CTRL_4BIT		(0x02)
+#define S3C_HSMMC_CTRL_8BIT		(0x20)
+
+//S3C_HSMMC_POWER_CONTROL	0x29
+#define S3C_HSMMC_POWER_OFF		0x00
+#define S3C_HSMMC_POWER_ON		0x01
+#define S3C_HSMMC_POWER_180		0x0A
+#define S3C_HSMMC_POWER_300		0x0C
+#define S3C_HSMMC_POWER_330		0x0E
+#define S3C_HSMMC_POWER_ON_ALL		0xFF
+
+//S3C_HSMMC_CLOCK_CONTROL	0x2C
+#define S3C_HSMMC_DIVIDER_SHIFT		8
+#define S3C_HSMMC_CLOCK_EXT_STABLE	0x0008
+#define S3C_HSMMC_CLOCK_CARD_EN		0x0004
+#define S3C_HSMMC_CLOCK_INT_STABLE	0x0002
+#define S3C_HSMMC_CLOCK_INT_EN		0x0001
+
+//S3C_HSMMC_TIMEOUT_CONTROL	0x2E
+#define S3C_HSMMC_TIMEOUT_MAX		0x0E
+
+//S3C_HSMMC_SOFTWARE_RESET	0x2F
+#define S3C_HSMMC_RESET_ALL		0x01
+#define S3C_HSMMC_RESET_CMD		0x02
+#define S3C_HSMMC_RESET_DATA		0x04
+
+//S3C_HSMMC_INT_STATUS	0x30
+#define S3C_HSMMC_NIS_ERR		0x00008000
+#define S3C_HSMMC_NIS_CMDCMP		0x00000001
+#define S3C_HSMMC_NIS_TRSCMP		0x00000002
+#define S3C_HSMMC_NIS_DMA		0x00000008
+
+//S3C_HSMMC_EIS_STATUS	0x32
+#define S3C_HSMMC_EIS_CMDTIMEOUT	0x00000001
+#define S3C_HSMMC_EIS_CMDERR		0x0000000E
+#define S3C_HSMMC_EIS_DATATIMEOUT	0x00000010
+#define S3C_HSMMC_EIS_DATAERR		0x00000060
+#define S3C_HSMMC_EIS_CMD12ERR		0x00000100
+
+
+//S3C_HSMMC_SIGNAL_ENABLE	0x38
+#define S3C_HSMMC_INT_MASK_ALL		0x0000
+#define S3C_HSMMC_INT_RESPONSE		0x00000001
+#define S3C_HSMMC_INT_DATA_END		0x00000002
+#define S3C_HSMMC_INT_DMA_END		0x00000008
+#define S3C_HSMMC_INT_SPACE_AVAIL	0x00000010
+#define S3C_HSMMC_INT_DATA_AVAIL	0x00000020
+#define S3C_HSMMC_INT_CARD_INSERT	0x00000040
+#define S3C_HSMMC_INT_CARD_REMOVE	0x00000080
+#define S3C_HSMMC_INT_CARD_CHANGE	0x000000c0 /* oring of above two */
+#define S3C_HSMMC_INT_CARD_INT		0x00000100
+#define S3C_HSMMC_INT_TIMEOUT		0x00010000
+#define S3C_HSMMC_INT_CRC		0x00020000
+#define S3C_HSMMC_INT_END_BIT		0x00040000
+#define S3C_HSMMC_INT_INDEX		0x00080000
+#define S3C_HSMMC_INT_DATA_TIMEOUT	0x00100000
+#define S3C_HSMMC_INT_DATA_CRC		0x00200000
+#define S3C_HSMMC_INT_DATA_END_BIT	0x00400000
+#define S3C_HSMMC_INT_BUS_POWER		0x00800000
+#define S3C_HSMMC_INT_ACMD12ERR		0x01000000
+
+#define S3C_HSMMC_INT_NORMAL_MASK	0x00007FFF
+#define S3C_HSMMC_INT_ERROR_MASK	0xFFFF8000
+
+#define S3C_HSMMC_INT_CMD_MASK	(S3C_HSMMC_INT_RESPONSE | \
+				 S3C_HSMMC_INT_TIMEOUT | \
+				 S3C_HSMMC_INT_CRC | \
+				 S3C_HSMMC_INT_END_BIT | \
+				 S3C_HSMMC_INT_INDEX | \
+				 S3C_HSMMC_NIS_ERR \
+				)
+#define S3C_HSMMC_INT_DATA_MASK	(S3C_HSMMC_INT_DATA_END | \
+				 S3C_HSMMC_INT_DMA_END | \
+				 S3C_HSMMC_INT_DATA_AVAIL | \
+				 S3C_HSMMC_INT_SPACE_AVAIL | \
+				 S3C_HSMMC_INT_DATA_TIMEOUT | \
+				 S3C_HSMMC_INT_DATA_CRC | \
+				 S3C_HSMMC_INT_DATA_END_BIT \
+				)
+
+//S3C_HSMMC_CAPABILITIES	0x40
+#define S3C_HSMMC_TIMEOUT_CLK_MASK	0x0000003F
+#define S3C_HSMMC_TIMEOUT_CLK_SHIFT	0
+#define S3C_HSMMC_TIMEOUT_CLK_UNIT	0x00000080
+#define S3C_HSMMC_CLOCK_BASE_MASK	0x00003F00
+#define S3C_HSMMC_CLOCK_BASE_SHIFT	8
+#define S3C_HSMMC_MAX_BLOCK_MASK	0x00030000
+#define S3C_HSMMC_MAX_BLOCK_SHIFT	16
+#define S3C_HSMMC_CAN_DO_DMA		0x00400000
+#define S3C_HSMMC_CAN_VDD_330		0x01000000
+#define S3C_HSMMC_CAN_VDD_300		0x02000000
+#define S3C_HSMMC_CAN_VDD_180		0x04000000
+
+//S3C_HSMMC_HOST_VERSION	0xFE
+#define S3C_HSMMC_VENDOR_VER_MASK	0xFF00
+#define S3C_HSMMC_VENDOR_VER_SHIFT	8
+#define S3C_HSMMC_SPEC_VER_MASK		0x00FF
+#define S3C_HSMMC_SPEC_VER_SHIFT	0
+
+#ifndef CONFIG_S3C_HSMMC_MAX_HW_SEGS
+#define CONFIG_S3C_HSMMC_MAX_HW_SEGS	32
+#endif
+
+struct s3c_hsmmc_dma_blk {
+	dma_addr_t	dma_address;	/* dma address			*/
+	uint		length;		/* length			*/
+	uint		boundary;	/* Host DMA Buffer Boundary	*/
+	void		*original;
+};
+
+struct s3c_hsmmc_host {
+	void __iomem		*base;
+	struct mmc_request	*mrq;
+	struct mmc_command	*cmd;
+	struct mmc_data		*data;
+	struct mmc_host		*mmc;
+	struct clk		*clk[3];
+	struct resource		*mem;
+
+	struct timer_list	timer;
+
+	struct s3c_hsmmc_cfg	*plat_data;
+
+	int			irq;
+	spinlock_t		lock;
+
+	struct tasklet_struct	card_tasklet;	/* Tasklet structures	*/
+	struct tasklet_struct	finish_tasklet;
+
+	unsigned int		clock;		/* Current clock (MHz)	*/
+	unsigned int		ctrl3[2];	/* 0: normal, 1: high	*/
+
+#define S3C_HSMMC_USE_DMA	(1<<0)
+	int			flags;		/* Host attributes	*/
+
+	uint			dma_dir;
+#ifdef CONFIG_HSMMC_PSEUDO_SCATTERGATHER
+	uint			sg_len;		/* size of scatter list	*/
+	uint			dma_blk;	/* total dmablk number	*/
+	uint			next_blk;	/* next block to send	*/
+	struct s3c_hsmmc_dma_blk dblk[CONFIG_S3C_HSMMC_MAX_HW_SEGS*4];
+
+	/* when pseudo algo cannot deal with sglist */
+#define S3C_HSMMC_MALLOC_SIZE	PAGE_SIZE
+#define S3C_HSMMC_MAX_SUB_BUF	CONFIG_S3C_HSMMC_MAX_HW_SEGS
+	void			*sub_block[S3C_HSMMC_MAX_SUB_BUF];
+#endif
+
+#ifdef CONFIG_HSMMC_PROC_DATA
+	unsigned int		rx_pkt[7];	/* 0 ~ 0x1000		*/
+						/* 0x1000 ~ 0x2000	*/
+						/* 0x2000 ~ 0x3000	*/
+						/* 0x3000 ~ 0x4000	*/
+						/* 0x4000 ~ 0x8000	*/
+						/* 0x8000 ~ 0x10000	*/
+						/* 0x10000 ~		*/
+	unsigned int		tx_pkt[7];	/* 0 ~ 0x1000		*/
+						/* 0x1000 ~ 0x2000	*/
+						/* 0x2000 ~ 0x3000	*/
+						/* 0x3000 ~ 0x4000	*/
+						/* 0x4000 ~ 0x8000	*/
+						/* 0x8000 ~ 0x10000	*/
+						/* 0x10000 ~		*/
+#endif
+};
+
diff --git a/drivers/mmc/host/s3cmci.c b/drivers/mmc/host/s3cmci.c
index 3b2085b57769..5287be0d590a 100644
--- a/drivers/mmc/host/s3cmci.c
+++ b/drivers/mmc/host/s3cmci.c
@@ -3,9 +3,6 @@
  *
  *  Copyright (C) 2004-2006 maintech GmbH, Thomas Kleffel <tk@maintech.de>
  *
- * Current driver maintained by Ben Dooks and Simtec Electronics
- *  Copyright (C) 2008 Simtec Electronics <ben-linux@fluff.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.
@@ -16,14 +13,17 @@
 #include <linux/clk.h>
 #include <linux/mmc/host.h>
 #include <linux/platform_device.h>
-#include <linux/cpufreq.h>
 #include <linux/irq.h>
 #include <linux/io.h>
-
+#include <linux/mmc/mmc.h>
 #include <asm/dma.h>
 
+/* Use the old headers system */
 #include <mach/regs-sdi.h>
 #include <mach/regs-gpio.h>
+#include <mach/gpio.h>
+#include <asm/delay.h>
+#include <linux/delay.h>
 
 #include <asm/plat-s3c24xx/mci.h>
 
@@ -31,6 +31,22 @@
 
 #define DRIVER_NAME "s3c-mci"
 
+#define printk_err(fmt, args...)                printk(KERN_ERR "[ ERROR ] s3c2443-sdi: " fmt, ## args)
+#define printk_info(fmt, args...)               printk(KERN_INFO "s3c2443-sdi: " fmt, ## args)
+#define printk_dbg(fmt, args...)                printk(KERN_DEBUG "s3c2443-sdi: " fmt, ## args)
+
+/* Enable/disable the debug messages */
+#if 0
+#define S3C2443_SDI_DEBUG
+#endif
+
+#ifdef S3C2443_SDI_DEBUG
+#  define printk_debug(fmt, args...)		printk(KERN_DEBUG "s3c2443-sdi: %s() " fmt, __FUNCTION__ , ## args)
+#else
+#  define printk_debug(fmt, args...)
+#endif
+
+
 enum dbg_channels {
 	dbg_err   = (1 << 0),
 	dbg_debug = (1 << 1),
@@ -43,9 +59,9 @@ enum dbg_channels {
 	dbg_conf  = (1 << 8),
 };
 
-static const int dbgmap_err   = dbg_fail;
+static const int dbgmap_err   = dbg_err | dbg_fail;
 static const int dbgmap_info  = dbg_info | dbg_conf;
-static const int dbgmap_debug = dbg_err | dbg_debug;
+static const int dbgmap_debug = dbg_debug;
 
 #define dbg(host, channels, args...)		  \
 	do {					  \
@@ -189,11 +205,17 @@ static inline u32 disable_imask(struct s3cmci_host *host, u32 imask)
 
 static inline void clear_imask(struct s3cmci_host *host)
 {
-	writel(0, host->base + host->sdiimsk);
+	ulong imsk, to_write = 0;
+       
+	imsk = readl(host->base + host->sdiimsk);
+	if ((imsk & S3C2410_SDIIMSK_SDIOIRQ) && host->sdio_irq)
+		to_write = S3C2410_SDIIMSK_SDIOIRQ;
+	
+	writel(to_write, host->base + host->sdiimsk);
 }
 
 static inline int get_data_buffer(struct s3cmci_host *host,
-				  u32 *bytes, u32 **pointer)
+				  u32 *words, u32 **pointer)
 {
 	struct scatterlist *sg;
 
@@ -210,7 +232,7 @@ static inline int get_data_buffer(struct s3cmci_host *host,
 	}
 	sg = &host->mrq->data->sg[host->pio_sgptr];
 
-	*bytes = sg->length;
+	*words = sg->length >> 2;
 	*pointer = sg_virt(sg);
 
 	host->pio_sgptr++;
@@ -226,7 +248,7 @@ static inline u32 fifo_count(struct s3cmci_host *host)
 	u32 fifostat = readl(host->base + S3C2410_SDIFSTA);
 
 	fifostat &= S3C2410_SDIFSTA_COUNTMASK;
-	return fifostat;
+	return fifostat >> 2;
 }
 
 static inline u32 fifo_free(struct s3cmci_host *host)
@@ -234,15 +256,13 @@ static inline u32 fifo_free(struct s3cmci_host *host)
 	u32 fifostat = readl(host->base + S3C2410_SDIFSTA);
 
 	fifostat &= S3C2410_SDIFSTA_COUNTMASK;
-	return 63 - fifostat;
+	return (63 - fifostat) >> 2;
 }
 
 static void do_pio_read(struct s3cmci_host *host)
 {
 	int res;
 	u32 fifo;
-	u32 *ptr;
-	u32 fifo_words;
 	void __iomem *from_ptr;
 
 	/* write real prescaler to host, it might be set slow to fix */
@@ -273,35 +293,14 @@ static void do_pio_read(struct s3cmci_host *host)
 		    fifo, host->pio_bytes,
 		    readl(host->base + S3C2410_SDIDCNT));
 
-		/* If we have reached the end of the block, we can
-		 * read a word and get 1 to 3 bytes.  If we in the
-		 * middle of the block, we have to read full words,
-		 * otherwise we will write garbage, so round down to
-		 * an even multiple of 4. */
-		if (fifo >= host->pio_bytes)
+		if (fifo > host->pio_bytes)
 			fifo = host->pio_bytes;
-		else
-			fifo -= fifo & 3;
 
 		host->pio_bytes -= fifo;
 		host->pio_count += fifo;
 
-		fifo_words = fifo >> 2;
-		ptr = host->pio_ptr;
-		while (fifo_words--)
-			*ptr++ = readl(from_ptr);
-		host->pio_ptr = ptr;
-
-		if (fifo & 3) {
-			u32 n = fifo & 3;
-			u32 data = readl(from_ptr);
-			u8 *p = (u8 *)host->pio_ptr;
-
-			while (n--) {
-				*p++ = data;
-				data >>= 8;
-			}
-		}
+		while (fifo--)
+			*(host->pio_ptr++) = readl(from_ptr);
 	}
 
 	if (!host->pio_bytes) {
@@ -325,7 +324,6 @@ static void do_pio_write(struct s3cmci_host *host)
 	void __iomem *to_ptr;
 	int res;
 	u32 fifo;
-	u32 *ptr;
 
 	to_ptr = host->base + host->sdidata;
 
@@ -347,23 +345,14 @@ static void do_pio_write(struct s3cmci_host *host)
 
 		}
 
-		/* If we have reached the end of the block, we have to
-		 * write exactly the remaining number of bytes.  If we
-		 * in the middle of the block, we have to write full
-		 * words, so round down to an even multiple of 4. */
-		if (fifo >= host->pio_bytes)
+		if (fifo > host->pio_bytes)
 			fifo = host->pio_bytes;
-		else
-			fifo -= fifo & 3;
 
 		host->pio_bytes -= fifo;
 		host->pio_count += fifo;
 
-		fifo = (fifo + 3) >> 2;
-		ptr = host->pio_ptr;
 		while (fifo--)
-			writel(*ptr++, to_ptr);
-		host->pio_ptr = ptr;
+			writel(*(host->pio_ptr++), to_ptr);
 	}
 
 	enable_imask(host, S3C2410_SDIIMSK_TXFIFOHALF);
@@ -399,6 +388,17 @@ static void pio_tasklet(unsigned long data)
 		enable_irq(host->irq);
 }
 
+/* The DMA-callback will call this function by errors or transfer completes */
+static void dma_tasklet(unsigned long data)
+{
+	struct s3cmci_host *host = (struct s3cmci_host *) data;
+
+	if (host->complete_what == COMPLETION_FINALIZE) {
+		clear_imask(host);
+		finalize_request(host);
+	}
+}
+
 /*
  * ISR for SDI Interface IRQ
  * Communication between driver and ISR works as follows:
@@ -444,6 +444,15 @@ static irqreturn_t s3cmci_irq(int irq, void *dev_id)
 	mci_cclear = 0;
 	mci_dclear = 0;
 
+	printk_debug("IRQ: cmd 0x%08x | dsta 0x%08x | imsk 0x%08x\n",
+		     mci_csta, mci_dsta, mci_imsk);
+
+	if (mci_dsta & S3C2410_SDIDSTA_SDIOIRQDETECT) {
+		printk_debug("SDIO IRQ detected\n");
+		mmc_signal_sdio_irq(host->mmc);
+		writel(S3C2410_SDIDSTA_SDIOIRQDETECT, host->base + S3C2410_SDIDSTA);
+	}
+	
 	if ((host->complete_what == COMPLETION_NONE) ||
 	    (host->complete_what == COMPLETION_FINALIZE)) {
 		host->status = "nothing to complete";
@@ -487,19 +496,22 @@ static irqreturn_t s3cmci_irq(int irq, void *dev_id)
 	}
 
 	if (mci_csta & S3C2410_SDICMDSTAT_CMDTIMEOUT) {
-		dbg(host, dbg_err, "CMDSTAT: error CMDTIMEOUT\n");
+		dbg(host, dbg_debug, "CMDSTAT: error CMDTIMEOUT\n");
 		cmd->error = -ETIMEDOUT;
 		host->status = "error: command timeout";
 		goto fail_transfer;
 	}
 
 	if (mci_csta & S3C2410_SDICMDSTAT_CMDSENT) {
+
+		/* @FIXME: Move the write-command to a correct place! (Luis G.) */
+		mci_cclear |= S3C2410_SDICMDSTAT_CMDSENT;
+		writel(mci_cclear, host->base + S3C2410_SDICMDSTAT);
+		
 		if (host->complete_what == COMPLETION_CMDSENT) {
 			host->status = "ok: command sent";
 			goto close_transfer;
 		}
-
-		mci_cclear |= S3C2410_SDICMDSTAT_CMDSENT;
 	}
 
 	if (mci_csta & S3C2410_SDICMDSTAT_CRCFAIL) {
@@ -603,8 +615,22 @@ close_transfer:
 	host->complete_what = COMPLETION_FINALIZE;
 
 	clear_imask(host);
-	tasklet_schedule(&host->pio_tasklet);
 
+	/*
+	 * If we have received an interrupt although we are waiting for the
+	 * DMA-callback (cmd->data), then something went wrong with the last
+	 * transfer. This happens when the card is removed before
+	 * the card initialization was completed.
+	 * (Luis Galdos)
+	 */
+	if (cmd->data && host->dodma && !cmd->data->error && !cmd->error) {
+		host->dma_complete = 1;
+		cmd->error = cmd->data->error = -EILSEQ;
+		printk_debug("[SD] Waiting DMA (CMD %08x | DAT %08x)\n",
+			     mci_csta, mci_dsta);
+	}
+
+	tasklet_schedule(&host->pio_tasklet);
 	goto irq_out;
 
 irq_out:
@@ -621,13 +647,26 @@ irq_out:
  * ISR for the CardDetect Pin
 */
 
+static int s3cmci_card_present(struct mmc_host *mmc);
+
 static irqreturn_t s3cmci_irq_cd(int irq, void *dev_id)
 {
 	struct s3cmci_host *host = (struct s3cmci_host *)dev_id;
-
-	dbg(host, dbg_irq, "card detect\n");
-
-	mmc_detect_change(host->mmc, msecs_to_jiffies(500));
+	int val;
+	static int card_present;
+
+	/*
+	 * Get the current status of the GPIO for checking if the card state has
+	 * really changed since the last interrupt. Otherwise it will generates
+	 * more than one interrupt for the same state
+	 * Luis Galdos
+	 */
+	val = s3cmci_card_present(host->mmc);
+	if (val != card_present) {
+		dbg(host, dbg_debug, "Card detect IRQ: %s\n", val ? "insert" : "remove");
+		card_present = val;
+		mmc_detect_change(host->mmc, msecs_to_jiffies(500));
+	}
 
 	return IRQ_HANDLED;
 }
@@ -652,29 +691,49 @@ static void s3cmci_dma_done_callback(struct s3c2410_dma_chan *dma_ch,
 	spin_lock_irqsave(&host->complete_lock, iflags);
 
 	if (result != S3C2410_RES_OK) {
-		dbg(host, dbg_fail, "DMA FAILED: csta=0x%08x dsta=0x%08x "
-			"fsta=0x%08x dcnt:0x%08x result:0x%08x toGo:%u\n",
-			mci_csta, mci_dsta, mci_fsta,
-			mci_dcnt, result, host->dmatogo);
+		struct mmc_request *mrq = host->mrq;
+		
+		if (mrq) {
+			struct mmc_command *cmd;
+
+			cmd = mrq->cmd;
+			dbg(host, dbg_fail, "DMA FAILED: CMD%i csta=0x%08x dsta=0x%08x "
+			    "fsta=0x%08x dcnt:0x%08x result:%i toGo:%u\n",
+			    cmd->opcode, mci_csta, mci_dsta, mci_fsta,
+			    mci_dcnt, result, host->dmatogo);
+                } else 
+			dbg(host, dbg_fail, "DMA FAILED: csta=0x%08x dsta=0x%08x "
+			    "fsta=0x%08x dcnt:0x%08x result:0x%08x toGo:%u\n",
+			    mci_csta, mci_dsta, mci_fsta,
+			    mci_dcnt, result, host->dmatogo);
 
 		goto fail_request;
 	}
 
 	host->dmatogo--;
 	if (host->dmatogo) {
-		dbg(host, dbg_dma, "DMA DONE  Size:%i DSTA:[%08x] "
-			"DCNT:[%08x] toGo:%u\n",
-			size, mci_dsta, mci_dcnt, host->dmatogo);
+		printk_debug("DMA DONE  Size:%i DSTA:[%08x] "
+			     "DCNT:[%08x] toGo:%u\n",
+			     size, mci_dsta, mci_dcnt, host->dmatogo);
 
-		goto out;
+		if (mci_dsta & S3C2410_SDIDSTA_XFERFINISH) {
+			printk_err("SDI has no more data, but DMA waits for more?\n");
+			goto fail_request;
+		}
+		
+		spin_unlock_irqrestore(&host->complete_lock, iflags);
+		return;
 	}
 
-	dbg(host, dbg_dma, "DMA FINISHED Size:%i DSTA:%08x DCNT:%08x\n",
-		size, mci_dsta, mci_dcnt);
+	printk_debug("DMA ENDE  Size:%i DSTA:[%08x] DCNT:[%08x]\n",
+		     size, mci_dsta, mci_dcnt);
 
+	host->dma_complete = 1;
 	host->complete_what = COMPLETION_FINALIZE;
 
 out:
+	/* @FIXME: Check the performance modification through this delay */
+	udelay(10);
 	tasklet_schedule(&host->pio_tasklet);
 	spin_unlock_irqrestore(&host->complete_lock, iflags);
 	return;
@@ -693,17 +752,22 @@ static void finalize_request(struct s3cmci_host *host)
 	struct mmc_command *cmd = host->cmd_is_stop ? mrq->stop : mrq->cmd;
 	int debug_as_failure = 0;
 
-	if (host->complete_what != COMPLETION_FINALIZE)
-		return;
+	spin_lock(&host->complete_lock);
+	
+	if (host->complete_what != COMPLETION_FINALIZE) {
+		printk_debug("Nothing to complete!\n");
+		goto exit_unlock;
+	}
 
-	if (!mrq)
-		return;
+	if (!mrq) {
+		printk_err("Empty MMC request found!\n");
+		goto exit_unlock;
+	}
 
-	if (cmd->data && (cmd->error == 0) &&
-	    (cmd->data->error == 0)) {
+	if (cmd->data && (cmd->error == 0) && (cmd->data->error == 0)) {
 		if (host->dodma && (!host->dma_complete)) {
-			dbg(host, dbg_dma, "DMA Missing!\n");
-			return;
+			printk_err("DMA complete missing (%i)\n", host->dma_complete);
+			goto exit_unlock;
 		}
 	}
 
@@ -725,17 +789,19 @@ static void finalize_request(struct s3cmci_host *host)
 
 	/* Cleanup controller */
 	writel(0, host->base + S3C2410_SDICMDARG);
-	writel(S3C2410_SDIDCON_STOP, host->base + S3C2410_SDIDCON);
 	writel(0, host->base + S3C2410_SDICMDCON);
-	writel(0, host->base + host->sdiimsk);
-
+	writel(S3C2410_SDIDCON_STOP, host->base + S3C2410_SDIDCON);
+	if (!host->sdio_irq) {
+		writel(0, host->base + host->sdiimsk);
+	}
+	
 	if (cmd->data && cmd->error)
 		cmd->data->error = cmd->error;
 
 	if (cmd->data && cmd->data->stop && (!host->cmd_is_stop)) {
 		host->cmd_is_stop = 1;
 		s3cmci_send_request(host->mmc);
-		return;
+		goto exit_unlock;
 	}
 
 	/* If we have no data transfer we are finished here */
@@ -776,28 +842,38 @@ request_done:
 	host->complete_what = COMPLETION_NONE;
 	host->mrq = NULL;
 	mmc_request_done(host->mmc, mrq);
+
+ exit_unlock:
+	spin_unlock(&host->complete_lock);
+
 }
 
 static void s3cmci_dma_setup(struct s3cmci_host *host,
 			     enum s3c2410_dmasrc source)
 {
 	static enum s3c2410_dmasrc last_source = -1;
-	static int setup_ok;
+	static int setup_ok = 0;
 
 	if (last_source == source)
 		return;
 
 	last_source = source;
 
-	s3c2410_dma_devconfig(host->dma, source, 3,
+	s3c2410_dma_devconfig(host->dma, source,
+			      S3C2410_DISRCC_INC | S3C2410_DISRCC_APB,
 			      host->mem->start + host->sdidata);
 
 	if (!setup_ok) {
-		s3c2410_dma_config(host->dma, 4,
-			(S3C2410_DCON_HWTRIG | S3C2410_DCON_CH0_SDI));
+		printk_debug("Setting up the DMA channel!\n");
+		s3c2410_dma_config(host->dma,
+				   4,
+				   S3C2410_DCON_HANDSHAKE |
+				   S3C2410_DCON_SYNC_PCLK |
+				   S3C2410_DCON_HWTRIG);
+
 		s3c2410_dma_set_buffdone_fn(host->dma,
 					    s3cmci_dma_done_callback);
-		s3c2410_dma_setflags(host->dma, S3C2410_DMAF_AUTOSTART);
+		s3c2410_dma_ctrl(host->dma, S3C2410_DMAOP_FLUSH);
 		setup_ok = 1;
 	}
 }
@@ -849,10 +925,11 @@ static int s3cmci_setup_data(struct s3cmci_host *host, struct mmc_data *data)
 		/* We cannot deal with unaligned blocks with more than
 		 * one block being transfered. */
 
-		if (data->blocks > 1) {
-			pr_warning("%s: can't do non-word sized block transfers (blksz %d)\n", __func__, data->blksz);
+		if (data->blocks > 1)
 			return -EINVAL;
-		}
+
+		/* No support yet for non-word block transfers. */
+		return -EINVAL;
 	}
 
 	while (readl(host->base + S3C2410_SDIDSTA) &
@@ -899,12 +976,15 @@ static int s3cmci_setup_data(struct s3cmci_host *host, struct mmc_data *data)
 	writel(dcon, host->base + S3C2410_SDIDCON);
 
 	/* write BSIZE register */
-
 	writel(data->blksz, host->base + S3C2410_SDIBSIZE);
 
 	/* add to IMASK register */
 	imsk = S3C2410_SDIIMSK_FIFOFAIL | S3C2410_SDIIMSK_DATACRC |
-	       S3C2410_SDIIMSK_DATATIMEOUT | S3C2410_SDIIMSK_DATAFINISH;
+		S3C2410_SDIIMSK_DATATIMEOUT;
+
+	/* By DMA-support we will use the DMA-callback for the transfer-complete */
+	if (!host->dodma)
+		imsk |= S3C2410_SDIIMSK_DATAFINISH;
 
 	enable_imask(host, imsk);
 
@@ -951,25 +1031,31 @@ static int s3cmci_prepare_dma(struct s3cmci_host *host, struct mmc_data *data)
 {
 	int dma_len, i;
 	int rw = (data->flags & MMC_DATA_WRITE) ? 1 : 0;
-
+	int retval;
+	
 	BUG_ON((data->flags & BOTH_DIR) == BOTH_DIR);
 
-	s3cmci_dma_setup(host, rw ? S3C2410_DMASRC_MEM : S3C2410_DMASRC_HW);
+	printk_debug("New DMA transfer | %i Blks | %i Blksz\n",
+		     data->blocks, data->blksz);
+
 	s3c2410_dma_ctrl(host->dma, S3C2410_DMAOP_FLUSH);
+	s3cmci_dma_setup(host, rw ? S3C2410_DMASRC_MEM : S3C2410_DMASRC_HW);
 
 	dma_len = dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
 			     (rw) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
 
-	if (dma_len == 0)
-		return -ENOMEM;
+	if (dma_len == 0) {
+		printk_err("dma_map_sg failed, no memory available?\n");
+		retval = -ENOMEM;
+		goto exit_all;
+	}
 
-	host->dma_complete = 0;
 	host->dmatogo = dma_len;
 
 	for (i = 0; i < dma_len; i++) {
 		int res;
 
-		dbg(host, dbg_dma, "enqueue %i:%u@%u\n", i,
+		printk_debug("Enqueue %i @ 0x%08x | %u bytes\n", i,
 			sg_dma_address(&data->sg[i]),
 			sg_dma_len(&data->sg[i]));
 
@@ -979,13 +1065,31 @@ static int s3cmci_prepare_dma(struct s3cmci_host *host, struct mmc_data *data)
 
 		if (res) {
 			s3c2410_dma_ctrl(host->dma, S3C2410_DMAOP_FLUSH);
-			return -EBUSY;
+			printk_err("Couldn't enqueue a DMA-buffer\n");
+			retval = -EBUSY;
+			goto exit_all;
 		}
 	}
 
+	/*
+	 * Disable the data counter interrupt, then the DMA-callback will be
+	 * responsible for finalizing the request
+	 */
+	disable_imask(host, S3C2410_SDIIMSK_DATAFINISH |
+		S3C2410_SDIIMSK_RXFIFOHALF |
+		S3C2410_SDIIMSK_RXFIFOFULL |
+		S3C2410_SDIIMSK_RXFIFOLAST |
+		S3C2410_SDIIMSK_TXFIFOEMPTY |
+		S3C2410_SDIIMSK_TXFIFOHALF);
+
+	host->dma_complete = 0;
 	s3c2410_dma_ctrl(host->dma, S3C2410_DMAOP_START);
 
-	return 0;
+	retval = 0;
+	
+ exit_all:
+	
+	return retval;
 }
 
 static void s3cmci_send_request(struct mmc_host *mmc)
@@ -997,6 +1101,10 @@ static void s3cmci_send_request(struct mmc_host *mmc)
 	host->ccnt++;
 	prepare_dbgmsg(host, cmd, host->cmd_is_stop);
 
+	/* @XXX: Sending the switch command with data leads to a DMA-failure, why? */
+	if (cmd->opcode == MMC_SWITCH && cmd->data)
+		udelay(200);
+
 	/* Clear command, data and fifo status registers
 	   Fifo clear only necessary on 2440, but doesn't hurt on 2410
 	*/
@@ -1007,6 +1115,11 @@ static void s3cmci_send_request(struct mmc_host *mmc)
 	if (cmd->data) {
 		int res = s3cmci_setup_data(host, cmd->data);
 
+		printk_debug("CMD%u: %s transfer | %i blks | %u blksz\n",
+			     cmd->opcode,
+			     (cmd->data->flags & MMC_DATA_READ) ? "RX" : "TX",
+			     cmd->data->blocks, cmd->data->blksz);
+		
 		host->dcnt++;
 
 		if (res) {
@@ -1033,11 +1146,28 @@ static void s3cmci_send_request(struct mmc_host *mmc)
 		}
 	}
 
+	/* Enable Interrupt */
+	if (!host->dodma)
+		enable_irq(host->irq);
+
 	/* Send command */
 	s3cmci_send_command(host, cmd);
 
-	/* Enable Interrupt */
-	enable_irq(host->irq);
+	/*
+	 * If the host supports DMA, then disable the data finish interrupts and
+	 * the FIFO-interrupts, then the DMA-controller will handle the data
+	 * transfer and will finish the transfer if no errors ocurrs
+	 * @XXX: This is probably not the correct place for this operation
+	 * (Luis Galdos)
+	 */
+	if (cmd->data && host->dodma)
+		disable_imask(host, S3C2410_SDIIMSK_DATAFINISH |
+			      S3C2410_SDIIMSK_RXFIFOHALF |
+			      S3C2410_SDIIMSK_RXFIFOFULL |
+			      S3C2410_SDIIMSK_RXFIFOLAST |
+			      S3C2410_SDIIMSK_TXFIFOEMPTY |
+			      S3C2410_SDIIMSK_TXFIFOHALF);
+
 }
 
 static int s3cmci_card_present(struct mmc_host *mmc)
@@ -1062,40 +1192,18 @@ static void s3cmci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	host->mrq = mrq;
 
 	if (s3cmci_card_present(mmc) == 0) {
-		dbg(host, dbg_err, "%s: no medium present\n", __func__);
+		dbg(host, dbg_debug, "%s: no medium present\n", __func__);
 		host->mrq->cmd->error = -ENOMEDIUM;
 		mmc_request_done(mmc, mrq);
 	} else
 		s3cmci_send_request(mmc);
 }
 
-static void s3cmci_set_clk(struct s3cmci_host *host, struct mmc_ios *ios)
-{
-	u32 mci_psc;
-
-	/* Set clock */
-	for (mci_psc = 0; mci_psc < 255; mci_psc++) {
-		host->real_rate = host->clk_rate / (host->clk_div*(mci_psc+1));
-
-		if (host->real_rate <= ios->clock)
-			break;
-	}
-
-	if (mci_psc > 255)
-		mci_psc = 255;
-
-	host->prescaler = mci_psc;
-	writel(host->prescaler, host->base + S3C2410_SDIPRE);
-
-	/* If requested clock is 0, real_rate will be 0, too */
-	if (ios->clock == 0)
-		host->real_rate = 0;
-}
-
 static void s3cmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 {
 	struct s3cmci_host *host = mmc_priv(mmc);
-	u32 mci_con;
+	u32 mci_psc, mci_con;
+	int waitclk;
 
 	/* Set the power state */
 
@@ -1104,13 +1212,6 @@ static void s3cmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	switch (ios->power_mode) {
 	case MMC_POWER_ON:
 	case MMC_POWER_UP:
-		s3c2410_gpio_cfgpin(S3C2410_GPE5, S3C2410_GPE5_SDCLK);
-		s3c2410_gpio_cfgpin(S3C2410_GPE6, S3C2410_GPE6_SDCMD);
-		s3c2410_gpio_cfgpin(S3C2410_GPE7, S3C2410_GPE7_SDDAT0);
-		s3c2410_gpio_cfgpin(S3C2410_GPE8, S3C2410_GPE8_SDDAT1);
-		s3c2410_gpio_cfgpin(S3C2410_GPE9, S3C2410_GPE9_SDDAT2);
-		s3c2410_gpio_cfgpin(S3C2410_GPE10, S3C2410_GPE10_SDDAT3);
-
 		if (host->pdata->set_power)
 			host->pdata->set_power(ios->power_mode, ios->vdd);
 
@@ -1121,9 +1222,6 @@ static void s3cmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	case MMC_POWER_OFF:
 	default:
-		s3c2410_gpio_setpin(S3C2410_GPE5, 0);
-		s3c2410_gpio_cfgpin(S3C2410_GPE5, S3C2410_GPE5_OUTP);
-
 		if (host->is2440)
 			mci_con |= S3C2440_SDICON_SDRESET;
 
@@ -1133,8 +1231,6 @@ static void s3cmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		break;
 	}
 
-	s3cmci_set_clk(host, ios);
-
 	/* Set CLOCK_ENABLE */
 	if (ios->clock)
 		mci_con |= S3C2410_SDICON_CLOCKTYPE;
@@ -1143,12 +1239,47 @@ static void s3cmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	writel(mci_con, host->base + S3C2410_SDICON);
 
+
+	/* Set clock */
+	for (mci_psc = 0; mci_psc < 255; mci_psc++) {
+		host->real_rate = host->clk_rate / (host->clk_div*(mci_psc+1));
+
+		if (host->real_rate <= ios->clock)
+			break;
+	}
+
+	if (mci_psc > 255)
+		mci_psc = 255;
+
+
+	/* If requested clock is 0, real_rate will be 0, too */
+	if (ios->clock == 0)
+		host->real_rate = 0;
+
+	host->prescaler = mci_psc;
+	writel(host->prescaler, host->base + S3C2410_SDIPRE);
+
+	/*
+	 * According to the data sheet first configure the register SDICON and
+	 * wait at least 74 SDCLK before initializing the SD card.
+	 * (Luis Galdos)
+	 */
+	waitclk = 1;
+	if (host->real_rate)
+		waitclk = 100 * (1000000 / host->real_rate);
+
+	if (waitclk > 1000)
+		mdelay(waitclk / 1000);
+	else
+		udelay(waitclk);
+
+	
 	if ((ios->power_mode == MMC_POWER_ON) ||
 	    (ios->power_mode == MMC_POWER_UP)) {
-		dbg(host, dbg_conf, "running at %lukHz (requested: %ukHz).\n",
+		dbg(host, dbg_debug, "running at %lukHz (requested: %ukHz).\n",
 			host->real_rate/1000, ios->clock/1000);
 	} else {
-		dbg(host, dbg_conf, "powered down.\n");
+		dbg(host, dbg_debug, "powered down.\n");
 	}
 
 	host->bus_width = ios->bus_width;
@@ -1179,11 +1310,42 @@ static int s3cmci_get_ro(struct mmc_host *mmc)
 	return ret;
 }
 
+/* Here only mask or unmask the SDIO interrupt */
+static void s3cmci_enable_sdio_irq(struct mmc_host *mmc, int enable)
+{
+	struct s3cmci_host *host;
+	ulong con, imsk;
+	
+	if (!(host = mmc_priv(mmc))) {
+		printk_err("NULL host pointer found!\n");
+		return;
+	}
+	
+	printk_debug("%s SDIO IRQ\n", enable ? "Enabling" : "Disabling");
+	
+	con = readl(host->base + S3C2410_SDICON);
+	imsk = readl(host->base + host->sdiimsk);
+	
+	if (enable) {
+		host->sdio_irq = 1;
+		imsk |= S3C2410_SDIIMSK_SDIOIRQ;
+		con |= S3C2410_SDICON_SDIOIRQ;
+	} else {
+		host->sdio_irq = 0;
+		
+		imsk |= S3C2410_SDIIMSK_SDIOIRQ;
+		con &= ~S3C2410_SDICON_SDIOIRQ;
+	}
+	
+	writel(con, host->base + S3C2410_SDICON);
+	writel(imsk, host->base + host->sdiimsk);
+}
+
 static struct mmc_host_ops s3cmci_ops = {
-	.request	= s3cmci_request,
-	.set_ios	= s3cmci_set_ios,
-	.get_ro		= s3cmci_get_ro,
-	.get_cd		= s3cmci_card_present,
+	.request	 = s3cmci_request,
+	.set_ios	 = s3cmci_set_ios,
+	.get_ro		 = s3cmci_get_ro,
+	.enable_sdio_irq = s3cmci_enable_sdio_irq,
 };
 
 static struct s3c24xx_mci_pdata s3cmci_def_pdata = {
@@ -1191,61 +1353,6 @@ static struct s3c24xx_mci_pdata s3cmci_def_pdata = {
 	 * checks. Any zero fields to ensure reaonable defaults are picked. */
 };
 
-#ifdef CONFIG_CPU_FREQ
-
-static int s3cmci_cpufreq_transition(struct notifier_block *nb,
-				     unsigned long val, void *data)
-{
-	struct s3cmci_host *host;
-	struct mmc_host *mmc;
-	unsigned long newclk;
-	unsigned long flags;
-
-	host = container_of(nb, struct s3cmci_host, freq_transition);
-	newclk = clk_get_rate(host->clk);
-	mmc = host->mmc;
-
-	if ((val == CPUFREQ_PRECHANGE && newclk > host->clk_rate) ||
-	    (val == CPUFREQ_POSTCHANGE && newclk < host->clk_rate)) {
-		spin_lock_irqsave(&mmc->lock, flags);
-
-		host->clk_rate = newclk;
-
-		if (mmc->ios.power_mode != MMC_POWER_OFF &&
-		    mmc->ios.clock != 0)
-			s3cmci_set_clk(host, &mmc->ios);
-
-		spin_unlock_irqrestore(&mmc->lock, flags);
-	}
-
-	return 0;
-}
-
-static inline int s3cmci_cpufreq_register(struct s3cmci_host *host)
-{
-	host->freq_transition.notifier_call = s3cmci_cpufreq_transition;
-
-	return cpufreq_register_notifier(&host->freq_transition,
-					 CPUFREQ_TRANSITION_NOTIFIER);
-}
-
-static inline void s3cmci_cpufreq_deregister(struct s3cmci_host *host)
-{
-	cpufreq_unregister_notifier(&host->freq_transition,
-				    CPUFREQ_TRANSITION_NOTIFIER);
-}
-
-#else
-static inline int s3cmci_cpufreq_register(struct s3cmci_host *host)
-{
-	return 0;
-}
-
-static inline void s3cmci_cpufreq_deregister(struct s3cmci_host *host)
-{
-}
-#endif
-
 static int __devinit s3cmci_probe(struct platform_device *pdev, int is2440)
 {
 	struct s3cmci_host *host;
@@ -1269,9 +1376,6 @@ static int __devinit s3cmci_probe(struct platform_device *pdev, int is2440)
 		host->pdata = &s3cmci_def_pdata;
 	}
 
-	spin_lock_init(&host->complete_lock);
-	tasklet_init(&host->pio_tasklet, pio_tasklet, (unsigned long) host);
-
 	if (is2440) {
 		host->sdiimsk	= S3C2440_SDIIMSK;
 		host->sdidata	= S3C2440_SDIDATA;
@@ -1282,12 +1386,22 @@ static int __devinit s3cmci_probe(struct platform_device *pdev, int is2440)
 		host->clk_div	= 2;
 	}
 
-	host->dodma		= 0;
+	/* Configure the DMA-support depending on the passed platform configuration */
+	host->dodma		= (host->pdata->dma_enable) ? 1 : 0;
 	host->complete_what 	= COMPLETION_NONE;
 	host->pio_active 	= XFER_NONE;
 
-	host->dma		= S3CMCI_DMA;
+	/* DMA-channel to use (include/asm-arm/arch-s3c2410/dma.h) */
+	host->dma		= DMACH_SDI;
+
+	spin_lock_init(&host->complete_lock);
 
+	/* Depending on the DMA-support use different tasklet-functions */
+	if (!host->dodma)
+		tasklet_init(&host->pio_tasklet, pio_tasklet, (unsigned long) host);
+	else
+		tasklet_init(&host->pio_tasklet, dma_tasklet, (unsigned long) host);
+	
 	host->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!host->mem) {
 		dev_err(&pdev->dev,
@@ -1352,10 +1466,12 @@ static int __devinit s3cmci_probe(struct platform_device *pdev, int is2440)
 		s3c2410_gpio_cfgpin(host->pdata->gpio_wprotect,
 				    S3C2410_GPIO_INPUT);
 
-	if (s3c2410_dma_request(S3CMCI_DMA, &s3cmci_dma_client, NULL) < 0) {
-		dev_err(&pdev->dev, "unable to get DMA channel.\n");
-		ret = -EBUSY;
-		goto probe_free_irq_cd;
+	if (host->dodma) {
+		if (s3c2410_dma_request(host->dma, &s3cmci_dma_client, NULL) < 0) {
+			dev_err(&pdev->dev, "unable to get DMA channel.\n");
+			ret = -EBUSY;
+			goto probe_free_irq_cd;
+		}
 	}
 
 	host->clk = clk_get(&pdev->dev, "sdi");
@@ -1363,7 +1479,7 @@ static int __devinit s3cmci_probe(struct platform_device *pdev, int is2440)
 		dev_err(&pdev->dev, "failed to find clock source.\n");
 		ret = PTR_ERR(host->clk);
 		host->clk = NULL;
-		goto probe_free_host;
+		goto probe_free_dmach;
 	}
 
 	ret = clk_enable(host->clk);
@@ -1376,7 +1492,7 @@ static int __devinit s3cmci_probe(struct platform_device *pdev, int is2440)
 
 	mmc->ops 	= &s3cmci_ops;
 	mmc->ocr_avail	= MMC_VDD_32_33 | MMC_VDD_33_34;
-	mmc->caps	= MMC_CAP_4_BIT_DATA;
+	mmc->caps	= MMC_CAP_4_BIT_DATA | MMC_CAP_SDIO_IRQ;
 	mmc->f_min 	= host->clk_rate / (host->clk_div * 256);
 	mmc->f_max 	= host->clk_rate / host->clk_div;
 
@@ -1396,25 +1512,30 @@ static int __devinit s3cmci_probe(struct platform_device *pdev, int is2440)
 	    (host->is2440?"2440":""),
 	    host->base, host->irq, host->irq_cd, host->dma);
 
-	ret = s3cmci_cpufreq_register(host);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to register cpufreq\n");
-		goto free_dmabuf;
-	}
-
 	ret = mmc_add_host(mmc);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to add mmc host.\n");
-		goto free_cpufreq;
+		goto free_dmabuf;
 	}
 
+	/* @FIXME: Get the GPIO-list from the platform-data */
+	s3c2410_gpio_cfgpin(S3C2410_GPE5, S3C2410_GPE5_SDCLK);
+	s3c2410_gpio_cfgpin(S3C2410_GPE6, S3C2410_GPE6_SDCMD);
+	s3c2410_gpio_cfgpin(S3C2410_GPE7, S3C2410_GPE7_SDDAT0);
+	s3c2410_gpio_cfgpin(S3C2410_GPE8, S3C2410_GPE8_SDDAT1);
+	s3c2410_gpio_cfgpin(S3C2410_GPE9, S3C2410_GPE9_SDDAT2);
+	s3c2410_gpio_cfgpin(S3C2410_GPE10, S3C2410_GPE10_SDDAT3);
+	
 	platform_set_drvdata(pdev, mmc);
 	dev_info(&pdev->dev, "initialisation done.\n");
 
-	return 0;
+        /* Enable the wakeup for this device (Luis Galdos) */
+	device_init_wakeup(&pdev->dev, 1); 
+        device_set_wakeup_enable(&pdev->dev, 0);
+	
+	enable_irq(host->irq);
 
- free_cpufreq:
-	s3cmci_cpufreq_deregister(host);
+	return 0;
 
  free_dmabuf:
 	clk_disable(host->clk);
@@ -1422,6 +1543,10 @@ static int __devinit s3cmci_probe(struct platform_device *pdev, int is2440)
  clk_free:
 	clk_put(host->clk);
 
+ probe_free_dmach:
+	if (host->dodma)
+		s3c2410_dma_free(host->dma, &s3cmci_dma_client);
+	
  probe_free_irq_cd:
 	if (host->irq_cd >= 0)
 		free_irq(host->irq_cd, host);
@@ -1449,11 +1574,26 @@ static void s3cmci_shutdown(struct platform_device *pdev)
 	if (host->irq_cd >= 0)
 		free_irq(host->irq_cd, host);
 
-	s3cmci_cpufreq_deregister(host);
 	mmc_remove_host(mmc);
 	clk_disable(host->clk);
 }
 
+/*
+ * Don't remove the MMC-host when going to shutdown the system. This can lead to some
+ * failures when the host has mounted a card with our root file system.
+ * (Luis Galdos)
+ */
+static void s3cmci_2440_shutdown(struct platform_device *pdev)
+{
+	struct mmc_host	*mmc = platform_get_drvdata(pdev);
+	struct s3cmci_host *host = mmc_priv(mmc);
+
+	if (host->irq_cd >= 0)
+		free_irq(host->irq_cd, host);
+
+	clk_disable(host->clk);
+}
+
 static int __devexit s3cmci_remove(struct platform_device *pdev)
 {
 	struct mmc_host		*mmc  = platform_get_drvdata(pdev);
@@ -1464,7 +1604,10 @@ static int __devexit s3cmci_remove(struct platform_device *pdev)
 	clk_put(host->clk);
 
 	tasklet_disable(&host->pio_tasklet);
-	s3c2410_dma_free(S3CMCI_DMA, &s3cmci_dma_client);
+
+	/* Free the DMA-channel */
+	if (host->dodma)
+		s3c2410_dma_free(host->dma, &s3cmci_dma_client);
 
 	free_irq(host->irq, host);
 
@@ -1492,18 +1635,94 @@ static int __devinit s3cmci_2440_probe(struct platform_device *dev)
 
 #ifdef CONFIG_PM
 
-static int s3cmci_suspend(struct platform_device *dev, pm_message_t state)
+static int s3cmci_suspend(struct platform_device *pdev, pm_message_t state)
 {
-	struct mmc_host *mmc = platform_get_drvdata(dev);
+	int retval;
+	struct mmc_host *mmc;
+	struct s3cmci_host *host;
+
+	mmc = platform_get_drvdata(pdev);
+	host = mmc_priv(mmc);
+
+ 	retval = mmc_suspend_host(mmc, state);
+	if (retval)
+		goto exit_suspend;
+	
+	/* Check if the wakeup is enabled. IN that case configure it as ext wakeup */
+	if (device_may_wakeup(&pdev->dev)) {
+		struct s3c24xx_mci_pdata *pdata;
+		unsigned long detect;
+
+		pdata = host->pdata;
+		
+		retval = enable_irq_wake(host->irq_cd);
+		if (retval) {
+			dev_err(&pdev->dev, "Couldn't enable wake IRQ %i\n",
+				host->irq_cd);
+			goto exit_suspend;
+		}
+
+		/*
+		 * Reconfigure the card for generating the wakeup when a new
+		 * card is plugged into the slot
+		 */
+		detect = (pdata->detect_invert) ? (IRQF_TRIGGER_RISING) :
+			(IRQF_TRIGGER_FALLING);
+		set_irq_type(host->irq_cd, detect);
+	}
 
-	return  mmc_suspend_host(mmc, state);
+	writel(S3C2440_SDICON_SDRESET | S3C2410_SDIDCON_STOP,
+	       host->base + S3C2410_SDICON);
+	
+	clk_disable(host->clk);
+exit_suspend:
+	return retval;
 }
 
-static int s3cmci_resume(struct platform_device *dev)
+static int s3cmci_resume(struct platform_device *pdev)
 {
-	struct mmc_host *mmc = platform_get_drvdata(dev);
+	struct s3cmci_host *host;
+	struct mmc_host *mmc;
+	int retval;
 
-	return mmc_resume_host(mmc);
+	mmc = platform_get_drvdata(pdev);
+	host = mmc_priv(mmc);
+
+	if (device_may_wakeup(&pdev->dev)) {
+		retval = disable_irq_wake(host->irq_cd);
+		if (retval) {
+			dev_err(&pdev->dev, "Couldn't disable wake IRQ %i\n",
+				host->irq_cd);
+			goto exit_resume;
+		}
+
+		set_irq_type(host->irq_cd, IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING);
+	}
+
+	clk_enable(host->clk);
+	
+	/* @FIXME: Why do we need to free que DMA-channel? */
+	s3c2410_dma_free(host->dma, &s3cmci_dma_client);
+	s3c2410_dma_request(host->dma, &s3cmci_dma_client, NULL);
+
+	/*
+	 * By unsafe resumes we MUST check the card state at this point, then the
+	 * higher MMC-layer is probably transferring some kind of data to the
+	 * block device that doesn't exist any more.
+	 */
+#if defined(CONFIG_MMC_UNSAFE_RESUME)
+	if (s3cmci_card_present(mmc))
+		retval = mmc_resume_host(mmc);
+	else {
+		retval = 0;
+		mmc_detect_change(mmc, msecs_to_jiffies(500));
+	}
+#else
+	retval = mmc_resume_host(mmc);
+#endif /* CONFIG_MMC_UNSAFE_RESUME */
+	
+exit_resume:
+	return retval;
 }
 
 #else /* CONFIG_PM */
@@ -1537,7 +1756,7 @@ static struct platform_driver s3cmci_2440_driver = {
 	.driver.owner	= THIS_MODULE,
 	.probe		= s3cmci_2440_probe,
 	.remove		= __devexit_p(s3cmci_remove),
-	.shutdown	= s3cmci_shutdown,
+	.shutdown	= s3cmci_2440_shutdown,
 	.suspend	= s3cmci_suspend,
 	.resume		= s3cmci_resume,
 };
@@ -1563,7 +1782,7 @@ module_exit(s3cmci_exit);
 
 MODULE_DESCRIPTION("Samsung S3C MMC/SD Card Interface driver");
 MODULE_LICENSE("GPL v2");
-MODULE_AUTHOR("Thomas Kleffel <tk@maintech.de>, Ben Dooks <ben-linux@fluff.org>");
+MODULE_AUTHOR("Thomas Kleffel <tk@maintech.de>");
 MODULE_ALIAS("platform:s3c2410-sdi");
 MODULE_ALIAS("platform:s3c2412-sdi");
 MODULE_ALIAS("platform:s3c2440-sdi");
diff --git a/drivers/mmc/host/s3cmci.h b/drivers/mmc/host/s3cmci.h
index ca1ba3d58cfd..25e6929a3dea 100644
--- a/drivers/mmc/host/s3cmci.h
+++ b/drivers/mmc/host/s3cmci.h
@@ -71,4 +71,6 @@ struct s3cmci_host {
 #ifdef CONFIG_CPU_FREQ
 	struct notifier_block	freq_transition;
 #endif
+
+	int sdio_irq;
 };
diff --git a/drivers/mmc/host/stmp3xxx_mmc.c b/drivers/mmc/host/stmp3xxx_mmc.c
new file mode 100644
index 000000000000..b5c9bc3580a4
--- /dev/null
+++ b/drivers/mmc/host/stmp3xxx_mmc.c
@@ -0,0 +1,1077 @@
+/*
+ * Copyright (C) 2007 SigmaTel, Inc., Ioannis Kappas <ikappas@sigmatel.com>
+ *
+ * Portions copyright (C) 2003 Russell King, PXA MMCI Driver
+ * Portions copyright (C) 2004-2005 Pierre Ossman, W83L51xD SD/MMC driver
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/highmem.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/completion.h>
+#include <linux/mmc/host.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+#include <mach/hardware.h>
+#include <mach/dma.h>
+#include <mach/regs-apbh.h>
+#include <mach/regs-ssp.h>
+#include <mach/regs-clkctrl.h>
+#include <mach/stmp3xxx.h>
+
+#define DRIVER_NAME	"stmp3xxx-mmc"
+
+#define CLOCKRATE_MIN 400000
+#define CLOCKRATE_MAX 48000000
+
+/*
+ * Card detect polling timeout
+ */
+#define STMP37XX_MMC_DETECT_TIMEOUT		(HZ/2)
+
+/* Max value supported for XFER_COUNT */
+#define SSP_BUFFER_SIZE		(65536 - 512)
+
+struct stmp3xxx_mmc_host {
+	struct device *dev;
+	struct mmc_host *mmc;
+
+	struct clk *clk;
+	unsigned int clkrt;
+
+	struct mmc_request *mrq;
+	struct mmc_command *cmd;
+	struct mmc_data *data;
+
+	/* Whether the card is capable of 4-bit data */
+	int bus_width_4:1;
+
+	/* Whether SD card is present */
+	unsigned present:1;
+
+	/* Polling timer */
+	struct timer_list timer;
+
+	/* SSP interface which MMC/SD card slot is attached to */
+	u32 ssp_base;
+
+	/* DMA channel used for this host */
+	unsigned int dmach;
+
+	/* IRQs */
+	int dmairq, errirq;
+
+	/* DMA descriptor to transfer data over SSP interface */
+	struct stmp3xxx_dma_descriptor dma_desc;
+
+	/* DMA buffer */
+	dma_addr_t dma_buf_phys;
+	char *dma_buf;
+
+	struct completion dma_done;
+	/* status on last interrupt */
+	u32 status;
+	int read_uA, write_uA;
+	struct regulator *regulator;
+};
+
+/* Return read only state of card */
+static int stmp3xxx_mmc_get_ro(struct mmc_host *mmc)
+{
+	struct stmp3xxx_mmc_host *host = mmc_priv(mmc);
+	struct stmp3xxxmmc_platform_data *pdata = host->dev->platform_data;
+
+	if (pdata && pdata->get_wp)
+		return pdata->get_wp();
+
+	return 0;
+}
+
+/* Detect if card is plugged */
+static inline int stmp3xxx_mmc_is_plugged(struct stmp3xxx_mmc_host *host)
+{
+	u32 status = HW_SSP_STATUS_RD_NB(host->ssp_base);
+	return !(status & BM_SSP_STATUS_CARD_DETECT);
+}
+
+/* Card detection polling function */
+static void stmp3xxx_mmc_detect_poll(unsigned long arg)
+{
+	struct stmp3xxx_mmc_host *host = (struct stmp3xxx_mmc_host *)arg;
+	int card_status;
+
+	card_status = stmp3xxx_mmc_is_plugged(host);
+	if (card_status != host->present) {
+		host->present = card_status;
+		mmc_detect_change(host->mmc, 0);
+	}
+
+	mod_timer(&host->timer, jiffies + STMP37XX_MMC_DETECT_TIMEOUT);
+}
+
+#define STMP3XXX_MMC_IRQ_BITS  (BM_SSP_CTRL1_SDIO_IRQ		| \
+				BM_SSP_CTRL1_RESP_ERR_IRQ	| \
+				BM_SSP_CTRL1_RESP_TIMEOUT_IRQ	| \
+				BM_SSP_CTRL1_DATA_TIMEOUT_IRQ	| \
+				BM_SSP_CTRL1_DATA_CRC_IRQ	| \
+				BM_SSP_CTRL1_FIFO_UNDERRUN_IRQ	| \
+				BM_SSP_CTRL1_RECV_TIMEOUT_IRQ   | \
+				BM_SSP_CTRL1_FIFO_OVERRUN_IRQ)
+
+/* SSP DMA interrupt handler */
+static irqreturn_t mmc_irq_handler(int irq, void *dev_id)
+{
+	struct stmp3xxx_mmc_host *host = dev_id;
+	u32 c1;
+
+	c1 = HW_SSP_CTRL1_RD_NB(host->ssp_base);
+	HW_SSP_CTRL1_CLR_NB(host->ssp_base, c1 & STMP3XXX_MMC_IRQ_BITS);
+	if (irq == host->dmairq)
+		stmp3xxx_dma_clear_interrupt(host->dmach);
+	host->status = HW_SSP_STATUS_RD_NB(host->ssp_base);
+
+	if (host->cmd) /* else it is a bogus interrupt */
+		complete(&host->dma_done);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Check for MMC command errors
+ * Returns error code or zerro if no errors
+ */
+static inline int stmp3xxx_mmc_cmd_error(u32 status)
+{
+	int err = 0;
+
+	if (status & BM_SSP_STATUS_TIMEOUT)
+		err = -ETIMEDOUT;
+	else if (status & BM_SSP_STATUS_RESP_TIMEOUT)
+		err = -ETIMEDOUT;
+	else if (status & BM_SSP_STATUS_RESP_CRC_ERR)
+		err = -EILSEQ;
+	else if (status & BM_SSP_STATUS_RESP_ERR)
+		err = -EIO;
+
+	return err;
+}
+
+/* Send the BC command to the device */
+static void stmp3xxx_mmc_bc(struct stmp3xxx_mmc_host *host)
+{
+	struct mmc_command *cmd = host->cmd;
+	struct stmp3xxx_dma_descriptor *dma_desc = &host->dma_desc;
+
+	dma_desc->command->cmd =
+	    BM_APBH_CHn_CMD_WAIT4ENDCMD |
+	    BM_APBH_CHn_CMD_SEMAPHORE |
+	    BM_APBH_CHn_CMD_IRQONCMPLT |
+	    BF_APBH_CHn_CMD_XFER_COUNT(0) |
+	    BF_APBH_CHn_CMD_CMDWORDS(3) |
+	    BF_APBH_CHn_CMD_COMMAND(0); /* NO_DMA_XFER */
+
+	dma_desc->command->pio_words[0] = BM_SSP_CTRL0_ENABLE |
+					  BM_SSP_CTRL0_IGNORE_CRC;
+	dma_desc->command->pio_words[1] = BF_SSP_CMD0_CMD(cmd->opcode) |
+					  BM_SSP_CMD0_APPEND_8CYC;
+	dma_desc->command->pio_words[2] = BF_SSP_CMD1_CMD_ARG(cmd->arg);
+
+	init_completion(&host->dma_done);
+	stmp3xxx_dma_reset_channel(host->dmach);
+	stmp3xxx_dma_go(host->dmach, dma_desc, 1);
+	wait_for_completion(&host->dma_done);
+
+	cmd->error = stmp3xxx_mmc_cmd_error(host->status);
+
+	if (stmp3xxx_dma_running(host->dmach))
+		dev_dbg(host->dev, "DMA command not finished\n");
+
+	if (cmd->error) {
+		dev_dbg(host->dev, "Command error 0x%x\n", cmd->error);
+		stmp3xxx_dma_reset_channel(host->dmach);
+	}
+}
+
+/* Send the ac command to the device */
+static void stmp3xxx_mmc_ac(struct stmp3xxx_mmc_host *host)
+{
+	struct mmc_command *cmd = host->cmd;
+	struct stmp3xxx_dma_descriptor *dma_desc = &host->dma_desc;
+	u32 ignore_crc, resp, long_resp;
+	u32 ssp_ctrl0;
+	u32 ssp_cmd0;
+	u32 ssp_cmd1;
+
+	ignore_crc = (mmc_resp_type(cmd) & MMC_RSP_CRC) ?
+		0 : BM_SSP_CTRL0_IGNORE_CRC;
+	resp = (mmc_resp_type(cmd) & MMC_RSP_PRESENT) ?
+		BM_SSP_CTRL0_GET_RESP : 0;
+	long_resp = (mmc_resp_type(cmd) & MMC_RSP_136) ?
+		BM_SSP_CTRL0_LONG_RESP : 0;
+
+	dma_desc->command->cmd =
+	    BM_APBH_CHn_CMD_WAIT4ENDCMD |
+	    BM_APBH_CHn_CMD_SEMAPHORE |
+	    BM_APBH_CHn_CMD_IRQONCMPLT |
+	    BF_APBH_CHn_CMD_XFER_COUNT(0) |
+	    BF_APBH_CHn_CMD_CMDWORDS(3) |
+	    BF_APBH_CHn_CMD_COMMAND(0);
+
+	ssp_ctrl0 = BM_SSP_CTRL0_ENABLE | ignore_crc | long_resp | resp;
+	ssp_cmd0 = BF_SSP_CMD0_CMD(cmd->opcode);
+	ssp_cmd1 = BF_SSP_CMD1_CMD_ARG(cmd->arg);
+
+	dma_desc->command->pio_words[0] = ssp_ctrl0;
+	dma_desc->command->pio_words[1] = ssp_cmd0;
+	dma_desc->command->pio_words[2] = ssp_cmd1;
+
+	stmp3xxx_dma_reset_channel(host->dmach);
+	init_completion(&host->dma_done);
+	stmp3xxx_dma_go(host->dmach, dma_desc, 1);
+	wait_for_completion(&host->dma_done);
+
+
+	switch (mmc_resp_type(cmd)) {
+	case MMC_RSP_NONE:
+		while (HW_SSP_CTRL0_RD_NB(host->ssp_base) & BM_SSP_CTRL0_RUN)
+			continue;
+		break;
+	case MMC_RSP_R1:
+	case MMC_RSP_R1B:
+	case MMC_RSP_R3:
+		cmd->resp[0] = HW_SSP_SDRESP0_RD_NB(host->ssp_base);
+		break;
+	case MMC_RSP_R2:
+		cmd->resp[3] = HW_SSP_SDRESP0_RD_NB(host->ssp_base);
+		cmd->resp[2] = HW_SSP_SDRESP1_RD_NB(host->ssp_base);
+		cmd->resp[1] = HW_SSP_SDRESP2_RD_NB(host->ssp_base);
+		cmd->resp[0] = HW_SSP_SDRESP3_RD_NB(host->ssp_base);
+		break;
+	default:
+		dev_warn(host->dev, "Unsupported response type 0x%x\n",
+			mmc_resp_type(cmd));
+		BUG();
+		break;
+	}
+
+	cmd->error = stmp3xxx_mmc_cmd_error(host->status);
+
+	if (stmp3xxx_dma_running(host->dmach))
+		dev_dbg(host->dev, "DMA command not finished\n");
+
+	if (cmd->error) {
+		dev_dbg(host->dev, "Command error 0x%x\n", cmd->error);
+		stmp3xxx_dma_reset_channel(host->dmach);
+	}
+}
+
+/* Copy data between sg list and dma buffer */
+static unsigned int stmp3xxx_sg_dma_copy(struct stmp3xxx_mmc_host *host,
+					 unsigned int size, int to_dma)
+{
+	struct mmc_data *data = host->cmd->data;
+	unsigned int copy_size, bytes_copied = 0;
+	struct scatterlist *sg;
+	char *dmabuf = host->dma_buf;
+	char *sgbuf;
+	int len, i;
+
+	sg = data->sg;
+	len = data->sg_len;
+
+	/*
+	 * Just loop through all entries. Size might not
+	 * be the entire list though so make sure that
+	 * we do not transfer too much.
+	 */
+	for (i = 0; i < len; i++) {
+		sgbuf = kmap_atomic(sg_page(&sg[i]), KM_BIO_SRC_IRQ) +
+			sg[i].offset;
+		copy_size = size < sg[i].length ? size : sg[i].length;
+		if (to_dma)
+			memcpy(dmabuf, sgbuf, copy_size);
+		else
+			memcpy(sgbuf, dmabuf, copy_size);
+		kunmap_atomic(sgbuf, KM_BIO_SRC_IRQ);
+
+		dmabuf += sg[i].length;
+
+		bytes_copied += copy_size;
+		size -= copy_size;
+
+		if (size == 0)
+			break;
+	}
+
+	return bytes_copied;
+}
+
+/* Convert ns to tick count according to the current sclk speed */
+static unsigned short stmp3xxx_ns_to_ssp_ticks(unsigned clock_rate, unsigned ns)
+{
+	const unsigned int ssp_timeout_mul = 4096;
+	/*
+	 * Calculate ticks in ms since ns are large numbers
+	 * and might overflow
+	 */
+	const unsigned int clock_per_ms = clock_rate / 1000;
+	const unsigned int ms = ns / 1000;
+	const unsigned int ticks = ms * clock_per_ms;
+	const unsigned int ssp_ticks = ticks / ssp_timeout_mul;
+
+	BUG_ON(ssp_ticks == 0);
+	return ssp_ticks;
+}
+
+static void __init_reg(struct device *dev, struct regulator **pp_reg)
+{
+	struct regulator *reg = *pp_reg;
+
+	if (!reg) {
+		reg = regulator_get(NULL, "mmc_ssp-1");
+		if (reg && !IS_ERR(reg))
+			regulator_set_mode(reg, REGULATOR_MODE_NORMAL);
+		else
+			reg = NULL;
+		*pp_reg = reg;
+	}
+}
+
+/* Send adtc command to the card */
+static void stmp3xxx_mmc_adtc(struct stmp3xxx_mmc_host *host)
+{
+	struct mmc_command *cmd = host->cmd;
+	struct stmp3xxx_dma_descriptor *dma_desc = &host->dma_desc;
+	int ignore_crc, resp, long_resp;
+	int is_reading = 0;
+	unsigned int copy_size;
+
+	u32 ssp_ctrl0;
+	u32 ssp_cmd0;
+	u32 ssp_cmd1;
+	u32 timeout;
+
+	u32 data_size = cmd->data->blksz * cmd->data->blocks;
+	u32 log2_block_size;
+
+	ignore_crc = mmc_resp_type(cmd) & MMC_RSP_CRC ? 0 : 1;
+	resp = mmc_resp_type(cmd) & MMC_RSP_PRESENT ? 1 : 0;
+	long_resp = mmc_resp_type(cmd) & MMC_RSP_136 ? 1 : 0;
+
+	dev_dbg(host->dev, "ADTC command:\n"
+		"response: %d, ignore crc: %d\n"
+		"data list: %u, blocksz: %u, blocks: %u, timeout: %uns %uclks, "
+		"flags: 0x%x\n", resp, ignore_crc, cmd->data->sg_len,
+		cmd->data->blksz, cmd->data->blocks, cmd->data->timeout_ns,
+		cmd->data->timeout_clks, cmd->data->flags);
+
+	if (cmd->data->flags & MMC_DATA_WRITE) {
+		dev_dbg(host->dev, "Data Write\n");
+		copy_size = stmp3xxx_sg_dma_copy(host, data_size, 1);
+		BUG_ON(copy_size < data_size);
+		is_reading = 0;
+		if (!host->regulator)
+			__init_reg(host->dev, &host->regulator);
+		if (host->regulator)
+			regulator_set_current_limit(host->regulator, host->write_uA, host->write_uA);
+	} else if (cmd->data->flags & MMC_DATA_READ) {
+		dev_dbg(host->dev, "Data Read\n");
+		is_reading = 1;
+		if (!host->regulator)
+			__init_reg(host->dev, &host->regulator);
+		if (host->regulator)
+			regulator_set_current_limit(host->regulator, host->read_uA, host->read_uA);
+	} else {
+		dev_warn(host->dev, "Unsuspported data mode, 0x%x\n",
+			 cmd->data->flags);
+		BUG();
+	}
+
+	BUG_ON(cmd->data->flags & MMC_DATA_STREAM);
+	BUG_ON((data_size % 8) > 0);
+
+	dma_desc->command->cmd =
+	    BM_APBH_CHn_CMD_WAIT4ENDCMD |
+	    BM_APBH_CHn_CMD_SEMAPHORE |
+	    BM_APBH_CHn_CMD_IRQONCMPLT |
+	    BF_APBH_CHn_CMD_XFER_COUNT(data_size) |
+	    BF_APBH_CHn_CMD_CMDWORDS(3);
+
+	/* when is_reading is set, DMA controller performs WRITE operation. */
+	dma_desc->command->cmd |=
+		BF_APBH_CHn_CMD_COMMAND(
+			is_reading ? BV_APBH_CHn_CMD_COMMAND__DMA_WRITE :
+				     BV_APBH_CHn_CMD_COMMAND__DMA_READ);
+	ssp_ctrl0 =
+	    (ignore_crc ? BM_SSP_CTRL0_IGNORE_CRC : 0) |
+	    (resp ? BM_SSP_CTRL0_GET_RESP : 0) |
+	    (long_resp ? BM_SSP_CTRL0_LONG_RESP : 0) |
+	    (is_reading ? BM_SSP_CTRL0_READ : 0) |
+	    BM_SSP_CTRL0_DATA_XFER |
+	    BM_SSP_CTRL0_WAIT_FOR_IRQ |
+	    BM_SSP_CTRL0_ENABLE |
+	    BF_SSP_CTRL0_XFER_COUNT(data_size) |
+	    BF_SSP_CTRL0_BUS_WIDTH(host->bus_width_4 ?
+				   BV_SSP_CTRL0_BUS_WIDTH__FOUR_BIT :
+				   BV_SSP_CTRL0_BUS_WIDTH__ONE_BIT);
+
+	/*
+	 * We need to set the hardware register to the logarithm to base 2 of
+	 * the block size.
+	 */
+	log2_block_size = ilog2(cmd->data->blksz);
+
+	ssp_cmd0 =
+	    BF_SSP_CMD0_BLOCK_SIZE(log2_block_size) |
+	    BF_SSP_CMD0_CMD(cmd->opcode) |
+	    BF_SSP_CMD0_BLOCK_COUNT(cmd->data->blocks - 1);
+
+	if (cmd->opcode == 12)
+		ssp_cmd0 |= BM_SSP_CMD0_APPEND_8CYC;
+
+	ssp_cmd1 = BF_SSP_CMD1_CMD_ARG(cmd->arg);
+
+	dma_desc->command->pio_words[0] = ssp_ctrl0;
+	dma_desc->command->pio_words[1] = ssp_cmd0;
+	dma_desc->command->pio_words[2] = ssp_cmd1;
+
+	/* Set the timeout count */
+	timeout = stmp3xxx_ns_to_ssp_ticks(host->clkrt, cmd->data->timeout_ns);
+	HW_SSP_TIMING_CLR_NB(host->ssp_base, BM_SSP_TIMING_TIMEOUT);
+	HW_SSP_TIMING_SET_NB(host->ssp_base, BF_SSP_TIMING_TIMEOUT(timeout));
+
+	init_completion(&host->dma_done);
+	stmp3xxx_dma_reset_channel(host->dmach);
+	stmp3xxx_dma_go(host->dmach, dma_desc, 1);
+	wait_for_completion(&host->dma_done);
+	if (host->regulator)
+		regulator_set_current_limit(host->regulator, 0, 0);
+
+	switch (mmc_resp_type(cmd)) {
+	case MMC_RSP_NONE:
+		break;
+	case MMC_RSP_R1:
+	case MMC_RSP_R3:
+		cmd->resp[0] = HW_SSP_SDRESP0_RD_NB(host->ssp_base);
+		break;
+	case MMC_RSP_R2:
+		cmd->resp[3] = HW_SSP_SDRESP0_RD_NB(host->ssp_base);
+		cmd->resp[2] = HW_SSP_SDRESP1_RD_NB(host->ssp_base);
+		cmd->resp[1] = HW_SSP_SDRESP2_RD_NB(host->ssp_base);
+		cmd->resp[0] = HW_SSP_SDRESP3_RD_NB(host->ssp_base);
+		break;
+	default:
+		dev_warn(host->dev, "Unsupported response type 0x%x\n",
+			 mmc_resp_type(cmd));
+		BUG();
+		break;
+	}
+
+	cmd->error = stmp3xxx_mmc_cmd_error(host->status);
+
+	if (stmp3xxx_dma_running(host->dmach))
+		dev_dbg(host->dev, "DMA command not finished\n");
+
+	if (cmd->error) {
+		dev_dbg(host->dev, "Command error 0x%x\n", cmd->error);
+		stmp3xxx_dma_reset_channel(host->dmach);
+	} else {
+		if (is_reading)
+			cmd->data->bytes_xfered =
+				stmp3xxx_sg_dma_copy(host, data_size, 0);
+		else
+			cmd->data->bytes_xfered = data_size;
+
+		dev_dbg(host->dev, "Transferred %u bytes\n",
+			cmd->data->bytes_xfered);
+	}
+}
+
+/* Begin sedning a command to the card */
+static void stmp3xxx_mmc_start_cmd(struct stmp3xxx_mmc_host *host,
+				   struct mmc_command *cmd)
+{
+	dev_dbg(host->dev, "MMC command:\n"
+		"type: 0x%x opcode: %u, arg: %u, flags 0x%x retries: %u\n",
+		mmc_cmd_type(cmd), cmd->opcode, cmd->arg, cmd->flags,
+		cmd->retries);
+
+	host->cmd = cmd;
+
+	switch (mmc_cmd_type(cmd)) {
+	case MMC_CMD_BC:
+		stmp3xxx_mmc_bc(host);
+		break;
+	case MMC_CMD_BCR:
+		stmp3xxx_mmc_ac(host);
+		break;
+	case MMC_CMD_AC:
+		stmp3xxx_mmc_ac(host);
+		break;
+	case MMC_CMD_ADTC:
+		stmp3xxx_mmc_adtc(host);
+		break;
+	default:
+		dev_warn(host->dev, "Unknown MMC command\n");
+		BUG();
+		break;
+	}
+
+	dev_dbg(host->dev, "response: %u %u %u %u errors: %u\n",
+		cmd->resp[0], cmd->resp[1], cmd->resp[2], cmd->resp[3],
+		cmd->error);
+}
+
+/* Handle MMC request */
+static void stmp3xxx_mmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct stmp3xxx_mmc_host *host = mmc_priv(mmc);
+
+	dev_dbg(host->dev, "MMC request\n");
+
+	host->mrq = mrq;
+
+	stmp3xxx_mmc_start_cmd(host, mrq->cmd);
+
+	if (mrq->data && mrq->data->stop) {
+		dev_dbg(host->dev, "Stop opcode is %u\n",
+			mrq->data->stop->opcode);
+		stmp3xxx_mmc_start_cmd(host, mrq->data->stop);
+	}
+
+	host->mrq = NULL;
+	mmc_request_done(mmc, mrq);
+}
+
+/*
+ * Change divisors to reflect the rate of 'hz'. Note that we should not
+ * play with clock rate, because the same source is used to clock both
+ * SSP ports.
+ */
+static void
+stmp3xxx_set_sclk_speed(struct stmp3xxx_mmc_host *host, unsigned int hz)
+{
+	unsigned long ssp;
+	u32 div1, div2;
+	struct stmp3xxxmmc_platform_data *pdata = host->dev->platform_data;
+
+	if (get_evk_board_version() == 1) {
+		/*EVK Ver1 max clock is 12M*/
+		if (hz > 12000000)
+			hz = 12000000;
+	}
+
+	if (pdata && pdata->setclock) {
+		/*
+		   if the SSP is buggy and platform provides callback...
+		   well, let it be.
+		 */
+		host->clkrt = pdata->setclock(hz);
+		return;
+	}
+
+	/*
+	   ...but the RightIdea(tm) is to set divisors to match
+	   the requested clock.
+	 */
+	hz /= 1000;
+
+	ssp = clk_get_rate(host->clk);
+
+	for (div1 = 2; div1 < 254; div1 += 2) {
+		div2 = ssp / hz / div1;
+		if (div2 < 0x100)
+			break;
+	}
+	if (div1 >= 254) {
+		dev_err(host->dev, "Cannot set clock to %dkHz\n", hz);
+		return;
+	}
+
+	dev_dbg(host->dev, "Setting clock rate to %ld kHz [%x+%x] "
+			"(requested %d), source %ldk\n",
+			ssp/div1/div2, div1, div2, hz, ssp);
+	HW_SSP_TIMING_CLR_NB(host->ssp_base,
+			BM_SSP_TIMING_CLOCK_DIVIDE | BM_SSP_TIMING_CLOCK_RATE);
+	HW_SSP_TIMING_SET_NB(host->ssp_base,
+			BF_SSP_TIMING_CLOCK_DIVIDE(div1) |
+			BF_SSP_TIMING_CLOCK_RATE(div2 - 1));
+	host->clkrt = ssp/div1/div2 * 1000;
+}
+
+/* Configure card */
+static void stmp3xxx_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct stmp3xxx_mmc_host *host = mmc_priv(mmc);
+	struct stmp3xxxmmc_platform_data *pdata;
+
+	dev_dbg(host->dev, "MMC set ios:\n"
+		"Clock %u, vdd %u, bus_mode %u, chip_select %u, "
+		"power mode %u, bus_width %u\n", ios->clock, ios->vdd,
+		ios->bus_mode, ios->chip_select, ios->power_mode,
+		ios->bus_width);
+
+	pdata = host->dev->platform_data;
+
+	if (pdata->cmd_pullup) {
+		if (ios->bus_mode == MMC_BUSMODE_PUSHPULL)
+			pdata->cmd_pullup(0);
+		else
+			pdata->cmd_pullup(1);
+	} else
+		dev_warn(host->dev,
+			 "Platform does not support CMD pin pullup control\n");
+
+	if (ios->bus_width == MMC_BUS_WIDTH_4)
+		host->bus_width_4 = 1;
+	else
+		host->bus_width_4 = 0;
+
+	if (ios->clock > 0)
+		stmp3xxx_set_sclk_speed(host, ios->clock);
+}
+
+static const struct mmc_host_ops stmp3xxx_mmc_ops = {
+	.request	= stmp3xxx_mmc_request,
+	.get_ro		= stmp3xxx_mmc_get_ro,
+	.set_ios	= stmp3xxx_mmc_set_ios,
+};
+
+
+/*
+ * STMP37XX MMC/SD driver initialization
+ */
+
+/* Reset ssp peripheral to default values */
+static void stmp3xxx_mmc_reset(struct stmp3xxx_mmc_host *host)
+{
+	u32 ssp_ctrl0;
+	u32 ssp_ctrl1;
+
+	stmp3xxx_reset_block(host->ssp_base, 0);
+
+	/* Configure SSP Control Register 0 */
+	ssp_ctrl0 =
+	    BM_SSP_CTRL0_IGNORE_CRC |
+	    BF_SSP_CTRL0_BUS_WIDTH(BV_SSP_CTRL0_BUS_WIDTH__ONE_BIT);
+
+	/* Configure SSP Control Register 1 */
+	ssp_ctrl1 =
+	    BM_SSP_CTRL1_DMA_ENABLE |
+	    BM_SSP_CTRL1_POLARITY |
+	    BM_SSP_CTRL1_RECV_TIMEOUT_IRQ_EN |
+	    BM_SSP_CTRL1_DATA_CRC_IRQ_EN |
+	    BM_SSP_CTRL1_DATA_TIMEOUT_IRQ_EN |
+	    BM_SSP_CTRL1_RESP_TIMEOUT_IRQ_EN |
+	    BM_SSP_CTRL1_RESP_ERR_IRQ_EN |
+	    BF_SSP_CTRL1_WORD_LENGTH(BV_SSP_CTRL1_WORD_LENGTH__EIGHT_BITS) |
+	    BF_SSP_CTRL1_SSP_MODE(BV_SSP_CTRL1_SSP_MODE__SD_MMC);
+
+	HW_SSP_TIMING_WR(BF_SSP_TIMING_TIMEOUT(0xFFFF) |
+			BF_SSP_TIMING_CLOCK_DIVIDE(2) |
+			BF_SSP_TIMING_CLOCK_RATE(0));
+
+	/* Write the SSP Control Register 0 and 1 values out to the interface */
+	HW_SSP_CTRL0_WR_NB(host->ssp_base, ssp_ctrl0);
+	HW_SSP_CTRL1_WR_NB(host->ssp_base, ssp_ctrl1);
+}
+
+static void stmp3xxx_mmc_irq_release(struct stmp3xxx_mmc_host *host)
+{
+	free_irq(host->dmairq, host);
+	free_irq(host->errirq, host);
+}
+
+static int __init stmp3xxx_mmc_irq_init(struct stmp3xxx_mmc_host *host)
+{
+	int ret;
+
+	ret = request_irq(host->dmairq, mmc_irq_handler, 0,
+			  DRIVER_NAME" dma", host);
+	if (ret) {
+		dev_err(host->dev, "Unable to set up DMA irq handler\n");
+		goto out0;
+	}
+
+	ret = request_irq(host->errirq, mmc_irq_handler, IRQF_SHARED,
+			  DRIVER_NAME" error", host);
+	if (ret) {
+		dev_err(host->dev, "Unable to set up SSP error irq handler\n");
+		goto out1;
+	}
+	return 0;
+
+out1:
+	free_irq(host->dmairq, host);
+out0:
+	return ret;
+}
+
+
+/* Allocate and initialise the DMA chains */
+static int stmp3xxx_mmc_dma_init(struct stmp3xxx_mmc_host *host, int reset)
+{
+	int ret;
+
+	if (!reset) {
+		/* Allocate DMA channel */
+		ret = stmp3xxx_dma_request(host->dmach,
+				host->dev, "STMP37XX MMC/SD");
+		if (ret) {
+			dev_err(host->dev, "Unable to request DMA channel\n");
+			return ret;
+		}
+
+		host->dma_buf = dma_alloc_coherent(host->dev, SSP_BUFFER_SIZE,
+					   &host->dma_buf_phys, GFP_DMA);
+		if (host->dma_buf == NULL) {
+			dev_err(host->dev, "Unable to allocate DMA memory\n");
+			ret = -ENOMEM;
+			goto out_mem;
+		}
+
+		ret = stmp3xxx_dma_allocate_command(host->dmach,
+				&host->dma_desc);
+		if (ret) {
+			dev_err(host->dev,
+				"Unable to allocate DMA descriptor\n");
+			goto out_cmd;
+		}
+
+		host->dma_desc.command->next = (u32)host->dma_desc.handle;
+		host->dma_desc.command->buf_ptr = (u32)host->dma_buf_phys;
+		host->dma_desc.virtual_buf_ptr = host->dma_buf;
+	}
+
+	/* Reset DMA channel */
+	stmp3xxx_dma_reset_channel(host->dmach);
+
+	/* Enable DMA interrupt */
+	stmp3xxx_dma_clear_interrupt(host->dmach);
+	stmp3xxx_dma_enable_interrupt(host->dmach);
+
+	return 0;
+
+out_cmd:
+	dma_free_coherent(host->dev, SSP_BUFFER_SIZE, host->dma_buf,
+			  host->dma_buf_phys);
+out_mem:
+	stmp3xxx_dma_release(host->dmach);
+
+	return ret;
+}
+
+static void stmp3xxx_mmc_dma_release(struct stmp3xxx_mmc_host *host)
+{
+	stmp3xxx_dma_reset_channel(host->dmach);
+
+	dma_free_coherent(host->dev, SSP_BUFFER_SIZE, host->dma_buf,
+			  host->dma_buf_phys);
+
+	stmp3xxx_dma_free_command(host->dmach, &host->dma_desc);
+	stmp3xxx_dma_release(host->dmach);
+}
+
+/* Probe peripheral for connected cards */
+static int __init stmp3xxx_mmc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct stmp3xxxmmc_platform_data *mmc_data;
+	struct stmp3xxx_mmc_host *host;
+	struct mmc_host *mmc;
+	struct resource *r;
+	int err = 0;
+
+	mmc_data = dev->platform_data;
+	if (mmc_data == NULL) {
+		err = -EINVAL;
+		dev_err(dev, "Missing platform data\n");
+		goto out;
+	}
+
+	/* Allocate main MMC host structure */
+	mmc = mmc_alloc_host(sizeof(struct stmp3xxx_mmc_host), dev);
+	if (!mmc) {
+		dev_err(dev, "Unable to allocate MMC host\n");
+		err = -ENOMEM;
+		goto out;
+	}
+	host = mmc_priv(mmc);
+
+	host->read_uA = mmc_data->read_uA;
+	host->write_uA = mmc_data->write_uA;
+	host->regulator = regulator_get(NULL, "mmc_ssp-1");
+	if (host->regulator && !IS_ERR(host->regulator))
+		regulator_set_mode(host->regulator, REGULATOR_MODE_NORMAL);
+	else
+		host->regulator = NULL;
+
+	/* get resources: */
+
+	/*
+	 *  1. io memory
+	 */
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "failed to get IORESOURCE_MEM\n");
+		err = -ENXIO;
+		goto out_res;
+	}
+	host->ssp_base = r->start; /* it is already ioremapped */
+
+	/*
+	 *  2. DMA channel
+	 */
+	r = platform_get_resource(pdev, IORESOURCE_DMA, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "failed to get IORESOURCE_DMA\n");
+		err = -ENXIO;
+		goto out_res;
+	}
+	host->dmach = r->start;
+
+	/*
+	 * 3. two IRQs
+	 */
+	host->dmairq = platform_get_irq(pdev, 0);
+	if (host->dmairq < 0) {
+		dev_err(&pdev->dev, "failed to get IORESOURCE_IRQ/0\n");
+		err = host->dmairq;
+		goto out_res;
+	}
+
+	host->errirq = platform_get_irq(pdev, 1);
+	if (host->errirq < 0) {
+		dev_err(&pdev->dev, "failed to get IORESOURCE_IRQ/1\n");
+		err = host->errirq;
+		goto out_res;
+	}
+
+	/* Set up MMC pins */
+	if (mmc_data->hw_init) {
+		err = mmc_data->hw_init();
+		if (err) {
+			dev_err(dev, "MMC HW configuration failed\n");
+			goto out_res;
+		}
+	}
+
+	host->mmc = mmc;
+	host->dev = dev;
+
+	/* Set minimal clock rate */
+	host->clk = clk_get(dev, "ssp");
+	if (IS_ERR(host->clk)) {
+		err = PTR_ERR(host->clk);
+		dev_err(dev, "Clocks initialization failed\n");
+		goto out_clk;
+	}
+
+	clk_enable(host->clk);
+	stmp3xxx_set_sclk_speed(host, CLOCKRATE_MIN);
+
+	/* Reset MMC block */
+	stmp3xxx_mmc_reset(host);
+
+	/* Enable DMA */
+	err = stmp3xxx_mmc_dma_init(host, 0);
+	if (err) {
+		dev_err(dev, "DMA init failed\n");
+		goto out_dma;
+	}
+
+	/* Set up interrupt handlers */
+	err = stmp3xxx_mmc_irq_init(host);
+	if (err) {
+		dev_err(dev, "IRQ initialization failed\n");
+		goto out_irq;
+	}
+
+	/* Get current card status for further cnanges tracking */
+	host->present = stmp3xxx_mmc_is_plugged(host);
+
+	/* Add a card detection polling timer */
+	init_timer(&host->timer);
+	host->timer.function = stmp3xxx_mmc_detect_poll;
+	host->timer.data = (unsigned long)host;
+	host->timer.expires = jiffies + STMP37XX_MMC_DETECT_TIMEOUT;
+	add_timer(&host->timer);
+
+	mmc->ops = &stmp3xxx_mmc_ops;
+	mmc->f_min = CLOCKRATE_MIN;
+	mmc->f_max = CLOCKRATE_MAX;
+	mmc->caps = MMC_CAP_4_BIT_DATA;
+
+	/* Maximum block count requests. */
+	mmc->max_blk_size = 512;
+	mmc->max_blk_count = SSP_BUFFER_SIZE / 512;
+	mmc->max_hw_segs = SSP_BUFFER_SIZE / 512;
+	mmc->max_phys_segs = SSP_BUFFER_SIZE / 512;
+	mmc->max_req_size = SSP_BUFFER_SIZE;
+	mmc->max_seg_size = SSP_BUFFER_SIZE;
+
+	mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34;
+
+	platform_set_drvdata(pdev, mmc);
+
+	err = mmc_add_host(mmc);
+	if (err) {
+		dev_err(dev, "Oh God. mmc_add_host failed\n");
+		goto out_all;
+	}
+
+	return err;
+
+out_all:
+
+out_irq:
+	stmp3xxx_mmc_dma_release(host);
+out_dma:
+	clk_disable(host->clk);
+out_clk:
+	if (mmc_data->hw_release)
+		mmc_data->hw_release();
+out_res:
+	mmc_free_host(mmc);
+out:
+	return err;
+}
+
+static int __exit stmp3xxx_mmc_remove(struct platform_device *pdev)
+{
+	struct stmp3xxx_mmc_host *host;
+	struct stmp3xxxmmc_platform_data *mmc_data;
+	struct mmc_host *mmc;
+
+	dev_info(&pdev->dev, "Removing\n");
+
+	mmc_data = pdev->dev.platform_data;
+	mmc = platform_get_drvdata(pdev);
+	platform_set_drvdata(pdev, NULL);
+
+	host = mmc_priv(mmc);
+	mmc_remove_host(mmc);
+
+	/* Disable SSP clock */
+	clk_disable(host->clk);
+	clk_put(host->clk);
+
+	/* Release IRQs */
+	stmp3xxx_mmc_irq_release(host);
+
+	/* Delete card detection timer */
+	del_timer(&host->timer);
+
+	/* Release DMA */
+	stmp3xxx_mmc_dma_release(host);
+	if (host->regulator)
+		regulator_put(host->regulator);
+
+	mmc_free_host(mmc);
+
+	if (mmc_data->hw_release)
+		mmc_data->hw_release();
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int stmp3xxx_mmc_suspend(struct platform_device *pdev,
+		pm_message_t state)
+{
+	struct stmp3xxx_mmc_host *host;
+	struct stmp3xxxmmc_platform_data *mmc_data;
+	struct mmc_host *mmc;
+	int ret = 0;
+
+	dev_dbg(&pdev->dev, "Suspending\n");
+
+	mmc_data = pdev->dev.platform_data;
+	mmc = platform_get_drvdata(pdev);
+	host = mmc_priv(mmc);
+
+	ret = mmc_suspend_host(mmc, state);
+	if (!ret) {
+		if (mmc_data && mmc_data->hw_release)
+			mmc_data->hw_release();
+		clk_disable(host->clk);
+	}
+	return ret;
+}
+
+static int stmp3xxx_mmc_resume(struct platform_device *pdev)
+{
+	struct stmp3xxx_mmc_host *host;
+	struct stmp3xxxmmc_platform_data *mmc_data;
+	struct mmc_host *mmc;
+
+	dev_dbg(&pdev->dev, "Resuming\n");
+
+	mmc_data = pdev->dev.platform_data;
+	mmc = platform_get_drvdata(pdev);
+	host = mmc_priv(mmc);
+
+	clk_enable(host->clk);
+
+	if (mmc_data->hw_init)
+		mmc_data->hw_init();
+	stmp3xxx_mmc_reset(host);
+	stmp3xxx_mmc_dma_init(host, 1);
+
+	return mmc_resume_host(mmc);
+}
+#else
+#define stmp3xxx_mmc_suspend	NULL
+#define stmp3xxx_mmc_resume	NULL
+#endif /* CONFIG_PM */
+
+static struct platform_driver stmp3xxx_mmc_driver = {
+	.probe		= stmp3xxx_mmc_probe,
+	.remove		= __exit_p(stmp3xxx_mmc_remove),
+	.suspend	= stmp3xxx_mmc_suspend,
+	.resume		= stmp3xxx_mmc_resume,
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init stmp3xxx_mmc_init(void)
+{
+	int ret = 0;
+
+	ret = platform_driver_register(&stmp3xxx_mmc_driver);
+	if (ret < 0)
+		return ret;
+
+	return ret;
+}
+
+static void __exit stmp3xxx_mmc_exit(void)
+{
+	platform_driver_unregister(&stmp3xxx_mmc_driver);
+}
+
+module_init(stmp3xxx_mmc_init);
+module_exit(stmp3xxx_mmc_exit);
+
+MODULE_DESCRIPTION("STMP37xx/378x MMC peripheral");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index 5ea169362164..868c4df8b7e4 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -540,5 +540,15 @@ config MTD_PLATRAM
 
 	  This selection automatically selects the map_ram driver.
 
+config MTD_MXC
+	bool "Map driver for Freescale MXC boards"
+	depends on MTD && ARCH_MXC
+	default y
+	select MTD_CFI
+	select MTD_PARTITIONS
+	help
+	  This enables access to the flash chips on Freescale MXC based
+	  platforms. If you have such a board, say 'Y'.
+
 endmenu
 
diff --git a/drivers/mtd/maps/Makefile b/drivers/mtd/maps/Makefile
index 6d9ba35caf11..9070771637dc 100644
--- a/drivers/mtd/maps/Makefile
+++ b/drivers/mtd/maps/Makefile
@@ -61,3 +61,4 @@ obj-$(CONFIG_MTD_PLATRAM)	+= plat-ram.o
 obj-$(CONFIG_MTD_OMAP_NOR)	+= omap_nor.o
 obj-$(CONFIG_MTD_INTEL_VR_NOR)	+= intel_vr_nor.o
 obj-$(CONFIG_MTD_BFIN_ASYNC)	+= bfin-async-flash.o
+obj-$(CONFIG_MTD_MXC)		+= mxc_nor.o
diff --git a/drivers/mtd/maps/mxc_nor.c b/drivers/mtd/maps/mxc_nor.c
new file mode 100644
index 000000000000..e89e534aa440
--- /dev/null
+++ b/drivers/mtd/maps/mxc_nor.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ * (c) 2005 MontaVista Software, Inc.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/ioport.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/partitions.h>
+#include <linux/clocksource.h>
+#include <asm/mach-types.h>
+#include <asm/mach/flash.h>
+
+#define DVR_VER "2.0"
+
+#ifdef CONFIG_MTD_PARTITIONS
+static const char *part_probes[] = { "RedBoot", "cmdlinepart", NULL };
+#endif
+
+struct clocksource *mtd_xip_clksrc;
+
+struct mxcflash_info {
+	struct mtd_partition *parts;
+	struct mtd_info *mtd;
+	struct map_info map;
+};
+
+/*!
+ * @defgroup NOR_MTD NOR Flash MTD Driver
+ */
+
+/*!
+ * @file mxc_nor.c
+ *
+ * @brief This file contains the MTD Mapping information on the MXC.
+ *
+ * @ingroup NOR_MTD
+ */
+
+static int __devinit mxcflash_probe(struct platform_device *pdev)
+{
+	int err, nr_parts = 0;
+	struct mxcflash_info *info;
+	struct flash_platform_data *flash = pdev->dev.platform_data;
+	struct resource *res = pdev->resource;
+	unsigned long size = res->end - res->start + 1;
+
+	info = kzalloc(sizeof(struct mxcflash_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	if (!request_mem_region(res->start, size, "flash")) {
+		err = -EBUSY;
+		goto out_free_info;
+	}
+	info->map.virt = ioremap(res->start, size);
+	if (!info->map.virt) {
+		err = -ENOMEM;
+		goto out_release_mem_region;
+	}
+	info->map.name = pdev->dev.bus_id;
+	info->map.phys = res->start;
+	info->map.size = size;
+	info->map.bankwidth = flash->width;
+
+	mtd_xip_clksrc = clocksource_get_next();
+
+	simple_map_init(&info->map);
+	info->mtd = do_map_probe(flash->map_name, &info->map);
+	if (!info->mtd) {
+		err = -EIO;
+		goto out_iounmap;
+	}
+	info->mtd->owner = THIS_MODULE;
+
+#ifdef CONFIG_MTD_PARTITIONS
+	nr_parts =
+	    parse_mtd_partitions(info->mtd, part_probes, &info->parts, 0);
+	if (nr_parts > 0) {
+		add_mtd_partitions(info->mtd, info->parts, nr_parts);
+	} else if (flash->parts) {
+		add_mtd_partitions(info->mtd, flash->parts, flash->nr_parts);
+	} else
+#endif
+	{
+		printk(KERN_NOTICE "MXC flash: no partition info "
+		       "available, registering whole flash\n");
+		add_mtd_device(info->mtd);
+	}
+
+	platform_set_drvdata(pdev, info);
+	return 0;
+
+      out_iounmap:
+	iounmap(info->map.virt);
+      out_release_mem_region:
+	release_mem_region(res->start, size);
+      out_free_info:
+	kfree(info);
+
+	return err;
+}
+
+static int __devexit mxcflash_remove(struct platform_device *pdev)
+{
+
+	struct mxcflash_info *info = platform_get_drvdata(pdev);
+	struct flash_platform_data *flash = pdev->dev.platform_data;
+
+	platform_set_drvdata(pdev, NULL);
+
+	if (info) {
+		if (info->parts) {
+			del_mtd_partitions(info->mtd);
+			kfree(info->parts);
+		} else if (flash->parts)
+			del_mtd_partitions(info->mtd);
+		else
+			del_mtd_device(info->mtd);
+
+		map_destroy(info->mtd);
+		release_mem_region(info->map.phys, info->map.size);
+		iounmap((void __iomem *)info->map.virt);
+		kfree(info);
+	}
+	return 0;
+}
+
+static struct platform_driver mxcflash_driver = {
+	.driver = {
+		   .name = "mxc_nor_flash",
+		   },
+	.probe = mxcflash_probe,
+	.remove = __devexit_p(mxcflash_remove),
+};
+
+/*!
+ * This is the module's entry function. It passes board specific
+ * config details into the MTD physmap driver which then does the
+ * real work for us. After this function runs, our job is done.
+ *
+ * @return  0 if successful; non-zero otherwise
+ */
+static int __init mxc_mtd_init(void)
+{
+	pr_info("MXC MTD nor Driver %s\n", DVR_VER);
+	if (platform_driver_register(&mxcflash_driver) != 0) {
+		printk(KERN_ERR "Driver register failed for mxcflash_driver\n");
+		return -ENODEV;
+	}
+	return 0;
+}
+
+/*!
+ * This function is the module's exit function. It's empty because the
+ * MTD physmap driver is doing the real work and our job was done after
+ * mxc_mtd_init() runs.
+ */
+static void __exit mxc_mtd_exit(void)
+{
+	platform_driver_unregister(&mxcflash_driver);
+}
+
+module_init(mxc_mtd_init);
+module_exit(mxc_mtd_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MTD map and partitions for Freescale MXC boards");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 1c2e9450d663..95ed64f5ac94 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -365,6 +365,102 @@ config MTD_NAND_NANDSIM
 	  The simulator may simulate various NAND flash chips for the
 	  MTD nand layer.
 
+config MTD_NAND_IMX_NFC
+	tristate "i.MX NAND Flash Controller driver"
+	depends on MTD_NAND
+	help
+	  Enables the i.MX NAND Flash controller driver.
+
+config MTD_NAND_MXC
+	tristate "MXC NAND support"
+	depends on MTD_NAND && ARCH_MXC_HAS_NFC_V1
+	help
+	  This enables the driver for the NAND flash controller on the
+	  MXC processors.
+
+config MTD_NAND_MXC_V2
+	tristate "MXC NAND Version 2 support"
+	depends on MTD_NAND && ARCH_MXC_HAS_NFC_V2
+	help
+	  This enables the driver for the version 2 of NAND flash controller
+	  on the MXC processors.
+
+config MTD_NAND_MXC_V3
+	tristate "MXC NAND Version 3 support"
+	depends on MTD_NAND && ARCH_MXC_HAS_NFC_V3
+	help
+	  This enables the driver for the version 3 of NAND flash controller
+	  on the MXC processors.
+
+config MTD_NAND_MXC_SWECC
+	bool "Software ECC support "
+	depends on MTD_NAND_MXC || MTD_NAND_MXC_V2 || MTD_NAND_MXC_V3
+	help
+	  This enables the support for Software ECC handling. By
+	  default MXC NAND controller Hardware ECC is supported.
+
+
+config MTD_NAND_MXC_FORCE_CE
+	bool "NAND chip select operation support"
+	 depends on MTD_NAND_MXC || MTD_NAND_MXC_V2|| MTD_NAND_MXC_V3
+	help
+	  This enables the NAND chip select by using CE control line. By
+	  default CE operation is disabled.
+
+config MTD_NAND_MXC_ECC_CORRECTION_OPTION2
+	bool "ECC correction in S/W"
+	depends on MTD_NAND_MXC
+	help
+	  This enables the Option2 NFC ECC correction in software. By
+	  default Option 1 is selected. Enable if you need option2 ECC correction.
+
+config MXC_NAND_LOW_LEVEL_ERASE
+	bool "Low level NAND erase"
+	depends on MTD_NAND_MXC || MTD_NAND_MXC_V2 || MTD_NAND_MXC_V3
+	help
+	  This enables the erase of whole NAND flash. By
+	  default low level erase operation is disabled.
+
+config MTD_NAND_GPMI_LBA
+	tristate "GPMI LBA NAND driver"
+	depends on MTD_NAND && ARCH_STMP3XXX
+	help
+	 Enables support of LBA devices on GPMI on 37xx/378x SigmaTel
+	 boards
+
+config MTD_NAND_GPMI
+	tristate "GPMI NAND driver"
+	depends on MTD_NAND && ARCH_STMP3XXX && !MTD_NAND_GPMI_LBA
+	help
+	 Enables support of NAND devices on GPMI on 37xx/378x SigmaTel
+	 boards
+
+config MTD_NAND_GPMI_SYSFS_ENTRIES
+	bool "Create /sys entries for GPMI device"
+	depends on MTD_NAND_GPMI
+	help
+	  Check this to enable /sys entries for GPMI devices
+
+config MTD_NAND_GPMI_BCH
+	bool "Enable BCH HWECC"
+	depends on MTD_NAND_GPMI
+	depends on ARCH_STMP378X
+	default y
+	help
+	  Check this to enable /sys entries for GPMI devices
+
+config MTD_NAND_GPMI_TA1
+	bool "Support for TA1 NCB format (Hamming code 22,16)"
+	depends on MTD_NAND_GPMI
+	depends on ARCH_STMP378X
+	default y
+
+config MTD_NAND_GPMI_TA3
+	bool "Support for TA3 NCB format (Hamming code 13,8)"
+	depends on MTD_NAND_GPMI
+	depends on ARCH_STMP378X
+	default y
+
 config MTD_NAND_PLATFORM
 	tristate "Support for generic platform NAND driver"
 	depends on MTD_NAND
@@ -420,4 +516,11 @@ config MTD_NAND_SH_FLCTL
 	  Several Renesas SuperH CPU has FLCTL. This option enables support
 	  for NAND Flash using FLCTL. This driver support SH7723.
 
+config MTD_NAND_CCX9X
+	tristate "NAND Flash on CC9X/CCW9C"
+	depends on MTD_NAND && (MODULE_CC9P9360 || MODULE_CC9C || MODULE_CCW9C)
+	help
+	  This enables the driver for the internal NAND flash on Digi NS9360
+	  based modules CC9P9360, CC9C and CCW9C.
+
 endif # MTD_NAND
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index b661586afbfc..f5021eaeb0d9 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -24,6 +24,12 @@ obj-$(CONFIG_MTD_NAND_CS553X)		+= cs553x_nand.o
 obj-$(CONFIG_MTD_NAND_NDFC)		+= ndfc.o
 obj-$(CONFIG_MTD_NAND_ATMEL)		+= atmel_nand.o
 obj-$(CONFIG_MTD_NAND_GPIO)		+= gpio.o
+obj-$(CONFIG_MTD_NAND_IMX_NFC)		+= imx_nfc.o
+obj-$(CONFIG_MTD_NAND_MXC)		+= mxc_nd.o
+obj-$(CONFIG_MTD_NAND_MXC_V2)		+= mxc_nd2.o
+obj-$(CONFIG_MTD_NAND_MXC_V3)		+= mxc_nd2.o
+obj-$(CONFIG_MTD_NAND_GPMI)		+= gpmi/
+obj-$(CONFIG_MTD_NAND_GPMI_LBA)		+= lba/
 obj-$(CONFIG_MTD_NAND_CM_X270)		+= cmx270_nand.o
 obj-$(CONFIG_MTD_NAND_BASLER_EXCITE)	+= excite_nandflash.o
 obj-$(CONFIG_MTD_NAND_PXA3xx)		+= pxa3xx_nand.o
@@ -36,5 +42,6 @@ obj-$(CONFIG_MTD_NAND_FSL_ELBC)		+= fsl_elbc_nand.o
 obj-$(CONFIG_MTD_NAND_FSL_UPM)		+= fsl_upm.o
 obj-$(CONFIG_MTD_NAND_SH_FLCTL)		+= sh_flctl.o
 obj-$(CONFIG_MTD_NAND_MXC)		+= mxc_nand.o
+obj-$(CONFIG_MTD_NAND_CCX9X)		+= ccx9x_nand.o
 
 nand-objs := nand_base.o nand_bbt.o
diff --git a/drivers/mtd/nand/ccx9x_nand.c b/drivers/mtd/nand/ccx9x_nand.c
new file mode 100644
index 000000000000..af2ace9b99b4
--- /dev/null
+++ b/drivers/mtd/nand/ccx9x_nand.c
@@ -0,0 +1,267 @@
+/*
+ * drivers/mtd/nand/ccx9x_nand.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ * Based on drivers/mtd/nand/ccx9x_nand.c by Markus Pietrek
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/mtd/ccx9x_nand.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/partitions.h>
+#include <linux/platform_device.h>
+
+#include <mach/gpio.h>
+
+#define DRIVER_NAME "ccx9x_nand"
+#define DEVICE_NAME "onboard_boot"
+
+#define mi2ncd(mi) container_of(mi, struct nand_ccx9x, mtd)
+
+#ifdef CONFIG_MTD_PARTITIONS
+static const char *probes[] = { "cmdlinepart", NULL };
+#endif
+
+struct nand_ccx9x {
+	struct mtd_info mtd;
+	struct nand_chip chip;
+
+	struct resource *mem;
+	struct ccx9x_nand_info *nand_info;
+};
+
+/* read in gpio ready pin */
+static int ccx9x_nand_dev_ready(struct mtd_info *mtd)
+{
+	struct nand_ccx9x *ncd = mi2ncd(mtd);
+
+	return gpio_get_value(ncd->nand_info->busy_pin);
+}
+
+static void ccx9x_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
+		unsigned int ctrl)
+{
+	struct nand_ccx9x *ncd = mi2ncd(mtd);
+
+	if (cmd == NAND_CMD_NONE)
+		return;
+
+	if (ctrl & NAND_CLE)
+		writeb(cmd, ncd->chip.IO_ADDR_W + ncd->nand_info->cmd_offset);
+	else
+		writeb(cmd, ncd->chip.IO_ADDR_W + ncd->nand_info->addr_offset);
+}
+
+static void ccx9x_nand_read_buf(struct mtd_info *mtd,
+		u_char *buf, int len)
+{
+	struct nand_ccx9x *ncd = mi2ncd(mtd);
+
+	while (len) {
+		*buf = ioread8(ncd->chip.IO_ADDR_R);
+		buf++;
+		len--;
+	}
+}
+
+static void ccx9x_nand_write_buf(struct mtd_info *mtd,
+		const u_char *buf, int len)
+{
+	struct nand_ccx9x *ncd = mi2ncd(mtd);
+
+	while (len) {
+		iowrite8(*buf, ncd->chip.IO_ADDR_W);
+		buf++;
+		len--;
+	}
+
+	/* FIXME: 32bit burst writing */
+}
+
+static int ccx9x_nand_verify_buf(struct mtd_info *mtd,
+		const u_char *buf, int len)
+{
+	static u_char *tmp;
+	int i, ret = 0;
+
+	tmp = kmalloc(len, GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	/* read fromd device */
+	ccx9x_nand_read_buf(mtd, tmp, len);
+
+	/* compare */
+	for (i = 0; i < len; i++)
+		if (tmp[i] != buf[i]) {
+			ret = -EFAULT;
+			break;
+		}
+
+	kfree(tmp);
+	return ret;
+
+}
+
+static int ccx9x_nand_probe(struct platform_device *pdev)
+{
+	struct nand_ccx9x *ncd;
+	struct resource *mem;
+	struct mtd_partition *mtd_parts;
+	int num_parts;
+	int ret;
+
+	ncd = kzalloc(sizeof(*ncd), GFP_KERNEL);
+	if (!ncd) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc\n", __func__);
+		goto err_alloc;
+	}
+
+	ncd->nand_info = pdev->dev.platform_data;
+	if (!ncd->nand_info) {
+		ret = -ENODEV;
+		dev_dbg(&pdev->dev, "%s: err_pdata\n", __func__);
+		goto err_pdata;
+	}
+
+	platform_set_drvdata(pdev, ncd);
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem) {
+		ret = -ENODEV;
+		dev_dbg(&pdev->dev, "%s: err_get_mem\n", __func__);
+		goto err_get_mem;
+	}
+
+	if (!request_mem_region(mem->start, mem->end - mem->start,
+				DRIVER_NAME)) {
+		ret = -EBUSY;
+		dev_dbg(&pdev->dev, "%s: err_request_mem\n", __func__);
+		goto err_request_mem;
+	}
+
+	ncd->chip.IO_ADDR_W = ioremap(mem->start, mem->end - mem->start);
+	if (!ncd->chip.IO_ADDR_W) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_ioremap\n", __func__);
+		goto err_ioremap;
+	}
+	ncd->chip.IO_ADDR_R = ncd->chip.IO_ADDR_W;
+	ncd->mem = mem;
+
+	if (ncd->nand_info->busy_pin) {
+		ret = gpio_request(ncd->nand_info->busy_pin, DRIVER_NAME);
+		if (ret) {
+			dev_dbg(&pdev->dev, "%s: err_gpio_request\n", __func__);
+			goto err_gpio_request;
+		}
+		gpio_direction_input(ncd->nand_info->busy_pin);
+
+		ncd->chip.dev_ready = ccx9x_nand_dev_ready;
+	}
+
+	ncd->chip.write_buf = ccx9x_nand_write_buf;
+	ncd->chip.read_buf = ccx9x_nand_read_buf;
+	ncd->chip.verify_buf = ccx9x_nand_verify_buf;
+	ncd->chip.cmd_ctrl = ccx9x_nand_cmd_ctrl;
+	ncd->chip.ecc.mode = NAND_ECC_SOFT;
+	ncd->chip.chip_delay = ncd->nand_info->delay;
+	ncd->chip.controller = NULL;
+
+	ncd->mtd.name = DEVICE_NAME;
+	ncd->mtd.priv = &ncd->chip;
+
+	if (nand_scan(&ncd->mtd, 1)) {
+		ret = -ENODEV;
+		dev_dbg(&pdev->dev, "%s: err_scan\n", __func__);
+		goto err_scan;
+	}
+
+	dev_dbg(&pdev->dev, "NAND Flash memory mapped to virtual %p\n",
+			ncd->chip.IO_ADDR_W);
+
+#ifdef CONFIG_MTD_CMDLINE_PARTS
+	/* read partition information from cmdline*/
+	num_parts = parse_mtd_partitions(&ncd->mtd, probes, &mtd_parts, 0);
+
+	/* register partitions */
+	add_mtd_partitions(&ncd->mtd, mtd_parts, num_parts);
+#endif
+
+	return 0;
+
+err_scan:
+	if (ncd->nand_info->busy_pin)
+		gpio_free(ncd->nand_info->busy_pin);
+err_gpio_request:
+	iounmap(ncd->chip.IO_ADDR_W);
+err_ioremap:
+	release_mem_region(mem->start, mem->end - mem->start);
+err_request_mem:
+	release_resource(mem);
+err_get_mem:
+err_pdata:
+	kfree(ncd);
+err_alloc:
+	return ret;
+}
+
+static int ccx9x_nand_remove(struct platform_device *pdev)
+{
+	struct nand_ccx9x *ncd = platform_get_drvdata(pdev);
+
+	if (ncd->nand_info->busy_pin)
+		gpio_free(ncd->nand_info->busy_pin);
+	iounmap(ncd->chip.IO_ADDR_W);
+	release_mem_region(ncd->mem->start,
+			ncd->mem->end - ncd->mem->start);
+	release_resource(ncd->mem);
+	kfree(ncd);
+
+	return 0;
+}
+
+static struct platform_driver ccx9x_nand_driver = {
+	.probe = ccx9x_nand_probe,
+	.remove = ccx9x_nand_remove,
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ccx9x_nand_init(void)
+{
+	int ret = -ENOMEM;
+
+	ret = platform_driver_register(&ccx9x_nand_driver);
+	if (ret) {
+		pr_debug("%s: err_pdev_register\n", __func__);
+		goto err_pdev_register;
+	}
+
+	return 0;
+
+err_pdev_register:
+	return ret;
+}
+
+static void __exit ccx9x_nand_exit(void)
+{
+	platform_driver_unregister(&ccx9x_nand_driver);
+}
+
+module_init(ccx9x_nand_init);
+module_exit(ccx9x_nand_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Matthias Ludwig");
+MODULE_DESCRIPTION("Digi CCx9x MTD NAND driver");
diff --git a/drivers/mtd/nand/gpmi/Makefile b/drivers/mtd/nand/gpmi/Makefile
new file mode 100644
index 000000000000..4a4b50d294fa
--- /dev/null
+++ b/drivers/mtd/nand/gpmi/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_MTD_NAND_GPMI) += gpmi.o
+gpmi-objs += gpmi-base.o gpmi-bbt.o
+gpmi-objs += gpmi-hamming-22-16.o
+gpmi-objs += gpmi-hamming-13-8.o
+gpmi-objs += gpmi-bch.o
+gpmi-objs += gpmi-ecc8.o
diff --git a/drivers/mtd/nand/gpmi/gpmi-base.c b/drivers/mtd/nand/gpmi/gpmi-base.c
new file mode 100644
index 000000000000..547dec9f29f8
--- /dev/null
+++ b/drivers/mtd/nand/gpmi/gpmi-base.c
@@ -0,0 +1,1979 @@
+/*
+ * Freescale STMP37XX/STMP378X GPMI (General-Purpose-Media-Interface)
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/partitions.h>
+#include <linux/mtd/concat.h>
+#include <linux/dma-mapping.h>
+#include <linux/ctype.h>
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+
+#include <mach/stmp3xxx.h>
+#include <mach/regs-ecc8.h>
+#include <mach/dma.h>
+#include "gpmi.h"
+
+static int debug;
+static int copies;
+static int map_buffers = true;
+static int ff_writes;
+static int raw_mode;
+static int add_mtd_entire;
+static int add_mtd_chip;
+static int ignorebad;
+static int max_chips = 4;
+static long clk = -1;
+static int bch /* = 0 */;
+
+static int gpmi_nand_init_hw(struct platform_device *pdev, int request_pins);
+static void gpmi_nand_release_hw(struct platform_device *pdev);
+static int gpmi_dma_exchange(struct gpmi_nand_data *g,
+		struct stmp3xxx_dma_descriptor *dma);
+static void gpmi_read_buf(struct mtd_info *mtd, uint8_t *buf, int len);
+
+struct gpmi_nand_timing gpmi_safe_timing = {
+	.address_setup = 25,
+	.data_setup = 80,
+	.data_hold = 60,
+	.dsample_time = 6,
+};
+
+/*
+ * define OOB placement schemes for 4k and 2k page devices
+ */
+static struct nand_ecclayout gpmi_oob_128 = {
+	.oobfree = {
+		{
+			.offset = 2,
+			.length = 56,
+		}, {
+			.length = 0,
+		},
+	},
+};
+
+static struct nand_ecclayout gpmi_oob_64 = {
+	.oobfree = {
+		{
+			.offset = 2,
+			.length = 16,
+		}, {
+			.length = 0,
+		},
+	},
+};
+
+static inline u32 gpmi_cycles_ceil(u32 ntime, u32 period)
+{
+	int k;
+
+	k = (ntime + period - 1) / period;
+	if (k == 0)
+		k++;
+	return k;
+}
+
+/**
+ * gpmi_timer_expiry - timer expiry handling
+ */
+static void gpmi_timer_expiry(unsigned long d)
+{
+#ifdef CONFIG_PM
+	struct gpmi_nand_data *g = (struct gpmi_nand_data *)d;
+
+	pr_debug("%s: timer expired\n", __func__);
+	del_timer_sync(&g->timer);
+
+	if (g->use_count ||
+	    HW_GPMI_CTRL0_RD() & BM_GPMI_CTRL0_RUN) {
+		g->timer.expires = jiffies + 4*HZ;
+		add_timer(&g->timer);
+	} else {
+		HW_GPMI_CTRL0_SET(BM_GPMI_CTRL0_CLKGATE);
+		clk_disable(g->clk);
+		g->self_suspended = 1;
+	}
+#endif
+}
+
+/**
+ * gpmi_self_wakeup - wakeup from self-pm light suspend
+ */
+static void gpmi_self_wakeup(struct gpmi_nand_data *g)
+{
+#ifdef CONFIG_PM
+	int i = 1000;
+	clk_enable(g->clk);
+	HW_GPMI_CTRL0_CLR(BM_GPMI_CTRL0_CLKGATE);
+	while (i-- && HW_GPMI_CTRL0_RD() & BM_GPMI_CTRL0_CLKGATE);
+
+	pr_debug("%s: i stopped at %d, data %p\n", __func__, i, g);
+	g->self_suspended = 0;
+	g->timer.expires = jiffies + 4*HZ;
+	add_timer(&g->timer);
+#endif
+}
+
+/**
+ * gpmi_set_timings - set GPMI timings
+ * @pdev: pointer to GPMI platform device
+ * @tm: pointer to structure &gpmi_nand_timing with new timings
+ *
+ * During initialization, GPMI uses safe sub-optimal timings, which
+ * can be changed after reading boot control blocks
+ */
+void gpmi_set_timings(struct platform_device *pdev, struct gpmi_nand_timing *tm)
+{
+	struct gpmi_nand_data *g = platform_get_drvdata(pdev);
+	u32 period_ns = 1000000 / clk_get_rate(g->clk) + 1;
+	u32 address_cycles, data_setup_cycles;
+	u32 data_hold_cycles, data_sample_cycles;
+	u32 busy_timeout;
+	u32 t0;
+
+	if (g->self_suspended)
+		gpmi_self_wakeup(g);
+	g->use_count++;
+
+	g->timing = *tm;
+
+	address_cycles = gpmi_cycles_ceil(tm->address_setup, period_ns);
+	data_setup_cycles = gpmi_cycles_ceil(tm->data_setup, period_ns);
+	data_hold_cycles = gpmi_cycles_ceil(tm->data_hold, period_ns);
+	data_sample_cycles = gpmi_cycles_ceil(tm->dsample_time + period_ns / 4,
+			period_ns / 2);
+	busy_timeout = gpmi_cycles_ceil(10000000 / 4096, period_ns);
+
+	dev_dbg(&pdev->dev,
+		"%s: ADDR %u, DSETUP %u, DH %u, DSAMPLE %u, BTO %u\n",
+		__func__,
+		address_cycles, data_setup_cycles, data_hold_cycles,
+		data_sample_cycles, busy_timeout);
+
+	t0 = BF_GPMI_TIMING0_ADDRESS_SETUP(address_cycles) |
+	    BF_GPMI_TIMING0_DATA_SETUP(data_setup_cycles) |
+	    BF_GPMI_TIMING0_DATA_HOLD(data_hold_cycles);
+	HW_GPMI_TIMING0_WR(t0);
+
+	HW_GPMI_TIMING1_WR(BF_GPMI_TIMING1_DEVICE_BUSY_TIMEOUT(busy_timeout));
+
+#ifdef CONFIG_ARCH_STMP378X
+	HW_GPMI_CTRL1_CLR(BM_GPMI_CTRL1_RDN_DELAY);
+	HW_GPMI_CTRL1_SET(BF_GPMI_CTRL1_RDN_DELAY(data_sample_cycles));
+#else
+	HW_GPMI_CTRL1_CLR(BM_GPMI_CTRL1_DSAMPLE_TIME);
+	HW_GPMI_CTRL1_SET(BF_GPMI_CTRL1_DSAMPLE_TIME(data_sample_cycles));
+#endif
+
+	g->use_count--;
+}
+
+static inline u32 bch_mode(void)
+{
+	u32 c1 = 0;
+
+#ifdef CONFIG_MTD_NAND_GPMI_BCH
+	if (bch)
+		c1 |= BM_GPMI_CTRL1_BCH_MODE;
+#endif
+	return c1;
+}
+/**
+ * gpmi_nand_init_hw - initialize the hardware
+ * @pdev: pointer to platform device
+ *
+ * Initialize GPMI hardware and set default (safe) timings for NAND access.
+ * Returns error code or 0 on success
+ */
+static int gpmi_nand_init_hw(struct platform_device *pdev, int request_pins)
+{
+	struct gpmi_nand_data *g = platform_get_drvdata(pdev);
+	char *devname = pdev->dev.bus_id;
+	int err = 0;
+
+	g->clk = clk_get(NULL, "gpmi");
+	if (IS_ERR(g->clk)) {
+		err = PTR_ERR(g->clk);
+		dev_err(&pdev->dev, "cannot set failsafe clockrate\n");
+		goto out;
+	}
+	clk_enable(g->clk);
+	if (clk <= 0)
+		clk = 24000; /* safe setting, some chips do not work on
+				    speeds >= 24kHz */
+	clk_set_rate(g->clk, clk);
+
+	clk = clk_get_rate(g->clk);
+
+	if (request_pins)
+		gpmi_pinmux_request(devname);
+
+	stmp3xxx_reset_block(HW_GPMI_CTRL0_OFFSET + REGS_GPMI_BASE, 1);
+
+	/* this CLEARS reset, despite of its name */
+	HW_GPMI_CTRL1_SET(BM_GPMI_CTRL1_DEV_RESET);
+
+	/* IRQ polarity */
+	HW_GPMI_CTRL1_SET(BM_GPMI_CTRL1_ATA_IRQRDY_POLARITY);
+
+	/* ...and ECC module */
+	HW_GPMI_CTRL1_SET(bch_mode());
+
+	/* choose NAND mode (1 means ATA, 0 - NAND */
+	HW_GPMI_CTRL1_CLR(BM_GPMI_CTRL1_GPMI_MODE);
+
+out:
+	return err;
+}
+
+/**
+ * gpmi_nand_release_hw - free the hardware
+ * @pdev: pointer to platform device
+ *
+ * In opposite to gpmi_nand_init_hw, release all acquired resources
+ */
+static void gpmi_nand_release_hw(struct platform_device *pdev)
+{
+	struct gpmi_nand_data *g = platform_get_drvdata(pdev);
+
+	HW_GPMI_CTRL0_SET(BM_GPMI_CTRL0_SFTRST);
+
+	clk_disable(g->clk);
+	clk_put(g->clk);
+	gpmi_pinmux_free(pdev->dev.bus_id);
+}
+
+static int gpmi_dma_is_error(struct gpmi_nand_data *g)
+{
+	/* u32 n = HW_APBH_CHn_NXTCMDAR_RD(g->dma_ch); */
+
+	/* CURrent DMA command */
+	u32 c = HW_APBH_CHn_NXTCMDAR_RD(g->cchip->dma_ch);
+
+	if (c == g->cchip->error.handle) {
+		pr_debug("%s: dma chain has reached error terminator\n",
+				__func__);
+		return -EIO;
+	}
+	return 0;
+}
+
+/**
+ * gpmi_dma_exchange - run DMA to exchange with NAND chip
+ *
+ * @g: structure associated with NAND chip
+ *
+ * Run DMA and wait for completion
+ */
+static int gpmi_dma_exchange(struct gpmi_nand_data *g,
+		struct stmp3xxx_dma_descriptor *d)
+{
+	struct platform_device *pdev = g->dev;
+	unsigned long timeout;
+	int err;
+
+	if (g->self_suspended)
+		gpmi_self_wakeup(g);
+	g->use_count++;
+
+	if (!g->regulator) {
+		g->regulator = regulator_get(&pdev->dev, "mmc_ssp-2");
+		if (g->regulator && !IS_ERR(g->regulator))
+			regulator_set_mode(
+				g->regulator,
+				REGULATOR_MODE_NORMAL);
+		else
+			g->regulator = NULL;
+	}
+
+	if (g->regulator)
+		regulator_set_current_limit(g->regulator, g->reg_uA, g->reg_uA);
+
+	init_completion(&g->done);
+	stmp3xxx_dma_enable_interrupt(g->cchip->dma_ch);
+	stmp3xxx_dma_go(g->cchip->dma_ch, d ? d : g->cchip->d, 1);
+
+	timeout = wait_for_completion_timeout(&g->done, msecs_to_jiffies(1000));
+	err = (timeout <= 0) ? -ETIMEDOUT : gpmi_dma_is_error(g);
+
+	if (err)
+		printk(KERN_ERR"%s: error %d, CS = %d, channel %d\n",
+			__func__, err, g->cchip->cs, g->cchip->dma_ch);
+
+	stmp3xxx_dma_reset_channel(g->cchip->dma_ch);
+	stmp3xxx_dma_clear_interrupt(g->cchip->dma_ch);
+
+	if (g->regulator)
+		regulator_set_current_limit(g->regulator, 0, 0);
+
+	mod_timer(&g->timer, jiffies + 4*HZ);
+	g->use_count--;
+
+	return err;
+}
+
+/**
+ * gpmi_ecc_read_page - replacement for nand_read_page
+ *
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @buf:	buffer to store read data
+ */
+static int gpmi_ecc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
+				   uint8_t *buf)
+{
+	struct gpmi_nand_data *g = chip->priv;
+	struct mtd_ecc_stats stats;
+	dma_addr_t bufphys, oobphys;
+	int err;
+
+	bufphys = oobphys = ~0;
+
+	if (map_buffers && virt_addr_valid(buf))
+		bufphys = dma_map_single(&g->dev->dev, buf,
+				mtd->writesize, DMA_FROM_DEVICE);
+	if (dma_mapping_error(&g->dev->dev, bufphys))
+		bufphys = g->data_buffer_handle;
+
+	if (map_buffers)
+		oobphys = dma_map_single(&g->dev->dev, chip->oob_poi,
+				mtd->oobsize, DMA_FROM_DEVICE);
+	if (dma_mapping_error(&g->dev->dev, oobphys))
+		oobphys = g->oob_buffer_handle;
+
+	/* ECC read */
+	(void)g->hc->read(g->hc, g->selected_chip, g->cchip->d,
+			g->cchip->error.handle,
+			bufphys, oobphys);
+
+	err = gpmi_dma_exchange(g, NULL);
+
+	g->hc->stat(g->hc, g->selected_chip, &stats);
+
+	if (stats.failed || stats.corrected) {
+
+		pr_debug("%s: ECC failed=%d, corrected=%d\n",
+				__func__, stats.failed, stats.corrected);
+
+		g->mtd.ecc_stats.failed += stats.failed;
+		g->mtd.ecc_stats.corrected += stats.corrected;
+	}
+
+
+	if (!dma_mapping_error(&g->dev->dev, oobphys)) {
+		if (oobphys != g->oob_buffer_handle)
+			dma_unmap_single(&g->dev->dev, oobphys,
+					mtd->oobsize, DMA_FROM_DEVICE);
+		else {
+			memcpy(chip->oob_poi, g->oob_buffer, mtd->oobsize);
+			copies++;
+		}
+	}
+
+	if (!dma_mapping_error(&g->dev->dev, bufphys)) {
+		if (bufphys != g->data_buffer_handle)
+			dma_unmap_single(&g->dev->dev, bufphys,
+					mtd->writesize, DMA_FROM_DEVICE);
+		else {
+			memcpy(buf, g->data_buffer, mtd->writesize);
+			copies++;
+		}
+	}
+
+	/* always fill the (possible ECC bytes with FF) */
+	memset(chip->oob_poi + g->oob_free, 0xff, mtd->oobsize - g->oob_free);
+
+	return err;
+}
+
+static inline int is_ff(const u8 *buffer, size_t size)
+{
+	while (size--) {
+		if (*buffer++ != 0xff)
+			return 0;
+	}
+	return 1;
+}
+
+/**
+ * gpmi_ecc_write_page - replacement for nand_write_page
+ *
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @buf:	data buffer
+ */
+static void gpmi_ecc_write_page(struct mtd_info *mtd,
+			    struct nand_chip *chip, const uint8_t *buf)
+{
+	struct gpmi_nand_data *g = chip->priv;
+	dma_addr_t bufphys, oobphys;
+	int err;
+
+	/* if we can't map it, copy it */
+	bufphys = oobphys = ~0;
+
+	if (map_buffers && virt_addr_valid(buf))
+		bufphys = dma_map_single(&g->dev->dev,
+				(void *)buf, mtd->writesize, DMA_TO_DEVICE);
+	if (dma_mapping_error(&g->dev->dev, bufphys)) {
+		bufphys = g->data_buffer_handle;
+		memcpy(g->data_buffer, buf, mtd->writesize);
+		copies++;
+	}
+
+	/* if OOB is all FF, leave it as such */
+	if (!is_ff(chip->oob_poi, mtd->oobsize)) {
+		if (map_buffers)
+			oobphys = dma_map_single(&g->dev->dev, chip->oob_poi,
+				mtd->oobsize, DMA_TO_DEVICE);
+		if (dma_mapping_error(&g->dev->dev, oobphys)) {
+			oobphys = g->oob_buffer_handle;
+			memcpy(g->oob_buffer, chip->oob_poi, mtd->oobsize);
+			copies++;
+		}
+	} else
+		ff_writes++;
+
+	/* call ECC */
+	g->hc->write(g->hc, g->selected_chip, g->cchip->d,
+			g->cchip->error.handle,
+			bufphys, oobphys);
+
+	err = gpmi_dma_exchange(g, NULL);
+	if (err < 0)
+		printk(KERN_ERR"%s: dma error\n", __func__);
+
+	if (!dma_mapping_error(&g->dev->dev, oobphys)) {
+		if (oobphys != g->oob_buffer_handle)
+			dma_unmap_single(&g->dev->dev, oobphys, mtd->oobsize,
+				DMA_TO_DEVICE);
+	}
+
+	if (bufphys != g->data_buffer_handle)
+		dma_unmap_single(&g->dev->dev, bufphys, mtd->writesize,
+			DMA_TO_DEVICE);
+}
+
+/**
+ * gpmi_write_buf - replacement for nand_write_buf
+ *
+ * @mtd: MTD device
+ * @buf: data buffer
+ * @len: length of the data buffer
+ */
+static void gpmi_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct gpmi_nand_data *g = chip->priv;
+	struct stmp3xxx_dma_descriptor *chain = g->cchip->d;
+	dma_addr_t phys;
+	int err;
+
+	BUG_ON(len > mtd->writesize);
+
+	phys = ~0;
+
+	if (map_buffers && virt_addr_valid(buf))
+		phys = dma_map_single(&g->dev->dev,
+				(void *)buf, len, DMA_TO_DEVICE);
+	if (dma_mapping_error(&g->dev->dev, phys)) {
+		phys = g->write_buffer_handle;
+		memcpy(g->write_buffer, buf, len);
+		copies++;
+	}
+
+	/* write plain data */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_XFER_COUNT(len) 	|
+		BF_APBH_CHn_CMD_CMDWORDS(4)			|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD			|
+		BM_APBH_CHn_CMD_NANDLOCK			|
+		BM_APBH_CHn_CMD_IRQONCMPLT			|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__DMA_READ);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(BV_GPMI_CTRL0_COMMAND_MODE__WRITE) |
+		BM_GPMI_CTRL0_WORD_LENGTH			|
+		BM_GPMI_CTRL0_LOCK_CS				|
+		BF_GPMI_CTRL0_CS(g->selected_chip)		|
+		BF_GPMI_CTRL0_ADDRESS(BV_GPMI_CTRL0_ADDRESS__NAND_DATA)	|
+		BF_GPMI_CTRL0_XFER_COUNT(len);
+
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] = 0;
+	chain->command->pio_words[3] = 0;
+	chain->command->buf_ptr = phys;
+
+	err = gpmi_dma_exchange(g, NULL);
+	if (err)
+		printk(KERN_ERR"%s: dma error\n", __func__);
+
+	if (phys != g->write_buffer_handle)
+		dma_unmap_single(&g->dev->dev, phys, len, DMA_TO_DEVICE);
+
+	if (debug >= 2)
+		print_hex_dump_bytes("WBUF ", DUMP_PREFIX_OFFSET, buf, len);
+}
+
+/**
+ * gpmi_read_buf - replacement for nand_read_buf
+ *
+ * @mtd: MTD device
+ * @buf: pointer to the buffer
+ * @len: size of the buffer
+ */
+static void gpmi_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct gpmi_nand_data *g = chip->priv;
+	struct stmp3xxx_dma_descriptor *chain;
+	dma_addr_t phys;
+	int err;
+
+	phys = ~0;
+
+	if (map_buffers && virt_addr_valid(buf))
+		phys = dma_map_single(&g->dev->dev,
+				buf, len, DMA_FROM_DEVICE);
+	if (dma_mapping_error(&g->dev->dev, phys))
+		phys = g->read_buffer_handle;
+
+	chain = g->cchip->d;
+
+	/* read data */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_XFER_COUNT(len)			|
+		BF_APBH_CHn_CMD_CMDWORDS(1)			|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD			|
+		BM_APBH_CHn_CMD_CHAIN				|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__DMA_WRITE);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(BV_GPMI_CTRL0_COMMAND_MODE__READ) |
+		BM_GPMI_CTRL0_WORD_LENGTH			|
+		BM_GPMI_CTRL0_LOCK_CS				|
+		BF_GPMI_CTRL0_CS(g->selected_chip)			|
+		BF_GPMI_CTRL0_ADDRESS(BV_GPMI_CTRL0_ADDRESS__NAND_DATA) |
+		BF_GPMI_CTRL0_XFER_COUNT(len);
+	chain->command->buf_ptr = phys;
+	chain++;
+
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(4) 	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_NANDWAIT4READY	|
+		BM_APBH_CHn_CMD_NANDLOCK	|
+		BM_APBH_CHn_CMD_IRQONCMPLT	|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(
+			BV_GPMI_CTRL0_COMMAND_MODE__WAIT_FOR_READY) |
+		BM_GPMI_CTRL0_WORD_LENGTH		  |
+		BF_GPMI_CTRL0_ADDRESS(BV_GPMI_CTRL0_ADDRESS__NAND_DATA) |
+		BM_GPMI_CTRL0_LOCK_CS				|
+		BF_GPMI_CTRL0_CS(g->selected_chip);
+	chain->command->pio_words[1] =
+	chain->command->pio_words[2] =
+	chain->command->pio_words[3] = 0;
+	chain->command->buf_ptr = 0;
+
+	err = gpmi_dma_exchange(g, NULL);
+	if (err)
+		printk(KERN_ERR"%s: dma error\n", __func__);
+
+	if (phys != g->read_buffer_handle)
+		dma_unmap_single(&g->dev->dev, phys, len, DMA_FROM_DEVICE);
+	else {
+		memcpy(buf, g->read_buffer, len);
+		copies++;
+	}
+
+	if (debug >= 2)
+		print_hex_dump_bytes("RBUF ", DUMP_PREFIX_OFFSET, buf, len);
+}
+
+/**
+ * gpmi_read_byte - replacement for nand_read_byte
+ * @mtd: MTD device
+ *
+ * Uses gpmi_read_buf to read 1 byte from device
+ */
+static u8 gpmi_read_byte(struct mtd_info *mtd)
+{
+	u8 b;
+
+	gpmi_read_buf(mtd, (uint8_t *)&b, 1);
+	return b;
+}
+
+/**
+ * gpmi_read_word - replacement for nand_read_word
+ * @mtd: MTD device
+ *
+ * Uses gpmi_read_buf to read 2 bytes from device
+ */
+static u16 gpmi_read_word(struct mtd_info *mtd)
+{
+	u16 w;
+
+	gpmi_read_buf(mtd, (uint8_t *)&w, sizeof(u16));
+	return w;
+}
+
+/**
+ * gpmi_erase - erase a block and update BBT table
+ *
+ * @mtd: MTD device
+ * @instr: erase instruction
+ */
+int gpmi_erase(struct mtd_info *mtd, struct erase_info *instr)
+{
+	int rc;
+	struct nand_chip *chip = mtd->priv;
+	struct gpmi_nand_data *g = chip->priv;
+	struct gpmi_nand_data *data = platform_get_drvdata(g->dev);
+
+	if (g->self_suspended)
+		gpmi_self_wakeup(data);
+	g->use_count++;
+
+	rc = nand_erase_nand(mtd, instr, 0);
+
+	if (rc == -EIO)		/* block cannot be erased */
+		gpmi_block_mark_as(chip,
+				   (instr->addr >> chip->bbt_erase_shift),
+				   0x01);
+
+	mod_timer(&g->timer, jiffies + 4*HZ);
+	g->use_count--;
+	return rc;
+}
+
+/**
+ * gpmi_dev_ready - poll the RDY pin
+ *
+ * @mtd: MTD device
+ */
+static int gpmi_dev_ready(struct mtd_info *mtd)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct gpmi_nand_data *g = chip->priv;
+	struct stmp3xxx_dma_descriptor *chain = g->cchip->d;
+	int ret;
+
+	/* wait for ready */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(4) 	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_NANDWAIT4READY	|
+		BM_APBH_CHn_CMD_NANDLOCK	|
+		BM_APBH_CHn_CMD_IRQONCMPLT	|
+		BF_APBH_CHn_CMD_COMMAND(
+				BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(
+			BV_GPMI_CTRL0_COMMAND_MODE__WAIT_FOR_READY) |
+		BM_GPMI_CTRL0_WORD_LENGTH	|
+		BF_GPMI_CTRL0_ADDRESS(
+				BV_GPMI_CTRL0_ADDRESS__NAND_DATA)   |
+		BF_GPMI_CTRL0_CS(g->selected_chip);
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] = 0;
+	chain->command->pio_words[3] = 0;
+	chain->command->buf_ptr = 0;
+	chain++;
+
+	ret = gpmi_dma_exchange(g, NULL);
+	if (ret != 0)
+		printk(KERN_ERR "gpmi: gpmi_dma_exchange() timeout!\n");
+	return ret == 0;
+}
+
+/**
+ * gpmi_hwcontrol - set command/address byte to the device
+ *
+ * @mtd: MTD device
+ * @cmd: command byte
+ * @ctrl: control flags
+ */
+static void gpmi_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct gpmi_nand_data *g = chip->priv;
+	struct stmp3xxx_dma_descriptor *chain = g->cchip->d;
+	int ret;
+
+	if ((ctrl & (NAND_ALE | NAND_CLE))) {
+		if (cmd != NAND_CMD_NONE)
+			g->cmd_buffer[g->cmd_buffer_sz++] = cmd;
+		return;
+	}
+
+	if (g->cmd_buffer_sz == 0)
+		return;
+
+	/* output command */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_XFER_COUNT(g->cmd_buffer_sz) 	|
+		BF_APBH_CHn_CMD_CMDWORDS(3) 			|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD			|
+		BM_APBH_CHn_CMD_NANDLOCK			|
+		BM_APBH_CHn_CMD_CHAIN				|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__DMA_READ);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(BV_GPMI_CTRL0_COMMAND_MODE__WRITE) |
+		BM_GPMI_CTRL0_WORD_LENGTH			|
+		BM_GPMI_CTRL0_LOCK_CS				|
+		BF_GPMI_CTRL0_CS(g->selected_chip)			|
+		BF_GPMI_CTRL0_ADDRESS(BV_GPMI_CTRL0_ADDRESS__NAND_CLE)	|
+		BF_GPMI_CTRL0_XFER_COUNT(g->cmd_buffer_sz);
+	if (g->cmd_buffer_sz > 0)
+		chain->command->pio_words[0] |= BM_GPMI_CTRL0_ADDRESS_INCREMENT;
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] = 0;
+	chain->command->buf_ptr = g->cmd_buffer_handle;
+	chain++;
+
+	/* emit IRQ */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(0)	|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER) |
+		BM_APBH_CHn_CMD_WAIT4ENDCMD;
+	chain++;
+
+	/* last in chain get the irq bit set */
+	chain[-1].command->cmd |= BM_APBH_CHn_CMD_IRQONCMPLT;
+
+	if (debug >= 3)
+		print_hex_dump(KERN_INFO, "CMD ", DUMP_PREFIX_OFFSET, 16, 1,
+			g->cmd_buffer, g->cmd_buffer_sz, 1);
+
+	ret = gpmi_dma_exchange(g, NULL);
+	if (ret != 0) {
+		printk(KERN_ERR"%s: chip %d, dma error %d on the command:\n",
+			__func__, g->selected_chip, ret);
+		print_hex_dump(KERN_INFO, "CMD ", DUMP_PREFIX_OFFSET, 16, 1,
+			g->cmd_buffer, g->cmd_buffer_sz, 1);
+	}
+
+	gpmi_dev_ready(mtd);
+
+	g->cmd_buffer_sz = 0;
+}
+
+/**
+ * gpmi_alloc_buffers - allocate DMA buffers for one chip
+ *
+ * @pdev:	GPMI platform device
+ * @g:		pointer to structure associated with NAND chip
+ *
+ * Allocate buffer using dma_alloc_coherent
+ */
+static int gpmi_alloc_buffers(struct platform_device *pdev,
+			      struct gpmi_nand_data *g)
+{
+	g->cmd_buffer = dma_alloc_coherent(&pdev->dev,
+					   g->cmd_buffer_size,
+					   &g->cmd_buffer_handle, GFP_DMA);
+	if (!g->cmd_buffer)
+		goto out1;
+
+	g->write_buffer = dma_alloc_coherent(&pdev->dev,
+					     g->write_buffer_size * 2,
+					     &g->write_buffer_handle, GFP_DMA);
+	if (!g->write_buffer)
+		goto out2;
+
+	g->read_buffer = g->write_buffer + g->write_buffer_size;
+	g->read_buffer_handle = g->write_buffer_handle + g->write_buffer_size;
+
+	g->data_buffer = dma_alloc_coherent(&pdev->dev,
+					    g->data_buffer_size,
+					    &g->data_buffer_handle, GFP_DMA);
+	if (!g->data_buffer)
+		goto out3;
+
+	g->oob_buffer = dma_alloc_coherent(&pdev->dev,
+					   g->oob_buffer_size,
+					   &g->oob_buffer_handle, GFP_DMA);
+	if (!g->oob_buffer)
+		goto out4;
+
+	g->verify_buffer = kzalloc(2 * (g->data_buffer_size +
+				g->oob_buffer_size), GFP_KERNEL);
+	if (!g->verify_buffer)
+		goto out5;
+
+	return 0;
+
+out5:
+	dma_free_coherent(&pdev->dev, g->oob_buffer_size,
+			  g->oob_buffer, g->oob_buffer_handle);
+out4:
+	dma_free_coherent(&pdev->dev, g->data_buffer_size,
+			  g->data_buffer, g->data_buffer_handle);
+out3:
+	dma_free_coherent(&pdev->dev, g->write_buffer_size * 2,
+			  g->write_buffer, g->write_buffer_handle);
+out2:
+	dma_free_coherent(&pdev->dev, g->cmd_buffer_size,
+			  g->cmd_buffer, g->cmd_buffer_handle);
+out1:
+	return -ENOMEM;
+}
+
+/**
+ * gpmi_free_buffers - free buffers allocated by gpmi_alloc_buffers
+ *
+ * @pdev:	platform device
+ * @g:		pointer to structure associated with NAND chip
+ *
+ * Deallocate buffers on exit
+ */
+static void gpmi_free_buffers(struct platform_device *pdev,
+			      struct gpmi_nand_data *g)
+{
+	kfree(g->verify_buffer);
+	dma_free_coherent(&pdev->dev, g->oob_buffer_size,
+			  g->oob_buffer, g->oob_buffer_handle);
+	dma_free_coherent(&pdev->dev, g->write_buffer_size * 2,
+			  g->write_buffer, g->write_buffer_handle);
+	dma_free_coherent(&pdev->dev, g->cmd_buffer_size,
+			  g->cmd_buffer, g->cmd_buffer_handle);
+	dma_free_coherent(&pdev->dev, g->data_buffer_size,
+			  g->data_buffer, g->data_buffer_handle);
+}
+
+/* only used in SW-ECC or NO-ECC cases */
+static int gpmi_verify_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct gpmi_nand_data *g = chip->priv;
+
+	chip->read_buf(mtd, g->verify_buffer, len);
+
+	if (memcmp(buf, g->verify_buffer, len))
+		return -EFAULT;
+
+	return 0;
+}
+
+/**
+ * gpmi_ecc_read_oob - replacement for nand_read_oob
+ *
+ * @mtd:	MTD device
+ * @chip:	mtd->priv
+ * @page:	page address
+ * @sndcmd:	flag indicates that command should be sent
+ */
+int gpmi_ecc_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
+		  int page, int sndcmd)
+{
+	struct gpmi_nand_data *g = chip->priv;
+	loff_t oob_offset;
+	struct mtd_ecc_stats stats;
+	dma_addr_t oobphys;
+	int ecc;
+	int ret;
+
+	ecc = g->raw_oob_mode == 0 && raw_mode == 0;
+
+	if (sndcmd) {
+		oob_offset = mtd->writesize;
+		if (likely(ecc))
+			oob_offset += chip->ecc.bytes * chip->ecc.steps;
+		chip->cmdfunc(mtd, NAND_CMD_READ0, oob_offset, page);
+		sndcmd = 0;
+	}
+
+	if (unlikely(!ecc)) {
+		chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
+		return 1;
+	}
+
+	oobphys = ~0;
+
+	if (map_buffers)
+		oobphys = dma_map_single(&g->dev->dev, chip->oob_poi,
+				mtd->oobsize, DMA_FROM_DEVICE);
+	if (dma_mapping_error(&g->dev->dev, oobphys))
+		oobphys = g->oob_buffer_handle;
+
+	/* ECC read */
+	(void)g->hc->read(g->hc, g->selected_chip, g->cchip->d,
+			g->cchip->error.handle, ~0, oobphys);
+
+	ret = gpmi_dma_exchange(g, NULL);
+
+	g->hc->stat(g->hc, g->selected_chip, &stats);
+
+	if (stats.failed || stats.corrected) {
+
+		printk(KERN_DEBUG "%s: ECC failed=%d, corrected=%d\n",
+			__func__, stats.failed, stats.corrected);
+
+		g->mtd.ecc_stats.failed += stats.failed;
+		g->mtd.ecc_stats.corrected += stats.corrected;
+	}
+
+	if (oobphys != g->oob_buffer_handle)
+		dma_unmap_single(&g->dev->dev, oobphys, mtd->oobsize,
+			DMA_FROM_DEVICE);
+	else {
+		memcpy(chip->oob_poi, g->oob_buffer, mtd->oobsize);
+		copies++;
+	}
+
+	/* fill rest with ff */
+	memset(chip->oob_poi + g->oob_free, 0xff, mtd->oobsize - g->oob_free);
+
+	return ret ? ret : 1;
+}
+
+/**
+ * gpmi_ecc_write_oob - replacement for nand_write_oob
+ *
+ * @mtd:	MTD device
+ * @chip:	mtd->priv
+ * @page:	page address
+ */
+static int gpmi_ecc_write_oob(struct mtd_info *mtd, struct nand_chip *chip,
+			  int page)
+{
+	int status = 0;
+	struct gpmi_nand_data *g = chip->priv;
+	loff_t oob_offset;
+	dma_addr_t oobphys;
+	int ecc;
+	int err = 0;
+
+	/* if OOB is all FF, leave it as such */
+	if (is_ff(chip->oob_poi, mtd->oobsize)) {
+		ff_writes++;
+
+		pr_debug("%s: Skipping an empty page 0x%x (0x%x)\n",
+				__func__,
+				page, page << chip->page_shift);
+		return 0;
+	}
+
+	ecc = g->raw_oob_mode == 0 && raw_mode == 0;
+
+	/* Send command to start input data	*/
+	oob_offset = mtd->writesize;
+	if (likely(ecc)) {
+		oob_offset += chip->ecc.bytes * chip->ecc.steps;
+		memset(chip->oob_poi + g->oob_free, 0xff,
+			mtd->oobsize - g->oob_free);
+	}
+	chip->cmdfunc(mtd, NAND_CMD_SEQIN, oob_offset, page);
+
+	/* call ECC */
+	if (likely(ecc)) {
+
+		oobphys = ~0;
+
+		if (map_buffers)
+			oobphys = dma_map_single(&g->dev->dev, chip->oob_poi,
+					mtd->oobsize, DMA_TO_DEVICE);
+		if (dma_mapping_error(&g->dev->dev, oobphys)) {
+			oobphys = g->oob_buffer_handle;
+			memcpy(g->oob_buffer, chip->oob_poi, mtd->oobsize);
+			copies++;
+		}
+
+		g->hc->write(g->hc, g->selected_chip, g->cchip->d,
+				g->cchip->error.handle, ~0, oobphys);
+
+		err = gpmi_dma_exchange(g, NULL);
+	} else
+		chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	/* Send command to program the OOB data */
+	chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
+
+	/* ..and wait for result		*/
+	status = chip->waitfunc(mtd, chip);
+
+	if (likely(ecc)) {
+		if (oobphys != g->oob_buffer_handle)
+			dma_unmap_single(&g->dev->dev, oobphys, mtd->oobsize,
+				DMA_TO_DEVICE);
+	}
+
+	if (status & NAND_STATUS_FAIL) {
+		pr_debug("%s: NAND_STATUS_FAIL\n", __func__);
+		return -EIO;
+	}
+
+	return err;
+}
+
+/**
+ * gpmi_irq - IRQ handler
+ *
+ * @irq:	irq no
+ * @context:	IRQ context, pointer to gpmi_nand_data
+ */
+static irqreturn_t gpmi_irq(int irq, void *context)
+{
+	struct gpmi_nand_data *g = context;
+
+	if (stmp3xxx_dma_is_interrupt(g->cchip->dma_ch)) {
+		stmp3xxx_dma_clear_interrupt(g->cchip->dma_ch);
+		complete(&g->done);
+	}
+	HW_GPMI_CTRL1_CLR(BM_GPMI_CTRL1_DEV_IRQ | BM_GPMI_CTRL1_TIMEOUT_IRQ);
+	return IRQ_HANDLED;
+}
+
+static void gpmi_select_chip(struct mtd_info *mtd, int chipnr)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct gpmi_nand_data *g = chip->priv;
+
+	if (chipnr == g->selected_chip)
+		return;
+
+	g->selected_chip = chipnr;
+	g->cchip = NULL;
+
+	if (chipnr == -1)
+		return;
+
+	g->cchip = g->chips + chipnr;
+}
+
+static void gpmi_command(struct mtd_info *mtd, unsigned int command,
+			 int column, int page_addr)
+{
+	register struct nand_chip *chip = mtd->priv;
+	struct gpmi_nand_data *g = chip->priv;
+
+	g->saved_command(mtd, command, column, page_addr);
+}
+
+static int gpmi_read_oob(struct mtd_info *mtd, loff_t from,
+		 struct mtd_oob_ops *ops)
+{
+	register struct nand_chip *chip = mtd->priv;
+	struct gpmi_nand_data *g = chip->priv;
+	int ret;
+
+	g->raw_oob_mode = ops->mode == MTD_OOB_RAW;
+	ret = g->saved_read_oob(mtd, from, ops);
+	g->raw_oob_mode = 0;
+	return ret;
+}
+
+static int gpmi_write_oob(struct mtd_info *mtd, loff_t to,
+			  struct mtd_oob_ops *ops)
+{
+	register struct nand_chip *chip = mtd->priv;
+	struct gpmi_nand_data *g = chip->priv;
+	int ret;
+
+	g->raw_oob_mode = ops->mode == MTD_OOB_RAW;
+	ret = g->saved_write_oob(mtd, to, ops);
+	g->raw_oob_mode = 0;
+	return ret;
+}
+
+/**
+ * perform the needed steps between nand_scan_ident and nand_scan_tail
+ */
+static int gpmi_scan_middle(struct gpmi_nand_data *g)
+{
+	int oobsize = 0;
+
+	/* Limit to 2G size due to Kernel larger 4G space support */
+	if (g->mtd.size == 0) {
+		g->mtd.size = 1 << 31;
+		g->chip.chipsize = g->mtd.size / g->chip.numchips;
+	}
+
+	g->ecc_oob_bytes = 9;
+	switch (g->mtd.writesize) {
+	case 2048:	/* 2K page */
+		g->chip.ecc.layout = &gpmi_oob_64;
+		g->chip.ecc.bytes = 9;
+		g->oob_free = 19;
+		g->hwecc_type_read = GPMI_ECC4_RD;
+		g->hwecc_type_write = GPMI_ECC4_WR;
+		oobsize = 64;
+		break;
+	case 4096:
+		g->chip.ecc.layout = &gpmi_oob_128;
+		g->chip.ecc.bytes = 18;
+		g->oob_free = 65;
+		g->hwecc_type_read = GPMI_ECC8_RD;
+		g->hwecc_type_write = GPMI_ECC8_WR;
+		oobsize = 218;
+		break;
+	default:
+		printk(KERN_ERR "Unsupported write_size %d.",
+			g->mtd.writesize);
+		break;
+	}
+
+	g->mtd.ecclayout = g->chip.ecc.layout;
+	/* sanity check */
+	if (oobsize > NAND_MAX_OOBSIZE ||
+	    g->mtd.writesize > NAND_MAX_PAGESIZE) {
+		printk(KERN_ERR "Internal error. Either page size "
+				"(%d) > max (%d) "
+				"or oob size (%d) > max(%d). Sorry.\n",
+					oobsize, NAND_MAX_OOBSIZE,
+					g->mtd.writesize, NAND_MAX_PAGESIZE);
+		return -ERANGE;
+	}
+
+	g->saved_command = g->chip.cmdfunc;
+	g->chip.cmdfunc = gpmi_command;
+
+	if (oobsize > 0) {
+		g->mtd.oobsize = oobsize;
+		/* otherwise error; oobsize should be set
+		   in valid cases */
+		g->hc = gpmi_hwecc_chip_find("ecc8");
+		g->hc->setup(g->hc, 0, g->mtd.writesize, g->mtd.oobsize);
+		return 0;
+	}
+
+	return -ENXIO;
+}
+
+/**
+ * gpmi_write_page - [REPLACEABLE] write one page
+ * @mtd:	MTD device structure
+ * @chip:	NAND chip descriptor
+ * @buf:	the data to write
+ * @page:	page number to write
+ * @cached:	cached programming
+ * @raw:	use _raw version of write_page
+ */
+static int gpmi_write_page(struct mtd_info *mtd, struct nand_chip *chip,
+			   const uint8_t *buf, int page, int cached, int raw)
+{
+	struct gpmi_nand_data *g = chip->priv;
+	int status, empty_data, empty_oob;
+	int oobsz;
+#if defined(CONFIG_MTD_NAND_VERIFY_WRITE)
+	void *vbuf, *obuf;
+#if 0
+	void *voob, *ooob;
+#endif
+#endif
+
+	oobsz = likely(g->raw_oob_mode == 0 && raw_mode == 0) ?
+		g->oob_free : mtd->oobsize;
+
+	empty_data = is_ff(buf, mtd->writesize);
+	empty_oob = is_ff(buf, oobsz);
+
+	if (empty_data && empty_oob) {
+		ff_writes++;
+
+		pr_debug("%s: Skipping an empty page 0x%x (0x%x)\n",
+				__func__,
+				page, page << chip->page_shift);
+		return 0;
+	}
+
+	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
+
+	if (likely(raw == 0))
+		chip->ecc.write_page(mtd, chip, buf);
+	else
+		chip->ecc.write_page_raw(mtd, chip, buf);
+
+	/*
+	 * Cached progamming disabled for now, Not sure if its worth the
+	 * trouble. The speed gain is not very impressive. (2.3->2.6Mib/s)
+	 */
+	cached = 0;
+
+	if (!cached || !(chip->options & NAND_CACHEPRG)) {
+
+		chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
+
+		status = chip->waitfunc(mtd, chip);
+
+		/*
+		 * See if operation failed and additional status checks are
+		 * available
+		 */
+		if ((status & NAND_STATUS_FAIL) && (chip->errstat))
+			status = chip->errstat(mtd, chip, FL_WRITING, status,
+					       page);
+
+		if (status & NAND_STATUS_FAIL) {
+			pr_debug("%s: NAND_STATUS_FAIL\n", __func__);
+			return -EIO;
+		}
+	} else {
+		chip->cmdfunc(mtd, NAND_CMD_CACHEDPROG, -1, -1);
+		status = chip->waitfunc(mtd, chip);
+	}
+
+#if defined(CONFIG_MTD_NAND_VERIFY_WRITE)
+	if (empty_data)
+		return 0;
+
+	obuf = g->verify_buffer;
+#if 1	/* make vbuf aligned by mtd->writesize */
+	vbuf = obuf + mtd->writesize;
+#else
+	ooob = obuf + mtd->writesize;
+	vbuf = ooob + mtd->oobsize;
+	voob = vbuf + mtd->writesize;
+#endif
+
+	/* keep data around */
+	memcpy(obuf, buf, mtd->writesize);
+#if 0
+	memcpy(ooob, chip->oob_poi, oobsz);
+#endif
+	/* Send command to read back the data */
+	chip->cmdfunc(mtd, NAND_CMD_READ0, 0, page);
+
+	if (likely(raw == 0))
+		chip->ecc.read_page(mtd, chip, vbuf);
+	else
+		chip->ecc.read_page_raw(mtd, chip, vbuf);
+
+#if 0
+	memcpy(voob, chip->oob_poi, oobsz);
+#endif
+
+	if (!empty_data && memcmp(obuf, vbuf, mtd->writesize) != 0)
+		return -EIO;
+#endif
+
+	return 0;
+}
+
+/**
+ * gpmi_read_page_raw - [Intern] read raw page data without ecc
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @buf:	buffer to store read data
+ */
+static int gpmi_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+			      uint8_t *buf)
+{
+	chip->read_buf(mtd, buf, mtd->writesize);
+	chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
+	return 0;
+}
+
+/**
+ * gpmi_write_page_raw - [Intern] raw page write function
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @buf:	data buffer
+ */
+static void gpmi_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+				const uint8_t *buf)
+{
+	chip->write_buf(mtd, buf, mtd->writesize);
+	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+}
+
+static int gpmi_init_chip(struct platform_device *pdev,
+		struct gpmi_nand_data *g, int n, unsigned dma_ch)
+{
+	int err;
+
+	g->chips[n].dma_ch = dma_ch;
+	g->chips[n].cs = n;
+
+	err = stmp3xxx_dma_request(dma_ch, NULL, pdev->dev.bus_id);
+	if (err) {
+		dev_err(&pdev->dev,
+			"can't request DMA channel 0x%x\n", dma_ch);
+		goto out_all;
+	}
+
+	err = stmp3xxx_dma_make_chain(
+		dma_ch,
+		&g->chips[n].chain,
+		g->chips[n].d, ARRAY_SIZE(g->chips[n].d));
+	if (err) {
+		dev_err(&pdev->dev, "can't setup DMA chain\n");
+		goto out_all;
+	}
+
+	err = stmp3xxx_dma_allocate_command(
+		dma_ch,
+		&g->chips[n].error);
+	if (err) {
+		dev_err(&pdev->dev, "can't setup DMA chain\n");
+		goto out_all;
+	}
+
+	g->chips[n].error.command->cmd =
+		BM_APBH_CHn_CMD_IRQONCMPLT |
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+out_all:
+	return err;
+}
+
+static void gpmi_deinit_chip(struct platform_device *pdev,
+		struct gpmi_nand_data *g, int n)
+{
+	int dma_ch;
+
+	if (n < 0) {
+		for (n = 0; n < ARRAY_SIZE(g->chips); n++)
+			gpmi_deinit_chip(pdev, g, n);
+		return;
+	}
+
+	if (g->chips[n].dma_ch <= 0)
+		return;
+
+	dma_ch = g->chips[n].dma_ch;
+
+	stmp3xxx_dma_free_command(dma_ch, &g->chips[n].error);
+	stmp3xxx_dma_free_chain(&g->chips[n].chain);
+	stmp3xxx_dma_release(dma_ch);
+}
+
+static int gpmi_to_concat(struct mtd_partition *part, char **list)
+{
+	while (list && *list) {
+		if (strcmp(part->name, *list) == 0) {
+			pr_debug("Partition '%s' will be concatenated\n",
+				part->name);
+			return true;
+		}
+		list++;
+	}
+	pr_debug("Partition '%s' is left as-is", part->name);
+	return false;
+}
+
+static void gpmi_create_partitions(struct gpmi_nand_data *g,
+		struct gpmi_platform_data *gpd, uint64_t chipsize)
+{
+#ifdef CONFIG_MTD_PARTITIONS
+	int chip, p;
+	char chipname[20];
+
+	if (g->numchips == 1)
+		g->masters[0] = &g->mtd;
+	else {
+		for (chip = 0; chip < g->numchips; chip++) {
+			memset(g->chip_partitions + chip,
+					0, sizeof(g->chip_partitions[chip]));
+			snprintf(chipname, sizeof(chipname),
+					"gpmi-chip-%d", chip);
+			g->chip_partitions[chip].name =
+				kstrdup(chipname, GFP_KERNEL);
+			g->chip_partitions[chip].size = chipsize;
+			g->chip_partitions[chip].offset = chipsize * chip;
+			g->chip_partitions[chip].mask_flags = 0;
+			g->chip_partitions[chip].mtdp = &g->masters[chip];
+		}
+		add_mtd_partitions(&g->mtd, g->chip_partitions, g->numchips);
+	}
+	g->n_concat = 0;
+	memset(g->concat, 0, sizeof(g->concat));
+	for (chip = 0; chip < g->numchips; chip++) {
+		if (add_mtd_chip) {
+			printk(KERN_NOTICE"Adding MTD for the chip %d\n", chip);
+			add_mtd_device(g->masters[chip]);
+		}
+		if (chip >= gpd->items)
+			continue;
+		for (p = 0; p < gpd->parts[chip].nr_partitions; p++) {
+			if (gpmi_to_concat(&gpd->parts[chip].partitions[p],
+					   gpd->concat_parts))
+				gpd->parts[chip].partitions[p].mtdp =
+					&g->concat[g->n_concat++];
+		}
+		add_mtd_partitions(g->masters[chip],
+			gpd->parts[chip].partitions,
+			gpd->parts[chip].nr_partitions);
+	}
+	if (g->n_concat > 0) {
+#ifdef CONFIG_MTD_CONCAT
+		if (g->n_concat == 1)
+#endif
+			for (p = 0; p < g->n_concat; p++)
+				add_mtd_device(g->concat[p]);
+#ifdef CONFIG_MTD_CONCAT
+		if (g->n_concat > 1) {
+			g->concat_mtd = mtd_concat_create(g->concat,
+				g->n_concat, gpd->concat_name);
+			if (g->concat_mtd)
+				add_mtd_device(g->concat_mtd);
+		}
+#endif
+	}
+	g->custom_partitions = true;
+#endif
+}
+
+static void gpmi_delete_partitions(struct gpmi_nand_data *g)
+{
+#ifdef CONFIG_MTD_PARTITIONS
+	int chip, p;
+
+	if (!g->custom_partitions)
+		return;
+#ifdef CONFIG_MTD_CONCAT
+	if (g->concat_mtd)
+		del_mtd_device(g->concat_mtd);
+	if (g->n_concat == 1)
+#endif
+		for (p = 0; p < g->n_concat; p++)
+			del_mtd_device(g->concat[p]);
+
+	for (chip = 0; chip < g->numchips; chip++) {
+		del_mtd_partitions(g->masters[chip]);
+		if (add_mtd_chip)
+			del_mtd_device(g->masters[chip]);
+		kfree(g->chip_partitions[chip].name);
+	}
+#endif
+}
+/**
+ * gpmi_nand_probe - probe for the GPMI device
+ *
+ * Probe for GPMI device and discover NAND chips
+ */
+static int __init gpmi_nand_probe(struct platform_device *pdev)
+{
+	struct gpmi_nand_data *g;
+	struct gpmi_platform_data *gpd;
+	const char *part_type = 0;
+	int err = 0;
+	struct resource *r;
+	int dma;
+	unsigned long long chipsize;
+
+	/* Allocate memory for the device structure (and zero it) */
+	g = kzalloc(sizeof(*g), GFP_KERNEL);
+	if (!g) {
+		dev_err(&pdev->dev, "failed to allocate gpmi_nand_data\n");
+		err = -ENOMEM;
+		goto out1;
+	}
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "failed to get resource\n");
+		err = -ENXIO;
+		goto out2;
+	}
+	g->io_base = ioremap(r->start, r->end - r->start + 1);
+	if (!g->io_base) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		err = -EIO;
+		goto out2;
+	}
+
+	r = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!r) {
+		err = -EIO;
+		dev_err(&pdev->dev, "can't get IRQ resource\n");
+		goto out3;
+	}
+
+	gpd = (struct gpmi_platform_data *)pdev->dev.platform_data;
+	platform_set_drvdata(pdev, g);
+	err = gpmi_nand_init_hw(pdev, 1);
+	if (err)
+		goto out3;
+
+	init_timer(&g->timer);
+	g->timer.data = (unsigned long)g;
+	g->timer.function = gpmi_timer_expiry;
+	g->timer.expires = jiffies + 4*HZ;
+	add_timer(&g->timer);
+	dev_dbg(&pdev->dev, "%s: timer set to %ld\n",
+		__func__, jiffies + 4*HZ);
+
+	g->reg_uA = gpd->io_uA;
+	g->regulator = regulator_get(&pdev->dev, "mmc_ssp-2");
+	if (g->regulator && !IS_ERR(g->regulator)) {
+		regulator_set_mode(
+			g->regulator,
+			REGULATOR_MODE_NORMAL);
+	} else
+		g->regulator = NULL;
+
+	gpmi_set_timings(pdev, &gpmi_safe_timing);
+
+	g->irq = r->start;
+	err = request_irq(g->irq,
+		  gpmi_irq, 0, pdev->dev.bus_id, g);
+	if (err) {
+		dev_err(&pdev->dev, "can't request GPMI IRQ\n");
+		goto out4;
+	}
+
+	r = platform_get_resource(pdev, IORESOURCE_DMA, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "can't get DMA resource\n");
+		goto out5;
+	}
+
+	if (r->end - r->start > GPMI_MAX_CHIPS)
+		dev_info(&pdev->dev, "too spread resource: max %d chips\n",
+			GPMI_MAX_CHIPS);
+
+	for (dma = r->start;
+	     dma < min_t(int, r->end, r->start + GPMI_MAX_CHIPS);
+	     dma++) {
+		err = gpmi_init_chip(pdev, g, dma - r->start, dma);
+		if (err)
+			goto out6;
+	}
+
+	g->cmd_buffer_size = GPMI_CMD_BUF_SZ;
+	g->write_buffer_size = GPMI_WRITE_BUF_SZ;
+	g->data_buffer_size = GPMI_DATA_BUF_SZ;
+	g->oob_buffer_size = GPMI_OOB_BUF_SZ;
+
+	err = gpmi_alloc_buffers(pdev, g);
+	if (err) {
+		dev_err(&pdev->dev, "can't setup buffers\n");
+		goto out6;
+	}
+
+	g->dev = pdev;
+	g->chip.priv = g;
+	g->timing = gpmi_safe_timing;
+	g->selected_chip = -1;
+	g->ignorebad = ignorebad;	/* copy global setting */
+
+	g->mtd.priv = &g->chip;
+	g->mtd.name = pdev->dev.bus_id;
+	g->mtd.owner = THIS_MODULE;
+
+	g->chip.cmd_ctrl = gpmi_hwcontrol;
+	g->chip.read_word = gpmi_read_word;
+	g->chip.read_byte = gpmi_read_byte;
+	g->chip.read_buf = gpmi_read_buf;
+	g->chip.write_buf = gpmi_write_buf;
+	g->chip.select_chip = gpmi_select_chip;
+	g->chip.verify_buf = gpmi_verify_buf;
+	g->chip.dev_ready = gpmi_dev_ready;
+
+	g->chip.ecc.mode = NAND_ECC_HW_SYNDROME;
+	g->chip.ecc.write_oob = gpmi_ecc_write_oob;
+	g->chip.ecc.read_oob = gpmi_ecc_read_oob;
+	g->chip.ecc.write_page = gpmi_ecc_write_page;
+	g->chip.ecc.read_page = gpmi_ecc_read_page;
+	g->chip.ecc.read_page_raw = gpmi_read_page_raw;
+	g->chip.ecc.write_page_raw = gpmi_write_page_raw;
+	g->chip.ecc.size = 512;
+
+	g->chip.write_page = gpmi_write_page;
+
+	g->chip.scan_bbt = gpmi_scan_bbt;
+	g->chip.block_bad = gpmi_block_bad;
+
+	g->cmd_buffer_sz = 0;
+
+	/* first scan to find the device and get the page size */
+	if (nand_scan_ident(&g->mtd, max_chips)
+		    || gpmi_scan_middle(g)
+		    || nand_scan_tail(&g->mtd)) {
+		dev_err(&pdev->dev, "No NAND found\n");
+		/* errors found on some step */
+		goto out7;
+	}
+
+	g->chip.options |= NAND_NO_SUBPAGE_WRITE;
+	g->chip.subpagesize = g->mtd.writesize;
+	g->mtd.subpage_sft = 0;
+
+	g->mtd.erase = gpmi_erase;
+
+	g->saved_read_oob = g->mtd.read_oob;
+	g->saved_write_oob = g->mtd.write_oob;
+	g->mtd.read_oob = gpmi_read_oob;
+	g->mtd.write_oob = gpmi_write_oob;
+
+#ifdef CONFIG_MTD_PARTITIONS
+	if (gpd == NULL)
+		goto out_all;
+
+	if (gpd->parts[0].part_probe_types) {
+		g->nr_parts = parse_mtd_partitions(&g->mtd,
+				   gpd->parts[0].part_probe_types,
+				   &g->parts, 0);
+		if (g->nr_parts > 0)
+			part_type = "command line";
+		else
+			g->nr_parts = 0;
+	}
+
+	if (g->nr_parts == 0 && gpd->parts[0].partitions) {
+		g->parts = gpd->parts[0].partitions;
+		g->nr_parts = gpd->parts[0].nr_partitions;
+		part_type = "static";
+	}
+
+	if (g->nr_parts == 0) {
+		dev_err(&pdev->dev, "Neither part_probe_types nor "
+			"partitions was specified in platform_data");
+		goto out_all;
+	}
+
+	dev_info(&pdev->dev, "Using %s partition definition\n",
+		 part_type);
+
+	g->numchips = g->chip.numchips;
+	chipsize = g->mtd.size;
+	do_div(chipsize, (unsigned long)g->numchips);
+
+	if (!strcmp(part_type, "command line"))
+		add_mtd_partitions(&g->mtd, g->parts, g->nr_parts);
+	else
+		gpmi_create_partitions(g, gpd, chipsize);
+
+	if (add_mtd_entire) {
+		printk(KERN_NOTICE "Adding MTD covering the whole flash\n");
+		add_mtd_device(&g->mtd);
+	}
+#else
+	add_mtd_device(&g->mtd);
+#endif
+	gpmi_uid_init("nand",
+		&g->mtd, gpd->uid_offset, gpd->uid_size);
+
+#ifdef CONFIG_MTD_NAND_GPMI_SYSFS_ENTRIES
+	gpmi_sysfs(pdev, true);
+#endif
+	return 0;
+
+out_all:
+	ecc8_exit();
+	bch_exit();
+out7:
+	nand_release(&g->mtd);
+	gpmi_free_buffers(pdev, g);
+out6:
+	gpmi_deinit_chip(pdev, g, -1);
+out5:
+	free_irq(g->irq, g);
+out4:
+	del_timer_sync(&g->timer);
+	gpmi_nand_release_hw(pdev);
+out3:
+	platform_set_drvdata(pdev, NULL);
+	iounmap(g->io_base);
+out2:
+	kfree(g);
+out1:
+	return err;
+}
+
+/**
+ * gpmi_nand_remove - remove a GPMI device
+ *
+ */
+static int __devexit gpmi_nand_remove(struct platform_device *pdev)
+{
+	struct gpmi_nand_data *g = platform_get_drvdata(pdev);
+	int i = 0;
+#ifdef CONFIG_MTD_PARTITIONS
+	struct gpmi_platform_data *gpd = pdev->dev.platform_data;
+	struct mtd_partition *platf_parts;
+#endif
+
+	gpmi_delete_partitions(g);
+	del_timer_sync(&g->timer);
+	gpmi_uid_remove("nand");
+#ifdef CONFIG_MTD_NAND_GPMI_SYSFS_ENTRIES
+	gpmi_sysfs(pdev, false);
+#endif
+	nand_release(&g->mtd);
+	gpmi_free_buffers(pdev, g);
+	gpmi_deinit_chip(pdev, g, -1);
+	gpmi_nand_release_hw(pdev);
+	free_irq(g->irq, g);
+	if (g->regulator)
+		regulator_put(g->regulator);
+
+#ifdef CONFIG_MTD_PARTITIONS
+	if (i < gpd->items && gpd->parts[i].partitions)
+		platf_parts = gpd->parts[i].partitions;
+	else
+		platf_parts = NULL;
+	if (g->parts && g->parts != platf_parts)
+		kfree(g->parts);
+#endif
+	iounmap(g->io_base);
+	kfree(g);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int gpmi_nand_suspend(struct platform_device *pdev, pm_message_t pm)
+{
+	struct gpmi_nand_data *g = platform_get_drvdata(pdev);
+	int r = 0;
+
+	if (g->self_suspended)
+		gpmi_self_wakeup(g);
+	del_timer_sync(&g->timer);
+
+	r = g->mtd.suspend(&g->mtd);
+	if (r == 0)
+		gpmi_nand_release_hw(pdev);
+
+	return r;
+}
+
+static int gpmi_nand_resume(struct platform_device *pdev)
+{
+	struct gpmi_nand_data *g = platform_get_drvdata(pdev);
+	int r;
+
+	r = gpmi_nand_init_hw(pdev, 1);
+	gpmi_set_timings(pdev, &g->timing);
+	g->mtd.resume(&g->mtd);
+	g->timer.expires = jiffies + 4*HZ;
+	add_timer(&g->timer);
+	return r;
+}
+#else
+#define gpmi_nand_suspend	NULL
+#define gpmi_nand_resume	NULL
+#endif
+
+#ifdef CONFIG_MTD_NAND_GPMI_SYSFS_ENTRIES
+static ssize_t show_timings(struct device *d, struct device_attribute *attr,
+			char *buf)
+{
+	struct gpmi_nand_timing *ptm;
+	struct gpmi_nand_data *g = dev_get_drvdata(d);
+
+	ptm = &g->timing;
+	return sprintf(buf, "DATA_SETUP %d, DATA_HOLD %d, "
+		"ADDR_SETUP %d, DSAMPLE_TIME %d\n",
+		ptm->data_setup, ptm->data_hold,
+		ptm->address_setup,
+		ptm->dsample_time);
+}
+
+static ssize_t store_timings(struct device *d, struct device_attribute *attr,
+			const char *buf, size_t size)
+{
+	const char *p, *end;
+	struct gpmi_nand_timing t;
+	struct gpmi_nand_data *g = dev_get_drvdata(d);
+	char tmps[20];
+	u8 *timings[] = {
+		&t.data_setup,
+		&t.data_hold,
+		&t.address_setup,
+		&t.dsample_time,
+		NULL,
+	};
+	u8 **timing = timings;
+
+	p = buf;
+
+	/* parse values */
+	while (*timing != NULL) {
+		unsigned long t_long;
+
+		end = strchr(p, ',');
+		memset(tmps, 0, sizeof(tmps));
+		if (end)
+			strncpy(tmps, p, min_t(int, sizeof(tmps) - 1, end - p));
+		else
+			strncpy(tmps, p, sizeof(tmps) - 1);
+
+		if (strict_strtoul(tmps, 0, &t_long) < 0)
+			return -EINVAL;
+
+		if (t_long > 255)
+			return -EINVAL;
+
+		**timing = (u8)t_long;
+		timing++;
+
+		if (!end && *timing)
+			return -EINVAL;
+		p = end + 1;
+	}
+
+	gpmi_set_timings(g->dev, &t);
+
+	return size;
+}
+
+static ssize_t show_stat(struct device *d, struct device_attribute *attr,
+			char *buf)
+{
+	return sprintf(buf, "copies\t\t%dff pages\t%d\n", copies, ff_writes);
+}
+
+static ssize_t show_chips(struct device *d, struct device_attribute *attr,
+			char *buf)
+{
+	struct gpmi_nand_data *g = dev_get_drvdata(d);
+	return sprintf(buf, "%d\n", g->numchips);
+}
+
+
+static ssize_t show_ignorebad(struct device *d, struct device_attribute *attr,
+			char *buf)
+{
+	struct gpmi_nand_data *g = dev_get_drvdata(d);
+
+	return sprintf(buf, "%d\n", g->ignorebad);
+}
+
+static ssize_t store_ignorebad(struct device *d, struct device_attribute *attr,
+			const char *buf, size_t size)
+{
+	struct gpmi_nand_data *g = dev_get_drvdata(d);
+	const char *p = buf;
+	unsigned long v;
+
+	if (strict_strtoul(p, 0, &v) < 0)
+			return size;
+	if (v > 0)
+		v = 1;
+	if (v != g->ignorebad) {
+		if (v) {
+			g->bbt = g->chip.bbt;
+			g->chip.bbt = NULL;
+			g->ignorebad = 1;
+		} else {
+			g->chip.bbt = g->bbt;
+			g->ignorebad = 0;
+		}
+	}
+	return size;
+}
+
+static DEVICE_ATTR(timings, 0644, show_timings, store_timings);
+static DEVICE_ATTR(stat, 0444, show_stat, NULL);
+static DEVICE_ATTR(ignorebad, 0644, show_ignorebad, store_ignorebad);
+static DEVICE_ATTR(numchips, 0444, show_chips, NULL);
+
+static struct device_attribute *gpmi_attrs[] = {
+	&dev_attr_timings,
+	&dev_attr_stat,
+	&dev_attr_ignorebad,
+	&dev_attr_numchips,
+	NULL,
+};
+
+int gpmi_sysfs(struct platform_device *pdev, int create)
+{
+	int err = 0;
+	int i;
+
+	if (create) {
+		for (i = 0; gpmi_attrs[i]; i++) {
+			err = device_create_file(&pdev->dev, gpmi_attrs[i]);
+			if (err)
+				break;
+		}
+		if (err)
+			while (--i >= 0)
+				device_remove_file(&pdev->dev, gpmi_attrs[i]);
+	} else {
+		for (i = 0; gpmi_attrs[i]; i++)
+			device_remove_file(&pdev->dev, gpmi_attrs[i]);
+	}
+	return err;
+}
+#endif
+
+static struct platform_driver gpmi_nand_driver = {
+	.probe = gpmi_nand_probe,
+	.remove = __devexit_p(gpmi_nand_remove),
+	.driver = {
+		   .name = "gpmi",
+		   .owner = THIS_MODULE,
+	},
+	.suspend = gpmi_nand_suspend,
+	.resume = gpmi_nand_resume,
+};
+
+static LIST_HEAD(gpmi_hwecc_chips);
+
+void gpmi_hwecc_chip_add(struct gpmi_hwecc_chip *chip)
+{
+	list_add(&chip->list, &gpmi_hwecc_chips);
+}
+EXPORT_SYMBOL_GPL(gpmi_hwecc_chip_add);
+
+void gpmi_hwecc_chip_remove(struct gpmi_hwecc_chip *chip)
+{
+	list_del(&chip->list);
+}
+EXPORT_SYMBOL_GPL(gpmi_hwecc_chip_remove);
+
+struct gpmi_hwecc_chip *gpmi_hwecc_chip_find(char *name)
+{
+	struct gpmi_hwecc_chip *c;
+
+	list_for_each_entry(c, &gpmi_hwecc_chips, list)
+		if (strncmp(c->name, name, sizeof(c->name)) == 0)
+			return c;
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(gpmi_hwecc_chip_find);
+
+static int __init gpmi_nand_init(void)
+{
+	bch_init();
+	ecc8_init();
+	return platform_driver_register(&gpmi_nand_driver);
+}
+
+static void __exit gpmi_nand_exit(void)
+{
+	platform_driver_unregister(&gpmi_nand_driver);
+}
+
+module_init(gpmi_nand_init);
+module_exit(gpmi_nand_exit);
+MODULE_AUTHOR("dmitry pervushin <dimka@embeddedalley.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("GPMI NAND driver");
+module_param(max_chips, int, 0400);
+module_param(clk, long, 0400);
+module_param(bch, int, 0600);
+module_param(map_buffers, int, 0600);
+module_param(raw_mode, int, 0600);
+module_param(debug, int, 0600);
+module_param(add_mtd_entire, int, 0400);
+module_param(add_mtd_chip, int, 0400);
+module_param(ignorebad, int, 0400);
diff --git a/drivers/mtd/nand/gpmi/gpmi-bbt.c b/drivers/mtd/nand/gpmi/gpmi-bbt.c
new file mode 100644
index 000000000000..54755f870440
--- /dev/null
+++ b/drivers/mtd/nand/gpmi/gpmi-bbt.c
@@ -0,0 +1,565 @@
+/*
+ * Freescale STMP37XX/STMP378X GPMI (General-Purpose-Media-Interface)
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/partitions.h>
+#include <linux/dma-mapping.h>
+#include <linux/ctype.h>
+#include <mach/dma.h>
+#include <mach/unique-id.h>
+#include "gpmi.h"
+
+static int boot_search_count;
+static int stride = 64;
+static int ncb_version = 3;
+module_param(boot_search_count, int, 0400);
+module_param(ncb_version, int, 0400);
+
+void *gpmi_read_page(struct mtd_info *mtd, loff_t start, void *data, int raw)
+{
+	int ret;
+	struct mtd_oob_ops ops;
+
+	if (!data)
+		data = kzalloc(mtd->writesize + mtd->oobsize, GFP_KERNEL);
+	if (!data)
+		return NULL;
+
+	if (raw)
+		ops.mode = MTD_OOB_RAW;
+	else
+		ops.mode = MTD_OOB_PLACE;
+	ops.datbuf = data;
+	ops.len = mtd->writesize;
+	ops.oobbuf = data + mtd->writesize;
+	ops.ooblen = mtd->oobsize;
+	ops.ooboffs = 0;
+	ret = nand_do_read_ops(mtd, start, &ops);
+
+	if (ret)
+		return NULL;
+	return data;
+}
+
+int gpmi_write_ncb(struct mtd_info *mtd, struct gpmi_bcb_info *b)
+{
+	struct gpmi_ncb *ncb = NULL, *unencoded_ncb = NULL;
+	struct nand_chip *chip = mtd->priv;
+	int err;
+	loff_t start = 0;
+	struct mtd_oob_ops ops;
+	struct erase_info instr;
+	int ncb_count;
+
+	ncb = kzalloc(mtd->writesize + mtd->oobsize, GFP_KERNEL);
+	if (!ncb) {
+		err = -ENOMEM;
+		goto out;
+	}
+	unencoded_ncb = kzalloc(mtd->writesize, GFP_KERNEL);
+	if (!unencoded_ncb) {
+		err = -ENOMEM;
+		goto out;
+	}
+	ops.mode = -1; /* if the value is not set in switch below,
+			  this will cause BUG. Take care. */
+	if (b && b->pre_ncb)
+		memcpy(unencoded_ncb, b->pre_ncb, b->pre_ncb_size);
+	else {
+		memcpy(&unencoded_ncb->fingerprint1, SIG1, sizeof(u32));
+		memcpy(&unencoded_ncb->fingerprint2, SIG_NCB, sizeof(u32));
+		if (b)
+			unencoded_ncb->timing = b->timing;
+	}
+
+	switch (ncb_version) {
+	case 0:
+		ops.mode = MTD_OOB_AUTO;
+		memcpy(ncb, unencoded_ncb, sizeof(*unencoded_ncb));
+		break;
+#ifdef CONFIG_MTD_NAND_GPMI_TA1
+	case 1:
+		ops.mode = MTD_OOB_RAW;
+		gpmi_encode_hamming_ncb_22_16(unencoded_ncb,
+			NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES,
+			ncb, mtd->writesize + mtd->oobsize);
+		break;
+#endif
+#ifdef CONFIG_MTD_NAND_GPMI_TA3
+	case 3:
+		ops.mode = MTD_OOB_RAW;
+		gpmi_encode_hamming_ncb_13_8(unencoded_ncb,
+			NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES,
+			ncb, mtd->writesize + mtd->oobsize);
+		break;
+#endif
+
+	default:
+		printk(KERN_ERR"Incorrect ncb_version = %d\n", ncb_version);
+		err = -EINVAL;
+		goto out;
+	}
+
+	ops.datbuf = (u8 *)ncb;
+	ops.len = mtd->writesize;
+	ops.oobbuf = (u8 *)ncb + mtd->writesize;
+	ops.ooblen = mtd->oobsize;
+	ops.ooboffs = 0;
+
+	ncb_count = 0;
+	do {
+		printk(KERN_NOTICE"GPMI: Trying to store NCB at addr %lx\n",
+				(unsigned long)start);
+		memset(&instr, 0, sizeof(instr));
+		instr.mtd = mtd;
+		instr.addr = start;
+		instr.len = (1 << chip->phys_erase_shift);
+		err = nand_erase_nand(mtd, &instr, 0);
+		if (err == 0) {
+			printk(KERN_NOTICE"GPMI: Erased, storing\n");
+			err = nand_do_write_ops(mtd, start, &ops);
+			printk(KERN_NOTICE"GPMI: NCB update %s (%d).\n",
+					err ? "failed" : "succeeded", err);
+		}
+		start += (1 << chip->phys_erase_shift);
+		ncb_count++;
+	} while (err != 0 && ncb_count < 100);
+
+	if (b)
+		b->ncbblock = start >> chip->bbt_erase_shift;
+
+out:
+	kfree(ncb);
+	kfree(unencoded_ncb);
+
+	return 0;
+}
+
+static int gpmi_redundancy_check_one(u8 *pg, int dsize, int esize, int offset,
+		int o1, int o2)
+{
+	int r;
+
+	if (o1 == o2)
+		return 0;
+
+	r = memcmp(pg + o1 * dsize, pg + o2 * dsize, dsize);
+	if (r) {
+		pr_debug("DATA copies %d and %d are different: %d\n",
+			o1, o2, r);
+		return r;
+	}
+
+	r = memcmp(pg + o1 * esize + offset,
+		   pg + o2 * esize + offset, esize);
+	if (r) {
+		pr_debug("ECC copies %d and %d are different: %d\n", o1, o2, r);
+		return r;
+	}
+	pr_debug("Both DATA and ECC copies %d and %d are identical\n", o1, o2);
+	return r;
+}
+
+static int gpmi_redundancy_check(u8 *pg, int dsize, int esize, int ecc_offset)
+{
+	if (gpmi_redundancy_check_one(pg, dsize, esize, ecc_offset, 0, 1) == 0)
+		return 0;
+	if (gpmi_redundancy_check_one(pg, dsize, esize, ecc_offset, 0, 2) == 0)
+		return 0;
+	if (gpmi_redundancy_check_one(pg, dsize, esize, ecc_offset, 1, 2) == 0)
+		return 1;
+	return -1;
+}
+
+static inline int gpmi_ncb1_redundancy_check(u8 *pg)
+{
+	return gpmi_redundancy_check(pg,
+		NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES,
+		NAND_HC_ECC_SIZEOF_PARITY_BLOCK_IN_BYTES,
+		NAND_HC_ECC_OFFSET_FIRST_PARITY_COPY);
+}
+
+static int gpmi_scan_sigmatel_bbt(
+	struct mtd_info *mtd, struct gpmi_bcb_info *nfo)
+{
+	int page, r;
+	u8 *pg;
+	struct gpmi_ncb *result = NULL;
+
+	if (boot_search_count == 0)
+		boot_search_count = 1;
+	if (nfo == NULL)
+		return -EINVAL;
+
+	pg = NULL;
+	printk(KERN_NOTICE"Scanning for NCB...\n");
+	for (page = 0; page < (1<<boot_search_count); page += stride) {
+		pg = gpmi_read_page(mtd, page * mtd->writesize,
+				pg, ncb_version != 0);
+
+		printk(KERN_NOTICE"GPMI: Checking page 0x%08X\n", page);
+
+		if (ncb_version == 0) {
+			if (memcmp(pg, SIG1, SIG_SIZE) != 0)
+				continue;
+			printk(KERN_NOTICE"GPMI: Signature found at 0x%08X\n",
+				 page);
+			result = (struct gpmi_ncb *)pg;
+		}
+
+#ifdef CONFIG_MTD_NAND_GPMI_TA1
+		if (ncb_version == 1) {
+			void *dptr, *eccptr;
+
+			if (memcmp(pg, SIG1, SIG_SIZE) != 0)
+				continue;
+			printk(KERN_NOTICE"GPMI: Signature found at 0x%08X\n",
+				 page);
+
+			r = gpmi_ncb1_redundancy_check(pg);
+
+			if (r < 0) {
+				printk(KERN_ERR"GPMI: Oops. All three "
+					"copies of NCB are differrent!\n");
+				continue;
+			}
+
+			dptr = pg + r * NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES;
+			eccptr = pg + NAND_HC_ECC_OFFSET_FIRST_PARITY_COPY +
+				r * NAND_HC_ECC_SIZEOF_PARITY_BLOCK_IN_BYTES;
+
+			if (gpmi_verify_hamming_22_16(dptr, eccptr,
+				NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES) < 0) {
+				printk(KERN_ERR"Verification failed.\n");
+				continue;
+			}
+			result = (struct gpmi_ncb *)pg;
+		}
+#endif
+
+#ifdef CONFIG_MTD_NAND_GPMI_TA3
+		if (ncb_version == 3) {
+
+			if (memcmp(pg + 12, SIG1, SIG_SIZE) != 0)
+				continue;
+
+			printk(KERN_NOTICE"GPMI: Signature found at 0x%08X\n",
+				 page);
+
+			if (gpmi_verify_hamming_13_8(
+				pg + 12,
+				pg + 524,
+				NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES) < 0) {
+				printk(KERN_ERR"Verification failed.\n");
+				continue;
+			}
+			result = (struct gpmi_ncb *)(pg + 12);
+		}
+#endif
+		if (result) {
+			printk(KERN_NOTICE"GPMI: Valid NCB found "
+					"at 0x%08x\n", page);
+			nfo->timing = result->timing;
+			nfo->ncbblock = page * mtd->writesize;
+			break;
+		}
+	}
+	kfree(pg);
+
+	return result != NULL;
+}
+
+int gpmi_scan_bbt(struct mtd_info *mtd)
+{
+	struct gpmi_bcb_info stmp_bbt;
+	struct nand_chip *this = mtd->priv;
+	struct gpmi_nand_data *g = this->priv;
+	int r;
+	int numblocks, from, i, ign;
+
+	memset(&stmp_bbt, 0, sizeof(stmp_bbt));
+	g->transcribe_bbmark = 0;
+
+	/*
+	   Since NCB uses the full page, including BB pattern bits,
+	   driver has to ignore result of gpmi_block_bad when reading
+	   these pages.
+	 */
+	ign = g->ignorebad;
+
+	g->ignorebad = true; 	/* strictly speaking, I'd have to hide
+				 * the BBT too.
+				 * But we still scanning it :)	*/
+	r = gpmi_scan_sigmatel_bbt(mtd, &stmp_bbt);
+
+	/* and then, driver has to restore the setting */
+	g->ignorebad = ign;
+
+	if (r) {
+		printk(KERN_NOTICE"Setting discovered timings: %d:%d:%d:%d\n",
+			stmp_bbt.timing.data_setup,
+			stmp_bbt.timing.data_hold,
+			stmp_bbt.timing.address_setup,
+			stmp_bbt.timing.dsample_time);
+
+		gpmi_set_timings(g->dev, &stmp_bbt.timing);
+		g->timing = stmp_bbt.timing;
+
+	} else {
+		g->transcribe_bbmark = !0;
+		numblocks = this->chipsize >> this->bbt_erase_shift;
+		from = 0;
+		printk(KERN_NOTICE"Checking BB on common-formatted flash\n");
+		for (i = stmp_bbt.ncbblock + 1; i < numblocks; i++) {
+			/* check the block and transcribe the bb if needed */
+			gpmi_block_bad(mtd, from, 0);
+			from += (1 << this->bbt_erase_shift);
+		}
+	}
+
+	r = nand_default_bbt(mtd);
+
+	if (g->transcribe_bbmark) {
+		/* NCB has been not found, so create NCB now */
+		g->transcribe_bbmark = 0;
+
+		stmp_bbt.timing = gpmi_safe_timing;
+		r = gpmi_write_ncb(mtd, &stmp_bbt);
+	} else {
+		/* NCB found, and its block should be marked as "good" */
+		gpmi_block_mark_as(this, stmp_bbt.ncbblock, 0x00);
+	}
+
+	return r;
+}
+
+int gpmi_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
+
+{
+	int page, res = 0;
+	struct nand_chip *chip = mtd->priv;
+	u16 bad;
+	struct gpmi_nand_data *g = chip->priv;
+	int chipnr;
+
+	/* badblockpos is an offset in OOB area */
+	int badblockpos = chip->ecc.steps * chip->ecc.bytes;
+
+	if (g->ignorebad)
+		return 0;
+
+	page = (int)(ofs >> chip->page_shift) & chip->pagemask;
+
+	chipnr = (int)(ofs >> chip->chip_shift);
+	chip->select_chip(mtd, chipnr);
+
+	if (g->transcribe_bbmark)
+		/* bad block marks still are on first byte of OOB */
+		badblockpos = 0;
+
+	if (chip->options & NAND_BUSWIDTH_16) {
+		chip->cmdfunc(mtd, NAND_CMD_READOOB, badblockpos & 0xFE,
+			      page);
+		bad = cpu_to_le16(chip->read_word(mtd));
+		if (badblockpos & 0x1)
+			bad >>= 8;
+		if ((bad & 0xFF) != 0xff)
+			res = 1;
+	} else {
+		chip->cmdfunc(mtd, NAND_CMD_READOOB, badblockpos, page);
+		if (chip->read_byte(mtd) != 0xff)
+			res = 1;
+	}
+
+	if (g->transcribe_bbmark && res)
+		chip->block_markbad(mtd, ofs);
+
+	chip->select_chip(mtd, -1);
+
+	return res;
+}
+
+#if defined(CONFIG_STMP3XXX_UNIQUE_ID)
+/*
+ * UID on NAND support
+ */
+const int uid_size = 256;
+
+struct gpmi_uid_context {
+	struct mtd_info *mtd;
+	struct nand_chip *nand;
+	u_int32_t start;
+	u_int32_t size;
+};
+
+static int gpmi_read_uid(struct gpmi_uid_context *ctx, void *result)
+{
+	void *pg = NULL;
+	int page, o;
+	int status = -ENOENT;
+	int h_size = gpmi_hamming_ecc_size_22_16(uid_size);
+
+	for (page = ctx->start >> ctx->nand->page_shift;
+	     page < (ctx->start + ctx->size) >> ctx->nand->page_shift;) {
+		pr_debug("%s: reading page 0x%x\n", __func__, page);
+		if (gpmi_block_bad(ctx->mtd, page * ctx->mtd->writesize, 0)) {
+			pr_debug("%s: bad block %x, skipping it\n",
+				__func__, page * ctx->mtd->writesize);
+			page += (1 << ctx->nand->phys_erase_shift)
+					>> ctx->nand->page_shift;
+			continue;
+		}
+		pg = gpmi_read_page(ctx->mtd, page * ctx->mtd->writesize,
+						pg, 0);
+		if (pg)
+			break;
+		page++;
+	}
+
+	if (!pg)
+		return status;
+
+	o = gpmi_redundancy_check(pg, uid_size, h_size, 3 * uid_size);
+	if (o >= 0) {
+		if (gpmi_verify_hamming_22_16(
+				pg + o * uid_size,
+				pg + 3 * uid_size + h_size, uid_size) >= 0) {
+			memcpy(result, pg + o * uid_size, uid_size);
+			status = 0;
+		}
+	}
+	kfree(pg);
+	return status;
+}
+
+static int gpmi_write_uid(struct gpmi_uid_context *ctx, void *src)
+{
+	struct mtd_oob_ops ops;
+	struct erase_info instr;
+	u8 *data = kzalloc(ctx->mtd->writesize + ctx->mtd->oobsize, GFP_KERNEL);
+	int h_size = gpmi_hamming_ecc_size_22_16(uid_size);
+	char ecc[h_size];
+	u_int32_t start;
+	int i;
+	int err;
+
+	if (!data)
+		return -ENOMEM;
+
+	gpmi_encode_hamming_22_16(src, uid_size, ecc, h_size);
+	for (i = 0; i < 3; i++) {
+		memcpy(data + i * uid_size, src, uid_size);
+		memcpy(data + 3 * uid_size + i * h_size, ecc, h_size);
+	}
+
+	ops.mode = MTD_OOB_AUTO;
+	ops.datbuf = data;
+	ops.len = ctx->mtd->writesize;
+	ops.oobbuf = NULL;
+	ops.ooblen = ctx->mtd->oobsize;
+	ops.ooboffs = 0;
+
+	start = ctx->start;
+
+	do {
+		memset(&instr, 0, sizeof(instr));
+		instr.mtd = ctx->mtd;
+		instr.addr = start;
+		instr.len = (1 << ctx->nand->phys_erase_shift);
+		err = nand_erase_nand(ctx->mtd, &instr, 0);
+		if (err == 0)
+			err = nand_do_write_ops(ctx->mtd, start, &ops);
+		start += (1 << ctx->nand->phys_erase_shift);
+		if (start > ctx->start + ctx->size)
+			break;
+	} while (err != 0);
+
+	return err;
+}
+
+static ssize_t gpmi_uid_store(void *context, const char *page,
+				size_t count, int ascii)
+{
+	u8 data[uid_size];
+
+	memset(data, 0, sizeof(data));
+	memcpy(data, page, uid_size < count ? uid_size : count);
+	gpmi_write_uid(context, data);
+	return count;
+}
+
+static ssize_t gpmi_uid_show(void *context, char *page, int ascii)
+{
+	u8 result[uid_size];
+	int i;
+	char *p = page;
+	int r;
+
+	r = gpmi_read_uid(context, result);
+	if (r < 0)
+		return r;
+
+	if (ascii) {
+		for (i = 0; i < uid_size; i++) {
+			if (i % 16 == 0) {
+				if (i)
+					*p++ = '\n';
+				sprintf(p, "%04X: ", i);
+				p += strlen(p);
+			}
+			sprintf(p, "%02X ", result[i]);
+			p += strlen(p);
+		}
+		*p++ = '\n';
+		return p - page;
+
+	} else {
+		memcpy(page, result, uid_size);
+		return uid_size;
+	}
+}
+
+static struct uid_ops gpmi_uid_ops = {
+	.id_show 	= gpmi_uid_show,
+	.id_store 	= gpmi_uid_store,
+};
+
+static struct gpmi_uid_context gpmi_uid_context;
+
+int  __init gpmi_uid_init(const char *name, struct mtd_info *mtd,
+			u_int32_t start, u_int32_t size)
+{
+	gpmi_uid_context.mtd = mtd;
+	gpmi_uid_context.nand = mtd->priv;
+	gpmi_uid_context.start = start;
+	gpmi_uid_context.size = size;
+	return uid_provider_init(name, &gpmi_uid_ops, &gpmi_uid_context) ?
+			 0 : -EFAULT;
+}
+
+void gpmi_uid_remove(const char *name)
+{
+	uid_provider_remove(name);
+}
+#endif
diff --git a/drivers/mtd/nand/gpmi/gpmi-bch.c b/drivers/mtd/nand/gpmi/gpmi-bch.c
new file mode 100644
index 000000000000..58f7354bcd18
--- /dev/null
+++ b/drivers/mtd/nand/gpmi/gpmi-bch.c
@@ -0,0 +1,289 @@
+/*
+ * Freescale STMP37XX/STMP378X GPMI (General-Purpose-Media-Interface)
+ *
+ * STMP378X BCH hardware ECC engine
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/dma.h>
+#include <mach/stmp3xxx.h>
+#include <mach/irqs.h>
+#include "gpmi.h"
+
+#define BCH_MAX_NANDS 4
+
+static int bch_available(void *context);
+static int bch_setup(void *context, int index, int writesize, int oobsize);
+static int bch_read(void *context,
+		int index,
+		struct stmp3xxx_dma_descriptor *chain,
+		dma_addr_t error,
+		dma_addr_t page, dma_addr_t oob);
+static int bch_stat(void *ctx, int index, struct mtd_ecc_stats *r);
+static int bch_reset(void *context, int index);
+
+struct bch_state_t {
+	struct gpmi_hwecc_chip chip;
+	struct {
+		struct mtd_ecc_stats stat;
+		struct completion done;
+		u32 writesize, oobsize;
+	} nands[BCH_MAX_NANDS];
+};
+
+static struct bch_state_t state = {
+	.chip = {
+		.name		= "bch",
+		.setup		= bch_setup,
+		.stat		= bch_stat,
+		.read		= bch_read,
+		.reset		= bch_reset,
+	},
+};
+
+static int bch_reset(void *context, int index)
+{
+	stmp3xxx_reset_block(REGS_BCH_BASE, true);
+	HW_BCH_CTRL_SET(BM_BCH_CTRL_COMPLETE_IRQ_EN);
+	return 0;
+}
+
+static int bch_stat(void *context, int index, struct mtd_ecc_stats *r)
+{
+	struct bch_state_t *state = context;
+
+	*r = state->nands[index].stat;
+	state->nands[index].stat.failed = 0;
+	state->nands[index].stat.corrected = 0;
+	return 0;
+}
+
+static irqreturn_t bch_irq(int irq, void *context)
+{
+	u32 b0, s0;
+	struct mtd_ecc_stats stat;
+	int r;
+	struct bch_state_t *state = context;
+
+	s0 = HW_BCH_STATUS0_RD();
+	r = (s0 & BM_BCH_STATUS0_COMPLETED_CE) >> 16;
+
+	stat.corrected = stat.failed = 0;
+
+	b0 = (s0 & BM_BCH_STATUS0_STATUS_BLK0) >> 8;
+	if (b0 <= 4)
+		stat.corrected += b0;
+	if (b0 == 0xFE)
+		stat.failed++;
+
+	if (s0 & BM_BCH_STATUS0_CORRECTED)
+		stat.corrected += (s0 & BM_BCH_STATUS0_CORRECTED);
+	if (s0 & BM_BCH_STATUS0_UNCORRECTABLE)
+		stat.failed++;
+
+	HW_BCH_CTRL_CLR(BM_BCH_CTRL_COMPLETE_IRQ);
+
+	pr_debug("%s: chip %d, failed %d, corrected %d\n",
+			__func__, r,
+			state->nands[r].stat.failed,
+			state->nands[r].stat.corrected);
+	state->nands[r].stat.corrected += stat.corrected;
+	state->nands[r].stat.failed += stat.failed;
+	complete(&state->nands[r].done);
+
+	return IRQ_HANDLED;
+}
+
+static int bch_available(void *context)
+{
+	stmp3xxx_reset_block(REGS_BCH_BASE, 0);
+	return HW_BCH_BLOCKNAME_RD() == 0x20484342;
+}
+
+static int bch_setup(void *context, int index, int writesize, int oobsize)
+{
+	struct bch_state_t *state = context;
+	u32 layout = (REGS_BCH_BASE + 0x80 /* HW_BCH_FLASH0LAYOUT0_ADDR */)
+		+ index * 0x20;
+	u32 ecc0, eccN;
+	int meta;
+
+	switch (writesize) {
+	case 2048:
+		ecc0 = 4;
+		eccN = 4;
+		meta = 5;
+		break;
+	case 4096:
+		ecc0 = 16;
+		eccN = 14;
+		meta = 10;
+		break;
+	default:
+		printk(KERN_ERR"%s: cannot tune BCH for page size %d\n",
+				__func__, writesize);
+		return -EINVAL;
+	}
+
+	state->nands[index].oobsize = oobsize;
+	state->nands[index].writesize = writesize;
+
+	__raw_writel(BF_BCH_FLASH0LAYOUT0_NBLOCKS(writesize/512) |
+		     BF_BCH_FLASH0LAYOUT0_META_SIZE(oobsize) |
+		     BF_BCH_FLASH0LAYOUT0_ECC0(ecc0 >> 1) | /* for oob */
+		     BF_BCH_FLASH0LAYOUT0_DATA0_SIZE(0x00), layout);
+	__raw_writel(BF_BCH_FLASH0LAYOUT1_PAGE_SIZE(writesize + oobsize) |
+		     BF_BCH_FLASH0LAYOUT1_ECCN(eccN >> 1) | /* for dblock */
+		     BF_BCH_FLASH0LAYOUT1_DATAN_SIZE(512), layout + 0x10);
+
+	/*
+	 * since driver only supports CS 0..3, layouts are mapped 1:1 :
+	 * 		FLASHnLAYOUT[1,2] => LAYOUTSELECT[n*2:n2*+1]
+	 */
+	HW_BCH_LAYOUTSELECT_CLR(0x03 << (index * 2));
+	HW_BCH_LAYOUTSELECT_SET(index << (index * 2));
+
+	bch_reset(context, index);
+
+	printk(KERN_DEBUG"%s: CS = %d, LAYOUT = 0x%08X, layout_reg = "
+			"0x%08x+0x%08x: 0x%08x+0x%08x\n",
+			__func__,
+			index, HW_BCH_LAYOUTSELECT_RD(),
+			layout, layout + 0x10,
+			__raw_readl(layout),
+			__raw_readl(layout+0x10));
+	return 0;
+}
+
+static int bch_read(void *context,
+		int index,
+		struct stmp3xxx_dma_descriptor *chain,
+		dma_addr_t error,
+		dma_addr_t page, dma_addr_t oob)
+{
+	unsigned long readsize = 0;
+	u32 bufmask = 0;
+	struct bch_state_t *state = context;
+
+	if (!dma_mapping_error(NULL, oob)) {
+		bufmask |= BV_GPMI_ECCCTRL_BUFFER_MASK__BCH_AUXONLY;
+		readsize += state->nands[index].oobsize;
+	}
+	if (!dma_mapping_error(NULL, page)) {
+		bufmask |= (BV_GPMI_ECCCTRL_BUFFER_MASK__BCH_PAGE
+				& ~BV_GPMI_ECCCTRL_BUFFER_MASK__BCH_AUXONLY);
+		readsize += state->nands[index].writesize;
+	}
+
+	printk(KERN_DEBUG"readsize = %ld, bufmask = 0x%X\n", readsize, bufmask);
+	bch_reset(context, index);
+
+	/* wait for ready */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(1)	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_NANDWAIT4READY	|
+		BM_APBH_CHn_CMD_CHAIN		|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(
+				BV_GPMI_CTRL0_COMMAND_MODE__WAIT_FOR_READY) |
+		BM_GPMI_CTRL0_WORD_LENGTH				|
+		BF_GPMI_CTRL0_ADDRESS(BV_GPMI_CTRL0_ADDRESS__NAND_DATA) |
+		BF_GPMI_CTRL0_CS(index);
+	chain->command->alternate = 0;
+	chain++;
+
+	/* enable BCH and read NAND data */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(6)	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_NANDLOCK	|
+		BM_APBH_CHn_CMD_CHAIN 		|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(BV_GPMI_CTRL0_COMMAND_MODE__READ) |
+		BM_GPMI_CTRL0_WORD_LENGTH	|
+		BF_GPMI_CTRL0_CS(index)		|
+		BF_GPMI_CTRL0_XFER_COUNT(readsize);
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] =
+		BM_GPMI_ECCCTRL_ENABLE_ECC 	|
+		BF_GPMI_ECCCTRL_ECC_CMD(0x02) 	|
+		BF_GPMI_ECCCTRL_BUFFER_MASK(bufmask);
+	chain->command->pio_words[3] = readsize;
+	chain->command->pio_words[4] = !dma_mapping_error(NULL, page) ? page : 0;
+	chain->command->pio_words[5] = !dma_mapping_error(NULL, oob) ? oob : 0;
+	chain->command->alternate = 0;
+	chain++;
+
+	/* disable BCH block */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(3)	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_NANDWAIT4READY	|
+		BM_APBH_CHn_CMD_NANDLOCK	|
+		BM_APBH_CHn_CMD_CHAIN 		|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(
+			BV_GPMI_CTRL0_COMMAND_MODE__WAIT_FOR_READY) |
+		BM_GPMI_CTRL0_WORD_LENGTH	|
+		BF_GPMI_CTRL0_CS(index)		|
+		BF_GPMI_CTRL0_XFER_COUNT(readsize);
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] = 0;
+	chain->command->alternate = 0;
+	chain++;
+
+	/* and deassert nand lock */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(0)	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_IRQONCMPLT	|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->alternate = 0;
+
+	return 0;
+}
+
+int __init bch_init(void)
+{
+	int err;
+
+	if (!bch_available(&state.chip))
+		return -ENXIO;
+	gpmi_hwecc_chip_add(&state.chip);
+	err = request_irq(IRQ_BCH, bch_irq, 0, state.chip.name, &state);
+	if (err)
+		return err;
+
+	printk(KERN_DEBUG"%s: initialized\n", __func__);
+	return 0;
+}
+
+void bch_exit(void)
+{
+	free_irq(IRQ_BCH, &state);
+	gpmi_hwecc_chip_remove(&state.chip);
+}
diff --git a/drivers/mtd/nand/gpmi/gpmi-ecc8.c b/drivers/mtd/nand/gpmi/gpmi-ecc8.c
new file mode 100644
index 000000000000..dad14c3809fc
--- /dev/null
+++ b/drivers/mtd/nand/gpmi/gpmi-ecc8.c
@@ -0,0 +1,382 @@
+/*
+ * Freescale STMP37XX/STMP378X GPMI (General-Purpose-Media-Interface)
+ *
+ * STMP37XX/STMP378X ECC8 hardware ECC engine
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/dma.h>
+#include <mach/stmp3xxx.h>
+#include <mach/irqs.h>
+#include "gpmi.h"
+
+#define ECC8_MAX_NANDS 4
+
+static int ecc8_available(void *context);
+static int ecc8_setup(void *context, int index, int writesize, int oobsize);
+static int ecc8_read(void *context, int index,
+		struct stmp3xxx_dma_descriptor *chain,
+		dma_addr_t error,
+		dma_addr_t page, dma_addr_t oob);
+static int ecc8_write(void *context, int index,
+		struct stmp3xxx_dma_descriptor *chain,
+		dma_addr_t error,
+		dma_addr_t page, dma_addr_t oob);
+static int ecc8_stat(void *ctx, int index, struct mtd_ecc_stats *r);
+static int ecc8_reset(void *context, int index);
+
+struct ecc8_nand {
+};
+
+struct ecc8_state_t {
+	struct gpmi_hwecc_chip chip;
+	struct completion done;
+	struct mtd_ecc_stats stat;
+	u32 writesize, oobsize;
+	u32 ecc_page, ecc_oob, oob_free;
+	u32 r, w;
+	int bits;
+	u32 s0_mask, s1_mask;
+};
+
+static struct ecc8_state_t state = {
+	.chip = {
+		.name	= "ecc8",
+		.setup	= ecc8_setup,
+		.stat	= ecc8_stat,
+		.read	= ecc8_read,
+		.write	= ecc8_write,
+		.reset	= ecc8_reset,
+	},
+};
+
+static int  ecc8_reset(void *context, int index)
+{
+	stmp3xxx_reset_block(REGS_ECC8_BASE, false);
+	while (HW_ECC8_CTRL_RD() & BM_ECC8_CTRL_AHBM_SFTRST)
+		HW_ECC8_CTRL_CLR(BM_ECC8_CTRL_AHBM_SFTRST);
+	HW_ECC8_CTRL_SET(BM_ECC8_CTRL_COMPLETE_IRQ_EN);
+	return 0;
+}
+
+static int ecc8_stat(void *context, int index, struct mtd_ecc_stats *r)
+{
+	struct ecc8_state_t *state = context;
+
+	wait_for_completion(&state->done);
+
+	*r = state->stat;
+	state->stat.failed = 0;
+	state->stat.corrected = 0;
+	return 0;
+}
+
+static irqreturn_t ecc8_irq(int irq, void *context)
+{
+	int r;
+	struct mtd_ecc_stats ecc_stats;
+	struct ecc8_state_t *state = context;
+	u32 corr;
+	u32 s0 = HW_ECC8_STATUS0_RD(), s1 = HW_ECC8_STATUS1_RD();
+
+	r = (s0 & BM_ECC8_STATUS0_COMPLETED_CE) >> 16;
+
+	if (s0 & BM_ECC8_STATUS0_CORRECTED ||
+	    s0 & BM_ECC8_STATUS0_UNCORRECTABLE) {
+
+		ecc_stats.failed = 0;
+		ecc_stats.corrected = 0;
+
+		s0 = (s0 & BM_ECC8_STATUS0_STATUS_AUX) >> 8;
+		if (s0 <= 4)
+		       ecc_stats.corrected += s0;
+		if (s0 == 0xE)
+		       ecc_stats.failed++;
+
+		for ( ; s1 != 0; s1 >>= 4) {
+		       corr = s1 & 0xF;
+		       if (corr == 0x0C)
+			      continue;
+		       if (corr == 0xE)
+			     ecc_stats.failed++;
+		       if (corr <= 8)
+			     ecc_stats.corrected += corr;
+		       s1 >>= 4;
+		}
+		state->stat.corrected += ecc_stats.corrected;
+		state->stat.failed += ecc_stats.failed;
+	}
+
+	complete(&state->done);
+
+	HW_ECC8_CTRL_CLR(BM_ECC8_CTRL_COMPLETE_IRQ);
+	return IRQ_HANDLED;
+}
+
+static int ecc8_available(void *context)
+{
+	return 1;
+}
+
+static int ecc8_setup(void *context, int index, int writesize, int oobsize)
+{
+	struct ecc8_state_t *state = context;
+
+	switch (writesize) {
+	case 2048:
+		state->ecc_page = 9;
+		state->ecc_oob = 9;
+		state->bits = 4;
+		state->r = BF_GPMI_ECCCTRL_ECC_CMD(
+				BV_GPMI_ECCCTRL_ECC_CMD__DECODE_4_BIT);
+		state->w = BF_GPMI_ECCCTRL_ECC_CMD(
+				BV_GPMI_ECCCTRL_ECC_CMD__ENCODE_4_BIT);
+		break;
+	case 4096:
+		state->ecc_page = 18;
+		state->ecc_oob = 9;
+		state->bits = 8;
+		state->r = BF_GPMI_ECCCTRL_ECC_CMD(
+				BV_GPMI_ECCCTRL_ECC_CMD__DECODE_8_BIT);
+		state->w = BF_GPMI_ECCCTRL_ECC_CMD(
+				BV_GPMI_ECCCTRL_ECC_CMD__ENCODE_8_BIT);
+		break;
+	default:
+		return -ENOTSUPP;
+	}
+
+	state->oob_free = oobsize -
+		(state->bits * state->ecc_page) - state->ecc_oob;
+	state->oobsize = oobsize;
+	state->writesize = writesize;
+
+	ecc8_reset(context, index);
+
+	return 0;
+}
+
+/**
+ * ecc8_read - create dma chain to read nand page
+ *
+ * @context: context data, pointer to ecc8_state
+ * @index: CS of nand to read
+ * @chain: dma chain to be filled
+ * @page: (physical) address of page data to be read to
+ * @oob: (physical) address of OOB data to be read to
+ *
+ * Return: status of operation -- 0 on success
+ */
+static int ecc8_read(void *context, int cs,
+		struct stmp3xxx_dma_descriptor *chain,
+		dma_addr_t error,
+		dma_addr_t page, dma_addr_t oob)
+{
+	unsigned long readsize = 0;
+	u32 bufmask = 0;
+	struct ecc8_state_t *state = context;
+
+	ecc8_reset(context, cs);
+
+	state->s0_mask = state->s1_mask = 0;
+
+	if (!dma_mapping_error(NULL, page)) {
+		bufmask |= (1 << state->bits) - 1;
+		readsize += (state->bits * state->ecc_page);
+		readsize += state->writesize;
+	}
+	if (!dma_mapping_error(NULL, oob)) {
+		bufmask |= BV_GPMI_ECCCTRL_BUFFER_MASK__AUXILIARY;
+		readsize += state->oob_free + state->ecc_oob;
+		state->s0_mask = BM_ECC8_STATUS0_STATUS_AUX;
+		if (dma_mapping_error(NULL, page))
+			page = oob;
+	}
+
+
+	/* wait for ready */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(1)	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_NANDWAIT4READY	|
+		BM_APBH_CHn_CMD_CHAIN		|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(
+				BV_GPMI_CTRL0_COMMAND_MODE__WAIT_FOR_READY) |
+		BM_GPMI_CTRL0_WORD_LENGTH				|
+		BF_GPMI_CTRL0_ADDRESS(BV_GPMI_CTRL0_ADDRESS__NAND_DATA) |
+		BF_GPMI_CTRL0_CS(cs);
+	chain->command->buf_ptr = 0;
+	chain++;
+
+	/* enable ECC and read NAND data */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(6)	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_NANDLOCK	|
+		BM_APBH_CHn_CMD_CHAIN 		|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(BV_GPMI_CTRL0_COMMAND_MODE__READ) |
+		BM_GPMI_CTRL0_WORD_LENGTH	|
+		BF_GPMI_CTRL0_CS(cs)		|
+		BF_GPMI_CTRL0_XFER_COUNT(readsize);
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] =
+		BM_GPMI_ECCCTRL_ENABLE_ECC 	|
+		state->r			|
+		BF_GPMI_ECCCTRL_BUFFER_MASK(bufmask);
+	chain->command->pio_words[3] = readsize;
+	chain->command->pio_words[4] = !dma_mapping_error(NULL, page) ? page : 0;
+	chain->command->pio_words[5] = !dma_mapping_error(NULL, oob) ? oob : 0;
+	chain->command->buf_ptr = 0;
+	chain++;
+
+	/* disable ECC block */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(3)	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_NANDWAIT4READY	|
+		BM_APBH_CHn_CMD_NANDLOCK	|
+		BM_APBH_CHn_CMD_CHAIN 		|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(
+				BV_GPMI_CTRL0_COMMAND_MODE__WAIT_FOR_READY) |
+		BM_GPMI_CTRL0_WORD_LENGTH	|
+		BF_GPMI_CTRL0_CS(cs);
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] = 0;
+	chain->command->buf_ptr = 0;
+	chain++;
+
+	/* and deassert nand lock */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(0)	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_IRQONCMPLT	|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->buf_ptr = 0;
+
+	init_completion(&state->done);
+
+	return 0;
+}
+
+static int ecc8_write(void *context, int cs,
+		struct stmp3xxx_dma_descriptor *chain,
+		dma_addr_t error,
+		dma_addr_t page, dma_addr_t oob)
+{
+	u32 bufmask = 0;
+	struct ecc8_state_t *state = context;
+	u32 data_w, data_w_ecc,
+	    oob_w, oob_w_ecc;
+
+	ecc8_reset(context, cs);
+
+	data_w = data_w_ecc = oob_w = oob_w_ecc = 0;
+
+	if (!dma_mapping_error(NULL, oob)) {
+		bufmask |= BV_GPMI_ECCCTRL_BUFFER_MASK__AUXILIARY;
+		oob_w = state->oob_free;
+		oob_w_ecc = oob_w + state->ecc_oob;
+	}
+	if (!dma_mapping_error(NULL, page)) {
+		bufmask |= (1 << state->bits) - 1;
+		data_w = state->bits * 512;
+		data_w_ecc = data_w + state->bits * state->ecc_page;
+	}
+
+	/* enable ECC and send NAND data (page only) */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_XFER_COUNT(data_w ? data_w : oob_w) |
+		BF_APBH_CHn_CMD_CMDWORDS(4)	|
+		BM_APBH_CHn_CMD_NANDLOCK	|
+		BM_APBH_CHn_CMD_CHAIN 		|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__DMA_READ);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(BV_GPMI_CTRL0_COMMAND_MODE__WRITE) |
+		BM_GPMI_CTRL0_WORD_LENGTH	|
+		BM_GPMI_CTRL0_LOCK_CS		|
+		BF_GPMI_CTRL0_CS(cs)		|
+		BF_GPMI_CTRL0_ADDRESS(BV_GPMI_CTRL0_ADDRESS__NAND_DATA) |
+		BF_GPMI_CTRL0_XFER_COUNT(data_w + oob_w);
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] =
+		BM_GPMI_ECCCTRL_ENABLE_ECC 	|
+		state->w				|
+		BF_GPMI_ECCCTRL_BUFFER_MASK(bufmask);
+	chain->command->pio_words[3] =
+		data_w_ecc + oob_w_ecc;
+	if (!dma_mapping_error(NULL, page))
+		chain->command->buf_ptr = page;
+	else
+		chain->command->buf_ptr = oob;
+	chain++;
+
+	if (!dma_mapping_error(NULL, page) && !dma_mapping_error(NULL, oob)) {
+		/* send NAND data (OOB only) */
+		chain->command->cmd =
+			BM_APBH_CHn_CMD_CHAIN |
+			BF_APBH_CHn_CMD_XFER_COUNT(oob_w) |
+			BF_APBH_CHn_CMD_CMDWORDS(0)	|
+			BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+			BM_APBH_CHn_CMD_NANDLOCK	|
+			BF_APBH_CHn_CMD_COMMAND(
+				BV_APBH_CHn_CMD_COMMAND__DMA_READ);
+		chain->command->buf_ptr = oob;	/* never dma_mapping_error() */
+		chain++;
+	}
+	/* emit IRQ */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(0)	|
+		BF_APBH_CHn_CMD_COMMAND(
+			BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER) |
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_IRQONCMPLT;
+
+	return 0;
+}
+
+int __init ecc8_init(void)
+{
+	int err;
+
+	if (!ecc8_available(&state))
+		return -ENXIO;
+
+	gpmi_hwecc_chip_add(&state.chip);
+	err = request_irq(IRQ_ECC8_IRQ, ecc8_irq, 0, state.chip.name, &state);
+	if (err)
+		return err;
+
+	printk(KERN_INFO"%s: initialized\n", __func__);
+	return 0;
+}
+
+void ecc8_exit(void)
+{
+	free_irq(IRQ_ECC8_IRQ, &state);
+	gpmi_hwecc_chip_remove(&state.chip);
+}
diff --git a/drivers/mtd/nand/gpmi/gpmi-hamming-13-8.c b/drivers/mtd/nand/gpmi/gpmi-hamming-13-8.c
new file mode 100644
index 000000000000..c48d70b38bc0
--- /dev/null
+++ b/drivers/mtd/nand/gpmi/gpmi-hamming-13-8.c
@@ -0,0 +1,131 @@
+/*
+ * Freescale STMP37XX/STMP378X GPMI (General-Purpose-Media-Interface)
+ *
+ * NCB software ECC Hamming code
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/platform_device.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <mach/dma.h>
+#include "gpmi.h"
+
+#define BIT_VAL(v, n) (((v) >> (n)) & 0x1)
+#define B(n) (BIT_VAL(d, n))
+
+static u8 calculate_parity(u8 d)
+{
+	u8 p = 0;
+
+	if (d == 0 || d == 0xFF)
+		return 0;	/* optimization :) */
+
+	p |= (B(6) ^ B(5) ^ B(3) ^ B(2)) 	<< 0;
+	p |= (B(7) ^ B(5) ^ B(4) ^ B(2) ^ B(1)) << 1;
+	p |= (B(7) ^ B(6) ^ B(5) ^ B(1) ^ B(0)) << 2;
+	p |= (B(7) ^ B(4) ^ B(3) ^ B(0)) 	<< 3;
+	p |= (B(6) ^ B(4) ^ B(3) ^ B(2) ^ B(1) ^ B(0)) << 4;
+	return p;
+}
+
+static inline int even_number_of_1s(u8 byte)
+{
+	int even = 1;
+
+	while (byte > 0) {
+		even ^= (byte & 0x1);
+		byte >>= 1;
+	}
+	return even;
+}
+
+static int lookup_single_error(u8 syndrome)
+{
+	int i;
+	u8 syndrome_table[] = {
+		0x1C, 0x16, 0x13, 0x19,
+		0x1A, 0x07, 0x15, 0x0E,
+		0x01, 0x02, 0x04, 0x08,
+		0x10,
+	};
+
+	for (i = 0; i < ARRAY_SIZE(syndrome_table); i++)
+		if (syndrome_table[i] == syndrome)
+			return i;
+	return -ENOENT;
+}
+
+int gpmi_verify_hamming_13_8(void *data, u8 *parity, size_t size)
+{
+	int i;
+	u8 *pdata = data;
+	int bit_to_flip;
+	u8 np, syndrome;
+	int errors = 0;
+
+	for (i = 0; i < size; i ++, pdata++) {
+		np = calculate_parity(*pdata);
+		syndrome = np ^ parity[i];
+		if (syndrome == 0) /* cool */ {
+			continue;
+		}
+
+		if (even_number_of_1s(syndrome))
+			return -i;	/* can't recover */
+
+		bit_to_flip = lookup_single_error(syndrome);
+		if (bit_to_flip < 0)
+			return -i;	/* can't fix the error */
+
+		if (bit_to_flip < 8) {
+			*pdata ^= (1 << bit_to_flip);
+			errors++;
+		}
+	}
+	return errors;
+}
+
+void gpmi_encode_hamming_13_8(void *source_block, size_t src_size,
+			void *source_ecc, size_t ecc_size)
+{
+	int i;
+	u8 *src = source_block;
+	u8 *ecc = source_ecc;
+
+	for (i = 0; i < src_size && i < ecc_size; i++)
+		ecc[i] = calculate_parity(src[i]);
+}
+
+void gpmi_encode_hamming_ncb_13_8(void *source_block, size_t source_size,
+		void *target_block, size_t target_size)
+{
+	if (target_size < 12 + 2 * NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES)
+		return;
+	memset(target_block, 0xFF, target_size);
+	memcpy((u8 *)target_block + 12, source_block, source_size);
+	gpmi_encode_hamming_13_8(source_block,
+	   NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES,
+	   (u8 *)target_block + 12 + NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES,
+	   NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES);
+}
+
+unsigned gpmi_hamming_ecc_size_13_8(int block_size)
+{
+	return block_size;
+}
+
diff --git a/drivers/mtd/nand/gpmi/gpmi-hamming-22-16.c b/drivers/mtd/nand/gpmi/gpmi-hamming-22-16.c
new file mode 100644
index 000000000000..8d3975efbc6e
--- /dev/null
+++ b/drivers/mtd/nand/gpmi/gpmi-hamming-22-16.c
@@ -0,0 +1,196 @@
+/*
+ * Freescale STMP37XX/STMP378X GPMI (General-Purpose-Media-Interface)
+ *
+ * NCB software ECC Hamming code
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/platform_device.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <mach/dma.h>
+#include "gpmi.h"
+#include "gpmi-hamming-22-16.h"
+
+#define BIT_VAL(v, n) (((v) >> (n)) & 0x1)
+#define B(n) (BIT_VAL(d, n))
+#define BSEQ(a1, a2, a3, a4, a5, a6, a7, a8) \
+	(B(a1) ^ B(a2) ^ B(a3) ^ B(a4) ^ B(a5) ^ B(a6) ^ B(a7) ^ B(a8))
+static u8 calculate_parity(u16 d)
+{
+	u8 p = 0;
+
+	if (d == 0 || d == 0xFFFF)
+		return 0;	/* optimization :) */
+
+	p |= BSEQ(15, 12, 11,  8,  5,  4,  3,  2) << 0;
+	p |= BSEQ(13, 12, 11, 10,  9,  7,  3,  1) << 1;
+	p |= BSEQ(15, 14, 13, 11, 10,  9,  6,  5) << 2;
+	p |= BSEQ(15, 14, 13,  8,  7,  6,  4,  0) << 3;
+	p |= BSEQ(12,  9,  8,  7,  6,  2,  1,  0) << 4;
+	p |= BSEQ(14, 10,  5,  4,  3,  2,  1,  0) << 5;
+	return p;
+}
+
+static inline int even_number_of_1s(u8 byte)
+{
+	int even = 1;
+
+	while (byte > 0) {
+		even ^= (byte & 0x1);
+		byte >>= 1;
+	}
+	return even;
+}
+
+static int lookup_single_error(u8 syndrome)
+{
+	int i;
+	u8 syndrome_table[] = {
+		0x38, 0x32, 0x31, 0x23, 0x29, 0x25, 0x1C, 0x1A,
+		0x19, 0x16, 0x26, 0x07, 0x13, 0x0E, 0x2C, 0x0D,
+		0x01, 0x02, 0x04, 0x08, 0x10, 0x20,
+	};
+
+	for (i = 0; i < ARRAY_SIZE(syndrome_table); i++)
+		if (syndrome_table[i] == syndrome)
+			return i;
+	return -ENOENT;
+}
+
+int gpmi_verify_hamming_22_16(void *data, u8 *parity, size_t size)
+{
+	int i, j;
+	int bit_index, bit_to_flip;
+	u16 *pdata = data;
+	u8 p = 0, np, syndrome;
+	int errors = 0;
+
+	for (bit_index = i = j = 0;
+	     i < size / sizeof(u16);
+	     i ++, pdata++) {
+
+		switch (bit_index) {
+
+		case 0:
+			p = parity[j] & 0x3F;
+			break;
+		case 2:
+			p = (parity[j++] & 0xC0) >> 6;
+			p |= (parity[j] & 0x0F) << 2;
+			break;
+		case 4:
+			p = (parity[j++] & 0xF0) >> 4;
+			p |= (parity[j] & 0x03) << 4;
+			break;
+		case 6:
+			p = (parity[j++] & 0xFC) >> 2;
+			break;
+		default:
+			BUG();	/* how did you get this ?! */
+			break;
+		}
+		bit_index = (bit_index + 2) % 8;
+
+		np = calculate_parity(*pdata);
+		syndrome = np ^ p;
+		if (syndrome == 0) /* cool */ {
+			continue;
+		}
+
+		if (even_number_of_1s(syndrome))
+			return -i;	/* can't recover */
+
+		bit_to_flip = lookup_single_error(syndrome);
+		if (bit_to_flip < 0)
+			return -i;	/* can't fix the error */
+
+		if (bit_to_flip < 16) {
+			*pdata ^= (1 << bit_to_flip);
+			errors++;
+		}
+	}
+	return errors;
+}
+
+void gpmi_encode_hamming_22_16(void *source_block, size_t src_size,
+			void *source_ecc, size_t ecc_size)
+{
+	int i, j, bit_index;
+	u16 *src = source_block;
+	u8 *ecc = source_ecc;
+	u8   np;
+
+	for (bit_index = j = i = 0;
+	     j < src_size/sizeof(u16) && i < ecc_size;
+	     j++) {
+
+		np = calculate_parity(src[j]);
+
+		switch (bit_index) {
+
+		case 0:
+			ecc[i] = np & 0x3F;
+			break;
+		case 2:
+			ecc[i++] |= (np & 0x03) << 6;
+			ecc[i]    = (np & 0x3C) >> 2;
+			break;
+		case 4:
+			ecc[i++] |= (np & 0x0F) << 4;
+			ecc[i]    = (np & 0x30) >> 4;
+			break;
+		case 6:
+			ecc[i++] |= (np & 0x3F) << 2;
+			break;
+		}
+		bit_index = (bit_index + 2) % 8;
+	}
+
+}
+
+void gpmi_encode_hamming_ncb_22_16(void *source_block, size_t source_size,
+		void *target_block, size_t target_size)
+{
+	u8  *dst = target_block;
+	u8   ecc[NAND_HC_ECC_SIZEOF_PARITY_BLOCK_IN_BYTES];
+
+	gpmi_encode_hamming_22_16(source_block,
+			NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES,
+			ecc,
+			NAND_HC_ECC_SIZEOF_PARITY_BLOCK_IN_BYTES);
+	/* create THREE copies of source block 	*/
+	memcpy(dst + NAND_HC_ECC_OFFSET_FIRST_DATA_COPY,
+		source_block, NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES);
+	memcpy(dst + NAND_HC_ECC_OFFSET_SECOND_DATA_COPY,
+		source_block, NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES);
+	memcpy(dst + NAND_HC_ECC_OFFSET_THIRD_DATA_COPY,
+		source_block, NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES);
+	/* ..and three copies of ECC block 	*/
+	memcpy(dst + NAND_HC_ECC_OFFSET_FIRST_PARITY_COPY,
+		ecc, NAND_HC_ECC_SIZEOF_PARITY_BLOCK_IN_BYTES);
+	memcpy(dst + NAND_HC_ECC_OFFSET_SECOND_PARITY_COPY,
+		ecc, NAND_HC_ECC_SIZEOF_PARITY_BLOCK_IN_BYTES);
+	memcpy(dst + NAND_HC_ECC_OFFSET_THIRD_PARITY_COPY,
+		ecc, NAND_HC_ECC_SIZEOF_PARITY_BLOCK_IN_BYTES);
+}
+
+unsigned gpmi_hamming_ecc_size_22_16(int block_size)
+{
+	return (((block_size * 8) / 16) * 6) / 8;
+}
+
diff --git a/drivers/mtd/nand/gpmi/gpmi-hamming-22-16.h b/drivers/mtd/nand/gpmi/gpmi-hamming-22-16.h
new file mode 100644
index 000000000000..6959bf3c599e
--- /dev/null
+++ b/drivers/mtd/nand/gpmi/gpmi-hamming-22-16.h
@@ -0,0 +1,43 @@
+/*
+ * Freescale STMP37XX/STMP378X GPMI (General-Purpose-Media-Interface)
+ *
+ * NCB software ECC Hamming code
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __LINUX_GPMI_H
+#define __LINUX_GPMI_H
+
+#define NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES	(512)
+#define NAND_HC_ECC_SIZEOF_PARITY_BLOCK_IN_BYTES	\
+	((((NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES*8)/16)*6)/8)
+#define NAND_HC_ECC_OFFSET_FIRST_DATA_COPY	    (0)
+#define NAND_HC_ECC_OFFSET_SECOND_DATA_COPY	   	\
+	(NAND_HC_ECC_OFFSET_FIRST_DATA_COPY + 		\
+	 NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES)
+#define NAND_HC_ECC_OFFSET_THIRD_DATA_COPY		\
+	(NAND_HC_ECC_OFFSET_SECOND_DATA_COPY + 		\
+	 NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES)
+#define NAND_HC_ECC_OFFSET_FIRST_PARITY_COPY		\
+	(NAND_HC_ECC_OFFSET_THIRD_DATA_COPY + 		\
+	 NAND_HC_ECC_SIZEOF_DATA_BLOCK_IN_BYTES)
+#define NAND_HC_ECC_OFFSET_SECOND_PARITY_COPY		\
+	(NAND_HC_ECC_OFFSET_FIRST_PARITY_COPY + 	\
+	 NAND_HC_ECC_SIZEOF_PARITY_BLOCK_IN_BYTES)
+#define NAND_HC_ECC_OFFSET_THIRD_PARITY_COPY		\
+	(NAND_HC_ECC_OFFSET_SECOND_PARITY_COPY + 	\
+	 NAND_HC_ECC_SIZEOF_PARITY_BLOCK_IN_BYTES)
+
+#endif
diff --git a/drivers/mtd/nand/gpmi/gpmi.h b/drivers/mtd/nand/gpmi/gpmi.h
new file mode 100644
index 000000000000..99c26988f370
--- /dev/null
+++ b/drivers/mtd/nand/gpmi/gpmi.h
@@ -0,0 +1,291 @@
+/*
+ * Freescale STMP37XX/STMP378X GPMI (General-Purpose-Media-Interface)
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __DRIVERS_GPMI_H
+#define __DRIVERS_GPMI_H
+
+#include <linux/mtd/partitions.h>
+#include <mach/gpmi.h>
+#include <mach/regs-gpmi.h>
+#include <mach/regs-apbh.h>
+#ifdef CONFIG_MTD_NAND_GPMI_BCH
+#include <mach/regs-bch.h>
+#endif
+#include <mach/regs-ecc8.h>
+
+#include "gpmi-hamming-22-16.h"
+
+#define GPMI_ECC4_WR \
+	(BM_GPMI_ECCCTRL_ENABLE_ECC | \
+	BF_GPMI_ECCCTRL_ECC_CMD(BV_GPMI_ECCCTRL_ECC_CMD__ENCODE_4_BIT))
+#define GPMI_ECC4_RD \
+	(BM_GPMI_ECCCTRL_ENABLE_ECC | \
+	BF_GPMI_ECCCTRL_ECC_CMD(BV_GPMI_ECCCTRL_ECC_CMD__DECODE_4_BIT))
+#define GPMI_ECC8_WR \
+	(BM_GPMI_ECCCTRL_ENABLE_ECC | \
+	BF_GPMI_ECCCTRL_ECC_CMD(BV_GPMI_ECCCTRL_ECC_CMD__ENCODE_8_BIT))
+#define GPMI_ECC8_RD \
+	(BM_GPMI_ECCCTRL_ENABLE_ECC | \
+	BF_GPMI_ECCCTRL_ECC_CMD(BV_GPMI_ECCCTRL_ECC_CMD__DECODE_8_BIT))
+
+/* fingerprints of BCB that can be found on STMP-formatted flash */
+#define SIG1		"STMP"
+#define SIG_NCB		"NCB "
+#define SIG_LDLB	"LDLB"
+#define SIG_DBBT	"DBBT"
+#define SIG_SIZE	4
+
+struct gpmi_nand_timing {
+	u8 data_setup;
+	u8 data_hold;
+	u8 address_setup;
+	u8 dsample_time;
+};
+
+struct gpmi_bcb_info {
+	struct gpmi_nand_timing timing;
+	loff_t ncbblock;
+	const void *pre_ncb;
+	size_t pre_ncb_size;
+};
+
+struct gpmi_ncb;
+
+int gpmi_erase(struct mtd_info *mtd, struct erase_info *instr);
+int gpmi_block_markbad(struct mtd_info *mtd, loff_t ofs);
+int gpmi_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip);
+int gpmi_scan_bbt(struct mtd_info *mtd);
+int gpmi_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip);
+#ifdef CONFIG_MTD_NAND_GPMI_SYSFS_ENTRIES
+int gpmi_sysfs(struct platform_device *p, int create);
+#endif
+int gpmi_ecc_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
+			int page, int sndcmd);
+void gpmi_set_timings(struct platform_device *pdev,
+			struct gpmi_nand_timing *tm);
+int gpmi_write_ncb(struct mtd_info *mtd, struct gpmi_bcb_info *b);
+
+unsigned gpmi_hamming_ecc_size_22_16(int block_size);
+void gpmi_encode_hamming_ncb_22_16(void *source_block, size_t source_size,
+		void *target_block, size_t target_size);
+void gpmi_encode_hamming_22_16(void *source_block, size_t src_size,
+		void *source_ecc, size_t ecc_size);
+int gpmi_verify_hamming_22_16(void *data, u8 *parity, size_t size);
+
+unsigned gpmi_hamming_ecc_size_13_8(int block_size);
+void gpmi_encode_hamming_ncb_13_8(void *source_block, size_t source_size,
+		void *target_block, size_t target_size);
+void gpmi_encode_hamming_13_8(void *source_block, size_t src_size,
+		void *source_ecc, size_t ecc_size);
+int gpmi_verify_hamming_13_8(void *data, u8 *parity, size_t size);
+
+#define GPMI_DMA_MAX_CHAIN	20	/* max DMA commands in chain */
+
+/*
+ * Sizes of data buffers to exchange commands/data with NAND chip
+ * Default values cover 4K NAND page (4096 data bytes + 218 bytes OOB)
+ */
+#define GPMI_CMD_BUF_SZ		10
+#define GPMI_DATA_BUF_SZ	NAND_MAX_PAGESIZE
+#define GPMI_WRITE_BUF_SZ	NAND_MAX_PAGESIZE
+#define GPMI_OOB_BUF_SZ		NAND_MAX_OOBSIZE
+
+#define GPMI_MAX_CHIPS		10
+
+struct gpmi_hwecc_chip {
+	char name[40];
+	struct list_head list;
+	int (*setup)(void *ctx, int index, int writesize, int oobsize);
+	int (*reset)(void *ctx, int index);
+	int (*read)(void *ctx, int index,
+			struct stmp3xxx_dma_descriptor *chain,
+			dma_addr_t error,
+			dma_addr_t page, dma_addr_t oob);
+	int (*write)(void *ctx, int index,
+			struct stmp3xxx_dma_descriptor *chain,
+			dma_addr_t error,
+			dma_addr_t page, dma_addr_t oob);
+	int (*stat)(void *ctx, int index, struct mtd_ecc_stats *r);
+};
+
+/* HWECC chips */
+struct gpmi_hwecc_chip *gpmi_hwecc_chip_find(char *name);
+void gpmi_hwecc_chip_add(struct gpmi_hwecc_chip *chip);
+void gpmi_hwecc_chip_remove(struct gpmi_hwecc_chip *chip);
+int bch_init(void);
+int ecc8_init(void);
+void bch_exit(void);
+void ecc8_exit(void);
+
+
+struct gpmi_nand_data {
+	void __iomem *io_base;
+	struct clk *clk;
+	int irq;
+	struct timer_list timer;
+	int self_suspended;
+	int use_count;
+	struct regulator *regulator;
+	int reg_uA;
+
+	int ignorebad;
+	void *bbt;
+
+	struct nand_chip	chip;
+	struct mtd_info		mtd;
+	struct platform_device  *dev;
+
+#ifdef CONFIG_MTD_PARTITIONS
+	int			nr_parts;
+	struct mtd_partition
+				*parts;
+#endif
+
+	struct completion	done;
+
+	u8			*cmd_buffer;
+	dma_addr_t		cmd_buffer_handle;
+	int			cmd_buffer_size, cmd_buffer_sz;
+
+	u8			*write_buffer;
+	dma_addr_t		write_buffer_handle;
+	int			write_buffer_size;
+	u8			*read_buffer;	/* point in write_buffer */
+	dma_addr_t		read_buffer_handle;
+
+	u8			*data_buffer;
+	dma_addr_t		data_buffer_handle;
+	int			data_buffer_size;
+
+	u8			*oob_buffer;
+	dma_addr_t		oob_buffer_handle;
+	int			oob_buffer_size;
+
+	void			*verify_buffer;
+
+	struct nchip {
+		unsigned dma_ch;
+		struct stmp37xx_circ_dma_chain chain;
+		struct stmp3xxx_dma_descriptor d[GPMI_DMA_MAX_CHAIN];
+		struct stmp3xxx_dma_descriptor error;
+		int cs;
+	} chips[GPMI_MAX_CHIPS];
+	struct nchip *cchip;
+	int selected_chip;
+
+	unsigned		hwecc_type_read, hwecc_type_write;
+	int			hwecc;
+
+	int			ecc_oob_bytes, oob_free;
+
+	int			transcribe_bbmark;
+	struct gpmi_nand_timing timing;
+
+	void (*saved_command)(struct mtd_info *mtd, unsigned int command,
+			int column, int page_addr);
+
+	int raw_oob_mode;
+	int (*saved_read_oob)(struct mtd_info *mtd, loff_t from,
+			 struct mtd_oob_ops *ops);
+	int (*saved_write_oob)(struct mtd_info *mtd, loff_t to,
+			  struct mtd_oob_ops *ops);
+
+	struct gpmi_hwecc_chip *hc;
+
+	int numchips;
+	int custom_partitions;
+	struct mtd_info *masters[GPMI_MAX_CHIPS];
+	struct mtd_partition chip_partitions[GPMI_MAX_CHIPS];
+	int n_concat;
+	struct mtd_info *concat[GPMI_MAX_CHIPS];
+	struct mtd_info *concat_mtd;
+};
+
+extern struct gpmi_nand_timing gpmi_safe_timing;
+
+struct gpmi_ncb {
+	u32			fingerprint1;
+	struct gpmi_nand_timing timing;
+	u32			pagesize;
+	u32			page_plus_oob_size;
+	u32			sectors_per_block;
+	u32			sector_in_page_mask;
+	u32			sector_to_page_shift;
+	u32			num_nands;
+	u32			reserved[3];
+	u32			fingerprint2;	  /* offset 0x2C     */
+};
+
+struct gpmi_ldlb {
+	u32			fingerprint1;
+	u16			major, minor, sub, reserved;
+	u32			nand_bitmap;
+	u32			reserved1[7];
+	u32			fingerprint2;
+	struct {
+		u32			fw_starting_nand;
+		u32			fw_starting_sector;
+		u32			fw_sector_stride;
+		u32			fw_sectors_total;
+	} fw[2];
+	u16			fw_major, fw_minor, fw_sub, fw_reserved;
+	u32			bbt_blk;
+	u32			bbt_blk_backup;
+};
+
+static inline void gpmi_block_mark_as(struct nand_chip *chip,
+					int block, int mark)
+{
+	u32 o;
+	int shift = (block & 0x03) << 1,
+	    index = block >> 2;
+
+	if (chip->bbt) {
+		mark &= 0x03;
+
+		o = chip->bbt[index];
+		o &= ~(0x03 << shift);
+		o |= (mark << shift);
+		chip->bbt[index] = o;
+	}
+}
+
+static inline int gpmi_block_badness(struct nand_chip *chip,
+					int block)
+{
+	u32 o;
+	int shift = (block & 0x03) << 1,
+	    index = block >> 2;
+
+	if (chip->bbt) {
+		o = (chip->bbt[index] >> shift) & 0x03;
+		pr_debug("%s: block = %d, o = %d\n", __func__, block, o);
+		return o;
+	}
+	return -1;
+}
+
+#ifdef CONFIG_STMP3XXX_UNIQUE_ID
+int __init gpmi_uid_init(const char *name, struct mtd_info *mtd,
+		 u_int32_t start, u_int32_t size);
+void gpmi_uid_remove(const char *name);
+#else
+#define gpmi_uid_init(name, mtd, start, size)
+#define gpmi_uid_remove(name)
+#endif
+
+#endif
diff --git a/drivers/mtd/nand/imx_nfc.c b/drivers/mtd/nand/imx_nfc.c
new file mode 100644
index 000000000000..65f72b4e468a
--- /dev/null
+++ b/drivers/mtd/nand/imx_nfc.c
@@ -0,0 +1,8286 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/mtd/partitions.h>
+#include <linux/io.h>
+
+#define DRIVER_VERSION "1.0"
+
+/* Define this macro to enable event reporting. */
+
+#define EVENT_REPORTING
+
+/*
+ * For detailed information that will be helpful in understanding this driver,
+ * see:
+ *
+ *     Documentation/imx_nfc.txt
+ */
+
+/*
+ * Macros that describe NFC hardware have names of the form:
+ *
+ *     NFC_*
+ *
+ * Macros that apply only to specific versions of the NFC have names of the
+ * following form:
+ *
+ *     NFC_<M>_<N>_*
+ *
+ * where:
+ *
+ *     <M> is the major version of the NFC hardware.
+ *     <N> is the minor version of the NFC hardware.
+ *
+ * The minor version can be 'X', which means that the macro applies to *all*
+ * NFCs of the same major version.
+ *
+ * For NFC versions with only one set of registers, macros that give offsets
+ * against the base address have names of the form:
+ *
+ *     *<RegisterName>_REG_OFF
+ *
+ * Macros that give the position of a field's LSB within a given register have
+ * names of the form:
+ *
+ *     *<RegisterName>_<FieldName>_POS
+ *
+ * Macros that mask a field within a given register have names of the form:
+ *
+ *     *<RegisterName>_<FieldName>_MSK
+ */
+
+/*
+ * Macro definitions for ALL NFC versions.
+ */
+
+#define NFC_MAIN_BUF_SIZE  (512)
+
+/*
+ * Macro definitions for version 1.0 NFCs.
+ */
+
+#define NFC_1_0_BUF_SIZE_REG_OFF             (0x00)
+#define NFC_1_0_BUF_ADDR_REG_OFF             (0x04)
+#define NFC_1_0_FLASH_ADDR_REG_OFF           (0x06)
+#define NFC_1_0_FLASH_CMD_REG_OFF            (0x08)
+#define NFC_1_0_CONFIG_REG_OFF               (0x0A)
+#define NFC_1_0_ECC_STATUS_RESULT_REG_OFF    (0x0C)
+#define NFC_1_0_RSLTMAIN_AREA_REG_OFF        (0x0E)
+#define NFC_1_0_RSLTSPARE_AREA_REG_OFF       (0x10)
+#define NFC_1_0_WRPROT_REG_OFF               (0x12)
+#define NFC_1_0_UNLOCKSTART_BLKADDR_REG_OFF  (0x14)
+#define NFC_1_0_UNLOCKEND_BLKADDR_REG_OFF    (0x16)
+#define NFC_1_0_NF_WRPRST_REG_OFF            (0x18)
+
+#define NFC_1_0_CONFIG1_REG_OFF              (0x1A)
+#define NFC_1_0_CONFIG1_NF_CE_POS            (7)
+#define NFC_1_0_CONFIG1_NF_CE_MSK            (0x1 << 7)
+
+#define NFC_1_0_CONFIG2_REG_OFF              (0x1C)
+
+/*
+* Macro definitions for version 2.X NFCs.
+*/
+
+#define NFC_2_X_FLASH_ADDR_REG_OFF   (0x06)
+#define NFC_2_X_FLASH_CMD_REG_OFF    (0x08)
+#define NFC_2_X_CONFIG_REG_OFF       (0x0A)
+
+#define NFC_2_X_WR_PROT_REG_OFF      (0x12)
+
+#define NFC_2_X_NF_WR_PR_ST_REG_OFF  (0x18)
+
+#define NFC_2_X_CONFIG2_REG_OFF      (0x1C)
+#define NFC_2_X_CONFIG2_FCMD_POS     (0)
+#define NFC_2_X_CONFIG2_FCMD_MSK     (0x1 << 0)
+#define NFC_2_X_CONFIG2_FADD_POS     (1)
+#define NFC_2_X_CONFIG2_FADD_MSK     (0x1 << 1)
+#define NFC_2_X_CONFIG2_FDI_POS      (2)
+#define NFC_2_X_CONFIG2_FDI_MSK      (0x1 << 2)
+#define NFC_2_X_CONFIG2_FDO_POS      (3)
+#define NFC_2_X_CONFIG2_FDO_MSK      (0x7 << 3)
+#define NFC_2_X_CONFIG2_INT_POS      (15)
+#define NFC_2_X_CONFIG2_INT_MSK      (0x1 << 15)
+
+/*
+* Macro definitions for version 2.0 NFCs.
+*/
+
+#define NFC_2_0_BUF_ADDR_REG_OFF       (0x04)
+#define NFC_2_0_BUF_ADDR_RBA_POS       (0)
+#define NFC_2_0_BUF_ADDR_RBA_MSK       (0xf << 0)
+
+#define NFC_2_0_ECC_STATUS_REG_OFF     (0x0C)
+#define NFC_2_0_ECC_STATUS_NOSER1_POS  (0)
+#define NFC_2_0_ECC_STATUS_NOSER1_MSK  (0xF << 0)
+#define NFC_2_0_ECC_STATUS_NOSER2_POS  (4)
+#define NFC_2_0_ECC_STATUS_NOSER2_MSK  (0xF << 4)
+#define NFC_2_0_ECC_STATUS_NOSER3_POS  (8)
+#define NFC_2_0_ECC_STATUS_NOSER3_MSK  (0xF << 8)
+#define NFC_2_0_ECC_STATUS_NOSER4_POS  (12)
+#define NFC_2_0_ECC_STATUS_NOSER4_MSK  (0xF << 12)
+
+#define NFC_2_0_CONFIG1_REG_OFF        (0x1A)
+#define NFC_2_0_CONFIG1_SP_EN_POS      (2)
+#define NFC_2_0_CONFIG1_SP_EN_MSK      (0x1 << 2)
+#define NFC_2_0_CONFIG1_ECC_EN_POS     (3)
+#define NFC_2_0_CONFIG1_ECC_EN_MSK     (0x1 << 3)
+#define NFC_2_0_CONFIG1_INT_MSK_POS    (4)
+#define NFC_2_0_CONFIG1_INT_MSK_MSK    (0x1 << 4)
+#define NFC_2_0_CONFIG1_NF_BIG_POS     (5)
+#define NFC_2_0_CONFIG1_NF_BIG_MSK     (0x1 << 5)
+#define NFC_2_0_CONFIG1_NFC_RST_POS    (6)
+#define NFC_2_0_CONFIG1_NFC_RST_MSK    (0x1 << 6)
+#define NFC_2_0_CONFIG1_NF_CE_POS      (7)
+#define NFC_2_0_CONFIG1_NF_CE_MSK      (0x1 << 7)
+#define NFC_2_0_CONFIG1_ONE_CYLE_POS   (8)
+#define NFC_2_0_CONFIG1_ONE_CYLE_MSK   (0x1 << 8)
+#define NFC_2_0_CONFIG1_MLC_POS        (9)
+#define NFC_2_0_CONFIG1_MLC_MSK        (0x1 << 9)
+
+#define NFC_2_0_UNLOCK_START_REG_OFF   (0x14)
+#define NFC_2_0_UNLOCK_END_REG_OFF     (0x16)
+
+/*
+* Macro definitions for version 2.1 NFCs.
+*/
+
+#define NFC_2_1_BUF_ADDR_REG_OFF        (0x04)
+#define NFC_2_1_BUF_ADDR_RBA_POS        (0)
+#define NFC_2_1_BUF_ADDR_RBA_MSK        (0x7 << 0)
+#define NFC_2_1_BUF_ADDR_CS_POS         (4)
+#define NFC_2_1_BUF_ADDR_CS_MSK         (0x3 << 4)
+
+#define NFC_2_1_ECC_STATUS_REG_OFF      (0x0C)
+#define NFC_2_1_ECC_STATUS_NOSER1_POS   (0)
+#define NFC_2_1_ECC_STATUS_NOSER1_MSK   (0xF << 0)
+#define NFC_2_1_ECC_STATUS_NOSER2_POS   (4)
+#define NFC_2_1_ECC_STATUS_NOSER2_MSK   (0xF << 4)
+#define NFC_2_1_ECC_STATUS_NOSER3_POS   (8)
+#define NFC_2_1_ECC_STATUS_NOSER3_MSK   (0xF << 8)
+#define NFC_2_1_ECC_STATUS_NOSER4_POS   (12)
+#define NFC_2_1_ECC_STATUS_NOSER4_MSK   (0xF << 12)
+
+#define NFC_2_1_CONFIG1_REG_OFF         (0x1A)
+#define NFC_2_1_CONFIG1_ECC_MODE_POS    (0)
+#define NFC_2_1_CONFIG1_ECC_MODE_MSK    (0x1 << 0)
+#define NFC_2_1_CONFIG1_DMA_MODE_POS    (1)
+#define NFC_2_1_CONFIG1_DMA_MODE_MSK    (0x1 << 1)
+#define NFC_2_1_CONFIG1_SP_EN_POS       (2)
+#define NFC_2_1_CONFIG1_SP_EN_MSK       (0x1 << 2)
+#define NFC_2_1_CONFIG1_ECC_EN_POS      (3)
+#define NFC_2_1_CONFIG1_ECC_EN_MSK      (0x1 << 3)
+#define NFC_2_1_CONFIG1_INT_MSK_POS     (4)
+#define NFC_2_1_CONFIG1_INT_MSK_MSK     (0x1 << 4)
+#define NFC_2_1_CONFIG1_NF_BIG_POS      (5)
+#define NFC_2_1_CONFIG1_NF_BIG_MSK      (0x1 << 5)
+#define NFC_2_1_CONFIG1_NFC_RST_POS     (6)
+#define NFC_2_1_CONFIG1_NFC_RST_MSK     (0x1 << 6)
+#define NFC_2_1_CONFIG1_NF_CE_POS       (7)
+#define NFC_2_1_CONFIG1_NF_CE_MSK       (0x1 << 7)
+#define NFC_2_1_CONFIG1_SYM_POS         (8)
+#define NFC_2_1_CONFIG1_SYM_MSK         (0x1 << 8)
+#define NFC_2_1_CONFIG1_PPB_POS         (9)
+#define NFC_2_1_CONFIG1_PPB_MSK         (0x3 << 9)
+#define NFC_2_1_CONFIG1_FP_INT_POS      (11)
+#define NFC_2_1_CONFIG1_FP_INT_MSK      (0x1 << 11)
+
+#define NFC_2_1_UNLOCK_START_0_REG_OFF  (0x20)
+#define NFC_2_1_UNLOCK_END_0_REG_OFF    (0x22)
+#define NFC_2_1_UNLOCK_START_1_REG_OFF  (0x24)
+#define NFC_2_1_UNLOCK_END_1_REG_OFF    (0x26)
+#define NFC_2_1_UNLOCK_START_2_REG_OFF  (0x28)
+#define NFC_2_1_UNLOCK_END_2_REG_OFF    (0x2A)
+#define NFC_2_1_UNLOCK_START_3_REG_OFF  (0x2C)
+#define NFC_2_1_UNLOCK_END_3_REG_OFF    (0x2E)
+
+/*
+* Macro definitions for version 3.X NFCs.
+*/
+
+/*
+* Macro definitions for version 3.1 NFCs.
+*/
+
+#define NFC_3_1_FLASH_ADDR_CMD_REG_OFF          (0x00)
+#define NFC_3_1_CONFIG1_REG_OFF                 (0x04)
+#define NFC_3_1_ECC_STATUS_RESULT_REG_OFF       (0x08)
+#define NFC_3_1_LAUNCH_NFC_REG_OFF              (0x0C)
+
+#define NFC_3_1_WRPROT_REG_OFF                  (0x00)
+#define NFC_3_1_WRPROT_UNLOCK_BLK_ADD0_REG_OFF  (0x04)
+#define NFC_3_1_CONFIG2_REG_OFF                 (0x14)
+#define NFC_3_1_IPC_REG_OFF                     (0x18)
+
+/*
+* Macro definitions for version 3.2 NFCs.
+*/
+
+#define NFC_3_2_CMD_REG_OFF                (0x00)
+
+#define NFC_3_2_ADD0_REG_OFF               (0x04)
+#define NFC_3_2_ADD1_REG_OFF               (0x08)
+#define NFC_3_2_ADD2_REG_OFF               (0x0C)
+#define NFC_3_2_ADD3_REG_OFF               (0x10)
+#define NFC_3_2_ADD4_REG_OFF               (0x14)
+#define NFC_3_2_ADD5_REG_OFF               (0x18)
+#define NFC_3_2_ADD6_REG_OFF               (0x1C)
+#define NFC_3_2_ADD7_REG_OFF               (0x20)
+#define NFC_3_2_ADD8_REG_OFF               (0x24)
+#define NFC_3_2_ADD9_REG_OFF               (0x28)
+#define NFC_3_2_ADD10_REG_OFF              (0x2C)
+#define NFC_3_2_ADD11_REG_OFF              (0x30)
+
+#define NFC_3_2_CONFIG1_REG_OFF            (0x34)
+#define NFC_3_2_CONFIG1_SP_EN_POS          (0)
+#define NFC_3_2_CONFIG1_SP_EN_MSK          (0x1 << 0)
+#define NFC_3_2_CONFIG1_NF_CE_POS          (1)
+#define NFC_3_2_CONFIG1_NF_CE_MSK          (0x1 << 1)
+#define NFC_3_2_CONFIG1_RST_POS            (2)
+#define NFC_3_2_CONFIG1_RST_MSK            (0x1 << 2)
+#define NFC_3_2_CONFIG1_RBA_POS            (4)
+#define NFC_3_2_CONFIG1_RBA_MSK            (0x7 << 4)
+#define NFC_3_2_CONFIG1_ITER_POS           (8)
+#define NFC_3_2_CONFIG1_ITER_MSK           (0xf << 8)
+#define NFC_3_2_CONFIG1_CS_POS             (12)
+#define NFC_3_2_CONFIG1_CS_MSK             (0x7 << 12)
+#define NFC_3_2_CONFIG1_STATUS_POS         (16)
+#define NFC_3_2_CONFIG1_STATUS_MSK         (0xFFFF<<16)
+
+#define NFC_3_2_ECC_STATUS_REG_OFF         (0x38)
+#define NFC_3_2_ECC_STATUS_NOBER1_POS      (0)
+#define NFC_3_2_ECC_STATUS_NOBER1_MSK      (0xF << 0)
+#define NFC_3_2_ECC_STATUS_NOBER2_POS      (4)
+#define NFC_3_2_ECC_STATUS_NOBER2_MSK      (0xF << 4)
+#define NFC_3_2_ECC_STATUS_NOBER3_POS      (8)
+#define NFC_3_2_ECC_STATUS_NOBER3_MSK      (0xF << 8)
+#define NFC_3_2_ECC_STATUS_NOBER4_POS      (12)
+#define NFC_3_2_ECC_STATUS_NOBER4_MSK      (0xF << 12)
+#define NFC_3_2_ECC_STATUS_NOBER5_POS      (16)
+#define NFC_3_2_ECC_STATUS_NOBER5_MSK      (0xF << 16)
+#define NFC_3_2_ECC_STATUS_NOBER6_POS      (20)
+#define NFC_3_2_ECC_STATUS_NOBER6_MSK      (0xF << 20)
+#define NFC_3_2_ECC_STATUS_NOBER7_POS      (24)
+#define NFC_3_2_ECC_STATUS_NOBER7_MSK      (0xF << 24)
+#define NFC_3_2_ECC_STATUS_NOBER8_POS      (28)
+#define NFC_3_2_ECC_STATUS_NOBER8_MSK      (0xF << 28)
+
+
+#define NFC_3_2_STATUS_SUM_REG_OFF         (0x3C)
+#define NFC_3_2_STATUS_SUM_NAND_SUM_POS    (0x0)
+#define NFC_3_2_STATUS_SUM_NAND_SUM_MSK    (0xFF << 0)
+#define NFC_3_2_STATUS_SUM_ECC_SUM_POS     (8)
+#define NFC_3_2_STATUS_SUM_ECC_SUM_MSK     (0xFF << 8)
+
+#define NFC_3_2_LAUNCH_REG_OFF             (0x40)
+#define NFC_3_2_LAUNCH_FCMD_POS            (0)
+#define NFC_3_2_LAUNCH_FCMD_MSK            (0x1 << 0)
+#define NFC_3_2_LAUNCH_FADD_POS            (1)
+#define NFC_3_2_LAUNCH_FADD_MSK            (0x1 << 1)
+#define NFC_3_2_LAUNCH_FDI_POS             (2)
+#define NFC_3_2_LAUNCH_FDI_MSK             (0x1 << 2)
+#define NFC_3_2_LAUNCH_FDO_POS             (3)
+#define NFC_3_2_LAUNCH_FDO_MSK             (0x7 << 3)
+#define NFC_3_2_LAUNCH_AUTO_PROG_POS       (6)
+#define NFC_3_2_LAUNCH_AUTO_PROG_MSK       (0x1 << 6)
+#define NFC_3_2_LAUNCH_AUTO_READ_POS       (7)
+#define NFC_3_2_LAUNCH_AUTO_READ_MSK       (0x1 << 7)
+#define NFC_3_2_LAUNCH_AUTO_ERASE_POS      (9)
+#define NFC_3_2_LAUNCH_AUTO_ERASE_MSK      (0x1 << 9)
+#define NFC_3_2_LAUNCH_COPY_BACK0_POS      (10)
+#define NFC_3_2_LAUNCH_COPY_BACK0_MSK      (0x1 << 10)
+#define NFC_3_2_LAUNCH_COPY_BACK1_POS      (11)
+#define NFC_3_2_LAUNCH_COPY_BACK1_MSK      (0x1 << 11)
+#define NFC_3_2_LAUNCH_AUTO_STATUS_POS     (12)
+#define NFC_3_2_LAUNCH_AUTO_STATUS_MSK     (0x1 << 12)
+
+#define NFC_3_2_WRPROT_REG_OFF             (0x00)
+#define NFC_3_2_WRPROT_WPC_POS             (0)
+#define NFC_3_2_WRPROT_WPC_MSK             (0x7 << 0)
+#define NFC_3_2_WRPROT_CS2L_POS            (3)
+#define NFC_3_2_WRPROT_CS2L_MSK            (0x7 << 3)
+#define NFC_3_2_WRPROT_BLS_POS             (6)
+#define NFC_3_2_WRPROT_BLS_MSK             (0x3 << 6)
+#define NFC_3_2_WRPROT_LTS0_POS            (8)
+#define NFC_3_2_WRPROT_LTS0_MSK            (0x1 << 8)
+#define NFC_3_2_WRPROT_LS0_POS             (9)
+#define NFC_3_2_WRPROT_LS0_MSK             (0x1 << 9)
+#define NFC_3_2_WRPROT_US0_POS             (10)
+#define NFC_3_2_WRPROT_US0_MSK             (0x1 << 10)
+#define NFC_3_2_WRPROT_LTS1_POS            (11)
+#define NFC_3_2_WRPROT_LTS1_MSK            (0x1 << 11)
+#define NFC_3_2_WRPROT_LS1_POS             (12)
+#define NFC_3_2_WRPROT_LS1_MSK             (0x1 << 12)
+#define NFC_3_2_WRPROT_US1_POS             (13)
+#define NFC_3_2_WRPROT_US1_MSK             (0x1 << 13)
+#define NFC_3_2_WRPROT_LTS2_POS            (14)
+#define NFC_3_2_WRPROT_LTS2_MSK            (0x1 << 14)
+#define NFC_3_2_WRPROT_LS2_POS             (15)
+#define NFC_3_2_WRPROT_LS2_MSK             (0x1 << 15)
+#define NFC_3_2_WRPROT_US2_POS             (16)
+#define NFC_3_2_WRPROT_US2_MSK             (0x1 << 16)
+#define NFC_3_2_WRPROT_LTS3_POS            (17)
+#define NFC_3_2_WRPROT_LTS3_MSK            (0x1 << 17)
+#define NFC_3_2_WRPROT_LS3_POS             (18)
+#define NFC_3_2_WRPROT_LS3_MSK             (0x1 << 18)
+#define NFC_3_2_WRPROT_US3_POS             (19)
+#define NFC_3_2_WRPROT_US3_MSK             (0x1 << 19)
+#define NFC_3_2_WRPROT_LTS4_POS            (20)
+#define NFC_3_2_WRPROT_LTS4_MSK            (0x1 << 20)
+#define NFC_3_2_WRPROT_LS4_POS             (21)
+#define NFC_3_2_WRPROT_LS4_MSK             (0x1 << 21)
+#define NFC_3_2_WRPROT_US4_POS             (22)
+#define NFC_3_2_WRPROT_US4_MSK             (0x1 << 22)
+#define NFC_3_2_WRPROT_LTS5_POS            (23)
+#define NFC_3_2_WRPROT_LTS5_MSK            (0x1 << 23)
+#define NFC_3_2_WRPROT_LS5_POS             (24)
+#define NFC_3_2_WRPROT_LS5_MSK             (0x1 << 24)
+#define NFC_3_2_WRPROT_US5_POS             (25)
+#define NFC_3_2_WRPROT_US5_MSK             (0x1 << 25)
+#define NFC_3_2_WRPROT_LTS6_POS            (26)
+#define NFC_3_2_WRPROT_LTS6_MSK            (0x1 << 26)
+#define NFC_3_2_WRPROT_LS6_POS             (27)
+#define NFC_3_2_WRPROT_LS6_MSK             (0x1 << 27)
+#define NFC_3_2_WRPROT_US6_POS             (28)
+#define NFC_3_2_WRPROT_US6_MSK             (0x1 << 28)
+#define NFC_3_2_WRPROT_LTS7_POS            (29)
+#define NFC_3_2_WRPROT_LTS7_MSK            (0x1 << 29)
+#define NFC_3_2_WRPROT_LS7_POS             (30)
+#define NFC_3_2_WRPROT_LS7_MSK             (0x1 << 30)
+#define NFC_3_2_WRPROT_US7_POS             (31)
+#define NFC_3_2_WRPROT_US7_MSK             (0x1 << 31)
+
+#define NFC_3_2_UNLOCK_BLK_ADD0_REG_OFF    (0x04)
+#define NFC_3_2_UNLOCK_BLK_ADD0_USBA0_POS  (0)
+#define NFC_3_2_UNLOCK_BLK_ADD0_USBA0_MSK  (0xFFFF << 0)
+#define NFC_3_2_UNLOCK_BLK_ADD0_UEBA0_POS  (16)
+#define NFC_3_2_UNLOCK_BLK_ADD0_UEBA0_MSK  (0xFFFF<<16)
+
+#define NFC_3_2_UNLOCK_BLK_ADD1_REG_OFF    (0x08)
+#define NFC_3_2_UNLOCK_BLK_ADD1_USBA1_POS  (0)
+#define NFC_3_2_UNLOCK_BLK_ADD1_USBA1_MSK  (0xFFFF << 0)
+#define NFC_3_2_UNLOCK_BLK_ADD1_UEBA1_POS  (16)
+#define NFC_3_2_UNLOCK_BLK_ADD1_UEBA1_MSK  (0xFFFF<<16)
+
+#define NFC_3_2_UNLOCK_BLK_ADD2_REG_OFF    (0x0C)
+#define NFC_3_2_UNLOCK_BLK_ADD2_USBA2_POS  (0)
+#define NFC_3_2_UNLOCK_BLK_ADD2_USBA2_MSK  (0xFFFF << 0)
+#define NFC_3_2_UNLOCK_BLK_ADD2_UEBA2_POS  (16)
+#define NFC_3_2_UNLOCK_BLK_ADD2_UEBA2_MSK  (0xFFFF<<16)
+
+#define NFC_3_2_UNLOCK_BLK_ADD3_REG_OFF    (0x10)
+#define NFC_3_2_UNLOCK_BLK_ADD3_USBA3_POS  (0)
+#define NFC_3_2_UNLOCK_BLK_ADD3_USBA3_MSK  (0xFFFF << 0)
+#define NFC_3_2_UNLOCK_BLK_ADD3_UEBA3_POS  (16)
+#define NFC_3_2_UNLOCK_BLK_ADD3_UEBA3_MSK  (0xFFFF<<16)
+
+#define NFC_3_2_UNLOCK_BLK_ADD4_REG_OFF    (0x14)
+#define NFC_3_2_UNLOCK_BLK_ADD4_USBA4_POS  (0)
+#define NFC_3_2_UNLOCK_BLK_ADD4_USBA4_MSK  (0xFFFF << 0)
+#define NFC_3_2_UNLOCK_BLK_ADD4_UEBA4_POS  (16)
+#define NFC_3_2_UNLOCK_BLK_ADD4_UEBA4_MSK  (0xFFFF<<16)
+
+#define NFC_3_2_UNLOCK_BLK_ADD5_REG_OFF    (0x18)
+#define NFC_3_2_UNLOCK_BLK_ADD5_USBA5_POS  (0)
+#define NFC_3_2_UNLOCK_BLK_ADD5_USBA5_MSK  (0xFFFF << 0)
+#define NFC_3_2_UNLOCK_BLK_ADD5_UEBA5_POS  (16)
+#define NFC_3_2_UNLOCK_BLK_ADD5_UEBA5_MSK  (0xFFFF<<16)
+
+#define NFC_3_2_UNLOCK_BLK_ADD6_REG_OFF    (0x1C)
+#define NFC_3_2_UNLOCK_BLK_ADD6_USBA6_POS  (0)
+#define NFC_3_2_UNLOCK_BLK_ADD6_USBA6_MSK  (0xFFFF << 0)
+#define NFC_3_2_UNLOCK_BLK_ADD6_UEBA6_POS  (16)
+#define NFC_3_2_UNLOCK_BLK_ADD6_UEBA6_MSK  (0xFFFF<<16)
+
+#define NFC_3_2_UNLOCK_BLK_ADD7_REG_OFF    (0x20)
+#define NFC_3_2_UNLOCK_BLK_ADD7_USBA7_POS  (0)
+#define NFC_3_2_UNLOCK_BLK_ADD7_USBA7_MSK  (0xFFFF << 0)
+#define NFC_3_2_UNLOCK_BLK_ADD7_UEBA7_POS  (16)
+#define NFC_3_2_UNLOCK_BLK_ADD7_UEBA7_MSK  (0xFFFF<<16)
+
+#define NFC_3_2_CONFIG2_REG_OFF            (0x24)
+#define NFC_3_2_CONFIG2_PS_POS             (0)
+#define NFC_3_2_CONFIG2_PS_MSK             (0x3 << 0)
+#define NFC_3_2_CONFIG2_SYM_POS            (2)
+#define NFC_3_2_CONFIG2_SYM_MSK            (0x1 << 2)
+#define NFC_3_2_CONFIG2_ECC_EN_POS         (3)
+#define NFC_3_2_CONFIG2_ECC_EN_MSK         (0x1 << 3)
+#define NFC_3_2_CONFIG2_CMD_PHASES_POS     (4)
+#define NFC_3_2_CONFIG2_CMD_PHASES_MSK     (0x1 << 4)
+#define NFC_3_2_CONFIG2_ADDR_PHASES0_POS   (5)
+#define NFC_3_2_CONFIG2_ADDR_PHASES0_MSK   (0x1 << 5)
+#define NFC_3_2_CONFIG2_ECC_MODE_POS       (6)
+#define NFC_3_2_CONFIG2_ECC_MODE_MSK       (0x1 << 6)
+#define NFC_3_2_CONFIG2_PPB_POS            (7)
+#define NFC_3_2_CONFIG2_PPB_MSK            (0x3 << 7)
+#define NFC_3_2_CONFIG2_EDC_POS            (9)
+#define NFC_3_2_CONFIG2_EDC_MSK            (0x7 << 9)
+#define NFC_3_2_CONFIG2_ADDR_PHASES1_POS   (12)
+#define NFC_3_2_CONFIG2_ADDR_PHASES1_MSK   (0x3 << 12)
+#define NFC_3_2_CONFIG2_AUTO_DONE_MSK_POS  (14)
+#define NFC_3_2_CONFIG2_AUTO_DONE_MSK_MSK  (0x1 << 14)
+#define NFC_3_2_CONFIG2_INT_MSK_POS        (15)
+#define NFC_3_2_CONFIG2_INT_MSK_MSK        (0x1 << 15)
+#define NFC_3_2_CONFIG2_SPAS_POS           (16)
+#define NFC_3_2_CONFIG2_SPAS_MSK           (0xFF << 16)
+#define NFC_3_2_CONFIG2_ST_CMD_POS         (24)
+#define NFC_3_2_CONFIG2_ST_CMD_MSK         (0xFF << 24)
+
+#define NFC_3_2_CONFIG3_REG_OFF            (0x28)
+#define NFC_3_2_CONFIG3_ADD_OP_POS         (0)
+#define NFC_3_2_CONFIG3_ADD_OP_MSK         (0x3 << 0)
+#define NFC_3_2_CONFIG3_TOO_POS            (2)
+#define NFC_3_2_CONFIG3_TOO_MSK            (0x1 << 2)
+#define NFC_3_2_CONFIG3_FW_POS             (3)
+#define NFC_3_2_CONFIG3_FW_MSK             (0x1 << 3)
+#define NFC_3_2_CONFIG3_SB2R_POS           (4)
+#define NFC_3_2_CONFIG3_SB2R_MSK           (0x7 << 4)
+#define NFC_3_2_CONFIG3_NF_BIG_POS         (7)
+#define NFC_3_2_CONFIG3_NF_BIG_MSK         (0x1 << 7)
+#define NFC_3_2_CONFIG3_SBB_POS            (8)
+#define NFC_3_2_CONFIG3_SBB_MSK            (0x7 << 8)
+#define NFC_3_2_CONFIG3_DMA_MODE_POS       (11)
+#define NFC_3_2_CONFIG3_DMA_MODE_MSK       (0x1 << 11)
+#define NFC_3_2_CONFIG3_NUM_OF_DEVICES_POS (12)
+#define NFC_3_2_CONFIG3_NUM_OF_DEVICES_MSK (0x7 << 12)
+#define NFC_3_2_CONFIG3_RBB_MODE_POS       (15)
+#define NFC_3_2_CONFIG3_RBB_MODE_MSK       (0x1 << 15)
+#define NFC_3_2_CONFIG3_FMP_POS            (16)
+#define NFC_3_2_CONFIG3_FMP_MSK            (0xF << 16)
+#define NFC_3_2_CONFIG3_NO_SDMA_POS        (20)
+#define NFC_3_2_CONFIG3_NO_SDMA_MSK        (0x1 << 20)
+
+#define NFC_3_2_IPC_REG_OFF                (0x2C)
+#define NFC_3_2_IPC_CREQ_POS               (0)
+#define NFC_3_2_IPC_CREQ_MSK               (0x1 << 0)
+#define NFC_3_2_IPC_CACK_POS               (1)
+#define NFC_3_2_IPC_CACK_MSK               (0x1 << 1)
+#define NFC_3_2_IPC_DMA_STATUS_POS         (26)
+#define NFC_3_2_IPC_DMA_STATUS_MSK         (0x3 << 26)
+#define NFC_3_2_IPC_RB_B_POS               (28)
+#define NFC_3_2_IPC_RB_B_MSK               (0x1 << 28)
+#define NFC_3_2_IPC_LPS_POS                (29)
+#define NFC_3_2_IPC_LPS_MSK                (0x1 << 29)
+#define NFC_3_2_IPC_AUTO_PROG_DONE_POS     (30)
+#define NFC_3_2_IPC_AUTO_PROG_DONE_MSK     (0x1 << 30)
+#define NFC_3_2_IPC_INT_POS                (31)
+#define NFC_3_2_IPC_INT_MSK                (0x1 << 31)
+
+#define NFC_3_2_AXI_ERR_ADD_REG_OFF        (0x30)
+
+/**
+ * enum override - Choices for overrides.
+ *
+ * Some functions of this driver can be overriden at run time. This is a
+ * convenient enumerated type for all such options.
+ */
+
+enum override {
+	NEVER         = -1,
+	DRIVER_CHOICE =  0,
+	ALWAYS        =  1,
+};
+
+/**
+ * struct physical_geometry - Physical geometry description.
+ *
+ * This structure describes the physical geometry of the medium.
+ *
+ * @chip_count:      The number of chips in the medium.
+ * @chip_size:       The size, in bytes, of a single chip (excluding the
+ *                   out-of-band bytes).
+ * @block_size:      The size, in bytes, of a single block (excluding the
+ *                   out-of-band bytes).
+ * @page_data_size:  The size, in bytes, of the data area in a page (excluding
+ *                   the out-of-band bytes).
+ * @page_oob_size:   The size, in bytes, of the out-of-band area in a page.
+ */
+
+struct physical_geometry {
+	unsigned int  chip_count;
+	uint64_t      chip_size;
+	unsigned int  block_size;
+	unsigned int  page_data_size;
+	unsigned int  page_oob_size;
+};
+
+/**
+ * struct nfc_geometry - NFC geometry description.
+ *
+ * This structure describes the NFC's view of the medium geometry, which may be
+ * different from the physical geometry (for example, we treat pages that are
+ * physically 2K+112 as if they are 2K+64).
+ *
+ * @page_data_size:  The size of the data area in a page (excluding the
+ *                   out-of-band bytes). This is almost certain to be the same
+ *                   as the physical data size.
+ * @page_oob_size:   The size of the out-of-band area in a page. This may be
+ *                   different from the physical OOB size.
+ * @ecc_algorithm:   The name of the ECC algorithm (e.g., "Reed-Solomon" or
+ *                   "BCH").
+ * @ecc_strength:    A number that describes the strength of the ECC algorithm.
+ *                   For example, various i.MX SoC's support ECC-1, ECC-4 or
+ *                   ECC-8 of the Reed-Solomon ECC algorithm.
+ * @buffer_count:    The number of main/spare buffers used with this geometry.
+ * @spare_buf_size:  The number of bytes held in each spare buffer.
+ * @spare_buf_spill: The number of extra bytes held in the last spare buffer.
+ * @mtd_layout:      The MTD layout that best matches the geometry described by
+ *                   the rest of this structure. The logical layer might not use
+ *                   this structure, especially when interleaving.
+ */
+
+struct nfc_geometry {
+	const unsigned int     page_data_size;
+	const unsigned int     page_oob_size;
+	const char             *ecc_algorithm;
+	const int              ecc_strength;
+	const unsigned int     buffer_count;
+	const unsigned int     spare_buf_size;
+	const unsigned int     spare_buf_spill;
+	struct nand_ecclayout  mtd_layout;
+};
+
+/**
+ * struct logical_geometry - Logical geometry description.
+ *
+ * This structure describes the logical geometry we expose to MTD. This geometry
+ * may be different from the physical or NFC geometries, especially when
+ * interleaving.
+ *
+ * @chip_count:      The number of chips in the medium.
+ * @chip_size:       The size, in bytes, of a single chip (excluding the
+ *                   out-of-band bytes).
+ * @usable_size:     The size, in bytes, of the medium that MTD can actually
+ *                   use. This may be less than the chip size multiplied by the
+ *                   number of chips.
+ * @block_size:      The size, in bytes, of a single block (excluding the
+ *                   out-of-band bytes).
+ * @page_data_size:  The size of the data area in a page (excluding the
+ *                   out-of-band bytes).
+ * @page_oob_size:   The size of the out-of-band area in a page.
+ */
+
+struct logical_geometry {
+	unsigned int           chip_count;
+	uint32_t               chip_size;
+	uint32_t               usable_size;
+	unsigned int           block_size;
+	unsigned int           page_data_size;
+	unsigned int           page_oob_size;
+	struct nand_ecclayout  *mtd_layout;
+};
+
+/**
+ * struct imx_nfc_data - i.MX NFC per-device data.
+ *
+ * Note that the "device" managed by this driver represents the NAND Flash
+ * controller *and* the NAND Flash medium behind it. Thus, the per-device data
+ * structure has information about the controller, the chips to which it is
+ * connected, and properties of the medium as a whole.
+ *
+ * @dev:                 A pointer to the owning struct device.
+ * @pdev:                A pointer to the owning struct platform_device.
+ * @pdata:               A pointer to the device's platform data.
+ * @buffers:             A pointer to the NFC buffers.
+ * @primary_regs:        A pointer to the NFC primary registers.
+ * @secondary_regs:      A pointer to the NFC secondary registers.
+ * @clock:               A pointer to the NFC struct clk.
+ * @interrupt:           The NFC interrupt number.
+ * @physical_geometry:   A description of the medium's physical geometry.
+ * @nfc:                 A pointer to the NFC HAL.
+ * @nfc_geometry:        A description of the medium geometry as viewed by the
+ *                       NFC.
+ * @done:                The struct completion we use to handle interrupts.
+ * @logical_geometry:    A description of the logical geometry exposed to MTD.
+ * @interrupt_override:  Can override how the driver uses interrupts when
+ *                       waiting for the NFC.
+ * @auto_op_override:    Can override whether the driver uses automatic NFC
+ *                       operations.
+ * @inject_ecc_error:    Indicates that the driver should inject an ECC error in
+ *                       the next read operation that uses ECC. User space
+ *                       programs can set this value through the sysfs node of
+ *                       the same name. If this value is less than zero, the
+ *                       driver will inject an uncorrectable ECC error. If this
+ *                       value is greater than zero, the driver will inject that
+ *                       number of correctable errors, capped at whatever
+ *                       possible maximum currently applies.
+ * @current_chip:        The chip currently selected by the NAND Fash MTD HIL.
+ *                       A negative value indicates that no chip is selected.
+ *                       We use this field to detect when the HIL begins and
+ *                       ends essential transactions. This helps us to know when
+ *                       we should turn the NFC clock on or off.
+ * @command:             The last command the HIL tried to send by calling
+ *                       cmdfunc(). Later functions use this information to
+ *                       adjust their behavior. The sentinel value ~0 indicates
+ *                       no command.
+ * @command_is_new:      Indicates the command has just come down to cmdfunc()
+ *                       from the HIL and hasn't yet been handled. Other
+ *                       functions use this to adjust their behavior.
+ * @page_address:        The current page address of interest. For reads, this
+ *                       information arrives in calls to cmdfunc(), but we don't
+ *                       actually use it until later.
+ * @nand:                The data structure that represents this NAND Flash
+ *                       medium to the MTD NAND Flash system.
+ * @mtd:                 The data structure that represents this NAND Flash
+ *                       medium to MTD.
+ * @partitions:          A pointer to a set of partitions collected from one of
+ *                       several possible sources (e.g., the boot loader, the
+ *                       kernel command line, etc.). See the global variable
+ *                       partition_source_types for the list of partition
+ *                       sources we examine. If this member is NULL, then no
+ *                       partitions were discovered.
+ * @partition_count:     The number of discovered partitions.
+ */
+
+struct imx_nfc_data {
+
+	/* System Interface */
+	struct device                 *dev;
+	struct platform_device        *pdev;
+
+	/* Platform Configuration */
+	struct imx_nfc_platform_data  *pdata;
+	void                          *buffers;
+	void                          *primary_regs;
+	void                          *secondary_regs;
+	struct clk                    *clock;
+	unsigned int                  interrupt;
+
+	/* Flash Hardware */
+	struct physical_geometry      physical_geometry;
+
+	/* NFC HAL and NFC Utilities */
+	struct nfc_hal                *nfc;
+	struct nfc_geometry           *nfc_geometry;
+	struct completion             done;
+
+	/* Medium Abstraction Layer */
+	struct logical_geometry       logical_geometry;
+	enum override                 interrupt_override;
+	enum override                 auto_op_override;
+	int                           inject_ecc_error;
+
+	/* MTD Interface Layer */
+	int                           current_chip;
+	unsigned int                  command;
+	int                           command_is_new;
+	int                           page_address;
+
+	/* NAND Flash MTD */
+	struct nand_chip              nand;
+
+	/* MTD */
+	struct mtd_info               mtd;
+	struct mtd_partition          *partitions;
+	unsigned int                  partition_count;
+
+};
+
+/**
+ * struct nfc_hal - i.MX NFC HAL
+ *
+ * This structure embodies an abstract interface to the underlying NFC hardware.
+ *
+ * @major_version:       The major version number of the NFC to which this
+ *                       structure applies.
+ * @minor_version:       The minor version number of the NFC to which this
+ *                       structure applies.
+ * @max_chip_count:      The maximum number of chips the NFC can possibly
+ *                       support. This may *not* be the actual number of chips
+ *                       currently connected. This value is constant for NFC's
+ *                       of a given version.
+ * @max_buffer_count:    The number of main/spare buffers available in the NFC's
+ *                       memory. This value is constant for NFC's of a given
+ *                       version.
+ * @spare_buf_stride:    The stride, in bytes, from the beginning of one spare
+ *                       buffer to the beginning of the next one. This value is
+ *                       constant for NFC's of a given version.
+ * @has_secondary_regs:  Indicates if the NFC has a secondary register set that
+ *                       must be mapped in.
+ * @can_be_symmetric:    Indicates if the NFC supports a "symmetric" clock. When
+ *                       the clock is "symmetric," the hardware waits one NFC
+ *                       clock for every read/write cycle. When the clock is
+ *                       "asymmetric," the hardware waits two NFC clocks for
+ *                       every read/write cycle.
+ * @init:                Initializes the NFC and any version-specific data
+ *                       structures. This function will be called after
+ *                       everything has been set up for communication with the
+ *                       NFC itself, but before the platform has set up off-chip
+ *                       communication. Thus, this function must not attempt to
+ *                       communicate with the NAND Flash hardware. A non-zero
+ *                       return value indicates failure.
+ * @set_geometry:        Based on the physical geometry, this function selects
+ *                       an NFC geometry structure and configures the NFC
+ *                       hardware to match. A  non-zero return value indicates
+ *                       failure.
+ * @exit:                Shuts down the NFC and cleans up version-specific data
+ *                       structures. This function will be called after the
+ *                       platform has shut down off-chip communication but while
+ *                       communication with the NFC still works.
+ * @set_closest_cycle:   Configures the hardware to make the NAND Flash bus
+ *                       cycle period as close as possible to the given cycle
+ *                       period. This function is called during boot up and may
+ *                       assume that, at the time it's called, the parent clock
+ *                       is running at the highest rate it will ever run. Thus,
+ *                       this function need never worry that the NAND Flash bus
+ *                       will run faster and potentially make it impossible to
+ *                       communicate with the NAND Flash device -- it will only
+ *                       run slower.
+ * @mask_interrupt:      Masks the NFC's interrupt.
+ * @unmask_interrupt:    Unmasks the NFC's interrupt.
+ * @clear_interrupt:     Clears the NFC's interrupt.
+ * @is_interrupting:     Returns true if the NFC is interrupting.
+ * @is_ready:            Returns true if all the chips in the medium are ready.
+ *                       This member may be set to NULL, which indicates that
+ *                       the underlying NFC hardware doesn't expose  ready/busy
+ *                       signals.
+ * @set_force_ce:        If passed true, forces the hardware chip enable signal
+ *                       for the current chip to be asserted always. If passed
+ *                       false, causes the chip enable signal to be asserted
+ *                       only during I/O.
+ * @set_ecc:             Sets ECC on or off.
+ * @get_ecc_status:      Examines the hardware ECC status and returns:
+ *                           == 0  =>  No errors.
+ *                           >  0  =>  The number of corrected errors.
+ *                           <  0  =>  There were uncorrectable errors.
+ * @get_symmetric:       Gets the current symmetric clock setting. For versions
+ *                       that don't support symmetric clocks, this function
+ *                       always returns false.
+ * @set_symmetric:       For versions that support symmetric clocks, sets
+ *                       whether or not the clock is symmetric.
+ * @select_chip:         Selects the current chip.
+ * @command_cycle:       Sends a command code and then returns immediately
+ *                       *without* waiting for the NFC to finish.
+ * @write_cycle:         Applies a single write cycle to the current chip,
+ * 			 sending the given byte, and waiting for the NFC to
+ *                       finish.
+ * @read_cycle:          Applies a single read cycle to the current chip and
+ *                       returns the result, necessarily waiting for the NFC to
+ *                       finish. The width of the result is the same as the
+ *                       width of the Flash bus.
+ * @read_page:           Applies read cycles to the current chip to read an
+ *                       entire page into the NFC. Note that ECC is enabled or
+ *                       disabled with the set_ecc function pointer (see above).
+ *                       This function waits for the NFC to finish before
+ *                       returning.
+ * @send_page:           Applies write cycles to send an entire page from the
+ *                       NFC to the current chip. Note that ECC is enabled or
+ *                       disabled with the set_ecc function pointer (see above).
+ *                       This function waits for the NFC to finish before
+ *                       returning.
+ * @start_auto_read:     Starts an automatic read operation. A NULL pointer
+ *                       indicates automatic read operations aren't available
+ *                       with this NFC version.
+ * @wait_for_auto_read:  Blocks until an automatic read operation is ready for
+ *                       the CPU to copy a page out of the NFC.
+ * @resume_auto_read:    Resumes an automatic read operation after the CPU has
+ *                       copied a page out.
+ * @start_auto_write:    Starts an automatic write operation. A NULL pointer
+ *                       indicates automatic write operations aren't available
+ *                       with this NFC version.
+ * @wait_for_auto_write: Blocks until an automatic write operation is ready for
+ *                       the CPU to copy a page into the NFC.
+ * @start_auto_erase:    Starts an automatic erase operation. A NULL pointer
+ *                       indicates automatic erase operations aren't available
+ *                       with this NFC version.
+ */
+
+struct nfc_hal {
+	const unsigned int  major_version;
+	const unsigned int  minor_version;
+	const unsigned int  max_chip_count;
+	const unsigned int  max_buffer_count;
+	const unsigned int  spare_buf_stride;
+	const int           has_secondary_regs;
+	const int           can_be_symmetric;
+	int       (*init)               (struct imx_nfc_data *);
+	int       (*set_geometry)       (struct imx_nfc_data *);
+	void      (*exit)               (struct imx_nfc_data *);
+	int       (*set_closest_cycle)  (struct imx_nfc_data *, unsigned ns);
+	void      (*mask_interrupt)     (struct imx_nfc_data *);
+	void      (*unmask_interrupt)   (struct imx_nfc_data *);
+	void      (*clear_interrupt)    (struct imx_nfc_data *);
+	int       (*is_interrupting)    (struct imx_nfc_data *);
+	int       (*is_ready)           (struct imx_nfc_data *);
+	void      (*set_force_ce)       (struct imx_nfc_data *, int on);
+	void      (*set_ecc)            (struct imx_nfc_data *, int on);
+	int       (*get_ecc_status)     (struct imx_nfc_data *);
+	int       (*get_symmetric)      (struct imx_nfc_data *);
+	void      (*set_symmetric)      (struct imx_nfc_data *, int on);
+	void      (*select_chip)        (struct imx_nfc_data *, int chip);
+	void      (*command_cycle)      (struct imx_nfc_data *, unsigned cmd);
+	void      (*write_cycle)        (struct imx_nfc_data *, unsigned byte);
+	unsigned  (*read_cycle)         (struct imx_nfc_data *);
+	void      (*read_page)          (struct imx_nfc_data *);
+	void      (*send_page)          (struct imx_nfc_data *);
+	int       (*start_auto_read)    (struct imx_nfc_data *,
+		unsigned start, unsigned count, unsigned column, unsigned page);
+	int       (*wait_for_auto_read) (struct imx_nfc_data *);
+	int       (*resume_auto_read)   (struct imx_nfc_data *);
+	int       (*start_auto_write)   (struct imx_nfc_data *,
+		unsigned start, unsigned count, unsigned column, unsigned page);
+	int       (*wait_for_auto_write)(struct imx_nfc_data *);
+	int       (*start_auto_erase)   (struct imx_nfc_data *,
+		unsigned start, unsigned count, unsigned page);
+};
+
+/*
+ * This variable controls whether or not probing is enabled. If false, then
+ * the driver will refuse to probe. The "enable" module parameter controls the
+ * value of this variable.
+ */
+
+static int imx_nfc_module_enable = true;
+
+#ifdef EVENT_REPORTING
+
+/*
+ * This variable controls whether the driver reports event information by
+ * printing to the console. The "report_events" module parameter controls the
+ * value of this variable.
+ */
+
+static int imx_nfc_module_report_events; /* implicitly initialized false*/
+
+#endif
+
+/*
+ * This variable potentially overrides the driver's choice to interleave. The
+ * "interleave_override" module parameter controls the value of this variable.
+ */
+
+static enum override imx_nfc_module_interleave_override = DRIVER_CHOICE;
+
+/*
+ * When set, this variable forces the driver to use the bytewise copy functions
+ * to get data in and out of the NFC. This is intended for testing, not typical
+ * use.
+ */
+
+static int imx_nfc_module_force_bytewise_copy; /* implicitly initialized false*/
+
+/*
+ * The list of algorithms we use to get partition information from the
+ * environment.
+ */
+
+#ifdef CONFIG_MTD_PARTITIONS
+static const char *partition_source_types[] = { "cmdlinepart", NULL };
+#endif
+
+/*
+ * The following structures describe the NFC geometries we support.
+ *
+ * Notice that pieces of some structure definitions are commented out and edited
+ * because various parts of the MTD system can't handle the way our hardware
+ * formats the out-of-band area.
+ *
+ * Here are the problems:
+ *
+ * - struct nand_ecclayout expects no more than 64 ECC bytes.
+ *
+ *     The eccpos member of struct nand_ecclayout can't hold more than 64 ECC
+ *     byte positions. Some of our formats have more so, unedited, they won't
+ *     even compile. We comment out all ECC byte positions after the 64th one.
+ *
+ * - struct nand_ecclayout expects no more than 8 free spans.
+ *
+ *     The oobfree member of struct nand_ecclayout can't hold more than 8 free
+ *     spans. Some of our formats have more so, unedited, they won't even
+ *     compile. We comment out all free spans after the eighth one.
+ *
+ * - The MEMGETOOBSEL command in the mtdchar driver.
+ *
+ *     The mtdchar ioctl command MEMGETOOBSEL checks the number of ECC bytes
+ *     against the length of the eccpos array in struct nand_oobinfo
+ *     (see include/mtd/mtd-abi.h). This array can handle up to 32 ECC bytes,
+ *     but some of our formats have more.
+ *
+ *     To make this work, we cap the value assigned to eccbytes at 32.
+ *
+ *     Notice that struct nand_oobinfo, used by mtdchar, is *different* from the
+ *     struct nand_ecclayout that MTD uses internally. The latter structure
+ *     can accomodate up to 64 ECC byte positions. Thus, we declare up to 64
+ *     ECC byte positions here, even if the advertised number of ECC bytes is
+ *     less.
+ *
+ *     This command is obsolete and, if no one used it, we wouldn't care about
+ *     this problem. Unfortunately The nandwrite program uses it, and everyone
+ *     expects nandwrite to work (it's how everyone usually lays down their
+ *     JFFS2 file systems).
+ */
+
+static struct nfc_geometry  nfc_geometry_512_16_RS_ECC1 = {
+	.page_data_size  = 512,
+	.page_oob_size   = 16,
+	.ecc_algorithm   = "Reed-Solomon",
+	.ecc_strength    = 1,
+	.buffer_count    = 1,
+	.spare_buf_size  = 16,
+	.spare_buf_spill = 0,
+	.mtd_layout = {
+		.eccbytes = 5,
+		.eccpos   = {6, 7, 8, 9, 10},
+		.oobavail = 11,
+		.oobfree  = { {0, 6}, {11, 5} },
+	}
+};
+
+static struct nfc_geometry  nfc_geometry_512_16_RS_ECC4 = {
+	.page_data_size  = 512,
+	.page_oob_size   = 16,
+	.ecc_algorithm   = "Reed-Solomon",
+	.ecc_strength    = 4,
+	.buffer_count    = 1,
+	.spare_buf_size  = 16,
+	.spare_buf_spill = 0,
+	.mtd_layout = {
+		.eccbytes = 9,
+		.eccpos   = {7, 8, 9, 10, 11, 12, 13, 14, 15},
+		.oobavail = 7,
+		.oobfree  = { {0, 7} },
+	}
+};
+
+static struct nfc_geometry  nfc_geometry_512_16_BCH_ECC4 = {
+	.page_data_size  = 512,
+	.page_oob_size   = 16,
+	.ecc_algorithm   = "BCH",
+	.ecc_strength    = 4,
+	.buffer_count    = 1,
+	.spare_buf_size  = 16,
+	.spare_buf_spill = 0,
+	.mtd_layout = {
+		.eccbytes = 8,
+		.eccpos   = {8, 9, 10, 11, 12, 13, 14, 15},
+		.oobavail = 8,
+		.oobfree  = { {0, 8} },
+	}
+};
+
+static struct nfc_geometry  nfc_geometry_2K_64_RS_ECC4 = {
+	.page_data_size  = 2048,
+	.page_oob_size   = 64,
+	.ecc_algorithm   = "Reed-Solomon",
+	.ecc_strength    = 4,
+	.buffer_count    = 4,
+	.spare_buf_size  = 16,
+	.spare_buf_spill = 0,
+	.mtd_layout = {
+		.eccbytes = 32 /*9 * 4*/, /* See notes above. */
+		.eccpos   = {
+
+			(0*16)+7 , (0*16)+8 , (0*16)+9 , (0*16)+10, (0*16)+11,
+			(0*16)+12, (0*16)+13, (0*16)+14, (0*16)+15,
+
+			(1*16)+7 , (1*16)+8 , (1*16)+9 , (1*16)+10, (1*16)+11,
+			(1*16)+12, (1*16)+13, (1*16)+14, (1*16)+15,
+
+			(2*16)+7 , (2*16)+8 , (2*16)+9 , (2*16)+10, (2*16)+11,
+			(2*16)+12, (2*16)+13, (2*16)+14, (2*16)+15,
+
+			(3*16)+7 , (3*16)+8 , (3*16)+9 , (3*16)+10, (3*16)+11,
+			(3*16)+12, (3*16)+13, (3*16)+14, (3*16)+15,
+
+		},
+		.oobavail = 7 * 4,
+		.oobfree  = {
+			{(0*16)+0, 7},
+			{(1*16)+0, 7},
+			{(2*16)+0, 7},
+			{(3*16)+0, 7},
+		},
+	}
+};
+
+static struct nfc_geometry  nfc_geometry_2K_64_BCH_ECC4 = {
+	.page_data_size  = 2048,
+	.page_oob_size   = 64,
+	.ecc_algorithm   = "BCH",
+	.ecc_strength    = 4,
+	.buffer_count    = 4,
+	.spare_buf_size  = 16,
+	.spare_buf_spill = 0,
+	.mtd_layout = {
+		.eccbytes = 8 * 4,
+		.eccpos   = {
+
+			(0*16)+8 , (0*16)+9 , (0*16)+10, (0*16)+11,
+			(0*16)+12, (0*16)+13, (0*16)+14, (0*16)+15,
+
+			(1*16)+8 , (1*16)+9 , (1*16)+10, (1*16)+11,
+			(1*16)+12, (1*16)+13, (1*16)+14, (1*16)+15,
+
+			(2*16)+8 , (2*16)+9 , (2*16)+10, (2*16)+11,
+			(2*16)+12, (2*16)+13, (2*16)+14, (2*16)+15,
+
+			(3*16)+8 , (3*16)+9 , (3*16)+10, (3*16)+11,
+			(3*16)+12, (3*16)+13, (3*16)+14, (3*16)+15,
+
+		},
+		.oobavail = 8 * 4,
+		.oobfree  = {
+			{(0*16)+0, 8},
+			{(1*16)+0, 8},
+			{(2*16)+0, 8},
+			{(3*16)+0, 8},
+		},
+	}
+};
+
+static struct nfc_geometry  nfc_geometry_4K_128_BCH_ECC4 = {
+	.page_data_size  = 4096,
+	.page_oob_size   = 128,
+	.ecc_algorithm   = "BCH",
+	.ecc_strength    = 4,
+	.buffer_count    = 8,
+	.spare_buf_size  = 16,
+	.spare_buf_spill = 0,
+	.mtd_layout = {
+		.eccbytes = 8 * 8,
+		.eccpos   = {
+
+			(0*16)+8 , (0*16)+9 , (0*16)+10, (0*16)+11,
+			(0*16)+12, (0*16)+13, (0*16)+14, (0*16)+15,
+
+			(1*16)+8 , (1*16)+9 , (1*16)+10, (1*16)+11,
+			(1*16)+12, (1*16)+13, (1*16)+14, (1*16)+15,
+
+			(2*16)+8 , (2*16)+9 , (2*16)+10, (2*16)+11,
+			(2*16)+12, (2*16)+13, (2*16)+14, (2*16)+15,
+
+			(3*16)+8 , (3*16)+9 , (3*16)+10, (3*16)+11,
+			(3*16)+12, (3*16)+13, (3*16)+14, (3*16)+15,
+
+			(4*16)+8 , (4*16)+9 , (4*16)+10, (4*16)+11,
+			(4*16)+12, (4*16)+13, (4*16)+14, (4*16)+15,
+
+			(5*16)+8 , (5*16)+9 , (5*16)+10, (5*16)+11,
+			(5*16)+12, (5*16)+13, (5*16)+14, (5*16)+15,
+
+			(6*16)+8 , (6*16)+9 , (6*16)+10, (6*16)+11,
+			(6*16)+12, (6*16)+13, (6*16)+14, (6*16)+15,
+
+			(7*16)+8 , (7*16)+9 , (7*16)+10, (7*16)+11,
+			(7*16)+12, (7*16)+13, (7*16)+14, (7*16)+15,
+
+		},
+		.oobavail = 8 * 8,
+		.oobfree  = {
+			{(0*16)+0, 8},
+			{(1*16)+0, 8},
+			{(2*16)+0, 8},
+			{(3*16)+0, 8},
+			{(4*16)+0, 8},
+			{(5*16)+0, 8},
+			{(6*16)+0, 8},
+			{(7*16)+0, 8},
+		},
+	}
+};
+
+static struct nfc_geometry  nfc_geometry_4K_218_BCH_ECC8 = {
+	.page_data_size  = 4096,
+	.page_oob_size   = 218,
+	.ecc_algorithm   = "BCH",
+	.ecc_strength    = 8,
+	.buffer_count    = 8,
+	.spare_buf_size  = 26,
+	.spare_buf_spill = 10,
+	.mtd_layout = {
+		.eccbytes = 32 /*10 * 8*/, /* See notes above. */
+		.eccpos   = {
+
+			(0*26)+12, (0*26)+13, (0*26)+14, (0*26)+15, (0*26)+16,
+			(0*26)+17, (0*26)+18, (0*26)+19, (0*26)+20, (0*26)+21,
+
+			(1*26)+12, (1*26)+13, (1*26)+14, (1*26)+15, (1*26)+16,
+			(1*26)+17, (1*26)+18, (1*26)+19, (1*26)+20, (1*26)+21,
+
+			(2*26)+12, (2*26)+13, (2*26)+14, (2*26)+15, (2*26)+16,
+			(2*26)+17, (2*26)+18, (2*26)+19, (2*26)+20, (2*26)+21,
+
+			(3*26)+12, (3*26)+13, (3*26)+14, (3*26)+15, (3*26)+16,
+			(3*26)+17, (3*26)+18, (3*26)+19, (3*26)+20, (3*26)+21,
+
+			(4*26)+12, (4*26)+13, (4*26)+14, (4*26)+15, (4*26)+16,
+			(4*26)+17, (4*26)+18, (4*26)+19, (4*26)+20, (4*26)+21,
+
+			(5*26)+12, (5*26)+13, (5*26)+14, (5*26)+15, (5*26)+16,
+			(5*26)+17, (5*26)+18, (5*26)+19, (5*26)+20, (5*26)+21,
+
+			(6*26)+12, (6*26)+13, (6*26)+14, (6*26)+15,
+			/*  See notes above.
+			(6*26)+16,
+			(6*26)+17, (6*26)+18, (6*26)+19, (6*26)+20, (6*26)+21,
+
+			(7*26)+12, (7*26)+13, (7*26)+14, (7*26)+15, (7*26)+16,
+			(7*26)+17, (7*26)+18, (7*26)+19, (7*26)+20, (7*26)+21,
+			*/
+
+		},
+		.oobavail = 96 /*(16 * 8) + 10*/, /* See notes above. */
+		.oobfree  = {
+			{(0*26)+0, 12}, {(0*26)+22, 4},
+			{(1*26)+0, 12}, {(1*26)+22, 4},
+			{(2*26)+0, 12}, {(2*26)+22, 4},
+			{(3*26)+0, 48}, /* See notes above. */
+			/* See notes above.
+			{(3*26)+0, 12}, {(3*26)+22, 4},
+			{(4*26)+0, 12}, {(4*26)+22, 4},
+			{(5*26)+0, 12}, {(5*26)+22, 4},
+			{(6*26)+0, 12}, {(6*26)+22, 4},
+			{(7*26)+0, 12}, {(7*26)+22, 4 + 10},
+			*/
+		},
+	}
+};
+
+/*
+ * When the logical geometry differs from the NFC geometry (e.g.,
+ * interleaving), we synthesize a layout rather than use the one that comes with
+ * the NFC geometry. See mal_set_logical_geometry().
+ */
+
+static struct nand_ecclayout  synthetic_layout;
+
+/*
+ * These structures describe how the BBT code will find block marks in the OOB
+ * area of a page. Don't be confused by the fact that this is the same type used
+ * to describe bad block tables. Some of the same information is needed, so the
+ * designers use the same structure for two conceptually distinct functions.
+ */
+
+static uint8_t block_mark_pattern[] = { 0xff, 0xff };
+
+static struct nand_bbt_descr small_page_block_mark_descriptor = {
+	.options = NAND_BBT_SCAN2NDPAGE,
+	.offs    = 5,
+	.len     = 1,
+	.pattern = block_mark_pattern,
+};
+
+static struct nand_bbt_descr large_page_block_mark_descriptor = {
+	.options = NAND_BBT_SCAN2NDPAGE,
+	.offs    = 0,
+	.len     = 2,
+	.pattern = block_mark_pattern,
+};
+
+/*
+ * Bad block table descriptors for the main and mirror tables.
+ */
+
+static uint8_t bbt_main_pattern[]   = { 'B', 'b', 't', '0' };
+static uint8_t bbt_mirror_pattern[] = { '1', 't', 'b', 'B' };
+
+static struct nand_bbt_descr bbt_main_descriptor = {
+	.options =
+		NAND_BBT_LASTBLOCK |
+		NAND_BBT_CREATE    |
+		NAND_BBT_WRITE     |
+		NAND_BBT_2BIT      |
+		NAND_BBT_VERSION   |
+		NAND_BBT_PERCHIP,
+	.offs      = 0,
+	.len       = 4,
+	.veroffs   = 4,
+	.maxblocks = 4,
+	.pattern   = bbt_main_pattern,
+};
+
+static struct nand_bbt_descr bbt_mirror_descriptor = {
+	.options =
+		NAND_BBT_LASTBLOCK |
+		NAND_BBT_CREATE    |
+		NAND_BBT_WRITE     |
+		NAND_BBT_2BIT      |
+		NAND_BBT_VERSION   |
+		NAND_BBT_PERCHIP,
+	.offs      = 0,
+	.len       = 4,
+	.veroffs   = 4,
+	.maxblocks = 4,
+	.pattern   = bbt_mirror_pattern,
+};
+
+#ifdef EVENT_REPORTING
+
+/**
+ * struct event - A single record in the event trace.
+ *
+ * @time:         The time at which the event occurred.
+ * @nesting:      Indicates function call nesting.
+ * @description:  A description of the event.
+ */
+
+struct event {
+	ktime_t       time;
+	unsigned int  nesting;
+	char          *description;
+};
+
+/**
+ * The event trace.
+ *
+ * @overhead:  The delay to take a time stamp and nothing else.
+ * @nesting:   The current nesting level.
+ * @overflow:  Indicates the trace overflowed.
+ * @next:      Index of the next event to write.
+ * @events:    The array of events.
+ */
+
+#define MAX_EVENT_COUNT  (200)
+
+static struct {
+	ktime_t       overhead;
+	int           nesting;
+	int           overflow;
+	unsigned int  next;
+	struct event  events[MAX_EVENT_COUNT];
+} event_trace;
+
+/**
+ * reset_event_trace() - Resets the event trace.
+ */
+static void reset_event_trace(void)
+{
+	event_trace.nesting  = 0;
+	event_trace.overflow = false;
+	event_trace.next     = 0;
+}
+
+/**
+ * add_event() - Adds an event to the event trace.
+ *
+ * @description:  A description of the event.
+ * @delta:        A delta to the nesting level for this event [-1, 0, 1].
+ */
+static inline void add_event(char *description, int delta)
+{
+	struct event  *event;
+
+	if (!imx_nfc_module_report_events)
+		return;
+
+	if (event_trace.overflow)
+		return;
+
+	if (event_trace.next >= MAX_EVENT_COUNT) {
+		event_trace.overflow = true;
+		return;
+	}
+
+	event = event_trace.events + event_trace.next;
+
+	event->time = ktime_get();
+
+	event->description = description;
+
+	if (!delta)
+		event->nesting = event_trace.nesting;
+	else if (delta < 0) {
+		event->nesting = event_trace.nesting - 1;
+		event_trace.nesting -= 2;
+	} else {
+		event->nesting = event_trace.nesting + 1;
+		event_trace.nesting += 2;
+	}
+
+	if (event_trace.nesting < 0)
+		event_trace.nesting = 0;
+
+	event_trace.next++;
+
+}
+
+/**
+ * add_state_event_l() - Adds an event to display some state.
+ *
+ * @address:   The address to examine.
+ * @mask:      A mask to apply to the contents of the given address.
+ * @clear:     An event message to add if the result is zero.
+ * @not_zero:  An event message to add if the result is not zero.
+ */
+static void add_state_event_l(void *address, uint32_t mask,
+						char *zero, char *not_zero)
+{
+	int  state;
+	state = !!(__raw_readl(address) & mask);
+	if (state)
+		add_event(not_zero, 0);
+	else
+		add_event(zero, 0);
+}
+
+/**
+ * start_event_trace() - Starts an event trace and adds the first event.
+ *
+ * @description:  A description of the first event.
+ */
+static void start_event_trace(char *description)
+{
+
+	ktime_t  t0;
+	ktime_t  t1;
+
+	if (!imx_nfc_module_report_events)
+		return;
+
+	reset_event_trace();
+
+	t0 = ktime_get();
+	t1 = ktime_get();
+
+	event_trace.overhead = ktime_sub(t1, t0);
+
+	add_event(description, 1);
+
+}
+
+/**
+ * dump_event_trace() - Dumps the event trace.
+ */
+static void dump_event_trace(void)
+{
+	unsigned int  i;
+	time_t        seconds;
+	long          nanoseconds;
+	char          line[100];
+	int           o;
+	struct event  *first_event;
+	struct event  *last_event;
+	struct event  *matching_event;
+	struct event  *event;
+	ktime_t       delta;
+
+	/* Check if event reporting is turned off. */
+
+	if (!imx_nfc_module_report_events)
+		return;
+
+	/* Print important facts about this event trace. */
+
+	printk(KERN_DEBUG "\n+--------------\n");
+
+	printk(KERN_DEBUG "|  Overhead    : [%d:%d]\n",
+				event_trace.overhead.tv.sec,
+				event_trace.overhead.tv.nsec);
+
+	if (!event_trace.next) {
+		printk(KERN_DEBUG "|  No Events\n");
+		return;
+	}
+
+	first_event = event_trace.events;
+	last_event  = event_trace.events + (event_trace.next - 1);
+
+	delta = ktime_sub(last_event->time, first_event->time);
+	printk(KERN_DEBUG "|  Elapsed Time: [%d:%d]\n",
+						delta.tv.sec, delta.tv.nsec);
+
+	if (event_trace.overflow)
+		printk(KERN_DEBUG "|  Overflow!\n");
+
+	/* Print the events in this history. */
+
+	for (i = 0, event = event_trace.events;
+					i < event_trace.next; i++, event++) {
+
+		/* Get the delta between this event and the previous event. */
+
+		if (!i) {
+			seconds     = 0;
+			nanoseconds = 0;
+		} else {
+			delta = ktime_sub(event[0].time, event[-1].time);
+			seconds     = delta.tv.sec;
+			nanoseconds = delta.tv.nsec;
+		}
+
+		/* Print the current event. */
+
+		o = 0;
+
+		o = snprintf(line, sizeof(line) - o, "|  [%ld:% 10ld]%*s %s",
+							seconds, nanoseconds,
+							event->nesting, "",
+							event->description);
+		/* Check if this is the last event in a nested series. */
+
+		if (i && (event[0].nesting < event[-1].nesting)) {
+
+			for (matching_event = event - 1;; matching_event--) {
+
+				if (matching_event < event_trace.events) {
+					matching_event = 0;
+					break;
+				}
+
+				if (matching_event->nesting == event->nesting)
+					break;
+
+			}
+
+			if (matching_event) {
+				delta = ktime_sub(event->time,
+							matching_event->time);
+				o += snprintf(line + o, sizeof(line) - o,
+						" <%d:%d]", delta.tv.sec,
+								delta.tv.nsec);
+			}
+
+		}
+
+		/* Check if this is the first event in a nested series. */
+
+		if ((i < event_trace.next - 1) &&
+				(event[0].nesting < event[1].nesting)) {
+
+			for (matching_event = event + 1;; matching_event++) {
+
+				if (matching_event >=
+					(event_trace.events+event_trace.next)) {
+					matching_event = 0;
+					break;
+				}
+
+				if (matching_event->nesting == event->nesting)
+					break;
+
+			}
+
+			if (matching_event) {
+				delta = ktime_sub(matching_event->time,
+								event->time);
+				o += snprintf(line + o, sizeof(line) - o,
+						" [%d:%d>", delta.tv.sec,
+								delta.tv.nsec);
+			}
+
+		}
+
+		printk(KERN_DEBUG "%s\n", line);
+
+	}
+
+	printk(KERN_DEBUG "+--------------\n");
+
+}
+
+/**
+ * stop_event_trace() - Stops an event trace.
+ *
+ * @description:  A description of the last event.
+ */
+static void stop_event_trace(char *description)
+{
+	struct event  *event;
+
+	if (!imx_nfc_module_report_events)
+		return;
+
+	/*
+	 * We want the end of the trace, no matter what happens. If the trace
+	 * has already overflowed, or is about to, just jam this event into the
+	 * last spot. Otherwise, add this event like any other.
+	 */
+
+	if (event_trace.overflow || (event_trace.next >= MAX_EVENT_COUNT)) {
+		event = event_trace.events + (MAX_EVENT_COUNT - 1);
+		event->time = ktime_get();
+		event->description = description;
+		event->nesting     = 0;
+	} else {
+		add_event(description, -1);
+	}
+
+	dump_event_trace();
+	reset_event_trace();
+
+}
+
+#else /* EVENT_REPORTING */
+
+#define start_event_trace(description)                   do {} while (0)
+#define add_event(description, delta)                    do {} while (0)
+#define add_state_event_l(address, mask, zero, not_zero) do {} while (0)
+#define stop_event_trace(description)                    do {} while (0)
+#define dump_event_trace()                               do {} while (0)
+
+#endif /* EVENT_REPORTING */
+
+/**
+ * unimplemented() - Announces intentionally unimplemented features.
+ *
+ * @this:  Per-device data.
+ * @msg:   A message about the unimplemented feature.
+ */
+static inline void unimplemented(struct imx_nfc_data *this, const char * msg)
+{
+	dev_err(this->dev, "Intentionally unimplemented: %s", msg);
+}
+
+/**
+ * raw_read_mask_w() - Reads masked bits in a 16-bit hardware register.
+ */
+static inline uint16_t raw_read_mask_w(uint16_t mask, void *address)
+{
+	return __raw_readw(address) & mask;
+}
+
+/**
+ * raw_set_mask_w() - Sets bits in a 16-bit hardware register.
+ */
+static inline void raw_set_mask_w(uint16_t mask, void *address)
+{
+	__raw_writew(__raw_readw(address) | mask, address);
+}
+
+/**
+ * raw_clr_mask_w() - Clears bits in a 16-bit hardware register.
+ */
+static inline void raw_clr_mask_w(uint16_t mask, void *address)
+{
+	__raw_writew(__raw_readw(address) & (~mask), address);
+}
+
+/**
+ * raw_read_mask_l() - Reads masked bits in a 32-bit hardware register.
+ */
+static inline uint32_t raw_read_mask_l(uint32_t mask, void *address)
+{
+	return __raw_readl(address) & mask;
+}
+
+/**
+ * raw_set_mask_l() - Sets bits in a 32-bit hardware register.
+ */
+static inline void raw_set_mask_l(uint32_t mask, void *address)
+{
+	__raw_writel(__raw_readl(address) | mask, address);
+}
+
+/**
+ * raw_clr_mask_l() - Clears bits in a 32-bit hardware register.
+ */
+static inline void raw_clr_mask_l(uint32_t mask, void *address)
+{
+	__raw_writel(__raw_readl(address) & (~mask), address);
+}
+
+/**
+ * is_large_page_chip() - Returns true for large page media.
+ *
+ * @this:  Per-device data.
+ */
+static inline int is_large_page_chip(struct imx_nfc_data *this)
+{
+	return (this->physical_geometry.page_data_size > 512);
+}
+
+/**
+ * is_small_page_chip() - Returns true for small page media.
+ *
+ * @this:  Per-device data.
+ */
+static inline int is_small_page_chip(struct imx_nfc_data *this)
+{
+	return !is_large_page_chip(this);
+}
+
+/**
+ * get_cycle_in_ns() - Returns the given device's cycle period, in ns.
+ *
+ * @this:  Per-device data.
+ */
+static inline unsigned get_cycle_in_ns(struct imx_nfc_data *this)
+{
+	unsigned long  cycle_in_ns;
+
+	cycle_in_ns = 1000000000 / clk_get_rate(this->clock);
+
+	if (!this->nfc->get_symmetric(this))
+		cycle_in_ns *= 2;
+
+	return cycle_in_ns;
+
+}
+
+/**
+ * nfc_util_set_best_cycle() - Sets the closest possible NAND Flash bus cycle.
+ *
+ * This function computes the clock setup that will best approximate the given
+ * target Flash bus cycle period.
+ *
+ * For some NFC versions, we can make the clock "symmetric." When the clock
+ * is "symmetric," the hardware waits one NFC clock for every read/write cycle.
+ * When the clock is "asymmetric," the hardware waits two NFC clocks for every
+ * read/write cycle. Thus, making the clock asymmetric essentially divides the
+ * NFC clock by two.
+ *
+ * We compute the target frequency that matches the given target period. We then
+ * discover the closest available match with that frequency and the closest
+ * available match with double that frequency (for use with an asymmetric
+ * clock). We implement the best choice of original clock and symmetric or
+ * asymmetric setting, preferring symmetric clocks.
+ *
+ * @this:      Per-device data.
+ * @ns:        The target cycle period, in nanoseconds.
+ * @no_asym:   Disallow making the clock asymmetric.
+ * @no_sym:    Disallow making the clock  symmetric.
+ */
+static int nfc_util_set_best_cycle(struct imx_nfc_data *this,
+				unsigned int ns, int no_asym, int no_sym)
+{
+	unsigned long  target_hz;
+	long           symmetric_hz;
+	long           symmetric_delta_hz;
+	long           asymmetric_hz;
+	long           asymmetric_delta_hz;
+	unsigned long  best_hz;
+	int            best_symmetry_setting;
+	struct device  *dev = this->dev;
+
+	/* The target cycle period must be greater than zero. */
+
+	if (!ns)
+		return -EINVAL;
+
+	/* Compute the target frequency. */
+
+	target_hz = 1000000000 / ns;
+
+	/* Find out how close we can get with a symmetric clock. */
+
+	if (!no_sym && this->nfc->can_be_symmetric)
+		symmetric_hz = clk_round_rate(this->clock, target_hz);
+	else
+		symmetric_hz = -EINVAL;
+
+	/* Find out how close we can get with an asymmetric clock. */
+
+	if (!no_asym)
+		asymmetric_hz = clk_round_rate(this->clock, target_hz * 2);
+	else
+		asymmetric_hz = -EINVAL;
+
+	/* Does anything work at all? */
+
+	if ((symmetric_hz == -EINVAL) && (asymmetric_hz == -EINVAL)) {
+		dev_err(dev, "Can't support Flash bus cycle of %uns\n", ns);
+		return -EINVAL;
+	}
+
+	/* Discover the best match. */
+
+	if ((symmetric_hz != -EINVAL) && (asymmetric_hz != -EINVAL)) {
+
+		symmetric_delta_hz  = target_hz - symmetric_hz;
+		asymmetric_delta_hz = target_hz - (asymmetric_hz / 2);
+
+		if (symmetric_delta_hz <= asymmetric_delta_hz)
+			best_symmetry_setting = true;
+		else
+			best_symmetry_setting = false;
+
+	} else if (symmetric_hz != -EINVAL) {
+		best_symmetry_setting = true;
+	} else {
+		best_symmetry_setting = false;
+	}
+
+	best_hz = best_symmetry_setting ? symmetric_hz : asymmetric_hz;
+
+	/* Implement the best match. */
+
+	this->nfc->set_symmetric(this, best_symmetry_setting);
+
+	return clk_set_rate(this->clock, best_hz);
+
+}
+
+/**
+ * nfc_util_wait_for_the_nfc() - Waits for the NFC to finish an operation.
+ *
+ * @this:     Per-device data.
+ * @use_irq:  Indicates that we should wait for an interrupt rather than polling
+ *            and delaying.
+ */
+static void nfc_util_wait_for_the_nfc(struct imx_nfc_data *this, int use_irq)
+{
+	unsigned       spin_count;
+	struct device  *dev = this->dev;
+
+	/* Apply the override, if any. */
+
+	switch (this->interrupt_override) {
+
+	case NEVER:
+		use_irq = false;
+		break;
+
+	case DRIVER_CHOICE:
+		break;
+
+	case ALWAYS:
+		use_irq = true;
+		break;
+
+	}
+
+	/* Check if we're using interrupts. */
+
+	if (use_irq) {
+
+		/*
+		 * If control arrives here, the caller wants to use interrupts.
+		 * Presumably, this operation is known to take a very long time.
+		 */
+
+		if (this->nfc->is_interrupting(this)) {
+			add_event("Waiting for the NFC (early interrupt)", 1);
+			this->nfc->clear_interrupt(this);
+		} else {
+			add_event("Waiting for the NFC (interrupt)", 1);
+			this->nfc->unmask_interrupt(this);
+			wait_for_completion(&this->done);
+		}
+
+		add_event("NFC done", -1);
+
+	} else {
+
+		/*
+		 * If control arrives here, the caller doesn't want to use
+		 * interrupts. Presumably, this operation is too quick to
+		 * justify the overhead. Leave the interrupt masked, and loop
+		 * until the interrupt bit lights up, or we time out.
+		 *
+		 * We spin for a maximum of about 2ms before declaring a time
+		 * out. No operation we could possibly spin on should take that
+		 * long.
+		 */
+
+		spin_count = 2000;
+
+		add_event("Waiting for the NFC (polling)", 1);
+
+		for (; spin_count > 0; spin_count--) {
+
+			if (this->nfc->is_interrupting(this)) {
+				this->nfc->clear_interrupt(this);
+				add_event("NFC done", -1);
+				return;
+			}
+
+			udelay(1);
+
+		}
+
+		/* Timed out. */
+
+		add_event("Timed out", -1);
+
+		dev_err(dev, "[wait_for_the_nfc] ===== Time Out =====\n");
+		dump_event_trace();
+
+	}
+
+}
+
+/**
+ *  nfc_util_bytewise_copy_from_nfc_mem() - Copies bytes from the NFC memory.
+ *
+ * @from:  A pointer to the source memory.
+ * @to:    A pointer to the destination memory.
+ * @size:  The number of bytes to copy.
+ */
+static void nfc_util_bytewise_copy_from_nfc_mem(const void *from,
+							void *to, size_t n)
+{
+	unsigned int   i;
+	const uint8_t  *f = from;
+	uint8_t        *t = to;
+	uint16_t       *p;
+	uint16_t       x;
+
+	for (i = 0; i < n; i++, f++, t++) {
+
+		p = (uint16_t *) (((unsigned long) f) & ~((unsigned long) 1));
+
+		x = __raw_readw(p);
+
+		if (((unsigned long) f) & 0x1)
+			*t = (x >> 8) & 0xff;
+		else
+			*t = (x >> 0) & 0xff;
+
+	}
+
+}
+
+/**
+ * nfc_util_bytewise_copy_to_nfc_mem() - Copies bytes to the NFC memory.
+ *
+ * @from:  A pointer to the source memory.
+ * @to:    A pointer to the destination memory.
+ * @size:  The number of bytes to copy.
+ */
+static void nfc_util_bytewise_copy_to_nfc_mem(const void *from,
+							void *to, size_t n)
+{
+	unsigned int   i;
+	const uint8_t  *f = from;
+	uint8_t        *t = to;
+	uint16_t       *p;
+	uint16_t       x;
+
+	for (i = 0; i < n; i++, f++, t++) {
+
+		p = (uint16_t *) (((unsigned long) t) & ~((unsigned long) 1));
+
+		x = __raw_readw(p);
+
+		if (((unsigned long) t) & 0x1)
+			((uint8_t *)(&x))[1] = *f;
+		else
+			((uint8_t *)(&x))[0] = *f;
+
+		__raw_writew(x, p);
+
+	}
+
+}
+
+/**
+ * nfc_util_copy_from_nfc_mem() - Copies from the NFC memory to main memory.
+ *
+ * @from:  A pointer to the source memory.
+ * @to:    A pointer to the destination memory.
+ * @size:  The number of bytes to copy.
+ */
+static void nfc_util_copy_from_nfc_mem(const void *from, void *to, size_t n)
+{
+	unsigned int  chunk_count;
+
+	/*
+	 * Check if we're testing bytewise copies.
+	 */
+
+	if (imx_nfc_module_force_bytewise_copy)
+		goto force_bytewise_copy;
+
+	/*
+	 * We'd like to use memcpy to get data out of the NFC but, because that
+	 * memory responds only to 16- and 32-byte reads, we can only do so
+	 * safely if both the start and end of both the source and destination
+	 * are perfectly aligned on 4-byte boundaries.
+	 */
+
+	if (!(((unsigned long) from) & 0x3) && !(((unsigned long) to) & 0x3)) {
+
+		/*
+		 * If control arrives here, both the source and destination are
+		 * aligned on 4-byte boundaries. Compute the number of whole,
+		 * 4-byte chunks we can move.
+		 */
+
+		chunk_count = n / 4;
+
+		/*
+		 * Move all the chunks we can, and then update the pointers and
+		 * byte count to show what's left.
+		 */
+
+		if (chunk_count) {
+			memcpy(to, from, chunk_count * 4);
+			from += chunk_count * 4;
+			to   += chunk_count * 4;
+			n    -= chunk_count * 4;
+		}
+
+	}
+
+	/*
+	 * Move what's left.
+	 */
+
+force_bytewise_copy:
+
+	nfc_util_bytewise_copy_from_nfc_mem(from, to, n);
+
+}
+
+/**
+ * nfc_util_copy_to_nfc_mem() - Copies from main memory to the NFC memory.
+ *
+ * @from:  A pointer to the source memory.
+ * @to:    A pointer to the destination memory.
+ * @size:  The number of bytes to copy.
+ */
+static void nfc_util_copy_to_nfc_mem(const void *from, void *to, size_t n)
+{
+	unsigned int  chunk_count;
+
+	/*
+	 * Check if we're testing bytewise copies.
+	 */
+
+	if (imx_nfc_module_force_bytewise_copy)
+		goto force_bytewise_copy;
+
+	/*
+	 * We'd like to use memcpy to get data into the NFC but, because that
+	 * memory responds only to 16- and 32-byte writes, we can only do so
+	 * safely if both the start and end of both the source and destination
+	 * are perfectly aligned on 4-byte boundaries.
+	 */
+
+	if (!(((unsigned long) from) & 0x3) && !(((unsigned long) to) & 0x3)) {
+
+		/*
+		 * If control arrives here, both the source and destination are
+		 * aligned on 4-byte boundaries. Compute the number of whole,
+		 * 4-byte chunks we can move.
+		 */
+
+		chunk_count = n / 4;
+
+		/*
+		 * Move all the chunks we can, and then update the pointers and
+		 * byte count to show what's left.
+		 */
+
+		if (chunk_count) {
+			memcpy(to, from, chunk_count * 4);
+			from += chunk_count * 4;
+			to   += chunk_count * 4;
+			n    -= chunk_count * 4;
+		}
+
+	}
+
+	/*
+	 * Move what's left.
+	 */
+
+force_bytewise_copy:
+
+	nfc_util_bytewise_copy_to_nfc_mem(from, to, n);
+
+}
+
+/**
+ * nfc_util_copy_from_the_nfc() - Copies bytes out of the NFC.
+ *
+ * This function makes the data in the NFC look like a contiguous, model page.
+ *
+ * @this:   Per-device data.
+ * @start:  The index of the starting byte in the NFC.
+ * @buf:    A pointer to the destination buffer.
+ * @len:    The number of bytes to copy out.
+ */
+static void nfc_util_copy_from_the_nfc(struct imx_nfc_data *this,
+			unsigned int start, uint8_t *buf, unsigned int len)
+{
+	unsigned int         i;
+	unsigned int         count;
+	unsigned int         offset;
+	unsigned int         data_size;
+	unsigned int         oob_size;
+	unsigned int         total_size;
+	void                 *spare_base;
+	unsigned int         first_spare;
+	void                 *from;
+	struct nfc_geometry  *geometry = this->nfc_geometry;
+
+	/*
+	 * During initialization, the HIL will attempt to read ID bytes. For
+	 * some NFC hardware versions, the ID bytes are deposited in the NFC
+	 * memory, so this function will be called to deliver them. At that
+	 * point, we won't know the NFC geometry. That's OK because we're only
+	 * going to be reading a byte at a time.
+	 *
+	 * If we don't yet know the NFC geometry, just plug in some values that
+	 * make things work for now.
+	 */
+
+	if (unlikely(!geometry)) {
+		data_size = NFC_MAIN_BUF_SIZE;
+		oob_size  = 0;
+	} else {
+		data_size = geometry->page_data_size;
+		oob_size  = geometry->page_oob_size;
+	}
+
+	total_size = data_size + oob_size;
+
+	/* Validate. */
+
+	if ((start >= total_size) || ((start + len) > total_size)) {
+		dev_err(this->dev, "Bad copy from NFC memory: [%u, %u]\n",
+								start, len);
+		return;
+	}
+
+	/* Check if we're copying anything at all. */
+
+	if (!len)
+		return;
+
+	/* Check if anything comes from the main area. */
+
+	if (start < data_size) {
+
+		/* Compute the bytes to copy from the main area. */
+
+		count = min(len, data_size - start);
+
+		/* Copy. */
+
+		nfc_util_copy_from_nfc_mem(this->buffers + start, buf, count);
+
+		buf   += count;
+		start += count;
+		len   -= count;
+
+	}
+
+	/* Check if we're done. */
+
+	if (!len)
+		return;
+
+	/* Compute the base address of the spare buffers. */
+
+	spare_base = this->buffers +
+			(this->nfc->max_buffer_count * NFC_MAIN_BUF_SIZE);
+
+	/* Discover in which spare buffer the copying begins. */
+
+	first_spare = (start - data_size) / geometry->spare_buf_size;
+
+	/* Check if anything comes from the regular spare buffer area. */
+
+	if (first_spare < geometry->buffer_count) {
+
+		/* Start copying from spare buffers. */
+
+		for (i = first_spare; i < geometry->buffer_count; i++) {
+
+			/* Compute the offset into this spare area. */
+
+			offset = start -
+				(data_size + (geometry->spare_buf_size * i));
+
+			/* Compute the address of that offset. */
+
+			from = spare_base + offset +
+					(this->nfc->spare_buf_stride * i);
+
+			/* Compute the bytes to copy from this spare area. */
+
+			count = min(len, geometry->spare_buf_size - offset);
+
+			/* Copy. */
+
+			nfc_util_copy_from_nfc_mem(from, buf, count);
+
+			buf   += count;
+			start += count;
+			len   -= count;
+
+		}
+
+	}
+
+	/* Check if we're done. */
+
+	if (!len)
+		return;
+
+	/* Compute the offset into the extra spare area. */
+
+	offset = start -
+		(data_size + (geometry->spare_buf_size*geometry->buffer_count));
+
+	/* Compute the address of that offset. */
+
+	from = spare_base + offset +
+			(this->nfc->spare_buf_stride * geometry->buffer_count);
+
+	/* Compute the bytes to copy from the extra spare area. */
+
+	count = min(len, geometry->spare_buf_spill - offset);
+
+	/* Copy. */
+
+	nfc_util_copy_from_nfc_mem(from, buf, count);
+
+}
+
+/**
+ * nfc_util_copy_to_the_nfc() - Copies bytes into the NFC memory.
+ *
+ * This function makes the data in the NFC look like a contiguous, model page.
+ *
+ * @this:   Per-device data.
+ * @buf:   A pointer to the source buffer.
+ * @start: The index of the starting byte in the NFC memory.
+ * @len:   The number of bytes to copy in.
+ */
+static void nfc_util_copy_to_the_nfc(struct imx_nfc_data *this,
+		const uint8_t *buf, unsigned int start, unsigned int len)
+{
+	unsigned int         i;
+	unsigned int         count;
+	unsigned int         offset;
+	unsigned int         data_size;
+	unsigned int         oob_size;
+	unsigned int         total_size;
+	void                 *spare_base;
+	unsigned int         first_spare;
+	void                 *to;
+	struct nfc_geometry  *geometry = this->nfc_geometry;
+
+	/* Establish some important facts. */
+
+	data_size  = geometry->page_data_size;
+	oob_size   = geometry->page_oob_size;
+	total_size = data_size + oob_size;
+
+	/* Validate. */
+
+	if ((start >= total_size) || ((start + len) > total_size)) {
+		dev_err(this->dev, "Bad copy to NFC memory: [%u, %u]\n",
+								start, len);
+		return;
+	}
+
+	/* Check if we're copying anything at all. */
+
+	if (!len)
+		return;
+
+	/* Check if anything goes to the main area. */
+
+	if (start < data_size) {
+
+		/* Compute the bytes to copy to the main area. */
+
+		count = min(len, data_size - start);
+
+		/* Copy. */
+
+		nfc_util_copy_to_nfc_mem(buf, this->buffers + start, count);
+
+		buf   += count;
+		start += count;
+		len   -= count;
+
+	}
+
+	/* Check if we're done. */
+
+	if (!len)
+		return;
+
+	/* Compute the base address of the spare buffers. */
+
+	spare_base = this->buffers +
+			(this->nfc->max_buffer_count * NFC_MAIN_BUF_SIZE);
+
+	/* Discover in which spare buffer the copying begins. */
+
+	first_spare = (start - data_size) / geometry->spare_buf_size;
+
+	/* Check if anything goes to the regular spare buffer area. */
+
+	if (first_spare < geometry->buffer_count) {
+
+		/* Start copying to spare buffers. */
+
+		for (i = first_spare; i < geometry->buffer_count; i++) {
+
+			/* Compute the offset into this spare area. */
+
+			offset = start -
+				(data_size + (geometry->spare_buf_size * i));
+
+			/* Compute the address of that offset. */
+
+			to = spare_base + offset +
+					(this->nfc->spare_buf_stride * i);
+
+			/* Compute the bytes to copy to this spare area. */
+
+			count = min(len, geometry->spare_buf_size - offset);
+
+			/* Copy. */
+
+			nfc_util_copy_to_nfc_mem(buf, to, count);
+
+			buf   += count;
+			start += count;
+			len   -= count;
+
+		}
+
+	}
+
+	/* Check if we're done. */
+
+	if (!len)
+		return;
+
+	/* Compute the offset into the extra spare area. */
+
+	offset = start -
+		(data_size + (geometry->spare_buf_size*geometry->buffer_count));
+
+	/* Compute the address of that offset. */
+
+	to = spare_base + offset +
+			(this->nfc->spare_buf_stride * geometry->buffer_count);
+
+	/* Compute the bytes to copy to the extra spare area. */
+
+	count = min(len, geometry->spare_buf_spill - offset);
+
+	/* Copy. */
+
+	nfc_util_copy_to_nfc_mem(buf, to, count);
+
+}
+
+/**
+ * nfc_util_isr() - i.MX NFC ISR.
+ *
+ * @irq:      The arriving interrupt number.
+ * @context:  A cookie for this ISR.
+ */
+static irqreturn_t nfc_util_isr(int irq, void *cookie)
+{
+	struct imx_nfc_data  *this = cookie;
+	this->nfc->mask_interrupt(this);
+	this->nfc->clear_interrupt(this);
+	complete(&this->done);
+	return IRQ_HANDLED;
+}
+
+/**
+ * nfc_util_send_cmd() - Sends a command to the current chip, without waiting.
+ *
+ * @this:     Per-device data.
+ * @command:  The command code.
+ */
+
+static void nfc_util_send_cmd(struct imx_nfc_data *this, unsigned int command)
+{
+
+	add_event("Entering nfc_util_send_cmd", 1);
+
+	this->nfc->command_cycle(this, command);
+
+	add_event("Exiting nfc_util_send_cmd", -1);
+
+}
+
+/**
+ * nfc_util_send_cmd_and_addrs() - Sends a cmd and addrs to the current chip.
+ *
+ * This function conveniently combines sending a command, and then sending
+ * optional addresses, waiting for the NFC to finish will all steps.
+ *
+ * @this:     Per-device data.
+ * @command:  The command code.
+ * @column:   The column address to send, or -1 if no column address applies.
+ * @page:     The page address to send, or -1 if no page address applies.
+ */
+
+static void nfc_util_send_cmd_and_addrs(struct imx_nfc_data *this,
+					unsigned command, int column, int page)
+{
+	uint32_t  page_mask;
+
+	add_event("Entering nfc_util_send_cmd_and_addrs", 1);
+
+	/* Send the command.*/
+
+	add_event("Sending the command...", 0);
+
+	nfc_util_send_cmd(this, command);
+
+	nfc_util_wait_for_the_nfc(this, false);
+
+	/* Send the addresses. */
+
+	add_event("Sending the addresses...", 0);
+
+	if (column != -1) {
+
+		this->nfc->write_cycle(this, (column >> 0) & 0xff);
+
+		if (is_large_page_chip(this))
+			this->nfc->write_cycle(this, (column >> 8) & 0xff);
+
+	}
+
+	if (page != -1) {
+
+		page_mask = this->nand.pagemask;
+
+		do {
+			this->nfc->write_cycle(this, page & 0xff);
+			page_mask >>= 8;
+			page      >>= 8;
+		} while (page_mask != 0);
+
+	}
+
+	add_event("Exiting nfc_util_send_cmd_and_addrs", -1);
+
+}
+
+/**
+ * nfc_2_x_exit() - Version-specific shut down.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_2_x_exit(struct imx_nfc_data *this)
+{
+}
+
+/**
+ * nfc_2_x_clear_interrupt() - Clears an interrupt.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_2_x_clear_interrupt(struct imx_nfc_data *this)
+{
+	void  *base = this->primary_regs;
+	raw_clr_mask_w(NFC_2_X_CONFIG2_INT_MSK, base + NFC_2_X_CONFIG2_REG_OFF);
+}
+
+/**
+ * nfc_2_x_is_interrupting() - Returns the interrupt bit status.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_2_x_is_interrupting(struct imx_nfc_data *this)
+{
+	void  *base = this->primary_regs;
+	return raw_read_mask_w(NFC_2_X_CONFIG2_INT_MSK,
+						base + NFC_2_X_CONFIG2_REG_OFF);
+}
+
+/**
+ * nfc_2_x_command_cycle() - Sends a command.
+ *
+ * @this:     Per-device data.
+ * @command:  The command code.
+ */
+static void nfc_2_x_command_cycle(struct imx_nfc_data *this, unsigned command)
+{
+	void  *base = this->primary_regs;
+
+	/* Write the command we want to send. */
+
+	__raw_writew(command, base + NFC_2_X_FLASH_CMD_REG_OFF);
+
+	/* Launch a command cycle. */
+
+	__raw_writew(NFC_2_X_CONFIG2_FCMD_MSK, base + NFC_2_X_CONFIG2_REG_OFF);
+
+}
+
+/**
+ * nfc_2_x_write_cycle() - Writes a single byte.
+ *
+ * @this:  Per-device data.
+ * @byte:  The byte.
+ */
+static void nfc_2_x_write_cycle(struct imx_nfc_data *this, unsigned int byte)
+{
+	void  *base = this->primary_regs;
+
+	/* Give the NFC the byte we want to write. */
+
+	__raw_writew(byte, base + NFC_2_X_FLASH_ADDR_REG_OFF);
+
+	/* Launch an address cycle.
+	 *
+	 * This is *sort* of a hack, but not really. The intent of the NFC
+	 * design is for this operation to send an address byte. In fact, the
+	 * NFC neither knows nor cares what we're sending. It justs runs a write
+	 * cycle.
+	 */
+
+	__raw_writew(NFC_2_X_CONFIG2_FADD_MSK, base + NFC_2_X_CONFIG2_REG_OFF);
+
+	/* Wait for the NFC to finish. */
+
+	nfc_util_wait_for_the_nfc(this, false);
+
+}
+
+/**
+ * nfc_2_0_init() - Version-specific hardware initialization.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_2_0_init(struct imx_nfc_data *this)
+{
+	void  *base = this->primary_regs;
+
+	/* Initialize the interrupt machinery. */
+
+	this->nfc->mask_interrupt(this);
+	this->nfc->clear_interrupt(this);
+
+	/* Unlock the NFC memory. */
+
+	__raw_writew(0x2, base + NFC_2_X_CONFIG_REG_OFF);
+
+	/* Set the unlocked block range to cover the entire medium. */
+
+	__raw_writew(0 , base + NFC_2_0_UNLOCK_START_REG_OFF);
+	__raw_writew(~0, base + NFC_2_0_UNLOCK_END_REG_OFF);
+
+	/* Unlock all blocks. */
+
+	__raw_writew(0x4, base + NFC_2_X_WR_PROT_REG_OFF);
+
+	/* Return success. */
+
+	return 0;
+
+}
+
+/**
+ * nfc_2_0_mask_interrupt() - Masks interrupts.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_2_0_mask_interrupt(struct imx_nfc_data *this)
+{
+	void  *base = this->primary_regs;
+	raw_set_mask_w(NFC_2_0_CONFIG1_INT_MSK_MSK,
+						base + NFC_2_0_CONFIG1_REG_OFF);
+}
+
+/**
+ * nfc_2_0_unmask_interrupt() - Unmasks interrupts.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_2_0_unmask_interrupt(struct imx_nfc_data *this)
+{
+	void  *base = this->primary_regs;
+	raw_clr_mask_w(NFC_2_0_CONFIG1_INT_MSK_MSK,
+						base + NFC_2_0_CONFIG1_REG_OFF);
+}
+
+/**
+ * nfc_2_0_set_ecc() - Turns ECC on or off.
+ *
+ * @this:  Per-device data.
+ * @on:    Indicates if ECC should be on or off.
+ */
+static void nfc_2_0_set_ecc(struct imx_nfc_data *this, int on)
+{
+	void  *base = this->primary_regs;
+
+	if (on)
+		raw_set_mask_w(NFC_2_0_CONFIG1_ECC_EN_MSK,
+						base + NFC_2_0_CONFIG1_REG_OFF);
+	else
+		raw_clr_mask_w(NFC_2_0_CONFIG1_ECC_EN_MSK,
+						base + NFC_2_0_CONFIG1_REG_OFF);
+
+}
+
+/**
+ * nfc_2_0_get_ecc_status() - Reports ECC errors.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_2_0_get_ecc_status(struct imx_nfc_data *this)
+{
+	unsigned int  i;
+	void          *base = this->primary_regs;
+	uint16_t      status_reg;
+	unsigned int  buffer_status[4];
+	int           status;
+
+	/* Get the entire status register. */
+
+	status_reg = __raw_readw(base + NFC_2_0_ECC_STATUS_REG_OFF);
+
+	/* Pick out the status for each buffer. */
+
+	buffer_status[0] = (status_reg & NFC_2_0_ECC_STATUS_NOSER1_MSK)
+					>> NFC_2_0_ECC_STATUS_NOSER1_POS;
+
+	buffer_status[1] = (status_reg & NFC_2_0_ECC_STATUS_NOSER2_MSK)
+					>> NFC_2_0_ECC_STATUS_NOSER2_POS;
+
+	buffer_status[2] = (status_reg & NFC_2_0_ECC_STATUS_NOSER3_MSK)
+					>> NFC_2_0_ECC_STATUS_NOSER3_POS;
+
+	buffer_status[3] = (status_reg & NFC_2_0_ECC_STATUS_NOSER4_MSK)
+					>> NFC_2_0_ECC_STATUS_NOSER4_POS;
+
+	/* Loop through the buffers we're actually using. */
+
+	status = 0;
+
+	for (i = 0; i < this->nfc_geometry->buffer_count; i++) {
+
+		if (buffer_status[i] > this->nfc_geometry->ecc_strength) {
+			status = -1;
+			break;
+		}
+
+		status += buffer_status[i];
+
+	}
+
+	/* Return the final result. */
+
+	return status;
+
+}
+
+/**
+ * nfc_2_0_get_symmetric() - Indicates if the clock is symmetric.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_2_0_get_symmetric(struct imx_nfc_data *this)
+{
+	void  *base = this->primary_regs;
+
+	return !!raw_read_mask_w(NFC_2_0_CONFIG1_ONE_CYLE_MSK,
+						base + NFC_2_0_CONFIG1_REG_OFF);
+
+}
+
+/**
+ * nfc_2_0_set_symmetric() - Turns symmetric clock mode on or off.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_2_0_set_symmetric(struct imx_nfc_data *this, int on)
+{
+	void  *base = this->primary_regs;
+
+	if (on)
+		raw_set_mask_w(NFC_2_0_CONFIG1_ONE_CYLE_MSK,
+						base + NFC_2_0_CONFIG1_REG_OFF);
+	else
+		raw_clr_mask_w(NFC_2_0_CONFIG1_ONE_CYLE_MSK,
+						base + NFC_2_0_CONFIG1_REG_OFF);
+
+}
+
+/**
+ * nfc_2_0_set_geometry() - Configures for the medium geometry.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_2_0_set_geometry(struct imx_nfc_data *this)
+{
+	struct physical_geometry  *physical = &this->physical_geometry;
+
+	/* Select an NFC geometry. */
+
+	switch (physical->page_data_size) {
+
+	case 512:
+		this->nfc_geometry = &nfc_geometry_512_16_RS_ECC4;
+		break;
+
+	case 2048:
+		this->nfc_geometry = &nfc_geometry_2K_64_RS_ECC4;
+		break;
+
+	default:
+		dev_err(this->dev, "NFC can't handle page size: %u",
+						physical->page_data_size);
+		return !0;
+		break;
+
+	}
+
+	/*
+	 * This NFC version receives page size information from a register
+	 * that's external to the NFC. We must rely on platform-specific code
+	 * to set this register for us.
+	 */
+
+	return this->pdata->set_page_size(physical->page_data_size);
+
+}
+
+/**
+ * nfc_2_0_select_chip() - Selects the current chip.
+ *
+ * @this:  Per-device data.
+ * @chip:  The chip number to select, or -1 to select no chip.
+ */
+static void nfc_2_0_select_chip(struct imx_nfc_data *this, int chip)
+{
+}
+
+/**
+ * nfc_2_0_read_cycle() - Applies a single read cycle to the current chip.
+ *
+ * @this:  Per-device data.
+ */
+static unsigned int nfc_2_0_read_cycle(struct imx_nfc_data *this)
+{
+	uint8_t       byte;
+	unsigned int  result;
+	void          *base = this->primary_regs;
+
+	/* Read into main buffer 0. */
+
+	__raw_writew(0x0, base + NFC_2_0_BUF_ADDR_REG_OFF);
+
+	/* Launch a "Data Out" operation. */
+
+	__raw_writew(0x4 << NFC_2_X_CONFIG2_FDO_POS,
+						base + NFC_2_X_CONFIG2_REG_OFF);
+
+	/* Wait for the NFC to finish. */
+
+	nfc_util_wait_for_the_nfc(this, false);
+
+	/* Get the result from the NFC memory. */
+
+	nfc_util_copy_from_the_nfc(this, 0, &byte, 1);
+	result = byte;
+
+	/* Return the results. */
+
+	return result;
+
+}
+
+/**
+ * nfc_2_0_read_page() - Reads a page from the current chip into the NFC.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_2_0_read_page(struct imx_nfc_data *this)
+{
+	unsigned int  i;
+	void          *base = this->primary_regs;
+
+	/* Loop over the number of buffers in use. */
+
+	for (i = 0; i < this->nfc_geometry->buffer_count; i++) {
+
+		/* Make the NFC read into the current buffer. */
+
+		__raw_writew(i << NFC_2_0_BUF_ADDR_RBA_POS,
+					base + NFC_2_0_BUF_ADDR_REG_OFF);
+
+		/* Launch a page data out operation. */
+
+		__raw_writew(0x1 << NFC_2_X_CONFIG2_FDO_POS,
+					base + NFC_2_X_CONFIG2_REG_OFF);
+
+		/* Wait for the NFC to finish. */
+
+		nfc_util_wait_for_the_nfc(this, true);
+
+	}
+
+}
+
+/**
+ * nfc_2_0_send_page() - Sends a page from the NFC to the current chip.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_2_0_send_page(struct imx_nfc_data *this)
+{
+	unsigned int  i;
+	void          *base = this->primary_regs;
+
+	/* Loop over the number of buffers in use. */
+
+	for (i = 0; i < this->nfc_geometry->buffer_count; i++) {
+
+		/* Make the NFC send from the current buffer. */
+
+		__raw_writew(i << NFC_2_0_BUF_ADDR_RBA_POS,
+					base + NFC_2_0_BUF_ADDR_REG_OFF);
+
+		/* Launch a page data in operation. */
+
+		__raw_writew(0x1 << NFC_2_X_CONFIG2_FDI_POS,
+					base + NFC_2_X_CONFIG2_REG_OFF);
+
+		/* Wait for the NFC to finish. */
+
+		nfc_util_wait_for_the_nfc(this, true);
+
+	}
+
+}
+
+/**
+ * nfc_3_2_init() - Hardware initialization.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_3_2_init(struct imx_nfc_data *this)
+{
+	int           error;
+	unsigned int  no_sdma;
+	unsigned int  fmp;
+	unsigned int  rbb_mode;
+	unsigned int  num_of_devices;
+	unsigned int  dma_mode;
+	unsigned int  sbb;
+	unsigned int  nf_big;
+	unsigned int  sb2r;
+	unsigned int  fw;
+	unsigned int  too;
+	unsigned int  add_op;
+	uint32_t      config3;
+	void          *primary_base   = this->primary_regs;
+	void          *secondary_base = this->secondary_regs;
+
+	/* Initialize the interrupt machinery. */
+
+	this->nfc->mask_interrupt(this);
+	this->nfc->clear_interrupt(this);
+
+	/* Set up the clock. */
+
+	error = this->nfc->set_closest_cycle(this,
+					this->pdata->target_cycle_in_ns);
+
+	if (error)
+		return !0;
+
+	/* We never read the spare area alone. */
+
+	raw_clr_mask_l(NFC_3_2_CONFIG1_SP_EN_MSK,
+				primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	/* Tell the NFC the "Read Status" command code. */
+
+	raw_clr_mask_l(NFC_3_2_CONFIG2_ST_CMD_MSK,
+				secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+
+	raw_set_mask_l(NAND_CMD_STATUS << NFC_3_2_CONFIG2_ST_CMD_POS,
+				secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+
+	/*
+	 * According to erratum ENGcm09051, the CONFIG3 register doesn't reset
+	 * correctly, so we need to re-build the entire register just in case.
+	 */
+
+	/*
+	 * Set the NO_SDMA bit to tell the NFC that we are NOT using SDMA. If
+	 * you clear this bit (to indicates you *are* using SDMA), but you
+	 * don't actually set up SDMA, the NFC has been observed to crash the
+	 * hardware when it asserts its DMA request signals. In the future, we
+	 * *may* use SDMA, but it's not worth the effort at this writing.
+	 */
+
+	no_sdma = 0x1;
+
+	/*
+	 * Set the default FIFO Mode Protection (128 bytes). FMP doesn't work if
+	 * the NO_SDMA bit is set.
+	 */
+
+	fmp = 0x2;
+
+	/*
+	 * The rbb_mode bit determines how the NFC figures out whether chips are
+	 * ready during automatic operations only (this has no effect on atomic
+	 * operations). The two choices are either to monitor the ready/busy
+	 * signals, or to read the status register. We monitor the ready/busy
+	 * signals.
+	 */
+
+	rbb_mode = 0x1;
+
+	/*
+	 * We don't yet know how many devices are connected. We'll find out in
+	 * out in nfc_3_2_set_geometry().
+	 */
+
+	num_of_devices = 0;
+
+	/* Set the default DMA mode. */
+
+	dma_mode = 0x0;
+
+	/* Set the default status busy bit. */
+
+	sbb = 0x6;
+
+	/* Little-endian (the default). */
+
+	nf_big = 0x0;
+
+	/* Set the default (standard) status bit to record. */
+
+	sb2r = 0x0;
+
+	/* We support only 8-bit Flash bus width. */
+
+	fw = 0x1;
+
+	/* We don't support "two-on-one." */
+
+	too = 0x0;
+
+	/* Set the addressing option. */
+
+	add_op = 0x3;
+
+	/* Set the CONFIG3 register. */
+
+	config3 = 0;
+
+	config3 |= no_sdma        << NFC_3_2_CONFIG3_NO_SDMA_POS;
+	config3 |= fmp            << NFC_3_2_CONFIG3_FMP_POS;
+	config3 |= rbb_mode       << NFC_3_2_CONFIG3_RBB_MODE_POS;
+	config3 |= num_of_devices << NFC_3_2_CONFIG3_NUM_OF_DEVICES_POS;
+	config3 |= dma_mode       << NFC_3_2_CONFIG3_DMA_MODE_POS;
+	config3 |= sbb            << NFC_3_2_CONFIG3_SBB_POS;
+	config3 |= nf_big         << NFC_3_2_CONFIG3_NF_BIG_POS;
+	config3 |= sb2r           << NFC_3_2_CONFIG3_SB2R_POS;
+	config3 |= fw             << NFC_3_2_CONFIG3_FW_POS;
+	config3 |= too            << NFC_3_2_CONFIG3_TOO_POS;
+	config3 |= add_op         << NFC_3_2_CONFIG3_ADD_OP_POS;
+
+	__raw_writel(config3, secondary_base + NFC_3_2_CONFIG3_REG_OFF);
+
+	/* Return success. */
+
+	return 0;
+
+}
+
+/**
+ * nfc_3_2_set_geometry() - Configures for the medium geometry.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_3_2_set_geometry(struct imx_nfc_data *this)
+{
+	unsigned int              ps;
+	unsigned int              cmd_phases;
+	unsigned int              pages_per_chip;
+	unsigned int              addr_phases0;
+	unsigned int              addr_phases1;
+	unsigned int              pages_per_block;
+	unsigned int              ecc_mode;
+	unsigned int              ppb;
+	unsigned int              spas;
+	unsigned int              mask;
+	uint32_t                  config2;
+	unsigned int              num_of_devices;
+	uint32_t                  config3;
+	unsigned int              x;
+	unsigned int              chip;
+	struct physical_geometry  *physical = &this->physical_geometry;
+	void                      *secondary_base = this->secondary_regs;
+
+	/*
+	 * Select an NFC geometry based on the physical geometry and the
+	 * capabilities of this NFC.
+	 */
+
+	switch (physical->page_data_size) {
+
+	case 512:
+		this->nfc_geometry = &nfc_geometry_512_16_BCH_ECC4;
+		ps = 0;
+		break;
+
+	case 2048:
+		this->nfc_geometry = &nfc_geometry_2K_64_BCH_ECC4;
+		ps = 1;
+		break;
+
+	case 4096:
+
+		switch (this->physical_geometry.page_oob_size) {
+
+		case 128:
+			this->nfc_geometry = &nfc_geometry_4K_128_BCH_ECC4;
+			break;
+
+		case 218:
+			this->nfc_geometry = &nfc_geometry_4K_218_BCH_ECC8;
+			break;
+
+		default:
+			dev_err(this->dev,
+				"NFC can't handle page geometry: %u+%u",
+				physical->page_data_size,
+				physical->page_oob_size);
+			return !0;
+			break;
+
+		}
+
+		ps = 2;
+
+		break;
+
+	default:
+		dev_err(this->dev, "NFC can't handle page size: %u",
+						physical->page_data_size);
+		return !0;
+		break;
+
+	}
+
+	/* Compute the ECC mode. */
+
+	switch (this->nfc_geometry->ecc_strength) {
+
+	case 4:
+		ecc_mode = 0;
+		break;
+
+	case 8:
+		ecc_mode = 1;
+		break;
+
+	default:
+		dev_err(this->dev, "NFC can't handle ECC strength: %u",
+					this->nfc_geometry->ecc_strength);
+		return !0;
+		break;
+
+	}
+
+	/* Compute the pages per block. */
+
+	pages_per_block = physical->block_size / physical->page_data_size;
+
+	switch (pages_per_block) {
+	case 32:
+		ppb = 0;
+		break;
+	case 64:
+		ppb = 1;
+		break;
+	case 128:
+		ppb = 2;
+		break;
+	case 256:
+		ppb = 3;
+		break;
+	default:
+		dev_err(this->dev, "NFC can't handle pages per block: %d",
+							pages_per_block);
+		return !0;
+		break;
+	}
+
+	/*
+	 * The hardware needs to know the physical size of the spare area, in
+	 * units of half-words (16 bits). This may be different from the amount
+	 * of the spare area we actually expose to MTD. For example, for for
+	 * 2K+112, we only expose 64 spare bytes, but the hardware needs to know
+	 * the real facts.
+	 */
+
+	spas = this->physical_geometry.page_oob_size >> 1;
+
+	/*
+	 * The number of command phases needed to read a page is directly
+	 * dependent on whether this is a small page or large page device. Large
+	 * page devices need more address phases, terminated by a second command
+	 * phase.
+	 */
+
+	cmd_phases = is_large_page_chip(this) ? 1 : 0;
+
+	/*
+	 * The num_adr_phases1 field contains the number of phases needed to
+	 * transmit addresses for read and program operations. This is the sum
+	 * of the number of phases for a page address and the number of phases
+	 * for a column address.
+	 *
+	 * The number of phases for a page address is the number of bytes needed
+	 * to contain a page address.
+	 *
+	 * The number of phases for a column address is the number of bytes
+	 * needed to contain a column address.
+	 *
+	 * After computing the sum, we subtract three because a value of zero in
+	 * this field indicates three address phases, and this is the minimum
+	 * number of phases the hardware can comprehend.
+	 *
+	 * We compute the number of phases based on the *physical* geometry, not
+	 * the NFC geometry. For example, even if we are treating a very large
+	 * device as if it contains fewer pages than it actually does, the
+	 * hardware still needs the additional address phases.
+	 */
+
+	pages_per_chip =
+		physical->chip_size >> (fls(physical->page_data_size) - 1);
+
+	addr_phases1 = (fls(pages_per_chip) >> 3) + 1;
+
+	addr_phases1 += (fls(physical->page_data_size) >> 3) + 1;
+
+	addr_phases1 -= 3;
+
+	/*
+	 * The num_adr_phases0 field contains the number of phases needed to
+	 * transmit a page address for an erase operation. That is, this is
+	 * the value of addr_phases1, less the number of phases for the column
+	 * address.
+	 *
+	 * The hardware expresses this phase count as one or two cycles less
+	 * than the count indicated by add_phases1 (see the reference manual).
+	 */
+
+	addr_phases0 = is_large_page_chip(this) ? 1 : 0;
+
+	/* Set the CONFIG2 register. */
+
+	mask =
+		NFC_3_2_CONFIG2_PS_MSK           |
+		NFC_3_2_CONFIG2_CMD_PHASES_MSK   |
+		NFC_3_2_CONFIG2_ADDR_PHASES0_MSK |
+		NFC_3_2_CONFIG2_ECC_MODE_MSK     |
+		NFC_3_2_CONFIG2_PPB_MSK          |
+		NFC_3_2_CONFIG2_ADDR_PHASES1_MSK |
+		NFC_3_2_CONFIG2_SPAS_MSK         ;
+
+	config2 = __raw_readl(secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+
+	config2 &= ~mask;
+
+	config2 |= ps           << NFC_3_2_CONFIG2_PS_POS;
+	config2 |= cmd_phases   << NFC_3_2_CONFIG2_CMD_PHASES_POS;
+	config2 |= addr_phases0 << NFC_3_2_CONFIG2_ADDR_PHASES0_POS;
+	config2 |= ecc_mode     << NFC_3_2_CONFIG2_ECC_MODE_POS;
+	config2 |= ppb          << NFC_3_2_CONFIG2_PPB_POS;
+	config2 |= addr_phases1 << NFC_3_2_CONFIG2_ADDR_PHASES1_POS;
+	config2 |= spas         << NFC_3_2_CONFIG2_SPAS_POS;
+
+	config2 = __raw_writel(config2,
+				secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+
+	/*
+	 * Compute the num_of_devices field.
+	 *
+	 * It's very important to set this field correctly. This controls the
+	 * set of ready/busy lines to which the NFC listens with automatic
+	 * transactions. If this number is too large, the NFC will listen to
+	 * ready/busy signals that are electrically floating, or it will try to
+	 * read the status registers of chips that don't exist. Conversely, if
+	 * the number is too small, the NFC could believe an operation is
+	 * finished when some chips are still busy.
+	 */
+
+	num_of_devices = physical->chip_count - 1;
+
+	/* Set the CONFIG3 register. */
+
+	mask = NFC_3_2_CONFIG3_NUM_OF_DEVICES_MSK;
+
+	config3 = __raw_readl(secondary_base + NFC_3_2_CONFIG3_REG_OFF);
+
+	config3 &= ~mask;
+
+	config3 |= num_of_devices << NFC_3_2_CONFIG3_NUM_OF_DEVICES_POS;
+
+	__raw_writel(config3, secondary_base + NFC_3_2_CONFIG3_REG_OFF);
+
+	/*
+	 * Check if the physical chip count is a power of 2. If not, then
+	 * automatic operations aren't available. This is because we use an
+	 * addressing option (see the ADD_OP field of CONFIG3) that requires
+	 * a number of chips that is a power of 2.
+	 */
+
+	if (ffs(physical->chip_count) != fls(physical->chip_count)) {
+		this->nfc->start_auto_read  = 0;
+		this->nfc->start_auto_write = 0;
+		this->nfc->start_auto_erase = 0;
+	}
+
+	/* Unlock the NFC RAM. */
+
+	x = __raw_readl(secondary_base + NFC_3_2_WRPROT_REG_OFF);
+	x &= ~NFC_3_2_WRPROT_BLS_MSK;
+	x |= 0x2  << NFC_3_2_WRPROT_BLS_POS;
+	__raw_writel(x, secondary_base + NFC_3_2_WRPROT_REG_OFF);
+
+	/* Loop over chip selects, setting the unlocked ranges. */
+
+	for (chip = 0; chip < this->nfc->max_chip_count; chip++) {
+
+		/* Set the unlocked range to cover the entire chip.*/
+
+		__raw_writel(0xffff0000, secondary_base +
+				NFC_3_2_UNLOCK_BLK_ADD0_REG_OFF + (chip * 4));
+
+		/* Unlock. */
+
+		x = __raw_readl(secondary_base + NFC_3_2_WRPROT_REG_OFF);
+		x &= ~(NFC_3_2_WRPROT_CS2L_MSK | NFC_3_2_WRPROT_WPC_MSK);
+		x |= chip << NFC_3_2_WRPROT_CS2L_POS;
+		x |= 0x4  << NFC_3_2_WRPROT_WPC_POS ;
+		__raw_writel(x, secondary_base + NFC_3_2_WRPROT_REG_OFF);
+
+	}
+
+	/* Return success. */
+
+	return 0;
+
+}
+
+/**
+ * nfc_3_2_exit() - Hardware cleanup.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_3_2_exit(struct imx_nfc_data *this)
+{
+}
+
+/**
+ * nfc_3_2_set_closest_cycle() - Version-specific hardware cleanup.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_3_2_set_closest_cycle(struct imx_nfc_data *this, unsigned ns)
+{
+	struct clk     *parent_clock;
+	unsigned long  parent_clock_rate_in_hz;
+	unsigned long  sym_low_clock_rate_in_hz;
+	unsigned long  asym_low_clock_rate_in_hz;
+	unsigned int   sym_high_cycle_in_ns;
+	unsigned int   asym_high_cycle_in_ns;
+
+	/*
+	 * According to ENGcm09121:
+	 *
+	 *  - If the NFC is set to SYMMETRIC mode, the NFC clock divider must
+	 *    divide the EMI Slow Clock by NO MORE THAN 4.
+	 *
+	 *  - If the NFC is set for ASYMMETRIC mode, the NFC clock divider must
+	 *    divide the EMI Slow Clock by NO MORE THAN 3.
+	 *
+	 * We need to compute the corresponding cycle time constraints. Start
+	 * by getting information about the parent clock.
+	 */
+
+	parent_clock            = clk_get_parent(this->clock);
+	parent_clock_rate_in_hz = clk_get_rate(parent_clock);
+
+	/* Compute the limit frequencies. */
+
+	sym_low_clock_rate_in_hz  = parent_clock_rate_in_hz / 4;
+	asym_low_clock_rate_in_hz = parent_clock_rate_in_hz / 3;
+
+	/* Compute the corresponding limit cycle periods. */
+
+	sym_high_cycle_in_ns  = 1000000000 / sym_low_clock_rate_in_hz;
+	asym_high_cycle_in_ns = (1000000000 / asym_low_clock_rate_in_hz) * 2;
+
+	/* Attempt to implement the given cycle. */
+
+	return nfc_util_set_best_cycle(this, ns,
+			ns > asym_high_cycle_in_ns, ns > sym_high_cycle_in_ns);
+
+}
+
+/**
+ * nfc_3_2_mask_interrupt() - Masks interrupts.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_3_2_mask_interrupt(struct imx_nfc_data *this)
+{
+	void  *secondary_base = this->secondary_regs;
+	raw_set_mask_l(NFC_3_2_CONFIG2_INT_MSK_MSK,
+				secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+}
+
+/**
+ * nfc_3_2_unmask_interrupt() - Unmasks interrupts.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_3_2_unmask_interrupt(struct imx_nfc_data *this)
+{
+	void  *secondary_base = this->secondary_regs;
+	raw_clr_mask_l(NFC_3_2_CONFIG2_INT_MSK_MSK,
+				secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+}
+
+/**
+ * nfc_3_2_clear_interrupt() - Clears an interrupt.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_3_2_clear_interrupt(struct imx_nfc_data *this)
+{
+	int   done;
+	void  *secondary_base = this->secondary_regs;
+
+	/* Request IP bus interface access. */
+
+	raw_set_mask_l(NFC_3_2_IPC_CREQ_MSK,
+					secondary_base + NFC_3_2_IPC_REG_OFF);
+
+	/* Wait for access. */
+
+	do
+		done = !!raw_read_mask_l(NFC_3_2_IPC_CACK_MSK,
+					secondary_base + NFC_3_2_IPC_REG_OFF);
+	while (!done);
+
+	/* Clear the interrupt. */
+
+	raw_clr_mask_l(NFC_3_2_IPC_INT_MSK,
+					secondary_base + NFC_3_2_IPC_REG_OFF);
+
+	/* Release the IP bus interface. */
+
+	raw_clr_mask_l(NFC_3_2_IPC_CREQ_MSK,
+					secondary_base + NFC_3_2_IPC_REG_OFF);
+
+}
+
+/**
+ * nfc_3_2_is_interrupting() - Returns the interrupt bit status.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_3_2_is_interrupting(struct imx_nfc_data *this)
+{
+	void  *secondary_base = this->secondary_regs;
+	return !!raw_read_mask_l(NFC_3_2_IPC_INT_MSK,
+					secondary_base + NFC_3_2_IPC_REG_OFF);
+}
+
+/**
+ * nfc_3_2_is_ready() - Returns the ready/busy status.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_3_2_is_ready(struct imx_nfc_data *this)
+{
+	void  *secondary_base = this->secondary_regs;
+	return !!raw_read_mask_l(NFC_3_2_IPC_RB_B_MSK,
+					secondary_base + NFC_3_2_IPC_REG_OFF);
+}
+
+/**
+ * nfc_3_2_set_force_ce() - Can force CE to be asserted always.
+ *
+ * @this:  Per-device data.
+ * @on:    Indicates if the hardware CE signal should be asserted always.
+ */
+static void nfc_3_2_set_force_ce(struct imx_nfc_data *this, int on)
+{
+	void  *primary_base = this->primary_regs;
+
+	if (on)
+		raw_set_mask_l(NFC_3_2_CONFIG1_NF_CE_MSK,
+					primary_base + NFC_3_2_CONFIG1_REG_OFF);
+	else
+		raw_clr_mask_l(NFC_3_2_CONFIG1_NF_CE_MSK,
+					primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+}
+
+/**
+ * nfc_3_2_set_ecc() - Turns ECC on or off.
+ *
+ * @this:  Per-device data.
+ * @on:    Indicates if ECC should be on or off.
+ */
+static void nfc_3_2_set_ecc(struct imx_nfc_data *this, int on)
+{
+	void  *secondary_base = this->secondary_regs;
+
+	if (on)
+		raw_set_mask_l(NFC_3_2_CONFIG2_ECC_EN_MSK,
+				secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+	else
+		raw_clr_mask_l(NFC_3_2_CONFIG2_ECC_EN_MSK,
+				secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+
+}
+
+/**
+ * nfc_3_2_get_ecc_status() - Reports ECC errors.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_3_2_get_ecc_status(struct imx_nfc_data *this)
+{
+	unsigned int  i;
+	void          *base = this->primary_regs;
+	uint16_t      status_reg;
+	unsigned int  buffer_status[8];
+	int           status;
+
+	/* Get the entire status register. */
+
+	status_reg = __raw_readw(base + NFC_3_2_ECC_STATUS_REG_OFF);
+
+	/* Pick out the status for each buffer. */
+
+	buffer_status[0] = (status_reg & NFC_3_2_ECC_STATUS_NOBER1_MSK)
+					>> NFC_3_2_ECC_STATUS_NOBER1_POS;
+
+	buffer_status[1] = (status_reg & NFC_3_2_ECC_STATUS_NOBER2_MSK)
+					>> NFC_3_2_ECC_STATUS_NOBER2_POS;
+
+	buffer_status[2] = (status_reg & NFC_3_2_ECC_STATUS_NOBER3_MSK)
+					>> NFC_3_2_ECC_STATUS_NOBER3_POS;
+
+	buffer_status[3] = (status_reg & NFC_3_2_ECC_STATUS_NOBER4_MSK)
+					>> NFC_3_2_ECC_STATUS_NOBER4_POS;
+
+	buffer_status[4] = (status_reg & NFC_3_2_ECC_STATUS_NOBER5_MSK)
+					>> NFC_3_2_ECC_STATUS_NOBER5_POS;
+
+	buffer_status[5] = (status_reg & NFC_3_2_ECC_STATUS_NOBER6_MSK)
+					>> NFC_3_2_ECC_STATUS_NOBER6_POS;
+
+	buffer_status[6] = (status_reg & NFC_3_2_ECC_STATUS_NOBER7_MSK)
+					>> NFC_3_2_ECC_STATUS_NOBER7_POS;
+
+	buffer_status[7] = (status_reg & NFC_3_2_ECC_STATUS_NOBER8_MSK)
+					>> NFC_3_2_ECC_STATUS_NOBER8_POS;
+
+	/* Loop through the buffers we're actually using. */
+
+	status = 0;
+
+	for (i = 0; i < this->nfc_geometry->buffer_count; i++) {
+
+		if (buffer_status[i] > this->nfc_geometry->ecc_strength) {
+			status = -1;
+			break;
+		}
+
+		status += buffer_status[i];
+
+	}
+
+	/* Return the final result. */
+
+	return status;
+
+}
+
+/**
+ * nfc_3_2_get_symmetric() - Indicates if the clock is symmetric.
+ *
+ * @this:  Per-device data.
+ */
+static int nfc_3_2_get_symmetric(struct imx_nfc_data *this)
+{
+	void  *secondary_base = this->secondary_regs;
+
+	return !!raw_read_mask_w(NFC_3_2_CONFIG2_SYM_MSK,
+				secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+
+}
+
+/**
+ * nfc_3_2_set_symmetric() - Turns symmetric clock mode on or off.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_3_2_set_symmetric(struct imx_nfc_data *this, int on)
+{
+	void  *secondary_base = this->secondary_regs;
+
+	if (on)
+		raw_set_mask_l(NFC_3_2_CONFIG2_SYM_MSK,
+				secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+	else
+		raw_clr_mask_l(NFC_3_2_CONFIG2_SYM_MSK,
+				secondary_base + NFC_3_2_CONFIG2_REG_OFF);
+
+}
+
+/**
+ * nfc_3_2_select_chip() - Selects the current chip.
+ *
+ * @this:  Per-device data.
+ * @chip:  The chip number to select, or -1 to select no chip.
+ */
+static void nfc_3_2_select_chip(struct imx_nfc_data *this, int chip)
+{
+	unsigned long  x;
+	void           *primary_base = this->primary_regs;
+
+	if (chip < 0)
+		return;
+
+	x = __raw_readl(primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	x &= ~NFC_3_2_CONFIG1_CS_MSK;
+
+	x |= (chip << NFC_3_2_CONFIG1_CS_POS) & NFC_3_2_CONFIG1_CS_MSK;
+
+	__raw_writel(x, primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+}
+
+/**
+ * nfc_3_2_command_cycle() - Sends a command.
+ *
+ * @this:     Per-device data.
+ * @command:  The command code.
+ */
+static void nfc_3_2_command_cycle(struct imx_nfc_data *this, unsigned command)
+{
+	void  *primary_base = this->primary_regs;
+
+	/* Write the command we want to send. */
+
+	__raw_writel(command, primary_base + NFC_3_2_CMD_REG_OFF);
+
+	/* Launch a command cycle. */
+
+	__raw_writel(NFC_3_2_LAUNCH_FCMD_MSK,
+					primary_base + NFC_3_2_LAUNCH_REG_OFF);
+
+}
+
+/**
+ * nfc_3_2_write_cycle() - writes a single byte.
+ *
+ * @this:  Per-device data.
+ * @byte:  The byte.
+ */
+static void nfc_3_2_write_cycle(struct imx_nfc_data *this, unsigned int byte)
+{
+	void  *primary_base = this->primary_regs;
+
+	/* Give the NFC the byte we want to write. */
+
+	__raw_writel(byte, primary_base + NFC_3_2_ADD0_REG_OFF);
+
+	/* Launch an address cycle.
+	 *
+	 * This is *sort* of a hack, but not really. The intent of the NFC
+	 * design is for this operation to send an address byte. In fact, the
+	 * NFC neither knows nor cares what we're sending. It justs runs a write
+	 * cycle.
+	 */
+
+	__raw_writel(NFC_3_2_LAUNCH_FADD_MSK,
+					primary_base + NFC_3_2_LAUNCH_REG_OFF);
+
+	/* Wait for the NFC to finish. */
+
+	nfc_util_wait_for_the_nfc(this, false);
+
+}
+
+/**
+ * nfc_3_2_read_cycle() - Applies a single read cycle to the current chip.
+ *
+ * @this:  Per-device data.
+ */
+static unsigned int nfc_3_2_read_cycle(struct imx_nfc_data *this)
+{
+	unsigned int  result;
+	void          *primary_base = this->primary_regs;
+
+	/* Launch a "Data Out" operation. */
+
+	__raw_writel(0x4 << NFC_3_2_LAUNCH_FDO_POS,
+					primary_base + NFC_3_2_LAUNCH_REG_OFF);
+
+	/* Wait for the NFC to finish. */
+
+	nfc_util_wait_for_the_nfc(this, false);
+
+	/* Get the result. */
+
+	result = __raw_readl(primary_base + NFC_3_2_CONFIG1_REG_OFF)
+						>> NFC_3_2_CONFIG1_STATUS_POS;
+	result &= 0xff;
+
+	/* Return the result. */
+
+	return result;
+
+}
+
+/**
+ * nfc_3_2_read_page() - Reads a page into the NFC memory.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_3_2_read_page(struct imx_nfc_data *this)
+{
+	void  *primary_base = this->primary_regs;
+
+	/* Start reading into buffer 0. */
+
+	raw_clr_mask_l(NFC_3_2_CONFIG1_RBA_MSK,
+					primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	/* Launch a page data out operation. */
+
+	__raw_writel(0x1 << NFC_3_2_LAUNCH_FDO_POS,
+					primary_base + NFC_3_2_LAUNCH_REG_OFF);
+
+	/* Wait for the NFC to finish. */
+
+	nfc_util_wait_for_the_nfc(this, true);
+
+}
+
+/**
+ * nfc_3_2_send_page() - Sends a page from the NFC to the current chip.
+ *
+ * @this:  Per-device data.
+ */
+static void nfc_3_2_send_page(struct imx_nfc_data *this)
+{
+	void  *primary_base = this->primary_regs;
+
+	/* Start sending from buffer 0. */
+
+	raw_clr_mask_l(NFC_3_2_CONFIG1_RBA_MSK,
+					primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	/* Launch a page data in operation. */
+
+	__raw_writel(NFC_3_2_LAUNCH_FDI_MSK,
+					primary_base + NFC_3_2_LAUNCH_REG_OFF);
+
+	/* Wait for the NFC to finish. */
+
+	nfc_util_wait_for_the_nfc(this, true);
+
+}
+
+/**
+ * nfc_3_2_add_state_events() - Adds events to display important state.
+ *
+ * @this:   Per-device data.
+ */
+static void nfc_3_2_add_state_events(struct imx_nfc_data *this)
+{
+#ifdef EVENT_REPORTING
+	void  *secondary_base = this->secondary_regs;
+
+	add_state_event_l
+		(
+		secondary_base + NFC_3_2_IPC_REG_OFF,
+		NFC_3_2_IPC_INT_MSK,
+		"  Interrupt     : 0",
+		"  Interrupt     : X"
+		);
+
+	add_state_event_l
+		(
+		secondary_base + NFC_3_2_IPC_REG_OFF,
+		NFC_3_2_IPC_AUTO_PROG_DONE_MSK,
+		"  auto_prog_done: 0",
+		"  auto_prog_done: X"
+		);
+
+	add_state_event_l
+		(
+		secondary_base + NFC_3_2_IPC_REG_OFF,
+		NFC_3_2_IPC_RB_B_MSK,
+		"  Medium        : Busy",
+		"  Medium        : Ready"
+		);
+#endif
+}
+
+/**
+ * nfc_3_2_get_auto_loop_params() - Gets automatic operation loop parameters.
+ *
+ * This function and the corresponding "setter" enable the automatic operations
+ * to keep some state as they iterate over chips.
+ *
+ * The most "obvious" way to save state would be to allocate a private data
+ * structure and hang it off the owning struct nfc_hal. On the other hand,
+ * writing the code to allocate the memory and then release it when the NFC
+ * shuts down is annoying - and we have some perfectly good memory in the NFC
+ * hardware that we can use. Since we only use two commands at a time, we can
+ * stash our loop limits and loop index in the top 16 bits of the NAND_CMD
+ * register. To paraphrase the reference manual:
+ *
+ *
+ * NAND_CMD
+ *
+ * |<--  4 bits  -->|<--  4 bits -->|<--          8 bits          -->|
+ * +----------------+---------------+--------------------------------+
+ * |     First      |     Last      |           Loop Index           |
+ * +----------------+---------------+--------------------------------+
+ * |         NAND COMMAND1          |         NAND COMMAND0          |
+ * +--------------------------------+--------------------------------+
+ * |<--         16 bits          -->|<--         16 bits          -->|
+ *
+ *
+ * @this:   Per-device data.
+ * @first:  A pointer to a variable that will receive the first chip number.
+ * @last:   A pointer to a variable that will receive the last chip number.
+ * @index:  A pointer to a variable that will receive the current chip number.
+ */
+static void nfc_3_2_get_auto_loop_params(struct imx_nfc_data *this,
+			unsigned *first, unsigned *last, unsigned *index)
+{
+	uint32_t  x;
+	void      *primary_base = this->primary_regs;
+
+	x = __raw_readl(primary_base + NFC_3_2_CMD_REG_OFF);
+
+	*first = (x >> 28) & 0x0f;
+	*last  = (x >> 24) & 0x0f;
+	*index = (x >> 16) & 0xff;
+
+}
+
+/**
+ * nfc_3_2_set_auto_loop_params() - Sets automatic operation loop parameters.
+ *
+ * See nfc_3_2_get_auto_loop_params() for detailed information about these
+ * functions.
+ *
+ * @this:   Per-device data.
+ * @first:  The first chip number.
+ * @last:   The last chip number.
+ * @index:  The current chip number.
+ */
+static void nfc_3_2_set_auto_loop_params(struct imx_nfc_data *this,
+				unsigned first, unsigned last, unsigned index)
+{
+	uint32_t  x;
+	void      *primary_base = this->primary_regs;
+
+	x = __raw_readl(primary_base + NFC_3_2_CMD_REG_OFF);
+
+	x &= 0x0000ffff;
+	x |= (first & 0x0f) << 28;
+	x |= (last  & 0x0f) << 24;
+	x |= (index & 0xff) << 16;
+
+	__raw_writel(x, primary_base + NFC_3_2_CMD_REG_OFF);
+
+}
+
+/**
+ * nfc_3_2_get_auto_addresses() - Gets automatic operation addresses.
+ *
+ * @this:    Per-device data.
+ * @group:   The address group number.
+ * @chip:    A pointer to a variable that will receive the chip number.
+ * @column:  A pointer to a variable that will receive the column address.
+ *           A NULL pointer indicates there is no column address.
+ * @page:    A pointer to a variable that will receive the page address.
+ */
+static void nfc_3_2_get_auto_addresses(struct imx_nfc_data *this,
+	unsigned group, unsigned *chip, unsigned *column, unsigned *page)
+{
+	uint32_t                  x;
+	unsigned int              chip_count;
+	unsigned int              cs_width;
+	unsigned int              cs_mask;
+	unsigned int              page_lsbs;
+	unsigned int              page_msbs;
+	uint32_t                  *low;
+	uint16_t                  *high;
+	void                      *primary_base   = this->primary_regs;
+	void                      *secondary_base = this->secondary_regs;
+
+	/*
+	 * The width of the chip select field depends on the number of connected
+	 * chips.
+	 *
+	 * Notice that these computations work only if the number of chips is a
+	 * power of 2. In fact, that is a fundamental limitation for using
+	 * automatic operations.
+	 */
+
+	x = __raw_readl(secondary_base + NFC_3_2_CONFIG3_REG_OFF);
+
+	chip_count =
+		(x & NFC_3_2_CONFIG3_NUM_OF_DEVICES_MSK) >>
+					NFC_3_2_CONFIG3_NUM_OF_DEVICES_POS;
+	chip_count++;
+
+	cs_width = ffs(chip_count) - 1;
+	cs_mask  = chip_count - 1;
+
+	/* Construct pointers to the pieces of the given address group. */
+
+	low = primary_base + NFC_3_2_ADD0_REG_OFF;
+	low += group;
+
+	high = primary_base + NFC_3_2_ADD8_REG_OFF;
+	high += group;
+
+	/* Check if there's a column address. */
+
+	if (column) {
+
+		/*
+		 * The low 32 bits of the address group look like this:
+		 *
+		 *      16 - n     n
+		 * | <-  bits  ->|<->|<-  16 bits   ->|
+		 * +-------------+---+----------------+
+		 * |  Page LSBs  |CS |     Column     |
+		 * +-------------+---+----------------+
+		 */
+
+		x = __raw_readl(low);
+
+		*column   = x & 0xffff;
+		*chip     = (x >> 16) & cs_mask;
+		page_lsbs = x >> (16 + cs_width);
+
+		/* The high 16 bits contain the MSB's of the page address. */
+
+		page_msbs = __raw_readw(high);
+
+		*page = (page_msbs << (16 - cs_width)) | page_lsbs;
+
+	} else {
+
+		/*
+		 * The low 32 bits of the address group look like this:
+		 *
+		 *                                 n
+		 * | <-     (32 - n) bits      ->|<->|
+		 * +-----------------------------+---+
+		 * |          Page LSBs          |CS |
+		 * +-----------------------------+---+
+		 */
+
+		x = __raw_readl(low);
+
+		*chip     = x &  cs_mask;
+		page_lsbs = x >> cs_width;
+
+		/* The high 16 bits contain the MSB's of the page address. */
+
+		page_msbs = __raw_readw(high);
+
+		*page = (page_msbs << (32 - cs_width)) | page_lsbs;
+
+	}
+
+}
+
+/**
+ * nfc_3_2_set_auto_addresses() - Sets automatic operation addresses.
+ *
+ * @this:    Per-device data.
+ * @group:   The address group number.
+ * @chip:    The chip number.
+ * @column:  The column address. The sentinel value ~0 indicates that there is
+ *           no column address.
+ * @page:    The page address.
+ */
+static void nfc_3_2_set_auto_addresses(struct imx_nfc_data *this,
+		unsigned group, unsigned chip, unsigned column, unsigned page)
+{
+	uint32_t                  x;
+	unsigned                  chip_count;
+	unsigned int              cs_width;
+	unsigned int              cs_mask;
+	uint32_t                  *low;
+	uint16_t                  *high;
+	void                      *primary_base = this->primary_regs;
+	void                      *secondary_base = this->secondary_regs;
+
+	/*
+	 * The width of the chip select field depends on the number of connected
+	 * chips.
+	 *
+	 * Notice that these computations work only if the number of chips is a
+	 * power of 2. In fact, that is a fundamental limitation for using
+	 * automatic operations.
+	 */
+
+	x = __raw_readl(secondary_base + NFC_3_2_CONFIG3_REG_OFF);
+
+	chip_count =
+		(x & NFC_3_2_CONFIG3_NUM_OF_DEVICES_MSK) >>
+					NFC_3_2_CONFIG3_NUM_OF_DEVICES_POS;
+	chip_count++;
+
+	cs_width = ffs(chip_count) - 1;
+	cs_mask  = chip_count - 1;
+
+	/* Construct pointers to the pieces of the given address group. */
+
+	low = primary_base + NFC_3_2_ADD0_REG_OFF;
+	low += group;
+
+	high = primary_base + NFC_3_2_ADD8_REG_OFF;
+	high += group;
+
+	/* Check if we have a column address. */
+
+	if (column != ~0) {
+
+		/*
+		 * The low 32 bits of the address group look like this:
+		 *
+		 *      16 - n     n
+		 * | <-  bits  ->|<->|<-  16 bits   ->|
+		 * +-------------+---+----------------+
+		 * |  Page LSBs  |CS |     Column     |
+		 * +-------------+---+----------------+
+		 */
+
+		x  = 0;
+		x |= column & 0xffff;
+		x |= (chip & cs_mask) << 16;
+		x |= page << (16 + cs_width);
+
+		__raw_writel(x, low);
+
+		/* The high 16 bits contain the MSB's of the page address. */
+
+		x = (page >> (16 - cs_width)) & 0xffff;
+
+		__raw_writew(x, high);
+
+	} else {
+
+		/*
+		 * The low 32 bits of the address group look like this:
+		 *
+		 *                                 n
+		 * | <-     (32 - n) bits      ->|<->|
+		 * +-----------------------------+---+
+		 * |          Page LSBs          |CS |
+		 * +-----------------------------+---+
+		 */
+
+		x  = 0;
+		x |= chip & cs_mask;
+		x |= page << cs_width;
+
+		__raw_writel(x, low);
+
+		/* The high 16 bits contain the MSB's of the page address. */
+
+		x = (page >> (32 - cs_width)) & 0xffff;
+
+		__raw_writew(x, high);
+
+	}
+
+}
+
+/**
+ * nfc_3_2_start_auto_read() - Starts an automatic read.
+ *
+ * This function returns 0 if everything went well.
+ *
+ * @this:   Per-device data.
+ * @start:  The first physical chip number on which to operate.
+ * @count:  The number of physical chips on which to operate.
+ * @column: The column address.
+ * @page:   The page address.
+ */
+static int nfc_3_2_start_auto_read(struct imx_nfc_data *this,
+		unsigned start, unsigned count, unsigned column, unsigned page)
+{
+	uint32_t  x;
+	int       return_value = 0;
+	void      *primary_base = this->primary_regs;
+
+	add_event("Entering nfc_3_2_start_auto_read", 1);
+
+	/* Check for nonsense. */
+
+	if ((start > 7) || (!count) || (count > 8)) {
+		return_value = !0;
+		goto exit;
+	}
+
+	/* Set state. */
+
+	nfc_3_2_set_auto_loop_params(this, start, start + count - 1, start);
+	nfc_3_2_set_auto_addresses(this, 0, start, column, page);
+
+	/* Set up for ONE iteration at a time. */
+
+	raw_clr_mask_l(NFC_3_2_CONFIG1_ITER_MSK,
+					primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	/* Reset to buffer 0. */
+
+	raw_clr_mask_l(NFC_3_2_CONFIG1_RBA_MSK,
+					primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	/*
+	 * Set up the commands. Note that the number of command phases was
+	 * configured in the set_geometry() function so, even though we're
+	 * giving both commands here, they won't necessarily both be used.
+	 */
+
+	x = __raw_readl(primary_base + NFC_3_2_CMD_REG_OFF);
+
+	x &= 0xffff0000;
+	x |= NAND_CMD_READ0     << 0;
+	x |= NAND_CMD_READSTART << 8;
+
+	__raw_writel(x, primary_base + NFC_3_2_CMD_REG_OFF);
+
+	/* Launch the operation. */
+
+	add_event("Launching", 0);
+
+	__raw_writel(NFC_3_2_LAUNCH_AUTO_READ_MSK,
+					primary_base + NFC_3_2_LAUNCH_REG_OFF);
+
+exit:	/* Return. */
+
+	add_event("Exiting nfc_3_2_start_auto_read", -1);
+
+	return return_value;
+
+}
+
+/**
+ * nfc_3_2_wait_for_auto_read() - Waits until auto read is ready for the CPU.
+ *
+ * This function returns 0 if everything went well.
+ *
+ * @this:   Per-device data.
+ */
+static int nfc_3_2_wait_for_auto_read(struct imx_nfc_data *this)
+{
+	unsigned int  first;
+	unsigned int  last;
+	unsigned int  index;
+	int           return_value = 0;
+
+	add_event("Entering nfc_3_2_wait_for_auto_read", 1);
+
+	/* Get state. */
+
+	nfc_3_2_get_auto_loop_params(this, &first, &last, &index);
+
+	/* This function should be called for every chip. */
+
+	if ((index < first) || (index > last)) {
+		return_value = !0;
+		goto exit;
+	}
+
+	/* Wait for the NFC to completely finish and interrupt. */
+
+	nfc_util_wait_for_the_nfc(this, true);
+
+exit:	/* Return. */
+
+	add_event("Exiting nfc_3_2_wait_for_auto_read", -1);
+
+	return return_value;
+
+}
+
+/**
+ * nfc_3_2_resume_auto_read() - Resumes auto read after CPU intervention.
+ *
+ * This function returns 0 if everything went well.
+ *
+ * @this:   Per-device data.
+ */
+static int nfc_3_2_resume_auto_read(struct imx_nfc_data *this)
+{
+	unsigned int  first;
+	unsigned int  last;
+	unsigned int  index;
+	unsigned int  chip;
+	unsigned int  column;
+	unsigned int  page;
+	int           return_value = 0;
+	void          *primary_base = this->primary_regs;
+
+	add_event("Entering nfc_3_2_resume_auto_read", 1);
+
+	/* Get state. */
+
+	nfc_3_2_get_auto_loop_params(this, &first, &last, &index);
+	nfc_3_2_get_auto_addresses(this, 0, &chip, &column, &page);
+
+	/* This function should be called for every chip, except the last. */
+
+	if ((index < first) || (index >= last)) {
+		return_value = !0;
+		goto exit;
+	}
+
+	/* Move to the next chip. */
+
+	index++;
+
+	/* Update state. */
+
+	nfc_3_2_set_auto_loop_params(this, first, last, index);
+	nfc_3_2_set_auto_addresses(this, 0, index, column, page);
+
+	/* Reset to buffer 0. */
+
+	raw_clr_mask_l(NFC_3_2_CONFIG1_RBA_MSK,
+					primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	/* Launch the operation. */
+
+	add_event("Launching", 0);
+
+	__raw_writel(NFC_3_2_LAUNCH_AUTO_READ_MSK,
+					primary_base + NFC_3_2_LAUNCH_REG_OFF);
+
+exit:	/* Return. */
+
+	add_event("Exiting nfc_3_2_resume_auto_read", -1);
+
+	return return_value;
+
+}
+
+/**
+ * nfc_3_2_start_auto_write() - Starts an automatic write.
+ *
+ * This function returns 0 if everything went well.
+ *
+ * @this:   Per-device data.
+ * @start:  The first physical chip number on which to operate.
+ * @count:  The number of physical chips on which to operate.
+ * @column: The column address.
+ * @page:   The page address.
+ */
+static int nfc_3_2_start_auto_write(struct imx_nfc_data *this,
+		unsigned start, unsigned count, unsigned column, unsigned page)
+{
+	uint32_t  x;
+	int       return_value = 0;
+	void      *primary_base   = this->primary_regs;
+	void      *secondary_base = this->secondary_regs;
+
+	add_event("Entering nfc_3_2_start_auto_write", 1);
+
+	/* Check for nonsense. */
+
+	if ((start > 7) || (!count) || (count > 8)) {
+		return_value = !0;
+		goto exit;
+	}
+
+	/* Set state. */
+
+	nfc_3_2_set_auto_loop_params(this, start, start + count - 1, start);
+	nfc_3_2_set_auto_addresses(this, 0, start, column, page);
+
+	/* Set up for ONE iteration at a time. */
+
+	raw_clr_mask_l(NFC_3_2_CONFIG1_ITER_MSK,
+					primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	/* Set up the commands. */
+
+	x = __raw_readl(primary_base + NFC_3_2_CMD_REG_OFF);
+
+	x &= 0xffff0000;
+	x |= NAND_CMD_SEQIN    << 0;
+	x |= NAND_CMD_PAGEPROG << 8;
+
+	__raw_writel(x, primary_base + NFC_3_2_CMD_REG_OFF);
+
+	/* Clear the auto_prog_done bit. */
+
+	raw_clr_mask_l(NFC_3_2_IPC_AUTO_PROG_DONE_MSK,
+					secondary_base + NFC_3_2_IPC_REG_OFF);
+
+exit:	/* Return. */
+
+	add_event("Exiting nfc_3_2_start_auto_write", -1);
+
+	return return_value;
+
+}
+
+/**
+ * nfc_3_2_wait_for_auto_write() - Waits for auto write to be writey for the CPU.
+ *
+ * This function returns 0 if everything went well.
+ *
+ * @this:   Per-device data.
+ */
+static int nfc_3_2_wait_for_auto_write(struct imx_nfc_data *this)
+{
+	unsigned int  first;
+	unsigned int  last;
+	unsigned int  index;
+	unsigned int  chip;
+	unsigned int  column;
+	unsigned int  page;
+	uint32_t      x;
+	int           interrupt;
+	int           transmitted;
+	int           ready;
+	int           return_value = 0;
+	void          *primary_base   = this->primary_regs;
+	void          *secondary_base = this->secondary_regs;
+
+	add_event("Entering nfc_3_2_wait_for_auto_write", 1);
+
+	/* Get state. */
+
+	nfc_3_2_get_auto_loop_params(this, &first, &last, &index);
+	nfc_3_2_get_auto_addresses(this, 0, &chip, &column, &page);
+
+	/* This function should be called for every chip. */
+
+	if ((index < first) || (index > last)) {
+		return_value = !0;
+		goto exit;
+	}
+
+	/* Reset to buffer 0. */
+
+	raw_clr_mask_l(NFC_3_2_CONFIG1_RBA_MSK,
+					primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	/* Launch the operation. */
+
+	nfc_3_2_add_state_events(this);
+
+	add_event("Launching", 0);
+
+	__raw_writel(NFC_3_2_LAUNCH_AUTO_PROG_MSK,
+					primary_base + NFC_3_2_LAUNCH_REG_OFF);
+
+	nfc_3_2_add_state_events(this);
+
+	/* Wait for the NFC to transmit the page. */
+
+	add_event("Spinning while the NFC transmits the page...", 0);
+
+	do
+		transmitted = !!raw_read_mask_l(NFC_3_2_IPC_AUTO_PROG_DONE_MSK,
+					secondary_base + NFC_3_2_IPC_REG_OFF);
+	while (!transmitted);
+
+	/*
+	 * When control arrives here, the auto_prog_done bit is set. This
+	 * indicates the NFC has finished transmitting the current page. The CPU
+	 * is now free to write the next page into the NFC's memory. The Flash
+	 * hardware is still busy programming the page into its storage array.
+	 *
+	 * Clear the auto_prog_done bit. This is analogous to acknowledging an
+	 * interrupt.
+	 */
+
+	nfc_3_2_add_state_events(this);
+
+	add_event("Acknowledging the page...", 0);
+
+	raw_clr_mask_l(NFC_3_2_IPC_AUTO_PROG_DONE_MSK,
+					secondary_base + NFC_3_2_IPC_REG_OFF);
+
+	nfc_3_2_add_state_events(this);
+
+	/*
+	 * If this is *not* the last iteration, move to the next chip and return
+	 * to the caller so he can put the next page in the NFC buffer.
+	 */
+
+	if (index < last) {
+
+		add_event("Moving to the next chip...", 0);
+
+		index++;
+
+		nfc_3_2_set_auto_loop_params(this, first, last, index);
+		nfc_3_2_set_auto_addresses(this, 0, index, column, page);
+
+		goto exit;
+
+	}
+
+	/*
+	 * If control arrives here, this is the last iteration, so it's time to
+	 * close out the entire operation. We need to wait for the medium to be
+	 * ready and then acknowledge the final interrupt.
+	 *
+	 * Because of the way the NFC hardware works, the code here requires a
+	 * bit of explanation. The most important rule is:
+	 *
+	 *         During automatic operations, the NFC sets its
+	 *         interrupt bit *whenever* it sees the ready/busy
+	 *         signal transition from "Busy" to "Ready".
+	 *
+	 * Recall that the ready/busy signals from all the chips in the medium
+	 * are "wire-anded." Thus, the NFC will only see that the medium is
+	 * ready if *all* chips are ready.
+	 *
+	 * Because of variability in NAND Flash timing, the medium *may* have
+	 * become ready during previous iterations, which means the interrupt
+	 * bit *may* be set at this moment. This is a "left-over" interrupt, and
+	 * can complicate our logic.
+	 *
+	 * The two bits of state that interest us here are the interrupt bit
+	 * and the ready/busy bit. It boils down to the following truth table:
+	 *
+	 * | Interrupt  | Ready/Busy | Description
+	 * +------------+------------+---------------
+	 * |            |            | Busy medium and no left-over interrupt.
+	 * |     0      |     0      | The final interrupt will arrive in the
+	 * |            |            | future.
+	 * +------------+------------+---------------
+	 * |            |            | Ready medium and no left-over interrupt.
+	 * |     0      |     1      | There will be no final interrupt. This
+	 * |            |            | case should be impossible.
+	 * +------------+------------+---------------
+	 * |            |            | Busy medium and left-over interrupt.
+	 * |     1      |     0      | The final interrupt will arrive in the
+	 * |            |            | future. This is the hard case.
+	 * +------------+------------+---------------
+	 * |            |            | Ready medium and left-over interrupt.
+	 * |     1      |     1      | The final interrupt has already
+	 * |            |            | arrived. Acknowledge it and exit.
+	 * +------------+------------+---------------
+	 *
+	 * Case #3 is a small problem. If we clear the interrupt, we may or may
+	 * not have another interrupt following.
+	 */
+
+	/* Sample the IPC register. */
+
+	x = __raw_readl(secondary_base + NFC_3_2_IPC_REG_OFF);
+
+	interrupt = !!(x & NFC_3_2_IPC_INT_MSK);
+	ready     = !!(x & NFC_3_2_IPC_RB_B_MSK);
+
+	/* Check for the easy cases. */
+
+	if (!interrupt && !ready) {
+		add_event("Waiting for the final interrupt..." , 0);
+		nfc_util_wait_for_the_nfc(this, true);
+		goto exit;
+	} else if (!interrupt && ready) {
+		add_event("Done." , 0);
+		goto exit;
+	} else if (interrupt && ready) {
+		add_event("Acknowledging the final interrupt..." , 0);
+		nfc_util_wait_for_the_nfc(this, false);
+		goto exit;
+
+	}
+
+	/*
+	 * If control arrives here, we hit case #3. Begin by acknowledging the
+	 * interrupt we have right now.
+	 */
+
+	add_event("Clearing the left-over interrupt..." , 0);
+	nfc_util_wait_for_the_nfc(this, false);
+
+	/*
+	 * Check the ready/busy bit again. If the medium is still busy, then
+	 * we're going to get one more interrupt.
+	 */
+
+	ready = !!raw_read_mask_l(NFC_3_2_IPC_RB_B_MSK,
+					secondary_base + NFC_3_2_IPC_REG_OFF);
+
+	if (!ready) {
+		add_event("Waiting for the final interrupt..." , 0);
+		nfc_util_wait_for_the_nfc(this, true);
+	}
+
+exit:	/* Return. */
+
+	add_event("Exiting nfc_3_2_wait_for_auto_write", -1);
+
+	return return_value;
+
+}
+
+/**
+ * nfc_3_2_start_auto_erase() - Starts an automatic erase.
+ *
+ * This function returns 0 if everything went well.
+ *
+ * @this:   Per-device data.
+ * @start:  The first physical chip number on which to operate.
+ * @count:  The number of physical chips on which to operate.
+ * @page:   The page address.
+ */
+static int nfc_3_2_start_auto_erase(struct imx_nfc_data *this,
+				unsigned start, unsigned count, unsigned page)
+{
+	uint32_t  x;
+	unsigned  i;
+	int       return_value = 0;
+	void      *primary_base = this->primary_regs;
+
+	add_event("Entering nfc_3_2_start_auto_erase", 1);
+
+	/* Check for nonsense. */
+
+	if ((start > 7) || (!count) || (count > 8)) {
+		return_value = !0;
+		goto exit;
+	}
+
+	/* Set up the commands. */
+
+	x = __raw_readl(primary_base + NFC_3_2_CMD_REG_OFF);
+
+	x &= 0xffff0000;
+	x |= NAND_CMD_ERASE1 << 0;
+	x |= NAND_CMD_ERASE2 << 8;
+
+	__raw_writel(x, primary_base + NFC_3_2_CMD_REG_OFF);
+
+	/* Set the iterations. */
+
+	x = __raw_readl(primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	x &= ~NFC_3_2_CONFIG1_ITER_MSK;
+	x |= ((count - 1) << NFC_3_2_CONFIG1_ITER_POS) &
+						NFC_3_2_CONFIG1_ITER_MSK;
+
+	__raw_writel(x, primary_base + NFC_3_2_CONFIG1_REG_OFF);
+
+	/* Loop over chips, setting up the address groups. */
+
+	for (i = 0; i < count; i++)
+		nfc_3_2_set_auto_addresses(this, i, start + i, ~0, page);
+
+	/* Launch the operation. */
+
+	add_event("Launching", 0);
+
+	__raw_writel(NFC_3_2_LAUNCH_AUTO_ERASE_MSK,
+					primary_base + NFC_3_2_LAUNCH_REG_OFF);
+
+exit:	/* Return. */
+
+	add_event("Exiting nfc_3_2_start_auto_erase", -1);
+
+	return return_value;
+
+}
+
+/*
+ * At this point, we've defined all the version-specific primitives. We're now
+ * ready to construct the NFC HAL structures for every version.
+ */
+
+struct nfc_hal nfc_1_0_hal = {
+		.major_version       = 1,
+		.minor_version       = 0,
+		.max_chip_count      = 1,
+		.max_buffer_count    = 4,
+		.spare_buf_stride    = 16,
+		.has_secondary_regs  = 0,
+		.can_be_symmetric    = 0,
+	};
+
+struct nfc_hal nfc_2_0_hal = {
+		.major_version       = 2,
+		.minor_version       = 0,
+		.max_chip_count      = 1,
+		.max_buffer_count    = 4,
+		.spare_buf_stride    = 16,
+		.has_secondary_regs  = false,
+		.can_be_symmetric    = true,
+		.init                = nfc_2_0_init,
+		.set_geometry        = nfc_2_0_set_geometry,
+		.exit                = nfc_2_x_exit,
+		.mask_interrupt      = nfc_2_0_mask_interrupt,
+		.unmask_interrupt    = nfc_2_0_unmask_interrupt,
+		.clear_interrupt     = nfc_2_x_clear_interrupt,
+		.is_interrupting     = nfc_2_x_is_interrupting,
+		.is_ready            = 0, /* Ready/Busy not exposed. */
+		.set_ecc             = nfc_2_0_set_ecc,
+		.get_ecc_status      = nfc_2_0_get_ecc_status,
+		.get_symmetric       = nfc_2_0_get_symmetric,
+		.set_symmetric       = nfc_2_0_set_symmetric,
+		.select_chip         = nfc_2_0_select_chip,
+		.command_cycle       = nfc_2_x_command_cycle,
+		.write_cycle         = nfc_2_x_write_cycle,
+		.read_cycle          = nfc_2_0_read_cycle,
+		.read_page           = nfc_2_0_read_page,
+		.send_page           = nfc_2_0_send_page,
+		.start_auto_read     = 0, /* Not supported. */
+		.wait_for_auto_read  = 0, /* Not supported. */
+		.resume_auto_read    = 0, /* Not supported. */
+		.start_auto_write    = 0, /* Not supported. */
+		.wait_for_auto_write = 0, /* Not supported. */
+		.start_auto_erase    = 0, /* Not supported. */
+	};
+
+struct nfc_hal nfc_2_1_hal = {
+		.major_version       = 2,
+		.minor_version       = 1,
+		.max_chip_count      = 4,
+		.max_buffer_count    = 8,
+		.spare_buf_stride    = 64,
+		.has_secondary_regs  = 0,
+		.can_be_symmetric    = !0,
+	};
+
+struct nfc_hal nfc_3_1_hal = {
+		.major_version       = 3,
+		.minor_version       = 1,
+		.max_chip_count      = 4,
+		.max_buffer_count    = 8,
+		.spare_buf_stride    = 64,
+		.has_secondary_regs  = !0,
+		.can_be_symmetric    = !0,
+	};
+
+struct nfc_hal nfc_3_2_hal = {
+		.major_version       = 3,
+		.minor_version       = 2,
+		.max_chip_count      = 8,
+		.max_buffer_count    = 8,
+		.spare_buf_stride    = 64,
+		.has_secondary_regs  = true,
+		.can_be_symmetric    = true,
+		.init                = nfc_3_2_init,
+		.set_geometry        = nfc_3_2_set_geometry,
+		.exit                = nfc_3_2_exit,
+		.set_closest_cycle   = nfc_3_2_set_closest_cycle,
+		.mask_interrupt      = nfc_3_2_mask_interrupt,
+		.unmask_interrupt    = nfc_3_2_unmask_interrupt,
+		.clear_interrupt     = nfc_3_2_clear_interrupt,
+		.is_interrupting     = nfc_3_2_is_interrupting,
+		.is_ready            = nfc_3_2_is_ready,
+		.set_force_ce        = nfc_3_2_set_force_ce,
+		.set_ecc             = nfc_3_2_set_ecc,
+		.get_ecc_status      = nfc_3_2_get_ecc_status,
+		.get_symmetric       = nfc_3_2_get_symmetric,
+		.set_symmetric       = nfc_3_2_set_symmetric,
+		.select_chip         = nfc_3_2_select_chip,
+		.command_cycle       = nfc_3_2_command_cycle,
+		.write_cycle         = nfc_3_2_write_cycle,
+		.read_cycle          = nfc_3_2_read_cycle,
+		.read_page           = nfc_3_2_read_page,
+		.send_page           = nfc_3_2_send_page,
+		.start_auto_read     = nfc_3_2_start_auto_read,
+		.wait_for_auto_read  = nfc_3_2_wait_for_auto_read,
+		.resume_auto_read    = nfc_3_2_resume_auto_read,
+		.start_auto_write    = nfc_3_2_start_auto_write,
+		.wait_for_auto_write = nfc_3_2_wait_for_auto_write,
+		.start_auto_erase    = nfc_3_2_start_auto_erase,
+	};
+
+/*
+ * This array has a pointer to every NFC HAL structure. The probing process will
+ * find the one that matches the version given by the platform.
+ */
+
+struct nfc_hal  *(nfc_hals[]) = {
+	&nfc_1_0_hal,
+	&nfc_2_0_hal,
+	&nfc_2_1_hal,
+	&nfc_3_1_hal,
+	&nfc_3_2_hal,
+};
+
+/**
+ * mal_init() - Initialize the Medium Abstraction Layer.
+ *
+ * @this:  Per-device data.
+ */
+static void mal_init(struct imx_nfc_data  *this)
+{
+	this->interrupt_override = DRIVER_CHOICE;
+	this->auto_op_override   = DRIVER_CHOICE;
+	this->inject_ecc_error   = 0;
+}
+
+/**
+ * mal_set_physical_geometry() - Set up the physical medium geometry.
+ *
+ * This function retrieves the physical geometry information discovered by
+ * nand_scan(), corrects it, and records it in the per-device data structure.
+ *
+ * @this:  Per-device data.
+ */
+static int mal_set_physical_geometry(struct imx_nfc_data  *this)
+{
+	struct mtd_info           *mtd  = &this->mtd;
+	struct nand_chip          *nand = &this->nand;
+	struct device             *dev  = this->dev;
+	uint8_t                   manufacturer_id;
+	uint8_t                   device_id;
+	unsigned int              block_size_in_pages;
+	unsigned int              chip_size_in_blocks;
+	unsigned int              chip_size_in_pages;
+	uint64_t                  medium_size_in_bytes;
+	struct physical_geometry  *physical = &this->physical_geometry;
+
+	/*
+	 * Begin by transcribing exactly what the MTD code discovered. If there
+	 * are any mistakes, we'll fix them in a moment.
+	 */
+
+	physical->chip_count     = nand->numchips;
+	physical->chip_size      = nand->chipsize;
+	physical->block_size     = mtd->erasesize;
+	physical->page_data_size = mtd->writesize;
+	physical->page_oob_size  = mtd->oobsize;
+
+	/* Read some of the ID bytes from the first NAND Flash chip. */
+
+	nand->select_chip(mtd, 0);
+
+	nfc_util_send_cmd_and_addrs(this, NAND_CMD_READID, 0x00, -1);
+
+	manufacturer_id = nand->read_byte(mtd);
+	device_id       = nand->read_byte(mtd);
+
+	/*
+	 * Most manufacturers sell 4K page devices with 218 out-of-band bytes
+	 * per page to accomodate ECC-8.
+	 *
+	 * Samsung and Hynix claim their parts have better reliability, so they
+	 * only need ECC-4 and they have only 128 out-of-band bytes.
+	 *
+	 * The MTD code pays no attention to the manufacturer ID (something that
+	 * eventually will have to change), so it believes that all 4K pages
+	 * have 218 out-of-band bytes.
+	 *
+	 * We correct that mistake here.
+	 */
+
+	if (physical->page_data_size == 4096) {
+		if ((manufacturer_id == NAND_MFR_SAMSUNG) ||
+					(manufacturer_id == NAND_MFR_HYNIX)) {
+			physical->page_oob_size = 128;
+		}
+	}
+
+	/* Compute some interesting facts. */
+
+	block_size_in_pages  =
+		physical->block_size / physical->page_data_size;
+	chip_size_in_pages   =
+		physical->chip_size >> (fls(physical->page_data_size) - 1);
+	chip_size_in_blocks  =
+		physical->chip_size >> (fls(physical->block_size) - 1);
+	medium_size_in_bytes =
+		physical->chip_size * physical->chip_count;
+
+	/* Report. */
+
+	dev_dbg(dev, "-----------------\n");
+	dev_dbg(dev, "Physical Geometry\n");
+	dev_dbg(dev, "-----------------\n");
+	dev_dbg(dev, "Chip Count             : %d\n", physical->chip_count);
+	dev_dbg(dev, "Page Data Size in Bytes: %u (0x%x)\n",
+			physical->page_data_size, physical->page_data_size);
+	dev_dbg(dev, "Page OOB Size in Bytes : %u\n",
+			physical->page_oob_size);
+	dev_dbg(dev, "Block Size in Bytes    : %u (0x%x)\n",
+			physical->block_size, physical->block_size);
+	dev_dbg(dev, "Block Size in Pages    : %u (0x%x)\n",
+			block_size_in_pages, block_size_in_pages);
+	dev_dbg(dev, "Chip Size in Bytes     : %llu (0x%llx)\n",
+			physical->chip_size, physical->chip_size);
+	dev_dbg(dev, "Chip Size in Pages     : %u (0x%x)\n",
+			chip_size_in_pages, chip_size_in_pages);
+	dev_dbg(dev, "Chip Size in Blocks    : %u (0x%x)\n",
+			chip_size_in_blocks, chip_size_in_blocks);
+	dev_dbg(dev, "Medium Size in Bytes   : %llu (0x%llx)\n",
+			medium_size_in_bytes, medium_size_in_bytes);
+
+	/* Return success. */
+
+	return 0;
+
+}
+
+/**
+ * mal_set_nfc_geometry() - Set up the NFC geometry.
+ *
+ * This function calls the NFC HAL to select an NFC geometry that is compatible
+ * with the medium's physical geometry.
+ *
+ * @this:  Per-device data.
+ */
+static int mal_set_nfc_geometry(struct imx_nfc_data  *this)
+{
+	struct device        *dev = this->dev;
+	struct nfc_geometry  *nfc;
+
+	/* Set the NFC geometry. */
+
+	if (this->nfc->set_geometry(this))
+		return !0;
+
+	/* Get a pointer to the new NFC geometry information. */
+
+	nfc = this->nfc_geometry;
+
+	/* Report. */
+
+	dev_dbg(dev, "------------\n");
+	dev_dbg(dev, "NFC Geometry\n");
+	dev_dbg(dev, "------------\n");
+	dev_dbg(dev, "Page Data Size in Bytes: %u (0x%x)\n",
+				nfc->page_data_size, nfc->page_data_size);
+	dev_dbg(dev, "Page OOB Size in Bytes : %u\n", nfc->page_oob_size);
+	dev_dbg(dev, "ECC Algorithm          : %s\n", nfc->ecc_algorithm);
+	dev_dbg(dev, "ECC Strength           : %d\n", nfc->ecc_strength);
+	dev_dbg(dev, "Buffer Count           : %u\n", nfc->buffer_count);
+	dev_dbg(dev, "Spare Buffer Size      : %u\n", nfc->spare_buf_size);
+	dev_dbg(dev, "Spare Buffer Spillover : %u\n", nfc->spare_buf_spill);
+	dev_dbg(dev, "Auto Read Available    : %s\n",
+				this->nfc->start_auto_read  ? "Yes" : "No");
+	dev_dbg(dev, "Auto Write Available   : %s\n",
+				this->nfc->start_auto_write ? "Yes" : "No");
+	dev_dbg(dev, "Auto Erase Available   : %s\n",
+				this->nfc->start_auto_erase ? "Yes" : "No");
+
+	/* Return success. */
+
+	return 0;
+
+}
+
+/**
+ * mal_set_logical_geometry() - Set up the logical medium geometry.
+ *
+ * This function constructs the logical geometry that we will expose to MTD,
+ * based on the physical and NFC geometries, and whether or not interleaving is
+ * on.
+ *
+ * @this:  Per-device data.
+ */
+static int mal_set_logical_geometry(struct imx_nfc_data  *this)
+{
+	const uint32_t            max_medium_size_in_bytes = ~0;
+	int                       we_are_interleaving;
+	uint64_t                  physical_medium_size_in_bytes;
+	unsigned int              usable_blocks;
+	unsigned int              block_size_in_pages;
+	unsigned int              chip_size_in_blocks;
+	unsigned int              chip_size_in_pages;
+	unsigned int              usable_medium_size_in_pages;
+	unsigned int              usable_medium_size_in_blocks;
+	struct physical_geometry  *physical = &this->physical_geometry;
+	struct nfc_geometry       *nfc      =  this->nfc_geometry;
+	struct logical_geometry   *logical  = &this->logical_geometry;
+	struct device             *dev      =  this->dev;
+
+	/* Figure out if we're interleaving. */
+
+	we_are_interleaving = this->pdata->interleave;
+
+	switch (imx_nfc_module_interleave_override) {
+
+	case NEVER:
+		we_are_interleaving = false;
+		break;
+
+	case DRIVER_CHOICE:
+		break;
+
+	case ALWAYS:
+		we_are_interleaving = true;
+		break;
+
+	}
+
+	/* Compute the physical size of the medium. */
+
+	physical_medium_size_in_bytes =
+				physical->chip_count * physical->chip_size;
+
+	/* Compute the logical geometry. */
+
+	if (!we_are_interleaving) {
+
+		/*
+		 * At this writing, MTD uses unsigned 32-bit variables to
+		 * represent the size of the medium. If the physical medium is
+		 * larger than that, the logical medium must be smaller. Here,
+		 * we compute the total number of physical blocks in the medium
+		 * that we can actually use.
+		 */
+
+		if (physical_medium_size_in_bytes <= max_medium_size_in_bytes) {
+			usable_blocks =
+				physical_medium_size_in_bytes >>
+						(ffs(physical->block_size) - 1);
+		} else {
+			usable_blocks =
+				max_medium_size_in_bytes / physical->block_size;
+		}
+
+		/* Set up the logical geometry.
+		 *
+		 * Notice that the usable medium size is not necessarily the
+		 * same as the chip size multiplied by the number of physical
+		 * chips. We can't afford to touch the physical chip size
+		 * because the NAND Flash MTD code *requires* it to be a power
+		 * of 2.
+		 */
+
+		logical->chip_count     = physical->chip_count;
+		logical->chip_size      = physical->chip_size;
+		logical->usable_size    = usable_blocks * physical->block_size;
+		logical->block_size     = physical->block_size;
+		logical->page_data_size = nfc->page_data_size;
+
+		/* Use the MTD layout that best matches the NFC geometry. */
+
+		logical->mtd_layout    = &nfc->mtd_layout;
+		logical->page_oob_size = nfc->mtd_layout.eccbytes +
+						nfc->mtd_layout.oobavail;
+
+	} else {
+
+		/*
+		 * If control arrives here, we are interleaving. Specifically,
+		 * we are "horizontally concatenating" the pages in all the
+		 * physical chips.
+		 *
+		 * - A logical page will be the size of a physical page
+		 *   multiplied by the number of physical chips.
+		 *
+		 * - A logical block will have the same number of pages as a
+		 *   physical block but, since the logical page size is larger,
+		 *   the logical block size is larger.
+		 *
+		 * - The entire medium will appear to be a single chip.
+		 *
+		 * At this writing, MTD uses unsigned 32-bit variables to
+		 * represent the size of the medium. If the physical medium is
+		 * larger than that, the logical medium must be smaller.
+		 *
+		 * The NAND Flash MTD code represents the size of a single chip
+		 * as an unsigned 32-bit value. It also *requires* that the size
+		 * of a chip be a power of two. Thus, the largest possible chip
+		 * size is 2GiB.
+		 *
+		 * When interleaving, the entire medium appears to be one chip.
+		 * Thus, when interleaving, the largest possible medium size is
+		 * 2GiB.
+		 */
+
+		if (physical_medium_size_in_bytes <= max_medium_size_in_bytes) {
+			logical->chip_size =
+				0x1 << (fls(physical_medium_size_in_bytes) - 1);
+		} else {
+			logical->chip_size =
+				0x1 << (fls(max_medium_size_in_bytes) - 1);
+		}
+
+		/*
+		 * If control arrives here, we're interleaving. The logical
+		 * geometry is very different from the physical geometry.
+		 */
+
+		logical->chip_count     = 1;
+		logical->usable_size    = logical->chip_size;
+		logical->block_size     =
+				physical->block_size * physical->chip_count;
+		logical->page_data_size =
+				nfc->page_data_size * physical->chip_count;
+
+		/*
+		 * Since the logical geometry doesn't match the physical
+		 * geometry, we can't use the MTD layout that matches the
+		 * NFC geometry. We synthesize one here.
+		 *
+		 * Our "logical" OOB will be the concatenation of the first 5
+		 * bytes of the "physical" OOB of every chip. This has some
+		 * important properties:
+		 *
+		 * - This will make the block mark of every physical chip
+		 *   visible (even for small page chips, which put their block
+		 *   mark in the 5th OOB byte).
+		 *
+		 * - None of the NFC controllers put ECC in the first 5 OOB
+		 *   bytes, so this layout exposes no ECC.
+		 */
+
+		logical->page_oob_size = 5 * physical->chip_count;
+
+		synthetic_layout.eccbytes = 0;
+		synthetic_layout.oobavail = 5 * physical->chip_count;
+		synthetic_layout.oobfree[0].offset = 0;
+		synthetic_layout.oobfree[0].length = synthetic_layout.oobavail;
+
+		/* Install the synthetic layout. */
+
+		logical->mtd_layout = &synthetic_layout;
+
+	}
+
+	/* Compute some interesting facts. */
+
+	block_size_in_pages = logical->block_size / logical->page_data_size;
+	chip_size_in_pages  = logical->chip_size / logical->page_data_size;
+	chip_size_in_blocks = logical->chip_size / logical->block_size;
+	usable_medium_size_in_pages =
+				logical->usable_size / logical->page_data_size;
+	usable_medium_size_in_blocks =
+				logical->usable_size / logical->block_size;
+
+	/* Report. */
+
+	dev_dbg(dev, "----------------\n");
+	dev_dbg(dev, "Logical Geometry\n");
+	dev_dbg(dev, "----------------\n");
+	dev_dbg(dev, "Chip Count             : %d\n", logical->chip_count);
+	dev_dbg(dev, "Page Data Size in Bytes: %u (0x%x)\n",
+		logical->page_data_size, logical->page_data_size);
+	dev_dbg(dev, "Page OOB Size in Bytes : %u\n",
+		logical->page_oob_size);
+	dev_dbg(dev, "Block Size in Bytes    : %u (0x%x)\n",
+		logical->block_size, logical->block_size);
+	dev_dbg(dev, "Block Size in Pages    : %u (0x%x)\n",
+		block_size_in_pages, block_size_in_pages);
+	dev_dbg(dev, "Chip Size in Bytes     : %u (0x%x)\n",
+		logical->chip_size, logical->chip_size);
+	dev_dbg(dev, "Chip Size in Pages     : %u (0x%x)\n",
+		chip_size_in_pages, chip_size_in_pages);
+	dev_dbg(dev, "Chip Size in Blocks    : %u (0x%x)\n",
+		chip_size_in_blocks, chip_size_in_blocks);
+	dev_dbg(dev, "Physical Size in Bytes : %llu (0x%llx)\n",
+		physical_medium_size_in_bytes, physical_medium_size_in_bytes);
+	dev_dbg(dev, "Usable Size in Bytes   : %u (0x%x)\n",
+		logical->usable_size, logical->usable_size);
+	dev_dbg(dev, "Usable Size in Pages   : %u (0x%x)\n",
+		usable_medium_size_in_pages, usable_medium_size_in_pages);
+	dev_dbg(dev, "Usable Size in Blocks  : %u (0x%x)\n",
+		usable_medium_size_in_blocks, usable_medium_size_in_blocks);
+
+	/* Return success. */
+
+	return 0;
+
+}
+
+/**
+ * mal_set_mtd_geometry() - Set up the MTD geometry.
+ *
+ * This function adjusts the owning MTD data structures to match the logical
+ * geometry we've chosen.
+ *
+ * @this:  Per-device data.
+ */
+static int mal_set_mtd_geometry(struct imx_nfc_data *this)
+{
+	struct logical_geometry  *logical = &this->logical_geometry;
+	struct mtd_info          *mtd     = &this->mtd;
+	struct nand_chip         *nand    = &this->nand;
+
+	/* Configure the struct mtd_info. */
+
+	mtd->size        = logical->usable_size;
+	mtd->erasesize   = logical->block_size;
+	mtd->writesize   = logical->page_data_size;
+	mtd->ecclayout   = logical->mtd_layout;
+	mtd->oobavail    = mtd->ecclayout->oobavail;
+	mtd->oobsize     = mtd->ecclayout->oobavail + mtd->ecclayout->eccbytes;
+	mtd->subpage_sft = 0; /* We don't support sub-page writing. */
+
+	/* Configure the struct nand_chip. */
+
+	nand->numchips         = logical->chip_count;
+	nand->chipsize         = logical->chip_size;
+	nand->page_shift       = ffs(logical->page_data_size) - 1;
+	nand->pagemask         = (nand->chipsize >> nand->page_shift) - 1;
+	nand->subpagesize      = mtd->writesize >> mtd->subpage_sft;
+	nand->phys_erase_shift = ffs(logical->block_size) - 1;
+	nand->bbt_erase_shift  = nand->phys_erase_shift;
+	nand->chip_shift       = ffs(logical->chip_size) - 1;
+	nand->oob_poi          = nand->buffers->databuf+logical->page_data_size;
+	nand->ecc.layout       = logical->mtd_layout;
+
+	/* Set up the pattern that describes block marks. */
+
+	if (is_small_page_chip(this))
+		nand->badblock_pattern = &small_page_block_mark_descriptor;
+	else
+		nand->badblock_pattern = &large_page_block_mark_descriptor;
+
+	/* Return success. */
+
+	return 0;
+}
+
+/**
+ * mal_set_geometry() - Set up the medium geometry.
+ *
+ * @this:  Per-device data.
+ */
+static int mal_set_geometry(struct imx_nfc_data  *this)
+{
+
+	/* Set up the various layers of geometry, in this specific order. */
+
+	if (mal_set_physical_geometry(this))
+		return !0;
+
+	if (mal_set_nfc_geometry(this))
+		return !0;
+
+	if (mal_set_logical_geometry(this))
+		return !0;
+
+	if (mal_set_mtd_geometry(this))
+		return !0;
+
+	/* Return success. */
+
+	return 0;
+
+}
+
+/**
+ * mal_reset() - Resets the given chip.
+ *
+ * This is the fully-generalized reset operation, including support for
+ * interleaving. All reset operations funnel through here.
+ *
+ * @this:  Per-device data.
+ * @chip:  The logical chip of interest.
+ */
+static void mal_reset(struct imx_nfc_data *this, unsigned chip)
+{
+	int                       we_are_interleaving;
+	unsigned int              start;
+	unsigned int              end;
+	unsigned int              i;
+	struct physical_geometry  *physical = &this->physical_geometry;
+	struct logical_geometry   *logical  = &this->logical_geometry;
+
+	add_event("Entering mal_get_status", 1);
+
+	/* Establish some important facts. */
+
+	we_are_interleaving = logical->chip_count != physical->chip_count;
+
+	/* Choose the loop bounds. */
+
+	if (we_are_interleaving) {
+		start = 0;
+		end   = physical->chip_count;
+	} else {
+		start = chip;
+		end   = start + 1;
+	}
+
+	/* Loop over physical chips. */
+
+	add_event("Looping over physical chips...", 0);
+
+	for (i = start; i < end; i++) {
+
+		/* Select the current chip. */
+
+		this->nfc->select_chip(this, i);
+
+		/* Reset the current chip. */
+
+		add_event("Resetting...", 0);
+
+		nfc_util_send_cmd(this, NAND_CMD_RESET);
+		nfc_util_wait_for_the_nfc(this, false);
+
+	}
+
+	add_event("Exiting mal_get_status", -1);
+
+}
+
+/**
+ * mal_get_status() - Abstracted status retrieval.
+ *
+ * For media with a single chip, or concatenated chips, the HIL explicitly
+ * addresses a single chip at a time and wants the status from that chip only.
+ *
+ * For interleaved media, we must combine the individual chip states. At this
+ * writing, the NAND MTD system knows about the following bits in status
+ * registers:
+ *
+ *    +------------------------+-------+---------+
+ *    |                        |       | Combine |
+ *    |         Macro          | Value |  With   |
+ *    +------------------------+-------+---------+
+ *    | NAND_STATUS_FAIL       |  0x01 |    OR   |
+ *    | NAND_STATUS_FAIL_N1    |  0x02 |    OR   |
+ *    | NAND_STATUS_TRUE_READY |  0x20 |   AND   |
+ *    | NAND_STATUS_READY      |  0x40 |   AND   |
+ *    | NAND_STATUS_WP         |  0x80 |   AND   |
+ *    +------------------------+-------+---------+
+ *
+ * @this:  Per-device data.
+ * @chip:  The logical chip of interest.
+ */
+static uint8_t mal_get_status(struct imx_nfc_data *this, unsigned chip)
+{
+	int                       we_are_interleaving;
+	unsigned int              start;
+	unsigned int              end;
+	unsigned int              i;
+	unsigned int              x;
+	unsigned int              or_mask;
+	unsigned int              and_mask;
+	uint8_t                   status;
+	struct physical_geometry  *physical = &this->physical_geometry;
+	struct logical_geometry   *logical  = &this->logical_geometry;
+
+	add_event("Entering mal_get_status", 1);
+
+	/* Establish some important facts. */
+
+	we_are_interleaving = logical->chip_count != physical->chip_count;
+
+	/* Compute the masks we need. */
+
+	or_mask  = NAND_STATUS_FAIL | NAND_STATUS_FAIL_N1;
+	and_mask = NAND_STATUS_TRUE_READY | NAND_STATUS_READY | NAND_STATUS_WP;
+
+	/* Assume the chip is successful, ready and writeable. */
+
+	status = and_mask & ~or_mask;
+
+	/* Choose the loop bounds. */
+
+	if (we_are_interleaving) {
+		start = 0;
+		end   = physical->chip_count;
+	} else {
+		start = chip;
+		end   = start + 1;
+	}
+
+	/* Loop over physical chips. */
+
+	add_event("Looping over physical chips...", 0);
+
+	for (i = start; i < end; i++) {
+
+		/* Select the current chip. */
+
+		this->nfc->select_chip(this, i);
+
+		/* Get the current chip's status. */
+
+		add_event("Sending the command...", 0);
+
+		nfc_util_send_cmd(this, NAND_CMD_STATUS);
+		nfc_util_wait_for_the_nfc(this, false);
+
+		add_event("Reading the status...", 0);
+
+		x = this->nfc->read_cycle(this);
+
+		/* Fold this chip's status into the combined status. */
+
+		status |= (x & or_mask);
+		status &= (x & and_mask) | or_mask;
+
+	}
+
+	add_event("Exiting mal_get_status", -1);
+
+	return status;
+
+}
+
+/**
+ * mal_read_a_page() - Abstracted page read.
+ *
+ * This function returns the ECC status for the entire read operation. A
+ * positive return value indicates the number of errors that were corrected
+ * (symbol errors for Reed-Solomon hardware engines, bit errors for BCH hardware
+ * engines). A negative return value indicates that the ECC engine failed to
+ * correct all errors and the data is corrupted. A zero return value indicates
+ * there were no errors at all.
+ *
+ * @this:    Per-device data.
+ * @use_ecc: Indicates if we're to use ECC.
+ * @chip:    The logical chip of interest.
+ * @page:    The logical page number to read.
+ * @data:    A pointer to the destination data buffer. If this pointer is null,
+ *           that indicates the caller doesn't want the data.
+ * @oob:     A pointer to the destination OOB buffer. If this pointer is null,
+ *           that indicates the caller doesn't want the OOB.
+ */
+static int mal_read_a_page(struct imx_nfc_data *this, int use_ecc,
+		unsigned chip, unsigned page, uint8_t *data, uint8_t *oob)
+{
+	int                       we_are_interleaving;
+	int                       use_automatic_op;
+	unsigned int              start;
+	unsigned int              end;
+	unsigned int              current_chip;
+	unsigned int              oob_bytes_to_copy;
+	unsigned int              data_bytes_to_copy;
+	int                       status;
+	unsigned int              worst_case_ecc_status;
+	int                       return_value = 0;
+	struct physical_geometry  *physical = &this->physical_geometry;
+	struct nfc_geometry       *nfc      = this->nfc_geometry;
+	struct logical_geometry   *logical  = &this->logical_geometry;
+
+	add_event("Entering mal_read_a_page", 1);
+
+	/* Establish some important facts. */
+
+	we_are_interleaving = logical->chip_count != physical->chip_count;
+	use_automatic_op    = !!this->nfc->start_auto_read;
+
+	/* Apply the automatic operation override, if any. */
+
+	switch (this->auto_op_override) {
+
+	case NEVER:
+		use_automatic_op = false;
+		break;
+
+	case DRIVER_CHOICE:
+		break;
+
+	case ALWAYS:
+		if (this->nfc->start_auto_read)
+			use_automatic_op = true;
+		break;
+
+	}
+
+	/* Set up ECC. */
+
+	this->nfc->set_ecc(this, use_ecc);
+
+	/* Check if we're interleaving and set up the loop iterations. */
+
+	if (we_are_interleaving) {
+
+		start = 0;
+		end   = physical->chip_count;
+
+		data_bytes_to_copy =
+			this->logical_geometry.page_data_size /
+					this->physical_geometry.chip_count;
+		oob_bytes_to_copy =
+			this->logical_geometry.page_oob_size /
+					this->physical_geometry.chip_count;
+
+	} else {
+
+		start = chip;
+		end   = start + 1;
+
+		data_bytes_to_copy = this->logical_geometry.page_data_size;
+		oob_bytes_to_copy  = this->logical_geometry.page_oob_size;
+
+	}
+
+	/* If we're using the automatic operation, start it now. */
+
+	if (use_automatic_op) {
+		add_event("Starting the automatic operation...", 0);
+		this->nfc->start_auto_read(this, start, end - start, 0, page);
+	}
+
+	/* Loop over physical chips. */
+
+	add_event("Looping over physical chips...", 0);
+
+	for (current_chip = start; current_chip < end; current_chip++) {
+
+		/* Check if we're using the automatic operation. */
+
+		if (use_automatic_op) {
+
+			add_event("Waiting...", 0);
+			this->nfc->wait_for_auto_read(this);
+
+		} else {
+
+			/* Select the current chip. */
+
+			this->nfc->select_chip(this, current_chip);
+
+			/* Set up the chip. */
+
+			add_event("Sending the command and addresses...", 0);
+
+			nfc_util_send_cmd_and_addrs(this,
+						NAND_CMD_READ0, 0, page);
+
+			if (is_large_page_chip(this)) {
+				add_event("Sending the final command...", 0);
+				nfc_util_send_cmd(this, NAND_CMD_READSTART);
+			}
+
+			/* Wait till the page is ready. */
+
+			add_event("Waiting for the page to arrive...", 0);
+
+			nfc_util_wait_for_the_nfc(this, true);
+
+			/* Read the page. */
+
+			add_event("Reading the page...", 0);
+
+			this->nfc->read_page(this);
+
+		}
+
+		/* Copy a page out of the NFC. */
+
+		add_event("Copying from the NFC...", 0);
+
+		if (oob) {
+			nfc_util_copy_from_the_nfc(this,
+				nfc->page_data_size, oob, oob_bytes_to_copy);
+			oob += oob_bytes_to_copy;
+		}
+
+		if (data) {
+			nfc_util_copy_from_the_nfc(this,
+						0, data, data_bytes_to_copy);
+			data += data_bytes_to_copy;
+		}
+
+		/*
+		 * If we're using ECC, and we haven't already seen an ECC
+		 * failure, continue to gather ECC status. Note that, if we
+		 * *do* see an ECC failure, we continue to read because the
+		 * client might want the data for forensic purposes.
+		 */
+
+		if (use_ecc && (return_value >= 0)) {
+
+			add_event("Getting ECC status...", 0);
+
+			status = this->nfc->get_ecc_status(this);
+
+			if (status >= 0)
+				return_value += status;
+			else
+				return_value = -1;
+
+		}
+
+		/* Check if we're using the automatic operation. */
+
+		if (use_automatic_op) {
+
+			/*
+			 * If this is not the last iteration, resume the
+			 * automatic operation.
+			 */
+
+			if (current_chip < (end - 1)) {
+				add_event("Resuming...", 0);
+				this->nfc->resume_auto_read(this);
+			}
+
+		}
+
+	}
+
+	/* Check if we're supposed to inject an ECC error. */
+
+	if (use_ecc && this->inject_ecc_error) {
+
+		/* Inject the appropriate error. */
+
+		if (this->inject_ecc_error < 0) {
+
+			add_event("Injecting an uncorrectable error...", 0);
+
+			return_value = -1;
+
+		} else {
+
+			add_event("Injecting correctable errors...", 0);
+
+			worst_case_ecc_status =
+				physical->chip_count *
+				nfc->buffer_count *
+				nfc->ecc_strength;
+
+			if (this->inject_ecc_error > worst_case_ecc_status)
+				return_value = worst_case_ecc_status;
+			else
+				return_value = this->inject_ecc_error;
+
+		}
+
+		/* Stop injecting further errors. */
+
+		this->inject_ecc_error = 0;
+
+	}
+
+	/* Return. */
+
+	add_event("Exiting mal_read_a_page", -1);
+
+	return return_value;
+
+}
+
+/**
+ * mal_write_a_page() - Abstracted page write.
+ *
+ * This function returns zero if the operation succeeded, or -EIO if the
+ * operation failed.
+ *
+ * @this:    Per-device data.
+ * @use_ecc: Indicates if we're to use ECC.
+ * @chip:    The logical chip of interest.
+ * @page:    The logical page number to write.
+ * @data:    A pointer to the source data buffer.
+ * @oob:     A pointer to the source OOB buffer.
+ */
+static int mal_write_a_page(struct imx_nfc_data *this, int use_ecc,
+	unsigned chip, unsigned page, const uint8_t *data, const uint8_t *oob)
+{
+	int                       we_are_interleaving;
+	int                       use_automatic_op;
+	unsigned int              start;
+	unsigned int              end;
+	unsigned int              current_chip;
+	unsigned int              oob_bytes_to_copy;
+	unsigned int              data_bytes_to_copy;
+	int                       return_value = 0;
+	struct physical_geometry  *physical = &this->physical_geometry;
+	struct nfc_geometry       *nfc      = this->nfc_geometry;
+	struct logical_geometry   *logical  = &this->logical_geometry;
+
+	add_event("Entering mal_write_a_page", 1);
+
+	/* Establish some important facts. */
+
+	we_are_interleaving = logical->chip_count != physical->chip_count;
+	use_automatic_op    = !!this->nfc->start_auto_write;
+
+	/* Apply the automatic operation override, if any. */
+
+	switch (this->auto_op_override) {
+
+	case NEVER:
+		use_automatic_op = false;
+		break;
+
+	case DRIVER_CHOICE:
+		break;
+
+	case ALWAYS:
+		if (this->nfc->start_auto_write)
+			use_automatic_op = true;
+		break;
+
+	}
+
+	/* Set up ECC. */
+
+	this->nfc->set_ecc(this, use_ecc);
+
+	/* Check if we're interleaving and set up the loop iterations. */
+
+	if (we_are_interleaving) {
+
+		start = 0;
+		end   = physical->chip_count;
+
+		data_bytes_to_copy =
+			this->logical_geometry.page_data_size /
+					this->physical_geometry.chip_count;
+		oob_bytes_to_copy =
+			this->logical_geometry.page_oob_size /
+					this->physical_geometry.chip_count;
+
+	} else {
+
+		start = chip;
+		end   = start + 1;
+
+		data_bytes_to_copy = this->logical_geometry.page_data_size;
+		oob_bytes_to_copy  = this->logical_geometry.page_oob_size;
+
+	}
+
+	/* If we're using the automatic operation, start the hardware now. */
+
+	if (use_automatic_op) {
+		add_event("Starting the automatic operation...", 0);
+		this->nfc->start_auto_write(this, start, end - start, 0, page);
+	}
+
+	/* Loop over physical chips. */
+
+	add_event("Looping over physical chips...", 0);
+
+	for (current_chip = start; current_chip < end; current_chip++) {
+
+		/* Copy a page into the NFC. */
+
+		add_event("Copying to the NFC...", 0);
+
+		nfc_util_copy_to_the_nfc(this, oob, nfc->page_data_size,
+							oob_bytes_to_copy);
+		oob += oob_bytes_to_copy;
+
+		nfc_util_copy_to_the_nfc(this, data, 0, data_bytes_to_copy);
+
+		data += data_bytes_to_copy;
+
+		/* Check if we're using the automatic operation. */
+
+		if (use_automatic_op) {
+
+			/* Wait for the write operation to finish. */
+
+			add_event("Waiting...", 0);
+
+			this->nfc->wait_for_auto_write(this);
+
+		} else {
+
+			/* Select the current chip. */
+
+			this->nfc->select_chip(this, current_chip);
+
+			/* Set up the chip. */
+
+			add_event("Sending the command and addresses...", 0);
+
+			nfc_util_send_cmd_and_addrs(this,
+						NAND_CMD_SEQIN, 0, page);
+
+			/* Send the page. */
+
+			add_event("Sending the page...", 0);
+
+			this->nfc->send_page(this);
+
+			/* Start programming the page. */
+
+			add_event("Programming the page...", 0);
+
+			nfc_util_send_cmd(this, NAND_CMD_PAGEPROG);
+
+			/* Wait until the page is finished. */
+
+			add_event("Waiting...", 0);
+
+			nfc_util_wait_for_the_nfc(this, true);
+
+		}
+
+	}
+
+	/* Get status. */
+
+	add_event("Gathering status...", 0);
+
+	if (mal_get_status(this, chip) & NAND_STATUS_FAIL) {
+		add_event("Bad status", 0);
+		return_value = -EIO;
+	} else {
+		add_event("Good status", 0);
+	}
+
+	/* Return. */
+
+	add_event("Exiting mal_write_a_page", -1);
+
+	return return_value;
+
+}
+
+/**
+ * mal_erase_a_block() - Abstract block erase operation.
+ *
+ * Note that this function does *not* wait for the operation to finish. The
+ * caller is expected to call waitfunc() at some later time.
+ *
+ * @this:  Per-device data.
+ * @chip:  The logical chip of interest.
+ * @page:  A logical page address that identifies the block to erase.
+ */
+static void mal_erase_a_block(struct imx_nfc_data *this,
+						unsigned chip, unsigned page)
+{
+	int                       we_are_interleaving;
+	int                       use_automatic_op;
+	unsigned int              start;
+	unsigned int              end;
+	unsigned int              i;
+	struct physical_geometry  *physical = &this->physical_geometry;
+	struct logical_geometry   *logical  = &this->logical_geometry;
+
+	add_event("Entering mal_erase_a_block", 1);
+
+	/* Establish some important facts. */
+
+	we_are_interleaving = logical->chip_count != physical->chip_count;
+	use_automatic_op    = !!this->nfc->start_auto_erase;
+
+	/* Apply the automatic operation override, if any. */
+
+	switch (this->auto_op_override) {
+
+	case NEVER:
+		use_automatic_op = false;
+		break;
+
+	case DRIVER_CHOICE:
+		break;
+
+	case ALWAYS:
+		if (this->nfc->start_auto_erase)
+			use_automatic_op = true;
+		break;
+
+	}
+
+	/* Choose the loop bounds. */
+
+	if (we_are_interleaving) {
+		start = 0;
+		end   = physical->chip_count;
+	} else {
+		start = chip;
+		end   = start + 1;
+	}
+
+	/* Check if we're using the automatic operation. */
+
+	if (use_automatic_op) {
+
+		/*
+		 * Start the operation. Note that we don't wait for it to
+		 * finish because the HIL will call our waitfunc().
+		 */
+
+		add_event("Starting the automatic operation...", 0);
+
+		this->nfc->start_auto_erase(this, start, end - start, page);
+
+	} else {
+
+		/* Loop over physical chips. */
+
+		add_event("Looping over physical chips...", 0);
+
+		for (i = start; i < end; i++) {
+
+			/* Select the current chip. */
+
+			this->nfc->select_chip(this, i);
+
+			/* Set up the chip. */
+
+			nfc_util_send_cmd_and_addrs(this,
+						NAND_CMD_ERASE1, -1, page);
+
+			/* Start the erase. */
+
+			nfc_util_send_cmd(this, NAND_CMD_ERASE2);
+
+			/*
+			 * If this is the last time through the loop, break out
+			 * now so we don't try to wait (the HIL will call our
+			 * waitfunc() for the final wait).
+			 */
+
+			if (i >= (end - 1))
+				break;
+
+			/* Wait for the erase on the current chip to finish. */
+
+			nfc_util_wait_for_the_nfc(this, true);
+
+		}
+
+	}
+
+	add_event("Exiting mal_erase_a_block", -1);
+
+}
+
+/**
+ * mal_is_block_bad() - Abstract bad block check.
+ *
+ * @this:    Per-device data.
+ * @chip:    The logical chip of interest.
+ * @page:    The logical page number to read.
+ */
+ #if 0
+
+/* TODO: Finish this function and plug it in. */
+
+static int mal_is_block_bad(struct imx_nfc_data *this,
+						unsigned chip, unsigned page)
+{
+	int                       we_are_interleaving;
+	unsigned int              start;
+	unsigned int              end;
+	unsigned int              i;
+	uint8_t                   *p;
+	int                       return_value = 0;
+	struct nand_chip          *nand = &this->nand;
+	struct physical_geometry  *physical = &this->physical_geometry;
+	struct logical_geometry   *logical  = &this->logical_geometry;
+
+	/* Figure out if we're interleaving. */
+
+	we_are_interleaving = logical->chip_count != physical->chip_count;
+
+	/*
+	 * We're about to use the NAND Flash MTD layer's buffer, so invalidate
+	 * the page cache.
+	 */
+
+	this->nand.pagebuf = -1;
+
+	/*
+	 * Read the OOB of the given page, using the NAND Flash MTD's buffer.
+	 *
+	 * Notice that ECC is off, which it *must* be when scanning block marks.
+	 */
+
+	mal_read_a_page(this, false,
+		this->current_chip, this->page_address, 0, nand->oob_poi);
+
+	/* Choose the loop bounds. */
+
+	if (we_are_interleaving) {
+		start = 0;
+		end   = physical->chip_count;
+	} else {
+		start = chip;
+		end   = start + 1;
+	}
+
+	/* Start scanning at the beginning of the OOB data. */
+
+	p = nand->oob_poi;
+
+	/* Loop over physical chips. */
+
+	add_event("Looping over physical chips...", 0);
+
+	for (i = start; i < end; i++, p += 5) {
+
+		/* Examine the OOB for this chip. */
+
+		if (p[nand->badblockpos] != 0xff) {
+			return_value = !0;
+			break;
+		}
+
+	}
+
+	/* Return. */
+
+	return return_value;
+
+}
+#endif
+
+/**
+ * mil_init() - Initializes the MTD Interface Layer.
+ *
+ * @this:    Per-device data.
+ */
+static void mil_init(struct imx_nfc_data *this)
+{
+	this->current_chip   = -1;    /* No chip is selected yet. */
+	this->command_is_new = false; /* No command yet.          */
+}
+
+/**
+ * mil_cmd_ctrl() - MTD Interface cmd_ctrl()
+ *
+ * @mtd:  A pointer to the owning MTD.
+ * @dat:  The data signals to present to the chip.
+ * @ctrl: The control signals to present to the chip.
+ */
+static void mil_cmd_ctrl(struct mtd_info *mtd, int dat, unsigned int ctrl)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+	unimplemented(this, __func__);
+}
+
+/**
+ * mil_dev_ready() - MTD Interface dev_ready()
+ *
+ * @mtd:   A pointer to the owning MTD.
+ */
+static int mil_dev_ready(struct mtd_info *mtd)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc dev_ready]\n");
+
+	add_event("Entering mil_dev_ready", 1);
+
+	if (this->nfc->is_ready(this)) {
+		add_event("Exiting mil_dev_ready - Returning ready", -1);
+		return !0;
+	} else {
+		add_event("Exiting mil_dev_ready - Returning busy", -1);
+		return 0;
+	}
+
+}
+
+/**
+ * mil_select_chip() - MTD Interface select_chip()
+ *
+ * @mtd:   A pointer to the owning MTD.
+ * @chip:  The chip number to select, or -1 to select no chip.
+ */
+static void mil_select_chip(struct mtd_info *mtd, int chip)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc select_chip] chip: %d\n", chip);
+
+	/* Figure out what kind of transition this is. */
+
+	if ((this->current_chip < 0) && (chip >= 0)) {
+		start_event_trace("Entering mil_select_chip");
+		if (this->pdata->force_ce)
+			this->nfc->set_force_ce(this, true);
+		clk_enable(this->clock);
+		add_event("Exiting mil_select_chip", -1);
+	} else if ((this->current_chip >= 0) && (chip < 0)) {
+		add_event("Entering mil_select_chip", 1);
+		if (this->pdata->force_ce)
+			this->nfc->set_force_ce(this, false);
+		clk_disable(this->clock);
+		stop_event_trace("Exiting mil_select_chip");
+	} else {
+		add_event("Entering mil_select_chip", 1);
+		add_event("Exiting mil_select_chip", -1);
+	}
+
+	this->current_chip = chip;
+
+}
+
+/**
+ * mil_cmdfunc() - MTD Interface cmdfunc()
+ *
+ * This function handles NAND Flash command codes from the HIL. Since only the
+ * HIL calls this function (none of the reference implementations we use do), it
+ * needs to handle very few command codes.
+ *
+ * @mtd:      A pointer to the owning MTD.
+ * @command:  The command code.
+ * @column:   The column address associated with this command code, or -1 if no
+ *            column address applies.
+ * @page:     The page address associated with this command code, or -1 if no
+ *            page address applies.
+ */
+static void mil_cmdfunc(struct mtd_info *mtd,
+					unsigned command, int column, int page)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc cmdfunc] command: 0x%02x, "
+		"column: 0x%04x, page: 0x%06x\n", command, column, page);
+
+	add_event("Entering mil_cmdfunc", 1);
+
+	/* Record the command and the fact that it hasn't yet been sent. */
+
+	this->command        = command;
+	this->command_is_new = true;
+
+	/*
+	 * Process the command code.
+	 *
+	 * Note the default case to trap unrecognized codes. Thus, every command
+	 * we support must have a case here, even if we don't have to do any
+	 * pre-processing work. If the HIL changes and starts sending commands
+	 * we haven't explicitly implemented, this will warn us.
+	 */
+
+	switch (command) {
+
+	case NAND_CMD_READ0:
+		add_event("NAND_CMD_READ0", 0);
+		/*
+		 * After calling this function to send the command and
+		 * addresses, the HIL will call ecc.read_page() or
+		 * ecc.read_page_raw() to collect the data.
+		 *
+		 * The column address from the HIL is always zero. The only
+		 * information we need to keep from this call is the page
+		 * address.
+		 */
+		this->page_address = page;
+		break;
+
+	case NAND_CMD_STATUS:
+		add_event("NAND_CMD_STATUS", 0);
+		/*
+		 * After calling this function to send the command, the HIL
+		 * will call read_byte() once to collect the status.
+		 */
+		break;
+
+	case NAND_CMD_READID:
+		add_event("NAND_CMD_READID", 0);
+		/*
+		 * After calling this function to send the command, the HIL
+		 * will call read_byte() repeatedly to collect ID bytes.
+		 */
+		break;
+
+	case NAND_CMD_RESET:
+		add_event("NAND_CMD_RESET", 0);
+		mal_reset(this, this->current_chip);
+		break;
+
+	default:
+		dev_emerg(this->dev, "Unsupported NAND Flash command code: "
+							"0x%02x\n", command);
+		BUG();
+		break;
+
+	}
+
+	add_event("Exiting mil_cmdfunc", -1);
+
+}
+
+/**
+ * mil_waitfunc() - MTD Interface waifunc()
+ *
+ * This function blocks until the current chip is ready and then returns the
+ * contents of the chip's status register. The HIL only calls this function
+ * after starting an erase operation.
+ *
+ * @mtd:   A pointer to the owning MTD.
+ * @nand:  A pointer to the owning NAND Flash MTD.
+ */
+static int mil_waitfunc(struct mtd_info *mtd, struct nand_chip *nand)
+{
+	int                  status;
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc waitfunc]\n");
+
+	add_event("Entering mil_waitfunc", 1);
+
+	/* Wait for the NFC to finish. */
+
+	nfc_util_wait_for_the_nfc(this, true);
+
+	/* Get the status. */
+
+	status = mal_get_status(this, this->current_chip);
+
+	add_event("Exiting mil_waitfunc", -1);
+
+	return status;
+
+}
+
+/**
+ * mil_read_byte() - MTD Interface read_byte().
+ *
+ * @mtd:  A pointer to the owning MTD.
+ */
+static uint8_t mil_read_byte(struct mtd_info *mtd)
+{
+	uint8_t              byte = 0;
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+
+	add_event("Entering mil_read_byte", 1);
+
+	/*
+	 * The command sent by the HIL before it called this function determines
+	 * how we get the byte we're going to return.
+	 */
+
+	switch (this->command) {
+
+	case NAND_CMD_STATUS:
+		add_event("NAND_CMD_STATUS", 0);
+		byte = mal_get_status(this, this->current_chip);
+		break;
+
+	case NAND_CMD_READID:
+		add_event("NAND_CMD_READID", 0);
+
+		/*
+		 * Check if the command is new. If so, then the HIL just
+		 * recently called cmdfunc(), so the current chip isn't selected
+		 * and the command hasn't been sent to the chip.
+		 */
+
+		if (this->command_is_new) {
+			add_event("Sending the \"Read ID\" command...", 0);
+			this->nfc->select_chip(this, this->current_chip);
+			nfc_util_send_cmd_and_addrs(this,
+							NAND_CMD_READID, 0, -1);
+			this->command_is_new = false;
+		}
+
+		/* Read the ID byte. */
+
+		add_event("Reading the ID byte...", 0);
+
+		byte = this->nfc->read_cycle(this);
+
+		break;
+
+	default:
+		dev_emerg(this->dev, "Unsupported NAND Flash command code: "
+						"0x%02x\n", this->command);
+		BUG();
+		break;
+
+	}
+
+	DEBUG(MTD_DEBUG_LEVEL2,
+			"[imx_nfc read_byte] Returning: 0x%02x\n", byte);
+
+	add_event("Exiting mil_read_byte", -1);
+
+	return byte;
+
+}
+
+/**
+ * mil_read_word() - MTD Interface read_word().
+ *
+ * @mtd:  A pointer to the owning MTD.
+ */
+static uint16_t mil_read_word(struct mtd_info *mtd)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+	unimplemented(this, __func__);
+	return 0;
+}
+
+/**
+ * mil_read_buf() - MTD Interface read_buf().
+ *
+ * @mtd:  A pointer to the owning MTD.
+ * @buf:  The destination buffer.
+ * @len:  The number of bytes to read.
+ */
+static void mil_read_buf(struct mtd_info *mtd, uint8_t * buf, int len)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+	unimplemented(this, __func__);
+}
+
+/**
+ * mil_write_buf() - MTD Interface write_buf().
+ *
+ * @mtd:  A pointer to the owning MTD.
+ * @buf:  The source buffer.
+ * @len:  The number of bytes to read.
+ */
+static void mil_write_buf(struct mtd_info *mtd, const uint8_t * buf, int len)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+	unimplemented(this, __func__);
+}
+
+/**
+ * mil_verify_buf() - MTD Interface verify_buf().
+ *
+ * @mtd:  A pointer to the owning MTD.
+ * @buf:  The destination buffer.
+ * @len:  The number of bytes to read.
+ */
+static int mil_verify_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+	unimplemented(this, __func__);
+	return 0;
+}
+
+/**
+ * mil_ecc_hwctl() - MTD Interface ecc.hwctl().
+ *
+ * @mtd:   A pointer to the owning MTD.
+ * @mode:  The ECC mode.
+ */
+static void mil_ecc_hwctl(struct mtd_info *mtd, int mode)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+	unimplemented(this, __func__);
+}
+
+/**
+ * mil_ecc_calculate() - MTD Interface ecc.calculate().
+ *
+ * @mtd:       A pointer to the owning MTD.
+ * @dat:       A pointer to the source data.
+ * @ecc_code:  A pointer to a buffer that will receive the resulting ECC.
+ */
+static int mil_ecc_calculate(struct mtd_info *mtd,
+					const uint8_t *dat, uint8_t *ecc_code)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+	unimplemented(this, __func__);
+	return 0;
+}
+
+/**
+ * mil_ecc_correct() - MTD Interface ecc.correct().
+ *
+ * @mtd:       A pointer to the owning MTD.
+ * @dat:       A pointer to the source data.
+ * @read_ecc:  A pointer to the ECC that was read from the medium.
+ * @calc_ecc:  A pointer to the ECC that was calculated for the source data.
+ */
+static int mil_ecc_correct(struct mtd_info *mtd,
+			uint8_t *dat, uint8_t *read_ecc, uint8_t *calc_ecc)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+	unimplemented(this, __func__);
+	return 0;
+}
+
+/**
+ * mil_ecc_read_page() - MTD Interface ecc.read_page().
+ *
+ * @mtd:   A pointer to the owning MTD.
+ * @nand:  A pointer to the owning NAND Flash MTD.
+ * @buf:   A pointer to the destination buffer.
+ */
+static int mil_ecc_read_page(struct mtd_info *mtd,
+					struct nand_chip *nand, uint8_t *buf)
+{
+	int                  ecc_status;
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc ecc_read_page]\n");
+
+	add_event("Entering mil_ecc_read_page", 1);
+
+	/* Read the page. */
+
+	ecc_status =
+		mal_read_a_page(this, true, this->current_chip,
+					this->page_address, buf, nand->oob_poi);
+
+	/* Propagate ECC information. */
+
+	if (ecc_status < 0) {
+		add_event("ECC Failure", 0);
+		mtd->ecc_stats.failed++;
+	} else if (ecc_status > 0) {
+		add_event("ECC Corrections", 0);
+		mtd->ecc_stats.corrected += ecc_status;
+	}
+
+	add_event("Exiting mil_ecc_read_page", -1);
+
+	return 0;
+
+}
+
+/**
+ * mil_ecc_read_page_raw() - MTD Interface ecc.read_page_raw().
+ *
+ * @mtd:   A pointer to the owning MTD.
+ * @nand:  A pointer to the owning NAND Flash MTD.
+ * @buf:   A pointer to the destination buffer.
+ */
+static int mil_ecc_read_page_raw(struct mtd_info *mtd,
+					struct nand_chip *nand, uint8_t *buf)
+{
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc ecc_read_page_raw]\n");
+
+	add_event("Entering mil_ecc_read_page_raw", 1);
+
+	mal_read_a_page(this, false, this->current_chip,
+					this->page_address, buf, nand->oob_poi);
+
+	add_event("Exiting mil_ecc_read_page_raw", -1);
+
+	return 0;
+
+}
+
+/**
+ * mil_ecc_write_page() - MTD Interface ecc.write_page().
+ *
+ * @mtd:   A pointer to the owning MTD.
+ * @nand:  A pointer to the owning NAND Flash MTD.
+ * @buf:   A pointer to the source buffer.
+ */
+static void mil_ecc_write_page(struct mtd_info *mtd,
+				struct nand_chip *nand, const uint8_t *buf)
+{
+	struct imx_nfc_data  *this = nand->priv;
+	unimplemented(this, __func__);
+}
+
+/**
+ * mil_ecc_write_page_raw() - MTD Interface ecc.write_page_raw().
+ *
+ * @mtd:   A pointer to the owning MTD.
+ * @nand:  A pointer to the owning NAND Flash MTD.
+ * @buf:   A pointer to the source buffer.
+ */
+static void mil_ecc_write_page_raw(struct mtd_info *mtd,
+				struct nand_chip *nand, const uint8_t *buf)
+{
+	struct imx_nfc_data  *this = nand->priv;
+	unimplemented(this, __func__);
+}
+
+/**
+ * mil_write_page() - MTD Interface ecc.write_page().
+ *
+ * @mtd:     A pointer to the owning MTD.
+ * @nand:    A pointer to the owning NAND Flash MTD.
+ * @buf:     A pointer to the source buffer.
+ * @page:    The page number to write.
+ * @cached:  Indicates cached programming (ignored).
+ * @raw:     Indicates not to use ECC.
+ */
+static int mil_write_page(struct mtd_info *mtd,
+		struct nand_chip *nand, const uint8_t *buf,
+		int page, int cached, int raw)
+{
+	int                  return_value;
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc write_page]\n");
+
+	add_event("Entering mil_write_page", 1);
+
+	return_value = mal_write_a_page(this, !raw,
+				this->current_chip, page, buf, nand->oob_poi);
+
+	add_event("Exiting mil_write_page", -1);
+
+	return return_value;
+
+}
+
+/**
+ * mil_ecc_read_oob() - MTD Interface read_oob().
+ *
+ * @mtd:     A pointer to the owning MTD.
+ * @nand:    A pointer to the owning NAND Flash MTD.
+ * @page:    The page number to read.
+ * @sndcmd:  Indicates this function should send a command to the chip before
+ *           reading the out-of-band bytes. This is only false for small page
+ *           chips that support auto-increment.
+ */
+static int mil_ecc_read_oob(struct mtd_info *mtd, struct nand_chip *nand,
+							int page, int sndcmd)
+{
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc ecc_read_oob] "
+		"page: 0x%06x, sndcmd: %s\n", page, sndcmd ? "Yes" : "No");
+
+	add_event("Entering mil_ecc_read_oob", 1);
+
+	mal_read_a_page(this, false,
+				this->current_chip, page, 0, nand->oob_poi);
+
+	add_event("Exiting mil_ecc_read_oob", -1);
+
+	/*
+	 * Return true, indicating that the next call to this function must send
+	 * a command.
+	 */
+
+	return true;
+
+}
+
+/**
+ * mil_ecc_write_oob() - MTD Interface write_oob().
+ *
+ * @mtd:   A pointer to the owning MTD.
+ * @nand:  A pointer to the owning NAND Flash MTD.
+ * @page:  The page number to write.
+ */
+static int mil_ecc_write_oob(struct mtd_info *mtd,
+					struct nand_chip *nand, int page)
+{
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc ecc_write_oob] page: 0x%06x\n", page);
+
+	/*
+	 * There are fundamental incompatibilities between the i.MX NFC and the
+	 * NAND Flash MTD model that make it essentially impossible to write the
+	 * out-of-band bytes.
+	 */
+
+	dev_emerg(this->dev, "This driver doesn't support writing the OOB\n");
+	WARN_ON(1);
+
+	/* Return status. */
+
+	return -EIO;
+
+}
+
+/**
+ * mil_erase_cmd() - MTD Interface erase_cmd().
+ *
+ * We set the erase_cmd pointer in struct nand_chip to point to this function.
+ * Thus, the HIL will call here for all erase operations.
+ *
+ * Strictly speaking, since the erase_cmd pointer is marked "Internal," we
+ * shouldn't be doing this. However, the only reason the HIL uses that pointer
+ * is to install a different function for erasing conventional NAND Flash or AND
+ * Flash. Since AND Flash is obsolete and we don't support it, this isn't
+ * important.
+ *
+ * Furthermore, to cleanly implement interleaving (which is critical to speeding
+ * up erase operations), we want to "hook into" the operation at the highest
+ * semantic level possible. If we don't hook this function, then the only way
+ * we'll know that an erase is happening is when the HIL calls cmdfunc() with
+ * an erase command. Implementing interleaving at that level is roughly a
+ * billion times less desirable.
+ *
+ * This function does *not* wait for the operation to finish. The HIL will call
+ * waitfunc() later to wait for the operation to finish.
+ *
+ * @mtd:   A pointer to the owning MTD.
+ * @page:  A page address that identifies the block to erase.
+ */
+static void mil_erase_cmd(struct mtd_info *mtd, int page)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc erase_cmd] page: 0x%06x\n", page);
+
+	add_event("Entering mil_erase_cmd", 1);
+
+	mal_erase_a_block(this, this->current_chip, page);
+
+	add_event("Exiting mil_erase_cmd", -1);
+
+}
+
+/**
+ * mil_block_bad() - MTD Interface block_bad().
+ *
+ * @mtd:      A pointer to the owning MTD.
+ * @ofs:      The offset of the block of interest, from the start of the medium.
+ * @getchip:  Indicates this function must acquire the MTD before using it.
+ */
+#if 0
+
+/* TODO: Finish this function and plug it in. */
+
+static int mil_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
+{
+	unsigned int         chip;
+	unsigned int         page;
+	int                  return_value;
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc block_bad] page: 0x%06x\n", page);
+
+	add_event("Entering mil_block_bad", 1);
+
+	/* Compute the logical chip number that contains the given offset. */
+
+	chip = (unsigned int) (ofs >> nand->chip_shift);
+
+	/* Compute the logical page address within the logical chip. */
+
+	page = ((unsigned int) (ofs >> nand->page_shift)) & nand->pagemask;
+
+	/* Check if the block is bad. */
+
+	return_value = mal_is_block_bad(this, chip, page);
+
+	if (return_value)
+		add_event("Bad block", 0);
+
+	/* Return. */
+
+	add_event("Exiting mil_block_bad", -1);
+
+	return return_value;
+
+}
+#endif
+
+/**
+ * mil_scan_bbt() - MTD Interface scan_bbt().
+ *
+ * The HIL calls this function once, when it initializes the NAND Flash MTD.
+ *
+ * Nominally, the purpose of this function is to look for or create the bad
+ * block table. In fact, since the HIL calls this function at the very end of
+ * the initialization process started by nand_scan(), and the HIL doesn't have a
+ * more formal mechanism, everyone "hooks" this function to continue the
+ * initialization process.
+ *
+ * At this point, the physical NAND Flash chips have been identified and
+ * counted, so we know the physical geometry. This enables us to make some
+ * important configuration decisions.
+ *
+ * The return value of this function propogates directly back to this driver's
+ * call to nand_scan(). Anything other than zero will cause this driver to
+ * tear everything down and declare failure.
+ *
+ * @mtd:  A pointer to the owning MTD.
+ */
+static int mil_scan_bbt(struct mtd_info *mtd)
+{
+	struct nand_chip     *nand = mtd->priv;
+	struct imx_nfc_data  *this = nand->priv;
+
+	DEBUG(MTD_DEBUG_LEVEL2, "[imx_nfc scan_bbt] \n");
+
+	add_event("Entering mil_scan_bbt", 1);
+
+	/*
+	 * We replace the erase_cmd() function that the MTD NAND Flash system
+	 * has installed with our own. See mil_erase_cmd() for the reasons.
+	 */
+
+	nand->erase_cmd = mil_erase_cmd;
+
+	/*
+	 * Tell MTD users that the out-of-band area can't be written.
+	 *
+	 * This flag is not part of the standard kernel source tree. It comes
+	 * from a patch that touches both MTD and JFFS2.
+	 *
+	 * The problem is that, without this patch, JFFS2 believes it can write
+	 * the data area and the out-of-band area separately. This is wrong for
+	 * two reasons:
+	 *
+	 *     1)  Our NFC distributes out-of-band bytes throughout the page,
+	 *         intermingled with the data, and covered by the same ECC.
+	 *         Thus, it's not possible to write the out-of-band bytes and
+	 *         data bytes separately.
+	 *
+	 *     2)  Large page (MLC) Flash chips don't support partial page
+	 *         writes. You must write the entire page at a time. Thus, even
+	 *         if our NFC didn't force you to write out-of-band and data
+	 *         bytes together, it would *still* be a bad idea to do
+	 *         otherwise.
+	 */
+
+	mtd->flags &= ~MTD_OOB_WRITEABLE;
+
+	/* Set up geometry. */
+
+	mal_set_geometry(this);
+
+	/* We use the reference implementation for bad block management. */
+
+	add_event("Exiting mil_scan_bbt", -1);
+
+	return nand_scan_bbt(mtd, nand->badblock_pattern);
+
+}
+
+/**
+ * parse_bool_param() - Parses the value of a boolean parameter string.
+ *
+ * @s: The string to parse.
+ */
+static int parse_bool_param(const char *s)
+{
+
+	if (!strcmp(s, "1") || !strcmp(s, "on") ||
+				!strcmp(s, "yes") ||	!strcmp(s, "true")) {
+		return 1;
+	} else if (!strcmp(s, "0") || !strcmp(s, "off") ||
+				!strcmp(s, "no") || !strcmp(s, "false")) {
+		return 0;
+	} else {
+		return -1;
+	}
+
+}
+
+/**
+ * set_module_enable() - Controls whether this driver is enabled.
+ *
+ * Note that this state can be controlled from the command line. Disabling this
+ * driver is sometimes useful for debugging.
+ *
+ * @s:   The new value of the parameter.
+ * @kp:  The owning kernel parameter.
+ */
+static int set_module_enable(const char *s, struct kernel_param *kp)
+{
+
+	switch (parse_bool_param(s)) {
+
+	case 1:
+		imx_nfc_module_enable = true;
+		break;
+
+	case 0:
+		imx_nfc_module_enable = false;
+		break;
+
+	default:
+		return -EINVAL;
+		break;
+
+	}
+
+	return 0;
+
+}
+
+/**
+ * get_module_enable() - Indicates whether this driver is enabled.
+ *
+ * @p:   A pointer to a (small) buffer that will receive the response.
+ * @kp:  The owning kernel parameter.
+ */
+static int get_module_enable(char *p, struct kernel_param *kp)
+{
+	p[0] = imx_nfc_module_enable ? '1' : '0';
+	p[1] = 0;
+	return 1;
+}
+
+#ifdef EVENT_REPORTING
+
+/**
+ * set_module_report_events() - Controls whether this driver reports events.
+ *
+ * @s:   The new value of the parameter.
+ * @kp:  The owning kernel parameter.
+ */
+static int set_module_report_events(const char *s, struct kernel_param *kp)
+{
+
+	switch (parse_bool_param(s)) {
+
+	case 1:
+		imx_nfc_module_report_events = true;
+		break;
+
+	case 0:
+		imx_nfc_module_report_events = false;
+		reset_event_trace();
+		break;
+
+	default:
+		return -EINVAL;
+		break;
+
+	}
+
+	return 0;
+
+}
+
+/**
+ * get_module_report_events() - Indicates whether the driver reports events.
+ *
+ * @p:   A pointer to a (small) buffer that will receive the response.
+ * @kp:  The owning kernel parameter.
+ */
+static int get_module_report_events(char *p, struct kernel_param *kp)
+{
+	p[0] = imx_nfc_module_report_events ? '1' : '0';
+	p[1] = 0;
+	return 1;
+}
+
+/**
+ * set_module_dump_events() - Forces the driver to dump current events.
+ *
+ * @s:   The new value of the parameter.
+ * @kp:  The owning kernel parameter.
+ */
+static int set_module_dump_events(const char *s, struct kernel_param *kp)
+{
+	dump_event_trace();
+	return 0;
+}
+
+#endif /*EVENT_REPORTING*/
+
+/**
+ * set_module_interleave_override() - Controls the interleave override.
+ *
+ * @s:   The new value of the parameter.
+ * @kp:  The owning kernel parameter.
+ */
+static int set_module_interleave_override(const char *s,
+							struct kernel_param *kp)
+{
+
+	if      (!strcmp(s, "-1"))
+		imx_nfc_module_interleave_override = NEVER;
+	else if (!strcmp(s, "0"))
+		imx_nfc_module_interleave_override = DRIVER_CHOICE;
+	else if (!strcmp(s, "1"))
+		imx_nfc_module_interleave_override = ALWAYS;
+	else
+		return -EINVAL;
+
+	return 0;
+
+}
+
+/**
+ * get_module_interleave_override() - Indicates the interleave override state.
+ *
+ * @p:   A pointer to a (small) buffer that will receive the response.
+ * @kp:  The owning kernel parameter.
+ */
+static int get_module_interleave_override(char *p, struct kernel_param *kp)
+{
+	return sprintf(p, "%d", imx_nfc_module_interleave_override);
+}
+
+/**
+ * set_force_bytewise_copy() - Controls forced bytewise copy from/to the NFC.
+ *
+ * @s:   The new value of the parameter.
+ * @kp:  The owning kernel parameter.
+ */
+static int set_module_force_bytewise_copy(const char *s,
+							struct kernel_param *kp)
+{
+
+	switch (parse_bool_param(s)) {
+
+	case 1:
+		imx_nfc_module_force_bytewise_copy = true;
+		break;
+
+	case 0:
+		imx_nfc_module_force_bytewise_copy = false;
+		break;
+
+	default:
+		return -EINVAL;
+		break;
+
+	}
+
+	return 0;
+
+}
+
+/**
+ * get_force_bytewise_copy() - Indicates whether bytewise copy is being forced.
+ *
+ * @p:   A pointer to a (small) buffer that will receive the response.
+ * @kp:  The owning kernel parameter.
+ */
+static int get_module_force_bytewise_copy(char *p, struct kernel_param *kp)
+{
+	p[0] = imx_nfc_module_force_bytewise_copy ? '1' : '0';
+	p[1] = 0;
+	return 1;
+}
+
+/* Module attributes that appear in sysfs. */
+
+module_param_call(enable, set_module_enable, get_module_enable, 0, 0444);
+MODULE_PARM_DESC(enable, "enables/disables probing");
+
+#ifdef EVENT_REPORTING
+module_param_call(report_events,
+		set_module_report_events, get_module_report_events, 0, 0644);
+MODULE_PARM_DESC(report_events, "enables/disables event reporting");
+
+module_param_call(dump_events, set_module_dump_events, 0, 0, 0644);
+MODULE_PARM_DESC(dump_events, "forces current event dump");
+#endif
+
+module_param_call(interleave_override, set_module_interleave_override,
+				get_module_interleave_override, 0, 0444);
+MODULE_PARM_DESC(interleave_override, "overrides interleaving choice");
+
+module_param_call(force_bytewise_copy, set_module_force_bytewise_copy,
+				get_module_force_bytewise_copy, 0, 0644);
+MODULE_PARM_DESC(force_bytewise_copy, "forces bytewise copy from/to NFC");
+
+/**
+ * show_device_platform_info() - Shows the device's platform information.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_platform_info(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int                           o = 0;
+	unsigned int                  i;
+	void                          *buffer_base;
+	void                          *primary_base;
+	void                          *secondary_base;
+	unsigned int                  interrupt_number;
+	struct resource               *r;
+	struct imx_nfc_data           *this  = dev_get_drvdata(dev);
+	struct platform_device        *pdev  = this->pdev;
+	struct imx_nfc_platform_data  *pdata = this->pdata;
+	struct mtd_partition          *partition;
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					IMX_NFC_BUFFERS_ADDR_RES_NAME);
+
+	buffer_base = (void *) r->start;
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					IMX_NFC_PRIMARY_REGS_ADDR_RES_NAME);
+
+	primary_base = (void *) r->start;
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					IMX_NFC_SECONDARY_REGS_ADDR_RES_NAME);
+
+	secondary_base = (void *) r->start;
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
+					IMX_NFC_INTERRUPT_RES_NAME);
+
+	interrupt_number = r->start;
+
+	o += sprintf(buf,
+		"NFC Major Version       : %u\n"
+		"NFC Minor Version       : %u\n"
+		"Force CE                : %s\n"
+		"Target Cycle in ns      : %u\n"
+		"Clock Name              : %s\n"
+		"Interleave              : %s\n"
+		"Buffer Base             : 0x%p\n"
+		"Primary Registers Base  : 0x%p\n"
+		"Secondary Registers Base: 0x%p\n"
+		"Interrupt Number        : %u\n"
+		,
+		pdata->nfc_major_version,
+		pdata->nfc_minor_version,
+		pdata->force_ce ? "Yes" : "No",
+		pdata->target_cycle_in_ns,
+		pdata->clock_name,
+		pdata->interleave ? "Yes" : "No",
+		buffer_base,
+		primary_base,
+		secondary_base,
+		interrupt_number
+		);
+
+	#ifdef CONFIG_MTD_PARTITIONS
+
+		o += sprintf(buf + o,
+			"Partition Count         : %u\n"
+			,
+			pdata->partition_count
+		);
+
+		/* Loop over partitions. */
+
+		for (i = 0; i < pdata->partition_count; i++) {
+
+			partition = pdata->partitions + i;
+
+			o += sprintf(buf+o, "  [%d]\n", i);
+			o += sprintf(buf+o, "  Name  : %s\n", partition->name);
+
+			switch (partition->offset) {
+
+			case MTDPART_OFS_NXTBLK:
+				o += sprintf(buf+o, "  Offset: "
+							"MTDPART_OFS_NXTBLK\n");
+				break;
+			case MTDPART_OFS_APPEND:
+				o += sprintf(buf+o, "  Offset: "
+							"MTDPART_OFS_APPEND\n");
+				break;
+			default:
+				o += sprintf(buf+o, "  Offset: %u (%u MiB)\n",
+					partition->offset,
+					partition->offset / (1024 * 1024));
+				break;
+
+			}
+
+			if (partition->size == MTDPART_SIZ_FULL) {
+				o += sprintf(buf+o, "  Size  : "
+							"MTDPART_SIZ_FULL\n");
+			} else {
+				o += sprintf(buf+o, "  Size  : %u (%u MiB)\n",
+					partition->size,
+					partition->size / (1024 * 1024));
+			}
+
+		}
+
+	#endif
+
+	return o;
+
+}
+
+/**
+ * show_device_physical_geometry() - Shows the physical Flash device geometry.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_physical_geometry(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct imx_nfc_data       *this     = dev_get_drvdata(dev);
+	struct physical_geometry  *physical = &this->physical_geometry;
+
+	return sprintf(buf,
+		"Chip Count             : %u\n"
+		"Chip Size in Bytes     : %llu\n"
+		"Block Size in Bytes    : %u\n"
+		"Page Data Size in Bytes: %u\n"
+		"Page OOB Size in Bytes : %u\n"
+		,
+		physical->chip_count,
+		physical->chip_size,
+		physical->block_size,
+		physical->page_data_size,
+		physical->page_oob_size
+	);
+
+}
+
+/**
+ * show_device_nfc_info() - Shows the NFC-specific information.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_nfc_info(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	unsigned long        parent_clock_rate_in_hz;
+	unsigned long        clock_rate_in_hz;
+	struct clk           *parent_clock;
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+	struct nfc_hal       *nfc  = this->nfc;
+
+	parent_clock            = clk_get_parent(this->clock);
+	parent_clock_rate_in_hz = clk_get_rate(parent_clock);
+	clock_rate_in_hz        = clk_get_rate(this->clock);
+
+	return sprintf(buf,
+		"Major Version           : %u\n"
+		"Minor Version           : %u\n"
+		"Max Chip Count          : %u\n"
+		"Max Buffer Count        : %u\n"
+		"Spare Buffer Stride     : %u\n"
+		"Has Secondary Registers : %s\n"
+		"Can Be Symmetric        : %s\n"
+		"Exposes Ready/Busy      : %s\n"
+		"Parent Clock Rate in Hz : %lu\n"
+		"Clock Rate in Hz        : %lu\n"
+		"Symmetric Clock         : %s\n"
+		,
+		nfc->major_version,
+		nfc->minor_version,
+		nfc->max_chip_count,
+		nfc->max_buffer_count,
+		nfc->spare_buf_stride,
+		nfc->has_secondary_regs ? "Yes" : "No",
+		nfc->can_be_symmetric ? "Yes" : "No",
+		nfc->is_ready ? "Yes" : "No",
+		parent_clock_rate_in_hz,
+		clock_rate_in_hz,
+		this->nfc->get_symmetric(this) ? "Yes" : "No"
+	);
+
+}
+
+/**
+ * show_device_nfc_geometry() - Shows the NFC view of the device geometry.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_nfc_geometry(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+
+	return sprintf(buf,
+		"Page Data Size in Bytes : %u\n"
+		"Page OOB Size in Bytes  : %u\n"
+		"ECC Algorithm           : %s\n"
+		"ECC Strength            : %u\n"
+		"Buffers in Use          : %u\n"
+		"Spare Buffer Size in Use: %u\n"
+		"Spare Buffer Spillover  : %u\n"
+		,
+		this->nfc_geometry->page_data_size,
+		this->nfc_geometry->page_oob_size,
+		this->nfc_geometry->ecc_algorithm,
+		this->nfc_geometry->ecc_strength,
+		this->nfc_geometry->buffer_count,
+		this->nfc_geometry->spare_buf_size,
+		this->nfc_geometry->spare_buf_spill
+	);
+
+}
+
+/**
+ * show_device_logical_geometry() - Shows the logical device geometry.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_logical_geometry(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct imx_nfc_data      *this    = dev_get_drvdata(dev);
+	struct logical_geometry  *logical = &this->logical_geometry;
+
+	return sprintf(buf,
+		"Chip Count             : %u\n"
+		"Chip Size in Bytes     : %u\n"
+		"Usable Size in Bytes   : %u\n"
+		"Block Size in Bytes    : %u\n"
+		"Page Data Size in Bytes: %u\n"
+		"Page OOB Size in Bytes : %u\n"
+		,
+		logical->chip_count,
+		logical->chip_size,
+		logical->usable_size,
+		logical->block_size,
+		logical->page_data_size,
+		logical->page_oob_size
+	);
+
+}
+
+/**
+ * show_device_mtd_nand_info() - Shows the device's MTD NAND-specific info.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_mtd_nand_info(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int                        o = 0;
+	unsigned int               i;
+	unsigned int               j;
+	static const unsigned int  columns = 8;
+	struct imx_nfc_data        *this = dev_get_drvdata(dev);
+	struct nand_chip           *nand = &this->nand;
+
+	o += sprintf(buf + o,
+		"Options              : 0x%08x\n"
+		"Chip Count           : %u\n"
+		"Chip Size            : %lu\n"
+		"Minimum Writable Size: %u\n"
+		"Page Shift           : %u\n"
+		"Page Mask            : 0x%x\n"
+		"Block Shift          : %u\n"
+		"BBT Block Shift      : %u\n"
+		"Chip Shift           : %u\n"
+		"Block Mark Offset    : %u\n"
+		"Cached Page Number   : %d\n"
+		,
+		nand->options,
+		nand->numchips,
+		nand->chipsize,
+		nand->subpagesize,
+		nand->page_shift,
+		nand->pagemask,
+		nand->phys_erase_shift,
+		nand->bbt_erase_shift,
+		nand->chip_shift,
+		nand->badblockpos,
+		nand->pagebuf
+	);
+
+	o += sprintf(buf + o,
+		"ECC Byte Count       : %u\n"
+		,
+		nand->ecc.layout->eccbytes
+	);
+
+	/* Loop over rows. */
+
+	for (i = 0; (i * columns) < nand->ecc.layout->eccbytes; i++) {
+
+		/* Loop over columns within rows. */
+
+		for (j = 0; j < columns; j++) {
+
+			if (((i * columns) + j) >= nand->ecc.layout->eccbytes)
+				break;
+
+			o += sprintf(buf + o, " %3u",
+				nand->ecc.layout->eccpos[(i * columns) + j]);
+
+		}
+
+		o += sprintf(buf + o, "\n");
+
+	}
+
+	o += sprintf(buf + o,
+		"OOB Available Bytes  : %u\n"
+		,
+		nand->ecc.layout->oobavail
+	);
+
+	j = 0;
+
+	for (i = 0; j < nand->ecc.layout->oobavail; i++) {
+
+		j += nand->ecc.layout->oobfree[i].length;
+
+		o += sprintf(buf + o,
+			"  [%3u, %2u]\n"
+			,
+			nand->ecc.layout->oobfree[i].offset,
+			nand->ecc.layout->oobfree[i].length
+		);
+
+	}
+
+	return o;
+
+}
+
+/**
+ * show_device_mtd_info() - Shows the device's MTD-specific information.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_mtd_info(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int                        o = 0;
+	unsigned int               i;
+	unsigned int               j;
+	static const unsigned int  columns = 8;
+	struct imx_nfc_data        *this = dev_get_drvdata(dev);
+	struct mtd_info            *mtd  = &this->mtd;
+
+	o += sprintf(buf + o,
+		"Name               : %s\n"
+		"Type               : %u\n"
+		"Flags              : 0x%08x\n"
+		"Size in Bytes      : %u\n"
+		"Erase Region Count : %d\n"
+		"Erase Size in Bytes: %u\n"
+		"Write Size in Bytes: %u\n"
+		"OOB Size in Bytes  : %u\n"
+		"Errors Corrected   : %u\n"
+		"Failed Reads       : %u\n"
+		"Bad Block Count    : %u\n"
+		"BBT Block Count    : %u\n"
+		,
+		mtd->name,
+		mtd->type,
+		mtd->flags,
+		mtd->size,
+		mtd->numeraseregions,
+		mtd->erasesize,
+		mtd->writesize,
+		mtd->oobsize,
+		mtd->ecc_stats.corrected,
+		mtd->ecc_stats.failed,
+		mtd->ecc_stats.badblocks,
+		mtd->ecc_stats.bbtblocks
+	);
+
+	o += sprintf(buf + o,
+		"ECC Byte Count     : %u\n"
+		,
+		mtd->ecclayout->eccbytes
+	);
+
+	/* Loop over rows. */
+
+	for (i = 0; (i * columns) < mtd->ecclayout->eccbytes; i++) {
+
+		/* Loop over columns within rows. */
+
+		for (j = 0; j < columns; j++) {
+
+			if (((i * columns) + j) >= mtd->ecclayout->eccbytes)
+				break;
+
+			o += sprintf(buf + o, " %3u",
+				mtd->ecclayout->eccpos[(i * columns) + j]);
+
+		}
+
+		o += sprintf(buf + o, "\n");
+
+	}
+
+	o += sprintf(buf + o,
+		"OOB Available Bytes: %u\n"
+		,
+		mtd->ecclayout->oobavail
+	);
+
+	j = 0;
+
+	for (i = 0; j < mtd->ecclayout->oobavail; i++) {
+
+		j += mtd->ecclayout->oobfree[i].length;
+
+		o += sprintf(buf + o,
+			"  [%3u, %2u]\n"
+			,
+			mtd->ecclayout->oobfree[i].offset,
+			mtd->ecclayout->oobfree[i].length
+		);
+
+	}
+
+	return o;
+
+}
+
+/**
+ * show_device_bbt_pages() - Shows the pages in which BBT's appear.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_bbt_pages(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int                  o = 0;
+	unsigned int         i;
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+	struct nand_chip     *nand = &this->nand;
+
+	/* Loop over main BBT pages. */
+
+	if (nand->bbt_td)
+		for (i = 0; i < NAND_MAX_CHIPS; i++)
+			o += sprintf(buf + o, "%d: 0x%08x\n",
+						i, nand->bbt_td->pages[i]);
+
+	/* Loop over mirror BBT pages. */
+
+	if (nand->bbt_md)
+		for (i = 0; i < NAND_MAX_CHIPS; i++)
+			o += sprintf(buf + o, "%d: 0x%08x\n",
+						i, nand->bbt_md->pages[i]);
+
+	return o;
+
+}
+
+/**
+ * show_device_cycle_in_ns() - Shows the device's cycle in ns.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_cycle_in_ns(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+	return sprintf(buf, "%u\n", get_cycle_in_ns(this));
+}
+
+/**
+ * store_device_cycle_in_ns() - Sets the device's cycle in ns.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer containing a new attribute value.
+ * @size:  The size of the buffer.
+ */
+static ssize_t store_device_cycle_in_ns(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	int                  error;
+	unsigned long        new_cycle_in_ns;
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+
+	/* Look for nonsense. */
+
+	if (!size)
+		return -EINVAL;
+
+	/* Try to understand the new cycle period. */
+
+	if (strict_strtoul(buf, 0, &new_cycle_in_ns))
+		return -EINVAL;
+
+	/* Try to implement the new cycle period. */
+
+	error = this->nfc->set_closest_cycle(this, new_cycle_in_ns);
+
+	if (error)
+		return -EINVAL;
+
+	/* Return success. */
+
+	return size;
+
+}
+
+/**
+ * show_device_interrupt_override() - Shows the device's interrupt override.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_interrupt_override(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+
+	switch (this->interrupt_override) {
+
+	case NEVER:
+		return sprintf(buf, "-1\n");
+		break;
+
+	case DRIVER_CHOICE:
+		return sprintf(buf, "0\n");
+		break;
+
+	case ALWAYS:
+		return sprintf(buf, "1\n");
+		break;
+
+	default:
+		return sprintf(buf, "?\n");
+		break;
+
+	}
+
+}
+
+/**
+ * store_device_interrupt_override() - Sets the device's interrupt override.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer containing a new attribute value.
+ * @size:  The size of the buffer.
+ */
+static ssize_t store_device_interrupt_override(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+
+	if      (!strcmp(buf, "-1"))
+		this->interrupt_override = NEVER;
+	else if (!strcmp(buf, "0"))
+		this->interrupt_override = DRIVER_CHOICE;
+	else if (!strcmp(buf, "1"))
+		this->interrupt_override = ALWAYS;
+	else
+		return -EINVAL;
+
+	return size;
+
+}
+
+/**
+ * show_device_auto_op_override() - Shows the device's automatic op override.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_auto_op_override(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+
+	switch (this->auto_op_override) {
+
+	case NEVER:
+		return sprintf(buf, "-1\n");
+		break;
+
+	case DRIVER_CHOICE:
+		return sprintf(buf, "0\n");
+		break;
+
+	case ALWAYS:
+		return sprintf(buf, "1\n");
+		break;
+
+	default:
+		return sprintf(buf, "?\n");
+		break;
+
+	}
+
+}
+
+/**
+ * store_device_auto_op_override() - Sets the device's automatic op override.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer containing a new attribute value.
+ * @size:  The size of the buffer.
+ */
+static ssize_t store_device_auto_op_override(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+
+	if      (!strcmp(buf, "-1"))
+		this->auto_op_override = NEVER;
+	else if (!strcmp(buf, "0"))
+		this->auto_op_override = DRIVER_CHOICE;
+	else if (!strcmp(buf, "1"))
+		this->auto_op_override = ALWAYS;
+	else
+		return -EINVAL;
+
+	return size;
+
+}
+
+/**
+ * show_device_inject_ecc_error() - Shows the device's error injection flag.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_inject_ecc_error(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", this->inject_ecc_error);
+
+}
+
+/**
+ * store_device_inject_ecc_error() - Sets the device's error injection flag.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer containing a new attribute value.
+ * @size:  The size of the buffer.
+ */
+static ssize_t store_device_inject_ecc_error(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	unsigned long        new_inject_ecc_error;
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+
+	/* Look for nonsense. */
+
+	if (!size)
+		return -EINVAL;
+
+	/* Try to understand the new cycle period. */
+
+	if (strict_strtol(buf, 0, &new_inject_ecc_error))
+		return -EINVAL;
+
+	/* Store the value. */
+
+	this->inject_ecc_error = new_inject_ecc_error;
+
+	/* Return success. */
+
+	return size;
+
+}
+
+/**
+ * store_device_invalidate_page_cache() - Invalidates the device's page cache.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer containing a new attribute value.
+ * @size:  The size of the buffer.
+ */
+static ssize_t store_device_invalidate_page_cache(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct imx_nfc_data  *this = dev_get_drvdata(dev);
+
+	/* Invalidate the page cache. */
+
+	this->nand.pagebuf = -1;
+
+	/* Return success. */
+
+	return size;
+
+}
+
+/* Device attributes that appear in sysfs. */
+
+static DEVICE_ATTR(platform_info    , 0444, show_device_platform_info    , 0);
+static DEVICE_ATTR(physical_geometry, 0444, show_device_physical_geometry, 0);
+static DEVICE_ATTR(nfc_info         , 0444, show_device_nfc_info         , 0);
+static DEVICE_ATTR(nfc_geometry     , 0444, show_device_nfc_geometry     , 0);
+static DEVICE_ATTR(logical_geometry , 0444, show_device_logical_geometry , 0);
+static DEVICE_ATTR(mtd_nand_info    , 0444, show_device_mtd_nand_info    , 0);
+static DEVICE_ATTR(mtd_info         , 0444, show_device_mtd_info         , 0);
+static DEVICE_ATTR(bbt_pages        , 0444, show_device_bbt_pages        , 0);
+
+static DEVICE_ATTR(cycle_in_ns, 0644,
+	show_device_cycle_in_ns, store_device_cycle_in_ns);
+
+static DEVICE_ATTR(interrupt_override, 0644,
+	show_device_interrupt_override, store_device_interrupt_override);
+
+static DEVICE_ATTR(auto_op_override, 0644,
+	show_device_auto_op_override, store_device_auto_op_override);
+
+static DEVICE_ATTR(inject_ecc_error, 0644,
+	show_device_inject_ecc_error, store_device_inject_ecc_error);
+
+static DEVICE_ATTR(invalidate_page_cache, 0644,
+	0, store_device_invalidate_page_cache);
+
+static struct device_attribute *device_attributes[] = {
+	&dev_attr_platform_info,
+	&dev_attr_physical_geometry,
+	&dev_attr_nfc_info,
+	&dev_attr_nfc_geometry,
+	&dev_attr_logical_geometry,
+	&dev_attr_mtd_nand_info,
+	&dev_attr_mtd_info,
+	&dev_attr_bbt_pages,
+	&dev_attr_cycle_in_ns,
+	&dev_attr_interrupt_override,
+	&dev_attr_auto_op_override,
+	&dev_attr_inject_ecc_error,
+	&dev_attr_invalidate_page_cache,
+};
+
+/**
+ * validate_the_platform() - Validates information about the platform.
+ *
+ * Note that this function doesn't validate the NFC version. That's done when
+ * the probing process attempts to configure for the specific hardware version.
+ *
+ * @pdev:	A pointer to the platform device.
+ */
+static int validate_the_platform(struct platform_device *pdev)
+{
+	struct device                 *dev   = &pdev->dev;
+	struct imx_nfc_platform_data  *pdata = pdev->dev.platform_data;
+
+	/* Validate the clock name. */
+
+	if (!pdata->clock_name) {
+		dev_err(dev, "No clock name\n");
+		return -ENXIO;
+	}
+
+	/* Validate the partitions. */
+
+	if ((pdata->partitions && (!pdata->partition_count)) ||
+			(!pdata->partitions && (pdata->partition_count))) {
+		dev_err(dev, "Bad partition data\n");
+		return -ENXIO;
+	}
+
+	/* Return success */
+
+	return 0;
+
+}
+
+/**
+ * set_up_the_nfc_hal() - Sets up for the specific NFC hardware HAL.
+ *
+ * @this:  Per-device data.
+ */
+static int set_up_the_nfc_hal(struct imx_nfc_data *this)
+{
+	unsigned int                  i;
+	struct nfc_hal                *p;
+	struct imx_nfc_platform_data  *pdata = this->pdata;
+
+	for (i = 0; i < ARRAY_SIZE(nfc_hals); i++) {
+
+		p = nfc_hals[i];
+
+		/*
+		 * Restrict to 3.2 until others are fully implemented.
+		 *
+		 * TODO: Remove this.
+		 */
+
+		if ((p->major_version != 3) && (p->minor_version != 2))
+			continue;
+
+		if ((p->major_version == pdata->nfc_major_version) &&
+			(p->minor_version == pdata->nfc_minor_version)) {
+			this->nfc = p;
+			return 0;
+			break;
+		}
+
+	}
+
+	dev_err(this->dev, "Unkown NFC version %d.%d\n",
+			pdata->nfc_major_version, pdata->nfc_minor_version);
+
+	return !0;
+
+}
+
+/**
+ * acquire_resources() - Tries to acquire resources.
+ *
+ * @this:  Per-device data.
+ */
+static int acquire_resources(struct imx_nfc_data *this)
+{
+
+	int                           error  = 0;
+	struct platform_device        *pdev  = this->pdev;
+	struct device                 *dev   = this->dev;
+	struct imx_nfc_platform_data  *pdata = this->pdata;
+	struct resource               *r;
+
+	/* Find the buffers and map them. */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+						IMX_NFC_BUFFERS_ADDR_RES_NAME);
+
+	if (!r) {
+		dev_err(dev, "Can't get '%s'\n", IMX_NFC_BUFFERS_ADDR_RES_NAME);
+		error = -ENXIO;
+		goto exit_buffers;
+	}
+
+	this->buffers = ioremap(r->start, r->end - r->start + 1);
+
+	if (!this->buffers) {
+		dev_err(dev, "Can't remap buffers\n");
+		error = -EIO;
+		goto exit_buffers;
+	}
+
+	/* Find the primary registers and map them. */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					IMX_NFC_PRIMARY_REGS_ADDR_RES_NAME);
+
+	if (!r) {
+		dev_err(dev, "Can't get '%s'\n",
+					IMX_NFC_PRIMARY_REGS_ADDR_RES_NAME);
+		error = -ENXIO;
+		goto exit_primary_registers;
+	}
+
+	this->primary_regs = ioremap(r->start, r->end - r->start + 1);
+
+	if (!this->primary_regs) {
+		dev_err(dev, "Can't remap the primary registers\n");
+		error = -EIO;
+		goto exit_primary_registers;
+	}
+
+	/* Check for secondary registers. */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					IMX_NFC_SECONDARY_REGS_ADDR_RES_NAME);
+
+	if (r && !this->nfc->has_secondary_regs) {
+
+		dev_err(dev, "Resource '%s' should not be present\n",
+					IMX_NFC_SECONDARY_REGS_ADDR_RES_NAME);
+		error = -ENXIO;
+		goto exit_secondary_registers;
+
+	}
+
+	if (this->nfc->has_secondary_regs) {
+
+		if (!r) {
+			dev_err(dev, "Can't get '%s'\n",
+					IMX_NFC_SECONDARY_REGS_ADDR_RES_NAME);
+			error = -ENXIO;
+			goto exit_secondary_registers;
+		}
+
+		this->secondary_regs = ioremap(r->start, r->end - r->start + 1);
+
+		if (!this->secondary_regs) {
+			dev_err(dev,
+				"Can't remap the secondary registers\n");
+			error = -EIO;
+			goto exit_secondary_registers;
+		}
+
+	}
+
+	/* Find out what our interrupt is and try to own it. */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
+						IMX_NFC_INTERRUPT_RES_NAME);
+
+	if (!r) {
+		dev_err(dev, "Can't get '%s'\n", IMX_NFC_INTERRUPT_RES_NAME);
+		error = -ENXIO;
+		goto exit_irq;
+	}
+
+	this->interrupt = r->start;
+
+	error = request_irq(this->interrupt,
+				nfc_util_isr, 0, this->dev->bus_id, this);
+
+	if (error) {
+		dev_err(dev, "Can't own interrupt %d\n", this->interrupt);
+		goto exit_irq;
+	}
+
+	/* Find out the name of our clock and try to own it. */
+
+	this->clock = clk_get(dev, pdata->clock_name);
+
+	if (this->clock == ERR_PTR(-ENOENT)) {
+		dev_err(dev, "Can't get clock '%s'\n", pdata->clock_name);
+		error = -ENXIO;
+		goto exit_clock;
+	}
+
+	/* Return success. */
+
+	return 0;
+
+	/* Error return paths begin here. */
+
+exit_clock:
+	free_irq(this->interrupt, this);
+exit_irq:
+	if (this->secondary_regs)
+		iounmap(this->secondary_regs);
+exit_secondary_registers:
+	iounmap(this->primary_regs);
+exit_primary_registers:
+	iounmap(this->buffers);
+exit_buffers:
+	return error;
+
+}
+
+/**
+ * release_resources() - Releases resources.
+ *
+ * @this:  Per-device data.
+ */
+static void release_resources(struct imx_nfc_data *this)
+{
+
+	/* Release our clock. */
+
+	clk_disable(this->clock);
+	clk_put(this->clock);
+
+	/* Release our interrupt. */
+
+	free_irq(this->interrupt, this);
+
+	/* Release mapped memory. */
+
+	iounmap(this->buffers);
+	iounmap(this->primary_regs);
+	if (this->secondary_regs)
+		iounmap(this->secondary_regs);
+
+}
+
+/**
+ * register_with_mtd() - Registers this medium with MTD.
+ *
+ * @this:  Per-device data.
+ */
+static int register_with_mtd(struct imx_nfc_data *this)
+{
+	int                           error  = 0;
+	struct mtd_info               *mtd   = &this->mtd;
+	struct nand_chip              *nand  = &this->nand;
+	struct device                 *dev   = this->dev;
+	struct imx_nfc_platform_data  *pdata = this->pdata;
+
+	/* Link the MTD structures together, along with our own data. */
+
+	mtd->priv  = nand;
+	nand->priv = this;
+
+	/* Prepare the MTD structure. */
+
+	mtd->owner = THIS_MODULE;
+
+	/*
+	 * Signal Control Functions
+	 */
+
+	nand->cmd_ctrl = mil_cmd_ctrl;
+
+	/*
+	 * Chip Control Functions
+	 *
+	 * Not all of our NFC hardware versions expose Ready/Busy signals. For
+	 * versions that don't, the is_ready function pointer will be NULL. In
+	 * those cases, we leave the dev_ready member unassigned, which will
+	 * cause the HIL to use a reference implementation's algorithm to
+	 * discover when the hardware is ready.
+	 */
+
+	nand->select_chip = mil_select_chip;
+	nand->cmdfunc     = mil_cmdfunc;
+	nand->waitfunc    = mil_waitfunc;
+
+	if (this->nfc->is_ready)
+		nand->dev_ready = mil_dev_ready;
+
+	/*
+	 * Low-level I/O Functions
+	 */
+
+	nand->read_byte  = mil_read_byte;
+	nand->read_word  = mil_read_word;
+	nand->read_buf   = mil_read_buf;
+	nand->write_buf  = mil_write_buf;
+	nand->verify_buf = mil_verify_buf;
+
+	/*
+	 * ECC Control Functions
+	 */
+
+	nand->ecc.hwctl     = mil_ecc_hwctl;
+	nand->ecc.calculate = mil_ecc_calculate;
+	nand->ecc.correct   = mil_ecc_correct;
+
+	/*
+	 * ECC-Aware I/O Functions
+	 */
+
+	nand->ecc.read_page      = mil_ecc_read_page;
+	nand->ecc.read_page_raw  = mil_ecc_read_page_raw;
+	nand->ecc.write_page     = mil_ecc_write_page;
+	nand->ecc.write_page_raw = mil_ecc_write_page_raw;
+
+	/*
+	 * High-level I/O Functions
+	 */
+
+	nand->write_page    = mil_write_page;
+	nand->ecc.read_oob  = mil_ecc_read_oob;
+	nand->ecc.write_oob = mil_ecc_write_oob;
+
+	/*
+	 * Bad Block Marking Functions
+	 *
+	 * We want to use the reference block_bad and block_markbad
+	 * implementations, so we don't assign those members.
+	 */
+
+	nand->scan_bbt = mil_scan_bbt;
+
+	/*
+	 * Error Recovery Functions
+	 *
+	 * We don't fill in the errstat function pointer because it's optional
+	 * and we don't have a need for it.
+	 */
+
+	/*
+	 * Device Attributes
+	 *
+	 * We don't fill in the chip_delay member because we don't have a need
+	 * for it.
+	 *
+	 * We support only 8-bit Flash bus width.
+	 */
+
+	/*
+	 * ECC Attributes
+	 *
+	 * Members that aren't set here are configured by a version-specific
+	 * set_geometry() function.
+	 */
+
+	nand->ecc.mode    = NAND_ECC_HW;
+	nand->ecc.size    = NFC_MAIN_BUF_SIZE;
+	nand->ecc.prepad  = 0;
+	nand->ecc.postpad = 0;
+
+	/*
+	 * Bad Block Management Attributes
+	 *
+	 * We don't fill in the following attributes:
+	 *
+	 *     badblockpos
+	 *     bbt
+	 *     badblock_pattern
+	 *     bbt_erase_shift
+	 *
+	 * These attributes aren't hardware-specific, and the HIL makes fine
+	 * choices without our help.
+	 */
+
+	nand->options |= NAND_USE_FLASH_BBT;
+	nand->bbt_td   = &bbt_main_descriptor;
+	nand->bbt_md   = &bbt_mirror_descriptor;
+
+	/*
+	 * Device Control
+	 *
+	 * We don't fill in the controller attribute. In principle, we could set
+	 * up a structure to represent the controller. However, since it's
+	 * vanishingly improbable that we'll have more than one medium behind
+	 * the controller, it's not worth the effort. We let the HIL handle it.
+	 */
+
+	/*
+	 * Memory-mapped I/O
+	 *
+	 * We don't fill in the following attributes:
+	 *
+	 *     IO_ADDR_R
+	 *     IO_ADDR_W
+	 *
+	 * None of these are necessary because we don't have a memory-mapped
+	 * implementation.
+	 */
+
+	/*
+	 * Install a "fake" ECC layout.
+	 *
+	 * We'll be calling nand_scan() to do the final MTD setup. If we haven't
+	 * already chosen an ECC layout, then nand_scan() will choose one based
+	 * on the part geometry it discovers. Unfortunately, it doesn't make
+	 * good choices. It would be best if we could install the correct ECC
+	 * layout now, before we call nand_scan(). We can't do that because we
+	 * don't know the medium geometry yet. Here, we install a "fake" ECC
+	 * layout just to stop nand_scan() from trying to pick on for itself.
+	 * Later, in imx_nfc_scan_bbt(), when we know the medium geometry, we'll
+	 * install the correct choice.
+	 *
+	 * Of course, this tactic depends critically on nand_scan() not using
+	 * the fake layout before we can install a good one. This is in fact the
+	 * case.
+	 */
+
+	nand->ecc.layout = &nfc_geometry_512_16_RS_ECC1.mtd_layout;
+
+	/*
+	 * Ask the NAND Flash system to scan for chips.
+	 *
+	 * This will fill in reference implementations for all the members of
+	 * the MTD structures that we didn't set, and will make the medium fully
+	 * usable.
+	 */
+
+	error = nand_scan(mtd, this->nfc->max_chip_count);
+
+	if (error) {
+		dev_err(dev, "Chip scan failed\n");
+		error = -ENXIO;
+		goto exit_scan;
+	}
+
+	/* Register the MTD that represents the entire medium. */
+
+	mtd->name = "NAND Flash Medium";
+
+	add_mtd_device(mtd);
+
+	/* Check if we're doing partitions and register MTD's accordingly. */
+
+	#ifdef CONFIG_MTD_PARTITIONS
+
+		/*
+		 * Look for partition information. If we find some, install
+		 * them. Otherwise, use the partitions handed to us by the
+		 * platform.
+		 */
+
+		this->partition_count =
+			parse_mtd_partitions(mtd, partition_source_types,
+							&this->partitions, 0);
+
+		if ((this->partition_count <= 0) && (pdata->partitions)) {
+			this->partition_count = pdata->partition_count;
+			this->partitions      = pdata->partitions;
+		}
+
+		if (this->partitions)
+			add_mtd_partitions(mtd, this->partitions,
+							this->partition_count);
+
+	#endif
+
+	/* Return success. */
+
+	return 0;
+
+	/* Error return paths begin here. */
+
+exit_scan:
+	return error;
+
+}
+
+/**
+ * unregister_with_mtd() - Unregisters this medium with MTD.
+ *
+ * @this:  Per-device data.
+ */
+static void unregister_with_mtd(struct imx_nfc_data *this)
+{
+
+	/* Get MTD to let go. */
+
+	nand_release(&this->mtd);
+
+}
+
+/**
+ * manage_sysfs_files() - Creates/removes sysfs files for this device.
+ *
+ * @this:  Per-device data.
+ */
+static void manage_sysfs_files(struct imx_nfc_data *this, int create)
+{
+	int                      error;
+	unsigned int             i;
+	struct device_attribute  **attr;
+
+	for (i = 0, attr = device_attributes;
+			i < ARRAY_SIZE(device_attributes); i++, attr++) {
+
+		if (create) {
+			error = device_create_file(this->dev, *attr);
+			if (error) {
+				while (--attr >= device_attributes)
+					device_remove_file(this->dev, *attr);
+				return;
+			}
+		} else {
+			device_remove_file(this->dev, *attr);
+		}
+
+	}
+
+}
+
+/**
+ * imx_nfc_probe() - Probes for a device and, if possible, takes ownership.
+ *
+ * @pdev:  A pointer to the platform device.
+ */
+static int imx_nfc_probe(struct platform_device *pdev)
+{
+	int                           error  = 0;
+	char                          *symmetric_clock;
+	struct clk                    *parent_clock;
+	unsigned long                 parent_clock_rate_in_hz;
+	unsigned long                 parent_clock_rate_in_mhz;
+	unsigned long                 nfc_clock_rate_in_hz;
+	unsigned long                 nfc_clock_rate_in_mhz;
+	unsigned long                 clock_divisor;
+	unsigned long                 cycle_in_ns;
+	struct device                 *dev   = &pdev->dev;
+	struct imx_nfc_platform_data  *pdata = pdev->dev.platform_data;
+	struct imx_nfc_data           *this  = 0;
+
+	/* Say hello. */
+
+	dev_info(dev, "Probing...\n");
+
+	/* Check if we're enabled. */
+
+	if (!imx_nfc_module_enable) {
+		dev_info(dev, "Disabled\n");
+		return -ENXIO;
+	}
+
+	/* Validate the platform device data. */
+
+	error = validate_the_platform(pdev);
+
+	if (error)
+		goto exit_validate_platform;
+
+	/* Allocate memory for the per-device data. */
+
+	this = kzalloc(sizeof(*this), GFP_KERNEL);
+
+	if (!this) {
+		dev_err(dev, "Failed to allocate per-device memory\n");
+		error = -ENOMEM;
+		goto exit_allocate_this;
+	}
+
+	/* Link our per-device data to the owning device. */
+
+	platform_set_drvdata(pdev, this);
+
+	/* Fill in the convenience pointers in our per-device data. */
+
+	this->pdev  = pdev;
+	this->dev   = &pdev->dev;
+	this->pdata = pdata;
+
+	/* Initialize the interrupt service pathway. */
+
+	init_completion(&this->done);
+
+	/* Set up the NFC HAL. */
+
+	error = set_up_the_nfc_hal(this);
+
+	if (error)
+		goto exit_set_up_nfc_hal;
+
+	/* Attempt to acquire the resources we need. */
+
+	error = acquire_resources(this);
+
+	if (error)
+		goto exit_acquire_resources;
+
+	/* Initialize the NFC HAL. */
+
+	if (this->nfc->init(this)) {
+		error = -ENXIO;
+		goto exit_nfc_init;
+	}
+
+	/* Tell the platform we're bringing this device up. */
+
+	if (pdata->init)
+		error = pdata->init();
+
+	if (error)
+		goto exit_platform_init;
+
+	/* Report. */
+
+	parent_clock             = clk_get_parent(this->clock);
+	parent_clock_rate_in_hz  = clk_get_rate(parent_clock);
+	parent_clock_rate_in_mhz = parent_clock_rate_in_hz / 1000000;
+	nfc_clock_rate_in_hz     = clk_get_rate(this->clock);
+	nfc_clock_rate_in_mhz    = nfc_clock_rate_in_hz / 1000000;
+
+	clock_divisor   = parent_clock_rate_in_hz / nfc_clock_rate_in_hz;
+	symmetric_clock = this->nfc->get_symmetric(this) ? "Yes" : "No";
+	cycle_in_ns     = get_cycle_in_ns(this);
+
+	dev_dbg(dev, "-------------\n");
+	dev_dbg(dev, "Configuration\n");
+	dev_dbg(dev, "-------------\n");
+	dev_dbg(dev, "NFC Version      : %d.%d\n"  , this->nfc->major_version,
+						     this->nfc->minor_version);
+	dev_dbg(dev, "Buffers          : 0x%p\n"   , this->buffers);
+	dev_dbg(dev, "Primary Regs     : 0x%p\n"   , this->primary_regs);
+	dev_dbg(dev, "Secondary Regs   : 0x%p\n"   , this->secondary_regs);
+	dev_dbg(dev, "Interrupt        : %u\n"     , this->interrupt);
+	dev_dbg(dev, "Clock Name       : %s\n"     , pdata->clock_name);
+	dev_dbg(dev, "Parent Clock Rate: %lu Hz (%lu MHz)\n",
+						     parent_clock_rate_in_hz,
+						     parent_clock_rate_in_mhz);
+	dev_dbg(dev, "Clock Divisor    : %lu\n",     clock_divisor);
+	dev_dbg(dev, "NFC Clock Rate   : %lu Hz (%lu MHz)\n",
+						     nfc_clock_rate_in_hz,
+						     nfc_clock_rate_in_mhz);
+	dev_dbg(dev, "Symmetric Clock  : %s\n"     , symmetric_clock);
+	dev_dbg(dev, "Actual Cycle     : %lu ns\n" , cycle_in_ns);
+	dev_dbg(dev, "Target Cycle     : %u ns\n"  , pdata->target_cycle_in_ns);
+
+	/* Initialize the Medium Abstraction Layer. */
+
+	mal_init(this);
+
+	/* Initialize the MTD Interface Layer. */
+
+	mil_init(this);
+
+	/* Register this medium with MTD. */
+
+	error = register_with_mtd(this);
+
+	if (error)
+		goto exit_mtd_registration;
+
+	/* Create sysfs entries for this device. */
+
+	manage_sysfs_files(this, true);
+
+	/* Return success. */
+
+	return 0;
+
+	/* Error return paths begin here. */
+
+exit_mtd_registration:
+	if (pdata->exit)
+		pdata->exit();
+exit_platform_init:
+	this->nfc->exit(this);
+exit_nfc_init:
+exit_acquire_resources:
+exit_set_up_nfc_hal:
+	platform_set_drvdata(pdev, NULL);
+	kfree(this);
+exit_allocate_this:
+exit_validate_platform:
+	return error;
+
+}
+
+/**
+ * imx_nfc_remove() - Dissociates this driver from the given device.
+ *
+ * @pdev:  A pointer to the device.
+ */
+static int __exit imx_nfc_remove(struct platform_device *pdev)
+{
+	struct imx_nfc_data  *this  = platform_get_drvdata(pdev);
+
+	/* Remove sysfs entries for this device. */
+
+	manage_sysfs_files(this, false);
+
+	/* Unregister with the NAND Flash MTD system. */
+
+	unregister_with_mtd(this);
+
+	/* Tell the platform we're shutting down this device. */
+
+	if (this->pdata->exit)
+		this->pdata->exit();
+
+	/* Shut down the NFC. */
+
+	this->nfc->exit(this);
+
+	/* Release our resources. */
+
+	release_resources(this);
+
+	/* Unlink our per-device data from the platform device. */
+
+	platform_set_drvdata(pdev, NULL);
+
+	/* Free our per-device data. */
+
+	kfree(this);
+
+	/* Return success. */
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+/**
+ * suspend() - Puts the NFC in a low power state.
+ *
+ * Refer to Documentation/driver-model/driver.txt for more information.
+ *
+ * @pdev:  A pointer to the device.
+ * @state: The new power state.
+ */
+
+static int imx_nfc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int                  error = 0;
+	struct imx_nfc_data  *this = platform_get_drvdata(pdev);
+	struct mtd_info      *mtd  = &this->mtd;
+	struct device        *dev  = &this->pdev->dev;
+
+	dev_dbg(dev, "Suspending...\n");
+
+	/* Suspend MTD's use of this device. */
+
+	error = mtd->suspend(mtd);
+
+	/* Suspend the actual hardware. */
+
+	clk_disable(this->clock);
+
+	return error;
+
+}
+
+/**
+ * resume() - Brings the NFC back from a low power state.
+ *
+ * Refer to Documentation/driver-model/driver.txt for more information.
+ *
+ * @pdev:  A pointer to the device.
+ */
+static int imx_nfc_resume(struct platform_device *pdev)
+{
+	struct imx_nfc_data  *this = platform_get_drvdata(pdev);
+	struct mtd_info      *mtd  = &this->mtd;
+	struct device        *dev  = &this->pdev->dev;
+
+	dev_dbg(dev, "Resuming...\n");
+
+	/* Resume MTD's use of this device. */
+
+	mtd->resume(mtd);
+
+	return 0;
+
+}
+
+#else
+
+#define suspend  NULL
+#define resume   NULL
+
+#endif  /* CONFIG_PM */
+
+/*
+ * This structure represents this driver to the platform management system.
+ */
+static struct platform_driver imx_nfc_driver = {
+	.driver = {
+		.name = IMX_NFC_DRIVER_NAME,
+	},
+	.probe   = imx_nfc_probe,
+	.remove  = __exit_p(imx_nfc_remove),
+	.suspend = imx_nfc_suspend,
+	.resume  = imx_nfc_resume,
+};
+
+/**
+ * imx_nfc_init() - Initializes this module.
+ */
+static int __init imx_nfc_init(void)
+{
+
+	pr_info("i.MX NFC driver %s\n", DRIVER_VERSION);
+
+	/* Register this driver with the platform management system. */
+
+	if (platform_driver_register(&imx_nfc_driver) != 0) {
+		pr_err("i.MX NFC driver registration failed\n");
+		return -ENODEV;
+	}
+
+	return 0;
+
+}
+
+/**
+ * imx_nfc_exit() - Deactivates this module.
+ */
+static void __exit imx_nfc_exit(void)
+{
+	pr_debug("i.MX NFC driver exiting...\n");
+	platform_driver_unregister(&imx_nfc_driver);
+}
+
+module_init(imx_nfc_init);
+module_exit(imx_nfc_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("i.MX NAND Flash Controller Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/nand/lba/Makefile b/drivers/mtd/nand/lba/Makefile
new file mode 100644
index 000000000000..0e576bfa856d
--- /dev/null
+++ b/drivers/mtd/nand/lba/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_MTD_NAND_GPMI_LBA) += gpmi_lba.o
+gpmi_lba-objs += gpmi-transport.o lba-core.o lba-blk.o
diff --git a/drivers/mtd/nand/lba/gpmi-transport.c b/drivers/mtd/nand/lba/gpmi-transport.c
new file mode 100644
index 000000000000..4a2f9301e756
--- /dev/null
+++ b/drivers/mtd/nand/lba/gpmi-transport.c
@@ -0,0 +1,828 @@
+/*
+ * Freescale STMP37XX/STMP378X GPMI transport layer for LBA driver
+ *
+ * Author: Dmitrij Frasenyak <sed@embeddedalley.com>
+ * Clock settings and hw init comes from
+ * gpmi driver by Dmitry Pervushin.
+ *
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/ctype.h>
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <asm/div64.h>
+#include <mach/stmp3xxx.h>
+#include <mach/dma.h>
+#include "gpmi.h"
+#include "lba.h"
+
+struct lba_data *g_data;
+static int max_chips = 1;
+static long clk = -1;
+
+struct gpmi_nand_timing gpmi_safe_timing = {
+	.address_setup = 25,
+	.data_setup = 80,
+	.data_hold = 60,
+	.dsample_time = 6,
+};
+
+/******************************************************************************
+ * HW init part
+ ******************************************************************************/
+
+/**
+ * gpmi_irq - IRQ handler
+ *
+ * @irq:	irq no
+ * @context:	IRQ context, pointer to gpmi_nand_data
+ */
+static irqreturn_t gpmi_irq(int irq, void *context)
+{
+	struct lba_data *data = context;
+	int i;
+
+	for (i = 0; i < max_chips; i++) {
+		if (stmp3xxx_dma_is_interrupt(data->nand[i].dma_ch)) {
+			stmp3xxx_dma_clear_interrupt(data->nand[i].dma_ch);
+			complete(&data->nand[i].done);
+		}
+
+	}
+	HW_GPMI_CTRL1_CLR(BM_GPMI_CTRL1_DEV_IRQ | BM_GPMI_CTRL1_TIMEOUT_IRQ);
+	return IRQ_HANDLED;
+}
+
+static inline u32 gpmi_cycles_ceil(u32 ntime, u32 period)
+{
+	int k;
+
+	k = (ntime + period - 1) / period;
+	if (k == 0)
+		k++;
+	return k ;
+}
+
+
+/**
+ * gpmi_set_timings - set GPMI timings
+ * @pdev: pointer to GPMI platform device
+ * @tm: pointer to structure &gpmi_nand_timing with new timings
+ *
+ * During initialization, GPMI uses safe sub-optimal timings, which
+ * can be changed after reading boot control blocks
+ */
+void gpmi_set_timings(struct lba_data *data, struct gpmi_nand_timing *tm)
+{
+	u32 period_ns = 1000000 / clk_get_rate(data->clk) + 1;
+	u32 address_cycles, data_setup_cycles;
+	u32 data_hold_cycles, data_sample_cycles;
+	u32 busy_timeout;
+	u32 t0;
+
+	address_cycles = gpmi_cycles_ceil(tm->address_setup, period_ns);
+	data_setup_cycles = gpmi_cycles_ceil(tm->data_setup, period_ns);
+	data_hold_cycles = gpmi_cycles_ceil(tm->data_hold, period_ns);
+	data_sample_cycles = gpmi_cycles_ceil(tm->dsample_time + period_ns / 4,
+			period_ns / 2);
+	busy_timeout = gpmi_cycles_ceil(10000000 / 4096, period_ns);
+
+	t0 = BF_GPMI_TIMING0_ADDRESS_SETUP(address_cycles) |
+	    BF_GPMI_TIMING0_DATA_SETUP(data_setup_cycles) |
+	    BF_GPMI_TIMING0_DATA_HOLD(data_hold_cycles);
+	HW_GPMI_TIMING0_WR(t0);
+
+	HW_GPMI_TIMING1_WR(BF_GPMI_TIMING1_DEVICE_BUSY_TIMEOUT(busy_timeout));
+
+#ifdef CONFIG_ARCH_STMP378X
+	HW_GPMI_CTRL1_CLR(BM_GPMI_CTRL1_RDN_DELAY);
+	HW_GPMI_CTRL1_SET(BF_GPMI_CTRL1_RDN_DELAY(data_sample_cycles));
+#else
+	HW_GPMI_CTRL1_CLR(BM_GPMI_CTRL1_DSAMPLE_TIME);
+	HW_GPMI_CTRL1_SET(BF_GPMI_CTRL1_DSAMPLE_TIME(data_sample_cycles));
+#endif
+
+}
+
+void queue_plug(struct lba_data *data)
+{
+	clk_enable(data->clk);
+	if (clk <= 0)
+		clk = 24000; /* safe setting, some chips do not work on
+				    speeds >= 24kHz */
+	clk_set_rate(data->clk, clk);
+
+	clk = clk_get_rate(data->clk);
+
+
+	stmp3xxx_reset_block(HW_GPMI_CTRL0_OFFSET + REGS_GPMI_BASE, 1);
+
+	/* write protection OFF	*/
+	HW_GPMI_CTRL1_SET(BM_GPMI_CTRL1_DEV_RESET);
+
+	/* IRQ polarity */
+	HW_GPMI_CTRL1_SET(BM_GPMI_CTRL1_ATA_IRQRDY_POLARITY);
+
+	/* ...and ECC module */
+	/*HW_GPMI_CTRL1_SET(bch_mode());*/
+
+	/* choose NAND mode (1 means ATA, 0 - NAND */
+	HW_GPMI_CTRL1_CLR(BM_GPMI_CTRL1_GPMI_MODE);
+
+	gpmi_set_timings(data, &gpmi_safe_timing);
+}
+
+void queue_release(struct lba_data *data)
+{
+	HW_GPMI_CTRL0_SET(BM_GPMI_CTRL0_SFTRST);
+
+	clk_disable(data->clk);
+}
+
+
+/**
+ * gpmi_init_hw - initialize the hardware
+ * @pdev: pointer to platform device
+ *
+ * Initialize GPMI hardware and set default (safe) timings for NAND access.
+ * Returns error code or 0 on success
+ */
+static int gpmi_init_hw(struct platform_device *pdev, int request_pins)
+{
+	struct lba_data *data = platform_get_drvdata(pdev);
+	char *devname = pdev->dev.bus_id;
+	int err = 0;
+
+	data->clk = clk_get(NULL, "gpmi");
+	if (IS_ERR(data->clk)) {
+		err = PTR_ERR(data->clk);
+		dev_err(&pdev->dev, "cannot set failsafe clockrate\n");
+		goto out;
+	}
+	if (request_pins)
+		gpmi_pinmux_request(devname);
+
+	queue_plug(data);
+
+out:
+	return err;
+}
+/**
+ * gpmi_release_hw - free the hardware
+ * @pdev: pointer to platform device
+ *
+ * In opposite to gpmi_init_hw, release all acquired resources
+ */
+static void gpmi_release_hw(struct platform_device *pdev)
+{
+	struct lba_data *data = platform_get_drvdata(pdev);
+
+	queue_release(data);
+	clk_put(data->clk);
+	gpmi_pinmux_free(pdev->dev.bus_id);
+}
+
+
+/**
+ * gpmi_alloc_buffers - allocate DMA buffers for one chip
+ *
+ * @pdev:	GPMI platform device
+ * @g:		pointer to structure associated with NAND chip
+ *
+ * Allocate buffer using dma_alloc_coherent
+ */
+static int gpmi_alloc_buffers(struct platform_device *pdev,
+			      struct gpmi_perchip_data *g)
+{
+	g->cmd_buffer = dma_alloc_coherent(&pdev->dev,
+					   g->cmd_buffer_size,
+					   &g->cmd_buffer_handle, GFP_DMA);
+	if (!g->cmd_buffer)
+		goto out1;
+
+	g->write_buffer = dma_alloc_coherent(&pdev->dev,
+					     g->write_buffer_size,
+					     &g->write_buffer_handle, GFP_DMA);
+	if (!g->write_buffer)
+		goto out2;
+
+	g->data_buffer = dma_alloc_coherent(&pdev->dev,
+					    g->data_buffer_size,
+					    &g->data_buffer_handle, GFP_DMA);
+	if (!g->data_buffer)
+		goto out3;
+
+	g->oob_buffer = dma_alloc_coherent(&pdev->dev,
+					   g->oob_buffer_size,
+					   &g->oob_buffer_handle, GFP_DMA);
+	if (!g->oob_buffer)
+		goto out4;
+
+	g->cmdtail_buffer = dma_alloc_coherent(&pdev->dev,
+					   g->cmdtail_buffer_size,
+					   &g->cmdtail_buffer_handle, GFP_DMA);
+	if (!g->oob_buffer)
+		goto out5;
+
+	return 0;
+
+out5:
+	dma_free_coherent(&pdev->dev, g->oob_buffer_size,
+			  g->oob_buffer, g->oob_buffer_handle);
+
+out4:
+	dma_free_coherent(&pdev->dev, g->data_buffer_size,
+			  g->data_buffer, g->data_buffer_handle);
+out3:
+	dma_free_coherent(&pdev->dev, g->write_buffer_size,
+			  g->write_buffer, g->write_buffer_handle);
+out2:
+	dma_free_coherent(&pdev->dev, g->cmd_buffer_size,
+			  g->cmd_buffer, g->cmd_buffer_handle);
+out1:
+	return -ENOMEM;
+}
+
+/**
+ * gpmi_free_buffers - free buffers allocated by gpmi_alloc_buffers
+ *
+ * @pdev:	platform device
+ * @g:		pointer to structure associated with NAND chip
+ *
+ * Deallocate buffers on exit
+ */
+static void gpmi_free_buffers(struct platform_device *pdev,
+			      struct gpmi_perchip_data *g)
+{
+	dma_free_coherent(&pdev->dev, g->oob_buffer_size,
+			  g->oob_buffer, g->oob_buffer_handle);
+	dma_free_coherent(&pdev->dev, g->write_buffer_size,
+			  g->write_buffer, g->write_buffer_handle);
+	dma_free_coherent(&pdev->dev, g->cmd_buffer_size,
+			  g->cmd_buffer, g->cmd_buffer_handle);
+	dma_free_coherent(&pdev->dev, g->cmdtail_buffer_size,
+			  g->cmdtail_buffer, g->cmdtail_buffer_handle);
+	dma_free_coherent(&pdev->dev, g->data_buffer_size,
+			  g->data_buffer, g->data_buffer_handle);
+}
+
+
+/******************************************************************************
+ * Arch specific chain_* callbaks
+ ******************************************************************************/
+
+/**
+ * chain_w4r - Initialize descriptor to perform W4R operation
+ *
+ * @chain:	Descriptor to use
+ * @cs:		CS for this operation
+ *
+ * Due to HW bug we have to put W4R into separate desc.
+ */
+static void chain_w4r(struct stmp3xxx_dma_descriptor *chain, int cs)
+{
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_CMDWORDS(4) 	|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD	|
+		BM_APBH_CHn_CMD_NANDWAIT4READY	|
+		BM_APBH_CHn_CMD_NANDLOCK	|
+		BM_APBH_CHn_CMD_CHAIN		|
+		BF_APBH_CHn_CMD_COMMAND(
+					BV_APBH_CHn_CMD_COMMAND__NO_DMA_XFER);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(
+			BV_GPMI_CTRL0_COMMAND_MODE__WAIT_FOR_READY) |
+		BM_GPMI_CTRL0_WORD_LENGTH	|
+		BF_GPMI_CTRL0_ADDRESS(
+				BV_GPMI_CTRL0_ADDRESS__NAND_DATA)   |
+		BF_GPMI_CTRL0_CS(cs);
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] = 0;
+	chain->command->pio_words[3] = 0;
+	chain->command->buf_ptr = 0;
+}
+
+/**
+ * chain_cmd - Initialize descriptor to push CMD to the bus
+ *
+ * @chain:	Descriptor to use
+ * @cmd_handle: dma_addr_t pointer that holds the command
+ * @lba_cmd:    flags and lenghth of this command.
+ * @cs:		CS for this operation
+ *
+ * CLE || CLE+ALE
+ */
+static void chain_cmd(struct stmp3xxx_dma_descriptor *chain,
+		      dma_addr_t cmd_handle,
+		      struct lba_cmd *lba_cmd,
+		      int cs)
+{
+	/* output command */
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_XFER_COUNT(lba_cmd->len) 	|
+		BF_APBH_CHn_CMD_CMDWORDS(3) 			|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD			|
+		BM_APBH_CHn_CMD_NANDLOCK			|
+		BF_APBH_CHn_CMD_COMMAND(BV_APBH_CHn_CMD_COMMAND__DMA_READ);
+	chain->command->cmd |= BM_APBH_CHn_CMD_CHAIN;
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(BV_GPMI_CTRL0_COMMAND_MODE__WRITE) |
+		BM_GPMI_CTRL0_WORD_LENGTH			|
+		BM_GPMI_CTRL0_LOCK_CS				|
+		BF_GPMI_CTRL0_CS(cs)				|
+		BF_GPMI_CTRL0_ADDRESS(BV_GPMI_CTRL0_ADDRESS__NAND_CLE)	|
+		BF_GPMI_CTRL0_XFER_COUNT(lba_cmd->len);
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] = 0;
+	chain->command->buf_ptr = cmd_handle;
+
+	if (lba_cmd->flag & FE_CMD_INC)
+		chain->command->pio_words[0] |=	BM_GPMI_CTRL0_ADDRESS_INCREMENT;
+/*BUG 	if (lba_cmd->flag & FE_W4R) */
+/* 		chain->command->cmd |= BM_APBH_CHn_CMD_NANDWAIT4READY; */
+}
+
+/**
+ * chain_cmd - Initialize descriptor to read data from the bus
+ *
+ * @chain:	Descriptor to use
+ * @data_handle: dma_addr_t pointer to buffer to store data
+ * @data_len:    the size of the data buffer to read
+ * @cmd_handle: dma_addr_t pointer that holds the command
+ * @lba_cmd:    flags and lenghth of this command.
+ * @cs:		CS for this operation
+ */
+static void chain_read_data(struct stmp3xxx_dma_descriptor *chain,
+			     dma_addr_t data_handle,
+			     dma_addr_t data_len,
+			     struct lba_cmd *lba_cmd,
+			     int cs)
+{
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_XFER_COUNT(data_len)	|
+		BF_APBH_CHn_CMD_CMDWORDS(4)			|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD			|
+		BM_APBH_CHn_CMD_NANDLOCK			|
+		BM_APBH_CHn_CMD_CHAIN				|
+		BF_APBH_CHn_CMD_COMMAND(
+			BV_APBH_CHn_CMD_COMMAND__DMA_WRITE);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(
+			BV_GPMI_CTRL0_COMMAND_MODE__READ)	|
+		BM_GPMI_CTRL0_WORD_LENGTH			|
+		BM_GPMI_CTRL0_LOCK_CS				|
+		BF_GPMI_CTRL0_CS(cs)				|
+		BF_GPMI_CTRL0_ADDRESS(
+			BV_GPMI_CTRL0_ADDRESS__NAND_DATA)	|
+		BF_GPMI_CTRL0_XFER_COUNT(data_len);
+
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] = 0;
+	chain->command->pio_words[3] = 0;
+	chain->command->buf_ptr = data_handle;
+
+	if (lba_cmd->flag & FE_CMD_INC)
+		chain->command->pio_words[0] |=	BM_GPMI_CTRL0_ADDRESS_INCREMENT;
+/*BUG 	if (lba_cmd->flag & FE_W4R) */
+/* 		chain->command->cmd |= BM_APBH_CHn_CMD_NANDWAIT4READY; */
+
+}
+
+/**
+ * chain_cmd - Initialize descriptor to read data from the bus
+ *
+ * @chain:	Descriptor to use
+ * @data_handle: dma_addr_t pointer to buffer to read data from
+ * @data_len:    the size of the data buffer to write
+ * @cmd_handle: dma_addr_t pointer that holds the command
+ * @lba_cmd:    flags and lenghth of this command.
+ * @cs:		CS for this operation
+ */
+static void chain_write_data(struct stmp3xxx_dma_descriptor *chain,
+			      dma_addr_t data_handle,
+			      int data_len,
+			      struct lba_cmd *lba_cmd,
+			      int cs)
+{
+
+	chain->command->cmd =
+		BF_APBH_CHn_CMD_XFER_COUNT(data_len)	|
+		BF_APBH_CHn_CMD_CMDWORDS(4)			|
+		BM_APBH_CHn_CMD_WAIT4ENDCMD			|
+		BM_APBH_CHn_CMD_NANDLOCK			|
+		BM_APBH_CHn_CMD_CHAIN				|
+		BF_APBH_CHn_CMD_COMMAND(
+			BV_APBH_CHn_CMD_COMMAND__DMA_READ);
+	chain->command->pio_words[0] =
+		BF_GPMI_CTRL0_COMMAND_MODE(
+			BV_GPMI_CTRL0_COMMAND_MODE__WRITE)	|
+		BM_GPMI_CTRL0_WORD_LENGTH			|
+		BM_GPMI_CTRL0_LOCK_CS				|
+		BF_GPMI_CTRL0_CS(cs)				|
+		BF_GPMI_CTRL0_ADDRESS(
+			BV_GPMI_CTRL0_ADDRESS__NAND_DATA)	|
+		BF_GPMI_CTRL0_XFER_COUNT(data_len);
+
+	chain->command->pio_words[1] = 0;
+	chain->command->pio_words[2] = 0;
+	chain->command->pio_words[3] = 0;
+	chain->command->buf_ptr = data_handle;
+
+	if (lba_cmd->flag & FE_CMD_INC)
+		chain->command->pio_words[0] |=	BM_GPMI_CTRL0_ADDRESS_INCREMENT;
+/*BUG 	if (lba_cmd->flag & FE_W4R) */
+/* 		chain->command->cmd |= BM_APBH_CHn_CMD_NANDWAIT4READY; */
+
+}
+
+
+/******************************************************************************
+ * Interface to arch independent part
+ ******************************************************************************/
+/**
+ * queue_cmd - Setup a chain of descriptors
+ *
+ * @priv:	 private data passed
+ * @cmd_buf:     pointer to command buffer (to be removed)
+ * @cmd_handle:  dma_addr_t pointer that holds the command
+ * @cmd_len:     the size of the command buffer (to be removed)
+ * @data_handle: dma_addr_t pointer to a data buffer
+ * @data_len:    the size of the data buffer
+ * @cmd_flags:   commands flags
+ */
+int queue_cmd(void *priv,
+	      uint8_t *cmd_buf, dma_addr_t cmd_handle, int cmd_len,
+	      dma_addr_t data, int data_len,
+	      struct lba_cmd *cmd_flags)
+{
+
+	struct gpmi_perchip_data *g = priv;
+	unsigned long flags;
+	int idx;
+	int ret = 0;
+	struct stmp3xxx_dma_descriptor *chain ;
+	int i;
+
+	if (!g || !(cmd_buf || cmd_handle))
+		BUG();
+
+	spin_lock_irqsave(&g->lock, flags);
+
+	/* Keep it for debug purpose */
+	chain = g->d;
+	for (i = g->d_tail; i < GPMI_DMA_MAX_CHAIN; i++) {
+		chain[i].command->cmd = 0;
+		chain[i].command->buf_ptr = 0;
+	}
+	/* End */
+
+	if (!cmd_handle) {
+		if (!cmd_buf)
+			BUG();
+		memcpy(g->cmd_buffer, cmd_buf, cmd_len);
+		cmd_handle = g->cmd_buffer_handle;
+	}
+
+	idx = g->d_tail;
+	chain = &g->d[idx];
+
+	do {
+		if (!cmd_flags)
+			BUG();
+
+		if (cmd_flags->flag & FE_W4R) {
+			/* there seems to be HW BUG with W4R flag.
+			 * IRQ controller hangs forever when it's combined
+			 * with real operation.
+			 */
+			chain_w4r(chain, g->cs);
+			chain++; idx++;
+		}
+
+
+		switch (cmd_flags->flag & F_MASK) {
+
+		case F_CMD:
+			chain_cmd(chain, cmd_handle, cmd_flags, g->cs);
+			break;
+		case F_DATA_READ:
+			chain_read_data(chain, data, data_len,
+					cmd_flags, g->cs);
+			break;
+		case F_DATA_WRITE:
+			chain_write_data(chain, data, data_len,
+					 cmd_flags, g->cs);
+			break;
+		default:{
+			if (cmd_flags->flag & FE_END)
+				goto out;
+			else{
+				printk(KERN_ERR "uknown cmd\n");
+				BUG();
+			}
+		}
+		}
+
+
+		chain++; idx++;
+		cmd_handle += cmd_flags->len;
+
+		if (idx >= GPMI_DMA_MAX_CHAIN) {
+			printk(KERN_ERR "to many chains; idx is 0x%x\n", idx);
+			BUG();
+		}
+
+	} while (!((cmd_flags++)->flag & FE_END));
+
+out:
+	if (idx < GPMI_DMA_MAX_CHAIN) {
+		ret = idx;
+		g->d_tail = idx;
+	}
+	spin_unlock_irqrestore(g->lock, flags);
+
+	return ret;
+
+}
+
+dma_addr_t queue_get_cmd_handle(void *priv)
+{
+	struct gpmi_perchip_data *g = priv;
+	return g->cmd_buffer_handle;
+}
+
+uint8_t *queue_get_cmd_ptr(void *priv)
+{
+	struct gpmi_perchip_data *g = priv;
+	return g->cmd_buffer;
+}
+
+dma_addr_t queue_get_data_handle(void *priv)
+{
+	struct gpmi_perchip_data *g = priv;
+	return g->data_buffer_handle;
+}
+
+uint8_t *queue_get_data_ptr(void *priv)
+{
+	struct gpmi_perchip_data *g = priv;
+	return g->data_buffer;
+}
+
+
+/**
+ * queue_run - run the chain
+ *
+ * @priv:	 private data.
+ */
+int queue_run(void *priv)
+{
+	struct gpmi_perchip_data *g = priv;
+
+	if (!g->d_tail)
+		return 0;
+	stmp3xxx_dma_reset_channel(g->dma_ch);
+	stmp3xxx_dma_clear_interrupt(g->dma_ch);
+	stmp3xxx_dma_enable_interrupt(g->dma_ch);
+
+	g->d[g->d_tail-1].command->cmd &= ~(BM_APBH_CHn_CMD_NANDLOCK |
+				       BM_APBH_CHn_CMD_CHAIN);
+	g->d[g->d_tail-1].command->cmd |= BM_APBH_CHn_CMD_IRQONCMPLT ;
+
+	g->d[g->d_tail-1].command->pio_words[0] &= ~BM_GPMI_CTRL0_LOCK_CS;
+
+#ifdef DEBUG
+	/*stmp37cc_dma_print_chain(&g->chain);*/
+#endif
+
+	init_completion(&g->done);
+	stmp3xxx_dma_go(g->dma_ch, g->d, 1);
+	wait_for_completion(&g->done);
+
+	g->d_tail = 0;
+
+	return 0;
+
+}
+
+/******************************************************************************
+ * Platform specific part / chard driver and misc functions
+ ******************************************************************************/
+
+
+
+static int __init lba_probe(struct platform_device *pdev)
+{
+	struct lba_data *data;
+	struct resource *r;
+	struct gpmi_perchip_data *g;
+	int err;
+
+	/* Allocate memory for the device structure (and zero it) */
+	data = kzalloc(sizeof(*data) + sizeof(struct gpmi_perchip_data),
+		       GFP_KERNEL);
+	if (!data) {
+		dev_err(&pdev->dev, "failed to allocate gpmi_nand_data\n");
+		err = -ENOMEM;
+		goto out1;
+	}
+	g_data = data;
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "failed to get resource\n");
+		err = -ENXIO;
+		goto out2;
+	}
+	data->io_base = ioremap(r->start, r->end - r->start + 1);
+	if (!data->io_base) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		err = -EIO;
+		goto out2;
+	}
+
+	r = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!r) {
+		err = -EIO;
+		dev_err(&pdev->dev, "can't get IRQ resource\n");
+		goto out3;
+	}
+	data->irq = r->start;
+
+	platform_set_drvdata(pdev, data);
+	err = gpmi_init_hw(pdev, 1);
+	if (err)
+		goto out3;
+
+
+	err = request_irq(data->irq,
+			  gpmi_irq, 0, pdev->dev.bus_id, data);
+	if (err) {
+		dev_err(&pdev->dev, "can't request GPMI IRQ\n");
+		goto out4;
+	}
+
+	g = data->nand;
+
+	r = platform_get_resource(pdev, IORESOURCE_DMA, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "can't get DMA resource\n");
+		goto out_res;
+	}
+	g->cs = 0;
+	g->dma_ch = r->start;
+
+	err = stmp3xxx_dma_request(g->dma_ch, NULL, pdev->dev.bus_id);
+	if (err) {
+		dev_err(&pdev->dev, "can't request DMA channel 0x%x\n",
+			g->dma_ch);
+		goto out_res;
+	}
+
+	err = stmp3xxx_dma_make_chain(g->dma_ch, &g->chain,
+				      g->d, ARRAY_SIZE(g->d));
+	if (err) {
+		dev_err(&pdev->dev, "can't setup DMA chain\n");
+		stmp3xxx_dma_release(g->dma_ch);
+		goto out_res;
+	}
+
+	g->cmd_buffer_size = GPMI_CMD_BUF_SZ;
+	g->cmdtail_buffer_size = GPMI_CMD_BUF_SZ;
+	g->write_buffer_size = GPMI_WRITE_BUF_SZ;
+	g->data_buffer_size = GPMI_DATA_BUF_SZ;
+	g->oob_buffer_size = GPMI_OOB_BUF_SZ;
+
+	err = gpmi_alloc_buffers(pdev, g);
+	if (err) {
+		dev_err(&pdev->dev, "can't setup buffers\n");
+		stmp3xxx_dma_free_chain(&g->chain);
+		stmp3xxx_dma_release(g->dma_ch);
+		goto out_res;
+	}
+
+	g->dev = pdev;
+	g->chip.priv = g;
+	g->index = 0;
+	g->timing = gpmi_safe_timing;
+
+	g->cmd_buffer_sz =
+		g->write_buffer_sz =
+		g->data_buffer_sz =
+		0;
+	g->valid = !0;	/* mark the data as valid */
+
+
+	lba_core_init(data);
+
+	return 0;
+
+out_res:
+	free_irq(data->irq, data);
+out4:
+	gpmi_release_hw(pdev);
+out3:
+	platform_set_drvdata(pdev, NULL);
+	iounmap(data->io_base);
+out2:
+	kfree(data);
+out1:
+	return err;
+}
+
+static int gpmi_suspend(struct platform_device *pdev, pm_message_t pm)
+{
+	struct lba_data *data = platform_get_drvdata(pdev);
+	int err;
+
+	printk(KERN_INFO "%s: %d\n", __func__, __LINE__);
+	err = lba_core_suspend(pdev, data);
+	if (!err)
+		gpmi_release_hw(pdev);
+
+	return err;
+}
+
+static int gpmi_resume(struct platform_device *pdev)
+{
+	struct lba_data *data = platform_get_drvdata(pdev);
+	int r;
+
+	printk(KERN_INFO "%s: %d\n", __func__, __LINE__);
+	r = gpmi_init_hw(pdev, 1);
+	lba_core_resume(pdev, data);
+	return r;
+}
+
+/**
+ * gpmi_nand_remove - remove a GPMI device
+ *
+ */
+static int __devexit lba_remove(struct platform_device *pdev)
+{
+	struct lba_data *data = platform_get_drvdata(pdev);
+	int i;
+
+	lba_core_remove(data);
+	gpmi_release_hw(pdev);
+	free_irq(data->irq, data);
+
+	for (i = 0; i < max_chips; i++) {
+		if (!data->nand[i].valid)
+			continue;
+		gpmi_free_buffers(pdev, &data->nand[i]);
+		stmp3xxx_dma_free_chain(&data->nand[i].chain);
+		stmp3xxx_dma_release(data->nand[i].dma_ch);
+	}
+	iounmap(data->io_base);
+	kfree(data);
+
+	return 0;
+}
+
+static struct platform_driver lba_driver = {
+	.probe = lba_probe,
+	.remove = __devexit_p(lba_remove),
+	.driver = {
+		   .name = "gpmi",
+		   .owner = THIS_MODULE,
+	},
+	.suspend = gpmi_suspend,
+	.resume = gpmi_resume,
+
+};
+
+
+static int __init lba_init(void)
+{
+	return platform_driver_register(&lba_driver);
+}
+
+static void lba_exit(void)
+{
+	platform_driver_unregister(&lba_driver);
+}
+
+module_init(lba_init);
+module_exit(lba_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/nand/lba/gpmi.h b/drivers/mtd/nand/lba/gpmi.h
new file mode 100644
index 000000000000..a647c948040f
--- /dev/null
+++ b/drivers/mtd/nand/lba/gpmi.h
@@ -0,0 +1,103 @@
+/*
+ * Freescale STMP37XX/STMP378X GPMI (General-Purpose-Media-Interface)
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __DRIVERS_GPMI_H
+#define __DRIVERS_GPMI_H
+
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <mach/stmp3xxx.h>
+#include <mach/dma.h>
+
+#include <mach/gpmi.h>
+#include <mach/regs-gpmi.h>
+#include <mach/regs-apbh.h>
+#ifdef CONFIG_MTD_NAND_GPMI_BCH
+#include <mach/regs-bch.h>
+#endif
+
+
+struct gpmi_nand_timing {
+	u8 data_setup;
+	u8 data_hold;
+	u8 address_setup;
+	u8 dsample_time;
+};
+
+#define GPMI_DMA_MAX_CHAIN	20	/* max DMA commands in chain */
+
+#define GPMI_CMD_BUF_SZ		10
+#define GPMI_DATA_BUF_SZ	4096
+#define GPMI_WRITE_BUF_SZ	4096
+#define GPMI_OOB_BUF_SZ		218
+
+
+struct gpmi_perchip_data {
+	int                     valid;
+	struct nand_chip	chip;
+	struct platform_device  *dev;
+	int index;
+
+	spinlock_t lock;        /* protect chain operations */
+	struct stmp37xx_circ_dma_chain chain;
+	struct stmp3xxx_dma_descriptor d[GPMI_DMA_MAX_CHAIN];
+	int d_tail;
+
+	struct completion	done;
+
+	u8			*cmd_buffer;
+	dma_addr_t		cmd_buffer_handle;
+	int			cmd_buffer_size, cmd_buffer_sz;
+
+	u8			*cmdtail_buffer;
+	dma_addr_t		cmdtail_buffer_handle;
+	int			cmdtail_buffer_size, cmdtail_buffer_sz;
+
+	u8			*write_buffer;
+	dma_addr_t		write_buffer_handle;
+	int			write_buffer_size, write_buffer_sz;
+
+	u8			*data_buffer;
+	dma_addr_t		data_buffer_handle;
+	u8			*data_buffer_cptr;
+	int			data_buffer_size, data_buffer_sz, bytes2read;
+
+	u8			*oob_buffer;
+	dma_addr_t		oob_buffer_handle;
+	int			oob_buffer_size;
+
+	int			cs;
+	unsigned		dma_ch;
+
+	int			ecc_oob_bytes, oob_free;
+
+	struct gpmi_nand_timing timing;
+
+	void *p2w, *oob2w, *p2r, *oob2r;
+	size_t p2w_size, oob2w_size, p2r_size, oob2r_size;
+	dma_addr_t p2w_dma, oob2w_dma, p2r_dma, oob2r_dma;
+	unsigned read_memcpy:1, write_memcpy:1,
+		 read_oob_memcpy:1, write_oob_memcpy:1;
+};
+
+
+extern struct gpmi_nand_timing gpmi_safe_timing;
+
+
+#endif
diff --git a/drivers/mtd/nand/lba/lba-blk.c b/drivers/mtd/nand/lba/lba-blk.c
new file mode 100644
index 000000000000..1991fad4f05d
--- /dev/null
+++ b/drivers/mtd/nand/lba/lba-blk.c
@@ -0,0 +1,345 @@
+/*
+ * Freescale STMP37XX/STMP378X LBA/block driver
+ *
+ * Author: Dmitrij Frasenyak <sed@embeddedalley.com>
+ *
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+
+#include <linux/sched.h>
+#include <linux/kernel.h>	/* printk() */
+#include <linux/slab.h>		/* kmalloc() */
+#include <linux/fs.h>		/* everything... */
+#include <linux/errno.h>	/* error codes */
+#include <linux/kthread.h>
+#include <linux/timer.h>
+#include <linux/types.h>	/* size_t */
+#include <linux/fcntl.h>	/* O_ACCMODE */
+#include <linux/hdreg.h>	/* HDIO_GETGEO */
+#include <linux/kdev_t.h>
+#include <linux/vmalloc.h>
+#include <linux/genhd.h>
+#include <linux/blkdev.h>
+#include <linux/buffer_head.h>	/* invalidate_bdev */
+#include <linux/bio.h>
+#include <linux/dma-mapping.h>
+#include <linux/hdreg.h>
+#include <linux/blkdev.h>
+#include "lba.h"
+
+static int lba_major;
+
+#define LBA_NAME "lba"
+
+#if 0
+#define TAG() printk(KERNE_ERR "%s: %d\n", __func__, __LINE__)
+#else
+#define TAG()
+#endif
+
+/*
+ * The internal representation of our device.
+ */
+struct lba_blk_dev {
+	int size;                       /* Device size in sectors */
+	spinlock_t lock;                /* For mutual exclusion */
+	int users;
+	struct request_queue *queue;    /* The device request queue */
+	struct gendisk *gd;             /* The gendisk structure */
+	struct lba_data *data;          /* pointer from lba core */
+
+	struct task_struct	*thread;
+	struct bio 		*bio_head;
+	struct bio		*bio_tail;
+	wait_queue_head_t	wait_q;
+	struct semaphore        busy;
+
+};
+
+static struct lba_blk_dev *g_lba_blk;
+
+static void blk_add_bio(struct lba_blk_dev *dev, struct bio *bio);
+
+
+/*
+ * Transfer a single BIO.
+ */
+static int lba_blk_xfer_bio(struct lba_blk_dev *dev, struct bio *bio)
+{
+	int i;
+	struct bio_vec *bvec;
+	sector_t sector = bio->bi_sector;
+	enum dma_data_direction dir;
+	int status = 0;
+	int (*lba_xfer)(void *priv,
+			unsigned int sector,
+			unsigned int count,
+			void *buffer,
+			dma_addr_t handle);
+
+	if  (bio_data_dir(bio) == WRITE) {
+		lba_xfer = lba_write_sectors;
+		dir = DMA_TO_DEVICE;
+	} else {
+		lba_xfer = lba_read_sectors;
+		dir = DMA_FROM_DEVICE;
+	}
+
+	/* Fixme: merge segments */
+	bio_for_each_segment(bvec, bio, i) {
+		void *buffer = page_address(bvec->bv_page);
+		dma_addr_t handle ;
+		if (!buffer)
+			BUG();
+		buffer += bvec->bv_offset;
+		handle = dma_map_single(&dev->data->nand->dev->dev,
+					    buffer,
+					    bvec->bv_len,
+					    dir);
+		status = lba_xfer(dev->data->nand, sector,
+				  bvec->bv_len >> 9,
+				  buffer,
+				  handle);
+
+		dma_unmap_single(&dev->data->nand->dev->dev,
+				 handle,
+				 bvec->bv_len,
+				 dir);
+		if (status)
+			break;
+
+		sector += bio_cur_sectors(bio);
+	}
+
+	return status;
+}
+
+
+/*
+ * The direct make request version.
+ */
+static int lba_make_request(struct request_queue *q, struct bio *bio)
+{
+	struct lba_blk_dev *dev = q->queuedata;
+
+	blk_add_bio(dev, bio);
+	return 0;
+}
+
+/*
+ * Open and close.
+ */
+
+static int lba_blk_open(struct block_device *bdev, fmode_t mode)
+{
+	struct lba_blk_dev *dev = bdev->bd_disk->private_data;
+
+	TAG();
+
+	spin_lock_irq(&dev->lock);
+	dev->users++;
+	spin_unlock_irq(&dev->lock);
+	return 0;
+}
+
+
+static int lba_blk_release(struct gendisk *gd, fmode_t mode)
+{
+	struct lba_blk_dev *dev = gd->private_data;
+
+	spin_lock(&dev->lock);
+	dev->users--;
+	spin_unlock(&dev->lock);
+
+	return 0;
+}
+
+static int lba_getgeo(struct block_device *bdev, struct hd_geometry *geo)
+{
+	/*
+	 * get geometry: we have to fake one...  trim the size to a
+	 * multiple of 2048 (1M): tell we have 32 sectors, 64 heads,
+	 * whatever cylinders.
+	 */
+	geo->heads = 1 << 6;
+	geo->sectors = 1 << 5;
+	geo->cylinders = get_capacity(bdev->bd_disk) >> 11;
+	return 0;
+}
+
+/*
+ * Add bio to back of pending list
+ */
+static void blk_add_bio(struct lba_blk_dev *dev, struct bio *bio)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&dev->lock, flags);
+	if (dev->bio_tail) {
+		dev->bio_tail->bi_next = bio;
+		dev->bio_tail = bio;
+	} else
+		dev->bio_head = dev->bio_tail = bio;
+	wake_up(&dev->wait_q);
+	spin_unlock_irqrestore(&dev->lock, flags);
+}
+
+/*
+ * Grab first pending buffer
+ */
+static struct bio *blk_get_bio(struct lba_blk_dev *dev)
+{
+	struct bio *bio;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	bio = dev->bio_head;
+	if (bio) {
+		if (bio == dev->bio_tail) {
+			dev->bio_tail = NULL;
+			dev->bio_head = NULL;
+		}
+		dev->bio_head = bio->bi_next;
+		bio->bi_next = NULL;
+	}
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	return bio;
+}
+
+static int lba_thread(void *data)
+{
+	struct lba_blk_dev *dev = data;
+	struct bio *bio;
+	int status;
+
+	set_user_nice(current, -20);
+
+	while (!kthread_should_stop() || dev->bio_head) {
+
+		wait_event_interruptible(dev->wait_q,
+				dev->bio_head || kthread_should_stop());
+
+		if (!dev->bio_head)
+			continue;
+
+		if (lba_core_lock_mode(dev->data, LBA_MODE_MDP))
+			continue;
+
+		bio = blk_get_bio(dev);
+		status = lba_blk_xfer_bio(dev, bio);
+		bio_endio(bio,  status);
+
+		lba_core_unlock_mode(dev->data);
+	}
+
+	return 0;
+}
+
+
+
+/*
+ * The device operations structure.
+ */
+static struct block_device_operations lba_blk_ops = {
+	.owner           = THIS_MODULE,
+	.open 	         = lba_blk_open,
+	.release 	 = lba_blk_release,
+	.getgeo		=  lba_getgeo,
+};
+
+
+int lba_blk_init(struct lba_data *data)
+{
+
+	struct lba_blk_dev *dev;
+	int err;
+	if (!data)
+		BUG();
+
+	printk(KERN_INFO "LBA block driver v0.1\n");
+	lba_major = LBA_MAJOR;
+	dev = g_lba_blk =  kzalloc(sizeof(struct lba_blk_dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	dev->data = data;
+	register_blkdev(lba_major, "lba");
+
+	spin_lock_init(&dev->lock);
+	init_waitqueue_head(&dev->wait_q);
+	sema_init(&dev->busy, 1);
+
+	dev->queue = blk_alloc_queue(GFP_KERNEL);
+	if (!dev->queue)
+		goto out2;
+	blk_queue_make_request(dev->queue, lba_make_request);
+	/*dev->queue->unplug_fn = lba_unplug_device;*/
+	blk_queue_hardsect_size(dev->queue, 512);
+
+	dev->queue->queuedata = dev;
+	dev->gd = alloc_disk(32);
+	if (!dev->gd) {
+		printk(KERN_ERR "failed to alloc disk\n");
+		goto out3;
+	}
+	dev->size = data->mdp_size  ;
+	printk(KERN_INFO "%s: set capacity of the device to 0x%x\n",
+	       __func__, dev->size);
+	dev->gd->major = lba_major;
+	dev->gd->first_minor = 0;
+	dev->gd->fops = &lba_blk_ops;
+	dev->gd->queue = dev->queue;
+	dev->gd->private_data = dev;
+	snprintf(dev->gd->disk_name, 8, LBA_NAME);
+	set_capacity(dev->gd, dev->size);
+
+	dev->thread = kthread_create(lba_thread, dev, "lba-%d", 1);
+	if (IS_ERR(dev->thread)) {
+		err = PTR_ERR(dev->thread);
+		goto out3;
+	}
+	wake_up_process(dev->thread);
+
+	add_disk(dev->gd);
+
+
+	TAG();
+
+	return 0;
+out3:
+out2:
+	unregister_blkdev(lba_major, "lba");
+	return -ENOMEM;
+}
+
+int lba_blk_remove(struct lba_data *data)
+{
+
+	struct lba_blk_dev *dev = g_lba_blk;
+
+	del_gendisk(dev->gd);
+	kthread_stop(dev->thread);
+	blk_cleanup_queue(dev->queue);
+	put_disk(dev->gd);
+
+	unregister_blkdev(lba_major, LBA_NAME);
+	kfree(dev);
+	return 0;
+}
+
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_BLOCKDEV_MAJOR(254);
diff --git a/drivers/mtd/nand/lba/lba-core.c b/drivers/mtd/nand/lba/lba-core.c
new file mode 100644
index 000000000000..cdf28ca42b2a
--- /dev/null
+++ b/drivers/mtd/nand/lba/lba-core.c
@@ -0,0 +1,619 @@
+/*
+ * Freescale STMP37XX/STMP378X LBA/core driver
+ *
+ * Author: Dmitrij Frasenyak <sed@embeddedalley.com>
+ *
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/kthread.h>
+#include <linux/dma-mapping.h>
+#include <linux/ctype.h>
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <mach/stmp3xxx.h>
+#include <mach/dma.h>
+#include "gpmi.h"
+#include "lba.h"
+
+#define LBA_SELFPM_TIMEOUT 2000 /* msecs */
+dma_addr_t g_cmd_handle;
+dma_addr_t g_data_handle;
+uint8_t    *g_data_buffer;
+uint8_t    *g_cmd_buffer;
+
+uint8_t lba_get_status1(void *priv)
+{
+	uint8_t cmd_buf[] = { 0x70 } ;
+	struct lba_cmd lba_flags[] = {
+		{1 , F_CMD |  FE_W4R},
+		{0,  F_DATA_READ | FE_END},
+	};
+	*g_data_buffer = 0;
+	queue_cmd(priv, cmd_buf, 0, 1, g_data_handle, 1, lba_flags);
+	queue_run(priv);
+	return *g_data_buffer;
+}
+
+
+int lba_wait_for_ready(void *priv)
+{
+	int stat;
+	unsigned long j_start = jiffies;
+
+	stat = lba_get_status1(priv);
+	if ((stat & 0x60) != 0x60) {
+		while (((stat & 0x60) != 0x60) &&
+		       (jiffies - j_start < msecs_to_jiffies(2000))) {
+			schedule();
+			stat = lba_get_status1(priv);
+		}
+	}
+	if (stat != 0x60)
+		return stat;
+
+	return 0;
+}
+
+int lba_write_sectors(void *priv, unsigned int sector,	unsigned int count,
+		      void *buffer, dma_addr_t handle)
+{
+	uint8_t cmd_buf[] = {
+		0x80,
+		count & 0xff, (count >> 8) & 0xff, /* Count */
+		(sector & 0xff), (sector >> 8) & 0xff, /* Address */
+		(sector >> 16) & 0xff, (sector >> 24) & 0xff, /* Addres */
+		/* Data goes here */
+		0x10
+	};
+
+	struct lba_cmd flags_t1[] = { /* Transmition mode 1/A */
+		{7 , F_CMD | FE_CMD_INC | FE_W4R},
+		{0,  F_DATA_WRITE},
+		{1 , F_CMD | FE_END}
+	};
+
+	if (count > 8)
+		return -EINVAL;
+
+	if (lba_wait_for_ready(priv))
+		return -EIO;
+
+	while (count) {
+		int cnt = (count < 8) ? count : 8;
+		int data_len = cnt * 512;
+
+		queue_cmd(priv, cmd_buf, 0, 8,
+			  handle, data_len, flags_t1);
+
+		handle += data_len;
+		count -= cnt;
+
+	}
+
+	queue_run(priv);
+
+	return count;
+
+}
+
+int lba_read_sectors(void *priv, unsigned int sector,	unsigned int count,
+		      void *buffer, dma_addr_t handle)
+{
+
+	int data_len;
+	int cnt;
+	uint8_t cmd_buf[] = {
+		0x00,
+		count & 0xff, (count >> 8) & 0xff, /* Count */
+		(sector & 0xff), (sector >> 8) & 0xff, /* Addr */
+		(sector >> 16) & 0xff, (sector >> 24) & 0xff, /* Addr */
+		0x30
+		/* Data goes here <data> */
+
+	};
+
+	struct lba_cmd flags_r3[] = { /* Read mode 3/A */
+		{7 , F_CMD | FE_CMD_INC | FE_W4R},
+		{1 , F_CMD },
+		{0 , F_DATA_READ | FE_W4R | FE_END  },
+	};
+	struct lba_cmd flags_r3c[] = { /* Read mode 3/A */
+		{0 , F_DATA_READ | FE_W4R | FE_END },
+	};
+	struct lba_cmd *flags = flags_r3;
+	int flags_len = 8;
+
+	if (count > 8)
+		return -EINVAL;
+
+	if (lba_wait_for_ready(priv))
+		return -EIO;
+
+	while (count) {
+		cnt = (count < 8) ? count : 8;
+		data_len = cnt * 512;
+		queue_cmd(priv, cmd_buf, 0, flags_len, handle, data_len, flags);
+		handle += data_len;
+		count -= cnt;
+		flags = flags_r3c;
+		flags_len = 0;
+	}
+
+	queue_run(priv);
+
+	return count;
+
+}
+
+
+uint8_t lba_get_id1(void *priv, uint8_t *ret_buffer)
+{
+	uint8_t cmd_buf[] = { 0x90 , 0x00, /* Data read 5bytes*/ };
+	struct lba_cmd lba_flags[] = {
+		{2 , F_CMD | FE_CMD_INC | FE_W4R},
+		{0,  F_DATA_READ | FE_END},
+	};
+
+	queue_cmd(priv, cmd_buf, 0, 2, g_data_handle, 5, lba_flags);
+	queue_run(priv);
+	memcpy(ret_buffer, g_data_buffer, 5);
+
+	return 0;
+}
+
+uint8_t lba_get_id2(void *priv, uint8_t *ret_buffer)
+{
+	uint8_t cmd_buf[] = { 0x92 , 0x00, /* Data read 5bytes*/ };
+	struct lba_cmd lba_flags[] = {
+		{2 , F_CMD | FE_CMD_INC | FE_W4R},
+		{0,  F_DATA_READ | FE_END},
+	};
+
+	queue_cmd(priv, cmd_buf, 0, 2, g_data_handle, 5, lba_flags);
+	queue_run(priv);
+	memcpy(ret_buffer, g_data_buffer, 5);
+	return 0;
+}
+
+uint8_t lba_get_status2(void *priv)
+{
+	uint8_t cmd_buf[] = { 0x71 };
+	struct lba_cmd lba_flags[] = {
+		{1 , F_CMD |  FE_CMD_INC | FE_W4R},
+		{0 , F_DATA_READ | FE_END},
+	};
+	*g_data_buffer = 0;
+	queue_cmd(priv, cmd_buf, 0, 1, g_data_handle, 1, lba_flags);
+	queue_run(priv);
+	return *g_data_buffer;
+}
+
+static uint8_t lba_parse_status2(void *priv)
+{
+	uint8_t stat;
+
+	stat = lba_get_status2(priv);
+	printk(KERN_INFO "Status2:|");
+	if (stat & 0x40)
+		printk(" C.PAR.ERR |"); /* no KERN_ here */
+	if (stat & 0x20)
+		printk(" NO spare |");
+	if (stat & 0x10)
+		printk(" ADDR OoRange |");
+	if (stat & 0x8)
+		printk(" high speed |");
+	if ((stat & 0x6) == 6)
+		printk(" MDP |");
+	if ((stat & 0x6) == 4)
+		printk(" VFP |");
+	if ((stat & 0x6) == 2)
+		printk(" PNP |");
+	if (stat & 1)
+		printk(" PSW |");
+
+	printk("\n");
+	return 0;
+}
+
+
+int lba_2mdp(void *priv)
+{
+	uint8_t cmd_buf[] = { 0xFC  };
+	struct lba_cmd lba_flags[] = {
+		{1 , F_CMD | FE_W4R | FE_END}
+	};
+
+	queue_cmd(priv, cmd_buf, 0, 1, 0, 0, lba_flags);
+	queue_run(priv);
+	return 0;
+}
+
+void _lba_misc_cmd_set(void *priv, uint8_t *cmd_buf)
+{
+	struct lba_cmd lba_flags[] = {
+		{6 , F_CMD | FE_CMD_INC | FE_W4R },
+		{1 , F_CMD | FE_END },
+	};
+
+	queue_cmd(priv, cmd_buf, 0, 7, 0, 0, lba_flags);
+	queue_run(priv);
+}
+
+uint8_t _lba_misc_cmd_get(void *priv, uint8_t *cmd_buf)
+{
+	struct lba_cmd lba_flags[] = {
+		{6 , F_CMD | FE_CMD_INC | FE_W4R },
+		{1 , F_CMD  },
+		{0 , F_DATA_READ | FE_W4R | FE_END}
+	};
+
+	queue_cmd(priv, cmd_buf, 0, 7, g_data_handle, 1, lba_flags);
+	queue_run(priv);
+	return *g_data_buffer;
+}
+void lba_mdp2vfp(void *priv, uint8_t pass[2])
+{
+	uint8_t cmd_buf[] = { 0x0, 0xbe, pass[0], pass[1], 0, 0, 0x57 };
+	_lba_misc_cmd_set(priv, cmd_buf);
+}
+
+void lba_bcm2vfp(void *priv, uint8_t pass[2])
+{
+	lba_mdp2vfp(priv, pass);
+}
+
+void lba_powersave_enable(void *priv)
+{
+	uint8_t cmd_buf[] = { 0x0, 0xba, 0, 0, 0, 0, 0x57 };
+	_lba_misc_cmd_set(priv, cmd_buf);
+}
+void lba_powersave_disable(void *priv)
+{
+	uint8_t cmd_buf[] = { 0x0, 0xbb, 0, 0, 0, 0, 0x57 };
+	_lba_misc_cmd_set(priv, cmd_buf);
+}
+
+void lba_highspeed_enable(void *priv)
+{
+	uint8_t cmd_buf[] = { 0x0, 0xbc, 0, 0, 0, 0, 0x57 };
+	_lba_misc_cmd_set(priv, cmd_buf);
+}
+
+void lba_highspeed_disable(void *priv)
+{
+	uint8_t cmd_buf[] = { 0x0, 0xbd, 0, 0, 0, 0, 0x57 };
+	_lba_misc_cmd_set(priv, cmd_buf);
+}
+
+void lba_prot1_set(void *priv, uint8_t mode)
+{
+	uint8_t cmd_buf[] = { 0x0, 0xa2, mode, 0, 0, 0, 0x57 };
+	_lba_misc_cmd_set(priv, cmd_buf);
+}
+
+void lba_prot2_set(void *priv, uint8_t mode)
+{
+	uint8_t cmd_buf[] = { 0x0, 0xa3, mode, 0, 0, 0, 0x57 };
+	_lba_misc_cmd_set(priv, cmd_buf);
+}
+
+uint8_t lba_prot1_get(void *priv)
+{
+	uint8_t cmd_buf[] = { 0x0, 0xb2, 0, 0, 0, 0, 0x57 };
+	return _lba_misc_cmd_get(priv, cmd_buf);
+}
+
+uint8_t lba_prot2_get(void *priv)
+{
+	uint8_t cmd_buf[] = { 0x0, 0xb3, 0, 0, 0, 0, 0x57 };
+	return _lba_misc_cmd_get(priv, cmd_buf);
+}
+
+uint64_t lba_mdp_size_get(void *priv)
+{
+	uint8_t cmd_buf[] = { 0x0, 0xb0, 0, 0, 0, 0, 0x57 };
+	struct lba_cmd lba_flags[] = {
+		{6 , F_CMD | FE_CMD_INC | FE_W4R },
+		{1 , F_CMD  },
+		{0 , F_DATA_READ | FE_W4R | FE_END}
+	};
+
+	memset((void *)g_data_buffer, 0, 8);
+	queue_cmd(priv, cmd_buf, 0, 7, g_data_handle, 5, lba_flags);
+	queue_run(priv);
+	return le64_to_cpu(*(long long *)g_data_buffer);
+}
+
+void lba_cache_flush(void *priv)
+{
+	uint8_t cmd_buf[] = { 0xF9 };
+	struct lba_cmd lba_flags[] = {
+		{1 , F_CMD | FE_W4R },
+		{0 , FE_W4R | FE_END}
+	};
+
+	queue_cmd(priv, cmd_buf, 0, 7, g_data_handle, 5, lba_flags);
+	queue_run(priv);
+}
+
+void lba_reboot(void *priv)
+{
+	uint8_t cmd_buf[] = { 0xFD };
+	struct lba_cmd lba_flags[] = {
+		{1 , F_CMD | FE_W4R },
+		{0 , FE_W4R | FE_END}
+	};
+
+	queue_cmd(priv, cmd_buf, 0, 7, g_data_handle, 5, lba_flags);
+	queue_run(priv);
+}
+
+void lba_def_state(void *priv)
+{
+	lba_wait_for_ready(priv);
+	lba_reboot(priv);
+
+	lba_wait_for_ready(priv);
+	lba_parse_status2(priv);
+
+	lba_wait_for_ready(priv);
+	lba_2mdp(priv);
+
+	lba_wait_for_ready(priv);
+	lba_prot1_set(priv, LBA_T_SIZE8); /* 512 * 8 */
+
+	lba_wait_for_ready(priv);
+/* Type C read; Type A write; */
+	lba_prot2_set(priv, LBA_P_WRITE_A | LBA_P_READ_C);
+}
+
+/*
+ * Should be called with mode locked
+ */
+void lba_core_setvfp_passwd(struct lba_data *data, uint8_t pass[2])
+{
+	memcpy(data->pass, pass, 2);
+}
+
+int lba_core_lock_mode(struct lba_data *data, int mode)
+{
+	void *priv = &data->nand;
+
+	if (down_interruptible(&data->mode_lock))
+		return -EAGAIN;
+	/*
+	 * MDP and VFP are the only supported
+	 * modes for now.
+	 */
+	if ((mode != LBA_MODE_MDP) &&
+	    (mode != LBA_MODE_VFP)) {
+		up(&data->mode_lock);
+		return -EINVAL;
+	}
+
+	while ((data->mode & LBA_MODE_MASK) == LBA_MODE_SUSP) {
+		up(&data->mode_lock);
+
+		if (wait_event_interruptible(
+			    data->suspend_q,
+			    (data->mode & LBA_MODE_MASK) != LBA_MODE_SUSP))
+			return -EAGAIN;
+
+		if (down_interruptible(&data->mode_lock))
+			return -EAGAIN;
+
+		data->last_access = jiffies;
+	}
+
+	if (data->mode & LBA_MODE_SELFPM) {
+		queue_plug(data);
+		data->mode &= ~LBA_MODE_SELFPM;
+	}
+
+	if (mode == data->mode)
+		return 0;
+
+	/*
+	 * mode = VFP || MDP only
+	 * Revisit when more modes are added
+	 */
+	switch (data->mode) {
+	case LBA_MODE_RST:
+	case LBA_MODE_PNR:
+	case LBA_MODE_BCM:
+		lba_def_state(priv);
+		if (mode == LBA_MODE_MDP) {
+			data->mode = LBA_MODE_MDP;
+			break;
+		}
+		/*no break -> fall down to set VFP mode*/
+	case LBA_MODE_MDP:
+		lba_wait_for_ready(priv);
+		lba_mdp2vfp(priv, data->pass);
+		data->mode = LBA_MODE_VFP;
+		break;
+	case LBA_MODE_VFP:
+		lba_wait_for_ready(priv);
+		lba_2mdp(priv);
+		data->mode = LBA_MODE_MDP;
+		break;
+	default:
+		up(&data->mode_lock);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int lba_core_unlock_mode(struct lba_data *data)
+{
+	data->last_access = jiffies;
+	up(&data->mode_lock);
+	wake_up(&data->selfpm_q);
+	return 0;
+}
+
+static int selfpm_timeout_expired(struct lba_data *data)
+{
+	return  jiffies_to_msecs(jiffies - data->last_access) > 2000;
+}
+
+static int lba_selfpm_thread(void *d)
+{
+	struct lba_data *data = d;
+
+	set_user_nice(current, -5);
+
+	while (!kthread_should_stop()) {
+
+		if (wait_event_interruptible(data->selfpm_q,
+			   kthread_should_stop() ||
+			    !(data->mode & LBA_MODE_SELFPM)))
+			continue;
+
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_timeout(msecs_to_jiffies(LBA_SELFPM_TIMEOUT));
+
+		if (down_trylock(&data->mode_lock))
+			continue;
+
+		if (!selfpm_timeout_expired(data)) {
+			up(&data->mode_lock);
+			continue;
+		}
+		data->mode |= LBA_MODE_SELFPM;
+		lba_wait_for_ready((void *)data->nand);
+		lba_cache_flush((void *)data->nand);
+		queue_release(data);
+		up(&data->mode_lock);
+
+	}
+
+	return 0;
+}
+
+int lba_core_init(struct lba_data *data)
+{
+	uint8_t id_buf[5];
+	uint8_t capacity;
+	uint8_t id1_template[5] = {0x98, 0xDC, 0x00, 0x15, 0x00};
+	uint8_t id2_template[5] = {0x98, 0x21, 0x00, 0x55, 0xAA};
+	void *priv = (void *)data->nand;
+
+
+	g_data = data;
+	g_cmd_handle = queue_get_cmd_handle(priv);
+	g_data_handle = queue_get_data_handle(priv);
+	g_data_buffer = queue_get_data_ptr(priv);
+	g_cmd_buffer = queue_get_cmd_ptr(priv);
+
+	spin_lock_init(&data->lock);
+	sema_init(&data->mode_lock, 1);
+	init_waitqueue_head(&data->suspend_q);
+	init_waitqueue_head(&data->selfpm_q);
+
+
+	lba_get_id1(data->nand, id_buf);
+	if (!memcmp(id_buf, id1_template, 5))
+		printk(KERN_INFO
+		       "LBA: Found LBA/SLC NAND emulated ID\n");
+	else
+		return -ENODEV;
+
+	lba_get_id2(data->nand, id_buf);
+	capacity = id_buf[2];
+	id_buf[2] = 0;
+
+	if (memcmp(id_buf, id2_template, 5)) {
+		printk(KERN_INFO
+		       "LBA: Uknown LBA device\n");
+		return -ENODEV;
+	}
+	printk(KERN_INFO
+	       "LBA: Found %dGbytes LBA NAND device\n",
+	       1 << capacity);
+
+	lba_wait_for_ready(priv);
+	lba_parse_status2(priv);
+
+	lba_def_state(priv);
+	data->mode = LBA_MODE_MDP;
+
+	g_data->pnp_size = 0xff;
+	g_data->vfp_size = 16384;
+
+	lba_wait_for_ready(priv);
+	g_data->mdp_size = lba_mdp_size_get(priv);
+
+	lba_wait_for_ready(priv);
+	/*lba_powersave_enable(priv);*/
+	/*lba_highspeed_enable(priv);*/
+
+	lba_wait_for_ready(priv);
+	lba_parse_status2(priv);
+
+	data->thread = kthread_create(lba_selfpm_thread,
+				      data, "lba-selfpm-%d", 1);
+	if (IS_ERR(data->thread))
+		return  PTR_ERR(data->thread);
+
+	lba_blk_init(g_data);
+
+	wake_up_process(data->thread);
+	return 0;
+
+};
+
+int lba_core_remove(struct lba_data *data)
+{
+	kthread_stop(data->thread);
+	lba_blk_remove(data);
+	lba_wait_for_ready((void *)data->nand);
+	lba_cache_flush((void *)data->nand);
+	return 0;
+}
+
+int lba_core_suspend(struct platform_device *pdev, struct lba_data *data)
+{
+	BUG_ON((data->mode & 0xffff) == LBA_MODE_SUSP);
+	if (down_interruptible(&data->mode_lock))
+	    return -EAGAIN;
+	if (data->mode & LBA_MODE_SELFPM)
+		queue_plug(data);
+
+	data->mode = LBA_MODE_SUSP | LBA_MODE_SELFPM;
+	up(&data->mode_lock);
+	lba_wait_for_ready((void *)data->nand);
+	lba_cache_flush((void *)data->nand);
+	return 0;
+}
+
+int lba_core_resume(struct platform_device *pdev, struct lba_data *data)
+{
+	BUG_ON((data->mode & 0xffff) != LBA_MODE_SUSP);
+	lba_def_state((void *)data->nand);
+	data->last_access = jiffies;
+	data->mode = LBA_MODE_MDP;
+	wake_up(&data->suspend_q);
+	return 0;
+}
diff --git a/drivers/mtd/nand/lba/lba.h b/drivers/mtd/nand/lba/lba.h
new file mode 100644
index 000000000000..4ad37ecbc44f
--- /dev/null
+++ b/drivers/mtd/nand/lba/lba.h
@@ -0,0 +1,141 @@
+/*
+ * Freescale STMP37XX/STMP378X LBA interface
+ *
+ * Author: Dmitrij Frasenyak <sed@embeddedalley.com>
+ *
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef __INCLUDE_LBA_H__
+#define __INCLUDE_LBA_H__
+
+
+#include <linux/spinlock.h>
+#include <linux/kthread.h>
+#include "gpmi.h"
+
+struct lba_cmd {
+	uint8_t len;
+#define F_MASK       0x0f
+#define F_ALE        0x01
+#define F_CMD        0x02
+#define F_DATA_READ  0x04
+#define F_DATA_WRITE 0x08
+
+#define FE_W4R        0x10
+#define FE_CMD_INC    0x20
+#define FE_END        0x40
+
+	uint8_t flag;
+};
+
+#define LBA_P_READ_A 0
+#define LBA_P_READ_B 2
+#define LBA_P_READ_C 3
+#define LBA_P_WRITE_A 0
+#define LBA_P_WRITE_B 4
+#define LBA_T_SIZE1 1
+#define LBA_T_SIZE4 2
+#define LBA_T_SIZE8 4
+#define LBA_T_CRC   (1 << 6)
+#define LBA_T_ECC_CHECK (2 << 6)
+#define LBA_T_ECC_CORRECT (3 << 6)
+
+struct lba_data {
+	void __iomem *io_base;
+	struct clk *clk;
+	int irq;
+
+	spinlock_t lock;
+	int use_count;
+	int mode;
+	struct semaphore mode_lock;
+#define LBA_MODE_MASK       0x0000ffff
+#define LBA_FLAG_MASK       0xffff0000
+#define LBA_MODE_RST        0
+#define LBA_MODE_PNR        1
+#define LBA_MODE_BCM        2
+#define LBA_MODE_MDP        3
+#define LBA_MODE_VFP        4
+#define LBA_MODE_SUSP       5
+#define LBA_MODE_SELFPM     0x80000000
+	wait_queue_head_t	suspend_q;
+	wait_queue_head_t	selfpm_q;
+	struct task_struct	*thread;
+	long long last_access;
+	/* PNR specific */
+	/* BCM specific */
+	/* VFP specific */
+	uint8_t pass[2];
+
+	/* Size of the partiotions: pages for PNP; sectors for others */
+	unsigned int   pnp_size;
+	unsigned int   vfp_size;
+	long long      mdp_size;
+	void *priv;
+	/*should be last*/
+	struct gpmi_perchip_data nand[0];
+
+};
+
+extern struct lba_data *g_data;
+
+void stmp37cc_dma_print_chain(struct stmp37xx_circ_dma_chain *chain);
+
+int lba_blk_init(struct lba_data *data);
+int lba_blk_remove(struct lba_data *data);
+int lba_blk_suspend(struct platform_device *pdev, struct lba_data *data);
+int lba_blk_resume(struct platform_device *pdev, struct lba_data *data);
+
+
+int lba_core_init(struct lba_data *data);
+int lba_core_remove(struct lba_data *data);
+int lba_core_suspend(struct platform_device *pdev, struct lba_data *data);
+int lba_core_resume(struct platform_device *pdev, struct lba_data *data);
+int lba_core_lock_mode(struct lba_data *data, int mode);
+int lba_core_unlock_mode(struct lba_data *data);
+
+int lba_write_sectors(void *priv, unsigned int sector,	unsigned int count,
+		      void *buffer, dma_addr_t handle);
+int lba_read_sectors(void *priv, unsigned int sector,	unsigned int count,
+		      void *buffer, dma_addr_t handle);
+void lba_protocol1_set(void *priv, uint8_t param);
+uint8_t lba_protocol1_get(void *priv);
+uint8_t lba_get_status1(void *priv);
+uint8_t lba_get_status2(void *priv);
+
+uint8_t lba_get_id1(void *priv, uint8_t *ret_buffer);
+uint8_t lba_get_id2(void *priv, uint8_t *);
+
+
+int queue_cmd(void *priv,
+	      uint8_t *cmd_buf, dma_addr_t cmd_handle, int cmd_len,
+	      dma_addr_t data, int data_len,
+	      struct lba_cmd *cmd_flags);
+
+int queue_run(void *priv);
+
+dma_addr_t queue_get_cmd_handle(void *priv);
+
+uint8_t *queue_get_cmd_ptr(void *priv);
+
+dma_addr_t queue_get_data_handle(void *priv);
+
+uint8_t *queue_get_data_ptr(void *priv);
+
+void queue_plug(struct lba_data *data);
+void queue_release(struct lba_data *data);
+
+
+#endif
+
diff --git a/drivers/mtd/nand/mxc_nd.c b/drivers/mtd/nand/mxc_nd.c
new file mode 100644
index 000000000000..fc95af3bd442
--- /dev/null
+++ b/drivers/mtd/nand/mxc_nd.c
@@ -0,0 +1,1413 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/partitions.h>
+#include <asm/io.h>
+#include <asm/mach/flash.h>
+
+#include "mxc_nd.h"
+
+#define DVR_VER "2.1"
+
+struct mxc_mtd_s {
+	struct mtd_info mtd;
+	struct nand_chip nand;
+	struct mtd_partition *parts;
+	struct device *dev;
+};
+
+static struct mxc_mtd_s *mxc_nand_data;
+
+/*
+ * Define delays in microsec for NAND device operations
+ */
+#define TROP_US_DELAY   2000
+/*
+ * Macros to get half word and bit positions of ECC
+ */
+#define COLPOS(x) ((x) >> 4)
+#define BITPOS(x) ((x) & 0xf)
+
+/* Define single bit Error positions in Main & Spare area */
+#define MAIN_SINGLEBIT_ERROR 0x4
+#define SPARE_SINGLEBIT_ERROR 0x1
+
+struct nand_info {
+	bool bSpareOnly;
+	bool bStatusRequest;
+	u16 colAddr;
+};
+
+static struct nand_info g_nandfc_info;
+
+#ifdef CONFIG_MTD_NAND_MXC_SWECC
+static int hardware_ecc = 0;
+#else
+static int hardware_ecc = 1;
+#endif
+
+#ifndef CONFIG_MTD_NAND_MXC_ECC_CORRECTION_OPTION2
+static int Ecc_disabled;
+#endif
+
+static int is2k_Pagesize = 0;
+
+static struct clk *nfc_clk;
+
+/*
+ * OOB placement block for use with hardware ecc generation
+ */
+static struct nand_ecclayout nand_hw_eccoob_8 = {
+	.eccbytes = 5,
+	.eccpos = {6, 7, 8, 9, 10},
+	.oobfree = {{0, 5}, {11, 5}}
+};
+
+static struct nand_ecclayout nand_hw_eccoob_16 = {
+	.eccbytes = 5,
+	.eccpos = {6, 7, 8, 9, 10},
+	.oobfree = {{0, 6}, {12, 4}}
+};
+
+static struct nand_ecclayout nand_hw_eccoob_2k = {
+	.eccbytes = 20,
+	.eccpos = {6, 7, 8, 9, 10, 22, 23, 24, 25, 26,
+		   38, 39, 40, 41, 42, 54, 55, 56, 57, 58},
+	.oobfree = {
+		    {.offset = 0,
+		     .length = 5},
+
+		    {.offset = 11,
+		     .length = 10},
+
+		    {.offset = 27,
+		     .length = 10},
+
+		    {.offset = 43,
+		     .length = 10},
+
+		    {.offset = 59,
+		     .length = 5}
+		    }
+};
+
+/*!
+ * @defgroup NAND_MTD NAND Flash MTD Driver for MXC processors
+ */
+
+/*!
+ * @file mxc_nd.c
+ *
+ * @brief This file contains the hardware specific layer for NAND Flash on
+ * MXC processor
+ *
+ * @ingroup NAND_MTD
+ */
+
+#ifdef CONFIG_MTD_PARTITIONS
+static const char *part_probes[] = { "RedBoot", "cmdlinepart", NULL };
+#endif
+
+static wait_queue_head_t irq_waitq;
+
+static irqreturn_t mxc_nfc_irq(int irq, void *dev_id)
+{
+	NFC_CONFIG1 |= NFC_INT_MSK;	/* Disable interrupt */
+	wake_up(&irq_waitq);
+
+	return IRQ_RETVAL(1);
+}
+
+/*!
+ * This function polls the NANDFC to wait for the basic operation to complete by
+ * checking the INT bit of config2 register.
+ *
+ * @param       maxRetries     number of retry attempts (separated by 1 us)
+ * @param       param           parameter for debug
+ * @param       useirq         True if IRQ should be used rather than polling
+ */
+static void wait_op_done(int maxRetries, u16 param, bool useirq)
+{
+	if (useirq) {
+		if ((NFC_CONFIG2 & NFC_INT) == 0) {
+			NFC_CONFIG1 &= ~NFC_INT_MSK;	/* Enable interrupt */
+			wait_event(irq_waitq, NFC_CONFIG2 & NFC_INT);
+		}
+		NFC_CONFIG2 &= ~NFC_INT;
+	} else {
+		while (maxRetries-- > 0) {
+			if (NFC_CONFIG2 & NFC_INT) {
+				NFC_CONFIG2 &= ~NFC_INT;
+				break;
+			}
+			udelay(1);
+		}
+		if (maxRetries <= 0)
+			DEBUG(MTD_DEBUG_LEVEL0, "%s(%d): INT not set\n",
+			      __FUNCTION__, param);
+	}
+}
+
+/*!
+ * This function issues the specified command to the NAND device and
+ * waits for completion.
+ *
+ * @param       cmd     command for NAND Flash
+ * @param       useirq  True if IRQ should be used rather than polling
+ */
+static void send_cmd(u16 cmd, bool useirq)
+{
+	DEBUG(MTD_DEBUG_LEVEL3, "send_cmd(0x%x, %d)\n", cmd, useirq);
+
+	NFC_FLASH_CMD = (u16) cmd;
+	NFC_CONFIG2 = NFC_CMD;
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, cmd, useirq);
+}
+
+/*!
+ * This function sends an address (or partial address) to the
+ * NAND device.  The address is used to select the source/destination for
+ * a NAND command.
+ *
+ * @param       addr    address to be written to NFC.
+ * @param       islast  True if this is the last address cycle for command
+ */
+static void send_addr(u16 addr, bool islast)
+{
+	DEBUG(MTD_DEBUG_LEVEL3, "send_addr(0x%x %d)\n", addr, islast);
+
+	NFC_FLASH_ADDR = addr;
+	NFC_CONFIG2 = NFC_ADDR;
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, addr, islast);
+}
+
+/*!
+ * This function requests the NANDFC to initate the transfer
+ * of data currently in the NANDFC RAM buffer to the NAND device.
+ *
+ * @param	buf_id	      Specify Internal RAM Buffer number (0-3)
+ * @param       bSpareOnly    set true if only the spare area is transferred
+ */
+static void send_prog_page(u8 buf_id, bool bSpareOnly)
+{
+	DEBUG(MTD_DEBUG_LEVEL3, "send_prog_page (%d)\n", bSpareOnly);
+
+	/* NANDFC buffer 0 is used for page read/write */
+
+	NFC_BUF_ADDR = buf_id;
+
+	/* Configure spare or page+spare access */
+	if (!is2k_Pagesize) {
+		if (bSpareOnly) {
+			NFC_CONFIG1 |= NFC_SP_EN;
+		} else {
+			NFC_CONFIG1 &= ~(NFC_SP_EN);
+		}
+	}
+	NFC_CONFIG2 = NFC_INPUT;
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, bSpareOnly, true);
+}
+
+/*!
+ * This function will correct the single bit ECC error
+ *
+ * @param  buf_id	Specify Internal RAM Buffer number (0-3)
+ * @param  eccpos 	Ecc byte and bit position
+ * @param  bSpareOnly  	set to true if only spare area needs correction
+ */
+
+static void mxc_nd_correct_error(u8 buf_id, u16 eccpos, bool bSpareOnly)
+{
+	u16 col;
+	u8 pos;
+	volatile u16 *buf;
+
+	/* Get col & bit position of error
+	   these macros works for both 8 & 16 bits */
+	col = COLPOS(eccpos);	/* Get half-word position */
+	pos = BITPOS(eccpos);	/* Get bit position */
+
+	DEBUG(MTD_DEBUG_LEVEL3,
+	      "mxc_nd_correct_error (col=%d pos=%d)\n", col, pos);
+
+	/* Set the pointer for main / spare area */
+	if (!bSpareOnly) {
+		buf = (volatile u16 *)(MAIN_AREA0 + col + (256 * buf_id));
+	} else {
+		buf = (volatile u16 *)(SPARE_AREA0 + col + (8 * buf_id));
+	}
+
+	/* Fix the data */
+	*buf ^= (1 << pos);
+}
+
+/*!
+ * This function will maintains state of single bit Error
+ * in Main & spare  area
+ *
+ * @param buf_id	Specify Internal RAM Buffer number (0-3)
+ * @param spare  	set to true if only spare area needs correction
+ */
+static void mxc_nd_correct_ecc(u8 buf_id, bool spare)
+{
+#ifdef CONFIG_MTD_NAND_MXC_ECC_CORRECTION_OPTION2
+	static int lastErrMain = 0, lastErrSpare = 0;	/* To maintain single bit
+							   error in previous page */
+#endif
+	u16 value, ecc_status;
+	/* Read the ECC result */
+	ecc_status = NFC_ECC_STATUS_RESULT;
+	DEBUG(MTD_DEBUG_LEVEL3,
+	      "mxc_nd_correct_ecc (Ecc status=%x)\n", ecc_status);
+
+#ifdef CONFIG_MTD_NAND_MXC_ECC_CORRECTION_OPTION2
+	/* Check for Error in Mainarea */
+	if ((ecc_status & 0xC) == MAIN_SINGLEBIT_ERROR) {
+		/* Check for error in previous page */
+		if (lastErrMain && !spare) {
+			value = NFC_RSLTMAIN_AREA;
+			/* Correct single bit error in Mainarea
+			   NFC will not correct the error in
+			   current page */
+			mxc_nd_correct_error(buf_id, value, false);
+		} else {
+			/* Set if single bit error in current page */
+			lastErrMain = 1;
+		}
+	} else {
+		/* Reset if no single bit error in current page */
+		lastErrMain = 0;
+	}
+
+	/* Check for Error in Sparearea */
+	if ((ecc_status & 0x3) == SPARE_SINGLEBIT_ERROR) {
+		/* Check for error in previous page */
+		if (lastErrSpare) {
+			value = NFC_RSLTSPARE_AREA;
+			/* Correct single bit error in Mainarea
+			   NFC will not correct the error in
+			   current page */
+			mxc_nd_correct_error(buf_id, value, true);
+		} else {
+			/* Set if single bit error in current page */
+			lastErrSpare = 1;
+		}
+	} else {
+		/* Reset if no single bit error in current page */
+		lastErrSpare = 0;
+	}
+#else
+	if (((ecc_status & 0xC) == MAIN_SINGLEBIT_ERROR)
+	    || ((ecc_status & 0x3) == SPARE_SINGLEBIT_ERROR)) {
+		if (Ecc_disabled) {
+			if ((ecc_status & 0xC) == MAIN_SINGLEBIT_ERROR) {
+				value = NFC_RSLTMAIN_AREA;
+				/* Correct single bit error in Mainarea
+				   NFC will not correct the error in
+				   current page */
+				mxc_nd_correct_error(buf_id, value, false);
+			}
+			if ((ecc_status & 0x3) == SPARE_SINGLEBIT_ERROR) {
+				value = NFC_RSLTSPARE_AREA;
+				/* Correct single bit error in Mainarea
+				   NFC will not correct the error in
+				   current page */
+				mxc_nd_correct_error(buf_id, value, true);
+			}
+
+		} else {
+			/* Disable ECC  */
+			NFC_CONFIG1 &= ~(NFC_ECC_EN);
+			Ecc_disabled = 1;
+		}
+	} else if (ecc_status == 0) {
+		if (Ecc_disabled) {
+			/* Enable ECC */
+			NFC_CONFIG1 |= NFC_ECC_EN;
+			Ecc_disabled = 0;
+		}
+	} else {
+		/* 2-bit Error  Do nothing */
+	}
+#endif				/* CONFIG_MTD_NAND_MXC_ECC_CORRECTION_OPTION2 */
+
+}
+
+/*!
+ * This function requests the NANDFC to initated the transfer
+ * of data from the NAND device into in the NANDFC ram buffer.
+ *
+ * @param  	buf_id		Specify Internal RAM Buffer number (0-3)
+ * @param       bSpareOnly    	set true if only the spare area is transferred
+ */
+static void send_read_page(u8 buf_id, bool bSpareOnly)
+{
+	DEBUG(MTD_DEBUG_LEVEL3, "send_read_page (%d)\n", bSpareOnly);
+
+	/* NANDFC buffer 0 is used for page read/write */
+	NFC_BUF_ADDR = buf_id;
+
+	/* Configure spare or page+spare access */
+	if (!is2k_Pagesize) {
+		if (bSpareOnly) {
+			NFC_CONFIG1 |= NFC_SP_EN;
+		} else {
+			NFC_CONFIG1 &= ~(NFC_SP_EN);
+		}
+	}
+
+	NFC_CONFIG2 = NFC_OUTPUT;
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, bSpareOnly, true);
+
+	/* If there are single bit errors in
+	   two consecutive page reads then
+	   the error is not  corrected by the
+	   NFC for the second page.
+	   Correct single bit error in driver */
+
+	mxc_nd_correct_ecc(buf_id, bSpareOnly);
+}
+
+/*!
+ * This function requests the NANDFC to perform a read of the
+ * NAND device ID.
+ */
+static void send_read_id(void)
+{
+	struct nand_chip *this = &mxc_nand_data->nand;
+
+	/* NANDFC buffer 0 is used for device ID output */
+	NFC_BUF_ADDR = 0x0;
+
+	/* Read ID into main buffer */
+	NFC_CONFIG1 &= (~(NFC_SP_EN));
+	NFC_CONFIG2 = NFC_ID;
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, 0, true);
+
+	if (this->options & NAND_BUSWIDTH_16) {
+		volatile u16 *mainBuf = MAIN_AREA0;
+
+		/*
+		 * Pack the every-other-byte result for 16-bit ID reads
+		 * into every-byte as the generic code expects and various
+		 * chips implement.
+		 */
+
+		mainBuf[0] = (mainBuf[0] & 0xff) | ((mainBuf[1] & 0xff) << 8);
+		mainBuf[1] = (mainBuf[2] & 0xff) | ((mainBuf[3] & 0xff) << 8);
+		mainBuf[2] = (mainBuf[4] & 0xff) | ((mainBuf[5] & 0xff) << 8);
+	}
+}
+
+/*!
+ * This function requests the NANDFC to perform a read of the
+ * NAND device status and returns the current status.
+ *
+ * @return  device status
+ */
+static u16 get_dev_status(void)
+{
+	volatile u16 *mainBuf = MAIN_AREA1;
+	u32 store;
+	u16 ret;
+	/* Issue status request to NAND device */
+
+	/* store the main area1 first word, later do recovery */
+	store = *((u32 *) mainBuf);
+	/*
+	 * NANDFC buffer 1 is used for device status to prevent
+	 * corruption of read/write buffer on status requests.
+	 */
+	NFC_BUF_ADDR = 1;
+
+	/* Read status into main buffer */
+	NFC_CONFIG1 &= (~(NFC_SP_EN));
+	NFC_CONFIG2 = NFC_STATUS;
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, 0, true);
+
+	/* Status is placed in first word of main buffer */
+	/* get status, then recovery area 1 data */
+	ret = mainBuf[0];
+	*((u32 *) mainBuf) = store;
+
+	return ret;
+}
+
+/*!
+ * This functions is used by upper layer to checks if device is ready
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ *
+ * @return  0 if device is busy else 1
+ */
+static int mxc_nand_dev_ready(struct mtd_info *mtd)
+{
+	/*
+	 * NFC handles R/B internally.Therefore,this function
+	 * always returns status as ready.
+	 */
+	return 1;
+}
+
+static void mxc_nand_enable_hwecc(struct mtd_info *mtd, int mode)
+{
+	/*
+	 * If HW ECC is enabled, we turn it on during init.  There is
+	 * no need to enable again here.
+	 */
+}
+
+static int mxc_nand_correct_data(struct mtd_info *mtd, u_char * dat,
+				 u_char * read_ecc, u_char * calc_ecc)
+{
+	/*
+	 * 1-Bit errors are automatically corrected in HW.  No need for
+	 * additional correction.  2-Bit errors cannot be corrected by
+	 * HW ECC, so we need to return failure
+	 */
+	u16 ecc_status = NFC_ECC_STATUS_RESULT;
+
+	if (((ecc_status & 0x3) == 2) || ((ecc_status >> 2) == 2)) {
+		DEBUG(MTD_DEBUG_LEVEL0,
+		      "MXC_NAND: HWECC uncorrectable 2-bit ECC error\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int mxc_nand_calculate_ecc(struct mtd_info *mtd, const u_char * dat,
+				  u_char * ecc_code)
+{
+	/*
+	 * Just return success.  HW ECC does not read/write the NFC spare
+	 * buffer.  Only the FLASH spare area contains the calcuated ECC.
+	 */
+	return 0;
+}
+
+/*!
+ * This function reads byte from the NAND Flash
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ *
+ * @return    data read from the NAND Flash
+ */
+static u_char mxc_nand_read_byte(struct mtd_info *mtd)
+{
+	u_char retVal = 0;
+	u16 col, rdWord;
+	volatile u16 *mainBuf = MAIN_AREA0;
+	volatile u16 *spareBuf = SPARE_AREA0;
+
+	/* Check for status request */
+	if (g_nandfc_info.bStatusRequest) {
+		return (get_dev_status() & 0xFF);
+	}
+
+	/* Get column for 16-bit access */
+	col = g_nandfc_info.colAddr >> 1;
+
+	/* If we are accessing the spare region */
+	if (g_nandfc_info.bSpareOnly) {
+		rdWord = spareBuf[col];
+	} else {
+		rdWord = mainBuf[col];
+	}
+
+	/* Pick upper/lower byte of word from RAM buffer */
+	if (g_nandfc_info.colAddr & 0x1) {
+		retVal = (rdWord >> 8) & 0xFF;
+	} else {
+		retVal = rdWord & 0xFF;
+	}
+
+	/* Update saved column address */
+	g_nandfc_info.colAddr++;
+
+	return retVal;
+}
+
+/*!
+  * This function reads word from the NAND Flash
+  *
+  * @param       mtd     MTD structure for the NAND Flash
+  *
+  * @return    data read from the NAND Flash
+  */
+static u16 mxc_nand_read_word(struct mtd_info *mtd)
+{
+	u16 col;
+	u16 rdWord, retVal;
+	volatile u16 *p;
+
+	DEBUG(MTD_DEBUG_LEVEL3,
+	      "mxc_nand_read_word(col = %d)\n", g_nandfc_info.colAddr);
+
+	col = g_nandfc_info.colAddr;
+	/* Adjust saved column address */
+	if (col < mtd->writesize && g_nandfc_info.bSpareOnly)
+		col += mtd->writesize;
+
+	if (col < mtd->writesize)
+		p = (MAIN_AREA0) + (col >> 1);
+	else
+		p = (SPARE_AREA0) + ((col - mtd->writesize) >> 1);
+
+	if (col & 1) {
+		rdWord = *p;
+		retVal = (rdWord >> 8) & 0xff;
+		rdWord = *(p + 1);
+		retVal |= (rdWord << 8) & 0xff00;
+
+	} else {
+		retVal = *p;
+
+	}
+
+	/* Update saved column address */
+	g_nandfc_info.colAddr = col + 2;
+
+	return retVal;
+}
+
+/*!
+ * This function writes data of length \b len to buffer \b buf. The data to be
+ * written on NAND Flash is first copied to RAMbuffer. After the Data Input
+ * Operation by the NFC, the data is written to NAND Flash
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ * @param       buf     data to be written to NAND Flash
+ * @param       len     number of bytes to be written
+ */
+static void mxc_nand_write_buf(struct mtd_info *mtd,
+			       const u_char * buf, int len)
+{
+	int n;
+	int col;
+	int i = 0;
+
+	DEBUG(MTD_DEBUG_LEVEL3,
+	      "mxc_nand_write_buf(col = %d, len = %d)\n", g_nandfc_info.colAddr,
+	      len);
+
+	col = g_nandfc_info.colAddr;
+
+	/* Adjust saved column address */
+	if (col < mtd->writesize && g_nandfc_info.bSpareOnly)
+		col += mtd->writesize;
+
+	n = mtd->writesize + mtd->oobsize - col;
+	n = min(len, n);
+
+	DEBUG(MTD_DEBUG_LEVEL3,
+	      "%s:%d: col = %d, n = %d\n", __FUNCTION__, __LINE__, col, n);
+
+	while (n) {
+		volatile u32 *p;
+		if (col < mtd->writesize)
+			p = (volatile u32 *)((ulong) (MAIN_AREA0) + (col & ~3));
+		else
+			p = (volatile u32 *)((ulong) (SPARE_AREA0) -
+					     mtd->writesize + (col & ~3));
+
+		DEBUG(MTD_DEBUG_LEVEL3, "%s:%d: p = %p\n", __FUNCTION__,
+		      __LINE__, p);
+
+		if (((col | (int)&buf[i]) & 3) || n < 16) {
+			u32 data = 0;
+
+			if (col & 3 || n < 4)
+				data = *p;
+
+			switch (col & 3) {
+			case 0:
+				if (n) {
+					data = (data & 0xffffff00) |
+					    (buf[i++] << 0);
+					n--;
+					col++;
+				}
+			case 1:
+				if (n) {
+					data = (data & 0xffff00ff) |
+					    (buf[i++] << 8);
+					n--;
+					col++;
+				}
+			case 2:
+				if (n) {
+					data = (data & 0xff00ffff) |
+					    (buf[i++] << 16);
+					n--;
+					col++;
+				}
+			case 3:
+				if (n) {
+					data = (data & 0x00ffffff) |
+					    (buf[i++] << 24);
+					n--;
+					col++;
+				}
+			}
+
+			*p = data;
+		} else {
+			int m = mtd->writesize - col;
+
+			if (col >= mtd->writesize)
+				m += mtd->oobsize;
+
+			m = min(n, m) & ~3;
+
+			DEBUG(MTD_DEBUG_LEVEL3,
+			      "%s:%d: n = %d, m = %d, i = %d, col = %d\n",
+			      __FUNCTION__, __LINE__, n, m, i, col);
+
+			memcpy((void *)(p), &buf[i], m);
+			col += m;
+			i += m;
+			n -= m;
+		}
+	}
+	/* Update saved column address */
+	g_nandfc_info.colAddr = col;
+
+}
+
+/*!
+ * This function id is used to read the data buffer from the NAND Flash. To
+ * read the data from NAND Flash first the data output cycle is initiated by
+ * the NFC, which copies the data to RAMbuffer. This data of length \b len is
+ * then copied to buffer \b buf.
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ * @param       buf     data to be read from NAND Flash
+ * @param       len     number of bytes to be read
+ */
+static void mxc_nand_read_buf(struct mtd_info *mtd, u_char * buf, int len)
+{
+
+	int n;
+	int col;
+	int i = 0;
+
+	DEBUG(MTD_DEBUG_LEVEL3,
+	      "mxc_nand_read_buf(col = %d, len = %d)\n", g_nandfc_info.colAddr,
+	      len);
+
+	col = g_nandfc_info.colAddr;
+	/* Adjust saved column address */
+	if (col < mtd->writesize && g_nandfc_info.bSpareOnly)
+		col += mtd->writesize;
+
+	n = mtd->writesize + mtd->oobsize - col;
+	n = min(len, n);
+
+	while (n) {
+		volatile u32 *p;
+
+		if (col < mtd->writesize)
+			p = (volatile u32 *)((ulong) (MAIN_AREA0) + (col & ~3));
+		else
+			p = (volatile u32 *)((ulong) (SPARE_AREA0) -
+					     mtd->writesize + (col & ~3));
+
+		if (((col | (int)&buf[i]) & 3) || n < 16) {
+			u32 data;
+
+			data = *p;
+			switch (col & 3) {
+			case 0:
+				if (n) {
+					buf[i++] = (u8) (data);
+					n--;
+					col++;
+				}
+			case 1:
+				if (n) {
+					buf[i++] = (u8) (data >> 8);
+					n--;
+					col++;
+				}
+			case 2:
+				if (n) {
+					buf[i++] = (u8) (data >> 16);
+					n--;
+					col++;
+				}
+			case 3:
+				if (n) {
+					buf[i++] = (u8) (data >> 24);
+					n--;
+					col++;
+				}
+			}
+		} else {
+			int m = mtd->writesize - col;
+
+			if (col >= mtd->writesize)
+				m += mtd->oobsize;
+
+			m = min(n, m) & ~3;
+			memcpy(&buf[i], (void *)(p), m);
+			col += m;
+			i += m;
+			n -= m;
+		}
+	}
+	/* Update saved column address */
+	g_nandfc_info.colAddr = col;
+
+}
+
+/*!
+ * This function is used by the upper layer to verify the data in NAND Flash
+ * with the data in the \b buf.
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ * @param       buf     data to be verified
+ * @param       len     length of the data to be verified
+ *
+ * @return      -EFAULT if error else 0
+ *
+ */
+static int
+mxc_nand_verify_buf(struct mtd_info *mtd, const u_char * buf, int len)
+{
+	return -EFAULT;
+}
+
+/*!
+ * This function is used by upper layer for select and deselect of the NAND
+ * chip
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ * @param       chip    val indicating select or deselect
+ */
+static void mxc_nand_select_chip(struct mtd_info *mtd, int chip)
+{
+#ifdef CONFIG_MTD_NAND_MXC_FORCE_CE
+	if (chip > 0) {
+		DEBUG(MTD_DEBUG_LEVEL0,
+		      "ERROR:  Illegal chip select (chip = %d)\n", chip);
+		return;
+	}
+
+	if (chip == -1) {
+		NFC_CONFIG1 &= (~(NFC_CE));
+		return;
+	}
+
+	NFC_CONFIG1 |= NFC_CE;
+#endif
+
+	switch (chip) {
+	case -1:
+		/* Disable the NFC clock */
+		clk_disable(nfc_clk);
+		break;
+	case 0:
+		/* Enable the NFC clock */
+		clk_enable(nfc_clk);
+		break;
+
+	default:
+		break;
+	}
+}
+
+/*!
+ * This function is used by the upper layer to write command to NAND Flash for
+ * different operations to be carried out on NAND Flash
+ *
+ * @param       mtd             MTD structure for the NAND Flash
+ * @param       command         command for NAND Flash
+ * @param       column          column offset for the page read
+ * @param       page_addr       page to be read from NAND Flash
+ */
+static void mxc_nand_command(struct mtd_info *mtd, unsigned command,
+			     int column, int page_addr)
+{
+	bool useirq = true;
+
+	DEBUG(MTD_DEBUG_LEVEL3,
+	      "mxc_nand_command (cmd = 0x%x, col = 0x%x, page = 0x%x)\n",
+	      command, column, page_addr);
+
+	/*
+	 * Reset command state information
+	 */
+	g_nandfc_info.bStatusRequest = false;
+
+	/* Reset column address to 0 */
+	g_nandfc_info.colAddr = 0;
+
+	/*
+	 * Command pre-processing step
+	 */
+	switch (command) {
+
+	case NAND_CMD_STATUS:
+		g_nandfc_info.bStatusRequest = true;
+		break;
+
+	case NAND_CMD_READ0:
+		g_nandfc_info.colAddr = column;
+		g_nandfc_info.bSpareOnly = false;
+		useirq = false;
+		break;
+
+	case NAND_CMD_READOOB:
+		g_nandfc_info.colAddr = column;
+		g_nandfc_info.bSpareOnly = true;
+		useirq = false;
+		if (is2k_Pagesize)
+			command = NAND_CMD_READ0;	/* only READ0 is valid */
+		break;
+
+	case NAND_CMD_SEQIN:
+		if (column >= mtd->writesize) {
+			if (is2k_Pagesize) {
+				/**
+				  * FIXME: before send SEQIN command for write OOB,
+				  * We must read one page out.
+				  * For K9F1GXX has no READ1 command to set current HW
+				  * pointer to spare area, we must write the whole page including OOB together.
+				  */
+				/* call itself to read a page */
+				mxc_nand_command(mtd, NAND_CMD_READ0, 0,
+						 page_addr);
+			}
+			g_nandfc_info.colAddr = column - mtd->writesize;
+			g_nandfc_info.bSpareOnly = true;
+			/* Set program pointer to spare region */
+			if (!is2k_Pagesize)
+				send_cmd(NAND_CMD_READOOB, false);
+		} else {
+			g_nandfc_info.bSpareOnly = false;
+			g_nandfc_info.colAddr = column;
+			/* Set program pointer to page start */
+			if (!is2k_Pagesize)
+				send_cmd(NAND_CMD_READ0, false);
+		}
+		useirq = false;
+		break;
+
+	case NAND_CMD_PAGEPROG:
+#ifndef CONFIG_MTD_NAND_MXC_ECC_CORRECTION_OPTION2
+		if (Ecc_disabled) {
+			/* Enable Ecc for page writes */
+			NFC_CONFIG1 |= NFC_ECC_EN;
+		}
+#endif
+
+		send_prog_page(0, g_nandfc_info.bSpareOnly);
+
+		if (is2k_Pagesize) {
+			/* data in 4 areas datas */
+			send_prog_page(1, g_nandfc_info.bSpareOnly);
+			send_prog_page(2, g_nandfc_info.bSpareOnly);
+			send_prog_page(3, g_nandfc_info.bSpareOnly);
+		}
+
+		break;
+
+	case NAND_CMD_ERASE1:
+		useirq = false;
+		break;
+	}
+
+	/*
+	 * Write out the command to the device.
+	 */
+	send_cmd(command, useirq);
+
+	/*
+	 * Write out column address, if necessary
+	 */
+	if (column != -1) {
+		/*
+		 * MXC NANDFC can only perform full page+spare or
+		 * spare-only read/write.  When the upper layers
+		 * layers perform a read/write buf operation,
+		 * we will used the saved column adress to index into
+		 * the full page.
+		 */
+		send_addr(0, page_addr == -1);
+		if (is2k_Pagesize)
+			/* another col addr cycle for 2k page */
+			send_addr(0, false);
+	}
+
+	/*
+	 * Write out page address, if necessary
+	 */
+	if (page_addr != -1) {
+		send_addr((page_addr & 0xff), false);	/* paddr_0 - p_addr_7 */
+
+		if (is2k_Pagesize) {
+			/* One more address cycle for higher density devices */
+			if (mtd->size >= 0x10000000) {
+				/* paddr_8 - paddr_15 */
+				send_addr((page_addr >> 8) & 0xff, false);
+				send_addr((page_addr >> 16) & 0xff, true);
+			} else
+				/* paddr_8 - paddr_15 */
+				send_addr((page_addr >> 8) & 0xff, true);
+		} else {
+			/* One more address cycle for higher density devices */
+			if (mtd->size >= 0x4000000) {
+				/* paddr_8 - paddr_15 */
+				send_addr((page_addr >> 8) & 0xff, false);
+				send_addr((page_addr >> 16) & 0xff, true);
+			} else
+				/* paddr_8 - paddr_15 */
+				send_addr((page_addr >> 8) & 0xff, true);
+		}
+	}
+
+	/*
+	 * Command post-processing step
+	 */
+	switch (command) {
+
+	case NAND_CMD_RESET:
+		break;
+
+	case NAND_CMD_READOOB:
+	case NAND_CMD_READ0:
+		if (is2k_Pagesize) {
+			/* send read confirm command */
+			send_cmd(NAND_CMD_READSTART, true);
+			/* read for each AREA */
+			send_read_page(0, g_nandfc_info.bSpareOnly);
+			send_read_page(1, g_nandfc_info.bSpareOnly);
+			send_read_page(2, g_nandfc_info.bSpareOnly);
+			send_read_page(3, g_nandfc_info.bSpareOnly);
+		} else {
+			send_read_page(0, g_nandfc_info.bSpareOnly);
+		}
+		break;
+
+	case NAND_CMD_READID:
+		send_read_id();
+		break;
+
+	case NAND_CMD_PAGEPROG:
+#ifndef CONFIG_MTD_NAND_MXC_ECC_CORRECTION_OPTION2
+		if (Ecc_disabled) {
+			/* Disble Ecc after page writes */
+			NFC_CONFIG1 &= ~(NFC_ECC_EN);
+		}
+#endif
+		break;
+
+	case NAND_CMD_STATUS:
+		break;
+
+	case NAND_CMD_ERASE2:
+		break;
+	}
+}
+
+/* Define some generic bad / good block scan pattern which are used
+ * while scanning a device for factory marked good / bad blocks. */
+static uint8_t scan_ff_pattern[] = { 0xff, 0xff };
+
+static struct nand_bbt_descr smallpage_memorybased = {
+	.options = NAND_BBT_SCAN2NDPAGE,
+	.offs = 5,
+	.len = 1,
+	.pattern = scan_ff_pattern
+};
+
+static struct nand_bbt_descr largepage_memorybased = {
+	.options = 0,
+	.offs = 0,
+	.len = 2,
+	.pattern = scan_ff_pattern
+};
+
+/* Generic flash bbt decriptors
+*/
+static uint8_t bbt_pattern[] = { 'B', 'b', 't', '0' };
+static uint8_t mirror_pattern[] = { '1', 't', 'b', 'B' };
+
+static struct nand_bbt_descr bbt_main_descr = {
+	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
+	    | NAND_BBT_2BIT | NAND_BBT_VERSION | NAND_BBT_PERCHIP,
+	.offs = 0,
+	.len = 4,
+	.veroffs = 4,
+	.maxblocks = 4,
+	.pattern = bbt_pattern
+};
+
+static struct nand_bbt_descr bbt_mirror_descr = {
+	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
+	    | NAND_BBT_2BIT | NAND_BBT_VERSION | NAND_BBT_PERCHIP,
+	.offs = 0,
+	.len = 4,
+	.veroffs = 4,
+	.maxblocks = 4,
+	.pattern = mirror_pattern
+};
+
+static int mxc_nand_scan_bbt(struct mtd_info *mtd)
+{
+	struct nand_chip *this = mtd->priv;
+
+	/* Config before scanning */
+	/* Do not rely on NFMS_BIT, set/clear NFMS bit based on mtd->writesize */
+	if (mtd->writesize == 2048) {
+		NFMS |= (1 << NFMS_BIT);
+		is2k_Pagesize = 1;
+	} else {
+		if ((NFMS >> NFMS_BIT) & 0x1) {	/* This case strangly happened on MXC91321 P1.2.2 */
+			printk(KERN_INFO
+			       "Oops... NFMS Bit set for 512B Page, resetting it. [RCSR: 0x%08x]\n",
+			       NFMS);
+			NFMS &= ~(1 << NFMS_BIT);
+		}
+		is2k_Pagesize = 0;
+	}
+
+	if (is2k_Pagesize)
+		this->ecc.layout = &nand_hw_eccoob_2k;
+
+	/* jffs2 not write oob */
+	mtd->flags &= ~MTD_OOB_WRITEABLE;
+
+	/* use flash based bbt */
+	this->bbt_td = &bbt_main_descr;
+	this->bbt_md = &bbt_mirror_descr;
+
+	/* update flash based bbt */
+	this->options |= NAND_USE_FLASH_BBT;
+
+	if (!this->badblock_pattern) {
+		if (mtd->writesize == 2048)
+			this->badblock_pattern = &smallpage_memorybased;
+		else
+			this->badblock_pattern = (mtd->writesize > 512) ?
+			    &largepage_memorybased : &smallpage_memorybased;
+	}
+	/* Build bad block table */
+	return nand_scan_bbt(mtd, this->badblock_pattern);
+}
+
+#ifdef CONFIG_MXC_NAND_LOW_LEVEL_ERASE
+static void mxc_low_erase(struct mtd_info *mtd)
+{
+
+	struct nand_chip *this = mtd->priv;
+	unsigned int page_addr, addr;
+	u_char status;
+
+	DEBUG(MTD_DEBUG_LEVEL0, "MXC_ND : mxc_low_erase:Erasing NAND\n");
+	for (addr = 0; addr < this->chipsize; addr += mtd->erasesize) {
+		page_addr = addr / mtd->writesize;
+		mxc_nand_command(mtd, NAND_CMD_ERASE1, -1, page_addr);
+		mxc_nand_command(mtd, NAND_CMD_ERASE2, -1, -1);
+		mxc_nand_command(mtd, NAND_CMD_STATUS, -1, -1);
+		status = mxc_nand_read_byte(mtd);
+		if (status & NAND_STATUS_FAIL) {
+			printk(KERN_ERR
+			       "ERASE FAILED(block = %d,status = 0x%x)\n",
+			       addr / mtd->erasesize, status);
+		}
+	}
+
+}
+#endif
+/*!
+ * This function is called during the driver binding process.
+ *
+ * @param   pdev  the device structure used to store device specific
+ *                information that is used by the suspend, resume and
+ *                remove functions
+ *
+ * @return  The function always returns 0.
+ */
+static int __init mxcnd_probe(struct platform_device *pdev)
+{
+	struct nand_chip *this;
+	struct mtd_info *mtd;
+	struct flash_platform_data *flash = pdev->dev.platform_data;
+	int nr_parts = 0;
+
+	int err = 0;
+	/* Allocate memory for MTD device structure and private data */
+	mxc_nand_data = kzalloc(sizeof(struct mxc_mtd_s), GFP_KERNEL);
+	if (!mxc_nand_data) {
+		printk(KERN_ERR "%s: failed to allocate mtd_info\n",
+		       __FUNCTION__);
+		err = -ENOMEM;
+		goto out;
+	}
+	memset((char *)&g_nandfc_info, 0, sizeof(g_nandfc_info));
+
+	mxc_nand_data->dev = &pdev->dev;
+	/* structures must be linked */
+	this = &mxc_nand_data->nand;
+	mtd = &mxc_nand_data->mtd;
+	mtd->priv = this;
+	mtd->owner = THIS_MODULE;
+
+	/* 50 us command delay time */
+	this->chip_delay = 5;
+
+	this->priv = mxc_nand_data;
+	this->dev_ready = mxc_nand_dev_ready;
+	this->cmdfunc = mxc_nand_command;
+	this->select_chip = mxc_nand_select_chip;
+	this->read_byte = mxc_nand_read_byte;
+	this->read_word = mxc_nand_read_word;
+	this->write_buf = mxc_nand_write_buf;
+	this->read_buf = mxc_nand_read_buf;
+	this->verify_buf = mxc_nand_verify_buf;
+	this->scan_bbt = mxc_nand_scan_bbt;
+
+	nfc_clk = clk_get(&pdev->dev, "nfc_clk");
+	clk_enable(nfc_clk);
+
+	NFC_CONFIG1 |= NFC_INT_MSK;
+	init_waitqueue_head(&irq_waitq);
+	err = request_irq(MXC_INT_NANDFC, mxc_nfc_irq, 0, "mxc_nd", NULL);
+	if (err) {
+		goto out_1;
+	}
+
+	if (hardware_ecc) {
+		this->ecc.calculate = mxc_nand_calculate_ecc;
+		this->ecc.hwctl = mxc_nand_enable_hwecc;
+		this->ecc.correct = mxc_nand_correct_data;
+		this->ecc.mode = NAND_ECC_HW;
+		this->ecc.size = 512;
+		this->ecc.bytes = 3;
+		this->ecc.layout = &nand_hw_eccoob_8;
+		NFC_CONFIG1 |= NFC_ECC_EN;
+	} else {
+		this->ecc.mode = NAND_ECC_SOFT;
+	}
+
+	/* Reset NAND */
+	this->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+
+	/* preset operation */
+	/* Unlock the internal RAM Buffer */
+	NFC_CONFIG = 0x2;
+
+	/* Blocks to be unlocked */
+	NFC_UNLOCKSTART_BLKADDR = 0x0;
+	NFC_UNLOCKEND_BLKADDR = 0x4000;
+
+	/* Unlock Block Command for given address range */
+	NFC_WRPROT = 0x4;
+
+	/* NAND bus width determines access funtions used by upper layer */
+	if (flash->width == 2) {
+		this->options |= NAND_BUSWIDTH_16;
+		this->ecc.layout = &nand_hw_eccoob_16;
+	} else {
+		this->options |= 0;
+	}
+
+	is2k_Pagesize = 0;
+
+	/* Scan to find existence of the device */
+	if (nand_scan(mtd, 1)) {
+		DEBUG(MTD_DEBUG_LEVEL0,
+		      "MXC_ND: Unable to find any NAND device.\n");
+		err = -ENXIO;
+		goto out_1;
+	}
+
+	/* Register the partitions */
+#ifdef CONFIG_MTD_PARTITIONS
+	nr_parts =
+	    parse_mtd_partitions(mtd, part_probes, &mxc_nand_data->parts, 0);
+	if (nr_parts > 0)
+		add_mtd_partitions(mtd, mxc_nand_data->parts, nr_parts);
+	else if (flash->parts)
+		add_mtd_partitions(mtd, flash->parts, flash->nr_parts);
+	else
+#endif
+	{
+		pr_info("Registering %s as whole device\n", mtd->name);
+		add_mtd_device(mtd);
+	}
+#ifdef CONFIG_MXC_NAND_LOW_LEVEL_ERASE
+	/* Erase all the blocks of a NAND */
+	mxc_low_erase(mtd);
+#endif
+
+	platform_set_drvdata(pdev, mtd);
+	return 0;
+
+      out_1:
+	kfree(mxc_nand_data);
+      out:
+	return err;
+
+}
+
+ /*!
+  * Dissociates the driver from the device.
+  *
+  * @param   pdev  the device structure used to give information on which
+  *
+  * @return  The function always returns 0.
+  */
+
+static int __exit mxcnd_remove(struct platform_device *pdev)
+{
+	struct mtd_info *mtd = platform_get_drvdata(pdev);
+
+	clk_put(nfc_clk);
+	platform_set_drvdata(pdev, NULL);
+
+	if (mxc_nand_data) {
+		nand_release(mtd);
+		free_irq(MXC_INT_NANDFC, NULL);
+		kfree(mxc_nand_data);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/*!
+ * This function is called to put the NAND in a low power state. Refer to the
+ * document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device information structure
+ *
+ * @param   state the power state the device is entering
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+
+static int mxcnd_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct mtd_info *info = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	DEBUG(MTD_DEBUG_LEVEL0, "MXC_ND : NAND suspend\n");
+	if (info)
+		ret = info->suspend(info);
+
+	/* Disable the NFC clock */
+	clk_disable(nfc_clk);
+
+	return ret;
+}
+
+/*!
+ * This function is called to bring the NAND back from a low power state. Refer
+ * to the document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device information structure
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int mxcnd_resume(struct platform_device *pdev)
+{
+	struct mtd_info *info = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	DEBUG(MTD_DEBUG_LEVEL0, "MXC_ND : NAND resume\n");
+	/* Enable the NFC clock */
+	clk_enable(nfc_clk);
+
+	if (info) {
+		info->resume(info);
+	}
+
+	return ret;
+}
+
+#else
+#define mxcnd_suspend   NULL
+#define mxcnd_resume    NULL
+#endif				/* CONFIG_PM */
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcnd_driver = {
+	.driver = {
+		   .name = "mxc_nand_flash",
+		   },
+	.probe = mxcnd_probe,
+	.remove = __exit_p(mxcnd_remove),
+	.suspend = mxcnd_suspend,
+	.resume = mxcnd_resume,
+};
+
+/*!
+ * Main initialization routine
+ * @return  0 if successful; non-zero otherwise
+ */
+static int __init mxc_nd_init(void)
+{
+	/* Register the device driver structure. */
+	pr_info("MXC MTD nand Driver %s\n", DVR_VER);
+	if (platform_driver_register(&mxcnd_driver) != 0) {
+		printk(KERN_ERR "Driver register failed for mxcnd_driver\n");
+		return -ENODEV;
+	}
+	return 0;
+}
+
+/*!
+ * Clean up routine
+ */
+static void __exit mxc_nd_cleanup(void)
+{
+	/* Unregister the device structure */
+	platform_driver_unregister(&mxcnd_driver);
+}
+
+module_init(mxc_nd_init);
+module_exit(mxc_nd_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC NAND MTD driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/nand/mxc_nd.h b/drivers/mtd/nand/mxc_nd.h
new file mode 100644
index 000000000000..e38a9a637c1d
--- /dev/null
+++ b/drivers/mtd/nand/mxc_nd.h
@@ -0,0 +1,112 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_nd.h
+ *
+ * @brief This file contains the NAND Flash Controller register information.
+ *
+ *
+ * @ingroup NAND_MTD
+ */
+
+#ifndef __MXC_ND_H__
+#define __MXC_ND_H__
+
+#include <mach/hardware.h>
+
+/*
+ * Addresses for NFC registers
+ */
+#define NFC_BUF_SIZE            (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE00)))
+#define NFC_BUF_ADDR            (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE04)))
+#define NFC_FLASH_ADDR          (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE06)))
+#define NFC_FLASH_CMD           (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE08)))
+#define NFC_CONFIG              (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE0A)))
+#define NFC_ECC_STATUS_RESULT   (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE0C)))
+#define NFC_RSLTMAIN_AREA       (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE0E)))
+#define NFC_RSLTSPARE_AREA      (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE10)))
+#define NFC_WRPROT              (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE12)))
+#define NFC_UNLOCKSTART_BLKADDR (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE14)))
+#define NFC_UNLOCKEND_BLKADDR   (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE16)))
+#define NFC_NF_WRPRST           (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE18)))
+#define NFC_CONFIG1             (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE1A)))
+#define NFC_CONFIG2             (*((volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0xE1C)))
+
+/*!
+ * Addresses for NFC RAM BUFFER Main area 0
+ */
+#define MAIN_AREA0        (volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0x000)
+#define MAIN_AREA1        (volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0x200)
+#define MAIN_AREA2        (volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0x400)
+#define MAIN_AREA3        (volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0x600)
+
+/*!
+ * Addresses for NFC SPARE BUFFER Spare area 0
+ */
+#define SPARE_AREA0       (volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0x800)
+#define SPARE_AREA1       (volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0x810)
+#define SPARE_AREA2       (volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0x820)
+#define SPARE_AREA3       (volatile u16 *)IO_ADDRESS(NFC_BASE_ADDR + 0x830)
+
+/*!
+ * Set INT to 0, FCMD to 1, rest to 0 in NFC_CONFIG2 Register for Command
+ * operation
+ */
+#define NFC_CMD            0x1
+
+/*!
+ * Set INT to 0, FADD to 1, rest to 0 in NFC_CONFIG2 Register for Address
+ * operation
+ */
+#define NFC_ADDR           0x2
+
+/*!
+ * Set INT to 0, FDI to 1, rest to 0 in NFC_CONFIG2 Register for Input
+ * operation
+ */
+#define NFC_INPUT          0x4
+
+/*!
+ * Set INT to 0, FDO to 001, rest to 0 in NFC_CONFIG2 Register for Data Output
+ * operation
+ */
+#define NFC_OUTPUT         0x8
+
+/*!
+ * Set INT to 0, FD0 to 010, rest to 0 in NFC_CONFIG2 Register for Read ID
+ * operation
+ */
+#define NFC_ID             0x10
+
+/*!
+ * Set INT to 0, FDO to 100, rest to 0 in NFC_CONFIG2 Register for Read Status
+ * operation
+ */
+#define NFC_STATUS         0x20
+
+/*!
+ * Set INT to 1, rest to 0 in NFC_CONFIG2 Register for Read Status
+ * operation
+ */
+#define NFC_INT            0x8000
+
+#define NFC_SP_EN           (1 << 2)
+#define NFC_ECC_EN          (1 << 3)
+#define NFC_INT_MSK         (1 << 4)
+#define NFC_BIG             (1 << 5)
+#define NFC_RST             (1 << 6)
+#define NFC_CE              (1 << 7)
+#define NFC_ONE_CYCLE       (1 << 8)
+
+#endif				/* MXCND_H */
diff --git a/drivers/mtd/nand/mxc_nd2.c b/drivers/mtd/nand/mxc_nd2.c
new file mode 100644
index 000000000000..d8589026d191
--- /dev/null
+++ b/drivers/mtd/nand/mxc_nd2.c
@@ -0,0 +1,1458 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/mtd/partitions.h>
+#include <asm/mach/flash.h>
+#include <asm/io.h>
+#include "mxc_nd2.h"
+
+#define DVR_VER "2.5"
+
+/* Global address Variables */
+static u32 nfc_axi_base, nfc_ip_base;
+
+struct mxc_mtd_s {
+	struct mtd_info mtd;
+	struct nand_chip nand;
+	struct mtd_partition *parts;
+	struct device *dev;
+};
+
+static struct mxc_mtd_s *mxc_nand_data;
+
+/*
+ * Define delays in microsec for NAND device operations
+ */
+#define TROP_US_DELAY   2000
+
+struct nand_info {
+	bool bStatusRequest;
+	u16 colAddr;
+};
+
+static struct nand_info g_nandfc_info;
+
+#ifdef CONFIG_MTD_NAND_MXC_SWECC
+static int hardware_ecc = 0;
+#else
+static int hardware_ecc = 1;
+#endif
+
+static u8 num_of_interleave = 1;
+
+static u8 *data_buf;
+static u8 *oob_buf;
+
+static int g_page_mask;
+
+static struct clk *nfc_clk;
+
+/*
+ * OOB placement block for use with hardware ecc generation
+ */
+static struct nand_ecclayout nand_hw_eccoob_512 = {
+	.eccbytes = 9,
+	.eccpos = {7, 8, 9, 10, 11, 12, 13, 14, 15},
+	.oobavail = 4,
+	.oobfree = {{0, 4}}
+};
+
+static struct nand_ecclayout nand_hw_eccoob_2k = {
+	.eccbytes = 9,
+	.eccpos = {7, 8, 9, 10, 11, 12, 13, 14, 15},
+	.oobavail = 4,
+	.oobfree = {{2, 4}}
+};
+
+static struct nand_ecclayout nand_hw_eccoob_4k = {
+	.eccbytes = 9,
+	.eccpos = {7, 8, 9, 10, 11, 12, 13, 14, 15},
+	.oobavail = 4,
+	.oobfree = {{2, 4}}
+};
+
+/*!
+ * @defgroup NAND_MTD NAND Flash MTD Driver for MXC processors
+ */
+
+/*!
+ * @file mxc_nd2.c
+ *
+ * @brief This file contains the hardware specific layer for NAND Flash on
+ * MXC processor
+ *
+ * @ingroup NAND_MTD
+ */
+
+#ifdef CONFIG_MTD_PARTITIONS
+static const char *part_probes[] = { "RedBoot", "cmdlinepart", NULL };
+#endif
+
+static wait_queue_head_t irq_waitq;
+
+static irqreturn_t mxc_nfc_irq(int irq, void *dev_id)
+{
+	/* Disable Interuupt */
+	raw_write(raw_read(REG_NFC_INTRRUPT) | NFC_INT_MSK, REG_NFC_INTRRUPT);
+	wake_up(&irq_waitq);
+
+	return IRQ_HANDLED;
+}
+
+static void nfc_memcpy(void *dest, void *src, int len)
+{
+	u8 *d = dest;
+	u8 *s = src;
+
+	while (len > 0) {
+		if (len >= 4) {
+			*(u32 *)d = *(u32 *)s;
+			d += 4;
+			s += 4;
+			len -= 4;
+		} else {
+			*(u16 *)d = *(u16 *)s;
+			len -= 2;
+			break;
+		}
+	}
+
+	if (len)
+		BUG();
+}
+
+/*
+ * Functions to transfer data to/from spare erea.
+ */
+static void
+copy_spare(struct mtd_info *mtd, void *pbuf, void *pspare, int len, bool bfrom)
+{
+	u16 i, j;
+	u16 m = mtd->oobsize;
+	u16 n = mtd->writesize >> 9;
+	u8 *d = (u8 *) pbuf;
+	u8 *s = (u8 *) pspare;
+	u16 t = SPARE_LEN;
+
+	m /= num_of_interleave;
+	n /= num_of_interleave;
+
+	j = (m / n >> 1) << 1;
+
+	if (bfrom) {
+		for (i = 0; i < n - 1; i++)
+			nfc_memcpy(&d[i * j], &s[i * t], j);
+
+		/* the last section */
+		nfc_memcpy(&d[i * j], &s[i * t], len - i * j);
+	} else {
+		for (i = 0; i < n - 1; i++)
+			nfc_memcpy(&s[i * t], &d[i * j], j);
+
+		/* the last section */
+		nfc_memcpy(&s[i * t], &d[i * j], len - i * j);
+	}
+}
+
+/*!
+ * This function polls the NFC to wait for the basic operation to complete by
+ * checking the INT bit of config2 register.
+ *
+ * @param       maxRetries     number of retry attempts (separated by 1 us)
+ * @param       useirq         True if IRQ should be used rather than polling
+ */
+static void wait_op_done(int maxRetries, bool useirq)
+{
+	if (useirq) {
+		if ((raw_read(REG_NFC_OPS_STAT) & NFC_OPS_STAT) == 0) {
+			/* Enable Interuupt */
+			raw_write(raw_read(REG_NFC_INTRRUPT) & ~NFC_INT_MSK,
+				  REG_NFC_INTRRUPT);
+			wait_event(irq_waitq,
+				   (raw_read(REG_NFC_OPS_STAT) & NFC_OPS_STAT));
+		}
+		WRITE_NFC_IP_REG((raw_read(REG_NFC_OPS_STAT) &
+				  ~NFC_OPS_STAT), REG_NFC_OPS_STAT);
+	} else {
+		while (1) {
+			maxRetries--;
+			if (raw_read(REG_NFC_OPS_STAT) & NFC_OPS_STAT) {
+				WRITE_NFC_IP_REG((raw_read(REG_NFC_OPS_STAT) &
+						  ~NFC_OPS_STAT),
+						 REG_NFC_OPS_STAT);
+				break;
+			}
+			udelay(1);
+			if (maxRetries <= 0) {
+				DEBUG(MTD_DEBUG_LEVEL0, "%s(%d): INT not set\n",
+				      __func__, __LINE__);
+			}
+		}
+	}
+}
+
+static inline void send_atomic_cmd(u16 cmd, bool useirq)
+{
+	/* fill command */
+	raw_write(cmd, REG_NFC_FLASH_CMD);
+
+	/* clear status */
+	ACK_OPS;
+
+	/* send out command */
+	raw_write(NFC_CMD, REG_NFC_OPS);
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, useirq);
+}
+
+static void mxc_do_addr_cycle(struct mtd_info *mtd, int column, int page_addr);
+static int mxc_check_ecc_status(struct mtd_info *mtd);
+
+#ifdef NFC_AUTO_MODE_ENABLE
+/*!
+ * This function handle the interleave related work
+ * @param	mtd	mtd info
+ * @param	cmd	command
+ */
+static void auto_cmd_interleave(struct mtd_info *mtd, u16 cmd)
+{
+	u32 i, page_addr, ncs;
+	u32 j = num_of_interleave;
+	struct nand_chip *this = mtd->priv;
+	u32 addr_low = raw_read(NFC_FLASH_ADDR0);
+	u32 addr_high = raw_read(NFC_FLASH_ADDR8);
+	u8 *dbuf = data_buf;
+	u8 *obuf = oob_buf;
+	u32 dlen = mtd->writesize / j;
+	u32 olen = mtd->oobsize / j;
+
+	/* adjust the addr value
+	 * since ADD_OP mode is 01
+	 */
+	if (cmd == NAND_CMD_ERASE2)
+		page_addr = addr_low;
+	else
+		page_addr = addr_low >> 16 | addr_high << 16;
+
+	ncs = page_addr >> (this->chip_shift - this->page_shift);
+
+	if (j > 1) {
+		page_addr *= j;
+	} else {
+		page_addr *= this->numchips;
+		page_addr += ncs;
+	}
+
+	switch (cmd) {
+	case NAND_CMD_PAGEPROG:
+		for (i = 0; i < j; i++) {
+			/* reset addr cycle */
+			mxc_do_addr_cycle(mtd, 0, page_addr++);
+
+			/* data transfer */
+			memcpy(MAIN_AREA0, dbuf, dlen);
+			copy_spare(mtd, obuf, SPARE_AREA0, olen, false);
+
+			/* update the value */
+			dbuf += dlen;
+			obuf += olen;
+
+			NFC_SET_RBA(0);
+			ACK_OPS;
+			raw_write(NFC_AUTO_PROG, REG_NFC_OPS);
+
+			/* wait auto_prog_done bit set */
+			while (!(raw_read(REG_NFC_OPS_STAT) & NFC_OP_DONE)) ;
+		}
+
+		wait_op_done(TROP_US_DELAY, false);
+		while (!(raw_read(REG_NFC_OPS_STAT) & NFC_RB)) ;
+
+		break;
+	case NAND_CMD_READSTART:
+		for (i = 0; i < j; i++) {
+			/* reset addr cycle */
+			mxc_do_addr_cycle(mtd, 0, page_addr++);
+
+			NFC_SET_RBA(0);
+			ACK_OPS;
+			raw_write(NFC_AUTO_READ, REG_NFC_OPS);
+			wait_op_done(TROP_US_DELAY, false);
+
+			/* check ecc error */
+			mxc_check_ecc_status(mtd);
+
+			/* data transfer */
+			memcpy(dbuf, MAIN_AREA0, dlen);
+			copy_spare(mtd, obuf, SPARE_AREA0, olen, true);
+
+			/* update the value */
+			dbuf += dlen;
+			obuf += olen;
+		}
+		break;
+	case NAND_CMD_ERASE2:
+		for (i = 0; i < j; i++) {
+			mxc_do_addr_cycle(mtd, -1, page_addr++);
+			ACK_OPS;
+			raw_write(NFC_AUTO_ERASE, REG_NFC_OPS);
+			wait_op_done(TROP_US_DELAY, true);
+		}
+		break;
+	case NAND_CMD_RESET:
+		for (i = 0; i < j; i++) {
+			if (j > 1)
+				NFC_SET_NFC_ACTIVE_CS(i);
+			send_atomic_cmd(cmd, false);
+		}
+		break;
+	default:
+		break;
+	}
+}
+#endif
+
+static void send_addr(u16 addr, bool useirq);
+
+/*!
+ * This function issues the specified command to the NAND device and
+ * waits for completion.
+ *
+ * @param       cmd     command for NAND Flash
+ * @param       useirq  True if IRQ should be used rather than polling
+ */
+static void send_cmd(struct mtd_info *mtd, u16 cmd, bool useirq)
+{
+	DEBUG(MTD_DEBUG_LEVEL3, "send_cmd(0x%x, %d)\n", cmd, useirq);
+
+#ifdef NFC_AUTO_MODE_ENABLE
+	switch (cmd) {
+	case NAND_CMD_READ0:
+	case NAND_CMD_READOOB:
+		raw_write(NAND_CMD_READ0, REG_NFC_FLASH_CMD);
+		break;
+	case NAND_CMD_SEQIN:
+	case NAND_CMD_ERASE1:
+		raw_write(cmd, REG_NFC_FLASH_CMD);
+		break;
+	case NAND_CMD_PAGEPROG:
+	case NAND_CMD_ERASE2:
+	case NAND_CMD_READSTART:
+		raw_write(raw_read(REG_NFC_FLASH_CMD) | cmd << NFC_CMD_1_SHIFT,
+			  REG_NFC_FLASH_CMD);
+		auto_cmd_interleave(mtd, cmd);
+		break;
+	case NAND_CMD_READID:
+		send_atomic_cmd(cmd, useirq);
+		send_addr(0, false);
+		break;
+	case NAND_CMD_RESET:
+		auto_cmd_interleave(mtd, cmd);
+		break;
+	case NAND_CMD_STATUS:
+		send_atomic_cmd(cmd, useirq);
+		break;
+	default:
+		break;
+	}
+#else
+	send_atomic_cmd(cmd, useirq);
+#endif
+}
+
+/*!
+ * This function sends an address (or partial address) to the
+ * NAND device.  The address is used to select the source/destination for
+ * a NAND command.
+ *
+ * @param       addr    address to be written to NFC.
+ * @param       useirq  True if IRQ should be used rather than polling
+ */
+static void send_addr(u16 addr, bool useirq)
+{
+	DEBUG(MTD_DEBUG_LEVEL3, "send_addr(0x%x %d)\n", addr, useirq);
+
+	/* fill address */
+	raw_write((addr << NFC_FLASH_ADDR_SHIFT), REG_NFC_FLASH_ADDR);
+
+	/* clear status */
+	ACK_OPS;
+
+	/* send out address */
+	raw_write(NFC_ADDR, REG_NFC_OPS);
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, useirq);
+}
+
+/*!
+ * This function requests the NFC to initate the transfer
+ * of data currently in the NFC RAM buffer to the NAND device.
+ *
+ * @param	buf_id	      Specify Internal RAM Buffer number
+ */
+static void send_prog_page(u8 buf_id)
+{
+#ifndef NFC_AUTO_MODE_ENABLE
+	DEBUG(MTD_DEBUG_LEVEL3, "%s\n", __FUNCTION__);
+
+	/* set ram buffer id */
+	NFC_SET_RBA(buf_id);
+
+	/* clear status */
+	ACK_OPS;
+
+	/* transfer data from NFC ram to nand */
+	raw_write(NFC_INPUT, REG_NFC_OPS);
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, false);
+#endif
+}
+
+/*!
+ * This function requests the NFC to initated the transfer
+ * of data from the NAND device into in the NFC ram buffer.
+ *
+ * @param  	buf_id		Specify Internal RAM Buffer number
+ */
+static void send_read_page(u8 buf_id)
+{
+#ifndef NFC_AUTO_MODE_ENABLE
+	DEBUG(MTD_DEBUG_LEVEL3, "%s(%d)\n", __FUNCTION__, buf_id);
+
+	/* set ram buffer id */
+	NFC_SET_RBA(buf_id);
+
+	/* clear status */
+	ACK_OPS;
+
+	/* transfer data from nand to NFC ram */
+	raw_write(NFC_OUTPUT, REG_NFC_OPS);
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, false);
+#endif
+}
+
+/*!
+ * This function requests the NFC to perform a read of the
+ * NAND device ID.
+ */
+static void send_read_id(void)
+{
+	/* Set RBA bits for BUFFER0 */
+	NFC_SET_RBA(0);
+
+	/* clear status */
+	ACK_OPS;
+
+	/* Read ID into main buffer */
+	raw_write(NFC_ID, REG_NFC_OPS);
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, false);
+
+}
+
+#ifdef NFC_AUTO_MODE_ENABLE
+static inline void read_dev_status(u16 *status)
+{
+	u32 mask = 0xFF << 16;
+
+	/* clear status */
+	ACK_OPS;
+
+
+	/* use atomic mode to read status instead
+	   of using auto mode,auto-mode has issues
+	   and the status is not correct.
+	*/
+	raw_write(NFC_STATUS, REG_NFC_OPS);
+
+	wait_op_done(TROP_US_DELAY, false);
+
+	*status = (raw_read(NFC_CONFIG1) & mask) >> 16;
+
+}
+#endif
+
+/*!
+ * This function requests the NFC to perform a read of the
+ * NAND device status and returns the current status.
+ *
+ * @return  device status
+ */
+static u16 get_dev_status(void)
+{
+#ifdef NFC_AUTO_MODE_ENABLE
+	int i;
+	u16 status = 0;
+	for (i = 0; i < num_of_interleave; i++) {
+
+		/* set ative cs */
+		NFC_SET_NFC_ACTIVE_CS(i);
+
+		/* FIXME, NFC Auto erase may have
+		 * problem, have to pollingit until
+		 * the nand get idle, otherwise
+		 * it may get error
+		 */
+		read_dev_status(&status);
+		if (status & NAND_STATUS_FAIL)
+			break;
+	}
+
+	return status;
+#else
+	volatile u16 *mainBuf = MAIN_AREA1;
+	u8 val = 1;
+	u16 ret;
+
+	/* Set ram buffer id */
+	NFC_SET_RBA(val);
+
+	/* clear status */
+	ACK_OPS;
+
+	/* Read status into main buffer */
+	raw_write(NFC_STATUS, REG_NFC_OPS);
+
+	/* Wait for operation to complete */
+	wait_op_done(TROP_US_DELAY, false);
+
+	/* Status is placed in first word of main buffer */
+	/* get status, then recovery area 1 data */
+	ret = *mainBuf;
+
+	return ret;
+#endif
+}
+
+static void mxc_nand_enable_hwecc(struct mtd_info *mtd, int mode)
+{
+	raw_write((raw_read(REG_NFC_ECC_EN) | NFC_ECC_EN), REG_NFC_ECC_EN);
+	return;
+}
+
+/*
+ * Function to record the ECC corrected/uncorrected errors resulted
+ * after a page read. This NFC detects and corrects upto to 4 symbols
+ * of 9-bits each.
+ */
+static int mxc_check_ecc_status(struct mtd_info *mtd)
+{
+	u32 ecc_stat, err;
+	int no_subpages = 1;
+	int ret = 0;
+	u8 ecc_bit_mask, err_limit;
+
+	ecc_bit_mask = (IS_4BIT_ECC ? 0x7 : 0xf);
+	err_limit = (IS_4BIT_ECC ? 0x4 : 0x8);
+
+	no_subpages = mtd->writesize >> 9;
+
+	no_subpages /= num_of_interleave;
+
+	ecc_stat = GET_NFC_ECC_STATUS();
+	do {
+		err = ecc_stat & ecc_bit_mask;
+		if (err > err_limit) {
+			mtd->ecc_stats.failed++;
+			printk(KERN_WARNING "UnCorrectable RS-ECC Error\n");
+			return -1;
+		} else {
+			ret += err;
+		}
+		ecc_stat >>= 4;
+	} while (--no_subpages);
+
+	mtd->ecc_stats.corrected += ret;
+	pr_debug("%d Symbol Correctable RS-ECC Error\n", ret);
+
+	return ret;
+}
+
+/*
+ * Function to correct the detected errors. This NFC corrects all the errors
+ * detected. So this function just return 0.
+ */
+static int mxc_nand_correct_data(struct mtd_info *mtd, u_char * dat,
+				 u_char * read_ecc, u_char * calc_ecc)
+{
+	return 0;
+}
+
+/*
+ * Function to calculate the ECC for the data to be stored in the Nand device.
+ * This NFC has a hardware RS(511,503) ECC engine together with the RS ECC
+ * CONTROL blocks are responsible for detection  and correction of up to
+ * 8 symbols of 9 bits each in 528 byte page.
+ * So this function is just return 0.
+ */
+
+static int mxc_nand_calculate_ecc(struct mtd_info *mtd, const u_char * dat,
+				  u_char * ecc_code)
+{
+	return 0;
+}
+
+/*!
+ * This function id is used to read the data buffer from the NAND Flash. To
+ * read the data from NAND Flash first the data output cycle is initiated by
+ * the NFC, which copies the data to RAMbuffer. This data of length \b len is
+ * then copied to buffer \b buf.
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ * @param       buf     data to be read from NAND Flash
+ * @param       len     number of bytes to be read
+ */
+static void mxc_nand_read_buf(struct mtd_info *mtd, u_char * buf, int len)
+{
+	u16 col = g_nandfc_info.colAddr;
+
+	if (mtd->writesize) {
+
+		int j = mtd->writesize - col;
+		int n = mtd->oobsize + j;
+
+		n = min(n, len);
+
+		if (j > 0) {
+			if (n > j) {
+				memcpy(buf, &data_buf[col], j);
+				memcpy(buf + j, &oob_buf[0], n - j);
+			} else {
+				memcpy(buf, &data_buf[col], n);
+			}
+		} else {
+			col -= mtd->writesize;
+			memcpy(buf, &oob_buf[col], len);
+		}
+
+		/* update */
+		g_nandfc_info.colAddr += n;
+
+	} else {
+		/* At flash identify phase,
+		 * mtd->writesize has not been
+		 * set correctly, it should
+		 * be zero.And len will less 2
+		 */
+		memcpy(buf, &data_buf[col], len);
+
+		/* update */
+		g_nandfc_info.colAddr += len;
+	}
+
+}
+
+/*!
+ * This function reads byte from the NAND Flash
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ *
+ * @return    data read from the NAND Flash
+ */
+static uint8_t mxc_nand_read_byte(struct mtd_info *mtd)
+{
+	uint8_t ret;
+
+	/* Check for status request */
+	if (g_nandfc_info.bStatusRequest) {
+		return (get_dev_status() & 0xFF);
+	}
+
+	mxc_nand_read_buf(mtd, &ret, 1);
+
+	return ret;
+}
+
+/*!
+  * This function reads word from the NAND Flash
+  *
+  * @param     mtd     MTD structure for the NAND Flash
+  *
+  * @return    data read from the NAND Flash
+  */
+static u16 mxc_nand_read_word(struct mtd_info *mtd)
+{
+	u16 ret;
+
+	mxc_nand_read_buf(mtd, (uint8_t *) &ret, sizeof(u16));
+
+	return ret;
+}
+
+/*!
+ * This function reads byte from the NAND Flash
+ *
+ * @param     mtd     MTD structure for the NAND Flash
+ *
+ * @return    data read from the NAND Flash
+ */
+static u_char mxc_nand_read_byte16(struct mtd_info *mtd)
+{
+	/* Check for status request */
+	if (g_nandfc_info.bStatusRequest) {
+		return (get_dev_status() & 0xFF);
+	}
+
+	return mxc_nand_read_word(mtd) & 0xFF;
+}
+
+/*!
+ * This function writes data of length \b len from buffer \b buf to the NAND
+ * internal RAM buffer's MAIN area 0.
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ * @param       buf     data to be written to NAND Flash
+ * @param       len     number of bytes to be written
+ */
+static void mxc_nand_write_buf(struct mtd_info *mtd,
+			       const u_char * buf, int len)
+{
+	u16 col = g_nandfc_info.colAddr;
+	int j = mtd->writesize - col;
+	int n = mtd->oobsize + j;
+
+	n = min(n, len);
+
+	if (j > 0) {
+		if (n > j) {
+			memcpy(&data_buf[col], buf, j);
+			memcpy(&oob_buf[0], buf + j, n - j);
+		} else {
+			memcpy(&data_buf[col], buf, n);
+		}
+	} else {
+		col -= mtd->writesize;
+		memcpy(&oob_buf[col], buf, len);
+	}
+
+	/* update */
+	g_nandfc_info.colAddr += n;
+}
+
+/*!
+ * This function is used by the upper layer to verify the data in NAND Flash
+ * with the data in the \b buf.
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ * @param       buf     data to be verified
+ * @param       len     length of the data to be verified
+ *
+ * @return      -EFAULT if error else 0
+ *
+ */
+static int mxc_nand_verify_buf(struct mtd_info *mtd, const u_char * buf,
+			       int len)
+{
+	u_char *s = data_buf;
+
+	const u_char *p = buf;
+
+	for (; len > 0; len--) {
+		if (*p++ != *s++)
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+/*!
+ * This function is used by upper layer for select and deselect of the NAND
+ * chip
+ *
+ * @param       mtd     MTD structure for the NAND Flash
+ * @param       chip    val indicating select or deselect
+ */
+static void mxc_nand_select_chip(struct mtd_info *mtd, int chip)
+{
+
+	switch (chip) {
+	case -1:
+		/* Disable the NFC clock */
+		clk_disable(nfc_clk);
+		break;
+	case 0 ... 7:
+		/* Enable the NFC clock */
+		clk_enable(nfc_clk);
+
+		NFC_SET_NFC_ACTIVE_CS(chip);
+		break;
+
+	default:
+		break;
+	}
+}
+
+/*
+ * Function to perform the address cycles.
+ */
+static void mxc_do_addr_cycle(struct mtd_info *mtd, int column, int page_addr)
+{
+#ifdef NFC_AUTO_MODE_ENABLE
+
+	if (page_addr != -1 && column != -1) {
+		u32 mask = 0xFFFF;
+		/* the column address */
+		raw_write(column & mask, NFC_FLASH_ADDR0);
+		raw_write((raw_read(NFC_FLASH_ADDR0) |
+			   ((page_addr & mask) << 16)), NFC_FLASH_ADDR0);
+		/* the row address */
+		raw_write(((raw_read(NFC_FLASH_ADDR8) & (mask << 16)) |
+			   ((page_addr & (mask << 16)) >> 16)),
+			  NFC_FLASH_ADDR8);
+	} else if (page_addr != -1) {
+		raw_write(page_addr, NFC_FLASH_ADDR0);
+		raw_write(0, NFC_FLASH_ADDR8);
+	}
+
+	DEBUG(MTD_DEBUG_LEVEL3,
+	      "AutoMode:the ADDR REGS value is (0x%x, 0x%x)\n",
+	      raw_read(NFC_FLASH_ADDR0), raw_read(NFC_FLASH_ADDR8));
+#else
+
+	u32 page_mask = g_page_mask;
+
+	if (column != -1) {
+		send_addr(column & 0xFF, false);
+		if (IS_2K_PAGE_NAND) {
+			/* another col addr cycle for 2k page */
+			send_addr((column >> 8) & 0xF, false);
+		} else if (IS_4K_PAGE_NAND) {
+			/* another col addr cycle for 4k page */
+			send_addr((column >> 8) & 0x1F, false);
+		}
+	}
+	if (page_addr != -1) {
+		do {
+			send_addr((page_addr & 0xff), false);
+			page_mask >>= 8;
+			page_addr >>= 8;
+		} while (page_mask != 0);
+	}
+#endif
+}
+
+/*!
+ * This function is used by the upper layer to write command to NAND Flash for
+ * different operations to be carried out on NAND Flash
+ *
+ * @param       mtd             MTD structure for the NAND Flash
+ * @param       command         command for NAND Flash
+ * @param       column          column offset for the page read
+ * @param       page_addr       page to be read from NAND Flash
+ */
+static void mxc_nand_command(struct mtd_info *mtd, unsigned command,
+			     int column, int page_addr)
+{
+	bool useirq = false;
+
+	DEBUG(MTD_DEBUG_LEVEL3,
+	      "mxc_nand_command (cmd = 0x%x, col = 0x%x, page = 0x%x)\n",
+	      command, column, page_addr);
+	/*
+	 * Reset command state information
+	 */
+	g_nandfc_info.bStatusRequest = false;
+
+	/*
+	 * Command pre-processing step
+	 */
+	switch (command) {
+	case NAND_CMD_STATUS:
+		g_nandfc_info.colAddr = 0;
+		g_nandfc_info.bStatusRequest = true;
+		break;
+
+	case NAND_CMD_READ0:
+		g_nandfc_info.colAddr = column;
+		break;
+
+	case NAND_CMD_READOOB:
+		g_nandfc_info.colAddr = column;
+		command = NAND_CMD_READ0;
+		break;
+
+	case NAND_CMD_SEQIN:
+		if (column != 0) {
+
+			/* FIXME: before send SEQIN command for
+			 * partial write,We need read one page out.
+			 * FSL NFC does not support partial write
+			 * It alway send out 512+ecc+512+ecc ...
+			 * for large page nand flash. But for small
+			 * page nand flash, it did support SPARE
+			 * ONLY operation. But to make driver
+			 * simple. We take the same as large page,read
+			 * whole page out and update. As for MLC nand
+			 * NOP(num of operation) = 1. Partial written
+			 * on one programed page is not allowed! We
+			 * can't limit it on the driver, it need the
+			 * upper layer applicaiton take care it
+			 */
+
+			mxc_nand_command(mtd, NAND_CMD_READ0, 0, page_addr);
+		}
+
+		g_nandfc_info.colAddr = column;
+		column = 0;
+
+		break;
+
+	case NAND_CMD_PAGEPROG:
+#ifndef NFC_AUTO_MODE_ENABLE
+		/* FIXME:the NFC interal buffer
+		 * access has some limitation, it
+		 * does not allow byte access. To
+		 * make the code simple and ease use
+		 * not every time check the address
+		 * alignment.Use the temp buffer
+		 * to accomadate the data.since We
+		 * know data_buf will be at leat 4
+		 * byte alignment, so we can use
+		 * memcpy safely
+		 */
+		nfc_memcpy(MAIN_AREA0, data_buf, mtd->writesize);
+		copy_spare(mtd, oob_buf, SPARE_AREA0, mtd->oobsize, false);
+#endif
+
+		if (IS_LARGE_PAGE_NAND)
+			PROG_PAGE();
+		else
+			send_prog_page(0);
+
+		useirq = true;
+
+		break;
+
+	case NAND_CMD_ERASE1:
+		break;
+	case NAND_CMD_ERASE2:
+		useirq = true;
+
+		break;
+	}
+
+	/*
+	 * Write out the command to the device.
+	 */
+	send_cmd(mtd, command, useirq);
+
+	mxc_do_addr_cycle(mtd, column, page_addr);
+
+	/*
+	 * Command post-processing step
+	 */
+	switch (command) {
+
+	case NAND_CMD_READOOB:
+	case NAND_CMD_READ0:
+		if (IS_LARGE_PAGE_NAND) {
+			/* send read confirm command */
+			send_cmd(mtd, NAND_CMD_READSTART, false);
+			/* read for each AREA */
+			READ_PAGE();
+		} else {
+			send_read_page(0);
+		}
+
+#ifndef NFC_AUTO_MODE_ENABLE
+		/* FIXME, the NFC interal buffer
+		 * access has some limitation, it
+		 * does not allow byte access. To
+		 * make the code simple and ease use
+		 * not every time check the address
+		 * alignment.Use the temp buffer
+		 * to accomadate the data.since We
+		 * know data_buf will be at leat 4
+		 * byte alignment, so we can use
+		 * memcpy safely
+		 */
+		nfc_memcpy(data_buf, MAIN_AREA0, mtd->writesize);
+		copy_spare(mtd, oob_buf, SPARE_AREA0, mtd->oobsize, true);
+#endif
+
+		break;
+
+	case NAND_CMD_READID:
+		send_read_id();
+		g_nandfc_info.colAddr = column;
+		nfc_memcpy(data_buf, MAIN_AREA0, 2048);
+
+		break;
+	}
+}
+
+static int mxc_nand_read_oob(struct mtd_info *mtd,
+			     struct nand_chip *chip, int page, int sndcmd)
+{
+	if (sndcmd) {
+
+		chip->cmdfunc(mtd, NAND_CMD_READ0, 0x00, page);
+		sndcmd = 0;
+	}
+
+	memcpy(chip->oob_poi, oob_buf, mtd->oobsize);
+
+	return sndcmd;
+}
+
+static int mxc_nand_read_page(struct mtd_info *mtd, struct nand_chip *chip,
+			      uint8_t * buf)
+{
+
+#ifndef NFC_AUTO_MODE_ENABLE
+	mxc_check_ecc_status(mtd);
+#endif
+
+	memcpy(buf, data_buf, mtd->writesize);
+	memcpy(chip->oob_poi, oob_buf, mtd->oobsize);
+
+	return 0;
+}
+
+static void mxc_nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
+				const uint8_t * buf)
+{
+	memcpy(data_buf, buf, mtd->writesize);
+	memcpy(oob_buf, chip->oob_poi, mtd->oobsize);
+
+}
+
+/* Define some generic bad / good block scan pattern which are used
+ * while scanning a device for factory marked good / bad blocks. */
+static uint8_t scan_ff_pattern[] = { 0xff, 0xff };
+
+static struct nand_bbt_descr smallpage_memorybased = {
+	.options = NAND_BBT_SCAN2NDPAGE,
+	.offs = 5,
+	.len = 1,
+	.pattern = scan_ff_pattern
+};
+
+static struct nand_bbt_descr largepage_memorybased = {
+	.options = 0,
+	.offs = 0,
+	.len = 2,
+	.pattern = scan_ff_pattern
+};
+
+/* Generic flash bbt decriptors
+*/
+static uint8_t bbt_pattern[] = { 'B', 'b', 't', '0' };
+static uint8_t mirror_pattern[] = { '1', 't', 'b', 'B' };
+
+static struct nand_bbt_descr bbt_main_descr = {
+	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
+	    | NAND_BBT_2BIT | NAND_BBT_VERSION,
+	.offs = 0,
+	.len = 4,
+	.veroffs = 4,
+	.maxblocks = 4,
+	.pattern = bbt_pattern
+};
+
+static struct nand_bbt_descr bbt_mirror_descr = {
+	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
+	    | NAND_BBT_2BIT | NAND_BBT_VERSION,
+	.offs = 0,
+	.len = 4,
+	.veroffs = 4,
+	.maxblocks = 4,
+	.pattern = mirror_pattern
+};
+
+static int mxc_nand_scan_bbt(struct mtd_info *mtd)
+{
+	struct nand_chip *this = mtd->priv;
+
+	g_page_mask = this->pagemask;
+
+	/* limit to 2G size due to Kernel
+	 * larger 4G space support,need fix
+	 * it later
+	 */
+	if (mtd->size == 0) {
+		mtd->size = 1 << 31;
+		this->numchips = 1;
+		this->chipsize = mtd->size;
+	}
+
+	if (IS_2K_PAGE_NAND) {
+		NFC_SET_NFMS(1 << NFMS_NF_PG_SZ);
+		this->ecc.layout = &nand_hw_eccoob_2k;
+	} else if (IS_4K_PAGE_NAND) {
+		NFC_SET_NFMS(1 << NFMS_NF_PG_SZ);
+		this->ecc.layout = &nand_hw_eccoob_4k;
+	} else {
+		this->ecc.layout = &nand_hw_eccoob_512;
+	}
+
+	/* propagate ecc.layout to mtd_info */
+	mtd->ecclayout = this->ecc.layout;
+
+	/* jffs2 not write oob */
+	mtd->flags &= ~MTD_OOB_WRITEABLE;
+
+	/* use flash based bbt */
+	this->bbt_td = &bbt_main_descr;
+	this->bbt_md = &bbt_mirror_descr;
+
+	/* update flash based bbt */
+	this->options |= NAND_USE_FLASH_BBT;
+
+	if (!this->badblock_pattern) {
+		this->badblock_pattern = (mtd->writesize > 512) ?
+		    &largepage_memorybased : &smallpage_memorybased;
+	}
+
+	/* Build bad block table */
+	return nand_scan_bbt(mtd, this->badblock_pattern);
+}
+
+static void mxc_nfc_init(void)
+{
+	/* Disable interrupt */
+	raw_write((raw_read(REG_NFC_INTRRUPT) | NFC_INT_MSK), REG_NFC_INTRRUPT);
+
+	/* disable spare enable */
+	raw_write(raw_read(REG_NFC_SP_EN) & ~NFC_SP_EN, REG_NFC_SP_EN);
+
+	/* Unlock the internal RAM Buffer */
+	raw_write(NFC_SET_BLS(NFC_BLS_UNLCOKED), REG_NFC_BLS);
+
+	/* Blocks to be unlocked */
+	UNLOCK_ADDR(0x0, 0xFFFF);
+
+	/* Unlock Block Command for given address range */
+	raw_write(NFC_SET_WPC(NFC_WPC_UNLOCK), REG_NFC_WPC);
+
+	/* Enable symetric mode by default except mx37TO1.0 */
+	if (!(cpu_is_mx37_rev(CHIP_REV_1_0) == 1))
+		raw_write(raw_read(REG_NFC_ONE_CYCLE) |
+			  NFC_ONE_CYCLE, REG_NFC_ONE_CYCLE);
+}
+
+static int mxc_alloc_buf(void)
+{
+	int err = 0;
+
+	data_buf = kzalloc(NAND_MAX_PAGESIZE, GFP_KERNEL);
+	if (!data_buf) {
+		printk(KERN_ERR "%s: failed to allocate data_buf\n", __func__);
+		err = -ENOMEM;
+		goto out;
+	}
+	oob_buf = kzalloc(NAND_MAX_OOBSIZE, GFP_KERNEL);
+	if (!oob_buf) {
+		printk(KERN_ERR "%s: failed to allocate oob_buf\n", __func__);
+		err = -ENOMEM;
+		goto out;
+	}
+
+      out:
+	return err;
+}
+
+static void mxc_free_buf(void)
+{
+	kfree(data_buf);
+	kfree(oob_buf);
+}
+
+/*!
+ * This function is called during the driver binding process.
+ *
+ * @param   pdev  the device structure used to store device specific
+ *                information that is used by the suspend, resume and
+ *                remove functions
+ *
+ * @return  The function always returns 0.
+ */
+static int __init mxcnd_probe(struct platform_device *pdev)
+{
+	struct nand_chip *this;
+	struct mtd_info *mtd;
+	struct flash_platform_data *flash = pdev->dev.platform_data;
+	int nr_parts = 0, err = 0;
+
+	nfc_axi_base = IO_ADDRESS(NFC_AXI_BASE_ADDR);
+	nfc_ip_base = IO_ADDRESS(NFC_BASE_ADDR);
+
+	/* init the nfc */
+	mxc_nfc_init();
+
+	/* init data buf */
+	if (mxc_alloc_buf())
+		goto out;
+
+	/* Allocate memory for MTD device structure and private data */
+	mxc_nand_data = kzalloc(sizeof(struct mxc_mtd_s), GFP_KERNEL);
+	if (!mxc_nand_data) {
+		printk(KERN_ERR "%s: failed to allocate mtd_info\n",
+		       __FUNCTION__);
+		err = -ENOMEM;
+		goto out;
+	}
+
+	memset((char *)&g_nandfc_info, 0, sizeof(g_nandfc_info));
+
+	mxc_nand_data->dev = &pdev->dev;
+	/* structures must be linked */
+	this = &mxc_nand_data->nand;
+	mtd = &mxc_nand_data->mtd;
+	mtd->priv = this;
+	mtd->owner = THIS_MODULE;
+
+	this->priv = mxc_nand_data;
+	this->cmdfunc = mxc_nand_command;
+	this->select_chip = mxc_nand_select_chip;
+	this->read_byte = mxc_nand_read_byte;
+	this->read_word = mxc_nand_read_word;
+	this->write_buf = mxc_nand_write_buf;
+	this->read_buf = mxc_nand_read_buf;
+	this->verify_buf = mxc_nand_verify_buf;
+	this->scan_bbt = mxc_nand_scan_bbt;
+
+	/* NAND bus width determines access funtions used by upper layer */
+	if (flash->width == 2) {
+		this->read_byte = mxc_nand_read_byte16;
+		this->options |= NAND_BUSWIDTH_16;
+		NFC_SET_NFMS(1 << NFMS_NF_DWIDTH);
+	} else {
+		NFC_SET_NFMS(0);
+	}
+
+	nfc_clk = clk_get(&pdev->dev, "nfc_clk");
+	clk_enable(nfc_clk);
+
+	init_waitqueue_head(&irq_waitq);
+	err = request_irq(MXC_INT_NANDFC, mxc_nfc_irq, 0, "mxc_nd", NULL);
+	if (err) {
+		goto out_1;
+	}
+
+	if (hardware_ecc) {
+		this->ecc.read_page = mxc_nand_read_page;
+		this->ecc.write_page = mxc_nand_write_page;
+		this->ecc.read_oob = mxc_nand_read_oob;
+		this->ecc.layout = &nand_hw_eccoob_512;
+		this->ecc.calculate = mxc_nand_calculate_ecc;
+		this->ecc.hwctl = mxc_nand_enable_hwecc;
+		this->ecc.correct = mxc_nand_correct_data;
+		this->ecc.mode = NAND_ECC_HW;
+		this->ecc.size = 512;
+		this->ecc.bytes = 9;
+		raw_write((raw_read(REG_NFC_ECC_EN) | NFC_ECC_EN),
+			  REG_NFC_ECC_EN);
+	} else {
+		this->ecc.mode = NAND_ECC_SOFT;
+		raw_write((raw_read(REG_NFC_ECC_EN) & ~NFC_ECC_EN),
+			  REG_NFC_ECC_EN);
+	}
+
+	/* config the gpio */
+	if (flash->init)
+		flash->init();
+
+	/* Reset NAND */
+	this->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+
+	/* Scan to find existence of the device */
+	if (nand_scan(mtd, NFC_GET_MAXCHIP_SP())) {
+		DEBUG(MTD_DEBUG_LEVEL0,
+		      "MXC_ND2: Unable to find any NAND device.\n");
+		err = -ENXIO;
+		goto out_1;
+	}
+
+	/* Register the partitions */
+#ifdef CONFIG_MTD_PARTITIONS
+#if defined(CONFIG_MACH_CCWMX51JS) || defined(CONFIG_MACH_CCMX51JS) || \
+    defined(CONFIG_MACH_CCWMX51) || defined(CONFIG_MACH_CCMX51)
+	mtd->name= "onboard_boot";
+#endif
+	nr_parts =
+	    parse_mtd_partitions(mtd, part_probes, &mxc_nand_data->parts, 0);
+	if (nr_parts > 0)
+		add_mtd_partitions(mtd, mxc_nand_data->parts, nr_parts);
+	else if (flash->parts)
+		add_mtd_partitions(mtd, flash->parts, flash->nr_parts);
+	else
+#endif
+	{
+		pr_info("Registering %s as whole device\n", mtd->name);
+		add_mtd_device(mtd);
+	}
+
+	platform_set_drvdata(pdev, mtd);
+
+	return 0;
+
+      out_1:
+	kfree(mxc_nand_data);
+      out:
+	return err;
+
+}
+
+ /*!
+  * Dissociates the driver from the device.
+  *
+  * @param   pdev  the device structure used to give information on which
+  *
+  * @return  The function always returns 0.
+  */
+
+static int __exit mxcnd_remove(struct platform_device *pdev)
+{
+	struct mtd_info *mtd = platform_get_drvdata(pdev);
+	struct flash_platform_data *flash = pdev->dev.platform_data;
+
+	if (flash->exit)
+		flash->exit();
+
+	mxc_free_buf();
+
+	clk_disable(nfc_clk);
+	clk_put(nfc_clk);
+	platform_set_drvdata(pdev, NULL);
+
+	if (mxc_nand_data) {
+		nand_release(mtd);
+		free_irq(MXC_INT_NANDFC, NULL);
+		kfree(mxc_nand_data);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/*!
+ * This function is called to put the NAND in a low power state. Refer to the
+ * document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device information structure
+ *
+ * @param   state the power state the device is entering
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+
+static int mxcnd_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct mtd_info *info = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	DEBUG(MTD_DEBUG_LEVEL0, "MXC_ND2 : NAND suspend\n");
+	if (info)
+		ret = info->suspend(info);
+
+	/* Disable the NFC clock */
+	clk_disable(nfc_clk);
+
+	/* Disable the NFC clock */
+	clk_disable(nfc_clk);
+
+	return ret;
+}
+
+/*!
+ * This function is called to bring the NAND back from a low power state. Refer
+ * to the document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device information structure
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int mxcnd_resume(struct platform_device *pdev)
+{
+	struct mtd_info *info = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	DEBUG(MTD_DEBUG_LEVEL0, "MXC_ND2 : NAND resume\n");
+	/* Enable the NFC clock */
+	clk_enable(nfc_clk);
+
+	if (info) {
+		info->resume(info);
+	}
+
+	return ret;
+}
+
+#else
+#define mxcnd_suspend   NULL
+#define mxcnd_resume    NULL
+#endif				/* CONFIG_PM */
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcnd_driver = {
+	.driver = {
+		   .name = "mxc_nandv2_flash",
+		   },
+	.probe = mxcnd_probe,
+	.remove = __exit_p(mxcnd_remove),
+	.suspend = mxcnd_suspend,
+	.resume = mxcnd_resume,
+};
+
+/*!
+ * Main initialization routine
+ * @return  0 if successful; non-zero otherwise
+ */
+static int __init mxc_nd_init(void)
+{
+	/* Register the device driver structure. */
+	pr_info("MXC MTD nand Driver %s\n", DVR_VER);
+	if (platform_driver_register(&mxcnd_driver) != 0) {
+		printk(KERN_ERR "Driver register failed for mxcnd_driver\n");
+		return -ENODEV;
+	}
+	return 0;
+}
+
+/*!
+ * Clean up routine
+ */
+static void __exit mxc_nd_cleanup(void)
+{
+	/* Unregister the device structure */
+	platform_driver_unregister(&mxcnd_driver);
+}
+
+module_init(mxc_nd_init);
+module_exit(mxc_nd_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC NAND MTD driver Version 2-5");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/nand/mxc_nd2.h b/drivers/mtd/nand/mxc_nd2.h
new file mode 100644
index 000000000000..b89b9318f372
--- /dev/null
+++ b/drivers/mtd/nand/mxc_nd2.h
@@ -0,0 +1,688 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_nd2.h
+ *
+ * @brief This file contains the NAND Flash Controller register information.
+ *
+ *
+ * @ingroup NAND_MTD
+ */
+
+#ifndef __MXC_ND2_H__
+#define __MXC_ND2_H__
+
+#include <mach/hardware.h>
+
+#define IS_2K_PAGE_NAND         ((mtd->writesize / num_of_interleave) \
+						== NAND_PAGESIZE_2KB)
+#define IS_4K_PAGE_NAND         ((mtd->writesize / num_of_interleave) \
+						== NAND_PAGESIZE_4KB)
+#define IS_LARGE_PAGE_NAND      ((mtd->writesize / num_of_interleave) > 512)
+
+#define GET_NAND_OOB_SIZE	(mtd->oobsize / num_of_interleave)
+
+#define NAND_PAGESIZE_2KB	2048
+#define NAND_PAGESIZE_4KB	4096
+
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V3
+/*
+ * For V3 NFC registers Definition
+ */
+/* AXI Bus Mapped */
+#define NFC_AXI_BASE_ADDR		NFC_BASE_ADDR_AXI
+
+#if defined(CONFIG_ARCH_MXC_HAS_NFC_V3_1)	/* mx37 */
+#define MXC_INT_NANDFC			MXC_INT_EMI
+#define NFC_FLASH_ADDR_CMD		(nfc_axi_base + 0x1E00)
+#define NFC_CONFIG1			(nfc_axi_base + 0x1E04)
+#define NFC_ECC_STATUS_RESULT		(nfc_axi_base + 0x1E08)
+#define LAUNCH_NFC			(nfc_axi_base + 0x1E0c)
+#define NFC_WRPROT			(nfc_ip_base + 0x00)
+#define NFC_WRPROT_UNLOCK_BLK_ADD0	(nfc_ip_base + 0x04)
+#define NFC_CONFIG2			(nfc_ip_base + 0x14)
+#define NFC_IPC				(nfc_ip_base + 0x18)
+#elif defined(CONFIG_ARCH_MXC_HAS_NFC_V3_2)	/* mx51 */
+#define MXC_INT_NANDFC			MXC_INT_NFC
+#define NFC_AUTO_MODE_ENABLE
+#define NFC_FLASH_CMD			(nfc_axi_base + 0x1E00)
+#define NFC_FLASH_ADDR0      		(nfc_axi_base + 0x1E04)
+#define NFC_FLASH_ADDR8			(nfc_axi_base + 0x1E24)
+#define NFC_CONFIG1         		(nfc_axi_base + 0x1E34)
+#define NFC_ECC_STATUS_RESULT		(nfc_axi_base + 0x1E38)
+#define NFC_ECC_STATUS_SUM		(nfc_axi_base + 0x1E3C)
+#define LAUNCH_NFC			(nfc_axi_base + 0x1E40)
+#define NFC_WRPROT			(nfc_ip_base + 0x00)
+#define NFC_WRPROT_UNLOCK_BLK_ADD0	(nfc_ip_base + 0x04)
+#define NFC_CONFIG2			(nfc_ip_base + 0x24)
+#define NFC_CONFIG3			(nfc_ip_base + 0x28)
+#define NFC_IPC				(nfc_ip_base + 0x2C)
+#define NFC_DELAY_LINE			(nfc_ip_base + 0x34)
+#else				/* skye */
+#define NFC_FLASH_ADDR_CMD		(nfc_axi_base + 0xE00)
+#define NFC_CONFIG1			(nfc_axi_base + 0xE04)
+#define NFC_ECC_STATUS_RESULT		(nfc_axi_base + 0xE08)
+#define LAUNCH_NFC			(nfc_axi_base + 0xE0C)
+#define NFC_WRPROT			(nfc_ip_base + 0x00)
+#define NFC_WRPROT_UNLOCK_BLK_ADD0	(nfc_ip_base + 0x04)
+#define NFC_CONFIG2			(nfc_ip_base + 0x14)
+#define NFC_IPC				(nfc_ip_base + 0x18)
+#endif
+/*!
+ * Addresses for NFC RAM BUFFER Main area 0
+ */
+#define MAIN_AREA0        		((u16 *)(nfc_axi_base + 0x000))
+#define MAIN_AREA1        		((u16 *)(nfc_axi_base + 0x200))
+
+/*!
+ * Addresses for NFC SPARE BUFFER Spare area 0
+ */
+#if defined(CONFIG_ARCH_MXC_HAS_NFC_V3_1) ||	\
+    defined(CONFIG_ARCH_MXC_HAS_NFC_V3_2)
+#define SPARE_AREA0       		((u16 *)(nfc_axi_base + 0x1000))
+#define SPARE_LEN			64
+#define SPARE_COUNT			8
+#define SPARE_SIZE			(SPARE_LEN * SPARE_COUNT)
+#else
+#define SPARE_AREA0       		((u16 *)(nfc_axi_base + 0x800))
+#define SPARE_LEN			16
+#define SPARE_COUNT			4
+#define SPARE_SIZE			(SPARE_LEN * SPARE_COUNT)
+#endif
+
+#if defined(CONFIG_ARCH_MXC_HAS_NFC_V3_1) ||	\
+    defined(CONFIG_ARCH_MXC_HAS_NFC_V3_2)
+#define NFC_SPAS_WIDTH 8
+#define NFC_SPAS_SHIFT 16
+
+#define IS_4BIT_ECC \
+( \
+	cpu_is_mx51_rev(CHIP_REV_2_0) > 0 ? \
+		!((raw_read(NFC_CONFIG2) & NFC_ECC_MODE_4) >> 6) : \
+		((raw_read(NFC_CONFIG2) & NFC_ECC_MODE_4) >> 6) \
+)
+
+#define NFC_SET_SPAS(v)			\
+	raw_write((((raw_read(NFC_CONFIG2) & \
+	NFC_FIELD_RESET(NFC_SPAS_WIDTH, NFC_SPAS_SHIFT)) | ((v) << 16))), \
+	NFC_CONFIG2)
+
+#define NFC_SET_ECC_MODE(v)		\
+do { \
+	if (cpu_is_mx51_rev(CHIP_REV_2_0) > 0) { \
+		if ((v) == NFC_SPAS_218 || (v) == NFC_SPAS_112) \
+			raw_write(((raw_read(NFC_CONFIG2) & \
+					NFC_ECC_MODE_MASK) | \
+					NFC_ECC_MODE_4), NFC_CONFIG2); \
+		else \
+			raw_write(((raw_read(NFC_CONFIG2) & \
+					NFC_ECC_MODE_MASK) & \
+					NFC_ECC_MODE_8), NFC_CONFIG2); \
+	} else { \
+		if ((v) == NFC_SPAS_218 || (v) == NFC_SPAS_112) \
+			raw_write(((raw_read(NFC_CONFIG2) & \
+					NFC_ECC_MODE_MASK) & \
+					NFC_ECC_MODE_8), NFC_CONFIG2); \
+		else \
+			raw_write(((raw_read(NFC_CONFIG2) & \
+					NFC_ECC_MODE_MASK) | \
+					NFC_ECC_MODE_4), NFC_CONFIG2); \
+	} \
+} while (0)
+
+#define WRITE_NFC_IP_REG(val,reg) 			\
+	do {	 					\
+		raw_write(NFC_IPC_CREQ, NFC_IPC);	\
+		while (!((raw_read(NFC_IPC) & NFC_IPC_ACK)>>1));\
+		raw_write(val, reg);			\
+		raw_write(0, NFC_IPC);			\
+	} while(0)
+
+#else
+#define IS_4BIT_ECC			1
+#define NFC_SET_SPAS(v)
+#define NFC_SET_ECC_MODE(v)
+#define NFC_SET_NFMS(v)	(NFMS |= (v))
+
+#define WRITE_NFC_IP_REG(val,reg) \
+	raw_write((raw_read(REG_NFC_OPS_STAT) & ~NFC_OPS_STAT),  \
+	REG_NFC_OPS_STAT)
+#endif
+
+#define GET_NFC_ECC_STATUS() raw_read(REG_NFC_ECC_STATUS_RESULT);
+
+/*!
+ * Set 1 to specific operation bit, rest to 0 in LAUNCH_NFC Register for
+ * Specific operation
+ */
+#define NFC_CMD            		0x1
+#define NFC_ADDR           		0x2
+#define NFC_INPUT          		0x4
+#define NFC_OUTPUT         		0x8
+#define NFC_ID             		0x10
+#define NFC_STATUS         		0x20
+
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V3_2	/* mx51 */
+#define NFC_AUTO_PROG 			0x40
+#define NFC_AUTO_READ           	0x80
+#define NFC_AUTO_ERASE          	0x200
+#define NFC_COPY_BACK_0			0x400
+#define NFC_COPY_BACK_1         	0x800
+#define NFC_AUTO_STATE          	0x1000
+#endif
+
+/* Bit Definitions for NFC_IPC*/
+#define NFC_OPS_STAT			(1 << 31)
+
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V3_2	/* mx51 */
+#define NFC_OP_DONE			(1 << 30)
+#define NFC_RB				(1 << 28)
+#define NFC_PS_WIDTH 			2
+#define NFC_PS_SHIFT 			0
+#define NFC_PS_512	 		0
+#define NFC_PS_2K	 		1
+#define NFC_PS_4K    			2
+#else
+#define NFC_RB				(1 << 29)
+#endif
+
+#define NFC_ONE_CYCLE			(1 << 2)
+
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V3_2	/* mx51 */
+#define NFC_INT_MSK			(1 << 15)
+#define NFC_AUTO_PROG_DONE_MSK 		(1 << 14)
+#define NFC_NUM_ADDR_PHASE1_WIDTH   	2
+#define NFC_NUM_ADDR_PHASE1_SHIFT  	12
+
+#define NFC_NUM_ADDR_PHASE0_WIDTH 	1
+#define NFC_NUM_ADDR_PHASE0_SHIFT  	5
+
+#define NFC_ONE_LESS_PHASE1 		0
+#define NFC_TWO_LESS_PHASE1 		1
+
+#define NFC_FLASH_ADDR_SHIFT		0
+#else
+#define NFC_INT_MSK			(1 << 4)
+#define NFC_BIG				(1 << 5)
+#define NFC_FLASH_ADDR_SHIFT		16
+#endif
+
+#define NFC_UNLOCK_END_ADDR_SHIFT	16
+
+/* Bit definition for NFC_CONFIGRATION_1 */
+#define NFC_SP_EN			(1 << 0)
+#define NFC_CE				(1 << 1)
+#define NFC_RST				(1 << 2)
+#define NFC_ECC_EN			(1 << 3)
+
+#define NFC_FIELD_RESET(width, shift) ~(((1 << (width)) - 1) << (shift))
+
+#define NFC_RBA_SHIFT       		4
+
+#if defined(CONFIG_ARCH_MXC_HAS_NFC_V3_1) ||	\
+    defined(CONFIG_ARCH_MXC_HAS_NFC_V3_2)	/* mx51 */
+#define NFC_RBA_WIDTH			3
+#else
+#define NFC_RBA_WIDTH			2
+#endif
+
+#if defined(CONFIG_ARCH_MXC_HAS_NFC_V3_2)	/* mx51 */
+#define NFC_ITERATION_SHIFT 8
+#define NFC_ITERATION_WIDTH 4
+#define NFC_ACTIVE_CS_SHIFT 12
+#define NFC_ACTIVE_CS_WIDTH 3
+/* bit definition for CONFIGRATION3 */
+#define NFC_NO_SDMA			(1 << 20)
+#define NFC_FMP_SHIFT 			16
+#define NFC_FMP_WIDTH			4
+#define NFC_RBB_MODE			(1 << 15)
+#define NFC_NUM_OF_DEVICES_SHIFT 	12
+#define NFC_NUM_OF_DEVICES_WIDTH 	4
+#define NFC_DMA_MODE_SHIFT 		11
+#define NFC_DMA_MODE_WIDTH  		1
+#define NFC_SBB_SHIFT 			8
+#define NFC_SBB_WIDTH 			3
+#define NFC_BIG				(1 << 7)
+#define NFC_SB2R_SHIFT 			4
+#define NFC_SB2R_WIDTH			3
+#define NFC_FW_SHIFT    		3
+#define NFC_FW_WIDTH 			1
+#define NFC_TOO				(1 << 2)
+#define NFC_ADD_OP_SHIFT 		0
+#define NFC_ADD_OP_WIDTH		2
+#define NFC_FW_8 			1
+#define NFC_FW_16			0
+#define NFC_ST_CMD_SHITF		24
+#define NFC_ST_CMD_WIDTH		8
+#endif
+
+#define NFC_PPB_32			(0 << 7)
+#define NFC_PPB_64			(1 << 7)
+#define NFC_PPB_128			(2 << 7)
+#define NFC_PPB_256			(3 << 7)
+#define NFC_PPB_RESET			~(3 << 7)
+
+#if defined(CONFIG_ARCH_MXC_HAS_NFC_V3_2)
+#define NFC_BLS_LOCKED			(0 << 6)
+#define NFC_BLS_LOCKED_DEFAULT		(1 << 6)
+#define NFC_BLS_UNLCOKED		(2 << 6)
+#define NFC_BLS_RESET			(~(3 << 6))
+#else
+#define NFC_BLS_LOCKED                  (0 << 16)
+#define NFC_BLS_LOCKED_DEFAULT          (1 << 16)
+#define NFC_BLS_UNLCOKED                (2 << 16)
+#define NFC_BLS_RESET                   (~(3 << 16))
+#endif
+
+#define NFC_WPC_LOCK_TIGHT		1
+#define NFC_WPC_LOCK			(1 << 1)
+#define NFC_WPC_UNLOCK			(1 << 2)
+#define NFC_WPC_RESET			~(7)
+#if defined(CONFIG_ARCH_MXC_HAS_NFC_V3_1) || \
+    defined(CONFIG_ARCH_MXC_HAS_NFC_V3_2)
+#define NFC_ECC_MODE_4    		(1 << 6)
+#define NFC_ECC_MODE_8			~(1 << 6)
+#define NFC_ECC_MODE_MASK 		~(1 << 6)
+#define NFC_SPAS_16			8
+#define NFC_SPAS_64		 	32
+#define NFC_SPAS_128			64
+#define NFC_SPAS_112			56
+#define NFC_SPAS_218		 	109
+#define NFC_IPC_CREQ			(1 << 0)
+#define NFC_IPC_ACK			(1 << 1)
+#endif
+
+#define REG_NFC_OPS_STAT		NFC_IPC
+#define REG_NFC_INTRRUPT		NFC_CONFIG2
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V3_2
+#define REG_NFC_FLASH_ADDR		NFC_FLASH_ADDR0
+#define REG_NFC_FLASH_CMD		NFC_FLASH_CMD
+#else
+#define REG_NFC_FLASH_ADDR		NFC_FLASH_ADDR_CMD
+#define REG_NFC_FLASH_CMD		NFC_FLASH_ADDR_CMD
+#endif
+#define REG_NFC_OPS			LAUNCH_NFC
+#define REG_NFC_SET_RBA			NFC_CONFIG1
+#define REG_NFC_RB			NFC_IPC
+#define REG_NFC_ECC_EN			NFC_CONFIG2
+#define REG_NFC_ECC_STATUS_RESULT	NFC_ECC_STATUS_RESULT
+#define REG_NFC_CE			NFC_CONFIG1
+#define REG_NFC_RST			NFC_CONFIG1
+#define REG_NFC_PPB			NFC_CONFIG2
+#define REG_NFC_SP_EN			NFC_CONFIG1
+#define REG_NFC_BLS			NFC_WRPROT
+#define REG_UNLOCK_BLK_ADD0		NFC_WRPROT_UNLOCK_BLK_ADD0
+#define REG_UNLOCK_BLK_ADD1		NFC_WRPROT_UNLOCK_BLK_ADD1
+#define REG_UNLOCK_BLK_ADD2		NFC_WRPROT_UNLOCK_BLK_ADD2
+#define REG_UNLOCK_BLK_ADD3		NFC_WRPROT_UNLOCK_BLK_ADD3
+#define REG_NFC_WPC			NFC_WRPROT
+#define REG_NFC_ONE_CYCLE		NFC_CONFIG2
+
+/* NFC V3 Specific MACRO functions definitions */
+#define raw_write(v,a)		__raw_writel(v,a)
+#define raw_read(a)		__raw_readl(a)
+
+/* Explcit ack ops status (if any), before issue of any command  */
+#define ACK_OPS	\
+	raw_write((raw_read(REG_NFC_OPS_STAT) & ~NFC_OPS_STAT), \
+	REG_NFC_OPS_STAT);
+
+/* Set RBA buffer id*/
+#define NFC_SET_RBA(val)       \
+	raw_write((raw_read(REG_NFC_SET_RBA) & \
+	(NFC_FIELD_RESET(NFC_RBA_WIDTH, NFC_RBA_SHIFT))) | \
+	((val) << NFC_RBA_SHIFT), REG_NFC_SET_RBA);
+
+#define NFC_SET_PS(val)       \
+	raw_write((raw_read(NFC_CONFIG2) & \
+	(NFC_FIELD_RESET(NFC_PS_WIDTH, NFC_PS_SHIFT))) | \
+	((val) << NFC_PS_SHIFT), NFC_CONFIG2);
+
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V3_2
+#define UNLOCK_ADDR(start_addr,end_addr)     \
+{ \
+	int i = 0; \
+	for (; i < NAND_MAX_CHIPS; i++)  \
+	raw_write(start_addr | \
+	(end_addr << NFC_UNLOCK_END_ADDR_SHIFT), \
+	REG_UNLOCK_BLK_ADD0 + (i << 2)); \
+}
+#define NFC_SET_NFC_ACTIVE_CS(val) \
+	raw_write((raw_read(NFC_CONFIG1) & \
+	(NFC_FIELD_RESET(NFC_ACTIVE_CS_WIDTH, NFC_ACTIVE_CS_SHIFT))) | \
+	((val) << NFC_ACTIVE_CS_SHIFT), NFC_CONFIG1);
+
+#define NFC_GET_MAXCHIP_SP() 		8
+
+#else
+#define UNLOCK_ADDR(start_addr,end_addr)     \
+	raw_write(start_addr | \
+	(end_addr << NFC_UNLOCK_END_ADDR_SHIFT), REG_UNLOCK_BLK_ADD0);
+
+#define NFC_SET_NFC_ACTIVE_CS(val)
+#define NFC_GET_MAXCHIP_SP() 		1
+#endif
+
+#define NFC_SET_BLS(val) ((raw_read(REG_NFC_BLS) & NFC_BLS_RESET) | val )
+#define NFC_SET_WPC(val) ((raw_read(REG_NFC_WPC) & NFC_WPC_RESET) | val )
+#define CHECK_NFC_RB    raw_read(REG_NFC_RB) & NFC_RB
+
+#if defined(CONFIG_ARCH_MXC_HAS_NFC_V3_2)
+#define NFC_SET_NFC_NUM_ADDR_PHASE1(val) \
+	raw_write((raw_read(NFC_CONFIG2) & \
+	(NFC_FIELD_RESET(NFC_NUM_ADDR_PHASE1_WIDTH, \
+	NFC_NUM_ADDR_PHASE1_SHIFT))) | \
+	((val) << NFC_NUM_ADDR_PHASE1_SHIFT), NFC_CONFIG2);
+
+#define NFC_SET_NFC_NUM_ADDR_PHASE0(val) \
+	raw_write((raw_read(NFC_CONFIG2) & \
+	(NFC_FIELD_RESET(NFC_NUM_ADDR_PHASE0_WIDTH, \
+	NFC_NUM_ADDR_PHASE0_SHIFT))) | \
+	((val) << NFC_NUM_ADDR_PHASE0_SHIFT), NFC_CONFIG2);
+
+#define NFC_SET_NFC_ITERATION(val) \
+	raw_write((raw_read(NFC_CONFIG1) & \
+	(NFC_FIELD_RESET(NFC_ITERATION_WIDTH, NFC_ITERATION_SHIFT))) | \
+	((val) << NFC_ITERATION_SHIFT), NFC_CONFIG1);
+
+#define NFC_SET_FW(val) \
+	raw_write((raw_read(NFC_CONFIG3) & \
+	(NFC_FIELD_RESET(NFC_FW_WIDTH, NFC_FW_SHIFT))) | \
+	((val) << NFC_FW_SHIFT), NFC_CONFIG3);
+
+#define NFC_SET_NUM_OF_DEVICE(val) \
+	raw_write((raw_read(NFC_CONFIG3) & \
+	(NFC_FIELD_RESET(NFC_NUM_OF_DEVICES_WIDTH, \
+	NFC_NUM_OF_DEVICES_SHIFT))) | \
+	((val) << NFC_NUM_OF_DEVICES_SHIFT), NFC_CONFIG3);
+
+#define NFC_SET_ADD_OP_MODE(val) \
+	 raw_write((raw_read(NFC_CONFIG3) & \
+	(NFC_FIELD_RESET(NFC_ADD_OP_WIDTH, NFC_ADD_OP_SHIFT))) | \
+	((val) << NFC_ADD_OP_SHIFT), NFC_CONFIG3);
+
+#define NFC_SET_ADD_CS_MODE(val) \
+{ \
+	NFC_SET_ADD_OP_MODE(val); \
+	NFC_SET_NUM_OF_DEVICE(this->numchips - 1); \
+}
+
+#define NFC_SET_ST_CMD(val) \
+	raw_write((raw_read(NFC_CONFIG2) & \
+	(NFC_FIELD_RESET(NFC_ST_CMD_WIDTH, \
+	NFC_ST_CMD_SHITF))) | \
+	((val) << NFC_ST_CMD_SHITF), NFC_CONFIG2);
+
+#define NFMS_NF_DWIDTH 0
+#define NFMS_NF_PG_SZ  1
+#define NFC_CMD_1_SHIFT 8
+
+#define NUM_OF_ADDR_CYCLE (fls(g_page_mask) >> 3)
+#define SET_NFC_DELAY_LINE(val) raw_write((val), NFC_DELAY_LINE)
+
+/*should set the fw,ps,spas,ppb*/
+#define NFC_SET_NFMS(v)	\
+do {	\
+	if (!(v)) \
+		NFC_SET_FW(NFC_FW_8);	\
+	if (((v) & (1 << NFMS_NF_DWIDTH)))	\
+		NFC_SET_FW(NFC_FW_16);	\
+	if (((v) & (1 << NFMS_NF_PG_SZ))) {	\
+		if (IS_2K_PAGE_NAND) {	\
+			NFC_SET_PS(NFC_PS_2K);	\
+			NFC_SET_NFC_NUM_ADDR_PHASE1(NUM_OF_ADDR_CYCLE); \
+			NFC_SET_NFC_NUM_ADDR_PHASE0(NFC_TWO_LESS_PHASE1); \
+		} else if (IS_4K_PAGE_NAND) {       \
+			NFC_SET_PS(NFC_PS_4K);	\
+			NFC_SET_NFC_NUM_ADDR_PHASE1(NUM_OF_ADDR_CYCLE); \
+			NFC_SET_NFC_NUM_ADDR_PHASE0(NFC_TWO_LESS_PHASE1); \
+		} else {	\
+			NFC_SET_PS(NFC_PS_512);	\
+			NFC_SET_NFC_NUM_ADDR_PHASE1(NUM_OF_ADDR_CYCLE - 1); \
+			NFC_SET_NFC_NUM_ADDR_PHASE0(NFC_ONE_LESS_PHASE1); \
+		}	\
+		NFC_SET_ADD_CS_MODE(1); \
+		NFC_SET_SPAS(GET_NAND_OOB_SIZE >> 1);	\
+		NFC_SET_ECC_MODE(GET_NAND_OOB_SIZE >> 1); \
+		NFC_SET_ST_CMD(0x70); \
+		raw_write(raw_read(NFC_CONFIG3) | NFC_NO_SDMA, NFC_CONFIG3); \
+		raw_write(raw_read(NFC_CONFIG3) | NFC_RBB_MODE, NFC_CONFIG3); \
+		SET_NFC_DELAY_LINE(0); \
+	} \
+} while (0)
+#endif
+
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V3_1
+#define NFC_SET_NFMS(v)
+#endif
+
+#define READ_PAGE()	send_read_page(0)
+#define PROG_PAGE() 	send_prog_page(0)
+
+#elif CONFIG_ARCH_MXC_HAS_NFC_V2
+
+/*
+ * For V1/V2 NFC registers Definition
+ */
+
+#define NFC_AXI_BASE_ADDR      		0x00
+/*
+ * Addresses for NFC registers
+ */
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V2_1
+#define NFC_REG_BASE			(nfc_ip_base + 0x1000)
+#else
+#define NFC_REG_BASE			nfc_ip_base
+#endif
+#define NFC_BUF_SIZE            	(NFC_REG_BASE + 0xE00)
+#define NFC_BUF_ADDR            	(NFC_REG_BASE + 0xE04)
+#define NFC_FLASH_ADDR          	(NFC_REG_BASE + 0xE06)
+#define NFC_FLASH_CMD           	(NFC_REG_BASE + 0xE08)
+#define NFC_CONFIG              	(NFC_REG_BASE + 0xE0A)
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V2_1
+#define NFC_ECC_STATUS_RESULT		(NFC_REG_BASE + 0xE0C)
+#define NFC_ECC_STATUS_RESULT_1		(NFC_REG_BASE + 0xE0C)
+#define NFC_ECC_STATUS_RESULT_2		(NFC_REG_BASE + 0xE0E)
+#define NFC_SPAS			(NFC_REG_BASE + 0xE10)
+#else
+#define NFC_ECC_STATUS_RESULT   	(NFC_REG_BASE + 0xE0C)
+#define NFC_RSLTMAIN_AREA       	(NFC_REG_BASE + 0xE0E)
+#define NFC_RSLTSPARE_AREA      	(NFC_REG_BASE + 0xE10)
+#endif
+#define NFC_WRPROT              	(NFC_REG_BASE + 0xE12)
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V2_1
+#define NFC_UNLOCKSTART_BLKADDR  	(NFC_REG_BASE + 0xE20)
+#define NFC_UNLOCKEND_BLKADDR    	(NFC_REG_BASE + 0xE22)
+#define NFC_UNLOCKSTART_BLKADDR1 	(NFC_REG_BASE + 0xE24)
+#define NFC_UNLOCKEND_BLKADDR1   	(NFC_REG_BASE + 0xE26)
+#define NFC_UNLOCKSTART_BLKADDR2 	(NFC_REG_BASE + 0xE28)
+#define NFC_UNLOCKEND_BLKADDR2   	(NFC_REG_BASE + 0xE2A)
+#define NFC_UNLOCKSTART_BLKADDR3 	(NFC_REG_BASE + 0xE2C)
+#define NFC_UNLOCKEND_BLKADDR3   	(NFC_REG_BASE + 0xE2E)
+#else
+#define NFC_UNLOCKSTART_BLKADDR  	(NFC_REG_BASE + 0xE14)
+#define NFC_UNLOCKEND_BLKADDR    	(NFC_REG_BASE + 0xE16)
+#endif
+#define NFC_NF_WRPRST            	(NFC_REG_BASE + 0xE18)
+#define NFC_CONFIG1              	(NFC_REG_BASE + 0xE1A)
+#define NFC_CONFIG2              	(NFC_REG_BASE + 0xE1C)
+
+/*!
+ * Addresses for NFC RAM BUFFER Main area 0
+ */
+#define MAIN_AREA0      		(u16 *)(nfc_ip_base + 0x000)
+#define MAIN_AREA1      		(u16 *)(nfc_ip_base + 0x200)
+
+/*!
+ * Addresses for NFC SPARE BUFFER Spare area 0
+ */
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V2_1
+#define SPARE_AREA0     		(u16 *)(nfc_ip_base + 0x1000)
+#define SPARE_LEN			64
+#define SPARE_COUNT			8
+#else
+#define SPARE_AREA0     		(u16 *)(nfc_ip_base + 0x800)
+#define SPARE_LEN       		16
+#define SPARE_COUNT     		4
+#endif
+#define SPARE_SIZE      		(SPARE_LEN * SPARE_COUNT)
+
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V2_1
+#define REG_NFC_ECC_MODE NFC_CONFIG1
+#define SPAS_SHIFT		(0)
+#define REG_NFC_SPAS NFC_SPAS
+#define SPAS_MASK	(0xFF00)
+#define IS_4BIT_ECC			\
+	((raw_read(REG_NFC_ECC_MODE) & NFC_ECC_MODE_4) >> 0)
+
+#define NFC_SET_SPAS(v)			\
+	raw_write(((raw_read(REG_NFC_SPAS) & SPAS_MASK) | ((v<<SPAS_SHIFT))), \
+								REG_NFC_SPAS)
+
+#define NFC_SET_ECC_MODE(v) 		 \
+do {	\
+	if ((v) == NFC_SPAS_218 || (v) == NFC_SPAS_112)  {	\
+		raw_write((raw_read(REG_NFC_ECC_MODE) & NFC_ECC_MODE_8), \
+							REG_NFC_ECC_MODE); \
+	} else {	\
+		raw_write((raw_read(REG_NFC_ECC_MODE) | NFC_ECC_MODE_4), \
+							REG_NFC_ECC_MODE); \
+	}	\
+} while (0)
+
+#define GET_ECC_STATUS()  __raw_readl(REG_NFC_ECC_STATUS_RESULT);
+#define NFC_SET_NFMS(v)	\
+do {	\
+	(NFMS |= (v));	\
+	if (((v) & (1 << NFMS_NF_PG_SZ))) {	\
+		NFC_SET_SPAS(GET_NAND_OOB_SIZE >> 1);	\
+		NFC_SET_ECC_MODE(GET_NAND_OOB_SIZE >> 1); \
+	} \
+} while (0)
+#else
+#define IS_4BIT_ECC			(1)
+#define NFC_SET_SPAS(v)
+#define NFC_SET_ECC_MODE(v)
+#define GET_ECC_STATUS()  raw_read(REG_NFC_ECC_STATUS_RESULT);
+#define NFC_SET_NFMS(v)     (NFMS |= (v))
+#endif
+
+#define WRITE_NFC_IP_REG(val,reg) \
+	raw_write((raw_read(REG_NFC_OPS_STAT) & ~NFC_OPS_STAT),  \
+	REG_NFC_OPS_STAT)
+
+#define GET_NFC_ECC_STATUS() raw_read(REG_NFC_ECC_STATUS_RESULT);
+
+/*!
+ * Set INT to 0, Set 1 to specific operation bit, rest to 0 in LAUNCH_NFC Register for
+ * Specific operation
+ */
+#define NFC_CMD            		0x1
+#define NFC_ADDR           		0x2
+#define NFC_INPUT          		0x4
+#define NFC_OUTPUT         		0x8
+#define NFC_ID             		0x10
+#define NFC_STATUS         		0x20
+
+/* Bit Definitions */
+#define NFC_OPS_STAT			(1 << 15)
+#define NFC_SP_EN           		(1 << 2)
+#define NFC_ECC_EN          		(1 << 3)
+#define NFC_INT_MSK         		(1 << 4)
+#define NFC_BIG             		(1 << 5)
+#define NFC_RST             		(1 << 6)
+#define NFC_CE              		(1 << 7)
+#define NFC_ONE_CYCLE       		(1 << 8)
+#define NFC_BLS_LOCKED			0
+#define NFC_BLS_LOCKED_DEFAULT		1
+#define NFC_BLS_UNLCOKED		2
+#define NFC_WPC_LOCK_TIGHT		1
+#define NFC_WPC_LOCK			(1 << 1)
+#define NFC_WPC_UNLOCK			(1 << 2)
+#define NFC_FLASH_ADDR_SHIFT 		0
+#define NFC_UNLOCK_END_ADDR_SHIFT	0
+
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V2_1
+#define NFC_ECC_MODE_4    		 (1<<0)
+#define NFC_ECC_MODE_8			 ~(1<<0)
+#define NFC_SPAS_16			 8
+#define NFC_SPAS_64			 32
+#define NFC_SPAS_112			 56
+#define NFC_SPAS_128			 64
+#define NFC_SPAS_218			 109
+#endif
+/* NFC Register Mapping */
+#define REG_NFC_OPS_STAT		NFC_CONFIG2
+#define REG_NFC_INTRRUPT		NFC_CONFIG1
+#define REG_NFC_FLASH_ADDR		NFC_FLASH_ADDR
+#define REG_NFC_FLASH_CMD		NFC_FLASH_CMD
+#define REG_NFC_OPS			NFC_CONFIG2
+#define REG_NFC_SET_RBA			NFC_BUF_ADDR
+#define REG_NFC_ECC_EN			NFC_CONFIG1
+#define REG_NFC_ECC_STATUS_RESULT  	NFC_ECC_STATUS_RESULT
+#define REG_NFC_CE			NFC_CONFIG1
+#define REG_NFC_SP_EN			NFC_CONFIG1
+#define REG_NFC_BLS			NFC_CONFIG
+#define REG_NFC_WPC			NFC_WRPROT
+#define REG_START_BLKADDR      		NFC_UNLOCKSTART_BLKADDR
+#define REG_END_BLKADDR        		NFC_UNLOCKEND_BLKADDR
+#define REG_NFC_RST			NFC_CONFIG1
+#define REG_NFC_ONE_CYCLE		NFC_CONFIG1
+
+/* NFC V1/V2 Specific MACRO functions definitions */
+
+#define raw_write(v,a)                  __raw_writew(v,a)
+#define raw_read(a)                     __raw_readw(a)
+
+#define NFC_SET_BLS(val)  		val
+
+#define UNLOCK_ADDR(start_addr,end_addr)		\
+{							\
+	raw_write(start_addr,REG_START_BLKADDR);	\
+        raw_write(end_addr,REG_END_BLKADDR);		\
+}
+
+#define NFC_SET_NFC_ACTIVE_CS(val)
+#define NFC_GET_MAXCHIP_SP() 		1
+#define NFC_SET_WPC(val)                val
+
+/* NULL Definitions */
+#define ACK_OPS
+#define NFC_SET_RBA(val) raw_write(val, REG_NFC_SET_RBA);
+
+#ifdef CONFIG_ARCH_MXC_HAS_NFC_V2_1
+#define READ_PAGE()	send_read_page(0)
+#define PROG_PAGE() 	send_prog_page(0)
+#else
+#define READ_PAGE()   \
+do {                     \
+	send_read_page(0);  \
+	send_read_page(1);  \
+	send_read_page(2);  \
+	send_read_page(3);  \
+} while (0)
+
+#define PROG_PAGE()   \
+do {                     \
+	send_prog_page(0);  \
+	send_prog_page(1);  \
+	send_prog_page(2);  \
+	send_prog_page(3);  \
+} while (0)
+#endif
+#define CHECK_NFC_RB            1
+
+#endif
+
+#endif				/* __MXC_ND2_H__ */
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 0a9c9cd33f96..338d7b61426a 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -1033,7 +1033,7 @@ static uint8_t *nand_transfer_oob(struct nand_chip *chip, uint8_t *oob,
  *
  * Internal function. Called with chip held.
  */
-static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
+int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 			    struct mtd_oob_ops *ops)
 {
 	int chipnr, page, realpage, col, bytes, aligned;
@@ -1161,6 +1161,7 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 
 	return  mtd->ecc_stats.corrected - stats.corrected ? -EUCLEAN : 0;
 }
+EXPORT_SYMBOL(nand_do_read_ops);
 
 /**
  * nand_read - [MTD Interface] MTD compability function for nand_do_read_ecc
@@ -1703,7 +1704,7 @@ static uint8_t *nand_fill_oob(struct nand_chip *chip, uint8_t *oob,
  *
  * NAND write with ECC
  */
-static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
+int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 			     struct mtd_oob_ops *ops)
 {
 	int chipnr, realpage, page, blockmask, column;
@@ -1795,6 +1796,7 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 		ops->oobretlen = ops->ooblen;
 	return ret;
 }
+EXPORT_SYMBOL(nand_do_write_ops);
 
 /**
  * nand_write - [MTD Interface] NAND write with ECC
@@ -2181,6 +2183,7 @@ int nand_erase_nand(struct mtd_info *mtd, struct erase_info *instr,
 	/* Return more or less happy */
 	return ret;
 }
+EXPORT_SYMBOL_GPL(nand_erase_nand);
 
 /**
  * nand_sync - [MTD Interface] sync
@@ -2378,7 +2381,8 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 		mtd->writesize = 1024 << (extid & 0x3);
 		extid >>= 2;
 		/* Calc oobsize */
-		mtd->oobsize = (8 << (extid & 0x01)) * (mtd->writesize >> 9);
+		mtd->oobsize = (*maf_id == 0x2c && dev_id == 0xd5) ?
+			218 : (8 << (extid & 0x01)) * (mtd->writesize >> 9);
 		extid >>= 2;
 		/* Calc blocksize. Blocksize is multiples of 64KiB */
 		mtd->erasesize = (64 * 1024) << (extid & 0x03);
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index 69ee2c90eb0b..74f49ed0afa8 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -109,6 +109,9 @@ struct nand_flash_dev nand_flash_ids[] = {
 	{"NAND 2GiB 1,8V 16-bit",	0xB5, 0, 2048, 0, LP_OPTIONS16},
 	{"NAND 2GiB 3,3V 16-bit",	0xC5, 0, 2048, 0, LP_OPTIONS16},
 
+	/* 32 Gigabit ,only use 2G due to the linux mtd limitation*/
+	{"NAND 4GiB 3,3V 8-bit",	0xD7, 0, 2048, 0, LP_OPTIONS},
+
 	/*
 	 * Renesas AND 1 Gigabit. Those chips do not support extended id and
 	 * have a strange page/block layout !  The chosen minimum erasesize is
diff --git a/drivers/mtd/nand/s3c2410.c b/drivers/mtd/nand/s3c2410.c
index 556139ed1fdf..e910b5f0d780 100644
--- a/drivers/mtd/nand/s3c2410.c
+++ b/drivers/mtd/nand/s3c2410.c
@@ -1,10 +1,26 @@
 /* linux/drivers/mtd/nand/s3c2410.c
  *
- * Copyright © 2004-2008 Simtec Electronics
- *	http://armlinux.simtec.co.uk/
+ * Copyright (c) 2004,2005 Simtec Electronics
+ *	http://www.simtec.co.uk/products/SWLINUX/
  *	Ben Dooks <ben@simtec.co.uk>
  *
- * Samsung S3C2410/S3C2440/S3C2412 NAND driver
+ * Samsung S3C2410/S3C240 NAND driver
+ *
+ * Changelog:
+ *	21-Sep-2004  BJD  Initial version
+ *	23-Sep-2004  BJD  Multiple device support
+ *	28-Sep-2004  BJD  Fixed ECC placement for Hardware mode
+ *	12-Oct-2004  BJD  Fixed errors in use of platform data
+ *	18-Feb-2005  BJD  Fix sparse errors
+ *	14-Mar-2005  BJD  Applied tglx's code reduction patch
+ *	02-May-2005  BJD  Fixed s3c2440 support
+ *	02-May-2005  BJD  Reduced hwcontrol decode
+ *	20-Jun-2005  BJD  Updated s3c2440 support, fixed timing bug
+ *	08-Jul-2005  BJD  Fix OOPS when no platform data supplied
+ *	20-Oct-2005  BJD  Fix timing calculation bug
+ *	14-Jan-2006  BJD  Allow clock to be stopped when idle
+ *
+ * $Id: s3c2410.c,v 1.23 2006/04/01 18:06:29 bjd Exp $
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -19,11 +35,7 @@
  * 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
-*/
-
-#ifdef CONFIG_MTD_NAND_S3C2410_DEBUG
-#define DEBUG
-#endif
+ */
 
 #include <linux/module.h>
 #include <linux/types.h>
@@ -36,7 +48,6 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
-#include <linux/cpufreq.h>
 
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
@@ -48,6 +59,14 @@
 #include <asm/plat-s3c/regs-nand.h>
 #include <asm/plat-s3c/nand.h>
 
+/* Name of the kernel boot parameter with the partitions table */
+#define PARTITON_PARAMETER_NAME "onboard_boot"
+
+#if defined(CONFIG_MTD_NAND_S3C2410_DEBUG) && !defined(DEBUG)
+#define DEBUG
+#endif
+
+
 #ifdef CONFIG_MTD_NAND_S3C2410_HWECC
 static int hardware_ecc = 1;
 #else
@@ -60,14 +79,34 @@ static int clock_stop = 1;
 static const int clock_stop = 0;
 #endif
 
-
-/* new oob placement block for use with hardware ecc generation
+/*
+ * These are the values that we are using in the U-Boot too.
+ * Luis Galdos
  */
-
 static struct nand_ecclayout nand_hw_eccoob = {
-	.eccbytes = 3,
-	.eccpos = {0, 1, 2},
-	.oobfree = {{8, 8}}
+	.eccbytes = 4,
+	.eccpos = { 40, 41, 42, 43 },
+	.oobfree = {
+		{
+			.offset = 2,
+			.length = 38
+		}
+	}
+};
+
+/*
+ * Nand flash oob definition for SLC 512b page size by jsgood
+ * IMPORTANT: These values are coming from the U-Boot
+ */
+static struct nand_ecclayout nand_hw_eccoob_16 = {
+        .eccbytes = 6,
+        .eccpos = { 0, 1, 2, 3, 6, 7 },
+        .oobfree = {
+                {
+			.offset = 8,
+			.length = 8
+		}
+	}
 };
 
 /* controller and mtd information */
@@ -105,13 +144,8 @@ struct s3c2410_nand_info {
 	int				sel_bit;
 	int				mtd_count;
 	unsigned long			save_sel;
-	unsigned long			clk_rate;
 
 	enum s3c_cpu_type		cpu_type;
-
-#ifdef CONFIG_CPU_FREQ
-	struct notifier_block	freq_transition;
-#endif
 };
 
 /* conversion functions */
@@ -169,18 +203,17 @@ static int s3c_nand_calc_rate(int wanted, unsigned long clk, int max)
 
 /* controller setup */
 
-static int s3c2410_nand_setrate(struct s3c2410_nand_info *info)
+static int s3c2410_nand_inithw(struct s3c2410_nand_info *info,
+			       struct platform_device *pdev)
 {
-	struct s3c2410_platform_nand *plat = info->platform;
+	struct s3c2410_platform_nand *plat = to_nand_plat(pdev);
+	unsigned long clkrate = clk_get_rate(info->clk);
 	int tacls_max = (info->cpu_type == TYPE_S3C2412) ? 8 : 4;
 	int tacls, twrph0, twrph1;
-	unsigned long clkrate = clk_get_rate(info->clk);
-	unsigned long set, cfg, mask;
-	unsigned long flags;
+	unsigned long cfg = 0;
 
 	/* calculate the timing information for the controller */
 
-	info->clk_rate = clkrate;
 	clkrate /= 1000;	/* turn clock into kHz for ease of use */
 
 	if (plat != NULL) {
@@ -200,71 +233,39 @@ static int s3c2410_nand_setrate(struct s3c2410_nand_info *info)
 	}
 
 	dev_info(info->device, "Tacls=%d, %dns Twrph0=%d %dns, Twrph1=%d %dns\n",
-	       tacls, to_ns(tacls, clkrate), twrph0, to_ns(twrph0, clkrate), twrph1, to_ns(twrph1, clkrate));
-
-	switch (info->cpu_type) {
-	case TYPE_S3C2410:
-		mask = (S3C2410_NFCONF_TACLS(3) |
-			S3C2410_NFCONF_TWRPH0(7) |
-			S3C2410_NFCONF_TWRPH1(7));
-		set = S3C2410_NFCONF_EN;
-		set |= S3C2410_NFCONF_TACLS(tacls - 1);
-		set |= S3C2410_NFCONF_TWRPH0(twrph0 - 1);
-		set |= S3C2410_NFCONF_TWRPH1(twrph1 - 1);
-		break;
-
-	case TYPE_S3C2440:
-	case TYPE_S3C2412:
-		mask = (S3C2410_NFCONF_TACLS(tacls_max - 1) |
-			S3C2410_NFCONF_TWRPH0(7) |
-			S3C2410_NFCONF_TWRPH1(7));
-
-		set = S3C2440_NFCONF_TACLS(tacls - 1);
-		set |= S3C2440_NFCONF_TWRPH0(twrph0 - 1);
-		set |= S3C2440_NFCONF_TWRPH1(twrph1 - 1);
-		break;
-
-	default:
-		/* keep compiler happy */
-		mask = 0;
-		set = 0;
-		BUG();
-	}
-
-	dev_dbg(info->device, "NF_CONF is 0x%lx\n", cfg);
-
-	local_irq_save(flags);
-
+		 tacls, to_ns(tacls, clkrate), twrph0, to_ns(twrph0, clkrate), twrph1, to_ns(twrph1, clkrate));
+
+	
+	/*
+	 * First get the current value of the configuration register, otherwise
+	 * some chip-information that is coming from the bootloader will be
+	 * gone (e.g. the page size, which is required for the HWECC)
+	 * Luis Galdos
+	 */
 	cfg = readl(info->regs + S3C2410_NFCONF);
-	cfg &= ~mask;
-	cfg |= set;
-	writel(cfg, info->regs + S3C2410_NFCONF);
-
-	local_irq_restore(flags);
-
-	return 0;
-}
-
-static int s3c2410_nand_inithw(struct s3c2410_nand_info *info)
-{
-	int ret;
-
-	ret = s3c2410_nand_setrate(info);
-	if (ret < 0)
-		return ret;
-
+	
  	switch (info->cpu_type) {
  	case TYPE_S3C2410:
-	default:
+		cfg |= S3C2410_NFCONF_EN;
+		cfg |= S3C2410_NFCONF_TACLS(tacls - 1);
+		cfg |= S3C2410_NFCONF_TWRPH0(twrph0 - 1);
+		cfg |= S3C2410_NFCONF_TWRPH1(twrph1 - 1);
 		break;
 
  	case TYPE_S3C2440:
  	case TYPE_S3C2412:
+		cfg |= S3C2440_NFCONF_TACLS(tacls - 1);
+		cfg |= S3C2440_NFCONF_TWRPH0(twrph0 - 1);
+		cfg |= S3C2440_NFCONF_TWRPH1(twrph1 - 1);
+
 		/* enable the controller and de-assert nFCE */
 
 		writel(S3C2440_NFCONT_ENABLE, info->regs + S3C2440_NFCONT);
 	}
 
+	dev_dbg(info->device, "NF_CONF is 0x%lx\n", cfg);
+
+	writel(cfg, info->regs + S3C2410_NFCONF);
 	return 0;
 }
 
@@ -310,7 +311,7 @@ static void s3c2410_nand_select_chip(struct mtd_info *mtd, int chip)
 /* s3c2410_nand_hwcontrol
  *
  * Issue command and address cycles to the chip
-*/
+ */
 
 static void s3c2410_nand_hwcontrol(struct mtd_info *mtd, int cmd,
 				   unsigned int ctrl)
@@ -345,7 +346,7 @@ static void s3c2440_nand_hwcontrol(struct mtd_info *mtd, int cmd,
 /* s3c2410_nand_devready()
  *
  * returns 0 if the nand is busy, 1 if it is ready
-*/
+ */
 
 static int s3c2410_nand_devready(struct mtd_info *mtd)
 {
@@ -371,81 +372,80 @@ static int s3c2410_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 				     u_char *read_ecc, u_char *calc_ecc)
 {
 	struct s3c2410_nand_info *info = s3c2410_nand_mtd_toinfo(mtd);
-	unsigned int diff0, diff1, diff2;
-	unsigned int bit, byte;
-
-	pr_debug("%s(%p,%p,%p,%p)\n", __func__, mtd, dat, read_ecc, calc_ecc);
-
-	diff0 = read_ecc[0] ^ calc_ecc[0];
-	diff1 = read_ecc[1] ^ calc_ecc[1];
-	diff2 = read_ecc[2] ^ calc_ecc[2];
-
-	pr_debug("%s: rd %02x%02x%02x calc %02x%02x%02x diff %02x%02x%02x\n",
-		 __func__,
-		 read_ecc[0], read_ecc[1], read_ecc[2],
-		 calc_ecc[0], calc_ecc[1], calc_ecc[2],
-		 diff0, diff1, diff2);
-
-	if (diff0 == 0 && diff1 == 0 && diff2 == 0)
-		return 0;		/* ECC is ok */
-
+	unsigned long nfmeccdata0, nfmeccdata1;
+	unsigned long nfestat0;
+	unsigned char err_type;
+	int retval;
+	unsigned int offset;
+	unsigned char wrong_value, corr_value;
+	struct s3c2410_nand_mtd *nmtd = s3c2410_nand_mtd_toours(mtd);
+	struct nand_chip *chip = &nmtd->chip;
+	
+	pr_debug("%s: read %02x%02x%02x%02x calc %02x%02x%02x%02x\n", __func__,
+		 read_ecc[0], read_ecc[1], read_ecc[2], read_ecc[3],
+		 calc_ecc[0], calc_ecc[1], calc_ecc[2], calc_ecc[3]);
+	
 	/* sometimes people do not think about using the ECC, so check
 	 * to see if we have an 0xff,0xff,0xff read ECC and then ignore
 	 * the error, on the assumption that this is an un-eccd page.
 	 */
 	if (read_ecc[0] == 0xff && read_ecc[1] == 0xff && read_ecc[2] == 0xff
-	    && info->platform->ignore_unset_ecc)
+	    && read_ecc[3] == 0xff && info->platform->ignore_unset_ecc) {
+		pr_debug("Ignoring the HWECC calculation?\n");
 		return 0;
-
-	/* Can we correct this ECC (ie, one row and column change).
-	 * Note, this is similar to the 256 error code on smartmedia */
-
-	if (((diff0 ^ (diff0 >> 1)) & 0x55) == 0x55 &&
-	    ((diff1 ^ (diff1 >> 1)) & 0x55) == 0x55 &&
-	    ((diff2 ^ (diff2 >> 1)) & 0x55) == 0x55) {
-		/* calculate the bit position of the error */
-
-		bit  = ((diff2 >> 3) & 1) |
-		       ((diff2 >> 4) & 2) |
-		       ((diff2 >> 5) & 4);
-
-		/* calculate the byte position of the error */
-
-		byte = ((diff2 << 7) & 0x100) |
-		       ((diff1 << 0) & 0x80)  |
-		       ((diff1 << 1) & 0x40)  |
-		       ((diff1 << 2) & 0x20)  |
-		       ((diff1 << 3) & 0x10)  |
-		       ((diff0 >> 4) & 0x08)  |
-		       ((diff0 >> 3) & 0x04)  |
-		       ((diff0 >> 2) & 0x02)  |
-		       ((diff0 >> 1) & 0x01);
-
-		dev_dbg(info->device, "correcting error bit %d, byte %d\n",
-			bit, byte);
-
-		dat[byte] ^= (1 << bit);
-		return 1;
 	}
 
-	/* if there is only one bit difference in the ECC, then
-	 * one of only a row or column parity has changed, which
-	 * means the error is most probably in the ECC itself */
-
-	diff0 |= (diff1 << 8);
-	diff0 |= (diff2 << 16);
-
-	if ((diff0 & ~(1<<fls(diff0))) == 0)
-		return 1;
-
-	return -1;
+	/*
+	 * If the chip has a small page then set the fourth byte to 0xff, so that
+	 * we can use the NAND-controller to check if the ECC is correct or not.
+	 * Please note that this "workaround" is coming from the U-Boot, which has
+	 * an ECC-layout with only three ECC-bytes (but why?).
+	 * (Luis Galdos)
+	 */
+	if (chip->page_shift <= 10) {
+		pr_debug("Setting the fourth byte to 0xff\n");
+		read_ecc[3] = 0xff;
+	}
+	
+	/*
+	 * The below code is coming from the driver 's3c_nand.c' (by jsgood)
+	 * Luis Galdos
+	 */
+	nfmeccdata0 = (read_ecc[1] << 16) | read_ecc[0];
+	nfmeccdata1 = (read_ecc[3] << 16) | read_ecc[2];
+	writel(nfmeccdata0, info->regs + S3C2440_NFECCD0);
+	writel(nfmeccdata1, info->regs + S3C2440_NFECCD1);
+	nfestat0 = readl(info->regs + S3C2412_NFMECC_ERR0);
+	err_type = nfestat0 & 0x3;
+		
+	switch (err_type) {
+	case 0:
+		retval = 0;
+		break;
+	case 1:
+		offset = (nfestat0 >> 7) & 0x7ff;
+		wrong_value = dat[offset];
+		corr_value = wrong_value ^ (1 << ((nfestat0 >> 4) & 0x7));
+		printk(KERN_DEBUG "[NAND BitErr]: 1 bit error detected at page byte %u. Correcting from 0x%02x to 0x%02x\n",
+		       offset, wrong_value, corr_value);
+		dat[offset] = corr_value;
+		retval = 1;
+		break;
+	default:
+		printk(KERN_ERR "[NAND BitErr]: More than 1 bit error detected. Uncorrectable error.\n");
+		retval = -1;
+		break;
+	}
+	
+	return retval;
 }
 
+
 /* ECC functions
  *
  * These allow the s3c2410 and s3c2440 to use the controller's ECC
  * generator block to ECC the data as it passes through]
-*/
+ */
 
 static void s3c2410_nand_enable_hwecc(struct mtd_info *mtd, int mode)
 {
@@ -462,8 +462,21 @@ static void s3c2412_nand_enable_hwecc(struct mtd_info *mtd, int mode)
 	struct s3c2410_nand_info *info = s3c2410_nand_mtd_toinfo(mtd);
 	unsigned long ctrl;
 
+	/*
+	 * According to the manual: unlock the main area and set the data transfer
+	 * direction (read or write) too
+	 * Luis Galdos
+	 */
 	ctrl = readl(info->regs + S3C2440_NFCONT);
-	writel(ctrl | S3C2412_NFCONT_INIT_MAIN_ECC, info->regs + S3C2440_NFCONT);
+	ctrl |= S3C2412_NFCONT_INIT_MAIN_ECC;
+	ctrl &= ~S3C2412_NFCONT_MAIN_ECC_LOCK;
+
+	if (mode == NAND_ECC_WRITE)
+		ctrl |= S3C2412_NFCONT_ECC4_DIRWR;
+	else
+		ctrl &= ~S3C2412_NFCONT_ECC4_DIRWR;
+	
+	writel(ctrl, info->regs + S3C2440_NFCONT);
 }
 
 static void s3c2440_nand_enable_hwecc(struct mtd_info *mtd, int mode)
@@ -492,13 +505,25 @@ static int s3c2410_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u
 static int s3c2412_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u_char *ecc_code)
 {
 	struct s3c2410_nand_info *info = s3c2410_nand_mtd_toinfo(mtd);
-	unsigned long ecc = readl(info->regs + S3C2412_NFMECC0);
+	unsigned long ecc, nfcont;
+	
+	/*
+	 * According to the manual: lock the main area before reading from the ECC
+	 * status register.
+	 * Luis Galdos
+	 */
+	nfcont = readl(info->regs + S3C2440_NFCONT);
+	nfcont |= S3C2412_NFCONT_MAIN_ECC_LOCK;
+	writel(nfcont, info->regs + S3C2440_NFCONT);
 
-	ecc_code[0] = ecc;
-	ecc_code[1] = ecc >> 8;
-	ecc_code[2] = ecc >> 16;
+	ecc = readl(info->regs + S3C2412_NFMECC0);
+	ecc_code[0] = ecc & 0xff;
+	ecc_code[1] = (ecc >> 8) & 0xff;
+	ecc_code[2] = (ecc >> 16) & 0xff;
+	ecc_code[3] = (ecc >> 24) & 0xff;
 
-	pr_debug("calculate_ecc: returning ecc %02x,%02x,%02x\n", ecc_code[0], ecc_code[1], ecc_code[2]);
+	pr_debug("calculate_ecc: returning ecc %02x,%02x,%02x,%02x\n",
+		 ecc_code[0], ecc_code[1], ecc_code[2], ecc_code[3]);
 
 	return 0;
 }
@@ -519,7 +544,7 @@ static int s3c2440_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u
 
 /* over-ride the standard functions for a little more speed. We can
  * use read/write block to move the data buffers to/from the controller
-*/
+ */
 
 static void s3c2410_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
@@ -545,52 +570,6 @@ static void s3c2440_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int
 	writesl(info->regs + S3C2440_NFDATA, buf, len / 4);
 }
 
-/* cpufreq driver support */
-
-#ifdef CONFIG_CPU_FREQ
-
-static int s3c2410_nand_cpufreq_transition(struct notifier_block *nb,
-					  unsigned long val, void *data)
-{
-	struct s3c2410_nand_info *info;
-	unsigned long newclk;
-
-	info = container_of(nb, struct s3c2410_nand_info, freq_transition);
-	newclk = clk_get_rate(info->clk);
-
-	if ((val == CPUFREQ_POSTCHANGE && newclk < info->clk_rate) ||
-	    (val == CPUFREQ_PRECHANGE && newclk > info->clk_rate)) {
-		s3c2410_nand_setrate(info);
-	}
-
-	return 0;
-}
-
-static inline int s3c2410_nand_cpufreq_register(struct s3c2410_nand_info *info)
-{
-	info->freq_transition.notifier_call = s3c2410_nand_cpufreq_transition;
-
-	return cpufreq_register_notifier(&info->freq_transition,
-					 CPUFREQ_TRANSITION_NOTIFIER);
-}
-
-static inline void s3c2410_nand_cpufreq_deregister(struct s3c2410_nand_info *info)
-{
-	cpufreq_unregister_notifier(&info->freq_transition,
-				    CPUFREQ_TRANSITION_NOTIFIER);
-}
-
-#else
-static inline int s3c2410_nand_cpufreq_register(struct s3c2410_nand_info *info)
-{
-	return 0;
-}
-
-static inline void s3c2410_nand_cpufreq_deregister(struct s3c2410_nand_info *info)
-{
-}
-#endif
-
 /* device management functions */
 
 static int s3c2410_nand_remove(struct platform_device *pdev)
@@ -602,10 +581,9 @@ static int s3c2410_nand_remove(struct platform_device *pdev)
 	if (info == NULL)
 		return 0;
 
-	s3c2410_nand_cpufreq_deregister(info);
-
-	/* Release all our mtds  and their partitions, then go through
-	 * freeing the resources used
+	/* first thing we need to do is release all our mtds
+	 * and their partitions, then go through freeing the
+	 * resources used
 	 */
 
 	if (info->mtds != NULL) {
@@ -670,7 +648,7 @@ static int s3c2410_nand_add_partition(struct s3c2410_nand_info *info,
 /* s3c2410_nand_init_chip
  *
  * init a single instance of an chip
-*/
+ */
 
 static void s3c2410_nand_init_chip(struct s3c2410_nand_info *info,
 				   struct s3c2410_nand_mtd *nmtd,
@@ -752,11 +730,14 @@ static void s3c2410_nand_init_chip(struct s3c2410_nand_info *info,
 		chip->ecc.mode	    = NAND_ECC_SOFT;
 	}
 
+	/* @FIXME: Here seems to be something wrong. (Luis Galdos) */
+#if 0
 	if (set->ecc_layout != NULL)
 		chip->ecc.layout = set->ecc_layout;
 
 	if (set->disable_ecc)
 		chip->ecc.mode	= NAND_ECC_NONE;
+#endif
 }
 
 /* s3c2410_nand_update_chip
@@ -770,20 +751,26 @@ static void s3c2410_nand_update_chip(struct s3c2410_nand_info *info,
 {
 	struct nand_chip *chip = &nmtd->chip;
 
-	dev_dbg(info->device, "chip %p => page shift %d\n",
-		chip, chip->page_shift);
+	printk("%s: chip %p: %d\n", __func__, chip, chip->page_shift);
 
 	if (hardware_ecc) {
 		/* change the behaviour depending on wether we are using
 		 * the large or small page nand device */
 
 		if (chip->page_shift > 10) {
-			chip->ecc.size	    = 256;
-			chip->ecc.bytes	    = 3;
+			chip->ecc.size	    = 2048;
+			chip->ecc.bytes	    = 4;
+			chip->ecc.layout    = &nand_hw_eccoob;
 		} else {
+		  /*
+		   * IMPORTANT: For using only three ECC-bytes is required to set
+		   * the fourth byte to '0xff', otherwise we can't use the internal
+		   * NAND-controller for checking if the ECC is correct or not.
+		   * (Luis Galdos)
+		   */
 			chip->ecc.size	    = 512;
 			chip->ecc.bytes	    = 3;
-			chip->ecc.layout    = &nand_hw_eccoob;
+			chip->ecc.layout    = &nand_hw_eccoob_16;
 		}
 	}
 }
@@ -794,7 +781,7 @@ static void s3c2410_nand_update_chip(struct s3c2410_nand_info *info,
  * one our driver can handled. This code checks to see if
  * it can allocate all necessary resources then calls the
  * nand layer to look for devices
-*/
+ */
 
 static int s3c24xx_nand_probe(struct platform_device *pdev,
 			      enum s3c_cpu_type cpu_type)
@@ -809,6 +796,18 @@ static int s3c24xx_nand_probe(struct platform_device *pdev,
 	int nr_sets;
 	int setno;
 
+	/*
+	 * Required variables for accessing to the partitions from the command line
+	 * Luis Galdos
+	 */
+#if defined(CONFIG_MTD_CMDLINE_PARTS)
+	struct mtd_partition *mtd_parts;
+	int nr_parts;
+	const char *name_back;
+	struct s3c2410_nand_set nand_sets[1];
+	const char *part_probes[] = { "cmdlinepart", NULL };
+#endif
+	
 	pr_debug("s3c2410_nand_probe(%p)\n", pdev);
 
 	info = kmalloc(sizeof(*info), GFP_KERNEL);
@@ -864,7 +863,7 @@ static int s3c24xx_nand_probe(struct platform_device *pdev,
 
 	/* initialise the hardware */
 
-	err = s3c2410_nand_inithw(info);
+	err = s3c2410_nand_inithw(info, pdev);
 	if (err != 0)
 		goto exit_error;
 
@@ -891,12 +890,33 @@ static int s3c24xx_nand_probe(struct platform_device *pdev,
 
 	for (setno = 0; setno < nr_sets; setno++, nmtd++) {
 		pr_debug("initialising set %d (%p, info %p)\n", setno, nmtd, info);
-
 		s3c2410_nand_init_chip(info, nmtd, sets);
-
 		nmtd->scan_res = nand_scan_ident(&nmtd->mtd,
 						 (sets) ? sets->nr_chips : 1);
 
+		/*
+		 * This is the code required for accessing to the partitions table
+		 * passed by the bootloader
+		 * (Luis Galdos)
+		 */
+#if defined(CONFIG_MTD_CMDLINE_PARTS)
+		/*
+		 * Need to reset the name of the MTD-device then the name of the
+		 * partition passed by the U-Boot differs from the assigned name.
+		 * U-Boot : onboard_boot
+		 * Device : NAND 128MiB 3,3V 8-bit
+		 */
+		name_back = nmtd->mtd.name;
+		nmtd->mtd.name= PARTITON_PARAMETER_NAME;
+		nr_parts = parse_mtd_partitions(&nmtd->mtd, part_probes, &mtd_parts, 0);
+		nmtd->mtd.name = name_back;
+		nand_sets[0].name = "NAND-Parts";
+		nand_sets[0].nr_chips = 1;
+		nand_sets[0].partitions = mtd_parts;
+		nand_sets[0].nr_partitions = nr_parts;
+		sets = nand_sets;
+#endif /* defined(CONFIG_MTD_CMDLINE_PARTS) */
+		
 		if (nmtd->scan_res == 0) {
 			s3c2410_nand_update_chip(info, nmtd);
 			nand_scan_tail(&nmtd->mtd);
@@ -907,12 +927,6 @@ static int s3c24xx_nand_probe(struct platform_device *pdev,
 			sets++;
 	}
 
-	err = s3c2410_nand_cpufreq_register(info);
-	if (err < 0) {
-		dev_err(&pdev->dev, "failed to init cpufreq support\n");
-		goto exit_error;
-	}
-
 	if (allow_clk_stop(info)) {
 		dev_info(&pdev->dev, "clock idle support enabled\n");
 		clk_disable(info->clk);
@@ -960,7 +974,7 @@ static int s3c24xx_nand_resume(struct platform_device *dev)
 
 	if (info) {
 		clk_enable(info->clk);
-		s3c2410_nand_inithw(info);
+		s3c2410_nand_inithw(info, dev);
 
 		/* Restore the state of the nFCE line. */
 
@@ -1035,9 +1049,16 @@ static int __init s3c2410_nand_init(void)
 {
 	printk("S3C24XX NAND Driver, (c) 2004 Simtec Electronics\n");
 
-	platform_driver_register(&s3c2412_nand_driver);
-	platform_driver_register(&s3c2440_nand_driver);
-	return platform_driver_register(&s3c2410_nand_driver);
+	if (platform_driver_register(&s3c2440_nand_driver))
+		printk(KERN_ERR "Registering S3C2440 NAND driver\n");
+	
+	if (platform_driver_register(&s3c2412_nand_driver))
+		printk(KERN_ERR "Registering S3C2412 NAND driver\n");
+	
+	if (platform_driver_register(&s3c2410_nand_driver))
+		printk(KERN_ERR "Registering S3C2410 NAND driver\n");
+
+	return 0;
 }
 
 static void __exit s3c2410_nand_exit(void)
diff --git a/drivers/mxc/Kconfig b/drivers/mxc/Kconfig
new file mode 100644
index 000000000000..72225bbe91de
--- /dev/null
+++ b/drivers/mxc/Kconfig
@@ -0,0 +1,39 @@
+# drivers/video/mxc/Kconfig
+
+if ARCH_MXC
+
+menu "MXC support drivers"
+
+config MXC_IPU
+	bool "Image Processing Unit Driver"
+	depends on !ARCH_MX21
+	depends on !ARCH_MX27
+	depends on !ARCH_MX25
+	select MXC_IPU_V1 if !ARCH_MX37 && !ARCH_MX51
+	select MXC_IPU_V3 if ARCH_MX37 || ARCH_MX51
+	select MXC_IPU_V3D if ARCH_MX37
+	select MXC_IPU_V3EX if ARCH_MX51
+	help
+	  If you plan to use the Image Processing unit, say
+	  Y here. IPU is needed by Framebuffer and V4L2 drivers.
+
+source "drivers/mxc/ipu/Kconfig"
+source "drivers/mxc/ipu3/Kconfig"
+
+source "drivers/mxc/ssi/Kconfig"
+source "drivers/mxc/dam/Kconfig"
+source "drivers/mxc/pmic/Kconfig"
+source "drivers/mxc/mcu_pmic/Kconfig"
+source "drivers/mxc/security/Kconfig"
+source "drivers/mxc/hmp4e/Kconfig"
+source "drivers/mxc/hw_event/Kconfig"
+source "drivers/mxc/vpu/Kconfig"
+source "drivers/mxc/asrc/Kconfig"
+source "drivers/mxc/bt/Kconfig"
+source "drivers/mxc/gps_ioctrl/Kconfig"
+source "drivers/mxc/mlb/Kconfig"
+source "drivers/mxc/adc/Kconfig"
+
+endmenu
+
+endif
diff --git a/drivers/mxc/Makefile b/drivers/mxc/Makefile
new file mode 100644
index 000000000000..6416bc429888
--- /dev/null
+++ b/drivers/mxc/Makefile
@@ -0,0 +1,17 @@
+obj-$(CONFIG_MXC_IPU_V1)			+= ipu/
+obj-$(CONFIG_MXC_IPU_V3)			+= ipu3/
+obj-$(CONFIG_MXC_SSI)               	+= ssi/
+obj-$(CONFIG_MXC_DAM)               	+= dam/
+
+obj-$(CONFIG_MXC_PMIC_MC9SDZ60)		+= mcu_pmic/
+obj-$(CONFIG_MXC_PMIC)			+= pmic/
+
+obj-$(CONFIG_MXC_HMP4E)         	+= hmp4e/
+obj-y                                   += security/
+obj-$(CONFIG_MXC_VPU)                   += vpu/
+obj-$(CONFIG_MXC_HWEVENT)               += hw_event/
+obj-$(CONFIG_MXC_ASRC)                 += asrc/
+obj-$(CONFIG_MXC_BLUETOOTH)		+= bt/
+obj-$(CONFIG_GPS_IOCTRL)		+= gps_ioctrl/
+obj-$(CONFIG_MXC_MLB)                   += mlb/
+obj-$(CONFIG_IMX_ADC)                   += adc/
diff --git a/drivers/mxc/adc/Kconfig b/drivers/mxc/adc/Kconfig
new file mode 100644
index 000000000000..91ad23bc7cbe
--- /dev/null
+++ b/drivers/mxc/adc/Kconfig
@@ -0,0 +1,14 @@
+#
+# i.MX ADC devices
+#
+
+menu "i.MX ADC support"
+
+config IMX_ADC
+	tristate "i.MX ADC"
+	depends on ARCH_MXC
+	default n
+	help
+	   This selects the Freescale i.MX on-chip ADC driver.
+
+endmenu
diff --git a/drivers/mxc/adc/Makefile b/drivers/mxc/adc/Makefile
new file mode 100644
index 000000000000..e21e48ee1185
--- /dev/null
+++ b/drivers/mxc/adc/Makefile
@@ -0,0 +1,4 @@
+#
+# Makefile for i.MX adc devices.
+#
+obj-$(CONFIG_IMX_ADC)		+= imx_adc.o
diff --git a/drivers/mxc/adc/imx_adc.c b/drivers/mxc/adc/imx_adc.c
new file mode 100644
index 000000000000..4f885a32a14b
--- /dev/null
+++ b/drivers/mxc/adc/imx_adc.c
@@ -0,0 +1,1047 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file adc/imx_adc.c
+ * @brief This is the main file of i.MX ADC driver.
+ *
+ * @ingroup IMX_ADC
+ */
+
+/*
+ * Includes
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/wait.h>
+#include <linux/imx_adc.h>
+#include "imx_adc_reg.h"
+
+static int imx_adc_major;
+
+/*!
+ * Number of users waiting in suspendq
+ */
+static int swait;
+
+/*!
+ * To indicate whether any of the adc devices are suspending
+ */
+static int suspend_flag;
+
+/*!
+ * The suspendq is used by blocking application calls
+ */
+static wait_queue_head_t suspendq;
+static wait_queue_head_t tsq;
+
+static bool imx_adc_ready;
+static bool ts_data_ready;
+static int tsi_data = TSI_DATA;
+static unsigned short ts_data_buf[16];
+
+static struct class *imx_adc_class;
+static struct imx_adc_data *adc_data;
+
+static DECLARE_MUTEX(general_convert_mutex);
+static DECLARE_MUTEX(ts_convert_mutex);
+
+unsigned long tsc_base;
+
+int is_imx_adc_ready(void)
+{
+	return imx_adc_ready;
+}
+EXPORT_SYMBOL(is_imx_adc_ready);
+
+void tsc_clk_enable(void)
+{
+	unsigned long reg;
+
+	clk_enable(adc_data->adc_clk);
+
+	reg = __raw_readl(tsc_base + TGCR);
+	reg |= TGCR_IPG_CLK_EN;
+	__raw_writel(reg, tsc_base + TGCR);
+}
+
+void tsc_clk_disable(void)
+{
+	unsigned long reg;
+
+	clk_disable(adc_data->adc_clk);
+
+	reg = __raw_readl(tsc_base + TGCR);
+	reg &= ~TGCR_IPG_CLK_EN;
+	__raw_writel(reg, tsc_base + TGCR);
+}
+
+void tsc_self_reset(void)
+{
+	unsigned long reg;
+
+	reg = __raw_readl(tsc_base + TGCR);
+	reg |= TGCR_TSC_RST;
+	__raw_writel(reg, tsc_base + TGCR);
+
+	while (__raw_readl(tsc_base + TGCR) & TGCR_TSC_RST)
+		continue;
+}
+
+/* Internal reference */
+void tsc_intref_enable(void)
+{
+	unsigned long reg;
+
+	reg = __raw_readl(tsc_base + TGCR);
+	reg |= TGCR_INTREFEN;
+	__raw_writel(reg, tsc_base + TGCR);
+}
+
+/* initialize touchscreen */
+void imx_tsc_init(void)
+{
+	unsigned long reg;
+	int lastitemid;
+	int dbtime;
+
+	/* Level sense */
+	reg = __raw_readl(tsc_base + TCQCR);
+	reg |= CQCR_PD_CFG;
+	reg |= (0xf << CQCR_FIFOWATERMARK_SHIFT);  /* watermark */
+	__raw_writel(reg, tsc_base + TCQCR);
+
+	/* Configure 4-wire */
+	reg = TSC_4WIRE_PRECHARGE;
+	reg |= CC_IGS;
+	__raw_writel(reg, tsc_base + TCC0);
+
+	reg = TSC_4WIRE_TOUCH_DETECT;
+	reg |= 3 << CC_NOS_SHIFT;	/* 4 samples */
+	reg |= 32 << CC_SETTLING_TIME_SHIFT;	/* it's important! */
+	__raw_writel(reg, tsc_base + TCC1);
+
+	reg = TSC_4WIRE_X_MEASUMENT;
+	reg |= 3 << CC_NOS_SHIFT;	/* 4 samples */
+	reg |= 16 << CC_SETTLING_TIME_SHIFT;	/* settling time */
+	__raw_writel(reg, tsc_base + TCC2);
+
+	reg = TSC_4WIRE_Y_MEASUMENT;
+	reg |= 3 << CC_NOS_SHIFT;	/* 4 samples */
+	reg |= 16 << CC_SETTLING_TIME_SHIFT;	/* settling time */
+	__raw_writel(reg, tsc_base + TCC3);
+
+	reg = (TCQ_ITEM_TCC0 << TCQ_ITEM7_SHIFT) |
+	      (TCQ_ITEM_TCC0 << TCQ_ITEM6_SHIFT) |
+	      (TCQ_ITEM_TCC1 << TCQ_ITEM5_SHIFT) |
+	      (TCQ_ITEM_TCC0 << TCQ_ITEM4_SHIFT) |
+	      (TCQ_ITEM_TCC3 << TCQ_ITEM3_SHIFT) |
+	      (TCQ_ITEM_TCC2 << TCQ_ITEM2_SHIFT) |
+	      (TCQ_ITEM_TCC1 << TCQ_ITEM1_SHIFT) |
+	      (TCQ_ITEM_TCC0 << TCQ_ITEM0_SHIFT);
+	__raw_writel(reg, tsc_base + TCQ_ITEM_7_0);
+
+	lastitemid = 5;
+	reg = __raw_readl(tsc_base + TCQCR);
+	reg = (reg & ~CQCR_LAST_ITEM_ID_MASK) |
+	      (lastitemid << CQCR_LAST_ITEM_ID_SHIFT);
+	__raw_writel(reg, tsc_base + TCQCR);
+
+	/* pen down enable */
+	reg = __raw_readl(tsc_base + TCQCR);
+	reg &= ~CQCR_PD_MSK;
+	__raw_writel(reg, tsc_base + TCQCR);
+	reg = __raw_readl(tsc_base + TCQMR);
+	reg &= ~TCQMR_PD_IRQ_MSK;
+	__raw_writel(reg, tsc_base + TCQMR);
+
+	/* Debounce time = dbtime*8 adc clock cycles */
+	reg = __raw_readl(tsc_base + TGCR);
+	dbtime = 3;
+	reg &= ~TGCR_PDBTIME_MASK;
+	reg |= dbtime << TGCR_PDBTIME_SHIFT;
+	reg |= TGCR_SLPC;
+	__raw_writel(reg, tsc_base + TGCR);
+
+}
+
+static irqreturn_t imx_adc_interrupt(int irq, void *dev_id)
+{
+	int reg;
+
+	/* disable pen down detect */
+	reg = __raw_readl(tsc_base + TGCR);
+	reg &= ~TGCR_PD_EN;
+	__raw_writel(reg, tsc_base + TGCR);
+
+	ts_data_ready = 1;
+	wake_up_interruptible(&tsq);
+	return IRQ_HANDLED;
+}
+
+enum IMX_ADC_STATUS imx_adc_read_general(unsigned short *result)
+{
+	unsigned long reg;
+	unsigned int data_num = 0;
+
+	reg = __raw_readl(tsc_base + GCQCR);
+	reg |= CQCR_FQS;
+	__raw_writel(reg, tsc_base + GCQCR);
+
+	while (!(__raw_readl(tsc_base + GCQSR) & CQSR_EOQ))
+		continue;
+	reg = __raw_readl(tsc_base + GCQCR);
+	reg &= ~CQCR_FQS;
+	__raw_writel(reg, tsc_base + GCQCR);
+	reg = __raw_readl(tsc_base + GCQSR);
+	reg |= CQSR_EOQ;
+	__raw_writel(reg, tsc_base + GCQSR);
+
+	while (!(__raw_readl(tsc_base + GCQSR) & CQSR_EMPT)) {
+		result[data_num] = __raw_readl(tsc_base + GCQFIFO) >>
+				 GCQFIFO_ADCOUT_SHIFT;
+		data_num++;
+	}
+	return IMX_ADC_SUCCESS;
+}
+
+/*!
+ * This function will get raw (X,Y) value by converting the voltage
+ * @param        touch_sample Pointer to touch sample
+ *
+ * return        This funciton returns 0 if successful.
+ *
+ *
+ */
+enum IMX_ADC_STATUS imx_adc_read_ts(struct t_touch_screen *touch_sample,
+				    int wait_tsi)
+{
+	int reg;
+	int data_num = 0;
+	int detect_sample1, detect_sample2;
+
+	memset(ts_data_buf, 0, sizeof ts_data_buf);
+	touch_sample->valid_flag = 1;
+
+	if (wait_tsi) {
+		/* Config idle for 4-wire */
+		reg = TSC_4WIRE_TOUCH_DETECT;
+		__raw_writel(reg, tsc_base + TICR);
+
+		/* Pen interrupt starts new conversion queue */
+		reg = __raw_readl(tsc_base + TCQCR);
+		reg &= ~CQCR_QSM_MASK;
+		reg |= CQCR_QSM_PEN;
+		__raw_writel(reg, tsc_base + TCQCR);
+
+		/* PDEN and PDBEN */
+		reg = __raw_readl(tsc_base + TGCR);
+		reg |= (TGCR_PDB_EN | TGCR_PD_EN);
+		__raw_writel(reg, tsc_base + TGCR);
+
+		wait_event_interruptible(tsq, ts_data_ready);
+		while (!(__raw_readl(tsc_base + TCQSR) & CQSR_EOQ))
+			continue;
+		/* stop the conversion */
+		reg = __raw_readl(tsc_base + TCQCR);
+		reg &= ~CQCR_QSM_MASK;
+		__raw_writel(reg, tsc_base + TCQCR);
+		reg = CQSR_PD | CQSR_EOQ;
+		__raw_writel(reg, tsc_base + TCQSR);
+
+		/* change configuration for FQS mode */
+		tsi_data = TSI_DATA;
+		reg = (0x1 << CC_YPLLSW_SHIFT) | (0x1 << CC_XNURSW_SHIFT) |
+		      CC_XPULSW;
+		__raw_writel(reg, tsc_base + TICR);
+	} else {
+		/* FQS semaphore */
+		down(&ts_convert_mutex);
+
+		reg = (0x1 << CC_YPLLSW_SHIFT) | (0x1 << CC_XNURSW_SHIFT) |
+		      CC_XPULSW;
+		__raw_writel(reg, tsc_base + TICR);
+
+		/* FQS */
+		reg = __raw_readl(tsc_base + TCQCR);
+		reg &= ~CQCR_QSM_MASK;
+		reg |= CQCR_QSM_FQS;
+		__raw_writel(reg, tsc_base + TCQCR);
+		reg = __raw_readl(tsc_base + TCQCR);
+		reg |= CQCR_FQS;
+		__raw_writel(reg, tsc_base + TCQCR);
+		while (!(__raw_readl(tsc_base + TCQSR) & CQSR_EOQ))
+			continue;
+
+		/* stop FQS */
+		reg = __raw_readl(tsc_base + TCQCR);
+		reg &= ~CQCR_QSM_MASK;
+		__raw_writel(reg, tsc_base + TCQCR);
+		reg = __raw_readl(tsc_base + TCQCR);
+		reg &= ~CQCR_FQS;
+		__raw_writel(reg, tsc_base + TCQCR);
+
+		/* clear status bit */
+		reg = __raw_readl(tsc_base + TCQSR);
+		reg |= CQSR_EOQ;
+		__raw_writel(reg, tsc_base + TCQSR);
+		tsi_data = FQS_DATA;
+	}
+
+	while (!(__raw_readl(tsc_base + TCQSR) & CQSR_EMPT)) {
+		reg = __raw_readl(tsc_base + TCQFIFO);
+		ts_data_buf[data_num] = reg;
+		data_num++;
+	}
+
+	touch_sample->x_position1 = ts_data_buf[4] >> 4;
+	touch_sample->x_position2 = ts_data_buf[5] >> 4;
+	touch_sample->x_position3 = ts_data_buf[6] >> 4;
+	touch_sample->y_position1 = ts_data_buf[9] >> 4;
+	touch_sample->y_position2 = ts_data_buf[10] >> 4;
+	touch_sample->y_position3 = ts_data_buf[11] >> 4;
+
+	detect_sample1 = ts_data_buf[0];
+	detect_sample2 = ts_data_buf[12];
+
+	if ((detect_sample1 > 0x6000) || (detect_sample2 > 0x6000))
+		touch_sample->valid_flag = 0;
+
+	ts_data_ready = 0;
+
+	if (!(touch_sample->x_position1 ||
+	      touch_sample->x_position2 || touch_sample->x_position3))
+		touch_sample->contact_resistance = 0;
+	else
+		touch_sample->contact_resistance = 1;
+
+	if (tsi_data == FQS_DATA)
+		up(&ts_convert_mutex);
+	return IMX_ADC_SUCCESS;
+}
+
+/*!
+ * This function performs filtering and rejection of excessive noise prone
+ * sampl.
+ *
+ * @param        ts_curr     Touch screen value
+ *
+ * @return       This function returns 0 on success, -1 otherwise.
+ */
+static int imx_adc_filter(struct t_touch_screen *ts_curr)
+{
+
+	unsigned int ydiff1, ydiff2, ydiff3, xdiff1, xdiff2, xdiff3;
+	unsigned int sample_sumx, sample_sumy;
+	static unsigned int prev_x[FILTLEN], prev_y[FILTLEN];
+	int index = 0;
+	unsigned int y_curr, x_curr;
+	static int filt_count;
+	/* Added a variable filt_type to decide filtering at run-time */
+	unsigned int filt_type = 0;
+
+	/* ignore the data converted when pen down and up */
+	if ((ts_curr->contact_resistance == 0) || tsi_data == TSI_DATA) {
+		ts_curr->x_position = 0;
+		ts_curr->y_position = 0;
+		filt_count = 0;
+		return 0;
+	}
+	/* ignore the data valid */
+	if (ts_curr->valid_flag == 0)
+		return -1;
+
+	ydiff1 = abs(ts_curr->y_position1 - ts_curr->y_position2);
+	ydiff2 = abs(ts_curr->y_position2 - ts_curr->y_position3);
+	ydiff3 = abs(ts_curr->y_position1 - ts_curr->y_position3);
+	if ((ydiff1 > DELTA_Y_MAX) ||
+	    (ydiff2 > DELTA_Y_MAX) || (ydiff3 > DELTA_Y_MAX)) {
+		pr_debug("imx_adc_filter: Ret pos 1\n");
+		return -1;
+	}
+
+	xdiff1 = abs(ts_curr->x_position1 - ts_curr->x_position2);
+	xdiff2 = abs(ts_curr->x_position2 - ts_curr->x_position3);
+	xdiff3 = abs(ts_curr->x_position1 - ts_curr->x_position3);
+
+	if ((xdiff1 > DELTA_X_MAX) ||
+	    (xdiff2 > DELTA_X_MAX) || (xdiff3 > DELTA_X_MAX)) {
+		pr_debug("imx_adc_filter: Ret pos 2\n");
+		return -1;
+	}
+	/* Compute two closer values among the three available Y readouts */
+
+	if (ydiff1 < ydiff2) {
+		if (ydiff1 < ydiff3) {
+			/* Sample 0 & 1 closest together */
+			sample_sumy = ts_curr->y_position1 +
+			    ts_curr->y_position2;
+		} else {
+			/* Sample 0 & 2 closest together */
+			sample_sumy = ts_curr->y_position1 +
+			    ts_curr->y_position3;
+		}
+	} else {
+		if (ydiff2 < ydiff3) {
+			/* Sample 1 & 2 closest together */
+			sample_sumy = ts_curr->y_position2 +
+			    ts_curr->y_position3;
+		} else {
+			/* Sample 0 & 2 closest together */
+			sample_sumy = ts_curr->y_position1 +
+			    ts_curr->y_position3;
+		}
+	}
+
+	/*
+	 * Compute two closer values among the three available X
+	 * readouts
+	 */
+	if (xdiff1 < xdiff2) {
+		if (xdiff1 < xdiff3) {
+			/* Sample 0 & 1 closest together */
+			sample_sumx = ts_curr->x_position1 +
+			    ts_curr->x_position2;
+		} else {
+			/* Sample 0 & 2 closest together */
+			sample_sumx = ts_curr->x_position1 +
+			    ts_curr->x_position3;
+		}
+	} else {
+		if (xdiff2 < xdiff3) {
+			/* Sample 1 & 2 closest together */
+			sample_sumx = ts_curr->x_position2 +
+			    ts_curr->x_position3;
+		} else {
+			/* Sample 0 & 2 closest together */
+			sample_sumx = ts_curr->x_position1 +
+			    ts_curr->x_position3;
+		}
+	}
+
+	/*
+	 * Wait FILTER_MIN_DELAY number of samples to restart
+	 * filtering
+	 */
+	if (filt_count < FILTER_MIN_DELAY) {
+		/*
+		 * Current output is the average of the two closer
+		 * values and no filtering is used
+		 */
+		y_curr = (sample_sumy / 2);
+		x_curr = (sample_sumx / 2);
+		ts_curr->y_position = y_curr;
+		ts_curr->x_position = x_curr;
+		filt_count++;
+
+	} else {
+		if (abs(sample_sumx - (prev_x[0] + prev_x[1])) >
+		    (DELTA_X_MAX * 16)) {
+			pr_debug("imx_adc_filter: : Ret pos 3\n");
+			return -1;
+		}
+		if (abs(sample_sumy - (prev_y[0] + prev_y[1])) >
+		    (DELTA_Y_MAX * 16)) {
+			pr_debug("imx_adc_filter: : Ret pos 4\n");
+			return -1;
+		}
+		sample_sumy /= 2;
+		sample_sumx /= 2;
+		/* Use hard filtering if the sample difference < 10 */
+		if ((abs(sample_sumy - prev_y[0]) > 10) ||
+		    (abs(sample_sumx - prev_x[0]) > 10))
+			filt_type = 1;
+
+		/*
+		 * Current outputs are the average of three previous
+		 * values and the present readout
+		 */
+		y_curr = sample_sumy;
+		for (index = 0; index < FILTLEN; index++) {
+			if (filt_type == 0)
+				y_curr = y_curr + (prev_y[index]);
+			else
+				y_curr = y_curr + (prev_y[index] / 3);
+		}
+		if (filt_type == 0)
+			y_curr = y_curr >> 2;
+		else
+			y_curr = y_curr >> 1;
+		ts_curr->y_position = y_curr;
+
+		x_curr = sample_sumx;
+		for (index = 0; index < FILTLEN; index++) {
+			if (filt_type == 0)
+				x_curr = x_curr + (prev_x[index]);
+			else
+				x_curr = x_curr + (prev_x[index] / 3);
+		}
+		if (filt_type == 0)
+			x_curr = x_curr >> 2;
+		else
+			x_curr = x_curr >> 1;
+		ts_curr->x_position = x_curr;
+
+	}
+
+	/* Update previous X and Y values */
+	for (index = (FILTLEN - 1); index > 0; index--) {
+		prev_x[index] = prev_x[index - 1];
+		prev_y[index] = prev_y[index - 1];
+	}
+
+	/*
+	 * Current output will be the most recent past for the
+	 * next sample
+	 */
+	prev_y[0] = y_curr;
+	prev_x[0] = x_curr;
+
+	return 0;
+
+}
+
+/*!
+ * This function retrieves the current touch screen (X,Y) coordinates.
+ *
+ * @param        touch_sample Pointer to touch sample.
+ *
+ * @return       This function returns IMX_ADC_SUCCESS if successful.
+ */
+enum IMX_ADC_STATUS imx_adc_get_touch_sample(struct t_touch_screen
+					     *touch_sample, int wait_tsi)
+{
+	if (imx_adc_read_ts(touch_sample, wait_tsi))
+		return IMX_ADC_ERROR;
+	if (!imx_adc_filter(touch_sample))
+		return IMX_ADC_SUCCESS;
+	else
+		return IMX_ADC_ERROR;
+}
+EXPORT_SYMBOL(imx_adc_get_touch_sample);
+
+/*!
+ * This is the suspend of power management for the i.MX ADC API.
+ * It supports SAVE and POWER_DOWN state.
+ *
+ * @param        pdev           the device
+ * @param        state          the state
+ *
+ * @return       This function returns 0 if successful.
+ */
+static int imx_adc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	if (device_may_wakeup(&pdev->dev)) {
+		enable_irq_wake(adc_data->irq);
+	} else {
+		suspend_flag = 1;
+		tsc_clk_disable();
+	}
+	return 0;
+};
+
+/*!
+ * This is the resume of power management for the i.MX adc API.
+ * It supports RESTORE state.
+ *
+ * @param        pdev           the device
+ *
+ * @return       This function returns 0 if successful.
+ */
+static int imx_adc_resume(struct platform_device *pdev)
+{
+	if (device_may_wakeup(&pdev->dev)) {
+		disable_irq_wake(adc_data->irq);
+	} else {
+		suspend_flag = 0;
+		tsc_clk_enable();
+		while (swait > 0) {
+			swait--;
+			wake_up_interruptible(&suspendq);
+		}
+	}
+	return 0;
+}
+
+/*!
+ * This function implements the open method on an i.MX ADC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int imx_adc_open(struct inode *inode, struct file *file)
+{
+	while (suspend_flag) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, !suspend_flag))
+			return -ERESTARTSYS;
+	}
+	pr_debug("imx_adc : imx_adc_open()\n");
+	return 0;
+}
+
+/*!
+ * This function implements the release method on an i.MX ADC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int imx_adc_free(struct inode *inode, struct file *file)
+{
+	pr_debug("imx_adc : imx_adc_free()\n");
+	return 0;
+}
+
+/*!
+ * This function initializes all ADC registers with default values. This
+ * function also registers the interrupt events.
+ *
+ * @return       This function returns IMX_ADC_SUCCESS if successful.
+ */
+int imx_adc_init(void)
+{
+	int reg;
+
+	pr_debug("imx_adc_init()\n");
+
+	if (suspend_flag)
+		return -EBUSY;
+
+	tsc_clk_enable();
+
+	/* Reset */
+	tsc_self_reset();
+
+	/* Internal reference */
+	tsc_intref_enable();
+
+	/* Set power mode */
+	reg = __raw_readl(tsc_base + TGCR) & ~TGCR_POWER_MASK;
+	reg |= TGCR_POWER_SAVE;
+	__raw_writel(reg, tsc_base + TGCR);
+
+	imx_tsc_init();
+
+	return IMX_ADC_SUCCESS;
+}
+EXPORT_SYMBOL(imx_adc_init);
+
+/*!
+ * This function disables the ADC, de-registers the interrupt events.
+ *
+ * @return       This function returns IMX_ADC_SUCCESS if successful.
+ */
+enum IMX_ADC_STATUS imx_adc_deinit(void)
+{
+	pr_debug("imx_adc_deinit()\n");
+
+	return IMX_ADC_SUCCESS;
+}
+EXPORT_SYMBOL(imx_adc_deinit);
+
+/*!
+ * This function triggers a conversion and returns one sampling result of one
+ * channel.
+ *
+ * @param        channel   The channel to be sampled
+ * @param        result    The pointer to the conversion result. The memory
+ *                         should be allocated by the caller of this function.
+ *
+ * @return       This function returns IMX_ADC_SUCCESS if successful.
+ */
+enum IMX_ADC_STATUS imx_adc_convert(enum t_channel channel,
+				    unsigned short *result)
+{
+	int reg;
+	int lastitemid;
+	struct t_touch_screen touch_sample;
+
+	switch (channel) {
+
+	case TS_X_POS:
+		imx_adc_get_touch_sample(&touch_sample, 0);
+		result[0] = touch_sample.x_position;
+
+		/* if no pen down ,recover the register configuration */
+		if (touch_sample.contact_resistance == 0) {
+			reg = __raw_readl(tsc_base + TCQCR);
+			reg &= ~CQCR_QSM_MASK;
+			reg |= CQCR_QSM_PEN;
+			__raw_writel(reg, tsc_base + TCQCR);
+			reg = __raw_readl(tsc_base + TGCR);
+			reg |= TGCR_PDB_EN | TGCR_PD_EN;
+			__raw_writel(reg, tsc_base + TGCR);
+		}
+		break;
+
+	case TS_Y_POS:
+		imx_adc_get_touch_sample(&touch_sample, 0);
+		result[1] = touch_sample.y_position;
+
+		/* if no pen down ,recover the register configuration */
+		if (touch_sample.contact_resistance == 0) {
+			reg = __raw_readl(tsc_base + TCQCR);
+			reg &= ~CQCR_QSM_MASK;
+			reg |= CQCR_QSM_PEN;
+			__raw_writel(reg, tsc_base + TCQCR);
+
+			reg = __raw_readl(tsc_base + TGCR);
+			reg |= TGCR_PDB_EN | TGCR_PD_EN;
+			__raw_writel(reg, tsc_base + TGCR);
+		}
+		break;
+
+	case GER_PURPOSE_ADC0:
+		down(&general_convert_mutex);
+
+		lastitemid = 0;
+		reg = (0xf << CQCR_FIFOWATERMARK_SHIFT) |
+		      (lastitemid << CQCR_LAST_ITEM_ID_SHIFT) | CQCR_QSM_FQS;
+		__raw_writel(reg, tsc_base + GCQCR);
+
+		reg = TSC_GENERAL_ADC_GCC0;
+		reg |= (3 << CC_NOS_SHIFT) | (16 << CC_SETTLING_TIME_SHIFT);
+		__raw_writel(reg, tsc_base + GCC0);
+
+		imx_adc_read_general(result);
+		up(&general_convert_mutex);
+		break;
+
+	case GER_PURPOSE_ADC1:
+		down(&general_convert_mutex);
+
+		lastitemid = 0;
+		reg = (0xf << CQCR_FIFOWATERMARK_SHIFT) |
+		      (lastitemid << CQCR_LAST_ITEM_ID_SHIFT) | CQCR_QSM_FQS;
+		__raw_writel(reg, tsc_base + GCQCR);
+
+		reg = TSC_GENERAL_ADC_GCC1;
+		reg |= (3 << CC_NOS_SHIFT) | (16 << CC_SETTLING_TIME_SHIFT);
+		__raw_writel(reg, tsc_base + GCC0);
+
+		imx_adc_read_general(result);
+		up(&general_convert_mutex);
+		break;
+
+	case GER_PURPOSE_ADC2:
+		down(&general_convert_mutex);
+
+		lastitemid = 0;
+		reg = (0xf << CQCR_FIFOWATERMARK_SHIFT) |
+		      (lastitemid << CQCR_LAST_ITEM_ID_SHIFT) | CQCR_QSM_FQS;
+		__raw_writel(reg, tsc_base + GCQCR);
+
+		reg = TSC_GENERAL_ADC_GCC2;
+		reg |= (3 << CC_NOS_SHIFT) | (16 << CC_SETTLING_TIME_SHIFT);
+		__raw_writel(reg, tsc_base + GCC0);
+
+		imx_adc_read_general(result);
+		up(&general_convert_mutex);
+		break;
+
+	case GER_PURPOSE_MULTICHNNEL:
+		down(&general_convert_mutex);
+
+		reg = TSC_GENERAL_ADC_GCC0;
+		reg |= (3 << CC_NOS_SHIFT) | (16 << CC_SETTLING_TIME_SHIFT);
+		__raw_writel(reg, tsc_base + GCC0);
+
+		reg = TSC_GENERAL_ADC_GCC1;
+		reg |= (3 << CC_NOS_SHIFT) | (16 << CC_SETTLING_TIME_SHIFT);
+		__raw_writel(reg, tsc_base + GCC1);
+
+		reg = TSC_GENERAL_ADC_GCC2;
+		reg |= (3 << CC_NOS_SHIFT) | (16 << CC_SETTLING_TIME_SHIFT);
+		__raw_writel(reg, tsc_base + GCC2);
+
+		reg = (GCQ_ITEM_GCC2 << GCQ_ITEM2_SHIFT) |
+		      (GCQ_ITEM_GCC1 << GCQ_ITEM1_SHIFT) |
+		      (GCQ_ITEM_GCC0 << GCQ_ITEM0_SHIFT);
+		__raw_writel(reg, tsc_base + GCQ_ITEM_7_0);
+
+		lastitemid = 2;
+		reg = (0xf << CQCR_FIFOWATERMARK_SHIFT) |
+		      (lastitemid << CQCR_LAST_ITEM_ID_SHIFT) | CQCR_QSM_FQS;
+		__raw_writel(reg, tsc_base + GCQCR);
+
+		imx_adc_read_general(result);
+		up(&general_convert_mutex);
+		break;
+	default:
+		pr_debug("%s: bad channel number\n", __func__);
+		return IMX_ADC_ERROR;
+	}
+
+	return IMX_ADC_SUCCESS;
+}
+EXPORT_SYMBOL(imx_adc_convert);
+
+/*!
+ * This function triggers a conversion and returns sampling results of each
+ * specified channel.
+ *
+ * @param        channels  This input parameter is bitmap to specify channels
+ *                         to be sampled.
+ * @param        result    The pointer to array to store sampling results.
+ *                         The memory should be allocated by the caller of this
+ *                         function.
+ *
+ * @return       This function returns IMX_ADC_SUCCESS if successful.
+ */
+enum IMX_ADC_STATUS imx_adc_convert_multichnnel(enum t_channel channels,
+						unsigned short *result)
+{
+	imx_adc_convert(GER_PURPOSE_MULTICHNNEL, result);
+	return IMX_ADC_SUCCESS;
+}
+EXPORT_SYMBOL(imx_adc_convert_multichnnel);
+
+/*!
+ * This function implements IOCTL controls on an i.MX ADC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @param        cmd         the command
+ * @param        arg         the parameter
+ * @return       This function returns 0 if successful.
+ */
+static int imx_adc_ioctl(struct inode *inode, struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	struct t_adc_convert_param *convert_param;
+
+	if ((_IOC_TYPE(cmd) != 'p') && (_IOC_TYPE(cmd) != 'D'))
+		return -ENOTTY;
+
+	while (suspend_flag) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, !suspend_flag))
+			return -ERESTARTSYS;
+	}
+
+	switch (cmd) {
+	case IMX_ADC_INIT:
+		pr_debug("init adc\n");
+		CHECK_ERROR(imx_adc_init());
+		break;
+
+	case IMX_ADC_DEINIT:
+		pr_debug("deinit adc\n");
+		CHECK_ERROR(imx_adc_deinit());
+		break;
+
+	case IMX_ADC_CONVERT:
+		convert_param = kmalloc(sizeof(*convert_param), GFP_KERNEL);
+		if (convert_param == NULL)
+			return -ENOMEM;
+		if (copy_from_user(convert_param,
+				   (struct t_adc_convert_param *)arg,
+				   sizeof(*convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		CHECK_ERROR_KFREE(imx_adc_convert(convert_param->channel,
+						  convert_param->result),
+				  (kfree(convert_param)));
+
+		if (copy_to_user((struct t_adc_convert_param *)arg,
+				 convert_param, sizeof(*convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		kfree(convert_param);
+		break;
+
+	case IMX_ADC_CONVERT_MULTICHANNEL:
+		convert_param = kmalloc(sizeof(*convert_param), GFP_KERNEL);
+		if (convert_param == NULL)
+			return -ENOMEM;
+		if (copy_from_user(convert_param,
+				   (struct t_adc_convert_param *)arg,
+				   sizeof(*convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		CHECK_ERROR_KFREE(imx_adc_convert_multichnnel
+				  (convert_param->channel,
+				   convert_param->result),
+				  (kfree(convert_param)));
+
+		if (copy_to_user((struct t_adc_convert_param *)arg,
+				 convert_param, sizeof(*convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		kfree(convert_param);
+		break;
+
+	default:
+		pr_debug("imx_adc_ioctl: unsupported ioctl command 0x%x\n",
+			 cmd);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static struct file_operations imx_adc_fops = {
+	.owner = THIS_MODULE,
+	.ioctl = imx_adc_ioctl,
+	.open = imx_adc_open,
+	.release = imx_adc_free,
+};
+
+static int imx_adc_module_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	int retval;
+	struct device *temp_class;
+	struct resource *res;
+	void __iomem *base;
+
+	/* ioremap the base address */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "No TSC base address provided\n");
+		goto err_out0;
+	}
+	base = ioremap(res->start, res->end - res->start);
+	if (base == NULL) {
+		dev_err(&pdev->dev, "failed to rebase TSC base address\n");
+		goto err_out0;
+	}
+	tsc_base = (unsigned long)base;
+
+	/* create the chrdev */
+	imx_adc_major = register_chrdev(0, "imx_adc", &imx_adc_fops);
+
+	if (imx_adc_major < 0) {
+		dev_err(&pdev->dev, "Unable to get a major for imx_adc\n");
+		return imx_adc_major;
+	}
+	init_waitqueue_head(&suspendq);
+	init_waitqueue_head(&tsq);
+
+	imx_adc_class = class_create(THIS_MODULE, "imx_adc");
+	if (IS_ERR(imx_adc_class)) {
+		dev_err(&pdev->dev, "Error creating imx_adc class.\n");
+		ret = PTR_ERR(imx_adc_class);
+		goto err_out1;
+	}
+
+	temp_class = device_create(imx_adc_class, NULL,
+				   MKDEV(imx_adc_major, 0), NULL, "imx_adc");
+	if (IS_ERR(temp_class)) {
+		dev_err(&pdev->dev, "Error creating imx_adc class device.\n");
+		ret = PTR_ERR(temp_class);
+		goto err_out2;
+	}
+
+	adc_data = kmalloc(sizeof(struct imx_adc_data), GFP_KERNEL);
+	if (adc_data == NULL)
+		return -ENOMEM;
+	adc_data->irq = platform_get_irq(pdev, 0);
+	retval = request_irq(adc_data->irq, imx_adc_interrupt,
+			     0, MOD_NAME, MOD_NAME);
+	if (retval) {
+		dev_dbg(&pdev->dev, "ADC: request_irq(%d) returned error %d\n",
+			MXC_INT_TSC, retval);
+		return retval;
+	}
+	adc_data->adc_clk = clk_get(&pdev->dev, "tchscrn_clk");
+
+	ret = imx_adc_init();
+
+	if (ret != IMX_ADC_SUCCESS) {
+		dev_err(&pdev->dev, "Error in imx_adc_init.\n");
+		goto err_out4;
+	}
+	imx_adc_ready = 1;
+
+	/* By default, devices should wakeup if they can */
+	/* So TouchScreen is set as "should wakeup" as it can */
+	device_init_wakeup(&pdev->dev, 1);
+
+	pr_info("i.MX ADC at 0x%x irq %d\n", (unsigned int)res->start,
+		adc_data->irq);
+	return ret;
+
+err_out4:
+	device_destroy(imx_adc_class, MKDEV(imx_adc_major, 0));
+err_out2:
+	class_destroy(imx_adc_class);
+err_out1:
+	unregister_chrdev(imx_adc_major, "imx_adc");
+err_out0:
+	return ret;
+}
+
+static int imx_adc_module_remove(struct platform_device *pdev)
+{
+	imx_adc_ready = 0;
+	imx_adc_deinit();
+	device_destroy(imx_adc_class, MKDEV(imx_adc_major, 0));
+	class_destroy(imx_adc_class);
+	unregister_chrdev(imx_adc_major, "imx_adc");
+	free_irq(adc_data->irq, MOD_NAME);
+	kfree(adc_data);
+	pr_debug("i.MX ADC successfully removed\n");
+	return 0;
+}
+
+static struct platform_driver imx_adc_driver = {
+	.driver = {
+		   .name = "imx_adc",
+		   },
+	.suspend = imx_adc_suspend,
+	.resume = imx_adc_resume,
+	.probe = imx_adc_module_probe,
+	.remove = imx_adc_module_remove,
+};
+
+/*
+ * Initialization and Exit
+ */
+static int __init imx_adc_module_init(void)
+{
+	pr_debug("i.MX ADC driver loading...\n");
+	return platform_driver_register(&imx_adc_driver);
+}
+
+static void __exit imx_adc_module_exit(void)
+{
+	platform_driver_unregister(&imx_adc_driver);
+	pr_debug("i.MX ADC driver successfully unloaded\n");
+}
+
+/*
+ * Module entry points
+ */
+
+module_init(imx_adc_module_init);
+module_exit(imx_adc_module_exit);
+
+MODULE_DESCRIPTION("i.MX ADC device driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/adc/imx_adc_reg.h b/drivers/mxc/adc/imx_adc_reg.h
new file mode 100644
index 000000000000..8ae776038fbc
--- /dev/null
+++ b/drivers/mxc/adc/imx_adc_reg.h
@@ -0,0 +1,242 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU Lesser General
+ * Public License.  You may obtain a copy of the GNU Lesser General
+ * Public License Version 2.1 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/lgpl-license.html
+ * http://www.gnu.org/copyleft/lgpl.html
+ */
+
+#ifndef __IMX_ADC_H__
+#define __IMX_ADC_H__
+
+/* TSC General Config Register */
+#define TGCR                  0x000
+#define TGCR_IPG_CLK_EN       (1 << 0)
+#define TGCR_TSC_RST          (1 << 1)
+#define TGCR_FUNC_RST         (1 << 2)
+#define TGCR_SLPC             (1 << 4)
+#define TGCR_STLC             (1 << 5)
+#define TGCR_HSYNC_EN         (1 << 6)
+#define TGCR_HSYNC_POL        (1 << 7)
+#define TGCR_POWERMODE_SHIFT  8
+#define TGCR_POWER_OFF        (0x0 << TGCR_POWERMODE_SHIFT)
+#define TGCR_POWER_SAVE       (0x1 << TGCR_POWERMODE_SHIFT)
+#define TGCR_POWER_ON         (0x3 << TGCR_POWERMODE_SHIFT)
+#define TGCR_POWER_MASK       (0x3 << TGCR_POWERMODE_SHIFT)
+#define TGCR_INTREFEN         (1 << 10)
+#define TGCR_ADCCLKCFG_SHIFT  16
+#define TGCR_PD_EN            (1 << 23)
+#define TGCR_PDB_EN           (1 << 24)
+#define TGCR_PDBTIME_SHIFT    25
+#define TGCR_PDBTIME128       (0x3f << TGCR_PDBTIME_SHIFT)
+#define TGCR_PDBTIME_MASK     (0x7f << TGCR_PDBTIME_SHIFT)
+
+/* TSC General Status Register */
+#define TGSR                  0x004
+#define TCQ_INT               (1 << 0)
+#define GCQ_INT               (1 << 1)
+#define SLP_INT               (1 << 2)
+#define TCQ_DMA               (1 << 16)
+#define GCQ_DMA               (1 << 17)
+
+/* TSC IDLE Config Register */
+#define TICR                  0x008
+
+/* TouchScreen Convert Queue FIFO Register */
+#define TCQFIFO               0x400
+/* TouchScreen Convert Queue Control Register */
+#define TCQCR                 0x404
+#define CQCR_QSM_SHIFT        0
+#define CQCR_QSM_STOP         (0x0 << CQCR_QSM_SHIFT)
+#define CQCR_QSM_PEN          (0x1 << CQCR_QSM_SHIFT)
+#define CQCR_QSM_FQS          (0x2 << CQCR_QSM_SHIFT)
+#define CQCR_QSM_FQS_PEN      (0x3 << CQCR_QSM_SHIFT)
+#define CQCR_QSM_MASK         (0x3 << CQCR_QSM_SHIFT)
+#define CQCR_FQS              (1 << 2)
+#define CQCR_RPT              (1 << 3)
+#define CQCR_LAST_ITEM_ID_SHIFT   4
+#define CQCR_LAST_ITEM_ID_MASK    (0xf << CQCR_LAST_ITEM_ID_SHIFT)
+#define CQCR_FIFOWATERMARK_SHIFT  8
+#define CQCR_FIFOWATERMARK_MASK   (0xf << CQCR_FIFOWATERMARK_SHIFT)
+#define CQCR_REPEATWAIT_SHIFT     12
+#define CQCR_REPEATWAIT_MASK      (0xf << CQCR_REPEATWAIT_SHIFT)
+#define CQCR_QRST             (1 << 16)
+#define CQCR_FRST             (1 << 17)
+#define CQCR_PD_MSK           (1 << 18)
+#define CQCR_PD_CFG           (1 << 19)
+
+/* TouchScreen Convert Queue Status Register */
+#define TCQSR                 0x408
+#define CQSR_PD               (1 << 0)
+#define CQSR_EOQ              (1 << 1)
+#define CQSR_FOR              (1 << 4)
+#define CQSR_FUR              (1 << 5)
+#define CQSR_FER              (1 << 6)
+#define CQSR_EMPT             (1 << 13)
+#define CQSR_FULL             (1 << 14)
+#define CQSR_FDRY             (1 << 15)
+
+/* TouchScreen Convert Queue Mask Register */
+#define TCQMR                 0x40c
+#define TCQMR_PD_IRQ_MSK      (1 << 0)
+#define TCQMR_EOQ_IRQ_MSK     (1 << 1)
+#define TCQMR_FOR_IRQ_MSK     (1 << 4)
+#define TCQMR_FUR_IRQ_MSK     (1 << 5)
+#define TCQMR_FER_IRQ_MSK     (1 << 6)
+#define TCQMR_PD_DMA_MSK      (1 << 16)
+#define TCQMR_EOQ_DMA_MSK     (1 << 17)
+#define TCQMR_FOR_DMA_MSK     (1 << 20)
+#define TCQMR_FUR_DMA_MSK     (1 << 21)
+#define TCQMR_FER_DMA_MSK     (1 << 22)
+#define TCQMR_FDRY_DMA_MSK    (1 << 31)
+
+/* TouchScreen Convert Queue ITEM 7~0 */
+#define TCQ_ITEM_7_0          0x420
+
+/* TouchScreen Convert Queue ITEM 15~8 */
+#define TCQ_ITEM_15_8         0x424
+
+#define TCQ_ITEM7_SHIFT       28
+#define TCQ_ITEM6_SHIFT       24
+#define TCQ_ITEM5_SHIFT       20
+#define TCQ_ITEM4_SHIFT       16
+#define TCQ_ITEM3_SHIFT       12
+#define TCQ_ITEM2_SHIFT       8
+#define TCQ_ITEM1_SHIFT       4
+#define TCQ_ITEM0_SHIFT       0
+
+#define TCQ_ITEM_TCC0         0x0
+#define TCQ_ITEM_TCC1         0x1
+#define TCQ_ITEM_TCC2         0x2
+#define TCQ_ITEM_TCC3         0x3
+#define TCQ_ITEM_TCC4         0x4
+#define TCQ_ITEM_TCC5         0x5
+#define TCQ_ITEM_TCC6         0x6
+#define TCQ_ITEM_TCC7         0x7
+#define TCQ_ITEM_GCC7         0x8
+#define TCQ_ITEM_GCC6         0x9
+#define TCQ_ITEM_GCC5         0xa
+#define TCQ_ITEM_GCC4         0xb
+#define TCQ_ITEM_GCC3         0xc
+#define TCQ_ITEM_GCC2         0xd
+#define TCQ_ITEM_GCC1         0xe
+#define TCQ_ITEM_GCC0         0xf
+
+/* TouchScreen Convert Config 0-7 */
+#define TCC0                  0x440
+#define TCC1                  0x444
+#define TCC2                  0x448
+#define TCC3                  0x44c
+#define TCC4                  0x450
+#define TCC5                  0x454
+#define TCC6                  0x458
+#define TCC7                  0x45c
+#define CC_PEN_IACK           (1 << 1)
+#define CC_SEL_REFN_SHIFT     2
+#define CC_SEL_REFN_YNLR      (0x1 << CC_SEL_REFN_SHIFT)
+#define CC_SEL_REFN_AGND      (0x2 << CC_SEL_REFN_SHIFT)
+#define CC_SEL_REFN_MASK      (0x3 << CC_SEL_REFN_SHIFT)
+#define CC_SELIN_SHIFT        4
+#define CC_SELIN_XPUL         (0x0 << CC_SELIN_SHIFT)
+#define CC_SELIN_YPLL         (0x1 << CC_SELIN_SHIFT)
+#define CC_SELIN_XNUR         (0x2 << CC_SELIN_SHIFT)
+#define CC_SELIN_YNLR         (0x3 << CC_SELIN_SHIFT)
+#define CC_SELIN_WIPER        (0x4 << CC_SELIN_SHIFT)
+#define CC_SELIN_INAUX0       (0x5 << CC_SELIN_SHIFT)
+#define CC_SELIN_INAUX1       (0x6 << CC_SELIN_SHIFT)
+#define CC_SELIN_INAUX2       (0x7 << CC_SELIN_SHIFT)
+#define CC_SELIN_MASK         (0x7 << CC_SELIN_SHIFT)
+#define CC_SELREFP_SHIFT      7
+#define CC_SELREFP_YPLL       (0x0 << CC_SELREFP_SHIFT)
+#define CC_SELREFP_XPUL       (0x1 << CC_SELREFP_SHIFT)
+#define CC_SELREFP_EXT        (0x2 << CC_SELREFP_SHIFT)
+#define CC_SELREFP_INT        (0x3 << CC_SELREFP_SHIFT)
+#define CC_SELREFP_MASK       (0x3 << CC_SELREFP_SHIFT)
+#define CC_XPULSW             (1 << 9)
+#define CC_XNURSW_SHIFT       10
+#define CC_XNURSW_HIGH        (0x0 << CC_XNURSW_SHIFT)
+#define CC_XNURSW_OFF         (0x1 << CC_XNURSW_SHIFT)
+#define CC_XNURSW_LOW         (0x3 << CC_XNURSW_SHIFT)
+#define CC_XNURSW_MASK        (0x3 << CC_XNURSW_SHIFT)
+#define CC_YPLLSW_SHIFT       12
+#define CC_YPLLSW_MASK        (0x3 << CC_YPLLSW_SHIFT)
+#define CC_YNLRSW             (1 << 14)
+#define CC_WIPERSW            (1 << 15)
+#define CC_NOS_SHIFT          16
+#define CC_YPLLSW_HIGH        (0x0 << CC_NOS_SHIFT)
+#define CC_YPLLSW_OFF         (0x1 << CC_NOS_SHIFT)
+#define CC_YPLLSW_LOW         (0x3 << CC_NOS_SHIFT
+#define CC_NOS_MASK           (0xf << CC_NOS_SHIFT)
+#define CC_IGS                (1 << 20)
+#define CC_SETTLING_TIME_SHIFT 24
+#define CC_SETTLING_TIME_MASK (0xff << CC_SETTLING_TIME_SHIFT)
+
+#define TSC_4WIRE_PRECHARGE    0x158c
+#define TSC_4WIRE_TOUCH_DETECT 0x578e
+
+#define TSC_4WIRE_X_MEASUMENT  0x1c90
+#define TSC_4WIRE_Y_MEASUMENT  0x4604
+
+#define TSC_GENERAL_ADC_GCC0   0x17dc
+#define TSC_GENERAL_ADC_GCC1   0x17ec
+#define TSC_GENERAL_ADC_GCC2   0x17fc
+
+/* GeneralADC Convert Queue FIFO Register */
+#define GCQFIFO                0x800
+#define GCQFIFO_ADCOUT_SHIFT   4
+#define GCQFIFO_ADCOUT_MASK    (0xfff << GCQFIFO_ADCOUT_SHIFT)
+/* GeneralADC Convert Queue Control Register */
+#define GCQCR                  0x804
+/* GeneralADC Convert Queue Status Register */
+#define GCQSR                  0x808
+/* GeneralADC Convert Queue Mask Register */
+#define GCQMR                  0x80c
+
+/* GeneralADC Convert Queue ITEM 7~0 */
+#define GCQ_ITEM_7_0           0x820
+/* GeneralADC Convert Queue ITEM 15~8 */
+#define GCQ_ITEM_15_8          0x824
+
+#define GCQ_ITEM7_SHIFT        28
+#define GCQ_ITEM6_SHIFT        24
+#define GCQ_ITEM5_SHIFT        20
+#define GCQ_ITEM4_SHIFT        16
+#define GCQ_ITEM3_SHIFT        12
+#define GCQ_ITEM2_SHIFT        8
+#define GCQ_ITEM1_SHIFT        4
+#define GCQ_ITEM0_SHIFT        0
+
+#define GCQ_ITEM_GCC0          0x0
+#define GCQ_ITEM_GCC1          0x1
+#define GCQ_ITEM_GCC2          0x2
+#define GCQ_ITEM_GCC3          0x3
+
+/* GeneralADC Convert Config 0-7 */
+#define GCC0                   0x840
+#define GCC1                   0x844
+#define GCC2                   0x848
+#define GCC3                   0x84c
+#define GCC4                   0x850
+#define GCC5                   0x854
+#define GCC6                   0x858
+#define GCC7                   0x85c
+
+/* TSC Test Register R/W */
+#define TTR                    0xc00
+/* TSC Monitor Register 1, 2 */
+#define MNT1                   0xc04
+#define MNT2                   0xc04
+
+#define DETECT_ITEM_ID_1       1
+#define DETECT_ITEM_ID_2       5
+#define TS_X_ITEM_ID           2
+#define TS_Y_ITEM_ID           3
+#define TSI_DATA               1
+#define FQS_DATA               0
+
+#endif				/* __IMX_ADC_H__ */
diff --git a/drivers/mxc/asrc/Kconfig b/drivers/mxc/asrc/Kconfig
new file mode 100644
index 000000000000..a4c66b19f067
--- /dev/null
+++ b/drivers/mxc/asrc/Kconfig
@@ -0,0 +1,13 @@
+#
+# ASRC configuration
+#
+
+menu "MXC Asynchronous Sample Rate Converter support"
+
+config MXC_ASRC
+	tristate "ASRC support"
+	depends on ARCH_MX35
+	---help---
+         Say Y to get the ASRC service.
+
+endmenu
diff --git a/drivers/mxc/asrc/Makefile b/drivers/mxc/asrc/Makefile
new file mode 100644
index 000000000000..0d2487d389c7
--- /dev/null
+++ b/drivers/mxc/asrc/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for the kernel Asynchronous Sample Rate Converter driver
+#
+
+ifeq ($(CONFIG_ARCH_MX35),y)
+	obj-$(CONFIG_MXC_ASRC) += mxc_asrc.o
+endif
diff --git a/drivers/mxc/asrc/mxc_asrc.c b/drivers/mxc/asrc/mxc_asrc.c
new file mode 100644
index 000000000000..653dc77540ea
--- /dev/null
+++ b/drivers/mxc/asrc/mxc_asrc.c
@@ -0,0 +1,1687 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_asrc.c
+ *
+ * @brief MXC Asynchronous Sample Rate Converter
+ *
+ * @ingroup SOUND
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/pagemap.h>
+#include <linux/vmalloc.h>
+#include <linux/types.h>
+#include <linux/version.h>
+#include <linux/interrupt.h>
+#include <linux/proc_fs.h>
+#include <linux/dma-mapping.h>
+#include <linux/mxc_asrc.h>
+#include <asm/irq.h>
+#include <asm/memory.h>
+#include <mach/dma.h>
+
+static int asrc_major;
+static struct class *asrc_class;
+#define ASRC_PROC_PATH        "driver/asrc"
+
+#define ASRC_RATIO_DECIMAL_DEPTH 26
+
+DEFINE_SPINLOCK(data_lock);
+DEFINE_SPINLOCK(input_int_lock);
+DEFINE_SPINLOCK(output_int_lock);
+
+#define AICPA		0	/* Input Clock Divider A Offset */
+#define AICDA		3	/* Input Clock Prescaler A Offset */
+#define AICPB           6	/* Input Clock Divider B Offset */
+#define AICDB           9	/* Input Clock Prescaler B Offset */
+#define AOCPA           12	/* Output Clock Divider A Offset */
+#define AOCDA           15	/* Output Clock Prescaler A Offset */
+#define AOCPB           18	/* Output Clock Divider B Offset */
+#define AOCDB           21	/* Output Clock Prescaler B Offset */
+#define AICPC           0	/* Input Clock Divider C Offset */
+#define AICDC           3	/* Input Clock Prescaler C Offset */
+#define AOCDC           6	/* Output Clock Prescaler C Offset */
+#define AOCPC           9	/* Output Clock Divider C Offset */
+
+char *asrc_pair_id[] = {
+	[0] = "ASRC RX PAIR A",
+	[1] = "ASRC TX PAIR A",
+	[2] = "ASRC RX PAIR B",
+	[3] = "ASRC TX PAIR B",
+	[4] = "ASRC RX PAIR C",
+	[5] = "ASRC TX PAIR C",
+};
+
+enum asrc_status {
+	ASRC_ASRSTR_AIDEA = 0x01,
+	ASRC_ASRSTR_AIDEB = 0x02,
+	ASRC_ASRSTR_AIDEC = 0x04,
+	ASRC_ASRSTR_AODFA = 0x08,
+	ASRC_ASRSTR_AODFB = 0x10,
+	ASRC_ASRSTR_AODFC = 0x20,
+	ASRC_ASRSTR_AOLE = 0x40,
+	ASRC_ASRSTR_FPWT = 0x80,
+	ASRC_ASRSTR_AIDUA = 0x100,
+	ASRC_ASRSTR_AIDUB = 0x200,
+	ASRC_ASRSTR_AIDUC = 0x400,
+	ASRC_ASRSTR_AODOA = 0x800,
+	ASRC_ASRSTR_AODOB = 0x1000,
+	ASRC_ASRSTR_AODOC = 0x2000,
+	ASRC_ASRSTR_AIOLA = 0x4000,
+	ASRC_ASRSTR_AIOLB = 0x8000,
+	ASRC_ASRSTR_AIOLC = 0x10000,
+	ASRC_ASRSTR_AOOLA = 0x20000,
+	ASRC_ASRSTR_AOOLB = 0x40000,
+	ASRC_ASRSTR_AOOLC = 0x80000,
+	ASRC_ASRSTR_ATQOL = 0x100000,
+	ASRC_ASRSTR_DSLCNT = 0x200000,
+};
+
+/* Sample rates are aligned with that defined in pcm.h file */
+static const unsigned char asrc_process_table[][8][2] = {
+	/* 32kHz 44.1kHz 48kHz   64kHz   88.2kHz 96kHz  176kHz   192kHz */
+/*5512Hz*/
+	{{0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0},},
+/*8kHz*/
+	{{0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0},},
+/*11025Hz*/
+	{{0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0},},
+/*16kHz*/
+	{{0, 1}, {0, 1}, {0, 1}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0},},
+/*22050Hz*/
+	{{0, 1}, {0, 1}, {0, 1}, {0, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 0},},
+/*32kHz*/
+	{{0, 1}, {0, 1}, {0, 1}, {0, 1}, {0, 1}, {0, 0}, {0, 0}, {0, 0},},
+/*44.1kHz*/
+	{{0, 2}, {0, 1}, {0, 1}, {0, 1}, {0, 1}, {0, 1}, {0, 0}, {0, 0},},
+/*48kHz*/
+	{{0, 2}, {0, 2}, {0, 1}, {0, 1}, {0, 1}, {0, 1}, {0, 0}, {0, 0},},
+/*64kHz*/
+	{{1, 2}, {0, 2}, {0, 2}, {0, 1}, {0, 1}, {0, 1}, {0, 1}, {0, 0},},
+/*88.2kHz*/
+	{{1, 2}, {1, 2}, {1, 2}, {1, 1}, {1, 1}, {1, 1}, {1, 1}, {1, 1},},
+/*96kHz*/
+	{{1, 2}, {1, 2}, {1, 2}, {1, 1}, {1, 1}, {1, 1}, {1, 1}, {1, 1},},
+/*176kHz*/
+	{{2, 2}, {2, 2}, {2, 2}, {2, 1}, {2, 1}, {2, 1}, {2, 1}, {2, 1},},
+/*192kHz*/
+	{{2, 2}, {2, 2}, {2, 2}, {2, 1}, {2, 1}, {2, 1}, {2, 1}, {2, 1},},
+};
+
+static const unsigned char asrc_divider_table[] = {
+/*5500Hz 8kHz 11025Hz 16kHz 22050kHz 32kHz 44.1kHz 48kHz 64kHz 88.2kHz 96kHz 176400Hz 192kHz*/
+	0x07, 0x15, 0x06, 0x14, 0x05, 0x13, 0x04, 0x04, 0x12, 0x03, 0x03, 0x02,
+	0x02,
+};
+
+static struct asrc_data *g_asrc_data;
+static struct proc_dir_entry *proc_asrc;
+static unsigned long asrc_vrt_base_addr;
+static struct mxc_asrc_platform_data *mxc_asrc_data;
+
+static int asrc_set_clock_ratio(enum asrc_pair_index index,
+				int input_sample_rate, int output_sample_rate)
+{
+	int i;
+	int integ = 0;
+	unsigned long reg_val = 0;
+
+	if (output_sample_rate == 0)
+		return -1;
+	while (input_sample_rate >= output_sample_rate) {
+		input_sample_rate -= output_sample_rate;
+		integ++;
+	}
+	reg_val |= (integ << 26);
+
+	for (i = 1; i <= ASRC_RATIO_DECIMAL_DEPTH; i++) {
+		if ((input_sample_rate * 2) >= output_sample_rate) {
+			reg_val |= (1 << (ASRC_RATIO_DECIMAL_DEPTH - i));
+			input_sample_rate =
+			    input_sample_rate * 2 - output_sample_rate;
+		} else
+			input_sample_rate = input_sample_rate << 1;
+
+		if (input_sample_rate == 0)
+			break;
+	}
+
+	__raw_writel(reg_val,
+		     (asrc_vrt_base_addr + ASRC_ASRIDRLA_REG + (index << 3)));
+	__raw_writel((reg_val >> 24),
+		     (asrc_vrt_base_addr + ASRC_ASRIDRHA_REG + (index << 3)));
+	return 0;
+}
+
+static int asrc_set_process_configuration(enum asrc_pair_index index,
+					  int input_sample_rate,
+					  int output_sample_rate)
+{
+	int i = 0, j = 0;
+	unsigned long reg;
+	switch (input_sample_rate) {
+	case 5512:
+		i = 0;
+	case 8000:
+		i = 1;
+		break;
+	case 11025:
+		i = 2;
+		break;
+	case 16000:
+		i = 3;
+		break;
+	case 22050:
+		i = 4;
+		break;
+	case 32000:
+		i = 5;
+		break;
+	case 44100:
+		i = 6;
+		break;
+	case 48000:
+		i = 7;
+		break;
+	case 64000:
+		i = 8;
+		break;
+	case 88200:
+		i = 9;
+		break;
+	case 96000:
+		i = 10;
+		break;
+	case 176400:
+		i = 11;
+		break;
+	case 192000:
+		i = 12;
+		break;
+	default:
+		return -1;
+	}
+
+	switch (output_sample_rate) {
+	case 32000:
+		j = 0;
+		break;
+	case 44100:
+		j = 1;
+		break;
+	case 48000:
+		j = 2;
+		break;
+	case 64000:
+		j = 3;
+		break;
+	case 88200:
+		j = 4;
+		break;
+	case 96000:
+		j = 5;
+		break;
+	case 176400:
+		j = 6;
+		break;
+	case 192000:
+		j = 7;
+		break;
+	default:
+		return -1;
+	}
+
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCFG_REG);
+	reg &= ~(0x0f << (6 + (index << 2)));
+	reg |=
+	    ((asrc_process_table[i][j][0] << (6 + (index << 2))) |
+	     (asrc_process_table[i][j][1] << (8 + (index << 2))));
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCFG_REG);
+
+	return 0;
+}
+
+static int asrc_get_asrck_clock_divider(int sample_rate)
+{
+	int i = 0;
+	switch (sample_rate) {
+	case 5500:
+		i = 0;
+		break;
+	case 8000:
+		i = 1;
+		break;
+	case 11025:
+		i = 2;
+		break;
+	case 16000:
+		i = 3;
+		break;
+	case 22050:
+		i = 4;
+		break;
+	case 32000:
+		i = 5;
+		break;
+	case 44100:
+		i = 6;
+		break;
+	case 48000:
+		i = 7;
+		break;
+	case 64000:
+		i = 8;
+		break;
+	case 88200:
+		i = 9;
+		break;
+	case 96000:
+		i = 10;
+		break;
+	case 176400:
+		i = 11;
+		break;
+	case 192000:
+		i = 12;
+		break;
+	default:
+		return -1;
+	}
+
+	return asrc_divider_table[i];
+}
+
+int asrc_req_pair(int chn_num, enum asrc_pair_index *index)
+{
+	int err = 0;
+	unsigned long lock_flags;
+	spin_lock_irqsave(&data_lock, lock_flags);
+
+	if (chn_num > 2) {
+		if (g_asrc_data->asrc_pair[ASRC_PAIR_C].active
+		    || (chn_num > g_asrc_data->asrc_pair[ASRC_PAIR_C].chn_max))
+			err = -EBUSY;
+		else {
+			*index = ASRC_PAIR_C;
+			g_asrc_data->asrc_pair[ASRC_PAIR_C].chn_num = chn_num;
+			g_asrc_data->asrc_pair[ASRC_PAIR_C].active = 1;
+		}
+	} else {
+		if (g_asrc_data->asrc_pair[ASRC_PAIR_A].active ||
+		    (g_asrc_data->asrc_pair[ASRC_PAIR_A].chn_max == 0)) {
+			if (g_asrc_data->asrc_pair[ASRC_PAIR_B].
+			    active
+			    || (g_asrc_data->asrc_pair[ASRC_PAIR_B].
+				chn_max == 0))
+				err = -EBUSY;
+			else {
+				*index = ASRC_PAIR_B;
+				g_asrc_data->asrc_pair[ASRC_PAIR_B].chn_num = 2;
+				g_asrc_data->asrc_pair[ASRC_PAIR_B].active = 1;
+			}
+		} else {
+			*index = ASRC_PAIR_A;
+			g_asrc_data->asrc_pair[ASRC_PAIR_A].chn_num = 2;
+			g_asrc_data->asrc_pair[ASRC_PAIR_A].active = 1;
+		}
+	}
+	spin_unlock_irqrestore(&data_lock, lock_flags);
+	return err;
+}
+
+EXPORT_SYMBOL(asrc_req_pair);
+
+void asrc_release_pair(enum asrc_pair_index index)
+{
+	unsigned long reg;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&data_lock, lock_flags);
+	g_asrc_data->asrc_pair[index].active = 0;
+	g_asrc_data->asrc_pair[index].overload_error = 0;
+	/********Disable PAIR*************/
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+	reg &= ~(1 << (index + 1));
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+	spin_unlock_irqrestore(&data_lock, lock_flags);
+}
+
+EXPORT_SYMBOL(asrc_release_pair);
+
+int asrc_config_pair(struct asrc_config *config)
+{
+	int err = 0;
+	int reg, tmp, channel_num;
+	unsigned long lock_flags;
+	/* Set the channel number */
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCNCR_REG);
+	spin_lock_irqsave(&data_lock, lock_flags);
+	g_asrc_data->asrc_pair[config->pair].chn_num = config->channel_num;
+	spin_unlock_irqrestore(&data_lock, lock_flags);
+	reg &=
+	    ~((0xFFFFFFFF >> (32 - mxc_asrc_data->channel_bits)) <<
+	      (mxc_asrc_data->channel_bits * config->pair));
+	if (mxc_asrc_data->channel_bits > 3)
+		channel_num = config->channel_num;
+	else
+		channel_num = (config->channel_num + 1) / 2;
+	tmp = channel_num << (mxc_asrc_data->channel_bits * config->pair);
+	reg |= tmp;
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCNCR_REG);
+
+	/* Set the clock source */
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCSR_REG);
+	tmp = ~(0x0f << (config->pair << 2));
+	reg &= tmp;
+	tmp = ~(0x0f << (12 + (config->pair << 2)));
+	reg &= tmp;
+	reg |=
+	    ((config->inclk << (config->pair << 2)) | (config->
+						       outclk << (12 +
+								  (config->
+								   pair <<
+								   2))));
+
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCSR_REG);
+
+	/* default setting */
+	/* automatic selection for processing mode */
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+	reg |= (1 << (20 + config->pair));
+	reg &= ~(1 << (14 + (config->pair << 1)));
+
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRRA_REG);
+	reg &= 0xffbfffff;
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRRA_REG);
+
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+	reg = reg & (~(1 << 23));
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+
+	/* Default Clock Divider Setting */
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCDR1_REG);
+	if (config->pair == ASRC_PAIR_A) {
+		reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCDR1_REG);
+		reg &= 0xfc0fc0;
+		/* Input Part */
+		if ((config->inclk & 0x0f) == INCLK_SPDIF_RX
+		    || (config->inclk & 0x0f) == INCLK_SPDIF_TX) {
+			reg |= 7 << AICPA;
+		} else if ((config->inclk & 0x0f) == INCLK_ASRCK1_CLK) {
+			tmp =
+			    asrc_get_asrck_clock_divider(config->
+							 input_sample_rate);
+			reg |= tmp << AICPA;
+		} else {
+			if (config->word_width == 16 || config->word_width == 8)
+				reg |= 5 << AICPA;
+			else if (config->word_width == 32
+				 || config->word_width == 24)
+				reg |= 6 << AICPA;
+			else
+				err = -EFAULT;
+		}
+		/* Output Part */
+		if ((config->outclk & 0x0f) == OUTCLK_SPDIF_RX
+		    || (config->outclk & 0x0f) == OUTCLK_SPDIF_TX) {
+			reg |= 7 << AOCPA;
+		} else if ((config->outclk & 0x0f) == OUTCLK_ASRCK1_CLK) {
+			tmp =
+			    asrc_get_asrck_clock_divider(config->
+							 output_sample_rate);
+			reg |= tmp << AOCPA;
+		} else {
+			if (config->word_width == 16 || config->word_width == 8)
+				reg |= 5 << AOCPA;
+			else if (config->word_width == 32
+				 || config->word_width == 24)
+				reg |= 6 << AOCPA;
+			else
+				err = -EFAULT;
+		}
+
+		__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCDR1_REG);
+
+	} else if (config->pair == ASRC_PAIR_B) {
+		reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCDR1_REG);
+		reg &= 0x03f03f;
+		/* Input Part */
+		if ((config->inclk & 0x0f) == INCLK_SPDIF_RX
+		    || (config->inclk & 0x0f) == INCLK_SPDIF_TX) {
+			reg |= 7 << AICPB;
+		} else if ((config->inclk & 0x0f) == INCLK_ASRCK1_CLK) {
+			tmp =
+			    asrc_get_asrck_clock_divider(config->
+							 input_sample_rate);
+			reg |= tmp << AICPB;
+		} else {
+			if (config->word_width == 16 || config->word_width == 8)
+				reg |= 5 << AICPB;
+			else if (config->word_width == 32
+				 || config->word_width == 24)
+				reg |= 6 << AICPB;
+			else
+				err = -EFAULT;
+		}
+		/* Output Part */
+		if ((config->outclk & 0x0f) == INCLK_SPDIF_RX
+		    || (config->outclk & 0x0f) == INCLK_SPDIF_TX) {
+			reg |= 7 << AOCPB;
+		} else if ((config->outclk & 0x0f) == INCLK_ASRCK1_CLK) {
+			tmp =
+			    asrc_get_asrck_clock_divider(config->
+							 output_sample_rate);
+			reg |= tmp << AOCPB;
+		} else {
+			if (config->word_width == 16 || config->word_width == 8)
+				reg |= 5 << AOCPB;
+			else if (config->word_width == 32
+				 || config->word_width == 24)
+				reg |= 6 << AOCPB;
+			else
+				err = -EFAULT;
+		}
+
+		__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCDR1_REG);
+
+	} else {
+		reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCDR2_REG);
+		reg &= 0;
+		/* Input Part */
+		if ((config->inclk & 0x0f) == INCLK_SPDIF_RX
+		    || (config->inclk & 0x0f) == INCLK_SPDIF_TX) {
+			reg |= 7 << AICPC;
+		} else if ((config->inclk & 0x0f) == INCLK_ASRCK1_CLK) {
+			tmp =
+			    asrc_get_asrck_clock_divider(config->
+							 input_sample_rate);
+			reg |= tmp << AICPC;
+		} else {
+			if (config->word_width == 16 || config->word_width == 8)
+				reg |= 5 << AICPC;
+			else if (config->word_width == 32
+				 || config->word_width == 24)
+				reg |= 6 << AICPC;
+			else
+				err = -EFAULT;
+		}
+		/* Output Part */
+		if ((config->outclk & 0x0f) == INCLK_SPDIF_RX
+		    || (config->outclk & 0x0f) == INCLK_SPDIF_TX) {
+			reg |= 7 << AOCPC;
+		} else if ((config->outclk & 0x0f) == INCLK_ASRCK1_CLK) {
+			tmp =
+			    asrc_get_asrck_clock_divider(config->
+							 output_sample_rate);
+			reg |= tmp << AOCPC;
+		} else {
+			if (config->word_width == 16 || config->word_width == 8)
+				reg |= 5 << AOCPC;
+			else if (config->word_width == 32
+				 || config->word_width == 24)
+				reg |= 6 << AOCPC;
+			else
+				err = -EFAULT;
+		}
+		__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCDR2_REG);
+
+	}
+
+	/* check whether ideal ratio is a must */
+	if ((config->inclk & 0x0f) == INCLK_NONE) {
+		reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+		reg &= ~(1 << (20 + config->pair));
+		reg |= (0x03 << (13 + (config->pair << 1)));
+		__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+		err = asrc_set_clock_ratio(config->pair,
+					   config->input_sample_rate,
+					   config->output_sample_rate);
+		if (err < 0)
+			return err;
+
+		err = asrc_set_process_configuration(config->pair,
+						     config->input_sample_rate,
+						     config->
+						     output_sample_rate);
+		if (err < 0)
+			return err;
+	} else if ((config->inclk & 0x0f) == INCLK_ASRCK1_CLK) {
+		if (config->input_sample_rate == 44100
+		    || config->input_sample_rate == 88200) {
+			pr_info
+			    ("ASRC core clock cann't support sample rate %d\n",
+			     config->input_sample_rate);
+			err = -EFAULT;
+		}
+	} else if ((config->outclk & 0x0f) == OUTCLK_ASRCK1_CLK) {
+		if (config->output_sample_rate == 44100
+		    || config->output_sample_rate == 88200) {
+			pr_info
+			    ("ASRC core clock cann't support sample rate %d\n",
+			     config->input_sample_rate);
+			err = -EFAULT;
+		}
+	}
+
+	return err;
+}
+
+EXPORT_SYMBOL(asrc_config_pair);
+
+void asrc_start_conv(enum asrc_pair_index index)
+{
+	int reg, reg_1;
+	unsigned long lock_flags;
+	int i;
+
+	spin_lock_irqsave(&data_lock, lock_flags);
+
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+	if ((reg & 0x0E) == 0)
+		clk_enable(mxc_asrc_data->asrc_audio_clk);
+	reg |= (1 << (1 + index));
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCFG_REG);
+	while (!(reg & (1 << (index + 21))))
+		reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCFG_REG);
+	reg_1 = __raw_readl(asrc_vrt_base_addr + ASRC_ASRSTR_REG);
+
+	reg = 0;
+	for (i = 0; i < 20; i++) {
+		__raw_writel(reg,
+			     asrc_vrt_base_addr + ASRC_ASRDIA_REG +
+			     (index << 3));
+		__raw_writel(reg,
+			     asrc_vrt_base_addr + ASRC_ASRDIA_REG +
+			     (index << 3));
+		__raw_writel(reg,
+			     asrc_vrt_base_addr + ASRC_ASRDIA_REG +
+			     (index << 3));
+		__raw_writel(reg,
+			     asrc_vrt_base_addr + ASRC_ASRDIA_REG +
+			     (index << 3));
+		__raw_writel(reg,
+			     asrc_vrt_base_addr + ASRC_ASRDIA_REG +
+			     (index << 3));
+		__raw_writel(reg,
+			     asrc_vrt_base_addr + ASRC_ASRDIA_REG +
+			     (index << 3));
+		__raw_writel(reg,
+			     asrc_vrt_base_addr + ASRC_ASRDIA_REG +
+			     (index << 3));
+		__raw_writel(reg,
+			     asrc_vrt_base_addr + ASRC_ASRDIA_REG +
+			     (index << 3));
+	}
+
+	__raw_writel(0x40, asrc_vrt_base_addr + ASRC_ASRIER_REG);
+	spin_unlock_irqrestore(&data_lock, lock_flags);
+	return;
+}
+
+EXPORT_SYMBOL(asrc_start_conv);
+
+void asrc_stop_conv(enum asrc_pair_index index)
+{
+	int reg;
+	unsigned long lock_flags;
+	spin_lock_irqsave(&data_lock, lock_flags);
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+	reg &= ~(1 << (1 + index));
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+	if ((reg & 0x0E) == 0)
+		clk_disable(mxc_asrc_data->asrc_audio_clk);
+	spin_unlock_irqrestore(&data_lock, lock_flags);
+	return;
+}
+
+EXPORT_SYMBOL(asrc_stop_conv);
+
+/*!
+ * @brief asrc interrupt handler
+ */
+static irqreturn_t asrc_isr(int irq, void *dev_id)
+{
+	unsigned long status;
+	int reg = 0x40;
+
+	status = __raw_readl(asrc_vrt_base_addr + ASRC_ASRSTR_REG);
+	if (g_asrc_data->asrc_pair[ASRC_PAIR_A].active == 1) {
+		if (status & ASRC_ASRSTR_ATQOL)
+			g_asrc_data->asrc_pair[ASRC_PAIR_A].overload_error |=
+			    ASRC_TASK_Q_OVERLOAD;
+		if (status & ASRC_ASRSTR_AOOLA)
+			g_asrc_data->asrc_pair[ASRC_PAIR_A].overload_error |=
+			    ASRC_OUTPUT_TASK_OVERLOAD;
+		if (status & ASRC_ASRSTR_AIOLA)
+			g_asrc_data->asrc_pair[ASRC_PAIR_A].overload_error |=
+			    ASRC_INPUT_TASK_OVERLOAD;
+		if (status & ASRC_ASRSTR_AODOA)
+			g_asrc_data->asrc_pair[ASRC_PAIR_A].overload_error |=
+			    ASRC_OUTPUT_BUFFER_OVERFLOW;
+		if (status & ASRC_ASRSTR_AIDUA)
+			g_asrc_data->asrc_pair[ASRC_PAIR_A].overload_error |=
+			    ASRC_INPUT_BUFFER_UNDERRUN;
+	} else if (g_asrc_data->asrc_pair[ASRC_PAIR_B].active == 1) {
+		if (status & ASRC_ASRSTR_ATQOL)
+			g_asrc_data->asrc_pair[ASRC_PAIR_B].overload_error |=
+			    ASRC_TASK_Q_OVERLOAD;
+		if (status & ASRC_ASRSTR_AOOLB)
+			g_asrc_data->asrc_pair[ASRC_PAIR_B].overload_error |=
+			    ASRC_OUTPUT_TASK_OVERLOAD;
+		if (status & ASRC_ASRSTR_AIOLB)
+			g_asrc_data->asrc_pair[ASRC_PAIR_B].overload_error |=
+			    ASRC_INPUT_TASK_OVERLOAD;
+		if (status & ASRC_ASRSTR_AODOB)
+			g_asrc_data->asrc_pair[ASRC_PAIR_B].overload_error |=
+			    ASRC_OUTPUT_BUFFER_OVERFLOW;
+		if (status & ASRC_ASRSTR_AIDUB)
+			g_asrc_data->asrc_pair[ASRC_PAIR_B].overload_error |=
+			    ASRC_INPUT_BUFFER_UNDERRUN;
+	} else if (g_asrc_data->asrc_pair[ASRC_PAIR_C].active == 1) {
+		if (status & ASRC_ASRSTR_ATQOL)
+			g_asrc_data->asrc_pair[ASRC_PAIR_C].overload_error |=
+			    ASRC_TASK_Q_OVERLOAD;
+		if (status & ASRC_ASRSTR_AOOLC)
+			g_asrc_data->asrc_pair[ASRC_PAIR_C].overload_error |=
+			    ASRC_OUTPUT_TASK_OVERLOAD;
+		if (status & ASRC_ASRSTR_AIOLC)
+			g_asrc_data->asrc_pair[ASRC_PAIR_C].overload_error |=
+			    ASRC_INPUT_TASK_OVERLOAD;
+		if (status & ASRC_ASRSTR_AODOC)
+			g_asrc_data->asrc_pair[ASRC_PAIR_C].overload_error |=
+			    ASRC_OUTPUT_BUFFER_OVERFLOW;
+		if (status & ASRC_ASRSTR_AIDUC)
+			g_asrc_data->asrc_pair[ASRC_PAIR_C].overload_error |=
+			    ASRC_INPUT_BUFFER_UNDERRUN;
+	}
+
+	/* try to clean the overload error  */
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRSTR_REG);
+
+	return IRQ_HANDLED;
+}
+
+void asrc_get_status(struct asrc_status_flags *flags)
+{
+	unsigned long lock_flags;
+	enum asrc_pair_index index;
+
+	spin_lock_irqsave(&data_lock, lock_flags);
+	index = flags->index;
+	flags->overload_error = g_asrc_data->asrc_pair[index].overload_error;
+
+	spin_unlock_irqrestore(&data_lock, lock_flags);
+	return;
+}
+
+EXPORT_SYMBOL(asrc_get_status);
+
+static int mxc_init_asrc(void)
+{
+	/* Halt ASRC internal FP when input FIFO needs data for pair A, B, C */
+	__raw_writel(0x0001, asrc_vrt_base_addr + ASRC_ASRCTR_REG);
+
+	/* Enable overflow interrupt */
+	__raw_writel(0x00, asrc_vrt_base_addr + ASRC_ASRIER_REG);
+
+	/* Default 6: 2: 2 channel assignment */
+	__raw_writel((0x06 << mxc_asrc_data->channel_bits *
+		      2) | (0x02 << mxc_asrc_data->channel_bits) | 0x02,
+		     asrc_vrt_base_addr + ASRC_ASRCNCR_REG);
+
+	/* Parameter Registers recommended settings */
+	__raw_writel(0x7fffff, asrc_vrt_base_addr + ASRC_ASRPM1_REG);
+	__raw_writel(0x255555, asrc_vrt_base_addr + ASRC_ASRPM2_REG);
+	__raw_writel(0xff7280, asrc_vrt_base_addr + ASRC_ASRPM3_REG);
+	__raw_writel(0xff7280, asrc_vrt_base_addr + ASRC_ASRPM4_REG);
+	__raw_writel(0xff7280, asrc_vrt_base_addr + ASRC_ASRPM5_REG);
+
+	__raw_writel(0x001f00, asrc_vrt_base_addr + ASRC_ASRTFR1);
+
+	/* Set the processing clock for 76KHz, 133M  */
+	__raw_writel(0x06D6, asrc_vrt_base_addr + ASRC_ASR76K_REG);
+
+	/* Set the processing clock for 56KHz, 133M */
+	__raw_writel(0x0947, asrc_vrt_base_addr + ASRC_ASR56K_REG);
+
+	if (request_irq(MXC_INT_ASRC, asrc_isr, 0, "asrc", NULL))
+		return -1;
+
+	return 0;
+}
+
+static int asrc_get_output_buffer_size(int input_buffer_size,
+				       int input_sample_rate,
+				       int output_sample_rate)
+{
+	int i = 0;
+	int outbuffer_size = 0;
+	int outsample = output_sample_rate;
+	while (outsample >= input_sample_rate) {
+		++i;
+		outsample -= input_sample_rate;
+	}
+	outbuffer_size = i * input_buffer_size;
+	i = 1;
+	while (((input_buffer_size >> i) > 2) && (outsample != 0)) {
+		if (((outsample << 1) - input_sample_rate) >= 0) {
+			outsample = (outsample << 1) - input_sample_rate;
+			outbuffer_size += (input_buffer_size >> i);
+		} else {
+			outsample = outsample << 1;
+		}
+		i++;
+	}
+	outbuffer_size = (outbuffer_size >> 3) << 3;
+	return outbuffer_size;
+}
+
+static void asrc_input_dma_callback(void *data, int error, unsigned int count)
+{
+	struct asrc_pair_params *params;
+	struct dma_block *block;
+	mxc_dma_requestbuf_t dma_request;
+	unsigned long lock_flags;
+
+	params = data;
+
+	spin_lock_irqsave(&input_int_lock, lock_flags);
+	params->input_queue_empty--;
+	if (!list_empty(&params->input_queue)) {
+		block =
+		    list_entry(params->input_queue.next,
+			       struct dma_block, queue);
+		dma_request.src_addr = (dma_addr_t) block->dma_paddr;
+		dma_request.dst_addr =
+		    (ASRC_BASE_ADDR + ASRC_ASRDIA_REG + (params->index << 3));
+		dma_request.num_of_bytes = block->length;
+		mxc_dma_config(params->input_dma_channel, &dma_request,
+			       1, MXC_DMA_MODE_WRITE);
+		list_del(params->input_queue.next);
+		list_add_tail(&block->queue, &params->input_done_queue);
+		params->input_queue_empty++;
+	}
+	params->input_counter++;
+	wake_up_interruptible(&params->input_wait_queue);
+	spin_unlock_irqrestore(&input_int_lock, lock_flags);
+	return;
+}
+
+static void asrc_output_dma_callback(void *data, int error, unsigned int count)
+{
+	struct asrc_pair_params *params;
+	struct dma_block *block;
+	mxc_dma_requestbuf_t dma_request;
+	unsigned long lock_flags;
+
+	params = data;
+
+	spin_lock_irqsave(&output_int_lock, lock_flags);
+	params->output_queue_empty--;
+
+	if (!list_empty(&params->output_queue)) {
+		block =
+		    list_entry(params->output_queue.next,
+			       struct dma_block, queue);
+		dma_request.src_addr =
+		    (ASRC_BASE_ADDR + ASRC_ASRDOA_REG + (params->index << 3));
+		dma_request.dst_addr = (dma_addr_t) block->dma_paddr;
+		dma_request.num_of_bytes = block->length;
+		mxc_dma_config(params->output_dma_channel, &dma_request,
+			       1, MXC_DMA_MODE_READ);
+		list_del(params->output_queue.next);
+		list_add_tail(&block->queue, &params->output_done_queue);
+		params->output_queue_empty++;
+	}
+	params->output_counter++;
+	wake_up_interruptible(&params->output_wait_queue);
+	spin_unlock_irqrestore(&output_int_lock, lock_flags);
+	return;
+}
+
+static void mxc_free_dma_buf(struct asrc_pair_params *params)
+{
+	int i;
+	for (i = 0; i < ASRC_DMA_BUFFER_NUM; i++) {
+		if (params->input_dma[i].dma_vaddr != NULL) {
+			dma_free_coherent(0,
+					  params->input_buffer_size,
+					  params->input_dma[i].
+					  dma_vaddr,
+					  params->input_dma[i].dma_paddr);
+			params->input_dma[i].dma_vaddr = NULL;
+		}
+		if (params->output_dma[i].dma_vaddr != NULL) {
+			dma_free_coherent(0,
+					  params->output_buffer_size,
+					  params->output_dma[i].
+					  dma_vaddr,
+					  params->output_dma[i].dma_paddr);
+			params->output_dma[i].dma_vaddr = NULL;
+		}
+	}
+
+	return;
+}
+
+static int mxc_allocate_dma_buf(struct asrc_pair_params *params)
+{
+	int i;
+	for (i = 0; i < ASRC_DMA_BUFFER_NUM; i++) {
+		params->input_dma[i].dma_vaddr =
+		    dma_alloc_coherent(0, params->input_buffer_size,
+				       &params->input_dma[i].dma_paddr,
+				       GFP_DMA | GFP_KERNEL);
+		if (params->input_dma[i].dma_vaddr == NULL) {
+			mxc_free_dma_buf(params);
+			pr_info("can't allocate buff\n");
+			return -ENOBUFS;
+		}
+	}
+	for (i = 0; i < ASRC_DMA_BUFFER_NUM; i++) {
+		params->output_dma[i].dma_vaddr =
+		    dma_alloc_coherent(0,
+				       params->output_buffer_size,
+				       &params->output_dma[i].dma_paddr,
+				       GFP_DMA | GFP_KERNEL);
+		if (params->output_dma[i].dma_vaddr == NULL) {
+			mxc_free_dma_buf(params);
+			return -ENOBUFS;
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * asrc interface - ioctl function
+ *
+ * @param inode      struct inode *
+ *
+ * @param file       struct file *
+ *
+ * @param cmd    unsigned int
+ *
+ * @param arg        unsigned long
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int asrc_ioctl(struct inode *inode, struct file *file,
+		      unsigned int cmd, unsigned long arg)
+{
+	int err = 0;
+	struct asrc_pair_params *params;
+	params = file->private_data;
+
+	if (down_interruptible(&params->busy_lock))
+		return -EBUSY;
+	switch (cmd) {
+	case ASRC_REQ_PAIR:
+		{
+			struct asrc_req req;
+			if (copy_from_user(&req, (void __user *)arg,
+					   sizeof(struct asrc_req))) {
+				err = -EFAULT;
+				break;
+			}
+			err = asrc_req_pair(req.chn_num, &req.index);
+			if (err < 0)
+				break;
+			params->pair_hold = 1;
+			params->index = req.index;
+			if (copy_to_user
+			    ((void __user *)arg, &req, sizeof(struct asrc_req)))
+				err = -EFAULT;
+
+			break;
+		}
+	case ASRC_CONFIG_PAIR:
+		{
+			struct asrc_config config;
+			mxc_dma_device_t rx_id, tx_id;
+			char *rx_name, *tx_name;
+			int channel = -1;
+			if (copy_from_user
+			    (&config, (void __user *)arg,
+			     sizeof(struct asrc_config))) {
+				err = -EFAULT;
+				break;
+			}
+			err = asrc_config_pair(&config);
+			if (err < 0)
+				break;
+			params->output_buffer_size =
+			    asrc_get_output_buffer_size(config.
+							dma_buffer_size,
+							config.
+							input_sample_rate,
+							config.
+							output_sample_rate);
+			params->input_buffer_size = config.dma_buffer_size;
+			if (config.buffer_num > ASRC_DMA_BUFFER_NUM)
+				params->buffer_num = ASRC_DMA_BUFFER_NUM;
+			else
+				params->buffer_num = config.buffer_num;
+			err = mxc_allocate_dma_buf(params);
+			if (err < 0)
+				break;
+
+			/* TBD - need to update when new SDMA interface ready */
+			if (config.pair == ASRC_PAIR_A) {
+				rx_id = MXC_DMA_ASRC_A_RX;
+				tx_id = MXC_DMA_ASRC_A_TX;
+				rx_name = asrc_pair_id[0];
+				tx_name = asrc_pair_id[1];
+			} else if (config.pair == ASRC_PAIR_B) {
+				rx_id = MXC_DMA_ASRC_B_RX;
+				tx_id = MXC_DMA_ASRC_B_TX;
+				rx_name = asrc_pair_id[2];
+				tx_name = asrc_pair_id[3];
+			} else {
+				rx_id = MXC_DMA_ASRC_C_RX;
+				tx_id = MXC_DMA_ASRC_C_TX;
+				rx_name = asrc_pair_id[4];
+				tx_name = asrc_pair_id[5];
+			}
+			channel = mxc_dma_request(rx_id, rx_name);
+			params->input_dma_channel = channel;
+			err = mxc_dma_callback_set(channel, (mxc_dma_callback_t)
+						   asrc_input_dma_callback,
+						   (void *)params);
+			channel = mxc_dma_request(tx_id, tx_name);
+			params->output_dma_channel = channel;
+			err = mxc_dma_callback_set(channel, (mxc_dma_callback_t)
+						   asrc_output_dma_callback,
+						   (void *)params);
+			/* TBD - need to update when new SDMA interface ready */
+			params->input_queue_empty = 0;
+			params->output_queue_empty = 0;
+			INIT_LIST_HEAD(&params->input_queue);
+			INIT_LIST_HEAD(&params->input_done_queue);
+			INIT_LIST_HEAD(&params->output_queue);
+			INIT_LIST_HEAD(&params->output_done_queue);
+			init_waitqueue_head(&params->input_wait_queue);
+			init_waitqueue_head(&params->output_wait_queue);
+
+			if (copy_to_user
+			    ((void __user *)arg, &config,
+			     sizeof(struct asrc_config)))
+				err = -EFAULT;
+			break;
+		}
+	case ASRC_QUERYBUF:
+		{
+			struct asrc_querybuf buffer;
+			if (copy_from_user
+			    (&buffer, (void __user *)arg,
+			     sizeof(struct asrc_querybuf))) {
+				err = -EFAULT;
+				break;
+			}
+			buffer.input_offset =
+			    (unsigned long)params->input_dma[buffer.
+							     buffer_index].
+			    dma_paddr;
+			buffer.input_length = params->input_buffer_size;
+			buffer.output_offset =
+			    (unsigned long)params->output_dma[buffer.
+							      buffer_index].
+			    dma_paddr;
+			buffer.output_length = params->output_buffer_size;
+			if (copy_to_user
+			    ((void __user *)arg, &buffer,
+			     sizeof(struct asrc_querybuf)))
+				err = -EFAULT;
+			break;
+		}
+	case ASRC_RELEASE_PAIR:
+		{
+			enum asrc_pair_index index;
+			if (copy_from_user
+			    (&index, (void __user *)arg,
+			     sizeof(enum asrc_pair_index))) {
+				err = -EFAULT;
+				break;
+			}
+
+			mxc_dma_free(params->input_dma_channel);
+			mxc_dma_free(params->output_dma_channel);
+			mxc_free_dma_buf(params);
+			asrc_release_pair(index);
+			params->pair_hold = 0;
+			break;
+		}
+	case ASRC_Q_INBUF:
+		{
+			struct asrc_buffer buf;
+			struct dma_block *block;
+			mxc_dma_requestbuf_t dma_request;
+			unsigned long lock_flags;
+			if (copy_from_user
+			    (&buf, (void __user *)arg,
+			     sizeof(struct asrc_buffer))) {
+				err = -EFAULT;
+				break;
+			}
+			spin_lock_irqsave(&input_int_lock, lock_flags);
+			params->input_dma[buf.index].index = buf.index;
+			params->input_dma[buf.index].length = buf.length;
+			list_add_tail(&params->input_dma[buf.index].
+				      queue, &params->input_queue);
+			if (params->asrc_active == 0
+			    || params->input_queue_empty == 0) {
+				block =
+				    list_entry(params->input_queue.next,
+					       struct dma_block, queue);
+				dma_request.src_addr =
+				    (dma_addr_t) block->dma_paddr;
+				dma_request.dst_addr =
+				    (ASRC_BASE_ADDR + ASRC_ASRDIA_REG +
+				     (params->index << 3));
+				dma_request.num_of_bytes = block->length;
+				mxc_dma_config(params->
+					       input_dma_channel,
+					       &dma_request, 1,
+					       MXC_DMA_MODE_WRITE);
+				params->input_queue_empty++;
+				list_del(params->input_queue.next);
+				list_add_tail(&block->queue,
+					      &params->input_done_queue);
+			}
+
+			spin_unlock_irqrestore(&input_int_lock, lock_flags);
+			break;
+		}
+	case ASRC_DQ_INBUF:{
+			struct asrc_buffer buf;
+			struct dma_block *block;
+			unsigned long lock_flags;
+			if (copy_from_user
+			    (&buf, (void __user *)arg,
+			     sizeof(struct asrc_buffer))) {
+				err = -EFAULT;
+				break;
+			}
+			/* if ASRC is inactive, nonsense to DQ buffer */
+			if (params->asrc_active == 0) {
+				err = -EFAULT;
+				buf.buf_valid = ASRC_BUF_NA;
+				if (copy_to_user
+				    ((void __user *)arg, &buf,
+				     sizeof(struct asrc_buffer)))
+					err = -EFAULT;
+				break;
+			}
+
+			if (!wait_event_interruptible_timeout
+			    (params->input_wait_queue,
+			     params->input_counter != 0, 10 * HZ)) {
+				pr_info
+				    ("ASRC_DQ_INBUF timeout counter %x\n",
+				     params->input_counter);
+				err = -ETIME;
+				break;
+			} else if (signal_pending(current)) {
+				pr_info("ASRC_DQ_INBUF interrupt received\n");
+				err = -ERESTARTSYS;
+				break;
+			}
+			spin_lock_irqsave(&input_int_lock, lock_flags);
+			params->input_counter--;
+			block =
+			    list_entry(params->input_done_queue.next,
+				       struct dma_block, queue);
+			list_del(params->input_done_queue.next);
+			spin_unlock_irqrestore(&input_int_lock, lock_flags);
+			buf.index = block->index;
+			buf.length = block->length;
+			buf.buf_valid = ASRC_BUF_AV;
+			if (copy_to_user
+			    ((void __user *)arg, &buf,
+			     sizeof(struct asrc_buffer)))
+				err = -EFAULT;
+
+			break;
+		}
+	case ASRC_Q_OUTBUF:{
+			struct asrc_buffer buf;
+			struct dma_block *block;
+			mxc_dma_requestbuf_t dma_request;
+			unsigned long lock_flags;
+			if (copy_from_user
+			    (&buf, (void __user *)arg,
+			     sizeof(struct asrc_buffer))) {
+				err = -EFAULT;
+				break;
+			}
+			spin_lock_irqsave(&output_int_lock, lock_flags);
+			params->output_dma[buf.index].index = buf.index;
+			params->output_dma[buf.index].length = buf.length;
+			list_add_tail(&params->output_dma[buf.index].
+				      queue, &params->output_queue);
+			if (params->asrc_active == 0
+			    || params->output_queue_empty == 0) {
+				block =
+				    list_entry(params->output_queue.
+					       next, struct dma_block, queue);
+				dma_request.src_addr =
+				    (ASRC_BASE_ADDR + ASRC_ASRDOA_REG +
+				     (params->index << 3));
+				dma_request.dst_addr =
+				    (dma_addr_t) block->dma_paddr;
+				dma_request.num_of_bytes = block->length;
+				mxc_dma_config(params->
+					       output_dma_channel,
+					       &dma_request, 1,
+					       MXC_DMA_MODE_READ);
+				list_del(params->output_queue.next);
+				list_add_tail(&block->queue,
+					      &params->output_done_queue);
+				params->output_queue_empty++;
+			}
+
+			spin_unlock_irqrestore(&output_int_lock, lock_flags);
+			break;
+		}
+	case ASRC_DQ_OUTBUF:{
+			struct asrc_buffer buf;
+			struct dma_block *block;
+			unsigned long lock_flags;
+			if (copy_from_user
+			    (&buf, (void __user *)arg,
+			     sizeof(struct asrc_buffer))) {
+				err = -EFAULT;
+				break;
+			}
+			/* if ASRC is inactive, nonsense to DQ buffer */
+			if (params->asrc_active == 0) {
+				buf.buf_valid = ASRC_BUF_NA;
+				err = -EFAULT;
+				if (copy_to_user
+				    ((void __user *)arg, &buf,
+				     sizeof(struct asrc_buffer)))
+					err = -EFAULT;
+				break;
+			}
+
+			if (!wait_event_interruptible_timeout
+			    (params->output_wait_queue,
+			     params->output_counter != 0, 10 * HZ)) {
+				pr_info
+				    ("ASRC_DQ_OUTBUF timeout counter %x\n",
+				     params->output_counter);
+				err = -ETIME;
+				break;
+			} else if (signal_pending(current)) {
+				pr_info("ASRC_DQ_INBUF interrupt received\n");
+				err = -ERESTARTSYS;
+				break;
+			}
+			spin_lock_irqsave(&output_int_lock, lock_flags);
+			params->output_counter--;
+			block =
+			    list_entry(params->output_done_queue.next,
+				       struct dma_block, queue);
+			list_del(params->output_done_queue.next);
+			spin_unlock_irqrestore(&output_int_lock, lock_flags);
+			buf.index = block->index;
+			buf.length = block->length;
+			buf.buf_valid = ASRC_BUF_AV;
+			if (copy_to_user
+			    ((void __user *)arg, &buf,
+			     sizeof(struct asrc_buffer)))
+				err = -EFAULT;
+
+			break;
+		}
+	case ASRC_START_CONV:{
+			enum asrc_pair_index index;
+			unsigned long lock_flags;
+			if (copy_from_user
+			    (&index, (void __user *)arg,
+			     sizeof(enum asrc_pair_index))) {
+				err = -EFAULT;
+				break;
+			}
+
+			spin_lock_irqsave(&input_int_lock, lock_flags);
+			if (params->input_queue_empty == 0) {
+				err = -EFAULT;
+				pr_info
+				    ("ASRC_START_CONV - no block available\n");
+				break;
+			}
+			spin_unlock_irqrestore(&input_int_lock, lock_flags);
+			params->asrc_active = 1;
+
+			asrc_start_conv(index);
+			mxc_dma_enable(params->input_dma_channel);
+			mxc_dma_enable(params->output_dma_channel);
+			break;
+		}
+	case ASRC_STOP_CONV:{
+			enum asrc_pair_index index;
+			if (copy_from_user
+			    (&index, (void __user *)arg,
+			     sizeof(enum asrc_pair_index))) {
+				err = -EFAULT;
+				break;
+			}
+			mxc_dma_disable(params->input_dma_channel);
+			mxc_dma_disable(params->output_dma_channel);
+			asrc_stop_conv(index);
+			params->asrc_active = 0;
+			break;
+		}
+	case ASRC_STATUS:{
+			struct asrc_status_flags flags;
+			if (copy_from_user
+			    (&flags, (void __user *)arg,
+			     sizeof(struct asrc_status_flags))) {
+				err = -EFAULT;
+				break;
+			}
+			asrc_get_status(&flags);
+			if (copy_to_user
+			    ((void __user *)arg, &flags,
+			     sizeof(struct asrc_status_flags)))
+				err = -EFAULT;
+			break;
+		}
+	case ASRC_FLUSH:{
+			/* flush input dma buffer */
+			unsigned long lock_flags;
+			mxc_dma_device_t rx_id, tx_id;
+			char *rx_name, *tx_name;
+			int channel = -1;
+			spin_lock_irqsave(&input_int_lock, lock_flags);
+			while (!list_empty(&params->input_queue))
+				list_del(params->input_queue.next);
+			while (!list_empty(&params->input_done_queue))
+				list_del(params->input_done_queue.next);
+			params->input_counter = 0;
+			params->input_queue_empty = 0;
+			spin_unlock_irqrestore(&input_int_lock, lock_flags);
+
+			/* flush output dma buffer */
+			spin_lock_irqsave(&output_int_lock, lock_flags);
+			while (!list_empty(&params->output_queue))
+				list_del(params->output_queue.next);
+			while (!list_empty(&params->output_done_queue))
+				list_del(params->output_done_queue.next);
+			params->output_counter = 0;
+			params->output_queue_empty = 0;
+			spin_unlock_irqrestore(&output_int_lock, lock_flags);
+
+			/* release DMA and request again */
+			mxc_dma_free(params->input_dma_channel);
+			mxc_dma_free(params->output_dma_channel);
+			if (params->index == ASRC_PAIR_A) {
+				rx_id = MXC_DMA_ASRC_A_RX;
+				tx_id = MXC_DMA_ASRC_A_TX;
+				rx_name = asrc_pair_id[0];
+				tx_name = asrc_pair_id[1];
+			} else if (params->index == ASRC_PAIR_B) {
+				rx_id = MXC_DMA_ASRC_B_RX;
+				tx_id = MXC_DMA_ASRC_B_TX;
+				rx_name = asrc_pair_id[2];
+				tx_name = asrc_pair_id[3];
+			} else {
+				rx_id = MXC_DMA_ASRC_C_RX;
+				tx_id = MXC_DMA_ASRC_C_TX;
+				rx_name = asrc_pair_id[4];
+				tx_name = asrc_pair_id[5];
+			}
+			channel = mxc_dma_request(rx_id, rx_name);
+			params->input_dma_channel = channel;
+			err = mxc_dma_callback_set(channel, (mxc_dma_callback_t)
+						   asrc_input_dma_callback,
+						   (void *)params);
+			channel = mxc_dma_request(tx_id, tx_name);
+			params->output_dma_channel = channel;
+			err = mxc_dma_callback_set(channel, (mxc_dma_callback_t)
+						   asrc_output_dma_callback,
+						   (void *)params);
+
+			break;
+		}
+	default:
+		break;
+	}
+
+	up(&params->busy_lock);
+	return err;
+}
+
+/*!
+ * asrc interface - open function
+ *
+ * @param inode        structure inode *
+ *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *      ENOBUFS failed to allocate buffer, ERESTARTSYS interrupted by user
+ */
+static int mxc_asrc_open(struct inode *inode, struct file *file)
+{
+	int err = 0;
+	struct asrc_pair_params *pair_params;
+	if (signal_pending(current))
+		return -EINTR;
+	pair_params = kzalloc(sizeof(struct asrc_pair_params), GFP_KERNEL);
+	if (pair_params == NULL) {
+		pr_debug("Failed to allocate pair_params\n");
+		err = -ENOBUFS;
+	}
+
+	init_MUTEX(&pair_params->busy_lock);
+	file->private_data = pair_params;
+	return err;
+}
+
+/*!
+ * asrc interface - close function
+ *
+ * @param inode    struct inode *
+ * @param file        structure file *
+ *
+ * @return status     0 Success, EINTR busy lock error, ENOBUFS remap_page error
+ */
+static int mxc_asrc_close(struct inode *inode, struct file *file)
+{
+	struct asrc_pair_params *pair_params;
+	pair_params = file->private_data;
+	if (pair_params->asrc_active == 1) {
+		mxc_dma_disable(pair_params->input_dma_channel);
+		mxc_dma_disable(pair_params->output_dma_channel);
+		asrc_stop_conv(pair_params->index);
+		wake_up_interruptible(&pair_params->input_wait_queue);
+		wake_up_interruptible(&pair_params->output_wait_queue);
+	}
+	if (pair_params->pair_hold == 1) {
+		mxc_dma_free(pair_params->input_dma_channel);
+		mxc_dma_free(pair_params->output_dma_channel);
+		mxc_free_dma_buf(pair_params);
+		asrc_release_pair(pair_params->index);
+	}
+	kfree(pair_params);
+	file->private_data = NULL;
+	return 0;
+}
+
+/*!
+ * asrc interface - mmap function
+ *
+ * @param file        structure file *
+ *
+ * @param vma         structure vm_area_struct *
+ *
+ * @return status     0 Success, EINTR busy lock error, ENOBUFS remap_page error
+ */
+static int mxc_asrc_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	unsigned long size;
+	int res = 0;
+	size = vma->vm_end - vma->vm_start;
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot))
+		return -ENOBUFS;
+
+	vma->vm_flags &= ~VM_IO;
+	return res;
+}
+
+static struct file_operations asrc_fops = {
+	.owner = THIS_MODULE,
+	.ioctl = asrc_ioctl,
+	.mmap = mxc_asrc_mmap,
+	.open = mxc_asrc_open,
+	.release = mxc_asrc_close,
+};
+
+static int asrc_read_proc_attr(char *page, char **start, off_t off,
+			       int count, int *eof, void *data)
+{
+	unsigned long reg;
+	int len = 0;
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCNCR_REG);
+
+	len += sprintf(page, "ANCA: %d\n",
+		       (int)(reg &
+			     (0xFFFFFFFF >>
+			      (32 - mxc_asrc_data->channel_bits))));
+	len +=
+	    sprintf(page + len, "ANCB: %d\n",
+		    (int)((reg >> mxc_asrc_data->
+			   channel_bits) & (0xFFFFFFFF >> (32 -
+							   mxc_asrc_data->
+							   channel_bits))));
+	len +=
+	    sprintf(page + len, "ANCC: %d\n",
+		    (int)((reg >> (mxc_asrc_data->channel_bits * 2)) &
+			  (0xFFFFFFFF >> (32 - mxc_asrc_data->channel_bits))));
+
+	if (off > len)
+		return 0;
+
+	*eof = (len <= count) ? 1 : 0;
+	*start = page + off;
+
+	return min(count, len - (int)off);
+}
+
+static int asrc_write_proc_attr(struct file *file, const char *buffer,
+				unsigned long count, void *data)
+{
+	char buf[50];
+	unsigned long reg;
+	int na, nb, nc;
+	int total;
+	if (count > 48)
+		return -EINVAL;
+	if (copy_from_user(buf, buffer, count)) {
+		pr_debug("Attr proc write, Failed to copy buffer from user\n");
+		return -EFAULT;
+	}
+
+	reg = __raw_readl(asrc_vrt_base_addr + ASRC_ASRCNCR_REG);
+	sscanf(buf, "ANCA: %d\nANCB: %d\nANCC: %d", &na, &nb, &nc);
+	if (mxc_asrc_data->channel_bits > 3)
+		total = 10;
+	else
+		total = 5;
+	if ((na + nb + nc) != total) {
+		pr_info("Wrong ASRCNR settings\n");
+		return -EFAULT;
+	}
+	reg = na | (nb << mxc_asrc_data->
+		    channel_bits) | (nc << (mxc_asrc_data->channel_bits * 2));
+
+	__raw_writel(reg, asrc_vrt_base_addr + ASRC_ASRCNCR_REG);
+
+	return count;
+}
+
+static void asrc_proc_create(void)
+{
+	struct proc_dir_entry *proc_attr;
+	proc_asrc = proc_mkdir(ASRC_PROC_PATH, NULL);
+	if (proc_asrc) {
+		proc_attr = create_proc_entry("ChSettings",
+					      S_IFREG | S_IRUGO |
+					      S_IWUSR, proc_asrc);
+		if (proc_attr) {
+			proc_attr->read_proc = asrc_read_proc_attr;
+			proc_attr->write_proc = asrc_write_proc_attr;
+			proc_attr->size = 48;
+			proc_attr->uid = proc_attr->gid = 0;
+			proc_attr->owner = THIS_MODULE;
+		} else {
+			remove_proc_entry(ASRC_PROC_PATH, NULL);
+			pr_info("Failed to create proc attribute entry \n");
+		}
+	} else {
+		pr_info("ASRC: Failed to create proc entry %s\n",
+			ASRC_PROC_PATH);
+	}
+}
+
+/*!
+ * Entry point for the asrc device
+ *
+ * @param	pdev Pionter to the registered platform device
+ * @return  Error code indicating success or failure
+ */
+static int mxc_asrc_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct resource *res;
+	struct device *temp_class;
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENOENT;
+
+	g_asrc_data = kzalloc(sizeof(struct asrc_data), GFP_KERNEL);
+
+	if (g_asrc_data == NULL) {
+		pr_info("Failed to allocate g_asrc_data\n");
+		return -ENOMEM;
+	}
+
+	g_asrc_data->asrc_pair[0].chn_max = 2;
+	g_asrc_data->asrc_pair[1].chn_max = 2;
+	g_asrc_data->asrc_pair[2].chn_max = 6;
+	g_asrc_data->asrc_pair[0].overload_error = 0;
+	g_asrc_data->asrc_pair[1].overload_error = 0;
+	g_asrc_data->asrc_pair[2].overload_error = 0;
+
+	asrc_major = register_chrdev(asrc_major, "mxc_asrc", &asrc_fops);
+	if (asrc_major < 0) {
+		pr_info("Unable to register asrc device\n");
+		err = -EBUSY;
+		goto error;
+	}
+
+	asrc_class = class_create(THIS_MODULE, "mxc_asrc");
+	if (IS_ERR(asrc_class)) {
+		err = PTR_ERR(asrc_class);
+		goto err_out_chrdev;
+	}
+
+	temp_class = device_create(asrc_class, NULL, MKDEV(asrc_major, 0),
+				   NULL, "mxc_asrc");
+	if (IS_ERR(temp_class)) {
+		err = PTR_ERR(temp_class);
+		goto err_out_class;
+	}
+
+	asrc_vrt_base_addr =
+	    (unsigned long)ioremap(res->start, res->end - res->start + 1);
+
+	mxc_asrc_data =
+	    (struct mxc_asrc_platform_data *)pdev->dev.platform_data;
+	clk_enable(mxc_asrc_data->asrc_core_clk);
+
+	asrc_proc_create();
+	err = mxc_init_asrc();
+	if (err < 0)
+		goto err_out_class;
+
+	goto out;
+
+      err_out_class:
+	clk_disable(mxc_asrc_data->asrc_core_clk);
+	device_destroy(asrc_class, MKDEV(asrc_major, 0));
+	class_destroy(asrc_class);
+      err_out_chrdev:
+	unregister_chrdev(asrc_major, "mxc_asrc");
+      error:
+	kfree(g_asrc_data);
+      out:
+	pr_info("mxc_asrc registered\n");
+	return err;
+}
+
+/*!
+ * Exit asrc device
+ *
+ * @param	pdev Pionter to the registered platform device
+ * @return  Error code indicating success or failure
+ */
+static int mxc_asrc_remove(struct platform_device *pdev)
+{
+	free_irq(MXC_INT_ASRC, NULL);
+	kfree(g_asrc_data);
+	clk_disable(mxc_asrc_data->asrc_core_clk);
+	mxc_asrc_data = NULL;
+	iounmap((unsigned long __iomem *)asrc_vrt_base_addr);
+	remove_proc_entry("ChSettings", proc_asrc);
+	remove_proc_entry(ASRC_PROC_PATH, NULL);
+	device_destroy(asrc_class, MKDEV(asrc_major, 0));
+	class_destroy(asrc_class);
+	unregister_chrdev(asrc_major, "mxc_asrc");
+	return 0;
+}
+
+/*! mxc asrc driver definition
+ *
+ */
+static struct platform_driver mxc_asrc_driver = {
+	.driver = {
+		   .name = "mxc_asrc",
+		   },
+	.probe = mxc_asrc_probe,
+	.remove = mxc_asrc_remove,
+};
+
+/*!
+ * Register asrc driver
+ *
+ */
+static __init int asrc_init(void)
+{
+	int ret;
+	ret = platform_driver_register(&mxc_asrc_driver);
+	return ret;
+}
+
+/*!
+ * Exit and free the asrc data
+ *
+ */ static void __exit asrc_exit(void)
+{
+	platform_driver_unregister(&mxc_asrc_driver);
+	return;
+}
+
+module_init(asrc_init);
+module_exit(asrc_exit);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Asynchronous Sample Rate Converter");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/bt/Kconfig b/drivers/mxc/bt/Kconfig
new file mode 100644
index 000000000000..9dbfbe57a14f
--- /dev/null
+++ b/drivers/mxc/bt/Kconfig
@@ -0,0 +1,13 @@
+#
+# Bluetooth configuration
+#
+
+menu "MXC Bluetooth support"
+
+config MXC_BLUETOOTH
+	tristate "MXC Bluetooth support"
+	depends on MACH_MX31_3DS || MACH_MX35_3DS || MACH_MX37_3DS || MACH_MX51_3DS
+	---help---
+         Say Y to get the third party Bluetooth service.
+
+endmenu
diff --git a/drivers/mxc/bt/Makefile b/drivers/mxc/bt/Makefile
new file mode 100644
index 000000000000..91bc4cff380e
--- /dev/null
+++ b/drivers/mxc/bt/Makefile
@@ -0,0 +1,4 @@
+#
+# Makefile for the kernel Bluetooth power-on/reset
+#
+obj-$(CONFIG_MXC_BLUETOOTH) += mxc_bt.o
diff --git a/drivers/mxc/bt/mxc_bt.c b/drivers/mxc/bt/mxc_bt.c
new file mode 100644
index 000000000000..6da165ad09dc
--- /dev/null
+++ b/drivers/mxc/bt/mxc_bt.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_bt.c
+ *
+ * @brief MXC Thirty party Bluetooth
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/regulator/consumer.h>
+
+static struct regulator *bt_vdd;
+static struct regulator *bt_vdd_parent;
+static struct regulator *bt_vusb;
+static struct regulator *bt_vusb_parent;
+
+/*!
+  * This function poweron the bluetooth hardware module
+  *
+  * @param      pdev    Pointer to the platform device
+  * @return              0 on success, -1 otherwise.
+  */
+static int mxc_bt_probe(struct platform_device *pdev)
+{
+	struct mxc_bt_platform_data *platform_data;
+	platform_data = (struct mxc_bt_platform_data *)pdev->dev.platform_data;
+	if (platform_data->bt_vdd) {
+		bt_vdd = regulator_get(&pdev->dev, platform_data->bt_vdd);
+		regulator_enable(bt_vdd);
+	}
+	if (platform_data->bt_vdd_parent) {
+		bt_vdd_parent =
+		    regulator_get(&pdev->dev, platform_data->bt_vdd_parent);
+		regulator_enable(bt_vdd_parent);
+	}
+	if (platform_data->bt_vusb) {
+		bt_vusb = regulator_get(&pdev->dev, platform_data->bt_vusb);
+		regulator_enable(bt_vusb);
+	}
+	if (platform_data->bt_vusb_parent) {
+		bt_vusb_parent =
+		    regulator_get(&pdev->dev, platform_data->bt_vusb_parent);
+		regulator_enable(bt_vusb_parent);
+	}
+
+	if (platform_data->bt_reset != NULL)
+		platform_data->bt_reset();
+	return 0;
+
+}
+
+/*!
+  * This function poweroff the bluetooth hardware module
+  *
+  * @param      pdev    Pointer to the platform device
+  * @return              0 on success, -1 otherwise.
+  */
+static int mxc_bt_remove(struct platform_device *pdev)
+{
+	struct mxc_bt_platform_data *platform_data;
+	platform_data = (struct mxc_bt_platform_data *)pdev->dev.platform_data;
+	if (bt_vdd) {
+		regulator_disable(bt_vdd);
+		regulator_put(bt_vdd);
+	}
+	if (bt_vdd_parent) {
+		regulator_disable(bt_vdd_parent);
+		regulator_put(bt_vdd_parent);
+	}
+	if (bt_vusb) {
+		regulator_disable(bt_vusb);
+		regulator_put(bt_vusb);
+	}
+	if (bt_vusb_parent) {
+		regulator_disable(bt_vusb_parent);
+		regulator_put(bt_vusb_parent);
+	}
+	return 0;
+
+}
+
+static struct platform_driver bluetooth_driver = {
+	.driver = {
+		   .name = "mxc_bt",
+		   },
+	.probe = mxc_bt_probe,
+	.remove = mxc_bt_remove,
+};
+
+/*!
+ * Register bluetooth driver module
+ *
+ */
+static __init int bluetooth_init(void)
+{
+	return platform_driver_register(&bluetooth_driver);
+}
+
+/*!
+ * Exit and free the bluetooth module
+ *
+ */
+static void __exit bluetooth_exit(void)
+{
+	platform_driver_unregister(&bluetooth_driver);
+}
+
+module_init(bluetooth_init);
+module_exit(bluetooth_exit);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC Thirty party Bluetooth");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/dam/Kconfig b/drivers/mxc/dam/Kconfig
new file mode 100644
index 000000000000..7b4bee92f648
--- /dev/null
+++ b/drivers/mxc/dam/Kconfig
@@ -0,0 +1,13 @@
+#
+# DAM API configuration
+#
+
+menu "MXC Digital Audio Multiplexer support"
+
+config MXC_DAM
+	tristate "DAM support"
+	depends on ARCH_MXC
+	---help---
+	  Say Y to get the Digital Audio Multiplexer services API available on MXC platform.
+
+endmenu
diff --git a/drivers/mxc/dam/Makefile b/drivers/mxc/dam/Makefile
new file mode 100644
index 000000000000..b5afdc143dfa
--- /dev/null
+++ b/drivers/mxc/dam/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for the kernel Digital Audio MUX (DAM) device driver.
+#
+
+ifeq ($(CONFIG_ARCH_MX27),y)
+	obj-$(CONFIG_MXC_DAM) += dam_v1.o
+else
+	obj-$(CONFIG_MXC_DAM) += dam.o
+endif
diff --git a/drivers/mxc/dam/dam.c b/drivers/mxc/dam/dam.c
new file mode 100644
index 000000000000..532b02f65bb2
--- /dev/null
+++ b/drivers/mxc/dam/dam.c
@@ -0,0 +1,427 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file dam.c
+ * @brief This is the brief documentation for this dam.c file.
+ *
+ * This file contains the implementation of the DAM driver main services
+ *
+ * @ingroup DAM
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <asm/uaccess.h>
+#include "dam.h"
+
+/*!
+ * This include to define bool type, false and true definitions.
+ */
+#include <mach/hardware.h>
+
+#define ModifyRegister32(a,b,c)	(c = ( ( (c)&(~(a)) ) | (b) ))
+
+#define DAM_VIRT_BASE_ADDR	IO_ADDRESS(AUDMUX_BASE_ADDR)
+
+#ifndef _reg_DAM_PTCR1
+#define    _reg_DAM_PTCR1   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x00)))
+#endif
+
+#ifndef _reg_DAM_PDCR1
+#define    _reg_DAM_PDCR1  (*((volatile unsigned long *) \
+                           (DAM_VIRT_BASE_ADDR + 0x04)))
+#endif
+
+#ifndef _reg_DAM_PTCR2
+#define    _reg_DAM_PTCR2   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x08)))
+#endif
+
+#ifndef _reg_DAM_PDCR2
+#define    _reg_DAM_PDCR2  (*((volatile unsigned long *) \
+                           (DAM_VIRT_BASE_ADDR + 0x0C)))
+#endif
+
+#ifndef _reg_DAM_PTCR3
+#define    _reg_DAM_PTCR3   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x10)))
+#endif
+
+#ifndef _reg_DAM_PDCR3
+#define    _reg_DAM_PDCR3  (*((volatile unsigned long *) \
+                           (DAM_VIRT_BASE_ADDR + 0x14)))
+#endif
+
+#ifndef _reg_DAM_PTCR4
+#define    _reg_DAM_PTCR4   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x18)))
+#endif
+
+#ifndef _reg_DAM_PDCR4
+#define    _reg_DAM_PDCR4  (*((volatile unsigned long *) \
+                           (DAM_VIRT_BASE_ADDR + 0x1C)))
+#endif
+
+#ifndef _reg_DAM_PTCR5
+#define    _reg_DAM_PTCR5   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x20)))
+#endif
+
+#ifndef _reg_DAM_PDCR5
+#define    _reg_DAM_PDCR5  (*((volatile unsigned long *) \
+                           (DAM_VIRT_BASE_ADDR + 0x24)))
+#endif
+
+#ifndef _reg_DAM_PTCR6
+#define    _reg_DAM_PTCR6   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x28)))
+#endif
+
+#ifndef _reg_DAM_PDCR6
+#define    _reg_DAM_PDCR6  (*((volatile unsigned long *) \
+                           (DAM_VIRT_BASE_ADDR + 0x2C)))
+#endif
+
+#ifndef _reg_DAM_PTCR7
+#define    _reg_DAM_PTCR7   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x30)))
+#endif
+
+#ifndef _reg_DAM_PDCR7
+#define    _reg_DAM_PDCR7  (*((volatile unsigned long *) \
+                           (DAM_VIRT_BASE_ADDR + 0x34)))
+#endif
+
+#ifndef _reg_DAM_CNMCR
+#define    _reg_DAM_CNMCR   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x38)))
+#endif
+
+#ifndef _reg_DAM_PTCR
+#define    _reg_DAM_PTCR(a)   (*((volatile unsigned long *) \
+                              (DAM_VIRT_BASE_ADDR + a*8)))
+#endif
+
+#ifndef _reg_DAM_PDCR
+#define    _reg_DAM_PDCR(a)   (*((volatile unsigned long *) \
+                              (DAM_VIRT_BASE_ADDR + 4 + a*8)))
+#endif
+
+/*!
+ * PTCR Registers bit shift definitions
+ */
+#define dam_synchronous_mode_shift               11
+#define dam_receive_clock_select_shift           12
+#define dam_receive_clock_direction_shift        16
+#define dam_receive_frame_sync_select_shift      17
+#define dam_receive_frame_sync_direction_shift   21
+#define dam_transmit_clock_select_shift          22
+#define dam_transmit_clock_direction_shift       26
+#define dam_transmit_frame_sync_select_shift     27
+#define dam_transmit_frame_sync_direction_shift  31
+#define dam_selection_mask                      0xF
+
+/*!
+ * HPDCR Register bit shift definitions
+ */
+#define dam_internal_network_mode_shift           0
+#define dam_mode_shift                            8
+#define dam_transmit_receive_switch_shift        12
+#define dam_receive_data_select_shift            13
+
+/*!
+ * HPDCR Register bit masq definitions
+ */
+#define dam_mode_masq                          0x03
+#define dam_internal_network_mode_mask         0xFF
+
+/*!
+ * CNMCR Register bit shift definitions
+ */
+#define dam_ce_bus_port_cntlow_shift              0
+#define dam_ce_bus_port_cnthigh_shift             8
+#define dam_ce_bus_port_clkpol_shift             16
+#define dam_ce_bus_port_fspol_shift              17
+#define dam_ce_bus_port_enable_shift             18
+
+#define DAM_NAME   "dam"
+
+EXPORT_SYMBOL(dam_select_mode);
+EXPORT_SYMBOL(dam_select_RxClk_direction);
+EXPORT_SYMBOL(dam_select_RxClk_source);
+EXPORT_SYMBOL(dam_select_RxD_source);
+EXPORT_SYMBOL(dam_select_RxFS_direction);
+EXPORT_SYMBOL(dam_select_RxFS_source);
+EXPORT_SYMBOL(dam_select_TxClk_direction);
+EXPORT_SYMBOL(dam_select_TxClk_source);
+EXPORT_SYMBOL(dam_select_TxFS_direction);
+EXPORT_SYMBOL(dam_select_TxFS_source);
+EXPORT_SYMBOL(dam_set_internal_network_mode_mask);
+EXPORT_SYMBOL(dam_set_synchronous);
+EXPORT_SYMBOL(dam_switch_Tx_Rx);
+EXPORT_SYMBOL(dam_reset_register);
+
+/*!
+ * This function selects the operation mode of the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        the_mode          the operation mode of the port
+ *
+ * @return       This function returns the result of the operation
+ *               (0 if successful, -1 otherwise).
+ */
+int dam_select_mode(dam_port port, dam_mode the_mode)
+{
+	int result;
+	result = 0;
+
+	ModifyRegister32(dam_mode_masq << dam_mode_shift,
+			 the_mode << dam_mode_shift, _reg_DAM_PDCR(port));
+
+	return result;
+}
+
+/*!
+ * This function controls Receive clock signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Rx clock signal direction
+ */
+void dam_select_RxClk_direction(dam_port port, signal_direction direction)
+{
+	ModifyRegister32(1 << dam_receive_clock_direction_shift,
+			 direction << dam_receive_clock_direction_shift,
+			 _reg_DAM_PTCR(port));
+}
+
+/*!
+ * This function controls Receive clock signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxClk        the signal comes from RxClk or TxClk of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_RxClk_source(dam_port p_config,
+			     bool from_RxClk, dam_port p_source)
+{
+	ModifyRegister32(dam_selection_mask << dam_receive_clock_select_shift,
+			 ((from_RxClk << 3) | p_source) <<
+			 dam_receive_clock_select_shift,
+			 _reg_DAM_PTCR(p_config));
+}
+
+/*!
+ * This function selects the source port for the RxD data.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        p_source          the source port
+ */
+void dam_select_RxD_source(dam_port p_config, dam_port p_source)
+{
+	ModifyRegister32(dam_selection_mask << dam_receive_data_select_shift,
+			 p_source << dam_receive_data_select_shift,
+			 _reg_DAM_PDCR(p_config));
+}
+
+/*!
+ * This function controls Receive Frame Sync signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Rx Frame Sync signal direction
+ */
+void dam_select_RxFS_direction(dam_port port, signal_direction direction)
+{
+	ModifyRegister32(1 << dam_receive_frame_sync_direction_shift,
+			 direction << dam_receive_frame_sync_direction_shift,
+			 _reg_DAM_PTCR(port));
+}
+
+/*!
+ * This function controls Receive Frame Sync signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxFS         the signal comes from RxFS or TxFS of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_RxFS_source(dam_port p_config,
+			    bool from_RxFS, dam_port p_source)
+{
+	ModifyRegister32(dam_selection_mask <<
+			 dam_receive_frame_sync_select_shift,
+			 ((from_RxFS << 3) | p_source) <<
+			 dam_receive_frame_sync_select_shift,
+			 _reg_DAM_PTCR(p_config));
+}
+
+/*!
+ * This function controls Transmit clock signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Tx clock signal direction
+ */
+void dam_select_TxClk_direction(dam_port port, signal_direction direction)
+{
+	ModifyRegister32(1 << dam_transmit_clock_direction_shift,
+			 direction << dam_transmit_clock_direction_shift,
+			 _reg_DAM_PTCR(port));
+}
+
+/*!
+ * This function controls Transmit clock signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxClk        the signal comes from RxClk or TxClk of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_TxClk_source(dam_port p_config,
+			     bool from_RxClk, dam_port p_source)
+{
+	ModifyRegister32(dam_selection_mask << dam_transmit_clock_select_shift,
+			 ((from_RxClk << 3) | p_source) <<
+			 dam_transmit_clock_select_shift,
+			 _reg_DAM_PTCR(p_config));
+}
+
+/*!
+ * This function controls Transmit Frame Sync signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Tx Frame Sync signal direction
+ */
+void dam_select_TxFS_direction(dam_port port, signal_direction direction)
+{
+	ModifyRegister32(1 << dam_transmit_frame_sync_direction_shift,
+			 direction << dam_transmit_frame_sync_direction_shift,
+			 _reg_DAM_PTCR(port));
+}
+
+/*!
+ * This function controls Transmit Frame Sync signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxFS         the signal comes from RxFS or TxFS of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_TxFS_source(dam_port p_config,
+			    bool from_RxFS, dam_port p_source)
+{
+	ModifyRegister32(dam_selection_mask <<
+			 dam_transmit_frame_sync_select_shift,
+			 ((from_RxFS << 3) | p_source) <<
+			 dam_transmit_frame_sync_select_shift,
+			 _reg_DAM_PTCR(p_config));
+}
+
+/*!
+ * This function sets a bit mask that selects the port from which of the RxD
+ * signals are to be ANDed together for internal network mode.
+ * Bit 6 represents RxD from Port7 and bit0 represents RxD from Port1.
+ * 1 excludes RxDn from ANDing. 0 includes RxDn for ANDing.
+ *
+ * @param        port              the DAM port to configure
+ * @param        bit_mask          the bit mask
+ *
+ * @return       This function returns the result of the operation
+ *               (0 if successful, -1 otherwise).
+ */
+int dam_set_internal_network_mode_mask(dam_port port, unsigned char bit_mask)
+{
+	int result;
+	result = 0;
+
+	ModifyRegister32(dam_internal_network_mode_mask <<
+			 dam_internal_network_mode_shift,
+			 bit_mask << dam_internal_network_mode_shift,
+			 _reg_DAM_PDCR(port));
+
+	return result;
+}
+
+/*!
+ * This function controls whether or not the port is in synchronous mode.
+ * When the synchronous mode is selected, the receive and the transmit sections
+ * use common clock and frame sync signals.
+ * When the synchronous mode is not selected, separate clock and frame sync
+ * signals are used for the transmit and the receive sections.
+ * The defaut value is the synchronous mode selected.
+ *
+ * @param        port              the DAM port to configure
+ * @param        synchronous       the state to assign
+ */
+void dam_set_synchronous(dam_port port, bool synchronous)
+{
+	ModifyRegister32(1 << dam_synchronous_mode_shift,
+			 synchronous << dam_synchronous_mode_shift,
+			 _reg_DAM_PTCR(port));
+}
+
+/*!
+ * This function swaps the transmit and receive signals from (Da-TxD, Db-RxD)
+ * to (Da-RxD, Db-TxD).
+ * This default signal configuration is Da-TxD, Db-RxD.
+ *
+ * @param        port              the DAM port to configure
+ * @param        value             the switch state
+ */
+void dam_switch_Tx_Rx(dam_port port, bool value)
+{
+	ModifyRegister32(1 << dam_transmit_receive_switch_shift,
+			 value << dam_transmit_receive_switch_shift,
+			 _reg_DAM_PDCR(port));
+}
+
+/*!
+ * This function resets the two registers of the selected port.
+ *
+ * @param        port              the DAM port to reset
+ */
+void dam_reset_register(dam_port port)
+{
+	ModifyRegister32(0xFFFFFFFF, 0x00000000, _reg_DAM_PTCR(port));
+	ModifyRegister32(0xFFFFFFFF, 0x00000000, _reg_DAM_PDCR(port));
+}
+
+/*!
+ * This function implements the init function of the DAM device.
+ * This function is called when the module is loaded.
+ *
+ * @return       This function returns 0.
+ */
+static int __init dam_init(void)
+{
+	return 0;
+}
+
+/*!
+ * This function implements the exit function of the SPI device.
+ * This function is called when the module is unloaded.
+ *
+ */
+static void __exit dam_exit(void)
+{
+}
+
+module_init(dam_init);
+module_exit(dam_exit);
+
+MODULE_DESCRIPTION("DAM char device driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/dam/dam.h b/drivers/mxc/dam/dam.h
new file mode 100644
index 000000000000..cb9ead53f689
--- /dev/null
+++ b/drivers/mxc/dam/dam.h
@@ -0,0 +1,258 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+ /*!
+  * @defgroup DAM Digital Audio Multiplexer (AUDMUX) Driver
+  */
+
+ /*!
+  * @file dam.h
+  * @brief This is the brief documentation for this dam.h file.
+  *
+  * This header file contains DAM driver functions prototypes.
+  *
+  * @ingroup DAM
+  */
+
+#ifndef __MXC_DAM_H__
+#define __MXC_DAM_H__
+
+/*!
+ * This enumeration describes the Digital Audio Multiplexer mode.
+ */
+typedef enum {
+
+	/*!
+	 * Normal mode
+	 */
+	normal_mode = 0,
+
+	/*!
+	 * Internal network mode
+	 */
+	internal_network_mode = 1,
+
+	/*!
+	 * CE bus network mode
+	 */
+	CE_bus_network_mode = 2
+} dam_mode;
+
+/*!
+ * This enumeration describes the port.
+ */
+typedef enum {
+
+	/*!
+	 * The port 1
+	 */
+	port_1 = 0,
+
+	/*!
+	 * The port 2
+	 */
+	port_2 = 1,
+
+	/*!
+	 * The port 3
+	 */
+	port_3 = 2,
+
+	/*!
+	 * The port 4
+	 */
+	port_4 = 3,
+
+	/*!
+	 * The port 5
+	 */
+	port_5 = 4,
+
+	/*!
+	 * The port 6
+	 */
+	port_6 = 5,
+
+	/*!
+	 * The port 7
+	 */
+	port_7 = 6
+} dam_port;
+
+/*!
+ * This enumeration describes the signal direction.
+ */
+typedef enum {
+
+	/*!
+	 * Signal In
+	 */
+	signal_in = 0,
+
+	/*!
+	 * Signal Out
+	 */
+	signal_out = 1
+} signal_direction;
+
+/*!
+ * Test purpose definition
+ */
+#define TEST_DAM 1
+
+#ifdef TEST_DAM
+
+#define DAM_IOCTL 0x55
+#define DAM_CONFIG_SSI1_MC13783 _IOWR(DAM_IOCTL, 1, int)
+#define DAM_CONFIG_SSI2_MC13783 _IOWR(DAM_IOCTL, 2, int)
+#define DAM_CONFIG_SSI_NETWORK_MODE_MC13783 _IOWR(DAM_IOCTL, 3, int)
+#endif
+
+/*!
+ * This function selects the operation mode of the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        the_mode          the operation mode of the port
+ * @return       This function returns the result of the operation
+ *               (0 if successful, -1 otherwise).
+ */
+int dam_select_mode(dam_port port, dam_mode the_mode);
+
+/*!
+ * This function controls Receive clock signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Rx clock signal direction
+ */
+void dam_select_RxClk_direction(dam_port port, signal_direction direction);
+
+/*!
+ * This function controls Receive clock signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxClk        the signal comes from RxClk or TxClk of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_RxClk_source(dam_port p_config, bool from_RxClk,
+			     dam_port p_source);
+
+/*!
+ * This function selects the source port for the RxD data.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        p_source          the source port
+ */
+void dam_select_RxD_source(dam_port p_config, dam_port p_source);
+
+/*!
+ * This function controls Receive Frame Sync signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Rx Frame Sync signal direction
+ */
+void dam_select_RxFS_direction(dam_port port, signal_direction direction);
+
+/*!
+ * This function controls Receive Frame Sync signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxFS         the signal comes from RxFS or TxFS of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_RxFS_source(dam_port p_config, bool from_RxFS,
+			    dam_port p_source);
+
+/*!
+ * This function controls Transmit clock signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Tx clock signal direction
+ */
+void dam_select_TxClk_direction(dam_port port, signal_direction direction);
+
+/*!
+ * This function controls Transmit clock signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxClk        the signal comes from RxClk or TxClk of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_TxClk_source(dam_port p_config, bool from_RxClk,
+			     dam_port p_source);
+
+/*!
+ * This function controls Transmit Frame Sync signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Tx Frame Sync signal direction
+ */
+void dam_select_TxFS_direction(dam_port port, signal_direction direction);
+
+/*!
+ * This function controls Transmit Frame Sync signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxFS         the signal comes from RxFS or TxFS of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_TxFS_source(dam_port p_config, bool from_RxFS,
+			    dam_port p_source);
+
+/*!
+ * This function sets a bit mask that selects the port from which of
+ * the RxD signals are to be ANDed together for internal network mode.
+ * Bit 6 represents RxD from Port7 and bit0 represents RxD from Port1.
+ * 1 excludes RxDn from ANDing. 0 includes RxDn for ANDing.
+ *
+ * @param        port              the DAM port to configure
+ * @param        bit_mask          the bit mask
+ * @return       This function returns the result of the operation
+ *               (0 if successful, -1 otherwise).
+ */
+int dam_set_internal_network_mode_mask(dam_port port, unsigned char bit_mask);
+
+/*!
+ * This function controls whether or not the port is in synchronous mode.
+ * When the synchronous mode is selected, the receive and the transmit sections
+ * use common clock and frame sync signals.
+ * When the synchronous mode is not selected, separate clock and frame sync
+ * signals are used for the transmit and the receive sections.
+ * The defaut value is the synchronous mode selected.
+ *
+ * @param        port              the DAM port to configure
+ * @param        synchronous       the state to assign
+ */
+void dam_set_synchronous(dam_port port, bool synchronous);
+
+/*!
+ * This function swaps the transmit and receive signals from (Da-TxD, Db-RxD) to
+ * (Da-RxD, Db-TxD).
+ * This default signal configuration is Da-TxD, Db-RxD.
+ *
+ * @param        port              the DAM port to configure
+ * @param        value             the switch state
+ */
+void dam_switch_Tx_Rx(dam_port port, bool value);
+
+/*!
+ * This function resets the two registers of the selected port.
+ *
+ * @param        port              the DAM port to reset
+ */
+void dam_reset_register(dam_port port);
+
+#endif
diff --git a/drivers/mxc/dam/dam_v1.c b/drivers/mxc/dam/dam_v1.c
new file mode 100644
index 000000000000..0fc88bb98a38
--- /dev/null
+++ b/drivers/mxc/dam/dam_v1.c
@@ -0,0 +1,617 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file dam_v1.c
+ * @brief This is the brief documentation for this dam_v1.c file.
+ *
+ * This file contains the implementation of the DAM driver main services
+ *
+ * @ingroup DAM
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/io.h>
+#include <asm/uaccess.h>
+#include "dam.h"
+
+/*!
+ * This include to define bool type, false and true definitions.
+ */
+#include <mach/hardware.h>
+
+#define DAM_VIRT_BASE_ADDR	IO_ADDRESS(AUDMUX_BASE_ADDR)
+
+#define ModifyRegister32(a,b,c)	do{\
+	__raw_writel( ((__raw_readl(c)) & (~(a))) | (b),(c) );\
+}while(0)
+
+#ifndef _reg_DAM_HPCR1
+#define    _reg_DAM_HPCR1   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x00)))
+#endif
+
+#ifndef _reg_DAM_HPCR2
+#define    _reg_DAM_HPCR2   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x04)))
+#endif
+
+#ifndef _reg_DAM_HPCR3
+#define    _reg_DAM_HPCR3   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x08)))
+#endif
+
+#ifndef _reg_DAM_PPCR1
+#define    _reg_DAM_PPCR1   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x10)))
+#endif
+
+#ifndef _reg_DAM_PPCR2
+#define    _reg_DAM_PPCR2  (*((volatile unsigned long *) \
+                           (DAM_VIRT_BASE_ADDR + 0x14)))
+#endif
+
+#ifndef _reg_DAM_PPCR3
+#define    _reg_DAM_PPCR3   (*((volatile unsigned long *) \
+                            (DAM_VIRT_BASE_ADDR + 0x1c)))
+#endif
+
+#ifndef _reg_DAM_HPCR
+#define    _reg_DAM_HPCR(a)   ((volatile unsigned long *) \
+                              (DAM_VIRT_BASE_ADDR + (a)*4))
+#endif
+
+#ifndef _reg_DAM_PPCR
+#define    _reg_DAM_PPCR(a)   ((volatile unsigned long *) \
+                              (DAM_VIRT_BASE_ADDR + 0x0c + (0x04 << (a-3)) ))
+#endif
+
+/*!
+ * HPCR/PPCR Registers bit shift definitions
+ */
+#define dam_transmit_frame_sync_direction_shift  31
+#define dam_transmit_clock_direction_shift       30
+#define dam_transmit_frame_sync_select_shift     26
+#define dam_transmit_clock_select_shift          26
+#define dam_receive_frame_sync_direction_shift   25
+#define dam_receive_clock_direction_shift        24
+#define dam_receive_clock_select_shift           20
+#define dam_receive_frame_sync_select_shift      20
+
+#define dam_receive_data_select_shift            13
+#define dam_synchronous_mode_shift               12
+
+#define dam_transmit_receive_switch_shift        10
+
+#define dam_mode_shift                            8
+#define dam_internal_network_mode_shift           0
+
+/*!
+ * HPCR/PPCR Register bit masq definitions
+ */
+//#define dam_selection_mask              0xF
+#define dam_fs_selection_mask             0xF
+#define dam_clk_selection_mask            0xF
+#define dam_dat_selection_mask		        0x7
+//#define dam_mode_masq                   0x03
+#define dam_internal_network_mode_mask    0xFF
+
+/*!
+ * HPCR/PPCR Register reset value definitions
+ */
+#define dam_hpcr_default_value 0x00001000
+#define dam_ppcr_default_value 0x00001000
+
+#define DAM_NAME   "dam"
+static struct class *mxc_dam_class;
+
+EXPORT_SYMBOL(dam_select_mode);
+EXPORT_SYMBOL(dam_select_RxClk_direction);
+EXPORT_SYMBOL(dam_select_RxClk_source);
+EXPORT_SYMBOL(dam_select_RxD_source);
+EXPORT_SYMBOL(dam_select_RxFS_direction);
+EXPORT_SYMBOL(dam_select_RxFS_source);
+EXPORT_SYMBOL(dam_select_TxClk_direction);
+EXPORT_SYMBOL(dam_select_TxClk_source);
+EXPORT_SYMBOL(dam_select_TxFS_direction);
+EXPORT_SYMBOL(dam_select_TxFS_source);
+EXPORT_SYMBOL(dam_set_internal_network_mode_mask);
+EXPORT_SYMBOL(dam_set_synchronous);
+EXPORT_SYMBOL(dam_switch_Tx_Rx);
+EXPORT_SYMBOL(dam_reset_register);
+
+/*!
+ * DAM major
+ */
+#ifdef TEST_DAM
+static int major_dam;
+
+typedef struct _mxc_cfg {
+	int reg;
+	int val;
+} mxc_cfg;
+
+#endif
+
+/*!
+ * This function selects the operation mode of the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        the_mode          the operation mode of the port
+ *
+ * @return       This function returns the result of the operation
+ *               (0 if successful, -1 otherwise).
+ */
+int dam_select_mode(dam_port port, dam_mode the_mode)
+{
+	int result;
+	result = 0;
+
+	if (port >= 3)
+		the_mode = normal_mode;
+	ModifyRegister32(1 << dam_mode_shift,
+			 the_mode << dam_mode_shift, _reg_DAM_HPCR(port));
+
+	return result;
+}
+
+/*!
+ * This function controls Receive clock signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Rx clock signal direction
+ */
+void dam_select_RxClk_direction(dam_port port, signal_direction direction)
+{
+	if (port < 3) {
+		ModifyRegister32(1 << dam_receive_clock_direction_shift,
+				 direction << dam_receive_clock_direction_shift,
+				 _reg_DAM_HPCR(port));
+	} else {
+		ModifyRegister32(1 << dam_receive_clock_direction_shift,
+				 direction << dam_receive_clock_direction_shift,
+				 _reg_DAM_PPCR(port));
+	}
+	return;
+}
+
+/*!
+ * This function controls Receive clock signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxClk        the signal comes from RxClk or TxClk of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_RxClk_source(dam_port p_config,
+			     bool from_RxClk, dam_port p_source)
+{
+	if (p_config < 3) {
+		ModifyRegister32(dam_clk_selection_mask <<
+				 dam_receive_clock_select_shift,
+				 ((from_RxClk << 3) | p_source) <<
+				 dam_receive_clock_select_shift,
+				 _reg_DAM_HPCR(p_config));
+	} else {
+		ModifyRegister32(dam_clk_selection_mask <<
+				 dam_receive_clock_select_shift,
+				 ((from_RxClk << 3) | p_source) <<
+				 dam_receive_clock_select_shift,
+				 _reg_DAM_PPCR(p_config));
+	}
+	return;
+}
+
+/*!
+ * This function selects the source port for the RxD data.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        p_source          the source port
+ */
+void dam_select_RxD_source(dam_port p_config, dam_port p_source)
+{
+	if (p_config < 3) {
+		ModifyRegister32(dam_dat_selection_mask <<
+				 dam_receive_data_select_shift,
+				 p_source << dam_receive_data_select_shift,
+				 _reg_DAM_HPCR(p_config));
+	} else {
+		ModifyRegister32(dam_dat_selection_mask <<
+				 dam_receive_data_select_shift,
+				 p_source << dam_receive_data_select_shift,
+				 _reg_DAM_PPCR(p_config));
+	}
+	return;
+}
+
+/*!
+ * This function controls Receive Frame Sync signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Rx Frame Sync signal direction
+ */
+void dam_select_RxFS_direction(dam_port port, signal_direction direction)
+{
+	if (port < 3) {
+		ModifyRegister32(1 << dam_receive_frame_sync_direction_shift,
+				 direction <<
+				 dam_receive_frame_sync_direction_shift,
+				 _reg_DAM_HPCR(port));
+	} else {
+		ModifyRegister32(1 << dam_receive_frame_sync_direction_shift,
+				 direction <<
+				 dam_receive_frame_sync_direction_shift,
+				 _reg_DAM_PPCR(port));
+	}
+	return;
+}
+
+/*!
+ * This function controls Receive Frame Sync signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxFS         the signal comes from RxFS or TxFS of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_RxFS_source(dam_port p_config,
+			    bool from_RxFS, dam_port p_source)
+{
+	if (p_config < 3) {
+		ModifyRegister32(dam_fs_selection_mask <<
+				 dam_receive_frame_sync_select_shift,
+				 ((from_RxFS << 3) | p_source) <<
+				 dam_receive_frame_sync_select_shift,
+				 _reg_DAM_HPCR(p_config));
+	} else {
+		ModifyRegister32(dam_fs_selection_mask <<
+				 dam_receive_frame_sync_select_shift,
+				 ((from_RxFS << 3) | p_source) <<
+				 dam_receive_frame_sync_select_shift,
+				 _reg_DAM_PPCR(p_config));
+	}
+	return;
+}
+
+/*!
+ * This function controls Transmit clock signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Tx clock signal direction
+ */
+void dam_select_TxClk_direction(dam_port port, signal_direction direction)
+{
+	if (port < 3) {
+		ModifyRegister32(1 << dam_transmit_clock_direction_shift,
+				 direction <<
+				 dam_transmit_clock_direction_shift,
+				 _reg_DAM_HPCR(port));
+	} else {
+		ModifyRegister32(1 << dam_transmit_clock_direction_shift,
+				 direction <<
+				 dam_transmit_clock_direction_shift,
+				 _reg_DAM_PPCR(port));
+	}
+	return;
+}
+
+/*!
+ * This function controls Transmit clock signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxClk        the signal comes from RxClk or TxClk of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_TxClk_source(dam_port p_config,
+			     bool from_RxClk, dam_port p_source)
+{
+	if (p_config < 3) {
+		ModifyRegister32(dam_clk_selection_mask <<
+				 dam_transmit_clock_select_shift,
+				 ((from_RxClk << 3) | p_source) <<
+				 dam_transmit_clock_select_shift,
+				 _reg_DAM_HPCR(p_config));
+	} else {
+		ModifyRegister32(dam_clk_selection_mask <<
+				 dam_transmit_clock_select_shift,
+				 ((from_RxClk << 3) | p_source) <<
+				 dam_transmit_clock_select_shift,
+				 _reg_DAM_PPCR(p_config));
+	}
+	return;
+}
+
+/*!
+ * This function controls Transmit Frame Sync signal direction for the port.
+ *
+ * @param        port              the DAM port to configure
+ * @param        direction         the Tx Frame Sync signal direction
+ */
+void dam_select_TxFS_direction(dam_port port, signal_direction direction)
+{
+	if (port < 3) {
+		ModifyRegister32(1 << dam_transmit_frame_sync_direction_shift,
+				 direction <<
+				 dam_transmit_frame_sync_direction_shift,
+				 _reg_DAM_HPCR(port));
+	} else {
+		ModifyRegister32(1 << dam_transmit_frame_sync_direction_shift,
+				 direction <<
+				 dam_transmit_frame_sync_direction_shift,
+				 _reg_DAM_HPCR(port));
+	}
+	return;
+}
+
+/*!
+ * This function controls Transmit Frame Sync signal source for the port.
+ *
+ * @param        p_config          the DAM port to configure
+ * @param        from_RxFS         the signal comes from RxFS or TxFS of
+ *                                 the source port
+ * @param        p_source          the source port
+ */
+void dam_select_TxFS_source(dam_port p_config,
+			    bool from_RxFS, dam_port p_source)
+{
+	if (p_config < 3) {
+		ModifyRegister32(dam_fs_selection_mask <<
+				 dam_transmit_frame_sync_select_shift,
+				 ((from_RxFS << 3) | p_source) <<
+				 dam_transmit_frame_sync_select_shift,
+				 _reg_DAM_HPCR(p_config));
+	} else {
+		ModifyRegister32(dam_fs_selection_mask <<
+				 dam_transmit_frame_sync_select_shift,
+				 ((from_RxFS << 3) | p_source) <<
+				 dam_transmit_frame_sync_select_shift,
+				 _reg_DAM_PPCR(p_config));
+	}
+	return;
+}
+
+/*!
+ * This function sets a bit mask that selects the port from which of the RxD
+ * signals are to be ANDed together for internal network mode.
+ * Bit 6 represents RxD from Port7 and bit0 represents RxD from Port1.
+ * 1 excludes RxDn from ANDing. 0 includes RxDn for ANDing.
+ *
+ * @param        port              the DAM port to configure
+ * @param        bit_mask          the bit mask
+ *
+ * @return       This function returns the result of the operation
+ *               (0 if successful, -1 otherwise).
+ */
+int dam_set_internal_network_mode_mask(dam_port port, unsigned char bit_mask)
+{
+	int result;
+	result = 0;
+
+	ModifyRegister32(dam_internal_network_mode_mask <<
+			 dam_internal_network_mode_shift,
+			 bit_mask << dam_internal_network_mode_shift,
+			 _reg_DAM_HPCR(port));
+	return result;
+}
+
+/*!
+ * This function controls whether or not the port is in synchronous mode.
+ * When the synchronous mode is selected, the receive and the transmit sections
+ * use common clock and frame sync signals.
+ * When the synchronous mode is not selected, separate clock and frame sync
+ * signals are used for the transmit and the receive sections.
+ * The defaut value is the synchronous mode selected.
+ *
+ * @param        port              the DAM port to configure
+ * @param        synchronous       the state to assign
+ */
+void dam_set_synchronous(dam_port port, bool synchronous)
+{
+	if (port < 3) {
+		ModifyRegister32(1 << dam_synchronous_mode_shift,
+				 synchronous << dam_synchronous_mode_shift,
+				 _reg_DAM_HPCR(port));
+	} else {
+		ModifyRegister32(1 << dam_synchronous_mode_shift,
+				 synchronous << dam_synchronous_mode_shift,
+				 _reg_DAM_PPCR(port));
+	}
+	return;
+}
+
+/*!
+ * This function swaps the transmit and receive signals from (Da-TxD, Db-RxD)
+ * to (Da-RxD, Db-TxD).
+ * This default signal configuration is Da-TxD, Db-RxD.
+ *
+ * @param        port              the DAM port to configure
+ * @param        value             the switch state
+ */
+void dam_switch_Tx_Rx(dam_port port, bool value)
+{
+	if (port < 3) {
+		ModifyRegister32(1 << dam_transmit_receive_switch_shift,
+				 value << dam_transmit_receive_switch_shift,
+				 _reg_DAM_HPCR(port));
+	} else {
+		ModifyRegister32(1 << dam_transmit_receive_switch_shift,
+				 value << dam_transmit_receive_switch_shift,
+				 _reg_DAM_PPCR(port));
+	}
+	return;
+}
+
+/*!
+ * This function resets the two registers of the selected port.
+ *
+ * @param        port              the DAM port to reset
+ */
+void dam_reset_register(dam_port port)
+{
+	if (port < 3) {
+		ModifyRegister32(0xFFFFFFFF, dam_hpcr_default_value,
+				 _reg_DAM_HPCR(port));
+	} else {
+		ModifyRegister32(0xFFFFFFFF, dam_ppcr_default_value,
+				 _reg_DAM_PPCR(port));
+	}
+	return;
+}
+
+#ifdef TEST_DAM
+
+/*!
+ * This function implements IOCTL controls on a DAM device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @param        cmd         the command
+ * @param        arg         the parameter :\n
+ * DAM_CONFIG_SSI1:\n
+ * data from port 1 to port 4, clock and FS from port 1 (SSI1)\n
+ * DAM_CONFIG_SSI2:\n
+ * data from port 2 to port 5, clock and FS from port 2 (SSI2)\n
+ * DAM_CONFIG_SSI_NETWORK_MODE:\n
+ * network mode for mix digital with data from port 1 to port4,\n
+ * data from port 2 to port 4, clock and FS from port 1 (SSI1)
+ *
+ * @return       This function returns 0 if successful.
+ */
+static int dam_ioctl(struct inode *inode,
+		     struct file *file, unsigned int cmd, unsigned long arg)
+{
+	return 0;
+}
+
+/*!
+ * This function implements the open method on a DAM device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ *
+ * @return       This function returns 0.
+ */
+static int dam_open(struct inode *inode, struct file *file)
+{
+	/* DBG_PRINTK("ssi : dam_open()\n"); */
+	return 0;
+}
+
+/*!
+ * This function implements the release method on a DAM device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ *
+ * @return       This function returns 0.
+ */
+static int dam_free(struct inode *inode, struct file *file)
+{
+	/* DBG_PRINTK("ssi : dam_free()\n"); */
+	return 0;
+}
+
+/*!
+ * This structure defines file operations for a DAM device.
+ */
+static struct file_operations dam_fops = {
+
+	/*!
+	 * the owner
+	 */
+	.owner = THIS_MODULE,
+
+	/*!
+	 * the ioctl operation
+	 */
+	.ioctl = dam_ioctl,
+
+	/*!
+	 * the open operation
+	 */
+	.open = dam_open,
+
+	/*!
+	 * the release operation
+	 */
+	.release = dam_free,
+};
+
+#endif
+
+/*!
+ * This function implements the init function of the DAM device.
+ * This function is called when the module is loaded.
+ *
+ * @return       This function returns 0.
+ */
+static int __init dam_init(void)
+{
+#ifdef TEST_DAM
+	struct device *temp_class;
+	printk(KERN_DEBUG "dam : dam_init(void) \n");
+
+	major_dam = register_chrdev(0, DAM_NAME, &dam_fops);
+	if (major_dam < 0) {
+		printk(KERN_WARNING "Unable to get a major for dam");
+		return major_dam;
+	}
+
+	mxc_dam_class = class_create(THIS_MODULE, DAM_NAME);
+	if (IS_ERR(mxc_dam_class)) {
+		goto err_out;
+	}
+
+	temp_class = device_create(mxc_dam_class, NULL,
+				   MKDEV(major_dam, 0), NULL, DAM_NAME);
+	if (IS_ERR(temp_class)) {
+		goto err_out;
+	}
+#endif
+	return 0;
+
+      err_out:
+	printk(KERN_ERR "Error creating dam class device.\n");
+	device_destroy(mxc_dam_class, MKDEV(major_dam, 0));
+	class_destroy(mxc_dam_class);
+	unregister_chrdev(major_dam, DAM_NAME);
+	return -1;
+}
+
+/*!
+ * This function implements the exit function of the SPI device.
+ * This function is called when the module is unloaded.
+ *
+ */
+static void __exit dam_exit(void)
+{
+#ifdef TEST_DAM
+	device_destroy(mxc_dam_class, MKDEV(major_dam, 0));
+	class_destroy(mxc_dam_class);
+	unregister_chrdev(major_dam, DAM_NAME);
+	printk(KERN_DEBUG "dam : successfully unloaded\n");
+#endif
+}
+
+module_init(dam_init);
+module_exit(dam_exit);
+
+MODULE_DESCRIPTION("DAM char device driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/gps_ioctrl/Kconfig b/drivers/mxc/gps_ioctrl/Kconfig
new file mode 100644
index 000000000000..0a85d1636dd7
--- /dev/null
+++ b/drivers/mxc/gps_ioctrl/Kconfig
@@ -0,0 +1,13 @@
+#
+# BROADCOM GPS configuration
+#
+
+menu "Broadcom GPS ioctrl support"
+
+config GPS_IOCTRL
+	tristate "GPS ioctrl support"
+	depends on MACH_MX31_3DS || MACH_MX35_3DS || MACH_MX37_3DS || MACH_MX51_3DS
+	---help---
+	  Say Y to enable Broadcom GPS ioctrl on MXC platform.
+
+endmenu
diff --git a/drivers/mxc/gps_ioctrl/Makefile b/drivers/mxc/gps_ioctrl/Makefile
new file mode 100644
index 000000000000..42a48fe3bd7b
--- /dev/null
+++ b/drivers/mxc/gps_ioctrl/Makefile
@@ -0,0 +1,5 @@
+#

+# Makefile for the GPIO device driver module.

+#

+obj-$(CONFIG_GPS_IOCTRL) 	+= gps_gpiodrv.o

+gps_gpiodrv-objs := agpsgpiodev.o

diff --git a/drivers/mxc/gps_ioctrl/agpsgpiodev.c b/drivers/mxc/gps_ioctrl/agpsgpiodev.c
new file mode 100644
index 000000000000..cf2c2b4056bf
--- /dev/null
+++ b/drivers/mxc/gps_ioctrl/agpsgpiodev.c
@@ -0,0 +1,329 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file agpsgpiodev.c
+ *
+ * @brief Main file for GPIO kernel module. Contains driver entry/exit
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/fs.h>		/* Async notification */
+#include <asm/uaccess.h>	/* for get_user, put_user, access_ok */
+#include <linux/sched.h>	/* jiffies */
+#include <linux/poll.h>
+#include <linux/regulator/consumer.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/cdev.h>
+#include "agpsgpiodev.h"
+
+extern void gpio_gps_active(void);
+extern void gpio_gps_inactive(void);
+extern int gpio_gps_access(int para);
+
+struct mxc_gps_platform_data *mxc_gps_ioctrl_data;
+static int Device_Open;		/* Only allow a single user of this device */
+static struct cdev mxc_gps_cdev;
+static dev_t agps_gpio_dev;
+static struct class *gps_class;
+static struct device *gps_class_dev;
+
+/* Write GPIO from user space */
+static int ioctl_writegpio(int arg)
+{
+
+	/* Bit 0 of arg identifies the GPIO pin to write:
+	   0 = GPS_RESET_GPIO, 1 = GPS_POWER_GPIO.
+	   Bit 1 of arg identifies the value to write (0 or 1). */
+
+	/* Bit 2 should be 0 to show this access is write */
+	return gpio_gps_access(arg & (~0x4));
+}
+
+/* Read GPIO from user space */
+static int ioctl_readgpio(int arg)
+{
+	/* Bit 0 of arg identifies the GPIO pin to read:
+	   0 = GPS_RESET_GPIO. 1 = GPS_POWER_GPIO
+	   Bit 2 should be 1 to show this access is read */
+	return gpio_gps_access(arg | 0x4);
+}
+
+static int device_open(struct inode *inode, struct file *fp)
+{
+	/* We don't want to talk to two processes at the same time. */
+	if (Device_Open) {
+		printk(KERN_DEBUG "device_open() - Returning EBUSY. \
+			Device already open... \n");
+		return -EBUSY;
+	}
+	Device_Open++;		/* BUGBUG : Not protected! */
+	try_module_get(THIS_MODULE);
+
+	return 0;
+}
+
+static int device_release(struct inode *inode, struct file *fp)
+{
+	/* We're now ready for our next caller */
+	Device_Open--;
+	module_put(THIS_MODULE);
+
+	return 0;
+}
+
+static int device_ioctl(struct inode *inode, struct file *fp,
+			unsigned int cmd, unsigned long arg)
+{
+	int err = 0;
+
+	/* Extract the type and number bitfields, and don't decode wrong cmds.
+	   Return ENOTTY (inappropriate ioctl) before access_ok() */
+	if (_IOC_TYPE(cmd) != MAJOR_NUM) {
+		printk(KERN_ERR
+		       "device_ioctl() - Error! IOC_TYPE = %d. Expected %d\n",
+		       _IOC_TYPE(cmd), MAJOR_NUM);
+		return -ENOTTY;
+	}
+	if (_IOC_NR(cmd) > IOCTL_MAXNUMBER) {
+		printk(KERN_ERR
+		       "device_ioctl() - Error!"
+		       "IOC_NR = %d greater than max supported(%d)\n",
+		       _IOC_NR(cmd), IOCTL_MAXNUMBER);
+		return -ENOTTY;
+	}
+
+	/* The direction is a bitmask, and VERIFY_WRITE catches R/W transfers.
+	   `Type' is user-oriented, while access_ok is kernel-oriented, so the
+	   concept of "read" and "write" is reversed. I think this is primarily
+	   for good coding practice. You can easily do any kind of R/W access
+	   without these checks and IOCTL code can be implemented "randomly"! */
+	if (_IOC_DIR(cmd) & _IOC_READ)
+		err =
+		    !access_ok(VERIFY_WRITE, (void __user *)arg,
+			       _IOC_SIZE(cmd));
+
+	else if (_IOC_DIR(cmd) & _IOC_WRITE)
+		err =
+		    !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	if (err) {
+		printk(KERN_ERR
+		       "device_ioctl() - Error! User arg not valid"
+		       "for selected access (R/W/RW). Cmd %d\n",
+		       _IOC_TYPE(cmd));
+		return -EFAULT;
+	}
+
+	/* Note: Read and writing data to user buffer can be done using regular
+	   pointer stuff but we may also use get_user() or put_user() */
+
+	/* Cmd and arg has been verified... */
+	switch (cmd) {
+	case IOCTL_WRITEGPIO:
+		return ioctl_writegpio((int)arg);
+	case IOCTL_READGPIO:
+		return ioctl_readgpio((int)arg);
+	default:
+		printk(KERN_ERR "device_ioctl() - Invalid IOCTL (0x%x)\n", cmd);
+		return EINVAL;
+	}
+	return 0;
+}
+
+struct file_operations Fops = {
+	.ioctl = device_ioctl,
+	.open = device_open,
+	.release = device_release,
+};
+
+/* Initialize the module - Register the character device */
+int init_chrdev(struct device *dev)
+{
+	int ret, gps_major;
+
+	ret = alloc_chrdev_region(&agps_gpio_dev, 1, 1,	"agps_gpio");
+	gps_major = MAJOR(agps_gpio_dev);
+	if (ret < 0) {
+		dev_err(dev, "can't get major %d\n", gps_major);
+		goto err3;
+	}
+
+	cdev_init(&mxc_gps_cdev, &Fops);
+	mxc_gps_cdev.owner = THIS_MODULE;
+
+	ret = cdev_add(&mxc_gps_cdev, agps_gpio_dev, 1);
+	if (ret) {
+		dev_err(dev, "can't add cdev\n");
+		goto err2;
+	}
+
+	/* create class and device for udev information */
+	gps_class = class_create(THIS_MODULE, "gps");
+	if (IS_ERR(gps_class)) {
+		dev_err(dev, "failed to create gps class\n");
+		ret = -ENOMEM;
+		goto err1;
+	}
+
+	gps_class_dev = device_create(gps_class, NULL, MKDEV(gps_major, 1), NULL,
+					AGPSGPIO_DEVICE_FILE_NAME);
+	if (IS_ERR(gps_class_dev)) {
+		dev_err(dev, "failed to create gps gpio class device\n");
+		ret = -ENOMEM;
+		goto err0;
+	}
+
+	return 0;
+err0:
+	class_destroy(gps_class);
+err1:
+	cdev_del(&mxc_gps_cdev);
+err2:
+	unregister_chrdev_region(agps_gpio_dev, 1);
+err3:
+	return ret;
+}
+
+/* Cleanup - unregister the appropriate file from /proc. */
+void cleanup_chrdev(void)
+{
+	/* destroy gps device class */
+	device_destroy(gps_class, MKDEV(MAJOR(agps_gpio_dev), 1));
+	class_destroy(gps_class);
+
+	/* Unregister the device */
+	cdev_del(&mxc_gps_cdev);
+	unregister_chrdev_region(agps_gpio_dev, 1);
+}
+
+/*!
+ * This function initializes the driver in terms of memory of the soundcard
+ * and some basic HW clock settings.
+ *
+ * @return              0 on success, -1 otherwise.
+ */
+static int __init gps_ioctrl_probe(struct platform_device *pdev)
+{
+	struct regulator *gps_regu;
+
+	mxc_gps_ioctrl_data =
+	    (struct mxc_gps_platform_data *)pdev->dev.platform_data;
+
+	/* open GPS GPO3 1v8 for GL gps support */
+	if (mxc_gps_ioctrl_data->core_reg != NULL) {
+		mxc_gps_ioctrl_data->gps_regu_core =
+		    regulator_get(&(pdev->dev), mxc_gps_ioctrl_data->core_reg);
+		gps_regu = mxc_gps_ioctrl_data->gps_regu_core;
+		if (!IS_ERR_VALUE((u32)gps_regu)) {
+			regulator_set_voltage(gps_regu, 1800000, 1800000);
+			regulator_enable(gps_regu);
+		} else {
+			return -1;
+		}
+	}
+	/* open GPS GPO1 2v8 for GL gps support */
+	if (mxc_gps_ioctrl_data->analog_reg != NULL) {
+		mxc_gps_ioctrl_data->gps_regu_analog =
+		    regulator_get(&(pdev->dev),
+				  mxc_gps_ioctrl_data->analog_reg);
+		gps_regu = mxc_gps_ioctrl_data->gps_regu_analog;
+		if (!IS_ERR_VALUE((u32)gps_regu)) {
+			regulator_set_voltage(gps_regu, 2800000, 2800000);
+			regulator_enable(gps_regu);
+		} else {
+			return -1;
+		}
+	}
+	gpio_gps_active();
+
+	/* Register character device */
+	init_chrdev(&(pdev->dev));
+	return 0;
+}
+
+static int gps_ioctrl_remove(struct platform_device *pdev)
+{
+	struct regulator *gps_regu;
+
+	mxc_gps_ioctrl_data =
+	    (struct mxc_gps_platform_data *)pdev->dev.platform_data;
+
+	/* Character device cleanup.. */
+	cleanup_chrdev();
+	gpio_gps_inactive();
+
+	/* close GPS GPO3 1v8 for GL gps */
+	gps_regu = mxc_gps_ioctrl_data->gps_regu_core;
+	if (mxc_gps_ioctrl_data->core_reg != NULL) {
+		regulator_disable(gps_regu);
+		regulator_put(gps_regu);
+	}
+	/* close GPS GPO1 2v8 for GL gps */
+	gps_regu = mxc_gps_ioctrl_data->gps_regu_analog;
+	if (mxc_gps_ioctrl_data->analog_reg != NULL) {
+		regulator_disable(gps_regu);
+		regulator_put(gps_regu);
+	}
+
+	return 0;
+}
+
+static int gps_ioctrl_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	/* PowerEn toggle off */
+	ioctl_writegpio(0x1);
+	return 0;
+}
+
+static int gps_ioctrl_resume(struct platform_device *pdev)
+{
+	/* PowerEn pull up */
+	ioctl_writegpio(0x3);
+	return 0;
+}
+
+static struct platform_driver gps_ioctrl_driver = {
+	.probe = gps_ioctrl_probe,
+	.remove = gps_ioctrl_remove,
+	.suspend = gps_ioctrl_suspend,
+	.resume = gps_ioctrl_resume,
+	.driver = {
+		   .name = "gps_ioctrl",
+		   },
+};
+
+/*!
+ * Entry point for GPS ioctrl module.
+ *
+ */
+static int __init gps_ioctrl_init(void)
+{
+	return platform_driver_register(&gps_ioctrl_driver);
+}
+
+/*!
+ * unloading module.
+ *
+ */
+static void __exit gps_ioctrl_exit(void)
+{
+	platform_driver_unregister(&gps_ioctrl_driver);
+}
+
+module_init(gps_ioctrl_init);
+module_exit(gps_ioctrl_exit);
+MODULE_DESCRIPTION("GPIO DEVICE DRIVER");
+MODULE_AUTHOR("Freescale Semiconductor");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/gps_ioctrl/agpsgpiodev.h b/drivers/mxc/gps_ioctrl/agpsgpiodev.h
new file mode 100644
index 000000000000..815890578f4a
--- /dev/null
+++ b/drivers/mxc/gps_ioctrl/agpsgpiodev.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file agpsgpiodev.h
+ *
+ * @brief head file of Simple character device interface for AGPS kernel module.
+ *
+ * @ingroup
+ */
+
+#ifndef AGPSGPIODEV_H
+#define AGPSGPIODEV_H
+
+#include <linux/ioctl.h>
+
+#define USE_BLOCKING		/* Test driver with blocking calls */
+#undef USE_FASYNC		/* Test driver with async notification */
+
+/* The major device number. We can't rely on dynamic registration any more
+   because ioctls need to know it */
+#define MAJOR_NUM 100
+
+#define IOCTL_WRITEGPIO   _IOWR(MAJOR_NUM, 1, char *)
+#define IOCTL_READGPIO    _IOR(MAJOR_NUM, 2, char *)
+#define IOCTL_MAXNUMBER   2
+
+/* The name of the device file */
+#define AGPSGPIO_DEVICE_FILE_NAME "agpsgpio"
+
+/* Exported prototypes */
+int init_chrdev(struct device *dev);
+void cleanup_chrdev(void);
+void wakeup(void);
+
+#endif
diff --git a/drivers/mxc/hmp4e/Kconfig b/drivers/mxc/hmp4e/Kconfig
new file mode 100644
index 000000000000..fdd7dbc041ba
--- /dev/null
+++ b/drivers/mxc/hmp4e/Kconfig
@@ -0,0 +1,24 @@
+#
+# MPEG4 Encoder kernel module  configuration
+#
+
+menu "MXC MPEG4 Encoder Kernel module support"
+
+config MXC_HMP4E
+	tristate "MPEG4 Encoder support"
+	depends on ARCH_MXC
+	depends on !ARCH_MX27
+	default y
+	---help---
+	  Say Y to get the MPEG4 Encoder kernel module available on
+	  MXC platform.
+
+config MXC_HMP4E_DEBUG
+	bool "MXC MPEG4 Debug messages"
+	depends on MXC_HMP4E != n
+	default n
+	---help---
+	  Say Y here if you need the Encoder driver to print debug messages.
+	  This is an option for developers, most people should say N here.
+
+endmenu
diff --git a/drivers/mxc/hmp4e/Makefile b/drivers/mxc/hmp4e/Makefile
new file mode 100644
index 000000000000..0efe11fbdbc8
--- /dev/null
+++ b/drivers/mxc/hmp4e/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for the MPEG4 Encoder kernel module.
+
+obj-$(CONFIG_MXC_HMP4E) 		+= mxc_hmp4e.o
+
+ifeq ($(CONFIG_MXC_HMP4E_DEBUG),y)
+EXTRA_CFLAGS += -DDEBUG
+endif
diff --git a/drivers/mxc/hmp4e/mxc_hmp4e.c b/drivers/mxc/hmp4e/mxc_hmp4e.c
new file mode 100644
index 000000000000..f795378cdae0
--- /dev/null
+++ b/drivers/mxc/hmp4e/mxc_hmp4e.c
@@ -0,0 +1,811 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * Encoder device driver (kernel module)
+ *
+ * Copyright (C) 2005  Hantro Products Oy.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>		/*  __init,__exit directives */
+#include <linux/mm.h>		/* remap_page_range / remap_pfn_range */
+#include <linux/fs.h>		/* for struct file_operations */
+#include <linux/errno.h>	/* standard error codes */
+#include <linux/platform_device.h>	/* for device registeration for PM */
+#include <linux/delay.h>	/* for msleep_interruptible */
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>	/* for dma_alloc_consistent */
+#include <linux/clk.h>
+#include <asm/uaccess.h>	/* for ioctl __get_user, __put_user */
+#include "mxc_hmp4e.h"		/* MPEG4 encoder specific */
+
+/* here's all the must remember stuff */
+typedef struct {
+	ulong buffaddr;
+	u32 buffsize;
+	ulong iobaseaddr;
+	u32 iosize;
+	volatile u32 *hwregs;
+	u32 irq;
+	u16 hwid_offset;
+	u16 intr_offset;
+	u16 busy_offset;
+	u16 type;		/* Encoder type, CIF = 0, VGA = 1 */
+	u16 clk_gate;
+	u16 busy_val;
+	struct fasync_struct *async_queue;
+#ifdef CONFIG_PM
+	s32 suspend_state;
+	wait_queue_head_t power_queue;
+#endif
+} hmp4e_t;
+
+/* and this is our MAJOR; use 0 for dynamic allocation (recommended)*/
+static s32 hmp4e_major;
+
+static u32 hmp4e_phys;
+static struct class *hmp4e_class;
+static hmp4e_t hmp4e_data;
+
+/*! MPEG4 enc clock handle. */
+static struct clk *hmp4e_clk;
+
+/*
+ * avoid "enable_irq(x) unbalanced from ..."
+ * error messages from the kernel, since {ena,dis}able_irq()
+ * calls are stacked in kernel.
+ */
+static bool irq_enable = false;
+
+ulong base_port = MPEG4_ENC_BASE_ADDR;
+u32 irq = MXC_INT_MPEG4_ENC;
+
+module_param(base_port, long, 000);
+module_param(irq, int, 000);
+
+/*!
+ * These variables store the register values when HMP4E is in suspend mode.
+ */
+#ifdef CONFIG_PM
+u32 io_regs[64];
+#endif
+
+static s32 hmp4e_map_buffer(struct file *filp, struct vm_area_struct *vma);
+static s32 hmp4e_map_hwregs(struct file *filp, struct vm_area_struct *vma);
+static void hmp4e_reset(hmp4e_t * dev);
+irqreturn_t hmp4e_isr(s32 irq, void *dev_id);
+
+/*!
+ * This funtion is called to write h/w register.
+ *
+ * @param   val		value to be written into the register
+ * @param   offset	register offset
+ *
+ */
+static inline void hmp4e_write(u32 val, u32 offset)
+{
+	hmp4e_t *dev = &hmp4e_data;
+	__raw_writel(val, (dev->hwregs + offset));
+}
+
+/*!
+ * This funtion is called to read h/w register.
+ *
+ * @param   offset	register offset
+ *
+ * @return  This function returns the value read from the register.
+ *
+ */
+static inline u32 hmp4e_read(u32 offset)
+{
+	hmp4e_t *dev = &hmp4e_data;
+	u32 val;
+
+	val = __raw_readl(dev->hwregs + offset);
+
+	return val;
+}
+
+/*!
+ * The device's mmap method. The VFS has kindly prepared the process's
+ * vm_area_struct for us, so we examine this to see what was requested.
+ *
+ * @param   filp	pointer to struct file
+ * @param   vma		pointer to struct vma_area_struct
+ *
+ * @return  This function returns 0 if successful or -ve value on error.
+ *
+ */
+static s32 hmp4e_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	s32 result;
+	ulong offset = vma->vm_pgoff << PAGE_SHIFT;
+
+	pr_debug("hmp4e_mmap: size = %lu off = 0x%08lx\n",
+		 (unsigned long)(vma->vm_end - vma->vm_start), offset);
+
+	if (offset == 0) {
+		result = hmp4e_map_buffer(filp, vma);
+	} else if (offset == hmp4e_data.iobaseaddr) {
+		result = hmp4e_map_hwregs(filp, vma);
+	} else {
+		pr_debug("hmp4e: mmap invalid value\n");
+		result = -EINVAL;
+	}
+
+	return result;
+}
+
+/*!
+ * This funtion is called to handle ioctls.
+ *
+ * @param   inode	pointer to struct inode
+ * @param   filp	pointer to struct file
+ * @param   cmd		ioctl command
+ * @param   arg		user data
+ *
+ * @return  This function returns 0 if successful or -ve value on error.
+ *
+ */
+static s32 hmp4e_ioctl(struct inode *inode, struct file *filp,
+		       u32 cmd, ulong arg)
+{
+	s32 err = 0, retval = 0;
+	ulong offset = 0;
+	hmp4e_t *dev = &hmp4e_data;
+	write_t bwrite;
+
+#ifdef CONFIG_PM
+	wait_event_interruptible(hmp4e_data.power_queue,
+				 hmp4e_data.suspend_state == 0);
+#endif
+
+	/*
+	 * extract the type and number bitfields, and don't decode
+	 * wrong cmds: return ENOTTY (inappropriate ioctl) before access_ok()
+	 */
+	if (_IOC_TYPE(cmd) != HMP4E_IOC_MAGIC) {
+		pr_debug("hmp4e: ioctl invalid magic\n");
+		return -ENOTTY;
+	}
+
+	if (_IOC_NR(cmd) > HMP4E_IOC_MAXNR) {
+		pr_debug("hmp4e: ioctl exceeds max ioctl\n");
+		return -ENOTTY;
+	}
+
+	/*
+	 * the direction is a bitmask, and VERIFY_WRITE catches R/W
+	 * transfers. `Type' is user-oriented, while
+	 * access_ok is kernel-oriented, so the concept of "read" and
+	 * "write" is reversed
+	 */
+	if (_IOC_DIR(cmd) & _IOC_READ) {
+		err = !access_ok(VERIFY_WRITE, (void *)arg, _IOC_SIZE(cmd));
+	} else if (_IOC_DIR(cmd) & _IOC_WRITE) {
+		err = !access_ok(VERIFY_READ, (void *)arg, _IOC_SIZE(cmd));
+	}
+
+	if (err) {
+		pr_debug("hmp4e: ioctl invalid direction\n");
+		return -EFAULT;
+	}
+
+	switch (cmd) {
+	case HMP4E_IOCHARDRESET:
+		break;
+
+	case HMP4E_IOCGBUFBUSADDRESS:
+		retval = __put_user((ulong) hmp4e_phys, (u32 *) arg);
+		break;
+
+	case HMP4E_IOCGBUFSIZE:
+		retval = __put_user(hmp4e_data.buffsize, (u32 *) arg);
+		break;
+
+	case HMP4E_IOCSREGWRITE:
+		if (dev->type != 1) {	/* This ioctl only for VGA */
+			pr_debug("hmp4e: HMP4E_IOCSREGWRITE invalid\n");
+			retval = -EINVAL;
+			break;
+		}
+
+		retval = __copy_from_user(&bwrite, (u32 *) arg,
+					  sizeof(write_t));
+
+		if (bwrite.offset <= hmp4e_data.iosize - 4) {
+			hmp4e_write(bwrite.data, (bwrite.offset / 4));
+		} else {
+			pr_debug("hmp4e: HMP4E_IOCSREGWRITE failed\n");
+			retval = -EFAULT;
+		}
+		break;
+
+	case HMP4E_IOCXREGREAD:
+		if (dev->type != 1) {	/* This ioctl only for VGA */
+			pr_debug("hmp4e: HMP4E_IOCSREGWRITE invalid\n");
+			retval = -EINVAL;
+			break;
+		}
+
+		retval = __get_user(offset, (ulong *) arg);
+		if (offset <= hmp4e_data.iosize - 4) {
+			__put_user(hmp4e_read((offset / 4)), (ulong *) arg);
+		} else {
+			pr_debug("hmp4e: HMP4E_IOCXREGREAD failed\n");
+			retval = -EFAULT;
+		}
+		break;
+
+	case HMP4E_IOCGHWOFFSET:
+		__put_user(hmp4e_data.iobaseaddr, (ulong *) arg);
+		break;
+
+	case HMP4E_IOCGHWIOSIZE:
+		__put_user(hmp4e_data.iosize, (u32 *) arg);
+		break;
+
+	case HMP4E_IOC_CLI:
+		if (irq_enable == true) {
+			disable_irq(hmp4e_data.irq);
+			irq_enable = false;
+		}
+		break;
+
+	case HMP4E_IOC_STI:
+		if (irq_enable == false) {
+			enable_irq(hmp4e_data.irq);
+			irq_enable = true;
+		}
+		break;
+
+	default:
+		pr_debug("unknown case %x\n", cmd);
+	}
+
+	return retval;
+}
+
+/*!
+ * This funtion is called when the device is opened.
+ *
+ * @param   inode	pointer to struct inode
+ * @param   filp	pointer to struct file
+ *
+ * @return  This function returns 0 if successful or -ve value on error.
+ *
+ */
+static s32 hmp4e_open(struct inode *inode, struct file *filp)
+{
+	hmp4e_t *dev = &hmp4e_data;
+
+	filp->private_data = (void *)dev;
+
+	if (request_irq(dev->irq, hmp4e_isr, 0, "mxc_hmp4e", dev) != 0) {
+		pr_debug("hmp4e: request irq failed\n");
+		return -EBUSY;
+	}
+
+	if (irq_enable == false) {
+		irq_enable = true;
+	}
+	clk_enable(hmp4e_clk);
+	return 0;
+}
+
+static s32 hmp4e_fasync(s32 fd, struct file *filp, s32 mode)
+{
+	hmp4e_t *dev = (hmp4e_t *) filp->private_data;
+	return fasync_helper(fd, filp, mode, &dev->async_queue);
+}
+
+/*!
+ * This funtion is called when the device is closed.
+ *
+ * @param   inode	pointer to struct inode
+ * @param   filp	pointer to struct file
+ *
+ * @return  This function returns 0.
+ *
+ */
+static s32 hmp4e_release(struct inode *inode, struct file *filp)
+{
+	hmp4e_t *dev = (hmp4e_t *) filp->private_data;
+
+	/* this is necessary if user process exited asynchronously */
+	if (irq_enable == true) {
+		disable_irq(dev->irq);
+		irq_enable = false;
+	}
+
+	/* reset hardware */
+	hmp4e_reset(&hmp4e_data);
+
+	/* free the encoder IRQ */
+	free_irq(dev->irq, (void *)dev);
+
+	/* remove this filp from the asynchronusly notified filp's */
+	hmp4e_fasync(-1, filp, 0);
+	clk_disable(hmp4e_clk);
+	return 0;
+}
+
+/* VFS methods */
+static struct file_operations hmp4e_fops = {
+	.owner = THIS_MODULE,
+	.open = hmp4e_open,
+	.release = hmp4e_release,
+	.ioctl = hmp4e_ioctl,
+	.mmap = hmp4e_mmap,
+	.fasync = hmp4e_fasync,
+};
+
+/*!
+ * This funtion allocates physical contigous memory.
+ *
+ * @param   size 	size of memory to be allocated
+ *
+ * @return  This function returns 0 if successful or -ve value on error.
+ *
+ */
+static s32 hmp4e_alloc(u32 size)
+{
+	hmp4e_data.buffsize = PAGE_ALIGN(size);
+	hmp4e_data.buffaddr =
+	    (ulong) dma_alloc_coherent(NULL, hmp4e_data.buffsize,
+				       (dma_addr_t *) & hmp4e_phys,
+				       GFP_DMA | GFP_KERNEL);
+
+	if (hmp4e_data.buffaddr == 0) {
+		printk(KERN_ERR "hmp4e: couldn't allocate data buffer\n");
+		return -ENOMEM;
+	}
+
+	memset((s8 *) hmp4e_data.buffaddr, 0, hmp4e_data.buffsize);
+	return 0;
+}
+
+/*!
+ * This funtion frees the DMAed memory.
+ */
+static void hmp4e_free(void)
+{
+	if (hmp4e_data.buffaddr != 0) {
+		dma_free_coherent(NULL, hmp4e_data.buffsize,
+				  (void *)hmp4e_data.buffaddr, hmp4e_phys);
+		hmp4e_data.buffaddr = 0;
+	}
+}
+
+/*!
+ * This funtion maps the shared buffer in memory.
+ *
+ * @param   filp 	pointer to struct file
+ * @param   vma		pointer to struct vm_area_struct
+ *
+ * @return  This function returns 0 if successful or -ve value on error.
+ *
+ */
+static s32 hmp4e_map_buffer(struct file *filp, struct vm_area_struct *vma)
+{
+	ulong phys;
+	ulong start = (u32) vma->vm_start;
+	ulong size = (u32) (vma->vm_end - vma->vm_start);
+
+	/* if userspace tries to mmap beyond end of our buffer, fail */
+	if (size > hmp4e_data.buffsize) {
+		pr_debug("hmp4e: hmp4e_map_buffer, invalid size\n");
+		return -EINVAL;
+	}
+
+	vma->vm_flags |= VM_RESERVED | VM_IO;
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	phys = hmp4e_phys;
+
+	if (remap_pfn_range(vma, start, phys >> PAGE_SHIFT, size,
+			    vma->vm_page_prot)) {
+		pr_debug("hmp4e: failed mmapping shared buffer\n");
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+
+/*!
+ * This funtion maps the h/w register space in memory.
+ *
+ * @param   filp 	pointer to struct file
+ * @param   vma		pointer to struct vm_area_struct
+ *
+ * @return  This function returns 0 if successful or -ve value on error.
+ *
+ */
+static s32 hmp4e_map_hwregs(struct file *filp, struct vm_area_struct *vma)
+{
+	ulong phys;
+	ulong start = (unsigned long)vma->vm_start;
+	ulong size = (unsigned long)(vma->vm_end - vma->vm_start);
+
+	/* if userspace tries to mmap beyond end of our buffer, fail */
+	if (size > PAGE_SIZE) {
+		pr_debug("hmp4e: hmp4e_map_hwregs, invalid size\n");
+		return -EINVAL;
+	}
+
+	vma->vm_flags |= VM_RESERVED | VM_IO;
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	/* Remember this won't work for vmalloc()d memory ! */
+	phys = hmp4e_data.iobaseaddr;
+
+	if (remap_pfn_range(vma, start, phys >> PAGE_SHIFT, hmp4e_data.iosize,
+			    vma->vm_page_prot)) {
+		pr_debug("hmp4e: failed mmapping HW registers\n");
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+
+/*!
+ * This function is the interrupt service routine.
+ *
+ * @param   irq		the irq number
+ * @param   dev_id	driver data when ISR was regiatered
+ *
+ * @return  The return value is IRQ_HANDLED.
+ *
+ */
+irqreturn_t hmp4e_isr(s32 irq, void *dev_id)
+{
+	hmp4e_t *dev = (hmp4e_t *) dev_id;
+	u32 offset = dev->intr_offset;
+
+	u32 irq_status = hmp4e_read(offset);
+
+	/* clear enc IRQ */
+	hmp4e_write(irq_status & (~0x01), offset);
+
+	if (dev->async_queue)
+		kill_fasync(&dev->async_queue, SIGIO, POLL_IN);
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * This function is called to reset the encoder.
+ *
+ * @param   dev	 pointer to struct hmp4e_data
+ *
+ */
+static void hmp4e_reset(hmp4e_t * dev)
+{
+	s32 i;
+
+	/* enable HCLK for register reset */
+	hmp4e_write(dev->clk_gate, 0);
+
+	/* Reset registers, except ECR0 (0x00) and ID (read-only) */
+	for (i = 1; i < (dev->iosize / 4); i += 1) {
+		if (i == dev->hwid_offset)	/* ID is read only */
+			continue;
+
+		/* Only for CIF, not used */
+		if ((dev->type == 0) && (i == 14))
+			continue;
+
+		hmp4e_write(0, i);
+	}
+
+	/* disable HCLK */
+	hmp4e_write(0, 0);
+	return;
+}
+
+/*!
+ * This function is called during the driver binding process. This function
+ * does the hardware initialization.
+ *
+ * @param   dev   the device structure used to store device specific
+ *                information that is used by the suspend, resume and remove
+ *                functions
+ *
+ * @return  The function returns 0 if successful.
+ */
+static s32 hmp4e_probe(struct platform_device *pdev)
+{
+	s32 result;
+	u32 hwid;
+	struct device *temp_class;
+
+	hmp4e_data.iobaseaddr = base_port;
+	hmp4e_data.irq = irq;
+	hmp4e_data.buffaddr = 0;
+
+	/* map hw i/o registers into kernel space */
+	hmp4e_data.hwregs = (volatile void *)IO_ADDRESS(hmp4e_data.iobaseaddr);
+
+	hmp4e_clk = clk_get(&pdev->dev, "mpeg4_clk");
+	if (IS_ERR(hmp4e_clk)) {
+		printk(KERN_INFO "hmp4e: Unable to get clock\n");
+		return -EIO;
+	}
+
+	clk_enable(hmp4e_clk);
+
+	/* check hw id for encoder signature */
+	hwid = hmp4e_read(7);
+	if ((hwid & 0xffff) == 0x1882) {	/* CIF first */
+		hmp4e_data.type = 0;
+		hmp4e_data.iosize = (16 * 4);
+		hmp4e_data.hwid_offset = 7;
+		hmp4e_data.intr_offset = 5;
+		hmp4e_data.clk_gate = (1 << 1);
+		hmp4e_data.buffsize = 512000;
+		hmp4e_data.busy_offset = 0;
+		hmp4e_data.busy_val = 1;
+	} else {
+		hwid = hmp4e_read((0x88 / 4));
+		if ((hwid & 0xffff0000) == 0x52510000) {	/* VGA */
+			hmp4e_data.type = 1;
+			hmp4e_data.iosize = (35 * 4);
+			hmp4e_data.hwid_offset = (0x88 / 4);
+			hmp4e_data.intr_offset = (0x10 / 4);
+			hmp4e_data.clk_gate = (1 << 12);
+			hmp4e_data.buffsize = 1048576;
+			hmp4e_data.busy_offset = (0x10 / 4);
+			hmp4e_data.busy_val = (1 << 1);
+		} else {
+			printk(KERN_INFO "hmp4e: HW ID not found\n");
+			goto error1;
+		}
+	}
+
+	/* Reset hardware */
+	hmp4e_reset(&hmp4e_data);
+
+	/* allocate memory shared with ewl */
+	result = hmp4e_alloc(hmp4e_data.buffsize);
+	if (result < 0)
+		goto error1;
+
+	result = register_chrdev(hmp4e_major, "hmp4e", &hmp4e_fops);
+	if (result <= 0) {
+		pr_debug("hmp4e: unable to get major %d\n", hmp4e_major);
+		goto error2;
+	}
+
+	hmp4e_major = result;
+
+	hmp4e_class = class_create(THIS_MODULE, "hmp4e");
+	if (IS_ERR(hmp4e_class)) {
+		pr_debug("Error creating hmp4e class.\n");
+		goto error3;
+	}
+
+	temp_class = device_create(hmp4e_class, NULL, MKDEV(hmp4e_major, 0), NULL,
+				   "hmp4e");
+	if (IS_ERR(temp_class)) {
+		pr_debug("Error creating hmp4e class device.\n");
+		goto error4;
+	}
+
+	platform_set_drvdata(pdev, &hmp4e_data);
+
+#ifdef CONFIG_PM
+	hmp4e_data.async_queue = NULL;
+	hmp4e_data.suspend_state = 0;
+	init_waitqueue_head(&hmp4e_data.power_queue);
+#endif
+
+	printk(KERN_INFO "hmp4e: %s encoder initialized\n",
+	       hmp4e_data.type ? "VGA" : "CIF");
+	clk_disable(hmp4e_clk);
+	return 0;
+
+      error4:
+	class_destroy(hmp4e_class);
+      error3:
+	unregister_chrdev(hmp4e_major, "hmp4e");
+      error2:
+	hmp4e_free();
+      error1:
+	clk_disable(hmp4e_clk);
+	clk_put(hmp4e_clk);
+	printk(KERN_INFO "hmp4e: module not inserted\n");
+	return -EIO;
+}
+
+/*!
+ * Dissociates the driver.
+ *
+ * @param   dev   the device structure
+ *
+ * @return  The function always returns 0.
+ */
+static s32 hmp4e_remove(struct platform_device *pdev)
+{
+	device_destroy(hmp4e_class, MKDEV(hmp4e_major, 0));
+	class_destroy(hmp4e_class);
+	unregister_chrdev(hmp4e_major, "hmp4e");
+	hmp4e_free();
+	clk_disable(hmp4e_clk);
+	clk_put(hmp4e_clk);
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/*!
+ * This is the suspend of power management for the Hantro MPEG4 module
+ *
+ * @param        dev            the device
+ * @param        state          the state
+ *
+ * @return       This function always returns 0.
+ */
+static s32 hmp4e_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	s32 i;
+	hmp4e_t *pdata = &hmp4e_data;
+
+	/*
+	 * how many times msleep_interruptible will be called before
+	 * giving up
+	 */
+	s32 timeout = 10;
+
+	pr_debug("hmp4e: Suspend\n");
+	hmp4e_data.suspend_state = 1;
+
+	/* check if encoder is currently running */
+	while ((hmp4e_read(pdata->busy_offset) & (pdata->busy_val)) &&
+	       --timeout) {
+		pr_debug("hmp4e: encoder is running, going to sleep\n");
+		msleep_interruptible((unsigned int)30);
+	}
+
+	if (!timeout) {
+		pr_debug("hmp4e: timeout suspending, resetting encoder\n");
+		hmp4e_write(hmp4e_read(pdata->busy_offset) &
+			    (~pdata->busy_val), pdata->busy_offset);
+	}
+
+	/* first read register 0 */
+	io_regs[0] = hmp4e_read(0);
+
+	/* then override HCLK to make sure other registers can be read */
+	hmp4e_write(pdata->clk_gate, 0);
+
+	/* read other registers */
+	for (i = 1; i < (pdata->iosize / 4); i += 1) {
+
+		/* Only for CIF, not used */
+		if ((pdata->type == 0) && (i == 14))
+			continue;
+
+		io_regs[i] = hmp4e_read(i);
+	}
+
+	/* restore value of register 0 */
+	hmp4e_write(io_regs[0], 0);
+
+	/* stop HCLK */
+	hmp4e_write(0, 0);
+	clk_disable(hmp4e_clk);
+	return 0;
+};
+
+/*!
+ * This is the resume of power management for the Hantro MPEG4 module
+ * It suports RESTORE state.
+ *
+ * @param        pdev            the platform device
+ *
+ * @return       This function always returns 0
+ */
+static s32 hmp4e_resume(struct platform_device *pdev)
+{
+	s32 i;
+	u32 status;
+	hmp4e_t *pdata = &hmp4e_data;
+
+	pr_debug("hmp4e: Resume\n");
+	clk_enable(hmp4e_clk);
+
+	/* override HCLK to make sure registers can be written */
+	hmp4e_write(pdata->clk_gate, 0x00);
+
+	for (i = 1; i < (pdata->iosize / 4); i += 1) {
+		if (i == pdata->hwid_offset)	/* Read only */
+			continue;
+
+		/* Only for CIF, not used */
+		if ((pdata->type == 0) && (i == 14))
+			continue;
+
+		hmp4e_write(io_regs[i], i);
+	}
+
+	/* write register 0 last */
+	hmp4e_write(io_regs[0], 0x00);
+
+	/* Clear the suspend flag */
+	hmp4e_data.suspend_state = 0;
+
+	/* Unblock the wait queue */
+	wake_up_interruptible(&hmp4e_data.power_queue);
+
+	/* Continue operations */
+	status = hmp4e_read(pdata->intr_offset);
+	if (status & 0x1) {
+		hmp4e_write(status & (~0x01), pdata->intr_offset);
+		if (hmp4e_data.async_queue)
+			kill_fasync(&hmp4e_data.async_queue, SIGIO, POLL_IN);
+	}
+
+	return 0;
+};
+
+#endif
+
+static struct platform_driver hmp4e_driver = {
+	.driver = {
+		   .name = "mxc_hmp4e",
+		   },
+	.probe = hmp4e_probe,
+	.remove = hmp4e_remove,
+#ifdef CONFIG_PM
+	.suspend = hmp4e_suspend,
+	.resume = hmp4e_resume,
+#endif
+};
+
+static s32 __init hmp4e_init(void)
+{
+	printk(KERN_INFO "hmp4e: init\n");
+	platform_driver_register(&hmp4e_driver);
+	return 0;
+}
+
+static void __exit hmp4e_cleanup(void)
+{
+	platform_driver_unregister(&hmp4e_driver);
+	printk(KERN_INFO "hmp4e: module removed\n");
+}
+
+module_init(hmp4e_init);
+module_exit(hmp4e_cleanup);
+
+/* module description */
+MODULE_AUTHOR("Hantro Products Oy");
+MODULE_DESCRIPTION("Device driver for Hantro's hardware based MPEG4 encoder");
+MODULE_SUPPORTED_DEVICE("5251/4251 MPEG4 Encoder");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/hmp4e/mxc_hmp4e.h b/drivers/mxc/hmp4e/mxc_hmp4e.h
new file mode 100644
index 000000000000..f58831716346
--- /dev/null
+++ b/drivers/mxc/hmp4e/mxc_hmp4e.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * Encoder device driver (kernel module headers)
+ *
+ * Copyright (C) 2005  Hantro Products Oy.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+#ifndef _HMP4ENC_H_
+#define _HMP4ENC_H_
+#include <linux/ioctl.h>	/* needed for the _IOW etc stuff used later */
+
+/* this is for writing data through ioctl to registers*/
+typedef struct {
+	unsigned long data;
+	unsigned long offset;
+} write_t;
+
+/*
+ * Ioctl definitions
+ */
+
+/* Use 'k' as magic number */
+#define HMP4E_IOC_MAGIC  'k'
+/*
+ * S means "Set" through a ptr,
+ * T means "Tell" directly with the argument value
+ * G means "Get": reply by setting through a pointer
+ * Q means "Query": response is on the return value
+ * X means "eXchange": G and S atomically
+ * H means "sHift": T and Q atomically
+ */
+#define HMP4E_IOCGBUFBUSADDRESS	_IOR(HMP4E_IOC_MAGIC,  1, unsigned long *)
+#define HMP4E_IOCGBUFSIZE	_IOR(HMP4E_IOC_MAGIC,  2, unsigned int *)
+#define HMP4E_IOCGHWOFFSET     	_IOR(HMP4E_IOC_MAGIC,  3, unsigned long *)
+#define HMP4E_IOCGHWIOSIZE     	_IOR(HMP4E_IOC_MAGIC,  4, unsigned int *)
+#define HMP4E_IOC_CLI          	_IO(HMP4E_IOC_MAGIC,   5)
+#define HMP4E_IOC_STI          	_IO(HMP4E_IOC_MAGIC,   6)
+#define HMP4E_IOCHARDRESET 	_IO(HMP4E_IOC_MAGIC,   7)
+#define HMP4E_IOCSREGWRITE	_IOW(HMP4E_IOC_MAGIC,  8, write_t)
+#define HMP4E_IOCXREGREAD	_IOWR(HMP4E_IOC_MAGIC, 9, unsigned long)
+
+#define HMP4E_IOC_MAXNR 9
+
+#endif				/* !_HMP4ENC_H_ */
diff --git a/drivers/mxc/hw_event/Kconfig b/drivers/mxc/hw_event/Kconfig
new file mode 100644
index 000000000000..bcf479689501
--- /dev/null
+++ b/drivers/mxc/hw_event/Kconfig
@@ -0,0 +1,11 @@
+menu "MXC HARDWARE EVENT"
+
+config MXC_HWEVENT
+	bool "MXC Hardware Event Handler"
+	default y
+	depends on ARCH_MXC
+	help
+	  If you plan to use the Hardware Event Handler in the MXC, say
+	  Y here. If unsure, select Y.
+
+endmenu
diff --git a/drivers/mxc/hw_event/Makefile b/drivers/mxc/hw_event/Makefile
new file mode 100644
index 000000000000..a53fe2b45e04
--- /dev/null
+++ b/drivers/mxc/hw_event/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_MXC_HWEVENT)	+= mxc_hw_event.o
diff --git a/drivers/mxc/hw_event/mxc_hw_event.c b/drivers/mxc/hw_event/mxc_hw_event.c
new file mode 100644
index 000000000000..5451c1c68859
--- /dev/null
+++ b/drivers/mxc/hw_event/mxc_hw_event.c
@@ -0,0 +1,265 @@
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * mxc_hw_event.c
+ * Collect the hardware events, send to user by netlink
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netlink.h>
+#include <linux/sched.h>
+#include <linux/list.h>
+#include <linux/signal.h>
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+#include <net/sock.h>
+
+#include <mach/hw_events.h>
+
+#define EVENT_POOL_SIZE	10
+
+struct hw_event_elem {
+	struct mxc_hw_event event;
+	struct list_head list;
+};
+
+static struct sock *nl_event_sock;	/* netlink socket */
+static struct list_head event_head;
+static struct list_head free_head;
+static struct hw_event_elem events_pool[EVENT_POOL_SIZE];	/* event pool */
+static DEFINE_SPINLOCK(list_lock);
+static DECLARE_WAIT_QUEUE_HEAD(event_wq);
+static unsigned int seq;	/* send seq */
+static int initialized;
+static struct task_struct *hwevent_kthread;
+
+/*!
+ * main HW event handler thread
+ */
+static int hw_event_thread(void *data)
+{
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh = NULL;
+	unsigned int size;
+	struct hw_event_elem *event, *n;
+	LIST_HEAD(tmp_head);
+	DEFINE_WAIT(wait);
+
+	while (1) {
+
+		prepare_to_wait(&event_wq, &wait, TASK_INTERRUPTIBLE);
+		/* wait for event coming */
+		if (!freezing(current) && !kthread_should_stop() &&
+		    list_empty(&event_head))
+			schedule();
+		finish_wait(&event_wq, &wait);
+
+		try_to_freeze();
+
+		if (kthread_should_stop())
+			break;
+
+		/* fetch event from list */
+		spin_lock_irq(&list_lock);
+		tmp_head = event_head;
+		tmp_head.prev->next = &tmp_head;
+		tmp_head.next->prev = &tmp_head;
+		/* clear the event list head */
+		INIT_LIST_HEAD(&event_head);
+		spin_unlock_irq(&list_lock);
+
+		list_for_each_entry_safe(event, n, &tmp_head, list) {
+
+			size = NLMSG_SPACE(sizeof(struct mxc_hw_event));
+			skb = alloc_skb(size, GFP_KERNEL);
+			if (!skb) {
+				/* if failed alloc skb, we drop this event */
+				printk(KERN_WARNING
+				       "mxc_hw_event: skb_alloc() failed\n");
+				goto alloc_failure;
+			}
+
+			/* put the netlink header struct to skb */
+			nlh =
+			    NLMSG_PUT(skb, 0, seq++, NLMSG_DONE,
+				      size - sizeof(*nlh));
+
+			/* fill the netlink data */
+			memcpy((struct mxc_hw_event *)NLMSG_DATA(nlh),
+			       &event->event, sizeof(struct mxc_hw_event));
+
+			/* free the event node, set to unused */
+			spin_lock_irq(&list_lock);
+			list_move(&event->list, &free_head);
+			spin_unlock_irq(&list_lock);
+
+			/* send to all process that create this socket */
+			NETLINK_CB(skb).pid = 0;	/* sender pid */
+			NETLINK_CB(skb).dst_group = HW_EVENT_GROUP;
+			/* broadcast the event */
+			netlink_broadcast(nl_event_sock, skb, 0, HW_EVENT_GROUP,
+					  GFP_KERNEL);
+
+			continue;
+		      nlmsg_failure:
+			printk(KERN_WARNING
+			       "mxc_hw_event: No tailroom for NLMSG in skb\n");
+		      alloc_failure:
+			/* free the event node, set to unused */
+			spin_lock_irq(&list_lock);
+			list_del(&event->list);
+			list_add_tail(&event->list, &free_head);
+			spin_unlock_irq(&list_lock);
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ *
+ * @priority the event priority, REALTIME, EMERENCY, NORMAL
+ * @new_event event id to be send
+ */
+int hw_event_send(int priority, struct mxc_hw_event *new_event)
+{
+	unsigned int size;
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh = NULL;
+	struct mxc_hw_event *event;
+	struct hw_event_elem *event_elem;
+	int ret;
+	unsigned long flag;
+	struct list_head *list_node;
+
+	if (!initialized) {
+		pr_info("HW Event module has not been initialized\n");
+		return -1;
+	}
+
+	if (priority == HWE_HIGH_PRIORITY) {
+		/**
+		 * the most high priority event,
+		 * we send it immediatly.
+		 */
+
+		size = NLMSG_SPACE(sizeof(struct mxc_hw_event));
+
+		/* alloc skb */
+		if (in_interrupt()) {
+			skb = alloc_skb(size, GFP_ATOMIC);
+		} else {
+			skb = alloc_skb(size, GFP_KERNEL);
+		}
+		if (!skb) {
+			/* if failed alloc skb, we drop this event */
+			printk(KERN_WARNING
+			       "hw_event send: skb_alloc() failed\n");
+			goto send_later;
+		}
+
+		/* put the netlink header struct to skb */
+		nlh = NLMSG_PUT(skb, 0, seq++, NLMSG_DONE, size - sizeof(*nlh));
+
+		/* fill the netlink data */
+		event = (struct mxc_hw_event *)NLMSG_DATA(nlh);
+		memcpy(event, new_event, sizeof(struct mxc_hw_event));
+
+		/* send to all process that create this socket */
+		NETLINK_CB(skb).pid = 0;	/* sender pid */
+		NETLINK_CB(skb).dst_group = HW_EVENT_GROUP;
+		/* broadcast the event */
+		ret = netlink_broadcast(nl_event_sock, skb, 0, HW_EVENT_GROUP,
+					in_interrupt()? GFP_ATOMIC :
+					GFP_KERNEL);
+		if (ret) {
+
+		      nlmsg_failure:
+			/* send failed */
+			kfree_skb(skb);
+			goto send_later;
+		}
+
+		return 0;
+	}
+
+      send_later:
+	spin_lock_irqsave(&list_lock, flag);
+	if (list_empty(&free_head)) {
+		spin_unlock_irqrestore(&list_lock, flag);
+		/* no more free event node */
+		printk(KERN_WARNING "mxc_event send: no more free node\n");
+		return -1;
+	}
+
+	/* get a free node from free list, and added to event list */
+	list_node = free_head.next;
+	/* fill event */
+	event_elem = list_entry(list_node, struct hw_event_elem, list);
+	event_elem->event = *new_event;
+	list_move(list_node, &event_head);
+	spin_unlock_irqrestore(&list_lock, flag);
+
+	wake_up(&event_wq);
+
+	return 0;
+}
+
+static int __init mxc_hw_event_init(void)
+{
+	int i;
+
+	/* initial the list head for event and free */
+	INIT_LIST_HEAD(&free_head);
+	INIT_LIST_HEAD(&event_head);
+
+	/* initial the free list */
+	for (i = 0; i < EVENT_POOL_SIZE; i++)
+		list_add_tail(&events_pool[i].list, &free_head);
+
+	/* create netlink kernel sock */
+	nl_event_sock =
+	    netlink_kernel_create(&init_net, NETLINK_USERSOCK, 0, NULL, NULL,
+				  THIS_MODULE);
+	if (!nl_event_sock) {
+		printk(KERN_WARNING
+		       "mxc_hw_event: Fail to create netlink socket.\n");
+		return 1;
+	}
+
+	hwevent_kthread = kthread_run(hw_event_thread, NULL, "hwevent");
+	if (IS_ERR(hwevent_kthread)) {
+		printk(KERN_WARNING
+		       "mxc_hw_event: Fail to create hwevent thread.\n");
+		return 1;
+	}
+
+	initialized = 1;
+
+	return 0;
+}
+
+static void __exit mxc_hw_event_exit(void)
+{
+	kthread_stop(hwevent_kthread);
+	/* wait for thread completion */
+	sock_release(nl_event_sock->sk_socket);
+}
+
+module_init(mxc_hw_event_init);
+module_exit(mxc_hw_event_exit);
+
+EXPORT_SYMBOL(hw_event_send);
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/ipu/Kconfig b/drivers/mxc/ipu/Kconfig
new file mode 100644
index 000000000000..a57f8ce74758
--- /dev/null
+++ b/drivers/mxc/ipu/Kconfig
@@ -0,0 +1,5 @@
+config MXC_IPU_V1
+	bool
+
+source "drivers/mxc/ipu/pf/Kconfig"
+
diff --git a/drivers/mxc/ipu/Makefile b/drivers/mxc/ipu/Makefile
new file mode 100644
index 000000000000..4e9f19f9afa5
--- /dev/null
+++ b/drivers/mxc/ipu/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_MXC_IPU_V1) = mxc_ipu.o
+
+mxc_ipu-objs := ipu_common.o ipu_sdc.o ipu_adc.o ipu_ic.o ipu_csi.o ipu_device.o ipu_calc_stripes_sizes.o
+
+obj-$(CONFIG_MXC_IPU_PF)        +=   pf/
diff --git a/drivers/mxc/ipu/ipu_adc.c b/drivers/mxc/ipu/ipu_adc.c
new file mode 100644
index 000000000000..a66087d71e9e
--- /dev/null
+++ b/drivers/mxc/ipu/ipu_adc.c
@@ -0,0 +1,688 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * @file ipu_adc.c
+ *
+ * @brief IPU ADC functions
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+#include "ipu_param_mem.h"
+
+/*#define ADC_CHAN1_SA_MASK 0xFF800000 */
+
+static void _ipu_set_cmd_data_mappings(display_port_t disp,
+				       uint32_t pixel_fmt, int ifc_width);
+
+int32_t _ipu_adc_init_channel(ipu_channel_t chan, display_port_t disp,
+			      mcu_mode_t cmd, int16_t x_pos, int16_t y_pos)
+{
+	uint32_t reg;
+	uint32_t start_addr, stride;
+	unsigned long lock_flags;
+	uint32_t size;
+
+	size = 0;
+
+	switch (disp) {
+	case DISP0:
+		reg = __raw_readl(ADC_DISP0_CONF);
+		stride = reg & ADC_DISP_CONF_SL_MASK;
+		break;
+	case DISP1:
+		reg = __raw_readl(ADC_DISP1_CONF);
+		stride = reg & ADC_DISP_CONF_SL_MASK;
+		break;
+	case DISP2:
+		reg = __raw_readl(ADC_DISP2_CONF);
+		stride = reg & ADC_DISP_CONF_SL_MASK;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (stride == 0)
+		return -EINVAL;
+
+	stride++;
+	start_addr = (y_pos * stride) + x_pos;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+	reg = __raw_readl(ADC_CONF);
+
+	switch (chan) {
+	case ADC_SYS1:
+		reg &= ~0x00FF4000;
+		reg |=
+		    ((uint32_t) size << 21 | (uint32_t) disp << 19 | (uint32_t)
+		     cmd << 16);
+
+		__raw_writel(start_addr, ADC_SYSCHA1_SA);
+		break;
+
+	case ADC_SYS2:
+		reg &= ~0xFF008000;
+		reg |=
+		    ((uint32_t) size << 29 | (uint32_t) disp << 27 | (uint32_t)
+		     cmd << 24);
+
+		__raw_writel(start_addr, ADC_SYSCHA2_SA);
+		break;
+
+	case CSI_PRP_VF_ADC:
+	case MEM_PRP_VF_ADC:
+		reg &= ~0x000000F9;
+		reg |=
+		    ((uint32_t) size << 5 | (uint32_t) disp << 3 |
+		     ADC_CONF_PRP_EN);
+
+		__raw_writel(start_addr, ADC_PRPCHAN_SA);
+		break;
+
+	case MEM_PP_ADC:
+		reg &= ~0x00003F02;
+		reg |=
+		    ((uint32_t) size << 10 | (uint32_t) disp << 8 |
+		     ADC_CONF_PP_EN);
+
+		__raw_writel(start_addr, ADC_PPCHAN_SA);
+		break;
+	default:
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+		return -1;
+		break;
+	}
+	__raw_writel(reg, ADC_CONF);
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return 0;
+}
+
+int32_t _ipu_adc_uninit_channel(ipu_channel_t chan)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+	reg = __raw_readl(ADC_CONF);
+
+	switch (chan) {
+	case ADC_SYS1:
+		reg &= ~0x00FF4000;
+		break;
+	case ADC_SYS2:
+		reg &= ~0xFF008000;
+		break;
+	case CSI_PRP_VF_ADC:
+	case MEM_PRP_VF_ADC:
+		reg &= ~0x000000F9;
+		break;
+	case MEM_PP_ADC:
+		reg &= ~0x00003F02;
+		break;
+	default:
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+		return -1;
+		break;
+	}
+	__raw_writel(reg, ADC_CONF);
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return 0;
+}
+
+int32_t ipu_adc_write_template(display_port_t disp, uint32_t * pCmd, bool write)
+{
+	uint32_t ima_addr = 0;
+	uint32_t row_nu;
+	int i;
+
+	/* Set IPU_IMA_ADDR (IPU Internal Memory Access Address) */
+	/* MEM_NU = 0x0001 (CPM) */
+	/* ROW_NU = 2*N ( N is channel number) */
+	/* WORD_NU = 0  */
+	if (write) {
+		row_nu = (uint32_t) disp *2 * ATM_ADDR_RANGE;
+	} else {
+		row_nu = ((uint32_t) disp * 2 + 1) * ATM_ADDR_RANGE;
+	}
+
+	/* form template addr for IPU_IMA_ADDR */
+	ima_addr = (0x3 << 16 /*Template memory */  | row_nu << 3);
+
+	__raw_writel(ima_addr, IPU_IMA_ADDR);
+
+	/* write template data for IPU_IMA_DATA */
+	for (i = 0; i < TEMPLATE_BUF_SIZE; i++)
+		/* only DATA field are needed */
+		__raw_writel(pCmd[i], IPU_IMA_DATA);
+
+	return 0;
+}
+
+int32_t
+ipu_adc_write_cmd(display_port_t disp, cmddata_t type,
+		  uint32_t cmd, const uint32_t * params, uint16_t numParams)
+{
+	uint16_t i;
+	int disable_di = 0;
+	u32 reg;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+	reg = __raw_readl(IPU_CONF);
+	if ((reg & IPU_CONF_DI_EN) == 0) {
+		disable_di = 1;
+		reg |= IPU_CONF_DI_EN;
+		__raw_writel(reg, IPU_CONF);
+	}
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	__raw_writel((uint32_t) ((type ? 0x0 : 0x1) | disp << 1 | 0x10),
+		     DI_DISP_LLA_CONF);
+	__raw_writel(cmd, DI_DISP_LLA_DATA);
+	udelay(3);
+
+	__raw_writel((uint32_t) (0x10 | disp << 1 | 0x11), DI_DISP_LLA_CONF);
+	for (i = 0; i < numParams; i++) {
+		__raw_writel(params[i], DI_DISP_LLA_DATA);
+		udelay(3);
+	}
+
+	if (disable_di) {
+		spin_lock_irqsave(&ipu_lock, lock_flags);
+		reg = __raw_readl(IPU_CONF);
+		reg &= ~IPU_CONF_DI_EN;
+		__raw_writel(reg, IPU_CONF);
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	}
+
+	return 0;
+}
+
+int32_t ipu_adc_set_update_mode(ipu_channel_t channel,
+				ipu_adc_update_mode_t mode,
+				uint32_t refresh_rate, unsigned long addr,
+				uint32_t * size)
+{
+	int32_t err = 0;
+	uint32_t ref_per, reg, src = 0;
+	unsigned long lock_flags;
+	uint32_t ipu_freq;
+
+	ipu_freq = clk_get_rate(g_ipu_clk);
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(IPU_FS_DISP_FLOW);
+	reg &= ~FS_AUTO_REF_PER_MASK;
+	switch (mode) {
+	case IPU_ADC_REFRESH_NONE:
+		src = 0;
+		break;
+	case IPU_ADC_AUTO_REFRESH:
+		if (refresh_rate == 0) {
+			err = -EINVAL;
+			goto err0;
+		}
+		ref_per = ipu_freq / ((1UL << 17) * refresh_rate);
+		ref_per--;
+		reg |= ref_per << FS_AUTO_REF_PER_OFFSET;
+
+		src = FS_SRC_AUTOREF;
+		break;
+	case IPU_ADC_AUTO_REFRESH_SNOOP:
+		if (refresh_rate == 0) {
+			err = -EINVAL;
+			goto err0;
+		}
+		ref_per = ipu_freq / ((1UL << 17) * refresh_rate);
+		ref_per--;
+		reg |= ref_per << FS_AUTO_REF_PER_OFFSET;
+
+		src = FS_SRC_AUTOREF_SNOOP;
+		break;
+	case IPU_ADC_SNOOPING:
+		src = FS_SRC_SNOOP;
+		break;
+	}
+
+	switch (channel) {
+	case ADC_SYS1:
+		reg &= ~FS_ADC1_SRC_SEL_MASK;
+		reg |= src << FS_ADC1_SRC_SEL_OFFSET;
+		break;
+	case ADC_SYS2:
+		reg &= ~FS_ADC2_SRC_SEL_MASK;
+		reg |= src << FS_ADC2_SRC_SEL_OFFSET;
+		break;
+	default:
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+		return -EINVAL;
+	}
+	__raw_writel(reg, IPU_FS_DISP_FLOW);
+
+	/* Setup bus snooping */
+	if ((mode == IPU_ADC_AUTO_REFRESH_SNOOP) || (mode == IPU_ADC_SNOOPING)) {
+		err = mxc_snoop_set_config(0, addr, *size);
+		if (err > 0) {
+			*size = err;
+			err = 0;
+		}
+	} else {
+		mxc_snoop_set_config(0, 0, 0);
+	}
+
+      err0:
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return err;
+}
+
+int32_t ipu_adc_get_snooping_status(uint32_t * statl, uint32_t * stath)
+{
+	return mxc_snoop_get_status(0, statl, stath);
+}
+
+int32_t ipu_adc_init_panel(display_port_t disp,
+			   uint16_t width, uint16_t height,
+			   uint32_t pixel_fmt,
+			   uint32_t stride,
+			   ipu_adc_sig_cfg_t sig,
+			   display_addressing_t addr,
+			   uint32_t vsync_width, vsync_t mode)
+{
+	uint32_t temp;
+	unsigned long lock_flags;
+	uint32_t ser_conf;
+	uint32_t disp_conf;
+	uint32_t adc_disp_conf;
+	uint32_t adc_disp_vsync;
+	uint32_t old_pol;
+
+	if ((disp != DISP1) && (disp != DISP2) &&
+	    (sig.ifc_mode >= IPU_ADC_IFC_MODE_3WIRE_SERIAL)) {
+		return -EINVAL;
+	}
+/*        adc_disp_conf = ((uint32_t)((((size == 3)||(size == 2))?1:0)<<14) |  */
+/*                         (uint32_t)addr<<12 | (stride-1)); */
+	adc_disp_conf = (uint32_t) addr << 12 | (stride - 1);
+
+	_ipu_set_cmd_data_mappings(disp, pixel_fmt, sig.ifc_width);
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+	disp_conf = __raw_readl(DI_DISP_IF_CONF);
+	old_pol = __raw_readl(DI_DISP_SIG_POL);
+	adc_disp_vsync = __raw_readl(ADC_DISP_VSYNC);
+
+	switch (disp) {
+	case DISP0:
+		__raw_writel(adc_disp_conf, ADC_DISP0_CONF);
+		__raw_writel((((height - 1) << 16) | (width - 1)),
+			     ADC_DISP0_SS);
+
+		adc_disp_vsync &= ~(ADC_DISP_VSYNC_D0_MODE_MASK |
+				    ADC_DISP_VSYNC_D0_WIDTH_MASK);
+		adc_disp_vsync |= (vsync_width << 16) | (uint32_t) mode;
+
+		old_pol &= ~0x2000003FL;
+		old_pol |= sig.data_pol | sig.cs_pol << 1 |
+		    sig.addr_pol << 2 | sig.read_pol << 3 |
+		    sig.write_pol << 4 | sig.Vsync_pol << 5 |
+		    sig.burst_pol << 29;
+		__raw_writel(old_pol, DI_DISP_SIG_POL);
+
+		disp_conf &= ~0x0000001FL;
+		disp_conf |= (sig.burst_mode << 3) | (sig.ifc_mode << 1) |
+		    DI_CONF_DISP0_EN;
+		__raw_writel(disp_conf, DI_DISP_IF_CONF);
+		break;
+	case DISP1:
+		__raw_writel(adc_disp_conf, ADC_DISP1_CONF);
+		__raw_writel((((height - 1) << 16) | (width - 1)),
+			     ADC_DISP12_SS);
+
+		adc_disp_vsync &= ~(ADC_DISP_VSYNC_D12_MODE_MASK |
+				    ADC_DISP_VSYNC_D12_WIDTH_MASK);
+		adc_disp_vsync |= (vsync_width << 16) | (uint32_t) mode;
+
+		old_pol &= ~0x4000FF00L;
+		old_pol |= (sig.Vsync_pol << 6 | sig.data_pol << 8 |
+			    sig.cs_pol << 9 | sig.addr_pol << 10 |
+			    sig.read_pol << 11 | sig.write_pol << 12 |
+			    sig.clk_pol << 14 | sig.burst_pol << 30);
+		__raw_writel(old_pol, DI_DISP_SIG_POL);
+
+		disp_conf &= ~0x00003F00L;
+		if (sig.ifc_mode >= IPU_ADC_IFC_MODE_3WIRE_SERIAL) {
+			ser_conf = (sig.ifc_width - 1) <<
+			    DI_SER_DISPx_CONF_SER_BIT_NUM_OFFSET;
+			if (sig.ser_preamble_len) {
+				ser_conf |= DI_SER_DISPx_CONF_PREAMBLE_EN;
+				ser_conf |= sig.ser_preamble <<
+				    DI_SER_DISPx_CONF_PREAMBLE_OFFSET;
+				ser_conf |= (sig.ser_preamble_len - 1) <<
+				    DI_SER_DISPx_CONF_PREAMBLE_LEN_OFFSET;
+			}
+
+			ser_conf |=
+			    sig.ser_rw_mode << DI_SER_DISPx_CONF_RW_CFG_OFFSET;
+
+			if (sig.burst_mode == IPU_ADC_BURST_SERIAL)
+				ser_conf |= DI_SER_DISPx_CONF_BURST_MODE_EN;
+			__raw_writel(ser_conf, DI_SER_DISP1_CONF);
+		} else {	/* parallel interface */
+			disp_conf |= (uint32_t) (sig.burst_mode << 12);
+		}
+		disp_conf |= (sig.ifc_mode << 9) | DI_CONF_DISP1_EN;
+		__raw_writel(disp_conf, DI_DISP_IF_CONF);
+		break;
+	case DISP2:
+		__raw_writel(adc_disp_conf, ADC_DISP2_CONF);
+		__raw_writel((((height - 1) << 16) | (width - 1)),
+			     ADC_DISP12_SS);
+
+		adc_disp_vsync &= ~(ADC_DISP_VSYNC_D12_MODE_MASK |
+				    ADC_DISP_VSYNC_D12_WIDTH_MASK);
+		adc_disp_vsync |= (vsync_width << 16) | (uint32_t) mode;
+
+		old_pol &= ~0x80FF0000L;
+		temp = (uint32_t) (sig.data_pol << 16 | sig.cs_pol << 17 |
+				   sig.addr_pol << 18 | sig.read_pol << 19 |
+				   sig.write_pol << 20 | sig.Vsync_pol << 6 |
+				   sig.burst_pol << 31 | sig.clk_pol << 22);
+		__raw_writel(temp | old_pol, DI_DISP_SIG_POL);
+
+		disp_conf &= ~0x003F0000L;
+		if (sig.ifc_mode >= IPU_ADC_IFC_MODE_3WIRE_SERIAL) {
+			ser_conf = (sig.ifc_width - 1) <<
+			    DI_SER_DISPx_CONF_SER_BIT_NUM_OFFSET;
+			if (sig.ser_preamble_len) {
+				ser_conf |= DI_SER_DISPx_CONF_PREAMBLE_EN;
+				ser_conf |= sig.ser_preamble <<
+				    DI_SER_DISPx_CONF_PREAMBLE_OFFSET;
+				ser_conf |= (sig.ser_preamble_len - 1) <<
+				    DI_SER_DISPx_CONF_PREAMBLE_LEN_OFFSET;
+
+			}
+
+			ser_conf |=
+			    sig.ser_rw_mode << DI_SER_DISPx_CONF_RW_CFG_OFFSET;
+
+			if (sig.burst_mode == IPU_ADC_BURST_SERIAL)
+				ser_conf |= DI_SER_DISPx_CONF_BURST_MODE_EN;
+			__raw_writel(ser_conf, DI_SER_DISP2_CONF);
+		} else {	/* parallel interface */
+			disp_conf |= (uint32_t) (sig.burst_mode << 20);
+		}
+		disp_conf |= (sig.ifc_mode << 17) | DI_CONF_DISP2_EN;
+		__raw_writel(disp_conf, DI_DISP_IF_CONF);
+		break;
+	default:
+		break;
+	}
+
+	__raw_writel(adc_disp_vsync, ADC_DISP_VSYNC);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+
+int32_t ipu_adc_init_ifc_timing(display_port_t disp, bool read,
+				uint32_t cycle_time,
+				uint32_t up_time,
+				uint32_t down_time,
+				uint32_t read_latch_time, uint32_t pixel_clk)
+{
+	uint32_t reg;
+	uint32_t time_conf3 = 0;
+	uint32_t clk_per;
+	uint32_t up_per;
+	uint32_t down_per;
+	uint32_t read_per;
+	uint32_t pixclk_per = 0;
+	uint32_t ipu_freq;
+
+	ipu_freq = clk_get_rate(g_ipu_clk);
+
+	clk_per = (cycle_time * (ipu_freq / 1000L) * 16L) / 1000000L;
+	up_per = (up_time * (ipu_freq / 1000L) * 4L) / 1000000L;
+	down_per = (down_time * (ipu_freq / 1000L) * 4L) / 1000000L;
+
+	reg = (clk_per << DISPx_IF_CLK_PER_OFFSET) |
+	    (up_per << DISPx_IF_CLK_UP_OFFSET) |
+	    (down_per << DISPx_IF_CLK_DOWN_OFFSET);
+
+	if (read) {
+		read_per =
+		    (read_latch_time * (ipu_freq / 1000L) * 4L) / 1000000L;
+		if (pixel_clk)
+			pixclk_per = (ipu_freq * 16L) / pixel_clk;
+		time_conf3 = (read_per << DISPx_IF_CLK_READ_EN_OFFSET) |
+		    (pixclk_per << DISPx_PIX_CLK_PER_OFFSET);
+	}
+
+	dev_dbg(g_ipu_dev, "DI_DISPx_TIME_CONF_1/2 = 0x%08X\n", reg);
+	dev_dbg(g_ipu_dev, "DI_DISPx_TIME_CONF_3 = 0x%08X\n", time_conf3);
+
+	switch (disp) {
+	case DISP0:
+		if (read) {
+			__raw_writel(reg, DI_DISP0_TIME_CONF_2);
+			__raw_writel(time_conf3, DI_DISP0_TIME_CONF_3);
+		} else {
+			__raw_writel(reg, DI_DISP0_TIME_CONF_1);
+		}
+		break;
+	case DISP1:
+		if (read) {
+			__raw_writel(reg, DI_DISP1_TIME_CONF_2);
+			__raw_writel(time_conf3, DI_DISP1_TIME_CONF_3);
+		} else {
+			__raw_writel(reg, DI_DISP1_TIME_CONF_1);
+		}
+		break;
+	case DISP2:
+		if (read) {
+			__raw_writel(reg, DI_DISP2_TIME_CONF_2);
+			__raw_writel(time_conf3, DI_DISP2_TIME_CONF_3);
+		} else {
+			__raw_writel(reg, DI_DISP2_TIME_CONF_1);
+		}
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+
+	return 0;
+}
+
+struct ipu_adc_di_map {
+	uint32_t map_byte1;
+	uint32_t map_byte2;
+	uint32_t map_byte3;
+	uint32_t cycle_cnt;
+};
+
+static const struct ipu_adc_di_map di_mappings[] = {
+	[0] = {
+	       /* RGB888, 8-bit bus */
+	       .map_byte1 = 0x1600AAAA,
+	       .map_byte2 = 0x00E05555,
+	       .map_byte2 = 0x00070000,
+	       .cycle_cnt = 3,
+	       },
+	[1] = {
+	       /* RGB666, 8-bit bus */
+	       .map_byte1 = 0x1C00AAAF,
+	       .map_byte2 = 0x00E0555F,
+	       .map_byte3 = 0x0007000F,
+	       .cycle_cnt = 3,
+	       },
+	[2] = {
+	       /* RGB565, 8-bit bus */
+	       .map_byte1 = 0x008055BF,
+	       .map_byte2 = 0x0142015F,
+	       .map_byte3 = 0x0007003F,
+	       .cycle_cnt = 2,
+	       },
+	[3] = {
+	       /* RGB888, 24-bit bus */
+	       .map_byte1 = 0x0007000F,
+	       .map_byte2 = 0x000F000F,
+	       .map_byte3 = 0x0017000F,
+	       .cycle_cnt = 1,
+	       },
+	[4] = {
+	       /* RGB666, 18-bit bus */
+	       .map_byte1 = 0x0005000F,
+	       .map_byte2 = 0x000B000F,
+	       .map_byte3 = 0x0011000F,
+	       .cycle_cnt = 1,
+	       },
+	[5] = {
+	       /* RGB565, 16-bit bus */
+	       .map_byte1 = 0x0004003F,
+	       .map_byte2 = 0x000A000F,
+	       .map_byte3 = 0x000F003F,
+	       .cycle_cnt = 1,
+	       },
+};
+
+/* Private methods */
+static void _ipu_set_cmd_data_mappings(display_port_t disp,
+				       uint32_t pixel_fmt, int ifc_width)
+{
+	uint32_t reg;
+	u32 map = 0;
+
+	if (ifc_width == 8) {
+		switch (pixel_fmt) {
+		case IPU_PIX_FMT_BGR24:
+			map = 0;
+			break;
+		case IPU_PIX_FMT_RGB666:
+			map = 1;
+			break;
+		case IPU_PIX_FMT_RGB565:
+			map = 2;
+			break;
+		default:
+			break;
+		}
+	} else if (ifc_width >= 16) {
+		switch (pixel_fmt) {
+		case IPU_PIX_FMT_BGR24:
+			map = 3;
+			break;
+		case IPU_PIX_FMT_RGB666:
+			map = 4;
+			break;
+		case IPU_PIX_FMT_RGB565:
+			map = 5;
+			break;
+		default:
+			break;
+		}
+	}
+
+	switch (disp) {
+	case DISP0:
+		if (ifc_width == 8) {
+			__raw_writel(0x00070000, DI_DISP0_CB0_MAP);
+			__raw_writel(0x0000FFFF, DI_DISP0_CB1_MAP);
+			__raw_writel(0x0000FFFF, DI_DISP0_CB2_MAP);
+		} else {
+			__raw_writel(0x00070000, DI_DISP0_CB0_MAP);
+			__raw_writel(0x000F0000, DI_DISP0_CB1_MAP);
+			__raw_writel(0x0000FFFF, DI_DISP0_CB2_MAP);
+		}
+		__raw_writel(di_mappings[map].map_byte1, DI_DISP0_DB0_MAP);
+		__raw_writel(di_mappings[map].map_byte2, DI_DISP0_DB1_MAP);
+		__raw_writel(di_mappings[map].map_byte3, DI_DISP0_DB2_MAP);
+		reg = __raw_readl(DI_DISP_ACC_CC);
+		reg &= ~DISP0_IF_CLK_CNT_D_MASK;
+		reg |= (di_mappings[map].cycle_cnt - 1) <<
+		    DISP0_IF_CLK_CNT_D_OFFSET;
+		__raw_writel(reg, DI_DISP_ACC_CC);
+		break;
+	case DISP1:
+		if (ifc_width == 8) {
+			__raw_writel(0x00070000, DI_DISP1_CB0_MAP);
+			__raw_writel(0x0000FFFF, DI_DISP1_CB1_MAP);
+			__raw_writel(0x0000FFFF, DI_DISP1_CB2_MAP);
+		} else {
+			__raw_writel(0x00070000, DI_DISP1_CB0_MAP);
+			__raw_writel(0x000F0000, DI_DISP1_CB1_MAP);
+			__raw_writel(0x0000FFFF, DI_DISP1_CB2_MAP);
+		}
+		__raw_writel(di_mappings[map].map_byte1, DI_DISP1_DB0_MAP);
+		__raw_writel(di_mappings[map].map_byte2, DI_DISP1_DB1_MAP);
+		__raw_writel(di_mappings[map].map_byte3, DI_DISP1_DB2_MAP);
+		reg = __raw_readl(DI_DISP_ACC_CC);
+		reg &= ~DISP1_IF_CLK_CNT_D_MASK;
+		reg |= (di_mappings[map].cycle_cnt - 1) <<
+		    DISP1_IF_CLK_CNT_D_OFFSET;
+		__raw_writel(reg, DI_DISP_ACC_CC);
+		break;
+	case DISP2:
+		if (ifc_width == 8) {
+			__raw_writel(0x00070000, DI_DISP2_CB0_MAP);
+			__raw_writel(0x0000FFFF, DI_DISP2_CB1_MAP);
+			__raw_writel(0x0000FFFF, DI_DISP2_CB2_MAP);
+		} else {
+			__raw_writel(0x00070000, DI_DISP2_CB0_MAP);
+			__raw_writel(0x000F0000, DI_DISP2_CB1_MAP);
+			__raw_writel(0x0000FFFF, DI_DISP2_CB2_MAP);
+		}
+		__raw_writel(di_mappings[map].map_byte1, DI_DISP2_DB0_MAP);
+		__raw_writel(di_mappings[map].map_byte2, DI_DISP2_DB1_MAP);
+		__raw_writel(di_mappings[map].map_byte3, DI_DISP2_DB2_MAP);
+		reg = __raw_readl(DI_DISP_ACC_CC);
+		reg &= ~DISP2_IF_CLK_CNT_D_MASK;
+		reg |= (di_mappings[map].cycle_cnt - 1) <<
+		    DISP2_IF_CLK_CNT_D_OFFSET;
+		__raw_writel(reg, DI_DISP_ACC_CC);
+		break;
+	default:
+		break;
+	}
+}
+
+void ipu_disp_direct_write(ipu_channel_t channel, u32 value, u32 offset)
+{
+	/*TODO*/
+}
+
+int ipu_init_async_panel(int disp, int type, uint32_t cycle_time,
+			 uint32_t pixel_fmt, ipu_adc_sig_cfg_t sig)
+{
+	/*TODO:uniform interface for ipu async panel init*/
+}
+
+EXPORT_SYMBOL(ipu_adc_write_template);
+EXPORT_SYMBOL(ipu_adc_write_cmd);
+EXPORT_SYMBOL(ipu_adc_set_update_mode);
+EXPORT_SYMBOL(ipu_adc_init_panel);
+EXPORT_SYMBOL(ipu_adc_init_ifc_timing);
diff --git a/drivers/mxc/ipu/ipu_calc_stripes_sizes.c b/drivers/mxc/ipu/ipu_calc_stripes_sizes.c
new file mode 100644
index 000000000000..2992cce7023d
--- /dev/null
+++ b/drivers/mxc/ipu/ipu_calc_stripes_sizes.c
@@ -0,0 +1,375 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * @file ipu_calc_stripes_sizes.c
+ *
+ * @brief IPU IC functions
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/ipu.h>
+#include <asm/div64.h>
+
+#define BPP_32 0
+#define BPP_16 3
+#define BPP_8 5
+#define BPP_24 1
+#define BPP_12 4
+#define BPP_18 2
+
+static u64 _do_div(u64 a, u32 b)
+{
+	u64 div;
+	div = a;
+	do_div(div, b);
+	return div;
+}
+
+static u32 truncate(u32 up, /* 0: down; else: up */
+					u64 a, /* must be non-negative */
+					u32 b)
+{
+	u32 d;
+	u64 div;
+	div = _do_div(a, b);
+	d = b * (div >> 32);
+	if (up && (a > (((u64)d) << 32)))
+		return d+b;
+	else
+		return d;
+}
+
+static unsigned int f_calc(unsigned int pfs, unsigned int bpp, unsigned int *write)
+{/* return input_f */
+	unsigned int f_calculated = 0;
+	switch (pfs) {
+	case IPU_PIX_FMT_YVU422P:
+	case IPU_PIX_FMT_YUV422P:
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YUV420P:
+		f_calculated = 16;
+		break;
+
+	case IPU_PIX_FMT_NV12:
+		f_calculated = 8;
+		break;
+
+	default:
+		f_calculated = 0;
+		break;
+
+	}
+	if (!f_calculated) {
+		switch (bpp) {
+		case BPP_32:
+			f_calculated = 2;
+			break;
+
+		case BPP_16:
+			f_calculated = 4;
+			break;
+
+		case BPP_8:
+		case BPP_24:
+			f_calculated = 8;
+			break;
+
+		case BPP_12:
+			f_calculated = 16;
+			break;
+
+		case BPP_18:
+			f_calculated = 32;
+			break;
+
+		default:
+			f_calculated = 0;
+			break;
+			}
+		}
+	return f_calculated;
+}
+
+
+static unsigned int m_calc(unsigned int pfs)
+{
+	unsigned int m_calculated = 0;
+	switch (pfs) {
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YUV420P:
+	case IPU_PIX_FMT_YVU422P:
+	case IPU_PIX_FMT_YUV422P:
+	case IPU_PIX_FMT_YVU420P:
+	case IPU_PIX_FMT_NV12:
+		m_calculated = 8;
+		break;
+
+	case IPU_PIX_FMT_YUYV:
+	case IPU_PIX_FMT_UYVY:
+		m_calculated = 2;
+		break;
+
+	default:
+		m_calculated = 1;
+		break;
+
+	}
+	return m_calculated;
+}
+
+
+/* Stripe parameters calculator */
+/**************************************************************************
+Notes:
+MSW = the maximal width allowed for a stripe
+	i.MX31: 720, i.MX35: 800, i.MX37/51/53: 1024
+cirr = the maximal inverse resizing ratio for which overlap in the input
+	is requested; typically cirr~2
+equal_stripes:
+	0: each stripe is allowed to have independent parameters
+		for maximal image quality
+	1: the stripes are requested to have identical parameters
+	(except the base address), for maximal performance
+If performance is the top priority (above image quality)
+	Avoid overlap, by setting CIRR = 0
+		This will also force effectively identical_stripes = 1
+	Choose IF & OF that corresponds to the same IOX/SX for both stripes
+	Choose IFW & OFW such that
+	IFW/IM, IFW/IF, OFW/OM, OFW/OF are even integers
+	The function returns an error status:
+	0: no error
+	1: invalid input parameters -> aborted without result
+		Valid parameters should satisfy the following conditions
+		IFW <= OFW, otherwise downsizing is required
+					 - which is not supported yet
+		4 <= IFW,OFW, so some interpolation may be needed even without overlap
+		IM, OM, IF, OF should not vanish
+		2*IF <= IFW
+		so the frame can be split to two equal stripes, even without overlap
+		2*(OF+IF/irr_opt) <= OFW
+		so a valid positive INW exists even for equal stripes
+		OF <= MSW, otherwise, the left stripe cannot be sufficiently large
+		MSW < OFW, so splitting to stripes is required
+		OFW <= 2*MSW, so two stripes are sufficient
+		(this also implies that 2<=MSW)
+	2: OF is not a multiple of OM - not fully-supported yet
+	Output is produced but OW is not guaranited to be a multiple of OM
+	4: OFW reduced to be a multiple of OM
+	8: CIRR > 1: truncated to 1
+	Overlap is not supported (and not needed) y for upsizing)
+**************************************************************************/
+int ipu_calc_stripes_sizes(const unsigned int input_frame_width,
+			   /* input frame width;>1 */
+			   unsigned int output_frame_width, /* output frame width; >1 */
+			   const unsigned int maximal_stripe_width,
+			   /* the maximal width allowed for a stripe */
+			   const unsigned long long cirr, /* see above */
+			   const unsigned int equal_stripes, /* see above */
+			   u32 input_pixelformat,/* pixel format after of read channel*/
+			   u32 output_pixelformat,/* pixel format after of write channel*/
+			   struct stripe_param *left,
+			   struct stripe_param *right)
+{
+	const unsigned int irr_frac_bits = 13;
+	const unsigned long irr_steps = 1 << irr_frac_bits;
+	const u64 dirr = ((u64)1) << (32 - 2);
+	/* The maximum relative difference allowed between the irrs */
+	const u64 cr = ((u64)4) << 32;
+	/* The importance ratio between the two terms in the cost function below */
+
+	unsigned int status;
+	unsigned int temp;
+	unsigned int onw_min;
+	unsigned int inw, onw, inw_best;
+	/* number of pixels in the left stripe NOT hidden by the right stripe */
+	u64 irr_opt; /* the optimal inverse resizing ratio */
+	u64 rr_opt; /* the optimal resizing ratio = 1/irr_opt*/
+	u64 dinw; /* the misalignment between the stripes */
+	/* (measured in units of input columns) */
+	u64 difwl, difwr;
+	/* The number of input columns not reflected in the output */
+	/* the resizing ratio used for the right stripe is */
+	/*   left->irr and right->irr respectively */
+	u64 cost, cost_min;
+	u64 div; /* result of division */
+
+	unsigned int input_m, input_f, output_m, output_f; /* parameters for upsizing by stripes */
+	u32 in_pfs, out_pfs;
+
+	status = 0;
+
+	/* M, F calculations */
+	/* read back pfs from params */
+
+	input_f = f_calc(input_pixelformat, 0, NULL);
+	/*in_m = m_calc(in_pfs);*/
+	input_m = 16;
+	/* BPP should be used in the out_F calc */
+	/* Temporarily not used */
+	/* out_F = F_calc(idmac->pfs, idmac->bpp, NULL); */
+
+	output_f = 16;
+	output_m = m_calc(output_pixelformat);
+
+
+	if ((output_frame_width < input_frame_width) || (input_frame_width < 4)
+	    || (output_frame_width < 4))
+		return 1;
+
+	irr_opt = _do_div((((u64)(input_frame_width - 1)) << 32),
+			  (output_frame_width - 1));
+	rr_opt = _do_div((((u64)(output_frame_width - 1)) << 32),
+			 (input_frame_width - 1));
+
+	if ((input_m == 0) || (output_m == 0) || (input_f == 0) || (output_f == 0)
+	    || (input_frame_width < (2 * input_f))
+	    || ((((u64)output_frame_width) << 32) <
+		(2 * ((((u64)output_f) << 32) + (input_f * rr_opt))))
+	    || (maximal_stripe_width < output_f)
+	    || (output_frame_width <= maximal_stripe_width)
+	    || ((2 * maximal_stripe_width) < output_frame_width))
+		return 1;
+
+	if (output_f % output_m)
+		status += 2;
+
+	temp = truncate(0, (((u64)output_frame_width) << 32), output_m);
+	if (temp < output_frame_width) {
+		output_frame_width = temp;
+		status += 4;
+	}
+
+	if (equal_stripes) {
+		if ((irr_opt > cirr) /* overlap in the input is not requested */
+		    && ((input_frame_width % (input_m << 1)) == 0)
+		    && ((input_frame_width % (input_f << 1)) == 0)
+		    && ((output_frame_width % (output_m << 1)) == 0)
+		    && ((output_frame_width % (output_f << 1)) == 0)) {
+			/* without overlap */
+			left->input_width = right->input_width = right->input_column =
+				input_frame_width >> 1;
+			left->output_width = right->output_width = right->output_column =
+				output_frame_width >> 1;
+			left->input_column = right->input_column = 0;
+			div = _do_div(((((u64)irr_steps) << 32) *
+				       (right->input_width - 1)), (right->output_width - 1));
+			left->irr = right->irr = truncate(0, div, 1);
+		} else { /* with overlap */
+			onw = truncate(0, (((u64)output_frame_width) << 32) >> 1,
+				       output_f);
+			inw = truncate(0, onw * irr_opt, input_f);
+			/* this is the maximal inw which allows the same resizing ratio */
+			/* in both stripes */
+			onw = truncate(1, (inw * rr_opt), output_f);
+			div = _do_div((((u64)(irr_steps * inw)) <<
+				       32), onw);
+			left->irr = right->irr = truncate(0, div, 1);
+			left->output_width = right->output_width =
+				output_frame_width - onw;
+			/* These are valid assignments for output_width, */
+			/* assuming output_f is a multiple of output_m */
+			div = (((u64)(left->output_width-1) * (left->irr)) << 32);
+			div = (((u64)1) << 32) + _do_div(div, irr_steps);
+
+			left->input_width = right->input_width = truncate(1, div, input_m);
+
+			div = _do_div((((u64)((right->output_width - 1) * right->irr)) <<
+				       32), irr_steps);
+			difwr = (((u64)(input_frame_width - 1 - inw)) << 32) - div;
+			div = _do_div((difwr + (((u64)input_f) << 32)), 2);
+			left->input_column = truncate(0, div, input_f);
+
+
+			/* This splits the truncated input columns evenly */
+			/*    between the left and right margins */
+			right->input_column = left->input_column + inw;
+			left->output_column = 0;
+			right->output_column = onw;
+		}
+	} else { /* independent stripes */
+		onw_min = output_frame_width - maximal_stripe_width;
+		/* onw is a multiple of output_f, in the range */
+		/* [max(output_f,output_frame_width-maximal_stripe_width),*/
+		/*min(output_frame_width-2,maximal_stripe_width)] */
+		/* definitely beyond the cost of any valid setting */
+		cost_min = (((u64)input_frame_width) << 32) + cr;
+		onw = truncate(0, ((u64)maximal_stripe_width), output_f);
+		if (output_frame_width - onw == 1)
+			onw -= output_f; /*  => onw and output_frame_width-1-onw are positive */
+		inw = truncate(0, onw * irr_opt, input_f);
+		/* this is the maximal inw which allows the same resizing ratio */
+		/* in both stripes */
+		onw = truncate(1, inw * rr_opt, output_f);
+		do {
+			div = _do_div((((u64)(irr_steps * inw)) << 32), onw);
+			left->irr = truncate(0, div, 1);
+			div = _do_div((((u64)(onw * left->irr)) << 32),
+				      irr_steps);
+			dinw = (((u64)inw) << 32) - div;
+
+			div = _do_div((((u64)((output_frame_width - 1 - onw) * left->irr)) <<
+				       32), irr_steps);
+
+			difwl = (((u64)(input_frame_width - 1 - inw)) << 32) - div;
+
+			cost = difwl + (((u64)(cr * dinw)) >> 32);
+
+			if (cost < cost_min) {
+				inw_best = inw;
+				cost_min = cost;
+			}
+
+			inw -= input_f;
+			onw = truncate(1, inw * rr_opt, output_f);
+			/* This is the minimal onw which allows the same resizing ratio */
+			/*     in both stripes */
+		} while (onw >= onw_min);
+
+		inw = inw_best;
+		onw = truncate(1, inw * rr_opt, output_f);
+		div = _do_div((((u64)(irr_steps * inw)) << 32), onw);
+		left->irr = truncate(0, div, 1);
+
+		left->output_width = onw;
+		right->output_width = output_frame_width - onw;
+		/* These are valid assignments for output_width, */
+		/* assuming output_f is a multiple of output_m */
+		left->input_width = truncate(1, ((u64)(inw + 1)) << 32, input_m);
+		right->input_width = truncate(1, ((u64)(input_frame_width - inw)) <<
+					      32, input_m);
+
+		div = _do_div((((u64)(irr_steps * (input_frame_width - 1 - inw))) <<
+			       32), (right->output_width - 1));
+		right->irr = truncate(0, div, 1);
+		temp = truncate(0, ((u64)left->irr) * ((((u64)1) << 32) + dirr), 1);
+		if (temp < right->irr)
+			right->irr = temp;
+		div = _do_div(((u64)((right->output_width - 1) * right->irr) <<
+			       32), irr_steps);
+		difwr = (u64)(input_frame_width - 1 - inw) - div;
+
+
+		div = _do_div((difwr + (((u64)input_f) << 32)), 2);
+		left->input_column = truncate(0, div, input_f);
+
+		/* This splits the truncated input columns evenly */
+		/*    between the left and right margins */
+		right->input_column = left->input_column + inw;
+		left->output_column = 0;
+		right->output_column = onw;
+	}
+	return status;
+}
+EXPORT_SYMBOL(ipu_calc_stripes_sizes);
diff --git a/drivers/mxc/ipu/ipu_common.c b/drivers/mxc/ipu/ipu_common.c
new file mode 100644
index 000000000000..bcfea6c85938
--- /dev/null
+++ b/drivers/mxc/ipu/ipu_common.c
@@ -0,0 +1,1902 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_common.c
+ *
+ * @brief This file contains the IPU driver common API functions.
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+#include "ipu_param_mem.h"
+
+/*
+ * This type definition is used to define a node in the GPIO interrupt queue for
+ * registered interrupts for GPIO pins. Each node contains the GPIO signal number
+ * associated with the ISR and the actual ISR function pointer.
+ */
+struct ipu_irq_node {
+	irqreturn_t(*handler) (int, void *);	/*!< the ISR */
+	const char *name;	/*!< device associated with the interrupt */
+	void *dev_id;		/*!< some unique information for the ISR */
+	__u32 flags;		/*!< not used */
+};
+
+/* Globals */
+struct clk *g_ipu_clk;
+struct clk *g_ipu_csi_clk;
+static struct clk *dfm_clk;
+int g_ipu_irq[2];
+int g_ipu_hw_rev;
+bool g_sec_chan_en[21];
+uint32_t g_channel_init_mask;
+DEFINE_SPINLOCK(ipu_lock);
+struct device *g_ipu_dev;
+
+static struct ipu_irq_node ipu_irq_list[IPU_IRQ_COUNT];
+static const char driver_name[] = "mxc_ipu";
+
+static uint32_t g_ipu_config = 0;
+static uint32_t g_channel_init_mask_backup = 0;
+static bool g_csi_used = false;
+
+/* Static functions */
+static irqreturn_t ipu_irq_handler(int irq, void *desc);
+static void _ipu_pf_init(ipu_channel_params_t * params);
+static void _ipu_pf_uninit(void);
+
+inline static uint32_t channel_2_dma(ipu_channel_t ch, ipu_buffer_t type)
+{
+	return ((type == IPU_INPUT_BUFFER) ? ((uint32_t) ch & 0xFF) :
+		((type == IPU_OUTPUT_BUFFER) ? (((uint32_t) ch >> 8) & 0xFF)
+		 : (((uint32_t) ch >> 16) & 0xFF)));
+};
+
+inline static uint32_t DMAParamAddr(uint32_t dma_ch)
+{
+	return (0x10000 | (dma_ch << 4));
+};
+
+/*!
+ * This function is called by the driver framework to initialize the IPU
+ * hardware.
+ *
+ * @param       dev       The device structure for the IPU passed in by the framework.
+ *
+ * @return      This function returns 0 on success or negative error code on error
+ */
+static
+int ipu_probe(struct platform_device *pdev)
+{
+//      struct platform_device *pdev = to_platform_device(dev);
+	struct mxc_ipu_config *ipu_conf = pdev->dev.platform_data;
+
+	spin_lock_init(&ipu_lock);
+
+	g_ipu_dev = &pdev->dev;
+	g_ipu_hw_rev = ipu_conf->rev;
+
+	/* Register IPU interrupts */
+	g_ipu_irq[0] = platform_get_irq(pdev, 0);
+	if (g_ipu_irq[0] < 0)
+		return -EINVAL;
+
+	if (request_irq(g_ipu_irq[0], ipu_irq_handler, 0, driver_name, 0) != 0) {
+		dev_err(g_ipu_dev, "request SYNC interrupt failed\n");
+		return -EBUSY;
+	}
+	/* Some platforms have 2 IPU interrupts */
+	g_ipu_irq[1] = platform_get_irq(pdev, 1);
+	if (g_ipu_irq[1] >= 0) {
+		if (request_irq
+		    (g_ipu_irq[1], ipu_irq_handler, 0, driver_name, 0) != 0) {
+			dev_err(g_ipu_dev, "request ERR interrupt failed\n");
+			return -EBUSY;
+		}
+	}
+
+	/* Enable IPU and CSI clocks */
+	/* Get IPU clock freq */
+	g_ipu_clk = clk_get(&pdev->dev, "ipu_clk");
+	dev_dbg(g_ipu_dev, "ipu_clk = %lu\n", clk_get_rate(g_ipu_clk));
+
+	g_ipu_csi_clk = clk_get(&pdev->dev, "csi_clk");
+
+	dfm_clk = clk_get(NULL, "dfm_clk");
+
+	clk_enable(g_ipu_clk);
+
+	/* resetting the CONF register of the IPU */
+	__raw_writel(0x00000000, IPU_CONF);
+
+	__raw_writel(0x00100010L, DI_HSP_CLK_PER);
+
+	/* Set SDC refresh channels as high priority */
+	__raw_writel(0x0000C000L, IDMAC_CHA_PRI);
+
+	/* Set to max back to back burst requests */
+	__raw_writel(0x00000000L, IDMAC_CONF);
+
+	register_ipu_device();
+
+	return 0;
+}
+
+/*!
+ * This function is called to initialize a logical IPU channel.
+ *
+ * @param       channel Input parameter for the logical channel ID to initalize.
+ *
+ * @param       params  Input parameter containing union of channel initialization
+ *                      parameters.
+ *
+ * @return      This function returns 0 on success or negative error code on fail
+ */
+int32_t ipu_init_channel(ipu_channel_t channel, ipu_channel_params_t * params)
+{
+	uint32_t ipu_conf;
+	uint32_t reg;
+	unsigned long lock_flags;
+
+	dev_dbg(g_ipu_dev, "init channel = %d\n", IPU_CHAN_ID(channel));
+
+	if ((channel != MEM_SDC_BG) && (channel != MEM_SDC_FG) &&
+	    (channel != MEM_ROT_ENC_MEM) && (channel != MEM_ROT_VF_MEM) &&
+	    (channel != MEM_ROT_PP_MEM) && (channel != CSI_MEM)
+	    && (params == NULL)) {
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	ipu_conf = __raw_readl(IPU_CONF);
+	if (ipu_conf == 0) {
+		clk_enable(g_ipu_clk);
+	}
+
+	if (g_channel_init_mask & (1L << IPU_CHAN_ID(channel))) {
+		dev_err(g_ipu_dev, "Warning: channel already initialized %d\n",
+			IPU_CHAN_ID(channel));
+	}
+
+	switch (channel) {
+	case CSI_PRP_VF_MEM:
+		reg = __raw_readl(IPU_FS_PROC_FLOW);
+		__raw_writel(reg & ~FS_VF_IN_VALID, IPU_FS_PROC_FLOW);
+
+		if (params->mem_prp_vf_mem.graphics_combine_en)
+			g_sec_chan_en[IPU_CHAN_ID(channel)] = true;
+
+		_ipu_ic_init_prpvf(params, true);
+		break;
+	case CSI_PRP_VF_ADC:
+		reg = __raw_readl(IPU_FS_PROC_FLOW);
+		__raw_writel(reg | (FS_DEST_ADC << FS_PRPVF_DEST_SEL_OFFSET),
+			     IPU_FS_PROC_FLOW);
+
+		_ipu_adc_init_channel(CSI_PRP_VF_ADC,
+				      params->csi_prp_vf_adc.disp,
+				      WriteTemplateNonSeq,
+				      params->csi_prp_vf_adc.out_left,
+				      params->csi_prp_vf_adc.out_top);
+
+		_ipu_ic_init_prpvf(params, true);
+		break;
+	case MEM_PRP_VF_MEM:
+		reg = __raw_readl(IPU_FS_PROC_FLOW);
+		__raw_writel(reg | FS_VF_IN_VALID, IPU_FS_PROC_FLOW);
+
+		if (params->mem_prp_vf_mem.graphics_combine_en)
+			g_sec_chan_en[IPU_CHAN_ID(channel)] = true;
+
+		_ipu_ic_init_prpvf(params, false);
+		break;
+	case MEM_ROT_VF_MEM:
+		_ipu_ic_init_rotate_vf(params);
+		break;
+	case CSI_PRP_ENC_MEM:
+		reg = __raw_readl(IPU_FS_PROC_FLOW);
+		__raw_writel(reg & ~FS_ENC_IN_VALID, IPU_FS_PROC_FLOW);
+		_ipu_ic_init_prpenc(params, true);
+		break;
+	case MEM_PRP_ENC_MEM:
+		reg = __raw_readl(IPU_FS_PROC_FLOW);
+		__raw_writel(reg | FS_ENC_IN_VALID, IPU_FS_PROC_FLOW);
+		_ipu_ic_init_prpenc(params, false);
+		break;
+	case MEM_ROT_ENC_MEM:
+		_ipu_ic_init_rotate_enc(params);
+		break;
+	case MEM_PP_ADC:
+		reg = __raw_readl(IPU_FS_PROC_FLOW);
+		__raw_writel(reg | (FS_DEST_ADC << FS_PP_DEST_SEL_OFFSET),
+			     IPU_FS_PROC_FLOW);
+
+		_ipu_adc_init_channel(MEM_PP_ADC, params->mem_pp_adc.disp,
+				      WriteTemplateNonSeq,
+				      params->mem_pp_adc.out_left,
+				      params->mem_pp_adc.out_top);
+
+		if (params->mem_pp_adc.graphics_combine_en)
+			g_sec_chan_en[IPU_CHAN_ID(channel)] = true;
+
+		_ipu_ic_init_pp(params);
+		break;
+	case MEM_PP_MEM:
+		if (params->mem_pp_mem.graphics_combine_en)
+			g_sec_chan_en[IPU_CHAN_ID(channel)] = true;
+
+		_ipu_ic_init_pp(params);
+		break;
+	case MEM_ROT_PP_MEM:
+		_ipu_ic_init_rotate_pp(params);
+		break;
+	case CSI_MEM:
+		_ipu_ic_init_csi(params);
+		break;
+
+	case MEM_PF_Y_MEM:
+	case MEM_PF_U_MEM:
+	case MEM_PF_V_MEM:
+		/* Enable PF block */
+		_ipu_pf_init(params);
+		break;
+
+	case MEM_SDC_BG:
+		break;
+	case MEM_SDC_FG:
+		break;
+	case ADC_SYS1:
+		_ipu_adc_init_channel(ADC_SYS1, params->adc_sys1.disp,
+				      params->adc_sys1.ch_mode,
+				      params->adc_sys1.out_left,
+				      params->adc_sys1.out_top);
+		break;
+	case ADC_SYS2:
+		_ipu_adc_init_channel(ADC_SYS2, params->adc_sys2.disp,
+				      params->adc_sys2.ch_mode,
+				      params->adc_sys2.out_left,
+				      params->adc_sys2.out_top);
+		break;
+	default:
+		dev_err(g_ipu_dev, "Missing channel initialization\n");
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+		return -EINVAL;
+	}
+
+	/* Enable IPU sub module */
+	g_channel_init_mask |= 1L << IPU_CHAN_ID(channel);
+
+	if (g_channel_init_mask & 0x00000066L) {	/*CSI */
+		ipu_conf |= IPU_CONF_CSI_EN;
+		if (cpu_is_mx31() || cpu_is_mx32()) {
+			g_csi_used = true;
+		}
+	}
+	if (g_channel_init_mask & 0x00001FFFL) {	/*IC */
+		ipu_conf |= IPU_CONF_IC_EN;
+	}
+	if (g_channel_init_mask & 0x00000A10L) {	/*ROT */
+		ipu_conf |= IPU_CONF_ROT_EN;
+	}
+	if (g_channel_init_mask & 0x0001C000L) {	/*SDC */
+		ipu_conf |= IPU_CONF_SDC_EN | IPU_CONF_DI_EN;
+	}
+	if (g_channel_init_mask & 0x00061140L) {	/*ADC */
+		ipu_conf |= IPU_CONF_ADC_EN | IPU_CONF_DI_EN;
+	}
+	if (g_channel_init_mask & 0x00380000L) {	/*PF */
+		ipu_conf |= IPU_CONF_PF_EN;
+	}
+	__raw_writel(ipu_conf, IPU_CONF);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+
+/*!
+ * This function is called to uninitialize a logical IPU channel.
+ *
+ * @param       channel Input parameter for the logical channel ID to uninitalize.
+ */
+void ipu_uninit_channel(ipu_channel_t channel)
+{
+	unsigned long lock_flags;
+	uint32_t reg;
+	uint32_t dma, mask = 0;
+	uint32_t ipu_conf;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if ((g_channel_init_mask & (1L << IPU_CHAN_ID(channel))) == 0) {
+		dev_err(g_ipu_dev, "Channel already uninitialized %d\n",
+			IPU_CHAN_ID(channel));
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+		return;
+	}
+
+	/* Make sure channel is disabled */
+	/* Get input and output dma channels */
+	dma = channel_2_dma(channel, IPU_OUTPUT_BUFFER);
+	if (dma != IDMA_CHAN_INVALID)
+		mask |= 1UL << dma;
+	dma = channel_2_dma(channel, IPU_INPUT_BUFFER);
+	if (dma != IDMA_CHAN_INVALID)
+		mask |= 1UL << dma;
+	/* Get secondary input dma channel */
+	if (g_sec_chan_en[IPU_CHAN_ID(channel)]) {
+		dma = channel_2_dma(channel, IPU_SEC_INPUT_BUFFER);
+		if (dma != IDMA_CHAN_INVALID) {
+			mask |= 1UL << dma;
+		}
+	}
+	if (mask & __raw_readl(IDMAC_CHA_EN)) {
+		dev_err(g_ipu_dev,
+			"Channel %d is not disabled, disable first\n",
+			IPU_CHAN_ID(channel));
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+		return;
+	}
+
+	/* Reset the double buffer */
+	reg = __raw_readl(IPU_CHA_DB_MODE_SEL);
+	__raw_writel(reg & ~mask, IPU_CHA_DB_MODE_SEL);
+
+	g_sec_chan_en[IPU_CHAN_ID(channel)] = false;
+
+	switch (channel) {
+	case CSI_MEM:
+		_ipu_ic_uninit_csi();
+		break;
+	case CSI_PRP_VF_ADC:
+		reg = __raw_readl(IPU_FS_PROC_FLOW);
+		__raw_writel(reg & ~FS_PRPVF_DEST_SEL_MASK, IPU_FS_PROC_FLOW);
+
+		_ipu_adc_uninit_channel(CSI_PRP_VF_ADC);
+
+		/* Fall thru */
+	case CSI_PRP_VF_MEM:
+	case MEM_PRP_VF_MEM:
+		_ipu_ic_uninit_prpvf();
+		break;
+	case MEM_PRP_VF_ADC:
+		break;
+	case MEM_ROT_VF_MEM:
+		_ipu_ic_uninit_rotate_vf();
+		break;
+	case CSI_PRP_ENC_MEM:
+	case MEM_PRP_ENC_MEM:
+		_ipu_ic_uninit_prpenc();
+		break;
+	case MEM_ROT_ENC_MEM:
+		_ipu_ic_uninit_rotate_enc();
+		break;
+	case MEM_PP_ADC:
+		reg = __raw_readl(IPU_FS_PROC_FLOW);
+		__raw_writel(reg & ~FS_PP_DEST_SEL_MASK, IPU_FS_PROC_FLOW);
+
+		_ipu_adc_uninit_channel(MEM_PP_ADC);
+
+		/* Fall thru */
+	case MEM_PP_MEM:
+		_ipu_ic_uninit_pp();
+		break;
+	case MEM_ROT_PP_MEM:
+		_ipu_ic_uninit_rotate_pp();
+		break;
+
+	case MEM_PF_Y_MEM:
+		_ipu_pf_uninit();
+		break;
+	case MEM_PF_U_MEM:
+	case MEM_PF_V_MEM:
+		break;
+
+	case MEM_SDC_BG:
+		break;
+	case MEM_SDC_FG:
+		break;
+	case ADC_SYS1:
+		_ipu_adc_uninit_channel(ADC_SYS1);
+		break;
+	case ADC_SYS2:
+		_ipu_adc_uninit_channel(ADC_SYS2);
+		break;
+	case MEM_SDC_MASK:
+	case CHAN_NONE:
+		break;
+	}
+
+	g_channel_init_mask &= ~(1L << IPU_CHAN_ID(channel));
+
+	ipu_conf = __raw_readl(IPU_CONF);
+	if ((g_channel_init_mask & 0x00000066L) == 0) {	/*CSI */
+		ipu_conf &= ~IPU_CONF_CSI_EN;
+	}
+	if ((g_channel_init_mask & 0x00001FFFL) == 0) {	/*IC */
+		ipu_conf &= ~IPU_CONF_IC_EN;
+	}
+	if ((g_channel_init_mask & 0x00000A10L) == 0) {	/*ROT */
+		ipu_conf &= ~IPU_CONF_ROT_EN;
+	}
+	if ((g_channel_init_mask & 0x0001C000L) == 0) {	/*SDC */
+		ipu_conf &= ~IPU_CONF_SDC_EN;
+	}
+	if ((g_channel_init_mask & 0x00061140L) == 0) {	/*ADC */
+		ipu_conf &= ~IPU_CONF_ADC_EN;
+	}
+	if ((g_channel_init_mask & 0x0007D140L) == 0) {	/*DI */
+		ipu_conf &= ~IPU_CONF_DI_EN;
+	}
+	if ((g_channel_init_mask & 0x00380000L) == 0) {	/*PF */
+		ipu_conf &= ~IPU_CONF_PF_EN;
+	}
+	__raw_writel(ipu_conf, IPU_CONF);
+	if (ipu_conf == 0) {
+		clk_disable(g_ipu_clk);
+	}
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+
+/*!
+ * This function is called to initialize a buffer for logical IPU channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       type            Input parameter which buffer to initialize.
+ *
+ * @param       pixel_fmt       Input parameter for pixel format of buffer. Pixel
+ *                              format is a FOURCC ASCII code.
+ *
+ * @param       width           Input parameter for width of buffer in pixels.
+ *
+ * @param       height          Input parameter for height of buffer in pixels.
+ *
+ * @param       stride          Input parameter for stride length of buffer
+ *                              in pixels.
+ *
+ * @param       rot_mode        Input parameter for rotation setting of buffer.
+ *                              A rotation setting other than \b IPU_ROTATE_VERT_FLIP
+ *                              should only be used for input buffers of rotation
+ *                              channels.
+ *
+ * @param       phyaddr_0       Input parameter buffer 0 physical address.
+ *
+ * @param       phyaddr_1       Input parameter buffer 1 physical address.
+ *                              Setting this to a value other than NULL enables
+ *                              double buffering mode.
+ *
+ * @param       u		       	private u offset for additional cropping,
+ *								zero if not used.
+ *
+ * @param       v		       	private v offset for additional cropping,
+ *								zero if not used.
+ *
+ * @return      This function returns 0 on success or negative error code on fail
+ */
+int32_t ipu_init_channel_buffer(ipu_channel_t channel, ipu_buffer_t type,
+				uint32_t pixel_fmt,
+				uint16_t width, uint16_t height,
+				uint32_t stride,
+				ipu_rotate_mode_t rot_mode,
+				dma_addr_t phyaddr_0, dma_addr_t phyaddr_1,
+				uint32_t u, uint32_t v)
+{
+	uint32_t params[10];
+	unsigned long lock_flags;
+	uint32_t reg;
+	uint32_t dma_chan;
+
+	dma_chan = channel_2_dma(channel, type);
+
+	if (stride < width * bytes_per_pixel(pixel_fmt))
+		stride = width * bytes_per_pixel(pixel_fmt);
+
+	if (dma_chan == IDMA_CHAN_INVALID)
+		return -EINVAL;
+
+	if (stride % 4) {
+		dev_err(g_ipu_dev,
+			"Stride must be 32-bit aligned, stride = %d\n", stride);
+		return -EINVAL;
+	}
+	/* IC channels' width must be multiple of 8 pixels     */
+	if ((dma_chan <= 13) && (width % 8)) {
+		dev_err(g_ipu_dev, "width must be 8 pixel multiple\n");
+		return -EINVAL;
+	}
+	/* Build parameter memory data for DMA channel */
+	_ipu_ch_param_set_size(params, pixel_fmt, width, height, stride, u, v);
+	_ipu_ch_param_set_buffer(params, phyaddr_0, phyaddr_1);
+	_ipu_ch_param_set_rotation(params, rot_mode);
+	/* Some channels (rotation) have restriction on burst length */
+	if ((dma_chan == 10) || (dma_chan == 11) || (dma_chan == 13)) {
+		_ipu_ch_param_set_burst_size(params, 8);
+	} else if (dma_chan == 24) {	/* PF QP channel */
+		_ipu_ch_param_set_burst_size(params, 4);
+	} else if (dma_chan == 25) {	/* PF H264 BS channel */
+		_ipu_ch_param_set_burst_size(params, 16);
+	} else if (((dma_chan == 14) || (dma_chan == 15)) &&
+		   pixel_fmt == IPU_PIX_FMT_RGB565) {
+		_ipu_ch_param_set_burst_size(params, 16);
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	_ipu_write_param_mem(DMAParamAddr(dma_chan), params, 10);
+
+	reg = __raw_readl(IPU_CHA_DB_MODE_SEL);
+	if (phyaddr_1) {
+		reg |= 1UL << dma_chan;
+	} else {
+		reg &= ~(1UL << dma_chan);
+	}
+	__raw_writel(reg, IPU_CHA_DB_MODE_SEL);
+
+	/* Reset to buffer 0 */
+	__raw_writel(1UL << dma_chan, IPU_CHA_CUR_BUF);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+
+/*!
+ * This function is called to update the physical address of a buffer for
+ * a logical IPU channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       type            Input parameter which buffer to initialize.
+ *
+ * @param       bufNum          Input parameter for which buffer number to update.
+ *                              0 or 1 are the only valid values.
+ *
+ * @param       phyaddr         Input parameter buffer physical address.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail. This function will fail if the buffer is set to ready.
+ */
+int32_t ipu_update_channel_buffer(ipu_channel_t channel, ipu_buffer_t type,
+				  uint32_t bufNum, dma_addr_t phyaddr)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+	uint32_t dma_chan = channel_2_dma(channel, type);
+
+	if (dma_chan == IDMA_CHAN_INVALID)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if (bufNum == 0) {
+		reg = __raw_readl(IPU_CHA_BUF0_RDY);
+		if (reg & (1UL << dma_chan)) {
+			spin_unlock_irqrestore(&ipu_lock, lock_flags);
+			return -EACCES;
+		}
+		__raw_writel(DMAParamAddr(dma_chan) + 0x0008UL, IPU_IMA_ADDR);
+		__raw_writel(phyaddr, IPU_IMA_DATA);
+	} else {
+		reg = __raw_readl(IPU_CHA_BUF1_RDY);
+		if (reg & (1UL << dma_chan)) {
+			spin_unlock_irqrestore(&ipu_lock, lock_flags);
+			return -EACCES;
+		}
+		__raw_writel(DMAParamAddr(dma_chan) + 0x0009UL, IPU_IMA_ADDR);
+		__raw_writel(phyaddr, IPU_IMA_DATA);
+	}
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	dev_dbg(g_ipu_dev, "IPU: update IDMA ch %d buf %d = 0x%08X\n",
+		dma_chan, bufNum, phyaddr);
+	return 0;
+}
+
+/*!
+ * This function is called to initialize a buffer for logical IPU channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       type            Input parameter which buffer to initialize.
+ *
+ * @param       pixel_fmt       Input parameter for pixel format of buffer.
+ *                              Pixel format is a FOURCC ASCII code.
+ *
+ * @param       width           Input parameter for width of buffer in pixels.
+ *
+ * @param       height          Input parameter for height of buffer in pixels.
+ *
+ * @param       stride          Input parameter for stride length of buffer
+ *                              in pixels.
+ *
+ * @param       u				predefined private u offset for additional cropping,
+ *								zero if not used.
+ *
+ * @param       v				predefined private v offset for additional cropping,
+ *								zero if not used.
+ *
+ * @param		vertical_offset vertical offset for Y coordinate
+ * 								in the existed frame
+ *
+ *
+ * @param		horizontal_offset horizontal offset for X coordinate
+ * 								in the existed frame
+ *
+ *
+ * @return      Returns 0 on success or negative error code on fail
+ *              This function will fail if any buffer is set to ready.
+ */
+
+int32_t ipu_update_channel_offset(ipu_channel_t channel, ipu_buffer_t type,
+				uint32_t pixel_fmt,
+				uint16_t width, uint16_t height,
+				uint32_t stride,
+				uint32_t u, uint32_t v,
+				uint32_t vertical_offset, uint32_t horizontal_offset)
+{
+	uint32_t reg;
+	int ret = 0;
+	unsigned long lock_flags;
+	uint32_t dma_chan = channel_2_dma(channel, type);
+
+	if (dma_chan == IDMA_CHAN_INVALID)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	ret = -EACCES;
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return ret;
+}
+EXPORT_SYMBOL(ipu_update_channel_offset);
+
+/*!
+ * This function is called to set a channel's buffer as ready.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       type            Input parameter which buffer to initialize.
+ *
+ * @param       bufNum          Input parameter for which buffer number set to
+ *                              ready state.
+ *
+ * @return      This function returns 0 on success or negative error code on fail
+ */
+int32_t ipu_select_buffer(ipu_channel_t channel, ipu_buffer_t type,
+			  uint32_t bufNum)
+{
+	uint32_t dma_chan = channel_2_dma(channel, type);
+
+	if (dma_chan == IDMA_CHAN_INVALID)
+		return -EINVAL;
+
+	if (bufNum == 0) {
+		/*Mark buffer 0 as ready. */
+		__raw_writel(1UL << dma_chan, IPU_CHA_BUF0_RDY);
+	} else {
+		/*Mark buffer 1 as ready. */
+		__raw_writel(1UL << dma_chan, IPU_CHA_BUF1_RDY);
+	}
+	return 0;
+}
+
+/*!
+ * This function links 2 channels together for automatic frame
+ * synchronization. The output of the source channel is linked to the input of
+ * the destination channel.
+ *
+ * @param       src_ch          Input parameter for the logical channel ID of
+ *                              the source channel.
+ *
+ * @param       dest_ch         Input parameter for the logical channel ID of
+ *                              the destination channel.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int32_t ipu_link_channels(ipu_channel_t src_ch, ipu_channel_t dest_ch)
+{
+	unsigned long lock_flags;
+	uint32_t out_dma;
+	uint32_t in_dma;
+	bool isProc;
+	uint32_t value;
+	uint32_t mask;
+	uint32_t offset;
+	uint32_t fs_proc_flow;
+	uint32_t fs_disp_flow;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	fs_proc_flow = __raw_readl(IPU_FS_PROC_FLOW);
+	fs_disp_flow = __raw_readl(IPU_FS_DISP_FLOW);
+
+	out_dma = (1UL << channel_2_dma(src_ch, IPU_OUTPUT_BUFFER));
+	in_dma = (1UL << channel_2_dma(dest_ch, IPU_INPUT_BUFFER));
+
+	/* PROCESS THE OUTPUT DMA CH */
+	switch (out_dma) {
+		/*VF-> */
+	case IDMA_IC_1:
+		pr_debug("Link VF->");
+		isProc = true;
+		mask = FS_PRPVF_DEST_SEL_MASK;
+		offset = FS_PRPVF_DEST_SEL_OFFSET;
+		value = (in_dma == IDMA_IC_11) ? FS_DEST_ROT :	/*->VF_ROT */
+		    (in_dma == IDMA_ADC_SYS1_WR) ? FS_DEST_ADC1 :	/* ->ADC1 */
+		    (in_dma == IDMA_ADC_SYS2_WR) ? FS_DEST_ADC2 :	/* ->ADC2 */
+		    (in_dma == IDMA_SDC_BG) ? FS_DEST_SDC_BG :	/*->SDC_BG */
+		    (in_dma == IDMA_SDC_FG) ? FS_DEST_SDC_FG :	/*->SDC_FG */
+		    (in_dma == IDMA_ADC_SYS1_WR) ? FS_DEST_ADC1 :	/*->ADC1 */
+		    /* ->ADCDirect */
+		    0;
+		break;
+
+		/*VF_ROT-> */
+	case IDMA_IC_9:
+		pr_debug("Link VF_ROT->");
+		isProc = true;
+		mask = FS_PRPVF_ROT_DEST_SEL_MASK;
+		offset = FS_PRPVF_ROT_DEST_SEL_OFFSET;
+		value = (in_dma == IDMA_ADC_SYS1_WR) ? FS_DEST_ADC1 :	/*->ADC1 */
+		    (in_dma == IDMA_ADC_SYS2_WR) ? FS_DEST_ADC2 :	/* ->ADC2 */
+		    (in_dma == IDMA_SDC_BG) ? FS_DEST_SDC_BG :	/*->SDC_BG */
+		    (in_dma == IDMA_SDC_FG) ? FS_DEST_SDC_FG :	/*->SDC_FG */
+		    0;
+		break;
+
+		/*ENC-> */
+	case IDMA_IC_0:
+		pr_debug("Link ENC->");
+		isProc = true;
+		mask = 0;
+		offset = 0;
+		value = (in_dma == IDMA_IC_10) ? FS_PRPENC_DEST_SEL :	/*->ENC_ROT	*/
+		    0;
+		break;
+
+		/*PP-> */
+	case IDMA_IC_2:
+		pr_debug("Link PP->");
+		isProc = true;
+		mask = FS_PP_DEST_SEL_MASK;
+		offset = FS_PP_DEST_SEL_OFFSET;
+		value = (in_dma == IDMA_IC_13) ? FS_DEST_ROT :	/*->PP_ROT */
+		    (in_dma == IDMA_ADC_SYS1_WR) ? FS_DEST_ADC1 :	/* ->ADC1 */
+		    (in_dma == IDMA_ADC_SYS2_WR) ? FS_DEST_ADC2 :	/* ->ADC2 */
+		    (in_dma == IDMA_SDC_BG) ? FS_DEST_SDC_BG :	/*->SDC_BG */
+		    (in_dma == IDMA_SDC_FG) ? FS_DEST_SDC_FG :	/*->SDC_FG */
+		    /* ->ADCDirect */
+		    0;
+		break;
+
+		/*PP_ROT-> */
+	case IDMA_IC_12:
+		pr_debug("Link PP_ROT->");
+		isProc = true;
+		mask = FS_PP_ROT_DEST_SEL_MASK;
+		offset = FS_PP_ROT_DEST_SEL_OFFSET;
+		value = (in_dma == IDMA_IC_5) ? FS_DEST_ROT :	/*->PP */
+		    (in_dma == IDMA_ADC_SYS1_WR) ? FS_DEST_ADC1 :	/* ->ADC1 */
+		    (in_dma == IDMA_ADC_SYS2_WR) ? FS_DEST_ADC2 :	/* ->ADC2 */
+		    (in_dma == IDMA_SDC_BG) ? FS_DEST_SDC_BG :	/*->SDC_BG */
+		    (in_dma == IDMA_SDC_FG) ? FS_DEST_SDC_FG :	/*->SDC_FG */
+		    0;
+		break;
+
+		/*PF-> */
+	case IDMA_PF_Y_OUT:
+	case IDMA_PF_U_OUT:
+	case IDMA_PF_V_OUT:
+		pr_debug("Link PF->");
+		isProc = true;
+		mask = FS_PF_DEST_SEL_MASK;
+		offset = FS_PF_DEST_SEL_OFFSET;
+		value = (in_dma == IDMA_IC_5) ? FS_PF_DEST_PP :
+		    (in_dma == IDMA_IC_13) ? FS_PF_DEST_ROT : 0;
+		break;
+
+		/* Invalid Chainings: ENC_ROT-> */
+	default:
+		pr_debug("Link Invalid->");
+		value = 0;
+		break;
+
+	}
+
+	if (value) {
+		if (isProc) {
+			fs_proc_flow &= ~mask;
+			fs_proc_flow |= (value << offset);
+		} else {
+			fs_disp_flow &= ~mask;
+			fs_disp_flow |= (value << offset);
+		}
+	} else {
+		dev_err(g_ipu_dev, "Invalid channel chaining %d -> %d\n",
+			out_dma, in_dma);
+		return -EINVAL;
+	}
+
+	/* PROCESS THE INPUT DMA CH */
+	switch (in_dma) {
+		/* ->VF_ROT */
+	case IDMA_IC_11:
+		pr_debug("VF_ROT\n");
+		isProc = true;
+		mask = 0;
+		offset = 0;
+		value = (out_dma == IDMA_IC_1) ? FS_PRPVF_ROT_SRC_SEL :	/*VF-> */
+		    0;
+		break;
+
+		/* ->ENC_ROT */
+	case IDMA_IC_10:
+		pr_debug("ENC_ROT\n");
+		isProc = true;
+		mask = 0;
+		offset = 0;
+		value = (out_dma == IDMA_IC_0) ? FS_PRPENC_ROT_SRC_SEL :	/*ENC-> */
+		    0;
+		break;
+
+		/* ->PP */
+	case IDMA_IC_5:
+		pr_debug("PP\n");
+		isProc = true;
+		mask = FS_PP_SRC_SEL_MASK;
+		offset = FS_PP_SRC_SEL_OFFSET;
+		value = (out_dma == IDMA_PF_Y_OUT) ? FS_PP_SRC_PF :	/*PF-> */
+		    (out_dma == IDMA_PF_U_OUT) ? FS_PP_SRC_PF :	/*PF-> */
+		    (out_dma == IDMA_PF_V_OUT) ? FS_PP_SRC_PF :	/*PF-> */
+		    (out_dma == IDMA_IC_12) ? FS_PP_SRC_ROT :	/*PP_ROT-> */
+		    0;
+		break;
+
+		/* ->PP_ROT */
+	case IDMA_IC_13:
+		pr_debug("PP_ROT\n");
+		isProc = true;
+		mask = FS_PP_ROT_SRC_SEL_MASK;
+		offset = FS_PP_ROT_SRC_SEL_OFFSET;
+		value = (out_dma == IDMA_PF_Y_OUT) ? FS_PP_SRC_PF :	/*PF-> */
+		    (out_dma == IDMA_PF_U_OUT) ? FS_PP_SRC_PF :	/*PF-> */
+		    (out_dma == IDMA_PF_V_OUT) ? FS_PP_SRC_PF :	/*PF-> */
+		    (out_dma == IDMA_IC_2) ? FS_ROT_SRC_PP :	/*PP-> */
+		    0;
+		break;
+
+		/* ->SDC_BG */
+	case IDMA_SDC_BG:
+		pr_debug("SDC_BG\n");
+		isProc = false;
+		mask = FS_SDC_BG_SRC_SEL_MASK;
+		offset = FS_SDC_BG_SRC_SEL_OFFSET;
+		value = (out_dma == IDMA_IC_9) ? FS_SRC_ROT_VF :	/*VF_ROT-> */
+		    (out_dma == IDMA_IC_12) ? FS_SRC_ROT_PP :	/*PP_ROT-> */
+		    (out_dma == IDMA_IC_1) ? FS_SRC_VF :	/*VF-> */
+		    (out_dma == IDMA_IC_2) ? FS_SRC_PP :	/*PP-> */
+		    0;
+		break;
+
+		/* ->SDC_FG */
+	case IDMA_SDC_FG:
+		pr_debug("SDC_FG\n");
+		isProc = false;
+		mask = FS_SDC_FG_SRC_SEL_MASK;
+		offset = FS_SDC_FG_SRC_SEL_OFFSET;
+		value = (out_dma == IDMA_IC_9) ? FS_SRC_ROT_VF :	/*VF_ROT-> */
+		    (out_dma == IDMA_IC_12) ? FS_SRC_ROT_PP :	/*PP_ROT-> */
+		    (out_dma == IDMA_IC_1) ? FS_SRC_VF :	/*VF-> */
+		    (out_dma == IDMA_IC_2) ? FS_SRC_PP :	/*PP-> */
+		    0;
+		break;
+
+		/* ->ADC1 */
+	case IDMA_ADC_SYS1_WR:
+		pr_debug("ADC_SYS1\n");
+		isProc = false;
+		mask = FS_ADC1_SRC_SEL_MASK;
+		offset = FS_ADC1_SRC_SEL_OFFSET;
+		value = (out_dma == IDMA_IC_9) ? FS_SRC_ROT_VF :	/*VF_ROT-> */
+		    (out_dma == IDMA_IC_12) ? FS_SRC_ROT_PP :	/*PP_ROT-> */
+		    (out_dma == IDMA_IC_1) ? FS_SRC_VF :	/*VF-> */
+		    (out_dma == IDMA_IC_2) ? FS_SRC_PP :	/*PP-> */
+		    0;
+		break;
+
+		/* ->ADC2 */
+	case IDMA_ADC_SYS2_WR:
+		pr_debug("ADC_SYS2\n");
+		isProc = false;
+		mask = FS_ADC2_SRC_SEL_MASK;
+		offset = FS_ADC2_SRC_SEL_OFFSET;
+		value = (out_dma == IDMA_IC_9) ? FS_SRC_ROT_VF :	/*VF_ROT-> */
+		    (out_dma == IDMA_IC_12) ? FS_SRC_ROT_PP :	/*PP_ROT-> */
+		    (out_dma == IDMA_IC_1) ? FS_SRC_VF :	/*VF-> */
+		    (out_dma == IDMA_IC_2) ? FS_SRC_PP :	/*PP-> */
+		    0;
+		break;
+
+		/*Invalid chains: */
+		/* ->ENC, ->VF, ->PF, ->VF_COMBINE, ->PP_COMBINE */
+	default:
+		pr_debug("Invalid\n");
+		value = 0;
+		break;
+
+	}
+
+	if (value) {
+		if (isProc) {
+			fs_proc_flow &= ~mask;
+			fs_proc_flow |= (value << offset);
+		} else {
+			fs_disp_flow &= ~mask;
+			fs_disp_flow |= (value << offset);
+		}
+	} else {
+		dev_err(g_ipu_dev, "Invalid channel chaining %d -> %d\n",
+			out_dma, in_dma);
+		return -EINVAL;
+	}
+
+	__raw_writel(fs_proc_flow, IPU_FS_PROC_FLOW);
+	__raw_writel(fs_disp_flow, IPU_FS_DISP_FLOW);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return 0;
+}
+
+/*!
+ * This function unlinks 2 channels and disables automatic frame
+ * synchronization.
+ *
+ * @param       src_ch          Input parameter for the logical channel ID of
+ *                              the source channel.
+ *
+ * @param       dest_ch         Input parameter for the logical channel ID of
+ *                              the destination channel.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int32_t ipu_unlink_channels(ipu_channel_t src_ch, ipu_channel_t dest_ch)
+{
+	unsigned long lock_flags;
+	uint32_t out_dma;
+	uint32_t in_dma;
+	uint32_t fs_proc_flow;
+	uint32_t fs_disp_flow;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	fs_proc_flow = __raw_readl(IPU_FS_PROC_FLOW);
+	fs_disp_flow = __raw_readl(IPU_FS_DISP_FLOW);
+
+	out_dma = (1UL << channel_2_dma(src_ch, IPU_OUTPUT_BUFFER));
+	in_dma = (1UL << channel_2_dma(dest_ch, IPU_INPUT_BUFFER));
+
+	/*clear the src_ch's output destination */
+	switch (out_dma) {
+		/*VF-> */
+	case IDMA_IC_1:
+		pr_debug("Unlink VF->");
+		fs_proc_flow &= ~FS_PRPVF_DEST_SEL_MASK;
+		break;
+
+		/*VF_ROT-> */
+	case IDMA_IC_9:
+		pr_debug("Unlink VF_Rot->");
+		fs_proc_flow &= ~FS_PRPVF_ROT_DEST_SEL_MASK;
+		break;
+
+		/*ENC-> */
+	case IDMA_IC_0:
+		pr_debug("Unlink ENC->");
+		fs_proc_flow &= ~FS_PRPENC_DEST_SEL;
+		break;
+
+		/*PP-> */
+	case IDMA_IC_2:
+		pr_debug("Unlink PP->");
+		fs_proc_flow &= ~FS_PP_DEST_SEL_MASK;
+		break;
+
+		/*PP_ROT-> */
+	case IDMA_IC_12:
+		pr_debug("Unlink PP_ROT->");
+		fs_proc_flow &= ~FS_PP_ROT_DEST_SEL_MASK;
+		break;
+
+		/*PF-> */
+	case IDMA_PF_Y_OUT:
+	case IDMA_PF_U_OUT:
+	case IDMA_PF_V_OUT:
+		pr_debug("Unlink PF->");
+		fs_proc_flow &= ~FS_PF_DEST_SEL_MASK;
+		break;
+
+	default:		/*ENC_ROT->     */
+		pr_debug("Unlink Invalid->");
+		break;
+	}
+
+	/*clear the dest_ch's input source */
+	switch (in_dma) {
+	/*->VF_ROT*/
+	case IDMA_IC_11:
+		pr_debug("VF_ROT\n");
+		fs_proc_flow &= ~FS_PRPVF_ROT_SRC_SEL;
+		break;
+
+	/*->Enc_ROT*/
+	case IDMA_IC_10:
+		pr_debug("ENC_ROT\n");
+		fs_proc_flow &= ~FS_PRPENC_ROT_SRC_SEL;
+		break;
+
+	/*->PP*/
+	case IDMA_IC_5:
+		pr_debug("PP\n");
+		fs_proc_flow &= ~FS_PP_SRC_SEL_MASK;
+		break;
+
+	/*->PP_ROT*/
+	case IDMA_IC_13:
+		pr_debug("PP_ROT\n");
+		fs_proc_flow &= ~FS_PP_ROT_SRC_SEL_MASK;
+		break;
+
+	/*->SDC_FG*/
+	case IDMA_SDC_FG:
+		pr_debug("SDC_FG\n");
+		fs_disp_flow &= ~FS_SDC_FG_SRC_SEL_MASK;
+		break;
+
+	/*->SDC_BG*/
+	case IDMA_SDC_BG:
+		pr_debug("SDC_BG\n");
+		fs_disp_flow &= ~FS_SDC_BG_SRC_SEL_MASK;
+		break;
+
+	/*->ADC1*/
+	case IDMA_ADC_SYS1_WR:
+		pr_debug("ADC_SYS1\n");
+		fs_disp_flow &= ~FS_ADC1_SRC_SEL_MASK;
+		break;
+
+	/*->ADC2*/
+	case IDMA_ADC_SYS2_WR:
+		pr_debug("ADC_SYS2\n");
+		fs_disp_flow &= ~FS_ADC2_SRC_SEL_MASK;
+		break;
+
+	default:		/*->VF, ->ENC, ->VF_COMBINE, ->PP_COMBINE, ->PF*/
+		pr_debug("Invalid\n");
+		break;
+	}
+
+	__raw_writel(fs_proc_flow, IPU_FS_PROC_FLOW);
+	__raw_writel(fs_disp_flow, IPU_FS_DISP_FLOW);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return 0;
+}
+
+/*!
+ * This function check whether a logical channel was enabled.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @return      This function returns 1 while request channel is enabled or
+ *              0 for not enabled.
+ */
+int32_t ipu_is_channel_busy(ipu_channel_t channel)
+{
+	uint32_t reg;
+	uint32_t in_dma;
+	uint32_t out_dma;
+
+	out_dma = channel_2_dma(channel, IPU_OUTPUT_BUFFER);
+	in_dma = channel_2_dma(channel, IPU_INPUT_BUFFER);
+
+	reg = __raw_readl(IDMAC_CHA_EN);
+
+	if (reg & ((1UL << out_dma) | (1UL << in_dma)))
+		return 1;
+	return 0;
+}
+EXPORT_SYMBOL(ipu_is_channel_busy);
+
+/*!
+ * This function enables a logical channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int32_t ipu_enable_channel(ipu_channel_t channel)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+	uint32_t in_dma;
+	uint32_t sec_dma;
+	uint32_t out_dma;
+	uint32_t chan_mask = 0;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(IDMAC_CHA_EN);
+
+	/* Get input and output dma channels */
+	out_dma = channel_2_dma(channel, IPU_OUTPUT_BUFFER);
+	if (out_dma != IDMA_CHAN_INVALID)
+		reg |= 1UL << out_dma;
+	in_dma = channel_2_dma(channel, IPU_INPUT_BUFFER);
+	if (in_dma != IDMA_CHAN_INVALID)
+		reg |= 1UL << in_dma;
+
+	/* Get secondary input dma channel */
+	if (g_sec_chan_en[IPU_CHAN_ID(channel)]) {
+		sec_dma = channel_2_dma(channel, IPU_SEC_INPUT_BUFFER);
+		if (sec_dma != IDMA_CHAN_INVALID) {
+			reg |= 1UL << sec_dma;
+		}
+	}
+
+	__raw_writel(reg | chan_mask, IDMAC_CHA_EN);
+
+	if (IPU_CHAN_ID(channel) <= IPU_CHAN_ID(MEM_PP_ADC)) {
+		_ipu_ic_enable_task(channel);
+	} else if (channel == MEM_SDC_BG) {
+		dev_dbg(g_ipu_dev, "Initializing SDC BG\n");
+		_ipu_sdc_bg_init(NULL);
+	} else if (channel == MEM_SDC_FG) {
+		dev_dbg(g_ipu_dev, "Initializing SDC FG\n");
+		_ipu_sdc_fg_init(NULL);
+	}
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return 0;
+}
+
+/*!
+ * This function disables a logical channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       wait_for_stop   Flag to set whether to wait for channel end
+ *                              of frame or return immediately.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int32_t ipu_disable_channel(ipu_channel_t channel, bool wait_for_stop)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+	uint32_t sec_dma;
+	uint32_t in_dma;
+	uint32_t out_dma;
+	uint32_t chan_mask = 0;
+	uint32_t timeout;
+	uint32_t eof_intr;
+	uint32_t enabled;
+
+	/* Get input and output dma channels */
+	out_dma = channel_2_dma(channel, IPU_OUTPUT_BUFFER);
+	if (out_dma != IDMA_CHAN_INVALID)
+		chan_mask = 1UL << out_dma;
+	in_dma = channel_2_dma(channel, IPU_INPUT_BUFFER);
+	if (in_dma != IDMA_CHAN_INVALID)
+		chan_mask |= 1UL << in_dma;
+	sec_dma = channel_2_dma(channel, IPU_SEC_INPUT_BUFFER);
+	if (sec_dma != IDMA_CHAN_INVALID)
+		chan_mask |= 1UL << sec_dma;
+
+	if (wait_for_stop && channel != MEM_SDC_FG && channel != MEM_SDC_BG) {
+		timeout = 40;
+		while ((__raw_readl(IDMAC_CHA_BUSY) & chan_mask) ||
+		       (_ipu_channel_status(channel) == TASK_STAT_ACTIVE)) {
+			timeout--;
+			msleep(10);
+			if (timeout == 0) {
+				printk
+				    (KERN_INFO
+				     "MXC IPU: Warning - timeout waiting for channel to stop,\n"
+				     "\tbuf0_rdy = 0x%08X, buf1_rdy = 0x%08X\n"
+				     "\tbusy = 0x%08X, tstat = 0x%08X\n\tmask = 0x%08X\n",
+				     __raw_readl(IPU_CHA_BUF0_RDY),
+				     __raw_readl(IPU_CHA_BUF1_RDY),
+				     __raw_readl(IDMAC_CHA_BUSY),
+				     __raw_readl(IPU_TASKS_STAT), chan_mask);
+				break;
+			}
+		}
+		dev_dbg(g_ipu_dev, "timeout = %d * 10ms\n", 40 - timeout);
+	}
+	/* SDC BG and FG must be disabled before DMA is disabled */
+	if ((channel == MEM_SDC_BG) || (channel == MEM_SDC_FG)) {
+
+		if (channel == MEM_SDC_BG)
+			eof_intr = IPU_IRQ_SDC_BG_EOF;
+		else
+			eof_intr = IPU_IRQ_SDC_FG_EOF;
+
+		/* Wait for any buffer flips to finsh */
+		timeout = 4;
+		while (timeout &&
+		       ((__raw_readl(IPU_CHA_BUF0_RDY) & chan_mask) ||
+			(__raw_readl(IPU_CHA_BUF1_RDY) & chan_mask))) {
+			msleep(10);
+			timeout--;
+		}
+
+		spin_lock_irqsave(&ipu_lock, lock_flags);
+		ipu_clear_irq(eof_intr);
+		if (channel == MEM_SDC_BG)
+			enabled = _ipu_sdc_bg_uninit();
+		else
+			enabled = _ipu_sdc_fg_uninit();
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+		if (enabled && wait_for_stop) {
+			timeout = 5;
+		} else {
+			timeout = 0;
+		}
+		while (timeout && !ipu_get_irq_status(eof_intr)) {
+			msleep(5);
+			timeout--;
+		}
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	/* Disable IC task */
+	if (IPU_CHAN_ID(channel) <= IPU_CHAN_ID(MEM_PP_ADC)) {
+		_ipu_ic_disable_task(channel);
+	}
+
+	/* Disable DMA channel(s) */
+	reg = __raw_readl(IDMAC_CHA_EN);
+	__raw_writel(reg & ~chan_mask, IDMAC_CHA_EN);
+
+	/* Clear DMA related interrupts */
+	__raw_writel(chan_mask, IPU_INT_STAT_1);
+	__raw_writel(chan_mask, IPU_INT_STAT_2);
+	__raw_writel(chan_mask, IPU_INT_STAT_4);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+
+static
+irqreturn_t ipu_irq_handler(int irq, void *desc)
+{
+	uint32_t line_base = 0;
+	uint32_t line;
+	irqreturn_t result = IRQ_NONE;
+	uint32_t int_stat;
+
+	if (g_ipu_irq[1]) {
+		disable_irq(g_ipu_irq[0]);
+		disable_irq(g_ipu_irq[1]);
+	}
+
+	int_stat = __raw_readl(IPU_INT_STAT_1);
+	int_stat &= __raw_readl(IPU_INT_CTRL_1);
+	__raw_writel(int_stat, IPU_INT_STAT_1);
+	while ((line = ffs(int_stat)) != 0) {
+		int_stat &= ~(1UL << (line - 1));
+		line += line_base - 1;
+		result |=
+		    ipu_irq_list[line].handler(line, ipu_irq_list[line].dev_id);
+	}
+
+	line_base = 32;
+	int_stat = __raw_readl(IPU_INT_STAT_2);
+	int_stat &= __raw_readl(IPU_INT_CTRL_2);
+	__raw_writel(int_stat, IPU_INT_STAT_2);
+	while ((line = ffs(int_stat)) != 0) {
+		int_stat &= ~(1UL << (line - 1));
+		line += line_base - 1;
+		result |=
+		    ipu_irq_list[line].handler(line, ipu_irq_list[line].dev_id);
+	}
+
+	line_base = 64;
+	int_stat = __raw_readl(IPU_INT_STAT_3);
+	int_stat &= __raw_readl(IPU_INT_CTRL_3);
+	__raw_writel(int_stat, IPU_INT_STAT_3);
+	while ((line = ffs(int_stat)) != 0) {
+		int_stat &= ~(1UL << (line - 1));
+		line += line_base - 1;
+		result |=
+		    ipu_irq_list[line].handler(line, ipu_irq_list[line].dev_id);
+	}
+
+	line_base = 96;
+	int_stat = __raw_readl(IPU_INT_STAT_4);
+	int_stat &= __raw_readl(IPU_INT_CTRL_4);
+	__raw_writel(int_stat, IPU_INT_STAT_4);
+	while ((line = ffs(int_stat)) != 0) {
+		int_stat &= ~(1UL << (line - 1));
+		line += line_base - 1;
+		result |=
+		    ipu_irq_list[line].handler(line, ipu_irq_list[line].dev_id);
+	}
+
+	line_base = 128;
+	int_stat = __raw_readl(IPU_INT_STAT_5);
+	int_stat &= __raw_readl(IPU_INT_CTRL_5);
+	__raw_writel(int_stat, IPU_INT_STAT_5);
+	while ((line = ffs(int_stat)) != 0) {
+		int_stat &= ~(1UL << (line - 1));
+		line += line_base - 1;
+		result |=
+		    ipu_irq_list[line].handler(line, ipu_irq_list[line].dev_id);
+	}
+
+	if (g_ipu_irq[1]) {
+		enable_irq(g_ipu_irq[0]);
+		enable_irq(g_ipu_irq[1]);
+	}
+	return result;
+}
+
+/*!
+ * This function enables the interrupt for the specified interrupt line.
+ * The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to enable interrupt for.
+ *
+ */
+void ipu_enable_irq(uint32_t irq)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(IPUIRQ_2_CTRLREG(irq));
+	reg |= IPUIRQ_2_MASK(irq);
+	__raw_writel(reg, IPUIRQ_2_CTRLREG(irq));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+
+/*!
+ * This function disables the interrupt for the specified interrupt line.
+ * The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to disable interrupt for.
+ *
+ */
+void ipu_disable_irq(uint32_t irq)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(IPUIRQ_2_CTRLREG(irq));
+	reg &= ~IPUIRQ_2_MASK(irq);
+	__raw_writel(reg, IPUIRQ_2_CTRLREG(irq));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+
+/*!
+ * This function clears the interrupt for the specified interrupt line.
+ * The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to clear interrupt for.
+ *
+ */
+void ipu_clear_irq(uint32_t irq)
+{
+	__raw_writel(IPUIRQ_2_MASK(irq), IPUIRQ_2_STATREG(irq));
+}
+
+/*!
+ * This function returns the current interrupt status for the specified interrupt
+ * line. The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to get status for.
+ *
+ * @return      Returns true if the interrupt is pending/asserted or false if
+ *              the interrupt is not pending.
+ */
+bool ipu_get_irq_status(uint32_t irq)
+{
+	uint32_t reg = __raw_readl(IPUIRQ_2_STATREG(irq));
+
+	if (reg & IPUIRQ_2_MASK(irq))
+		return true;
+	else
+		return false;
+}
+
+/*!
+ * This function registers an interrupt handler function for the specified
+ * interrupt line. The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to get status for.
+ *
+ * @param       handler         Input parameter for address of the handler
+ *                              function.
+ *
+ * @param       irq_flags       Flags for interrupt mode. Currently not used.
+ *
+ * @param       devname         Input parameter for string name of driver
+ *                              registering the handler.
+ *
+ * @param       dev_id          Input parameter for pointer of data to be passed
+ *                              to the handler.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int ipu_request_irq(uint32_t irq,
+		    irqreturn_t(*handler) (int, void *),
+		    uint32_t irq_flags, const char *devname, void *dev_id)
+{
+	unsigned long lock_flags;
+
+	BUG_ON(irq >= IPU_IRQ_COUNT);
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if (ipu_irq_list[irq].handler != NULL) {
+		dev_err(g_ipu_dev,
+			"ipu_request_irq - handler already installed on irq %d\n",
+			irq);
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+		return -EINVAL;
+	}
+
+	ipu_irq_list[irq].handler = handler;
+	ipu_irq_list[irq].flags = irq_flags;
+	ipu_irq_list[irq].dev_id = dev_id;
+	ipu_irq_list[irq].name = devname;
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	ipu_enable_irq(irq);	/* enable the interrupt */
+
+	return 0;
+}
+
+/*!
+ * This function unregisters an interrupt handler for the specified interrupt
+ * line. The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to get status for.
+ *
+ * @param       dev_id          Input parameter for pointer of data to be passed
+ *                              to the handler. This must match value passed to
+ *                              ipu_request_irq().
+ *
+ */
+void ipu_free_irq(uint32_t irq, void *dev_id)
+{
+	ipu_disable_irq(irq);	/* disable the interrupt */
+
+	if (ipu_irq_list[irq].dev_id == dev_id) {
+		ipu_irq_list[irq].handler = NULL;
+	}
+}
+
+/*!
+ * This function sets the post-filter pause row for h.264 mode.
+ *
+ * @param       pause_row       The last row to process before pausing.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ *
+ */
+int32_t ipu_pf_set_pause_row(uint32_t pause_row)
+{
+	int32_t retval = 0;
+	uint32_t timeout = 5;
+	unsigned long lock_flags;
+	uint32_t reg;
+
+	reg = __raw_readl(IPU_TASKS_STAT);
+	while ((reg & TSTAT_PF_MASK) && ((reg & TSTAT_PF_H264_PAUSE) == 0)) {
+		timeout--;
+		msleep(5);
+		if (timeout == 0) {
+			dev_err(g_ipu_dev, "PF Timeout - tstat = 0x%08X\n",
+				__raw_readl(IPU_TASKS_STAT));
+			retval = -ETIMEDOUT;
+			goto err0;
+		}
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(PF_CONF);
+
+	/* Set the pause row */
+	if (pause_row) {
+		reg &= ~PF_CONF_PAUSE_ROW_MASK;
+		reg |= PF_CONF_PAUSE_EN | pause_row << PF_CONF_PAUSE_ROW_SHIFT;
+	} else {
+		reg &= ~(PF_CONF_PAUSE_EN | PF_CONF_PAUSE_ROW_MASK);
+	}
+	__raw_writel(reg, PF_CONF);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+      err0:
+	return retval;
+}
+
+/* Private functions */
+void _ipu_write_param_mem(uint32_t addr, uint32_t * data, uint32_t numWords)
+{
+	for (; numWords > 0; numWords--) {
+		dev_dbg(g_ipu_dev,
+			"write param mem - addr = 0x%08X, data = 0x%08X\n",
+			addr, *data);
+		__raw_writel(addr, IPU_IMA_ADDR);
+		__raw_writel(*data++, IPU_IMA_DATA);
+		addr++;
+		if ((addr & 0x7) == 5) {
+			addr &= ~0x7;	/* set to word 0 */
+			addr += 8;	/* increment to next row */
+		}
+	}
+}
+
+static void _ipu_pf_init(ipu_channel_params_t * params)
+{
+	uint32_t reg;
+
+	/*Setup the type of filtering required */
+	switch (params->mem_pf_mem.operation) {
+	case PF_MPEG4_DEBLOCK:
+	case PF_MPEG4_DERING:
+	case PF_MPEG4_DEBLOCK_DERING:
+		g_sec_chan_en[IPU_CHAN_ID(MEM_PF_Y_MEM)] = true;
+		g_sec_chan_en[IPU_CHAN_ID(MEM_PF_U_MEM)] = false;
+		break;
+	case PF_H264_DEBLOCK:
+		g_sec_chan_en[IPU_CHAN_ID(MEM_PF_Y_MEM)] = true;
+		g_sec_chan_en[IPU_CHAN_ID(MEM_PF_U_MEM)] = true;
+		break;
+	default:
+		g_sec_chan_en[IPU_CHAN_ID(MEM_PF_Y_MEM)] = false;
+		g_sec_chan_en[IPU_CHAN_ID(MEM_PF_U_MEM)] = false;
+		return;
+		break;
+	}
+	reg = params->mem_pf_mem.operation;
+	__raw_writel(reg, PF_CONF);
+}
+
+static void _ipu_pf_uninit(void)
+{
+	__raw_writel(0x0L, PF_CONF);
+	g_sec_chan_en[IPU_CHAN_ID(MEM_PF_Y_MEM)] = false;
+	g_sec_chan_en[IPU_CHAN_ID(MEM_PF_U_MEM)] = false;
+}
+
+uint32_t _ipu_channel_status(ipu_channel_t channel)
+{
+	uint32_t stat = 0;
+	uint32_t task_stat_reg = __raw_readl(IPU_TASKS_STAT);
+
+	switch (channel) {
+	case CSI_MEM:
+		stat =
+		    (task_stat_reg & TSTAT_CSI2MEM_MASK) >>
+		    TSTAT_CSI2MEM_OFFSET;
+		break;
+	case CSI_PRP_VF_ADC:
+	case CSI_PRP_VF_MEM:
+	case MEM_PRP_VF_ADC:
+	case MEM_PRP_VF_MEM:
+		stat = (task_stat_reg & TSTAT_VF_MASK) >> TSTAT_VF_OFFSET;
+		break;
+	case MEM_ROT_VF_MEM:
+		stat =
+		    (task_stat_reg & TSTAT_VF_ROT_MASK) >> TSTAT_VF_ROT_OFFSET;
+		break;
+	case CSI_PRP_ENC_MEM:
+	case MEM_PRP_ENC_MEM:
+		stat = (task_stat_reg & TSTAT_ENC_MASK) >> TSTAT_ENC_OFFSET;
+		break;
+	case MEM_ROT_ENC_MEM:
+		stat =
+		    (task_stat_reg & TSTAT_ENC_ROT_MASK) >>
+		    TSTAT_ENC_ROT_OFFSET;
+		break;
+	case MEM_PP_ADC:
+	case MEM_PP_MEM:
+		stat = (task_stat_reg & TSTAT_PP_MASK) >> TSTAT_PP_OFFSET;
+		break;
+	case MEM_ROT_PP_MEM:
+		stat =
+		    (task_stat_reg & TSTAT_PP_ROT_MASK) >> TSTAT_PP_ROT_OFFSET;
+		break;
+
+	case MEM_PF_Y_MEM:
+	case MEM_PF_U_MEM:
+	case MEM_PF_V_MEM:
+		stat = (task_stat_reg & TSTAT_PF_MASK) >> TSTAT_PF_OFFSET;
+		break;
+	case MEM_SDC_BG:
+		break;
+	case MEM_SDC_FG:
+		break;
+	case ADC_SYS1:
+		stat =
+		    (task_stat_reg & TSTAT_ADCSYS1_MASK) >>
+		    TSTAT_ADCSYS1_OFFSET;
+		break;
+	case ADC_SYS2:
+		stat =
+		    (task_stat_reg & TSTAT_ADCSYS2_MASK) >>
+		    TSTAT_ADCSYS2_OFFSET;
+		break;
+	case MEM_SDC_MASK:
+	default:
+		stat = TASK_STAT_IDLE;
+		break;
+	}
+	return stat;
+}
+
+uint32_t bytes_per_pixel(uint32_t fmt)
+{
+	switch (fmt) {
+	case IPU_PIX_FMT_GENERIC:	/*generic data */
+	case IPU_PIX_FMT_RGB332:
+	case IPU_PIX_FMT_YUV420P:
+	case IPU_PIX_FMT_YUV422P:
+		return 1;
+		break;
+	case IPU_PIX_FMT_RGB565:
+	case IPU_PIX_FMT_YUYV:
+	case IPU_PIX_FMT_UYVY:
+		return 2;
+		break;
+	case IPU_PIX_FMT_BGR24:
+	case IPU_PIX_FMT_RGB24:
+		return 3;
+		break;
+	case IPU_PIX_FMT_GENERIC_32:	/*generic data */
+	case IPU_PIX_FMT_BGR32:
+	case IPU_PIX_FMT_RGB32:
+	case IPU_PIX_FMT_ABGR32:
+		return 4;
+		break;
+	default:
+		return 1;
+		break;
+	}
+	return 0;
+}
+
+void ipu_set_csc_coefficients(ipu_channel_t channel, int32_t param[][3])
+{
+	/* TODO */
+}
+EXPORT_SYMBOL(ipu_set_csc_coefficients);
+
+ipu_color_space_t format_to_colorspace(uint32_t fmt)
+{
+	switch (fmt) {
+	case IPU_PIX_FMT_RGB565:
+	case IPU_PIX_FMT_BGR24:
+	case IPU_PIX_FMT_RGB24:
+	case IPU_PIX_FMT_BGR32:
+	case IPU_PIX_FMT_RGB32:
+		return RGB;
+		break;
+
+	default:
+		return YCbCr;
+		break;
+	}
+	return RGB;
+
+}
+
+static u32 saved_disp3_time_conf;
+static bool in_lpdr_mode;
+static struct clk *default_ipu_parent_clk;
+
+int ipu_lowpwr_display_enable(void)
+{
+	unsigned long rate, div;
+	struct clk *parent_clk = g_ipu_clk;
+
+	if (in_lpdr_mode || IS_ERR(dfm_clk)) {
+		return -EINVAL;
+	}
+
+	if (g_channel_init_mask != (1L << IPU_CHAN_ID(MEM_SDC_BG))) {
+		dev_err(g_ipu_dev, "LPDR mode requires only SDC BG active.\n");
+		return -EINVAL;
+	}
+
+	default_ipu_parent_clk = clk_get_parent(g_ipu_clk);
+	in_lpdr_mode = true;
+
+	/* Calculate current pixel clock rate */
+	rate = clk_get_rate(g_ipu_clk) * 16;
+	saved_disp3_time_conf = __raw_readl(DI_DISP3_TIME_CONF);
+	div = saved_disp3_time_conf & 0xFFF;
+	rate /= div;
+	rate *= 4;		/* min hsp clk is 4x pixel clk */
+
+	/* Initialize DFM clock */
+	rate = clk_round_rate(dfm_clk, rate);
+	clk_set_rate(dfm_clk, rate);
+	clk_enable(dfm_clk);
+
+	/* Wait for next VSYNC */
+	__raw_writel(IPUIRQ_2_MASK(IPU_IRQ_SDC_DISP3_VSYNC),
+		     IPUIRQ_2_STATREG(IPU_IRQ_SDC_DISP3_VSYNC));
+	while ((__raw_readl(IPUIRQ_2_STATREG(IPU_IRQ_SDC_DISP3_VSYNC)) &
+		IPUIRQ_2_MASK(IPU_IRQ_SDC_DISP3_VSYNC)) == 0)
+		msleep_interruptible(1);
+
+	/* Set display clock divider to divide by 4 */
+	__raw_writel(((0x8) << 22) | 0x40, DI_DISP3_TIME_CONF);
+
+	clk_set_parent(parent_clk, dfm_clk);
+
+	return 0;
+}
+
+int ipu_lowpwr_display_disable(void)
+{
+	struct clk *parent_clk = g_ipu_clk;
+
+	if (!in_lpdr_mode || IS_ERR(dfm_clk)) {
+		return -EINVAL;
+	}
+
+	if (g_channel_init_mask != (1L << IPU_CHAN_ID(MEM_SDC_BG))) {
+		dev_err(g_ipu_dev, "LPDR mode requires only SDC BG active.\n");
+		return -EINVAL;
+	}
+
+	in_lpdr_mode = false;
+
+	/* Wait for next VSYNC */
+	__raw_writel(IPUIRQ_2_MASK(IPU_IRQ_SDC_DISP3_VSYNC),
+		     IPUIRQ_2_STATREG(IPU_IRQ_SDC_DISP3_VSYNC));
+	while ((__raw_readl(IPUIRQ_2_STATREG(IPU_IRQ_SDC_DISP3_VSYNC)) &
+		IPUIRQ_2_MASK(IPU_IRQ_SDC_DISP3_VSYNC)) == 0)
+		msleep_interruptible(1);
+
+	__raw_writel(saved_disp3_time_conf, DI_DISP3_TIME_CONF);
+	clk_set_parent(parent_clk, default_ipu_parent_clk);
+	clk_disable(dfm_clk);
+
+	return 0;
+}
+
+static int ipu_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	if (cpu_is_mx31() || cpu_is_mx32()) {
+		/* work-around for i.Mx31 SR mode after camera related test */
+		if (g_csi_used) {
+			g_ipu_config = __raw_readl(IPU_CONF);
+			clk_enable(g_ipu_csi_clk);
+			__raw_writel(0x51, IPU_CONF);
+			g_channel_init_mask_backup = g_channel_init_mask;
+			g_channel_init_mask |= 2;
+		}
+	} else if (cpu_is_mx35()) {
+		g_ipu_config = __raw_readl(IPU_CONF);
+		/* Disable the clock of display otherwise the backlight cannot
+		 * be close after camera/tvin related test */
+		__raw_writel(g_ipu_config & 0xfbf, IPU_CONF);
+	}
+
+	return 0;
+}
+
+static int ipu_resume(struct platform_device *pdev)
+{
+	if (cpu_is_mx31() || cpu_is_mx32()) {
+		/* work-around for i.Mx31 SR mode after camera related test */
+		if (g_csi_used) {
+			__raw_writel(g_ipu_config, IPU_CONF);
+			clk_disable(g_ipu_csi_clk);
+			g_channel_init_mask = g_channel_init_mask_backup;
+		}
+	} else if (cpu_is_mx35()) {
+		__raw_writel(g_ipu_config, IPU_CONF);
+    }
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcipu_driver = {
+	.driver = {
+		   .name = "mxc_ipu",
+		   },
+	.probe = ipu_probe,
+	.suspend = ipu_suspend,
+	.resume = ipu_resume,
+};
+
+int32_t __init ipu_gen_init(void)
+{
+	int32_t ret;
+
+	ret = platform_driver_register(&mxcipu_driver);
+	return 0;
+}
+
+subsys_initcall(ipu_gen_init);
+
+static void __exit ipu_gen_uninit(void)
+{
+	if (g_ipu_irq[0])
+		free_irq(g_ipu_irq[0], 0);
+	if (g_ipu_irq[1])
+		free_irq(g_ipu_irq[1], 0);
+
+	platform_driver_unregister(&mxcipu_driver);
+}
+
+module_exit(ipu_gen_uninit);
+
+EXPORT_SYMBOL(ipu_init_channel);
+EXPORT_SYMBOL(ipu_uninit_channel);
+EXPORT_SYMBOL(ipu_init_channel_buffer);
+EXPORT_SYMBOL(ipu_unlink_channels);
+EXPORT_SYMBOL(ipu_update_channel_buffer);
+EXPORT_SYMBOL(ipu_select_buffer);
+EXPORT_SYMBOL(ipu_link_channels);
+EXPORT_SYMBOL(ipu_enable_channel);
+EXPORT_SYMBOL(ipu_disable_channel);
+EXPORT_SYMBOL(ipu_enable_irq);
+EXPORT_SYMBOL(ipu_disable_irq);
+EXPORT_SYMBOL(ipu_clear_irq);
+EXPORT_SYMBOL(ipu_get_irq_status);
+EXPORT_SYMBOL(ipu_request_irq);
+EXPORT_SYMBOL(ipu_free_irq);
+EXPORT_SYMBOL(ipu_pf_set_pause_row);
+EXPORT_SYMBOL(bytes_per_pixel);
diff --git a/drivers/mxc/ipu/ipu_csi.c b/drivers/mxc/ipu/ipu_csi.c
new file mode 100644
index 000000000000..10708cf3ba54
--- /dev/null
+++ b/drivers/mxc/ipu/ipu_csi.c
@@ -0,0 +1,222 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_csi.c
+ *
+ * @brief IPU CMOS Sensor interface functions
+ *
+ * @ingroup IPU
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/ipu.h>
+
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+
+static bool gipu_csi_get_mclk_flag = false;
+static int csi_mclk_flag = 0;
+
+extern void gpio_sensor_suspend(bool flag);
+
+/*!
+ * ipu_csi_init_interface
+ *    Sets initial values for the CSI registers.
+ *    The width and height of the sensor and the actual frame size will be
+ *    set to the same values.
+ * @param       width        Sensor width
+ * @param       height       Sensor height
+ * @param       pixel_fmt    pixel format
+ * @param       sig          ipu_csi_signal_cfg_t structure
+ *
+ * @return      0 for success, -EINVAL for error
+ */
+int32_t
+ipu_csi_init_interface(uint16_t width, uint16_t height, uint32_t pixel_fmt,
+		       ipu_csi_signal_cfg_t sig)
+{
+	uint32_t data = 0;
+
+	/* Set SENS_DATA_FORMAT bits (8 and 9)
+	   RGB or YUV444 is 0 which is current value in data so not set explicitly
+	   This is also the default value if attempts are made to set it to
+	   something invalid. */
+	switch (pixel_fmt) {
+	case IPU_PIX_FMT_UYVY:
+		data = CSI_SENS_CONF_DATA_FMT_YUV422;
+		break;
+	case IPU_PIX_FMT_RGB24:
+	case IPU_PIX_FMT_BGR24:
+		data = CSI_SENS_CONF_DATA_FMT_RGB_YUV444;
+		break;
+	case IPU_PIX_FMT_GENERIC:
+		data = CSI_SENS_CONF_DATA_FMT_BAYER;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Set the CSI_SENS_CONF register remaining fields */
+	data |= sig.data_width << CSI_SENS_CONF_DATA_WIDTH_SHIFT |
+	    sig.data_pol << CSI_SENS_CONF_DATA_POL_SHIFT |
+	    sig.Vsync_pol << CSI_SENS_CONF_VSYNC_POL_SHIFT |
+	    sig.Hsync_pol << CSI_SENS_CONF_HSYNC_POL_SHIFT |
+	    sig.pixclk_pol << CSI_SENS_CONF_PIX_CLK_POL_SHIFT |
+	    sig.ext_vsync << CSI_SENS_CONF_EXT_VSYNC_SHIFT |
+	    sig.clk_mode << CSI_SENS_CONF_SENS_PRTCL_SHIFT |
+	    sig.sens_clksrc << CSI_SENS_CONF_SENS_CLKSRC_SHIFT;
+
+	__raw_writel(data, CSI_SENS_CONF);
+
+	/* Setup frame size     */
+	__raw_writel(width | height << 16, CSI_SENS_FRM_SIZE);
+
+	__raw_writel(width << 16, CSI_FLASH_STROBE_1);
+	__raw_writel(height << 16 | 0x22, CSI_FLASH_STROBE_2);
+
+	/* Set CCIR registers */
+	if ((sig.clk_mode == IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE) ||
+	    (sig.clk_mode == IPU_CSI_CLK_MODE_CCIR656_INTERLACED)) {
+		__raw_writel(0x40030, CSI_CCIR_CODE_1);
+		__raw_writel(0xFF0000, CSI_CCIR_CODE_3);
+	}
+
+	dev_dbg(g_ipu_dev, "CSI_SENS_CONF = 0x%08X\n",
+		__raw_readl(CSI_SENS_CONF));
+	dev_dbg(g_ipu_dev, "CSI_ACT_FRM_SIZE = 0x%08X\n",
+		__raw_readl(CSI_ACT_FRM_SIZE));
+
+	return 0;
+}
+
+/*!
+ * ipu_csi_flash_strobe
+ *
+ * @param       flag         true to turn on flash strobe
+ *
+ * @return      0 for success
+ */
+void ipu_csi_flash_strobe(bool flag)
+{
+	if (flag == true) {
+		__raw_writel(__raw_readl(CSI_FLASH_STROBE_2) | 0x1,
+			     CSI_FLASH_STROBE_2);
+	}
+}
+
+/*!
+ * ipu_csi_enable_mclk
+ *
+ * @param       src         enum define which source to control the clk
+ *                          CSI_MCLK_VF CSI_MCLK_ENC CSI_MCLK_RAW CSI_MCLK_I2C
+ * @param       flag        true to enable mclk, false to disable mclk
+ * @param       wait        true to wait 100ms make clock stable, false not wait
+ *
+ * @return      0 for success
+ */
+int32_t ipu_csi_enable_mclk(int src, bool flag, bool wait)
+{
+	if (flag == true) {
+		csi_mclk_flag |= src;
+	} else {
+		csi_mclk_flag &= ~src;
+	}
+
+	if (gipu_csi_get_mclk_flag == flag)
+		return 0;
+
+	if (flag == true) {
+		clk_enable(g_ipu_csi_clk);
+		if (wait == true)
+			msleep(10);
+		/*printk("enable csi clock from source %d\n", src);     */
+		gipu_csi_get_mclk_flag = true;
+	} else if (csi_mclk_flag == 0) {
+		clk_disable(g_ipu_csi_clk);
+		/*printk("disable csi clock from source %d\n", src); */
+		gipu_csi_get_mclk_flag = flag;
+	}
+
+	return 0;
+}
+
+/*!
+ * ipu_csi_read_mclk_flag
+ *
+ * @return  csi_mclk_flag
+ */
+int ipu_csi_read_mclk_flag(void)
+{
+	return csi_mclk_flag;
+}
+
+/*!
+ * ipu_csi_get_window_size
+ *
+ * @param       width        pointer to window width
+ * @param       height       pointer to window height
+ * @param       dummy        dummy for IPUv1 to keep the same interface with IPUv3
+ *
+ */
+void ipu_csi_get_window_size(uint32_t *width, uint32_t *height,
+		uint32_t dummy)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(CSI_ACT_FRM_SIZE);
+	*width = (reg & 0xFFFF) + 1;
+	*height = (reg >> 16 & 0xFFFF) + 1;
+}
+
+/*!
+ * ipu_csi_set_window_size
+ *
+ * @param       width        window width
+ * @param       height       window height
+ * @param       dummy        dummy for IPUv1 to keep the same interface with IPUv3
+ *
+ */
+void ipu_csi_set_window_size(uint32_t width, uint32_t height, uint32_t dummy)
+{
+	__raw_writel((width - 1) | (height - 1) << 16, CSI_ACT_FRM_SIZE);
+}
+
+/*!
+ * ipu_csi_set_window_pos
+ *
+ * @param       left        uint32 window x start
+ * @param       top         uint32 window y start
+ * @param       dummy       dummy for IPUv1 to keep the same interface with IPUv3
+ *
+ */
+void ipu_csi_set_window_pos(uint32_t left, uint32_t top, uint32_t dummy)
+{
+	uint32_t temp = __raw_readl(CSI_OUT_FRM_CTRL);
+	temp &= 0xffff0000;
+	temp = top | (left << 8) | temp;
+	__raw_writel(temp, CSI_OUT_FRM_CTRL);
+}
+
+/* Exported symbols for modules. */
+EXPORT_SYMBOL(ipu_csi_set_window_pos);
+EXPORT_SYMBOL(ipu_csi_set_window_size);
+EXPORT_SYMBOL(ipu_csi_get_window_size);
+EXPORT_SYMBOL(ipu_csi_read_mclk_flag);
+EXPORT_SYMBOL(ipu_csi_enable_mclk);
+EXPORT_SYMBOL(ipu_csi_flash_strobe);
+EXPORT_SYMBOL(ipu_csi_init_interface);
diff --git a/drivers/mxc/ipu/ipu_device.c b/drivers/mxc/ipu/ipu_device.c
new file mode 100644
index 000000000000..5fd1c51ec9b1
--- /dev/null
+++ b/drivers/mxc/ipu/ipu_device.c
@@ -0,0 +1,696 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_device.c
+ *
+ * @brief This file contains the IPU driver device interface and fops functions.
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/wait.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+#include "ipu_param_mem.h"
+
+/* Strucutures and variables for exporting MXC IPU as device*/
+
+#define MAX_Q_SIZE 10
+
+static int mxc_ipu_major;
+static struct class *mxc_ipu_class;
+
+DEFINE_SPINLOCK(queue_lock);
+static DECLARE_MUTEX(user_mutex);
+
+static wait_queue_head_t waitq;
+static int pending_events = 0;
+int read_ptr = 0;
+int write_ptr = 0;
+
+ipu_event_info events[MAX_Q_SIZE];
+
+int register_ipu_device(void);
+
+/* Static functions */
+
+int get_events(ipu_event_info *p)
+{
+	unsigned long flags;
+	int ret = 0, i, cnt, found = 0;
+	spin_lock_irqsave(&queue_lock, flags);
+	if (pending_events != 0) {
+		if (write_ptr > read_ptr)
+			cnt = write_ptr - read_ptr;
+		else
+			cnt = MAX_Q_SIZE - read_ptr + write_ptr;
+		for (i = 0; i < cnt; i++) {
+			if (p->irq == events[read_ptr].irq) {
+				*p = events[read_ptr];
+				events[read_ptr].irq = 0;
+				read_ptr++;
+				if (read_ptr >= MAX_Q_SIZE)
+					read_ptr = 0;
+				found = 1;
+				break;
+			}
+
+			if (events[read_ptr].irq) {
+				events[write_ptr] = events[read_ptr];
+				events[read_ptr].irq = 0;
+				write_ptr++;
+				if (write_ptr >= MAX_Q_SIZE)
+					write_ptr = 0;
+			} else
+				pending_events--;
+
+			read_ptr++;
+			if (read_ptr >= MAX_Q_SIZE)
+				read_ptr = 0;
+		}
+		if (found)
+			pending_events--;
+		else
+			ret = -1;
+	} else {
+		ret = -1;
+	}
+
+	spin_unlock_irqrestore(&queue_lock, flags);
+	return ret;
+}
+
+static irqreturn_t mxc_ipu_generic_handler(int irq, void *dev_id)
+{
+	ipu_event_info e;
+
+	e.irq = irq;
+	e.dev = dev_id;
+	events[write_ptr] = e;
+	write_ptr++;
+	if (write_ptr >= MAX_Q_SIZE)
+		write_ptr = 0;
+	pending_events++;
+	/* Wakeup any blocking user context */
+	wake_up_interruptible(&waitq);
+	return IRQ_HANDLED;
+}
+
+static int mxc_ipu_open(struct inode *inode, struct file *file)
+{
+	int ret = 0;
+	return ret;
+}
+static int mxc_ipu_ioctl(struct inode *inode, struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	int ret = 0;
+
+	switch (cmd) {
+
+	case IPU_INIT_CHANNEL:
+		{
+			ipu_channel_parm parm;
+			if (copy_from_user
+			    (&parm, (ipu_channel_parm *) arg,
+			     sizeof(ipu_channel_parm))) {
+				return -EFAULT;
+			}
+			if (!parm.flag) {
+				ret =
+				    ipu_init_channel(parm.channel,
+						     &parm.params);
+			} else {
+				ret = ipu_init_channel(parm.channel, NULL);
+			}
+		}
+		break;
+
+	case IPU_UNINIT_CHANNEL:
+		{
+			ipu_channel_t ch;
+			int __user *argp = (void __user *)arg;
+			if (get_user(ch, argp))
+				return -EFAULT;
+			ipu_uninit_channel(ch);
+		}
+		break;
+
+	case IPU_INIT_CHANNEL_BUFFER:
+		{
+			ipu_channel_buf_parm parm;
+			if (copy_from_user
+			    (&parm, (ipu_channel_buf_parm *) arg,
+			     sizeof(ipu_channel_buf_parm))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_init_channel_buffer(parm.channel, parm.type,
+						    parm.pixel_fmt,
+						    parm.width, parm.height,
+						    parm.stride,
+						    parm.rot_mode,
+						    parm.phyaddr_0,
+						    parm.phyaddr_1,
+						    parm.u_offset,
+						    parm.v_offset);
+
+		}
+		break;
+
+	case IPU_UPDATE_CHANNEL_BUFFER:
+		{
+			ipu_channel_buf_parm parm;
+			if (copy_from_user
+			    (&parm, (ipu_channel_buf_parm *) arg,
+			     sizeof(ipu_channel_buf_parm))) {
+				return -EFAULT;
+			}
+			if ((parm.phyaddr_0 != (dma_addr_t) NULL)
+			    && (parm.phyaddr_1 == (dma_addr_t) NULL)) {
+				ret =
+				    ipu_update_channel_buffer(parm.channel,
+							      parm.type,
+							      parm.bufNum,
+							      parm.phyaddr_0);
+			} else if ((parm.phyaddr_0 == (dma_addr_t) NULL)
+				   && (parm.phyaddr_1 != (dma_addr_t) NULL)) {
+				ret =
+				    ipu_update_channel_buffer(parm.channel,
+							      parm.type,
+							      parm.bufNum,
+							      parm.phyaddr_1);
+			} else {
+				ret = -1;
+			}
+
+		}
+		break;
+	case IPU_SELECT_CHANNEL_BUFFER:
+		{
+			ipu_channel_buf_parm parm;
+			if (copy_from_user
+			    (&parm, (ipu_channel_buf_parm *) arg,
+			     sizeof(ipu_channel_buf_parm))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_select_buffer(parm.channel, parm.type,
+					      parm.bufNum);
+
+		}
+		break;
+	case IPU_LINK_CHANNELS:
+		{
+			ipu_channel_link link;
+			if (copy_from_user
+			    (&link, (ipu_channel_link *) arg,
+			     sizeof(ipu_channel_link))) {
+				return -EFAULT;
+			}
+			ret = ipu_link_channels(link.src_ch, link.dest_ch);
+
+		}
+		break;
+	case IPU_UNLINK_CHANNELS:
+		{
+			ipu_channel_link link;
+			if (copy_from_user
+			    (&link, (ipu_channel_link *) arg,
+			     sizeof(ipu_channel_link))) {
+				return -EFAULT;
+			}
+			ret = ipu_unlink_channels(link.src_ch, link.dest_ch);
+
+		}
+		break;
+	case IPU_ENABLE_CHANNEL:
+		{
+			ipu_channel_t ch;
+			int __user *argp = (void __user *)arg;
+			if (get_user(ch, argp))
+				return -EFAULT;
+			ipu_enable_channel(ch);
+		}
+		break;
+	case IPU_DISABLE_CHANNEL:
+		{
+			ipu_channel_info info;
+			if (copy_from_user
+			    (&info, (ipu_channel_info *) arg,
+			     sizeof(ipu_channel_info))) {
+				return -EFAULT;
+			}
+			ret = ipu_disable_channel(info.channel, info.stop);
+		}
+		break;
+	case IPU_ENABLE_IRQ:
+		{
+			uint32_t irq;
+			int __user *argp = (void __user *)arg;
+			if (get_user(irq, argp))
+				return -EFAULT;
+			ipu_enable_irq(irq);
+		}
+		break;
+	case IPU_DISABLE_IRQ:
+		{
+			uint32_t irq;
+			int __user *argp = (void __user *)arg;
+			if (get_user(irq, argp))
+				return -EFAULT;
+			ipu_disable_irq(irq);
+		}
+		break;
+	case IPU_CLEAR_IRQ:
+		{
+			uint32_t irq;
+			int __user *argp = (void __user *)arg;
+			if (get_user(irq, argp))
+				return -EFAULT;
+			ipu_clear_irq(irq);
+		}
+		break;
+	case IPU_FREE_IRQ:
+		{
+			ipu_irq_info info;
+			if (copy_from_user
+			    (&info, (ipu_irq_info *) arg,
+			     sizeof(ipu_irq_info))) {
+				return -EFAULT;
+			}
+			ipu_free_irq(info.irq, info.dev_id);
+		}
+		break;
+	case IPU_REQUEST_IRQ_STATUS:
+		{
+			uint32_t irq;
+			int __user *argp = (void __user *)arg;
+			if (get_user(irq, argp))
+				return -EFAULT;
+			ret = ipu_get_irq_status(irq);
+		}
+		break;
+	case IPU_SDC_INIT_PANEL:
+		{
+			ipu_sdc_panel_info sinfo;
+			if (copy_from_user
+			    (&sinfo, (ipu_sdc_panel_info *) arg,
+			     sizeof(ipu_sdc_panel_info))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_sdc_init_panel(sinfo.panel, sinfo.pixel_clk,
+					       sinfo.width, sinfo.height,
+					       sinfo.pixel_fmt,
+					       sinfo.hStartWidth,
+					       sinfo.hSyncWidth,
+					       sinfo.hEndWidth,
+					       sinfo.vStartWidth,
+					       sinfo.vSyncWidth,
+					       sinfo.vEndWidth, sinfo.signal);
+		}
+		break;
+	case IPU_SDC_SET_WIN_POS:
+		{
+			ipu_sdc_window_pos pos;
+			if (copy_from_user
+			    (&pos, (ipu_sdc_window_pos *) arg,
+			     sizeof(ipu_sdc_window_pos))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_disp_set_window_pos(pos.channel, pos.x_pos,
+						   pos.y_pos);
+
+		}
+		break;
+	case IPU_SDC_SET_GLOBAL_ALPHA:
+		{
+			ipu_sdc_global_alpha g;
+			if (copy_from_user
+			    (&g, (ipu_sdc_global_alpha *) arg,
+			     sizeof(ipu_sdc_global_alpha))) {
+				return -EFAULT;
+			}
+			ret = ipu_sdc_set_global_alpha(g.enable, g.alpha);
+		}
+		break;
+	case IPU_SDC_SET_COLOR_KEY:
+		{
+			ipu_sdc_color_key c;
+			if (copy_from_user
+			    (&c, (ipu_sdc_color_key *) arg,
+			     sizeof(ipu_sdc_color_key))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_sdc_set_color_key(c.channel, c.enable,
+						  c.colorKey);
+		}
+		break;
+	case IPU_SDC_SET_BRIGHTNESS:
+		{
+			uint8_t b;
+			int __user *argp = (void __user *)arg;
+			if (get_user(b, argp))
+				return -EFAULT;
+			ret = ipu_sdc_set_brightness(b);
+
+		}
+		break;
+	case IPU_REGISTER_GENERIC_ISR:
+		{
+			ipu_event_info info;
+			if (copy_from_user
+			    (&info, (ipu_event_info *) arg,
+			     sizeof(ipu_event_info))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_request_irq(info.irq, mxc_ipu_generic_handler,
+					    0, "video_sink", info.dev);
+		}
+		break;
+	case IPU_GET_EVENT:
+		/* User will have to allocate event_type structure and pass the pointer in arg */
+		{
+			ipu_event_info info;
+			int r = -1;
+
+			if (copy_from_user
+					(&info, (ipu_event_info *) arg,
+					 sizeof(ipu_event_info)))
+				return -EFAULT;
+
+			r = get_events(&info);
+			if (r == -1) {
+				wait_event_interruptible_timeout(waitq,
+						(pending_events != 0), 2 * HZ);
+				r = get_events(&info);
+			}
+			ret = -1;
+			if (r == 0) {
+				if (!copy_to_user((ipu_event_info *) arg,
+					&info, sizeof(ipu_event_info)))
+					ret = 0;
+			}
+		}
+		break;
+	case IPU_ADC_WRITE_TEMPLATE:
+		{
+			ipu_adc_template temp;
+			if (copy_from_user
+			    (&temp, (ipu_adc_template *) arg, sizeof(temp))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_adc_write_template(temp.disp, temp.pCmd,
+						   temp.write);
+		}
+		break;
+	case IPU_ADC_UPDATE:
+		{
+			ipu_adc_update update;
+			if (copy_from_user
+			    (&update, (ipu_adc_update *) arg, sizeof(update))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_adc_set_update_mode(update.channel, update.mode,
+						    update.refresh_rate,
+						    update.addr, update.size);
+		}
+		break;
+	case IPU_ADC_SNOOP:
+		{
+			ipu_adc_snoop snoop;
+			if (copy_from_user
+			    (&snoop, (ipu_adc_snoop *) arg, sizeof(snoop))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_adc_get_snooping_status(snoop.statl,
+							snoop.stath);
+		}
+		break;
+	case IPU_ADC_CMD:
+		{
+			ipu_adc_cmd cmd;
+			if (copy_from_user
+			    (&cmd, (ipu_adc_cmd *) arg, sizeof(cmd))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_adc_write_cmd(cmd.disp, cmd.type, cmd.cmd,
+					      cmd.params, cmd.numParams);
+		}
+		break;
+	case IPU_ADC_INIT_PANEL:
+		{
+			ipu_adc_panel panel;
+			if (copy_from_user
+			    (&panel, (ipu_adc_panel *) arg, sizeof(panel))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_adc_init_panel(panel.disp, panel.width,
+					       panel.height, panel.pixel_fmt,
+					       panel.stride, panel.signal,
+					       panel.addr, panel.vsync_width,
+					       panel.mode);
+		}
+		break;
+	case IPU_ADC_IFC_TIMING:
+		{
+			ipu_adc_ifc_timing t;
+			if (copy_from_user
+			    (&t, (ipu_adc_ifc_timing *) arg, sizeof(t))) {
+				return -EFAULT;
+			}
+			ret =
+			    ipu_adc_init_ifc_timing(t.disp, t.read,
+						    t.cycle_time, t.up_time,
+						    t.down_time,
+						    t.read_latch_time,
+						    t.pixel_clk);
+		}
+		break;
+	case IPU_CSI_INIT_INTERFACE:
+		{
+			ipu_csi_interface c;
+			if (copy_from_user
+			    (&c, (ipu_csi_interface *) arg, sizeof(c)))
+				return -EFAULT;
+			ret =
+			    ipu_csi_init_interface(c.width, c.height,
+						   c.pixel_fmt, c.signal);
+		}
+		break;
+	case IPU_CSI_ENABLE_MCLK:
+		{
+			ipu_csi_mclk m;
+			if (copy_from_user(&m, (ipu_csi_mclk *) arg, sizeof(m)))
+				return -EFAULT;
+			ret = ipu_csi_enable_mclk(m.src, m.flag, m.wait);
+		}
+		break;
+	case IPU_CSI_READ_MCLK_FLAG:
+		{
+			ret = ipu_csi_read_mclk_flag();
+		}
+		break;
+	case IPU_CSI_FLASH_STROBE:
+		{
+			bool strobe;
+			int __user *argp = (void __user *)arg;
+			if (get_user(strobe, argp))
+				return -EFAULT;
+			ipu_csi_flash_strobe(strobe);
+		}
+		break;
+	case IPU_CSI_GET_WIN_SIZE:
+		{
+			ipu_csi_window_size w;
+			int dummy = 0;
+			ipu_csi_get_window_size(&w.width, &w.height, dummy);
+			if (copy_to_user
+			    ((ipu_csi_window_size *) arg, &w, sizeof(w)))
+				return -EFAULT;
+		}
+		break;
+	case IPU_CSI_SET_WIN_SIZE:
+		{
+			ipu_csi_window_size w;
+			int dummy = 0;
+			if (copy_from_user
+			    (&w, (ipu_csi_window_size *) arg, sizeof(w)))
+				return -EFAULT;
+			ipu_csi_set_window_size(w.width, w.height, dummy);
+		}
+		break;
+	case IPU_CSI_SET_WINDOW:
+		{
+			ipu_csi_window p;
+			int dummy = 0;
+			if (copy_from_user
+			    (&p, (ipu_csi_window *) arg, sizeof(p)))
+				return -EFAULT;
+			ipu_csi_set_window_pos(p.left, p.top, dummy);
+		}
+		break;
+	case IPU_PF_SET_PAUSE_ROW:
+		{
+			uint32_t p;
+			int __user *argp = (void __user *)arg;
+			if (get_user(p, argp))
+				return -EFAULT;
+			ret = ipu_pf_set_pause_row(p);
+		}
+		break;
+	case IPU_ALOC_MEM:
+		{
+			ipu_mem_info info;
+			if (copy_from_user
+					(&info, (ipu_mem_info *) arg,
+					 sizeof(ipu_mem_info)))
+				return -EFAULT;
+
+			info.vaddr = dma_alloc_coherent(0,
+					PAGE_ALIGN(info.size),
+					&info.paddr,
+					GFP_DMA | GFP_KERNEL);
+			if (info.vaddr == 0) {
+				printk(KERN_ERR "dma alloc failed!\n");
+				return -ENOBUFS;
+			}
+			if (copy_to_user((ipu_mem_info *) arg, &info,
+					sizeof(ipu_mem_info)) > 0)
+				return -EFAULT;
+		}
+		break;
+	case IPU_FREE_MEM:
+		{
+			ipu_mem_info info;
+			if (copy_from_user
+					(&info, (ipu_mem_info *) arg,
+					 sizeof(ipu_mem_info)))
+				return -EFAULT;
+
+			if (info.vaddr != 0)
+				dma_free_coherent(0, PAGE_ALIGN(info.size),
+						info.vaddr, info.paddr);
+			else
+				return -EFAULT;
+		}
+		break;
+	case IPU_IS_CHAN_BUSY:
+		{
+			ipu_channel_t chan;
+			if (copy_from_user
+					(&chan, (ipu_channel_t *)arg,
+					 sizeof(ipu_channel_t)))
+				return -EFAULT;
+
+			if (ipu_is_channel_busy(chan))
+				ret = 1;
+			else
+				ret = 0;
+		}
+		break;
+	default:
+		break;
+
+	}
+	return ret;
+}
+
+static int mxc_ipu_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	vma->vm_page_prot = pgprot_writethru(vma->vm_page_prot);
+
+	if (remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
+				vma->vm_end - vma->vm_start,
+				vma->vm_page_prot)) {
+		printk(KERN_ERR
+				"mmap failed!\n");
+		return -ENOBUFS;
+	}
+	return 0;
+}
+
+static int mxc_ipu_release(struct inode *inode, struct file *file)
+{
+	return 0;
+}
+
+static struct file_operations mxc_ipu_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_ipu_open,
+	.mmap = mxc_ipu_mmap,
+	.release = mxc_ipu_release,
+	.ioctl = mxc_ipu_ioctl
+};
+
+int register_ipu_device()
+{
+	int ret = 0;
+	struct device *temp;
+	mxc_ipu_major = register_chrdev(0, "mxc_ipu", &mxc_ipu_fops);
+	if (mxc_ipu_major < 0) {
+		printk(KERN_ERR
+		       "Unable to register Mxc Ipu as a char device\n");
+		return mxc_ipu_major;
+	}
+
+	mxc_ipu_class = class_create(THIS_MODULE, "mxc_ipu");
+	if (IS_ERR(mxc_ipu_class)) {
+		printk(KERN_ERR "Unable to create class for Mxc Ipu\n");
+		ret = PTR_ERR(mxc_ipu_class);
+		goto err1;
+	}
+
+	temp = device_create(mxc_ipu_class, NULL, MKDEV(mxc_ipu_major, 0), NULL,
+			     "mxc_ipu");
+
+	if (IS_ERR(temp)) {
+		printk(KERN_ERR "Unable to create class device for Mxc Ipu\n");
+		ret = PTR_ERR(temp);
+		goto err2;
+	}
+	spin_lock_init(&queue_lock);
+	init_waitqueue_head(&waitq);
+	return ret;
+
+      err2:
+	class_destroy(mxc_ipu_class);
+      err1:
+	unregister_chrdev(mxc_ipu_major, "mxc_ipu");
+	return ret;
+
+}
diff --git a/drivers/mxc/ipu/ipu_ic.c b/drivers/mxc/ipu/ipu_ic.c
new file mode 100644
index 000000000000..cdf823a2760b
--- /dev/null
+++ b/drivers/mxc/ipu/ipu_ic.c
@@ -0,0 +1,592 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * @file ipu_ic.c
+ *
+ * @brief IPU IC functions
+ *
+ * @ingroup IPU
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+#include "ipu_param_mem.h"
+
+enum {
+	IC_TASK_VIEWFINDER,
+	IC_TASK_ENCODER,
+	IC_TASK_POST_PROCESSOR
+};
+
+extern int g_ipu_hw_rev;
+static void _init_csc(uint8_t ic_task, ipu_color_space_t in_format,
+		      ipu_color_space_t out_format);
+static bool _calc_resize_coeffs(uint32_t inSize, uint32_t outSize,
+				uint32_t * resizeCoeff,
+				uint32_t * downsizeCoeff);
+
+void _ipu_ic_enable_task(ipu_channel_t channel)
+{
+	uint32_t ic_conf;
+
+	ic_conf = __raw_readl(IC_CONF);
+	switch (channel) {
+	case CSI_PRP_VF_ADC:
+	case MEM_PRP_VF_ADC:
+	case CSI_PRP_VF_MEM:
+	case MEM_PRP_VF_MEM:
+		ic_conf |= IC_CONF_PRPVF_EN;
+		break;
+	case MEM_ROT_VF_MEM:
+		ic_conf |= IC_CONF_PRPVF_ROT_EN;
+		break;
+	case CSI_PRP_ENC_MEM:
+	case MEM_PRP_ENC_MEM:
+		ic_conf |= IC_CONF_PRPENC_EN;
+		break;
+	case MEM_ROT_ENC_MEM:
+		ic_conf |= IC_CONF_PRPENC_ROT_EN;
+		break;
+	case MEM_PP_ADC:
+	case MEM_PP_MEM:
+		ic_conf |= IC_CONF_PP_EN;
+		break;
+	case MEM_ROT_PP_MEM:
+		ic_conf |= IC_CONF_PP_ROT_EN;
+		break;
+	case CSI_MEM:
+		// ???
+		ic_conf |= IC_CONF_RWS_EN | IC_CONF_PRPENC_EN;
+		break;
+	default:
+		break;
+	}
+	__raw_writel(ic_conf, IC_CONF);
+}
+
+void _ipu_ic_disable_task(ipu_channel_t channel)
+{
+	uint32_t ic_conf;
+
+	ic_conf = __raw_readl(IC_CONF);
+	switch (channel) {
+	case CSI_PRP_VF_ADC:
+	case MEM_PRP_VF_ADC:
+	case CSI_PRP_VF_MEM:
+	case MEM_PRP_VF_MEM:
+		ic_conf &= ~IC_CONF_PRPVF_EN;
+		break;
+	case MEM_ROT_VF_MEM:
+		ic_conf &= ~IC_CONF_PRPVF_ROT_EN;
+		break;
+	case CSI_PRP_ENC_MEM:
+	case MEM_PRP_ENC_MEM:
+		ic_conf &= ~IC_CONF_PRPENC_EN;
+		break;
+	case MEM_ROT_ENC_MEM:
+		ic_conf &= ~IC_CONF_PRPENC_ROT_EN;
+		break;
+	case MEM_PP_ADC:
+	case MEM_PP_MEM:
+		ic_conf &= ~IC_CONF_PP_EN;
+		break;
+	case MEM_ROT_PP_MEM:
+		ic_conf &= ~IC_CONF_PP_ROT_EN;
+		break;
+	case CSI_MEM:
+		// ???
+		ic_conf &= ~(IC_CONF_RWS_EN | IC_CONF_PRPENC_EN);
+		break;
+	default:
+		break;
+	}
+	__raw_writel(ic_conf, IC_CONF);
+}
+
+void _ipu_ic_init_prpvf(ipu_channel_params_t * params, bool src_is_csi)
+{
+	uint32_t reg, ic_conf;
+	uint32_t downsizeCoeff, resizeCoeff;
+	ipu_color_space_t in_fmt, out_fmt;
+
+	/* Setup vertical resizing */
+	_calc_resize_coeffs(params->mem_prp_vf_mem.in_height,
+			    params->mem_prp_vf_mem.out_height,
+			    &resizeCoeff, &downsizeCoeff);
+	reg = (downsizeCoeff << 30) | (resizeCoeff << 16);
+
+	/* Setup horizontal resizing */
+	_calc_resize_coeffs(params->mem_prp_vf_mem.in_width,
+			    params->mem_prp_vf_mem.out_width,
+			    &resizeCoeff, &downsizeCoeff);
+	reg |= (downsizeCoeff << 14) | resizeCoeff;
+
+	__raw_writel(reg, IC_PRP_VF_RSC);
+
+	ic_conf = __raw_readl(IC_CONF);
+
+	/* Setup color space conversion */
+	in_fmt = format_to_colorspace(params->mem_prp_vf_mem.in_pixel_fmt);
+	out_fmt = format_to_colorspace(params->mem_prp_vf_mem.out_pixel_fmt);
+	if (in_fmt == RGB) {
+		if ((out_fmt == YCbCr) || (out_fmt == YUV)) {
+			_init_csc(IC_TASK_VIEWFINDER, RGB, out_fmt);
+			ic_conf |= IC_CONF_PRPVF_CSC1;	/* Enable RGB->YCBCR CSC */
+		}
+	}
+	if ((in_fmt == YCbCr) || (in_fmt == YUV)) {
+		if (out_fmt == RGB) {
+			_init_csc(IC_TASK_VIEWFINDER, YCbCr, RGB);
+			ic_conf |= IC_CONF_PRPVF_CSC1;	/* Enable YCBCR->RGB CSC */
+		} else {
+			/* TODO: Support YUV<->YCbCr conversion? */
+		}
+	}
+
+	if (params->mem_prp_vf_mem.graphics_combine_en) {
+		ic_conf |= IC_CONF_PRPVF_CMB;
+
+		/* need transparent CSC1 conversion */
+		_init_csc(IC_TASK_POST_PROCESSOR, RGB, RGB);
+		ic_conf |= IC_CONF_PRPVF_CSC1;	/* Enable RGB->RGB CSC */
+
+		if (params->mem_prp_vf_mem.global_alpha_en) {
+			ic_conf |= IC_CONF_IC_GLB_LOC_A;
+		} else {
+			ic_conf &= ~IC_CONF_IC_GLB_LOC_A;
+		}
+
+		if (params->mem_prp_vf_mem.key_color_en) {
+			ic_conf |= IC_CONF_KEY_COLOR_EN;
+		} else {
+			ic_conf &= ~IC_CONF_KEY_COLOR_EN;
+		}
+	} else {
+		ic_conf &= ~IC_CONF_PP_CMB;
+	}
+
+#ifndef CONFIG_VIRTIO_SUPPORT	/* Setting RWS_EN doesn't work in Virtio */
+	if (src_is_csi) {
+		ic_conf &= ~IC_CONF_RWS_EN;
+	} else {
+		ic_conf |= IC_CONF_RWS_EN;
+	}
+#endif
+	__raw_writel(ic_conf, IC_CONF);
+}
+
+void _ipu_ic_uninit_prpvf(void)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(IC_CONF);
+	reg &= ~(IC_CONF_PRPVF_EN | IC_CONF_PRPVF_CMB |
+		 IC_CONF_PRPVF_CSC2 | IC_CONF_PRPVF_CSC1);
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_rotate_vf(ipu_channel_params_t * params)
+{
+}
+
+void _ipu_ic_uninit_rotate_vf(void)
+{
+	uint32_t reg;
+	reg = __raw_readl(IC_CONF);
+	reg &= ~IC_CONF_PRPVF_ROT_EN;
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_csi(ipu_channel_params_t * params)
+{
+	uint32_t reg;
+	reg = __raw_readl(IC_CONF);
+	reg &= ~IC_CONF_CSI_MEM_WR_EN;
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_uninit_csi(void)
+{
+	uint32_t reg;
+	reg = __raw_readl(IC_CONF);
+	reg &= ~(IC_CONF_RWS_EN | IC_CONF_PRPENC_EN);
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_prpenc(ipu_channel_params_t * params, bool src_is_csi)
+{
+	uint32_t reg, ic_conf;
+	uint32_t downsizeCoeff, resizeCoeff;
+	ipu_color_space_t in_fmt, out_fmt;
+
+	/* Setup vertical resizing */
+	_calc_resize_coeffs(params->mem_prp_enc_mem.in_height,
+			    params->mem_prp_enc_mem.out_height,
+			    &resizeCoeff, &downsizeCoeff);
+	reg = (downsizeCoeff << 30) | (resizeCoeff << 16);
+
+	/* Setup horizontal resizing */
+	_calc_resize_coeffs(params->mem_prp_enc_mem.in_width,
+			    params->mem_prp_enc_mem.out_width,
+			    &resizeCoeff, &downsizeCoeff);
+	reg |= (downsizeCoeff << 14) | resizeCoeff;
+
+	__raw_writel(reg, IC_PRP_ENC_RSC);
+
+	ic_conf = __raw_readl(IC_CONF);
+
+	/* Setup color space conversion */
+	in_fmt = format_to_colorspace(params->mem_prp_enc_mem.in_pixel_fmt);
+	out_fmt = format_to_colorspace(params->mem_prp_enc_mem.out_pixel_fmt);
+	if (in_fmt == RGB) {
+		if ((out_fmt == YCbCr) || (out_fmt == YUV)) {
+			/* TODO: ERROR! */
+		}
+	}
+	if ((in_fmt == YCbCr) || (in_fmt == YUV)) {
+		if (out_fmt == RGB) {
+			_init_csc(IC_TASK_ENCODER, YCbCr, RGB);
+			ic_conf |= IC_CONF_PRPENC_CSC1;	/* Enable YCBCR->RGB CSC */
+		} else {
+			/* TODO: Support YUV<->YCbCr conversion? */
+		}
+	}
+
+	if (src_is_csi) {
+		ic_conf &= ~IC_CONF_RWS_EN;
+	} else {
+		ic_conf |= IC_CONF_RWS_EN;
+	}
+
+	__raw_writel(ic_conf, IC_CONF);
+}
+
+void _ipu_ic_uninit_prpenc(void)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(IC_CONF);
+	reg &= ~(IC_CONF_PRPENC_EN | IC_CONF_PRPENC_CSC1);
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_rotate_enc(ipu_channel_params_t * params)
+{
+}
+
+void _ipu_ic_uninit_rotate_enc(void)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(IC_CONF);
+	reg &= ~(IC_CONF_PRPENC_ROT_EN);
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_pp(ipu_channel_params_t * params)
+{
+	uint32_t reg, ic_conf;
+	uint32_t downsizeCoeff, resizeCoeff;
+	ipu_color_space_t in_fmt, out_fmt;
+
+	/* Setup vertical resizing */
+	_calc_resize_coeffs(params->mem_pp_mem.in_height,
+			    params->mem_pp_mem.out_height,
+			    &resizeCoeff, &downsizeCoeff);
+	reg = (downsizeCoeff << 30) | (resizeCoeff << 16);
+
+	/* Setup horizontal resizing */
+	_calc_resize_coeffs(params->mem_pp_mem.in_width,
+			    params->mem_pp_mem.out_width,
+			    &resizeCoeff, &downsizeCoeff);
+	reg |= (downsizeCoeff << 14) | resizeCoeff;
+
+	__raw_writel(reg, IC_PP_RSC);
+
+	ic_conf = __raw_readl(IC_CONF);
+
+	/* Setup color space conversion */
+	in_fmt = format_to_colorspace(params->mem_pp_mem.in_pixel_fmt);
+	out_fmt = format_to_colorspace(params->mem_pp_mem.out_pixel_fmt);
+	if (in_fmt == RGB) {
+		if ((out_fmt == YCbCr) || (out_fmt == YUV)) {
+			_init_csc(IC_TASK_POST_PROCESSOR, RGB, out_fmt);
+			ic_conf |= IC_CONF_PP_CSC2;	/* Enable RGB->YCBCR CSC */
+		}
+	}
+	if ((in_fmt == YCbCr) || (in_fmt == YUV)) {
+		if (out_fmt == RGB) {
+			_init_csc(IC_TASK_POST_PROCESSOR, YCbCr, RGB);
+			ic_conf |= IC_CONF_PP_CSC1;	/* Enable YCBCR->RGB CSC */
+		} else {
+			/* TODO: Support YUV<->YCbCr conversion? */
+		}
+	}
+
+	if (params->mem_pp_mem.graphics_combine_en) {
+		ic_conf |= IC_CONF_PP_CMB;
+
+		/* need transparent CSC1 conversion */
+		_init_csc(IC_TASK_POST_PROCESSOR, RGB, RGB);
+		ic_conf |= IC_CONF_PP_CSC1;	/* Enable RGB->RGB CSC */
+
+		if (params->mem_pp_mem.global_alpha_en) {
+			ic_conf |= IC_CONF_IC_GLB_LOC_A;
+		} else {
+			ic_conf &= ~IC_CONF_IC_GLB_LOC_A;
+		}
+
+		if (params->mem_pp_mem.key_color_en) {
+			ic_conf |= IC_CONF_KEY_COLOR_EN;
+		} else {
+			ic_conf &= ~IC_CONF_KEY_COLOR_EN;
+		}
+	} else {
+		ic_conf &= ~IC_CONF_PP_CMB;
+	}
+
+	__raw_writel(ic_conf, IC_CONF);
+}
+
+void _ipu_ic_uninit_pp(void)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(IC_CONF);
+	reg &= ~(IC_CONF_PP_EN | IC_CONF_PP_CSC1 | IC_CONF_PP_CSC2 |
+		 IC_CONF_PP_CMB);
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_rotate_pp(ipu_channel_params_t * params)
+{
+}
+
+void _ipu_ic_uninit_rotate_pp(void)
+{
+	uint32_t reg;
+	reg = __raw_readl(IC_CONF);
+	reg &= ~IC_CONF_PP_ROT_EN;
+	__raw_writel(reg, IC_CONF);
+}
+
+static void _init_csc(uint8_t ic_task, ipu_color_space_t in_format,
+		      ipu_color_space_t out_format)
+{
+/*     Y = R *  .299 + G *  .587 + B *  .114;
+       U = R * -.169 + G * -.332 + B *  .500 + 128.;
+       V = R *  .500 + G * -.419 + B * -.0813 + 128.;*/
+	static const uint32_t rgb2ycbcr_coeff[4][3] = {
+		{0x004D, 0x0096, 0x001D},
+		{0x01D5, 0x01AB, 0x0080},
+		{0x0080, 0x0195, 0x01EB},
+		{0x0000, 0x0200, 0x0200},	/* A0, A1, A2 */
+	};
+
+	/* transparent RGB->RGB matrix for combining
+	 */
+	static const uint32_t rgb2rgb_coeff[4][3] = {
+		{0x0080, 0x0000, 0x0000},
+		{0x0000, 0x0080, 0x0000},
+		{0x0000, 0x0000, 0x0080},
+		{0x0000, 0x0000, 0x0000},	/* A0, A1, A2 */
+	};
+
+/*     R = (1.164 * (Y - 16)) + (1.596 * (Cr - 128));
+       G = (1.164 * (Y - 16)) - (0.392 * (Cb - 128)) - (0.813 * (Cr - 128));
+       B = (1.164 * (Y - 16)) + (2.017 * (Cb - 128); */
+	static const uint32_t ycbcr2rgb_coeff[4][3] = {
+		{149, 0, 204},
+		{149, 462, 408},
+		{149, 255, 0},
+		{8192 - 446, 266, 8192 - 554},	/* A0, A1, A2 */
+	};
+
+	uint32_t param[2];
+	uint32_t address = 0;
+
+	if (g_ipu_hw_rev > 1) {
+		if (ic_task == IC_TASK_VIEWFINDER) {
+			address = 0x645 << 3;
+		} else if (ic_task == IC_TASK_ENCODER) {
+			address = 0x321 << 3;
+		} else if (ic_task == IC_TASK_POST_PROCESSOR) {
+			address = 0x96C << 3;
+		} else {
+			BUG();
+		}
+	} else {
+		if (ic_task == IC_TASK_VIEWFINDER) {
+			address = 0x5a5 << 3;
+		} else if (ic_task == IC_TASK_ENCODER) {
+			address = 0x2d1 << 3;
+		} else if (ic_task == IC_TASK_POST_PROCESSOR) {
+			address = 0x87c << 3;
+		} else {
+			BUG();
+		}
+	}
+
+	if ((in_format == YCbCr) && (out_format == RGB)) {
+		/* Init CSC1 (YCbCr->RGB) */
+		param[0] =
+		    (ycbcr2rgb_coeff[3][0] << 27) | (ycbcr2rgb_coeff[0][0] <<
+						     18) |
+		    (ycbcr2rgb_coeff[1][1] << 9) | ycbcr2rgb_coeff[2][2];
+		/* scale = 2, sat = 0 */
+		param[1] = (ycbcr2rgb_coeff[3][0] >> 5) | (2L << (40 - 32));
+		_ipu_write_param_mem(address, param, 2);
+		dev_dbg(g_ipu_dev,
+			"addr 0x%04X: word0 = 0x%08X, word1 = 0x%08X\n",
+			address, param[0], param[1]);
+
+		param[0] =
+		    (ycbcr2rgb_coeff[3][1] << 27) | (ycbcr2rgb_coeff[0][1] <<
+						     18) |
+		    (ycbcr2rgb_coeff[1][0] << 9) | ycbcr2rgb_coeff[2][0];
+		param[1] = (ycbcr2rgb_coeff[3][1] >> 5);
+		address += 1L << 3;
+		_ipu_write_param_mem(address, param, 2);
+		dev_dbg(g_ipu_dev,
+			"addr 0x%04X: word0 = 0x%08X, word1 = 0x%08X\n",
+			address, param[0], param[1]);
+
+		param[0] =
+		    (ycbcr2rgb_coeff[3][2] << 27) | (ycbcr2rgb_coeff[0][2] <<
+						     18) |
+		    (ycbcr2rgb_coeff[1][2] << 9) | ycbcr2rgb_coeff[2][1];
+		param[1] = (ycbcr2rgb_coeff[3][2] >> 5);
+		address += 1L << 3;
+		_ipu_write_param_mem(address, param, 2);
+		dev_dbg(g_ipu_dev,
+			"addr 0x%04X: word0 = 0x%08X, word1 = 0x%08X\n",
+			address, param[0], param[1]);
+	} else if ((in_format == RGB) && (out_format == YCbCr)) {
+		/* Init CSC1 (RGB->YCbCr) */
+		param[0] =
+		    (rgb2ycbcr_coeff[3][0] << 27) | (rgb2ycbcr_coeff[0][0] <<
+						     18) |
+		    (rgb2ycbcr_coeff[1][1] << 9) | rgb2ycbcr_coeff[2][2];
+		/* scale = 1, sat = 0 */
+		param[1] = (rgb2ycbcr_coeff[3][0] >> 5) | (1UL << 8);
+		_ipu_write_param_mem(address, param, 2);
+		dev_dbg(g_ipu_dev,
+			"addr 0x%04X: word0 = 0x%08X, word1 = 0x%08X\n",
+			address, param[0], param[1]);
+
+		param[0] =
+		    (rgb2ycbcr_coeff[3][1] << 27) | (rgb2ycbcr_coeff[0][1] <<
+						     18) |
+		    (rgb2ycbcr_coeff[1][0] << 9) | rgb2ycbcr_coeff[2][0];
+		param[1] = (rgb2ycbcr_coeff[3][1] >> 5);
+		address += 1L << 3;
+		_ipu_write_param_mem(address, param, 2);
+		dev_dbg(g_ipu_dev,
+			"addr 0x%04X: word0 = 0x%08X, word1 = 0x%08X\n",
+			address, param[0], param[1]);
+
+		param[0] =
+		    (rgb2ycbcr_coeff[3][2] << 27) | (rgb2ycbcr_coeff[0][2] <<
+						     18) |
+		    (rgb2ycbcr_coeff[1][2] << 9) | rgb2ycbcr_coeff[2][1];
+		param[1] = (rgb2ycbcr_coeff[3][2] >> 5);
+		address += 1L << 3;
+		_ipu_write_param_mem(address, param, 2);
+		dev_dbg(g_ipu_dev,
+			"addr 0x%04X: word0 = 0x%08X, word1 = 0x%08X\n",
+			address, param[0], param[1]);
+	} else if ((in_format == RGB) && (out_format == RGB)) {
+		/* Init CSC1 */
+		param[0] =
+		    (rgb2rgb_coeff[3][0] << 27) | (rgb2rgb_coeff[0][0] << 18) |
+		    (rgb2rgb_coeff[1][1] << 9) | rgb2rgb_coeff[2][2];
+		/* scale = 2, sat = 0 */
+		param[1] = (rgb2rgb_coeff[3][0] >> 5) | (2UL << 8);
+
+		_ipu_write_param_mem(address, param, 2);
+
+		dev_dbg(g_ipu_dev,
+			"addr 0x%04X: word0 = 0x%08X, word1 = 0x%08X\n",
+			address, param[0], param[1]);
+
+		param[0] =
+		    (rgb2rgb_coeff[3][1] << 27) | (rgb2rgb_coeff[0][1] << 18) |
+		    (rgb2rgb_coeff[1][0] << 9) | rgb2rgb_coeff[2][0];
+		param[1] = (rgb2rgb_coeff[3][1] >> 5);
+
+		address += 1L << 3;
+		_ipu_write_param_mem(address, param, 2);
+
+		dev_dbg(g_ipu_dev,
+			"addr 0x%04X: word0 = 0x%08X, word1 = 0x%08X\n",
+			address, param[0], param[1]);
+
+		param[0] =
+		    (rgb2rgb_coeff[3][2] << 27) | (rgb2rgb_coeff[0][2] << 18) |
+		    (rgb2rgb_coeff[1][2] << 9) | rgb2rgb_coeff[2][1];
+		param[1] = (rgb2rgb_coeff[3][2] >> 5);
+
+		address += 1L << 3;
+		_ipu_write_param_mem(address, param, 2);
+
+		dev_dbg(g_ipu_dev,
+			"addr 0x%04X: word0 = 0x%08X, word1 = 0x%08X\n",
+			address, param[0], param[1]);
+	} else {
+		dev_err(g_ipu_dev, "Unsupported color space conversion\n");
+	}
+}
+
+static bool _calc_resize_coeffs(uint32_t inSize, uint32_t outSize,
+				uint32_t * resizeCoeff,
+				uint32_t * downsizeCoeff)
+{
+	uint32_t tempSize;
+	uint32_t tempDownsize;
+
+	/* Cannot downsize more than 8:1 */
+	if ((outSize << 3) < inSize) {
+		return false;
+	}
+	/* compute downsizing coefficient */
+	tempDownsize = 0;
+	tempSize = inSize;
+	while ((tempSize >= outSize * 2) && (tempDownsize < 2)) {
+		tempSize >>= 1;
+		tempDownsize++;
+	}
+	*downsizeCoeff = tempDownsize;
+
+	/* compute resizing coefficient using the following equation:
+	   resizeCoeff = M*(SI -1)/(SO - 1)
+	   where M = 2^13, SI - input size, SO - output size    */
+	*resizeCoeff = (8192L * (tempSize - 1)) / (outSize - 1);
+	if (*resizeCoeff >= 16384L) {
+		dev_err(g_ipu_dev, "Warning! Overflow on resize coeff.\n");
+		*resizeCoeff = 0x3FFF;
+	}
+
+	dev_dbg(g_ipu_dev, "resizing from %u -> %u pixels, "
+		"downsize=%u, resize=%u.%lu (reg=%u)\n", inSize, outSize,
+		*downsizeCoeff, (*resizeCoeff >= 8192L) ? 1 : 0,
+		((*resizeCoeff & 0x1FFF) * 10000L) / 8192L, *resizeCoeff);
+
+	return true;
+}
diff --git a/drivers/mxc/ipu/ipu_param_mem.h b/drivers/mxc/ipu/ipu_param_mem.h
new file mode 100644
index 000000000000..07bd03a81e3f
--- /dev/null
+++ b/drivers/mxc/ipu/ipu_param_mem.h
@@ -0,0 +1,176 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __INCLUDE_IPU_PARAM_MEM_H__
+#define __INCLUDE_IPU_PARAM_MEM_H__
+
+#include <linux/types.h>
+
+static __inline void _ipu_ch_param_set_size(uint32_t * params,
+					    uint32_t pixel_fmt, uint16_t width,
+					    uint16_t height, uint16_t stride,
+					    uint32_t u, uint32_t v)
+{
+	uint32_t u_offset = 0;
+	uint32_t v_offset = 0;
+	memset(params, 0, 40);
+
+	params[3] =
+	    (uint32_t) ((width - 1) << 12) | ((uint32_t) (height - 1) << 24);
+	params[4] = (uint32_t) (height - 1) >> 8;
+	params[7] = (uint32_t) (stride - 1) << 3;
+
+	switch (pixel_fmt) {
+	case IPU_PIX_FMT_GENERIC:
+		/*Represents 8-bit Generic data */
+		params[7] |= 3 | (7UL << (81 - 64)) | (31L << (89 - 64));	/* BPP & PFS */
+		params[8] = 2;	/* SAT = use 32-bit access */
+		break;
+	case IPU_PIX_FMT_GENERIC_32:
+		/*Represents 32-bit Generic data */
+		params[7] |= (7UL << (81 - 64)) | (7L << (89 - 64));	/* BPP & PFS */
+		params[8] = 2;	/* SAT = use 32-bit access */
+		break;
+	case IPU_PIX_FMT_RGB565:
+		params[7] |= 2L | (4UL << (81 - 64)) | (7L << (89 - 64));	/* BPP & PFS */
+		params[8] = 2 |	/* SAT = 32-bit access */
+		    (0UL << (99 - 96)) |	/* Red bit offset */
+		    (5UL << (104 - 96)) |	/* Green bit offset     */
+		    (11UL << (109 - 96)) |	/* Blue bit offset */
+		    (16UL << (114 - 96)) |	/* Alpha bit offset     */
+		    (4UL << (119 - 96)) |	/* Red bit width - 1 */
+		    (5UL << (122 - 96)) |	/* Green bit width - 1 */
+		    (4UL << (125 - 96));	/* Blue bit width - 1 */
+		break;
+	case IPU_PIX_FMT_BGR24:	/* 24 BPP & RGB PFS */
+		params[7] |= 1 | (4UL << (81 - 64)) | (7L << (89 - 64));
+		params[8] = 2 |	/* SAT = 32-bit access */
+		    (8UL << (104 - 96)) |	/* Green bit offset     */
+		    (16UL << (109 - 96)) |	/* Blue bit offset */
+		    (24UL << (114 - 96)) |	/* Alpha bit offset     */
+		    (7UL << (119 - 96)) |	/* Red bit width - 1 */
+		    (7UL << (122 - 96)) |	/* Green bit width - 1 */
+		    (uint32_t) (7UL << (125 - 96));	/* Blue bit width - 1 */
+		break;
+	case IPU_PIX_FMT_RGB24:	/* 24 BPP & RGB PFS     */
+		params[7] |= 1 | (4UL << (81 - 64)) | (7L << (89 - 64));
+		params[8] = 2 |	/* SAT = 32-bit access */
+		    (16UL << (99 - 96)) |	/* Red bit offset */
+		    (8UL << (104 - 96)) |	/* Green bit offset */
+		    (0UL << (109 - 96)) |	/* Blue bit offset */
+		    (24UL << (114 - 96)) |	/* Alpha bit offset */
+		    (7UL << (119 - 96)) |	/* Red bit width - 1 */
+		    (7UL << (122 - 96)) |	/* Green bit width - 1 */
+		    (uint32_t) (7UL << (125 - 96));	/* Blue bit width - 1 */
+		break;
+	case IPU_PIX_FMT_BGRA32:
+	case IPU_PIX_FMT_BGR32:
+		/* BPP & pixel fmt */
+		params[7] |= 0 | (4UL << (81 - 64)) | (7 << (89 - 64));
+		params[8] = 2 |	/* SAT = 32-bit access */
+		    (8UL << (99 - 96)) |	/* Red bit offset */
+		    (16UL << (104 - 96)) |	/* Green bit offset     */
+		    (24UL << (109 - 96)) |	/* Blue bit offset */
+		    (0UL << (114 - 96)) |	/* Alpha bit offset     */
+		    (7UL << (119 - 96)) |	/* Red bit width - 1 */
+		    (7UL << (122 - 96)) |	/* Green bit width - 1 */
+		    (uint32_t) (7UL << (125 - 96));	/* Blue bit width - 1 */
+		params[9] = 7;	/* Alpha bit width - 1 */
+		break;
+	case IPU_PIX_FMT_RGBA32:
+	case IPU_PIX_FMT_RGB32:
+		/* BPP & pixel fmt */
+		params[7] |= 0 | (4UL << (81 - 64)) | (7 << (89 - 64));
+		params[8] = 2 |	/* SAT = 32-bit access */
+		    (24UL << (99 - 96)) |	/* Red bit offset */
+		    (16UL << (104 - 96)) |	/* Green bit offset     */
+		    (8UL << (109 - 96)) |	/* Blue bit offset */
+		    (0UL << (114 - 96)) |	/* Alpha bit offset     */
+		    (7UL << (119 - 96)) |	/* Red bit width - 1 */
+		    (7UL << (122 - 96)) |	/* Green bit width - 1 */
+		    (uint32_t) (7UL << (125 - 96));	/* Blue bit width - 1 */
+		params[9] = 7;	/* Alpha bit width - 1 */
+		break;
+	case IPU_PIX_FMT_ABGR32:
+		/* BPP & pixel fmt */
+		params[7] |= 0 | (4UL << (81 - 64)) | (7 << (89 - 64));
+		params[8] = 2 |	/* SAT = 32-bit access */
+		    (0UL << (99 - 96)) |	/* Alpha bit offset     */
+		    (8UL << (104 - 96)) |	/* Blue bit offset */
+		    (16UL << (109 - 96)) |	/* Green bit offset     */
+		    (24UL << (114 - 96)) |	/* Red bit offset */
+		    (7UL << (119 - 96)) |	/* Alpha bit width - 1 */
+		    (7UL << (122 - 96)) |	/* Blue bit width - 1 */
+		    (uint32_t) (7UL << (125 - 96));	/* Green bit width - 1 */
+		params[9] = 7;	/* Red bit width - 1 */
+		break;
+	case IPU_PIX_FMT_UYVY:
+		/* BPP & pixel format */
+		params[7] |= 2 | (6UL << 17) | (7 << (89 - 64));
+		params[8] = 2;	/* SAT = 32-bit access */
+		break;
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YUV420P:
+		/* BPP & pixel format */
+		params[7] |= 3 | (3UL << 17) | (7 << (89 - 64));
+		params[8] = 2;	/* SAT = 32-bit access */
+		u_offset = (u == 0) ? stride * height : u;
+		v_offset = (v == 0) ? u_offset + u_offset / 4 : v;
+		break;
+	case IPU_PIX_FMT_YVU422P:
+		/* BPP & pixel format */
+		params[7] |= 3 | (2UL << 17) | (7 << (89 - 64));
+		params[8] = 2;	/* SAT = 32-bit access */
+		v_offset = (v == 0) ? stride * height : v;
+		u_offset = (u == 0) ? v_offset + v_offset / 2 : u;
+		break;
+	case IPU_PIX_FMT_YUV422P:
+		/* BPP & pixel format */
+		params[7] |= 3 | (2UL << 17) | (7 << (89 - 64));
+		params[8] = 2;	/* SAT = 32-bit access */
+		u_offset = (u == 0) ? stride * height : u;
+		v_offset = (v == 0) ? u_offset + u_offset / 2 : v;
+		break;
+	default:
+		dev_err(g_ipu_dev, "mxc ipu: unimplemented pixel format\n");
+		break;
+	}
+
+	params[1] = (1UL << (46 - 32)) | (u_offset << (53 - 32));
+	params[2] = u_offset >> (64 - 53);
+	params[2] |= v_offset << (79 - 64);
+	params[3] |= v_offset >> (96 - 79);
+}
+
+static __inline void _ipu_ch_param_set_burst_size(uint32_t * params,
+						  uint16_t burst_pixels)
+{
+	params[7] &= ~(0x3FL << (89 - 64));
+	params[7] |= (uint32_t) (burst_pixels - 1) << (89 - 64);
+};
+
+static __inline void _ipu_ch_param_set_buffer(uint32_t * params,
+					      dma_addr_t buf0, dma_addr_t buf1)
+{
+	params[5] = buf0;
+	params[6] = buf1;
+};
+
+static __inline void _ipu_ch_param_set_rotation(uint32_t * params,
+						ipu_rotate_mode_t rot)
+{
+	params[7] |= (uint32_t) rot << (84 - 64);
+};
+
+void _ipu_write_param_mem(uint32_t addr, uint32_t * data, uint32_t numWords);
+
+#endif
diff --git a/drivers/mxc/ipu/ipu_prv.h b/drivers/mxc/ipu/ipu_prv.h
new file mode 100644
index 000000000000..b76a2f2357fd
--- /dev/null
+++ b/drivers/mxc/ipu/ipu_prv.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __INCLUDE_IPU_PRV_H__
+#define __INCLUDE_IPU_PRV_H__
+
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <mach/hardware.h>
+
+/* Globals */
+extern struct device *g_ipu_dev;
+extern spinlock_t ipu_lock;
+extern struct clk *g_ipu_clk;
+extern struct clk *g_ipu_csi_clk;
+
+int register_ipu_device(void);
+ipu_color_space_t format_to_colorspace(uint32_t fmt);
+
+uint32_t _ipu_channel_status(ipu_channel_t channel);
+
+void _ipu_sdc_fg_init(ipu_channel_params_t * params);
+uint32_t _ipu_sdc_fg_uninit(void);
+void _ipu_sdc_bg_init(ipu_channel_params_t * params);
+uint32_t _ipu_sdc_bg_uninit(void);
+
+void _ipu_ic_enable_task(ipu_channel_t channel);
+void _ipu_ic_disable_task(ipu_channel_t channel);
+void _ipu_ic_init_prpvf(ipu_channel_params_t * params, bool src_is_csi);
+void _ipu_ic_uninit_prpvf(void);
+void _ipu_ic_init_rotate_vf(ipu_channel_params_t * params);
+void _ipu_ic_uninit_rotate_vf(void);
+void _ipu_ic_init_csi(ipu_channel_params_t * params);
+void _ipu_ic_uninit_csi(void);
+void _ipu_ic_init_prpenc(ipu_channel_params_t * params, bool src_is_csi);
+void _ipu_ic_uninit_prpenc(void);
+void _ipu_ic_init_rotate_enc(ipu_channel_params_t * params);
+void _ipu_ic_uninit_rotate_enc(void);
+void _ipu_ic_init_pp(ipu_channel_params_t * params);
+void _ipu_ic_uninit_pp(void);
+void _ipu_ic_init_rotate_pp(ipu_channel_params_t * params);
+void _ipu_ic_uninit_rotate_pp(void);
+
+int32_t _ipu_adc_init_channel(ipu_channel_t chan, display_port_t disp,
+			      mcu_mode_t cmd, int16_t x_pos, int16_t y_pos);
+int32_t _ipu_adc_uninit_channel(ipu_channel_t chan);
+
+#endif				/* __INCLUDE_IPU_PRV_H__ */
diff --git a/drivers/mxc/ipu/ipu_regs.h b/drivers/mxc/ipu/ipu_regs.h
new file mode 100644
index 000000000000..d3ac76ea7834
--- /dev/null
+++ b/drivers/mxc/ipu/ipu_regs.h
@@ -0,0 +1,396 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * @file ipu_regs.h
+ *
+ * @brief IPU Register definitions
+ *
+ * @ingroup IPU
+ */
+#ifndef __IPU_REGS_INCLUDED__
+#define __IPU_REGS_INCLUDED__
+
+#define IPU_REG_BASE    IO_ADDRESS(IPU_CTRL_BASE_ADDR)
+
+/* Register addresses */
+/* IPU Common registers */
+#define IPU_CONF                (IPU_REG_BASE + 0x0000)
+#define IPU_CHA_BUF0_RDY        (IPU_REG_BASE + 0x0004)
+#define IPU_CHA_BUF1_RDY        (IPU_REG_BASE + 0x0008)
+#define IPU_CHA_DB_MODE_SEL     (IPU_REG_BASE + 0x000C)
+#define IPU_CHA_CUR_BUF         (IPU_REG_BASE + 0x0010)
+#define IPU_FS_PROC_FLOW        (IPU_REG_BASE + 0x0014)
+#define IPU_FS_DISP_FLOW        (IPU_REG_BASE + 0x0018)
+#define IPU_TASKS_STAT          (IPU_REG_BASE + 0x001C)
+#define IPU_IMA_ADDR            (IPU_REG_BASE + 0x0020)
+#define IPU_IMA_DATA            (IPU_REG_BASE + 0x0024)
+#define IPU_INT_CTRL_1          (IPU_REG_BASE + 0x0028)
+#define IPU_INT_CTRL_2          (IPU_REG_BASE + 0x002C)
+#define IPU_INT_CTRL_3          (IPU_REG_BASE + 0x0030)
+#define IPU_INT_CTRL_4          (IPU_REG_BASE + 0x0034)
+#define IPU_INT_CTRL_5          (IPU_REG_BASE + 0x0038)
+#define IPU_INT_STAT_1          (IPU_REG_BASE + 0x003C)
+#define IPU_INT_STAT_2          (IPU_REG_BASE + 0x0040)
+#define IPU_INT_STAT_3          (IPU_REG_BASE + 0x0044)
+#define IPU_INT_STAT_4          (IPU_REG_BASE + 0x0048)
+#define IPU_INT_STAT_5          (IPU_REG_BASE + 0x004C)
+#define IPU_BRK_CTRL_1          (IPU_REG_BASE + 0x0050)
+#define IPU_BRK_CTRL_2          (IPU_REG_BASE + 0x0054)
+#define IPU_BRK_STAT            (IPU_REG_BASE + 0x0058)
+#define IPU_DIAGB_CTRL          (IPU_REG_BASE + 0x005C)
+/* CMOS Sensor Interface Registers */
+#define CSI_SENS_CONF           (IPU_REG_BASE + 0x0060)
+#define CSI_SENS_FRM_SIZE       (IPU_REG_BASE + 0x0064)
+#define CSI_ACT_FRM_SIZE        (IPU_REG_BASE + 0x0068)
+#define CSI_OUT_FRM_CTRL        (IPU_REG_BASE + 0x006C)
+#define CSI_TST_CTRL            (IPU_REG_BASE + 0x0070)
+#define CSI_CCIR_CODE_1         (IPU_REG_BASE + 0x0074)
+#define CSI_CCIR_CODE_2         (IPU_REG_BASE + 0x0078)
+#define CSI_CCIR_CODE_3         (IPU_REG_BASE + 0x007C)
+#define CSI_FLASH_STROBE_1      (IPU_REG_BASE + 0x0080)
+#define CSI_FLASH_STROBE_2      (IPU_REG_BASE + 0x0084)
+/* Image Converter Registers */
+#define IC_CONF                 (IPU_REG_BASE + 0x0088)
+#define IC_PRP_ENC_RSC          (IPU_REG_BASE + 0x008C)
+#define IC_PRP_VF_RSC           (IPU_REG_BASE + 0x0090)
+#define IC_PP_RSC               (IPU_REG_BASE + 0x0094)
+#define IC_CMBP_1               (IPU_REG_BASE + 0x0098)
+#define IC_CMBP_2               (IPU_REG_BASE + 0x009C)
+#define PF_CONF                 (IPU_REG_BASE + 0x00A0)
+#define IDMAC_CONF              (IPU_REG_BASE + 0x00A4)
+#define IDMAC_CHA_EN            (IPU_REG_BASE + 0x00A8)
+#define IDMAC_CHA_PRI           (IPU_REG_BASE + 0x00AC)
+#define IDMAC_CHA_BUSY          (IPU_REG_BASE + 0x00B0)
+/* SDC Registers */
+#define SDC_COM_CONF            (IPU_REG_BASE + 0x00B4)
+#define SDC_GW_CTRL             (IPU_REG_BASE + 0x00B8)
+#define SDC_FG_POS              (IPU_REG_BASE + 0x00BC)
+#define SDC_BG_POS              (IPU_REG_BASE + 0x00C0)
+#define SDC_CUR_POS             (IPU_REG_BASE + 0x00C4)
+#define SDC_PWM_CTRL            (IPU_REG_BASE + 0x00C8)
+#define SDC_CUR_MAP             (IPU_REG_BASE + 0x00CC)
+#define SDC_HOR_CONF            (IPU_REG_BASE + 0x00D0)
+#define SDC_VER_CONF            (IPU_REG_BASE + 0x00D4)
+#define SDC_SHARP_CONF_1        (IPU_REG_BASE + 0x00D8)
+#define SDC_SHARP_CONF_2        (IPU_REG_BASE + 0x00DC)
+/* ADC Registers */
+#define ADC_CONF                (IPU_REG_BASE + 0x00E0)
+#define ADC_SYSCHA1_SA          (IPU_REG_BASE + 0x00E4)
+#define ADC_SYSCHA2_SA          (IPU_REG_BASE + 0x00E8)
+#define ADC_PRPCHAN_SA          (IPU_REG_BASE + 0x00EC)
+#define ADC_PPCHAN_SA           (IPU_REG_BASE + 0x00F0)
+#define ADC_DISP0_CONF          (IPU_REG_BASE + 0x00F4)
+#define ADC_DISP0_RD_AP         (IPU_REG_BASE + 0x00F8)
+#define ADC_DISP0_RDM           (IPU_REG_BASE + 0x00FC)
+#define ADC_DISP0_SS            (IPU_REG_BASE + 0x0100)
+#define ADC_DISP1_CONF          (IPU_REG_BASE + 0x0104)
+#define ADC_DISP1_RD_AP         (IPU_REG_BASE + 0x0108)
+#define ADC_DISP1_RDM           (IPU_REG_BASE + 0x010C)
+#define ADC_DISP12_SS           (IPU_REG_BASE + 0x0110)
+#define ADC_DISP2_CONF          (IPU_REG_BASE + 0x0114)
+#define ADC_DISP2_RD_AP         (IPU_REG_BASE + 0x0118)
+#define ADC_DISP2_RDM           (IPU_REG_BASE + 0x011C)
+#define ADC_DISP_VSYNC          (IPU_REG_BASE + 0x0120)
+/* Display Interface re(sters */
+#define DI_DISP_IF_CONF         (IPU_REG_BASE + 0x0124)
+#define DI_DISP_SIG_POL         (IPU_REG_BASE + 0x0128)
+#define DI_SER_DISP1_CONF       (IPU_REG_BASE + 0x012C)
+#define DI_SER_DISP2_CONF       (IPU_REG_BASE + 0x0130)
+#define DI_HSP_CLK_PER          (IPU_REG_BASE + 0x0134)
+#define DI_DISP0_TIME_CONF_1    (IPU_REG_BASE + 0x0138)
+#define DI_DISP0_TIME_CONF_2    (IPU_REG_BASE + 0x013C)
+#define DI_DISP0_TIME_CONF_3    (IPU_REG_BASE + 0x0140)
+#define DI_DISP1_TIME_CONF_1    (IPU_REG_BASE + 0x0144)
+#define DI_DISP1_TIME_CONF_2    (IPU_REG_BASE + 0x0148)
+#define DI_DISP1_TIME_CONF_3    (IPU_REG_BASE + 0x014C)
+#define DI_DISP2_TIME_CONF_1    (IPU_REG_BASE + 0x0150)
+#define DI_DISP2_TIME_CONF_2    (IPU_REG_BASE + 0x0154)
+#define DI_DISP2_TIME_CONF_3    (IPU_REG_BASE + 0x0158)
+#define DI_DISP3_TIME_CONF      (IPU_REG_BASE + 0x015C)
+#define DI_DISP0_DB0_MAP        (IPU_REG_BASE + 0x0160)
+#define DI_DISP0_DB1_MAP        (IPU_REG_BASE + 0x0164)
+#define DI_DISP0_DB2_MAP        (IPU_REG_BASE + 0x0168)
+#define DI_DISP0_CB0_MAP        (IPU_REG_BASE + 0x016C)
+#define DI_DISP0_CB1_MAP        (IPU_REG_BASE + 0x0170)
+#define DI_DISP0_CB2_MAP        (IPU_REG_BASE + 0x0174)
+#define DI_DISP1_DB0_MAP        (IPU_REG_BASE + 0x0178)
+#define DI_DISP1_DB1_MAP        (IPU_REG_BASE + 0x017C)
+#define DI_DISP1_DB2_MAP        (IPU_REG_BASE + 0x0180)
+#define DI_DISP1_CB0_MAP        (IPU_REG_BASE + 0x0184)
+#define DI_DISP1_CB1_MAP        (IPU_REG_BASE + 0x0188)
+#define DI_DISP1_CB2_MAP        (IPU_REG_BASE + 0x018C)
+#define DI_DISP2_DB0_MAP        (IPU_REG_BASE + 0x0190)
+#define DI_DISP2_DB1_MAP        (IPU_REG_BASE + 0x0194)
+#define DI_DISP2_DB2_MAP        (IPU_REG_BASE + 0x0198)
+#define DI_DISP2_CB0_MAP        (IPU_REG_BASE + 0x019C)
+#define DI_DISP2_CB1_MAP        (IPU_REG_BASE + 0x01A0)
+#define DI_DISP2_CB2_MAP        (IPU_REG_BASE + 0x01A4)
+#define DI_DISP3_B0_MAP         (IPU_REG_BASE + 0x01A8)
+#define DI_DISP3_B1_MAP         (IPU_REG_BASE + 0x01AC)
+#define DI_DISP3_B2_MAP         (IPU_REG_BASE + 0x01B0)
+#define DI_DISP_ACC_CC          (IPU_REG_BASE + 0x01B4)
+#define DI_DISP_LLA_CONF        (IPU_REG_BASE + 0x01B8)
+#define DI_DISP_LLA_DATA        (IPU_REG_BASE + 0x01BC)
+
+#define IPUIRQ_2_STATREG(int)   (IPU_INT_STAT_1 + 4*(int>>5))
+#define IPUIRQ_2_CTRLREG(int)   (IPU_INT_CTRL_1 + 4*(int>>5))
+#define IPUIRQ_2_MASK(int)      (1UL << (int & 0x1F))
+
+enum {
+	IPU_CONF_CSI_EN = 0x00000001,
+	IPU_CONF_IC_EN = 0x00000002,
+	IPU_CONF_ROT_EN = 0x00000004,
+	IPU_CONF_PF_EN = 0x00000008,
+	IPU_CONF_SDC_EN = 0x00000010,
+	IPU_CONF_ADC_EN = 0x00000020,
+	IPU_CONF_DI_EN = 0x00000040,
+	IPU_CONF_DU_EN = 0x00000080,
+	IPU_CONF_PXL_ENDIAN = 0x00000100,
+
+	FS_PRPVF_ROT_SRC_SEL = 0x00000040,
+	FS_PRPENC_ROT_SRC_SEL = 0x00000020,
+	FS_PRPENC_DEST_SEL = 0x00000010,
+	FS_PP_SRC_SEL_MASK = 0x00000300,
+	FS_PP_SRC_SEL_OFFSET = 8,
+	FS_PP_ROT_SRC_SEL_MASK = 0x00000C00,
+	FS_PP_ROT_SRC_SEL_OFFSET = 10,
+	FS_PF_DEST_SEL_MASK = 0x00003000,
+	FS_PF_DEST_SEL_OFFSET = 12,
+	FS_PRPVF_DEST_SEL_MASK = 0x00070000,
+	FS_PRPVF_DEST_SEL_OFFSET = 16,
+	FS_PRPVF_ROT_DEST_SEL_MASK = 0x00700000,
+	FS_PRPVF_ROT_DEST_SEL_OFFSET = 20,
+	FS_PP_DEST_SEL_MASK = 0x07000000,
+	FS_PP_DEST_SEL_OFFSET = 24,
+	FS_PP_ROT_DEST_SEL_MASK = 0x70000000,
+	FS_PP_ROT_DEST_SEL_OFFSET = 28,
+	FS_VF_IN_VALID = 0x00000002,
+	FS_ENC_IN_VALID = 0x00000001,
+
+	FS_SDC_BG_SRC_SEL_MASK = 0x00000007,
+	FS_SDC_BG_SRC_SEL_OFFSET = 0,
+	FS_SDC_FG_SRC_SEL_MASK = 0x00000070,
+	FS_SDC_FG_SRC_SEL_OFFSET = 4,
+	FS_ADC1_SRC_SEL_MASK = 0x00000700,
+	FS_ADC1_SRC_SEL_OFFSET = 8,
+	FS_ADC2_SRC_SEL_MASK = 0x00007000,
+	FS_ADC2_SRC_SEL_OFFSET = 12,
+	FS_AUTO_REF_PER_MASK = 0x03FF0000,
+	FS_AUTO_REF_PER_OFFSET = 16,
+
+	FS_DEST_ARM = 0,
+	FS_DEST_ROT = 1,
+	FS_DEST_PP = 1,
+	FS_DEST_ADC1 = 2,
+	FS_DEST_ADC2 = 3,
+	FS_DEST_SDC_BG = 4,
+	FS_DEST_SDC_FG = 5,
+	FS_DEST_ADC = 6,
+
+	FS_SRC_ARM = 0,
+	FS_PP_SRC_PF = 1,
+	FS_PP_SRC_ROT = 2,
+
+	FS_ROT_SRC_PP = 1,
+	FS_ROT_SRC_PF = 2,
+
+	FS_PF_DEST_PP = 1,
+	FS_PF_DEST_ROT = 2,
+
+	FS_SRC_ROT_VF = 1,
+	FS_SRC_ROT_PP = 2,
+	FS_SRC_VF = 3,
+	FS_SRC_PP = 4,
+	FS_SRC_SNOOP = 5,
+	FS_SRC_AUTOREF = 6,
+	FS_SRC_AUTOREF_SNOOP = 7,
+
+	TSTAT_PF_H264_PAUSE = 0x00000001,
+	TSTAT_CSI2MEM_MASK = 0x0000000C,
+	TSTAT_CSI2MEM_OFFSET = 2,
+	TSTAT_VF_MASK = 0x00000600,
+	TSTAT_VF_OFFSET = 9,
+	TSTAT_VF_ROT_MASK = 0x000C0000,
+	TSTAT_VF_ROT_OFFSET = 18,
+	TSTAT_ENC_MASK = 0x00000180,
+	TSTAT_ENC_OFFSET = 7,
+	TSTAT_ENC_ROT_MASK = 0x00030000,
+	TSTAT_ENC_ROT_OFFSET = 16,
+	TSTAT_PP_MASK = 0x00001800,
+	TSTAT_PP_OFFSET = 11,
+	TSTAT_PP_ROT_MASK = 0x00300000,
+	TSTAT_PP_ROT_OFFSET = 20,
+	TSTAT_PF_MASK = 0x00C00000,
+	TSTAT_PF_OFFSET = 22,
+	TSTAT_ADCSYS1_MASK = 0x03000000,
+	TSTAT_ADCSYS1_OFFSET = 24,
+	TSTAT_ADCSYS2_MASK = 0x0C000000,
+	TSTAT_ADCSYS2_OFFSET = 26,
+
+	TASK_STAT_IDLE = 0,
+	TASK_STAT_ACTIVE = 1,
+	TASK_STAT_WAIT4READY = 2,
+
+	/* Register bits */
+	SDC_COM_TFT_COLOR = 0x00000001UL,
+	SDC_COM_FG_EN = 0x00000010UL,
+	SDC_COM_GWSEL = 0x00000020UL,
+	SDC_COM_GLB_A = 0x00000040UL,
+	SDC_COM_KEY_COLOR_G = 0x00000080UL,
+	SDC_COM_BG_EN = 0x00000200UL,
+	SDC_COM_SHARP = 0x00001000UL,
+
+	SDC_V_SYNC_WIDTH_L = 0x00000001UL,
+
+	ADC_CONF_PRP_EN = 0x00000001L,
+	ADC_CONF_PP_EN = 0x00000002L,
+	ADC_CONF_MCU_EN = 0x00000004L,
+
+	ADC_DISP_CONF_SL_MASK = 0x00000FFFL,
+	ADC_DISP_CONF_TYPE_MASK = 0x00003000L,
+	ADC_DISP_CONF_TYPE_XY = 0x00002000L,
+
+	ADC_DISP_VSYNC_D0_MODE_MASK = 0x00000003L,
+	ADC_DISP_VSYNC_D0_WIDTH_MASK = 0x003F0000L,
+	ADC_DISP_VSYNC_D12_MODE_MASK = 0x0000000CL,
+	ADC_DISP_VSYNC_D12_WIDTH_MASK = 0x3F000000L,
+
+	/* Image Converter Register bits */
+	IC_CONF_PRPENC_EN = 0x00000001,
+	IC_CONF_PRPENC_CSC1 = 0x00000002,
+	IC_CONF_PRPENC_ROT_EN = 0x00000004,
+	IC_CONF_PRPVF_EN = 0x00000100,
+	IC_CONF_PRPVF_CSC1 = 0x00000200,
+	IC_CONF_PRPVF_CSC2 = 0x00000400,
+	IC_CONF_PRPVF_CMB = 0x00000800,
+	IC_CONF_PRPVF_ROT_EN = 0x00001000,
+	IC_CONF_PP_EN = 0x00010000,
+	IC_CONF_PP_CSC1 = 0x00020000,
+	IC_CONF_PP_CSC2 = 0x00040000,
+	IC_CONF_PP_CMB = 0x00080000,
+	IC_CONF_PP_ROT_EN = 0x00100000,
+	IC_CONF_IC_GLB_LOC_A = 0x10000000,
+	IC_CONF_KEY_COLOR_EN = 0x20000000,
+	IC_CONF_RWS_EN = 0x40000000,
+	IC_CONF_CSI_MEM_WR_EN = 0x80000000,
+
+	IDMA_CHAN_INVALID = 0x000000FF,
+	IDMA_IC_0 = 0x00000001,
+	IDMA_IC_1 = 0x00000002,
+	IDMA_IC_2 = 0x00000004,
+	IDMA_IC_3 = 0x00000008,
+	IDMA_IC_4 = 0x00000010,
+	IDMA_IC_5 = 0x00000020,
+	IDMA_IC_6 = 0x00000040,
+	IDMA_IC_7 = 0x00000080,
+	IDMA_IC_8 = 0x00000100,
+	IDMA_IC_9 = 0x00000200,
+	IDMA_IC_10 = 0x00000400,
+	IDMA_IC_11 = 0x00000800,
+	IDMA_IC_12 = 0x00001000,
+	IDMA_IC_13 = 0x00002000,
+	IDMA_SDC_BG = 0x00004000,
+	IDMA_SDC_FG = 0x00008000,
+	IDMA_SDC_MASK = 0x00010000,
+	IDMA_SDC_PARTIAL = 0x00020000,
+	IDMA_ADC_SYS1_WR = 0x00040000,
+	IDMA_ADC_SYS2_WR = 0x00080000,
+	IDMA_ADC_SYS1_CMD = 0x00100000,
+	IDMA_ADC_SYS2_CMD = 0x00200000,
+	IDMA_ADC_SYS1_RD = 0x00400000,
+	IDMA_ADC_SYS2_RD = 0x00800000,
+	IDMA_PF_QP = 0x01000000,
+	IDMA_PF_BSP = 0x02000000,
+	IDMA_PF_Y_IN = 0x04000000,
+	IDMA_PF_U_IN = 0x08000000,
+	IDMA_PF_V_IN = 0x10000000,
+	IDMA_PF_Y_OUT = 0x20000000,
+	IDMA_PF_U_OUT = 0x40000000,
+	IDMA_PF_V_OUT = 0x80000000,
+
+	CSI_SENS_CONF_DATA_FMT_SHIFT = 8,
+	CSI_SENS_CONF_DATA_FMT_RGB_YUV444 = 0x00000000L,
+	CSI_SENS_CONF_DATA_FMT_YUV422 = 0x00000200L,
+	CSI_SENS_CONF_DATA_FMT_BAYER = 0x00000300L,
+
+	CSI_SENS_CONF_VSYNC_POL_SHIFT = 0,
+	CSI_SENS_CONF_HSYNC_POL_SHIFT = 1,
+	CSI_SENS_CONF_DATA_POL_SHIFT = 2,
+	CSI_SENS_CONF_PIX_CLK_POL_SHIFT = 3,
+	CSI_SENS_CONF_SENS_PRTCL_SHIFT = 4,
+	CSI_SENS_CONF_SENS_CLKSRC_SHIFT = 7,
+	CSI_SENS_CONF_DATA_WIDTH_SHIFT = 10,
+	CSI_SENS_CONF_EXT_VSYNC_SHIFT = 15,
+	CSI_SENS_CONF_DIVRATIO_SHIFT = 16,
+
+	PF_CONF_TYPE_MASK = 0x00000007,
+	PF_CONF_TYPE_SHIFT = 0,
+	PF_CONF_PAUSE_EN = 0x00000010,
+	PF_CONF_RESET = 0x00008000,
+	PF_CONF_PAUSE_ROW_MASK = 0x00FF0000,
+	PF_CONF_PAUSE_ROW_SHIFT = 16,
+
+	/* DI_DISP_SIG_POL bits */
+	DI_D3_VSYNC_POL_SHIFT = 28,
+	DI_D3_HSYNC_POL_SHIFT = 27,
+	DI_D3_DRDY_SHARP_POL_SHIFT = 26,
+	DI_D3_CLK_POL_SHIFT = 25,
+	DI_D3_DATA_POL_SHIFT = 24,
+
+	/* DI_DISP_IF_CONF bits */
+	DI_D3_CLK_IDLE_SHIFT = 26,
+	DI_D3_CLK_SEL_SHIFT = 25,
+	DI_D3_DATAMSK_SHIFT = 24,
+
+	DISPx_IF_CLK_DOWN_OFFSET = 22,
+	DISPx_IF_CLK_UP_OFFSET = 12,
+	DISPx_IF_CLK_PER_OFFSET = 0,
+	DISPx_IF_CLK_READ_EN_OFFSET = 16,
+	DISPx_PIX_CLK_PER_OFFSET = 0,
+
+	DI_CONF_DISP0_EN = 0x00000001L,
+	DI_CONF_DISP0_IF_MODE_OFFSET = 1,
+	DI_CONF_DISP0_BURST_MODE_OFFSET = 3,
+	DI_CONF_DISP1_EN = 0x00000100L,
+	DI_CONF_DISP1_IF_MODE_OFFSET = 9,
+	DI_CONF_DISP1_BURST_MODE_OFFSET = 12,
+	DI_CONF_DISP2_EN = 0x00010000L,
+	DI_CONF_DISP2_IF_MODE_OFFSET = 17,
+	DI_CONF_DISP2_BURST_MODE_OFFSET = 20,
+
+	DI_SER_DISPx_CONF_SER_BIT_NUM_OFFSET = 16,
+	DI_SER_DISPx_CONF_PREAMBLE_OFFSET = 8,
+	DI_SER_DISPx_CONF_PREAMBLE_LEN_OFFSET = 4,
+	DI_SER_DISPx_CONF_RW_CFG_OFFSET = 1,
+	DI_SER_DISPx_CONF_BURST_MODE_EN = 0x01000000L,
+	DI_SER_DISPx_CONF_PREAMBLE_EN = 0x00000001L,
+
+	/* DI_DISP_ACC_CC */
+	DISP0_IF_CLK_CNT_D_MASK = 0x00000003L,
+	DISP0_IF_CLK_CNT_D_OFFSET = 0,
+	DISP0_IF_CLK_CNT_C_MASK = 0x0000000CL,
+	DISP0_IF_CLK_CNT_C_OFFSET = 2,
+	DISP1_IF_CLK_CNT_D_MASK = 0x00000030L,
+	DISP1_IF_CLK_CNT_D_OFFSET = 4,
+	DISP1_IF_CLK_CNT_C_MASK = 0x000000C0L,
+	DISP1_IF_CLK_CNT_C_OFFSET = 6,
+	DISP2_IF_CLK_CNT_D_MASK = 0x00000300L,
+	DISP2_IF_CLK_CNT_D_OFFSET = 8,
+	DISP2_IF_CLK_CNT_C_MASK = 0x00000C00L,
+	DISP2_IF_CLK_CNT_C_OFFSET = 10,
+	DISP3_IF_CLK_CNT_MASK = 0x00003000L,
+	DISP3_IF_CLK_CNT_OFFSET = 12,
+};
+
+#endif
diff --git a/drivers/mxc/ipu/ipu_sdc.c b/drivers/mxc/ipu/ipu_sdc.c
new file mode 100644
index 000000000000..a36eb6dc6a4e
--- /dev/null
+++ b/drivers/mxc/ipu/ipu_sdc.c
@@ -0,0 +1,357 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_sdc.c
+ *
+ * @brief IPU SDC submodule API functions
+ *
+ * @ingroup IPU
+ */
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+#include "ipu_param_mem.h"
+
+static uint32_t g_h_start_width;
+static uint32_t g_v_start_width;
+
+static const uint32_t di_mappings[] = {
+	0x1600AAAA, 0x00E05555, 0x00070000, 3,	/* RGB888 */
+	0x0005000F, 0x000B000F, 0x0011000F, 1,	/* RGB666 */
+	0x0011000F, 0x000B000F, 0x0005000F, 1,	/* BGR666 */
+	0x0004003F, 0x000A000F, 0x000F003F, 1	/* RGB565 */
+};
+
+/*!
+ * This function is called to initialize a synchronous LCD panel.
+ *
+ * @param       panel           The type of panel.
+ *
+ * @param       pixel_clk       Desired pixel clock frequency in Hz.
+ *
+ * @param       pixel_fmt       Input parameter for pixel format of buffer. Pixel
+ *                              format is a FOURCC ASCII code.
+ *
+ * @param       width           The width of panel in pixels.
+ *
+ * @param       height          The height of panel in pixels.
+ *
+ * @param       hStartWidth     The number of pixel clocks between the HSYNC
+ *                              signal pulse and the start of valid data.
+ *
+ * @param       hSyncWidth      The width of the HSYNC signal in units of pixel
+ *                              clocks.
+ *
+ * @param       hEndWidth       The number of pixel clocks between the end of
+ *                              valid data and the HSYNC signal for next line.
+ *
+ * @param       vStartWidth     The number of lines between the VSYNC
+ *                              signal pulse and the start of valid data.
+ *
+ * @param       vSyncWidth      The width of the VSYNC signal in units of lines
+ *
+ * @param       vEndWidth       The number of lines between the end of valid
+ *                              data and the VSYNC signal for next frame.
+ *
+ * @param       sig             Bitfield of signal polarities for LCD interface.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int32_t ipu_sdc_init_panel(ipu_panel_t panel,
+			   uint32_t pixel_clk,
+			   uint16_t width, uint16_t height,
+			   uint32_t pixel_fmt,
+			   uint16_t h_start_width, uint16_t h_sync_width,
+			   uint16_t h_end_width, uint16_t v_start_width,
+			   uint16_t v_sync_width, uint16_t v_end_width,
+			   ipu_di_signal_cfg_t sig)
+{
+	unsigned long lock_flags;
+	uint32_t reg;
+	uint32_t old_conf;
+	uint32_t div;
+
+	dev_dbg(g_ipu_dev, "panel size = %d x %d\n", width, height);
+
+	if ((v_sync_width == 0) || (h_sync_width == 0))
+		return EINVAL;
+
+	/* Init panel size and blanking periods */
+	reg =
+	    ((uint32_t) (h_sync_width - 1) << 26) |
+	    ((uint32_t) (width + h_sync_width + h_start_width + h_end_width - 1)
+	     << 16);
+	__raw_writel(reg, SDC_HOR_CONF);
+
+	reg = ((uint32_t) (v_sync_width - 1) << 26) | SDC_V_SYNC_WIDTH_L |
+	    ((uint32_t)
+	     (height + v_sync_width + v_start_width + v_end_width - 1) << 16);
+	__raw_writel(reg, SDC_VER_CONF);
+
+	g_h_start_width = h_start_width + h_sync_width;
+	g_v_start_width = v_start_width + v_sync_width;
+
+	switch (panel) {
+	case IPU_PANEL_SHARP_TFT:
+		__raw_writel(0x00FD0102L, SDC_SHARP_CONF_1);
+		__raw_writel(0x00F500F4L, SDC_SHARP_CONF_2);
+		__raw_writel(SDC_COM_SHARP | SDC_COM_TFT_COLOR, SDC_COM_CONF);
+		break;
+	case IPU_PANEL_TFT:
+		__raw_writel(SDC_COM_TFT_COLOR, SDC_COM_CONF);
+		break;
+	default:
+		return EINVAL;
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	/* Init clocking */
+
+	/* Calculate divider */
+	/* fractional part is 4 bits so simply multiple by 2^4 to get fractional part */
+	dev_dbg(g_ipu_dev, "pixel clk = %d\n", pixel_clk);
+	div = (clk_get_rate(g_ipu_clk) * 16) / pixel_clk;
+	if (div < 0x40) {	/* Divider less than 4 */
+		dev_dbg(g_ipu_dev,
+			"InitPanel() - Pixel clock divider less than 1\n");
+		div = 0x40;
+	}
+	/* DISP3_IF_CLK_DOWN_WR is half the divider value and 2 less fraction bits */
+	/* Subtract 1 extra from DISP3_IF_CLK_DOWN_WR based on timing debug     */
+	/* DISP3_IF_CLK_UP_WR is 0 */
+	__raw_writel((((div / 8) - 1) << 22) | div, DI_DISP3_TIME_CONF);
+
+	/* DI settings */
+	old_conf = __raw_readl(DI_DISP_IF_CONF) & 0x78FFFFFF;
+	old_conf |= sig.datamask_en << DI_D3_DATAMSK_SHIFT |
+	    sig.clksel_en << DI_D3_CLK_SEL_SHIFT |
+	    sig.clkidle_en << DI_D3_CLK_IDLE_SHIFT;
+	__raw_writel(old_conf, DI_DISP_IF_CONF);
+
+	old_conf = __raw_readl(DI_DISP_SIG_POL) & 0xE0FFFFFF;
+	old_conf |= sig.data_pol << DI_D3_DATA_POL_SHIFT |
+	    sig.clk_pol << DI_D3_CLK_POL_SHIFT |
+	    sig.enable_pol << DI_D3_DRDY_SHARP_POL_SHIFT |
+	    sig.Hsync_pol << DI_D3_HSYNC_POL_SHIFT |
+	    sig.Vsync_pol << DI_D3_VSYNC_POL_SHIFT;
+	__raw_writel(old_conf, DI_DISP_SIG_POL);
+
+	switch (pixel_fmt) {
+	case IPU_PIX_FMT_RGB24:
+		__raw_writel(di_mappings[0], DI_DISP3_B0_MAP);
+		__raw_writel(di_mappings[1], DI_DISP3_B1_MAP);
+		__raw_writel(di_mappings[2], DI_DISP3_B2_MAP);
+		__raw_writel(__raw_readl(DI_DISP_ACC_CC) |
+			     ((di_mappings[3] - 1) << 12), DI_DISP_ACC_CC);
+		break;
+	case IPU_PIX_FMT_RGB666:
+		__raw_writel(di_mappings[4], DI_DISP3_B0_MAP);
+		__raw_writel(di_mappings[5], DI_DISP3_B1_MAP);
+		__raw_writel(di_mappings[6], DI_DISP3_B2_MAP);
+		__raw_writel(__raw_readl(DI_DISP_ACC_CC) |
+			     ((di_mappings[7] - 1) << 12), DI_DISP_ACC_CC);
+		break;
+	case IPU_PIX_FMT_BGR666:
+		__raw_writel(di_mappings[8], DI_DISP3_B0_MAP);
+		__raw_writel(di_mappings[9], DI_DISP3_B1_MAP);
+		__raw_writel(di_mappings[10], DI_DISP3_B2_MAP);
+		__raw_writel(__raw_readl(DI_DISP_ACC_CC) |
+			     ((di_mappings[11] - 1) << 12), DI_DISP_ACC_CC);
+		break;
+	default:
+		__raw_writel(di_mappings[12], DI_DISP3_B0_MAP);
+		__raw_writel(di_mappings[13], DI_DISP3_B1_MAP);
+		__raw_writel(di_mappings[14], DI_DISP3_B2_MAP);
+		__raw_writel(__raw_readl(DI_DISP_ACC_CC) |
+			     ((di_mappings[15] - 1) << 12), DI_DISP_ACC_CC);
+		break;
+	}
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	dev_dbg(g_ipu_dev, "DI_DISP_IF_CONF = 0x%08X\n",
+		__raw_readl(DI_DISP_IF_CONF));
+	dev_dbg(g_ipu_dev, "DI_DISP_SIG_POL = 0x%08X\n",
+		__raw_readl(DI_DISP_SIG_POL));
+	dev_dbg(g_ipu_dev, "DI_DISP3_TIME_CONF = 0x%08X\n",
+		__raw_readl(DI_DISP3_TIME_CONF));
+
+	return 0;
+}
+
+/*!
+ * This function sets the foreground and background plane global alpha blending
+ * modes.
+ *
+ * @param       enable          Boolean to enable or disable global alpha
+ *                              blending. If disabled, per pixel blending is used.
+ *
+ * @param       alpha           Global alpha value.
+ *
+ * @return      This function returns 0 on success or negative error code on fail
+ */
+int32_t ipu_sdc_set_global_alpha(bool enable, uint8_t alpha)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if (enable) {
+		reg = __raw_readl(SDC_GW_CTRL) & 0x00FFFFFFL;
+		__raw_writel(reg | ((uint32_t) alpha << 24), SDC_GW_CTRL);
+
+		reg = __raw_readl(SDC_COM_CONF);
+		__raw_writel(reg | SDC_COM_GLB_A, SDC_COM_CONF);
+	} else {
+		reg = __raw_readl(SDC_COM_CONF);
+		__raw_writel(reg & ~SDC_COM_GLB_A, SDC_COM_CONF);
+	}
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+
+/*!
+ * This function sets the transparent color key for SDC graphic plane.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       enable          Boolean to enable or disable color key
+ *
+ * @param       colorKey        24-bit RGB color to use as transparent color key.
+ *
+ * @return      This function returns 0 on success or negative error code on fail
+ */
+int32_t ipu_sdc_set_color_key(ipu_channel_t channel, bool enable,
+			      uint32_t color_key)
+{
+	uint32_t reg, sdc_conf;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	sdc_conf = __raw_readl(SDC_COM_CONF);
+	if (channel == MEM_SDC_BG) {
+		sdc_conf &= ~SDC_COM_GWSEL;
+	} else {
+		sdc_conf |= SDC_COM_GWSEL;
+	}
+
+	if (enable) {
+		reg = __raw_readl(SDC_GW_CTRL) & 0xFF000000L;
+		__raw_writel(reg | (color_key & 0x00FFFFFFL), SDC_GW_CTRL);
+
+		sdc_conf |= SDC_COM_KEY_COLOR_G;
+	} else {
+		sdc_conf &= ~SDC_COM_KEY_COLOR_G;
+	}
+	__raw_writel(sdc_conf, SDC_COM_CONF);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+
+int32_t ipu_sdc_set_brightness(uint8_t value)
+{
+	__raw_writel(0x03000000UL | value << 16, SDC_PWM_CTRL);
+	return 0;
+}
+
+/*!
+ * This function sets the window position of the foreground or background plane.
+ * modes.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       x_pos           The X coordinate position to place window at.
+ *                              The position is relative to the top left corner.
+ *
+ * @param       y_pos           The Y coordinate position to place window at.
+ *                              The position is relative to the top left corner.
+ *
+ * @return      This function returns 0 on success or negative error code on fail
+ */
+int32_t ipu_disp_set_window_pos(ipu_channel_t channel, int16_t x_pos,
+			       int16_t y_pos)
+{
+	x_pos += g_h_start_width;
+	y_pos += g_v_start_width;
+
+	if (channel == MEM_SDC_BG) {
+		__raw_writel((x_pos << 16) | y_pos, SDC_BG_POS);
+	} else if (channel == MEM_SDC_FG) {
+		__raw_writel((x_pos << 16) | y_pos, SDC_FG_POS);
+	} else {
+		return EINVAL;
+	}
+	return 0;
+}
+
+void _ipu_sdc_fg_init(ipu_channel_params_t * params)
+{
+	uint32_t reg;
+	(void)params;
+
+	/* Enable FG channel */
+	reg = __raw_readl(SDC_COM_CONF);
+	__raw_writel(reg | SDC_COM_FG_EN | SDC_COM_BG_EN, SDC_COM_CONF);
+}
+
+uint32_t _ipu_sdc_fg_uninit(void)
+{
+	uint32_t reg;
+
+	/* Disable FG channel */
+	reg = __raw_readl(SDC_COM_CONF);
+	__raw_writel(reg & ~SDC_COM_FG_EN, SDC_COM_CONF);
+
+	return (reg & SDC_COM_FG_EN);
+}
+
+void _ipu_sdc_bg_init(ipu_channel_params_t * params)
+{
+	uint32_t reg;
+	(void)params;
+
+	/* Enable FG channel */
+	reg = __raw_readl(SDC_COM_CONF);
+	__raw_writel(reg | SDC_COM_BG_EN, SDC_COM_CONF);
+}
+
+uint32_t _ipu_sdc_bg_uninit(void)
+{
+	uint32_t reg;
+
+	/* Disable BG channel */
+	reg = __raw_readl(SDC_COM_CONF);
+	__raw_writel(reg & ~SDC_COM_BG_EN, SDC_COM_CONF);
+
+	return (reg & SDC_COM_BG_EN);
+}
+
+/* Exported symbols for modules. */
+EXPORT_SYMBOL(ipu_sdc_init_panel);
+EXPORT_SYMBOL(ipu_sdc_set_global_alpha);
+EXPORT_SYMBOL(ipu_sdc_set_color_key);
+EXPORT_SYMBOL(ipu_sdc_set_brightness);
+EXPORT_SYMBOL(ipu_disp_set_window_pos);
diff --git a/drivers/mxc/ipu/pf/Kconfig b/drivers/mxc/ipu/pf/Kconfig
new file mode 100644
index 000000000000..fa5a777cf727
--- /dev/null
+++ b/drivers/mxc/ipu/pf/Kconfig
@@ -0,0 +1,7 @@
+config MXC_IPU_PF
+	tristate "MXC MPEG4/H.264 Post Filter Driver"
+	depends on MXC_IPU_V1
+    default y
+	help
+        Driver for MPEG4 dering and deblock and H.264 deblock
+        using MXC IPU h/w
diff --git a/drivers/mxc/ipu/pf/Makefile b/drivers/mxc/ipu/pf/Makefile
new file mode 100644
index 000000000000..641adf4be4bd
--- /dev/null
+++ b/drivers/mxc/ipu/pf/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_MXC_IPU_PF) += mxc_pf.o
diff --git a/drivers/mxc/ipu/pf/mxc_pf.c b/drivers/mxc/ipu/pf/mxc_pf.c
new file mode 100644
index 000000000000..744152415e3a
--- /dev/null
+++ b/drivers/mxc/ipu/pf/mxc_pf.c
@@ -0,0 +1,993 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_pf.c
+ *
+ * @brief MXC IPU MPEG4/H.264 Post-filtering driver
+ *
+ * User-level API for IPU Hardware MPEG4/H.264 Post-filtering.
+ *
+ * @ingroup MXC_PF
+ */
+
+#include <linux/pagemap.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/poll.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/ipu.h>
+#include <linux/mxc_pf.h>
+
+struct mxc_pf_data {
+	pf_operation_t mode;
+	u32 pf_enabled;
+	u32 width;
+	u32 height;
+	u32 stride;
+	uint32_t qp_size;
+	dma_addr_t qp_paddr;
+	void *qp_vaddr;
+	pf_buf buf[PF_MAX_BUFFER_CNT];
+	void *buf_vaddr[PF_MAX_BUFFER_CNT];
+	wait_queue_head_t pf_wait;
+	volatile int done_mask;
+	volatile int wait_mask;
+	volatile int busy_flag;
+	struct semaphore busy_lock;
+};
+
+static struct mxc_pf_data pf_data;
+static u8 open_count = 0;
+static struct class *mxc_pf_class;
+
+/*
+ * Function definitions
+ */
+
+static irqreturn_t mxc_pf_irq_handler(int irq, void *dev_id)
+{
+	struct mxc_pf_data *pf = dev_id;
+
+	if (irq == IPU_IRQ_PF_Y_OUT_EOF) {
+		pf->done_mask |= PF_WAIT_Y;
+	} else if (irq == IPU_IRQ_PF_U_OUT_EOF) {
+		pf->done_mask |= PF_WAIT_U;
+	} else if (irq == IPU_IRQ_PF_V_OUT_EOF) {
+		pf->done_mask |= PF_WAIT_V;
+	} else {
+		return IRQ_NONE;
+	}
+
+	if (pf->wait_mask && ((pf->done_mask & pf->wait_mask) == pf->wait_mask)) {
+		wake_up_interruptible(&pf->pf_wait);
+	}
+	return IRQ_HANDLED;
+}
+
+/*!
+ * This function handles PF_IOCTL_INIT calls. It initializes the PF channels,
+ * interrupt handlers, and channel buffers.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              error.
+ */
+static int mxc_pf_init(pf_init_params * pf_init)
+{
+	int err;
+	ipu_channel_params_t params;
+	u32 w;
+	u32 stride;
+	u32 h;
+	u32 qp_size = 0;
+	u32 qp_stride;
+
+	if ((pf_init->pf_mode > 4) || (pf_init->width > 1024) ||
+	    (pf_init->height > 1024) || (pf_init->stride < pf_init->width)) {
+		return -EINVAL;
+	}
+
+	pf_data.mode = pf_init->pf_mode;
+	w = pf_data.width = pf_init->width;
+	h = pf_data.height = pf_init->height;
+	stride = pf_data.stride = pf_init->stride;
+	pf_data.qp_size = pf_init->qp_size;
+
+	memset(&params, 0, sizeof(params));
+	params.mem_pf_mem.operation = pf_data.mode;
+	err = ipu_init_channel(MEM_PF_Y_MEM, &params);
+	if (err < 0) {
+		printk(KERN_ERR "mxc_pf: error initializing channel\n");
+		goto err0;
+	}
+
+	err = ipu_init_channel_buffer(MEM_PF_Y_MEM, IPU_INPUT_BUFFER,
+				      IPU_PIX_FMT_GENERIC, w, h, stride,
+				      IPU_ROTATE_NONE, 0, 0, 0, 0);
+	if (err < 0) {
+		printk(KERN_ERR "mxc_pf: error initializing Y input buffer\n");
+		goto err0;
+	}
+
+	err = ipu_init_channel_buffer(MEM_PF_Y_MEM, IPU_OUTPUT_BUFFER,
+				      IPU_PIX_FMT_GENERIC, w, h, stride,
+				      IPU_ROTATE_NONE, 0, 0, 0, 0);
+	if (err < 0) {
+		printk(KERN_ERR "mxc_pf: error initializing Y output buffer\n");
+		goto err0;
+	}
+
+	w = w / 2;
+	h = h / 2;
+	stride = stride / 2;
+
+	if (pf_data.mode != PF_MPEG4_DERING) {
+		err = ipu_init_channel_buffer(MEM_PF_U_MEM, IPU_INPUT_BUFFER,
+					      IPU_PIX_FMT_GENERIC, w, h, stride,
+					      IPU_ROTATE_NONE, 0, 0, 0, 0);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error initializing U input buffer\n");
+			goto err0;
+		}
+
+		err = ipu_init_channel_buffer(MEM_PF_U_MEM, IPU_OUTPUT_BUFFER,
+					      IPU_PIX_FMT_GENERIC, w, h, stride,
+					      IPU_ROTATE_NONE, 0, 0, 0, 0);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error initializing U output buffer\n");
+			goto err0;
+		}
+
+		err = ipu_init_channel_buffer(MEM_PF_V_MEM, IPU_INPUT_BUFFER,
+					      IPU_PIX_FMT_GENERIC, w, h, stride,
+					      IPU_ROTATE_NONE, 0, 0, 0, 0);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error initializing V input buffer\n");
+			goto err0;
+		}
+
+		err = ipu_init_channel_buffer(MEM_PF_V_MEM, IPU_OUTPUT_BUFFER,
+					      IPU_PIX_FMT_GENERIC, w, h, stride,
+					      IPU_ROTATE_NONE, 0, 0, 0, 0);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error initializing V output buffer\n");
+			goto err0;
+		}
+	}
+
+	/*Setup Channel QF and BSC Params */
+	if (pf_data.mode == PF_H264_DEBLOCK) {
+		w = ((pf_data.width + 15) / 16);
+		h = (pf_data.height + 15) / 16;
+		qp_stride = w;
+		qp_size = 4 * qp_stride * h;
+		pr_debug("H264 QP width = %d, height = %d\n", w, h);
+		err = ipu_init_channel_buffer(MEM_PF_Y_MEM,
+					      IPU_SEC_INPUT_BUFFER,
+					      IPU_PIX_FMT_GENERIC_32, w, h,
+					      qp_stride, IPU_ROTATE_NONE, 0, 0,
+					      0, 0);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error initializing H264 QP buffer\n");
+			goto err0;
+		}
+/*                w = (pf_data.width + 3) / 4; */
+		w *= 4;
+		h *= 4;
+		qp_stride = w;
+		err = ipu_init_channel_buffer(MEM_PF_U_MEM,
+					      IPU_SEC_INPUT_BUFFER,
+					      IPU_PIX_FMT_GENERIC, w, h,
+					      qp_stride, IPU_ROTATE_NONE, 0, 0,
+					      0, 0);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error initializing H264 BSB buffer\n");
+			goto err0;
+		}
+		qp_size += qp_stride * h;
+	} else {		/* MPEG4 mode */
+
+		w = (pf_data.width + 15) / 16;
+		h = (pf_data.height + 15) / 16;
+		qp_stride = (w + 3) & ~0x3UL;
+		pr_debug("MPEG4 QP width = %d, height = %d, stride = %d\n",
+			 w, h, qp_stride);
+		err = ipu_init_channel_buffer(MEM_PF_Y_MEM,
+					      IPU_SEC_INPUT_BUFFER,
+					      IPU_PIX_FMT_GENERIC, w, h,
+					      qp_stride, IPU_ROTATE_NONE, 0, 0,
+					      0, 0);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error initializing MPEG4 QP buffer\n");
+			goto err0;
+		}
+		qp_size = qp_stride * h;
+	}
+
+	/* Support 2 QP buffers */
+	qp_size *= 2;
+
+	if (pf_data.qp_size > qp_size)
+		qp_size = pf_data.qp_size;
+	else
+		pf_data.qp_size = qp_size;
+
+	pf_data.qp_vaddr = dma_alloc_coherent(NULL, pf_data.qp_size,
+					      &pf_data.qp_paddr,
+					      GFP_KERNEL | GFP_DMA);
+	if (!pf_data.qp_vaddr)
+		return -ENOMEM;
+
+	pf_init->qp_paddr = pf_data.qp_paddr;
+	pf_init->qp_size = pf_data.qp_size;
+
+	return 0;
+
+      err0:
+	return err;
+}
+
+/*!
+ * This function handles PF_IOCTL_UNINIT calls. It uninitializes the PF
+ * channels and interrupt handlers.
+ *
+ * @return      This function returns 0 on success or negative error code
+ *              on error.
+ */
+static int mxc_pf_uninit(void)
+{
+	pf_data.pf_enabled = 0;
+	ipu_disable_irq(IPU_IRQ_PF_Y_OUT_EOF);
+	ipu_disable_irq(IPU_IRQ_PF_U_OUT_EOF);
+	ipu_disable_irq(IPU_IRQ_PF_V_OUT_EOF);
+
+	ipu_disable_channel(MEM_PF_Y_MEM, true);
+	ipu_disable_channel(MEM_PF_U_MEM, true);
+	ipu_disable_channel(MEM_PF_V_MEM, true);
+	ipu_uninit_channel(MEM_PF_Y_MEM);
+	ipu_uninit_channel(MEM_PF_U_MEM);
+	ipu_uninit_channel(MEM_PF_V_MEM);
+
+	if (pf_data.qp_vaddr) {
+		dma_free_coherent(NULL, pf_data.qp_size, pf_data.qp_vaddr,
+				  pf_data.qp_paddr);
+		pf_data.qp_vaddr = NULL;
+	}
+
+	return 0;
+}
+
+/*!
+ * This function handles PF_IOCTL_REQBUFS calls. It initializes the PF channels
+ * and channel buffers.
+ *
+ * @param       reqbufs         Input/Output Structure containing buffer mode,
+ *                              type, offset, and size. The offset and size of
+ *                              the buffer are returned for PF_MEMORY_MMAP mode.
+ *
+ * @return      This function returns 0 on success or negative error code
+ *              on error.
+ */
+static int mxc_pf_reqbufs(pf_reqbufs_params * reqbufs)
+{
+	int err;
+	uint32_t buf_size;
+	int i;
+	int alloc_cnt = 0;
+	pf_buf *buf = pf_data.buf;
+	if (reqbufs->count > PF_MAX_BUFFER_CNT) {
+		reqbufs->count = PF_MAX_BUFFER_CNT;
+	}
+	/* Deallocate mmapped buffers */
+	if (reqbufs->count == 0) {
+		for (i = 0; i < PF_MAX_BUFFER_CNT; i++) {
+			if (buf[i].index != -1) {
+				dma_free_coherent(NULL, buf[i].size,
+						  pf_data.buf_vaddr[i],
+						  buf[i].offset);
+				pf_data.buf_vaddr[i] = NULL;
+				buf[i].index = -1;
+				buf[i].size = 0;
+			}
+		}
+		return 0;
+	}
+
+	buf_size = (pf_data.stride * pf_data.height * 3) / 2;
+	if (reqbufs->req_size > buf_size) {
+		buf_size = reqbufs->req_size;
+		pr_debug("using requested buffer size of %d\n", buf_size);
+	} else {
+		reqbufs->req_size = buf_size;
+		pr_debug("using default buffer size of %d\n", buf_size);
+	}
+
+	for (i = 0; alloc_cnt < reqbufs->count; i++) {
+		buf[i].index = i;
+		buf[i].size = buf_size;
+		pf_data.buf_vaddr[i] = dma_alloc_coherent(NULL, buf[i].size,
+							  &buf[i].offset,
+							  GFP_KERNEL | GFP_DMA);
+		if (!pf_data.buf_vaddr[i] || !buf[i].offset) {
+			printk(KERN_ERR
+			       "mxc_pf: unable to allocate IPU buffers.\n");
+			err = -ENOMEM;
+			goto err0;
+		}
+		pr_debug("Allocated buffer %d at paddr 0x%08X, vaddr %p\n",
+			 i, buf[i].offset, pf_data.buf_vaddr[i]);
+
+		alloc_cnt++;
+	}
+
+	return 0;
+      err0:
+	for (i = 0; i < alloc_cnt; i++) {
+		dma_free_coherent(NULL, buf[i].size, pf_data.buf_vaddr[i],
+				  buf[i].offset);
+		pf_data.buf_vaddr[i] = NULL;
+		buf[i].index = -1;
+		buf[i].size = 0;
+	}
+	return err;
+}
+
+/*!
+ * This function handles PF_IOCTL_START calls. It sets the PF channel buffers
+ * addresses and starts the channels
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              error.
+ */
+static int mxc_pf_start(pf_buf * in, pf_buf * out, int qp_buf)
+{
+	int err;
+	dma_addr_t y_in_paddr;
+	dma_addr_t u_in_paddr;
+	dma_addr_t v_in_paddr;
+	dma_addr_t p1_in_paddr;
+	dma_addr_t p2_in_paddr;
+	dma_addr_t y_out_paddr;
+	dma_addr_t u_out_paddr;
+	dma_addr_t v_out_paddr;
+
+	/* H.264 requires output buffer equal to input */
+	if (pf_data.mode == PF_H264_DEBLOCK)
+		out = in;
+
+	y_in_paddr = in->offset + in->y_offset;
+	if (in->u_offset)
+		u_in_paddr = in->offset + in->u_offset;
+	else
+		u_in_paddr = y_in_paddr + (pf_data.stride * pf_data.height);
+	if (in->v_offset)
+		v_in_paddr = in->offset + in->v_offset;
+	else
+		v_in_paddr = u_in_paddr + (pf_data.stride * pf_data.height) / 4;
+	p1_in_paddr = pf_data.qp_paddr;
+	if (qp_buf)
+		p1_in_paddr += pf_data.qp_size / 2;
+
+	if (pf_data.mode == PF_H264_DEBLOCK) {
+		p2_in_paddr = p1_in_paddr +
+		    ((pf_data.width + 15) / 16) *
+		    ((pf_data.height + 15) / 16) * 4;
+	} else {
+		p2_in_paddr = 0;
+	}
+
+	pr_debug("y_in_paddr = 0x%08X\nu_in_paddr = 0x%08X\n"
+		 "v_in_paddr = 0x%08X\n"
+		 "qp_paddr = 0x%08X\nbsb_paddr = 0x%08X\n",
+		 y_in_paddr, u_in_paddr, v_in_paddr, p1_in_paddr, p2_in_paddr);
+
+	y_out_paddr = out->offset + out->y_offset;
+	if (out->u_offset)
+		u_out_paddr = out->offset + out->u_offset;
+	else
+		u_out_paddr = y_out_paddr + (pf_data.stride * pf_data.height);
+	if (out->v_offset)
+		v_out_paddr = out->offset + out->v_offset;
+	else
+		v_out_paddr =
+		    u_out_paddr + (pf_data.stride * pf_data.height) / 4;
+
+	pr_debug("y_out_paddr = 0x%08X\nu_out_paddr = 0x%08X\n"
+		 "v_out_paddr = 0x%08X\n",
+		 y_out_paddr, u_out_paddr, v_out_paddr);
+
+	pf_data.done_mask = 0;
+
+	ipu_enable_irq(IPU_IRQ_PF_Y_OUT_EOF);
+	if (pf_data.mode != PF_MPEG4_DERING) {
+		ipu_enable_irq(IPU_IRQ_PF_U_OUT_EOF);
+		ipu_enable_irq(IPU_IRQ_PF_V_OUT_EOF);
+	}
+
+	err = ipu_update_channel_buffer(MEM_PF_Y_MEM, IPU_INPUT_BUFFER, 0,
+					y_in_paddr);
+	if (err < 0) {
+		printk(KERN_ERR "mxc_pf: error setting Y input buffer\n");
+		goto err0;
+	}
+
+	err = ipu_update_channel_buffer(MEM_PF_Y_MEM, IPU_OUTPUT_BUFFER, 0,
+					y_out_paddr);
+	if (err < 0) {
+		printk(KERN_ERR "mxc_pf: error setting Y output buffer\n");
+		goto err0;
+	}
+
+	if (pf_data.mode != PF_MPEG4_DERING) {
+		err =
+		    ipu_update_channel_buffer(MEM_PF_U_MEM, IPU_INPUT_BUFFER, 0,
+					      u_in_paddr);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error setting U input buffer\n");
+			goto err0;
+		}
+
+		err =
+		    ipu_update_channel_buffer(MEM_PF_U_MEM, IPU_OUTPUT_BUFFER,
+					      0, u_out_paddr);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error setting U output buffer\n");
+			goto err0;
+		}
+
+		err =
+		    ipu_update_channel_buffer(MEM_PF_V_MEM, IPU_INPUT_BUFFER, 0,
+					      v_in_paddr);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error setting V input buffer\n");
+			goto err0;
+		}
+
+		err =
+		    ipu_update_channel_buffer(MEM_PF_V_MEM, IPU_OUTPUT_BUFFER,
+					      0, v_out_paddr);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error setting V output buffer\n");
+			goto err0;
+		}
+	}
+
+	err = ipu_update_channel_buffer(MEM_PF_Y_MEM, IPU_SEC_INPUT_BUFFER, 0,
+					p1_in_paddr);
+	if (err < 0) {
+		printk(KERN_ERR "mxc_pf: error setting QP buffer\n");
+		goto err0;
+	}
+
+	if (pf_data.mode == PF_H264_DEBLOCK) {
+
+		err = ipu_update_channel_buffer(MEM_PF_U_MEM,
+						IPU_SEC_INPUT_BUFFER, 0,
+						p2_in_paddr);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "mxc_pf: error setting H264 BSB buffer\n");
+			goto err0;
+		}
+		ipu_select_buffer(MEM_PF_U_MEM, IPU_SEC_INPUT_BUFFER, 0);
+	}
+
+	ipu_select_buffer(MEM_PF_Y_MEM, IPU_OUTPUT_BUFFER, 0);
+	ipu_select_buffer(MEM_PF_Y_MEM, IPU_SEC_INPUT_BUFFER, 0);
+	ipu_select_buffer(MEM_PF_Y_MEM, IPU_INPUT_BUFFER, 0);
+	if (pf_data.mode != PF_MPEG4_DERING) {
+		ipu_select_buffer(MEM_PF_U_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_PF_V_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_PF_U_MEM, IPU_INPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_PF_V_MEM, IPU_INPUT_BUFFER, 0);
+	}
+
+	if (!pf_data.pf_enabled) {
+		pf_data.pf_enabled = 1;
+		if (pf_data.mode != PF_MPEG4_DERING) {
+			ipu_enable_channel(MEM_PF_V_MEM);
+			ipu_enable_channel(MEM_PF_U_MEM);
+		}
+		ipu_enable_channel(MEM_PF_Y_MEM);
+	}
+
+	return 0;
+      err0:
+	return err;
+}
+
+/*!
+ * Post Filter driver open function. This function implements the Linux
+ * file_operations.open() API function.
+ *
+ * @param       inode           struct inode *
+ *
+ * @param       filp            struct file *
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              error.
+ */
+static int mxc_pf_open(struct inode *inode, struct file *filp)
+{
+	int i;
+
+	if (open_count) {
+		return -EBUSY;
+	}
+
+	open_count++;
+
+	memset(&pf_data, 0, sizeof(pf_data));
+	for (i = 0; i < PF_MAX_BUFFER_CNT; i++) {
+		pf_data.buf[i].index = -1;
+	}
+	init_waitqueue_head(&pf_data.pf_wait);
+	init_MUTEX(&pf_data.busy_lock);
+
+	pf_data.busy_flag = 1;
+
+	ipu_request_irq(IPU_IRQ_PF_Y_OUT_EOF, mxc_pf_irq_handler,
+			0, "mxc_ipu_pf", &pf_data);
+
+	ipu_request_irq(IPU_IRQ_PF_U_OUT_EOF, mxc_pf_irq_handler,
+			0, "mxc_ipu_pf", &pf_data);
+
+	ipu_request_irq(IPU_IRQ_PF_V_OUT_EOF, mxc_pf_irq_handler,
+			0, "mxc_ipu_pf", &pf_data);
+
+	ipu_disable_irq(IPU_IRQ_PF_Y_OUT_EOF);
+	ipu_disable_irq(IPU_IRQ_PF_U_OUT_EOF);
+	ipu_disable_irq(IPU_IRQ_PF_V_OUT_EOF);
+
+	return 0;
+}
+
+/*!
+ * Post Filter driver release function. This function implements the Linux
+ * file_operations.release() API function.
+ *
+ * @param       inode           struct inode *
+ *
+ * @param       filp            struct file *
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              error.
+ */
+static int mxc_pf_release(struct inode *inode, struct file *filp)
+{
+	pf_reqbufs_params req_buf;
+
+	if (open_count) {
+		mxc_pf_uninit();
+
+		/* Free any allocated buffers */
+		req_buf.count = 0;
+		mxc_pf_reqbufs(&req_buf);
+
+		ipu_free_irq(IPU_IRQ_PF_V_OUT_EOF, &pf_data);
+		ipu_free_irq(IPU_IRQ_PF_U_OUT_EOF, &pf_data);
+		ipu_free_irq(IPU_IRQ_PF_Y_OUT_EOF, &pf_data);
+		open_count--;
+	}
+	return 0;
+}
+
+/*!
+ * Post Filter driver ioctl function. This function implements the Linux
+ * file_operations.ioctl() API function.
+ *
+ * @param       inode           struct inode *
+ *
+ * @param       filp            struct file *
+ *
+ * @param       cmd             IOCTL command to handle
+ *
+ * @param       arg             Pointer to arguments for IOCTL
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              error.
+ */
+static int mxc_pf_ioctl(struct inode *inode, struct file *filp,
+			unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+
+	switch (cmd) {
+	case PF_IOCTL_INIT:
+		{
+			pf_init_params pf_init;
+
+			pr_debug("PF_IOCTL_INIT\n");
+			if (copy_from_user(&pf_init, (void *)arg,
+					   _IOC_SIZE(cmd))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			retval = mxc_pf_init(&pf_init);
+			if (retval < 0)
+				break;
+			pf_init.qp_paddr = pf_data.qp_paddr;
+			pf_init.qp_size = pf_data.qp_size;
+
+			/* Return size of memory allocated */
+			if (copy_to_user((void *)arg, &pf_init, _IOC_SIZE(cmd))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			pf_data.busy_flag = 0;
+			break;
+		}
+	case PF_IOCTL_UNINIT:
+		pr_debug("PF_IOCTL_UNINIT\n");
+		retval = mxc_pf_uninit();
+		break;
+	case PF_IOCTL_REQBUFS:
+		{
+			pf_reqbufs_params reqbufs;
+			pr_debug("PF_IOCTL_REQBUFS\n");
+
+			if (copy_from_user
+			    (&reqbufs, (void *)arg, _IOC_SIZE(cmd))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			retval = mxc_pf_reqbufs(&reqbufs);
+
+			/* Return size of memory allocated */
+			if (copy_to_user((void *)arg, &reqbufs, _IOC_SIZE(cmd))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			break;
+		}
+	case PF_IOCTL_QUERYBUF:
+		{
+			pf_buf buf;
+			pr_debug("PF_IOCTL_QUERYBUF\n");
+
+			if (copy_from_user(&buf, (void *)arg, _IOC_SIZE(cmd))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			if ((buf.index < 0) ||
+			    (buf.index >= PF_MAX_BUFFER_CNT) ||
+			    (pf_data.buf[buf.index].index != buf.index)) {
+				retval = -EINVAL;
+				break;
+			}
+			/* Return size of memory allocated */
+			if (copy_to_user((void *)arg, &pf_data.buf[buf.index],
+					 _IOC_SIZE(cmd))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			break;
+		}
+	case PF_IOCTL_START:
+		{
+			int index;
+			pf_start_params start_params;
+			pr_debug("PF_IOCTL_START\n");
+
+			if (pf_data.busy_flag) {
+				retval = -EBUSY;
+				break;
+			}
+
+			if (copy_from_user(&start_params, (void *)arg,
+					   _IOC_SIZE(cmd))) {
+				retval = -EFAULT;
+				break;
+			}
+			if (start_params.h264_pause_row >=
+			    ((pf_data.height + 15) / 16)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			pf_data.busy_flag = 1;
+
+			index = start_params.in.index;
+			if ((index >= 0) && (index < PF_MAX_BUFFER_CNT)) {
+				if (pf_data.buf[index].offset !=
+				    start_params.in.offset) {
+					retval = -EINVAL;
+					break;
+				}
+			}
+
+			index = start_params.out.index;
+			if ((index >= 0) && (index < PF_MAX_BUFFER_CNT)) {
+				if (pf_data.buf[index].offset !=
+				    start_params.out.offset) {
+					retval = -EINVAL;
+					break;
+				}
+			}
+
+			ipu_pf_set_pause_row(start_params.h264_pause_row);
+
+			/*Update y, u, v buffers in DMA Channels */
+			if ((retval =
+			     mxc_pf_start(&start_params.in, &start_params.out,
+					  start_params.qp_buf))
+			    < 0) {
+				break;
+			}
+
+			pr_debug("PF_IOCTL_START - processing started\n");
+
+			if (!start_params.wait) {
+				break;
+			}
+
+			pr_debug("PF_IOCTL_START - waiting for completion\n");
+
+			pf_data.wait_mask = PF_WAIT_ALL;
+			/* Fall thru to wait */
+		}
+	case PF_IOCTL_WAIT:
+		{
+			if (!pf_data.wait_mask)
+				pf_data.wait_mask = (u32) arg;
+
+			if (pf_data.mode == PF_MPEG4_DERING)
+				pf_data.wait_mask &= PF_WAIT_Y;
+
+			if (!pf_data.wait_mask) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (!wait_event_interruptible_timeout(pf_data.pf_wait,
+							      ((pf_data.
+								done_mask &
+								pf_data.
+								wait_mask) ==
+							       pf_data.
+							       wait_mask),
+							      1 * HZ)) {
+				pr_debug
+				    ("PF_IOCTL_WAIT: timeout, done_mask = %d\n",
+				     pf_data.done_mask);
+				retval = -ETIME;
+				break;
+			} else if (signal_pending(current)) {
+				pr_debug("PF_IOCTL_WAIT: interrupt received\n");
+				retval = -ERESTARTSYS;
+				break;
+			}
+			pf_data.busy_flag = 0;
+			pf_data.wait_mask = 0;
+
+			pr_debug("PF_IOCTL_WAIT - finished\n");
+			break;
+		}
+	case PF_IOCTL_RESUME:
+		{
+			int pause_row;
+			pr_debug("PF_IOCTL_RESUME\n");
+
+			if (pf_data.busy_flag == 0) {
+				retval = -EFAULT;
+				break;
+			}
+
+			if (copy_from_user(&pause_row, (void *)arg,
+					   _IOC_SIZE(cmd))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			if (pause_row >= ((pf_data.height + 15) / 16)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			ipu_pf_set_pause_row(pause_row);
+			break;
+		}
+
+	default:
+		printk(KERN_ERR "ipu_pf_ioctl not supported ioctl\n");
+		retval = -1;
+	}
+
+	if (retval < 0)
+		pr_debug("return = %d\n", retval);
+	return retval;
+}
+
+/*!
+ * Post Filter driver mmap function. This function implements the Linux
+ * file_operations.mmap() API function for mapping driver buffers to user space.
+ *
+ * @param       file            struct file *
+ *
+ * @param       vma             structure vm_area_struct *
+ *
+ * @return      0 Success, EINTR busy lock error,
+ *                      ENOBUFS remap_page error.
+ */
+static int mxc_pf_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	unsigned long size = vma->vm_end - vma->vm_start;
+	int res = 0;
+
+	pr_debug("pgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		 vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&pf_data.busy_lock))
+		return -EINTR;
+
+	/* make buffers write-thru cacheable */
+	vma->vm_page_prot = __pgprot(pgprot_val(vma->vm_page_prot) &
+				     ~L_PTE_BUFFERABLE);
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		printk(KERN_ERR "mxc_pf: remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+      mmap_exit:
+	up(&pf_data.busy_lock);
+	return res;
+}
+
+/*!
+ * Post Filter driver fsync function. This function implements the Linux
+ * file_operations.fsync() API function.
+ *
+ * The user must call fsync() before reading an output buffer. This
+ * call flushes the L1 and L2 caches
+ *
+ * @param filp          structure file *
+ *
+ * @param dentry        struct dentry *
+ *
+ * @param datasync      unused
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+int mxc_pf_fsync(struct file *filp, struct dentry *dentry, int datasync)
+{
+	flush_cache_all();
+	outer_flush_all();
+	return 0;
+}
+
+/*!
+ * Post Filter driver poll function. This function implements the Linux
+ * file_operations.poll() API function.
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_pf_poll(struct file *file, poll_table * wait)
+{
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	if (down_interruptible(&pf_data.busy_lock))
+		return -EINTR;
+
+	queue = &pf_data.pf_wait;
+	poll_wait(file, queue, wait);
+
+	up(&pf_data.busy_lock);
+
+	return res;
+}
+
+/*!
+ * File operation structure functions pointers.
+ */
+static struct file_operations mxc_pf_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_pf_open,
+	.release = mxc_pf_release,
+	.ioctl = mxc_pf_ioctl,
+	.poll = mxc_pf_poll,
+	.mmap = mxc_pf_mmap,
+	.fsync = mxc_pf_fsync,
+};
+
+static int mxc_pf_major = 0;
+
+/*!
+ * Post Filter driver module initialization function.
+ */
+int mxc_pf_dev_init(void)
+{
+	int ret = 0;
+	struct device *temp_class;
+
+	mxc_pf_major = register_chrdev(0, "mxc_ipu_pf", &mxc_pf_fops);
+
+	if (mxc_pf_major < 0) {
+		printk(KERN_INFO "Unable to get a major for mxc_ipu_pf");
+		return mxc_pf_major;
+	}
+
+	mxc_pf_class = class_create(THIS_MODULE, "mxc_ipu_pf");
+	if (IS_ERR(mxc_pf_class)) {
+		printk(KERN_ERR "Error creating mxc_ipu_pf class.\n");
+		ret = PTR_ERR(mxc_pf_class);
+		goto err_out1;
+	}
+
+	temp_class = device_create(mxc_pf_class, NULL, MKDEV(mxc_pf_major, 0), NULL,
+				   "mxc_ipu_pf");
+	if (IS_ERR(temp_class)) {
+		printk(KERN_ERR "Error creating mxc_ipu_pf class device.\n");
+		ret = PTR_ERR(temp_class);
+		goto err_out2;
+	}
+
+	printk(KERN_INFO "IPU Post-filter loading\n");
+
+	return 0;
+
+      err_out2:
+	class_destroy(mxc_pf_class);
+      err_out1:
+	unregister_chrdev(mxc_pf_major, "mxc_ipu_pf");
+	return ret;
+}
+
+/*!
+ * Post Filter driver module exit function.
+ */
+static void mxc_pf_exit(void)
+{
+	if (mxc_pf_major > 0) {
+		device_destroy(mxc_pf_class, MKDEV(mxc_pf_major, 0));
+		class_destroy(mxc_pf_class);
+		unregister_chrdev(mxc_pf_major, "mxc_ipu_pf");
+	}
+}
+
+module_init(mxc_pf_dev_init);
+module_exit(mxc_pf_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC MPEG4/H.264 Postfilter Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/ipu3/Kconfig b/drivers/mxc/ipu3/Kconfig
new file mode 100644
index 000000000000..0ae0ffa9e19d
--- /dev/null
+++ b/drivers/mxc/ipu3/Kconfig
@@ -0,0 +1,5 @@
+config MXC_IPU_V3
+	bool
+
+config MXC_IPU_V3D
+	bool
diff --git a/drivers/mxc/ipu3/Makefile b/drivers/mxc/ipu3/Makefile
new file mode 100644
index 000000000000..aa3e7b1bb501
--- /dev/null
+++ b/drivers/mxc/ipu3/Makefile
@@ -0,0 +1,4 @@
+obj-$(CONFIG_MXC_IPU_V3) = mxc_ipu.o
+
+mxc_ipu-objs := ipu_common.o ipu_ic.o ipu_disp.o ipu_capture.o ipu_device.o ipu_calc_stripes_sizes.o
+
diff --git a/drivers/mxc/ipu3/ipu_calc_stripes_sizes.c b/drivers/mxc/ipu3/ipu_calc_stripes_sizes.c
new file mode 100644
index 000000000000..2992cce7023d
--- /dev/null
+++ b/drivers/mxc/ipu3/ipu_calc_stripes_sizes.c
@@ -0,0 +1,375 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * @file ipu_calc_stripes_sizes.c
+ *
+ * @brief IPU IC functions
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/ipu.h>
+#include <asm/div64.h>
+
+#define BPP_32 0
+#define BPP_16 3
+#define BPP_8 5
+#define BPP_24 1
+#define BPP_12 4
+#define BPP_18 2
+
+static u64 _do_div(u64 a, u32 b)
+{
+	u64 div;
+	div = a;
+	do_div(div, b);
+	return div;
+}
+
+static u32 truncate(u32 up, /* 0: down; else: up */
+					u64 a, /* must be non-negative */
+					u32 b)
+{
+	u32 d;
+	u64 div;
+	div = _do_div(a, b);
+	d = b * (div >> 32);
+	if (up && (a > (((u64)d) << 32)))
+		return d+b;
+	else
+		return d;
+}
+
+static unsigned int f_calc(unsigned int pfs, unsigned int bpp, unsigned int *write)
+{/* return input_f */
+	unsigned int f_calculated = 0;
+	switch (pfs) {
+	case IPU_PIX_FMT_YVU422P:
+	case IPU_PIX_FMT_YUV422P:
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YUV420P:
+		f_calculated = 16;
+		break;
+
+	case IPU_PIX_FMT_NV12:
+		f_calculated = 8;
+		break;
+
+	default:
+		f_calculated = 0;
+		break;
+
+	}
+	if (!f_calculated) {
+		switch (bpp) {
+		case BPP_32:
+			f_calculated = 2;
+			break;
+
+		case BPP_16:
+			f_calculated = 4;
+			break;
+
+		case BPP_8:
+		case BPP_24:
+			f_calculated = 8;
+			break;
+
+		case BPP_12:
+			f_calculated = 16;
+			break;
+
+		case BPP_18:
+			f_calculated = 32;
+			break;
+
+		default:
+			f_calculated = 0;
+			break;
+			}
+		}
+	return f_calculated;
+}
+
+
+static unsigned int m_calc(unsigned int pfs)
+{
+	unsigned int m_calculated = 0;
+	switch (pfs) {
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YUV420P:
+	case IPU_PIX_FMT_YVU422P:
+	case IPU_PIX_FMT_YUV422P:
+	case IPU_PIX_FMT_YVU420P:
+	case IPU_PIX_FMT_NV12:
+		m_calculated = 8;
+		break;
+
+	case IPU_PIX_FMT_YUYV:
+	case IPU_PIX_FMT_UYVY:
+		m_calculated = 2;
+		break;
+
+	default:
+		m_calculated = 1;
+		break;
+
+	}
+	return m_calculated;
+}
+
+
+/* Stripe parameters calculator */
+/**************************************************************************
+Notes:
+MSW = the maximal width allowed for a stripe
+	i.MX31: 720, i.MX35: 800, i.MX37/51/53: 1024
+cirr = the maximal inverse resizing ratio for which overlap in the input
+	is requested; typically cirr~2
+equal_stripes:
+	0: each stripe is allowed to have independent parameters
+		for maximal image quality
+	1: the stripes are requested to have identical parameters
+	(except the base address), for maximal performance
+If performance is the top priority (above image quality)
+	Avoid overlap, by setting CIRR = 0
+		This will also force effectively identical_stripes = 1
+	Choose IF & OF that corresponds to the same IOX/SX for both stripes
+	Choose IFW & OFW such that
+	IFW/IM, IFW/IF, OFW/OM, OFW/OF are even integers
+	The function returns an error status:
+	0: no error
+	1: invalid input parameters -> aborted without result
+		Valid parameters should satisfy the following conditions
+		IFW <= OFW, otherwise downsizing is required
+					 - which is not supported yet
+		4 <= IFW,OFW, so some interpolation may be needed even without overlap
+		IM, OM, IF, OF should not vanish
+		2*IF <= IFW
+		so the frame can be split to two equal stripes, even without overlap
+		2*(OF+IF/irr_opt) <= OFW
+		so a valid positive INW exists even for equal stripes
+		OF <= MSW, otherwise, the left stripe cannot be sufficiently large
+		MSW < OFW, so splitting to stripes is required
+		OFW <= 2*MSW, so two stripes are sufficient
+		(this also implies that 2<=MSW)
+	2: OF is not a multiple of OM - not fully-supported yet
+	Output is produced but OW is not guaranited to be a multiple of OM
+	4: OFW reduced to be a multiple of OM
+	8: CIRR > 1: truncated to 1
+	Overlap is not supported (and not needed) y for upsizing)
+**************************************************************************/
+int ipu_calc_stripes_sizes(const unsigned int input_frame_width,
+			   /* input frame width;>1 */
+			   unsigned int output_frame_width, /* output frame width; >1 */
+			   const unsigned int maximal_stripe_width,
+			   /* the maximal width allowed for a stripe */
+			   const unsigned long long cirr, /* see above */
+			   const unsigned int equal_stripes, /* see above */
+			   u32 input_pixelformat,/* pixel format after of read channel*/
+			   u32 output_pixelformat,/* pixel format after of write channel*/
+			   struct stripe_param *left,
+			   struct stripe_param *right)
+{
+	const unsigned int irr_frac_bits = 13;
+	const unsigned long irr_steps = 1 << irr_frac_bits;
+	const u64 dirr = ((u64)1) << (32 - 2);
+	/* The maximum relative difference allowed between the irrs */
+	const u64 cr = ((u64)4) << 32;
+	/* The importance ratio between the two terms in the cost function below */
+
+	unsigned int status;
+	unsigned int temp;
+	unsigned int onw_min;
+	unsigned int inw, onw, inw_best;
+	/* number of pixels in the left stripe NOT hidden by the right stripe */
+	u64 irr_opt; /* the optimal inverse resizing ratio */
+	u64 rr_opt; /* the optimal resizing ratio = 1/irr_opt*/
+	u64 dinw; /* the misalignment between the stripes */
+	/* (measured in units of input columns) */
+	u64 difwl, difwr;
+	/* The number of input columns not reflected in the output */
+	/* the resizing ratio used for the right stripe is */
+	/*   left->irr and right->irr respectively */
+	u64 cost, cost_min;
+	u64 div; /* result of division */
+
+	unsigned int input_m, input_f, output_m, output_f; /* parameters for upsizing by stripes */
+	u32 in_pfs, out_pfs;
+
+	status = 0;
+
+	/* M, F calculations */
+	/* read back pfs from params */
+
+	input_f = f_calc(input_pixelformat, 0, NULL);
+	/*in_m = m_calc(in_pfs);*/
+	input_m = 16;
+	/* BPP should be used in the out_F calc */
+	/* Temporarily not used */
+	/* out_F = F_calc(idmac->pfs, idmac->bpp, NULL); */
+
+	output_f = 16;
+	output_m = m_calc(output_pixelformat);
+
+
+	if ((output_frame_width < input_frame_width) || (input_frame_width < 4)
+	    || (output_frame_width < 4))
+		return 1;
+
+	irr_opt = _do_div((((u64)(input_frame_width - 1)) << 32),
+			  (output_frame_width - 1));
+	rr_opt = _do_div((((u64)(output_frame_width - 1)) << 32),
+			 (input_frame_width - 1));
+
+	if ((input_m == 0) || (output_m == 0) || (input_f == 0) || (output_f == 0)
+	    || (input_frame_width < (2 * input_f))
+	    || ((((u64)output_frame_width) << 32) <
+		(2 * ((((u64)output_f) << 32) + (input_f * rr_opt))))
+	    || (maximal_stripe_width < output_f)
+	    || (output_frame_width <= maximal_stripe_width)
+	    || ((2 * maximal_stripe_width) < output_frame_width))
+		return 1;
+
+	if (output_f % output_m)
+		status += 2;
+
+	temp = truncate(0, (((u64)output_frame_width) << 32), output_m);
+	if (temp < output_frame_width) {
+		output_frame_width = temp;
+		status += 4;
+	}
+
+	if (equal_stripes) {
+		if ((irr_opt > cirr) /* overlap in the input is not requested */
+		    && ((input_frame_width % (input_m << 1)) == 0)
+		    && ((input_frame_width % (input_f << 1)) == 0)
+		    && ((output_frame_width % (output_m << 1)) == 0)
+		    && ((output_frame_width % (output_f << 1)) == 0)) {
+			/* without overlap */
+			left->input_width = right->input_width = right->input_column =
+				input_frame_width >> 1;
+			left->output_width = right->output_width = right->output_column =
+				output_frame_width >> 1;
+			left->input_column = right->input_column = 0;
+			div = _do_div(((((u64)irr_steps) << 32) *
+				       (right->input_width - 1)), (right->output_width - 1));
+			left->irr = right->irr = truncate(0, div, 1);
+		} else { /* with overlap */
+			onw = truncate(0, (((u64)output_frame_width) << 32) >> 1,
+				       output_f);
+			inw = truncate(0, onw * irr_opt, input_f);
+			/* this is the maximal inw which allows the same resizing ratio */
+			/* in both stripes */
+			onw = truncate(1, (inw * rr_opt), output_f);
+			div = _do_div((((u64)(irr_steps * inw)) <<
+				       32), onw);
+			left->irr = right->irr = truncate(0, div, 1);
+			left->output_width = right->output_width =
+				output_frame_width - onw;
+			/* These are valid assignments for output_width, */
+			/* assuming output_f is a multiple of output_m */
+			div = (((u64)(left->output_width-1) * (left->irr)) << 32);
+			div = (((u64)1) << 32) + _do_div(div, irr_steps);
+
+			left->input_width = right->input_width = truncate(1, div, input_m);
+
+			div = _do_div((((u64)((right->output_width - 1) * right->irr)) <<
+				       32), irr_steps);
+			difwr = (((u64)(input_frame_width - 1 - inw)) << 32) - div;
+			div = _do_div((difwr + (((u64)input_f) << 32)), 2);
+			left->input_column = truncate(0, div, input_f);
+
+
+			/* This splits the truncated input columns evenly */
+			/*    between the left and right margins */
+			right->input_column = left->input_column + inw;
+			left->output_column = 0;
+			right->output_column = onw;
+		}
+	} else { /* independent stripes */
+		onw_min = output_frame_width - maximal_stripe_width;
+		/* onw is a multiple of output_f, in the range */
+		/* [max(output_f,output_frame_width-maximal_stripe_width),*/
+		/*min(output_frame_width-2,maximal_stripe_width)] */
+		/* definitely beyond the cost of any valid setting */
+		cost_min = (((u64)input_frame_width) << 32) + cr;
+		onw = truncate(0, ((u64)maximal_stripe_width), output_f);
+		if (output_frame_width - onw == 1)
+			onw -= output_f; /*  => onw and output_frame_width-1-onw are positive */
+		inw = truncate(0, onw * irr_opt, input_f);
+		/* this is the maximal inw which allows the same resizing ratio */
+		/* in both stripes */
+		onw = truncate(1, inw * rr_opt, output_f);
+		do {
+			div = _do_div((((u64)(irr_steps * inw)) << 32), onw);
+			left->irr = truncate(0, div, 1);
+			div = _do_div((((u64)(onw * left->irr)) << 32),
+				      irr_steps);
+			dinw = (((u64)inw) << 32) - div;
+
+			div = _do_div((((u64)((output_frame_width - 1 - onw) * left->irr)) <<
+				       32), irr_steps);
+
+			difwl = (((u64)(input_frame_width - 1 - inw)) << 32) - div;
+
+			cost = difwl + (((u64)(cr * dinw)) >> 32);
+
+			if (cost < cost_min) {
+				inw_best = inw;
+				cost_min = cost;
+			}
+
+			inw -= input_f;
+			onw = truncate(1, inw * rr_opt, output_f);
+			/* This is the minimal onw which allows the same resizing ratio */
+			/*     in both stripes */
+		} while (onw >= onw_min);
+
+		inw = inw_best;
+		onw = truncate(1, inw * rr_opt, output_f);
+		div = _do_div((((u64)(irr_steps * inw)) << 32), onw);
+		left->irr = truncate(0, div, 1);
+
+		left->output_width = onw;
+		right->output_width = output_frame_width - onw;
+		/* These are valid assignments for output_width, */
+		/* assuming output_f is a multiple of output_m */
+		left->input_width = truncate(1, ((u64)(inw + 1)) << 32, input_m);
+		right->input_width = truncate(1, ((u64)(input_frame_width - inw)) <<
+					      32, input_m);
+
+		div = _do_div((((u64)(irr_steps * (input_frame_width - 1 - inw))) <<
+			       32), (right->output_width - 1));
+		right->irr = truncate(0, div, 1);
+		temp = truncate(0, ((u64)left->irr) * ((((u64)1) << 32) + dirr), 1);
+		if (temp < right->irr)
+			right->irr = temp;
+		div = _do_div(((u64)((right->output_width - 1) * right->irr) <<
+			       32), irr_steps);
+		difwr = (u64)(input_frame_width - 1 - inw) - div;
+
+
+		div = _do_div((difwr + (((u64)input_f) << 32)), 2);
+		left->input_column = truncate(0, div, input_f);
+
+		/* This splits the truncated input columns evenly */
+		/*    between the left and right margins */
+		right->input_column = left->input_column + inw;
+		left->output_column = 0;
+		right->output_column = onw;
+	}
+	return status;
+}
+EXPORT_SYMBOL(ipu_calc_stripes_sizes);
diff --git a/drivers/mxc/ipu3/ipu_capture.c b/drivers/mxc/ipu3/ipu_capture.c
new file mode 100755
index 000000000000..256fb9aa17e5
--- /dev/null
+++ b/drivers/mxc/ipu3/ipu_capture.c
@@ -0,0 +1,711 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_capture.c
+ *
+ * @brief IPU capture dase functions
+ *
+ * @ingroup IPU
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/ipu.h>
+#include <linux/clk.h>
+
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+
+/*!
+ * ipu_csi_init_interface
+ *	Sets initial values for the CSI registers.
+ *	The width and height of the sensor and the actual frame size will be
+ *	set to the same values.
+ * @param	width		Sensor width
+ * @param       height		Sensor height
+ * @param       pixel_fmt	pixel format
+ * @param       cfg_param	ipu_csi_signal_cfg_t structure
+ * @param       csi             csi 0 or csi 1
+ *
+ * @return      0 for success, -EINVAL for error
+ */
+int32_t
+ipu_csi_init_interface(uint16_t width, uint16_t height, uint32_t pixel_fmt,
+	ipu_csi_signal_cfg_t cfg_param)
+{
+	uint32_t data = 0;
+	uint32_t csi = cfg_param.csi;
+	unsigned long lock_flags;
+
+	/* Set SENS_DATA_FORMAT bits (8, 9 and 10)
+	   RGB or YUV444 is 0 which is current value in data so not set
+	   explicitly
+	   This is also the default value if attempts are made to set it to
+	   something invalid. */
+	switch (pixel_fmt) {
+	case IPU_PIX_FMT_YUYV:
+		cfg_param.data_fmt = CSI_SENS_CONF_DATA_FMT_YUV422_YUYV;
+		break;
+	case IPU_PIX_FMT_UYVY:
+		cfg_param.data_fmt = CSI_SENS_CONF_DATA_FMT_YUV422_UYVY;
+		break;
+	case IPU_PIX_FMT_RGB24:
+	case IPU_PIX_FMT_BGR24:
+		cfg_param.data_fmt = CSI_SENS_CONF_DATA_FMT_RGB_YUV444;
+		break;
+	case IPU_PIX_FMT_GENERIC:
+		cfg_param.data_fmt = CSI_SENS_CONF_DATA_FMT_BAYER;
+		break;
+	case IPU_PIX_FMT_RGB565:
+		cfg_param.data_fmt = CSI_SENS_CONF_DATA_FMT_RGB565;
+		break;
+	case IPU_PIX_FMT_RGB555:
+		cfg_param.data_fmt = CSI_SENS_CONF_DATA_FMT_RGB555;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Set the CSI_SENS_CONF register remaining fields */
+	data |= cfg_param.data_width << CSI_SENS_CONF_DATA_WIDTH_SHIFT |
+		cfg_param.data_fmt << CSI_SENS_CONF_DATA_FMT_SHIFT |
+		cfg_param.data_pol << CSI_SENS_CONF_DATA_POL_SHIFT |
+		cfg_param.Vsync_pol << CSI_SENS_CONF_VSYNC_POL_SHIFT |
+		cfg_param.Hsync_pol << CSI_SENS_CONF_HSYNC_POL_SHIFT |
+		cfg_param.pixclk_pol << CSI_SENS_CONF_PIX_CLK_POL_SHIFT |
+		cfg_param.ext_vsync << CSI_SENS_CONF_EXT_VSYNC_SHIFT |
+		cfg_param.clk_mode << CSI_SENS_CONF_SENS_PRTCL_SHIFT |
+		cfg_param.pack_tight << CSI_SENS_CONF_PACK_TIGHT_SHIFT |
+		cfg_param.force_eof << CSI_SENS_CONF_FORCE_EOF_SHIFT |
+		cfg_param.data_en_pol << CSI_SENS_CONF_DATA_EN_POL_SHIFT;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	__raw_writel(data, CSI_SENS_CONF(csi));
+
+	/* Setup sensor frame size */
+	__raw_writel((width - 1) | (height - 1) << 16, CSI_SENS_FRM_SIZE(csi));
+
+	/* Set CCIR registers */
+	if ((cfg_param.clk_mode == IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE) ||
+	    (cfg_param.clk_mode == IPU_CSI_CLK_MODE_CCIR656_INTERLACED)) {
+		_ipu_csi_ccir_err_detection_enable(csi);
+		__raw_writel(0x40030, CSI_CCIR_CODE_1(csi));
+		__raw_writel(0xFF0000, CSI_CCIR_CODE_3(csi));
+	} else if ((cfg_param.clk_mode ==
+			IPU_CSI_CLK_MODE_CCIR1120_PROGRESSIVE_DDR) ||
+		(cfg_param.clk_mode ==
+			IPU_CSI_CLK_MODE_CCIR1120_PROGRESSIVE_SDR) ||
+		(cfg_param.clk_mode ==
+			IPU_CSI_CLK_MODE_CCIR1120_INTERLACED_DDR) ||
+		(cfg_param.clk_mode ==
+			IPU_CSI_CLK_MODE_CCIR1120_INTERLACED_SDR)) {
+		_ipu_csi_ccir_err_detection_enable(csi);
+		__raw_writel(0x40030, CSI_CCIR_CODE_1(csi));
+		__raw_writel(0xFF0000, CSI_CCIR_CODE_3(csi));
+	} else if ((cfg_param.clk_mode == IPU_CSI_CLK_MODE_GATED_CLK) ||
+		   (cfg_param.clk_mode == IPU_CSI_CLK_MODE_NONGATED_CLK)) {
+		_ipu_csi_ccir_err_detection_disable(csi);
+	}
+
+	dev_dbg(g_ipu_dev, "CSI_SENS_CONF = 0x%08X\n",
+		__raw_readl(CSI_SENS_CONF(csi)));
+	dev_dbg(g_ipu_dev, "CSI_ACT_FRM_SIZE = 0x%08X\n",
+		__raw_readl(CSI_ACT_FRM_SIZE(csi)));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_csi_init_interface);
+
+/*! _ipu_csi_mclk_set
+ *
+ * @param	pixel_clk   desired pixel clock frequency in Hz
+ * @param	csi         csi 0 or csi 1
+ *
+ * @return	Returns 0 on success or negative error code on fail
+ */
+int _ipu_csi_mclk_set(uint32_t pixel_clk, uint32_t csi)
+{
+	uint32_t temp;
+	uint32_t div_ratio;
+
+	div_ratio = (clk_get_rate(g_ipu_clk) / pixel_clk) - 1;
+
+	if (div_ratio > 0xFF || div_ratio < 0) {
+		dev_dbg(g_ipu_dev, "The value of pixel_clk extends normal range\n");
+		return -EINVAL;
+	}
+
+	temp = __raw_readl(CSI_SENS_CONF(csi));
+	temp &= ~CSI_SENS_CONF_DIVRATIO_MASK;
+	__raw_writel(temp | (div_ratio << CSI_SENS_CONF_DIVRATIO_SHIFT),
+			CSI_SENS_CONF(csi));
+
+	return 0;
+}
+
+/*!
+ * ipu_csi_enable_mclk
+ *
+ * @param	csi         csi 0 or csi 1
+ * @param       flag        true to enable mclk, false to disable mclk
+ * @param       wait        true to wait 100ms make clock stable, false not wait
+ *
+ * @return      Returns 0 on success
+ */
+int ipu_csi_enable_mclk(int csi, bool flag, bool wait)
+{
+	if (flag) {
+		clk_enable(g_csi_clk[csi]);
+		if (wait == true)
+			msleep(10);
+	} else
+		clk_disable(g_csi_clk[csi]);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_csi_enable_mclk);
+
+/*!
+ * ipu_csi_get_window_size
+ *
+ * @param	width	pointer to window width
+ * @param	height	pointer to window height
+ * @param	csi	csi 0 or csi 1
+ */
+void ipu_csi_get_window_size(uint32_t *width, uint32_t *height, uint32_t csi)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(CSI_ACT_FRM_SIZE(csi));
+	*width = (reg & 0xFFFF) + 1;
+	*height = (reg >> 16 & 0xFFFF) + 1;
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+EXPORT_SYMBOL(ipu_csi_get_window_size);
+
+/*!
+ * ipu_csi_set_window_size
+ *
+ * @param	width	window width
+ * @param       height	window height
+ * @param       csi	csi 0 or csi 1
+ */
+void ipu_csi_set_window_size(uint32_t width, uint32_t height, uint32_t csi)
+{
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	__raw_writel((width - 1) | (height - 1) << 16, CSI_ACT_FRM_SIZE(csi));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+EXPORT_SYMBOL(ipu_csi_set_window_size);
+
+/*!
+ * ipu_csi_set_window_pos
+ *
+ * @param       left	uint32 window x start
+ * @param       top	uint32 window y start
+ * @param       csi	csi 0 or csi 1
+ */
+void ipu_csi_set_window_pos(uint32_t left, uint32_t top, uint32_t csi)
+{
+	uint32_t temp;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(CSI_OUT_FRM_CTRL(csi));
+	temp &= ~(CSI_HSC_MASK | CSI_VSC_MASK);
+	temp |= ((top << CSI_VSC_SHIFT) | (left << CSI_HSC_SHIFT));
+	__raw_writel(temp, CSI_OUT_FRM_CTRL(csi));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+EXPORT_SYMBOL(ipu_csi_set_window_pos);
+
+/*!
+ * _ipu_csi_horizontal_downsize_enable
+ *	Enable horizontal downsizing(decimation) by 2.
+ *
+ * @param	csi	csi 0 or csi 1
+ */
+void _ipu_csi_horizontal_downsize_enable(uint32_t csi)
+{
+	uint32_t temp;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(CSI_OUT_FRM_CTRL(csi));
+	temp |= CSI_HORI_DOWNSIZE_EN;
+	__raw_writel(temp, CSI_OUT_FRM_CTRL(csi));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+
+/*!
+ * _ipu_csi_horizontal_downsize_disable
+ *	Disable horizontal downsizing(decimation) by 2.
+ *
+ * @param	csi	csi 0 or csi 1
+ */
+void _ipu_csi_horizontal_downsize_disable(uint32_t csi)
+{
+	uint32_t temp;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(CSI_OUT_FRM_CTRL(csi));
+	temp &= ~CSI_HORI_DOWNSIZE_EN;
+	__raw_writel(temp, CSI_OUT_FRM_CTRL(csi));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+
+/*!
+ * _ipu_csi_vertical_downsize_enable
+ *	Enable vertical downsizing(decimation) by 2.
+ *
+ * @param	csi	csi 0 or csi 1
+ */
+void _ipu_csi_vertical_downsize_enable(uint32_t csi)
+{
+	uint32_t temp;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(CSI_OUT_FRM_CTRL(csi));
+	temp |= CSI_VERT_DOWNSIZE_EN;
+	__raw_writel(temp, CSI_OUT_FRM_CTRL(csi));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+
+/*!
+ * _ipu_csi_vertical_downsize_disable
+ *	Disable vertical downsizing(decimation) by 2.
+ *
+ * @param	csi	csi 0 or csi 1
+ */
+void _ipu_csi_vertical_downsize_disable(uint32_t csi)
+{
+	uint32_t temp;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(CSI_OUT_FRM_CTRL(csi));
+	temp &= ~CSI_VERT_DOWNSIZE_EN;
+	__raw_writel(temp, CSI_OUT_FRM_CTRL(csi));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+
+/*!
+ * ipu_csi_set_test_generator
+ *
+ * @param	active       1 for active and 0 for inactive
+ * @param       r_value	     red value for the generated pattern of even pixel
+ * @param       g_value      green value for the generated pattern of even
+ *			     pixel
+ * @param       b_value      blue value for the generated pattern of even pixel
+ * @param	pixel_clk   desired pixel clock frequency in Hz
+ * @param       csi          csi 0 or csi 1
+ */
+void ipu_csi_set_test_generator(bool active, uint32_t r_value,
+	uint32_t g_value, uint32_t b_value, uint32_t pix_clk, uint32_t csi)
+{
+	uint32_t temp;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(CSI_TST_CTRL(csi));
+
+	if (active == false) {
+		temp &= ~CSI_TEST_GEN_MODE_EN;
+		__raw_writel(temp, CSI_TST_CTRL(csi));
+	} else {
+		/* Set sensb_mclk div_ratio*/
+		_ipu_csi_mclk_set(pix_clk, csi);
+
+		temp &= ~(CSI_TEST_GEN_R_MASK | CSI_TEST_GEN_G_MASK |
+			CSI_TEST_GEN_B_MASK);
+		temp |= CSI_TEST_GEN_MODE_EN;
+		temp |= (r_value << CSI_TEST_GEN_R_SHIFT) |
+			(g_value << CSI_TEST_GEN_G_SHIFT) |
+			(b_value << CSI_TEST_GEN_B_SHIFT);
+		__raw_writel(temp, CSI_TST_CTRL(csi));
+	}
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+EXPORT_SYMBOL(ipu_csi_set_test_generator);
+
+/*!
+ * _ipu_csi_ccir_err_detection_en
+ *	Enable error detection and correction for
+ *	CCIR interlaced mode with protection bit.
+ *
+ * @param	csi	csi 0 or csi 1
+ */
+void _ipu_csi_ccir_err_detection_enable(uint32_t csi)
+{
+	uint32_t temp;
+
+	temp = __raw_readl(CSI_CCIR_CODE_1(csi));
+	temp |= CSI_CCIR_ERR_DET_EN;
+	__raw_writel(temp, CSI_CCIR_CODE_1(csi));
+}
+
+/*!
+ * _ipu_csi_ccir_err_detection_disable
+ *	Disable error detection and correction for
+ *	CCIR interlaced mode with protection bit.
+ *
+ * @param	csi	csi 0 or csi 1
+ */
+void _ipu_csi_ccir_err_detection_disable(uint32_t csi)
+{
+	uint32_t temp;
+
+	temp = __raw_readl(CSI_CCIR_CODE_1(csi));
+	temp &= ~CSI_CCIR_ERR_DET_EN;
+	__raw_writel(temp, CSI_CCIR_CODE_1(csi));
+}
+
+/*!
+ * _ipu_csi_set_mipi_di
+ *
+ * @param	num	MIPI data identifier 0-3 handled by CSI
+ * @param	di_val	data identifier value
+ * @param	csi	csi 0 or csi 1
+ *
+ * @return	Returns 0 on success or negative error code on fail
+ */
+int _ipu_csi_set_mipi_di(uint32_t num, uint32_t di_val, uint32_t csi)
+{
+	uint32_t temp;
+	int retval = 0;
+	unsigned long lock_flags;
+
+	if (di_val > 0xFFL) {
+		retval = -EINVAL;
+		goto err;
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(CSI_MIPI_DI(csi));
+
+	switch (num) {
+	case IPU_CSI_MIPI_DI0:
+		temp &= ~CSI_MIPI_DI0_MASK;
+		temp |= (di_val << CSI_MIPI_DI0_SHIFT);
+		__raw_writel(temp, CSI_MIPI_DI(csi));
+		break;
+	case IPU_CSI_MIPI_DI1:
+		temp &= ~CSI_MIPI_DI1_MASK;
+		temp |= (di_val << CSI_MIPI_DI1_SHIFT);
+		__raw_writel(temp, CSI_MIPI_DI(csi));
+		break;
+	case IPU_CSI_MIPI_DI2:
+		temp &= ~CSI_MIPI_DI2_MASK;
+		temp |= (di_val << CSI_MIPI_DI2_SHIFT);
+		__raw_writel(temp, CSI_MIPI_DI(csi));
+		break;
+	case IPU_CSI_MIPI_DI3:
+		temp &= ~CSI_MIPI_DI3_MASK;
+		temp |= (di_val << CSI_MIPI_DI3_SHIFT);
+		__raw_writel(temp, CSI_MIPI_DI(csi));
+		break;
+	default:
+		retval = -EINVAL;
+		goto err;
+	}
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+err:
+	return retval;
+}
+
+/*!
+ * _ipu_csi_set_skip_isp
+ *
+ * @param	skip		select frames to be skipped and set the
+ *				correspond bits to 1
+ * @param	max_ratio	number of frames in a skipping set and the
+ * 				maximum value of max_ratio is 5
+ * @param	csi		csi 0 or csi 1
+ *
+ * @return	Returns 0 on success or negative error code on fail
+ */
+int _ipu_csi_set_skip_isp(uint32_t skip, uint32_t max_ratio, uint32_t csi)
+{
+	uint32_t temp;
+	int retval = 0;
+	unsigned long lock_flags;
+
+	if (max_ratio > 5) {
+		retval = -EINVAL;
+		goto err;
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(CSI_SKIP(csi));
+	temp &= ~(CSI_MAX_RATIO_SKIP_ISP_MASK | CSI_SKIP_ISP_MASK);
+	temp |= (max_ratio << CSI_MAX_RATIO_SKIP_ISP_SHIFT) |
+		(skip << CSI_SKIP_ISP_SHIFT);
+	__raw_writel(temp, CSI_SKIP(csi));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+err:
+	return retval;
+}
+
+/*!
+ * _ipu_csi_set_skip_smfc
+ *
+ * @param	skip		select frames to be skipped and set the
+ *				correspond bits to 1
+ * @param	max_ratio	number of frames in a skipping set and the
+ *				maximum value of max_ratio is 5
+ * @param	id		csi to smfc skipping id
+ * @param	csi		csi 0 or csi 1
+ *
+ * @return	Returns 0 on success or negative error code on fail
+ */
+int _ipu_csi_set_skip_smfc(uint32_t skip, uint32_t max_ratio,
+	uint32_t id, uint32_t csi)
+{
+	uint32_t temp;
+	int retval = 0;
+	unsigned long lock_flags;
+
+	if (max_ratio > 5 || id > 3) {
+		retval = -EINVAL;
+		goto err;
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(CSI_SKIP(csi));
+	temp &= ~(CSI_MAX_RATIO_SKIP_SMFC_MASK | CSI_ID_2_SKIP_MASK |
+			CSI_SKIP_SMFC_MASK);
+	temp |= (max_ratio << CSI_MAX_RATIO_SKIP_SMFC_SHIFT) |
+			(id << CSI_ID_2_SKIP_SHIFT) |
+			(skip << CSI_SKIP_SMFC_SHIFT);
+	__raw_writel(temp, CSI_SKIP(csi));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+err:
+	return retval;
+}
+
+/*!
+ * _ipu_smfc_init
+ *	Map CSI frames to IDMAC channels.
+ *
+ * @param	channel		IDMAC channel 0-3
+ * @param	mipi_id		mipi id number 0-3
+ * @param	csi		csi0 or csi1
+ */
+void _ipu_smfc_init(ipu_channel_t channel, uint32_t mipi_id, uint32_t csi)
+{
+	uint32_t temp;
+
+	temp = __raw_readl(SMFC_MAP);
+
+	switch (channel) {
+	case CSI_MEM0:
+		temp &= ~SMFC_MAP_CH0_MASK;
+		temp |= ((csi << 2) | mipi_id) << SMFC_MAP_CH0_SHIFT;
+		break;
+	case CSI_MEM1:
+		temp &= ~SMFC_MAP_CH1_MASK;
+		temp |= ((csi << 2) | mipi_id) << SMFC_MAP_CH1_SHIFT;
+		break;
+	case CSI_MEM2:
+		temp &= ~SMFC_MAP_CH2_MASK;
+		temp |= ((csi << 2) | mipi_id) << SMFC_MAP_CH2_SHIFT;
+		break;
+	case CSI_MEM3:
+		temp &= ~SMFC_MAP_CH3_MASK;
+		temp |= ((csi << 2) | mipi_id) << SMFC_MAP_CH3_SHIFT;
+		break;
+	default:
+		return;
+	}
+
+	__raw_writel(temp, SMFC_MAP);
+}
+
+/*!
+ * _ipu_smfc_set_wmc
+ *	Caution: The number of required channels,  the enabled channels
+ *	and the FIFO size per channel are configured restrictedly.
+ *
+ * @param	channel		IDMAC channel 0-3
+ * @param	set		set 1 or clear 0
+ * @param	level		water mark level when FIFO is on the
+ *				relative size
+ */
+void _ipu_smfc_set_wmc(ipu_channel_t channel, bool set, uint32_t level)
+{
+	uint32_t temp;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(SMFC_WMC);
+
+	switch (channel) {
+	case CSI_MEM0:
+		if (set == true) {
+			temp &= ~SMFC_WM0_SET_MASK;
+			temp |= level << SMFC_WM0_SET_SHIFT;
+		} else {
+			temp &= ~SMFC_WM0_CLR_MASK;
+			temp |= level << SMFC_WM0_CLR_SHIFT;
+		}
+		break;
+	case CSI_MEM1:
+		if (set == true) {
+			temp &= ~SMFC_WM1_SET_MASK;
+			temp |= level << SMFC_WM1_SET_SHIFT;
+		} else {
+			temp &= ~SMFC_WM1_CLR_MASK;
+			temp |= level << SMFC_WM1_CLR_SHIFT;
+		}
+		break;
+	case CSI_MEM2:
+		if (set == true) {
+			temp &= ~SMFC_WM2_SET_MASK;
+			temp |= level << SMFC_WM2_SET_SHIFT;
+		} else {
+			temp &= ~SMFC_WM2_CLR_MASK;
+			temp |= level << SMFC_WM2_CLR_SHIFT;
+		}
+		break;
+	case CSI_MEM3:
+		if (set == true) {
+			temp &= ~SMFC_WM3_SET_MASK;
+			temp |= level << SMFC_WM3_SET_SHIFT;
+		} else {
+			temp &= ~SMFC_WM3_CLR_MASK;
+			temp |= level << SMFC_WM3_CLR_SHIFT;
+		}
+		break;
+	default:
+		return;
+	}
+
+	__raw_writel(temp, SMFC_WMC);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+
+/*!
+ * _ipu_smfc_set_burst_size
+ *
+ * @param	channel		IDMAC channel 0-3
+ * @param	bs		burst size of IDMAC channel,
+ *				the value programmed here shoud be BURST_SIZE-1
+ */
+void _ipu_smfc_set_burst_size(ipu_channel_t channel, uint32_t bs)
+{
+	uint32_t temp;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	temp = __raw_readl(SMFC_BS);
+
+	switch (channel) {
+	case CSI_MEM0:
+		temp &= ~SMFC_BS0_MASK;
+		temp |= bs << SMFC_BS0_SHIFT;
+		break;
+	case CSI_MEM1:
+		temp &= ~SMFC_BS1_MASK;
+		temp |= bs << SMFC_BS1_SHIFT;
+		break;
+	case CSI_MEM2:
+		temp &= ~SMFC_BS2_MASK;
+		temp |= bs << SMFC_BS2_SHIFT;
+		break;
+	case CSI_MEM3:
+		temp &= ~SMFC_BS3_MASK;
+		temp |= bs << SMFC_BS3_SHIFT;
+		break;
+	default:
+		return;
+	}
+
+	__raw_writel(temp, SMFC_BS);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+}
+
+/*!
+ * _ipu_csi_init
+ *
+ * @param	channel      IDMAC channel
+ * @param	csi	     csi 0 or csi 1
+ *
+ * @return	Returns 0 on success or negative error code on fail
+ */
+int _ipu_csi_init(ipu_channel_t channel, uint32_t csi)
+{
+	uint32_t csi_sens_conf, csi_dest;
+	int retval = 0;
+
+	switch (channel) {
+	case CSI_MEM0:
+	case CSI_MEM1:
+	case CSI_MEM2:
+	case CSI_MEM3:
+		csi_dest = CSI_DATA_DEST_IDMAC;
+		break;
+	case CSI_PRP_ENC_MEM:
+	case CSI_PRP_VF_MEM:
+		csi_dest = CSI_DATA_DEST_IC;
+		break;
+	default:
+		retval = -EINVAL;
+		goto err;
+	}
+
+	csi_sens_conf = __raw_readl(CSI_SENS_CONF(csi));
+	csi_sens_conf &= ~CSI_SENS_CONF_DATA_DEST_MASK;
+	__raw_writel(csi_sens_conf | (csi_dest <<
+		CSI_SENS_CONF_DATA_DEST_SHIFT), CSI_SENS_CONF(csi));
+err:
+	return retval;
+}
diff --git a/drivers/mxc/ipu3/ipu_common.c b/drivers/mxc/ipu3/ipu_common.c
new file mode 100644
index 000000000000..4c46db8b0abc
--- /dev/null
+++ b/drivers/mxc/ipu3/ipu_common.c
@@ -0,0 +1,2318 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_common.c
+ *
+ * @brief This file contains the IPU driver common API functions.
+ *
+ * @ingroup IPU
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+#include <linux/clk.h>
+
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+#include "ipu_param_mem.h"
+
+struct ipu_irq_node {
+	irqreturn_t(*handler) (int, void *);	/*!< the ISR */
+	const char *name;	/*!< device associated with the interrupt */
+	void *dev_id;		/*!< some unique information for the ISR */
+	__u32 flags;		/*!< not used */
+};
+
+/* Globals */
+struct clk *g_ipu_clk;
+bool g_ipu_clk_enabled;
+struct clk *g_di_clk[2];
+struct clk *g_csi_clk[2];
+unsigned char g_dc_di_assignment[10];
+ipu_channel_t g_ipu_csi_channel[2];
+int g_ipu_irq[2];
+int g_ipu_hw_rev;
+bool g_sec_chan_en[22];
+bool g_thrd_chan_en[21];
+uint32_t g_channel_init_mask;
+uint32_t g_channel_enable_mask;
+DEFINE_SPINLOCK(ipu_lock);
+struct device *g_ipu_dev;
+
+static struct ipu_irq_node ipu_irq_list[IPU_IRQ_COUNT];
+static const char driver_name[] = "mxc_ipu";
+
+static int ipu_dc_use_count;
+static int ipu_dp_use_count;
+static int ipu_dmfc_use_count;
+static int ipu_smfc_use_count;
+static int ipu_ic_use_count;
+static int ipu_rot_use_count;
+static int ipu_vdi_use_count;
+static int ipu_di_use_count[2];
+static int ipu_csi_use_count[2];
+/* Set to the follow using IC direct channel, default non */
+static ipu_channel_t using_ic_dirct_ch;
+
+/* for power gating */
+static uint32_t ipu_conf_reg;
+static uint32_t ic_conf_reg;
+static uint32_t ipu_cha_db_mode_reg[4];
+static uint32_t ipu_cha_cur_buf_reg[4];
+static uint32_t idma_enable_reg[2];
+static uint32_t buf_ready_reg[8];
+
+u32 *ipu_cm_reg;
+u32 *ipu_idmac_reg;
+u32 *ipu_dp_reg;
+u32 *ipu_ic_reg;
+u32 *ipu_dc_reg;
+u32 *ipu_dc_tmpl_reg;
+u32 *ipu_dmfc_reg;
+u32 *ipu_di_reg[2];
+u32 *ipu_smfc_reg;
+u32 *ipu_csi_reg[2];
+u32 *ipu_cpmem_base;
+u32 *ipu_tpmem_base;
+u32 *ipu_disp_base[2];
+u32 *ipu_vdi_reg;
+
+/* Static functions */
+static irqreturn_t ipu_irq_handler(int irq, void *desc);
+
+static inline uint32_t channel_2_dma(ipu_channel_t ch, ipu_buffer_t type)
+{
+	return ((uint32_t) ch >> (6 * type)) & 0x3F;
+};
+
+static inline int _ipu_is_ic_chan(uint32_t dma_chan)
+{
+	return ((dma_chan >= 11) && (dma_chan <= 22) && (dma_chan != 17) && (dma_chan != 18));
+}
+
+static inline int _ipu_is_ic_graphic_chan(uint32_t dma_chan)
+{
+	return (dma_chan == 14 || dma_chan == 15);
+}
+
+/* Either DP BG or DP FG can be graphic window */
+static inline int _ipu_is_dp_graphic_chan(uint32_t dma_chan)
+{
+	return (dma_chan == 23 || dma_chan == 27);
+}
+
+static inline int _ipu_is_irt_chan(uint32_t dma_chan)
+{
+	return ((dma_chan >= 45) && (dma_chan <= 50));
+}
+
+static inline int _ipu_is_dmfc_chan(uint32_t dma_chan)
+{
+	return ((dma_chan >= 23) && (dma_chan <= 29));
+}
+
+static inline int _ipu_is_smfc_chan(uint32_t dma_chan)
+{
+	return ((dma_chan >= 0) && (dma_chan <= 3));
+}
+
+#define idma_is_valid(ch)	(ch != NO_DMA)
+#define idma_mask(ch)		(idma_is_valid(ch) ? (1UL << (ch & 0x1F)) : 0)
+#define idma_is_set(reg, dma)	(__raw_readl(reg(dma)) & idma_mask(dma))
+
+/*!
+ * This function is called by the driver framework to initialize the IPU
+ * hardware.
+ *
+ * @param	dev	The device structure for the IPU passed in by the
+ *			driver framework.
+ *
+ * @return      Returns 0 on success or negative error code on error
+ */
+static int ipu_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct mxc_ipu_config *plat_data = pdev->dev.platform_data;
+	unsigned long ipu_base;
+
+	spin_lock_init(&ipu_lock);
+
+	g_ipu_hw_rev = plat_data->rev;
+
+	g_ipu_dev = &pdev->dev;
+
+	/* Register IPU interrupts */
+	g_ipu_irq[0] = platform_get_irq(pdev, 0);
+	if (g_ipu_irq[0] < 0)
+		return -EINVAL;
+
+	if (request_irq(g_ipu_irq[0], ipu_irq_handler, 0, pdev->name, 0) != 0) {
+		dev_err(g_ipu_dev, "request SYNC interrupt failed\n");
+		return -EBUSY;
+	}
+	/* Some platforms have 2 IPU interrupts */
+	g_ipu_irq[1] = platform_get_irq(pdev, 1);
+	if (g_ipu_irq[1] >= 0) {
+		if (request_irq
+		    (g_ipu_irq[1], ipu_irq_handler, 0, pdev->name, 0) != 0) {
+			dev_err(g_ipu_dev, "request ERR interrupt failed\n");
+			return -EBUSY;
+		}
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (IS_ERR(res))
+		return -ENODEV;
+
+	ipu_base = res->start;
+	ipu_cm_reg = ioremap(ipu_base + IPU_CM_REG_BASE, PAGE_SIZE);
+	ipu_ic_reg = ioremap(ipu_base + IPU_IC_REG_BASE, PAGE_SIZE);
+	ipu_idmac_reg = ioremap(ipu_base + IPU_IDMAC_REG_BASE, PAGE_SIZE);
+	/* DP Registers are accessed thru the SRM */
+	ipu_dp_reg = ioremap(ipu_base + IPU_SRM_REG_BASE, PAGE_SIZE);
+	ipu_dc_reg = ioremap(ipu_base + IPU_DC_REG_BASE, PAGE_SIZE);
+	ipu_dmfc_reg = ioremap(ipu_base + IPU_DMFC_REG_BASE, PAGE_SIZE);
+	ipu_di_reg[0] = ioremap(ipu_base + IPU_DI0_REG_BASE, PAGE_SIZE);
+	ipu_di_reg[1] = ioremap(ipu_base + IPU_DI1_REG_BASE, PAGE_SIZE);
+	ipu_smfc_reg = ioremap(ipu_base + IPU_SMFC_REG_BASE, PAGE_SIZE);
+	ipu_csi_reg[0] = ioremap(ipu_base + IPU_CSI0_REG_BASE, PAGE_SIZE);
+	ipu_csi_reg[1] = ioremap(ipu_base + IPU_CSI1_REG_BASE, PAGE_SIZE);
+	ipu_cpmem_base = ioremap(ipu_base + IPU_CPMEM_REG_BASE, PAGE_SIZE);
+	ipu_tpmem_base = ioremap(ipu_base + IPU_TPM_REG_BASE, SZ_64K);
+	ipu_dc_tmpl_reg = ioremap(ipu_base + IPU_DC_TMPL_REG_BASE, SZ_128K);
+	ipu_disp_base[1] = ioremap(ipu_base + IPU_DISP1_BASE, SZ_4K);
+	ipu_vdi_reg = ioremap(ipu_base + IPU_VDI_REG_BASE, PAGE_SIZE);
+
+	dev_dbg(g_ipu_dev, "IPU VDI Regs = %p\n", ipu_vdi_reg);
+	dev_dbg(g_ipu_dev, "IPU CM Regs = %p\n", ipu_cm_reg);
+	dev_dbg(g_ipu_dev, "IPU IC Regs = %p\n", ipu_ic_reg);
+	dev_dbg(g_ipu_dev, "IPU IDMAC Regs = %p\n", ipu_idmac_reg);
+	dev_dbg(g_ipu_dev, "IPU DP Regs = %p\n", ipu_dp_reg);
+	dev_dbg(g_ipu_dev, "IPU DC Regs = %p\n", ipu_dc_reg);
+	dev_dbg(g_ipu_dev, "IPU DMFC Regs = %p\n", ipu_dmfc_reg);
+	dev_dbg(g_ipu_dev, "IPU DI0 Regs = %p\n", ipu_di_reg[0]);
+	dev_dbg(g_ipu_dev, "IPU DI1 Regs = %p\n", ipu_di_reg[1]);
+	dev_dbg(g_ipu_dev, "IPU SMFC Regs = %p\n", ipu_smfc_reg);
+	dev_dbg(g_ipu_dev, "IPU CSI0 Regs = %p\n", ipu_csi_reg[0]);
+	dev_dbg(g_ipu_dev, "IPU CSI1 Regs = %p\n", ipu_csi_reg[1]);
+	dev_dbg(g_ipu_dev, "IPU CPMem = %p\n", ipu_cpmem_base);
+	dev_dbg(g_ipu_dev, "IPU TPMem = %p\n", ipu_tpmem_base);
+	dev_dbg(g_ipu_dev, "IPU DC Template Mem = %p\n", ipu_dc_tmpl_reg);
+	dev_dbg(g_ipu_dev, "IPU Display Region 1 Mem = %p\n", ipu_disp_base[1]);
+
+	/* Enable IPU and CSI clocks */
+	/* Get IPU clock freq */
+	g_ipu_clk = clk_get(&pdev->dev, "ipu_clk");
+	dev_dbg(g_ipu_dev, "ipu_clk = %lu\n", clk_get_rate(g_ipu_clk));
+
+	clk_enable(g_ipu_clk);
+
+	g_di_clk[0] = plat_data->di_clk[0];
+	g_di_clk[1] = plat_data->di_clk[1];
+
+	g_csi_clk[0] = clk_get(&pdev->dev, "csi_mclk1");
+	g_csi_clk[1] = clk_get(&pdev->dev, "csi_mclk2");
+
+	__raw_writel(0x807FFFFF, IPU_MEM_RST);
+	while (__raw_readl(IPU_MEM_RST) & 0x80000000) ;
+
+	_ipu_init_dc_mappings();
+
+	/* Enable error interrupts by default */
+	__raw_writel(0xFFFFFFFF, IPU_INT_CTRL(5));
+	__raw_writel(0xFFFFFFFF, IPU_INT_CTRL(6));
+	__raw_writel(0xFFFFFFFF, IPU_INT_CTRL(9));
+	__raw_writel(0xFFFFFFFF, IPU_INT_CTRL(10));
+
+	/* DMFC Init */
+	_ipu_dmfc_init();
+
+	/* Set sync refresh channels as high priority */
+	__raw_writel(0x18800000L, IDMAC_CHA_PRI(0));
+
+	/* Set MCU_T to divide MCU access window into 2 */
+	__raw_writel(0x00400000L | (IPU_MCU_T_DEFAULT << 18), IPU_DISP_GEN);
+
+	clk_disable(g_ipu_clk);
+
+	register_ipu_device();
+
+	return 0;
+}
+
+int ipu_remove(struct platform_device *pdev)
+{
+	if (g_ipu_irq[0])
+		free_irq(g_ipu_irq[0], 0);
+	if (g_ipu_irq[1])
+		free_irq(g_ipu_irq[1], 0);
+
+	clk_put(g_ipu_clk);
+
+	iounmap(ipu_cm_reg);
+	iounmap(ipu_ic_reg);
+	iounmap(ipu_idmac_reg);
+	iounmap(ipu_dc_reg);
+	iounmap(ipu_dp_reg);
+	iounmap(ipu_dmfc_reg);
+	iounmap(ipu_di_reg[0]);
+	iounmap(ipu_di_reg[1]);
+	iounmap(ipu_smfc_reg);
+	iounmap(ipu_csi_reg[0]);
+	iounmap(ipu_csi_reg[1]);
+	iounmap(ipu_cpmem_base);
+	iounmap(ipu_tpmem_base);
+	iounmap(ipu_dc_tmpl_reg);
+	iounmap(ipu_disp_base[1]);
+	iounmap(ipu_vdi_reg);
+
+	return 0;
+}
+
+void ipu_dump_registers(void)
+{
+	printk(KERN_DEBUG "IPU_CONF = \t0x%08X\n", __raw_readl(IPU_CONF));
+	printk(KERN_DEBUG "IDMAC_CONF = \t0x%08X\n", __raw_readl(IDMAC_CONF));
+	printk(KERN_DEBUG "IDMAC_CHA_EN1 = \t0x%08X\n",
+	       __raw_readl(IDMAC_CHA_EN(0)));
+	printk(KERN_DEBUG "IDMAC_CHA_EN2 = \t0x%08X\n",
+	       __raw_readl(IDMAC_CHA_EN(32)));
+	printk(KERN_DEBUG "IDMAC_CHA_PRI1 = \t0x%08X\n",
+	       __raw_readl(IDMAC_CHA_PRI(0)));
+	printk(KERN_DEBUG "IDMAC_CHA_PRI2 = \t0x%08X\n",
+	       __raw_readl(IDMAC_CHA_PRI(32)));
+	printk(KERN_DEBUG "IDMAC_BAND_EN1 = \t0x%08X\n",
+	       __raw_readl(IDMAC_BAND_EN(0)));
+	printk(KERN_DEBUG "IDMAC_BAND_EN2 = \t0x%08X\n",
+	       __raw_readl(IDMAC_BAND_EN(32)));
+	printk(KERN_DEBUG "IPU_CHA_DB_MODE_SEL0 = \t0x%08X\n",
+	       __raw_readl(IPU_CHA_DB_MODE_SEL(0)));
+	printk(KERN_DEBUG "IPU_CHA_DB_MODE_SEL1 = \t0x%08X\n",
+	       __raw_readl(IPU_CHA_DB_MODE_SEL(32)));
+	printk(KERN_DEBUG "DMFC_WR_CHAN = \t0x%08X\n",
+	       __raw_readl(DMFC_WR_CHAN));
+	printk(KERN_DEBUG "DMFC_WR_CHAN_DEF = \t0x%08X\n",
+	       __raw_readl(DMFC_WR_CHAN_DEF));
+	printk(KERN_DEBUG "DMFC_DP_CHAN = \t0x%08X\n",
+	       __raw_readl(DMFC_DP_CHAN));
+	printk(KERN_DEBUG "DMFC_DP_CHAN_DEF = \t0x%08X\n",
+	       __raw_readl(DMFC_DP_CHAN_DEF));
+	printk(KERN_DEBUG "DMFC_IC_CTRL = \t0x%08X\n",
+	       __raw_readl(DMFC_IC_CTRL));
+	printk(KERN_DEBUG "IPU_FS_PROC_FLOW1 = \t0x%08X\n",
+	       __raw_readl(IPU_FS_PROC_FLOW1));
+	printk(KERN_DEBUG "IPU_FS_PROC_FLOW2 = \t0x%08X\n",
+	       __raw_readl(IPU_FS_PROC_FLOW2));
+	printk(KERN_DEBUG "IPU_FS_PROC_FLOW3 = \t0x%08X\n",
+	       __raw_readl(IPU_FS_PROC_FLOW3));
+	printk(KERN_DEBUG "IPU_FS_DISP_FLOW1 = \t0x%08X\n",
+	       __raw_readl(IPU_FS_DISP_FLOW1));
+}
+
+/*!
+ * This function is called to initialize a logical IPU channel.
+ *
+ * @param       channel Input parameter for the logical channel ID to init.
+ *
+ * @param       params  Input parameter containing union of channel
+ *                      initialization parameters.
+ *
+ * @return      Returns 0 on success or negative error code on fail
+ */
+int32_t ipu_init_channel(ipu_channel_t channel, ipu_channel_params_t *params)
+{
+	int ret = 0;
+	uint32_t ipu_conf;
+	uint32_t reg;
+	unsigned long lock_flags;
+
+	dev_dbg(g_ipu_dev, "init channel = %d\n", IPU_CHAN_ID(channel));
+
+	/* re-enable error interrupts every time a channel is initialized */
+	__raw_writel(0xFFFFFFFF, IPU_INT_CTRL(5));
+	__raw_writel(0xFFFFFFFF, IPU_INT_CTRL(6));
+	__raw_writel(0xFFFFFFFF, IPU_INT_CTRL(9));
+	__raw_writel(0xFFFFFFFF, IPU_INT_CTRL(10));
+
+	if (g_ipu_clk_enabled == false) {
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if (g_channel_init_mask & (1L << IPU_CHAN_ID(channel))) {
+		dev_err(g_ipu_dev, "Warning: channel already initialized %d\n",
+			IPU_CHAN_ID(channel));
+	}
+
+	ipu_conf = __raw_readl(IPU_CONF);
+
+	switch (channel) {
+	case CSI_MEM0:
+	case CSI_MEM1:
+	case CSI_MEM2:
+	case CSI_MEM3:
+		if (params->csi_mem.csi > 1) {
+			ret = -EINVAL;
+			goto err;
+		}
+
+		ipu_smfc_use_count++;
+		ipu_csi_use_count[params->csi_mem.csi]++;
+		g_ipu_csi_channel[params->csi_mem.csi] = channel;
+
+		/*SMFC setting*/
+		if (params->csi_mem.mipi_en) {
+			ipu_conf |= (1 << (IPU_CONF_CSI0_DATA_SOURCE_OFFSET +
+				params->csi_mem.csi));
+			_ipu_smfc_init(channel, params->csi_mem.mipi_id,
+				params->csi_mem.csi);
+		} else {
+			ipu_conf &= ~(1 << (IPU_CONF_CSI0_DATA_SOURCE_OFFSET +
+				params->csi_mem.csi));
+			_ipu_smfc_init(channel, 0, params->csi_mem.csi);
+		}
+
+		/*CSI data (include compander) dest*/
+		_ipu_csi_init(channel, params->csi_mem.csi);
+		break;
+	case CSI_PRP_ENC_MEM:
+		if (params->csi_prp_enc_mem.csi > 1) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (using_ic_dirct_ch == MEM_VDI_PRP_VF_MEM) {
+			ret = -EINVAL;
+			goto err;
+		}
+		using_ic_dirct_ch = CSI_PRP_ENC_MEM;
+
+		ipu_ic_use_count++;
+		ipu_csi_use_count[params->csi_prp_enc_mem.csi]++;
+		g_ipu_csi_channel[params->csi_prp_enc_mem.csi] = channel;
+
+		/*Without SMFC, CSI only support parallel data source*/
+		ipu_conf &= ~(1 << (IPU_CONF_CSI0_DATA_SOURCE_OFFSET +
+			params->csi_prp_enc_mem.csi));
+
+		/*CSI0/1 feed into IC*/
+		ipu_conf &= ~IPU_CONF_IC_INPUT;
+		if (params->csi_prp_enc_mem.csi)
+			ipu_conf |= IPU_CONF_CSI_SEL;
+		else
+			ipu_conf &= ~IPU_CONF_CSI_SEL;
+
+		/*PRP skip buffer in memory, only valid when RWS_EN is true*/
+		reg = __raw_readl(IPU_FS_PROC_FLOW1);
+		__raw_writel(reg & ~FS_ENC_IN_VALID, IPU_FS_PROC_FLOW1);
+
+		/*CSI data (include compander) dest*/
+		_ipu_csi_init(channel, params->csi_prp_enc_mem.csi);
+		_ipu_ic_init_prpenc(params, true);
+		break;
+	case CSI_PRP_VF_MEM:
+		if (params->csi_prp_vf_mem.csi > 1) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (using_ic_dirct_ch == MEM_VDI_PRP_VF_MEM) {
+			ret = -EINVAL;
+			goto err;
+		}
+		using_ic_dirct_ch = CSI_PRP_VF_MEM;
+
+		ipu_ic_use_count++;
+		ipu_csi_use_count[params->csi_prp_vf_mem.csi]++;
+		g_ipu_csi_channel[params->csi_prp_vf_mem.csi] = channel;
+
+		/*Without SMFC, CSI only support parallel data source*/
+		ipu_conf &= ~(1 << (IPU_CONF_CSI0_DATA_SOURCE_OFFSET +
+			params->csi_prp_vf_mem.csi));
+
+		/*CSI0/1 feed into IC*/
+		ipu_conf &= ~IPU_CONF_IC_INPUT;
+		if (params->csi_prp_vf_mem.csi)
+			ipu_conf |= IPU_CONF_CSI_SEL;
+		else
+			ipu_conf &= ~IPU_CONF_CSI_SEL;
+
+		/*PRP skip buffer in memory, only valid when RWS_EN is true*/
+		reg = __raw_readl(IPU_FS_PROC_FLOW1);
+		__raw_writel(reg & ~FS_VF_IN_VALID, IPU_FS_PROC_FLOW1);
+
+		/*CSI data (include compander) dest*/
+		_ipu_csi_init(channel, params->csi_prp_vf_mem.csi);
+		_ipu_ic_init_prpvf(params, true);
+		break;
+	case MEM_PRP_VF_MEM:
+		ipu_ic_use_count++;
+		reg = __raw_readl(IPU_FS_PROC_FLOW1);
+		__raw_writel(reg | FS_VF_IN_VALID, IPU_FS_PROC_FLOW1);
+
+		if (params->mem_prp_vf_mem.graphics_combine_en)
+			g_sec_chan_en[IPU_CHAN_ID(channel)] = true;
+		if (params->mem_prp_vf_mem.alpha_chan_en)
+			g_thrd_chan_en[IPU_CHAN_ID(channel)] = true;
+
+		_ipu_ic_init_prpvf(params, false);
+		break;
+	case MEM_VDI_PRP_VF_MEM:
+		if ((using_ic_dirct_ch == CSI_PRP_VF_MEM) ||
+		     (using_ic_dirct_ch == CSI_PRP_ENC_MEM)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		using_ic_dirct_ch = MEM_VDI_PRP_VF_MEM;
+		ipu_ic_use_count++;
+		ipu_vdi_use_count++;
+		reg = __raw_readl(IPU_FS_PROC_FLOW1);
+		reg &= ~FS_VDI_SRC_SEL_MASK;
+		__raw_writel(reg , IPU_FS_PROC_FLOW1);
+
+		if (params->mem_prp_vf_mem.graphics_combine_en)
+			g_sec_chan_en[IPU_CHAN_ID(channel)] = true;
+		_ipu_ic_init_prpvf(params, false);
+		_ipu_vdi_init(channel, params);
+		break;
+	case MEM_VDI_PRP_VF_MEM_P:
+		_ipu_vdi_init(channel, params);
+		break;
+	case MEM_VDI_PRP_VF_MEM_N:
+		_ipu_vdi_init(channel, params);
+		break;
+	case MEM_ROT_VF_MEM:
+		ipu_ic_use_count++;
+		ipu_rot_use_count++;
+		_ipu_ic_init_rotate_vf(params);
+		break;
+	case MEM_PRP_ENC_MEM:
+		ipu_ic_use_count++;
+		reg = __raw_readl(IPU_FS_PROC_FLOW1);
+		__raw_writel(reg | FS_ENC_IN_VALID, IPU_FS_PROC_FLOW1);
+		_ipu_ic_init_prpenc(params, false);
+		break;
+	case MEM_ROT_ENC_MEM:
+		ipu_ic_use_count++;
+		ipu_rot_use_count++;
+		_ipu_ic_init_rotate_enc(params);
+		break;
+	case MEM_PP_MEM:
+		if (params->mem_pp_mem.graphics_combine_en)
+			g_sec_chan_en[IPU_CHAN_ID(channel)] = true;
+		if (params->mem_pp_mem.alpha_chan_en)
+			g_thrd_chan_en[IPU_CHAN_ID(channel)] = true;
+		_ipu_ic_init_pp(params);
+		ipu_ic_use_count++;
+		break;
+	case MEM_ROT_PP_MEM:
+		_ipu_ic_init_rotate_pp(params);
+		ipu_ic_use_count++;
+		ipu_rot_use_count++;
+		break;
+	case MEM_DC_SYNC:
+		if (params->mem_dc_sync.di > 1) {
+			ret = -EINVAL;
+			goto err;
+		}
+
+		g_dc_di_assignment[1] = params->mem_dc_sync.di;
+		_ipu_dc_init(1, params->mem_dc_sync.di,
+			     params->mem_dc_sync.interlaced);
+		ipu_di_use_count[params->mem_dc_sync.di]++;
+		ipu_dc_use_count++;
+		ipu_dmfc_use_count++;
+		break;
+	case MEM_BG_SYNC:
+		if (params->mem_dp_bg_sync.di > 1) {
+			ret = -EINVAL;
+			goto err;
+		}
+
+		if (params->mem_dp_bg_sync.alpha_chan_en)
+			g_thrd_chan_en[IPU_CHAN_ID(channel)] = true;
+
+		g_dc_di_assignment[5] = params->mem_dp_bg_sync.di;
+		_ipu_dp_init(channel, params->mem_dp_bg_sync.in_pixel_fmt,
+			     params->mem_dp_bg_sync.out_pixel_fmt);
+		_ipu_dc_init(5, params->mem_dp_bg_sync.di,
+			     params->mem_dp_bg_sync.interlaced);
+		ipu_di_use_count[params->mem_dp_bg_sync.di]++;
+		ipu_dc_use_count++;
+		ipu_dp_use_count++;
+		ipu_dmfc_use_count++;
+		break;
+	case MEM_FG_SYNC:
+		_ipu_dp_init(channel, params->mem_dp_fg_sync.in_pixel_fmt,
+			     params->mem_dp_fg_sync.out_pixel_fmt);
+
+		if (params->mem_dp_fg_sync.alpha_chan_en)
+			g_thrd_chan_en[IPU_CHAN_ID(channel)] = true;
+
+		ipu_dc_use_count++;
+		ipu_dp_use_count++;
+		ipu_dmfc_use_count++;
+		break;
+	case DIRECT_ASYNC0:
+		if (params->direct_async.di > 1) {
+			ret = -EINVAL;
+			goto err;
+		}
+
+		g_dc_di_assignment[8] = params->direct_async.di;
+		_ipu_dc_init(8, params->direct_async.di, false);
+		ipu_di_use_count[params->direct_async.di]++;
+		ipu_dc_use_count++;
+		break;
+	case DIRECT_ASYNC1:
+		if (params->direct_async.di > 1) {
+			ret = -EINVAL;
+			goto err;
+		}
+
+		g_dc_di_assignment[9] = params->direct_async.di;
+		_ipu_dc_init(9, params->direct_async.di, false);
+		ipu_di_use_count[params->direct_async.di]++;
+		ipu_dc_use_count++;
+		break;
+	default:
+		dev_err(g_ipu_dev, "Missing channel initialization\n");
+		break;
+	}
+
+	/* Enable IPU sub module */
+	g_channel_init_mask |= 1L << IPU_CHAN_ID(channel);
+	if (ipu_ic_use_count == 1)
+		ipu_conf |= IPU_CONF_IC_EN;
+	if (ipu_vdi_use_count == 1) {
+		ipu_conf |= IPU_CONF_VDI_EN;
+		ipu_conf |= IPU_CONF_IC_INPUT;
+	}
+	if (ipu_rot_use_count == 1)
+		ipu_conf |= IPU_CONF_ROT_EN;
+	if (ipu_dc_use_count == 1)
+		ipu_conf |= IPU_CONF_DC_EN;
+	if (ipu_dp_use_count == 1)
+		ipu_conf |= IPU_CONF_DP_EN;
+	if (ipu_dmfc_use_count == 1)
+		ipu_conf |= IPU_CONF_DMFC_EN;
+	if (ipu_di_use_count[0] == 1) {
+		ipu_conf |= IPU_CONF_DI0_EN;
+		clk_enable(g_di_clk[0]);
+	}
+	if (ipu_di_use_count[1] == 1) {
+		ipu_conf |= IPU_CONF_DI1_EN;
+		clk_enable(g_di_clk[1]);
+	}
+	if (ipu_smfc_use_count == 1)
+		ipu_conf |= IPU_CONF_SMFC_EN;
+	if (ipu_csi_use_count[0] == 1)
+		ipu_conf |= IPU_CONF_CSI0_EN;
+	if (ipu_csi_use_count[1] == 1)
+		ipu_conf |= IPU_CONF_CSI1_EN;
+
+	__raw_writel(ipu_conf, IPU_CONF);
+
+err:
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return ret;
+}
+EXPORT_SYMBOL(ipu_init_channel);
+
+/*!
+ * This function is called to uninitialize a logical IPU channel.
+ *
+ * @param       channel Input parameter for the logical channel ID to uninit.
+ */
+void ipu_uninit_channel(ipu_channel_t channel)
+{
+	unsigned long lock_flags;
+	uint32_t reg;
+	uint32_t in_dma, out_dma = 0;
+	uint32_t ipu_conf;
+
+	if ((g_channel_init_mask & (1L << IPU_CHAN_ID(channel))) == 0) {
+		dev_err(g_ipu_dev, "Channel already uninitialized %d\n",
+			IPU_CHAN_ID(channel));
+		return;
+	}
+
+	/* Make sure channel is disabled */
+	/* Get input and output dma channels */
+	in_dma = channel_2_dma(channel, IPU_OUTPUT_BUFFER);
+	out_dma = channel_2_dma(channel, IPU_VIDEO_IN_BUFFER);
+
+	if (idma_is_set(IDMAC_CHA_EN, in_dma) ||
+	    idma_is_set(IDMAC_CHA_EN, out_dma)) {
+		dev_err(g_ipu_dev,
+			"Channel %d is not disabled, disable first\n",
+			IPU_CHAN_ID(channel));
+		return;
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	/* Reset the double buffer */
+	reg = __raw_readl(IPU_CHA_DB_MODE_SEL(in_dma));
+	__raw_writel(reg & ~idma_mask(in_dma), IPU_CHA_DB_MODE_SEL(in_dma));
+	reg = __raw_readl(IPU_CHA_DB_MODE_SEL(out_dma));
+	__raw_writel(reg & ~idma_mask(out_dma), IPU_CHA_DB_MODE_SEL(out_dma));
+
+	g_sec_chan_en[IPU_CHAN_ID(channel)] = false;
+	g_thrd_chan_en[IPU_CHAN_ID(channel)] = false;
+
+	switch (channel) {
+	case CSI_MEM0:
+	case CSI_MEM1:
+	case CSI_MEM2:
+	case CSI_MEM3:
+		ipu_smfc_use_count--;
+		if (g_ipu_csi_channel[0] == channel) {
+			g_ipu_csi_channel[0] = CHAN_NONE;
+			ipu_csi_use_count[0]--;
+		} else if (g_ipu_csi_channel[1] == channel) {
+			g_ipu_csi_channel[1] = CHAN_NONE;
+			ipu_csi_use_count[1]--;
+		}
+		break;
+	case CSI_PRP_ENC_MEM:
+		ipu_ic_use_count--;
+		if (using_ic_dirct_ch == CSI_PRP_ENC_MEM)
+			using_ic_dirct_ch = 0;
+		_ipu_ic_uninit_prpenc();
+		if (g_ipu_csi_channel[0] == channel) {
+			g_ipu_csi_channel[0] = CHAN_NONE;
+			ipu_csi_use_count[0]--;
+		} else if (g_ipu_csi_channel[1] == channel) {
+			g_ipu_csi_channel[1] = CHAN_NONE;
+			ipu_csi_use_count[1]--;
+		}
+		break;
+	case CSI_PRP_VF_MEM:
+		ipu_ic_use_count--;
+		if (using_ic_dirct_ch == CSI_PRP_VF_MEM)
+			using_ic_dirct_ch = 0;
+		_ipu_ic_uninit_prpvf();
+		if (g_ipu_csi_channel[0] == channel) {
+			g_ipu_csi_channel[0] = CHAN_NONE;
+			ipu_csi_use_count[0]--;
+		} else if (g_ipu_csi_channel[1] == channel) {
+			g_ipu_csi_channel[1] = CHAN_NONE;
+			ipu_csi_use_count[1]--;
+		}
+		break;
+	case MEM_PRP_VF_MEM:
+		ipu_ic_use_count--;
+		_ipu_ic_uninit_prpvf();
+		reg = __raw_readl(IPU_FS_PROC_FLOW1);
+		__raw_writel(reg & ~FS_VF_IN_VALID, IPU_FS_PROC_FLOW1);
+		break;
+	case MEM_VDI_PRP_VF_MEM:
+		ipu_ic_use_count--;
+		ipu_vdi_use_count--;
+		if (using_ic_dirct_ch == MEM_VDI_PRP_VF_MEM)
+			using_ic_dirct_ch = 0;
+		_ipu_ic_uninit_prpvf();
+		_ipu_vdi_uninit();
+		reg = __raw_readl(IPU_FS_PROC_FLOW1);
+		__raw_writel(reg & ~FS_VF_IN_VALID, IPU_FS_PROC_FLOW1);
+		break;
+	case MEM_ROT_VF_MEM:
+		ipu_rot_use_count--;
+		ipu_ic_use_count--;
+		_ipu_ic_uninit_rotate_vf();
+		break;
+	case MEM_PRP_ENC_MEM:
+		ipu_ic_use_count--;
+		_ipu_ic_uninit_prpenc();
+		reg = __raw_readl(IPU_FS_PROC_FLOW1);
+		__raw_writel(reg & ~FS_ENC_IN_VALID, IPU_FS_PROC_FLOW1);
+		break;
+	case MEM_ROT_ENC_MEM:
+		ipu_rot_use_count--;
+		ipu_ic_use_count--;
+		_ipu_ic_uninit_rotate_enc();
+		break;
+	case MEM_PP_MEM:
+		ipu_ic_use_count--;
+		_ipu_ic_uninit_pp();
+		break;
+	case MEM_ROT_PP_MEM:
+		ipu_rot_use_count--;
+		ipu_ic_use_count--;
+		_ipu_ic_uninit_rotate_pp();
+		break;
+	case MEM_DC_SYNC:
+		_ipu_dc_uninit(1);
+		ipu_di_use_count[g_dc_di_assignment[1]]--;
+		ipu_dc_use_count--;
+		ipu_dmfc_use_count--;
+		break;
+	case MEM_BG_SYNC:
+		_ipu_dp_uninit(channel);
+		_ipu_dc_uninit(5);
+		ipu_di_use_count[g_dc_di_assignment[5]]--;
+		ipu_dc_use_count--;
+		ipu_dp_use_count--;
+		ipu_dmfc_use_count--;
+		break;
+	case MEM_FG_SYNC:
+		_ipu_dp_uninit(channel);
+		ipu_dc_use_count--;
+		ipu_dp_use_count--;
+		ipu_dmfc_use_count--;
+		break;
+	case DIRECT_ASYNC0:
+		_ipu_dc_uninit(8);
+		ipu_di_use_count[g_dc_di_assignment[8]]--;
+		ipu_dc_use_count--;
+		break;
+	case DIRECT_ASYNC1:
+		_ipu_dc_uninit(9);
+		ipu_di_use_count[g_dc_di_assignment[9]]--;
+		ipu_dc_use_count--;
+		break;
+	default:
+		break;
+	}
+
+	g_channel_init_mask &= ~(1L << IPU_CHAN_ID(channel));
+
+	ipu_conf = __raw_readl(IPU_CONF);
+
+	if (ipu_ic_use_count == 0)
+		ipu_conf &= ~IPU_CONF_IC_EN;
+	if (ipu_vdi_use_count == 0) {
+		ipu_conf &= ~IPU_CONF_VDI_EN;
+		ipu_conf &= ~IPU_CONF_IC_INPUT;
+	}
+	if (ipu_rot_use_count == 0)
+		ipu_conf &= ~IPU_CONF_ROT_EN;
+	if (ipu_dc_use_count == 0)
+		ipu_conf &= ~IPU_CONF_DC_EN;
+	if (ipu_dp_use_count == 0)
+		ipu_conf &= ~IPU_CONF_DP_EN;
+	if (ipu_dmfc_use_count == 0)
+		ipu_conf &= ~IPU_CONF_DMFC_EN;
+	if (ipu_di_use_count[0] == 0) {
+		ipu_conf &= ~IPU_CONF_DI0_EN;
+		clk_disable(g_di_clk[0]);
+	}
+	if (ipu_di_use_count[1] == 0) {
+		ipu_conf &= ~IPU_CONF_DI1_EN;
+		clk_disable(g_di_clk[1]);
+	}
+	if (ipu_smfc_use_count == 0)
+		ipu_conf &= ~IPU_CONF_SMFC_EN;
+	if (ipu_csi_use_count[0] == 0)
+		ipu_conf &= ~IPU_CONF_CSI0_EN;
+	if (ipu_csi_use_count[1] == 0)
+		ipu_conf &= ~IPU_CONF_CSI1_EN;
+
+	__raw_writel(ipu_conf, IPU_CONF);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	if (ipu_conf == 0) {
+		clk_disable(g_ipu_clk);
+		g_ipu_clk_enabled = false;
+	}
+
+	WARN_ON(ipu_ic_use_count < 0);
+	WARN_ON(ipu_vdi_use_count < 0);
+	WARN_ON(ipu_rot_use_count < 0);
+	WARN_ON(ipu_dc_use_count < 0);
+	WARN_ON(ipu_dp_use_count < 0);
+	WARN_ON(ipu_dmfc_use_count < 0);
+	WARN_ON(ipu_smfc_use_count < 0);
+}
+EXPORT_SYMBOL(ipu_uninit_channel);
+
+/*!
+ * This function is called to initialize a buffer for logical IPU channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       type            Input parameter which buffer to initialize.
+ *
+ * @param       pixel_fmt       Input parameter for pixel format of buffer.
+ *                              Pixel format is a FOURCC ASCII code.
+ *
+ * @param       width           Input parameter for width of buffer in pixels.
+ *
+ * @param       height          Input parameter for height of buffer in pixels.
+ *
+ * @param       stride          Input parameter for stride length of buffer
+ *                              in pixels.
+ *
+ * @param       rot_mode        Input parameter for rotation setting of buffer.
+ *                              A rotation setting other than
+ *                              IPU_ROTATE_VERT_FLIP
+ *                              should only be used for input buffers of
+ *                              rotation channels.
+ *
+ * @param       phyaddr_0       Input parameter buffer 0 physical address.
+ *
+ * @param       phyaddr_1       Input parameter buffer 1 physical address.
+ *                              Setting this to a value other than NULL enables
+ *                              double buffering mode.
+ *
+ * @param       u		private u offset for additional cropping,
+ *				zero if not used.
+ *
+ * @param       v		private v offset for additional cropping,
+ *				zero if not used.
+ *
+ * @return      Returns 0 on success or negative error code on fail
+ */
+int32_t ipu_init_channel_buffer(ipu_channel_t channel, ipu_buffer_t type,
+				uint32_t pixel_fmt,
+				uint16_t width, uint16_t height,
+				uint32_t stride,
+				ipu_rotate_mode_t rot_mode,
+				dma_addr_t phyaddr_0, dma_addr_t phyaddr_1,
+				uint32_t u, uint32_t v)
+{
+	unsigned long lock_flags;
+	uint32_t reg;
+	uint32_t dma_chan;
+	uint32_t burst_size;
+
+	dma_chan = channel_2_dma(channel, type);
+	if (!idma_is_valid(dma_chan))
+		return -EINVAL;
+
+	if (stride < width * bytes_per_pixel(pixel_fmt))
+		stride = width * bytes_per_pixel(pixel_fmt);
+
+	if (stride % 4) {
+		dev_err(g_ipu_dev,
+			"Stride not 32-bit aligned, stride = %d\n", stride);
+		return -EINVAL;
+	}
+	/* IC & IRT channels' width must be multiple of 8 pixels */
+	if ((_ipu_is_ic_chan(dma_chan) || _ipu_is_irt_chan(dma_chan))
+		&& (width % 8)) {
+		dev_err(g_ipu_dev, "Width must be 8 pixel multiple\n");
+		return -EINVAL;
+	}
+
+	/* Build parameter memory data for DMA channel */
+	_ipu_ch_param_init(dma_chan, pixel_fmt, width, height, stride, u, v, 0,
+			   phyaddr_0, phyaddr_1);
+
+	/* Set correlative channel parameter of local alpha channel */
+	if ((_ipu_is_ic_graphic_chan(dma_chan) ||
+	     _ipu_is_dp_graphic_chan(dma_chan)) &&
+	    (g_thrd_chan_en[IPU_CHAN_ID(channel)] == true)) {
+		_ipu_ch_param_set_alpha_use_separate_channel(dma_chan, true);
+		_ipu_ch_param_set_alpha_buffer_memory(dma_chan);
+		_ipu_ch_param_set_alpha_condition_read(dma_chan);
+		/* fix alpha width as 8 and burst size as 16*/
+		_ipu_ch_params_set_alpha_width(dma_chan, 8);
+		_ipu_ch_param_set_burst_size(dma_chan, 16);
+	} else if (_ipu_is_ic_graphic_chan(dma_chan) &&
+		   ipu_pixel_format_has_alpha(pixel_fmt))
+		_ipu_ch_param_set_alpha_use_separate_channel(dma_chan, false);
+
+	if (rot_mode)
+		_ipu_ch_param_set_rotation(dma_chan, rot_mode);
+
+	/* IC and ROT channels have restriction of 8 or 16 pix burst length */
+	if (_ipu_is_ic_chan(dma_chan)) {
+		if ((width % 16) == 0)
+			_ipu_ch_param_set_burst_size(dma_chan, 16);
+		else
+			_ipu_ch_param_set_burst_size(dma_chan, 8);
+	} else if (_ipu_is_irt_chan(dma_chan)) {
+		_ipu_ch_param_set_burst_size(dma_chan, 8);
+		_ipu_ch_param_set_block_mode(dma_chan);
+	} else if (_ipu_is_dmfc_chan(dma_chan))
+		_ipu_dmfc_set_wait4eot(dma_chan, width);
+
+	if (_ipu_chan_is_interlaced(channel)) {
+		_ipu_ch_param_set_interlaced_scan(dma_chan);
+	}
+
+	if (_ipu_is_ic_chan(dma_chan) || _ipu_is_irt_chan(dma_chan)) {
+		burst_size = _ipu_ch_param_get_burst_size(dma_chan);
+		_ipu_ic_idma_init(dma_chan, width, height, burst_size,
+			rot_mode);
+	} else if (_ipu_is_smfc_chan(dma_chan)) {
+		burst_size = _ipu_ch_param_get_burst_size(dma_chan);
+		if ((pixel_fmt == IPU_PIX_FMT_GENERIC) &&
+			((_ipu_ch_param_get_bpp(dma_chan) == 5) ||
+			(_ipu_ch_param_get_bpp(dma_chan) == 3)))
+			burst_size = burst_size >> 4;
+		else
+			burst_size = burst_size >> 2;
+		_ipu_smfc_set_burst_size(channel, burst_size-1);
+	}
+
+	if (idma_is_set(IDMAC_CHA_PRI, dma_chan))
+		_ipu_ch_param_set_high_priority(dma_chan);
+
+	_ipu_ch_param_dump(dma_chan);
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(IPU_CHA_DB_MODE_SEL(dma_chan));
+	if (phyaddr_1)
+		reg |= idma_mask(dma_chan);
+	else
+		reg &= ~idma_mask(dma_chan);
+	__raw_writel(reg, IPU_CHA_DB_MODE_SEL(dma_chan));
+
+	/* Reset to buffer 0 */
+	__raw_writel(idma_mask(dma_chan), IPU_CHA_CUR_BUF(dma_chan));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_init_channel_buffer);
+
+/*!
+ * This function is called to update the physical address of a buffer for
+ * a logical IPU channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       type            Input parameter which buffer to initialize.
+ *
+ * @param       bufNum          Input parameter for buffer number to update.
+ *                              0 or 1 are the only valid values.
+ *
+ * @param       phyaddr         Input parameter buffer physical address.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail. This function will fail if the buffer is set to ready.
+ */
+int32_t ipu_update_channel_buffer(ipu_channel_t channel, ipu_buffer_t type,
+				  uint32_t bufNum, dma_addr_t phyaddr)
+{
+	uint32_t reg;
+	int ret = 0;
+	unsigned long lock_flags;
+	uint32_t dma_chan = channel_2_dma(channel, type);
+	if (dma_chan == IDMA_CHAN_INVALID)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if (bufNum == 0)
+		reg = __raw_readl(IPU_CHA_BUF0_RDY(dma_chan));
+	else
+		reg = __raw_readl(IPU_CHA_BUF1_RDY(dma_chan));
+
+	if ((reg & idma_mask(dma_chan)) == 0)
+		_ipu_ch_param_set_buffer(dma_chan, bufNum, phyaddr);
+	else
+		ret = -EACCES;
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return ret;
+}
+EXPORT_SYMBOL(ipu_update_channel_buffer);
+
+
+/*!
+ * This function is called to initialize a buffer for logical IPU channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       type            Input parameter which buffer to initialize.
+ *
+ * @param       pixel_fmt       Input parameter for pixel format of buffer.
+ *                              Pixel format is a FOURCC ASCII code.
+ *
+ * @param       width           Input parameter for width of buffer in pixels.
+ *
+ * @param       height          Input parameter for height of buffer in pixels.
+ *
+ * @param       stride          Input parameter for stride length of buffer
+ *                              in pixels.
+ *
+ * @param       u				predefined private u offset for additional cropping,
+ *								zero if not used.
+ *
+ * @param       v				predefined private v offset for additional cropping,
+ *								zero if not used.
+ *
+ * @param		vertical_offset vertical offset for Y coordinate
+ * 								in the existed frame
+ *
+ *
+ * @param		horizontal_offset horizontal offset for X coordinate
+ * 								in the existed frame
+ *
+ *
+ * @return      Returns 0 on success or negative error code on fail
+ *              This function will fail if any buffer is set to ready.
+ */
+
+int32_t ipu_update_channel_offset(ipu_channel_t channel, ipu_buffer_t type,
+				uint32_t pixel_fmt,
+				uint16_t width, uint16_t height,
+				uint32_t stride,
+				uint32_t u, uint32_t v,
+				uint32_t vertical_offset, uint32_t horizontal_offset)
+{
+	uint32_t reg;
+	int ret = 0;
+	unsigned long lock_flags;
+	uint32_t dma_chan = channel_2_dma(channel, type);
+
+	if (dma_chan == IDMA_CHAN_INVALID)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if ((__raw_readl(IPU_CHA_BUF0_RDY(dma_chan)) & idma_mask(dma_chan)) ||
+		(__raw_readl(IPU_CHA_BUF0_RDY(dma_chan)) & idma_mask(dma_chan)))
+		ret = -EACCES;
+	else
+		_ipu_ch_offset_update(dma_chan,
+								pixel_fmt,
+								width,
+								height,
+								stride,
+								u,
+								v,
+								0,
+								vertical_offset,
+								horizontal_offset);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return ret;
+}
+EXPORT_SYMBOL(ipu_update_channel_offset);
+
+
+/*!
+ * This function is called to set a channel's buffer as ready.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       type            Input parameter which buffer to initialize.
+ *
+ * @param       bufNum          Input parameter for which buffer number set to
+ *                              ready state.
+ *
+ * @return      Returns 0 on success or negative error code on fail
+ */
+int32_t ipu_select_buffer(ipu_channel_t channel, ipu_buffer_t type,
+			  uint32_t bufNum)
+{
+	uint32_t dma_chan = channel_2_dma(channel, type);
+
+	if (dma_chan == IDMA_CHAN_INVALID)
+		return -EINVAL;
+
+	if (bufNum == 0) {
+		/*Mark buffer 0 as ready. */
+		__raw_writel(idma_mask(dma_chan), IPU_CHA_BUF0_RDY(dma_chan));
+	} else {
+		/*Mark buffer 1 as ready. */
+		__raw_writel(idma_mask(dma_chan), IPU_CHA_BUF1_RDY(dma_chan));
+	}
+	if (channel == MEM_VDI_PRP_VF_MEM)
+		_ipu_vdi_toggle_top_field_man();
+	return 0;
+}
+EXPORT_SYMBOL(ipu_select_buffer);
+
+/*!
+ * This function is called to set a channel's buffer as ready.
+ *
+ * @param       bufNum          Input parameter for which buffer number set to
+ *                              ready state.
+ *
+ * @return      Returns 0 on success or negative error code on fail
+ */
+int32_t ipu_select_multi_vdi_buffer(uint32_t bufNum)
+{
+
+	uint32_t dma_chan = channel_2_dma(MEM_VDI_PRP_VF_MEM, IPU_INPUT_BUFFER);
+	uint32_t mask_bit =
+		idma_mask(channel_2_dma(MEM_VDI_PRP_VF_MEM_P, IPU_INPUT_BUFFER))|
+		idma_mask(dma_chan)|
+		idma_mask(channel_2_dma(MEM_VDI_PRP_VF_MEM_N, IPU_INPUT_BUFFER));
+
+	if (bufNum == 0) {
+		/*Mark buffer 0 as ready. */
+		__raw_writel(mask_bit, IPU_CHA_BUF0_RDY(dma_chan));
+	} else {
+		/*Mark buffer 1 as ready. */
+		__raw_writel(mask_bit, IPU_CHA_BUF1_RDY(dma_chan));
+	}
+	_ipu_vdi_toggle_top_field_man();
+	return 0;
+}
+EXPORT_SYMBOL(ipu_select_multi_vdi_buffer);
+
+#define NA	-1
+static int proc_dest_sel[] =
+  { 0, 1, 1, 3, 5, 5, 4, 7, 8, 9, 10, 11, 12, 14, 15, 16,
+    0, 1, 1, 5, 5, 5, 5, 5, 7, 8, 9, 10, 11, 12, 14, 31 };
+static int proc_src_sel[] = { 0, 6, 7, 6, 7, 8, 5, NA, NA, NA,
+  NA, NA, NA, NA, NA,  1,  2,  3,  4,  7,  8, NA, NA, NA };
+static int disp_src_sel[] = { 0, 6, 7, 8, 3, 4, 5, NA, NA, NA,
+  NA, NA, NA, NA, NA,  1, NA,  2, NA,  3,  4,  4,  4,  4 };
+
+
+/*!
+ * This function links 2 channels together for automatic frame
+ * synchronization. The output of the source channel is linked to the input of
+ * the destination channel.
+ *
+ * @param       src_ch          Input parameter for the logical channel ID of
+ *                              the source channel.
+ *
+ * @param       dest_ch         Input parameter for the logical channel ID of
+ *                              the destination channel.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int32_t ipu_link_channels(ipu_channel_t src_ch, ipu_channel_t dest_ch)
+{
+	int retval = 0;
+	unsigned long lock_flags;
+	uint32_t fs_proc_flow1;
+	uint32_t fs_proc_flow2;
+	uint32_t fs_proc_flow3;
+	uint32_t fs_disp_flow1;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	fs_proc_flow1 = __raw_readl(IPU_FS_PROC_FLOW1);
+	fs_proc_flow2 = __raw_readl(IPU_FS_PROC_FLOW2);
+	fs_proc_flow3 = __raw_readl(IPU_FS_PROC_FLOW3);
+	fs_disp_flow1 = __raw_readl(IPU_FS_DISP_FLOW1);
+
+	switch (src_ch) {
+	case CSI_MEM0:
+		fs_proc_flow3 &= ~FS_SMFC0_DEST_SEL_MASK;
+		fs_proc_flow3 |=
+			proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+			FS_SMFC0_DEST_SEL_OFFSET;
+		break;
+	case CSI_MEM1:
+		fs_proc_flow3 &= ~FS_SMFC1_DEST_SEL_MASK;
+		fs_proc_flow3 |=
+			proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+			FS_SMFC1_DEST_SEL_OFFSET;
+		break;
+	case CSI_MEM2:
+		fs_proc_flow3 &= ~FS_SMFC2_DEST_SEL_MASK;
+		fs_proc_flow3 |=
+			proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+			FS_SMFC2_DEST_SEL_OFFSET;
+		break;
+	case CSI_MEM3:
+		fs_proc_flow3 &= ~FS_SMFC3_DEST_SEL_MASK;
+		fs_proc_flow3 |=
+			proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+			FS_SMFC3_DEST_SEL_OFFSET;
+		break;
+	case CSI_PRP_ENC_MEM:
+		fs_proc_flow2 &= ~FS_PRPENC_DEST_SEL_MASK;
+		fs_proc_flow2 |=
+			proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+			FS_PRPENC_DEST_SEL_OFFSET;
+		break;
+	case CSI_PRP_VF_MEM:
+		fs_proc_flow2 &= ~FS_PRPVF_DEST_SEL_MASK;
+		fs_proc_flow2 |=
+			proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+			FS_PRPVF_DEST_SEL_OFFSET;
+		break;
+	case MEM_PP_MEM:
+		fs_proc_flow2 &= ~FS_PP_DEST_SEL_MASK;
+		fs_proc_flow2 |=
+		    proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+		    FS_PP_DEST_SEL_OFFSET;
+		break;
+	case MEM_ROT_PP_MEM:
+		fs_proc_flow2 &= ~FS_PP_ROT_DEST_SEL_MASK;
+		fs_proc_flow2 |=
+		    proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+		    FS_PP_ROT_DEST_SEL_OFFSET;
+		break;
+	case MEM_PRP_ENC_MEM:
+		fs_proc_flow2 &= ~FS_PRPENC_DEST_SEL_MASK;
+		fs_proc_flow2 |=
+		    proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+		    FS_PRPENC_DEST_SEL_OFFSET;
+		break;
+	case MEM_ROT_ENC_MEM:
+		fs_proc_flow2 &= ~FS_PRPENC_ROT_DEST_SEL_MASK;
+		fs_proc_flow2 |=
+		    proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+		    FS_PRPENC_ROT_DEST_SEL_OFFSET;
+		break;
+	case MEM_PRP_VF_MEM:
+		fs_proc_flow2 &= ~FS_PRPVF_DEST_SEL_MASK;
+		fs_proc_flow2 |=
+		    proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+		    FS_PRPVF_DEST_SEL_OFFSET;
+		break;
+	case MEM_VDI_PRP_VF_MEM:
+		fs_proc_flow2 &= ~FS_PRPVF_DEST_SEL_MASK;
+		fs_proc_flow2 |=
+		    proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+		    FS_PRPVF_DEST_SEL_OFFSET;
+		break;
+	case MEM_ROT_VF_MEM:
+		fs_proc_flow2 &= ~FS_PRPVF_ROT_DEST_SEL_MASK;
+		fs_proc_flow2 |=
+		    proc_dest_sel[IPU_CHAN_ID(dest_ch)] <<
+		    FS_PRPVF_ROT_DEST_SEL_OFFSET;
+		break;
+	default:
+		retval = -EINVAL;
+		goto err;
+	}
+
+	switch (dest_ch) {
+	case MEM_PP_MEM:
+		fs_proc_flow1 &= ~FS_PP_SRC_SEL_MASK;
+		fs_proc_flow1 |=
+		    proc_src_sel[IPU_CHAN_ID(src_ch)] << FS_PP_SRC_SEL_OFFSET;
+		break;
+	case MEM_ROT_PP_MEM:
+		fs_proc_flow1 &= ~FS_PP_ROT_SRC_SEL_MASK;
+		fs_proc_flow1 |=
+		    proc_src_sel[IPU_CHAN_ID(src_ch)] <<
+		    FS_PP_ROT_SRC_SEL_OFFSET;
+		break;
+	case MEM_PRP_ENC_MEM:
+		fs_proc_flow1 &= ~FS_PRP_SRC_SEL_MASK;
+		fs_proc_flow1 |=
+		    proc_src_sel[IPU_CHAN_ID(src_ch)] << FS_PRP_SRC_SEL_OFFSET;
+		break;
+	case MEM_ROT_ENC_MEM:
+		fs_proc_flow1 &= ~FS_PRPENC_ROT_SRC_SEL_MASK;
+		fs_proc_flow1 |=
+		    proc_src_sel[IPU_CHAN_ID(src_ch)] <<
+		    FS_PRPENC_ROT_SRC_SEL_OFFSET;
+		break;
+	case MEM_PRP_VF_MEM:
+		fs_proc_flow1 &= ~FS_PRP_SRC_SEL_MASK;
+		fs_proc_flow1 |=
+		    proc_src_sel[IPU_CHAN_ID(src_ch)] << FS_PRP_SRC_SEL_OFFSET;
+		break;
+	case MEM_VDI_PRP_VF_MEM:
+		fs_proc_flow1 &= ~FS_PRP_SRC_SEL_MASK;
+		fs_proc_flow1 |=
+		    proc_src_sel[IPU_CHAN_ID(src_ch)] << FS_PRP_SRC_SEL_OFFSET;
+		break;
+	case MEM_ROT_VF_MEM:
+		fs_proc_flow1 &= ~FS_PRPVF_ROT_SRC_SEL_MASK;
+		fs_proc_flow1 |=
+		    proc_src_sel[IPU_CHAN_ID(src_ch)] <<
+		    FS_PRPVF_ROT_SRC_SEL_OFFSET;
+		break;
+	case MEM_DC_SYNC:
+		fs_disp_flow1 &= ~FS_DC1_SRC_SEL_MASK;
+		fs_disp_flow1 |=
+		    disp_src_sel[IPU_CHAN_ID(src_ch)] << FS_DC1_SRC_SEL_OFFSET;
+		break;
+	case MEM_BG_SYNC:
+		fs_disp_flow1 &= ~FS_DP_SYNC0_SRC_SEL_MASK;
+		fs_disp_flow1 |=
+		    disp_src_sel[IPU_CHAN_ID(src_ch)] <<
+		    FS_DP_SYNC0_SRC_SEL_OFFSET;
+		break;
+	case MEM_FG_SYNC:
+		fs_disp_flow1 &= ~FS_DP_SYNC1_SRC_SEL_MASK;
+		fs_disp_flow1 |=
+		    disp_src_sel[IPU_CHAN_ID(src_ch)] <<
+		    FS_DP_SYNC1_SRC_SEL_OFFSET;
+		break;
+	case MEM_DC_ASYNC:
+		fs_disp_flow1 &= ~FS_DC2_SRC_SEL_MASK;
+		fs_disp_flow1 |=
+		    disp_src_sel[IPU_CHAN_ID(src_ch)] << FS_DC2_SRC_SEL_OFFSET;
+		break;
+	case MEM_BG_ASYNC0:
+		fs_disp_flow1 &= ~FS_DP_ASYNC0_SRC_SEL_MASK;
+		fs_disp_flow1 |=
+		    disp_src_sel[IPU_CHAN_ID(src_ch)] <<
+		    FS_DP_ASYNC0_SRC_SEL_OFFSET;
+		break;
+	case MEM_FG_ASYNC0:
+		fs_disp_flow1 &= ~FS_DP_ASYNC1_SRC_SEL_MASK;
+		fs_disp_flow1 |=
+		    disp_src_sel[IPU_CHAN_ID(src_ch)] <<
+		    FS_DP_ASYNC1_SRC_SEL_OFFSET;
+		break;
+	default:
+		retval = -EINVAL;
+		goto err;
+	}
+
+	__raw_writel(fs_proc_flow1, IPU_FS_PROC_FLOW1);
+	__raw_writel(fs_proc_flow2, IPU_FS_PROC_FLOW2);
+	__raw_writel(fs_proc_flow3, IPU_FS_PROC_FLOW3);
+	__raw_writel(fs_disp_flow1, IPU_FS_DISP_FLOW1);
+
+err:
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return retval;
+}
+EXPORT_SYMBOL(ipu_link_channels);
+
+/*!
+ * This function unlinks 2 channels and disables automatic frame
+ * synchronization.
+ *
+ * @param       src_ch          Input parameter for the logical channel ID of
+ *                              the source channel.
+ *
+ * @param       dest_ch         Input parameter for the logical channel ID of
+ *                              the destination channel.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int32_t ipu_unlink_channels(ipu_channel_t src_ch, ipu_channel_t dest_ch)
+{
+	int retval = 0;
+	unsigned long lock_flags;
+	uint32_t fs_proc_flow1;
+	uint32_t fs_proc_flow2;
+	uint32_t fs_proc_flow3;
+	uint32_t fs_disp_flow1;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	fs_proc_flow1 = __raw_readl(IPU_FS_PROC_FLOW1);
+	fs_proc_flow2 = __raw_readl(IPU_FS_PROC_FLOW2);
+	fs_proc_flow3 = __raw_readl(IPU_FS_PROC_FLOW3);
+	fs_disp_flow1 = __raw_readl(IPU_FS_DISP_FLOW1);
+
+	switch (src_ch) {
+	case CSI_MEM0:
+		fs_proc_flow3 &= ~FS_SMFC0_DEST_SEL_MASK;
+		break;
+	case CSI_MEM1:
+		fs_proc_flow3 &= ~FS_SMFC1_DEST_SEL_MASK;
+		break;
+	case CSI_MEM2:
+		fs_proc_flow3 &= ~FS_SMFC2_DEST_SEL_MASK;
+		break;
+	case CSI_MEM3:
+		fs_proc_flow3 &= ~FS_SMFC3_DEST_SEL_MASK;
+		break;
+	case CSI_PRP_ENC_MEM:
+		fs_proc_flow2 &= ~FS_PRPENC_DEST_SEL_MASK;
+		break;
+	case CSI_PRP_VF_MEM:
+		fs_proc_flow2 &= ~FS_PRPVF_DEST_SEL_MASK;
+		break;
+	case MEM_PP_MEM:
+		fs_proc_flow2 &= ~FS_PP_DEST_SEL_MASK;
+		break;
+	case MEM_ROT_PP_MEM:
+		fs_proc_flow2 &= ~FS_PP_ROT_DEST_SEL_MASK;
+		break;
+	case MEM_PRP_ENC_MEM:
+		fs_proc_flow2 &= ~FS_PRPENC_DEST_SEL_MASK;
+		break;
+	case MEM_ROT_ENC_MEM:
+		fs_proc_flow2 &= ~FS_PRPENC_ROT_DEST_SEL_MASK;
+		break;
+	case MEM_PRP_VF_MEM:
+		fs_proc_flow2 &= ~FS_PRPVF_DEST_SEL_MASK;
+		break;
+	case MEM_VDI_PRP_VF_MEM:
+		fs_proc_flow2 &= ~FS_PRPVF_DEST_SEL_MASK;
+		break;
+	case MEM_ROT_VF_MEM:
+		fs_proc_flow2 &= ~FS_PRPVF_ROT_DEST_SEL_MASK;
+		break;
+	default:
+		retval = -EINVAL;
+		goto err;
+	}
+
+	switch (dest_ch) {
+	case MEM_PP_MEM:
+		fs_proc_flow1 &= ~FS_PP_SRC_SEL_MASK;
+		break;
+	case MEM_ROT_PP_MEM:
+		fs_proc_flow1 &= ~FS_PP_ROT_SRC_SEL_MASK;
+		break;
+	case MEM_PRP_ENC_MEM:
+		fs_proc_flow1 &= ~FS_PRP_SRC_SEL_MASK;
+		break;
+	case MEM_ROT_ENC_MEM:
+		fs_proc_flow1 &= ~FS_PRPENC_ROT_SRC_SEL_MASK;
+		break;
+	case MEM_PRP_VF_MEM:
+		fs_proc_flow1 &= ~FS_PRP_SRC_SEL_MASK;
+		break;
+	case MEM_VDI_PRP_VF_MEM:
+		fs_proc_flow1 &= ~FS_PRP_SRC_SEL_MASK;
+		break;
+	case MEM_ROT_VF_MEM:
+		fs_proc_flow1 &= ~FS_PRPVF_ROT_SRC_SEL_MASK;
+		break;
+	case MEM_DC_SYNC:
+		fs_disp_flow1 &= ~FS_DC1_SRC_SEL_MASK;
+		break;
+	case MEM_BG_SYNC:
+		fs_disp_flow1 &= ~FS_DP_SYNC0_SRC_SEL_MASK;
+		break;
+	case MEM_FG_SYNC:
+		fs_disp_flow1 &= ~FS_DP_SYNC1_SRC_SEL_MASK;
+		break;
+	case MEM_DC_ASYNC:
+		fs_disp_flow1 &= ~FS_DC2_SRC_SEL_MASK;
+		break;
+	case MEM_BG_ASYNC0:
+		fs_disp_flow1 &= ~FS_DP_ASYNC0_SRC_SEL_MASK;
+		break;
+	case MEM_FG_ASYNC0:
+		fs_disp_flow1 &= ~FS_DP_ASYNC1_SRC_SEL_MASK;
+		break;
+	default:
+		retval = -EINVAL;
+		goto err;
+	}
+
+	__raw_writel(fs_proc_flow1, IPU_FS_PROC_FLOW1);
+	__raw_writel(fs_proc_flow2, IPU_FS_PROC_FLOW2);
+	__raw_writel(fs_proc_flow3, IPU_FS_PROC_FLOW3);
+	__raw_writel(fs_disp_flow1, IPU_FS_DISP_FLOW1);
+
+err:
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return retval;
+}
+EXPORT_SYMBOL(ipu_unlink_channels);
+
+/*!
+ * This function check whether a logical channel was enabled.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @return      This function returns 1 while request channel is enabled or
+ *              0 for not enabled.
+ */
+int32_t ipu_is_channel_busy(ipu_channel_t channel)
+{
+	uint32_t reg;
+	uint32_t in_dma;
+	uint32_t out_dma;
+
+	out_dma = channel_2_dma(channel, IPU_OUTPUT_BUFFER);
+	in_dma = channel_2_dma(channel, IPU_VIDEO_IN_BUFFER);
+
+	reg = __raw_readl(IDMAC_CHA_EN(in_dma));
+	if (reg & idma_mask(in_dma))
+		return 1;
+	reg = __raw_readl(IDMAC_CHA_EN(out_dma));
+	if (reg & idma_mask(out_dma))
+		return 1;
+	return 0;
+}
+EXPORT_SYMBOL(ipu_is_channel_busy);
+
+/*!
+ * This function enables a logical channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int32_t ipu_enable_channel(ipu_channel_t channel)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+	uint32_t in_dma;
+	uint32_t out_dma;
+	uint32_t sec_dma;
+	uint32_t thrd_dma;
+
+	if (g_channel_enable_mask & (1L << IPU_CHAN_ID(channel))) {
+		dev_err(g_ipu_dev, "Warning: channel already enabled %d\n",
+			IPU_CHAN_ID(channel));
+	}
+
+	/* Get input and output dma channels */
+	out_dma = channel_2_dma(channel, IPU_OUTPUT_BUFFER);
+	in_dma = channel_2_dma(channel, IPU_VIDEO_IN_BUFFER);
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if (idma_is_valid(in_dma)) {
+		reg = __raw_readl(IDMAC_CHA_EN(in_dma));
+		__raw_writel(reg | idma_mask(in_dma), IDMAC_CHA_EN(in_dma));
+	}
+	if (idma_is_valid(out_dma)) {
+		reg = __raw_readl(IDMAC_CHA_EN(out_dma));
+		__raw_writel(reg | idma_mask(out_dma), IDMAC_CHA_EN(out_dma));
+	}
+
+	if ((g_sec_chan_en[IPU_CHAN_ID(channel)]) &&
+		((channel == MEM_PP_MEM) || (channel == MEM_PRP_VF_MEM) ||
+		 (channel == MEM_VDI_PRP_VF_MEM))) {
+		sec_dma = channel_2_dma(channel, IPU_GRAPH_IN_BUFFER);
+		reg = __raw_readl(IDMAC_CHA_EN(sec_dma));
+		__raw_writel(reg | idma_mask(sec_dma), IDMAC_CHA_EN(sec_dma));
+	}
+	if ((g_thrd_chan_en[IPU_CHAN_ID(channel)]) &&
+		((channel == MEM_PP_MEM) || (channel == MEM_PRP_VF_MEM))) {
+		thrd_dma = channel_2_dma(channel, IPU_ALPHA_IN_BUFFER);
+		reg = __raw_readl(IDMAC_CHA_EN(thrd_dma));
+		__raw_writel(reg | idma_mask(thrd_dma), IDMAC_CHA_EN(thrd_dma));
+
+		sec_dma = channel_2_dma(channel, IPU_GRAPH_IN_BUFFER);
+		reg = __raw_readl(IDMAC_SEP_ALPHA);
+		__raw_writel(reg | idma_mask(sec_dma), IDMAC_SEP_ALPHA);
+	} else if ((g_thrd_chan_en[IPU_CHAN_ID(channel)]) &&
+		   ((channel == MEM_BG_SYNC) || (channel == MEM_FG_SYNC))) {
+		thrd_dma = channel_2_dma(channel, IPU_ALPHA_IN_BUFFER);
+		reg = __raw_readl(IDMAC_CHA_EN(thrd_dma));
+		__raw_writel(reg | idma_mask(thrd_dma), IDMAC_CHA_EN(thrd_dma));
+		reg = __raw_readl(IDMAC_SEP_ALPHA);
+		__raw_writel(reg | idma_mask(in_dma), IDMAC_SEP_ALPHA);
+	}
+
+	if ((channel == MEM_DC_SYNC) || (channel == MEM_BG_SYNC) ||
+	    (channel == MEM_FG_SYNC))
+		_ipu_dp_dc_enable(channel);
+
+	if (_ipu_is_ic_chan(in_dma) || _ipu_is_ic_chan(out_dma) ||
+		_ipu_is_irt_chan(in_dma) || _ipu_is_irt_chan(out_dma))
+		_ipu_ic_enable_task(channel);
+
+	g_channel_enable_mask |= 1L << IPU_CHAN_ID(channel);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_enable_channel);
+
+static irqreturn_t disable_chan_irq_handler(int irq, void *dev_id)
+{
+	struct completion *comp = dev_id;
+
+	complete(comp);
+	return IRQ_HANDLED;
+}
+
+/*!
+ * This function disables a logical channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       wait_for_stop   Flag to set whether to wait for channel end
+ *                              of frame or return immediately.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int32_t ipu_disable_channel(ipu_channel_t channel, bool wait_for_stop)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+	uint32_t in_dma;
+	uint32_t out_dma;
+	uint32_t sec_dma = NO_DMA;
+	uint32_t thrd_dma = NO_DMA;
+
+	if ((g_channel_enable_mask & (1L << IPU_CHAN_ID(channel))) == 0) {
+		dev_err(g_ipu_dev, "Channel already disabled %d\n",
+			IPU_CHAN_ID(channel));
+		return 0;
+	}
+
+	/* Get input and output dma channels */
+	out_dma = channel_2_dma(channel, IPU_OUTPUT_BUFFER);
+	in_dma = channel_2_dma(channel, IPU_VIDEO_IN_BUFFER);
+
+	if ((idma_is_valid(in_dma) &&
+		!idma_is_set(IDMAC_CHA_EN, in_dma))
+		&& (idma_is_valid(out_dma) &&
+		!idma_is_set(IDMAC_CHA_EN, out_dma)))
+		return -EINVAL;
+
+	if (g_sec_chan_en[IPU_CHAN_ID(channel)])
+		sec_dma = channel_2_dma(channel, IPU_GRAPH_IN_BUFFER);
+	if (g_thrd_chan_en[IPU_CHAN_ID(channel)]) {
+		sec_dma = channel_2_dma(channel, IPU_GRAPH_IN_BUFFER);
+		thrd_dma = channel_2_dma(channel, IPU_ALPHA_IN_BUFFER);
+	}
+
+	if ((channel == MEM_BG_SYNC) || (channel == MEM_FG_SYNC) ||
+	    (channel == MEM_DC_SYNC)) {
+		_ipu_dp_dc_disable(channel, false);
+	} else if (wait_for_stop) {
+		while (idma_is_set(IDMAC_CHA_BUSY, in_dma) ||
+		       idma_is_set(IDMAC_CHA_BUSY, out_dma) ||
+			(g_sec_chan_en[IPU_CHAN_ID(channel)] &&
+			idma_is_set(IDMAC_CHA_BUSY, sec_dma)) ||
+			(g_thrd_chan_en[IPU_CHAN_ID(channel)] &&
+			idma_is_set(IDMAC_CHA_BUSY, thrd_dma)) ||
+		       (_ipu_channel_status(channel) == TASK_STAT_ACTIVE)) {
+			uint32_t ret, irq = out_dma;
+			DECLARE_COMPLETION_ONSTACK(disable_comp);
+
+			if (idma_is_set(IDMAC_CHA_BUSY, out_dma))
+				irq = out_dma;
+			if (g_sec_chan_en[IPU_CHAN_ID(channel)] &&
+				idma_is_set(IDMAC_CHA_BUSY, sec_dma))
+				irq = sec_dma;
+			if (g_thrd_chan_en[IPU_CHAN_ID(channel)] &&
+				idma_is_set(IDMAC_CHA_BUSY, thrd_dma))
+				irq = thrd_dma;
+			if (idma_is_set(IDMAC_CHA_BUSY, in_dma))
+				irq = in_dma;
+
+			ret = ipu_request_irq(irq, disable_chan_irq_handler, 0, NULL, &disable_comp);
+			if (ret < 0) {
+				dev_err(g_ipu_dev, "irq %d in use\n", irq);
+			} else {
+				ret = wait_for_completion_timeout(&disable_comp, msecs_to_jiffies(50));
+				ipu_free_irq(irq, &disable_comp);
+				if (ret == msecs_to_jiffies(50))
+					ipu_dump_registers();
+			}
+		}
+	}
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	/* Disable IC task */
+	if (_ipu_is_ic_chan(in_dma) || _ipu_is_ic_chan(out_dma) ||
+		_ipu_is_irt_chan(in_dma) || _ipu_is_irt_chan(out_dma))
+		_ipu_ic_disable_task(channel);
+
+	/* Disable DMA channel(s) */
+	if (idma_is_valid(in_dma)) {
+		reg = __raw_readl(IDMAC_CHA_EN(in_dma));
+		__raw_writel(reg & ~idma_mask(in_dma), IDMAC_CHA_EN(in_dma));
+		__raw_writel(idma_mask(in_dma), IPU_CHA_CUR_BUF(in_dma));
+	}
+	if (idma_is_valid(out_dma)) {
+		reg = __raw_readl(IDMAC_CHA_EN(out_dma));
+		__raw_writel(reg & ~idma_mask(out_dma), IDMAC_CHA_EN(out_dma));
+		__raw_writel(idma_mask(out_dma), IPU_CHA_CUR_BUF(out_dma));
+	}
+	if (g_sec_chan_en[IPU_CHAN_ID(channel)] && idma_is_valid(sec_dma)) {
+		reg = __raw_readl(IDMAC_CHA_EN(sec_dma));
+		__raw_writel(reg & ~idma_mask(sec_dma), IDMAC_CHA_EN(sec_dma));
+		__raw_writel(idma_mask(sec_dma), IPU_CHA_CUR_BUF(sec_dma));
+	}
+	if (g_thrd_chan_en[IPU_CHAN_ID(channel)] && idma_is_valid(thrd_dma)) {
+		reg = __raw_readl(IDMAC_CHA_EN(thrd_dma));
+		__raw_writel(reg & ~idma_mask(thrd_dma), IDMAC_CHA_EN(thrd_dma));
+		if (channel == MEM_BG_SYNC || channel == MEM_FG_SYNC) {
+			reg = __raw_readl(IDMAC_SEP_ALPHA);
+			__raw_writel(reg & ~idma_mask(in_dma), IDMAC_SEP_ALPHA);
+		} else {
+			reg = __raw_readl(IDMAC_SEP_ALPHA);
+			__raw_writel(reg & ~idma_mask(sec_dma), IDMAC_SEP_ALPHA);
+		}
+		__raw_writel(idma_mask(thrd_dma), IPU_CHA_CUR_BUF(thrd_dma));
+	}
+
+	/* Set channel buffers NOT to be ready */
+	__raw_writel(0xF0000000, IPU_GPR); /* write one to clear */
+	if (idma_is_valid(in_dma)) {
+		if (idma_is_set(IPU_CHA_BUF0_RDY, in_dma)) {
+			__raw_writel(idma_mask(in_dma),
+				     IPU_CHA_BUF0_RDY(in_dma));
+		}
+		if (idma_is_set(IPU_CHA_BUF1_RDY, in_dma)) {
+			__raw_writel(idma_mask(in_dma),
+				     IPU_CHA_BUF1_RDY(in_dma));
+		}
+	}
+	if (idma_is_valid(out_dma)) {
+		if (idma_is_set(IPU_CHA_BUF0_RDY, out_dma)) {
+			__raw_writel(idma_mask(out_dma),
+				     IPU_CHA_BUF0_RDY(out_dma));
+		}
+		if (idma_is_set(IPU_CHA_BUF1_RDY, out_dma)) {
+			__raw_writel(idma_mask(out_dma),
+				     IPU_CHA_BUF1_RDY(out_dma));
+		}
+	}
+	if (g_sec_chan_en[IPU_CHAN_ID(channel)] && idma_is_valid(sec_dma)) {
+		if (idma_is_set(IPU_CHA_BUF0_RDY, sec_dma)) {
+			__raw_writel(idma_mask(sec_dma),
+				     IPU_CHA_BUF0_RDY(sec_dma));
+		}
+		if (idma_is_set(IPU_CHA_BUF1_RDY, sec_dma)) {
+			__raw_writel(idma_mask(sec_dma),
+				     IPU_CHA_BUF1_RDY(sec_dma));
+		}
+	}
+	if (g_thrd_chan_en[IPU_CHAN_ID(channel)] && idma_is_valid(thrd_dma)) {
+		if (idma_is_set(IPU_CHA_BUF0_RDY, thrd_dma)) {
+			__raw_writel(idma_mask(thrd_dma),
+				     IPU_CHA_BUF0_RDY(thrd_dma));
+		}
+		if (idma_is_set(IPU_CHA_BUF1_RDY, thrd_dma)) {
+			__raw_writel(idma_mask(thrd_dma),
+				     IPU_CHA_BUF1_RDY(thrd_dma));
+		}
+	}
+	__raw_writel(0x0, IPU_GPR); /* write one to set */
+
+	g_channel_enable_mask &= ~(1L << IPU_CHAN_ID(channel));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_disable_channel);
+
+static irqreturn_t ipu_irq_handler(int irq, void *desc)
+{
+	int i;
+	uint32_t line;
+	irqreturn_t result = IRQ_NONE;
+	uint32_t int_stat;
+	const int err_reg[] = { 5, 6, 9, 10, 0 };
+	const int int_reg[] = { 1, 2, 3, 4, 11, 12, 13, 14, 15, 0 };
+
+	if (g_ipu_irq[1]) {
+		disable_irq(g_ipu_irq[0]);
+		disable_irq(g_ipu_irq[1]);
+	}
+
+	for (i = 0;; i++) {
+		if (err_reg[i] == 0)
+			break;
+		int_stat = __raw_readl(IPU_INT_STAT(err_reg[i]));
+		int_stat &= __raw_readl(IPU_INT_CTRL(err_reg[i]));
+		if (int_stat) {
+			__raw_writel(int_stat, IPU_INT_STAT(err_reg[i]));
+			dev_err(g_ipu_dev,
+				"IPU Error - IPU_INT_STAT_%d = 0x%08X\n",
+				err_reg[i], int_stat);
+			/* Disable interrupts so we only get error once */
+			int_stat =
+			    __raw_readl(IPU_INT_CTRL(err_reg[i])) & ~int_stat;
+			__raw_writel(int_stat, IPU_INT_CTRL(err_reg[i]));
+		}
+	}
+
+	for (i = 0;; i++) {
+		if (int_reg[i] == 0)
+			break;
+		int_stat = __raw_readl(IPU_INT_STAT(int_reg[i]));
+		int_stat &= __raw_readl(IPU_INT_CTRL(int_reg[i]));
+		__raw_writel(int_stat, IPU_INT_STAT(int_reg[i]));
+		while ((line = ffs(int_stat)) != 0) {
+			line--;
+			int_stat &= ~(1UL << line);
+			line += (int_reg[i] - 1) * 32;
+			result |=
+			    ipu_irq_list[line].handler(line,
+						       ipu_irq_list[line].
+						       dev_id);
+		}
+	}
+
+	if (g_ipu_irq[1]) {
+		enable_irq(g_ipu_irq[0]);
+		enable_irq(g_ipu_irq[1]);
+	}
+	return result;
+}
+
+/*!
+ * This function enables the interrupt for the specified interrupt line.
+ * The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to enable interrupt for.
+ *
+ */
+void ipu_enable_irq(uint32_t irq)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+
+	if (!g_ipu_clk_enabled)
+		clk_enable(g_ipu_clk);
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(IPUIRQ_2_CTRLREG(irq));
+	reg |= IPUIRQ_2_MASK(irq);
+	__raw_writel(reg, IPUIRQ_2_CTRLREG(irq));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	if (!g_ipu_clk_enabled)
+		clk_disable(g_ipu_clk);
+}
+EXPORT_SYMBOL(ipu_enable_irq);
+
+/*!
+ * This function disables the interrupt for the specified interrupt line.
+ * The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to disable interrupt for.
+ *
+ */
+void ipu_disable_irq(uint32_t irq)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+
+	if (!g_ipu_clk_enabled)
+		clk_enable(g_ipu_clk);
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(IPUIRQ_2_CTRLREG(irq));
+	reg &= ~IPUIRQ_2_MASK(irq);
+	__raw_writel(reg, IPUIRQ_2_CTRLREG(irq));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	if (!g_ipu_clk_enabled)
+		clk_disable(g_ipu_clk);
+}
+EXPORT_SYMBOL(ipu_disable_irq);
+
+/*!
+ * This function clears the interrupt for the specified interrupt line.
+ * The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to clear interrupt for.
+ *
+ */
+void ipu_clear_irq(uint32_t irq)
+{
+	if (!g_ipu_clk_enabled)
+		clk_enable(g_ipu_clk);
+
+	__raw_writel(IPUIRQ_2_MASK(irq), IPUIRQ_2_STATREG(irq));
+
+	if (!g_ipu_clk_enabled)
+		clk_disable(g_ipu_clk);
+}
+EXPORT_SYMBOL(ipu_clear_irq);
+
+/*!
+ * This function returns the current interrupt status for the specified
+ * interrupt line. The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to get status for.
+ *
+ * @return      Returns true if the interrupt is pending/asserted or false if
+ *              the interrupt is not pending.
+ */
+bool ipu_get_irq_status(uint32_t irq)
+{
+	uint32_t reg;
+
+	if (!g_ipu_clk_enabled)
+		clk_enable(g_ipu_clk);
+
+	reg = __raw_readl(IPUIRQ_2_STATREG(irq));
+
+	if (!g_ipu_clk_enabled)
+		clk_disable(g_ipu_clk);
+
+	if (reg & IPUIRQ_2_MASK(irq))
+		return true;
+	else
+		return false;
+}
+EXPORT_SYMBOL(ipu_get_irq_status);
+
+/*!
+ * This function registers an interrupt handler function for the specified
+ * interrupt line. The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to get status for.
+ *
+ * @param       handler         Input parameter for address of the handler
+ *                              function.
+ *
+ * @param       irq_flags       Flags for interrupt mode. Currently not used.
+ *
+ * @param       devname         Input parameter for string name of driver
+ *                              registering the handler.
+ *
+ * @param       dev_id          Input parameter for pointer of data to be
+ *                              passed to the handler.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int ipu_request_irq(uint32_t irq,
+		    irqreturn_t(*handler) (int, void *),
+		    uint32_t irq_flags, const char *devname, void *dev_id)
+{
+	unsigned long lock_flags;
+
+	BUG_ON(irq >= IPU_IRQ_COUNT);
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if (ipu_irq_list[irq].handler != NULL) {
+		dev_err(g_ipu_dev,
+			"handler already installed on irq %d\n", irq);
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+		return -EINVAL;
+	}
+
+	ipu_irq_list[irq].handler = handler;
+	ipu_irq_list[irq].flags = irq_flags;
+	ipu_irq_list[irq].dev_id = dev_id;
+	ipu_irq_list[irq].name = devname;
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	ipu_enable_irq(irq);	/* enable the interrupt */
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_request_irq);
+
+/*!
+ * This function unregisters an interrupt handler for the specified interrupt
+ * line. The interrupt lines are defined in \b ipu_irq_line enum.
+ *
+ * @param       irq             Interrupt line to get status for.
+ *
+ * @param       dev_id          Input parameter for pointer of data to be passed
+ *                              to the handler. This must match value passed to
+ *                              ipu_request_irq().
+ *
+ */
+void ipu_free_irq(uint32_t irq, void *dev_id)
+{
+	ipu_disable_irq(irq);	/* disable the interrupt */
+
+	if (ipu_irq_list[irq].dev_id == dev_id)
+		ipu_irq_list[irq].handler = NULL;
+}
+EXPORT_SYMBOL(ipu_free_irq);
+
+uint32_t _ipu_channel_status(ipu_channel_t channel)
+{
+	uint32_t stat = 0;
+	uint32_t task_stat_reg = __raw_readl(IPU_PROC_TASK_STAT);
+
+	switch (channel) {
+	case MEM_PRP_VF_MEM:
+		stat = (task_stat_reg & TSTAT_VF_MASK) >> TSTAT_VF_OFFSET;
+		break;
+	case MEM_VDI_PRP_VF_MEM:
+		stat = (task_stat_reg & TSTAT_VF_MASK) >> TSTAT_VF_OFFSET;
+		break;
+	case MEM_ROT_VF_MEM:
+		stat =
+		    (task_stat_reg & TSTAT_VF_ROT_MASK) >> TSTAT_VF_ROT_OFFSET;
+		break;
+	case MEM_PRP_ENC_MEM:
+		stat = (task_stat_reg & TSTAT_ENC_MASK) >> TSTAT_ENC_OFFSET;
+		break;
+	case MEM_ROT_ENC_MEM:
+		stat =
+		    (task_stat_reg & TSTAT_ENC_ROT_MASK) >>
+		    TSTAT_ENC_ROT_OFFSET;
+		break;
+	case MEM_PP_MEM:
+		stat = (task_stat_reg & TSTAT_PP_MASK) >> TSTAT_PP_OFFSET;
+		break;
+	case MEM_ROT_PP_MEM:
+		stat =
+		    (task_stat_reg & TSTAT_PP_ROT_MASK) >> TSTAT_PP_ROT_OFFSET;
+		break;
+
+	default:
+		stat = TASK_STAT_IDLE;
+		break;
+	}
+	return stat;
+}
+
+int32_t ipu_swap_channel(ipu_channel_t from_ch, ipu_channel_t to_ch)
+{
+	uint32_t reg;
+	unsigned long lock_flags;
+
+	int from_dma = channel_2_dma(from_ch, IPU_INPUT_BUFFER);
+	int to_dma = channel_2_dma(to_ch, IPU_INPUT_BUFFER);
+
+	/* enable target channel */
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(IDMAC_CHA_EN(to_dma));
+	__raw_writel(reg | idma_mask(to_dma), IDMAC_CHA_EN(to_dma));
+
+	g_channel_enable_mask |= 1L << IPU_CHAN_ID(to_ch);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	/* switch dp dc */
+	_ipu_dp_dc_disable(from_ch, true);
+
+	/* disable source channel */
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(IDMAC_CHA_EN(from_dma));
+	__raw_writel(reg & ~idma_mask(from_dma), IDMAC_CHA_EN(from_dma));
+	__raw_writel(idma_mask(from_dma), IPU_CHA_CUR_BUF(from_dma));
+
+	g_channel_enable_mask &= ~(1L << IPU_CHAN_ID(from_ch));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_swap_channel);
+
+uint32_t bytes_per_pixel(uint32_t fmt)
+{
+	switch (fmt) {
+	case IPU_PIX_FMT_GENERIC:	/*generic data */
+	case IPU_PIX_FMT_RGB332:
+	case IPU_PIX_FMT_YUV420P:
+	case IPU_PIX_FMT_YUV422P:
+		return 1;
+		break;
+	case IPU_PIX_FMT_RGB565:
+	case IPU_PIX_FMT_YUYV:
+	case IPU_PIX_FMT_UYVY:
+		return 2;
+		break;
+	case IPU_PIX_FMT_BGR24:
+	case IPU_PIX_FMT_RGB24:
+		return 3;
+		break;
+	case IPU_PIX_FMT_GENERIC_32:	/*generic data */
+	case IPU_PIX_FMT_BGR32:
+	case IPU_PIX_FMT_BGRA32:
+	case IPU_PIX_FMT_RGB32:
+	case IPU_PIX_FMT_RGBA32:
+	case IPU_PIX_FMT_ABGR32:
+		return 4;
+		break;
+	default:
+		return 1;
+		break;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(bytes_per_pixel);
+
+ipu_color_space_t format_to_colorspace(uint32_t fmt)
+{
+	switch (fmt) {
+	case IPU_PIX_FMT_RGB666:
+	case IPU_PIX_FMT_RGB565:
+	case IPU_PIX_FMT_BGR24:
+	case IPU_PIX_FMT_RGB24:
+	case IPU_PIX_FMT_BGR32:
+	case IPU_PIX_FMT_BGRA32:
+	case IPU_PIX_FMT_RGB32:
+	case IPU_PIX_FMT_RGBA32:
+	case IPU_PIX_FMT_ABGR32:
+	case IPU_PIX_FMT_LVDS666:
+	case IPU_PIX_FMT_LVDS888:
+		return RGB;
+		break;
+
+	default:
+		return YCbCr;
+		break;
+	}
+	return RGB;
+}
+
+bool ipu_pixel_format_has_alpha(uint32_t fmt)
+{
+	switch (fmt) {
+	case IPU_PIX_FMT_RGBA32:
+	case IPU_PIX_FMT_BGRA32:
+	case IPU_PIX_FMT_ABGR32:
+		return true;
+		break;
+	default:
+		return false;
+		break;
+	}
+	return false;
+}
+
+void ipu_set_csc_coefficients(ipu_channel_t channel, int32_t param[][3])
+{
+	_ipu_dp_set_csc_coefficients(channel, param);
+}
+EXPORT_SYMBOL(ipu_set_csc_coefficients);
+
+static int ipu_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	if (g_ipu_clk_enabled) {
+		/* save and disable enabled channels*/
+		idma_enable_reg[0] = __raw_readl(IDMAC_CHA_EN(0));
+		idma_enable_reg[1] = __raw_readl(IDMAC_CHA_EN(32));
+		while ((__raw_readl(IDMAC_CHA_BUSY(0)) & idma_enable_reg[0])
+			|| (__raw_readl(IDMAC_CHA_BUSY(32)) &
+				idma_enable_reg[1])) {
+			/* disable channel not busy already */
+			uint32_t chan_should_disable, timeout = 1000, time = 0;
+
+			chan_should_disable =
+				__raw_readl(IDMAC_CHA_BUSY(0))
+					^ idma_enable_reg[0];
+			__raw_writel((~chan_should_disable) &
+					idma_enable_reg[0], IDMAC_CHA_EN(0));
+			chan_should_disable =
+				__raw_readl(IDMAC_CHA_BUSY(1))
+					^ idma_enable_reg[1];
+			__raw_writel((~chan_should_disable) &
+					idma_enable_reg[1], IDMAC_CHA_EN(32));
+			msleep(2);
+			time += 2;
+			if (time >= timeout)
+				return -1;
+		}
+		__raw_writel(0, IDMAC_CHA_EN(0));
+		__raw_writel(0, IDMAC_CHA_EN(32));
+
+		/* save double buffer select regs */
+		ipu_cha_db_mode_reg[0] = __raw_readl(IPU_CHA_DB_MODE_SEL(0));
+		ipu_cha_db_mode_reg[1] = __raw_readl(IPU_CHA_DB_MODE_SEL(32));
+		ipu_cha_db_mode_reg[2] =
+			__raw_readl(IPU_ALT_CHA_DB_MODE_SEL(0));
+		ipu_cha_db_mode_reg[3] =
+			__raw_readl(IPU_ALT_CHA_DB_MODE_SEL(32));
+
+		/* save current buffer regs */
+		ipu_cha_cur_buf_reg[0] = __raw_readl(IPU_CHA_CUR_BUF(0));
+		ipu_cha_cur_buf_reg[1] = __raw_readl(IPU_CHA_CUR_BUF(32));
+		ipu_cha_cur_buf_reg[2] = __raw_readl(IPU_ALT_CUR_BUF0);
+		ipu_cha_cur_buf_reg[3] = __raw_readl(IPU_ALT_CUR_BUF1);
+
+		/* save sub-modules status and disable all */
+		ic_conf_reg = __raw_readl(IC_CONF);
+		__raw_writel(0, IC_CONF);
+		ipu_conf_reg = __raw_readl(IPU_CONF);
+		__raw_writel(0, IPU_CONF);
+
+		/* save buf ready regs */
+		buf_ready_reg[0] = __raw_readl(IPU_CHA_BUF0_RDY(0));
+		buf_ready_reg[1] = __raw_readl(IPU_CHA_BUF0_RDY(32));
+		buf_ready_reg[2] = __raw_readl(IPU_CHA_BUF1_RDY(0));
+		buf_ready_reg[3] = __raw_readl(IPU_CHA_BUF1_RDY(32));
+		buf_ready_reg[4] = __raw_readl(IPU_ALT_CHA_BUF0_RDY(0));
+		buf_ready_reg[5] = __raw_readl(IPU_ALT_CHA_BUF0_RDY(32));
+		buf_ready_reg[6] = __raw_readl(IPU_ALT_CHA_BUF1_RDY(0));
+		buf_ready_reg[7] = __raw_readl(IPU_ALT_CHA_BUF1_RDY(32));
+	}
+
+	mxc_pg_enable(pdev);
+
+	return 0;
+}
+
+static int ipu_resume(struct platform_device *pdev)
+{
+	mxc_pg_disable(pdev);
+
+	if (g_ipu_clk_enabled) {
+
+		/* restore buf ready regs */
+		__raw_writel(buf_ready_reg[0], IPU_CHA_BUF0_RDY(0));
+		__raw_writel(buf_ready_reg[1], IPU_CHA_BUF0_RDY(32));
+		__raw_writel(buf_ready_reg[2], IPU_CHA_BUF1_RDY(0));
+		__raw_writel(buf_ready_reg[3], IPU_CHA_BUF1_RDY(32));
+		__raw_writel(buf_ready_reg[4], IPU_ALT_CHA_BUF0_RDY(0));
+		__raw_writel(buf_ready_reg[5], IPU_ALT_CHA_BUF0_RDY(32));
+		__raw_writel(buf_ready_reg[6], IPU_ALT_CHA_BUF1_RDY(0));
+		__raw_writel(buf_ready_reg[7], IPU_ALT_CHA_BUF1_RDY(32));
+
+		/* re-enable sub-modules*/
+		__raw_writel(ipu_conf_reg, IPU_CONF);
+		__raw_writel(ic_conf_reg, IC_CONF);
+
+		/* restore double buffer select regs */
+		__raw_writel(ipu_cha_db_mode_reg[0], IPU_CHA_DB_MODE_SEL(0));
+		__raw_writel(ipu_cha_db_mode_reg[1], IPU_CHA_DB_MODE_SEL(32));
+		__raw_writel(ipu_cha_db_mode_reg[2],
+				IPU_ALT_CHA_DB_MODE_SEL(0));
+		__raw_writel(ipu_cha_db_mode_reg[3],
+				IPU_ALT_CHA_DB_MODE_SEL(32));
+
+		/* restore current buffer select regs */
+		__raw_writel(~(ipu_cha_cur_buf_reg[0]), IPU_CHA_CUR_BUF(0));
+		__raw_writel(~(ipu_cha_cur_buf_reg[1]), IPU_CHA_CUR_BUF(32));
+		__raw_writel(~(ipu_cha_cur_buf_reg[2]), IPU_ALT_CUR_BUF0);
+		__raw_writel(~(ipu_cha_cur_buf_reg[3]), IPU_ALT_CUR_BUF1);
+
+		/* restart idma channel*/
+		__raw_writel(idma_enable_reg[0], IDMAC_CHA_EN(0));
+		__raw_writel(idma_enable_reg[1], IDMAC_CHA_EN(32));
+	} else {
+		clk_enable(g_ipu_clk);
+		_ipu_dmfc_init();
+		_ipu_init_dc_mappings();
+
+		/* Set sync refresh channels as high priority */
+		__raw_writel(0x18800000L, IDMAC_CHA_PRI(0));
+		clk_disable(g_ipu_clk);
+	}
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcipu_driver = {
+	.driver = {
+		   .name = "mxc_ipu",
+		   },
+	.probe = ipu_probe,
+	.remove = ipu_remove,
+	.suspend_late = ipu_suspend,
+	.resume_early = ipu_resume,
+};
+
+int32_t __init ipu_gen_init(void)
+{
+	int32_t ret;
+
+	ret = platform_driver_register(&mxcipu_driver);
+	return 0;
+}
+
+subsys_initcall(ipu_gen_init);
+
+static void __exit ipu_gen_uninit(void)
+{
+	platform_driver_unregister(&mxcipu_driver);
+}
+
+module_exit(ipu_gen_uninit);
diff --git a/drivers/mxc/ipu3/ipu_device.c b/drivers/mxc/ipu3/ipu_device.c
new file mode 100644
index 000000000000..4c98d4c7edd3
--- /dev/null
+++ b/drivers/mxc/ipu3/ipu_device.c
@@ -0,0 +1,446 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_device.c
+ *
+ * @brief This file contains the IPUv3 driver device interface and fops functions.
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/wait.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+#include <asm/cacheflush.h>
+
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+#include "ipu_param_mem.h"
+
+/* Strucutures and variables for exporting MXC IPU as device*/
+
+static int mxc_ipu_major;
+static struct class *mxc_ipu_class;
+
+DEFINE_SPINLOCK(event_lock);
+
+struct ipu_dev_irq_info {
+	wait_queue_head_t waitq;
+	int irq_pending;
+} irq_info[480];
+
+int register_ipu_device(void);
+
+/* Static functions */
+
+int get_events(ipu_event_info *p)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&event_lock, flags);
+	if (irq_info[p->irq].irq_pending > 0)
+		irq_info[p->irq].irq_pending--;
+	else
+		ret = -1;
+	spin_unlock_irqrestore(&event_lock, flags);
+
+	return ret;
+}
+
+static irqreturn_t mxc_ipu_generic_handler(int irq, void *dev_id)
+{
+	irq_info[irq].irq_pending++;
+
+	/* Wakeup any blocking user context */
+	wake_up_interruptible(&(irq_info[irq].waitq));
+	return IRQ_HANDLED;
+}
+
+static int mxc_ipu_open(struct inode *inode, struct file *file)
+{
+	int ret = 0;
+	return ret;
+}
+static int mxc_ipu_ioctl(struct inode *inode, struct file *file,
+		unsigned int cmd, unsigned long arg)
+{
+	int ret = 0;
+
+	switch (cmd) {
+	case IPU_INIT_CHANNEL:
+		{
+			ipu_channel_parm parm;
+
+			if (copy_from_user
+					(&parm, (ipu_channel_parm *) arg,
+					 sizeof(ipu_channel_parm)))
+				return -EFAULT;
+
+			if (!parm.flag) {
+				ret =
+					ipu_init_channel(parm.channel,
+							&parm.params);
+			} else {
+				ret = ipu_init_channel(parm.channel, NULL);
+			}
+		}
+		break;
+	case IPU_UNINIT_CHANNEL:
+		{
+		ipu_channel_t ch;
+		int __user *argp = (void __user *)arg;
+		if (get_user(ch, argp))
+				return -EFAULT;
+			ipu_uninit_channel(ch);
+		}
+		break;
+	case IPU_INIT_CHANNEL_BUFFER:
+		{
+			ipu_channel_buf_parm parm;
+			if (copy_from_user
+				(&parm, (ipu_channel_buf_parm *) arg,
+				sizeof(ipu_channel_buf_parm)))
+				return -EFAULT;
+
+			ret =
+				ipu_init_channel_buffer(
+						parm.channel, parm.type,
+						parm.pixel_fmt,
+						parm.width, parm.height,
+						parm.stride,
+						parm.rot_mode,
+						parm.phyaddr_0,
+						parm.phyaddr_1,
+						parm.u_offset,
+						parm.v_offset);
+
+		}
+		break;
+	case IPU_UPDATE_CHANNEL_BUFFER:
+		{
+			ipu_channel_buf_parm parm;
+			if (copy_from_user
+				(&parm, (ipu_channel_buf_parm *) arg,
+				sizeof(ipu_channel_buf_parm)))
+				return -EFAULT;
+
+			if ((parm.phyaddr_0 != (dma_addr_t) NULL)
+				&& (parm.phyaddr_1 == (dma_addr_t) NULL)) {
+				ret =
+					ipu_update_channel_buffer(
+							parm.channel,
+							parm.type,
+							parm.bufNum,
+							parm.phyaddr_0);
+			} else if ((parm.phyaddr_0 == (dma_addr_t) NULL)
+				&& (parm.phyaddr_1 != (dma_addr_t) NULL)) {
+				ret =
+					ipu_update_channel_buffer(
+							parm.channel,
+							parm.type,
+							parm.bufNum,
+							parm.phyaddr_1);
+			} else {
+				ret = -1;
+			}
+
+		}
+		break;
+	case IPU_SELECT_CHANNEL_BUFFER:
+		{
+			ipu_channel_buf_parm parm;
+			if (copy_from_user
+				(&parm, (ipu_channel_buf_parm *) arg,
+				sizeof(ipu_channel_buf_parm)))
+				return -EFAULT;
+
+			ret =
+				ipu_select_buffer(parm.channel,
+					parm.type, parm.bufNum);
+
+		}
+		break;
+	case IPU_LINK_CHANNELS:
+		{
+			ipu_channel_link link;
+			if (copy_from_user
+				(&link, (ipu_channel_link *) arg,
+				sizeof(ipu_channel_link)))
+				return -EFAULT;
+
+			ret = ipu_link_channels(link.src_ch,
+				link.dest_ch);
+
+		}
+		break;
+	case IPU_UNLINK_CHANNELS:
+		{
+			ipu_channel_link link;
+			if (copy_from_user
+				(&link, (ipu_channel_link *) arg,
+				sizeof(ipu_channel_link)))
+				return -EFAULT;
+
+			ret = ipu_unlink_channels(link.src_ch,
+				link.dest_ch);
+
+		}
+		break;
+	case IPU_ENABLE_CHANNEL:
+		{
+			ipu_channel_t ch;
+			int __user *argp = (void __user *)arg;
+			if (get_user(ch, argp))
+				return -EFAULT;
+			ipu_enable_channel(ch);
+		}
+		break;
+	case IPU_DISABLE_CHANNEL:
+		{
+			ipu_channel_info info;
+			if (copy_from_user
+				(&info, (ipu_channel_info *) arg,
+				 sizeof(ipu_channel_info)))
+				return -EFAULT;
+
+			ret = ipu_disable_channel(info.channel,
+				info.stop);
+		}
+		break;
+	case IPU_ENABLE_IRQ:
+		{
+			uint32_t irq;
+			int __user *argp = (void __user *)arg;
+			if (get_user(irq, argp))
+				return -EFAULT;
+			ipu_enable_irq(irq);
+		}
+		break;
+	case IPU_DISABLE_IRQ:
+		{
+			uint32_t irq;
+			int __user *argp = (void __user *)arg;
+			if (get_user(irq, argp))
+				return -EFAULT;
+			ipu_disable_irq(irq);
+		}
+		break;
+	case IPU_CLEAR_IRQ:
+		{
+			uint32_t irq;
+			int __user *argp = (void __user *)arg;
+			if (get_user(irq, argp))
+				return -EFAULT;
+			ipu_clear_irq(irq);
+		}
+		break;
+	case IPU_FREE_IRQ:
+		{
+			ipu_irq_info info;
+
+			if (copy_from_user
+					(&info, (ipu_irq_info *) arg,
+					 sizeof(ipu_irq_info)))
+				return -EFAULT;
+
+			ipu_free_irq(info.irq, info.dev_id);
+			irq_info[info.irq].irq_pending = 0;
+		}
+		break;
+	case IPU_REQUEST_IRQ_STATUS:
+		{
+			uint32_t irq;
+			int __user *argp = (void __user *)arg;
+			if (get_user(irq, argp))
+				return -EFAULT;
+			ret = ipu_get_irq_status(irq);
+		}
+		break;
+	case IPU_REGISTER_GENERIC_ISR:
+		{
+			ipu_event_info info;
+			if (copy_from_user
+					(&info, (ipu_event_info *) arg,
+					 sizeof(ipu_event_info)))
+				return -EFAULT;
+
+			ret =
+				ipu_request_irq(info.irq,
+					mxc_ipu_generic_handler,
+					0, "video_sink", info.dev);
+			if (ret == 0)
+				init_waitqueue_head(&(irq_info[info.irq].waitq));
+		}
+		break;
+	case IPU_GET_EVENT:
+		/* User will have to allocate event_type
+		structure and pass the pointer in arg */
+		{
+			ipu_event_info info;
+			int r = -1;
+
+			if (copy_from_user
+					(&info, (ipu_event_info *) arg,
+					 sizeof(ipu_event_info)))
+				return -EFAULT;
+
+			r = get_events(&info);
+			if (r == -1) {
+				if ((file->f_flags & O_NONBLOCK) &&
+					(irq_info[info.irq].irq_pending == 0))
+					return -EAGAIN;
+				wait_event_interruptible_timeout(irq_info[info.irq].waitq,
+						(irq_info[info.irq].irq_pending != 0), 2 * HZ);
+				r = get_events(&info);
+			}
+			ret = -1;
+			if (r == 0) {
+				if (!copy_to_user((ipu_event_info *) arg,
+					&info, sizeof(ipu_event_info)))
+					ret = 0;
+			}
+		}
+		break;
+	case IPU_ALOC_MEM:
+		{
+			ipu_mem_info info;
+			if (copy_from_user
+					(&info, (ipu_mem_info *) arg,
+					 sizeof(ipu_mem_info)))
+				return -EFAULT;
+
+			info.vaddr = dma_alloc_coherent(0,
+					PAGE_ALIGN(info.size),
+					&info.paddr,
+					GFP_DMA | GFP_KERNEL);
+			if (info.vaddr == 0) {
+				printk(KERN_ERR "dma alloc failed!\n");
+				return -ENOBUFS;
+			}
+			if (copy_to_user((ipu_mem_info *) arg, &info,
+					sizeof(ipu_mem_info)) > 0)
+				return -EFAULT;
+		}
+		break;
+	case IPU_FREE_MEM:
+		{
+			ipu_mem_info info;
+			if (copy_from_user
+					(&info, (ipu_mem_info *) arg,
+					 sizeof(ipu_mem_info)))
+				return -EFAULT;
+
+			if (info.vaddr)
+				dma_free_coherent(0, PAGE_ALIGN(info.size),
+					info.vaddr, info.paddr);
+			else
+				return -EFAULT;
+		}
+		break;
+	case IPU_IS_CHAN_BUSY:
+		{
+			ipu_channel_t chan;
+			if (copy_from_user
+					(&chan, (ipu_channel_t *)arg,
+					 sizeof(ipu_channel_t)))
+				return -EFAULT;
+
+			if (ipu_is_channel_busy(chan))
+				ret = 1;
+			else
+				ret = 0;
+		}
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+static int mxc_ipu_mmap(struct file *file, struct vm_area_struct *vma)
+{
+//	vma->vm_page_prot = pgprot_writethru(vma->vm_page_prot);
+
+	if (remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
+				vma->vm_end - vma->vm_start,
+				vma->vm_page_prot)) {
+		printk(KERN_ERR
+				"mmap failed!\n");
+		return -ENOBUFS;
+	}
+	return 0;
+}
+
+static int mxc_ipu_release(struct inode *inode, struct file *file)
+{
+	return 0;
+}
+
+static struct file_operations mxc_ipu_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_ipu_open,
+	.mmap = mxc_ipu_mmap,
+	.release = mxc_ipu_release,
+	.ioctl = mxc_ipu_ioctl
+};
+
+int register_ipu_device()
+{
+	int ret = 0;
+	struct device *temp;
+	mxc_ipu_major = register_chrdev(0, "mxc_ipu", &mxc_ipu_fops);
+	if (mxc_ipu_major < 0) {
+		printk(KERN_ERR
+			"Unable to register Mxc Ipu as a char device\n");
+		return mxc_ipu_major;
+	}
+
+	mxc_ipu_class = class_create(THIS_MODULE, "mxc_ipu");
+	if (IS_ERR(mxc_ipu_class)) {
+		printk(KERN_ERR "Unable to create class for Mxc Ipu\n");
+		ret = PTR_ERR(mxc_ipu_class);
+		goto err1;
+	}
+
+	temp = device_create(mxc_ipu_class, NULL, MKDEV(mxc_ipu_major, 0),
+			NULL, "mxc_ipu");
+
+	if (IS_ERR(temp)) {
+		printk(KERN_ERR "Unable to create class device for Mxc Ipu\n");
+		ret = PTR_ERR(temp);
+		goto err2;
+	}
+	spin_lock_init(&event_lock);
+
+	return ret;
+
+err2:
+	class_destroy(mxc_ipu_class);
+err1:
+	unregister_chrdev(mxc_ipu_major, "mxc_ipu");
+	return ret;
+
+}
diff --git a/drivers/mxc/ipu3/ipu_disp.c b/drivers/mxc/ipu3/ipu_disp.c
new file mode 100644
index 000000000000..79b0c2123c22
--- /dev/null
+++ b/drivers/mxc/ipu3/ipu_disp.c
@@ -0,0 +1,1515 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_disp.c
+ *
+ * @brief IPU display submodule API functions
+ *
+ * @ingroup IPU
+ */
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+#include <asm/atomic.h>
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+#include "ipu_param_mem.h"
+
+enum csc_type_t {
+	RGB2YUV = 0,
+	YUV2RGB,
+	RGB2RGB,
+	YUV2YUV,
+	CSC_NONE,
+	CSC_NUM
+};
+
+struct dp_csc_param_t {
+	int mode;
+	void *coeff;
+};
+
+#define SYNC_WAVE 0
+#define ASYNC_SER_WAVE 6
+
+/* DC display ID assignments */
+#define DC_DISP_ID_SYNC(di)	(di)
+#define DC_DISP_ID_SERIAL	2
+#define DC_DISP_ID_ASYNC	3
+
+
+/* all value below is determined by fix reg setting in _ipu_dmfc_init*/
+#define DMFC_FIFO_SIZE_28	(128*4)
+#define DMFC_FIFO_SIZE_29	(64*4)
+#define DMFC_FIFO_SIZE_24	(64*4)
+#define DMFC_FIFO_SIZE_27	(128*4)
+#define DMFC_FIFO_SIZE_23	(128*4)
+
+void _ipu_dmfc_init(void)
+{
+	/* disable DMFC-IC channel*/
+	__raw_writel(0x2, DMFC_IC_CTRL);
+	/* 1 - segment 0 and 1; 2, 1C and 2C unused */
+	__raw_writel(0x00000088, DMFC_WR_CHAN);
+	__raw_writel(0x202020F6, DMFC_WR_CHAN_DEF);
+	/* 5B - segment 2 and 3; 5F - segment 4 and 5; */
+	/* 6B - segment 6; 6F - segment 7 */
+	__raw_writel(0x1F1E9694, DMFC_DP_CHAN);
+	/* Enable chan 5 watermark set at 5 bursts and clear at 7 bursts */
+	__raw_writel(0x2020F6F6, DMFC_DP_CHAN_DEF);
+}
+
+void _ipu_dmfc_set_wait4eot(int dma_chan, int width)
+{
+	u32 dmfc_gen1 = __raw_readl(DMFC_GENERAL1);
+
+	if (dma_chan == 23) { /*5B*/
+		if (DMFC_FIFO_SIZE_23/width > 3)
+			dmfc_gen1 |= 1UL << 20;
+		else
+			dmfc_gen1 &= ~(1UL << 20);
+	} else if (dma_chan == 24) { /*6B*/
+		if (DMFC_FIFO_SIZE_24/width > 1)
+			dmfc_gen1 |= 1UL << 22;
+		else
+			dmfc_gen1 &= ~(1UL << 22);
+	} else if (dma_chan == 27) { /*5F*/
+		if (DMFC_FIFO_SIZE_27/width > 2)
+			dmfc_gen1 |= 1UL << 21;
+		else
+			dmfc_gen1 &= ~(1UL << 21);
+	} else if (dma_chan == 28) { /*1*/
+		if (DMFC_FIFO_SIZE_28/width > 2)
+			dmfc_gen1 |= 1UL << 16;
+		else
+			dmfc_gen1 &= ~(1UL << 16);
+	} else if (dma_chan == 29) { /*6F*/
+		if (DMFC_FIFO_SIZE_29/width > 1)
+			dmfc_gen1 |= 1UL << 23;
+		else
+			dmfc_gen1 &= ~(1UL << 23);
+	}
+
+	__raw_writel(dmfc_gen1, DMFC_GENERAL1);
+}
+
+static void _ipu_di_data_wave_config(int di,
+				     int wave_gen,
+				     int access_size, int component_size)
+{
+	u32 reg;
+	reg = (access_size << DI_DW_GEN_ACCESS_SIZE_OFFSET) |
+	    (component_size << DI_DW_GEN_COMPONENT_SIZE_OFFSET);
+	__raw_writel(reg, DI_DW_GEN(di, wave_gen));
+}
+
+static void _ipu_di_data_pin_config(int di, int wave_gen, int di_pin, int set,
+				    int up, int down)
+{
+	u32 reg;
+
+	reg = __raw_readl(DI_DW_GEN(di, wave_gen));
+	reg &= ~(0x3 << (di_pin * 2));
+	reg |= set << (di_pin * 2);
+	__raw_writel(reg, DI_DW_GEN(di, wave_gen));
+
+	__raw_writel((down << 16) | up, DI_DW_SET(di, wave_gen, set));
+}
+
+static void _ipu_di_sync_config(int di, int wave_gen,
+				int run_count, int run_src,
+				int offset_count, int offset_src,
+				int repeat_count, int cnt_clr_src,
+				int cnt_polarity_gen_en,
+				int cnt_polarity_clr_src,
+				int cnt_polarity_trigger_src,
+				int cnt_up, int cnt_down)
+{
+	u32 reg;
+
+	if ((run_count >= 0x1000) || (offset_count >= 0x1000) || (repeat_count >= 0x1000) ||
+		(cnt_up >= 0x400) || (cnt_down >= 0x400)) {
+		dev_err(g_ipu_dev, "DI%d counters out of range.\n", di);
+		return;
+	}
+
+	reg = (run_count << 19) | (++run_src << 16) |
+	    (offset_count << 3) | ++offset_src;
+	__raw_writel(reg, DI_SW_GEN0(di, wave_gen));
+	reg = (cnt_polarity_gen_en << 29) | (++cnt_clr_src << 25) |
+	    (++cnt_polarity_trigger_src << 12) | (++cnt_polarity_clr_src << 9);
+	reg |= (cnt_down << 16) | cnt_up;
+	if (repeat_count == 0) {
+		/* Enable auto reload */
+		reg |= 0x10000000;
+	}
+	__raw_writel(reg, DI_SW_GEN1(di, wave_gen));
+	reg = __raw_readl(DI_STP_REP(di, wave_gen));
+	reg &= ~(0xFFFF << (16 * ((wave_gen - 1) & 0x1)));
+	reg |= repeat_count << (16 * ((wave_gen - 1) & 0x1));
+	__raw_writel(reg, DI_STP_REP(di, wave_gen));
+}
+
+static void _ipu_dc_map_config(int map, int byte_num, int offset, int mask)
+{
+	int ptr = map * 3 + byte_num;
+	u32 reg;
+
+	reg = __raw_readl(DC_MAP_CONF_VAL(ptr));
+	reg &= ~(0xFFFF << (16 * (ptr & 0x1)));
+	reg |= ((offset << 8) | mask) << (16 * (ptr & 0x1));
+	__raw_writel(reg, DC_MAP_CONF_VAL(ptr));
+
+	reg = __raw_readl(DC_MAP_CONF_PTR(map));
+	reg &= ~(0x1F << ((16 * (map & 0x1)) + (5 * byte_num)));
+	reg |= ptr << ((16 * (map & 0x1)) + (5 * byte_num));
+	__raw_writel(reg, DC_MAP_CONF_PTR(map));
+}
+
+static void _ipu_dc_map_clear(int map)
+{
+	u32 reg = __raw_readl(DC_MAP_CONF_PTR(map));
+	__raw_writel(reg & ~(0xFFFF << (16 * (map & 0x1))),
+		     DC_MAP_CONF_PTR(map));
+}
+
+static void _ipu_dc_write_tmpl(int word, u32 opcode, u32 operand, int map,
+			       int wave, int glue, int sync)
+{
+	u32 reg;
+	int stop = 1;
+
+	reg = sync;
+	reg |= (glue << 4);
+	reg |= (++wave << 11);
+	reg |= (++map << 15);
+	reg |= (operand << 20) & 0xFFF00000;
+	__raw_writel(reg, ipu_dc_tmpl_reg + word * 2);
+
+	reg = (operand >> 12);
+	reg |= opcode << 4;
+	reg |= (stop << 9);
+	__raw_writel(reg, ipu_dc_tmpl_reg + word * 2 + 1);
+}
+
+static void _ipu_dc_link_event(int chan, int event, int addr, int priority)
+{
+	u32 reg;
+
+	reg = __raw_readl(DC_RL_CH(chan, event));
+	reg &= ~(0xFFFF << (16 * (event & 0x1)));
+	reg |= ((addr << 8) | priority) << (16 * (event & 0x1));
+	__raw_writel(reg, DC_RL_CH(chan, event));
+}
+
+/*     Y = R *  .299 + G *  .587 + B *  .114;
+       U = R * -.169 + G * -.332 + B *  .500 + 128.;
+       V = R *  .500 + G * -.419 + B * -.0813 + 128.;*/
+static const int rgb2ycbcr_coeff[5][3] = {
+	{153, 301, 58},
+	{-87, -170, 0x0100},
+	{0x100, -215, -42},
+	{0x0000, 0x0200, 0x0200},	/* B0, B1, B2 */
+	{0x2, 0x2, 0x2},	/* S0, S1, S2 */
+};
+
+/*     R = (1.164 * (Y - 16)) + (1.596 * (Cr - 128));
+       G = (1.164 * (Y - 16)) - (0.392 * (Cb - 128)) - (0.813 * (Cr - 128));
+       B = (1.164 * (Y - 16)) + (2.017 * (Cb - 128); */
+static const int ycbcr2rgb_coeff[5][3] = {
+	{0x095, 0x000, 0x0CC},
+	{0x095, 0x3CE, 0x398},
+	{0x095, 0x0FF, 0x000},
+	{0x3E42, 0x010A, 0x3DD6},	/*B0,B1,B2 */
+	{0x1, 0x1, 0x1},	/*S0,S1,S2 */
+};
+
+#define mask_a(a) ((u32)(a) & 0x3FF)
+#define mask_b(b) ((u32)(b) & 0x3FFF)
+
+/* Pls keep S0, S1 and S2 as 0x2 by using this convertion */
+static int _rgb_to_yuv(int n, int red, int green, int blue)
+{
+	int c;
+	c = red * rgb2ycbcr_coeff[n][0];
+	c += green * rgb2ycbcr_coeff[n][1];
+	c += blue * rgb2ycbcr_coeff[n][2];
+	c /= 16;
+	c += rgb2ycbcr_coeff[3][n] * 4;
+	c += 8;
+	c /= 16;
+	if (c < 0)
+		c = 0;
+	if (c > 255)
+		c = 255;
+	return c;
+}
+
+/*
+ * Row is for BG: 	RGB2YUV YUV2RGB RGB2RGB YUV2YUV CSC_NONE
+ * Column is for FG:	RGB2YUV YUV2RGB RGB2RGB YUV2YUV CSC_NONE
+ */
+static struct dp_csc_param_t dp_csc_array[CSC_NUM][CSC_NUM] = {
+{{DP_COM_CONF_CSC_DEF_BOTH, &rgb2ycbcr_coeff}, {0, 0}, {0, 0}, {DP_COM_CONF_CSC_DEF_BG, &rgb2ycbcr_coeff}, {DP_COM_CONF_CSC_DEF_BG, &rgb2ycbcr_coeff} },
+{{0, 0}, {DP_COM_CONF_CSC_DEF_BOTH, &ycbcr2rgb_coeff}, {DP_COM_CONF_CSC_DEF_BG, &ycbcr2rgb_coeff}, {0, 0}, {DP_COM_CONF_CSC_DEF_BG, &ycbcr2rgb_coeff} },
+{{0, 0}, {DP_COM_CONF_CSC_DEF_FG, &ycbcr2rgb_coeff}, {0, 0}, {0, 0}, {0, 0} },
+{{DP_COM_CONF_CSC_DEF_FG, &rgb2ycbcr_coeff}, {0, 0}, {0, 0}, {0, 0}, {0, 0} },
+{{DP_COM_CONF_CSC_DEF_FG, &rgb2ycbcr_coeff}, {DP_COM_CONF_CSC_DEF_FG, &ycbcr2rgb_coeff}, {0, 0}, {0, 0}, {0, 0} }
+};
+
+static enum csc_type_t fg_csc_type = CSC_NONE, bg_csc_type = CSC_NONE;
+static int color_key_4rgb = 1;
+
+void __ipu_dp_csc_setup(int dp, struct dp_csc_param_t dp_csc_param,
+			bool srm_mode_update)
+{
+	u32 reg;
+	const int (*coeff)[5][3];
+
+	if (dp_csc_param.mode >= 0) {
+		reg = __raw_readl(DP_COM_CONF(dp));
+		reg &= ~DP_COM_CONF_CSC_DEF_MASK;
+		reg |= dp_csc_param.mode;
+		__raw_writel(reg, DP_COM_CONF(dp));
+	}
+
+	coeff = dp_csc_param.coeff;
+
+	if (coeff) {
+		__raw_writel(mask_a((*coeff)[0][0]) |
+				(mask_a((*coeff)[0][1]) << 16), DP_CSC_A_0(dp));
+		__raw_writel(mask_a((*coeff)[0][2]) |
+				(mask_a((*coeff)[1][0]) << 16), DP_CSC_A_1(dp));
+		__raw_writel(mask_a((*coeff)[1][1]) |
+				(mask_a((*coeff)[1][2]) << 16), DP_CSC_A_2(dp));
+		__raw_writel(mask_a((*coeff)[2][0]) |
+				(mask_a((*coeff)[2][1]) << 16), DP_CSC_A_3(dp));
+		__raw_writel(mask_a((*coeff)[2][2]) |
+				(mask_b((*coeff)[3][0]) << 16) |
+				((*coeff)[4][0] << 30), DP_CSC_0(dp));
+		__raw_writel(mask_b((*coeff)[3][1]) | ((*coeff)[4][1] << 14) |
+				(mask_b((*coeff)[3][2]) << 16) |
+				((*coeff)[4][2] << 30), DP_CSC_1(dp));
+	}
+
+	if (srm_mode_update) {
+		reg = __raw_readl(IPU_SRM_PRI2) | 0x8;
+		__raw_writel(reg, IPU_SRM_PRI2);
+	}
+}
+
+int _ipu_dp_init(ipu_channel_t channel, uint32_t in_pixel_fmt,
+		 uint32_t out_pixel_fmt)
+{
+	int in_fmt, out_fmt;
+	int dp;
+	int partial = false;
+	uint32_t reg;
+
+	if (channel == MEM_FG_SYNC) {
+		dp = DP_SYNC;
+		partial = true;
+	} else if (channel == MEM_BG_SYNC) {
+		dp = DP_SYNC;
+		partial = false;
+	} else if (channel == MEM_BG_ASYNC0) {
+		dp = DP_ASYNC0;
+		partial = false;
+	} else {
+		return -EINVAL;
+	}
+
+	in_fmt = format_to_colorspace(in_pixel_fmt);
+	out_fmt = format_to_colorspace(out_pixel_fmt);
+
+	if (partial) {
+		if (in_fmt == RGB) {
+			if (out_fmt == RGB)
+				fg_csc_type = RGB2RGB;
+			else
+				fg_csc_type = RGB2YUV;
+		} else {
+			if (out_fmt == RGB)
+				fg_csc_type = YUV2RGB;
+			else
+				fg_csc_type = YUV2YUV;
+		}
+	} else {
+		if (in_fmt == RGB) {
+			if (out_fmt == RGB)
+				bg_csc_type = RGB2RGB;
+			else
+				bg_csc_type = RGB2YUV;
+		} else {
+			if (out_fmt == RGB)
+				bg_csc_type = YUV2RGB;
+			else
+				bg_csc_type = YUV2YUV;
+		}
+	}
+
+	/* Transform color key from rgb to yuv if CSC is enabled */
+	reg = __raw_readl(DP_COM_CONF(dp));
+	if (color_key_4rgb && (reg & DP_COM_CONF_GWCKE) &&
+			(((fg_csc_type == RGB2YUV) && (bg_csc_type == YUV2YUV)) ||
+			 ((fg_csc_type == YUV2YUV) && (bg_csc_type == RGB2YUV)) ||
+			 ((fg_csc_type == YUV2YUV) && (bg_csc_type == YUV2YUV)) ||
+			 ((fg_csc_type == YUV2RGB) && (bg_csc_type == YUV2RGB)))) {
+		int red, green, blue;
+		int y, u, v;
+		uint32_t color_key = __raw_readl(DP_GRAPH_WIND_CTRL(dp)) & 0xFFFFFFL;
+
+		dev_dbg(g_ipu_dev, "_ipu_dp_init color key 0x%x need change to yuv fmt!\n", color_key);
+
+		red = (color_key >> 16) & 0xFF;
+		green = (color_key >> 8) & 0xFF;
+		blue = color_key & 0xFF;
+
+		y = _rgb_to_yuv(0, red, green, blue);
+		u = _rgb_to_yuv(1, red, green, blue);
+		v = _rgb_to_yuv(2, red, green, blue);
+		color_key = (y << 16) | (u << 8) | v;
+
+		reg = __raw_readl(DP_GRAPH_WIND_CTRL(dp)) & 0xFF000000L;
+		__raw_writel(reg | color_key, DP_GRAPH_WIND_CTRL(dp));
+		color_key_4rgb = 0;
+
+		dev_dbg(g_ipu_dev, "_ipu_dp_init color key change to yuv fmt 0x%x!\n", color_key);
+	}
+
+	__ipu_dp_csc_setup(dp, dp_csc_array[bg_csc_type][fg_csc_type], true);
+
+	return 0;
+}
+
+void _ipu_dp_uninit(ipu_channel_t channel)
+{
+	int dp;
+	int partial = false;
+
+	if (channel == MEM_FG_SYNC) {
+		dp = DP_SYNC;
+		partial = true;
+	} else if (channel == MEM_BG_SYNC) {
+		dp = DP_SYNC;
+		partial = false;
+	} else if (channel == MEM_BG_ASYNC0) {
+		dp = DP_ASYNC0;
+		partial = false;
+	} else {
+		return;
+	}
+
+	if (partial)
+		fg_csc_type = CSC_NONE;
+	else
+		bg_csc_type = CSC_NONE;
+
+	__ipu_dp_csc_setup(dp, dp_csc_array[bg_csc_type][fg_csc_type], false);
+}
+
+void _ipu_dc_init(int dc_chan, int di, bool interlaced)
+{
+	u32 reg = 0;
+
+	if ((dc_chan == 1) || (dc_chan == 5)) {
+		if (interlaced) {
+			_ipu_dc_link_event(dc_chan, DC_EVT_NL, 0, 3);
+			_ipu_dc_link_event(dc_chan, DC_EVT_EOL, 0, 2);
+			_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA, 0, 1);
+		} else {
+			if (di) {
+				_ipu_dc_link_event(dc_chan, DC_EVT_NL, 2, 3);
+				_ipu_dc_link_event(dc_chan, DC_EVT_EOL, 3, 2);
+				_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA, 4, 1);
+			} else {
+				_ipu_dc_link_event(dc_chan, DC_EVT_NL, 5, 3);
+				_ipu_dc_link_event(dc_chan, DC_EVT_EOL, 6, 2);
+				_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA, 7, 1);
+			}
+		}
+		_ipu_dc_link_event(dc_chan, DC_EVT_NF, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NFIELD, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_EOF, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_EOFIELD, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_CHAN, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_ADDR, 0, 0);
+
+		reg = 0x2;
+		reg |= DC_DISP_ID_SYNC(di) << DC_WR_CH_CONF_PROG_DISP_ID_OFFSET;
+		reg |= di << 2;
+		if (interlaced)
+			reg |= DC_WR_CH_CONF_FIELD_MODE;
+	} else if ((dc_chan == 8) || (dc_chan == 9)) {
+		/* async channels */
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA_W_0, 0x64, 1);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA_W_1, 0x64, 1);
+
+		reg = 0x3;
+		reg |= DC_DISP_ID_SERIAL << DC_WR_CH_CONF_PROG_DISP_ID_OFFSET;
+	}
+	__raw_writel(reg, DC_WR_CH_CONF(dc_chan));
+
+	__raw_writel(0x00000000, DC_WR_CH_ADDR(dc_chan));
+
+	__raw_writel(0x00000084, DC_GEN);
+}
+
+void _ipu_dc_uninit(int dc_chan)
+{
+	if ((dc_chan == 1) || (dc_chan == 5)) {
+		_ipu_dc_link_event(dc_chan, DC_EVT_NL, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_EOL, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NF, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NFIELD, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_EOF, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_EOFIELD, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_CHAN, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_ADDR, 0, 0);
+	} else if ((dc_chan == 8) || (dc_chan == 9)) {
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_ADDR_W_0, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_ADDR_W_1, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_CHAN_W_0, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_CHAN_W_1, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA_W_0, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA_W_1, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_ADDR_R_0, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_ADDR_R_1, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_CHAN_R_0, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_CHAN_R_1, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA_R_0, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA_R_1, 0, 0);
+	}
+}
+
+int _ipu_chan_is_interlaced(ipu_channel_t channel)
+{
+	if (channel == MEM_DC_SYNC)
+		return !!(__raw_readl(DC_WR_CH_CONF_1) &
+			  DC_WR_CH_CONF_FIELD_MODE);
+	else if ((channel == MEM_BG_SYNC) || (channel == MEM_FG_SYNC))
+		return !!(__raw_readl(DC_WR_CH_CONF_5) &
+			  DC_WR_CH_CONF_FIELD_MODE);
+	return 0;
+}
+
+void _ipu_dp_dc_enable(ipu_channel_t channel)
+{
+	int di;
+	uint32_t reg;
+	uint32_t dc_chan;
+	int irq = 0;
+
+	if (channel == MEM_FG_SYNC)
+		irq = IPU_IRQ_DP_SF_END;
+	else if (channel == MEM_DC_SYNC)
+		dc_chan = 1;
+	else if (channel == MEM_BG_SYNC)
+		dc_chan = 5;
+	else
+		return;
+
+	if (channel == MEM_FG_SYNC) {
+		/* Enable FG channel */
+		reg = __raw_readl(DP_COM_CONF(DP_SYNC));
+		__raw_writel(reg | DP_COM_CONF_FG_EN, DP_COM_CONF(DP_SYNC));
+
+		reg = __raw_readl(IPU_SRM_PRI2) | 0x8;
+		__raw_writel(reg, IPU_SRM_PRI2);
+		return;
+	}
+
+	di = g_dc_di_assignment[dc_chan];
+
+	/* Make sure other DC sync channel is not assigned same DI */
+	reg = __raw_readl(DC_WR_CH_CONF(6 - dc_chan));
+	if ((di << 2) == (reg & DC_WR_CH_CONF_PROG_DI_ID)) {
+		reg &= ~DC_WR_CH_CONF_PROG_DI_ID;
+		reg |= di ? 0 : DC_WR_CH_CONF_PROG_DI_ID;
+		__raw_writel(reg, DC_WR_CH_CONF(6 - dc_chan));
+	}
+
+	reg = __raw_readl(DC_WR_CH_CONF(dc_chan));
+	reg |= 4 << DC_WR_CH_CONF_PROG_TYPE_OFFSET;
+	__raw_writel(reg, DC_WR_CH_CONF(dc_chan));
+
+	reg = __raw_readl(IPU_DISP_GEN);
+	if (di)
+		reg |= DI1_COUNTER_RELEASE;
+	else
+		reg |= DI0_COUNTER_RELEASE;
+	__raw_writel(reg, IPU_DISP_GEN);
+}
+
+static bool dc_swap;
+
+static irqreturn_t dc_irq_handler(int irq, void *dev_id)
+{
+	struct completion *comp = dev_id;
+
+	complete(comp);
+	return IRQ_HANDLED;
+}
+
+void _ipu_dp_dc_disable(ipu_channel_t channel, bool swap)
+{
+	int ret;
+	unsigned long lock_flags;
+	uint32_t reg;
+	uint32_t csc;
+	uint32_t dc_chan;
+	int irq = 0;
+	int timeout = 50;
+	DECLARE_COMPLETION_ONSTACK(dc_comp);
+
+	dc_swap = swap;
+
+	if (channel == MEM_DC_SYNC) {
+		dc_chan = 1;
+		irq = IPU_IRQ_DC_FC_1;
+	} else if (channel == MEM_BG_SYNC) {
+		dc_chan = 5;
+		irq = IPU_IRQ_DP_SF_END;
+	} else if (channel == MEM_FG_SYNC) {
+		/* Disable FG channel */
+		dc_chan = 5;
+
+		spin_lock_irqsave(&ipu_lock, lock_flags);
+
+		reg = __raw_readl(DP_COM_CONF(DP_SYNC));
+		csc = reg & DP_COM_CONF_CSC_DEF_MASK;
+		if (csc == DP_COM_CONF_CSC_DEF_FG)
+			reg &= ~DP_COM_CONF_CSC_DEF_MASK;
+
+		reg &= ~DP_COM_CONF_FG_EN;
+		__raw_writel(reg, DP_COM_CONF(DP_SYNC));
+
+		reg = __raw_readl(IPU_SRM_PRI2) | 0x8;
+		__raw_writel(reg, IPU_SRM_PRI2);
+
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+		__raw_writel(IPUIRQ_2_MASK(IPU_IRQ_DP_SF_END),
+			     IPUIRQ_2_STATREG(IPU_IRQ_DP_SF_END));
+		while ((__raw_readl(IPUIRQ_2_STATREG(IPU_IRQ_DP_SF_END)) &
+			IPUIRQ_2_MASK(IPU_IRQ_DP_SF_END)) == 0) {
+			msleep(2);
+			timeout -= 2;
+			if (timeout <= 0)
+				break;
+		}
+
+		timeout = 50;
+
+		/*
+		 * Wait for DC triple buffer to empty,
+		 * this check is useful for tv overlay.
+		 */
+		if (g_dc_di_assignment[dc_chan] == 0)
+			while ((__raw_readl(DC_STAT) & 0x00000002)
+			       != 0x00000002) {
+				msleep(2);
+				timeout -= 2;
+				if (timeout <= 0)
+					break;
+			}
+		else if (g_dc_di_assignment[dc_chan] == 1)
+			while ((__raw_readl(DC_STAT) & 0x00000020)
+			       != 0x00000020) {
+				msleep(2);
+				timeout -= 2;
+				if (timeout <= 0)
+					break;
+			}
+		return;
+	} else {
+		return;
+	}
+
+	if (!dc_swap)
+		__raw_writel(IPUIRQ_2_MASK(IPU_IRQ_VSYNC_PRE_0
+			+ g_dc_di_assignment[dc_chan]),
+		     IPUIRQ_2_STATREG(IPU_IRQ_VSYNC_PRE_0
+			+ g_dc_di_assignment[dc_chan]));
+	ipu_clear_irq(irq);
+	ret = ipu_request_irq(irq, dc_irq_handler, 0, NULL, &dc_comp);
+	if (ret < 0) {
+		dev_err(g_ipu_dev, "DC irq %d in use\n", irq);
+		return;
+	}
+	ret = wait_for_completion_timeout(&dc_comp, msecs_to_jiffies(50));
+
+	dev_dbg(g_ipu_dev, "DC stop timeout - %d * 10ms\n", 5 - ret);
+	ipu_free_irq(irq, &dc_comp);
+
+	if (dc_swap) {
+		spin_lock_irqsave(&ipu_lock, lock_flags);
+		/* Swap DC channel 1 and 5 settings, and disable old dc chan */
+		reg = __raw_readl(DC_WR_CH_CONF(dc_chan));
+		__raw_writel(reg, DC_WR_CH_CONF(6 - dc_chan));
+		reg &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
+		reg ^= DC_WR_CH_CONF_PROG_DI_ID;
+		__raw_writel(reg, DC_WR_CH_CONF(dc_chan));
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	} else {
+		timeout = 50;
+
+		/* Wait for DC triple buffer to empty */
+		if (g_dc_di_assignment[dc_chan] == 0)
+			while ((__raw_readl(DC_STAT) & 0x00000002)
+				!= 0x00000002) {
+				msleep(2);
+				timeout -= 2;
+				if (timeout <= 0)
+					break;
+			}
+		else if (g_dc_di_assignment[dc_chan] == 1)
+			while ((__raw_readl(DC_STAT) & 0x00000020)
+				!= 0x00000020) {
+				msleep(2);
+				timeout -= 2;
+				if (timeout <= 0)
+					break;
+			}
+
+		spin_lock_irqsave(&ipu_lock, lock_flags);
+		reg = __raw_readl(DC_WR_CH_CONF(dc_chan));
+		reg &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
+		__raw_writel(reg, DC_WR_CH_CONF(dc_chan));
+
+		reg = __raw_readl(IPU_DISP_GEN);
+		if (g_dc_di_assignment[dc_chan])
+			reg &= ~DI1_COUNTER_RELEASE;
+		else
+			reg &= ~DI0_COUNTER_RELEASE;
+		__raw_writel(reg, IPU_DISP_GEN);
+
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+		if (__raw_readl(IPUIRQ_2_STATREG(IPU_IRQ_VSYNC_PRE_0
+			+ g_dc_di_assignment[dc_chan])) &
+			IPUIRQ_2_MASK(IPU_IRQ_VSYNC_PRE_0
+			+ g_dc_di_assignment[dc_chan]))
+			dev_dbg(g_ipu_dev,
+				"VSyncPre occurred before DI%d disable\n",
+				g_dc_di_assignment[dc_chan]);
+	}
+}
+
+void _ipu_init_dc_mappings(void)
+{
+	/* IPU_PIX_FMT_RGB24 */
+	_ipu_dc_map_clear(0);
+	_ipu_dc_map_config(0, 0, 7, 0xFF);
+	_ipu_dc_map_config(0, 1, 15, 0xFF);
+	_ipu_dc_map_config(0, 2, 23, 0xFF);
+
+	/* IPU_PIX_FMT_RGB666 */
+	_ipu_dc_map_clear(1);
+	_ipu_dc_map_config(1, 0, 5, 0xFC);
+	_ipu_dc_map_config(1, 1, 11, 0xFC);
+	_ipu_dc_map_config(1, 2, 17, 0xFC);
+
+	/* IPU_PIX_FMT_YUV444 */
+	_ipu_dc_map_clear(2);
+	_ipu_dc_map_config(2, 0, 15, 0xFF);
+	_ipu_dc_map_config(2, 1, 23, 0xFF);
+	_ipu_dc_map_config(2, 2, 7, 0xFF);
+
+	/* IPU_PIX_FMT_RGB565 */
+	_ipu_dc_map_clear(3);
+	_ipu_dc_map_config(3, 0, 4, 0xF8);
+	_ipu_dc_map_config(3, 1, 10, 0xFC);
+	_ipu_dc_map_config(3, 2, 15, 0xF8);
+
+	/* IPU_PIX_FMT_LVDS666 */
+	_ipu_dc_map_clear(4);
+	_ipu_dc_map_config(4, 0, 5, 0xFC);
+	_ipu_dc_map_config(4, 1, 13, 0xFC);
+	_ipu_dc_map_config(4, 2, 21, 0xFC);
+}
+
+int _ipu_pixfmt_to_map(uint32_t fmt)
+{
+	switch (fmt) {
+	case IPU_PIX_FMT_GENERIC:
+	case IPU_PIX_FMT_RGB24:
+		return 0;
+	case IPU_PIX_FMT_RGB666:
+		return 1;
+	case IPU_PIX_FMT_YUV444:
+		return 2;
+	case IPU_PIX_FMT_RGB565:
+		return 3;
+	case IPU_PIX_FMT_LVDS666:
+		return 4;
+	}
+
+	return -1;
+}
+
+/*!
+ * This function sets the colorspace for of dp.
+ * modes.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       param         	If it's not NULL, update the csc table
+ *                              with this parameter.
+ *
+ * @return      N/A
+ */
+void _ipu_dp_set_csc_coefficients(ipu_channel_t channel, int32_t param[][3])
+{
+	int dp;
+	struct dp_csc_param_t dp_csc_param;
+
+	if (channel == MEM_FG_SYNC)
+		dp = DP_SYNC;
+	else if (channel == MEM_BG_SYNC)
+		dp = DP_SYNC;
+	else if (channel == MEM_BG_ASYNC0)
+		dp = DP_ASYNC0;
+	else
+		return;
+
+	dp_csc_param.mode = -1;
+	dp_csc_param.coeff = param;
+	__ipu_dp_csc_setup(dp, dp_csc_param, true);
+}
+
+/*!
+ * This function is called to initialize a synchronous LCD panel.
+ *
+ * @param       disp            The DI the panel is attached to.
+ *
+ * @param       pixel_clk       Desired pixel clock frequency in Hz.
+ *
+ * @param       pixel_fmt       Input parameter for pixel format of buffer.
+ *                              Pixel format is a FOURCC ASCII code.
+ *
+ * @param       width           The width of panel in pixels.
+ *
+ * @param       height          The height of panel in pixels.
+ *
+ * @param       hStartWidth     The number of pixel clocks between the HSYNC
+ *                              signal pulse and the start of valid data.
+ *
+ * @param       hSyncWidth      The width of the HSYNC signal in units of pixel
+ *                              clocks.
+ *
+ * @param       hEndWidth       The number of pixel clocks between the end of
+ *                              valid data and the HSYNC signal for next line.
+ *
+ * @param       vStartWidth     The number of lines between the VSYNC
+ *                              signal pulse and the start of valid data.
+ *
+ * @param       vSyncWidth      The width of the VSYNC signal in units of lines
+ *
+ * @param       vEndWidth       The number of lines between the end of valid
+ *                              data and the VSYNC signal for next frame.
+ *
+ * @param       sig             Bitfield of signal polarities for LCD interface.
+ *
+ * @return      This function returns 0 on success or negative error code on
+ *              fail.
+ */
+int32_t ipu_init_sync_panel(int disp, uint32_t pixel_clk,
+			    uint16_t width, uint16_t height,
+			    uint32_t pixel_fmt,
+			    uint16_t h_start_width, uint16_t h_sync_width,
+			    uint16_t h_end_width, uint16_t v_start_width,
+			    uint16_t v_sync_width, uint16_t v_end_width,
+			    uint32_t v_to_h_sync, ipu_di_signal_cfg_t sig)
+{
+	unsigned long lock_flags;
+	uint32_t field0_offset = 0;
+	uint32_t field1_offset;
+	uint32_t reg;
+	uint32_t disp_gen, di_gen, vsync_cnt;
+	uint32_t div;
+	uint32_t h_total, v_total;
+	int map;
+	struct clk *di_clk;
+
+	dev_dbg(g_ipu_dev, "panel size = %d x %d\n", width, height);
+
+	if ((v_sync_width == 0) || (h_sync_width == 0))
+		return EINVAL;
+
+	h_total = width + h_sync_width + h_start_width + h_end_width;
+	v_total = height + v_sync_width + v_start_width + v_end_width;
+
+	/* Init clocking */
+	dev_dbg(g_ipu_dev, "pixel clk = %d\n", pixel_clk);
+	if (sig.ext_clk)
+		di_clk = g_di_clk[disp];
+	else
+		di_clk = g_ipu_clk;
+
+	/*
+	 * Calculate divider
+	 * Fractional part is 4 bits,
+	 * so simply multiply by 2^4 to get fractional part.
+	 */
+	div = (clk_get_rate(di_clk) * 16) / pixel_clk;
+	if (div < 0x10)	/* Min DI disp clock divider is 1 */
+		div = 0x10;
+	/*
+	 * DI disp clock offset is zero,
+	 * and fractional part is rounded off to 0.5.
+	 */
+	div &= 0xFF8;
+
+	reg = __raw_readl(DI_GENERAL(disp));
+	if (sig.ext_clk)
+		__raw_writel(reg | DI_GEN_DI_CLK_EXT, DI_GENERAL(disp));
+	else
+		__raw_writel(reg & ~DI_GEN_DI_CLK_EXT, DI_GENERAL(disp));
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	disp_gen = __raw_readl(IPU_DISP_GEN);
+	disp_gen &= disp ? ~DI1_COUNTER_RELEASE : ~DI0_COUNTER_RELEASE;
+	__raw_writel(disp_gen, IPU_DISP_GEN);
+
+	__raw_writel(div, DI_BS_CLKGEN0(disp));
+
+	/* Setup pixel clock timing */
+	/* FIXME: needs to be more flexible */
+	/* Down time is half of period */
+	__raw_writel((div / 16) << 16, DI_BS_CLKGEN1(disp));
+
+	_ipu_di_data_wave_config(disp, SYNC_WAVE, div / 16 - 1, div / 16 - 1);
+	_ipu_di_data_pin_config(disp, SYNC_WAVE, DI_PIN15, 3, 0, div / 16 * 2);
+
+	div = div / 16;		/* Now divider is integer portion */
+
+	map = _ipu_pixfmt_to_map(pixel_fmt);
+	if (map < 0) {
+		dev_dbg(g_ipu_dev, "IPU_DISP: No MAP\n");
+		spin_unlock_irqrestore(&ipu_lock, lock_flags);
+		return -EINVAL;
+	}
+
+	di_gen = 0;
+	if (sig.ext_clk)
+		di_gen |= DI_GEN_DI_CLK_EXT;
+
+	if (sig.interlaced) {
+		if (cpu_is_mx51_rev(CHIP_REV_2_0)) {
+			/* Setup internal HSYNC waveform */
+			_ipu_di_sync_config(
+					disp, 			/* display */
+					1, 				/* counter */
+					h_total/2 - 1, 	/* run count */
+					DI_SYNC_CLK,		 /* run_resolution */
+					0, 				/* offset */
+					DI_SYNC_NONE, 	/* offset resolution */
+					0, 				/* repeat count */
+					DI_SYNC_NONE, 	/* CNT_CLR_SEL */
+					0, 				/* CNT_POLARITY_GEN_EN */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+					0, 				/* COUNT UP */
+					0				/* COUNT DOWN */
+					);
+
+			/* Field 1 VSYNC waveform */
+			_ipu_di_sync_config(
+					disp, 			/* display */
+					2, 				/* counter */
+					h_total - 1, 		/* run count */
+					DI_SYNC_CLK,		/* run_resolution */
+					0, 				/* offset */
+					DI_SYNC_NONE, 	/* offset resolution */
+					0, 				/* repeat count */
+					DI_SYNC_NONE, 	/* CNT_CLR_SEL */
+					0, 				/* CNT_POLARITY_GEN_EN */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+					0, 				/* COUNT UP */
+					4				/* COUNT DOWN */
+					);
+
+			/* Setup internal HSYNC waveform */
+			_ipu_di_sync_config(
+					disp, 			/* display */
+					3, 				/* counter */
+					v_total*2 - 1, 	/* run count */
+					DI_SYNC_INT_HSYNC,	/* run_resolution */
+					1, 				/* offset */
+					DI_SYNC_INT_HSYNC, 	/* offset resolution */
+					0, 				/* repeat count */
+					DI_SYNC_NONE, 	/* CNT_CLR_SEL */
+					0, 				/* CNT_POLARITY_GEN_EN */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+					0, 				/* COUNT UP */
+					4				/* COUNT DOWN */
+					);
+
+			/* Active Field ? */
+			_ipu_di_sync_config(
+					disp, 			/* display */
+					4, 				/* counter */
+					v_total/2 - 1, 	/* run count */
+					DI_SYNC_HSYNC,	/* run_resolution */
+					v_start_width, 	/*  offset */
+					DI_SYNC_HSYNC, 	/* offset resolution */
+					2, 				/* repeat count */
+					DI_SYNC_VSYNC, 	/* CNT_CLR_SEL */
+					0, 				/* CNT_POLARITY_GEN_EN */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+					0, 				/* COUNT UP */
+					0				/* COUNT DOWN */
+					);
+
+			/* Active Line */
+			_ipu_di_sync_config(
+					disp, 			/* display */
+					5, 				/* counter */
+					0, 				/* run count */
+					DI_SYNC_HSYNC,	/* run_resolution */
+					0, 				/*  offset */
+					DI_SYNC_NONE, 	/* offset resolution */
+					height/2, 		/* repeat count */
+					4, 				/* CNT_CLR_SEL */
+					0, 				/* CNT_POLARITY_GEN_EN */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+					0, 				/* COUNT UP */
+					0				/* COUNT DOWN */
+					);
+
+			/* Field 0 VSYNC waveform */
+			_ipu_di_sync_config(
+					disp, 			/* display */
+					6, 				/* counter */
+					v_total - 1, 	/* run count */
+					DI_SYNC_HSYNC,	/* run_resolution */
+					0, 				/* offset */
+					DI_SYNC_NONE, 	/* offset resolution */
+					0, 				/* repeat count */
+					DI_SYNC_NONE, 	/* CNT_CLR_SEL  */
+					0, 				/* CNT_POLARITY_GEN_EN */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+					0, 				/* COUNT UP */
+					0				/* COUNT DOWN */
+					);
+
+			/* DC VSYNC waveform */
+			vsync_cnt = 7;
+			_ipu_di_sync_config(
+					disp, 			/* display */
+					7, 				/* counter */
+					v_total/2 - 1, 	/* run count */
+					DI_SYNC_HSYNC,	/* run_resolution  */
+					9, 				/* offset  */
+					DI_SYNC_HSYNC, 	/* offset resolution */
+					2, 				/* repeat count */
+					DI_SYNC_VSYNC, 	/* CNT_CLR_SEL */
+					0, 				/* CNT_POLARITY_GEN_EN */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+					0, 				/* COUNT UP */
+					0				/* COUNT DOWN */
+					);
+
+			/* active pixel waveform */
+			_ipu_di_sync_config(
+					disp, 			/* display */
+					8, 				/* counter */
+					0, 	/* run count  */
+					DI_SYNC_CLK,	/* run_resolution */
+					h_start_width, 				/* offset  */
+					DI_SYNC_CLK, 	/* offset resolution */
+					width, 				/* repeat count  */
+					5, 	/* CNT_CLR_SEL  */
+					0, 				/* CNT_POLARITY_GEN_EN  */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL  */
+					0, 				/* COUNT UP  */
+					0				/* COUNT DOWN */
+					);
+
+			/* ??? */
+			_ipu_di_sync_config(
+					disp, 			/* display */
+					9, 				/* counter */
+					v_total - 1, 	/* run count */
+					DI_SYNC_INT_HSYNC,	/* run_resolution */
+					v_total/2, 			/* offset  */
+					DI_SYNC_INT_HSYNC, 	/* offset resolution  */
+					0, 				/* repeat count */
+					DI_SYNC_HSYNC, 	/* CNT_CLR_SEL */
+					0, 				/* CNT_POLARITY_GEN_EN  */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL  */
+					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+					0, 				/* COUNT UP */
+					4				/* COUNT DOWN */
+					);
+
+			/* set gentime select and tag sel */
+			reg = __raw_readl(DI_SW_GEN1(disp, 9));
+			reg &= 0x1FFFFFFF;
+			reg |= (3-1)<<29 | 0x00008000;
+			__raw_writel(reg, DI_SW_GEN1(disp, 9));
+
+			__raw_writel(v_total / 2 - 1, DI_SCR_CONF(disp));
+
+			/* set y_sel = 1 */
+			di_gen |= 0x10000000;
+			di_gen |= DI_GEN_POLARITY_5;
+			di_gen |= DI_GEN_POLARITY_8;
+		} else {
+			/* Setup internal HSYNC waveform */
+			_ipu_di_sync_config(disp, 1, h_total - 1, DI_SYNC_CLK,
+					0, DI_SYNC_NONE, 0, DI_SYNC_NONE, 0, DI_SYNC_NONE,
+					DI_SYNC_NONE, 0, 0);
+
+			field1_offset = v_sync_width + v_start_width + height / 2 +
+				v_end_width;
+			if (sig.odd_field_first) {
+				field0_offset = field1_offset - 1;
+				field1_offset = 0;
+			}
+			v_total += v_start_width + v_end_width;
+
+			/* Field 1 VSYNC waveform */
+			_ipu_di_sync_config(disp, 2, v_total - 1, 1,
+					field0_offset,
+					field0_offset ? 1 : DI_SYNC_NONE,
+					0, DI_SYNC_NONE, 0,
+					DI_SYNC_NONE, DI_SYNC_NONE, 0, 4);
+
+			/* Setup internal HSYNC waveform */
+			_ipu_di_sync_config(disp, 3, h_total - 1, DI_SYNC_CLK,
+					0, DI_SYNC_NONE, 0, DI_SYNC_NONE, 0,
+					DI_SYNC_NONE, DI_SYNC_NONE, 0, 4);
+
+			/* Active Field ? */
+			_ipu_di_sync_config(disp, 4,
+					field0_offset ?
+					field0_offset : field1_offset - 2,
+					1, v_start_width + v_sync_width, 1, 2, 2,
+					0, DI_SYNC_NONE, DI_SYNC_NONE, 0, 0);
+
+			/* Active Line */
+			_ipu_di_sync_config(disp, 5, 0, 1,
+					0, DI_SYNC_NONE,
+					height / 2, 4, 0, DI_SYNC_NONE,
+					DI_SYNC_NONE, 0, 0);
+
+			/* Field 0 VSYNC waveform */
+			_ipu_di_sync_config(disp, 6, v_total - 1, 1,
+					0, DI_SYNC_NONE,
+					0, DI_SYNC_NONE, 0, DI_SYNC_NONE,
+					DI_SYNC_NONE, 0, 0);
+
+			/* DC VSYNC waveform */
+			vsync_cnt = 7;
+			_ipu_di_sync_config(disp, 7, 0, 1,
+					field1_offset,
+					field1_offset ? 1 : DI_SYNC_NONE,
+					1, 2, 0, DI_SYNC_NONE, DI_SYNC_NONE, 0, 0);
+
+			/* active pixel waveform */
+			_ipu_di_sync_config(disp, 8, 0, DI_SYNC_CLK,
+					h_sync_width + h_start_width, DI_SYNC_CLK,
+					width, 5, 0, DI_SYNC_NONE, DI_SYNC_NONE,
+					0, 0);
+
+			/* ??? */
+			_ipu_di_sync_config(disp, 9, v_total - 1, 2,
+					0, DI_SYNC_NONE,
+					0, DI_SYNC_NONE, 6, DI_SYNC_NONE,
+					DI_SYNC_NONE, 0, 0);
+
+			reg = __raw_readl(DI_SW_GEN1(disp, 9));
+			reg |= 0x8000;
+			__raw_writel(reg, DI_SW_GEN1(disp, 9));
+
+			__raw_writel(v_sync_width + v_start_width +
+					v_end_width + height / 2 - 1, DI_SCR_CONF(disp));
+		}
+
+		/* Init template microcode */
+		_ipu_dc_write_tmpl(0, WROD(0), 0, map, SYNC_WAVE, 0, 8);
+
+		if (sig.Hsync_pol)
+			di_gen |= DI_GEN_POLARITY_3;
+		if (sig.Vsync_pol)
+			di_gen |= DI_GEN_POLARITY_2;
+	} else {
+		/* Setup internal HSYNC waveform */
+		_ipu_di_sync_config(disp, 1, h_total - 1, DI_SYNC_CLK,
+					0, DI_SYNC_NONE, 0, DI_SYNC_NONE, 0, DI_SYNC_NONE,
+					DI_SYNC_NONE, 0, 0);
+
+		/* Setup external (delayed) HSYNC waveform */
+		_ipu_di_sync_config(disp, DI_SYNC_HSYNC, h_total - 1,
+				    DI_SYNC_CLK, div * v_to_h_sync, DI_SYNC_CLK,
+				    0, DI_SYNC_NONE, 1, DI_SYNC_NONE,
+				    DI_SYNC_CLK, 0, h_sync_width * 2);
+		/* Setup VSYNC waveform */
+		vsync_cnt = DI_SYNC_VSYNC;
+		_ipu_di_sync_config(disp, DI_SYNC_VSYNC, v_total - 1,
+				    DI_SYNC_INT_HSYNC, 0, DI_SYNC_NONE, 0,
+				    DI_SYNC_NONE, 1, DI_SYNC_NONE,
+				    DI_SYNC_INT_HSYNC, 0, v_sync_width * 2);
+		__raw_writel(v_total - 1, DI_SCR_CONF(disp));
+
+		/* Setup active data waveform to sync with DC */
+		_ipu_di_sync_config(disp, 4, 0, DI_SYNC_HSYNC,
+				    v_sync_width + v_start_width, DI_SYNC_HSYNC, height,
+				    DI_SYNC_VSYNC, 0, DI_SYNC_NONE,
+				    DI_SYNC_NONE, 0, 0);
+		_ipu_di_sync_config(disp, 5, 0, DI_SYNC_CLK,
+				    h_sync_width + h_start_width, DI_SYNC_CLK,
+				    width, 4, 0, DI_SYNC_NONE, DI_SYNC_NONE, 0,
+				    0);
+
+		/* reset all unused counters */
+		__raw_writel(0, DI_SW_GEN0(disp, 6));
+		__raw_writel(0, DI_SW_GEN1(disp, 6));
+		__raw_writel(0, DI_SW_GEN0(disp, 7));
+		__raw_writel(0, DI_SW_GEN1(disp, 7));
+		__raw_writel(0, DI_SW_GEN0(disp, 8));
+		__raw_writel(0, DI_SW_GEN1(disp, 8));
+		__raw_writel(0, DI_SW_GEN0(disp, 9));
+		__raw_writel(0, DI_SW_GEN1(disp, 9));
+
+		reg = __raw_readl(DI_STP_REP(disp, 6));
+		reg &= 0x0000FFFF;
+		__raw_writel(reg, DI_STP_REP(disp, 6));
+		__raw_writel(0, DI_STP_REP(disp, 7));
+		__raw_writel(0, DI_STP_REP(disp, 9));
+
+		/* Init template microcode */
+		if (disp) {
+		   _ipu_dc_write_tmpl(2, WROD(0), 0, map, SYNC_WAVE, 8, 5);
+		   _ipu_dc_write_tmpl(3, WROD(0), 0, map, SYNC_WAVE, 4, 5);
+		   _ipu_dc_write_tmpl(4, WROD(0), 0, map, SYNC_WAVE, 0, 5);
+		} else {
+		   _ipu_dc_write_tmpl(5, WROD(0), 0, map, SYNC_WAVE, 8, 5);
+		   _ipu_dc_write_tmpl(6, WROD(0), 0, map, SYNC_WAVE, 4, 5);
+		   _ipu_dc_write_tmpl(7, WROD(0), 0, map, SYNC_WAVE, 0, 5);
+		}
+
+		if (sig.Hsync_pol)
+			di_gen |= DI_GEN_POLARITY_2;
+		if (sig.Vsync_pol)
+			di_gen |= DI_GEN_POLARITY_3;
+	}
+
+	__raw_writel(di_gen, DI_GENERAL(disp));
+	__raw_writel((--vsync_cnt << DI_VSYNC_SEL_OFFSET) | 0x00000002,
+		     DI_SYNC_AS_GEN(disp));
+
+	reg = __raw_readl(DI_POL(disp));
+	reg &= ~(DI_POL_DRDY_DATA_POLARITY | DI_POL_DRDY_POLARITY_15);
+	if (sig.enable_pol)
+		reg |= DI_POL_DRDY_POLARITY_15;
+	if (sig.data_pol)
+		reg |= DI_POL_DRDY_DATA_POLARITY;
+	__raw_writel(reg, DI_POL(disp));
+
+	__raw_writel(width, DC_DISP_CONF2(DC_DISP_ID_SYNC(disp)));
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_init_sync_panel);
+
+
+int ipu_init_async_panel(int disp, int type, uint32_t cycle_time,
+			 uint32_t pixel_fmt, ipu_adc_sig_cfg_t sig)
+{
+	unsigned long lock_flags;
+	int map;
+	u32 ser_conf = 0;
+	u32 div;
+	u32 di_clk = clk_get_rate(g_ipu_clk);
+
+	/* round up cycle_time, then calcalate the divider using scaled math */
+	cycle_time += (1000000000UL / di_clk) - 1;
+	div = (cycle_time * (di_clk / 256UL)) / (1000000000UL / 256UL);
+
+	map = _ipu_pixfmt_to_map(pixel_fmt);
+	if (map < 0)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if (type == IPU_PANEL_SERIAL) {
+		__raw_writel((div << 24) | ((sig.ifc_width - 1) << 4),
+			     DI_DW_GEN(disp, ASYNC_SER_WAVE));
+
+		_ipu_di_data_pin_config(disp, ASYNC_SER_WAVE, DI_PIN_CS,
+					0, 0, (div * 2) + 1);
+		_ipu_di_data_pin_config(disp, ASYNC_SER_WAVE, DI_PIN_SER_CLK,
+					1, div, div * 2);
+		_ipu_di_data_pin_config(disp, ASYNC_SER_WAVE, DI_PIN_SER_RS,
+					2, 0, 0);
+
+		_ipu_dc_write_tmpl(0x64, WROD(0), 0, map, ASYNC_SER_WAVE, 0, 0);
+
+		/* Configure DC for serial panel */
+		__raw_writel(0x14, DC_DISP_CONF1(DC_DISP_ID_SERIAL));
+
+		if (sig.clk_pol)
+			ser_conf |= DI_SER_CONF_SERIAL_CLK_POL;
+		if (sig.data_pol)
+			ser_conf |= DI_SER_CONF_SERIAL_DATA_POL;
+		if (sig.rs_pol)
+			ser_conf |= DI_SER_CONF_SERIAL_RS_POL;
+		if (sig.cs_pol)
+			ser_conf |= DI_SER_CONF_SERIAL_CS_POL;
+		__raw_writel(ser_conf, DI_SER_CONF(disp));
+	}
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	return 0;
+}
+EXPORT_SYMBOL(ipu_init_async_panel);
+
+/*!
+ * This function sets the foreground and background plane global alpha blending
+ * modes. This function also sets the DP graphic plane according to the
+ * parameter of IPUv3 DP channel.
+ *
+ * @param	channel		IPUv3 DP channel
+ *
+ * @param       enable          Boolean to enable or disable global alpha
+ *                              blending. If disabled, local blending is used.
+ *
+ * @param       alpha           Global alpha value.
+ *
+ * @return      Returns 0 on success or negative error code on fail
+ */
+int32_t ipu_disp_set_global_alpha(ipu_channel_t channel, bool enable,
+				  uint8_t alpha)
+{
+	uint32_t reg;
+	uint32_t flow;
+	unsigned long lock_flags;
+	bool bg_chan;
+
+	if (channel == MEM_BG_SYNC || channel == MEM_FG_SYNC)
+		flow = DP_SYNC;
+	else if (channel == MEM_BG_ASYNC0 || channel == MEM_FG_ASYNC0)
+		flow = DP_ASYNC0;
+	else if (channel == MEM_BG_ASYNC1 || channel == MEM_FG_ASYNC1)
+		flow = DP_ASYNC1;
+	else
+		return -EINVAL;
+
+	if (channel == MEM_BG_SYNC || channel == MEM_BG_ASYNC0 ||
+	    channel == MEM_BG_ASYNC1)
+		bg_chan = true;
+	else
+		bg_chan = false;
+
+	if (!g_ipu_clk_enabled)
+		clk_enable(g_ipu_clk);
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	if (bg_chan) {
+		reg = __raw_readl(DP_COM_CONF(flow));
+		__raw_writel(reg & ~DP_COM_CONF_GWSEL, DP_COM_CONF(flow));
+	} else {
+		reg = __raw_readl(DP_COM_CONF(flow));
+		__raw_writel(reg | DP_COM_CONF_GWSEL, DP_COM_CONF(flow));
+	}
+
+	if (enable) {
+		reg = __raw_readl(DP_GRAPH_WIND_CTRL(flow)) & 0x00FFFFFFL;
+		__raw_writel(reg | ((uint32_t) alpha << 24),
+			     DP_GRAPH_WIND_CTRL(flow));
+
+		reg = __raw_readl(DP_COM_CONF(flow));
+		__raw_writel(reg | DP_COM_CONF_GWAM, DP_COM_CONF(flow));
+	} else {
+		reg = __raw_readl(DP_COM_CONF(flow));
+		__raw_writel(reg & ~DP_COM_CONF_GWAM, DP_COM_CONF(flow));
+	}
+
+	reg = __raw_readl(IPU_SRM_PRI2) | 0x8;
+	__raw_writel(reg, IPU_SRM_PRI2);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	if (!g_ipu_clk_enabled)
+		clk_disable(g_ipu_clk);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_disp_set_global_alpha);
+
+/*!
+ * This function sets the transparent color key for SDC graphic plane.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       enable          Boolean to enable or disable color key
+ *
+ * @param       colorKey        24-bit RGB color for transparent color key.
+ *
+ * @return      Returns 0 on success or negative error code on fail
+ */
+int32_t ipu_disp_set_color_key(ipu_channel_t channel, bool enable,
+			       uint32_t color_key)
+{
+	uint32_t reg, flow;
+	int y, u, v;
+	int red, green, blue;
+	unsigned long lock_flags;
+
+	if (channel == MEM_BG_SYNC || channel == MEM_FG_SYNC)
+		flow = DP_SYNC;
+	else if (channel == MEM_BG_ASYNC0 || channel == MEM_FG_ASYNC0)
+		flow = DP_ASYNC0;
+	else if (channel == MEM_BG_ASYNC1 || channel == MEM_FG_ASYNC1)
+		flow = DP_ASYNC1;
+	else
+		return -EINVAL;
+
+	if (!g_ipu_clk_enabled)
+		clk_enable(g_ipu_clk);
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	color_key_4rgb = 1;
+	/* Transform color key from rgb to yuv if CSC is enabled */
+	if (((fg_csc_type == RGB2YUV) && (bg_csc_type == YUV2YUV)) ||
+			((fg_csc_type == YUV2YUV) && (bg_csc_type == RGB2YUV)) ||
+			((fg_csc_type == YUV2YUV) && (bg_csc_type == YUV2YUV)) ||
+			((fg_csc_type == YUV2RGB) && (bg_csc_type == YUV2RGB))) {
+
+		dev_dbg(g_ipu_dev, "color key 0x%x need change to yuv fmt\n", color_key);
+
+		red = (color_key >> 16) & 0xFF;
+		green = (color_key >> 8) & 0xFF;
+		blue = color_key & 0xFF;
+
+		y = _rgb_to_yuv(0, red, green, blue);
+		u = _rgb_to_yuv(1, red, green, blue);
+		v = _rgb_to_yuv(2, red, green, blue);
+		color_key = (y << 16) | (u << 8) | v;
+
+		color_key_4rgb = 0;
+
+		dev_dbg(g_ipu_dev, "color key change to yuv fmt 0x%x\n", color_key);
+	}
+
+	if (enable) {
+		reg = __raw_readl(DP_GRAPH_WIND_CTRL(flow)) & 0xFF000000L;
+		__raw_writel(reg | color_key, DP_GRAPH_WIND_CTRL(flow));
+
+		reg = __raw_readl(DP_COM_CONF(flow));
+		__raw_writel(reg | DP_COM_CONF_GWCKE, DP_COM_CONF(flow));
+	} else {
+		reg = __raw_readl(DP_COM_CONF(flow));
+		__raw_writel(reg & ~DP_COM_CONF_GWCKE, DP_COM_CONF(flow));
+	}
+
+	reg = __raw_readl(IPU_SRM_PRI2) | 0x8;
+	__raw_writel(reg, IPU_SRM_PRI2);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	if (!g_ipu_clk_enabled)
+		clk_disable(g_ipu_clk);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_disp_set_color_key);
+
+/*!
+ * This function sets the window position of the foreground or background plane.
+ * modes.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       x_pos           The X coordinate position to place window at.
+ *                              The position is relative to the top left corner.
+ *
+ * @param       y_pos           The Y coordinate position to place window at.
+ *                              The position is relative to the top left corner.
+ *
+ * @return      Returns 0 on success or negative error code on fail
+ */
+int32_t ipu_disp_set_window_pos(ipu_channel_t channel, int16_t x_pos,
+				int16_t y_pos)
+{
+	u32 reg;
+	unsigned long lock_flags;
+	uint32_t flow = 0;
+
+	if (channel == MEM_FG_SYNC)
+		flow = DP_SYNC;
+	else if (channel == MEM_FG_ASYNC0)
+		flow = DP_ASYNC0;
+	else if (channel == MEM_FG_ASYNC1)
+		flow = DP_ASYNC1;
+	else
+		return -EINVAL;
+
+	if (!g_ipu_clk_enabled)
+		clk_enable(g_ipu_clk);
+
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	__raw_writel((x_pos << 16) | y_pos, DP_FG_POS(flow));
+
+	reg = __raw_readl(IPU_SRM_PRI2) | 0x8;
+	__raw_writel(reg, IPU_SRM_PRI2);
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	if (!g_ipu_clk_enabled)
+		clk_disable(g_ipu_clk);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_disp_set_window_pos);
+
+void ipu_disp_direct_write(ipu_channel_t channel, u32 value, u32 offset)
+{
+	if (channel == DIRECT_ASYNC0)
+		__raw_writel(value, ipu_disp_base[0] + offset);
+	else if (channel == DIRECT_ASYNC1)
+		__raw_writel(value, ipu_disp_base[1] + offset);
+}
+EXPORT_SYMBOL(ipu_disp_direct_write);
+
+void ipu_reset_disp_panel(void)
+{
+	uint32_t tmp;
+
+	tmp = __raw_readl(DI_GENERAL(1));
+	__raw_writel(tmp | 0x08, DI_GENERAL(1));
+	msleep(10); /* tRES >= 100us */
+	tmp = __raw_readl(DI_GENERAL(1));
+	__raw_writel(tmp & ~0x08, DI_GENERAL(1));
+	msleep(60);
+
+	return;
+}
+EXPORT_SYMBOL(ipu_reset_disp_panel);
diff --git a/drivers/mxc/ipu3/ipu_ic.c b/drivers/mxc/ipu3/ipu_ic.c
new file mode 100644
index 000000000000..1a4bda3f071a
--- /dev/null
+++ b/drivers/mxc/ipu3/ipu_ic.c
@@ -0,0 +1,826 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * @file ipu_ic.c
+ *
+ * @brief IPU IC functions
+ *
+ * @ingroup IPU
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/videodev2.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+
+#include "ipu_prv.h"
+#include "ipu_regs.h"
+#include "ipu_param_mem.h"
+
+enum {
+	IC_TASK_VIEWFINDER,
+	IC_TASK_ENCODER,
+	IC_TASK_POST_PROCESSOR
+};
+
+static void _init_csc(uint8_t ic_task, ipu_color_space_t in_format,
+		      ipu_color_space_t out_format, int csc_index);
+static bool _calc_resize_coeffs(uint32_t inSize, uint32_t outSize,
+				uint32_t *resizeCoeff,
+				uint32_t *downsizeCoeff);
+
+void _ipu_vdi_set_top_field_man(bool top_field_0)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(VDI_C);
+	if (top_field_0)
+		reg &= ~VDI_C_TOP_FIELD_MAN_1;
+	else
+		reg |= VDI_C_TOP_FIELD_MAN_1;
+	__raw_writel(reg, VDI_C);
+}
+
+void _ipu_vdi_set_motion(ipu_motion_sel motion_sel)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(VDI_C);
+	reg &= ~(VDI_C_MOT_SEL_FULL | VDI_C_MOT_SEL_MED | VDI_C_MOT_SEL_LOW);
+	if (motion_sel == HIGH_MOTION)
+		reg |= VDI_C_MOT_SEL_FULL;
+	else if (motion_sel == MED_MOTION)
+		reg |= VDI_C_MOT_SEL_MED;
+	else
+		reg |= VDI_C_MOT_SEL_LOW;
+
+	__raw_writel(reg, VDI_C);
+}
+
+void ic_dump_register(void)
+{
+	printk(KERN_DEBUG "IC_CONF = \t0x%08X\n", __raw_readl(IC_CONF));
+	printk(KERN_DEBUG "IC_PRP_ENC_RSC = \t0x%08X\n",
+	       __raw_readl(IC_PRP_ENC_RSC));
+	printk(KERN_DEBUG "IC_PRP_VF_RSC = \t0x%08X\n",
+	       __raw_readl(IC_PRP_VF_RSC));
+	printk(KERN_DEBUG "IC_PP_RSC = \t0x%08X\n", __raw_readl(IC_PP_RSC));
+	printk(KERN_DEBUG "IC_IDMAC_1 = \t0x%08X\n", __raw_readl(IC_IDMAC_1));
+	printk(KERN_DEBUG "IC_IDMAC_2 = \t0x%08X\n", __raw_readl(IC_IDMAC_2));
+	printk(KERN_DEBUG "IC_IDMAC_3 = \t0x%08X\n", __raw_readl(IC_IDMAC_3));
+}
+
+void _ipu_ic_enable_task(ipu_channel_t channel)
+{
+	uint32_t ic_conf;
+
+	ic_conf = __raw_readl(IC_CONF);
+	switch (channel) {
+	case CSI_PRP_VF_MEM:
+	case MEM_PRP_VF_MEM:
+		ic_conf |= IC_CONF_PRPVF_EN;
+		break;
+	case MEM_VDI_PRP_VF_MEM:
+		ic_conf |= IC_CONF_PRPVF_EN;
+		break;
+	case MEM_ROT_VF_MEM:
+		ic_conf |= IC_CONF_PRPVF_ROT_EN;
+		break;
+	case CSI_PRP_ENC_MEM:
+	case MEM_PRP_ENC_MEM:
+		ic_conf |= IC_CONF_PRPENC_EN;
+		break;
+	case MEM_ROT_ENC_MEM:
+		ic_conf |= IC_CONF_PRPENC_ROT_EN;
+		break;
+	case MEM_PP_MEM:
+		ic_conf |= IC_CONF_PP_EN;
+		break;
+	case MEM_ROT_PP_MEM:
+		ic_conf |= IC_CONF_PP_ROT_EN;
+		break;
+	default:
+		break;
+	}
+	__raw_writel(ic_conf, IC_CONF);
+}
+
+void _ipu_ic_disable_task(ipu_channel_t channel)
+{
+	uint32_t ic_conf;
+
+	ic_conf = __raw_readl(IC_CONF);
+	switch (channel) {
+	case CSI_PRP_VF_MEM:
+	case MEM_PRP_VF_MEM:
+		ic_conf &= ~IC_CONF_PRPVF_EN;
+		break;
+	case MEM_VDI_PRP_VF_MEM:
+		ic_conf &= ~IC_CONF_PRPVF_EN;
+		break;
+	case MEM_ROT_VF_MEM:
+		ic_conf &= ~IC_CONF_PRPVF_ROT_EN;
+		break;
+	case CSI_PRP_ENC_MEM:
+	case MEM_PRP_ENC_MEM:
+		ic_conf &= ~IC_CONF_PRPENC_EN;
+		break;
+	case MEM_ROT_ENC_MEM:
+		ic_conf &= ~IC_CONF_PRPENC_ROT_EN;
+		break;
+	case MEM_PP_MEM:
+		ic_conf &= ~IC_CONF_PP_EN;
+		break;
+	case MEM_ROT_PP_MEM:
+		ic_conf &= ~IC_CONF_PP_ROT_EN;
+		break;
+	default:
+		break;
+	}
+	__raw_writel(ic_conf, IC_CONF);
+}
+
+void _ipu_vdi_init(ipu_channel_t channel, ipu_channel_params_t *params)
+{
+	uint32_t reg;
+	uint32_t pixel_fmt;
+	bool top_field_0;
+
+	reg = ((params->mem_prp_vf_mem.in_height-1) << 16) |
+	  (params->mem_prp_vf_mem.in_width-1);
+	__raw_writel(reg, VDI_FSIZE);
+
+	/* Full motion, only vertical filter is used
+	   Burst size is 4 accesses */
+	pixel_fmt =
+	    (params->mem_prp_vf_mem.in_pixel_fmt ==
+	     V4L2_PIX_FMT_YUV422P) ? VDI_C_CH_422 : VDI_C_CH_420;
+
+	reg = __raw_readl(VDI_C);
+	reg |= pixel_fmt;
+	switch (channel) {
+	case MEM_VDI_PRP_VF_MEM:
+		reg |= VDI_C_BURST_SIZE2_4;
+		break;
+	case MEM_VDI_PRP_VF_MEM_P:
+		reg |= VDI_C_BURST_SIZE1_4 | VDI_C_VWM1_SET_1 | VDI_C_VWM1_CLR_2;
+		break;
+	case MEM_VDI_PRP_VF_MEM_N:
+		reg |= VDI_C_BURST_SIZE3_4 | VDI_C_VWM3_SET_1 | VDI_C_VWM3_CLR_2;
+		break;
+	default:
+		break;
+	}
+	__raw_writel(reg, VDI_C);
+
+	/* MED_MOTION and LOW_MOTION algorithm that are using 3 fields
+	 * should start presenting using the 2nd field.
+	 */
+	if (((params->mem_prp_vf_mem.field_fmt == V4L2_FIELD_INTERLACED_TB) &&
+	     (params->mem_prp_vf_mem.motion_sel != HIGH_MOTION)) ||
+	    ((params->mem_prp_vf_mem.field_fmt == V4L2_FIELD_INTERLACED_BT) &&
+	     (params->mem_prp_vf_mem.motion_sel == HIGH_MOTION)))
+		top_field_0 = false;
+	else
+		top_field_0 = true;
+
+	/* Buffer selection toggle the value therefore init val is inverted. */
+	_ipu_vdi_set_top_field_man(!top_field_0);
+
+	_ipu_vdi_set_motion(params->mem_prp_vf_mem.motion_sel);
+
+	reg = __raw_readl(IC_CONF);
+	reg &= ~IC_CONF_RWS_EN;
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_vdi_uninit(void)
+{
+	__raw_writel(0, VDI_FSIZE);
+	__raw_writel(0, VDI_C);
+}
+
+void _ipu_ic_init_prpvf(ipu_channel_params_t *params, bool src_is_csi)
+{
+	uint32_t reg, ic_conf;
+	uint32_t downsizeCoeff, resizeCoeff;
+	ipu_color_space_t in_fmt, out_fmt;
+
+	/* Setup vertical resizing */
+	_calc_resize_coeffs(params->mem_prp_vf_mem.in_height,
+			    params->mem_prp_vf_mem.out_height,
+			    &resizeCoeff, &downsizeCoeff);
+	reg = (downsizeCoeff << 30) | (resizeCoeff << 16);
+
+	/* Setup horizontal resizing */
+	_calc_resize_coeffs(params->mem_prp_vf_mem.in_width,
+			    params->mem_prp_vf_mem.out_width,
+			    &resizeCoeff, &downsizeCoeff);
+	reg |= (downsizeCoeff << 14) | resizeCoeff;
+
+	__raw_writel(reg, IC_PRP_VF_RSC);
+
+	ic_conf = __raw_readl(IC_CONF);
+
+	/* Setup color space conversion */
+	in_fmt = format_to_colorspace(params->mem_prp_vf_mem.in_pixel_fmt);
+	out_fmt = format_to_colorspace(params->mem_prp_vf_mem.out_pixel_fmt);
+	if (in_fmt == RGB) {
+		if ((out_fmt == YCbCr) || (out_fmt == YUV)) {
+			/* Enable RGB->YCBCR CSC1 */
+			_init_csc(IC_TASK_VIEWFINDER, RGB, out_fmt, 1);
+			ic_conf |= IC_CONF_PRPVF_CSC1;
+		}
+	}
+	if ((in_fmt == YCbCr) || (in_fmt == YUV)) {
+		if (out_fmt == RGB) {
+			/* Enable YCBCR->RGB CSC1 */
+			_init_csc(IC_TASK_VIEWFINDER, YCbCr, RGB, 1);
+			ic_conf |= IC_CONF_PRPVF_CSC1;
+		} else {
+			/* TODO: Support YUV<->YCbCr conversion? */
+		}
+	}
+
+	if (params->mem_prp_vf_mem.graphics_combine_en) {
+		ic_conf |= IC_CONF_PRPVF_CMB;
+
+		if (!(ic_conf & IC_CONF_PRPVF_CSC1)) {
+			/* need transparent CSC1 conversion */
+			_init_csc(IC_TASK_VIEWFINDER, RGB, RGB, 1);
+			ic_conf |= IC_CONF_PRPVF_CSC1;  /* Enable RGB->RGB CSC */
+		}
+		in_fmt = format_to_colorspace(params->mem_prp_vf_mem.in_g_pixel_fmt);
+		out_fmt = format_to_colorspace(params->mem_prp_vf_mem.out_pixel_fmt);
+		if (in_fmt == RGB) {
+			if ((out_fmt == YCbCr) || (out_fmt == YUV)) {
+				/* Enable RGB->YCBCR CSC2 */
+				_init_csc(IC_TASK_VIEWFINDER, RGB, out_fmt, 2);
+				ic_conf |= IC_CONF_PRPVF_CSC2;
+			}
+		}
+		if ((in_fmt == YCbCr) || (in_fmt == YUV)) {
+			if (out_fmt == RGB) {
+				/* Enable YCBCR->RGB CSC2 */
+				_init_csc(IC_TASK_VIEWFINDER, YCbCr, RGB, 2);
+				ic_conf |= IC_CONF_PRPVF_CSC2;
+			} else {
+				/* TODO: Support YUV<->YCbCr conversion? */
+			}
+		}
+
+		if (params->mem_prp_vf_mem.global_alpha_en) {
+			ic_conf |= IC_CONF_IC_GLB_LOC_A;
+			reg = __raw_readl(IC_CMBP_1);
+			reg &= ~(0xff);
+			reg |= params->mem_prp_vf_mem.alpha;
+			__raw_writel(reg, IC_CMBP_1);
+		} else
+			ic_conf &= ~IC_CONF_IC_GLB_LOC_A;
+
+		if (params->mem_prp_vf_mem.key_color_en) {
+			ic_conf |= IC_CONF_KEY_COLOR_EN;
+			__raw_writel(params->mem_prp_vf_mem.key_color,
+					IC_CMBP_2);
+		} else
+			ic_conf &= ~IC_CONF_KEY_COLOR_EN;
+	} else {
+		ic_conf &= ~IC_CONF_PRPVF_CMB;
+	}
+
+	if (src_is_csi)
+		ic_conf &= ~IC_CONF_RWS_EN;
+	else
+		ic_conf |= IC_CONF_RWS_EN;
+
+	__raw_writel(ic_conf, IC_CONF);
+}
+
+void _ipu_ic_uninit_prpvf(void)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(IC_CONF);
+	reg &= ~(IC_CONF_PRPVF_EN | IC_CONF_PRPVF_CMB |
+		 IC_CONF_PRPVF_CSC2 | IC_CONF_PRPVF_CSC1);
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_rotate_vf(ipu_channel_params_t *params)
+{
+}
+
+void _ipu_ic_uninit_rotate_vf(void)
+{
+	uint32_t reg;
+	reg = __raw_readl(IC_CONF);
+	reg &= ~IC_CONF_PRPVF_ROT_EN;
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_prpenc(ipu_channel_params_t *params, bool src_is_csi)
+{
+	uint32_t reg, ic_conf;
+	uint32_t downsizeCoeff, resizeCoeff;
+	ipu_color_space_t in_fmt, out_fmt;
+
+	/* Setup vertical resizing */
+	_calc_resize_coeffs(params->mem_prp_enc_mem.in_height,
+			    params->mem_prp_enc_mem.out_height,
+			    &resizeCoeff, &downsizeCoeff);
+	reg = (downsizeCoeff << 30) | (resizeCoeff << 16);
+
+	/* Setup horizontal resizing */
+	_calc_resize_coeffs(params->mem_prp_enc_mem.in_width,
+			    params->mem_prp_enc_mem.out_width,
+			    &resizeCoeff, &downsizeCoeff);
+	reg |= (downsizeCoeff << 14) | resizeCoeff;
+
+	__raw_writel(reg, IC_PRP_ENC_RSC);
+
+	ic_conf = __raw_readl(IC_CONF);
+
+	/* Setup color space conversion */
+	in_fmt = format_to_colorspace(params->mem_prp_enc_mem.in_pixel_fmt);
+	out_fmt = format_to_colorspace(params->mem_prp_enc_mem.out_pixel_fmt);
+	if (in_fmt == RGB) {
+		if ((out_fmt == YCbCr) || (out_fmt == YUV)) {
+			/* Enable RGB->YCBCR CSC1 */
+			_init_csc(IC_TASK_ENCODER, RGB, out_fmt, 1);
+			ic_conf |= IC_CONF_PRPENC_CSC1;
+		}
+	}
+	if ((in_fmt == YCbCr) || (in_fmt == YUV)) {
+		if (out_fmt == RGB) {
+			/* Enable YCBCR->RGB CSC1 */
+			_init_csc(IC_TASK_ENCODER, YCbCr, RGB, 1);
+			ic_conf |= IC_CONF_PRPENC_CSC1;
+		} else {
+			/* TODO: Support YUV<->YCbCr conversion? */
+		}
+	}
+
+	if (src_is_csi)
+		ic_conf &= ~IC_CONF_RWS_EN;
+	else
+		ic_conf |= IC_CONF_RWS_EN;
+
+	__raw_writel(ic_conf, IC_CONF);
+}
+
+void _ipu_ic_uninit_prpenc(void)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(IC_CONF);
+	reg &= ~(IC_CONF_PRPENC_EN | IC_CONF_PRPENC_CSC1);
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_rotate_enc(ipu_channel_params_t *params)
+{
+}
+
+void _ipu_ic_uninit_rotate_enc(void)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(IC_CONF);
+	reg &= ~(IC_CONF_PRPENC_ROT_EN);
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_pp(ipu_channel_params_t *params)
+{
+	uint32_t reg, ic_conf;
+	uint32_t downsizeCoeff, resizeCoeff;
+	ipu_color_space_t in_fmt, out_fmt;
+
+	/* Setup vertical resizing */
+	_calc_resize_coeffs(params->mem_pp_mem.in_height,
+			    params->mem_pp_mem.out_height,
+			    &resizeCoeff, &downsizeCoeff);
+	reg = (downsizeCoeff << 30) | (resizeCoeff << 16);
+
+	/* Setup horizontal resizing */
+   /* Upadeted for IC split case */
+	if (!(params->mem_pp_mem.out_resize_ratio)) {
+		_calc_resize_coeffs(params->mem_pp_mem.in_width,
+							params->mem_pp_mem.out_width,
+							&resizeCoeff, &downsizeCoeff);
+		reg |= (downsizeCoeff << 14) | resizeCoeff;
+	} else
+		reg |= params->mem_pp_mem.out_resize_ratio;
+
+	__raw_writel(reg, IC_PP_RSC);
+
+	ic_conf = __raw_readl(IC_CONF);
+
+	/* Setup color space conversion */
+	in_fmt = format_to_colorspace(params->mem_pp_mem.in_pixel_fmt);
+	out_fmt = format_to_colorspace(params->mem_pp_mem.out_pixel_fmt);
+	if (in_fmt == RGB) {
+		if ((out_fmt == YCbCr) || (out_fmt == YUV)) {
+			/* Enable RGB->YCBCR CSC1 */
+			_init_csc(IC_TASK_POST_PROCESSOR, RGB, out_fmt, 1);
+			ic_conf |= IC_CONF_PP_CSC1;
+		}
+	}
+	if ((in_fmt == YCbCr) || (in_fmt == YUV)) {
+		if (out_fmt == RGB) {
+			/* Enable YCBCR->RGB CSC1 */
+			_init_csc(IC_TASK_POST_PROCESSOR, YCbCr, RGB, 1);
+			ic_conf |= IC_CONF_PP_CSC1;
+		} else {
+			/* TODO: Support YUV<->YCbCr conversion? */
+		}
+	}
+
+	if (params->mem_pp_mem.graphics_combine_en) {
+		ic_conf |= IC_CONF_PP_CMB;
+
+		if (!(ic_conf & IC_CONF_PP_CSC1)) {
+			/* need transparent CSC1 conversion */
+			_init_csc(IC_TASK_POST_PROCESSOR, RGB, RGB, 1);
+			ic_conf |= IC_CONF_PP_CSC1;  /* Enable RGB->RGB CSC */
+		}
+
+		in_fmt = format_to_colorspace(params->mem_pp_mem.in_g_pixel_fmt);
+		out_fmt = format_to_colorspace(params->mem_pp_mem.out_pixel_fmt);
+		if (in_fmt == RGB) {
+			if ((out_fmt == YCbCr) || (out_fmt == YUV)) {
+				/* Enable RGB->YCBCR CSC2 */
+				_init_csc(IC_TASK_POST_PROCESSOR, RGB, out_fmt, 2);
+				ic_conf |= IC_CONF_PP_CSC2;
+			}
+		}
+		if ((in_fmt == YCbCr) || (in_fmt == YUV)) {
+			if (out_fmt == RGB) {
+				/* Enable YCBCR->RGB CSC2 */
+				_init_csc(IC_TASK_POST_PROCESSOR, YCbCr, RGB, 2);
+				ic_conf |= IC_CONF_PP_CSC2;
+			} else {
+				/* TODO: Support YUV<->YCbCr conversion? */
+			}
+		}
+
+		if (params->mem_pp_mem.global_alpha_en) {
+			ic_conf |= IC_CONF_IC_GLB_LOC_A;
+			reg = __raw_readl(IC_CMBP_1);
+			reg &= ~(0xff00);
+			reg |= (params->mem_pp_mem.alpha << 8);
+			__raw_writel(reg, IC_CMBP_1);
+		} else
+			ic_conf &= ~IC_CONF_IC_GLB_LOC_A;
+
+		if (params->mem_pp_mem.key_color_en) {
+			ic_conf |= IC_CONF_KEY_COLOR_EN;
+			__raw_writel(params->mem_pp_mem.key_color,
+					IC_CMBP_2);
+		} else
+			ic_conf &= ~IC_CONF_KEY_COLOR_EN;
+	} else {
+		ic_conf &= ~IC_CONF_PP_CMB;
+	}
+
+	__raw_writel(ic_conf, IC_CONF);
+}
+
+void _ipu_ic_uninit_pp(void)
+{
+	uint32_t reg;
+
+	reg = __raw_readl(IC_CONF);
+	reg &= ~(IC_CONF_PP_EN | IC_CONF_PP_CSC1 | IC_CONF_PP_CSC2 |
+		 IC_CONF_PP_CMB);
+	__raw_writel(reg, IC_CONF);
+}
+
+void _ipu_ic_init_rotate_pp(ipu_channel_params_t *params)
+{
+}
+
+void _ipu_ic_uninit_rotate_pp(void)
+{
+	uint32_t reg;
+	reg = __raw_readl(IC_CONF);
+	reg &= ~IC_CONF_PP_ROT_EN;
+	__raw_writel(reg, IC_CONF);
+}
+
+int _ipu_ic_idma_init(int dma_chan, uint16_t width, uint16_t height,
+		      int burst_size, ipu_rotate_mode_t rot)
+{
+	u32 ic_idmac_1, ic_idmac_2, ic_idmac_3;
+	u32 temp_rot = bitrev8(rot) >> 5;
+	bool need_hor_flip = false;
+
+	if ((burst_size != 8) && (burst_size != 16)) {
+		dev_dbg(g_ipu_dev, "Illegal burst length for IC\n");
+		return -EINVAL;
+	}
+
+	width--;
+	height--;
+
+	if (temp_rot & 0x2)	/* Need horizontal flip */
+		need_hor_flip = true;
+
+	ic_idmac_1 = __raw_readl(IC_IDMAC_1);
+	ic_idmac_2 = __raw_readl(IC_IDMAC_2);
+	ic_idmac_3 = __raw_readl(IC_IDMAC_3);
+	if (dma_chan == 22) {	/* PP output - CB2 */
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB2_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB2_BURST_16;
+
+		if (need_hor_flip)
+			ic_idmac_1 |= IC_IDMAC_1_PP_FLIP_RS;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_PP_FLIP_RS;
+
+		ic_idmac_2 &= ~IC_IDMAC_2_PP_HEIGHT_MASK;
+		ic_idmac_2 |= height << IC_IDMAC_2_PP_HEIGHT_OFFSET;
+
+		ic_idmac_3 &= ~IC_IDMAC_3_PP_WIDTH_MASK;
+		ic_idmac_3 |= width << IC_IDMAC_3_PP_WIDTH_OFFSET;
+
+	} else if (dma_chan == 11) {	/* PP Input - CB5 */
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB5_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB5_BURST_16;
+	} else if (dma_chan == 47) {	/* PP Rot input */
+		ic_idmac_1 &= ~IC_IDMAC_1_PP_ROT_MASK;
+		ic_idmac_1 |= temp_rot << IC_IDMAC_1_PP_ROT_OFFSET;
+	}
+
+	if (dma_chan == 12) {	/* PRP Input - CB6 */
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB6_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB6_BURST_16;
+	}
+
+	if (dma_chan == 20) {	/* PRP ENC output - CB0 */
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB0_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB0_BURST_16;
+
+		if (need_hor_flip)
+			ic_idmac_1 |= IC_IDMAC_1_PRPENC_FLIP_RS;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_PRPENC_FLIP_RS;
+
+		ic_idmac_2 &= ~IC_IDMAC_2_PRPENC_HEIGHT_MASK;
+		ic_idmac_2 |= height << IC_IDMAC_2_PRPENC_HEIGHT_OFFSET;
+
+		ic_idmac_3 &= ~IC_IDMAC_3_PRPENC_WIDTH_MASK;
+		ic_idmac_3 |= width << IC_IDMAC_3_PRPENC_WIDTH_OFFSET;
+
+	} else if (dma_chan == 45) {	/* PRP ENC Rot input */
+		ic_idmac_1 &= ~IC_IDMAC_1_PRPENC_ROT_MASK;
+		ic_idmac_1 |= temp_rot << IC_IDMAC_1_PRPENC_ROT_OFFSET;
+	}
+
+	if (dma_chan == 21) {	/* PRP VF output - CB1 */
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB1_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB1_BURST_16;
+
+		if (need_hor_flip)
+			ic_idmac_1 |= IC_IDMAC_1_PRPVF_FLIP_RS;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_PRPVF_FLIP_RS;
+
+		ic_idmac_2 &= ~IC_IDMAC_2_PRPVF_HEIGHT_MASK;
+		ic_idmac_2 |= height << IC_IDMAC_2_PRPVF_HEIGHT_OFFSET;
+
+		ic_idmac_3 &= ~IC_IDMAC_3_PRPVF_WIDTH_MASK;
+		ic_idmac_3 |= width << IC_IDMAC_3_PRPVF_WIDTH_OFFSET;
+
+	} else if (dma_chan == 46) {	/* PRP VF Rot input */
+		ic_idmac_1 &= ~IC_IDMAC_1_PRPVF_ROT_MASK;
+		ic_idmac_1 |= temp_rot << IC_IDMAC_1_PRPVF_ROT_OFFSET;
+	}
+
+	if (dma_chan == 14) {	/* PRP VF graphics combining input - CB3 */
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB3_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB3_BURST_16;
+	} else if (dma_chan == 15) {	/* PP graphics combining input - CB4 */
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB4_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB4_BURST_16;
+	}
+
+	__raw_writel(ic_idmac_1, IC_IDMAC_1);
+	__raw_writel(ic_idmac_2, IC_IDMAC_2);
+	__raw_writel(ic_idmac_3, IC_IDMAC_3);
+
+	return 0;
+}
+
+static void _init_csc(uint8_t ic_task, ipu_color_space_t in_format,
+		      ipu_color_space_t out_format, int csc_index)
+{
+
+/*     Y = R *  .299 + G *  .587 + B *  .114;
+       U = R * -.169 + G * -.332 + B *  .500 + 128.;
+       V = R *  .500 + G * -.419 + B * -.0813 + 128.;*/
+	static const uint32_t rgb2ycbcr_coeff[4][3] = {
+		{0x004D, 0x0096, 0x001D},
+		{0x01D5, 0x01AB, 0x0080},
+		{0x0080, 0x0195, 0x01EB},
+		{0x0000, 0x0200, 0x0200},	/* A0, A1, A2 */
+	};
+
+	/* transparent RGB->RGB matrix for combining
+	 */
+	static const uint32_t rgb2rgb_coeff[4][3] = {
+		{0x0080, 0x0000, 0x0000},
+		{0x0000, 0x0080, 0x0000},
+		{0x0000, 0x0000, 0x0080},
+		{0x0000, 0x0000, 0x0000},	/* A0, A1, A2 */
+	};
+
+/*     R = (1.164 * (Y - 16)) + (1.596 * (Cr - 128));
+       G = (1.164 * (Y - 16)) - (0.392 * (Cb - 128)) - (0.813 * (Cr - 128));
+       B = (1.164 * (Y - 16)) + (2.017 * (Cb - 128); */
+	static const uint32_t ycbcr2rgb_coeff[4][3] = {
+		{149, 0, 204},
+		{149, 462, 408},
+		{149, 255, 0},
+		{8192 - 446, 266, 8192 - 554},	/* A0, A1, A2 */
+	};
+
+	uint32_t param;
+	uint32_t *base = NULL;
+
+	if (ic_task == IC_TASK_ENCODER) {
+		base = ipu_tpmem_base + 0x2008 / 4;
+	} else if (ic_task == IC_TASK_VIEWFINDER) {
+		if (csc_index == 1)
+			base = ipu_tpmem_base + 0x4028 / 4;
+		else
+			base = ipu_tpmem_base + 0x4040 / 4;
+	} else if (ic_task == IC_TASK_POST_PROCESSOR) {
+		if (csc_index == 1)
+			base = ipu_tpmem_base + 0x6060 / 4;
+		else
+			base = ipu_tpmem_base + 0x6078 / 4;
+	} else {
+		BUG();
+	}
+
+	if ((in_format == YCbCr) && (out_format == RGB)) {
+		/* Init CSC (YCbCr->RGB) */
+		param = (ycbcr2rgb_coeff[3][0] << 27) |
+			(ycbcr2rgb_coeff[0][0] << 18) |
+			(ycbcr2rgb_coeff[1][1] << 9) | ycbcr2rgb_coeff[2][2];
+		__raw_writel(param, base++);
+		/* scale = 2, sat = 0 */
+		param = (ycbcr2rgb_coeff[3][0] >> 5) | (2L << (40 - 32));
+		__raw_writel(param, base++);
+
+		param = (ycbcr2rgb_coeff[3][1] << 27) |
+			(ycbcr2rgb_coeff[0][1] << 18) |
+			(ycbcr2rgb_coeff[1][0] << 9) | ycbcr2rgb_coeff[2][0];
+		__raw_writel(param, base++);
+		param = (ycbcr2rgb_coeff[3][1] >> 5);
+		__raw_writel(param, base++);
+
+		param = (ycbcr2rgb_coeff[3][2] << 27) |
+			(ycbcr2rgb_coeff[0][2] << 18) |
+			(ycbcr2rgb_coeff[1][2] << 9) | ycbcr2rgb_coeff[2][1];
+		__raw_writel(param, base++);
+		param = (ycbcr2rgb_coeff[3][2] >> 5);
+		__raw_writel(param, base++);
+	} else if ((in_format == RGB) && (out_format == YCbCr)) {
+		/* Init CSC (RGB->YCbCr) */
+		param = (rgb2ycbcr_coeff[3][0] << 27) |
+			(rgb2ycbcr_coeff[0][0] << 18) |
+			(rgb2ycbcr_coeff[1][1] << 9) | rgb2ycbcr_coeff[2][2];
+		__raw_writel(param, base++);
+		/* scale = 1, sat = 0 */
+		param = (rgb2ycbcr_coeff[3][0] >> 5) | (1UL << 8);
+		__raw_writel(param, base++);
+
+		param = (rgb2ycbcr_coeff[3][1] << 27) |
+			(rgb2ycbcr_coeff[0][1] << 18) |
+			(rgb2ycbcr_coeff[1][0] << 9) | rgb2ycbcr_coeff[2][0];
+		__raw_writel(param, base++);
+		param = (rgb2ycbcr_coeff[3][1] >> 5);
+		__raw_writel(param, base++);
+
+		param = (rgb2ycbcr_coeff[3][2] << 27) |
+			(rgb2ycbcr_coeff[0][2] << 18) |
+			(rgb2ycbcr_coeff[1][2] << 9) | rgb2ycbcr_coeff[2][1];
+		__raw_writel(param, base++);
+		param = (rgb2ycbcr_coeff[3][2] >> 5);
+		__raw_writel(param, base++);
+	} else if ((in_format == RGB) && (out_format == RGB)) {
+		/* Init CSC */
+		param =
+		    (rgb2rgb_coeff[3][0] << 27) | (rgb2rgb_coeff[0][0] << 18) |
+		    (rgb2rgb_coeff[1][1] << 9) | rgb2rgb_coeff[2][2];
+		__raw_writel(param, base++);
+		/* scale = 2, sat = 0 */
+		param = (rgb2rgb_coeff[3][0] >> 5) | (2UL << 8);
+		__raw_writel(param, base++);
+
+		param =
+		    (rgb2rgb_coeff[3][1] << 27) | (rgb2rgb_coeff[0][1] << 18) |
+		    (rgb2rgb_coeff[1][0] << 9) | rgb2rgb_coeff[2][0];
+		__raw_writel(param, base++);
+		param = (rgb2rgb_coeff[3][1] >> 5);
+		__raw_writel(param, base++);
+
+		param =
+		    (rgb2rgb_coeff[3][2] << 27) | (rgb2rgb_coeff[0][2] << 18) |
+		    (rgb2rgb_coeff[1][2] << 9) | rgb2rgb_coeff[2][1];
+		__raw_writel(param, base++);
+		param = (rgb2rgb_coeff[3][2] >> 5);
+		__raw_writel(param, base++);
+	} else {
+		dev_err(g_ipu_dev, "Unsupported color space conversion\n");
+	}
+}
+
+static bool _calc_resize_coeffs(uint32_t inSize, uint32_t outSize,
+				uint32_t *resizeCoeff,
+				uint32_t *downsizeCoeff)
+{
+	uint32_t tempSize;
+	uint32_t tempDownsize;
+
+	/* Input size cannot be more than 4096 */
+	/* Output size cannot be more than 1024 */
+	if ((inSize > 4096) || (outSize > 1024))
+		return false;
+
+	/* Cannot downsize more than 8:1 */
+	if ((outSize << 3) < inSize)
+		return false;
+
+	/* Compute downsizing coefficient */
+	/* Output of downsizing unit cannot be more than 1024 */
+	tempDownsize = 0;
+	tempSize = inSize;
+	while (((tempSize > 1024) || (tempSize >= outSize * 2)) &&
+	       (tempDownsize < 2)) {
+		tempSize >>= 1;
+		tempDownsize++;
+	}
+	*downsizeCoeff = tempDownsize;
+
+	/* compute resizing coefficient using the following equation:
+	   resizeCoeff = M*(SI -1)/(SO - 1)
+	   where M = 2^13, SI - input size, SO - output size    */
+	*resizeCoeff = (8192L * (tempSize - 1)) / (outSize - 1);
+	if (*resizeCoeff >= 16384L) {
+		dev_err(g_ipu_dev, "Warning! Overflow on resize coeff.\n");
+		*resizeCoeff = 0x3FFF;
+	}
+
+	dev_dbg(g_ipu_dev, "resizing from %u -> %u pixels, "
+		"downsize=%u, resize=%u.%lu (reg=%u)\n", inSize, outSize,
+		*downsizeCoeff, (*resizeCoeff >= 8192L) ? 1 : 0,
+		((*resizeCoeff & 0x1FFF) * 10000L) / 8192L, *resizeCoeff);
+
+	return true;
+}
+
+void _ipu_vdi_toggle_top_field_man()
+{
+	uint32_t reg;
+	uint32_t mask_reg;
+
+	reg = __raw_readl(VDI_C);
+	mask_reg = reg & VDI_C_TOP_FIELD_MAN_1;
+	if (mask_reg == VDI_C_TOP_FIELD_MAN_1)
+		reg &= ~VDI_C_TOP_FIELD_MAN_1;
+	else
+		reg |= VDI_C_TOP_FIELD_MAN_1;
+
+	__raw_writel(reg, VDI_C);
+}
+
diff --git a/drivers/mxc/ipu3/ipu_param_mem.h b/drivers/mxc/ipu3/ipu_param_mem.h
new file mode 100644
index 000000000000..dd30cb38c207
--- /dev/null
+++ b/drivers/mxc/ipu3/ipu_param_mem.h
@@ -0,0 +1,494 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __INCLUDE_IPU_PARAM_MEM_H__
+#define __INCLUDE_IPU_PARAM_MEM_H__
+
+#include <linux/types.h>
+#include <linux/bitrev.h>
+
+extern u32 *ipu_cpmem_base;
+
+struct ipu_ch_param_word {
+	uint32_t data[5];
+	uint32_t res[3];
+};
+
+struct ipu_ch_param {
+	struct ipu_ch_param_word word[2];
+};
+
+#define ipu_ch_param_addr(ch) (((struct ipu_ch_param *)ipu_cpmem_base) + (ch))
+
+#define _param_word(base, w) \
+	(((struct ipu_ch_param *)(base))->word[(w)].data)
+
+#define ipu_ch_param_set_field(base, w, bit, size, v) { \
+	int i = (bit) / 32; \
+	int off = (bit) % 32; \
+	_param_word(base, w)[i] |= (v) << off; \
+	if (((bit)+(size)-1)/32 > i) { \
+		_param_word(base, w)[i + 1] |= (v) >> (off ? (32 - off) : 0); \
+	} \
+}
+
+#define ipu_ch_param_mod_field(base, w, bit, size, v) { \
+	int i = (bit) / 32; \
+	int off = (bit) % 32; \
+	u32 mask = (1UL << size) - 1; \
+	u32 temp = _param_word(base, w)[i]; \
+	temp &= ~(mask << off); \
+	_param_word(base, w)[i] = temp | (v) << off; \
+	if (((bit)+(size)-1)/32 > i) { \
+		temp = _param_word(base, w)[i + 1]; \
+		temp &= ~(mask >> (32 - off)); \
+		_param_word(base, w)[i + 1] = \
+			temp | ((v) >> (off ? (32 - off) : 0)); \
+	} \
+}
+
+#define ipu_ch_param_read_field(base, w, bit, size) ({ \
+	u32 temp2; \
+	int i = (bit) / 32; \
+	int off = (bit) % 32; \
+	u32 mask = (1UL << size) - 1; \
+	u32 temp1 = _param_word(base, w)[i]; \
+	temp1 = mask & (temp1 >> off); \
+	if (((bit)+(size)-1)/32 > i) { \
+		temp2 = _param_word(base, w)[i + 1]; \
+		temp2 &= mask >> (off ? (32 - off) : 0); \
+		temp1 |= temp2 << (off ? (32 - off) : 0); \
+	} \
+	temp1; \
+})
+
+static inline void _ipu_ch_params_set_packing(struct ipu_ch_param *p,
+					      int red_width, int red_offset,
+					      int green_width, int green_offset,
+					      int blue_width, int blue_offset,
+					      int alpha_width, int alpha_offset)
+{
+	/* Setup red width and offset */
+	ipu_ch_param_set_field(p, 1, 116, 3, red_width - 1);
+	ipu_ch_param_set_field(p, 1, 128, 5, red_offset);
+	/* Setup green width and offset */
+	ipu_ch_param_set_field(p, 1, 119, 3, green_width - 1);
+	ipu_ch_param_set_field(p, 1, 133, 5, green_offset);
+	/* Setup blue width and offset */
+	ipu_ch_param_set_field(p, 1, 122, 3, blue_width - 1);
+	ipu_ch_param_set_field(p, 1, 138, 5, blue_offset);
+	/* Setup alpha width and offset */
+	ipu_ch_param_set_field(p, 1, 125, 3, alpha_width - 1);
+	ipu_ch_param_set_field(p, 1, 143, 5, alpha_offset);
+}
+
+static inline void _ipu_ch_param_dump(int ch)
+{
+	struct ipu_ch_param *p = ipu_ch_param_addr(ch);
+	pr_debug("ch %d word 0 - %08X %08X %08X %08X %08X\n", ch,
+		 p->word[0].data[0], p->word[0].data[1], p->word[0].data[2],
+		 p->word[0].data[3], p->word[0].data[4]);
+	pr_debug("ch %d word 1 - %08X %08X %08X %08X %08X\n", ch,
+		 p->word[1].data[0], p->word[1].data[1], p->word[1].data[2],
+		 p->word[1].data[3], p->word[1].data[4]);
+	pr_debug("PFS 0x%x, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 85, 4));
+	pr_debug("BPP 0x%x, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 0, 107, 3));
+	pr_debug("NPB 0x%x\n",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 78, 7));
+
+	pr_debug("FW %d, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 0, 125, 13));
+	pr_debug("FH %d, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 0, 138, 12));
+	pr_debug("Stride %d\n",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 102, 14));
+
+	pr_debug("Width0 %d+1, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 116, 3));
+	pr_debug("Width1 %d+1, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 119, 3));
+	pr_debug("Width2 %d+1, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 122, 3));
+	pr_debug("Width3 %d+1, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 125, 3));
+	pr_debug("Offset0 %d, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 128, 5));
+	pr_debug("Offset1 %d, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 133, 5));
+	pr_debug("Offset2 %d, ",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 138, 5));
+	pr_debug("Offset3 %d\n",
+		 ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 143, 5));
+}
+
+static inline void _ipu_ch_param_init(int ch,
+				      uint32_t pixel_fmt, uint32_t width,
+				      uint32_t height, uint32_t stride,
+				      uint32_t u, uint32_t v,
+				      uint32_t uv_stride, dma_addr_t addr0,
+				      dma_addr_t addr1)
+{
+	uint32_t u_offset = 0;
+	uint32_t v_offset = 0;
+	struct ipu_ch_param params;
+
+	memset(&params, 0, sizeof(params));
+
+	ipu_ch_param_set_field(&params, 0, 125, 13, width - 1);
+
+	if ((ch == 8) || (ch == 9) || (ch == 10)) {
+		ipu_ch_param_set_field(&params, 0, 138, 12, (height / 2) - 1);
+		ipu_ch_param_set_field(&params, 1, 102, 14, (stride * 2) - 1);
+	} else {
+		ipu_ch_param_set_field(&params, 0, 138, 12, height - 1);
+		ipu_ch_param_set_field(&params, 1, 102, 14, stride - 1);
+	}
+
+	ipu_ch_param_set_field(&params, 1, 0, 29, addr0 >> 3);
+	ipu_ch_param_set_field(&params, 1, 29, 29, addr1 >> 3);
+
+	switch (pixel_fmt) {
+	case IPU_PIX_FMT_GENERIC:
+		/*Represents 8-bit Generic data */
+		ipu_ch_param_set_field(&params, 0, 107, 3, 5);	/* bits/pixel */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 6);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 63);	/* burst size */
+
+		break;
+	case IPU_PIX_FMT_GENERIC_32:
+		/*Represents 32-bit Generic data */
+		break;
+	case IPU_PIX_FMT_RGB565:
+		ipu_ch_param_set_field(&params, 0, 107, 3, 3);	/* bits/pixel */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 7);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 15);	/* burst size */
+
+		_ipu_ch_params_set_packing(&params, 5, 0, 6, 5, 5, 11, 8, 16);
+		break;
+	case IPU_PIX_FMT_BGR24:
+		ipu_ch_param_set_field(&params, 0, 107, 3, 1);	/* bits/pixel */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 7);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 19);	/* burst size */
+
+		_ipu_ch_params_set_packing(&params, 8, 0, 8, 8, 8, 16, 8, 24);
+		break;
+	case IPU_PIX_FMT_RGB24:
+	case IPU_PIX_FMT_YUV444:
+		ipu_ch_param_set_field(&params, 0, 107, 3, 1);	/* bits/pixel */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 7);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 19);	/* burst size */
+
+		_ipu_ch_params_set_packing(&params, 8, 16, 8, 8, 8, 0, 8, 24);
+		break;
+	case IPU_PIX_FMT_BGRA32:
+	case IPU_PIX_FMT_BGR32:
+		ipu_ch_param_set_field(&params, 0, 107, 3, 0);	/* bits/pixel */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 7);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 15);	/* burst size */
+
+		_ipu_ch_params_set_packing(&params, 8, 8, 8, 16, 8, 24, 8, 0);
+		break;
+	case IPU_PIX_FMT_RGBA32:
+	case IPU_PIX_FMT_RGB32:
+		ipu_ch_param_set_field(&params, 0, 107, 3, 0);	/* bits/pixel */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 7);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 15);	/* burst size */
+
+		_ipu_ch_params_set_packing(&params, 8, 24, 8, 16, 8, 8, 8, 0);
+		break;
+	case IPU_PIX_FMT_ABGR32:
+		ipu_ch_param_set_field(&params, 0, 107, 3, 0);	/* bits/pixel */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 7);	/* pix format */
+
+		_ipu_ch_params_set_packing(&params, 8, 0, 8, 8, 8, 16, 8, 24);
+		break;
+	case IPU_PIX_FMT_UYVY:
+		ipu_ch_param_set_field(&params, 0, 107, 3, 3);	/* bits/pixel */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 0xA);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 15);	/* burst size */
+		break;
+	case IPU_PIX_FMT_YUYV:
+		ipu_ch_param_set_field(&params, 0, 107, 3, 3);	/* bits/pixel */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 0x8);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 31);	/* burst size */
+		break;
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YUV420P:
+		ipu_ch_param_set_field(&params, 1, 85, 4, 2);	/* pix format */
+
+		if (uv_stride < stride / 2)
+			uv_stride = stride / 2;
+
+		u_offset = stride * height;
+		v_offset = u_offset + (uv_stride * height / 2);
+		if ((ch == 8) || (ch == 9) || (ch == 10)) {
+			ipu_ch_param_set_field(&params, 1, 78, 7, 15);  /* burst size */
+			uv_stride = uv_stride*2;
+		} else {
+			ipu_ch_param_set_field(&params, 1, 78, 7, 31);  /* burst size */
+		}
+		break;
+	case IPU_PIX_FMT_YVU422P:
+		/* BPP & pixel format */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 1);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 31);	/* burst size */
+
+		if (uv_stride < stride / 2)
+			uv_stride = stride / 2;
+
+		v_offset = (v == 0) ? stride * height : v;
+		u_offset = (u == 0) ? v_offset + v_offset / 2 : u;
+		break;
+	case IPU_PIX_FMT_YUV422P:
+		/* BPP & pixel format */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 1);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 31);	/* burst size */
+
+		if (uv_stride < stride / 2)
+			uv_stride = stride / 2;
+
+		u_offset = (u == 0) ? stride * height : u;
+		v_offset = (v == 0) ? u_offset + u_offset / 2 : v;
+		break;
+	case IPU_PIX_FMT_NV12:
+		/* BPP & pixel format */
+		ipu_ch_param_set_field(&params, 1, 85, 4, 4);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 31);	/* burst size */
+		uv_stride = stride;
+		u_offset = (u == 0) ? stride * height : u;
+		break;
+	default:
+		dev_err(g_ipu_dev, "mxc ipu: unimplemented pixel format\n");
+		break;
+	}
+	/*set burst size to 16*/
+
+
+	if (uv_stride)
+		ipu_ch_param_set_field(&params, 1, 128, 14, uv_stride - 1);
+
+	if (u > u_offset)
+		u_offset = u;
+
+	if (v > v_offset)
+		v_offset = v;
+
+	ipu_ch_param_set_field(&params, 0, 46, 22, u_offset / 8);
+	ipu_ch_param_set_field(&params, 0, 68, 22, v_offset / 8);
+
+	pr_debug("initializing idma ch %d @ %p\n", ch, ipu_ch_param_addr(ch));
+	memcpy(ipu_ch_param_addr(ch), &params, sizeof(params));
+};
+
+static inline void _ipu_ch_param_set_burst_size(uint32_t ch,
+						uint16_t burst_pixels)
+{
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 78, 7,
+			       burst_pixels - 1);
+};
+
+static inline int _ipu_ch_param_get_burst_size(uint32_t ch)
+{
+	return ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 78, 7) + 1;
+};
+
+static inline int _ipu_ch_param_get_bpp(uint32_t ch)
+{
+	return ipu_ch_param_read_field(ipu_ch_param_addr(ch), 0, 107, 3);
+};
+
+static inline void _ipu_ch_param_set_buffer(uint32_t ch, int bufNum,
+					    dma_addr_t phyaddr)
+{
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 29 * bufNum, 29,
+			       phyaddr / 8);
+};
+
+static inline void _ipu_ch_param_set_rotation(uint32_t ch,
+					      ipu_rotate_mode_t rot)
+{
+	u32 temp_rot = bitrev8(rot) >> 5;
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 0, 119, 3, temp_rot);
+};
+
+static inline void _ipu_ch_param_set_block_mode(uint32_t ch)
+{
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 0, 117, 2, 1);
+};
+
+static inline void _ipu_ch_param_set_alpha_use_separate_channel(uint32_t ch,
+								bool option)
+{
+	if (option) {
+		ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 89, 1, 1);
+	} else {
+		ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 89, 1, 0);
+	}
+};
+
+static inline void _ipu_ch_param_set_alpha_condition_read(uint32_t ch)
+{
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 149, 1, 1);
+};
+
+static inline void _ipu_ch_param_set_alpha_buffer_memory(uint32_t ch)
+{
+	int alp_mem_idx;
+
+	switch (ch) {
+	case 14: /* PRP graphic */
+		alp_mem_idx = 0;
+		break;
+	case 15: /* PP graphic */
+		alp_mem_idx = 1;
+		break;
+	case 23: /* DP BG SYNC graphic */
+		alp_mem_idx = 4;
+		break;
+	case 27: /* DP FG SYNC graphic */
+		alp_mem_idx = 2;
+		break;
+	default:
+		dev_err(g_ipu_dev, "unsupported correlative channel of local "
+			"alpha channel\n");
+		return;
+	}
+
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 90, 3, alp_mem_idx);
+};
+
+static inline void _ipu_ch_param_set_interlaced_scan(uint32_t ch)
+{
+	u32 stride;
+	ipu_ch_param_set_field(ipu_ch_param_addr(ch), 0, 113, 1, 1);
+	stride = ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 102, 14) + 1;
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 58, 20, stride / 8);
+	stride *= 2;
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 102, 14, stride - 1);
+};
+
+static inline void _ipu_ch_param_set_high_priority(uint32_t ch)
+{
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 93, 2, 1);
+};
+
+/* IDMAC U/V offset changing support */
+/* U and V input is not affected, */
+/* the update is done by new calculation according to */
+/* vertical_offset and horizontal_offset */
+static inline void _ipu_ch_offset_update(int ch,
+					uint32_t pixel_fmt,
+					uint32_t width,
+					uint32_t height,
+					uint32_t stride,
+					uint32_t u,
+					uint32_t v,
+					uint32_t uv_stride,
+					uint32_t vertical_offset,
+					uint32_t horizontal_offset)
+{
+	uint32_t u_offset = 0;
+	uint32_t v_offset = 0;
+	uint32_t u_fix = 0;
+	uint32_t v_fix = 0;
+
+	switch (pixel_fmt) {
+	case IPU_PIX_FMT_GENERIC:
+	case IPU_PIX_FMT_GENERIC_32:
+	case IPU_PIX_FMT_RGB565:
+	case IPU_PIX_FMT_BGR24:
+	case IPU_PIX_FMT_RGB24:
+	case IPU_PIX_FMT_YUV444:
+	case IPU_PIX_FMT_BGRA32:
+	case IPU_PIX_FMT_BGR32:
+	case IPU_PIX_FMT_RGBA32:
+	case IPU_PIX_FMT_RGB32:
+	case IPU_PIX_FMT_ABGR32:
+	case IPU_PIX_FMT_UYVY:
+	case IPU_PIX_FMT_YUYV:
+		break;
+
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YUV420P:
+		if (uv_stride < stride / 2)
+			uv_stride = stride / 2;
+
+		u_fix = u + (uv_stride * vertical_offset / 2) + horizontal_offset / 4;
+		v_fix = v + (uv_stride * vertical_offset / 2) + horizontal_offset / 4;
+		u_offset = (u == 0) ? stride * (height - vertical_offset - 1) +
+								(stride - horizontal_offset) +
+								(uv_stride * vertical_offset / 2) +
+								horizontal_offset / 2 : u_fix;
+		v_offset = (v == 0) ? u_offset + (uv_stride * height / 2) : v_fix;
+
+		break;
+	case IPU_PIX_FMT_YVU422P:
+		if (uv_stride < stride / 2)
+			uv_stride = stride / 2;
+
+		u_fix = u + (uv_stride * vertical_offset) + horizontal_offset / 2;
+		v_fix = v + (uv_stride * vertical_offset) + horizontal_offset / 2;
+
+		v_offset = (v == 0) ? stride * (height - vertical_offset - 1) +
+								(stride - horizontal_offset) +
+								(uv_stride * vertical_offset) +
+								horizontal_offset / 2 :	v_fix;
+		u_offset = (u == 0) ? v_offset + uv_stride * height : u_fix;
+		break;
+	case IPU_PIX_FMT_YUV422P:
+		if (uv_stride < stride / 2)
+			uv_stride = stride / 2;
+
+		u_fix = u + (uv_stride * vertical_offset) + horizontal_offset / 2;
+		v_fix = v + (uv_stride * vertical_offset) + horizontal_offset / 2;
+
+		u_offset = (u == 0) ? stride * (height - vertical_offset - 1) +
+								(stride - horizontal_offset) +
+								(uv_stride * vertical_offset) +
+								horizontal_offset / 2 :	u_fix;
+		v_offset = (v == 0) ? u_offset + uv_stride * height : v_fix;
+		break;
+
+	case IPU_PIX_FMT_NV12:
+		uv_stride = stride;
+		u_fix = u + (uv_stride * vertical_offset) + horizontal_offset;
+		u_offset = (u == 0) ? stride * (height - vertical_offset - 1) +
+								(stride - horizontal_offset) +
+								(uv_stride * vertical_offset) +
+								horizontal_offset :	u_fix;
+
+		break;
+	default:
+		dev_err(g_ipu_dev, "mxc ipu: unimplemented pixel format\n");
+		break;
+	}
+
+
+
+	if (u_fix > u_offset)
+		u_offset = u_fix;
+
+	if (v_fix > v_offset)
+		v_offset = v_fix;
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 0, 46, 22, u_offset / 8);
+	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 0, 68, 22, v_offset / 8);
+
+};
+
+static inline void _ipu_ch_params_set_alpha_width(uint32_t ch, int alpha_width)
+{
+	ipu_ch_param_set_field(ipu_ch_param_addr(ch), 1, 125, 3, alpha_width - 1);
+};
+
+#endif
diff --git a/drivers/mxc/ipu3/ipu_prv.h b/drivers/mxc/ipu3/ipu_prv.h
new file mode 100644
index 000000000000..c9884068283f
--- /dev/null
+++ b/drivers/mxc/ipu3/ipu_prv.h
@@ -0,0 +1,92 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __INCLUDE_IPU_PRV_H__
+#define __INCLUDE_IPU_PRV_H__
+
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <mach/hardware.h>
+
+/* Globals */
+extern struct device *g_ipu_dev;
+extern spinlock_t ipu_lock;
+extern bool g_ipu_clk_enabled;
+extern struct clk *g_ipu_clk;
+extern struct clk *g_di_clk[2];
+extern struct clk *g_csi_clk[2];
+extern unsigned char g_dc_di_assignment[];
+extern int g_ipu_hw_rev;
+
+#define IDMA_CHAN_INVALID	0xFF
+
+struct ipu_channel {
+	u8 video_in_dma;
+	u8 alpha_in_dma;
+	u8 graph_in_dma;
+	u8 out_dma;
+};
+
+int register_ipu_device(void);
+ipu_color_space_t format_to_colorspace(uint32_t fmt);
+bool ipu_pixel_format_has_alpha(uint32_t fmt);
+
+void ipu_dump_registers(void);
+
+uint32_t _ipu_channel_status(ipu_channel_t channel);
+
+void _ipu_init_dc_mappings(void);
+int _ipu_dp_init(ipu_channel_t channel, uint32_t in_pixel_fmt,
+		 uint32_t out_pixel_fmt);
+void _ipu_dp_uninit(ipu_channel_t channel);
+void _ipu_dc_init(int dc_chan, int di, bool interlaced);
+void _ipu_dc_uninit(int dc_chan);
+void _ipu_dp_dc_enable(ipu_channel_t channel);
+void _ipu_dp_dc_disable(ipu_channel_t channel, bool swap);
+void _ipu_dmfc_init(void);
+void _ipu_dmfc_set_wait4eot(int dma_chan, int width);
+int _ipu_chan_is_interlaced(ipu_channel_t channel);
+
+void _ipu_ic_enable_task(ipu_channel_t channel);
+void _ipu_ic_disable_task(ipu_channel_t channel);
+void _ipu_ic_init_prpvf(ipu_channel_params_t *params, bool src_is_csi);
+void _ipu_vdi_init(ipu_channel_t channel, ipu_channel_params_t *params);
+void _ipu_vdi_uninit(void);
+void _ipu_ic_uninit_prpvf(void);
+void _ipu_ic_init_rotate_vf(ipu_channel_params_t *params);
+void _ipu_ic_uninit_rotate_vf(void);
+void _ipu_ic_init_csi(ipu_channel_params_t *params);
+void _ipu_ic_uninit_csi(void);
+void _ipu_ic_init_prpenc(ipu_channel_params_t *params, bool src_is_csi);
+void _ipu_ic_uninit_prpenc(void);
+void _ipu_ic_init_rotate_enc(ipu_channel_params_t *params);
+void _ipu_ic_uninit_rotate_enc(void);
+void _ipu_ic_init_pp(ipu_channel_params_t *params);
+void _ipu_ic_uninit_pp(void);
+void _ipu_ic_init_rotate_pp(ipu_channel_params_t *params);
+void _ipu_ic_uninit_rotate_pp(void);
+int _ipu_ic_idma_init(int dma_chan, uint16_t width, uint16_t height,
+		      int burst_size, ipu_rotate_mode_t rot);
+void _ipu_vdi_toggle_top_field_man(void);
+int _ipu_csi_init(ipu_channel_t channel, uint32_t csi);
+void ipu_csi_set_test_generator(bool active, uint32_t r_value,
+		uint32_t g_value, uint32_t b_value,
+		uint32_t pix_clk, uint32_t csi);
+void _ipu_csi_ccir_err_detection_enable(uint32_t csi);
+void _ipu_csi_ccir_err_detection_disable(uint32_t csi);
+void _ipu_smfc_init(ipu_channel_t channel, uint32_t mipi_id, uint32_t csi);
+void _ipu_smfc_set_burst_size(ipu_channel_t channel, uint32_t bs);
+void _ipu_dp_set_csc_coefficients(ipu_channel_t channel, int32_t param[][3]);
+
+#endif				/* __INCLUDE_IPU_PRV_H__ */
diff --git a/drivers/mxc/ipu3/ipu_regs.h b/drivers/mxc/ipu3/ipu_regs.h
new file mode 100644
index 000000000000..f4b266aef794
--- /dev/null
+++ b/drivers/mxc/ipu3/ipu_regs.h
@@ -0,0 +1,651 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * @file ipu_regs.h
+ *
+ * @brief IPU Register definitions
+ *
+ * @ingroup IPU
+ */
+#ifndef __IPU_REGS_INCLUDED__
+#define __IPU_REGS_INCLUDED__
+
+#define IPU_DISP0_BASE		0x00000000
+#define IPU_MCU_T_DEFAULT	8
+#define IPU_DISP1_BASE		(IPU_MCU_T_DEFAULT << 25)
+#define IPU_CM_REG_BASE		0x1E000000
+#define IPU_IDMAC_REG_BASE	0x1E008000
+#define IPU_ISP_REG_BASE	0x1E010000
+#define IPU_DP_REG_BASE		0x1E018000
+#define IPU_IC_REG_BASE		0x1E020000
+#define IPU_IRT_REG_BASE	0x1E028000
+#define IPU_CSI0_REG_BASE	0x1E030000
+#define IPU_CSI1_REG_BASE	0x1E038000
+#define IPU_DI0_REG_BASE	0x1E040000
+#define IPU_DI1_REG_BASE	0x1E048000
+#define IPU_SMFC_REG_BASE	0x1E050000
+#define IPU_DC_REG_BASE		0x1E058000
+#define IPU_DMFC_REG_BASE	0x1E060000
+#define IPU_CPMEM_REG_BASE	0x1F000000
+#define IPU_LUT_REG_BASE	0x1F020000
+#define IPU_SRM_REG_BASE	0x1F040000
+#define IPU_TPM_REG_BASE	0x1F060000
+#define IPU_DC_TMPL_REG_BASE	0x1F080000
+#define IPU_ISP_TBPR_REG_BASE	0x1F0C0000
+#define IPU_VDI_REG_BASE	0x1E068000
+
+
+extern u32 *ipu_cm_reg;
+extern u32 *ipu_idmac_reg;
+extern u32 *ipu_dp_reg;
+extern u32 *ipu_ic_reg;
+extern u32 *ipu_dc_reg;
+extern u32 *ipu_dc_tmpl_reg;
+extern u32 *ipu_dmfc_reg;
+extern u32 *ipu_di_reg[];
+extern u32 *ipu_smfc_reg;
+extern u32 *ipu_csi_reg[];
+extern u32 *ipu_tpmem_base;
+extern u32 *ipu_disp_base[];
+extern u32 *ipu_vdi_reg;
+
+/* Register addresses */
+/* IPU Common registers */
+#define IPU_CONF		(ipu_cm_reg)
+
+#define IPU_SRM_PRI1		(ipu_cm_reg + 0x00A0/4)
+#define IPU_SRM_PRI2		(ipu_cm_reg + 0x00A4/4)
+#define IPU_FS_PROC_FLOW1	(ipu_cm_reg + 0x00A8/4)
+#define IPU_FS_PROC_FLOW2	(ipu_cm_reg + 0x00AC/4)
+#define IPU_FS_PROC_FLOW3	(ipu_cm_reg + 0x00B0/4)
+#define IPU_FS_DISP_FLOW1	(ipu_cm_reg + 0x00B4/4)
+#define IPU_FS_DISP_FLOW2	(ipu_cm_reg + 0x00B8/4)
+#define IPU_SKIP		(ipu_cm_reg + 0x00BC/4)
+#define IPU_DISP_ALT_CONF	(ipu_cm_reg + 0x00C0/4)
+#define IPU_DISP_GEN		(ipu_cm_reg + 0x00C4/4)
+#define IPU_DISP_ALT1		(ipu_cm_reg + 0x00C8/4)
+#define IPU_DISP_ALT2		(ipu_cm_reg + 0x00CC/4)
+#define IPU_DISP_ALT3		(ipu_cm_reg + 0x00D0/4)
+#define IPU_DISP_ALT4		(ipu_cm_reg + 0x00D4/4)
+#define IPU_SNOOP		(ipu_cm_reg + 0x00D8/4)
+#define IPU_MEM_RST		(ipu_cm_reg + 0x00DC/4)
+#define IPU_PM			(ipu_cm_reg + 0x00E0/4)
+#define IPU_GPR			(ipu_cm_reg + 0x00E4/4)
+#define IPU_CHA_DB_MODE_SEL(ch)	(ipu_cm_reg + 0x0150/4 + (ch / 32))
+#define IPU_ALT_CHA_DB_MODE_SEL(ch) (ipu_cm_reg + 0x0168/4 + (ch / 32))
+#define IPU_CHA_CUR_BUF(ch)	({g_ipu_hw_rev == 2 ? \
+				  (ipu_cm_reg + 0x023C/4 + (ch / 32)) : \
+				  (ipu_cm_reg + 0x0124/4 + (ch / 32)); })
+#define IPU_ALT_CUR_BUF0	({g_ipu_hw_rev == 2 ? \
+				  (ipu_cm_reg + 0x0244/4) : \
+				  (ipu_cm_reg + 0x012C/4); })
+#define IPU_ALT_CUR_BUF1	({g_ipu_hw_rev == 2 ? \
+				  (ipu_cm_reg + 0x0248/4) : \
+				  (ipu_cm_reg + 0x0130/4); })
+#define IPU_SRM_STAT		({g_ipu_hw_rev == 2 ? \
+				  (ipu_cm_reg + 0x024C/4) : \
+				  (ipu_cm_reg + 0x0134/4); })
+#define IPU_PROC_TASK_STAT	({g_ipu_hw_rev == 2 ? \
+				  (ipu_cm_reg + 0x0250/4) : \
+				  (ipu_cm_reg + 0x0138/4); })
+#define IPU_DISP_TASK_STAT	({g_ipu_hw_rev == 2 ? \
+				  (ipu_cm_reg + 0x0254/4) : \
+				  (ipu_cm_reg + 0x013C/4); })
+#define IPU_CHA_BUF0_RDY(ch)	({g_ipu_hw_rev == 2 ? \
+				  (ipu_cm_reg + 0x0268/4 + (ch / 32)) : \
+				  (ipu_cm_reg + 0x0140/4 + (ch / 32)); })
+#define IPU_CHA_BUF1_RDY(ch)	({g_ipu_hw_rev == 2 ? \
+				  (ipu_cm_reg + 0x0270/4 + (ch / 32)) : \
+				  (ipu_cm_reg + 0x0148/4 + (ch / 32)); })
+#define IPU_ALT_CHA_BUF0_RDY(ch) ({g_ipu_hw_rev == 2 ? \
+				   (ipu_cm_reg + 0x0278/4 + (ch / 32)) : \
+				   (ipu_cm_reg + 0x0158/4 + (ch / 32)); })
+#define IPU_ALT_CHA_BUF1_RDY(ch) ({g_ipu_hw_rev == 2 ? \
+				   (ipu_cm_reg + 0x0280/4 + (ch / 32)) : \
+				   (ipu_cm_reg + 0x0160/4 + (ch / 32)); })
+
+#define IPU_INT_CTRL(n)		(ipu_cm_reg + 0x003C/4 + ((n) - 1))
+#define IPU_INT_CTRL_IRQ(irq)	IPU_INT_CTRL(((irq) / 32))
+#define IPU_INT_STAT_IRQ(irq)	IPU_INT_STAT(((irq) / 32))
+#define IPU_INT_STAT(n)		({g_ipu_hw_rev == 2 ? \
+				  (ipu_cm_reg + 0x0200/4 + ((n) - 1)) : \
+				  (ipu_cm_reg + 0x00E8/4 + ((n) - 1)); })
+
+#define IPUIRQ_2_STATREG(irq)	(IPU_INT_STAT(1) + ((irq) / 32))
+#define IPUIRQ_2_CTRLREG(irq)	(IPU_INT_CTRL(1) + ((irq) / 32))
+#define IPUIRQ_2_MASK(irq)	(1UL << ((irq) & 0x1F))
+
+#define VDI_FSIZE (ipu_vdi_reg)
+#define VDI_C (ipu_vdi_reg + 0x0004/4)
+
+/* CMOS Sensor Interface Registers */
+#define CSI_SENS_CONF(csi)	(ipu_csi_reg[csi])
+#define CSI_SENS_FRM_SIZE(csi)	(ipu_csi_reg[csi] + 0x0004/4)
+#define CSI_ACT_FRM_SIZE(csi)	(ipu_csi_reg[csi] + 0x0008/4)
+#define CSI_OUT_FRM_CTRL(csi)	(ipu_csi_reg[csi] + 0x000C/4)
+#define CSI_TST_CTRL(csi)	(ipu_csi_reg[csi] + 0x0010/4)
+#define CSI_CCIR_CODE_1(csi)	(ipu_csi_reg[csi] + 0x0014/4)
+#define CSI_CCIR_CODE_2(csi)	(ipu_csi_reg[csi] + 0x0018/4)
+#define CSI_CCIR_CODE_3(csi)	(ipu_csi_reg[csi] + 0x001C/4)
+#define CSI_MIPI_DI(csi)	(ipu_csi_reg[csi] + 0x0020/4)
+#define CSI_SKIP(csi)		(ipu_csi_reg[csi] + 0x0024/4)
+#define CSI_CPD_CTRL(csi)	(ipu_csi_reg[csi] + 0x0028/4)
+#define CSI_CPD_RC(csi, n)	(ipu_csi_reg[csi] + 0x002C/4 + n)
+#define CSI_CPD_RS(csi, n)	(ipu_csi_reg[csi] + 0x004C/4 + n)
+#define CSI_CPD_GRC(csi, n)	(ipu_csi_reg[csi] + 0x005C/4 + n)
+#define CSI_CPD_GRS(csi, n)	(ipu_csi_reg[csi] + 0x007C/4 + n)
+#define CSI_CPD_GBC(csi, n)	(ipu_csi_reg[csi] + 0x008C/4 + n)
+#define CSI_CPD_GBS(csi, n)	(ipu_csi_reg[csi] + 0x00AC/4 + n)
+#define CSI_CPD_BC(csi, n)	(ipu_csi_reg[csi] + 0x00BC/4 + n)
+#define CSI_CPD_BS(csi, n)	(ipu_csi_reg[csi] + 0x00DC/4 + n)
+#define CSI_CPD_OFFSET1(csi)	(ipu_csi_reg[csi] + 0x00EC/4)
+#define CSI_CPD_OFFSET2(csi)	(ipu_csi_reg[csi] + 0x00F0/4)
+
+/*SMFC Registers */
+#define SMFC_MAP	(ipu_smfc_reg)
+#define SMFC_WMC	(ipu_smfc_reg + 0x0004/4)
+#define SMFC_BS		(ipu_smfc_reg + 0x0008/4)
+
+/* Image Converter Registers */
+#define IC_CONF			(ipu_ic_reg)
+#define IC_PRP_ENC_RSC		(ipu_ic_reg + 0x0004/4)
+#define IC_PRP_VF_RSC		(ipu_ic_reg + 0x0008/4)
+#define IC_PP_RSC		(ipu_ic_reg + 0x000C/4)
+#define IC_CMBP_1		(ipu_ic_reg + 0x0010/4)
+#define IC_CMBP_2		(ipu_ic_reg + 0x0014/4)
+#define IC_IDMAC_1		(ipu_ic_reg + 0x0018/4)
+#define IC_IDMAC_2		(ipu_ic_reg + 0x001C/4)
+#define IC_IDMAC_3		(ipu_ic_reg + 0x0020/4)
+#define IC_IDMAC_4		(ipu_ic_reg + 0x0024/4)
+
+#define IDMAC_CONF		(ipu_idmac_reg + 0x0000)
+#define IDMAC_CHA_EN(ch)	(ipu_idmac_reg + 0x0004/4 + (ch/32))
+#define IDMAC_SEP_ALPHA		(ipu_idmac_reg + 0x000C/4)
+#define IDMAC_ALT_SEP_ALPHA	(ipu_idmac_reg + 0x0010/4)
+#define IDMAC_CHA_PRI(ch)	(ipu_idmac_reg + 0x0014/4 + (ch/32))
+#define IDMAC_WM_EN(ch)		(ipu_idmac_reg + 0x001C/4 + (ch/32))
+#define IDMAC_CH_LOCK_EN_1	({g_ipu_hw_rev == 2 ? \
+				  (ipu_idmac_reg + 0x0024/4) : 0; })
+#define IDMAC_CH_LOCK_EN_2	({g_ipu_hw_rev == 2 ? \
+				  (ipu_idmac_reg + 0x0028/4) : \
+				  (ipu_idmac_reg + 0x0024/4); })
+#define IDMAC_SUB_ADDR_0	({g_ipu_hw_rev == 2 ? \
+				  (ipu_idmac_reg + 0x002C/4) : \
+				  (ipu_idmac_reg + 0x0028/4); })
+#define IDMAC_SUB_ADDR_1	({g_ipu_hw_rev == 2 ? \
+				  (ipu_idmac_reg + 0x0030/4) : \
+				  (ipu_idmac_reg + 0x002C/4); })
+#define IDMAC_SUB_ADDR_2	({g_ipu_hw_rev == 2 ? \
+				  (ipu_idmac_reg + 0x0034/4) : \
+				  (ipu_idmac_reg + 0x0030/4); })
+#define IDMAC_BAND_EN(ch)	({g_ipu_hw_rev == 2 ? \
+				  (ipu_idmac_reg + 0x0040/4 + (ch/32)) : \
+				  (ipu_idmac_reg + 0x0034/4 + (ch/32)); })
+#define IDMAC_CHA_BUSY(ch)	({g_ipu_hw_rev == 2 ? \
+				  (ipu_idmac_reg + 0x0100/4 + (ch/32)) : \
+				  (ipu_idmac_reg + 0x0040/4 + (ch/32)); })
+
+#define DI_GENERAL(di)		(ipu_di_reg[di])
+#define DI_BS_CLKGEN0(di)	(ipu_di_reg[di] + 0x0004/4)
+#define DI_BS_CLKGEN1(di)	(ipu_di_reg[di] + 0x0008/4)
+
+#define DI_SW_GEN0(di, gen)	(ipu_di_reg[di] + 0x000C/4 + (gen - 1))
+#define DI_SW_GEN1(di, gen)	(ipu_di_reg[di] + 0x0030/4 + (gen - 1))
+#define DI_STP_REP(di, gen)	(ipu_di_reg[di] + 0x0148/4 + (gen - 1)/2)
+#define DI_SYNC_AS_GEN(di)	(ipu_di_reg[di] + 0x0054/4)
+#define DI_DW_GEN(di, gen)	(ipu_di_reg[di] + 0x0058/4 + gen)
+#define DI_DW_SET(di, gen, set)	(ipu_di_reg[di] + 0x0088/4 + gen + 0xC*set)
+#define DI_SER_CONF(di)		(ipu_di_reg[di] + 0x015C/4)
+#define DI_SSC(di)		(ipu_di_reg[di] + 0x0160/4)
+#define DI_POL(di)		(ipu_di_reg[di] + 0x0164/4)
+#define DI_AW0(di)		(ipu_di_reg[di] + 0x0168/4)
+#define DI_AW1(di)		(ipu_di_reg[di] + 0x016C/4)
+#define DI_SCR_CONF(di)		(ipu_di_reg[di] + 0x0170/4)
+#define DI_STAT(di)		(ipu_di_reg[di] + 0x0174/4)
+
+#define DMFC_RD_CHAN		(ipu_dmfc_reg)
+#define DMFC_WR_CHAN		(ipu_dmfc_reg + 0x0004/4)
+#define DMFC_WR_CHAN_DEF	(ipu_dmfc_reg + 0x0008/4)
+#define DMFC_DP_CHAN		(ipu_dmfc_reg + 0x000C/4)
+#define DMFC_DP_CHAN_DEF	(ipu_dmfc_reg + 0x0010/4)
+#define DMFC_GENERAL1		(ipu_dmfc_reg + 0x0014/4)
+#define DMFC_GENERAL2		(ipu_dmfc_reg + 0x0018/4)
+#define DMFC_IC_CTRL		(ipu_dmfc_reg + 0x001C/4)
+#define DMFC_STAT		(ipu_dmfc_reg + 0x0020/4)
+
+#define DC_MAP_CONF_PTR(n)	(ipu_dc_reg + 0x0108/4 + n/2)
+#define DC_MAP_CONF_VAL(n)	(ipu_dc_reg + 0x0144/4 + n/2)
+
+#define _RL_CH_2_OFFSET(ch)	((ch == 0) ? 8 : ( \
+				 (ch == 1) ? 0x24 : ( \
+				 (ch == 2) ? 0x40 : ( \
+				 (ch == 5) ? 0x64 : ( \
+				 (ch == 6) ? 0x80 : ( \
+				 (ch == 8) ? 0x9C : ( \
+				 (ch == 9) ? 0xBC : (-1))))))))
+#define DC_RL_CH(ch, evt)	(ipu_dc_reg + _RL_CH_2_OFFSET(ch)/4 + evt/2)
+
+#define DC_EVT_NF		0
+#define DC_EVT_NL		1
+#define DC_EVT_EOF		2
+#define DC_EVT_NFIELD		3
+#define DC_EVT_EOL		4
+#define DC_EVT_EOFIELD		5
+#define DC_EVT_NEW_ADDR		6
+#define DC_EVT_NEW_CHAN		7
+#define DC_EVT_NEW_DATA		8
+
+#define DC_EVT_NEW_ADDR_W_0	0
+#define DC_EVT_NEW_ADDR_W_1	1
+#define DC_EVT_NEW_CHAN_W_0	2
+#define DC_EVT_NEW_CHAN_W_1	3
+#define DC_EVT_NEW_DATA_W_0	4
+#define DC_EVT_NEW_DATA_W_1	5
+#define DC_EVT_NEW_ADDR_R_0	6
+#define DC_EVT_NEW_ADDR_R_1	7
+#define DC_EVT_NEW_CHAN_R_0	8
+#define DC_EVT_NEW_CHAN_R_1	9
+#define DC_EVT_NEW_DATA_R_0	10
+#define DC_EVT_NEW_DATA_R_1	11
+
+#define dc_ch_offset(ch) \
+({ \
+	const u8 _offset[] = { \
+		0, 0x1C, 0x38, 0x54, 0x58, 0x5C, 0x78, 0, 0x94, 0xB4}; \
+	_offset[ch]; \
+})
+#define DC_WR_CH_CONF(ch)	(ipu_dc_reg + dc_ch_offset(ch)/4)
+#define DC_WR_CH_ADDR(ch)	(ipu_dc_reg + dc_ch_offset(ch)/4 + 4/4)
+
+#define DC_WR_CH_CONF_1		(ipu_dc_reg + 0x001C/4)
+#define DC_WR_CH_ADDR_1		(ipu_dc_reg + 0x0020/4)
+#define DC_WR_CH_CONF_5		(ipu_dc_reg + 0x005C/4)
+#define DC_WR_CH_ADDR_5		(ipu_dc_reg + 0x0060/4)
+#define DC_GEN			(ipu_dc_reg + 0x00D4/4)
+#define DC_DISP_CONF1(disp)	(ipu_dc_reg + 0x00D8/4 + disp)
+#define DC_DISP_CONF2(disp)	(ipu_dc_reg + 0x00E8/4 + disp)
+#define DC_STAT			(ipu_dc_reg + 0x01C8/4)
+#define DC_UGDE_0(evt)		(ipu_dc_reg + 0x0174/4 + evt*4)
+#define DC_UGDE_1(evt)		(ipu_dc_reg + 0x0178/4 + evt*4)
+#define DC_UGDE_2(evt)		(ipu_dc_reg + 0x017C/4 + evt*4)
+#define DC_UGDE_3(evt)		(ipu_dc_reg + 0x0180/4 + evt*4)
+
+#define DP_SYNC 0
+#define DP_ASYNC0 0x60
+#define DP_ASYNC1 0xBC
+#define DP_COM_CONF(flow)	(ipu_dp_reg + flow/4)
+#define DP_GRAPH_WIND_CTRL(flow) (ipu_dp_reg + 0x0004/4 + flow/4)
+#define DP_FG_POS(flow)		(ipu_dp_reg + 0x0008/4 + flow/4)
+#define DP_CSC_A_0(flow)	(ipu_dp_reg + 0x0044/4 + flow/4)
+#define DP_CSC_A_1(flow)	(ipu_dp_reg + 0x0048/4 + flow/4)
+#define DP_CSC_A_2(flow)	(ipu_dp_reg + 0x004C/4 + flow/4)
+#define DP_CSC_A_3(flow)	(ipu_dp_reg + 0x0050/4 + flow/4)
+#define DP_CSC_0(flow)		(ipu_dp_reg + 0x0054/4 + flow/4)
+#define DP_CSC_1(flow)		(ipu_dp_reg + 0x0058/4 + flow/4)
+
+enum {
+	IPU_CONF_CSI0_EN = 0x00000001,
+	IPU_CONF_CSI1_EN = 0x00000002,
+	IPU_CONF_IC_EN = 0x00000004,
+	IPU_CONF_ROT_EN = 0x00000008,
+	IPU_CONF_ISP_EN = 0x00000010,
+	IPU_CONF_DP_EN = 0x00000020,
+	IPU_CONF_DI0_EN = 0x00000040,
+	IPU_CONF_DI1_EN = 0x00000080,
+	IPU_CONF_DMFC_EN = 0x00000400,
+	IPU_CONF_SMFC_EN = 0x00000100,
+	IPU_CONF_DC_EN = 0x00000200,
+	IPU_CONF_VDI_EN = 0x00001000,
+	IPU_CONF_IDMAC_DIS = 0x00400000,
+	IPU_CONF_IC_DMFC_SEL = 0x02000000,
+	IPU_CONF_IC_DMFC_SYNC = 0x04000000,
+	IPU_CONF_VDI_DMFC_SYNC = 0x08000000,
+	IPU_CONF_CSI0_DATA_SOURCE = 0x10000000,
+	IPU_CONF_CSI0_DATA_SOURCE_OFFSET = 28,
+	IPU_CONF_CSI1_DATA_SOURCE = 0x20000000,
+	IPU_CONF_IC_INPUT = 0x40000000,
+	IPU_CONF_CSI_SEL = 0x80000000,
+
+	DI0_COUNTER_RELEASE = 0x01000000,
+	DI1_COUNTER_RELEASE = 0x02000000,
+
+	FS_PRPVF_ROT_SRC_SEL_MASK = 0x00000F00,
+	FS_PRPVF_ROT_SRC_SEL_OFFSET = 8,
+	FS_PRPENC_ROT_SRC_SEL_MASK = 0x0000000F,
+	FS_PRPENC_ROT_SRC_SEL_OFFSET = 0,
+	FS_PP_ROT_SRC_SEL_MASK = 0x000F0000,
+	FS_PP_ROT_SRC_SEL_OFFSET = 16,
+	FS_PP_SRC_SEL_MASK = 0x0000F000,
+	FS_PP_SRC_SEL_OFFSET = 12,
+	FS_PRP_SRC_SEL_MASK = 0x0F000000,
+	FS_PRP_SRC_SEL_OFFSET = 24,
+	FS_VF_IN_VALID = 0x80000000,
+	FS_ENC_IN_VALID = 0x40000000,
+	FS_VDI_SRC_SEL_MASK = 0x30000000,
+	FS_VDI_SRC_SEL_OFFSET = 28,
+
+
+	FS_PRPENC_DEST_SEL_MASK = 0x0000000F,
+	FS_PRPENC_DEST_SEL_OFFSET = 0,
+	FS_PRPVF_DEST_SEL_MASK = 0x000000F0,
+	FS_PRPVF_DEST_SEL_OFFSET = 4,
+	FS_PRPVF_ROT_DEST_SEL_MASK = 0x00000F00,
+	FS_PRPVF_ROT_DEST_SEL_OFFSET = 8,
+	FS_PP_DEST_SEL_MASK = 0x0000F000,
+	FS_PP_DEST_SEL_OFFSET = 12,
+	FS_PP_ROT_DEST_SEL_MASK = 0x000F0000,
+	FS_PP_ROT_DEST_SEL_OFFSET = 16,
+	FS_PRPENC_ROT_DEST_SEL_MASK = 0x00F00000,
+	FS_PRPENC_ROT_DEST_SEL_OFFSET = 20,
+
+	FS_SMFC0_DEST_SEL_MASK = 0x0000000F,
+	FS_SMFC0_DEST_SEL_OFFSET = 0,
+	FS_SMFC1_DEST_SEL_MASK = 0x00000070,
+	FS_SMFC1_DEST_SEL_OFFSET = 4,
+	FS_SMFC2_DEST_SEL_MASK = 0x00000780,
+	FS_SMFC2_DEST_SEL_OFFSET = 7,
+	FS_SMFC3_DEST_SEL_MASK = 0x00003800,
+	FS_SMFC3_DEST_SEL_OFFSET = 11,
+
+	FS_DC1_SRC_SEL_MASK = 0x00F00000,
+	FS_DC1_SRC_SEL_OFFSET = 20,
+	FS_DC2_SRC_SEL_MASK = 0x000F0000,
+	FS_DC2_SRC_SEL_OFFSET = 16,
+	FS_DP_SYNC0_SRC_SEL_MASK = 0x0000000F,
+	FS_DP_SYNC0_SRC_SEL_OFFSET = 0,
+	FS_DP_SYNC1_SRC_SEL_MASK = 0x000000F0,
+	FS_DP_SYNC1_SRC_SEL_OFFSET = 4,
+	FS_DP_ASYNC0_SRC_SEL_MASK = 0x00000F00,
+	FS_DP_ASYNC0_SRC_SEL_OFFSET = 8,
+	FS_DP_ASYNC1_SRC_SEL_MASK = 0x0000F000,
+	FS_DP_ASYNC1_SRC_SEL_OFFSET = 12,
+
+	FS_AUTO_REF_PER_MASK = 0,
+	FS_AUTO_REF_PER_OFFSET = 16,
+
+	TSTAT_VF_MASK = 0x0000000C,
+	TSTAT_VF_OFFSET = 2,
+	TSTAT_VF_ROT_MASK = 0x00000300,
+	TSTAT_VF_ROT_OFFSET = 8,
+	TSTAT_ENC_MASK = 0x00000003,
+	TSTAT_ENC_OFFSET = 0,
+	TSTAT_ENC_ROT_MASK = 0x000000C0,
+	TSTAT_ENC_ROT_OFFSET = 6,
+	TSTAT_PP_MASK = 0x00000030,
+	TSTAT_PP_OFFSET = 4,
+	TSTAT_PP_ROT_MASK = 0x00000C00,
+	TSTAT_PP_ROT_OFFSET = 10,
+
+	TASK_STAT_IDLE = 0,
+	TASK_STAT_ACTIVE = 1,
+	TASK_STAT_WAIT4READY = 2,
+
+	/* Image Converter Register bits */
+	IC_CONF_PRPENC_EN = 0x00000001,
+	IC_CONF_PRPENC_CSC1 = 0x00000002,
+	IC_CONF_PRPENC_ROT_EN = 0x00000004,
+	IC_CONF_PRPVF_EN = 0x00000100,
+	IC_CONF_PRPVF_CSC1 = 0x00000200,
+	IC_CONF_PRPVF_CSC2 = 0x00000400,
+	IC_CONF_PRPVF_CMB = 0x00000800,
+	IC_CONF_PRPVF_ROT_EN = 0x00001000,
+	IC_CONF_PP_EN = 0x00010000,
+	IC_CONF_PP_CSC1 = 0x00020000,
+	IC_CONF_PP_CSC2 = 0x00040000,
+	IC_CONF_PP_CMB = 0x00080000,
+	IC_CONF_PP_ROT_EN = 0x00100000,
+	IC_CONF_IC_GLB_LOC_A = 0x10000000,
+	IC_CONF_KEY_COLOR_EN = 0x20000000,
+	IC_CONF_RWS_EN = 0x40000000,
+	IC_CONF_CSI_MEM_WR_EN = 0x80000000,
+
+	IC_IDMAC_1_CB0_BURST_16 = 0x00000001,
+	IC_IDMAC_1_CB1_BURST_16 = 0x00000002,
+	IC_IDMAC_1_CB2_BURST_16 = 0x00000004,
+	IC_IDMAC_1_CB3_BURST_16 = 0x00000008,
+	IC_IDMAC_1_CB4_BURST_16 = 0x00000010,
+	IC_IDMAC_1_CB5_BURST_16 = 0x00000020,
+	IC_IDMAC_1_CB6_BURST_16 = 0x00000040,
+	IC_IDMAC_1_CB7_BURST_16 = 0x00000080,
+	IC_IDMAC_1_PRPENC_ROT_MASK = 0x00003800,
+	IC_IDMAC_1_PRPENC_ROT_OFFSET = 11,
+	IC_IDMAC_1_PRPVF_ROT_MASK = 0x0001C000,
+	IC_IDMAC_1_PRPVF_ROT_OFFSET = 14,
+	IC_IDMAC_1_PP_ROT_MASK = 0x000E0000,
+	IC_IDMAC_1_PP_ROT_OFFSET = 17,
+	IC_IDMAC_1_PP_FLIP_RS = 0x00400000,
+	IC_IDMAC_1_PRPVF_FLIP_RS = 0x00200000,
+	IC_IDMAC_1_PRPENC_FLIP_RS = 0x00100000,
+
+	IC_IDMAC_2_PRPENC_HEIGHT_MASK = 0x000003FF,
+	IC_IDMAC_2_PRPENC_HEIGHT_OFFSET = 0,
+	IC_IDMAC_2_PRPVF_HEIGHT_MASK = 0x000FFC00,
+	IC_IDMAC_2_PRPVF_HEIGHT_OFFSET = 10,
+	IC_IDMAC_2_PP_HEIGHT_MASK = 0x3FF00000,
+	IC_IDMAC_2_PP_HEIGHT_OFFSET = 20,
+
+	IC_IDMAC_3_PRPENC_WIDTH_MASK = 0x000003FF,
+	IC_IDMAC_3_PRPENC_WIDTH_OFFSET = 0,
+	IC_IDMAC_3_PRPVF_WIDTH_MASK = 0x000FFC00,
+	IC_IDMAC_3_PRPVF_WIDTH_OFFSET = 10,
+	IC_IDMAC_3_PP_WIDTH_MASK = 0x3FF00000,
+	IC_IDMAC_3_PP_WIDTH_OFFSET = 20,
+
+	CSI_SENS_CONF_DATA_FMT_SHIFT = 8,
+	CSI_SENS_CONF_DATA_FMT_MASK = 0x00000700,
+	CSI_SENS_CONF_DATA_FMT_RGB_YUV444 = 0L,
+	CSI_SENS_CONF_DATA_FMT_YUV422_YUYV = 1L,
+	CSI_SENS_CONF_DATA_FMT_YUV422_UYVY = 2L,
+	CSI_SENS_CONF_DATA_FMT_BAYER = 3L,
+	CSI_SENS_CONF_DATA_FMT_RGB565 = 4L,
+	CSI_SENS_CONF_DATA_FMT_RGB555 = 5L,
+	CSI_SENS_CONF_DATA_FMT_RGB444 = 6L,
+	CSI_SENS_CONF_DATA_FMT_JPEG = 7L,
+
+	CSI_SENS_CONF_VSYNC_POL_SHIFT = 0,
+	CSI_SENS_CONF_HSYNC_POL_SHIFT = 1,
+	CSI_SENS_CONF_DATA_POL_SHIFT = 2,
+	CSI_SENS_CONF_PIX_CLK_POL_SHIFT = 3,
+	CSI_SENS_CONF_SENS_PRTCL_SHIFT = 4,
+	CSI_SENS_CONF_PACK_TIGHT_SHIFT = 7,
+	CSI_SENS_CONF_DATA_WIDTH_SHIFT = 11,
+	CSI_SENS_CONF_EXT_VSYNC_SHIFT = 15,
+	CSI_SENS_CONF_DIVRATIO_SHIFT = 16,
+
+	CSI_SENS_CONF_DIVRATIO_MASK = 0x00FF0000L,
+	CSI_SENS_CONF_DATA_DEST_SHIFT = 24,
+	CSI_SENS_CONF_DATA_DEST_MASK = 0x07000000L,
+	CSI_SENS_CONF_JPEG8_EN_SHIFT = 27,
+	CSI_SENS_CONF_JPEG_EN_SHIFT = 28,
+	CSI_SENS_CONF_FORCE_EOF_SHIFT = 29,
+	CSI_SENS_CONF_DATA_EN_POL_SHIFT = 31,
+
+	CSI_DATA_DEST_ISP = 1L,
+	CSI_DATA_DEST_IC = 2L,
+	CSI_DATA_DEST_IDMAC = 4L,
+
+	CSI_CCIR_ERR_DET_EN = 0x01000000L,
+	CSI_HORI_DOWNSIZE_EN = 0x80000000L,
+	CSI_VERT_DOWNSIZE_EN = 0x40000000L,
+	CSI_TEST_GEN_MODE_EN = 0x01000000L,
+
+	CSI_HSC_MASK = 0x1FFF0000,
+	CSI_HSC_SHIFT = 16,
+	CSI_VSC_MASK = 0x00000FFF,
+	CSI_VSC_SHIFT = 0,
+
+	CSI_TEST_GEN_R_MASK = 0x000000FFL,
+	CSI_TEST_GEN_R_SHIFT = 0,
+	CSI_TEST_GEN_G_MASK = 0x0000FF00L,
+	CSI_TEST_GEN_G_SHIFT = 8,
+	CSI_TEST_GEN_B_MASK = 0x00FF0000L,
+	CSI_TEST_GEN_B_SHIFT = 16,
+
+	CSI_MIPI_DI0_MASK = 0x000000FFL,
+	CSI_MIPI_DI0_SHIFT = 0,
+	CSI_MIPI_DI1_MASK = 0x0000FF00L,
+	CSI_MIPI_DI1_SHIFT = 8,
+	CSI_MIPI_DI2_MASK = 0x00FF0000L,
+	CSI_MIPI_DI2_SHIFT = 16,
+	CSI_MIPI_DI3_MASK = 0xFF000000L,
+	CSI_MIPI_DI3_SHIFT = 24,
+
+	CSI_MAX_RATIO_SKIP_ISP_MASK = 0x00070000L,
+	CSI_MAX_RATIO_SKIP_ISP_SHIFT = 16,
+	CSI_SKIP_ISP_MASK = 0x00F80000L,
+	CSI_SKIP_ISP_SHIFT = 19,
+	CSI_MAX_RATIO_SKIP_SMFC_MASK = 0x00000007L,
+	CSI_MAX_RATIO_SKIP_SMFC_SHIFT = 0,
+	CSI_SKIP_SMFC_MASK = 0x000000F8L,
+	CSI_SKIP_SMFC_SHIFT = 3,
+	CSI_ID_2_SKIP_MASK = 0x00000300L,
+	CSI_ID_2_SKIP_SHIFT = 8,
+
+	CSI_COLOR_FIRST_ROW_MASK = 0x00000002L,
+	CSI_COLOR_FIRST_COMP_MASK = 0x00000001L,
+
+	SMFC_MAP_CH0_MASK = 0x00000007L,
+	SMFC_MAP_CH0_SHIFT = 0,
+	SMFC_MAP_CH1_MASK = 0x00000038L,
+	SMFC_MAP_CH1_SHIFT = 3,
+	SMFC_MAP_CH2_MASK = 0x000001C0L,
+	SMFC_MAP_CH2_SHIFT = 6,
+	SMFC_MAP_CH3_MASK = 0x00000E00L,
+	SMFC_MAP_CH3_SHIFT = 9,
+
+	SMFC_WM0_SET_MASK = 0x00000007L,
+	SMFC_WM0_SET_SHIFT = 0,
+	SMFC_WM1_SET_MASK = 0x000001C0L,
+	SMFC_WM1_SET_SHIFT = 6,
+	SMFC_WM2_SET_MASK = 0x00070000L,
+	SMFC_WM2_SET_SHIFT = 16,
+	SMFC_WM3_SET_MASK = 0x01C00000L,
+	SMFC_WM3_SET_SHIFT = 22,
+
+	SMFC_WM0_CLR_MASK = 0x00000038L,
+	SMFC_WM0_CLR_SHIFT = 3,
+	SMFC_WM1_CLR_MASK = 0x00000E00L,
+	SMFC_WM1_CLR_SHIFT = 9,
+	SMFC_WM2_CLR_MASK = 0x00380000L,
+	SMFC_WM2_CLR_SHIFT = 19,
+	SMFC_WM3_CLR_MASK = 0x0E000000L,
+	SMFC_WM3_CLR_SHIFT = 25,
+
+	SMFC_BS0_MASK = 0x0000000FL,
+	SMFC_BS0_SHIFT = 0,
+	SMFC_BS1_MASK = 0x000000F0L,
+	SMFC_BS1_SHIFT = 4,
+	SMFC_BS2_MASK = 0x00000F00L,
+	SMFC_BS2_SHIFT = 8,
+	SMFC_BS3_MASK = 0x0000F000L,
+	SMFC_BS3_SHIFT = 12,
+
+	PF_CONF_TYPE_MASK = 0x00000007,
+	PF_CONF_TYPE_SHIFT = 0,
+	PF_CONF_PAUSE_EN = 0x00000010,
+	PF_CONF_RESET = 0x00008000,
+	PF_CONF_PAUSE_ROW_MASK = 0x00FF0000,
+	PF_CONF_PAUSE_ROW_SHIFT = 16,
+
+	DI_DW_GEN_ACCESS_SIZE_OFFSET = 24,
+	DI_DW_GEN_COMPONENT_SIZE_OFFSET = 16,
+
+	DI_GEN_DI_CLK_EXT = 0x100000,
+	DI_GEN_POLARITY_1 = 0x00000001,
+	DI_GEN_POLARITY_2 = 0x00000002,
+	DI_GEN_POLARITY_3 = 0x00000004,
+	DI_GEN_POLARITY_4 = 0x00000008,
+	DI_GEN_POLARITY_5 = 0x00000010,
+	DI_GEN_POLARITY_6 = 0x00000020,
+	DI_GEN_POLARITY_7 = 0x00000040,
+	DI_GEN_POLARITY_8 = 0x00000080,
+
+	DI_POL_DRDY_DATA_POLARITY = 0x00000080,
+	DI_POL_DRDY_POLARITY_15 = 0x00000010,
+
+	DI_VSYNC_SEL_OFFSET = 13,
+
+	DC_WR_CH_CONF_FIELD_MODE = 0x00000200,
+	DC_WR_CH_CONF_PROG_TYPE_OFFSET = 5,
+	DC_WR_CH_CONF_PROG_TYPE_MASK = 0x000000E0,
+	DC_WR_CH_CONF_PROG_DI_ID = 0x00000004,
+	DC_WR_CH_CONF_PROG_DISP_ID_OFFSET = 3,
+	DC_WR_CH_CONF_PROG_DISP_ID_MASK = 0x00000018,
+
+	DC_UGDE_0_ODD_EN = 0x02000000,
+	DC_UGDE_0_ID_CODED_MASK = 0x00000007,
+	DC_UGDE_0_ID_CODED_OFFSET = 0,
+	DC_UGDE_0_EV_PRIORITY_MASK = 0x00000078,
+	DC_UGDE_0_EV_PRIORITY_OFFSET = 3,
+
+	DP_COM_CONF_FG_EN = 0x00000001,
+	DP_COM_CONF_GWSEL = 0x00000002,
+	DP_COM_CONF_GWAM = 0x00000004,
+	DP_COM_CONF_GWCKE = 0x00000008,
+	DP_COM_CONF_CSC_DEF_MASK = 0x00000300,
+	DP_COM_CONF_CSC_DEF_OFFSET = 8,
+	DP_COM_CONF_CSC_DEF_FG = 0x00000300,
+	DP_COM_CONF_CSC_DEF_BG = 0x00000200,
+	DP_COM_CONF_CSC_DEF_BOTH = 0x00000100,
+
+	DI_SER_CONF_LLA_SER_ACCESS = 0x00000020,
+	DI_SER_CONF_SERIAL_CLK_POL = 0x00000010,
+	DI_SER_CONF_SERIAL_DATA_POL = 0x00000008,
+	DI_SER_CONF_SERIAL_RS_POL = 0x00000004,
+	DI_SER_CONF_SERIAL_CS_POL = 0x00000002,
+	DI_SER_CONF_WAIT4SERIAL = 0x00000001,
+
+	VDI_C_CH_420 = 0x00000000,
+	VDI_C_CH_422 = 0x00000002,
+	VDI_C_MOT_SEL_FULL = 0x00000008,
+	VDI_C_MOT_SEL_LOW = 0x00000004,
+	VDI_C_MOT_SEL_MED = 0x00000000,
+	VDI_C_BURST_SIZE1_4 = 0x00000030,
+	VDI_C_BURST_SIZE2_4 = 0x00000300,
+	VDI_C_BURST_SIZE3_4 = 0x00003000,
+	VDI_C_VWM1_SET_1 = 0x00000000,
+	VDI_C_VWM1_CLR_2 = 0x00080000,
+	VDI_C_VWM3_SET_1 = 0x00000000,
+	VDI_C_VWM3_CLR_2 = 0x02000000,
+	VDI_C_TOP_FIELD_MAN_1 = 0x40000000,
+	VDI_C_TOP_FIELD_AUTO_1 = 0x80000000,
+};
+
+enum di_pins {
+	DI_PIN11 = 0,
+	DI_PIN12 = 1,
+	DI_PIN13 = 2,
+	DI_PIN14 = 3,
+	DI_PIN15 = 4,
+	DI_PIN16 = 5,
+	DI_PIN17 = 6,
+	DI_PIN_CS = 7,
+
+	DI_PIN_SER_CLK = 0,
+	DI_PIN_SER_RS = 1,
+};
+
+enum di_sync_wave {
+	DI_SYNC_NONE = -1,
+	DI_SYNC_CLK = 0,
+	DI_SYNC_INT_HSYNC = 1,
+	DI_SYNC_HSYNC = 2,
+	DI_SYNC_VSYNC = 3,
+	DI_SYNC_DE = 5,
+};
+
+/* DC template opcodes */
+#define WROD(lf)		(0x18 | (lf << 1))
+
+#endif
diff --git a/drivers/mxc/mcu_pmic/Kconfig b/drivers/mxc/mcu_pmic/Kconfig
new file mode 100644
index 000000000000..43addf06f82e
--- /dev/null
+++ b/drivers/mxc/mcu_pmic/Kconfig
@@ -0,0 +1,19 @@
+#
+# PMIC Modules configuration
+#
+
+config MXC_PMIC_MC9S08DZ60
+	tristate "MC9S08DZ60 PMIC"
+	depends on ARCH_MXC && I2C
+	  ---help---
+	  This is the MXC MC9S08DZ60(MCU) PMIC support.
+
+config MXC_MC9SDZ60_RTC
+	tristate "MC9SDZ60 Real Time Clock (RTC) support"
+	depends on MXC_PMIC_MC9SDZ60
+	---help---
+	This is the MC9SDZ60 RTC module driver. This module provides kernel API
+	for RTC part of MC9SDZ60.
+	If you want MC9SDZ60 RTC support, you should say Y here
+	
+
diff --git a/drivers/mxc/mcu_pmic/Makefile b/drivers/mxc/mcu_pmic/Makefile
new file mode 100644
index 000000000000..dee58bfc1051
--- /dev/null
+++ b/drivers/mxc/mcu_pmic/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for the mc9sdz60 pmic drivers.
+#
+
+obj-$(CONFIG_MXC_PMIC_MC9SDZ60) += pmic_mc9sdz60_mod.o
+pmic_mc9sdz60_mod-objs := mcu_pmic_core.o max8660.o mc9s08dz60.o mcu_pmic_gpio.o
+
+
diff --git a/drivers/mxc/mcu_pmic/max8660.c b/drivers/mxc/mcu_pmic/max8660.c
new file mode 100644
index 000000000000..f48899f212f9
--- /dev/null
+++ b/drivers/mxc/mcu_pmic/max8660.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file max8660.c
+ * @brief Driver for max8660
+ *
+ * @ingroup pmic
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/spinlock.h>
+#include <linux/proc_fs.h>
+#include <linux/i2c.h>
+#include <linux/mfd/mc9s08dz60/pmic.h>
+#include <asm/uaccess.h>
+#include "mcu_pmic_core.h"
+#include "max8660.h"
+
+/* I2C bus id and device address of mcu */
+#define I2C1_BUS	0
+#define MAX8660_I2C_ADDR				0x68
+
+static struct i2c_client *max8660_i2c_client;
+
+    /* reg names for max8660
+       REG_MAX8660_OUTPUT_ENABLE_1,
+       REG_MAX8660_OUTPUT_ENABLE_2,
+       REG_MAX8660_VOLT__CHANGE_1,
+       REG_MAX8660_V3_TARGET_VOLT_1,
+       REG_MAX8660_V3_TARGET_VOLT_2,
+       REG_MAX8660_V4_TARGET_VOLT_1,
+       REG_MAX8660_V4_TARGET_VOLT_2,
+       REG_MAX8660_V5_TARGET_VOLT_1,
+       REG_MAX8660_V5_TARGET_VOLT_2,
+       REG_MAX8660_V6V7_TARGET_VOLT,
+       REG_MAX8660_FORCE_PWM
+     */
+
+    /* save down the reg values for the device is write only */
+static u8 max8660_reg_value_table[] =
+    { 0x0, 0x0, 0x0, 0x17, 0x17, 0x1F, 0x1F, 0x04, 0x04, 0x0, 0x0
+};
+static int max8660_dev_present;
+
+int is_max8660_present(void)
+{
+	return max8660_dev_present;
+}
+
+int max8660_get_buffered_reg_val(int reg_name, u8 *value)
+{
+	if (!max8660_dev_present)
+		return -1;
+	/* outof range */
+	if (reg_name < REG_MAX8660_OUTPUT_ENABLE_1
+	    || reg_name > REG_MAX8660_FORCE_PWM)
+		return -1;
+	*value =
+	    max8660_reg_value_table[reg_name - REG_MAX8660_OUTPUT_ENABLE_1];
+	return 0;
+}
+int max8660_save_buffered_reg_val(int reg_name, u8 value)
+{
+
+	/* outof range */
+	if (reg_name < REG_MAX8660_OUTPUT_ENABLE_1
+	    || reg_name > REG_MAX8660_FORCE_PWM)
+		return -1;
+	max8660_reg_value_table[reg_name - REG_MAX8660_OUTPUT_ENABLE_1] = value;
+	return 0;
+}
+
+int max8660_write_reg(u8 reg, u8 value)
+{
+	if (max8660_dev_present && (i2c_smbus_write_byte_data(
+		max8660_i2c_client, reg, value) >= 0))
+		return 0;
+	return -1;
+}
+
+/*!
+ * max8660 I2C attach function
+ *
+ * @param adapter            struct i2c_client *
+ * @return  0 for max8660 successfully detected
+ */
+static int max8660_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int retval;
+	max8660_i2c_client = client;
+	retval = i2c_smbus_write_byte_data(max8660_i2c_client,
+				MAX8660_OUTPUT_ENABLE_1, 0);
+	if (retval == 0) {
+		max8660_dev_present = 1;
+		pr_info("max8660 probed !\n");
+	} else {
+		max8660_dev_present = 0;
+		pr_info("max8660 not detected!\n");
+	}
+	return retval;
+}
+
+/*!
+ * max8660 I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  0
+ */
+static int max8660_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id max8660_id[] = {
+	{ "max8660", 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, max8660_id);
+
+static struct i2c_driver max8660_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "max8660",},
+	.probe = max8660_probe,
+	.remove = max8660_remove,
+	.id_table = max8660_id,
+};
+
+/* called by pmic core when init*/
+int max8660_init(void)
+{
+	int err;
+	err = i2c_add_driver(&max8660_i2c_driver);
+	return err;
+}
+void max8660_exit(void)
+{
+	i2c_del_driver(&max8660_i2c_driver);
+}
diff --git a/drivers/mxc/mcu_pmic/max8660.h b/drivers/mxc/mcu_pmic/max8660.h
new file mode 100644
index 000000000000..567784611d80
--- /dev/null
+++ b/drivers/mxc/mcu_pmic/max8660.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright 2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file max8660.h
+ * @brief Driver for max8660
+ *
+ * @ingroup pmic
+ */
+#ifndef _MAX8660_H_
+#define _MAX8660_H_
+
+#ifdef __KERNEL__
+
+#define MAX8660_OUTPUT_ENABLE_1	0x10
+#define MAX8660_OUTPUT_ENABLE_2	0x12
+#define MAX8660_VOLT_CHANGE_CONTROL		0x20
+#define MAX8660_V3_TARGET_VOLT_1	0x23
+#define MAX8660_V3_TARGET_VOLT_2	0x24
+#define MAX8660_V4_TARGET_VOLT_1	0x29
+#define MAX8660_V4_TARGET_VOLT_2	0x2A
+#define MAX8660_V5_TARGET_VOLT_1	0x32
+#define MAX8660_V5_TARGET_VOLT_2	0x33
+#define MAX8660_V6V7_TARGET_VOLT	0x39
+#define MAX8660_FORCE_PWM			0x80
+
+int is_max8660_present(void);
+int max8660_write_reg(u8 reg, u8 value);
+int max8660_save_buffered_reg_val(int reg_name, u8 value);
+int max8660_get_buffered_reg_val(int reg_name, u8 *value);
+int max8660_init(void);
+void max8660_exit(void);
+
+extern int reg_max8660_probe(void);
+extern int reg_max8660_remove(void);
+
+#endif				/* __KERNEL__ */
+
+#endif				/* _MAX8660_H_ */
diff --git a/drivers/mxc/mcu_pmic/mc9s08dz60.c b/drivers/mxc/mcu_pmic/mc9s08dz60.c
new file mode 100644
index 000000000000..1e5da6319b12
--- /dev/null
+++ b/drivers/mxc/mcu_pmic/mc9s08dz60.c
@@ -0,0 +1,197 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+    /*!
+     * @file mc9s08dz60.c
+     * @brief Driver for MC9sdz60
+     *
+     * @ingroup pmic
+     */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/spinlock.h>
+#include <linux/proc_fs.h>
+#include <linux/i2c.h>
+#include <linux/mfd/mc9s08dz60/core.h>
+
+#include <mach/clock.h>
+#include <linux/uaccess.h>
+#include "mc9s08dz60.h"
+
+/* I2C bus id and device address of mcu */
+#define I2C1_BUS	0
+#define MC9S08DZ60_I2C_ADDR	0xD2	/* 7bits I2C address */
+static struct i2c_client *mc9s08dz60_i2c_client;
+
+int mc9s08dz60_read_reg(u8 reg, u8 *value)
+{
+	*value = (u8) i2c_smbus_read_byte_data(mc9s08dz60_i2c_client, reg);
+	return 0;
+}
+
+int mc9s08dz60_write_reg(u8 reg, u8 value)
+{
+	if (i2c_smbus_write_byte_data(mc9s08dz60_i2c_client, reg, value) < 0)
+		return -1;
+	return 0;
+}
+
+static ssize_t mc9s08dz60_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	unsigned int i;
+	u8 value;
+	int offset = 7;
+
+	for (i = 0; i < 7; i++) {
+		mc9s08dz60_read_reg(i, &value);
+		pr_info("reg%02x: %02x\t", i, value);
+		mc9s08dz60_read_reg(i + offset, &value);
+		pr_info("reg%02x: %02x\t", i + offset, value);
+		mc9s08dz60_read_reg(i + offset * 2, &value);
+		pr_info("reg%02x: %02x\t", i + offset * 2, value);
+		mc9s08dz60_read_reg(i + offset * 3, &value);
+		pr_info("reg%02x: %02x\t", i + offset * 3, value);
+		mc9s08dz60_read_reg(i + offset * 4, &value);
+		pr_info("reg%02x: %02x\t", i + offset * 4, value);
+		mc9s08dz60_read_reg(i + offset * 5, &value);
+		pr_info("reg%02x: %02x\n", i + offset * 5, value);
+	}
+
+	return 0;
+}
+
+static ssize_t mc9s08dz60_store(struct device *dev,
+			     struct device_attribute *attr, const char *buf,
+			     size_t count)
+{
+	int ret;
+	unsigned long reg, new_value;
+	u8 value;
+	char *p;
+
+	strict_strtoul(buf, 16, &reg);
+
+	p = NULL;
+	p = memchr(buf, ' ', count);
+
+	if (p == NULL) {
+		mc9s08dz60_read_reg(reg, &value);
+		pr_info("reg%02lu: %06x\n", reg, value);
+		return count;
+	}
+
+	p += 1;
+
+	strict_strtoul(p, 16, &new_value);
+	value = new_value;
+
+	ret = mc9s08dz60_write_reg((u8)reg, value);
+	if (ret == 0)
+		pr_info("write reg%02lx: %06x\n", reg, value);
+	else
+		pr_info("register update failed\n");
+
+	return count;
+}
+
+static struct device_attribute mc9s08dz60_dev_attr = {
+	.attr = {
+		 .name = "mc9s08dz60_ctl",
+		 .mode = S_IRUSR | S_IWUSR,
+		 },
+	.show = mc9s08dz60_show,
+	.store = mc9s08dz60_store,
+};
+
+
+/*!
+ * mc9s08dz60 I2C attach function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  0
+ */
+static int mc9s08dz60_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret = 0;
+	struct mc9s08dz60 *mc9s08dz60 = NULL;
+	struct mc9s08dz60_platform_data *plat_data = client->dev.platform_data;
+	pr_info("mc9s08dz60 probing .... \n");
+
+	mc9s08dz60 = kzalloc(sizeof(struct mc9s08dz60), GFP_KERNEL);
+	if (mc9s08dz60 == NULL)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, mc9s08dz60);
+	mc9s08dz60->dev = &client->dev;
+	mc9s08dz60->i2c_client = client;
+
+	if (plat_data && plat_data->init) {
+		ret = plat_data->init(mc9s08dz60);
+		if (ret != 0)
+			return -1;
+	}
+
+	ret = device_create_file(&client->dev, &mc9s08dz60_dev_attr);
+	if (ret)
+		dev_err(&client->dev, "create device file failed!\n");
+
+
+	mc9s08dz60_i2c_client = client;
+
+	return 0;
+}
+
+/*!
+ * mc9s08dz60 I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  0
+ */
+static int mc9s08dz60_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id mc9s08dz60_id[] = {
+	{ "mc9s08dz60", 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, mc9s08dz60_id);
+
+static struct i2c_driver mc9s08dz60_i2c_driver = {
+	.driver = {.owner = THIS_MODULE,
+		   .name = "mc9s08dz60",
+		   },
+	.probe = mc9s08dz60_probe,
+	.remove = mc9s08dz60_remove,
+	.id_table = mc9s08dz60_id,
+};
+
+#define SET_BIT_IN_BYTE(byte, pos) (byte |= (0x01 << pos))
+#define CLEAR_BIT_IN_BYTE(byte, pos) (byte &= ~(0x01 << pos))
+
+int mc9s08dz60_init(void)
+{
+	int err;
+	err = i2c_add_driver(&mc9s08dz60_i2c_driver);
+	return err;
+}
+void mc9s08dz60_exit(void)
+{
+	i2c_del_driver(&mc9s08dz60_i2c_driver);
+}
diff --git a/drivers/mxc/mcu_pmic/mc9s08dz60.h b/drivers/mxc/mcu_pmic/mc9s08dz60.h
new file mode 100644
index 000000000000..b47b20c79828
--- /dev/null
+++ b/drivers/mxc/mcu_pmic/mc9s08dz60.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc9s08dz60.h
+ * @brief Driver for mc9s08dz60
+ *
+ * @ingroup pmic
+ */
+#ifndef _MC9SDZ60_H_
+#define _MC9SDZ60_H_
+
+#define MCU_VERSION	0x00
+/*#define Reserved	0x01*/
+#define MCU_SECS		0x02
+#define MCU_MINS		0x03
+#define MCU_HRS			0x04
+#define MCU_DAY			0x05
+#define MCU_DATE		0x06
+#define MCU_MONTH		0x07
+#define MCU_YEAR		0x08
+
+#define MCU_ALARM_SECS	0x09
+#define MCU_ALARM_MINS	0x0A
+#define MCU_ALARM_HRS	0x0B
+/* #define Reserved	0x0C*/
+/* #define Reserved	0x0D*/
+#define MCU_TS_CONTROL	0x0E
+#define MCU_X_LOW	0x0F
+#define MCU_Y_LOW	0x10
+#define MCU_XY_HIGH	0x11
+#define MCU_X_LEFT_LOW	0x12
+#define MCU_X_LEFT_HIGH	0x13
+#define MCU_X_RIGHT	0x14
+#define MCU_Y_TOP_LOW	0x15
+#define MCU_Y_TOP_HIGH	0x16
+#define MCU_Y_BOTTOM	0x17
+/* #define Reserved	0x18*/
+/* #define Reserved	0x19*/
+#define MCU_RESET_1	0x1A
+#define MCU_RESET_2	0x1B
+#define MCU_POWER_CTL	0x1C
+#define MCU_DELAY_CONFIG	0x1D
+/* #define Reserved	0x1E */
+/* #define Reserved	0x1F */
+#define MCU_GPIO_1	0x20
+#define MCU_GPIO_2	0x21
+#define MCU_KPD_1	0x22
+#define MCU_KPD_2	0x23
+#define MCU_KPD_CONTROL	0x24
+#define MCU_INT_ENABLE_1	0x25
+#define MCU_INT_ENABLE_2	0x26
+#define MCU_INT_FLAG_1	0x27
+#define MCU_INT_FLAG_2	0x28
+#define MCU_DES_FLAG		0x29
+int mc9s08dz60_read_reg(u8 reg, u8 *value);
+int mc9s08dz60_write_reg(u8 reg, u8 value);
+int mc9s08dz60_init(void);
+void mc9s08dz60_exit(void);
+
+extern int reg_mc9s08dz60_probe(void);
+extern int reg_mc9s08dz60_remove(void);
+
+#endif	/* _MC9SDZ60_H_ */
+
diff --git a/drivers/mxc/mcu_pmic/mcu_pmic_core.c b/drivers/mxc/mcu_pmic/mcu_pmic_core.c
new file mode 100644
index 000000000000..55b2f5fe6e3e
--- /dev/null
+++ b/drivers/mxc/mcu_pmic/mcu_pmic_core.c
@@ -0,0 +1,226 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc9s08dz60/mcu_pmic_core.c
+ * @brief This is the main file of mc9s08dz60 Power Control driver.
+ *
+ * @ingroup PMIC_POWER
+ */
+
+/*
+ * Includes
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/mfd/mc9s08dz60/pmic.h>
+#include <asm/ioctl.h>
+#include <asm/uaccess.h>
+#include <mach/gpio.h>
+
+#include "mcu_pmic_core.h"
+#include "mc9s08dz60.h"
+#include "max8660.h"
+
+/* bitfield macros for mcu pmic*/
+#define SET_BIT_IN_BYTE(byte, pos) (byte |= (0x01 << pos))
+#define CLEAR_BIT_IN_BYTE(byte, pos) (byte &= ~(0x01 << pos))
+
+
+/* map reg names (enum pmic_reg in pmic_external.h) to real addr*/
+const static u8 mcu_pmic_reg_addr_table[] = {
+	MCU_VERSION,
+	MCU_SECS,
+	MCU_MINS,
+	MCU_HRS,
+	MCU_DAY,
+	MCU_DATE,
+	MCU_MONTH,
+	MCU_YEAR,
+	MCU_ALARM_SECS,
+	MCU_ALARM_MINS,
+	MCU_ALARM_HRS,
+	MCU_TS_CONTROL,
+	MCU_X_LOW,
+	MCU_Y_LOW,
+	MCU_XY_HIGH,
+	MCU_X_LEFT_LOW,
+	MCU_X_LEFT_HIGH,
+	MCU_X_RIGHT,
+	MCU_Y_TOP_LOW,
+	MCU_Y_TOP_HIGH,
+	MCU_Y_BOTTOM,
+	MCU_RESET_1,
+	MCU_RESET_2,
+	MCU_POWER_CTL,
+	MCU_DELAY_CONFIG,
+	MCU_GPIO_1,
+	MCU_GPIO_2,
+	MCU_KPD_1,
+	MCU_KPD_2,
+	MCU_KPD_CONTROL,
+	MCU_INT_ENABLE_1,
+	MCU_INT_ENABLE_2,
+	MCU_INT_FLAG_1,
+	MCU_INT_FLAG_2,
+	MCU_DES_FLAG,
+	MAX8660_OUTPUT_ENABLE_1,
+	MAX8660_OUTPUT_ENABLE_2,
+	MAX8660_VOLT_CHANGE_CONTROL,
+	MAX8660_V3_TARGET_VOLT_1,
+	MAX8660_V3_TARGET_VOLT_2,
+	MAX8660_V4_TARGET_VOLT_1,
+	MAX8660_V4_TARGET_VOLT_2,
+	MAX8660_V5_TARGET_VOLT_1,
+	MAX8660_V5_TARGET_VOLT_2,
+	MAX8660_V6V7_TARGET_VOLT,
+	MAX8660_FORCE_PWM
+};
+
+static int mcu_pmic_read(int reg_num, unsigned int *reg_val)
+{
+	int ret;
+	u8 value = 0;
+	/* mcu ops */
+	if (reg_num >= REG_MCU_VERSION && reg_num <= REG_MCU_DES_FLAG)
+		ret = mc9s08dz60_read_reg(mcu_pmic_reg_addr_table[reg_num],
+					&value);
+	else if (reg_num >= REG_MAX8660_OUTPUT_ENABLE_1
+		   && reg_num <= REG_MAX8660_FORCE_PWM)
+		ret = max8660_get_buffered_reg_val(reg_num, &value);
+	else
+		return -1;
+
+	if (ret < 0)
+		return -1;
+	*reg_val = value;
+
+	return 0;
+}
+
+static int mcu_pmic_write(int reg_num, const unsigned int reg_val)
+{
+	int ret;
+	u8 value = reg_val;
+	/* mcu ops */
+	if (reg_num >= REG_MCU_VERSION && reg_num <= REG_MCU_DES_FLAG) {
+
+		ret =
+		    mc9s08dz60_write_reg(
+			mcu_pmic_reg_addr_table[reg_num], value);
+		if (ret < 0)
+			return -1;
+	} else if (reg_num >= REG_MAX8660_OUTPUT_ENABLE_1
+		   && reg_num <= REG_MAX8660_FORCE_PWM) {
+		ret =
+		    max8660_write_reg(mcu_pmic_reg_addr_table[reg_num], value);
+
+		if (ret < 0)
+			return -1;
+
+		ret = max8660_save_buffered_reg_val(reg_num, value);
+	} else
+		return -1;
+
+	return 0;
+}
+
+int mcu_pmic_read_reg(int reg, unsigned int *reg_value,
+			  unsigned int reg_mask)
+{
+	int ret = 0;
+	unsigned int temp = 0;
+
+	ret = mcu_pmic_read(reg, &temp);
+	if (ret != 0)
+		return -1;
+	*reg_value = (temp & reg_mask);
+
+	pr_debug("Read REG[ %d ] = 0x%x\n", reg, *reg_value);
+
+	return ret;
+}
+
+
+int mcu_pmic_write_reg(int reg, unsigned int reg_value,
+			   unsigned int reg_mask)
+{
+	int ret = 0;
+	unsigned int temp = 0;
+
+	ret = mcu_pmic_read(reg, &temp);
+	if (ret != 0)
+		return -1;
+	temp = (temp & (~reg_mask)) | reg_value;
+
+	ret = mcu_pmic_write(reg, temp);
+	if (ret != 0)
+		return -1;
+
+	pr_debug("Write REG[ %d ] = 0x%x\n", reg, reg_value);
+
+	return ret;
+}
+
+/*!
+ * make max8660 - mc9s08dz60 enter low-power mode
+ */
+static void pmic_power_off(void)
+{
+	mcu_pmic_write_reg(REG_MCU_POWER_CTL, 0x10, 0x10);
+}
+
+static int __init mcu_pmic_init(void)
+{
+	int err;
+
+	/* init chips */
+	err = max8660_init();
+	if (err)
+		goto fail1;
+
+	err = mc9s08dz60_init();
+	if (err)
+		goto fail1;
+
+	if (is_max8660_present()) {
+		pr_info("max8660 is present \n");
+		pm_power_off = pmic_power_off;
+	} else
+		pr_debug("max8660 is not present\n");
+	pr_info("mcu_pmic_init completed!\n");
+	return 0;
+
+fail1:
+	pr_err("mcu_pmic_init failed!\n");
+	return err;
+}
+
+static void __exit mcu_pmic_exit(void)
+{
+	reg_max8660_remove();
+	mc9s08dz60_exit();
+	max8660_exit();
+}
+
+subsys_initcall_sync(mcu_pmic_init);
+module_exit(mcu_pmic_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("mcu pmic driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/mcu_pmic/mcu_pmic_core.h b/drivers/mxc/mcu_pmic/mcu_pmic_core.h
new file mode 100644
index 000000000000..9ab07356f8a8
--- /dev/null
+++ b/drivers/mxc/mcu_pmic/mcu_pmic_core.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mcu_pmic_core.h
+ * @brief Driver for max8660
+ *
+ * @ingroup pmic
+ */
+#ifndef _MCU_PMIC_CORE_H_
+#define _MCU_PMIC_CORE_H_
+
+#include <linux/mfd/mc9s08dz60/pmic.h>
+
+#define MAX8660_REG_START (REG_MCU_DES_FLAG + 1)
+enum {
+
+	/* reg names for max8660 */
+	REG_MAX8660_OUTPUT_ENABLE_1 = MAX8660_REG_START,
+	REG_MAX8660_OUTPUT_ENABLE_2,
+	REG_MAX8660_VOLT_CHANGE_CONTROL_1,
+	REG_MAX8660_V3_TARGET_VOLT_1,
+	REG_MAX8660_V3_TARGET_VOLT_2,
+	REG_MAX8660_V4_TARGET_VOLT_1,
+	REG_MAX8660_V4_TARGET_VOLT_2,
+	REG_MAX8660_V5_TARGET_VOLT_1,
+	REG_MAX8660_V5_TARGET_VOLT_2,
+	REG_MAX8660_V6V7_TARGET_VOLT,
+	REG_MAX8660_FORCE_PWM
+};
+
+
+#endif				/* _MCU_PMIC_CORE_H_ */
diff --git a/drivers/mxc/mcu_pmic/mcu_pmic_gpio.c b/drivers/mxc/mcu_pmic/mcu_pmic_gpio.c
new file mode 100644
index 000000000000..bf3a0f97513a
--- /dev/null
+++ b/drivers/mxc/mcu_pmic/mcu_pmic_gpio.c
@@ -0,0 +1,132 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc9s08dz60/mcu_pmic_gpio.c
+ * @brief This is the main file of mc9s08dz60 Power Control driver.
+ *
+ * @ingroup PMIC_POWER
+ */
+
+/*
+ * Includes
+ */
+#include <linux/platform_device.h>
+#include <linux/mfd/mc9s08dz60/pmic.h>
+#include <linux/pmic_status.h>
+#include <linux/ioctl.h>
+
+#define SET_BIT_IN_BYTE(byte, pos) (byte |= (0x01 << pos))
+#define CLEAR_BIT_IN_BYTE(byte, pos) (byte &= ~(0x01 << pos))
+
+int pmic_gpio_set_bit_val(int reg, unsigned int bit,
+				  unsigned int val)
+{
+	int reg_name;
+	u8 reg_mask = 0;
+
+	if (bit > 7)
+		return -1;
+
+	switch (reg) {
+	case MCU_GPIO_REG_RESET_1:
+		reg_name = REG_MCU_RESET_1;
+		break;
+	case MCU_GPIO_REG_RESET_2:
+		reg_name = REG_MCU_RESET_2;
+		break;
+	case MCU_GPIO_REG_POWER_CONTROL:
+		reg_name = REG_MCU_POWER_CTL;
+		break;
+	case MCU_GPIO_REG_GPIO_CONTROL_1:
+		reg_name = REG_MCU_GPIO_1;
+		break;
+	case MCU_GPIO_REG_GPIO_CONTROL_2:
+		reg_name = REG_MCU_GPIO_2;
+		break;
+	default:
+		return -1;
+	}
+
+	SET_BIT_IN_BYTE(reg_mask, bit);
+	if (0 == val)
+		CHECK_ERROR(mcu_pmic_write_reg(reg_name, 0, reg_mask));
+	else
+		CHECK_ERROR(mcu_pmic_write_reg(reg_name, reg_mask, reg_mask));
+
+	return 0;
+}
+EXPORT_SYMBOL(pmic_gpio_set_bit_val);
+
+int pmic_gpio_get_bit_val(int reg, unsigned int bit,
+				  unsigned int *val)
+{
+	int reg_name;
+	unsigned int reg_read_val;
+	u8 reg_mask = 0;
+
+	if (bit > 7)
+		return -1;
+
+	switch (reg) {
+	case MCU_GPIO_REG_RESET_1:
+		reg_name = REG_MCU_RESET_1;
+		break;
+	case MCU_GPIO_REG_RESET_2:
+		reg_name = REG_MCU_RESET_2;
+		break;
+	case MCU_GPIO_REG_POWER_CONTROL:
+		reg_name = REG_MCU_POWER_CTL;
+		break;
+	case MCU_GPIO_REG_GPIO_CONTROL_1:
+		reg_name = REG_MCU_GPIO_1;
+		break;
+	case MCU_GPIO_REG_GPIO_CONTROL_2:
+		reg_name = REG_MCU_GPIO_2;
+		break;
+	default:
+		return -1;
+	}
+
+	SET_BIT_IN_BYTE(reg_mask, bit);
+	CHECK_ERROR(mcu_pmic_read_reg(reg_name, &reg_read_val, reg_mask));
+	if (0 == reg_read_val)
+		*val = 0;
+	else
+		*val = 1;
+
+	return 0;
+}
+EXPORT_SYMBOL(pmic_gpio_get_bit_val);
+
+int pmic_gpio_get_designation_bit_val(unsigned int bit,
+					unsigned int *val)
+{
+	unsigned int reg_read_val;
+	u8 reg_mask = 0;
+
+	if (bit > 7)
+		return -1;
+
+	SET_BIT_IN_BYTE(reg_mask, bit);
+	CHECK_ERROR(
+		mcu_pmic_read_reg(REG_MCU_DES_FLAG, &reg_read_val, reg_mask));
+	if (0 == reg_read_val)
+		*val = 0;
+	else
+		*val = 1;
+
+	return 0;
+}
+EXPORT_SYMBOL(pmic_gpio_get_designation_bit_val);
+
diff --git a/drivers/mxc/mlb/Kconfig b/drivers/mxc/mlb/Kconfig
new file mode 100644
index 000000000000..294c9776fb4d
--- /dev/null
+++ b/drivers/mxc/mlb/Kconfig
@@ -0,0 +1,13 @@
+#
+# MLB configuration
+#
+
+menu "MXC Media Local Bus Driver"
+
+config MXC_MLB
+	tristate "MLB support"
+	depends on ARCH_MX35
+	---help---
+         Say Y to get the MLB support.
+
+endmenu
diff --git a/drivers/mxc/mlb/Makefile b/drivers/mxc/mlb/Makefile
new file mode 100644
index 000000000000..60662eb1c031
--- /dev/null
+++ b/drivers/mxc/mlb/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for the kernel MLB driver
+#
+
+obj-$(CONFIG_MXC_MLB) += mxc_mlb.o
diff --git a/drivers/mxc/mlb/mxc_mlb.c b/drivers/mxc/mlb/mxc_mlb.c
new file mode 100644
index 000000000000..b170c925dbd1
--- /dev/null
+++ b/drivers/mxc/mlb/mxc_mlb.c
@@ -0,0 +1,1049 @@
+/*
+ * linux/drivers/mxc/mlb/mxc_mlb.c
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/poll.h>
+#include <linux/cdev.h>
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/mxc_mlb.h>
+
+#include <asm/irq.h>
+#include <asm/uaccess.h>
+
+/*!
+ * MLB module memory map registers define
+ */
+#define MLB_REG_DCCR	0x0
+#define MLB_REG_SSCR	0x4
+#define MLB_REG_SDCR	0x8
+#define MLB_REG_SMCR	0xC
+#define MLB_REG_VCCR	0x1C
+#define MLB_REG_SBCR	0x20
+#define MLB_REG_ABCR	0x24
+#define MLB_REG_CBCR	0x28
+#define MLB_REG_IBCR	0x2C
+#define MLB_REG_CICR	0x30
+#define MLB_REG_CECRn	0x40
+#define MLB_REG_CSCRn	0x44
+#define MLB_REG_CCBCRn	0x48
+#define MLB_REG_CNBCRn	0x4C
+#define MLB_REG_LCBCRn	0x280
+
+#define MLB_DCCR_FS_OFFSET	28
+#define MLB_DCCR_EN		(1 << 31)
+#define MLB_DCCR_LBM_OFFSET	30
+#define MLB_DCCR_RESET		(1 << 23)
+#define MLB_CECR_CE		(1 << 31)
+#define MLB_CECR_TR		(1 << 30)
+#define MLB_CECR_CT_OFFSET	28
+#define MLB_CECR_MBS		(1 << 19)
+#define MLB_CSCR_CBPE		(1 << 0)
+#define MLB_CSCR_CBDB		(1 << 1)
+#define MLB_CSCR_CBD		(1 << 2)
+#define MLB_CSCR_CBS		(1 << 3)
+#define MLB_CSCR_BE		(1 << 4)
+#define MLB_CSCR_ABE		(1 << 5)
+#define MLB_CSCR_LFS		(1 << 6)
+#define MLB_CSCR_PBPE		(1 << 8)
+#define MLB_CSCR_PBDB		(1 << 9)
+#define MLB_CSCR_PBD		(1 << 10)
+#define MLB_CSCR_PBS		(1 << 11)
+#define MLB_CSCR_RDY		(1 << 16)
+#define MLB_CSCR_BM		(1 << 31)
+#define MLB_CSCR_BF		(1 << 30)
+#define MLB_SSCR_SDML		(1 << 5)
+
+#define MLB_CONTROL_TX_CHANN		(0 << 4)
+#define MLB_CONTROL_RX_CHANN		(1 << 4)
+#define MLB_ASYNC_TX_CHANN		(2 << 4)
+#define MLB_ASYNC_RX_CHANN		(3 << 4)
+
+#define MLB_MINOR_DEVICES		2
+#define MLB_CONTROL_DEV_NAME		"ctrl"
+#define MLB_ASYNC_DEV_NAME		"async"
+
+#define TX_CHANNEL			0
+#define RX_CHANNEL			1
+#define TX_CHANNEL_BUF_SIZE		1024
+#define RX_CHANNEL_BUF_SIZE		2*1024
+/* max package data size */
+#define ASYNC_PACKET_SIZE			1024
+#define CTRL_PACKET_SIZE			64
+#define RX_RING_NODES			10
+
+#define _get_txchan(dev)		mlb_devinfo[dev].channels[TX_CHANNEL]
+#define _get_rxchan(dev)		mlb_devinfo[dev].channels[RX_CHANNEL]
+
+enum {
+	MLB_CTYPE_SYNC,
+	MLB_CTYPE_ISOC,
+	MLB_CTYPE_ASYNC,
+	MLB_CTYPE_CTRL,
+};
+
+/*!
+ * Rx ring buffer
+ */
+struct mlb_rx_ringnode {
+	int size;
+	char *data;
+};
+
+struct mlb_channel_info {
+
+	/* channel offset in memmap */
+	const unsigned int reg_offset;
+	/* channel address */
+	int address;
+	/*!
+	 * channel buffer start address
+	 * for Rx, buf_head pointer to a loop ring buffer
+	 */
+	unsigned long buf_head;
+	/* physical buffer head address */
+	unsigned long phy_head;
+	/* channel buffer size */
+	unsigned int buf_size;
+	/* channel buffer current ptr */
+	unsigned long buf_ptr;
+	/* buffer spin lock */
+	rwlock_t buf_lock;
+};
+
+struct mlb_dev_info {
+
+	/* device node name */
+	const char dev_name[20];
+	/* channel type */
+	const unsigned int channel_type;
+	/* channel info for tx/rx */
+	struct mlb_channel_info channels[2];
+	/* rx ring buffer */
+	struct mlb_rx_ringnode rx_bufs[RX_RING_NODES];
+	/* rx ring buffer read/write ptr */
+	unsigned int rdpos, wtpos;
+	/* exception event */
+	unsigned long ex_event;
+	/* channel started up or not */
+	atomic_t on;
+	/* device open count */
+	atomic_t opencnt;
+	/* wait queue head for channel */
+	wait_queue_head_t rd_wq;
+	wait_queue_head_t wt_wq;
+	/* spinlock for event access */
+	spinlock_t event_lock;
+};
+
+static struct mlb_dev_info mlb_devinfo[MLB_MINOR_DEVICES] = {
+	{
+	 .dev_name = MLB_CONTROL_DEV_NAME,
+	 .channel_type = MLB_CTYPE_CTRL,
+	 .channels = {
+		      [0] = {
+			     .reg_offset = MLB_CONTROL_TX_CHANN,
+			     .buf_size = TX_CHANNEL_BUF_SIZE,
+			     .buf_lock =
+			     __RW_LOCK_UNLOCKED(mlb_devinfo[0].channels[0].
+						buf_lock),
+			     },
+		      [1] = {
+			     .reg_offset = MLB_CONTROL_RX_CHANN,
+			     .buf_size = RX_CHANNEL_BUF_SIZE,
+			     .buf_lock =
+			     __RW_LOCK_UNLOCKED(mlb_devinfo[0].channels[1].
+						buf_lock),
+			     },
+		      },
+	 .on = ATOMIC_INIT(0),
+	 .opencnt = ATOMIC_INIT(0),
+	 .rd_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[0].rd_wq),
+	 .wt_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[0].wt_wq),
+	 .event_lock = __SPIN_LOCK_UNLOCKED(mlb_devinfo[0].event_lock),
+	 },
+	{
+	 .dev_name = MLB_ASYNC_DEV_NAME,
+	 .channel_type = MLB_CTYPE_ASYNC,
+	 .channels = {
+		      [0] = {
+			     .reg_offset = MLB_ASYNC_TX_CHANN,
+			     .buf_size = TX_CHANNEL_BUF_SIZE,
+			     .buf_lock =
+			     __RW_LOCK_UNLOCKED(mlb_devinfo[1].channels[0].
+						buf_lock),
+			     },
+		      [1] = {
+			     .reg_offset = MLB_ASYNC_RX_CHANN,
+			     .buf_size = RX_CHANNEL_BUF_SIZE,
+			     .buf_lock =
+			     __RW_LOCK_UNLOCKED(mlb_devinfo[1].channels[1].
+						buf_lock),
+			     },
+		      },
+	 .on = ATOMIC_INIT(0),
+	 .opencnt = ATOMIC_INIT(0),
+	 .rd_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[1].rd_wq),
+	 .wt_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[1].wt_wq),
+	 .event_lock = __SPIN_LOCK_UNLOCKED(mlb_devinfo[1].event_lock),
+	 },
+};
+
+static struct regulator *reg_nvcc;	/* NVCC_MLB regulator */
+static struct clk *mlb_clk;
+static struct cdev mxc_mlb_dev;	/* chareset device */
+static dev_t dev;
+static struct class *mlb_class;	/* device class */
+static struct device *class_dev;
+static unsigned long mlb_base;	/* mlb module base address */
+static unsigned int irq;
+
+/*!
+ * Initial the MLB module device
+ */
+static void mlb_dev_init(void)
+{
+	unsigned long dccr_val;
+	unsigned long phyaddr;
+
+	/* reset the MLB module */
+	__raw_writel(MLB_DCCR_RESET, mlb_base + MLB_REG_DCCR);
+	while (__raw_readl(mlb_base + MLB_REG_DCCR)
+	       & MLB_DCCR_RESET) ;
+
+	/*!
+	 * Enable MLB device, disable loopback mode,
+	 * set default fps to 512, set mlb device address to 0
+	 */
+	dccr_val = MLB_DCCR_EN;
+	__raw_writel(dccr_val, mlb_base + MLB_REG_DCCR);
+
+	/* disable all the system interrupt */
+	__raw_writel(0x5F, mlb_base + MLB_REG_SMCR);
+
+	/* write async, control tx/rx base address */
+	phyaddr = _get_txchan(0).phy_head >> 16;
+	__raw_writel(phyaddr << 16 | phyaddr, mlb_base + MLB_REG_CBCR);
+	phyaddr = _get_txchan(1).phy_head >> 16;
+	__raw_writel(phyaddr << 16 | phyaddr, mlb_base + MLB_REG_ABCR);
+
+}
+
+static void mlb_dev_exit(void)
+{
+	__raw_writel(0, mlb_base + MLB_REG_DCCR);
+}
+
+/*!
+ * MLB receive start function
+ *
+ * load phy_head to next buf register to start next rx
+ * here use single-packet buffer, set start=end
+ */
+static void mlb_start_rx(int cdev_id)
+{
+	struct mlb_channel_info *chinfo = &_get_rxchan(cdev_id);
+	unsigned long next;
+
+	next = chinfo->phy_head & 0xFFFC;
+	/* load next buf */
+	__raw_writel((next << 16) | next, mlb_base +
+		     MLB_REG_CNBCRn + chinfo->reg_offset);
+	/* set ready bit to start next rx */
+	__raw_writel(MLB_CSCR_RDY, mlb_base + MLB_REG_CSCRn
+		     + chinfo->reg_offset);
+}
+
+/*!
+ * MLB transmit start function
+ * make sure aquiring the rw buf_lock, when calling this
+ */
+static void mlb_start_tx(int cdev_id)
+{
+	struct mlb_channel_info *chinfo = &_get_txchan(cdev_id);
+	unsigned long begin, end;
+
+	begin = chinfo->phy_head;
+	end = (chinfo->phy_head + chinfo->buf_ptr - chinfo->buf_head) & 0xFFFC;
+	/* load next buf */
+	__raw_writel((begin << 16) | end, mlb_base +
+		     MLB_REG_CNBCRn + chinfo->reg_offset);
+	/* set ready bit to start next tx */
+	__raw_writel(MLB_CSCR_RDY, mlb_base + MLB_REG_CSCRn
+		     + chinfo->reg_offset);
+}
+
+/*!
+ * Enable the MLB channel
+ */
+static void mlb_channel_enable(int chan_dev_id, int on)
+{
+	unsigned long tx_regval = 0, rx_regval = 0;
+	/*!
+	 * setup the direction, enable, channel type,
+	 * mode select, channel address and mask buf start
+	 */
+	if (on) {
+		unsigned int ctype = mlb_devinfo[chan_dev_id].channel_type;
+		tx_regval = MLB_CECR_CE | MLB_CECR_TR | MLB_CECR_MBS |
+		    (ctype << MLB_CECR_CT_OFFSET) |
+		    _get_txchan(chan_dev_id).address;
+		rx_regval = MLB_CECR_CE | MLB_CECR_MBS |
+		    (ctype << MLB_CECR_CT_OFFSET) |
+		    _get_rxchan(chan_dev_id).address;
+
+		atomic_set(&mlb_devinfo[chan_dev_id].on, 1);
+	} else {
+		atomic_set(&mlb_devinfo[chan_dev_id].on, 0);
+	}
+
+	/* update the rx/tx channel entry config */
+	__raw_writel(tx_regval, mlb_base + MLB_REG_CECRn +
+		     _get_txchan(chan_dev_id).reg_offset);
+	__raw_writel(rx_regval, mlb_base + MLB_REG_CECRn +
+		     _get_rxchan(chan_dev_id).reg_offset);
+
+	if (on)
+		mlb_start_rx(chan_dev_id);
+}
+
+/*!
+ * MLB interrupt handler
+ */
+void mlb_tx_isr(int minor, unsigned int cis)
+{
+	struct mlb_channel_info *chinfo = &_get_txchan(minor);
+
+	if (cis & MLB_CSCR_CBD) {
+		/* buffer done, reset the buf_ptr */
+		write_lock(&chinfo->buf_lock);
+		chinfo->buf_ptr = chinfo->buf_head;
+		write_unlock(&chinfo->buf_lock);
+		/* wake up the writer */
+		wake_up_interruptible(&mlb_devinfo[minor].wt_wq);
+	}
+}
+
+void mlb_rx_isr(int minor, unsigned int cis)
+{
+	struct mlb_channel_info *chinfo = &_get_rxchan(minor);
+	unsigned long end;
+	unsigned int len;
+
+	if (cis & MLB_CSCR_CBD) {
+
+		int wpos, rpos;
+
+		rpos = mlb_devinfo[minor].rdpos;
+		wpos = mlb_devinfo[minor].wtpos;
+
+		/* buffer done, get current buffer ptr */
+		end =
+		    __raw_readl(mlb_base + MLB_REG_CCBCRn + chinfo->reg_offset);
+		end >>= 16;	/* end here is phy */
+		len = end - (chinfo->phy_head & 0xFFFC);
+
+		/*!
+		 * copy packet from IRAM buf to ring buf.
+		 * if the wpos++ == rpos, drop this packet
+		 */
+		if (((wpos + 1) % RX_RING_NODES) != rpos) {
+
+#ifdef DEBUG
+			if (mlb_devinfo[minor].channel_type == MLB_CTYPE_CTRL) {
+				if (len > CTRL_PACKET_SIZE)
+					pr_debug
+					    ("mxc_mlb: ctrl packet"
+					     "overflow\n");
+			} else {
+				if (len > ASYNC_PACKET_SIZE)
+					pr_debug
+					    ("mxc_mlb: async packet"
+					     "overflow\n");
+			}
+#endif
+			memcpy(mlb_devinfo[minor].rx_bufs[wpos].data,
+			       (const void *)chinfo->buf_head, len);
+			mlb_devinfo[minor].rx_bufs[wpos].size = len;
+
+			/* update the ring wpos */
+			mlb_devinfo[minor].wtpos = (wpos + 1) % RX_RING_NODES;
+
+			/* wake up the reader */
+			wake_up_interruptible(&mlb_devinfo[minor].rd_wq);
+
+			pr_debug("recv package, len:%d, rdpos: %d, wtpos: %d\n",
+				 len, rpos, mlb_devinfo[minor].wtpos);
+		} else {
+			pr_debug
+			    ("drop package, due to no space, (%d,%d)\n",
+			     rpos, mlb_devinfo[minor].wtpos);
+		}
+
+		/* start next rx */
+		mlb_start_rx(minor);
+	}
+}
+
+static irqreturn_t mlb_isr(int irq, void *dev_id)
+{
+	unsigned long int_status, sscr, tx_cis, rx_cis;
+	struct mlb_dev_info *pdev;
+	int minor;
+
+	sscr = __raw_readl(mlb_base + MLB_REG_SSCR);
+	pr_debug("mxc_mlb: system interrupt:%lx\n", sscr);
+	__raw_writel(0x7F, mlb_base + MLB_REG_SSCR);
+
+	int_status = __raw_readl(mlb_base + MLB_REG_CICR) & 0xFFFF;
+	pr_debug("mxc_mlb: channel interrupt ids: %lx\n", int_status);
+
+	for (minor = 0; minor < MLB_MINOR_DEVICES; minor++) {
+
+		pdev = &mlb_devinfo[minor];
+		tx_cis = rx_cis = 0;
+
+		/* get tx channel interrupt status */
+		if (int_status & (1 << (_get_txchan(minor).reg_offset >> 4)))
+			tx_cis = __raw_readl(mlb_base + MLB_REG_CSCRn
+					     + _get_txchan(minor).reg_offset);
+		/* get rx channel interrupt status */
+		if (int_status & (1 << (_get_rxchan(minor).reg_offset >> 4)))
+			rx_cis = __raw_readl(mlb_base + MLB_REG_CSCRn
+					     + _get_rxchan(minor).reg_offset);
+
+		if (!tx_cis && !rx_cis)
+			continue;
+
+		pr_debug("tx/rx int status: 0x%08lx/0x%08lx\n", tx_cis, rx_cis);
+		/* fill exception event */
+		spin_lock(&pdev->event_lock);
+		pdev->ex_event |= tx_cis & 0x303;
+		pdev->ex_event |= (rx_cis & 0x303) << 16;
+		spin_unlock(&pdev->event_lock);
+
+		/* clear the interrupt status */
+		__raw_writel(tx_cis & 0xFFFF, mlb_base + MLB_REG_CSCRn
+			     + _get_txchan(minor).reg_offset);
+		__raw_writel(rx_cis & 0xFFFF, mlb_base + MLB_REG_CSCRn
+			     + _get_rxchan(minor).reg_offset);
+
+		/* handel tx channel */
+		if (tx_cis)
+			mlb_tx_isr(minor, tx_cis);
+		/* handle rx channel */
+		if (rx_cis)
+			mlb_rx_isr(minor, rx_cis);
+
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int mxc_mlb_open(struct inode *inode, struct file *filp)
+{
+	int minor;
+
+	minor = MINOR(inode->i_rdev);
+
+	if (minor < 0 || minor >= MLB_MINOR_DEVICES)
+		return -ENODEV;
+
+	/* open for each channel device */
+	if (atomic_cmpxchg(&mlb_devinfo[minor].opencnt, 0, 1) != 0)
+		return -EBUSY;
+
+	/* reset the buffer read/write ptr */
+	_get_txchan(minor).buf_ptr = _get_txchan(minor).buf_head;
+	_get_rxchan(minor).buf_ptr = _get_rxchan(minor).buf_head;
+	mlb_devinfo[minor].rdpos = mlb_devinfo[minor].wtpos = 0;
+	mlb_devinfo[minor].ex_event = 0;
+
+	return 0;
+}
+
+static int mxc_mlb_release(struct inode *inode, struct file *filp)
+{
+	int minor;
+
+	minor = MINOR(inode->i_rdev);
+
+	/* clear channel settings and info */
+	mlb_channel_enable(minor, 0);
+
+	/* decrease the open count */
+	atomic_set(&mlb_devinfo[minor].opencnt, 0);
+
+	return 0;
+}
+
+static int mxc_mlb_ioctl(struct inode *inode, struct file *filp,
+			 unsigned int cmd, unsigned long arg)
+{
+	void __user *argp = (void __user *)arg;
+	unsigned long flags, event;
+	int minor;
+
+	minor = MINOR(inode->i_rdev);
+
+	switch (cmd) {
+
+	case MLB_CHAN_SETADDR:
+		{
+			unsigned int caddr;
+			/* get channel address from user space */
+			if (copy_from_user(&caddr, argp, sizeof(caddr))) {
+				pr_err("mxc_mlb: copy from user failed\n");
+				return -EFAULT;
+			}
+			_get_txchan(minor).address = (caddr >> 16) & 0xFFFF;
+			_get_rxchan(minor).address = caddr & 0xFFFF;
+			break;
+		}
+
+	case MLB_CHAN_STARTUP:
+		if (atomic_read(&mlb_devinfo[minor].on)) {
+			pr_debug("mxc_mlb: channel areadly startup\n");
+			break;
+		}
+		mlb_channel_enable(minor, 1);
+		break;
+	case MLB_CHAN_SHUTDOWN:
+		if (atomic_read(&mlb_devinfo[minor].on) == 0) {
+			pr_debug("mxc_mlb: channel areadly shutdown\n");
+			break;
+		}
+		mlb_channel_enable(minor, 0);
+		break;
+	case MLB_CHAN_GETEVENT:
+		/* get and clear the ex_event */
+		spin_lock_irqsave(&mlb_devinfo[minor].event_lock, flags);
+		event = mlb_devinfo[minor].ex_event;
+		mlb_devinfo[minor].ex_event = 0;
+		spin_unlock_irqrestore(&mlb_devinfo[minor].event_lock, flags);
+
+		if (event) {
+			if (copy_to_user(argp, &event, sizeof(event))) {
+				pr_err("mxc_mlb: copy to user failed\n");
+				return -EFAULT;
+			}
+		} else {
+			pr_debug("mxc_mlb: no exception event now\n");
+			return -EAGAIN;
+		}
+		break;
+	case MLB_SET_FPS:
+		{
+			unsigned int fps;
+			unsigned long dccr_val;
+
+			/* get fps from user space */
+			if (copy_from_user(&fps, argp, sizeof(fps))) {
+				pr_err("mxc_mlb: copy from user failed\n");
+				return -EFAULT;
+			}
+
+			/* check fps value */
+			if (fps != 256 && fps != 512 && fps != 1024) {
+				pr_debug("mxc_mlb: invalid fps argument\n");
+				return -EINVAL;
+			}
+
+			dccr_val = __raw_readl(mlb_base + MLB_REG_DCCR);
+			dccr_val &= ~(0x3 << MLB_DCCR_FS_OFFSET);
+			dccr_val |= (fps >> 9) << MLB_DCCR_FS_OFFSET;
+			__raw_writel(dccr_val, mlb_base + MLB_REG_DCCR);
+			break;
+		}
+
+	case MLB_GET_VER:
+		{
+			unsigned long version;
+
+			/* get MLB device module version */
+			version = __raw_readl(mlb_base + MLB_REG_VCCR);
+
+			if (copy_to_user(argp, &version, sizeof(version))) {
+				pr_err("mxc_mlb: copy to user failed\n");
+				return -EFAULT;
+			}
+			break;
+		}
+
+	case MLB_SET_DEVADDR:
+		{
+			unsigned long dccr_val;
+			unsigned char devaddr;
+
+			/* get MLB device address from user space */
+			if (copy_from_user
+			    (&devaddr, argp, sizeof(unsigned char))) {
+				pr_err("mxc_mlb: copy from user failed\n");
+				return -EFAULT;
+			}
+
+			dccr_val = __raw_readl(mlb_base + MLB_REG_DCCR);
+			dccr_val &= ~0xFF;
+			dccr_val |= devaddr;
+			__raw_writel(dccr_val, mlb_base + MLB_REG_DCCR);
+
+			break;
+		}
+	default:
+		pr_info("mxc_mlb: Invalid ioctl command\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*!
+ * MLB read routine
+ *
+ * Read the current received data from queued buffer,
+ * and free this buffer for hw to fill ingress data.
+ */
+static ssize_t mxc_mlb_read(struct file *filp, char __user *buf,
+			    size_t count, loff_t *f_pos)
+{
+	int minor, ret;
+	int size, rdpos;
+	struct mlb_rx_ringnode *rxbuf;
+
+	minor = MINOR(filp->f_dentry->d_inode->i_rdev);
+
+	rdpos = mlb_devinfo[minor].rdpos;
+	rxbuf = mlb_devinfo[minor].rx_bufs;
+
+	/* check the current rx buffer is available or not */
+	if (rdpos == mlb_devinfo[minor].wtpos) {
+		if (filp->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+		/* if !O_NONBLOCK, we wait for recv packet */
+		ret = wait_event_interruptible(mlb_devinfo[minor].rd_wq,
+						(mlb_devinfo[minor].wtpos !=
+						rdpos));
+		if (ret < 0)
+			return ret;
+	}
+
+	size = rxbuf[rdpos].size;
+	if (size > count) {
+		/* the user buffer is too small */
+		pr_warning
+		    ("mxc_mlb: received data size is bigger than count\n");
+		return -EINVAL;
+	}
+
+	/* copy rx buffer data to user buffer */
+	if (copy_to_user(buf, rxbuf[rdpos].data, size)) {
+		pr_err("mxc_mlb: copy from user failed\n");
+		return -EFAULT;
+	}
+
+	/* update the read ptr */
+	mlb_devinfo[minor].rdpos = (rdpos + 1) % RX_RING_NODES;
+
+	*f_pos = 0;
+
+	return size;
+}
+
+/*!
+ * MLB write routine
+ *
+ * Copy the user data to tx channel buffer,
+ * and prepare the channel current/next buffer ptr.
+ */
+static ssize_t mxc_mlb_write(struct file *filp, const char __user *buf,
+			     size_t count, loff_t *f_pos)
+{
+	int minor;
+	unsigned long flags;
+	DEFINE_WAIT(__wait);
+	int ret;
+
+	minor = MINOR(filp->f_dentry->d_inode->i_rdev);
+
+	if (count > _get_txchan(minor).buf_size) {
+		/* too many data to write */
+		pr_warning("mxc_mlb: overflow write data\n");
+		return -EFBIG;
+	}
+
+	*f_pos = 0;
+
+	/* check the current tx buffer is used or not */
+	write_lock_irqsave(&_get_txchan(minor).buf_lock, flags);
+	if (_get_txchan(minor).buf_ptr != _get_txchan(minor).buf_head) {
+		write_unlock_irqrestore(&_get_txchan(minor).buf_lock, flags);
+
+		/* there's already some datas being transmit now */
+		if (filp->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+
+		/* if !O_NONBLOCK, we wait for transmit finish */
+		for (;;) {
+			prepare_to_wait(&mlb_devinfo[minor].wt_wq,
+					&__wait, TASK_INTERRUPTIBLE);
+
+			write_lock_irqsave(&_get_txchan(minor).buf_lock, flags);
+			if (_get_txchan(minor).buf_ptr ==
+			    _get_txchan(minor).buf_head)
+				break;
+
+			write_unlock_irqrestore(&_get_txchan(minor).buf_lock,
+						flags);
+			if (!signal_pending(current)) {
+				schedule();
+				continue;
+			}
+			return -ERESTARTSYS;
+		}
+		finish_wait(&mlb_devinfo[minor].wt_wq, &__wait);
+	}
+
+	/* copy user buffer to tx buffer */
+	if (copy_from_user((void *)_get_txchan(minor).buf_ptr, buf, count)) {
+		pr_err("mxc_mlb: copy from user failed\n");
+		ret = -EFAULT;
+		goto out;
+	}
+	_get_txchan(minor).buf_ptr += count;
+
+	/* set current/next buffer start/end */
+	mlb_start_tx(minor);
+
+	ret = count;
+
+out:
+	write_unlock_irqrestore(&_get_txchan(minor).buf_lock, flags);
+	return ret;
+}
+
+static unsigned int mxc_mlb_poll(struct file *filp,
+				 struct poll_table_struct *wait)
+{
+	int minor;
+	unsigned int ret = 0;
+	unsigned long flags;
+
+	minor = MINOR(filp->f_dentry->d_inode->i_rdev);
+
+	poll_wait(filp, &mlb_devinfo[minor].rd_wq, wait);
+	poll_wait(filp, &mlb_devinfo[minor].wt_wq, wait);
+
+	/* check the tx buffer is avaiable or not */
+	read_lock_irqsave(&_get_txchan(minor).buf_lock, flags);
+	if (_get_txchan(minor).buf_ptr == _get_txchan(minor).buf_head)
+		ret |= POLLOUT | POLLWRNORM;
+	read_unlock_irqrestore(&_get_txchan(minor).buf_lock, flags);
+
+	/* check the rx buffer filled or not */
+	if (mlb_devinfo[minor].rdpos != mlb_devinfo[minor].wtpos)
+		ret |= POLLIN | POLLRDNORM;
+
+	/* check the exception event */
+	if (mlb_devinfo[minor].ex_event)
+		ret |= POLLIN | POLLRDNORM;
+
+	return ret;
+}
+
+/*!
+ * char dev file operations structure
+ */
+static struct file_operations mxc_mlb_fops = {
+
+	.owner = THIS_MODULE,
+	.open = mxc_mlb_open,
+	.release = mxc_mlb_release,
+	.ioctl = mxc_mlb_ioctl,
+	.poll = mxc_mlb_poll,
+	.read = mxc_mlb_read,
+	.write = mxc_mlb_write,
+};
+
+/*!
+ * This function is called whenever the MLB device is detected.
+ */
+static int __devinit mxc_mlb_probe(struct platform_device *pdev)
+{
+	int ret, mlb_major, i, j;
+	struct mxc_mlb_platform_data *plat_data;
+	struct resource *res;
+	void __iomem *base;
+	unsigned long bufaddr, phyaddr;
+
+	/* malloc the Rx ring buffer firstly */
+	for (i = 0; i < MLB_MINOR_DEVICES; i++) {
+		char *buf;
+		int bufsize;
+
+		if (mlb_devinfo[i].channel_type == MLB_CTYPE_ASYNC)
+			bufsize = ASYNC_PACKET_SIZE;
+		else
+			bufsize = CTRL_PACKET_SIZE;
+
+		buf = kmalloc(bufsize * RX_RING_NODES, GFP_KERNEL);
+		if (buf == NULL) {
+			ret = -ENOMEM;
+			dev_err(&pdev->dev, "can not alloc rx buffers\n");
+			goto err4;
+		}
+		for (j = 0; j < RX_RING_NODES; j++) {
+			mlb_devinfo[i].rx_bufs[j].data = buf;
+			buf += bufsize;
+		}
+	}
+
+	/**
+	 * Register MLB lld as two character devices
+	 * One for Packet date channel, the other for control data channel
+	 */
+	ret = alloc_chrdev_region(&dev, 0, MLB_MINOR_DEVICES, "mxc_mlb");
+	mlb_major = MAJOR(dev);
+
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can't get major %d\n", mlb_major);
+		goto err3;
+	}
+
+	cdev_init(&mxc_mlb_dev, &mxc_mlb_fops);
+	mxc_mlb_dev.owner = THIS_MODULE;
+
+	ret = cdev_add(&mxc_mlb_dev, dev, MLB_MINOR_DEVICES);
+	if (ret) {
+		dev_err(&pdev->dev, "can't add cdev\n");
+		goto err2;
+	}
+
+	/* create class and device for udev information */
+	mlb_class = class_create(THIS_MODULE, "mlb");
+	if (IS_ERR(mlb_class)) {
+		dev_err(&pdev->dev, "failed to create mlb class\n");
+		ret = -ENOMEM;
+		goto err2;
+	}
+
+	for (i = 0; i < MLB_MINOR_DEVICES; i++) {
+
+		class_dev = device_create(mlb_class, NULL, MKDEV(mlb_major, i),
+					  NULL, mlb_devinfo[i].dev_name);
+		if (IS_ERR(class_dev)) {
+			dev_err(&pdev->dev, "failed to create mlb %s"
+				" class device\n", mlb_devinfo[i].dev_name);
+			ret = -ENOMEM;
+			goto err1;
+		}
+	}
+
+	/* get irq line */
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "No mlb irq line provided\n");
+		goto err1;
+	}
+
+	irq = res->start;
+	/* request irq */
+	if (request_irq(irq, mlb_isr, 0, "mlb", NULL)) {
+		dev_err(&pdev->dev, "failed to request irq\n");
+		ret = -EBUSY;
+		goto err1;
+	}
+
+	/* ioremap from phy mlb to kernel space */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "No mlb base address provided\n");
+		goto err0;
+	}
+
+	base = ioremap(res->start, res->end - res->start);
+	dev_dbg(&pdev->dev, "mapped mlb base address: 0x%08x\n",
+		(unsigned int)base);
+
+	if (base == NULL) {
+		dev_err(&pdev->dev, "failed to do ioremap with mlb base\n");
+		goto err0;
+	}
+	mlb_base = (unsigned long)base;
+
+	/*!
+	 * get rx/tx buffer address from platform data
+	 * make sure the buf_address is 4bytes aligned
+	 *
+	 * -------------------   <-- plat_data->buf_address
+	 * | minor 0 tx buf  |
+	 *  -----------------
+	 * | minor 0 rx buf  |
+	 *  -----------------
+	 * |    ....         |
+	 *  -----------------
+	 * | minor n tx buf  |
+	 *  -----------------
+	 * | minor n rx buf  |
+	 * -------------------
+	 */
+
+	plat_data = (struct mxc_mlb_platform_data *)pdev->dev.platform_data;
+
+	bufaddr = plat_data->buf_address & ~0x3;
+	phyaddr = plat_data->phy_address & ~0x3;
+
+	for (i = 0; i < MLB_MINOR_DEVICES; i++) {
+		/* set the virtual and physical buf head address */
+		_get_txchan(i).buf_head = bufaddr;
+		_get_txchan(i).phy_head = phyaddr;
+
+		bufaddr += TX_CHANNEL_BUF_SIZE;
+		phyaddr += TX_CHANNEL_BUF_SIZE;
+
+		_get_rxchan(i).buf_head = bufaddr;
+		_get_rxchan(i).phy_head = phyaddr;
+
+		bufaddr += RX_CHANNEL_BUF_SIZE;
+		phyaddr += RX_CHANNEL_BUF_SIZE;
+
+		dev_dbg(&pdev->dev, "phy_head: tx(%lx), rx(%lx)\n",
+			_get_txchan(i).phy_head, _get_rxchan(i).phy_head);
+		dev_dbg(&pdev->dev, "buf_head: tx(%lx), rx(%lx)\n",
+			_get_txchan(i).buf_head, _get_rxchan(i).buf_head);
+	}
+
+	/* enable GPIO */
+	gpio_mlb_active();
+
+	/* power on MLB */
+	reg_nvcc = regulator_get(&pdev->dev, plat_data->reg_nvcc);
+	/* set MAX LDO6 for NVCC to 2.5V */
+	regulator_set_voltage(reg_nvcc, 2500000, 2500000);
+	regulator_enable(reg_nvcc);
+
+	/* enable clock */
+	mlb_clk = clk_get(&pdev->dev, plat_data->mlb_clk);
+	clk_enable(mlb_clk);
+
+	/* initial MLB module */
+	mlb_dev_init();
+
+	return 0;
+
+err0:
+	free_irq(irq, NULL);
+err1:
+	for (--i; i >= 0; i--)
+		device_destroy(mlb_class, MKDEV(mlb_major, i));
+
+	class_destroy(mlb_class);
+err2:
+	cdev_del(&mxc_mlb_dev);
+err3:
+	unregister_chrdev_region(dev, MLB_MINOR_DEVICES);
+err4:
+	for (i = 0; i < MLB_MINOR_DEVICES; i++)
+		kfree(mlb_devinfo[i].rx_bufs[0].data);
+
+	return ret;
+}
+
+static int __devexit mxc_mlb_remove(struct platform_device *pdev)
+{
+	int i;
+
+	mlb_dev_exit();
+
+	/* disable mlb clock */
+	clk_disable(mlb_clk);
+	clk_put(mlb_clk);
+
+	/* disable mlb power */
+	regulator_disable(reg_nvcc);
+	regulator_put(reg_nvcc);
+
+	/* inactive GPIO */
+	gpio_mlb_inactive();
+
+	/* iounmap */
+	iounmap((void *)mlb_base);
+
+	free_irq(irq, NULL);
+
+	/* destroy mlb device class */
+	for (i = MLB_MINOR_DEVICES - 1; i >= 0; i--)
+		device_destroy(mlb_class, MKDEV(MAJOR(dev), i));
+	class_destroy(mlb_class);
+
+	/* Unregister the two MLB devices */
+	cdev_del(&mxc_mlb_dev);
+	unregister_chrdev_region(dev, MLB_MINOR_DEVICES);
+
+	for (i = 0; i < MLB_MINOR_DEVICES; i++)
+		kfree(mlb_devinfo[i].rx_bufs[0].data);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int mxc_mlb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	return 0;
+}
+
+static int mxc_mlb_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+#else
+#define mxc_mlb_suspend NULL
+#define mxc_mlb_resume NULL
+#endif
+
+/*!
+ * platform driver structure for MLB
+ */
+static struct platform_driver mxc_mlb_driver = {
+	.driver = {
+		   .name = "mxc_mlb"},
+	.probe = mxc_mlb_probe,
+	.remove = __devexit_p(mxc_mlb_remove),
+	.suspend = mxc_mlb_suspend,
+	.resume = mxc_mlb_resume,
+};
+
+static int __init mxc_mlb_init(void)
+{
+	return platform_driver_register(&mxc_mlb_driver);
+}
+
+static void __exit mxc_mlb_exit(void)
+{
+	platform_driver_unregister(&mxc_mlb_driver);
+}
+
+module_init(mxc_mlb_init);
+module_exit(mxc_mlb_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MLB low level driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/Kconfig b/drivers/mxc/pmic/Kconfig
new file mode 100644
index 000000000000..d2273996cb2c
--- /dev/null
+++ b/drivers/mxc/pmic/Kconfig
@@ -0,0 +1,64 @@
+#
+# PMIC device driver configuration
+#
+
+menu "MXC PMIC support"
+
+config MXC_PMIC
+	boolean
+
+config MXC_PMIC_MC13783
+	tristate "MC13783 PMIC"
+	depends on ARCH_MXC && SPI
+	select MXC_PMIC
+	---help---
+	  This is the MXC MC13783(PMIC) support. It include
+	  ADC, Audio, Battery, Connectivity, Light, Power and RTC.
+
+config MXC_PMIC_MC13892
+	tristate "MC13892 PMIC"
+	depends on ARCH_MXC && (I2C || SPI)
+	select MXC_PMIC
+	---help---
+	  This is the MXC MC13892(PMIC) support. It include
+	  ADC, Battery, Connectivity, Light, Power and RTC.
+
+config MXC_PMIC_I2C
+  	bool "Support PMIC I2C Interface"
+	depends on MXC_PMIC_MC13892 && I2C
+
+config MXC_PMIC_SPI
+  	bool "Support PMIC SPI Interface"
+	depends on (MXC_PMIC_MC13892 || MXC_PMIC_MC13783) && SPI
+
+config MXC_PMIC_MC34704
+	tristate "MC34704 PMIC"
+	depends on ARCH_MXC && I2C
+	select MXC_PMIC
+	  ---help---
+	  This is the MXC MC34704 PMIC support.
+
+config MXC_PMIC_MC9SDZ60
+	tristate "MC9sDZ60 PMIC"
+	depends on ARCH_MXC && I2C
+	select MXC_PMIC
+	  ---help---
+	  This is the MXC MC9sDZ60(MCU) PMIC support.
+
+config MXC_PMIC_CHARDEV
+	tristate "MXC PMIC device interface"
+	depends on MXC_PMIC
+	help
+	  Say Y here to use "pmic" device files, found in the /dev directory
+	  on the system.  They make it possible to have user-space programs
+	  use or controll PMIC. Mainly its useful for notifying PMIC events
+	  to user-space programs.
+
+comment "MXC PMIC Client Drivers"
+	depends on MXC_PMIC
+
+source "drivers/mxc/pmic/mc13783/Kconfig"
+
+source "drivers/mxc/pmic/mc13892/Kconfig"
+
+endmenu
diff --git a/drivers/mxc/pmic/Makefile b/drivers/mxc/pmic/Makefile
new file mode 100644
index 000000000000..ef58a5889587
--- /dev/null
+++ b/drivers/mxc/pmic/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for the MXC PMIC drivers.
+#
+
+obj-y				+= core/
+obj-$(CONFIG_MXC_PMIC_MC13783)	+= mc13783/
+obj-$(CONFIG_MXC_PMIC_MC13892)	+= mc13892/
+
diff --git a/drivers/mxc/pmic/core/Makefile b/drivers/mxc/pmic/core/Makefile
new file mode 100644
index 000000000000..e746702bc569
--- /dev/null
+++ b/drivers/mxc/pmic/core/Makefile
@@ -0,0 +1,22 @@
+#
+# Makefile for the PMIC core drivers.
+#
+obj-$(CONFIG_MXC_PMIC_MC13783) += pmic_mc13783_mod.o
+pmic_mc13783_mod-objs := pmic_external.o pmic_event.o pmic_common.o pmic_core_spi.o mc13783.o
+
+obj-$(CONFIG_MXC_PMIC_MC13892) += pmic_mc13892_mod.o
+pmic_mc13892_mod-objs := pmic_external.o pmic_event.o pmic_common.o mc13892.o
+
+ifneq ($(CONFIG_MXC_PMIC_SPI),)
+pmic_mc13892_mod-objs += pmic_core_spi.o
+endif
+
+ifneq ($(CONFIG_MXC_PMIC_I2C),)
+pmic_mc13892_mod-objs += pmic_core_i2c.o
+endif
+
+obj-$(CONFIG_MXC_PMIC_MC34704) += pmic_mc34704_mod.o
+pmic_mc34704_mod-objs := pmic_external.o pmic_event.o mc34704.o
+
+obj-$(CONFIG_MXC_PMIC_CHARDEV)	+= pmic-dev.o
+
diff --git a/drivers/mxc/pmic/core/mc13783.c b/drivers/mxc/pmic/core/mc13783.c
new file mode 100644
index 000000000000..179cad815ebf
--- /dev/null
+++ b/drivers/mxc/pmic/core/mc13783.c
@@ -0,0 +1,380 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file pmic/core/mc13783.c
+ * @brief This file contains MC13783 specific PMIC code. This implementaion
+ * may differ for each PMIC chip.
+ *
+ * @ingroup PMIC_CORE
+ */
+
+/*
+ * Includes
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/pmic_external.h>
+#include <linux/pmic_status.h>
+#include <linux/spi/spi.h>
+#include <linux/mfd/mc13783/core.h>
+
+#include <asm/uaccess.h>
+
+#include "pmic.h"
+
+/*
+ * Defines
+ */
+#define EVENT_MASK_0			0x697fdf
+#define EVENT_MASK_1			0x3efffb
+#define MXC_PMIC_FRAME_MASK		0x00FFFFFF
+#define MXC_PMIC_MAX_REG_NUM		0x3F
+#define MXC_PMIC_REG_NUM_SHIFT		0x19
+#define MXC_PMIC_WRITE_BIT_SHIFT	31
+
+static unsigned int events_enabled0 = 0;
+static unsigned int events_enabled1 = 0;
+struct mxc_pmic pmic_drv_data;
+
+/*!
+ * This function is called to read a register on PMIC.
+ *
+ * @param        reg_num     number of the pmic register to be read
+ * @param        reg_val   return value of register
+ *
+ * @return       Returns 0 on success -1 on failure.
+ */
+int pmic_read(unsigned int reg_num, unsigned int *reg_val)
+{
+	unsigned int frame = 0;
+	int ret = 0;
+
+	if (reg_num > MXC_PMIC_MAX_REG_NUM)
+		return PMIC_ERROR;
+
+	frame |= reg_num << MXC_PMIC_REG_NUM_SHIFT;
+
+	ret = spi_rw(pmic_drv_data.spi, (u8 *) & frame, 1);
+
+	*reg_val = frame & MXC_PMIC_FRAME_MASK;
+
+	return ret;
+}
+
+/*!
+ * This function is called to write a value to the register on PMIC.
+ *
+ * @param        reg_num     number of the pmic register to be written
+ * @param        reg_val   value to be written
+ *
+ * @return       Returns 0 on success -1 on failure.
+ */
+int pmic_write(int reg_num, const unsigned int reg_val)
+{
+	unsigned int frame = 0;
+	int ret = 0;
+
+	if (reg_num > MXC_PMIC_MAX_REG_NUM)
+		return PMIC_ERROR;
+
+	frame |= (1 << MXC_PMIC_WRITE_BIT_SHIFT);
+
+	frame |= reg_num << MXC_PMIC_REG_NUM_SHIFT;
+
+	frame |= reg_val & MXC_PMIC_FRAME_MASK;
+
+	ret = spi_rw(pmic_drv_data.spi, (u8 *) & frame, 1);
+
+	return ret;
+}
+
+void *pmic_alloc_data(struct device *dev)
+{
+	struct mc13783 *mc13783;
+
+	mc13783 = kzalloc(sizeof(struct mc13783), GFP_KERNEL);
+	if (mc13783 == NULL)
+		return NULL;
+
+	mc13783->dev = dev;
+
+	return (void *)mc13783;
+}
+
+/*!
+ * This function initializes the SPI device parameters for this PMIC.
+ *
+ * @param    spi	the SPI slave device(PMIC)
+ *
+ * @return   None
+ */
+int pmic_spi_setup(struct spi_device *spi)
+{
+	/* Setup the SPI slave i.e.PMIC */
+	pmic_drv_data.spi = spi;
+
+	spi->mode = SPI_MODE_2 | SPI_CS_HIGH;
+	spi->bits_per_word = 32;
+
+	return spi_setup(spi);
+}
+
+/*!
+ * This function initializes the PMIC registers.
+ *
+ * @return   None
+ */
+int pmic_init_registers(void)
+{
+	CHECK_ERROR(pmic_write(REG_INTERRUPT_MASK_0, MXC_PMIC_FRAME_MASK));
+	CHECK_ERROR(pmic_write(REG_INTERRUPT_MASK_1, MXC_PMIC_FRAME_MASK));
+	CHECK_ERROR(pmic_write(REG_INTERRUPT_STATUS_0, MXC_PMIC_FRAME_MASK));
+	CHECK_ERROR(pmic_write(REG_INTERRUPT_STATUS_1, MXC_PMIC_FRAME_MASK));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function returns the PMIC version in system.
+ *
+ * @param 	ver	pointer to the pmic_version_t structure
+ *
+ * @return       This function returns PMIC version.
+ */
+void pmic_get_revision(pmic_version_t * ver)
+{
+	int rev_id = 0;
+	int rev1 = 0;
+	int rev2 = 0;
+	int finid = 0;
+	int icid = 0;
+
+	ver->id = PMIC_MC13783;
+	pmic_read(REG_REVISION, &rev_id);
+
+	rev1 = (rev_id & 0x018) >> 3;
+	rev2 = (rev_id & 0x007);
+	icid = (rev_id & 0x01C0) >> 6;
+	finid = (rev_id & 0x01E00) >> 9;
+
+	/* Ver 0.2 is actually 3.2a.  Report as 3.2 */
+	if ((rev1 == 0) && (rev2 == 2)) {
+		rev1 = 3;
+	}
+
+	if (rev1 == 0 || icid != 2) {
+		ver->revision = -1;
+		printk(KERN_NOTICE
+		       "mc13783: Not detected.\tAccess failed\t!!!\n");
+	} else {
+		ver->revision = ((rev1 * 10) + rev2);
+		printk(KERN_INFO "mc13783 Rev %d.%d FinVer %x detected\n", rev1,
+		       rev2, finid);
+	}
+
+	return;
+
+}
+
+/*!
+ * This function reads the interrupt status registers of PMIC
+ * and determine the current active events.
+ *
+ * @param 	active_events array pointer to be used to return active
+ *		event numbers.
+ *
+ * @return       This function returns PMIC version.
+ */
+unsigned int pmic_get_active_events(unsigned int *active_events)
+{
+	unsigned int count = 0;
+	unsigned int status0, status1;
+	int bit_set;
+
+	pmic_read(REG_INTERRUPT_STATUS_0, &status0);
+	pmic_read(REG_INTERRUPT_STATUS_1, &status1);
+	pmic_write(REG_INTERRUPT_STATUS_0, status0);
+	pmic_write(REG_INTERRUPT_STATUS_1, status1);
+	status0 &= events_enabled0;
+	status1 &= events_enabled1;
+
+	while (status0) {
+		bit_set = ffs(status0) - 1;
+		*(active_events + count) = bit_set;
+		count++;
+		status0 ^= (1 << bit_set);
+	}
+	while (status1) {
+		bit_set = ffs(status1) - 1;
+		*(active_events + count) = bit_set + 24;
+		count++;
+		status1 ^= (1 << bit_set);
+	}
+
+	return count;
+}
+
+/*!
+ * This function unsets a bit in mask register of pmic to unmask an event IT.
+ *
+ * @param	event 	the event to be unmasked
+ *
+ * @return    This function returns PMIC_SUCCESS on SUCCESS, error on FAILURE.
+ */
+int pmic_event_unmask(type_event event)
+{
+	unsigned int event_mask = 0;
+	unsigned int mask_reg = 0;
+	unsigned int event_bit = 0;
+	int ret;
+
+	if (event < EVENT_E1HZI) {
+		mask_reg = REG_INTERRUPT_MASK_0;
+		event_mask = EVENT_MASK_0;
+		event_bit = (1 << event);
+		events_enabled0 |= event_bit;
+	} else {
+		event -= 24;
+		mask_reg = REG_INTERRUPT_MASK_1;
+		event_mask = EVENT_MASK_1;
+		event_bit = (1 << event);
+		events_enabled1 |= event_bit;
+	}
+
+	if ((event_bit & event_mask) == 0) {
+		pr_debug("Error: unmasking a reserved/unused event\n");
+		return PMIC_ERROR;
+	}
+
+	ret = pmic_write_reg(mask_reg, 0, event_bit);
+
+	pr_debug("Enable Event : %d\n", event);
+
+	return ret;
+}
+
+/*!
+ * This function sets a bit in mask register of pmic to disable an event IT.
+ *
+ * @param	event 	the event to be masked
+ *
+ * @return     This function returns PMIC_SUCCESS on SUCCESS, error on FAILURE.
+ */
+int pmic_event_mask(type_event event)
+{
+	unsigned int event_mask = 0;
+	unsigned int mask_reg = 0;
+	unsigned int event_bit = 0;
+	int ret;
+
+	if (event < EVENT_E1HZI) {
+		mask_reg = REG_INTERRUPT_MASK_0;
+		event_mask = EVENT_MASK_0;
+		event_bit = (1 << event);
+		events_enabled0 &= ~event_bit;
+	} else {
+		event -= 24;
+		mask_reg = REG_INTERRUPT_MASK_1;
+		event_mask = EVENT_MASK_1;
+		event_bit = (1 << event);
+		events_enabled1 &= ~event_bit;
+	}
+
+	if ((event_bit & event_mask) == 0) {
+		pr_debug("Error: masking a reserved/unused event\n");
+		return PMIC_ERROR;
+	}
+
+	ret = pmic_write_reg(mask_reg, event_bit, event_bit);
+
+	pr_debug("Disable Event : %d\n", event);
+
+	return ret;
+}
+
+/*!
+ * This function is called to read all sensor bits of PMIC.
+ *
+ * @param        sensor    Sensor to be checked.
+ *
+ * @return       This function returns true if the sensor bit is high;
+ *               or returns false if the sensor bit is low.
+ */
+bool pmic_check_sensor(t_sensor sensor)
+{
+	unsigned int reg_val = 0;
+
+	CHECK_ERROR(pmic_read_reg
+		    (REG_INTERRUPT_SENSE_0, &reg_val, PMIC_ALL_BITS));
+
+	if ((1 << sensor) & reg_val)
+		return true;
+	else
+		return false;
+}
+
+/*!
+ * This function checks one sensor of PMIC.
+ *
+ * @param        sensor_bits  structure of all sensor bits.
+ *
+ * @return    This function returns PMIC_SUCCESS on SUCCESS, error on FAILURE.
+ */
+
+PMIC_STATUS pmic_get_sensors(t_sensor_bits * sensor_bits)
+{
+	int sense_0 = 0;
+	int sense_1 = 0;
+
+	memset(sensor_bits, 0, sizeof(t_sensor_bits));
+
+	pmic_read_reg(REG_INTERRUPT_SENSE_0, &sense_0, 0xffffff);
+	pmic_read_reg(REG_INTERRUPT_SENSE_1, &sense_1, 0xffffff);
+
+	sensor_bits->sense_chgdets = (sense_0 & (1 << 6)) ? true : false;
+	sensor_bits->sense_chgovs = (sense_0 & (1 << 7)) ? true : false;
+	sensor_bits->sense_chgrevs = (sense_0 & (1 << 8)) ? true : false;
+	sensor_bits->sense_chgshorts = (sense_0 & (1 << 9)) ? true : false;
+	sensor_bits->sense_cccvs = (sense_0 & (1 << 10)) ? true : false;
+	sensor_bits->sense_chgcurrs = (sense_0 & (1 << 11)) ? true : false;
+	sensor_bits->sense_bpons = (sense_0 & (1 << 12)) ? true : false;
+	sensor_bits->sense_lobatls = (sense_0 & (1 << 13)) ? true : false;
+	sensor_bits->sense_lobaths = (sense_0 & (1 << 14)) ? true : false;
+	sensor_bits->sense_usb4v4s = (sense_0 & (1 << 16)) ? true : false;
+	sensor_bits->sense_usb2v0s = (sense_0 & (1 << 17)) ? true : false;
+	sensor_bits->sense_usb0v8s = (sense_0 & (1 << 18)) ? true : false;
+	sensor_bits->sense_id_floats = (sense_0 & (1 << 19)) ? true : false;
+	sensor_bits->sense_id_gnds = (sense_0 & (1 << 20)) ? true : false;
+	sensor_bits->sense_se1s = (sense_0 & (1 << 21)) ? true : false;
+	sensor_bits->sense_ckdets = (sense_0 & (1 << 22)) ? true : false;
+
+	sensor_bits->sense_onofd1s = (sense_1 & (1 << 3)) ? true : false;
+	sensor_bits->sense_onofd2s = (sense_1 & (1 << 4)) ? true : false;
+	sensor_bits->sense_onofd3s = (sense_1 & (1 << 5)) ? true : false;
+	sensor_bits->sense_pwrrdys = (sense_1 & (1 << 11)) ? true : false;
+	sensor_bits->sense_thwarnhs = (sense_1 & (1 << 12)) ? true : false;
+	sensor_bits->sense_thwarnls = (sense_1 & (1 << 13)) ? true : false;
+	sensor_bits->sense_clks = (sense_1 & (1 << 14)) ? true : false;
+	sensor_bits->sense_mc2bs = (sense_1 & (1 << 17)) ? true : false;
+	sensor_bits->sense_hsdets = (sense_1 & (1 << 18)) ? true : false;
+	sensor_bits->sense_hsls = (sense_1 & (1 << 19)) ? true : false;
+	sensor_bits->sense_alspths = (sense_1 & (1 << 20)) ? true : false;
+	sensor_bits->sense_ahsshorts = (sense_1 & (1 << 21)) ? true : false;
+	return PMIC_SUCCESS;
+}
+
+EXPORT_SYMBOL(pmic_check_sensor);
+EXPORT_SYMBOL(pmic_get_sensors);
diff --git a/drivers/mxc/pmic/core/mc13892.c b/drivers/mxc/pmic/core/mc13892.c
new file mode 100644
index 000000000000..34ceec59221e
--- /dev/null
+++ b/drivers/mxc/pmic/core/mc13892.c
@@ -0,0 +1,333 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file pmic/core/mc13892.c
+ * @brief This file contains MC13892 specific PMIC code. This implementaion
+ * may differ for each PMIC chip.
+ *
+ * @ingroup PMIC_CORE
+ */
+
+/*
+ * Includes
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/spi/spi.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/pmic_external.h>
+#include <linux/pmic_status.h>
+#include <linux/mfd/mc13892/core.h>
+
+#include <asm/mach-types.h>
+#include <asm/uaccess.h>
+
+#include "pmic.h"
+
+/*
+ * Defines
+ */
+#define MC13892_I2C_RETRY_TIMES 10
+#define MXC_PMIC_FRAME_MASK		0x00FFFFFF
+#define MXC_PMIC_MAX_REG_NUM		0x3F
+#define MXC_PMIC_REG_NUM_SHIFT		0x19
+#define MXC_PMIC_WRITE_BIT_SHIFT		31
+
+static unsigned int events_enabled0;
+static unsigned int events_enabled1;
+static struct mxc_pmic pmic_drv_data;
+#ifndef CONFIG_MXC_PMIC_I2C
+struct i2c_client *mc13892_client;
+#endif
+
+int pmic_i2c_24bit_read(struct i2c_client *client, unsigned int reg_num,
+			unsigned int *value)
+{
+	unsigned char buf[3];
+	int ret;
+	int i;
+
+	memset(buf, 0, 3);
+	for (i = 0; i < MC13892_I2C_RETRY_TIMES; i++) {
+		ret = i2c_smbus_read_i2c_block_data(client, reg_num, 3, buf);
+		if (ret == 3)
+			break;
+		msleep(1);
+	}
+
+	if (ret == 3) {
+		*value = buf[0] << 16 | buf[1] << 8 | buf[2];
+		return ret;
+	} else {
+		pr_debug("24bit read error, ret = %d\n", ret);
+		return -1;	/* return -1 on failure */
+	}
+}
+
+int pmic_i2c_24bit_write(struct i2c_client *client,
+			 unsigned int reg_num, unsigned int reg_val)
+{
+	char buf[3];
+	int ret;
+	int i;
+
+	buf[0] = (reg_val >> 16) & 0xff;
+	buf[1] = (reg_val >> 8) & 0xff;
+	buf[2] = (reg_val) & 0xff;
+
+	for (i = 0; i < MC13892_I2C_RETRY_TIMES; i++) {
+		ret = i2c_smbus_write_i2c_block_data(client, reg_num, 3, buf);
+		if (ret == 0)
+			break;
+		msleep(1);
+	}
+
+	return ret;
+}
+
+int pmic_read(int reg_num, unsigned int *reg_val)
+{
+	unsigned int frame = 0;
+	int ret = 0;
+
+	if (pmic_drv_data.spi != NULL) {
+		if (reg_num > MXC_PMIC_MAX_REG_NUM)
+			return PMIC_ERROR;
+
+		frame |= reg_num << MXC_PMIC_REG_NUM_SHIFT;
+
+		ret = spi_rw(pmic_drv_data.spi, (u8 *) &frame, 1);
+
+		*reg_val = frame & MXC_PMIC_FRAME_MASK;
+	} else {
+		if (mc13892_client == NULL)
+			return PMIC_ERROR;
+
+		if (pmic_i2c_24bit_read(mc13892_client, reg_num, reg_val) == -1)
+			return PMIC_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+int pmic_write(int reg_num, const unsigned int reg_val)
+{
+	unsigned int frame = 0;
+	int ret = 0;
+
+	if (pmic_drv_data.spi != NULL) {
+		if (reg_num > MXC_PMIC_MAX_REG_NUM)
+			return PMIC_ERROR;
+
+		frame |= (1 << MXC_PMIC_WRITE_BIT_SHIFT);
+
+		frame |= reg_num << MXC_PMIC_REG_NUM_SHIFT;
+
+		frame |= reg_val & MXC_PMIC_FRAME_MASK;
+
+		ret = spi_rw(pmic_drv_data.spi, (u8 *) &frame, 1);
+
+		return ret;
+	} else {
+		if (mc13892_client == NULL)
+			return PMIC_ERROR;
+
+		return pmic_i2c_24bit_write(mc13892_client, reg_num, reg_val);
+	}
+}
+
+void *pmic_alloc_data(struct device *dev)
+{
+	struct mc13892 *mc13892;
+
+	mc13892 = kzalloc(sizeof(struct mc13892), GFP_KERNEL);
+	if (mc13892 == NULL)
+		return NULL;
+
+	mc13892->dev = dev;
+
+	return (void *)mc13892;
+}
+
+/*!
+ * This function initializes the SPI device parameters for this PMIC.
+ *
+ * @param    spi	the SPI slave device(PMIC)
+ *
+ * @return   None
+ */
+int pmic_spi_setup(struct spi_device *spi)
+{
+	/* Setup the SPI slave i.e.PMIC */
+	pmic_drv_data.spi = spi;
+
+	spi->mode = SPI_MODE_0 | SPI_CS_HIGH;
+	spi->bits_per_word = 32;
+
+	return spi_setup(spi);
+}
+
+int pmic_init_registers(void)
+{
+	CHECK_ERROR(pmic_write(REG_INT_MASK0, 0xFFFFFF));
+	CHECK_ERROR(pmic_write(REG_INT_MASK0, 0xFFFFFF));
+	CHECK_ERROR(pmic_write(REG_INT_STATUS0, 0xFFFFFF));
+	CHECK_ERROR(pmic_write(REG_INT_STATUS1, 0xFFFFFF));
+	/* disable auto charge */
+	if (machine_is_mx51_3ds())
+		CHECK_ERROR(pmic_write(REG_CHARGE, 0xB40003));
+
+	pm_power_off = mc13892_power_off;
+
+	return PMIC_SUCCESS;
+}
+
+unsigned int pmic_get_active_events(unsigned int *active_events)
+{
+	unsigned int count = 0;
+	unsigned int status0, status1;
+	int bit_set;
+
+	pmic_read(REG_INT_STATUS0, &status0);
+	pmic_read(REG_INT_STATUS1, &status1);
+	pmic_write(REG_INT_STATUS0, status0);
+	pmic_write(REG_INT_STATUS1, status1);
+	status0 &= events_enabled0;
+	status1 &= events_enabled1;
+
+	while (status0) {
+		bit_set = ffs(status0) - 1;
+		*(active_events + count) = bit_set;
+		count++;
+		status0 ^= (1 << bit_set);
+	}
+	while (status1) {
+		bit_set = ffs(status1) - 1;
+		*(active_events + count) = bit_set + 24;
+		count++;
+		status1 ^= (1 << bit_set);
+	}
+
+	return count;
+}
+
+#define EVENT_MASK_0			0x387fff
+#define EVENT_MASK_1			0x1177eb
+
+int pmic_event_unmask(type_event event)
+{
+	unsigned int event_mask = 0;
+	unsigned int mask_reg = 0;
+	unsigned int event_bit = 0;
+	int ret;
+
+	if (event < EVENT_1HZI) {
+		mask_reg = REG_INT_MASK0;
+		event_mask = EVENT_MASK_0;
+		event_bit = (1 << event);
+		events_enabled0 |= event_bit;
+	} else {
+		event -= 24;
+		mask_reg = REG_INT_MASK1;
+		event_mask = EVENT_MASK_1;
+		event_bit = (1 << event);
+		events_enabled1 |= event_bit;
+	}
+
+	if ((event_bit & event_mask) == 0) {
+		pr_debug("Error: unmasking a reserved/unused event\n");
+		return PMIC_ERROR;
+	}
+
+	ret = pmic_write_reg(mask_reg, 0, event_bit);
+
+	pr_debug("Enable Event : %d\n", event);
+
+	return ret;
+}
+
+int pmic_event_mask(type_event event)
+{
+	unsigned int event_mask = 0;
+	unsigned int mask_reg = 0;
+	unsigned int event_bit = 0;
+	int ret;
+
+	if (event < EVENT_1HZI) {
+		mask_reg = REG_INT_MASK0;
+		event_mask = EVENT_MASK_0;
+		event_bit = (1 << event);
+		events_enabled0 &= ~event_bit;
+	} else {
+		event -= 24;
+		mask_reg = REG_INT_MASK1;
+		event_mask = EVENT_MASK_1;
+		event_bit = (1 << event);
+		events_enabled1 &= ~event_bit;
+	}
+
+	if ((event_bit & event_mask) == 0) {
+		pr_debug("Error: masking a reserved/unused event\n");
+		return PMIC_ERROR;
+	}
+
+	ret = pmic_write_reg(mask_reg, event_bit, event_bit);
+
+	pr_debug("Disable Event : %d\n", event);
+
+	return ret;
+}
+
+/*!
+ * This function returns the PMIC version in system.
+ *
+ * @param 	ver	pointer to the pmic_version_t structure
+ *
+ * @return       This function returns PMIC version.
+ */
+void pmic_get_revision(pmic_version_t *ver)
+{
+	int rev_id = 0;
+	int rev1 = 0;
+	int rev2 = 0;
+	int finid = 0;
+	int icid = 0;
+
+	ver->id = PMIC_MC13892;
+	pmic_read(REG_IDENTIFICATION, &rev_id);
+
+	rev1 = (rev_id & 0x018) >> 3;
+	rev2 = (rev_id & 0x007);
+	icid = (rev_id & 0x01C0) >> 6;
+	finid = (rev_id & 0x01E00) >> 9;
+
+	ver->revision = ((rev1 * 10) + rev2);
+	printk(KERN_INFO "mc13892 Rev %d.%d FinVer %x detected\n", rev1,
+	       rev2, finid);
+}
+
+void mc13892_power_off(void)
+{
+	unsigned int value;
+
+	pmic_read_reg(REG_POWER_CTL0, &value, 0xffffff);
+
+	value |= 0x000008;
+
+	pmic_write_reg(REG_POWER_CTL0, value, 0xffffff);
+}
diff --git a/drivers/mxc/pmic/core/mc34704.c b/drivers/mxc/pmic/core/mc34704.c
new file mode 100644
index 000000000000..f0ec05afe0ab
--- /dev/null
+++ b/drivers/mxc/pmic/core/mc34704.c
@@ -0,0 +1,329 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file pmic/core/mc34704.c
+ * @brief This file contains MC34704 specific PMIC code.
+ *
+ * @ingroup PMIC_CORE
+ */
+
+/*
+ * Includes
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/uaccess.h>
+#include <linux/mfd/mc34704/core.h>
+#include <linux/pmic_external.h>
+#include <linux/pmic_status.h>
+
+#include "pmic.h"
+
+/*
+ * Globals
+ */
+static pmic_version_t mxc_pmic_version = {
+	.id = PMIC_MC34704,
+	.revision = 0,
+};
+static unsigned int events_enabled;
+unsigned int active_events[MAX_ACTIVE_EVENTS];
+struct i2c_client *mc34704_client;
+static void pmic_trigger_poll(void);
+
+#define MAX_MC34704_REG 0x59
+static unsigned int mc34704_reg_readonly[MAX_MC34704_REG / 32 + 1] = {
+	(1 << 0x03) || (1 << 0x05) || (1 << 0x07) || (1 << 0x09) ||
+	    (1 << 0x0B) || (1 << 0x0E) || (1 << 0x11) || (1 << 0x14) ||
+	    (1 << 0x17) || (1 << 0x18),
+	0,
+};
+static unsigned int mc34704_reg_written[MAX_MC34704_REG / 32 + 1];
+static unsigned char mc34704_shadow_regs[MAX_MC34704_REG - 1];
+#define IS_READONLY(r) ((1 << ((r) % 32)) & mc34704_reg_readonly[(r) / 32])
+#define WAS_WRITTEN(r) ((1 << ((r) % 32)) & mc34704_reg_written[(r) / 32])
+#define MARK_WRITTEN(r) do { \
+	mc34704_reg_written[(r) / 32] |= (1 << ((r) % 32)); \
+} while (0)
+
+int pmic_read(int reg_nr, unsigned int *reg_val)
+{
+	int c;
+
+	/*
+	 * Use the shadow register if we've written to it
+	 */
+	if (WAS_WRITTEN(reg_nr)) {
+		*reg_val = mc34704_shadow_regs[reg_nr];
+		return PMIC_SUCCESS;
+	}
+
+	/*
+	 * Otherwise, actually read the real register.
+	 * Write-only registers will read as zero.
+	 */
+	c = i2c_smbus_read_byte_data(mc34704_client, reg_nr);
+	if (c == -1) {
+		pr_debug("mc34704: error reading register 0x%02x\n", reg_nr);
+		return PMIC_ERROR;
+	} else {
+		*reg_val = c;
+		return PMIC_SUCCESS;
+	}
+}
+
+int pmic_write(int reg_nr, const unsigned int reg_val)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(mc34704_client, reg_nr, reg_val);
+	if (ret == -1) {
+		return PMIC_ERROR;
+	} else {
+		/*
+		 * Update our software copy of the register since you
+		 * can't read what you wrote.
+		 */
+		if (!IS_READONLY(reg_nr)) {
+			mc34704_shadow_regs[reg_nr] = reg_val;
+			MARK_WRITTEN(reg_nr);
+		}
+		return PMIC_SUCCESS;
+	}
+}
+
+unsigned int pmic_get_active_events(unsigned int *active_events)
+{
+	unsigned int count = 0;
+	unsigned int faults;
+	int bit_set;
+
+	/* Check for any relevant PMIC faults */
+	pmic_read(REG_MC34704_FAULTS, &faults);
+	faults &= events_enabled;
+
+	/*
+	 * Mask all active events, because there is no way to acknowledge
+	 * or dismiss them in the PMIC -- they're sticky.
+	 */
+	events_enabled &= ~faults;
+
+	/* Account for all unmasked faults */
+	while (faults) {
+		bit_set = ffs(faults) - 1;
+		*(active_events + count) = bit_set;
+		count++;
+		faults ^= (1 << bit_set);
+	}
+	return count;
+}
+
+int pmic_event_unmask(type_event event)
+{
+	unsigned int event_bit = 0;
+	unsigned int prior_events = events_enabled;
+
+	event_bit = (1 << event);
+	events_enabled |= event_bit;
+
+	pr_debug("Enable Event : %d\n", event);
+
+	/* start the polling task as needed */
+	if (events_enabled && prior_events == 0)
+		pmic_trigger_poll();
+
+	return 0;
+}
+
+int pmic_event_mask(type_event event)
+{
+	unsigned int event_bit = 0;
+
+	event_bit = (1 << event);
+	events_enabled &= ~event_bit;
+
+	pr_debug("Disable Event : %d\n", event);
+
+	return 0;
+}
+
+/*!
+ * PMIC event polling task.  This task is called periodically to poll
+ * for possible MC34704 events (No interrupt supplied by the hardware).
+ */
+static void pmic_event_task(struct work_struct *work);
+DECLARE_DELAYED_WORK(pmic_ws, pmic_event_task);
+
+static void pmic_trigger_poll(void)
+{
+	schedule_delayed_work(&pmic_ws, HZ / 10);
+}
+
+static void pmic_event_task(struct work_struct *work)
+{
+	unsigned int count = 0;
+	int i;
+
+	count = pmic_get_active_events(active_events);
+	pr_debug("active events number %d\n", count);
+
+	/* call handlers for all active events */
+	for (i = 0; i < count; i++)
+		pmic_event_callback(active_events[i]);
+
+	/* re-trigger this task, but only if somebody is watching */
+	if (events_enabled)
+		pmic_trigger_poll();
+
+	return;
+}
+
+pmic_version_t pmic_get_version(void)
+{
+	return mxc_pmic_version;
+}
+EXPORT_SYMBOL(pmic_get_version);
+
+int __devinit pmic_init_registers(void)
+{
+	/*
+	 * Set some registers to what they should be,
+	 * if for no other reason than to initialize our
+	 * software register copies.
+	 */
+	CHECK_ERROR(pmic_write(REG_MC34704_GENERAL2, 0x09));
+	CHECK_ERROR(pmic_write(REG_MC34704_VGSET1, 0));
+	CHECK_ERROR(pmic_write(REG_MC34704_REG2SET1, 0));
+	CHECK_ERROR(pmic_write(REG_MC34704_REG3SET1, 0));
+	CHECK_ERROR(pmic_write(REG_MC34704_REG4SET1, 0));
+	CHECK_ERROR(pmic_write(REG_MC34704_REG5SET1, 0));
+
+	return PMIC_SUCCESS;
+}
+
+static int __devinit is_chip_onboard(struct i2c_client *client)
+{
+	int val;
+
+	/*
+	 * This PMIC has no version or ID register, so just see
+	 * if it ACK's and returns 0 on some write-only register as
+	 * evidence of its presence.
+	 */
+	val = i2c_smbus_read_byte_data(client, REG_MC34704_GENERAL2);
+	if (val != 0)
+		return -1;
+
+	return 0;
+}
+
+static int __devinit pmic_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	int ret = 0;
+	struct mc34704 *mc34704;
+	struct mc34704_platform_data *plat_data = client->dev.platform_data;
+
+	if (!plat_data || !plat_data->init)
+		return -ENODEV;
+
+	ret = is_chip_onboard(client);
+
+	if (ret == -1)
+		return -ENODEV;
+
+	mc34704 = kzalloc(sizeof(struct mc34704), GFP_KERNEL);
+	if (mc34704 == NULL)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, mc34704);
+	mc34704->dev = &client->dev;
+	mc34704->i2c_client = client;
+
+	mc34704_client = client;
+
+	/* Initialize the PMIC event handling */
+	pmic_event_list_init();
+
+	/* Initialize PMI registers */
+	if (pmic_init_registers() != PMIC_SUCCESS)
+		return PMIC_ERROR;
+
+	ret = plat_data->init(mc34704);
+	if (ret != 0)
+		return PMIC_ERROR;
+
+	dev_info(&client->dev, "Loaded\n");
+
+	return PMIC_SUCCESS;
+}
+
+static int pmic_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+static int pmic_suspend(struct i2c_client *client, pm_message_t state)
+{
+	return 0;
+}
+
+static int pmic_resume(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id mc34704_id[] = {
+	{"mc34704", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, mc34704_id);
+
+static struct i2c_driver pmic_driver = {
+	.driver = {
+		   .name = "mc34704",
+		   .bus = NULL,
+		   },
+	.probe = pmic_probe,
+	.remove = pmic_remove,
+	.suspend = pmic_suspend,
+	.resume = pmic_resume,
+	.id_table = mc34704_id,
+};
+
+static int __init pmic_init(void)
+{
+	return i2c_add_driver(&pmic_driver);
+}
+
+static void __exit pmic_exit(void)
+{
+	i2c_del_driver(&pmic_driver);
+}
+
+/*
+ * Module entry points
+ */
+subsys_initcall_sync(pmic_init);
+module_exit(pmic_exit);
+
+MODULE_DESCRIPTION("MC34704 PMIC driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/core/pmic-dev.c b/drivers/mxc/pmic/core/pmic-dev.c
new file mode 100644
index 000000000000..fee100bfacec
--- /dev/null
+++ b/drivers/mxc/pmic/core/pmic-dev.c
@@ -0,0 +1,317 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All rights reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file pmic-dev.c
+ * @brief This provides /dev interface to the user program. They make it
+ * possible to have user-space programs use or control PMIC. Mainly its
+ * useful for notifying PMIC events to user-space programs.
+ *
+ * @ingroup PMIC_CORE
+ */
+
+/*
+ * Includes
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/kdev_t.h>
+#include <linux/circ_buf.h>
+#include <linux/major.h>
+#include <linux/init.h>
+#include <linux/fs.h>
+#include <linux/pmic_external.h>
+
+#include <asm/uaccess.h>
+
+#define PMIC_NAME	"pmic"
+#define CIRC_BUF_MAX	16
+#define CIRC_ADD(elem,cir_buf,size)				\
+	down(&event_mutex);					\
+	if(CIRC_SPACE(cir_buf.head, cir_buf.tail, size)){	\
+		cir_buf.buf[cir_buf.head] = (char)elem;		\
+		cir_buf.head = (cir_buf.head + 1) & (size - 1);	\
+	} else {						\
+		pr_info("Failed to notify event to the user\n");\
+	}							\
+	up(&event_mutex);
+
+#define CIRC_REMOVE(elem,cir_buf,size)				\
+	down(&event_mutex);					\
+	if(CIRC_CNT(cir_buf.head, cir_buf.tail, size)){         \
+		elem = (int)cir_buf.buf[cir_buf.tail];          \
+		cir_buf.tail = (cir_buf.tail + 1) & (size - 1); \
+	} else {                                                \
+		elem = -1;                                      \
+		pr_info("No valid notified event\n");           \
+	}							\
+	up(&event_mutex);
+
+static int pmic_major;
+static struct class *pmic_class;
+static struct fasync_struct *pmic_dev_queue;
+
+static DECLARE_MUTEX(event_mutex);
+static struct circ_buf pmic_events;
+
+static void callbackfn(void *event)
+{
+	printk(KERN_INFO "\n\n DETECTED PMIC EVENT : %d\n\n",
+	       (unsigned int)event);
+}
+
+static void user_notify_callback(void *event)
+{
+	CIRC_ADD((int)event, pmic_events, CIRC_BUF_MAX);
+	kill_fasync(&pmic_dev_queue, SIGIO, POLL_IN);
+}
+
+/*!
+ * This function implements IOCTL controls on a PMIC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @param        cmd         the command
+ * @param        arg         the parameter
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_dev_ioctl(struct inode *inode, struct file *file,
+			  unsigned int cmd, unsigned long arg)
+{
+	register_info reg_info;
+	pmic_event_callback_t event_sub;
+	type_event event;
+	int ret = 0;
+
+	if (_IOC_TYPE(cmd) != 'P')
+		return -ENOTTY;
+
+	switch (cmd) {
+	case PMIC_READ_REG:
+		if (copy_from_user(&reg_info, (register_info *) arg,
+				   sizeof(register_info))) {
+			return -EFAULT;
+		}
+		ret =
+		    pmic_read_reg(reg_info.reg, &(reg_info.reg_value),
+				  0x00ffffff);
+		pr_debug("read reg %d %x\n", reg_info.reg, reg_info.reg_value);
+		if (copy_to_user((register_info *) arg, &reg_info,
+				 sizeof(register_info))) {
+			return -EFAULT;
+		}
+		break;
+
+	case PMIC_WRITE_REG:
+		if (copy_from_user(&reg_info, (register_info *) arg,
+				   sizeof(register_info))) {
+			return -EFAULT;
+		}
+		ret =
+		    pmic_write_reg(reg_info.reg, reg_info.reg_value,
+				   0x00ffffff);
+		pr_debug("write reg %d %x\n", reg_info.reg, reg_info.reg_value);
+		if (copy_to_user((register_info *) arg, &reg_info,
+				 sizeof(register_info))) {
+			return -EFAULT;
+		}
+		break;
+
+	case PMIC_SUBSCRIBE:
+		if (get_user(event, (int __user *)arg)) {
+			return -EFAULT;
+		}
+		event_sub.func = callbackfn;
+		event_sub.param = (void *)event;
+		ret = pmic_event_subscribe(event, event_sub);
+		pr_debug("subscribe done\n");
+		break;
+
+	case PMIC_UNSUBSCRIBE:
+		if (get_user(event, (int __user *)arg)) {
+			return -EFAULT;
+		}
+		event_sub.func = callbackfn;
+		event_sub.param = (void *)event;
+		ret = pmic_event_unsubscribe(event, event_sub);
+		pr_debug("unsubscribe done\n");
+		break;
+
+	case PMIC_NOTIFY_USER:
+		if (get_user(event, (int __user *)arg)) {
+			return -EFAULT;
+		}
+		event_sub.func = user_notify_callback;
+		event_sub.param = (void *)event;
+		ret = pmic_event_subscribe(event, event_sub);
+		break;
+
+	case PMIC_GET_NOTIFY:
+		CIRC_REMOVE(event, pmic_events, CIRC_BUF_MAX);
+		if (put_user(event, (int __user *)arg)) {
+			return -EFAULT;
+		}
+		break;
+
+	default:
+		printk(KERN_ERR "%d unsupported ioctl command\n", (int)cmd);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+/*!
+ * This function implements the open method on a PMIC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_dev_open(struct inode *inode, struct file *file)
+{
+	pr_debug("open\n");
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function implements the release method on a PMIC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ *
+ * @return       This function returns 0.
+ */
+static int pmic_dev_free(struct inode *inode, struct file *file)
+{
+	pr_debug("free\n");
+	return PMIC_SUCCESS;
+}
+
+static int pmic_dev_fasync(int fd, struct file *filp, int mode)
+{
+	return fasync_helper(fd, filp, mode, &pmic_dev_queue);
+}
+
+/*!
+ * This structure defines file operations for a PMIC device.
+ */
+static struct file_operations pmic_fops = {
+	/*!
+	 * the owner
+	 */
+	.owner = THIS_MODULE,
+	/*!
+	 * the ioctl operation
+	 */
+	.ioctl = pmic_dev_ioctl,
+	/*!
+	 * the open operation
+	 */
+	.open = pmic_dev_open,
+	/*!
+	 * the release operation
+	 */
+	.release = pmic_dev_free,
+	/*!
+	 * the release operation
+	 */
+	.fasync = pmic_dev_fasync,
+};
+
+/*!
+ * This function implements the init function of the PMIC char device.
+ * This function is called when the module is loaded. It registers
+ * the character device for PMIC to be used by user-space programs.
+ *
+ * @return       This function returns 0.
+ */
+static int __init pmic_dev_init(void)
+{
+	int ret = 0;
+	struct device *pmic_device;
+	pmic_version_t pmic_ver;
+
+	pmic_ver = pmic_get_version();
+	if (pmic_ver.revision < 0) {
+		printk(KERN_ERR "No PMIC device found\n");
+		return -ENODEV;
+	}
+
+	pmic_major = register_chrdev(0, PMIC_NAME, &pmic_fops);
+	if (pmic_major < 0) {
+		printk(KERN_ERR "unable to get a major for pmic\n");
+		return pmic_major;
+	}
+
+	pmic_class = class_create(THIS_MODULE, PMIC_NAME);
+	if (IS_ERR(pmic_class)) {
+		printk(KERN_ERR "Error creating pmic class.\n");
+		ret = PMIC_ERROR;
+		goto err;
+	}
+
+	pmic_device = device_create(pmic_class, NULL, MKDEV(pmic_major, 0), NULL,
+				    PMIC_NAME);
+	if (IS_ERR(pmic_device)) {
+		printk(KERN_ERR "Error creating pmic class device.\n");
+		ret = PMIC_ERROR;
+		goto err1;
+	}
+
+	pmic_events.buf = kmalloc(CIRC_BUF_MAX * sizeof(char), GFP_KERNEL);
+	if (NULL == pmic_events.buf) {
+		ret = -ENOMEM;
+		goto err2;
+	}
+	pmic_events.head = pmic_events.tail = 0;
+
+	printk(KERN_INFO "PMIC Character device: successfully loaded\n");
+	return ret;
+      err2:
+	device_destroy(pmic_class, MKDEV(pmic_major, 0));
+      err1:
+	class_destroy(pmic_class);
+      err:
+	unregister_chrdev(pmic_major, PMIC_NAME);
+	return ret;
+
+}
+
+/*!
+ * This function implements the exit function of the PMIC character device.
+ * This function is called when the module is unloaded. It unregisters
+ * the PMIC character device.
+ *
+ */
+static void __exit pmic_dev_exit(void)
+{
+	device_destroy(pmic_class, MKDEV(pmic_major, 0));
+	class_destroy(pmic_class);
+
+	unregister_chrdev(pmic_major, PMIC_NAME);
+
+	printk(KERN_INFO "PMIC Character device: successfully unloaded\n");
+}
+
+/*
+ * Module entry points
+ */
+
+module_init(pmic_dev_init);
+module_exit(pmic_dev_exit);
+
+MODULE_DESCRIPTION("PMIC Protocol /dev entries driver");
+MODULE_AUTHOR("FreeScale");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/core/pmic.h b/drivers/mxc/pmic/core/pmic.h
new file mode 100644
index 000000000000..b1382a34e850
--- /dev/null
+++ b/drivers/mxc/pmic/core/pmic.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __PMIC_H__
+#define __PMIC_H__
+
+ /*!
+  * @file pmic.h
+  * @brief This file contains prototypes of all the functions to be
+  * defined for each PMIC chip. The implementation of these may differ
+  * from PMIC chip to PMIC chip.
+  *
+  * @ingroup PMIC_CORE
+  */
+
+#include <linux/spi/spi.h>
+
+#define MAX_ACTIVE_EVENTS		10
+
+/*!
+ * This structure is a way for the PMIC core driver to define their own
+ * \b spi_device structure. This structure includes the core \b spi_device
+ * structure that is provided by Linux SPI Framework/driver as an
+ * element and may contain other elements that are required by core driver.
+ */
+struct mxc_pmic {
+	/*!
+	 * Master side proxy for an SPI slave device(PMIC)
+	 */
+	struct spi_device *spi;
+};
+
+/*!
+ * This function is called to transfer data to PMIC on SPI.
+ *
+ * @param    spi	the SPI slave device(PMIC)
+ * @param    buf   	the pointer to the data buffer
+ * @param    len    	the length of the data to be transferred
+ *
+ * @return   Returns 0 on success -1 on failure.
+ */
+static inline int spi_rw(struct spi_device *spi, u8 * buf, size_t len)
+{
+	struct spi_transfer t = {
+		.tx_buf = (const void *)buf,
+		.rx_buf = buf,
+		.len = len,
+		.cs_change = 0,
+		.delay_usecs = 0,
+	};
+	struct spi_message m;
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t, &m);
+	if (spi_sync(spi, &m) != 0 || m.status != 0)
+		return PMIC_ERROR;
+	return (len - m.actual_length);
+}
+
+/*!
+ * This function returns the PMIC version in system.
+ *
+ * @param 	ver	pointer to the pmic_version_t structure
+ *
+ * @return       This function returns PMIC version.
+ */
+void pmic_get_revision(pmic_version_t * ver);
+
+/*!
+ * This function initializes the SPI device parameters for this PMIC.
+ *
+ * @param    spi	the SPI slave device(PMIC)
+ *
+ * @return   None
+ */
+int pmic_spi_setup(struct spi_device *spi);
+
+/*!
+ * This function initializes the PMIC registers.
+ *
+ * @return   None
+ */
+int pmic_init_registers(void);
+
+/*!
+ * This function reads the interrupt status registers of PMIC
+ * and determine the current active events.
+ *
+ * @param 	active_events array pointer to be used to return active
+ *		event numbers.
+ *
+ * @return       This function returns PMIC version.
+ */
+unsigned int pmic_get_active_events(unsigned int *active_events);
+
+/*!
+ * This function sets a bit in mask register of pmic to disable an event IT.
+ *
+ * @param	event 	the event to be masked
+ *
+ * @return     This function returns PMIC_SUCCESS on SUCCESS, error on FAILURE.
+ */
+int pmic_event_mask(type_event event);
+
+/*!
+ * This function unsets a bit in mask register of pmic to unmask an event IT.
+ *
+ * @param	event 	the event to be unmasked
+ *
+ * @return    This function returns PMIC_SUCCESS on SUCCESS, error on FAILURE.
+ */
+int pmic_event_unmask(type_event event);
+
+#ifdef CONFIG_MXC_PMIC_FIXARB
+extern PMIC_STATUS pmic_fix_arbitration(struct spi_device *spi);
+#else
+static inline PMIC_STATUS pmic_fix_arbitration(struct spi_device *spi)
+{
+	return PMIC_SUCCESS;
+}
+#endif
+
+void *pmic_alloc_data(struct device *dev);
+
+#endif				/* __PMIC_H__ */
diff --git a/drivers/mxc/pmic/core/pmic_common.c b/drivers/mxc/pmic/core/pmic_common.c
new file mode 100644
index 000000000000..55f34b29cd93
--- /dev/null
+++ b/drivers/mxc/pmic/core/pmic_common.c
@@ -0,0 +1,98 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file pmic_common.c
+ * @brief This is the common file for the PMIC Core/Protocol driver.
+ *
+ * @ingroup PMIC_CORE
+ */
+
+/*
+ * Includes
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/pmic_external.h>
+#include <linux/pmic_status.h>
+
+#include <asm/uaccess.h>
+
+#include "pmic.h"
+
+/*
+ * Global variables
+ */
+pmic_version_t mxc_pmic_version;
+unsigned int active_events[MAX_ACTIVE_EVENTS];
+struct workqueue_struct *pmic_event_wq;
+
+void pmic_bh_handler(struct work_struct *work);
+/*!
+ * Bottom half handler of PMIC event handling.
+ */
+DECLARE_WORK(pmic_ws, pmic_bh_handler);
+
+/*!
+ * This function is the bottom half handler of the PMIC interrupt.
+ * It checks for active events and launches callback for the
+ * active events.
+ */
+void pmic_bh_handler(struct work_struct *work)
+{
+	unsigned int loop;
+	unsigned int count = 0;
+
+	count = pmic_get_active_events(active_events);
+	pr_debug("active events number %d\n", count);
+
+	for (loop = 0; loop < count; loop++)
+		pmic_event_callback(active_events[loop]);
+
+	return;
+}
+
+/*!
+ * This function is called when pmic interrupt occurs on the processor.
+ * It is the interrupt handler for the pmic module.
+ *
+ * @param        irq        the irq number
+ * @param        dev_id     the pointer on the device
+ *
+ * @return       The function returns IRQ_HANDLED when handled.
+ */
+irqreturn_t pmic_irq_handler(int irq, void *dev_id)
+{
+	/* prepare a task */
+	queue_work(pmic_event_wq, &pmic_ws);
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * This function is used to determine the PMIC type and its revision.
+ *
+ * @return      Returns the PMIC type and its revision.
+ */
+
+pmic_version_t pmic_get_version(void)
+{
+	return mxc_pmic_version;
+}
+EXPORT_SYMBOL(pmic_get_version);
diff --git a/drivers/mxc/pmic/core/pmic_core_i2c.c b/drivers/mxc/pmic/core/pmic_core_i2c.c
new file mode 100644
index 000000000000..5f393aa93484
--- /dev/null
+++ b/drivers/mxc/pmic/core/pmic_core_i2c.c
@@ -0,0 +1,346 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file pmic_core_i2c.c
+ * @brief This is the main file for the PMIC Core/Protocol driver. i2c
+ * should be providing the interface between the PMIC and the MCU.
+ *
+ * @ingroup PMIC_CORE
+ */
+
+/*
+ * Includes
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/i2c.h>
+#include <linux/mfd/mc13892/core.h>
+#include <linux/pmic_external.h>
+#include <linux/pmic_status.h>
+
+#include <asm/uaccess.h>
+
+#include "pmic.h"
+
+#define MC13892_GENERATION_ID_LSH	6
+#define MC13892_IC_ID_LSH		13
+
+#define MC13892_GENERATION_ID_WID	3
+#define MC13892_IC_ID_WID		6
+
+#define MC13892_GEN_ID_VALUE	0x7
+#define MC13892_IC_ID_VALUE		1
+
+/*
+ * Global variables
+ */
+struct i2c_client *mc13892_client;
+
+extern struct workqueue_struct *pmic_event_wq;
+extern pmic_version_t mxc_pmic_version;
+extern irqreturn_t pmic_irq_handler(int irq, void *dev_id);
+
+/*
+ * Platform device structure for PMIC client drivers
+ */
+static struct platform_device adc_ldm = {
+	.name = "pmic_adc",
+	.id = 1,
+};
+static struct platform_device battery_ldm = {
+	.name = "pmic_battery",
+	.id = 1,
+};
+static struct platform_device power_ldm = {
+	.name = "pmic_power",
+	.id = 1,
+};
+static struct platform_device rtc_ldm = {
+	.name = "pmic_rtc",
+	.id = 1,
+};
+static struct platform_device light_ldm = {
+	.name = "pmic_light",
+	.id = 1,
+};
+static struct platform_device rleds_ldm = {
+	.name = "pmic_leds",
+	.id = 'r',
+};
+static struct platform_device gleds_ldm = {
+	.name = "pmic_leds",
+	.id = 'g',
+};
+static struct platform_device bleds_ldm = {
+	.name = "pmic_leds",
+	.id = 'b',
+};
+
+static void pmic_pdev_register(struct device *dev)
+{
+	platform_device_register(&adc_ldm);
+	platform_device_register(&battery_ldm);
+	platform_device_register(&rtc_ldm);
+	platform_device_register(&power_ldm);
+	platform_device_register(&light_ldm);
+	platform_device_register(&rleds_ldm);
+	platform_device_register(&gleds_ldm);
+	platform_device_register(&bleds_ldm);
+}
+
+/*!
+ * This function unregisters platform device structures for
+ * PMIC client drivers.
+ */
+static void pmic_pdev_unregister(void)
+{
+	platform_device_unregister(&adc_ldm);
+	platform_device_unregister(&battery_ldm);
+	platform_device_unregister(&rtc_ldm);
+	platform_device_unregister(&power_ldm);
+	platform_device_unregister(&light_ldm);
+}
+
+static int __devinit is_chip_onboard(struct i2c_client *client)
+{
+	unsigned int ret = 0;
+
+	/*bind the right device to the driver */
+	if (pmic_i2c_24bit_read(client, REG_IDENTIFICATION, &ret) == -1)
+		return -1;
+
+	if (MC13892_GEN_ID_VALUE != BITFEXT(ret, MC13892_GENERATION_ID)) {
+		/*compare the address value */
+		dev_err(&client->dev,
+			"read generation ID 0x%x is not equal to 0x%x!\n",
+			BITFEXT(ret, MC13892_GENERATION_ID),
+			MC13892_GEN_ID_VALUE);
+		return -1;
+	}
+
+	return 0;
+}
+
+static ssize_t mc13892_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	int i, value;
+	int offset = (REG_TEST4 + 1) / 4;
+
+	for (i = 0; i < offset; i++) {
+		pmic_read(i, &value);
+		pr_info("reg%02d: %06x\t\t", i, value);
+		pmic_read(i + offset, &value);
+		pr_info("reg%02d: %06x\t\t", i + offset, value);
+		pmic_read(i + offset * 2, &value);
+		pr_info("reg%02d: %06x\t\t", i + offset * 2, value);
+		pmic_read(i + offset * 3, &value);
+		pr_info("reg%02d: %06x\n", i + offset * 3, value);
+	}
+
+	return 0;
+}
+
+static ssize_t mc13892_store(struct device *dev,
+			     struct device_attribute *attr, const char *buf,
+			     size_t count)
+{
+	int reg, value, ret;
+	char *p;
+
+	reg = simple_strtoul(buf, NULL, 10);
+
+	p = NULL;
+	p = memchr(buf, ' ', count);
+
+	if (p == NULL) {
+		pmic_read(reg, &value);
+		pr_debug("reg%02d: %06x\n", reg, value);
+		return count;
+	}
+
+	p += 1;
+
+	value = simple_strtoul(p, NULL, 16);
+
+	ret = pmic_write(reg, value);
+	if (ret == 0)
+		pr_debug("write reg%02d: %06x\n", reg, value);
+	else
+		pr_debug("register update failed\n");
+
+	return count;
+}
+
+static struct device_attribute mc13892_dev_attr = {
+	.attr = {
+		 .name = "mc13892_ctl",
+		 .mode = S_IRUSR | S_IWUSR,
+		 },
+	.show = mc13892_show,
+	.store = mc13892_store,
+};
+
+static int __devinit pmic_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	int ret = 0;
+	int pmic_irq;
+	struct mc13892 *mc13892;
+	struct mc13892_platform_data *plat_data = client->dev.platform_data;
+
+	ret = is_chip_onboard(client);
+	if (ret == -1)
+		return -ENODEV;
+
+	mc13892 = kzalloc(sizeof(struct mc13892), GFP_KERNEL);
+	if (mc13892 == NULL)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, mc13892);
+	mc13892->dev = &client->dev;
+	mc13892->i2c_client = client;
+
+	/* so far, we got matched chip on board */
+
+	mc13892_client = client;
+
+	/* Initialize the PMIC event handling */
+	pmic_event_list_init();
+
+	/* Initialize GPIO for PMIC Interrupt */
+	gpio_pmic_active();
+
+	/* Get the PMIC Version */
+	pmic_get_revision(&mxc_pmic_version);
+	if (mxc_pmic_version.revision < 0) {
+		dev_err((struct device *)client,
+			"PMIC not detected!!! Access Failed\n");
+		return -ENODEV;
+	} else {
+		dev_dbg((struct device *)client,
+			"Detected pmic core IC version number is %d\n",
+			mxc_pmic_version.revision);
+	}
+
+	/* Initialize the PMIC parameters */
+	ret = pmic_init_registers();
+	if (ret != PMIC_SUCCESS)
+		return PMIC_ERROR;
+
+	pmic_event_wq = create_workqueue("mc13892");
+	if (!pmic_event_wq) {
+		pr_err("mc13892 pmic driver init: fail to create work queue");
+		return -EFAULT;
+	}
+
+	/* Set and install PMIC IRQ handler */
+	pmic_irq = (int)(client->irq);
+	if (pmic_irq == 0)
+		return PMIC_ERROR;
+
+	set_irq_type(IOMUX_TO_IRQ(pmic_irq), IRQF_TRIGGER_RISING);
+	ret =
+	    request_irq(IOMUX_TO_IRQ(pmic_irq), pmic_irq_handler, 0, "PMIC_IRQ",
+			0);
+
+	if (ret) {
+		dev_err(&client->dev, "request irq %d error!\n", pmic_irq);
+		return ret;
+	}
+	enable_irq_wake(IOMUX_TO_IRQ(pmic_irq));
+
+	if (plat_data && plat_data->init) {
+		ret = plat_data->init(mc13892);
+		if (ret != 0)
+			return PMIC_ERROR;
+	}
+
+	ret = device_create_file(&client->dev, &mc13892_dev_attr);
+	if (ret)
+		dev_err(&client->dev, "create device file failed!\n");
+
+	pmic_pdev_register(&client->dev);
+
+	dev_info(&client->dev, "Loaded\n");
+
+	return PMIC_SUCCESS;
+}
+
+static int pmic_remove(struct i2c_client *client)
+{
+	int pmic_irq = (int)(client->irq);
+
+	if (pmic_event_wq)
+		destroy_workqueue(pmic_event_wq);
+
+	free_irq(pmic_irq, 0);
+	pmic_pdev_unregister();
+	return 0;
+}
+
+static int pmic_suspend(struct i2c_client *client, pm_message_t state)
+{
+	return 0;
+}
+
+static int pmic_resume(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id mc13892_id[] = {
+	{"mc13892", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, mc13892_id);
+
+static struct i2c_driver pmic_driver = {
+	.driver = {
+		   .name = "mc13892",
+		   .bus = NULL,
+		   },
+	.probe = pmic_probe,
+	.remove = pmic_remove,
+	.suspend = pmic_suspend,
+	.resume = pmic_resume,
+	.id_table = mc13892_id,
+};
+
+static int __init pmic_init(void)
+{
+	return i2c_add_driver(&pmic_driver);
+}
+
+static void __exit pmic_exit(void)
+{
+	i2c_del_driver(&pmic_driver);
+}
+
+/*
+ * Module entry points
+ */
+subsys_initcall_sync(pmic_init);
+module_exit(pmic_exit);
+
+MODULE_DESCRIPTION("Core/Protocol driver for PMIC");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/core/pmic_core_spi.c b/drivers/mxc/pmic/core/pmic_core_spi.c
new file mode 100644
index 000000000000..bada77ff34d6
--- /dev/null
+++ b/drivers/mxc/pmic/core/pmic_core_spi.c
@@ -0,0 +1,314 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file pmic_core_spi.c
+ * @brief This is the main file for the PMIC Core/Protocol driver. SPI
+ * should be providing the interface between the PMIC and the MCU.
+ *
+ * @ingroup PMIC_CORE
+ */
+
+/*
+ * Includes
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/spi/spi.h>
+#include <linux/pmic_external.h>
+#include <linux/pmic_status.h>
+
+#include <asm/uaccess.h>
+
+#include "pmic.h"
+
+/*
+ * Static functions
+ */
+static void pmic_pdev_register(void);
+static void pmic_pdev_unregister(void);
+
+/*
+ * Platform device structure for PMIC client drivers
+ */
+static struct platform_device adc_ldm = {
+	.name = "pmic_adc",
+	.id = 1,
+};
+static struct platform_device battery_ldm = {
+	.name = "pmic_battery",
+	.id = 1,
+};
+static struct platform_device power_ldm = {
+	.name = "pmic_power",
+	.id = 1,
+};
+static struct platform_device rtc_ldm = {
+	.name = "pmic_rtc",
+	.id = 1,
+};
+static struct platform_device light_ldm = {
+	.name = "pmic_light",
+	.id = 1,
+};
+static struct platform_device rleds_ldm = {
+	.name = "pmic_leds",
+	.id = 'r',
+};
+static struct platform_device gleds_ldm = {
+	.name = "pmic_leds",
+	.id = 'g',
+};
+static struct platform_device bleds_ldm = {
+	.name = "pmic_leds",
+	.id = 'b',
+};
+
+/*
+ * External functions
+ */
+extern void pmic_event_list_init(void);
+extern void pmic_event_callback(type_event event);
+extern void gpio_pmic_active(void);
+extern irqreturn_t pmic_irq_handler(int irq, void *dev_id);
+extern pmic_version_t mxc_pmic_version;
+extern struct workqueue_struct *pmic_event_wq;
+
+/*!
+ * This function registers platform device structures for
+ * PMIC client drivers.
+ */
+static void pmic_pdev_register(void)
+{
+	platform_device_register(&adc_ldm);
+	platform_device_register(&battery_ldm);
+	platform_device_register(&rtc_ldm);
+	platform_device_register(&power_ldm);
+	platform_device_register(&light_ldm);
+	platform_device_register(&rleds_ldm);
+	platform_device_register(&gleds_ldm);
+	platform_device_register(&bleds_ldm);
+}
+
+/*!
+ * This function unregisters platform device structures for
+ * PMIC client drivers.
+ */
+static void pmic_pdev_unregister(void)
+{
+	platform_device_unregister(&adc_ldm);
+	platform_device_unregister(&battery_ldm);
+	platform_device_unregister(&rtc_ldm);
+	platform_device_unregister(&power_ldm);
+	platform_device_unregister(&light_ldm);
+}
+
+/*!
+ * This function puts the SPI slave device in low-power mode/state.
+ *
+ * @param	spi	the SPI slave device
+ * @param	message	the power state to enter
+ *
+ * @return 	Returns 0 on SUCCESS and error on FAILURE.
+ */
+static int pmic_suspend(struct spi_device *spi, pm_message_t message)
+{
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function brings the SPI slave device back from low-power mode/state.
+ *
+ * @param	spi	the SPI slave device
+ *
+ * @return 	Returns 0 on SUCCESS and error on FAILURE.
+ */
+static int pmic_resume(struct spi_device *spi)
+{
+	return PMIC_SUCCESS;
+}
+
+static struct spi_driver pmic_driver;
+
+/*!
+ * This function is called whenever the SPI slave device is detected.
+ *
+ * @param	spi	the SPI slave device
+ *
+ * @return 	Returns 0 on SUCCESS and error on FAILURE.
+ */
+static int __devinit pmic_probe(struct spi_device *spi)
+{
+	int ret = 0;
+	struct pmic_platform_data *plat_data = spi->dev.platform_data;
+
+	if (!strcmp(spi->dev.bus_id, PMIC_ARBITRATION)) {
+		if (PMIC_SUCCESS != pmic_fix_arbitration(spi)) {
+			dev_err((struct device *)spi,
+				"Unable to fix arbitration!! Access Failed\n");
+			return -EACCES;
+		}
+		return PMIC_SUCCESS;
+	}
+
+	/* Initialize the PMIC parameters */
+	ret = pmic_spi_setup(spi);
+	if (ret != PMIC_SUCCESS) {
+		return PMIC_ERROR;
+	}
+
+	/* Initialize the PMIC event handling */
+	pmic_event_list_init();
+
+	/* Initialize GPIO for PMIC Interrupt */
+	gpio_pmic_active();
+
+	/* Get the PMIC Version */
+	pmic_get_revision(&mxc_pmic_version);
+	if (mxc_pmic_version.revision < 0) {
+		dev_err((struct device *)spi,
+			"PMIC not detected!!! Access Failed\n");
+		return -ENODEV;
+	} else {
+		dev_dbg((struct device *)spi,
+			"Detected pmic core IC version number is %d\n",
+			mxc_pmic_version.revision);
+	}
+
+	spi_set_drvdata(spi, pmic_alloc_data(&(spi->dev)));
+
+	/* Initialize the PMIC parameters */
+	ret = pmic_init_registers();
+	if (ret != PMIC_SUCCESS) {
+		kfree(spi_get_drvdata(spi));
+		spi_set_drvdata(spi, NULL);
+		return PMIC_ERROR;
+	}
+
+	pmic_event_wq = create_workqueue("pmic_spi");
+	if (!pmic_event_wq) {
+		pr_err("pmic driver init: fail to create work queue");
+		kfree(spi_get_drvdata(spi));
+		spi_set_drvdata(spi, NULL);
+		return -EFAULT;
+	}
+
+	/* Set and install PMIC IRQ handler */
+	set_irq_type(spi->irq, IRQF_TRIGGER_RISING);
+	ret = request_irq(spi->irq, pmic_irq_handler, 0, "PMIC_IRQ", 0);
+	if (ret) {
+		kfree(spi_get_drvdata(spi));
+		spi_set_drvdata(spi, NULL);
+		dev_err((struct device *)spi, "gpio1: irq%d error.", spi->irq);
+		return ret;
+	}
+
+	enable_irq_wake(spi->irq);
+
+	if (plat_data && plat_data->init) {
+		ret = plat_data->init(spi_get_drvdata(spi));
+		if (ret != 0) {
+			kfree(spi_get_drvdata(spi));
+			spi_set_drvdata(spi, NULL);
+			return PMIC_ERROR;
+		}
+	}
+
+	power_ldm.dev.platform_data = spi->dev.platform_data;
+
+	pmic_pdev_register();
+
+	printk(KERN_INFO "Device %s probed\n", spi->dev.bus_id);
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is called whenever the SPI slave device is removed.
+ *
+ * @param	spi	the SPI slave device
+ *
+ * @return 	Returns 0 on SUCCESS and error on FAILURE.
+ */
+static int __devexit pmic_remove(struct spi_device *spi)
+{
+	if (pmic_event_wq)
+		destroy_workqueue(pmic_event_wq);
+
+	free_irq(spi->irq, 0);
+
+	pmic_pdev_unregister();
+
+	printk(KERN_INFO "Device %s removed\n", spi->dev.bus_id);
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct spi_driver pmic_driver = {
+	.driver = {
+		   .name = "pmic_spi",
+		   .bus = &spi_bus_type,
+		   .owner = THIS_MODULE,
+		   },
+	.probe = pmic_probe,
+	.remove = __devexit_p(pmic_remove),
+	.suspend = pmic_suspend,
+	.resume = pmic_resume,
+};
+
+/*
+ * Initialization and Exit
+ */
+
+/*!
+ * This function implements the init function of the PMIC device.
+ * This function is called when the module is loaded. It registers
+ * the PMIC Protocol driver.
+ *
+ * @return       This function returns 0.
+ */
+static int __init pmic_init(void)
+{
+	return spi_register_driver(&pmic_driver);
+}
+
+/*!
+ * This function implements the exit function of the PMIC device.
+ * This function is called when the module is unloaded. It unregisters
+ * the PMIC Protocol driver.
+ *
+ */
+static void __exit pmic_exit(void)
+{
+	pr_debug("Unregistering the PMIC Protocol Driver\n");
+	spi_unregister_driver(&pmic_driver);
+}
+
+/*
+ * Module entry points
+ */
+subsys_initcall_sync(pmic_init);
+module_exit(pmic_exit);
+
+MODULE_DESCRIPTION("Core/Protocol driver for PMIC");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/core/pmic_event.c b/drivers/mxc/pmic/core/pmic_event.c
new file mode 100644
index 000000000000..2d1bebfef448
--- /dev/null
+++ b/drivers/mxc/pmic/core/pmic_event.c
@@ -0,0 +1,235 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file pmic_event.c
+ * @brief This file manage all event of PMIC component.
+ *
+ * It contains event subscription, unsubscription and callback
+ * launch methods implemeted.
+ *
+ * @ingroup PMIC_CORE
+ */
+
+/*
+ * Includes
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/list.h>
+#include <linux/pmic_external.h>
+#include <linux/pmic_status.h>
+#include "pmic.h"
+
+/*!
+ * This structure is used to keep a list of subscribed
+ * callbacks for an event.
+ */
+typedef struct {
+	/*!
+	 * Keeps a list of subscribed clients to an event.
+	 */
+	struct list_head list;
+
+	/*!
+	 * Callback function with parameter, called when event occurs
+	 */
+	pmic_event_callback_t callback;
+} pmic_event_callback_list_t;
+
+/* Create a mutex to be used to prevent concurrent access to the event list */
+static DECLARE_MUTEX(event_mutex);
+
+/* This is a pointer to the event handler array. It defines the currently
+ * active set of events and user-defined callback functions.
+ */
+static struct list_head pmic_events[PMIC_MAX_EVENTS];
+
+/*!
+ * This function initializes event list for PMIC event handling.
+ *
+ */
+void pmic_event_list_init(void)
+{
+	int i;
+
+	for (i = 0; i < PMIC_MAX_EVENTS; i++) {
+		INIT_LIST_HEAD(&pmic_events[i]);
+	}
+
+	sema_init(&event_mutex, 1);
+	return;
+}
+
+/*!
+ * This function is used to subscribe on an event.
+ *
+ * @param	event   the event number to be subscribed
+ * @param	callback the callback funtion to be subscribed
+ *
+ * @return       This function returns 0 on SUCCESS, error on FAILURE.
+ */
+PMIC_STATUS pmic_event_subscribe(type_event event,
+				 pmic_event_callback_t callback)
+{
+	pmic_event_callback_list_t *new = NULL;
+
+	pr_debug("Event:%d Subscribe\n", event);
+
+	/* Check whether the event & callback are valid? */
+	if (event >= PMIC_MAX_EVENTS) {
+		pr_debug("Invalid Event:%d\n", event);
+		return -EINVAL;
+	}
+	if (NULL == callback.func) {
+		pr_debug("Null or Invalid Callback\n");
+		return -EINVAL;
+	}
+
+	/* Create a new linked list entry */
+	new = kmalloc(sizeof(pmic_event_callback_list_t), GFP_KERNEL);
+	if (NULL == new) {
+		return -ENOMEM;
+	}
+	/* Initialize the list node fields */
+	new->callback.func = callback.func;
+	new->callback.param = callback.param;
+	INIT_LIST_HEAD(&new->list);
+
+	/* Obtain the lock to access the list */
+	if (down_interruptible(&event_mutex)) {
+		kfree(new);
+		return PMIC_SYSTEM_ERROR_EINTR;
+	}
+
+	/* Unmask the requested event */
+	if (list_empty(&pmic_events[event])) {
+		if (pmic_event_unmask(event) != PMIC_SUCCESS) {
+			kfree(new);
+			up(&event_mutex);
+			return PMIC_ERROR;
+		}
+	}
+
+	/* Add this entry to the event list */
+	list_add_tail(&new->list, &pmic_events[event]);
+
+	/* Release the lock */
+	up(&event_mutex);
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is used to unsubscribe on an event.
+ *
+ * @param	event   the event number to be unsubscribed
+ * @param	callback the callback funtion to be unsubscribed
+ *
+ * @return       This function returns 0 on SUCCESS, error on FAILURE.
+ */
+PMIC_STATUS pmic_event_unsubscribe(type_event event,
+				   pmic_event_callback_t callback)
+{
+	struct list_head *p;
+	struct list_head *n;
+	pmic_event_callback_list_t *temp = NULL;
+	int ret = PMIC_EVENT_NOT_SUBSCRIBED;
+
+	pr_debug("Event:%d Unsubscribe\n", event);
+
+	/* Check whether the event & callback are valid? */
+	if (event >= PMIC_MAX_EVENTS) {
+		pr_debug("Invalid Event:%d\n", event);
+		return -EINVAL;
+	}
+
+	if (NULL == callback.func) {
+		pr_debug("Null or Invalid Callback\n");
+		return -EINVAL;
+	}
+
+	/* Obtain the lock to access the list */
+	if (down_interruptible(&event_mutex)) {
+		return PMIC_SYSTEM_ERROR_EINTR;
+	}
+
+	/* Find the entry in the list */
+	list_for_each_safe(p, n, &pmic_events[event]) {
+		temp = list_entry(p, pmic_event_callback_list_t, list);
+		if (temp->callback.func == callback.func
+		    && temp->callback.param == callback.param) {
+			/* Remove the entry from the list */
+			list_del(p);
+			kfree(temp);
+			ret = PMIC_SUCCESS;
+			break;
+		}
+	}
+
+	/* Unmask the requested event */
+	if (list_empty(&pmic_events[event])) {
+		if (pmic_event_mask(event) != PMIC_SUCCESS) {
+			ret = PMIC_UNSUBSCRIBE_ERROR;
+		}
+	}
+
+	/* Release the lock */
+	up(&event_mutex);
+
+	return ret;
+}
+
+/*!
+ * This function calls all callback of a specific event.
+ *
+ * @param	event   the active event number
+ *
+ * @return 	None
+ */
+void pmic_event_callback(type_event event)
+{
+	struct list_head *p;
+	pmic_event_callback_list_t *temp = NULL;
+
+	/* Obtain the lock to access the list */
+	if (down_interruptible(&event_mutex)) {
+		return;
+	}
+
+	if (list_empty(&pmic_events[event])) {
+		pr_debug("PMIC Event:%d detected. No callback subscribed\n",
+			 event);
+		up(&event_mutex);
+		return;
+	}
+
+	list_for_each(p, &pmic_events[event]) {
+		temp = list_entry(p, pmic_event_callback_list_t, list);
+		temp->callback.func(temp->callback.param);
+	}
+
+	/* Release the lock */
+	up(&event_mutex);
+
+	return;
+
+}
+
+EXPORT_SYMBOL(pmic_event_subscribe);
+EXPORT_SYMBOL(pmic_event_unsubscribe);
diff --git a/drivers/mxc/pmic/core/pmic_external.c b/drivers/mxc/pmic/core/pmic_external.c
new file mode 100644
index 000000000000..bea23af3207d
--- /dev/null
+++ b/drivers/mxc/pmic/core/pmic_external.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file pmic_external.c
+ * @brief This file contains all external functions of PMIC drivers.
+ *
+ * @ingroup PMIC_CORE
+ */
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/wait.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+
+#include <linux/pmic_external.h>
+#include <linux/pmic_status.h>
+
+/*
+ * External Functions
+ */
+extern int pmic_read(int reg_num, unsigned int *reg_val);
+extern int pmic_write(int reg_num, const unsigned int reg_val);
+
+/*!
+ * This function is called by PMIC clients to read a register on PMIC.
+ *
+ * @param        reg        number of register
+ * @param        reg_value  return value of register
+ * @param        reg_mask   Bitmap mask indicating which bits to modify
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_read_reg(int reg, unsigned int *reg_value,
+			  unsigned int reg_mask)
+{
+	int ret = 0;
+	unsigned int temp = 0;
+
+	ret = pmic_read(reg, &temp);
+	if (ret != PMIC_SUCCESS) {
+		return PMIC_ERROR;
+	}
+	*reg_value = (temp & reg_mask);
+
+	pr_debug("Read REG[ %d ] = 0x%x\n", reg, *reg_value);
+
+	return ret;
+}
+
+/*!
+ * This function is called by PMIC clients to write a register on PMIC.
+ *
+ * @param        reg        number of register
+ * @param        reg_value  New value of register
+ * @param        reg_mask   Bitmap mask indicating which bits to modify
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_write_reg(int reg, unsigned int reg_value,
+			   unsigned int reg_mask)
+{
+	int ret = 0;
+	unsigned int temp = 0;
+
+	ret = pmic_read(reg, &temp);
+	if (ret != PMIC_SUCCESS) {
+		return PMIC_ERROR;
+	}
+	temp = (temp & (~reg_mask)) | reg_value;
+#ifdef CONFIG_MXC_PMIC_MC13783
+	if (reg == REG_POWER_MISCELLANEOUS)
+		temp &= 0xFFFE7FFF;
+#endif
+	ret = pmic_write(reg, temp);
+	if (ret != PMIC_SUCCESS) {
+		return PMIC_ERROR;
+	}
+
+	pr_debug("Write REG[ %d ] = 0x%x\n", reg, reg_value);
+
+	return ret;
+}
+
+EXPORT_SYMBOL(pmic_read_reg);
+EXPORT_SYMBOL(pmic_write_reg);
diff --git a/drivers/mxc/pmic/mc13783/Kconfig b/drivers/mxc/pmic/mc13783/Kconfig
new file mode 100644
index 000000000000..02496c624e2e
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/Kconfig
@@ -0,0 +1,55 @@
+#
+# PMIC Modules configuration
+#
+
+config MXC_MC13783_ADC
+	tristate "MC13783 ADC support"
+	depends on MXC_PMIC_MC13783
+	---help---
+	This is the MC13783 ADC module driver. This module provides kernel API
+	for the ADC system of MC13783.
+	It controls also the touch screen interface.
+	If you want MC13783 ADC support, you should say Y here
+
+config MXC_MC13783_AUDIO
+	tristate "MC13783 Audio support"
+	depends on MXC_PMIC_MC13783
+	---help---
+	This is the MC13783 audio module driver. This module provides kernel API
+	for audio part of MC13783.
+	If you want MC13783 audio support, you should say Y here
+config MXC_MC13783_RTC
+	tristate "MC13783 Real Time Clock (RTC) support"
+	depends on MXC_PMIC_MC13783
+	---help---
+	This is the MC13783 RTC module driver. This module provides kernel API
+	for RTC part of MC13783.
+	If you want MC13783 RTC support, you should say Y here
+config MXC_MC13783_LIGHT
+	tristate "MC13783 Light and Backlight support"
+	depends on MXC_PMIC_MC13783
+	---help---
+	This is the MC13783 Light module driver. This module provides kernel API
+	for led and backlight control part of MC13783.
+	If you want MC13783 Light support, you should say Y here
+config MXC_MC13783_BATTERY
+	tristate "MC13783 Battery API support"
+	depends on MXC_PMIC_MC13783
+	---help---
+	This is the MC13783 battery module driver. This module provides kernel API
+	for battery control part of MC13783.
+	If you want MC13783 battery support, you should say Y here
+config MXC_MC13783_CONNECTIVITY
+	tristate "MC13783 Connectivity API support"
+	depends on MXC_PMIC_MC13783
+	---help---
+	This is the MC13783 connectivity module driver. This module provides kernel API
+	for USB/RS232 connectivity control part of MC13783.
+	If you want MC13783 connectivity support, you should say Y here
+config MXC_MC13783_POWER
+	tristate "MC13783 Power API support"
+	depends on MXC_PMIC_MC13783
+	---help---
+	This is the MC13783 power and supplies module driver. This module provides kernel API
+	for power and regulator control part of MC13783.
+	If you want MC13783 power support, you should say Y here
diff --git a/drivers/mxc/pmic/mc13783/Makefile b/drivers/mxc/pmic/mc13783/Makefile
new file mode 100644
index 000000000000..7bbba23f5ab9
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/Makefile
@@ -0,0 +1,18 @@
+#
+# Makefile for the mc13783 pmic drivers.
+#
+
+obj-$(CONFIG_MXC_MC13783_ADC) += pmic_adc-mod.o
+obj-$(CONFIG_MXC_MC13783_AUDIO) += pmic_audio-mod.o
+obj-$(CONFIG_MXC_MC13783_RTC) += pmic_rtc-mod.o
+obj-$(CONFIG_MXC_MC13783_LIGHT) += pmic_light-mod.o
+obj-$(CONFIG_MXC_MC13783_BATTERY) += pmic_battery-mod.o
+obj-$(CONFIG_MXC_MC13783_CONNECTIVITY) += pmic_convity-mod.o
+obj-$(CONFIG_MXC_MC13783_POWER) += pmic_power-mod.o
+pmic_adc-mod-objs := pmic_adc.o
+pmic_audio-mod-objs := pmic_audio.o
+pmic_rtc-mod-objs := pmic_rtc.o
+pmic_light-mod-objs := pmic_light.o
+pmic_battery-mod-objs := pmic_battery.o
+pmic_convity-mod-objs := pmic_convity.o
+pmic_power-mod-objs := pmic_power.o
diff --git a/drivers/mxc/pmic/mc13783/pmic_adc.c b/drivers/mxc/pmic/mc13783/pmic_adc.c
new file mode 100644
index 000000000000..1554faffd288
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_adc.c
@@ -0,0 +1,1542 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_adc.c
+ * @brief This is the main file of PMIC(mc13783) ADC driver.
+ *
+ * @ingroup PMIC_ADC
+ */
+
+/*
+ * Includes
+ */
+
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/pmic_adc.h>
+#include <linux/pmic_status.h>
+
+#include "../core/pmic.h"
+#include "pmic_adc_defs.h"
+
+#define NB_ADC_REG      5
+
+static int pmic_adc_major;
+
+/* internal function */
+static void callback_tsi(void *);
+static void callback_adcdone(void *);
+static void callback_adcbisdone(void *);
+static void callback_adc_comp_high(void *);
+
+/*!
+ * Number of users waiting in suspendq
+ */
+static int swait = 0;
+
+/*!
+ * To indicate whether any of the adc devices are suspending
+ */
+static int suspend_flag = 0;
+
+/*!
+ * The suspendq is used by blocking application calls
+ */
+static wait_queue_head_t suspendq;
+
+static struct class *pmic_adc_class;
+
+/*
+ * ADC mc13783 API
+ */
+/* EXPORTED FUNCTIONS */
+EXPORT_SYMBOL(pmic_adc_init);
+EXPORT_SYMBOL(pmic_adc_deinit);
+EXPORT_SYMBOL(pmic_adc_convert);
+EXPORT_SYMBOL(pmic_adc_convert_8x);
+EXPORT_SYMBOL(pmic_adc_convert_multichnnel);
+EXPORT_SYMBOL(pmic_adc_set_touch_mode);
+EXPORT_SYMBOL(pmic_adc_get_touch_mode);
+EXPORT_SYMBOL(pmic_adc_get_touch_sample);
+EXPORT_SYMBOL(pmic_adc_get_battery_current);
+EXPORT_SYMBOL(pmic_adc_active_comparator);
+EXPORT_SYMBOL(pmic_adc_deactive_comparator);
+
+static DECLARE_COMPLETION(adcdone_it);
+static DECLARE_COMPLETION(adcbisdone_it);
+static DECLARE_COMPLETION(adc_tsi);
+static pmic_event_callback_t tsi_event;
+static pmic_event_callback_t event_adc;
+static pmic_event_callback_t event_adc_bis;
+static pmic_event_callback_t adc_comp_h;
+static bool data_ready_adc_1;
+static bool data_ready_adc_2;
+static bool adc_ts;
+static bool wait_ts;
+static bool monitor_en;
+static bool monitor_adc;
+static t_check_mode wcomp_mode;
+static DECLARE_MUTEX(convert_mutex);
+
+void (*monitoring_cb) (void);	/*call back to be called when event is detected. */
+
+static DECLARE_WAIT_QUEUE_HEAD(queue_adc_busy);
+static t_adc_state adc_dev[2];
+
+static unsigned channel_num[] = {
+	0,
+	1,
+	3,
+	4,
+	2,
+	12,
+	13,
+	14,
+	15,
+	-1,
+	5,
+	6,
+	7,
+	8,
+	9,
+	10,
+	11,
+	7,
+	6,
+	-1,
+	-1,
+	-1,
+	-1,
+	5,
+	7
+};
+
+static bool pmic_adc_ready;
+
+int is_pmic_adc_ready()
+{
+	return pmic_adc_ready;
+}
+EXPORT_SYMBOL(is_pmic_adc_ready);
+
+
+/*!
+ * This is the suspend of power management for the mc13783 ADC API.
+ * It supports SAVE and POWER_DOWN state.
+ *
+ * @param        pdev           the device
+ * @param        state          the state
+ *
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_adc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	unsigned int reg_value = 0;
+	suspend_flag = 1;
+	CHECK_ERROR(pmic_write_reg(REG_ADC_0, DEF_ADC_0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_1, reg_value, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_2, reg_value, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_3, DEF_ADC_3, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_4, reg_value, PMIC_ALL_BITS));
+
+	return 0;
+};
+
+/*!
+ * This is the resume of power management for the mc13783 adc API.
+ * It supports RESTORE state.
+ *
+ * @param        pdev           the device
+ *
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_adc_resume(struct platform_device *pdev)
+{
+	/* nothing for mc13783 adc */
+	unsigned int adc_0_reg, adc_1_reg;
+	suspend_flag = 0;
+
+	/* let interrupt of TSI again */
+	adc_0_reg = ADC_WAIT_TSI_0;
+	CHECK_ERROR(pmic_write_reg(REG_ADC_0, adc_0_reg, PMIC_ALL_BITS));
+	adc_1_reg = ADC_WAIT_TSI_1 | (ADC_BIS * adc_ts);
+	CHECK_ERROR(pmic_write_reg(REG_ADC_1, adc_1_reg, PMIC_ALL_BITS));
+
+	while (swait > 0) {
+		swait--;
+		wake_up_interruptible(&suspendq);
+	}
+
+	return 0;
+};
+
+/*
+ * Call back functions
+ */
+
+/*!
+ * This is the callback function called on TSI mc13783 event, used in synchronous call.
+ */
+static void callback_tsi(void *unused)
+{
+	pr_debug("*** TSI IT mc13783 PMIC_ADC_GET_TOUCH_SAMPLE ***\n");
+	if (wait_ts) {
+		complete(&adc_tsi);
+		pmic_event_mask(EVENT_TSI);
+	}
+}
+
+/*!
+ * This is the callback function called on ADCDone mc13783 event.
+ */
+static void callback_adcdone(void *unused)
+{
+	if (data_ready_adc_1) {
+		complete(&adcdone_it);
+	}
+}
+
+/*!
+ * This is the callback function called on ADCDone mc13783 event.
+ */
+static void callback_adcbisdone(void *unused)
+{
+	pr_debug("* adcdone bis it callback *\n");
+	if (data_ready_adc_2) {
+		complete(&adcbisdone_it);
+	}
+}
+
+/*!
+ * This is the callback function called on mc13783 event.
+ */
+static void callback_adc_comp_high(void *unused)
+{
+	pr_debug("* adc comp it high *\n");
+	if (wcomp_mode == CHECK_HIGH || wcomp_mode == CHECK_LOW_OR_HIGH) {
+		/* launch callback */
+		if (monitoring_cb != NULL) {
+			monitoring_cb();
+		}
+	}
+}
+
+/*!
+ * This function performs filtering and rejection of excessive noise prone
+ * samples.
+ *
+ * @param        ts_curr     Touch screen value
+ *
+ * @return       This function returns 0 on success, -1 otherwise.
+ */
+static int pmic_adc_filter(t_touch_screen * ts_curr)
+{
+	unsigned int ydiff1, ydiff2, ydiff3, xdiff1, xdiff2, xdiff3;
+	unsigned int sample_sumx, sample_sumy;
+	static unsigned int prev_x[FILTLEN], prev_y[FILTLEN];
+	int index = 0;
+	unsigned int y_curr, x_curr;
+	static int filt_count = 0;
+	/* Added a variable filt_type to decide filtering at run-time */
+	unsigned int filt_type = 0;
+
+	if (ts_curr->contact_resistance == 0) {
+		ts_curr->x_position = 0;
+		ts_curr->y_position = 0;
+		filt_count = 0;
+		return 0;
+	}
+
+	ydiff1 = abs(ts_curr->y_position1 - ts_curr->y_position2);
+	ydiff2 = abs(ts_curr->y_position2 - ts_curr->y_position3);
+	ydiff3 = abs(ts_curr->y_position1 - ts_curr->y_position3);
+	if ((ydiff1 > DELTA_Y_MAX) ||
+	    (ydiff2 > DELTA_Y_MAX) || (ydiff3 > DELTA_Y_MAX)) {
+		pr_debug("pmic_adc_filter: Ret pos 1\n");
+		return -1;
+	}
+
+	xdiff1 = abs(ts_curr->x_position1 - ts_curr->x_position2);
+	xdiff2 = abs(ts_curr->x_position2 - ts_curr->x_position3);
+	xdiff3 = abs(ts_curr->x_position1 - ts_curr->x_position3);
+
+	if ((xdiff1 > DELTA_X_MAX) ||
+	    (xdiff2 > DELTA_X_MAX) || (xdiff3 > DELTA_X_MAX)) {
+		pr_debug("mc13783_adc_filter: Ret pos 2\n");
+		return -1;
+	}
+	/* Compute two closer values among the three available Y readouts */
+
+	if (ydiff1 < ydiff2) {
+		if (ydiff1 < ydiff3) {
+			// Sample 0 & 1 closest together
+			sample_sumy = ts_curr->y_position1 +
+			    ts_curr->y_position2;
+		} else {
+			// Sample 0 & 2 closest together
+			sample_sumy = ts_curr->y_position1 +
+			    ts_curr->y_position3;
+		}
+	} else {
+		if (ydiff2 < ydiff3) {
+			// Sample 1 & 2 closest together
+			sample_sumy = ts_curr->y_position2 +
+			    ts_curr->y_position3;
+		} else {
+			// Sample 0 & 2 closest together
+			sample_sumy = ts_curr->y_position1 +
+			    ts_curr->y_position3;
+		}
+	}
+
+	/*
+	 * Compute two closer values among the three available X
+	 * readouts
+	 */
+	if (xdiff1 < xdiff2) {
+		if (xdiff1 < xdiff3) {
+			// Sample 0 & 1 closest together
+			sample_sumx = ts_curr->x_position1 +
+			    ts_curr->x_position2;
+		} else {
+			// Sample 0 & 2 closest together
+			sample_sumx = ts_curr->x_position1 +
+			    ts_curr->x_position3;
+		}
+	} else {
+		if (xdiff2 < xdiff3) {
+			// Sample 1 & 2 closest together
+			sample_sumx = ts_curr->x_position2 +
+			    ts_curr->x_position3;
+		} else {
+			// Sample 0 & 2 closest together
+			sample_sumx = ts_curr->x_position1 +
+			    ts_curr->x_position3;
+		}
+	}
+	/*
+	 * Wait FILTER_MIN_DELAY number of samples to restart
+	 * filtering
+	 */
+	if (filt_count < FILTER_MIN_DELAY) {
+		/*
+		 * Current output is the average of the two closer
+		 * values and no filtering is used
+		 */
+		y_curr = (sample_sumy / 2);
+		x_curr = (sample_sumx / 2);
+		ts_curr->y_position = y_curr;
+		ts_curr->x_position = x_curr;
+		filt_count++;
+	} else {
+		if (abs(sample_sumx - (prev_x[0] + prev_x[1])) >
+		    (DELTA_X_MAX * 16)) {
+			pr_debug("pmic_adc_filter: : Ret pos 3\n");
+			return -1;
+		}
+		if (abs(sample_sumy - (prev_y[0] + prev_y[1])) >
+		    (DELTA_Y_MAX * 16)) {
+			return -1;
+		}
+		sample_sumy /= 2;
+		sample_sumx /= 2;
+		/* Use hard filtering if the sample difference < 10 */
+		if ((abs(sample_sumy - prev_y[0]) > 10) ||
+		    (abs(sample_sumx - prev_x[0]) > 10)) {
+			filt_type = 1;
+		}
+
+		/*
+		 * Current outputs are the average of three previous
+		 * values and the present readout
+		 */
+		y_curr = sample_sumy;
+		for (index = 0; index < FILTLEN; index++) {
+			if (filt_type == 0) {
+				y_curr = y_curr + (prev_y[index]);
+			} else {
+				y_curr = y_curr + (prev_y[index] / 3);
+			}
+		}
+		if (filt_type == 0) {
+			y_curr = y_curr >> 2;
+		} else {
+			y_curr = y_curr >> 1;
+		}
+		ts_curr->y_position = y_curr;
+
+		x_curr = sample_sumx;
+		for (index = 0; index < FILTLEN; index++) {
+			if (filt_type == 0) {
+				x_curr = x_curr + (prev_x[index]);
+			} else {
+				x_curr = x_curr + (prev_x[index] / 3);
+			}
+		}
+		if (filt_type == 0) {
+			x_curr = x_curr >> 2;
+		} else {
+			x_curr = x_curr >> 1;
+		}
+		ts_curr->x_position = x_curr;
+
+	}
+
+	/* Update previous X and Y values */
+	for (index = (FILTLEN - 1); index > 0; index--) {
+		prev_x[index] = prev_x[index - 1];
+		prev_y[index] = prev_y[index - 1];
+	}
+
+	/*
+	 * Current output will be the most recent past for the
+	 * next sample
+	 */
+	prev_y[0] = y_curr;
+	prev_x[0] = x_curr;
+
+	return 0;
+}
+
+/*!
+ * This function implements the open method on a MC13783 ADC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_adc_open(struct inode *inode, struct file *file)
+{
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0))) {
+			return -ERESTARTSYS;
+		}
+	}
+	pr_debug("mc13783_adc : mc13783_adc_open()\n");
+	return 0;
+}
+
+/*!
+ * This function implements the release method on a MC13783 ADC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_adc_free(struct inode *inode, struct file *file)
+{
+	pr_debug("mc13783_adc : mc13783_adc_free()\n");
+	return 0;
+}
+
+/*!
+ * This function initializes all ADC registers with default values. This
+ * function also registers the interrupt events.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+int pmic_adc_init(void)
+{
+	unsigned int reg_value = 0, i = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	for (i = 0; i < ADC_NB_AVAILABLE; i++) {
+		adc_dev[i] = ADC_FREE;
+	}
+	CHECK_ERROR(pmic_write_reg(REG_ADC_0, DEF_ADC_0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_1, reg_value, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_2, reg_value, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_3, DEF_ADC_3, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_4, reg_value, PMIC_ALL_BITS));
+	reg_value = 0x001000;
+	CHECK_ERROR(pmic_write_reg(REG_ARBITRATION_PERIPHERAL_AUDIO, reg_value,
+				   0xFFFFFF));
+
+	data_ready_adc_1 = false;
+	data_ready_adc_2 = false;
+	adc_ts = false;
+	wait_ts = false;
+	monitor_en = false;
+	monitor_adc = false;
+	wcomp_mode = CHECK_LOW;
+	monitoring_cb = NULL;
+	/* sub to ADCDone IT */
+	event_adc.param = NULL;
+	event_adc.func = callback_adcdone;
+	CHECK_ERROR(pmic_event_subscribe(EVENT_ADCDONEI, event_adc));
+
+	/* sub to ADCDoneBis IT */
+	event_adc_bis.param = NULL;
+	event_adc_bis.func = callback_adcbisdone;
+	CHECK_ERROR(pmic_event_subscribe(EVENT_ADCBISDONEI, event_adc_bis));
+
+	/* sub to Touch Screen IT */
+	tsi_event.param = NULL;
+	tsi_event.func = callback_tsi;
+	CHECK_ERROR(pmic_event_subscribe(EVENT_TSI, tsi_event));
+
+	/* ADC reading above high limit */
+	adc_comp_h.param = NULL;
+	adc_comp_h.func = callback_adc_comp_high;
+	CHECK_ERROR(pmic_event_subscribe(EVENT_WHIGHI, adc_comp_h));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function disables the ADC, de-registers the interrupt events.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_deinit(void)
+{
+	CHECK_ERROR(pmic_event_unsubscribe(EVENT_ADCDONEI, event_adc));
+	CHECK_ERROR(pmic_event_unsubscribe(EVENT_ADCBISDONEI, event_adc_bis));
+	CHECK_ERROR(pmic_event_unsubscribe(EVENT_TSI, tsi_event));
+	CHECK_ERROR(pmic_event_unsubscribe(EVENT_WHIGHI, adc_comp_h));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function initializes adc_param structure.
+ *
+ * @param        adc_param     Structure to be initialized.
+ *
+ * @return       This function returns 0 if successful.
+ */
+int mc13783_adc_init_param(t_adc_param * adc_param)
+{
+	int i = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	adc_param->delay = 0;
+	adc_param->conv_delay = false;
+	adc_param->single_channel = false;
+	adc_param->group = false;
+	adc_param->channel_0 = BATTERY_VOLTAGE;
+	adc_param->channel_1 = BATTERY_VOLTAGE;
+	adc_param->read_mode = 0;
+	adc_param->wait_tsi = 0;
+	adc_param->chrgraw_devide_5 = true;
+	adc_param->read_ts = false;
+	adc_param->ts_value.x_position = 0;
+	adc_param->ts_value.y_position = 0;
+	adc_param->ts_value.contact_resistance = 0;
+	for (i = 0; i <= MAX_CHANNEL; i++) {
+		adc_param->value[i] = 0;
+	}
+	return 0;
+}
+
+/*!
+ * This function starts the convert.
+ *
+ * @param        adc_param      contains all adc configuration and return value.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS mc13783_adc_convert(t_adc_param * adc_param)
+{
+	bool use_bis = false;
+	unsigned int adc_0_reg = 0, adc_1_reg = 0, reg_1 = 0, result_reg =
+	    0, i = 0;
+	unsigned int result = 0, temp = 0;
+	pmic_version_t mc13783_ver;
+	pr_debug("mc13783 ADC - mc13783_adc_convert ....\n");
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	if (adc_param->wait_tsi) {
+		/* we need to set ADCEN 1 for TSI interrupt on mc13783 1.x */
+		/* configure adc to wait tsi interrupt */
+		INIT_COMPLETION(adc_tsi);
+		pr_debug("mc13783 ADC - pmic_write_reg ....\n");
+		/*for ts don't use bis */
+		adc_0_reg = 0x001c00 | (ADC_BIS * 0);
+		pmic_event_unmask(EVENT_TSI);
+		CHECK_ERROR(pmic_write_reg
+			    (REG_ADC_0, adc_0_reg, PMIC_ALL_BITS));
+		/*for ts don't use bis */
+		adc_1_reg = 0x200001 | (ADC_BIS * 0);
+		CHECK_ERROR(pmic_write_reg
+			    (REG_ADC_1, adc_1_reg, PMIC_ALL_BITS));
+		pr_debug("wait tsi ....\n");
+		wait_ts = true;
+		wait_for_completion_interruptible(&adc_tsi);
+		wait_ts = false;
+	}
+	if (adc_param->read_ts == false)
+		down(&convert_mutex);
+	use_bis = mc13783_adc_request(adc_param->read_ts);
+	if (use_bis < 0) {
+		pr_debug("process has received a signal and got interrupted\n");
+		return -EINTR;
+	}
+
+	/* CONFIGURE ADC REG 0 */
+	adc_0_reg = 0;
+	adc_1_reg = 0;
+	if (adc_param->read_ts == false) {
+		adc_0_reg = adc_param->read_mode & 0x00003F;
+		/* add auto inc */
+		adc_0_reg |= ADC_INC;
+		if (use_bis) {
+			/* add adc bis */
+			adc_0_reg |= ADC_BIS;
+		}
+		mc13783_ver = pmic_get_version();
+		if (mc13783_ver.revision >= 20) {
+			if (adc_param->chrgraw_devide_5) {
+				adc_0_reg |= ADC_CHRGRAW_D5;
+			}
+		}
+		if (adc_param->single_channel) {
+			adc_1_reg |= ADC_SGL_CH;
+		}
+
+		if (adc_param->conv_delay) {
+			adc_1_reg |= ADC_ATO;
+		}
+
+		if (adc_param->group) {
+			adc_1_reg |= ADC_ADSEL;
+		}
+
+		if (adc_param->single_channel) {
+			adc_1_reg |= ADC_SGL_CH;
+		}
+
+		adc_1_reg |= (adc_param->channel_0 << ADC_CH_0_POS) &
+		    ADC_CH_0_MASK;
+		adc_1_reg |= (adc_param->channel_1 << ADC_CH_1_POS) &
+		    ADC_CH_1_MASK;
+	} else {
+		adc_0_reg = 0x003c00 | (ADC_BIS * use_bis) | ADC_INC;
+	}
+	pr_debug("Write Reg %i = %x\n", REG_ADC_0, adc_0_reg);
+	/*Change has been made here */
+	CHECK_ERROR(pmic_write_reg(REG_ADC_0, adc_0_reg,
+				   ADC_INC | ADC_BIS | ADC_CHRGRAW_D5 |
+				   0xfff00ff));
+	/* CONFIGURE ADC REG 1 */
+	if (adc_param->read_ts == false) {
+		adc_1_reg |= ADC_NO_ADTRIG;
+		adc_1_reg |= ADC_EN;
+		adc_1_reg |= (adc_param->delay << ADC_DELAY_POS) &
+		    ADC_DELAY_MASK;
+		if (use_bis) {
+			adc_1_reg |= ADC_BIS;
+		}
+	} else {
+		/* configure and start convert to read x and y position */
+		/* configure to read 2 value in channel selection 1 & 2 */
+		adc_1_reg = 0x100409 | (ADC_BIS * use_bis) | ADC_NO_ADTRIG;
+	}
+	reg_1 = adc_1_reg;
+	if (use_bis == 0) {
+		data_ready_adc_1 = false;
+		adc_1_reg |= ASC_ADC;
+		data_ready_adc_1 = true;
+		pr_debug("Write Reg %i = %x\n", REG_ADC_1, adc_1_reg);
+		INIT_COMPLETION(adcdone_it);
+		CHECK_ERROR(pmic_write_reg(REG_ADC_1, adc_1_reg,
+					   ADC_SGL_CH | ADC_ATO | ADC_ADSEL
+					   | ADC_CH_0_MASK | ADC_CH_1_MASK |
+					   ADC_NO_ADTRIG | ADC_EN |
+					   ADC_DELAY_MASK | ASC_ADC | ADC_BIS));
+		pr_debug("wait adc done \n");
+		wait_for_completion_interruptible(&adcdone_it);
+		data_ready_adc_1 = false;
+	} else {
+		data_ready_adc_2 = false;
+		adc_1_reg |= ASC_ADC;
+		data_ready_adc_2 = true;
+		INIT_COMPLETION(adcbisdone_it);
+		CHECK_ERROR(pmic_write_reg(REG_ADC_1, adc_1_reg, 0xFFFFFF));
+		temp = 0x800000;
+		CHECK_ERROR(pmic_write_reg(REG_ADC_3, temp, 0xFFFFFF));
+		temp = 0x001000;
+		pmic_write_reg(REG_ARBITRATION_PERIPHERAL_AUDIO, temp,
+			       0xFFFFFF);
+		pr_debug("wait adc done bis\n");
+		wait_for_completion_interruptible(&adcbisdone_it);
+		data_ready_adc_2 = false;
+	}
+	/* read result and store in adc_param */
+	result = 0;
+	if (use_bis == 0) {
+		result_reg = REG_ADC_2;
+	} else {
+		result_reg = REG_ADC_4;
+	}
+	CHECK_ERROR(pmic_write_reg(REG_ADC_1, 4 << ADC_CH_1_POS,
+				   ADC_CH_0_MASK | ADC_CH_1_MASK));
+
+	for (i = 0; i <= 3; i++) {
+		CHECK_ERROR(pmic_read_reg(result_reg, &result, PMIC_ALL_BITS));
+		pr_debug("result %i = %x\n", result_reg, result);
+		adc_param->value[i] = ((result & ADD1_RESULT_MASK) >> 2);
+		adc_param->value[i + 4] = ((result & ADD2_RESULT_MASK) >> 14);
+	}
+	if (adc_param->read_ts) {
+		adc_param->ts_value.x_position = adc_param->value[2];
+		adc_param->ts_value.x_position1 = adc_param->value[0];
+		adc_param->ts_value.x_position2 = adc_param->value[1];
+		adc_param->ts_value.x_position3 = adc_param->value[2];
+		adc_param->ts_value.y_position1 = adc_param->value[3];
+		adc_param->ts_value.y_position2 = adc_param->value[4];
+		adc_param->ts_value.y_position3 = adc_param->value[5];
+		adc_param->ts_value.y_position = adc_param->value[5];
+		adc_param->ts_value.contact_resistance = adc_param->value[6];
+
+	}
+
+	/*if (adc_param->read_ts) {
+	   adc_param->ts_value.x_position = adc_param->value[2];
+	   adc_param->ts_value.y_position = adc_param->value[5];
+	   adc_param->ts_value.contact_resistance = adc_param->value[6];
+	   } */
+	mc13783_adc_release(use_bis);
+	if (adc_param->read_ts == false)
+		up(&convert_mutex);
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function select the required read_mode for a specific channel.
+ *
+ * @param        channel   The channel to be sampled
+ *
+ * @return       This function returns the requires read_mode
+ */
+t_reading_mode mc13783_set_read_mode(t_channel channel)
+{
+	t_reading_mode read_mode = 0;
+
+	switch (channel) {
+	case LICELL:
+		read_mode = M_LITHIUM_CELL;
+		break;
+	case CHARGE_CURRENT:
+		read_mode = M_CHARGE_CURRENT;
+		break;
+	case BATTERY_CURRENT:
+		read_mode = M_BATTERY_CURRENT;
+		break;
+	case THERMISTOR:
+		read_mode = M_THERMISTOR;
+		break;
+	case DIE_TEMP:
+		read_mode = M_DIE_TEMPERATURE;
+		break;
+	case USB_ID:
+		read_mode = M_UID;
+		break;
+	default:
+		read_mode = 0;
+	}
+
+	return read_mode;
+}
+
+/*!
+ * This function triggers a conversion and returns one sampling result of one
+ * channel.
+ *
+ * @param        channel   The channel to be sampled
+ * @param        result    The pointer to the conversion result. The memory
+ *                         should be allocated by the caller of this function.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_convert(t_channel channel, unsigned short *result)
+{
+	t_adc_param adc_param;
+	PMIC_STATUS ret;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	channel = channel_num[channel];
+	if (channel == -1) {
+		pr_debug("Wrong channel ID\n");
+		return PMIC_PARAMETER_ERROR;
+	}
+	mc13783_adc_init_param(&adc_param);
+	pr_debug("pmic_adc_convert\n");
+	adc_param.read_ts = false;
+	adc_param.read_mode = mc13783_set_read_mode(channel);
+
+	adc_param.single_channel = true;
+	/* Find the group */
+	if ((channel >= 0) && (channel <= 7)) {
+		adc_param.channel_0 = channel;
+		adc_param.group = false;
+	} else if ((channel >= 8) && (channel <= 15)) {
+		adc_param.channel_0 = channel & 0x07;
+		adc_param.group = true;
+	} else {
+		return PMIC_PARAMETER_ERROR;
+	}
+	ret = mc13783_adc_convert(&adc_param);
+	*result = adc_param.value[0];
+	return ret;
+}
+
+/*!
+ * This function triggers a conversion and returns eight sampling results of
+ * one channel.
+ *
+ * @param        channel   The channel to be sampled
+ * @param        result    The pointer to array to store eight sampling results.
+ *                         The memory should be allocated by the caller of this
+ *                         function.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_convert_8x(t_channel channel, unsigned short *result)
+{
+	t_adc_param adc_param;
+	int i;
+	PMIC_STATUS ret;
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	channel = channel_num[channel];
+
+	if (channel == -1) {
+		pr_debug("Wrong channel ID\n");
+		return PMIC_PARAMETER_ERROR;
+	}
+	mc13783_adc_init_param(&adc_param);
+	pr_debug("pmic_adc_convert_8x\n");
+	adc_param.read_ts = false;
+	adc_param.single_channel = true;
+	adc_param.read_mode = mc13783_set_read_mode(channel);
+	if ((channel >= 0) && (channel <= 7)) {
+		adc_param.channel_0 = channel;
+		adc_param.channel_1 = channel;
+		adc_param.group = false;
+	} else if ((channel >= 8) && (channel <= 15)) {
+		adc_param.channel_0 = channel & 0x07;
+		adc_param.channel_1 = channel & 0x07;
+		adc_param.group = true;
+	} else {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	ret = mc13783_adc_convert(&adc_param);
+	for (i = 0; i <= 7; i++) {
+		result[i] = adc_param.value[i];
+	}
+	return ret;
+}
+
+/*!
+ * This function triggers a conversion and returns sampling results of each
+ * specified channel.
+ *
+ * @param        channels  This input parameter is bitmap to specify channels
+ *                         to be sampled.
+ * @param        result    The pointer to array to store sampling results.
+ *                         The memory should be allocated by the caller of this
+ *                         function.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_convert_multichnnel(t_channel channels,
+					 unsigned short *result)
+{
+	t_adc_param adc_param;
+	int i;
+	PMIC_STATUS ret;
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	mc13783_adc_init_param(&adc_param);
+	pr_debug("pmic_adc_convert_multichnnel\n");
+
+	channels = channel_num[channels];
+
+	if (channels == -1) {
+		pr_debug("Wrong channel ID\n");
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	adc_param.read_ts = false;
+	adc_param.single_channel = false;
+	if ((channels >= 0) && (channels <= 7)) {
+		adc_param.channel_0 = channels;
+		adc_param.channel_1 = ((channels + 4) % 4) + 4;
+		adc_param.group = false;
+	} else if ((channels >= 8) && (channels <= 15)) {
+		channels = channels & 0x07;
+		adc_param.channel_0 = channels;
+		adc_param.channel_1 = ((channels + 4) % 4) + 4;
+		adc_param.group = true;
+	} else {
+		return PMIC_PARAMETER_ERROR;
+	}
+	adc_param.read_mode = 0x00003f;
+	adc_param.read_ts = false;
+	ret = mc13783_adc_convert(&adc_param);
+
+	for (i = 0; i <= 7; i++) {
+		result[i] = adc_param.value[i];
+	}
+	return ret;
+}
+
+/*!
+ * This function sets touch screen operation mode.
+ *
+ * @param        touch_mode   Touch screen operation mode.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_set_touch_mode(t_touch_mode touch_mode)
+{
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	CHECK_ERROR(pmic_write_reg(REG_ADC_0,
+				   BITFVAL(MC13783_ADC0_TS_M, touch_mode),
+				   BITFMASK(MC13783_ADC0_TS_M)));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrieves the current touch screen operation mode.
+ *
+ * @param        touch_mode   Pointer to the retrieved touch screen operation
+ *                            mode.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_get_touch_mode(t_touch_mode * touch_mode)
+{
+	unsigned int value;
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	CHECK_ERROR(pmic_read_reg(REG_ADC_0, &value, PMIC_ALL_BITS));
+
+	*touch_mode = BITFEXT(value, MC13783_ADC0_TS_M);
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrieves the current touch screen (X,Y) coordinates.
+ *
+ * @param        touch_sample Pointer to touch sample.
+ * @param        wait	indicates whether this call must block or not.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_get_touch_sample(t_touch_screen * touch_sample, int wait)
+{
+	if (mc13783_adc_read_ts(touch_sample, wait) != 0)
+		return PMIC_ERROR;
+	if (0 == pmic_adc_filter(touch_sample))
+		return PMIC_SUCCESS;
+	else
+		return PMIC_ERROR;
+}
+
+/*!
+ * This function read the touch screen value.
+ *
+ * @param        ts_value    return value of touch screen
+ * @param        wait_tsi    if true, this function is synchronous (wait in TSI event).
+ *
+ * @return       This function returns 0.
+ */
+PMIC_STATUS mc13783_adc_read_ts(t_touch_screen * ts_value, int wait_tsi)
+{
+	t_adc_param param;
+	pr_debug("mc13783_adc : mc13783_adc_read_ts\n");
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	if (wait_ts) {
+		pr_debug("mc13783_adc : error TS busy \n");
+		return PMIC_ERROR;
+	}
+	mc13783_adc_init_param(&param);
+	param.wait_tsi = wait_tsi;
+	param.read_ts = true;
+	if (mc13783_adc_convert(&param) != 0)
+		return PMIC_ERROR;
+	/* check if x-y is ok */
+	if ((param.ts_value.x_position1 < TS_X_MAX) &&
+	    (param.ts_value.x_position1 >= TS_X_MIN) &&
+	    (param.ts_value.y_position1 < TS_Y_MAX) &&
+	    (param.ts_value.y_position1 >= TS_Y_MIN) &&
+	    (param.ts_value.x_position2 < TS_X_MAX) &&
+	    (param.ts_value.x_position2 >= TS_X_MIN) &&
+	    (param.ts_value.y_position2 < TS_Y_MAX) &&
+	    (param.ts_value.y_position2 >= TS_Y_MIN) &&
+	    (param.ts_value.x_position3 < TS_X_MAX) &&
+	    (param.ts_value.x_position3 >= TS_X_MIN) &&
+	    (param.ts_value.y_position3 < TS_Y_MAX) &&
+	    (param.ts_value.y_position3 >= TS_Y_MIN)) {
+		ts_value->x_position = param.ts_value.x_position;
+		ts_value->x_position1 = param.ts_value.x_position1;
+		ts_value->x_position2 = param.ts_value.x_position2;
+		ts_value->x_position3 = param.ts_value.x_position3;
+		ts_value->y_position = param.ts_value.y_position;
+		ts_value->y_position1 = param.ts_value.y_position1;
+		ts_value->y_position2 = param.ts_value.y_position2;
+		ts_value->y_position3 = param.ts_value.y_position3;
+		ts_value->contact_resistance =
+		    param.ts_value.contact_resistance + 1;
+
+	} else {
+		ts_value->x_position = 0;
+		ts_value->y_position = 0;
+		ts_value->contact_resistance = 0;
+
+	}
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function starts a Battery Current mode conversion.
+ *
+ * @param        mode      Conversion mode.
+ * @param        result    Battery Current measurement result.
+ *                         if \a mode = ADC_8CHAN_1X, the result is \n
+ *                             result[0] = (BATTP - BATT_I) \n
+ *                         if \a mode = ADC_1CHAN_8X, the result is \n
+ *                             result[0] = BATTP \n
+ *                             result[1] = BATT_I \n
+ *                             result[2] = BATTP \n
+ *                             result[3] = BATT_I \n
+ *                             result[4] = BATTP \n
+ *                             result[5] = BATT_I \n
+ *                             result[6] = BATTP \n
+ *                             result[7] = BATT_I
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_get_battery_current(t_conversion_mode mode,
+					 unsigned short *result)
+{
+	PMIC_STATUS ret;
+	t_channel channel;
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	channel = BATTERY_CURRENT;
+	if (mode == ADC_8CHAN_1X) {
+		ret = pmic_adc_convert(channel, result);
+	} else {
+		ret = pmic_adc_convert_8x(channel, result);
+	}
+	return ret;
+}
+
+/*!
+ * This function request a ADC.
+ *
+ * @return      This function returns index of ADC to be used (0 or 1) if successful.
+                return -1 if error.
+ */
+int mc13783_adc_request(bool read_ts)
+{
+	int adc_index = -1;
+	if (read_ts != 0) {
+		/*for ts we use bis=0 */
+		if (adc_dev[0] == ADC_USED)
+			return -1;
+		/*no wait here */
+		adc_dev[0] = ADC_USED;
+		adc_index = 0;
+	} else {
+		/*for other adc use bis = 1 */
+		if (adc_dev[1] == ADC_USED) {
+			return -1;
+			/*no wait here */
+		}
+		adc_dev[1] = ADC_USED;
+		adc_index = 1;
+	}
+	pr_debug("mc13783_adc : request ADC %d\n", adc_index);
+	return adc_index;
+}
+
+/*!
+ * This function release an ADC.
+ *
+ * @param        adc_index     index of ADC to be released.
+ *
+ * @return       This function returns 0 if successful.
+ */
+int mc13783_adc_release(int adc_index)
+{
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0))) {
+			return -ERESTARTSYS;
+		}
+	}
+
+	pr_debug("mc13783_adc : release ADC %d\n", adc_index);
+	if ((adc_dev[adc_index] == ADC_MONITORING) ||
+	    (adc_dev[adc_index] == ADC_USED)) {
+		adc_dev[adc_index] = ADC_FREE;
+		wake_up(&queue_adc_busy);
+		return 0;
+	}
+	return -1;
+}
+
+/*!
+ * This function initializes monitoring structure.
+ *
+ * @param        monitor     Structure to be initialized.
+ *
+ * @return       This function returns 0 if successful.
+ */
+int mc13783_adc_init_monitor_param(t_monitoring_param * monitor)
+{
+	pr_debug("mc13783_adc : init monitor\n");
+	monitor->delay = 0;
+	monitor->conv_delay = false;
+	monitor->channel = BATTERY_VOLTAGE;
+	monitor->read_mode = 0;
+	monitor->comp_low = 0;
+	monitor->comp_high = 0;
+	monitor->group = 0;
+	monitor->check_mode = CHECK_LOW_OR_HIGH;
+	monitor->callback = NULL;
+	return 0;
+}
+
+/*!
+ * This function actives the comparator.  When comparator is active and ADC
+ * is enabled, the 8th converted value will be digitally compared against the
+ * window defined by WLOW and WHIGH registers.
+ *
+ * @param        low      Comparison window low threshold (WLOW).
+ * @param        high     Comparison window high threshold (WHIGH).
+ * @param        channel  The channel to be sampled
+ * @param        callback Callback function to be called when the converted
+ *                        value is beyond the comparison window.  The callback
+ *                        function will pass a parameter of type
+ *                        \b t_comp_expection to indicate the reason of
+ *                        comparator exception.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_active_comparator(unsigned char low,
+				       unsigned char high,
+				       t_channel channel,
+				       t_comparator_cb callback)
+{
+	bool use_bis = false;
+	unsigned int adc_0_reg = 0, adc_1_reg = 0, adc_3_reg = 0;
+	t_monitoring_param monitoring;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	if (monitor_en) {
+		pr_debug("mc13783_adc : monitoring already configured\n");
+		return PMIC_ERROR;
+	}
+	monitor_en = true;
+	mc13783_adc_init_monitor_param(&monitoring);
+	monitoring.comp_low = low;
+	monitoring.comp_high = high;
+	monitoring.channel = channel;
+	monitoring.callback = (void *)callback;
+
+	use_bis = mc13783_adc_request(false);
+	if (use_bis < 0) {
+		pr_debug("mc13783_adc : request error\n");
+		return PMIC_ERROR;
+	}
+	monitor_adc = use_bis;
+
+	adc_0_reg = 0;
+
+	/* TO DO ADOUT CONFIGURE */
+	adc_0_reg = monitoring.read_mode & ADC_MODE_MASK;
+	if (use_bis) {
+		/* add adc bis */
+		adc_0_reg |= ADC_BIS;
+	}
+	adc_0_reg |= ADC_WCOMP;
+
+	/* CONFIGURE ADC REG 1 */
+	adc_1_reg = 0;
+	adc_1_reg |= ADC_EN;
+	if (monitoring.conv_delay) {
+		adc_1_reg |= ADC_ATO;
+	}
+	if (monitoring.group) {
+		adc_1_reg |= ADC_ADSEL;
+	}
+	adc_1_reg |= (monitoring.channel << ADC_CH_0_POS) & ADC_CH_0_MASK;
+	adc_1_reg |= (monitoring.delay << ADC_DELAY_POS) & ADC_DELAY_MASK;
+	if (use_bis) {
+		adc_1_reg |= ADC_BIS;
+	}
+
+	adc_3_reg |= (monitoring.comp_high << ADC_WCOMP_H_POS) &
+	    ADC_WCOMP_H_MASK;
+	adc_3_reg |= (monitoring.comp_low << ADC_WCOMP_L_POS) &
+	    ADC_WCOMP_L_MASK;
+	if (use_bis) {
+		adc_3_reg |= ADC_BIS;
+	}
+
+	wcomp_mode = monitoring.check_mode;
+	/* call back to be called when event is detected. */
+	monitoring_cb = monitoring.callback;
+
+	CHECK_ERROR(pmic_write_reg(REG_ADC_0, adc_0_reg, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_1, adc_1_reg, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_3, adc_3_reg, PMIC_ALL_BITS));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function deactivates the comparator.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_deactive_comparator(void)
+{
+	unsigned int reg_value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	if (!monitor_en) {
+		pr_debug("mc13783_adc : adc monitoring free\n");
+		return PMIC_ERROR;
+	}
+
+	if (monitor_en) {
+		reg_value = ADC_BIS;
+	}
+
+	/* clear all reg value */
+	CHECK_ERROR(pmic_write_reg(REG_ADC_0, reg_value, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_1, reg_value, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC_3, reg_value, PMIC_ALL_BITS));
+
+	reg_value = 0;
+
+	if (monitor_adc) {
+		CHECK_ERROR(pmic_write_reg
+			    (REG_ADC_4, reg_value, PMIC_ALL_BITS));
+	} else {
+		CHECK_ERROR(pmic_write_reg
+			    (REG_ADC_2, reg_value, PMIC_ALL_BITS));
+	}
+
+	mc13783_adc_release(monitor_adc);
+	monitor_en = false;
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function implements IOCTL controls on a MC13783 ADC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @param        cmd         the command
+ * @param        arg         the parameter
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_adc_ioctl(struct inode *inode, struct file *file,
+			  unsigned int cmd, unsigned long arg)
+{
+	t_adc_convert_param *convert_param;
+	t_touch_mode touch_mode;
+	t_touch_screen touch_sample;
+	unsigned short b_current;
+	t_adc_comp_param *comp_param;
+	if ((_IOC_TYPE(cmd) != 'p') && (_IOC_TYPE(cmd) != 'D'))
+		return -ENOTTY;
+
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0))) {
+			return -ERESTARTSYS;
+		}
+	}
+
+	switch (cmd) {
+	case PMIC_ADC_INIT:
+		pr_debug("init adc\n");
+		CHECK_ERROR(pmic_adc_init());
+		break;
+
+	case PMIC_ADC_DEINIT:
+		pr_debug("deinit adc\n");
+		CHECK_ERROR(pmic_adc_deinit());
+		break;
+
+	case PMIC_ADC_CONVERT:
+		if ((convert_param = kmalloc(sizeof(t_adc_convert_param),
+					     GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(convert_param, (t_adc_convert_param *) arg,
+				   sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		CHECK_ERROR_KFREE(pmic_adc_convert(convert_param->channel,
+						   convert_param->result),
+				  (kfree(convert_param)));
+
+		if (copy_to_user((t_adc_convert_param *) arg, convert_param,
+				 sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		kfree(convert_param);
+		break;
+
+	case PMIC_ADC_CONVERT_8X:
+		if ((convert_param = kmalloc(sizeof(t_adc_convert_param),
+					     GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(convert_param, (t_adc_convert_param *) arg,
+				   sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		CHECK_ERROR_KFREE(pmic_adc_convert_8x(convert_param->channel,
+						      convert_param->result),
+				  (kfree(convert_param)));
+
+		if (copy_to_user((t_adc_convert_param *) arg, convert_param,
+				 sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		kfree(convert_param);
+		break;
+
+	case PMIC_ADC_CONVERT_MULTICHANNEL:
+		if ((convert_param = kmalloc(sizeof(t_adc_convert_param),
+					     GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(convert_param, (t_adc_convert_param *) arg,
+				   sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+
+		CHECK_ERROR_KFREE(pmic_adc_convert_multichnnel
+				  (convert_param->channel,
+				   convert_param->result),
+				  (kfree(convert_param)));
+
+		if (copy_to_user((t_adc_convert_param *) arg, convert_param,
+				 sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		kfree(convert_param);
+		break;
+
+	case PMIC_ADC_SET_TOUCH_MODE:
+		CHECK_ERROR(pmic_adc_set_touch_mode((t_touch_mode) arg));
+		break;
+
+	case PMIC_ADC_GET_TOUCH_MODE:
+		CHECK_ERROR(pmic_adc_get_touch_mode(&touch_mode));
+		if (copy_to_user((t_touch_mode *) arg, &touch_mode,
+				 sizeof(t_touch_mode))) {
+			return -EFAULT;
+		}
+		break;
+
+	case PMIC_ADC_GET_TOUCH_SAMPLE:
+		pr_debug("pmic_adc_ioctl: " "PMIC_ADC_GET_TOUCH_SAMPLE\n");
+		CHECK_ERROR(pmic_adc_get_touch_sample(&touch_sample, 1));
+		if (copy_to_user((t_touch_screen *) arg, &touch_sample,
+				 sizeof(t_touch_screen))) {
+			return -EFAULT;
+		}
+		break;
+
+	case PMIC_ADC_GET_BATTERY_CURRENT:
+		CHECK_ERROR(pmic_adc_get_battery_current(ADC_8CHAN_1X,
+							 &b_current));
+		if (copy_to_user((unsigned short *)arg, &b_current,
+				 sizeof(unsigned short))) {
+
+			return -EFAULT;
+		}
+		break;
+
+	case PMIC_ADC_ACTIVATE_COMPARATOR:
+		if ((comp_param = kmalloc(sizeof(t_adc_comp_param), GFP_KERNEL))
+		    == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(comp_param, (t_adc_comp_param *) arg,
+				   sizeof(t_adc_comp_param))) {
+			kfree(comp_param);
+			return -EFAULT;
+		}
+		CHECK_ERROR_KFREE(pmic_adc_active_comparator(comp_param->wlow,
+							     comp_param->whigh,
+							     comp_param->
+							     channel,
+							     comp_param->
+							     callback),
+				  (kfree(comp_param)));
+		break;
+
+	case PMIC_ADC_DEACTIVE_COMPARATOR:
+		CHECK_ERROR(pmic_adc_deactive_comparator());
+		break;
+
+	default:
+		pr_debug("pmic_adc_ioctl: unsupported ioctl command 0x%x\n",
+			 cmd);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static struct file_operations mc13783_adc_fops = {
+	.owner = THIS_MODULE,
+	.ioctl = pmic_adc_ioctl,
+	.open = pmic_adc_open,
+	.release = pmic_adc_free,
+};
+
+static int pmic_adc_module_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct device *temp_class;
+
+	pmic_adc_major = register_chrdev(0, "pmic_adc", &mc13783_adc_fops);
+
+	if (pmic_adc_major < 0) {
+		pr_debug(KERN_ERR "Unable to get a major for pmic_adc\n");
+		return pmic_adc_major;
+	}
+	init_waitqueue_head(&suspendq);
+
+	pmic_adc_class = class_create(THIS_MODULE, "pmic_adc");
+	if (IS_ERR(pmic_adc_class)) {
+		pr_debug(KERN_ERR "Error creating pmic_adc class.\n");
+		ret = PTR_ERR(pmic_adc_class);
+		goto err_out1;
+	}
+
+	temp_class = device_create(pmic_adc_class, NULL,
+				   MKDEV(pmic_adc_major, 0), NULL, "pmic_adc");
+	if (IS_ERR(temp_class)) {
+		pr_debug(KERN_ERR "Error creating pmic_adc class device.\n");
+		ret = PTR_ERR(temp_class);
+		goto err_out2;
+	}
+
+	ret = pmic_adc_init();
+	if (ret != PMIC_SUCCESS) {
+		pr_debug(KERN_ERR "Error in pmic_adc_init.\n");
+		goto err_out4;
+	}
+
+	pmic_adc_ready = 1;
+	pr_debug(KERN_INFO "PMIC ADC successfully probed\n");
+	return ret;
+
+      err_out4:
+	device_destroy(pmic_adc_class, MKDEV(pmic_adc_major, 0));
+      err_out2:
+	class_destroy(pmic_adc_class);
+      err_out1:
+	unregister_chrdev(pmic_adc_major, "pmic_adc");
+	return ret;
+}
+
+static int pmic_adc_module_remove(struct platform_device *pdev)
+{
+	pmic_adc_ready = 0;
+	pmic_adc_deinit();
+	device_destroy(pmic_adc_class, MKDEV(pmic_adc_major, 0));
+	class_destroy(pmic_adc_class);
+	unregister_chrdev(pmic_adc_major, "pmic_adc");
+	pr_debug(KERN_INFO "PMIC ADC successfully removed\n");
+	return 0;
+}
+
+static struct platform_driver pmic_adc_driver_ldm = {
+	.driver = {
+		   .name = "pmic_adc",
+		   },
+	.suspend = pmic_adc_suspend,
+	.resume = pmic_adc_resume,
+	.probe = pmic_adc_module_probe,
+	.remove = pmic_adc_module_remove,
+};
+
+/*
+ * Initialization and Exit
+ */
+static int __init pmic_adc_module_init(void)
+{
+	pr_debug("PMIC ADC driver loading...\n");
+	return platform_driver_register(&pmic_adc_driver_ldm);
+}
+
+static void __exit pmic_adc_module_exit(void)
+{
+	platform_driver_unregister(&pmic_adc_driver_ldm);
+	pr_debug("PMIC ADC driver successfully unloaded\n");
+}
+
+/*
+ * Module entry points
+ */
+
+module_init(pmic_adc_module_init);
+module_exit(pmic_adc_module_exit);
+
+MODULE_DESCRIPTION("PMIC ADC device driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/mc13783/pmic_adc_defs.h b/drivers/mxc/pmic/mc13783/pmic_adc_defs.h
new file mode 100644
index 000000000000..db1b08232c77
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_adc_defs.h
@@ -0,0 +1,321 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_adc_defs.h
+ * @brief This header contains all defines for PMIC(mc13783) ADC driver.
+ *
+ * @ingroup PMIC_ADC
+ */
+
+#ifndef __MC13783_ADC__DEFS_H__
+#define __MC13783_ADC__DEFS_H__
+
+#define MC13783_ADC_DEVICE "/dev/mc13783_adc"
+
+#define         DEF_ADC_0     0x008000
+#define         DEF_ADC_3     0x000080
+
+#define ADC_NB_AVAILABLE        2
+
+#define MAX_CHANNEL             7
+
+/*
+ * Maximun allowed variation in the three X/Y co-ordinates acquired from
+ * touch-screen
+ */
+#define DELTA_Y_MAX             50
+#define DELTA_X_MAX             50
+
+/* Upon clearing the filter, this is the delay in restarting the filter */
+#define FILTER_MIN_DELAY        4
+
+/* Length of X and Y Touch screen filters */
+#define FILTLEN                 3
+
+#define TS_X_MAX                1000
+#define TS_Y_MAX                1000
+
+#define TS_X_MIN                80
+#define TS_Y_MIN                80
+
+#define MC13783_ADC0_TS_M_LSH	14
+#define MC13783_ADC0_TS_M_WID	3
+/*
+ * ADC 0
+ */
+#define ADC_WAIT_TSI_0		0x001C00
+
+/*
+ * ADC 1
+ */
+
+#define ADC_EN                  0x000001
+#define ADC_SGL_CH              0x000002
+#define ADC_ADSEL               0x000008
+#define ADC_CH_0_POS            5
+#define ADC_CH_0_MASK           0x0000E0
+#define ADC_CH_1_POS            8
+#define ADC_CH_1_MASK           0x000700
+#define ADC_DELAY_POS           11
+#define ADC_DELAY_MASK          0x07F800
+#define ADC_ATO                 0x080000
+#define ASC_ADC                 0x100000
+#define ADC_WAIT_TSI_1		0x300001
+#define ADC_CHRGRAW_D5          0x008000
+
+/*
+ * ADC 2 - 4
+ */
+#define ADD1_RESULT_MASK        0x00000FFC
+#define ADD2_RESULT_MASK        0x00FFC000
+#define ADC_TS_MASK             0x00FFCFFC
+
+/*
+ * ADC 3
+ */
+#define ADC_INC                 0x030000
+#define ADC_BIS                 0x800000
+
+/*
+ * ADC 3
+ */
+#define ADC_NO_ADTRIG           0x200000
+#define ADC_WCOMP               0x040000
+#define ADC_WCOMP_H_POS         0
+#define ADC_WCOMP_L_POS         9
+#define ADC_WCOMP_H_MASK        0x00003F
+#define ADC_WCOMP_L_MASK        0x007E00
+
+#define ADC_MODE_MASK           0x00003F
+
+/*
+ * Interrupt Status 0
+ */
+#define ADC_INT_BISDONEI        0x02
+
+/*!
+ * Define state mode of ADC.
+ */
+typedef enum adc_state {
+	/*!
+	 * Free.
+	 */
+	ADC_FREE,
+	/*!
+	 * Used.
+	 */
+	ADC_USED,
+	/*!
+	 * Monitoring
+	 */
+	ADC_MONITORING,
+} t_adc_state;
+
+/*!
+ * This enumeration, is used to configure the mode of ADC.
+ */
+typedef enum reading_mode {
+	/*!
+	 * Enables lithium cell reading
+	 */
+	M_LITHIUM_CELL = 0x000001,
+	/*!
+	 * Enables charge current reading
+	 */
+	M_CHARGE_CURRENT = 0x000002,
+	/*!
+	 * Enables battery current reading
+	 */
+	M_BATTERY_CURRENT = 0x000004,
+	/*!
+	 * Enables thermistor reading
+	 */
+	M_THERMISTOR = 0x000008,
+	/*!
+	 * Enables die temperature reading
+	 */
+	M_DIE_TEMPERATURE = 0x000010,
+	/*!
+	 * Enables UID reading
+	 */
+	M_UID = 0x000020,
+} t_reading_mode;
+
+/*!
+ * This enumeration, is used to configure the monitoring mode.
+ */
+typedef enum check_mode {
+	/*!
+	 * Comparator low level
+	 */
+	CHECK_LOW,
+	/*!
+	 * Comparator high level
+	 */
+	CHECK_HIGH,
+	/*!
+	 * Comparator low or high level
+	 */
+	CHECK_LOW_OR_HIGH,
+} t_check_mode;
+
+/*!
+ * This structure is used to configure and report adc value.
+ */
+typedef struct {
+	/*!
+	 * Delay before first conversion
+	 */
+	unsigned int delay;
+	/*!
+	 * sets the ATX bit for delay on all conversion
+	 */
+	bool conv_delay;
+	/*!
+	 * Sets the single channel mode
+	 */
+	bool single_channel;
+	/*!
+	 * Selects the set of inputs
+	 */
+	bool group;
+	/*!
+	 * Channel selection 1
+	 */
+	t_channel channel_0;
+	/*!
+	 * Channel selection 2
+	 */
+	t_channel channel_1;
+	/*!
+	 * Used to configure ADC mode with t_reading_mode
+	 */
+	t_reading_mode read_mode;
+	/*!
+	 * Sets the Touch screen mode
+	 */
+	bool read_ts;
+	/*!
+	 * Wait TSI event before touch screen reading
+	 */
+	bool wait_tsi;
+	/*!
+	 * Sets CHRGRAW scaling to divide by 5
+	 * Only supported on 2.0 and higher
+	 */
+	bool chrgraw_devide_5;
+	/*!
+	 * Return ADC values
+	 */
+	unsigned int value[8];
+	/*!
+	 * Return touch screen values
+	 */
+	t_touch_screen ts_value;
+} t_adc_param;
+
+/*!
+ * This structure is used to configure the monitoring mode of ADC.
+ */
+typedef struct {
+	/*!
+	 * Delay before first conversion
+	 */
+	unsigned int delay;
+	/*!
+	 * sets the ATX bit for delay on all conversion
+	 */
+	bool conv_delay;
+	/*!
+	 * Channel selection 1
+	 */
+	t_channel channel;
+	/*!
+	 * Selects the set of inputs
+	 */
+	bool group;
+	/*!
+	 * Used to configure ADC mode with t_reading_mode
+	 */
+	unsigned int read_mode;
+	/*!
+	 * Comparator low level in WCOMP mode
+	 */
+	unsigned int comp_low;
+	/*!
+	 * Comparator high level in WCOMP mode
+	 */
+	unsigned int comp_high;
+	/*!
+	 * Sets type of monitoring (low, high or both)
+	 */
+	t_check_mode check_mode;
+	/*!
+	 * Callback to be launched when event is detected
+	 */
+	void (*callback) (void);
+} t_monitoring_param;
+
+/*!
+ * This function performs filtering and rejection of excessive noise prone
+ * samples.
+ *
+ * @param        ts_curr     Touch screen value
+ *
+ * @return       This function returns 0 on success, -1 otherwise.
+ */
+static int pmic_adc_filter(t_touch_screen * ts_curr);
+
+/*!
+ * This function request a ADC.
+ *
+ * @return      This function returns index of ADC to be used (0 or 1) if successful.
+                return -1 if error.
+ */
+int mc13783_adc_request(bool read_ts);
+
+/*!
+ * This function is used to update buffer of touch screen value in read mode.
+ */
+void update_buffer(void);
+
+/*!
+ * This function release an ADC.
+ *
+ * @param        adc_index     index of ADC to be released.
+ *
+ * @return       This function returns 0 if successful.
+ */
+int mc13783_adc_release(int adc_index);
+
+/*!
+ * This function select the required read_mode for a specific channel.
+ *
+ * @param        channel   The channel to be sampled
+ *
+ * @return       This function returns the requires read_mode
+ */
+t_reading_mode mc13783_set_read_mode(t_channel channel);
+
+/*!
+ * This function read the touch screen value.
+ *
+ * @param        touch_sample    return value of touch screen
+ * @param        wait_tsi    if true, this function is synchronous (wait in TSI event).
+ *
+ * @return       This function returns 0.
+ */
+PMIC_STATUS mc13783_adc_read_ts(t_touch_screen * touch_sample, int wait_tsi);
+
+#endif				/* __MC13783_ADC__DEFS_H__ */
diff --git a/drivers/mxc/pmic/mc13783/pmic_audio.c b/drivers/mxc/pmic/mc13783/pmic_audio.c
new file mode 100644
index 000000000000..6aab4b83f6ea
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_audio.c
@@ -0,0 +1,5876 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_audio.c
+ * @brief Implementation of the PMIC(mc13783) Audio driver APIs.
+ *
+ * The PMIC Audio driver and this API were developed to support the
+ * audio playback, recording, and mixing capabilities of the power
+ * management ICs that are available from Freescale Semiconductor, Inc.
+ *
+ * The following operating modes are supported:
+ *
+ * @verbatim
+       Operating Mode               mc13783
+       ---------------------------- -------
+       Stereo DAC Playback           Yes
+       Stereo DAC Input Mixing       Yes
+       Voice CODEC Playback          Yes
+       Voice CODEC Input Mixing      Yes
+       Voice CODEC Mono Recording    Yes
+       Voice CODEC Stereo Recording  Yes
+       Microphone Bias Control       Yes
+       Output Amplifier Control      Yes
+       Output Mixing Control         Yes
+       Input Amplifier Control       Yes
+       Master/Slave Mode Select      Yes
+       Anti Pop Bias Circuit Control Yes
+   @endverbatim
+ *
+ * Note that the Voice CODEC may also be referred to as the Telephone
+ * CODEC in the PMIC DTS documentation. Also note that, while the power
+ * management ICs do provide similar audio capabilities, each PMIC may
+ * support additional configuration settings and features. Therefore, it
+ * is highly recommended that the appropriate power management IC DTS
+ * documents be used in conjunction with this API interface.
+ *
+ * @ingroup PMIC_AUDIO
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>	/* For tasklet interface.              */
+#include <linux/platform_device.h>	/* For kernel module interface.        */
+#include <linux/init.h>
+#include <linux/spinlock.h>	/* For spinlock interface.             */
+#include <linux/pmic_adc.h>	/* For PMIC ADC driver interface.      */
+#include <linux/pmic_status.h>
+#include <mach/pmic_audio.h>	/* For PMIC Audio driver interface.    */
+
+/*
+ * mc13783 PMIC Audio API
+ */
+
+/* EXPORTED FUNCTIONS */
+EXPORT_SYMBOL(MIN_STDAC_SAMPLING_RATE_HZ);
+EXPORT_SYMBOL(MAX_STDAC_SAMPLING_RATE_HZ);
+EXPORT_SYMBOL(pmic_audio_open);
+EXPORT_SYMBOL(pmic_audio_close);
+EXPORT_SYMBOL(pmic_audio_set_protocol);
+EXPORT_SYMBOL(pmic_audio_get_protocol);
+EXPORT_SYMBOL(pmic_audio_enable);
+EXPORT_SYMBOL(pmic_audio_disable);
+EXPORT_SYMBOL(pmic_audio_reset);
+EXPORT_SYMBOL(pmic_audio_reset_all);
+EXPORT_SYMBOL(pmic_audio_set_callback);
+EXPORT_SYMBOL(pmic_audio_clear_callback);
+EXPORT_SYMBOL(pmic_audio_get_callback);
+EXPORT_SYMBOL(pmic_audio_antipop_enable);
+EXPORT_SYMBOL(pmic_audio_antipop_disable);
+EXPORT_SYMBOL(pmic_audio_digital_filter_reset);
+EXPORT_SYMBOL(pmic_audio_vcodec_set_clock);
+EXPORT_SYMBOL(pmic_audio_vcodec_get_clock);
+EXPORT_SYMBOL(pmic_audio_vcodec_set_rxtx_timeslot);
+EXPORT_SYMBOL(pmic_audio_vcodec_get_rxtx_timeslot);
+EXPORT_SYMBOL(pmic_audio_vcodec_set_secondary_txslot);
+EXPORT_SYMBOL(pmic_audio_vcodec_get_secondary_txslot);
+EXPORT_SYMBOL(pmic_audio_vcodec_set_config);
+EXPORT_SYMBOL(pmic_audio_vcodec_clear_config);
+EXPORT_SYMBOL(pmic_audio_vcodec_get_config);
+EXPORT_SYMBOL(pmic_audio_vcodec_enable_bypass);
+EXPORT_SYMBOL(pmic_audio_vcodec_disable_bypass);
+EXPORT_SYMBOL(pmic_audio_stdac_set_clock);
+EXPORT_SYMBOL(pmic_audio_stdac_get_clock);
+EXPORT_SYMBOL(pmic_audio_stdac_set_rxtx_timeslot);
+EXPORT_SYMBOL(pmic_audio_stdac_get_rxtx_timeslot);
+EXPORT_SYMBOL(pmic_audio_stdac_set_config);
+EXPORT_SYMBOL(pmic_audio_stdac_clear_config);
+EXPORT_SYMBOL(pmic_audio_stdac_get_config);
+EXPORT_SYMBOL(pmic_audio_input_set_config);
+EXPORT_SYMBOL(pmic_audio_input_clear_config);
+EXPORT_SYMBOL(pmic_audio_input_get_config);
+EXPORT_SYMBOL(pmic_audio_vcodec_set_mic);
+EXPORT_SYMBOL(pmic_audio_vcodec_get_mic);
+EXPORT_SYMBOL(pmic_audio_vcodec_set_mic_on_off);
+EXPORT_SYMBOL(pmic_audio_vcodec_get_mic_on_off);
+EXPORT_SYMBOL(pmic_audio_vcodec_set_record_gain);
+EXPORT_SYMBOL(pmic_audio_vcodec_get_record_gain);
+EXPORT_SYMBOL(pmic_audio_vcodec_enable_micbias);
+EXPORT_SYMBOL(pmic_audio_vcodec_disable_micbias);
+EXPORT_SYMBOL(pmic_audio_vcodec_enable_mixer);
+EXPORT_SYMBOL(pmic_audio_vcodec_disable_mixer);
+EXPORT_SYMBOL(pmic_audio_stdac_enable_mixer);
+EXPORT_SYMBOL(pmic_audio_stdac_disable_mixer);
+EXPORT_SYMBOL(pmic_audio_output_set_port);
+EXPORT_SYMBOL(pmic_audio_output_get_port);
+EXPORT_SYMBOL(pmic_audio_output_clear_port);
+EXPORT_SYMBOL(pmic_audio_output_set_stereo_in_gain);
+EXPORT_SYMBOL(pmic_audio_output_get_stereo_in_gain);
+EXPORT_SYMBOL(pmic_audio_output_set_pgaGain);
+EXPORT_SYMBOL(pmic_audio_output_get_pgaGain);
+EXPORT_SYMBOL(pmic_audio_output_enable_mixer);
+EXPORT_SYMBOL(pmic_audio_output_disable_mixer);
+EXPORT_SYMBOL(pmic_audio_output_set_balance);
+EXPORT_SYMBOL(pmic_audio_output_get_balance);
+EXPORT_SYMBOL(pmic_audio_output_enable_mono_adder);
+EXPORT_SYMBOL(pmic_audio_output_disable_mono_adder);
+EXPORT_SYMBOL(pmic_audio_output_set_mono_adder_gain);
+EXPORT_SYMBOL(pmic_audio_output_get_mono_adder_gain);
+EXPORT_SYMBOL(pmic_audio_output_set_config);
+EXPORT_SYMBOL(pmic_audio_output_clear_config);
+EXPORT_SYMBOL(pmic_audio_output_get_config);
+EXPORT_SYMBOL(pmic_audio_output_enable_phantom_ground);
+EXPORT_SYMBOL(pmic_audio_output_disable_phantom_ground);
+EXPORT_SYMBOL(pmic_audio_set_autodetect);
+#ifdef DEBUG_AUDIO
+EXPORT_SYMBOL(pmic_audio_dump_registers);
+#endif				/* DEBUG_AUDIO */
+/*!
+ * Define the minimum sampling rate (in Hz) that is supported by the
+ * Stereo DAC.
+ */
+const unsigned MIN_STDAC_SAMPLING_RATE_HZ = 8000;
+
+/*!
+ * Define the maximum sampling rate (in Hz) that is supported by the
+ * Stereo DAC.
+ */
+const unsigned MAX_STDAC_SAMPLING_RATE_HZ = 96000;
+
+/*! @def SET_BITS
+ * Set a register field to a given value.
+ */
+#define SET_BITS(reg, field, value)    (((value) << reg.field.offset) & \
+                                        reg.field.mask)
+/*! @def GET_BITS
+ * Get the current value of a given register field.
+ */
+#define GET_BITS(reg, field, value)    (((value) & reg.field.mask) >> \
+                                        reg.field.offset)
+
+/*!
+ * @brief Define the possible states for a device handle.
+ *
+ * This enumeration is used to track the current state of each device handle.
+ */
+typedef enum {
+	HANDLE_FREE,		/*!< Handle is available for use. */
+	HANDLE_IN_USE		/*!< Handle is currently in use.  */
+} HANDLE_STATE;
+
+/*!
+ * @brief Identifies the hardware interrupt source.
+ *
+ * This enumeration identifies which of the possible hardware interrupt
+ * sources actually caused the current interrupt handler to be called.
+ */
+typedef enum {
+	CORE_EVENT_MC2BI,	/*!< Microphone Bias 2 detect. */
+	CORE_EVENT_HSDETI,	/*!< Detect Headset attach  */
+	CORE_EVENT_HSLI,	/*!< Detect Stereo Headset  */
+	CORE_EVENT_ALSPTHI,	/*!< Detect Thermal shutdown of ALSP  */
+	CORE_EVENT_AHSSHORTI	/*!< Detect Short circuit on AHS outputs  */
+} PMIC_CORE_EVENT;
+
+/*!
+ * @brief This structure is used to track the state of a microphone input.
+ */
+typedef struct {
+	PMIC_AUDIO_INPUT_PORT mic;	/*!< Microphone input port.      */
+	PMIC_AUDIO_INPUT_MIC_STATE micOnOff;	/*!< Microphone On/Off state.    */
+	PMIC_AUDIO_MIC_AMP_MODE ampMode;	/*!< Input amplifier mode.       */
+	PMIC_AUDIO_MIC_GAIN gain;	/*!< Input amplifier gain level. */
+} PMIC_MICROPHONE_STATE;
+
+/*!
+ * @brief Tracks whether a headset is currently attached or not.
+ */
+typedef enum {
+	NO_HEADSET,		/*!< No headset currently attached. */
+	HEADSET_ON		/*!< Headset has been attached.     */
+} HEADSET_STATUS;
+
+/*!
+ * @brief mc13783 only enum that indicates the path to output taken
+ * by the voice codec output
+ */
+typedef enum {
+	VCODEC_DIRECT_OUT,	/*!< Vcodec signal out direct */
+	VCODEC_MIXER_OUT	/*!< Output via the mixer     */
+} PMIC_AUDIO_VCODEC_OUTPUT_PATH;
+
+/*!
+ * @brief This structure is used to define a specific hardware register field.
+ *
+ * All hardware register fields are defined using an offset to the LSB
+ * and a mask. The offset is used to right shift a register value before
+ * applying the mask to actually obtain the value of the field.
+ */
+typedef struct {
+	const unsigned char offset;	/*!< Offset of LSB of register field.          */
+	const unsigned int mask;	/*!< Mask value used to isolate register field. */
+} REGFIELD;
+
+/*!
+ * @brief This structure lists all fields of the AUD_CODEC hardware register.
+ */
+typedef struct {
+	REGFIELD CDCSSISEL;	/*!< codec SSI bus select              */
+	REGFIELD CDCCLKSEL;	/*!< Codec clock input select          */
+	REGFIELD CDCSM;		/*!< Codec slave / master select       */
+	REGFIELD CDCBCLINV;	/*!< Codec bitclock inversion          */
+	REGFIELD CDCFSINV;	/*!< Codec framesync inversion         */
+	REGFIELD CDCFS;		/*!< Bus protocol selection - 2 bits   */
+	REGFIELD CDCCLK;	/*!< Codec clock setting - 3 bits      */
+	REGFIELD CDCFS8K16K;	/*!< Codec framesync select            */
+	REGFIELD CDCEN;		/*!< Codec enable                      */
+	REGFIELD CDCCLKEN;	/*!< Codec clocking enable             */
+	REGFIELD CDCTS;		/*!< Codec SSI tristate                */
+	REGFIELD CDCDITH;	/*!< Codec dithering                   */
+	REGFIELD CDCRESET;	/*!< Codec filter reset                */
+	REGFIELD CDCBYP;	/*!< Codec bypass                      */
+	REGFIELD CDCALM;	/*!< Codec analog loopback             */
+	REGFIELD CDCDLM;	/*!< Codec digital loopback            */
+	REGFIELD AUDIHPF;	/*!< Transmit high pass filter enable  */
+	REGFIELD AUDOHPF;	/*!< Receive high pass filter enable   */
+} REGISTER_AUD_CODEC;
+
+/*!
+ * @brief This variable is used to access the AUD_CODEC hardware register.
+ *
+ * This variable defines how to access all of the fields within the
+ * AUD_CODEC hardware register. The initial values consist of the offset
+ * and mask values needed to access each of the register fields.
+ */
+static const REGISTER_AUD_CODEC regAUD_CODEC = {
+	{0, 0x000001},		/* CDCSSISEL    */
+	{1, 0x000002},		/* CDCCLKSEL    */
+	{2, 0x000004},		/* CDCSM        */
+	{3, 0x000008},		/* CDCBCLINV    */
+	{4, 0x000010},		/* CDCFSINV     */
+	{5, 0x000060},		/* CDCFS        */
+	{7, 0x000380},		/* CDCCLK       */
+	{10, 0x000400},		/* CDCFS8K16K   */
+	{11, 0x000800},		/* CDCEN        */
+	{12, 0x001000},		/* CDCCLKEN     */
+	{13, 0x002000},		/* CDCTS        */
+	{14, 0x004000},		/* CDCDITH      */
+	{15, 0x008000},		/* CDCRESET     */
+	{16, 0x010000},		/* CDCBYP       */
+	{17, 0x020000},		/* CDCALM       */
+	{18, 0x040000},		/* CDCDLM       */
+	{19, 0x080000},		/* AUDIHPF      */
+	{20, 0x100000}		/* AUDOHPF      */
+	/* Unused       */
+	/* Unused       */
+	/* Unused       */
+
+};
+
+/*!
+ * @brief This structure lists all fields of the ST_DAC hardware register.
+ */
+ /* VVV */
+typedef struct {
+	REGFIELD STDCSSISEL;	/*!< Stereo DAC SSI bus select                            */
+	REGFIELD STDCCLKSEL;	/*!< Stereo DAC clock input select                        */
+	REGFIELD STDCSM;	/*!< Stereo DAC slave / master select                     */
+	REGFIELD STDCBCLINV;	/*!< Stereo DAC bitclock inversion                        */
+	REGFIELD STDCFSINV;	/*!< Stereo DAC framesync inversion                       */
+	REGFIELD STDCFS;	/*!< Bus protocol selection - 2 bits                      */
+	REGFIELD STDCCLK;	/*!< Stereo DAC clock setting - 3 bits                    */
+	REGFIELD STDCFSDLYB;	/*!< Stereo DAC framesync delay bar                       */
+	REGFIELD STDCEN;	/*!< Stereo DAC enable                                    */
+	REGFIELD STDCCLKEN;	/*!< Stereo DAC clocking enable                           */
+	REGFIELD STDCRESET;	/*!< Stereo DAC filter reset                              */
+	REGFIELD SPDIF;		/*!< Stereo DAC SSI SPDIF mode. Mode no longer available. */
+	REGFIELD SR;		/*!< Stereo DAC sample rate - 4 bits                      */
+} REGISTER_ST_DAC;
+
+/*!
+ * @brief This variable is used to access the ST_DAC hardware register.
+ *
+ * This variable defines how to access all of the fields within the
+ * ST_DAC hardware register. The initial values consist of the offset
+ * and mask values needed to access each of the register fields.
+ */
+static const REGISTER_ST_DAC regST_DAC = {
+	{0, 0x000001},		/* STDCSSISEL        */
+	{1, 0x000002},		/* STDCCLKSEL        */
+	{2, 0x000004},		/* STDCSM            */
+	{3, 0x000008},		/* STDCBCLINV        */
+	{4, 0x000010},		/* STDCFSINV         */
+	{5, 0x000060},		/* STDCFS            */
+	{7, 0x000380},		/* STDCCLK           */
+	{10, 0x000400},		/* STDCFSDLYB        */
+	{11, 0x000800},		/* STDCEN            */
+	{12, 0x001000},		/* STDCCLKEN         */
+	{15, 0x008000},		/* STDCRESET         */
+	{16, 0x010000},		/* SPDIF             */
+	{17, 0x1E0000}		/* SR                */
+};
+
+/*!
+ * @brief This structure lists all of the fields in the SSI_NETWORK hardware register.
+ */
+typedef struct {
+	REGFIELD CDCTXRXSLOT;	/*!< Codec timeslot assignment  - 2 bits                                  */
+	REGFIELD CDCTXSECSLOT;	/*!< Codec secondary transmit timeslot - 2 bits                           */
+	REGFIELD CDCRXSECSLOT;	/*!< Codec secondary receive timeslot - 2 bits                            */
+	REGFIELD CDCRXSECGAIN;	/*!< Codec secondary receive channel gain setting - 2 bits                */
+	REGFIELD CDCSUMGAIN;	/*!< Codec summed receive signal gain setting                             */
+	REGFIELD CDCFSDLY;	/*!< Codec framesync delay                                                */
+	REGFIELD STDCSLOTS;	/*!< Stereo DAC number of timeslots select  - 2 bits                      */
+	REGFIELD STDCRXSLOT;	/*!< Stereo DAC timeslot assignment - 2 bits                              */
+	REGFIELD STDCRXSECSLOT;	/*!< Stereo DAC secondary receive timeslot  - 2 bits                      */
+	REGFIELD STDCRXSECGAIN;	/*!< Stereo DAC secondary receive channel gain setting   - 2 bits         */
+	REGFIELD STDCSUMGAIN;	/*!< Stereo DAC summed receive signal gain setting                        */
+} REGISTER_SSI_NETWORK;
+
+/*!
+ * @brief This variable is used to access the SSI_NETWORK hardware register.
+ *
+ * This variable defines how to access all of the fields within the
+ * SSI_NETWORK hardware register. The initial values consist of the offset
+ * and mask values needed to access each of the register fields.
+ */
+static const REGISTER_SSI_NETWORK regSSI_NETWORK = {
+	{2, 0x00000c},		/* CDCTXRXSLOT          */
+	{4, 0x000030},		/* CDCTXSECSLOT         */
+	{6, 0x0000c0},		/* CDCRXSECSLOT         */
+	{8, 0x000300},		/* CDCRXSECGAIN         */
+	{10, 0x000400},		/* CDCSUMGAIN           */
+	{11, 0x000800},		/* CDCFSDLY             */
+	{12, 0x003000},		/* STDCSLOTS            */
+	{14, 0x00c000},		/* STDCRXSLOT           */
+	{16, 0x030000},		/* STDCRXSECSLOT        */
+	{18, 0x0c0000},		/* STDCRXSECGAIN        */
+	{20, 0x100000}		/* STDCSUMGAIN          */
+};
+
+/*!
+ * @brief This structure lists all fields of the AUDIO_TX hardware register.
+ *
+ *
+ */
+typedef struct {
+	REGFIELD MC1BEN;	/*!< Microphone bias 1 enable                                */
+	REGFIELD MC2BEN;	/*!< Microphone bias 2 enable                                */
+	REGFIELD MC2BDETDBNC;	/*!< Microphone bias detect debounce setting                 */
+	REGFIELD MC2BDETEN;	/*!< Microphone bias 2 detect enable                         */
+	REGFIELD AMC1REN;	/*!< Amplifier Amc1R enable                                  */
+	REGFIELD AMC1RITOV;	/*!< Amplifier Amc1R current to voltage mode enable          */
+	REGFIELD AMC1LEN;	/*!< Amplifier Amc1L enable                                  */
+	REGFIELD AMC1LITOV;	/*!< Amplifier Amc1L current to voltage mode enable          */
+	REGFIELD AMC2EN;	/*!< Amplifier Amc2 enable                                   */
+	REGFIELD AMC2ITOV;	/*!< Amplifier Amc2 current to voltage mode enable           */
+	REGFIELD ATXINEN;	/*!< Amplifier Atxin enable                                  */
+	REGFIELD ATXOUTEN;	/*!< Reserved for output TXOUT enable, currently not used    */
+	REGFIELD RXINREC;	/*!< RXINR/RXINL to voice CODEC ADC routing enable           */
+	REGFIELD PGATXR;	/*!< Transmit gain setting right - 5 bits                    */
+	REGFIELD PGATXL;	/*!< Transmit gain setting left  - 5 bits                    */
+} REGISTER_AUDIO_TX;
+
+/*!
+ * @brief This variable is used to access the AUDIO_TX hardware register.
+ *
+ * This variable defines how to access all of the fields within the
+ * AUDIO_TX hardware register. The initial values consist of the offset
+ * and mask values needed to access each of the register fields.
+ */
+static const REGISTER_AUDIO_TX regAUDIO_TX = {
+	{0, 0x000001},		/* MC1BEN              */
+	{1, 0x000002},		/* MC2BEN              */
+	{2, 0x000004},		/* MC2BDETDBNC         */
+	{3, 0x000008},		/* MC2BDETEN           */
+	{5, 0x000020},		/* AMC1REN             */
+	{6, 0x000040},		/* AMC1RITOV           */
+	{7, 0x000080},		/* AMC1LEN             */
+	{8, 0x000100},		/* AMC1LITOV           */
+	{9, 0x000200},		/* AMC2EN              */
+	{10, 0x000400},		/* AMC2ITOV            */
+	{11, 0x000800},		/* ATXINEN             */
+	{12, 0x001000},		/* ATXOUTEN            */
+	{13, 0x002000},		/* RXINREC             */
+	{14, 0x07c000},		/* PGATXR              */
+	{19, 0xf80000}		/* PGATXL              */
+};
+
+/*!
+ * @brief This structure lists all fields of the AUDIO_RX_0 hardware register.
+ */
+typedef struct {
+	REGFIELD VAUDIOON;	/*!<    Forces VAUDIO in active on mode               */
+	REGFIELD BIASEN;	/*!<    Audio bias enable                             */
+	REGFIELD BIASSPEED;	/*!<    Turn on ramp speed of the audio bias          */
+	REGFIELD ASPEN;		/*!<    Amplifier Asp enable                          */
+	REGFIELD ASPSEL;	/*!<    Asp input selector                            */
+	REGFIELD ALSPEN;	/*!<    Amplifier Alsp enable                         */
+	REGFIELD ALSPREF;	/*!<    Bias Alsp at common audio reference           */
+	REGFIELD ALSPSEL;	/*!<    Alsp input selector                           */
+	REGFIELD LSPLEN;	/*!<    Output LSPL enable                            */
+	REGFIELD AHSREN;	/*!<    Amplifier AhsR enable                         */
+	REGFIELD AHSLEN;	/*!<    Amplifier AhsL enable                         */
+	REGFIELD AHSSEL;	/*!<    Ahsr and Ahsl input selector                  */
+	REGFIELD HSPGDIS;	/*!<    Phantom ground disable                        */
+	REGFIELD HSDETEN;	/*!<    Headset detect enable                         */
+	REGFIELD HSDETAUTOB;	/*!<    Amplifier state determined by headset detect  */
+	REGFIELD ARXOUTREN;	/*!<    Output RXOUTR enable                          */
+	REGFIELD ARXOUTLEN;	/*!<    Output RXOUTL enable                          */
+	REGFIELD ARXOUTSEL;	/*!<    Arxout input selector                         */
+	REGFIELD CDCOUTEN;	/*!<    Output CDCOUT enable                          */
+	REGFIELD HSLDETEN;	/*!<    Headset left channel detect enable            */
+	REGFIELD ADDCDC;	/*!<    Adder channel codec selection                 */
+	REGFIELD ADDSTDC;	/*!<    Adder channel stereo DAC selection            */
+	REGFIELD ADDRXIN;	/*!<    Adder channel line in selection               */
+} REGISTER_AUDIO_RX_0;
+
+/*!
+ * @brief This variable is used to access the AUDIO_RX_0 hardware register.
+ *
+ * This variable defines how to access all of the fields within the
+ * AUDIO_RX_0 hardware register. The initial values consist of the offset
+ * and mask values needed to access each of the register fields.
+ */
+static const REGISTER_AUDIO_RX_0 regAUDIO_RX_0 = {
+	{0, 0x000001},		/* VAUDIOON    */
+	{1, 0x000002},		/* BIASEN      */
+	{2, 0x000004},		/* BIASSPEED   */
+	{3, 0x000008},		/* ASPEN       */
+	{4, 0x000010},		/* ASPSEL      */
+	{5, 0x000020},		/* ALSPEN      */
+	{6, 0x000040},		/* ALSPREF     */
+	{7, 0x000080},		/* ALSPSEL     */
+	{8, 0x000100},		/* LSPLEN      */
+	{9, 0x000200},		/* AHSREN      */
+	{10, 0x000400},		/* AHSLEN      */
+	{11, 0x000800},		/* AHSSEL      */
+	{12, 0x001000},		/* HSPGDIS     */
+	{13, 0x002000},		/* HSDETEN     */
+	{14, 0x004000},		/* HSDETAUTOB  */
+	{15, 0x008000},		/* ARXOUTREN   */
+	{16, 0x010000},		/* ARXOUTLEN   */
+	{17, 0x020000},		/* ARXOUTSEL   */
+	{18, 0x040000},		/* CDCOUTEN    */
+	{19, 0x080000},		/* HSLDETEN    */
+	{21, 0x200000},		/* ADDCDC      */
+	{22, 0x400000},		/* ADDSTDC     */
+	{23, 0x800000}		/* ADDRXIN     */
+};
+
+/*!
+ * @brief This structure lists all fields of the AUDIO_RX_1 hardware register.
+ */
+typedef struct {
+	REGFIELD PGARXEN;	/*!<    Codec receive PGA enable                 */
+	REGFIELD PGARX;		/*!<    Codec receive gain setting - 4 bits      */
+	REGFIELD PGASTEN;	/*!<    Stereo DAC PGA enable                    */
+	REGFIELD PGAST;		/*!<    Stereo DAC gain setting  - 4 bits        */
+	REGFIELD ARXINEN;	/*!<    Amplifier Arx enable                     */
+	REGFIELD ARXIN;		/*!<    Amplifier Arx additional gain setting    */
+	REGFIELD PGARXIN;	/*!<    PGArxin gain setting  - 4 bits           */
+	REGFIELD MONO;		/*!<    Mono adder setting   - 2 bits            */
+	REGFIELD BAL;		/*!<    Balance control      - 3 bits            */
+	REGFIELD BALLR;		/*!<    Left / right balance                     */
+} REGISTER_AUDIO_RX_1;
+
+/*!
+ * @brief This variable is used to access the AUDIO_RX_1 hardware register.
+ *
+ * This variable defines how to access all of the fields within the
+ * AUDIO_RX_1 hardware register. The initial values consist of the offset
+ * and mask values needed to access each of the register fields.
+ */
+static const REGISTER_AUDIO_RX_1 regAUDIO_RX_1 = {
+	{0, 0x000001},		/* PGARXEN    */
+	{1, 0x00001e},		/* PGARX      */
+	{5, 0x000020},		/* PGASTEN   */
+	{6, 0x0003c0},		/* PGAST       */
+	{10, 0x000400},		/* ARXINEN      */
+	{11, 0x000800},		/* ARXIN      */
+	{12, 0x00f000},		/* PGARXIN     */
+	{16, 0x030000},		/* MONO     */
+	{18, 0x1c0000},		/* BAL      */
+	{21, 0x200000}		/* BALLR      */
+};
+
+/*! Define a mask to access the entire hardware register. */
+static const unsigned int REG_FULLMASK = 0xffffff;
+
+/*! Reset value for the AUD_CODEC register. */
+static const unsigned int RESET_AUD_CODEC = 0x180027;
+
+/*! Reset value for the ST_DAC register.
+ *
+ *  Note that we avoid resetting any of the arbitration bits.
+ */
+static const unsigned int RESET_ST_DAC = 0x0E0004;
+
+/*! Reset value for the SSI_NETWORK register. */
+static const unsigned int RESET_SSI_NETWORK = 0x013060;
+
+/*! Reset value for the AUDIO_TX register.
+ *
+ *  Note that we avoid resetting any of the arbitration bits.
+ */
+static const unsigned int RESET_AUDIO_TX = 0x420000;
+
+/*! Reset value for the AUDIO_RX_0 register. */
+static const unsigned int RESET_AUDIO_RX_0 = 0x001000;
+
+/*! Reset value for the AUDIO_RX_1 register. */
+static const unsigned int RESET_AUDIO_RX_1 = 0x00D35A;
+
+/*! Reset mask for the SSI network Vcodec part. first 12 bits
+ * 0 - 11 */
+static const unsigned int REG_SSI_VCODEC_MASK = 0x000fff;
+
+/*! Reset mask for the SSI network STDAC part. last 12 bits
+ * 12 - 24 */
+static const unsigned int REG_SSI_STDAC_MASK = 0xfff000;
+
+/*! Constant NULL value for initializing/reseting the audio handles. */
+static const PMIC_AUDIO_HANDLE AUDIO_HANDLE_NULL = (PMIC_AUDIO_HANDLE) NULL;
+
+/*!
+ * @brief This structure maintains the current state of the Stereo DAC.
+ */
+typedef struct {
+	PMIC_AUDIO_HANDLE handle;	/*!< Handle used to access
+					   the Stereo DAC.         */
+	HANDLE_STATE handleState;	/*!< Current handle state.   */
+	PMIC_AUDIO_DATA_BUS busID;	/*!< Data bus used to access
+					   the Stereo DAC.         */
+	bool protocol_set;
+	PMIC_AUDIO_BUS_PROTOCOL protocol;	/*!< Data bus protocol.      */
+	PMIC_AUDIO_BUS_MODE masterSlave;	/*!< Master/Slave mode
+						   select.                 */
+	PMIC_AUDIO_NUMSLOTS numSlots;	/*!< Number of timeslots
+					   used.                   */
+	PMIC_AUDIO_CALLBACK callback;	/*!< Event notification
+					   callback function
+					   pointer.                */
+	PMIC_AUDIO_EVENTS eventMask;	/*!< Event notification mask. */
+	PMIC_AUDIO_CLOCK_IN_SOURCE clockIn;	/*!< Stereo DAC clock input
+						   source select.          */
+	PMIC_AUDIO_STDAC_SAMPLING_RATE samplingRate;	/*!< Stereo DAC sampling rate
+							   select.                 */
+	PMIC_AUDIO_STDAC_CLOCK_IN_FREQ clockFreq;	/*!< Stereo DAC clock input
+							   frequency.              */
+	PMIC_AUDIO_CLOCK_INVERT invert;	/*!< Stereo DAC clock signal
+					   invert select.          */
+	PMIC_AUDIO_STDAC_TIMESLOTS timeslot;	/*!< Stereo DAC data
+						   timeslots select.       */
+	PMIC_AUDIO_STDAC_CONFIG config;	/*!< Stereo DAC configuration
+					   options.                */
+} PMIC_AUDIO_STDAC_STATE;
+
+/*!
+ * @brief This variable maintains the current state of the Stereo DAC.
+ *
+ * This variable tracks the current state of the Stereo DAC audio hardware
+ * along with any information that is required by the device driver to
+ * manage the hardware (e.g., callback functions and event notification
+ * masks).
+ *
+ * The initial values represent the reset/power on state of the Stereo DAC.
+ */
+static PMIC_AUDIO_STDAC_STATE stDAC = {
+	(PMIC_AUDIO_HANDLE) NULL,	/* handle       */
+	HANDLE_FREE,		/* handleState  */
+	AUDIO_DATA_BUS_1,	/* busID        */
+	false,
+	NORMAL_MSB_JUSTIFIED_MODE,	/* protocol     */
+	BUS_MASTER_MODE,	/* masterSlave  */
+	USE_2_TIMESLOTS,	/* numSlots     */
+	(PMIC_AUDIO_CALLBACK) NULL,	/* callback     */
+	(PMIC_AUDIO_EVENTS) NULL,	/* eventMask    */
+	CLOCK_IN_CLIA,		/* clockIn      */
+	STDAC_RATE_44_1_KHZ,	/* samplingRate */
+	STDAC_CLI_13MHZ,	/* clockFreq    */
+	NO_INVERT,		/* invert       */
+	USE_TS0_TS1,		/* timeslot     */
+	(PMIC_AUDIO_STDAC_CONFIG) 0	/* config       */
+};
+
+/*!
+ * @brief This structure maintains the current state of the Voice CODEC.
+ */
+typedef struct {
+	PMIC_AUDIO_HANDLE handle;	/*!< Handle used to access
+					   the Voice CODEC.       */
+	HANDLE_STATE handleState;	/*!< Current handle state.  */
+	PMIC_AUDIO_DATA_BUS busID;	/*!< Data bus used to access
+					   the Voice CODEC.       */
+	bool protocol_set;
+	PMIC_AUDIO_BUS_PROTOCOL protocol;	/*!< Data bus protocol.     */
+	PMIC_AUDIO_BUS_MODE masterSlave;	/*!< Master/Slave mode
+						   select.                */
+	PMIC_AUDIO_NUMSLOTS numSlots;	/*!< Number of timeslots
+					   used.                  */
+	PMIC_AUDIO_CALLBACK callback;	/*!< Event notification
+					   callback function
+					   pointer.               */
+	PMIC_AUDIO_EVENTS eventMask;	/*!< Event notification
+					   mask.                  */
+	PMIC_AUDIO_CLOCK_IN_SOURCE clockIn;	/*!< Voice CODEC clock input
+						   source select.         */
+	PMIC_AUDIO_VCODEC_SAMPLING_RATE samplingRate;	/*!< Voice CODEC sampling
+							   rate select.           */
+	PMIC_AUDIO_VCODEC_CLOCK_IN_FREQ clockFreq;	/*!< Voice CODEC clock input
+							   frequency.             */
+	PMIC_AUDIO_CLOCK_INVERT invert;	/*!< Voice CODEC clock
+					   signal invert select.  */
+	PMIC_AUDIO_VCODEC_TIMESLOT timeslot;	/*!< Voice CODEC data
+						   timeslot select.       */
+	PMIC_AUDIO_VCODEC_TIMESLOT secondaryTXtimeslot;
+
+	PMIC_AUDIO_VCODEC_CONFIG config;	/*!< Voice CODEC
+						   configuration
+						   options.            */
+	PMIC_MICROPHONE_STATE leftChannelMic;	/*!< Left channel
+						   microphone
+						   configuration.      */
+	PMIC_MICROPHONE_STATE rightChannelMic;	/*!< Right channel
+						   microphone
+						   configuration.      */
+} PMIC_AUDIO_VCODEC_STATE;
+
+/*!
+ * @brief This variable maintains the current state of the Voice CODEC.
+ *
+ * This variable tracks the current state of the Voice CODEC audio hardware
+ * along with any information that is required by the device driver to
+ * manage the hardware (e.g., callback functions and event notification
+ * masks).
+ *
+ * The initial values represent the reset/power on state of the Voice CODEC.
+ */
+static PMIC_AUDIO_VCODEC_STATE vCodec = {
+	(PMIC_AUDIO_HANDLE) NULL,	/* handle          */
+	HANDLE_FREE,		/* handleState     */
+	AUDIO_DATA_BUS_2,	/* busID           */
+	false,
+	NETWORK_MODE,		/* protocol        */
+	BUS_SLAVE_MODE,		/* masterSlave     */
+	USE_4_TIMESLOTS,	/* numSlots        */
+	(PMIC_AUDIO_CALLBACK) NULL,	/* callback        */
+	(PMIC_AUDIO_EVENTS) NULL,	/* eventMask       */
+	CLOCK_IN_CLIB,		/* clockIn         */
+	VCODEC_RATE_8_KHZ,	/* samplingRate    */
+	VCODEC_CLI_13MHZ,	/* clockFreq       */
+	NO_INVERT,		/* invert          */
+	USE_TS0,		/* timeslot pri    */
+	USE_TS2,		/* timeslot sec TX */
+	INPUT_HIGHPASS_FILTER | OUTPUT_HIGHPASS_FILTER,	/* config          */
+	/* leftChannelMic  */
+	{NO_MIC,		/*    mic          */
+	 MICROPHONE_OFF,	/*    micOnOff     */
+	 AMP_OFF,		/*    ampMode      */
+	 MIC_GAIN_0DB		/*    gain         */
+	 },
+	/* rightChannelMic */
+	{NO_MIC,		/*    mic          */
+	 MICROPHONE_OFF,	/*    micOnOff     */
+	 AMP_OFF,		/*    ampMode      */
+	 MIC_GAIN_0DB		/*    gain         */
+	 }
+};
+
+/*!
+ * @brief This maintains the current state of the External Stereo Input.
+ */
+typedef struct {
+	PMIC_AUDIO_HANDLE handle;	/*!< Handle used to access the
+					   External Stereo Inputs.     */
+	HANDLE_STATE handleState;	/*!< Current handle state.       */
+	PMIC_AUDIO_CALLBACK callback;	/*!< Event notification callback
+					   function pointer.           */
+	PMIC_AUDIO_EVENTS eventMask;	/*!< Event notification mask.    */
+	PMIC_AUDIO_STEREO_IN_GAIN inputGain;	/*!< External Stereo Input
+						   amplifier gain level.       */
+} PMIC_AUDIO_EXT_STEREO_IN_STATE;
+
+/*!
+ * @brief This maintains the current state of the External Stereo Input.
+ *
+ * This variable tracks the current state of the External Stereo Input audio
+ * hardware along with any information that is required by the device driver
+ * to manage the hardware (e.g., callback functions and event notification
+ * masks).
+ *
+ * The initial values represent the reset/power on state of the External
+ * Stereo Input.
+ */
+static PMIC_AUDIO_EXT_STEREO_IN_STATE extStereoIn = {
+	(PMIC_AUDIO_HANDLE) NULL,	/* handle      */
+	HANDLE_FREE,		/* handleState */
+	(PMIC_AUDIO_CALLBACK) NULL,	/* callback    */
+	(PMIC_AUDIO_EVENTS) NULL,	/* eventMask   */
+	STEREO_IN_GAIN_0DB	/* inputGain   */
+};
+
+/*!
+ * @brief This maintains the current state of the callback & Eventmask.
+ */
+typedef struct {
+	PMIC_AUDIO_CALLBACK callback;	/*!< Event notification callback
+					   function pointer.           */
+	PMIC_AUDIO_EVENTS eventMask;	/*!< Event notification mask.    */
+} PMIC_AUDIO_EVENT_STATE;
+
+static PMIC_AUDIO_EVENT_STATE event_state = {
+	(PMIC_AUDIO_CALLBACK) NULL,	/*Callback */
+	(PMIC_AUDIO_EVENTS) NULL,	/* EventMask */
+
+};
+
+/*!
+ * @brief This maintains the current state of the Audio Output Section.
+ */
+typedef struct {
+	PMIC_AUDIO_OUTPUT_PORT outputPort;	/*!< Current audio
+						   output port.     */
+	PMIC_AUDIO_OUTPUT_PGA_GAIN vCodecoutputPGAGain;	/*!< Output PGA gain
+							   level codec      */
+	PMIC_AUDIO_OUTPUT_PGA_GAIN stDacoutputPGAGain;	/*!< Output PGA gain
+							   level stDAC      */
+	PMIC_AUDIO_OUTPUT_PGA_GAIN extStereooutputPGAGain;	/*!< Output PGA gain
+								   level stereo ext */
+	PMIC_AUDIO_OUTPUT_BALANCE_GAIN balanceLeftGain;	/*!< Left channel
+							   balance gain
+							   level.           */
+	PMIC_AUDIO_OUTPUT_BALANCE_GAIN balanceRightGain;	/*!< Right channel
+								   balance gain
+								   level.           */
+	PMIC_AUDIO_MONO_ADDER_OUTPUT_GAIN monoAdderGain;	/*!< Mono adder gain
+								   level.           */
+	PMIC_AUDIO_OUTPUT_CONFIG config;	/*!< Audio output
+						   section config
+						   options.         */
+	PMIC_AUDIO_VCODEC_OUTPUT_PATH vCodecOut;
+
+} PMIC_AUDIO_AUDIO_OUTPUT_STATE;
+
+/*!
+ * @brief This variable maintains the current state of the Audio Output Section.
+ *
+ * This variable tracks the current state of the Audio Output Section.
+ *
+ * The initial values represent the reset/power on state of the Audio
+ * Output Section.
+ */
+static PMIC_AUDIO_AUDIO_OUTPUT_STATE audioOutput = {
+	(PMIC_AUDIO_OUTPUT_PORT) NULL,	/* outputPort       */
+	OUTPGA_GAIN_0DB,	/* outputPGAGain    */
+	OUTPGA_GAIN_0DB,	/* outputPGAGain    */
+	OUTPGA_GAIN_0DB,	/* outputPGAGain    */
+	BAL_GAIN_0DB,		/* balanceLeftGain  */
+	BAL_GAIN_0DB,		/* balanceRightGain */
+	MONOADD_GAIN_0DB,	/* monoAdderGain    */
+	(PMIC_AUDIO_OUTPUT_CONFIG) 0,	/* config           */
+	VCODEC_DIRECT_OUT
+};
+
+/*! The current headset status. */
+static HEADSET_STATUS headsetState = NO_HEADSET;
+
+/* Removed PTT variable */
+/*! Define a 1 ms wait interval that is needed to ensure that certain
+ *  hardware operations are successfully completed.
+ */
+static const unsigned long delay_1ms = (HZ / 1000);
+
+/*!
+ * @brief This spinlock is used to provide mutual exclusion.
+ *
+ * Create a spinlock that can be used to provide mutually exclusive
+ * read/write access to the globally accessible data structures
+ * that were defined above. Mutually exclusive access is required to
+ * ensure that the audio data structures are consistent at all times
+ * when possibly accessed by multiple threads of execution (for example,
+ * while simultaneously handling a user request and an interrupt event).
+ *
+ * We need to use a spinlock whenever we do need to provide mutual
+ * exclusion while possibly executing in a hardware interrupt context.
+ * Spinlocks should be held for the minimum time that is necessary
+ * because hardware interrupts are disabled while a spinlock is held.
+ *
+ */
+
+static spinlock_t lock = SPIN_LOCK_UNLOCKED;
+/*!
+ * @brief This mutex is used to provide mutual exclusion.
+ *
+ * Create a mutex that can be used to provide mutually exclusive
+ * read/write access to the globally accessible data structures
+ * that were defined above. Mutually exclusive access is required to
+ * ensure that the audio data structures are consistent at all times
+ * when possibly accessed by multiple threads of execution.
+ *
+ * Note that we use a mutex instead of the spinlock whenever disabling
+ * interrupts while in the critical section is not required. This helps
+ * to minimize kernel interrupt handling latency.
+ */
+static DECLARE_MUTEX(mutex);
+
+/*!
+ * @brief Global variable to track currently active interrupt events.
+ *
+ * This global variable is used to keep track of all of the currently
+ * active interrupt events for the audio driver. Note that access to this
+ * variable may occur while within an interrupt context and, therefore,
+ * must be guarded by using a spinlock.
+ */
+/* static PMIC_CORE_EVENT eventID = 0; */
+
+/* Prototypes for all static audio driver functions. */
+/*
+static PMIC_STATUS pmic_audio_mic_boost_enable(void);
+static PMIC_STATUS pmic_audio_mic_boost_disable(void);*/
+static PMIC_STATUS pmic_audio_close_handle(const PMIC_AUDIO_HANDLE handle);
+static PMIC_STATUS pmic_audio_reset_device(const PMIC_AUDIO_HANDLE handle);
+
+static PMIC_STATUS pmic_audio_deregister(void *callback,
+					 PMIC_AUDIO_EVENTS * const eventMask);
+
+/*************************************************************************
+ * Audio device access APIs.
+ *************************************************************************
+ */
+
+/*!
+ * @name General Setup and Configuration APIs
+ * Functions for general setup and configuration of the PMIC Audio
+ * hardware.
+ */
+/*@{*/
+
+PMIC_STATUS pmic_audio_set_autodetect(int val)
+{
+	PMIC_STATUS status;
+	unsigned int reg_mask = 0, reg_write = 0;
+	reg_mask = SET_BITS(regAUDIO_RX_0, VAUDIOON, 1);
+	status = pmic_write_reg(REG_AUDIO_RX_0, reg_mask, reg_mask);
+	if (status != PMIC_SUCCESS)
+		return status;
+	reg_mask = 0;
+	if (val == 1) {
+		reg_write = SET_BITS(regAUDIO_RX_0, HSDETEN, 1) |
+		    SET_BITS(regAUDIO_RX_0, HSDETAUTOB, 1);
+	} else {
+		reg_write = 0;
+	}
+	reg_mask =
+	    SET_BITS(regAUDIO_RX_0, HSDETEN, 1) | SET_BITS(regAUDIO_RX_0,
+							   HSDETAUTOB, 1);
+	status = pmic_write_reg(REG_AUDIO_RX_0, reg_write, reg_mask);
+
+	return status;
+}
+
+/*!
+ * @brief Request exclusive access to the PMIC Audio hardware.
+ *
+ * Attempt to open and gain exclusive access to a key PMIC audio hardware
+ * component (e.g., the Stereo DAC or the Voice CODEC). Depending upon the
+ * type of audio operation that is desired and the nature of the audio data
+ * stream, the Stereo DAC and/or the Voice CODEC will be a required hardware
+ * component and needs to be acquired by calling this function.
+ *
+ * If the open request is successful, then a numeric handle is returned
+ * and this handle must be used in all subsequent function calls to complete
+ * the configuration of either the Stereo DAC or the Voice CODEC and along
+ * with any other associated audio hardware components that will be needed.
+ *
+ * The same handle must also be used in the close call when use of the PMIC
+ * audio hardware is no longer required.
+ *
+ * The open request will fail if the requested audio hardware component has
+ * already been acquired by a previous open call but not yet closed.
+ *
+ * @param  handle          Device handle to be used for subsequent PMIC
+ *                              audio API calls.
+ * @param  device          The required PMIC audio hardware component.
+ *
+ * @retval      PMIC_SUCCESS         If the open request was successful
+ * @retval      PMIC_PARAMETER_ERROR If the handle argument is NULL.
+ * @retval      PMIC_ERROR           If the audio hardware component is
+ *                                   unavailable.
+ */
+PMIC_STATUS pmic_audio_open(PMIC_AUDIO_HANDLE * const handle,
+			    const PMIC_AUDIO_SOURCE device)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	if (handle == (PMIC_AUDIO_HANDLE *) NULL) {
+		/* Do not dereference a NULL pointer. */
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	/* We only need to acquire a mutex here because the interrupt handler
+	 * never modifies the device handle or device handle state. Therefore,
+	 * we don't need to worry about conflicts with the interrupt handler
+	 * or the need to execute in an interrupt context.
+	 *
+	 * But we do need a critical section here to avoid problems in case
+	 * multiple calls to pmic_audio_open() are made since we can only allow
+	 * one of them to succeed.
+	 */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	/* Check the current device handle state and acquire the handle if
+	 * it is available.
+	 */
+
+	if ((device == STEREO_DAC) && (stDAC.handleState == HANDLE_FREE)) {
+		stDAC.handle = (PMIC_AUDIO_HANDLE) (&stDAC);
+		stDAC.handleState = HANDLE_IN_USE;
+		*handle = stDAC.handle;
+		rc = PMIC_SUCCESS;
+	} else if ((device == VOICE_CODEC)
+		   && (vCodec.handleState == HANDLE_FREE)) {
+		vCodec.handle = (PMIC_AUDIO_HANDLE) (&vCodec);
+		vCodec.handleState = HANDLE_IN_USE;
+		*handle = vCodec.handle;
+		rc = PMIC_SUCCESS;
+	} else if ((device == EXTERNAL_STEREO_IN) &&
+		   (extStereoIn.handleState == HANDLE_FREE)) {
+		extStereoIn.handle = (PMIC_AUDIO_HANDLE) (&extStereoIn);
+		extStereoIn.handleState = HANDLE_IN_USE;
+		*handle = extStereoIn.handle;
+		rc = PMIC_SUCCESS;
+	} else {
+		*handle = AUDIO_HANDLE_NULL;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Terminate further access to the PMIC audio hardware.
+ *
+ * Terminate further access to the PMIC audio hardware that was previously
+ * acquired by calling pmic_audio_open(). This now allows another thread to
+ * successfully call pmic_audio_open() to gain access.
+ *
+ * Note that we will shutdown/reset the Voice CODEC or Stereo DAC as well as
+ * any associated audio input/output components that are no longer required.
+ *
+ * @param   handle          Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the close request was successful.
+ * @retval      PMIC_PARAMETER_ERROR If the handle is invalid.
+ */
+PMIC_STATUS pmic_audio_close(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* We need a critical section here to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	/* We can now call pmic_audio_close_handle() to actually do the work. */
+	rc = pmic_audio_close_handle(handle);
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Configure the data bus protocol to be used.
+ *
+ * Provide the parameters needed to properly configure the audio data bus
+ * protocol so that data can be read/written to either the Stereo DAC or
+ * the Voice CODEC.
+ *
+ * @param   handle          Device handle from pmic_audio_open() call.
+ * @param   busID           Select data bus to be used.
+ * @param   protocol        Select the data bus protocol.
+ * @param   masterSlave     Select the data bus timing mode.
+ * @param   numSlots        Define the number of timeslots (only if in
+ *                              master mode).
+ *
+ * @retval      PMIC_SUCCESS         If the protocol was successful configured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or the protocol parameters
+ *                                   are invalid.
+ */
+PMIC_STATUS pmic_audio_set_protocol(const PMIC_AUDIO_HANDLE handle,
+				    const PMIC_AUDIO_DATA_BUS busID,
+				    const PMIC_AUDIO_BUS_PROTOCOL protocol,
+				    const PMIC_AUDIO_BUS_MODE masterSlave,
+				    const PMIC_AUDIO_NUMSLOTS numSlots)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	const unsigned int ST_DAC_MASK = SET_BITS(regST_DAC, STDCSSISEL, 1) |
+	    SET_BITS(regST_DAC, STDCFS, 3) | SET_BITS(regST_DAC, STDCSM, 1);
+
+	unsigned int reg_mask;
+	/*unsigned int VCODEC_MASK = SET_BITS(regAUD_CODEC, CDCSSISEL, 1) |
+	   SET_BITS(regAUD_CODEC, CDCFS, 3) | SET_BITS(regAUD_CODEC, CDCSM, 1); */
+
+	unsigned int SSI_NW_MASK = SET_BITS(regSSI_NETWORK, STDCSLOTS, 1);
+	unsigned int reg_value = 0;
+	unsigned int ssi_nw_value = 0;
+
+	/* Enter a critical section so that we can ensure only one
+	 * state change request is completed at a time.
+	 */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if (handle == (PMIC_AUDIO_HANDLE) NULL) {
+		rc = PMIC_PARAMETER_ERROR;
+	} else {
+		if ((handle == vCodec.handle) &&
+		    (vCodec.handleState == HANDLE_IN_USE)) {
+			if ((stDAC.handleState == HANDLE_IN_USE) &&
+			    (stDAC.busID == busID) && (stDAC.protocol_set)) {
+				pr_debug("The requested bus already in USE\n");
+				rc = PMIC_PARAMETER_ERROR;
+			} else if ((masterSlave == BUS_MASTER_MODE)
+				   && (numSlots != USE_4_TIMESLOTS)) {
+				pr_debug
+				    ("mc13783 supports only 4 slots in Master mode\n");
+				rc = PMIC_NOT_SUPPORTED;
+			} else if ((masterSlave == BUS_SLAVE_MODE)
+				   && (numSlots != USE_4_TIMESLOTS)) {
+				pr_debug
+				    ("Driver currently supports only 4 slots in Slave mode\n");
+				rc = PMIC_NOT_SUPPORTED;
+			} else if (!((protocol == NETWORK_MODE) ||
+				     (protocol == I2S_MODE))) {
+				pr_debug
+				    ("mc13783 Voice codec works only in Network and I2S modes\n");
+				rc = PMIC_NOT_SUPPORTED;
+			} else {
+				pr_debug
+				    ("Proceeding to configure Voice Codec\n");
+				if (busID == AUDIO_DATA_BUS_1) {
+					reg_value =
+					    SET_BITS(regAUD_CODEC, CDCSSISEL,
+						     0);
+				} else {
+					reg_value =
+					    SET_BITS(regAUD_CODEC, CDCSSISEL,
+						     1);
+				}
+				reg_mask = SET_BITS(regAUD_CODEC, CDCSSISEL, 1);
+				if (PMIC_SUCCESS !=
+				    pmic_write_reg(REG_AUDIO_CODEC,
+						   reg_value, reg_mask))
+					return PMIC_ERROR;
+
+				if (masterSlave == BUS_MASTER_MODE) {
+					reg_value =
+					    SET_BITS(regAUD_CODEC, CDCSM, 0);
+				} else {
+					reg_value =
+					    SET_BITS(regAUD_CODEC, CDCSM, 1);
+				}
+				reg_mask = SET_BITS(regAUD_CODEC, CDCSM, 1);
+				if (PMIC_SUCCESS !=
+				    pmic_write_reg(REG_AUDIO_CODEC,
+						   reg_value, reg_mask))
+					return PMIC_ERROR;
+
+				if (protocol == NETWORK_MODE) {
+					reg_value =
+					    SET_BITS(regAUD_CODEC, CDCFS, 1);
+				} else {	/* protocol == I2S, other options have been already eliminated */
+					reg_value =
+					    SET_BITS(regAUD_CODEC, CDCFS, 2);
+				}
+				reg_mask = SET_BITS(regAUD_CODEC, CDCFS, 3);
+				if (PMIC_SUCCESS !=
+				    pmic_write_reg(REG_AUDIO_CODEC,
+						   reg_value, reg_mask))
+					return PMIC_ERROR;
+
+				ssi_nw_value =
+				    SET_BITS(regSSI_NETWORK, CDCFSDLY, 1);
+				/*if (pmic_write_reg
+				   (REG_AUDIO_CODEC, reg_value,
+				   VCODEC_MASK) != PMIC_SUCCESS) {
+				   rc = PMIC_ERROR;
+				   } else { */
+				vCodec.busID = busID;
+				vCodec.protocol = protocol;
+				vCodec.masterSlave = masterSlave;
+				vCodec.numSlots = numSlots;
+				vCodec.protocol_set = true;
+				//pmic_write_reg(REG_AUDIO_SSI_NETWORK, ssi_nw_value, ssi_nw_value);
+
+				pr_debug
+				    ("mc13783 Voice codec successfully configured\n");
+				rc = PMIC_SUCCESS;
+				//}
+
+			}
+
+		} else if ((handle == stDAC.handle) &&
+			   (stDAC.handleState == HANDLE_IN_USE)) {
+			if ((vCodec.handleState == HANDLE_IN_USE) &&
+			    (vCodec.busID == busID) && (vCodec.protocol_set)) {
+				pr_debug("The requested bus already in USE\n");
+				rc = PMIC_PARAMETER_ERROR;
+			} else if (((protocol == NORMAL_MSB_JUSTIFIED_MODE) ||
+				    (protocol == I2S_MODE))
+				   && (numSlots != USE_2_TIMESLOTS)) {
+				pr_debug
+				    ("STDAC uses only 2 slots in Normal and I2S modes\n");
+				rc = PMIC_PARAMETER_ERROR;
+			} else if ((protocol == NETWORK_MODE) &&
+				   !((numSlots == USE_2_TIMESLOTS) ||
+				     (numSlots == USE_4_TIMESLOTS) ||
+				     (numSlots == USE_8_TIMESLOTS))) {
+				pr_debug
+				    ("STDAC uses only 2,4 or 8 slots in Network mode\n");
+				rc = PMIC_PARAMETER_ERROR;
+			} else if (protocol == SPD_IF_MODE) {
+				pr_debug
+				    ("STDAC driver currently does not support SPD IF mode\n");
+				rc = PMIC_NOT_SUPPORTED;
+			} else {
+				pr_debug
+				    ("Proceeding to configure Stereo DAC\n");
+				if (busID == AUDIO_DATA_BUS_1) {
+					reg_value =
+					    SET_BITS(regST_DAC, STDCSSISEL, 0);
+				} else {
+					reg_value =
+					    SET_BITS(regST_DAC, STDCSSISEL, 1);
+				}
+				if (masterSlave == BUS_MASTER_MODE) {
+					reg_value |=
+					    SET_BITS(regST_DAC, STDCSM, 0);
+				} else {
+					reg_value |=
+					    SET_BITS(regST_DAC, STDCSM, 1);
+				}
+				if (protocol == NETWORK_MODE) {
+					reg_value |=
+					    SET_BITS(regST_DAC, STDCFS, 1);
+				} else if (protocol ==
+					   NORMAL_MSB_JUSTIFIED_MODE) {
+					reg_value |=
+					    SET_BITS(regST_DAC, STDCFS, 0);
+				} else {	/* I2S mode as the other option has already been eliminated */
+					reg_value |=
+					    SET_BITS(regST_DAC, STDCFS, 2);
+				}
+
+				if (pmic_write_reg
+				    (REG_AUDIO_STEREO_DAC,
+				     reg_value, ST_DAC_MASK) != PMIC_SUCCESS) {
+					rc = PMIC_ERROR;
+				} else {
+					if (numSlots == USE_2_TIMESLOTS) {
+						reg_value =
+						    SET_BITS(regSSI_NETWORK,
+							     STDCSLOTS, 3);
+					} else if (numSlots == USE_4_TIMESLOTS) {
+						reg_value =
+						    SET_BITS(regSSI_NETWORK,
+							     STDCSLOTS, 2);
+					} else {	/* Use 8 timeslots - L , R and 6 other */
+						reg_value =
+						    SET_BITS(regSSI_NETWORK,
+							     STDCSLOTS, 1);
+					}
+					if (pmic_write_reg
+					    (REG_AUDIO_SSI_NETWORK,
+					     reg_value,
+					     SSI_NW_MASK) != PMIC_SUCCESS) {
+						rc = PMIC_ERROR;
+					} else {
+						stDAC.busID = busID;
+						stDAC.protocol = protocol;
+						stDAC.protocol_set = true;
+						stDAC.masterSlave = masterSlave;
+						stDAC.numSlots = numSlots;
+						pr_debug
+						    ("mc13783 Stereo DAC successfully configured\n");
+						rc = PMIC_SUCCESS;
+					}
+				}
+
+			}
+		} else {
+			rc = PMIC_PARAMETER_ERROR;
+			/* Handle  can only be Voice Codec or Stereo DAC */
+			pr_debug("Handles only STDAC and VCODEC\n");
+		}
+
+	}
+	/* Exit critical section. */
+	up(&mutex);
+	return rc;
+}
+
+/*!
+ * @brief Retrieve the current data bus protocol configuration.
+ *
+ * Retrieve the parameters that define the current audio data bus protocol.
+ *
+ * @param  handle          Device handle from pmic_audio_open() call.
+ * @param  busID           The data bus being used.
+ * @param  protocol        The data bus protocol being used.
+ * @param  masterSlave     The data bus timing mode being used.
+ * @param  numSlots        The number of timeslots being used (if in
+ *                              master mode).
+ *
+ * @retval      PMIC_SUCCESS         If the protocol was successful retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle is invalid.
+ */
+PMIC_STATUS pmic_audio_get_protocol(const PMIC_AUDIO_HANDLE handle,
+				    PMIC_AUDIO_DATA_BUS * const busID,
+				    PMIC_AUDIO_BUS_PROTOCOL * const protocol,
+				    PMIC_AUDIO_BUS_MODE * const masterSlave,
+				    PMIC_AUDIO_NUMSLOTS * const numSlots)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	if ((busID != (PMIC_AUDIO_DATA_BUS *) NULL) &&
+	    (protocol != (PMIC_AUDIO_BUS_PROTOCOL *) NULL) &&
+	    (masterSlave != (PMIC_AUDIO_BUS_MODE *) NULL) &&
+	    (numSlots != (PMIC_AUDIO_NUMSLOTS *) NULL)) {
+		/* Enter a critical section so that we return a consistent state. */
+		if (down_interruptible(&mutex))
+			return PMIC_SYSTEM_ERROR_EINTR;
+
+		if ((handle == stDAC.handle) &&
+		    (stDAC.handleState == HANDLE_IN_USE)) {
+			*busID = stDAC.busID;
+			*protocol = stDAC.protocol;
+			*masterSlave = stDAC.masterSlave;
+			*numSlots = stDAC.numSlots;
+			rc = PMIC_SUCCESS;
+		} else if ((handle == vCodec.handle) &&
+			   (vCodec.handleState == HANDLE_IN_USE)) {
+			*busID = vCodec.busID;
+			*protocol = vCodec.protocol;
+			*masterSlave = vCodec.masterSlave;
+			*numSlots = vCodec.numSlots;
+			rc = PMIC_SUCCESS;
+		}
+
+		/* Exit critical section. */
+		up(&mutex);
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief Enable the Stereo DAC or the Voice CODEC.
+ *
+ * Explicitly enable the Stereo DAC or the Voice CODEC to begin audio
+ * playback or recording as required. This should only be done after
+ * successfully configuring all of the associated audio components (e.g.,
+ * microphones, amplifiers, etc.).
+ *
+ * Note that the timed delays used in this function are necessary to
+ * ensure reliable operation of the Voice CODEC and Stereo DAC. The
+ * Stereo DAC seems to be particularly sensitive and it has been observed
+ * to fail to generate the required master mode clock signals if it is
+ * not allowed enough time to initialize properly.
+ *
+ * @param      handle          Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the device was successful enabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle is invalid.
+ * @retval      PMIC_ERROR           If the device could not be enabled.
+ */
+PMIC_STATUS pmic_audio_enable(const PMIC_AUDIO_HANDLE handle)
+{
+	const unsigned int AUDIO_BIAS_ENABLE = SET_BITS(regAUDIO_RX_0,
+							VAUDIOON, 1);
+	const unsigned int STDAC_ENABLE = SET_BITS(regST_DAC, STDCEN, 1);
+	const unsigned int VCODEC_ENABLE = SET_BITS(regAUD_CODEC, CDCEN, 1);
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE)) {
+		pmic_write_reg(REG_AUDIO_RX_0, AUDIO_BIAS_ENABLE,
+			       AUDIO_BIAS_ENABLE);
+		reg_mask =
+		    SET_BITS(regAUDIO_RX_0, HSDETEN,
+			     1) | SET_BITS(regAUDIO_RX_0, HSDETAUTOB, 1);
+		reg_write =
+		    SET_BITS(regAUDIO_RX_0, HSDETEN,
+			     1) | SET_BITS(regAUDIO_RX_0, HSDETAUTOB, 1);
+		rc = pmic_write_reg(REG_AUDIO_RX_0, reg_write, reg_mask);
+		if (rc == PMIC_SUCCESS)
+			pr_debug("pmic_audio_enable\n");
+		/* We can enable the Stereo DAC. */
+		rc = pmic_write_reg(REG_AUDIO_STEREO_DAC,
+				    STDAC_ENABLE, STDAC_ENABLE);
+		/*pmic_read_reg(REG_AUDIO_STEREO_DAC, &reg_value); */
+		if (rc != PMIC_SUCCESS) {
+			pr_debug("Failed to enable the Stereo DAC\n");
+			rc = PMIC_ERROR;
+		}
+	} else if ((handle == vCodec.handle)
+		   && (vCodec.handleState == HANDLE_IN_USE)) {
+		/* Must first set the audio bias bit to power up the audio circuits. */
+		pmic_write_reg(REG_AUDIO_RX_0, AUDIO_BIAS_ENABLE,
+			       AUDIO_BIAS_ENABLE);
+		reg_mask = SET_BITS(regAUDIO_RX_0, HSDETEN, 1) |
+		    SET_BITS(regAUDIO_RX_0, HSDETAUTOB, 1);
+		reg_write = SET_BITS(regAUDIO_RX_0, HSDETEN, 1) |
+		    SET_BITS(regAUDIO_RX_0, HSDETAUTOB, 1);
+		rc = pmic_write_reg(REG_AUDIO_RX_0, reg_write, reg_mask);
+
+		/* Then we can enable the Voice CODEC. */
+		rc = pmic_write_reg(REG_AUDIO_CODEC, VCODEC_ENABLE,
+				    VCODEC_ENABLE);
+
+		/* pmic_read_reg(REG_AUDIO_CODEC, &reg_value); */
+		if (rc != PMIC_SUCCESS) {
+			pr_debug("Failed to enable the Voice codec\n");
+			rc = PMIC_ERROR;
+		}
+	}
+	/* Exit critical section. */
+	up(&mutex);
+	return rc;
+}
+
+/*!
+ * @brief Disable the Stereo DAC or the Voice CODEC.
+ *
+ * Explicitly disable the Stereo DAC or the Voice CODEC to end audio
+ * playback or recording as required.
+ *
+ * @param   	handle          Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the device was successful disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle is invalid.
+ * @retval      PMIC_ERROR           If the device could not be disabled.
+ */
+PMIC_STATUS pmic_audio_disable(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	const unsigned int STDAC_DISABLE = SET_BITS(regST_DAC, STDCEN, 1);
+	const unsigned int VCODEC_DISABLE = SET_BITS(regAUD_CODEC, CDCEN, 1);
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+	if ((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE)) {
+		rc = pmic_write_reg(REG_AUDIO_STEREO_DAC, 0, STDAC_DISABLE);
+	} else if ((handle == vCodec.handle)
+		   && (vCodec.handleState == HANDLE_IN_USE)) {
+		rc = pmic_write_reg(REG_AUDIO_CODEC, 0, VCODEC_DISABLE);
+	}
+	if (rc == PMIC_SUCCESS) {
+		pr_debug("Disabled successfully\n");
+	}
+	/* Exit critical section. */
+	up(&mutex);
+	return rc;
+}
+
+/*!
+ * @brief Reset the selected audio hardware control registers to their
+ *        power on state.
+ *
+ * This resets all of the audio hardware control registers currently
+ * associated with the device handle back to their power on states. For
+ * example, if the handle is associated with the Stereo DAC and a
+ * specific output port and output amplifiers, then this function will
+ * reset all of those components to their initial power on state.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the reset operation was successful.
+ * @retval      PMIC_PARAMETER_ERROR If the handle is invalid.
+ * @retval      PMIC_ERROR           If the reset was unsuccessful.
+ */
+PMIC_STATUS pmic_audio_reset(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	rc = pmic_audio_reset_device(handle);
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Reset all audio hardware control registers to their power on state.
+ *
+ * This resets all of the audio hardware control registers back to their
+ * power on states. Use this function with care since it also invalidates
+ * (i.e., automatically closes) all currently opened device handles.
+ *
+ * @retval      PMIC_SUCCESS         If the reset operation was successful.
+ * @retval      PMIC_ERROR           If the reset was unsuccessful.
+ */
+PMIC_STATUS pmic_audio_reset_all(void)
+{
+	PMIC_STATUS rc = PMIC_SUCCESS;
+	unsigned int audio_ssi_reset = 0;
+	unsigned int audio_rx1_reset = 0;
+	/* We need a critical section here to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	/* First close all opened device handles, also deregisters callbacks. */
+	pmic_audio_close_handle(stDAC.handle);
+	pmic_audio_close_handle(vCodec.handle);
+	pmic_audio_close_handle(extStereoIn.handle);
+
+	if (pmic_write_reg(REG_AUDIO_RX_1, RESET_AUDIO_RX_1,
+			   PMIC_ALL_BITS) != PMIC_SUCCESS) {
+		rc = PMIC_ERROR;
+	} else {
+		audio_rx1_reset = 1;
+	}
+	if (pmic_write_reg(REG_AUDIO_SSI_NETWORK, RESET_SSI_NETWORK,
+			   PMIC_ALL_BITS) != PMIC_SUCCESS) {
+		rc = PMIC_ERROR;
+	} else {
+		audio_ssi_reset = 1;
+	}
+	if (pmic_write_reg
+	    (REG_AUDIO_STEREO_DAC, RESET_ST_DAC,
+	     PMIC_ALL_BITS) != PMIC_SUCCESS) {
+		rc = PMIC_ERROR;
+	} else {
+		/* Also reset the driver state information to match. Note that we
+		 * keep the device handle and event callback settings unchanged
+		 * since these don't affect the actual hardware and we rely on
+		 * the user to explicitly close the handle or deregister callbacks
+		 */
+		if (audio_ssi_reset) {
+			/* better to check if SSI is also reset as some fields are represennted in SSI reg */
+			stDAC.busID = AUDIO_DATA_BUS_1;
+			stDAC.protocol = NORMAL_MSB_JUSTIFIED_MODE;
+			stDAC.masterSlave = BUS_MASTER_MODE;
+			stDAC.protocol_set = false;
+			stDAC.numSlots = USE_2_TIMESLOTS;
+			stDAC.clockIn = CLOCK_IN_CLIA;
+			stDAC.samplingRate = STDAC_RATE_44_1_KHZ;
+			stDAC.clockFreq = STDAC_CLI_13MHZ;
+			stDAC.invert = NO_INVERT;
+			stDAC.timeslot = USE_TS0_TS1;
+			stDAC.config = (PMIC_AUDIO_STDAC_CONFIG) 0;
+		}
+	}
+
+	if (pmic_write_reg(REG_AUDIO_CODEC, RESET_AUD_CODEC,
+			   PMIC_ALL_BITS) != PMIC_SUCCESS) {
+		rc = PMIC_ERROR;
+	} else {
+		/* Also reset the driver state information to match. Note that we
+		 * keep the device handle and event callback settings unchanged
+		 * since these don't affect the actual hardware and we rely on
+		 * the user to explicitly close the handle or deregister callbacks
+		 */
+		if (audio_ssi_reset) {
+			vCodec.busID = AUDIO_DATA_BUS_2;
+			vCodec.protocol = NETWORK_MODE;
+			vCodec.masterSlave = BUS_SLAVE_MODE;
+			vCodec.protocol_set = false;
+			vCodec.numSlots = USE_4_TIMESLOTS;
+			vCodec.clockIn = CLOCK_IN_CLIB;
+			vCodec.samplingRate = VCODEC_RATE_8_KHZ;
+			vCodec.clockFreq = VCODEC_CLI_13MHZ;
+			vCodec.invert = NO_INVERT;
+			vCodec.timeslot = USE_TS0;
+			vCodec.config =
+			    INPUT_HIGHPASS_FILTER | OUTPUT_HIGHPASS_FILTER;
+		}
+	}
+
+	if (pmic_write_reg(REG_AUDIO_RX_0, RESET_AUDIO_RX_0,
+			   PMIC_ALL_BITS) != PMIC_SUCCESS) {
+		rc = PMIC_ERROR;
+	} else {
+		/* Also reset the driver state information to match. */
+		audioOutput.outputPort = (PMIC_AUDIO_OUTPUT_PORT) NULL;
+		audioOutput.vCodecoutputPGAGain = OUTPGA_GAIN_0DB;
+		audioOutput.stDacoutputPGAGain = OUTPGA_GAIN_0DB;
+		audioOutput.extStereooutputPGAGain = OUTPGA_GAIN_0DB;
+		audioOutput.balanceLeftGain = BAL_GAIN_0DB;
+		audioOutput.balanceRightGain = BAL_GAIN_0DB;
+		audioOutput.monoAdderGain = MONOADD_GAIN_0DB;
+		audioOutput.config = (PMIC_AUDIO_OUTPUT_CONFIG) 0;
+		audioOutput.vCodecOut = VCODEC_DIRECT_OUT;
+	}
+
+	if (pmic_write_reg(REG_AUDIO_TX, RESET_AUDIO_TX,
+			   PMIC_ALL_BITS) != PMIC_SUCCESS) {
+		rc = PMIC_ERROR;
+	} else {
+		/* Also reset the driver state information to match. Note that we
+		 * reset the vCodec fields since all of the input/recording
+		 * devices are only connected to the Voice CODEC and are managed
+		 * as part of the Voice CODEC state.
+		 */
+		if (audio_rx1_reset) {
+			vCodec.leftChannelMic.mic = NO_MIC;
+			vCodec.leftChannelMic.micOnOff = MICROPHONE_OFF;
+			vCodec.leftChannelMic.ampMode = CURRENT_TO_VOLTAGE;
+			vCodec.leftChannelMic.gain = MIC_GAIN_0DB;
+			vCodec.rightChannelMic.mic = NO_MIC;
+			vCodec.rightChannelMic.micOnOff = MICROPHONE_OFF;
+			vCodec.rightChannelMic.ampMode = AMP_OFF;
+			vCodec.rightChannelMic.gain = MIC_GAIN_0DB;
+		}
+	}
+	/* Finally, also reset any global state variables. */
+	headsetState = NO_HEADSET;
+	/* Exit the critical section. */
+	up(&mutex);
+	return rc;
+}
+
+/*!
+ * @brief Set the Audio callback function.
+ *
+ * Register a callback function that will be used to signal PMIC audio
+ * events. For example, the OSS audio driver should register a callback
+ * function in order to be notified of headset connect/disconnect events.
+ *
+ * @param   func            A pointer to the callback function.
+ * @param   eventMask       A mask selecting events to be notified.
+ * @param   hs_state        To know the headset state.
+ *
+ *
+ *
+ * @retval      PMIC_SUCCESS         If the callback was successfully
+ *                                   registered.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or the eventMask is invalid.
+ */
+PMIC_STATUS pmic_audio_set_callback(void *func,
+				    const PMIC_AUDIO_EVENTS eventMask,
+				    PMIC_HS_STATE * hs_state)
+{
+	unsigned long flags;
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	pmic_event_callback_t eventNotify;
+
+	/* We need to start a critical section here to ensure a consistent state
+	 * in case simultaneous calls to pmic_audio_set_callback() are made. In
+	 * that case, we must serialize the calls to ensure that the "callback"
+	 * and "eventMask" state variables are always consistent.
+	 *
+	 * Note that we don't actually need to acquire the spinlock until later
+	 * when we are finally ready to update the "callback" and "eventMask"
+	 * state variables which are shared with the interrupt handler.
+	 */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	rc = PMIC_ERROR;
+	/* Register for PMIC events from the core protocol driver. */
+	if (eventMask & MICROPHONE_DETECTED) {
+		/* We need to register for the A1 amplifier interrupt. */
+		eventNotify.func = func;
+		eventNotify.param = (void *)(CORE_EVENT_MC2BI);
+		rc = pmic_event_subscribe(EVENT_MC2BI, eventNotify);
+
+		if (rc != PMIC_SUCCESS) {
+			pr_debug
+			    ("%s: pmic_event_subscribe() for EVENT_HSDETI "
+			     "failed\n", __FILE__);
+			goto End;
+		}
+	}
+
+	if (eventMask & HEADSET_DETECTED) {
+		/* We need to register for the A1 amplifier interrupt. */
+		eventNotify.func = func;
+		eventNotify.param = (void *)(CORE_EVENT_HSDETI);
+		rc = pmic_event_subscribe(EVENT_HSDETI, eventNotify);
+
+		if (rc != PMIC_SUCCESS) {
+			pr_debug
+			    ("%s: pmic_event_subscribe() for EVENT_HSDETI "
+			     "failed\n", __FILE__);
+			goto Cleanup_HDT;
+		}
+
+	}
+	if (eventMask & HEADSET_STEREO) {
+		/* We need to register for the A1 amplifier interrupt. */
+		eventNotify.func = func;
+		eventNotify.param = (void *)(CORE_EVENT_HSLI);
+		rc = pmic_event_subscribe(EVENT_HSLI, eventNotify);
+
+		if (rc != PMIC_SUCCESS) {
+			pr_debug
+			    ("%s: pmic_event_subscribe() for EVENT_HSLI "
+			     "failed\n", __FILE__);
+			goto Cleanup_HST;
+		}
+	}
+	if (eventMask & HEADSET_THERMAL_SHUTDOWN) {
+		/* We need to register for the A1 amplifier interrupt. */
+		eventNotify.func = func;
+		eventNotify.param = (void *)(CORE_EVENT_ALSPTHI);
+		rc = pmic_event_subscribe(EVENT_ALSPTHI, eventNotify);
+
+		if (rc != PMIC_SUCCESS) {
+			pr_debug
+			    ("%s: pmic_event_subscribe() for EVENT_ALSPTHI "
+			     "failed\n", __FILE__);
+			goto Cleanup_TSD;
+		}
+		pr_debug("Registered for EVENT_ALSPTHI\n");
+	}
+	if (eventMask & HEADSET_SHORT_CIRCUIT) {
+		/* We need to register for the A1 amplifier interrupt. */
+		eventNotify.func = func;
+		eventNotify.param = (void *)(CORE_EVENT_AHSSHORTI);
+		rc = pmic_event_subscribe(EVENT_AHSSHORTI, eventNotify);
+
+		if (rc != PMIC_SUCCESS) {
+			pr_debug
+			    ("%s: pmic_event_subscribe() for EVENT_AHSSHORTI "
+			     "failed\n", __FILE__);
+			goto Cleanup_HShort;
+		}
+		pr_debug("Registered for EVENT_AHSSHORTI\n");
+	}
+
+	/* We also need the spinlock here to avoid possible problems
+	 * with the interrupt handler  when we update the
+	 * "callback" and "eventMask" state variables.
+	 */
+	spin_lock_irqsave(&lock, flags);
+
+	/* Successfully registered for all events. */
+	event_state.callback = func;
+	event_state.eventMask = eventMask;
+
+	/* The spinlock is no longer needed now that we've finished
+	 * updating the "callback" and "eventMask" state variables.
+	 */
+	spin_unlock_irqrestore(&lock, flags);
+
+	goto End;
+
+	/* This section unregisters any already registered events if we should
+	 * encounter an error partway through the registration process. Note
+	 * that we don't check the return status here since it is already set
+	 * to PMIC_ERROR before we get here.
+	 */
+      Cleanup_HShort:
+
+	if (eventMask & HEADSET_SHORT_CIRCUIT) {
+		eventNotify.func = func;
+		eventNotify.param = (void *)(CORE_EVENT_AHSSHORTI);
+		pmic_event_unsubscribe(EVENT_AHSSHORTI, eventNotify);
+	}
+
+      Cleanup_TSD:
+
+	if (eventMask & HEADSET_THERMAL_SHUTDOWN) {
+		eventNotify.func = func;
+		eventNotify.param = (void *)(CORE_EVENT_ALSPTHI);
+		pmic_event_unsubscribe(EVENT_ALSPTHI, eventNotify);
+	}
+
+      Cleanup_HST:
+
+	if (eventMask & HEADSET_STEREO) {
+		eventNotify.func = func;
+		eventNotify.param = (void *)(CORE_EVENT_HSLI);
+		pmic_event_unsubscribe(EVENT_HSLI, eventNotify);
+	}
+
+      Cleanup_HDT:
+
+	if (eventMask & HEADSET_DETECTED) {
+		eventNotify.func = func;
+		eventNotify.param = (void *)(CORE_EVENT_HSDETI);
+		pmic_event_unsubscribe(EVENT_HSDETI, eventNotify);
+	}
+
+      End:
+	/* Exit the critical section. */
+	up(&mutex);
+	return rc;
+}
+
+/*!
+ * @brief Deregisters the existing audio callback function.
+ *
+ * Deregister the callback function that was previously registered by calling
+ * pmic_audio_set_callback().
+ *
+ *
+ * @retval      PMIC_SUCCESS         If the callback was successfully
+ *                                   deregistered.
+ * @retval      PMIC_PARAMETER_ERROR If the handle is invalid.
+ */
+PMIC_STATUS pmic_audio_clear_callback(void)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* We need a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if (event_state.callback != (PMIC_AUDIO_CALLBACK) NULL) {
+		rc = pmic_audio_deregister(&(event_state.callback),
+					   &(event_state.eventMask));
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+	return rc;
+}
+
+/*!
+ * @brief Get the current audio callback function settings.
+ *
+ * Get the current callback function and event mask.
+ *
+ * @param   func            The current callback function.
+ * @param   eventMask       The current event selection mask.
+ *
+ * @retval      PMIC_SUCCESS         If the callback information was
+ *                                   successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle is invalid.
+ */
+PMIC_STATUS pmic_audio_get_callback(PMIC_AUDIO_CALLBACK * const func,
+				    PMIC_AUDIO_EVENTS * const eventMask)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* We only need to acquire the mutex here because we will not be updating
+	 * anything that may affect the interrupt handler. We just need to ensure
+	 * that the callback fields are not changed while we are in the critical
+	 * section by calling either pmic_audio_set_callback() or
+	 * pmic_audio_clear_callback().
+	 */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((func != (PMIC_AUDIO_CALLBACK *) NULL) &&
+	    (eventMask != (PMIC_AUDIO_EVENTS *) NULL)) {
+
+		*func = event_state.callback;
+		*eventMask = event_state.eventMask;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+	return rc;
+}
+
+/*!
+ * @brief Enable the anti-pop circuitry to avoid extra noise when inserting
+ *        or removing a external device (e.g., a headset).
+ *
+ * Enable the use of the built-in anti-pop circuitry to prevent noise from
+ * being generated when an external audio device is inserted or removed
+ * from an audio plug. A slow ramp speed may be needed to avoid extra noise.
+ *
+ * @param       rampSpeed       The desired anti-pop circuitry ramp speed.
+ *
+ * @retval      PMIC_SUCCESS         If the anti-pop circuitry was successfully
+ *                                   enabled.
+ * @retval      PMIC_ERROR           If the anti-pop circuitry could not be
+ *                                   enabled.
+ */
+PMIC_STATUS pmic_audio_antipop_enable(const PMIC_AUDIO_ANTI_POP_RAMP_SPEED
+				      rampSpeed)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+	unsigned int reg_value = 0;
+	const unsigned int reg_mask = SET_BITS(regAUDIO_RX_0, BIASEN, 1) |
+	    SET_BITS(regAUDIO_RX_0, BIASSPEED, 1);
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	/*
+	 * Antipop is enabled by enabling the BIAS (BIASEN) and setting the
+	 * BIASSPEED .
+	 * BIASEN is just to make sure that BIAS is enabled
+	 */
+	reg_value = SET_BITS(regAUDIO_RX_0, BIASEN, 1)
+	    | SET_BITS(regAUDIO_RX_0, BIASSPEED, 0) | SET_BITS(regAUDIO_RX_0,
+							       HSLDETEN, 1);
+	rc = pmic_write_reg(REG_AUDIO_RX_0, reg_value, reg_mask);
+	return rc;
+}
+
+/*!
+ * @brief Disable the anti-pop circuitry.
+ *
+ * Disable the use of the built-in anti-pop circuitry to prevent noise from
+ * being generated when an external audio device is inserted or removed
+ * from an audio plug.
+ *
+ * @retval      PMIC_SUCCESS         If the anti-pop circuitry was successfully
+ *                                   disabled.
+ * @retval      PMIC_ERROR           If the anti-pop circuitry could not be
+ *                                   disabled.
+ */
+PMIC_STATUS pmic_audio_antipop_disable(void)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+	const unsigned int reg_mask = SET_BITS(regAUDIO_RX_0, BIASSPEED, 1) |
+	    SET_BITS(regAUDIO_RX_0, BIASEN, 1);
+	const unsigned int reg_write = SET_BITS(regAUDIO_RX_0, BIASSPEED, 1) |
+	    SET_BITS(regAUDIO_RX_0, BIASEN, 0);
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	/*
+	 * Antipop is disabled by setting BIASSPEED  = 0. BIASEN bit remains set
+	 * as only antipop needs to be disabled
+	 */
+	rc = pmic_write_reg(REG_AUDIO_RX_0, reg_write, reg_mask);
+
+	return rc;
+}
+
+/*!
+ * @brief Performs a reset of the Voice CODEC/Stereo DAC digital filter.
+ *
+ * The digital filter should be reset whenever the clock or sampling rate
+ * configuration has been changed.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the digital filter was successfully
+ *                                   reset.
+ * @retval      PMIC_ERROR           If the digital filter could not be reset.
+ */
+PMIC_STATUS pmic_audio_digital_filter_reset(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_mask = 0;
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if ((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE)) {
+		reg_mask = SET_BITS(regST_DAC, STDCRESET, 1);
+		if (pmic_write_reg(REG_AUDIO_STEREO_DAC, reg_mask,
+				   reg_mask) != PMIC_SUCCESS) {
+			rc = PMIC_ERROR;
+		} else {
+			pr_debug("STDAC filter reset\n");
+		}
+
+	} else if ((handle == vCodec.handle) &&
+		   (vCodec.handleState == HANDLE_IN_USE)) {
+		reg_mask = SET_BITS(regAUD_CODEC, CDCRESET, 1);
+		if (pmic_write_reg(REG_AUDIO_CODEC, reg_mask,
+				   reg_mask) != PMIC_SUCCESS) {
+			rc = PMIC_ERROR;
+		} else {
+			pr_debug("CODEC filter reset\n");
+		}
+	}
+	return rc;
+}
+
+/*!
+ * @brief Get the most recent PTT button voltage reading.
+ *
+ * This feature is not supported by mc13783
+ * @param       level                PTT button level.
+ *
+ * @retval      PMIC_SUCCESS         If the most recent PTT button voltage was
+ *                                   returned.
+ * @retval      PMIC_PARAMETER_ERROR If a NULL pointer argument was given.
+ */
+PMIC_STATUS pmic_audio_get_ptt_button_level(unsigned int *const level)
+{
+	PMIC_STATUS rc = PMIC_NOT_SUPPORTED;
+	return rc;
+}
+
+#ifdef DEBUG_AUDIO
+
+/*!
+ * @brief Provide a hexadecimal dump of all PMIC audio registers (DEBUG only)
+ *
+ * This function is intended strictly for debugging purposes only and will
+ * print the current values of the following PMIC registers:
+ *
+ * - AUD_CODEC
+ * - ST_DAC
+ * - AUDIO_RX_0
+ * - AUDIO_RX_1
+ * - AUDIO_TX
+ * - AUDIO_SSI_NW
+ *
+ * The register fields will not be decoded.
+ *
+ * Note that we don't dump any of the arbitration bits because we cannot
+ * access the true arbitration bit settings when reading the registers
+ * from the secondary SPI bus.
+ *
+ * Also note that we must not call this function with interrupts disabled,
+ * for example, while holding a spinlock, because calls to pmic_read_reg()
+ * eventually end up in the SPI driver which will want to perform a
+ * schedule() operation. If schedule() is called with interrupts disabled,
+ * then you will see messages like the following:
+ *
+ * BUG: scheduling while atomic: ...
+ *
+ */
+void pmic_audio_dump_registers(void)
+{
+	unsigned int reg_value = 0;
+
+	/* Dump the AUD_CODEC (Voice CODEC) register. */
+	if (pmic_read_reg(REG_AUDIO_CODEC, &reg_value, REG_FULLMASK)
+	    == PMIC_SUCCESS) {
+		pr_debug("Audio Codec = 0x%x\n", reg_value);
+	} else {
+		pr_debug("Failed to read audio codec\n");
+	}
+
+	/* Dump the ST DAC (Stereo DAC) register. */
+	if (pmic_read_reg
+	    (REG_AUDIO_STEREO_DAC, &reg_value, REG_FULLMASK) == PMIC_SUCCESS) {
+		pr_debug("Stereo DAC = 0x%x\n", reg_value);
+	} else {
+		pr_debug("Failed to read Stereo DAC\n");
+	}
+
+	/* Dump the SSI NW register. */
+	if (pmic_read_reg
+	    (REG_AUDIO_SSI_NETWORK, &reg_value, REG_FULLMASK) == PMIC_SUCCESS) {
+		pr_debug("SSI Network = 0x%x\n", reg_value);
+	} else {
+		pr_debug("Failed to read SSI network\n");
+	}
+
+	/* Dump the Audio RX 0 register. */
+	if (pmic_read_reg(REG_AUDIO_RX_0, &reg_value, REG_FULLMASK)
+	    == PMIC_SUCCESS) {
+		pr_debug("Audio RX 0 = 0x%x\n", reg_value);
+	} else {
+		pr_debug("Failed to read audio RX 0\n");
+	}
+
+	/* Dump the Audio RX 1 register. */
+	if (pmic_read_reg(REG_AUDIO_RX_1, &reg_value, REG_FULLMASK)
+	    == PMIC_SUCCESS) {
+		pr_debug("Audio RX 1 = 0x%x\n", reg_value);
+	} else {
+		pr_debug("Failed to read audio RX 1\n");
+	}
+	/* Dump the Audio TX register. */
+	if (pmic_read_reg(REG_AUDIO_TX, &reg_value, REG_FULLMASK) ==
+	    PMIC_SUCCESS) {
+		pr_debug("Audio Tx = 0x%x\n", reg_value);
+	} else {
+		pr_debug("Failed to read audio TX\n");
+	}
+
+}
+
+#endif				/* DEBUG_AUDIO */
+
+/*@}*/
+
+/*************************************************************************
+ * General Voice CODEC configuration.
+ *************************************************************************
+ */
+
+/*!
+ * @name General Voice CODEC Setup and Configuration APIs
+ * Functions for general setup and configuration of the PMIC Voice
+ * CODEC hardware.
+ */
+/*@{*/
+
+/*!
+ * @brief Set the Voice CODEC clock source and operating characteristics.
+ *
+ * Define the Voice CODEC clock source and operating characteristics. This
+ * must be done before the Voice CODEC is enabled.
+ *
+ *
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ * @param       clockIn         Select the clock signal source.
+ * @param       clockFreq       Select the clock signal frequency.
+ * @param       samplingRate    Select the audio data sampling rate.
+ * @param       invert          Enable inversion of the frame sync and/or
+ *                              bit clock inputs.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC clock settings were
+ *                                   successfully configured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or clock configuration was
+ *                                   invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC clock configuration
+ *                                   could not be set.
+ */
+PMIC_STATUS pmic_audio_vcodec_set_clock(const PMIC_AUDIO_HANDLE handle,
+					const PMIC_AUDIO_CLOCK_IN_SOURCE
+					clockIn,
+					const PMIC_AUDIO_VCODEC_CLOCK_IN_FREQ
+					clockFreq,
+					const PMIC_AUDIO_VCODEC_SAMPLING_RATE
+					samplingRate,
+					const PMIC_AUDIO_CLOCK_INVERT invert)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_value = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	/* Validate all of the calling parameters. */
+	if (handle == (PMIC_AUDIO_HANDLE) NULL) {
+		rc = PMIC_PARAMETER_ERROR;
+	} else if ((handle == vCodec.handle) &&
+		   (vCodec.handleState == HANDLE_IN_USE)) {
+		if ((clockIn != CLOCK_IN_CLIA) && (clockIn != CLOCK_IN_CLIB)) {
+			rc = PMIC_PARAMETER_ERROR;
+		} else if (!((clockFreq >= VCODEC_CLI_13MHZ)
+			     && (clockFreq <= VCODEC_CLI_33_6MHZ))) {
+			rc = PMIC_PARAMETER_ERROR;
+		} else if ((samplingRate != VCODEC_RATE_8_KHZ)
+			   && (samplingRate != VCODEC_RATE_16_KHZ)) {
+			rc = PMIC_PARAMETER_ERROR;
+		} else if (!((invert >= NO_INVERT)
+			     && (invert <= INVERT_FRAMESYNC))) {
+			rc = PMIC_PARAMETER_ERROR;
+		} else {
+			/*reg_mask = SET_BITS(regAUD_CODEC, CDCCLK, 7) |
+			   SET_BITS(regAUD_CODEC, CDCCLKSEL, 1) |
+			   SET_BITS(regAUD_CODEC, CDCFS8K16K, 1) |
+			   SET_BITS(regAUD_CODEC, CDCBCLINV, 1) |
+			   SET_BITS(regAUD_CODEC, CDCFSINV, 1); */
+			if (clockIn == CLOCK_IN_CLIA) {
+				reg_value =
+				    SET_BITS(regAUD_CODEC, CDCCLKSEL, 0);
+			} else {
+				reg_value =
+				    SET_BITS(regAUD_CODEC, CDCCLKSEL, 1);
+			}
+			reg_mask = SET_BITS(regAUD_CODEC, CDCCLKSEL, 1);
+			if (PMIC_SUCCESS !=
+			    pmic_write_reg(REG_AUDIO_CODEC,
+					   reg_value, reg_mask))
+				return PMIC_ERROR;
+
+			reg_value = 0;
+			if (clockFreq == VCODEC_CLI_13MHZ) {
+				reg_value |= SET_BITS(regAUD_CODEC, CDCCLK, 0);
+			} else if (clockFreq == VCODEC_CLI_15_36MHZ) {
+				reg_value |= SET_BITS(regAUD_CODEC, CDCCLK, 1);
+			} else if (clockFreq == VCODEC_CLI_16_8MHZ) {
+				reg_value |= SET_BITS(regAUD_CODEC, CDCCLK, 2);
+			} else if (clockFreq == VCODEC_CLI_26MHZ) {
+				reg_value |= SET_BITS(regAUD_CODEC, CDCCLK, 4);
+			} else {
+				reg_value |= SET_BITS(regAUD_CODEC, CDCCLK, 7);
+			}
+			reg_mask = SET_BITS(regAUD_CODEC, CDCCLK, 7);
+			if (PMIC_SUCCESS !=
+			    pmic_write_reg(REG_AUDIO_CODEC,
+					   reg_value, reg_mask))
+				return PMIC_ERROR;
+
+			reg_value = 0;
+			reg_mask = 0;
+
+			if (samplingRate == VCODEC_RATE_8_KHZ) {
+				reg_value |=
+				    SET_BITS(regAUD_CODEC, CDCFS8K16K, 0);
+			} else {
+				reg_value |=
+				    SET_BITS(regAUD_CODEC, CDCFS8K16K, 1);
+			}
+			reg_mask = SET_BITS(regAUD_CODEC, CDCFS8K16K, 1);
+			if (PMIC_SUCCESS !=
+			    pmic_write_reg(REG_AUDIO_CODEC,
+					   reg_value, reg_mask))
+				return PMIC_ERROR;
+			reg_value = 0;
+			reg_mask =
+			    SET_BITS(regAUD_CODEC, CDCBCLINV,
+				     1) | SET_BITS(regAUD_CODEC, CDCFSINV, 1);
+
+			if (invert & INVERT_BITCLOCK) {
+				reg_value |=
+				    SET_BITS(regAUD_CODEC, CDCBCLINV, 1);
+			}
+			if (invert & INVERT_FRAMESYNC) {
+				reg_value |=
+				    SET_BITS(regAUD_CODEC, CDCFSINV, 1);
+			}
+			if (invert & NO_INVERT) {
+				reg_value |=
+				    SET_BITS(regAUD_CODEC, CDCBCLINV, 0);
+				reg_value |=
+				    SET_BITS(regAUD_CODEC, CDCFSINV, 0);
+			}
+			if (pmic_write_reg
+			    (REG_AUDIO_CODEC, reg_value,
+			     reg_mask) != PMIC_SUCCESS) {
+				rc = PMIC_ERROR;
+			} else {
+				pr_debug("CODEC clock set\n");
+				vCodec.clockIn = clockIn;
+				vCodec.clockFreq = clockFreq;
+				vCodec.samplingRate = samplingRate;
+				vCodec.invert = invert;
+			}
+
+		}
+
+	} else {
+		rc = PMIC_PARAMETER_ERROR;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the Voice CODEC clock source and operating characteristics.
+ *
+ * Get the current Voice CODEC clock source and operating characteristics.
+ *
+ * @param  	handle          Device handle from pmic_audio_open() call.
+ * @param  	clockIn         The clock signal source.
+ * @param  	clockFreq       The clock signal frequency.
+ * @param  	samplingRate    The audio data sampling rate.
+ * @param       invert          Inversion of the frame sync and/or
+ *                              bit clock inputs is enabled/disabled.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC clock settings were
+ *                                   successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC clock configuration
+ *                                   could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_vcodec_get_clock(const PMIC_AUDIO_HANDLE handle,
+					PMIC_AUDIO_CLOCK_IN_SOURCE *
+					const clockIn,
+					PMIC_AUDIO_VCODEC_CLOCK_IN_FREQ *
+					const clockFreq,
+					PMIC_AUDIO_VCODEC_SAMPLING_RATE *
+					const samplingRate,
+					PMIC_AUDIO_CLOCK_INVERT * const invert)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure that we return a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) &&
+	    (vCodec.handleState == HANDLE_IN_USE) &&
+	    (clockIn != (PMIC_AUDIO_CLOCK_IN_SOURCE *) NULL) &&
+	    (clockFreq != (PMIC_AUDIO_VCODEC_CLOCK_IN_FREQ *) NULL) &&
+	    (samplingRate != (PMIC_AUDIO_VCODEC_SAMPLING_RATE *) NULL) &&
+	    (invert != (PMIC_AUDIO_CLOCK_INVERT *) NULL)) {
+		*clockIn = vCodec.clockIn;
+		*clockFreq = vCodec.clockFreq;
+		*samplingRate = vCodec.samplingRate;
+		*invert = vCodec.invert;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Set the Voice CODEC primary audio channel timeslot.
+ *
+ * Set the Voice CODEC primary audio channel timeslot. This function must be
+ * used if the default timeslot for the primary audio channel is to be changed.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ * @param       timeslot        Select the primary audio channel timeslot.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC primary audio channel
+ *                                   timeslot was successfully configured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or audio channel timeslot
+ *                                   was invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC primary audio channel
+ *                                   timeslot could not be set.
+ */
+PMIC_STATUS pmic_audio_vcodec_set_rxtx_timeslot(const PMIC_AUDIO_HANDLE handle,
+						const PMIC_AUDIO_VCODEC_TIMESLOT
+						timeslot)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	const unsigned int reg_mask = SET_BITS(regSSI_NETWORK, CDCTXRXSLOT, 3);
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) &&
+	    (vCodec.handleState == HANDLE_IN_USE) &&
+	    ((timeslot == USE_TS0) || (timeslot == USE_TS1) ||
+	     (timeslot == USE_TS2) || (timeslot == USE_TS3))) {
+		reg_write = SET_BITS(regSSI_NETWORK, CDCTXRXSLOT, timeslot);
+
+		rc = pmic_write_reg(REG_AUDIO_SSI_NETWORK, reg_write, reg_mask);
+
+		if (rc == PMIC_SUCCESS) {
+			vCodec.timeslot = timeslot;
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the current Voice CODEC primary audio channel timeslot.
+ *
+ * Get the current Voice CODEC primary audio channel timeslot.
+ *
+ * @param	handle          Device handle from pmic_audio_open() call.
+ * @param       timeslot        The primary audio channel timeslot.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC primary audio channel
+ *                                   timeslot was successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC primary audio channel
+ *                                   timeslot could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_vcodec_get_rxtx_timeslot(const PMIC_AUDIO_HANDLE handle,
+						PMIC_AUDIO_VCODEC_TIMESLOT *
+						const timeslot)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) &&
+	    (vCodec.handleState == HANDLE_IN_USE) &&
+	    (timeslot != (PMIC_AUDIO_VCODEC_TIMESLOT *) NULL)) {
+		*timeslot = vCodec.timeslot;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Set the Voice CODEC secondary recording audio channel timeslot.
+ *
+ * Set the Voice CODEC secondary audio channel timeslot. This function must be
+ * used if the default timeslot for the secondary audio channel is to be
+ * changed. The secondary audio channel timeslot is used to transmit the audio
+ * data that was recorded by the Voice CODEC from the secondary audio input
+ * channel.
+ *
+ * @param   handle          Device handle from pmic_audio_open() call.
+ * @param   timeslot        Select the secondary audio channel timeslot.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC secondary audio channel
+ *                                   timeslot was successfully configured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or audio channel timeslot
+ *                                   was invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC secondary audio channel
+ *                                   timeslot could not be set.
+ */
+PMIC_STATUS pmic_audio_vcodec_set_secondary_txslot(const PMIC_AUDIO_HANDLE
+						   handle,
+						   const
+						   PMIC_AUDIO_VCODEC_TIMESLOT
+						   timeslot)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_mask = SET_BITS(regSSI_NETWORK, CDCTXSECSLOT, 3);
+	unsigned int reg_write = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		/* How to handle primary slot and secondary slot being the same */
+		if ((timeslot >= USE_TS0) && (timeslot <= USE_TS3)
+		    && (timeslot != vCodec.timeslot)) {
+			reg_write =
+			    SET_BITS(regSSI_NETWORK, CDCTXSECSLOT, timeslot);
+
+			rc = pmic_write_reg(REG_AUDIO_SSI_NETWORK,
+					    reg_write, reg_mask);
+
+			if (rc == PMIC_SUCCESS) {
+				vCodec.secondaryTXtimeslot = timeslot;
+			}
+		}
+
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the Voice CODEC secondary recording audio channel timeslot.
+ *
+ * Get the Voice CODEC secondary audio channel timeslot.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ * @param       timeslot        The secondary audio channel timeslot.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC secondary audio channel
+ *                                   timeslot was successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC secondary audio channel
+ *                                   timeslot could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_vcodec_get_secondary_txslot(const PMIC_AUDIO_HANDLE
+						   handle,
+						   PMIC_AUDIO_VCODEC_TIMESLOT *
+						   const timeslot)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) &&
+	    (vCodec.handleState == HANDLE_IN_USE) &&
+	    (timeslot != (PMIC_AUDIO_VCODEC_TIMESLOT *) NULL)) {
+		rc = PMIC_SUCCESS;
+		*timeslot = vCodec.secondaryTXtimeslot;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+	return rc;
+}
+
+/*!
+ * @brief Set/Enable the Voice CODEC options.
+ *
+ * Set or enable various Voice CODEC options. The available options include
+ * the use of dithering, highpass digital filters, and loopback modes.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ * @param       config          The Voice CODEC options to enable.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC options were
+ *                                   successfully configured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or Voice CODEC options
+ *                                   were invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC options could not be
+ *                                   successfully set/enabled.
+ */
+PMIC_STATUS pmic_audio_vcodec_set_config(const PMIC_AUDIO_HANDLE handle,
+					 const PMIC_AUDIO_VCODEC_CONFIG config)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		if (config & DITHERING) {
+			reg_write = SET_BITS(regAUD_CODEC, CDCDITH, 0);
+			reg_mask = SET_BITS(regAUD_CODEC, CDCDITH, 1);
+		}
+
+		if (config & INPUT_HIGHPASS_FILTER) {
+			reg_write |= SET_BITS(regAUD_CODEC, AUDIHPF, 1);
+			reg_mask |= SET_BITS(regAUD_CODEC, AUDIHPF, 1);
+		}
+
+		if (config & OUTPUT_HIGHPASS_FILTER) {
+			reg_write |= SET_BITS(regAUD_CODEC, AUDOHPF, 1);
+			reg_mask |= SET_BITS(regAUD_CODEC, AUDOHPF, 1);
+		}
+
+		if (config & DIGITAL_LOOPBACK) {
+			reg_write |= SET_BITS(regAUD_CODEC, CDCDLM, 1);
+			reg_mask |= SET_BITS(regAUD_CODEC, CDCDLM, 1);
+		}
+
+		if (config & ANALOG_LOOPBACK) {
+			reg_write |= SET_BITS(regAUD_CODEC, CDCALM, 1);
+			reg_mask |= SET_BITS(regAUD_CODEC, CDCALM, 1);
+		}
+
+		if (config & VCODEC_MASTER_CLOCK_OUTPUTS) {
+			reg_write |= SET_BITS(regAUD_CODEC, CDCCLKEN, 1) |
+			    SET_BITS(regAUD_CODEC, CDCTS, 0);
+			reg_mask |= SET_BITS(regAUD_CODEC, CDCCLKEN, 1) |
+			    SET_BITS(regAUD_CODEC, CDCTS, 1);
+
+		}
+
+		if (config & TRISTATE_TS) {
+			reg_write |= SET_BITS(regAUD_CODEC, CDCTS, 1);
+			reg_mask |= SET_BITS(regAUD_CODEC, CDCTS, 1);
+		}
+
+		if (reg_mask == 0) {
+			/* We should not reach this point without having to configure
+			 * anything so we flag it as an error.
+			 */
+			rc = PMIC_ERROR;
+		} else {
+			rc = pmic_write_reg(REG_AUDIO_CODEC,
+					    reg_write, reg_mask);
+		}
+
+		if (rc == PMIC_SUCCESS) {
+			vCodec.config |= config;
+		}
+	}
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Clear/Disable the Voice CODEC options.
+ *
+ * Clear or disable various Voice CODEC options.
+ *
+ * @param   handle          Device handle from pmic_audio_open() call.
+ * @param   config          The Voice CODEC options to be cleared/disabled.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC options were
+ *                                   successfully cleared/disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or the Voice CODEC options
+ *                                   were invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC options could not be
+ *                                   cleared/disabled.
+ */
+PMIC_STATUS pmic_audio_vcodec_clear_config(const PMIC_AUDIO_HANDLE handle,
+					   const PMIC_AUDIO_VCODEC_CONFIG
+					   config)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		if (config & DITHERING) {
+			reg_mask = SET_BITS(regAUD_CODEC, CDCDITH, 1);
+			reg_write = SET_BITS(regAUD_CODEC, CDCDITH, 1);
+		}
+
+		if (config & INPUT_HIGHPASS_FILTER) {
+			reg_mask |= SET_BITS(regAUD_CODEC, AUDIHPF, 1);
+		}
+
+		if (config & OUTPUT_HIGHPASS_FILTER) {
+			reg_mask |= SET_BITS(regAUD_CODEC, AUDOHPF, 1);
+		}
+
+		if (config & DIGITAL_LOOPBACK) {
+			reg_mask |= SET_BITS(regAUD_CODEC, CDCDLM, 1);
+		}
+
+		if (config & ANALOG_LOOPBACK) {
+			reg_mask |= SET_BITS(regAUD_CODEC, CDCALM, 1);
+		}
+
+		if (config & VCODEC_MASTER_CLOCK_OUTPUTS) {
+			reg_mask |= SET_BITS(regAUD_CODEC, CDCCLKEN, 1);
+		}
+
+		if (config & TRISTATE_TS) {
+			reg_mask |= SET_BITS(regAUD_CODEC, CDCTS, 1);
+		}
+
+		if (reg_mask == 0) {
+			/* We should not reach this point without having to configure
+			 * anything so we flag it as an error.
+			 */
+			rc = PMIC_ERROR;
+		} else {
+			rc = pmic_write_reg(REG_AUDIO_CODEC,
+					    reg_write, reg_mask);
+		}
+
+		if (rc == PMIC_SUCCESS) {
+			vCodec.config |= config;
+		}
+
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the current Voice CODEC options.
+ *
+ * Get the current Voice CODEC options.
+ *
+ * @param   	handle          Device handle from pmic_audio_open() call.
+ * @param	config          The current set of Voice CODEC options.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC options were
+ *                                   successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC options could not be
+ *                                   retrieved.
+ */
+PMIC_STATUS pmic_audio_vcodec_get_config(const PMIC_AUDIO_HANDLE handle,
+					 PMIC_AUDIO_VCODEC_CONFIG *
+					 const config)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) &&
+	    (vCodec.handleState == HANDLE_IN_USE) &&
+	    (config != (PMIC_AUDIO_VCODEC_CONFIG *) NULL)) {
+		*config = vCodec.config;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Enable the Voice CODEC bypass audio pathway.
+ *
+ * Enables the Voice CODEC bypass pathway for audio data. This allows direct
+ * output of the voltages on the TX data bus line to the output amplifiers
+ * (bypassing the digital-to-analog converters within the Voice CODEC).
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC bypass was successfully
+ *                                   enabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC bypass could not be
+ *                                   enabled.
+ */
+PMIC_STATUS pmic_audio_vcodec_enable_bypass(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	const unsigned int reg_write = SET_BITS(regAUD_CODEC, CDCBYP, 1);
+	const unsigned int reg_mask = SET_BITS(regAUD_CODEC, CDCBYP, 1);
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		rc = pmic_write_reg(REG_AUDIO_CODEC, reg_write, reg_mask);
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief Disable the Voice CODEC bypass audio pathway.
+ *
+ * Disables the Voice CODEC bypass pathway for audio data. This means that
+ * the TX data bus line will deliver digital data to the digital-to-analog
+ * converters within the Voice CODEC.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC bypass was successfully
+ *                                   disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC bypass could not be
+ *                                   disabled.
+ */
+PMIC_STATUS pmic_audio_vcodec_disable_bypass(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	const unsigned int reg_write = 0;
+	const unsigned int reg_mask = SET_BITS(regAUD_CODEC, CDCBYP, 1);
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		rc = pmic_write_reg(REG_AUDIO_CODEC, reg_write, reg_mask);
+	}
+
+	return rc;
+}
+
+/*@}*/
+
+/*************************************************************************
+ * General Stereo DAC configuration.
+ *************************************************************************
+ */
+
+/*!
+ * @name General Stereo DAC Setup and Configuration APIs
+ * Functions for general setup and configuration of the PMIC Stereo
+ * DAC hardware.
+ */
+/*@{*/
+
+/*!
+ * @brief Set the Stereo DAC clock source and operating characteristics.
+ *
+ * Define the Stereo DAC clock source and operating characteristics. This
+ * must be done before the Stereo DAC is enabled.
+ *
+ *
+ * @param   handle          Device handle from pmic_audio_open() call.
+ * @param   clockIn         Select the clock signal source.
+ * @param   clockFreq       Select the clock signal frequency.
+ * @param   samplingRate    Select the audio data sampling rate.
+ * @param   invert          Enable inversion of the frame sync and/or
+ *                              bit clock inputs.
+ *
+ * @retval      PMIC_SUCCESS         If the Stereo DAC clock settings were
+ *                                   successfully configured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or clock configuration was
+ *                                   invalid.
+ * @retval      PMIC_ERROR           If the Stereo DAC clock configuration
+ *                                   could not be set.
+ */
+PMIC_STATUS pmic_audio_stdac_set_clock(const PMIC_AUDIO_HANDLE handle,
+				       const PMIC_AUDIO_CLOCK_IN_SOURCE clockIn,
+				       const PMIC_AUDIO_STDAC_CLOCK_IN_FREQ
+				       clockFreq,
+				       const PMIC_AUDIO_STDAC_SAMPLING_RATE
+				       samplingRate,
+				       const PMIC_AUDIO_CLOCK_INVERT invert)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_value = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+	/* Validate all of the calling parameters. */
+	if (handle == (PMIC_AUDIO_HANDLE) NULL) {
+		rc = PMIC_PARAMETER_ERROR;
+	} else if ((handle == stDAC.handle) &&
+		   (stDAC.handleState == HANDLE_IN_USE)) {
+		if ((clockIn != CLOCK_IN_CLIA) && (clockIn != CLOCK_IN_CLIB)) {
+			rc = PMIC_PARAMETER_ERROR;
+		} else if ((stDAC.masterSlave == BUS_MASTER_MODE)
+			   && !((clockFreq >= STDAC_CLI_3_36864MHZ)
+				&& (clockFreq <= STDAC_CLI_33_6MHZ))) {
+			rc = PMIC_PARAMETER_ERROR;
+		} else if ((stDAC.masterSlave == BUS_SLAVE_MODE)
+			   && !((clockFreq >= STDAC_MCLK_PLL_DISABLED)
+				&& (clockFreq <= STDAC_BCLK_IN_PLL))) {
+			rc = PMIC_PARAMETER_ERROR;
+		} else if (!((samplingRate >= STDAC_RATE_8_KHZ)
+			     && (samplingRate <= STDAC_RATE_96_KHZ))) {
+			rc = PMIC_PARAMETER_ERROR;
+		}
+		/*
+		   else if(!((invert >= NO_INVERT) && (invert <= INVERT_FRAMESYNC)))
+		   {
+		   rc = PMIC_PARAMETER_ERROR;
+		   } */
+		else {
+			reg_mask = SET_BITS(regST_DAC, STDCCLK, 7) |
+			    SET_BITS(regST_DAC, STDCCLKSEL, 1) |
+			    SET_BITS(regST_DAC, SR, 15) |
+			    SET_BITS(regST_DAC, STDCBCLINV, 1) |
+			    SET_BITS(regST_DAC, STDCFSINV, 1);
+			if (clockIn == CLOCK_IN_CLIA) {
+				reg_value = SET_BITS(regST_DAC, STDCCLKSEL, 0);
+			} else {
+				reg_value = SET_BITS(regST_DAC, STDCCLKSEL, 1);
+			}
+			/* How to take care of sample rates in SLAVE mode */
+			if ((clockFreq == STDAC_CLI_3_36864MHZ)
+			    || ((clockFreq == STDAC_FSYNC_IN_PLL))) {
+				reg_value |= SET_BITS(regST_DAC, STDCCLK, 6);
+			} else if ((clockFreq == STDAC_CLI_12MHZ)
+				   || (clockFreq == STDAC_MCLK_PLL_DISABLED)) {
+				reg_value |= SET_BITS(regST_DAC, STDCCLK, 5);
+			} else if (clockFreq == STDAC_CLI_13MHZ) {
+				reg_value |= SET_BITS(regST_DAC, STDCCLK, 0);
+			} else if (clockFreq == STDAC_CLI_15_36MHZ) {
+				reg_value |= SET_BITS(regST_DAC, STDCCLK, 1);
+			} else if (clockFreq == STDAC_CLI_16_8MHZ) {
+				reg_value |= SET_BITS(regST_DAC, STDCCLK, 2);
+			} else if (clockFreq == STDAC_CLI_26MHZ) {
+				reg_value |= SET_BITS(regST_DAC, STDCCLK, 4);
+			} else if ((clockFreq == STDAC_CLI_33_6MHZ)
+				   || (clockFreq == STDAC_BCLK_IN_PLL)) {
+				reg_value |= SET_BITS(regST_DAC, STDCCLK, 7);
+			}
+
+			reg_value |= SET_BITS(regST_DAC, SR, samplingRate);
+
+			if (invert & INVERT_BITCLOCK) {
+				reg_value |= SET_BITS(regST_DAC, STDCBCLINV, 1);
+			}
+			if (invert & INVERT_FRAMESYNC) {
+				reg_value |= SET_BITS(regST_DAC, STDCFSINV, 1);
+			}
+			if (invert & NO_INVERT) {
+				reg_value |= SET_BITS(regST_DAC, STDCBCLINV, 0);
+				reg_value |= SET_BITS(regST_DAC, STDCFSINV, 0);
+			}
+			if (pmic_write_reg
+			    (REG_AUDIO_STEREO_DAC, reg_value,
+			     reg_mask) != PMIC_SUCCESS) {
+				rc = PMIC_ERROR;
+			} else {
+				pr_debug("STDAC clock set\n");
+				rc = PMIC_SUCCESS;
+				stDAC.clockIn = clockIn;
+				stDAC.clockFreq = clockFreq;
+				stDAC.samplingRate = samplingRate;
+				stDAC.invert = invert;
+			}
+
+		}
+
+	} else {
+		rc = PMIC_PARAMETER_ERROR;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the Stereo DAC clock source and operating characteristics.
+ *
+ * Get the current Stereo DAC clock source and operating characteristics.
+ *
+ * @param  handle          Device handle from pmic_audio_open() call.
+ * @param  clockIn         The clock signal source.
+ * @param  clockFreq       The clock signal frequency.
+ * @param  samplingRate    The audio data sampling rate.
+ * @param  invert          Inversion of the frame sync and/or
+ *                         bit clock inputs is enabled/disabled.
+ *
+ * @retval      PMIC_SUCCESS         If the Stereo DAC clock settings were
+ *                                   successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle invalid.
+ * @retval      PMIC_ERROR           If the Stereo DAC clock configuration
+ *                                   could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_stdac_get_clock(const PMIC_AUDIO_HANDLE handle,
+				       PMIC_AUDIO_CLOCK_IN_SOURCE *
+				       const clockIn,
+				       PMIC_AUDIO_STDAC_SAMPLING_RATE *
+				       const samplingRate,
+				       PMIC_AUDIO_STDAC_CLOCK_IN_FREQ *
+				       const clockFreq,
+				       PMIC_AUDIO_CLOCK_INVERT * const invert)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == stDAC.handle) &&
+	    (stDAC.handleState == HANDLE_IN_USE) &&
+	    (clockIn != (PMIC_AUDIO_CLOCK_IN_SOURCE *) NULL) &&
+	    (samplingRate != (PMIC_AUDIO_STDAC_SAMPLING_RATE *) NULL) &&
+	    (clockFreq != (PMIC_AUDIO_STDAC_CLOCK_IN_FREQ *) NULL) &&
+	    (invert != (PMIC_AUDIO_CLOCK_INVERT *) NULL)) {
+		*clockIn = stDAC.clockIn;
+		*samplingRate = stDAC.samplingRate;
+		*clockFreq = stDAC.clockFreq;
+		*invert = stDAC.invert;
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Set the Stereo DAC primary audio channel timeslot.
+ *
+ * Set the Stereo DAC primary audio channel timeslot. This function must be
+ * used if the default timeslot for the primary audio channel is to be changed.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ * @param	timeslot        Select the primary audio channel timeslot.
+ *
+ * @retval      PMIC_SUCCESS         If the Stereo DAC primary audio channel
+ *                                   timeslot was successfully configured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or audio channel timeslot
+ *                                   was invalid.
+ * @retval      PMIC_ERROR           If the Stereo DAC primary audio channel
+ *                                   timeslot could not be set.
+ */
+PMIC_STATUS pmic_audio_stdac_set_rxtx_timeslot(const PMIC_AUDIO_HANDLE handle,
+					       const PMIC_AUDIO_STDAC_TIMESLOTS
+					       timeslot)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_mask = SET_BITS(regSSI_NETWORK, STDCRXSLOT, 3);
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE)) {
+		if ((timeslot == USE_TS0_TS1) || (timeslot == USE_TS2_TS3)
+		    || (timeslot == USE_TS4_TS5) || (timeslot == USE_TS6_TS7)) {
+			if (pmic_write_reg
+			    (REG_AUDIO_SSI_NETWORK, timeslot,
+			     reg_mask) != PMIC_SUCCESS) {
+				rc = PMIC_ERROR;
+			} else {
+				pr_debug("STDAC primary timeslot set\n");
+				stDAC.timeslot = timeslot;
+				rc = PMIC_SUCCESS;
+			}
+
+		} else {
+			rc = PMIC_PARAMETER_ERROR;
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the current Stereo DAC primary audio channel timeslot.
+ *
+ * Get the current Stereo DAC primary audio channel timeslot.
+ *
+ * @param   handle          Device handle from pmic_audio_open() call.
+ * @param   timeslot        The primary audio channel timeslot.
+ *
+ * @retval      PMIC_SUCCESS         If the Stereo DAC primary audio channel
+ *                                   timeslot was successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the Stereo DAC primary audio channel
+ *                                   timeslot could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_stdac_get_rxtx_timeslot(const PMIC_AUDIO_HANDLE handle,
+					       PMIC_AUDIO_STDAC_TIMESLOTS *
+					       const timeslot)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == stDAC.handle) &&
+	    (stDAC.handleState == HANDLE_IN_USE) &&
+	    (timeslot != (PMIC_AUDIO_STDAC_TIMESLOTS *) NULL)) {
+		*timeslot = stDAC.timeslot;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Set/Enable the Stereo DAC options.
+ *
+ * Set or enable various Stereo DAC options. The available options include
+ * resetting the digital filter and enabling the bus master clock outputs.
+ *
+ * @param   handle          Device handle from pmic_audio_open() call.
+ * @param   config          The Stereo DAC options to enable.
+ *
+ * @retval      PMIC_SUCCESS         If the Stereo DAC options were
+ *                                   successfully configured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or Stereo DAC options
+ *                                   were invalid.
+ * @retval      PMIC_ERROR           If the Stereo DAC options could not be
+ *                                   successfully set/enabled.
+ */
+PMIC_STATUS pmic_audio_stdac_set_config(const PMIC_AUDIO_HANDLE handle,
+					const PMIC_AUDIO_STDAC_CONFIG config)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE)) {
+		if (config & STDAC_MASTER_CLOCK_OUTPUTS) {
+			reg_write |= SET_BITS(regST_DAC, STDCCLKEN, 1);
+			reg_mask |= SET_BITS(regST_DAC, STDCCLKEN, 1);
+		}
+
+		rc = pmic_write_reg(REG_AUDIO_STEREO_DAC, reg_write, reg_mask);
+
+		if (rc == PMIC_SUCCESS) {
+			stDAC.config |= config;
+			pr_debug("STDAC config set\n");
+
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Clear/Disable the Stereo DAC options.
+ *
+ * Clear or disable various Stereo DAC options.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ * @param       config          The Stereo DAC options to be cleared/disabled.
+ *
+ * @retval      PMIC_SUCCESS         If the Stereo DAC options were
+ *                                   successfully cleared/disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or the Stereo DAC options
+ *                                   were invalid.
+ * @retval      PMIC_ERROR           If the Stereo DAC options could not be
+ *                                   cleared/disabled.
+ */
+PMIC_STATUS pmic_audio_stdac_clear_config(const PMIC_AUDIO_HANDLE handle,
+					  const PMIC_AUDIO_STDAC_CONFIG config)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE)) {
+
+		if (config & STDAC_MASTER_CLOCK_OUTPUTS) {
+			reg_mask |= SET_BITS(regST_DAC, STDCCLKEN, 1);
+		}
+
+		if (reg_mask != 0) {
+			rc = pmic_write_reg(REG_AUDIO_STEREO_DAC,
+					    reg_write, reg_mask);
+
+			if (rc == PMIC_SUCCESS) {
+				stDAC.config &= ~config;
+			}
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the current Stereo DAC options.
+ *
+ * Get the current Stereo DAC options.
+ *
+ * @param   	handle          Device handle from pmic_audio_open() call.
+ * @param   	config          The current set of Stereo DAC options.
+ *
+ * @retval      PMIC_SUCCESS         If the Stereo DAC options were
+ *                                   successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the Stereo DAC options could not be
+ *                                   retrieved.
+ */
+PMIC_STATUS pmic_audio_stdac_get_config(const PMIC_AUDIO_HANDLE handle,
+					PMIC_AUDIO_STDAC_CONFIG * const config)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == stDAC.handle) &&
+	    (stDAC.handleState == HANDLE_IN_USE) &&
+	    (config != (PMIC_AUDIO_STDAC_CONFIG *) NULL)) {
+		*config = stDAC.config;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*@}*/
+
+/*************************************************************************
+ * Audio input section configuration.
+ *************************************************************************
+ */
+
+/*!
+ * @name Audio Input Setup and Configuration APIs
+ * Functions for general setup and configuration of the PMIC audio
+ * input hardware.
+ */
+/*@{*/
+
+/*!
+ * @brief Set/Enable the audio input section options.
+ *
+ * Set or enable various audio input section options. The only available
+ * option right now is to enable the automatic disabling of the microphone
+ * input amplifiers when a microphone/headset is inserted or removed.
+ * NOT SUPPORTED BY MC13783
+ *
+ * @param  	handle          Device handle from pmic_audio_open() call.
+ * @param   	config          The audio input section options to enable.
+ *
+ * @retval      PMIC_SUCCESS         If the audio input section options were
+ *                                   successfully configured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or audio input section
+ *                                   options were invalid.
+ * @retval      PMIC_ERROR           If the audio input section options could
+ *                                   not be successfully set/enabled.
+ */
+PMIC_STATUS pmic_audio_input_set_config(const PMIC_AUDIO_HANDLE handle,
+					const PMIC_AUDIO_INPUT_CONFIG config)
+{
+	PMIC_STATUS rc = PMIC_NOT_SUPPORTED;
+	return rc;
+}
+
+/*!
+ * @brief Clear/Disable the audio input section options.
+ *
+ * Clear or disable various audio input section options.
+ *
+ * @param   	handle          Device handle from pmic_audio_open() call.
+ * @param   	config          The audio input section options to be
+ *                              cleared/disabled.
+ * NOT SUPPORTED BY MC13783
+ *
+ * @retval      PMIC_SUCCESS         If the audio input section options were
+ *                                   successfully cleared/disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or the audio input section
+ *                                   options were invalid.
+ * @retval      PMIC_ERROR           If the audio input section options could
+ *                                   not be cleared/disabled.
+ */
+PMIC_STATUS pmic_audio_input_clear_config(const PMIC_AUDIO_HANDLE handle,
+					  const PMIC_AUDIO_INPUT_CONFIG config)
+{
+	PMIC_STATUS rc = PMIC_NOT_SUPPORTED;
+	return rc;
+
+}
+
+/*!
+ * @brief Get the current audio input section options.
+ *
+ * Get the current audio input section options.
+ *
+ * @param[in]   handle          Device handle from pmic_audio_open() call.
+ * @param[out]  config          The current set of audio input section options.
+ * NOT SUPPORTED BY MC13783
+ *
+ * @retval      PMIC_SUCCESS         If the audio input section options were
+ *                                   successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the audio input section options could
+ *                                   not be retrieved.
+ */
+PMIC_STATUS pmic_audio_input_get_config(const PMIC_AUDIO_HANDLE handle,
+					PMIC_AUDIO_INPUT_CONFIG * const config)
+{
+	PMIC_STATUS rc = PMIC_NOT_SUPPORTED;
+	return rc;
+}
+
+/*@}*/
+
+/*************************************************************************
+ * Audio recording using the Voice CODEC.
+ *************************************************************************
+ */
+
+/*!
+ * @name Audio Recording Using the Voice CODEC Setup and Configuration APIs
+ * Functions for general setup and configuration of the PMIC Voice CODEC
+ * to perform audio recording.
+ */
+/*@{*/
+
+/*!
+ * @brief Select the microphone inputs to be used for Voice CODEC recording.
+ *
+ * Select left (mc13783-only) and right microphone inputs for Voice CODEC
+ * recording. It is possible to disable or not use a particular microphone
+ * input channel by specifying NO_MIC as a parameter.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ * @param       leftChannel     Select the left microphone input channel.
+ * @param       rightChannel    Select the right microphone input channel.
+ *
+ * @retval      PMIC_SUCCESS         If the microphone input channels were
+ *                                   successfully enabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or microphone input ports
+ *                                   were invalid.
+ * @retval      PMIC_ERROR           If the microphone input channels could
+ *                                   not be successfully enabled.
+ */
+PMIC_STATUS pmic_audio_vcodec_set_mic(const PMIC_AUDIO_HANDLE handle,
+				      const PMIC_AUDIO_INPUT_PORT leftChannel,
+				      const PMIC_AUDIO_INPUT_PORT rightChannel)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		if (!((leftChannel == NO_MIC) || (leftChannel == MIC1_LEFT))) {
+			rc = PMIC_PARAMETER_ERROR;
+		} else if (!((rightChannel == NO_MIC)
+			     || (rightChannel == MIC1_RIGHT_MIC_MONO)
+			     || (rightChannel == TXIN_EXT)
+			     || (rightChannel == MIC2_AUX))) {
+			rc = PMIC_PARAMETER_ERROR;
+		} else {
+			if (leftChannel == NO_MIC) {
+			} else {	/* Left channel MIC enable */
+				reg_mask = SET_BITS(regAUDIO_TX, AMC1LEN, 1) |
+				    SET_BITS(regAUDIO_TX, RXINREC, 1);
+				reg_write = SET_BITS(regAUDIO_TX, AMC1LEN, 1) |
+				    SET_BITS(regAUDIO_TX, RXINREC, 0);
+			}
+			/*For right channel enable one and clear the other two as well as RXINREC */
+			if (rightChannel == NO_MIC) {
+			} else if (rightChannel == MIC1_RIGHT_MIC_MONO) {
+				reg_mask |= SET_BITS(regAUDIO_TX, AMC1REN, 1) |
+				    SET_BITS(regAUDIO_TX, RXINREC, 1) |
+				    SET_BITS(regAUDIO_TX, AMC2EN, 1) |
+				    SET_BITS(regAUDIO_TX, ATXINEN, 1);
+				reg_write |= SET_BITS(regAUDIO_TX, AMC1REN, 1) |
+				    SET_BITS(regAUDIO_TX, RXINREC, 0) |
+				    SET_BITS(regAUDIO_TX, AMC2EN, 0) |
+				    SET_BITS(regAUDIO_TX, ATXINEN, 0);
+			} else if (rightChannel == MIC2_AUX) {
+				reg_mask |= SET_BITS(regAUDIO_TX, AMC1REN, 1) |
+				    SET_BITS(regAUDIO_TX, RXINREC, 1) |
+				    SET_BITS(regAUDIO_TX, AMC2EN, 1) |
+				    SET_BITS(regAUDIO_TX, ATXINEN, 1);
+				reg_write |= SET_BITS(regAUDIO_TX, AMC1REN, 0) |
+				    SET_BITS(regAUDIO_TX, RXINREC, 0) |
+				    SET_BITS(regAUDIO_TX, AMC2EN, 1) |
+				    SET_BITS(regAUDIO_TX, ATXINEN, 0);
+			} else {	/* TX line in */
+				reg_mask |= SET_BITS(regAUDIO_TX, AMC1REN, 1) |
+				    SET_BITS(regAUDIO_TX, RXINREC, 1) |
+				    SET_BITS(regAUDIO_TX, AMC2EN, 1) |
+				    SET_BITS(regAUDIO_TX, ATXINEN, 1);
+				reg_write |= SET_BITS(regAUDIO_TX, AMC1REN, 0) |
+				    SET_BITS(regAUDIO_TX, RXINREC, 0) |
+				    SET_BITS(regAUDIO_TX, AMC2EN, 0) |
+				    SET_BITS(regAUDIO_TX, ATXINEN, 1);
+			}
+
+			if (reg_mask == 0) {
+				rc = PMIC_PARAMETER_ERROR;
+			} else {
+				rc = pmic_write_reg(REG_AUDIO_TX,
+						    reg_write, reg_mask);
+
+				if (rc == PMIC_SUCCESS) {
+					pr_debug
+					    ("MIC inputs configured successfully\n");
+					vCodec.leftChannelMic.mic = leftChannel;
+					vCodec.rightChannelMic.mic =
+					    rightChannel;
+
+				}
+			}
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the current microphone inputs being used for Voice CODEC
+ *        recording.
+ *
+ * Get the left (mc13783-only) and right microphone inputs currently being
+ * used for Voice CODEC recording.
+ *
+ * @param   	handle          Device handle from pmic_audio_open() call.
+ * @param 	leftChannel     The left microphone input channel.
+ * @param	rightChannel    The right microphone input channel.
+ *
+ * @retval      PMIC_SUCCESS         If the microphone input channels were
+ *                                   successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the microphone input channels could
+ *                                   not be retrieved.
+ */
+PMIC_STATUS pmic_audio_vcodec_get_mic(const PMIC_AUDIO_HANDLE handle,
+				      PMIC_AUDIO_INPUT_PORT * const leftChannel,
+				      PMIC_AUDIO_INPUT_PORT *
+				      const rightChannel)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) &&
+	    (vCodec.handleState == HANDLE_IN_USE) &&
+	    (leftChannel != (PMIC_AUDIO_INPUT_PORT *) NULL) &&
+	    (rightChannel != (PMIC_AUDIO_INPUT_PORT *) NULL)) {
+		*leftChannel = vCodec.leftChannelMic.mic;
+		*rightChannel = vCodec.rightChannelMic.mic;
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+	return rc;
+}
+
+/*!
+ * @brief Enable/disable the microphone input.
+ *
+ * This function enables/disables the current microphone input channel. The
+ * input amplifier is automatically turned off when the microphone input is
+ * disabled.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ * @param       leftChannel     The left microphone input channel state.
+ * @param       rightChannel    the right microphone input channel state.
+ *
+ * @retval      PMIC_SUCCESS         If the microphone input channels were
+ *                                   successfully reconfigured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or microphone input states
+ *                                   were invalid.
+ * @retval      PMIC_ERROR           If the microphone input channels could
+ *                                   not be reconfigured.
+ */
+PMIC_STATUS pmic_audio_vcodec_set_mic_on_off(const PMIC_AUDIO_HANDLE handle,
+					     const PMIC_AUDIO_INPUT_MIC_STATE
+					     leftChannel,
+					     const PMIC_AUDIO_INPUT_MIC_STATE
+					     rightChannel)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+	unsigned int curr_left = 0;
+	unsigned int curr_right = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		curr_left = vCodec.leftChannelMic.mic;
+		curr_right = vCodec.rightChannelMic.mic;
+		if ((curr_left == NO_MIC) && (curr_right == NO_MIC)) {
+			rc = PMIC_PARAMETER_ERROR;
+		} else {
+			if (curr_left == MIC1_LEFT) {
+				if ((leftChannel == MICROPHONE_ON) &&
+				    (vCodec.leftChannelMic.micOnOff ==
+				     MICROPHONE_OFF)) {
+					/* Enable the microphone */
+					reg_mask |=
+					    SET_BITS(regAUDIO_TX, AMC1LEN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   RXINREC, 1);
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC1LEN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   RXINREC, 0);
+
+				} else if ((leftChannel == MICROPHONE_OFF) &&
+					   (vCodec.leftChannelMic.micOnOff ==
+					    MICROPHONE_ON)) {
+					/* Disable the microphone */
+					reg_mask |=
+					    SET_BITS(regAUDIO_TX, AMC1LEN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   RXINREC, 1);
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC1LEN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   RXINREC, 0);
+
+				} else {
+					/* Both are in same state . Nothing to be done */
+				}
+
+			}
+			if (curr_right == MIC1_RIGHT_MIC_MONO) {
+				if ((rightChannel == MICROPHONE_ON) &&
+				    (vCodec.leftChannelMic.micOnOff ==
+				     MICROPHONE_OFF)) {
+					/* Enable the microphone */
+					reg_mask |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   1) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 1);
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   0) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 0);
+				} else if ((rightChannel == MICROPHONE_OFF)
+					   && (vCodec.leftChannelMic.micOnOff ==
+					       MICROPHONE_ON)) {
+					/* Disable the microphone */
+					reg_mask |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   1) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 1);
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   0) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 0);
+				} else {
+					/* Both are in same state . Nothing to be done */
+				}
+			} else if (curr_right == MIC2_AUX) {
+				if ((rightChannel == MICROPHONE_ON)
+				    && (vCodec.leftChannelMic.micOnOff ==
+					MICROPHONE_OFF)) {
+					/* Enable the microphone */
+					reg_mask |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   1) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 1);
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   0) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 0);
+				} else if ((rightChannel == MICROPHONE_OFF)
+					   && (vCodec.leftChannelMic.micOnOff ==
+					       MICROPHONE_ON)) {
+					/* Disable the microphone */
+					reg_mask |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   1) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 1);
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   0) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 0);
+				} else {
+					/* Both are in same state . Nothing to be done */
+				}
+			} else if (curr_right == TXIN_EXT) {
+				if ((rightChannel == MICROPHONE_ON)
+				    && (vCodec.leftChannelMic.micOnOff ==
+					MICROPHONE_OFF)) {
+					/* Enable the microphone */
+					reg_mask |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   1) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 1);
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   0) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 1);
+				} else if ((rightChannel == MICROPHONE_OFF)
+					   && (vCodec.leftChannelMic.micOnOff ==
+					       MICROPHONE_ON)) {
+					/* Disable the microphone */
+					reg_mask |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   1) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     1) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 1);
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC1REN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   RXINREC,
+								   0) |
+					    SET_BITS(regAUDIO_TX, AMC2EN,
+						     0) | SET_BITS(regAUDIO_TX,
+								   ATXINEN, 0);
+				} else {
+					/* Both are in same state . Nothing to be done */
+				}
+			}
+			if (reg_mask == 0) {
+			} else {
+				rc = pmic_write_reg(REG_AUDIO_TX,
+						    reg_write, reg_mask);
+
+				if (rc == PMIC_SUCCESS) {
+					pr_debug
+					    ("MIC states configured successfully\n");
+					vCodec.leftChannelMic.micOnOff =
+					    leftChannel;
+					vCodec.rightChannelMic.micOnOff =
+					    rightChannel;
+				}
+			}
+		}
+
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Return the current state of the microphone inputs.
+ *
+ * This function returns the current state (on/off) of the microphone
+ * input channels.
+ *
+ * @param       handle          Device handle from pmic_audio_open() call.
+ * @param	leftChannel     The current left microphone input channel
+ *                              state.
+ * @param	rightChannel    the current right microphone input channel
+ *                              state.
+ *
+ * @retval      PMIC_SUCCESS         If the microphone input channel states
+ *                                   were successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the microphone input channel states
+ *                                   could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_vcodec_get_mic_on_off(const PMIC_AUDIO_HANDLE handle,
+					     PMIC_AUDIO_INPUT_MIC_STATE *
+					     const leftChannel,
+					     PMIC_AUDIO_INPUT_MIC_STATE *
+					     const rightChannel)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) &&
+	    (vCodec.handleState == HANDLE_IN_USE) &&
+	    (leftChannel != (PMIC_AUDIO_INPUT_MIC_STATE *) NULL) &&
+	    (rightChannel != (PMIC_AUDIO_INPUT_MIC_STATE *) NULL)) {
+		*leftChannel = vCodec.leftChannelMic.micOnOff;
+		*rightChannel = vCodec.rightChannelMic.micOnOff;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Set the microphone input amplifier mode and gain level.
+ *
+ * This function sets the current microphone input amplifier operating mode
+ * and gain level.
+ *
+ * @param       handle           Device handle from pmic_audio_open() call.
+ * @param       leftChannelMode  The left microphone input amplifier mode.
+ * @param       leftChannelGain  The left microphone input amplifier gain level.
+ * @param       rightChannelMode The right microphone input amplifier mode.
+ * @param       rightChannelGain The right microphone input amplifier gain
+ *                               level.
+ *
+ * @retval      PMIC_SUCCESS         If the microphone input amplifiers were
+ *                                   successfully reconfigured.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or microphone input amplifier
+ *                                   modes or gain levels were invalid.
+ * @retval      PMIC_ERROR           If the microphone input amplifiers could
+ *                                   not be reconfigured.
+ */
+PMIC_STATUS pmic_audio_vcodec_set_record_gain(const PMIC_AUDIO_HANDLE handle,
+					      const PMIC_AUDIO_MIC_AMP_MODE
+					      leftChannelMode,
+					      const PMIC_AUDIO_MIC_GAIN
+					      leftChannelGain,
+					      const PMIC_AUDIO_MIC_AMP_MODE
+					      rightChannelMode,
+					      const PMIC_AUDIO_MIC_GAIN
+					      rightChannelGain)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		if (!(((leftChannelGain >= MIC_GAIN_MINUS_8DB)
+		       && (leftChannelGain <= MIC_GAIN_PLUS_23DB))
+		      && ((rightChannelGain >= MIC_GAIN_MINUS_8DB)
+			  && (rightChannelGain <= MIC_GAIN_PLUS_23DB)))) {
+			rc = PMIC_PARAMETER_ERROR;
+			pr_debug("VCODEC set record gain - wrong gain value\n");
+		} else if (((leftChannelMode != AMP_OFF)
+			    && (leftChannelMode != VOLTAGE_TO_VOLTAGE)
+			    && (leftChannelMode != CURRENT_TO_VOLTAGE))
+			   || ((rightChannelMode != VOLTAGE_TO_VOLTAGE)
+			       && (rightChannelMode != CURRENT_TO_VOLTAGE)
+			       && (rightChannelMode != AMP_OFF))) {
+			rc = PMIC_PARAMETER_ERROR;
+			pr_debug("VCODEC set record gain - wrong amp mode\n");
+		} else {
+			if (vCodec.leftChannelMic.mic == MIC1_LEFT) {
+				reg_mask = SET_BITS(regAUDIO_TX, AMC1LITOV, 1) |
+				    SET_BITS(regAUDIO_TX, PGATXL, 31);
+				if (leftChannelMode == VOLTAGE_TO_VOLTAGE) {
+					reg_write =
+					    SET_BITS(regAUDIO_TX, AMC1LITOV, 0);
+				} else {
+					reg_write =
+					    SET_BITS(regAUDIO_TX, AMC1LITOV, 1);
+				}
+				reg_write |=
+				    SET_BITS(regAUDIO_TX, PGATXL,
+					     leftChannelGain);
+			}
+			if (vCodec.rightChannelMic.mic == MIC1_RIGHT_MIC_MONO) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_TX, AMC1RITOV,
+					     1) | SET_BITS(regAUDIO_TX, PGATXR,
+							   31);
+				if (rightChannelMode == VOLTAGE_TO_VOLTAGE) {
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC1RITOV, 0);
+				} else {
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC1RITOV, 1);
+				}
+				reg_write |=
+				    SET_BITS(regAUDIO_TX, PGATXR,
+					     rightChannelGain);
+			} else if (vCodec.rightChannelMic.mic == MIC2_AUX) {
+				reg_mask |= SET_BITS(regAUDIO_TX, AMC2ITOV, 1);
+				reg_mask |= SET_BITS(regAUDIO_TX, PGATXR, 31);
+				if (rightChannelMode == VOLTAGE_TO_VOLTAGE) {
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC2ITOV, 0);
+				} else {
+					reg_write |=
+					    SET_BITS(regAUDIO_TX, AMC2ITOV, 1);
+				}
+				reg_write |=
+				    SET_BITS(regAUDIO_TX, PGATXR,
+					     rightChannelGain);
+			} else if (vCodec.rightChannelMic.mic == TXIN_EXT) {
+				reg_mask |= SET_BITS(regAUDIO_TX, PGATXR, 31);
+				/* No current to voltage option for TX IN amplifier */
+				reg_write |=
+				    SET_BITS(regAUDIO_TX, PGATXR,
+					     rightChannelGain);
+			}
+
+			if (reg_mask == 0) {
+			} else {
+				rc = pmic_write_reg(REG_AUDIO_TX,
+						    reg_write, reg_mask);
+				reg_write =
+				    SET_BITS(regAUDIO_TX, PGATXL,
+					     leftChannelGain);
+				reg_mask = SET_BITS(regAUDIO_TX, PGATXL, 31);
+				rc = pmic_write_reg(REG_AUDIO_TX,
+						    reg_write, reg_mask);
+
+				if (rc == PMIC_SUCCESS) {
+					pr_debug("MIC amp mode and gain set\n");
+					vCodec.leftChannelMic.ampMode =
+					    leftChannelMode;
+					vCodec.leftChannelMic.gain =
+					    leftChannelGain;
+					vCodec.rightChannelMic.ampMode =
+					    rightChannelMode;
+					vCodec.rightChannelMic.gain =
+					    rightChannelGain;
+
+				}
+			}
+		}
+	}
+
+	/* Exit critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the current microphone input amplifier mode and gain level.
+ *
+ * This function gets the current microphone input amplifier operating mode
+ * and gain level.
+ *
+ * @param	handle           Device handle from pmic_audio_open() call.
+ * @param	leftChannelMode  The left microphone input amplifier mode.
+ * @param	leftChannelGain  The left microphone input amplifier gain level.
+ * @param 	rightChannelMode The right microphone input amplifier mode.
+ * @param       rightChannelGain The right microphone input amplifier gain
+ *                               level.
+ *
+ * @retval      PMIC_SUCCESS         If the microphone input amplifier modes
+ *                                   and gain levels were successfully
+ *                                   retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the microphone input amplifier modes
+ *                                   and gain levels could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_vcodec_get_record_gain(const PMIC_AUDIO_HANDLE handle,
+					      PMIC_AUDIO_MIC_AMP_MODE *
+					      const leftChannelMode,
+					      PMIC_AUDIO_MIC_GAIN *
+					      const leftChannelGain,
+					      PMIC_AUDIO_MIC_AMP_MODE *
+					      const rightChannelMode,
+					      PMIC_AUDIO_MIC_GAIN *
+					      const rightChannelGain)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == vCodec.handle) &&
+	    (vCodec.handleState == HANDLE_IN_USE) &&
+	    (leftChannelMode != (PMIC_AUDIO_MIC_AMP_MODE *) NULL) &&
+	    (leftChannelGain != (PMIC_AUDIO_MIC_GAIN *) NULL) &&
+	    (rightChannelMode != (PMIC_AUDIO_MIC_AMP_MODE *) NULL) &&
+	    (rightChannelGain != (PMIC_AUDIO_MIC_GAIN *) NULL)) {
+		*leftChannelMode = vCodec.leftChannelMic.ampMode;
+		*leftChannelGain = vCodec.leftChannelMic.gain;
+		*rightChannelMode = vCodec.rightChannelMic.ampMode;
+		*rightChannelGain = vCodec.rightChannelMic.gain;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Enable a microphone bias circuit.
+ *
+ * This function enables one of the available microphone bias circuits.
+ *
+ * @param       handle           Device handle from pmic_audio_open() call.
+ * @param       biasCircuit      The microphone bias circuit to be enabled.
+ *
+ * @retval      PMIC_SUCCESS         If the microphone bias circuit was
+ *                                   successfully enabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or selected microphone bias
+ *                                   circuit was invalid.
+ * @retval      PMIC_ERROR           If the microphone bias circuit could not
+ *                                   be enabled.
+ */
+PMIC_STATUS pmic_audio_vcodec_enable_micbias(const PMIC_AUDIO_HANDLE handle,
+					     const PMIC_AUDIO_MIC_BIAS
+					     biasCircuit)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		if (biasCircuit & MIC_BIAS1) {
+			reg_write = SET_BITS(regAUDIO_TX, MC1BEN, 1);
+			reg_mask = SET_BITS(regAUDIO_TX, MC1BEN, 1);
+		}
+		if (biasCircuit & MIC_BIAS2) {
+			reg_write |= SET_BITS(regAUDIO_TX, MC2BEN, 1);
+			reg_mask |= SET_BITS(regAUDIO_TX, MC2BEN, 1);
+		}
+		if (reg_mask != 0) {
+			rc = pmic_write_reg(REG_AUDIO_TX, reg_write, reg_mask);
+		}
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief Disable a microphone bias circuit.
+ *
+ * This function disables one of the available microphone bias circuits.
+ *
+ * @param      handle           Device handle from pmic_audio_open() call.
+ * @param      biasCircuit      The microphone bias circuit to be disabled.
+ *
+ * @retval      PMIC_SUCCESS         If the microphone bias circuit was
+ *                                   successfully disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or selected microphone bias
+ *                                   circuit was invalid.
+ * @retval      PMIC_ERROR           If the microphone bias circuit could not
+ *                                   be disabled.
+ */
+PMIC_STATUS pmic_audio_vcodec_disable_micbias(const PMIC_AUDIO_HANDLE handle,
+					      const PMIC_AUDIO_MIC_BIAS
+					      biasCircuit)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		if (biasCircuit & MIC_BIAS1) {
+			reg_mask = SET_BITS(regAUDIO_TX, MC1BEN, 1);
+		}
+
+		if (biasCircuit & MIC_BIAS2) {
+			reg_mask |= SET_BITS(regAUDIO_TX, MC2BEN, 1);
+		}
+
+		if (reg_mask != 0) {
+			rc = pmic_write_reg(REG_AUDIO_TX, reg_write, reg_mask);
+		}
+	}
+
+	return rc;
+}
+
+/*@}*/
+
+/*************************************************************************
+ * Audio Playback Using the Voice CODEC.
+ *************************************************************************
+ */
+
+/*!
+ * @name Audio Playback Using the Voice CODEC Setup and Configuration APIs
+ * Functions for general setup and configuration of the PMIC Voice CODEC
+ * to perform audio playback.
+ */
+/*@{*/
+
+/*!
+ * @brief Configure and enable the Voice CODEC mixer.
+ *
+ * This function configures and enables the Voice CODEC mixer.
+ *
+ * @param       handle              Device handle from pmic_audio_open() call.
+ * @param       rxSecondaryTimeslot The timeslot used for the secondary audio
+ *                                  channel.
+ * @param       gainIn              The secondary audio channel gain level.
+ * @param       gainOut             The mixer output gain level.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC mixer was successfully
+ *                                   configured and enabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or mixer configuration
+ *                                   was invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC mixer could not be
+ *                                   reconfigured or enabled.
+ */
+PMIC_STATUS pmic_audio_vcodec_enable_mixer(const PMIC_AUDIO_HANDLE handle,
+					   const PMIC_AUDIO_VCODEC_TIMESLOT
+					   rxSecondaryTimeslot,
+					   const PMIC_AUDIO_VCODEC_MIX_IN_GAIN
+					   gainIn,
+					   const PMIC_AUDIO_VCODEC_MIX_OUT_GAIN
+					   gainOut)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		if (!((rxSecondaryTimeslot >= USE_TS0)
+		      && (rxSecondaryTimeslot <= USE_TS3))) {
+			pr_debug
+			    ("VCODEC enable mixer - wrong sec rx timeslot\n");
+		} else if (!((gainIn >= VCODEC_NO_MIX)
+			     && (gainIn <= VCODEC_MIX_IN_MINUS_12DB))) {
+			pr_debug("VCODEC enable mixer - wrong mix in gain\n");
+
+		} else if (!((gainOut >= VCODEC_MIX_OUT_0DB)
+			     && (gainOut <= VCODEC_MIX_OUT_MINUS_6DB))) {
+			pr_debug("VCODEC enable mixer - wrong mix out gain\n");
+		} else {
+
+			reg_mask = SET_BITS(regSSI_NETWORK, CDCRXSECSLOT, 3) |
+			    SET_BITS(regSSI_NETWORK, CDCRXSECGAIN, 3) |
+			    SET_BITS(regSSI_NETWORK, CDCSUMGAIN, 1);
+			reg_write =
+			    SET_BITS(regSSI_NETWORK, CDCRXSECSLOT,
+				     rxSecondaryTimeslot) |
+			    SET_BITS(regSSI_NETWORK, CDCRXSECGAIN,
+				     gainIn) | SET_BITS(regSSI_NETWORK,
+							CDCSUMGAIN, gainOut);
+			rc = pmic_write_reg(REG_AUDIO_SSI_NETWORK,
+					    reg_write, reg_mask);
+			if (rc == PMIC_SUCCESS) {
+				pr_debug("Vcodec mixer enabled\n");
+			}
+		}
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief Disable the Voice CODEC mixer.
+ *
+ * This function disables the Voice CODEC mixer.
+ *
+ * @param       handle              Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the Voice CODEC mixer was successfully
+ *                                   disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the Voice CODEC mixer could not be
+ *                                   disabled.
+ */
+PMIC_STATUS pmic_audio_vcodec_disable_mixer(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_mask;
+
+	if ((handle == vCodec.handle) && (vCodec.handleState == HANDLE_IN_USE)) {
+		reg_mask = SET_BITS(regSSI_NETWORK, CDCRXSECGAIN, 1);
+		rc = pmic_write_reg(REG_AUDIO_SSI_NETWORK,
+				    VCODEC_NO_MIX, reg_mask);
+
+		if (rc == PMIC_SUCCESS) {
+			pr_debug("Vcodec mixer disabled\n");
+		}
+
+	}
+
+	return rc;
+}
+
+/*@}*/
+
+/*************************************************************************
+ * Audio Playback Using the Stereo DAC.
+ *************************************************************************
+ */
+
+/*!
+ * @name Audio Playback Using the Stereo DAC Setup and Configuration APIs
+ * Functions for general setup and configuration of the PMIC Stereo DAC
+ * to perform audio playback.
+ */
+/*@{*/
+
+/*!
+ * @brief Configure and enable the Stereo DAC mixer.
+ *
+ * This function configures and enables the Stereo DAC mixer.
+ *
+ * @param      handle              Device handle from pmic_audio_open() call.
+ * @param      rxSecondaryTimeslot The timeslot used for the secondary audio
+ *                                  channel.
+ * @param      gainIn              The secondary audio channel gain level.
+ * @param      gainOut             The mixer output gain level.
+ *
+ * @retval      PMIC_SUCCESS         If the Stereo DAC mixer was successfully
+ *                                   configured and enabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or mixer configuration
+ *                                   was invalid.
+ * @retval      PMIC_ERROR           If the Stereo DAC mixer could not be
+ *                                   reconfigured or enabled.
+ */
+PMIC_STATUS pmic_audio_stdac_enable_mixer(const PMIC_AUDIO_HANDLE handle,
+					  const PMIC_AUDIO_STDAC_TIMESLOTS
+					  rxSecondaryTimeslot,
+					  const PMIC_AUDIO_STDAC_MIX_IN_GAIN
+					  gainIn,
+					  const PMIC_AUDIO_STDAC_MIX_OUT_GAIN
+					  gainOut)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if ((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE)) {
+		if (!((rxSecondaryTimeslot >= USE_TS0_TS1)
+		      && (rxSecondaryTimeslot <= USE_TS6_TS7))) {
+			rc = PMIC_PARAMETER_ERROR;
+			pr_debug("STDAC enable mixer - wrong sec timeslot\n");
+		} else if (!((gainIn >= STDAC_NO_MIX)
+			     && (gainIn <= STDAC_MIX_IN_MINUS_12DB))) {
+			rc = PMIC_PARAMETER_ERROR;
+			pr_debug("STDAC enable mixer - wrong mix in gain\n");
+		} else if (!((gainOut >= STDAC_MIX_OUT_0DB)
+			     && (gainOut <= STDAC_MIX_OUT_MINUS_6DB))) {
+			rc = PMIC_PARAMETER_ERROR;
+			pr_debug("STDAC enable mixer - wrong mix out gain\n");
+		} else {
+
+			reg_mask = SET_BITS(regSSI_NETWORK, STDCRXSECSLOT, 3) |
+			    SET_BITS(regSSI_NETWORK, STDCRXSECGAIN, 3) |
+			    SET_BITS(regSSI_NETWORK, STDCSUMGAIN, 1);
+			reg_write =
+			    SET_BITS(regSSI_NETWORK, STDCRXSECSLOT,
+				     rxSecondaryTimeslot) |
+			    SET_BITS(regSSI_NETWORK, STDCRXSECGAIN,
+				     gainIn) | SET_BITS(regSSI_NETWORK,
+							STDCSUMGAIN, gainOut);
+			rc = pmic_write_reg(REG_AUDIO_SSI_NETWORK,
+					    reg_write, reg_mask);
+			if (rc == PMIC_SUCCESS) {
+				pr_debug("STDAC mixer enabled\n");
+			}
+		}
+
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief Disable the Stereo DAC mixer.
+ *
+ * This function disables the Stereo DAC mixer.
+ *
+ * @param       handle              Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the Stereo DAC mixer was successfully
+ *                                   disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the Stereo DAC mixer could not be
+ *                                   disabled.
+ */
+PMIC_STATUS pmic_audio_stdac_disable_mixer(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	const unsigned int reg_write = 0;
+	const unsigned int reg_mask =
+	    SET_BITS(regSSI_NETWORK, STDCRXSECGAIN, 1);
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if ((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE)) {
+		rc = pmic_write_reg(REG_AUDIO_SSI_NETWORK, reg_write, reg_mask);
+	}
+
+	return rc;
+}
+
+/*@}*/
+
+/*************************************************************************
+ * Audio Output Control
+ *************************************************************************
+ */
+
+/*!
+ * @name Audio Output Section Setup and Configuration APIs
+ * Functions for general setup and configuration of the PMIC audio output
+ * section to support playback.
+ */
+/*@{*/
+
+/*!
+ * @brief Select the audio output ports.
+ *
+ * This function selects the audio output ports to be used. This also enables
+ * the appropriate output amplifiers.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   port                The audio output ports to be used.
+ *
+ * @retval      PMIC_SUCCESS         If the audio output ports were successfully
+ *                                   acquired.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or output ports were
+ *                                   invalid.
+ * @retval      PMIC_ERROR           If the audio output ports could not be
+ *                                   acquired.
+ */
+PMIC_STATUS pmic_audio_output_set_port(const PMIC_AUDIO_HANDLE handle,
+				       const PMIC_AUDIO_OUTPUT_PORT port)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((port == MONO_ALERT) || (port == MONO_EXTOUT)) {
+		rc = PMIC_NOT_SUPPORTED;
+	} else {
+		if (((handle == stDAC.handle)
+		     && (stDAC.handleState == HANDLE_IN_USE))
+		    || ((handle == extStereoIn.handle)
+			&& (extStereoIn.handleState == HANDLE_IN_USE))
+		    || ((handle == vCodec.handle)
+			&& (vCodec.handleState == HANDLE_IN_USE)
+			&& (audioOutput.vCodecOut == VCODEC_MIXER_OUT))) {
+			/* Stereo signal and MIXER source needs to be routed to the port
+			   / Avoid Codec direct out */
+
+			if (port & MONO_SPEAKER) {
+				reg_mask = SET_BITS(regAUDIO_RX_0, ASPEN, 1) |
+				    SET_BITS(regAUDIO_RX_0, ASPSEL, 1);
+				reg_write = SET_BITS(regAUDIO_RX_0, ASPEN, 1) |
+				    SET_BITS(regAUDIO_RX_0, ASPSEL, 1);
+			}
+			if (port & MONO_LOUDSPEAKER) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, ALSPEN, 1) |
+				    SET_BITS(regAUDIO_RX_0, ALSPREF, 1) |
+				    SET_BITS(regAUDIO_RX_0, ALSPSEL, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ALSPEN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   ALSPREF,
+							   1) |
+				    SET_BITS(regAUDIO_RX_0, ALSPSEL, 1);
+			}
+			if (port & STEREO_HEADSET_LEFT) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, AHSLEN, 1) |
+				    SET_BITS(regAUDIO_RX_0, AHSSEL, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, AHSLEN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   AHSSEL, 1);
+			}
+			if (port & STEREO_HEADSET_RIGHT) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, AHSREN, 1) |
+				    SET_BITS(regAUDIO_RX_0, AHSSEL, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, AHSREN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   AHSSEL, 1);
+			}
+			if (port & STEREO_OUT_LEFT) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTLEN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   ARXOUTSEL, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTLEN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   ARXOUTSEL, 1);
+			}
+			if (port & STEREO_OUT_RIGHT) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTREN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   ARXOUTSEL, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTREN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   ARXOUTSEL, 1);
+			}
+			if (port & STEREO_LEFT_LOW_POWER) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, LSPLEN, 1);
+
+				reg_write |= SET_BITS(regAUDIO_RX_0, LSPLEN, 1);
+			}
+		} else if ((handle == vCodec.handle)
+			   && (vCodec.handleState == HANDLE_IN_USE)
+			   && (audioOutput.vCodecOut = VCODEC_DIRECT_OUT)) {
+			if (port & MONO_SPEAKER) {
+				reg_mask = SET_BITS(regAUDIO_RX_0, ASPEN, 1) |
+				    SET_BITS(regAUDIO_RX_0, ASPSEL, 1);
+				reg_write = SET_BITS(regAUDIO_RX_0, ASPEN, 1) |
+				    SET_BITS(regAUDIO_RX_0, ASPSEL, 0);
+			}
+			if (port & MONO_LOUDSPEAKER) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, ALSPEN, 1) |
+				    SET_BITS(regAUDIO_RX_0, ALSPREF, 1) |
+				    SET_BITS(regAUDIO_RX_0, ALSPSEL, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ALSPEN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   ALSPREF,
+							   1) |
+				    SET_BITS(regAUDIO_RX_0, ALSPSEL, 0);
+			}
+
+			if (port & STEREO_HEADSET_LEFT) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, AHSLEN, 1) |
+				    SET_BITS(regAUDIO_RX_0, AHSSEL, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, AHSLEN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   AHSSEL, 0);
+			}
+			if (port & STEREO_HEADSET_RIGHT) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, AHSREN, 1) |
+				    SET_BITS(regAUDIO_RX_0, AHSSEL, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, AHSREN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   AHSSEL, 0);
+			}
+			if (port & STEREO_OUT_LEFT) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTLEN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   ARXOUTSEL, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTLEN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   ARXOUTSEL, 0);
+			}
+			if (port & STEREO_OUT_RIGHT) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTREN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   ARXOUTSEL, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTREN,
+					     1) | SET_BITS(regAUDIO_RX_0,
+							   ARXOUTSEL, 0);
+			}
+			if (port & MONO_CDCOUT) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, CDCOUTEN, 1);
+
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, CDCOUTEN, 1);
+			}
+		}
+
+		if (reg_mask == 0) {
+
+		} else {
+			rc = pmic_write_reg(REG_AUDIO_RX_0,
+					    reg_write, reg_mask);
+
+			if (rc == PMIC_SUCCESS) {
+				pr_debug("output ports  enabled\n");
+				audioOutput.outputPort = port;
+
+			}
+		}
+	}
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Deselect/disable the audio output ports.
+ *
+ * This function disables the audio output ports that were previously enabled
+ * by calling pmic_audio_output_set_port().
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   port                The audio output ports to be disabled.
+ *
+ * @retval      PMIC_SUCCESS         If the audio output ports were successfully
+ *                                   disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or output ports were
+ *                                   invalid.
+ * @retval      PMIC_ERROR           If the audio output ports could not be
+ *                                   disabled.
+ */
+PMIC_STATUS pmic_audio_output_clear_port(const PMIC_AUDIO_HANDLE handle,
+					 const PMIC_AUDIO_OUTPUT_PORT port)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((port == MONO_ALERT) || (port == MONO_EXTOUT)) {
+		rc = PMIC_NOT_SUPPORTED;
+	} else {
+		if (((handle == stDAC.handle)
+		     && (stDAC.handleState == HANDLE_IN_USE))
+		    || ((handle == extStereoIn.handle)
+			&& (extStereoIn.handleState == HANDLE_IN_USE))
+		    || ((handle == vCodec.handle)
+			&& (vCodec.handleState == HANDLE_IN_USE)
+			&& (audioOutput.vCodecOut = VCODEC_MIXER_OUT))) {
+			/* Stereo signal and MIXER source needs to be routed to the port /
+			   Avoid Codec direct out */
+			if (port & MONO_SPEAKER) {
+				reg_mask = SET_BITS(regAUDIO_RX_0, ASPEN, 1);
+				reg_write = SET_BITS(regAUDIO_RX_0, ASPEN, 0);
+			}
+			if (port & MONO_LOUDSPEAKER) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, ALSPEN, 1) |
+				    SET_BITS(regAUDIO_RX_0, ALSPREF, 1);
+
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ALSPEN,
+					     0) | SET_BITS(regAUDIO_RX_0,
+							   ALSPREF, 0);
+
+			}
+			if (port & STEREO_HEADSET_LEFT) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, AHSLEN, 1);
+				reg_write |= SET_BITS(regAUDIO_RX_0, AHSLEN, 0);
+			}
+			if (port & STEREO_HEADSET_RIGHT) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, AHSREN, 1);
+				reg_write |= SET_BITS(regAUDIO_RX_0, AHSREN, 0);
+			}
+			if (port & STEREO_OUT_LEFT) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTLEN, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTLEN, 0);
+			}
+			if (port & STEREO_OUT_RIGHT) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTREN, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTREN, 0);
+			}
+			if (port & STEREO_LEFT_LOW_POWER) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, LSPLEN, 1);
+				reg_write |= SET_BITS(regAUDIO_RX_0, LSPLEN, 0);
+			}
+		} else if ((handle == vCodec.handle)
+			   && (vCodec.handleState == HANDLE_IN_USE)
+			   && (audioOutput.vCodecOut = VCODEC_DIRECT_OUT)) {
+			if (port & MONO_SPEAKER) {
+				reg_mask = SET_BITS(regAUDIO_RX_0, ASPEN, 1);
+				reg_write = SET_BITS(regAUDIO_RX_0, ASPEN, 0);
+			}
+			if (port & MONO_LOUDSPEAKER) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, ALSPEN, 1) |
+				    SET_BITS(regAUDIO_RX_0, ALSPREF, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ALSPEN,
+					     0) | SET_BITS(regAUDIO_RX_0,
+							   ALSPREF, 0);
+			}
+			if (port & STEREO_HEADSET_LEFT) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, AHSLEN, 1);
+				reg_write |= SET_BITS(regAUDIO_RX_0, AHSLEN, 0);
+			}
+			if (port & STEREO_HEADSET_RIGHT) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, AHSREN, 1);
+				reg_write |= SET_BITS(regAUDIO_RX_0, AHSREN, 0);
+			}
+			if (port & STEREO_OUT_LEFT) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTLEN, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTLEN, 0);
+			}
+			if (port & STEREO_OUT_RIGHT) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTREN, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, ARXOUTREN, 0);
+			}
+			if (port & MONO_CDCOUT) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, CDCOUTEN, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, CDCOUTEN, 0);
+			}
+		}
+#ifdef CONFIG_HEADSET_DETECT_ENABLE
+
+		if (port & STEREO_HEADSET_LEFT) {
+			reg_mask |= SET_BITS(regAUDIO_RX_0, AHSLEN, 1);
+			reg_write |= SET_BITS(regAUDIO_RX_0, AHSLEN, 0);
+		}
+		if (port & STEREO_HEADSET_RIGHT) {
+			reg_mask |= SET_BITS(regAUDIO_RX_0, AHSREN, 1);
+			reg_write |= SET_BITS(regAUDIO_RX_0, AHSREN, 0);
+		}
+#endif
+
+		if (reg_mask == 0) {
+
+		} else {
+			rc = pmic_write_reg(REG_AUDIO_RX_0,
+					    reg_write, reg_mask);
+			if (rc == PMIC_SUCCESS) {
+				pr_debug("output ports disabled\n");
+				audioOutput.outputPort &= ~port;
+			}
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the current audio output ports.
+ *
+ * This function retrieves the audio output ports that are currently being
+ * used.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   port                The audio output ports currently being used.
+ *
+ * @retval      PMIC_SUCCESS         If the audio output ports were successfully
+ *                                   retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the audio output ports could not be
+ *                                   retrieved.
+ */
+PMIC_STATUS pmic_audio_output_get_port(const PMIC_AUDIO_HANDLE handle,
+				       PMIC_AUDIO_OUTPUT_PORT * const port)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((((handle == stDAC.handle) &&
+	      (stDAC.handleState == HANDLE_IN_USE)) ||
+	     ((handle == vCodec.handle) &&
+	      (vCodec.handleState == HANDLE_IN_USE)) ||
+	     ((handle == extStereoIn.handle) &&
+	      (extStereoIn.handleState == HANDLE_IN_USE))) &&
+	    (port != (PMIC_AUDIO_OUTPUT_PORT *) NULL)) {
+		*port = audioOutput.outputPort;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Set the gain level for the external stereo inputs.
+ *
+ * This function sets the gain levels for the external stereo inputs.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   gain                The external stereo input gain level.
+ *
+ * @retval      PMIC_SUCCESS         If the gain level was successfully set.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or gain level was invalid.
+ * @retval      PMIC_ERROR           If the gain level could not be set.
+ */
+PMIC_STATUS pmic_audio_output_set_stereo_in_gain(const PMIC_AUDIO_HANDLE handle,
+						 const PMIC_AUDIO_STEREO_IN_GAIN
+						 gain)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_mask = SET_BITS(regAUDIO_RX_1, ARXINEN, 1) |
+	    SET_BITS(regAUDIO_RX_1, ARXIN, 1);
+	unsigned int reg_write = SET_BITS(regAUDIO_RX_1, ARXINEN, 1);
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	/* The ARX amplifier for stereo is also enabled over here */
+
+	if ((gain == STEREO_IN_GAIN_0DB) || (gain == STEREO_IN_GAIN_PLUS_18DB)) {
+		if ((handle == extStereoIn.handle) &&
+		    (extStereoIn.handleState == HANDLE_IN_USE)) {
+
+			if (gain == STEREO_IN_GAIN_0DB) {
+				reg_write |= SET_BITS(regAUDIO_RX_1, ARXIN, 1);
+			} else {
+				reg_write |= SET_BITS(regAUDIO_RX_1, ARXIN, 0);
+			}
+
+			rc = pmic_write_reg(REG_AUDIO_RX_1,
+					    reg_write, reg_mask);
+
+			if (rc == PMIC_SUCCESS) {
+				pr_debug("Ext stereo gain set\n");
+				extStereoIn.inputGain = gain;
+
+			}
+
+		} else {
+			rc = PMIC_PARAMETER_ERROR;
+		}
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief Get the current gain level for the external stereo inputs.
+ *
+ * This function retrieves the current gain levels for the external stereo
+ * inputs.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   gain                The current external stereo input gain
+ *                                  level.
+ *
+ * @retval      PMIC_SUCCESS         If the gain level was successfully
+ *                                   retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the gain level could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_output_get_stereo_in_gain(const PMIC_AUDIO_HANDLE handle,
+						 PMIC_AUDIO_STEREO_IN_GAIN *
+						 const gain)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == extStereoIn.handle) &&
+	    (extStereoIn.handleState == HANDLE_IN_USE) &&
+	    (gain != (PMIC_AUDIO_STEREO_IN_GAIN *) NULL)) {
+		*gain = extStereoIn.inputGain;
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Set the output PGA gain level.
+ *
+ * This function sets the audio output PGA gain level.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   gain                The output PGA gain level.
+ *
+ * @retval      PMIC_SUCCESS         If the gain level was successfully set.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or gain level was invalid.
+ * @retval      PMIC_ERROR           If the gain level could not be set.
+ */
+PMIC_STATUS pmic_audio_output_set_pgaGain(const PMIC_AUDIO_HANDLE handle,
+					  const PMIC_AUDIO_OUTPUT_PGA_GAIN gain)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = 0;
+	unsigned int reg_gain;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if (!((gain >= OUTPGA_GAIN_MINUS_33DB)
+	      && (gain <= OUTPGA_GAIN_PLUS_6DB))) {
+		rc = PMIC_NOT_SUPPORTED;
+		pr_debug("output set PGA gain - wrong gain value\n");
+	} else {
+		reg_gain = gain + 2;
+		if ((handle == extStereoIn.handle) &&
+		    (extStereoIn.handleState == HANDLE_IN_USE)) {
+			reg_mask = SET_BITS(regAUDIO_RX_1, ARXIN, 15) |
+			    SET_BITS(regAUDIO_RX_1, ARXINEN, 1);
+			reg_write = SET_BITS(regAUDIO_RX_1, ARXIN, reg_gain) |
+			    SET_BITS(regAUDIO_RX_1, ARXINEN, 1);
+		} else if ((handle == vCodec.handle) &&
+			   (vCodec.handleState == HANDLE_IN_USE)) {
+			reg_mask = SET_BITS(regAUDIO_RX_1, PGARX, 15);
+			reg_write = SET_BITS(regAUDIO_RX_1, PGARX, reg_gain);
+		} else if ((handle == stDAC.handle) &&
+			   (stDAC.handleState == HANDLE_IN_USE)) {
+			reg_mask = SET_BITS(regAUDIO_RX_1, PGAST, 15);
+			reg_write = SET_BITS(regAUDIO_RX_1, PGAST, reg_gain);
+		}
+
+		if (reg_mask == 0) {
+
+		} else {
+			rc = pmic_write_reg(REG_AUDIO_RX_1,
+					    reg_write, reg_mask);
+
+			if (rc == PMIC_SUCCESS) {
+				pr_debug("Output PGA gains set\n");
+
+				if (handle == stDAC.handle) {
+					audioOutput.stDacoutputPGAGain = gain;
+				} else if (handle == vCodec.handle) {
+					audioOutput.vCodecoutputPGAGain = gain;
+				} else {
+					audioOutput.extStereooutputPGAGain =
+					    gain;
+				}
+			} else {
+				pr_debug
+				    ("Error writing PGA gains to register\n");
+			}
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the output PGA gain level.
+ *
+ * This function retrieves the current audio output PGA gain level.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   gain                The current output PGA gain level.
+ *
+ * @retval      PMIC_SUCCESS         If the gain level was successfully
+ *                                   retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the gain level could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_output_get_pgaGain(const PMIC_AUDIO_HANDLE handle,
+					  PMIC_AUDIO_OUTPUT_PGA_GAIN *
+					  const gain)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if (gain != (PMIC_AUDIO_OUTPUT_PGA_GAIN *) NULL) {
+		if ((handle == extStereoIn.handle) &&
+		    (extStereoIn.handleState == HANDLE_IN_USE)) {
+			*gain = audioOutput.extStereooutputPGAGain;
+			rc = PMIC_SUCCESS;
+		} else if ((handle == vCodec.handle) &&
+			   (vCodec.handleState == HANDLE_IN_USE)) {
+			*gain = audioOutput.vCodecoutputPGAGain;
+			rc = PMIC_SUCCESS;
+		} else if ((handle == stDAC.handle) &&
+			   (stDAC.handleState == HANDLE_IN_USE)) {
+			*gain = audioOutput.stDacoutputPGAGain;
+			rc = PMIC_SUCCESS;
+		} else {
+			rc = PMIC_PARAMETER_ERROR;
+		}
+	} else {
+		rc = PMIC_PARAMETER_ERROR;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Enable the output mixer.
+ *
+ * This function enables the output mixer for the audio stream that
+ * corresponds to the current handle (i.e., the Voice CODEC, Stereo DAC, or
+ * the external stereo inputs).
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the mixer was successfully enabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the mixer could not be enabled.
+ */
+PMIC_STATUS pmic_audio_output_enable_mixer(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_mask = 0;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask_mix = 0;
+	unsigned int reg_write_mix = 0;
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if (((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE))) {
+		reg_mask = SET_BITS(regAUDIO_RX_1, PGASTEN, 1);
+		reg_write = SET_BITS(regAUDIO_RX_1, PGASTEN, 1);
+		reg_mask_mix = SET_BITS(regAUDIO_RX_0, ADDSTDC, 1);
+		reg_write_mix = SET_BITS(regAUDIO_RX_0, ADDSTDC, 1);
+	} else if ((handle == vCodec.handle) &&
+		   (vCodec.handleState == HANDLE_IN_USE)) {
+		reg_mask = SET_BITS(regAUDIO_RX_1, PGARXEN, 1);
+		reg_write = SET_BITS(regAUDIO_RX_1, PGARXEN, 1);
+		audioOutput.vCodecOut = VCODEC_MIXER_OUT;
+
+		reg_mask_mix = SET_BITS(regAUDIO_RX_0, ADDCDC, 1);
+		reg_write_mix = SET_BITS(regAUDIO_RX_0, ADDCDC, 1);
+	} else if ((handle == extStereoIn.handle) &&
+		   (extStereoIn.handleState == HANDLE_IN_USE)) {
+		reg_mask = SET_BITS(regAUDIO_RX_1, ARXINEN, 1);
+		reg_write = SET_BITS(regAUDIO_RX_1, ARXINEN, 1);
+		reg_mask_mix = SET_BITS(regAUDIO_RX_0, ADDRXIN, 1);
+		reg_write_mix = SET_BITS(regAUDIO_RX_0, ADDRXIN, 1);
+	}
+
+	if (reg_mask == 0) {
+
+	} else {
+		rc = pmic_write_reg(REG_AUDIO_RX_1, reg_write, reg_mask);
+
+		if (rc == PMIC_SUCCESS) {
+
+			rc = pmic_write_reg(REG_AUDIO_RX_0,
+					    reg_write_mix, reg_mask_mix);
+			if (rc == PMIC_SUCCESS) {
+				pr_debug("Output PGA mixers enabled\n");
+				rc = PMIC_SUCCESS;
+			}
+
+		} else {
+			pr_debug("Error writing mixer enable to register\n");
+		}
+
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief Disable the output mixer.
+ *
+ * This function disables the output mixer for the audio stream that
+ * corresponds to the current handle (i.e., the Voice CODEC, Stereo DAC, or
+ * the external stereo inputs).
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the mixer was successfully disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the mixer could not be disabled.
+ */
+PMIC_STATUS pmic_audio_output_disable_mixer(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_mask = 0;
+	unsigned int reg_write = 0;
+
+	unsigned int reg_mask_mix = 0;
+	unsigned int reg_write_mix = 0;
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+	if (((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE))) {
+		/*reg_mask = SET_BITS(regAUDIO_RX_1, PGASTEN, 1);
+		   reg_write = SET_BITS(regAUDIO_RX_1, PGASTEN, 0); */
+
+		reg_mask_mix = SET_BITS(regAUDIO_RX_0, ADDSTDC, 1);
+		reg_write_mix = SET_BITS(regAUDIO_RX_0, ADDSTDC, 0);
+	} else if ((handle == vCodec.handle) &&
+		   (vCodec.handleState == HANDLE_IN_USE)) {
+		reg_mask = SET_BITS(regAUDIO_RX_1, PGARXEN, 1);
+		reg_write = SET_BITS(regAUDIO_RX_1, PGARXEN, 0);
+		audioOutput.vCodecOut = VCODEC_DIRECT_OUT;
+
+		reg_mask_mix = SET_BITS(regAUDIO_RX_0, ADDCDC, 1);
+		reg_write_mix = SET_BITS(regAUDIO_RX_0, ADDCDC, 0);
+	} else if ((handle == extStereoIn.handle) &&
+		   (extStereoIn.handleState == HANDLE_IN_USE)) {
+		/*reg_mask = SET_BITS(regAUDIO_RX_1, ARXINEN, 1);
+		   reg_write = SET_BITS(regAUDIO_RX_1, ARXINEN, 0); */
+
+		reg_mask_mix = SET_BITS(regAUDIO_RX_0, ADDRXIN, 1);
+		reg_write_mix = SET_BITS(regAUDIO_RX_0, ADDRXIN, 1);
+	}
+
+	if (reg_mask == 0) {
+
+	} else {
+		rc = pmic_write_reg(REG_AUDIO_RX_1, reg_write, reg_mask);
+
+		if (rc == PMIC_SUCCESS) {
+
+			rc = pmic_write_reg(REG_AUDIO_RX_0,
+					    reg_write_mix, reg_mask_mix);
+			if (rc == PMIC_SUCCESS) {
+				pr_debug("Output PGA mixers disabled\n");
+			}
+		}
+	}
+	return rc;
+}
+
+/*!
+ * @brief Configure and enable the output balance amplifiers.
+ *
+ * This function configures and enables the output balance amplifiers.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   leftGain            The desired left channel gain level.
+ * @param   rightGain           The desired right channel gain level.
+ *
+ * @retval      PMIC_SUCCESS         If the output balance amplifiers were
+ *                                   successfully configured and enabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or gain levels were invalid.
+ * @retval      PMIC_ERROR           If the output balance amplifiers could not
+ *                                   be reconfigured or enabled.
+ */
+PMIC_STATUS pmic_audio_output_set_balance(const PMIC_AUDIO_HANDLE handle,
+					  const PMIC_AUDIO_OUTPUT_BALANCE_GAIN
+					  leftGain,
+					  const PMIC_AUDIO_OUTPUT_BALANCE_GAIN
+					  rightGain)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_mask = 0;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask_ch = 0;
+	unsigned int reg_write_ch = 0;
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if (!((leftGain >= BAL_GAIN_MINUS_21DB) && (leftGain <= BAL_GAIN_0DB))) {
+		rc = PMIC_PARAMETER_ERROR;
+	} else if (!((rightGain >= BAL_GAIN_MINUS_21DB)
+		     && (rightGain <= BAL_GAIN_0DB))) {
+		rc = PMIC_PARAMETER_ERROR;
+	} else {
+		if (((handle == stDAC.handle) &&
+		     (stDAC.handleState == HANDLE_IN_USE)) ||
+		    ((handle == vCodec.handle) &&
+		     (vCodec.handleState == HANDLE_IN_USE)) ||
+		    ((handle == extStereoIn.handle) &&
+		     (extStereoIn.handleState == HANDLE_IN_USE))) {
+			/* In mc13783 only one channel can be attenuated wrt the other.
+			 * It is not possible to specify attenuation for both
+			 * This function will return an error if both channels
+			 * are required to be attenuated
+			 * The BALLR bit is set/reset depending on whether leftGain
+			 * or rightGain is specified*/
+			if ((rightGain == BAL_GAIN_0DB)
+			    && (leftGain == BAL_GAIN_0DB)) {
+				/* Nothing to be done */
+			} else if ((rightGain != BAL_GAIN_0DB)
+				   && (leftGain == BAL_GAIN_0DB)) {
+				/* Attenuate right channel */
+				reg_mask = SET_BITS(regAUDIO_RX_1, BAL, 7);
+				reg_mask_ch = SET_BITS(regAUDIO_RX_1, BALLR, 1);
+				reg_write =
+				    SET_BITS(regAUDIO_RX_1, BAL,
+					     (BAL_GAIN_0DB - rightGain));
+				/* The enum and the register values are reversed in order .. */
+				reg_write_ch =
+				    SET_BITS(regAUDIO_RX_1, BALLR, 0);
+				/* BALLR = 0 selects right channel for atten */
+			} else if ((rightGain == BAL_GAIN_0DB)
+				   && (leftGain != BAL_GAIN_0DB)) {
+				/* Attenuate left channel */
+
+				reg_mask = SET_BITS(regAUDIO_RX_1, BAL, 7);
+				reg_mask_ch = SET_BITS(regAUDIO_RX_1, BALLR, 1);
+				reg_write =
+				    SET_BITS(regAUDIO_RX_1, BAL,
+					     (BAL_GAIN_0DB - leftGain));
+				reg_write_ch =
+				    SET_BITS(regAUDIO_RX_1, BALLR, 1);
+				/* BALLR = 1 selects left channel for atten */
+			} else {
+				rc = PMIC_PARAMETER_ERROR;
+			}
+
+			if ((reg_mask == 0) || (reg_mask_ch == 0)) {
+
+			} else {
+				rc = pmic_write_reg(REG_AUDIO_RX_1,
+						    reg_write_ch, reg_mask_ch);
+
+				if (rc == PMIC_SUCCESS) {
+					rc = pmic_write_reg(REG_AUDIO_RX_1,
+							    reg_write,
+							    reg_mask);
+
+					if (rc == PMIC_SUCCESS) {
+						pr_debug
+						    ("Output balance attenuation set\n");
+						audioOutput.balanceLeftGain =
+						    leftGain;
+						audioOutput.balanceRightGain =
+						    rightGain;
+					}
+				}
+			}
+		}
+	}
+	return rc;
+}
+
+/*!
+ * @brief Get the current output balance amplifier gain levels.
+ *
+ * This function retrieves the current output balance amplifier gain levels.
+ *
+ * @param   	handle              Device handle from pmic_audio_open() call.
+ * @param  	leftGain            The current left channel gain level.
+ * @param 	rightGain           The current right channel gain level.
+ *
+ * @retval      PMIC_SUCCESS         If the output balance amplifier gain levels
+ *                                   were successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the output balance amplifier gain levels
+ *                                   could be retrieved.
+ */
+PMIC_STATUS pmic_audio_output_get_balance(const PMIC_AUDIO_HANDLE handle,
+					  PMIC_AUDIO_OUTPUT_BALANCE_GAIN *
+					  const leftGain,
+					  PMIC_AUDIO_OUTPUT_BALANCE_GAIN *
+					  const rightGain)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((((handle == stDAC.handle) &&
+	      (stDAC.handleState == HANDLE_IN_USE)) ||
+	     ((handle == vCodec.handle) &&
+	      (vCodec.handleState == HANDLE_IN_USE)) ||
+	     ((handle == extStereoIn.handle) &&
+	      (extStereoIn.handleState == HANDLE_IN_USE))) &&
+	    ((leftGain != (PMIC_AUDIO_OUTPUT_BALANCE_GAIN *) NULL) &&
+	     (rightGain != (PMIC_AUDIO_OUTPUT_BALANCE_GAIN *) NULL))) {
+		*leftGain = audioOutput.balanceLeftGain;
+		*rightGain = audioOutput.balanceRightGain;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Configure and enable the output mono adder.
+ *
+ * This function configures and enables the output mono adder.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   mode                The desired mono adder operating mode.
+ *
+ * @retval      PMIC_SUCCESS         If the mono adder was successfully
+ *                                   configured and enabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or mono adder mode was
+ *                                   invalid.
+ * @retval      PMIC_ERROR           If the mono adder could not be reconfigured
+ *                                   or enabled.
+ */
+PMIC_STATUS pmic_audio_output_enable_mono_adder(const PMIC_AUDIO_HANDLE handle,
+						const PMIC_AUDIO_MONO_ADDER_MODE
+						mode)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_write = 0;
+	unsigned int reg_mask = SET_BITS(regAUDIO_RX_1, MONO, 3);
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if ((mode >= MONO_ADDER_OFF) && (mode <= STEREO_OPPOSITE_PHASE)) {
+		if (((handle == stDAC.handle) &&
+		     (stDAC.handleState == HANDLE_IN_USE)) ||
+		    ((handle == vCodec.handle) &&
+		     (vCodec.handleState == HANDLE_IN_USE)) ||
+		    ((handle == extStereoIn.handle) &&
+		     (extStereoIn.handleState == HANDLE_IN_USE))) {
+			if (mode == MONO_ADDER_OFF) {
+				reg_write = SET_BITS(regAUDIO_RX_1, MONO, 0);
+			} else if (mode == MONO_ADD_LEFT_RIGHT) {
+				reg_write = SET_BITS(regAUDIO_RX_1, MONO, 2);
+			} else if (mode == MONO_ADD_OPPOSITE_PHASE) {
+				reg_write = SET_BITS(regAUDIO_RX_1, MONO, 3);
+			} else {	/* stereo opposite */
+
+				reg_write = SET_BITS(regAUDIO_RX_1, MONO, 1);
+			}
+
+			rc = pmic_write_reg(REG_AUDIO_RX_1,
+					    reg_write, reg_mask);
+
+			if (rc == PMIC_SUCCESS) {
+				pr_debug("Output mono adder mode set\n");
+
+			}
+
+		} else {
+			rc = PMIC_PARAMETER_ERROR;
+		}
+	} else {
+		rc = PMIC_PARAMETER_ERROR;
+	}
+	return rc;
+}
+
+/*!
+ * @brief Disable the output mono adder.
+ *
+ * This function disables the output mono adder.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the mono adder was successfully
+ *                                   disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the mono adder could not be disabled.
+ */
+PMIC_STATUS pmic_audio_output_disable_mono_adder(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	const unsigned int reg_write = 0;
+	const unsigned int reg_mask = SET_BITS(regAUDIO_RX_1, MONO, 3);
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	if (((handle == stDAC.handle) &&
+	     (stDAC.handleState == HANDLE_IN_USE)) ||
+	    ((handle == vCodec.handle) &&
+	     (vCodec.handleState == HANDLE_IN_USE)) ||
+	    ((handle == extStereoIn.handle) &&
+	     (extStereoIn.handleState == HANDLE_IN_USE))) {
+		rc = pmic_write_reg(REG_AUDIO_RX_1, reg_write, reg_mask);
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief Configure the mono adder output gain level.
+ *
+ * This function configures the mono adder output amplifier gain level.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   gain                The desired output gain level.
+ *
+ * @retval      PMIC_SUCCESS         If the mono adder output amplifier gain
+ *                                   level was successfully set.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or gain level was invalid.
+ * @retval      PMIC_ERROR           If the mono adder output amplifier gain
+ *                                   level could not be reconfigured.
+ */
+PMIC_STATUS pmic_audio_output_set_mono_adder_gain(const PMIC_AUDIO_HANDLE
+						  handle,
+						  const
+						  PMIC_AUDIO_MONO_ADDER_OUTPUT_GAIN
+						  gain)
+{
+	PMIC_STATUS rc = PMIC_NOT_SUPPORTED;
+	return rc;
+}
+
+/*!
+ * @brief Get the current mono adder output gain level.
+ *
+ * This function retrieves the current mono adder output amplifier gain level.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   gain                The current output gain level.
+ *
+ * @retval      PMIC_SUCCESS         If the mono adder output amplifier gain
+ *                                   level was successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the mono adder output amplifier gain
+ *                                   level could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_output_get_mono_adder_gain(const PMIC_AUDIO_HANDLE
+						  handle,
+						  PMIC_AUDIO_MONO_ADDER_OUTPUT_GAIN
+						  * const gain)
+{
+	PMIC_STATUS rc = PMIC_NOT_SUPPORTED;
+	return rc;
+}
+
+/*!
+ * @brief Set various audio output section options.
+ *
+ * This function sets one or more audio output section configuration
+ * options. The currently supported options include whether to disable
+ * the non-inverting mono speaker output, enabling the loudspeaker common
+ * bias circuit, enabling detection of headset insertion/removal, and
+ * whether to automatically disable the headset amplifiers when a headset
+ * insertion/removal has been detected.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   config              The desired audio output section
+ *                                  configuration options to be set.
+ *
+ * @retval      PMIC_SUCCESS         If the desired configuration options were
+ *                                   all successfully set.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or configuration options
+ *                                   were invalid.
+ * @retval      PMIC_ERROR           If the desired configuration options
+ *                                   could not be set.
+ */
+PMIC_STATUS pmic_audio_output_set_config(const PMIC_AUDIO_HANDLE handle,
+					 const PMIC_AUDIO_OUTPUT_CONFIG config)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_mask = 0;
+	unsigned int reg_write = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if (((handle == stDAC.handle) &&
+	     (stDAC.handleState == HANDLE_IN_USE)) ||
+	    ((handle == vCodec.handle) &&
+	     (vCodec.handleState == HANDLE_IN_USE)) ||
+	    ((handle == extStereoIn.handle) &&
+	     (extStereoIn.handleState == HANDLE_IN_USE))) {
+		if (config & MONO_SPEAKER_INVERT_OUT_ONLY) {
+			/* If this is one of the parameters */
+			rc = PMIC_NOT_SUPPORTED;
+		} else {
+			if (config & MONO_LOUDSPEAKER_COMMON_BIAS) {
+				reg_mask = SET_BITS(regAUDIO_RX_0, ALSPREF, 1);
+				reg_write = SET_BITS(regAUDIO_RX_0, ALSPREF, 1);
+			}
+			if (config & HEADSET_DETECT_ENABLE) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, HSDETEN, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, HSDETEN, 1);
+			}
+			if (config & STEREO_HEADSET_AMP_AUTO_DISABLE) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, HSDETAUTOB, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, HSDETAUTOB, 1);
+			}
+
+			if (reg_mask == 0) {
+				rc = PMIC_PARAMETER_ERROR;
+			} else {
+				rc = pmic_write_reg(REG_AUDIO_RX_0,
+						    reg_write, reg_mask);
+
+				if (rc == PMIC_SUCCESS) {
+					pr_debug("Output config set\n");
+					audioOutput.config |= config;
+
+				}
+			}
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Clear various audio output section options.
+ *
+ * This function clears one or more audio output section configuration
+ * options.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param   config              The desired audio output section
+ *                                  configuration options to be cleared.
+ *
+ * @retval      PMIC_SUCCESS         If the desired configuration options were
+ *                                   all successfully cleared.
+ * @retval      PMIC_PARAMETER_ERROR If the handle or configuration options
+ *                                   were invalid.
+ * @retval      PMIC_ERROR           If the desired configuration options
+ *                                   could not be cleared.
+ */
+PMIC_STATUS pmic_audio_output_clear_config(const PMIC_AUDIO_HANDLE handle,
+					   const PMIC_AUDIO_OUTPUT_CONFIG
+					   config)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	/*unsigned int reg_write_RX = 0;
+	   unsigned int reg_mask_RX  = 0;
+	   unsigned int reg_write_TX = 0;
+	   unsigned int reg_mask_TX  = 0; */
+	unsigned int reg_mask = 0;
+	unsigned int reg_write = 0;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if (((handle == stDAC.handle) &&
+	     (stDAC.handleState == HANDLE_IN_USE)) ||
+	    ((handle == vCodec.handle) &&
+	     (vCodec.handleState == HANDLE_IN_USE)) ||
+	    ((handle == extStereoIn.handle) &&
+	     (extStereoIn.handleState == HANDLE_IN_USE))) {
+		if (config & MONO_SPEAKER_INVERT_OUT_ONLY) {
+			/* If this is one of the parameters */
+			rc = PMIC_NOT_SUPPORTED;
+		} else {
+			if (config & MONO_LOUDSPEAKER_COMMON_BIAS) {
+				reg_mask = SET_BITS(regAUDIO_RX_0, ALSPREF, 1);
+				reg_write = SET_BITS(regAUDIO_RX_0, ALSPREF, 0);
+			}
+
+			if (config & HEADSET_DETECT_ENABLE) {
+				reg_mask |= SET_BITS(regAUDIO_RX_0, HSDETEN, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, HSDETEN, 0);
+			}
+
+			if (config & STEREO_HEADSET_AMP_AUTO_DISABLE) {
+				reg_mask |=
+				    SET_BITS(regAUDIO_RX_0, HSDETAUTOB, 1);
+				reg_write |=
+				    SET_BITS(regAUDIO_RX_0, HSDETAUTOB, 0);
+			}
+
+			if (reg_mask == 0) {
+				rc = PMIC_PARAMETER_ERROR;
+			} else {
+				rc = pmic_write_reg(REG_AUDIO_RX_0,
+						    reg_write, reg_mask);
+
+				if (rc == PMIC_SUCCESS) {
+					pr_debug("Output config cleared\n");
+					audioOutput.config &= ~config;
+
+				}
+			}
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Get the current audio output section options.
+ *
+ * This function retrieves the current audio output section configuration
+ * option settings.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ * @param  config              The current audio output section
+ *                                  configuration option settings.
+ *
+ * @retval      PMIC_SUCCESS         If the current configuration options were
+ *                                   successfully retrieved.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the current configuration options
+ *                                   could not be retrieved.
+ */
+PMIC_STATUS pmic_audio_output_get_config(const PMIC_AUDIO_HANDLE handle,
+					 PMIC_AUDIO_OUTPUT_CONFIG *
+					 const config)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Use a critical section to ensure a consistent hardware state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((((handle == stDAC.handle) &&
+	      (stDAC.handleState == HANDLE_IN_USE)) ||
+	     ((handle == vCodec.handle) &&
+	      (vCodec.handleState == HANDLE_IN_USE)) ||
+	     ((handle == extStereoIn.handle) &&
+	      (extStereoIn.handleState == HANDLE_IN_USE))) &&
+	    (config != (PMIC_AUDIO_OUTPUT_CONFIG *) NULL)) {
+		*config = audioOutput.config;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * @brief Enable the phantom ground circuit that is used to help identify
+ *        the type of headset that has been inserted.
+ *
+ * This function enables the phantom ground circuit that is used to help
+ * identify the type of headset (e.g., stereo or mono) that has been inserted.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the phantom ground circuit was
+ *                                   successfully enabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the phantom ground circuit could not
+ *                                   be enabled.
+ */
+PMIC_STATUS pmic_audio_output_enable_phantom_ground()
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	const unsigned int reg_mask = SET_BITS(regAUDIO_RX_0, HSPGDIS, 1);
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	rc = pmic_write_reg(REG_AUDIO_RX_0, 0, reg_mask);
+	if (rc == PMIC_SUCCESS) {
+		pr_debug("Phantom ground enabled\n");
+
+	}
+	return rc;
+}
+
+/*!
+ * @brief Disable the phantom ground circuit that is used to help identify
+ *        the type of headset that has been inserted.
+ *
+ * This function disables the phantom ground circuit that is used to help
+ * identify the type of headset (e.g., stereo or mono) that has been inserted.
+ *
+ * @param   handle              Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the phantom ground circuit was
+ *                                   successfully disabled.
+ * @retval      PMIC_PARAMETER_ERROR If the handle was invalid.
+ * @retval      PMIC_ERROR           If the phantom ground circuit could not
+ *                                   be disabled.
+ */
+PMIC_STATUS pmic_audio_output_disable_phantom_ground()
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	const unsigned int reg_mask = SET_BITS(regAUDIO_RX_0, HSPGDIS, 1);
+
+	/* No critical section required here since we are not updating any
+	 * global data.
+	 */
+
+	rc = pmic_write_reg(REG_AUDIO_RX_0, 1, reg_mask);
+	if (rc == PMIC_SUCCESS) {
+		pr_debug("Phantom ground disabled\n");
+
+	}
+	return rc;
+}
+
+/*@}*/
+
+/**************************************************************************
+ * Static functions.
+ **************************************************************************
+ */
+
+/*!
+ * @name Audio Driver Internal Support Functions
+ * These non-exported internal functions are used to support the functionality
+ * of the exported audio APIs.
+ */
+/*@{*/
+
+/*!
+ * @brief Enables the 5.6V boost for the microphone bias 2 circuit.
+ *
+ * This function enables the switching regulator SW3 and configures it to
+ * provide the 5.6V boost that is required for driving the microphone bias 2
+ * circuit when using a 5-pole jack configuration (which is the case for the
+ * Sphinx board).
+ *
+ * @retval      PMIC_SUCCESS         The 5.6V boost was successfully enabled.
+ * @retval      PMIC_ERROR           Failed to enable the 5.6V boost.
+ */
+/*
+static PMIC_STATUS pmic_audio_mic_boost_enable(void)
+{
+	PMIC_STATUS rc = PMIC_NOT_SUPPORTED;
+
+	return rc;
+}
+*/
+/*!
+ * @brief Disables the 5.6V boost for the microphone bias 2 circuit.
+ *
+ * This function disables the switching regulator SW3 to turn off the 5.6V
+ * boost for the microphone bias 2 circuit.
+ *
+ * @retval      PMIC_SUCCESS         The 5.6V boost was successfully disabled.
+ * @retval      PMIC_ERROR           Failed to disable the 5.6V boost.
+ */
+/*
+static PMIC_STATUS pmic_audio_mic_boost_disable(void)
+{
+	PMIC_STATUS rc = PMIC_NOT_SUPPORTED;
+
+	return rc;
+}
+*/
+
+/*!
+ * @brief Free a device handle previously acquired by calling pmic_audio_open().
+ *
+ * Terminate further access to the PMIC audio hardware that was previously
+ * acquired by calling pmic_audio_open(). This now allows another thread to
+ * successfully call pmic_audio_open() to gain access.
+ *
+ * Note that we will shutdown/reset the Voice CODEC or Stereo DAC as well as
+ * any associated audio input/output components that are no longer required.
+ *
+ * Also note that this function should only be called with the mutex already
+ * acquired.
+ *
+ * @param   handle          Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the close request was successful.
+ * @retval      PMIC_PARAMETER_ERROR If the handle is invalid.
+ */
+static PMIC_STATUS pmic_audio_close_handle(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+
+	/* Match up the handle to the audio device and then close it. */
+	if ((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE)) {
+		/* Also shutdown the Stereo DAC hardware. The simplest way to
+		 * do this is to simply call pmic_audio_reset_device() which will
+		 * restore the ST_DAC register to it's initial power-on state.
+		 *
+		 * This will also shutdown the audio output section if no one
+		 * else is still using it.
+		 */
+		rc = pmic_audio_reset_device(stDAC.handle);
+
+		if (rc == PMIC_SUCCESS) {
+			stDAC.handle = AUDIO_HANDLE_NULL;
+			stDAC.handleState = HANDLE_FREE;
+		}
+	} else if ((handle == vCodec.handle) &&
+		   (vCodec.handleState == HANDLE_IN_USE)) {
+		/* Also shutdown the Voice CODEC and audio input hardware. The
+		 * simplest way to do this is to simply call pmic_audio_reset_device()
+		 * which will restore the AUD_CODEC register to it's initial
+		 * power-on state.
+		 *
+		 * This will also shutdown the audio output section if no one
+		 * else is still using it.
+		 */
+		rc = pmic_audio_reset_device(vCodec.handle);
+		if (rc == PMIC_SUCCESS) {
+			vCodec.handle = AUDIO_HANDLE_NULL;
+			vCodec.handleState = HANDLE_FREE;
+		}
+	} else if ((handle == extStereoIn.handle) &&
+		   (extStereoIn.handleState == HANDLE_IN_USE)) {
+
+		/* Call pmic_audio_reset_device() here to shutdown the audio output
+		 * section if no one else is still using it.
+		 */
+		rc = pmic_audio_reset_device(extStereoIn.handle);
+
+		if (rc == PMIC_SUCCESS) {
+			extStereoIn.handle = AUDIO_HANDLE_NULL;
+			extStereoIn.handleState = HANDLE_FREE;
+		}
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief Reset the selected audio hardware control registers to their
+ *        power on state.
+ *
+ * This resets all of the audio hardware control registers currently
+ * associated with the device handle back to their power on states. For
+ * example, if the handle is associated with the Stereo DAC and a
+ * specific output port and output amplifiers, then this function will
+ * reset all of those components to their initial power on state.
+ *
+ * This function can only be called if the mutex has already been acquired.
+ *
+ * @param   handle          Device handle from pmic_audio_open() call.
+ *
+ * @retval      PMIC_SUCCESS         If the reset operation was successful.
+ * @retval      PMIC_PARAMETER_ERROR If the handle is invalid.
+ * @retval      PMIC_ERROR           If the reset was unsuccessful.
+ */
+static PMIC_STATUS pmic_audio_reset_device(const PMIC_AUDIO_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	unsigned int reg_mask = 0;
+
+	if ((handle == stDAC.handle) && (stDAC.handleState == HANDLE_IN_USE)) {
+		/* Also shutdown the audio output section if nobody else is using it.
+		   if ((vCodec.handleState == HANDLE_FREE) &&
+		   (extStereoIn.handleState == HANDLE_FREE))
+		   {
+		   pmic_write_reg(REG_RX_AUD_AMPS, RESET_RX_AUD_AMPS,
+		   REG_FULLMASK);
+		   } */
+
+		rc = pmic_write_reg(REG_AUDIO_STEREO_DAC,
+				    RESET_ST_DAC, REG_FULLMASK);
+
+		if (rc == PMIC_SUCCESS) {
+			rc = pmic_write_reg(REG_AUDIO_SSI_NETWORK,
+					    RESET_SSI_NETWORK,
+					    REG_SSI_STDAC_MASK);
+			if (rc == PMIC_SUCCESS) {
+				/* Also reset the driver state information to match. Note that we
+				 * keep the device handle and event callback settings unchanged
+				 * since these don't affect the actual hardware and we rely on
+				 * the user to explicitly close the handle or deregister callbacks
+				 */
+				stDAC.busID = AUDIO_DATA_BUS_1;
+				stDAC.protocol = NORMAL_MSB_JUSTIFIED_MODE;
+				stDAC.protocol_set = false;
+				stDAC.masterSlave = BUS_MASTER_MODE;
+				stDAC.numSlots = USE_2_TIMESLOTS;
+				stDAC.clockIn = CLOCK_IN_CLIA;
+				stDAC.samplingRate = STDAC_RATE_44_1_KHZ;
+				stDAC.clockFreq = STDAC_CLI_13MHZ;
+				stDAC.invert = NO_INVERT;
+				stDAC.timeslot = USE_TS0_TS1;
+				stDAC.config = (PMIC_AUDIO_STDAC_CONFIG) 0;
+
+			}
+		}
+	} else if ((handle == vCodec.handle)
+		   && (vCodec.handleState == HANDLE_IN_USE)) {
+		/* Disable the audio input section when disabling the Voice CODEC. */
+		pmic_write_reg(REG_AUDIO_TX, RESET_AUDIO_TX, REG_FULLMASK);
+
+		rc = pmic_write_reg(REG_AUDIO_CODEC,
+				    RESET_AUD_CODEC, REG_FULLMASK);
+
+		if (rc == PMIC_SUCCESS) {
+			rc = pmic_write_reg(REG_AUDIO_SSI_NETWORK,
+					    RESET_SSI_NETWORK,
+					    REG_SSI_VCODEC_MASK);
+			if (rc == PMIC_SUCCESS) {
+
+				/* Also reset the driver state information to match. Note that we
+				 * keep the device handle and event callback settings unchanged
+				 * since these don't affect the actual hardware and we rely on
+				 * the user to explicitly close the handle or deregister callbacks
+				 */
+				vCodec.busID = AUDIO_DATA_BUS_2;
+				vCodec.protocol = NETWORK_MODE;
+				vCodec.protocol_set = false;
+				vCodec.masterSlave = BUS_SLAVE_MODE;
+				vCodec.numSlots = USE_4_TIMESLOTS;
+				vCodec.clockIn = CLOCK_IN_CLIB;
+				vCodec.samplingRate = VCODEC_RATE_8_KHZ;
+				vCodec.clockFreq = VCODEC_CLI_13MHZ;
+				vCodec.invert = NO_INVERT;
+				vCodec.timeslot = USE_TS0;
+				vCodec.config =
+				    INPUT_HIGHPASS_FILTER |
+				    OUTPUT_HIGHPASS_FILTER;
+
+			}
+		}
+
+	} else if ((handle == extStereoIn.handle) &&
+		   (extStereoIn.handleState == HANDLE_IN_USE)) {
+		/* Disable the Ext stereo Amplifier and disable it as analog mixer input */
+		reg_mask = SET_BITS(regAUDIO_RX_1, ARXINEN, 1);
+		pmic_write_reg(REG_AUDIO_RX_1, 0, reg_mask);
+
+		reg_mask = SET_BITS(regAUDIO_RX_0, ADDRXIN, 1);
+		pmic_write_reg(REG_AUDIO_RX_0, 0, reg_mask);
+
+		/* We don't need to reset any other registers for this case. */
+		rc = PMIC_SUCCESS;
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief Deregister the callback function and event mask currently associated
+ *        with an audio device handle.
+ *
+ * This function deregisters any existing callback function and event mask for
+ * the given audio device handle. This is done by either calling the
+ * pmic_audio_clear_callback() API or by closing the device handle.
+ *
+ * Note that this function should only be called with the mutex already
+ * acquired. We will also acquire the spinlock here to prevent possible
+ * race conditions with the interrupt handler.
+ *
+ * @param[in]   callback            The current event callback function pointer.
+ * @param[in]   eventMask           The current audio event mask.
+ *
+ * @retval      PMIC_SUCCESS         If the callback function and event mask
+ *                                   were both successfully deregistered.
+ * @retval      PMIC_ERROR           If either the callback function or the
+ *                                   event mask was not successfully
+ *                                   deregistered.
+ */
+
+static PMIC_STATUS pmic_audio_deregister(void *callback,
+					 PMIC_AUDIO_EVENTS * const eventMask)
+{
+	unsigned long flags;
+	pmic_event_callback_t eventNotify;
+	PMIC_STATUS rc = PMIC_SUCCESS;
+
+	/* Deregister each of the PMIC events that we had previously
+	 * registered for by calling pmic_event_subscribe().
+	 */
+	if (*eventMask & (HEADSET_DETECTED)) {
+		/* We need to deregister for the A1 amplifier interrupt. */
+		eventNotify.func = callback;
+		eventNotify.param = (void *)(CORE_EVENT_HSDETI);
+		if (pmic_event_unsubscribe(EVENT_HSDETI, eventNotify) ==
+		    PMIC_SUCCESS) {
+			*eventMask &= ~(HEADSET_DETECTED);
+			pr_debug("Deregistered for EVENT_HSDETI\n");
+		} else {
+			rc = PMIC_ERROR;
+		}
+	}
+
+	if (*eventMask & (HEADSET_STEREO)) {
+		/* We need to deregister for the A1 amplifier interrupt. */
+		eventNotify.func = callback;
+		eventNotify.param = (void *)(CORE_EVENT_HSLI);
+		if (pmic_event_unsubscribe(EVENT_HSLI, eventNotify) ==
+		    PMIC_SUCCESS) {
+			*eventMask &= ~(HEADSET_STEREO);
+			pr_debug("Deregistered for EVENT_HSLI\n");
+		} else {
+			rc = PMIC_ERROR;
+		}
+	}
+	if (*eventMask & (HEADSET_THERMAL_SHUTDOWN)) {
+		/* We need to deregister for the A1 amplifier interrupt. */
+		eventNotify.func = callback;
+		eventNotify.param = (void *)(CORE_EVENT_ALSPTHI);
+		if (pmic_event_unsubscribe(EVENT_ALSPTHI, eventNotify) ==
+		    PMIC_SUCCESS) {
+			*eventMask &= ~(HEADSET_THERMAL_SHUTDOWN);
+			pr_debug("Deregistered for EVENT_ALSPTHI\n");
+		} else {
+			rc = PMIC_ERROR;
+		}
+	}
+	if (*eventMask & (HEADSET_SHORT_CIRCUIT)) {
+		/* We need to deregister for the A1 amplifier interrupt. */
+		eventNotify.func = callback;
+		eventNotify.param = (void *)(CORE_EVENT_AHSSHORTI);
+		if (pmic_event_unsubscribe(EVENT_AHSSHORTI, eventNotify) ==
+		    PMIC_SUCCESS) {
+			*eventMask &= ~(HEADSET_SHORT_CIRCUIT);
+			pr_debug("Deregistered for EVENT_AHSSHORTI\n");
+		} else {
+			rc = PMIC_ERROR;
+		}
+	}
+
+	if (rc == PMIC_SUCCESS) {
+		/* We need to grab the spinlock here to create a critical section to
+		 * avoid any possible race conditions with the interrupt handler
+		 */
+		spin_lock_irqsave(&lock, flags);
+
+		/* Restore the initial reset values for the callback function
+		 * and event mask parameters. This should be NULL and zero,
+		 * respectively.
+		 */
+		callback = NULL;
+		*eventMask = 0;
+
+		/* Exit the critical section. */
+		spin_unlock_irqrestore(&lock, flags);
+	}
+
+	return rc;
+}
+
+/*!
+ * @brief enable/disable fm output.
+ *
+ * @param[in]   enable            true to enable false to disable
+ */
+PMIC_STATUS pmic_audio_fm_output_enable(bool enable)
+{
+	unsigned int reg_mask = 0;
+	unsigned int reg_write = 0;
+	PMIC_STATUS rc = PMIC_PARAMETER_ERROR;
+	if (enable) {
+		pmic_audio_antipop_enable(ANTI_POP_RAMP_FAST);
+		reg_mask |= SET_BITS(regAUDIO_RX_0, AHSLEN, 1);
+		reg_write |= SET_BITS(regAUDIO_RX_0, AHSLEN, 1);
+		reg_mask |= SET_BITS(regAUDIO_RX_0, AHSREN, 1);
+		reg_write |= SET_BITS(regAUDIO_RX_0, AHSREN, 1);
+
+		reg_mask |= SET_BITS(regAUDIO_RX_0, AHSSEL, 1);
+		reg_write |= SET_BITS(regAUDIO_RX_0, AHSSEL, 1);
+
+		reg_mask |= SET_BITS(regAUDIO_RX_0, ADDRXIN, 1);
+		reg_write |= SET_BITS(regAUDIO_RX_0, ADDRXIN, 1);
+
+		reg_mask |= SET_BITS(regAUDIO_RX_0, HSPGDIS, 1);
+		reg_write |= SET_BITS(regAUDIO_RX_0, HSPGDIS, 0);
+	} else {
+		reg_mask |= SET_BITS(regAUDIO_RX_0, ADDRXIN, 1);
+		reg_write |= SET_BITS(regAUDIO_RX_0, ADDRXIN, 0);
+	}
+	rc = pmic_write_reg(REG_AUDIO_RX_0, reg_write, reg_mask);
+	if (rc != PMIC_SUCCESS)
+		return rc;
+	if (enable) {
+		reg_mask = SET_BITS(regAUDIO_RX_1, ARXINEN, 1);
+		reg_write = SET_BITS(regAUDIO_RX_1, ARXINEN, 1);
+	} else {
+		reg_mask = SET_BITS(regAUDIO_RX_1, ARXINEN, 1);
+		reg_write = SET_BITS(regAUDIO_RX_1, ARXINEN, 0);
+	}
+	rc = pmic_write_reg(REG_AUDIO_RX_1, reg_write, reg_mask);
+	return rc;
+}
+
+/*@}*/
+
+/**************************************************************************
+ * Module initialization and termination functions.
+ *
+ * Note that if this code is compiled into the kernel, then the
+ * module_init() function will be called within the device_initcall()
+ * group.
+ **************************************************************************
+ */
+
+/*!
+ * @name Audio Driver Loading/Unloading Functions
+ * These non-exported internal functions are used to support the audio
+ * device driver initialization and de-initialization operations.
+ */
+/*@{*/
+
+/*!
+ * @brief This is the audio device driver initialization function.
+ *
+ * This function is called by the kernel when this device driver is first
+ * loaded.
+ */
+static int __init mc13783_pmic_audio_init(void)
+{
+	printk(KERN_INFO "PMIC Audio driver loading...\n");
+
+	return 0;
+}
+
+/*!
+ * @brief This is the audio device driver de-initialization function.
+ *
+ * This function is called by the kernel when this device driver is about
+ * to be unloaded.
+ */
+static void __exit mc13783_pmic_audio_exit(void)
+{
+	printk(KERN_INFO "PMIC Audio driver unloading...\n");
+
+	/* Close all device handles that are still open. This will also
+	 * deregister any callbacks that may still be active.
+	 */
+	if (stDAC.handleState == HANDLE_IN_USE) {
+		pmic_audio_close(stDAC.handle);
+	}
+	if (vCodec.handleState == HANDLE_IN_USE) {
+		pmic_audio_close(vCodec.handle);
+	}
+	if (extStereoIn.handleState == HANDLE_IN_USE) {
+		pmic_audio_close(extStereoIn.handle);
+	}
+
+	/* Explicitly reset all of the audio registers so that there is no
+	 * possibility of leaving the  audio hardware in a state
+	 * where it can cause problems if there is no device driver loaded.
+	 */
+	pmic_write_reg(REG_AUDIO_STEREO_DAC, RESET_ST_DAC, REG_FULLMASK);
+	pmic_write_reg(REG_AUDIO_CODEC, RESET_AUD_CODEC, REG_FULLMASK);
+	pmic_write_reg(REG_AUDIO_TX, RESET_AUDIO_TX, REG_FULLMASK);
+	pmic_write_reg(REG_AUDIO_SSI_NETWORK, RESET_SSI_NETWORK, REG_FULLMASK);
+	pmic_write_reg(REG_AUDIO_RX_0, RESET_AUDIO_RX_0, REG_FULLMASK);
+	pmic_write_reg(REG_AUDIO_RX_1, RESET_AUDIO_RX_1, REG_FULLMASK);
+}
+
+/*@}*/
+
+/*
+ * Module entry points and description information.
+ */
+
+module_init(mc13783_pmic_audio_init);
+module_exit(mc13783_pmic_audio_exit);
+
+MODULE_DESCRIPTION("PMIC - mc13783 ADC driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/mc13783/pmic_battery.c b/drivers/mxc/pmic/mc13783/pmic_battery.c
new file mode 100644
index 000000000000..775ebe77a5c0
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_battery.c
@@ -0,0 +1,1221 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_battery.c
+ * @brief This is the main file of PMIC(mc13783) Battery driver.
+ *
+ * @ingroup PMIC_BATTERY
+ */
+
+/*
+ * Includes
+ */
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+
+#include <linux/pmic_battery.h>
+#include <linux/pmic_adc.h>
+#include <linux/pmic_status.h>
+
+#include "pmic_battery_defs.h"
+
+#include <mach/pmic_power.h>
+#ifdef CONFIG_MXC_HWEVENT
+#include <mach/hw_events.h>
+#endif
+
+static int pmic_battery_major;
+
+/*!
+ * Number of users waiting in suspendq
+ */
+static int swait = 0;
+
+/*!
+ * To indicate whether any of the battery devices are suspending
+ */
+static int suspend_flag = 0;
+
+/*!
+ * The suspendq is used to block application calls
+ */
+static wait_queue_head_t suspendq;
+
+static struct class *pmic_battery_class;
+
+/* EXPORTED FUNCTIONS */
+EXPORT_SYMBOL(pmic_batt_enable_charger);
+EXPORT_SYMBOL(pmic_batt_disable_charger);
+EXPORT_SYMBOL(pmic_batt_set_charger);
+EXPORT_SYMBOL(pmic_batt_get_charger_setting);
+EXPORT_SYMBOL(pmic_batt_get_charge_current);
+EXPORT_SYMBOL(pmic_batt_enable_eol);
+EXPORT_SYMBOL(pmic_batt_bp_enable_eol);
+EXPORT_SYMBOL(pmic_batt_disable_eol);
+EXPORT_SYMBOL(pmic_batt_set_out_control);
+EXPORT_SYMBOL(pmic_batt_set_threshold);
+EXPORT_SYMBOL(pmic_batt_led_control);
+EXPORT_SYMBOL(pmic_batt_set_reverse_supply);
+EXPORT_SYMBOL(pmic_batt_set_unregulated);
+EXPORT_SYMBOL(pmic_batt_set_5k_pull);
+EXPORT_SYMBOL(pmic_batt_event_subscribe);
+EXPORT_SYMBOL(pmic_batt_event_unsubscribe);
+
+static DECLARE_MUTEX(count_mutex);	/* open count mutex */
+static int open_count;		/* open count for device file */
+
+/*!
+ * Callback function for events, we want on MGN board
+ */
+static void callback_chg_detect(void)
+{
+#ifdef CONFIG_MXC_HWEVENT
+	t_sensor_bits sensor;
+	struct mxc_hw_event event = { HWE_BAT_CHARGER_PLUG, 0 };
+
+	pr_debug("In callback_chg_detect\n");
+
+	/* get sensor values */
+	pmic_get_sensors(&sensor);
+
+	pr_debug("Callback, charger detect:%d\n", sensor.sense_chgdets);
+
+	if (sensor.sense_chgdets)
+		event.args = 1;
+	else
+		event.args = 0;
+	/* send hardware event */
+	hw_event_send(HWE_DEF_PRIORITY, &event);
+#endif
+}
+
+static void callback_low_battery(void)
+{
+#ifdef CONFIG_MXC_HWEVENT
+	struct mxc_hw_event event = { HWE_BAT_BATTERY_LOW, 0 };
+
+	pr_debug("In callback_low_battery\n");
+	/* send hardware event */
+	hw_event_send(HWE_DEF_PRIORITY, &event);
+#endif
+}
+
+static void callback_power_fail(void)
+{
+#ifdef CONFIG_MXC_HWEVENT
+	struct mxc_hw_event event = { HWE_BAT_POWER_FAILED, 0 };
+
+	pr_debug("In callback_power_fail\n");
+	/* send hardware event */
+	hw_event_send(HWE_DEF_PRIORITY, &event);
+#endif
+}
+
+static void callback_chg_overvoltage(void)
+{
+#ifdef CONFIG_MXC_HWEVENT
+	struct mxc_hw_event event = { HWE_BAT_CHARGER_OVERVOLTAGE, 0 };
+
+	pr_debug("In callback_chg_overvoltage\n");
+	/* send hardware event */
+	hw_event_send(HWE_DEF_PRIORITY, &event);
+#endif
+}
+
+static void callback_chg_full(void)
+{
+#ifdef CONFIG_MXC_HWEVENT
+	t_sensor_bits sensor;
+	struct mxc_hw_event event = { HWE_BAT_CHARGER_FULL, 0 };
+
+	pr_debug("In callback_chg_full\n");
+
+	/* disable charge function */
+	pmic_batt_disable_charger(BATT_MAIN_CHGR);
+
+	/* get charger sensor */
+	pmic_get_sensors(&sensor);
+
+	/* if did not detect the charger */
+	if (sensor.sense_chgdets)
+		return;
+	/* send hardware event */
+	hw_event_send(HWE_DEF_PRIORITY, &event);
+#endif
+}
+
+/*!
+ * This is the suspend of power management for the pmic battery API.
+ * It suports SAVE and POWER_DOWN state.
+ *
+ * @param        pdev           the device
+ * @param        state          the state
+ *
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_battery_suspend(struct platform_device *pdev,
+				pm_message_t state)
+{
+	unsigned int reg_value = 0;
+
+	suspend_flag = 1;
+	CHECK_ERROR(pmic_write_reg(REG_CHARGER, reg_value, PMIC_ALL_BITS));
+
+	return 0;
+};
+
+/*!
+ * This is the resume of power management for the pmic battery API.
+ * It suports RESTORE state.
+ *
+ * @param        pdev           the device
+ *
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_battery_resume(struct platform_device *pdev)
+{
+	suspend_flag = 0;
+	while (swait > 0) {
+		swait--;
+		wake_up_interruptible(&suspendq);
+	}
+
+	return 0;
+};
+
+/*!
+ * This function is used to start charging a battery. For different charger,
+ * different voltage and current range are supported. \n
+ *
+ *
+ * @param      chgr        Charger as defined in \b t_batt_charger.
+ * @param      c_voltage   Charging voltage.
+ * @param      c_current   Charging current.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_enable_charger(t_batt_charger chgr,
+				     unsigned char c_voltage,
+				     unsigned char c_current)
+{
+	unsigned int val, mask, reg;
+
+	val = 0;
+	mask = 0;
+	reg = 0;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	switch (chgr) {
+	case BATT_MAIN_CHGR:
+		val = BITFVAL(MC13783_BATT_DAC_DAC, c_current) |
+		    BITFVAL(MC13783_BATT_DAC_V_DAC, c_voltage);
+		mask = BITFMASK(MC13783_BATT_DAC_DAC) |
+		    BITFMASK(MC13783_BATT_DAC_V_DAC);
+		reg = REG_CHARGER;
+		break;
+
+	case BATT_CELL_CHGR:
+		val = BITFVAL(MC13783_BATT_DAC_V_COIN, c_voltage) |
+		    BITFVAL(MC13783_BATT_DAC_COIN_CH_EN,
+			    MC13783_BATT_DAC_COIN_CH_EN_ENABLED);
+		mask = BITFMASK(MC13783_BATT_DAC_V_COIN) |
+		    BITFMASK(MC13783_BATT_DAC_COIN_CH_EN);
+		reg = REG_POWER_CONTROL_0;
+		break;
+
+	case BATT_TRCKLE_CHGR:
+		val = BITFVAL(MC13783_BATT_DAC_TRCKLE, c_current);
+		mask = BITFMASK(MC13783_BATT_DAC_TRCKLE);
+		reg = REG_CHARGER;
+		break;
+
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, val, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function turns off a charger.
+ *
+ * @param      chgr        Charger as defined in \b t_batt_charger.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_disable_charger(t_batt_charger chgr)
+{
+	unsigned int val, mask, reg;
+
+	val = 0;
+	mask = 0;
+	reg = 0;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+	switch (chgr) {
+	case BATT_MAIN_CHGR:
+		val = BITFVAL(MC13783_BATT_DAC_DAC, 0) |
+		    BITFVAL(MC13783_BATT_DAC_V_DAC, 0);
+		mask = BITFMASK(MC13783_BATT_DAC_DAC) |
+		    BITFMASK(MC13783_BATT_DAC_V_DAC);
+		reg = REG_CHARGER;
+		break;
+
+	case BATT_CELL_CHGR:
+		val = BITFVAL(MC13783_BATT_DAC_COIN_CH_EN,
+			      MC13783_BATT_DAC_COIN_CH_EN_DISABLED);
+		mask = BITFMASK(MC13783_BATT_DAC_COIN_CH_EN);
+		reg = REG_POWER_CONTROL_0;
+		break;
+
+	case BATT_TRCKLE_CHGR:
+		val = BITFVAL(MC13783_BATT_DAC_TRCKLE, 0);
+		mask = BITFMASK(MC13783_BATT_DAC_TRCKLE);
+		reg = REG_CHARGER;
+		break;
+
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, val, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is used to change the charger setting.
+ *
+ * @param      chgr        Charger as defined in \b t_batt_charger.
+ * @param      c_voltage   Charging voltage.
+ * @param      c_current   Charging current.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_set_charger(t_batt_charger chgr,
+				  unsigned char c_voltage,
+				  unsigned char c_current)
+{
+	unsigned int val, mask, reg;
+
+	val = 0;
+	mask = 0;
+	reg = 0;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	switch (chgr) {
+	case BATT_MAIN_CHGR:
+		val = BITFVAL(MC13783_BATT_DAC_DAC, c_current) |
+		    BITFVAL(MC13783_BATT_DAC_V_DAC, c_voltage);
+		mask = BITFMASK(MC13783_BATT_DAC_DAC) |
+		    BITFMASK(MC13783_BATT_DAC_V_DAC);
+		reg = REG_CHARGER;
+		break;
+
+	case BATT_CELL_CHGR:
+		val = BITFVAL(MC13783_BATT_DAC_V_COIN, c_voltage);
+		mask = BITFMASK(MC13783_BATT_DAC_V_COIN);
+		reg = REG_POWER_CONTROL_0;
+		break;
+
+	case BATT_TRCKLE_CHGR:
+		val = BITFVAL(MC13783_BATT_DAC_TRCKLE, c_current);
+		mask = BITFMASK(MC13783_BATT_DAC_TRCKLE);
+		reg = REG_CHARGER;
+		break;
+
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, val, mask));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is used to retrive the charger setting.
+ *
+ * @param      chgr        Charger as defined in \b t_batt_charger.
+ * @param      c_voltage   Output parameter for charging voltage setting.
+ * @param      c_current   Output parameter for charging current setting.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_get_charger_setting(t_batt_charger chgr,
+					  unsigned char *c_voltage,
+					  unsigned char *c_current)
+{
+	unsigned int val, reg;
+
+	reg = 0;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	switch (chgr) {
+	case BATT_MAIN_CHGR:
+	case BATT_TRCKLE_CHGR:
+		reg = REG_CHARGER;
+		break;
+	case BATT_CELL_CHGR:
+		reg = REG_POWER_CONTROL_0;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &val, PMIC_ALL_BITS));
+
+	switch (chgr) {
+	case BATT_MAIN_CHGR:
+		*c_voltage = BITFEXT(val, MC13783_BATT_DAC_V_DAC);;
+		*c_current = BITFEXT(val, MC13783_BATT_DAC_DAC);
+		break;
+
+	case BATT_CELL_CHGR:
+		*c_voltage = BITFEXT(val, MC13783_BATT_DAC_V_COIN);
+		*c_current = 0;
+		break;
+
+	case BATT_TRCKLE_CHGR:
+		*c_voltage = 0;
+		*c_current = BITFEXT(val, MC13783_BATT_DAC_TRCKLE);
+		break;
+
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is retrives the main battery voltage.
+ *
+ * @param      b_voltage   Output parameter for voltage setting.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_get_batt_voltage(unsigned short *b_voltage)
+{
+	t_channel channel;
+	unsigned short result[8];
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+	channel = BATTERY_VOLTAGE;
+	CHECK_ERROR(pmic_adc_convert(channel, result));
+	*b_voltage = result[0];
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is retrives the main battery current.
+ *
+ * @param      b_current   Output parameter for current setting.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_get_batt_current(unsigned short *b_current)
+{
+	t_channel channel;
+	unsigned short result[8];
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	channel = BATTERY_CURRENT;
+	CHECK_ERROR(pmic_adc_convert(channel, result));
+	*b_current = result[0];
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is retrives the main battery temperature.
+ *
+ * @param      b_temper   Output parameter for temperature setting.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_get_batt_temperature(unsigned short *b_temper)
+{
+	t_channel channel;
+	unsigned short result[8];
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	channel = GEN_PURPOSE_AD5;
+	CHECK_ERROR(pmic_adc_convert(channel, result));
+	*b_temper = result[0];
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is retrives the main battery charging voltage.
+ *
+ * @param      c_voltage   Output parameter for charging voltage setting.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_get_charge_voltage(unsigned short *c_voltage)
+{
+	t_channel channel;
+	unsigned short result[8];
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	channel = CHARGE_VOLTAGE;
+	CHECK_ERROR(pmic_adc_convert(channel, result));
+	*c_voltage = result[0];
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is retrives the main battery charging current.
+ *
+ * @param      c_current   Output parameter for charging current setting.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_get_charge_current(unsigned short *c_current)
+{
+	t_channel channel;
+	unsigned short result[8];
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	channel = CHARGE_CURRENT;
+	CHECK_ERROR(pmic_adc_convert(channel, result));
+	*c_current = result[0];
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function enables End-of-Life comparator. Not supported on
+ * mc13783. Use pmic_batt_bp_enable_eol function.
+ *
+ * @param      threshold  End-of-Life threshold.
+ *
+ * @return     This function returns PMIC_UNSUPPORTED
+ */
+PMIC_STATUS pmic_batt_enable_eol(unsigned char threshold)
+{
+	return PMIC_NOT_SUPPORTED;
+}
+
+/*!
+ * This function enables End-of-Life comparator.
+ *
+ * @param      typical  Falling Edge Threshold threshold.
+ * 			@verbatim
+			BPDET	UVDET	LOBATL
+			____	_____   ___________
+			0	2.6	UVDET + 0.2
+			1	2.6	UVDET + 0.3
+			2	2.6	UVDET + 0.4
+			3	2.6	UVDET + 0.5
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_bp_enable_eol(t_bp_threshold typical)
+{
+	unsigned int val, mask;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	val = BITFVAL(MC13783_BATT_DAC_EOL_CMP_EN,
+		      MC13783_BATT_DAC_EOL_CMP_EN_ENABLE) |
+	    BITFVAL(MC13783_BATT_DAC_EOL_SEL, typical);
+	mask = BITFMASK(MC13783_BATT_DAC_EOL_CMP_EN) |
+	    BITFMASK(MC13783_BATT_DAC_EOL_SEL);
+
+	CHECK_ERROR(pmic_write_reg(REG_POWER_CONTROL_0, val, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function disables End-of-Life comparator.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_disable_eol(void)
+{
+	unsigned int val, mask;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	val = BITFVAL(MC13783_BATT_DAC_EOL_CMP_EN,
+		      MC13783_BATT_DAC_EOL_CMP_EN_DISABLE);
+	mask = BITFMASK(MC13783_BATT_DAC_EOL_CMP_EN);
+
+	CHECK_ERROR(pmic_write_reg(REG_POWER_CONTROL_0, val, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the output controls.
+ * It sets the FETOVRD and FETCTRL bits of mc13783
+ *
+ * @param        control        type of control.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_batt_set_out_control(t_control control)
+{
+	unsigned int val, mask;
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	switch (control) {
+	case CONTROL_HARDWARE:
+		val = BITFVAL(MC13783_BATT_DAC_FETOVRD_EN, 0) |
+		    BITFVAL(MC13783_BATT_DAC_FETCTRL_EN, 0);
+		mask = BITFMASK(MC13783_BATT_DAC_FETOVRD_EN) |
+		    BITFMASK(MC13783_BATT_DAC_FETCTRL_EN);
+		break;
+	case CONTROL_BPFET_LOW:
+		val = BITFVAL(MC13783_BATT_DAC_FETOVRD_EN, 1) |
+		    BITFVAL(MC13783_BATT_DAC_FETCTRL_EN, 0);
+		mask = BITFMASK(MC13783_BATT_DAC_FETOVRD_EN) |
+		    BITFMASK(MC13783_BATT_DAC_FETCTRL_EN);
+		break;
+	case CONTROL_BPFET_HIGH:
+		val = BITFVAL(MC13783_BATT_DAC_FETOVRD_EN, 1) |
+		    BITFVAL(MC13783_BATT_DAC_FETCTRL_EN, 1);
+		mask = BITFMASK(MC13783_BATT_DAC_FETOVRD_EN) |
+		    BITFMASK(MC13783_BATT_DAC_FETCTRL_EN);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(REG_CHARGER, val, mask));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets over voltage threshold.
+ *
+ * @param        threshold      value of over voltage threshold.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_batt_set_threshold(int threshold)
+{
+	unsigned int val, mask;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	if (threshold > BAT_THRESHOLD_MAX)
+		return PMIC_PARAMETER_ERROR;
+
+	val = BITFVAL(MC13783_BATT_DAC_OVCTRL, threshold);
+	mask = BITFMASK(MC13783_BATT_DAC_OVCTRL);
+	CHECK_ERROR(pmic_write_reg(REG_CHARGER, val, mask));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function controls charge LED.
+ *
+ * @param      on   If on is ture, LED will be turned on,
+ *                  or otherwise, LED will be turned off.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_led_control(bool on)
+{
+	unsigned val, mask;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	val = BITFVAL(MC13783_BATT_DAC_LED_EN, on);
+	mask = BITFMASK(MC13783_BATT_DAC_LED_EN);
+
+	CHECK_ERROR(pmic_write_reg(REG_CHARGER, val, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets reverse supply mode.
+ *
+ * @param      enable     If enable is ture, reverse supply mode is enable,
+ *                        or otherwise, reverse supply mode is disabled.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_set_reverse_supply(bool enable)
+{
+	unsigned val, mask;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	val = BITFVAL(MC13783_BATT_DAC_REVERSE_SUPPLY, enable);
+	mask = BITFMASK(MC13783_BATT_DAC_REVERSE_SUPPLY);
+
+	CHECK_ERROR(pmic_write_reg(REG_CHARGER, val, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets unregulatored charging mode on main battery.
+ *
+ * @param      enable     If enable is ture, unregulated charging mode is
+ *                        enable, or otherwise, disabled.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_set_unregulated(bool enable)
+{
+	unsigned val, mask;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	val = BITFVAL(MC13783_BATT_DAC_UNREGULATED, enable);
+	mask = BITFMASK(MC13783_BATT_DAC_UNREGULATED);
+
+	CHECK_ERROR(pmic_write_reg(REG_CHARGER, val, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets a 5K pull down at CHRGRAW.
+ * To be used in the dual path charging configuration.
+ *
+ * @param      enable     If enable is true, 5k pull down is
+ *                        enable, or otherwise, disabled.
+ *
+ * @return     This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_batt_set_5k_pull(bool enable)
+{
+	unsigned val, mask;
+
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	val = BITFVAL(MC13783_BATT_DAC_5K, enable);
+	mask = BITFMASK(MC13783_BATT_DAC_5K);
+
+	CHECK_ERROR(pmic_write_reg(REG_CHARGER, val, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is used to un/subscribe on battery event IT.
+ *
+ * @param        event          type of event.
+ * @param        callback       event callback function.
+ * @param        sub            define if Un/subscribe event.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS mc13783_battery_event(t_batt_event event, void *callback, bool sub)
+{
+	pmic_event_callback_t bat_callback;
+	type_event bat_event;
+
+	bat_callback.func = callback;
+	bat_callback.param = NULL;
+	switch (event) {
+	case BAT_IT_CHG_DET:
+		bat_event = EVENT_CHGDETI;
+		break;
+	case BAT_IT_CHG_OVERVOLT:
+		bat_event = EVENT_CHGOVI;
+		break;
+	case BAT_IT_CHG_REVERSE:
+		bat_event = EVENT_CHGREVI;
+		break;
+	case BAT_IT_CHG_SHORT_CIRCUIT:
+		bat_event = EVENT_CHGSHORTI;
+		break;
+	case BAT_IT_CCCV:
+		bat_event = EVENT_CCCVI;
+		break;
+	case BAT_IT_BELOW_THRESHOLD:
+		bat_event = EVENT_CHRGCURRI;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	if (sub == true) {
+		CHECK_ERROR(pmic_event_subscribe(bat_event, bat_callback));
+	} else {
+		CHECK_ERROR(pmic_event_unsubscribe(bat_event, bat_callback));
+	}
+	return 0;
+}
+
+/*!
+ * This function is used to subscribe on battery event IT.
+ *
+ * @param        event          type of event.
+ * @param        callback       event callback function.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_batt_event_subscribe(t_batt_event event, void *callback)
+{
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	return mc13783_battery_event(event, callback, true);
+}
+
+/*!
+ * This function is used to un subscribe on battery event IT.
+ *
+ * @param        event          type of event.
+ * @param        callback       event callback function.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_batt_event_unsubscribe(t_batt_event event, void *callback)
+{
+	if (suspend_flag == 1)
+		return PMIC_ERROR;
+
+	return mc13783_battery_event(event, callback, false);
+}
+
+/*!
+ * This function implements IOCTL controls on a PMIC Battery device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @param        cmd         the command
+ * @param        arg         the parameter
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_battery_ioctl(struct inode *inode, struct file *file,
+			      unsigned int cmd, unsigned long arg)
+{
+	t_charger_setting *chgr_setting = NULL;
+	unsigned short c_current;
+	unsigned int bc_info;
+	t_eol_setting *eol_setting;
+
+	if (_IOC_TYPE(cmd) != 'p')
+		return -ENOTTY;
+
+	switch (cmd) {
+	case PMIC_BATT_CHARGER_CONTROL:
+		if ((chgr_setting = kmalloc(sizeof(t_charger_setting),
+					    GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(chgr_setting, (t_charger_setting *) arg,
+				   sizeof(t_charger_setting))) {
+			kfree(chgr_setting);
+			return -EFAULT;
+		}
+
+		if (chgr_setting->on != false) {
+			CHECK_ERROR_KFREE(pmic_batt_enable_charger
+					  (chgr_setting->chgr,
+					   chgr_setting->c_voltage,
+					   chgr_setting->c_current),
+					  (kfree(chgr_setting)));
+		} else {
+			CHECK_ERROR(pmic_batt_disable_charger
+				    (chgr_setting->chgr));
+		}
+
+		kfree(chgr_setting);
+		break;
+
+	case PMIC_BATT_SET_CHARGER:
+		if ((chgr_setting = kmalloc(sizeof(t_charger_setting),
+					    GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(chgr_setting, (t_charger_setting *) arg,
+				   sizeof(t_charger_setting))) {
+			kfree(chgr_setting);
+			return -EFAULT;
+		}
+
+		CHECK_ERROR_KFREE(pmic_batt_set_charger(chgr_setting->chgr,
+							chgr_setting->c_voltage,
+							chgr_setting->
+							c_current),
+				  (kfree(chgr_setting)));
+
+		kfree(chgr_setting);
+		break;
+
+	case PMIC_BATT_GET_CHARGER:
+		if ((chgr_setting = kmalloc(sizeof(t_charger_setting),
+					    GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(chgr_setting, (t_charger_setting *) arg,
+				   sizeof(t_charger_setting))) {
+			kfree(chgr_setting);
+			return -EFAULT;
+		}
+
+		CHECK_ERROR_KFREE(pmic_batt_get_charger_setting
+				  (chgr_setting->chgr, &chgr_setting->c_voltage,
+				   &chgr_setting->c_current),
+				  (kfree(chgr_setting)));
+		if (copy_to_user
+		    ((t_charger_setting *) arg, chgr_setting,
+		     sizeof(t_charger_setting))) {
+			return -EFAULT;
+		}
+
+		kfree(chgr_setting);
+		break;
+
+	case PMIC_BATT_GET_CHARGER_SENSOR:
+		{
+			t_sensor_bits sensor;
+			pmic_get_sensors(&sensor);
+			if (copy_to_user
+			    ((unsigned int *)arg, &sensor.sense_chgdets,
+			     sizeof(unsigned int)))
+				return -EFAULT;
+
+			break;
+		}
+	case PMIC_BATT_GET_BATTERY_VOLTAGE:
+		CHECK_ERROR(pmic_batt_get_batt_voltage(&c_current));
+		bc_info = (unsigned int)c_current * 2300 / 1023 + 2400;
+		if (copy_to_user((unsigned int *)arg, &bc_info,
+				 sizeof(unsigned int)))
+			return -EFAULT;
+
+		break;
+
+	case PMIC_BATT_GET_BATTERY_CURRENT:
+		CHECK_ERROR(pmic_batt_get_batt_current(&c_current));
+		bc_info = (unsigned int)c_current * 5750 / 1023;
+		if (copy_to_user((unsigned int *)arg, &bc_info,
+				 sizeof(unsigned int)))
+			return -EFAULT;
+		break;
+
+	case PMIC_BATT_GET_BATTERY_TEMPERATURE:
+		CHECK_ERROR(pmic_batt_get_batt_temperature(&c_current));
+		bc_info = (unsigned int)c_current;
+		if (copy_to_user((unsigned int *)arg, &bc_info,
+				 sizeof(unsigned int)))
+			return -EFAULT;
+
+		break;
+
+	case PMIC_BATT_GET_CHARGER_VOLTAGE:
+		CHECK_ERROR(pmic_batt_get_charge_voltage(&c_current));
+		bc_info = (unsigned int)c_current * 23000 / 1023;
+		if (copy_to_user((unsigned int *)arg, &bc_info,
+				 sizeof(unsigned int)))
+			return -EFAULT;
+
+		break;
+
+	case PMIC_BATT_GET_CHARGER_CURRENT:
+		CHECK_ERROR(pmic_batt_get_charge_current(&c_current));
+		bc_info = (unsigned int)c_current * 5750 / 1023;
+		if (copy_to_user((unsigned int *)arg, &bc_info,
+				 sizeof(unsigned int)))
+			return -EFAULT;
+
+		break;
+
+	case PMIC_BATT_EOL_CONTROL:
+		if ((eol_setting = kmalloc(sizeof(t_eol_setting), GFP_KERNEL))
+		    == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(eol_setting, (t_eol_setting *) arg,
+				   sizeof(t_eol_setting))) {
+			kfree(eol_setting);
+			return -EFAULT;
+		}
+
+		if (eol_setting->enable != false) {
+			CHECK_ERROR_KFREE(pmic_batt_bp_enable_eol
+					  (eol_setting->typical),
+					  (kfree(chgr_setting)));
+		} else {
+			CHECK_ERROR_KFREE(pmic_batt_disable_eol(),
+					  (kfree(chgr_setting)));
+		}
+
+		kfree(eol_setting);
+		break;
+
+	case PMIC_BATT_SET_OUT_CONTROL:
+		CHECK_ERROR(pmic_batt_set_out_control((t_control) arg));
+		break;
+
+	case PMIC_BATT_SET_THRESHOLD:
+		CHECK_ERROR(pmic_batt_set_threshold((int)arg));
+		break;
+
+	case PMIC_BATT_LED_CONTROL:
+		CHECK_ERROR(pmic_batt_led_control((bool) arg));
+		break;
+
+	case PMIC_BATT_REV_SUPP_CONTROL:
+		CHECK_ERROR(pmic_batt_set_reverse_supply((bool) arg));
+		break;
+
+	case PMIC_BATT_UNREG_CONTROL:
+		CHECK_ERROR(pmic_batt_set_unregulated((bool) arg));
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*!
+ * This function implements the open method on a Pmic battery device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_battery_open(struct inode *inode, struct file *file)
+{
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0))) {
+			return -ERESTARTSYS;
+		}
+	}
+
+	/* check open count, if open firstly, register callbacks */
+	down(&count_mutex);
+	if (open_count++ > 0) {
+		up(&count_mutex);
+		return 0;
+	}
+
+	pr_debug("Subscribe the callbacks\n");
+	/* register battery event callback */
+	if (pmic_batt_event_subscribe(BAT_IT_CHG_DET, callback_chg_detect)) {
+		pr_debug("Failed to subscribe the charger detect callback\n");
+		goto event_err1;
+	}
+	if (pmic_power_event_sub(PWR_IT_LOBATLI, callback_power_fail)) {
+		pr_debug("Failed to subscribe the power failed callback\n");
+		goto event_err2;
+	}
+	if (pmic_power_event_sub(PWR_IT_LOBATHI, callback_low_battery)) {
+		pr_debug("Failed to subscribe the low battery callback\n");
+		goto event_err3;
+	}
+	if (pmic_batt_event_subscribe
+	    (BAT_IT_CHG_OVERVOLT, callback_chg_overvoltage)) {
+		pr_debug("Failed to subscribe the low battery callback\n");
+		goto event_err4;
+	}
+	if (pmic_batt_event_subscribe
+	    (BAT_IT_BELOW_THRESHOLD, callback_chg_full)) {
+		pr_debug("Failed to subscribe the charge full callback\n");
+		goto event_err5;
+	}
+
+	up(&count_mutex);
+
+	return 0;
+
+	/* un-subscribe the event callbacks */
+event_err5:
+	pmic_batt_event_unsubscribe(BAT_IT_CHG_OVERVOLT,
+				    callback_chg_overvoltage);
+event_err4:
+	pmic_power_event_unsub(PWR_IT_LOBATHI, callback_low_battery);
+event_err3:
+	pmic_power_event_unsub(PWR_IT_LOBATLI, callback_power_fail);
+event_err2:
+	pmic_batt_event_unsubscribe(BAT_IT_CHG_DET, callback_chg_detect);
+event_err1:
+
+	open_count--;
+	up(&count_mutex);
+
+	return -EFAULT;
+
+}
+
+/*!
+ * This function implements the release method on a Pmic battery device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_battery_release(struct inode *inode, struct file *file)
+{
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0))) {
+			return -ERESTARTSYS;
+		}
+	}
+
+	/* check open count, if open firstly, register callbacks */
+	down(&count_mutex);
+	if (--open_count == 0) {
+		/* unregister these event callback */
+		pr_debug("Unsubscribe the callbacks\n");
+		pmic_batt_event_unsubscribe(BAT_IT_BELOW_THRESHOLD,
+					    callback_chg_full);
+		pmic_batt_event_unsubscribe(BAT_IT_CHG_OVERVOLT,
+					    callback_chg_overvoltage);
+		pmic_power_event_unsub(PWR_IT_LOBATHI, callback_low_battery);
+		pmic_power_event_unsub(PWR_IT_LOBATLI, callback_power_fail);
+		pmic_batt_event_unsubscribe(BAT_IT_CHG_DET,
+					    callback_chg_detect);
+	}
+	up(&count_mutex);
+
+	return 0;
+}
+
+static struct file_operations pmic_battery_fops = {
+	.owner = THIS_MODULE,
+	.ioctl = pmic_battery_ioctl,
+	.open = pmic_battery_open,
+	.release = pmic_battery_release,
+};
+
+static int pmic_battery_remove(struct platform_device *pdev)
+{
+	device_destroy(pmic_battery_class, MKDEV(pmic_battery_major, 0));
+	class_destroy(pmic_battery_class);
+	unregister_chrdev(pmic_battery_major, PMIC_BATTERY_STRING);
+	return 0;
+}
+
+static int pmic_battery_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct device *temp_class;
+
+	pmic_battery_major = register_chrdev(0, PMIC_BATTERY_STRING,
+					     &pmic_battery_fops);
+
+	if (pmic_battery_major < 0) {
+		printk(KERN_ERR "Unable to get a major for pmic_battery\n");
+		return pmic_battery_major;
+	}
+	init_waitqueue_head(&suspendq);
+
+	pmic_battery_class = class_create(THIS_MODULE, PMIC_BATTERY_STRING);
+	if (IS_ERR(pmic_battery_class)) {
+		printk(KERN_ERR "Error creating PMIC battery class.\n");
+		ret = PTR_ERR(pmic_battery_class);
+		goto err_out1;
+	}
+
+	temp_class = device_create(pmic_battery_class, NULL,
+				   MKDEV(pmic_battery_major, 0), NULL,
+				   PMIC_BATTERY_STRING);
+	if (IS_ERR(temp_class)) {
+		printk(KERN_ERR "Error creating PMIC battery class device.\n");
+		ret = PTR_ERR(temp_class);
+		goto err_out2;
+	}
+
+	pmic_batt_led_control(true);
+	pmic_batt_set_5k_pull(true);
+
+	printk(KERN_INFO "PMIC Battery successfully probed\n");
+
+	return ret;
+
+      err_out2:
+	class_destroy(pmic_battery_class);
+      err_out1:
+	unregister_chrdev(pmic_battery_major, PMIC_BATTERY_STRING);
+	return ret;
+}
+
+static struct platform_driver pmic_battery_driver_ldm = {
+	.driver = {
+		   .name = "pmic_battery",
+		   .bus = &platform_bus_type,
+		   },
+	.suspend = pmic_battery_suspend,
+	.resume = pmic_battery_resume,
+	.probe = pmic_battery_probe,
+	.remove = pmic_battery_remove,
+};
+
+/*
+ * Init and Exit
+ */
+
+static int __init pmic_battery_init(void)
+{
+	pr_debug("PMIC Battery driver loading...\n");
+	return platform_driver_register(&pmic_battery_driver_ldm);
+}
+
+static void __exit pmic_battery_exit(void)
+{
+	platform_driver_unregister(&pmic_battery_driver_ldm);
+	pr_debug("PMIC Battery driver successfully unloaded\n");
+}
+
+/*
+ * Module entry points
+ */
+
+module_init(pmic_battery_init);
+module_exit(pmic_battery_exit);
+
+MODULE_DESCRIPTION("pmic_battery driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/mc13783/pmic_battery_defs.h b/drivers/mxc/pmic/mc13783/pmic_battery_defs.h
new file mode 100644
index 000000000000..9f66a185cd2f
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_battery_defs.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_battery_defs.h
+ * @brief This is the internal header for PMIC(mc13783) Battery driver.
+ *
+ * @ingroup PMIC_BATTERY
+ */
+
+#ifndef __PMIC_BATTERY_DEFS_H__
+#define __PMIC_BATTERY_DEFS_H__
+
+#define         PMIC_BATTERY_STRING    "pmic_battery"
+
+/* REG_CHARGE */
+#define MC13783_BATT_DAC_V_DAC_LSH		0
+#define MC13783_BATT_DAC_V_DAC_WID		3
+#define MC13783_BATT_DAC_DAC_LSH			3
+#define MC13783_BATT_DAC_DAC_WID			4
+#define	MC13783_BATT_DAC_TRCKLE_LSH		7
+#define	MC13783_BATT_DAC_TRCKLE_WID		3
+#define MC13783_BATT_DAC_FETOVRD_EN_LSH		10
+#define MC13783_BATT_DAC_FETOVRD_EN_WID		1
+#define MC13783_BATT_DAC_FETCTRL_EN_LSH		11
+#define MC13783_BATT_DAC_FETCTRL_EN_WID		1
+#define MC13783_BATT_DAC_REVERSE_SUPPLY_LSH	13
+#define MC13783_BATT_DAC_REVERSE_SUPPLY_WID	1
+#define MC13783_BATT_DAC_OVCTRL_LSH		15
+#define MC13783_BATT_DAC_OVCTRL_WID		2
+#define MC13783_BATT_DAC_UNREGULATED_LSH		17
+#define MC13783_BATT_DAC_UNREGULATED_WID		1
+#define MC13783_BATT_DAC_LED_EN_LSH		18
+#define MC13783_BATT_DAC_LED_EN_WID		1
+#define MC13783_BATT_DAC_5K_LSH			19
+#define MC13783_BATT_DAC_5K_WID			1
+
+#define         BITS_OUT_VOLTAGE        0
+#define         LONG_OUT_VOLTAGE        3
+#define         BITS_CURRENT_MAIN       3
+#define         LONG_CURRENT_MAIN       4
+#define         BITS_CURRENT_TRICKLE    7
+#define         LONG_CURRENT_TRICKLE    3
+#define         BIT_FETOVRD             10
+#define         BIT_FETCTRL             11
+#define         BIT_RVRSMODE            13
+#define         BITS_OVERVOLTAGE        15
+#define         LONG_OVERVOLTAGE        2
+#define         BIT_UNREGULATED         17
+#define         BIT_CHRG_LED            18
+#define         BIT_CHRGRAWPDEN         19
+
+/* REG_POWXER_CONTROL_0 */
+#define MC13783_BATT_DAC_V_COIN_LSH		20
+#define MC13783_BATT_DAC_V_COIN_WID		3
+#define MC13783_BATT_DAC_COIN_CH_EN_LSH		23
+#define MC13783_BATT_DAC_COIN_CH_EN_WID		1
+#define MC13783_BATT_DAC_COIN_CH_EN_ENABLED	1
+#define MC13783_BATT_DAC_COIN_CH_EN_DISABLED	0
+#define MC13783_BATT_DAC_EOL_CMP_EN_LSH		18
+#define MC13783_BATT_DAC_EOL_CMP_EN_WID		1
+#define MC13783_BATT_DAC_EOL_CMP_EN_ENABLE	1
+#define MC13783_BATT_DAC_EOL_CMP_EN_DISABLE	0
+#define MC13783_BATT_DAC_EOL_SEL_LSH		16
+#define MC13783_BATT_DAC_EOL_SEL_WID		2
+
+#define         DEF_VALUE               0
+
+#define         BAT_THRESHOLD_MAX       3
+
+#endif				/*  __PMIC_BATTERY_DEFS_H__ */
diff --git a/drivers/mxc/pmic/mc13783/pmic_convity.c b/drivers/mxc/pmic/mc13783/pmic_convity.c
new file mode 100644
index 000000000000..5d634ebebacf
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_convity.c
@@ -0,0 +1,2482 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_convity.c
+ * @brief Implementation of the PMIC Connectivity driver APIs.
+ *
+ * The PMIC connectivity device driver and this API were developed to support
+ * the external connectivity capabilities of several power management ICs that
+ * are available from Freescale Semiconductor, Inc.
+ *
+ * The following operating modes, in terms of external connectivity, are
+ * supported:
+ *
+ * @verbatim
+       Operating Mode     mc13783
+       ---------------    -------
+       USB (incl. OTG)     Yes
+       RS-232              Yes
+       CEA-936             Yes
+
+   @endverbatim
+ *
+ * @ingroup PMIC_CONNECTIVITY
+ */
+
+#include <linux/interrupt.h>	/* For tasklet interface.                  */
+#include <linux/platform_device.h>	/* For kernel module interface.            */
+#include <linux/spinlock.h>	/* For spinlock interface.                 */
+#include <linux/pmic_adc.h>	/* For PMIC ADC driver interface.          */
+#include <linux/pmic_status.h>
+#include <mach/pmic_convity.h>	/* For PMIC Connectivity driver interface. */
+
+/*
+ * mc13783 Connectivity API
+ */
+/* EXPORTED FUNCTIONS */
+EXPORT_SYMBOL(pmic_convity_open);
+EXPORT_SYMBOL(pmic_convity_close);
+EXPORT_SYMBOL(pmic_convity_set_mode);
+EXPORT_SYMBOL(pmic_convity_get_mode);
+EXPORT_SYMBOL(pmic_convity_reset);
+EXPORT_SYMBOL(pmic_convity_set_callback);
+EXPORT_SYMBOL(pmic_convity_clear_callback);
+EXPORT_SYMBOL(pmic_convity_get_callback);
+EXPORT_SYMBOL(pmic_convity_usb_set_speed);
+EXPORT_SYMBOL(pmic_convity_usb_get_speed);
+EXPORT_SYMBOL(pmic_convity_usb_set_power_source);
+EXPORT_SYMBOL(pmic_convity_usb_get_power_source);
+EXPORT_SYMBOL(pmic_convity_usb_set_xcvr);
+EXPORT_SYMBOL(pmic_convity_usb_get_xcvr);
+EXPORT_SYMBOL(pmic_convity_usb_otg_set_dlp_duration);
+EXPORT_SYMBOL(pmic_convity_usb_otg_get_dlp_duration);
+EXPORT_SYMBOL(pmic_convity_usb_otg_set_config);
+EXPORT_SYMBOL(pmic_convity_usb_otg_clear_config);
+EXPORT_SYMBOL(pmic_convity_usb_otg_get_config);
+EXPORT_SYMBOL(pmic_convity_set_output);
+EXPORT_SYMBOL(pmic_convity_rs232_set_config);
+EXPORT_SYMBOL(pmic_convity_rs232_get_config);
+EXPORT_SYMBOL(pmic_convity_cea936_exit_signal);
+
+/*! @def SET_BITS
+ * Set a register field to a given value.
+ */
+
+#define SET_BITS(reg, field, value)    (((value) << reg.field.offset) & \
+                                        reg.field.mask)
+
+/*! @def GET_BITS
+ * Get the current value of a given register field.
+ */
+#define GET_BITS(reg, value)    (((value) & reg.mask) >> \
+                                 reg.offset)
+
+/*!
+ * @brief Define the possible states for a device handle.
+ *
+ * This enumeration is used to track the current state of each device handle.
+ */
+typedef enum {
+	HANDLE_FREE,		/*!< Handle is available for use. */
+	HANDLE_IN_USE		/*!< Handle is currently in use.  */
+} HANDLE_STATE;
+
+/*
+ * This structure is used to define a specific hardware register field.
+ *
+ * All hardware register fields are defined using an offset to the LSB
+ * and a mask. The offset is used to right shift a register value before
+ * applying the mask to actually obtain the value of the field.
+ */
+typedef struct {
+	const unsigned char offset;	/* Offset of LSB of register field.           */
+	const unsigned int mask;	/* Mask value used to isolate register field. */
+} REGFIELD;
+
+/*!
+ * @brief This structure is used to identify the fields in the USBCNTRL_REG_0 hardware register.
+ *
+ * This structure lists all of the fields within the USBCNTRL_REG_0 hardware
+ * register.
+ */
+typedef struct {
+	REGFIELD FSENB;		/*!< USB Full Speed Enable                            */
+	REGFIELD USB_SUSPEND;	/*!< USB Suspend Mode Enable                          */
+	REGFIELD USB_PU;	/*!< USB Pullup Enable                                */
+	REGFIELD UDP_PD;	/*!< USB Data Plus Pulldown Enable                    */
+	REGFIELD UDM_PD;	/*!< USB 150K UDP Pullup Enable                       */
+	REGFIELD DP150K_PU;	/*!< USB Pullup/Pulldown Override Enable              */
+	REGFIELD VBUSPDENB;	/*!< USB VBUS Pulldown NMOS Switch Enable             */
+	REGFIELD CURRENT_LIMIT;	/*!< USB Regulator Current Limit Setting-3 bits       */
+	REGFIELD DLP_SRP;	/*!< USB Data Line Pulsing Timer Enable               */
+	REGFIELD SE0_CONN;	/*!< USB Pullup Connect When SE0 Detected             */
+	REGFIELD USBXCVREN;	/*!< USB Transceiver Enabled When INTERFACE_MODE[2:0]=000 and RESETB=high */
+	REGFIELD PULLOVR;	/*!< 1K5 Pullup and UDP/UDM Pulldown Disable When UTXENB=Low             */
+	REGFIELD INTERFACE_MODE;	/*!< Connectivity Interface Mode Select-3 Bits        */
+	REGFIELD DATSE0;	/*!< USB Single or Differential Mode Select           */
+	REGFIELD BIDIR;		/*!< USB Unidirectional/Bidirectional Transmission    */
+	REGFIELD USBCNTRL;	/*!< USB Mode of Operation controlled By USBEN/SPI Pin */
+	REGFIELD IDPD;		/*!< USB UID Pulldown Enable                          */
+	REGFIELD IDPULSE;	/*!< USB Pulse to Gnd on UID Line Generated           */
+	REGFIELD IDPUCNTRL;	/*!< USB UID Pin pulled high By 5ua Curr Source       */
+	REGFIELD DMPULSE;	/*!< USB Positive pulse on the UDM Line Generated     */
+} USBCNTRL_REG_0;
+
+/*!
+ * @brief This variable is used to access the USBCNTRL_REG_0 hardware register.
+ *
+ * This variable defines how to access all of the fields within the
+ * USBCNTRL_REG_0  hardware register. The initial values consist of the offset
+ * and mask values needed to access each of the register fields.
+ */
+static const USBCNTRL_REG_0 regUSB0 = {
+	{0, 0x000001},		/*!< FSENB        */
+	{1, 0x000002},		/*!< USB_SUSPEND  */
+	{2, 0x000004},		/*!< USB_PU       */
+	{3, 0x000008},		/*!< UDP_PD       */
+	{4, 0x000010},		/*!< UDM_PD       */
+	{5, 0x000020},		/*!< DP150K_PU    */
+	{6, 0x000040},		/*!< VBUSPDENB    */
+	{7, 0x000380},		/*!< CURRENT_LIMIT */
+	{10, 0x000400},		/*!< DLP_SRP      */
+	{11, 0x000800},		/*!< SE0_CONN     */
+	{12, 0x001000},		/*!< USBXCVREN    */
+	{13, 0x002000},		/*!< PULLOVR      */
+	{14, 0x01c000},		/*!< INTERFACE_MODE */
+	{17, 0x020000},		/*!< DATSE0       */
+	{18, 0x040000},		/*!< BIDIR        */
+	{19, 0x080000},		/*!< USBCNTRL     */
+	{20, 0x100000},		/*!< IDPD         */
+	{21, 0x200000},		/*!< IDPULSE      */
+	{22, 0x400000},		/*!< IDPUCNTRL    */
+	{23, 0x800000}		/*!< DMPULSE      */
+
+};
+
+/*!
+ * @brief This structure is used to identify the fields in the USBCNTRL_REG_1 hardware register.
+ *
+ * This structure lists all of the fields within the USBCNTRL_REG_1 hardware
+ * register.
+ */
+typedef struct {
+	REGFIELD VUSBIN;	/*!< Controls The Input Source For VUSB             */
+	REGFIELD VUSB;		/*!< VUSB Output Voltage Select-High=3.3V Low=2.775V */
+	REGFIELD VUSBEN;	/*!< VUSB Output Enable-                            */
+	REGFIELD VBUSEN;	/*!< VBUS Output Enable-                            */
+	REGFIELD RSPOL;		/*!< Low=RS232 TX on UDM, RX on UDP
+				   High= RS232 TX on UDP, RX on UDM               */
+	REGFIELD RSTRI;		/*!< TX Forced To Tristate in RS232 Mode Only       */
+	REGFIELD ID100kPU;	/*!< 100k UID Pullup Enabled                        */
+} USBCNTRL_REG_1;
+
+/*!
+ * @brief This variable is used to access the USBCNTRL_REG_1 hardware register.
+ *
+ * This variable defines how to access all of the fields within the
+ * USBCNTRL_REG_1  hardware register. The initial values consist of the offset
+ * and mask values needed to access each of the register fields.
+ */
+static const USBCNTRL_REG_1 regUSB1 = {
+	{0, 0x000003},		/*!< VUSBIN-2 Bits  */
+	{2, 0x000004},		/*!< VUSB           */
+	{3, 0x000008},		/*!< VUSBEN         */
+	/*{4, 0x000010} *//*!< Reserved       */
+	{5, 0x000020},		/*!< VBUSEN         */
+	{6, 0x000040},		/*!< RSPOL          */
+	{7, 0x000080},		/*!< RSTRI          */
+	{8, 0x000100}		/*!< ID100kPU       */
+	/*!< 9-23 Unused    */
+};
+
+/*! Define a mask to access the entire hardware register. */
+static const unsigned int REG_FULLMASK = 0xffffff;
+
+/*! Define the mc13783 USBCNTRL_REG_0 register power on reset state. */
+static const unsigned int RESET_USBCNTRL_REG_0 = 0x080060;
+
+/*! Define the mc13783 USBCNTRL_REG_1 register power on reset state. */
+static const unsigned int RESET_USBCNTRL_REG_1 = 0x000006;
+
+static pmic_event_callback_t eventNotify;
+
+/*!
+ * @brief This structure is used to maintain the current device driver state.
+ *
+ * This structure maintains the current state of the connectivity driver. This
+ * includes both the PMIC hardware state as well as the device handle and
+ * callback states.
+ */
+
+typedef struct {
+	PMIC_CONVITY_HANDLE handle;	/*!< Device handle.   */
+	HANDLE_STATE handle_state;	/*!< Device handle
+					   state.           */
+	PMIC_CONVITY_MODE mode;	/*!< Device mode.     */
+	PMIC_CONVITY_CALLBACK callback;	/*!< Event callback function pointer. */
+	PMIC_CONVITY_EVENTS eventMask;	/*!< Event mask.      */
+	PMIC_CONVITY_USB_SPEED usbSpeed;	/*!< USB connection
+						   speed.           */
+	PMIC_CONVITY_USB_MODE usbMode;	/*!< USB connection
+					   mode.            */
+	PMIC_CONVITY_USB_POWER_IN usbPowerIn;	/*!< USB transceiver
+						   power source.    */
+	PMIC_CONVITY_USB_POWER_OUT usbPowerOut;	/*!< USB transceiver
+						   power output
+						   level.           */
+	PMIC_CONVITY_USB_TRANSCEIVER_MODE usbXcvrMode;	/*!< USB transceiver
+							   mode.            */
+	unsigned int usbDlpDuration;	/*!< USB Data Line
+					   Pulsing duration. */
+	PMIC_CONVITY_USB_OTG_CONFIG usbOtgCfg;	/*!< USB OTG
+						   configuration
+						   options.         */
+	PMIC_CONVITY_RS232_INTERNAL rs232CfgInternal;	/*!< RS-232 internal
+							   connections.     */
+	PMIC_CONVITY_RS232_EXTERNAL rs232CfgExternal;	/*!< RS-232 external
+							   connections.     */
+} pmic_convity_state_struct;
+
+/*!
+ * @brief This structure is used to maintain the current device driver state.
+ *
+ * This structure maintains the current state of the driver in USB mode. This
+ * includes both the PMIC hardware state as well as the device handle and
+ * callback states.
+ */
+
+typedef struct {
+	PMIC_CONVITY_HANDLE handle;	/*!< Device handle.   */
+	HANDLE_STATE handle_state;	/*!< Device handle
+					   state.           */
+	PMIC_CONVITY_MODE mode;	/*!< Device mode.     */
+	PMIC_CONVITY_CALLBACK callback;	/*!< Event callback function pointer. */
+	PMIC_CONVITY_EVENTS eventMask;	/*!< Event mask.      */
+	PMIC_CONVITY_USB_SPEED usbSpeed;	/*!< USB connection
+						   speed.           */
+	PMIC_CONVITY_USB_MODE usbMode;	/*!< USB connection
+					   mode.            */
+	PMIC_CONVITY_USB_POWER_IN usbPowerIn;	/*!< USB transceiver
+						   power source.    */
+	PMIC_CONVITY_USB_POWER_OUT usbPowerOut;	/*!< USB transceiver
+						   power output
+						   level.           */
+	PMIC_CONVITY_USB_TRANSCEIVER_MODE usbXcvrMode;	/*!< USB transceiver
+							   mode.            */
+	unsigned int usbDlpDuration;	/*!< USB Data Line
+					   Pulsing duration. */
+	PMIC_CONVITY_USB_OTG_CONFIG usbOtgCfg;	/*!< USB OTG
+						   configuration
+						   options.         */
+} pmic_convity_usb_state;
+
+/*!
+ * @brief This structure is used to maintain the current device driver state.
+ *
+ * This structure maintains the current state of the driver in RS_232 mode. This
+ * includes both the PMIC hardware state as well as the device handle and
+ * callback states.
+ */
+
+typedef struct {
+	PMIC_CONVITY_HANDLE handle;	/*!< Device handle.   */
+	HANDLE_STATE handle_state;	/*!< Device handle
+					   state.           */
+	PMIC_CONVITY_MODE mode;	/*!< Device mode.     */
+	PMIC_CONVITY_CALLBACK callback;	/*!< Event callback function pointer. */
+	PMIC_CONVITY_EVENTS eventMask;	/*!< Event mask.      */
+	PMIC_CONVITY_RS232_INTERNAL rs232CfgInternal;	/*!< RS-232 internal
+							   connections.     */
+	PMIC_CONVITY_RS232_EXTERNAL rs232CfgExternal;	/*!< RS-232 external
+							   connections.     */
+} pmic_convity_rs232_state;
+
+/*!
+ * @brief This structure is used to maintain the current device driver state.
+ *
+ * This structure maintains the current state of the driver in cea-936 mode. This
+ * includes both the PMIC hardware state as well as the device handle and
+ * callback states.
+ */
+
+typedef struct {
+	PMIC_CONVITY_HANDLE handle;	/*!< Device handle.   */
+	HANDLE_STATE handle_state;	/*!< Device handle
+					   state.           */
+	PMIC_CONVITY_MODE mode;	/*!< Device mode.     */
+	PMIC_CONVITY_CALLBACK callback;	/*!< Event callback function pointer. */
+	PMIC_CONVITY_EVENTS eventMask;	/*!< Event mask.      */
+
+} pmic_convity_cea936_state;
+
+/*!
+ * @brief Identifies the hardware interrupt source.
+ *
+ * This enumeration identifies which of the possible hardware interrupt
+ * sources actually caused the current interrupt handler to be called.
+ */
+typedef enum {
+	CORE_EVENT_4V4 = 1,	/*!< Detected USB 4.4 V event.              */
+	CORE_EVENT_2V0 = 2,	/*!< Detected USB 2.0 V event.              */
+	CORE_EVENT_0V8 = 4,	/*!< Detected USB 0.8 V event.              */
+	CORE_EVENT_ABDET = 8	/*!< Detected USB mini A-B connector event. */
+} PMIC_CORE_EVENT;
+
+/*!
+ * @brief This structure defines the reset/power on state for the Connectivity driver.
+ */
+static const pmic_convity_state_struct reset = {
+	0,
+	HANDLE_FREE,
+	USB,
+	NULL,
+	0,
+	USB_FULL_SPEED,
+	USB_PERIPHERAL,
+	USB_POWER_INTERNAL,
+	USB_POWER_3V3,
+	USB_TRANSCEIVER_OFF,
+	0,
+	USB_PULL_OVERRIDE | USB_VBUS_CURRENT_LIMIT_HIGH,
+	RS232_TX_USE0VM_RX_UDATVP,
+	RS232_TX_UDM_RX_UDP
+};
+
+/*!
+ * @brief This structure maintains the current state of the Connectivity driver.
+ *
+ * The initial values must be identical to the reset state defined by the
+ * #reset variable.
+ */
+static pmic_convity_usb_state usb = {
+	0,
+	HANDLE_FREE,
+	USB,
+	NULL,
+	0,
+	USB_FULL_SPEED,
+	USB_PERIPHERAL,
+	USB_POWER_INTERNAL,
+	USB_POWER_3V3,
+	USB_TRANSCEIVER_OFF,
+	0,
+	USB_PULL_OVERRIDE | USB_VBUS_CURRENT_LIMIT_HIGH,
+};
+
+/*!
+ * @brief This structure maintains the current state of the Connectivity driver.
+ *
+ * The initial values must be identical to the reset state defined by the
+ * #reset variable.
+ */
+static pmic_convity_rs232_state rs_232 = {
+	0,
+	HANDLE_FREE,
+	RS232_1,
+	NULL,
+	0,
+	RS232_TX_USE0VM_RX_UDATVP,
+	RS232_TX_UDM_RX_UDP
+};
+
+/*!
+ * @brief This structure maintains the current state of the Connectivity driver.
+ *
+ * The initial values must be identical to the reset state defined by the
+ * #reset variable.
+ */
+static pmic_convity_cea936_state cea_936 = {
+	0,
+	HANDLE_FREE,
+	CEA936_MONO,
+	NULL,
+	0,
+};
+
+/*!
+ * @brief This spinlock is used to provide mutual exclusion.
+ *
+ * Create a spinlock that can be used to provide mutually exclusive
+ * read/write access to the globally accessible "convity" data structure
+ * that was defined above. Mutually exclusive access is required to
+ * ensure that the convity data structure is consistent at all times
+ * when possibly accessed by multiple threads of execution (for example,
+ * while simultaneously handling a user request and an interrupt event).
+ *
+ * We need to use a spinlock sometimes because we need to provide mutual
+ * exclusion while handling a hardware interrupt.
+ */
+static spinlock_t lock = SPIN_LOCK_UNLOCKED;
+
+/*!
+ * @brief This mutex is used to provide mutual exclusion.
+ *
+ * Create a mutex that can be used to provide mutually exclusive
+ * read/write access to the globally accessible data structures
+ * that were defined above. Mutually exclusive access is required to
+ * ensure that the Connectivity data structures are consistent at all
+ * times when possibly accessed by multiple threads of execution.
+ *
+ * Note that we use a mutex instead of the spinlock whenever disabling
+ * interrupts while in the critical section is not required. This helps
+ * to minimize kernel interrupt handling latency.
+ */
+static DECLARE_MUTEX(mutex);
+
+/* Prototype for the connectivity driver tasklet function. */
+static void pmic_convity_tasklet(struct work_struct *work);
+
+/*!
+ * @brief Tasklet handler for the connectivity driver.
+ *
+ * Declare a tasklet that will do most of the processing for all of the
+ * connectivity-related interrupt events (USB4.4VI, USB2.0VI, USB0.8VI,
+ * and AB_DETI). Note that we cannot do all of the required processing
+ * within the interrupt handler itself because we may need to call the
+ * ADC driver to measure voltages as well as calling any user-registered
+ * callback functions.
+ */
+DECLARE_WORK(convityTasklet, pmic_convity_tasklet);
+
+/*!
+ * @brief Global variable to track currently active interrupt events.
+ *
+ * This global variable is used to keep track of all of the currently
+ * active interrupt events for the connectivity driver. Note that access
+ * to this variable may occur while within an interrupt context and,
+ * therefore, must be guarded by using a spinlock.
+ */
+static PMIC_CORE_EVENT eventID = 0;
+
+/* Prototypes for all static connectivity driver functions. */
+static PMIC_STATUS pmic_convity_set_mode_internal(const PMIC_CONVITY_MODE mode);
+static PMIC_STATUS pmic_convity_deregister_all(void);
+static void pmic_convity_event_handler(void *param);
+
+/**************************************************************************
+ * General setup and configuration functions.
+ **************************************************************************
+ */
+
+/*!
+ * @name General Setup and Configuration Connectivity APIs
+ * Functions for setting up and configuring the connectivity hardware.
+ */
+/*@{*/
+
+/*!
+ * Attempt to open and gain exclusive access to the PMIC connectivity
+ * hardware. An initial operating mode must also be specified.
+ *
+ * If the open request is successful, then a numeric handle is returned
+ * and this handle must be used in all subsequent function calls. The
+ * same handle must also be used in the pmic_convity_close() call when use
+ * of the PMIC connectivity hardware is no longer required.
+ *
+ * @param       handle          device handle from open() call
+ * @param       mode            initial connectivity operating mode
+ *
+ * @return      PMIC_SUCCESS    if the open request was successful
+ */
+PMIC_STATUS pmic_convity_open(PMIC_CONVITY_HANDLE * const handle,
+			      const PMIC_CONVITY_MODE mode)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	if (handle == (PMIC_CONVITY_HANDLE *) NULL) {
+		/* Do not dereference a NULL pointer. */
+		return PMIC_ERROR;
+	}
+
+	/* We only need to acquire a mutex here because the interrupt handler
+	 * never modifies the device handle or device handle state. Therefore,
+	 * we don't need to worry about conflicts with the interrupt handler
+	 * or the need to execute in an interrupt context.
+	 *
+	 * But we do need a critical section here to avoid problems in case
+	 * multiple calls to pmic_convity_open() are made since we can only
+	 * allow one of them to succeed.
+	 */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	/* Check the current device handle state and acquire the handle if
+	 * it is available.
+	 */
+	if ((usb.handle_state != HANDLE_FREE)
+	    && (rs_232.handle_state != HANDLE_FREE)
+	    && (cea_936.handle_state != HANDLE_FREE)) {
+
+		/* Cannot open the PMIC connectivity hardware at this time or an invalid
+		 * mode was requested.
+		 */
+		*handle = reset.handle;
+	} else {
+
+		if (mode == USB) {
+			usb.handle = (PMIC_CONVITY_HANDLE) (&usb);
+			usb.handle_state = HANDLE_IN_USE;
+		} else if ((mode == RS232_1) || (mode == RS232_2)) {
+			rs_232.handle = (PMIC_CONVITY_HANDLE) (&rs_232);
+			rs_232.handle_state = HANDLE_IN_USE;
+		} else if ((mode == CEA936_STEREO) || (mode == CEA936_MONO)
+			   || (mode == CEA936_TEST_LEFT)
+			   || (mode == CEA936_TEST_RIGHT)) {
+			cea_936.handle = (PMIC_CONVITY_HANDLE) (&cea_936);
+			cea_936.handle_state = HANDLE_IN_USE;
+
+		}
+		/* Let's begin by acquiring the connectivity device handle. */
+		/* Then we can try to set the desired operating mode. */
+		rc = pmic_convity_set_mode_internal(mode);
+
+		if (rc == PMIC_SUCCESS) {
+			/* Successfully set the desired operating mode, now return the
+			 * handle to the caller.
+			 */
+			if (mode == USB) {
+				*handle = usb.handle;
+			} else if ((mode == RS232_1) || (mode == RS232_2)) {
+				*handle = rs_232.handle;
+			} else if ((mode == CEA936_STEREO)
+				   || (mode == CEA936_MONO)
+				   || (mode == CEA936_TEST_LEFT)
+				   || (mode == CEA936_TEST_RIGHT)) {
+				*handle = cea_936.handle;
+			}
+		} else {
+			/* Failed to set the desired mode, return the handle to an unused
+			 * state.
+			 */
+			if (mode == USB) {
+				usb.handle = reset.handle;
+				usb.handle_state = reset.handle_state;
+			} else if ((mode == RS232_1) || (mode == RS232_2)) {
+				rs_232.handle = reset.handle;
+				rs_232.handle_state = reset.handle_state;
+			} else if ((mode == CEA936_STEREO)
+				   || (mode == CEA936_MONO)
+				   || (mode == CEA936_TEST_LEFT)
+				   || (mode == CEA936_TEST_RIGHT)) {
+				cea_936.handle = reset.handle;
+				cea_936.handle_state = reset.handle_state;
+			}
+
+			*handle = reset.handle;
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Terminate further access to the PMIC connectivity hardware. Also allows
+ * another process to call pmic_convity_open() to gain access.
+ *
+ * @param       handle          device handle from open() call
+ *
+ * @return      PMIC_SUCCESS    if the close request was successful
+ */
+PMIC_STATUS pmic_convity_close(const PMIC_CONVITY_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Begin a critical section here to avoid the possibility of race
+	 * conditions if multiple threads happen to call this function and
+	 * pmic_convity_open() at the same time.
+	 */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	/* Confirm that the device handle matches the one assigned in the
+	 * pmic_convity_open() call and then close the connection.
+	 */
+	if (((handle == usb.handle) &&
+	     (usb.handle_state == HANDLE_IN_USE)) || ((handle == rs_232.handle)
+						      && (rs_232.handle_state ==
+							  HANDLE_IN_USE))
+	    || ((handle == cea_936.handle)
+		&& (cea_936.handle_state == HANDLE_IN_USE))) {
+		rc = PMIC_SUCCESS;
+
+		/* Deregister for all existing callbacks if necessary and make sure
+		 * that the event handling settings are consistent following the
+		 * close operation.
+		 */
+		if ((usb.callback != reset.callback)
+		    || (rs_232.callback != reset.callback)
+		    || (cea_936.callback != reset.callback)) {
+			/* Deregister the existing callback function and all registered
+			 * events before we completely close the handle.
+			 */
+			rc = pmic_convity_deregister_all();
+			if (rc == PMIC_SUCCESS) {
+
+			} else if (usb.eventMask != reset.eventMask) {
+				/* Having a non-zero eventMask without a callback function being
+				 * defined should never occur but let's just make sure here that
+				 * we keep things consistent.
+				 */
+				usb.eventMask = reset.eventMask;
+				/* Mark the connectivity device handle as being closed. */
+				usb.handle = reset.handle;
+				usb.handle_state = reset.handle_state;
+
+			} else if (rs_232.eventMask != reset.eventMask) {
+
+				rs_232.eventMask = reset.eventMask;
+				/* Mark the connectivity device handle as being closed. */
+				rs_232.handle = reset.handle;
+				rs_232.handle_state = reset.handle_state;
+
+			} else if (cea_936.eventMask != reset.eventMask) {
+				cea_936.eventMask = reset.eventMask;
+				/* Mark the connectivity device handle as being closed. */
+				cea_936.handle = reset.handle;
+				cea_936.handle_state = reset.handle_state;
+
+			}
+
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Change the current operating mode of the PMIC connectivity hardware.
+ * The available connectivity operating modes is hardware dependent and
+ * consists of one or more of the following: USB (including USB On-the-Go),
+ * RS-232, and CEA-936. Requesting an operating mode that is not supported
+ * by the PMIC hardware will return PMIC_NOT_SUPPORTED.
+ *
+ * @param       handle          device handle from
+					open() call
+ * @param       mode            desired operating mode
+ *
+ * @return      PMIC_SUCCESS    if the requested mode was successfully set
+ */
+PMIC_STATUS pmic_convity_set_mode(const PMIC_CONVITY_HANDLE handle,
+				  const PMIC_CONVITY_MODE mode)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if (((handle == usb.handle) &&
+	     (usb.handle_state == HANDLE_IN_USE)) || ((handle == rs_232.handle)
+						      && (rs_232.handle_state ==
+							  HANDLE_IN_USE))
+	    || ((handle == cea_936.handle)
+		&& (cea_936.handle_state == HANDLE_IN_USE))) {
+		rc = pmic_convity_set_mode_internal(mode);
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Get the current operating mode for the PMIC connectivity hardware.
+ *
+ * @param       handle          device handle from open() call
+ * @param       mode            the current PMIC connectivity operating mode
+ *
+ * @return      PMIC_SUCCESS    if the requested mode was successfully set
+ */
+PMIC_STATUS pmic_convity_get_mode(const PMIC_CONVITY_HANDLE handle,
+				  PMIC_CONVITY_MODE * const mode)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((((handle == usb.handle) &&
+	      (usb.handle_state == HANDLE_IN_USE)) || ((handle == rs_232.handle)
+						       && (rs_232.
+							   handle_state ==
+							   HANDLE_IN_USE))
+	     || ((handle == cea_936.handle)
+		 && (cea_936.handle_state == HANDLE_IN_USE)))
+	    && (mode != (PMIC_CONVITY_MODE *) NULL)) {
+
+		*mode = usb.mode;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Restore all registers to the initial power-on/reset state.
+ *
+ * @param       handle          device handle from open() call
+ *
+ * @return      PMIC_SUCCESS    if the reset was successful
+ */
+PMIC_STATUS pmic_convity_reset(const PMIC_CONVITY_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+	if (((handle == usb.handle) &&
+	     (usb.handle_state == HANDLE_IN_USE)) || ((handle == rs_232.handle)
+						      && (rs_232.handle_state ==
+							  HANDLE_IN_USE))
+	    || ((handle == cea_936.handle)
+		&& (cea_936.handle_state == HANDLE_IN_USE))) {
+
+		/* Reset the PMIC Connectivity register to it's power on state. */
+		rc = pmic_write_reg(REG_USB, RESET_USBCNTRL_REG_0,
+				    REG_FULLMASK);
+
+		rc = pmic_write_reg(REG_CHARGE_USB_SPARE,
+				    RESET_USBCNTRL_REG_1, REG_FULLMASK);
+
+		if (rc == PMIC_SUCCESS) {
+			/* Also reset the device driver state data structure. */
+
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Register a callback function that will be used to signal PMIC connectivity
+ * events. For example, the USB subsystem should register a callback function
+ * in order to be notified of device connect/disconnect events. Note, however,
+ * that non-USB events may also be signalled depending upon the PMIC hardware
+ * capabilities. Therefore, the callback function must be able to properly
+ * handle all of the possible events if support for non-USB peripherals is
+ * also to be included.
+ *
+ * @param       handle          device handle from open() call
+ * @param       func            a pointer to the callback function
+ * @param       eventMask       a mask selecting events to be notified
+ *
+ * @return      PMIC_SUCCESS    if the callback was successful registered
+ */
+PMIC_STATUS pmic_convity_set_callback(const PMIC_CONVITY_HANDLE handle,
+				      const PMIC_CONVITY_CALLBACK func,
+				      const PMIC_CONVITY_EVENTS eventMask)
+{
+	unsigned long flags;
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* We need to start a critical section here to ensure a consistent state
+	 * in case simultaneous calls to pmic_convity_set_callback() are made. In
+	 * that case, we must serialize the calls to ensure that the "callback"
+	 * and "eventMask" state variables are always consistent.
+	 *
+	 * Note that we don't actually need to acquire the spinlock until later
+	 * when we are finally ready to update the "callback" and "eventMask"
+	 * state variables which are shared with the interrupt handler.
+	 */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == usb.handle) && (usb.handle_state == HANDLE_IN_USE)) {
+
+		/* Return an error if either the callback function or event mask
+		 * is not properly defined.
+		 *
+		 * It is also considered an error if a callback function has already
+		 * been defined. If you wish to register for a new set of events,
+		 * then you must first call pmic_convity_clear_callback() to
+		 * deregister the existing callback function and list of events
+		 * before trying to register a new callback function.
+		 */
+		if ((func == NULL) || (eventMask == 0)
+		    || (usb.callback != NULL)) {
+			rc = PMIC_ERROR;
+
+			/* Register for PMIC events from the core protocol driver. */
+		} else {
+
+			if ((eventMask & USB_DETECT_4V4_RISE) ||
+			    (eventMask & USB_DETECT_4V4_FALL)) {
+				/* We need to register for the 4.4V interrupt. */
+				//EVENT_USBI or EVENT_USB_44VI
+				eventNotify.func = pmic_convity_event_handler;
+				eventNotify.param = (void *)(CORE_EVENT_4V4);
+				rc = pmic_event_subscribe(EVENT_USBI,
+							  eventNotify);
+
+				if (rc != PMIC_SUCCESS) {
+					return rc;
+				}
+			}
+
+			if ((eventMask & USB_DETECT_2V0_RISE) ||
+			    (eventMask & USB_DETECT_2V0_FALL)) {
+				/* We need to register for the 2.0V interrupt. */
+				//EVENT_USB_20VI or EVENT_USBI
+				eventNotify.func = pmic_convity_event_handler;
+				eventNotify.param = (void *)(CORE_EVENT_2V0);
+				rc = pmic_event_subscribe(EVENT_USBI,
+							  eventNotify);
+
+				if (rc != PMIC_SUCCESS) {
+					goto Cleanup_4V4;
+				}
+			}
+
+			if ((eventMask & USB_DETECT_0V8_RISE) ||
+			    (eventMask & USB_DETECT_0V8_FALL)) {
+				/* We need to register for the 0.8V interrupt. */
+				//EVENT_USB_08VI or EVENT_USBI
+				eventNotify.func = pmic_convity_event_handler;
+				eventNotify.param = (void *)(CORE_EVENT_0V8);
+				rc = pmic_event_subscribe(EVENT_USBI,
+							  eventNotify);
+
+				if (rc != PMIC_SUCCESS) {
+					goto Cleanup_2V0;
+				}
+			}
+
+			if ((eventMask & USB_DETECT_MINI_A) ||
+			    (eventMask & USB_DETECT_MINI_B)
+			    || (eventMask & USB_DETECT_NON_USB_ACCESSORY)
+			    || (eventMask & USB_DETECT_FACTORY_MODE)) {
+				/* We need to register for the AB_DET interrupt. */
+				//EVENT_AB_DETI or EVENT_IDI
+				eventNotify.func = pmic_convity_event_handler;
+				eventNotify.param = (void *)(CORE_EVENT_ABDET);
+				rc = pmic_event_subscribe(EVENT_IDI,
+							  eventNotify);
+
+				if (rc != PMIC_SUCCESS) {
+					goto Cleanup_0V8;
+				}
+			}
+
+			/* Use a critical section to maintain a consistent state. */
+			spin_lock_irqsave(&lock, flags);
+
+			/* Successfully registered for all events. */
+			usb.callback = func;
+			usb.eventMask = eventMask;
+			spin_unlock_irqrestore(&lock, flags);
+
+			goto End;
+
+			/* This section unregisters any already registered events if we should
+			 * encounter an error partway through the registration process. Note
+			 * that we don't check the return status here since it is already set
+			 * to PMIC_ERROR before we get here.
+			 */
+		      Cleanup_0V8:
+
+			if ((eventMask & USB_DETECT_0V8_RISE) ||
+			    (eventMask & USB_DETECT_0V8_FALL)) {
+				//EVENT_USB_08VI or EVENT_USBI
+				eventNotify.func = pmic_convity_event_handler;
+				eventNotify.param = (void *)(CORE_EVENT_0V8);
+				pmic_event_unsubscribe(EVENT_USBI, eventNotify);
+				goto End;
+			}
+
+		      Cleanup_2V0:
+
+			if ((eventMask & USB_DETECT_2V0_RISE) ||
+			    (eventMask & USB_DETECT_2V0_FALL)) {
+				//EVENT_USB_20VI or EVENT_USBI
+				eventNotify.func = pmic_convity_event_handler;
+				eventNotify.param = (void *)(CORE_EVENT_2V0);
+				pmic_event_unsubscribe(EVENT_USBI, eventNotify);
+				goto End;
+			}
+
+		      Cleanup_4V4:
+
+			if ((eventMask & USB_DETECT_4V4_RISE) ||
+			    (eventMask & USB_DETECT_4V4_FALL)) {
+				//EVENT_USB_44VI or EVENT_USBI
+				eventNotify.func = pmic_convity_event_handler;
+				eventNotify.param = (void *)(CORE_EVENT_4V4);
+				pmic_event_unsubscribe(EVENT_USBI, eventNotify);
+			}
+		}
+		/* Exit the critical section. */
+
+	}
+      End:up(&mutex);
+	return rc;
+
+}
+
+/*!
+ * Clears the current callback function. If this function returns successfully
+ * then all future Connectivity events will only be handled by the default
+ * handler within the Connectivity driver.
+ *
+ * @param       handle          device handle from open() call
+ *
+ * @return      PMIC_SUCCESS    if the callback was successful cleared
+ */
+PMIC_STATUS pmic_convity_clear_callback(const PMIC_CONVITY_HANDLE handle)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+	if (((handle == usb.handle) &&
+	     (usb.handle_state == HANDLE_IN_USE)) || ((handle == rs_232.handle)
+						      && (rs_232.handle_state ==
+							  HANDLE_IN_USE))
+	    || ((handle == cea_936.handle)
+		&& (cea_936.handle_state == HANDLE_IN_USE))) {
+
+		rc = pmic_convity_deregister_all();
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Get the current callback function and event mask.
+ *
+ * @param       handle          device handle from open() call
+ * @param       func            the current callback function
+ * @param       eventMask       the current event selection mask
+ *
+ * @return      PMIC_SUCCESS    if the callback information was successful
+ *                              retrieved
+ */
+PMIC_STATUS pmic_convity_get_callback(const PMIC_CONVITY_HANDLE handle,
+				      PMIC_CONVITY_CALLBACK * const func,
+				      PMIC_CONVITY_EVENTS * const eventMask)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+	if ((((handle == usb.handle) &&
+	      (usb.handle_state == HANDLE_IN_USE)) || ((handle == rs_232.handle)
+						       && (rs_232.
+							   handle_state ==
+							   HANDLE_IN_USE))
+	     || ((handle == cea_936.handle)
+		 && (cea_936.handle_state == HANDLE_IN_USE)))
+	    && (func != (PMIC_CONVITY_CALLBACK *) NULL)
+	    && (eventMask != (PMIC_CONVITY_EVENTS *) NULL)) {
+		*func = usb.callback;
+		*eventMask = usb.eventMask;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+
+	up(&mutex);
+
+	return rc;
+}
+
+/*@*/
+
+/**************************************************************************
+ * USB-specific configuration and setup functions.
+ **************************************************************************
+ */
+
+/*!
+ * @name USB and USB-OTG Connectivity APIs
+ * Functions for controlling USB and USB-OTG connectivity.
+ */
+/*@{*/
+
+/*!
+ * Set the USB transceiver speed.
+ *
+ * @param       handle          device handle from open() call
+ * @param       speed           the desired USB transceiver speed
+ *
+ * @return      PMIC_SUCCESS    if the transceiver speed was successfully set
+ */
+PMIC_STATUS pmic_convity_usb_set_speed(const PMIC_CONVITY_HANDLE handle,
+				       const PMIC_CONVITY_USB_SPEED speed)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+	unsigned int reg_value = 0;
+	unsigned int reg_mask = SET_BITS(regUSB0, FSENB, 1);
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if (handle == (rs_232.handle || cea_936.handle)) {
+		return PMIC_PARAMETER_ERROR;
+	} else {
+		if ((handle == usb.handle) &&
+		    (usb.handle_state == HANDLE_IN_USE)) {
+			/* Validate the function parameters and if they are valid, then
+			 * configure the pull-up and pull-down resistors as required for
+			 * the desired operating mode.
+			 */
+			if ((speed == USB_HIGH_SPEED)) {
+				/*
+				 * The USB transceiver also does not support the high speed mode
+				 * (which is also optional under the USB OTG specification).
+				 */
+				rc = PMIC_NOT_SUPPORTED;
+			} else if ((speed != USB_LOW_SPEED)
+				   && (speed != USB_FULL_SPEED)) {
+				/* Final validity check on the speed parameter. */
+				rc = PMIC_ERROR;;
+			} else {
+				/* First configure the D+ and D- pull-up/pull-down resistors as
+				 * per the USB OTG specification.
+				 */
+				if (speed == USB_FULL_SPEED) {
+					/* Activate pull-up on D+ and pull-down on D-. */
+					reg_value =
+					    SET_BITS(regUSB0, UDM_PD, 1);
+				} else if (speed == USB_LOW_SPEED) {
+					/* Activate pull-up on D+ and pull-down on D-. */
+					reg_value = SET_BITS(regUSB0, FSENB, 1);
+				}
+
+				/* Now set the desired USB transceiver speed. Note that
+				 * USB_FULL_SPEED simply requires FSENB=0 (which it
+				 * already is).
+				 */
+
+				rc = pmic_write_reg(REG_USB, reg_value,
+						    reg_mask);
+
+				if (rc == PMIC_SUCCESS) {
+					usb.usbSpeed = speed;
+				}
+			}
+		}
+	}
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Get the USB transceiver speed.
+ *
+ * @param       handle          device handle from open() call
+ * @param       speed           the current USB transceiver speed
+ * @param       mode            the current USB transceiver mode
+ *
+ * @return      PMIC_SUCCESS    if the transceiver speed was successfully
+ *                              obtained
+ */
+PMIC_STATUS pmic_convity_usb_get_speed(const PMIC_CONVITY_HANDLE handle,
+				       PMIC_CONVITY_USB_SPEED * const speed,
+				       PMIC_CONVITY_USB_MODE * const mode)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == usb.handle) &&
+	    (usb.handle_state == HANDLE_IN_USE) &&
+	    (speed != (PMIC_CONVITY_USB_SPEED *) NULL) &&
+	    (mode != (PMIC_CONVITY_USB_MODE *) NULL)) {
+		*speed = usb.usbSpeed;
+		*mode = usb.usbMode;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * This function enables/disables VUSB and VBUS output.
+ * This API configures the VUSBEN and VBUSEN bits of USB register
+ *
+ * @param       handle          device handle from open() call
+ * @param        out_type true, for VBUS
+ *                        false, for VUSB
+ * @param        out 	if true, output is enabled
+ *                      if false, output is disabled
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_convity_set_output(const PMIC_CONVITY_HANDLE handle,
+				    bool out_type, bool out)
+{
+
+	PMIC_STATUS rc = PMIC_ERROR;
+	unsigned int reg_value = 0;
+
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == usb.handle) && (usb.handle_state == HANDLE_IN_USE)) {
+
+		if ((out_type == 0) && (out == 1)) {
+
+			reg_value = SET_BITS(regUSB1, VUSBEN, 1);
+			reg_mask = SET_BITS(regUSB1, VUSBEN, 1);
+
+			rc = pmic_write_reg(REG_CHARGE_USB_SPARE,
+					    reg_value, reg_mask);
+		} else if (out_type == 0 && out == 0) {
+			reg_mask = SET_BITS(regUSB1, VBUSEN, 1);
+
+			rc = pmic_write_reg(REG_CHARGE_USB_SPARE,
+					    reg_value, reg_mask);
+		} else if (out_type == 1 && out == 1) {
+
+			reg_value = SET_BITS(regUSB1, VBUSEN, 1);
+			reg_mask = SET_BITS(regUSB1, VBUSEN, 1);
+
+			rc = pmic_write_reg(REG_CHARGE_USB_SPARE,
+					    reg_value, reg_mask);
+		}
+
+		else if (out_type == 1 && out == 0) {
+
+			reg_mask = SET_BITS(regUSB1, VBUSEN, 1);
+			rc = pmic_write_reg(REG_CHARGE_USB_SPARE,
+					    reg_value, reg_mask);
+		}
+
+		/*else {
+
+		   rc = pmic_write_reg(REG_CHARGE_USB_SPARE,
+		   reg_value, reg_mask);
+		   } */
+	}
+
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Set the USB transceiver's power supply configuration.
+ *
+ * @param       handle          device handle from open() call
+ * @param       pwrin           USB transceiver regulator input power source
+ * @param       pwrout          USB transceiver regulator output power level
+ *
+ * @return      PMIC_SUCCESS    if the USB transceiver's power supply
+ *                              configuration was successfully set
+ */
+PMIC_STATUS pmic_convity_usb_set_power_source(const PMIC_CONVITY_HANDLE handle,
+					      const PMIC_CONVITY_USB_POWER_IN
+					      pwrin,
+					      const PMIC_CONVITY_USB_POWER_OUT
+					      pwrout)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+	unsigned int reg_value = 0;
+	unsigned int reg_mask = 0;
+	/* SET_BITS(regUSB1, VUSBEN, 1) | SET_BITS(regUSB1, VBUSEN,
+	   1) | SET_BITS(regUSB1,
+	   VUSBIN,
+	   2) | */
+	//    SET_BITS(regUSB1, VUSB, 1);
+/*	    SET_BITS(regUSB1, VUSBIN, 1) | SET_BITS(regUSB1, VUSBEN,
+						    1) | SET_BITS(regUSB1,
+								  VBUSEN, 1);*/
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == usb.handle) && (usb.handle_state == HANDLE_IN_USE)) {
+
+		if (pwrin == USB_POWER_INTERNAL_BOOST) {
+			reg_value |= SET_BITS(regUSB1, VUSBIN, 0);
+			reg_mask = SET_BITS(regUSB1, VUSBIN, 1);
+		} else if (pwrin == USB_POWER_VBUS) {
+			reg_value |= SET_BITS(regUSB1, VUSBIN, 1);
+			reg_mask = SET_BITS(regUSB1, VUSBIN, 1);
+		}
+
+		else if (pwrin == USB_POWER_INTERNAL) {
+			reg_value |= SET_BITS(regUSB1, VUSBIN, 2);
+			reg_mask = SET_BITS(regUSB1, VUSBIN, 1);
+		}
+
+		if (pwrout == USB_POWER_3V3) {
+			reg_value |= SET_BITS(regUSB1, VUSB, 1);
+			reg_mask |= SET_BITS(regUSB1, VUSB, 1);
+		}
+
+		else if (pwrout == USB_POWER_2V775) {
+			reg_value |= SET_BITS(regUSB1, VUSB, 0);
+			reg_mask |= SET_BITS(regUSB1, VUSB, 1);
+		}
+		rc = pmic_write_reg(REG_CHARGE_USB_SPARE, reg_value, reg_mask);
+
+		if (rc == PMIC_SUCCESS) {
+			usb.usbPowerIn = pwrin;
+			usb.usbPowerOut = pwrout;
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Get the USB transceiver's current power supply configuration.
+ *
+ * @param       handle          device handle from open() call
+ * @param       pwrin           USB transceiver regulator input power source
+ * @param       pwrout          USB transceiver regulator output power level
+ *
+ * @return      PMIC_SUCCESS    if the USB transceiver's power supply
+ *                              configuration was successfully retrieved
+ */
+PMIC_STATUS pmic_convity_usb_get_power_source(const PMIC_CONVITY_HANDLE handle,
+					      PMIC_CONVITY_USB_POWER_IN *
+					      const pwrin,
+					      PMIC_CONVITY_USB_POWER_OUT *
+					      const pwrout)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == usb.handle) &&
+	    (usb.handle_state == HANDLE_IN_USE) &&
+	    (pwrin != (PMIC_CONVITY_USB_POWER_IN *) NULL) &&
+	    (pwrout != (PMIC_CONVITY_USB_POWER_OUT *) NULL)) {
+		*pwrin = usb.usbPowerIn;
+		*pwrout = usb.usbPowerOut;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Set the USB transceiver's operating mode.
+ *
+ * @param       handle          device handle from open() call
+ * @param       mode            desired operating mode
+ *
+ * @return      PMIC_SUCCESS    if the USB transceiver's operating mode
+ *                              was successfully configured
+ */
+PMIC_STATUS pmic_convity_usb_set_xcvr(const PMIC_CONVITY_HANDLE handle,
+				      const PMIC_CONVITY_USB_TRANSCEIVER_MODE
+				      mode)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+	unsigned int reg_value = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == usb.handle) && (usb.handle_state == HANDLE_IN_USE)) {
+
+		if (mode == USB_TRANSCEIVER_OFF) {
+			reg_value = SET_BITS(regUSB0, USBXCVREN, 0);
+			reg_mask |= SET_BITS(regUSB0, USB_SUSPEND, 1);
+
+			rc = pmic_write_reg(REG_USB, reg_value, reg_mask);
+
+		}
+
+		if (mode == USB_SINGLE_ENDED_UNIDIR) {
+			reg_value |=
+			    SET_BITS(regUSB0, DATSE0, 1) | SET_BITS(regUSB0,
+								    BIDIR, 0);
+			reg_mask |=
+			    SET_BITS(regUSB0, USB_SUSPEND,
+				     1) | SET_BITS(regUSB0, DATSE0,
+						   1) | SET_BITS(regUSB0, BIDIR,
+								 1);
+		} else if (mode == USB_SINGLE_ENDED_BIDIR) {
+			reg_value |=
+			    SET_BITS(regUSB0, DATSE0, 1) | SET_BITS(regUSB0,
+								    BIDIR, 1);
+			reg_mask |=
+			    SET_BITS(regUSB0, USB_SUSPEND,
+				     1) | SET_BITS(regUSB0, DATSE0,
+						   1) | SET_BITS(regUSB0, BIDIR,
+								 1);
+		} else if (mode == USB_DIFFERENTIAL_UNIDIR) {
+			reg_value |=
+			    SET_BITS(regUSB0, DATSE0, 0) | SET_BITS(regUSB0,
+								    BIDIR, 0);
+			reg_mask |=
+			    SET_BITS(regUSB0, USB_SUSPEND,
+				     1) | SET_BITS(regUSB0, DATSE0,
+						   1) | SET_BITS(regUSB0, BIDIR,
+								 1);
+		} else if (mode == USB_DIFFERENTIAL_BIDIR) {
+			reg_value |=
+			    SET_BITS(regUSB0, DATSE0, 0) | SET_BITS(regUSB0,
+								    BIDIR, 1);
+			reg_mask |=
+			    SET_BITS(regUSB0, USB_SUSPEND,
+				     1) | SET_BITS(regUSB0, DATSE0,
+						   1) | SET_BITS(regUSB0, BIDIR,
+								 1);
+		}
+
+		if (mode == USB_SUSPEND_ON) {
+			reg_value |= SET_BITS(regUSB0, USB_SUSPEND, 1);
+			reg_mask |= SET_BITS(regUSB0, USB_SUSPEND, 1);
+		} else if (mode == USB_SUSPEND_OFF) {
+			reg_value |= SET_BITS(regUSB0, USB_SUSPEND, 0);
+			reg_mask |= SET_BITS(regUSB0, USB_SUSPEND, 1);
+		}
+
+		if (mode == USB_OTG_SRP_DLP_START) {
+			reg_value |= SET_BITS(regUSB0, USB_PU, 0);
+			reg_mask |= SET_BITS(regUSB0, USB_SUSPEND, 1);
+		} else if (mode == USB_OTG_SRP_DLP_STOP) {
+			reg_value &= SET_BITS(regUSB0, USB_PU, 1);
+			reg_mask |= SET_BITS(regUSB0, USB_PU, 1);
+		}
+
+		rc = pmic_write_reg(REG_USB, reg_value, reg_mask);
+
+		if (rc == PMIC_SUCCESS) {
+			usb.usbXcvrMode = mode;
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Get the USB transceiver's current operating mode.
+ *
+ * @param       handle          device handle from open() call
+ * @param       mode            current operating mode
+ *
+ * @return      PMIC_SUCCESS    if the USB transceiver's operating mode
+ *                              was successfully retrieved
+ */
+PMIC_STATUS pmic_convity_usb_get_xcvr(const PMIC_CONVITY_HANDLE handle,
+				      PMIC_CONVITY_USB_TRANSCEIVER_MODE *
+				      const mode)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == usb.handle) &&
+	    (usb.handle_state == HANDLE_IN_USE) &&
+	    (mode != (PMIC_CONVITY_USB_TRANSCEIVER_MODE *) NULL)) {
+		*mode = usb.usbXcvrMode;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Set the Data Line Pulse duration (in milliseconds) for the USB OTG
+ * Session Request Protocol.
+ *
+ * For mc13783, this feature is not supported.So return PMIC_NOT_SUPPORTED
+ *
+ * @param       handle          device handle from open() call
+ * @param       duration        the data line pulse duration (ms)
+ *
+ * @return      PMIC_SUCCESS    if the pulse duration was successfully set
+ */
+PMIC_STATUS pmic_convity_usb_otg_set_dlp_duration(const PMIC_CONVITY_HANDLE
+						  handle,
+						  const unsigned int duration)
+{
+	PMIC_STATUS rc = PMIC_NOT_SUPPORTED;
+
+	/* The setting of the dlp duration is not supported by the mc13783 PMIC hardware. */
+
+	/* No critical section is required. */
+
+	if ((handle != usb.handle)
+	    || (usb.handle_state != HANDLE_IN_USE)) {
+		/* Must return error indication for invalid handle parameter to be
+		 * consistent with other APIs.
+		 */
+		rc = PMIC_ERROR;
+	}
+
+	return rc;
+}
+
+/*!
+ * Get the current Data Line Pulse duration (in milliseconds) for the USB
+ * OTG Session Request Protocol.
+ *
+ * @param       handle          device handle from open() call
+ * @param       duration        the data line pulse duration (ms)
+ *
+ * @return      PMIC_SUCCESS    if the pulse duration was successfully obtained
+ */
+PMIC_STATUS pmic_convity_usb_otg_get_dlp_duration(const PMIC_CONVITY_HANDLE
+						  handle,
+						  unsigned int *const duration)
+{
+	PMIC_STATUS rc = PMIC_NOT_SUPPORTED;
+
+	/* The setting of dlp duration is not supported by the mc13783 PMIC hardware. */
+
+	/* No critical section is required. */
+
+	if ((handle != usb.handle)
+	    || (usb.handle_state != HANDLE_IN_USE)) {
+		/* Must return error indication for invalid handle parameter to be
+		 * consistent with other APIs.
+		 */
+		rc = PMIC_ERROR;
+	}
+
+	return rc;
+}
+
+/*!
+ * Set the USB On-The-Go (OTG) configuration.
+ *
+ * @param       handle          device handle from open() call
+ * @param       cfg             desired USB OTG configuration
+ *
+ * @return      PMIC_SUCCESS    if the OTG configuration was successfully set
+ */
+PMIC_STATUS pmic_convity_usb_otg_set_config(const PMIC_CONVITY_HANDLE handle,
+					    const PMIC_CONVITY_USB_OTG_CONFIG
+					    cfg)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+	unsigned int reg_value = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == usb.handle) && (usb.handle_state == HANDLE_IN_USE)) {
+		if (cfg & USB_OTG_SE0CONN) {
+			reg_value = SET_BITS(regUSB0, SE0_CONN, 1);
+			reg_mask = SET_BITS(regUSB0, SE0_CONN, 1);
+		}
+		if (cfg & USBXCVREN) {
+			reg_value |= SET_BITS(regUSB0, USBXCVREN, 1);
+			reg_mask |= SET_BITS(regUSB0, USBXCVREN, 1);
+		}
+
+		if (cfg & USB_OTG_DLP_SRP) {
+			reg_value |= SET_BITS(regUSB0, DLP_SRP, 1);
+			reg_mask |= SET_BITS(regUSB0, DLP_SRP, 1);
+		}
+
+		if (cfg & USB_PULL_OVERRIDE) {
+			reg_value |= SET_BITS(regUSB0, PULLOVR, 1);
+			reg_mask |= SET_BITS(regUSB0, PULLOVR, 1);
+		}
+
+		if (cfg & USB_PU) {
+			reg_value |= SET_BITS(regUSB0, USB_PU, 1);
+			reg_mask |= SET_BITS(regUSB0, USB_PU, 1);
+		}
+
+		if (cfg & USB_UDM_PD) {
+			reg_value |= SET_BITS(regUSB0, UDM_PD, 1);
+			reg_mask |= SET_BITS(regUSB0, UDM_PD, 1);
+		}
+
+		if (cfg & USB_UDP_PD) {
+			reg_value |= SET_BITS(regUSB0, UDP_PD, 1);
+			reg_mask |= SET_BITS(regUSB0, UDP_PD, 1);
+		}
+
+		if (cfg & USB_DP150K_PU) {
+			reg_value |= SET_BITS(regUSB0, DP150K_PU, 1);
+			reg_mask |= SET_BITS(regUSB0, DP150K_PU, 1);
+		}
+
+		if (cfg & USB_USBCNTRL) {
+			reg_value |= SET_BITS(regUSB0, USBCNTRL, 1);
+			reg_mask |= SET_BITS(regUSB0, USBCNTRL, 1);
+		}
+
+		if (cfg & USB_VBUS_CURRENT_LIMIT_HIGH) {
+			reg_value |= SET_BITS(regUSB0, CURRENT_LIMIT, 0);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_10MS) {
+			reg_value |= SET_BITS(regUSB0, CURRENT_LIMIT, 1);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_20MS) {
+			reg_value |= SET_BITS(regUSB0, CURRENT_LIMIT, 2);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_30MS) {
+			reg_value |= SET_BITS(regUSB0, CURRENT_LIMIT, 3);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_40MS) {
+			reg_value |= SET_BITS(regUSB0, CURRENT_LIMIT, 4);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_50MS) {
+			reg_value |= SET_BITS(regUSB0, CURRENT_LIMIT, 5);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_60MS) {
+			reg_value |= SET_BITS(regUSB0, CURRENT_LIMIT, 6);
+		}
+		if (cfg & USB_VBUS_CURRENT_LIMIT_LOW) {
+			reg_value |= SET_BITS(regUSB0, CURRENT_LIMIT, 7);
+			reg_mask |= SET_BITS(regUSB0, CURRENT_LIMIT, 7);
+		}
+
+		if (cfg & USB_VBUS_PULLDOWN) {
+			reg_value |= SET_BITS(regUSB0, VBUSPDENB, 1);
+			reg_mask |= SET_BITS(regUSB0, VBUSPDENB, 1);
+		}
+
+		rc = pmic_write_reg(REG_USB, reg_value, reg_mask);
+
+		if (rc == PMIC_SUCCESS) {
+			if ((cfg & USB_VBUS_CURRENT_LIMIT_HIGH) ||
+			    (cfg & USB_VBUS_CURRENT_LIMIT_LOW) ||
+			    (cfg & USB_VBUS_CURRENT_LIMIT_LOW_10MS) ||
+			    (cfg & USB_VBUS_CURRENT_LIMIT_LOW_20MS) ||
+			    (cfg & USB_VBUS_CURRENT_LIMIT_LOW_30MS) ||
+			    (cfg & USB_VBUS_CURRENT_LIMIT_LOW_40MS) ||
+			    (cfg & USB_VBUS_CURRENT_LIMIT_LOW_50MS) ||
+			    (cfg & USB_VBUS_CURRENT_LIMIT_LOW_60MS)) {
+				/* Make sure that the VBUS current limit state is
+				 * correctly set to either USB_VBUS_CURRENT_LIMIT_HIGH
+				 * or USB_VBUS_CURRENT_LIMIT_LOW but never both at the
+				 * same time.
+				 *
+				 * We guarantee this by first clearing both of the
+				 * status bits and then resetting the correct one.
+				 */
+				usb.usbOtgCfg &=
+				    ~(USB_VBUS_CURRENT_LIMIT_HIGH |
+				      USB_VBUS_CURRENT_LIMIT_LOW |
+				      USB_VBUS_CURRENT_LIMIT_LOW_10MS |
+				      USB_VBUS_CURRENT_LIMIT_LOW_20MS |
+				      USB_VBUS_CURRENT_LIMIT_LOW_30MS |
+				      USB_VBUS_CURRENT_LIMIT_LOW_40MS |
+				      USB_VBUS_CURRENT_LIMIT_LOW_50MS |
+				      USB_VBUS_CURRENT_LIMIT_LOW_60MS);
+			}
+
+			usb.usbOtgCfg |= cfg;
+		}
+	}
+	//}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Clears the USB On-The-Go (OTG) configuration. Multiple configuration settings
+ * may be OR'd together in a single call. However, selecting conflicting
+ * settings (e.g., multiple VBUS current limits) will result in undefined
+ * behavior.
+ *
+ * @param   handle          Device handle from open() call.
+ * @param   cfg             USB OTG configuration settings to be cleared.
+ *
+ * @retval      PMIC_SUCCESS         If the OTG configuration was successfully
+ *                                   cleared.
+ * @retval      PMIC_PARAMETER_ERROR If the handle is invalid.
+ * @retval      PMIC_NOT_SUPPORTED   If the desired USB OTG configuration is
+ *                                   not supported by the PMIC hardware.
+ */
+PMIC_STATUS pmic_convity_usb_otg_clear_config(const PMIC_CONVITY_HANDLE handle,
+					      const PMIC_CONVITY_USB_OTG_CONFIG
+					      cfg)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+	unsigned int reg_value = 0;
+	unsigned int reg_mask = 0;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == usb.handle) && (usb.handle_state == HANDLE_IN_USE)) {
+		/* if ((cfg & USB_VBUS_CURRENT_LIMIT_LOW_10MS) ||
+		   (cfg & USB_VBUS_CURRENT_LIMIT_LOW_20MS) ||
+		   (cfg & USB_VBUS_CURRENT_LIMIT_LOW_30MS) ||
+		   (cfg & USB_VBUS_CURRENT_LIMIT_LOW_40MS) ||
+		   (cfg & USB_VBUS_CURRENT_LIMIT_LOW_50MS) ||
+		   (cfg & USB_VBUS_CURRENT_LIMIT_LOW_60MS))
+		   {
+		   rc = PMIC_NOT_SUPPORTED;
+		   } */
+		//else
+
+		if (cfg & USB_OTG_SE0CONN) {
+			reg_mask = SET_BITS(regUSB0, SE0_CONN, 1);
+		}
+
+		if (cfg & USB_OTG_DLP_SRP) {
+			reg_mask |= SET_BITS(regUSB0, DLP_SRP, 1);
+		}
+
+		if (cfg & USB_DP150K_PU) {
+			reg_mask |= SET_BITS(regUSB0, DP150K_PU, 1);
+		}
+
+		if (cfg & USB_PULL_OVERRIDE) {
+			reg_mask |= SET_BITS(regUSB0, PULLOVR, 1);
+		}
+
+		if (cfg & USB_PU) {
+
+			reg_mask |= SET_BITS(regUSB0, USB_PU, 1);
+		}
+
+		if (cfg & USB_UDM_PD) {
+
+			reg_mask |= SET_BITS(regUSB0, UDM_PD, 1);
+		}
+
+		if (cfg & USB_UDP_PD) {
+
+			reg_mask |= SET_BITS(regUSB0, UDP_PD, 1);
+		}
+
+		if (cfg & USB_USBCNTRL) {
+			reg_mask |= SET_BITS(regUSB0, USBCNTRL, 1);
+		}
+
+		if (cfg & USB_VBUS_CURRENT_LIMIT_HIGH) {
+			reg_mask |= SET_BITS(regUSB0, CURRENT_LIMIT, 0);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_10MS) {
+			reg_mask |= SET_BITS(regUSB0, CURRENT_LIMIT, 1);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_20MS) {
+			reg_mask |= SET_BITS(regUSB0, CURRENT_LIMIT, 2);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_30MS) {
+			reg_mask |= SET_BITS(regUSB0, CURRENT_LIMIT, 3);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_40MS) {
+			reg_mask |= SET_BITS(regUSB0, CURRENT_LIMIT, 4);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_50MS) {
+			reg_mask |= SET_BITS(regUSB0, CURRENT_LIMIT, 5);
+		} else if (cfg & USB_VBUS_CURRENT_LIMIT_LOW_60MS) {
+			reg_mask |= SET_BITS(regUSB0, CURRENT_LIMIT, 6);
+		}
+
+		if (cfg & USB_VBUS_PULLDOWN) {
+			//      reg_value |= SET_BITS(regUSB0, VBUSPDENB, 1);
+			reg_mask |= SET_BITS(regUSB0, VBUSPDENB, 1);
+		}
+
+		rc = pmic_write_reg(REG_USB, reg_value, reg_mask);
+
+		if (rc == PMIC_SUCCESS) {
+			usb.usbOtgCfg &= ~cfg;
+		}
+	}
+	//}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Get the current USB On-The-Go (OTG) configuration.
+ *
+ * @param       handle          device handle from open() call
+ * @param       cfg             the current USB OTG configuration
+ *
+ * @return      PMIC_SUCCESS    if the OTG configuration was successfully
+ *                              retrieved
+ */
+PMIC_STATUS pmic_convity_usb_otg_get_config(const PMIC_CONVITY_HANDLE handle,
+					    PMIC_CONVITY_USB_OTG_CONFIG *
+					    const cfg)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == usb.handle) &&
+	    (usb.handle_state == HANDLE_IN_USE) &&
+	    (cfg != (PMIC_CONVITY_USB_OTG_CONFIG *) NULL)) {
+		*cfg = usb.usbOtgCfg;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*@}*/
+
+/**************************************************************************
+ * RS-232-specific configuration and setup functions.
+ **************************************************************************
+ */
+
+/*!
+ * @name RS-232 Serial Connectivity APIs
+ * Functions for controlling RS-232 serial connectivity.
+ */
+/*@{*/
+
+/*!
+ * Set the connectivity interface to the selected RS-232 operating
+ * configuration. Note that the RS-232 operating mode will be automatically
+ * overridden if the USB_EN is asserted at any time (e.g., when a USB device
+ * is attached). However, we will also automatically return to the RS-232
+ * mode once the USB device is detached.
+ *
+ * @param       handle          device handle from open() call
+ * @param       cfgInternal     RS-232 transceiver internal connections
+ * @param       cfgExternal     RS-232 transceiver external connections
+ *
+ * @return      PMIC_SUCCESS    if the requested mode was set
+ */
+PMIC_STATUS pmic_convity_rs232_set_config(const PMIC_CONVITY_HANDLE handle,
+					  const PMIC_CONVITY_RS232_INTERNAL
+					  cfgInternal,
+					  const PMIC_CONVITY_RS232_EXTERNAL
+					  cfgExternal)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+	unsigned int reg_value0 = 0, reg_value1 = 0;
+	unsigned int reg_mask = SET_BITS(regUSB1, RSPOL, 1);
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == rs_232.handle) && (rs_232.handle_state == HANDLE_IN_USE)) {
+		rc = PMIC_SUCCESS;
+
+		/* Validate the calling parameters. */
+		/*if ((cfgInternal !=  RS232_TX_USE0VM_RX_UDATVP) &&
+		   (cfgInternal != RS232_TX_RX_INTERNAL_DEFAULT) && (cfgInternal !=  RS232_TX_UDATVP_RX_URXVM))
+		   {
+
+		   rc = PMIC_NOT_SUPPORTED;
+		   } */
+		if (cfgInternal == RS232_TX_USE0VM_RX_UDATVP) {
+
+			reg_value0 = SET_BITS(regUSB0, INTERFACE_MODE, 1);
+
+		} else if (cfgInternal == RS232_TX_RX_INTERNAL_DEFAULT) {
+
+			reg_value0 = SET_BITS(regUSB0, INTERFACE_MODE, 1);
+			reg_mask |= SET_BITS(regUSB1, RSPOL, 1);
+
+		} else if (cfgInternal == RS232_TX_UDATVP_RX_URXVM) {
+
+			reg_value0 = SET_BITS(regUSB0, INTERFACE_MODE, 2);
+			reg_value1 |= SET_BITS(regUSB1, RSPOL, 1);
+
+		} else if ((cfgExternal == RS232_TX_UDM_RX_UDP) ||
+			   (cfgExternal == RS232_TX_RX_EXTERNAL_DEFAULT)) {
+			/* Configure for TX on D+ and RX on D-. */
+			reg_value0 |= SET_BITS(regUSB0, INTERFACE_MODE, 1);
+			reg_value1 |= SET_BITS(regUSB1, RSPOL, 0);
+		} else if (cfgExternal != RS232_TX_UDM_RX_UDP) {
+			/* Any other RS-232 configuration is an error. */
+			rc = PMIC_ERROR;
+		}
+
+		if (rc == PMIC_SUCCESS) {
+			/* Configure for TX on D- and RX on D+. */
+			rc = pmic_write_reg(REG_USB, reg_value0, reg_mask);
+
+			rc = pmic_write_reg(REG_CHARGE_USB_SPARE,
+					    reg_value1, reg_mask);
+
+			if (rc == PMIC_SUCCESS) {
+				rs_232.rs232CfgInternal = cfgInternal;
+				rs_232.rs232CfgExternal = cfgExternal;
+			}
+		}
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*!
+ * Get the connectivity interface's current RS-232 operating configuration.
+ *
+ * @param       handle          device handle from open() call
+ * @param       cfgInternal     RS-232 transceiver internal connections
+ * @param       cfgExternal     RS-232 transceiver external connections
+ *
+ * @return      PMIC_SUCCESS    if the requested mode was retrieved
+ */
+PMIC_STATUS pmic_convity_rs232_get_config(const PMIC_CONVITY_HANDLE handle,
+					  PMIC_CONVITY_RS232_INTERNAL *
+					  const cfgInternal,
+					  PMIC_CONVITY_RS232_EXTERNAL *
+					  const cfgExternal)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle == rs_232.handle) &&
+	    (rs_232.handle_state == HANDLE_IN_USE) &&
+	    (cfgInternal != (PMIC_CONVITY_RS232_INTERNAL *) NULL) &&
+	    (cfgExternal != (PMIC_CONVITY_RS232_EXTERNAL *) NULL)) {
+		*cfgInternal = rs_232.rs232CfgInternal;
+		*cfgExternal = rs_232.rs232CfgExternal;
+
+		rc = PMIC_SUCCESS;
+	}
+
+	/* Exit the critical section. */
+	up(&mutex);
+
+	return rc;
+}
+
+/*@}*/
+
+/**************************************************************************
+ * CEA-936-specific configuration and setup functions.
+ **************************************************************************
+ */
+
+/*!
+ * @name CEA-936 Connectivity APIs
+ * Functions for controlling CEA-936 connectivity.
+ */
+/*@{*/
+
+/*!
+ * Signal the attached device to exit the current CEA-936 operating mode.
+ * Returns an error if the current operating mode is not CEA-936.
+ *
+ * @param       handle          device handle from open() call
+ * @param       signal          type of exit signal to be sent
+ *
+ * @return      PMIC_SUCCESS    if exit signal was sent
+ */
+PMIC_STATUS pmic_convity_cea936_exit_signal(const PMIC_CONVITY_HANDLE handle,
+					    const
+					    PMIC_CONVITY_CEA936_EXIT_SIGNAL
+					    signal)
+{
+	PMIC_STATUS rc = PMIC_ERROR;
+	unsigned int reg_value = 0;
+	unsigned int reg_mask =
+	    SET_BITS(regUSB0, IDPD, 1) | SET_BITS(regUSB0, IDPULSE, 1);
+
+	/* Use a critical section to maintain a consistent state. */
+	if (down_interruptible(&mutex))
+		return PMIC_SYSTEM_ERROR_EINTR;
+
+	if ((handle != cea_936.handle)
+	    || (cea_936.handle_state != HANDLE_IN_USE)) {
+		/* Must return error indication for invalid handle parameter to be
+		 * consistent with other APIs.
+		 */
+		rc = PMIC_ERROR;
+	} else if (signal == CEA936_UID_PULLDOWN_6MS) {
+		reg_value =
+		    SET_BITS(regUSB0, IDPULSE, 0) | SET_BITS(regUSB0, IDPD, 0);
+	} else if (signal == CEA936_UID_PULLDOWN_6MS) {
+		reg_value = SET_BITS(regUSB0, IDPULSE, 1);
+	} else if (signal == CEA936_UID_PULLDOWN) {
+		reg_value = SET_BITS(regUSB0, IDPD, 1);
+	} else if (signal == CEA936_UDMPULSE) {
+		reg_value = SET_BITS(regUSB0, DMPULSE, 1);
+	}
+
+	rc = pmic_write_reg(REG_USB, reg_value, reg_mask);
+
+	up(&mutex);
+
+	return rc;
+}
+
+/*@}*/
+
+/**************************************************************************
+ * Static functions.
+ **************************************************************************
+ */
+
+/*!
+ * @name Connectivity Driver Internal Support Functions
+ * These non-exported internal functions are used to support the functionality
+ * of the exported connectivity APIs.
+ */
+/*@{*/
+
+/*!
+ * This internal helper function sets the desired operating mode (either USB
+ * OTG or RS-232). It must be called with the mutex already acquired.
+ *
+ * @param       mode               the desired operating mode (USB or RS232)
+ *
+ * @return      PMIC_SUCCESS       if the desired operating mode was set
+ * @return      PMIC_NOT_SUPPORTED if the desired operating mode is invalid
+ */
+static PMIC_STATUS pmic_convity_set_mode_internal(const PMIC_CONVITY_MODE mode)
+{
+	unsigned int reg_value0 = 0, reg_value1 = 0;
+	unsigned int reg_mask0 = 0, reg_mask1 = 0;
+
+	//unsigned int reg_mask1 = SET_BITS(regUSB1, VBUSEN, 1);
+
+	PMIC_STATUS rc = PMIC_SUCCESS;
+
+	switch (mode) {
+	case USB:
+		/* For the USB mode, we start by tri-stating the USB bus (by
+		 * setting VBUSEN = 0) until a device is connected (i.e.,
+		 * until we receive a 4.4V rising edge event). All pull-up
+		 * and pull-down resistors are also disabled until a USB
+		 * device is actually connected and we have determined which
+		 * device is the host and the desired USB bus speed.
+		 *
+		 * Also tri-state the RS-232 buffers (by setting RSTRI = 1).
+		 * This prevents the hardware from automatically returning to
+		 * the RS-232 mode when the USB device is detached.
+		 */
+
+		reg_value0 = SET_BITS(regUSB0, INTERFACE_MODE, 0);
+		reg_mask0 = SET_BITS(regUSB0, INTERFACE_MODE, 7);
+
+		/*reg_value1 = SET_BITS(regUSB1, RSTRI, 1); */
+
+		rc = pmic_write_reg(REG_USB, reg_value0, reg_mask0);
+		/*      if (rc == PMIC_SUCCESS) {
+		   CHECK_ERROR(pmic_write_reg
+		   (REG_CHARGE_USB_SPARE,
+		   reg_value1, reg_mask1));
+		   } */
+
+		break;
+
+	case RS232_1:
+		/* For the RS-232 mode, we tri-state the USB bus (by setting
+		 * VBUSEN = 0) and enable the RS-232 transceiver (by setting
+		 * RS232ENB = 0).
+		 *
+		 * Note that even in the RS-232 mode, if a USB device is
+		 * plugged in, we will receive a 4.4V rising edge event which
+		 * will automatically disable the RS-232 transceiver and
+		 * tri-state the RS-232 buffers. This allows us to temporarily
+		 * switch over to USB mode while the USB device is attached.
+		 * The RS-232 transceiver and buffers will be automatically
+		 * re-enabled when the USB device is detached.
+		 */
+
+		/* Explicitly disconnect all of the USB pull-down resistors
+		 * and the VUSB power regulator here just to be safe.
+		 *
+		 * But we do connect the internal pull-up resistor on USB_D+
+		 * to avoid having an extra load on the USB_D+ line when in
+		 * RS-232 mode.
+		 */
+
+		reg_value0 |= SET_BITS(regUSB0, INTERFACE_MODE, 1) |
+		    SET_BITS(regUSB0, VBUSPDENB, 1) |
+		    SET_BITS(regUSB0, USB_PU, 1);
+		reg_mask0 =
+		    SET_BITS(regUSB0, INTERFACE_MODE, 7) | SET_BITS(regUSB0,
+								    VBUSPDENB,
+								    1) |
+		    SET_BITS(regUSB0, USB_PU, 1);
+
+		reg_value1 = SET_BITS(regUSB1, RSPOL, 0);
+
+		rc = pmic_write_reg(REG_USB, reg_value0, reg_mask0);
+
+		if (rc == PMIC_SUCCESS) {
+			CHECK_ERROR(pmic_write_reg
+				    (REG_CHARGE_USB_SPARE,
+				     reg_value1, reg_mask1));
+		}
+		break;
+
+	case RS232_2:
+		reg_value0 |= SET_BITS(regUSB0, INTERFACE_MODE, 2) |
+		    SET_BITS(regUSB0, VBUSPDENB, 1) |
+		    SET_BITS(regUSB0, USB_PU, 1);
+		reg_mask0 =
+		    SET_BITS(regUSB0, INTERFACE_MODE, 7) | SET_BITS(regUSB0,
+								    VBUSPDENB,
+								    1) |
+		    SET_BITS(regUSB0, USB_PU, 1);
+
+		reg_value1 = SET_BITS(regUSB1, RSPOL, 1);
+
+		rc = pmic_write_reg(REG_USB, reg_value0, reg_mask0);
+
+		if (rc == PMIC_SUCCESS) {
+			CHECK_ERROR(pmic_write_reg
+				    (REG_CHARGE_USB_SPARE,
+				     reg_value1, reg_mask1));
+		}
+		break;
+
+	case CEA936_MONO:
+		reg_value0 |= SET_BITS(regUSB0, INTERFACE_MODE, 4);
+
+		rc = pmic_write_reg(REG_USB, reg_value0, reg_mask0);
+		break;
+
+	case CEA936_STEREO:
+		reg_value0 |= SET_BITS(regUSB0, INTERFACE_MODE, 5);
+		reg_mask0 = SET_BITS(regUSB0, INTERFACE_MODE, 7);
+
+		rc = pmic_write_reg(REG_USB, reg_value0, reg_mask0);
+		break;
+
+	case CEA936_TEST_RIGHT:
+		reg_value0 |= SET_BITS(regUSB0, INTERFACE_MODE, 6);
+		reg_mask0 = SET_BITS(regUSB0, INTERFACE_MODE, 7);
+
+		rc = pmic_write_reg(REG_USB, reg_value0, reg_mask0);
+		break;
+
+	case CEA936_TEST_LEFT:
+		reg_value0 |= SET_BITS(regUSB0, INTERFACE_MODE, 7);
+		reg_mask0 = SET_BITS(regUSB0, INTERFACE_MODE, 7);
+
+		rc = pmic_write_reg(REG_USB, reg_value0, reg_mask0);
+		break;
+
+	default:
+		rc = PMIC_NOT_SUPPORTED;
+	}
+
+	if (rc == PMIC_SUCCESS) {
+		if (mode == USB) {
+			usb.mode = mode;
+		} else if ((mode == RS232_1) || (mode == RS232_1)) {
+			rs_232.mode = mode;
+		} else if ((mode == CEA936_MONO) || (mode == CEA936_STEREO) ||
+			   (mode == CEA936_TEST_RIGHT)
+			   || (mode == CEA936_TEST_LEFT)) {
+			cea_936.mode = mode;
+		}
+	}
+
+	return rc;
+}
+
+/*!
+ * This internal helper function deregisters all of the currently registered
+ * callback events. It must be called with the mutual exclusion spinlock
+ * already acquired.
+ *
+ * We've defined the event and callback deregistration code here as a separate
+ * function because it can be called by either the pmic_convity_close() or the
+ * pmic_convity_clear_callback() APIs. We also wanted to avoid any possible
+ * issues with having the same thread calling spin_lock_irq() twice.
+ *
+ * Note that the mutex must have already been acquired. We will also acquire
+ * the spinlock here to avoid any possible race conditions with the interrupt
+ * handler.
+ *
+ * @return      PMIC_SUCCESS    if all of the callback events were cleared
+ */
+static PMIC_STATUS pmic_convity_deregister_all(void)
+{
+	unsigned long flags;
+	PMIC_STATUS rc = PMIC_SUCCESS;
+
+	/* Deregister each of the PMIC events that we had previously
+	 * registered for by using pmic_event_subscribe().
+	 */
+
+	if ((usb.eventMask & USB_DETECT_MINI_A) ||
+	    (usb.eventMask & USB_DETECT_MINI_B) ||
+	    (usb.eventMask & USB_DETECT_NON_USB_ACCESSORY) ||
+	    (usb.eventMask & USB_DETECT_FACTORY_MODE)) {
+		//EVENT_AB_DETI or EVENT_IDI
+		eventNotify.func = pmic_convity_event_handler;
+		eventNotify.param = (void *)(CORE_EVENT_ABDET);
+
+		if (pmic_event_unsubscribe(EVENT_IDI, eventNotify) ==
+		    PMIC_SUCCESS) {
+			/* Also acquire the spinlock here to avoid any possible race
+			 * conditions with the interrupt handler.
+			 */
+
+			spin_lock_irqsave(&lock, flags);
+
+			usb.eventMask &= ~(USB_DETECT_MINI_A |
+					   USB_DETECT_MINI_B |
+					   USB_DETECT_NON_USB_ACCESSORY |
+					   USB_DETECT_FACTORY_MODE);
+
+			spin_unlock_irqrestore(&lock, flags);
+		} else {
+			pr_debug
+			    ("%s: pmic_event_unsubscribe() for EVENT_AB_DETI failed\n",
+			     __FILE__);
+			rc = PMIC_ERROR;
+		}
+	}
+
+	else if ((usb.eventMask & USB_DETECT_0V8_RISE) ||
+		 (usb.eventMask & USB_DETECT_0V8_FALL)) {
+		//EVENT_USB_08VI or EVENT_USBI
+		eventNotify.func = pmic_convity_event_handler;
+		eventNotify.param = (void *)(CORE_EVENT_0V8);
+		if (pmic_event_unsubscribe(EVENT_USBI, eventNotify) ==
+		    PMIC_SUCCESS) {
+			/* Also acquire the spinlock here to avoid any possible race
+			 * conditions with the interrupt handler.
+			 */
+			spin_lock_irqsave(&lock, flags);
+
+			usb.eventMask &= ~(USB_DETECT_0V8_RISE |
+					   USB_DETECT_0V8_FALL);
+
+			spin_unlock_irqrestore(&lock, flags);
+		} else {
+			pr_debug
+			    ("%s: pmic_event_unsubscribe() for EVENT_USB_08VI failed\n",
+			     __FILE__);
+			rc = PMIC_ERROR;
+		}
+
+	}
+
+	else if ((usb.eventMask & USB_DETECT_2V0_RISE) ||
+		 (usb.eventMask & USB_DETECT_2V0_FALL)) {
+		//EVENT_USB_20VI or EVENT_USBI
+		eventNotify.func = pmic_convity_event_handler;
+		eventNotify.param = (void *)(CORE_EVENT_2V0);
+		if (pmic_event_unsubscribe(EVENT_USBI, eventNotify) ==
+		    PMIC_SUCCESS) {
+			/* Also acquire the spinlock here to avoid any possible race
+			 * conditions with the interrupt handler.
+			 */
+			spin_lock_irqsave(&lock, flags);
+
+			usb.eventMask &= ~(USB_DETECT_2V0_RISE |
+					   USB_DETECT_2V0_FALL);
+
+			spin_unlock_irqrestore(&lock, flags);
+		} else {
+			pr_debug
+			    ("%s: pmic_event_unsubscribe() for EVENT_USB_20VI failed\n",
+			     __FILE__);
+			rc = PMIC_ERROR;
+		}
+	}
+
+	else if ((usb.eventMask & USB_DETECT_4V4_RISE) ||
+		 (usb.eventMask & USB_DETECT_4V4_FALL)) {
+
+		//EVENT_USB_44VI or EVENT_USBI
+		eventNotify.func = pmic_convity_event_handler;
+		eventNotify.param = (void *)(CORE_EVENT_4V4);
+
+		if (pmic_event_unsubscribe(EVENT_USBI, eventNotify) ==
+		    PMIC_SUCCESS) {
+
+			/* Also acquire the spinlock here to avoid any possible race
+			 * conditions with the interrupt handler.
+			 */
+			spin_lock_irqsave(&lock, flags);
+
+			usb.eventMask &= ~(USB_DETECT_4V4_RISE |
+					   USB_DETECT_4V4_FALL);
+
+			spin_unlock_irqrestore(&lock, flags);
+		} else {
+			pr_debug
+			    ("%s: pmic_event_unsubscribe() for EVENT_USB_44VI failed\n",
+			     __FILE__);
+			rc = PMIC_ERROR;
+		}
+	}
+
+	if (rc == PMIC_SUCCESS) {
+		/* Also acquire the spinlock here to avoid any possible race
+		 * conditions with the interrupt handler.
+		 */
+		spin_lock_irqsave(&lock, flags);
+
+		/* Restore the initial reset values for the callback function
+		 * and event mask parameters. This should be NULL and zero,
+		 * respectively.
+		 *
+		 * Note that we wait until the end here to fully reset everything
+		 * just in case some of the pmic_event_unsubscribe() calls above
+		 * failed for some reason (which normally shouldn't happen).
+		 */
+		usb.callback = reset.callback;
+		usb.eventMask = reset.eventMask;
+
+		spin_unlock_irqrestore(&lock, flags);
+	}
+	return rc;
+}
+
+/*!
+ * This is the default event handler for all connectivity-related events
+ * and hardware interrupts.
+ *
+ * @param       param           event ID
+ */
+static void pmic_convity_event_handler(void *param)
+{
+	unsigned long flags;
+
+	/* Update the global list of active interrupt events. */
+	spin_lock_irqsave(&lock, flags);
+	eventID |= (PMIC_CORE_EVENT) (param);
+	spin_unlock_irqrestore(&lock, flags);
+
+	/* Schedule the tasklet to be run as soon as it is convenient to do so. */
+	schedule_work(&convityTasklet);
+}
+
+/*!
+ * @brief This is the connectivity driver tasklet that handles interrupt events.
+ *
+ * This function is scheduled by the connectivity driver interrupt handler
+ * pmic_convity_event_handler() to complete the processing of all of the
+ * connectivity-related interrupt events.
+ *
+ * Since this tasklet runs with interrupts enabled, we can safely call
+ * the ADC driver, if necessary, to properly detect the type of USB connection
+ * that is being made and to call any user-registered callback functions.
+ *
+ * @param   arg                The parameter that was provided above in
+ *                                  the DECLARE_TASKLET() macro (unused).
+ */
+static void pmic_convity_tasklet(struct work_struct *work)
+{
+
+	PMIC_CONVITY_EVENTS activeEvents = 0;
+	unsigned long flags = 0;
+
+	/* Check the interrupt sense bits to determine exactly what
+	 * event just occurred.
+	 */
+	if (eventID & CORE_EVENT_4V4) {
+		spin_lock_irqsave(&lock, flags);
+		eventID &= ~CORE_EVENT_4V4;
+		spin_unlock_irqrestore(&lock, flags);
+
+		activeEvents |= pmic_check_sensor(SENSE_USB4V4S) ?
+		    USB_DETECT_4V4_RISE : USB_DETECT_4V4_FALL;
+
+		if (activeEvents & ~usb.eventMask) {
+			/* The default handler for 4.4 V rising/falling edge detection
+			 * is to simply ignore the event.
+			 */
+			;
+		}
+	}
+	if (eventID & CORE_EVENT_2V0) {
+		spin_lock_irqsave(&lock, flags);
+		eventID &= ~CORE_EVENT_2V0;
+		spin_unlock_irqrestore(&lock, flags);
+
+		activeEvents |= pmic_check_sensor(SENSE_USB2V0S) ?
+		    USB_DETECT_2V0_RISE : USB_DETECT_2V0_FALL;
+		if (activeEvents & ~usb.eventMask) {
+			/* The default handler for 2.0 V rising/falling edge detection
+			 * is to simply ignore the event.
+			 */
+			;
+		}
+	}
+	if (eventID & CORE_EVENT_0V8) {
+		spin_lock_irqsave(&lock, flags);
+		eventID &= ~CORE_EVENT_0V8;
+		spin_unlock_irqrestore(&lock, flags);
+
+		activeEvents |= pmic_check_sensor(SENSE_USB0V8S) ?
+		    USB_DETECT_0V8_RISE : USB_DETECT_0V8_FALL;
+
+		if (activeEvents & ~usb.eventMask) {
+			/* The default handler for 0.8 V rising/falling edge detection
+			 * is to simply ignore the event.
+			 */
+			;
+		}
+	}
+	if (eventID & CORE_EVENT_ABDET) {
+		spin_lock_irqsave(&lock, flags);
+		eventID &= ~CORE_EVENT_ABDET;
+		spin_unlock_irqrestore(&lock, flags);
+
+		activeEvents |= pmic_check_sensor(SENSE_ID_GNDS) ?
+		    USB_DETECT_MINI_A : 0;
+
+		activeEvents |= pmic_check_sensor(SENSE_ID_FLOATS) ?
+		    USB_DETECT_MINI_B : 0;
+	}
+
+	/* Begin a critical section here so that we don't register/deregister
+	 * for events or open/close the connectivity driver while the existing
+	 * event handler (if it is currently defined) is in the middle of handling
+	 * the current event.
+	 */
+	spin_lock_irqsave(&lock, flags);
+
+	/* Finally, call the user-defined callback function if required. */
+	if ((usb.handle_state == HANDLE_IN_USE) &&
+	    (usb.callback != NULL) && (activeEvents & usb.eventMask)) {
+		(*usb.callback) (activeEvents);
+	}
+
+	spin_unlock_irqrestore(&lock, flags);
+}
+
+/*@}*/
+
+/**************************************************************************
+ * Module initialization and termination functions.
+ *
+ * Note that if this code is compiled into the kernel, then the
+ * module_init() function will be called within the device_initcall()
+ * group.
+ **************************************************************************
+ */
+
+/*!
+ * @name Connectivity Driver Loading/Unloading Functions
+ * These non-exported internal functions are used to support the connectivity
+ * device driver initialization and de-initialization operations.
+ */
+/*@{*/
+
+/*!
+ * @brief This is the connectivity device driver initialization function.
+ *
+ * This function is called by the kernel when this device driver is first
+ * loaded.
+ */
+static int __init mc13783_pmic_convity_init(void)
+{
+	printk(KERN_INFO "PMIC Connectivity driver loading..\n");
+
+	return 0;
+}
+
+/*!
+ * @brief This is the Connectivity device driver de-initialization function.
+ *
+ * This function is called by the kernel when this device driver is about
+ * to be unloaded.
+ */
+static void __exit mc13783_pmic_convity_exit(void)
+{
+	printk(KERN_INFO "PMIC Connectivity driver unloading\n");
+
+	/* Close the device handle if it is still open. This will also
+	 * deregister any callbacks that may still be active.
+	 */
+	if (usb.handle_state == HANDLE_IN_USE) {
+		pmic_convity_close(usb.handle);
+	} else if (usb.handle_state == HANDLE_IN_USE) {
+		pmic_convity_close(rs_232.handle);
+	} else if (usb.handle_state == HANDLE_IN_USE) {
+		pmic_convity_close(cea_936.handle);
+	}
+
+	/* Reset the PMIC Connectivity register to it's power on state.
+	 * We should do this when unloading the module so that we don't
+	 * leave the hardware in a state which could cause problems when
+	 * no device driver is loaded.
+	 */
+	pmic_write_reg(REG_USB, RESET_USBCNTRL_REG_0, REG_FULLMASK);
+	pmic_write_reg(REG_CHARGE_USB_SPARE, RESET_USBCNTRL_REG_1,
+		       REG_FULLMASK);
+	/* Note that there is no need to reset the "convity" device driver
+	 * state structure to the reset state since we are in the final
+	 * stage of unloading the device driver. The device driver state
+	 * structure will be automatically and properly reinitialized if
+	 * this device driver is reloaded.
+	 */
+}
+
+/*@}*/
+
+/*
+ * Module entry points and description information.
+ */
+
+module_init(mc13783_pmic_convity_init);
+module_exit(mc13783_pmic_convity_exit);
+
+MODULE_DESCRIPTION("mc13783 Connectivity device driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/mc13783/pmic_light.c b/drivers/mxc/pmic/mc13783/pmic_light.c
new file mode 100644
index 000000000000..f6f5121d28c2
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_light.c
@@ -0,0 +1,2768 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_light.c
+ * @brief This is the main file of PMIC(mc13783) Light and Backlight driver.
+ *
+ * @ingroup PMIC_LIGHT
+ */
+
+/*
+ * Includes
+ */
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/pmic_light.h>
+#include <linux/pmic_status.h>
+#include "pmic_light_defs.h"
+
+#define NB_LIGHT_REG      6
+
+static int pmic_light_major;
+
+/*!
+ * Number of users waiting in suspendq
+ */
+static int swait = 0;
+
+/*!
+ * To indicate whether any of the light devices are suspending
+ */
+static int suspend_flag = 0;
+
+/*!
+ * The suspendq is used to block application calls
+ */
+static wait_queue_head_t suspendq;
+
+static struct class *pmic_light_class;
+
+/* EXPORTED FUNCTIONS */
+EXPORT_SYMBOL(pmic_bklit_tcled_master_enable);
+EXPORT_SYMBOL(pmic_bklit_tcled_master_disable);
+EXPORT_SYMBOL(pmic_bklit_master_enable);
+EXPORT_SYMBOL(pmic_bklit_master_disable);
+EXPORT_SYMBOL(pmic_bklit_set_current);
+EXPORT_SYMBOL(pmic_bklit_get_current);
+EXPORT_SYMBOL(pmic_bklit_set_dutycycle);
+EXPORT_SYMBOL(pmic_bklit_get_dutycycle);
+EXPORT_SYMBOL(pmic_bklit_set_cycle_time);
+EXPORT_SYMBOL(pmic_bklit_get_cycle_time);
+EXPORT_SYMBOL(pmic_bklit_set_mode);
+EXPORT_SYMBOL(pmic_bklit_get_mode);
+EXPORT_SYMBOL(pmic_bklit_rampup);
+EXPORT_SYMBOL(pmic_bklit_off_rampup);
+EXPORT_SYMBOL(pmic_bklit_rampdown);
+EXPORT_SYMBOL(pmic_bklit_off_rampdown);
+EXPORT_SYMBOL(pmic_bklit_enable_edge_slow);
+EXPORT_SYMBOL(pmic_bklit_disable_edge_slow);
+EXPORT_SYMBOL(pmic_bklit_get_edge_slow);
+EXPORT_SYMBOL(pmic_bklit_set_strobemode);
+EXPORT_SYMBOL(pmic_tcled_enable);
+EXPORT_SYMBOL(pmic_tcled_disable);
+EXPORT_SYMBOL(pmic_tcled_get_mode);
+EXPORT_SYMBOL(pmic_tcled_ind_set_current);
+EXPORT_SYMBOL(pmic_tcled_ind_get_current);
+EXPORT_SYMBOL(pmic_tcled_ind_set_blink_pattern);
+EXPORT_SYMBOL(pmic_tcled_ind_get_blink_pattern);
+EXPORT_SYMBOL(pmic_tcled_fun_set_current);
+EXPORT_SYMBOL(pmic_tcled_fun_get_current);
+EXPORT_SYMBOL(pmic_tcled_fun_set_cycletime);
+EXPORT_SYMBOL(pmic_tcled_fun_get_cycletime);
+EXPORT_SYMBOL(pmic_tcled_fun_set_dutycycle);
+EXPORT_SYMBOL(pmic_tcled_fun_get_dutycycle);
+EXPORT_SYMBOL(pmic_tcled_fun_blendedramps);
+EXPORT_SYMBOL(pmic_tcled_fun_sawramps);
+EXPORT_SYMBOL(pmic_tcled_fun_blendedbowtie);
+EXPORT_SYMBOL(pmic_tcled_fun_chasinglightspattern);
+EXPORT_SYMBOL(pmic_tcled_fun_strobe);
+EXPORT_SYMBOL(pmic_tcled_fun_rampup);
+EXPORT_SYMBOL(pmic_tcled_get_fun_rampup);
+EXPORT_SYMBOL(pmic_tcled_fun_rampdown);
+EXPORT_SYMBOL(pmic_tcled_get_fun_rampdown);
+EXPORT_SYMBOL(pmic_tcled_fun_triode_on);
+EXPORT_SYMBOL(pmic_tcled_fun_triode_off);
+EXPORT_SYMBOL(pmic_tcled_enable_edge_slow);
+EXPORT_SYMBOL(pmic_tcled_disable_edge_slow);
+EXPORT_SYMBOL(pmic_tcled_enable_half_current);
+EXPORT_SYMBOL(pmic_tcled_disable_half_current);
+EXPORT_SYMBOL(pmic_tcled_enable_audio_modulation);
+EXPORT_SYMBOL(pmic_tcled_disable_audio_modulation);
+EXPORT_SYMBOL(pmic_bklit_set_boost_mode);
+EXPORT_SYMBOL(pmic_bklit_get_boost_mode);
+EXPORT_SYMBOL(pmic_bklit_config_boost_mode);
+EXPORT_SYMBOL(pmic_bklit_gets_boost_mode);
+
+/*!
+ * This is the suspend of power management for the pmic light API.
+ * It suports SAVE and POWER_DOWN state.
+ *
+ * @param        pdev           the device
+ * @param        state          the state
+ *
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_light_suspend(struct platform_device *dev, pm_message_t state)
+{
+	suspend_flag = 1;
+	/* switch off all leds and backlights */
+	CHECK_ERROR(pmic_light_init_reg());
+
+	return 0;
+};
+
+/*!
+ * This is the resume of power management for the pmic light API.
+ * It suports RESTORE state.
+ *
+ * @param        dev            the device
+ *
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_light_resume(struct platform_device *pdev)
+{
+	suspend_flag = 0;
+	while (swait > 0) {
+		swait--;
+		wake_up_interruptible(&suspendq);
+	}
+
+	return 0;
+};
+
+/*!
+ * This function enables backlight & tcled.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_tcled_master_enable(void)
+{
+	unsigned int reg_value = 0;
+	unsigned int mask = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	reg_value = BITFVAL(BIT_LEDEN, 1);
+	mask = BITFMASK(BIT_LEDEN);
+	CHECK_ERROR(pmic_write_reg(LREG_0, reg_value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function disables backlight & tcled.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful
+ */
+PMIC_STATUS pmic_bklit_tcled_master_disable(void)
+{
+	unsigned int reg_value = 0;
+	unsigned int mask = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	reg_value = BITFVAL(BIT_LEDEN, 0);
+	mask = BITFMASK(BIT_LEDEN);
+	CHECK_ERROR(pmic_write_reg(LREG_0, reg_value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function enables backlight. Not supported on mc13783
+ * Use pmic_bklit_tcled_master_enable.
+ *
+ * @return       This function returns PMIC_NOT_SUPPORTED
+ */
+PMIC_STATUS pmic_bklit_master_enable(void)
+{
+	return PMIC_NOT_SUPPORTED;
+}
+
+/*!
+ * This function disables backlight. Not supported on mc13783
+ * Use pmic_bklit_tcled_master_enable.
+ *
+ * @return       This function returns PMIC_NOT_SUPPORTED
+ */
+PMIC_STATUS pmic_bklit_master_disable(void)
+{
+	return PMIC_NOT_SUPPORTED;
+}
+
+/*!
+ * This function sets backlight current level.
+ *
+ * @param        channel   Backlight channel
+ * @param        level     Backlight current level, as the following table.
+ *                         @verbatim
+                               level     main & aux   keyboard
+                               ------    -----------  --------
+                                 0         0 mA         0 mA
+                                 1         3 mA         12 mA
+                                 2         6 mA         24 mA
+                                 3         9 mA         36 mA
+                                 4         12 mA        48 mA
+                                 5         15 mA        60 mA
+                                 6         18 mA        72 mA
+                                 7         21 mA        84 mA
+                            @endverbatim
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_set_current(t_bklit_channel channel, unsigned char level)
+{
+	unsigned int mask;
+	unsigned int value;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		value = BITFVAL(BIT_CL_MAIN, level);
+		mask = BITFMASK(BIT_CL_MAIN);
+		break;
+	case BACKLIGHT_LED2:
+		value = BITFVAL(BIT_CL_AUX, level);
+		mask = BITFMASK(BIT_CL_AUX);
+		break;
+	case BACKLIGHT_LED3:
+		value = BITFVAL(BIT_CL_KEY, level);
+		mask = BITFMASK(BIT_CL_KEY);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	CHECK_ERROR(pmic_write_reg(LREG_2, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives backlight current level.
+ * The channels are not individually adjustable, hence
+ * the channel parameter is ignored.
+ *
+ * @param        channel   Backlight channel (Ignored because the
+ *                         channels are not individually adjustable)
+ * @param        level     Pointer to store backlight current level result.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_get_current(t_bklit_channel channel,
+				   unsigned char *level)
+{
+	unsigned int reg_value = 0;
+	unsigned int mask = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		mask = BITFMASK(BIT_CL_MAIN);
+		break;
+	case BACKLIGHT_LED2:
+		mask = BITFMASK(BIT_CL_AUX);
+		break;
+	case BACKLIGHT_LED3:
+		mask = BITFMASK(BIT_CL_KEY);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(LREG_2, &reg_value, mask));
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		*level = BITFEXT(reg_value, BIT_CL_MAIN);
+		break;
+	case BACKLIGHT_LED2:
+		*level = BITFEXT(reg_value, BIT_CL_AUX);
+		break;
+	case BACKLIGHT_LED3:
+		*level = BITFEXT(reg_value, BIT_CL_KEY);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets a backlight channel duty cycle.
+ * LED perceived brightness for each zone may be individually set by setting
+ * duty cycle. The default setting is for 0% duty cycle; this keeps all zone
+ * drivers turned off even after the master enable command. Each LED current
+ * sink can be turned on and adjusted for brightness with an independent 4 bit
+ * word for a duty cycle ranging from 0% to 100% in approximately 6.7% steps.
+ *
+ * @param        channel   Backlight channel.
+ * @param        dc        Backlight duty cycle, as the following table.
+ *                         @verbatim
+                                dc        Duty Cycle (% On-time over Cycle Time)
+                               ------    ---------------------------------------
+                                  0        0%
+                                  1        6.7%
+                                  2        13.3%
+                                  3        20%
+                                  4        26.7%
+                                  5        33.3%
+                                  6        40%
+                                  7        46.7%
+                                  8        53.3%
+                                  9        60%
+                                 10        66.7%
+                                 11        73.3%
+                                 12        80%
+                                 13        86.7%
+                                 14        93.3%
+                                 15        100%
+                             @endverbatim
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_set_dutycycle(t_bklit_channel channel, unsigned char dc)
+{
+	unsigned int reg_value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	if (dc > 15) {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(LREG_2, &reg_value, PMIC_ALL_BITS));
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		reg_value = reg_value & (~MASK_DUTY_CYCLE);
+		reg_value = reg_value | (dc << BIT_DUTY_CYCLE);
+		break;
+	case BACKLIGHT_LED2:
+		reg_value = reg_value & (~(MASK_DUTY_CYCLE << INDEX_AUX));
+		reg_value = reg_value | (dc << (BIT_DUTY_CYCLE + INDEX_AUX));
+		break;
+	case BACKLIGHT_LED3:
+		reg_value = reg_value & (~(MASK_DUTY_CYCLE << INDEX_KYD));
+		reg_value = reg_value | (dc << (BIT_DUTY_CYCLE + INDEX_KYD));
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	CHECK_ERROR(pmic_write_reg(LREG_2, reg_value, PMIC_ALL_BITS));
+	return PMIC_SUCCESS;
+
+}
+
+/*!
+ * This function retrives a backlight channel duty cycle.
+ *
+ * @param        channel   Backlight channel.
+ * @param        dc        Pointer to backlight duty cycle.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_get_dutycycle(t_bklit_channel channel, unsigned char *dc)
+{
+	unsigned int reg_value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	CHECK_ERROR(pmic_read_reg(LREG_2, &reg_value, PMIC_ALL_BITS));
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		*dc = (int)((reg_value & (MASK_DUTY_CYCLE))
+			    >> BIT_DUTY_CYCLE);
+
+		break;
+	case BACKLIGHT_LED2:
+		*dc = (int)((reg_value & (MASK_DUTY_CYCLE << INDEX_AUX))
+			    >> (BIT_DUTY_CYCLE + INDEX_AUX));
+		break;
+	case BACKLIGHT_LED3:
+		*dc = (int)((reg_value & (MASK_DUTY_CYCLE <<
+					  INDEX_KYD)) >> (BIT_DUTY_CYCLE +
+							  INDEX_KYD));
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets a backlight channel cycle time.
+ * Cycle Time is defined as the period of a complete cycle of
+ * Time_on + Time_off. The default Cycle Time is set to 0.01 seconds such that
+ * the 100 Hz on-off cycling is averaged out by the eye to eliminate
+ * flickering. Additionally, the Cycle Time can be programmed to intentionally
+ * extend the period of on-off cycles for a visual pulsating or blinking effect.
+ *
+ * @param        period    Backlight cycle time, as the following table.
+ *                         @verbatim
+                                period      Cycle Time
+                               --------    ------------
+                                  0          0.01 seconds
+                                  1          0.1 seconds
+                                  2          0.5 seconds
+                                  3          2 seconds
+                             @endverbatim
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_set_cycle_time(unsigned char period)
+{
+	unsigned int mask;
+	unsigned int value;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	if (period > 3) {
+		return PMIC_PARAMETER_ERROR;
+	}
+	mask = BITFMASK(BIT_PERIOD);
+	value = BITFVAL(BIT_PERIOD, period);
+	CHECK_ERROR(pmic_write_reg(LREG_2, value, mask));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives a backlight channel cycle time setting.
+ *
+ * @param        period    Pointer to save backlight cycle time setting result.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_get_cycle_time(unsigned char *period)
+{
+	unsigned int mask;
+	unsigned int value;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	mask = BITFMASK(BIT_PERIOD);
+	CHECK_ERROR(pmic_read_reg(LREG_2, &value, mask));
+	*period = BITFEXT(value, BIT_PERIOD);
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets backlight operation mode. There are two modes of
+ * operations: current control and triode mode.
+ * The Duty Cycle/Cycle Time control is retained in Triode Mode. Audio
+ * coupling is not available in Triode Mode.
+ *
+ * @param        channel   Backlight channel.
+ * @param        mode      Backlight operation mode.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_set_mode(t_bklit_channel channel, t_bklit_mode mode)
+{
+	unsigned int reg_value = 0;
+	unsigned int clear_val = 0;
+	unsigned int triode_val = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	CHECK_ERROR(pmic_read_reg(LREG_0, &reg_value, PMIC_ALL_BITS));
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		clear_val = ~(MASK_TRIODE_MAIN_BL);
+		triode_val = MASK_TRIODE_MAIN_BL;
+		break;
+	case BACKLIGHT_LED2:
+		clear_val = ~(MASK_TRIODE_MAIN_BL << INDEX_AUXILIARY);
+		triode_val = (MASK_TRIODE_MAIN_BL << INDEX_AUXILIARY);
+		break;
+	case BACKLIGHT_LED3:
+		clear_val = ~(MASK_TRIODE_MAIN_BL << INDEX_KEYPAD);
+		triode_val = (MASK_TRIODE_MAIN_BL << INDEX_KEYPAD);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	reg_value = (reg_value & clear_val);
+
+	if (mode == BACKLIGHT_TRIODE_MODE) {
+		reg_value = (reg_value | triode_val);
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_0, reg_value, PMIC_ALL_BITS));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets backlight operation mode. There are two modes of
+ * operations: current control and triode mode.
+ * The Duty Cycle/Cycle Time control is retained in Triode Mode. Audio
+ * coupling is not available in Triode Mode.
+ *
+ * @param        channel   Backlight channel.
+ * @param        mode      Backlight operation mode.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_get_mode(t_bklit_channel channel, t_bklit_mode * mode)
+{
+	unsigned int reg_value = 0;
+	unsigned int mask = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		mask = BITFMASK(BIT_TRIODE_MAIN_BL);
+		break;
+	case BACKLIGHT_LED2:
+		mask = BITFMASK(BIT_TRIODE_AUX_BL);
+		break;
+	case BACKLIGHT_LED3:
+		mask = BITFMASK(BIT_TRIODE_KEY_BL);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(LREG_0, &reg_value, mask));
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		*mode = BITFEXT(reg_value, BIT_TRIODE_MAIN_BL);
+		break;
+	case BACKLIGHT_LED2:
+		*mode = BITFEXT(reg_value, BIT_TRIODE_AUX_BL);
+		break;
+	case BACKLIGHT_LED3:
+		*mode = BITFEXT(reg_value, BIT_TRIODE_KEY_BL);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function starts backlight brightness ramp up function; ramp time is
+ * fixed at 0.5 seconds.
+ *
+ * @param        channel   Backlight channel.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_rampup(t_bklit_channel channel)
+{
+	unsigned int reg_value = 0;
+	unsigned int mask = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		mask = BITFMASK(BIT_UP_MAIN_BL);
+		reg_value = BITFVAL(BIT_UP_MAIN_BL, 1);
+		break;
+	case BACKLIGHT_LED2:
+		mask = BITFMASK(BIT_UP_AUX_BL);
+		reg_value = BITFVAL(BIT_UP_AUX_BL, 1);
+		break;
+	case BACKLIGHT_LED3:
+		mask = BITFMASK(BIT_UP_KEY_BL);
+		reg_value = BITFVAL(BIT_UP_KEY_BL, 1);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_0, reg_value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function stops backlight brightness ramp up function;
+ *
+ * @param        channel   Backlight channel.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_off_rampup(t_bklit_channel channel)
+{
+	unsigned int reg_value = 0;
+	unsigned int mask = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		mask = BITFMASK(BIT_UP_MAIN_BL);
+		break;
+	case BACKLIGHT_LED2:
+		mask = BITFMASK(BIT_UP_AUX_BL);
+		break;
+	case BACKLIGHT_LED3:
+		mask = BITFMASK(BIT_UP_KEY_BL);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_0, reg_value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function starts backlight brightness ramp down function; ramp time is
+ * fixed at 0.5 seconds.
+ *
+ * @param        channel   Backlight channel.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_rampdown(t_bklit_channel channel)
+{
+	unsigned int reg_value = 0;
+	unsigned int mask = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		mask = BITFMASK(BIT_DOWN_MAIN_BL);
+		reg_value = BITFVAL(BIT_DOWN_MAIN_BL, 1);
+		break;
+	case BACKLIGHT_LED2:
+		mask = BITFMASK(BIT_DOWN_AUX_BL);
+		reg_value = BITFVAL(BIT_DOWN_AUX_BL, 1);
+		break;
+	case BACKLIGHT_LED3:
+		mask = BITFMASK(BIT_DOWN_KEY_BL);
+		reg_value = BITFVAL(BIT_DOWN_KEY_BL, 1);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_0, reg_value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function stops backlight brightness ramp down function.
+ *
+ * @param        channel   Backlight channel.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_off_rampdown(t_bklit_channel channel)
+{
+	unsigned int reg_value = 0;
+	unsigned int mask = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (channel) {
+	case BACKLIGHT_LED1:
+		mask = BITFMASK(BIT_DOWN_MAIN_BL);
+		break;
+	case BACKLIGHT_LED2:
+		mask = BITFMASK(BIT_DOWN_AUX_BL);
+		break;
+	case BACKLIGHT_LED3:
+		mask = BITFMASK(BIT_DOWN_KEY_BL);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_0, reg_value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function enables backlight analog edge slowing mode. Analog Edge
+ * Slowing slows down the transient edges to reduce the chance of coupling LED
+ * modulation activity into other circuits. Rise and fall times will be targeted
+ * for approximately 50usec.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_enable_edge_slow(void)
+{
+	unsigned int mask;
+	unsigned int value;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	mask = BITFMASK(BIT_SLEWLIMBL);
+	value = BITFVAL(BIT_SLEWLIMBL, 1);
+	CHECK_ERROR(pmic_write_reg(LREG_2, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function disables backlight analog edge slowing mode. The backlight
+ * drivers will default to an <93>Instant On<94> mode.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_disable_edge_slow(void)
+{
+	unsigned int mask;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	mask = BITFMASK(BIT_SLEWLIMBL);
+	CHECK_ERROR(pmic_write_reg(LREG_2, 0, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets backlight analog edge slowing mode. DThe backlight
+ *
+ * @param        edge      Edge slowing mode.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_bklit_get_edge_slow(bool * edge)
+{
+	unsigned int mask;
+	unsigned int value;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	mask = BITFMASK(BIT_SLEWLIMBL);
+	CHECK_ERROR(pmic_read_reg(LREG_2, &value, mask));
+	*edge = (bool) BITFEXT(value, BIT_SLEWLIMBL);
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets backlight Strobe Light Pulsing mode.
+ *
+ * @param        channel   Backlight channel.
+ * @param        mode      Strobe Light Pulsing mode.
+ *
+ * @return       This function returns PMIC_NOT_SUPPORTED.
+ */
+PMIC_STATUS pmic_bklit_set_strobemode(t_bklit_channel channel,
+				      t_bklit_strobe_mode mode)
+{
+	return PMIC_NOT_SUPPORTED;
+}
+
+/*!
+ * This function enables tri-color LED.
+ *
+ * @param        mode      Tri-color LED operation mode.
+ * @param	 bank      Selected tri-color bank
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_enable(t_tcled_mode mode, t_funlight_bank bank)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (mode) {
+	case TCLED_FUN_MODE:
+		switch (bank) {
+		case TCLED_FUN_BANK1:
+			mask = MASK_BK1_FL;
+			value = MASK_BK1_FL;
+			break;
+		case TCLED_FUN_BANK2:
+			mask = MASK_BK2_FL;
+			value = MASK_BK2_FL;
+			break;
+		case TCLED_FUN_BANK3:
+			mask = MASK_BK3_FL;
+			value = MASK_BK3_FL;
+			break;
+		default:
+			return PMIC_PARAMETER_ERROR;
+		}
+		break;
+	case TCLED_IND_MODE:
+		mask = MASK_BK1_FL | MASK_BK2_FL | MASK_BK3_FL;
+		break;
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_0, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function disables tri-color LED.
+ *
+ * @param        bank      Selected tri-color bank
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ *
+ */
+PMIC_STATUS pmic_tcled_disable(t_funlight_bank bank)
+{
+	unsigned int mask = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		mask = MASK_BK1_FL;
+		break;
+	case TCLED_FUN_BANK2:
+		mask = MASK_BK2_FL;
+		break;
+	case TCLED_FUN_BANK3:
+		mask = MASK_BK3_FL;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_0, 0, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives tri-color LED operation mode.
+ *
+ * @param        mode      Pointer to Tri-color LED operation mode.
+ * @param        bank      Selected tri-color bank
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_get_mode(t_tcled_mode * mode, t_funlight_bank bank)
+{
+	unsigned int val;
+	unsigned int mask;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		mask = MASK_BK1_FL;
+		break;
+	case TCLED_FUN_BANK2:
+		mask = MASK_BK2_FL;
+		break;
+	case TCLED_FUN_BANK3:
+		mask = MASK_BK3_FL;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(LREG_0, &val, mask));
+
+	if (val) {
+		*mode = TCLED_FUN_MODE;
+	} else {
+		*mode = TCLED_IND_MODE;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets a tri-color LED channel current level in indicator mode.
+ *
+ * @param        channel      Tri-color LED channel.
+ * @param        level        Current level.
+ * @param        bank         Selected tri-color bank
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_ind_set_current(t_ind_channel channel,
+				       t_tcled_cur_level level,
+				       t_funlight_bank bank)
+{
+	unsigned int reg_conf = 0;
+	unsigned int mask;
+	unsigned int value;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	if (level > TCLED_CUR_LEVEL_4) {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		reg_conf = LREG_3;
+		break;
+	case TCLED_FUN_BANK2:
+		reg_conf = LREG_4;
+		break;
+	case TCLED_FUN_BANK3:
+		reg_conf = LREG_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_IND_RED:
+		value = BITFVAL(BITS_CL_RED, level);
+		mask = BITFMASK(BITS_CL_RED);
+		break;
+	case TCLED_IND_GREEN:
+		value = BITFVAL(BITS_CL_GREEN, level);
+		mask = BITFMASK(BITS_CL_GREEN);
+		break;
+	case TCLED_IND_BLUE:
+		value = BITFVAL(BITS_CL_BLUE, level);
+		mask = BITFMASK(BITS_CL_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg_conf, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives a tri-color LED channel current level
+ * in indicator mode.
+ *
+ * @param        channel      Tri-color LED channel.
+ * @param        level        Pointer to current level.
+ * @param        bank         Selected tri-color bank
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_ind_get_current(t_ind_channel channel,
+				       t_tcled_cur_level * level,
+				       t_funlight_bank bank)
+{
+	unsigned int reg_conf = 0;
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		reg_conf = LREG_3;
+		break;
+	case TCLED_FUN_BANK2:
+		reg_conf = LREG_4;
+		break;
+	case TCLED_FUN_BANK3:
+		reg_conf = LREG_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_IND_RED:
+		mask = BITFMASK(BITS_CL_RED);
+		break;
+	case TCLED_IND_GREEN:
+		mask = BITFMASK(BITS_CL_GREEN);
+		break;
+	case TCLED_IND_BLUE:
+		mask = BITFMASK(BITS_CL_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg_conf, &value, mask));
+
+	switch (channel) {
+	case TCLED_IND_RED:
+		*level = BITFEXT(value, BITS_CL_RED);
+		break;
+	case TCLED_IND_GREEN:
+		*level = BITFEXT(value, BITS_CL_GREEN);
+		break;
+	case TCLED_IND_BLUE:
+		*level = BITFEXT(value, BITS_CL_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets a tri-color LED channel blinking pattern in indication
+ * mode.
+ *
+ * @param        channel      Tri-color LED channel.
+ * @param        pattern      Blinking pattern.
+ * @param        skip         If true, skip a cycle after each cycle.
+ * @param        bank         Selected tri-color bank
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_ind_set_blink_pattern(t_ind_channel channel,
+					     t_tcled_ind_blink_pattern pattern,
+					     bool skip, t_funlight_bank bank)
+{
+	unsigned int reg_conf = 0;
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	if (skip == true) {
+		return PMIC_NOT_SUPPORTED;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		reg_conf = LREG_3;
+		break;
+	case TCLED_FUN_BANK2:
+		reg_conf = LREG_4;
+		break;
+	case TCLED_FUN_BANK3:
+		reg_conf = LREG_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_IND_RED:
+		value = BITFVAL(BITS_DC_RED, pattern);
+		mask = BITFMASK(BITS_DC_RED);
+		break;
+	case TCLED_IND_GREEN:
+		value = BITFVAL(BITS_DC_GREEN, pattern);
+		mask = BITFMASK(BITS_DC_GREEN);
+		break;
+	case TCLED_IND_BLUE:
+		value = BITFVAL(BITS_DC_BLUE, pattern);
+		mask = BITFMASK(BITS_DC_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg_conf, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives a tri-color LED channel blinking pattern in
+ * indication mode.
+ *
+ * @param        channel      Tri-color LED channel.
+ * @param        pattern      Pointer to Blinking pattern.
+ * @param        skip         Pointer to a boolean varible indicating if skip
+ * @param        bank         Selected tri-color bank
+ *                            a cycle after each cycle.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_ind_get_blink_pattern(t_ind_channel channel,
+					     t_tcled_ind_blink_pattern *
+					     pattern, bool * skip,
+					     t_funlight_bank bank)
+{
+	unsigned int reg_conf = 0;
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		reg_conf = LREG_3;
+		break;
+	case TCLED_FUN_BANK2:
+		reg_conf = LREG_4;
+		break;
+	case TCLED_FUN_BANK3:
+		reg_conf = LREG_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_IND_RED:
+		mask = BITFMASK(BITS_DC_RED);
+		break;
+	case TCLED_IND_GREEN:
+		mask = BITFMASK(BITS_DC_GREEN);
+		break;
+	case TCLED_IND_BLUE:
+		mask = BITFMASK(BITS_DC_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg_conf, &value, mask));
+
+	switch (channel) {
+	case TCLED_IND_RED:
+		*pattern = BITFEXT(value, BITS_DC_RED);
+		break;
+	case TCLED_IND_GREEN:
+		*pattern = BITFEXT(value, BITS_DC_GREEN);
+		break;
+	case TCLED_IND_BLUE:
+		*pattern = BITFEXT(value, BITS_DC_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets a tri-color LED channel current level in Fun Light mode.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ * @param        level        Current level.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_set_current(t_funlight_bank bank,
+				       t_funlight_channel channel,
+				       t_tcled_cur_level level)
+{
+	unsigned int reg_conf = 0;
+	unsigned int mask;
+	unsigned int value;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	if (level > TCLED_CUR_LEVEL_4) {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		reg_conf = LREG_3;
+		break;
+	case TCLED_FUN_BANK2:
+		reg_conf = LREG_4;
+		break;
+	case TCLED_FUN_BANK3:
+		reg_conf = LREG_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_FUN_CHANNEL1:
+		value = BITFVAL(BITS_CL_RED, level);
+		mask = BITFMASK(BITS_CL_RED);
+		break;
+	case TCLED_FUN_CHANNEL2:
+		value = BITFVAL(BITS_CL_GREEN, level);
+		mask = BITFMASK(BITS_CL_GREEN);
+		break;
+	case TCLED_FUN_CHANNEL3:
+		value = BITFVAL(BITS_CL_BLUE, level);
+		mask = BITFMASK(BITS_CL_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg_conf, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives a tri-color LED channel current level
+ * in Fun Light mode.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ * @param        level        Pointer to current level.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_get_current(t_funlight_bank bank,
+				       t_funlight_channel channel,
+				       t_tcled_cur_level * level)
+{
+	unsigned int reg_conf = 0;
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		reg_conf = LREG_3;
+		break;
+	case TCLED_FUN_BANK2:
+		reg_conf = LREG_4;
+		break;
+	case TCLED_FUN_BANK3:
+		reg_conf = LREG_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_FUN_CHANNEL1:
+		mask = BITFMASK(BITS_CL_RED);
+		break;
+	case TCLED_FUN_CHANNEL2:
+		mask = BITFMASK(BITS_CL_GREEN);
+		break;
+	case TCLED_FUN_CHANNEL3:
+		mask = BITFMASK(BITS_CL_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg_conf, &value, mask));
+
+	switch (channel) {
+	case TCLED_FUN_CHANNEL1:
+		*level = BITFEXT(value, BITS_CL_RED);
+		break;
+	case TCLED_FUN_CHANNEL2:
+		*level = BITFEXT(value, BITS_CL_GREEN);
+		break;
+	case TCLED_FUN_CHANNEL3:
+		*level = BITFEXT(value, BITS_CL_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets tri-color LED cycle time.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        ct           Cycle time.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_set_cycletime(t_funlight_bank bank,
+					 t_tcled_fun_cycle_time ct)
+{
+	unsigned int reg_conf = 0;
+	unsigned int mask;
+	unsigned int value;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	if (ct > TC_CYCLE_TIME_4) {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		reg_conf = LREG_3;
+		break;
+	case TCLED_FUN_BANK2:
+		reg_conf = LREG_4;
+		break;
+	case TCLED_FUN_BANK3:
+		reg_conf = LREG_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	value = BITFVAL(BIT_PERIOD, ct);
+	mask = BITFMASK(BIT_PERIOD);
+
+	CHECK_ERROR(pmic_write_reg(reg_conf, value, mask));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives tri-color LED cycle time in Fun Light mode.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        ct           Pointer to cycle time.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_get_cycletime(t_funlight_bank bank,
+					 t_tcled_fun_cycle_time * ct)
+{
+	unsigned int reg_conf = 0;
+	unsigned int mask;
+	unsigned int value;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	if (*ct > TC_CYCLE_TIME_4) {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		reg_conf = LREG_3;
+		break;
+	case TCLED_FUN_BANK2:
+		reg_conf = LREG_4;
+		break;
+	case TCLED_FUN_BANK3:
+		reg_conf = LREG_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	mask = BITFMASK(BIT_PERIOD);
+	CHECK_ERROR(pmic_read_reg(reg_conf, &value, mask));
+
+	*ct = BITFVAL(BIT_PERIOD, value);
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets a tri-color LED channel duty cycle in Fun Light mode.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ * @param        dc           Duty cycle.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_set_dutycycle(t_funlight_bank bank,
+					 t_funlight_channel channel,
+					 unsigned char dc)
+{
+	unsigned int reg_conf = 0;
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		reg_conf = LREG_3;
+		break;
+	case TCLED_FUN_BANK2:
+		reg_conf = LREG_4;
+		break;
+	case TCLED_FUN_BANK3:
+		reg_conf = LREG_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_FUN_CHANNEL1:
+		value = BITFVAL(BITS_DC_RED, dc);
+		mask = BITFMASK(BITS_DC_RED);
+		break;
+	case TCLED_FUN_CHANNEL2:
+		value = BITFVAL(BITS_DC_GREEN, dc);
+		mask = BITFMASK(BITS_DC_GREEN);
+		break;
+	case TCLED_FUN_CHANNEL3:
+		value = BITFVAL(BITS_DC_BLUE, dc);
+		mask = BITFMASK(BITS_DC_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg_conf, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives a tri-color LED channel duty cycle in Fun Light mode.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ * @param        dc           Pointer to duty cycle.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_get_dutycycle(t_funlight_bank bank,
+					 t_funlight_channel channel,
+					 unsigned char *dc)
+{
+	unsigned int reg_conf = 0;
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		reg_conf = LREG_3;
+		break;
+	case TCLED_FUN_BANK2:
+		reg_conf = LREG_4;
+		break;
+	case TCLED_FUN_BANK3:
+		reg_conf = LREG_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_FUN_CHANNEL1:
+		mask = BITFMASK(BITS_DC_RED);
+		break;
+	case TCLED_FUN_CHANNEL2:
+		mask = BITFMASK(BITS_DC_GREEN);
+		break;
+	case TCLED_FUN_CHANNEL3:
+		mask = BITFMASK(BITS_DC_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg_conf, &value, mask));
+
+	switch (channel) {
+	case TCLED_FUN_CHANNEL1:
+		*dc = BITFEXT(value, BITS_DC_RED);
+		break;
+	case TCLED_FUN_CHANNEL2:
+		*dc = BITFEXT(value, BITS_DC_GREEN);
+		break;
+	case TCLED_FUN_CHANNEL3:
+		*dc = BITFEXT(value, BITS_DC_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function initiates Blended Ramp fun light pattern.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        speed        Speed of pattern.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_blendedramps(t_funlight_bank bank,
+					t_tcled_fun_speed speed)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (speed) {
+	case TC_OFF:
+		value = BITFVAL(BITS_FUN_LIGHT, FUN_LIGHTS_OFF);
+		break;
+	case TC_SLOW:
+		value = BITFVAL(BITS_FUN_LIGHT, BLENDED_RAMPS_SLOW);
+		break;
+	case TC_FAST:
+		value = BITFVAL(BITS_FUN_LIGHT, BLENDED_RAMPS_FAST);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	mask = BITFMASK(BITS_FUN_LIGHT);
+	CHECK_ERROR(pmic_write_reg(LREG_0, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function initiates Saw Ramp fun light pattern.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        speed        Speed of pattern.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_sawramps(t_funlight_bank bank,
+				    t_tcled_fun_speed speed)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (speed) {
+	case TC_OFF:
+		value = BITFVAL(BITS_FUN_LIGHT, FUN_LIGHTS_OFF);
+		break;
+	case TC_SLOW:
+		value = BITFVAL(BITS_FUN_LIGHT, SAW_RAMPS_SLOW);
+		break;
+	case TC_FAST:
+		value = BITFVAL(BITS_FUN_LIGHT, SAW_RAMPS_FAST);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	mask = BITFMASK(BITS_FUN_LIGHT);
+	CHECK_ERROR(pmic_write_reg(LREG_0, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function initiates Blended Bowtie fun light pattern.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        speed        Speed of pattern.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_blendedbowtie(t_funlight_bank bank,
+					 t_tcled_fun_speed speed)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (speed) {
+	case TC_OFF:
+		value = BITFVAL(BITS_FUN_LIGHT, FUN_LIGHTS_OFF);
+		break;
+	case TC_SLOW:
+		value = BITFVAL(BITS_FUN_LIGHT, BLENDED_INVERSE_RAMPS_SLOW);
+		break;
+	case TC_FAST:
+		value = BITFVAL(BITS_FUN_LIGHT, BLENDED_INVERSE_RAMPS_FAST);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	mask = BITFMASK(BITS_FUN_LIGHT);
+	CHECK_ERROR(pmic_write_reg(LREG_0, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function initiates Chasing Lights fun light pattern.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        pattern      Chasing light pattern mode.
+ * @param        speed        Speed of pattern.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_chasinglightspattern(t_funlight_bank bank,
+						t_chaselight_pattern pattern,
+						t_tcled_fun_speed speed)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	if (pattern > BGR) {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (speed) {
+	case TC_OFF:
+		value = BITFVAL(BITS_FUN_LIGHT, FUN_LIGHTS_OFF);
+		break;
+	case TC_SLOW:
+		if (pattern == PMIC_RGB) {
+			value =
+			    BITFVAL(BITS_FUN_LIGHT, CHASING_LIGHTS_RGB_SLOW);
+		} else {
+			value =
+			    BITFVAL(BITS_FUN_LIGHT, CHASING_LIGHTS_BGR_SLOW);
+		}
+		break;
+	case TC_FAST:
+		if (pattern == PMIC_RGB) {
+			value =
+			    BITFVAL(BITS_FUN_LIGHT, CHASING_LIGHTS_RGB_FAST);
+		} else {
+			value =
+			    BITFVAL(BITS_FUN_LIGHT, CHASING_LIGHTS_BGR_FAST);
+		}
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	mask = BITFMASK(BITS_FUN_LIGHT);
+	CHECK_ERROR(pmic_write_reg(LREG_0, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function initiates Strobe Mode fun light pattern.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ * @param        speed        Speed of pattern.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_strobe(t_funlight_bank bank,
+				  t_funlight_channel channel,
+				  t_tcled_fun_strobe_speed speed)
+{
+	/* not supported on mc13783 */
+
+	return PMIC_NOT_SUPPORTED;
+}
+
+/*!
+ * This function initiates Tri-color LED brightness Ramp Up function; Ramp time
+ * is fixed at 1 second.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ * @param        rampup       Ramp-up configuration.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_rampup(t_funlight_bank bank,
+				  t_funlight_channel channel, bool rampup)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		mask = LEDR1RAMPUP;
+		value = LEDR1RAMPUP;
+		break;
+	case TCLED_FUN_BANK2:
+		mask = LEDR2RAMPUP;
+		value = LEDR2RAMPUP;
+		break;
+	case TCLED_FUN_BANK3:
+		mask = LEDR3RAMPUP;
+		value = LEDR3RAMPUP;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_FUN_CHANNEL1:
+		mask = mask;
+		value = value;
+		break;
+	case TCLED_FUN_CHANNEL2:
+		mask = mask * 2;
+		value = value * 2;
+		break;
+	case TCLED_FUN_CHANNEL3:
+		mask = mask * 4;
+		value = value * 4;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	if (!rampup) {
+		value = 0;
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_1, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets Tri-color LED brightness Ramp Up function; Ramp time
+ * is fixed at 1 second.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ * @param        rampup       Ramp-up configuration.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_get_fun_rampup(t_funlight_bank bank,
+				      t_funlight_channel channel, bool * rampup)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		mask = LEDR1RAMPUP;
+		break;
+	case TCLED_FUN_BANK2:
+		mask = LEDR2RAMPUP;
+		break;
+	case TCLED_FUN_BANK3:
+		mask = LEDR3RAMPUP;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_FUN_CHANNEL1:
+		mask = mask;
+		break;
+	case TCLED_FUN_CHANNEL2:
+		mask = mask * 2;
+		break;
+	case TCLED_FUN_CHANNEL3:
+		mask = mask * 4;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(LREG_1, &value, mask));
+	if (value) {
+		*rampup = true;
+	} else {
+		*rampup = false;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function initiates Tri-color LED brightness Ramp Down function; Ramp
+ * time is fixed at 1 second.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ * @param        rampdown     Ramp-down configuration.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_rampdown(t_funlight_bank bank,
+				    t_funlight_channel channel, bool rampdown)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		mask = LEDR1RAMPDOWN;
+		value = LEDR1RAMPDOWN;
+		break;
+	case TCLED_FUN_BANK2:
+		mask = LEDR2RAMPDOWN;
+		value = LEDR2RAMPDOWN;
+		break;
+	case TCLED_FUN_BANK3:
+		mask = LEDR3RAMPDOWN;
+		value = LEDR3RAMPDOWN;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_FUN_CHANNEL1:
+		mask = mask;
+		value = value;
+		break;
+	case TCLED_FUN_CHANNEL2:
+		mask = mask * 2;
+		value = value * 2;
+		break;
+	case TCLED_FUN_CHANNEL3:
+		mask = mask * 4;
+		value = value * 4;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	if (!rampdown) {
+		value = 0;
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_1, value, mask));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function initiates Tri-color LED brightness Ramp Down function; Ramp
+ * time is fixed at 1 second.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ * @param        rampdown     Ramp-down configuration.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_get_fun_rampdown(t_funlight_bank bank,
+					t_funlight_channel channel,
+					bool * rampdown)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		mask = LEDR1RAMPDOWN;
+		break;
+	case TCLED_FUN_BANK2:
+		mask = LEDR2RAMPDOWN;
+		break;
+	case TCLED_FUN_BANK3:
+		mask = LEDR3RAMPDOWN;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (channel) {
+	case TCLED_FUN_CHANNEL1:
+		mask = mask;
+		break;
+	case TCLED_FUN_CHANNEL2:
+		mask = mask * 2;
+		break;
+	case TCLED_FUN_CHANNEL3:
+		mask = mask * 4;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(LREG_1, &value, mask));
+	if (value) {
+		*rampdown = true;
+	} else {
+		*rampdown = false;
+	}
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function enables a Tri-color channel triode mode.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_triode_on(t_funlight_bank bank,
+				     t_funlight_channel channel)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		mask = MASK_BK1_FL;
+		value = ENABLE_BK1_FL;
+		break;
+	case TCLED_FUN_BANK2:
+		mask = MASK_BK2_FL;
+		value = ENABLE_BK2_FL;
+		break;
+	case TCLED_FUN_BANK3:
+		mask = MASK_BK3_FL;
+		value = ENABLE_BK2_FL;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_0, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function disables a Tri-color LED channel triode mode.
+ *
+ * @param        bank         Tri-color LED bank
+ * @param        channel      Tri-color LED channel.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_fun_triode_off(t_funlight_bank bank,
+				      t_funlight_channel channel)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	switch (bank) {
+	case TCLED_FUN_BANK1:
+		mask = MASK_BK1_FL;
+		break;
+	case TCLED_FUN_BANK2:
+		mask = MASK_BK2_FL;
+		break;
+	case TCLED_FUN_BANK3:
+		mask = MASK_BK3_FL;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(LREG_0, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function enables Tri-color LED edge slowing.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_enable_edge_slow(void)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	value = BITFVAL(BIT_SLEWLIMTC, 1);
+	mask = BITFMASK(BIT_SLEWLIMTC);
+
+	CHECK_ERROR(pmic_write_reg(LREG_1, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function disables Tri-color LED edge slowing.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_disable_edge_slow(void)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	value = BITFVAL(BIT_SLEWLIMTC, 0);
+	mask = BITFMASK(BIT_SLEWLIMTC);
+
+	CHECK_ERROR(pmic_write_reg(LREG_1, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function enables Tri-color LED half current mode.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_enable_half_current(void)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	value = BITFVAL(BIT_TC1HALF, 1);
+	mask = BITFMASK(BIT_TC1HALF);
+
+	CHECK_ERROR(pmic_write_reg(LREG_1, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function disables Tri-color LED half current mode.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_tcled_disable_half_current(void)
+{
+	unsigned int mask = 0;
+	unsigned int value = 0;
+
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+
+	value = BITFVAL(BIT_TC1HALF, 0);
+	mask = BITFMASK(BIT_TC1HALF);
+
+	CHECK_ERROR(pmic_write_reg(LREG_1, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function enables backlight or Tri-color LED audio modulation.
+ *
+ * @return       This function returns PMIC_NOT_SUPPORTED.
+ */
+PMIC_STATUS pmic_tcled_enable_audio_modulation(t_led_channel channel,
+					       t_aud_path path,
+					       t_aud_gain gain, bool lpf_bypass)
+{
+	return PMIC_NOT_SUPPORTED;
+}
+
+/*!
+ * This function disables backlight or Tri-color LED audio modulation.
+ *
+ * @return       This function returns PMIC_NOT_SUPPORTED.
+ */
+PMIC_STATUS pmic_tcled_disable_audio_modulation(void)
+{
+	return PMIC_NOT_SUPPORTED;
+}
+
+/*!
+ * This function enables the boost mode.
+ * Only on mc13783 2.0 or higher
+ *
+ * @param       en_dis   Enable or disable the boost mode
+ *
+ * @return      This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_bklit_set_boost_mode(bool en_dis)
+{
+
+	pmic_version_t mc13783_ver;
+	unsigned int mask;
+	unsigned int value;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+
+		if (suspend_flag == 1) {
+			return -EBUSY;
+		}
+
+		value = BITFVAL(BIT_BOOSTEN, en_dis);
+		mask = BITFMASK(BIT_BOOSTEN);
+		CHECK_ERROR(pmic_write_reg(LREG_0, value, mask));
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function gets the boost mode.
+ * Only on mc13783 2.0 or higher
+ *
+ * @param       en_dis   Enable or disable the boost mode
+ *
+ * @return      This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_bklit_get_boost_mode(bool * en_dis)
+{
+	pmic_version_t mc13783_ver;
+	unsigned int mask;
+	unsigned int value;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+
+		if (suspend_flag == 1) {
+			return -EBUSY;
+		}
+		mask = BITFMASK(BIT_BOOSTEN);
+		CHECK_ERROR(pmic_read_reg(LREG_0, &value, mask));
+		*en_dis = BITFEXT(value, BIT_BOOSTEN);
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function sets boost mode configuration
+ * Only on mc13783 2.0 or higher
+ *
+ * @param    abms      Define adaptive boost mode selection
+ * @param    abr       Define adaptive boost reference
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_bklit_config_boost_mode(unsigned int abms, unsigned int abr)
+{
+	unsigned int conf_boost = 0;
+	unsigned int mask;
+	unsigned int value;
+	pmic_version_t mc13783_ver;
+
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+		if (suspend_flag == 1) {
+			return -EBUSY;
+		}
+
+		if (abms > MAX_BOOST_ABMS) {
+			return PMIC_PARAMETER_ERROR;
+		}
+
+		if (abr > MAX_BOOST_ABR) {
+			return PMIC_PARAMETER_ERROR;
+		}
+
+		conf_boost = abms | (abr << 3);
+
+		value = BITFVAL(BITS_BOOST, conf_boost);
+		mask = BITFMASK(BITS_BOOST);
+		CHECK_ERROR(pmic_write_reg(LREG_0, value, mask));
+
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function gets boost mode configuration
+ * Only on mc13783 2.0 or higher
+ *
+ * @param    abms      Define adaptive boost mode selection
+ * @param    abr       Define adaptive boost reference
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_bklit_gets_boost_mode(unsigned int *abms, unsigned int *abr)
+{
+	unsigned int mask;
+	unsigned int value;
+	pmic_version_t mc13783_ver;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+		if (suspend_flag == 1) {
+			return -EBUSY;
+		}
+
+		mask = BITFMASK(BITS_BOOST_ABMS);
+		CHECK_ERROR(pmic_read_reg(LREG_0, &value, mask));
+		*abms = BITFEXT(value, BITS_BOOST_ABMS);
+
+		mask = BITFMASK(BITS_BOOST_ABR);
+		CHECK_ERROR(pmic_read_reg(LREG_0, &value, mask));
+		*abr = BITFEXT(value, BITS_BOOST_ABR);
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function implements IOCTL controls on a PMIC Light device.
+ *
+
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @param        cmd         the command
+ * @param        arg         the parameter
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_light_ioctl(struct inode *inode, struct file *file,
+			    unsigned int cmd, unsigned long arg)
+{
+	t_bklit_setting_param *bklit_setting = NULL;
+	t_tcled_enable_param *tcled_setting;
+	t_fun_param *fun_param;
+	t_tcled_ind_param *tcled_ind;
+
+	if (_IOC_TYPE(cmd) != 'p')
+		return -ENOTTY;
+
+	switch (cmd) {
+	case PMIC_BKLIT_TCLED_ENABLE:
+		pmic_bklit_tcled_master_enable();
+		break;
+
+	case PMIC_BKLIT_TCLED_DISABLE:
+		pmic_bklit_tcled_master_disable();
+		break;
+
+	case PMIC_BKLIT_ENABLE:
+		pmic_bklit_master_enable();
+		break;
+
+	case PMIC_BKLIT_DISABLE:
+		pmic_bklit_master_disable();
+		break;
+
+	case PMIC_SET_BKLIT:
+		if ((bklit_setting = kmalloc(sizeof(t_bklit_setting_param),
+					     GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(bklit_setting, (t_bklit_setting_param *) arg,
+				   sizeof(t_bklit_setting_param))) {
+			kfree(bklit_setting);
+			return -EFAULT;
+		}
+
+		CHECK_ERROR_KFREE(pmic_bklit_set_mode(bklit_setting->channel,
+						      bklit_setting->mode),
+				  (kfree(bklit_setting)));
+
+		CHECK_ERROR_KFREE(pmic_bklit_set_current(bklit_setting->channel,
+							 bklit_setting->
+							 current_level),
+				  (kfree(bklit_setting)));
+		CHECK_ERROR_KFREE(pmic_bklit_set_dutycycle
+				  (bklit_setting->channel,
+				   bklit_setting->duty_cycle),
+				  (kfree(bklit_setting)));
+		CHECK_ERROR_KFREE(pmic_bklit_set_cycle_time
+				  (bklit_setting->cycle_time),
+				  (kfree(bklit_setting)));
+		CHECK_ERROR_KFREE(pmic_bklit_set_boost_mode
+				  (bklit_setting->en_dis),
+				  (kfree(bklit_setting)));
+		CHECK_ERROR_KFREE(pmic_bklit_config_boost_mode
+				  (bklit_setting->abms, bklit_setting->abr),
+				  (kfree(bklit_setting)));
+		if (bklit_setting->edge_slow != false) {
+			CHECK_ERROR_KFREE(pmic_bklit_enable_edge_slow(),
+					  (kfree(bklit_setting)));
+		} else {
+			CHECK_ERROR_KFREE(pmic_bklit_disable_edge_slow(),
+					  (kfree(bklit_setting)));
+		}
+
+		kfree(bklit_setting);
+		break;
+
+	case PMIC_GET_BKLIT:
+		if ((bklit_setting = kmalloc(sizeof(t_bklit_setting_param),
+					     GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+
+		if (copy_from_user(bklit_setting, (t_bklit_setting_param *) arg,
+				   sizeof(t_bklit_setting_param))) {
+			kfree(bklit_setting);
+			return -EFAULT;
+		}
+
+		CHECK_ERROR_KFREE(pmic_bklit_get_current(bklit_setting->channel,
+							 &bklit_setting->
+							 current_level),
+				  (kfree(bklit_setting)));
+		CHECK_ERROR_KFREE(pmic_bklit_get_cycle_time
+				  (&bklit_setting->cycle_time),
+				  (kfree(bklit_setting)));
+		CHECK_ERROR_KFREE(pmic_bklit_get_dutycycle
+				  (bklit_setting->channel,
+				   &bklit_setting->duty_cycle),
+				  (kfree(bklit_setting)));
+		bklit_setting->strobe = BACKLIGHT_STROBE_NONE;
+		CHECK_ERROR_KFREE(pmic_bklit_get_mode(bklit_setting->channel,
+						      &bklit_setting->mode),
+				  (kfree(bklit_setting)));
+		CHECK_ERROR_KFREE(pmic_bklit_get_edge_slow
+				  (&bklit_setting->edge_slow),
+				  (kfree(bklit_setting)));
+		CHECK_ERROR_KFREE(pmic_bklit_get_boost_mode
+				  (&bklit_setting->en_dis),
+				  (kfree(bklit_setting)));
+		CHECK_ERROR_KFREE(pmic_bklit_gets_boost_mode
+				  (&bklit_setting->abms, &bklit_setting->abr),
+				  (kfree(bklit_setting)));
+
+		if (copy_to_user((t_bklit_setting_param *) arg, bklit_setting,
+				 sizeof(t_bklit_setting_param))) {
+			kfree(bklit_setting);
+			return -EFAULT;
+		}
+		kfree(bklit_setting);
+		break;
+
+	case PMIC_RAMPUP_BKLIT:
+		CHECK_ERROR(pmic_bklit_rampup((t_bklit_channel) arg));
+		break;
+
+	case PMIC_RAMPDOWN_BKLIT:
+		CHECK_ERROR(pmic_bklit_rampdown((t_bklit_channel) arg));
+		break;
+
+	case PMIC_OFF_RAMPUP_BKLIT:
+		CHECK_ERROR(pmic_bklit_off_rampup((t_bklit_channel) arg));
+		break;
+
+	case PMIC_OFF_RAMPDOWN_BKLIT:
+		CHECK_ERROR(pmic_bklit_off_rampdown((t_bklit_channel) arg));
+		break;
+
+	case PMIC_TCLED_ENABLE:
+		if ((tcled_setting = kmalloc(sizeof(t_tcled_enable_param),
+					     GFP_KERNEL))
+		    == NULL) {
+			return -ENOMEM;
+		}
+
+		if (copy_from_user(tcled_setting, (t_tcled_enable_param *) arg,
+				   sizeof(t_tcled_enable_param))) {
+			kfree(tcled_setting);
+			return -EFAULT;
+		}
+		CHECK_ERROR_KFREE(pmic_tcled_enable(tcled_setting->mode,
+						    tcled_setting->bank),
+				  (kfree(bklit_setting)));
+		break;
+
+	case PMIC_TCLED_DISABLE:
+		CHECK_ERROR(pmic_tcled_disable((t_funlight_bank) arg));
+		break;
+
+	case PMIC_TCLED_PATTERN:
+		if ((fun_param = kmalloc(sizeof(t_fun_param),
+					 GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(fun_param,
+				   (t_fun_param *) arg, sizeof(t_fun_param))) {
+			kfree(fun_param);
+			return -EFAULT;
+		}
+
+		switch (fun_param->pattern) {
+		case BLENDED_RAMPS_SLOW:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_blendedramps
+					  (fun_param->bank, TC_SLOW),
+					  (kfree(fun_param)));
+			break;
+
+		case BLENDED_RAMPS_FAST:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_blendedramps
+					  (fun_param->bank, TC_FAST),
+					  (kfree(fun_param)));
+			break;
+
+		case SAW_RAMPS_SLOW:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_sawramps
+					  (fun_param->bank, TC_SLOW),
+					  (kfree(fun_param)));
+			break;
+
+		case SAW_RAMPS_FAST:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_sawramps
+					  (fun_param->bank, TC_FAST),
+					  (kfree(fun_param)));
+			break;
+
+		case BLENDED_BOWTIE_SLOW:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_blendedbowtie
+					  (fun_param->bank, TC_SLOW),
+					  (kfree(fun_param)));
+			break;
+
+		case BLENDED_BOWTIE_FAST:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_blendedbowtie
+					  (fun_param->bank, TC_FAST),
+					  (kfree(fun_param)));
+			break;
+
+		case STROBE_SLOW:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_strobe
+					  (fun_param->bank, fun_param->channel,
+					   TC_STROBE_SLOW), (kfree(fun_param)));
+			break;
+
+		case STROBE_FAST:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_strobe
+					  (fun_param->bank,
+					   fun_param->channel, TC_STROBE_SLOW),
+					  (kfree(fun_param)));
+			break;
+
+		case CHASING_LIGHT_RGB_SLOW:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_chasinglightspattern
+					  (fun_param->bank, PMIC_RGB, TC_SLOW),
+					  (kfree(fun_param)));
+			break;
+
+		case CHASING_LIGHT_RGB_FAST:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_chasinglightspattern
+					  (fun_param->bank, PMIC_RGB, TC_FAST),
+					  (kfree(fun_param)));
+			break;
+
+		case CHASING_LIGHT_BGR_SLOW:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_chasinglightspattern
+					  (fun_param->bank, BGR, TC_SLOW),
+					  (kfree(fun_param)));
+			break;
+
+		case CHASING_LIGHT_BGR_FAST:
+			CHECK_ERROR_KFREE(pmic_tcled_fun_chasinglightspattern
+					  (fun_param->bank, BGR, TC_FAST),
+					  (kfree(fun_param)));
+			break;
+		}
+
+		kfree(fun_param);
+		break;
+
+	case PMIC_SET_TCLED:
+		if ((tcled_ind = kmalloc(sizeof(t_tcled_ind_param), GFP_KERNEL))
+		    == NULL) {
+			return -ENOMEM;
+		}
+
+		if (copy_from_user(tcled_ind, (t_tcled_ind_param *) arg,
+				   sizeof(t_tcled_ind_param))) {
+			kfree(tcled_ind);
+			return -EFAULT;
+		}
+		CHECK_ERROR_KFREE(pmic_tcled_ind_set_current(tcled_ind->channel,
+							     tcled_ind->level,
+							     tcled_ind->bank),
+				  (kfree(tcled_ind)));
+		CHECK_ERROR_KFREE(pmic_tcled_ind_set_blink_pattern
+				  (tcled_ind->channel, tcled_ind->pattern,
+				   tcled_ind->skip, tcled_ind->bank),
+				  (kfree(tcled_ind)));
+		CHECK_ERROR_KFREE(pmic_tcled_fun_rampup
+				  (tcled_ind->bank, tcled_ind->channel,
+				   tcled_ind->rampup), (kfree(tcled_ind)));
+		CHECK_ERROR_KFREE(pmic_tcled_fun_rampdown
+				  (tcled_ind->bank, tcled_ind->channel,
+				   tcled_ind->rampdown), (kfree(tcled_ind)));
+		if (tcled_ind->half_current) {
+			CHECK_ERROR_KFREE(pmic_tcled_enable_half_current(),
+					  (kfree(tcled_ind)));
+		} else {
+			CHECK_ERROR_KFREE(pmic_tcled_disable_half_current(),
+					  (kfree(tcled_ind)));
+		}
+
+		kfree(tcled_ind);
+		break;
+
+	case PMIC_GET_TCLED:
+		if ((tcled_ind = kmalloc(sizeof(t_tcled_ind_param), GFP_KERNEL))
+		    == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(tcled_ind, (t_tcled_ind_param *) arg,
+				   sizeof(t_tcled_ind_param))) {
+			kfree(tcled_ind);
+			return -EFAULT;
+		}
+		CHECK_ERROR_KFREE(pmic_tcled_ind_get_current(tcled_ind->channel,
+							     &tcled_ind->level,
+							     tcled_ind->bank),
+				  (kfree(tcled_ind)));
+		CHECK_ERROR_KFREE(pmic_tcled_ind_get_blink_pattern
+				  (tcled_ind->channel, &tcled_ind->pattern,
+				   &tcled_ind->skip, tcled_ind->bank),
+				  (kfree(tcled_ind)));
+		CHECK_ERROR_KFREE(pmic_tcled_get_fun_rampup
+				  (tcled_ind->bank, tcled_ind->channel,
+				   &tcled_ind->rampup), (kfree(tcled_ind)));
+		CHECK_ERROR_KFREE(pmic_tcled_get_fun_rampdown
+				  (tcled_ind->bank, tcled_ind->channel,
+				   &tcled_ind->rampdown), (kfree(tcled_ind)));
+		if (copy_to_user
+		    ((t_tcled_ind_param *) arg, tcled_ind,
+		     sizeof(t_tcled_ind_param))) {
+			return -EFAULT;
+		}
+		kfree(tcled_ind);
+
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*!
+ * This function initialize Light registers of mc13783 with 0.
+ *
+ * @return       This function returns 0 if successful.
+ */
+int pmic_light_init_reg(void)
+{
+	CHECK_ERROR(pmic_write_reg(LREG_0, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(LREG_1, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(LREG_2, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(LREG_3, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(LREG_4, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(LREG_5, 0, PMIC_ALL_BITS));
+	return 0;
+}
+
+/*!
+ * This function implements the open method on a mc13783 light device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_light_open(struct inode *inode, struct file *file)
+{
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0))) {
+			return -ERESTARTSYS;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * This function implements the release method on a mc13783 light device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_light_release(struct inode *inode, struct file *file)
+{
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0))) {
+			return -ERESTARTSYS;
+		}
+	}
+	return 0;
+}
+
+static struct file_operations pmic_light_fops = {
+	.owner = THIS_MODULE,
+	.ioctl = pmic_light_ioctl,
+	.open = pmic_light_open,
+	.release = pmic_light_release,
+};
+
+static int pmic_light_remove(struct platform_device *pdev)
+{
+	device_destroy(pmic_light_class, MKDEV(pmic_light_major, 0));
+	class_destroy(pmic_light_class);
+	unregister_chrdev(pmic_light_major, "pmic_light");
+	return 0;
+}
+
+static int pmic_light_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct device *temp_class;
+
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0))) {
+			return -ERESTARTSYS;
+		}
+	}
+	pmic_light_major = register_chrdev(0, "pmic_light", &pmic_light_fops);
+
+	if (pmic_light_major < 0) {
+		printk(KERN_ERR "Unable to get a major for pmic_light\n");
+		return pmic_light_major;
+	}
+	init_waitqueue_head(&suspendq);
+
+	pmic_light_class = class_create(THIS_MODULE, "pmic_light");
+	if (IS_ERR(pmic_light_class)) {
+		printk(KERN_ERR "Error creating pmic_light class.\n");
+		ret = PTR_ERR(pmic_light_class);
+		goto err_out1;
+	}
+
+	temp_class = device_create(pmic_light_class, NULL,
+				   MKDEV(pmic_light_major, 0), NULL,
+				   "pmic_light");
+	if (IS_ERR(temp_class)) {
+		printk(KERN_ERR "Error creating pmic_light class device.\n");
+		ret = PTR_ERR(temp_class);
+		goto err_out2;
+	}
+
+	ret = pmic_light_init_reg();
+	if (ret != PMIC_SUCCESS) {
+		goto err_out3;
+	}
+
+	printk(KERN_INFO "PMIC Light successfully loaded\n");
+	return ret;
+
+      err_out3:
+	device_destroy(pmic_light_class, MKDEV(pmic_light_major, 0));
+      err_out2:
+	class_destroy(pmic_light_class);
+      err_out1:
+	unregister_chrdev(pmic_light_major, "pmic_light");
+	return ret;
+}
+
+static struct platform_driver pmic_light_driver_ldm = {
+	.driver = {
+		   .name = "pmic_light",
+		   },
+	.suspend = pmic_light_suspend,
+	.resume = pmic_light_resume,
+	.probe = pmic_light_probe,
+	.remove = pmic_light_remove,
+};
+
+/*
+ * Initialization and Exit
+ */
+
+static int __init pmic_light_init(void)
+{
+	pr_debug("PMIC Light driver loading...\n");
+	return platform_driver_register(&pmic_light_driver_ldm);
+}
+static void __exit pmic_light_exit(void)
+{
+	platform_driver_unregister(&pmic_light_driver_ldm);
+	pr_debug("PMIC Light driver successfully unloaded\n");
+}
+
+/*
+ * Module entry points
+ */
+
+subsys_initcall(pmic_light_init);
+module_exit(pmic_light_exit);
+
+MODULE_DESCRIPTION("PMIC_LIGHT");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/mc13783/pmic_light_defs.h b/drivers/mxc/pmic/mc13783/pmic_light_defs.h
new file mode 100644
index 000000000000..80082363d9ed
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_light_defs.h
@@ -0,0 +1,144 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_light_defs.h
+ * @brief This is the internal header PMIC(mc13783) Light and Backlight driver.
+ *
+ * @ingroup PMIC_LIGHT
+ */
+
+#ifndef __MC13783_LIGHT_DEFS_H__
+#define __MC13783_LIGHT_DEFS_H__
+
+#define LREG_0   REG_LED_CONTROL_0
+#define LREG_1   REG_LED_CONTROL_1
+#define LREG_2   REG_LED_CONTROL_2
+#define LREG_3   REG_LED_CONTROL_3
+#define LREG_4   REG_LED_CONTROL_4
+#define LREG_5   REG_LED_CONTROL_5
+
+/* REG_LED_CONTROL_0 */
+
+#define         BIT_LEDEN_LSH           0
+#define         BIT_LEDEN_WID           1
+#define         MASK_TRIODE_MAIN_BL     0x080
+#define         INDEX_AUXILIARY         1
+#define         INDEX_KEYPAD            2
+#define         BITS_FUN_LIGHT_LSH      17
+#define         BITS_FUN_LIGHT_WID      4
+#define         MASK_FUN_LIGHT          0x1E0000
+#define         MASK_BK1_FL             0x200000
+#define         ENABLE_BK1_FL           0x200000
+#define         MASK_BK2_FL             0x400000
+#define         ENABLE_BK2_FL           0x400000
+#define         MASK_BK3_FL             0x800000
+#define         ENABLE_BK3_FL           0x800000
+#define 	BIT_UP_MAIN_BL_LSH	1
+#define 	BIT_UP_MAIN_BL_WID	1
+#define 	BIT_UP_AUX_BL_LSH	2
+#define 	BIT_UP_AUX_BL_WID	1
+#define 	BIT_UP_KEY_BL_LSH	3
+#define 	BIT_UP_KEY_BL_WID	1
+#define 	BIT_DOWN_MAIN_BL_LSH	4
+#define 	BIT_DOWN_MAIN_BL_WID	1
+#define 	BIT_DOWN_AUX_BL_LSH	5
+#define 	BIT_DOWN_AUX_BL_WID	1
+#define 	BIT_DOWN_KEY_BL_LSH	6
+#define 	BIT_DOWN_KEY_BL_WID	1
+#define 	BIT_TRIODE_MAIN_BL_LSH	7
+#define 	BIT_TRIODE_MAIN_BL_WID	1
+#define 	BIT_TRIODE_AUX_BL_LSH	8
+#define 	BIT_TRIODE_AUX_BL_WID	1
+#define 	BIT_TRIODE_KEY_BL_LSH	9
+#define 	BIT_TRIODE_KEY_BL_WID	1
+
+#define         BIT_BOOSTEN_LSH         10
+#define         BIT_BOOSTEN_WID         1
+#define         BITS_BOOST_LSH          11
+#define         BITS_BOOST_WID          5
+#define         BITS_BOOST_ABMS_LSH     11
+#define         BITS_BOOST_ABMS_WID     3
+#define         BITS_BOOST_ABR_LSH      14
+#define         BITS_BOOST_ABR_WID      2
+
+#define         MAX_BOOST_ABMS          7
+#define         MAX_BOOST_ABR           3
+
+/* REG_LED_CONTROL_1 */
+
+#define         BIT_SLEWLIMTC_LSH       23
+#define         BIT_SLEWLIMTC_WID       1
+#define         BIT_TC1HALF_LSH         18
+#define         BIT_TC1HALF_WID         1
+#define         LEDR1RAMPUP             0x000001
+#define         LEDR2RAMPUP             0x000040
+#define         LEDR3RAMPUP             0x001000
+#define         LEDR1RAMPDOWN           0x000008
+#define         LEDR2RAMPDOWN           0x000200
+#define         LEDR3RAMPDOWN           0x008000
+
+/* REG_LED_CONTROL_2 */
+
+#define         BIT_SLEWLIMBL_LSH       23
+#define         BIT_SLEWLIMBL_WID       1
+#define         BIT_DUTY_CYCLE          9
+#define         MASK_DUTY_CYCLE         0x001E00
+#define         INDEX_AUX               4
+#define         INDEX_KYD               8
+#define         BIT_CL_MAIN_LSH		0
+#define         BIT_CL_MAIN_WID		3
+#define         BIT_CL_AUX_LSH		3
+#define         BIT_CL_AUX_WID		3
+#define         BIT_CL_KEY_LSH		6
+#define         BIT_CL_KEY_WID		3
+
+/* REG_LED_CONTROL_3 4 5 */
+#define         BITS_CL_RED_LSH         0
+#define         BITS_CL_RED_WID         2
+#define         BITS_CL_GREEN_LSH       2
+#define         BITS_CL_GREEN_WID       2
+#define         BITS_CL_BLUE_LSH        4
+#define         BITS_CL_BLUE_WID        2
+#define         BITS_DC_RED_LSH         6
+#define         BITS_DC_RED_WID         5
+#define         BITS_DC_GREEN_LSH       11
+#define         BITS_DC_GREEN_WID       5
+#define         BITS_DC_BLUE_LSH        16
+#define         BITS_DC_BLUE_WID        5
+#define         BIT_PERIOD_LSH          21
+#define         BIT_PERIOD_WID          2
+
+#define         DUTY_CYCLE_MAX          31
+
+/* Fun light pattern */
+#define		BLENDED_RAMPS_SLOW      	0
+#define		BLENDED_RAMPS_FAST      	1
+#define		SAW_RAMPS_SLOW  		2
+#define		SAW_RAMPS_FAST	     		3
+#define		BLENDED_INVERSE_RAMPS_SLOW	4
+#define		BLENDED_INVERSE_RAMPS_FAST	5
+#define		CHASING_LIGHTS_RGB_SLOW		6
+#define		CHASING_LIGHTS_RGB_FAST		7
+#define		CHASING_LIGHTS_BGR_SLOW		8
+#define		CHASING_LIGHTS_BGR_FAST		9
+#define		FUN_LIGHTS_OFF			15
+
+/*!
+ * This function initialize Light registers of mc13783 with 0.
+ *
+ * @return       This function returns 0 if successful.
+ */
+int pmic_light_init_reg(void);
+
+#endif				/*  __MC13783_LIGHT_DEFS_H__ */
diff --git a/drivers/mxc/pmic/mc13783/pmic_power.c b/drivers/mxc/pmic/mc13783/pmic_power.c
new file mode 100644
index 000000000000..8f877b887e16
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_power.c
@@ -0,0 +1,3146 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_power.c
+ * @brief This is the main file of PMIC(mc13783) Power driver.
+ *
+ * @ingroup PMIC_POWER
+ */
+
+/*
+ * Includes
+ */
+
+#include <linux/platform_device.h>
+#include <linux/ioctl.h>
+#include <linux/pmic_status.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <mach/pmic_power.h>
+
+#include "pmic_power_defs.h"
+
+#ifdef CONFIG_MXC_HWEVENT
+#include <mach/hw_events.h>
+#endif
+
+#include <asm/mach-types.h>
+
+#define MC13783_REGCTRL_GPOx_MASK 0x18000
+
+static bool VBKUP1_EN = false;
+static bool VBKUP2_EN = false;
+
+/*
+ * Power Pmic API
+ */
+
+/* EXPORTED FUNCTIONS */
+EXPORT_SYMBOL(pmic_power_off);
+EXPORT_SYMBOL(pmic_power_set_pc_config);
+EXPORT_SYMBOL(pmic_power_get_pc_config);
+EXPORT_SYMBOL(pmic_power_regulator_on);
+EXPORT_SYMBOL(pmic_power_regulator_off);
+EXPORT_SYMBOL(pmic_power_regulator_set_voltage);
+EXPORT_SYMBOL(pmic_power_regulator_get_voltage);
+EXPORT_SYMBOL(pmic_power_regulator_set_config);
+EXPORT_SYMBOL(pmic_power_regulator_get_config);
+EXPORT_SYMBOL(pmic_power_vbkup2_auto_en);
+EXPORT_SYMBOL(pmic_power_get_vbkup2_auto_state);
+EXPORT_SYMBOL(pmic_power_bat_det_en);
+EXPORT_SYMBOL(pmic_power_get_bat_det_state);
+EXPORT_SYMBOL(pmic_power_vib_pin_en);
+EXPORT_SYMBOL(pmic_power_gets_vib_pin_state);
+EXPORT_SYMBOL(pmic_power_get_power_mode_sense);
+EXPORT_SYMBOL(pmic_power_set_regen_assig);
+EXPORT_SYMBOL(pmic_power_get_regen_assig);
+EXPORT_SYMBOL(pmic_power_set_regen_inv);
+EXPORT_SYMBOL(pmic_power_get_regen_inv);
+EXPORT_SYMBOL(pmic_power_esim_v_en);
+EXPORT_SYMBOL(pmic_power_gets_esim_v_state);
+EXPORT_SYMBOL(pmic_power_set_auto_reset_en);
+EXPORT_SYMBOL(pmic_power_get_auto_reset_en);
+EXPORT_SYMBOL(pmic_power_set_conf_button);
+EXPORT_SYMBOL(pmic_power_get_conf_button);
+EXPORT_SYMBOL(pmic_power_event_sub);
+EXPORT_SYMBOL(pmic_power_event_unsub);
+
+/*!
+ * This function is called to put the power in a low power state.
+ * Switching off the platform cannot be decided by
+ * the power module. It has to be handled by the
+ * client application.
+ *
+ * @param   pdev  the device structure used to give information on which power
+ *                device (0 through 3 channels) to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function always returns 0.
+ */
+static int pmic_power_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	return 0;
+};
+
+/*!
+ * This function is called to resume the power from a low power state.
+ *
+ * @param   pdev  the device structure used to give information on which power
+ *                device (0 through 3 channels) to suspend
+ *
+ * @return  The function always returns 0.
+ */
+static int pmic_power_resume(struct platform_device *pdev)
+{
+	return 0;
+};
+
+/*!
+ * This function sets user power off in power control register and thus powers
+ * off the phone.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+void pmic_power_off(void)
+{
+	unsigned int mask, value;
+
+	mask = BITFMASK(MC13783_PWRCTRL_USER_OFF_SPI);
+	value = BITFVAL(MC13783_PWRCTRL_USER_OFF_SPI,
+			MC13783_PWRCTRL_USER_OFF_SPI_ENABLE);
+
+	pmic_write_reg(REG_POWER_CONTROL_0, value, mask);
+}
+
+/*!
+ * This function sets the power control configuration.
+ *
+ * @param        pc_config   power control configuration.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_set_pc_config(t_pc_config * pc_config)
+{
+	unsigned int pwrctrl_val_reg0 = 0;
+	unsigned int pwrctrl_val_reg1 = 0;
+	unsigned int pwrctrl_mask_reg0 = 0;
+	unsigned int pwrctrl_mask_reg1 = 0;
+
+	if (pc_config == NULL) {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	if (pc_config->pc_enable != false) {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_PCEN,
+					    MC13783_PWRCTRL_PCEN_ENABLE);
+		pwrctrl_val_reg1 |= BITFVAL(MC13783_PWRCTRL_PCT,
+					    pc_config->pc_timer);
+	} else {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_PCEN,
+					    MC13783_PWRCTRL_PCEN_DISABLE);
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_PCEN);
+	pwrctrl_mask_reg1 |= BITFMASK(MC13783_PWRCTRL_PCT);
+
+	if (pc_config->pc_count_enable != false) {
+
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_PC_COUNT_EN,
+					    MC13783_PWRCTRL_PC_COUNT_EN_ENABLE);
+		pwrctrl_val_reg1 |= BITFVAL(MC13783_PWRCTRL_PC_COUNT,
+					    pc_config->pc_count);
+		pwrctrl_val_reg1 |= BITFVAL(MC13783_PWRCTRL_PC_MAX_CNT,
+					    pc_config->pc_max_count);
+	} else {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_PC_COUNT_EN,
+					    MC13783_PWRCTRL_PC_COUNT_EN_DISABLE);
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_PC_COUNT_EN);
+	pwrctrl_mask_reg1 |= BITFMASK(MC13783_PWRCTRL_PC_MAX_CNT) |
+	    BITFMASK(MC13783_PWRCTRL_PC_COUNT);
+
+	if (pc_config->warm_enable != false) {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_WARM_EN,
+					    MC13783_PWRCTRL_WARM_EN_ENABLE);
+	} else {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_WARM_EN,
+					    MC13783_PWRCTRL_WARM_EN_DISABLE);
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_WARM_EN);
+
+	if (pc_config->user_off_pc != false) {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_USER_OFF_PC,
+					    MC13783_PWRCTRL_USER_OFF_PC_ENABLE);
+	} else {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_WARM_EN,
+					    MC13783_PWRCTRL_USER_OFF_PC_DISABLE);
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_USER_OFF_PC);
+
+	if (pc_config->clk_32k_user_off != false) {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_32OUT_USER_OFF,
+					    MC13783_PWRCTRL_32OUT_USER_OFF_ENABLE);
+	} else {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_32OUT_USER_OFF,
+					    MC13783_PWRCTRL_32OUT_USER_OFF_DISABLE);
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_32OUT_USER_OFF);
+
+	if (pc_config->clk_32k_enable != false) {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_32OUT_EN,
+					    MC13783_PWRCTRL_32OUT_EN_ENABLE);
+	} else {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_32OUT_EN,
+					    MC13783_PWRCTRL_32OUT_EN_DISABLE);
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_32OUT_EN);
+
+	if (pc_config->en_vbkup1 != false) {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_VBKUP1_EN,
+					    MC13783_PWRCTRL_VBKUP_ENABLE);
+		VBKUP1_EN = true;
+	} else {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_VBKUP1_EN,
+					    MC13783_PWRCTRL_VBKUP_DISABLE);
+		VBKUP1_EN = false;
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_VBKUP1_EN);
+
+	if (pc_config->en_vbkup2 != false) {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_VBKUP2_EN,
+					    MC13783_PWRCTRL_VBKUP_ENABLE);
+		VBKUP2_EN = true;
+	} else {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_VBKUP2_EN,
+					    MC13783_PWRCTRL_VBKUP_DISABLE);
+		VBKUP2_EN = false;
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_VBKUP2_EN);
+
+	if (pc_config->auto_en_vbkup1 != false) {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_VBKUP1_AUTO_EN,
+					    MC13783_PWRCTRL_VBKUP_ENABLE);
+	} else {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_VBKUP1_AUTO_EN,
+					    MC13783_PWRCTRL_VBKUP_DISABLE);
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_VBKUP1_AUTO_EN);
+
+	if (pc_config->auto_en_vbkup2 != false) {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_VBKUP2_AUTO_EN,
+					    MC13783_PWRCTRL_VBKUP_ENABLE);
+	} else {
+		pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_VBKUP2_AUTO_EN,
+					    MC13783_PWRCTRL_VBKUP_DISABLE);
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_VBKUP2_AUTO_EN);
+
+	if (VBKUP1_EN != false) {
+		if (pc_config->vhold_voltage > 3
+		    || pc_config->vhold_voltage < 0) {
+			return PMIC_PARAMETER_ERROR;
+		} else {
+
+			pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_VBKUP1,
+						    pc_config->vhold_voltage);
+		}
+	}
+	if (VBKUP2_EN != false) {
+		if (pc_config->vhold_voltage > 3
+		    || pc_config->vhold_voltage < 0) {
+			return PMIC_PARAMETER_ERROR;
+		} else {
+			pwrctrl_val_reg0 |= BITFVAL(MC13783_PWRCTRL_VBKUP2,
+						    pc_config->vhold_voltage2);
+		}
+	}
+	pwrctrl_mask_reg0 |= BITFMASK(MC13783_PWRCTRL_VBKUP1) |
+	    BITFMASK(MC13783_PWRCTRL_VBKUP2);
+
+	if (pc_config->mem_allon != false) {
+		pwrctrl_val_reg1 |= BITFVAL(MC13783_PWRCTRL_MEM_ALLON,
+					    MC13783_PWRCTRL_MEM_ALLON_ENABLE);
+		pwrctrl_val_reg1 |= BITFVAL(MC13783_PWRCTRL_MEM_TMR,
+					    pc_config->mem_timer);
+	} else {
+		pwrctrl_val_reg1 |= BITFVAL(MC13783_PWRCTRL_MEM_ALLON,
+					    MC13783_PWRCTRL_MEM_ALLON_DISABLE);
+	}
+	pwrctrl_mask_reg1 |= BITFMASK(MC13783_PWRCTRL_MEM_ALLON) |
+	    BITFMASK(MC13783_PWRCTRL_MEM_TMR);
+
+	CHECK_ERROR(pmic_write_reg(REG_POWER_CONTROL_0,
+				   pwrctrl_val_reg0, pwrctrl_mask_reg0));
+	CHECK_ERROR(pmic_write_reg(REG_POWER_CONTROL_1,
+				   pwrctrl_val_reg1, pwrctrl_mask_reg1));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives the power control configuration.
+ *
+ * @param        pc_config   pointer to power control configuration.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_get_pc_config(t_pc_config * pc_config)
+{
+	unsigned int pwrctrl_val_reg0 = 0;
+	unsigned int pwrctrl_val_reg1 = 0;
+
+	if (pc_config == NULL) {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(REG_POWER_CONTROL_0,
+				  &pwrctrl_val_reg0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_read_reg(REG_POWER_CONTROL_1,
+				  &pwrctrl_val_reg1, PMIC_ALL_BITS));
+
+	if (BITFEXT(pwrctrl_val_reg0, MC13783_PWRCTRL_PCEN)
+	    == MC13783_PWRCTRL_PCEN_ENABLE) {
+		pc_config->pc_enable = true;
+		pc_config->pc_timer = BITFEXT(pwrctrl_val_reg1,
+					      MC13783_PWRCTRL_PCT);
+
+	} else {
+		pc_config->pc_enable = false;
+		pc_config->pc_timer = 0;
+	}
+
+	if (BITFEXT(pwrctrl_val_reg0, MC13783_PWRCTRL_PC_COUNT_EN)
+	    == MC13783_PWRCTRL_PCEN_ENABLE) {
+		pc_config->pc_count_enable = true;
+		pc_config->pc_count = BITFEXT(pwrctrl_val_reg1,
+					      MC13783_PWRCTRL_PC_COUNT);
+		pc_config->pc_max_count = BITFEXT(pwrctrl_val_reg1,
+						  MC13783_PWRCTRL_PC_MAX_CNT);
+	} else {
+		pc_config->pc_count_enable = false;
+		pc_config->pc_count = 0;
+		pc_config->pc_max_count = 0;
+	}
+
+	if (BITFEXT(pwrctrl_val_reg0, MC13783_PWRCTRL_WARM_EN)
+	    == MC13783_PWRCTRL_WARM_EN_ENABLE) {
+		pc_config->warm_enable = true;
+	} else {
+		pc_config->warm_enable = false;
+	}
+
+	if (BITFEXT(pwrctrl_val_reg0, MC13783_PWRCTRL_USER_OFF_PC)
+	    == MC13783_PWRCTRL_USER_OFF_PC_ENABLE) {
+		pc_config->user_off_pc = true;
+	} else {
+		pc_config->user_off_pc = false;
+	}
+
+	if (BITFEXT(pwrctrl_val_reg0, MC13783_PWRCTRL_32OUT_USER_OFF)
+	    == MC13783_PWRCTRL_32OUT_USER_OFF_ENABLE) {
+		pc_config->clk_32k_user_off = true;
+	} else {
+		pc_config->clk_32k_user_off = false;
+	}
+
+	if (BITFEXT(pwrctrl_val_reg0, MC13783_PWRCTRL_32OUT_EN)
+	    == MC13783_PWRCTRL_32OUT_EN_ENABLE) {
+		pc_config->clk_32k_enable = true;
+	} else {
+		pc_config->clk_32k_enable = false;
+	}
+
+	if (BITFEXT(pwrctrl_val_reg0, MC13783_PWRCTRL_VBKUP1_AUTO_EN)
+	    == MC13783_PWRCTRL_VBKUP_ENABLE) {
+		pc_config->auto_en_vbkup1 = true;
+	} else {
+		pc_config->auto_en_vbkup1 = false;
+	}
+
+	if (BITFEXT(pwrctrl_val_reg0, MC13783_PWRCTRL_VBKUP2_AUTO_EN)
+	    == MC13783_PWRCTRL_VBKUP_ENABLE) {
+		pc_config->auto_en_vbkup2 = true;
+	} else {
+		pc_config->auto_en_vbkup2 = false;
+	}
+
+	if (BITFEXT(pwrctrl_val_reg0, MC13783_PWRCTRL_VBKUP1_EN)
+	    == MC13783_PWRCTRL_VBKUP_ENABLE) {
+		pc_config->en_vbkup1 = true;
+		pc_config->vhold_voltage = BITFEXT(pwrctrl_val_reg0,
+						   MC13783_PWRCTRL_VBKUP1);
+	} else {
+		pc_config->en_vbkup1 = false;
+		pc_config->vhold_voltage = 0;
+	}
+
+	if (BITFEXT(pwrctrl_val_reg0, MC13783_PWRCTRL_VBKUP2_EN)
+	    == MC13783_PWRCTRL_VBKUP_ENABLE) {
+		pc_config->en_vbkup2 = true;
+		pc_config->vhold_voltage2 = BITFEXT(pwrctrl_val_reg0,
+						    MC13783_PWRCTRL_VBKUP2);
+	} else {
+		pc_config->en_vbkup2 = false;
+		pc_config->vhold_voltage2 = 0;
+	}
+
+	if (BITFEXT(pwrctrl_val_reg1, MC13783_PWRCTRL_MEM_ALLON) ==
+	    MC13783_PWRCTRL_MEM_ALLON_ENABLE) {
+		pc_config->mem_allon = true;
+		pc_config->mem_timer = BITFEXT(pwrctrl_val_reg1,
+					       MC13783_PWRCTRL_MEM_TMR);
+	} else {
+		pc_config->mem_allon = false;
+		pc_config->mem_timer = 0;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function turns on a regulator.
+ *
+ * @param        regulator    The regulator to be truned on.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_regulator_on(t_pmic_regulator regulator)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_PLL:
+		reg_val = BITFVAL(MC13783_SWCTRL_PLL_EN,
+				  MC13783_SWCTRL_PLL_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_SWCTRL_PLL_EN);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW3:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW3_EN,
+				  MC13783_SWCTRL_SW3_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW3_EN);
+		reg = REG_SWITCHERS_5;
+		break;
+	case REGU_VAUDIO:
+		reg_val = BITFVAL(MC13783_REGCTRL_VAUDIO_EN,
+				  MC13783_REGCTRL_VAUDIO_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VAUDIO_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VIOHI:
+		reg_val = BITFVAL(MC13783_REGCTRL_VIOHI_EN,
+				  MC13783_REGCTRL_VIOHI_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VIOHI_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VIOLO:
+		reg_val = BITFVAL(MC13783_REGCTRL_VIOLO_EN,
+				  MC13783_REGCTRL_VIOLO_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VIOLO_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VDIG:
+		reg_val = BITFVAL(MC13783_REGCTRL_VDIG_EN,
+				  MC13783_REGCTRL_VDIG_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VDIG_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VGEN:
+		reg_val = BITFVAL(MC13783_REGCTRL_VGEN_EN,
+				  MC13783_REGCTRL_VGEN_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VGEN_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFDIG:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFDIG_EN,
+				  MC13783_REGCTRL_VRFDIG_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFDIG_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFREF:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFREF_EN,
+				  MC13783_REGCTRL_VRFREF_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFREF_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFCP:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFCP_EN,
+				  MC13783_REGCTRL_VRFCP_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFCP_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VSIM:
+		reg_val = BITFVAL(MC13783_REGCTRL_VSIM_EN,
+				  MC13783_REGCTRL_VSIM_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VSIM_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VESIM:
+		reg_val = BITFVAL(MC13783_REGCTRL_VESIM_EN,
+				  MC13783_REGCTRL_VESIM_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VESIM_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VCAM:
+		reg_val = BITFVAL(MC13783_REGCTRL_VCAM_EN,
+				  MC13783_REGCTRL_VCAM_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VCAM_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRFBG:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFBG_EN,
+				  MC13783_REGCTRL_VRFBG_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFBG_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VVIB:
+		reg_val = BITFVAL(MC13783_REGCTRL_VVIB_EN,
+				  MC13783_REGCTRL_VVIB_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VVIB_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRF1:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRF1_EN,
+				  MC13783_REGCTRL_VRF1_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRF1_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRF2:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRF2_EN,
+				  MC13783_REGCTRL_VRF2_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRF2_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VMMC1:
+		reg_val = BITFVAL(MC13783_REGCTRL_VMMC1_EN,
+				  MC13783_REGCTRL_VMMC1_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VMMC1_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VMMC2:
+		reg_val = BITFVAL(MC13783_REGCTRL_VMMC2_EN,
+				  MC13783_REGCTRL_VMMC2_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VMMC2_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_GPO1:
+		reg_val = BITFVAL(MC13783_REGCTRL_GPO1_EN,
+				  MC13783_REGCTRL_GPO1_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_GPO1_EN);
+		reg = REG_POWER_MISCELLANEOUS;
+		break;
+	case REGU_GPO2:
+		reg_val = BITFVAL(MC13783_REGCTRL_GPO2_EN,
+				  MC13783_REGCTRL_GPO2_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_GPO2_EN);
+		reg = REG_POWER_MISCELLANEOUS;
+		break;
+	case REGU_GPO3:
+		reg_val = BITFVAL(MC13783_REGCTRL_GPO3_EN,
+				  MC13783_REGCTRL_GPO3_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_GPO3_EN);
+		reg = REG_POWER_MISCELLANEOUS;
+		break;
+	case REGU_GPO4:
+		reg_val = BITFVAL(MC13783_REGCTRL_GPO4_EN,
+				  MC13783_REGCTRL_GPO4_EN_ENABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_GPO4_EN);
+		reg = REG_POWER_MISCELLANEOUS;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function turns off a regulator.
+ *
+ * @param        regulator    The regulator to be truned off.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_regulator_off(t_pmic_regulator regulator)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_PLL:
+		reg_val = BITFVAL(MC13783_SWCTRL_PLL_EN,
+				  MC13783_SWCTRL_PLL_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_SWCTRL_PLL_EN);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW3:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW3_EN,
+				  MC13783_SWCTRL_SW3_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW3_EN);
+		reg = REG_SWITCHERS_5;
+		break;
+	case REGU_VAUDIO:
+		reg_val = BITFVAL(MC13783_REGCTRL_VAUDIO_EN,
+				  MC13783_REGCTRL_VAUDIO_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VAUDIO_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VIOHI:
+		reg_val = BITFVAL(MC13783_REGCTRL_VIOHI_EN,
+				  MC13783_REGCTRL_VIOHI_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VIOHI_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VIOLO:
+		reg_val = BITFVAL(MC13783_REGCTRL_VIOLO_EN,
+				  MC13783_REGCTRL_VIOLO_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VIOLO_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VDIG:
+		reg_val = BITFVAL(MC13783_REGCTRL_VDIG_EN,
+				  MC13783_REGCTRL_VDIG_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VDIG_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VGEN:
+		reg_val = BITFVAL(MC13783_REGCTRL_VGEN_EN,
+				  MC13783_REGCTRL_VGEN_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VGEN_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFDIG:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFDIG_EN,
+				  MC13783_REGCTRL_VRFDIG_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFDIG_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFREF:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFREF_EN,
+				  MC13783_REGCTRL_VRFREF_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFREF_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFCP:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFCP_EN,
+				  MC13783_REGCTRL_VRFCP_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFCP_EN);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VSIM:
+		reg_val = BITFVAL(MC13783_REGCTRL_VSIM_EN,
+				  MC13783_REGCTRL_VSIM_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VSIM_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VESIM:
+		reg_val = BITFVAL(MC13783_REGCTRL_VESIM_EN,
+				  MC13783_REGCTRL_VESIM_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VESIM_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VCAM:
+		reg_val = BITFVAL(MC13783_REGCTRL_VCAM_EN,
+				  MC13783_REGCTRL_VCAM_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VCAM_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRFBG:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFBG_EN,
+				  MC13783_REGCTRL_VRFBG_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFBG_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VVIB:
+		reg_val = BITFVAL(MC13783_REGCTRL_VVIB_EN,
+				  MC13783_REGCTRL_VVIB_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VVIB_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRF1:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRF1_EN,
+				  MC13783_REGCTRL_VRF1_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRF1_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRF2:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRF2_EN,
+				  MC13783_REGCTRL_VRF2_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRF2_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VMMC1:
+		reg_val = BITFVAL(MC13783_REGCTRL_VMMC1_EN,
+				  MC13783_REGCTRL_VMMC1_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VMMC1_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VMMC2:
+		reg_val = BITFVAL(MC13783_REGCTRL_VMMC2_EN,
+				  MC13783_REGCTRL_VMMC2_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VMMC2_EN);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_GPO1:
+		reg_val = BITFVAL(MC13783_REGCTRL_GPO1_EN,
+				  MC13783_REGCTRL_GPO1_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_GPO1_EN);
+		reg = REG_POWER_MISCELLANEOUS;
+		break;
+	case REGU_GPO2:
+		reg_val = BITFVAL(MC13783_REGCTRL_GPO2_EN,
+				  MC13783_REGCTRL_GPO2_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_GPO2_EN);
+		reg = REG_POWER_MISCELLANEOUS;
+		break;
+	case REGU_GPO3:
+		reg_val = BITFVAL(MC13783_REGCTRL_GPO3_EN,
+				  MC13783_REGCTRL_GPO3_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_GPO3_EN);
+		reg = REG_POWER_MISCELLANEOUS;
+		break;
+	case REGU_GPO4:
+		reg_val = BITFVAL(MC13783_REGCTRL_GPO4_EN,
+				  MC13783_REGCTRL_GPO4_EN_DISABLE);
+		reg_mask = BITFMASK(MC13783_REGCTRL_GPO4_EN);
+		reg = REG_POWER_MISCELLANEOUS;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the regulator output voltage.
+ *
+ * @param        regulator    The regulator to be configured.
+ * @param        voltage      The regulator output voltage.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_regulator_set_voltage(t_pmic_regulator regulator,
+					     t_regulator_voltage voltage)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_SW1A:
+		if ((voltage.sw1a < SW1A_0_9V) || (voltage.sw1a > SW1A_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW1A, voltage.sw1a);
+		reg_mask = BITFMASK(MC13783_SWSET_SW1A);
+		reg = REG_SWITCHERS_0;
+		break;
+	case SW_SW1B:
+		if ((voltage.sw1b < SW1B_0_9V) || (voltage.sw1b > SW1B_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW1B, voltage.sw1b);
+		reg_mask = BITFMASK(MC13783_SWSET_SW1B);
+		reg = REG_SWITCHERS_1;
+		break;
+	case SW_SW2A:
+		if ((voltage.sw2a < SW2A_0_9V) || (voltage.sw2a > SW2A_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW2A, voltage.sw1a);
+		reg_mask = BITFMASK(MC13783_SWSET_SW2A);
+		reg = REG_SWITCHERS_2;
+		break;
+	case SW_SW2B:
+		if ((voltage.sw2b < SW2B_0_9V) || (voltage.sw2b > SW2B_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW2B, voltage.sw2b);
+		reg_mask = BITFMASK(MC13783_SWSET_SW1A);
+		reg = REG_SWITCHERS_3;
+		break;
+	case SW_SW3:
+		if (voltage.sw3 != SW3_5V) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW3, voltage.sw3);
+		reg_mask = BITFMASK(MC13783_SWSET_SW3);
+		reg = REG_SWITCHERS_5;
+		break;
+	case REGU_VIOLO:
+		if ((voltage.violo < VIOLO_1_2V) ||
+		    (voltage.violo > VIOLO_1_8V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VIOLO, voltage.violo);
+		reg_mask = BITFMASK(MC13783_REGSET_VIOLO);
+		reg = REG_REGULATOR_SETTING_0;
+		break;
+	case REGU_VDIG:
+		if ((voltage.vdig < VDIG_1_2V) || (voltage.vdig > VDIG_1_8V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VDIG, voltage.vdig);
+		reg_mask = BITFMASK(MC13783_REGSET_VDIG);
+		reg = REG_REGULATOR_SETTING_0;
+		break;
+	case REGU_VGEN:
+		if ((voltage.vgen < VGEN_1_2V) || (voltage.vgen > VGEN_2_4V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VGEN, voltage.vgen);
+		reg_mask = BITFMASK(MC13783_REGSET_VGEN);
+		reg = REG_REGULATOR_SETTING_0;
+		break;
+	case REGU_VRFDIG:
+		if ((voltage.vrfdig < VRFDIG_1_2V) ||
+		    (voltage.vrfdig > VRFDIG_1_875V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VRFDIG, voltage.vrfdig);
+		reg_mask = BITFMASK(MC13783_REGSET_VRFDIG);
+		reg = REG_REGULATOR_SETTING_0;
+		break;
+	case REGU_VRFREF:
+		if ((voltage.vrfref < VRFREF_2_475V) ||
+		    (voltage.vrfref > VRFREF_2_775V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VRFREF, voltage.vrfref);
+		reg_mask = BITFMASK(MC13783_REGSET_VRFREF);
+		reg = REG_REGULATOR_SETTING_0;
+		break;
+	case REGU_VRFCP:
+		if ((voltage.vrfcp < VRFCP_2_7V) ||
+		    (voltage.vrfcp > VRFCP_2_775V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VRFCP, voltage.vrfcp);
+		reg_mask = BITFMASK(MC13783_REGSET_VRFCP);
+		reg = REG_REGULATOR_SETTING_0;
+		break;
+	case REGU_VSIM:
+		if ((voltage.vsim < VSIM_1_8V) || (voltage.vsim > VSIM_2_9V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VSIM, voltage.vsim);
+		reg_mask = BITFMASK(MC13783_REGSET_VSIM);
+		reg = REG_REGULATOR_SETTING_0;
+		break;
+	case REGU_VESIM:
+		if ((voltage.vesim < VESIM_1_8V) ||
+		    (voltage.vesim > VESIM_2_9V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VESIM, voltage.vesim);
+		reg_mask = BITFMASK(MC13783_REGSET_VESIM);
+		reg = REG_REGULATOR_SETTING_0;
+		break;
+	case REGU_VCAM:
+		if ((voltage.vcam < VCAM_1_5V) || (voltage.vcam > VCAM_3V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VCAM, voltage.vcam);
+		reg_mask = BITFMASK(MC13783_REGSET_VCAM);
+		reg = REG_REGULATOR_SETTING_0;
+		break;
+	case REGU_VVIB:
+		if ((voltage.vvib < VVIB_1_3V) || (voltage.vvib > VVIB_3V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VVIB, voltage.vvib);
+		reg_mask = BITFMASK(MC13783_REGSET_VVIB);
+		reg = REG_REGULATOR_SETTING_1;
+		break;
+	case REGU_VRF1:
+		if ((voltage.vrf1 < VRF1_1_5V) || (voltage.vrf1 > VRF1_2_775V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VRF1, voltage.vrf1);
+		reg_mask = BITFMASK(MC13783_REGSET_VRF1);
+		reg = REG_REGULATOR_SETTING_1;
+		break;
+	case REGU_VRF2:
+		if ((voltage.vrf2 < VRF2_1_5V) || (voltage.vrf2 > VRF2_2_775V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VRF2, voltage.vrf2);
+		reg_mask = BITFMASK(MC13783_REGSET_VRF2);
+		reg = REG_REGULATOR_SETTING_1;
+		break;
+	case REGU_VMMC1:
+		if ((voltage.vmmc1 < VMMC1_1_6V) || (voltage.vmmc1 > VMMC1_3V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VMMC1, voltage.vmmc1);
+		reg_mask = BITFMASK(MC13783_REGSET_VMMC1);
+		reg = REG_REGULATOR_SETTING_1;
+		break;
+	case REGU_VMMC2:
+		if ((voltage.vmmc2 < VMMC2_1_6V) || (voltage.vmmc2 > VMMC2_3V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGSET_VMMC2, voltage.vmmc2);
+		reg_mask = BITFMASK(MC13783_REGSET_VMMC2);
+		reg = REG_REGULATOR_SETTING_1;
+		break;
+	case REGU_VAUDIO:
+	case REGU_VIOHI:
+	case REGU_VRFBG:
+	case REGU_GPO1:
+	case REGU_GPO2:
+	case REGU_GPO3:
+	case REGU_GPO4:
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives the regulator output voltage.
+ *
+ * @param        regulator    The regulator to be truned off.
+ * @param        voltage      Pointer to regulator output voltage.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_regulator_get_voltage(t_pmic_regulator regulator,
+					     t_regulator_voltage * voltage)
+{
+	unsigned int reg_val = 0;
+
+	if (regulator == SW_SW1A) {
+		CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_0,
+					  &reg_val, PMIC_ALL_BITS));
+	} else if (regulator == SW_SW1B) {
+		CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_1,
+					  &reg_val, PMIC_ALL_BITS));
+	} else if (regulator == SW_SW2A) {
+		CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_2,
+					  &reg_val, PMIC_ALL_BITS));
+	} else if (regulator == SW_SW2B) {
+		CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_3,
+					  &reg_val, PMIC_ALL_BITS));
+	} else if (regulator == SW_SW3) {
+		CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_5,
+					  &reg_val, PMIC_ALL_BITS));
+	} else if ((regulator == REGU_VIOLO) || (regulator == REGU_VDIG) ||
+		   (regulator == REGU_VGEN) ||
+		   (regulator == REGU_VRFDIG) ||
+		   (regulator == REGU_VRFREF) ||
+		   (regulator == REGU_VRFCP) ||
+		   (regulator == REGU_VSIM) ||
+		   (regulator == REGU_VESIM) || (regulator == REGU_VCAM)) {
+		CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_0,
+					  &reg_val, PMIC_ALL_BITS));
+	} else if ((regulator == REGU_VVIB) || (regulator == REGU_VRF1) ||
+		   (regulator == REGU_VRF2) ||
+		   (regulator == REGU_VMMC1) || (regulator == REGU_VMMC2)) {
+		CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_1,
+					  &reg_val, PMIC_ALL_BITS));
+	}
+
+	switch (regulator) {
+	case SW_SW1A:
+		voltage->sw1a = BITFEXT(reg_val, MC13783_SWSET_SW1A);
+		break;
+	case SW_SW1B:
+		voltage->sw1b = BITFEXT(reg_val, MC13783_SWSET_SW1B);
+		break;
+	case SW_SW2A:
+		voltage->sw2a = BITFEXT(reg_val, MC13783_SWSET_SW2A);
+		break;
+	case SW_SW2B:
+		voltage->sw2b = BITFEXT(reg_val, MC13783_SWSET_SW2B);
+		break;
+	case SW_SW3:
+		voltage->sw3 = BITFEXT(reg_val, MC13783_SWSET_SW3);
+		break;
+	case REGU_VIOLO:
+		voltage->violo = BITFEXT(reg_val, MC13783_REGSET_VIOLO);
+		break;
+	case REGU_VDIG:
+		voltage->vdig = BITFEXT(reg_val, MC13783_REGSET_VDIG);
+		break;
+	case REGU_VGEN:
+		voltage->vgen = BITFEXT(reg_val, MC13783_REGSET_VGEN);
+		break;
+	case REGU_VRFDIG:
+		voltage->vrfdig = BITFEXT(reg_val, MC13783_REGSET_VRFDIG);
+		break;
+	case REGU_VRFREF:
+		voltage->vrfref = BITFEXT(reg_val, MC13783_REGSET_VRFREF);
+		break;
+	case REGU_VRFCP:
+		voltage->vrfcp = BITFEXT(reg_val, MC13783_REGSET_VRFCP);
+		break;
+	case REGU_VSIM:
+		voltage->vsim = BITFEXT(reg_val, MC13783_REGSET_VSIM);
+		break;
+	case REGU_VESIM:
+		voltage->vesim = BITFEXT(reg_val, MC13783_REGSET_VESIM);
+		break;
+	case REGU_VCAM:
+		voltage->vcam = BITFEXT(reg_val, MC13783_REGSET_VCAM);
+		break;
+	case REGU_VVIB:
+		voltage->vvib = BITFEXT(reg_val, MC13783_REGSET_VVIB);
+		break;
+	case REGU_VRF1:
+		voltage->vrf1 = BITFEXT(reg_val, MC13783_REGSET_VRF1);
+		break;
+	case REGU_VRF2:
+		voltage->vrf2 = BITFEXT(reg_val, MC13783_REGSET_VRF2);
+		break;
+	case REGU_VMMC1:
+		voltage->vmmc1 = BITFEXT(reg_val, MC13783_REGSET_VMMC1);
+		break;
+	case REGU_VMMC2:
+		voltage->vmmc2 = BITFEXT(reg_val, MC13783_REGSET_VMMC2);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the DVS voltage
+ *
+ * @param        regulator    The regulator to be configured.
+ * @param        dvs          The switch Dynamic Voltage Scaling
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_set_dvs(t_pmic_regulator regulator,
+					t_regulator_voltage dvs)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_SW1A:
+		if ((dvs.sw1a < SW1A_0_9V) || (dvs.sw1a > SW1A_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW1A_DVS, dvs.sw1a);
+		reg_mask = BITFMASK(MC13783_SWSET_SW1A_DVS);
+		reg = REG_SWITCHERS_0;
+		break;
+	case SW_SW1B:
+		if ((dvs.sw1b < SW1B_0_9V) || (dvs.sw1b > SW1B_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW1B_DVS, dvs.sw1b);
+		reg_mask = BITFMASK(MC13783_SWSET_SW1B_DVS);
+		reg = REG_SWITCHERS_1;
+		break;
+	case SW_SW2A:
+		if ((dvs.sw2a < SW2A_0_9V) || (dvs.sw2a > SW2A_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW2A_DVS, dvs.sw2a);
+		reg_mask = BITFMASK(MC13783_SWSET_SW2A_DVS);
+		reg = REG_SWITCHERS_2;
+		break;
+	case SW_SW2B:
+		if ((dvs.sw2b < SW2B_0_9V) || (dvs.sw2b > SW2B_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW2B_DVS, dvs.sw2b);
+		reg_mask = BITFMASK(MC13783_SWSET_SW2B_DVS);
+		reg = REG_SWITCHERS_3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets the DVS voltage
+ *
+ * @param        regulator    The regulator to be handled.
+ * @param        dvs          The switch Dynamic Voltage Scaling
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_get_dvs(t_pmic_regulator regulator,
+					t_regulator_voltage * dvs)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_SW1A:
+		reg_mask = BITFMASK(MC13783_SWSET_SW1A_DVS);
+		reg = REG_SWITCHERS_0;
+		break;
+	case SW_SW1B:
+		reg_mask = BITFMASK(MC13783_SWSET_SW1B_DVS);
+		reg = REG_SWITCHERS_1;
+		break;
+	case SW_SW2A:
+		reg_mask = BITFMASK(MC13783_SWSET_SW2A_DVS);
+		reg = REG_SWITCHERS_2;
+		break;
+	case SW_SW2B:
+		reg_mask = BITFMASK(MC13783_SWSET_SW2B_DVS);
+		reg = REG_SWITCHERS_3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &reg_val, reg_mask));
+
+	switch (regulator) {
+	case SW_SW1A:
+		*dvs = (t_regulator_voltage) BITFEXT(reg_val,
+						     MC13783_SWSET_SW1A_DVS);
+		break;
+	case SW_SW1B:
+		*dvs = (t_regulator_voltage) BITFEXT(reg_val,
+						     MC13783_SWSET_SW1B_DVS);
+		break;
+	case SW_SW2A:
+		*dvs = (t_regulator_voltage) BITFEXT(reg_val,
+						     MC13783_SWSET_SW2A_DVS);
+		break;
+	case SW_SW2B:
+		*dvs = (t_regulator_voltage) BITFEXT(reg_val,
+						     MC13783_SWSET_SW2B_DVS);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the standiby voltage
+ *
+ * @param        regulator    The regulator to be configured.
+ * @param        stby         The switch standby voltage
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_set_stby(t_pmic_regulator regulator,
+					 t_regulator_voltage stby)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_SW1A:
+		if ((stby.sw1a < SW1A_0_9V) || (stby.sw1a > SW1A_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW1A_STDBY, stby.sw1a);
+		reg_mask = BITFMASK(MC13783_SWSET_SW1A_STDBY);
+		reg = REG_SWITCHERS_0;
+		break;
+	case SW_SW1B:
+		if ((stby.sw1b < SW1B_0_9V) || (stby.sw1b > SW1B_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW1B_STDBY, stby.sw1b);
+		reg_mask = BITFMASK(MC13783_SWSET_SW1B_STDBY);
+		reg = REG_SWITCHERS_1;
+		break;
+	case SW_SW2A:
+		if ((stby.sw2a < SW2A_0_9V) || (stby.sw2a > SW2A_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW2A_STDBY, stby.sw2a);
+		reg_mask = BITFMASK(MC13783_SWSET_SW2A_STDBY);
+		reg = REG_SWITCHERS_2;
+		break;
+	case SW_SW2B:
+		if ((stby.sw2b < SW2B_0_9V) || (stby.sw2b > SW2B_2_2V)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_SWSET_SW2B_STDBY, stby.sw2b);
+		reg_mask = BITFMASK(MC13783_SWSET_SW2B_STDBY);
+		reg = REG_SWITCHERS_3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets the standiby voltage
+ *
+ * @param        regulator    The regulator to be handled.
+ * @param        stby         The switch standby voltage
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_get_stby(t_pmic_regulator regulator,
+					 t_regulator_voltage * stby)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_SW1A:
+		reg_mask = BITFMASK(MC13783_SWSET_SW1A_STDBY);
+		reg = REG_SWITCHERS_0;
+		break;
+	case SW_SW1B:
+		reg_mask = BITFMASK(MC13783_SWSET_SW1B_STDBY);
+		reg = REG_SWITCHERS_1;
+		break;
+	case SW_SW2A:
+		reg_mask = BITFMASK(MC13783_SWSET_SW2A_STDBY);
+		reg = REG_SWITCHERS_2;
+		break;
+	case SW_SW2B:
+		reg_mask = BITFMASK(MC13783_SWSET_SW2B_STDBY);
+		reg = REG_SWITCHERS_3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &reg_val, reg_mask));
+
+	switch (regulator) {
+	case SW_SW1A:
+		*stby = (t_regulator_voltage) BITFEXT(reg_val,
+						      MC13783_SWSET_SW1A_STDBY);
+		break;
+	case SW_SW1B:
+		*stby = (t_regulator_voltage) BITFEXT(reg_val,
+						      MC13783_SWSET_SW1B_STDBY);
+		break;
+	case SW_SW2A:
+		*stby = (t_regulator_voltage) BITFEXT(reg_val,
+						      MC13783_SWSET_SW2A_STDBY);
+		break;
+	case SW_SW2B:
+		*stby = (t_regulator_voltage) BITFEXT(reg_val,
+						      MC13783_SWSET_SW2B_STDBY);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the switchers mode.
+ *
+ * @param        regulator    The regulator to be configured.
+ * @param        mode	      The switcher mode
+ * @param        stby	      Switch between main and standby.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_set_mode(t_pmic_regulator regulator,
+					 t_regulator_sw_mode mode, bool stby)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+	unsigned int l_mode;
+
+	if (mode == SYNC_RECT) {
+		l_mode = MC13783_SWCTRL_SW_MODE_SYNC_RECT_EN;
+	} else if (mode == NO_PULSE_SKIP) {
+		l_mode = MC13783_SWCTRL_SW_MODE_PULSE_NO_SKIP_EN;
+	} else if (mode == PULSE_SKIP) {
+		l_mode = MC13783_SWCTRL_SW_MODE_PULSE_SKIP_EN;
+	} else if (mode == LOW_POWER) {
+		l_mode = MC13783_SWCTRL_SW_MODE_LOW_POWER_EN;
+	} else {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (regulator) {
+	case SW_SW1A:
+		if (stby) {
+			reg_val =
+			    BITFVAL(MC13783_SWCTRL_SW1A_STBY_MODE, l_mode);
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW1A_STBY_MODE);
+		} else {
+			reg_val = BITFVAL(MC13783_SWCTRL_SW1A_MODE, l_mode);
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW1A_MODE);
+		}
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW1B:
+		if (stby) {
+			reg_val =
+			    BITFVAL(MC13783_SWCTRL_SW1B_STBY_MODE, l_mode);
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW1B_STBY_MODE);
+		} else {
+			reg_val = BITFVAL(MC13783_SWCTRL_SW1B_MODE, l_mode);
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW1B_MODE);
+		}
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW2A:
+		if (stby) {
+			reg_val =
+			    BITFVAL(MC13783_SWCTRL_SW2A_STBY_MODE, l_mode);
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW2A_STBY_MODE);
+		} else {
+			reg_val = BITFVAL(MC13783_SWCTRL_SW2A_MODE, l_mode);
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW2A_MODE);
+		}
+		reg = REG_SWITCHERS_5;
+		break;
+	case SW_SW2B:
+		if (stby) {
+			reg_val =
+			    BITFVAL(MC13783_SWCTRL_SW2B_STBY_MODE, l_mode);
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW2B_STBY_MODE);
+		} else {
+			reg_val = BITFVAL(MC13783_SWCTRL_SW2B_MODE, l_mode);
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW2B_MODE);
+		}
+		reg = REG_SWITCHERS_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets the switchers mode.
+ *
+ * @param        regulator    The regulator to be handled.
+ * @param        mode         The switcher mode.
+ * @param        stby         Switch between main and standby.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_get_mode(t_pmic_regulator regulator,
+					 t_regulator_sw_mode * mode, bool stby)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg = 0;
+	unsigned int l_mode = 0;
+
+	switch (regulator) {
+	case SW_SW1A:
+		if (stby) {
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW1A_STBY_MODE);
+		} else {
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW1A_MODE);
+		}
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW1B:
+		if (stby) {
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW1B_STBY_MODE);
+		} else {
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW1B_MODE);
+		}
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW2A:
+		if (stby) {
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW2A_STBY_MODE);
+		} else {
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW2A_MODE);
+		}
+		reg = REG_SWITCHERS_5;
+		break;
+	case SW_SW2B:
+		if (stby) {
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW2B_STBY_MODE);
+		} else {
+			reg_mask = BITFMASK(MC13783_SWCTRL_SW2B_MODE);
+		}
+		reg = REG_SWITCHERS_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &reg_val, reg_mask));
+
+	switch (regulator) {
+	case SW_SW1A:
+		if (stby) {
+			l_mode =
+			    BITFEXT(reg_val, MC13783_SWCTRL_SW1A_STBY_MODE);
+		} else {
+			l_mode = BITFEXT(reg_val, MC13783_SWCTRL_SW1A_MODE);
+		}
+		break;
+	case SW_SW1B:
+		if (stby) {
+			l_mode =
+			    BITFEXT(reg_val, MC13783_SWCTRL_SW1B_STBY_MODE);
+		} else {
+			l_mode = BITFEXT(reg_val, MC13783_SWCTRL_SW1B_MODE);
+		}
+		break;
+	case SW_SW2A:
+		if (stby) {
+			l_mode =
+			    BITFEXT(reg_val, MC13783_SWCTRL_SW2A_STBY_MODE);
+		} else {
+			l_mode = BITFEXT(reg_val, MC13783_SWCTRL_SW2A_MODE);
+		}
+		break;
+	case SW_SW2B:
+		if (stby) {
+			l_mode =
+			    BITFEXT(reg_val, MC13783_SWCTRL_SW2B_STBY_MODE);
+		} else {
+			l_mode = BITFEXT(reg_val, MC13783_SWCTRL_SW2B_MODE);
+		}
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	if (l_mode == MC13783_SWCTRL_SW_MODE_SYNC_RECT_EN) {
+		*mode = SYNC_RECT;
+	} else if (l_mode == MC13783_SWCTRL_SW_MODE_PULSE_NO_SKIP_EN) {
+		*mode = NO_PULSE_SKIP;
+	} else if (l_mode == MC13783_SWCTRL_SW_MODE_PULSE_SKIP_EN) {
+		*mode = PULSE_SKIP;
+	} else if (l_mode == MC13783_SWCTRL_SW_MODE_LOW_POWER_EN) {
+		*mode = LOW_POWER;
+	} else {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the switch dvs speed
+ *
+ * @param        regulator    The regulator to be configured.
+ * @param        speed	      The dvs speed.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_set_dvs_speed(t_pmic_regulator regulator,
+					      t_switcher_dvs_speed speed)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+	if (speed > 3 || speed < 0) {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (regulator) {
+	case SW_SW1A:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW1A_DVS_SPEED, speed);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1A_DVS_SPEED);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW1B:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW2B_DVS_SPEED, speed);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1B_DVS_SPEED);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW2A:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW2A_DVS_SPEED, speed);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2A_DVS_SPEED);
+		reg = REG_SWITCHERS_5;
+		break;
+	case SW_SW2B:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW2B_DVS_SPEED, speed);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2B_DVS_SPEED);
+		reg = REG_SWITCHERS_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets the switch dvs speed
+ *
+ * @param        regulator    The regulator to be handled.
+ * @param        speed        The dvs speed.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_get_dvs_speed(t_pmic_regulator regulator,
+					      t_switcher_dvs_speed * speed)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_SW1A:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1A_DVS_SPEED);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW1B:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1B_DVS_SPEED);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW2A:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2A_DVS_SPEED);
+		reg = REG_SWITCHERS_5;
+		break;
+	case SW_SW2B:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2B_DVS_SPEED);
+		reg = REG_SWITCHERS_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &reg_val, reg_mask));
+
+	switch (regulator) {
+	case SW_SW1A:
+		*speed = BITFEXT(reg_val, MC13783_SWCTRL_SW1A_DVS_SPEED);
+		break;
+	case SW_SW1B:
+		*speed = BITFEXT(reg_val, MC13783_SWCTRL_SW1B_DVS_SPEED);
+		break;
+	case SW_SW2A:
+		*speed = BITFEXT(reg_val, MC13783_SWCTRL_SW2A_DVS_SPEED);
+		break;
+	case SW_SW2B:
+		*speed = BITFEXT(reg_val, MC13783_SWCTRL_SW2B_DVS_SPEED);
+		break;
+	default:
+		break;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the switch panic mode
+ *
+ * @param        regulator    The regulator to be configured.
+ * @param        panic_mode   Enable or disable panic mode
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_set_panic_mode(t_pmic_regulator regulator,
+					       bool panic_mode)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_SW1A:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW1A_PANIC_MODE, panic_mode);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1A_PANIC_MODE);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW1B:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW2B_PANIC_MODE, panic_mode);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1B_PANIC_MODE);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW2A:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW2A_PANIC_MODE, panic_mode);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2A_PANIC_MODE);
+		reg = REG_SWITCHERS_5;
+		break;
+	case SW_SW2B:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW2B_PANIC_MODE, panic_mode);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2B_PANIC_MODE);
+		reg = REG_SWITCHERS_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets the switch panic mode
+ *
+ * @param        regulator    The regulator to be handled
+ * @param        panic_mode   Enable or disable panic mode
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_get_panic_mode(t_pmic_regulator regulator,
+					       bool * panic_mode)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_SW1A:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1A_PANIC_MODE);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW1B:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1B_PANIC_MODE);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW2A:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2A_PANIC_MODE);
+		reg = REG_SWITCHERS_5;
+		break;
+	case SW_SW2B:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2B_PANIC_MODE);
+		reg = REG_SWITCHERS_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &reg_val, reg_mask));
+
+	switch (regulator) {
+	case SW_SW1A:
+		*panic_mode = BITFEXT(reg_val, MC13783_SWCTRL_SW1A_PANIC_MODE);
+		break;
+	case SW_SW1B:
+		*panic_mode = BITFEXT(reg_val, MC13783_SWCTRL_SW1B_PANIC_MODE);
+		break;
+	case SW_SW2A:
+		*panic_mode = BITFEXT(reg_val, MC13783_SWCTRL_SW2A_PANIC_MODE);
+		break;
+	case SW_SW2B:
+		*panic_mode = BITFEXT(reg_val, MC13783_SWCTRL_SW2B_PANIC_MODE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the switch softstart mode
+ *
+ * @param        regulator    The regulator to be configured.
+ * @param        softstart    Enable or disable softstart.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_set_softstart(t_pmic_regulator regulator,
+					      bool softstart)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_SW1A:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW1A_SOFTSTART, softstart);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1A_SOFTSTART);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW1B:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW2B_SOFTSTART, softstart);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1B_SOFTSTART);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW2A:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW2A_SOFTSTART, softstart);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2A_SOFTSTART);
+		reg = REG_SWITCHERS_5;
+		break;
+	case SW_SW2B:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW2B_SOFTSTART, softstart);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2B_SOFTSTART);
+		reg = REG_SWITCHERS_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets the switch softstart mode
+ *
+ * @param        regulator    The regulator to be handled
+ * @param        softstart    Enable or disable softstart.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_get_softstart(t_pmic_regulator regulator,
+					      bool * softstart)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+
+	switch (regulator) {
+	case SW_SW1A:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1A_SOFTSTART);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW1B:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW1B_SOFTSTART);
+		reg = REG_SWITCHERS_4;
+		break;
+	case SW_SW2A:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2A_SOFTSTART);
+		reg = REG_SWITCHERS_5;
+		break;
+	case SW_SW2B:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW2B_SOFTSTART);
+		reg = REG_SWITCHERS_5;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &reg_val, reg_mask));
+
+	switch (regulator) {
+	case SW_SW1A:
+		*softstart = BITFEXT(reg_val, MC13783_SWCTRL_SW1A_SOFTSTART);
+		break;
+	case SW_SW1B:
+		*softstart = BITFEXT(reg_val, MC13783_SWCTRL_SW2B_SOFTSTART);
+		break;
+	case SW_SW2A:
+		*softstart = BITFEXT(reg_val, MC13783_SWCTRL_SW2A_SOFTSTART);
+		break;
+	case SW_SW2B:
+		*softstart = BITFEXT(reg_val, MC13783_SWCTRL_SW2B_SOFTSTART);
+		break;
+	default:
+		break;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the PLL multiplication factor
+ *
+ * @param        regulator    The regulator to be configured.
+ * @param        factor       The multiplication factor.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_set_factor(t_pmic_regulator regulator,
+					   t_switcher_factor factor)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+
+	if (regulator != SW_PLL) {
+		return PMIC_PARAMETER_ERROR;
+	}
+	if (factor < FACTOR_28 || factor > FACTOR_35) {
+		return PMIC_PARAMETER_ERROR;
+	}
+	reg_val = BITFVAL(MC13783_SWCTRL_PLL_FACTOR, factor);
+	reg_mask = BITFMASK(MC13783_SWCTRL_PLL_FACTOR);
+
+	CHECK_ERROR(pmic_write_reg(REG_SWITCHERS_4, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets the PLL multiplication factor
+ *
+ * @param        regulator    The regulator to be handled
+ * @param        factor       The multiplication factor.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_switcher_get_factor(t_pmic_regulator regulator,
+					   t_switcher_factor * factor)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+
+	if (regulator != SW_PLL) {
+		return PMIC_PARAMETER_ERROR;
+	}
+	reg_mask = BITFMASK(MC13783_SWCTRL_PLL_FACTOR);
+
+	CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_4, &reg_val, reg_mask));
+
+	*factor = BITFEXT(reg_val, MC13783_SWCTRL_PLL_FACTOR);
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function enables or disables low power mode.
+ *
+ * @param        regulator    The regulator to be configured.
+ * @param        lp_mode      Select nominal or low power mode.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_regulator_set_lp_mode(t_pmic_regulator regulator,
+					     t_regulator_lp_mode lp_mode)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+	unsigned int l_mode, l_stby;
+
+	if (lp_mode == LOW_POWER_DISABLED) {
+		l_mode = MC13783_REGTRL_LP_MODE_DISABLE;
+		l_stby = MC13783_REGTRL_STBY_MODE_DISABLE;
+	} else if (lp_mode == LOW_POWER_CTRL_BY_PIN) {
+		l_mode = MC13783_REGTRL_LP_MODE_DISABLE;
+		l_stby = MC13783_REGTRL_STBY_MODE_ENABLE;
+	} else if (lp_mode == LOW_POWER_EN) {
+		l_mode = MC13783_REGTRL_LP_MODE_ENABLE;
+		l_stby = MC13783_REGTRL_STBY_MODE_DISABLE;
+	} else if (lp_mode == LOW_POWER_AND_LOW_POWER_CTRL_BY_PIN) {
+		l_mode = MC13783_REGTRL_LP_MODE_ENABLE;
+		l_stby = MC13783_REGTRL_STBY_MODE_ENABLE;
+	} else {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (regulator) {
+	case SW_SW3:
+		reg_val = BITFVAL(MC13783_SWCTRL_SW3_MODE, l_mode) |
+		    BITFVAL(MC13783_SWCTRL_SW3_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW3_MODE) |
+		    BITFMASK(MC13783_SWCTRL_SW3_STBY);
+		reg = REG_SWITCHERS_5;
+		break;
+	case REGU_VAUDIO:
+		reg_val = BITFVAL(MC13783_REGCTRL_VAUDIO_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VAUDIO_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VAUDIO_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VAUDIO_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VIOHI:
+		reg_val = BITFVAL(MC13783_REGCTRL_VIOHI_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VIOHI_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VIOHI_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VIOHI_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VIOLO:
+		reg_val = BITFVAL(MC13783_REGCTRL_VIOLO_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VIOLO_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VIOLO_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VIOLO_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VDIG:
+		reg_val = BITFVAL(MC13783_REGCTRL_VDIG_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VDIG_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VDIG_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VDIG_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VGEN:
+		reg_val = BITFVAL(MC13783_REGCTRL_VGEN_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VGEN_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VGEN_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VGEN_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFDIG:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFDIG_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VRFDIG_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFDIG_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VRFDIG_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFREF:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFREF_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VRFREF_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFREF_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VRFREF_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFCP:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFCP_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VRFCP_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFCP_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VRFCP_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VSIM:
+		reg_val = BITFVAL(MC13783_REGCTRL_VSIM_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VSIM_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VSIM_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VSIM_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VESIM:
+		reg_val = BITFVAL(MC13783_REGCTRL_VESIM_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VESIM_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VESIM_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VESIM_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VCAM:
+		reg_val = BITFVAL(MC13783_REGCTRL_VCAM_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VCAM_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VCAM_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VCAM_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRFBG:
+		if ((lp_mode == LOW_POWER) ||
+		    (lp_mode == LOW_POWER_AND_LOW_POWER_CTRL_BY_PIN)) {
+			return PMIC_PARAMETER_ERROR;
+		}
+		reg_val = BITFVAL(MC13783_REGCTRL_VRFBG_STBY, l_mode);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFBG_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRF1:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRF1_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VRF1_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRF1_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VRF1_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRF2:
+		reg_val = BITFVAL(MC13783_REGCTRL_VRF2_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VRF2_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRF2_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VRF2_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VMMC1:
+		reg_val = BITFVAL(MC13783_REGCTRL_VMMC1_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VMMC1_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VMMC1_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VMMC1_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VMMC2:
+		reg_val = BITFVAL(MC13783_REGCTRL_VMMC2_MODE, l_mode) |
+		    BITFVAL(MC13783_REGCTRL_VMMC2_STBY, l_stby);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VMMC2_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VMMC2_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(reg, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets low power mode.
+ *
+ * @param        regulator    The regulator to be handled
+ * @param        lp_mode      Select nominal or low power mode.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_regulator_get_lp_mode(t_pmic_regulator regulator,
+					     t_regulator_lp_mode * lp_mode)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int reg;
+	unsigned int l_mode, l_stby;
+
+	switch (regulator) {
+	case SW_SW3:
+		reg_mask = BITFMASK(MC13783_SWCTRL_SW3_MODE) |
+		    BITFMASK(MC13783_SWCTRL_SW3_STBY);
+		reg = REG_SWITCHERS_5;
+		break;
+	case REGU_VAUDIO:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VAUDIO_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VAUDIO_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VIOHI:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VIOHI_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VIOHI_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VIOLO:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VIOLO_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VIOLO_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VDIG:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VDIG_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VDIG_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VGEN:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VGEN_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VGEN_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFDIG:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFDIG_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VRFDIG_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFREF:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFREF_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VRFREF_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VRFCP:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFCP_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VRFCP_STBY);
+		reg = REG_REGULATOR_MODE_0;
+		break;
+	case REGU_VSIM:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VSIM_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VSIM_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VESIM:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VESIM_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VESIM_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VCAM:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VCAM_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VCAM_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRFBG:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRFBG_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRF1:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRF1_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VRF1_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VRF2:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VRF2_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VRF2_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VMMC1:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VMMC1_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VMMC1_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	case REGU_VMMC2:
+		reg_mask = BITFMASK(MC13783_REGCTRL_VMMC2_MODE) |
+		    BITFMASK(MC13783_REGCTRL_VMMC2_STBY);
+		reg = REG_REGULATOR_MODE_1;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &reg_val, reg_mask));
+
+	switch (regulator) {
+	case SW_SW3:
+		l_mode = BITFEXT(reg_val, MC13783_SWCTRL_SW3_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_SWCTRL_SW3_STBY);
+		break;
+	case REGU_VAUDIO:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VAUDIO_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VAUDIO_STBY);
+		break;
+	case REGU_VIOHI:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VIOHI_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VIOHI_STBY);
+		break;
+	case REGU_VIOLO:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VIOLO_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VIOLO_STBY);
+		break;
+	case REGU_VDIG:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VDIG_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VDIG_STBY);
+		break;
+	case REGU_VGEN:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VGEN_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VGEN_STBY);
+		break;
+	case REGU_VRFDIG:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VRFDIG_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VRFDIG_STBY);
+		break;
+	case REGU_VRFREF:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VRFREF_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VRFREF_STBY);
+		break;
+	case REGU_VRFCP:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VRFCP_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VRFCP_STBY);
+		break;
+	case REGU_VSIM:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VSIM_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VSIM_STBY);
+		break;
+	case REGU_VESIM:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VESIM_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VESIM_STBY);
+		break;
+	case REGU_VCAM:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VCAM_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VCAM_STBY);
+		break;
+	case REGU_VRFBG:
+		l_mode = MC13783_REGTRL_LP_MODE_DISABLE;
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VRFBG_STBY);
+		break;
+	case REGU_VRF1:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VRF1_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VRF1_STBY);
+		break;
+	case REGU_VRF2:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VRF2_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VRF2_STBY);
+		break;
+	case REGU_VMMC1:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VMMC1_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VMMC1_STBY);
+		break;
+	case REGU_VMMC2:
+		l_mode = BITFEXT(reg_val, MC13783_REGCTRL_VMMC2_MODE);
+		l_stby = BITFEXT(reg_val, MC13783_REGCTRL_VMMC2_STBY);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	if ((l_mode == MC13783_REGTRL_LP_MODE_DISABLE) &&
+	    (l_stby == MC13783_REGTRL_STBY_MODE_DISABLE)) {
+		*lp_mode = LOW_POWER_DISABLED;
+	} else if ((l_mode == MC13783_REGTRL_LP_MODE_DISABLE) &&
+		   (l_stby == MC13783_REGTRL_STBY_MODE_ENABLE)) {
+		*lp_mode = LOW_POWER_CTRL_BY_PIN;
+	} else if ((l_mode == MC13783_REGTRL_LP_MODE_ENABLE) &&
+		   (l_stby == MC13783_REGTRL_STBY_MODE_DISABLE)) {
+		*lp_mode = LOW_POWER_EN;
+	} else {
+		*lp_mode = LOW_POWER_AND_LOW_POWER_CTRL_BY_PIN;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the regulator configuration.
+ *
+ * @param        regulator    The regulator to be configured.
+ * @param        config       The regulator output configuration.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_regulator_set_config(t_pmic_regulator regulator,
+					    t_regulator_config * config)
+{
+	if (config == NULL) {
+		return PMIC_ERROR;
+	}
+
+	switch (regulator) {
+	case SW_SW1A:
+	case SW_SW1B:
+	case SW_SW2A:
+	case SW_SW2B:
+		CHECK_ERROR(pmic_power_regulator_set_voltage
+			    (regulator, config->voltage));
+		CHECK_ERROR(pmic_power_switcher_set_dvs
+			    (regulator, config->voltage_lvs));
+		CHECK_ERROR(pmic_power_switcher_set_stby
+			    (regulator, config->voltage_stby));
+		CHECK_ERROR(pmic_power_switcher_set_mode
+			    (regulator, config->mode, false));
+		CHECK_ERROR(pmic_power_switcher_set_mode
+			    (regulator, config->stby_mode, true));
+		CHECK_ERROR(pmic_power_switcher_set_dvs_speed
+			    (regulator, config->dvs_speed));
+		CHECK_ERROR(pmic_power_switcher_set_panic_mode
+			    (regulator, config->panic_mode));
+		CHECK_ERROR(pmic_power_switcher_set_softstart
+			    (regulator, config->softstart));
+		break;
+	case SW_PLL:
+		CHECK_ERROR(pmic_power_switcher_set_factor
+			    (regulator, config->factor));
+		break;
+	case SW_SW3:
+	case REGU_VIOLO:
+	case REGU_VDIG:
+	case REGU_VGEN:
+	case REGU_VRFDIG:
+	case REGU_VRFREF:
+	case REGU_VRFCP:
+	case REGU_VSIM:
+	case REGU_VESIM:
+	case REGU_VCAM:
+	case REGU_VRF1:
+	case REGU_VRF2:
+	case REGU_VMMC1:
+	case REGU_VMMC2:
+		CHECK_ERROR(pmic_power_regulator_set_voltage
+			    (regulator, config->voltage));
+		CHECK_ERROR(pmic_power_regulator_set_lp_mode
+			    (regulator, config->lp_mode));
+		break;
+	case REGU_VVIB:
+		CHECK_ERROR(pmic_power_regulator_set_voltage
+			    (regulator, config->voltage));
+		break;
+	case REGU_VAUDIO:
+	case REGU_VIOHI:
+	case REGU_VRFBG:
+		CHECK_ERROR(pmic_power_regulator_set_lp_mode
+			    (regulator, config->lp_mode));
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function retrives the regulator output configuration.
+ *
+ * @param        regulator    The regulator to be truned off.
+ * @param        config       Pointer to regulator configuration.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_regulator_get_config(t_pmic_regulator regulator,
+					    t_regulator_config * config)
+{
+	if (config == NULL) {
+		return PMIC_ERROR;
+	}
+
+	switch (regulator) {
+	case SW_SW1A:
+	case SW_SW1B:
+	case SW_SW2A:
+	case SW_SW2B:
+		CHECK_ERROR(pmic_power_regulator_get_voltage
+			    (regulator, &config->voltage));
+		CHECK_ERROR(pmic_power_switcher_get_dvs
+			    (regulator, &config->voltage_lvs));
+		CHECK_ERROR(pmic_power_switcher_get_stby
+			    (regulator, &config->voltage_stby));
+		CHECK_ERROR(pmic_power_switcher_get_mode
+			    (regulator, &config->mode, false));
+		CHECK_ERROR(pmic_power_switcher_get_mode
+			    (regulator, &config->stby_mode, true));
+		CHECK_ERROR(pmic_power_switcher_get_dvs_speed
+			    (regulator, &config->dvs_speed));
+		CHECK_ERROR(pmic_power_switcher_get_panic_mode
+			    (regulator, &config->panic_mode));
+		CHECK_ERROR(pmic_power_switcher_get_softstart
+			    (regulator, &config->softstart));
+		break;
+	case SW_PLL:
+		CHECK_ERROR(pmic_power_switcher_get_factor
+			    (regulator, &config->factor));
+		break;
+	case SW_SW3:
+	case REGU_VIOLO:
+	case REGU_VDIG:
+	case REGU_VGEN:
+	case REGU_VRFDIG:
+	case REGU_VRFREF:
+	case REGU_VRFCP:
+	case REGU_VSIM:
+	case REGU_VESIM:
+	case REGU_VCAM:
+	case REGU_VRF1:
+	case REGU_VRF2:
+	case REGU_VMMC1:
+	case REGU_VMMC2:
+		CHECK_ERROR(pmic_power_regulator_get_voltage
+			    (regulator, &config->voltage));
+		CHECK_ERROR(pmic_power_regulator_get_lp_mode
+			    (regulator, &config->lp_mode));
+		break;
+	case REGU_VVIB:
+		CHECK_ERROR(pmic_power_regulator_get_voltage
+			    (regulator, &config->voltage));
+		break;
+	case REGU_VAUDIO:
+	case REGU_VIOHI:
+	case REGU_VRFBG:
+		CHECK_ERROR(pmic_power_regulator_get_lp_mode
+			    (regulator, &config->lp_mode));
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function enables automatically VBKUP2 in the memory hold modes.
+ * Only on mc13783 2.0 or higher
+ *
+ * @param        en           if true, enable VBKUP2AUTOMH
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_vbkup2_auto_en(bool en)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	pmic_version_t mc13783_ver;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+		reg_val = BITFVAL(MC13783_REGCTRL_VBKUP2AUTOMH, en);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VBKUP2AUTOMH);
+
+		CHECK_ERROR(pmic_write_reg(REG_POWER_CONTROL_0,
+					   reg_val, reg_mask));
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function gets state of automatically VBKUP2.
+ * Only on mc13783 2.0 or higher
+ *
+ * @param        en           if true, VBKUP2AUTOMH is enabled
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_get_vbkup2_auto_state(bool * en)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	pmic_version_t mc13783_ver;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+		reg_mask = BITFMASK(MC13783_REGCTRL_VBKUP2AUTOMH);
+		CHECK_ERROR(pmic_read_reg(REG_POWER_CONTROL_0,
+					  &reg_val, reg_mask));
+		*en = BITFEXT(reg_val, MC13783_REGCTRL_VBKUP2AUTOMH);
+
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function enables battery detect function.
+ * Only on mc13783 2.0 or higher
+ *
+ * @param        en           if true, enable BATTDETEN
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_bat_det_en(bool en)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	pmic_version_t mc13783_ver;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+		reg_val = BITFVAL(MC13783_REGCTRL_BATTDETEN, en);
+		reg_mask = BITFMASK(MC13783_REGCTRL_BATTDETEN);
+
+		CHECK_ERROR(pmic_write_reg(REG_POWER_CONTROL_0,
+					   reg_val, reg_mask));
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function gets state of battery detect function.
+ * Only on mc13783 2.0 or higher
+ *
+ * @param        en           if true, BATTDETEN is enabled
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_get_bat_det_state(bool * en)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	pmic_version_t mc13783_ver;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+		reg_mask = BITFMASK(MC13783_REGCTRL_BATTDETEN);
+
+		CHECK_ERROR(pmic_read_reg(REG_POWER_CONTROL_0,
+					  &reg_val, reg_mask));
+		*en = BITFEXT(reg_val, MC13783_REGCTRL_BATTDETEN);
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function enables control of VVIB by VIBEN pin.
+ * Only on mc13783 2.0 or higher
+ *
+ * @param        en           if true, enable VIBPINCTRL
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_vib_pin_en(bool en)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	pmic_version_t mc13783_ver;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+		reg_val = BITFVAL(MC13783_REGCTRL_VIBPINCTRL, en);
+		reg_mask = BITFMASK(MC13783_REGCTRL_VIBPINCTRL);
+
+		CHECK_ERROR(pmic_write_reg(REG_POWER_MISCELLANEOUS,
+					   reg_val, reg_mask));
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function gets state of control of VVIB by VIBEN pin.
+ * Only on mc13783 2.0 or higher
+ * @param        en           if true, VIBPINCTRL is enabled
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_gets_vib_pin_state(bool * en)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	pmic_version_t mc13783_ver;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+		reg_mask = BITFMASK(MC13783_REGCTRL_VIBPINCTRL);
+		CHECK_ERROR(pmic_read_reg(REG_POWER_MISCELLANEOUS,
+					  &reg_val, reg_mask));
+		*en = BITFEXT(reg_val, MC13783_REGCTRL_VIBPINCTRL);
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function returns power up sense value
+ *
+ * @param        p_up_sense     value of power up sense
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_power_get_power_mode_sense(struct t_p_up_sense * p_up_sense)
+{
+	unsigned int reg_value = 0;
+	CHECK_ERROR(pmic_read_reg(REG_POWER_UP_MODE_SENSE,
+				  &reg_value, PMIC_ALL_BITS));
+	p_up_sense->state_ictest = (STATE_ICTEST_MASK & reg_value);
+	p_up_sense->state_clksel = ((STATE_CLKSEL_MASK & reg_value)
+				    >> STATE_CLKSEL_BIT);
+	p_up_sense->state_pums1 = ((STATE_PUMS1_MASK & reg_value)
+				   >> STATE_PUMS1_BITS);
+	p_up_sense->state_pums2 = ((STATE_PUMS2_MASK & reg_value)
+				   >> STATE_PUMS2_BITS);
+	p_up_sense->state_pums3 = ((STATE_PUMS3_MASK & reg_value)
+				   >> STATE_PUMS3_BITS);
+	p_up_sense->state_chrgmode0 = ((STATE_CHRGM1_MASK & reg_value)
+				       >> STATE_CHRGM1_BITS);
+	p_up_sense->state_chrgmode1 = ((STATE_CHRGM2_MASK & reg_value)
+				       >> STATE_CHRGM2_BITS);
+	p_up_sense->state_umod = ((STATE_UMOD_MASK & reg_value)
+				  >> STATE_UMOD_BITS);
+	p_up_sense->state_usben = ((STATE_USBEN_MASK & reg_value)
+				   >> STATE_USBEN_BIT);
+	p_up_sense->state_sw_1a1b_joined = ((STATE_SW1A_J_B_MASK & reg_value)
+					    >> STATE_SW1A_J_B_BIT);
+	p_up_sense->state_sw_2a2b_joined = ((STATE_SW2A_J_B_MASK & reg_value)
+					    >> STATE_SW2A_J_B_BIT);
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function configures the Regen assignment for all regulator
+ *
+ * @param        regulator      type of regulator
+ * @param        en_dis         if true, the regulator is enabled by regen.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_set_regen_assig(t_pmic_regulator regulator, bool en_dis)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+
+	switch (regulator) {
+	case REGU_VAUDIO:
+		reg_val = BITFVAL(MC13783_REGGEN_VAUDIO, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VAUDIO);
+		break;
+	case REGU_VIOHI:
+		reg_val = BITFVAL(MC13783_REGGEN_VIOHI, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VIOHI);
+		break;
+	case REGU_VIOLO:
+		reg_val = BITFVAL(MC13783_REGGEN_VIOLO, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VIOLO);
+		break;
+	case REGU_VDIG:
+		reg_val = BITFVAL(MC13783_REGGEN_VDIG, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VDIG);
+		break;
+	case REGU_VGEN:
+		reg_val = BITFVAL(MC13783_REGGEN_VGEN, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VGEN);
+		break;
+	case REGU_VRFDIG:
+		reg_val = BITFVAL(MC13783_REGGEN_VRFDIG, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VRFDIG);
+		break;
+	case REGU_VRFREF:
+		reg_val = BITFVAL(MC13783_REGGEN_VRFREF, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VRFREF);
+		break;
+	case REGU_VRFCP:
+		reg_val = BITFVAL(MC13783_REGGEN_VRFCP, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VRFCP);
+		break;
+	case REGU_VCAM:
+		reg_val = BITFVAL(MC13783_REGGEN_VCAM, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VCAM);
+		break;
+	case REGU_VRFBG:
+		reg_val = BITFVAL(MC13783_REGGEN_VRFBG, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VRFBG);
+		break;
+	case REGU_VRF1:
+		reg_val = BITFVAL(MC13783_REGGEN_VRF1, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VRF1);
+		break;
+	case REGU_VRF2:
+		reg_val = BITFVAL(MC13783_REGGEN_VRF2, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VRF2);
+		break;
+	case REGU_VMMC1:
+		reg_val = BITFVAL(MC13783_REGGEN_VMMC1, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VMMC1);
+		break;
+	case REGU_VMMC2:
+		reg_val = BITFVAL(MC13783_REGGEN_VMMC2, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_VMMC2);
+		break;
+	case REGU_GPO1:
+		reg_val = BITFVAL(MC13783_REGGEN_GPO1, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_GPO1);
+		break;
+	case REGU_GPO2:
+		reg_val = BITFVAL(MC13783_REGGEN_GPO2, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_GPO2);
+		break;
+	case REGU_GPO3:
+		reg_val = BITFVAL(MC13783_REGGEN_GPO3, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_GPO3);
+		break;
+	case REGU_GPO4:
+		reg_val = BITFVAL(MC13783_REGGEN_GPO4, en_dis);
+		reg_mask = BITFMASK(MC13783_REGGEN_GPO4);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(REG_REGEN_ASSIGNMENT, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets the Regen assignment for all regulator
+ *
+ * @param        regulator      type of regulator
+ * @param        en_dis         return value, if true :
+ *                              the regulator is enabled by regen.
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_get_regen_assig(t_pmic_regulator regulator,
+				       bool * en_dis)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+
+	switch (regulator) {
+	case REGU_VAUDIO:
+		reg_mask = BITFMASK(MC13783_REGGEN_VAUDIO);
+		break;
+	case REGU_VIOHI:
+		reg_mask = BITFMASK(MC13783_REGGEN_VIOHI);
+		break;
+	case REGU_VIOLO:
+		reg_mask = BITFMASK(MC13783_REGGEN_VIOLO);
+		break;
+	case REGU_VDIG:
+		reg_mask = BITFMASK(MC13783_REGGEN_VDIG);
+		break;
+	case REGU_VGEN:
+		reg_mask = BITFMASK(MC13783_REGGEN_VGEN);
+		break;
+	case REGU_VRFDIG:
+		reg_mask = BITFMASK(MC13783_REGGEN_VRFDIG);
+		break;
+	case REGU_VRFREF:
+		reg_mask = BITFMASK(MC13783_REGGEN_VRFREF);
+		break;
+	case REGU_VRFCP:
+		reg_mask = BITFMASK(MC13783_REGGEN_VRFCP);
+		break;
+	case REGU_VCAM:
+		reg_mask = BITFMASK(MC13783_REGGEN_VCAM);
+		break;
+	case REGU_VRFBG:
+		reg_mask = BITFMASK(MC13783_REGGEN_VRFBG);
+		break;
+	case REGU_VRF1:
+		reg_mask = BITFMASK(MC13783_REGGEN_VRF1);
+		break;
+	case REGU_VRF2:
+		reg_mask = BITFMASK(MC13783_REGGEN_VRF2);
+		break;
+	case REGU_VMMC1:
+		reg_mask = BITFMASK(MC13783_REGGEN_VMMC1);
+		break;
+	case REGU_VMMC2:
+		reg_mask = BITFMASK(MC13783_REGGEN_VMMC2);
+		break;
+	case REGU_GPO1:
+		reg_mask = BITFMASK(MC13783_REGGEN_GPO1);
+		break;
+	case REGU_GPO2:
+		reg_mask = BITFMASK(MC13783_REGGEN_GPO2);
+		break;
+	case REGU_GPO3:
+		reg_mask = BITFMASK(MC13783_REGGEN_GPO3);
+		break;
+	case REGU_GPO4:
+		reg_mask = BITFMASK(MC13783_REGGEN_GPO4);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(REG_REGEN_ASSIGNMENT, &reg_val, reg_mask));
+
+	switch (regulator) {
+	case REGU_VAUDIO:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VAUDIO);
+		break;
+	case REGU_VIOHI:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VIOHI);
+		break;
+	case REGU_VIOLO:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VIOLO);
+		break;
+	case REGU_VDIG:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VDIG);
+		break;
+	case REGU_VGEN:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VGEN);
+		break;
+	case REGU_VRFDIG:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VRFDIG);
+		break;
+	case REGU_VRFREF:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VRFREF);
+		break;
+	case REGU_VRFCP:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VRFCP);
+		break;
+	case REGU_VCAM:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VCAM);
+		break;
+	case REGU_VRFBG:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VRFBG);
+		break;
+	case REGU_VRF1:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VRF1);
+		break;
+	case REGU_VRF2:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VRF2);
+		break;
+	case REGU_VMMC1:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VMMC1);
+		break;
+	case REGU_VMMC2:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_VMMC2);
+		break;
+	case REGU_GPO1:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_GPO1);
+		break;
+	case REGU_GPO2:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_GPO2);
+		break;
+	case REGU_GPO3:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_GPO3);
+		break;
+	case REGU_GPO4:
+		*en_dis = BITFEXT(reg_val, MC13783_REGGEN_GPO4);
+		break;
+	default:
+		break;
+	}
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function sets the Regen polarity.
+ *
+ * @param        en_dis         If true regen is inverted.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_set_regen_inv(bool en_dis)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+
+	reg_val = BITFVAL(MC13783_REGGEN_INV, en_dis);
+	reg_mask = BITFMASK(MC13783_REGGEN_INV);
+
+	CHECK_ERROR(pmic_write_reg(REG_REGEN_ASSIGNMENT, reg_val, reg_mask));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets the Regen polarity.
+ *
+ * @param        en_dis         If true regen is inverted.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_get_regen_inv(bool * en_dis)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+
+	reg_mask = BITFMASK(MC13783_REGGEN_INV);
+	CHECK_ERROR(pmic_read_reg(REG_REGEN_ASSIGNMENT, &reg_val, reg_mask));
+	*en_dis = BITFEXT(reg_val, MC13783_REGGEN_INV);
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function enables esim control voltage.
+ * Only on mc13783 2.0 or higher
+ *
+ * @param        vesim          if true, enable VESIMESIMEN
+ * @param        vmmc1          if true, enable VMMC1ESIMEN
+ * @param        vmmc2          if true, enable VMMC2ESIMEN
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_esim_v_en(bool vesim, bool vmmc1, bool vmmc2)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	pmic_version_t mc13783_ver;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+		reg_val = BITFVAL(MC13783_REGGEN_VESIMESIM, vesim) |
+		    BITFVAL(MC13783_REGGEN_VMMC1ESIM, vesim) |
+		    BITFVAL(MC13783_REGGEN_VMMC2ESIM, vesim);
+		reg_mask = BITFMASK(MC13783_REGGEN_VESIMESIM) |
+		    BITFMASK(MC13783_REGGEN_VMMC1ESIM) |
+		    BITFMASK(MC13783_REGGEN_VMMC2ESIM);
+		CHECK_ERROR(pmic_write_reg(REG_REGEN_ASSIGNMENT,
+					   reg_val, reg_mask));
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function gets esim control voltage values.
+ * Only on mc13783 2.0 or higher
+ *
+ * @param        vesim          if true, enable VESIMESIMEN
+ * @param        vmmc1          if true, enable VMMC1ESIMEN
+ * @param        vmmc2          if true, enable VMMC2ESIMEN
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_gets_esim_v_state(bool * vesim, bool * vmmc1,
+					 bool * vmmc2)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	pmic_version_t mc13783_ver;
+	mc13783_ver = pmic_get_version();
+	if (mc13783_ver.revision >= 20) {
+		reg_mask = BITFMASK(MC13783_REGGEN_VESIMESIM) |
+		    BITFMASK(MC13783_REGGEN_VMMC1ESIM) |
+		    BITFMASK(MC13783_REGGEN_VMMC2ESIM);
+		CHECK_ERROR(pmic_read_reg(REG_REGEN_ASSIGNMENT,
+					  &reg_val, reg_mask));
+		*vesim = BITFEXT(reg_val, MC13783_REGGEN_VESIMESIM);
+		*vmmc1 = BITFEXT(reg_val, MC13783_REGGEN_VMMC1ESIM);
+		*vmmc2 = BITFEXT(reg_val, MC13783_REGGEN_VMMC2ESIM);
+		return PMIC_SUCCESS;
+	} else {
+		return PMIC_NOT_SUPPORTED;
+	}
+}
+
+/*!
+ * This function enables auto reset after a system reset.
+ *
+ * @param        en         if true, the auto reset is enabled
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_set_auto_reset_en(bool en)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+
+	reg_val = BITFVAL(MC13783_AUTO_RESTART, en);
+	reg_mask = BITFMASK(MC13783_AUTO_RESTART);
+
+	CHECK_ERROR(pmic_write_reg(REG_POWER_CONTROL_2, reg_val, reg_mask));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets auto reset configuration.
+ *
+ * @param        en         if true, the auto reset is enabled
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_get_auto_reset_en(bool * en)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+
+	reg_mask = BITFMASK(MC13783_AUTO_RESTART);
+	CHECK_ERROR(pmic_read_reg(REG_POWER_CONTROL_2, &reg_val, reg_mask));
+	*en = BITFEXT(reg_val, MC13783_AUTO_RESTART);
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function configures a system reset on a button.
+ *
+ * @param       bt         type of button.
+ * @param       sys_rst    if true, enable the system reset on this button
+ * @param       deb_time   sets the debounce time on this button pin
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_set_conf_button(t_button bt, bool sys_rst, int deb_time)
+{
+	int max_val = 0;
+	unsigned int reg_val = 0, reg_mask = 0;
+
+	max_val = (1 << MC13783_DEB_BT_ON1B_WID) - 1;
+	if (deb_time > max_val) {
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	switch (bt) {
+	case BT_ON1B:
+		reg_val = BITFVAL(MC13783_EN_BT_ON1B, sys_rst) |
+		    BITFVAL(MC13783_DEB_BT_ON1B, deb_time);
+		reg_mask = BITFMASK(MC13783_EN_BT_ON1B) |
+		    BITFMASK(MC13783_DEB_BT_ON1B);
+		break;
+	case BT_ON2B:
+		reg_val = BITFVAL(MC13783_EN_BT_ON2B, sys_rst) |
+		    BITFVAL(MC13783_DEB_BT_ON2B, deb_time);
+		reg_mask = BITFMASK(MC13783_EN_BT_ON2B) |
+		    BITFMASK(MC13783_DEB_BT_ON2B);
+		break;
+	case BT_ON3B:
+		reg_val = BITFVAL(MC13783_EN_BT_ON3B, sys_rst) |
+		    BITFVAL(MC13783_DEB_BT_ON3B, deb_time);
+		reg_mask = BITFMASK(MC13783_EN_BT_ON3B) |
+		    BITFMASK(MC13783_DEB_BT_ON3B);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(REG_POWER_CONTROL_2, reg_val, reg_mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function gets configuration of a button.
+ *
+ * @param       bt         type of button.
+ * @param       sys_rst    if true, the system reset is enabled on this button
+ * @param       deb_time   gets the debounce time on this button pin
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_get_conf_button(t_button bt,
+				       bool * sys_rst, int *deb_time)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+
+	switch (bt) {
+	case BT_ON1B:
+		reg_mask = BITFMASK(MC13783_EN_BT_ON1B) |
+		    BITFMASK(MC13783_DEB_BT_ON1B);
+		break;
+	case BT_ON2B:
+		reg_mask = BITFMASK(MC13783_EN_BT_ON2B) |
+		    BITFMASK(MC13783_DEB_BT_ON2B);
+		break;
+	case BT_ON3B:
+		reg_mask = BITFMASK(MC13783_EN_BT_ON3B) |
+		    BITFMASK(MC13783_DEB_BT_ON3B);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(REG_POWER_CONTROL_2, &reg_val, reg_mask));
+
+	switch (bt) {
+	case BT_ON1B:
+		*sys_rst = BITFEXT(reg_val, MC13783_EN_BT_ON1B);
+		*deb_time = BITFEXT(reg_val, MC13783_DEB_BT_ON1B);
+		break;
+	case BT_ON2B:
+		*sys_rst = BITFEXT(reg_val, MC13783_EN_BT_ON2B);
+		*deb_time = BITFEXT(reg_val, MC13783_DEB_BT_ON2B);
+		break;
+	case BT_ON3B:
+		*sys_rst = BITFEXT(reg_val, MC13783_EN_BT_ON3B);
+		*deb_time = BITFEXT(reg_val, MC13783_DEB_BT_ON3B);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is used to un/subscribe on power event IT.
+ *
+ * @param        event          type of event.
+ * @param        callback       event callback function.
+ * @param        sub            define if Un/subscribe event.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_event(t_pwr_int event, void *callback, bool sub)
+{
+	pmic_event_callback_t power_callback;
+	type_event power_event;
+
+	power_callback.func = callback;
+	power_callback.param = NULL;
+	switch (event) {
+	case PWR_IT_BPONI:
+		power_event = EVENT_BPONI;
+		break;
+	case PWR_IT_LOBATLI:
+		power_event = EVENT_LOBATLI;
+		break;
+	case PWR_IT_LOBATHI:
+		power_event = EVENT_LOBATHI;
+		break;
+	case PWR_IT_ONOFD1I:
+		power_event = EVENT_ONOFD1I;
+		break;
+	case PWR_IT_ONOFD2I:
+		power_event = EVENT_ONOFD2I;
+		break;
+	case PWR_IT_ONOFD3I:
+		power_event = EVENT_ONOFD3I;
+		break;
+	case PWR_IT_SYSRSTI:
+		power_event = EVENT_SYSRSTI;
+		break;
+	case PWR_IT_PWRRDYI:
+		power_event = EVENT_PWRRDYI;
+		break;
+	case PWR_IT_PCI:
+		power_event = EVENT_PCI;
+		break;
+	case PWR_IT_WARMI:
+		power_event = EVENT_WARMI;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	if (sub == true) {
+		CHECK_ERROR(pmic_event_subscribe(power_event, power_callback));
+	} else {
+		CHECK_ERROR(pmic_event_unsubscribe
+			    (power_event, power_callback));
+	}
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is used to subscribe on power event IT.
+ *
+ * @param        event          type of event.
+ * @param        callback       event callback function.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_event_sub(t_pwr_int event, void *callback)
+{
+	return pmic_power_event(event, callback, true);
+}
+
+/*!
+ * This function is used to un subscribe on power event IT.
+ *
+ * @param        event          type of event.
+ * @param        callback       event callback function.
+ *
+ * @return       This function returns 0 if successful.
+ */
+PMIC_STATUS pmic_power_event_unsub(t_pwr_int event, void *callback)
+{
+	return pmic_power_event(event, callback, false);
+}
+
+void pmic_power_key_callback(void)
+{
+#ifdef CONFIG_MXC_HWEVENT
+	/*read the power key is pressed or up */
+	t_sensor_bits sense;
+	struct mxc_hw_event event = { HWE_POWER_KEY, 0 };
+
+	pmic_get_sensors(&sense);
+	if (sense.sense_onofd1s) {
+		pr_debug("PMIC Power key up\n");
+		event.args = PWRK_UNPRESS;
+	} else {
+		pr_debug("PMIC Power key pressed\n");
+		event.args = PWRK_PRESS;
+	}
+	/* send hw event */
+	hw_event_send(HWE_DEF_PRIORITY, &event);
+#endif
+}
+
+static irqreturn_t power_key_int(int irq, void *dev_id)
+{
+	pr_info(KERN_INFO "on-off key pressed\n");
+
+	return 0;
+}
+
+extern void gpio_power_key_active(void);
+
+/*
+ * Init and Exit
+ */
+
+static int pmic_power_probe(struct platform_device *pdev)
+{
+	int irq, ret;
+	struct pmic_platform_data *ppd;
+
+	/* configure on/off button */
+	gpio_power_key_active();
+
+	ppd = pdev->dev.platform_data;
+	if (ppd)
+		irq = ppd->power_key_irq;
+	else
+		goto done;
+
+	if (irq == 0) {
+		pr_info(KERN_INFO "PMIC Power has no platform data\n");
+		goto done;
+	}
+	set_irq_type(irq, IRQF_TRIGGER_RISING);
+
+	ret = request_irq(irq, power_key_int, 0, "power_key", 0);
+	if (ret)
+		pr_info(KERN_ERR "register on-off key interrupt failed\n");
+
+	set_irq_wake(irq, 1);
+
+      done:
+	pr_info(KERN_INFO "PMIC Power successfully probed\n");
+	return 0;
+}
+
+static struct platform_driver pmic_power_driver_ldm = {
+	.driver = {
+		   .name = "pmic_power",
+		   },
+	.suspend = pmic_power_suspend,
+	.resume = pmic_power_resume,
+	.probe = pmic_power_probe,
+	.remove = NULL,
+};
+
+static int __init pmic_power_init(void)
+{
+	pr_debug("PMIC Power driver loading..\n");
+	pmic_power_event_sub(PWR_IT_ONOFD1I, pmic_power_key_callback);
+	/* set power off hook to mc13783 power off */
+	pm_power_off = pmic_power_off;
+	return platform_driver_register(&pmic_power_driver_ldm);
+}
+static void __exit pmic_power_exit(void)
+{
+	pmic_power_event_unsub(PWR_IT_ONOFD1I, pmic_power_key_callback);
+	platform_driver_unregister(&pmic_power_driver_ldm);
+	pr_debug("PMIC Power driver successfully unloaded\n");
+}
+
+/*
+ * Module entry points
+ */
+
+subsys_initcall_sync(pmic_power_init);
+module_exit(pmic_power_exit);
+
+MODULE_DESCRIPTION("pmic_power driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/mc13783/pmic_power_defs.h b/drivers/mxc/pmic/mc13783/pmic_power_defs.h
new file mode 100644
index 000000000000..38e554146a70
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_power_defs.h
@@ -0,0 +1,509 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_power_defs.h
+ * @brief This is the internal header define of PMIC(mc13783) Power driver.
+ *
+ * @ingroup PMIC_POWER
+ */
+
+/*
+ * Includes
+ */
+
+#ifndef __MC13783_POWER_DEFS_H__
+#define __MC13783_POWER_DEFS_H__
+
+/*
+ * Power Up Mode Sense bits
+ */
+
+#define         STATE_ICTEST_MASK       0x000001
+
+#define         STATE_CLKSEL_BIT        1
+#define         STATE_CLKSEL_MASK       0x000002
+
+#define         STATE_PUMS1_BITS        2
+#define         STATE_PUMS1_MASK        0x00000C
+
+#define         STATE_PUMS2_BITS        4
+#define         STATE_PUMS2_MASK        0x000030
+
+#define         STATE_PUMS3_BITS        6
+#define         STATE_PUMS3_MASK        0x0000C0
+
+#define         STATE_CHRGM1_BITS       8
+#define         STATE_CHRGM1_MASK       0x000300
+
+#define         STATE_CHRGM2_BITS       10
+#define         STATE_CHRGM2_MASK       0x000C00
+
+#define         STATE_UMOD_BITS         12
+#define         STATE_UMOD_MASK         0x003000
+
+#define         STATE_USBEN_BIT         14
+#define         STATE_USBEN_MASK        0x004000
+
+#define         STATE_SW1A_J_B_BIT      15
+#define         STATE_SW1A_J_B_MASK     0x008000
+
+#define         STATE_SW2A_J_B_BIT      16
+#define         STATE_SW2A_J_B_MASK     0x010000
+
+#define         PC_COUNT_MAX            3
+#define         PC_COUNT_MIN            0
+/*
+ * Reg Regen
+ */
+#define MC13783_REGGEN_VAUDIO_LSH			0
+#define MC13783_REGGEN_VAUDIO_WID			1
+#define MC13783_REGGEN_VIOHI_LSH			1
+#define MC13783_REGGEN_VIOHI_WID			1
+#define MC13783_REGGEN_VIOLO_LSH			2
+#define MC13783_REGGEN_VIOLO_WID			1
+#define MC13783_REGGEN_VDIG_LSH			3
+#define MC13783_REGGEN_VDIG_WID			1
+#define MC13783_REGGEN_VGEN_LSH			4
+#define MC13783_REGGEN_VGEN_WID			1
+#define MC13783_REGGEN_VRFDIG_LSH			5
+#define MC13783_REGGEN_VRFDIG_WID			1
+#define MC13783_REGGEN_VRFREF_LSH			6
+#define MC13783_REGGEN_VRFREF_WID			1
+#define MC13783_REGGEN_VRFCP_LSH			7
+#define MC13783_REGGEN_VRFCP_WID			1
+#define MC13783_REGGEN_VCAM_LSH			8
+#define MC13783_REGGEN_VCAM_WID			1
+#define MC13783_REGGEN_VRFBG_LSH			9
+#define MC13783_REGGEN_VRFBG_WID			1
+#define MC13783_REGGEN_VRF1_LSH			10
+#define MC13783_REGGEN_VRF1_WID			1
+#define MC13783_REGGEN_VRF2_LSH			11
+#define MC13783_REGGEN_VRF2_WID			1
+#define MC13783_REGGEN_VMMC1_LSH			12
+#define MC13783_REGGEN_VMMC1_WID			1
+#define MC13783_REGGEN_VMMC2_LSH			13
+#define MC13783_REGGEN_VMMC2_WID			1
+#define MC13783_REGGEN_GPO1_LSH			16
+#define MC13783_REGGEN_GPO1_WID			1
+#define MC13783_REGGEN_GPO2_LSH			17
+#define MC13783_REGGEN_GPO2_WID			1
+#define MC13783_REGGEN_GPO3_LSH			18
+#define MC13783_REGGEN_GPO3_WID			1
+#define MC13783_REGGEN_GPO4_LSH			19
+#define MC13783_REGGEN_GPO4_WID			1
+#define MC13783_REGGEN_INV_LSH			20
+#define MC13783_REGGEN_INV_WID			1
+#define MC13783_REGGEN_VESIMESIM_LSH		21
+#define MC13783_REGGEN_VESIMESIM_WID		1
+#define MC13783_REGGEN_VMMC1ESIM_LSH		22
+#define MC13783_REGGEN_VMMC1ESIM_WID		1
+#define MC13783_REGGEN_VMMC2ESIM_LSH		23
+#define MC13783_REGGEN_VMMC2ESIM_WID		1
+
+/*
+ * Reg Power Control 0
+ */
+#define MC13783_PWRCTRL_PCEN_LSH			0
+#define MC13783_PWRCTRL_PCEN_WID			1
+#define MC13783_PWRCTRL_PCEN_ENABLE		1
+#define MC13783_PWRCTRL_PCEN_DISABLE		0
+#define MC13783_PWRCTRL_PC_COUNT_EN_LSH		1
+#define MC13783_PWRCTRL_PC_COUNT_EN_WID		1
+#define MC13783_PWRCTRL_PC_COUNT_EN_ENABLE	1
+#define MC13783_PWRCTRL_PC_COUNT_EN_DISABLE	0
+#define MC13783_PWRCTRL_WARM_EN_LSH		2
+#define MC13783_PWRCTRL_WARM_EN_WID		1
+#define MC13783_PWRCTRL_WARM_EN_ENABLE		1
+#define MC13783_PWRCTRL_WARM_EN_DISABLE		0
+#define MC13783_PWRCTRL_USER_OFF_SPI_LSH		3
+#define MC13783_PWRCTRL_USER_OFF_SPI_WID		1
+#define MC13783_PWRCTRL_USER_OFF_SPI_ENABLE	1
+#define MC13783_PWRCTRL_USER_OFF_PC_LSH		4
+#define MC13783_PWRCTRL_USER_OFF_PC_WID		1
+#define MC13783_PWRCTRL_USER_OFF_PC_ENABLE	1
+#define MC13783_PWRCTRL_USER_OFF_PC_DISABLE	0
+#define MC13783_PWRCTRL_32OUT_USER_OFF_LSH	5
+#define MC13783_PWRCTRL_32OUT_USER_OFF_WID	1
+#define MC13783_PWRCTRL_32OUT_USER_OFF_ENABLE	1
+#define MC13783_PWRCTRL_32OUT_USER_OFF_DISABLE	0
+#define MC13783_PWRCTRL_32OUT_EN_LSH		6
+#define MC13783_PWRCTRL_32OUT_EN_WID		1
+#define MC13783_PWRCTRL_32OUT_EN_ENABLE		1
+#define MC13783_PWRCTRL_32OUT_EN_DISABLE		0
+#define MC13783_REGCTRL_VBKUP2AUTOMH_LSH		7
+#define MC13783_REGCTRL_VBKUP2AUTOMH_WID		1
+#define MC13783_PWRCTRL_VBKUP1_EN_LSH		8
+#define MC13783_PWRCTRL_VBKUP1_EN_WID		1
+#define MC13783_PWRCTRL_VBKUP_ENABLE		1
+#define MC13783_PWRCTRL_VBKUP_DISABLE		0
+#define MC13783_PWRCTRL_VBKUP1_AUTO_EN_LSH	9
+#define MC13783_PWRCTRL_VBKUP1_AUTO_EN_WID	1
+#define MC13783_PWRCTRL_VBKUP1_LSH		10
+#define MC13783_PWRCTRL_VBKUP1_WID		2
+#define MC13783_PWRCTRL_VBKUP2_EN_LSH		12
+#define MC13783_PWRCTRL_VBKUP2_EN_WID		1
+#define MC13783_PWRCTRL_VBKUP2_AUTO_EN_LSH	13
+#define MC13783_PWRCTRL_VBKUP2_AUTO_EN_WID	1
+#define MC13783_PWRCTRL_VBKUP2_LSH		14
+#define MC13783_PWRCTRL_VBKUP2_WID		2
+#define MC13783_REGCTRL_BATTDETEN_LSH		19
+#define MC13783_REGCTRL_BATTDETEN_WID		1
+
+/*
+ * Reg Power Control 1
+ */
+#define MC13783_PWRCTRL_PCT_LSH		0
+#define MC13783_PWRCTRL_PCT_WID		8
+#define MC13783_PWRCTRL_PC_COUNT_LSH	8
+#define MC13783_PWRCTRL_PC_COUNT_WID	4
+#define MC13783_PWRCTRL_PC_MAX_CNT_LSH	12
+#define MC13783_PWRCTRL_PC_MAX_CNT_WID	4
+#define MC13783_PWRCTRL_MEM_TMR_LSH	16
+#define MC13783_PWRCTRL_MEM_TMR_WID	4
+#define MC13783_PWRCTRL_MEM_ALLON_LSH	20
+#define MC13783_PWRCTRL_MEM_ALLON_WID	1
+#define MC13783_PWRCTRL_MEM_ALLON_ENABLE	1
+#define MC13783_PWRCTRL_MEM_ALLON_DISABLE	0
+
+/*
+ * Reg Power Control 2
+ */
+#define MC13783_AUTO_RESTART_LSH	0
+#define MC13783_AUTO_RESTART_WID	1
+#define MC13783_EN_BT_ON1B_LSH		1
+#define MC13783_EN_BT_ON1B_WID		1
+#define MC13783_EN_BT_ON2B_LSH		2
+#define MC13783_EN_BT_ON2B_WID		1
+#define MC13783_EN_BT_ON3B_LSH		3
+#define MC13783_EN_BT_ON3B_WID		1
+#define MC13783_DEB_BT_ON1B_LSH		4
+#define MC13783_DEB_BT_ON1B_WID		2
+#define MC13783_DEB_BT_ON2B_LSH		6
+#define MC13783_DEB_BT_ON2B_WID		2
+#define MC13783_DEB_BT_ON3B_LSH		8
+#define MC13783_DEB_BT_ON3B_WID		2
+
+/*
+ * Reg Regulator Mode 0
+ */
+#define MC13783_REGCTRL_VAUDIO_EN_LSH	0
+#define MC13783_REGCTRL_VAUDIO_EN_WID	1
+#define MC13783_REGCTRL_VAUDIO_EN_ENABLE	1
+#define MC13783_REGCTRL_VAUDIO_EN_DISABLE	0
+#define MC13783_REGCTRL_VAUDIO_STBY_LSH	1
+#define MC13783_REGCTRL_VAUDIO_STBY_WID	1
+#define MC13783_REGCTRL_VAUDIO_MODE_LSH	2
+#define MC13783_REGCTRL_VAUDIO_MODE_WID	1
+#define MC13783_REGCTRL_VIOHI_EN_LSH	3
+#define MC13783_REGCTRL_VIOHI_EN_WID	1
+#define MC13783_REGCTRL_VIOHI_EN_ENABLE	1
+#define MC13783_REGCTRL_VIOHI_EN_DISABLE	0
+#define MC13783_REGCTRL_VIOHI_STBY_LSH	4
+#define MC13783_REGCTRL_VIOHI_STBY_WID	1
+#define MC13783_REGCTRL_VIOHI_MODE_LSH	5
+#define MC13783_REGCTRL_VIOHI_MODE_WID	1
+#define MC13783_REGCTRL_VIOLO_EN_LSH	6
+#define MC13783_REGCTRL_VIOLO_EN_WID	1
+#define MC13783_REGCTRL_VIOLO_EN_ENABLE	1
+#define MC13783_REGCTRL_VIOLO_EN_DISABLE	0
+#define MC13783_REGCTRL_VIOLO_STBY_LSH	7
+#define MC13783_REGCTRL_VIOLO_STBY_WID	1
+#define MC13783_REGCTRL_VIOLO_MODE_LSH	8
+#define MC13783_REGCTRL_VIOLO_MODE_WID	1
+#define MC13783_REGCTRL_VDIG_EN_LSH	9
+#define MC13783_REGCTRL_VDIG_EN_WID	1
+#define MC13783_REGCTRL_VDIG_EN_ENABLE	1
+#define MC13783_REGCTRL_VDIG_EN_DISABLE	0
+#define MC13783_REGCTRL_VDIG_STBY_LSH	10
+#define MC13783_REGCTRL_VDIG_STBY_WID	1
+#define MC13783_REGCTRL_VDIG_MODE_LSH	11
+#define MC13783_REGCTRL_VDIG_MODE_WID	1
+#define MC13783_REGCTRL_VGEN_EN_LSH	12
+#define MC13783_REGCTRL_VGEN_EN_WID	1
+#define MC13783_REGCTRL_VGEN_EN_ENABLE	1
+#define MC13783_REGCTRL_VGEN_EN_DISABLE	0
+#define MC13783_REGCTRL_VGEN_STBY_LSH	13
+#define MC13783_REGCTRL_VGEN_STBY_WID	1
+#define MC13783_REGCTRL_VGEN_MODE_LSH	14
+#define MC13783_REGCTRL_VGEN_MODE_WID	1
+#define MC13783_REGCTRL_VRFDIG_EN_LSH	15
+#define MC13783_REGCTRL_VRFDIG_EN_WID	1
+#define MC13783_REGCTRL_VRFDIG_EN_ENABLE	1
+#define MC13783_REGCTRL_VRFDIG_EN_DISABLE	0
+#define MC13783_REGCTRL_VRFDIG_STBY_LSH	16
+#define MC13783_REGCTRL_VRFDIG_STBY_WID	1
+#define MC13783_REGCTRL_VRFDIG_MODE_LSH	17
+#define MC13783_REGCTRL_VRFDIG_MODE_WID	1
+#define MC13783_REGCTRL_VRFREF_EN_LSH	18
+#define MC13783_REGCTRL_VRFREF_EN_WID	1
+#define MC13783_REGCTRL_VRFREF_EN_ENABLE	1
+#define MC13783_REGCTRL_VRFREF_EN_DISABLE	0
+#define MC13783_REGCTRL_VRFREF_STBY_LSH	19
+#define MC13783_REGCTRL_VRFREF_STBY_WID	1
+#define MC13783_REGCTRL_VRFREF_MODE_LSH	20
+#define MC13783_REGCTRL_VRFREF_MODE_WID	1
+#define MC13783_REGCTRL_VRFCP_EN_LSH	21
+#define MC13783_REGCTRL_VRFCP_EN_WID	1
+#define MC13783_REGCTRL_VRFCP_EN_ENABLE	1
+#define MC13783_REGCTRL_VRFCP_EN_DISABLE	0
+#define MC13783_REGCTRL_VRFCP_STBY_LSH	22
+#define MC13783_REGCTRL_VRFCP_STBY_WID	1
+#define MC13783_REGCTRL_VRFCP_MODE_LSH	23
+#define MC13783_REGCTRL_VRFCP_MODE_WID	1
+
+/*
+ * Reg Regulator Mode 1
+ */
+#define MC13783_REGCTRL_VSIM_EN_LSH	0
+#define MC13783_REGCTRL_VSIM_EN_WID	1
+#define MC13783_REGCTRL_VSIM_EN_ENABLE	1
+#define MC13783_REGCTRL_VSIM_EN_DISABLE	0
+#define MC13783_REGCTRL_VSIM_STBY_LSH	1
+#define MC13783_REGCTRL_VSIM_STBY_WID	1
+#define MC13783_REGCTRL_VSIM_MODE_LSH	2
+#define MC13783_REGCTRL_VSIM_MODE_WID	1
+#define MC13783_REGCTRL_VESIM_EN_LSH	3
+#define MC13783_REGCTRL_VESIM_EN_WID	1
+#define MC13783_REGCTRL_VESIM_EN_ENABLE	1
+#define MC13783_REGCTRL_VESIM_EN_DISABLE	0
+#define MC13783_REGCTRL_VESIM_STBY_LSH	4
+#define MC13783_REGCTRL_VESIM_STBY_WID	1
+#define MC13783_REGCTRL_VESIM_MODE_LSH	5
+#define MC13783_REGCTRL_VESIM_MODE_WID	1
+#define MC13783_REGCTRL_VCAM_EN_LSH	6
+#define MC13783_REGCTRL_VCAM_EN_WID	1
+#define MC13783_REGCTRL_VCAM_EN_ENABLE	1
+#define MC13783_REGCTRL_VCAM_EN_DISABLE	0
+#define MC13783_REGCTRL_VCAM_STBY_LSH	7
+#define MC13783_REGCTRL_VCAM_STBY_WID	1
+#define MC13783_REGCTRL_VCAM_MODE_LSH	8
+#define MC13783_REGCTRL_VCAM_MODE_WID	1
+#define	MC13783_REGCTRL_VRFBG_EN_LSH	9
+#define	MC13783_REGCTRL_VRFBG_EN_WID	1
+#define MC13783_REGCTRL_VRFBG_EN_ENABLE	1
+#define MC13783_REGCTRL_VRFBG_EN_DISABLE	0
+#define MC13783_REGCTRL_VRFBG_STBY_LSH	10
+#define MC13783_REGCTRL_VRFBG_STBY_WID	1
+#define MC13783_REGCTRL_VVIB_EN_LSH	11
+#define MC13783_REGCTRL_VVIB_EN_WID	1
+#define MC13783_REGCTRL_VVIB_EN_ENABLE	1
+#define MC13783_REGCTRL_VVIB_EN_DISABLE	0
+#define MC13783_REGCTRL_VRF1_EN_LSH	12
+#define MC13783_REGCTRL_VRF1_EN_WID	1
+#define MC13783_REGCTRL_VRF1_EN_ENABLE	1
+#define MC13783_REGCTRL_VRF1_EN_DISABLE	0
+#define MC13783_REGCTRL_VRF1_STBY_LSH	13
+#define MC13783_REGCTRL_VRF1_STBY_WID	1
+#define MC13783_REGCTRL_VRF1_MODE_LSH	14
+#define MC13783_REGCTRL_VRF1_MODE_WID	1
+#define MC13783_REGCTRL_VRF2_EN_LSH	15
+#define MC13783_REGCTRL_VRF2_EN_WID	1
+#define MC13783_REGCTRL_VRF2_EN_ENABLE	1
+#define MC13783_REGCTRL_VRF2_EN_DISABLE	0
+#define MC13783_REGCTRL_VRF2_STBY_LSH	16
+#define MC13783_REGCTRL_VRF2_STBY_WID	1
+#define MC13783_REGCTRL_VRF2_MODE_LSH	17
+#define MC13783_REGCTRL_VRF2_MODE_WID	1
+#define MC13783_REGCTRL_VMMC1_EN_LSH	18
+#define MC13783_REGCTRL_VMMC1_EN_WID	1
+#define MC13783_REGCTRL_VMMC1_EN_ENABLE	1
+#define MC13783_REGCTRL_VMMC1_EN_DISABLE	0
+#define MC13783_REGCTRL_VMMC1_STBY_LSH	19
+#define MC13783_REGCTRL_VMMC1_STBY_WID	1
+#define MC13783_REGCTRL_VMMC1_MODE_LSH	20
+#define MC13783_REGCTRL_VMMC1_MODE_WID	1
+#define MC13783_REGCTRL_VMMC2_EN_LSH	21
+#define MC13783_REGCTRL_VMMC2_EN_WID	1
+#define MC13783_REGCTRL_VMMC2_EN_ENABLE	1
+#define MC13783_REGCTRL_VMMC2_EN_DISABLE	0
+#define MC13783_REGCTRL_VMMC2_STBY_LSH	22
+#define MC13783_REGCTRL_VMMC2_STBY_WID	1
+#define MC13783_REGCTRL_VMMC2_MODE_LSH	23
+#define MC13783_REGCTRL_VMMC2_MODE_WID	1
+
+/*
+ * Reg Regulator Misc.
+ */
+#define MC13783_REGCTRL_GPO1_EN_LSH	6
+#define MC13783_REGCTRL_GPO1_EN_WID	1
+#define MC13783_REGCTRL_GPO1_EN_ENABLE	1
+#define MC13783_REGCTRL_GPO1_EN_DISABLE	0
+#define MC13783_REGCTRL_GPO2_EN_LSH	8
+#define MC13783_REGCTRL_GPO2_EN_WID	1
+#define MC13783_REGCTRL_GPO2_EN_ENABLE	1
+#define MC13783_REGCTRL_GPO2_EN_DISABLE	0
+#define MC13783_REGCTRL_GPO3_EN_LSH	10
+#define MC13783_REGCTRL_GPO3_EN_WID	1
+#define MC13783_REGCTRL_GPO3_EN_ENABLE	1
+#define MC13783_REGCTRL_GPO3_EN_DISABLE	0
+#define MC13783_REGCTRL_GPO4_EN_LSH	12
+#define MC13783_REGCTRL_GPO4_EN_WID	1
+#define MC13783_REGCTRL_GPO4_EN_ENABLE	1
+#define MC13783_REGCTRL_GPO4_EN_DISABLE	0
+#define MC13783_REGCTRL_VIBPINCTRL_LSH	14
+#define MC13783_REGCTRL_VIBPINCTRL_WID	1
+
+/*
+ * Reg Regulator Setting 0
+ */
+#define MC13783_REGSET_VIOLO_LSH		2
+#define MC13783_REGSET_VIOLO_WID		2
+#define MC13783_REGSET_VDIG_LSH		4
+#define MC13783_REGSET_VDIG_WID		2
+#define MC13783_REGSET_VGEN_LSH		6
+#define MC13783_REGSET_VGEN_WID		3
+#define MC13783_REGSET_VRFDIG_LSH		9
+#define MC13783_REGSET_VRFDIG_WID		2
+#define MC13783_REGSET_VRFREF_LSH		11
+#define MC13783_REGSET_VRFREF_WID		2
+#define MC13783_REGSET_VRFCP_LSH		13
+#define MC13783_REGSET_VRFCP_WID		1
+#define MC13783_REGSET_VSIM_LSH		14
+#define MC13783_REGSET_VSIM_WID		1
+#define MC13783_REGSET_VESIM_LSH		15
+#define MC13783_REGSET_VESIM_WID		1
+#define MC13783_REGSET_VCAM_LSH		16
+#define MC13783_REGSET_VCAM_WID		3
+
+/*
+ * Reg Regulator Setting 1
+ */
+#define MC13783_REGSET_VVIB_LSH		0
+#define MC13783_REGSET_VVIB_WID		2
+#define MC13783_REGSET_VRF1_LSH		2
+#define MC13783_REGSET_VRF1_WID		2
+#define MC13783_REGSET_VRF2_LSH		4
+#define MC13783_REGSET_VRF2_WID		2
+#define MC13783_REGSET_VMMC1_LSH		6
+#define MC13783_REGSET_VMMC1_WID		3
+#define MC13783_REGSET_VMMC2_LSH		9
+#define MC13783_REGSET_VMMC2_WID		3
+
+/*
+ * Reg Switcher 0
+ */
+#define MC13783_SWSET_SW1A_LSH		0
+#define MC13783_SWSET_SW1A_WID		6
+#define MC13783_SWSET_SW1A_DVS_LSH	6
+#define MC13783_SWSET_SW1A_DVS_WID	6
+#define MC13783_SWSET_SW1A_STDBY_LSH	12
+#define MC13783_SWSET_SW1A_STDBY_WID	6
+
+/*
+ * Reg Switcher 1
+ */
+#define MC13783_SWSET_SW1B_LSH		0
+#define MC13783_SWSET_SW1B_WID		6
+#define MC13783_SWSET_SW1B_DVS_LSH	6
+#define MC13783_SWSET_SW1B_DVS_WID	6
+#define MC13783_SWSET_SW1B_STDBY_LSH	12
+#define MC13783_SWSET_SW1B_STDBY_WID	6
+
+/*
+ * Reg Switcher 2
+ */
+#define MC13783_SWSET_SW2A_LSH		0
+#define MC13783_SWSET_SW2A_WID		6
+#define MC13783_SWSET_SW2A_DVS_LSH	6
+#define MC13783_SWSET_SW2A_DVS_WID	6
+#define MC13783_SWSET_SW2A_STDBY_LSH	12
+#define MC13783_SWSET_SW2A_STDBY_WID	6
+
+/*
+ * Reg Switcher 3
+ */
+#define MC13783_SWSET_SW2B_LSH		0
+#define MC13783_SWSET_SW2B_WID		6
+#define MC13783_SWSET_SW2B_DVS_LSH	6
+#define MC13783_SWSET_SW2B_DVS_WID	6
+#define MC13783_SWSET_SW2B_STDBY_LSH	12
+#define MC13783_SWSET_SW2B_STDBY_WID	6
+
+/*
+ * Reg Switcher 4
+ */
+#define MC13783_SWCTRL_SW1A_MODE_LSH		0
+#define MC13783_SWCTRL_SW1A_MODE_WID		2
+#define MC13783_SWCTRL_SW1A_STBY_MODE_LSH		2
+#define MC13783_SWCTRL_SW1A_STBY_MODE_WID		2
+#define MC13783_SWCTRL_SW1A_DVS_SPEED_LSH		6
+#define MC13783_SWCTRL_SW1A_DVS_SPEED_WID		2
+#define MC13783_SWCTRL_SW1A_PANIC_MODE_LSH	8
+#define MC13783_SWCTRL_SW1A_PANIC_MODE_WID	1
+#define MC13783_SWCTRL_SW1A_SOFTSTART_LSH		9
+#define MC13783_SWCTRL_SW1A_SOFTSTART_WID		1
+#define MC13783_SWCTRL_SW1B_MODE_LSH		10
+#define MC13783_SWCTRL_SW1B_MODE_WID		2
+#define MC13783_SWCTRL_SW1B_STBY_MODE_LSH		12
+#define MC13783_SWCTRL_SW1B_STBY_MODE_WID		2
+#define MC13783_SWCTRL_SW1B_DVS_SPEED_LSH		14
+#define MC13783_SWCTRL_SW1B_DVS_SPEED_WID		2
+#define MC13783_SWCTRL_SW1B_PANIC_MODE_LSH	16
+#define MC13783_SWCTRL_SW1B_PANIC_MODE_WID	1
+#define MC13783_SWCTRL_SW1B_SOFTSTART_LSH		17
+#define MC13783_SWCTRL_SW1B_SOFTSTART_WID		1
+#define MC13783_SWCTRL_PLL_EN_LSH			18
+#define MC13783_SWCTRL_PLL_EN_WID			1
+#define MC13783_SWCTRL_PLL_EN_ENABLE		1
+#define MC13783_SWCTRL_PLL_EN_DISABLE		0
+#define MC13783_SWCTRL_PLL_FACTOR_LSH		19
+#define MC13783_SWCTRL_PLL_FACTOR_WID		3
+
+/*
+ * Reg Switcher 5
+ */
+#define MC13783_SWCTRL_SW2A_MODE_LSH		0
+#define MC13783_SWCTRL_SW2A_MODE_WID		2
+#define MC13783_SWCTRL_SW2A_STBY_MODE_LSH		2
+#define MC13783_SWCTRL_SW2A_STBY_MODE_WID		2
+#define MC13783_SWCTRL_SW2A_DVS_SPEED_LSH		6
+#define MC13783_SWCTRL_SW2A_DVS_SPEED_WID		2
+#define MC13783_SWCTRL_SW2A_PANIC_MODE_LSH	8
+#define MC13783_SWCTRL_SW2A_PANIC_MODE_WID	1
+#define MC13783_SWCTRL_SW2A_SOFTSTART_LSH		9
+#define MC13783_SWCTRL_SW2A_SOFTSTART_WID		1
+#define MC13783_SWCTRL_SW2B_MODE_LSH		10
+#define MC13783_SWCTRL_SW2B_MODE_WID		2
+#define MC13783_SWCTRL_SW2B_STBY_MODE_LSH		12
+#define MC13783_SWCTRL_SW2B_STBY_MODE_WID		2
+#define MC13783_SWCTRL_SW2B_DVS_SPEED_LSH		14
+#define MC13783_SWCTRL_SW2B_DVS_SPEED_WID		2
+#define MC13783_SWCTRL_SW2B_PANIC_MODE_LSH	16
+#define MC13783_SWCTRL_SW2B_PANIC_MODE_WID	1
+#define MC13783_SWCTRL_SW2B_SOFTSTART_LSH		17
+#define MC13783_SWCTRL_SW2B_SOFTSTART_WID		1
+#define MC13783_SWSET_SW3_LSH			18
+#define MC13783_SWSET_SW3_WID			2
+#define MC13783_SWCTRL_SW3_EN_LSH			20
+#define MC13783_SWCTRL_SW3_EN_WID			2
+#define MC13783_SWCTRL_SW3_EN_ENABLE		1
+#define MC13783_SWCTRL_SW3_EN_DISABLE		0
+#define MC13783_SWCTRL_SW3_STBY_LSH		21
+#define MC13783_SWCTRL_SW3_STBY_WID		1
+#define MC13783_SWCTRL_SW3_MODE_LSH		22
+#define MC13783_SWCTRL_SW3_MODE_WID		1
+
+/*
+ * Switcher configuration
+ */
+#define MC13783_SWCTRL_SW_MODE_SYNC_RECT_EN	0
+#define MC13783_SWCTRL_SW_MODE_PULSE_NO_SKIP_EN	1
+#define MC13783_SWCTRL_SW_MODE_PULSE_SKIP_EN	2
+#define MC13783_SWCTRL_SW_MODE_LOW_POWER_EN	3
+#define MC13783_REGTRL_LP_MODE_ENABLE		1
+#define MC13783_REGTRL_LP_MODE_DISABLE		0
+#define MC13783_REGTRL_STBY_MODE_ENABLE		1
+#define MC13783_REGTRL_STBY_MODE_DISABLE		0
+
+#endif				/*  __MC13783_POWER_DEFS_H__ */
diff --git a/drivers/mxc/pmic/mc13783/pmic_rtc.c b/drivers/mxc/pmic/mc13783/pmic_rtc.c
new file mode 100644
index 000000000000..50f086f87022
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_rtc.c
@@ -0,0 +1,552 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13783/pmic_rtc.c
+ * @brief This is the main file of PMIC(mc13783) RTC driver.
+ *
+ * @ingroup PMIC_RTC
+ */
+
+/*
+ * Includes
+ */
+#include <linux/wait.h>
+#include <linux/poll.h>
+#include <linux/platform_device.h>
+#include <linux/pmic_rtc.h>
+#include <linux/pmic_status.h>
+
+#include "pmic_rtc_defs.h"
+
+#define PMIC_LOAD_ERROR_MSG		\
+"PMIC card was not correctly detected. Stop loading PMIC RTC driver\n"
+
+/*
+ * Global variables
+ */
+static int pmic_rtc_major;
+static void callback_alarm_asynchronous(void *);
+static void callback_alarm_synchronous(void *);
+static unsigned int pmic_rtc_poll(struct file *file, poll_table * wait);
+static DECLARE_WAIT_QUEUE_HEAD(queue_alarm);
+static DECLARE_WAIT_QUEUE_HEAD(pmic_rtc_wait);
+static pmic_event_callback_t alarm_callback;
+static pmic_event_callback_t rtc_callback;
+static int pmic_rtc_detected = 0;
+static bool pmic_rtc_done = 0;
+static struct class *pmic_rtc_class;
+
+static DECLARE_MUTEX(mutex);
+
+/* EXPORTED FUNCTIONS */
+EXPORT_SYMBOL(pmic_rtc_set_time);
+EXPORT_SYMBOL(pmic_rtc_get_time);
+EXPORT_SYMBOL(pmic_rtc_set_time_alarm);
+EXPORT_SYMBOL(pmic_rtc_get_time_alarm);
+EXPORT_SYMBOL(pmic_rtc_wait_alarm);
+EXPORT_SYMBOL(pmic_rtc_event_sub);
+EXPORT_SYMBOL(pmic_rtc_event_unsub);
+EXPORT_SYMBOL(pmic_rtc_loaded);
+
+/*
+ * Real Time Clock Pmic API
+ */
+
+/*!
+ * This is the callback function called on TSI Pmic event, used in asynchronous
+ * call.
+ */
+static void callback_alarm_asynchronous(void *unused)
+{
+	pmic_rtc_done = true;
+}
+
+/*!
+ * This is the callback function is used in test code for (un)sub.
+ */
+static void callback_test_sub(void)
+{
+	printk(KERN_INFO "*****************************************\n");
+	printk(KERN_INFO "***** PMIC RTC 'Alarm IT CallBack' ******\n");
+	printk(KERN_INFO "*****************************************\n");
+}
+
+/*!
+ * This is the callback function called on TSI Pmic event, used in synchronous
+ * call.
+ */
+static void callback_alarm_synchronous(void *unused)
+{
+	printk(KERN_INFO "*** Alarm IT Pmic ***\n");
+	wake_up(&queue_alarm);
+}
+
+/*!
+ * This function wait the Alarm event
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_rtc_wait_alarm(void)
+{
+	DEFINE_WAIT(wait);
+	alarm_callback.func = callback_alarm_synchronous;
+	alarm_callback.param = NULL;
+	CHECK_ERROR(pmic_event_subscribe(EVENT_TODAI, alarm_callback));
+	prepare_to_wait(&queue_alarm, &wait, TASK_UNINTERRUPTIBLE);
+	schedule();
+	finish_wait(&queue_alarm, &wait);
+	CHECK_ERROR(pmic_event_unsubscribe(EVENT_TODAI, alarm_callback));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function set the real time clock of PMIC
+ *
+ * @param        pmic_time  	value of date and time
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_rtc_set_time(struct timeval * pmic_time)
+{
+	unsigned int tod_reg_val = 0;
+	unsigned int day_reg_val = 0;
+	unsigned int mask, value;
+
+	tod_reg_val = pmic_time->tv_sec % 86400;
+	day_reg_val = pmic_time->tv_sec / 86400;
+
+	mask = BITFMASK(MC13783_RTCTIME_TIME);
+	value = BITFVAL(MC13783_RTCTIME_TIME, tod_reg_val);
+	CHECK_ERROR(pmic_write_reg(REG_RTC_TIME, value, mask));
+
+	mask = BITFMASK(MC13783_RTCDAY_DAY);
+	value = BITFVAL(MC13783_RTCDAY_DAY, day_reg_val);
+	CHECK_ERROR(pmic_write_reg(REG_RTC_DAY, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function get the real time clock of PMIC
+ *
+ * @param        pmic_time  	return value of date and time
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_rtc_get_time(struct timeval * pmic_time)
+{
+	unsigned int tod_reg_val = 0;
+	unsigned int day_reg_val = 0;
+	unsigned int mask, value;
+
+	mask = BITFMASK(MC13783_RTCTIME_TIME);
+	CHECK_ERROR(pmic_read_reg(REG_RTC_TIME, &value, mask));
+	tod_reg_val = BITFEXT(value, MC13783_RTCTIME_TIME);
+
+	mask = BITFMASK(MC13783_RTCDAY_DAY);
+	CHECK_ERROR(pmic_read_reg(REG_RTC_DAY, &value, mask));
+	day_reg_val = BITFEXT(value, MC13783_RTCDAY_DAY);
+
+	pmic_time->tv_sec = (unsigned long)((unsigned long)(tod_reg_val &
+							    0x0001FFFF) +
+					    (unsigned long)(day_reg_val *
+							    86400));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function set the real time clock alarm of PMIC
+ *
+ * @param        pmic_time  	value of date and time
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_rtc_set_time_alarm(struct timeval * pmic_time)
+{
+	unsigned int tod_reg_val = 0;
+	unsigned int day_reg_val = 0;
+	unsigned int mask, value;
+	int ret;
+
+	if ((ret = down_interruptible(&mutex)) < 0)
+		return ret;
+
+	tod_reg_val = pmic_time->tv_sec % 86400;
+	day_reg_val = pmic_time->tv_sec / 86400;
+
+	mask = BITFMASK(MC13783_RTCALARM_TIME);
+	value = BITFVAL(MC13783_RTCALARM_TIME, tod_reg_val);
+	CHECK_ERROR(pmic_write_reg(REG_RTC_ALARM, value, mask));
+
+	mask = BITFMASK(MC13783_RTCALARM_DAY);
+	value = BITFVAL(MC13783_RTCALARM_DAY, day_reg_val);
+	CHECK_ERROR(pmic_write_reg(REG_RTC_DAY_ALARM, value, mask));
+	up(&mutex);
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function get the real time clock alarm of PMIC
+ *
+ * @param        pmic_time  	return value of date and time
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_rtc_get_time_alarm(struct timeval * pmic_time)
+{
+	unsigned int tod_reg_val = 0;
+	unsigned int day_reg_val = 0;
+	unsigned int mask, value;
+
+	mask = BITFMASK(MC13783_RTCALARM_TIME);
+	CHECK_ERROR(pmic_read_reg(REG_RTC_ALARM, &value, mask));
+	tod_reg_val = BITFEXT(value, MC13783_RTCALARM_TIME);
+
+	mask = BITFMASK(MC13783_RTCALARM_DAY);
+	CHECK_ERROR(pmic_read_reg(REG_RTC_DAY_ALARM, &value, mask));
+	day_reg_val = BITFEXT(value, MC13783_RTCALARM_DAY);
+
+	pmic_time->tv_sec = (unsigned long)((unsigned long)(tod_reg_val &
+							    0x0001FFFF) +
+					    (unsigned long)(day_reg_val *
+							    86400));
+
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is used to un/subscribe on RTC event IT.
+ *
+ * @param        event  	type of event.
+ * @param        callback  	event callback function.
+ * @param        sub      	define if Un/subscribe event.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_rtc_event(t_rtc_int event, void *callback, bool sub)
+{
+	type_event rtc_event;
+	if (callback == NULL) {
+		return PMIC_ERROR;
+	} else {
+		rtc_callback.func = callback;
+		rtc_callback.param = NULL;
+	}
+	switch (event) {
+	case RTC_IT_ALARM:
+		rtc_event = EVENT_TODAI;
+		break;
+	case RTC_IT_1HZ:
+		rtc_event = EVENT_E1HZI;
+		break;
+	case RTC_IT_RST:
+		rtc_event = EVENT_RTCRSTI;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	if (sub == true) {
+		CHECK_ERROR(pmic_event_subscribe(rtc_event, rtc_callback));
+	} else {
+		CHECK_ERROR(pmic_event_unsubscribe(rtc_event, rtc_callback));
+	}
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is used to subscribe on RTC event IT.
+ *
+ * @param        event  	type of event.
+ * @param        callback  	event callback function.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_rtc_event_sub(t_rtc_int event, void *callback)
+{
+	CHECK_ERROR(pmic_rtc_event(event, callback, true));
+	return PMIC_SUCCESS;
+}
+
+/*!
+ * This function is used to un subscribe on RTC event IT.
+ *
+ * @param        event  	type of event.
+ * @param        callback  	event callback function.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_rtc_event_unsub(t_rtc_int event, void *callback)
+{
+	CHECK_ERROR(pmic_rtc_event(event, callback, false));
+	return PMIC_SUCCESS;
+}
+
+/* Called without the kernel lock - fine */
+static unsigned int pmic_rtc_poll(struct file *file, poll_table * wait)
+{
+	/*poll_wait(file, &pmic_rtc_wait, wait); */
+
+	if (pmic_rtc_done)
+		return POLLIN | POLLRDNORM;
+	return 0;
+}
+
+/*!
+ * This function implements IOCTL controls on a PMIC RTC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @param        cmd         the command
+ * @param        arg         the parameter
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_rtc_ioctl(struct inode *inode, struct file *file,
+			  unsigned int cmd, unsigned long arg)
+{
+	struct timeval *pmic_time = NULL;
+
+	if (_IOC_TYPE(cmd) != 'p')
+		return -ENOTTY;
+
+	if (arg) {
+		if ((pmic_time = kmalloc(sizeof(struct timeval),
+					 GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		/*      if (copy_from_user(pmic_time, (struct timeval *)arg,
+		   sizeof(struct timeval))) {
+		   return -EFAULT;
+		   } */
+	}
+
+	switch (cmd) {
+	case PMIC_RTC_SET_TIME:
+		if (copy_from_user(pmic_time, (struct timeval *)arg,
+				   sizeof(struct timeval))) {
+			return -EFAULT;
+		}
+		pr_debug("SET RTC\n");
+		CHECK_ERROR(pmic_rtc_set_time(pmic_time));
+		break;
+	case PMIC_RTC_GET_TIME:
+		if (copy_to_user((struct timeval *)arg, pmic_time,
+				 sizeof(struct timeval))) {
+			return -EFAULT;
+		}
+		pr_debug("GET RTC\n");
+		CHECK_ERROR(pmic_rtc_get_time(pmic_time));
+		break;
+	case PMIC_RTC_SET_ALARM:
+		if (copy_from_user(pmic_time, (struct timeval *)arg,
+				   sizeof(struct timeval))) {
+			return -EFAULT;
+		}
+		pr_debug("SET RTC ALARM\n");
+		CHECK_ERROR(pmic_rtc_set_time_alarm(pmic_time));
+		break;
+	case PMIC_RTC_GET_ALARM:
+		if (copy_to_user((struct timeval *)arg, pmic_time,
+				 sizeof(struct timeval))) {
+			return -EFAULT;
+		}
+		pr_debug("GET RTC ALARM\n");
+		CHECK_ERROR(pmic_rtc_get_time_alarm(pmic_time));
+		break;
+	case PMIC_RTC_WAIT_ALARM:
+		printk(KERN_INFO "WAIT ALARM...\n");
+		CHECK_ERROR(pmic_rtc_event_sub(RTC_IT_ALARM,
+					       callback_test_sub));
+		CHECK_ERROR(pmic_rtc_wait_alarm());
+		printk(KERN_INFO "ALARM DONE\n");
+		CHECK_ERROR(pmic_rtc_event_unsub(RTC_IT_ALARM,
+						 callback_test_sub));
+		break;
+	case PMIC_RTC_ALARM_REGISTER:
+		printk(KERN_INFO "PMIC RTC ALARM REGISTER\n");
+		alarm_callback.func = callback_alarm_asynchronous;
+		alarm_callback.param = NULL;
+		CHECK_ERROR(pmic_event_subscribe(EVENT_TODAI, alarm_callback));
+		break;
+	case PMIC_RTC_ALARM_UNREGISTER:
+		printk(KERN_INFO "PMIC RTC ALARM UNREGISTER\n");
+		alarm_callback.func = callback_alarm_asynchronous;
+		alarm_callback.param = NULL;
+		CHECK_ERROR(pmic_event_unsubscribe
+			    (EVENT_TODAI, alarm_callback));
+		pmic_rtc_done = false;
+		break;
+	default:
+		pr_debug("%d unsupported ioctl command\n", (int)cmd);
+		return -EINVAL;
+	}
+
+	if (arg) {
+		if (copy_to_user((struct timeval *)arg, pmic_time,
+				 sizeof(struct timeval))) {
+			return -EFAULT;
+		}
+		kfree(pmic_time);
+	}
+
+	return 0;
+}
+
+/*!
+ * This function implements the open method on a PMIC RTC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_rtc_open(struct inode *inode, struct file *file)
+{
+	return 0;
+}
+
+/*!
+ * This function implements the release method on a PMIC RTC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_rtc_release(struct inode *inode, struct file *file)
+{
+	return 0;
+}
+
+/*!
+ * This function is called to put the RTC in a low power state.
+ * There is no need for power handlers for the RTC device.
+ * The RTC cannot be suspended.
+ *
+ * @param   pdev  the device structure used to give information on which RTC
+ *                device (0 through 3 channels) to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function always returns 0.
+ */
+static int pmic_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	return 0;
+}
+
+/*!
+ * This function is called to resume the RTC from a low power state.
+ *
+ * @param   pdev  the device structure used to give information on which RTC
+ *                device (0 through 3 channels) to suspend
+ *
+ * @return  The function always returns 0.
+ */
+static int pmic_rtc_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+
+static struct file_operations pmic_rtc_fops = {
+	.owner = THIS_MODULE,
+	.ioctl = pmic_rtc_ioctl,
+	.poll = pmic_rtc_poll,
+	.open = pmic_rtc_open,
+	.release = pmic_rtc_release,
+};
+
+int pmic_rtc_loaded(void)
+{
+	return pmic_rtc_detected;
+}
+
+static int pmic_rtc_remove(struct platform_device *pdev)
+{
+	device_destroy(pmic_rtc_class, MKDEV(pmic_rtc_major, 0));
+	class_destroy(pmic_rtc_class);
+	unregister_chrdev(pmic_rtc_major, "pmic_rtc");
+	return 0;
+}
+
+static int pmic_rtc_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct device *temp_class;
+
+	pmic_rtc_major = register_chrdev(0, "pmic_rtc", &pmic_rtc_fops);
+	if (pmic_rtc_major < 0) {
+		printk(KERN_ERR "Unable to get a major for pmic_rtc\n");
+		return pmic_rtc_major;
+	}
+
+	pmic_rtc_class = class_create(THIS_MODULE, "pmic_rtc");
+	if (IS_ERR(pmic_rtc_class)) {
+		printk(KERN_ERR "Error creating pmic rtc class.\n");
+		ret = PTR_ERR(pmic_rtc_class);
+		goto err_out1;
+	}
+
+	temp_class = device_create(pmic_rtc_class, NULL,
+				   MKDEV(pmic_rtc_major, 0), NULL,
+				   "pmic_rtc");
+	if (IS_ERR(temp_class)) {
+		printk(KERN_ERR "Error creating pmic rtc class device.\n");
+		ret = PTR_ERR(temp_class);
+		goto err_out2;
+	}
+
+	pmic_rtc_detected = 1;
+	printk(KERN_INFO "PMIC RTC successfully probed\n");
+	return ret;
+
+      err_out2:
+	class_destroy(pmic_rtc_class);
+      err_out1:
+	unregister_chrdev(pmic_rtc_major, "pmic_rtc");
+	return ret;
+}
+
+static struct platform_driver pmic_rtc_driver_ldm = {
+	.driver = {
+		   .name = "pmic_rtc",
+		   .owner = THIS_MODULE,
+		   },
+	.suspend = pmic_rtc_suspend,
+	.resume = pmic_rtc_resume,
+	.probe = pmic_rtc_probe,
+	.remove = pmic_rtc_remove,
+};
+
+static int __init pmic_rtc_init(void)
+{
+	pr_debug("PMIC RTC driver loading...\n");
+	return platform_driver_register(&pmic_rtc_driver_ldm);
+}
+static void __exit pmic_rtc_exit(void)
+{
+	platform_driver_unregister(&pmic_rtc_driver_ldm);
+	pr_debug("PMIC RTC driver successfully unloaded\n");
+}
+
+/*
+ * Module entry points
+ */
+
+subsys_initcall(pmic_rtc_init);
+module_exit(pmic_rtc_exit);
+
+MODULE_DESCRIPTION("Pmic_rtc driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/mc13783/pmic_rtc_defs.h b/drivers/mxc/pmic/mc13783/pmic_rtc_defs.h
new file mode 100644
index 000000000000..16e968dd9977
--- /dev/null
+++ b/drivers/mxc/pmic/mc13783/pmic_rtc_defs.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __MC13783_RTC_DEFS_H__
+#define __MC13783_RTC_DEFS_H__
+
+/*!
+ * @file mc13783/pmic_rtc_defs.h
+ * @brief This is the internal header of PMIC(mc13783) RTC driver.
+ *
+ * @ingroup PMIC_RTC
+ */
+
+/*
+ * RTC Time
+ */
+#define MC13783_RTCTIME_TIME_LSH	0
+#define MC13783_RTCTIME_TIME_WID	17
+
+/*
+ * RTC Alarm
+ */
+#define MC13783_RTCALARM_TIME_LSH	0
+#define MC13783_RTCALARM_TIME_WID	17
+
+/*
+ * RTC Day
+ */
+#define MC13783_RTCDAY_DAY_LSH		0
+#define MC13783_RTCDAY_DAY_WID		15
+
+/*
+ * RTC Day alarm
+ */
+#define MC13783_RTCALARM_DAY_LSH	0
+#define MC13783_RTCALARM_DAY_WID	15
+
+#endif				/* __MC13783_RTC_DEFS_H__ */
diff --git a/drivers/mxc/pmic/mc13892/Kconfig b/drivers/mxc/pmic/mc13892/Kconfig
new file mode 100644
index 000000000000..930e06ab2282
--- /dev/null
+++ b/drivers/mxc/pmic/mc13892/Kconfig
@@ -0,0 +1,48 @@
+#
+# PMIC Modules configuration
+#
+
+config MXC_MC13892_ADC
+	tristate "MC13892 ADC support"
+	depends on MXC_PMIC_MC13892
+	---help---
+	This is the MC13892 ADC module driver. This module provides kernel API
+	for the ADC system of MC13892.
+	It controls also the touch screen interface.
+	If you want MC13892 ADC support, you should say Y here
+
+config MXC_MC13892_RTC
+	tristate "MC13892 Real Time Clock (RTC) support"
+	depends on MXC_PMIC_MC13892
+	---help---
+	This is the MC13892 RTC module driver. This module provides kernel API
+	for RTC part of MC13892.
+	If you want MC13892 RTC support, you should say Y here
+config MXC_MC13892_LIGHT
+	tristate "MC13892 Light and Backlight support"
+	depends on MXC_PMIC_MC13892
+	---help---
+	This is the MC13892 Light module driver. This module provides kernel API
+	for led and backlight control part of MC13892.
+	If you want MC13892 Light support, you should say Y here
+config MXC_MC13892_BATTERY
+	tristate "MC13892 Battery API support"
+	depends on MXC_PMIC_MC13892
+	---help---
+	This is the MC13892 battery module driver. This module provides kernel API
+	for battery control part of MC13892.
+	If you want MC13892 battery support, you should say Y here
+config MXC_MC13892_CONNECTIVITY
+	tristate "MC13892 Connectivity API support"
+	depends on MXC_PMIC_MC13892
+	---help---
+	This is the MC13892 connectivity module driver. This module provides kernel API
+	for USB/RS232 connectivity control part of MC13892.
+	If you want MC13892 connectivity support, you should say Y here
+config MXC_MC13892_POWER
+	tristate "MC13892 Power API support"
+	depends on MXC_PMIC_MC13892
+	---help---
+	This is the MC13892 power and supplies module driver. This module provides kernel API
+	for power and regulator control part of MC13892.
+	If you want MC13892 power support, you should say Y here
diff --git a/drivers/mxc/pmic/mc13892/Makefile b/drivers/mxc/pmic/mc13892/Makefile
new file mode 100644
index 000000000000..0ed2b7eb4c11
--- /dev/null
+++ b/drivers/mxc/pmic/mc13892/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for the mc13783 pmic drivers.
+#
+
+obj-$(CONFIG_MXC_MC13892_ADC) += pmic_adc.o
+#obj-$(CONFIG_MXC_MC13892_RTC) += pmic_rtc.o
+obj-$(CONFIG_MXC_MC13892_LIGHT) += pmic_light.o
+obj-$(CONFIG_MXC_MC13892_BATTERY) += pmic_battery.o
+#obj-$(CONFIG_MXC_MC13892_CONNECTIVITY) += pmic_convity.o
+#obj-$(CONFIG_MXC_MC13892_POWER) += pmic_power.o
diff --git a/drivers/mxc/pmic/mc13892/pmic_adc.c b/drivers/mxc/pmic/mc13892/pmic_adc.c
new file mode 100644
index 000000000000..cec4d6045974
--- /dev/null
+++ b/drivers/mxc/pmic/mc13892/pmic_adc.c
@@ -0,0 +1,984 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/device.h>
+
+#include <linux/pmic_adc.h>
+#include <linux/pmic_status.h>
+
+#include "../core/pmic.h"
+
+#define         DEF_ADC_0     0x008000
+#define         DEF_ADC_3     0x0001c0
+
+#define ADC_NB_AVAILABLE        2
+
+#define MAX_CHANNEL             7
+
+#define MC13892_ADC0_TS_M_LSH	14
+#define MC13892_ADC0_TS_M_WID	3
+
+/*
+ * Maximun allowed variation in the three X/Y co-ordinates acquired from
+ * touch-screen
+ */
+#define DELTA_Y_MAX             50
+#define DELTA_X_MAX             50
+
+/*
+ * ADC 0
+ */
+#define ADC_WAIT_TSI_0		0x001400
+
+#define ADC_INC                 0x030000
+#define ADC_BIS                 0x800000
+#define ADC_CHRGRAW_D5          0x008000
+
+/*
+ * ADC 1
+ */
+
+#define ADC_EN                  0x000001
+#define ADC_SGL_CH              0x000002
+#define ADC_ADSEL               0x000008
+#define ADC_CH_0_POS            5
+#define ADC_CH_0_MASK           0x0000E0
+#define ADC_CH_1_POS            8
+#define ADC_CH_1_MASK           0x000700
+#define ADC_DELAY_POS           11
+#define ADC_DELAY_MASK          0x07F800
+#define ADC_ATO                 0x080000
+#define ASC_ADC                 0x100000
+#define ADC_WAIT_TSI_1		0x200001
+#define ADC_NO_ADTRIG           0x200000
+
+/*
+ * ADC 2 - 4
+ */
+#define ADD1_RESULT_MASK        0x00000FFC
+#define ADD2_RESULT_MASK        0x00FFC000
+#define ADC_TS_MASK             0x00FFCFFC
+
+#define ADC_WCOMP               0x040000
+#define ADC_WCOMP_H_POS         0
+#define ADC_WCOMP_L_POS         9
+#define ADC_WCOMP_H_MASK        0x00003F
+#define ADC_WCOMP_L_MASK        0x007E00
+
+#define ADC_MODE_MASK           0x00003F
+
+#define ADC_INT_BISDONEI        0x02
+#define ADC_TSMODE_MASK 0x007000
+
+typedef enum adc_state {
+	ADC_FREE,
+	ADC_USED,
+	ADC_MONITORING,
+} t_adc_state;
+
+typedef enum reading_mode {
+	/*!
+	 * Enables lithium cell reading
+	 */
+	M_LITHIUM_CELL = 0x000001,
+	/*!
+	 * Enables charge current reading
+	 */
+	M_CHARGE_CURRENT = 0x000002,
+	/*!
+	 * Enables battery current reading
+	 */
+	M_BATTERY_CURRENT = 0x000004,
+} t_reading_mode;
+
+typedef struct {
+	/*!
+	 * Delay before first conversion
+	 */
+	unsigned int delay;
+	/*!
+	 * sets the ATX bit for delay on all conversion
+	 */
+	bool conv_delay;
+	/*!
+	 * Sets the single channel mode
+	 */
+	bool single_channel;
+	/*!
+	 * Channel selection 1
+	 */
+	t_channel channel_0;
+	/*!
+	 * Channel selection 2
+	 */
+	t_channel channel_1;
+	/*!
+	 * Used to configure ADC mode with t_reading_mode
+	 */
+	t_reading_mode read_mode;
+	/*!
+	 * Sets the Touch screen mode
+	 */
+	bool read_ts;
+	/*!
+	 * Wait TSI event before touch screen reading
+	 */
+	bool wait_tsi;
+	/*!
+	 * Sets CHRGRAW scaling to divide by 5
+	 * Only supported on 2.0 and higher
+	 */
+	bool chrgraw_devide_5;
+	/*!
+	 * Return ADC values
+	 */
+	unsigned int value[8];
+	/*!
+	 * Return touch screen values
+	 */
+	t_touch_screen ts_value;
+} t_adc_param;
+
+static int pmic_adc_filter(t_touch_screen *ts_curr);
+int mc13892_adc_request(bool read_ts);
+int mc13892_adc_release(int adc_index);
+t_reading_mode mc13892_set_read_mode(t_channel channel);
+PMIC_STATUS mc13892_adc_read_ts(t_touch_screen *touch_sample, int wait_tsi);
+
+/* internal function */
+static void callback_tsi(void *);
+static void callback_adcdone(void *);
+static void callback_adcbisdone(void *);
+
+static int swait;
+
+static int suspend_flag;
+
+static wait_queue_head_t suspendq;
+
+/* EXPORTED FUNCTIONS */
+EXPORT_SYMBOL(pmic_adc_init);
+EXPORT_SYMBOL(pmic_adc_deinit);
+EXPORT_SYMBOL(pmic_adc_convert);
+EXPORT_SYMBOL(pmic_adc_convert_8x);
+EXPORT_SYMBOL(pmic_adc_set_touch_mode);
+EXPORT_SYMBOL(pmic_adc_get_touch_mode);
+EXPORT_SYMBOL(pmic_adc_get_touch_sample);
+
+static DECLARE_COMPLETION(adcdone_it);
+static DECLARE_COMPLETION(adcbisdone_it);
+static DECLARE_COMPLETION(adc_tsi);
+static pmic_event_callback_t tsi_event;
+static pmic_event_callback_t event_adc;
+static pmic_event_callback_t event_adc_bis;
+static bool data_ready_adc_1;
+static bool data_ready_adc_2;
+static bool adc_ts;
+static bool wait_ts;
+static bool monitor_en;
+static bool monitor_adc;
+static DECLARE_MUTEX(convert_mutex);
+
+static DECLARE_WAIT_QUEUE_HEAD(queue_adc_busy);
+static t_adc_state adc_dev[2];
+
+static unsigned channel_num[] = {
+	0,
+	1,
+	3,
+	4,
+	2,
+	0,
+	1,
+	3,
+	4,
+	-1,
+	5,
+	6,
+	7,
+	-1,
+	-1,
+	-1,
+	-1,
+	-1,
+	-1,
+	-1,
+	-1,
+	-1,
+	-1,
+	-1,
+	-1
+};
+
+static bool pmic_adc_ready;
+
+int is_pmic_adc_ready()
+{
+	return pmic_adc_ready;
+}
+EXPORT_SYMBOL(is_pmic_adc_ready);
+
+
+static int pmic_adc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	suspend_flag = 1;
+	CHECK_ERROR(pmic_write_reg(REG_ADC0, DEF_ADC_0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC1, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC2, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC3, DEF_ADC_3, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC4, 0, PMIC_ALL_BITS));
+
+	return 0;
+};
+
+static int pmic_adc_resume(struct platform_device *pdev)
+{
+	/* nothing for mc13892 adc */
+	unsigned int adc_0_reg, adc_1_reg, reg_mask;
+	suspend_flag = 0;
+
+	/* let interrupt of TSI again */
+	adc_0_reg = ADC_WAIT_TSI_0;
+	reg_mask = ADC_WAIT_TSI_0;
+	CHECK_ERROR(pmic_write_reg(REG_ADC0, adc_0_reg, reg_mask));
+	adc_1_reg = ADC_WAIT_TSI_1 | (ADC_BIS * adc_ts);
+	CHECK_ERROR(pmic_write_reg(REG_ADC1, adc_1_reg, PMIC_ALL_BITS));
+
+	while (swait > 0) {
+		swait--;
+		wake_up_interruptible(&suspendq);
+	}
+
+	return 0;
+};
+
+static void callback_tsi(void *unused)
+{
+	pr_debug("*** TSI IT mc13892 PMIC_ADC_GET_TOUCH_SAMPLE ***\n");
+	if (wait_ts) {
+		complete(&adc_tsi);
+		pmic_event_mask(EVENT_TSI);
+	}
+}
+
+static void callback_adcdone(void *unused)
+{
+	if (data_ready_adc_1)
+		complete(&adcdone_it);
+}
+
+static void callback_adcbisdone(void *unused)
+{
+	pr_debug("* adcdone bis it callback *\n");
+	if (data_ready_adc_2)
+		complete(&adcbisdone_it);
+}
+
+static int pmic_adc_filter(t_touch_screen *ts_curr)
+{
+	unsigned int ydiff, xdiff;
+	unsigned int sample_sumx, sample_sumy;
+
+	if (ts_curr->contact_resistance == 0) {
+		ts_curr->x_position = 0;
+		ts_curr->y_position = 0;
+		return 0;
+	}
+
+	ydiff = abs(ts_curr->y_position1 - ts_curr->y_position2);
+	if (ydiff > DELTA_Y_MAX) {
+		pr_debug("pmic_adc_filter: Ret pos y\n");
+		return -1;
+	}
+
+	xdiff = abs(ts_curr->x_position1 - ts_curr->x_position2);
+	if (xdiff > DELTA_X_MAX) {
+		pr_debug("mc13892_adc_filter: Ret pos x\n");
+		return -1;
+	}
+
+	sample_sumx = ts_curr->x_position1 + ts_curr->x_position2;
+	sample_sumy = ts_curr->y_position1 + ts_curr->y_position2;
+
+	ts_curr->y_position = sample_sumy / 2;
+	ts_curr->x_position = sample_sumx / 2;
+
+	return 0;
+}
+
+int pmic_adc_init(void)
+{
+	unsigned int reg_value = 0, i = 0;
+
+	if (suspend_flag == 1)
+		return -EBUSY;
+
+	for (i = 0; i < ADC_NB_AVAILABLE; i++)
+		adc_dev[i] = ADC_FREE;
+
+	CHECK_ERROR(pmic_write_reg(REG_ADC0, DEF_ADC_0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC1, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC2, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC3, DEF_ADC_3, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_ADC4, 0, PMIC_ALL_BITS));
+	reg_value = 0x001000;
+
+	data_ready_adc_1 = false;
+	data_ready_adc_2 = false;
+	adc_ts = false;
+	wait_ts = false;
+	monitor_en = false;
+	monitor_adc = false;
+
+	/* sub to ADCDone IT */
+	event_adc.param = NULL;
+	event_adc.func = callback_adcdone;
+	CHECK_ERROR(pmic_event_subscribe(EVENT_ADCDONEI, event_adc));
+
+	/* sub to ADCDoneBis IT */
+	event_adc_bis.param = NULL;
+	event_adc_bis.func = callback_adcbisdone;
+	CHECK_ERROR(pmic_event_subscribe(EVENT_ADCBISDONEI, event_adc_bis));
+
+	/* sub to Touch Screen IT */
+	tsi_event.param = NULL;
+	tsi_event.func = callback_tsi;
+	CHECK_ERROR(pmic_event_subscribe(EVENT_TSI, tsi_event));
+
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS pmic_adc_deinit(void)
+{
+	CHECK_ERROR(pmic_event_unsubscribe(EVENT_ADCDONEI, event_adc));
+	CHECK_ERROR(pmic_event_unsubscribe(EVENT_ADCBISDONEI, event_adc_bis));
+	CHECK_ERROR(pmic_event_unsubscribe(EVENT_TSI, tsi_event));
+
+	return PMIC_SUCCESS;
+}
+
+int mc13892_adc_init_param(t_adc_param * adc_param)
+{
+	int i = 0;
+
+	if (suspend_flag == 1)
+		return -EBUSY;
+
+	adc_param->delay = 0;
+	adc_param->conv_delay = false;
+	adc_param->single_channel = false;
+	adc_param->channel_0 = BATTERY_VOLTAGE;
+	adc_param->channel_1 = BATTERY_VOLTAGE;
+	adc_param->read_mode = 0;
+	adc_param->wait_tsi = 0;
+	adc_param->chrgraw_devide_5 = true;
+	adc_param->read_ts = false;
+	adc_param->ts_value.x_position = 0;
+	adc_param->ts_value.y_position = 0;
+	adc_param->ts_value.contact_resistance = 0;
+	for (i = 0; i <= MAX_CHANNEL; i++)
+		adc_param->value[i] = 0;
+
+	return 0;
+}
+
+PMIC_STATUS mc13892_adc_convert(t_adc_param * adc_param)
+{
+	bool use_bis = false;
+	unsigned int adc_0_reg = 0, adc_1_reg = 0, reg_1 = 0, result_reg =
+	    0, i = 0;
+	unsigned int result = 0, temp = 0;
+	pmic_version_t mc13892_ver;
+	pr_debug("mc13892 ADC - mc13892_adc_convert ....\n");
+	if (suspend_flag == 1)
+		return -EBUSY;
+
+	if (adc_param->wait_tsi) {
+		/* configure adc to wait tsi interrupt */
+		INIT_COMPLETION(adc_tsi);
+
+		/*for ts don't use bis */
+		/*put ts in interrupt mode */
+		/* still kep reference? */
+		adc_0_reg = 0x001400 | (ADC_BIS * 0);
+		pmic_event_unmask(EVENT_TSI);
+		CHECK_ERROR(pmic_write_reg(REG_ADC0, adc_0_reg, PMIC_ALL_BITS));
+		/*for ts don't use bis */
+		adc_1_reg = 0x200001 | (ADC_BIS * 0);
+		CHECK_ERROR(pmic_write_reg(REG_ADC1, adc_1_reg, PMIC_ALL_BITS));
+		pr_debug("wait tsi ....\n");
+		wait_ts = true;
+		wait_for_completion_interruptible(&adc_tsi);
+		wait_ts = false;
+	}
+	down(&convert_mutex);
+	use_bis = mc13892_adc_request(adc_param->read_ts);
+	if (use_bis < 0) {
+		pr_debug("process has received a signal and got interrupted\n");
+		return -EINTR;
+	}
+
+	/* CONFIGURE ADC REG 0 */
+	adc_0_reg = 0;
+	adc_1_reg = 0;
+	if (adc_param->read_ts == false) {
+		adc_0_reg = adc_param->read_mode & 0x00003F;
+		/* add auto inc */
+		adc_0_reg |= ADC_INC;
+		if (use_bis) {
+			/* add adc bis */
+			adc_0_reg |= ADC_BIS;
+		}
+		mc13892_ver = pmic_get_version();
+		if (mc13892_ver.revision >= 20)
+			if (adc_param->chrgraw_devide_5)
+				adc_0_reg |= ADC_CHRGRAW_D5;
+
+		if (adc_param->single_channel)
+			adc_1_reg |= ADC_SGL_CH;
+
+		if (adc_param->conv_delay)
+			adc_1_reg |= ADC_ATO;
+
+		if (adc_param->single_channel)
+			adc_1_reg |= ADC_SGL_CH;
+
+		adc_1_reg |= (adc_param->channel_0 << ADC_CH_0_POS) &
+		    ADC_CH_0_MASK;
+		adc_1_reg |= (adc_param->channel_1 << ADC_CH_1_POS) &
+		    ADC_CH_1_MASK;
+	} else {
+		adc_0_reg = 0x002400 | (ADC_BIS * use_bis) | ADC_INC;
+	}
+	pr_debug("Write Reg %i = %x\n", REG_ADC0, adc_0_reg);
+	/*Change has been made here */
+	CHECK_ERROR(pmic_write_reg(REG_ADC0, adc_0_reg,
+				   ADC_INC | ADC_BIS | ADC_CHRGRAW_D5 |
+				   0xfff00ff));
+	/* CONFIGURE ADC REG 1 */
+	if (adc_param->read_ts == false) {
+		adc_1_reg |= ADC_NO_ADTRIG;
+		adc_1_reg |= ADC_EN;
+		adc_1_reg |= (adc_param->delay << ADC_DELAY_POS) &
+		    ADC_DELAY_MASK;
+		if (use_bis)
+			adc_1_reg |= ADC_BIS;
+	} else {
+		/* configure and start convert to read x and y position */
+		/* configure to read 2 value in channel selection 1 & 2 */
+		adc_1_reg = 0x100409 | (ADC_BIS * use_bis) | ADC_NO_ADTRIG;
+		/* set ATOx = 5, it could be better for ts ADC */
+		adc_1_reg |= 0x002800;
+	}
+	reg_1 = adc_1_reg;
+	if (use_bis == 0) {
+		data_ready_adc_1 = false;
+		adc_1_reg |= ASC_ADC;
+		data_ready_adc_1 = true;
+		pr_debug("Write Reg %i = %x\n", REG_ADC1, adc_1_reg);
+		INIT_COMPLETION(adcdone_it);
+		CHECK_ERROR(pmic_write_reg(REG_ADC1, adc_1_reg,
+					   ADC_SGL_CH | ADC_ATO | ADC_ADSEL
+					   | ADC_CH_0_MASK | ADC_CH_1_MASK |
+					   ADC_NO_ADTRIG | ADC_EN |
+					   ADC_DELAY_MASK | ASC_ADC | ADC_BIS));
+		pr_debug("wait adc done \n");
+		wait_for_completion_interruptible(&adcdone_it);
+		data_ready_adc_1 = false;
+	} else {
+		data_ready_adc_2 = false;
+		adc_1_reg |= ASC_ADC;
+		data_ready_adc_2 = true;
+		INIT_COMPLETION(adcbisdone_it);
+		CHECK_ERROR(pmic_write_reg(REG_ADC1, adc_1_reg, 0xFFFFFF));
+		temp = 0x800000;
+		CHECK_ERROR(pmic_write_reg(REG_ADC3, temp, 0xFFFFFF));
+		pr_debug("wait adc done bis\n");
+		wait_for_completion_interruptible(&adcbisdone_it);
+		data_ready_adc_2 = false;
+	}
+	/* read result and store in adc_param */
+	result = 0;
+	if (use_bis == 0)
+		result_reg = REG_ADC2;
+	else
+		result_reg = REG_ADC4;
+
+	CHECK_ERROR(pmic_write_reg(REG_ADC1, 4 << ADC_CH_1_POS,
+				   ADC_CH_0_MASK | ADC_CH_1_MASK));
+
+	for (i = 0; i <= 3; i++) {
+		CHECK_ERROR(pmic_read_reg(result_reg, &result, PMIC_ALL_BITS));
+		adc_param->value[i] = ((result & ADD1_RESULT_MASK) >> 2);
+		adc_param->value[i + 4] = ((result & ADD2_RESULT_MASK) >> 14);
+		pr_debug("value[%d] = %d, value[%d] = %d\n",
+			 i, adc_param->value[i],
+			 i + 4, adc_param->value[i + 4]);
+	}
+	if (adc_param->read_ts) {
+		adc_param->ts_value.x_position = adc_param->value[0];
+		adc_param->ts_value.x_position1 = adc_param->value[0];
+		adc_param->ts_value.x_position2 = adc_param->value[1];
+		adc_param->ts_value.y_position = adc_param->value[3];
+		adc_param->ts_value.y_position1 = adc_param->value[3];
+		adc_param->ts_value.y_position2 = adc_param->value[4];
+		adc_param->ts_value.contact_resistance = adc_param->value[6];
+		CHECK_ERROR(pmic_write_reg(REG_ADC0, 0x0,
+				   ADC_TSMODE_MASK));
+	}
+
+	/*if (adc_param->read_ts) {
+	   adc_param->ts_value.x_position = adc_param->value[2];
+	   adc_param->ts_value.y_position = adc_param->value[5];
+	   adc_param->ts_value.contact_resistance = adc_param->value[6];
+	   } */
+	mc13892_adc_release(use_bis);
+	up(&convert_mutex);
+
+	return PMIC_SUCCESS;
+}
+
+t_reading_mode mc13892_set_read_mode(t_channel channel)
+{
+	t_reading_mode read_mode = 0;
+
+	switch (channel) {
+	case CHARGE_CURRENT:
+		read_mode = M_CHARGE_CURRENT;
+		break;
+	case BATTERY_CURRENT:
+		read_mode = M_BATTERY_CURRENT;
+		break;
+	default:
+		read_mode = 0;
+	}
+
+	return read_mode;
+}
+
+PMIC_STATUS pmic_adc_convert(t_channel channel, unsigned short *result)
+{
+	t_adc_param adc_param;
+	PMIC_STATUS ret;
+
+	if (suspend_flag == 1)
+		return -EBUSY;
+
+	channel = channel_num[channel];
+	if (channel == -1) {
+		pr_debug("Wrong channel ID\n");
+		return PMIC_PARAMETER_ERROR;
+	}
+	mc13892_adc_init_param(&adc_param);
+	pr_debug("pmic_adc_convert\n");
+	adc_param.read_ts = false;
+	adc_param.single_channel = true;
+	adc_param.read_mode = mc13892_set_read_mode(channel);
+
+	/* Find the group */
+	if (channel <= 7)
+		adc_param.channel_0 = channel;
+	else
+		return PMIC_PARAMETER_ERROR;
+
+	ret = mc13892_adc_convert(&adc_param);
+	*result = adc_param.value[0];
+	return ret;
+}
+
+PMIC_STATUS pmic_adc_convert_8x(t_channel channel, unsigned short *result)
+{
+	t_adc_param adc_param;
+	int i;
+	PMIC_STATUS ret;
+	if (suspend_flag == 1)
+		return -EBUSY;
+
+	channel = channel_num[channel];
+
+	if (channel == -1) {
+		pr_debug("Wrong channel ID\n");
+		return PMIC_PARAMETER_ERROR;
+	}
+	mc13892_adc_init_param(&adc_param);
+	pr_debug("pmic_adc_convert_8x\n");
+	adc_param.read_ts = false;
+	adc_param.single_channel = true;
+	adc_param.read_mode = mc13892_set_read_mode(channel);
+
+	if (channel <= 7) {
+		adc_param.channel_0 = channel;
+		adc_param.channel_1 = channel;
+	} else
+		return PMIC_PARAMETER_ERROR;
+
+	ret = mc13892_adc_convert(&adc_param);
+	for (i = 0; i <= 7; i++)
+		result[i] = adc_param.value[i];
+
+	return ret;
+}
+
+PMIC_STATUS pmic_adc_set_touch_mode(t_touch_mode touch_mode)
+{
+	if (suspend_flag == 1)
+		return -EBUSY;
+
+	CHECK_ERROR(pmic_write_reg(REG_ADC0,
+				   BITFVAL(MC13892_ADC0_TS_M, touch_mode),
+				   BITFMASK(MC13892_ADC0_TS_M)));
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS pmic_adc_get_touch_mode(t_touch_mode * touch_mode)
+{
+	unsigned int value;
+	if (suspend_flag == 1)
+		return -EBUSY;
+
+	CHECK_ERROR(pmic_read_reg(REG_ADC0, &value, PMIC_ALL_BITS));
+
+	*touch_mode = BITFEXT(value, MC13892_ADC0_TS_M);
+
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS pmic_adc_get_touch_sample(t_touch_screen *touch_sample, int wait)
+{
+	if (mc13892_adc_read_ts(touch_sample, wait) != 0)
+		return PMIC_ERROR;
+	if (0 == pmic_adc_filter(touch_sample))
+		return PMIC_SUCCESS;
+	else
+		return PMIC_ERROR;
+}
+
+PMIC_STATUS mc13892_adc_read_ts(t_touch_screen *ts_value, int wait_tsi)
+{
+	t_adc_param param;
+	pr_debug("mc13892_adc : mc13892_adc_read_ts\n");
+	if (suspend_flag == 1)
+		return -EBUSY;
+
+	if (wait_ts) {
+		pr_debug("mc13892_adc : error TS busy \n");
+		return PMIC_ERROR;
+	}
+	mc13892_adc_init_param(&param);
+	param.wait_tsi = wait_tsi;
+	param.read_ts = true;
+	if (mc13892_adc_convert(&param) != 0)
+		return PMIC_ERROR;
+	/* check if x-y is ok */
+	if (param.ts_value.contact_resistance < 1000) {
+		ts_value->x_position = param.ts_value.x_position;
+		ts_value->x_position1 = param.ts_value.x_position1;
+		ts_value->x_position2 = param.ts_value.x_position2;
+
+		ts_value->y_position = param.ts_value.y_position;
+		ts_value->y_position1 = param.ts_value.y_position1;
+		ts_value->y_position2 = param.ts_value.y_position2;
+
+		ts_value->contact_resistance =
+		    param.ts_value.contact_resistance + 1;
+
+	} else {
+		ts_value->x_position = 0;
+		ts_value->y_position = 0;
+		ts_value->contact_resistance = 0;
+
+	}
+	return PMIC_SUCCESS;
+}
+
+int mc13892_adc_request(bool read_ts)
+{
+	int adc_index = -1;
+	if (read_ts != 0) {
+		/*for ts we use bis=0 */
+		if (adc_dev[0] == ADC_USED)
+			return -1;
+		/*no wait here */
+		adc_dev[0] = ADC_USED;
+		adc_index = 0;
+	} else {
+		/*for other adc use bis = 1 */
+		if (adc_dev[1] == ADC_USED) {
+			return -1;
+			/*no wait here */
+		}
+		adc_dev[1] = ADC_USED;
+		adc_index = 1;
+	}
+	pr_debug("mc13892_adc : request ADC %d\n", adc_index);
+	return adc_index;
+}
+
+int mc13892_adc_release(int adc_index)
+{
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0)))
+			return -ERESTARTSYS;
+	}
+
+	pr_debug("mc13892_adc : release ADC %d\n", adc_index);
+	if ((adc_dev[adc_index] == ADC_MONITORING) ||
+	    (adc_dev[adc_index] == ADC_USED)) {
+		adc_dev[adc_index] = ADC_FREE;
+		wake_up(&queue_adc_busy);
+		return 0;
+	}
+	return -1;
+}
+
+#ifdef DEBUG
+static t_adc_param adc_param_db;
+
+static ssize_t adc_info(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	int *value = adc_param_db.value;
+
+	pr_debug("adc_info\n");
+
+	pr_debug("ch0\t\t%d\n", adc_param_db.channel_0);
+	pr_debug("ch1\t\t%d\n", adc_param_db.channel_1);
+	pr_debug("d5\t\t%d\n", adc_param_db.chrgraw_devide_5);
+	pr_debug("conv delay\t%d\n", adc_param_db.conv_delay);
+	pr_debug("delay\t\t%d\n", adc_param_db.delay);
+	pr_debug("read mode\t%d\n", adc_param_db.read_mode);
+	pr_debug("read ts\t\t%d\n", adc_param_db.read_ts);
+	pr_debug("single ch\t%d\n", adc_param_db.single_channel);
+	pr_debug("wait ts int\t%d\n", adc_param_db.wait_tsi);
+	pr_debug("value0-3:\t%d\t%d\t%d\t%d\n", value[0], value[1],
+		 value[2], value[3]);
+	pr_debug("value4-7:\t%d\t%d\t%d\t%d\n", value[4], value[5],
+		 value[6], value[7]);
+
+	return 0;
+}
+
+enum {
+	ADC_SET_CH0 = 0,
+	ADC_SET_CH1,
+	ADC_SET_DV5,
+	ADC_SET_CON_DELAY,
+	ADC_SET_DELAY,
+	ADC_SET_RM,
+	ADC_SET_RT,
+	ADC_SET_S_CH,
+	ADC_SET_WAIT_TS,
+	ADC_INIT_P,
+	ADC_START,
+	ADC_TS,
+	ADC_TS_READ,
+	ADC_TS_CAL,
+	ADC_CMD_MAX
+};
+
+static const char *const adc_cmd[ADC_CMD_MAX] = {
+	[ADC_SET_CH0] = "ch0",
+	[ADC_SET_CH1] = "ch1",
+	[ADC_SET_DV5] = "dv5",
+	[ADC_SET_CON_DELAY] = "cd",
+	[ADC_SET_DELAY] = "dl",
+	[ADC_SET_RM] = "rm",
+	[ADC_SET_RT] = "rt",
+	[ADC_SET_S_CH] = "sch",
+	[ADC_SET_WAIT_TS] = "wt",
+	[ADC_INIT_P] = "init",
+	[ADC_START] = "start",
+	[ADC_TS] = "touch",
+	[ADC_TS_READ] = "touchr",
+	[ADC_TS_CAL] = "cal"
+};
+
+static int cmd(unsigned int index, int value)
+{
+	t_touch_screen ts;
+
+	switch (index) {
+	case ADC_SET_CH0:
+		adc_param_db.channel_0 = value;
+		break;
+	case ADC_SET_CH1:
+		adc_param_db.channel_1 = value;
+		break;
+	case ADC_SET_DV5:
+		adc_param_db.chrgraw_devide_5 = value;
+		break;
+	case ADC_SET_CON_DELAY:
+		adc_param_db.conv_delay = value;
+		break;
+	case ADC_SET_RM:
+		adc_param_db.read_mode = value;
+		break;
+	case ADC_SET_RT:
+		adc_param_db.read_ts = value;
+		break;
+	case ADC_SET_S_CH:
+		adc_param_db.single_channel = value;
+		break;
+	case ADC_SET_WAIT_TS:
+		adc_param_db.wait_tsi = value;
+		break;
+	case ADC_INIT_P:
+		mc13892_adc_init_param(&adc_param_db);
+		break;
+	case ADC_START:
+		mc13892_adc_convert(&adc_param_db);
+		break;
+	case ADC_TS:
+		pmic_adc_get_touch_sample(&ts, 1);
+		pr_debug("x = %d\n", ts.x_position);
+		pr_debug("y = %d\n", ts.y_position);
+		pr_debug("p = %d\n", ts.contact_resistance);
+		break;
+	case ADC_TS_READ:
+		pmic_adc_get_touch_sample(&ts, 0);
+		pr_debug("x = %d\n", ts.x_position);
+		pr_debug("y = %d\n", ts.y_position);
+		pr_debug("p = %d\n", ts.contact_resistance);
+		break;
+	case ADC_TS_CAL:
+		break;
+	default:
+		pr_debug("error command\n");
+		break;
+	}
+	return 0;
+}
+
+static ssize_t adc_ctl(struct device *dev, struct device_attribute *attr,
+		       const char *buf, size_t count)
+{
+	int state = 0;
+	const char *const *s;
+	char *p, *q;
+	int error;
+	int len, value = 0;
+
+	pr_debug("adc_ctl\n");
+
+	q = NULL;
+	q = memchr(buf, ' ', count);
+
+	if (q != NULL) {
+		len = q - buf;
+		q += 1;
+		value = simple_strtoul(q, NULL, 10);
+	} else {
+		p = memchr(buf, '\n', count);
+		len = p ? p - buf : count;
+	}
+
+	for (s = &adc_cmd[state]; state < ADC_CMD_MAX; s++, state++) {
+		if (*s && !strncmp(buf, *s, len))
+			break;
+	}
+	if (state < ADC_CMD_MAX && *s)
+		error = cmd(state, value);
+	else
+		error = -EINVAL;
+
+	return count;
+}
+
+#else
+static ssize_t adc_info(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	return 0;
+}
+
+static ssize_t adc_ctl(struct device *dev, struct device_attribute *attr,
+		       const char *buf, size_t count)
+{
+	return count;
+}
+
+#endif
+
+static DEVICE_ATTR(adc, 0644, adc_info, adc_ctl);
+
+static int pmic_adc_module_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+
+	pr_debug("PMIC ADC start probe\n");
+	ret = device_create_file(&(pdev->dev), &dev_attr_adc);
+	if (ret) {
+		pr_debug("Can't create device file!\n");
+		return -ENODEV;
+	}
+
+	init_waitqueue_head(&suspendq);
+
+	ret = pmic_adc_init();
+	if (ret != PMIC_SUCCESS) {
+		pr_debug("Error in pmic_adc_init.\n");
+		goto rm_dev_file;
+	}
+
+	pmic_adc_ready = 1;
+	pr_debug("PMIC ADC successfully probed\n");
+	return 0;
+
+      rm_dev_file:
+	device_remove_file(&(pdev->dev), &dev_attr_adc);
+	return ret;
+}
+
+static int pmic_adc_module_remove(struct platform_device *pdev)
+{
+	pmic_adc_deinit();
+	pmic_adc_ready = 0;
+	pr_debug("PMIC ADC successfully removed\n");
+	return 0;
+}
+
+static struct platform_driver pmic_adc_driver_ldm = {
+	.driver = {
+		   .name = "pmic_adc",
+		   },
+	.suspend = pmic_adc_suspend,
+	.resume = pmic_adc_resume,
+	.probe = pmic_adc_module_probe,
+	.remove = pmic_adc_module_remove,
+};
+
+static int __init pmic_adc_module_init(void)
+{
+	pr_debug("PMIC ADC driver loading...\n");
+	return platform_driver_register(&pmic_adc_driver_ldm);
+}
+
+static void __exit pmic_adc_module_exit(void)
+{
+	platform_driver_unregister(&pmic_adc_driver_ldm);
+	pr_debug("PMIC ADC driver successfully unloaded\n");
+}
+
+module_init(pmic_adc_module_init);
+module_exit(pmic_adc_module_exit);
+
+MODULE_DESCRIPTION("PMIC ADC device driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/mc13892/pmic_battery.c b/drivers/mxc/pmic/mc13892/pmic_battery.c
new file mode 100644
index 000000000000..f2337cda51db
--- /dev/null
+++ b/drivers/mxc/pmic/mc13892/pmic_battery.c
@@ -0,0 +1,634 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * Includes
+ */
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+
+#include <linux/delay.h>
+#include <asm/mach-types.h>
+#include <linux/pmic_battery.h>
+#include <linux/pmic_adc.h>
+#include <linux/pmic_status.h>
+
+#define BIT_CHG_VOL_LSH		0
+#define BIT_CHG_VOL_WID		3
+
+#define BIT_CHG_CURR_LSH		3
+#define BIT_CHG_CURR_WID		4
+
+#define BIT_CHG_PLIM_LSH		15
+#define BIT_CHG_PLIM_WID		2
+
+#define BIT_CHG_DETS_LSH 6
+#define BIT_CHG_DETS_WID 1
+#define BIT_CHG_CURRS_LSH 11
+#define BIT_CHG_CURRS_WID 1
+
+#define TRICKLE_CHG_EN_LSH	7
+#define	LOW_POWER_BOOT_ACK_LSH	8
+#define BAT_TH_CHECK_DIS_LSH	9
+#define	BATTFET_CTL_EN_LSH	10
+#define BATTFET_CTL_LSH		11
+#define	REV_MOD_EN_LSH		13
+#define PLIM_DIS_LSH		17
+#define	CHG_LED_EN_LSH		18
+#define RESTART_CHG_STAT_LSH	20
+#define	AUTO_CHG_DIS_LSH	21
+#define CYCLING_DIS_LSH		22
+#define	VI_PROGRAM_EN_LSH	23
+
+#define TRICKLE_CHG_EN_WID	1
+#define	LOW_POWER_BOOT_ACK_WID	1
+#define BAT_TH_CHECK_DIS_WID	1
+#define	BATTFET_CTL_EN_WID	1
+#define BATTFET_CTL_WID		1
+#define	REV_MOD_EN_WID		1
+#define PLIM_DIS_WID		1
+#define	CHG_LED_EN_WID		1
+#define RESTART_CHG_STAT_WID	1
+#define	AUTO_CHG_DIS_WID	1
+#define CYCLING_DIS_WID		1
+#define	VI_PROGRAM_EN_WID	1
+
+#define ACC_STARTCC_LSH		0
+#define ACC_STARTCC_WID		1
+#define ACC_RSTCC_LSH		1
+#define ACC_RSTCC_WID		1
+#define ACC_CCFAULT_LSH		7
+#define ACC_CCFAULT_WID		7
+#define ACC_CCOUT_LSH		8
+#define ACC_CCOUT_WID		16
+#define ACC1_ONEC_LSH		0
+#define ACC1_ONEC_WID		15
+
+#define ACC_CALIBRATION 0x17
+#define ACC_START_COUNTER 0x07
+#define ACC_STOP_COUNTER 0x2
+#define ACC_CONTROL_BIT_MASK 0x1f
+#define ACC_ONEC_VALUE 2621
+#define ACC_COULOMB_PER_LSB 1
+#define ACC_CALIBRATION_DURATION_MSECS 20
+
+#define BAT_VOLTAGE_UNIT_UV 4692
+#define BAT_CURRENT_UNIT_UA 5870
+#define CHG_VOLTAGE_UINT_UV 23474
+#define CHG_MIN_CURRENT_UA 3500
+
+#define COULOMB_TO_UAH(c) (10000 * c / 36)
+
+enum chg_setting {
+       TRICKLE_CHG_EN,
+       LOW_POWER_BOOT_ACK,
+       BAT_TH_CHECK_DIS,
+       BATTFET_CTL_EN,
+       BATTFET_CTL,
+       REV_MOD_EN,
+       PLIM_DIS,
+       CHG_LED_EN,
+       RESTART_CHG_STAT,
+       AUTO_CHG_DIS,
+       CYCLING_DIS,
+       VI_PROGRAM_EN
+};
+
+static int pmic_set_chg_current(unsigned short curr)
+{
+	unsigned int mask;
+	unsigned int value;
+
+	value = BITFVAL(BIT_CHG_CURR, curr);
+	mask = BITFMASK(BIT_CHG_CURR);
+	CHECK_ERROR(pmic_write_reg(REG_CHARGE, value, mask));
+
+	return 0;
+}
+
+static int pmic_set_chg_misc(enum chg_setting type, unsigned short flag)
+{
+
+	unsigned int reg_value = 0;
+	unsigned int mask = 0;
+
+	switch (type) {
+	case TRICKLE_CHG_EN:
+		reg_value = BITFVAL(TRICKLE_CHG_EN, flag);
+		mask = BITFMASK(TRICKLE_CHG_EN);
+		break;
+	case LOW_POWER_BOOT_ACK:
+		reg_value = BITFVAL(LOW_POWER_BOOT_ACK, flag);
+		mask = BITFMASK(LOW_POWER_BOOT_ACK);
+		break;
+	case BAT_TH_CHECK_DIS:
+		reg_value = BITFVAL(BAT_TH_CHECK_DIS, flag);
+		mask = BITFMASK(BAT_TH_CHECK_DIS);
+		break;
+	case BATTFET_CTL_EN:
+		reg_value = BITFVAL(BATTFET_CTL_EN, flag);
+		mask = BITFMASK(BATTFET_CTL_EN);
+		break;
+	case BATTFET_CTL:
+		reg_value = BITFVAL(BATTFET_CTL, flag);
+		mask = BITFMASK(BATTFET_CTL);
+		break;
+	case REV_MOD_EN:
+		reg_value = BITFVAL(REV_MOD_EN, flag);
+		mask = BITFMASK(REV_MOD_EN);
+		break;
+	case PLIM_DIS:
+		reg_value = BITFVAL(PLIM_DIS, flag);
+		mask = BITFMASK(PLIM_DIS);
+		break;
+	case CHG_LED_EN:
+		reg_value = BITFVAL(CHG_LED_EN, flag);
+		mask = BITFMASK(CHG_LED_EN);
+		break;
+	case RESTART_CHG_STAT:
+		reg_value = BITFVAL(RESTART_CHG_STAT, flag);
+		mask = BITFMASK(RESTART_CHG_STAT);
+		break;
+	case AUTO_CHG_DIS:
+		reg_value = BITFVAL(AUTO_CHG_DIS, flag);
+		mask = BITFMASK(AUTO_CHG_DIS);
+		break;
+	case CYCLING_DIS:
+		reg_value = BITFVAL(CYCLING_DIS, flag);
+		mask = BITFMASK(CYCLING_DIS);
+		break;
+	case VI_PROGRAM_EN:
+		reg_value = BITFVAL(VI_PROGRAM_EN, flag);
+		mask = BITFMASK(VI_PROGRAM_EN);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_write_reg(REG_CHARGE, reg_value, mask));
+
+	return 0;
+}
+
+static int pmic_get_batt_voltage(unsigned short *voltage)
+{
+	t_channel channel;
+	unsigned short result[8];
+
+	channel = BATTERY_VOLTAGE;
+	CHECK_ERROR(pmic_adc_convert(channel, result));
+	*voltage = result[0];
+
+	return 0;
+}
+
+static int pmic_get_batt_current(unsigned short *curr)
+{
+	t_channel channel;
+	unsigned short result[8];
+
+	channel = BATTERY_CURRENT;
+	CHECK_ERROR(pmic_adc_convert(channel, result));
+	*curr = result[0];
+
+	return 0;
+}
+
+static int coulomb_counter_calibration;
+static unsigned int coulomb_counter_start_time_msecs;
+
+static int pmic_start_coulomb_counter(void)
+{
+	/* set scaler */
+	CHECK_ERROR(pmic_write_reg(REG_ACC1,
+		ACC_COULOMB_PER_LSB * ACC_ONEC_VALUE, BITFMASK(ACC1_ONEC)));
+
+	CHECK_ERROR(pmic_write_reg(
+		REG_ACC0, ACC_START_COUNTER, ACC_CONTROL_BIT_MASK));
+	coulomb_counter_start_time_msecs = jiffies_to_msecs(jiffies);
+	pr_debug("coulomb counter start time %u\n",
+		coulomb_counter_start_time_msecs);
+	return 0;
+}
+
+static int pmic_stop_coulomb_counter(void)
+{
+	CHECK_ERROR(pmic_write_reg(
+		REG_ACC0, ACC_STOP_COUNTER, ACC_CONTROL_BIT_MASK));
+	return 0;
+}
+
+static int pmic_calibrate_coulomb_counter(void)
+{
+	int ret;
+	unsigned int value;
+
+	/* set scaler */
+	CHECK_ERROR(pmic_write_reg(REG_ACC1,
+		0x1, BITFMASK(ACC1_ONEC)));
+
+	CHECK_ERROR(pmic_write_reg(
+		REG_ACC0, ACC_CALIBRATION, ACC_CONTROL_BIT_MASK));
+	msleep(ACC_CALIBRATION_DURATION_MSECS);
+
+	ret = pmic_read_reg(REG_ACC0, &value, BITFMASK(ACC_CCOUT));
+	if (ret != 0)
+		return -1;
+	value = BITFEXT(value, ACC_CCOUT);
+	pr_debug("calibrate value = %x\n", value);
+	coulomb_counter_calibration = (int)((s16)((u16) value));
+	pr_debug("coulomb_counter_calibration = %d\n",
+		coulomb_counter_calibration);
+
+	return 0;
+
+}
+
+static int pmic_get_charger_coulomb(int *coulomb)
+{
+	int ret;
+	unsigned int value;
+	int calibration;
+	unsigned int time_diff_msec;
+
+	ret = pmic_read_reg(REG_ACC0, &value, BITFMASK(ACC_CCOUT));
+	if (ret != 0)
+		return -1;
+	value = BITFEXT(value, ACC_CCOUT);
+	pr_debug("counter value = %x\n", value);
+	*coulomb = ((s16)((u16)value)) * ACC_COULOMB_PER_LSB;
+
+	if (abs(*coulomb) >= ACC_COULOMB_PER_LSB) {
+			/* calibrate */
+		time_diff_msec = jiffies_to_msecs(jiffies);
+		time_diff_msec =
+			(time_diff_msec > coulomb_counter_start_time_msecs) ?
+			(time_diff_msec - coulomb_counter_start_time_msecs) :
+			(0xffffffff - coulomb_counter_start_time_msecs
+			+ time_diff_msec);
+		calibration = coulomb_counter_calibration * (int)time_diff_msec
+			/ (ACC_ONEC_VALUE * ACC_CALIBRATION_DURATION_MSECS);
+		*coulomb -= calibration;
+	}
+
+	return 0;
+}
+
+static int pmic_restart_charging(void)
+{
+	pmic_set_chg_misc(BAT_TH_CHECK_DIS, 1);
+	pmic_set_chg_misc(AUTO_CHG_DIS, 0);
+	pmic_set_chg_misc(VI_PROGRAM_EN, 1);
+	pmic_set_chg_current(0x8);
+	pmic_set_chg_misc(RESTART_CHG_STAT, 1);
+	return 0;
+}
+
+struct mc13892_dev_info {
+	struct device *dev;
+
+	unsigned short voltage_raw;
+	int voltage_uV;
+	unsigned short current_raw;
+	int current_uA;
+	int battery_status;
+	int full_counter;
+	int charger_online;
+	int charger_voltage_uV;
+	int accum_current_uAh;
+
+	struct power_supply bat;
+	struct power_supply charger;
+
+	struct workqueue_struct *monitor_wqueue;
+	struct delayed_work monitor_work;
+};
+
+#define mc13892_SENSER	25
+#define to_mc13892_dev_info(x) container_of((x), struct mc13892_dev_info, \
+					      bat);
+
+static enum power_supply_property mc13892_battery_props[] = {
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_STATUS,
+};
+
+static enum power_supply_property mc13892_charger_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+};
+
+static int mc13892_charger_update_status(struct mc13892_dev_info *di)
+{
+	int ret;
+	unsigned int value;
+	int online;
+
+	ret = pmic_read_reg(REG_INT_SENSE0, &value, BITFMASK(BIT_CHG_DETS));
+
+	if (ret == 0) {
+		online = BITFEXT(value, BIT_CHG_DETS);
+		if (online != di->charger_online) {
+			di->charger_online = online;
+			dev_info(di->charger.dev, "charger status: %s\n",
+				online ? "online" : "offline");
+			power_supply_changed(&di->charger);
+
+			cancel_delayed_work(&di->monitor_work);
+			queue_delayed_work(di->monitor_wqueue,
+				&di->monitor_work, HZ / 10);
+			if (online) {
+				pmic_start_coulomb_counter();
+				pmic_restart_charging();
+			} else
+				pmic_stop_coulomb_counter();
+		}
+	}
+
+	return ret;
+}
+
+static int mc13892_charger_get_property(struct power_supply *psy,
+				       enum power_supply_property psp,
+				       union power_supply_propval *val)
+{
+	struct mc13892_dev_info *di =
+		container_of((psy), struct mc13892_dev_info, charger);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = di->charger_online;
+		return 0;
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
+static int mc13892_battery_read_status(struct mc13892_dev_info *di)
+{
+	int retval;
+	int coulomb;
+	retval = pmic_get_batt_voltage(&(di->voltage_raw));
+	if (retval == 0)
+		di->voltage_uV = di->voltage_raw * BAT_VOLTAGE_UNIT_UV;
+
+	retval = pmic_get_batt_current(&(di->current_raw));
+	if (retval == 0) {
+		if (di->current_raw & 0x200)
+			di->current_uA =
+				(0x1FF - (di->current_raw & 0x1FF)) *
+				BAT_CURRENT_UNIT_UA * (-1);
+		else
+			di->current_uA =
+				(di->current_raw & 0x1FF) * BAT_CURRENT_UNIT_UA;
+	}
+	retval = pmic_get_charger_coulomb(&coulomb);
+	if (retval == 0)
+		di->accum_current_uAh = COULOMB_TO_UAH(coulomb);
+
+	return retval;
+}
+
+static void mc13892_battery_update_status(struct mc13892_dev_info *di)
+{
+	unsigned int value;
+	int retval;
+	int old_battery_status = di->battery_status;
+
+	if (di->battery_status == POWER_SUPPLY_STATUS_UNKNOWN)
+		di->full_counter = 0;
+
+	if (di->charger_online) {
+		retval = pmic_read_reg(REG_INT_SENSE0,
+					&value, BITFMASK(BIT_CHG_CURRS));
+
+		if (retval == 0) {
+			value = BITFEXT(value, BIT_CHG_CURRS);
+			if (value)
+				di->battery_status =
+					POWER_SUPPLY_STATUS_CHARGING;
+			else
+				di->battery_status =
+					POWER_SUPPLY_STATUS_NOT_CHARGING;
+		}
+
+		if (di->battery_status == POWER_SUPPLY_STATUS_NOT_CHARGING)
+			di->full_counter++;
+		else
+			di->full_counter = 0;
+	} else {
+		di->battery_status = POWER_SUPPLY_STATUS_DISCHARGING;
+		di->full_counter = 0;
+	}
+
+	dev_dbg(di->bat.dev, "bat status: %d\n",
+		di->battery_status);
+
+	if (di->battery_status != old_battery_status)
+		power_supply_changed(&di->bat);
+}
+
+static void mc13892_battery_work(struct work_struct *work)
+{
+	struct mc13892_dev_info *di = container_of(work,
+						     struct mc13892_dev_info,
+						     monitor_work.work);
+	const int interval = HZ * 60;
+
+	dev_dbg(di->dev, "%s\n", __func__);
+
+	mc13892_battery_update_status(di);
+	queue_delayed_work(di->monitor_wqueue, &di->monitor_work, interval);
+}
+
+static void charger_online_event_callback(void *para)
+{
+	struct mc13892_dev_info *di = (struct mc13892_dev_info *) para;
+	pr_info("\n\n DETECTED charger plug/unplug event\n");
+	mc13892_charger_update_status(di);
+}
+
+
+static int mc13892_battery_get_property(struct power_supply *psy,
+				       enum power_supply_property psp,
+				       union power_supply_propval *val)
+{
+	struct mc13892_dev_info *di = to_mc13892_dev_info(psy);
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		if (di->battery_status == POWER_SUPPLY_STATUS_UNKNOWN) {
+			mc13892_charger_update_status(di);
+			mc13892_battery_update_status(di);
+		}
+		val->intval = di->battery_status;
+		return 0;
+	default:
+		break;
+	}
+
+	mc13892_battery_read_status(di);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		val->intval = di->voltage_uV;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		val->intval = di->current_uA;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		val->intval = di->accum_current_uAh;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
+		val->intval = 3800000;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
+		val->intval = 3300000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int pmic_battery_remove(struct platform_device *pdev)
+{
+	pmic_event_callback_t bat_event_callback;
+	struct mc13892_dev_info *di = platform_get_drvdata(pdev);
+
+	bat_event_callback.func = charger_online_event_callback;
+	bat_event_callback.param = (void *) di;
+	pmic_event_unsubscribe(EVENT_CHGDETI, bat_event_callback);
+
+	cancel_rearming_delayed_workqueue(di->monitor_wqueue,
+					  &di->monitor_work);
+	destroy_workqueue(di->monitor_wqueue);
+	power_supply_unregister(&di->bat);
+	power_supply_unregister(&di->charger);
+
+	kfree(di);
+
+	return 0;
+}
+
+static int pmic_battery_probe(struct platform_device *pdev)
+{
+	int retval = 0;
+	struct mc13892_dev_info *di;
+	pmic_event_callback_t bat_event_callback;
+	pmic_version_t pmic_version;
+
+	/* Only apply battery driver for MC13892 V2.0 due to ENGR108085 */
+	pmic_version = pmic_get_version();
+	if (pmic_version.revision < 20) {
+		pr_debug("Battery driver is only applied for MC13892 V2.0\n");
+		return -1;
+	}
+	if (machine_is_mx51_babbage()) {
+		pr_debug("mc13892 charger is not used for this platform\n");
+		return -1;
+	}
+
+	di = kzalloc(sizeof(*di), GFP_KERNEL);
+	if (!di) {
+		retval = -ENOMEM;
+		goto di_alloc_failed;
+	}
+
+	platform_set_drvdata(pdev, di);
+
+	di->dev	= &pdev->dev;
+	di->bat.name	= "mc13892_bat";
+	di->bat.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->bat.properties = mc13892_battery_props;
+	di->bat.num_properties = ARRAY_SIZE(mc13892_battery_props);
+	di->bat.get_property = mc13892_battery_get_property;
+	di->bat.use_for_apm = 1;
+
+	di->battery_status = POWER_SUPPLY_STATUS_UNKNOWN;
+
+	retval = power_supply_register(&pdev->dev, &di->bat);
+	if (retval) {
+		dev_err(di->dev, "failed to register battery\n");
+		goto batt_failed;
+	}
+	di->charger.name	= "mc13892_charger";
+	di->charger.type = POWER_SUPPLY_TYPE_MAINS;
+	di->charger.properties = mc13892_charger_props;
+	di->charger.num_properties = ARRAY_SIZE(mc13892_charger_props);
+	di->charger.get_property = mc13892_charger_get_property;
+	retval = power_supply_register(&pdev->dev, &di->charger);
+	if (retval) {
+		dev_err(di->dev, "failed to register charger\n");
+		goto charger_failed;
+	}
+	INIT_DELAYED_WORK(&di->monitor_work, mc13892_battery_work);
+	di->monitor_wqueue = create_singlethread_workqueue(pdev->dev.bus_id);
+	if (!di->monitor_wqueue) {
+		retval = -ESRCH;
+		goto workqueue_failed;
+	}
+	queue_delayed_work(di->monitor_wqueue, &di->monitor_work, HZ * 10);
+
+	bat_event_callback.func = charger_online_event_callback;
+	bat_event_callback.param = (void *) di;
+	pmic_event_subscribe(EVENT_CHGDETI, bat_event_callback);
+
+	pmic_stop_coulomb_counter();
+	pmic_calibrate_coulomb_counter();
+	goto success;
+
+workqueue_failed:
+	power_supply_unregister(&di->charger);
+charger_failed:
+	power_supply_unregister(&di->bat);
+batt_failed:
+	kfree(di);
+di_alloc_failed:
+success:
+	dev_dbg(di->dev, "%s battery probed!\n", __func__);
+	return retval;
+
+
+	return 0;
+}
+
+static struct platform_driver pmic_battery_driver_ldm = {
+	.driver = {
+		   .name = "pmic_battery",
+		   .bus = &platform_bus_type,
+		   },
+	.probe = pmic_battery_probe,
+	.remove = pmic_battery_remove,
+};
+
+static int __init pmic_battery_init(void)
+{
+	pr_debug("PMIC Battery driver loading...\n");
+	return platform_driver_register(&pmic_battery_driver_ldm);
+}
+
+static void __exit pmic_battery_exit(void)
+{
+	platform_driver_unregister(&pmic_battery_driver_ldm);
+	pr_debug("PMIC Battery driver successfully unloaded\n");
+}
+
+module_init(pmic_battery_init);
+module_exit(pmic_battery_exit);
+
+MODULE_DESCRIPTION("pmic_battery driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/pmic/mc13892/pmic_light.c b/drivers/mxc/pmic/mc13892/pmic_light.c
new file mode 100644
index 000000000000..ae02430a1981
--- /dev/null
+++ b/drivers/mxc/pmic/mc13892/pmic_light.c
@@ -0,0 +1,685 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc13892/pmic_light.c
+ * @brief This is the main file of PMIC(mc13783) Light and Backlight driver.
+ *
+ * @ingroup PMIC_LIGHT
+ */
+
+/*
+ * Includes
+ */
+#define DEBUG
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/pmic_light.h>
+#include <linux/pmic_status.h>
+
+#define BIT_CL_MAIN_LSH		9
+#define BIT_CL_AUX_LSH		21
+#define BIT_CL_KEY_LSH		9
+#define BIT_CL_RED_LSH		9
+#define BIT_CL_GREEN_LSH	21
+#define BIT_CL_BLUE_LSH		9
+
+#define BIT_CL_MAIN_WID		3
+#define BIT_CL_AUX_WID		3
+#define BIT_CL_KEY_WID		3
+#define BIT_CL_RED_WID		3
+#define BIT_CL_GREEN_WID	3
+#define BIT_CL_BLUE_WID		3
+
+#define BIT_DC_MAIN_LSH		3
+#define BIT_DC_AUX_LSH		15
+#define BIT_DC_KEY_LSH		3
+#define BIT_DC_RED_LSH		3
+#define BIT_DC_GREEN_LSH	15
+#define BIT_DC_BLUE_LSH		3
+
+#define BIT_DC_MAIN_WID		6
+#define BIT_DC_AUX_WID		6
+#define BIT_DC_KEY_WID		6
+#define BIT_DC_RED_WID		6
+#define BIT_DC_GREEN_WID	6
+#define BIT_DC_BLUE_WID		6
+
+#define BIT_RP_MAIN_LSH		2
+#define BIT_RP_AUX_LSH		14
+#define BIT_RP_KEY_LSH		2
+#define BIT_RP_RED_LSH		2
+#define BIT_RP_GREEN_LSH	14
+#define BIT_RP_BLUE_LSH		2
+
+#define BIT_RP_MAIN_WID		1
+#define BIT_RP_AUX_WID		1
+#define BIT_RP_KEY_WID		1
+#define BIT_RP_RED_WID		1
+#define BIT_RP_GREEN_WID	1
+#define BIT_RP_BLUE_WID		1
+
+#define BIT_HC_MAIN_LSH		1
+#define BIT_HC_AUX_LSH		13
+#define BIT_HC_KEY_LSH		1
+
+#define BIT_HC_MAIN_WID		1
+#define BIT_HC_AUX_WID		1
+#define BIT_HC_KEY_WID		1
+
+#define BIT_BP_RED_LSH		0
+#define BIT_BP_GREEN_LSH	12
+#define BIT_BP_BLUE_LSH		0
+
+#define BIT_BP_RED_WID		2
+#define BIT_BP_GREEN_WID	2
+#define BIT_BP_BLUE_WID		2
+
+int pmic_light_init_reg(void)
+{
+	CHECK_ERROR(pmic_write_reg(REG_LED_CTL0, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_LED_CTL1, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_LED_CTL2, 0, PMIC_ALL_BITS));
+	CHECK_ERROR(pmic_write_reg(REG_LED_CTL3, 0, PMIC_ALL_BITS));
+
+	return 0;
+}
+
+static int pmic_light_suspend(struct platform_device *dev, pm_message_t state)
+{
+	return 0;
+};
+
+static int pmic_light_resume(struct platform_device *pdev)
+{
+	return 0;
+};
+
+PMIC_STATUS mc13892_bklit_set_hi_current(enum lit_channel channel, int mode)
+{
+	unsigned int mask;
+	unsigned int value;
+	int reg;
+
+	switch (channel) {
+	case LIT_MAIN:
+		value = BITFVAL(BIT_HC_MAIN, mode);
+		mask = BITFMASK(BIT_HC_MAIN);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_AUX:
+		value = BITFVAL(BIT_HC_AUX, mode);
+		mask = BITFMASK(BIT_HC_AUX);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_KEY:
+		value = BITFVAL(BIT_HC_KEY, mode);
+		mask = BITFMASK(BIT_HC_KEY);
+		reg = REG_LED_CTL1;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	CHECK_ERROR(pmic_write_reg(reg, value, mask));
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS mc13892_bklit_get_hi_current(enum lit_channel channel, int *mode)
+{
+	unsigned int mask;
+	int reg;
+
+	switch (channel) {
+	case LIT_MAIN:
+		mask = BITFMASK(BIT_HC_MAIN);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_AUX:
+		mask = BITFMASK(BIT_HC_AUX);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_KEY:
+		mask = BITFMASK(BIT_HC_KEY);
+		reg = REG_LED_CTL1;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, mode, mask));
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS mc13892_bklit_set_current(enum lit_channel channel,
+				      unsigned char level)
+{
+	unsigned int mask;
+	unsigned int value;
+	int reg;
+
+	if (level > LIT_CURR_HI_42)
+		return PMIC_PARAMETER_ERROR;
+	else if (level >= LIT_CURR_HI_0) {
+		CHECK_ERROR(mc13892_bklit_set_hi_current(channel, 1));
+		level -= LIT_CURR_HI_0;
+	}
+
+	switch (channel) {
+	case LIT_MAIN:
+		value = BITFVAL(BIT_CL_MAIN, level);
+		mask = BITFMASK(BIT_CL_MAIN);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_AUX:
+		value = BITFVAL(BIT_CL_AUX, level);
+		mask = BITFMASK(BIT_CL_AUX);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_KEY:
+		value = BITFVAL(BIT_CL_KEY, level);
+		mask = BITFMASK(BIT_CL_KEY);
+		reg = REG_LED_CTL1;
+		break;
+	case LIT_RED:
+		value = BITFVAL(BIT_CL_RED, level);
+		mask = BITFMASK(BIT_CL_RED);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_GREEN:
+		value = BITFVAL(BIT_CL_GREEN, level);
+		mask = BITFMASK(BIT_CL_GREEN);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_BLUE:
+		value = BITFVAL(BIT_CL_BLUE, level);
+		mask = BITFMASK(BIT_CL_BLUE);
+		reg = REG_LED_CTL3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	CHECK_ERROR(pmic_write_reg(reg, value, mask));
+
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS mc13892_bklit_get_current(enum lit_channel channel,
+				      unsigned char *level)
+{
+	unsigned int reg_value = 0;
+	unsigned int mask = 0;
+	int reg, mode;
+
+	CHECK_ERROR(mc13892_bklit_get_hi_current(channel, &mode));
+
+	switch (channel) {
+	case LIT_MAIN:
+		mask = BITFMASK(BIT_CL_MAIN);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_AUX:
+		mask = BITFMASK(BIT_CL_AUX);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_KEY:
+		mask = BITFMASK(BIT_CL_KEY);
+		reg = REG_LED_CTL1;
+		break;
+	case LIT_RED:
+		mask = BITFMASK(BIT_CL_RED);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_GREEN:
+		mask = BITFMASK(BIT_CL_GREEN);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_BLUE:
+		mask = BITFMASK(BIT_CL_BLUE);
+		reg = REG_LED_CTL3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &reg_value, mask));
+
+	switch (channel) {
+	case LIT_MAIN:
+		*level = BITFEXT(reg_value, BIT_CL_MAIN);
+		break;
+	case LIT_AUX:
+		*level = BITFEXT(reg_value, BIT_CL_AUX);
+		break;
+	case LIT_KEY:
+		*level = BITFEXT(reg_value, BIT_CL_KEY);
+		break;
+	case LIT_RED:
+		*level = BITFEXT(reg_value, BIT_CL_RED);
+		break;
+	case LIT_GREEN:
+		*level = BITFEXT(reg_value, BIT_CL_GREEN);
+		break;
+	case LIT_BLUE:
+		*level = BITFEXT(reg_value, BIT_CL_BLUE);
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	if (mode == 1)
+		*level += LIT_CURR_HI_0;
+
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS mc13892_bklit_set_dutycycle(enum lit_channel channel,
+					unsigned char dc)
+{
+	unsigned int mask;
+	unsigned int value;
+	int reg;
+
+	switch (channel) {
+	case LIT_MAIN:
+		value = BITFVAL(BIT_DC_MAIN, dc);
+		mask = BITFMASK(BIT_DC_MAIN);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_AUX:
+		value = BITFVAL(BIT_DC_AUX, dc);
+		mask = BITFMASK(BIT_DC_AUX);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_KEY:
+		value = BITFVAL(BIT_DC_KEY, dc);
+		mask = BITFMASK(BIT_DC_KEY);
+		reg = REG_LED_CTL1;
+		break;
+	case LIT_RED:
+		value = BITFVAL(BIT_DC_RED, dc);
+		mask = BITFMASK(BIT_DC_RED);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_GREEN:
+		value = BITFVAL(BIT_DC_GREEN, dc);
+		mask = BITFMASK(BIT_DC_GREEN);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_BLUE:
+		value = BITFVAL(BIT_DC_BLUE, dc);
+		mask = BITFMASK(BIT_DC_BLUE);
+		reg = REG_LED_CTL3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	CHECK_ERROR(pmic_write_reg(reg, value, mask));
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS mc13892_bklit_get_dutycycle(enum lit_channel channel,
+					unsigned char *dc)
+{
+	unsigned int mask;
+	int reg;
+	unsigned int reg_value = 0;
+
+	switch (channel) {
+	case LIT_MAIN:
+		mask = BITFMASK(BIT_DC_MAIN);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_AUX:
+		mask = BITFMASK(BIT_DC_AUX);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_KEY:
+		mask = BITFMASK(BIT_DC_KEY);
+		reg = REG_LED_CTL1;
+		break;
+	case LIT_RED:
+		mask = BITFMASK(BIT_DC_RED);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_GREEN:
+		mask = BITFMASK(BIT_DC_GREEN);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_BLUE:
+		mask = BITFMASK(BIT_DC_BLUE);
+		reg = REG_LED_CTL3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &reg_value, mask));
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS mc13892_bklit_set_ramp(enum lit_channel channel, int flag)
+{
+	unsigned int mask;
+	unsigned int value;
+	int reg;
+
+	switch (channel) {
+	case LIT_MAIN:
+		value = BITFVAL(BIT_RP_MAIN, flag);
+		mask = BITFMASK(BIT_RP_MAIN);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_AUX:
+		value = BITFVAL(BIT_RP_AUX, flag);
+		mask = BITFMASK(BIT_RP_AUX);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_KEY:
+		value = BITFVAL(BIT_RP_KEY, flag);
+		mask = BITFMASK(BIT_RP_KEY);
+		reg = REG_LED_CTL1;
+		break;
+	case LIT_RED:
+		value = BITFVAL(BIT_RP_RED, flag);
+		mask = BITFMASK(BIT_RP_RED);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_GREEN:
+		value = BITFVAL(BIT_RP_GREEN, flag);
+		mask = BITFMASK(BIT_RP_GREEN);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_BLUE:
+		value = BITFVAL(BIT_RP_BLUE, flag);
+		mask = BITFMASK(BIT_RP_BLUE);
+		reg = REG_LED_CTL3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	CHECK_ERROR(pmic_write_reg(reg, value, mask));
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS mc13892_bklit_get_ramp(enum lit_channel channel, int *flag)
+{
+	unsigned int mask;
+	int reg;
+
+	switch (channel) {
+	case LIT_MAIN:
+		mask = BITFMASK(BIT_RP_MAIN);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_AUX:
+		mask = BITFMASK(BIT_RP_AUX);
+		reg = REG_LED_CTL0;
+		break;
+	case LIT_KEY:
+		mask = BITFMASK(BIT_RP_KEY);
+		reg = REG_LED_CTL1;
+		break;
+	case LIT_RED:
+		mask = BITFMASK(BIT_RP_RED);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_GREEN:
+		mask = BITFMASK(BIT_RP_GREEN);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_BLUE:
+		mask = BITFMASK(BIT_RP_BLUE);
+		reg = REG_LED_CTL3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, flag, mask));
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS mc13892_bklit_set_blink_p(enum lit_channel channel, int period)
+{
+	unsigned int mask;
+	unsigned int value;
+	int reg;
+
+	switch (channel) {
+	case LIT_RED:
+		value = BITFVAL(BIT_BP_RED, period);
+		mask = BITFMASK(BIT_BP_RED);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_GREEN:
+		value = BITFVAL(BIT_BP_GREEN, period);
+		mask = BITFMASK(BIT_BP_GREEN);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_BLUE:
+		value = BITFVAL(BIT_BP_BLUE, period);
+		mask = BITFMASK(BIT_BP_BLUE);
+		reg = REG_LED_CTL3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+	CHECK_ERROR(pmic_write_reg(reg, value, mask));
+	return PMIC_SUCCESS;
+}
+
+PMIC_STATUS mc13892_bklit_get_blink_p(enum lit_channel channel, int *period)
+{
+	unsigned int mask;
+	int reg;
+
+	switch (channel) {
+	case LIT_RED:
+		mask = BITFMASK(BIT_BP_RED);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_GREEN:
+		mask = BITFMASK(BIT_BP_GREEN);
+		reg = REG_LED_CTL2;
+		break;
+	case LIT_BLUE:
+		mask = BITFMASK(BIT_BP_BLUE);
+		reg = REG_LED_CTL3;
+		break;
+	default:
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, period, mask));
+	return PMIC_SUCCESS;
+}
+
+EXPORT_SYMBOL(mc13892_bklit_set_current);
+EXPORT_SYMBOL(mc13892_bklit_get_current);
+EXPORT_SYMBOL(mc13892_bklit_set_dutycycle);
+EXPORT_SYMBOL(mc13892_bklit_get_dutycycle);
+EXPORT_SYMBOL(mc13892_bklit_set_ramp);
+EXPORT_SYMBOL(mc13892_bklit_get_ramp);
+EXPORT_SYMBOL(mc13892_bklit_set_blink_p);
+EXPORT_SYMBOL(mc13892_bklit_get_blink_p);
+
+static int pmic_light_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+#ifdef DEBUG
+static ssize_t lit_info(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	return 0;
+}
+
+enum {
+	SET_CURR = 0,
+	SET_DC,
+	SET_RAMP,
+	SET_BP,
+	SET_CH,
+	LIT_CMD_MAX
+};
+
+static const char *const lit_cmd[LIT_CMD_MAX] = {
+	[SET_CURR] = "cur",
+	[SET_DC] = "dc",
+	[SET_RAMP] = "ra",
+	[SET_BP] = "bp",
+	[SET_CH] = "ch"
+};
+
+static int cmd(unsigned int index, int value)
+{
+	static int ch = LIT_MAIN;
+	int ret = 0;
+
+	switch (index) {
+	case SET_CH:
+		ch = value;
+		break;
+	case SET_CURR:
+		pr_debug("set %d cur %d\n", ch, value);
+		ret = mc13892_bklit_set_current(ch, value);
+		break;
+	case SET_DC:
+		pr_debug("set %d dc %d\n", ch, value);
+		ret = mc13892_bklit_set_dutycycle(ch, value);
+		break;
+	case SET_RAMP:
+		pr_debug("set %d ramp %d\n", ch, value);
+		ret = mc13892_bklit_set_ramp(ch, value);
+		break;
+	case SET_BP:
+		pr_debug("set %d bp %d\n", ch, value);
+		ret = mc13892_bklit_set_blink_p(ch, value);
+		break;
+	default:
+		pr_debug("error command\n");
+		break;
+	}
+
+	if (ret == PMIC_SUCCESS)
+		pr_debug("command exec successfully!\n");
+
+	return 0;
+}
+
+static ssize_t lit_ctl(struct device *dev, struct device_attribute *attr,
+		       const char *buf, size_t count)
+{
+	int state = 0;
+	const char *const *s;
+	char *p, *q;
+	int error;
+	int len, value = 0;
+
+	pr_debug("lit_ctl\n");
+
+	q = NULL;
+	q = memchr(buf, ' ', count);
+
+	if (q != NULL) {
+		len = q - buf;
+		q += 1;
+		value = simple_strtoul(q, NULL, 10);
+	} else {
+		p = memchr(buf, '\n', count);
+		len = p ? p - buf : count;
+	}
+
+	for (s = &lit_cmd[state]; state < LIT_CMD_MAX; s++, state++) {
+		if (*s && !strncmp(buf, *s, len))
+			break;
+	}
+	if (state < LIT_CMD_MAX && *s)
+		error = cmd(state, value);
+	else
+		error = -EINVAL;
+
+	return count;
+}
+
+#else
+static ssize_t lit_info(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	return 0;
+}
+
+static ssize_t lit_ctl(struct device *dev, struct device_attribute *attr,
+		       const char *buf, size_t count)
+{
+	return count;
+}
+
+#endif
+
+static DEVICE_ATTR(lit, 0644, lit_info, lit_ctl);
+
+static int pmic_light_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+
+	pr_debug("PMIC ADC start probe\n");
+	ret = device_create_file(&(pdev->dev), &dev_attr_lit);
+	if (ret) {
+		pr_debug("Can't create device file!\n");
+		return -ENODEV;
+	}
+
+	pmic_light_init_reg();
+
+	pr_debug("PMIC Light successfully loaded\n");
+	return 0;
+}
+
+static struct platform_driver pmic_light_driver_ldm = {
+	.driver = {
+		   .name = "pmic_light",
+		   },
+	.suspend = pmic_light_suspend,
+	.resume = pmic_light_resume,
+	.probe = pmic_light_probe,
+	.remove = pmic_light_remove,
+};
+
+/*
+ * Initialization and Exit
+ */
+
+static int __init pmic_light_init(void)
+{
+	pr_debug("PMIC Light driver loading...\n");
+	return platform_driver_register(&pmic_light_driver_ldm);
+}
+static void __exit pmic_light_exit(void)
+{
+	platform_driver_unregister(&pmic_light_driver_ldm);
+	pr_debug("PMIC Light driver successfully unloaded\n");
+}
+
+/*
+ * Module entry points
+ */
+
+subsys_initcall(pmic_light_init);
+module_exit(pmic_light_exit);
+
+MODULE_DESCRIPTION("PMIC_LIGHT");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/security/Kconfig b/drivers/mxc/security/Kconfig
new file mode 100644
index 000000000000..0e5e3fe2ed7a
--- /dev/null
+++ b/drivers/mxc/security/Kconfig
@@ -0,0 +1,64 @@
+menu "MXC Security Drivers"
+
+config MXC_SECURITY_SCC
+	tristate "MXC SCC Driver"
+	default n
+	---help---
+	  This module contains the core API's for accessing the SCC module.
+	  If you are unsure about this, say N here.
+
+config MXC_SECURITY_SCC2
+        tristate "MXC SCC2 Driver"
+        depends on ARCH_MX37 || ARCH_MX51
+        default n
+        ---help---
+          This module contains the core API's for accessing the SCC2 module.
+          If you are unsure about this, say N here.
+
+config SCC_DEBUG
+        bool "MXC SCC Module debugging"
+        depends on MXC_SECURITY_SCC || MXC_SECURITY_SCC2
+        ---help---
+        This is an option for use by developers; most people should
+        say N here.  This enables SCC module debugging.
+
+config MXC_SECURITY_RNG
+	tristate "MXC RNG Driver"
+	depends on ARCH_MXC
+	depends on !ARCH_MXC91321
+	depends on !ARCH_MX27
+	default n
+	select MXC_SECURITY_CORE
+	---help---
+	  This module contains the core API's for accessing the RNG module.
+	  If you are unsure about this, say N here.
+
+config MXC_RNG_TEST_DRIVER
+	bool "MXC RNG debug register"
+	depends on MXC_SECURITY_RNG
+	default n
+	---help---
+	  This option enables the RNG kcore driver to provide peek-poke facility
+	  into the RNG device registers. Enable this, only for development and
+	  testing purposes.
+config MXC_RNG_DEBUG
+        bool "MXC RNG Module Dubugging"
+        depends on MXC_SECURITY_RNG
+        default n
+        ---help---
+         This is an option for use by developers; most people should
+         say N here. This enables RNG module debugging.
+ 
+config MXC_DRYICE
+        tristate "MXC DryIce Driver"
+        depends on ARCH_MX25
+        default n
+        ---help---
+          This module contains the core API's for accessing the DryIce module.
+          If you are unsure about this, say N here.
+
+if (ARCH_MX37 || ARCH_MX51 || ARCH_MX27)
+source "drivers/mxc/security/sahara2/Kconfig"
+endif
+
+endmenu
diff --git a/drivers/mxc/security/Makefile b/drivers/mxc/security/Makefile
new file mode 100644
index 000000000000..f643a09a423d
--- /dev/null
+++ b/drivers/mxc/security/Makefile
@@ -0,0 +1,11 @@
+# Makefile for the Linux MXC Security API
+ifeq ($( SCC_DEBUG),y)
+EXTRA_CFLAGS += -DDEBUG
+endif
+EXTRA_CFLAGS += -DMXC -DLINUX_KERNEL
+
+obj-$(CONFIG_MXC_SECURITY_SCC2) += scc2_driver.o
+obj-$(CONFIG_MXC_SECURITY_SCC) += mxc_scc.o
+obj-$(CONFIG_MXC_SECURITY_RNG) += rng/
+obj-$(CONFIG_MXC_SAHARA) += sahara2/
+obj-$(CONFIG_MXC_DRYICE) += dryice.o
diff --git a/drivers/mxc/security/dryice-regs.h b/drivers/mxc/security/dryice-regs.h
new file mode 100644
index 000000000000..b8d3858bdb7d
--- /dev/null
+++ b/drivers/mxc/security/dryice-regs.h
@@ -0,0 +1,207 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+
+#ifndef __DRYICE_REGS_H__
+#define __DRYICE_REGS_H__
+
+/***********************************************************************
+ * DryIce Register Definitions
+ ***********************************************************************/
+
+/* DryIce Time Counter MSB Reg */
+#define DTCMR   0x00
+
+/* DryIce Time Counter LSB Reg */
+#define DTCLR   0x04
+
+/* DryIce Clock Alarm MSB Reg */
+#define DCAMR   0x08
+
+/* DryIce Clock Alarm LSB Reg */
+#define DCALR   0x0c
+
+/* DryIce Control Reg */
+#define DCR     0x10
+#define DCR_TDCHL   (1 << 30)  /* Tamper Detect Config Hard Lock */
+#define DCR_TDCSL   (1 << 29)  /* Tamper Detect COnfig Soft Lock */
+#define DCR_KSHL    (1 << 28)  /* Key Select Hard Lock */
+#define DCR_KSSL    (1 << 27)  /* Key Select Soft Lock */
+#define DCR_RKHL    (1 << 26)  /* Random Key Hard Lock */
+#define DCR_RKSL    (1 << 25)  /* Random Key Soft Lock */
+#define DCR_PKRHL   (1 << 24)  /* Programmed Key Read Hard Lock */
+#define DCR_PKRSL   (1 << 23)  /* Programmed Key Read Soft Lock */
+#define DCR_PKWHL   (1 << 22)  /* Programmed Key Write Hard Lock */
+#define DCR_PKWSL   (1 << 21)  /* Programmed Key Write Soft Lock */
+#define DCR_MCHL    (1 << 20)  /* Monotonic Counter Hard Lock */
+#define DCR_MCSL    (1 << 19)  /* Monotonic Counter Soft Lock */
+#define DCR_TCHL    (1 << 18)  /* Time Counter Hard Lock */
+#define DCR_TCSL    (1 << 17)  /* Time Counter Soft Lock */
+#define DCR_FSHL    (1 << 16)  /* Failure State Hard Lock */
+#define DCR_NSA     (1 << 15)  /* Non-Secure Access */
+#define DCR_OSCB    (1 << 14)  /* Oscillator Bypass */
+#define DCR_APE     (1 << 4)   /* Alarm Pin Enable */
+#define DCR_TCE     (1 << 3)   /* Time Counter Enable */
+#define DCR_MCE     (1 << 2)   /* Monotonic Counter Enable */
+#define DCR_SWR     (1 << 0)   /* Software Reset (w/o) */
+
+/* DryIce Status Reg */
+#define DSR     0x14
+#define DSR_WTD     (1 << 23)  /* Wire-mesh Tampering Detected */
+#define DSR_ETBD    (1 << 22)  /* External Tampering B Detected */
+#define DSR_ETAD    (1 << 21)  /* External Tampering A Detected */
+#define DSR_EBD     (1 << 20)  /* External Boot Detected */
+#define DSR_SAD     (1 << 19)  /* Security Alarm Detected */
+#define DSR_TTD     (1 << 18)  /* Temperature Tampering Detected */
+#define DSR_CTD     (1 << 17)  /* Clock Tampering Detected */
+#define DSR_VTD     (1 << 16)  /* Voltage Tampering Detected */
+#define DSR_KBF     (1 << 11)  /* Key Busy Flag */
+#define DSR_WBF     (1 << 10)  /* Write Busy Flag */
+#define DSR_WNF     (1 << 9)   /* Write Next Flag */
+#define DSR_WCF     (1 << 8)   /* Write Complete Flag */
+#define DSR_WEF     (1 << 7)   /* Write Error Flag */
+#define DSR_RKE     (1 << 6)   /* Random Key Error */
+#define DSR_RKV     (1 << 5)   /* Random Key Valid */
+#define DSR_CAF     (1 << 4)   /* Clock Alarm Flag */
+#define DSR_MCO     (1 << 3)   /* Monotonic Counter Overflow */
+#define DSR_TCO     (1 << 2)   /* Time Counter Overflow */
+#define DSR_NVF     (1 << 1)   /* Non-Valid Flag */
+#define DSR_SVF     (1 << 0)   /* Security Violation Flag */
+
+#define DSR_TAMPER_BITS (DSR_WTD | DSR_ETBD | DSR_ETAD | DSR_EBD | DSR_SAD | \
+			 DSR_TTD | DSR_CTD | DSR_VTD | DSR_MCO | DSR_TCO)
+
+/* ensure that external tamper defs match register bits */
+#if DSR_WTD != DI_TAMPER_EVENT_WTD
+#error "Mismatch between DSR_WTD and DI_TAMPER_EVENT_WTD"
+#endif
+#if DSR_ETBD != DI_TAMPER_EVENT_ETBD
+#error "Mismatch between DSR_ETBD and DI_TAMPER_EVENT_ETBD"
+#endif
+#if DSR_ETAD != DI_TAMPER_EVENT_ETAD
+#error "Mismatch between DSR_ETAD and DI_TAMPER_EVENT_ETAD"
+#endif
+#if DSR_EBD != DI_TAMPER_EVENT_EBD
+#error "Mismatch between DSR_EBD and DI_TAMPER_EVENT_EBD"
+#endif
+#if DSR_SAD != DI_TAMPER_EVENT_SAD
+#error "Mismatch between DSR_SAD and DI_TAMPER_EVENT_SAD"
+#endif
+#if DSR_TTD != DI_TAMPER_EVENT_TTD
+#error "Mismatch between DSR_TTD and DI_TAMPER_EVENT_TTD"
+#endif
+#if DSR_CTD != DI_TAMPER_EVENT_CTD
+#error "Mismatch between DSR_CTD and DI_TAMPER_EVENT_CTD"
+#endif
+#if DSR_VTD != DI_TAMPER_EVENT_VTD
+#error "Mismatch between DSR_VTD and DI_TAMPER_EVENT_VTD"
+#endif
+#if DSR_MCO != DI_TAMPER_EVENT_MCO
+#error "Mismatch between DSR_MCO and DI_TAMPER_EVENT_MCO"
+#endif
+#if DSR_TCO != DI_TAMPER_EVENT_TCO
+#error "Mismatch between DSR_TCO and DI_TAMPER_EVENT_TCO"
+#endif
+
+/* DryIce Interrupt Enable Reg */
+#define DIER    0x18
+#define DIER_WNIE   (1 << 9)   /* Write Next Interrupt Enable */
+#define DIER_WCIE   (1 << 8)   /* Write Complete Interrupt Enable */
+#define DIER_WEIE   (1 << 7)   /* Write Error Interrupt Enable */
+#define DIER_RKIE   (1 << 5)   /* Random Key Interrupt Enable */
+#define DIER_CAIE   (1 << 4)   /* Clock Alarm Interrupt Enable */
+#define DIER_MOIE   (1 << 3)   /* Monotonic Overflow Interrupt En */
+#define DIER_TOIE   (1 << 2)   /* Time Overflow Interrupt Enable */
+#define DIER_SVIE   (1 << 0)   /* Security Violation Interrupt En */
+
+/* DryIce Monotonic Counter Reg */
+#define DMCR    0x1c
+
+/* DryIce Key Select Reg */
+#define DKSR    0x20
+#define DKSR_IIM_KEY           0x0
+#define DKSR_PROG_KEY          0x4
+#define DKSR_RAND_KEY          0x5
+#define DKSR_PROG_XOR_IIM_KEY  0x6
+#define DKSR_RAND_XOR_IIM_KEY  0x7
+
+/* DryIce Key Control Reg */
+#define DKCR    0x24
+#define DKCR_LRK    (1 << 0)   /* Load Random Key */
+
+/* DryIce Tamper Configuration Reg */
+#define DTCR    0x28
+#define DTCR_ETGFB_SHIFT  27   /* Ext Tamper Glitch Filter B */
+#define DTCR_ETGFB_MASK   0xf8000000
+#define DTCR_ETGFA_SHIFT  22   /* Ext Tamper Glitch Filter A */
+#define DTCR_ETGFA_MASK   0x07c00000
+#define DTCR_WTGF_SHIFT   17   /* Wire-mesh Tamper Glitch Filter */
+#define DTCR_WTGF_MASK    0x003e0000
+#define DTCR_WGFE   (1 << 16)  /* Wire-mesh Glitch Filter Enable */
+#define DTCR_SAOE   (1 << 15)  /* Security Alarm Output Enable */
+#define DTCR_MOE    (1 << 9)   /* Monotonic Overflow Enable */
+#define DTCR_TOE    (1 << 8)   /* Time Overflow Enable */
+#define DTCR_WTE    (1 << 7)   /* Wire-mesh Tampering Enable */
+#define DTCR_ETBE   (1 << 6)   /* External Tampering B Enable */
+#define DTCR_ETAE   (1 << 5)   /* External Tampering A Enable */
+#define DTCR_EBE    (1 << 4)   /* External Boot Enable */
+#define DTCR_SAIE   (1 << 3)   /* Security Alarm Input Enable */
+#define DTCR_TTE    (1 << 2)   /* Temperature Tamper Enable */
+#define DTCR_CTE    (1 << 1)   /* Clock Tamper Enable */
+#define DTCR_VTE    (1 << 0)   /* Voltage Tamper Enable */
+
+/* DryIce Analog Configuration Reg */
+#define DACR    0x2c
+#define DACR_VRC_SHIFT    6    /* Voltage Reference Configuration */
+#define DACR_VRC_MASK     0x000001c0
+#define DACR_HTDC_SHIFT   3    /* High Temperature Detect Configuration */
+#define DACR_HTDC_MASK    0x00000038
+#define DACR_LTDC_SHIFT   0    /* Low Temperature Detect Configuration */
+#define DACR_LTDC_MASK    0x00000007
+
+/* DryIce General Purpose Reg */
+#define DGPR    0x3c
+
+/* DryIce Programmed Key0-7 Regs */
+#define DPKR0   0x40
+#define DPKR1   0x44
+#define DPKR2   0x48
+#define DPKR3   0x4c
+#define DPKR4   0x50
+#define DPKR5   0x54
+#define DPKR6   0x58
+#define DPKR7   0x5c
+
+/* DryIce Random Key0-7 Regs */
+#define DRKR0   0x60
+#define DRKR1   0x64
+#define DRKR2   0x68
+#define DRKR3   0x6c
+#define DRKR4   0x70
+#define DRKR5   0x74
+#define DRKR6   0x78
+#define DRKR7   0x7c
+
+#define DI_ADDRESS_RANGE  (DRKR7 + 4)
+
+/*
+ * this doesn't really belong here but the
+ * portability layer doesn't include it
+ */
+#ifdef LINUX_KERNEL
+#define EXTERN_SYMBOL(symbol)  EXPORT_SYMBOL(symbol)
+#else
+#define EXTERN_SYMBOL(symbol)  do {} while (0)
+#endif
+
+#endif /* __DRYICE_REGS_H__ */
diff --git a/drivers/mxc/security/dryice.c b/drivers/mxc/security/dryice.c
new file mode 100644
index 000000000000..f7cefb6ae599
--- /dev/null
+++ b/drivers/mxc/security/dryice.c
@@ -0,0 +1,1123 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+
+#undef DI_DEBUG        /* enable debug messages */
+#undef DI_DEBUG_REGIO  /* show register read/write */
+#undef DI_TESTING      /* include test code */
+
+#ifdef DI_DEBUG
+#define di_debug(fmt, arg...) os_printk(KERN_INFO fmt, ##arg)
+#else
+#define di_debug(fmt, arg...) do {} while (0)
+#endif
+
+#define di_info(fmt, arg...) os_printk(KERN_INFO fmt, ##arg)
+#define di_warn(fmt, arg...) os_printk(KERN_WARNING fmt, ##arg)
+
+#include "sahara2/include/portable_os.h"
+#include "dryice.h"
+#include "dryice-regs.h"
+
+/* mask of the lock-related function flags */
+#define DI_FUNC_LOCK_FLAGS  (DI_FUNC_FLAG_READ_LOCK  | \
+			     DI_FUNC_FLAG_WRITE_LOCK | \
+			     DI_FUNC_FLAG_HARD_LOCK)
+
+/*
+ * dryice hardware states
+ */
+enum di_states {
+	DI_STATE_VALID = 0,
+	DI_STATE_NON_VALID,
+	DI_STATE_FAILURE,
+};
+
+/*
+ * todo list actions
+ */
+enum todo_actions {
+	TODO_ACT_WRITE_VAL,
+	TODO_ACT_WRITE_PTR,
+	TODO_ACT_WRITE_PTR32,
+	TODO_ACT_ASSIGN,
+	TODO_ACT_WAIT_RKG,
+};
+
+/*
+ * todo list status
+ */
+enum todo_status {
+	TODO_ST_LOADING,
+	TODO_ST_READY,
+	TODO_ST_PEND_WCF,
+	TODO_ST_PEND_RKG,
+	TODO_ST_DONE,
+};
+
+OS_DEV_INIT_DCL(dryice_init)
+OS_DEV_SHUTDOWN_DCL(dryice_exit)
+OS_DEV_ISR_DCL(dryice_norm_irq)
+OS_WAIT_OBJECT(done_queue);
+OS_WAIT_OBJECT(exit_queue);
+
+struct dryice_data {
+	int busy;               /* enforce exclusive access */
+	os_lock_t busy_lock;
+	int exit_flag;          /* don't start new operations */
+
+	uint32_t baseaddr;      /* physical base address */
+	void *ioaddr;           /* virtual base address */
+
+	/* interrupt handling */
+	struct irq_struct {
+		os_interrupt_id_t irq;
+		int set;
+	} irq_norm, irq_sec;
+
+	struct clk *clk;        /* clock control */
+
+	int key_programmed;     /* key has been programmed */
+	int key_selected;       /* key has been selected */
+
+	/* callback function and cookie */
+	void (*cb_func)(di_return_t rc, unsigned long cookie);
+	unsigned long cb_cookie;
+} *di = NULL;
+
+#define TODO_LIST_LEN	12
+static struct {
+	struct td {
+		enum todo_actions action;
+		uint32_t src;
+		uint32_t dst;
+		int num;
+	} list[TODO_LIST_LEN];
+	int cur;                /* current todo pointer */
+	int num;		/* number of todo's on the list */
+	int async;              /* non-zero if list is async */
+	int status;             /* current status of the list */
+	di_return_t rc;         /* return code generated by the list */
+} todo;
+
+/*
+ * dryice register read/write functions
+ */
+#ifdef DI_DEBUG_REGIO
+static uint32_t di_read(int reg)
+{
+	uint32_t val = os_read32(di->ioaddr + (reg));
+	di_info("di_read(0x%02x) = 0x%08x\n", reg, val);
+
+	return val;
+}
+
+static void di_write(uint32_t val, int reg)
+{
+	di_info("dryice_write_reg(0x%08x, 0x%02x)\n", val, reg);
+	os_write32(di->ioaddr + (reg), val);
+}
+#else
+#define di_read(reg)        os_read32(di->ioaddr + (reg))
+#define di_write(val, reg)  os_write32(di->ioaddr + (reg), val);
+#endif
+
+/*
+ * set the dryice busy flag atomically, allowing
+ * for case where the driver is trying to exit.
+ */
+static int di_busy_set(void)
+{
+	os_lock_context_t context;
+	int rc = 0;
+
+	os_lock_save_context(di->busy_lock, context);
+	if (di->exit_flag || di->busy)
+		rc = 1;
+	else
+		di->busy = 1;
+	os_unlock_restore_context(di->busy_lock, context);
+
+	return rc;
+}
+
+/*
+ * clear the dryice busy flag
+ */
+static inline void di_busy_clear(void)
+{
+	/* don't acquire the lock because the race is benign */
+	di->busy = 0;
+
+	if (di->exit_flag)
+		os_wake_sleepers(exit_queue);
+}
+
+/*
+ * return the current state of dryice
+ * (valid, non-valid, or failure)
+ */
+static enum di_states di_state(void)
+{
+	enum di_states state = DI_STATE_VALID;
+	uint32_t dsr = di_read(DSR);
+
+	if (dsr & DSR_NVF)
+		state = DI_STATE_NON_VALID;
+	else if (dsr & DSR_SVF)
+		state = DI_STATE_FAILURE;
+
+	return state;
+}
+
+#define DI_WRITE_LOOP_CNT 0x1000
+/*
+ * the write-error flag is something that shouldn't get set
+ * during normal operation.  if it's set something is terribly
+ * wrong.  the best we can do is try to clear the bit and hope
+ * that dryice will recover.  this situation is similar to an
+ * unexpected bus fault in terms of severity.
+ */
+static void try_to_clear_wef(void)
+{
+	int cnt;
+
+	while (1) {
+		di_write(DSR_WEF, DSR);
+		for (cnt = 0; cnt < DI_WRITE_LOOP_CNT; cnt++) {
+			if ((di_read(DSR) & DSR_WEF) == 0)
+				break;
+		}
+		di_warn("WARNING: DryIce cannot clear DSR_WEF "
+			"(Write Error Flag)!\n");
+	}
+}
+
+/*
+ * write a dryice register and loop, waiting for it
+ * to complete. use only during driver initialization.
+ * returns 0 on success or 1 on write failure.
+ */
+static int di_write_loop(uint32_t val, int reg)
+{
+	int rc = 0;
+	int cnt;
+
+	di_debug("FUNC: %s\n", __func__);
+	di_write(val, reg);
+
+	for (cnt = 0; cnt < DI_WRITE_LOOP_CNT; cnt++) {
+		uint32_t dsr = di_read(DSR);
+		if (dsr & DSR_WEF) {
+			try_to_clear_wef();
+			rc = 1;
+		}
+		if (dsr & DSR_WCF)
+			break;
+	}
+	di_debug("wait_write_loop looped %d times\n", cnt);
+	if (cnt == DI_WRITE_LOOP_CNT)
+		rc = 1;
+
+	if (rc)
+		di_warn("DryIce wait_write_done: WRITE ERROR!\n");
+	return rc;
+}
+
+/*
+ * initialize the todo list. must be called
+ * before adding items to the list.
+ */
+static void todo_init(int async_flag)
+{
+	di_debug("FUNC: %s\n", __func__);
+	todo.cur = 0;
+	todo.num = 0;
+	todo.async = async_flag;
+	todo.rc = 0;
+	todo.status = TODO_ST_LOADING;
+}
+
+/*
+ * perform the current action on the todo list
+ */
+#define TC  todo.list[todo.cur]
+void todo_cur(void)
+{
+	di_debug("FUNC: %s[%d]\n", __func__, todo.cur);
+	switch (TC.action) {
+	case TODO_ACT_WRITE_VAL:
+		di_debug("  TODO_ACT_WRITE_VAL\n");
+		/* enable the write-completion interrupt */
+		todo.status = TODO_ST_PEND_WCF;
+		di_write(di_read(DIER) | DIER_WCIE, DIER);
+
+		di_write(TC.src, TC.dst);
+		break;
+
+	case TODO_ACT_WRITE_PTR32:
+		di_debug("  TODO_ACT_WRITE_PTR32\n");
+		/* enable the write-completion interrupt */
+		todo.status = TODO_ST_PEND_WCF;
+		di_write(di_read(DIER) | DIER_WCIE, DIER);
+
+		di_write(*(uint32_t *)TC.src, TC.dst);
+		break;
+
+	case TODO_ACT_WRITE_PTR:
+		{
+			uint8_t *p = (uint8_t *)TC.src;
+			uint32_t val = 0;
+			int num = TC.num;
+
+			di_debug("  TODO_ACT_WRITE_PTR\n");
+			while (num--)
+				val = (val << 8) | *p++;
+
+			/* enable the write-completion interrupt */
+			todo.status = TODO_ST_PEND_WCF;
+			di_write(di_read(DIER) | DIER_WCIE, DIER);
+
+			di_write(val, TC.dst);
+		}
+		break;
+
+	case TODO_ACT_ASSIGN:
+		di_debug("  TODO_ACT_ASSIGN\n");
+		switch (TC.num) {
+		case 1:
+			*(uint8_t *)TC.dst = TC.src;
+			break;
+		case 2:
+			*(uint16_t *)TC.dst = TC.src;
+			break;
+		case 4:
+			*(uint32_t *)TC.dst = TC.src;
+			break;
+		default:
+			di_warn("Unexpected size in TODO_ACT_ASSIGN\n");
+			break;
+		}
+		break;
+
+	case TODO_ACT_WAIT_RKG:
+		di_debug("  TODO_ACT_WAIT_RKG\n");
+		/* enable the random-key interrupt */
+		todo.status = TODO_ST_PEND_RKG;
+		di_write(di_read(DIER) | DIER_RKIE, DIER);
+		break;
+
+	default:
+		di_debug("  TODO_ACT_NOOP\n");
+		break;
+	}
+}
+
+/*
+ * called when done with the todo list.
+ * if async, it does the callback.
+ * if blocking, it wakes up the caller.
+ */
+static void todo_done(di_return_t rc)
+{
+	todo.rc = rc;
+	todo.status = TODO_ST_DONE;
+	if (todo.async) {
+		di_busy_clear();
+		if (di->cb_func)
+			di->cb_func(rc, di->cb_cookie);
+	} else
+		os_wake_sleepers(done_queue);
+}
+
+/*
+ * performs the actions sequentially from the todo list
+ * until it encounters an item that isn't ready.
+ */
+static void todo_run(void)
+{
+	di_debug("FUNC: %s\n", __func__);
+	while (todo.status == TODO_ST_READY) {
+		if (todo.cur == todo.num) {
+			todo_done(0);
+			break;
+		}
+		todo_cur();
+		if (todo.status != TODO_ST_READY)
+			break;
+		todo.cur++;
+	}
+}
+
+/*
+ * kick off the todo list by making it ready
+ */
+static void todo_start(void)
+{
+	di_debug("FUNC: %s\n", __func__);
+	todo.status = TODO_ST_READY;
+	todo_run();
+}
+
+/*
+ * blocking callers sleep here until the todo list is done
+ */
+static int todo_wait_done(void)
+{
+	di_debug("FUNC: %s\n", __func__);
+	os_sleep(done_queue, todo.status == TODO_ST_DONE, 0);
+
+	return todo.rc;
+}
+
+/*
+ * add a dryice register write to the todo list.
+ * the value to be written is supplied.
+ */
+#define todo_write_val(val, reg) \
+		todo_add(TODO_ACT_WRITE_VAL, val, reg, 0)
+
+/*
+ * add a dryice register write to the todo list.
+ * "size" bytes pointed to by addr will be written.
+ */
+#define todo_write_ptr(addr, reg, size) \
+		todo_add(TODO_ACT_WRITE_PTR, (uint32_t)addr, reg, size)
+
+/*
+ * add a dryice register write to the todo list.
+ * the word pointed to by addr will be written.
+ */
+#define todo_write_ptr32(addr, reg) \
+		todo_add(TODO_ACT_WRITE_PTR32, (uint32_t)addr, reg, 0)
+
+/*
+ * add a dryice memory write to the todo list.
+ * object can only have a size of 1, 2, or 4 bytes.
+ */
+#define todo_assign(var, val) \
+		todo_add(TODO_ACT_ASSIGN, val, (uint32_t)&(var), sizeof(var))
+
+#define todo_wait_rkg() \
+		todo_add(TODO_ACT_WAIT_RKG, 0, 0, 0)
+
+static void todo_add(int action, uint32_t src, uint32_t dst, int num)
+{
+	struct td *p = &todo.list[todo.num];
+
+	di_debug("FUNC: %s\n", __func__);
+	if (todo.num == TODO_LIST_LEN) {
+		di_warn("WARNING: DryIce todo-list overflow!\n");
+		return;
+	}
+	p->action = action;
+	p->src = src;
+	p->dst = dst;
+	p->num = num;
+	todo.num++;
+}
+
+#if defined(DI_DEBUG) || defined(DI_TESTING)
+/*
+ * print out the contents of the dryice status register
+ * with all the bits decoded
+ */
+static void show_dsr(const char *heading)
+{
+	uint32_t dsr = di_read(DSR);
+
+	di_info("%s\n", heading);
+	if (dsr & DSR_TAMPER_BITS) {
+		if (dsr & DSR_WTD)
+			di_info("Wire-mesh Tampering Detected\n");
+		if (dsr & DSR_ETBD)
+			di_info("External Tampering B Detected\n");
+		if (dsr & DSR_ETAD)
+			di_info("External Tampering A Detected\n");
+		if (dsr & DSR_EBD)
+			di_info("External Boot Detected\n");
+		if (dsr & DSR_SAD)
+			di_info("Security Alarm Detected\n");
+		if (dsr & DSR_TTD)
+			di_info("Temperature Tampering Detected\n");
+		if (dsr & DSR_CTD)
+			di_info("Clock Tampering Detected\n");
+		if (dsr & DSR_VTD)
+			di_info("Voltage Tampering Detected\n");
+		if (dsr & DSR_MCO)
+			di_info("Monotonic Counter Overflow\n");
+		if (dsr & DSR_TCO)
+			di_info("Time Counter Overflow\n");
+	} else
+		di_info("No Tamper Events Detected\n");
+
+	di_info("%d Key Busy Flag\n",           !!(dsr & DSR_KBF));
+	di_info("%d Write Busy Flag\n",         !!(dsr & DSR_WBF));
+	di_info("%d Write Next Flag\n",         !!(dsr & DSR_WNF));
+	di_info("%d Write Complete Flag\n",     !!(dsr & DSR_WCF));
+	di_info("%d Write Error Flag\n",        !!(dsr & DSR_WEF));
+	di_info("%d Random Key Error\n",        !!(dsr & DSR_RKE));
+	di_info("%d Random Key Valid\n",        !!(dsr & DSR_RKV));
+	di_info("%d Clock Alarm Flag\n",        !!(dsr & DSR_CAF));
+	di_info("%d Non-Valid Flag\n",          !!(dsr & DSR_NVF));
+	di_info("%d Security Violation Flag\n", !!(dsr & DSR_SVF));
+}
+
+/*
+ * print out a key in hex
+ */
+static void print_key(const char *tag, uint8_t *key, int bits)
+{
+	int bytes = (bits + 7) / 8;
+
+	di_info("%s", tag);
+	while (bytes--)
+		os_printk("%02x", *key++);
+	os_printk("\n");
+}
+#endif  /* defined(DI_DEBUG) || defined(DI_TESTING) */
+
+/*
+ * dryice normal interrupt service routine
+ */
+OS_DEV_ISR(dryice_norm_irq)
+{
+	/* save dryice status register */
+	uint32_t dsr = di_read(DSR);
+
+	if (dsr & DSR_WCF) {
+		/* disable the write-completion interrupt */
+		di_write(di_read(DIER) & ~DIER_WCIE, DIER);
+
+		if (todo.status == TODO_ST_PEND_WCF) {
+			if (dsr & DSR_WEF) {
+				try_to_clear_wef();
+				todo_done(DI_ERR_WRITE);
+			} else {
+				todo.cur++;
+				todo.status = TODO_ST_READY;
+				todo_run();
+			}
+		}
+	} else if (dsr & (DSR_RKV | DSR_RKE)) {
+		/* disable the random-key-gen interrupt */
+		di_write(di_read(DIER) & ~DIER_RKIE, DIER);
+
+		if (todo.status == TODO_ST_PEND_RKG) {
+			if (dsr & DSR_RKE)
+				todo_done(DI_ERR_FAIL);
+			else {
+				todo.cur++;
+				todo.status = TODO_ST_READY;
+				todo_run();
+			}
+		}
+	}
+
+	os_dev_isr_return(1);
+}
+
+/* write loop with error handling -- for init only */
+#define di_write_loop_goto(val, reg, rc, label) \
+		do {if (di_write_loop(val, reg)) \
+		{rc = OS_ERROR_FAIL_S; goto label; } } while (0)
+
+/*
+ * dryice driver initialization
+ */
+OS_DEV_INIT(dryice_init)
+{
+	di_return_t rc = 0;
+
+	di_info("MXC DryIce driver\n");
+
+	/* allocate memory */
+	di = os_alloc_memory(sizeof(*di), GFP_KERNEL);
+	if (di == NULL) {
+		rc = OS_ERROR_NO_MEMORY_S;
+		goto err_alloc;
+	}
+	memset(di, 0, sizeof(*di));
+	di->baseaddr = DRYICE_BASE_ADDR;
+	di->irq_norm.irq = MXC_INT_DRYICE_NORM;
+	di->irq_sec.irq = MXC_INT_DRYICE_SEC;
+
+	/* map i/o registers */
+	di->ioaddr = os_map_device(di->baseaddr, DI_ADDRESS_RANGE);
+	if (di->ioaddr == NULL) {
+		rc = OS_ERROR_FAIL_S;
+		goto err_iomap;
+	}
+
+	/* allocate locks */
+	di->busy_lock = os_lock_alloc_init();
+	if (di->busy_lock == NULL) {
+		rc = OS_ERROR_NO_MEMORY_S;
+		goto err_locks;
+	}
+
+	/* enable clocks (is there a portable way to do this?) */
+	di->clk = clk_get(NULL, "dryice_clk");
+	clk_enable(di->clk);
+
+	/* register for interrupts */
+	/* os_register_interrupt() dosen't support an option to make the
+	    interrupt as shared. Replaced it with request_irq().*/
+	rc = request_irq(di->irq_norm.irq, dryice_norm_irq, IRQF_SHARED,
+				"dry_ice", di);
+	if (rc)
+		goto err_irqs;
+	else
+		di->irq_norm.set = 1;
+
+	/*
+	 * DRYICE HARDWARE INIT
+	 */
+
+#ifdef DI_DEBUG
+	show_dsr("DSR Pre-Initialization State");
+#endif
+
+	if (di_state() == DI_STATE_NON_VALID) {
+		uint32_t dsr = di_read(DSR);
+
+		di_debug("initializing from non-valid state\n");
+
+		/* clear security violation flag */
+		if (dsr & DSR_SVF)
+			di_write_loop_goto(DSR_SVF, DSR, rc, err_write);
+
+		/* clear tamper detect flags */
+		if (dsr & DSR_TAMPER_BITS)
+			di_write_loop_goto(DSR_TAMPER_BITS, DSR, rc, err_write);
+
+		/* initialize timers */
+		di_write_loop_goto(0, DTCLR, rc, err_write);
+		di_write_loop_goto(0, DTCMR, rc, err_write);
+		di_write_loop_goto(0, DMCR, rc, err_write);
+
+		/* clear non-valid flag */
+		di_write_loop_goto(DSR_NVF, DSR, rc, err_write);
+	}
+
+	/* set tamper events we are interested in watching */
+	di_write_loop_goto(DTCR_WTE | DTCR_ETBE | DTCR_ETAE, DTCR, rc,
+			   err_write);
+#ifdef DI_DEBUG
+	show_dsr("DSR Post-Initialization State");
+#endif
+	os_dev_init_return(OS_ERROR_OK_S);
+
+err_write:
+	/* unregister interrupts */
+	if (di->irq_norm.set)
+		os_deregister_interrupt(di->irq_norm.irq);
+	if (di->irq_sec.set)
+		os_deregister_interrupt(di->irq_sec.irq);
+
+	/* turn off clocks (is there a portable way to do this?) */
+	clk_disable(di->clk);
+	clk_put(di->clk);
+
+err_irqs:
+	/* unallocate locks */
+	os_lock_deallocate(di->busy_lock);
+
+err_locks:
+	/* unmap i/o registers */
+	os_unmap_device(di->ioaddr, DI_ADDRESS_RANGE);
+
+err_iomap:
+	/* free the dryice struct */
+	os_free_memory(di);
+
+err_alloc:
+	os_dev_init_return(rc);
+}
+
+/*
+ * dryice driver exit routine
+ */
+OS_DEV_SHUTDOWN(dryice_exit)
+{
+	/* don't allow new operations */
+	di->exit_flag = 1;
+
+	/* wait for the current operation to complete */
+	os_sleep(exit_queue, di->busy == 0, 0);
+
+	/* unregister interrupts */
+	if (di->irq_norm.set)
+		os_deregister_interrupt(di->irq_norm.irq);
+	if (di->irq_sec.set)
+		os_deregister_interrupt(di->irq_sec.irq);
+
+	/* turn off clocks (is there a portable way to do this?) */
+	clk_disable(di->clk);
+	clk_put(di->clk);
+
+	/* unallocate locks */
+	os_lock_deallocate(di->busy_lock);
+
+	/* unmap i/o registers */
+	os_unmap_device(di->ioaddr, DI_ADDRESS_RANGE);
+
+	/* free the dryice struct */
+	os_free_memory(di);
+
+	os_dev_shutdown_return(OS_ERROR_OK_S);
+}
+
+di_return_t dryice_set_programmed_key(const void *key_data, int key_bits,
+				      int flags)
+{
+	uint32_t dcr;
+	int key_bytes, reg;
+	di_return_t rc = 0;
+
+	if (di_busy_set())
+		return DI_ERR_BUSY;
+
+	if (key_data == NULL) {
+		rc = DI_ERR_INVAL;
+		goto err;
+	}
+	if (key_bits < 0 || key_bits > MAX_KEY_LEN || key_bits % 8) {
+		rc = DI_ERR_INVAL;
+		goto err;
+	}
+	if (flags & DI_FUNC_FLAG_WORD_KEY) {
+		if (key_bits % 32 || (uint32_t)key_data & 0x3) {
+			rc = DI_ERR_INVAL;
+			goto err;
+		}
+	}
+	if (di->key_programmed) {
+		rc = DI_ERR_INUSE;
+		goto err;
+	}
+	if (di_state() == DI_STATE_FAILURE) {
+		rc = DI_ERR_STATE;
+		goto err;
+	}
+	dcr = di_read(DCR);
+	if (dcr & DCR_PKWHL) {
+		rc = DI_ERR_HLOCK;
+		goto err;
+	}
+	if (dcr & DCR_PKWSL) {
+		rc = DI_ERR_SLOCK;
+		goto err;
+	}
+	key_bytes = key_bits / 8;
+
+	todo_init((flags & DI_FUNC_FLAG_ASYNC) != 0);
+
+	/* accomodate busses that can only do 32-bit transfers */
+	if (flags & DI_FUNC_FLAG_WORD_KEY) {
+		uint32_t *keyp = (void *)key_data;
+
+		for (reg = 0; reg < MAX_KEY_WORDS; reg++) {
+			if (reg < MAX_KEY_WORDS - key_bytes / 4)
+				todo_write_val(0, DPKR7 - reg * 4);
+			else {
+				todo_write_ptr32(keyp, DPKR7 - reg * 4);
+				keyp++;
+			}
+		}
+	} else {
+		uint8_t *keyp = (void *)key_data;
+
+		for (reg = 0; reg < MAX_KEY_WORDS; reg++) {
+			int size = key_bytes - (MAX_KEY_WORDS - reg - 1) * 4;
+			if (size <= 0)
+				todo_write_val(0, DPKR7 - reg * 4);
+			else {
+				if (size > 4)
+					size = 4;
+				todo_write_ptr(keyp, DPKR7 - reg * 4, size);
+				keyp += size;
+			}
+		}
+	}
+	todo_assign(di->key_programmed, 1);
+
+	if (flags & DI_FUNC_LOCK_FLAGS) {
+		dcr = di_read(DCR);
+		if (flags & DI_FUNC_FLAG_READ_LOCK) {
+			if (flags & DI_FUNC_FLAG_HARD_LOCK)
+				dcr |= DCR_PKRHL;
+			else
+				dcr |= DCR_PKRSL;
+		}
+		if (flags & DI_FUNC_FLAG_WRITE_LOCK) {
+			if (flags & DI_FUNC_FLAG_HARD_LOCK)
+				dcr |= DCR_PKWHL;
+			else
+				dcr |= DCR_PKWSL;
+		}
+		todo_write_val(dcr, DCR);
+	}
+	todo_start();
+
+	if (flags & DI_FUNC_FLAG_ASYNC)
+		return 0;
+
+	rc = todo_wait_done();
+err:
+	di_busy_clear();
+	return rc;
+}
+EXTERN_SYMBOL(dryice_set_programmed_key);
+
+di_return_t dryice_get_programmed_key(uint8_t *key_data, int key_bits)
+{
+	int reg, byte, key_bytes;
+	uint32_t dcr, dpkr;
+	di_return_t rc = 0;
+
+	if (di_busy_set())
+		return DI_ERR_BUSY;
+
+	if (key_data == NULL) {
+		rc = DI_ERR_INVAL;
+		goto err;
+	}
+	if (key_bits < 0 || key_bits > MAX_KEY_LEN || key_bits % 8) {
+		rc = DI_ERR_INVAL;
+		goto err;
+	}
+	#if 0
+	if (!di->key_programmed) {
+		rc = DI_ERR_UNSET;
+		goto err;
+	}
+	#endif
+	if (di_state() == DI_STATE_FAILURE) {
+		rc = DI_ERR_STATE;
+		goto err;
+	}
+	dcr = di_read(DCR);
+	if (dcr & DCR_PKRHL) {
+		rc = DI_ERR_HLOCK;
+		goto err;
+	}
+	if (dcr & DCR_PKRSL) {
+		rc = DI_ERR_SLOCK;
+		goto err;
+	}
+	key_bytes = key_bits / 8;
+
+	/* read key */
+	for (reg = 0; reg < MAX_KEY_WORDS; reg++) {
+		if (reg < (MAX_KEY_BYTES - key_bytes) / 4)
+			continue;
+		dpkr = di_read(DPKR7 - reg * 4);
+
+		for (byte = 0; byte < 4; byte++) {
+			if (reg * 4 + byte >= MAX_KEY_BYTES - key_bytes) {
+				int shift = 24 - byte * 8;
+				*key_data++ = (dpkr >> shift) & 0xff;
+			}
+		}
+		dpkr = 0;	/* cleared for security */
+	}
+err:
+	di_busy_clear();
+	return rc;
+}
+EXTERN_SYMBOL(dryice_get_programmed_key);
+
+di_return_t dryice_release_programmed_key(void)
+{
+	uint32_t dcr;
+	di_return_t rc = 0;
+
+	if (di_busy_set())
+		return DI_ERR_BUSY;
+
+	if (!di->key_programmed) {
+		rc = DI_ERR_UNSET;
+		goto err;
+	}
+	dcr = di_read(DCR);
+	if (dcr & DCR_PKWHL) {
+		rc = DI_ERR_HLOCK;
+		goto err;
+	}
+	if (dcr & DCR_PKWSL) {
+		rc = DI_ERR_SLOCK;
+		goto err;
+	}
+	di->key_programmed = 0;
+
+err:
+	di_busy_clear();
+	return rc;
+}
+EXTERN_SYMBOL(dryice_release_programmed_key);
+
+di_return_t dryice_set_random_key(int flags)
+{
+	uint32_t dcr;
+	di_return_t rc = 0;
+
+	if (di_busy_set())
+		return DI_ERR_BUSY;
+
+	if (di_state() == DI_STATE_FAILURE) {
+		rc = DI_ERR_STATE;
+		goto err;
+	}
+	dcr = di_read(DCR);
+	if (dcr & DCR_RKHL) {
+		rc = DI_ERR_HLOCK;
+		goto err;
+	}
+	if (dcr & DCR_RKSL) {
+		rc = DI_ERR_SLOCK;
+		goto err;
+	}
+	todo_init((flags & DI_FUNC_FLAG_ASYNC) != 0);
+
+	/* clear Random Key Error bit, if set */
+	if (di_read(DSR) & DSR_RKE)
+		todo_write_val(DSR_RKE, DCR);
+
+	/* load random key */
+	todo_write_val(DKCR_LRK, DKCR);
+
+	/* wait for RKV (valid) or RKE (error) */
+	todo_wait_rkg();
+
+	if (flags & DI_FUNC_LOCK_FLAGS) {
+		dcr = di_read(DCR);
+		if (flags & DI_FUNC_FLAG_WRITE_LOCK) {
+			if (flags & DI_FUNC_FLAG_HARD_LOCK)
+				dcr |= DCR_RKHL;
+			else
+				dcr |= DCR_RKSL;
+		}
+		todo_write_val(dcr, DCR);
+	}
+	todo_start();
+
+	if (flags & DI_FUNC_FLAG_ASYNC)
+		return 0;
+
+	rc = todo_wait_done();
+err:
+	di_busy_clear();
+	return rc;
+}
+EXTERN_SYMBOL(dryice_set_random_key);
+
+di_return_t dryice_select_key(di_key_t key, int flags)
+{
+	uint32_t dcr, dksr;
+	di_return_t rc = 0;
+
+	if (di_busy_set())
+		return DI_ERR_BUSY;
+
+	switch (key) {
+	case DI_KEY_FK:
+		dksr = DKSR_IIM_KEY;
+		break;
+	case DI_KEY_PK:
+		dksr = DKSR_PROG_KEY;
+		break;
+	case DI_KEY_RK:
+		dksr = DKSR_RAND_KEY;
+		break;
+	case DI_KEY_FPK:
+		dksr = DKSR_PROG_XOR_IIM_KEY;
+		break;
+	case DI_KEY_FRK:
+		dksr = DKSR_RAND_XOR_IIM_KEY;
+		break;
+	default:
+		rc = DI_ERR_INVAL;
+		goto err;
+	}
+	if (di->key_selected) {
+		rc = DI_ERR_INUSE;
+		goto err;
+	}
+	if (di_state() != DI_STATE_VALID) {
+		rc = DI_ERR_STATE;
+		goto err;
+	}
+	dcr = di_read(DCR);
+	if (dcr & DCR_KSHL) {
+		rc = DI_ERR_HLOCK;
+		goto err;
+	}
+	if (dcr & DCR_KSSL) {
+		rc = DI_ERR_SLOCK;
+		goto err;
+	}
+	todo_init((flags & DI_FUNC_FLAG_ASYNC) != 0);
+
+	/* select key */
+	todo_write_val(dksr, DKSR);
+
+	todo_assign(di->key_selected, 1);
+
+	if (flags & DI_FUNC_LOCK_FLAGS) {
+		dcr = di_read(DCR);
+		if (flags & DI_FUNC_FLAG_WRITE_LOCK) {
+			if (flags & DI_FUNC_FLAG_HARD_LOCK)
+				dcr |= DCR_KSHL;
+			else
+				dcr |= DCR_KSSL;
+		}
+		todo_write_val(dcr, DCR);
+	}
+	todo_start();
+
+	if (flags & DI_FUNC_FLAG_ASYNC)
+		return 0;
+
+	rc = todo_wait_done();
+err:
+	di_busy_clear();
+	return rc;
+}
+EXTERN_SYMBOL(dryice_select_key);
+
+di_return_t dryice_check_key(di_key_t *key)
+{
+	uint32_t dksr;
+	di_return_t rc = 0;
+
+	if (di_busy_set())
+		return DI_ERR_BUSY;
+
+	if (key == NULL) {
+		rc = DI_ERR_INVAL;
+		goto err;
+	}
+
+	dksr = di_read(DKSR);
+
+	if (di_state() != DI_STATE_VALID) {
+		dksr = DKSR_IIM_KEY;
+		rc = DI_ERR_STATE;
+	} else if (dksr == DI_KEY_RK || dksr == DI_KEY_FRK) {
+		if (!(di_read(DSR) & DSR_RKV)) {
+			dksr = DKSR_IIM_KEY;
+			rc = DI_ERR_UNSET;
+		}
+	}
+	switch (dksr) {
+	case DKSR_IIM_KEY:
+		*key = DI_KEY_FK;
+		break;
+	case DKSR_PROG_KEY:
+		*key = DI_KEY_PK;
+		break;
+	case DKSR_RAND_KEY:
+		*key = DI_KEY_RK;
+		break;
+	case DKSR_PROG_XOR_IIM_KEY:
+		*key = DI_KEY_FPK;
+		break;
+	case DKSR_RAND_XOR_IIM_KEY:
+		*key = DI_KEY_FRK;
+		break;
+	}
+err:
+	di_busy_clear();
+	return rc;
+}
+EXTERN_SYMBOL(dryice_check_key);
+
+di_return_t dryice_release_key_selection(void)
+{
+	uint32_t dcr;
+	di_return_t rc = 0;
+
+	if (di_busy_set())
+		return DI_ERR_BUSY;
+
+	if (!di->key_selected) {
+		rc = DI_ERR_UNSET;
+		goto err;
+	}
+	dcr = di_read(DCR);
+	if (dcr & DCR_KSHL) {
+		rc = DI_ERR_HLOCK;
+		goto err;
+	}
+	if (dcr & DCR_KSSL) {
+		rc = DI_ERR_SLOCK;
+		goto err;
+	}
+	di->key_selected = 0;
+
+err:
+	di_busy_clear();
+	return rc;
+}
+EXTERN_SYMBOL(dryice_release_key_selection);
+
+di_return_t dryice_get_tamper_event(uint32_t *events, uint32_t *timestamp,
+				    int flags)
+{
+	di_return_t rc = 0;
+
+	if (di_busy_set())
+		return DI_ERR_BUSY;
+
+	if (di_state() == DI_STATE_VALID) {
+		rc = DI_ERR_STATE;
+		goto err;
+	}
+	if (events == NULL) {
+		rc = DI_ERR_INVAL;
+		goto err;
+	}
+		*events = di_read(DSR) & DSR_TAMPER_BITS;
+	if (timestamp) {
+		if (di_state() == DI_STATE_NON_VALID)
+			*timestamp = di_read(DTCMR);
+		else
+			*timestamp = 0;
+	}
+err:
+	di_busy_clear();
+	return rc;
+}
+EXTERN_SYMBOL(dryice_get_tamper_event);
+
+di_return_t dryice_register_callback(void (*func)(di_return_t,
+						  unsigned long cookie),
+				     unsigned long cookie)
+{
+	di_return_t rc = 0;
+
+	if (di_busy_set())
+		return DI_ERR_BUSY;
+
+	di->cb_func = func;
+	di->cb_cookie = cookie;
+
+	di_busy_clear();
+	return rc;
+}
+EXTERN_SYMBOL(dryice_register_callback);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("DryIce");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/security/dryice.h b/drivers/mxc/security/dryice.h
new file mode 100644
index 000000000000..8334b5098d31
--- /dev/null
+++ b/drivers/mxc/security/dryice.h
@@ -0,0 +1,287 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+
+#ifndef __DRYICE_H__
+#define __DRYICE_H__
+
+
+/*!
+ * @file dryice.h
+ * @brief Definition of DryIce API.
+ */
+
+/*! @page dryice_api DryIce API
+ *
+ * Definition of the DryIce API.
+ *
+ * The DryIce API implements a software interface to the DryIce hardware
+ * block. Methods are provided to store, retrieve, generate, and  manage
+ * cryptographic keys and to monitor security tamper events.
+ *
+ * See @ref dryice_api for the DryIce API.
+ */
+
+/*!
+ * This defines the SCC key length (in bits)
+ */
+#define SCC_KEY_LEN     168
+
+/*!
+ * This defines the maximum key length (in bits)
+ */
+#define MAX_KEY_LEN     256
+#define MAX_KEY_BYTES	((MAX_KEY_LEN) / 8)
+#define MAX_KEY_WORDS	((MAX_KEY_LEN) / 32)
+
+/*!
+ * @name DryIce Function Flags
+ */
+/*@{*/
+#define DI_FUNC_FLAG_ASYNC       0x01  /*!< do not block */
+#define DI_FUNC_FLAG_READ_LOCK   0x02  /*!< set read lock for this resource */
+#define DI_FUNC_FLAG_WRITE_LOCK  0x04  /*!< set write lock for resource */
+#define DI_FUNC_FLAG_HARD_LOCK   0x08  /*!< locks will be hard (default soft) */
+#define DI_FUNC_FLAG_WORD_KEY    0x10  /*!< key provided as 32-bit words */
+/*@}*/
+
+/*!
+ * @name DryIce Tamper Events
+ */
+/*@{*/
+#define DI_TAMPER_EVENT_WTD   (1 << 23)  /*!< wire-mesh tampering det */
+#define DI_TAMPER_EVENT_ETBD  (1 << 22)  /*!< ext tampering det: input B */
+#define DI_TAMPER_EVENT_ETAD  (1 << 21)  /*!< ext tampering det: input A */
+#define DI_TAMPER_EVENT_EBD   (1 << 20)  /*!< external boot detected */
+#define DI_TAMPER_EVENT_SAD   (1 << 19)  /*!< security alarm detected */
+#define DI_TAMPER_EVENT_TTD   (1 << 18)  /*!< temperature tampering det */
+#define DI_TAMPER_EVENT_CTD   (1 << 17)  /*!< clock tampering det */
+#define DI_TAMPER_EVENT_VTD   (1 << 16)  /*!< voltage tampering det */
+#define DI_TAMPER_EVENT_MCO   (1 <<  3)  /*!< monotonic counter overflow */
+#define DI_TAMPER_EVENT_TCO   (1 <<  2)  /*!< time counter overflow */
+/*@}*/
+
+/*!
+ * DryIce Key Sources
+ */
+typedef enum di_key {
+	DI_KEY_FK,   /*!< the fused (IIM) key */
+	DI_KEY_PK,   /*!< the programmed key */
+	DI_KEY_RK,   /*!< the random key */
+	DI_KEY_FPK,  /*!< the programmed key XORed with the fused key */
+	DI_KEY_FRK,  /*!< the random key XORed with the fused key */
+} di_key_t;
+
+/*!
+ * DryIce Error Codes
+ */
+typedef enum dryice_return {
+	DI_SUCCESS = 0,  /*!< operation was successful */
+	DI_ERR_BUSY,     /*!< device or resource busy */
+	DI_ERR_STATE,    /*!< dryice is in incompatible state */
+	DI_ERR_INUSE,    /*!< resource is already in use */
+	DI_ERR_UNSET,    /*!< resource has not been initialized */
+	DI_ERR_WRITE,    /*!< error occurred during register write */
+	DI_ERR_INVAL,    /*!< invalid argument */
+	DI_ERR_FAIL,     /*!< operation failed */
+	DI_ERR_HLOCK,    /*!< resource is hard locked */
+	DI_ERR_SLOCK,    /*!< resource is soft locked */
+	DI_ERR_NOMEM,    /*!< out of memory */
+} di_return_t;
+
+/*!
+ * These functions define the DryIce API.
+ */
+
+/*!
+ * Write a given key to the Programmed Key registers in DryIce, and
+ * optionally lock the Programmed Key against either reading or further
+ * writing. The value is held until a call to the release_programmed_key
+ * interface is made, or until the appropriate HW reset if the write-lock
+ * flags are used.  Unused key bits will be zeroed.
+ *
+ * @param[in]  key_data   A pointer to the key data to be programmed, with
+ *                        the most significant byte or word first.  This
+ *                        will be interpreted as a byte pointer unless the
+ *                        WORD_KEY flag is set, in which case it will be
+ *                        treated as a word pointer and the key data will be
+ *                        read a word at a time, starting with the MSW.
+ *                        When called asynchronously, the data pointed to by
+ *                        key_data must persist until the operation completes.
+ *
+ * @param[in]  key_bits   The number of bits in the key to be stored.
+ *                        This must be a multiple of 8 and within the
+ *                        range of 0 and MAX_KEY_LEN.
+ *
+ * @param[in]  flags      This is a bit-wise OR of the flags to be passed
+ *                        to the function.  Flags can include:
+ *                        ASYNC, READ_LOCK, WRITE_LOCK, HARD_LOCK, and
+ *                        WORD_KEY.
+ *
+ * @return                Returns SUCCESS (0), BUSY if DryIce is busy, INVAL
+ *                        on invalid arguments, INUSE if key has already been
+ *                        programmed, STATE if DryIce is in the wrong state,
+ *                        HLOCK or SLOCK if the key registers are locked for
+ *                        writing, and WRITE if a write error occurs
+ *                        (See #di_return_t).
+ */
+extern di_return_t dryice_set_programmed_key(const void *key_data, int key_bits,
+					     int flags);
+
+/*!
+ * Read the Programmed Key registers and write the contents into a buffer.
+ *
+ * @param[out] key_data   A byte pointer to where the key data will be written,
+ *                        with the most significant byte being written first.
+ *
+ * @param[in]  key_bits   The number of bits of the key to be retrieved.
+ *                        This must be a multiple of 8 and within the
+ *                        range of 0 and MAX_KEY_LEN.
+ *
+ * @return                Returns SUCCESS (0), BUSY if DryIce is busy, INVAL
+ *                        on invalid arguments, UNSET if key has not been
+ *                        programmed, STATE if DryIce is in the wrong state,
+ *                        and HLOCK or SLOCK if the key registers are locked for
+ *                        reading (See #di_return_t).
+ */
+extern di_return_t dryice_get_programmed_key(uint8_t *key_data, int key_bits);
+
+/*!
+ * Allow the set_programmed_key interface to be used to write a new
+ * Programmed Key to DryIce. Note that this interface does not overwrite
+ * the value in the Programmed Key registers.
+ *
+ * @return                Returns SUCCESS (0), BUSY if DryIce is busy,
+ *                        UNSET if the key has not been previously set, and
+ *                        HLOCK or SLOCK if the key registers are locked for
+ *                        writing (See #di_return_t).
+ */
+extern di_return_t dryice_release_programmed_key(void);
+
+/*!
+ * Generate and load a new Random Key in DryIce, and optionally lock the
+ * Random Key against further change.
+ *
+ * @param[in]  flags      This is a bit-wise OR of the flags to be passed
+ *                        to the function.  Flags can include:
+ *                        ASYNC, READ_LOCK, WRITE_LOCK, and HARD_LOCK.
+ *
+ * @return                Returns SUCCESS (0), BUSY if DryIce is busy, STATE
+ *                        if DryIce is in the wrong state, FAIL if the key gen
+ *                        failed, HLOCK or SLOCK if the key registers are
+ *                        locked, and WRITE if a write error occurs
+ *                        (See #di_return_t).
+ */
+extern di_return_t dryice_set_random_key(int flags);
+
+/*!
+ * Set the key selection in DryIce to determine the key used by an
+ * encryption module such as SCC. The selection is held until a call to the
+ * Release Selected Key interface is made, or until the appropriate HW
+ * reset if the LOCK flags are used.
+ *
+ * @param[in]   key       The source of the key to be used by the SCC
+ *                        (See #di_key_t).
+ *
+ * @param[in]  flags      This is a bit-wise OR of the flags to be passed
+ *                        to the function.  Flags can include:
+ *                        ASYNC, WRITE_LOCK, and HARD_LOCK.
+ *
+ * @return                Returns SUCCESS (0), BUSY if DryIce is busy, INVAL
+ *                        on invalid arguments, INUSE if a selection has already
+ *                        been made, STATE if DryIce is in the wrong state,
+ *                        HLOCK or SLOCK if the selection register is locked,
+ *                        and WRITE if a write error occurs
+ */
+extern di_return_t dryice_select_key(di_key_t key, int flags);
+
+/*!
+ * Check which key will be used in the SCC. This is needed because in some
+ * DryIce states, the Key Select Register is overridden by a default value
+ * (the Fused/IIM key).
+ *
+ * @param[out] key        The source of the key that is currently selected for
+ *                        use by the SCC.  This may be different from the key
+ *                        specified by the dryice_select_key function
+ *                        (See #di_key_t).  This value is set even if an error
+ *                        code (except for BUSY) is returned.
+ *
+ * @return                Returns SUCCESS (0), BUSY if DryIce is busy, STATE if
+ *                        DryIce is in the wrong state, INVAL on invalid
+ *                        arguments, or UNSET if no key has been selected
+ *                        (See #di_return_t).
+ */
+extern di_return_t dryice_check_key(di_key_t *key);
+
+/*!
+ * Allow the dryice_select_key interface to be used to set a new key selection
+ * in DryIce. Note that this interface does not overwrite the value in DryIce.
+ *
+ * @return                Returns SUCCESS (0), BUSY if DryIce is busy, UNSET
+ *                        if the no selection has been made previously, and
+ *                        HLOCK or SLOCK if the selection register is locked
+ *                        (See #di_return_t).
+ */
+extern di_return_t dryice_release_key_selection(void);
+
+/*!
+ * Returns tamper-detection status bits. Also an optional timestamp when
+ * DryIce is in the Non-valid state. If DryIce is not in Failure or Non-valid
+ * state, this interface returns a failure code.
+ *
+ * @param[out] events     This is a bit-wise OR of the following events:
+ *                        WTD (Wire Mesh), ETBD (External Tamper B),
+ *                        ETAD (External Tamper A), EBD (External Boot),
+ *                        SAD (Security Alarm), TTD (Temperature Tamper),
+ *                        CTD (Clock Tamper), VTD (Voltage Tamper),
+ *                        MCO (Monolithic Counter Overflow), and
+ *                        TCO (Time Counter Overflow).
+ *
+ * @param[out] timestamp  This is the value of the time counter in seconds
+ *                        when the tamper occurred.  A timestamp will not be
+ *                        returned if a NULL pointer is specified.  If DryIce
+ *                        is not in the Non-valid state the time cannot be
+ *                        read, so a timestamp of 0 will be returned.
+ *
+ * @param[in]  flags      This is a bit-wise OR of the flags to be passed
+ *                        to the function.  Flags is ignored currently by
+ *                        this function.
+ *
+ * @return                Returns SUCCESS (0), BUSY if DryIce is busy, and
+ *                        INVAL on invalid arguments (See #di_return_t).
+ */
+extern di_return_t
+dryice_get_tamper_event(uint32_t *events, uint32_t *timestamp, int flags);
+
+/*!
+ * Provide a callback function to be called upon the completion of DryIce calls
+ * that are executed asynchronously.
+ *
+ * @param[in]  func       This is a pointer to a function of type:
+ *                        void callback(di_return_t rc, unsigned long cookie)
+ *                        The return code of the async function is passed
+ *                        back in "rc" along with the cookie provided when
+ *                        registering the callback.
+ *
+ * @param[in]  cookie     This is an "opaque" cookie of type unsigned long that
+ *                        is returned on subsequent callbacks.  It may be of any
+ *                        value.
+ *
+ * @return                Returns SUCCESS (0), or BUSY if DryIce is busy
+ *                        (See #di_return_t).
+ */
+extern di_return_t dryice_register_callback(void (*func)(di_return_t rc,
+							 unsigned long cookie),
+					    unsigned long cookie);
+
+#endif /* __DRYICE_H__ */
diff --git a/drivers/mxc/security/mxc_scc.c b/drivers/mxc/security/mxc_scc.c
new file mode 100644
index 000000000000..82f971d3454b
--- /dev/null
+++ b/drivers/mxc/security/mxc_scc.c
@@ -0,0 +1,2386 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_scc.c
+ *
+ * This is the driver code for the Security Controller (SCC).  It has no device
+ * driver interface, so no user programs may access it.  Its interaction with
+ * the Linux kernel is from calls to #scc_init() when the driver is loaded, and
+ * #scc_cleanup() should the driver be unloaded.  The driver uses locking and
+ * (task-sleep/task-wakeup) functions of the kernel.  It also registers itself
+ * to handle the interrupt line(s) from the SCC.
+ *
+ * Other drivers in the kernel may use the remaining API functions to get at
+ * the services of the SCC.  The main service provided is the Secure Memory,
+ * which allows encoding and decoding of secrets with a per-chip secret key.
+ *
+ * The SCC is single-threaded, and so is this module.  When the scc_crypt()
+ * routine is called, it will lock out other accesses to the function.  If
+ * another task is already in the module, the subsequent caller will spin on a
+ * lock waiting for the other access to finish.
+ *
+ * Note that long crypto operations could cause a task to spin for a while,
+ * preventing other kernel work (other than interrupt processing) to get done.
+ *
+ * The external (kernel module) interface is through the following functions:
+ * @li scc_get_configuration()
+ * @li scc_crypt()
+ * @li scc_zeroize_memories()
+ * @li scc_monitor_security_failure()
+ * @li scc_stop_monitoring_security_failure()
+ * @li scc_set_sw_alarm()
+ * @li scc_read_register()
+ * @li scc_write_register()
+ *
+ * All other functions are internal to the driver.
+ *
+ * @ingroup MXCSCC
+*/
+#include "sahara2/include/fsl_platform.h"
+#include "sahara2/include/portable_os.h"
+#include "mxc_scc_internals.h"
+
+#include <linux/delay.h>
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,18))
+
+#include <linux/device.h>
+#include <mach/clock.h>
+#include <linux/device.h>
+
+#else
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+
+#endif
+
+/*!
+ * This is the set of errors which signal that access to the SCM RAM has
+ * failed or will fail.
+ */
+#define SCM_ACCESS_ERRORS                                                  \
+       (SCM_ERR_USER_ACCESS | SCM_ERR_ILLEGAL_ADDRESS |                    \
+        SCM_ERR_ILLEGAL_MASTER | SCM_ERR_CACHEABLE_ACCESS |                \
+        SCM_ERR_UNALIGNED_ACCESS | SCM_ERR_BYTE_ACCESS |                   \
+        SCM_ERR_INTERNAL_ERROR | SCM_ERR_SMN_BLOCKING_ACCESS |             \
+        SCM_ERR_CIPHERING | SCM_ERR_ZEROIZING | SCM_ERR_BUSY)
+/******************************************************************************
+ *
+ *  Global / Static Variables
+ *
+ *****************************************************************************/
+
+/*!
+ * This is type void* so that a) it cannot directly be dereferenced,
+ * and b) pointer arithmetic on it will function in a 'normal way' for
+ * the offsets in scc_defines.h
+ *
+ * scc_base is the location in the iomap where the SCC's registers
+ * (and memory) start.
+ *
+ * The referenced data is declared volatile so that the compiler will
+ * not make any assumptions about the value of registers in the SCC,
+ * and thus will always reload the register into CPU memory before
+ * using it (i.e. wherever it is referenced in the driver).
+ *
+ * This value should only be referenced by the #SCC_READ_REGISTER and
+ * #SCC_WRITE_REGISTER macros and their ilk.  All dereferences must be
+ * 32 bits wide.
+ */
+static volatile void *scc_base;
+
+/*! Array to hold function pointers registered by
+    #scc_monitor_security_failure() and processed by
+    #scc_perform_callbacks() */
+static void (*scc_callbacks[SCC_CALLBACK_SIZE]) (void);
+
+/*! Structure returned by #scc_get_configuration() */
+static scc_config_t scc_configuration = {
+	.driver_major_version = SCC_DRIVER_MAJOR_VERSION_1,
+	.driver_minor_version = SCC_DRIVER_MINOR_VERSION_8,
+	.scm_version = -1,
+	.smn_version = -1,
+	.block_size_bytes = -1,
+	.black_ram_size_blocks = -1,
+	.red_ram_size_blocks = -1
+};
+
+/*! Key Control Information.  Integrity is controlled by use of
+    #scc_crypto_lock. */
+static struct scc_key_slot scc_key_info[SCC_KEY_SLOTS];
+
+/*! Internal flag to know whether SCC is in Failed state (and thus many
+ *  registers are unavailable).  Once it goes failed, it never leaves it. */
+static volatile enum scc_status scc_availability = SCC_STATUS_INITIAL;
+
+/*! Flag to say whether interrupt handler has been registered for
+ * SMN interrupt */
+static int smn_irq_set = 0;
+
+/*! Flag to say whether interrupt handler has been registered for
+ * SCM interrupt */
+static int scm_irq_set = 0;
+
+/*! This lock protects the #scc_callbacks list as well as the @c
+ * callbacks_performed flag in #scc_perform_callbacks.  Since the data this
+ * protects may be read or written from either interrupt or base level, all
+ * operations should use the irqsave/irqrestore or similar to make sure that
+ * interrupts are inhibited when locking from base level.
+ */
+static spinlock_t scc_callbacks_lock = SPIN_LOCK_UNLOCKED;
+
+/*!
+ * Ownership of this lock prevents conflicts on the crypto operation in the SCC
+ * and the integrity of the #scc_key_info.
+ */
+static spinlock_t scc_crypto_lock = SPIN_LOCK_UNLOCKED;
+
+/*! Calculated once for quick reference to size of the unreserved space in one
+ *  RAM in SCM.
+ */
+static uint32_t scc_memory_size_bytes;
+
+/*! Calculated once for quick reference to size of SCM address space */
+static uint32_t scm_highest_memory_address;
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,18))
+#ifndef SCC_CLOCK_NOT_GATED
+/*! Pointer to SCC's clock information.  Initialized during scc_init(). */
+static struct clk *scc_clk = NULL;
+#endif
+#endif
+
+/*! The lookup table for an 8-bit value.  Calculated once
+ * by #scc_init_ccitt_crc().
+ */
+static uint16_t scc_crc_lookup_table[256];
+
+/*! Fixed padding for appending to plaintext to fill out a block */
+static uint8_t scc_block_padding[8] =
+    { SCC_DRIVER_PAD_CHAR, 0, 0, 0, 0, 0, 0, 0 };
+
+/******************************************************************************
+ *
+ *  Function Implementations - Externally Accessible
+ *
+ *****************************************************************************/
+
+/*****************************************************************************/
+/* fn scc_init()                                                             */
+/*****************************************************************************/
+/*!
+ *  Initialize the driver at boot time or module load time.
+ *
+ *  Register with the kernel as the interrupt handler for the SCC interrupt
+ *  line(s).
+ *
+ *  Map the SCC's register space into the driver's memory space.
+ *
+ *  Query the SCC for its configuration and status.  Save the configuration in
+ *  #scc_configuration and save the status in #scc_availability.  Called by the
+ *  kernel.
+ *
+ *  Do any locking/wait queue initialization which may be necessary.
+ *
+ *  The availability fuse may be checked, depending on platform.
+ */
+static int scc_init(void)
+{
+	uint32_t smn_status;
+	int i;
+	int return_value = -EIO;	/* assume error */
+	if (scc_availability == SCC_STATUS_INITIAL) {
+
+		/* Set this until we get an initial reading */
+		scc_availability = SCC_STATUS_CHECKING;
+
+		/* Initialize the constant for the CRC function */
+		scc_init_ccitt_crc();
+
+		/* initialize the callback table */
+		for (i = 0; i < SCC_CALLBACK_SIZE; i++) {
+			scc_callbacks[i] = 0;
+		}
+
+		/* Initialize key slots */
+		for (i = 0; i < SCC_KEY_SLOTS; i++) {
+			scc_key_info[i].offset = i * SCC_KEY_SLOT_SIZE;
+			scc_key_info[i].status = 0;	/* unassigned */
+		}
+
+		/* Enable the SCC clock on platforms where it is gated */
+#ifndef SCC_CLOCK_NOT_GATED
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,18))
+		mxc_clks_enable(SCC_CLK);
+#else
+
+		scc_clk = clk_get(NULL, "scc_clk");
+		if (scc_clk != ERR_PTR(ENOENT)) {
+			clk_enable(scc_clk);
+		}
+#endif				/* LINUX_VERSION_CODE */
+
+#endif				/* SCC_CLOCK_NOT_GATED */
+		/* See whether there is an SCC available */
+		if (0 && !SCC_ENABLED()) {
+			os_printk(KERN_ERR
+				  "SCC: Fuse for SCC is set to disabled.  Exiting.\n");
+		} else {
+			/* Map the SCC (SCM and SMN) memory on the internal bus into
+			   kernel address space */
+			scc_base = (void *)IO_ADDRESS(SCC_BASE);
+
+			/* If that worked, we can try to use the SCC */
+			if (scc_base == NULL) {
+				os_printk(KERN_ERR
+					  "SCC: Register mapping failed.  Exiting.\n");
+			} else {
+				/* Get SCM into 'clean' condition w/interrupts cleared &
+				   disabled */
+				SCC_WRITE_REGISTER(SCM_INTERRUPT_CTRL,
+						   SCM_INTERRUPT_CTRL_CLEAR_INTERRUPT
+						   |
+						   SCM_INTERRUPT_CTRL_MASK_INTERRUPTS);
+
+				/* Clear error status register (any write will do it) */
+				SCC_WRITE_REGISTER(SCM_ERROR_STATUS, 0);
+
+				/*
+				 * There is an SCC.  Determine its current state.  Side effect
+				 * is to populate scc_config and scc_availability
+				 */
+				smn_status = scc_grab_config_values();
+
+				/* Try to set up interrupt handler(s) */
+				if (scc_availability == SCC_STATUS_OK) {
+					if (setup_interrupt_handling() != 0) {
+						unsigned condition;
+			/*!
+			 * The error could be only that the SCM interrupt was
+			 * not set up.  This interrupt is always masked, so
+			 * that is not an issue.
+			 *
+			 * The SMN's interrupt may be shared on that line, it
+			 * may be separate, or it may not be wired.  Do what
+			 * is necessary to check its status.
+			 *
+			 * Although the driver is coded for possibility of not
+			 * having SMN interrupt, the fact that there is one
+			 * means it should be available and used.
+			 */
+#ifdef USE_SMN_INTERRUPT
+						condition = !smn_irq_set;	/* Separate. Check SMN binding */
+#elif !defined(NO_SMN_INTERRUPT)
+						condition = !scm_irq_set;	/* Shared. Check SCM binding */
+#else
+						condition = FALSE;	/*  SMN not wired at all.  Ignore. */
+#endif
+						/* setup was not able to set up SMN interrupt */
+						scc_availability =
+						    SCC_STATUS_UNIMPLEMENTED;
+					}	/* interrupt handling returned non-zero */
+				}	/* availability is OK */
+				if (scc_availability == SCC_STATUS_OK) {
+					/* Get SMN into 'clean' condition w/interrupts cleared &
+					   enabled */
+					SCC_WRITE_REGISTER(SMN_COMMAND,
+							   SMN_COMMAND_CLEAR_INTERRUPT
+							   |
+							   SMN_COMMAND_ENABLE_INTERRUPT);
+				}
+				/* availability is still OK */
+			}	/* if scc_base != NULL */
+
+		}		/* if SCC_ENABLED() */
+
+		/*
+		 * If status is SCC_STATUS_UNIMPLEMENTED or is still
+		 * SCC_STATUS_CHECKING, could be leaving here with the driver partially
+		 * initialized.  In either case, cleanup (which will mark the SCC as
+		 * UNIMPLEMENTED).
+		 */
+		if (scc_availability == SCC_STATUS_CHECKING ||
+		    scc_availability == SCC_STATUS_UNIMPLEMENTED) {
+			scc_cleanup();
+		} else {
+			return_value = 0;	/* All is well */
+		}
+	}
+	/* ! STATUS_INITIAL */
+	pr_debug("SCC: Driver Status is %s\n",
+		 (scc_availability == SCC_STATUS_INITIAL) ? "INITIAL" :
+		 (scc_availability == SCC_STATUS_CHECKING) ? "CHECKING" :
+		 (scc_availability ==
+		  SCC_STATUS_UNIMPLEMENTED) ? "UNIMPLEMENTED"
+		 : (scc_availability ==
+		    SCC_STATUS_OK) ? "OK" : (scc_availability ==
+					     SCC_STATUS_FAILED) ? "FAILED" :
+		 "UNKNOWN");
+
+	return return_value;
+}				/* scc_init */
+
+/*****************************************************************************/
+/* fn scc_cleanup()                                                          */
+/*****************************************************************************/
+/*!
+ * Perform cleanup before driver/module is unloaded by setting the machine
+ * state close to what it was when the driver was loaded.  This function is
+ * called when the kernel is shutting down or when this driver is being
+ * unloaded.
+ *
+ * A driver like this should probably never be unloaded, especially if there
+ * are other module relying upon the callback feature for monitoring the SCC
+ * status.
+ *
+ * In any case, cleanup the callback table (by clearing out all of the
+ * pointers).  Deregister the interrupt handler(s).  Unmap SCC registers.
+ *
+ */
+static void scc_cleanup(void)
+{
+	int i;
+
+	/* Mark the driver / SCC as unusable. */
+	scc_availability = SCC_STATUS_UNIMPLEMENTED;
+
+	/* Clear out callback table */
+	for (i = 0; i < SCC_CALLBACK_SIZE; i++) {
+		scc_callbacks[i] = 0;
+	}
+
+	/* If SCC has been mapped in, clean it up and unmap it */
+	if (scc_base) {
+		/* For the SCM, disable interrupts, zeroize RAMs.  Interrupt
+		 * status will appear because zeroize will complete. */
+		SCC_WRITE_REGISTER(SCM_INTERRUPT_CTRL,
+				   SCM_INTERRUPT_CTRL_MASK_INTERRUPTS |
+				   SCM_INTERRUPT_CTRL_ZEROIZE_MEMORY);
+
+		/* For the SMN, clear and disable interrupts */
+		SCC_WRITE_REGISTER(SMN_COMMAND, SMN_COMMAND_CLEAR_INTERRUPT);
+
+		/* remove virtual mapping */
+		iounmap((void *)scc_base);
+	}
+
+	/* Now that interrupts cannot occur, disassociate driver from the interrupt
+	 * lines.
+	 */
+
+	/* Deregister SCM interrupt handler */
+	if (scm_irq_set) {
+		os_deregister_interrupt(INT_SCC_SCM);
+	}
+
+	/* Deregister SMN interrupt handler */
+	if (smn_irq_set) {
+#ifdef USE_SMN_INTERRUPT
+		os_deregister_interrupt(INT_SCC_SMN);
+#endif
+	}
+	pr_debug("SCC driver cleaned up.\n");
+
+}				/* scc_cleanup */
+
+/*****************************************************************************/
+/* fn scc_get_configuration()                                                */
+/*****************************************************************************/
+scc_config_t *scc_get_configuration(void)
+{
+	/*
+	 * If some other driver calls scc before the kernel does, make sure that
+	 * this driver's initialization is performed.
+	 */
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+  /*!
+     * If there is no SCC, yet the driver exists, the value -1 will be in
+     * the #scc_config_t fields for other than the driver versions.
+     */
+	return &scc_configuration;
+}				/* scc_get_configuration */
+
+/*****************************************************************************/
+/* fn scc_zeroize_memories()                                                 */
+/*****************************************************************************/
+scc_return_t scc_zeroize_memories(void)
+{
+	scc_return_t return_status = SCC_RET_FAIL;
+	uint32_t status;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	if (scc_availability == SCC_STATUS_OK) {
+		unsigned long irq_flags;	/* for IRQ save/restore */
+
+		/* Lock access to crypto memory of the SCC */
+		spin_lock_irqsave(&scc_crypto_lock, irq_flags);
+
+		/* Start the Zeroize by setting a bit in the SCM_INTERRUPT_CTRL
+		 * register */
+		SCC_WRITE_REGISTER(SCM_INTERRUPT_CTRL,
+				   SCM_INTERRUPT_CTRL_MASK_INTERRUPTS
+				   | SCM_INTERRUPT_CTRL_ZEROIZE_MEMORY);
+
+		scc_wait_completion();
+
+		/* Get any error info */
+		status = SCC_READ_REGISTER(SCM_ERROR_STATUS);
+
+		/* unlock the SCC */
+		spin_unlock_irqrestore(&scc_crypto_lock, irq_flags);
+
+		if (!(status & SCM_ERR_ZEROIZE_FAILED)) {
+			return_status = SCC_RET_OK;
+		} else {
+			pr_debug
+			    ("SCC: Zeroize failed.  SCM Error Status is 0x%08x\n",
+			     status);
+		}
+
+		/* Clear out any status. */
+		SCC_WRITE_REGISTER(SCM_INTERRUPT_CTRL,
+				   SCM_INTERRUPT_CTRL_CLEAR_INTERRUPT
+				   | SCM_INTERRUPT_CTRL_MASK_INTERRUPTS);
+
+		/* and any error status */
+		SCC_WRITE_REGISTER(SCM_ERROR_STATUS, 0);
+	}
+
+	return return_status;
+}				/* scc_zeroize_memories */
+
+/*****************************************************************************/
+/* fn scc_crypt()                                                            */
+/*****************************************************************************/
+scc_return_t
+scc_crypt(unsigned long count_in_bytes, const uint8_t * data_in,
+	  const uint8_t * init_vector,
+	  scc_enc_dec_t direction, scc_crypto_mode_t crypto_mode,
+	  scc_verify_t check_mode, uint8_t * data_out,
+	  unsigned long *count_out_bytes)
+
+{
+	scc_return_t return_code = SCC_RET_FAIL;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	(void)scc_update_state();	/* in case no interrupt line from SMN */
+
+	/* make initial error checks */
+	if (scc_availability != SCC_STATUS_OK
+	    || count_in_bytes == 0
+	    || data_in == 0
+	    || data_out == 0
+	    || (crypto_mode != SCC_CBC_MODE && crypto_mode != SCC_ECB_MODE)
+	    || (crypto_mode == SCC_CBC_MODE && init_vector == NULL)
+	    || (direction != SCC_ENCRYPT && direction != SCC_DECRYPT)
+	    || (check_mode == SCC_VERIFY_MODE_NONE &&
+		count_in_bytes % SCC_BLOCK_SIZE_BYTES() != 0)
+	    || (direction == SCC_DECRYPT &&
+		count_in_bytes % SCC_BLOCK_SIZE_BYTES() != 0)
+	    || (check_mode != SCC_VERIFY_MODE_NONE &&
+		check_mode != SCC_VERIFY_MODE_CCITT_CRC)) {
+		pr_debug
+		    ("SCC: scc_crypt() count_in_bytes_ok = %d; data_in_ok = %d;"
+		     " data_out_ok = %d; iv_ok = %d\n", !(count_in_bytes == 0),
+		     !(data_in == 0), !(data_out == 0),
+		     !(crypto_mode == SCC_CBC_MODE && init_vector == NULL));
+		pr_debug("SCC: scc_crypt() mode_ok=%d; direction_ok=%d;"
+			 " size_ok=%d, check_mode_ok=%d\n",
+			 !(crypto_mode != SCC_CBC_MODE
+			   && crypto_mode != SCC_ECB_MODE),
+			 !(direction != SCC_ENCRYPT
+			   && direction != SCC_DECRYPT),
+			 !((check_mode == SCC_VERIFY_MODE_NONE
+			    && count_in_bytes % SCC_BLOCK_SIZE_BYTES() != 0)
+			   || (direction == SCC_DECRYPT
+			       && count_in_bytes % SCC_BLOCK_SIZE_BYTES() !=
+			       0)), !(check_mode != SCC_VERIFY_MODE_NONE
+				      && check_mode !=
+				      SCC_VERIFY_MODE_CCITT_CRC));
+		pr_debug("SCC: scc_crypt() detected bad argument\n");
+	} else {
+		/* Start settings for write to SCM_CONTROL register  */
+		uint32_t scc_control = SCM_CONTROL_START_CIPHER;
+		unsigned long irq_flags;	/* for IRQ save/restore */
+
+		/* Lock access to crypto memory of the SCC */
+		spin_lock_irqsave(&scc_crypto_lock, irq_flags);
+
+		/* Special needs for CBC Mode */
+		if (crypto_mode == SCC_CBC_MODE) {
+			scc_control |= SCM_CBC_MODE;	/* change default of ECB */
+			/* Put in Initial Context.  Vector registers are contiguous */
+			copy_to_scc(init_vector, SCM_INIT_VECTOR_0,
+				    SCC_BLOCK_SIZE_BYTES(), NULL);
+		}
+
+		/* Fill the RED_START register */
+		SCC_WRITE_REGISTER(SCM_RED_START,
+				   SCM_NON_RESERVED_OFFSET /
+				   SCC_BLOCK_SIZE_BYTES());
+
+		/* Fill the BLACK_START register */
+		SCC_WRITE_REGISTER(SCM_BLACK_START,
+				   SCM_NON_RESERVED_OFFSET /
+				   SCC_BLOCK_SIZE_BYTES());
+
+		if (direction == SCC_ENCRYPT) {
+			/* Check for sufficient space in data_out */
+			if (check_mode == SCC_VERIFY_MODE_NONE) {
+				if (*count_out_bytes < count_in_bytes) {
+					return_code =
+					    SCC_RET_INSUFFICIENT_SPACE;
+				}
+			} else {	/* SCC_VERIFY_MODE_CCITT_CRC */
+				/* Calculate extra bytes needed for crc (2) and block
+				   padding */
+				int padding_needed =
+				    CRC_SIZE_BYTES + SCC_BLOCK_SIZE_BYTES() -
+				    ((count_in_bytes + CRC_SIZE_BYTES)
+				     % SCC_BLOCK_SIZE_BYTES());
+
+				/* Verify space is available */
+				if (*count_out_bytes <
+				    count_in_bytes + padding_needed) {
+					return_code =
+					    SCC_RET_INSUFFICIENT_SPACE;
+				}
+			}
+			/* If did not detect space error, do the encryption */
+			if (return_code != SCC_RET_INSUFFICIENT_SPACE) {
+				return_code =
+				    scc_encrypt(count_in_bytes, data_in,
+						scc_control, data_out,
+						check_mode ==
+						SCC_VERIFY_MODE_CCITT_CRC,
+						count_out_bytes);
+			}
+
+		}
+		/* direction == SCC_ENCRYPT */
+		else {		/* SCC_DECRYPT */
+			/* Check for sufficient space in data_out */
+			if (check_mode == SCC_VERIFY_MODE_NONE) {
+				if (*count_out_bytes < count_in_bytes) {
+					return_code =
+					    SCC_RET_INSUFFICIENT_SPACE;
+				}
+			} else {	/* SCC_VERIFY_MODE_CCITT_CRC */
+				/* Do initial check.  Assume last block (of padding) and CRC
+				 * will get stripped.  After decipher is done and padding is
+				 * removed, will know exact value.
+				 */
+				int possible_size =
+				    (int)count_in_bytes - CRC_SIZE_BYTES -
+				    SCC_BLOCK_SIZE_BYTES();
+				if ((int)*count_out_bytes < possible_size) {
+					pr_debug
+					    ("SCC: insufficient decrypt space %ld/%d.\n",
+					     *count_out_bytes, possible_size);
+					return_code =
+					    SCC_RET_INSUFFICIENT_SPACE;
+				}
+			}
+
+			/* If did not detect space error, do the decryption */
+			if (return_code != SCC_RET_INSUFFICIENT_SPACE) {
+				return_code =
+				    scc_decrypt(count_in_bytes, data_in,
+						scc_control, data_out,
+						check_mode ==
+						SCC_VERIFY_MODE_CCITT_CRC,
+						count_out_bytes);
+			}
+
+		}		/* SCC_DECRYPT */
+
+		/* unlock the SCC */
+		spin_unlock_irqrestore(&scc_crypto_lock, irq_flags);
+
+	}			/* else no initial error */
+
+	return return_code;
+}				/* scc_crypt */
+
+/*****************************************************************************/
+/* fn scc_set_sw_alarm()                                                     */
+/*****************************************************************************/
+void scc_set_sw_alarm(void)
+{
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	/* Update scc_availability based on current SMN status.  This might
+	 * perform callbacks.
+	 */
+	(void)scc_update_state();
+
+	/* if everything is OK, make it fail */
+	if (scc_availability == SCC_STATUS_OK) {
+
+		/* sound the alarm (and disable SMN interrupts */
+		SCC_WRITE_REGISTER(SMN_COMMAND, SMN_COMMAND_SET_SOFTWARE_ALARM);
+
+		scc_availability = SCC_STATUS_FAILED;	/* Remember what we've done */
+
+		/* In case SMN interrupt is not available, tell the world */
+		scc_perform_callbacks();
+	}
+
+	return;
+}				/* scc_set_sw_alarm */
+
+/*****************************************************************************/
+/* fn scc_monitor_security_failure()                                         */
+/*****************************************************************************/
+scc_return_t scc_monitor_security_failure(void callback_func(void))
+{
+	int i;
+	unsigned long irq_flags;	/* for IRQ save/restore */
+	scc_return_t return_status = SCC_RET_TOO_MANY_FUNCTIONS;
+	int function_stored = FALSE;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	/* Acquire lock of callbacks table.  Could be spin_lock_irq() if this
+	 * routine were just called from base (not interrupt) level
+	 */
+	spin_lock_irqsave(&scc_callbacks_lock, irq_flags);
+
+	/* Search through table looking for empty slot */
+	for (i = 0; i < SCC_CALLBACK_SIZE; i++) {
+		if (scc_callbacks[i] == callback_func) {
+			if (function_stored) {
+				/* Saved duplicate earlier.  Clear this later one. */
+				scc_callbacks[i] = NULL;
+			}
+			/* Exactly one copy is now stored */
+			return_status = SCC_RET_OK;
+			break;
+		} else if (scc_callbacks[i] == NULL && !function_stored) {
+			/* Found open slot.  Save it and remember */
+			scc_callbacks[i] = callback_func;
+			return_status = SCC_RET_OK;
+			function_stored = TRUE;
+		}
+	}
+
+	/* Free the lock */
+	spin_unlock_irqrestore(&scc_callbacks_lock, irq_flags);
+
+	return return_status;
+}				/* scc_monitor_security_failure */
+
+/*****************************************************************************/
+/* fn scc_stop_monitoring_security_failure()                                 */
+/*****************************************************************************/
+void scc_stop_monitoring_security_failure(void callback_func(void))
+{
+	unsigned long irq_flags;	/* for IRQ save/restore */
+	int i;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	/* Acquire lock of callbacks table.  Could be spin_lock_irq() if this
+	 * routine were just called from base (not interrupt) level
+	 */
+	spin_lock_irqsave(&scc_callbacks_lock, irq_flags);
+
+	/* Search every entry of the table for this function */
+	for (i = 0; i < SCC_CALLBACK_SIZE; i++) {
+		if (scc_callbacks[i] == callback_func) {
+			scc_callbacks[i] = NULL;	/* found instance - clear it out */
+			break;
+		}
+	}
+
+	/* Free the lock */
+	spin_unlock_irqrestore(&scc_callbacks_lock, irq_flags);
+
+	return;
+}				/* scc_stop_monitoring_security_failure */
+
+/*****************************************************************************/
+/* fn scc_read_register()                                                    */
+/*****************************************************************************/
+scc_return_t scc_read_register(int register_offset, uint32_t * value)
+{
+	scc_return_t return_status = SCC_RET_FAIL;
+	uint32_t smn_status;
+	uint32_t scm_status;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	/* First layer of protection -- completely unaccessible SCC */
+	if (scc_availability != SCC_STATUS_UNIMPLEMENTED) {
+
+		/* Second layer -- that offset is valid */
+		if (register_offset != SMN_BITBANK_DECREMENT &&	/* write only! */
+		    check_register_offset(register_offset) == SCC_RET_OK) {
+
+			/* Get current status / update local state */
+			smn_status = scc_update_state();
+			scm_status = SCC_READ_REGISTER(SCM_STATUS);
+
+			/*
+			 * Third layer - verify that the register being requested is
+			 * available in the current state of the SCC.
+			 */
+			if ((return_status =
+			     check_register_accessible(register_offset,
+						       smn_status,
+						       scm_status)) ==
+			    SCC_RET_OK) {
+				*value = SCC_READ_REGISTER(register_offset);
+			}
+		}
+	}
+
+	return return_status;
+}				/* scc_read_register */
+
+/*****************************************************************************/
+/* fn scc_write_register()                                                   */
+/*****************************************************************************/
+scc_return_t scc_write_register(int register_offset, uint32_t value)
+{
+	scc_return_t return_status = SCC_RET_FAIL;
+	uint32_t smn_status;
+	uint32_t scm_status;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	/* First layer of protection -- completely unaccessible SCC */
+	if (scc_availability != SCC_STATUS_UNIMPLEMENTED) {
+
+		/* Second layer -- that offset is valid */
+		if (!(register_offset == SCM_STATUS ||	/* These registers are */
+		      register_offset == SCM_CONFIGURATION ||	/*  Read Only */
+		      register_offset == SMN_BIT_COUNT ||
+		      register_offset == SMN_TIMER) &&
+		    check_register_offset(register_offset) == SCC_RET_OK) {
+
+			/* Get current status / update local state */
+			smn_status = scc_update_state();
+			scm_status = SCC_READ_REGISTER(SCM_STATUS);
+
+			/*
+			 * Third layer - verify that the register being requested is
+			 * available in the current state of the SCC.
+			 */
+			if (check_register_accessible
+			    (register_offset, smn_status, scm_status) == 0) {
+				SCC_WRITE_REGISTER(register_offset, value);
+				return_status = SCC_RET_OK;
+			}
+		}
+	}
+
+	return return_status;
+}				/* scc_write_register() */
+
+/******************************************************************************
+ *
+ *  Function Implementations - Internal
+ *
+ *****************************************************************************/
+
+/*****************************************************************************/
+/* fn scc_irq()                                                              */
+/*****************************************************************************/
+/*!
+ * This is the interrupt handler for the SCC.
+ *
+ * This function checks the SMN Status register to see whether it
+ * generated the interrupt, then it checks the SCM Status register to
+ * see whether it needs attention.
+ *
+ * If an SMN Interrupt is active, then the SCC state set to failure, and
+ * #scc_perform_callbacks() is invoked to notify any interested parties.
+ *
+ * The SCM Interrupt should be masked, as this driver uses polling to determine
+ * when the SCM has completed a crypto or zeroing operation.  Therefore, if the
+ * interrupt is active, the driver will just clear the interrupt and (re)mask.
+ *
+ */
+OS_DEV_ISR(scc_irq)
+{
+	uint32_t smn_status;
+	uint32_t scm_status;
+	int handled = 0;	/* assume interrupt isn't from SMN */
+#if defined(USE_SMN_INTERRUPT)
+	int smn_irq = INT_SCC_SMN;	/* SMN interrupt is on a line by itself */
+#elif defined (NO_SMN_INTERRUPT)
+	int smn_irq = -1;	/* not wired to CPU at all */
+#else
+	int smn_irq = INT_SCC_SCM;	/* SMN interrupt shares a line with SCM */
+#endif
+
+	/* Update current state... This will perform callbacks... */
+	smn_status = scc_update_state();
+
+	/* SMN is on its own interrupt line.  Verify the IRQ was triggered
+	 * before clearing the interrupt and marking it handled.  */
+	if ((os_dev_get_irq() == smn_irq) &&
+	    (smn_status & SMN_STATUS_SMN_STATUS_IRQ)) {
+		SCC_WRITE_REGISTER(SMN_COMMAND, SMN_COMMAND_CLEAR_INTERRUPT);
+		handled++;	/* tell kernel that interrupt was handled */
+	}
+
+	/* Check on the health of the SCM */
+	scm_status = SCC_READ_REGISTER(SCM_STATUS);
+
+	/* The driver masks interrupts, so this should never happen. */
+	if (os_dev_get_irq() == INT_SCC_SCM) {
+		/* but if it does, try to prevent it in the future */
+		SCC_WRITE_REGISTER(SCM_INTERRUPT_CTRL,
+				   SCM_INTERRUPT_CTRL_CLEAR_INTERRUPT
+				   | SCM_INTERRUPT_CTRL_MASK_INTERRUPTS);
+		handled++;
+	}
+
+	/* Any non-zero value of handled lets kernel know we got something */
+	return IRQ_RETVAL(handled);
+}
+
+/*****************************************************************************/
+/* fn scc_perform_callbacks()                                                */
+/*****************************************************************************/
+/*! Perform callbacks registered by #scc_monitor_security_failure().
+ *
+ *  Make sure callbacks only happen once...  Since there may be some reason why
+ *  the interrupt isn't generated, this routine could be called from base(task)
+ *  level.
+ *
+ *  One at a time, go through #scc_callbacks[] and call any non-null pointers.
+ */
+static void scc_perform_callbacks(void)
+{
+	static int callbacks_performed = 0;
+	unsigned long irq_flags;	/* for IRQ save/restore */
+	int i;
+
+	/* Acquire lock of callbacks table and callbacks_performed flag */
+	spin_lock_irqsave(&scc_callbacks_lock, irq_flags);
+
+	if (!callbacks_performed) {
+		callbacks_performed = 1;
+
+		/* Loop over all of the entries in the table */
+		for (i = 0; i < SCC_CALLBACK_SIZE; i++) {
+			/* If not null, ... */
+			if (scc_callbacks[i]) {
+				scc_callbacks[i] ();	/* invoke the callback routine */
+			}
+		}
+	}
+
+	spin_unlock_irqrestore(&scc_callbacks_lock, irq_flags);
+
+	return;
+}
+
+/*****************************************************************************/
+/* fn copy_to_scc()                                                          */
+/*****************************************************************************/
+/*!
+ *  Move data from possibly unaligned source and realign for SCC, possibly
+ *  while calculating CRC.
+ *
+ *  Multiple calls can be made to this routine (without intervening calls to
+ *  #copy_from_scc(), as long as the sum total of bytes copied is a multiple of
+ *  four (SCC native word size).
+ *
+ *  @param[in]     from        Location in memory
+ *  @param[out]    to          Location in SCC
+ *  @param[in]     count_bytes Number of bytes to copy
+ *  @param[in,out] crc         Pointer to CRC.  Initial value must be
+ *                             #CRC_CCITT_START if this is the start of
+ *                             message.  Output is the resulting (maybe
+ *                             partial) CRC.  If NULL, no crc is calculated.
+ *
+ * @return  Zero - success.  Non-zero - SCM status bits defining failure.
+ */
+static uint32_t
+copy_to_scc(const uint8_t * from, uint32_t to, unsigned long count_bytes,
+	    uint16_t * crc)
+{
+	int i;
+	uint32_t scm_word;
+	uint16_t current_crc = 0;	/* local copy for fast access */
+	uint32_t status;
+
+	pr_debug("SCC: copying %ld bytes to 0x%0x.\n", count_bytes, to);
+
+	status = SCC_READ_REGISTER(SCM_ERROR_STATUS) & SCM_ACCESS_ERRORS;
+	if (status != 0) {
+		pr_debug
+		    ("SCC copy_to_scc(): Error status detected (before copy):"
+		     " %08x\n", status);
+		/* clear out errors left behind by somebody else */
+		SCC_WRITE_REGISTER(SCM_ERROR_STATUS, status);
+	}
+
+	if (crc) {
+		current_crc = *crc;
+	}
+
+	/* Initialize value being built for SCM.  If we are starting 'clean',
+	 * set it to zero.  Otherwise pick up partial value which had been saved
+	 * earlier. */
+	if (SCC_BYTE_OFFSET(to) == 0) {
+		scm_word = 0;
+	} else {
+		scm_word = SCC_READ_REGISTER(SCC_WORD_PTR(to));	/* recover */
+	}
+
+	/* Now build up SCM words and write them out when each is full */
+	for (i = 0; i < count_bytes; i++) {
+		uint8_t byte = *from++;	/* value from plaintext */
+
+#if defined(__BIG_ENDIAN) || defined(FSL_HAVE_DRYICE)
+		scm_word = (scm_word << 8) | byte;	/* add byte to SCM word */
+#else
+		scm_word = (byte << 24) | (scm_word >> 8);
+#endif
+		/* now calculate CCITT CRC */
+		if (crc) {
+			CALC_CRC(byte, current_crc);
+		}
+
+		to++;		/* bump location in SCM */
+
+		/* check for full word */
+		if (SCC_BYTE_OFFSET(to) == 0) {
+			SCC_WRITE_REGISTER((uint32_t) (to - 4), scm_word);	/* write it out */
+		}
+	}
+
+	/* If at partial word after previous loop, save it in SCM memory for
+	   next time. */
+	if (SCC_BYTE_OFFSET(to) != 0) {
+		SCC_WRITE_REGISTER(SCC_WORD_PTR(to), scm_word);	/* save */
+	}
+
+	/* Copy CRC back */
+	if (crc) {
+		*crc = current_crc;
+	}
+
+	status = SCC_READ_REGISTER(SCM_ERROR_STATUS) & SCM_ACCESS_ERRORS;
+	if (status != 0) {
+		pr_debug("SCC copy_to_scc(): Error status detected: %08x\n",
+			 status);
+		/* Clear any/all bits. */
+		SCC_WRITE_REGISTER(SCM_ERROR_STATUS, status);
+	}
+	return status;
+}
+
+/*****************************************************************************/
+/* fn copy_from_scc()                                                        */
+/*****************************************************************************/
+/*!
+ *  Move data from aligned 32-bit source and place in (possibly unaligned)
+ *  target, and maybe calculate CRC at the same time.
+ *
+ *  Multiple calls can be made to this routine (without intervening calls to
+ *  #copy_to_scc(), as long as the sum total of bytes copied is be a multiple
+ *  of four.
+ *
+ *  @param[in]     from        Location in SCC
+ *  @param[out]    to          Location in memory
+ *  @param[in]     count_bytes Number of bytes to copy
+ *  @param[in,out] crc         Pointer to CRC.  Initial value must be
+ *                             #CRC_CCITT_START if this is the start of
+ *                             message.  Output is the resulting (maybe
+ *                             partial) CRC.  If NULL, crc is not calculated.
+ *
+ * @return  Zero - success.  Non-zero - SCM status bits defining failure.
+ */
+static uint32_t
+copy_from_scc(const uint32_t from, uint8_t * to, unsigned long count_bytes,
+	      uint16_t * crc)
+{
+	uint32_t running_from = from;
+	uint32_t scm_word;
+	uint16_t current_crc = 0;	/* local copy for fast access */
+	uint32_t status;
+	pr_debug("SCC: copying %ld bytes from 0x%x.\n", count_bytes, from);
+	status = SCC_READ_REGISTER(SCM_ERROR_STATUS) & SCM_ACCESS_ERRORS;
+	if (status != 0) {
+		pr_debug
+		    ("SCC copy_from_scc(): Error status detected (before copy):"
+		     " %08x\n", status);
+		/* clear out errors left behind by somebody else */
+		SCC_WRITE_REGISTER(SCM_ERROR_STATUS, status);
+	}
+
+	if (crc) {
+		current_crc = *crc;
+	}
+
+	/* Read word which is sitting in SCM memory.  Ignore byte offset */
+	scm_word = SCC_READ_REGISTER(SCC_WORD_PTR(running_from));
+
+	/* If necessary, move the 'first' byte into place */
+	if (SCC_BYTE_OFFSET(running_from) != 0) {
+#if defined(__BIG_ENDIAN) || defined(FSL_HAVE_DRYICE)
+		scm_word <<= 8 * SCC_BYTE_OFFSET(running_from);
+#else
+		scm_word >>= 8 * SCC_BYTE_OFFSET(running_from);
+#endif
+	}
+
+	/* Now build up SCM words and write them out when each is full */
+	while (count_bytes--) {
+		uint8_t byte;	/* value from plaintext */
+
+#if defined(__BIG_ENDIAN) || defined(FSL_HAVE_DRYICE)
+		byte = (scm_word & 0xff000000) >> 24;	/* pull byte out of SCM word */
+		scm_word <<= 8;	/* shift over to remove the just-pulled byte */
+#else
+		byte = (scm_word & 0xff);
+		scm_word >>= 8;	/* shift over to remove the just-pulled byte */
+#endif
+		*to++ = byte;	/* send byte to memory */
+
+		/* now calculate CRC */
+		if (crc) {
+			CALC_CRC(byte, current_crc);
+		}
+
+		running_from++;
+		/* check for empty word */
+		if (count_bytes && SCC_BYTE_OFFSET(running_from) == 0) {
+			/* read one in */
+			scm_word = SCC_READ_REGISTER((uint32_t) running_from);
+		}
+	}
+
+	if (crc) {
+		*crc = current_crc;
+	}
+
+	status = SCC_READ_REGISTER(SCM_ERROR_STATUS) & SCM_ACCESS_ERRORS;
+	if (status != 0) {
+		pr_debug("SCC copy_from_scc(): Error status detected: %08x\n",
+			 status);
+		/* Clear any/all bits. */
+		SCC_WRITE_REGISTER(SCM_ERROR_STATUS, status);
+	}
+
+	return status;
+}
+
+/*****************************************************************************/
+/* fn scc_strip_padding()                                                    */
+/*****************************************************************************/
+/*!
+ *  Remove padding from plaintext.  Search backwards for #SCC_DRIVER_PAD_CHAR,
+ *  verifying that each byte passed over is zero (0).  Maximum number of bytes
+ *  to examine is 8.
+ *
+ *  @param[in]     from           Pointer to byte after end of message
+ *  @param[out]    count_bytes_stripped Number of padding bytes removed by this
+ *                                      function.
+ *
+ *  @return   #SCC_RET_OK if all goes, well, #SCC_RET_FAIL if padding was
+ *            not present.
+*/
+static scc_return_t
+scc_strip_padding(uint8_t * from, unsigned *count_bytes_stripped)
+{
+	int i = SCC_BLOCK_SIZE_BYTES();
+	scc_return_t return_code = SCC_RET_VERIFICATION_FAILED;
+
+	/*
+	 * Search backwards looking for the magic marker.  If it isn't found,
+	 * make sure that a 0 byte is there in its place.  Stop after the maximum
+	 * amount of padding (8 bytes) has been searched);
+	 */
+	while (i-- > 0) {
+		if (*--from == SCC_DRIVER_PAD_CHAR) {
+			*count_bytes_stripped = SCC_BLOCK_SIZE_BYTES() - i;
+			return_code = SCC_RET_OK;
+			break;
+		} else if (*from != 0) {	/* if not marker, check for 0 */
+			pr_debug("SCC: Found non-zero interim pad: 0x%x\n",
+				 *from);
+			break;
+		}
+	}
+
+	return return_code;
+}
+
+/*****************************************************************************/
+/* fn scc_update_state()                                                     */
+/*****************************************************************************/
+/*!
+ * Make certain SCC is still running.
+ *
+ * Side effect is to update #scc_availability and, if the state goes to failed,
+ * run #scc_perform_callbacks().
+ *
+ * (If #SCC_BRINGUP is defined, bring SCC to secure state if it is found to be
+ * in health check state)
+ *
+ * @return Current value of #SMN_STATUS register.
+ */
+static uint32_t scc_update_state(void)
+{
+	uint32_t smn_status_register = SMN_STATE_FAIL;
+	int smn_state;
+
+	/* if FAIL or UNIMPLEMENTED, don't bother */
+	if (scc_availability == SCC_STATUS_CHECKING ||
+	    scc_availability == SCC_STATUS_OK) {
+
+		smn_status_register = SCC_READ_REGISTER(SMN_STATUS);
+		smn_state = smn_status_register & SMN_STATUS_STATE_MASK;
+
+#ifdef SCC_BRINGUP
+		/* If in Health Check while booting, try to 'bringup' to Secure mode */
+		if (scc_availability == SCC_STATUS_CHECKING &&
+		    smn_state == SMN_STATE_HEALTH_CHECK) {
+			/* Code up a simple algorithm for the ASC */
+			SCC_WRITE_REGISTER(SMN_SEQUENCE_START, 0xaaaa);
+			SCC_WRITE_REGISTER(SMN_SEQUENCE_END, 0x5555);
+			SCC_WRITE_REGISTER(SMN_SEQUENCE_CHECK, 0x5555);
+			/* State should be SECURE now */
+			smn_status_register = SCC_READ_REGISTER(SMN_STATUS);
+			smn_state = smn_status_register & SMN_STATUS_STATE_MASK;
+		}
+#endif
+
+		/*
+		 * State should be SECURE or NON_SECURE for operation of the part.  If
+		 * FAIL, mark failed (i.e. limited access to registers).  Any other
+		 * state, mark unimplemented, as the SCC is unuseable.
+		 */
+		if (smn_state == SMN_STATE_SECURE
+		    || smn_state == SMN_STATE_NON_SECURE) {
+			/* Healthy */
+			scc_availability = SCC_STATUS_OK;
+		} else if (smn_state == SMN_STATE_FAIL) {
+			scc_availability = SCC_STATUS_FAILED;	/* uh oh - unhealthy */
+			scc_perform_callbacks();
+			os_printk(KERN_ERR "SCC: SCC went into FAILED mode\n");
+		} else {
+			/* START, ZEROIZE RAM, HEALTH CHECK, or unknown */
+			scc_availability = SCC_STATUS_UNIMPLEMENTED;	/* unuseable */
+			os_printk(KERN_ERR "SCC: SCC declared UNIMPLEMENTED\n");
+		}
+	}
+	/* if availability is initial or ok */
+	return smn_status_register;
+}
+
+/*****************************************************************************/
+/* fn scc_init_ccitt_crc()                                                   */
+/*****************************************************************************/
+/*!
+ * Populate the partial CRC lookup table.
+ *
+ * @return   none
+ *
+ */
+static void scc_init_ccitt_crc(void)
+{
+	int dividend;		/* index for lookup table */
+	uint16_t remainder;	/* partial value for a given dividend */
+	int bit;		/* index into bits of a byte */
+
+	/*
+	 * Compute the remainder of each possible dividend.
+	 */
+	for (dividend = 0; dividend < 256; ++dividend) {
+		/*
+		 * Start with the dividend followed by zeros.
+		 */
+		remainder = dividend << (8);
+
+		/*
+		 * Perform modulo-2 division, a bit at a time.
+		 */
+		for (bit = 8; bit > 0; --bit) {
+			/*
+			 * Try to divide the current data bit.
+			 */
+			if (remainder & 0x8000) {
+				remainder = (remainder << 1) ^ CRC_POLYNOMIAL;
+			} else {
+				remainder = (remainder << 1);
+			}
+		}
+
+		/*
+		 * Store the result into the table.
+		 */
+		scc_crc_lookup_table[dividend] = remainder;
+	}
+
+}				/* scc_init_ccitt_crc() */
+
+/*****************************************************************************/
+/* fn grab_config_values()                                                   */
+/*****************************************************************************/
+/*!
+ * grab_config_values() will read the SCM Configuration and SMN Status
+ * registers and store away version and size information for later use.
+ *
+ * @return The current value of the SMN Status register.
+ */
+static uint32_t scc_grab_config_values(void)
+{
+	uint32_t config_register;
+	uint32_t smn_status_register = SMN_STATE_FAIL;
+
+	if (scc_availability != SCC_STATUS_UNIMPLEMENTED) {
+		/* access the SCC - these are 'safe' registers */
+		config_register = SCC_READ_REGISTER(SCM_CONFIGURATION);
+		pr_debug("SCC Driver: SCM config is 0x%08x\n", config_register);
+
+		/* Get SMN status and update scc_availability */
+		smn_status_register = scc_update_state();
+		pr_debug("SCC Driver: SMN status is 0x%08x\n",
+			 smn_status_register);
+
+		/* save sizes and versions information for later use */
+		scc_configuration.block_size_bytes = (config_register &
+						      SCM_CFG_BLOCK_SIZE_MASK)
+		    >> SCM_CFG_BLOCK_SIZE_SHIFT;
+
+		scc_configuration.red_ram_size_blocks = (config_register &
+							 SCM_CFG_RED_SIZE_MASK)
+		    >> SCM_CFG_RED_SIZE_SHIFT;
+
+		scc_configuration.black_ram_size_blocks = (config_register &
+							   SCM_CFG_BLACK_SIZE_MASK)
+		    >> SCM_CFG_BLACK_SIZE_SHIFT;
+
+		scc_configuration.scm_version = (config_register
+						 & SCM_CFG_VERSION_ID_MASK)
+		    >> SCM_CFG_VERSION_ID_SHIFT;
+
+		scc_configuration.smn_version = (smn_status_register &
+						 SMN_STATUS_VERSION_ID_MASK)
+		    >> SMN_STATUS_VERSION_ID_SHIFT;
+
+		if (scc_configuration.scm_version != SCM_VERSION_1) {
+			scc_availability = SCC_STATUS_UNIMPLEMENTED;	/* Unknown version */
+		}
+
+		scc_memory_size_bytes = (SCC_BLOCK_SIZE_BYTES() *
+					 scc_configuration.
+					 black_ram_size_blocks)
+		    - SCM_NON_RESERVED_OFFSET;
+
+		/* This last is for driver consumption only */
+		scm_highest_memory_address = SCM_BLACK_MEMORY +
+		    (SCC_BLOCK_SIZE_BYTES() *
+		     scc_configuration.black_ram_size_blocks);
+	}
+
+	return smn_status_register;
+}				/* grab_config_values */
+
+/*****************************************************************************/
+/* fn setup_interrupt_handling()                                             */
+/*****************************************************************************/
+/*!
+ * Register the SCM and SMN interrupt handlers.
+ *
+ * Called from #scc_init()
+ *
+ * @return 0 on success
+ */
+static int setup_interrupt_handling(void)
+{
+	int smn_error_code = -1;
+	int scm_error_code = -1;
+
+	/* Disnable SCM interrupts */
+	SCC_WRITE_REGISTER(SCM_INTERRUPT_CTRL,
+			   SCM_INTERRUPT_CTRL_CLEAR_INTERRUPT
+			   | SCM_INTERRUPT_CTRL_MASK_INTERRUPTS);
+
+#ifdef USE_SMN_INTERRUPT
+	/* Install interrupt service routine for SMN. */
+	smn_error_code = os_register_interrupt(SCC_DRIVER_NAME,
+					       INT_SCC_SMN, scc_irq);
+	if (smn_error_code != 0) {
+		os_printk
+		    ("SCC Driver: Error installing SMN Interrupt Handler: %d\n",
+		     smn_error_code);
+	} else {
+		smn_irq_set = 1;	/* remember this for cleanup */
+		/* Enable SMN interrupts */
+		SCC_WRITE_REGISTER(SMN_COMMAND,
+				   SMN_COMMAND_CLEAR_INTERRUPT |
+				   SMN_COMMAND_ENABLE_INTERRUPT);
+	}
+#else
+	smn_error_code = 0;	/* no problems... will handle later */
+#endif
+
+	/*
+	 * Install interrupt service routine for SCM (or both together).
+	 */
+	scm_error_code = os_register_interrupt(SCC_DRIVER_NAME,
+					       INT_SCC_SCM, scc_irq);
+	if (scm_error_code != 0) {
+#ifndef MXC
+		os_printk
+		    ("SCC Driver: Error installing SCM Interrupt Handler: %d\n",
+		     scm_error_code);
+#else
+		os_printk
+		    ("SCC Driver: Error installing SCC Interrupt Handler: %d\n",
+		     scm_error_code);
+#endif
+	} else {
+		scm_irq_set = 1;	/* remember this for cleanup */
+#if defined(USE_SMN_INTERRUPT) && !defined(NO_SMN_INTERRUPT)
+		/* Enable SMN interrupts */
+		SCC_WRITE_REGISTER(SMN_COMMAND,
+				   SMN_COMMAND_CLEAR_INTERRUPT |
+				   SMN_COMMAND_ENABLE_INTERRUPT);
+#endif
+	}
+
+	/* Return an error if one was encountered */
+	return scm_error_code ? scm_error_code : smn_error_code;
+}				/* setup_interrupt_handling */
+
+/*****************************************************************************/
+/* fn scc_do_crypto()                                                        */
+/*****************************************************************************/
+/*! Have the SCM perform the crypto function.
+ *
+ * Set up length register, and the store @c scm_control into control register
+ * to kick off the operation.  Wait for completion, gather status, clear
+ * interrupt / status.
+ *
+ * @param byte_count  number of bytes to perform in this operation
+ * @param scm_control Bit values to be set in @c SCM_CONTROL register
+ *
+ * @return 0 on success, value of #SCM_ERROR_STATUS on failure
+ */
+static uint32_t scc_do_crypto(int byte_count, uint32_t scm_control)
+{
+	int block_count = byte_count / SCC_BLOCK_SIZE_BYTES();
+	uint32_t crypto_status;
+
+	/* clear any outstanding flags */
+	SCC_WRITE_REGISTER(SCM_INTERRUPT_CTRL,
+			   SCM_INTERRUPT_CTRL_CLEAR_INTERRUPT
+			   | SCM_INTERRUPT_CTRL_MASK_INTERRUPTS);
+
+	/* In length register, 0 means 1, etc. */
+	SCC_WRITE_REGISTER(SCM_LENGTH, block_count - 1);
+
+	/* set modes and kick off the operation */
+	SCC_WRITE_REGISTER(SCM_CONTROL, scm_control);
+
+	scc_wait_completion();
+
+	/* Mask for done and error bits */
+	crypto_status = SCC_READ_REGISTER(SCM_STATUS)
+	    & (SCM_STATUS_CIPHERING_DONE
+	       | SCM_STATUS_LENGTH_ERROR | SCM_STATUS_INTERNAL_ERROR);
+
+	/* Only done bit should be on */
+	if (crypto_status != SCM_STATUS_CIPHERING_DONE) {
+		/* Replace with error status instead */
+		crypto_status = SCC_READ_REGISTER(SCM_ERROR_STATUS);
+		pr_debug("SCM Failure: 0x%x\n", crypto_status);
+		if (crypto_status == 0) {
+			/* That came up 0.  Turn on arbitrary bit to signal error. */
+			crypto_status = SCM_ERR_INTERNAL_ERROR;
+		}
+	} else {
+		crypto_status = 0;
+	}
+
+	pr_debug("SCC: Done waiting.\n");
+
+	/* Clear out any status. */
+	SCC_WRITE_REGISTER(SCM_INTERRUPT_CTRL,
+			   SCM_INTERRUPT_CTRL_CLEAR_INTERRUPT
+			   | SCM_INTERRUPT_CTRL_MASK_INTERRUPTS);
+
+	/* And clear any error status */
+	SCC_WRITE_REGISTER(SCM_ERROR_STATUS, 0);
+
+	return crypto_status;
+}
+
+/*****************************************************************************/
+/* fn scc_encrypt()                                                          */
+/*****************************************************************************/
+/*!
+ * Perform an encryption on the input.  If @c verify_crc is true, a CRC must be
+ * calculated on the plaintext, and appended, with padding, before computing
+ * the ciphertext.
+ *
+ * @param[in]     count_in_bytes  Count of bytes of plaintext
+ * @param[in]     data_in         Pointer to the plaintext
+ * @param[in]     scm_control     Bit values for the SCM_CONTROL register
+ * @param[in,out] data_out        Pointer for storing ciphertext
+ * @param[in]     add_crc         Flag for computing CRC - 0 no, else yes
+ * @param[in,out] count_out_bytes Number of bytes available at @c data_out
+ */
+static scc_return_t
+scc_encrypt(uint32_t count_in_bytes, const uint8_t * data_in,
+	    uint32_t scm_control,
+	    uint8_t * data_out, int add_crc, unsigned long *count_out_bytes)
+
+{
+	scc_return_t return_code = SCC_RET_FAIL;	/* initialised for failure */
+	uint32_t input_bytes_left = count_in_bytes;	/* local copy */
+	uint32_t output_bytes_copied = 0;	/* running total */
+	uint32_t bytes_to_process;	/* multi-purpose byte counter */
+	uint16_t crc = CRC_CCITT_START;	/* running CRC value */
+	crc_t *crc_ptr = NULL;	/* Reset if CRC required */
+	/* byte address  into SCM RAM */
+	uint32_t scm_location = SCM_RED_MEMORY + SCM_NON_RESERVED_OFFSET;
+	/* free RED RAM */
+	uint32_t scm_bytes_remaining = scc_memory_size_bytes;
+	/* CRC+padding holder */
+	uint8_t padding_buffer[PADDING_BUFFER_MAX_BYTES];
+	unsigned padding_byte_count = 0;	/* Reset if padding required */
+	uint32_t scm_error_status = 0;	/* No known SCM error initially */
+	uint32_t i; 	/* Counter for clear data loop */
+	uint32_t dirty_bytes;	/* Number of bytes of memory used
+					   temporarily during encryption,
+					   which need to be wiped after
+					   completion of the operation. */
+
+	/* Set location of CRC and prepare padding bytes if required */
+	if (add_crc != 0) {
+		crc_ptr = &crc;
+		padding_byte_count = SCC_BLOCK_SIZE_BYTES()
+		    - (count_in_bytes +
+		       CRC_SIZE_BYTES) % SCC_BLOCK_SIZE_BYTES();
+		memcpy(padding_buffer + CRC_SIZE_BYTES, scc_block_padding,
+		       padding_byte_count);
+	}
+
+	/* Process remaining input or padding data */
+	while (input_bytes_left > 0) {
+
+		/* Determine how much work to do this pass */
+		bytes_to_process = (input_bytes_left > scm_bytes_remaining) ?
+		    scm_bytes_remaining : input_bytes_left;
+
+		/* Copy plaintext into SCM RAM, calculating CRC if required */
+		copy_to_scc(data_in, scm_location, bytes_to_process, crc_ptr);
+
+		/* Adjust pointers & counters */
+		input_bytes_left -= bytes_to_process;
+		data_in += bytes_to_process;
+		scm_location += bytes_to_process;
+		scm_bytes_remaining -= bytes_to_process;
+
+		/* Add CRC and padding after the last byte is copied if required */
+		if ((input_bytes_left == 0) && (crc_ptr != NULL)) {
+
+			/* Copy CRC into padding buffer MSB first */
+			padding_buffer[0] = (crc >> 8) & 0xFF;
+			padding_buffer[1] = crc & 0xFF;
+
+			/* Reset pointers and counter */
+			data_in = padding_buffer;
+			input_bytes_left = CRC_SIZE_BYTES + padding_byte_count;
+			crc_ptr = NULL;	/* CRC no longer required */
+
+			/* Go round loop again to copy CRC and padding to SCM */
+			continue;
+		}
+
+		/* if no input and crc_ptr */
+		/* Now have block-sized plaintext in SCM to encrypt */
+		/* Encrypt plaintext; exit loop on error */
+		bytes_to_process = scm_location -
+		    (SCM_RED_MEMORY + SCM_NON_RESERVED_OFFSET);
+
+		if (output_bytes_copied + bytes_to_process > *count_out_bytes) {
+			return_code = SCC_RET_INSUFFICIENT_SPACE;
+			scm_error_status = -1;	/* error signal */
+			pr_debug
+			    ("SCC: too many ciphertext bytes for space available\n");
+			break;
+		}
+		pr_debug("SCC: Starting encryption. %x for %d bytes (%p/%p)\n",
+			 scm_control, bytes_to_process,
+			 (void *)SCC_READ_REGISTER(SCM_RED_START),
+			 (void *)SCC_READ_REGISTER(SCM_BLACK_START));
+		scm_error_status = scc_do_crypto(bytes_to_process, scm_control);
+		if (scm_error_status != 0) {
+			break;
+		}
+
+		/* Copy out ciphertext */
+		copy_from_scc(SCM_BLACK_MEMORY + SCM_NON_RESERVED_OFFSET,
+			      data_out, bytes_to_process, NULL);
+
+		/* Adjust pointers and counters for next loop */
+		output_bytes_copied += bytes_to_process;
+		data_out += bytes_to_process;
+		scm_location = SCM_RED_MEMORY + SCM_NON_RESERVED_OFFSET;
+		scm_bytes_remaining = scc_memory_size_bytes;
+
+	}			/* input_bytes_left > 0 */
+	/* Clear all red and black memory used during ephemeral encryption */
+	dirty_bytes = (count_in_bytes > scc_memory_size_bytes) ?
+			scc_memory_size_bytes : count_in_bytes;
+	
+	for (i = 0; i < dirty_bytes; i += 4) {
+		SCC_WRITE_REGISTER(SCM_RED_MEMORY + SCM_NON_RESERVED_OFFSET + i,
+					   0);
+		SCC_WRITE_REGISTER(SCM_BLACK_MEMORY + SCM_NON_RESERVED_OFFSET +
+					   i, 0);
+	}
+
+	/* If no SCM error, set OK status and save ouput byte count */
+	if (scm_error_status == 0) {
+		return_code = SCC_RET_OK;
+		*count_out_bytes = output_bytes_copied;
+	}
+
+	return return_code;
+}				/* scc_encrypt */
+
+/*****************************************************************************/
+/* fn scc_decrypt()                                                          */
+/*****************************************************************************/
+/*!
+ * Perform a decryption on the input.  If @c verify_crc is true, the last block
+ * (maybe the two last blocks) is special - it should contain a CRC and
+ * padding.  These must be stripped and verified.
+ *
+ * @param[in]     count_in_bytes  Count of bytes of ciphertext
+ * @param[in]     data_in         Pointer to the ciphertext
+ * @param[in]     scm_control     Bit values for the SCM_CONTROL register
+ * @param[in,out] data_out        Pointer for storing plaintext
+ * @param[in]     verify_crc      Flag for running CRC - 0 no, else yes
+ * @param[in,out] count_out_bytes Number of bytes available at @c data_out
+
+ */
+static scc_return_t
+scc_decrypt(uint32_t count_in_bytes, const uint8_t * data_in,
+	    uint32_t scm_control,
+	    uint8_t * data_out, int verify_crc, unsigned long *count_out_bytes)
+{
+	scc_return_t return_code = SCC_RET_FAIL;
+	uint32_t bytes_left = count_in_bytes;	/* local copy */
+	uint32_t bytes_copied = 0;	/* running total of bytes going to user */
+	uint32_t bytes_to_copy = 0;	/* Number in this encryption 'chunk' */
+	uint16_t crc = CRC_CCITT_START;	/* running CRC value */
+	/* next target for	ctext */
+	uint32_t scm_location = SCM_BLACK_MEMORY + SCM_NON_RESERVED_OFFSET;
+	unsigned padding_byte_count;	/* number of bytes of padding stripped */
+	uint8_t last_two_blocks[2 * SCC_BLOCK_SIZE_BYTES()];	/* temp */
+	uint32_t scm_error_status = 0;	/* register value */
+	uint32_t i; 	/* Counter for clear data loop */
+	uint32_t dirty_bytes;	/* Number of bytes of memory used
+					   temporarily during decryption,
+					   which need to be wiped after
+					   completion of the operation. */
+
+	scm_control |= SCM_DECRYPT_MODE;
+
+	if (verify_crc) {
+		/* Save last two blocks (if there are at least two) of ciphertext for
+		   special treatment. */
+		bytes_left -= SCC_BLOCK_SIZE_BYTES();
+		if (bytes_left >= SCC_BLOCK_SIZE_BYTES()) {
+			bytes_left -= SCC_BLOCK_SIZE_BYTES();
+		}
+	}
+
+	/* Copy ciphertext into SCM BLACK memory */
+	while (bytes_left && scm_error_status == 0) {
+
+		/* Determine how much work to do this pass */
+		if (bytes_left > (scc_memory_size_bytes)) {
+			bytes_to_copy = scc_memory_size_bytes;
+		} else {
+			bytes_to_copy = bytes_left;
+		}
+
+		if (bytes_copied + bytes_to_copy > *count_out_bytes) {
+			scm_error_status = -1;
+			break;
+		}
+		copy_to_scc(data_in, scm_location, bytes_to_copy, NULL);
+		data_in += bytes_to_copy;	/* move pointer */
+
+		pr_debug("SCC: Starting decryption of %d bytes.\n",
+			 bytes_to_copy);
+
+		/*  Do the work, wait for completion */
+		scm_error_status = scc_do_crypto(bytes_to_copy, scm_control);
+
+		copy_from_scc(SCM_RED_MEMORY + SCM_NON_RESERVED_OFFSET,
+			      data_out, bytes_to_copy, &crc);
+		bytes_copied += bytes_to_copy;
+		data_out += bytes_to_copy;
+		scm_location = SCM_BLACK_MEMORY + SCM_NON_RESERVED_OFFSET;
+
+		/* Do housekeeping */
+		bytes_left -= bytes_to_copy;
+
+	}			/* while bytes_left */
+
+	/* At this point, either the process is finished, or this is verify mode */
+
+	if (scm_error_status == 0) {
+		if (!verify_crc) {
+			*count_out_bytes = bytes_copied;
+			return_code = SCC_RET_OK;
+		} else {
+			/* Verify mode.  There are one or two blocks of unprocessed
+			 * ciphertext sitting at data_in.  They need to be moved to the
+			 * SCM, decrypted, searched to remove padding, then the plaintext
+			 * copied back to the user (while calculating CRC, of course).
+			 */
+
+			/* Calculate ciphertext still left */
+			bytes_to_copy = count_in_bytes - bytes_copied;
+
+			copy_to_scc(data_in, scm_location, bytes_to_copy, NULL);
+			data_in += bytes_to_copy;	/* move pointer */
+
+			pr_debug("SCC: Finishing decryption (%d bytes).\n",
+				 bytes_to_copy);
+
+			/*  Do the work, wait for completion */
+			scm_error_status =
+			    scc_do_crypto(bytes_to_copy, scm_control);
+
+			if (scm_error_status == 0) {
+				/* Copy decrypted data back from SCM RED memory */
+				copy_from_scc(SCM_RED_MEMORY +
+					      SCM_NON_RESERVED_OFFSET,
+					      last_two_blocks, bytes_to_copy,
+					      NULL);
+
+				/* (Plaintext) + crc + padding should be in temp buffer */
+				if (scc_strip_padding
+				    (last_two_blocks + bytes_to_copy,
+				     &padding_byte_count) == SCC_RET_OK) {
+					bytes_to_copy -=
+					    padding_byte_count + CRC_SIZE_BYTES;
+
+					/* verify enough space in user buffer */
+					if (bytes_copied + bytes_to_copy <=
+					    *count_out_bytes) {
+						int i = 0;
+
+						/* Move out last plaintext and calc CRC */
+						while (i < bytes_to_copy) {
+							CALC_CRC(last_two_blocks
+								 [i], crc);
+							*data_out++ =
+							    last_two_blocks
+							    [i++];
+							bytes_copied++;
+						}
+
+						/* Verify the CRC by running over itself */
+						CALC_CRC(last_two_blocks
+							 [bytes_to_copy], crc);
+						CALC_CRC(last_two_blocks
+							 [bytes_to_copy + 1],
+							 crc);
+						if (crc == 0) {
+							/* Just fine ! */
+							*count_out_bytes =
+							    bytes_copied;
+							return_code =
+							    SCC_RET_OK;
+						} else {
+							return_code =
+							    SCC_RET_VERIFICATION_FAILED;
+							pr_debug
+							    ("SCC:  CRC values are %04x, %02x%02x\n",
+							     crc,
+							     last_two_blocks
+							     [bytes_to_copy],
+							     last_two_blocks
+							     [bytes_to_copy +
+							      1]);
+						}
+					}	/* if space available */
+				} /* if scc_strip_padding... */
+				else {
+					/* bad padding means bad verification */
+					return_code =
+					    SCC_RET_VERIFICATION_FAILED;
+				}
+			}
+			/* scm_error_status == 0 */
+		}		/* verify_crc */
+	}
+
+	/* scm_error_status == 0 */
+	/* Clear all red and black memory used during ephemeral decryption */
+	dirty_bytes = (count_in_bytes > scc_memory_size_bytes) ?
+	    scc_memory_size_bytes : count_in_bytes;
+
+	for (i = 0; i < dirty_bytes; i += 4) {
+		SCC_WRITE_REGISTER(SCM_RED_MEMORY + SCM_NON_RESERVED_OFFSET + i,
+				   0);
+		SCC_WRITE_REGISTER(SCM_BLACK_MEMORY + SCM_NON_RESERVED_OFFSET +
+				   i, 0);
+	}
+	return return_code;
+}				/* scc_decrypt */
+
+/*****************************************************************************/
+/* fn scc_alloc_slot()                                                       */
+/*****************************************************************************/
+/*!
+ * Allocate a key slot to fit the requested size.
+ *
+ * @param value_size_bytes   Size of the key or other secure data
+ * @param owner_id           Value to tie owner to slot
+ * @param[out] slot          Handle to access or deallocate slot
+ *
+ * @return SCC_RET_OK on success, SCC_RET_INSUFFICIENT_SPACE if not slots of
+ *         requested size are available.
+ */
+scc_return_t
+scc_alloc_slot(uint32_t value_size_bytes, uint64_t owner_id, uint32_t * slot)
+{
+	scc_return_t status = SCC_RET_FAIL;
+	unsigned long irq_flags;
+
+	if (scc_availability != SCC_STATUS_OK) {
+		goto out;
+	}
+	/* ACQUIRE LOCK to prevent others from using SCC crypto */
+	spin_lock_irqsave(&scc_crypto_lock, irq_flags);
+
+	pr_debug("SCC: Allocating %d-byte slot for 0x%Lx\n",
+		 value_size_bytes, owner_id);
+
+	if ((value_size_bytes != 0) && (value_size_bytes <= SCC_MAX_KEY_SIZE)) {
+		int i;
+
+		for (i = 0; i < SCC_KEY_SLOTS; i++) {
+			if (scc_key_info[i].status == 0) {
+				scc_key_info[i].owner_id = owner_id;
+				scc_key_info[i].length = value_size_bytes;
+				scc_key_info[i].status = 1;	/* assigned! */
+				*slot = i;
+				status = SCC_RET_OK;
+				break;	/* exit 'for' loop */
+			}
+		}
+
+		if (status != SCC_RET_OK) {
+			status = SCC_RET_INSUFFICIENT_SPACE;
+		} else {
+			pr_debug("SCC: Allocated slot %d (0x%Lx)\n", i,
+				 owner_id);
+		}
+	}
+
+	spin_unlock_irqrestore(&scc_crypto_lock, irq_flags);
+
+      out:
+	return status;
+}
+
+/*****************************************************************************/
+/* fn verify_slot_access()                                                   */
+/*****************************************************************************/
+inline static scc_return_t
+verify_slot_access(uint64_t owner_id, uint32_t slot, uint32_t access_len)
+{
+	scc_return_t status = SCC_RET_FAIL;
+	if (scc_availability != SCC_STATUS_OK) {
+		goto out;
+	}
+
+	if ((slot < SCC_KEY_SLOTS) && scc_key_info[slot].status
+	    && (scc_key_info[slot].owner_id == owner_id)
+	    && (access_len <= SCC_KEY_SLOT_SIZE)) {
+		status = SCC_RET_OK;
+		pr_debug("SCC: Verify on slot %d succeeded\n", slot);
+	} else {
+		if (slot >= SCC_KEY_SLOTS) {
+			pr_debug("SCC: Verify on bad slot (%d) failed\n", slot);
+		} else if (scc_key_info[slot].status) {
+			pr_debug("SCC: Verify on slot %d failed (%Lx) \n", slot,
+				 owner_id);
+		} else {
+			pr_debug
+			    ("SCC: Verify on slot %d failed: not allocated\n",
+			     slot);
+		}
+	}
+
+      out:
+	return status;
+}
+
+scc_return_t
+scc_verify_slot_access(uint64_t owner_id, uint32_t slot, uint32_t access_len)
+{
+	return verify_slot_access(owner_id, slot, access_len);
+}
+
+/*****************************************************************************/
+/* fn scc_dealloc_slot()                                                     */
+/*****************************************************************************/
+scc_return_t scc_dealloc_slot(uint64_t owner_id, uint32_t slot)
+{
+	scc_return_t status;
+	unsigned long irq_flags;
+	int i;
+
+	/* ACQUIRE LOCK to prevent others from using SCC crypto */
+	spin_lock_irqsave(&scc_crypto_lock, irq_flags);
+
+	status = verify_slot_access(owner_id, slot, 0);
+
+	if (status == SCC_RET_OK) {
+		scc_key_info[slot].owner_id = 0;
+		scc_key_info[slot].status = 0;	/* unassign */
+
+		/* clear old info */
+		for (i = 0; i < SCC_KEY_SLOT_SIZE; i += 4) {
+			SCC_WRITE_REGISTER(SCM_RED_MEMORY +
+					   scc_key_info[slot].offset + i, 0);
+			SCC_WRITE_REGISTER(SCM_BLACK_MEMORY +
+					   scc_key_info[slot].offset + i, 0);
+		}
+		pr_debug("SCC: Deallocated slot %d\n", slot);
+	}
+
+	spin_unlock_irqrestore(&scc_crypto_lock, irq_flags);
+
+	return status;
+}
+
+/*****************************************************************************/
+/* fn scc_load_slot()                                                        */
+/*****************************************************************************/
+/*!
+ * Load a value into a slot.
+ *
+ * @param owner_id      Value of owner of slot
+ * @param slot          Handle of slot
+ * @param key_data      Data to load into the slot
+ * @param key_length    Length, in bytes, of @c key_data to copy to SCC.
+ *
+ * @return SCC_RET_OK on success.  SCC_RET_FAIL will be returned if slot
+ * specified cannot be accessed for any reason, or SCC_RET_INSUFFICIENT_SPACE
+ * if @c key_length exceeds the size of the slot.
+ */
+scc_return_t
+scc_load_slot(uint64_t owner_id, uint32_t slot, const uint8_t * key_data,
+	      uint32_t key_length)
+{
+	scc_return_t status;
+	unsigned long irq_flags;
+
+	/* ACQUIRE LOCK to prevent others from using SCC crypto */
+	spin_lock_irqsave(&scc_crypto_lock, irq_flags);
+
+	status = verify_slot_access(owner_id, slot, key_length);
+	if ((status == SCC_RET_OK) && (key_data != NULL)) {
+		status = SCC_RET_FAIL;	/* reset expectations */
+
+		if (key_length > SCC_KEY_SLOT_SIZE) {
+			pr_debug
+			    ("SCC: scc_load_slot() rejecting key of %d bytes.\n",
+			     key_length);
+			status = SCC_RET_INSUFFICIENT_SPACE;
+		} else {
+			if (copy_to_scc(key_data,
+					SCM_RED_MEMORY +
+					scc_key_info[slot].offset, key_length,
+					NULL)) {
+				pr_debug("SCC: RED copy_to_scc() failed for"
+					 " scc_load_slot()\n");
+			} else {
+				if ((key_length % 4) != 0) {
+					uint32_t zeros = 0;
+
+					/* zero-pad to get remainder bytes in correct place */
+					copy_to_scc((uint8_t *) & zeros,
+						    SCM_RED_MEMORY
+						    +
+						    scc_key_info[slot].offset +
+						    key_length,
+						    4 - (key_length % 4), NULL);
+				}
+				status = SCC_RET_OK;
+			}
+		}
+	}
+
+	spin_unlock_irqrestore(&scc_crypto_lock, irq_flags);
+
+	return status;
+}				/* scc_load_slot */
+
+scc_return_t
+scc_read_slot(uint64_t owner_id, uint32_t slot, uint32_t key_length,
+	      uint8_t * key_data)
+{
+	scc_return_t status;
+	unsigned long irq_flags;
+
+	/* ACQUIRE LOCK to prevent others from using SCC crypto */
+	spin_lock_irqsave(&scc_crypto_lock, irq_flags);
+
+	status = verify_slot_access(owner_id, slot, key_length);
+	if ((status == SCC_RET_OK) && (key_data != NULL)) {
+		status = SCC_RET_FAIL;	/* reset expectations */
+
+		if (key_length > SCC_KEY_SLOT_SIZE) {
+			pr_debug
+			    ("SCC: scc_read_slot() rejecting key of %d bytes.\n",
+			     key_length);
+			status = SCC_RET_INSUFFICIENT_SPACE;
+		} else {
+			if (copy_from_scc
+			    (SCM_RED_MEMORY + scc_key_info[slot].offset,
+			     key_data, key_length, NULL)) {
+				pr_debug("SCC: RED copy_from_scc() failed for"
+					 " scc_read_slot()\n");
+			} else {
+				status = SCC_RET_OK;
+			}
+		}
+	}
+
+	spin_unlock_irqrestore(&scc_crypto_lock, irq_flags);
+
+	return status;
+}				/* scc_read_slot */
+
+/*****************************************************************************/
+/* fn scc_encrypt_slot()                                                     */
+/*****************************************************************************/
+/*!
+ * Encrypt the key data stored in a slot.
+ *
+ * @param owner_id      Value of owner of slot
+ * @param slot          Handle of slot
+ * @param length        Length, in bytes, of @c black_data
+ * @param black_data    Location to store result of encrypting RED data in slot
+ *
+ * @return SCC_RET_OK on success, SCC_RET_FAIL if slot specified cannot be
+ *         accessed for any reason.
+ */
+scc_return_t scc_encrypt_slot(uint64_t owner_id, uint32_t slot,
+			      uint32_t length, uint8_t * black_data)
+{
+	unsigned long irq_flags;
+	scc_return_t status;
+	uint32_t crypto_status;
+	uint32_t slot_offset =
+	    scc_key_info[slot].offset / SCC_BLOCK_SIZE_BYTES();
+
+	/* ACQUIRE LOCK to prevent others from using crypto or releasing slot */
+	spin_lock_irqsave(&scc_crypto_lock, irq_flags);
+
+	status = verify_slot_access(owner_id, slot, length);
+	if (status == SCC_RET_OK) {
+		SCC_WRITE_REGISTER(SCM_BLACK_START, slot_offset);
+		SCC_WRITE_REGISTER(SCM_RED_START, slot_offset);
+
+		/* Use OwnerID as CBC IV to tie Owner to data */
+		SCC_WRITE_REGISTER(SCM_INIT_VECTOR_0, *(uint32_t *) & owner_id);
+		SCC_WRITE_REGISTER(SCM_INIT_VECTOR_1,
+				   *(((uint32_t *) & owner_id) + 1));
+
+		/* Set modes and kick off the encryption */
+		crypto_status = scc_do_crypto(length,
+					      SCM_CONTROL_START_CIPHER |
+					      SCM_CBC_MODE);
+
+		if (crypto_status != 0) {
+			pr_debug("SCM encrypt red crypto failure: 0x%x\n",
+				 crypto_status);
+		} else {
+
+			/* Give blob back to caller */
+			if (!copy_from_scc
+			    (SCM_BLACK_MEMORY + scc_key_info[slot].offset,
+			     black_data, length, NULL)) {
+				status = SCC_RET_OK;
+				pr_debug("SCC: Encrypted slot %d\n", slot);
+			}
+		}
+	}
+
+	spin_unlock_irqrestore(&scc_crypto_lock, irq_flags);
+
+	return status;
+}
+
+/*****************************************************************************/
+/* fn scc_decrypt_slot()                                                     */
+/*****************************************************************************/
+/*!
+ * Decrypt some black data and leave result in the slot.
+ *
+ * @param owner_id      Value of owner of slot
+ * @param slot          Handle of slot
+ * @param length        Length, in bytes, of @c black_data
+ * @param black_data    Location of data to dencrypt and store in slot
+ *
+ * @return SCC_RET_OK on success, SCC_RET_FAIL if slot specified cannot be
+ *         accessed for any reason.
+ */
+scc_return_t scc_decrypt_slot(uint64_t owner_id, uint32_t slot,
+			      uint32_t length, const uint8_t * black_data)
+{
+	unsigned long irq_flags;
+	scc_return_t status;
+	uint32_t crypto_status;
+	uint32_t slot_offset =
+	    scc_key_info[slot].offset / SCC_BLOCK_SIZE_BYTES();
+
+	/* ACQUIRE LOCK to prevent others from using crypto or releasing slot */
+	spin_lock_irqsave(&scc_crypto_lock, irq_flags);
+
+	status = verify_slot_access(owner_id, slot, length);
+	if (status == SCC_RET_OK) {
+		status = SCC_RET_FAIL;	/* reset expectations */
+
+		/* Place black key in to BLACK RAM and set up the SCC */
+		copy_to_scc(black_data,
+			    SCM_BLACK_MEMORY + scc_key_info[slot].offset,
+			    length, NULL);
+
+		SCC_WRITE_REGISTER(SCM_BLACK_START, slot_offset);
+		SCC_WRITE_REGISTER(SCM_RED_START, slot_offset);
+
+		/* Use OwnerID as CBC IV to tie Owner to data */
+		SCC_WRITE_REGISTER(SCM_INIT_VECTOR_0, *(uint32_t *) & owner_id);
+		SCC_WRITE_REGISTER(SCM_INIT_VECTOR_1,
+				   *(((uint32_t *) & owner_id) + 1));
+
+		/* Set modes and kick off the decryption */
+		crypto_status = scc_do_crypto(length,
+					      SCM_CONTROL_START_CIPHER
+					      | SCM_CBC_MODE |
+					      SCM_DECRYPT_MODE);
+
+		if (crypto_status != 0) {
+			pr_debug("SCM decrypt black crypto failure: 0x%x\n",
+				 crypto_status);
+		} else {
+			status = SCC_RET_OK;
+		}
+	}
+
+	spin_unlock_irqrestore(&scc_crypto_lock, irq_flags);
+
+	return status;
+}
+
+/*****************************************************************************/
+/* fn scc_get_slot_info()                                                    */
+/*****************************************************************************/
+/*!
+ * Determine address and value length for a give slot.
+ *
+ * @param owner_id      Value of owner of slot
+ * @param slot          Handle of slot
+ * @param address       Location to store kernel address of slot data
+ * @param value_size_bytes Location to store allocated length of data in slot.
+ *                         May be NULL if value is not needed by caller.
+ * @param slot_size_bytes  Location to store max length data in slot
+ *                         May be NULL if value is not needed by caller.
+ *
+ * @return SCC_RET_OK or error indication
+ */
+scc_return_t
+scc_get_slot_info(uint64_t owner_id, uint32_t slot, uint32_t * address,
+		  uint32_t * value_size_bytes, uint32_t * slot_size_bytes)
+{
+	scc_return_t status = verify_slot_access(owner_id, slot, 0);
+
+	if (status == SCC_RET_OK) {
+		*address =
+		    SCC_BASE + SCM_RED_MEMORY + scc_key_info[slot].offset;
+		if (value_size_bytes != NULL) {
+			*value_size_bytes = scc_key_info[slot].length;
+		}
+		if (slot_size_bytes != NULL) {
+			*slot_size_bytes = SCC_KEY_SLOT_SIZE;
+		}
+	}
+
+	return status;
+}
+
+/*****************************************************************************/
+/* fn scc_wait_completion()                                                  */
+/*****************************************************************************/
+/*!
+ * Poll looking for end-of-cipher indication. Only used
+ * if @c SCC_SCM_SLEEP is not defined.
+ *
+ * @internal
+ *
+ * On a Tahiti, crypto under 230 or so bytes is done after the first loop, all
+ * the way up to five sets of spins for 1024 bytes.  (8- and 16-byte functions
+ * are done when we first look.  Zeroizing takes one pass around.
+ */
+static void scc_wait_completion(void)
+{
+	int i = 0;
+
+	/* check for completion by polling */
+	while (!is_cipher_done() && (i++ < SCC_CIPHER_MAX_POLL_COUNT)) {
+		udelay(10);
+	}
+	pr_debug("SCC: Polled DONE %d times\n", i);
+}				/* scc_wait_completion() */
+
+/*****************************************************************************/
+/* fn is_cipher_done()                                                       */
+/*****************************************************************************/
+/*!
+ * This function returns non-zero if SCM Status register indicates
+ * that a cipher has terminated or some other interrupt-generating
+ * condition has occurred.
+ */
+static int is_cipher_done(void)
+{
+	register uint32_t scm_status;
+	register int cipher_done;
+
+	scm_status = SCC_READ_REGISTER(SCM_STATUS);
+
+	/*
+	 * Done when 'SCM is currently performing a function' bits are zero
+	 */
+	cipher_done = !(scm_status & (SCM_STATUS_ZEROIZING |
+				      SCM_STATUS_CIPHERING));
+
+	return cipher_done;
+}				/* is_cipher_done() */
+
+/*****************************************************************************/
+/* fn offset_within_smn()                                                    */
+/*****************************************************************************/
+/*!
+ *  Check that the offset is with the bounds of the SMN register set.
+ *
+ *  @param[in]  register_offset    register offset of SMN.
+ *
+ *  @return   1 if true, 0 if false (not within SMN)
+ */
+static inline int offset_within_smn(uint32_t register_offset)
+{
+	return register_offset >= SMN_STATUS && register_offset <= SMN_TIMER;
+}
+
+/*****************************************************************************/
+/* fn offset_within_scm()                                                    */
+/*****************************************************************************/
+/*!
+ *  Check that the offset is with the bounds of the SCM register set.
+ *
+ *  @param[in]  register_offset    Register offset of SCM
+ *
+ *  @return   1 if true, 0 if false (not within SCM)
+ */
+static inline int offset_within_scm(uint32_t register_offset)
+{
+	return (register_offset >= SCM_RED_START)
+	    && (register_offset < scm_highest_memory_address);
+	/* Although this would cause trouble for zeroize testing, this change would
+	 * close a security whole which currently allows any kernel program to access
+	 * any location in RED RAM.  Perhaps enforce in non-SCC_DEBUG compiles?
+	 && (register_offset <= SCM_INIT_VECTOR_1); */
+}
+
+/*****************************************************************************/
+/* fn check_register_accessible()                                            */
+/*****************************************************************************/
+/*!
+ *  Given the current SCM and SMN status, verify that access to the requested
+ *  register should be OK.
+ *
+ *  @param[in]   register_offset  register offset within SCC
+ *  @param[in]   smn_status  recent value from #SMN_STATUS
+ *  @param[in]   scm_status  recent value from #SCM_STATUS
+ *
+ *  @return   #SCC_RET_OK if ok, #SCC_RET_FAIL if not
+ */
+static scc_return_t
+check_register_accessible(uint32_t register_offset, uint32_t smn_status,
+			  uint32_t scm_status)
+{
+	int error_code = SCC_RET_FAIL;
+
+	/* Verify that the register offset passed in is not among the verboten set
+	 * if the SMN is in Fail mode.
+	 */
+	if (offset_within_smn(register_offset)) {
+		if ((smn_status & SMN_STATUS_STATE_MASK) == SMN_STATE_FAIL) {
+			if (!((register_offset == SMN_STATUS) ||
+			      (register_offset == SMN_COMMAND) ||
+			      (register_offset == SMN_DEBUG_DETECT_STAT))) {
+				pr_debug
+				    ("SCC Driver: Note: Security State is in FAIL state.\n");
+			} /* register not a safe one */
+			else {
+				/* SMN is in  FAIL, but register is a safe one */
+				error_code = SCC_RET_OK;
+			}
+		} /* State is FAIL */
+		else {
+			/* State is not fail.  All registers accessible. */
+			error_code = SCC_RET_OK;
+		}
+	}
+	/* offset within SMN */
+	/*  Not SCM register.  Check for SCM busy. */
+	else if (offset_within_scm(register_offset)) {
+		/* This is the 'cannot access' condition in the SCM */
+		if ((scm_status & SCM_STATUS_BUSY)
+		    /* these are always available  - rest fail on busy */
+		    && !((register_offset == SCM_STATUS) ||
+			 (register_offset == SCM_ERROR_STATUS) ||
+			 (register_offset == SCM_INTERRUPT_CTRL) ||
+			 (register_offset == SCM_CONFIGURATION))) {
+			pr_debug
+			    ("SCC Driver: Note: Secure Memory is in BUSY state.\n");
+		} /* status is busy & register inaccessible */
+		else {
+			error_code = SCC_RET_OK;
+		}
+	}
+	/* offset within SCM */
+	return error_code;
+
+}				/* check_register_accessible() */
+
+/*****************************************************************************/
+/* fn check_register_offset()                                                */
+/*****************************************************************************/
+/*!
+ *  Check that the offset is with the bounds of the SCC register set.
+ *
+ *  @param[in]  register_offset    register offset of SMN.
+ *
+ * #SCC_RET_OK if ok, #SCC_RET_FAIL if not
+ */
+static scc_return_t check_register_offset(uint32_t register_offset)
+{
+	int return_value = SCC_RET_FAIL;
+
+	/* Is it valid word offset ? */
+	if (SCC_BYTE_OFFSET(register_offset) == 0) {
+		/* Yes. Is register within SCM? */
+		if (offset_within_scm(register_offset)) {
+			return_value = SCC_RET_OK;	/* yes, all ok */
+		}
+		/* Not in SCM.  Now look within the SMN */
+		else if (offset_within_smn(register_offset)) {
+			return_value = SCC_RET_OK;	/* yes, all ok */
+		}
+	}
+
+	return return_value;
+}
+
+#ifdef SCC_REGISTER_DEBUG
+
+/*****************************************************************************/
+/* fn dbg_scc_read_register()                                                */
+/*****************************************************************************/
+/*!
+ * Noisily read a 32-bit value to an SCC register.
+ * @param offset        The address of the register to read.
+ *
+ * @return  The register value
+ * */
+static uint32_t dbg_scc_read_register(uint32_t offset)
+{
+	uint32_t value;
+
+	value = readl(scc_base + offset);
+
+#ifndef SCC_RAM_DEBUG		/* print no RAM references */
+	if ((offset < SCM_RED_MEMORY) || (offset >= scm_highest_memory_address)) {
+#endif
+		pr_debug("SCC RD: 0x%4x : 0x%08x\n", offset, value);
+#ifndef SCC_RAM_DEBUG
+	}
+#endif
+
+	return value;
+}
+
+/*****************************************************************************/
+/* fn dbg_scc_write_register()                                               */
+/*****************************************************************************/
+/*
+ * Noisily read a 32-bit value to an SCC register.
+ * @param offset        The address of the register to written.
+ *
+ * @param value         The new register value
+ */
+static void dbg_scc_write_register(uint32_t offset, uint32_t value)
+{
+
+#ifndef SCC_RAM_DEBUG		/* print no RAM references */
+	if ((offset < SCM_RED_MEMORY) || (offset >= scm_highest_memory_address)) {
+#endif
+		pr_debug("SCC WR: 0x%4x : 0x%08x\n", offset, value);
+#ifndef SCC_RAM_DEBUG
+	}
+#endif
+
+	(void)writel(value, scc_base + offset);
+}
+
+#endif				/* SCC_REGISTER_DEBUG */
diff --git a/drivers/mxc/security/mxc_scc_internals.h b/drivers/mxc/security/mxc_scc_internals.h
new file mode 100644
index 000000000000..f86fb9104b54
--- /dev/null
+++ b/drivers/mxc/security/mxc_scc_internals.h
@@ -0,0 +1,499 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __MXC_SCC_INTERNALS_H__
+#define __MXC_SCC_INTERNALS_H__
+
+/*!
+ * @file mxc_scc_internals.h
+ *
+ * @brief This is intended to be the file which contains most or all of the code or
+ *  changes need to port the driver.  It also includes other definitions needed
+ *  by the driver.
+ *
+ *  This header file should only ever be included by scc_driver.c
+ *
+ *  Compile-time flags minimally needed:
+ *
+ *  @li Some sort of platform flag.
+ *  @li Some start-of-SCC consideration, such as SCC_BASE_ADDR
+ *
+ *  Some changes which could be made when porting this driver:
+ *  #SCC_SPIN_COUNT
+ *
+ * @ingroup MXCSCC
+ */
+#if 0
+#include <linux/version.h>	/* Current version Linux kernel */
+#include <linux/module.h>	/* Basic support for loadable modules,
+				   printk */
+#include <linux/init.h>		/* module_init, module_exit */
+#include <linux/kernel.h>	/* General kernel system calls */
+#include <linux/sched.h>	/* for interrupt.h */
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>	/* IRQ / interrupt definitions */
+#include <linux/io.h>		/* ioremap() */
+#endif
+#include <linux/mxc_scc_driver.h>
+
+/* Get handle on certain per-platform symbols */
+#ifdef TAHITI
+#include <asm/arch/mx2.h>
+
+/*
+ * Mark the SCC as always there... as Tahiti is not officially supported by
+ * driver.  Porting opportunity.
+ */
+#define SCC_ENABLED() (1)
+
+#elif defined(MXC)
+
+#include <mach/iim.h>
+#include <mach/mxc_scc.h>
+
+#ifdef SCC_FUSE
+
+/*
+ * This macro is used to determine whether the SCC is enabled/available
+ * on the platform.  This macro may need to be ported.
+ */
+#define SCC_ENABLED() ((SCC_FUSE & MXC_IIMHWV1_SCC_DISABLE) == 0)
+
+#else
+
+#warning SCC_FUSE not defined, assuming the SCC is enabled.
+#define SCC_ENABLED() (1)
+
+#endif
+
+#else				/* neither TAHITI nor MXC */
+
+#error Do not understand target architecture
+
+#endif				/* TAHITI */
+
+/* Temporarily define compile-time flags to make Doxygen happy. */
+#ifdef DOXYGEN_HACK
+/*! @addtogroup scccompileflags */
+/*! @{ */
+
+/*! @def NO_SMN_INTERRUPT
+ * The SMN interrupt is not wired to the CPU at all.
+ */
+#define NO_SMN_INTERRUPT
+
+/*!
+ * Register an interrupt handler for the SMN as well as
+ * the SCM.  In some implementations, the SMN is not connected at all (see
+ * #NO_SMN_INTERRUPT), and in others, it is on the same interrupt line as the
+ * SCM.  When defining this flag, the SMN interrupt should be on a separate
+ * line from the SCM interrupt.
+ */
+
+#define USE_SMN_INTERRUPT
+
+/*!
+ * Turn on generation of run-time operational, debug, and error messages
+ */
+#define SCC_DEBUG
+
+/*!
+ * Turn on generation of run-time logging of access to the SCM and SMN
+ * registers.
+ */
+#define SCC_REGISTER_DEBUG
+
+/*!
+ * Turn on generation of run-time logging of access to the SCM Red and
+ * Black memories.  Will only work if #SCC_REGISTER_DEBUG is also defined.
+ */
+#define SCC_RAM_DEBUG
+
+/*!
+ *  If the driver finds the SCC in HEALTH_CHECK state, go ahead and
+ *  run a quick ASC to bring it to SECURE state.
+ */
+#define SCC_BRINGUP
+
+/*!
+ * Expected to come from platform header files or compile command line.
+ * This symbol must be the address of the SCC
+ */
+#define SCC_BASE
+
+/*!
+ *  This must be the interrupt line number of the SCM interrupt.
+ */
+#define INT_SCM
+
+/*!
+ *  if #USE_SMN_INTERRUPT is defined, this must be the interrupt line number of
+ *  the SMN interrupt.
+ */
+#define INT_SMN
+
+/*!
+ * Define the number of Stored Keys which the SCC driver will make available.
+ * Value shall be from 0 to 20.  Default is zero (0).
+ */
+#define SCC_KEY_SLOTS 
+
+/*!
+ * Make sure that this flag is defined if compiling for a Little-Endian
+ * platform.  Linux Kernel builds provide this flag.
+ */
+#define __LITTLE_ENDIAN
+
+/*!
+ * Make sure that this flag is defined if compiling for a Big-Endian platform.
+ * Linux Kernel builds provide this flag.
+ */
+#define __BIG_ENDIAN
+
+/*!
+ * Read a 32-bit register value from a 'peripheral'.  Standard Linux/Unix
+ * macro.
+ *
+ * @param offset   Bus address of register to be read
+ *
+ * @return The value of the register
+ */
+#define readl(offset)
+
+/*!
+ * Write a 32-bit value to a register in a 'peripheral'.  Standard Linux/Unix
+ * macro.
+ *
+ * @param value    The 32-bit value to store
+ * @param offset   Bus address of register to be written
+ *
+ * return (none)
+ */
+#define writel(value,offset)
+
+	   /*! @} *//* end group scccompileflags */
+
+#endif				/* DOXYGEN_HACK */
+
+/*!
+ * Define the number of Stored Keys which the SCC driver will make available.
+ * Value shall be from 0 to 20.  Default is zero (0).
+ */
+#define SCC_KEY_SLOTS 20
+
+#ifndef SCC_KEY_SLOTS
+#define SCC_KEY_SLOTS 0
+
+#else
+
+#if (SCC_KEY_SLOTS < 0) || (SCC_KEY_SLOTS > 20)
+#error Bad value for SCC_KEY_SLOTS
+#endif
+
+/*!
+ * Maximum length of key/secret value which can be stored in SCC.
+ */
+#define SCC_MAX_KEY_SIZE 32
+
+/*!
+ * This is the size, in bytes, of each key slot, and therefore the maximum size
+ * of the wrapped key.
+ */
+#define SCC_KEY_SLOT_SIZE 32
+
+/*!
+ *  This is the offset into each RAM of the base of the area which is
+ *  not used for Stored Keys.
+ */
+#define SCM_NON_RESERVED_OFFSET (SCC_KEY_SLOTS * SCC_KEY_SLOT_SIZE)
+
+#endif
+
+/* These come for free with Linux, but may need to be set in a port. */
+#ifndef __BIG_ENDIAN
+#ifndef __LITTLE_ENDIAN
+#error One of __LITTLE_ENDIAN or __BIG_ENDIAN must be #defined
+#endif
+#else
+#ifdef __LITTLE_ENDIAN
+#error Exactly one of __LITTLE_ENDIAN or __BIG_ENDIAN must be #defined
+#endif
+#endif
+
+#ifndef SCC_CALLBACK_SIZE
+/*! The number of function pointers which can be stored in #scc_callbacks.
+ *  Defaults to 4, can be overridden with compile-line argument.
+ */
+#define SCC_CALLBACK_SIZE 4
+#endif
+
+/*! Initial CRC value for CCITT-CRC calculation. */
+#define CRC_CCITT_START 0xFFFF
+
+#ifdef TAHITI
+
+/*!
+ * The SCC_BASE has to be SMN_BASE_ADDR on TAHITI, as the banks of
+ * registers are swapped in place.
+ */
+#define SCC_BASE       SMN_BASE_ADDR
+
+/*! The interrupt number for the SCC (SCM only!) on Tahiti */
+#define INT_SCC_SCM       62
+
+/*! Tahiti does not have the SMN interrupt wired to the CPU.  */
+#define NO_SMN_INTERRUPT
+
+#endif				/* TAHITI */
+
+/*! Number of times to spin between polling of SCC while waiting for cipher
+ *  or zeroizing function to complete. See also #SCC_CIPHER_MAX_POLL_COUNT. */
+#define SCC_SPIN_COUNT 1000
+
+/*! Number of times to polling SCC while waiting for cipher
+ *  or zeroizing function to complete.  See also #SCC_SPIN_COUNT.  */
+#define SCC_CIPHER_MAX_POLL_COUNT 100
+
+/*!
+ * @def SCC_READ_REGISTER
+ * Read a 32-bit value from an SCC register.  Macro which depends upon
+ * #scc_base.  Linux readl()/writel() macros operate on 32-bit quantities, as
+ * do SCC register reads/writes.
+ *
+ * @param     offset  Register offset within SCC.
+ *
+ * @return    The value from the SCC's register.
+ */
+#ifndef SCC_REGISTER_DEBUG
+#define SCC_READ_REGISTER(offset) __raw_readl(scc_base+(offset))
+#else
+#define SCC_READ_REGISTER(offset) dbg_scc_read_register(offset)
+#endif
+
+/*!
+ * Write a 32-bit value to an SCC register.  Macro depends upon #scc_base.
+ * Linux readl()/writel() macros operate on 32-bit quantities, as do SCC
+ * register reads/writes.
+ *
+ * @param   offset  Register offset within SCC.
+ * @param   value   32-bit value to store into the register
+ *
+ * @return   (void)
+ */
+#ifndef SCC_REGISTER_DEBUG
+#define SCC_WRITE_REGISTER(offset,value) (void)__raw_writel(value, scc_base+(offset))
+#else
+#define SCC_WRITE_REGISTER(offset,value) dbg_scc_write_register(offset, value)
+#endif
+
+/*!
+ * Calculates the byte offset into a word
+ *  @param   bp  The byte (char*) pointer
+ *  @return      The offset (0, 1, 2, or 3)
+ */
+#define SCC_BYTE_OFFSET(bp) ((uint32_t)(bp) % sizeof(uint32_t))
+
+/*!
+ * Converts (by rounding down) a byte pointer into a word pointer
+ *  @param  bp  The byte (char*) pointer
+ *  @return     The word (uint32_t) as though it were an aligned (uint32_t*)
+ */
+#define SCC_WORD_PTR(bp) (((uint32_t)(bp)) & ~(sizeof(uint32_t)-1))
+
+/*!
+ * Determine number of bytes in an SCC block
+ *
+ * @return Bytes / block
+ */
+#define SCC_BLOCK_SIZE_BYTES() scc_configuration.block_size_bytes
+
+/*!
+ * Maximum number of additional bytes which may be added in CRC+padding mode.
+ */
+#define PADDING_BUFFER_MAX_BYTES (CRC_SIZE_BYTES + sizeof(scc_block_padding))
+
+/*!
+ *  Shorthand (clearer, anyway) for number of bytes in a CRC.
+ */
+#define CRC_SIZE_BYTES (sizeof(crc_t))
+
+/*!
+ * The polynomial used in CCITT-CRC calculation
+ */
+#define CRC_POLYNOMIAL 0x1021
+
+/*!
+ * Calculate CRC on one byte of data
+ *
+ * @param[in,out] running_crc  A value of type crc_t where CRC is kept. This
+ *                             must be an rvalue and an lvalue.
+ * @param[in]     byte_value   The byte (uint8_t, char) to be put in the CRC
+ *
+ * @return      none
+ */
+#define CALC_CRC(byte_value,running_crc)  {                                  \
+    uint8_t data;                                                            \
+    data = (0xff&(byte_value)) ^ (running_crc >> 8);                         \
+    running_crc = scc_crc_lookup_table[data] ^ (running_crc << 8);           \
+}
+
+/*! Value of 'beginning of padding' marker in driver-provided padding */
+#define SCC_DRIVER_PAD_CHAR  0x80
+
+/*! Name of the driver.  Used (on Linux, anyway) when registering interrupts */
+#define SCC_DRIVER_NAME "scc"
+
+/* Port -- these symbols are defined in Linux 2.6 and later.  They are defined
+ * here for backwards compatibility because this started life as a 2.4
+ * driver, and as a guide to portation to other platforms.
+ */
+
+#if !defined(LINUX_VERSION_CODE) || LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+
+#define irqreturn_t void	/* Return type of an interrupt handler */
+
+#define IRQ_HANDLED		/* Would be '1' for handled -- as in return IRQ_HANDLED; */
+
+#define IRQ_NONE		/* would be '0' for not handled -- as in return IRQ_NONE; */
+
+#define IRQ_RETVAL(x)		/* Return x==0 (not handled) or non-zero (handled) */
+
+#endif				/* LINUX earlier than 2.5 */
+
+/* These are nice to have around */
+#ifndef FALSE
+#define FALSE 0
+#endif
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+/*! Provide a typedef for the CRC which can be used in encrypt/decrypt */
+typedef uint16_t crc_t;
+
+/*! Gives high-level view of state of the SCC */
+enum scc_status {
+	SCC_STATUS_INITIAL,	/*!< State of driver before ever checking */
+	SCC_STATUS_CHECKING,	/*!< Transient state while driver loading  */
+	SCC_STATUS_UNIMPLEMENTED,	/*!< SCC is non-existent or unuseable  */
+	SCC_STATUS_OK,		/*!< SCC is in Secure or Default state  */
+	SCC_STATUS_FAILED	/*!< In Failed state */
+};
+
+/*!
+ * Information about a key slot.
+ */
+struct scc_key_slot {
+	uint64_t owner_id;	/*!< Access control value. */
+	uint32_t length;	/*!< Length of value in slot. */
+	uint32_t offset;	/*!< Offset of value from start of each RAM. */
+	uint32_t status;	/*!< 0 = unassigned, 1 = assigned. */
+};
+
+/* Forward-declare a number routines which are not part of user api */
+static int scc_init(void);
+static void scc_cleanup(void);
+
+/* Forward defines of internal functions */
+OS_DEV_ISR(scc_irq);
+/*! Perform callbacks registered by #scc_monitor_security_failure().
+ *
+ *  Make sure callbacks only happen once...  Since there may be some reason why
+ *  the interrupt isn't generated, this routine could be called from base(task)
+ *  level.
+ *
+ *  One at a time, go through #scc_callbacks[] and call any non-null pointers.
+ */
+static void scc_perform_callbacks(void);
+static uint32_t copy_to_scc(const uint8_t * from, uint32_t to,
+			    unsigned long count_bytes, uint16_t * crc);
+static uint32_t copy_from_scc(const uint32_t from, uint8_t * to,
+			      unsigned long count_bytes, uint16_t * crc);
+static scc_return_t scc_strip_padding(uint8_t * from,
+				      unsigned *count_bytes_stripped);
+static uint32_t scc_update_state(void);
+static void scc_init_ccitt_crc(void);
+static uint32_t scc_grab_config_values(void);
+static int setup_interrupt_handling(void);
+/*!
+ * Perform an encryption on the input.  If @c verify_crc is true, a CRC must be
+ * calculated on the plaintext, and appended, with padding, before computing
+ * the ciphertext.
+ *
+ * @param[in]     count_in_bytes  Count of bytes of plaintext
+ * @param[in]     data_in         Pointer to the plaintext
+ * @param[in]     scm_control     Bit values for the SCM_CONTROL register
+ * @param[in,out] data_out        Pointer for storing ciphertext
+ * @param[in]     add_crc         Flag for computing CRC - 0 no, else yes
+ * @param[in,out] count_out_bytes Number of bytes available at @c data_out
+ */
+static scc_return_t scc_encrypt(uint32_t count_in_bytes,
+				const uint8_t * data_in,
+				uint32_t scm_control, uint8_t * data_out,
+				int add_crc, unsigned long *count_out_bytes);
+
+/*!
+ * Perform a decryption on the input.  If @c verify_crc is true, the last block
+ * (maybe the two last blocks) is special - it should contain a CRC and
+ * padding.  These must be stripped and verified.
+ *
+ * @param[in]     count_in_bytes  Count of bytes of ciphertext
+ * @param[in]     data_in         Pointer to the ciphertext
+ * @param[in]     scm_control     Bit values for the SCM_CONTROL register
+ * @param[in,out] data_out        Pointer for storing plaintext
+ * @param[in]     verify_crc      Flag for running CRC - 0 no, else yes
+ * @param[in,out] count_out_bytes Number of bytes available at @c data_out
+
+ */
+static scc_return_t scc_decrypt(uint32_t count_in_bytes,
+				const uint8_t * data_in,
+				uint32_t scm_control, uint8_t * data_out,
+				int verify_crc, unsigned long *count_out_bytes);
+
+static void scc_wait_completion(void);
+static int is_cipher_done(void);
+static scc_return_t check_register_accessible(uint32_t offset,
+					      uint32_t smn_status,
+					      uint32_t scm_status);
+static scc_return_t check_register_offset(uint32_t offset);
+
+#ifdef SCC_REGISTER_DEBUG
+static uint32_t dbg_scc_read_register(uint32_t offset);
+static void dbg_scc_write_register(uint32_t offset, uint32_t value);
+#endif
+
+/* For Linux kernel, export the API functions to other kernel modules */
+EXPORT_SYMBOL(scc_get_configuration);
+EXPORT_SYMBOL(scc_zeroize_memories);
+EXPORT_SYMBOL(scc_crypt);
+EXPORT_SYMBOL(scc_set_sw_alarm);
+EXPORT_SYMBOL(scc_monitor_security_failure);
+EXPORT_SYMBOL(scc_stop_monitoring_security_failure);
+EXPORT_SYMBOL(scc_read_register);
+EXPORT_SYMBOL(scc_write_register);
+EXPORT_SYMBOL(scc_alloc_slot);
+EXPORT_SYMBOL(scc_dealloc_slot);
+EXPORT_SYMBOL(scc_load_slot);
+EXPORT_SYMBOL(scc_encrypt_slot);
+EXPORT_SYMBOL(scc_decrypt_slot);
+EXPORT_SYMBOL(scc_get_slot_info);
+
+/* Tell Linux where to invoke driver at boot/module load time */
+module_init(scc_init);
+/* Tell Linux where to invoke driver on module unload  */
+module_exit(scc_cleanup);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Device Driver for SCC (SMN/SCM)");
+
+#endif				/* __MXC_SCC_INTERNALS_H__ */
diff --git a/drivers/mxc/security/rng/Makefile b/drivers/mxc/security/rng/Makefile
new file mode 100644
index 000000000000..aaf6ace8aa5a
--- /dev/null
+++ b/drivers/mxc/security/rng/Makefile
@@ -0,0 +1,35 @@
+# Makefile for the Linux RNG Driver
+#
+# This makefile works within a kernel driver tree
+
+ # Makefile for rng_driver
+
+
+# Possible configurable paramters
+CFG_RNG += -DRNGA_MAX_REQUEST_SIZE=32
+
+#DBG_RNGA = -DRNGA_DEBUG
+#DBG_RNGA += -DRNGA_REGISTER_DEBUG
+#DBG_RNGA += -DRNGA_ENTROPY_DEBUG
+
+EXTRA_CFLAGS = -DLINUX_KERNEL $(CFG_RNG) $(DBG_RNG)
+
+
+ifeq ($(CONFIG_MXC_RNG_TEST_DRIVER),y)
+EXTRA_CFLAGS += -DRNG_REGISTER_PEEK_POKE
+endif
+ifeq ($(CONFIG_RNG_DEBUG),y)
+EXTRA_CFLAGS += -DDEBUG
+endif
+
+
+EXTRA_CFLAGS += -Idrivers/mxc/security/rng/include -Idrivers/mxc/security/sahara2/include
+
+obj-$(CONFIG_MXC_SECURITY_RNG)	+= shw.o
+#shw-objs := shw_driver.o shw_memory_mapper.o ../sahara2/fsl_shw_keystore.o
+shw-objs := shw_driver.o shw_memory_mapper.o ../sahara2/fsl_shw_keystore.o  \
+            fsl_shw_sym.o  fsl_shw_wrap.o shw_dryice.o  des_key.o           \
+            shw_hash.o  shw_hmac.o
+            
+obj-$(CONFIG_MXC_SECURITY_RNG)	+= rng.o
+rng-objs :=	rng_driver.o
diff --git a/drivers/mxc/security/rng/des_key.c b/drivers/mxc/security/rng/des_key.c
new file mode 100644
index 000000000000..741a800130b6
--- /dev/null
+++ b/drivers/mxc/security/rng/des_key.c
@@ -0,0 +1,385 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+
+/*!
+ * @file des_key.c
+ *
+ * This file implements the function #fsl_shw_permute1_bytes().
+ *
+ * The code was lifted from crypto++ v5.5.2, which is public domain code.  The
+ * code to handle words instead of bytes was extensively modified from the byte
+ * version and then converted to handle one to three keys at once.
+ *
+ */
+
+#include "shw_driver.h"
+#ifdef DIAG_SECURITY_FUNC
+#include "apihelp.h"
+#endif
+
+#ifndef __KERNEL__
+#include <asm/types.h>
+#include <linux/byteorder/little_endian.h>	/* or whichever is proper for target arch */
+#endif
+
+#ifdef DEBUG
+#undef DEBUG			/* TEMPORARY */
+#endif
+
+#if defined(DEBUG) || defined(SELF_TEST)
+static void DUMP_BYTES(const char *label, const uint8_t * data, int len)
+{
+	int i;
+
+	printf("%s: ", label);
+	for (i = 0; i < len; i++) {
+		printf("%02X", data[i]);
+		if ((i % 8 == 0) && (i != 0)) {
+			printf("_");	/* key separator */
+		}
+	}
+	printf("\n");
+}
+
+static void DUMP_WORDS(const char *label, const uint32_t * data, int len)
+{
+	int i, j;
+
+	printf("%s: ", label);
+	/* Dump the words in reverse order, so that they are intelligible */
+	for (i = len - 1; i >= 0; i--) {
+		for (j = 3; j >= 0; j--) {
+			uint32_t word = data[i];
+			printf("%02X", (word >> ((j * 8)) & 0xff));
+			if ((i != 0) && ((((i) * 4 + 5 + j) % 7) == 5))
+				printf("_");	/* key separator */
+		}
+		printf("|");	/* word separator */
+	}
+	printf("\n");
+}
+#else
+#define DUMP_BYTES(label, data,len)
+#define DUMP_WORDS(label, data,len)
+#endif
+
+/*!
+ * permuted choice table (key)
+ *
+ * Note that this table has had one subtracted from each element so that the
+ * code doesn't have to do it.
+ */
+static const uint8_t pc1[] = {
+	56, 48, 40, 32, 24, 16, 8,
+	0, 57, 49, 41, 33, 25, 17,
+	9, 1, 58, 50, 42, 34, 26,
+	18, 10, 2, 59, 51, 43, 35,
+	62, 54, 46, 38, 30, 22, 14,
+	6, 61, 53, 45, 37, 29, 21,
+	13, 5, 60, 52, 44, 36, 28,
+	20, 12, 4, 27, 19, 11, 3,
+};
+
+/*! bit 0 is left-most in byte */
+static const int bytebit[] = {
+	0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01
+};
+
+/*!
+ * Convert a 3-key 3DES key into the first-permutation 168-bit version.
+ *
+ * This is the format of the input key:
+ *
+ * @verbatim
+   BIT:  |191                                 128|127                                  64|63                                    0|
+   BYTE: |  0 |  1 |  2 |  3 |  4 |  5 |  6 |  7 |  8 |  9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 |
+   KEY:  |                   0                   |                   1                   |                   2                   |
+  @endverbatim
+ *
+ * This is the format of the output key:
+ *
+ * @verbatim
+   BIT:  |167                            112|111                             56|55                               0|
+   BYTE: |  0 |  1 |  2 |  3 |  4 |  5 |  6 |  7 |  8 |  9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 |
+   KEY:  |                 1                |                 2                |                 3                |
+  @endverbatim
+ * 
+ * @param[in]   key          bytes of 3DES key
+ * @param[out] permuted_key  21 bytes of permuted key
+ * @param[in]  key_count     How many DES keys (2 or 3)
+ */
+void fsl_shw_permute1_bytes(const uint8_t * key, uint8_t * permuted_key,
+			    int key_count)
+{
+	int i;
+	int j;
+	int l;
+	int m;
+
+	DUMP_BYTES("Input key", key, 8 * key_count);
+
+	/* For each individual sub-key */
+	for (i = 0; i < 3; i++) {
+		DUMP_BYTES("(key)", key, 8);
+		memset(permuted_key, 0, 7);
+		/* For each bit of key */
+		for (j = 0; j < 56; j++) {	/* convert pc1 to bits of key */
+			l = pc1[j];	/* integer bit location  */
+			m = l & 07;	/* find bit              */
+			permuted_key[j >> 3] |= (((key[l >> 3] &	/* find which key byte l is in */
+						   bytebit[m])	/* and which bit of that byte */
+						  ? 0x80 : 0) >> (j % 8));	/* and store 1-bit result */
+		}
+		switch (i) {
+		case 0:
+			if (key_count != 1)
+				key += 8;	/* move on to second key */
+			break;
+		case 1:
+			if (key_count == 2)
+				key -= 8;	/* go back to first key */
+			else if (key_count == 3)
+				key += 8;	/* move on to third key */
+			break;
+		default:
+			break;
+		}
+		permuted_key += 7;
+	}
+	DUMP_BYTES("Output key (bytes)", permuted_key - 21, 21);
+}
+
+#ifdef SELF_TEST
+const uint8_t key1_in[] = {
+	/* FE01FE01FE01FE01_01FE01FE01FE01FE_FEFE0101FEFE0101 */
+	0xFE, 0x01, 0xFE, 0x01, 0xFE, 0x01, 0xFE, 0x01,
+	0x01, 0xFE, 0x01, 0xFE, 0x01, 0xFE, 0x01, 0xFE,
+	0xFE, 0xFE, 0x01, 0x01, 0xFE, 0xFE, 0x01, 0x01
+};
+
+const uint32_t key1_word_in[] = {
+	0xFE01FE01, 0xFE01FE01,
+	0x01FE01FE, 0x01FE01FE,
+	0xFEFE0101, 0xFEFE0101
+};
+
+uint8_t exp_key1_out[] = {
+	0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55,
+	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
+	0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33
+};
+
+uint32_t exp_word_key1_out[] = {
+	0x33333333, 0xAA333333, 0xAAAAAAAA, 0x5555AAAA,
+	0x55555555, 0x00000055,
+};
+
+const uint8_t key2_in[] = {
+	0xEF, 0x10, 0xBB, 0xA4, 0x23, 0x49, 0x42, 0x58,
+	0x01, 0x28, 0x01, 0x4A, 0x10, 0xE4, 0x03, 0x59,
+	0xFE, 0x84, 0x30, 0x29, 0x8E, 0xF1, 0x10, 0x5A
+};
+
+const uint32_t key2_word_in[] = {
+	0xEF10BBA4, 0x23494258,
+	0x0128014A, 0x10E40359,
+	0xFE843029, 0x8EF1105A
+};
+
+uint8_t exp_key2_out[] = {
+	0x0D, 0xE1, 0x1D, 0x85, 0x50, 0x9A, 0x56, 0x20,
+	0xA8, 0x22, 0x94, 0x82, 0x08, 0xA0, 0x33, 0xA1,
+	0x2D, 0xE9, 0x11, 0x39, 0x95
+};
+
+uint32_t exp_word_key2_out[] = {
+	0xE9113995, 0xA033A12D, 0x22948208, 0x9A5620A8,
+	0xE11D8550, 0x0000000D
+};
+
+const uint8_t key3_in[] = {
+	0x3F, 0xE9, 0x49, 0x4B, 0x67, 0x57, 0x07, 0x3C,
+	0x89, 0x77, 0x73, 0x0C, 0xA0, 0x05, 0x41, 0x69,
+	0xB3, 0x7C, 0x98, 0xD8, 0xC9, 0x35, 0x57, 0x19
+};
+
+const uint32_t key3_word_in[] = {
+	0xEF10BBA4, 0x23494258,
+	0x0128014A, 0x10E40359,
+	0xFE843029, 0x8EF1105A
+};
+
+uint8_t exp_key3_out[] = {
+	0x02, 0x3E, 0x93, 0xA7, 0x9F, 0x18, 0xF1, 0x11,
+	0xC6, 0x96, 0x00, 0x62, 0xA8, 0x96, 0x02, 0x3E,
+	0x93, 0xA7, 0x9F, 0x18, 0xF1
+};
+
+uint32_t exp_word_key3_out[] = {
+	0xE9113995, 0xA033A12D, 0x22948208, 0x9A5620A8,
+	0xE11D8550, 0x0000000D
+};
+
+const uint8_t key4_in[] = {
+	0x3F, 0xE9, 0x49, 0x4B, 0x67, 0x57, 0x07, 0x3C,
+	0x89, 0x77, 0x73, 0x0C, 0xA0, 0x05, 0x41, 0x69,
+};
+
+const uint32_t key4_word_in[] = {
+	0xEF10BBA4, 0x23494258,
+	0x0128014A, 0x10E40359,
+	0xFE843029, 0x8EF1105A
+};
+
+const uint8_t key5_in[] = {
+	0x3F, 0xE9, 0x49, 0x4B, 0x67, 0x57, 0x07, 0x3C,
+	0x89, 0x77, 0x73, 0x0C, 0xA0, 0x05, 0x41, 0x69,
+	0x3F, 0xE9, 0x49, 0x4B, 0x67, 0x57, 0x07, 0x3C,
+};
+
+uint8_t exp_key4_out[] = {
+	0x0D, 0xE1, 0x1D, 0x85, 0x50, 0x9A, 0x56, 0x20,
+	0xA8, 0x22, 0x94, 0x82, 0x08, 0xA0, 0x33, 0xA1,
+	0x2D, 0xE9, 0x11, 0x39, 0x95
+};
+
+uint32_t exp_word_key4_out[] = {
+	0xE9113995, 0xA033A12D, 0x22948208, 0x9A5620A8,
+	0xE11D8550, 0x0000000D
+};
+
+const uint8_t key6_in[] = {
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01
+};
+
+uint8_t exp_key6_out[] = {
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+uint32_t exp_word_key6_out[] = {
+	0x00000000, 0x0000000, 0x0000000, 0x00000000,
+	0x00000000, 0x0000000
+};
+
+const uint8_t key7_in[] = {
+	/* 01FE01FE01FE01FE_FE01FE01FE01FE01_0101FEFE0101FEFE */
+	/*  0101FEFE0101FEFE_FE01FE01FE01FE01_01FE01FE01FE01FE */
+	0x01, 0x01, 0xFE, 0xFE, 0x01, 0x01, 0xFE, 0xFE,
+	0xFE, 0x01, 0xFE, 0x01, 0xFE, 0x01, 0xFE, 0x01,
+	0x01, 0xFE, 0x01, 0xFE, 0x01, 0xFE, 0x01, 0xFE,
+};
+
+uint8_t exp_key7_out[] = {
+	0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0x55,
+	0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0xaa, 0xaa,
+	0xaa, 0xaa, 0xaa, 0xaa, 0xaa
+};
+
+uint32_t exp_word_key7_out[] = {
+	0xcccccccc, 0x55cccccc, 0x55555555, 0xaaaa5555,
+	0xaaaaaaaa, 0x000000aa
+};
+
+int run_test(const uint8_t * key_in,
+	     const int key_count,
+	     const uint32_t * key_word_in,
+	     const uint8_t * exp_bytes_key_out,
+	     const uint32_t * exp_word_key_out)
+{
+	uint8_t key_out[22];
+	uint32_t word_key_out[6];
+	int failed = 0;
+
+	memset(key_out, 0x42, 22);
+	fsl_shw_permute1_bytes(key_in, key_out, key_count);
+	if (memcmp(key_out, exp_bytes_key_out, 21) != 0) {
+		printf("bytes_to_bytes: ERROR: \n");
+		DUMP_BYTES("key_in", key_in, 8 * key_count);
+		DUMP_BYTES("key_out", key_out, 21);
+		DUMP_BYTES("exp_out", exp_bytes_key_out, 21);
+		failed |= 1;
+	} else if (key_out[21] != 0x42) {
+		printf("bytes_to_bytes: ERROR: Buffer overflow 0x%02x\n",
+		       (int)key_out[21]);
+	} else {
+		printf("bytes_to_bytes: OK\n");
+	}
+#if 0
+	memset(word_key_out, 0x42, 21);
+	fsl_shw_permute1_bytes_to_words(key_in, word_key_out, key_count);
+	if (memcmp(word_key_out, exp_word_key_out, 21) != 0) {
+		printf("bytes_to_words: ERROR: \n");
+		DUMP_BYTES("key_in", key_in, 8 * key_count);
+		DUMP_WORDS("key_out", word_key_out, 6);
+		DUMP_WORDS("exp_out", exp_word_key_out, 6);
+		failed |= 1;
+	} else {
+		printf("bytes_to_words: OK\n");
+	}
+
+	if (key_word_in != NULL) {
+		memset(word_key_out, 0x42, 21);
+		fsl_shw_permute1_words_to_words(key_word_in, word_key_out);
+		if (memcmp(word_key_out, exp_word_key_out, 21) != 0) {
+			printf("words_to_words: ERROR: \n");
+			DUMP_BYTES("key_in", key_in, 24);
+			DUMP_WORDS("key_out", word_key_out, 6);
+			DUMP_WORDS("exp_out", exp_word_key_out, 6);
+			failed |= 1;
+		} else {
+			printf("words_to_words: OK\n");
+		}
+	}
+#endif
+
+	return failed;
+}				/* end fn run_test */
+
+int main()
+{
+	int failed = 0;
+
+	printf("key1\n");
+	failed |=
+	    run_test(key1_in, 3, key1_word_in, exp_key1_out, exp_word_key1_out);
+	printf("\nkey2\n");
+	failed |=
+	    run_test(key2_in, 3, key2_word_in, exp_key2_out, exp_word_key2_out);
+	printf("\nkey3\n");
+	failed |= run_test(key3_in, 3, NULL, exp_key3_out, exp_word_key3_out);
+	printf("\nkey4\n");
+	failed |= run_test(key4_in, 2, NULL, exp_key4_out, exp_word_key4_out);
+	printf("\nkey5\n");
+	failed |= run_test(key5_in, 3, NULL, exp_key4_out, exp_word_key4_out);
+	printf("\nkey6 - 3\n");
+	failed |= run_test(key6_in, 3, NULL, exp_key6_out, exp_word_key6_out);
+	printf("\nkey6 - 2\n");
+	failed |= run_test(key6_in, 2, NULL, exp_key6_out, exp_word_key6_out);
+	printf("\nkey6 - 1\n");
+	failed |= run_test(key6_in, 1, NULL, exp_key6_out, exp_word_key6_out);
+	printf("\nkey7\n");
+	failed |= run_test(key7_in, 3, NULL, exp_key7_out, exp_word_key7_out);
+	printf("\n");
+
+	if (failed != 0) {
+		printf("TEST FAILED\n");
+	}
+	return failed;
+}
+
+#endif				/* SELF_TEST */
diff --git a/drivers/mxc/security/rng/fsl_shw_hash.c b/drivers/mxc/security/rng/fsl_shw_hash.c
new file mode 100644
index 000000000000..60572862a0fa
--- /dev/null
+++ b/drivers/mxc/security/rng/fsl_shw_hash.c
@@ -0,0 +1,84 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+
+/*!
+ * @file fsl_shw_hash.c
+ *
+ * This file implements Cryptographic Hashing functions of the FSL SHW API
+ * for Sahara.  This does not include HMAC.
+ */
+
+#include "shw_driver.h"
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HASH-005 */
+/*!
+ * Hash a stream of data with a cryptographic hash algorithm.
+ *
+ * The flags in the @a hash_ctx control the operation of this function.
+ *
+ * Hashing functions work on 64 octets of message at a time.  Therefore, when
+ * any partial hashing of a long message is performed, the message @a length of
+ * each segment must be a multiple of 64.  When ready to
+ * #FSL_HASH_FLAGS_FINALIZE the hash, the @a length may be any value.
+ *
+ * With the #FSL_HASH_FLAGS_INIT and #FSL_HASH_FLAGS_FINALIZE flags on, a
+ * one-shot complete hash, including padding, will be performed.  The @a length
+ * may be any value.
+ *
+ * The first octets of a data stream can be hashed by setting the
+ * #FSL_HASH_FLAGS_INIT and #FSL_HASH_FLAGS_SAVE flags.  The @a length must be
+ * a multiple of 64.
+ *
+ * The flag #FSL_HASH_FLAGS_LOAD is used to load a context previously saved by
+ * #FSL_HASH_FLAGS_SAVE.  The two in combination will allow a (multiple-of-64
+ * octets) 'middle sequence' of the data stream to be hashed with the
+ * beginning.  The @a length must again be a multiple of 64.
+ *
+ * Since the flag #FSL_HASH_FLAGS_LOAD is used to load a context previously
+ * saved by #FSL_HASH_FLAGS_SAVE, the #FSL_HASH_FLAGS_LOAD and
+ * #FSL_HASH_FLAGS_FINALIZE flags, used together, can be used to finish the
+ * stream.  The @a length may be any value.
+ *
+ * If the user program wants to do the padding for the hash, it can leave off
+ * the #FSL_HASH_FLAGS_FINALIZE flag.  The @a length must then be a multiple of
+ * 64 octets.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param[in,out] hash_ctx Hashing algorithm and state of the cipher.
+ * @param      msg       Pointer to the data to be hashed.
+ * @param      length    Length, in octets, of the @a msg.
+ * @param[out] result    If not null, pointer to where to store the hash
+ *                       digest.
+ * @param      result_len Number of octets to store in @a result.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_hash(fsl_shw_uco_t * user_ctx,
+			      fsl_shw_hco_t * hash_ctx,
+			      const uint8_t * msg,
+			      uint32_t length,
+			      uint8_t * result, uint32_t result_len)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+
+	/* Unused */
+	(void)user_ctx;
+	(void)hash_ctx;
+	(void)msg;
+	(void)length;
+	(void)result;
+	(void)result_len;
+
+	return ret;
+}
diff --git a/drivers/mxc/security/rng/fsl_shw_hmac.c b/drivers/mxc/security/rng/fsl_shw_hmac.c
new file mode 100644
index 000000000000..4d2a104afcd8
--- /dev/null
+++ b/drivers/mxc/security/rng/fsl_shw_hmac.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+
+/*!
+ * @file fsl_shw_hmac.c
+ *
+ * This file implements Hashed Message Authentication Code functions of the FSL
+ * SHW API.
+ */
+
+#include "shw_driver.h"
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HMAC-001 */
+/*!
+ * Get the precompute information
+ *
+ *
+ * @param   user_ctx
+ * @param   key_info
+ * @param   hmac_ctx
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_hmac_precompute(fsl_shw_uco_t * user_ctx,
+					 fsl_shw_sko_t * key_info,
+					 fsl_shw_hmco_t * hmac_ctx)
+{
+	fsl_shw_return_t status = FSL_RETURN_ERROR_S;
+
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+	(void)hmac_ctx;
+
+	return status;
+}
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HMAC-002 */
+/*!
+ * Get the hmac
+ *
+ *
+ * @param         user_ctx    Info for acquiring memory
+ * @param         key_info
+ * @param         hmac_ctx
+ * @param         msg
+ * @param         length
+ * @param         result
+ * @param         result_len
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_hmac(fsl_shw_uco_t * user_ctx,
+			      fsl_shw_sko_t * key_info,
+			      fsl_shw_hmco_t * hmac_ctx,
+			      const uint8_t * msg,
+			      uint32_t length,
+			      uint8_t * result, uint32_t result_len)
+{
+	fsl_shw_return_t status = FSL_RETURN_ERROR_S;
+
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+	(void)hmac_ctx;
+	(void)msg;
+	(void)length;
+	(void)result;
+	(void)result_len;
+
+	return status;
+}
diff --git a/drivers/mxc/security/rng/fsl_shw_rand.c b/drivers/mxc/security/rng/fsl_shw_rand.c
new file mode 100644
index 000000000000..aa4d426c70fe
--- /dev/null
+++ b/drivers/mxc/security/rng/fsl_shw_rand.c
@@ -0,0 +1,122 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+
+/*!
+ * @file fsl_shw_rand.c
+ *
+ * This file implements Random Number Generation functions of the FSL SHW API
+ * in USER MODE for talking to a standalone RNGA/RNGC device driver.
+ *
+ * It contains the fsl_shw_get_random() and fsl_shw_add_entropy() functions.
+ *
+ * These routines will build a request block and pass it to the SHW driver.
+ */
+
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/fcntl.h>
+#include <sys/ioctl.h>
+#include <signal.h>
+
+#ifdef FSL_DEBUG
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+#endif				/* FSL_DEBUG */
+
+#include "shw_driver.h"
+
+extern fsl_shw_return_t validate_uco(fsl_shw_uco_t * uco);
+
+#if defined(FSL_HAVE_RNGA) || defined(FSL_HAVE_RNGB) || defined(FSL_HAVE_RNGC)
+
+/* REQ-S2LRD-PINTFC-API-BASIC-RNG-002 */
+fsl_shw_return_t fsl_shw_get_random(fsl_shw_uco_t * user_ctx,
+				    uint32_t length, uint8_t * data)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+
+	/* perform a sanity check / update uco */
+	ret = validate_uco(user_ctx);
+	if (ret == FSL_RETURN_OK_S) {
+		struct get_random_req *req = malloc(sizeof(*req));
+
+		if (req == NULL) {
+			ret = FSL_RETURN_NO_RESOURCE_S;
+		} else {
+
+			init_req(&req->hdr, user_ctx);
+			req->size = length;
+			req->random = data;
+
+			ret =
+			    send_req(SHW_USER_REQ_GET_RANDOM, &req->hdr,
+				     user_ctx);
+		}
+	}
+
+	return ret;
+}
+
+fsl_shw_return_t fsl_shw_add_entropy(fsl_shw_uco_t * user_ctx,
+				     uint32_t length, uint8_t * data)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+
+	/* perform a sanity check on the uco */
+	ret = validate_uco(user_ctx);
+	if (ret == FSL_RETURN_OK_S) {
+		struct add_entropy_req *req = malloc(sizeof(*req));
+
+		if (req == NULL) {
+			ret = FSL_RETURN_NO_RESOURCE_S;
+		} else {
+			init_req(&req->hdr, user_ctx);
+			req->size = length;
+			req->entropy = data;
+
+			ret =
+			    send_req(SHW_USER_REQ_ADD_ENTROPY, &req->hdr,
+				     user_ctx);
+		}
+	}
+
+	return ret;
+}
+
+#else				/* no H/W RNG block */
+
+fsl_shw_return_t fsl_shw_get_random(fsl_shw_uco_t * user_ctx,
+				    uint32_t length, uint8_t * data)
+{
+
+	(void)user_ctx;
+	(void)length;
+	(void)data;
+
+	return FSL_RETURN_ERROR_S;
+}
+
+fsl_shw_return_t fsl_shw_add_entropy(fsl_shw_uco_t * user_ctx,
+				     uint32_t length, uint8_t * data)
+{
+
+	(void)user_ctx;
+	(void)length;
+	(void)data;
+
+	return FSL_RETURN_ERROR_S;
+}
+#endif
diff --git a/drivers/mxc/security/rng/fsl_shw_sym.c b/drivers/mxc/security/rng/fsl_shw_sym.c
new file mode 100644
index 000000000000..e844e3ae7420
--- /dev/null
+++ b/drivers/mxc/security/rng/fsl_shw_sym.c
@@ -0,0 +1,317 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+
+/*!
+ * @file fsl_shw_sym.c
+ *
+ * This file implements the Symmetric Cipher functions of the FSL SHW API. Its
+ * features are limited to what can be done with the combination of SCC and
+ * DryIce.
+ */
+#include "fsl_platform.h"
+#include "shw_driver.h"
+
+#if defined(__KERNEL__) && defined(FSL_HAVE_DRYICE)
+
+#include "../dryice.h"
+#include <linux/mxc_scc_driver.h>
+#ifdef DIAG_SECURITY_FUNC
+#include "apihelp.h"
+#endif
+
+#include <diagnostic.h>
+
+#define SYM_DECRYPT 0
+#define SYM_ENCRYPT 1
+
+extern fsl_shw_return_t shw_convert_pf_key(fsl_shw_pf_key_t shw_pf_key,
+					   di_key_t * di_keyp);
+
+/*! 'Initial' IV for presence of FSL_SYM_CTX_LOAD flag */
+static uint8_t zeros[8] = {
+	0, 0, 0, 0, 0, 0, 0, 0
+};
+
+/*!
+ * Common function for encryption and decryption
+ *
+ * This is for a device with DryIce.
+ *
+ * A key must either refer to a 'pure' HW key, or, if PRG or PRG_IIM,
+ * established, then that key will be programmed.  Then, the HW_key in the
+ * object will be selected.  After this setup, the ciphering will be performed
+ * by calling the SCC driver..
+ *
+ * The function 'releases' the reservations before it completes.
+ */
+fsl_shw_return_t do_symmetric(fsl_shw_uco_t * user_ctx,
+			      fsl_shw_sko_t * key_info,
+			      fsl_shw_scco_t * sym_ctx,
+			      int encrypt,
+			      uint32_t length,
+			      const uint8_t * in, uint8_t * out)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+	int key_selected = 0;
+	uint8_t *iv = NULL;
+	unsigned long count_out = length;
+	di_key_t di_key = DI_KEY_PK;	/* default for user key */
+	di_key_t di_key_orig;	/* currently selected key */
+	di_key_t selected_key = -1;
+	di_return_t di_code;
+	scc_return_t scc_code;
+
+	/* For now, only blocking mode calls are supported */
+	if (!(user_ctx->flags & FSL_UCO_BLOCKING_MODE)) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	/* No software keys allowed */
+	if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+	}
+
+	/* The only algorithm the SCC supports */
+	if (key_info->algorithm != FSL_KEY_ALG_TDES) {
+		ret = FSL_RETURN_BAD_ALGORITHM_S;
+		goto out;
+	}
+
+	/* Validate key length */
+	if ((key_info->key_length != 16)
+	    && (key_info->key_length != 21)
+	    && (key_info->key_length != 24)) {
+		ret = FSL_RETURN_BAD_KEY_LENGTH_S;
+		goto out;
+	}
+
+	/* Validate data is multiple of DES/TDES block */
+	if ((length & 7) != 0) {
+		ret = FSL_RETURN_BAD_DATA_LENGTH_S;
+		goto out;
+	}
+
+	/* Do some setup according to where the key lives */
+	if (key_info->flags & FSL_SKO_KEY_ESTABLISHED) {
+		if ((key_info->pf_key != FSL_SHW_PF_KEY_PRG)
+		    && (key_info->pf_key != FSL_SHW_PF_KEY_IIM_PRG)) {
+			ret = FSL_RETURN_ERROR_S;
+		}
+	} else if (key_info->flags & FSL_SKO_KEY_PRESENT) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+	} else if (key_info->flags & FSL_SKO_KEY_SELECT_PF_KEY) {
+		/*
+		 * No key present or established, just refer to HW
+		 * as programmed.
+		 */
+	} else {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	/* Now make proper selection */
+	ret = shw_convert_pf_key(key_info->pf_key, &di_key);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Determine the current DI key selection */
+	di_code = dryice_check_key(&di_key_orig);
+	if (di_code != DI_SUCCESS) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("Could not save current DI key state: %s\n",
+					  di_error_string(di_code));
+#endif
+		ret = FSL_RETURN_ERROR_S;
+		goto out;
+	}
+	
+	/* If the requested DI key is already selected, don't re-select it. */
+	if (di_key != di_key_orig) {
+	   di_code = dryice_select_key(di_key, 0);
+	   if (di_code != DI_SUCCESS) {
+#ifdef DIAG_SECURITY_FUNC
+		  LOG_DIAG_ARGS("Error from select_key: %s\n",
+			      di_error_string(di_code));
+#endif
+		  ret = FSL_RETURN_INTERNAL_ERROR_S;
+		  goto out;
+	   }
+	}
+	key_selected = 1;
+
+	/* Verify that we are using the key we want */
+	di_code = dryice_check_key(&selected_key);
+	if (di_code != DI_SUCCESS) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("Error from check_key: %s\n",
+			      di_error_string(di_code));
+#endif
+		ret = FSL_RETURN_INTERNAL_ERROR_S;
+		goto out;
+	}
+
+	if (di_key != selected_key) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("Wrong key in use: %d instead of %d\n\n",
+			      selected_key, di_key);
+#endif
+		ret = FSL_RETURN_ERROR_S;
+		goto out;
+	}
+
+	if (sym_ctx->mode == FSL_SYM_MODE_CBC) {
+		if ((sym_ctx->flags & FSL_SYM_CTX_LOAD)
+		    && !(sym_ctx->flags & FSL_SYM_CTX_INIT)) {
+			iv = sym_ctx->context;
+		} else if ((sym_ctx->flags & FSL_SYM_CTX_INIT)
+			   && !(sym_ctx->flags & FSL_SYM_CTX_LOAD)) {
+			iv = zeros;
+		} else {
+			/* Exactly one must be set! */
+			ret = FSL_RETURN_BAD_FLAG_S;
+			goto out;
+		}
+	}
+
+	/* Now run the data through the SCC */
+	scc_code = scc_crypt(length, in, iv,
+			     encrypt ? SCC_ENCRYPT : SCC_DECRYPT,
+			     (sym_ctx->mode == FSL_SYM_MODE_ECB)
+			     ? SCC_ECB_MODE : SCC_CBC_MODE,
+			     SCC_VERIFY_MODE_NONE, out, &count_out);
+	if (scc_code != SCC_RET_OK) {
+		ret = FSL_RETURN_INTERNAL_ERROR_S;
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("scc_code from scc_crypt() is %d\n", scc_code);
+#endif
+		goto out;
+	}
+
+	if ((sym_ctx->mode == FSL_SYM_MODE_CBC)
+	    && (sym_ctx->flags & FSL_SYM_CTX_SAVE)) {
+		/* Save the context for the caller */
+		if (encrypt) {
+			/* Last ciphertext block ... */
+			memcpy(sym_ctx->context, out + length - 8, 8);
+		} else {
+			/* Last ciphertext block ... */
+			memcpy(sym_ctx->context, in + length - 8, 8);
+		}
+	}
+
+	ret = FSL_RETURN_OK_S;
+
+      out:
+	if (key_selected) {
+		(void)dryice_release_key_selection();
+	}
+
+	return ret;
+}
+
+EXPORT_SYMBOL(fsl_shw_symmetric_encrypt);
+/*!
+ * Compute symmetric encryption
+ *
+ *
+ * @param    user_ctx
+ * @param    key_info
+ * @param    sym_ctx
+ * @param    length
+ * @param    pt
+ * @param    ct
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_symmetric_encrypt(fsl_shw_uco_t * user_ctx,
+					   fsl_shw_sko_t * key_info,
+					   fsl_shw_scco_t * sym_ctx,
+					   uint32_t length,
+					   const uint8_t * pt, uint8_t * ct)
+{
+	fsl_shw_return_t ret;
+
+	ret = do_symmetric(user_ctx, key_info, sym_ctx, SYM_ENCRYPT,
+			   length, pt, ct);
+
+	return ret;
+}
+
+EXPORT_SYMBOL(fsl_shw_symmetric_decrypt);
+/*!
+ * Compute symmetric decryption
+ *
+ *
+ * @param    user_ctx
+ * @param    key_info
+ * @param    sym_ctx
+ * @param    length
+ * @param    pt
+ * @param    ct
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_symmetric_decrypt(fsl_shw_uco_t * user_ctx,
+					   fsl_shw_sko_t * key_info,
+					   fsl_shw_scco_t * sym_ctx,
+					   uint32_t length,
+					   const uint8_t * ct, uint8_t * pt)
+{
+	fsl_shw_return_t ret;
+
+	ret = do_symmetric(user_ctx, key_info, sym_ctx, SYM_DECRYPT,
+			   length, ct, pt);
+
+	return ret;
+}
+
+#else				/* __KERNEL__ && DRYICE */
+
+fsl_shw_return_t fsl_shw_symmetric_encrypt(fsl_shw_uco_t * user_ctx,
+					   fsl_shw_sko_t * key_info,
+					   fsl_shw_scco_t * sym_ctx,
+					   uint32_t length,
+					   const uint8_t * pt, uint8_t * ct)
+{
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+	(void)sym_ctx;
+	(void)length;
+	(void)pt;
+	(void)ct;
+
+	return FSL_RETURN_ERROR_S;
+}
+
+fsl_shw_return_t fsl_shw_symmetric_decrypt(fsl_shw_uco_t * user_ctx,
+					   fsl_shw_sko_t * key_info,
+					   fsl_shw_scco_t * sym_ctx,
+					   uint32_t length,
+					   const uint8_t * ct, uint8_t * pt)
+{
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+	(void)sym_ctx;
+	(void)length;
+	(void)ct;
+	(void)pt;
+
+	return FSL_RETURN_ERROR_S;
+}
+
+#endif				/* __KERNEL__ and DRYICE */
diff --git a/drivers/mxc/security/rng/fsl_shw_wrap.c b/drivers/mxc/security/rng/fsl_shw_wrap.c
new file mode 100644
index 000000000000..90e241e5af86
--- /dev/null
+++ b/drivers/mxc/security/rng/fsl_shw_wrap.c
@@ -0,0 +1,1301 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_shw_wrap.c
+ *
+ * This file implements Key-Wrap (Black Key) and Key Establishment functions of
+ * the FSL SHW API for the SHW (non-SAHARA) driver.
+ *
+ * This is the Black Key information:
+ *
+ * <ul>
+ * <li> Ownerid is an 8-byte, user-supplied, value to keep KEY
+ *      confidential.</li>
+ * <li> KEY is a 1-32 byte value which starts in SCC RED RAM before
+ *     wrapping, and ends up there on unwrap.  Length is limited because of
+ *    size of SCC1 RAM.</li>
+ * <li> KEY' is the encrypted KEY</li>
+ * <li> LEN is a 1-byte (for now) byte-length of KEY</li>
+ * <li> ALG is a 1-byte value for the algorithm which which the key is
+ *    associated.  Values are defined by the FSL SHW API</li>
+ * <li> FLAGS is a 1-byte value contain information like "this key is for
+ *    software" (TBD)</li>
+ * <li> Ownerid, LEN, and ALG come from the user's "key_info" object, as does
+ *    the slot number where KEY already is/will be.</li>
+ * <li> T is a Nonce</li>
+ * <li> T' is the encrypted T</li>
+ * <li> KEK is a Key-Encryption Key for the user's Key</li>
+ * <li> ICV is the "Integrity Check Value" for the wrapped key</li>
+ * <li> Black Key is the string of bytes returned as the wrapped key</li>
+ * <li> Wrap Key is the user's choice for encrypting the nonce.  One of
+ *      the Fused Key, the Random Key, or the XOR of the two.
+ * </ul>
+<table border="0">
+<tr><TD align="right">BLACK_KEY <TD width="3">=<TD>ICV | T' | LEN | ALG |
+     FLAGS | KEY'</td></tr>
+<tr><td>&nbsp;</td></tr>
+
+<tr><th>To Wrap</th></tr>
+<tr><TD align="right">T</td> <TD width="3">=</td> <TD>RND()<sub>16</sub>
+    </td></tr>
+<tr><TD align="right">KEK</td><TD width="3">=</td><TD>HASH<sub>sha256</sub>(T |
+     Ownerid)<sub>16</sub></td></tr>
+<tr><TD align="right">KEY'<TD width="3">=</td><TD>
+     TDES<sub>cbc-enc</sub>(Key=KEK, Data=KEY, IV=Ownerid)</td></tr>
+<tr><TD align="right">ICV</td><TD width="3">=</td><td>HMAC<sub>sha256</sub>
+     (Key=T, Data=Ownerid | LEN | ALG | FLAGS | KEY')<sub>16</sub></td></tr>
+<tr><TD align="right">T'</td><TD width="3">=</td><TD>TDES<sub>ecb-enc</sub>
+     (Key=Wrap_Key, IV=Ownerid, Data=T)</td></tr>
+
+<tr><td>&nbsp;</td></tr>
+
+<tr><th>To Unwrap</th></tr>
+<tr><TD align="right">T</td><TD width="3">=</td><TD>TDES<sub>ecb-dec</sub>
+    (Key=Wrap_Key, IV=Ownerid, Data=T')</td></tr>
+<tr><TD align="right">ICV</td><TD width="3">=</td><td>HMAC<sub>sha256</sub>
+    (Key=T, Data=Ownerid | LEN | ALG | FLAGS | KEY')<sub>16</sub></td></tr>
+<tr><TD align="right">KEK</td><TD width="3">=</td><td>HASH<sub>sha256</sub>
+    (T | Ownerid)<sub>16</sub></td></tr>
+<tr><TD align="right">KEY<TD width="3">=</td><TD>TDES<sub>cbc-dec</sub>
+    (Key=KEK, Data=KEY', IV=Ownerid)</td></tr>
+</table>
+
+ * This code supports two types of keys: Software Keys and keys destined for
+ * (or residing in) the DryIce Programmed Key Register.
+ *
+ * Software Keys go to / from the keystore.
+ *
+ * PK keys go to / from the DryIce Programmed Key Register.
+ *
+ * This code only works on a platform with DryIce.  "software" keys go into
+ * the keystore.  "Program" keys go to the DryIce Programmed Key Register.
+ * As far as this code is concerned, the size of that register is 21 bytes,
+ * the size of a 3DES key with parity stripped.
+ *
+ * The maximum key size supported for wrapped/unwrapped keys depends upon
+ * LENGTH_LENGTH.  Currently, it is one byte, so the maximum key size is
+ * 255 bytes.  However, key objects cannot currently hold a key of this
+ * length, so a smaller key size is the max.
+ */
+
+#include "fsl_platform.h"
+	 
+/* This code only works in kernel mode */
+
+#include "shw_driver.h"
+#ifdef DIAG_SECURITY_FUNC
+#include "apihelp.h"
+#endif
+
+#if defined(__KERNEL__) && defined(FSL_HAVE_DRYICE)
+
+#include "../dryice.h"
+#include <linux/mxc_scc_driver.h>
+
+#include "portable_os.h"
+#include "fsl_shw_keystore.h"
+
+#include <diagnostic.h>
+
+#include "shw_hmac.h"
+#include "shw_hash.h"
+
+#define ICV_LENGTH 16
+#define T_LENGTH 16
+#define KEK_LENGTH 21
+#define LENGTH_LENGTH 1
+#define ALGORITHM_LENGTH 1
+#define FLAGS_LENGTH 1
+
+/* ICV | T' | LEN | ALG | FLAGS | KEY' */
+#define ICV_OFFSET       0
+#define T_PRIME_OFFSET   (ICV_OFFSET + ICV_LENGTH)
+#define LENGTH_OFFSET    (T_PRIME_OFFSET + T_LENGTH)
+#define ALGORITHM_OFFSET (LENGTH_OFFSET + LENGTH_LENGTH)
+#define FLAGS_OFFSET     (ALGORITHM_OFFSET + ALGORITHM_LENGTH)
+#define KEY_PRIME_OFFSET (FLAGS_OFFSET + FLAGS_LENGTH)
+
+#define FLAGS_SW_KEY     0x01
+
+#define LENGTH_PATCH  8
+#define LENGTH_PATCH_MASK  (LENGTH_PATCH - 1)
+
+/*! rounded up from 168 bits to the next word size */
+#define HW_KEY_LEN_WORDS_BITS 192
+
+/*!
+ * Round a key length up to the TDES block size
+ *
+ * @param len         Length of key, in bytes
+ *
+ * @return Length rounded up, if necessary
+ */
+#define ROUND_LENGTH(len)                                                  \
+({                                                                         \
+    uint32_t orig_len = len;                                               \
+    uint32_t new_len;                                                      \
+                                                                           \
+   if ((orig_len & LENGTH_PATCH_MASK) != 0) {                              \
+       new_len = (orig_len + LENGTH_PATCH                                  \
+            - (orig_len & LENGTH_PATCH_MASK));                             \
+   }                                                                       \
+   else {                                                                  \
+       new_len = orig_len;                                                 \
+   }                                                                       \
+                                                                           \
+   new_len;                                                                \
+})
+
+/* This is the system keystore object */
+extern fsl_shw_kso_t system_keystore;
+
+#ifdef DIAG_SECURITY_FUNC
+static void dump(const char *name, const uint8_t * data, unsigned int len)
+{
+	os_printk("%s: ", name);
+	while (len > 0) {
+		os_printk("%02x ", (unsigned)*data++);
+		len--;
+	}
+	os_printk("\n");
+}
+#endif
+
+/*
+ * For testing of the algorithm implementation,, the DO_REPEATABLE_WRAP flag
+ * causes the T_block to go into the T field during a wrap operation.  This
+ * will make the black key value repeatable (for a given SCC secret key, or
+ * always if the default key is in use).
+ *
+ * Normally, a random sequence is used.
+ */
+#ifdef DO_REPEATABLE_WRAP
+/*!
+ * Block of zeroes which is maximum Symmetric block size, used for
+ * initializing context register, etc.
+ */
+static uint8_t T_block[16] = {
+	0x42, 0, 0, 0x42, 0x42, 0, 0, 0x42,
+	0x42, 0, 0, 0x42, 0x42, 0, 0, 0x42
+};
+#endif
+
+EXPORT_SYMBOL(fsl_shw_establish_key);
+EXPORT_SYMBOL(fsl_shw_read_key);
+EXPORT_SYMBOL(fsl_shw_extract_key);
+EXPORT_SYMBOL(fsl_shw_release_key);
+
+extern fsl_shw_return_t alloc_slot(fsl_shw_uco_t * user_ctx,
+				   fsl_shw_sko_t * key_info);
+
+extern fsl_shw_return_t load_slot(fsl_shw_uco_t * user_ctx,
+				  fsl_shw_sko_t * key_info,
+				  const uint8_t * key);
+
+extern fsl_shw_return_t dealloc_slot(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info);
+
+/*!
+ * Initalialize SKO and SCCO used for T <==> T' cipher operation
+ *
+ * @param wrap_key  Which wrapping key user wants
+ * @param key_info  Key object for selecting wrap key
+ * @param wrap_ctx  Sym Context object for doing the cipher op
+ */
+static inline void init_wrap_key(fsl_shw_pf_key_t wrap_key,
+				 fsl_shw_sko_t * key_info,
+				 fsl_shw_scco_t * wrap_ctx)
+{
+	fsl_shw_sko_init_pf_key(key_info, FSL_KEY_ALG_TDES, wrap_key);
+	fsl_shw_scco_init(wrap_ctx, FSL_KEY_ALG_TDES, FSL_SYM_MODE_ECB);
+}
+
+/*!
+ * Insert descriptors to calculate ICV = HMAC(key=T, data=LEN|ALG|KEY')
+ *
+ * @param  user_ctx      A user context from #fsl_shw_register_user().
+ * @param  T             Location of nonce (length is T_LENGTH bytes)
+ * @param  userid        Location of userid/ownerid
+ * @param  userid_len    Length, in bytes of @c userid
+ * @param  black_key     Beginning of Black Key region
+ * @param  key_length    Number of bytes of key' there are in @c black_key
+ * @param[out] hmac      Location to store ICV.  Will be tagged "USES" so
+ *                       sf routines will not try to free it.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static fsl_shw_return_t calc_icv(const uint8_t * T,
+				 const uint8_t * userid,
+				 unsigned int userid_len,
+				 const uint8_t * black_key,
+				 uint32_t key_length, uint8_t * hmac)
+{
+	fsl_shw_return_t code;
+	shw_hmac_state_t hmac_state;
+
+	/* Load up T as key for the HMAC */
+	code = shw_hmac_init(&hmac_state, T, T_LENGTH);
+	if (code != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Previous step loaded key; Now set up to hash the data */
+
+	/* Input - start with ownerid */
+	code = shw_hmac_update(&hmac_state, userid, userid_len);
+	if (code != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Still input  - Append black-key fields len, alg, key' */
+	code = shw_hmac_update(&hmac_state,
+			       (void *)black_key + LENGTH_OFFSET,
+			       (LENGTH_LENGTH
+				+ ALGORITHM_LENGTH
+				+ FLAGS_LENGTH + key_length));
+	if (code != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Output - computed ICV/HMAC */
+	code = shw_hmac_final(&hmac_state, hmac, ICV_LENGTH);
+	if (code != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+      out:
+
+	return code;
+}				/* calc_icv */
+
+/*!
+ * Compute and return the KEK (Key Encryption Key) from the inputs
+ *
+ * @param userid      The user's 'secret' for the key
+ * @param userid_len  Length, in bytes of @c userid
+ * @param T           The nonce
+ * @param[out] kek    Location to store the computed KEK. It will
+ *                    be 21 bytes long.
+ *
+ * @return the usual error code
+ */
+static fsl_shw_return_t calc_kek(const uint8_t * userid,
+				 unsigned int userid_len,
+				 const uint8_t * T, uint8_t * kek)
+{
+	fsl_shw_return_t code = FSL_RETURN_INTERNAL_ERROR_S;
+	shw_hash_state_t hash_state;
+
+	code = shw_hash_init(&hash_state, FSL_HASH_ALG_SHA256);
+	if (code != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+	LOG_DIAG_ARGS("Hash init failed: %s\n", fsl_error_string(code));
+#endif
+		goto out;
+	}
+
+	code = shw_hash_update(&hash_state, T, T_LENGTH);
+	if (code != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+	LOG_DIAG_ARGS("Hash for T failed: %s\n",
+			      fsl_error_string(code));
+#endif
+		goto out;
+	}
+
+	code = shw_hash_update(&hash_state, userid, userid_len);
+	if (code != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("Hash for userid failed: %s\n",
+			      fsl_error_string(code));
+#endif
+		goto out;
+	}
+
+	code = shw_hash_final(&hash_state, kek, KEK_LENGTH);
+	if (code != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("Could not extract kek: %s\n",
+			      fsl_error_string(code));
+#endif
+		goto out;
+	}
+
+#if KEK_LENGTH != 21
+	{
+		uint8_t permuted_kek[21];
+
+		fsl_shw_permute1_bytes(kek, permuted_kek, KEK_LENGTH / 8);
+		memcpy(kek, permuted_kek, 21);
+		memset(permuted_kek, 0, sizeof(permuted_kek));
+	}
+#endif
+
+#ifdef DIAG_SECURITY_FUNC
+	dump("kek", kek, 21);
+#endif
+
+      out:
+
+	return code;
+}				/* end fn calc_kek */
+
+/*!
+ * Validate user's wrap key selection
+ *
+ * @param wrap_key   The user's desired wrapping key
+ */
+static fsl_shw_return_t check_wrap_key(fsl_shw_pf_key_t wrap_key)
+{
+	/* unable to use desired key */
+	fsl_shw_return_t ret = FSL_RETURN_NO_RESOURCE_S;
+
+	if ((wrap_key != FSL_SHW_PF_KEY_IIM) &&
+	    (wrap_key != FSL_SHW_PF_KEY_RND) &&
+	    (wrap_key != FSL_SHW_PF_KEY_IIM_RND)) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Invalid wrap_key in key wrap/unwrap attempt");
+#endif
+		goto out;
+	}
+	ret = FSL_RETURN_OK_S;
+
+      out:
+	return ret;
+}				/* end fn check_wrap_key */
+
+/*!
+ * Perform unwrapping of a black key into a RED slot
+ *
+ * @param         user_ctx      A user context from #fsl_shw_register_user().
+ * @param[in,out] key_info      The information about the key to be which will
+ *                              be unwrapped... key length, slot info, etc.
+ * @param         black_key     Encrypted key
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static fsl_shw_return_t unwrap(fsl_shw_uco_t * user_ctx,
+			       fsl_shw_sko_t * key_info,
+			       const uint8_t * black_key)
+{
+	fsl_shw_return_t ret;
+	uint8_t hmac[ICV_LENGTH];
+	uint8_t T[T_LENGTH];
+	uint8_t kek[KEK_LENGTH + 20];
+	int key_length = black_key[LENGTH_OFFSET];
+	int rounded_key_length = ROUND_LENGTH(key_length);
+	uint8_t key[rounded_key_length];
+	fsl_shw_sko_t t_key_info;
+	fsl_shw_scco_t t_key_ctx;
+	fsl_shw_sko_t kek_key_info;
+	fsl_shw_scco_t kek_ctx;
+	int unwrapping_sw_key = key_info->flags & FSL_SKO_KEY_SW_KEY;
+	int pk_needs_restoration = 0;	/* bool */
+	unsigned original_key_length = key_info->key_length;
+	int pk_was_held = 0;
+	uint8_t current_pk[21];
+	di_return_t di_code;
+
+	ret = check_wrap_key(user_ctx->wrap_key);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	if (black_key == NULL) {
+		ret = FSL_RETURN_ERROR_S;
+		goto out;
+	}
+#ifdef DIAG_SECURITY_FUNC
+	dump("black", black_key, KEY_PRIME_OFFSET + key_length);
+#endif
+	/* Validate SW flags to prevent misuse */
+		if ((key_info->flags & FSL_SKO_KEY_SW_KEY)
+			&& !(black_key[FLAGS_OFFSET] & FLAGS_SW_KEY)) {
+			ret = FSL_RETURN_BAD_FLAG_S;
+			goto out;
+		}
+
+	/* Compute T = 3des-dec-ecb(wrap_key, T') */
+	init_wrap_key(user_ctx->wrap_key, &t_key_info, &t_key_ctx);
+	ret = fsl_shw_symmetric_decrypt(user_ctx, &t_key_info, &t_key_ctx,
+					T_LENGTH,
+					black_key + T_PRIME_OFFSET, T);
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Recovery of nonce (T) failed");
+#endif				/*DIAG_SECURITY_FUNC */
+		goto out;
+	}
+
+	/* Compute ICV = HMAC(T, ownerid | len | alg | flags | key' */
+	ret = calc_icv(T, (uint8_t *) & key_info->userid,
+		       sizeof(key_info->userid),
+		       black_key, original_key_length, hmac);
+
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Calculation of ICV failed");
+#endif				/*DIAG_SECURITY_FUNC */
+		goto out;
+	}
+#ifdef DIAG_SECURITY_FUNC
+	LOG_DIAG("Validating MAC of wrapped key");
+#endif
+
+	/* Check computed ICV against value in Black Key */
+	if (memcmp(black_key + ICV_OFFSET, hmac, ICV_LENGTH) != 0) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Computed ICV fails validation\n");
+#endif
+		ret = FSL_RETURN_AUTH_FAILED_S;
+		goto out;
+	}
+
+	/* Compute KEK = SHA256(T | ownerid). */
+	ret = calc_kek((uint8_t *) & key_info->userid, sizeof(key_info->userid),
+		       T, kek);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	if (unwrapping_sw_key) {
+		di_code = dryice_get_programmed_key(current_pk, 8 * 21);
+		if (di_code != DI_SUCCESS) {
+#ifdef DIAG_SECURITY_FUNC
+			LOG_DIAG_ARGS("Could not save current PK: %s\n",
+				      di_error_string(di_code));
+#endif
+			ret = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+	}
+
+	/*
+	 * "Establish" the KEK in the PK.  If the PK was held and unwrapping a
+	 * software key, then release it and try again, but remember that we need
+	 * to leave it 'held' if we are unwrapping a software key.
+	 *
+	 * If the PK is held while we are unwrapping a key for the PK, then
+	 * the user didn't call release, so gets an error.
+	 */
+	di_code = dryice_set_programmed_key(kek, 8 * 21, 0);
+	if ((di_code == DI_ERR_INUSE) && unwrapping_sw_key) {
+		/* Temporarily reprogram the PK out from under the user */
+		pk_was_held = 1;
+		dryice_release_programmed_key();
+		di_code = dryice_set_programmed_key(kek, 8 * 21, 0);
+	}
+	if (di_code != DI_SUCCESS) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("Could not program KEK: %s\n",
+			      di_error_string(di_code));
+#endif
+		ret = FSL_RETURN_ERROR_S;
+		goto out;
+	}
+
+	if (unwrapping_sw_key) {
+		pk_needs_restoration = 1;
+	}
+	dryice_release_programmed_key();	/* Because of previous 'set' */
+
+	/* Compute KEY = TDES-decrypt(KEK, KEY') */
+	fsl_shw_sko_init_pf_key(&kek_key_info, FSL_KEY_ALG_TDES,
+				FSL_SHW_PF_KEY_PRG);
+	fsl_shw_sko_set_key_length(&kek_key_info, KEK_LENGTH);
+
+	fsl_shw_scco_init(&kek_ctx, FSL_KEY_ALG_TDES, FSL_SYM_MODE_CBC);
+	fsl_shw_scco_set_flags(&kek_ctx, FSL_SYM_CTX_LOAD);
+	fsl_shw_scco_set_context(&kek_ctx, (uint8_t *) & key_info->userid);
+#ifdef DIAG_SECURITY_FUNC
+	dump("KEY'", black_key + KEY_PRIME_OFFSET, rounded_key_length);
+#endif
+	ret = fsl_shw_symmetric_decrypt(user_ctx, &kek_key_info, &kek_ctx,
+					rounded_key_length,
+					black_key + KEY_PRIME_OFFSET, key);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+#ifdef DIAG_SECURITY_FUNC
+	dump("KEY", key, original_key_length);
+#endif
+	/* Now either put key into PK or into a slot */
+	if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+		ret = load_slot(user_ctx, key_info, key);
+	} else {
+		/*
+		 * Since we have just unwrapped a program key, it had
+		 * to have been wrapped as a program key, so it must
+		 * be 168 bytes long and permuted ...
+		 */
+		ret = dryice_set_programmed_key(key, 8 * key_length, 0);
+		if (ret != FSL_RETURN_OK_S) {
+			goto out;
+		}
+	}
+
+      out:
+	key_info->key_length = original_key_length;
+
+	if (pk_needs_restoration) {
+		di_code = dryice_set_programmed_key(current_pk, 8 * 21, 0);
+	}
+
+	if (!pk_was_held) {
+		dryice_release_programmed_key();
+	}
+
+	/* Erase tracks of confidential data */
+	memset(T, 0, T_LENGTH);
+	memset(key, 0, rounded_key_length);
+	memset(current_pk, 0, sizeof(current_pk));
+	memset(&t_key_info, 0, sizeof(t_key_info));
+	memset(&t_key_ctx, 0, sizeof(t_key_ctx));
+	memset(&kek_key_info, 0, sizeof(kek_key_info));
+	memset(&kek_ctx, 0, sizeof(kek_ctx));
+	memset(kek, 0, KEK_LENGTH);
+
+	return ret;
+}				/* unwrap */
+
+/*!
+ * Perform wrapping of a black key from a RED slot (or the PK register)
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param[in,out] key_info      The information about the key to be which will
+ *                              be wrapped... key length, slot info, etc.
+ * @param      black_key        Place to store encrypted key
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static fsl_shw_return_t wrap(fsl_shw_uco_t * user_ctx,
+			     fsl_shw_sko_t * key_info, uint8_t * black_key)
+{
+	fsl_shw_return_t ret = FSL_RETURN_OK_S;
+	fsl_shw_sko_t t_key_info;	/* for holding T */
+	fsl_shw_scco_t t_key_ctx;
+	fsl_shw_sko_t kek_key_info;
+	fsl_shw_scco_t kek_ctx;
+	unsigned original_key_length = key_info->key_length;
+	unsigned rounded_key_length;
+	uint8_t T[T_LENGTH];
+	uint8_t kek[KEK_LENGTH + 20];
+	uint8_t *red_key = 0;
+	int red_key_malloced = 0;	/* bool */
+	int pk_was_held = 0;	/* bool */
+	uint8_t saved_pk[21];
+	uint8_t pk_needs_restoration;	/* bool */
+	di_return_t di_code;
+
+	ret = check_wrap_key(user_ctx->wrap_key);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	if (black_key == NULL) {
+		ret = FSL_RETURN_ERROR_S;
+		goto out;
+	}
+
+	if (key_info->flags & FSL_SKO_KEY_SELECT_PF_KEY) {
+		if ((key_info->pf_key != FSL_SHW_PF_KEY_PRG)
+		    && (key_info->pf_key != FSL_SHW_PF_KEY_IIM_PRG)) {
+			ret = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+	} else {
+		if (!(key_info->flags & FSL_SKO_KEY_ESTABLISHED)) {
+			ret = FSL_RETURN_BAD_FLAG_S;	/* not established! */
+			goto out;
+		}
+	}
+
+	black_key[ALGORITHM_OFFSET] = key_info->algorithm;
+
+#ifndef DO_REPEATABLE_WRAP
+	/* Compute T = RND() */
+	ret = fsl_shw_get_random(user_ctx, T_LENGTH, T);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+#else
+	memcpy(T, T_block, T_LENGTH);
+#endif
+
+	/* Compute KEK = SHA256(T | ownerid). */
+	ret = calc_kek((uint8_t *) & key_info->userid, sizeof(key_info->userid),
+		       T, kek);
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Calculation of KEK failed\n");
+#endif				/*DIAG_SECURITY_FUNC */
+		goto out;
+	}
+
+	rounded_key_length = ROUND_LENGTH(original_key_length);
+
+	di_code = dryice_get_programmed_key(saved_pk, 8 * 21);
+	if (di_code != DI_SUCCESS) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("Could not save current PK: %s\n",
+			      di_error_string(di_code));
+#endif
+		ret = FSL_RETURN_ERROR_S;
+		goto out;
+	}
+
+	/*
+	 * Load KEK into DI PKR.  Note that we are NOT permuting it before loading,
+	 * so we are using it as though it is a 168-bit key ready for the SCC.
+	 */
+	di_code = dryice_set_programmed_key(kek, 8 * 21, 0);
+	if (di_code == DI_ERR_INUSE) {
+		/* Temporarily reprogram the PK out from under the user */
+		pk_was_held = 1;
+		dryice_release_programmed_key();
+		di_code = dryice_set_programmed_key(kek, 8 * 21, 0);
+	}
+	if (di_code != DI_SUCCESS) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("Could not program KEK: %s\n",
+			      di_error_string(di_code));
+#endif
+		ret = FSL_RETURN_ERROR_S;
+		goto out;
+	}
+	pk_needs_restoration = 1;
+	dryice_release_programmed_key();
+
+	/* Find red key */
+	if (key_info->flags & FSL_SKO_KEY_SELECT_PF_KEY) {
+		black_key[LENGTH_OFFSET] = 21;
+		rounded_key_length = 24;
+
+		red_key = saved_pk;
+	} else {
+		black_key[LENGTH_OFFSET] = key_info->key_length;
+
+		red_key = os_alloc_memory(key_info->key_length, 0);
+		if (red_key == NULL) {
+			ret = FSL_RETURN_NO_RESOURCE_S;
+			goto out;
+		}
+		red_key_malloced = 1;
+
+		ret = fsl_shw_read_key(user_ctx, key_info, red_key);
+		if (ret != FSL_RETURN_OK_S) {
+			goto out;
+		}
+	}
+
+#ifdef DIAG_SECURITY_FUNC
+	dump("KEY", red_key, black_key[LENGTH_OFFSET]);
+#endif
+	/* Compute KEY' = TDES-encrypt(KEK, KEY) */
+	fsl_shw_sko_init_pf_key(&kek_key_info, FSL_KEY_ALG_TDES,
+				FSL_SHW_PF_KEY_PRG);
+	fsl_shw_sko_set_key_length(&kek_key_info, KEK_LENGTH);
+
+	fsl_shw_scco_init(&kek_ctx, FSL_KEY_ALG_TDES, FSL_SYM_MODE_CBC);
+	fsl_shw_scco_set_flags(&kek_ctx, FSL_SYM_CTX_LOAD);
+	fsl_shw_scco_set_context(&kek_ctx, (uint8_t *) & key_info->userid);
+	ret = fsl_shw_symmetric_encrypt(user_ctx, &kek_key_info, &kek_ctx,
+					rounded_key_length,
+					red_key, black_key + KEY_PRIME_OFFSET);
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Encryption of KEY failed\n");
+#endif				/*DIAG_SECURITY_FUNC */
+		goto out;
+	}
+
+	/* Set up flags info */
+	black_key[FLAGS_OFFSET] = 0;
+	if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+		black_key[FLAGS_OFFSET] |= FLAGS_SW_KEY;
+	}
+#ifdef DIAG_SECURITY_FUNC
+	dump("KEY'", black_key + KEY_PRIME_OFFSET, rounded_key_length);
+#endif
+	/* Compute and store ICV into Black Key */
+	ret = calc_icv(T,
+		       (uint8_t *) & key_info->userid,
+		       sizeof(key_info->userid),
+		       black_key, original_key_length, black_key + ICV_OFFSET);
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Calculation of ICV failed\n");
+#endif				/*DIAG_SECURITY_FUNC */
+		goto out;
+	}
+
+	/* Compute T' = 3des-enc-ecb(wrap_key, T); Result goes to Black Key */
+	init_wrap_key(user_ctx->wrap_key, &t_key_info, &t_key_ctx);
+	ret = fsl_shw_symmetric_encrypt(user_ctx, &t_key_info, &t_key_ctx,
+					T_LENGTH,
+					T, black_key + T_PRIME_OFFSET);
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Encryption of nonce failed");
+#endif
+		goto out;
+	}
+#ifdef DIAG_SECURITY_FUNC
+	dump("black", black_key, KEY_PRIME_OFFSET + black_key[LENGTH_OFFSET]);
+#endif
+
+      out:
+	if (pk_needs_restoration) {
+		dryice_set_programmed_key(saved_pk, 8 * 21, 0);
+	}
+
+	if (!pk_was_held) {
+		dryice_release_programmed_key();
+	}
+
+	if (red_key_malloced) {
+		memset(red_key, 0, key_info->key_length);
+		os_free_memory(red_key);
+	}
+
+	key_info->key_length = original_key_length;
+
+	/* Erase tracks of confidential data */
+	memset(T, 0, T_LENGTH);
+	memset(&t_key_info, 0, sizeof(t_key_info));
+	memset(&t_key_ctx, 0, sizeof(t_key_ctx));
+	memset(&kek_key_info, 0, sizeof(kek_key_info));
+	memset(&kek_ctx, 0, sizeof(kek_ctx));
+	memset(kek, 0, sizeof(kek));
+	memset(saved_pk, 0, sizeof(saved_pk));
+
+	return ret;
+}				/* wrap */
+
+static fsl_shw_return_t create(fsl_shw_uco_t * user_ctx,
+			       fsl_shw_sko_t * key_info)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+	unsigned key_length = key_info->key_length;
+	di_return_t di_code;
+
+	if (!(key_info->flags & FSL_SKO_KEY_SW_KEY)) {
+		/* Must be creating key for PK */
+		if ((key_info->algorithm != FSL_KEY_ALG_TDES) ||
+		    ((key_info->key_length != 16)
+		     && (key_info->key_length != 21)	/* permuted 168-bit key */
+		     &&(key_info->key_length != 24))) {
+			ret = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+
+		key_length = 21;	/* 168-bit PK */
+	}
+
+	/* operational block */
+	{
+		uint8_t key_value[key_length];
+
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Creating random key\n");
+#endif
+		ret = fsl_shw_get_random(user_ctx, key_length, key_value);
+		if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+			LOG_DIAG("get_random for CREATE KEY failed\n");
+#endif
+			goto out;
+		}
+
+		if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+			ret = load_slot(user_ctx, key_info, key_value);
+		} else {
+			di_code =
+			    dryice_set_programmed_key(key_value, 8 * key_length,
+						      0);
+			if (di_code != 0) {
+#ifdef DIAG_SECURITY_FUNC
+				LOG_DIAG_ARGS("di set_pk failed: %s\n",
+					      di_error_string(di_code));
+#endif
+				ret = FSL_RETURN_ERROR_S;
+				goto out;
+			}
+			ret = FSL_RETURN_OK_S;
+		}
+		memset(key_value, 0, key_length);
+	}			/* end operational block */
+
+#ifdef DIAG_SECURITY_FUNC
+	if (ret != FSL_RETURN_OK_S) {
+		LOG_DIAG("Loading random key failed");
+	}
+#endif
+
+      out:
+
+	return ret;
+}				/* end fn create */
+
+static fsl_shw_return_t accept(fsl_shw_uco_t * user_ctx,
+			       fsl_shw_sko_t * key_info, const uint8_t * key)
+{
+	uint8_t permuted_key[21];
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+
+	if (key == NULL) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("ACCEPT:  Red Key is NULL");
+#endif
+		ret = FSL_RETURN_ERROR_S;
+		goto out;
+	}
+#ifdef DIAG_SECURITY_FUNC
+	dump("red", key, key_info->key_length);
+#endif
+	/* Only SW keys go into the keystore */
+	if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+
+		/* Copy in safe number of bytes of Red key */
+		ret = load_slot(user_ctx, key_info, key);
+	} else {		/* not SW key */
+		di_return_t di_ret;
+
+		/* Only 3DES PGM key types can be established */
+		if (((key_info->pf_key != FSL_SHW_PF_KEY_PRG)
+		     && (key_info->pf_key != FSL_SHW_PF_KEY_IIM_PRG))
+		    || (key_info->algorithm != FSL_KEY_ALG_TDES)) {
+#ifdef DIAG_SECURITY_FUNC
+			LOG_DIAG_ARGS
+			    ("ACCEPT:  Failed trying to establish non-PRG"
+			     " or invalid 3DES Key: iim%d, iim_prg%d, alg%d\n",
+			     (key_info->pf_key != FSL_SHW_PF_KEY_PRG),
+			     (key_info->pf_key != FSL_SHW_PF_KEY_IIM_PRG),
+			     (key_info->algorithm != FSL_KEY_ALG_TDES));
+#endif
+			ret = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+		if ((key_info->key_length != 16)
+		    && (key_info->key_length != 21)
+		    && (key_info->key_length != 24)) {
+#ifdef DIAG_SECURITY_FUNC
+			LOG_DIAG_ARGS("ACCEPT:  Failed trying to establish"
+				      " invalid 3DES Key: len=%d (%d)\n",
+				      key_info->key_length,
+				      ((key_info->key_length != 16)
+				       && (key_info->key_length != 21)
+				       && (key_info->key_length != 24)));
+#endif
+			ret = FSL_RETURN_BAD_KEY_LENGTH_S;
+			goto out;
+		}
+
+		/* Convert key into 168-bit value and put it into PK */
+		if (key_info->key_length != 21) {
+			fsl_shw_permute1_bytes(key, permuted_key,
+					       key_info->key_length / 8);
+			di_ret =
+			    dryice_set_programmed_key(permuted_key, 168, 0);
+		} else {
+			/* Already permuted ! */
+			di_ret = dryice_set_programmed_key(key, 168, 0);
+		}
+		if (di_ret != DI_SUCCESS) {
+#ifdef DIAG_SECURITY_FUNC
+			LOG_DIAG_ARGS
+			    ("ACCEPT:  DryIce error setting Program Key: %s",
+			     di_error_string(di_ret));
+#endif
+			ret = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+	}
+
+	ret = FSL_RETURN_OK_S;
+
+      out:
+	memset(permuted_key, 0, 21);
+
+	return ret;
+}				/* end fn accept */
+
+/*!
+ * Place a key into a protected location for use only by cryptographic
+ * algorithms.
+ *
+ * This only needs to be used to a) unwrap a key, or b) set up a key which
+ * could be wrapped by calling #fsl_shw_extract_key() at some later time).
+ *
+ * The protected key will not be available for use until this operation
+ * successfully completes.
+ *
+ * @bug This whole discussion needs review.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param[in,out] key_info      The information about the key to be which will
+ *                              be established.  In the create case, the key
+ *                              length must be set.
+ * @param      establish_type   How @a key will be interpreted to establish a
+ *                              key for use.
+ * @param key                   If @a establish_type is #FSL_KEY_WRAP_UNWRAP,
+ *                              this is the location of a wrapped key.  If
+ *                              @a establish_type is #FSL_KEY_WRAP_CREATE, this
+ *                              parameter can be @a NULL.  If @a establish_type
+ *                              is #FSL_KEY_WRAP_ACCEPT, this is the location
+ *                              of a plaintext key.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_establish_key(fsl_shw_uco_t * user_ctx,
+				       fsl_shw_sko_t * key_info,
+				       fsl_shw_key_wrap_t establish_type,
+				       const uint8_t * key)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+	unsigned original_key_length = key_info->key_length;
+	unsigned rounded_key_length;
+	unsigned slot_allocated = 0;
+
+	/* For now, only blocking mode calls are supported */
+	if (!(user_ctx->flags & FSL_UCO_BLOCKING_MODE)) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("%s: Non-blocking call not supported\n",
+			      __FUNCTION__);
+#endif
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	/*
+	   HW keys are always 'established', but otherwise do not allow user
+	   * to establish over the top of an established key.
+	 */
+	if ((key_info->flags & FSL_SKO_KEY_ESTABLISHED)
+	    && !(key_info->flags & FSL_SKO_KEY_SELECT_PF_KEY)) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("%s: Key already established\n", __FUNCTION__);
+#endif
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	/* @bug VALIDATE KEY flags here -- SW or PRG/IIM_PRG */
+
+	/* Write operations into SCC memory require word-multiple number of
+	 * bytes.  For ACCEPT and CREATE functions, the key length may need
+	 * to be rounded up.  Calculate. */
+	if (LENGTH_PATCH && (original_key_length & LENGTH_PATCH_MASK) != 0) {
+		rounded_key_length = original_key_length + LENGTH_PATCH
+		    - (original_key_length & LENGTH_PATCH_MASK);
+	} else {
+		rounded_key_length = original_key_length;
+	}
+
+	/* SW keys need a place to live */
+	if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+		ret = alloc_slot(user_ctx, key_info);
+		if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+			LOG_DIAG("Slot allocation failed\n");
+#endif
+			goto out;
+		}
+		slot_allocated = 1;
+	}
+
+	switch (establish_type) {
+	case FSL_KEY_WRAP_CREATE:
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Creating random key\n");
+#endif
+		ret = create(user_ctx, key_info);
+		break;
+
+	case FSL_KEY_WRAP_ACCEPT:
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Accepting plaintext key\n");
+#endif
+		ret = accept(user_ctx, key_info, key);
+		break;
+
+	case FSL_KEY_WRAP_UNWRAP:
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Unwrapping wrapped key\n");
+#endif
+		ret = unwrap(user_ctx, key_info, key);
+		break;
+
+	default:
+		ret = FSL_RETURN_BAD_FLAG_S;
+		break;
+	}			/* switch */
+
+      out:
+	if (ret != FSL_RETURN_OK_S) {
+		if (slot_allocated) {
+			(void)dealloc_slot(user_ctx, key_info);
+		}
+		key_info->flags &= ~FSL_SKO_KEY_ESTABLISHED;
+	} else {
+		key_info->flags |= FSL_SKO_KEY_ESTABLISHED;
+	}
+
+	return ret;
+}				/* end fn fsl_shw_establish_key */
+
+/*!
+ * Wrap a key and retrieve the wrapped value.
+ *
+ * A wrapped key is a key that has been cryptographically obscured.  It is
+ * only able to be used with #fsl_shw_establish_key().
+ *
+ * This function will also release a software  key (see #fsl_shw_release_key())
+ * so it must be re-established before reuse. This is not true of PGM keys.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      key_info         The information about the key to be deleted.
+ * @param[out] covered_key      The location to store the 48-octet wrapped key.
+ *                              (This size is based upon the maximum key size
+ *                              of 32 octets).
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_extract_key(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info,
+				     uint8_t * covered_key)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+
+	/* For now, only blocking mode calls are supported */
+	if (!(user_ctx->flags & FSL_UCO_BLOCKING_MODE)) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+#ifdef DIAG_SECURITY_FUNC
+	LOG_DIAG("Wrapping a key\n");
+#endif
+
+	if (!(key_info->flags & FSL_SKO_KEY_ESTABLISHED)) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("%s: Key not established\n", __FUNCTION__);
+#endif
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+	/* Verify that a SW key info really belongs to a SW key */
+	if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+	/*	  ret = FSL_RETURN_BAD_FLAG_S;
+		  goto out;*/
+	}
+
+	ret = wrap(user_ctx, key_info, covered_key);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+		/* Need to deallocate on successful extraction */
+		(void)dealloc_slot(user_ctx, key_info);
+		/* Mark key not available in the flags */
+		key_info->flags &=
+		    ~(FSL_SKO_KEY_ESTABLISHED | FSL_SKO_KEY_PRESENT);
+		memset(key_info->key, 0, sizeof(key_info->key));
+	}
+
+      out:
+	return ret;
+}				/* end fn fsl_shw_extract_key */
+
+/*!
+ * De-establish a key so that it can no longer be accessed.
+ *
+ * The key will need to be re-established before it can again be used.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      key_info         The information about the key to be deleted.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_release_key(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+
+	/* For now, only blocking mode calls are supported */
+	if (!(user_ctx->flags & FSL_UCO_BLOCKING_MODE)) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Not in blocking mode\n");
+#endif
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+#ifdef DIAG_SECURITY_FUNC
+	LOG_DIAG("Releasing a key\n");
+#endif
+
+	if (!(key_info->flags & FSL_SKO_KEY_ESTABLISHED)) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Key not established\n");
+#endif
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+		(void)dealloc_slot(user_ctx, key_info);
+		/* Turn off 'established' flag */
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("dealloc_slot() called\n");
+#endif
+		key_info->flags &= ~FSL_SKO_KEY_ESTABLISHED;
+		ret = FSL_RETURN_OK_S;
+		goto out;
+	}
+
+	if ((key_info->pf_key == FSL_SHW_PF_KEY_PRG)
+	    || (key_info->pf_key == FSL_SHW_PF_KEY_IIM_PRG)) {
+		di_return_t di_ret;
+
+		di_ret = dryice_release_programmed_key();
+		if (di_ret != DI_SUCCESS) {
+#ifdef DIAG_SECURITY_FUNC
+			LOG_DIAG_ARGS
+			    ("dryice_release_programmed_key() failed: %d\n",
+			     di_ret);
+#endif
+			ret = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+	} else {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Neither SW nor HW key\n");
+#endif
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	ret = FSL_RETURN_OK_S;
+
+      out:
+	return ret;
+}				/* end fn fsl_shw_release_key */
+
+fsl_shw_return_t fsl_shw_read_key(fsl_shw_uco_t * user_ctx,
+				  fsl_shw_sko_t * key_info, uint8_t * key)
+{
+	fsl_shw_return_t ret = FSL_RETURN_INTERNAL_ERROR_S;
+
+	/* Only blocking mode calls are supported */
+	if (!(user_ctx->flags & FSL_UCO_BLOCKING_MODE)) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+	printk("Reading a key\n");
+#ifdef DIAG_SECURITY_FUNC
+	LOG_DIAG("Reading a key");
+#endif
+	if (key_info->flags & FSL_SKO_KEY_PRESENT) {
+		memcpy(key_info->key, key, key_info->key_length);
+		ret = FSL_RETURN_OK_S;
+	} else if (key_info->flags & FSL_SKO_KEY_ESTABLISHED) {
+	printk("key established\n");
+		if (key_info->keystore == NULL) {
+			printk("keystore is null\n");
+			/* First verify that the key access is valid */
+			ret =
+			    system_keystore.slot_verify_access(system_keystore.
+							       user_data,
+							       key_info->userid,
+							       key_info->
+							       handle);
+
+			printk("key in system keystore\n");
+
+			/* Key is in system keystore */
+			ret = keystore_slot_read(&system_keystore,
+						 key_info->userid,
+						 key_info->handle,
+						 key_info->key_length, key);
+		} else {
+		printk("key goes in user keystore.\n");
+			/* Key goes in user keystore */
+			ret = keystore_slot_read(key_info->keystore,
+						 key_info->userid,
+						 key_info->handle,
+						 key_info->key_length, key);
+		}
+	}
+
+      out:
+	return ret;
+}				/* end fn fsl_shw_read_key */
+
+#else				/* __KERNEL__ && DRYICE */
+
+/* User mode -- these functions are unsupported */
+
+fsl_shw_return_t fsl_shw_establish_key(fsl_shw_uco_t * user_ctx,
+				       fsl_shw_sko_t * key_info,
+				       fsl_shw_key_wrap_t establish_type,
+				       const uint8_t * key)
+{
+	(void)user_ctx;
+	(void)key_info;
+	(void)establish_type;
+	(void)key;
+
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t fsl_shw_extract_key(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info,
+				     uint8_t * covered_key)
+{
+	(void)user_ctx;
+	(void)key_info;
+	(void)covered_key;
+
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t fsl_shw_release_key(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info)
+{
+	(void)user_ctx;
+	(void)key_info;
+
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t fsl_shw_read_key(fsl_shw_uco_t * user_ctx,
+				  fsl_shw_sko_t * key_info, uint8_t * key)
+{
+	(void)user_ctx;
+	(void)key_info;
+	(void)key;
+
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+#endif				/* __KERNEL__ && DRYICE */
diff --git a/drivers/mxc/security/rng/include/rng_driver.h b/drivers/mxc/security/rng/include/rng_driver.h
new file mode 100644
index 000000000000..7d6d24dde915
--- /dev/null
+++ b/drivers/mxc/security/rng/include/rng_driver.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU Lesser General
+ * Public License.  You may obtain a copy of the GNU Lesser General
+ * Public License Version 2.1 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/lgpl-license.html
+ * http://www.gnu.org/copyleft/lgpl.html
+ */
+
+#ifndef RNG_DRIVER_H
+#define RNG_DRIVER_H
+
+#include "shw_driver.h"
+
+/* This is a Linux flag meaning 'compiling kernel code'...  */
+#ifndef __KERNEL__
+#include <inttypes.h>
+#include <stdlib.h>
+#include <memory.h>
+#else
+#include "../../sahara2/include/portable_os.h"
+#endif
+
+#include "../../sahara2/include/fsl_platform.h"
+
+/*! @file rng_driver.h
+ *
+ * @brief Header file to use the RNG driver.
+ *
+ *  @ingroup RNG
+ */
+
+#if defined(FSL_HAVE_RNGA)
+
+#include "rng_rnga.h"
+
+#elif defined(FSL_HAVE_RNGB) || defined(FSL_HAVE_RNGC)
+
+#include "rng_rngc.h"
+
+#else				/* neither RNGA,  RNGB,  nor  RNGC */
+
+#error NO_RNG_TYPE_IDENTIFIED
+
+#endif
+
+/*****************************************************************************
+ * Enumerations
+ *****************************************************************************/
+
+/*! Values from Version ID register  */
+enum rng_type {
+	/*! Type RNGA. */
+	RNG_TYPE_RNGA = 0,
+	/*! Type RNGB. */
+	RNG_TYPE_RNGB = 1,
+	/*! Type RNGC */
+	RNG_TYPE_RNGC = 2
+};
+
+/*!
+ * Return values (error codes) for kernel register interface functions
+ */
+typedef enum rng_return {
+	RNG_RET_OK = 0,		/*!< Function succeeded  */
+	RNG_RET_FAIL		/*!< Non-specific failure */
+} rng_return_t;
+
+/*****************************************************************************
+ * Data Structures
+ *****************************************************************************/
+/*!
+ * An entry in the RNG Work Queue.  Based upon standard SHW queue entry.
+ *
+ * This entry also gets saved (for non-blocking requests) in the user's result
+ * pool.  When the user picks up the request, the final processing (copy from
+ * data_local to data_user) will get made if status was good.
+ */
+typedef struct rng_work_entry {
+	struct shw_queue_entry_t hdr;	/*!< Standards SHW queue info.  */
+	uint32_t length;	/*!< Number of bytes still needed to satisfy request. */
+	uint32_t *data_local;	/*!< Where data from RNG FIFO gets placed. */
+	uint8_t *data_user;	/*!< Ultimate target of data.  */
+	unsigned completed;	/*!< Non-zero if job is done.  */
+} rng_work_entry_t;
+
+/*****************************************************************************
+ * Function Prototypes
+ *****************************************************************************/
+
+#ifdef RNG_REGISTER_PEEK_POKE
+/*!
+ * Read value from an RNG register.
+ * The offset will be checked for validity as well as whether it is
+ * accessible at the time of the call.
+ *
+ * This routine cannot be used to read the RNG's Output FIFO if the RNG is in
+ * High Assurance mode.
+ *
+ * @param[in]   register_offset  The (byte) offset within the RNG block
+ *                               of the register to be queried.  See
+ *                               RNG(A, C) registers for meanings.
+ * @param[out]  value            Pointer to where value from the register
+ *                               should be placed.
+ *
+ * @return                   See #rng_return_t.
+ */
+/* REQ-FSLSHW-PINTFC-API-LLF-001 */
+extern rng_return_t rng_read_register(uint32_t register_offset,
+				      uint32_t * value);
+
+/*!
+ * Write a new value into an RNG register.
+ *
+ * The offset will be checked for validity as well as whether it is
+ * accessible at the time of the call.
+ *
+ * @param[in]  register_offset  The (byte) offset within the RNG block
+ *                              of the register to be modified.  See
+ *                              RNG(A, C) registers for meanings.
+ * @param[in]  value            The value to store into the register.
+ *
+ * @return                   See #rng_return_t.
+ */
+/* REQ-FSLSHW-PINTFC-API-LLF-002 */
+extern rng_return_t rng_write_register(uint32_t register_offset,
+				       uint32_t value);
+#endif				/* RNG_REGISTER_PEEK_POKE */
+
+#endif				/* RNG_DRIVER_H */
diff --git a/drivers/mxc/security/rng/include/rng_internals.h b/drivers/mxc/security/rng/include/rng_internals.h
new file mode 100644
index 000000000000..62d195bf4df7
--- /dev/null
+++ b/drivers/mxc/security/rng/include/rng_internals.h
@@ -0,0 +1,680 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef RNG_INTERNALS_H
+#define RNG_INTERNALS_H
+
+/*! @file rng_internals.h
+ *
+ *  This file contains definitions which are internal to the RNG driver.
+ *
+ *  This header file should only ever be needed by rng_driver.c
+ *
+ *  Compile-time flags minimally needed:
+ *
+ *  @li Some sort of platform flag.  (FSL_HAVE_RNGA or FSL_HAVE_RNGC)
+ *
+ *  @ingroup RNG
+ */
+
+#include "portable_os.h"
+#include "shw_driver.h"
+#include "rng_driver.h"
+
+/*! @defgroup rngcompileflags RNG Compile Flags
+ *
+ * These are flags which are used to configure the RNG driver at compilation
+ * time.
+ *
+ * Most of them default to good values for normal operation, but some
+ * (#INT_RNG and #RNG_BASE_ADDR) need to be provided.
+ *
+ * The terms 'defined' and 'undefined' refer to whether a @c \#define (or -D on
+ * a compile command) has defined a given preprocessor symbol.  If a given
+ * symbol is defined, then @c \#ifdef \<symbol\> will succeed.  Some symbols
+ * described below default to not having a definition, i.e. they are undefined.
+ *
+ */
+
+/*! @addtogroup rngcompileflags */
+/*! @{ */
+
+/*!
+ * This is the maximum number of times the driver will loop waiting for the
+ * RNG hardware to say that it has generated random data.  It prevents the
+ * driver from stalling forever should there be a hardware problem.
+ *
+ * Default value is 100.  It should be revisited as CPU clocks speed up.
+ */
+#ifndef RNG_MAX_TRIES
+#define RNG_MAX_TRIES 100
+#endif
+
+/* Temporarily define compile-time flags to make Doxygen happy and allow them
+   to get into the documentation. */
+#ifdef DOXYGEN_HACK
+
+/*!
+ * This symbol is the base address of the RNG in the CPU memory map.  It may
+ * come from some included header file, or it may come from the compile command
+ * line.  This symbol has no default, and the driver will not compile without
+ * it.
+ */
+#define RNG_BASE_ADDR
+#undef RNG_BASE_ADDR
+
+/*!
+ * This symbol is the Interrupt Number of the RNG in the CPU.  It may come
+ * from some included header file, or it may come from the compile command
+ * line.  This symbol has no default, and the driver will not compile without
+ * it.
+ */
+#define INT_RNG
+#undef INT_RNG
+
+/*!
+ * Defining this symbol will allow other kernel programs to call the
+ * #rng_read_register() and #rng_write_register() functions.  If this symbol is
+ * not defined, those functions will not be present in the driver.
+ */
+#define RNG_REGISTER_PEEK_POKE
+#undef RNG_REGISTER_PEEK_POKE
+
+/*!
+ * Turn on compilation of run-time operational, debug, and error messages.
+ *
+ * This flag is undefined by default.
+ */
+/* REQ-FSLSHW-DEBUG-001 */
+
+/*!
+ * Turn on compilation of run-time logging of access to the RNG registers,
+ * except for the RNG's Output FIFO register.  See #RNG_ENTROPY_DEBUG.
+ *
+ *  This flag is undefined by default
+ */
+#define RNG_REGISTER_DEBUG
+#undef  RNG_REGISTER_DEBUG
+
+/*!
+ * Turn on compilation of run-time logging of reading of the RNG's Output FIFO
+ * register.  This flag does nothing if #RNG_REGISTER_DEBUG is not defined.
+ *
+ *  This flag is undefined by default
+ */
+#define RNG_ENTROPY_DEBUG
+#undef  RNG_ENTROPY_DEBUG
+
+/*!
+ * If this flag is defined, the driver will not attempt to put the RNG into
+ * High Assurance mode.
+
+ * If it is undefined, the driver will attempt to put the RNG into High
+ * Assurance mode.  If RNG fails to go into High Assurance mode, the driver
+ * will fail to initialize.
+
+ * In either case, if the RNG is already in this mode, the driver will operate
+ * normally.
+ *
+ *  This flag is undefined by default.
+ */
+#define RNG_NO_FORCE_HIGH_ASSURANCE
+#undef RNG_NO_FORCE_HIGH_ASSURANCE
+
+/*!
+ * If this flag is defined, the driver will put the RNG into low power mode
+ * every opportunity.
+ *
+ *  This flag is undefined by default.
+ */
+#define RNG_USE_LOW_POWER_MODE
+#undef RNG_USE_LOW_POWER_MODE
+
+/*! @} */
+#endif				/* end DOXYGEN_HACK */
+
+/*!
+ * If this flag is defined, the driver will not attempt to put the RNG into
+ * High Assurance mode.
+
+ * If it is undefined, the driver will attempt to put the RNG into High
+ * Assurance mode.  If RNG fails to go into High Assurance mode, the driver
+ * will fail to initialize.
+
+ * In either case, if the RNG is already in this mode, the driver will operate
+ * normally.
+ *
+ */
+#define RNG_NO_FORCE_HIGH_ASSURANCE
+
+/*!
+ * Read a 32-bit value from an RNG register.  This macro depends upon
+ * #rng_base.  The os_read32() macro operates on 32-bit quantities, as do
+ * all RNG register reads.
+ *
+ * @param     offset  Register byte offset within RNG.
+ *
+ * @return    The value from the RNG's register.
+ */
+#ifndef RNG_REGISTER_DEBUG
+#define RNG_READ_REGISTER(offset) os_read32(rng_base+(offset))
+#else
+#define RNG_READ_REGISTER(offset) dbg_rng_read_register(offset)
+#endif
+
+/*!
+ * Write a 32-bit value to an RNG register.  This macro depends upon
+ * #rng_base.  The os_write32() macro operates on 32-bit quantities, as do
+ * all RNG register writes.
+ *
+ * @param   offset  Register byte offset within RNG.
+ * @param   value   32-bit value to store into the register
+ *
+ * @return   (void)
+ */
+#ifndef RNG_REGISTER_DEBUG
+#define RNG_WRITE_REGISTER(offset,value)                                    \
+    (void)os_write32(rng_base+(offset), value)
+#else
+#define RNG_WRITE_REGISTER(offset,value) dbg_rng_write_register(offset,value)
+#endif
+
+#ifndef RNG_DRIVER_NAME
+/*! @addtogroup rngcompileflags */
+/*! @{ */
+/*! Name the driver will use to register itself to the kernel as the driver. */
+#define RNG_DRIVER_NAME "rng"
+/*! @} */
+#endif
+
+/*!
+ * Calculate number of words needed to hold the given number of bytes.
+ *
+ * @param byte_count    Number of bytes
+ *
+ * @return              Number of words
+ */
+#define BYTES_TO_WORDS(byte_count)                                          \
+    (((byte_count)+sizeof(uint32_t)-1)/sizeof(uint32_t))
+
+/*! Gives high-level view of state of the RNG */
+typedef enum rng_status {
+	RNG_STATUS_INITIAL,	/*!< Driver status before ever starting. */
+	RNG_STATUS_CHECKING,	/*!< During driver initialization. */
+	RNG_STATUS_UNIMPLEMENTED,	/*!< Hardware is non-existent / unreachable. */
+	RNG_STATUS_OK,		/*!< Hardware is In Secure or Default state. */
+	RNG_STATUS_FAILED	/*!< Hardware is In Failed state / other fatal
+				   problem.  Driver is still able to read/write
+				   some registers, but cannot get Random
+				   data. */
+} rng_status_t;
+
+static shw_queue_t rng_work_queue;
+
+/*****************************************************************************
+ *
+ *  Function Declarations
+ *
+ *****************************************************************************/
+
+/* kernel interface functions */
+OS_DEV_INIT_DCL(rng_init);
+OS_DEV_TASK_DCL(rng_entropy_task);
+OS_DEV_SHUTDOWN_DCL(rng_shutdown);
+OS_DEV_ISR_DCL(rng_irq);
+
+#define RNG_ADD_QUEUE_ENTRY(pool, entry)                                    \
+      SHW_ADD_QUEUE_ENTRY(pool, (shw_queue_entry_t*)entry)
+
+#define RNG_REMOVE_QUEUE_ENTRY(pool, entry)                                 \
+      SHW_REMOVE_QUEUE_ENTRY(pool, (shw_queue_entry_t*)entry)
+#define  RNG_GET_WORK_ENTRY()                                               \
+      (rng_work_entry_t*)SHW_POP_FIRST_ENTRY(&rng_work_queue)
+
+/*!
+ * Add an work item to a work list.  Item will be marked incomplete.
+ *
+ * @param work Work entry to place at tail of list.
+ *
+ * @return none
+ */
+inline static void RNG_ADD_WORK_ENTRY(rng_work_entry_t * work)
+{
+	work->completed = FALSE;
+
+	SHW_ADD_QUEUE_ENTRY(&rng_work_queue, (shw_queue_entry_t *) work);
+
+	os_dev_schedule_task(rng_entropy_task);
+}
+
+/*!
+ * For #rng_check_register_accessible(), check read permission on given
+ * register.
+ */
+#define RNG_CHECK_READ 0
+
+/*!
+ * For #rng_check_register_accessible(), check write permission on given
+ * register.
+ */
+#define RNG_CHECK_WRITE 1
+
+/* Define different helper symbols based on RNG type */
+#ifdef FSL_HAVE_RNGA
+
+/******************************************************************************
+ *
+ *  RNGA support
+ *
+ *****************************************************************************/
+
+/*! Interrupt number for driver. */
+#if defined(MXC_INT_RNG)
+/* Most modern definition */
+#define INT_RNG MXC_INT_RNG
+#elif defined(MXC_INT_RNGA)
+#define INT_RNG MXC_INT_RNGA
+#else
+#define INT_RNG INT_RNGA
+#endif
+
+/*! Base (bus?) address of RNG component. */
+#define RNG_BASE_ADDR RNGA_BASE_ADDR
+
+/*! Read and return the status register. */
+#define RNG_GET_STATUS()                                                 \
+    RNG_READ_REGISTER(RNGA_STATUS)
+/*! Configure RNG for Auto seeding */
+#define RNG_AUTO_SEED()
+/* Put RNG for Seed Generation */
+#define RNG_SEED_GEN()
+/*!
+ * Return RNG Type value.  Should be RNG_TYPE_RNGA, RNG_TYPE_RNGB,
+ * or RNG_TYPE_RNGC.
+ */
+#define RNG_GET_RNG_TYPE()                                                \
+    ((RNG_READ_REGISTER(RNGA_CONTROL) & RNGA_CONTROL_RNG_TYPE_MASK)       \
+     >> RNGA_CONTROL_RNG_TYPE_SHIFT)
+
+/*!
+ * Verify Type value of RNG.
+ *
+ * Returns true of OK, false if not.
+ */
+#define RNG_VERIFY_TYPE(type)                                             \
+    ((type) == RNG_TYPE_RNGA)
+
+/*! Returns non-zero if RNG device is reporting an error. */
+#define RNG_HAS_ERROR()                                                   \
+    (RNG_READ_REGISTER(RNGA_STATUS) & RNGA_STATUS_ERROR_INTERRUPT)
+/*! Returns non-zero if Bad Key is selected */
+#define RNG_HAS_BAD_KEY()     0
+/*! Return non-zero if Self Test Done */
+#define RNG_SELF_TEST_DONE()  0
+/*! Returns non-zero if RNG ring oscillators have failed. */
+#define RNG_OSCILLATOR_FAILED()                                           \
+    (RNG_READ_REGISTER(RNGA_STATUS) & RNGA_STATUS_OSCILLATOR_DEAD)
+
+/*! Returns maximum number of 32-bit words in the RNG's output fifo. */
+#define RNG_GET_FIFO_SIZE()                                               \
+    ((RNG_READ_REGISTER(RNGA_STATUS) & RNGA_STATUS_OUTPUT_FIFO_SIZE_MASK) \
+     >> RNGA_STATUS_OUTPUT_FIFO_SIZE_SHIFT)
+
+/*! Returns number of 32-bit words currently in the RNG's output fifo. */
+#define RNG_GET_WORDS_IN_FIFO()                                            \
+    ((RNG_READ_REGISTER(RNGA_STATUS) & RNGA_STATUS_OUTPUT_FIFO_LEVEL_MASK) \
+     >> RNGA_STATUS_OUTPUT_FIFO_LEVEL_SHIFT)
+/* Configuring RNG for Self Test */
+#define RNG_SELF_TEST()
+/*! Get a random value from the RNG's output FIFO. */
+#define RNG_READ_FIFO()                                                  \
+    RNG_READ_REGISTER(RNGA_OUTPUT_FIFO)
+
+/*! Put entropy into the RNG's algorithm.
+ *  @param value  32-bit value to add to RNG's entropy.
+ **/
+#define RNG_ADD_ENTROPY(value)                                           \
+    RNG_WRITE_REGISTER(RNGA_ENTROPY, (value))
+/*! Return non-zero in case of Error during Self Test */
+#define RNG_CHECK_SELF_ERR() 0
+/*! Return non-zero in case of Error during Seed Generation */
+#define RNG_CHECK_SEED_ERR() 0
+/*! Get the RNG started at generating output. */
+#define RNG_GO()                                                         \
+{                                                                        \
+    register uint32_t control = RNG_READ_REGISTER(RNGA_CONTROL);         \
+    RNG_WRITE_REGISTER(RNGA_CONTROL, control | RNGA_CONTROL_GO);         \
+}
+/*! To clear all Error Bits in Error Status Register */
+#define RNG_CLEAR_ERR()
+/*! Put RNG into High Assurance mode */
+#define RNG_SET_HIGH_ASSURANCE()                                              \
+{                                                                             \
+    register uint32_t control = RNG_READ_REGISTER(RNGA_CONTROL);              \
+    RNG_WRITE_REGISTER(RNGA_CONTROL, control | RNGA_CONTROL_HIGH_ASSURANCE);  \
+}
+
+/*! Return non-zero if the RNG is in High Assurance mode. */
+#define RNG_GET_HIGH_ASSURANCE()                                              \
+    (RNG_READ_REGISTER(RNGA_CONTROL) & RNGA_CONTROL_HIGH_ASSURANCE)
+
+/*! Clear all status, error and otherwise. */
+#define RNG_CLEAR_ALL_STATUS()                                                \
+{                                                                             \
+    register uint32_t control = RNG_READ_REGISTER(RNGA_CONTROL);              \
+    RNG_WRITE_REGISTER(RNGA_CONTROL, control | RNGA_CONTROL_CLEAR_INTERRUPT); \
+}
+/* Return non-zero if RESEED Required */
+#define RNG_RESEED() 1
+
+/*! Return non-zero if Seeding is done */
+#define RNG_SEED_DONE()  1
+
+/*! Return non-zero if everything seems OK with the RNG. */
+#define RNG_WORKING()                                                    \
+    ((RNG_READ_REGISTER(RNGA_STATUS)                                     \
+      & (RNGA_STATUS_SLEEP | RNGA_STATUS_SECURITY_VIOLATION              \
+         | RNGA_STATUS_ERROR_INTERRUPT | RNGA_STATUS_FIFO_UNDERFLOW      \
+         | RNGA_STATUS_LAST_READ_STATUS )) == 0)
+
+/*! Put the RNG into sleep (low-power) mode. */
+#define RNG_SLEEP()                                                      \
+{                                                                        \
+    register uint32_t control = RNG_READ_REGISTER(RNGA_CONTROL);         \
+    RNG_WRITE_REGISTER(RNGA_CONTROL, control | RNGA_CONTROL_SLEEP);      \
+}
+
+/*! Wake the RNG from sleep (low-power) mode. */
+#define RNG_WAKE()                                                       \
+{                                                                        \
+    uint32_t control = RNG_READ_REGISTER(RNGA_CONTROL);                  \
+     RNG_WRITE_REGISTER(RNGA_CONTROL, control & ~RNGA_CONTROL_SLEEP);    \
+}
+
+/*! Mask interrupts so that the driver/OS will not see them. */
+#define RNG_MASK_ALL_INTERRUPTS()                                             \
+{                                                                             \
+    register uint32_t control = RNG_READ_REGISTER(RNGA_CONTROL);              \
+    RNG_WRITE_REGISTER(RNGA_CONTROL, control | RNGA_CONTROL_MASK_INTERRUPTS); \
+}
+
+/*! Unmask interrupts so that the driver/OS will see them. */
+#define RNG_UNMASK_ALL_INTERRUPTS()                                           \
+{                                                                             \
+    register uint32_t control = RNG_READ_REGISTER(RNGA_CONTROL);              \
+    RNG_WRITE_REGISTER(RNGA_CONTROL, control & ~RNGA_CONTROL_MASK_INTERRUPTS);\
+}
+
+/*!
+ * @def RNG_PUT_RNG_TO_SLEEP()
+ *
+ * If compiled with #RNG_USE_LOW_POWER_MODE, this routine will put the RNG
+ * to sleep (low power mode).
+ *
+ * @return none
+ */
+/*!
+ * @def RNG_WAKE_RNG_FROM_SLEEP()
+ *
+ * If compiled with #RNG_USE_LOW_POWER_MODE, this routine will wake the RNG
+ * from sleep (low power mode).
+ *
+ * @return none
+ */
+#ifdef RNG_USE_LOW_POWER_MODE
+
+#define RNG_PUT_RNG_TO_SLEEP()                                                \
+    RNG_SLEEP()
+
+#define RNG_WAKE_FROM_SLEEP()                                                 \
+    RNG_WAKE()  1
+
+#else				/* not low power mode */
+
+#define RNG_PUT_RNG_TO_SLEEP()
+
+#define RNG_WAKE_FROM_SLEEP()
+
+#endif				/* Use low-power mode */
+
+#else				/* FSL_HAVE_RNGB or FSL_HAVE_RNGC */
+
+/******************************************************************************
+ *
+ *  RNGB and RNGC support
+ *
+ *****************************************************************************/
+/*
+ * The operational interfaces for RNGB and RNGC are almost identical, so
+ * the defines for RNGC work fine for both.  There are minor differences
+ * which will be treated within this conditional block.
+ */
+
+/*! Interrupt number for driver. */
+#if defined(MXC_INT_RNG)
+/* Most modern definition */
+#define INT_RNG MXC_INT_RNG
+#elif defined(MXC_INT_RNGC)
+#define INT_RNG MXC_INT_RNGC
+#elif defined(MXC_INT_RNGB)
+#define INT_RNG MXC_INT_RNGB
+#elif defined(INT_RNGC)
+#define INT_RNG INT_RNGC
+#else
+#error NO_INTERRUPT_DEFINED
+#endif
+
+/*! Base address of RNG component. */
+#ifdef FSL_HAVE_RNGB
+#define RNG_BASE_ADDR RNGB_BASE_ADDR
+#else
+#define RNG_BASE_ADDR RNGC_BASE_ADDR
+#endif
+
+/*! Read and return the status register. */
+#define RNG_GET_STATUS()                                                 \
+    RNG_READ_REGISTER(RNGC_ERROR)
+
+/*!
+ * Return RNG Type value.  Should be RNG_TYPE_RNGA or RNG_TYPE_RNGC.
+ */
+#define RNG_GET_RNG_TYPE()                                                \
+    ((RNG_READ_REGISTER(RNGC_VERSION_ID) & RNGC_VERID_RNG_TYPE_MASK)      \
+     >> RNGC_VERID_RNG_TYPE_SHIFT)
+
+/*!
+ * Verify Type value of RNG.
+ *
+ * Returns true of OK, false if not.
+ */
+#ifdef FSL_HAVE_RNGB
+#define RNG_VERIFY_TYPE(type)                                             \
+    ((type) == RNG_TYPE_RNGB)
+#else				/* RNGC */
+#define RNG_VERIFY_TYPE(type)                                             \
+    ((type) == RNG_TYPE_RNGC)
+#endif
+
+/*! Returns non-zero if RNG device is reporting an error. */
+#define RNG_HAS_ERROR()                                                   \
+    (RNG_READ_REGISTER(RNGC_STATUS) & RNGC_STATUS_ERROR)
+/*! Returns non-zero if Bad Key is selected */
+#define RNG_HAS_BAD_KEY()                                                 \
+     (RNG_READ_REGISTER(RNGC_ERROR) & RNGC_ERROR_STATUS_BAD_KEY)
+/*! Returns non-zero if RNG ring oscillators have failed. */
+#define RNG_OSCILLATOR_FAILED()                                           \
+    (RNG_READ_REGISTER(RNGC_ERROR) & RNGC_ERROR_STATUS_OSC_ERR)
+
+/*! Returns maximum number of 32-bit words in the RNG's output fifo. */
+#define RNG_GET_FIFO_SIZE()                                               \
+    ((RNG_READ_REGISTER(RNGC_STATUS) & RNGC_STATUS_FIFO_SIZE_MASK)        \
+     >> RNGC_STATUS_FIFO_SIZE_SHIFT)
+
+/*! Returns number of 32-bit words currently in the RNG's output fifo. */
+#define RNG_GET_WORDS_IN_FIFO()                                           \
+    ((RNG_READ_REGISTER(RNGC_STATUS) & RNGC_STATUS_FIFO_LEVEL_MASK)       \
+     >> RNGC_STATUS_FIFO_LEVEL_SHIFT)
+
+/*! Get a random value from the RNG's output FIFO. */
+#define RNG_READ_FIFO()                                                   \
+    RNG_READ_REGISTER(RNGC_FIFO)
+
+/*! Put entropy into the RNG's algorithm.
+ *  @param value  32-bit value to add to RNG's entropy.
+ **/
+#ifdef FSL_HAVE_RNGB
+#define RNG_ADD_ENTROPY(value)                                             \
+    RNG_WRITE_REGISTER(RNGB_ENTROPY, value)
+#else				/* RNGC does not have Entropy register */
+#define RNG_ADD_ENTROPY(value)
+#endif
+/*! Wake the RNG from sleep (low-power) mode. */
+#define RNG_WAKE()  1
+/*! Get the RNG started at generating output. */
+#define RNG_GO()
+/*! Put RNG into High Assurance mode. */
+#define RNG_SET_HIGH_ASSURANCE()
+/*! Returns non-zero in case of Error during Self Test       */
+#define RNG_CHECK_SELF_ERR()                                                 \
+        (RNG_READ_REGISTER(RNGC_ERROR) & RNGC_ERROR_STATUS_ST_ERR)
+/*! Return non-zero in case of Error during Seed Generation */
+#define RNG_CHECK_SEED_ERR()                                                 \
+        (RNG_READ_REGISTER(RNGC_ERROR) & RNGC_ERROR_STATUS_STAT_ERR)
+
+/*! Configure RNG for Self Test */
+#define RNG_SELF_TEST()                                                       \
+{                                                                             \
+    register uint32_t command = RNG_READ_REGISTER(RNGC_COMMAND);              \
+    RNG_WRITE_REGISTER(RNGC_COMMAND, command                                  \
+                                    | RNGC_COMMAND_SELF_TEST);                \
+}
+/*! Clearing the Error bits in Error Status Register */
+#define RNG_CLEAR_ERR()                                                       \
+{                                                                             \
+    register uint32_t command = RNG_READ_REGISTER(RNGC_COMMAND);              \
+    RNG_WRITE_REGISTER(RNGC_COMMAND, command                                  \
+                                    | RNGC_COMMAND_CLEAR_ERROR);              \
+}
+
+/*! Return non-zero if Self Test Done */
+#define RNG_SELF_TEST_DONE()                                                  \
+         (RNG_READ_REGISTER(RNGC_STATUS) & RNGC_STATUS_ST_DONE)
+/* Put RNG for SEED Generation */
+#define RNG_SEED_GEN()                                                        \
+{                                                                             \
+    register uint32_t command = RNG_READ_REGISTER(RNGC_COMMAND);              \
+    RNG_WRITE_REGISTER(RNGC_COMMAND, command                                  \
+                                    | RNGC_COMMAND_SEED);                     \
+}
+/* Return non-zero if RESEED Required */
+#define RNG_RESEED()                                                          \
+    (RNG_READ_REGISTER(RNGC_STATUS) & RNGC_STATUS_RESEED)
+
+/*! Return non-zero if the RNG is in High Assurance mode. */
+#define RNG_GET_HIGH_ASSURANCE() (RNG_READ_REGISTER(RNGC_STATUS) &            \
+                                  RNGC_STATUS_SEC_STATE)
+
+/*! Clear all status, error and otherwise. */
+#define RNG_CLEAR_ALL_STATUS()                                                \
+    RNG_WRITE_REGISTER(RNGC_COMMAND,                                          \
+                       RNGC_COMMAND_CLEAR_INTERRUPT                           \
+                       | RNGC_COMMAND_CLEAR_ERROR)
+
+/*! Return non-zero if everything seems OK with the RNG. */
+#define RNG_WORKING()                                                         \
+    ((RNG_READ_REGISTER(RNGC_ERROR)                                           \
+      & (RNGC_ERROR_STATUS_STAT_ERR | RNGC_ERROR_STATUS_RAND_ERR              \
+       | RNGC_ERROR_STATUS_FIFO_ERR | RNGC_ERROR_STATUS_ST_ERR |              \
+         RNGC_ERROR_STATUS_OSC_ERR  | RNGC_ERROR_STATUS_LFSR_ERR )) == 0)
+/*! Return Non zero if SEEDING is DONE */
+#define RNG_SEED_DONE()                                                       \
+     ((RNG_READ_REGISTER(RNGC_STATUS) & RNGC_STATUS_SEED_DONE) != 0)
+
+/*! Put the RNG into sleep (low-power) mode. */
+#define RNG_SLEEP()
+
+/*! Wake the RNG from sleep (low-power) mode. */
+
+/*! Mask interrupts so that the driver/OS will not see them. */
+#define RNG_MASK_ALL_INTERRUPTS()                                             \
+{                                                                             \
+    register uint32_t control = RNG_READ_REGISTER(RNGC_CONTROL);              \
+    RNG_WRITE_REGISTER(RNGC_CONTROL, control                                  \
+                                    | RNGC_CONTROL_MASK_DONE                  \
+                                    | RNGC_CONTROL_MASK_ERROR);               \
+}
+/*! Configuring RNGC for self Test. */
+
+#define RNG_AUTO_SEED()                                                       \
+{                                                                             \
+    register uint32_t control = RNG_READ_REGISTER(RNGC_CONTROL);              \
+    RNG_WRITE_REGISTER(RNGC_CONTROL, control                                  \
+                                    | RNGC_CONTROL_AUTO_SEED);                \
+}
+
+/*! Unmask interrupts so that the driver/OS will see them. */
+#define RNG_UNMASK_ALL_INTERRUPTS()                                           \
+{                                                                             \
+    register uint32_t control = RNG_READ_REGISTER(RNGC_CONTROL);              \
+    RNG_WRITE_REGISTER(RNGC_CONTROL,                                          \
+                control & ~(RNGC_CONTROL_MASK_DONE|RNGC_CONTROL_MASK_ERROR)); \
+}
+
+/*! Put RNG to sleep if appropriate. */
+#define RNG_PUT_RNG_TO_SLEEP()
+
+/*! Wake RNG from sleep if necessary. */
+#define RNG_WAKE_FROM_SLEEP()
+
+#endif				/* RNG TYPE */
+
+/* internal functions */
+static os_error_code rng_map_RNG_memory(void);
+static os_error_code rng_setup_interrupt_handling(void);
+#ifdef RNG_REGISTER_PEEK_POKE
+inline static int rng_check_register_offset(uint32_t offset);
+inline static int rng_check_register_accessible(uint32_t offset,
+						int access_write);
+#endif				/*  DEBUG_RNG_REGISTERS */
+static fsl_shw_return_t rng_drain_fifo(uint32_t * random_p, int count_words);
+static os_error_code rng_grab_config_values(void);
+static void rng_cleanup(void);
+
+#ifdef FSL_HAVE_RNGA
+static void rng_sec_failure(void);
+#endif
+
+#ifdef RNG_REGISTER_DEBUG
+static uint32_t dbg_rng_read_register(uint32_t offset);
+static void dbg_rng_write_register(uint32_t offset, uint32_t value);
+#endif
+
+#if defined(LINUX_VERSION_CODE)
+
+EXPORT_SYMBOL(fsl_shw_add_entropy);
+EXPORT_SYMBOL(fsl_shw_get_random);
+
+#ifdef RNG_REGISTER_PEEK_POKE
+/* For Linux kernel, export the API functions to other kernel modules */
+EXPORT_SYMBOL(rng_read_register);
+EXPORT_SYMBOL(rng_write_register);
+#endif				/* DEBUG_RNG_REGISTERS */
+
+
+
+MODULE_AUTHOR("Freescale Semiconductor");
+MODULE_DESCRIPTION("Device Driver for RNG");
+
+#endif				/* LINUX_VERSION_CODE */
+
+#endif				/* RNG_INTERNALS_H */
diff --git a/drivers/mxc/security/rng/include/rng_rnga.h b/drivers/mxc/security/rng/include/rng_rnga.h
new file mode 100644
index 000000000000..971064d51522
--- /dev/null
+++ b/drivers/mxc/security/rng/include/rng_rnga.h
@@ -0,0 +1,181 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef RNG_RNGA_H
+#define RNG_RNGA_H
+
+/*! @defgroup rngaregs RNGA Registers
+ *  @ingroup RNG
+ * These are the definitions for the RNG registers and their offsets
+ * within the RNG.  They are used in the @c register_offset parameter of
+ * #rng_read_register() and #rng_write_register().
+ */
+/*! @addtogroup rngaregs */
+/*! @{ */
+
+/*! Control Register.  See @ref rngacontrolreg.  */
+#define RNGA_CONTROL                    0x00
+/*! Status Register. See @ref rngastatusreg. */
+#define RNGA_STATUS                     0x04
+/*! Register for adding to the Entropy of the RNG */
+#define RNGA_ENTROPY                    0x08
+/*! Register containing latest 32 bits of random value */
+#define RNGA_OUTPUT_FIFO                0x0c
+/*! Mode Register.  Non-secure mode access only.  See @ref rngmodereg. */
+#define RNGA_MODE                       0x10
+/*! Verification Control Register.  Non-secure mode access only.  See
+ *  @ref rngvfctlreg. */
+#define RNGA_VERIFICATION_CONTROL       0x14
+/*! Oscillator Control Counter Register.  Non-secure mode access only.
+ *  See @ref rngosccntctlreg. */
+#define RNGA_OSCILLATOR_CONTROL_COUNTER 0x18
+/*! Oscillator 1 Counter Register.  Non-secure mode access only.  See
+ *  @ref rngosccntreg. */
+#define RNGA_OSCILLATOR1_COUNTER        0x1c
+/*! Oscillator 2 Counter Register.  Non-secure mode access only.  See
+ *  @ref rngosccntreg. */
+#define RNGA_OSCILLATOR2_COUNTER        0x20
+/*! Oscillator Counter Status Register.  Non-secure mode access only.  See
+ *  @ref rngosccntstatreg. */
+#define RNGA_OSCILLATOR_COUNTER_STATUS  0x24
+/*! @} */
+
+/*! Total address space of the RNGA, in bytes */
+#define RNG_ADDRESS_RANGE 0x28
+
+/*! @defgroup rngacontrolreg RNGA Control Register Definitions
+ *  @ingroup RNG
+ */
+/*! @addtogroup rngacontrolreg */
+/*! @{ */
+/*! These bits are unimplemented or reserved */
+#define RNGA_CONTROL_ZEROS_MASK      0x0fffffe0
+/*! 'RNG type' - should be 0 for RNGA */
+#define RNGA_CONTROL_RNG_TYPE_MASK   0xf0000000
+/*! Number of bits to shift the type to get it to LSB */
+#define RNGA_CONTROL_RNG_TYPE_SHIFT  28
+/*! Put RNG to sleep */
+#define RNGA_CONTROL_SLEEP           0x00000010
+/*! Clear interrupt & status */
+#define RNGA_CONTROL_CLEAR_INTERRUPT 0x00000008
+/*! Mask interrupt generation */
+#define RNGA_CONTROL_MASK_INTERRUPTS 0x00000004
+/*! Enter into Secure Mode.  Notify SCC of security violation should FIFO
+ *  underflow occur. */
+#define RNGA_CONTROL_HIGH_ASSURANCE  0x00000002
+/*! Load data into FIFO */
+#define RNGA_CONTROL_GO              0x00000001
+/*! @} */
+
+/*! @defgroup rngastatusreg RNGA Status Register Definitions
+ *  @ingroup RNG
+ */
+/*! @addtogroup rngastatusreg */
+/*! @{ */
+/*! RNG Oscillator not working */
+#define RNGA_STATUS_OSCILLATOR_DEAD         0x80000000
+/*! These bits are undefined or reserved */
+#define RNGA_STATUS_ZEROS1_MASK             0x7f000000
+/*! How big FIFO is, in bytes */
+#define RNGA_STATUS_OUTPUT_FIFO_SIZE_MASK   0x00ff0000
+/*! How many bits right to shift fifo size to make it LSB */
+#define RNGA_STATUS_OUTPUT_FIFO_SIZE_SHIFT  16
+/*! How many bytes are currently in the FIFO */
+#define RNGA_STATUS_OUTPUT_FIFO_LEVEL_MASK  0x0000ff00
+/*! How many bits right to shift fifo level to make it LSB */
+#define RNGA_STATUS_OUTPUT_FIFO_LEVEL_SHIFT 8
+/*! These bits are undefined or reserved. */
+#define RNGA_STATUS_ZEROS2_MASK             0x000000e0
+/*! RNG is sleeping. */
+#define RNGA_STATUS_SLEEP                   0x00000010
+/*! Error detected. */
+#define RNGA_STATUS_ERROR_INTERRUPT         0x00000008
+/*! FIFO was empty on some read since last status read. */
+#define RNGA_STATUS_FIFO_UNDERFLOW          0x00000004
+/*! FIFO was empty on most recent read. */
+#define RNGA_STATUS_LAST_READ_STATUS        0x00000002
+/*! Security violation occurred.  Will only happen in High Assurance mode.  */
+#define RNGA_STATUS_SECURITY_VIOLATION      0x00000001
+/*! @} */
+
+/*! @defgroup rngmodereg RNG Mode Register Definitions
+ *  @ingroup RNG
+ */
+/*! @addtogroup rngmodereg */
+/*! @{ */
+/*! These bits are undefined or reserved */
+#define RNGA_MODE_ZEROS_MASK                0xfffffffc
+/*! RNG is in / put RNG in Oscillator Frequency Test Mode. */
+#define RNGA_MODE_OSCILLATOR_FREQ_TEST      0x00000002
+/*! Put RNG in verification mode / RNG is in verification mode. */
+#define RNGA_MODE_VERIFICATION              0x00000001
+/*! @} */
+
+/*! @defgroup rngvfctlreg RNG Verification Control Register Definitions
+ *  @ingroup RNG
+ */
+/*! @addtogroup rngvfctlreg */
+/*! @{ */
+/*! These bits are undefined or reserved. */
+#define RNGA_VFCTL_ZEROS_MASK               0xfffffff8
+/*! Reset the shift registers. */
+#define RNGA_VFCTL_RESET_SHIFT_REGISTERS    0x00000004
+/*! Drive shift registers from system clock. */
+#define RNGA_VFCTL_FORCE_SYSTEM_CLOCK       0x00000002
+/*! Turn off shift register clocks. */
+#define RNGA_VFCTL_SHIFT_CLOCK_OFF          0x00000001
+/*! @} */
+
+/*!
+ * @defgroup rngosccntctlreg RNG Oscillator Counter Control Register Definitions
+ * @ingroup RNG
+ */
+/*! @addtogroup rngosccntctlreg */
+/*! @{ */
+/*! These bits are undefined or reserved. */
+#define RNGA_OSCCR_ZEROS_MASK               0xfffc0000
+/*! Bits containing clock cycle counter */
+#define RNGA_OSCCR_CLOCK_CYCLES_MASK        0x0003ffff
+/*! Bits to shift right RNG_OSCCR_CLOCK_CYCLES_MASK */
+#define RNGA_OSCCR_CLOCK_CYCLES_SHIFT       0
+/*! @} */
+
+/*!
+ * @defgroup rngosccntreg RNG Oscillator (1 and 2) Counter Register Definitions
+ * @ingroup RNG
+ */
+/*! @addtogroup rngosccntreg */
+/*! @{ */
+/*! These bits are undefined or reserved. */
+#define RNGA_COUNTER_ZEROS_MASK             0xfff00000
+/*! Bits containing number of clock pulses received from the oscillator. */
+#define RNGA_COUNTER_PULSES_MASK            0x000fffff
+/*! Bits right to shift RNG_COUNTER_PULSES_MASK to make it LSB. */
+#define RNGA_COUNTER_PULSES_SHIFT           0
+/*! @} */
+
+/*!
+ * @defgroup rngosccntstatreg RNG Oscillator Counter Status Register Definitions
+ * @ingroup RNG
+ */
+/*! @addtogroup rngosccntstatreg */
+/*! @{ */
+/*! These bits are undefined or reserved. */
+#define RNGA_COUNTER_STATUS_ZEROS_MASK      0xfffffffc
+/*! Oscillator 2 has toggled 0x400 times */
+#define RNGA_COUNTER_STATUS_OSCILLATOR2     0x00000002
+/*! Oscillator 1 has toggled 0x400 times */
+#define RNGA_COUNTER_STATUS_OSCILLATOR1     0x00000001
+/*! @} */
+
+#endif				/* RNG_RNGA_H */
diff --git a/drivers/mxc/security/rng/include/rng_rngc.h b/drivers/mxc/security/rng/include/rng_rngc.h
new file mode 100644
index 000000000000..336b5fd5f20d
--- /dev/null
+++ b/drivers/mxc/security/rng/include/rng_rngc.h
@@ -0,0 +1,235 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/*!
+  * @file rng_rngc.h
+  *
+  * Definition of the registers for the RNGB and RNGC.	The names start with
+  * RNGC where they are in common or relate only to the RNGC;  the RNGB-only
+  * definitions begin with RNGB.
+  *
+  */
+
+#ifndef RNG_RNGC_H
+#define RNG_RNGC_H
+
+#define RNGC_VERSION_MAJOR3 3
+
+/*! @defgroup rngcregs RNGB/RNGC Registers 
+ * These are the definitions for the RNG registers and their offsets
+ * within the RNG.  They are used in the @c register_offset parameter of
+ * #rng_read_register() and #rng_write_register().
+ *
+ *  @ingroup RNG
+ */
+/*! @addtogroup rngcregs */
+/*! @{ */
+
+/*! RNGC Version ID Register R/W */
+#define RNGC_VERSION_ID 0x0000
+/*! RNGC Command Register R/W */
+#define RNGC_COMMAND 0x0004
+/*! RNGC Control Register R/W */
+#define RNGC_CONTROL 0x0008
+/*! RNGC Status Register R */
+#define RNGC_STATUS 0x000C
+/*! RNGC Error Status Register R */
+#define RNGC_ERROR 0x0010
+/*! RNGC FIFO Register W */
+#define RNGC_FIFO 0x0014
+/*! Undefined */
+#define RNGC_UNDEF_18 0x0018
+/*! RNGB Entropy Register W */
+#define RNGB_ENTROPY 0x0018
+/*! Undefined */
+#define RNGC_UNDEF_1C 0x001C
+/*! RNGC Verification Control Register1 R/W */
+#define RNGC_VERIFICATION_CONTROL 0x0020
+/*! Undefined */
+#define RNGC_UNDEF_24 0x0024
+/*! RNGB XKEY Data Register R */
+#define RNGB_XKEY 0x0024
+/*! RNGC Oscillator Counter Control Register1 R/W */
+#define RNGC_OSC_COUNTER_CONTROL 0x0028
+/*! RNGC Oscillator Counter Register1 R */
+#define RNGC_OSC_COUNTER 0x002C
+/*! RNGC Oscillator Counter Status Register1 R */
+#define RNGC_OSC_COUNTER_STATUS 0x0030
+/*! @} */
+
+/*! @defgroup rngcveridreg RNGB/RNGC Version ID Register Definitions
+ *  @ingroup RNG
+ */
+/*! @addtogroup rngcveridreg */
+/*! @{ */
+/*! These bits are unimplemented or reserved */
+#define RNGC_VERID_ZEROS_MASK          0x0f000000
+/*! Mask for RNG TYPE */
+#define RNGC_VERID_RNG_TYPE_MASK       0xf0000000
+/*! Shift to make RNG TYPE be LSB */
+#define RNGC_VERID_RNG_TYPE_SHIFT      28
+/*! Mask for RNG Chip Version */
+#define RNGC_VERID_CHIP_VERSION_MASK   0x00ff0000
+/*! Shift to make RNG Chip version be LSB */
+#define RNGC_VERID_CHIP_VERSION_SHIFT  16
+/*! Mask for RNG Major Version */
+#define RNGC_VERID_VERSION_MAJOR_MASK  0x0000ff00
+/*! Shift to make RNG Major version be LSB */
+#define RNGC_VERID_VERSION_MAJOR_SHIFT 8
+/*! Mask for RNG Minor Version */
+#define RNGC_VERID_VERSION_MINOR_MASK  0x000000ff
+/*! Shift to make RNG Minor version be LSB */
+#define RNGC_VERID_VERSION_MINOR_SHIFT 0
+/*! @} */
+
+/*! @defgroup rngccommandreg RNGB/RNGC Command Register Definitions
+ *  @ingroup RNG
+ */
+/*! @addtogroup rngccommandreg */
+/*! @{ */
+/*! These bits are unimplemented or reserved. */
+#define RNGC_COMMAND_ZEROS_MASK      0xffffff8c
+/*! Perform a software reset of the RNGC. */
+#define RNGC_COMMAND_SOFTWARE_RESET          0x00000040
+/*! Clear error from Error Status register (and interrupt). */
+#define RNGC_COMMAND_CLEAR_ERROR             0x00000020
+/*! Clear interrupt & status. */
+#define RNGC_COMMAND_CLEAR_INTERRUPT         0x00000010
+/*! Start RNGC seed generation. */
+#define RNGC_COMMAND_SEED                    0x00000002
+/*! Perform a self test of (and reset) the RNGC. */
+#define RNGC_COMMAND_SELF_TEST               0x00000001
+/*! @} */
+
+/*! @defgroup rngccontrolreg RNGB/RNGC Control Register Definitions
+ *  @ingroup RNG
+ */
+/*! @addtogroup rngccontrolreg */
+/*! @{ */
+/*! These bits are unimplemented or reserved */
+#define RNGC_CONTROL_ZEROS_MASK       0xfffffc8c
+/*! Allow access to verification registers. */
+#define RNGC_CONTROL_CTL_ACC          0x00000200
+/*! Put RNGC into deterministic verifcation mode. */
+#define RNGC_CONTROL_VERIF_MODE       0x00000100
+/*! Prevent RNGC from generating interrupts caused by errors. */
+#define RNGC_CONTROL_MASK_ERROR       0x00000040
+
+/*!
+ * Prevent RNGB/RNGC from generating interrupts after Seed Done or Self Test
+ * Mode completion.
+ */
+#define RNGC_CONTROL_MASK_DONE        0x00000020
+/*! Allow RNGC to generate a new seed whenever it is needed. */
+#define RNGC_CONTROL_AUTO_SEED        0x00000010
+/*! Set FIFO Underflow Response.*/
+#define RNGC_CONTROL_FIFO_UFLOW_MASK  0x00000003
+/*! Shift value to make FIFO Underflow Response be LSB. */
+#define RNGC_CONTROL_FIFO_UFLOW_SHIFT 0
+
+/*! @} */
+
+/*! @{  */
+/*! FIFO Underflow should cause ... */
+#define RNGC_CONTROL_FIFO_UFLOW_ZEROS_ERROR 0
+/*! FIFO Underflow should cause ... */
+#define RNGC_CONTROL_FIFO_UFLOW_ZEROS_ERROR2 1
+/*! FIFO Underflow should cause ... */
+#define RNGC_CONTROL_FIFO_UFLOW_BUS_XFR     2
+/*! FIFO Underflow should cause ... */
+#define RNGC_CONTROL_FIFO_UFLOW_ZEROS_INTR  3
+/*! @} */
+
+/*! @defgroup rngcstatusreg RNGB/RNGC Status Register Definitions
+ *  @ingroup RNG
+ */
+/*! @addtogroup rngcstatusreg */
+/*! @{ */
+/*! Unused or MBZ.  */
+#define RNGC_STATUS_ZEROS_MASK              0x003e0080
+/*!
+ * Statistical tests pass-fail.  Individual bits on indicate failure of a
+ * particular test.
+ */
+#define RNGC_STATUS_STAT_TEST_PF_MASK       0xff000000
+/*! Mask to get Statistical PF to be LSB. */
+#define RNGC_STATUS_STAT_TEST_PF_SHIFT      24
+/*!
+ * Self tests pass-fail.  Individual bits on indicate failure of a
+ * particular test.
+ */
+#define RNGC_STATUS_ST_PF_MASK              0x00c00000
+/*! Shift value to get Self Test PF field to be LSB. */
+#define RNGC_STATUS_ST_PF_SHIFT             22
+/* TRNG Self test pass-fail */
+#define RNGC_STATUS_ST_PF_TRNG              0x00800000
+/* PRNG Self test pass-fail */
+#define RNGC_STATUS_ST_PF_PRNG              0x00400000
+/*! Error detected in RNGC.  See Error Status register. */
+#define RNGC_STATUS_ERROR                   0x00010000
+/*! Size of the internal FIFO in 32-bit words. */
+#define RNGC_STATUS_FIFO_SIZE_MASK          0x0000f000
+/*! Shift value to get FIFO Size to be LSB. */
+#define RNGC_STATUS_FIFO_SIZE_SHIFT         12
+/*! The level (available data) of the internal FIFO in 32-bit words. */
+#define RNGC_STATUS_FIFO_LEVEL_MASK         0x00000f00
+/*! Shift value to get FIFO Level to be LSB. */
+#define RNGC_STATUS_FIFO_LEVEL_SHIFT        8
+/*! A new seed is ready for use. */
+#define RNGC_STATUS_NEXT_SEED_DONE          0x00000040
+/*! The first seed has been generated. */
+#define RNGC_STATUS_SEED_DONE               0x00000020
+/*! Self Test has been completed. */
+#define RNGC_STATUS_ST_DONE                 0x00000010
+/*! Reseed is necessary. */
+#define RNGC_STATUS_RESEED                  0x00000008
+/*! RNGC is sleeping. */
+#define RNGC_STATUS_SLEEP                   0x00000004
+/*! RNGC is currently generating numbers, seeding, generating next seed, or
+    performing a self test. */
+#define RNGC_STATUS_BUSY                    0x00000002
+/*! RNGC is in secure state. */
+#define RNGC_STATUS_SEC_STATE               0x00000001
+
+/*! @} */
+
+/*! @defgroup rngcerrstatusreg RNGB/RNGC Error Status Register Definitions
+ *  @ingroup RNG
+ */
+/*! @addtogroup rngcerrstatusreg */
+/*! @{ */
+/*! Unused or MBZ. */
+#define RNGC_ERROR_STATUS_ZEROS_MASK        0xffffff80
+/*! Bad Key Error Status */
+#define RNGC_ERROR_STATUS_BAD_KEY          0x00000040
+/*! Random Compare Error.  Previous number matched the current number. */
+#define RNGC_ERROR_STATUS_RAND_ERR          0x00000020
+/*! FIFO Underflow.  FIFO was read while empty. */
+#define RNGC_ERROR_STATUS_FIFO_ERR          0x00000010
+/*! Statistic Error Statistic Test failed for the last seed. */
+#define RNGC_ERROR_STATUS_STAT_ERR          0x00000008
+/*! Self-test error.  Some self test has failed. */
+#define RNGC_ERROR_STATUS_ST_ERR            0x00000004
+/*!
+ * Oscillator Error.  The oscillator may be broken.  Clear by hard or soft
+ * reset.
+ */
+#define RNGC_ERROR_STATUS_OSC_ERR           0x00000002
+/*! LFSR Error.  Clear by hard or soft reset. */
+#define RNGC_ERROR_STATUS_LFSR_ERR          0x00000001
+
+/*! @} */
+
+/*! Total address space of the RNGB/RNGC registers, in bytes */
+#define RNG_ADDRESS_RANGE 0x34
+
+#endif /* RNG_RNGC_H */
diff --git a/drivers/mxc/security/rng/include/shw_driver.h b/drivers/mxc/security/rng/include/shw_driver.h
new file mode 100644
index 000000000000..5c2280cee2b2
--- /dev/null
+++ b/drivers/mxc/security/rng/include/shw_driver.h
@@ -0,0 +1,2971 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU Lesser General
+ * Public License.  You may obtain a copy of the GNU Lesser General
+ * Public License Version 2.1 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/lgpl-license.html
+ * http://www.gnu.org/copyleft/lgpl.html
+ */
+
+#ifndef SHW_DRIVER_H
+#define SHW_DRIVER_H
+
+/* This is a Linux flag meaning 'compiling kernel code'...  */
+#ifndef __KERNEL__
+#include <inttypes.h>
+#include <stdlib.h>
+#include <sys/ioctl.h>
+#include <memory.h>
+#include <stdio.h>
+#else
+#include "../../sahara2/include/portable_os.h"
+#endif				/* __KERNEL__ */
+
+#include "../../sahara2/include/fsl_platform.h"
+
+/*! @file shw_driver.h
+ *
+ * @brief Header file to use the SHW driver.
+ *
+ * The SHW driver is used in two modes: By a user, from the FSL SHW API in user
+ * space, which goes through /dev/fsl_shw to make open(), ioctl(), and close()
+ * calls; and by other kernel modules/drivers, which use the FSL SHW API, parts
+ * of which are supported directly by the SHW driver.
+ *
+ * Testing is performed by using the apitest and kernel api test routines
+ * developed for the Sahara2 driver.
+  */
+/*#define DIAG_SECURITY_FUNC*/
+/*! Perform a security function.  */
+#define SHW_IOCTL_REQUEST 21
+
+/* This definition may need a new name, and needs to go somewhere which
+ * can determine platform, kernel vs. user, os, etc.
+ */
+#define copy_bytes(out, in, len) memcpy(out, in, len)
+
+
+/*!
+ * This is part of the IOCTL request type passed between kernel and user space.
+ * It is added to #SHW_IOCTL_REQUEST to generate the actual value.
+ */
+typedef enum shw_user_request_t {
+    SHW_USER_REQ_REGISTER_USER,    /*!< Initialize user-kernel discussion. */
+    SHW_USER_REQ_DEREGISTER_USER,  /*!< Terminate user-kernel discussion. */
+    SHW_USER_REQ_GET_RESULTS,      /*!< Get information on outstanding
+                                      results. */
+    SHW_USER_REQ_GET_CAPABILITIES, /*!< Get information on hardware support. */
+    SHW_USER_REQ_GET_RANDOM,       /*!< Get random data from RNG. */
+    SHW_USER_REQ_ADD_ENTROPY,      /*!< Add entropy to hardware RNG. */
+    SHW_USER_REQ_DROP_PERMS,       /*!< Diminish the permissions of a block of
+                                      secure memory */
+    SHW_USER_REQ_SSTATUS,          /*!< Check the status of a block of secure 
+                                      memory */
+    SHW_USER_REQ_SFREE,            /*!< Free a block of secure memory */
+    SHW_USER_REQ_SCC_ENCRYPT,      /*!< Encrypt a region of user-owned secure
+                                     memory */
+    SHW_USER_REQ_SCC_DECRYPT,     /*!< Decrypt a region of user-owned secure
+                                     memory */
+} shw_user_request_t;
+
+
+/*!
+ * @typedef scc_partition_status_t
+ */
+/** Partition status information. */
+typedef enum fsl_shw_partition_status_t {
+    FSL_PART_S_UNUSABLE,          /*!< Partition not implemented */
+    FSL_PART_S_UNAVAILABLE,       /*!< Partition owned by other host */
+    FSL_PART_S_AVAILABLE,         /*!< Partition available */
+    FSL_PART_S_ALLOCATED,         /*!< Partition owned by host but not engaged
+                                   */
+    FSL_PART_S_ENGAGED,           /*!< Partition owned by host and engaged */
+} fsl_shw_partition_status_t;
+
+
+/*
+ * Structure passed during user ioctl() calls to manage secure partitions.
+ */
+typedef struct scc_partition_info_t {
+    uint32_t user_base;            /*!< Userspace pointer to base of partition */
+    uint32_t permissions;          /*!< Permissions to give the partition (only
+                                        used in call to _DROP_PERMS) */
+    fsl_shw_partition_status_t status;  /*!< Status of the partition */
+} scc_partition_info_t;
+
+
+/******************************************************************************
+ * Enumerations
+ *****************************************************************************/
+/*!
+ * Flags for the state of the User Context Object (#fsl_shw_uco_t).
+ */
+typedef enum fsl_shw_user_ctx_flags_t
+{
+  /*!
+     * API will block the caller until operation completes.  The result will be
+     * available in the return code.  If this is not set, user will have to get
+     * results using #fsl_shw_get_results().
+     */
+    FSL_UCO_BLOCKING_MODE = 0x01,
+  /*!
+     * User wants callback (at the function specified with
+     * #fsl_shw_uco_set_callback()) when the operation completes.  This flag is
+     * valid only if #FSL_UCO_BLOCKING_MODE is not set.
+     */
+    FSL_UCO_CALLBACK_MODE = 0x02,
+    /*! Do not free descriptor chain after driver (adaptor) finishes */
+    FSL_UCO_SAVE_DESC_CHAIN = 0x04,
+  /*!
+     * User has made at least one request with callbacks requested, so API is
+     * ready to handle others.
+     */
+    FSL_UCO_CALLBACK_SETUP_COMPLETE = 0x08,
+  /*!
+     * (virtual) pointer to descriptor chain is completely linked with physical
+     * (DMA) addresses, ready for the hardware.  This flag should not be used
+     * by FSL SHW API programs.
+     */
+    FSL_UCO_CHAIN_PREPHYSICALIZED = 0x10,
+  /*!
+     * The user has changed the context but the changes have not been copied to
+     * the kernel driver.
+     */
+    FSL_UCO_CONTEXT_CHANGED = 0x20,
+    /*! Internal Use.  This context belongs to a user-mode API user. */
+    FSL_UCO_USERMODE_USER = 0x40,
+} fsl_shw_user_ctx_flags_t;
+
+
+/*!
+ * Return code for FSL_SHW library.
+ *
+ * These codes may be returned from a function call.  In non-blocking mode,
+ * they will appear as the status in a Result Object.
+ */
+/* REQ-FSLSHW-ERR-001 */
+typedef enum fsl_shw_return_t
+{
+  /*!
+     * No error.  As a function return code in Non-blocking mode, this may
+     * simply mean that the operation was accepted for eventual execution.
+     */
+    FSL_RETURN_OK_S = 0,
+   	/*! Failure for non-specific reason. */
+    FSL_RETURN_ERROR_S,
+ /*!
+    * Operation failed because some resource was not able to be allocated.
+    */
+    FSL_RETURN_NO_RESOURCE_S,
+    /*! Crypto algorithm unrecognized or improper. */
+    FSL_RETURN_BAD_ALGORITHM_S,
+    /*! Crypto mode unrecognized or improper. */
+    FSL_RETURN_BAD_MODE_S,
+    /*! Flag setting unrecognized or inconsistent. */
+    FSL_RETURN_BAD_FLAG_S,
+    /*! Improper or unsupported key length for algorithm. */
+    FSL_RETURN_BAD_KEY_LENGTH_S,
+    /*! Improper parity in a (DES, TDES) key. */
+    FSL_RETURN_BAD_KEY_PARITY_S,
+  /*!
+     * Improper or unsupported data length for algorithm or internal buffer.
+     */
+    FSL_RETURN_BAD_DATA_LENGTH_S,
+    /*! Authentication / Integrity Check code check failed. */
+    FSL_RETURN_AUTH_FAILED_S,
+    /*! A memory error occurred. */
+    FSL_RETURN_MEMORY_ERROR_S,
+    /*! An error internal to the hardware occurred. */
+    FSL_RETURN_INTERNAL_ERROR_S,
+    /*! ECC detected Point at Infinity */
+    FSL_RETURN_POINT_AT_INFINITY_S,
+    /*! ECC detected No Point at Infinity */
+    FSL_RETURN_POINT_NOT_AT_INFINITY_S,
+    /*! GCD is One */
+    FSL_RETURN_GCD_IS_ONE_S,
+    /*! GCD is not One */
+    FSL_RETURN_GCD_IS_NOT_ONE_S,
+    /*! Candidate is Prime */
+    FSL_RETURN_PRIME_S,
+    /*! Candidate is not Prime */
+    FSL_RETURN_NOT_PRIME_S,
+    /*! N register loaded improperly with even value */
+    FSL_RETURN_EVEN_MODULUS_ERROR_S,
+    /*! Divisor is zero. */
+    FSL_RETURN_DIVIDE_BY_ZERO_ERROR_S,
+    /*! Bad Exponent or Scalar value for Point Multiply */
+    FSL_RETURN_BAD_EXPONENT_ERROR_S,
+    /*! RNG hardware problem. */
+    FSL_RETURN_OSCILLATOR_ERROR_S,
+    /*! RNG hardware problem. */
+    FSL_RETURN_STATISTICS_ERROR_S,
+} fsl_shw_return_t;
+
+
+/*!
+ * Algorithm Identifier.
+ *
+ * Selection of algorithm will determine how large the block size of the
+ * algorithm is.   Context size is the same length unless otherwise specified.
+ * Selection of algorithm also affects the allowable key length.
+ */
+typedef enum fsl_shw_key_alg_t
+{
+  /*!
+     * Key will be used to perform an HMAC.  Key size is 1 to 64 octets.  Block
+     * size is 64 octets.
+     */
+    FSL_KEY_ALG_HMAC,
+  /*!
+     * Advanced Encryption Standard (Rijndael).  Block size is 16 octets.  Key
+     * size is 16 octets.  (The single choice of key size is a Sahara platform
+     * limitation.)
+     */
+    FSL_KEY_ALG_AES,
+  /*!
+     * Data Encryption Standard.  Block size is 8 octets.  Key size is 8
+     * octets.
+     */
+    FSL_KEY_ALG_DES,
+  /*!
+     * 2- or 3-key Triple DES.  Block size is 8 octets.  Key size is 16 octets
+     * for 2-key Triple DES, and 24 octets for 3-key.
+     */
+    FSL_KEY_ALG_TDES,
+  /*!
+     * ARC4.  No block size.  Context size is 259 octets.  Allowed key size is
+     * 1-16 octets.  (The choices for key size are a Sahara platform
+     * limitation.)
+     */
+    FSL_KEY_ALG_ARC4,
+} fsl_shw_key_alg_t;
+
+
+/*!
+ * Mode selector for Symmetric Ciphers.
+ *
+ * The selection of mode determines how a cryptographic algorithm will be
+ * used to process the plaintext or ciphertext.
+ *
+ * For all modes which are run block-by-block (that is, all but
+ * #FSL_SYM_MODE_STREAM), any partial operations must be performed on a text
+ * length which is multiple of the block size.  Except for #FSL_SYM_MODE_CTR,
+ * these block-by-block algorithms must also be passed a total number of octets
+ * which is a multiple of the block size.
+ *
+ * In modes which require that the total number of octets of data be a multiple
+ * of the block size (#FSL_SYM_MODE_ECB and #FSL_SYM_MODE_CBC), and the user
+ * has a total number of octets which are not a multiple of the block size, the
+ * user must perform any necessary padding to get to the correct data length.
+ */
+typedef enum fsl_shw_sym_mode_t
+{
+  /*!
+     * Stream.  There is no associated block size.  Any request to process data
+     * may be of any length.  This mode is only for ARC4 operations, and is
+     * also the only mode used for ARC4.
+     */
+    FSL_SYM_MODE_STREAM,
+
+  /*!
+     * Electronic Codebook.  Each block of data is encrypted/decrypted.  The
+     * length of the data stream must be a multiple of the block size.  This
+     * mode may be used for DES, 3DES, and AES.  The block size is determined
+     * by the algorithm.
+     */
+    FSL_SYM_MODE_ECB,
+  /*!
+     * Cipher-Block Chaining.  Each block of data is encrypted/decrypted and
+     * then "chained" with the previous block by an XOR function.  Requires
+     * context to start the XOR (previous block).  This mode may be used for
+     * DES, 3DES, and AES.  The block size is determined by the algorithm.
+     */
+    FSL_SYM_MODE_CBC,
+  /*!
+     * Counter.  The counter is encrypted, then XORed with a block of data.
+     * The counter is then incremented (using modulus arithmetic) for the next
+     * block. The final operation may be non-multiple of block size.  This mode
+     * may be used for AES.  The block size is determined by the algorithm.
+     */
+    FSL_SYM_MODE_CTR,
+} fsl_shw_sym_mode_t;
+
+
+/*!
+ * Algorithm selector for Cryptographic Hash functions.
+ *
+ * Selection of algorithm determines how large the context and digest will be.
+ * Context is the same size as the digest (resulting hash), unless otherwise
+ * specified.
+ */
+typedef enum fsl_shw_hash_alg_t
+{
+    FSL_HASH_ALG_MD5,           /*!< MD5 algorithm.  Digest is 16 octets. */
+    FSL_HASH_ALG_SHA1,          /*!< SHA-1 (aka SHA or SHA-160) algorithm.
+                                   Digest is 20 octets. */
+    FSL_HASH_ALG_SHA224,        /*!< SHA-224 algorithm.  Digest is 28 octets,
+                                   though context is 32 octets. */
+    FSL_HASH_ALG_SHA256         /*!< SHA-256 algorithm.  Digest is 32
+                                   octets. */
+} fsl_shw_hash_alg_t;
+
+
+/*!
+ * The type of Authentication-Cipher function which will be performed.
+ */
+typedef enum fsl_shw_acc_mode_t
+{
+  /*!
+     * CBC-MAC for Counter.  Requires context and modulus.  Final operation may
+     * be non-multiple of block size.  This mode may be used for AES.
+     */
+    FSL_ACC_MODE_CCM,
+  /*!
+     * SSL mode.  Not supported.  Combines HMAC and encrypt (or decrypt).
+     * Needs one key object for encryption, another for the HMAC.  The usual
+     * hashing and symmetric encryption algorithms are supported.
+     */
+    FSL_ACC_MODE_SSL
+} fsl_shw_acc_mode_t;
+
+
+/* REQ-FSLSHW-PINTFC-COA-HCO-001 */
+/*!
+ * Flags which control a Hash operation.
+ */
+typedef enum fsl_shw_hash_ctx_flags_t
+{
+    FSL_HASH_FLAGS_INIT = 0x01,     /*!< Context is empty.  Hash is started
+                                       from scratch, with a message-processed
+                                       count of zero. */
+    FSL_HASH_FLAGS_SAVE = 0x02,     /*!< Retrieve context from hardware after
+                                       hashing.  If used with the
+                                       #FSL_HASH_FLAGS_FINALIZE flag, the final
+                                       digest value will be saved in the
+                                       object. */
+    FSL_HASH_FLAGS_LOAD = 0x04,     /*!< Place context into hardware before
+                                       hashing. */
+    FSL_HASH_FLAGS_FINALIZE = 0x08, /*!< PAD message and perform final digest
+                                       operation.  If user message is
+                                       pre-padded, this flag should not be
+                                       used. */
+} fsl_shw_hash_ctx_flags_t;
+
+
+/*!
+ * Flags which control an HMAC operation.
+ *
+ * These may be combined by ORing them together.  See #fsl_shw_hmco_set_flags()
+ * and #fsl_shw_hmco_clear_flags().
+ */
+typedef enum fsl_shw_hmac_ctx_flags_t
+{
+    FSL_HMAC_FLAGS_INIT = 1,        /**< Message context is empty.  HMAC is
+                                       started from scratch (with key) or from
+                                       precompute of inner hash, depending on
+                                       whether
+                                       #FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT is
+                                       set. */
+    FSL_HMAC_FLAGS_SAVE = 2,        /**< Retrieve ongoing context from hardware
+                                       after hashing.  If used with the
+                                       #FSL_HMAC_FLAGS_FINALIZE flag, the final
+                                       digest value (HMAC) will be saved in the
+                                       object. */
+    FSL_HMAC_FLAGS_LOAD = 4,        /**< Place ongoing context into hardware
+                                       before hashing. */
+    FSL_HMAC_FLAGS_FINALIZE = 8,    /**< PAD message and perform final HMAC
+                                       operations of inner and outer hashes. */
+    FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT = 16 /**< This means that the context
+                                        contains precomputed inner and outer
+                                        hash values. */
+} fsl_shw_hmac_ctx_flags_t;
+
+
+/**
+ * Flags to control use of the #fsl_shw_scco_t.
+ *
+ * These may be ORed together to get the desired effect.
+ * See #fsl_shw_scco_set_flags() and #fsl_shw_scco_clear_flags()
+ */
+typedef enum fsl_shw_sym_ctx_flags_t
+{
+    /**
+     * Context is empty.  In ARC4, this means that the S-Box needs to be
+     * generated from the key.  In #FSL_SYM_MODE_CBC mode, this allows an IV of
+     * zero to be specified.  In #FSL_SYM_MODE_CTR mode, it means that an
+     * initial CTR value of zero is desired.
+     */
+    FSL_SYM_CTX_INIT = 1,
+    /**
+     * Load context from object into hardware before running cipher.  In
+     * #FSL_SYM_MODE_CTR mode, this would refer to the Counter Value.
+     */
+    FSL_SYM_CTX_LOAD = 2,
+    /**
+     * Save context from hardware into object after running cipher.  In
+     * #FSL_SYM_MODE_CTR mode, this would refer to the Counter Value.
+     */
+    FSL_SYM_CTX_SAVE = 4,
+    /**
+     * Context (SBox) is to be unwrapped and wrapped on each use.
+     * This flag is unsupported.
+     * */
+    FSL_SYM_CTX_PROTECT = 8,
+} fsl_shw_sym_ctx_flags_t;
+
+
+/**
+ * Flags which describe the state of the #fsl_shw_sko_t.
+ *
+ * These may be ORed together to get the desired effect.
+ * See #fsl_shw_sko_set_flags() and #fsl_shw_sko_clear_flags()
+ */
+typedef enum fsl_shw_key_flags_t
+{
+    FSL_SKO_KEY_IGNORE_PARITY = 1, /*!< If algorithm is DES or 3DES, do not
+                                      validate the key parity bits. */
+    FSL_SKO_KEY_PRESENT = 2,       /*!< Clear key is present in the object. */
+    FSL_SKO_KEY_ESTABLISHED = 4,   /*!< Key has been established for use.  This
+                                       feature is not available for all
+                                       platforms, nor for all algorithms and
+                                       modes.*/
+    FSL_SKO_USE_SECRET_KEY = 8,    /*!< Use device-unique key.  Not always
+                                       available. */
+    FSL_SKO_KEY_SW_KEY = 16,	/*!< Clear key can be provided to the user */
+	FSL_SKO_KEY_SELECT_PF_KEY = 32,	/*!< Internal flag to show that this key
+					   references one of the hardware keys, and
+					   its value is in pf_key. */
+} fsl_shw_key_flags_t;
+
+
+/**
+ * Type of value which is associated with an established key.
+ */
+typedef uint64_t key_userid_t;
+
+
+/**
+ * Flags which describe the state of the #fsl_shw_acco_t.
+ *
+ * The @a FSL_ACCO_CTX_INIT and @a FSL_ACCO_CTX_FINALIZE flags, when used
+ * together, provide for a one-shot operation.
+ */
+typedef enum fsl_shw_auth_ctx_flags_t
+{
+    FSL_ACCO_CTX_INIT = 1,      /**< Initialize Context(s) */
+    FSL_ACCO_CTX_LOAD = 2,      /**< Load intermediate context(s).
+                                   This flag is unsupported. */
+    FSL_ACCO_CTX_SAVE = 4,      /**< Save intermediate context(s).
+                                   This flag is unsupported. */
+    FSL_ACCO_CTX_FINALIZE = 8,  /**< Create MAC during this operation. */
+    FSL_ACCO_NIST_CCM = 0x10,   /**< Formatting of CCM input data is
+                                    performed by calls to
+                                    #fsl_shw_ccm_nist_format_ctr_and_iv() and
+                                    #fsl_shw_ccm_nist_update_ctr_and_iv().  */
+}  fsl_shw_auth_ctx_flags_t;
+
+
+/**
+ * The operation which controls the behavior of #fsl_shw_establish_key().
+ *
+ * These values are passed to #fsl_shw_establish_key().
+ */
+typedef enum fsl_shw_key_wrap_t
+{
+    FSL_KEY_WRAP_CREATE,        /**< Generate a key from random values. */
+    FSL_KEY_WRAP_ACCEPT,        /**< Use the provided clear key. */
+    FSL_KEY_WRAP_UNWRAP         /**< Unwrap a previously wrapped key. */
+} fsl_shw_key_wrap_t;
+
+
+/**
+ *  Modulus Selector for CTR modes.
+ *
+ * The incrementing of the Counter value may be modified by a modulus.  If no
+ * modulus is needed or desired for AES, use #FSL_CTR_MOD_128.
+ */
+typedef enum fsl_shw_ctr_mod_t
+{
+    FSL_CTR_MOD_8,              /**< Run counter with modulus of 2^8. */
+    FSL_CTR_MOD_16,             /**< Run counter with modulus of 2^16. */
+    FSL_CTR_MOD_24,             /**< Run counter with modulus of 2^24. */
+    FSL_CTR_MOD_32,             /**< Run counter with modulus of 2^32. */
+    FSL_CTR_MOD_40,             /**< Run counter with modulus of 2^40. */
+    FSL_CTR_MOD_48,             /**< Run counter with modulus of 2^48. */
+    FSL_CTR_MOD_56,             /**< Run counter with modulus of 2^56. */
+    FSL_CTR_MOD_64,             /**< Run counter with modulus of 2^64. */
+    FSL_CTR_MOD_72,             /**< Run counter with modulus of 2^72. */
+    FSL_CTR_MOD_80,             /**< Run counter with modulus of 2^80. */
+    FSL_CTR_MOD_88,             /**< Run counter with modulus of 2^88. */
+    FSL_CTR_MOD_96,             /**< Run counter with modulus of 2^96. */
+    FSL_CTR_MOD_104,            /**< Run counter with modulus of 2^104. */
+    FSL_CTR_MOD_112,            /**< Run counter with modulus of 2^112. */
+    FSL_CTR_MOD_120,            /**< Run counter with modulus of 2^120. */
+    FSL_CTR_MOD_128             /**< Run counter with modulus of 2^128. */
+} fsl_shw_ctr_mod_t;
+
+
+/**
+ * A work type associated with a work/result queue request.
+ */
+typedef enum shw_work_type_t
+{
+    SHW_WORK_GET_RANDOM = 1,    /**< fsl_shw_get_random() request.  */
+    SHW_WORK_ADD_RANDOM,        /**< fsl_shw_add_entropy() request.  */
+} shw_work_type_t;
+
+
+/**
+ * Permissions flags for Secure Partitions
+ */
+typedef enum fsl_shw_permission_t
+{
+/** SCM Access Permission: Do not zeroize/deallocate partition on SMN Fail state */
+    FSL_PERM_NO_ZEROIZE =       0x80000000,
+/** SCM Access Permission: Enforce trusted key read in  */
+    FSL_PERM_TRUSTED_KEY_READ = 0x40000000,
+/** SCM Access Permission: Ignore Supervisor/User mode in permission determination */
+    FSL_PERM_HD_S =             0x00000800,
+/** SCM Access Permission: Allow Read Access to  Host Domain */
+    FSL_PERM_HD_R =             0x00000400,
+/** SCM Access Permission: Allow Write Access to  Host Domain */
+    FSL_PERM_HD_W =             0x00000200,
+/** SCM Access Permission: Allow Execute Access to  Host Domain */
+    FSL_PERM_HD_X =             0x00000100,
+/** SCM Access Permission: Allow Read Access to Trusted Host Domain */
+    FSL_PERM_TH_R =             0x00000040,
+/** SCM Access Permission: Allow Write Access to Trusted Host Domain */
+    FSL_PERM_TH_W =             0x00000020,
+/** SCM Access Permission: Allow Read Access to Other/World Domain */
+    FSL_PERM_OT_R =             0x00000004,
+/** SCM Access Permission: Allow Write Access to Other/World Domain */
+    FSL_PERM_OT_W =             0x00000002,
+/** SCM Access Permission: Allow Execute Access to Other/World Domain */
+    FSL_PERM_OT_X =             0x00000001,
+} fsl_shw_permission_t;
+
+/*!
+ * Select the cypher mode to use for partition cover/uncover operations.
+ *
+ * They currently map directly to the values used in the SCC2 driver, but this
+ * is not guarinteed behavior.
+ */
+typedef enum fsl_shw_cypher_mode_t
+{
+    FSL_SHW_CYPHER_MODE_ECB = 1,       /*!< ECB mode */
+    FSL_SHW_CYPHER_MODE_CBC = 2,       /*!< CBC mode */
+} fsl_shw_cypher_mode_t;
+
+/*!
+ * Which platform key should be presented for cryptographic use.
+ */
+typedef enum fsl_shw_pf_key_t {
+	FSL_SHW_PF_KEY_IIM,	/*!< Present fused IIM key */
+	FSL_SHW_PF_KEY_PRG,	/*!< Present Program key */
+	FSL_SHW_PF_KEY_IIM_PRG,	/*!< Present IIM ^ Program key */
+	FSL_SHW_PF_KEY_IIM_RND,	/*!< Present Random key */
+	FSL_SHW_PF_KEY_RND,	/*!< Present IIM ^ Random key */
+} fsl_shw_pf_key_t;
+
+/*!
+ * The various security tamper events
+ */
+typedef enum fsl_shw_tamper_t {
+	FSL_SHW_TAMPER_NONE,	/*!< No error detected */
+	FSL_SHW_TAMPER_WTD,	/*!< wire-mesh tampering det */
+	FSL_SHW_TAMPER_ETBD,	/*!< ext tampering det: input B */
+	FSL_SHW_TAMPER_ETAD,	/*!< ext tampering det: input A */
+	FSL_SHW_TAMPER_EBD,	/*!< external boot detected */
+	FSL_SHW_TAMPER_SAD,	/*!< security alarm detected */
+	FSL_SHW_TAMPER_TTD,	/*!< temperature tampering det */
+	FSL_SHW_TAMPER_CTD,	/*!< clock tampering det */
+	FSL_SHW_TAMPER_VTD,	/*!< voltage tampering det */
+	FSL_SHW_TAMPER_MCO,	/*!< monotonic counter overflow */
+	FSL_SHW_TAMPER_TCO,	/*!< time counter overflow */
+} fsl_shw_tamper_t;
+
+/*
+ * Structure passed during user ioctl() calls to manage data stored in secure
+ * partitions.
+ */
+
+typedef struct scc_region_t {
+	uint32_t partition_base;	/*!< Base address of partition */
+	uint32_t offset;	/*!< Byte offset into partition */
+	uint32_t length;	/*!< Number of bytes in request */
+	uint8_t *black_data;	/*!< Address of cipher text */
+	uint64_t owner_id;	/*!< user's secret */
+	fsl_shw_cypher_mode_t cypher_mode;	/*!< ECB or CBC */
+	uint32_t IV[4];		/*!< IV for CBC mode */
+} scc_region_t;
+
+/******************************************************************************
+ * Data Structures
+ *****************************************************************************/
+
+/**
+ * Initialization Object
+ */
+typedef struct fsl_sho_ibo
+{
+} fsl_sho_ibo_t;
+
+
+/**
+ * Common Entry structure for work queues, results queues.
+ */
+typedef struct shw_queue_entry_t {
+    struct shw_queue_entry_t* next; /**< Next entry in queue. */
+    struct fsl_shw_uco_t*  user_ctx; /**< Associated user context. */
+    uint32_t         flags;     /**< User context flags at time of request. */
+    void (*callback)(struct fsl_shw_uco_t* uco); /**< Any callback request. */
+    uint32_t         user_ref;  /**< User's reference for this request. */
+    fsl_shw_return_t code;      /**< FSL SHW result of this operation. */
+    uint32_t         detail1;   /**< Any extra error info.  */
+    uint32_t         detail2;   /**< More any extra error info.  */
+    void*            user_mode_req; /**<  Pointer into user space.  */
+    uint32_t (*postprocess)(struct shw_queue_entry_t* q); /**< (internal)
+                                                           function to call
+                                                           when this operation
+                                                           completes.
+  */
+} shw_queue_entry_t;
+
+
+/**
+ * A queue.  Fields must be initialized to NULL before use.
+ */
+typedef struct shw_queue_t
+{
+    struct shw_queue_entry_t* head; /**< First entry in queue. */
+    struct shw_queue_entry_t* tail; /**< Last entry. */
+} shw_queue_t;
+
+
+/**
+ * Secure Partition information
+ */
+typedef struct fsl_shw_spo_t
+{
+    uint32_t                user_base;
+    void*                   kernel_base;
+    struct fsl_shw_spo_t*   next;
+} fsl_shw_spo_t;
+
+
+/* REQ-FSLSHW-PINTFC-COA-UCO-001 */
+/**
+ * User Context Object
+ */
+typedef struct fsl_shw_uco_t
+{
+    int           openfd;       /**< user-mode file descriptor */
+    uint32_t      user_ref;     /**< User's reference */
+    void (*callback)(struct fsl_shw_uco_t* uco); /**< User's callback fn  */
+    uint32_t      flags;             /**< from fsl_shw_user_ctx_flags_t */
+    unsigned      pool_size;    /**< maximum size of user result pool */
+#ifdef __KERNEL__
+    shw_queue_t   result_pool;  /**< where non-blocking results go */
+    os_process_handle_t  process; /**< remember for signalling User mode */
+    fsl_shw_spo_t* partition;    /**< chain of secure partitions owned by
+                                          the user */
+#endif
+    struct fsl_shw_uco_t* next; /**< To allow user-mode chaining of contexts,
+                                 for signalling and in kernel, to link user
+                                 contexts.  */
+    fsl_shw_pf_key_t wrap_key;	/*!< What key for ciphering T */
+} fsl_shw_uco_t;
+
+
+/* REQ-FSLSHW-PINTFC-API-GEN-006  ??  */
+/**
+ * Result object
+ */
+typedef struct fsl_shw_result_t
+{
+    uint32_t         user_ref;  /**< User's reference at time of request. */
+    fsl_shw_return_t code;      /**< Return code from request. */
+    uint32_t         detail1;   /**< Extra error info. Unused in SHW driver. */
+    uint32_t         detail2;   /**< Extra error info. Unused in SHW driver. */
+    void*            user_req;  /**< Pointer to original user request. */
+} fsl_shw_result_t;
+
+
+/**
+ * Keystore Object
+ */
+typedef struct fsl_shw_kso_t
+{
+#ifdef __KERNEL__
+    os_lock_t       lock;           /**< Pointer to lock that controls access to
+                                      the keystore. */
+#endif
+    void*           user_data;      /**< Pointer to user structure that handles
+                                      the internals of the keystore. */
+    fsl_shw_return_t (*data_init)        (fsl_shw_uco_t* user_ctx,
+                                          void** user_data);
+    void             (*data_cleanup)     (fsl_shw_uco_t* user_ctx,
+                                          void** user_data);
+    fsl_shw_return_t (*slot_verify_access)(void* user_data, uint64_t owner_id,
+                                          uint32_t slot);
+    fsl_shw_return_t (*slot_alloc)       (void* user_data, uint32_t size_bytes,
+                                          uint64_t owner_id, uint32_t* slot);
+    fsl_shw_return_t (*slot_dealloc)     (void* user_data,
+                                          uint64_t owner_id, uint32_t slot);
+    void*            (*slot_get_address) (void* user_data, uint32_t slot);
+    uint32_t         (*slot_get_base)    (void* user_data, uint32_t slot);
+    uint32_t         (*slot_get_offset)  (void* user_data, uint32_t slot);
+    uint32_t         (*slot_get_slot_size)  (void* user_data, uint32_t slot);
+} fsl_shw_kso_t;
+
+
+/* REQ-FSLSHW-PINTFC-COA-SKO-001 */
+/**
+ * Secret Key Context Object
+ */
+typedef struct fsl_shw_sko_t
+{
+    uint32_t          flags;    /**<  Flags from #fsl_shw_sym_ctx_flags_t. */
+    fsl_shw_key_alg_t algorithm; /**< Algorithm for this key. */
+    key_userid_t      userid;   /**< User's identifying value for Black key. */
+    uint32_t          handle;   /**< Reference in SCC driver for Red key. */
+    uint16_t          key_length;  /**< Length of stored key, in bytes. */
+    uint8_t           key[64];  /**< Bytes of stored key. */
+    struct fsl_shw_kso_t*   keystore;   /**< If present, key is in keystore */
+	fsl_shw_pf_key_t pf_key;	/*!< What key to select for use when this key
+					   is doing ciphering.  If FSL_SHW_PF_KEY_PRG
+					   or FSL_SHW_PF_KEY_PRG_IIM is the value, then
+					   a 'present' or 'established' key will be
+					   programed into the PK. */
+} fsl_shw_sko_t;
+
+
+/* REQ-FSLSHW-PINTFC-COA-CO-001 */
+/**
+ * Platform Capability Object
+ *
+ * Pointer to this structure is returned by fsl_shw_get_capabilities() and
+ * queried with the various fsl_shw_pco_() functions.
+ */
+typedef struct fsl_shw_pco_t
+{
+    int api_major;              /**< Major version number for API. */
+    int api_minor;              /**< Minor version number for API. */
+    int driver_major;           /**< Major version of some driver. */
+    int driver_minor;           /**< Minor version of some driver. */
+    unsigned sym_algorithm_count;           /**< Number of sym_algorithms. */
+    fsl_shw_key_alg_t*  sym_algorithms; /**< Pointer to array. */
+    unsigned sym_mode_count;    /**< Number of sym_modes. */
+    fsl_shw_sym_mode_t* sym_modes; /**< Pointer to array. */
+    unsigned hash_algorithm_count; /**< Number of hash_algorithms. */
+    fsl_shw_hash_alg_t* hash_algorithms; /**< Pointer to array */
+    uint8_t sym_support[5][4];  /**< indexed by key alg then mode */
+
+    int scc_driver_major;
+    int scc_driver_minor;
+    int scm_version;            /**< Version from SCM Configuration register */
+    int smn_version;            /**< Version from SMN Status register */
+    int block_size_bytes;       /**< Number of bytes per block of RAM; also
+                                   block size of the crypto algorithm. */
+    union {
+        struct scc_info {
+            int black_ram_size_blocks;  /**< Number of blocks of Black RAM */
+            int red_ram_size_blocks;    /**< Number of blocks of Red RAM */
+        } scc_info;
+        struct scc2_info {
+            int partition_size_bytes;   /**< Number of bytes in each partition */
+            int partition_count;        /**< Number of partitions on this platform */
+        } scc2_info;
+    } u;
+} fsl_shw_pco_t;
+
+
+/* REQ-FSLSHW-PINTFC-COA-HCO-001 */
+/**
+ * Hash Context Object
+ */
+typedef struct fsl_shw_hco_t /* fsl_shw_hash_context_object */
+{
+    fsl_shw_hash_alg_t       algorithm; /**< Algorithm for this context. */
+    uint32_t                 flags; /**< Flags from
+                                       #fsl_shw_hash_ctx_flags_t. */
+    uint8_t                  digest_length; /**< hash result length in bytes */
+    uint8_t                  context_length; /**< Context length in bytes */
+    uint8_t                  context_register_length; /**< in bytes */
+    uint32_t                 context[9]; /**< largest digest + msg size */
+} fsl_shw_hco_t;
+
+
+/* REQ-FSLSHW-PINTFC-COA-HCO-001 */
+/**
+ * HMAC Context Object
+ */
+typedef struct fsl_shw_hmco_t /* fsl_shw_hmac_context_object */
+{
+    fsl_shw_hash_alg_t       algorithm; /**< Hash algorithm for the HMAC. */
+    uint32_t                 flags; /**< Flags from
+                                       #fsl_shw_hmac_ctx_flags_t. */
+    uint8_t                  digest_length; /**< in bytes */
+    uint8_t                  context_length; /**< in bytes */
+    uint8_t                  context_register_length; /**< in bytes */
+    uint32_t                 ongoing_context[9]; /**< largest digest + msg
+                                                    size */
+    uint32_t                 inner_precompute[9]; /**< largest digest + msg
+                                                      size */
+    uint32_t                 outer_precompute[9]; /**< largest digest + msg
+                                                      size */
+} fsl_shw_hmco_t;
+
+
+/* REQ-FSLSHW-PINTFC-COA-SCCO-001 */
+/**
+ * Symmetric Crypto Context Object Context Object
+ */
+typedef struct fsl_shw_scco_t
+{
+    uint32_t                flags; /**< Flags from #fsl_shw_sym_ctx_flags_t. */
+    unsigned                block_size_bytes; /**< Both block and ctx size */
+    fsl_shw_sym_mode_t      mode; /**< Symmetric mode for this context. */
+    /* Could put modulus plus 16-octet context in union with arc4
+       sbox+ptrs... */
+    fsl_shw_ctr_mod_t       modulus_exp; /**< Exponent value for CTR modulus */
+    uint8_t                 context[8]; /**< Stored context.  Large enough
+                                             for 3DES.  */
+} fsl_shw_scco_t;
+
+
+/**
+ * Authenticate-Cipher Context Object
+
+ * An object for controlling the function of, and holding information about,
+ * data for the authenticate-cipher functions, #fsl_shw_gen_encrypt() and
+ * #fsl_shw_auth_decrypt().
+ */
+typedef struct fsl_shw_acco_t
+{
+    uint32_t                 flags; /**< See #fsl_shw_auth_ctx_flags_t for
+                                       meanings */
+    fsl_shw_acc_mode_t       mode; /**< CCM only */
+    uint8_t                  mac_length; /**< User's value for length  */
+    unsigned                 q_length; /**< NIST parameter - */
+    fsl_shw_scco_t           cipher_ctx_info; /**< For running
+                                                 encrypt/decrypt. */
+    union {
+        fsl_shw_scco_t       CCM_ctx_info; /**< For running the CBC in
+                                              AES-CCM.  */
+        fsl_shw_hco_t        hash_ctx_info; /**< For running the hash */
+    } auth_info;                /**< "auth" info struct  */
+    uint8_t                  unencrypted_mac[16]; /**< max block size... */
+} fsl_shw_acco_t;
+
+
+/**
+ * Common header in request structures between User-mode API and SHW driver.
+ */
+struct shw_req_header {
+    uint32_t           flags;   /**< Flags - from user-mode context. */
+    uint32_t           user_ref; /**< Reference - from user-mode context. */
+    fsl_shw_return_t   code;    /**< Result code for operation. */
+};
+
+/**
+ *  Used by user-mode API to retrieve completed non-blocking results in
+ *  SHW_USER_REQ_GET_RESULTS ioctl().
+ */
+struct results_req {
+    struct shw_req_header hdr;  /**< Boilerplate. */
+    unsigned         requested; /**< number of results requested, */
+    unsigned         actual;    /**< number of results obtained. */
+    fsl_shw_result_t *results;  /**< pointer to memory to hold results. */
+};
+
+
+/**
+ * Used by user-mode API to retrieve hardware capabilities in
+ * SHW_USER_REQ_GET_CAPABILITIES ioctl().
+ */
+struct capabilities_req {
+    struct shw_req_header hdr;  /**< Boilerplate. */
+    unsigned         size;      /**< Size, in bytes, capabilities. */
+    fsl_shw_pco_t*   capabilities; /**< Place to copy out the info. */
+};
+
+
+/**
+ * Used by user-mode API to get a random number
+ */
+struct get_random_req {
+    struct shw_req_header hdr;  /**< Boilerplate. */
+    unsigned         size;      /**< Size, in bytes, of random. */
+    uint8_t*         random;    /**< Place to copy out the random number. */
+};
+
+
+/**
+ * Used by API to add entropy to a random number generator
+ */
+struct add_entropy_req {
+    struct shw_req_header hdr;  /**< Boilerplate. */
+    unsigned         size;      /**< Size, in bytes, of entropy. */
+    uint8_t*         entropy;   /**< Location of the entropy to be added. */
+};
+
+
+/******************************************************************************
+ * External variables
+ *****************************************************************************/
+#ifdef __KERNEL__
+extern os_lock_t shw_queue_lock;
+
+extern fsl_shw_uco_t* user_list;
+#endif
+
+
+/******************************************************************************
+ * Access Macros for Objects
+ *****************************************************************************/
+/**
+ * Get FSL SHW API version
+ *
+ * @param      pcobject  The Platform Capababilities Object to query.
+ * @param[out] pcmajor   A pointer to where the major version
+ *                       of the API is to be stored.
+ * @param[out] pcminor   A pointer to where the minor version
+ *                       of the API is to be stored.
+ */
+#define fsl_shw_pco_get_version(pcobject, pcmajor, pcminor)                   \
+do {                                                                          \
+    *(pcmajor) = (pcobject)->api_major;                                       \
+    *(pcminor) = (pcobject)->api_minor;                                       \
+} while (0)
+
+
+/**
+ * Get underlying driver version.
+ *
+ * @param      pcobject  The Platform Capababilities Object to query.
+ * @param[out] pcmajor   A pointer to where the major version
+ *                       of the driver is to be stored.
+ * @param[out] pcminor   A pointer to where the minor version
+ *                       of the driver is to be stored.
+ */
+#define fsl_shw_pco_get_driver_version(pcobject, pcmajor, pcminor)            \
+do {                                                                          \
+    *(pcmajor) = (pcobject)->driver_major;                                    \
+    *(pcminor) = (pcobject)->driver_minor;                                    \
+} while (0)
+
+
+/**
+ * Get list of symmetric algorithms supported.
+ *
+ * @param pcobject           The Platform Capababilities Object to query.
+ * @param[out] pcalgorithms  A pointer to where to store the location of
+ *                           the list of algorithms.
+ * @param[out] pcacount      A pointer to where to store the number of
+ *                           algorithms in the list at @a algorithms.
+ */
+#define fsl_shw_pco_get_sym_algorithms(pcobject, pcalgorithms, pcacount)      \
+do {                                                                          \
+    *(pcalgorithms) = (pcobject)->sym_algorithms;                             \
+    *(pcacount) = (pcobject)->sym_algorithm_count;                            \
+} while (0)
+
+
+/**
+ * Get list of symmetric modes supported.
+ *
+ * @param pcobject        The Platform Capababilities Object to query.
+ * @param[out] gsmodes    A pointer to where to store the location of
+ *                        the list of modes.
+ * @param[out] gsacount   A pointer to where to store the number of
+ *                        algorithms in the list at @a modes.
+ */
+#define fsl_shw_pco_get_sym_modes(pcobject, gsmodes, gsacount)                \
+do {                                                                          \
+    *(gsmodes) = (pcobject)->sym_modes;                                       \
+    *(gsacount) = (pcobject)->sym_mode_count;                                 \
+} while (0)
+
+
+/**
+ * Get list of hash algorithms supported.
+ *
+ * @param pcobject           The Platform Capababilities Object to query.
+ * @param[out] gsalgorithms  A pointer which will be set to the list of
+ *                           algorithms.
+ * @param[out] gsacount      The number of algorithms in the list at @a
+ *                           algorithms.
+ */
+#define fsl_shw_pco_get_hash_algorithms(pcobject, gsalgorithms, gsacount)     \
+do {                                                                          \
+    *(gsalgorithms) = (pcobject)->hash_algorithms;                            \
+    *(gsacount) = (pcobject)->hash_algorithm_count;                           \
+} while (0)
+
+
+/**
+ * Determine whether the combination of a given symmetric algorithm and a given
+ * mode is supported.
+ *
+ * @param pcobject   The Platform Capababilities Object to query.
+ * @param pcalg      A Symmetric Cipher algorithm.
+ * @param pcmode     A Symmetric Cipher mode.
+ *
+ * @return 0 if combination is not supported, non-zero if supported.
+ */
+#if defined(FSL_HAVE_DRYICE) && defined(__KERNEL__)
+#define fsl_shw_pco_check_sym_supported(pcobject, pcalg, pcmode)              \
+    ((pcobject)->sym_support[pcalg][pcmode])
+#else
+#define fsl_shw_pco_check_sym_supported(pcobject, pcalg, pcmode)              \
+    0
+#endif
+
+/**
+ * Determine whether a given Encryption-Authentication mode is supported.
+ *
+ * @param pcobject  The Platform Capababilities Object to query.
+ * @param pcmode    The Authentication mode.
+ *
+ * @return 0 if mode is not supported, non-zero if supported.
+ */
+#define fsl_shw_pco_check_auth_supported(pcobject, pcmode)                    \
+    0
+
+
+/**
+ * Determine whether Black Keys (key establishment / wrapping) is supported.
+ *
+ * @param pcobject  The Platform Capababilities Object to query.
+ *
+ * @return 0 if wrapping is not supported, non-zero if supported.
+ */
+#if defined(FSL_HAVE_DRYICE) && defined(__KERNEL__)
+#define fsl_shw_pco_check_black_key_supported(pcobject)                       \
+    1
+#else
+#define fsl_shw_pco_check_black_key_supported(pcobject)                       \
+    0
+
+#endif
+
+/*!
+ * Determine whether Programmed Key features are available
+ *
+ * @param pcobject          The Platform Capabilities Object to query.
+ *
+ * @return  1 if Programmed Key features are available, otherwise zero.
+ */
+#if defined(FSL_HAVE_DRYICE) && defined(__KERNEL__)
+#define fsl_shw_pco_check_pk_supported(pcobject)        \
+    1
+#else
+#define fsl_shw_pco_check_pk_supported(pcobject)        \
+    0
+#endif
+
+/*!
+ * Determine whether Software Key features are available
+ *
+ * @param pc_info          The Platform Capabilities Object to query.
+ *
+ * @return  1 if Software key features are available, otherwise zero.
+ */
+#if defined(FSL_HAVE_DRYICE) && defined(__KERNEL__)
+#define fsl_shw_pco_check_sw_keys_supported(pcobject)        \
+    1
+#else
+#define fsl_shw_pco_check_sw_keys_supported(pcobject)        \
+    0
+#endif
+
+/*!
+ * Get FSL SHW SCC driver version
+ *
+ * @param      pcobject  The Platform Capababilities Object to query.
+ * @param[out] pcmajor   A pointer to where the major version
+ *                       of the SCC driver is to be stored.
+ * @param[out] pcminor   A pointer to where the minor version
+ *                       of the SCC driver is to be stored.
+ */
+#define fsl_shw_pco_get_scc_driver_version(pcobject, pcmajor, pcminor)        \
+{                                                                             \
+    *(pcmajor) = (pcobject)->scc_driver_major;                                \
+    *(pcminor) = (pcobject)->scc_driver_minor;                                \
+}
+
+
+/**
+ * Get SCM hardware version
+ *
+ * @param      pcobject  The Platform Capababilities Object to query.
+ * @return               The SCM hardware version
+ */
+#define fsl_shw_pco_get_scm_version(pcobject)                                 \
+    ((pcobject)->scm_version)
+
+
+/**
+ * Get SMN hardware version
+ *
+ * @param      pcobject  The Platform Capababilities Object to query.
+ * @return               The SMN hardware version
+ */
+#define fsl_shw_pco_get_smn_version(pcobject)                                 \
+    ((pcobject)->smn_version)
+
+
+/**
+ * Get the size of an SCM block, in bytes
+ *
+ * @param      pcobject  The Platform Capababilities Object to query.
+ * @return               The size of an SCM block, in bytes.
+ */
+#define fsl_shw_pco_get_scm_block_size(pcobject)                              \
+    ((pcobject)->block_size_bytes)
+
+
+/**
+ * Get size of Black and Red RAM memory
+ *
+ * @param      pcobject    The Platform Capababilities Object to query.
+ * @param[out] black_size  A pointer to where the size of the Black RAM, in
+ *                         blocks, is to be placed.
+ * @param[out] red_size    A pointer to where the size of the Red RAM, in 
+ *                         blocks, is to be placed.
+ */
+#define fsl_shw_pco_get_smn_size(pcobject, black_size, red_size)              \
+{                                                                             \
+    if ((pcobject)->scm_version == 1) {                                       \
+        *(black_size) = (pcobject)->u.scc_info.black_ram_size_blocks;           \
+        *(red_size)   = (pcobject)->u.scc_info.red_ram_size_blocks;             \
+    } else {                                                                  \
+        *(black_size) = 0;                                                    \
+        *(red_size)   = 0;                                                    \
+    }                                                                         \
+}
+
+
+/**
+ * Determine whether Secure Partitions are supported
+ *
+ * @param pcobject   The Platform Capababilities Object to query.
+ *
+ * @return 0 if secure partitions are not supported, non-zero if supported.
+ */
+#define fsl_shw_pco_check_spo_supported(pcobject)                           \
+    ((pcobject)->scm_version == 2)
+
+
+/**
+ * Get the size of a Secure Partitions
+ *
+ * @param pcobject   The Platform Capababilities Object to query.
+ * 
+ * @return Partition size, in bytes.  0 if Secure Partitions not supported.
+ */
+#define fsl_shw_pco_get_spo_size_bytes(pcobject)                            \
+    (((pcobject)->scm_version == 2) ?                                       \
+        ((pcobject)->u.scc2_info.partition_size_bytes) : 0 )                  \
+
+
+/**
+ * Get the number of Secure Partitions on this platform
+ *
+ * @param pcobject   The Platform Capababilities Object to query.
+ * 
+ * @return Number of partitions. 0 if Secure Paritions not supported.  Note
+ *         that this returns the total number of partitions, not all may be
+ *         available to the user.
+ */
+#define fsl_shw_pco_get_spo_count(pcobject)                                 \
+    (((pcobject)->scm_version == 2) ?                                       \
+        ((pcobject)->u.scc2_info.partition_count) : 0 )                       \
+
+
+/*!
+ * Initialize a Secret Key Object.
+ *
+ * This function must be called before performing any other operation with
+ * the Object.
+ *
+ * @param skobject     The Secret Key Object to be initialized.
+ * @param skalgorithm  DES, AES, etc.
+ *
+ */
+#define fsl_shw_sko_init(skobject,skalgorithm)                          \
+{                                                                       \
+    fsl_shw_sko_t* skop = skobject;                                     \
+                                                                        \
+    skop->algorithm = skalgorithm;                                      \
+    skop->flags = 0;                                                    \
+    skop->keystore = NULL;                                              \
+    skop->pf_key = FSL_SHW_PF_KEY_PRG;                                  \
+}
+
+/*!
+ * Initialize a Secret Key Object to use a Platform Key register.
+ *
+ * This function must be called before performing any other operation with
+ * the Object.
+ *
+ * @param skobject     The Secret Key Object to be initialized.
+ * @param skalgorithm  DES, AES, etc.
+ * @param skhwkey      one of the fsl_shw_pf_key_t values.
+ *
+ */
+#define fsl_shw_sko_init_pf_key(skobject,skalgorithm,skhwkey)           \
+{                                                                       \
+    fsl_shw_sko_t* skop = skobject;                                     \
+    fsl_shw_key_alg_t alg = skalgorithm;                                \
+    fsl_shw_pf_key_t key = skhwkey;                                     \
+                                                                        \
+    skop->algorithm = alg;                                              \
+    if (alg == FSL_KEY_ALG_TDES) {                                      \
+        skop->key_length = 21;                                          \
+    }                                                                   \
+    skop->keystore = NULL;                                              \
+    skop->flags = FSL_SKO_KEY_SELECT_PF_KEY;                            \
+    skop->pf_key = key;                                                 \
+    if ((key == FSL_SHW_PF_KEY_IIM) || (key == FSL_SHW_PF_KEY_PRG)      \
+        || (key == FSL_SHW_PF_KEY_IIM_PRG)                              \
+        || (key == FSL_SHW_PF_KEY_IIM_RND)                              \
+        || (key == FSL_SHW_PF_KEY_RND)) {                               \
+        skop->flags |= FSL_SKO_KEY_ESTABLISHED;                         \
+    }                                                                   \
+}
+
+/*!
+ * Store a cleartext key in the key object.
+ *
+ * This has the side effect of setting the #FSL_SKO_KEY_PRESENT flag and
+ * resetting the #FSL_SKO_KEY_ESTABLISHED flag.
+ *
+ * @param skobject     A variable of type #fsl_shw_sko_t.
+ * @param skkey        A pointer to the beginning of the key.
+ * @param skkeylen     The length, in octets, of the key.  The value should be
+ *                     appropriate to the key size supported by the algorithm.
+ *                     64 octets is the absolute maximum value allowed for this
+ *                     call.
+ */
+#define fsl_shw_sko_set_key(skobject, skkey, skkeylen)                       \
+{                                                                            \
+       (skobject)->key_length = skkeylen;                                    \
+       copy_bytes((skobject)->key, skkey, skkeylen);                         \
+       (skobject)->flags |= FSL_SKO_KEY_PRESENT;                             \
+       (skobject)->flags &= ~FSL_SKO_KEY_ESTABLISHED;                        \
+}
+
+/**
+ * Set a size for the key.
+ *
+ * This function would normally be used when the user wants the key to be
+ * generated from a random source.
+ *
+ * @param skobject   A variable of type #fsl_shw_sko_t.
+ * @param skkeylen   The length, in octets, of the key.  The value should be
+ *                   appropriate to the key size supported by the algorithm.
+ *                   64 octets is the absolute maximum value allowed for this
+ *                   call.
+ */
+#define fsl_shw_sko_set_key_length(skobject, skkeylen)                       \
+       (skobject)->key_length = skkeylen;
+
+
+/**
+ * Set the User ID associated with the key.
+ *
+ * @param skobject   A variable of type #fsl_shw_sko_t.
+ * @param skuserid   The User ID to identify authorized users of the key.
+ */
+#define fsl_shw_sko_set_user_id(skobject, skuserid)                           \
+       (skobject)->userid = (skuserid)
+
+/**
+ * Establish a user Keystore to hold the key.
+ */
+#define fsl_shw_sko_set_keystore(skobject, user_keystore)                     \
+       (skobject)->keystore = (user_keystore)
+
+
+
+/**
+ * Set the establish key handle into a key object.
+ *
+ * The @a userid field will be used to validate the access to the unwrapped
+ * key.  This feature is not available for all platforms, nor for all
+ * algorithms and modes.
+ *
+ * The #FSL_SKO_KEY_ESTABLISHED will be set (and the #FSL_SKO_KEY_PRESENT flag
+ * will be cleared).
+ *
+ * @param skobject   A variable of type #fsl_shw_sko_t.
+ * @param skuserid   The User ID to verify this user is an authorized user of
+ *                   the key.
+ * @param skhandle   A @a handle from #fsl_shw_sko_get_established_info.
+ */
+#define fsl_shw_sko_set_established_info(skobject, skuserid, skhandle)        \
+{                                                                             \
+       (skobject)->userid = (skuserid);                                       \
+       (skobject)->handle = (skhandle);                                       \
+       (skobject)->flags |= FSL_SKO_KEY_ESTABLISHED;                          \
+       (skobject)->flags &=                                                   \
+                       ~(FSL_SKO_KEY_PRESENT);   \
+}
+
+
+/**
+ * Retrieve the established-key handle from a key object.
+ *
+ * @param skobject   A variable of type #fsl_shw_sko_t.
+ * @param skhandle   The location to store the @a handle of the unwrapped
+ *                   key.
+ */
+#define fsl_shw_sko_get_established_info(skobject, skhandle)                  \
+       *(skhandle) = (skobject)->handle
+
+
+/**
+ * Extract the algorithm from a key object.
+ *
+ * @param      skobject     The Key Object to be queried.
+ * @param[out] skalgorithm  A pointer to the location to store the algorithm.
+ */
+#define fsl_shw_sko_get_algorithm(skobject, skalgorithm)                      \
+       *(skalgorithm) = (skobject)->algorithm
+
+
+/**
+ * Retrieve the cleartext key from a key object that is stored in a user
+ * keystore.
+ *
+ * @param      skobject     The Key Object to be queried.
+ * @param[out] skkey        A pointer to the location to store the key.  NULL
+ *                          if the key is not stored in a user keystore.
+ */
+#define fsl_shw_sko_get_key(skobject, skkey)                                  \
+{                                                                             \
+    fsl_shw_kso_t* keystore = (skobject)->keystore;                           \
+    if (keystore != NULL) {                                                   \
+        *(skkey) = keystore->slot_get_address(keystore->user_data,            \
+                                              (skobject)->handle);            \
+    } else {                                                                  \
+        *(skkey) = NULL;                                                      \
+    }                                                                         \
+}
+
+
+/*!
+ * Determine the size of a wrapped key based upon the cleartext key's length.
+ *
+ * This function can be used to calculate the number of octets that
+ * #fsl_shw_extract_key() will write into the location at @a covered_key.
+ *
+ * If zero is returned at @a length, this means that the key length in
+ * @a key_info is not supported.
+ *
+ * @param      wkeyinfo         Information about a key to be wrapped.
+ * @param      wkeylen          Location to store the length of a wrapped
+ *                              version of the key in @a key_info.
+ */
+#define fsl_shw_sko_calculate_wrapped_size(wkeyinfo, wkeylen)           \
+{                                                                       \
+    register fsl_shw_sko_t* kp = wkeyinfo;                              \
+    register uint32_t kl = kp->key_length;                              \
+    int key_blocks;                                                     \
+    int base_size = 35; /* ICV + T' + ALG + LEN + FLAGS */              \
+                                                                        \
+    if (kp->flags & FSL_SKO_KEY_SELECT_PF_KEY) {                        \
+        kl = 21;  /* 168-bit 3DES key */                                \
+    }                                                                   \
+    key_blocks = (kl + 7) / 8;                                          \
+    /* Round length up to 3DES block size for CBC mode */               \
+    *(wkeylen) = base_size + 8 * key_blocks;                            \
+}
+
+/*!
+ * Set some flags in the key object.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param skobject     A variable of type #fsl_shw_sko_t.
+ * @param skflags      (One or more) ORed members of #fsl_shw_key_flags_t which
+ *                     are to be set.
+ */
+#define fsl_shw_sko_set_flags(skobject, skflags)                              \
+      (skobject)->flags |= (skflags)
+
+
+/**
+ * Clear some flags in the key object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param skobject      A variable of type #fsl_shw_sko_t.
+ * @param skflags       (One or more) ORed members of #fsl_shw_key_flags_t
+ *                      which are to be reset.
+ */
+#define fsl_shw_sko_clear_flags(skobject, skflags)                            \
+      (skobject)->flags &= ~(skflags)
+
+/**
+ * Initialize a User Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  It sets the User Context Object to initial values, and set the size
+ * of the results pool.  The mode will be set to a default of
+ * #FSL_UCO_BLOCKING_MODE.
+ *
+ * When using non-blocking operations, this sets the maximum number of
+ * operations which can be outstanding.  This number includes the counts of
+ * operations waiting to start, operation(s) being performed, and results which
+ * have not been retrieved.
+ *
+ * Changes to this value are ignored once user registration has completed.  It
+ * should be set to 1 if only blocking operations will ever be performed.
+ *
+ * @param ucontext     The User Context object to operate on.
+ * @param usize        The maximum number of operations which can be
+ *                     outstanding.
+ */
+#ifdef __KERNEL__
+
+#define fsl_shw_uco_init(ucontext, usize)                                     \
+do {                                                                          \
+    fsl_shw_uco_t* uco = ucontext;                                            \
+                                                                              \
+    (uco)->pool_size = usize;                                                 \
+    (uco)->flags = FSL_UCO_BLOCKING_MODE | FSL_UCO_CONTEXT_CHANGED;           \
+    (uco)->openfd = -1;                                                       \
+    (uco)->callback = NULL;                                                   \
+    (uco)->partition = NULL;                                                  \
+    (uco)->wrap_key = FSL_SHW_PF_KEY_IIM;                                     \
+} while (0)
+
+#else /* __KERNEL__ */
+
+#define fsl_shw_uco_init(ucontext, usize)                                     \
+do {                                                                          \
+    fsl_shw_uco_t* uco = ucontext;                                            \
+                                                                              \
+    (uco)->pool_size = usize;                                                 \
+    (uco)->flags = FSL_UCO_BLOCKING_MODE | FSL_UCO_CONTEXT_CHANGED;           \
+    (uco)->openfd = -1;                                                       \
+    (uco)->callback = NULL;                                                   \
+    (uco)->wrap_key = FSL_SHW_PF_KEY_IIM;                                     \
+} while (0)
+
+#endif /* __KERNEL__ */
+
+
+/**
+ * Set the User Reference for the User Context.
+ *
+ * @param ucontext     The User Context object to operate on.
+ * @param uref         A value which will be passed back with a result.
+ */
+#define fsl_shw_uco_set_reference(ucontext, uref)                             \
+do {                                                                          \
+    fsl_shw_uco_t* uco = ucontext;                                            \
+                                                                              \
+    (uco)->user_ref = uref;                                                   \
+    (uco)->flags |= FSL_UCO_CONTEXT_CHANGED;                                  \
+} while (0)
+
+
+/**
+ * Set the User Reference for the User Context.
+ *
+ * @param ucontext     The User Context object to operate on.
+ * @param ucallback    The function the API will invoke when an operation
+ *                     completes.
+ */
+#define fsl_shw_uco_set_callback(ucontext, ucallback)                         \
+do {                                                                          \
+    fsl_shw_uco_t* uco = ucontext;                                            \
+                                                                              \
+    (uco)->callback = ucallback;                                              \
+    (uco)->flags |= FSL_UCO_CONTEXT_CHANGED;                                  \
+} while (0)
+
+/**
+ * Set flags in the User Context.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param ucontext     The User Context object to operate on.
+ * @param uflags       ORed values from #fsl_shw_user_ctx_flags_t.
+ */
+#define fsl_shw_uco_set_flags(ucontext, uflags)                               \
+      (ucontext)->flags |= (uflags) | FSL_UCO_CONTEXT_CHANGED
+
+
+/**
+ * Clear flags in the User Context.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param ucontext     The User Context object to operate on.
+ * @param uflags       ORed values from #fsl_shw_user_ctx_flags_t.
+ */
+#define fsl_shw_uco_clear_flags(ucontext, uflags)                             \
+do {                                                                          \
+    fsl_shw_uco_t* uco = ucontext;                                            \
+                                                                              \
+    (uco)->flags &= ~(uflags);                                                \
+    (uco)->flags |= FSL_UCO_CONTEXT_CHANGED;                                  \
+} while (0)
+
+
+/**
+ * Retrieve the reference value from a Result Object.
+ *
+ * @param robject  The result object to query.
+ *
+ * @return The reference associated with the request.
+ */
+#define fsl_shw_ro_get_reference(robject)                                    \
+       (robject)->user_ref
+
+
+/**
+ * Retrieve the status code from a Result Object.
+ *
+ * @param robject  The result object to query.
+ *
+ * @return The status of the request.
+ */
+#define fsl_shw_ro_get_status(robject)                                       \
+       (robject)->code
+
+
+
+/* REQ-FSL-SHW-PINTFC-API-BASIC-HASH-004 */
+/**
+ * Initialize a Hash Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  It sets the current message length and hash algorithm in the hash
+ * context object.
+ *
+ * @param      hcobject    The hash context to operate upon.
+ * @param      hcalgorithm The hash algorithm to be used (#FSL_HASH_ALG_MD5,
+ *                         #FSL_HASH_ALG_SHA256, etc).
+ *
+ */
+#define fsl_shw_hco_init(hcobject, hcalgorithm)                               \
+do {                                                                          \
+     (hcobject)->algorithm = hcalgorithm;                                     \
+     (hcobject)->flags = 0;                                                   \
+     switch (hcalgorithm) {                                                   \
+     case FSL_HASH_ALG_MD5:                                                   \
+         (hcobject)->digest_length = 16;                                      \
+         (hcobject)->context_length = 16;                                     \
+         (hcobject)->context_register_length = 24;                            \
+         break;                                                               \
+     case FSL_HASH_ALG_SHA1:                                                  \
+         (hcobject)->digest_length = 20;                                      \
+         (hcobject)->context_length = 20;                                     \
+         (hcobject)->context_register_length = 24;                            \
+         break;                                                               \
+     case FSL_HASH_ALG_SHA224:                                                \
+         (hcobject)->digest_length = 28;                                      \
+         (hcobject)->context_length = 32;                                     \
+         (hcobject)->context_register_length = 36;                            \
+         break;                                                               \
+     case FSL_HASH_ALG_SHA256:                                                \
+         (hcobject)->digest_length = 32;                                      \
+         (hcobject)->context_length = 32;                                     \
+         (hcobject)->context_register_length = 36;                            \
+         break;                                                               \
+     default:                                                                 \
+         /* error ! */                                                        \
+         (hcobject)->digest_length = 1;                                       \
+         (hcobject)->context_length = 1;                                      \
+         (hcobject)->context_register_length = 1;                             \
+         break;                                                               \
+     }                                                                        \
+} while (0)
+
+
+/* REQ-FSL-SHW-PINTFC-API-BASIC-HASH-001 */
+/**
+ * Get the current hash value and message length from the hash context object.
+ *
+ * The algorithm must have already been specified.  See #fsl_shw_hco_init().
+ *
+ * @param      hcobject   The hash context to query.
+ * @param[out] hccontext  Pointer to the location of @a length octets where to
+ *                        store a copy of the current value of the digest.
+ * @param      hcclength  Number of octets of hash value to copy.
+ * @param[out] hcmsglen   Pointer to the location to store the number of octets
+ *                        already hashed.
+ */
+#define fsl_shw_hco_get_digest(hcobject, hccontext, hcclength, hcmsglen)      \
+do {                                                                          \
+     memcpy(hccontext, (hcobject)->context, hcclength);                       \
+         if ((hcobject)->algorithm == FSL_HASH_ALG_SHA224                     \
+             || (hcobject)->algorithm == FSL_HASH_ALG_SHA256) {               \
+             *(hcmsglen) = (hcobject)->context[8];                            \
+         } else {                                                             \
+             *(hcmsglen) = (hcobject)->context[5];                            \
+         }                                                                    \
+} while (0)
+
+
+/* REQ-FSL-SHW-PINTFC-API-BASIC-HASH-002 */
+/**
+ * Get the hash algorithm from the hash context object.
+ *
+ * @param      hcobject    The hash context to query.
+ * @param[out] hcalgorithm Pointer to where the algorithm is to be stored.
+ */
+#define fsl_shw_hco_get_info(hcobject, hcalgorithm)                           \
+do {                                                                          \
+     *(hcalgorithm) = (hcobject)->algorithm;                                  \
+} while (0)
+
+
+/* REQ-FSL-SHW-PINTFC-API-BASIC-HASH-003 */
+/* REQ-FSL-SHW-PINTFC-API-BASIC-HASH-004 */
+/**
+ * Set the current hash value and message length in the hash context object.
+ *
+ * The algorithm must have already been specified.  See #fsl_shw_hco_init().
+ *
+ * @param      hcobject  The hash context to operate upon.
+ * @param      hccontext Pointer to buffer of appropriate length to copy into
+ *                       the hash context object.
+ * @param      hcmsglen  The number of octets of the message which have
+ *                        already been hashed.
+ *
+ */
+#define fsl_shw_hco_set_digest(hcobject, hccontext, hcmsglen)                 \
+do {                                                                          \
+     memcpy((hcobject)->context, hccontext, (hcobject)->context_length);      \
+     if (((hcobject)->algorithm == FSL_HASH_ALG_SHA224)                       \
+         || ((hcobject)->algorithm == FSL_HASH_ALG_SHA256)) {                 \
+         (hcobject)->context[8] = hcmsglen;                                   \
+     } else {                                                                 \
+         (hcobject)->context[5] = hcmsglen;                                   \
+     }                                                                        \
+} while (0)
+
+
+/**
+ * Set flags in a Hash Context Object.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hcobject   The hash context to be operated on.
+ * @param hcflags    The flags to be set in the context.  These can be ORed
+ *                   members of #fsl_shw_hash_ctx_flags_t.
+ */
+#define fsl_shw_hco_set_flags(hcobject, hcflags)                              \
+      (hcobject)->flags |= (hcflags)
+
+
+/**
+ * Clear flags in a Hash Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hcobject   The hash context to be operated on.
+ * @param hcflags    The flags to be reset in the context.  These can be ORed
+ *                   members of #fsl_shw_hash_ctx_flags_t.
+ */
+#define fsl_shw_hco_clear_flags(hcobject, hcflags)                            \
+      (hcobject)->flags &= ~(hcflags)
+
+
+/**
+ * Initialize an HMAC Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  It sets the current message length and hash algorithm in the HMAC
+ * context object.
+ *
+ * @param      hcobject    The HMAC context to operate upon.
+ * @param      hcalgorithm The hash algorithm to be used (#FSL_HASH_ALG_MD5,
+ *                         #FSL_HASH_ALG_SHA256, etc).
+ *
+ */
+#define fsl_shw_hmco_init(hcobject, hcalgorithm)                              \
+    fsl_shw_hco_init(hcobject, hcalgorithm)
+
+
+/**
+ * Set flags in an HMAC Context Object.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hcobject   The HMAC context to be operated on.
+ * @param hcflags    The flags to be set in the context.  These can be ORed
+ *                   members of #fsl_shw_hmac_ctx_flags_t.
+ */
+#define fsl_shw_hmco_set_flags(hcobject, hcflags)                             \
+      (hcobject)->flags |= (hcflags)
+
+
+/**
+ * Clear flags in an HMAC Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hcobject   The HMAC context to be operated on.
+ * @param hcflags    The flags to be reset in the context.  These can be ORed
+ *                   members of #fsl_shw_hmac_ctx_flags_t.
+ */
+#define fsl_shw_hmco_clear_flags(hcobject, hcflags)                           \
+      (hcobject)->flags &= ~(hcflags)
+
+
+/**
+ * Initialize a Symmetric Cipher Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  This will set the @a mode and @a algorithm and initialize the
+ * Object.
+ *
+ * @param scobject  The context object to operate on.
+ * @param scalg     The cipher algorithm this context will be used with.
+ * @param scmode    #FSL_SYM_MODE_CBC, #FSL_SYM_MODE_ECB, etc.
+ *
+ */
+#define fsl_shw_scco_init(scobject, scalg, scmode)                            \
+do {                                                                          \
+      register uint32_t bsb;   /* block-size bytes */                         \
+                                                                              \
+      switch (scalg) {                                                        \
+      case FSL_KEY_ALG_AES:                                                   \
+          bsb = 16;                                                           \
+          break;                                                              \
+      case FSL_KEY_ALG_DES:                                                   \
+          /* fall through */                                                  \
+      case FSL_KEY_ALG_TDES:                                                  \
+          bsb = 8;                                                            \
+          break;                                                              \
+      case FSL_KEY_ALG_ARC4:                                                  \
+          bsb = 259;                                                          \
+          break;                                                              \
+      case FSL_KEY_ALG_HMAC:                                                  \
+          bsb = 1;  /* meaningless */                                         \
+          break;                                                              \
+      default:                                                                \
+          bsb = 00;                                                           \
+      }                                                                       \
+      (scobject)->block_size_bytes = bsb;                                     \
+      (scobject)->mode = scmode;                                              \
+      (scobject)->flags = 0;                                                  \
+} while (0)
+
+
+/**
+ * Set the flags for a Symmetric Cipher Context.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param scobject The context object to operate on.
+ * @param scflags  The flags to reset (one or more values from
+ *                 #fsl_shw_sym_ctx_flags_t ORed together).
+ *
+ */
+#define fsl_shw_scco_set_flags(scobject, scflags)                             \
+       (scobject)->flags |= (scflags)
+
+
+/**
+ * Clear some flags in a Symmetric Cipher Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param scobject The context object to operate on.
+ * @param scflags  The flags to reset (one or more values from
+ *                 #fsl_shw_sym_ctx_flags_t ORed together).
+ *
+ */
+#define fsl_shw_scco_clear_flags(scobject, scflags)                           \
+       (scobject)->flags &= ~(scflags)
+
+
+/**
+ * Set the Context (IV) for a Symmetric Cipher Context.
+ *
+ * This is to set the context/IV for #FSL_SYM_MODE_CBC mode, or to set the
+ * context (the S-Box and pointers) for ARC4.  The full context size will
+ * be copied.
+ *
+ * @param scobject  The context object to operate on.
+ * @param sccontext A pointer to the buffer which contains the context.
+ *
+ */
+#define fsl_shw_scco_set_context(scobject, sccontext)                         \
+       memcpy((scobject)->context, sccontext,                                 \
+                  (scobject)->block_size_bytes)
+
+
+/**
+ * Get the Context for a Symmetric Cipher Context.
+ *
+ * This is to retrieve the context/IV for #FSL_SYM_MODE_CBC mode, or to
+ * retrieve context (the S-Box and pointers) for ARC4.  The full context
+ * will be copied.
+ *
+ * @param      scobject  The context object to operate on.
+ * @param[out] sccontext Pointer to location where context will be stored.
+ */
+#define fsl_shw_scco_get_context(scobject, sccontext)                         \
+       memcpy(sccontext, (scobject)->context, (scobject)->block_size_bytes)
+
+
+/**
+ * Set the Counter Value for a Symmetric Cipher Context.
+ *
+ * This will set the Counter Value for CTR mode.
+ *
+ * @param scobject  The context object to operate on.
+ * @param sccounter The starting counter value.  The number of octets.
+ *                  copied will be the block size for the algorithm.
+ * @param scmodulus The modulus for controlling the incrementing of the
+ *                  counter.
+ *
+ */
+#define fsl_shw_scco_set_counter_info(scobject, sccounter, scmodulus)        \
+do {                                                                         \
+           if ((sccounter) != NULL) {                                        \
+               memcpy((scobject)->context, sccounter,                        \
+                          (scobject)->block_size_bytes);                     \
+           }                                                                 \
+           (scobject)->modulus_exp = scmodulus;                              \
+} while (0)
+
+
+/**
+ * Get the Counter Value for a Symmetric Cipher Context.
+ *
+ * This will retrieve the Counter Value is for CTR mode.
+ *
+ * @param     scobject    The context object to query.
+ * @param[out] sccounter  Pointer to location to store the current counter
+ *                        value.  The number of octets copied will be the
+ *                        block size for the algorithm.
+ * @param[out] scmodulus  Pointer to location to store the modulus.
+ *
+ */
+#define fsl_shw_scco_get_counter_info(scobject, sccounter, scmodulus)        \
+do {                                                                         \
+           if ((sccounter) != NULL) {                                        \
+               memcpy(sccounter, (scobject)->context,                        \
+                          (scobject)->block_size_bytes);                     \
+           }                                                                 \
+           if ((scmodulus) != NULL) {                                        \
+               *(scmodulus) = (scobject)->modulus_exp;                       \
+           }                                                                 \
+} while (0)
+
+
+/**
+ * Initialize a Authentication-Cipher Context.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param acmode    The mode for this object (only #FSL_ACC_MODE_CCM
+ *                  supported).
+ */
+#define fsl_shw_acco_init(acobject, acmode)                                   \
+do {                                                                          \
+       (acobject)->flags = 0;                                                 \
+       (acobject)->mode = (acmode);                                           \
+} while (0)
+
+
+/**
+ * Set the flags for a Authentication-Cipher Context.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param acflags   The flags to set (one or more from
+ *                  #fsl_shw_auth_ctx_flags_t ORed together).
+ *
+ */
+#define fsl_shw_acco_set_flags(acobject, acflags)                             \
+       (acobject)->flags |= (acflags)
+
+
+/**
+ * Clear some flags in a Authentication-Cipher Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param acflags   The flags to reset (one or more from
+ *                  #fsl_shw_auth_ctx_flags_t ORed together).
+ *
+ */
+#define fsl_shw_acco_clear_flags(acobject, acflags)                           \
+       (acobject)->flags &= ~(acflags)
+
+
+/**
+ * Set up the Authentication-Cipher Object for CCM mode.
+ *
+ * This will set the @a auth_object for CCM mode and save the @a ctr,
+ * and @a mac_length.  This function can be called instead of
+ * #fsl_shw_acco_init().
+ *
+ * The paramater @a ctr is Counter Block 0, (counter value 0), which is for the
+ * MAC.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param acalg     Cipher algorithm.  Only AES is supported.
+ * @param accounter The initial counter value.
+ * @param acmaclen  The number of octets used for the MAC.  Valid values are
+ *                  4, 6, 8, 10, 12, 14, and 16.
+ */
+/* Do we need to stash the +1 value of the CTR somewhere? */
+#define fsl_shw_acco_set_ccm(acobject, acalg, accounter, acmaclen)            \
+ do {                                                                         \
+      (acobject)->flags = 0;                                                  \
+      (acobject)->mode = FSL_ACC_MODE_CCM;                                    \
+      (acobject)->auth_info.CCM_ctx_info.block_size_bytes = 16;               \
+      (acobject)->cipher_ctx_info.block_size_bytes = 16;                      \
+      (acobject)->mac_length = acmaclen;                                      \
+      fsl_shw_scco_set_counter_info(&(acobject)->cipher_ctx_info, accounter,  \
+            FSL_CTR_MOD_128);                                                 \
+} while (0)
+
+
+/**
+ * Format the First Block (IV) & Initial Counter Value per NIST CCM.
+ *
+ * This function will also set the IV and CTR values per Appendix A of NIST
+ * Special Publication 800-38C (May 2004).  It will also perform the
+ * #fsl_shw_acco_set_ccm() operation with information derived from this set of
+ * parameters.
+ *
+ * Note this function assumes the algorithm is AES.  It initializes the
+ * @a auth_object by setting the mode to #FSL_ACC_MODE_CCM and setting the
+ * flags to be #FSL_ACCO_NIST_CCM.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param act       The number of octets used for the MAC.  Valid values are
+ *                  4, 6, 8, 10, 12, 14, and 16.
+ * @param acad      Number of octets of Associated Data (may be zero).
+ * @param acq       A value for the size of the length of @a q field.  Valid
+ *                  values are 1-8.
+ * @param acN       The Nonce (packet number or other changing value). Must
+ *                  be (15 - @a q_length) octets long.
+ * @param acQ       The value of Q (size of the payload in octets).
+ *
+ */
+#define fsl_shw_ccm_nist_format_ctr_and_iv(acobject, act, acad, acq, acN, acQ)\
+ do {                                                                         \
+        uint64_t Q = acQ;                                                     \
+        uint8_t bflag = ((acad)?0x40:0) | ((((act)-2)/2)<<3) | ((acq)-1);     \
+        unsigned i;                                                           \
+        uint8_t* qptr = (acobject)->auth_info.CCM_ctx_info.context + 15;      \
+        (acobject)->auth_info.CCM_ctx_info.block_size_bytes = 16;             \
+        (acobject)->cipher_ctx_info.block_size_bytes = 16;                    \
+        (acobject)->mode = FSL_ACC_MODE_CCM;                                  \
+        (acobject)->flags  = FSL_ACCO_NIST_CCM;                               \
+                                                                              \
+        /* Store away the MAC length (after calculating actual value */       \
+        (acobject)->mac_length = (act);                                       \
+        /* Set Flag field in Block 0 */                                       \
+        *((acobject)->auth_info.CCM_ctx_info.context) = bflag;                \
+        /* Set Nonce field in Block 0 */                                      \
+        memcpy((acobject)->auth_info.CCM_ctx_info.context+1, acN,             \
+                   15-(acq));                                                 \
+        /* Set Flag field in ctr */                                           \
+        *((acobject)->cipher_ctx_info.context) = (acq)-1;                     \
+        /* Update the Q (payload length) field of Block0 */                   \
+        (acobject)->q_length = acq;                                           \
+        for (i = 0; i < (acq); i++) {                                         \
+            *qptr-- = Q & 0xFF;                                               \
+            Q >>= 8;                                                          \
+        }                                                                     \
+        /* Set the Nonce field of the ctr */                                  \
+        memcpy((acobject)->cipher_ctx_info.context+1, acN, 15-(acq));         \
+        /* Clear the block counter field of the ctr */                        \
+        memset((acobject)->cipher_ctx_info.context+16-(acq), 0, (acq)+1);     \
+   } while (0)
+
+
+/**
+ * Update the First Block (IV) & Initial Counter Value per NIST CCM.
+ *
+ * This function will set the IV and CTR values per Appendix A of NIST Special
+ * Publication 800-38C (May 2004).
+ *
+ * Note this function assumes that #fsl_shw_ccm_nist_format_ctr_and_iv() has
+ * previously been called on the @a auth_object.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param acN       The Nonce (packet number or other changing value). Must
+ *                  be (15 - @a q_length) octets long.
+ * @param acQ       The value of Q (size of the payload in octets).
+ *
+ */
+/* Do we need to stash the +1 value of the CTR somewhere? */
+#define fsl_shw_ccm_nist_update_ctr_and_iv(acobject, acN, acQ)                \
+  do {                                                                        \
+        uint64_t Q = acQ;                                                     \
+        unsigned i;                                                           \
+        uint8_t* qptr = (acobject)->auth_info.CCM_ctx_info.context + 15;      \
+                                                                              \
+        /* Update the Nonce field field of Block0 */                          \
+        memcpy((acobject)->auth_info.CCM_ctx_info.context+1, acN,             \
+               15 - (acobject)->q_length);                                    \
+        /* Update the Q (payload length) field of Block0 */                   \
+        for (i = 0; i < (acobject)->q_length; i++) {                          \
+            *qptr-- = Q & 0xFF;                                               \
+            Q >>= 8;                                                          \
+        }                                                                     \
+        /* Update the Nonce field of the ctr */                               \
+        memcpy((acobject)->cipher_ctx_info.context+1, acN,                    \
+               15 - (acobject)->q_length);                                    \
+   } while (0)
+
+
+/******************************************************************************
+ * Library functions
+ *****************************************************************************/
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSLSHW-PINTFC-API-GEN-003 */
+/**
+ * Determine the hardware security capabilities of this platform.
+ *
+ * Though a user context object is passed into this function, it will always
+ * act in a non-blocking manner.
+ *
+ * @param  user_ctx   The user context which will be used for the query.
+ *
+ * @return  A pointer to the capabilities object.
+ */
+extern fsl_shw_pco_t* fsl_shw_get_capabilities(fsl_shw_uco_t* user_ctx);
+
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSLSHW-PINTFC-API-GEN-004 */
+/**
+ * Create an association between the the user and the provider of the API.
+ *
+ * @param  user_ctx   The user context which will be used for this association.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_register_user(fsl_shw_uco_t* user_ctx);
+
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSLSHW-PINTFC-API-GEN-005 */
+/**
+ * Destroy the association between the the user and the provider of the API.
+ *
+ * @param  user_ctx   The user context which is no longer needed.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_deregister_user(fsl_shw_uco_t* user_ctx);
+
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSLSHW-PINTFC-API-GEN-006 */
+/**
+ * Retrieve results from earlier operations.
+ *
+ * @param         user_ctx     The user's context.
+ * @param         result_size  The number of array elements of @a results.
+ * @param[in,out] results      Pointer to first of the (array of) locations to
+ *                             store results.
+ * @param[out]    result_count Pointer to store the number of results which
+ *                             were returned.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_get_results(fsl_shw_uco_t* user_ctx,
+                                            unsigned result_size,
+                                            fsl_shw_result_t results[],
+                                            unsigned* result_count);
+
+/**
+ * Place a key into a protected location for use only by cryptographic
+ * algorithms.
+ *
+ * This only needs to be used to a) unwrap a key, or b) set up a key which
+ * could be wrapped with a later call to #fsl_shw_extract_key().  Normal
+ * cleartext keys can simply be placed into #fsl_shw_sko_t key objects with
+ * #fsl_shw_sko_set_key() and used directly.
+ *
+ * The maximum key size supported for wrapped/unwrapped keys is 32 octets.
+ * (This is the maximum reasonable key length on Sahara - 32 octets for an HMAC
+ * key based on SHA-256.)  The key size is determined by the @a key_info.  The
+ * expected length of @a key can be determined by
+ * #fsl_shw_sko_calculate_wrapped_size()
+ *
+ * The protected key will not be available for use until this operation
+ * successfully completes.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param[in,out] key_info      The information about the key to be which will
+ *                              be established.  In the create case, the key
+ *                              length must be set.
+ * @param      establish_type   How @a key will be interpreted to establish a
+ *                              key for use.
+ * @param key                   If @a establish_type is #FSL_KEY_WRAP_UNWRAP,
+ *                              this is the location of a wrapped key.  If
+ *                              @a establish_type is #FSL_KEY_WRAP_CREATE, this
+ *                              parameter can be @a NULL.  If @a establish_type
+ *                              is #FSL_KEY_WRAP_ACCEPT, this is the location
+ *                              of a plaintext key.
+ */
+extern fsl_shw_return_t fsl_shw_establish_key(
+                                fsl_shw_uco_t* user_ctx,
+                                fsl_shw_sko_t* key_info,
+                                fsl_shw_key_wrap_t establish_type,
+                                const uint8_t* key);
+
+
+/**
+ * Wrap a key and retrieve the wrapped value.
+ *
+ * A wrapped key is a key that has been cryptographically obscured.  It is
+ * only able to be used with #fsl_shw_establish_key().
+ *
+ * This function will also release the key (see #fsl_shw_release_key()) so
+ * that it must be re-established before reuse.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      key_info         The information about the key to be deleted.
+ * @param[out] covered_key      The location to store the wrapped key.
+ *                              (This size is based upon the maximum key size
+ *                              of 32 octets).
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_extract_key(fsl_shw_uco_t* user_ctx,
+                                fsl_shw_sko_t* key_info,
+                                uint8_t* covered_key);
+
+/*!
+ * Read the key value from a key object.
+ *
+ * Only a key marked as a software key (#FSL_SKO_KEY_SW_KEY) can be read with
+ * this call.  It has no effect on the status of the key store.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      key_info         The referenced key.
+ * @param[out] key              The location to store the key value.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_read_key(fsl_shw_uco_t * user_ctx,
+					 fsl_shw_sko_t * key_info,
+					 uint8_t * key);
+
+/**
+ * De-establish a key so that it can no longer be accessed.
+ *
+ * The key will need to be re-established before it can again be used.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      key_info         The information about the key to be deleted.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_release_key(
+                                fsl_shw_uco_t* user_ctx,
+                                fsl_shw_sko_t* key_info);
+
+
+/*
+ * In userspace, partition assignments will be tracked using the user context.
+ * In kernel mode, partition assignments are based on address only.
+ */
+
+/**
+ * Allocate a block of secure memory
+ *
+ * @param       user_ctx        User context
+ * @param       size            Memory size (octets).  Note: currently only
+ *                              supports only single-partition sized blocks.
+ * @param       UMID            User Mode ID to use when registering the
+ *                              partition.
+ * @param       permissions     Permissions to initialize the partition with.
+ *                              Can be made by ORing flags from the
+ *                              #fsl_shw_permission_t.
+ *
+ * @return                      Address of the allocated memory.  NULL if the
+ *                              call was not successful.
+ */
+extern void *fsl_shw_smalloc(fsl_shw_uco_t* user_ctx,
+                             uint32_t size,
+                             const uint8_t* UMID,
+                             uint32_t permissions);
+
+
+/** 
+ * Free a block of secure memory that was allocated with #fsl_shw_smalloc
+ *
+ * @param       user_ctx        User context
+ * @param       address         Address of the block of secure memory to be
+ *                              released.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_sfree(
+                                fsl_shw_uco_t* user_ctx,
+                                void* address);
+
+
+/** 
+ * Check the status of a block of a secure memory that was allocated with
+ * #fsl_shw_smalloc
+ *
+ * @param       user_ctx        User context
+ * @param       address         Address of the block of secure memory to be
+ *                              released.
+ * @param       status          Status of the partition, of type
+ *                              #fsl_partition_status_t
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_sstatus(fsl_shw_uco_t* user_ctx,
+                                        void* address,
+                                        fsl_shw_partition_status_t* status);
+
+
+/**
+ * Diminish the permissions of a block of secure memory.  Note that permissions
+ * can only be revoked.
+ *
+ * @param       user_ctx        User context
+ * @param       address         Base address of the secure memory to work with
+ * @param       permissions     Permissions to initialize the partition with.
+ *                              Can be made by ORing flags from the
+ *                              #fsl_shw_permission_t.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_diminish_perms(
+        fsl_shw_uco_t* user_ctx,
+        void* address,
+        uint32_t permissions);
+
+extern fsl_shw_return_t do_scc_engage_partition(
+        fsl_shw_uco_t* user_ctx,
+        void* address,
+        const uint8_t* UMID,
+        uint32_t permissions);
+
+extern fsl_shw_return_t do_system_keystore_slot_alloc(
+        fsl_shw_uco_t* user_ctx,
+        uint32_t key_lenth,
+        uint64_t ownerid,
+        uint32_t *slot);
+
+extern fsl_shw_return_t do_system_keystore_slot_dealloc(
+        fsl_shw_uco_t* user_ctx,
+        uint64_t ownerid,
+        uint32_t slot);
+
+extern fsl_shw_return_t do_system_keystore_slot_load(
+        fsl_shw_uco_t* user_ctx,
+        uint64_t ownerid,
+        uint32_t slot,
+        const uint8_t *key,
+        uint32_t key_length);
+
+extern fsl_shw_return_t do_system_keystore_slot_encrypt(
+        fsl_shw_uco_t* user_ctx,
+        uint64_t ownerid,
+        uint32_t slot,
+        uint32_t key_length,
+        uint8_t* black_data);
+
+extern fsl_shw_return_t do_system_keystore_slot_decrypt(
+        fsl_shw_uco_t* user_ctx,
+        uint64_t ownerid,
+        uint32_t slot,
+        uint32_t key_length,
+        const uint8_t* black_data);
+
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSL-SHW-PINTFC-COA-SKO */
+/* REQ-FSL-SHW-PINTFC-COA-SCCO */
+/* REQ-FSLSHW-PINTFC-API-BASIC-SYM-001 */
+/* PINTFC-API-BASIC-SYM-ARC4-001 */
+/* PINTFC-API-BASIC-SYM-ARC4-002 */
+/**
+ * Encrypt a stream of data with a symmetric-key algorithm.
+ *
+ * In ARC4, and also in #FSL_SYM_MODE_CBC and #FSL_SYM_MODE_CTR modes, the
+ * flags of the @a sym_ctx object will control part of the operation of this
+ * function.  The #FSL_SYM_CTX_INIT flag means that there is no context info in
+ * the object.  The #FSL_SYM_CTX_LOAD means to use information in the
+ * @a sym_ctx at the start of the operation, and the #FSL_SYM_CTX_SAVE flag
+ * means to update the object's context information after the operation has
+ * been performed.
+ *
+ * All of the data for an operation can be run through at once using the
+ * #FSL_SYM_CTX_INIT or #FSL_SYM_CTX_LOAD flags, as appropriate, and then using
+ * a @a length for the whole of the data.
+ *
+ * If a #FSL_SYM_CTX_SAVE flag were added, an additional call to the function
+ * would "pick up" where the previous call left off, allowing the user to
+ * perform the larger function in smaller steps.
+ *
+ * In #FSL_SYM_MODE_CBC and #FSL_SYM_MODE_ECB modes, the @a length must always
+ * be a multiple of the block size for the algorithm being used.  For proper
+ * operation in #FSL_SYM_MODE_CTR mode, the @a length must be a multiple of the
+ * block size until the last operation on the total octet stream.
+ *
+ * Some users of ARC4 may want to compute the context (S-Box and pointers) from
+ * the key before any data is available.  This may be done by running this
+ * function with a @a length of zero, with the init & save flags flags on in
+ * the @a sym_ctx.  Subsequent operations would then run as normal with the
+ * load and save flags.  Note that they key object is still required.
+ *
+ * @param         user_ctx  A user context from #fsl_shw_register_user().
+ * @param         key_info  Key and algorithm  being used for this operation.
+ * @param[in,out] sym_ctx   Info on cipher mode, state of the cipher.
+ * @param         length   Length, in octets, of the pt (and ct).
+ * @param         pt       pointer to plaintext to be encrypted.
+ * @param[out]    ct       pointer to where to store the resulting ciphertext.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ *
+ */
+extern fsl_shw_return_t fsl_shw_symmetric_encrypt(
+                                fsl_shw_uco_t* user_ctx,
+                                fsl_shw_sko_t* key_info,
+                                fsl_shw_scco_t* sym_ctx,
+                                uint32_t length,
+                                const uint8_t* pt,
+                                uint8_t* ct);
+
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSL-SHW-PINTFC-COA-SKO */
+/* REQ-FSL-SHW-PINTFC-COA-SCCO */
+/* PINTFC-API-BASIC-SYM-002 */
+/* PINTFC-API-BASIC-SYM-ARC4-001 */
+/* PINTFC-API-BASIC-SYM-ARC4-002 */
+/**
+ * Decrypt a stream of data with a symmetric-key algorithm.
+ *
+ * In ARC4, and also in #FSL_SYM_MODE_CBC and #FSL_SYM_MODE_CTR modes, the
+ * flags of the @a sym_ctx object will control part of the operation of this
+ * function.  The #FSL_SYM_CTX_INIT flag means that there is no context info in
+ * the object.  The #FSL_SYM_CTX_LOAD means to use information in the
+ * @a sym_ctx at the start of the operation, and the #FSL_SYM_CTX_SAVE flag
+ * means to update the object's context information after the operation has
+ * been performed.
+ *
+ * All of the data for an operation can be run through at once using the
+ * #FSL_SYM_CTX_INIT or #FSL_SYM_CTX_LOAD flags, as appropriate, and then using
+ * a @a length for the whole of the data.
+ *
+ * If a #FSL_SYM_CTX_SAVE flag were added, an additional call to the function
+ * would "pick up" where the previous call left off, allowing the user to
+ * perform the larger function in smaller steps.
+ *
+ * In #FSL_SYM_MODE_CBC and #FSL_SYM_MODE_ECB modes, the @a length must always
+ * be a multiple of the block size for the algorithm being used.  For proper
+ * operation in #FSL_SYM_MODE_CTR mode, the @a length must be a multiple of the
+ * block size until the last operation on the total octet stream.
+ *
+ * Some users of ARC4 may want to compute the context (S-Box and pointers) from
+ * the key before any data is available.  This may be done by running this
+ * function with a @a length of zero, with the #FSL_SYM_CTX_INIT &
+ * #FSL_SYM_CTX_SAVE flags on in the @a sym_ctx.  Subsequent operations would
+ * then run as normal with the load & save flags.  Note that they key object is
+ * still required.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param      key_info The key and algorithm being used in this operation.
+ * @param[in,out] sym_ctx Info on cipher mode, state of the cipher.
+ * @param      length   Length, in octets, of the ct (and pt).
+ * @param      ct       pointer to ciphertext to be decrypted.
+ * @param[out] pt       pointer to where to store the resulting plaintext.
+ *
+ * @return    A return code of type #fsl_shw_return_t
+ *
+ */
+extern fsl_shw_return_t fsl_shw_symmetric_decrypt(
+                                fsl_shw_uco_t* user_ctx,
+                                fsl_shw_sko_t* key_info,
+                                fsl_shw_scco_t* sym_ctx,
+                                uint32_t length,
+                                const uint8_t* ct,
+                                uint8_t* pt);
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSL-SHW-PINTFC-COA-HCO */
+/* REQ-FSLSHW-PINTFC-API-BASIC-HASH-005 */
+/**
+ * Hash a stream of data with a cryptographic hash algorithm.
+ *
+ * The flags in the @a hash_ctx control the operation of this function.
+ *
+ * Hashing functions work on 64 octets of message at a time.  Therefore, when
+ * any partial hashing of a long message is performed, the message @a length of
+ * each segment must be a multiple of 64.  When ready to
+ * #FSL_HASH_FLAGS_FINALIZE the hash, the @a length may be any value.
+ *
+ * With the #FSL_HASH_FLAGS_INIT and #FSL_HASH_FLAGS_FINALIZE flags on, a
+ * one-shot complete hash, including padding, will be performed.  The @a length
+ * may be any value.
+ *
+ * The first octets of a data stream can be hashed by setting the
+ * #FSL_HASH_FLAGS_INIT and #FSL_HASH_FLAGS_SAVE flags.  The @a length must be
+ * a multiple of 64.
+ *
+ * The flag #FSL_HASH_FLAGS_LOAD is used to load a context previously saved by
+ * #FSL_HASH_FLAGS_SAVE.  The two in combination will allow a (multiple-of-64
+ * octets) 'middle sequence' of the data stream to be hashed with the
+ * beginning.  The @a length must again be a multiple of 64.
+ *
+ * Since the flag #FSL_HASH_FLAGS_LOAD is used to load a context previously
+ * saved by #FSL_HASH_FLAGS_SAVE, the #FSL_HASH_FLAGS_LOAD and
+ * #FSL_HASH_FLAGS_FINALIZE flags, used together, can be used to finish the
+ * stream.  The @a length may be any value.
+ *
+ * If the user program wants to do the padding for the hash, it can leave off
+ * the #FSL_HASH_FLAGS_FINALIZE flag.  The @a length must then be a multiple of
+ * 64 octets.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param[in,out] hash_ctx Hashing algorithm and state of the cipher.
+ * @param      msg       Pointer to the data to be hashed.
+ * @param      length    Length, in octets, of the @a msg.
+ * @param[out] result    If not null, pointer to where to store the hash
+ *                       digest.
+ * @param      result_len Number of octets to store in @a result.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_hash(
+                                fsl_shw_uco_t* user_ctx,
+                                fsl_shw_hco_t* hash_ctx,
+                                const uint8_t* msg,
+                                uint32_t length,
+                                uint8_t* result,
+                                uint32_t result_len);
+
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSL-SHW-PINTFC-API-BASIC-HMAC-001 */
+/**
+ * Precompute the Key hashes for an HMAC operation.
+ *
+ * This function may be used to calculate the inner and outer precomputes,
+ * which are the hash contexts resulting from hashing the XORed key for the
+ * 'inner hash' and the 'outer hash', respectively, of the HMAC function.
+ *
+ * After execution of this function, the @a hmac_ctx will contain the
+ * precomputed inner and outer contexts, so that they may be used by
+ * #fsl_shw_hmac().  The flags of @a hmac_ctx will be updated with
+ * #FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT to mark their presence.  In addtion, the
+ * #FSL_HMAC_FLAGS_INIT flag will be set.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param      key_info  The key being used in this operation.  Key must be
+ *                       1 to 64 octets long.
+ * @param[in,out] hmac_ctx The context which controls, by its flags and
+ *                         algorithm, the operation of this function.
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_hmac_precompute(
+                                fsl_shw_uco_t* user_ctx,
+                                fsl_shw_sko_t* key_info,
+                                fsl_shw_hmco_t* hmac_ctx);
+
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSLSHW-PINTFC-API-BASIC-HMAC-002 */
+/**
+ * Continue, finalize, or one-shot an HMAC operation.
+ *
+ * There are a number of ways to use this function.  The flags in the
+ * @a hmac_ctx object will determine what operations occur.
+ *
+ * If #FSL_HMAC_FLAGS_INIT is set, then the hash will be started either from
+ * the @a key_info, or from the precomputed inner hash value in the
+ * @a hmac_ctx, depending on the value of #FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT.
+ *
+ * If, instead, #FSL_HMAC_FLAGS_LOAD is set, then the hash will be continued
+ * from the ongoing inner hash computation in the @a hmac_ctx.
+ *
+ * If #FSL_HMAC_FLAGS_FINALIZE are set, then the @a msg will be padded, hashed,
+ * the outer hash will be performed, and the @a result will be generated.
+ *
+ * If the #FSL_HMAC_FLAGS_SAVE flag is set, then the (ongoing or final) digest
+ * value will be stored in the ongoing inner hash computation field of the @a
+ * hmac_ctx.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param key_info       If #FSL_HMAC_FLAGS_INIT is set in the @a hmac_ctx,
+ *                       this is the key being used in this operation, and the
+ *                       IPAD.  If #FSL_HMAC_FLAGS_INIT is set in the @a
+ *                       hmac_ctx and @a key_info is NULL, then
+ *                       #fsl_shw_hmac_precompute() has been used to populate
+ *                       the @a inner_precompute and @a outer_precompute
+ *                       contexts.  If #FSL_HMAC_FLAGS_INIT is not set, this
+ *                       parameter is ignored.
+
+ * @param[in,out] hmac_ctx The context which controls, by its flags and
+ *                       algorithm, the operation of this function.
+ * @param      msg               Pointer to the message to be hashed.
+ * @param      length            Length, in octets, of the @a msg.
+ * @param[out] result            Pointer, of @a result_len octets, to where to
+ *                               store the HMAC.
+ * @param      result_len        Length of @a result buffer.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_hmac(
+                                fsl_shw_uco_t* user_ctx,
+                                fsl_shw_sko_t* key_info,
+                                fsl_shw_hmco_t* hmac_ctx,
+                                const uint8_t* msg,
+                                uint32_t length,
+                                uint8_t* result,
+                                uint32_t result_len);
+
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSLSHW-PINTFC-API-BASIC-RNG-002 */
+/**
+ * Get random data.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param      length    The number of octets of @a data being requested.
+ * @param[out] data      A pointer to a location of @a length octets to where
+ *                       random data will be returned.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_get_random(
+                                fsl_shw_uco_t* user_ctx,
+                                uint32_t length,
+                                uint8_t* data);
+
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSLSHW-PINTFC-API-BASIC-RNG-003 */
+/**
+ * Add entropy to random number generator.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param      length    Number of bytes at @a data.
+ * @param      data      Entropy to add to random number generator.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_add_entropy(
+                                fsl_shw_uco_t* user_ctx,
+                                uint32_t length,
+                                uint8_t* data);
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSL-SHW-PINTFC-COA-SKO */
+/**
+ * Perform Generation-Encryption by doing a Cipher and a Hash.
+ *
+ * Generate the authentication value @a auth_value as well as encrypt the @a
+ * payload into @a ct (the ciphertext).  This is a one-shot function, so all of
+ * the @a auth_data and the total message @a payload must passed in one call.
+ * This also means that the flags in the @a auth_ctx must be #FSL_ACCO_CTX_INIT
+ * and #FSL_ACCO_CTX_FINALIZE.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      auth_ctx         Controlling object for Authenticate-decrypt.
+ * @param      cipher_key_info  The key being used for the cipher part of this
+ *                              operation.  In CCM mode, this key is used for
+ *                              both parts.
+ * @param      auth_key_info    The key being used for the authentication part
+ *                              of this operation.  In CCM mode, this key is
+ *                              ignored and may be NULL.
+ * @param      auth_data_length Length, in octets, of @a auth_data.
+ * @param      auth_data        Data to be authenticated but not encrypted.
+ * @param      payload_length   Length, in octets, of @a payload.
+ * @param      payload          Pointer to the plaintext to be encrypted.
+ * @param[out] ct               Pointer to the where the encrypted @a payload
+ *                              will be stored.  Must be @a payload_length
+ *                              octets long.
+ * @param[out] auth_value       Pointer to where the generated authentication
+ *                              field will be stored. Must be as many octets as
+ *                              indicated by MAC length in the @a function_ctx.
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_gen_encrypt(
+                                fsl_shw_uco_t* user_ctx,
+                                fsl_shw_acco_t* auth_ctx,
+                                fsl_shw_sko_t* cipher_key_info,
+                                fsl_shw_sko_t* auth_key_info,
+                                uint32_t auth_data_length,
+                                const uint8_t* auth_data,
+                                uint32_t payload_length,
+                                const uint8_t* payload,
+                                uint8_t* ct,
+                                uint8_t* auth_value);
+
+/* REQ-FSL-SHW-PINTFC-COA-UCO */
+/* REQ-FSL-SHW-PINTFC-COA-SKO */
+/**
+ * Perform Authentication-Decryption in Cipher + Hash.
+ *
+ * This function will perform a one-shot decryption of a data stream as well as
+ * authenticate the authentication value.  This is a one-shot function, so all
+ * of the @a auth_data and the total message @a payload must passed in one
+ * call.  This also means that the flags in the @a auth_ctx must be
+ * #FSL_ACCO_CTX_INIT and #FSL_ACCO_CTX_FINALIZE.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      auth_ctx         Controlling object for Authenticate-decrypt.
+ * @param      cipher_key_info  The key being used for the cipher part of this
+ *                              operation.  In CCM mode, this key is used for
+ *                              both parts.
+ * @param      auth_key_info    The key being used for the authentication part
+ *                              of this operation.  In CCM mode, this key is
+ *                              ignored and may be NULL.
+ * @param      auth_data_length Length, in octets, of @a auth_data.
+ * @param      auth_data        Data to be authenticated but not decrypted.
+ * @param      payload_length   Length, in octets, of @a ct and @a pt.
+ * @param      ct               Pointer to the encrypted input stream.
+ * @param      auth_value       The (encrypted) authentication value which will
+ *                              be authenticated.  This is the same data as the
+ *                              (output) @a auth_value argument to
+ *                              #fsl_shw_gen_encrypt().
+ * @param[out] payload          Pointer to where the plaintext resulting from
+ *                              the decryption will be stored.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_auth_decrypt(
+                                fsl_shw_uco_t* user_ctx,
+                                fsl_shw_acco_t* auth_ctx,
+                                fsl_shw_sko_t* cipher_key_info,
+                                fsl_shw_sko_t* auth_key_info,
+                                uint32_t auth_data_length,
+                                const uint8_t* auth_data,
+                                uint32_t payload_length,
+                                const uint8_t* ct,
+                                const uint8_t* auth_value,
+                                uint8_t* payload);
+
+/*!
+ * Cause the hardware to create a new random key for secure memory use.
+ *
+ * Have the hardware use the secure hardware random number generator to load a
+ * new secret key into the hardware random key register.  It will not be made
+ * active without a call to #fsl_shw_select_pf_key().
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+#ifdef __KERNEL__
+
+extern fsl_shw_return_t fsl_shw_gen_random_pf_key(fsl_shw_uco_t * user_ctx);
+
+#else
+
+#define fsl_shw_gen_random_pf_key(user_ctx) FSL_RETURN_NO_RESOURCE_S
+
+#endif				/* __KERNEL__ */
+
+/*!
+ * Retrieve the detected tamper event.
+ *
+ * Note that if more than one event was detected, this routine will only ever
+ * return one of them.
+ *
+ * @param[in]  user_ctx         A user context from #fsl_shw_register_user().
+ * @param[out] tamperp          Location to store the tamper information.
+ * @param[out] timestampp       Locate to store timestamp from hardwhare when
+ *                              an event was detected.
+ *
+ *
+ * @return    A return code of type #fsl_shw_return_t (for instance, if the platform
+ *            is not in a fail state.
+ */
+#ifdef __KERNEL__
+
+extern fsl_shw_return_t fsl_shw_read_tamper_event(fsl_shw_uco_t * user_ctx,
+						  fsl_shw_tamper_t * tamperp,
+						  uint64_t * timestampp);
+#else
+
+#define fsl_shw_read_tamper_event(user_ctx,tamperp,timestampp)         \
+    FSL_RETURN_NO_RESOURCE_S
+
+#endif				/* __KERNEL__ */
+
+/*****************************************************************************
+ *
+ * Functions internal to SHW driver.
+ *
+*****************************************************************************/
+
+fsl_shw_return_t 
+do_scc_encrypt_region(fsl_shw_uco_t* user_ctx,
+                      void* partition_base, uint32_t offset_bytes,
+                      uint32_t byte_count, uint8_t* black_data,
+                      uint32_t* IV, fsl_shw_cypher_mode_t cypher_mode);
+
+fsl_shw_return_t 
+do_scc_decrypt_region(fsl_shw_uco_t* user_ctx,
+                      void* partition_base, uint32_t offset_bytes,
+                      uint32_t byte_count, const uint8_t* black_data,
+                      uint32_t* IV, fsl_shw_cypher_mode_t cypher_mode);
+
+
+/*****************************************************************************
+ *
+ * Functions available to other SHW-family drivers.
+ *
+*****************************************************************************/
+
+#ifdef __KERNEL__
+/**
+ * Add an entry to a work/result queue.
+ *
+ * @param pool  Pointer to list structure
+ * @param entry Entry to place at tail of list
+ *
+ * @return void
+ */
+inline static void SHW_ADD_QUEUE_ENTRY(shw_queue_t* pool,
+                                       shw_queue_entry_t* entry)
+{
+    os_lock_context_t lock_context;
+
+    entry->next = NULL;
+    os_lock_save_context(shw_queue_lock, lock_context);
+
+    if (pool->tail != NULL) {
+        pool->tail->next = entry;
+    } else {
+        /* Queue was empty, so this is also the head. */
+        pool->head = entry;
+    }
+    pool->tail = entry;
+
+    os_unlock_restore_context(shw_queue_lock, lock_context);
+
+    return;
+
+
+}
+
+
+/**
+ * Get first entry on the queue and remove it from the queue.
+ *
+ * @return Pointer to first entry, or NULL if none.
+ */
+inline static shw_queue_entry_t* SHW_POP_FIRST_ENTRY(shw_queue_t* queue)
+{
+    shw_queue_entry_t* entry;
+    os_lock_context_t lock_context;
+
+    os_lock_save_context(shw_queue_lock, lock_context);
+
+    entry = queue->head;
+
+    if (entry != NULL) {
+        queue->head = entry->next;
+        entry->next = NULL;
+        /* If this was only entry, clear the tail. */
+        if (queue->tail == entry) {
+            queue->tail = NULL;
+        }
+    }
+
+    os_unlock_restore_context(shw_queue_lock, lock_context);
+
+    return entry;
+}
+
+
+
+/**
+ * Remove an entry from the list.
+ *
+ * If the entry not on the queue, no error will be returned.
+ *
+ * @param pool Pointer to work queue
+ * @param entry Entry to remove from queue
+ *
+ * @return void
+ *
+ */
+inline static void SHW_QUEUE_REMOVE_ENTRY(shw_queue_t* pool,
+                                   shw_queue_entry_t* entry)
+{
+    os_lock_context_t lock_context;
+
+    os_lock_save_context(shw_queue_lock, lock_context);
+
+    /* Check for quick case.*/
+    if (pool->head == entry) {
+        pool->head = entry->next;
+        entry->next = NULL;
+        if (pool->tail == entry) {
+            pool->tail = NULL;
+        }
+    } else {
+        register shw_queue_entry_t* prev = pool->head;
+
+        /* We know it is not the head, so start looking at entry after head. */
+        while (prev->next) {
+            if (prev->next != entry) {
+                prev = prev->next; /* Try another */
+                continue;
+            } else {
+                /* Unlink from chain. */
+                prev->next = entry->next;
+                entry->next = NULL;
+                /* If last in chain, update tail. */
+                if (pool->tail == entry) {
+                    pool->tail = prev;
+                }
+                break;
+            }
+        } /* while */
+    }
+
+    os_unlock_restore_context(shw_queue_lock, lock_context);
+
+    return;
+}
+#endif /* __KERNEL__ */
+
+
+/*****************************************************************************
+ *
+ * Functions available to User-Mode API functions
+ *
+ ****************************************************************************/
+#ifndef __KERNEL__
+
+
+ /**
+ * Sanity checks the user context object fields to ensure that they make some
+ * sense before passing the uco as a parameter.
+ *
+ * @brief Verify the user context object
+ *
+ * @param  uco  user context object
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t validate_uco(fsl_shw_uco_t *uco);
+
+
+/**
+ * Initialize a request block to go to the driver.
+ *
+ * @param hdr      Pointer to request block header
+ * @param user_ctx Pointer to user's context
+ *
+ * @return void
+ */
+inline static void init_req(struct shw_req_header* hdr,
+                            fsl_shw_uco_t* user_ctx)
+{
+    hdr->flags = user_ctx->flags;
+    hdr->user_ref = user_ctx->user_ref;
+    hdr->code = FSL_RETURN_ERROR_S;
+
+    return;
+}
+
+
+/**
+ * Send a request block off to the driver.
+ *
+ * If this is a non-blocking request, then req will be freed.
+ *
+ * @param type  The type of request being sent
+ * @param req   Pointer to the request block
+ * @param ctx   Pointer to user's context
+ *
+ * @return code from driver if ioctl() succeeded, otherwise
+ *              FSL_RETURN_INTERNAL_ERROR_S.
+ */
+inline static fsl_shw_return_t send_req(shw_user_request_t type,
+                                        struct shw_req_header* req,
+                                        fsl_shw_uco_t* ctx)
+{
+    fsl_shw_return_t ret = FSL_RETURN_INTERNAL_ERROR_S;
+    unsigned blocking = ctx->flags & FSL_UCO_BLOCKING_MODE;
+    int code;
+
+    code = ioctl(ctx->openfd, SHW_IOCTL_REQUEST + type, req);
+
+    if (code == 0) {
+        if (blocking) {
+            ret = req->code;
+        } else {
+            ret = FSL_RETURN_OK_S;
+        }
+    } else {
+#ifdef FSL_DEBUG
+        fprintf(stderr, "SHW: send_req failed with (%d), %s\n", errno,
+                strerror(errno));
+#endif
+    }
+
+    if (blocking) {
+        free(req);
+    }
+
+    return ret;
+}
+
+
+#endif				/* no __KERNEL__ */
+
+#if defined(FSL_HAVE_DRYICE)
+/* Some kernel functions */
+void fsl_shw_permute1_bytes(const uint8_t * key, uint8_t * permuted_key,
+			    int key_count);
+void fsl_shw_permute1_bytes_to_words(const uint8_t * key,
+				     uint32_t * permuted_key, int key_count);
+
+#define PFKEY_TO_STR(key_in)                                    \
+({                                                              \
+    di_key_t key = key_in;                                      \
+                                                                \
+    ((key == DI_KEY_FK) ? "IIM" :                               \
+     ((key == DI_KEY_PK) ? "PRG" :                              \
+      ((key == DI_KEY_RK) ? "RND" :                             \
+       ((key == DI_KEY_FPK) ? "IIM_PRG" :                       \
+        ((key == DI_KEY_FRK) ? "IIM_RND" : "unk")))));          \
+})
+
+#ifdef DIAG_SECURITY_FUNC
+extern const char *di_error_string(int code);
+#endif
+
+#endif				/* HAVE DRYICE */
+
+#endif				/* SHW_DRIVER_H */
diff --git a/drivers/mxc/security/rng/include/shw_hash.h b/drivers/mxc/security/rng/include/shw_hash.h
new file mode 100644
index 000000000000..5cd229003637
--- /dev/null
+++ b/drivers/mxc/security/rng/include/shw_hash.h
@@ -0,0 +1,96 @@
+/*

+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.

+ */

+

+/*

+ * The code contained herein is licensed under the GNU Lesser General

+ * Public License.  You may obtain a copy of the GNU Lesser General

+ * Public License Version 2.1 or later at the following locations:

+ *

+ * http://www.opensource.org/licenses/lgpl-license.html

+ * http://www.gnu.org/copyleft/lgpl.html

+ */

+

+/*!

+ * @file shw_hash.h

+ *

+ * This file contains definitions for use of the (internal) SHW hash

+ * software computation.  It defines the usual three steps:

+ *

+ * - #shw_hash_init()

+ * - #shw_hash_update()

+ * - #shw_hash_final()

+ *

+ * The only other item of note to callers is #SHW_HASH_LEN, which is the number

+ * of bytes calculated for the hash.

+ */

+

+#ifndef SHW_HASH_H

+#define SHW_HASH_H

+

+/*! Define which gives the number of bytes available in an hash result */

+#define SHW_HASH_LEN 32

+

+/* Define which matches block length in bytes of the underlying hash */

+#define SHW_HASH_BLOCK_LEN 64

+

+/* "Internal" define which matches SHA-256 state size (32-bit words) */

+#define SHW_HASH_STATE_WORDS 8

+

+/* "Internal" define which matches word length in blocks of the underlying

+   hash. */

+#define SHW_HASH_BLOCK_WORD_SIZE 16

+

+#define SHW_HASH_STATE_SIZE 32

+

+/*!

+ * State for a SHA-1/SHA-2 Hash

+ *

+ * (Note to maintainers: state needs to be updated to uint64_t to handle

+ * SHA-384/SHA-512)... And bit_count to uint128_t (heh).

+ */

+typedef struct shw_hash_state {

+	unsigned int partial_count_bytes;	/*!< Number of bytes of message sitting

+						 * in @c partial_block */

+	uint8_t partial_block[SHW_HASH_BLOCK_LEN];	/*!< Data waiting to be processed as a block  */

+	uint32_t state[SHW_HASH_STATE_WORDS];	/*!< Current hash state variables */

+	uint64_t bit_count;	/*!< Number of bits sent through the update function */

+} shw_hash_state_t;

+

+/*!

+ * Initialize the hash state structure

+ *

+ * @param state     Address of hash state structure.

+ * @param algorithm Which hash algorithm to use (must be FSL_HASH_ALG_SHA256)

+ *

+ * @return  FSL_RETURN_OK_S if all went well, otherwise an error code.

+ */

+fsl_shw_return_t shw_hash_init(shw_hash_state_t * state,

+			       fsl_shw_hash_alg_t algorithm);

+

+/*!

+ * Put data into the hash calculation

+ *

+ * @param state     Address of hash state structure.

+ * @param msg       Address of the message data for the hash.

+ * @param msg_len   Number of bytes of @c msg.

+ *

+ * @return  FSL_RETURN_OK_S if all went well, otherwise an error code.

+ */

+fsl_shw_return_t shw_hash_update(shw_hash_state_t * state,

+				 const uint8_t * msg, unsigned int msg_len);

+

+/*!

+ * Calculate the final hash value

+ *

+ * @param state     Address of hash state structure.

+ * @param hash      Address of location to store the hash.

+ * @param hash_len  Number of bytes of @c hash to be stored.

+ *

+ * @return FSL_RETURN_OK_S if all went well, FSL_RETURN_BAD_DATA_LENGTH_S if

+ * hash_len is too long, otherwise an error code.

+ */

+fsl_shw_return_t shw_hash_final(shw_hash_state_t * state,

+				uint8_t * hash, unsigned int hash_len);

+

+#endif				/* SHW_HASH_H */

diff --git a/drivers/mxc/security/rng/include/shw_hmac.h b/drivers/mxc/security/rng/include/shw_hmac.h
new file mode 100644
index 000000000000..de44941f2ef2
--- /dev/null
+++ b/drivers/mxc/security/rng/include/shw_hmac.h
@@ -0,0 +1,82 @@
+/*

+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.

+ */

+

+/*

+ * The code contained herein is licensed under the GNU Lesser General

+ * Public License.  You may obtain a copy of the GNU Lesser General

+ * Public License Version 2.1 or later at the following locations:

+ *

+ * http://www.opensource.org/licenses/lgpl-license.html

+ * http://www.gnu.org/copyleft/lgpl.html

+ */

+

+/*!

+ * @file shw_hmac.h

+ *

+ * This file contains definitions for use of the (internal) SHW HMAC

+ * software computation.  It defines the usual three steps:

+ *

+ * - #shw_hmac_init()

+ * - #shw_hmac_update()

+ * - #shw_hmac_final()

+ *

+ * The only other item of note to callers is #SHW_HASH_LEN, which is the number

+ * of bytes calculated for the HMAC.

+ */

+

+#ifndef SHW_HMAC_H

+#define SHW_HMAC_H

+

+#include "shw_hash.h"

+

+/*!

+ * State for an HMAC

+ *

+ * Note to callers: This structure contains key material and should be kept in

+ * a secure location, such as internal RAM.

+ */

+typedef struct shw_hmac_state {

+	shw_hash_state_t inner_hash;	/*!< Current state of inner hash */

+	shw_hash_state_t outer_hash;	/*!< Current state of outer hash */

+} shw_hmac_state_t;

+

+/*!

+ * Initialize the HMAC state structure with the HMAC key

+ *

+ * @param state     Address of HMAC state structure.

+ * @param key       Address of the key to be used for the HMAC.

+ * @param key_len   Number of bytes of @c key.  This must not be greater than

+ *                  the block size of the underlying hash (#SHW_HASH_BLOCK_LEN).

+ *

+ * @return  FSL_RETURN_OK_S if all went well, otherwise an error code.

+ */

+fsl_shw_return_t shw_hmac_init(shw_hmac_state_t * state,

+			       const uint8_t * key, unsigned int key_len);

+

+/*!

+ * Put data into the HMAC calculation

+ *

+ * @param state     Address of HMAC state structure.

+ * @param msg       Address of the message data for the HMAC.

+ * @param msg_len   Number of bytes of @c msg.

+ *

+ * @return  FSL_RETURN_OK_S if all went well, otherwise an error code.

+ */

+fsl_shw_return_t shw_hmac_update(shw_hmac_state_t * state,

+				 const uint8_t * msg, unsigned int msg_len);

+

+/*!

+ * Calculate the final HMAC

+ *

+ * @param state     Address of HMAC state structure.

+ * @param hmac      Address of location to store the HMAC.

+ * @param hmac_len  Number of bytes of @c mac to be stored.  Probably best if

+ *                  this value is no greater than #SHW_HASH_LEN.

+ *

+ * @return  FSL_RETURN_OK_S if all went well, otherwise an error code.

+ */

+fsl_shw_return_t shw_hmac_final(shw_hmac_state_t * state,

+				uint8_t * hmac, unsigned int hmac_len);

+

+#endif				/* SHW_HMAC_H */

diff --git a/drivers/mxc/security/rng/include/shw_internals.h b/drivers/mxc/security/rng/include/shw_internals.h
new file mode 100644
index 000000000000..7e45af868053
--- /dev/null
+++ b/drivers/mxc/security/rng/include/shw_internals.h
@@ -0,0 +1,162 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef SHW_INTERNALS_H
+#define SHW_INTERNALS_H
+
+/*! @file shw_internals.h
+ *
+ *  This file contains definitions which are internal to the SHW driver.
+ *
+ *  This header file should only ever be included by shw_driver.c
+ *
+ *  Compile-time flags minimally needed:
+ *
+ *  @li Some sort of platform flag.
+ *
+ */
+
+#include "portable_os.h"
+#include "shw_driver.h"
+
+/*! @defgroup shwcompileflags SHW Compile Flags
+ *
+ * These are flags which are used to configure the SHW driver at compilation
+ * time.
+ *
+ * The terms 'defined' and 'undefined' refer to whether a @c \#define (or -D on
+ * a compile command) has defined a given preprocessor symbol.  If a given
+ * symbol is defined, then @c \#ifdef \<symbol\> will succeed.  Some symbols
+ * described below default to not having a definition, i.e. they are undefined.
+ *
+ */
+
+/*! @addtogroup shwcompileflags */
+/*! @{ */
+#ifndef SHW_MAJOR_NODE
+/*!
+ * This should be configured in a Makefile/compile command line.  It is the
+ * value the driver will use to register itself as a device driver for a
+ * /dev/node file.  Zero means allow (Linux) to assign a value.  Any positive
+ * number will be attempted as the registration value, to allow for
+ * coordination with the creation/existence of a /dev/fsl_shw (for instance)
+ * file in the filesystem.
+ */
+#define SHW_MAJOR_NODE 0
+#endif
+
+/* Temporarily define compile-time flags to make Doxygen happy and allow them
+   to get into the documentation. */
+#ifdef DOXYGEN_HACK
+
+/*!
+ * Turn on compilation of run-time operational, debug, and error messages.
+ *
+ * This flag is undefined by default.
+ */
+/* REQ-FSLSHW-DEBUG-001 */
+#define SHW_DEBUG
+#undef SHW_DEBUG
+
+/*! @} */
+#endif				/* end DOXYGEN_HACK */
+
+#ifndef SHW_DRIVER_NAME
+/*! @addtogroup shwcompileflags */
+/*! @{ */
+/*! Name the driver will use to register itself to the kernel as the driver for
+ *  the #shw_major_node and interrupt handling. */
+#define SHW_DRIVER_NAME "fsl_shw"
+/*! @} */
+#endif
+/*#define SHW_DEBUG*/
+
+/*!
+ * Add a user context onto the list of registered users.
+ *
+ * Place it at the head of the #user_list queue.
+ *
+ * @param ctx  A pointer to a user context
+ *
+ * @return void
+ */
+inline static void SHW_ADD_USER(fsl_shw_uco_t * ctx)
+{
+	os_lock_context_t lock_context;
+
+	os_lock_save_context(shw_queue_lock, lock_context);
+	ctx->next = user_list;
+	user_list = ctx;
+	os_unlock_restore_context(shw_queue_lock, lock_context);
+
+}
+
+/*!
+ * Remove a user context from the list of registered users.
+ *
+ * @param ctx  A pointer to a user context
+ *
+ * @return void
+ *
+ */
+inline static void SHW_REMOVE_USER(fsl_shw_uco_t * ctx)
+{
+	fsl_shw_uco_t *prev_ctx = user_list;
+	os_lock_context_t lock_context;
+
+	os_lock_save_context(shw_queue_lock, lock_context);
+
+	if (prev_ctx == ctx) {
+		/* Found at head, so just set new head */
+		user_list = ctx->next;
+	} else {
+		for (; (prev_ctx != NULL); prev_ctx = prev_ctx->next) {
+			if (prev_ctx->next == ctx) {
+				prev_ctx->next = ctx->next;
+				break;
+			}
+		}
+	}
+	os_unlock_restore_context(shw_queue_lock, lock_context);
+}
+
+static void shw_user_callback(fsl_shw_uco_t * uco);
+
+/* internal functions */
+static os_error_code shw_setup_user_driver_interaction(void);
+static void shw_cleanup(void);
+
+static os_error_code init_uco(fsl_shw_uco_t * user_ctx, void *user_mode_uco);
+static os_error_code get_capabilities(fsl_shw_uco_t * user_ctx,
+				      void *user_mode_pco_request);
+static os_error_code get_results(fsl_shw_uco_t * user_ctx,
+				 void *user_mode_result_req);
+static os_error_code get_random(fsl_shw_uco_t * user_ctx,
+				void *user_mode_get_random_req);
+static os_error_code add_entropy(fsl_shw_uco_t * user_ctx,
+				 void *user_mode_add_entropy_req);
+
+void* wire_user_memory(void* address, uint32_t length, void** page_ctx);
+void unwire_user_memory(void** page_ctx);
+os_error_code map_user_memory(struct vm_area_struct* vma, 
+                              uint32_t physical_addr, uint32_t size);
+os_error_code unmap_user_memory(uint32_t user_addr, uint32_t size);
+
+#if defined(LINUX_VERSION_CODE)
+
+MODULE_AUTHOR("Freescale Semiconductor");
+MODULE_DESCRIPTION("Device Driver for FSL SHW API");
+
+#endif				/* LINUX_VERSION_CODE */
+
+#endif				/* SHW_INTERNALS_H */
diff --git a/drivers/mxc/security/rng/rng_driver.c b/drivers/mxc/security/rng/rng_driver.c
new file mode 100644
index 000000000000..bad901926945
--- /dev/null
+++ b/drivers/mxc/security/rng/rng_driver.c
@@ -0,0 +1,1150 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*! @file rng_driver.c
+ *
+ * This is the driver code for the hardware Random Number Generator (RNG).
+ *
+ * It provides the following functions to callers:
+ *   fsl_shw_return_t fsl_shw_get_random(fsl_shw_uco_t* user_ctx,
+ *                                       uint32_t length,
+ *                                       uint8_t* data);
+ *
+ *   fsl_shw_return_t fsl_shw_add_entropy(fsl_shw_uco_t* user_ctx,
+ *                                        uint32_t length,
+ *                                        uint8_t* data);
+ *
+ * The life of the driver starts at boot (or module load) time, with a call by
+ * the kernel to #rng_init().  As part of initialization, a background task
+ * running #rng_entropy_task() will be created.
+ *
+ * The life of the driver ends when the kernel is shutting down (or the driver
+ * is being unloaded).  At this time, #rng_shutdown() is called.  No function
+ * will ever be called after that point.  In the case that the driver is
+ * reloaded, a new copy of the driver, with fresh global values, etc., is
+ * loaded, and there will be a new call to #rng_init().
+ *
+ * A call to fsl_shw_get_random() gets converted into a work entry which is
+ * queued and handed off to a background task for fulfilment.  This provides
+ * for a single thread of control for reading the RNG's FIFO register, which
+ * might otherwise underflow if not carefully managed.
+ *
+ * A call to fsl_shw_add_entropy() will cause the additional entropy to
+ * be passed directly into the hardware.
+ *
+ * In a debug configuration, it provides the following kernel functions:
+ * rng_return_t rng_read_register(uint32_t byte_offset, uint32_t* valuep);
+ * rng_return_t rng_write_register(uint32_t byte_offset, uint32_t value);
+ *  @ingroup RNG
+ */
+
+#include "portable_os.h"
+#include "fsl_shw.h"
+#include "rng_internals.h"
+
+#ifdef FSL_HAVE_SCC2
+#include <linux/mxc_scc2_driver.h>
+#else
+#include <linux/mxc_scc_driver.h>
+#endif
+
+#if defined(RNG_DEBUG) || defined(RNG_ENTROPY_DEBUG) ||                     \
+    defined(RNG_REGISTER_DEBUG)
+    
+#include <diagnostic.h>
+
+#else
+
+#define LOG_KDIAG_ARGS(fmt, ...)
+#define LOG_KDIAG(diag)
+
+#endif
+
+/* These are often handy */
+#ifndef FALSE
+/*! Non-true value for arguments, return values. */
+#define FALSE 0
+#endif
+#ifndef TRUE
+/*! True value for arguments, return values. */
+#define TRUE 1
+#endif
+
+/******************************************************************************
+ *
+ *  Global / Static Variables
+ *
+ *****************************************************************************/
+
+/*!
+ * This is type void* so that a) it cannot directly be dereferenced, and b)
+ * pointer arithmetic on it will function for the byte offsets in rng_rnga.h
+ * and rng_rngc.h
+ *
+ * rng_base is the location in the iomap where the RNG's registers
+ * (and memory) start.
+ *
+ * The referenced data is declared volatile so that the compiler will
+ * not make any assumptions about the value of registers in the RNG,
+ * and thus will always reload the register into CPU memory before
+ * using it (i.e. wherever it is referenced in the driver).
+ *
+ * This value should only be referenced by the #RNG_READ_REGISTER and
+ * #RNG_WRITE_REGISTER macros and their ilk.  All dereferences must be
+ * 32 bits wide.
+ */
+static volatile void *rng_base;
+
+/*!
+ * Flag to say whether interrupt handler has been registered for RNG
+ * interrupt */
+static int rng_irq_set = FALSE;
+
+/*!
+ * Size of the RNG's OUTPUT_FIFO, in words.  Retrieved with
+ * #RNG_GET_FIFO_SIZE() during driver initialization.
+ */
+static int rng_output_fifo_size;
+
+/*! Major number for device driver. */
+static int rng_major;
+
+/*! Registration handle for registering driver with OS. */
+os_driver_reg_t rng_reg_handle;
+
+/*!
+ * Internal flag to know whether RNG is in Failed state (and thus many
+ * registers are unavailable).  If the value ever goes to #RNG_STATUS_FAILED,
+ * it will never change.
+ */
+static volatile rng_status_t rng_availability = RNG_STATUS_INITIAL;
+
+/*!
+ * Global lock for the RNG driver.  Mainly used for entries on the RNG work
+ * queue.
+ */
+static os_lock_t rng_queue_lock = NULL;
+
+/*!
+ * Queue for the RNG task to process.
+ */
+static shw_queue_t rng_work_queue;
+
+/*!
+ * Flag to say whether task initialization succeeded.
+ */
+static unsigned task_started = FALSE;
+/*!
+ * Waiting queue for RNG SELF TESTING 
+ */
+static DECLARE_COMPLETION(rng_self_testing);
+static DECLARE_COMPLETION(rng_seed_done);
+/*!
+ *  Object for blocking-mode callers of RNG driver to sleep.
+ */
+OS_WAIT_OBJECT(rng_wait_queue);
+
+/******************************************************************************
+ *
+ *  Function Implementations - Externally Accessible
+ *
+ *****************************************************************************/
+
+/*****************************************************************************/
+/* fn rng_init()                                                             */
+/*****************************************************************************/
+/*!
+ * Initialize the driver.
+ *
+ * Set up the driver to have access to RNG device registers and verify that
+ * it appears to be a proper working device.
+ *
+ * Set up interrupt handling.  Assure RNG is ready to go and (possibly) set it
+ * into High Assurance mode.  Create a background task to run
+ * #rng_entropy_task().  Set up up a callback with the SCC driver should the
+ * security alarm go off.  Tell the kernel that the driver is here.
+ *
+ * This routine is called during kernel init or module load (insmod).
+ *
+ * The function will fail in one of two ways: Returning OK to the caller so
+ * that kernel startup / driver initialization completes, or returning an
+ * error.  In the success case, the function could set the rng_avaailability to
+ * RNG_STATUS_FAILED so that only minimal support (e.g. register peek / poke)
+ * is available in the driver.
+ *
+ * @return a call to os_dev_init_return()
+ */
+OS_DEV_INIT(rng_init)
+{
+	struct clk *clk;
+	os_error_code return_code = OS_ERROR_FAIL_S;
+	rng_availability = RNG_STATUS_CHECKING;
+
+#if !defined(FSL_HAVE_RNGA)
+	INIT_COMPLETION(rng_self_testing);
+	INIT_COMPLETION(rng_seed_done);
+#endif
+	rng_work_queue.head = NULL;
+	rng_work_queue.tail = NULL;
+
+	clk = clk_get(NULL, "rng_clk");
+
+	// Check that the clock was found
+	if (IS_ERR(clk)) {
+		LOG_KDIAG("RNG: Failed to find rng_clock.");
+		return_code = OS_ERROR_FAIL_S;
+		goto check_err;
+	}
+
+	clk_enable(clk);
+
+	os_printk(KERN_INFO "RNG Driver: Loading\n");
+
+	return_code = rng_map_RNG_memory();
+	if (return_code != OS_ERROR_OK_S) {
+		rng_availability = RNG_STATUS_UNIMPLEMENTED;
+		LOG_KDIAG_ARGS("RNG: Driver failed to map RNG registers. %d",
+			       return_code);
+		goto check_err;
+	}
+	LOG_KDIAG_ARGS("RNG Driver: rng_base is 0x%08x", (uint32_t) rng_base);
+	/*Check SCC keys are fused */
+	if (RNG_HAS_ERROR()) {
+		if (RNG_HAS_BAD_KEY()) {
+#ifdef RNG_DEBUG
+#if !defined(FSL_HAVE_RNGA)
+			LOG_KDIAG("ERROR: BAD KEYS SELECTED");
+			{
+				uint32_t rngc_status =
+				    RNG_READ_REGISTER(RNGC_STATUS);
+				uint32_t rngc_error =
+				    RNG_READ_REGISTER(RNGC_ERROR);
+				LOG_KDIAG_ARGS
+				    ("status register: %08x, error status: %08x",
+				     rngc_status, rngc_error);
+			}
+#endif
+#endif
+			rng_availability = RNG_STATUS_FAILED;
+			return_code = OS_ERROR_FAIL_S;
+			goto check_err;
+		}
+	}
+
+	/* Check RNG configuration and status */
+	return_code = rng_grab_config_values();
+	if (return_code != OS_ERROR_OK_S) {
+		rng_availability = RNG_STATUS_UNIMPLEMENTED;
+		goto check_err;
+	}
+
+	/* Masking All Interrupts */
+	/* They are unmasked later in rng_setup_interrupt_handling() */
+	RNG_MASK_ALL_INTERRUPTS();
+
+	RNG_WAKE();
+
+	/* Determine status of RNG */
+	if (RNG_OSCILLATOR_FAILED()) {
+		LOG_KDIAG("RNG Driver: RNG Oscillator is dead");
+		rng_availability = RNG_STATUS_FAILED;
+		goto check_err;
+	}
+
+	/* Oscillator not dead.  Setup interrupt code and start the RNG. */
+	if ((return_code = rng_setup_interrupt_handling()) == OS_ERROR_OK_S) {
+#if defined(FSL_HAVE_RNGA)
+		scc_return_t scc_code;
+#endif
+
+		RNG_GO();
+
+		/* Self Testing For RNG */
+		do {
+			RNG_CLEAR_ERR();
+
+			/* wait for Clearing Erring finished */
+			msleep(1);
+
+			RNG_UNMASK_ALL_INTERRUPTS();
+			RNG_SELF_TEST();
+#if !defined(FSL_HAVE_RNGA)
+			wait_for_completion(&rng_self_testing);
+#endif
+		} while (RNG_CHECK_SELF_ERR());
+
+		RNG_CLEAR_ALL_STATUS();
+		/* checking for RNG SEED done */
+		do {
+			RNG_CLEAR_ERR();
+			RNG_SEED_GEN();
+#if !defined(FSL_HAVE_RNGA)
+			wait_for_completion(&rng_seed_done);
+#endif
+		} while (RNG_CHECK_SEED_ERR());
+#ifndef RNG_NO_FORCE_HIGH_ASSURANCE
+		RNG_SET_HIGH_ASSURANCE();
+#endif
+		if (RNG_GET_HIGH_ASSURANCE()) {
+			LOG_KDIAG("RNG Driver: RNG is in High Assurance mode");
+		} else {
+#ifndef RNG_NO_FORCE_HIGH_ASSURANCE
+			LOG_KDIAG
+			    ("RNG Driver: RNG could not be put in High Assurance mode");
+			rng_availability = RNG_STATUS_FAILED;
+			goto check_err;
+#endif				/* RNG_NO_FORCE_HIGH_ASSURANCE */
+		}
+
+		/* Check that RNG is OK */
+		if (!RNG_WORKING()) {
+			LOG_KDIAG_ARGS
+			    ("RNG determined to be inoperable.  Status %08x",
+			     RNG_GET_STATUS());
+			/* Couldn't wake it up or other problem */
+			rng_availability = RNG_STATUS_FAILED;
+			goto check_err;
+		}
+
+		rng_queue_lock = os_lock_alloc_init();
+		if (rng_queue_lock == NULL) {
+			LOG_KDIAG("RNG: lock initialization failed");
+			rng_availability = RNG_STATUS_FAILED;
+			goto check_err;
+		}
+
+		return_code = os_create_task(rng_entropy_task);
+		if (return_code != OS_ERROR_OK_S) {
+			LOG_KDIAG("RNG: task initialization failed");
+			rng_availability = RNG_STATUS_FAILED;
+			goto check_err;
+		} else {
+			task_started = TRUE;
+		}
+#ifdef FSL_HAVE_RNGA
+		scc_code = scc_monitor_security_failure(rng_sec_failure);
+		if (scc_code != SCC_RET_OK) {
+			LOG_KDIAG_ARGS("Failed to register SCC callback: %d",
+				       scc_code);
+#ifndef RNG_NO_FORCE_HIGH_ASSURANCE
+			return_code = OS_ERROR_FAIL_S;
+			goto check_err;
+#endif
+		}
+#endif				/* FSL_HAVE_RNGA */
+		return_code = os_driver_init_registration(rng_reg_handle);
+		if (return_code != OS_ERROR_OK_S) {
+			goto check_err;
+		}
+		/* add power suspend here */
+		/* add power resume here */
+		return_code =
+		    os_driver_complete_registration(rng_reg_handle,
+						    rng_major, RNG_DRIVER_NAME);
+	}
+	/* RNG is working */
+
+      check_err:
+
+	/* If FIFO underflow or other error occurred during drain, this will fail,
+	 * as system will have been put into fail mode by SCC. */
+	if ((return_code == OS_ERROR_OK_S)
+	    && (rng_availability == RNG_STATUS_CHECKING)) {
+		RNG_PUT_RNG_TO_SLEEP();
+		rng_availability = RNG_STATUS_OK;	/* RNG & driver are ready */
+	} else if (return_code != OS_ERROR_OK_S) {
+		os_printk(KERN_ALERT "Driver initialization failed. %d",
+			  return_code);
+		rng_cleanup();
+	}
+
+	os_dev_init_return(return_code);
+
+}				/* rng_init */
+
+/*****************************************************************************/
+/* fn rng_shutdown()                                                         */
+/*****************************************************************************/
+/*!
+ * Prepare driver for exit.
+ *
+ * This is called during @c rmmod when the driver is unloading.
+ * Try to undo whatever was done during #rng_init(), to make the machine be
+ * in the same state, if possible.
+ *
+ * Calls rng_cleanup() to do all work, and then unmap device's register space.
+ */
+OS_DEV_SHUTDOWN(rng_shutdown)
+{
+	LOG_KDIAG("shutdown called");
+
+	rng_cleanup();
+
+	os_driver_remove_registration(rng_reg_handle);
+	if (rng_base != NULL) {
+		/* release the virtual memory map to the RNG */
+		os_unmap_device((void *)rng_base, RNG_ADDRESS_RANGE);
+		rng_base = NULL;
+	}
+
+	os_dev_shutdown_return(OS_ERROR_OK_S);
+}				/* rng_shutdown */
+
+/*****************************************************************************/
+/* fn rng_cleanup()                                                          */
+/*****************************************************************************/
+/*!
+ * Undo everything done by rng_init() and place driver in fail mode.
+ *
+ * Deregister from SCC, stop tasklet, shutdown the RNG.  Leave the register
+ * map in place in case other drivers call rng_read/write_register()
+ *
+ * @return void
+ */
+static void rng_cleanup(void)
+{
+	struct clk *clk;
+
+#ifdef FSL_HAVE_RNGA
+	scc_stop_monitoring_security_failure(rng_sec_failure);
+#endif
+
+	clk = clk_get(NULL, "rng_clk");
+	clk_disable(clk);
+	if (task_started) {
+		os_dev_stop_task(rng_entropy_task);
+	}
+
+	if (rng_base != NULL) {
+		/* mask off RNG interrupts */
+		RNG_MASK_ALL_INTERRUPTS();
+		RNG_SLEEP();
+
+		if (rng_irq_set) {
+			/* unmap the interrupts from the IRQ lines */
+			os_deregister_interrupt(INT_RNG);
+			rng_irq_set = FALSE;
+		}
+		LOG_KDIAG("Leaving rng driver status as failed");
+		rng_availability = RNG_STATUS_FAILED;
+	} else {
+		LOG_KDIAG("Leaving rng driver status as unimplemented");
+		rng_availability = RNG_STATUS_UNIMPLEMENTED;
+	}
+	LOG_KDIAG("Cleaned up");
+}				/* rng_cleanup */
+
+/*!
+ * Post-process routine for fsl_shw_get_random().
+ *
+ * This function will copy the random data generated by the background task
+ * into the user's buffer and then free the local buffer.
+ *
+ * @param gen_entry   The work request.
+ *
+ * @return 0 = meaning work completed, pass back result.
+ */
+static uint32_t finish_random(shw_queue_entry_t * gen_entry)
+{
+	rng_work_entry_t *work = (rng_work_entry_t *) gen_entry;
+
+	if (work->hdr.flags & FSL_UCO_USERMODE_USER) {
+		os_copy_to_user(work->data_user, work->data_local,
+				work->length);
+	} else {
+		memcpy(work->data_user, work->data_local, work->length);
+	}
+
+	os_free_memory(work->data_local);
+	work->data_local = NULL;
+
+	return 0;		/* means completed. */
+}
+
+/* REQ-FSLSHW-PINTFC-API-BASIC-RNG-002 */
+/*****************************************************************************/
+/* fn fsl_shw_get_random()                                                   */
+/*****************************************************************************/
+/*!
+ * Get random data.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param      length    The number of octets of @a data being requested.
+ * @param data      A pointer to a location of @a length octets to where
+ *                       random data will be returned.
+ *
+ * @return     FSL_RETURN_NO_RESOURCE_S  A return code of type #fsl_shw_return_t.
+ *             FSL_RETURN_OK_S 
+ */
+fsl_shw_return_t fsl_shw_get_random(fsl_shw_uco_t * user_ctx, uint32_t length,
+				    uint8_t * data)
+{
+	fsl_shw_return_t return_code = FSL_RETURN_NO_RESOURCE_S;
+	/* Boost up length to cover any 'missing' bytes at end of a word */
+	uint32_t *buf = os_alloc_memory(length + 3, 0);
+	volatile rng_work_entry_t *work = os_alloc_memory(sizeof(*work), 0);
+
+	if ((rng_availability != RNG_STATUS_OK) || (buf == NULL)
+	    || (work == NULL)) {
+		if (rng_availability != RNG_STATUS_OK) {
+			LOG_KDIAG_ARGS("rng not available: %d\n",
+				       rng_availability);
+		} else {
+			LOG_KDIAG_ARGS
+			    ("Resource allocation failure: %d or %d bytes",
+			     length, sizeof(*work));
+		}
+		/* Cannot perform function.  Clean up and clear out. */
+		if (buf != NULL) {
+			os_free_memory(buf);
+		}
+		if (work != NULL) {
+			os_free_memory((void *)work);
+		}
+	} else {
+		unsigned blocking = user_ctx->flags & FSL_UCO_BLOCKING_MODE;
+
+		work->hdr.user_ctx = user_ctx;
+		work->hdr.flags = user_ctx->flags;
+		work->hdr.callback = user_ctx->callback;
+		work->hdr.user_ref = user_ctx->user_ref;
+		work->hdr.postprocess = finish_random;
+		work->length = length;
+		work->data_local = buf;
+		work->data_user = data;
+
+		RNG_ADD_WORK_ENTRY((rng_work_entry_t *) work);
+
+		if (blocking) {
+			os_sleep(rng_wait_queue, work->completed != FALSE,
+				 FALSE);
+			finish_random((shw_queue_entry_t *) work);
+			return_code = work->hdr.code;
+			os_free_memory((void *)work);
+		} else {
+			return_code = FSL_RETURN_OK_S;
+		}
+	}
+
+	return return_code;
+}				/* fsl_shw_get_entropy */
+
+/*****************************************************************************/
+/* fn fsl_shw_add_entropy()                                                  */
+/*****************************************************************************/
+/*!
+ * Add entropy to random number generator.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param      length    Number of bytes at @a data.
+ * @param      data      Entropy to add to random number generator.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_add_entropy(fsl_shw_uco_t * user_ctx, uint32_t length,
+				     uint8_t * data)
+{
+	fsl_shw_return_t return_code = FSL_RETURN_NO_RESOURCE_S;
+#if defined(FSL_HAVE_RNGC)
+	/* No Entropy Register in RNGC */
+	return_code = FSL_RETURN_OK_S;
+#else
+	uint32_t *local_data = NULL;
+	if (rng_availability == RNG_STATUS_OK) {
+		/* make 32-bit aligned place to hold data */
+		local_data = os_alloc_memory(length + 3, 0);
+		if (local_data == NULL) {
+			return_code = FSL_RETURN_NO_RESOURCE_S;
+		} else {
+			memcpy(local_data, data, length);
+
+			/* Copy one word at a time to hardware */
+			while (TRUE) {
+				register uint32_t *ptr = local_data;
+
+				RNG_ADD_ENTROPY(*ptr++);
+				if (length <= 4) {
+					break;
+				}
+				length -= 4;
+			}
+			return_code = FSL_RETURN_OK_S;
+			os_free_memory(local_data);
+		}		/* else local_data not NULL */
+
+	}
+#endif
+	/* rng_availability is OK */
+	return return_code;
+}				/* fsl_shw_add_entropy */
+
+#ifdef RNG_REGISTER_PEEK_POKE
+/*****************************************************************************/
+/* fn rng_read_register()                                                   */
+/*****************************************************************************/
+/*
+ * Debug routines to allow reading of RNG registers.
+ *
+ * This routine is only for accesses by other than this driver.
+ *
+ * @param register_offset  The byte offset of the register to be read.
+ * @param value            Pointer to store the value of the register.
+ *
+ * @return RNG_RET_OK or an error return.
+ */
+rng_return_t rng_read_register(uint32_t register_offset, uint32_t * value)
+{
+	rng_return_t return_code = RNG_RET_FAIL;
+
+	if ((rng_availability == RNG_STATUS_OK)
+	    || (rng_availability == RNG_STATUS_FAILED)) {
+		if ((rng_check_register_offset(register_offset)
+		     && rng_check_register_accessible(register_offset,
+						      RNG_CHECK_READ))) {
+			/* The guards let the check through */
+			*value = RNG_READ_REGISTER(register_offset);
+			return_code = RNG_RET_OK;
+		}
+	}
+
+	return return_code;
+}				/* rng_read_register */
+
+/*****************************************************************************/
+/* fn rng_write_register()                                                  */
+/*****************************************************************************/
+/*
+ * Debug routines to allow writing of RNG registers.
+ *
+ * This routine is only for accesses by other than this driver.
+ *
+ * @param register_offset  The byte offset of the register to be written.
+ * @param value            Value to store in the register.
+ *
+ * @return RNG_RET_OK or an error return.
+ */
+rng_return_t rng_write_register(uint32_t register_offset, uint32_t value)
+{
+	rng_return_t return_code = RNG_RET_FAIL;
+
+	if ((rng_availability == RNG_STATUS_OK)
+	    || (rng_availability == RNG_STATUS_FAILED)) {
+		if ((rng_check_register_offset(register_offset)
+		     && rng_check_register_accessible(register_offset,
+						      RNG_CHECK_WRITE))) {
+			RNG_WRITE_REGISTER(register_offset, value);
+			return_code = RNG_RET_OK;
+		}
+	}
+
+	return return_code;
+}				/* rng_write_register */
+#endif				/*  RNG_REGISTER_PEEK_POKE */
+
+/******************************************************************************
+ *
+ *  Function Implementations - Internal
+ *
+ *****************************************************************************/
+
+#ifdef RNG_REGISTER_PEEK_POKE
+/*****************************************************************************/
+/* fn check_register_offset()                                                */
+/*****************************************************************************/
+/*!
+ * Verify that the @c offset is appropriate for the RNG's register set.
+ *
+ * @param[in]  offset  The (byte) offset within the RNG block
+ *                     of the register to be accessed.  See
+ *                     RNG(A, C) register definitions for meanings.
+ *
+ * This routine is only for checking accesses by other than this driver.
+ *
+ * @return  0 if register offset out of bounds, 1 if ok to use
+ */
+inline int rng_check_register_offset(uint32_t offset)
+{
+	int return_code = FALSE;	/* invalid */
+
+	/* Make sure offset isn't too high and also that it is aligned to
+	 * aa 32-bit offset (multiple of four).
+	 */
+	if ((offset < RNG_ADDRESS_RANGE) && (offset % sizeof(uint32_t) == 0)) {
+		return_code = TRUE;	/* OK */
+	} else {
+		pr_debug("RNG: Denied access to offset %8x\n", offset);
+	}
+
+	return return_code;
+
+}				/* rng_check_register */
+
+/*****************************************************************************/
+/* fn check_register_accessible()                                            */
+/*****************************************************************************/
+/*!
+ * Make sure that register access is legal.
+ *
+ * Verify that, if in secure mode, only safe registers are used.
+ * For any register access, make sure that read-only registers are not written
+ * and that write-only registers are not read.  This check also disallows any
+ * access to the RNG's Output FIFO, to prevent other drivers from draining the
+ * FIFO and causing an underflow condition.
+ *
+ * This routine is only for checking accesses by other than this driver.
+ *
+ * @param  offset   The (byte) offset within the RNG block
+ *                      of the register to be accessed.  See
+ *                      @ref rngregs for meanings.
+ * @param access_write  0 for read, anything else for write
+ *
+ * @return 0 if invalid, 1 if OK.
+ */
+static int rng_check_register_accessible(uint32_t offset, int access_write)
+{
+	int return_code = FALSE;	/* invalid */
+	uint32_t secure = RNG_GET_HIGH_ASSURANCE();
+
+	/* First check for RNG in Secure Mode -- most registers inaccessible.
+	 * Also disallowing access to RNG_OUTPUT_FIFO except by the driver.
+	 */
+	if (!
+#ifdef FSL_HAVE_RNGA
+	    (secure &&
+	     ((offset == RNGA_OUTPUT_FIFO) ||
+	      (offset == RNGA_MODE) ||
+	      (offset == RNGA_VERIFICATION_CONTROL) ||
+	      (offset == RNGA_OSCILLATOR_CONTROL_COUNTER) ||
+	      (offset == RNGA_OSCILLATOR1_COUNTER) ||
+	      (offset == RNGA_OSCILLATOR2_COUNTER) ||
+	      (offset == RNGA_OSCILLATOR_COUNTER_STATUS)))
+#else				/* RNGB or RNGC */
+	    (secure &&
+	     ((offset == RNGC_FIFO) ||
+	      (offset == RNGC_VERIFICATION_CONTROL) ||
+	      (offset == RNGC_OSC_COUNTER_CONTROL) ||
+	      (offset == RNGC_OSC_COUNTER) ||
+	      (offset == RNGC_OSC_COUNTER_STATUS)))
+#endif
+	    ) {
+
+		/*  Passed that test.  Either not in high assurance, and/or are
+		   checking register that is always available.  Now check
+		   R/W permissions. */
+		if (access_write == RNG_CHECK_READ) {	/* read request */
+			/* Only the entropy register is write-only */
+#ifdef FSL_HAVE_RNGC
+			/* No registers are write-only */
+			return_code = TRUE;
+#else				/* else RNGA or RNGB */
+#ifdef FSL_HAVE_RNGA
+			if (1) {
+#else
+			if (!(offset == RNGB_ENTROPY)) {
+#endif
+				return_code = TRUE;	/* Let all others be read */
+			} else {
+				pr_debug
+				    ("RNG: Offset %04x denied read access\n",
+				     offset);
+			}
+#endif				/* RNGA or RNGB */
+		} /* read */
+		else {		/* access_write means write */
+			/* Check against list of non-writable registers */
+			if (!
+#ifdef FSL_HAVE_RNGA
+			    ((offset == RNGA_STATUS) ||
+			     (offset == RNGA_OUTPUT_FIFO) ||
+			     (offset == RNGA_OSCILLATOR1_COUNTER) ||
+			     (offset == RNGA_OSCILLATOR2_COUNTER) ||
+			     (offset == RNGA_OSCILLATOR_COUNTER_STATUS))
+#else				/* FSL_HAVE_RNGB or FSL_HAVE_RNGC */
+			    ((offset == RNGC_STATUS) ||
+			     (offset == RNGC_FIFO) ||
+			     (offset == RNGC_OSC_COUNTER) ||
+			     (offset == RNGC_OSC_COUNTER_STATUS))
+#endif
+			    ) {
+				return_code = TRUE;	/* can be written */
+			} else {
+				LOG_KDIAG_ARGS
+				    ("Offset %04x denied write access", offset);
+			}
+		}		/* write */
+	} /* not high assurance and inaccessible register... */
+	else {
+		LOG_KDIAG_ARGS("Offset %04x denied high-assurance access",
+			       offset);
+	}
+
+	return return_code;
+}				/* rng_check_register_accessible */
+#endif				/*  RNG_REGISTER_PEEK_POKE */
+
+/*****************************************************************************/
+/* fn rng_irq()                                                             */
+/*****************************************************************************/
+/*!
+ * This is the interrupt handler for the RNG.  It is only ever invoked if the
+ * RNG detects a FIFO Underflow error.
+ *
+ * If the error is a Security Violation, this routine will
+ * set the #rng_availability to #RNG_STATUS_FAILED, as the entropy pool may
+ * have been corrupted.  The RNG will also be placed into low power mode.  The
+ * SCC will have noticed the problem as well.
+ *
+ * The other possibility, if the RNG is not in High Assurance mode, would be
+ * simply a FIFO Underflow.  No special action, other than to
+ * clear the interrupt, is taken.
+ */
+OS_DEV_ISR(rng_irq)
+{
+	int handled = FALSE;	/* assume interrupt isn't from RNG */
+
+	LOG_KDIAG("rng irq!");
+
+	if (RNG_SEED_DONE()) {
+		complete(&rng_seed_done);
+		RNG_CLEAR_ALL_STATUS();
+		handled = TRUE;
+	}
+
+	if (RNG_SELF_TEST_DONE()) {
+		complete(&rng_self_testing);
+		RNG_CLEAR_ALL_STATUS();
+		handled = TRUE;
+	}
+	/* Look to see whether RNG needs attention */
+	if (RNG_HAS_ERROR()) {
+		if (RNG_GET_HIGH_ASSURANCE()) {
+			RNG_SLEEP();
+			rng_availability = RNG_STATUS_FAILED;
+			RNG_MASK_ALL_INTERRUPTS();
+		}
+		handled = TRUE;
+		/* Clear the interrupt */
+		RNG_CLEAR_ALL_STATUS();
+
+	}
+	os_dev_isr_return(handled);
+}				/* rng_irq */
+
+/*****************************************************************************/
+/* fn map_RNG_memory()                                                      */
+/*****************************************************************************/
+/*!
+ * Place the RNG's memory into kernel virtual space.
+ *
+ * @return OS_ERROR_OK_S on success, os_error_code on failure
+ */
+static os_error_code rng_map_RNG_memory(void)
+{
+	os_error_code error_code = OS_ERROR_FAIL_S;
+
+	rng_base = os_map_device(RNG_BASE_ADDR, RNG_ADDRESS_RANGE);
+	if (rng_base == NULL) {
+		/* failure ! */
+		LOG_KDIAG("RNG Driver: ioremap failed.");
+	} else {
+		error_code = OS_ERROR_OK_S;
+	}
+
+	return error_code;
+}				/* rng_map_RNG_memory */
+
+/*****************************************************************************/
+/* fn rng_setup_interrupt_handling()                                        */
+/*****************************************************************************/
+/*!
+ * Register #rng_irq() as the interrupt handler for #INT_RNG.
+ *
+ * @return OS_ERROR_OK_S on success, os_error_code on failure
+ */
+static os_error_code rng_setup_interrupt_handling(void)
+{
+	os_error_code error_code;
+
+	/*
+	 * Install interrupt service routine for the RNG. Ignore the
+	 * assigned IRQ number.
+	 */
+	error_code = os_register_interrupt(RNG_DRIVER_NAME, INT_RNG,
+					   OS_DEV_ISR_REF(rng_irq));
+	if (error_code != OS_ERROR_OK_S) {
+		LOG_KDIAG("RNG Driver: Error installing Interrupt Handler");
+	} else {
+		rng_irq_set = TRUE;
+		RNG_UNMASK_ALL_INTERRUPTS();
+	}
+
+	return error_code;
+}				/* rng_setup_interrupt_handling */
+
+/*****************************************************************************/
+/* fn rng_grab_config_values()                                               */
+/*****************************************************************************/
+/*!
+ * Read configuration information from the RNG.
+ *
+ * Sets #rng_output_fifo_size.
+ *
+ * @return  A error code indicating whether the part is the expected one.
+ */
+static os_error_code rng_grab_config_values(void)
+{
+	enum rng_type type;
+	os_error_code ret = OS_ERROR_FAIL_S;
+
+	/* Go for type, versions... */
+	type = RNG_GET_RNG_TYPE();
+
+	/* Make sure type is the one this code has been compiled for. */
+	if (RNG_VERIFY_TYPE(type)) {
+		rng_output_fifo_size = RNG_GET_FIFO_SIZE();
+		if (rng_output_fifo_size != 0) {
+			ret = OS_ERROR_OK_S;
+		}
+	}
+	if (ret != OS_ERROR_OK_S) {
+		LOG_KDIAG_ARGS
+		    ("Unknown or unexpected RNG type %d (FIFO size %d)."
+		     "  Failing driver initialization", type,
+		     rng_output_fifo_size);
+	}
+
+	return ret;
+}
+
+ /* rng_grab_config_values */
+
+/*****************************************************************************/
+/* fn rng_drain_fifo()                                                       */
+/*****************************************************************************/
+/*!
+ * This function copies words from the RNG FIFO into the caller's buffer.
+ *
+ *
+ * @param random_p    Location to copy random data
+ * @param count_words Number of words to copy
+ *
+ * @return An error code.
+ */
+static fsl_shw_return_t rng_drain_fifo(uint32_t * random_p, int count_words)
+{
+
+	int words_in_rng;	/* Number of words available now in RNG */
+	fsl_shw_return_t code = FSL_RETURN_ERROR_S;
+	int sequential_count = 0;	/* times through big while w/empty FIFO */
+	int fifo_empty_count = 0;	/* number of times FIFO was empty */
+	int max_sequential = 0;	/* max times 0 seen in a row */
+#if !defined(FSL_HAVE_RNGA)
+	int count_for_reseed = 0;
+	INIT_COMPLETION(rng_seed_done);
+#endif
+#if !defined(FSL_HAVE_RNGA)
+	if (RNG_RESEED()) {
+		do {
+			LOG_KDIAG("Reseeding RNG");
+
+			RNG_CLEAR_ERR();
+			RNG_SEED_GEN();
+			wait_for_completion(&rng_seed_done);
+			if (count_for_reseed == 3) {
+				os_printk(KERN_ALERT
+					  "Device was not able to enter RESEED Mode\n");
+				code = FSL_RETURN_INTERNAL_ERROR_S;
+			}
+			count_for_reseed++;
+		} while (RNG_CHECK_SEED_ERR());
+	}
+#endif
+	/* Copy all of them in.  Stop if pool fills. */
+	while ((rng_availability == RNG_STATUS_OK) && (count_words > 0)) {
+		/* Ask RNG how many words currently in FIFO */
+		words_in_rng = RNG_GET_WORDS_IN_FIFO();
+		if (words_in_rng == 0) {
+			++sequential_count;
+			fifo_empty_count++;
+			if (sequential_count > max_sequential) {
+				max_sequential = sequential_count;
+			}
+			if (sequential_count >= RNG_MAX_TRIES) {
+				LOG_KDIAG_ARGS("FIFO staying empty (%d)",
+					       words_in_rng);
+				code = FSL_RETURN_NO_RESOURCE_S;
+				break;
+			}
+		} else {
+			/* Found at least one word */
+			sequential_count = 0;
+			/* Now adjust: words_in_rng = MAX(count_words, words_in_rng) */
+			words_in_rng = (count_words < words_in_rng)
+			    ? count_words : words_in_rng;
+		}		/* else found words */
+
+#ifdef RNG_FORCE_FIFO_UNDERFLOW
+		/*
+		 * For unit test, force occasional extraction of more words than
+		 * available.  This should cause FIFO Underflow, and IRQ invocation.
+		 */
+		words_in_rng = count_words;
+#endif
+
+		/* Copy out all available & neeeded data */
+		while (words_in_rng-- > 0) {
+			*random_p++ = RNG_READ_FIFO();
+			count_words--;
+		}
+	}			/* while words still needed */
+
+	if (count_words == 0) {
+		code = FSL_RETURN_OK_S;
+	}
+	if (fifo_empty_count != 0) {
+		LOG_KDIAG_ARGS("FIFO empty %d times, max loop count %d",
+			       fifo_empty_count, max_sequential);
+	}
+
+	return code;
+}				/* rng_drain_fifo */
+
+/*****************************************************************************/
+/* fn rng_entropy_task()                                                     */
+/*****************************************************************************/
+/*!
+ * This is the background task of the driver.  It is scheduled by
+ * RNG_ADD_WORK_ENTRY().
+ *
+ * This will process each entry on the #rng_work_queue.  Blocking requests will
+ * cause sleepers to be awoken.  Non-blocking requests will be placed on the
+ * results queue, and if appropriate, the callback function will be invoked.
+ */
+OS_DEV_TASK(rng_entropy_task)
+{
+	rng_work_entry_t *work;
+
+	os_dev_task_begin();
+
+#ifdef RNG_ENTROPY_DEBUG
+	LOG_KDIAG("entropy task starting");
+#endif
+
+	while ((work = RNG_GET_WORK_ENTRY()) != NULL) {
+#ifdef RNG_ENTROPY_DEBUG
+		LOG_KDIAG_ARGS("found %d bytes of work at %p (%p)",
+			       work->length, work, work->data_local);
+#endif
+		work->hdr.code = rng_drain_fifo(work->data_local,
+						BYTES_TO_WORDS(work->length));
+		work->completed = TRUE;
+
+		if (work->hdr.flags & FSL_UCO_BLOCKING_MODE) {
+#ifdef RNG_ENTROPY_DEBUG
+			LOG_KDIAG("Waking queued processes");
+#endif
+			os_wake_sleepers(rng_wait_queue);
+		} else {
+			os_lock_context_t lock_context;
+
+			os_lock_save_context(rng_queue_lock, lock_context);
+			RNG_ADD_QUEUE_ENTRY(&work->hdr.user_ctx->result_pool,
+					    work);
+			os_unlock_restore_context(rng_queue_lock, lock_context);
+
+			if (work->hdr.flags & FSL_UCO_CALLBACK_MODE) {
+				if (work->hdr.callback != NULL) {
+					work->hdr.callback(work->hdr.user_ctx);
+				} else {
+#ifdef RNG_ENTROPY_DEBUG
+					LOG_KDIAG_ARGS
+					    ("Callback ptr for %p is NULL",
+					     work);
+#endif
+				}
+			}
+		}
+	}			/* while */
+
+#ifdef RNG_ENTROPY_DEBUG
+	LOG_KDIAG("entropy task ending");
+#endif
+
+	os_dev_task_return(OS_ERROR_OK_S);
+}				/* rng_entropy_task */
+
+#ifdef FSL_HAVE_RNGA
+/*****************************************************************************/
+/* fn rng_sec_failure()                                                      */
+/*****************************************************************************/
+/*!
+ * Function to handle "Security Alarm" indication from SCC.
+ *
+ * This function is registered with the Security Monitor ans the callback
+ * function for the RNG driver.  Upon alarm, it will shut down the driver so
+ * that no more random data can be retrieved.
+ *
+ * @return void
+ */
+static void rng_sec_failure(void)
+{
+	os_printk(KERN_ALERT "RNG Driver: Security Failure Alarm received.\n");
+
+	rng_cleanup();
+
+	return;
+}
+#endif
+
+#ifdef RNG_REGISTER_DEBUG
+/*****************************************************************************/
+/* fn dbg_rng_read_register()                                                */
+/*****************************************************************************/
+/*!
+ * Noisily read a 32-bit value to an RNG register.
+ * @param offset        The address of the register to read.
+ *
+ * @return  The register value
+ * */
+static uint32_t dbg_rng_read_register(uint32_t offset)
+{
+	uint32_t value;
+
+	value = os_read32(rng_base + offset);
+#ifndef RNG_ENTROPY_DEBUG
+	if (offset != RNG_OUTPUT_FIFO) {
+#endif
+		pr_debug("RNG RD: 0x%4x : 0x%08x\n", offset, value);
+#ifndef RNG_ENTROPY_DEBUG
+	}
+#endif
+	return value;
+}
+
+/*****************************************************************************/
+/* fn dbg_rng_write_register()                                               */
+/*****************************************************************************/
+/*!
+ * Noisily write a 32-bit value to an RNG register.
+ * @param offset        The address of the register to written.
+ *
+ * @param value         The new register value
+ */
+static void dbg_rng_write_register(uint32_t offset, uint32_t value)
+{
+	LOG_KDIAG_ARGS("WR: 0x%4x : 0x%08x", offset, value);
+	os_write32(value, rng_base + offset);
+	return;
+}
+
+#endif				/* RNG_REGISTER_DEBUG */
diff --git a/drivers/mxc/security/rng/shw_driver.c b/drivers/mxc/security/rng/shw_driver.c
new file mode 100644
index 000000000000..81212e55919e
--- /dev/null
+++ b/drivers/mxc/security/rng/shw_driver.c
@@ -0,0 +1,2335 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*! @file shw_driver.c
+ *
+ * This is the user-mode driver code for the FSL Security Hardware (SHW) API.
+ * as well as the 'common' FSL SHW API code for kernel API users.
+ *
+ * Its interaction with the Linux kernel is from calls to shw_init() when the
+ * driver is loaded, and shw_shutdown() should the driver be unloaded.
+ *
+ * The User API (driver interface) is handled by the following functions:
+ * @li shw_open()    - handles open() system call on FSL SHW device
+ * @li shw_release() - handles close() system call on FSL SHW device
+ * @li shw_ioctl()    - handles ioctl() system call on FSL SHW device
+ *
+ * The driver also provides the following functions for kernel users of the FSL
+ * SHW API:
+ * @li fsl_shw_register_user()
+ * @li fsl_shw_deregister_user()
+ * @li fsl_shw_get_capabilities()
+ * @li fsl_shw_get_results()
+ *
+ * All other functions are internal to the driver.
+ *
+ * The life of the driver starts at boot (or module load) time, with a call by
+ * the kernel to shw_init().
+ *
+ * The life of the driver ends when the kernel is shutting down (or the driver
+ * is being unloaded).  At this time, shw_shutdown() is called.  No function
+ * will ever be called after that point.
+ *
+ * In the case that the driver is reloaded, a new copy of the driver, with
+ * fresh global values, etc., is loaded, and there will be a new call to
+ * shw_init().
+ *
+ * In user mode, the user's fsl_shw_register_user() call causes an open() event
+ * on the driver followed by a ioctl() with the registration information.  Any
+ * subsequent API calls by the user are handled through the ioctl() function
+ * and shuffled off to the appropriate routine (or driver) for service.  The
+ * fsl_shw_deregister_user() call by the user results in a close() function
+ * call on the driver.
+ *
+ * In kernel mode, the driver provides the functions fsl_shw_register_user(),
+ * fsl_shw_deregister_user(), fsl_shw_get_capabilities(), and
+ * fsl_shw_get_results().  Other parts of the API are provided by other
+ * drivers, if available, to support the cryptographic functions.
+ */
+
+#include "portable_os.h"
+#include "fsl_shw.h"
+#include "fsl_shw_keystore.h"
+
+#include "shw_internals.h"
+
+#ifdef FSL_HAVE_SCC2
+#include <linux/mxc_scc2_driver.h>
+#else
+#include <linux/mxc_scc_driver.h>
+#endif
+
+#ifdef SHW_DEBUG
+#include <diagnostic.h>
+#endif
+
+/******************************************************************************
+ *
+ *  Function Declarations
+ *
+ *****************************************************************************/
+
+/* kernel interface functions */
+OS_DEV_INIT_DCL(shw_init);
+OS_DEV_SHUTDOWN_DCL(shw_shutdown);
+OS_DEV_IOCTL_DCL(shw_ioctl);
+OS_DEV_MMAP_DCL(shw_mmap);
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_smalloc);
+EXPORT_SYMBOL(fsl_shw_sfree);
+EXPORT_SYMBOL(fsl_shw_sstatus);
+EXPORT_SYMBOL(fsl_shw_diminish_perms);
+EXPORT_SYMBOL(do_scc_encrypt_region);
+EXPORT_SYMBOL(do_scc_decrypt_region);
+
+EXPORT_SYMBOL(do_system_keystore_slot_alloc);
+EXPORT_SYMBOL(do_system_keystore_slot_dealloc);
+EXPORT_SYMBOL(do_system_keystore_slot_load);
+EXPORT_SYMBOL(do_system_keystore_slot_encrypt);
+EXPORT_SYMBOL(do_system_keystore_slot_decrypt);
+#endif
+
+static os_error_code
+shw_handle_scc_sfree(fsl_shw_uco_t * user_ctx, uint32_t info);
+
+static os_error_code
+shw_handle_scc_sstatus(fsl_shw_uco_t * user_ctx, uint32_t info);
+
+static os_error_code
+shw_handle_scc_drop_perms(fsl_shw_uco_t * user_ctx, uint32_t info);
+
+static os_error_code
+shw_handle_scc_encrypt(fsl_shw_uco_t * user_ctx, uint32_t info);
+
+static os_error_code
+shw_handle_scc_decrypt(fsl_shw_uco_t * user_ctx, uint32_t info);
+
+#ifdef FSL_HAVE_SCC2
+static fsl_shw_return_t register_user_partition(fsl_shw_uco_t * user_ctx,
+						uint32_t user_base,
+						void *kernel_base);
+static fsl_shw_return_t deregister_user_partition(fsl_shw_uco_t * user_ctx,
+						  uint32_t user_base);
+void *lookup_user_partition(fsl_shw_uco_t * user_ctx, uint32_t user_base);
+
+#endif				/* FSL_HAVE_SCC2 */
+
+/******************************************************************************
+ *
+ *  Global / Static Variables
+ *
+ *****************************************************************************/
+
+/*!
+ *  Major node (user/device interaction value) of this driver.
+ */
+static int shw_major_node = SHW_MAJOR_NODE;
+
+/*!
+ *  Flag to know whether the driver has been associated with its user device
+ *  node (e.g. /dev/shw).
+ */
+static int shw_device_registered = 0;
+
+/*!
+ * OS-dependent handle used for registering user interface of a driver.
+ */
+static os_driver_reg_t reg_handle;
+
+/*!
+ * Linked List of registered users of the API
+ */
+fsl_shw_uco_t *user_list;
+
+/*!
+ * This is the lock for all user request pools.  H/W component drivers may also
+ * use it for their own work queues.
+ */
+os_lock_t shw_queue_lock = NULL;
+
+/* This is the system keystore object */
+fsl_shw_kso_t system_keystore;
+
+#ifndef FSL_HAVE_SAHARA
+/*! Empty list of supported symmetric algorithms. */
+static fsl_shw_key_alg_t pf_syms[] = {
+};
+
+/*! Empty list of supported symmetric modes. */
+static fsl_shw_sym_mode_t pf_modes[] = {
+};
+
+/*! Empty list of supported hash algorithms. */
+static fsl_shw_hash_alg_t pf_hashes[] = {
+};
+#endif				/* no Sahara */
+
+/*! This matches SHW capabilities... */
+static fsl_shw_pco_t cap = {
+	1, 3,			/* api version number - major & minor */
+	2, 3,			/* driver version number - major & minor */
+	sizeof(pf_syms) / sizeof(fsl_shw_key_alg_t),	/* key alg count */
+	pf_syms,		/* key alg list ptr */
+	sizeof(pf_modes) / sizeof(fsl_shw_sym_mode_t),	/* sym mode count */
+	pf_modes,		/* modes list ptr */
+	sizeof(pf_hashes) / sizeof(fsl_shw_hash_alg_t),	/* hash alg count */
+	pf_hashes,		/* hash list ptr */
+	/*
+	 * The following table must be set to handle all values of key algorithm
+	 * and sym mode, and be in the correct order..
+	 */
+	{			/* Stream, ECB, CBC, CTR */
+	 {0, 0, 0, 0}
+	 ,			/* HMAC */
+	 {0, 0, 0, 0}
+	 ,			/* AES  */
+	 {0, 0, 0, 0}
+	 ,			/* DES */
+#ifdef FSL_HAVE_DRYICE
+	 {0, 1, 1, 0}
+	 ,			/* 3DES - ECB and CBC */
+#else
+	 {0, 0, 0, 0}
+	 ,			/* 3DES */
+#endif
+	 {0, 0, 0, 0}		/* ARC4 */
+	 }
+	,
+	0, 0,			/* SCC driver version */
+	0, 0, 0,		/* SCC version/capabilities */
+	{{0, 0}
+	 }
+	,			/* (filled in during OS_INIT) */
+};
+
+/* These are often handy */
+#ifndef FALSE
+/*! Not true.  Guaranteed to be zero. */
+#define FALSE 0
+#endif
+#ifndef TRUE
+/*! True.  Guaranteed to be non-zero. */
+#define TRUE 1
+#endif
+
+/******************************************************************************
+ *
+ *  Function Implementations - Externally Accessible
+ *
+ *****************************************************************************/
+
+/*****************************************************************************/
+/* fn shw_init()                                                             */
+/*****************************************************************************/
+/*!
+ * Initialize the driver.
+ *
+ * This routine is called during kernel init or module load (insmod).
+ *
+ * @return OS_ERROR_OK_S on success, errno on failure
+ */
+OS_DEV_INIT(shw_init)
+{
+	os_error_code error_code = OS_ERROR_NO_MEMORY_S;	/* assume failure */
+	scc_config_t *shw_capabilities;
+
+#ifdef SHW_DEBUG
+	LOG_KDIAG("SHW Driver: Loading");
+#endif
+
+	user_list = NULL;
+	shw_queue_lock = os_lock_alloc_init();
+
+	if (shw_queue_lock != NULL) {
+		error_code = shw_setup_user_driver_interaction();
+		if (error_code != OS_ERROR_OK_S) {
+#ifdef SHW_DEBUG
+			LOG_KDIAG_ARGS
+			    ("SHW Driver: Failed to setup user i/f: %d",
+			     error_code);
+#endif
+		}
+	}
+
+	/* queue_lock not NULL */
+	/* Fill in the SCC portion of the capabilities object */
+	shw_capabilities = scc_get_configuration();
+	cap.scc_driver_major = shw_capabilities->driver_major_version;
+	cap.scc_driver_minor = shw_capabilities->driver_minor_version;
+	cap.scm_version = shw_capabilities->scm_version;
+	cap.smn_version = shw_capabilities->smn_version;
+	cap.block_size_bytes = shw_capabilities->block_size_bytes;
+
+#ifdef FSL_HAVE_SCC
+	cap.u.scc_info.black_ram_size_blocks =
+	    shw_capabilities->black_ram_size_blocks;
+	cap.u.scc_info.red_ram_size_blocks =
+	    shw_capabilities->red_ram_size_blocks;
+#elif defined(FSL_HAVE_SCC2)
+	cap.u.scc2_info.partition_size_bytes =
+	    shw_capabilities->partition_size_bytes;
+	cap.u.scc2_info.partition_count = shw_capabilities->partition_count;
+#endif
+
+#if defined(FSL_HAVE_SCC2) || defined(FSL_HAVE_DRYICE)
+	if (error_code == OS_ERROR_OK_S) {
+		/* set up the system keystore, using the default keystore handler */
+		fsl_shw_init_keystore_default(&system_keystore);
+
+		if (fsl_shw_establish_keystore(NULL, &system_keystore)
+		    == FSL_RETURN_OK_S) {
+			error_code = OS_ERROR_OK_S;
+		} else {
+			error_code = OS_ERROR_FAIL_S;
+		}
+
+		if (error_code != OS_ERROR_OK_S) {
+#ifdef SHW_DEBUG
+			LOG_KDIAG_ARGS
+			    ("Registering the system keystore failed with error"
+			     " code: %d\n", error_code);
+#endif
+		}
+	}
+#endif				/* FSL_HAVE_SCC2 */
+
+	if (error_code != OS_ERROR_OK_S) {
+#ifdef SHW_DEBUG
+		LOG_KDIAG_ARGS("SHW: Driver initialization failed. %d",
+			       error_code);
+#endif
+		shw_cleanup();
+	} else {
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: Driver initialization complete.");
+#endif
+	}
+
+	os_dev_init_return(error_code);
+}				/* shw_init */
+
+/*****************************************************************************/
+/* fn shw_shutdown()                                                         */
+/*****************************************************************************/
+/*!
+ * Prepare driver for exit.
+ *
+ * This is called during @c rmmod when the driver is unloading or when the
+ * kernel is shutting down.
+ *
+ * Calls shw_cleanup() to do all work to undo anything that happened during
+ * initialization or while driver was running.
+ */
+OS_DEV_SHUTDOWN(shw_shutdown)
+{
+
+#ifdef SHW_DEBUG
+	LOG_KDIAG("SHW: shutdown called");
+#endif
+	shw_cleanup();
+
+	os_dev_shutdown_return(OS_ERROR_OK_S);
+}				/* shw_shutdown */
+
+/*****************************************************************************/
+/* fn shw_cleanup()                                                          */
+/*****************************************************************************/
+/*!
+ * Prepare driver for shutdown.
+ *
+ * Remove the driver registration.
+ *
+ */
+static void shw_cleanup(void)
+{
+	if (shw_device_registered) {
+
+		/* Turn off the all association with OS */
+		os_driver_remove_registration(reg_handle);
+		shw_device_registered = 0;
+	}
+
+	if (shw_queue_lock != NULL) {
+		os_lock_deallocate(shw_queue_lock);
+	}
+#ifdef SHW_DEBUG
+	LOG_KDIAG("SHW Driver: Cleaned up");
+#endif
+}				/* shw_cleanup */
+
+/*****************************************************************************/
+/* fn shw_open()                                                             */
+/*****************************************************************************/
+/*!
+ * Handle @c open() call from user.
+ *
+ * @return OS_ERROR_OK_S on success (always!)
+ */
+OS_DEV_OPEN(shw_open)
+{
+	os_error_code status = OS_ERROR_OK_S;
+
+	os_dev_set_user_private(NULL);	/* Make sure */
+
+	os_dev_open_return(status);
+}				/* shw_open */
+
+/*****************************************************************************/
+/* fn shw_ioctl()                                                            */
+/*****************************************************************************/
+/*!
+ * Process an ioctl() request from user-mode API.
+ *
+ * This code determines which of the API requests the user has made and then
+ * sends the request off to the appropriate function.
+ *
+ * @return ioctl_return()
+ */
+OS_DEV_IOCTL(shw_ioctl)
+{
+	os_error_code code = OS_ERROR_FAIL_S;
+
+	fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+#ifdef SHW_DEBUG
+	LOG_KDIAG_ARGS("SHW: IOCTL %d received", os_dev_get_ioctl_op());
+#endif
+	switch (os_dev_get_ioctl_op()) {
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_REGISTER_USER:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: register_user ioctl received");
+#endif
+		{
+			fsl_shw_uco_t *user_ctx =
+			    os_alloc_memory(sizeof(*user_ctx), 0);
+
+			if (user_ctx == NULL) {
+				code = OS_ERROR_NO_MEMORY_S;
+			} else {
+				code =
+				    init_uco(user_ctx,
+					     (fsl_shw_uco_t *)
+					     os_dev_get_ioctl_arg());
+				if (code == OS_ERROR_OK_S) {
+					os_dev_set_user_private(user_ctx);
+				} else {
+					os_free_memory(user_ctx);
+				}
+			}
+		}
+		break;
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_DEREGISTER_USER:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: deregister_user ioctl received");
+#endif
+		{
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+			SHW_REMOVE_USER(user_ctx);
+		}
+		break;
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_GET_RESULTS:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: get_results ioctl received");
+#endif
+		code = get_results(user_ctx,
+				   (struct results_req *)
+				   os_dev_get_ioctl_arg());
+		break;
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_GET_CAPABILITIES:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: get_capabilities ioctl received");
+#endif
+		code = get_capabilities(user_ctx,
+					(fsl_shw_pco_t *)
+					os_dev_get_ioctl_arg());
+		break;
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_GET_RANDOM:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: get_random ioctl received");
+#endif
+		code = get_random(user_ctx,
+				  (struct get_random_req *)
+				  os_dev_get_ioctl_arg());
+		break;
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_ADD_ENTROPY:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: add_entropy ioctl received");
+#endif
+		code = add_entropy(user_ctx,
+				   (struct add_entropy_req *)
+				   os_dev_get_ioctl_arg());
+		break;
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_DROP_PERMS:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: drop permissions ioctl received");
+#endif
+		code =
+		    shw_handle_scc_drop_perms(user_ctx, os_dev_get_ioctl_arg());
+		break;
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_SSTATUS:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: sstatus ioctl received");
+#endif
+		code = shw_handle_scc_sstatus(user_ctx, os_dev_get_ioctl_arg());
+		break;
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_SFREE:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: sfree ioctl received");
+#endif
+		code = shw_handle_scc_sfree(user_ctx, os_dev_get_ioctl_arg());
+		break;
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_SCC_ENCRYPT:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: scc encrypt ioctl received");
+#endif
+		code = shw_handle_scc_encrypt(user_ctx, os_dev_get_ioctl_arg());
+		break;
+
+	case SHW_IOCTL_REQUEST + SHW_USER_REQ_SCC_DECRYPT:
+#ifdef SHW_DEBUG
+		LOG_KDIAG("SHW: scc decrypt ioctl received");
+#endif
+		code = shw_handle_scc_decrypt(user_ctx, os_dev_get_ioctl_arg());
+		break;
+
+	default:
+#ifdef SHW_DEBUG
+		LOG_KDIAG_ARGS("SHW: Unexpected ioctl %d",
+			       os_dev_get_ioctl_op());
+#endif
+		break;
+	}
+
+	os_dev_ioctl_return(code);
+}
+
+#ifdef FSL_HAVE_SCC2
+
+/*****************************************************************************/
+/* fn get_user_smid()                                                        */
+/*****************************************************************************/
+uint32_t get_user_smid(void *proc)
+{
+	/*
+	 * A real implementation would have some way to handle signed applications
+	 * which wouild be assigned distinct SMIDs.  For the reference
+	 * implementation, we show where this would be determined (here), but
+	 * always provide a fixed answer, thus not separating users at all.
+	 */
+
+	return 0x42eaae42;
+}
+
+/* user_base: userspace base address of the partition
+ * kernel_base: kernel mode base address of the partition
+ */
+static fsl_shw_return_t register_user_partition(fsl_shw_uco_t * user_ctx,
+						uint32_t user_base,
+						void *kernel_base)
+{
+	fsl_shw_spo_t *partition_info;
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+
+	if (user_ctx == NULL) {
+		goto out;
+	}
+
+	partition_info = os_alloc_memory(sizeof(fsl_shw_spo_t), GFP_KERNEL);
+
+	if (partition_info == NULL) {
+		goto out;
+	}
+
+	/* stuff the partition info, then put it at the front of the chain */
+	partition_info->user_base = user_base;
+	partition_info->kernel_base = kernel_base;
+	partition_info->next = user_ctx->partition;
+
+	user_ctx->partition = (struct fsl_shw_spo_t *)partition_info;
+
+#ifdef SHW_DEBUG
+	LOG_KDIAG_ARGS
+	    ("partition with user_base=%p, kernel_base=%p registered.",
+	     (void *)user_base, kernel_base);
+#endif
+
+	ret = FSL_RETURN_OK_S;
+
+      out:
+
+	return ret;
+}
+
+/* if the partition is in the users list, remove it */
+static fsl_shw_return_t deregister_user_partition(fsl_shw_uco_t * user_ctx,
+						  uint32_t user_base)
+{
+	fsl_shw_spo_t *curr = (fsl_shw_spo_t *) user_ctx->partition;
+	fsl_shw_spo_t *last = (fsl_shw_spo_t *) user_ctx->partition;
+
+	while (curr != NULL) {
+		if (curr->user_base == user_base) {
+
+#ifdef SHW_DEBUG
+			LOG_KDIAG_ARGS
+			    ("deregister_user_partition: partition with "
+			     "user_base=%p, kernel_base=%p deregistered.\n",
+			     (void *)curr->user_base, curr->kernel_base);
+#endif
+
+			if (last == curr) {
+				user_ctx->partition = curr->next;
+				os_free_memory(curr);
+				return FSL_RETURN_OK_S;
+			} else {
+				last->next = curr->next;
+				os_free_memory(curr);
+				return FSL_RETURN_OK_S;
+			}
+		}
+		last = curr;
+		curr = (fsl_shw_spo_t *) curr->next;
+	}
+
+	return FSL_RETURN_ERROR_S;
+}
+
+/* Find the kernel-mode address of the partition.
+ * This can then be passed to the SCC functions.
+ */
+void *lookup_user_partition(fsl_shw_uco_t * user_ctx, uint32_t user_base)
+{
+	/* search through the partition chain to find one that matches the user base
+	 * address.
+	 */
+	fsl_shw_spo_t *curr = (fsl_shw_spo_t *) user_ctx->partition;
+
+	while (curr != NULL) {
+		if (curr->user_base == user_base) {
+			return curr->kernel_base;
+		}
+		curr = (fsl_shw_spo_t *) curr->next;
+	}
+	return NULL;
+}
+
+#endif				/* FSL_HAVE_SCC2 */
+
+/*!
+*******************************************************************************
+* This function implements the smalloc() function for userspace programs, by
+* making a call to the SCC2 mmap() function that acquires a region of secure
+* memory on behalf of the user, and then maps it into the users memory space.
+* Currently, the only memory size supported is that of a single SCC2 partition.
+* Requests for other sized memory regions will fail.
+*/
+OS_DEV_MMAP(shw_mmap)
+{
+	os_error_code status = OS_ERROR_NO_MEMORY_S;
+
+#ifdef FSL_HAVE_SCC2
+	{
+		scc_return_t scc_ret;
+		fsl_shw_return_t fsl_ret;
+		uint32_t partition_registered = FALSE;
+
+		uint32_t user_base;
+		void *partition_base;
+		uint32_t smid;
+		scc_config_t *scc_configuration;
+
+		int part_no = -1;
+		uint32_t part_phys;
+
+		fsl_shw_uco_t *user_ctx =
+		    (fsl_shw_uco_t *) os_dev_get_user_private();
+
+		/* Make sure that the user context is valid */
+		if (user_ctx == NULL) {
+			user_ctx =
+			    os_alloc_memory(sizeof(*user_ctx), GFP_KERNEL);
+
+			if (user_ctx == NULL) {
+				status = OS_ERROR_NO_MEMORY_S;
+				goto out;
+			}
+			fsl_shw_register_user(user_ctx);
+			os_dev_set_user_private(user_ctx);
+		}
+
+		/* Determine the size of a secure partition */
+		scc_configuration = scc_get_configuration();
+
+		/* Check that the memory size requested is equal to the partition
+		 * size, and that the requested destination is on a page boundary.
+		 */
+		if (((os_mmap_user_base() % PAGE_SIZE) != 0) ||
+		    (os_mmap_memory_size() !=
+		     scc_configuration->partition_size_bytes)) {
+			status = OS_ERROR_BAD_ARG_S;
+			goto out;
+		}
+
+		/* Retrieve the SMID associated with the user */
+		smid = get_user_smid(user_ctx->process);
+
+		/* Attempt to allocate a secure partition */
+		scc_ret =
+		    scc_allocate_partition(smid, &part_no, &partition_base,
+					   &part_phys);
+		if (scc_ret != SCC_RET_OK) {
+			pr_debug
+			    ("SCC mmap() request failed to allocate partition;"
+			     " error %d\n", status);
+			status = OS_ERROR_FAIL_S;
+			goto out;
+		}
+
+		pr_debug("scc_mmap() acquired partition %d at %08x\n",
+			 part_no, part_phys);
+
+		/* Record partition info in the user context */
+		user_base = os_mmap_user_base();
+		fsl_ret =
+		    register_user_partition(user_ctx, user_base,
+					    partition_base);
+
+		if (fsl_ret != FSL_RETURN_OK_S) {
+			pr_debug
+			    ("SCC mmap() request failed to register partition with user"
+			     " context, error: %d\n", fsl_ret);
+			status = OS_ERROR_FAIL_S;
+		}
+
+		partition_registered = TRUE;
+
+		status = map_user_memory(os_mmap_memory_ctx(), part_phys,
+					 os_mmap_memory_size());
+
+#ifdef SHW_DEBUG
+		if (status == OS_ERROR_OK_S) {
+			LOG_KDIAG_ARGS
+			    ("Partition allocated: user_base=%p, partition_base=%p.",
+			     (void *)user_base, partition_base);
+		}
+#endif
+
+	      out:
+		/* If there is an error it has to be handled here */
+		if (status != OS_ERROR_OK_S) {
+			/* if the partition was registered with the user, unregister it. */
+			if (partition_registered == TRUE) {
+				deregister_user_partition(user_ctx, user_base);
+			}
+
+			/* if the partition was allocated, deallocate it */
+			if (partition_base != NULL) {
+				scc_release_partition(partition_base);
+			}
+		}
+	}
+#endif				/* FSL_HAVE_SCC2 */
+
+	return status;
+}
+
+/*****************************************************************************/
+/* fn shw_release()                                                         */
+/*****************************************************************************/
+/*!
+ * Handle @c close() call from user.
+ * This is a Linux device driver interface routine.
+ *
+ * @return OS_ERROR_OK_S on success (always!)
+ */
+OS_DEV_CLOSE(shw_release)
+{
+	fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+	os_error_code code = OS_ERROR_OK_S;
+
+	if (user_ctx != NULL) {
+
+		fsl_shw_deregister_user(user_ctx);
+		os_free_memory(user_ctx);
+		os_dev_set_user_private(NULL);
+
+	}
+
+	os_dev_close_return(code);
+}				/* shw_release */
+
+/*****************************************************************************/
+/* fn shw_user_callback()                                                    */
+/*****************************************************************************/
+/*!
+ * FSL SHW User callback function.
+ *
+ * This function is set in the kernel version of the user context as the
+ * callback function when the user mode user wants a callback.  Its job is to
+ * inform the user process that results (may) be available.  It does this by
+ * sending a SIGUSR2 signal which is then caught by the user-mode FSL SHW
+ * library.
+ *
+ * @param user_ctx        Kernel version of uco associated with the request.
+ *
+ * @return void
+ */
+static void shw_user_callback(fsl_shw_uco_t * user_ctx)
+{
+#ifdef SHW_DEBUG
+	LOG_KDIAG_ARGS("SHW: Signalling callback user process for context %p\n",
+		       user_ctx);
+#endif
+	os_send_signal(user_ctx->process, SIGUSR2);
+}
+
+/*****************************************************************************/
+/* fn setup_user_driver_interaction()                                        */
+/*****************************************************************************/
+/*!
+ * Register the driver with the kernel as the driver for shw_major_node.  Note
+ * that this value may be zero, in which case the major number will be assigned
+ * by the OS.  shw_major_node is never modified.
+ *
+ * The open(), ioctl(), and close() handles for the driver ned to be registered
+ * with the kernel.  Upon success, shw_device_registered will be true;
+ *
+ * @return OS_ERROR_OK_S on success, or an os err code
+ */
+static os_error_code shw_setup_user_driver_interaction(void)
+{
+	os_error_code error_code;
+
+	os_driver_init_registration(reg_handle);
+	os_driver_add_registration(reg_handle, OS_FN_OPEN,
+				   OS_DEV_OPEN_REF(shw_open));
+	os_driver_add_registration(reg_handle, OS_FN_IOCTL,
+				   OS_DEV_IOCTL_REF(shw_ioctl));
+	os_driver_add_registration(reg_handle, OS_FN_CLOSE,
+				   OS_DEV_CLOSE_REF(shw_release));
+	os_driver_add_registration(reg_handle, OS_FN_MMAP,
+				   OS_DEV_MMAP_REF(shw_mmap));
+	error_code = os_driver_complete_registration(reg_handle, shw_major_node,
+						     SHW_DRIVER_NAME);
+
+	if (error_code != OS_ERROR_OK_S) {
+		/* failure ! */
+#ifdef SHW_DEBUG
+		LOG_KDIAG_ARGS("SHW Driver: register device driver failed: %d",
+			       error_code);
+#endif
+	} else {		/* success */
+		shw_device_registered = TRUE;
+#ifdef SHW_DEBUG
+		LOG_KDIAG_ARGS("SHW Driver:  Major node is %d\n",
+			       os_driver_get_major(reg_handle));
+#endif
+	}
+
+	return error_code;
+}				/* shw_setup_user_driver_interaction */
+
+/******************************************************************/
+/* User Mode Support                                              */
+/******************************************************************/
+
+/*!
+ * Initialze kernel User Context Object from User-space version.
+ *
+ * Copy user UCO into kernel UCO, set flags and fields for operation
+ * within kernel space.  Add user to driver's list of users.
+ *
+ * @param user_ctx        Pointer to kernel space UCO
+ * @param user_mode_uco   User pointer to user space version
+ *
+ * @return os_error_code
+ */
+static os_error_code init_uco(fsl_shw_uco_t * user_ctx, void *user_mode_uco)
+{
+	os_error_code code;
+
+	code = os_copy_from_user(user_ctx, user_mode_uco, sizeof(*user_ctx));
+	if (code == OS_ERROR_OK_S) {
+		user_ctx->flags |= FSL_UCO_USERMODE_USER;
+		user_ctx->result_pool.head = NULL;
+		user_ctx->result_pool.tail = NULL;
+		user_ctx->process = os_get_process_handle();
+		user_ctx->callback = shw_user_callback;
+		SHW_ADD_USER(user_ctx);
+	}
+#ifdef SHW_DEBUG
+	LOG_KDIAG_ARGS("SHW: init uco returning %d (flags %x)",
+		       code, user_ctx->flags);
+#endif
+
+	return code;
+}
+
+/*!
+ * Copy array from kernel to user space.
+ *
+ * This routine will check bounds before trying to copy, and return failure
+ * on bounds violation or error during the copy.
+ *
+ * @param userloc   Location in userloc to place data.  If NULL, the function
+ *                  will do nothing (except return NULL).
+ * @param userend   Address beyond allowed copy region at @c userloc.
+ * @param data_start Location of data to be copied
+ * @param element_size  sizeof() an element
+ * @param element_count Number of elements of size element_size to copy.
+ * @return New value of userloc, or NULL if there was an error.
+ */
+inline static void *copy_array(void *userloc, void *userend, void *data_start,
+			       unsigned element_size, unsigned element_count)
+{
+	unsigned byte_count = element_size * element_count;
+
+	if ((userloc == NULL) || (userend == NULL)
+	    || ((userloc + byte_count) >= userend) ||
+	    (copy_to_user(userloc, data_start, byte_count) != OS_ERROR_OK_S)) {
+		userloc = NULL;
+	} else {
+		userloc += byte_count;
+	}
+
+	return userloc;
+}
+
+/*!
+ * Send an FSL SHW API return code up into the user-space request structure.
+ *
+ * @param user_header   User address of request block / request header
+ * @param result_code   The FSL SHW API code to be placed at header.code
+ *
+ * @return an os_error_code
+ *
+ * NOTE: This function assumes that the shw_req_header is at the beginning of
+ * each request structure.
+ */
+inline static os_error_code copy_fsl_code(void *user_header,
+					  fsl_shw_return_t result_code)
+{
+	return os_copy_to_user(user_header +
+			       offsetof(struct shw_req_header, code),
+			       &result_code, sizeof(result_code));
+}
+
+static os_error_code shw_handle_scc_drop_perms(fsl_shw_uco_t * user_ctx,
+					       uint32_t info)
+{
+	os_error_code status = OS_ERROR_NO_MEMORY_S;
+#ifdef FSL_HAVE_SCC2
+	scc_return_t scc_ret;
+	scc_partition_info_t partition_info;
+	void *kernel_base;
+
+	status =
+	    os_copy_from_user(&partition_info, (void *)info,
+			      sizeof(partition_info));
+
+	if (status != OS_ERROR_OK_S) {
+		goto out;
+	}
+
+	/* validate that the user owns this partition, and look up its handle */
+	kernel_base = lookup_user_partition(user_ctx, partition_info.user_base);
+
+	if (kernel_base == NULL) {
+		status = OS_ERROR_FAIL_S;
+#ifdef SHW_DEBUG
+		LOG_KDIAG("_scc_drop_perms(): failed to find partition\n");
+#endif
+		goto out;
+	}
+
+	/* call scc driver to perform the drop */
+	scc_ret = scc_diminish_permissions(kernel_base,
+					   partition_info.permissions);
+	if (scc_ret == SCC_RET_OK) {
+		status = OS_ERROR_OK_S;
+	} else {
+		status = OS_ERROR_FAIL_S;
+	}
+
+      out:
+#endif				/* FSL_HAVE_SCC2 */
+	return status;
+}
+
+static os_error_code shw_handle_scc_sstatus(fsl_shw_uco_t * user_ctx,
+					    uint32_t info)
+{
+	os_error_code status = OS_ERROR_NO_MEMORY_S;
+#ifdef FSL_HAVE_SCC2
+	scc_partition_info_t partition_info;
+	void *kernel_base;
+
+	status = os_copy_from_user(&partition_info,
+				   (void *)info, sizeof(partition_info));
+
+	if (status != OS_ERROR_OK_S) {
+		goto out;
+	}
+
+	/* validate that the user owns this partition, and look up its handle */
+	kernel_base = lookup_user_partition(user_ctx, partition_info.user_base);
+
+	if (kernel_base == NULL) {
+		status = OS_ERROR_FAIL_S;
+#ifdef SHW_DEBUG
+		LOG_KDIAG("Failed to find partition\n");
+#endif
+		goto out;
+	}
+
+	/* Call the SCC driver to ask about the partition status */
+	partition_info.status = scc_partition_status(kernel_base);
+
+	/* and copy the structure out */
+	status = os_copy_to_user((void *)info,
+				 &partition_info, sizeof(partition_info));
+
+      out:
+#endif				/* FSL_HAVE_SCC2 */
+	return status;
+}
+
+static os_error_code shw_handle_scc_sfree(fsl_shw_uco_t * user_ctx,
+					  uint32_t info)
+{
+	os_error_code status = OS_ERROR_NO_MEMORY_S;
+#ifdef FSL_HAVE_SCC2
+	{
+		scc_partition_info_t partition_info;
+		void *kernel_base;
+		int ret;
+
+		status = os_copy_from_user(&partition_info,
+					   (void *)info,
+					   sizeof(partition_info));
+
+		/* check that the copy was successful */
+		if (status != OS_ERROR_OK_S) {
+			goto out;
+		}
+
+		/* validate that the user owns this partition, and look up its handle */
+		kernel_base =
+		    lookup_user_partition(user_ctx, partition_info.user_base);
+
+		if (kernel_base == NULL) {
+			status = OS_ERROR_FAIL_S;
+#ifdef SHW_DEBUG
+			LOG_KDIAG("failed to find partition\n");
+#endif				/*SHW_DEBUG */
+			goto out;
+		}
+
+		/* Unmap the memory region (see sys_munmap in mmap.c) */
+		ret = unmap_user_memory(partition_info.user_base, 8192);
+
+		/* If the memory was successfully released */
+		if (ret == OS_ERROR_OK_S) {
+
+			/* release the partition */
+			scc_release_partition(kernel_base);
+
+			/* and remove it from the users context */
+			deregister_user_partition(user_ctx,
+						  partition_info.user_base);
+
+			status = OS_ERROR_OK_S;
+
+		} else {
+#ifdef SHW_DEBUG
+			LOG_KDIAG("do_munmap not successful!");
+#endif
+		}
+
+	}
+      out:
+#endif				/* FSL_HAVE_SCC2 */
+	return status;
+}
+
+static os_error_code shw_handle_scc_encrypt(fsl_shw_uco_t * user_ctx,
+					    uint32_t info)
+{
+	os_error_code status = OS_ERROR_FAIL_S;
+#ifdef FSL_HAVE_SCC2
+	{
+		fsl_shw_return_t retval;
+		scc_region_t region_info;
+		void *page_ctx = NULL;
+		void *black_addr = NULL;
+		void *partition_base = NULL;
+		scc_config_t *scc_configuration;
+
+		status =
+		    os_copy_from_user(&region_info, (void *)info,
+				      sizeof(region_info));
+
+		if (status != OS_ERROR_OK_S) {
+			goto out;
+		}
+
+		/* validate that the user owns this partition, and look up its handle */
+		partition_base = lookup_user_partition(user_ctx,
+						       region_info.
+						       partition_base);
+
+		if (partition_base == NULL) {
+			status = OS_ERROR_FAIL_S;
+#ifdef SHW_DEBUG
+			LOG_KDIAG("failed to find secure partition\n");
+#endif
+			goto out;
+		}
+
+		/* Check that the memory size requested is correct */
+		scc_configuration = scc_get_configuration();
+		if (region_info.offset + region_info.length >
+		    scc_configuration->partition_size_bytes) {
+			status = OS_ERROR_FAIL_S;
+			goto out;
+		}
+
+		/* wire down black_data */
+		black_addr = wire_user_memory(region_info.black_data,
+					      region_info.length, &page_ctx);
+
+		if (black_addr == NULL) {
+			status = OS_ERROR_FAIL_S;
+			goto out;
+		}
+
+		retval =
+		    do_scc_encrypt_region(NULL, partition_base,
+					  region_info.offset,
+					  region_info.length, black_addr,
+					  region_info.IV,
+					  region_info.cypher_mode);
+
+		if (retval == FSL_RETURN_OK_S) {
+			status = OS_ERROR_OK_S;
+		} else {
+			status = OS_ERROR_FAIL_S;
+		}
+
+		/* release black data */
+		unwire_user_memory(&page_ctx);
+	}
+      out:
+
+#endif				/* FSL_HAVE_SCC2 */
+	return status;
+}
+
+static os_error_code shw_handle_scc_decrypt(fsl_shw_uco_t * user_ctx,
+					    uint32_t info)
+{
+	os_error_code status = OS_ERROR_FAIL_S;
+#ifdef FSL_HAVE_SCC2
+	{
+		fsl_shw_return_t retval;
+		scc_region_t region_info;
+		void *page_ctx = NULL;
+		void *black_addr;
+		void *partition_base;
+		scc_config_t *scc_configuration;
+
+		status =
+		    os_copy_from_user(&region_info, (void *)info,
+				      sizeof(region_info));
+
+#ifdef SHW_DEBUG
+		LOG_KDIAG_ARGS
+		    ("partition_base: %p, offset: %i, length: %i, black data: %p",
+		     (void *)region_info.partition_base, region_info.offset,
+		     region_info.length, (void *)region_info.black_data);
+#endif
+
+		if (status != OS_ERROR_OK_S) {
+			goto out;
+		}
+
+		/* validate that the user owns this partition, and look up its handle */
+		partition_base = lookup_user_partition(user_ctx,
+						       region_info.
+						       partition_base);
+
+		if (partition_base == NULL) {
+			status = OS_ERROR_FAIL_S;
+#ifdef SHW_DEBUG
+			LOG_KDIAG("failed to find partition\n");
+#endif
+			goto out;
+		}
+
+		/* Check that the memory size requested is correct */
+		scc_configuration = scc_get_configuration();
+		if (region_info.offset + region_info.length >
+		    scc_configuration->partition_size_bytes) {
+			status = OS_ERROR_FAIL_S;
+			goto out;
+		}
+
+		/* wire down black_data */
+		black_addr = wire_user_memory(region_info.black_data,
+					      region_info.length, &page_ctx);
+
+		if (black_addr == NULL) {
+			status = OS_ERROR_FAIL_S;
+			goto out;
+		}
+
+		retval =
+		    do_scc_decrypt_region(NULL, partition_base,
+					  region_info.offset,
+					  region_info.length, black_addr,
+					  region_info.IV,
+					  region_info.cypher_mode);
+
+		if (retval == FSL_RETURN_OK_S) {
+			status = OS_ERROR_OK_S;
+		} else {
+			status = OS_ERROR_FAIL_S;
+		}
+
+		/* release black data */
+		unwire_user_memory(&page_ctx);
+	}
+      out:
+
+#endif				/* FSL_HAVE_SCC2 */
+	return status;
+}
+
+fsl_shw_return_t do_system_keystore_slot_alloc(fsl_shw_uco_t * user_ctx,
+					       uint32_t key_length,
+					       uint64_t ownerid,
+					       uint32_t * slot)
+{
+	(void)user_ctx;
+	return keystore_slot_alloc(&system_keystore, key_length, ownerid, slot);
+}
+
+fsl_shw_return_t do_system_keystore_slot_dealloc(fsl_shw_uco_t * user_ctx,
+						 uint64_t ownerid,
+						 uint32_t slot)
+{
+	(void)user_ctx;
+	return keystore_slot_dealloc(&system_keystore, ownerid, slot);
+}
+
+fsl_shw_return_t do_system_keystore_slot_load(fsl_shw_uco_t * user_ctx,
+					      uint64_t ownerid,
+					      uint32_t slot,
+					      const uint8_t * key,
+					      uint32_t key_length)
+{
+	(void)user_ctx;
+	return keystore_slot_load(&system_keystore, ownerid, slot,
+				  (void *)key, key_length);
+}
+
+fsl_shw_return_t do_system_keystore_slot_encrypt(fsl_shw_uco_t * user_ctx,
+						 uint64_t ownerid,
+						 uint32_t slot,
+						 uint32_t key_length,
+						 uint8_t * black_data)
+{
+	(void)user_ctx;
+	return keystore_slot_encrypt(NULL, &system_keystore, ownerid,
+				     slot, key_length, black_data);
+}
+
+fsl_shw_return_t do_system_keystore_slot_decrypt(fsl_shw_uco_t * user_ctx,
+						 uint64_t ownerid,
+						 uint32_t slot,
+						 uint32_t key_length,
+						 const uint8_t * black_data)
+{
+	(void)user_ctx;
+	return keystore_slot_decrypt(NULL, &system_keystore, ownerid,
+				     slot, key_length, black_data);
+}
+
+fsl_shw_return_t do_system_keystore_slot_read(fsl_shw_uco_t * user_ctx,
+					      uint64_t ownerid,
+					      uint32_t slot,
+					      uint32_t key_length,
+					      uint8_t * key_data)
+{
+	(void)user_ctx;
+
+	return keystore_slot_read(&system_keystore, ownerid,
+				  slot, key_length, key_data);
+}
+
+/*!
+ * Handle user-mode Get Capabilities request
+ *
+ * Right now, this function can only have a failure if the user has failed to
+ * provide a pointer to a location in user space with enough room to hold the
+ * fsl_shw_pco_t structure and any associated data.  It will treat this failure
+ * as an ioctl failure and return an ioctl error code, instead of treating it
+ * as an API failure.
+ *
+ * @param user_ctx    The kernel version of user's context
+ * @param user_mode_pco_request  Pointer to user-space request
+ *
+ * @return an os_error_code
+ */
+static os_error_code get_capabilities(fsl_shw_uco_t * user_ctx,
+				      void *user_mode_pco_request)
+{
+	os_error_code code;
+	struct capabilities_req req;
+	fsl_shw_pco_t local_cap;
+
+	memcpy(&local_cap, &cap, sizeof(cap));
+	/* Initialize pointers to out-of-struct arrays */
+	local_cap.sym_algorithms = NULL;
+	local_cap.sym_modes = NULL;
+	local_cap.sym_modes = NULL;
+
+	code = os_copy_from_user(&req, user_mode_pco_request, sizeof(req));
+	if (code == OS_ERROR_OK_S) {
+		void *endcap;
+		void *user_bounds;
+#ifdef SHW_DEBUG
+		LOG_KDIAG_ARGS("SHE: Received get_cap request: 0x%p/%u/0x%x",
+			       req.capabilities, req.size,
+			       sizeof(fsl_shw_pco_t));
+#endif
+		endcap = req.capabilities + 1;	/* point to end of structure */
+		user_bounds = (void *)req.capabilities + req.size;	/* end of area */
+
+		/* First verify that request is big enough for the main structure */
+		if (endcap >= user_bounds) {
+			endcap = NULL;	/* No! */
+		}
+
+		/* Copy any Symmetric Algorithm suppport */
+		if (cap.sym_algorithm_count != 0) {
+			local_cap.sym_algorithms = endcap;
+			endcap =
+			    copy_array(endcap, user_bounds, cap.sym_algorithms,
+				       sizeof(fsl_shw_key_alg_t),
+				       cap.sym_algorithm_count);
+		}
+
+		/* Copy any Symmetric Modes suppport */
+		if (cap.sym_mode_count != 0) {
+			local_cap.sym_modes = endcap;
+			endcap = copy_array(endcap, user_bounds, cap.sym_modes,
+					    sizeof(fsl_shw_sym_mode_t),
+					    cap.sym_mode_count);
+		}
+
+		/* Copy any Hash Algorithm suppport */
+		if (cap.hash_algorithm_count != 0) {
+			local_cap.hash_algorithms = endcap;
+			endcap =
+			    copy_array(endcap, user_bounds, cap.hash_algorithms,
+				       sizeof(fsl_shw_hash_alg_t),
+				       cap.hash_algorithm_count);
+		}
+
+		/* Now copy up the (possibly modified) main structure */
+		if (endcap != NULL) {
+			code =
+			    os_copy_to_user(req.capabilities, &local_cap,
+					    sizeof(cap));
+		}
+
+		if (endcap == NULL) {
+			code = OS_ERROR_BAD_ADDRESS_S;
+		}
+
+		/* And return the FSL SHW code in the request structure. */
+		if (code == OS_ERROR_OK_S) {
+			code =
+			    copy_fsl_code(user_mode_pco_request,
+					  FSL_RETURN_OK_S);
+		}
+	}
+
+	/* code may already be set to an error.  This is another error case.  */
+
+#ifdef SHW_DEBUG
+	LOG_KDIAG_ARGS("SHW: get capabilities returning %d", code);
+#endif
+
+	return code;
+}
+
+/*!
+ * Handle user-mode Get Results request
+ *
+ * Get arguments from user space into kernel space, then call
+ * fsl_shw_get_results, and then copy its return code and any results from
+ * kernel space back to user space.
+ *
+ * @param user_ctx    The kernel version of user's context
+ * @param user_mode_results_req  Pointer to user-space request
+ *
+ * @return an os_error_code
+ */
+static os_error_code get_results(fsl_shw_uco_t * user_ctx,
+				 void *user_mode_results_req)
+{
+	os_error_code code;
+	struct results_req req;
+	fsl_shw_result_t *results = NULL;
+	int loop;
+
+	code = os_copy_from_user(&req, user_mode_results_req, sizeof(req));
+	loop = 0;
+
+	if (code == OS_ERROR_OK_S) {
+		results = os_alloc_memory(req.requested * sizeof(*results), 0);
+		if (results == NULL) {
+			code = OS_ERROR_NO_MEMORY_S;
+		}
+	}
+
+	if (code == OS_ERROR_OK_S) {
+		fsl_shw_return_t err =
+		    fsl_shw_get_results(user_ctx, req.requested,
+					results, &req.actual);
+
+		/* Send API return code up to user. */
+		code = copy_fsl_code(user_mode_results_req, err);
+
+		if ((code == OS_ERROR_OK_S) && (err == FSL_RETURN_OK_S)) {
+			/* Now copy up the result count */
+			code = os_copy_to_user(user_mode_results_req
+					       + offsetof(struct results_req,
+							  actual), &req.actual,
+					       sizeof(req.actual));
+			if ((code == OS_ERROR_OK_S) && (req.actual != 0)) {
+				/* now copy up the results... */
+				code = os_copy_to_user(req.results, results,
+						       req.actual *
+						       sizeof(*results));
+			}
+		}
+	}
+
+	if (results != NULL) {
+		os_free_memory(results);
+	}
+
+	return code;
+}
+
+/*!
+ * Process header of user-mode request.
+ *
+ * Mark header as User Mode request.  Update UCO's flags and reference fields
+ * with current versions from the header.
+ *
+ * @param user_ctx  Pointer to kernel version of UCO.
+ * @param hdr       Pointer to common part of user request.
+ *
+ * @return void
+ */
+inline static void process_hdr(fsl_shw_uco_t * user_ctx,
+			       struct shw_req_header *hdr)
+{
+	hdr->flags |= FSL_UCO_USERMODE_USER;
+	user_ctx->flags = hdr->flags;
+	user_ctx->user_ref = hdr->user_ref;
+
+	return;
+}
+
+/*!
+ * Handle user-mode Get Random request
+ *
+ * @param user_ctx    The kernel version of user's context
+ * @param user_mode_get_random_req  Pointer to user-space request
+ *
+ * @return an os_error_code
+ */
+static os_error_code get_random(fsl_shw_uco_t * user_ctx,
+				void *user_mode_get_random_req)
+{
+	os_error_code code;
+	struct get_random_req req;
+
+	code = os_copy_from_user(&req, user_mode_get_random_req, sizeof(req));
+	if (code == OS_ERROR_OK_S) {
+		process_hdr(user_ctx, &req.hdr);
+#ifdef SHW_DEBUG
+		LOG_KDIAG_ARGS
+		    ("SHW: get_random() for %d bytes in %sblocking mode",
+		     req.size,
+		     (req.hdr.flags & FSL_UCO_BLOCKING_MODE) ? "" : "non-");
+#endif
+		req.hdr.code =
+		    fsl_shw_get_random(user_ctx, req.size, req.random);
+
+#ifdef SHW_DEBUG
+		LOG_KDIAG_ARGS("SHW: get_random() returning %d", req.hdr.code);
+#endif
+
+		/* Copy FSL function status back to user */
+		code = copy_fsl_code(user_mode_get_random_req, req.hdr.code);
+	}
+
+	return code;
+}
+
+/*!
+ * Handle user-mode Add Entropy request
+ *
+ * @param user_ctx    Pointer to the kernel version of user's context
+ * @param user_mode_add_entropy_req  Address of user-space request
+ *
+ * @return an os_error_code
+ */
+static os_error_code add_entropy(fsl_shw_uco_t * user_ctx,
+				 void *user_mode_add_entropy_req)
+{
+	os_error_code code;
+	struct add_entropy_req req;
+	uint8_t *local_buffer = NULL;
+
+	code = os_copy_from_user(&req, user_mode_add_entropy_req, sizeof(req));
+	if (code == OS_ERROR_OK_S) {
+		local_buffer = os_alloc_memory(req.size, 0);	/* for random */
+		if (local_buffer != NULL) {
+			code =
+			    os_copy_from_user(local_buffer, req.entropy,
+					      req.size);
+		}
+		if (code == OS_ERROR_OK_S) {
+			req.hdr.code = fsl_shw_add_entropy(user_ctx, req.size,
+							   local_buffer);
+
+			code =
+			    copy_fsl_code(user_mode_add_entropy_req,
+					  req.hdr.code);
+		}
+	}
+
+	if (local_buffer != NULL) {
+		os_free_memory(local_buffer);
+	}
+
+	return code;
+}
+
+/******************************************************************/
+/* End User Mode Support                                          */
+/******************************************************************/
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_register_user);
+#endif
+/* REQ-S2LRD-PINTFC-API-GEN-004 */
+/*
+ * Handle user registration.
+ *
+ * @param  user_ctx   The user context for the registration.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_register_user(fsl_shw_uco_t * user_ctx)
+{
+	fsl_shw_return_t code = FSL_RETURN_INTERNAL_ERROR_S;
+
+	if ((user_ctx->flags & FSL_UCO_BLOCKING_MODE) &&
+	    (user_ctx->flags & FSL_UCO_CALLBACK_MODE)) {
+		code = FSL_RETURN_BAD_FLAG_S;
+		goto error_exit;
+	} else if (user_ctx->pool_size == 0) {
+		code = FSL_RETURN_NO_RESOURCE_S;
+		goto error_exit;
+	} else {
+		user_ctx->result_pool.head = NULL;
+		user_ctx->result_pool.tail = NULL;
+		SHW_ADD_USER(user_ctx);
+		code = FSL_RETURN_OK_S;
+	}
+
+      error_exit:
+	return code;
+}
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_deregister_user);
+#endif
+/* REQ-S2LRD-PINTFC-API-GEN-005 */
+/*!
+ * Destroy the association between the the user and the provider of the API.
+ *
+ * @param  user_ctx   The user context which is no longer needed.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_deregister_user(fsl_shw_uco_t * user_ctx)
+{
+	shw_queue_entry_t *finished_request;
+	fsl_shw_return_t ret = FSL_RETURN_OK_S;
+
+	/* Clean up what we find in result pool. */
+	do {
+		os_lock_context_t lock_context;
+		os_lock_save_context(shw_queue_lock, lock_context);
+		finished_request = user_ctx->result_pool.head;
+
+		if (finished_request != NULL) {
+			SHW_QUEUE_REMOVE_ENTRY(&user_ctx->result_pool,
+					       finished_request);
+			os_unlock_restore_context(shw_queue_lock, lock_context);
+			os_free_memory(finished_request);
+		} else {
+			os_unlock_restore_context(shw_queue_lock, lock_context);
+		}
+	} while (finished_request != NULL);
+
+#ifdef FSL_HAVE_SCC2
+	{
+		fsl_shw_spo_t *partition;
+		struct mm_struct *mm = current->mm;
+
+		while ((user_ctx->partition != NULL)
+		       && (ret == FSL_RETURN_OK_S)) {
+
+			partition = user_ctx->partition;
+
+#ifdef SHW_DEBUG
+			LOG_KDIAG_ARGS
+			    ("Found an abandoned secure partition at %p, releasing",
+			     partition);
+#endif
+
+			/* It appears that current->mm is not valid if this is called from a
+			 * close routine (perhaps only if the program raised an exception that
+			 * caused it to close?)  If that is the case, then still free the 
+			 * partition, but do not remove it from the memory space (dangerous?)
+			 */
+
+			if (mm == NULL) {
+#ifdef SHW_DEBUG
+				LOG_KDIAG
+				    ("Warning: no mm structure found, not unmapping "
+				     "partition from user memory\n");
+#endif
+			} else {
+				/* Unmap the memory region (see sys_munmap in mmap.c) */
+				/* Note that this assumes a single memory partition */
+				unmap_user_memory(partition->user_base, 8192);
+			}
+
+			/* If the memory was successfully released */
+			if (ret == OS_ERROR_OK_S) {
+				/* release the partition */
+				scc_release_partition(partition->kernel_base);
+
+				/* and remove it from the users context */
+				deregister_user_partition(user_ctx,
+							  partition->user_base);
+
+				ret = FSL_RETURN_OK_S;
+			} else {
+				ret = FSL_RETURN_ERROR_S;
+
+				goto out;
+			}
+		}
+	}
+      out:
+#endif				/* FSL_HAVE_SCC2 */
+
+	SHW_REMOVE_USER(user_ctx);
+
+	return ret;
+}
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_get_results);
+#endif
+/* REQ-S2LRD-PINTFC-API-GEN-006 */
+fsl_shw_return_t fsl_shw_get_results(fsl_shw_uco_t * user_ctx,
+				     unsigned result_size,
+				     fsl_shw_result_t results[],
+				     unsigned *result_count)
+{
+	shw_queue_entry_t *finished_request;
+	unsigned loop = 0;
+
+	do {
+		os_lock_context_t lock_context;
+
+		/* Protect state of user's result pool until we have retrieved and
+		 * remove the first entry, or determined that the pool is empty. */
+		os_lock_save_context(shw_queue_lock, lock_context);
+		finished_request = user_ctx->result_pool.head;
+
+		if (finished_request != NULL) {
+			uint32_t code = 0;
+
+			SHW_QUEUE_REMOVE_ENTRY(&user_ctx->result_pool,
+					       finished_request);
+			os_unlock_restore_context(shw_queue_lock, lock_context);
+
+			results[loop].user_ref = finished_request->user_ref;
+			results[loop].code = finished_request->code;
+			results[loop].detail1 = 0;
+			results[loop].detail2 = 0;
+			results[loop].user_req =
+			    finished_request->user_mode_req;
+			if (finished_request->postprocess != NULL) {
+				code =
+				    finished_request->
+				    postprocess(finished_request);
+			}
+
+			results[loop].code = finished_request->code;
+			os_free_memory(finished_request);
+			if (code == 0) {
+				loop++;
+			}
+		} else {	/* finished_request is NULL */
+			/* pool is empty */
+			os_unlock_restore_context(shw_queue_lock, lock_context);
+		}
+
+	} while ((loop < result_size) && (finished_request != NULL));
+
+	*result_count = loop;
+
+	return FSL_RETURN_OK_S;
+}
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_get_capabilities);
+#endif
+fsl_shw_pco_t *fsl_shw_get_capabilities(fsl_shw_uco_t * user_ctx)
+{
+
+	/* Unused */
+	(void)user_ctx;
+
+	return &cap;
+}
+
+#if !(defined(FSL_HAVE_SAHARA) || defined(FSL_HAVE_RNGA)                    \
+      || defined(FSL_HAVE_RNGB) || defined(FSL_HAVE_RNGC))
+
+#if defined(LINUX_VERSION_CODE)
+EXPORT_SYMBOL(fsl_shw_get_random);
+#endif
+fsl_shw_return_t fsl_shw_get_random(fsl_shw_uco_t * user_ctx,
+				    uint32_t length, uint8_t * data)
+{
+
+	/* Unused */
+	(void)user_ctx;
+	(void)length;
+	(void)data;
+
+	return FSL_RETURN_ERROR_S;
+}
+
+#if defined(LINUX_VERSION_CODE)
+EXPORT_SYMBOL(fsl_shw_add_entropy);
+#endif
+fsl_shw_return_t fsl_shw_add_entropy(fsl_shw_uco_t * user_ctx,
+				     uint32_t length, uint8_t * data)
+{
+
+	/* Unused */
+	(void)user_ctx;
+	(void)length;
+	(void)data;
+
+	return FSL_RETURN_ERROR_S;
+}
+#endif
+
+#if !defined(FSL_HAVE_DRYICE) && !defined(FSL_HAVE_SAHARA2)
+#if 0
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_symmetric_decrypt);
+#endif
+fsl_shw_return_t fsl_shw_symmetric_decrypt(fsl_shw_uco_t * user_ctx,
+					   fsl_shw_sko_t * key_info,
+					   fsl_shw_scco_t * sym_ctx,
+					   uint32_t length,
+					   const uint8_t * ct, uint8_t * pt)
+{
+
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+	(void)sym_ctx;
+	(void)length;
+	(void)ct;
+	(void)pt;
+
+	return FSL_RETURN_ERROR_S;
+}
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_symmetric_encrypt);
+#endif
+fsl_shw_return_t fsl_shw_symmetric_encrypt(fsl_shw_uco_t * user_ctx,
+					   fsl_shw_sko_t * key_info,
+					   fsl_shw_scco_t * sym_ctx,
+					   uint32_t length,
+					   const uint8_t * pt, uint8_t * ct)
+{
+
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+	(void)sym_ctx;
+	(void)length;
+	(void)pt;
+	(void)ct;
+
+	return FSL_RETURN_ERROR_S;
+}
+
+/* DryIce support provided in separate file */
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_establish_key);
+#endif
+fsl_shw_return_t fsl_shw_establish_key(fsl_shw_uco_t * user_ctx,
+				       fsl_shw_sko_t * key_info,
+				       fsl_shw_key_wrap_t establish_type,
+				       const uint8_t * key)
+{
+
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+	(void)establish_type;
+	(void)key;
+
+	return FSL_RETURN_ERROR_S;
+}
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_extract_key);
+#endif
+fsl_shw_return_t fsl_shw_extract_key(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info,
+				     uint8_t * covered_key)
+{
+
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+	(void)covered_key;
+
+	return FSL_RETURN_ERROR_S;
+}
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_release_key);
+#endif
+fsl_shw_return_t fsl_shw_release_key(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info)
+{
+
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+
+	return FSL_RETURN_ERROR_S;
+}
+#endif
+#endif				/* SAHARA or DRYICE */
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_hash);
+#endif
+#if !defined(FSL_HAVE_SAHARA)
+fsl_shw_return_t fsl_shw_hash(fsl_shw_uco_t * user_ctx,
+			      fsl_shw_hco_t * hash_ctx,
+			      const uint8_t * msg,
+			      uint32_t length,
+			      uint8_t * result, uint32_t result_len)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+
+	/* Unused */
+	(void)user_ctx;
+	(void)hash_ctx;
+	(void)msg;
+	(void)length;
+	(void)result;
+	(void)result_len;
+
+	return ret;
+}
+#endif
+
+#ifndef FSL_HAVE_SAHARA
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_hmac_precompute);
+#endif
+
+fsl_shw_return_t fsl_shw_hmac_precompute(fsl_shw_uco_t * user_ctx,
+					 fsl_shw_sko_t * key_info,
+					 fsl_shw_hmco_t * hmac_ctx)
+{
+	fsl_shw_return_t status = FSL_RETURN_ERROR_S;
+
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+	(void)hmac_ctx;
+
+	return status;
+}
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_hmac);
+#endif
+
+fsl_shw_return_t fsl_shw_hmac(fsl_shw_uco_t * user_ctx,
+			      fsl_shw_sko_t * key_info,
+			      fsl_shw_hmco_t * hmac_ctx,
+			      const uint8_t * msg,
+			      uint32_t length,
+			      uint8_t * result, uint32_t result_len)
+{
+	fsl_shw_return_t status = FSL_RETURN_ERROR_S;
+
+	/* Unused */
+	(void)user_ctx;
+	(void)key_info;
+	(void)hmac_ctx;
+	(void)msg;
+	(void)length;
+	(void)result;
+	(void)result_len;
+
+	return status;
+}
+#endif
+
+/*!
+ * Call the proper function to encrypt a region of encrypted secure memory
+ *
+ * @brief 
+ *
+ * @param   user_ctx        User context of the partition owner (NULL in kernel)
+ * @param   partition_base  Base address (physical) of the partition
+ * @param   offset_bytes    Offset from base address of the data to be encrypted
+ * @param   byte_count      Length of the message (bytes)
+ * @param   black_data      Pointer to where the encrypted data is stored
+ * @param   IV              IV to use for encryption
+ * @param   cypher_mode     Cyphering mode to use, specified by type
+ *                          #fsl_shw_cypher_mode_t
+ *
+ * @return  status
+ */
+fsl_shw_return_t
+do_scc_encrypt_region(fsl_shw_uco_t * user_ctx,
+		      void *partition_base, uint32_t offset_bytes,
+		      uint32_t byte_count, uint8_t * black_data,
+		      uint32_t * IV, fsl_shw_cypher_mode_t cypher_mode)
+{
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+#ifdef FSL_HAVE_SCC2
+
+	scc_return_t scc_ret;
+
+#ifdef SHW_DEBUG
+	uint32_t *owner_32 = (uint32_t *) & (owner_id);
+
+	LOG_KDIAG_ARGS
+	    ("partition base: %p, offset: %i, count: %i, black data: %p\n",
+	     partition_base, offset_bytes, byte_count, (void *)black_data);
+
+	LOG_KDIAG_ARGS("Owner ID: %08x%08x\n", owner_32[1], owner_32[0]);
+#endif				/* SHW_DEBUG */
+	(void)user_ctx;
+
+	os_cache_flush_range(black_data, byte_count);
+
+	scc_ret =
+	    scc_encrypt_region((uint32_t) partition_base, offset_bytes,
+			       byte_count, __virt_to_phys(black_data), IV,
+			       cypher_mode);
+
+	if (scc_ret == SCC_RET_OK) {
+		retval = FSL_RETURN_OK_S;
+	} else {
+		retval = FSL_RETURN_ERROR_S;
+	}
+
+	/* The SCC2 DMA engine should have written to the black ram, so we need to
+	 * invalidate that region of memory.  Note that the red ram is not an
+	 * because it is mapped with the cache disabled.
+	 */
+	os_cache_inv_range(black_data, byte_count);
+
+#endif				/* FSL_HAVE_SCC2 */
+	return retval;
+}
+
+/*!
+ * Call the proper function to decrypt a region of encrypted secure memory
+ *
+ * @brief 
+ *
+ * @param   user_ctx        User context of the partition owner (NULL in kernel)
+ * @param   partition_base  Base address (physical) of the partition
+ * @param   offset_bytes    Offset from base address that the decrypted data
+ *                          shall be placed
+ * @param   byte_count      Length of the message (bytes)
+ * @param   black_data      Pointer to where the encrypted data is stored
+ * @param   IV              IV to use for decryption
+ * @param   cypher_mode     Cyphering mode to use, specified by type
+ *                          #fsl_shw_cypher_mode_t
+ *
+ * @return  status
+ */
+fsl_shw_return_t
+do_scc_decrypt_region(fsl_shw_uco_t * user_ctx,
+		      void *partition_base, uint32_t offset_bytes,
+		      uint32_t byte_count, const uint8_t * black_data,
+		      uint32_t * IV, fsl_shw_cypher_mode_t cypher_mode)
+{
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+
+#ifdef FSL_HAVE_SCC2
+
+	scc_return_t scc_ret;
+
+#ifdef SHW_DEBUG
+	uint32_t *owner_32 = (uint32_t *) & (owner_id);
+
+	LOG_KDIAG_ARGS
+	    ("partition base: %p, offset: %i, count: %i, black data: %p\n",
+	     partition_base, offset_bytes, byte_count, (void *)black_data);
+
+	LOG_KDIAG_ARGS("Owner ID: %08x%08x\n", owner_32[1], owner_32[0]);
+#endif				/* SHW_DEBUG */
+
+	(void)user_ctx;
+
+	/* The SCC2 DMA engine will be reading from the black ram, so we need to
+	 * make sure that the data is pushed out of the cache.  Note that the red
+	 * ram is not an issue because it is mapped with the cache disabled.
+	 */
+	os_cache_flush_range(black_data, byte_count);
+
+	scc_ret =
+	    scc_decrypt_region((uint32_t) partition_base, offset_bytes,
+			       byte_count,
+			       (uint8_t *) __virt_to_phys(black_data), IV,
+			       cypher_mode);
+
+	if (scc_ret == SCC_RET_OK) {
+		retval = FSL_RETURN_OK_S;
+	} else {
+		retval = FSL_RETURN_ERROR_S;
+	}
+
+#endif				/* FSL_HAVE_SCC2 */
+
+	return retval;
+}
+
+void *fsl_shw_smalloc(fsl_shw_uco_t * user_ctx,
+		      uint32_t size, const uint8_t * UMID, uint32_t permissions)
+{
+#ifdef FSL_HAVE_SCC2
+	int part_no;
+	void *part_base;
+	uint32_t part_phys;
+	scc_config_t *scc_configuration;
+
+	/* Check that the memory size requested is correct */
+	scc_configuration = scc_get_configuration();
+	if (size != scc_configuration->partition_size_bytes) {
+		return NULL;
+	}
+
+	/* attempt to grab a partition. */
+	if (scc_allocate_partition(0, &part_no, &part_base, &part_phys)
+	    != SCC_RET_OK) {
+		return NULL;
+	}
+#ifdef SHW_DEBUG
+	LOG_KDIAG_ARGS("Partition_base: %p, partition_base_phys: %p\n",
+		       part_base, (void *)part_phys);
+#endif
+
+	if (scc_engage_partition(part_base, UMID, permissions)
+	    != SCC_RET_OK) {
+		/* Engagement failed, so the partition needs to be de-allocated */
+
+#ifdef SHW_DEBUG
+		LOG_KDIAG_ARGS("Failed to engage partition %p, de-allocating",
+			       part_base);
+#endif
+		scc_release_partition(part_base);
+
+		return NULL;
+	}
+
+	return part_base;
+
+#else				/* FSL_HAVE_SCC2 */
+
+	(void)user_ctx;
+	(void)size;
+	(void)UMID;
+	(void)permissions;
+	return NULL;
+
+#endif				/* FSL_HAVE_SCC2 */
+}
+
+/* Release a block of secure memory */
+fsl_shw_return_t fsl_shw_sfree(fsl_shw_uco_t * user_ctx, void *address)
+{
+	(void)user_ctx;
+
+#ifdef FSL_HAVE_SCC2
+	if (scc_release_partition(address) == SCC_RET_OK) {
+		return FSL_RETURN_OK_S;
+	}
+#endif
+
+	return FSL_RETURN_ERROR_S;
+}
+
+/* Check the status of a block of secure memory */
+fsl_shw_return_t fsl_shw_sstatus(fsl_shw_uco_t * user_ctx,
+				 void *address,
+				 fsl_shw_partition_status_t * part_status)
+{
+	(void)user_ctx;
+
+#ifdef FSL_HAVE_SCC2
+	*part_status = scc_partition_status(address);
+
+	return FSL_RETURN_OK_S;
+#endif
+
+	return FSL_RETURN_ERROR_S;
+}
+
+/* Diminish permissions on some secure memory */
+fsl_shw_return_t fsl_shw_diminish_perms(fsl_shw_uco_t * user_ctx,
+					void *address, uint32_t permissions)
+{
+
+	(void)user_ctx;		/* unused parameter warning */
+
+#ifdef FSL_HAVE_SCC2
+	if (scc_diminish_permissions(address, permissions) == SCC_RET_OK) {
+		return FSL_RETURN_OK_S;
+	}
+#endif
+	return FSL_RETURN_ERROR_S;
+}
+
+#ifndef FSL_HAVE_SAHARA
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_gen_encrypt);
+#endif
+
+fsl_shw_return_t fsl_shw_gen_encrypt(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_acco_t * auth_ctx,
+				     fsl_shw_sko_t * cipher_key_info,
+				     fsl_shw_sko_t * auth_key_info,
+				     uint32_t auth_data_length,
+				     const uint8_t * auth_data,
+				     uint32_t payload_length,
+				     const uint8_t * payload,
+				     uint8_t * ct, uint8_t * auth_value)
+{
+	volatile fsl_shw_return_t status = FSL_RETURN_ERROR_S;
+
+	/* Unused */
+	(void)user_ctx;
+	(void)auth_ctx;
+	(void)cipher_key_info;
+	(void)auth_key_info;	/* save compilation warning */
+	(void)auth_data_length;
+	(void)auth_data;
+	(void)payload_length;
+	(void)payload;
+	(void)ct;
+	(void)auth_value;
+
+	return status;
+}
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_auth_decrypt);
+#endif
+/*!
+ * @brief Authenticate and decrypt a (CCM) stream.
+ *
+ * @param user_ctx         The user's context
+ * @param auth_ctx         Info on this Auth operation
+ * @param cipher_key_info  Key to encrypt payload
+ * @param auth_key_info    (unused - same key in CCM)
+ * @param auth_data_length Length in bytes of @a auth_data
+ * @param auth_data        Any auth-only data
+ * @param payload_length   Length in bytes of @a payload
+ * @param ct               The encrypted data
+ * @param auth_value       The authentication code to validate
+ * @param[out] payload     The location to store decrypted data
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_auth_decrypt(fsl_shw_uco_t * user_ctx,
+				      fsl_shw_acco_t * auth_ctx,
+				      fsl_shw_sko_t * cipher_key_info,
+				      fsl_shw_sko_t * auth_key_info,
+				      uint32_t auth_data_length,
+				      const uint8_t * auth_data,
+				      uint32_t payload_length,
+				      const uint8_t * ct,
+				      const uint8_t * auth_value,
+				      uint8_t * payload)
+{
+	volatile fsl_shw_return_t status = FSL_RETURN_ERROR_S;
+
+	/* Unused */
+	(void)user_ctx;
+	(void)auth_ctx;
+	(void)cipher_key_info;
+	(void)auth_key_info;	/* save compilation warning */
+	(void)auth_data_length;
+	(void)auth_data;
+	(void)payload_length;
+	(void)ct;
+	(void)auth_value;
+	(void)payload;
+
+	return status;
+}
+
+#endif				/* no SAHARA */
+
+#ifndef FSL_HAVE_DRYICE
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_gen_random_pf_key);
+#endif
+/*!
+ * Cause the hardware to create a new random key for secure memory use.
+ *
+ * Have the hardware use the secure hardware random number generator to load a
+ * new secret key into the hardware random key register.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_gen_random_pf_key(fsl_shw_uco_t * user_ctx)
+{
+	volatile fsl_shw_return_t status = FSL_RETURN_ERROR_S;
+
+	return status;
+}
+
+#endif				/* not have DRYICE */ 
+
+fsl_shw_return_t alloc_slot(fsl_shw_uco_t * user_ctx, fsl_shw_sko_t * key_info)
+{
+	fsl_shw_return_t ret = FSL_RETURN_INTERNAL_ERROR_S;
+
+	if (key_info->keystore == NULL) {
+		/* Key goes in system keystore */
+		ret = do_system_keystore_slot_alloc(user_ctx,
+						    key_info->key_length,
+						    key_info->userid,
+						    &(key_info->handle));
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("key length: %i, handle: %i",
+			      key_info->key_length, key_info->handle);
+#endif
+
+	} else {
+		/* Key goes in user keystore */
+		ret = keystore_slot_alloc(key_info->keystore,
+					  key_info->key_length,
+					  key_info->userid,
+					  &(key_info->handle));
+	}
+
+	return ret;
+}				/* end fn alloc_slot */
+
+fsl_shw_return_t load_slot(fsl_shw_uco_t * user_ctx,
+			   fsl_shw_sko_t * key_info, const uint8_t * key)
+{
+	fsl_shw_return_t ret = FSL_RETURN_INTERNAL_ERROR_S;
+
+	if (key_info->keystore == NULL) {
+		/* Key goes in system keystore */
+		ret = do_system_keystore_slot_load(user_ctx,
+						   key_info->userid,
+						   key_info->handle, key,
+						   key_info->key_length);
+	} else {
+		/* Key goes in user keystore */
+		ret = keystore_slot_load(key_info->keystore,
+					 key_info->userid,
+					 key_info->handle, key,
+					 key_info->key_length);
+	}
+
+	return ret;
+}				/* end fn load_slot */
+
+fsl_shw_return_t dealloc_slot(fsl_shw_uco_t * user_ctx,
+			      fsl_shw_sko_t * key_info)
+{
+	fsl_shw_return_t ret = FSL_RETURN_INTERNAL_ERROR_S;
+
+	if (key_info->keystore == NULL) {
+		/* Key goes in system keystore */
+		do_system_keystore_slot_dealloc(user_ctx,
+						key_info->userid,
+						key_info->handle);
+	} else {
+		/* Key goes in user keystore */
+		keystore_slot_dealloc(key_info->keystore,
+				      key_info->userid, key_info->handle);
+	}
+
+	key_info->flags &= ~(FSL_SKO_KEY_ESTABLISHED | FSL_SKO_KEY_PRESENT);
+
+	return ret;
+}				/* end fn slot_dealloc */
diff --git a/drivers/mxc/security/rng/shw_dryice.c b/drivers/mxc/security/rng/shw_dryice.c
new file mode 100644
index 000000000000..1fbd4bfef986
--- /dev/null
+++ b/drivers/mxc/security/rng/shw_dryice.c
@@ -0,0 +1,204 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include "shw_driver.h"
+#include "../dryice.h"
+
+#include <diagnostic.h>
+
+#ifdef FSL_HAVE_DRYICE
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_gen_random_pf_key);
+#endif
+/*!
+ * Cause the hardware to create a new random key for secure memory use.
+ *
+ * Have the hardware use the secure hardware random number generator to load a
+ * new secret key into the hardware random key register.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_gen_random_pf_key(fsl_shw_uco_t * user_ctx)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+	di_return_t di_ret;
+
+	/* For now, only blocking mode calls are supported */
+	if (!(user_ctx->flags & FSL_UCO_BLOCKING_MODE)) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	di_ret = dryice_set_random_key(0);
+	if (di_ret != DI_SUCCESS) {
+		printk("dryice_set_random_key returned %d\n", di_ret);
+		goto out;
+	}
+
+	ret = FSL_RETURN_OK_S;
+
+      out:
+	return ret;
+}
+
+#ifdef LINUX_VERSION_CODE
+EXPORT_SYMBOL(fsl_shw_read_tamper_event);
+#endif
+fsl_shw_return_t fsl_shw_read_tamper_event(fsl_shw_uco_t * user_ctx,
+					   fsl_shw_tamper_t * tamperp,
+					   uint64_t * timestampp)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+	di_return_t di_ret;
+	uint32_t di_events = 0;
+	uint32_t di_time_stamp;
+
+	/* Only blocking mode calls are supported */
+	if (!(user_ctx->flags & FSL_UCO_BLOCKING_MODE)) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	di_ret = dryice_get_tamper_event(&di_events, &di_time_stamp, 0);
+	if ((di_ret != DI_SUCCESS) && (di_ret != DI_ERR_STATE)) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS("dryice_get_tamper_event returned %s\n",
+			      di_error_string(di_ret));
+#endif
+		goto out;
+	}
+
+	/* Pass time back to caller */
+	*timestampp = (uint64_t) di_time_stamp;
+
+	if (di_events & DI_TAMPER_EVENT_WTD) {
+		*tamperp = FSL_SHW_TAMPER_WTD;
+	} else if (di_events & DI_TAMPER_EVENT_ETBD) {
+		*tamperp = FSL_SHW_TAMPER_ETBD;
+	} else if (di_events & DI_TAMPER_EVENT_ETAD) {
+		*tamperp = FSL_SHW_TAMPER_ETAD;
+	} else if (di_events & DI_TAMPER_EVENT_EBD) {
+		*tamperp = FSL_SHW_TAMPER_EBD;
+	} else if (di_events & DI_TAMPER_EVENT_SAD) {
+		*tamperp = FSL_SHW_TAMPER_SAD;
+	} else if (di_events & DI_TAMPER_EVENT_TTD) {
+		*tamperp = FSL_SHW_TAMPER_TTD;
+	} else if (di_events & DI_TAMPER_EVENT_CTD) {
+		*tamperp = FSL_SHW_TAMPER_CTD;
+	} else if (di_events & DI_TAMPER_EVENT_VTD) {
+		*tamperp = FSL_SHW_TAMPER_VTD;
+	} else if (di_events & DI_TAMPER_EVENT_MCO) {
+		*tamperp = FSL_SHW_TAMPER_MCO;
+	} else if (di_events & DI_TAMPER_EVENT_TCO) {
+		*tamperp = FSL_SHW_TAMPER_TCO;
+	} else if (di_events != 0) {
+		/* Apparentliy a tamper type not known to this driver was detected */
+		goto out;
+	} else {
+		*tamperp = FSL_SHW_TAMPER_NONE;
+	}
+
+	ret = FSL_RETURN_OK_S;
+
+      out:
+	return ret;
+}				/* end fn fsl_shw_read_tamper_event */
+#endif
+/*!
+ * Convert an SHW HW key reference into a DI driver key reference
+ *
+ * @param shw_pf_key   An SHW HW key value
+ * @param di_keyp      Location to store the equivalent DI driver key
+ *
+ * @return FSL_RETURN_OK_S, or error if key is unknown or cannot translate.
+ */
+fsl_shw_return_t shw_convert_pf_key(fsl_shw_pf_key_t shw_pf_key,
+				    di_key_t * di_keyp)
+{
+	fsl_shw_return_t ret = FSL_RETURN_BAD_FLAG_S;
+
+	switch (shw_pf_key) {
+	case FSL_SHW_PF_KEY_IIM:
+		*di_keyp = DI_KEY_FK;
+		break;
+	case FSL_SHW_PF_KEY_RND:
+		*di_keyp = DI_KEY_RK;
+		break;
+	case FSL_SHW_PF_KEY_IIM_RND:
+		*di_keyp = DI_KEY_FRK;
+		break;
+	case FSL_SHW_PF_KEY_PRG:
+		*di_keyp = DI_KEY_PK;
+		break;
+	case FSL_SHW_PF_KEY_IIM_PRG:
+		*di_keyp = DI_KEY_FPK;
+		break;
+	default:
+		goto out;
+	}
+
+	ret = FSL_RETURN_OK_S;
+
+      out:
+	return ret;
+}
+
+#ifdef DIAG_SECURITY_FUNC
+const char *di_error_string(int code)
+{
+	char *str = "unknown";
+
+	switch (code) {
+	case DI_SUCCESS:
+		str = "operation was successful";
+		break;
+	case DI_ERR_BUSY:
+		str = "device or resource busy";
+		break;
+	case DI_ERR_STATE:
+		str = "dryice is in incompatible state";
+		break;
+	case DI_ERR_INUSE:
+		str = "resource is already in use";
+		break;
+	case DI_ERR_UNSET:
+		str = "resource has not been initialized";
+		break;
+	case DI_ERR_WRITE:
+		str = "error occurred during register write";
+		break;
+	case DI_ERR_INVAL:
+		str = "invalid argument";
+		break;
+	case DI_ERR_FAIL:
+		str = "operation failed";
+		break;
+	case DI_ERR_HLOCK:
+		str = "resource is hard locked";
+		break;
+	case DI_ERR_SLOCK:
+		str = "resource is soft locked";
+		break;
+	case DI_ERR_NOMEM:
+		str = "out of memory";
+		break;
+	default:
+		break;
+	}
+
+	return str;
+}
+#endif				/* HAVE DRYICE */
diff --git a/drivers/mxc/security/rng/shw_hash.c b/drivers/mxc/security/rng/shw_hash.c
new file mode 100644
index 000000000000..ad9e7f319625
--- /dev/null
+++ b/drivers/mxc/security/rng/shw_hash.c
@@ -0,0 +1,328 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file shw_hash.c
+ *
+ * This file contains implementations for use of the (internal) SHW hash
+ * software computation.  It defines the usual three steps:
+ *
+ * - #shw_hash_init()
+ * - #shw_hash_update()
+ * - #shw_hash_final()
+ *
+ * In support of the above functions, it also contains these functions:
+ * - #sha256_init()
+ * - #sha256_process_block()
+ *
+ *
+ * These functions depend upon the Linux Endian functions __be32_to_cpu(),
+ * __cpu_to_be32() to convert a 4-byte big-endian array to an integer and
+ * vice-versa.  For those without Linux, it should be pretty obvious what they
+ * do.
+ *
+ * The #shw_hash_update() and #shw_hash_final() functions are generic enough to
+ * support SHA-1/SHA-224/SHA-256, as needed.  Some extra tweaking would be
+ * necessary to get them to support SHA-384/SHA-512.
+ * 
+ */
+
+#include "shw_driver.h"
+#include "shw_hash.h"
+
+#ifndef __KERNEL__
+#include <asm/types.h>
+#include <linux/byteorder/little_endian.h>	/* or whichever is proper for target arch */
+#define printk printf
+#endif
+
+/*!
+ * Rotate a value right by a number of bits.
+ *
+ * @param x   Word of data which needs rotating
+ * @param y   Number of bits to rotate
+ *
+ * @return The new value
+ */
+inline uint32_t rotr32fixed(uint32_t x, unsigned int y)
+{
+	return (uint32_t) ((x >> y) | (x << (32 - y)));
+}
+
+#define blk0(i) (W[i] = data[i])
+// Referencing parameters so many times is really poor practice.  Do not imitate these macros
+#define blk2(i) (W[i & 15] += s1(W[(i - 2) & 15]) + W[(i - 7) & 15] + s0(W[(i - 15) & 15]))
+
+#define Ch(x,y,z) (z ^ (x & (y ^ z)))
+#define Maj(x,y,z) ((x & y) | (z & (x | y)))
+
+#define a(i) T[(0 - i) & 7]
+#define b(i) T[(1 - i) & 7]
+#define c(i) T[(2 - i) & 7]
+#define d(i) T[(3 - i) & 7]
+#define e(i) T[(4 - i) & 7]
+#define f(i) T[(5 - i) & 7]
+#define g(i) T[(6 - i) & 7]
+#define h(i) T[(7 - i) & 7]
+
+// This is a bad way to write a multi-statement macro... and referencing 'i' so many
+// times is really poor practice.  Do not imitate.
+#define R(i) h(i) += S1( e(i)) + Ch(e(i), f(i), g(i)) + K[i + j] +(j ? blk2(i) : blk0(i));\
+        d(i) += h(i);h(i) += S0(a(i)) + Maj(a(i), b(i), c(i))
+
+// for SHA256
+#define S0(x) (rotr32fixed(x, 2) ^ rotr32fixed(x, 13) ^ rotr32fixed(x, 22))
+#define S1(x) (rotr32fixed(x, 6) ^ rotr32fixed(x, 11) ^ rotr32fixed(x, 25))
+#define s0(x) (rotr32fixed(x, 7) ^ rotr32fixed(x, 18) ^ (x >> 3))
+#define s1(x) (rotr32fixed(x, 17) ^ rotr32fixed(x, 19) ^ (x >> 10))
+
+/*!
+ * Initialize the Hash State
+ *
+ *  Constructs the SHA256 hash engine.
+ *  Specification:
+ *      State Size  = 32 bytes
+ *      Block Size  = 64 bytes
+ *      Digest Size = 32 bytes
+ *
+ * @param state     Address of hash state structure
+ *
+ */
+void sha256_init(shw_hash_state_t * state)
+{
+	state->bit_count = 0;
+	state->partial_count_bytes = 0;
+
+	state->state[0] = 0x6a09e667;
+	state->state[1] = 0xbb67ae85;
+	state->state[2] = 0x3c6ef372;
+	state->state[3] = 0xa54ff53a;
+	state->state[4] = 0x510e527f;
+	state->state[5] = 0x9b05688c;
+	state->state[6] = 0x1f83d9ab;
+	state->state[7] = 0x5be0cd19;
+}
+
+const uint32_t K[64] = {
+	0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5,
+	0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5,
+	0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3,
+	0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174,
+	0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc,
+	0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da,
+	0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7,
+	0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967,
+	0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13,
+	0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85,
+	0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3,
+	0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070,
+	0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5,
+	0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3,
+	0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208,
+	0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2
+};
+
+/*!
+ * Hash a block of data into the SHA-256 hash state.
+ *
+ * This function hash the block of data in the @c partial_block
+ * element of the state structure into the state variables of the
+ * state structure.
+ *
+ * @param state     Address of hash state structure
+ *
+ */
+static void sha256_process_block(shw_hash_state_t * state)
+{
+	uint32_t W[16];
+	uint32_t T[8];
+	uint32_t stack_buffer[SHW_HASH_BLOCK_WORD_SIZE];
+	uint32_t *data = &stack_buffer[0];
+	uint8_t *input = state->partial_block;
+	unsigned int i;
+	unsigned int j;
+
+	/* Copy byte-oriented input block into word-oriented registers */
+	for (i = 0; i < SHW_HASH_BLOCK_LEN / sizeof(uint32_t);
+	     i++, input += sizeof(uint32_t)) {
+		stack_buffer[i] = __be32_to_cpu(*(uint32_t *) input);
+	}
+
+	/* Copy context->state[] to working vars */
+	memcpy(T, state->state, sizeof(T));
+
+	/* 64 operations, partially loop unrolled */
+	for (j = 0; j < SHW_HASH_BLOCK_LEN; j += 16) {
+		R(0);
+		R(1);
+		R(2);
+		R(3);
+		R(4);
+		R(5);
+		R(6);
+		R(7);
+		R(8);
+		R(9);
+		R(10);
+		R(11);
+		R(12);
+		R(13);
+		R(14);
+		R(15);
+	}
+	/* Add the working vars back into context.state[] */
+	state->state[0] += a(0);
+	state->state[1] += b(0);
+	state->state[2] += c(0);
+	state->state[3] += d(0);
+	state->state[4] += e(0);
+	state->state[5] += f(0);
+	state->state[6] += g(0);
+	state->state[7] += h(0);
+
+	/* Wipe variables */
+	memset(W, 0, sizeof(W));
+	memset(T, 0, sizeof(T));
+}
+
+/*!
+ * Initialize the hash state structure
+ *
+ * @param state     Address of hash state structure.
+ * @param alg Which hash algorithm to use (must be FSL_HASH_ALG_SHA1)
+ *
+ * @return  FSL_RETURN_OK_S if all went well, otherwise an error code.
+ */
+fsl_shw_return_t shw_hash_init(shw_hash_state_t * state, fsl_shw_hash_alg_t alg)
+{
+	if (alg != FSL_HASH_ALG_SHA256) {
+		return FSL_RETURN_BAD_ALGORITHM_S;
+	}
+
+	sha256_init(state);
+
+	return FSL_RETURN_OK_S;
+}
+
+/*!
+ * Add input bytes to the hash 
+ *
+ * The bytes are added to the partial_block element of the hash state, and as
+ * the partial block is filled, it is processed by sha1_process_block().  This
+ * function also updates the bit_count element of the hash state.
+ *
+ * @param state     Address of hash state structure
+ * @param input     Address of bytes to add to the hash
+ * @param input_len Numbef of bytes at @c input
+ *
+ */
+fsl_shw_return_t shw_hash_update(shw_hash_state_t * state,
+				 const uint8_t * input, unsigned int input_len)
+{
+	unsigned int bytes_needed;	/* Needed to fill a block */
+	unsigned int bytes_to_copy;	/* to copy into the block */
+
+	/* Account for new data */
+	state->bit_count += 8 * input_len;
+
+	/*
+	 * Process input bytes into the ongoing block; process the block when it
+	 * gets full.
+	 */
+	while (input_len > 0) {
+		bytes_needed = SHW_HASH_BLOCK_LEN - state->partial_count_bytes;
+		bytes_to_copy = ((input_len < bytes_needed) ?
+				 input_len : bytes_needed);
+
+		/* Add in the bytes and do the accounting */
+		memcpy(state->partial_block + state->partial_count_bytes,
+		       input, bytes_to_copy);
+		input += bytes_to_copy;
+		input_len -= bytes_to_copy;
+		state->partial_count_bytes += bytes_to_copy;
+
+		/* Run a full block through the transform */
+		if (state->partial_count_bytes == SHW_HASH_BLOCK_LEN) {
+			sha256_process_block(state);
+			state->partial_count_bytes = 0;
+		}
+	}
+
+	return FSL_RETURN_OK_S;
+}				/* end fn shw_hash_update */
+
+/*!
+ * Finalize the hash
+ *
+ * Performs the finalize operation on the previous input data & returns the
+ * resulting digest.  The finalize operation performs the appropriate padding
+ * up to the block size.
+ *
+ * @param state     Address of hash state structure
+ * @param result    Location to store the hash result
+ * @param result_len Number of bytes of @c result to be stored.
+ *
+ * @return FSL_RETURN_OK_S if all went well, FSL_RETURN_BAD_DATA_LENGTH_S if
+ * hash_len is too long, otherwise an error code.
+ */
+fsl_shw_return_t shw_hash_final(shw_hash_state_t * state, uint8_t * result,
+				unsigned int result_len)
+{
+	static const uint8_t pad[SHW_HASH_BLOCK_LEN * 2] = {
+		0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+	};
+
+	uint8_t data[sizeof(state->bit_count)];
+	uint32_t pad_length;
+	uint64_t bit_count = state->bit_count;
+	uint8_t hash[SHW_HASH_LEN];
+	int i;
+
+	if (result_len > SHW_HASH_LEN) {
+		return FSL_RETURN_BAD_DATA_LENGTH_S;
+	}
+
+	/* Save the length before padding. */
+	for (i = sizeof(state->bit_count) - 1; i >= 0; i--) {
+		data[i] = bit_count & 0xFF;
+		bit_count >>= 8;
+	}
+	pad_length = ((state->partial_count_bytes < 56) ?
+		      (56 - state->partial_count_bytes) :
+		      (120 - state->partial_count_bytes));
+
+	/* Pad to 56 bytes mod 64 (BLOCK_SIZE). */
+	shw_hash_update(state, pad, pad_length);
+
+	/*
+	 * Append the length.  This should trigger transform of the final block.
+	 */
+	shw_hash_update(state, data, sizeof(state->bit_count));
+
+	/* Copy the result into a byte array */
+	for (i = 0; i < SHW_HASH_STATE_WORDS; i++) {
+		*(uint32_t *) (hash + 4 * i) = __cpu_to_be32(state->state[i]);
+	}
+
+	/* And copy the result out to caller */
+	memcpy(result, hash, result_len);
+
+	return FSL_RETURN_OK_S;
+}				/* end fn shw_hash_final */
diff --git a/drivers/mxc/security/rng/shw_hmac.c b/drivers/mxc/security/rng/shw_hmac.c
new file mode 100644
index 000000000000..0e8728299860
--- /dev/null
+++ b/drivers/mxc/security/rng/shw_hmac.c
@@ -0,0 +1,145 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file shw_hmac.c
+ *
+ * This file contains implementations for use of the (internal) SHW HMAC
+ * software computation.  It defines the usual three steps:
+ *
+ * - #shw_hmac_init()
+ * - #shw_hmac_update()
+ * - #shw_hmac_final()
+ *
+ * 
+ */
+
+#include "shw_driver.h"
+#include "shw_hmac.h"
+
+#ifndef __KERNEL__
+#include <asm/types.h>
+#include <linux/byteorder/little_endian.h>	/* or whichever is proper for target arch */
+#define printk printf
+#endif
+
+/*! XOR value for HMAC inner key */
+#define INNER_HASH_CONSTANT 0x36
+
+/*! XOR value for HMAC outer key */
+#define OUTER_HASH_CONSTANT 0x5C
+
+/*!
+ * Initialize the HMAC state structure with the HMAC key
+ *
+ * @param state     Address of HMAC state structure
+ * @param key       Address of the key to be used for the HMAC.
+ * @param key_len   Number of bytes of @c key.
+ *
+ * Convert the key into its equivalent inner and outer hash state objects.
+ *
+ * @return  FSL_RETURN_OK_S if all went well, otherwise an error code.
+ */
+fsl_shw_return_t shw_hmac_init(shw_hmac_state_t * state,
+			       const uint8_t * key, unsigned int key_len)
+{
+	fsl_shw_return_t code = FSL_RETURN_ERROR_S;
+	uint8_t first_block[SHW_HASH_BLOCK_LEN];
+	unsigned int i;
+
+	/* Don't bother handling the pre-hash. */
+	if (key_len > SHW_HASH_BLOCK_LEN) {
+		code = FSL_RETURN_BAD_KEY_LENGTH_S;
+		goto out;
+	}
+
+	/* Prepare inner hash */
+	for (i = 0; i < SHW_HASH_BLOCK_LEN; i++) {
+		if (i < key_len) {
+			first_block[i] = key[i] ^ INNER_HASH_CONSTANT;
+		} else {
+			first_block[i] = INNER_HASH_CONSTANT;
+		}
+	}
+	code = shw_hash_init(&state->inner_hash, FSL_HASH_ALG_SHA256);
+	if (code != FSL_RETURN_OK_S) {
+		goto out;
+	}
+	shw_hash_update(&state->inner_hash, first_block, SHW_HASH_BLOCK_LEN);
+
+	/* Prepare outer hash */
+	for (i = 0; i < SHW_HASH_BLOCK_LEN; i++) {
+		if (i < key_len) {
+			first_block[i] = key[i] ^ OUTER_HASH_CONSTANT;
+		} else {
+			first_block[i] = OUTER_HASH_CONSTANT;
+		}
+	}
+	code = shw_hash_init(&state->outer_hash, FSL_HASH_ALG_SHA256);
+	if (code != FSL_RETURN_OK_S) {
+		goto out;
+	}
+	shw_hash_update(&state->outer_hash, first_block, SHW_HASH_BLOCK_LEN);
+
+	/* Wipe evidence of key */
+	memset(first_block, 0, SHW_HASH_BLOCK_LEN);
+
+      out:
+	return code;
+}
+
+/*!
+ * Put data into the HMAC calculation
+ *
+ * Send the msg data inner inner hash's update function.
+ *
+ * @param state     Address of HMAC state structure.
+ * @param msg       Address of the message data for the HMAC.
+ * @param msg_len   Number of bytes of @c msg.
+ *
+ * @return  FSL_RETURN_OK_S if all went well, otherwise an error code.
+ */
+fsl_shw_return_t shw_hmac_update(shw_hmac_state_t * state,
+				 const uint8_t * msg, unsigned int msg_len)
+{
+	shw_hash_update(&state->inner_hash, msg, msg_len);
+
+	return FSL_RETURN_OK_S;
+}
+
+/*!
+ * Calculate the final HMAC
+ *
+ * @param state     Address of HMAC state structure.
+ * @param hmac      Address of location to store the HMAC.
+ * @param hmac_len  Number of bytes of @c mac to be stored.  Probably best if
+ *                  this value is no greater than #SHW_HASH_LEN.
+ *
+ * This function finalizes the internal hash, and uses that result as
+ * data for the outer hash.  As many bytes of that result are passed
+ * to the user as desired.
+ *
+ * @return  FSL_RETURN_OK_S if all went well, otherwise an error code.
+ */
+fsl_shw_return_t shw_hmac_final(shw_hmac_state_t * state,
+				uint8_t * hmac, unsigned int hmac_len)
+{
+	uint8_t hash_result[SHW_HASH_LEN];
+
+	shw_hash_final(&state->inner_hash, hash_result, sizeof(hash_result));
+	shw_hash_update(&state->outer_hash, hash_result, SHW_HASH_LEN);
+
+	shw_hash_final(&state->outer_hash, hmac, hmac_len);
+
+	return FSL_RETURN_OK_S;
+}
diff --git a/drivers/mxc/security/rng/shw_memory_mapper.c b/drivers/mxc/security/rng/shw_memory_mapper.c
new file mode 100644
index 000000000000..71f1d301f02a
--- /dev/null
+++ b/drivers/mxc/security/rng/shw_memory_mapper.c
@@ -0,0 +1,213 @@
+/*
+ * Copyright 2005-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+
+
+/**
+ * Memory management functions, from Sahara Crypto API
+ *
+ * This is a subset of the memory management functions from the Sahara Crypto
+ * API, and is intended to support user secure partitions.
+ */
+
+#include "portable_os.h"
+#include "fsl_shw.h"
+
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/pagemap.h>
+
+#ifdef SHW_DEBUG
+#include <diagnostic.h>
+#endif
+
+/* Page context structure.  Used by wire_user_memory and unwire_user_memory */
+typedef struct page_ctx_t {
+    uint32_t        count;
+    struct page   **local_pages;
+} page_ctx_t;
+
+/**
+*******************************************************************************
+* Map and wire down a region of user memory.
+*
+*
+* @param    address     Userspace address of the memory to wire
+* @param    length      Length of the memory region to wire
+* @param    page_ctx    Page context, to be passed to unwire_user_memory
+*
+* @return   (if successful) Kernel virtual address of the wired pages
+*/
+void* wire_user_memory(void* address, uint32_t length, void **page_ctx)
+{
+    void* kernel_black_addr = NULL;
+    int result = -1;
+    int page_index = 0;
+    page_ctx_t *page_context;
+    int nr_pages = 0;
+    unsigned long start_page;
+    fsl_shw_return_t status;
+
+    /* Determine the number of pages being used for this link */
+    nr_pages = (((unsigned long)(address) & ~PAGE_MASK)
+                + length + ~PAGE_MASK) >> PAGE_SHIFT;
+
+    start_page = (unsigned long)(address) & PAGE_MASK;
+
+    /* Allocate some memory to keep track of the wired user pages, so that
+     * they can be deallocated later.  The block of memory will contain both
+     * the structure and the array of pages.
+     */
+    page_context = kmalloc(sizeof(page_ctx_t)
+                           + nr_pages * sizeof(struct page *), GFP_KERNEL);
+
+    if (page_context == NULL) {
+        status = FSL_RETURN_NO_RESOURCE_S; /* no memory! */
+#ifdef DIAG_DRV_IF
+        LOG_KDIAG("kmalloc() failed.");
+#endif
+        return NULL;
+    }
+
+    /* Set the page pointer to point to the allocated region of memory */
+    page_context->local_pages = (void*)page_context + sizeof(page_ctx_t);
+
+#ifdef DIAG_DRV_IF
+    LOG_KDIAG_ARGS("page_context at: %p, local_pages at: %p",
+                   (void *)page_context,
+                   (void *)(page_context->local_pages));
+#endif
+
+    /* Wire down the pages from user space */
+    down_read(&current->mm->mmap_sem);
+    result = get_user_pages(current, current->mm,
+                            start_page, nr_pages,
+                            WRITE, 0 /* noforce */,
+                            (page_context->local_pages), NULL);
+    up_read(&current->mm->mmap_sem);
+
+    if (result < nr_pages) {
+#ifdef DIAG_DRV_IF
+        LOG_KDIAG("get_user_pages() failed.");
+#endif
+        if (result > 0) {
+            for (page_index = 0; page_index < result; page_index++)
+                page_cache_release((page_context->local_pages[page_index]));
+
+            kfree(page_context);
+        }
+        return NULL;
+    }
+
+    kernel_black_addr = page_address(page_context->local_pages[0]) +
+                        ((unsigned long)address & ~PAGE_MASK);
+
+    page_context->count = nr_pages;
+    *page_ctx = page_context;
+
+    return kernel_black_addr;
+}
+
+
+/**
+*******************************************************************************
+* Release and unmap a region of user memory.
+*
+* @param    page_ctx    Page context from wire_user_memory
+*/
+void unwire_user_memory(void** page_ctx)
+{
+    int page_index = 0;
+    struct page_ctx_t *page_context = *page_ctx;
+
+#ifdef DIAG_DRV_IF
+    LOG_KDIAG_ARGS("page_context at: %p, first page at:%p, count: %i",
+                   (void *)page_context,
+                   (void *)(page_context->local_pages),
+                   page_context->count);
+#endif
+
+    if ((page_context != NULL) && (page_context->local_pages != NULL)) {
+        for (page_index = 0; page_index < page_context->count; page_index++)
+            page_cache_release(page_context->local_pages[page_index]);
+
+        kfree(page_context);
+        *page_ctx = NULL;
+    }
+}
+
+
+/**
+*******************************************************************************
+* Map some physical memory into a users memory space
+*
+* @param    vma             Memory structure to map to
+* @param    physical_addr   Physical address of the memory to be mapped in
+* @param    size            Size of the memory to map (bytes)
+*
+* @return
+*/
+os_error_code
+map_user_memory(struct vm_area_struct *vma, uint32_t physical_addr, uint32_t size)
+{
+    os_error_code retval;
+
+    /* Map the acquired partition into the user's memory space */
+    vma->vm_end = vma->vm_start + size;
+
+    /* set cache policy to uncached so that each write of the UMID and
+     * permissions get directly to the SCC2 in order to engage it
+     * properly.  Once the permissions have been written, it may be
+     * useful to provide a service for the user to request a different
+     * cache policy
+     */
+    vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+    /* Make sure that the user cannot fork() a child which will inherit
+     * this mapping, as it creates a security hole.  Likewise, do not
+     * allow the user to 'expand' his mapping beyond this partition.
+     */
+    vma->vm_flags |= VM_IO | VM_RESERVED | VM_DONTCOPY | VM_DONTEXPAND;
+
+    retval = remap_pfn_range(vma,
+                             vma->vm_start,
+                             __phys_to_pfn(physical_addr),
+                             size,
+                             vma->vm_page_prot);
+
+    return retval;
+}
+
+
+/**
+*******************************************************************************
+* Remove some memory from a user's memory space
+*
+* @param    user_addr       Userspace address of the memory to be unmapped
+* @param    size            Size of the memory to map (bytes)
+*
+* @return
+*/
+os_error_code
+unmap_user_memory(uint32_t user_addr, uint32_t size)
+{
+    os_error_code retval;
+    struct mm_struct *mm = current->mm;
+
+    /* Unmap the memory region (see sys_munmap in mmap.c) */
+    down_write(&mm->mmap_sem);
+    retval = do_munmap(mm, (unsigned long)user_addr, size);
+    up_write(&mm->mmap_sem);
+
+    return retval;
+}
diff --git a/drivers/mxc/security/sahara2/Kconfig b/drivers/mxc/security/sahara2/Kconfig
new file mode 100644
index 000000000000..ab4e6fdc2cfc
--- /dev/null
+++ b/drivers/mxc/security/sahara2/Kconfig
@@ -0,0 +1,35 @@
+menu "SAHARA2 Security Hardware Support"
+
+config MXC_SAHARA
+	tristate "Security Hardware Support (FSL SHW)"
+	---help---
+	  Provides driver and kernel mode API for using cryptographic
+	  accelerators.
+
+config MXC_SAHARA_USER_MODE
+	tristate "User Mode API for FSL SHW"
+	depends on MXC_SAHARA
+	---help---
+	  Provides kernel driver for User Mode API.
+
+config MXC_SAHARA_POLL_MODE
+	bool "Force driver to POLL for hardware completion."
+	depends on MXC_SAHARA
+	default n
+	---help---
+	  When this flag is yes, the driver will not use interrupts to
+	  determine when the hardware has completed a task, but instead
+	  will hold onto the CPU and continually poll the hardware until
+	  it completes.
+
+config MXC_SAHARA_POLL_MODE_TIMEOUT
+    hex "Poll loop timeout"
+    depends on MXC_SAHARA_POLL_MODE
+    default "0xFFFFFFFF"
+    help
+        To avoid infinite polling, a timeout is provided. Should the
+        timeout be reached, a fault is reported, indicating there must
+        be something wrong with SAHARA, and SAHARA is reset. The loop
+        will exit after the given number of iterations.
+
+endmenu
diff --git a/drivers/mxc/security/sahara2/Makefile b/drivers/mxc/security/sahara2/Makefile
new file mode 100644
index 000000000000..b515709be29b
--- /dev/null
+++ b/drivers/mxc/security/sahara2/Makefile
@@ -0,0 +1,47 @@
+# Makefile for the Linux Sahara2 driver
+#
+# This makefile works within a kernel driver tree
+
+# Need to augment this to support optionally building user-mode support
+API_SOURCES = fsl_shw_sym.c  fsl_shw_user.c  fsl_shw_hash.c  fsl_shw_auth.c \
+              fsl_shw_hmac.c  fsl_shw_rand.c  sf_util.c  km_adaptor.c  fsl_shw_keystore.c  \
+              fsl_shw_wrap.c \
+
+
+SOURCES = sah_driver_interface.c  sah_hardware_interface.c            \
+          sah_interrupt_handler.c  sah_queue.c  sah_queue_manager.c   \
+          sah_status_manager.c  sah_memory_mapper.c
+
+
+# Turn on for mostly full debugging
+# DIAGS = -DDIAG_DRV_STATUS -DDIAG_DRV_QUEUE -DDIAG_DRV_INTERRUPT -DDIAG_DRV_IF
+# DIAGS += -DDIAG_DURING_INTERRUPT
+
+# Turn on for lint-type checking
+#EXTRA_CFLAGS = -Wall -W -Wstrict-prototypes -Wmissing-prototypes
+EXTRA_CFLAGS += -DLINUX_KERNEL $(DIAGS)
+
+
+ifeq ($(CONFIG_MXC_SAHARA_POLL_MODE),y)
+EXTRA_CFLAGS += -DSAHARA_POLL_MODE
+EXTRA_CFLAGS += -DSAHARA_POLL_MODE_TIMEOUT=$(CONFIG_SAHARA_POLL_MODE_TIMEOUT)
+endif
+
+ifeq ($(CONFIG_MXC_SAHARA_USER_MODE),y)
+EXTRA_CFLAGS += -DSAHARA_USER_MODE
+SOURCES +=
+endif
+
+ifeq ($(CONFIG_PM),y)
+EXTRA_CFLAGS += -DSAHARA_POWER_MANAGMENT
+endif
+
+EXTRA_CFLAGS += -Idrivers/mxc/security/sahara2/include
+
+# handle buggy BSP -- uncomment if these are undefined during build
+#EXTRA_CFLAGS += -DSAHARA_BASE_ADDR=HAC_BASE_ADDR -DINT_SAHARA=INT_HAC_RTIC
+
+
+obj-$(CONFIG_MXC_SAHARA) += sahara.o
+
+sahara-objs := $(SOURCES:.c=.o) $(API_SOURCES:.c=.o)
diff --git a/drivers/mxc/security/sahara2/fsl_shw_auth.c b/drivers/mxc/security/sahara2/fsl_shw_auth.c
new file mode 100644
index 000000000000..d3100f01380a
--- /dev/null
+++ b/drivers/mxc/security/sahara2/fsl_shw_auth.c
@@ -0,0 +1,706 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/*!
+ * @file fsl_shw_auth.c
+ *
+ * This file contains the routines which do the combined encrypt+authentication
+ * functions.  For now, only AES-CCM is supported.
+ */
+
+#include "sahara.h"
+#include "adaptor.h"
+#include "sf_util.h"
+
+#ifdef __KERNEL__
+EXPORT_SYMBOL(fsl_shw_gen_encrypt);
+EXPORT_SYMBOL(fsl_shw_auth_decrypt);
+#endif
+
+
+/*! Size of buffer to repetively sink useless CBC output */
+#define CBC_BUF_LEN 4096
+
+/*!
+ * Compute the size, in bytes, of the encoded auth length
+ *
+ * @param l    The actual associated data length
+ *
+ * @return The encoded length
+ */
+#define COMPUTE_NIST_AUTH_LEN_SIZE(l)                                         \
+({                                                                            \
+    unsigned val;                                                             \
+    uint32_t len = l;                                                         \
+    if (len == 0) {                                                           \
+        val = 0;                                                              \
+    } else if (len < 65280) {                                                 \
+        val = 2;                                                              \
+    } else {                    /* cannot handle >= 2^32 */                   \
+        val = 6;                                                              \
+    }                                                                         \
+    val;                                                                      \
+})
+
+/*!
+ * Store the encoded Auth Length into the Auth Data
+ *
+ * @param l    The actual Auth Length
+ * @param p    Location to store encoding (must be uint8_t*)
+ *
+ * @return void
+ */
+#define STORE_NIST_AUTH_LEN(l, p)                                             \
+{                                                                             \
+    register uint32_t L = l;                                                  \
+    if ((uint32_t)(l) < 65280) {                                              \
+        (p)[1] = L & 0xff;                                                    \
+        L >>= 8;                                                              \
+        (p)[0] = L & 0xff;                                                    \
+    } else {                    /* cannot handle >= 2^32 */                   \
+        int i;                                                                \
+        for (i = 5; i > 1; i--) {                                             \
+            (p)[i] = L & 0xff;                                                \
+            L >>= 8;                                                          \
+        }                                                                     \
+        (p)[1] = 0xfe;  /* Markers */                                         \
+        (p)[0] = 0xff;                                                        \
+    }                                                                         \
+}
+
+#if defined (FSL_HAVE_SAHARA2) || defined (USE_S2_CCM_DECRYPT_CHAIN)          \
+    || defined (USE_S2_CCM_ENCRYPT_CHAIN)
+/*! Buffer to repetively sink useless CBC output */
+static uint8_t cbc_buffer[CBC_BUF_LEN];
+#endif
+
+/*!
+ * Place to store useless output (while bumping CTR0 to CTR1, for instance.
+ * Must be maximum Symmetric block size
+ */
+static uint8_t garbage_output[16];
+
+/*!
+ * Block of zeroes which is maximum Symmetric block size, used for
+ * initializing context register, etc.
+ */
+static uint8_t block_zeros[16] = {
+	0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0
+};
+
+/*!
+ * Append a descriptor chain which will compute CBC over the
+ * formatted associated data blocks.
+ *
+ * @param[in,out] link1       Where to append the new link
+ * @param[in,out] data_len    Location of current/updated auth-only data length
+ * @param         user_ctx    Info for acquiring memory
+ * @param         auth_ctx    Location of block0 value
+ * @param         auth_data   Unformatted associated data
+ * @param         auth_data_length Length in octets of @a auth_data
+ * @param[in,out] temp_buf    Location of in-process data.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static inline fsl_shw_return_t process_assoc_from_nist_params(sah_Link ** link1,
+							      uint32_t *
+							      data_len,
+							      fsl_shw_uco_t *
+							      user_ctx,
+							      fsl_shw_acco_t *
+							      auth_ctx,
+							      const uint8_t *
+							      auth_data,
+							      uint32_t
+							      auth_data_length,
+							      uint8_t **
+							      temp_buf)
+{
+	fsl_shw_return_t status;
+	uint32_t auth_size_length =
+	    COMPUTE_NIST_AUTH_LEN_SIZE(auth_data_length);
+	uint32_t auth_pad_length =
+	    auth_ctx->auth_info.CCM_ctx_info.block_size_bytes -
+	    (auth_data_length +
+	     auth_size_length) %
+	    auth_ctx->auth_info.CCM_ctx_info.block_size_bytes;
+
+	if (auth_pad_length ==
+	    auth_ctx->auth_info.CCM_ctx_info.block_size_bytes) {
+		auth_pad_length = 0;
+	}
+
+	/* Put in Block0 */
+	status = sah_Create_Link(user_ctx->mem_util, link1,
+				 auth_ctx->auth_info.CCM_ctx_info.context,
+				 auth_ctx->auth_info.CCM_ctx_info.
+				 block_size_bytes, SAH_USES_LINK_DATA);
+
+	if (auth_data_length != 0) {
+		if (status == FSL_RETURN_OK_S) {
+			/* Add on length preamble to auth data */
+			STORE_NIST_AUTH_LEN(auth_data_length, *temp_buf);
+			status = sah_Append_Link(user_ctx->mem_util, *link1,
+						 *temp_buf, auth_size_length,
+						 SAH_OWNS_LINK_DATA);
+			*temp_buf += auth_size_length;	/* 2, 6, or 10 bytes */
+		}
+
+		if (status == FSL_RETURN_OK_S) {
+			/* Add in auth data */
+			status = sah_Append_Link(user_ctx->mem_util, *link1,
+						 (uint8_t *) auth_data,
+						 auth_data_length,
+						 SAH_USES_LINK_DATA);
+		}
+
+		if ((status == FSL_RETURN_OK_S) && (auth_pad_length > 0)) {
+			status = sah_Append_Link(user_ctx->mem_util, *link1,
+						 block_zeros, auth_pad_length,
+						 SAH_USES_LINK_DATA);
+		}
+	}
+	/* ... if auth_data_length != 0 */
+	*data_len = auth_ctx->auth_info.CCM_ctx_info.block_size_bytes +
+	    auth_data_length + auth_size_length + auth_pad_length;
+
+	return status;
+}				/* end fn process_assoc_from_nist_params */
+
+/*!
+ * Add a Descriptor which will process with CBC the NIST preamble data
+ *
+ * @param     desc_chain   Current chain
+ * @param     user_ctx     User's context
+ * @param     auth_ctx     Inf
+ * @pararm    encrypt      0 => decrypt, non-zero => encrypt
+ * @param     auth_data    Additional auth data for this call
+ * @param     auth_data_length   Length in bytes of @a auth_data
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static inline fsl_shw_return_t add_assoc_preamble(sah_Head_Desc ** desc_chain,
+						  fsl_shw_uco_t * user_ctx,
+						  fsl_shw_acco_t * auth_ctx,
+						  int encrypt,
+						  const uint8_t * auth_data,
+						  uint32_t auth_data_length)
+{
+	uint8_t *temp_buf;
+	sah_Link *link1 = NULL;
+	sah_Link *link2 = NULL;
+	fsl_shw_return_t status = FSL_RETURN_OK_S;
+	uint32_t cbc_data_length = 0;
+	/* Assume AES */
+	uint32_t header = SAH_HDR_SKHA_ENC_DEC;
+	uint32_t temp_buf_flag;
+	unsigned chain_s2 = 1;
+
+#if defined (FSL_HAVE_SAHARA4) && !defined (USE_S2_CCM_DECRYPT_CHAIN)
+	if (!encrypt) {
+		chain_s2 = 0;
+	}
+#endif
+#if defined (FSL_HAVE_SAHARA4) && !defined (USE_S2_CCM_ENCRYPT_CHAIN)
+	if (encrypt) {
+		chain_s2 = 0;
+	}
+#endif
+	/* Grab a block big enough for multiple uses so that only one allocate
+	 * request needs to be made.
+	 */
+	temp_buf =
+	    user_ctx->mem_util->mu_malloc(user_ctx->mem_util->mu_ref,
+					  3 *
+					  auth_ctx->auth_info.CCM_ctx_info.
+					  block_size_bytes);
+
+	if (temp_buf == NULL) {
+		status = FSL_RETURN_NO_RESOURCE_S;
+		goto out;
+	}
+
+	if (auth_ctx->flags & FSL_ACCO_NIST_CCM) {
+		status = process_assoc_from_nist_params(&link1,
+							&cbc_data_length,
+							user_ctx,
+							auth_ctx,
+							auth_data,
+							auth_data_length,
+							&temp_buf);
+		if (status != FSL_RETURN_OK_S) {
+			goto out;
+		}
+		/* temp_buf has been referenced (and incremented).  Only 'own' it
+		 * once, at its first value.  Since the nist routine called above
+		 * bumps it...
+		 */
+		temp_buf_flag = SAH_USES_LINK_DATA;
+	} else {		/* if NIST */
+		status = sah_Create_Link(user_ctx->mem_util, &link1,
+					 (uint8_t *) auth_data,
+					 auth_data_length, SAH_USES_LINK_DATA);
+		if (status != FSL_RETURN_OK_S) {
+			goto out;
+		}
+		/* for next/first use of temp_buf */
+		temp_buf_flag = SAH_OWNS_LINK_DATA;
+		cbc_data_length = auth_data_length;
+	}			/* else not NIST */
+
+#if defined (FSL_HAVE_SAHARA2) || defined (USE_S2_CCM_ENCRYPT_CHAIN)   \
+    || defined (USE_S2_CCM_DECRYPT_CHAIN)
+
+	if (!chain_s2) {
+		header = SAH_HDR_SKHA_CBC_ICV
+		    ^ sah_insert_skha_mode_cbc ^ sah_insert_skha_aux0
+		    ^ sah_insert_skha_encrypt;
+	} else {
+		/*
+		 * Auth data links have been created.  Now create link for the
+		 * useless output of the CBC calculation.
+		 */
+		status = sah_Create_Link(user_ctx->mem_util, &link2,
+					 temp_buf,
+					 auth_ctx->auth_info.CCM_ctx_info.
+					 block_size_bytes,
+					 temp_buf_flag | SAH_OUTPUT_LINK);
+		if (status != FSL_RETURN_OK_S) {
+			goto out;
+		}
+
+		temp_buf += auth_ctx->auth_info.CCM_ctx_info.block_size_bytes;
+
+		cbc_data_length -=
+		    auth_ctx->auth_info.CCM_ctx_info.block_size_bytes;
+		if (cbc_data_length != 0) {
+			while ((status == FSL_RETURN_OK_S)
+			       && (cbc_data_length != 0)) {
+				uint32_t linklen =
+				    (cbc_data_length >
+				     CBC_BUF_LEN) ? CBC_BUF_LEN :
+				    cbc_data_length;
+
+				status =
+				    sah_Append_Link(user_ctx->mem_util, link2,
+						    cbc_buffer, linklen,
+						    SAH_USES_LINK_DATA |
+						    SAH_OUTPUT_LINK);
+				if (status != FSL_RETURN_OK_S) {
+					goto out;
+				}
+				cbc_data_length -= linklen;
+			}
+		}
+	}
+#else
+	header = SAH_HDR_SKHA_CBC_ICV
+	    ^ sah_insert_skha_mode_cbc ^ sah_insert_skha_aux0
+	    ^ sah_insert_skha_encrypt;
+#endif
+	/* Crank through auth data */
+	status = sah_Append_Desc(user_ctx->mem_util, desc_chain,
+				 header, link1, link2);
+
+      out:
+	if (status != FSL_RETURN_OK_S) {
+		if (link1 != NULL) {
+			sah_Destroy_Link(user_ctx->mem_util, link1);
+		}
+		if (link2 != NULL) {
+			sah_Destroy_Link(user_ctx->mem_util, link2);
+		}
+	}
+
+	(void)encrypt;
+	return status;
+}				/* add_assoc_preamble() */
+
+#if SUPPORT_SSL
+/*!
+ * Generate an SSL value
+ *
+ * @param         user_ctx         Info for acquiring memory
+ * @param         auth_ctx         Info for CTR0, size of MAC
+ * @param         cipher_key_info
+ * @param         auth_key_info
+ * @param         auth_data_length
+ * @param         auth_data
+ * @param         payload_length
+ * @param         payload
+ * @param         ct
+ * @param         auth_value
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static fsl_shw_return_t do_ssl_gen(fsl_shw_uco_t * user_ctx,
+				   fsl_shw_acco_t * auth_ctx,
+				   fsl_shw_sko_t * cipher_key_info,
+				   fsl_shw_sko_t * auth_key_info,
+				   uint32_t auth_data_length,
+				   const uint8_t * auth_data,
+				   uint32_t payload_length,
+				   const uint8_t * payload,
+				   uint8_t * ct, uint8_t * auth_value)
+{
+	SAH_SF_DCLS;
+	uint8_t *ptr1 = NULL;
+
+	/* Assume one-shot init-finalize... no precomputes */
+	header = SAH_HDR_MDHA_SET_MODE_MD_KEY ^
+	    sah_insert_mdha_algorithm[auth_ctx->auth_info.hash_ctx_info.
+				      algorithm] ^ sah_insert_mdha_init ^
+	    sah_insert_mdha_ssl ^ sah_insert_mdha_pdata ^
+	    sah_insert_mdha_mac_full;
+
+	/* set up hmac */
+	DESC_IN_KEY(header, 0, NULL, auth_key_info);
+
+	/* This is wrong -- need to find 16 extra bytes of data from
+	 * somewhere */
+	DESC_IN_OUT(SAH_HDR_MDHA_HASH, payload_length, payload, 1, auth_value);
+
+	/* set up encrypt */
+	header = SAH_HDR_SKHA_SET_MODE_IV_KEY
+	    ^ sah_insert_skha_mode[auth_ctx->cipher_ctx_info.mode]
+	    ^ sah_insert_skha_encrypt
+	    ^ sah_insert_skha_algorithm[cipher_key_info->algorithm];
+
+	/* Honor 'no key parity checking' for DES and TDES */
+	if ((cipher_key_info->flags & FSL_SKO_KEY_IGNORE_PARITY) &&
+	    ((cipher_key_info->algorithm == FSL_KEY_ALG_DES) ||
+	     (cipher_key_info->algorithm == FSL_KEY_ALG_TDES))) {
+		header ^= sah_insert_skha_no_key_parity;
+	}
+
+	if (auth_ctx->cipher_ctx_info.mode == FSL_SYM_MODE_CTR) {
+		header ^=
+		    sah_insert_skha_modulus[auth_ctx->cipher_ctx_info.
+					    modulus_exp];
+	}
+
+	if ((auth_ctx->cipher_ctx_info.mode == FSL_SYM_MODE_ECB)
+	    || (auth_ctx->cipher_ctx_info.flags & FSL_SYM_CTX_INIT)) {
+		ptr1 = block_zeros;
+	} else {
+		ptr1 = auth_ctx->cipher_ctx_info.context;
+	}
+
+	DESC_IN_KEY(header, auth_ctx->cipher_ctx_info.block_size_bytes, ptr1,
+		    cipher_key_info);
+
+	/* This is wrong -- need to find 16 extra bytes of data from
+	 * somewhere...
+	 */
+	if (payload_length != 0) {
+		DESC_IN_OUT(SAH_HDR_SKHA_ENC_DEC,
+			    payload_length, payload, payload_length, ct);
+	}
+
+	SAH_SF_EXECUTE();
+
+      out:
+	SAH_SF_DESC_CLEAN();
+
+	/* Eliminate compiler warnings until full implementation... */
+	(void)auth_data;
+	(void)auth_data_length;
+
+	return ret;
+}				/* do_ssl_gen() */
+#endif
+
+/*!
+ * @brief Generate a (CCM) auth code and encrypt the payload.
+ *
+ * This is a very complicated function.  Seven (or eight) descriptors are
+ * required to perform a CCM calculation.
+ *
+ * First:  Load CTR0 and key.
+ *
+ * Second: Run an octet of data through to bump to CTR1.  (This could be
+ * done in software, but software will have to bump and later decrement -
+ * or copy and bump.
+ *
+ * Third: (in Virtio) Load a descriptor with data of zeros for CBC IV.
+ *
+ * Fourth: Run any (optional) "additional data" through the CBC-mode
+ * portion of the algorithm.
+ *
+ * Fifth: Run the payload through in CCM mode.
+ *
+ * Sixth: Extract the unencrypted MAC.
+ *
+ * Seventh: Load CTR0.
+ *
+ * Eighth: Encrypt the MAC.
+ *
+ * @param user_ctx         The user's context
+ * @param auth_ctx         Info on this Auth operation
+ * @param cipher_key_info  Key to encrypt payload
+ * @param auth_key_info    (unused - same key in CCM)
+ * @param auth_data_length Length in bytes of @a auth_data
+ * @param auth_data        Any auth-only data
+ * @param payload_length   Length in bytes of @a payload
+ * @param payload          The data to encrypt
+ * @param[out] ct          The location to store encrypted data
+ * @param[out] auth_value  The location to store authentication code
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_gen_encrypt(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_acco_t * auth_ctx,
+				     fsl_shw_sko_t * cipher_key_info,
+				     fsl_shw_sko_t * auth_key_info,
+				     uint32_t auth_data_length,
+				     const uint8_t * auth_data,
+				     uint32_t payload_length,
+				     const uint8_t * payload,
+				     uint8_t * ct, uint8_t * auth_value)
+{
+	SAH_SF_DCLS;
+
+	SAH_SF_USER_CHECK();
+
+	if (auth_ctx->mode == FSL_ACC_MODE_SSL) {
+#if SUPPORT_SSL
+		ret = do_ssl_gen(user_ctx, auth_ctx, cipher_key_info,
+				 auth_key_info, auth_data_length, auth_data,
+				 payload_length, payload, ct, auth_value);
+#else
+		ret = FSL_RETURN_BAD_MODE_S;
+#endif
+		goto out;
+	}
+
+	if (auth_ctx->mode != FSL_ACC_MODE_CCM) {
+		ret = FSL_RETURN_BAD_MODE_S;
+		goto out;
+	}
+
+	/* Only support INIT and FINALIZE flags right now. */
+	if ((auth_ctx->flags & (FSL_ACCO_CTX_INIT | FSL_ACCO_CTX_LOAD |
+				FSL_ACCO_CTX_SAVE | FSL_ACCO_CTX_FINALIZE))
+	    != (FSL_ACCO_CTX_INIT | FSL_ACCO_CTX_FINALIZE)) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	/* Load CTR0 and Key */
+	header = (SAH_HDR_SKHA_SET_MODE_IV_KEY
+		  ^ sah_insert_skha_mode_ctr
+		  ^ sah_insert_skha_modulus_128 ^ sah_insert_skha_encrypt);
+	DESC_IN_KEY(header,
+		    auth_ctx->cipher_ctx_info.block_size_bytes,
+		    auth_ctx->cipher_ctx_info.context, cipher_key_info);
+
+	/* Encrypt dummy data to bump to CTR1 */
+	header = SAH_HDR_SKHA_ENC_DEC;
+	DESC_IN_OUT(header, auth_ctx->mac_length, garbage_output,
+		    auth_ctx->mac_length, garbage_output);
+
+#if defined(FSL_HAVE_SAHARA2) || defined(USE_S2_CCM_ENCRYPT_CHAIN)
+#ifndef NO_ZERO_IV_LOAD
+	header = (SAH_HDR_SKHA_SET_MODE_IV_KEY
+		  ^ sah_insert_skha_encrypt ^ sah_insert_skha_mode_cbc);
+	DESC_IN_IN(header,
+		   auth_ctx->auth_info.CCM_ctx_info.block_size_bytes,
+		   block_zeros, 0, NULL);
+#endif
+#endif
+
+	ret = add_assoc_preamble(&desc_chain, user_ctx,
+				 auth_ctx, 1, auth_data, auth_data_length);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Process the payload */
+	header = (SAH_HDR_SKHA_SET_MODE_ENC_DEC
+		  ^ sah_insert_skha_mode_ccm
+		  ^ sah_insert_skha_modulus_128 ^ sah_insert_skha_encrypt);
+#if defined (FSL_HAVE_SAHARA4) && !defined (USE_S2_CCM_ENCRYPT_CHAIN)
+	header ^= sah_insert_skha_aux0;
+#endif
+	if (payload_length != 0) {
+		DESC_IN_OUT(header, payload_length, payload, payload_length,
+			    ct);
+	} else {
+		DESC_IN_OUT(header, 0, NULL, 0, NULL);
+	}			/* if payload_length */
+
+#if defined (FSL_HAVE_SAHARA4) && !defined (USE_S2_CCM_ENCRYPT_CHAIN)
+	/* Pull out the CBC-MAC value. */
+	DESC_OUT_OUT(SAH_HDR_SKHA_READ_CONTEXT_IV, 0, NULL,
+		     auth_ctx->mac_length, auth_value);
+#else
+	/* Pull out the unencrypted CBC-MAC value. */
+	DESC_OUT_OUT(SAH_HDR_SKHA_READ_CONTEXT_IV,
+		     0, NULL, auth_ctx->mac_length, auth_ctx->unencrypted_mac);
+
+	/* Now load CTR0 in, and encrypt the MAC */
+	header = SAH_HDR_SKHA_SET_MODE_IV_KEY
+	    ^ sah_insert_skha_encrypt
+	    ^ sah_insert_skha_mode_ctr ^ sah_insert_skha_modulus_128;
+	DESC_IN_IN(header,
+		   auth_ctx->cipher_ctx_info.block_size_bytes,
+		   auth_ctx->cipher_ctx_info.context, 0, NULL);
+
+	header = SAH_HDR_SKHA_ENC_DEC;	/* Desc. #4 SKHA Enc/Dec */
+	DESC_IN_OUT(header,
+		    auth_ctx->mac_length, auth_ctx->unencrypted_mac,
+		    auth_ctx->mac_length, auth_value);
+#endif
+
+	SAH_SF_EXECUTE();
+
+      out:
+	SAH_SF_DESC_CLEAN();
+
+	(void)auth_key_info;
+	return ret;
+}				/* fsl_shw_gen_encrypt() */
+
+/*!
+ * @brief Authenticate and decrypt a (CCM) stream.
+ *
+ * @param user_ctx         The user's context
+ * @param auth_ctx         Info on this Auth operation
+ * @param cipher_key_info  Key to encrypt payload
+ * @param auth_key_info    (unused - same key in CCM)
+ * @param auth_data_length Length in bytes of @a auth_data
+ * @param auth_data        Any auth-only data
+ * @param payload_length   Length in bytes of @a payload
+ * @param ct               The encrypted data
+ * @param auth_value       The authentication code to validate
+ * @param[out] payload     The location to store decrypted data
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_auth_decrypt(fsl_shw_uco_t * user_ctx,
+				      fsl_shw_acco_t * auth_ctx,
+				      fsl_shw_sko_t * cipher_key_info,
+				      fsl_shw_sko_t * auth_key_info,
+				      uint32_t auth_data_length,
+				      const uint8_t * auth_data,
+				      uint32_t payload_length,
+				      const uint8_t * ct,
+				      const uint8_t * auth_value,
+				      uint8_t * payload)
+{
+	SAH_SF_DCLS;
+#if defined(FSL_HAVE_SAHARA2) || defined(USE_S2_CCM_DECRYPT_CHAIN)
+	uint8_t *calced_auth = NULL;
+	unsigned blocking = user_ctx->flags & FSL_UCO_BLOCKING_MODE;
+#endif
+
+	SAH_SF_USER_CHECK();
+
+	/* Only support CCM */
+	if (auth_ctx->mode != FSL_ACC_MODE_CCM) {
+		ret = FSL_RETURN_BAD_MODE_S;
+		goto out;
+	}
+	/* Only support INIT and FINALIZE flags right now. */
+	if ((auth_ctx->flags & (FSL_ACCO_CTX_INIT | FSL_ACCO_CTX_LOAD |
+				FSL_ACCO_CTX_SAVE | FSL_ACCO_CTX_FINALIZE))
+	    != (FSL_ACCO_CTX_INIT | FSL_ACCO_CTX_FINALIZE)) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	/* Load CTR0 and Key */
+	header = SAH_HDR_SKHA_SET_MODE_IV_KEY
+	    ^ sah_insert_skha_mode_ctr ^ sah_insert_skha_modulus_128;
+#if defined (FSL_HAVE_SAHARA4) && !defined (USE_S2_CCM_DECRYPT_CHAIN)
+	header ^= sah_insert_skha_aux0;
+#endif
+	DESC_IN_KEY(header,
+		    auth_ctx->cipher_ctx_info.block_size_bytes,
+		    auth_ctx->cipher_ctx_info.context, cipher_key_info);
+
+	/* Decrypt the MAC which the user passed in */
+	header = SAH_HDR_SKHA_ENC_DEC;
+	DESC_IN_OUT(header,
+		    auth_ctx->mac_length, auth_value,
+		    auth_ctx->mac_length, auth_ctx->unencrypted_mac);
+
+#if defined(FSL_HAVE_SAHARA2) || defined(USE_S2_CCM_DECRYPT_CHAIN)
+#ifndef NO_ZERO_IV_LOAD
+	header = (SAH_HDR_SKHA_SET_MODE_IV_KEY
+		  ^ sah_insert_skha_encrypt ^ sah_insert_skha_mode_cbc);
+	DESC_IN_IN(header,
+		   auth_ctx->auth_info.CCM_ctx_info.block_size_bytes,
+		   block_zeros, 0, NULL);
+#endif
+#endif
+
+	ret = add_assoc_preamble(&desc_chain, user_ctx,
+				 auth_ctx, 0, auth_data, auth_data_length);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Process the payload */
+	header = (SAH_HDR_SKHA_SET_MODE_ENC_DEC
+		  ^ sah_insert_skha_mode_ccm ^ sah_insert_skha_modulus_128);
+#if defined (FSL_HAVE_SAHARA4) && !defined (USE_S2_CCM_DECRYPT_CHAIN)
+	header ^= sah_insert_skha_aux0;
+#endif
+	if (payload_length != 0) {
+		DESC_IN_OUT(header, payload_length, ct, payload_length,
+			    payload);
+	} else {
+		DESC_IN_OUT(header, 0, NULL, 0, NULL);
+	}
+
+#if defined (FSL_HAVE_SAHARA2) || defined (USE_S2_CCM_DECRYPT_CHAIN)
+	/* Now pull CBC context (unencrypted MAC) out for comparison. */
+	/* Need to allocate a place for it, to handle non-blocking mode
+	 * when this stack frame will disappear!
+	 */
+	calced_auth = DESC_TEMP_ALLOC(auth_ctx->mac_length);
+	header = SAH_HDR_SKHA_READ_CONTEXT_IV;
+	DESC_OUT_OUT(header, 0, NULL, auth_ctx->mac_length, calced_auth);
+	if (!blocking) {
+		/* get_results will need this for comparison */
+		desc_chain->out1_ptr = calced_auth;
+		desc_chain->out2_ptr = auth_ctx->unencrypted_mac;
+		desc_chain->out_len = auth_ctx->mac_length;
+	}
+#endif
+
+	SAH_SF_EXECUTE();
+
+#if defined (FSL_HAVE_SAHARA2) || defined (USE_S2_CCM_DECRYPT_CHAIN)
+	if (blocking && (ret == FSL_RETURN_OK_S)) {
+		unsigned i;
+		/* Validate the auth code */
+		for (i = 0; i < auth_ctx->mac_length; i++) {
+			if (calced_auth[i] != auth_ctx->unencrypted_mac[i]) {
+				ret = FSL_RETURN_AUTH_FAILED_S;
+				break;
+			}
+		}
+	}
+#endif
+
+      out:
+	SAH_SF_DESC_CLEAN();
+#if defined (FSL_HAVE_SAHARA2) || defined (USE_S2_CCM_DECRYPT_CHAIN)
+	DESC_TEMP_FREE(calced_auth);
+#endif
+
+	(void)auth_key_info;
+	return ret;
+}				/* fsl_shw_gen_decrypt() */
diff --git a/drivers/mxc/security/sahara2/fsl_shw_hash.c b/drivers/mxc/security/sahara2/fsl_shw_hash.c
new file mode 100644
index 000000000000..1551173c1c62
--- /dev/null
+++ b/drivers/mxc/security/sahara2/fsl_shw_hash.c
@@ -0,0 +1,186 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_shw_hash.c
+ *
+ * This file implements Cryptographic Hashing functions of the FSL SHW API
+ * for Sahara.  This does not include HMAC.
+ */
+
+#include "sahara.h"
+#include "sf_util.h"
+
+#ifdef LINUX_KERNEL
+EXPORT_SYMBOL(fsl_shw_hash);
+#endif
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HASH-005 */
+/*!
+ * Hash a stream of data with a cryptographic hash algorithm.
+ *
+ * The flags in the @a hash_ctx control the operation of this function.
+ *
+ * Hashing functions work on 64 octets of message at a time.  Therefore, when
+ * any partial hashing of a long message is performed, the message @a length of
+ * each segment must be a multiple of 64.  When ready to
+ * #FSL_HASH_FLAGS_FINALIZE the hash, the @a length may be any value.
+ *
+ * With the #FSL_HASH_FLAGS_INIT and #FSL_HASH_FLAGS_FINALIZE flags on, a
+ * one-shot complete hash, including padding, will be performed.  The @a length
+ * may be any value.
+ *
+ * The first octets of a data stream can be hashed by setting the
+ * #FSL_HASH_FLAGS_INIT and #FSL_HASH_FLAGS_SAVE flags.  The @a length must be
+ * a multiple of 64.
+ *
+ * The flag #FSL_HASH_FLAGS_LOAD is used to load a context previously saved by
+ * #FSL_HASH_FLAGS_SAVE.  The two in combination will allow a (multiple-of-64
+ * octets) 'middle sequence' of the data stream to be hashed with the
+ * beginning.  The @a length must again be a multiple of 64.
+ *
+ * Since the flag #FSL_HASH_FLAGS_LOAD is used to load a context previously
+ * saved by #FSL_HASH_FLAGS_SAVE, the #FSL_HASH_FLAGS_LOAD and
+ * #FSL_HASH_FLAGS_FINALIZE flags, used together, can be used to finish the
+ * stream.  The @a length may be any value.
+ *
+ * If the user program wants to do the padding for the hash, it can leave off
+ * the #FSL_HASH_FLAGS_FINALIZE flag.  The @a length must then be a multiple of
+ * 64 octets.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param[in,out] hash_ctx Hashing algorithm and state of the cipher.
+ * @param      msg       Pointer to the data to be hashed.
+ * @param      length    Length, in octets, of the @a msg.
+ * @param[out] result    If not null, pointer to where to store the hash
+ *                       digest.
+ * @param      result_len Number of octets to store in @a result.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_hash(fsl_shw_uco_t * user_ctx,
+			      fsl_shw_hco_t * hash_ctx,
+			      const uint8_t * msg,
+			      uint32_t length,
+			      uint8_t * result, uint32_t result_len)
+{
+	SAH_SF_DCLS;
+	unsigned ctx_flags = (hash_ctx->flags & (FSL_HASH_FLAGS_INIT
+						 | FSL_HASH_FLAGS_LOAD
+						 | FSL_HASH_FLAGS_SAVE
+						 | FSL_HASH_FLAGS_FINALIZE));
+
+	SAH_SF_USER_CHECK();
+
+	/* Reset expectations if user gets overly zealous. */
+	if (result_len > hash_ctx->digest_length) {
+		result_len = hash_ctx->digest_length;
+	}
+
+	/* Validate hash ctx flags.
+	 * Need INIT or LOAD but not both.
+	 * Need SAVE or digest ptr (both is ok).
+	 */
+	if (((ctx_flags & (FSL_HASH_FLAGS_INIT | FSL_HASH_FLAGS_LOAD))
+	     == (FSL_HASH_FLAGS_INIT | FSL_HASH_FLAGS_LOAD))
+	    || ((ctx_flags & (FSL_HASH_FLAGS_INIT | FSL_HASH_FLAGS_LOAD)) == 0)
+	    || (!(ctx_flags & FSL_HASH_FLAGS_SAVE) && (result == NULL))) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	if (ctx_flags & FSL_HASH_FLAGS_INIT) {
+		sah_Oct_Str out_ptr;
+		unsigned out_len;
+
+		/* Create desc to perform the initial hashing operation */
+		/* Desc. #8 w/INIT and algorithm */
+		header = SAH_HDR_MDHA_SET_MODE_HASH
+		    ^ sah_insert_mdha_init
+		    ^ sah_insert_mdha_algorithm[hash_ctx->algorithm];
+
+		/* If user wants one-shot, set padding operation. */
+		if (ctx_flags & FSL_HASH_FLAGS_FINALIZE) {
+			header ^= sah_insert_mdha_pdata;
+		}
+
+		/* Determine where Digest will go - hash_ctx or result */
+		if (ctx_flags & FSL_HASH_FLAGS_SAVE) {
+			out_ptr = (sah_Oct_Str) hash_ctx->context;
+			out_len = hash_ctx->context_register_length;
+		} else {
+			out_ptr = result;
+			out_len = (result_len > hash_ctx->digest_length)
+			    ? hash_ctx->digest_length : result_len;
+		}
+
+		DESC_IN_OUT(header, length, (sah_Oct_Str) msg, out_len,
+			    out_ptr);
+	} else {		/* not doing hash INIT */
+		void *out_ptr;
+		unsigned out_len;
+
+		/*
+		 * Build two descriptors -- one to load in context/set mode, the
+		 * other to compute & retrieve hash/context value.
+		 *
+		 * First up - Desc. #6 to load context.
+		 */
+		/* Desc. #8 w/algorithm */
+		header = SAH_HDR_MDHA_SET_MODE_MD_KEY
+		    ^ sah_insert_mdha_algorithm[hash_ctx->algorithm];
+
+		if (ctx_flags & FSL_HASH_FLAGS_FINALIZE) {
+			header ^= sah_insert_mdha_pdata;
+		}
+
+		/* Message Digest (in) */
+		DESC_IN_IN(header,
+			   hash_ctx->context_register_length,
+			   (sah_Oct_Str) hash_ctx->context, 0, NULL);
+
+		if (ctx_flags & FSL_HASH_FLAGS_SAVE) {
+			out_ptr = hash_ctx->context;
+			out_len = hash_ctx->context_register_length;
+		} else {
+			out_ptr = result;
+			out_len = result_len;
+		}
+
+		/* Second -- run data through and retrieve ctx regs */
+		/* Desc. #10 - no mode register with this. */
+		header = SAH_HDR_MDHA_HASH;
+		DESC_IN_OUT(header, length, (sah_Oct_Str) msg, out_len,
+			    out_ptr);
+	}			/* else not INIT */
+
+	/* Now that execution is rejoined, we can append another descriptor
+	   to extract the digest/context a second time, into the result. */
+	if ((ctx_flags & FSL_HASH_FLAGS_SAVE)
+	    && (result != NULL) && (result_len != 0)) {
+
+		header = SAH_HDR_MDHA_STORE_DIGEST;
+
+		/* Message Digest (out) */
+		DESC_IN_OUT(header, 0, NULL,
+			    (result_len > hash_ctx->digest_length)
+			    ? hash_ctx->digest_length : result_len, result);
+	}
+
+	SAH_SF_EXECUTE();
+
+      out:
+	SAH_SF_DESC_CLEAN();
+
+	return ret;
+}				/* fsl_shw_hash() */
diff --git a/drivers/mxc/security/sahara2/fsl_shw_hmac.c b/drivers/mxc/security/sahara2/fsl_shw_hmac.c
new file mode 100644
index 000000000000..4184d9b280dd
--- /dev/null
+++ b/drivers/mxc/security/sahara2/fsl_shw_hmac.c
@@ -0,0 +1,266 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_shw_hmac.c
+ *
+ * This file implements Hashed Message Authentication Code functions of the FSL
+ * SHW API for Sahara.
+ */
+
+#include "sahara.h"
+#include "sf_util.h"
+
+#ifdef __KERNEL__
+EXPORT_SYMBOL(fsl_shw_hmac_precompute);
+EXPORT_SYMBOL(fsl_shw_hmac);
+#endif
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HMAC-001 */
+/*!
+ * Get the precompute information
+ *
+ *
+ * @param   user_ctx
+ * @param   key_info
+ * @param   hmac_ctx
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_hmac_precompute(fsl_shw_uco_t * user_ctx,
+					 fsl_shw_sko_t * key_info,
+					 fsl_shw_hmco_t * hmac_ctx)
+{
+	SAH_SF_DCLS;
+
+	SAH_SF_USER_CHECK();
+
+	if ((key_info->algorithm != FSL_KEY_ALG_HMAC) ||
+	    (key_info->key_length > 64)) {
+		return FSL_RETURN_BAD_ALGORITHM_S;
+	} else if (key_info->key_length == 0) {
+		return FSL_RETURN_BAD_KEY_LENGTH_S;
+	}
+
+	/* Set up to start the Inner Calculation */
+	/* Desc. #8 w/IPAD, & INIT */
+	header = SAH_HDR_MDHA_SET_MODE_HASH
+	    ^ sah_insert_mdha_ipad
+	    ^ sah_insert_mdha_init
+	    ^ sah_insert_mdha_algorithm[hmac_ctx->algorithm];
+
+	DESC_KEY_OUT(header, key_info,
+		     hmac_ctx->context_register_length,
+		     (uint8_t *) hmac_ctx->inner_precompute);
+
+	/* Set up for starting Outer calculation */
+	/* exchange IPAD bit for OPAD bit */
+	header ^= (sah_insert_mdha_ipad ^ sah_insert_mdha_opad);
+
+	/* Theoretically, we can leave this link out and use the key which is
+	 * already in the register... however, if we do, the resulting opad
+	 * hash does not have the correct value when using the model. */
+	DESC_KEY_OUT(header, key_info,
+		     hmac_ctx->context_register_length,
+		     (uint8_t *) hmac_ctx->outer_precompute);
+
+	SAH_SF_EXECUTE();
+	if (ret == FSL_RETURN_OK_S) {
+		/* flag that precomputes have been entered in this hco
+		 * assume it'll first be used for initilizing */
+		hmac_ctx->flags |= (FSL_HMAC_FLAGS_INIT |
+				    FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT);
+	}
+
+      out:
+	SAH_SF_DESC_CLEAN();
+
+	return ret;
+}
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HMAC-002 */
+/*!
+ * Get the hmac
+ *
+ *
+ * @param         user_ctx    Info for acquiring memory
+ * @param         key_info
+ * @param         hmac_ctx
+ * @param         msg
+ * @param         length
+ * @param         result
+ * @param         result_len
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_hmac(fsl_shw_uco_t * user_ctx,
+			      fsl_shw_sko_t * key_info,
+			      fsl_shw_hmco_t * hmac_ctx,
+			      const uint8_t * msg,
+			      uint32_t length,
+			      uint8_t * result, uint32_t result_len)
+{
+	SAH_SF_DCLS;
+
+	SAH_SF_USER_CHECK();
+
+	/* check flag consistency */
+	/* Note that Final, Init, and Save are an illegal combination when a key
+	 * is being used. Because of the logic flow of this routine, that is
+	 * taken care of without being explict */
+	if (
+		   /* nothing to work on */
+		   (((hmac_ctx->flags & FSL_HMAC_FLAGS_INIT) == 0) &&
+		    ((hmac_ctx->flags & FSL_HMAC_FLAGS_LOAD) == 0)) ||
+		   /* can't do both */
+		   ((hmac_ctx->flags & FSL_HMAC_FLAGS_INIT) &&
+		    (hmac_ctx->flags & FSL_HMAC_FLAGS_LOAD)) ||
+		   /* must be some output */
+		   (((hmac_ctx->flags & FSL_HMAC_FLAGS_SAVE) == 0) &&
+		    ((hmac_ctx->flags & FSL_HMAC_FLAGS_FINALIZE) == 0))) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	/* build descriptor #6 */
+
+	/* start building descriptor header */
+	header = SAH_HDR_MDHA_SET_MODE_MD_KEY ^
+	    sah_insert_mdha_algorithm[hmac_ctx->algorithm] ^
+	    sah_insert_mdha_init;
+
+	/* if this is to finalize the digest, mark to pad last block */
+	if (hmac_ctx->flags & FSL_HMAC_FLAGS_FINALIZE) {
+		header ^= sah_insert_mdha_pdata;
+	}
+
+	/* Check if this is a one shot */
+	if ((hmac_ctx->flags & FSL_HMAC_FLAGS_INIT) &&
+	    (hmac_ctx->flags & FSL_HMAC_FLAGS_FINALIZE)) {
+
+		header ^= sah_insert_mdha_hmac;
+
+		/* See if this uses Precomputes */
+		if (hmac_ctx->flags & FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT) {
+			DESC_IN_IN(header,
+				   hmac_ctx->context_register_length,
+				   (uint8_t *) hmac_ctx->inner_precompute,
+				   hmac_ctx->context_length,
+				   (uint8_t *) hmac_ctx->outer_precompute);
+		} else {	/* Precomputes not requested, try using Key */
+			if (key_info->key != NULL) {
+				/* first, validate the key fields and related flag */
+				if ((key_info->key_length == 0)
+				    || (key_info->key_length > 64)) {
+					ret = FSL_RETURN_BAD_KEY_LENGTH_S;
+					goto out;
+				} else {
+					if (key_info->algorithm !=
+					    FSL_KEY_ALG_HMAC) {
+						ret =
+						    FSL_RETURN_BAD_ALGORITHM_S;
+						goto out;
+					}
+				}
+
+				/* finish building descriptor header (Key specific) */
+				header ^= sah_insert_mdha_mac_full;
+				DESC_IN_KEY(header, 0, NULL, key_info);
+			} else {	/* not using Key or Precomputes, so die */
+				ret = FSL_RETURN_BAD_FLAG_S;
+				goto out;
+			}
+		}
+	} else {		/* it's not a one shot, must be multi-step */
+		/* this the last chunk? */
+		if (hmac_ctx->flags & FSL_HMAC_FLAGS_FINALIZE) {
+			header ^= sah_insert_mdha_hmac;
+			DESC_IN_IN(header,
+				   hmac_ctx->context_register_length,
+				   (uint8_t *) hmac_ctx->ongoing_context,
+				   hmac_ctx->context_length,
+				   (uint8_t *) hmac_ctx->outer_precompute);
+		} else {	/* not last chunk */
+			uint8_t *ptr1;
+
+			if (hmac_ctx->flags & FSL_HMAC_FLAGS_INIT) {
+				/* must be using precomputes, cannot 'chunk' message
+				 * starting with a key */
+				if (hmac_ctx->
+				    flags & FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT)
+				{
+					ptr1 =
+					    (uint8_t *) hmac_ctx->
+					    inner_precompute;
+				} else {
+					ret = FSL_RETURN_NO_RESOURCE_S;
+					goto out;
+				}
+			} else {
+				ptr1 = (uint8_t *) hmac_ctx->ongoing_context;
+			}
+
+			if (ret != FSL_RETURN_OK_S) {
+				goto out;
+			}
+			DESC_IN_IN(header,
+				   hmac_ctx->context_register_length, ptr1,
+				   0, NULL);
+		}
+	}			/* multi-step */
+
+	/* build descriptor #10 & maybe 11 */
+	header = SAH_HDR_MDHA_HASH;
+
+	if (hmac_ctx->flags & FSL_HMAC_FLAGS_FINALIZE) {
+		/* check that the results parameters seem reasonable */
+		if ((result_len != 0) && (result != NULL)) {
+			if (result_len > hmac_ctx->context_register_length) {
+				result_len = hmac_ctx->context_register_length;
+			}
+
+			/* message in / digest out (descriptor #10) */
+			DESC_IN_OUT(header, length, msg, result_len, result);
+
+			/* see if descriptor #11 needs to be built */
+			if (hmac_ctx->flags & FSL_HMAC_FLAGS_SAVE) {
+				header = SAH_HDR_MDHA_STORE_DIGEST;
+				/* nothing in / context out */
+				DESC_IN_IN(header, 0, NULL,
+					   hmac_ctx->context_register_length,
+					   (uint8_t *) hmac_ctx->
+					   ongoing_context);
+			}
+		} else {
+			/* something wrong with result or its length */
+			ret = FSL_RETURN_BAD_DATA_LENGTH_S;
+		}
+	} else {		/* finalize not set, so store in ongoing context field */
+		if ((length % 64) == 0) {	/* this will change for 384/512 support */
+			/* message in / context out */
+			DESC_IN_OUT(header, length, msg,
+				    hmac_ctx->context_register_length,
+				    (uint8_t *) hmac_ctx->ongoing_context);
+		} else {
+			/* not final data, and not multiple of block length */
+			ret = FSL_RETURN_BAD_DATA_LENGTH_S;
+		}
+	}
+
+	SAH_SF_EXECUTE();
+
+      out:
+	SAH_SF_DESC_CLEAN();
+
+	return ret;
+}				/* fsl_shw_hmac() */
diff --git a/drivers/mxc/security/sahara2/fsl_shw_keystore.c b/drivers/mxc/security/sahara2/fsl_shw_keystore.c
new file mode 100644
index 000000000000..3a8a22a81305
--- /dev/null
+++ b/drivers/mxc/security/sahara2/fsl_shw_keystore.c
@@ -0,0 +1,837 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/**
+ * @file fsl_shw_keystore.c
+ *
+ * File which implements a default keystore policy, for use as the system
+ * keystore.
+ */
+#include "fsl_platform.h"
+#include "fsl_shw.h"
+#include "fsl_shw_keystore.h"
+
+#if defined(DIAG_DRV_IF)
+#include <diagnostic.h>
+#endif
+
+#if !defined(FSL_HAVE_SCC2) && defined(__KERNEL__)
+#include <linux/mxc_scc_driver.h>
+#endif
+
+/* Define a semaphore to protect the keystore data */
+#ifdef __KERNEL__
+#define LOCK_INCLUDES os_lock_context_t context
+#define ACQUIRE_LOCK os_lock_save_context(keystore->lock, context)
+#define RELEASE_LOCK os_unlock_restore_context(keystore->lock, context);
+#else
+#define LOCK_INCLUDES
+#define ACQUIRE_LOCK
+#define RELEASE_LOCK
+#endif	/* __KERNEL__ */
+
+/*!
+ * Calculates the byte offset into a word
+ *  @param   bp  The byte (char*) pointer
+ *  @return      The offset (0, 1, 2, or 3)
+ */
+#define SCC_BYTE_OFFSET(bp) ((uint32_t)(bp) % sizeof(uint32_t))
+
+/*!
+ * Converts (by rounding down) a byte pointer into a word pointer
+ *  @param  bp  The byte (char*) pointer
+ *  @return     The word (uint32_t) as though it were an aligned (uint32_t*)
+ */
+#define SCC_WORD_PTR(bp) (((uint32_t)(bp)) & ~(sizeof(uint32_t)-1))
+
+/* Depending on the architecture, these functions should be defined
+ * differently.  On Platforms with SCC2, the functions use the secure
+ * partition interface and should be available in both user and kernel space.
+ * On platforms with SCC, they use the SCC keystore interface.  This is only
+ * available in kernel mode, so they should be stubbed out in user mode.
+ */
+#if defined(FSL_HAVE_SCC2) || (defined(FSL_HAVE_SCC) && defined(__KERNEL__))
+EXPORT_SYMBOL(fsl_shw_init_keystore);
+void fsl_shw_init_keystore(
+			    fsl_shw_kso_t *keystore,
+			    fsl_shw_return_t(*data_init) (fsl_shw_uco_t *user_ctx,
+							   void **user_data),
+			    void (*data_cleanup) (fsl_shw_uco_t *user_ctx,
+						   void **user_data),
+			    fsl_shw_return_t(*slot_alloc) (void *user_data,
+							    uint32_t size,
+							    uint64_t owner_id,
+							    uint32_t *slot),
+			    fsl_shw_return_t(*slot_dealloc) (void *user_data,
+							      uint64_t
+							      owner_id,
+							      uint32_t slot),
+			    fsl_shw_return_t(*slot_verify_access) (void
+								    *user_data,
+								    uint64_t
+								    owner_id,
+								    uint32_t
+								    slot),
+			    void *(*slot_get_address) (void *user_data,
+							uint32_t handle),
+			    uint32_t(*slot_get_base) (void *user_data,
+						       uint32_t handle),
+			    uint32_t(*slot_get_offset) (void *user_data,
+							 uint32_t handle),
+			    uint32_t(*slot_get_slot_size) (void *user_data,
+							    uint32_t handle))
+{
+	keystore->data_init = data_init;
+	keystore->data_cleanup = data_cleanup;
+	keystore->slot_alloc = slot_alloc;
+	keystore->slot_dealloc = slot_dealloc;
+	keystore->slot_verify_access = slot_verify_access;
+	keystore->slot_get_address = slot_get_address;
+	keystore->slot_get_base = slot_get_base;
+	keystore->slot_get_offset = slot_get_offset;
+	keystore->slot_get_slot_size = slot_get_slot_size;
+}
+
+EXPORT_SYMBOL(fsl_shw_init_keystore_default);
+void fsl_shw_init_keystore_default(fsl_shw_kso_t *keystore)
+{
+	keystore->data_init = shw_kso_init_data;
+	keystore->data_cleanup = shw_kso_cleanup_data;
+	keystore->slot_alloc = shw_slot_alloc;
+	keystore->slot_dealloc = shw_slot_dealloc;
+	keystore->slot_verify_access = shw_slot_verify_access;
+	keystore->slot_get_address = shw_slot_get_address;
+	keystore->slot_get_base = shw_slot_get_base;
+	keystore->slot_get_offset = shw_slot_get_offset;
+	keystore->slot_get_slot_size = shw_slot_get_slot_size;
+}
+
+/*!
+ * Do any keystore specific initializations
+ */
+EXPORT_SYMBOL(fsl_shw_establish_keystore);
+fsl_shw_return_t fsl_shw_establish_keystore(fsl_shw_uco_t *user_ctx,
+						fsl_shw_kso_t *keystore)
+{
+	if (keystore->data_init == NULL) {
+		return FSL_RETURN_ERROR_S;
+	}
+
+    /* Call the data_init function for any user setup */
+    return keystore->data_init(user_ctx, &(keystore->user_data));
+}
+
+EXPORT_SYMBOL(fsl_shw_release_keystore);
+void fsl_shw_release_keystore(fsl_shw_uco_t *user_ctx,
+				 fsl_shw_kso_t *keystore)
+{
+
+    /* Call the data_cleanup function for any keystore cleanup.
+	  * NOTE: The keystore doesn't have any way of telling which keys are using
+	  * it, so it is up to the user program to manage their key objects
+	  * correctly.
+	  */
+	 if ((keystore != NULL) && (keystore->data_cleanup != NULL)) {
+		keystore->data_cleanup(user_ctx, &(keystore->user_data));
+	}
+	return;
+}
+
+fsl_shw_return_t keystore_slot_alloc(fsl_shw_kso_t *keystore, uint32_t size,
+				       uint64_t owner_id, uint32_t *slot)
+{
+	LOCK_INCLUDES;
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+
+#ifdef DIAG_DRV_IF
+	    LOG_DIAG("In keystore_slot_alloc.");
+
+#endif
+	ACQUIRE_LOCK;
+	if ((keystore->slot_alloc == NULL) || (keystore->user_data == NULL)) {
+		goto out;
+	}
+
+#ifdef DIAG_DRV_IF
+	    LOG_DIAG_ARGS("key length: %i, handle: %i\n", size, *slot);
+
+#endif
+retval = keystore->slot_alloc(keystore->user_data, size, owner_id, slot);
+out:RELEASE_LOCK;
+	return retval;
+}
+
+fsl_shw_return_t keystore_slot_dealloc(fsl_shw_kso_t *keystore,
+					 uint64_t owner_id, uint32_t slot)
+{
+	LOCK_INCLUDES;
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+	ACQUIRE_LOCK;
+	if ((keystore->slot_alloc == NULL) || (keystore->user_data == NULL)) {
+		goto out;
+	}
+	retval =
+	    keystore->slot_dealloc(keystore->user_data, owner_id, slot);
+out:RELEASE_LOCK;
+	return retval;
+}
+
+fsl_shw_return_t
+keystore_slot_load(fsl_shw_kso_t * keystore, uint64_t owner_id, uint32_t slot,
+		   const uint8_t * key_data, uint32_t key_length)
+{
+
+#ifdef FSL_HAVE_SCC2
+	    LOCK_INCLUDES;
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+	uint32_t slot_size;
+	uint32_t i;
+	uint8_t * slot_location;
+	ACQUIRE_LOCK;
+	if ((keystore->slot_verify_access == NULL) ||
+	      (keystore->user_data == NULL))
+		goto out;
+	if (keystore->
+	      slot_verify_access(keystore->user_data, owner_id,
+				 slot) !=FSL_RETURN_OK_S) {
+		retval = FSL_RETURN_AUTH_FAILED_S;
+		goto out;
+	}
+	slot_size = keystore->slot_get_slot_size(keystore->user_data, slot);
+	if (key_length > slot_size) {
+		retval = FSL_RETURN_BAD_DATA_LENGTH_S;
+		goto out;
+	}
+	slot_location = keystore->slot_get_address(keystore->user_data, slot);
+	for (i = 0; i < key_length; i++) {
+		slot_location[i] = key_data[i];
+	}
+	retval = FSL_RETURN_OK_S;
+out:RELEASE_LOCK;
+	return retval;
+
+#else	/* FSL_HAVE_SCC2 */
+	fsl_shw_return_t retval;
+	scc_return_t scc_ret;
+	scc_ret =
+	    scc_load_slot(owner_id, slot, (uint8_t *) key_data, key_length);
+	switch (scc_ret) {
+	case SCC_RET_OK:
+		retval = FSL_RETURN_OK_S;
+		break;
+	case SCC_RET_VERIFICATION_FAILED:
+		retval = FSL_RETURN_AUTH_FAILED_S;
+		break;
+	case SCC_RET_INSUFFICIENT_SPACE:
+		retval = FSL_RETURN_BAD_DATA_LENGTH_S;
+		break;
+	default:
+		retval = FSL_RETURN_ERROR_S;
+	}
+	return retval;
+
+#endif	/* FSL_HAVE_SCC2 */
+}
+
+fsl_shw_return_t
+keystore_slot_read(fsl_shw_kso_t * keystore, uint64_t owner_id, uint32_t slot,
+		   uint32_t key_length, uint8_t * key_data)
+{
+#ifdef FSL_HAVE_SCC2
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+	uint8_t *slot_addr;
+	uint32_t slot_size;
+
+	slot_addr = keystore->slot_get_address(keystore->user_data, slot);
+	slot_size = keystore->slot_get_slot_size(keystore->user_data, slot);
+
+	if (key_length > slot_size) {
+		retval = FSL_RETURN_BAD_KEY_LENGTH_S;
+		goto out;
+	}
+
+	memcpy(key_data, slot_addr, key_length);
+	retval = FSL_RETURN_OK_S;
+
+      out:
+	return retval;
+
+#else				/* Have SCC2 */
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+	scc_return_t scc_ret;
+	printk("keystore SCC \n");
+
+	scc_ret =
+	    scc_read_slot(owner_id, slot, key_length, (uint8_t *) key_data);
+	printk("keystore SCC Ret value: %d \n", scc_ret);
+	switch (scc_ret) {
+	case SCC_RET_OK:
+		retval = FSL_RETURN_OK_S;
+		break;
+	case SCC_RET_VERIFICATION_FAILED:
+		retval = FSL_RETURN_AUTH_FAILED_S;
+		break;
+	case SCC_RET_INSUFFICIENT_SPACE:
+		retval = FSL_RETURN_BAD_DATA_LENGTH_S;
+		break;
+	default:
+		retval = FSL_RETURN_ERROR_S;
+	}
+
+	return retval;
+
+#endif				/* FSL_HAVE_SCC2 */
+}/* end fn keystore_slot_read */
+
+fsl_shw_return_t
+keystore_slot_encrypt(fsl_shw_uco_t *user_ctx, fsl_shw_kso_t *keystore,
+		      uint64_t owner_id, uint32_t slot, uint32_t length,
+		      uint8_t *destination)
+{
+
+#ifdef FSL_HAVE_SCC2
+	LOCK_INCLUDES;
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+	uint32_t slot_length;
+	uint32_t IV[4];
+	uint32_t * iv_ptr = (uint32_t *) & (owner_id);
+
+	/* Build the IV */
+	IV[0] = iv_ptr[0];
+	IV[1] = iv_ptr[1];
+	IV[2] = 0;
+	IV[3] = 0;
+	ACQUIRE_LOCK;
+
+	/* Ensure that the data will fit in the key slot */
+	slot_length =
+	    keystore->slot_get_slot_size(keystore->user_data, slot);
+	if (length > slot_length) {
+		goto out;
+	}
+
+	  /* Call scc encrypt function to encrypt the data. */
+	    retval = do_scc_encrypt_region(user_ctx,
+					   (void *)keystore->
+					   slot_get_base(keystore->user_data,
+							 slot),
+					   keystore->slot_get_offset(keystore->
+								      user_data,
+								      slot),
+					   length, destination, IV,
+					   FSL_SHW_CYPHER_MODE_CBC);
+	goto out;
+out:RELEASE_LOCK;
+	return retval;
+
+#else
+	scc_return_t retval;
+	retval = scc_encrypt_slot(owner_id, slot, length, destination);
+	if (retval == SCC_RET_OK)
+		return FSL_RETURN_OK_S;
+	return FSL_RETURN_ERROR_S;
+
+#endif	/* FSL_HAVE_SCC2 */
+}
+
+fsl_shw_return_t
+keystore_slot_decrypt(fsl_shw_uco_t *user_ctx, fsl_shw_kso_t *keystore,
+		      uint64_t owner_id, uint32_t slot, uint32_t length,
+		      const uint8_t *source)
+{
+
+#ifdef FSL_HAVE_SCC2
+	LOCK_INCLUDES;
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+	uint32_t slot_length;
+	uint32_t IV[4];
+	uint32_t *iv_ptr = (uint32_t *) & (owner_id);
+
+    /* Build the IV */
+    IV[0] = iv_ptr[0];
+	IV[1] = iv_ptr[1];
+	IV[2] = 0;
+	IV[3] = 0;
+	ACQUIRE_LOCK;
+
+	/* Call scc decrypt function to decrypt the data. */
+
+	/* Ensure that the data will fit in the key slot */
+	    slot_length =
+	    keystore->slot_get_slot_size(keystore->user_data, slot);
+	if (length > slot_length)
+		goto out;
+
+	/* Call scc decrypt function to encrypt the data. */
+	    retval = do_scc_decrypt_region(user_ctx,
+					   (void *)keystore->
+					   slot_get_base(keystore->user_data,
+							 slot),
+					   keystore->slot_get_offset(keystore->
+								      user_data,
+								      slot),
+					   length, source, IV,
+					   FSL_SHW_CYPHER_MODE_CBC);
+	goto out;
+out:RELEASE_LOCK;
+	return retval;
+
+#else
+	scc_return_t retval;
+	retval = scc_decrypt_slot(owner_id, slot, length, source);
+	if (retval == SCC_RET_OK)
+		return FSL_RETURN_OK_S;
+	return FSL_RETURN_ERROR_S;
+
+#endif	/* FSL_HAVE_SCC2 */
+}
+
+#else	/* SCC in userspace */
+void fsl_shw_init_keystore(
+			    fsl_shw_kso_t *keystore,
+			    fsl_shw_return_t(*data_init) (fsl_shw_uco_t *user_ctx,
+							   void **user_data),
+			    void (*data_cleanup) (fsl_shw_uco_t *user_ctx,
+						   void **user_data),
+			    fsl_shw_return_t(*slot_alloc) (void *user_data,
+							    uint32_t size,
+							    uint64_t owner_id,
+							    uint32_t *slot),
+			    fsl_shw_return_t(*slot_dealloc) (void *user_data,
+							      uint64_t
+							      owner_id,
+							      uint32_t slot),
+			    fsl_shw_return_t(*slot_verify_access) (void
+								    *user_data,
+								    uint64_t
+								    owner_id,
+								    uint32_t
+								    slot),
+			    void *(*slot_get_address) (void *user_data,
+							uint32_t handle),
+			    uint32_t(*slot_get_base) (void *user_data,
+						       uint32_t handle),
+			    uint32_t(*slot_get_offset) (void *user_data,
+							 uint32_t handle),
+			    uint32_t(*slot_get_slot_size) (void *user_data,
+							    uint32_t handle))
+{
+	(void)keystore;
+	(void)data_init;
+	(void)data_cleanup;
+	(void)slot_alloc;
+	(void)slot_dealloc;
+	(void)slot_verify_access;
+	(void)slot_get_address;
+	(void)slot_get_base;
+	(void)slot_get_offset;
+	(void)slot_get_slot_size;
+}
+
+void fsl_shw_init_keystore_default(fsl_shw_kso_t * keystore)
+{
+	(void)keystore;
+}
+fsl_shw_return_t fsl_shw_establish_keystore(fsl_shw_uco_t *user_ctx,
+						 fsl_shw_kso_t *keystore)
+{
+	(void)user_ctx;
+	(void)keystore;
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+void fsl_shw_release_keystore(fsl_shw_uco_t *user_ctx,
+				fsl_shw_kso_t *keystore)
+{
+	(void)user_ctx;
+	(void)keystore;
+	return;
+}
+
+fsl_shw_return_t keystore_slot_alloc(fsl_shw_kso_t *keystore, uint32_t size,
+					uint64_t owner_id, uint32_t *slot)
+{
+	(void)keystore;
+	(void)size;
+	(void)owner_id;
+	(void)slot;
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t keystore_slot_dealloc(fsl_shw_kso_t *keystore,
+					 uint64_t owner_id, uint32_t slot)
+{
+	(void)keystore;
+	(void)owner_id;
+	(void)slot;
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t
+keystore_slot_load(fsl_shw_kso_t *keystore, uint64_t owner_id, uint32_t slot,
+		   const uint8_t *key_data, uint32_t key_length)
+{
+	(void)keystore;
+	(void)owner_id;
+	(void)slot;
+	(void)key_data;
+	(void)key_length;
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t
+keystore_slot_read(fsl_shw_kso_t * keystore, uint64_t owner_id, uint32_t slot,
+		   uint32_t key_length, uint8_t * key_data)
+{
+	(void)keystore;
+	(void)owner_id;
+	(void)slot;
+	(void)key_length;
+	(void)key_data;
+
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t
+keystore_slot_decrypt(fsl_shw_uco_t *user_ctx, fsl_shw_kso_t *keystore,
+		      uint64_t owner_id, uint32_t slot, uint32_t length,
+		      const uint8_t *source)
+{
+	(void)user_ctx;
+	(void)keystore;
+	(void)owner_id;
+	(void)slot;
+	(void)length;
+	(void)source;
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t
+keystore_slot_encrypt(fsl_shw_uco_t *user_ctx, fsl_shw_kso_t *keystore,
+		      uint64_t owner_id, uint32_t slot, uint32_t length,
+		      uint8_t *destination)
+{
+	(void)user_ctx;
+	(void)keystore;
+	(void)owner_id;
+	(void)slot;
+	(void)length;
+	(void)destination;
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+
+#endif	/* FSL_HAVE_SCC2 */
+
+/***** Default keystore implementation **************************************/
+
+#ifdef FSL_HAVE_SCC2
+    fsl_shw_return_t shw_kso_init_data(fsl_shw_uco_t *user_ctx,
+					void **user_data)
+{
+	int retval = FSL_RETURN_ERROR_S;
+	keystore_data_t *keystore_data = NULL;
+	fsl_shw_pco_t *capabilities = fsl_shw_get_capabilities(user_ctx);
+	uint32_t partition_size;
+	uint32_t slot_count;
+	uint32_t keystore_data_size;
+	uint8_t UMID[16] = {
+	0x42, 0, 0, 0, 0x43, 0, 0, 0, 0x19, 0, 0, 0, 0x59, 0, 0, 0};
+	uint32_t permissions =
+	    FSL_PERM_TH_R | FSL_PERM_TH_W | FSL_PERM_HD_R | FSL_PERM_HD_W |
+	    FSL_PERM_HD_X;
+
+	/* Look up the size of a partition to see how big to make the keystore */
+	partition_size = fsl_shw_pco_get_spo_size_bytes(capabilities);
+
+	/* Calculate the required size of the keystore data structure, based on the
+	  * number of keys that can fit in the partition.
+	  */
+	slot_count = partition_size / KEYSTORE_SLOT_SIZE;
+	keystore_data_size =
+	    sizeof(keystore_data_t) +
+	    slot_count * sizeof(keystore_data_slot_info_t);
+
+#ifdef __KERNEL__
+	keystore_data = os_alloc_memory(keystore_data_size, GFP_KERNEL);
+
+#else
+	keystore_data = malloc(keystore_data_size);
+
+#endif
+	if (keystore_data == NULL) {
+		retval = FSL_RETURN_NO_RESOURCE_S;
+		goto out;
+	}
+
+    /* Clear the memory (effectively clear all key assignments) */
+    memset(keystore_data, 0, keystore_data_size);
+
+   /* Place the slot information structure directly after the keystore data
+   	* structure.
+	*/
+	    keystore_data->slot =
+	    (keystore_data_slot_info_t *) (keystore_data + 1);
+	keystore_data->slot_count = slot_count;
+
+	/* Retrieve a secure partition to put the keystore in. */
+	keystore_data->base_address =
+	    fsl_shw_smalloc(user_ctx, partition_size, UMID, permissions);
+	if (keystore_data->base_address == NULL) {
+		retval = FSL_RETURN_NO_RESOURCE_S;
+		goto out;
+	}
+	*user_data = keystore_data;
+	retval = FSL_RETURN_OK_S;
+out:if (retval != FSL_RETURN_OK_S) {
+		if (keystore_data != NULL) {
+			if (keystore_data->base_address != NULL)
+				fsl_shw_sfree(NULL,
+					       keystore_data->base_address);
+
+#ifdef __KERNEL__
+			    os_free_memory(keystore_data);
+
+#else
+			    free(keystore_data);
+
+#endif
+		}
+	}
+	return retval;
+}
+void shw_kso_cleanup_data(fsl_shw_uco_t *user_ctx, void **user_data)
+{
+	if (user_data != NULL) {
+		keystore_data_t * keystore_data =
+		    (keystore_data_t *) (*user_data);
+		fsl_shw_sfree(user_ctx, keystore_data->base_address);
+
+#ifdef __KERNEL__
+		    os_free_memory(*user_data);
+
+#else
+		    free(*user_data);
+
+#endif
+	}
+	return;
+}
+
+fsl_shw_return_t shw_slot_verify_access(void *user_data, uint64_t owner_id,
+					  uint32_t slot)
+{
+	keystore_data_t * data = user_data;
+	if (data->slot[slot].owner == owner_id) {
+		return FSL_RETURN_OK_S;
+	} else {
+
+#ifdef DIAG_DRV_IF
+		    LOG_DIAG_ARGS("Access to slot %i fails.\n", slot);
+
+#endif
+		    return FSL_RETURN_AUTH_FAILED_S;
+	}
+}
+
+fsl_shw_return_t shw_slot_alloc(void *user_data, uint32_t size,
+				   uint64_t owner_id, uint32_t *slot)
+{
+	keystore_data_t *data = user_data;
+	uint32_t i;
+	if (size > KEYSTORE_SLOT_SIZE)
+		return FSL_RETURN_BAD_KEY_LENGTH_S;
+	for (i = 0; i < data->slot_count; i++) {
+		if (data->slot[i].allocated == 0) {
+			data->slot[i].allocated = 1;
+			data->slot[i].owner = owner_id;
+			(*slot) = i;
+
+#ifdef DIAG_DRV_IF
+			    LOG_DIAG_ARGS("Keystore: allocated slot %i. Slot "
+					  "address: %p\n",
+					  (*slot),
+					  data->base_address +
+					  (*slot) * KEYSTORE_SLOT_SIZE);
+
+#endif
+			    return FSL_RETURN_OK_S;
+		}
+	}
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t shw_slot_dealloc(void *user_data, uint64_t owner_id,
+				    uint32_t slot)
+{
+	keystore_data_t * data = user_data;
+	(void)owner_id;
+	(void)slot;
+	if (slot >= data->slot_count)
+		return FSL_RETURN_ERROR_S;
+	if (data->slot[slot].allocated == 1) {
+		/* Forcibly remove the data from the keystore */
+		memset(shw_slot_get_address(user_data, slot), 0,
+		       KEYSTORE_SLOT_SIZE);
+		data->slot[slot].allocated = 0;
+		return FSL_RETURN_OK_S;
+	}
+	return FSL_RETURN_ERROR_S;
+}
+
+void *shw_slot_get_address(void *user_data, uint32_t slot)
+{
+	keystore_data_t * data = user_data;
+	if (slot >= data->slot_count)
+		return NULL;
+	return data->base_address + slot * KEYSTORE_SLOT_SIZE;
+}
+
+uint32_t shw_slot_get_base(void *user_data, uint32_t slot)
+{
+	keystore_data_t * data = user_data;
+
+	/* There could potentially be more than one secure partition object
+	  * associated with this keystore.  For now, there is just one.
+	  */
+	(void)slot;
+	return (uint32_t) (data->base_address);
+}
+
+uint32_t shw_slot_get_offset(void *user_data, uint32_t slot)
+{
+	keystore_data_t *data = user_data;
+	if (slot >= data->slot_count)
+		return FSL_RETURN_ERROR_S;
+	return (slot * KEYSTORE_SLOT_SIZE);
+}
+
+uint32_t shw_slot_get_slot_size(void *user_data, uint32_t slot)
+{
+	(void)user_data;
+	(void)slot;
+
+	/* All slots are the same size in the default implementation */
+	return KEYSTORE_SLOT_SIZE;
+}
+
+#else	/* FSL_HAVE_SCC2 */
+
+#ifdef __KERNEL__
+    fsl_shw_return_t shw_kso_init_data(fsl_shw_uco_t *user_ctx,
+					void **user_data)
+{
+
+   /* The SCC does its own initialization.  All that needs to be done here is
+   	* make sure an SCC exists.
+	*/
+	*user_data = (void *)0xFEEDFEED;
+	return FSL_RETURN_OK_S;
+}
+void shw_kso_cleanup_data(fsl_shw_uco_t *user_ctx, void **user_data)
+{
+
+    /* The SCC does its own cleanup. */
+	*user_data = NULL;
+	return;
+}
+
+fsl_shw_return_t shw_slot_verify_access(void *user_data, uint64_t owner_id,
+					  uint32_t slot)
+{
+
+	/* Zero is used for the size because the newer interface does bounds
+	  * checking later.
+	  */
+	scc_return_t retval;
+	retval = scc_verify_slot_access(owner_id, slot, 0);
+	if (retval == SCC_RET_OK) {
+		return FSL_RETURN_OK_S;
+	}
+	return FSL_RETURN_AUTH_FAILED_S;
+}
+
+fsl_shw_return_t shw_slot_alloc(void *user_data, uint32_t size,
+				   uint64_t owner_id, uint32_t *slot)
+{
+	scc_return_t retval;
+
+#ifdef DIAG_DRV_IF
+	    LOG_DIAG_ARGS("key length: %i, handle: %i\n", size, *slot);
+
+#endif
+	retval = scc_alloc_slot(size, owner_id, slot);
+	if (retval == SCC_RET_OK)
+		return FSL_RETURN_OK_S;
+
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t shw_slot_dealloc(void *user_data, uint64_t owner_id,
+				    uint32_t slot)
+{
+	scc_return_t retval;
+	retval = scc_dealloc_slot(owner_id, slot);
+	if (retval == SCC_RET_OK)
+		return FSL_RETURN_OK_S;
+
+	return FSL_RETURN_ERROR_S;
+}
+void *shw_slot_get_address(void *user_data, uint32_t slot)
+{
+	uint64_t owner_id = *((uint64_t *) user_data);
+	uint32_t address;
+	uint32_t value_size_bytes;
+	uint32_t slot_size_bytes;
+	scc_return_t scc_ret;
+	scc_ret =
+	    scc_get_slot_info(owner_id, slot, &address, &value_size_bytes,
+			      &slot_size_bytes);
+	if (scc_ret == SCC_RET_OK) {
+		return (void *)address;
+	}
+	return NULL;
+}
+
+uint32_t shw_slot_get_base(void *user_data, uint32_t slot)
+{
+	return 0;
+}
+
+uint32_t shw_slot_get_offset(void *user_data, uint32_t slot)
+{
+	return 0;
+}
+
+
+/* Return the size of the key slot, in octets */
+uint32_t shw_slot_get_slot_size(void *user_data, uint32_t slot)
+{
+	uint64_t owner_id = *((uint64_t *) user_data);
+	uint32_t address;
+	uint32_t value_size_bytes;
+	uint32_t slot_size_bytes;
+	scc_return_t scc_ret;
+	scc_ret =
+	    scc_get_slot_info(owner_id, slot, &address, &value_size_bytes,
+			      &slot_size_bytes);
+	if (scc_ret == SCC_RET_OK)
+		return slot_size_bytes;
+	return 0;
+}
+
+
+#endif	/* __KERNEL__ */
+
+#endif	/* FSL_HAVE_SCC2 */
+
+/*****************************************************************************/
diff --git a/drivers/mxc/security/sahara2/fsl_shw_rand.c b/drivers/mxc/security/sahara2/fsl_shw_rand.c
new file mode 100644
index 000000000000..566c9e5c1e0f
--- /dev/null
+++ b/drivers/mxc/security/sahara2/fsl_shw_rand.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_shw_rand.c
+ *
+ * This file implements Random Number Generation functions of the FSL SHW API
+ * for Sahara.
+ */
+
+#include "sahara.h"
+#include "sf_util.h"
+
+#ifdef __KERNEL__
+EXPORT_SYMBOL(fsl_shw_get_random);
+#endif
+
+/* REQ-S2LRD-PINTFC-API-BASIC-RNG-002 */
+/*!
+ * Get a random number
+ *
+ *
+ * @param    user_ctx
+ * @param    length
+ * @param    data
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_get_random(fsl_shw_uco_t * user_ctx,
+				    uint32_t length, uint8_t * data)
+{
+	SAH_SF_DCLS;
+
+	/* perform a sanity check on the uco */
+	ret = sah_validate_uco(user_ctx);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	header = SAH_HDR_RNG_GENERATE;	/* Desc. #18 */
+	DESC_OUT_OUT(header, length, data, 0, NULL);
+
+	SAH_SF_EXECUTE();
+
+      out:
+	SAH_SF_DESC_CLEAN();
+
+	return ret;
+}
+
+#ifdef __KERNEL__
+EXPORT_SYMBOL(fsl_shw_add_entropy);
+#endif
+
+/*!
+ * Add entropy to a random number generator
+
+ * @param    user_ctx
+ * @param    length
+ * @param    data
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_add_entropy(fsl_shw_uco_t * user_ctx,
+				     uint32_t length, uint8_t * data)
+{
+	SAH_SF_DCLS;
+
+	/* perform a sanity check on the uco */
+	ret = sah_validate_uco(user_ctx);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	header = SAH_HDR_RNG_GENERATE;	/* Desc. #18 */
+
+	/* create descriptor #18. Generate random data */
+	DESC_IN_IN(header, 0, NULL, length, data)
+
+	    SAH_SF_EXECUTE();
+
+      out:
+	SAH_SF_DESC_CLEAN();
+
+	return ret;
+}
diff --git a/drivers/mxc/security/sahara2/fsl_shw_sym.c b/drivers/mxc/security/sahara2/fsl_shw_sym.c
new file mode 100644
index 000000000000..454905bbcf68
--- /dev/null
+++ b/drivers/mxc/security/sahara2/fsl_shw_sym.c
@@ -0,0 +1,281 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_shw_sym.c
+ *
+ * This file implements Symmetric Cipher functions of the FSL SHW API for
+ * Sahara.  This does not include CCM.
+ */
+
+#include "sahara.h"
+#include "fsl_platform.h"
+
+#include "sf_util.h"
+#include "adaptor.h"
+
+#ifdef LINUX_KERNEL
+EXPORT_SYMBOL(fsl_shw_symmetric_encrypt);
+EXPORT_SYMBOL(fsl_shw_symmetric_decrypt);
+#endif
+
+#if defined(NEED_CTR_WORKAROUND)
+/* CTR mode needs block-multiple data in/out */
+#define LENGTH_PATCH  (sym_ctx->block_size_bytes)
+#define LENGTH_PATCH_MASK  (sym_ctx->block_size_bytes-1)
+#else
+#define LENGTH_PATCH  0
+#define LENGTH_PATCH_MASK  0	/* du not use! */
+#endif
+
+/*!
+ * Block of zeroes which is maximum Symmetric block size, used for
+ * initializing context register, etc.
+ */
+static uint32_t block_zeros[4] = {
+	0, 0, 0, 0
+};
+
+typedef enum cipher_direction {
+	SYM_DECRYPT,
+	SYM_ENCRYPT
+} cipher_direction_t;
+
+/*!
+ * Create and run the chain for a symmetric-key operation.
+ *
+ * @param user_ctx    Who the user is
+ * @param key_info    What key is to be used
+ * @param sym_ctx     Info details about algorithm
+ * @param encrypt     0 = decrypt, non-zero = encrypt
+ * @param length      Number of octets at @a in and @a out
+ * @param in          Pointer to input data
+ * @param out         Location to store output data
+ *
+ * @return         The status of handing chain to driver,
+ *                 or an earlier argument/flag or allocation
+ *                 error.
+ */
+static fsl_shw_return_t do_symmetric(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info,
+				     fsl_shw_scco_t * sym_ctx,
+				     cipher_direction_t encrypt,
+				     uint32_t length,
+				     const uint8_t * in, uint8_t * out)
+{
+	SAH_SF_DCLS;
+	uint8_t *sink = NULL;
+	sah_Link *link1 = NULL;
+	sah_Link *link2 = NULL;
+	sah_Oct_Str ptr1;
+	uint32_t size1 = sym_ctx->block_size_bytes;
+
+	SAH_SF_USER_CHECK();
+
+	/* Two different sets of chains, depending on algorithm */
+	if (key_info->algorithm == FSL_KEY_ALG_ARC4) {
+		if (sym_ctx->flags & FSL_SYM_CTX_INIT) {
+			/* Desc. #35 w/ARC4 - start from key */
+			header = SAH_HDR_ARC4_SET_MODE_KEY
+			    ^ sah_insert_skha_algorithm_arc4;
+
+			DESC_IN_KEY(header, 0, NULL, key_info);
+		} else {	/* load SBox */
+			/* Desc. #33 w/ARC4 and NO PERMUTE */
+			header = SAH_HDR_ARC4_SET_MODE_SBOX
+			    ^ sah_insert_skha_no_permute
+			    ^ sah_insert_skha_algorithm_arc4;
+			DESC_IN_IN(header, 256, sym_ctx->context,
+				   3, sym_ctx->context + 256);
+		}		/* load SBox */
+
+		/* Add in-out data descriptor to process the data */
+		if (length != 0) {
+			DESC_IN_OUT(SAH_HDR_SKHA_ENC_DEC, length, in, length,
+				    out);
+		}
+
+		/* Operation is done ... save what came out? */
+		if (sym_ctx->flags & FSL_SYM_CTX_SAVE) {
+			/* Desc. #34 - Read SBox, pointers */
+			header = SAH_HDR_ARC4_READ_SBOX;
+			DESC_OUT_OUT(header, 256, sym_ctx->context,
+				     3, sym_ctx->context + 256);
+		}
+	} else {		/* not ARC4 */
+		/* Doing 1- or 2- descriptor chain. */
+		/* Desc. #1 and algorithm and mode */
+		header = SAH_HDR_SKHA_SET_MODE_IV_KEY
+		    ^ sah_insert_skha_mode[sym_ctx->mode]
+		    ^ sah_insert_skha_algorithm[key_info->algorithm];
+
+		/* Honor 'no key parity checking' for DES and TDES */
+		if ((key_info->flags & FSL_SKO_KEY_IGNORE_PARITY) &&
+		    ((key_info->algorithm == FSL_KEY_ALG_DES) ||
+		     (key_info->algorithm == FSL_KEY_ALG_TDES))) {
+			header ^= sah_insert_skha_no_key_parity;
+		}
+
+		/* Header by default is decrypting, so... */
+		if (encrypt == SYM_ENCRYPT) {
+			header ^= sah_insert_skha_encrypt;
+		}
+
+		if (sym_ctx->mode == FSL_SYM_MODE_CTR) {
+			header ^= sah_insert_skha_modulus[sym_ctx->modulus_exp];
+		}
+
+		if (sym_ctx->mode == FSL_SYM_MODE_ECB) {
+			ptr1 = NULL;
+			size1 = 0;
+		} else if (sym_ctx->flags & FSL_SYM_CTX_INIT) {
+			ptr1 = (uint8_t *) block_zeros;
+		} else {
+			ptr1 = sym_ctx->context;
+		}
+
+		DESC_IN_KEY(header, sym_ctx->block_size_bytes, ptr1, key_info);
+
+		/* Add in-out data descriptor */
+		if (length != 0) {
+			header = SAH_HDR_SKHA_ENC_DEC;
+			if (LENGTH_PATCH && (sym_ctx->mode == FSL_SYM_MODE_CTR)
+			    && ((length & LENGTH_PATCH_MASK) != 0)) {
+				sink = DESC_TEMP_ALLOC(LENGTH_PATCH);
+				ret =
+				    sah_Create_Link(user_ctx->mem_util, &link1,
+						    (uint8_t *) in, length,
+						    SAH_USES_LINK_DATA);
+				ret =
+				    sah_Append_Link(user_ctx->mem_util, link1,
+						    (uint8_t *) sink,
+						    LENGTH_PATCH -
+						    (length &
+						     LENGTH_PATCH_MASK),
+						    SAH_USES_LINK_DATA);
+				if (ret != FSL_RETURN_OK_S) {
+					goto out;
+				}
+				ret =
+				    sah_Create_Link(user_ctx->mem_util, &link2,
+						    out, length,
+						    SAH_USES_LINK_DATA |
+						    SAH_OUTPUT_LINK);
+				if (ret != FSL_RETURN_OK_S) {
+					goto out;
+				}
+				ret = sah_Append_Link(user_ctx->mem_util, link2,
+						      sink,
+						      LENGTH_PATCH -
+						      (length &
+						       LENGTH_PATCH_MASK),
+						      SAH_USES_LINK_DATA);
+				if (ret != FSL_RETURN_OK_S) {
+					goto out;
+				}
+				ret =
+				    sah_Append_Desc(user_ctx->mem_util,
+						    &desc_chain, header, link1,
+						    link2);
+				if (ret != FSL_RETURN_OK_S) {
+					goto out;
+				}
+				link1 = link2 = NULL;
+			} else {
+				DESC_IN_OUT(header, length, in, length, out);
+			}
+		}
+
+		/* Unload any desired context */
+		if (sym_ctx->flags & FSL_SYM_CTX_SAVE) {
+			DESC_OUT_OUT(SAH_HDR_SKHA_READ_CONTEXT_IV, 0, NULL,
+				     sym_ctx->block_size_bytes,
+				     sym_ctx->context);
+		}
+
+	}			/* not ARC4 */
+
+	SAH_SF_EXECUTE();
+
+      out:
+	SAH_SF_DESC_CLEAN();
+	DESC_TEMP_FREE(sink);
+	if (LENGTH_PATCH) {
+		sah_Destroy_Link(user_ctx->mem_util, link1);
+		sah_Destroy_Link(user_ctx->mem_util, link2);
+	}
+
+	return ret;
+}
+
+/* REQ-S2LRD-PINTFC-API-BASIC-SYM-002 */
+/* PINTFC-API-BASIC-SYM-ARC4-001 */
+/* PINTFC-API-BASIC-SYM-ARC4-002 */
+
+/*!
+ * Compute symmetric encryption
+ *
+ *
+ * @param    user_ctx
+ * @param    key_info
+ * @param    sym_ctx
+ * @param    length
+ * @param    pt
+ * @param    ct
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_symmetric_encrypt(fsl_shw_uco_t * user_ctx,
+					   fsl_shw_sko_t * key_info,
+					   fsl_shw_scco_t * sym_ctx,
+					   uint32_t length,
+					   const uint8_t * pt, uint8_t * ct)
+{
+	fsl_shw_return_t ret;
+
+	ret = do_symmetric(user_ctx, key_info, sym_ctx, SYM_ENCRYPT,
+			   length, pt, ct);
+
+	return ret;
+}
+
+/* PINTFC-API-BASIC-SYM-002 */
+/* PINTFC-API-BASIC-SYM-ARC4-001 */
+/* PINTFC-API-BASIC-SYM-ARC4-002 */
+
+/*!
+ * Compute symmetric decryption
+ *
+ *
+ * @param    user_ctx
+ * @param    key_info
+ * @param    sym_ctx
+ * @param    length
+ * @param    pt
+ * @param    ct
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_symmetric_decrypt(fsl_shw_uco_t * user_ctx,
+					   fsl_shw_sko_t * key_info,
+					   fsl_shw_scco_t * sym_ctx,
+					   uint32_t length,
+					   const uint8_t * ct, uint8_t * pt)
+{
+	fsl_shw_return_t ret;
+
+	ret = do_symmetric(user_ctx, key_info, sym_ctx, SYM_DECRYPT,
+			   length, ct, pt);
+
+	return ret;
+}
diff --git a/drivers/mxc/security/sahara2/fsl_shw_user.c b/drivers/mxc/security/sahara2/fsl_shw_user.c
new file mode 100644
index 000000000000..49ec97e7662a
--- /dev/null
+++ b/drivers/mxc/security/sahara2/fsl_shw_user.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_shw_user.c
+ *
+ * This file implements user and platform capabilities functions of the FSL SHW
+ * API for Sahara
+ */
+#include "sahara.h"
+#include <adaptor.h>
+#include <sf_util.h>
+
+#ifdef __KERNEL__
+EXPORT_SYMBOL(fsl_shw_get_capabilities);
+EXPORT_SYMBOL(fsl_shw_register_user);
+EXPORT_SYMBOL(fsl_shw_deregister_user);
+EXPORT_SYMBOL(fsl_shw_get_results);
+#endif				/* __KERNEL__ */
+
+struct cap_t {
+	unsigned populated;
+	union {
+		uint32_t buffer[sizeof(fsl_shw_pco_t)];
+		fsl_shw_pco_t pco;
+	};
+};
+
+static struct cap_t cap = {
+	0,
+	{}
+};
+
+/* REQ-S2LRD-PINTFC-API-GEN-003 */
+/*!
+ * Determine the hardware security capabilities of this platform.
+ *
+ * Though a user context object is passed into this function, it will always
+ * act in a non-blocking manner.
+ *
+ * @param  user_ctx   The user context which will be used for the query.
+ *
+ * @return  A pointer to the capabilities object.
+ */
+fsl_shw_pco_t *fsl_shw_get_capabilities(fsl_shw_uco_t * user_ctx)
+{
+	fsl_shw_pco_t *retval = NULL;
+
+	if (cap.populated) {
+		retval = &cap.pco;
+	} else {
+		if (get_capabilities(user_ctx, &cap.pco) == FSL_RETURN_OK_S) {
+			cap.populated = 1;
+			retval = &cap.pco;
+		}
+	}
+	return retval;
+}
+
+/* REQ-S2LRD-PINTFC-API-GEN-004 */
+
+/*!
+ * Create an association between the the user and the provider of the API.
+ *
+ * @param  user_ctx   The user context which will be used for this association.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_register_user(fsl_shw_uco_t * user_ctx)
+{
+	return sah_register(user_ctx);
+}
+
+/* REQ-S2LRD-PINTFC-API-GEN-005 */
+
+/*!
+ * Destroy the association between the the user and the provider of the API.
+ *
+ * @param  user_ctx   The user context which is no longer needed.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_deregister_user(fsl_shw_uco_t * user_ctx)
+{
+	return sah_deregister(user_ctx);
+}
+
+/* REQ-S2LRD-PINTFC-API-GEN-006 */
+
+/*!
+ * Retrieve results from earlier operations.
+ *
+ * @param         user_ctx     The user's context.
+ * @param         result_size  The number of array elements of @a results.
+ * @param[in,out] results      Pointer to first of the (array of) locations to
+ *                             store results.
+ * @param[out]    result_count Pointer to store the number of results which
+ *                             were returned.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_get_results(fsl_shw_uco_t * user_ctx,
+				     unsigned result_size,
+				     fsl_shw_result_t results[],
+				     unsigned *result_count)
+{
+	fsl_shw_return_t status;
+
+	/* perform a sanity check on the uco */
+	status = sah_validate_uco(user_ctx);
+
+	/* if uco appears ok, build structure and pass to get results */
+	if (status == FSL_RETURN_OK_S) {
+		sah_results arg;
+
+		/* if requested is zero, it's done before it started */
+		if (result_size > 0) {
+			arg.requested = result_size;
+			arg.actual = result_count;
+			arg.results = results;
+			/* get the results */
+			status = sah_get_results(&arg, user_ctx);
+		}
+	}
+
+	return status;
+}
diff --git a/drivers/mxc/security/sahara2/fsl_shw_wrap.c b/drivers/mxc/security/sahara2/fsl_shw_wrap.c
new file mode 100644
index 000000000000..08fd7fa8dfee
--- /dev/null
+++ b/drivers/mxc/security/sahara2/fsl_shw_wrap.c
@@ -0,0 +1,967 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_shw_wrap.c
+ *
+ * This file implements Key-Wrap (Black Key) functions of the FSL SHW API for
+ * Sahara.
+ *
+ * - Ownerid is an 8-byte, user-supplied, value to keep KEY confidential.
+ * - KEY is a 1-32 byte value which starts in SCC RED RAM before
+ *     wrapping, and ends up there on unwrap.  Length is limited because of
+ *    size of SCC1 RAM.
+ * - KEY' is the encrypted KEY
+ * - LEN is a 1-byte (for now) byte-length of KEY
+ * - ALG is a 1-byte value for the algorithm which which the key is
+ *    associated.  Values are defined by the FSL SHW API
+ * - Ownerid, LEN, and ALG come from the user's "key_info" object, as does the
+ *    slot number where KEY already is/will be.
+ * - T is a Nonce
+ * - T' is the encrypted T
+ * - KEK is a Key-Encryption Key for the user's Key
+ * - ICV is the "Integrity Check Value" for the wrapped key
+ * - Black Key is the string of bytes returned as the wrapped key
+<table>
+<tr><TD align="right">BLACK_KEY <TD width="3">=<TD>ICV | T' | LEN | ALG |
+     KEY'</td></tr>
+<tr><td>&nbsp;</td></tr>
+
+<tr><th>To Wrap</th></tr>
+<tr><TD align="right">T</td> <TD width="3">=</td> <TD>RND()<sub>16</sub>
+    </td></tr>
+<tr><TD align="right">KEK</td><TD width="3">=</td><TD>HASH<sub>sha256</sub>(T |
+     Ownerid)<sub>16</sub></td></tr>
+<tr><TD align="right">KEY'<TD width="3">=</td><TD>
+     AES<sub>ctr-enc</sub>(Key=KEK, CTR=0, Data=KEY)</td></tr>
+<tr><TD align="right">ICV</td><TD width="3">=</td><td>HMAC<sub>sha256</sub>
+     (Key=T, Data=Ownerid | LEN | ALG | KEY')<sub>16</sub></td></tr>
+<tr><TD align="right">T'</td><TD width="3">=</td><TD>TDES<sub>cbc-enc</sub>
+     (Key=SLID, IV=Ownerid, Data=T)</td></tr>
+
+<tr><td>&nbsp;</td></tr>
+
+<tr><th>To Unwrap</th></tr>
+<tr><TD align="right">T</td><TD width="3">=</td><TD>TDES<sub>ecb-dec</sub>
+    (Key=SLID, IV=Ownerid, Data=T')</sub></td></tr>
+<tr><TD align="right">ICV</td><TD width="3">=</td><td>HMAC<sub>sha256</sub>
+    (Key=T, Data=Ownerid | LEN | ALG | KEY')<sub>16</sub></td></tr>
+<tr><TD align="right">KEK</td><TD width="3">=</td><td>HASH<sub>sha256</sub>
+    (T | Ownerid)<sub>16</sub></td></tr>
+<tr><TD align="right">KEY<TD width="3">=</td><TD>AES<sub>ctr-dec</sub>
+    (Key=KEK, CTR=0, Data=KEY')</td></tr>
+</table>
+
+ */
+
+#include "sahara.h"
+#include "fsl_platform.h"
+#include "fsl_shw_keystore.h"
+
+#include "sf_util.h"
+#include "adaptor.h"
+
+#if defined(DIAG_SECURITY_FUNC)
+#include <diagnostic.h>
+#endif
+
+#if defined(NEED_CTR_WORKAROUND)
+/* CTR mode needs block-multiple data in/out */
+#define LENGTH_PATCH  16
+#define LENGTH_PATCH_MASK  0xF
+#else
+#define LENGTH_PATCH  4
+#define LENGTH_PATCH_MASK  3
+#endif
+
+#if LENGTH_PATCH
+#define ROUND_LENGTH(len)                                                  \
+({                                                                         \
+    uint32_t orig_len = len;                                               \
+    uint32_t new_len;                                                      \
+                                                                           \
+   if ((orig_len & LENGTH_PATCH_MASK) != 0) {                              \
+       new_len = (orig_len + LENGTH_PATCH                                  \
+            - (orig_len & LENGTH_PATCH_MASK));                             \
+   }                                                                       \
+   else {                                                                  \
+       new_len = orig_len;                                                 \
+   }                                                                       \
+       new_len;                                                            \
+})
+#else
+#define ROUND_LENGTH(len) (len)
+#endif
+
+#ifdef __KERNEL__
+EXPORT_SYMBOL(fsl_shw_establish_key);
+EXPORT_SYMBOL(fsl_shw_extract_key);
+EXPORT_SYMBOL(fsl_shw_release_key);
+EXPORT_SYMBOL(fsl_shw_read_key);
+#endif
+
+#define ICV_LENGTH 16
+#define T_LENGTH 16
+#define KEK_LENGTH 16
+#define LENGTH_LENGTH 1
+#define ALGORITHM_LENGTH 1
+#define FLAGS_LENGTH 1
+
+/* ICV | T' | LEN | ALG | KEY' */
+#define ICV_OFFSET       0
+#define T_PRIME_OFFSET   (ICV_OFFSET + ICV_LENGTH)
+#define LENGTH_OFFSET    (T_PRIME_OFFSET + T_LENGTH)
+#define ALGORITHM_OFFSET (LENGTH_OFFSET + LENGTH_LENGTH)
+#define FLAGS_OFFSET     (ALGORITHM_OFFSET + ALGORITHM_LENGTH)
+#define KEY_PRIME_OFFSET (FLAGS_OFFSET + FLAGS_LENGTH)
+#define FLAGS_SW_KEY     0x01
+
+/*
+ * For testing of the algorithm implementation,, the DO_REPEATABLE_WRAP flag
+ * causes the T_block to go into the T field during a wrap operation.  This
+ * will make the black key value repeatable (for a given SCC secret key, or
+ * always if the default key is in use).
+ *
+ * Normally, a random sequence is used.
+ */
+#ifdef DO_REPEATABLE_WRAP
+/*!
+ * Block of zeroes which is maximum Symmetric block size, used for
+ * initializing context register, etc.
+ */
+static uint8_t T_block_[16] = {
+	0x42, 0, 0, 0x42, 0x42, 0, 0, 0x42,
+	0x42, 0, 0, 0x42, 0x42, 0, 0, 0x42
+};
+#endif
+
+/*!
+ * Insert descriptors to calculate ICV = HMAC(key=T, data=LEN|ALG|KEY')
+ *
+ * @param  user_ctx      User's context for this operation
+ * @param  desc_chain    Descriptor chain to append to
+ * @param  t_key_info    T's key object
+ * @param  black_key     Beginning of Black Key region
+ * @param  key_length    Number of bytes of key' there are in @c black_key
+ * @param[out] hmac      Location to store ICV.  Will be tagged "USES" so
+ *                       sf routines will not try to free it.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static inline fsl_shw_return_t create_icv_calc(fsl_shw_uco_t * user_ctx,
+					       sah_Head_Desc ** desc_chain,
+					       fsl_shw_sko_t * t_key_info,
+					       const uint8_t * black_key,
+					       uint32_t key_length,
+					       uint8_t * hmac)
+{
+	fsl_shw_return_t sah_code;
+	uint32_t header;
+	sah_Link *link1 = NULL;
+	sah_Link *link2 = NULL;
+
+	/* Load up T as key for the HMAC */
+	header = (SAH_HDR_MDHA_SET_MODE_MD_KEY	/* #6 */
+		  ^ sah_insert_mdha_algorithm_sha256
+		  ^ sah_insert_mdha_init ^ sah_insert_mdha_hmac ^
+		  sah_insert_mdha_pdata ^ sah_insert_mdha_mac_full);
+	sah_code = sah_add_in_key_desc(header, NULL, 0, t_key_info,	/* Reference T in RED */
+				       user_ctx->mem_util, desc_chain);
+	if (sah_code != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Previous step loaded key; Now set up to hash the data */
+	header = SAH_HDR_MDHA_HASH;	/* #10 */
+
+	/* Input - start with ownerid */
+	sah_code = sah_Create_Link(user_ctx->mem_util, &link1,
+				   (void *)&t_key_info->userid,
+				   sizeof(t_key_info->userid),
+				   SAH_USES_LINK_DATA);
+	if (sah_code != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Still input  - Append black-key fields len, alg, key' */
+	sah_code = sah_Append_Link(user_ctx->mem_util, link1,
+				   (void *)black_key + LENGTH_OFFSET,
+				   (LENGTH_LENGTH
+				    + ALGORITHM_LENGTH
+				    + key_length), SAH_USES_LINK_DATA);
+
+	if (sah_code != FSL_RETURN_OK_S) {
+		goto out;
+	}
+	/* Output - computed ICV/HMAC */
+	sah_code = sah_Create_Link(user_ctx->mem_util, &link2,
+				   hmac, ICV_LENGTH,
+				   SAH_USES_LINK_DATA | SAH_OUTPUT_LINK);
+	if (sah_code != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	sah_code = sah_Append_Desc(user_ctx->mem_util, desc_chain,
+				   header, link1, link2);
+
+      out:
+	if (sah_code != FSL_RETURN_OK_S) {
+		(void)sah_Destroy_Link(user_ctx->mem_util, link1);
+		(void)sah_Destroy_Link(user_ctx->mem_util, link2);
+	}
+
+	return sah_code;
+}				/* create_icv_calc */
+
+/*!
+ * Perform unwrapping of a black key into a RED slot
+ *
+ * @param         user_ctx      A user context from #fsl_shw_register_user().
+ * @param[in,out] key_info      The information about the key to be which will
+ *                              be unwrapped... key length, slot info, etc.
+ * @param         black_key     Encrypted key
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static fsl_shw_return_t unwrap(fsl_shw_uco_t * user_ctx,
+			       fsl_shw_sko_t * key_info,
+			       const uint8_t * black_key)
+{
+	SAH_SF_DCLS;
+	uint8_t *hmac = NULL;
+	fsl_shw_sko_t t_key_info;
+	sah_Link *link1 = NULL;
+	sah_Link *link2 = NULL;
+	unsigned i;
+	unsigned rounded_key_length;
+	unsigned original_key_length = key_info->key_length;
+
+	hmac = DESC_TEMP_ALLOC(ICV_LENGTH);
+
+	/* Set up key_info for "T" - use same slot as eventual key */
+	fsl_shw_sko_init(&t_key_info, FSL_KEY_ALG_AES);
+	t_key_info.userid = key_info->userid;
+	t_key_info.handle = key_info->handle;
+	t_key_info.flags = key_info->flags;
+	t_key_info.key_length = T_LENGTH;
+	t_key_info.keystore = key_info->keystore;
+
+	/* Validate SW flags to prevent misuse */
+	if ((key_info->flags & FSL_SKO_KEY_SW_KEY)
+		&& !(black_key[FLAGS_OFFSET] & FLAGS_SW_KEY)) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	/* Compute T = SLID_decrypt(T'); leave in RED slot */
+	if (key_info->keystore == NULL) {
+		/* Key goes in system keystore */
+		ret = do_system_keystore_slot_decrypt(user_ctx,
+					key_info->userid,
+				    t_key_info.handle,
+				    T_LENGTH, 
+				    black_key + T_PRIME_OFFSET);
+	
+	} else {
+			/* Key goes in user keystore */
+			ret = keystore_slot_decrypt(user_ctx,
+							key_info->keystore,
+							key_info->userid,
+							t_key_info.handle,
+							T_LENGTH,
+					   	    black_key + T_PRIME_OFFSET);
+	}
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Compute ICV = HMAC(T, ownerid | len | alg | key' */
+	ret = create_icv_calc(user_ctx, &desc_chain, &t_key_info,
+			      black_key, original_key_length, hmac);
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Creation of sah_Key_Link failed due to bad key"
+			 " flag!\n");
+#endif				/*DIAG_SECURITY_FUNC */
+		goto out;
+	}
+#ifdef DIAG_SECURITY_FUNC
+	LOG_DIAG("Validating MAC of wrapped key");
+#endif
+	SAH_SF_EXECUTE();
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+	SAH_SF_DESC_CLEAN();
+
+	/* Check computed ICV against value in Black Key */
+	for (i = 0; i < ICV_LENGTH; i++) {
+		if (black_key[ICV_OFFSET + i] != hmac[i]) {
+#ifdef DIAG_SECURITY_FUNC
+			LOG_DIAG_ARGS("computed ICV fails at offset %i\n", i);
+
+			{
+				char buff[300];
+				int a;
+				for (a = 0; a < ICV_LENGTH; a++)
+					sprintf(&(buff[a * 2]), "%02x",
+						black_key[ICV_OFFSET + a]);
+				buff[a * 2 + 1] = 0;
+				LOG_DIAG_ARGS("black key: %s", buff);
+
+				for (a = 0; a < ICV_LENGTH; a++)
+					sprintf(&(buff[a * 2]), "%02x",
+						hmac[a]);
+				buff[a * 2 + 1] = 0;
+				LOG_DIAG_ARGS("hmac:      %s", buff);
+			}
+#endif
+			ret = FSL_RETURN_AUTH_FAILED_S;
+			goto out;
+		}
+	}
+
+	/* This is no longer needed. */
+	DESC_TEMP_FREE(hmac);
+
+	/* Compute KEK = SHA256(T | ownerid).  Rewrite slot with value */
+	header = (SAH_HDR_MDHA_SET_MODE_HASH	/* #8 */
+		  ^ sah_insert_mdha_init
+		  ^ sah_insert_mdha_algorithm_sha256 ^ sah_insert_mdha_pdata);
+
+	/* Input - Start with T */
+	ret = sah_Create_Key_Link(user_ctx->mem_util, &link1, &t_key_info);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Still input - append ownerid */
+	ret = sah_Append_Link(user_ctx->mem_util, link1,
+			      (void *)&key_info->userid,
+			      sizeof(key_info->userid), SAH_USES_LINK_DATA);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Output - KEK goes into RED slot */
+	ret = sah_Create_Key_Link(user_ctx->mem_util, &link2, &t_key_info);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Put the Hash calculation into the chain. */
+	ret = sah_Append_Desc(user_ctx->mem_util, &desc_chain,
+			      header, link1, link2);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	/* Compute KEY = AES-decrypt(KEK, KEY') */
+	header = (SAH_HDR_SKHA_SET_MODE_IV_KEY	/* #1 */
+		  ^ sah_insert_skha_mode_ctr
+		  ^ sah_insert_skha_algorithm_aes
+		  ^ sah_insert_skha_modulus_128);
+	/* Load KEK in as the key to use */
+	DESC_IN_KEY(header, 0, NULL, &t_key_info);
+
+	rounded_key_length = ROUND_LENGTH(original_key_length);
+	key_info->key_length = rounded_key_length;
+
+	/* Now set up for computation.  Result in RED */
+	header = SAH_HDR_SKHA_ENC_DEC;	/* #4 */
+	DESC_IN_KEY(header, rounded_key_length, black_key + KEY_PRIME_OFFSET,
+		    key_info);
+
+	/* Perform the operation */
+#ifdef DIAG_SECURITY_FUNC
+	LOG_DIAG("Decrypting key with KEK");
+#endif
+	SAH_SF_EXECUTE();
+
+      out:
+	key_info->key_length = original_key_length;
+	SAH_SF_DESC_CLEAN();
+
+	DESC_TEMP_FREE(hmac);
+
+	/* Erase tracks */
+	t_key_info.userid = 0xdeadbeef;
+	t_key_info.handle = 0xdeadbeef;
+
+	return ret;
+}				/* unwrap */
+
+/*!
+ * Perform wrapping of a black key from a RED slot
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param[in,out] key_info      The information about the key to be which will
+ *                              be wrapped... key length, slot info, etc.
+ * @param      black_key        Place to store encrypted key
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static fsl_shw_return_t wrap(fsl_shw_uco_t * user_ctx,
+			     fsl_shw_sko_t * key_info, uint8_t * black_key)
+{
+	SAH_SF_DCLS;
+	unsigned slots_allocated = 0;	/* boolean */
+	fsl_shw_sko_t T_key_info;	/* for holding T */
+	fsl_shw_sko_t KEK_key_info;	/* for holding KEK */
+	unsigned original_key_length = key_info->key_length;
+	unsigned rounded_key_length;
+	sah_Link *link1;
+	sah_Link *link2;
+
+	black_key[LENGTH_OFFSET] = key_info->key_length;
+	black_key[ALGORITHM_OFFSET] = key_info->algorithm;
+
+	memcpy(&T_key_info, key_info, sizeof(T_key_info));
+	fsl_shw_sko_set_key_length(&T_key_info, T_LENGTH);
+	T_key_info.algorithm = FSL_KEY_ALG_HMAC;
+
+	memcpy(&KEK_key_info, &T_key_info, sizeof(KEK_key_info));
+	KEK_key_info.algorithm = FSL_KEY_ALG_AES;
+
+	if (key_info->keystore == NULL) {
+		/* Key goes in system keystore */
+		ret = do_system_keystore_slot_alloc(user_ctx,
+				T_LENGTH, key_info->userid,
+			    &T_key_info.handle);
+	
+	} else {
+		/* Key goes in user keystore */
+		ret = keystore_slot_alloc(key_info->keystore,
+				  T_LENGTH,
+				  key_info->userid, &T_key_info.handle);
+	}
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	if (key_info->keystore == NULL) {
+		/* Key goes in system keystore */
+		ret = do_system_keystore_slot_alloc(user_ctx,
+				KEK_LENGTH, key_info->userid,
+				&KEK_key_info.handle);
+		
+	} else {
+		/* Key goes in user keystore */
+		ret = keystore_slot_alloc(key_info->keystore,
+			  KEK_LENGTH,  key_info->userid, 
+			  &KEK_key_info.handle);
+	}
+
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("do_scc_slot_alloc() failed");
+#endif
+		if (key_info->keystore == NULL) {
+			/* Key goes in system keystore */
+			(void)do_system_keystore_slot_dealloc(user_ctx,
+				key_info->userid, T_key_info.handle);
+	
+		} else {
+			/* Key goes in user keystore */
+			(void)keystore_slot_dealloc(key_info->keystore,
+				  	key_info->userid, T_key_info.handle);
+		}
+	} else {
+		slots_allocated = 1;
+	}
+
+	/* Set up to compute everything except T' ... */
+#ifndef DO_REPEATABLE_WRAP
+	/* Compute T = RND() */
+	header = SAH_HDR_RNG_GENERATE;	/* Desc. #18 */
+	DESC_KEY_OUT(header, &T_key_info, 0, NULL);
+#else
+	if (key_info->keystore == NULL) {
+		/* Key goes in system keystore */
+		ret = do_system_keystore_slot_load(user_ctx,
+			   T_key_info.userid,
+			   T_key_info.handle, T_block,
+			   T_key_info.key_length);
+	} else {
+		/* Key goes in user keystore */
+		ret = keystore_slot_load(key_info->keystore,
+				 T_key_info.userid,
+				 T_key_info.handle,
+				 T_block, T_key_info.key_length);
+	}
+
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+#endif
+
+	/* Compute KEK = SHA256(T | Ownerid) */
+	header = (SAH_HDR_MDHA_SET_MODE_HASH	/* #8 */
+		  ^ sah_insert_mdha_init
+		  ^ sah_insert_mdha_algorithm[FSL_HASH_ALG_SHA256]
+		  ^ sah_insert_mdha_pdata);
+	/* Input - Start with T */
+	ret = sah_Create_Key_Link(user_ctx->mem_util, &link1, &T_key_info);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+	/* Still input - append ownerid */
+	ret = sah_Append_Link(user_ctx->mem_util, link1,
+			      (void *)&key_info->userid,
+			      sizeof(key_info->userid), SAH_USES_LINK_DATA);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+	/* Output - KEK goes into RED slot */
+	ret = sah_Create_Key_Link(user_ctx->mem_util, &link2, &KEK_key_info);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+	/* Put the Hash calculation into the chain. */
+	ret = sah_Append_Desc(user_ctx->mem_util, &desc_chain,
+			      header, link1, link2);
+	if (ret != FSL_RETURN_OK_S) {
+		goto out;
+	}
+#if defined(NEED_CTR_WORKAROUND)
+	rounded_key_length = ROUND_LENGTH(original_key_length);
+	key_info->key_length = rounded_key_length;
+#else
+	rounded_key_length = original_key_length;
+#endif
+	/* Compute KEY' = AES-encrypt(KEK, KEY) */
+	header = (SAH_HDR_SKHA_SET_MODE_IV_KEY	/* #1 */
+		  ^ sah_insert_skha_mode[FSL_SYM_MODE_CTR]
+		  ^ sah_insert_skha_algorithm[FSL_KEY_ALG_AES]
+		  ^ sah_insert_skha_modulus[FSL_CTR_MOD_128]);
+	/* Set up KEK as key to use */
+	DESC_IN_KEY(header, 0, NULL, &KEK_key_info);
+	header = SAH_HDR_SKHA_ENC_DEC;
+	DESC_KEY_OUT(header, key_info,
+		     key_info->key_length, black_key + KEY_PRIME_OFFSET);
+
+	/* Set up flags info */
+	black_key[FLAGS_OFFSET] = 0;
+	if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+		black_key[FLAGS_OFFSET] |= FLAGS_SW_KEY;
+	}
+
+	/* Compute and store ICV into Black Key */
+	ret = create_icv_calc(user_ctx, &desc_chain, &T_key_info,
+			      black_key, original_key_length,
+			      black_key + ICV_OFFSET);
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Creation of sah_Key_Link failed due to bad key"
+			 " flag!\n");
+#endif				/*DIAG_SECURITY_FUNC */
+		goto out;
+	}
+
+	/* Now get Sahara to do the work. */
+#ifdef DIAG_SECURITY_FUNC
+	LOG_DIAG("Encrypting key with KEK");
+#endif
+	SAH_SF_EXECUTE();
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("sah_Descriptor_Chain_Execute() failed");
+#endif
+		goto out;
+	}
+
+	/* Compute T' = SLID_encrypt(T); Result goes to Black Key */
+	if (key_info->keystore == NULL) {
+		/* Key goes in system keystore */
+		ret = do_system_keystore_slot_encrypt(user_ctx,
+					T_key_info.userid,  T_key_info.handle,
+					T_LENGTH, black_key + T_PRIME_OFFSET);
+	} else {
+		/* Key goes in user keystore */
+		ret = keystore_slot_encrypt(user_ctx,
+					    key_info->keystore,
+					    T_key_info.userid,
+					    T_key_info.handle,
+					    T_LENGTH,
+				  	    black_key + T_PRIME_OFFSET);
+	}
+
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("do_scc_slot_encrypt() failed");
+#endif
+		goto out;
+	}
+
+      out:
+	key_info->key_length = original_key_length;
+
+	SAH_SF_DESC_CLEAN();
+	if (slots_allocated) {
+		if (key_info->keystore == NULL) {
+			/* Key goes in system keystore */
+			(void)do_system_keystore_slot_dealloc(user_ctx,
+							      key_info->userid,
+							      T_key_info.
+							      handle);
+			(void)do_system_keystore_slot_dealloc(user_ctx,
+							      key_info->userid,
+							      KEK_key_info.
+							      handle);
+		} else {
+			/* Key goes in user keystore */
+			(void)keystore_slot_dealloc(key_info->keystore,
+						    key_info->userid,
+				    T_key_info.handle);
+			(void)keystore_slot_dealloc(key_info->keystore,
+				    key_info->userid,
+				    KEK_key_info.handle);
+	    }
+	}	
+
+	return ret;
+}				/* wrap */
+
+/*!
+ * Place a key into a protected location for use only by cryptographic
+ * algorithms.
+ *
+ * This only needs to be used to a) unwrap a key, or b) set up a key which
+ * could be wrapped with a later call to #fsl_shw_extract_key().  Normal
+ * cleartext keys can simply be placed into #fsl_shw_sko_t key objects with
+ * #fsl_shw_sko_set_key() and used directly.
+ *
+ * The maximum key size supported for wrapped/unwrapped keys is 32 octets.
+ * (This is the maximum reasonable key length on Sahara - 32 octets for an HMAC
+ * key based on SHA-256.)  The key size is determined by the @a key_info.  The
+ * expected length of @a key can be determined by
+ * #fsl_shw_sko_calculate_wrapped_size()
+ *
+ * The protected key will not be available for use until this operation
+ * successfully completes.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param[in,out] key_info      The information about the key to be which will
+ *                              be established.  In the create case, the key
+ *                              length must be set.
+ * @param      establish_type   How @a key will be interpreted to establish a
+ *                              key for use.
+ * @param key                   If @a establish_type is #FSL_KEY_WRAP_UNWRAP,
+ *                              this is the location of a wrapped key.  If
+ *                              @a establish_type is #FSL_KEY_WRAP_CREATE, this
+ *                              parameter can be @a NULL.  If @a establish_type
+ *                              is #FSL_KEY_WRAP_ACCEPT, this is the location
+ *                              of a plaintext key.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_establish_key(fsl_shw_uco_t * user_ctx,
+				       fsl_shw_sko_t * key_info,
+				       fsl_shw_key_wrap_t establish_type,
+				       const uint8_t * key)
+{
+	SAH_SF_DCLS;
+	unsigned original_key_length = key_info->key_length;
+	unsigned rounded_key_length;
+	unsigned slot_allocated = 0;
+	uint32_t old_flags;
+
+	header = SAH_HDR_RNG_GENERATE;	/* Desc. #18 for rand */
+
+	/* TODO: THIS STILL NEEDS TO BE REFACTORED */
+
+	/* Write operations into SCC memory require word-multiple number of
+	 * bytes.  For ACCEPT and CREATE functions, the key length may need
+	 * to be rounded up.  Calculate. */
+	if (LENGTH_PATCH && (original_key_length & LENGTH_PATCH_MASK) != 0) {
+		rounded_key_length = original_key_length + LENGTH_PATCH
+		    - (original_key_length & LENGTH_PATCH_MASK);
+	} else {
+		rounded_key_length = original_key_length;
+	}
+
+	SAH_SF_USER_CHECK();
+	
+	if (key_info->flags & FSL_SKO_KEY_ESTABLISHED) {
+#ifdef DIAG_SECURITY_FUNC
+			ret = FSL_RETURN_BAD_FLAG_S;
+			LOG_DIAG("Key already established\n");
+#endif
+	}
+
+
+	if (key_info->keystore == NULL) {
+		/* Key goes in system keystore */
+		ret = do_system_keystore_slot_alloc(user_ctx,
+						    key_info->key_length,
+						    key_info->userid,
+						    &(key_info->handle));
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG_ARGS
+		    ("key length: %i, handle: %i, rounded key length: %i",
+		     key_info->key_length, key_info->handle,
+		     rounded_key_length);
+#endif
+
+	} else {
+		/* Key goes in user keystore */
+		ret = keystore_slot_alloc(key_info->keystore,
+					  key_info->key_length,
+					  key_info->userid,
+					  &(key_info->handle));
+	}
+	if (ret != FSL_RETURN_OK_S) {
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Slot allocation failed\n");
+#endif
+		goto out;
+	}
+	slot_allocated = 1;
+
+	key_info->flags |= FSL_SKO_KEY_ESTABLISHED;
+	switch (establish_type) {
+	case FSL_KEY_WRAP_CREATE:
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Creating random key\n");
+#endif
+		/* Use safe version of key length */
+		key_info->key_length = rounded_key_length;
+		/* Generate descriptor to put random value into */
+		DESC_KEY_OUT(header, key_info, 0, NULL);
+		/* Restore actual, desired key length */
+		key_info->key_length = original_key_length;
+
+		old_flags = user_ctx->flags;
+		/* Now put random value into key */
+		SAH_SF_EXECUTE();
+		/* Restore user's old flag value */
+		user_ctx->flags = old_flags;
+#ifdef DIAG_SECURITY_FUNC
+		if (ret == FSL_RETURN_OK_S) {
+			LOG_DIAG("ret is ok");
+		} else {
+			LOG_DIAG("ret is not ok");
+		}
+#endif
+		break;
+
+	case FSL_KEY_WRAP_ACCEPT:
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Accepting plaintext key\n");
+#endif
+		if (key == NULL) {
+#ifdef DIAG_SECURITY_FUNC
+			LOG_DIAG("ACCEPT:  Red Key is NULL");
+#endif
+			ret = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+		/* Copy in safe number of bytes of Red key */
+		if (key_info->keystore == NULL) {
+			/* Key goes in system keystore */
+			ret = do_system_keystore_slot_load(user_ctx,
+						   key_info->userid,
+						   key_info->handle, key,
+						   rounded_key_length);
+		} else {
+			/* Key goes in user keystore */
+			ret = keystore_slot_load(key_info->keystore,
+								 key_info->userid,
+								 key_info->handle, key,
+								 key_info->key_length);
+		}
+		break;
+
+	case FSL_KEY_WRAP_UNWRAP:
+#ifdef DIAG_SECURITY_FUNC
+		LOG_DIAG("Unwrapping wrapped key\n");
+#endif
+		/* For now, disallow non-blocking calls. */
+		if (!(user_ctx->flags & FSL_UCO_BLOCKING_MODE)) {
+			ret = FSL_RETURN_BAD_FLAG_S;
+		} else if (key == NULL) {
+			ret = FSL_RETURN_ERROR_S;
+		} else {
+			ret = unwrap(user_ctx, key_info, key);
+		}
+		break;
+
+	default:
+		ret = FSL_RETURN_BAD_FLAG_S;
+		break;
+	}			/* switch */
+
+      out:
+	if (slot_allocated && (ret != FSL_RETURN_OK_S)) {
+		fsl_shw_return_t scc_err;
+		
+		if (key_info->keystore == NULL) {
+			/* Key goes in system keystore */
+			scc_err = do_system_keystore_slot_dealloc(user_ctx,
+						  key_info->userid,
+						  key_info->handle);
+		} else {
+			/* Key goes in user keystore */
+			scc_err = keystore_slot_dealloc(key_info->keystore,
+						key_info->userid,  key_info->handle);
+		}
+		
+		key_info->flags &= ~FSL_SKO_KEY_ESTABLISHED;
+	}
+
+	SAH_SF_DESC_CLEAN();
+
+	return ret;
+}				/* fsl_shw_establish_key() */
+
+/*!
+ * Wrap a key and retrieve the wrapped value.
+ *
+ * A wrapped key is a key that has been cryptographically obscured.  It is
+ * only able to be used with #fsl_shw_establish_key().
+ *
+ * This function will also release the key (see #fsl_shw_release_key()) so
+ * that it must be re-established before reuse.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      key_info         The information about the key to be deleted.
+ * @param[out] covered_key      The location to store the 48-octet wrapped key.
+ *                              (This size is based upon the maximum key size
+ *                              of 32 octets).
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_extract_key(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info,
+				     uint8_t * covered_key)
+{
+	SAH_SF_DCLS;
+
+	SAH_SF_USER_CHECK();
+
+	/* For now, only blocking mode calls are supported */
+	if (user_ctx->flags & FSL_UCO_BLOCKING_MODE) {
+		if (key_info->flags & FSL_SKO_KEY_ESTABLISHED) {
+			ret = wrap(user_ctx, key_info, covered_key);
+			if (ret != FSL_RETURN_OK_S) {
+				goto out;
+			}
+
+			/* Verify that a SW key info really belongs to a SW key */
+			if (key_info->flags & FSL_SKO_KEY_SW_KEY) {
+			/*  ret = FSL_RETURN_BAD_FLAG_S;
+			  goto out;*/
+			}
+
+			/* Need to deallocate on successful extraction */
+			if (key_info->keystore == NULL) {
+				/* Key goes in system keystore */
+				ret = do_system_keystore_slot_dealloc(user_ctx,
+						key_info->userid, key_info->handle);
+			} else {
+				/* Key goes in user keystore */
+				ret = keystore_slot_dealloc(key_info->keystore,
+						key_info->userid, key_info->handle);
+			}	
+			/* Mark key not available in the flags */
+			key_info->flags &=
+			    ~(FSL_SKO_KEY_ESTABLISHED | FSL_SKO_KEY_PRESENT);
+		}
+	}
+
+out:
+	SAH_SF_DESC_CLEAN();
+
+	return ret;
+}
+
+/*!
+ * De-establish a key so that it can no longer be accessed.
+ *
+ * The key will need to be re-established before it can again be used.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      key_info         The information about the key to be deleted.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t fsl_shw_release_key(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info)
+{
+	SAH_SF_DCLS;
+
+	SAH_SF_USER_CHECK();
+
+	if (key_info->flags & FSL_SKO_KEY_ESTABLISHED) {
+		if (key_info->keystore == NULL) {
+			/* Key goes in system keystore */
+			do_system_keystore_slot_dealloc(user_ctx,
+							key_info->userid,
+							key_info->handle);
+		} else {
+			/* Key goes in user keystore */
+			keystore_slot_dealloc(key_info->keystore,
+						key_info->userid,
+					  	key_info->handle);
+		}	
+		key_info->flags &= ~(FSL_SKO_KEY_ESTABLISHED |
+				     FSL_SKO_KEY_PRESENT);
+	}
+
+out:
+	SAH_SF_DESC_CLEAN();
+
+	return ret;
+}
+
+fsl_shw_return_t fsl_shw_read_key(fsl_shw_uco_t * user_ctx,
+				  fsl_shw_sko_t * key_info, uint8_t * key)
+{
+	SAH_SF_DCLS;
+
+	SAH_SF_USER_CHECK();
+
+	if (!(key_info->flags & FSL_SKO_KEY_ESTABLISHED)
+	    || !(key_info->flags & FSL_SKO_KEY_SW_KEY)) {
+		ret = FSL_RETURN_BAD_FLAG_S;
+		goto out;
+	}
+
+	if (key_info->keystore == NULL) {
+		/* Key lives in system keystore */
+		ret = do_system_keystore_slot_read(user_ctx,
+						   key_info->userid,
+						   key_info->handle,
+						   key_info->key_length, key);
+	} else {
+		/* Key lives in user keystore */
+		ret = keystore_slot_read(key_info->keystore,
+					 key_info->userid,
+					 key_info->handle,
+					 key_info->key_length, key);
+	}
+
+      out:
+	SAH_SF_DESC_CLEAN();
+
+	return ret;
+}
diff --git a/drivers/mxc/security/sahara2/include/adaptor.h b/drivers/mxc/security/sahara2/include/adaptor.h
new file mode 100644
index 000000000000..d2cb35b21e4c
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/adaptor.h
@@ -0,0 +1,113 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+* @file adaptor.h
+*
+* @brief The Adaptor component provides an interface to the device
+* driver.
+*
+* Intended to be used by the FSL SHW API, this can also be called directly
+*/
+
+#ifndef ADAPTOR_H
+#define ADAPTOR_H
+
+#include <sahara.h>
+
+/*!
+ * Structure passed during user ioctl() call to submit request.
+ */
+typedef struct sah_dar {
+	sah_Desc *desc_addr;	/*!< head of descriptor chain */
+	uint32_t uco_flags;	/*!< copy of fsl_shw_uco flags field */
+	uint32_t uco_user_ref;	/*!< copy of fsl_shw_uco user_ref */
+	uint32_t result;	/*!< result of descriptor chain request */
+	struct sah_dar *next;	/*!< for driver use */
+} sah_dar_t;
+
+/*!
+ * Structure passed during user ioctl() call to Register a user
+ */
+typedef struct sah_register {
+	uint32_t pool_size;	/*!< max number of outstanding requests possible */
+	uint32_t result;	/*!< result of registration request */
+} sah_register_t;
+
+/*!
+ * Structure passed during ioctl() call to request SCC operation
+ */
+typedef struct scc_data {
+	uint32_t length;	/*!< length of data */
+	uint8_t *in;		/*!< input data */
+	uint8_t *out;		/*!< output data */
+	unsigned direction;	/*!< encrypt or decrypt */
+	fsl_shw_sym_mode_t crypto_mode;	/*!< CBC or EBC */
+	uint8_t *init_vector;	/*!< initialization vector or NULL */
+} scc_data_t;
+
+/*!
+ * Structure passed during user ioctl() calls to manage stored keys and
+ * stored-key slots.
+ */
+typedef struct scc_slot_t {
+	uint64_t ownerid;	/*!< Owner's id to check/set permissions */
+	uint32_t key_length;	/*!< Length of key */
+	uint32_t slot;		/*!< Slot to operation on, or returned slot
+				   number. */
+	uint8_t *key;		/*!< User-memory pointer to key value */
+	fsl_shw_return_t code;	/*!< API return code from operation */
+} scc_slot_t;
+
+/*
+ * Structure passed during user ioctl() calls to manage data stored in secure
+ * partitions.
+ */
+typedef struct scc_region_t {
+    uint32_t partition_base;	/*!< User virtual address of the
+					   partition base. */
+	uint32_t offset;	/*!< Offset from the start of the
+				   partition where the cleartext data
+				   is located. */
+	uint32_t length;	/*!< Length of the region to be
+				   operated on */
+	uint8_t *black_data;	/*!< User virtual address of any black
+				   (encrypted) data. */
+	fsl_shw_cypher_mode_t cypher_mode;	/*!< Cypher mode to use in an encryt/
+						   decrypt operation. */
+	uint32_t IV[4];		/*!< Intialization vector to use in an
+				   encrypt/decrypt operation. */
+	fsl_shw_return_t code;	/*!< API return code from operation */
+} scc_region_t;
+
+/*
+ * Structure passed during user ioctl() calls to manage secure partitions.
+ */
+typedef struct scc_partition_info_t {
+    uint32_t user_base;            /**< Userspace pointer to base of partition */
+    uint32_t permissions;          /**< Permissions to give the partition (only
+                                        used in call to _DROP_PERMS) */
+	fsl_shw_partition_status_t status;	/*!< Status of the partition */
+} scc_partition_info_t;
+
+fsl_shw_return_t adaptor_Exec_Descriptor_Chain(sah_Head_Desc * dar,
+					       fsl_shw_uco_t * uco);
+fsl_shw_return_t sah_get_results(sah_results * arg, fsl_shw_uco_t * uco);
+fsl_shw_return_t sah_register(fsl_shw_uco_t * user_ctx);
+fsl_shw_return_t sah_deregister(fsl_shw_uco_t * user_ctx);
+fsl_shw_return_t get_capabilities(fsl_shw_uco_t * user_ctx,
+							fsl_shw_pco_t *capabilities);
+
+#endif				/* ADAPTOR_H */
+
+/* End of adaptor.h */
diff --git a/drivers/mxc/security/sahara2/include/diagnostic.h b/drivers/mxc/security/sahara2/include/diagnostic.h
new file mode 100644
index 000000000000..57f84d4cbb05
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/diagnostic.h
@@ -0,0 +1,116 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+* @file    diagnostic.h
+*
+* @brief   Macros for outputting kernel and user space diagnostics.
+*/
+
+#ifndef DIAGNOSTIC_H
+#define DIAGNOSTIC_H
+
+#ifndef __KERNEL__		/* linux flag */
+#include <stdio.h>
+#endif
+#include "fsl_platform.h"
+
+#if defined(FSL_HAVE_SAHARA2) || defined(FSL_HAVE_SAHARA4)
+#define DEV_NAME "sahara"
+#elif defined(FSL_HAVE_RNGA) || defined(FSL_HAVE_RNGB) ||                     \
+      defined(FSL_HAVE_RNGC)
+#define DEV_NAME "shw"
+#endif
+
+/*!
+********************************************************************
+* @brief   This macro logs diagnostic messages to stderr.
+*
+* @param   diag  String that must be logged, char *.
+*
+* @return   void
+*
+*/
+//#if defined DIAG_SECURITY_FUNC || defined DIAG_ADAPTOR
+#define LOG_DIAG(diag)                                              \
+({                                                                  \
+    const char* fname = strrchr(__FILE__, '/');                           \
+                                                                    \
+     sah_Log_Diag(fname ? fname+1 : __FILE__, __LINE__, diag);     \
+})
+
+#ifdef __KERNEL__
+
+#define LOG_DIAG_ARGS(fmt, ...)                                               \
+({                                                                            \
+    const char* fname = strrchr(__FILE__, '/');                               \
+    os_printk(KERN_ALERT "%s:%i: " fmt "\n",                                  \
+              fname ? fname+1 : __FILE__,                                     \
+              __LINE__,                                                       \
+              __VA_ARGS__);                                                   \
+})
+
+#else
+
+#define LOG_DIAG_ARGS(fmt, ...)                                               \
+({                                                                            \
+    const char* fname = strrchr(__FILE__, '/');                               \
+    printf("%s:%i: " fmt "\n",                                                \
+           fname ? fname+1 : __FILE__,                                        \
+           __LINE__,                                                          \
+           __VA_ARGS__);                                                      \
+})
+
+#ifndef __KERNEL__
+void sah_Log_Diag(char *source_name, int source_line, char *diag);
+#endif
+#endif /* if define DIAG_SECURITY_FUNC ... */
+
+#ifdef __KERNEL__
+/*!
+********************************************************************
+* @brief   This macro logs kernel diagnostic  messages to the kernel
+*          log.
+*
+* @param   diag  String that must be logged, char *.
+*
+* @return  As for printf()
+*/
+#if 0
+#if defined(DIAG_DRV_IF) || defined(DIAG_DRV_QUEUE) ||                        \
+  defined(DIAG_DRV_STATUS) || defined(DIAG_DRV_INTERRUPT) ||                  \
+   defined(DIAG_MEM) || defined(DIAG_SECURITY_FUNC) || defined(DIAG_ADAPTOR)
+#endif
+#endif
+
+#define LOG_KDIAG_ARGS(fmt, ...)                                              \
+({                                                                            \
+    os_printk (KERN_ALERT "%s (%s:%i): " fmt "\n",                            \
+               DEV_NAME, strrchr(__FILE__, '/')+1, __LINE__, __VA_ARGS__);              \
+})
+
+#define LOG_KDIAG(diag)                                                       \
+    os_printk (KERN_ALERT "%s (%s:%i): %s\n",                             \
+               DEV_NAME, strrchr(__FILE__, '/')+1, __LINE__, diag);
+
+#define sah_Log_Diag(n, l, d)                                                 \
+    os_printk(KERN_ALERT "%s:%i: %s\n", n, l, d)
+
+#else				/* not KERNEL */
+
+#define sah_Log_Diag(n, l, d)                                                 \
+    printf("%s:%i: %s\n", n, l, d)
+
+#endif				/* __KERNEL__ */
+
+#endif				/* DIAGNOSTIC_H */
diff --git a/drivers/mxc/security/sahara2/include/fsl_platform.h b/drivers/mxc/security/sahara2/include/fsl_platform.h
new file mode 100644
index 000000000000..e7a6fd1718b2
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/fsl_platform.h
@@ -0,0 +1,161 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_platform.h
+ *
+ * Header file to isolate code which might be platform-dependent
+ */
+
+#ifndef FSL_PLATFORM_H
+#define FSL_PLATFORM_H
+
+#ifdef __KERNEL__
+#include "portable_os.h"
+#endif
+
+#if defined(FSL_PLATFORM_OTHER)
+
+/* Have Makefile or other method of setting FSL_HAVE_* flags */
+
+#elif defined(CONFIG_ARCH_MX3)	/* i.MX31 */
+
+#define FSL_HAVE_SCC
+#define FSL_HAVE_RTIC
+#define FSL_HAVE_RNGA
+
+#elif defined(CONFIG_ARCH_MX21)
+
+#define FSL_HAVE_HAC
+#define FSL_HAVE_RNGA
+#define FSL_HAVE_SCC
+
+#elif defined(CONFIG_ARCH_MX25)
+
+#define FSL_HAVE_SCC
+#define FSL_HAVE_RNGB
+#define FSL_HAVE_RTIC3
+#define FSL_HAVE_DRYICE
+
+#elif defined(CONFIG_ARCH_MX27)
+
+#define FSL_HAVE_SAHARA2
+#define SUBMIT_MULTIPLE_DARS
+#define FSL_HAVE_RTIC
+#define FSL_HAVE_SCC
+#define ALLOW_LLO_DESCRIPTORS
+
+#elif defined(CONFIG_ARCH_MX35)
+
+#define FSL_HAVE_SCC
+#define FSL_HAVE_RNGC
+#define FSL_HAVE_RTIC
+
+#elif defined(CONFIG_ARCH_MX37)
+
+#define FSL_HAVE_SCC2
+#define FSL_HAVE_RNGC
+#define FSL_HAVE_RTIC2
+#define FSL_HAVE_SRTC
+
+#elif defined(CONFIG_ARCH_MX51)
+
+#define FSL_HAVE_SCC2
+#define FSL_HAVE_SAHARA4
+#define FSL_HAVE_RTIC3
+#define FSL_HAVE_SRTC
+#define NO_RESEED_WORKAROUND
+#define NEED_CTR_WORKAROUND
+#define USE_S2_CCM_ENCRYPT_CHAIN
+#define USE_S2_CCM_DECRYPT_CHAIN
+#define ALLOW_LLO_DESCRIPTORS
+
+#elif defined(CONFIG_ARCH_MXC91131)
+
+#define FSL_HAVE_SCC
+#define FSL_HAVE_RNGC
+#define FSL_HAVE_HAC
+
+#elif defined(CONFIG_ARCH_MXC91221)
+
+#define FSL_HAVE_SCC
+#define FSL_HAVE_RNGC
+#define FSL_HAVE_RTIC2
+
+#elif defined(CONFIG_ARCH_MXC91231)
+
+#define FSL_HAVE_SAHARA2
+#define FSL_HAVE_RTIC
+#define FSL_HAVE_SCC
+#define NO_OUTPUT_1K_CROSSING
+
+#elif defined(CONFIG_ARCH_MXC91311)
+
+#define FSL_HAVE_SCC
+#define FSL_HAVE_RNGC
+
+#elif defined(CONFIG_ARCH_MXC91314)
+
+#define FSL_HAVE_SCC
+#define FSL_HAVE_SAHAR4
+#define FSL_HAVE_RTIC3
+#define NO_RESEED_WORKAROUND
+#define NEED_CTR_WORKAROUND
+#define USE_S2_CCM_ENCRYPT_CHAIN
+#define USE_S2_CCM_DECRYPT_CHAIN
+#define ALLOW_LLO_DESCRIPTORS
+
+#elif defined(CONFIG_ARCH_MXC91321)
+
+#define FSL_HAVE_SAHARA2
+#define FSL_HAVE_RTIC
+#define FSL_HAVE_SCC
+#define SCC_CLOCK_NOT_GATED
+#define NO_OUTPUT_1K_CROSSING
+
+#elif defined(CONFIG_ARCH_MXC92323)
+
+#define FSL_HAVE_SCC2
+#define FSL_HAVE_SAHARA4
+#define FSL_HAVE_PKHA
+#define FSL_HAVE_RTIC2
+#define NO_1K_CROSSING
+#define NO_RESEED_WORKAROUND
+#define NEED_CTR_WORKAROUND
+#define USE_S2_CCM_ENCRYPT_CHAIN
+#define USE_S2_CCM_DECRYPT_CHAIN
+#define ALLOW_LLO_DESCRIPTORS
+
+
+#elif  defined(CONFIG_ARCH_MXC91331)
+
+#define FSL_HAVE_SCC
+#define FSL_HAVE_RNGA
+#define FSL_HAVE_HAC
+#define FSL_HAVE_RTIC
+
+#elif defined(CONFIG_8548)
+
+#define FSL_HAVE_SEC2x
+
+#elif defined(CONFIG_MPC8374)
+
+#define FSL_HAVE_SEC3x
+
+#else
+
+#error UNKNOWN_PLATFORM
+
+#endif				/* platform checks */
+
+#endif				/* FSL_PLATFORM_H */
diff --git a/drivers/mxc/security/sahara2/include/fsl_shw.h b/drivers/mxc/security/sahara2/include/fsl_shw.h
new file mode 100644
index 000000000000..1a2a7639b92a
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/fsl_shw.h
@@ -0,0 +1,2515 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+ * NOTE TO MAINTAINERS: Although this header file is *the* header file to be
+ * #include'd by FSL SHW programs, it does not itself make any definitions for
+ * the API.  Instead, it uses the fsl_platform.h file and / or compiler
+ * environment variables to determine which actual driver header file to
+ * include.  This allows different implementations to contain different
+ * implementations of the various objects, macros, etc., or even to change
+ * which functions are macros and which are not.
+ */
+
+/*!
+ * @file fsl_shw.h
+ *
+ * @brief Definition of the Freescale Security Hardware API.
+ *
+ * See @ref index for an overview of the API.
+ */
+
+/*!
+ * @if USE_MAINPAGE
+ * @mainpage Common API for Freescale Security Hardware (FSL SHW API)
+ * @endif
+ *
+ * @section intro_sec Introduction
+ *
+ * This is the interface definition for the Freescale Security Hardware API
+ * (FSL SHW API) for User Mode and Kernel Mode to access Freescale Security
+ * Hardware components for cryptographic acceleration. The API is intended to
+ * provide cross-platform access to security hardware components of Freescale.
+ *
+ * This documentation has not been approved, and should not be taken to
+ * mean anything definite about future direction.
+ *
+ * Some example code is provided to give some idea of usage of this API.
+ *
+ * Note: This first version has been defined around the capabilities of the
+ * Sahara2 cryptographic accelerator, and may be expanded in the future to
+ * provide support for other platforms.  The Platform Capabilities Object is
+ * intended as a way to allow programs to adapt to different platforms.
+ *
+ * The i.MX25 is an example of a platform without a SAHARA but yet has
+ * capabilities supported by this API.  These include #fsl_shw_get_random() and
+ * #fsl_shw_add_entropy(), and the use of Triple-DES (TDEA) cipher algorithm
+ * (with no checking of key parity supported) in ECB and CBC modes with @ref
+ * sym_sec.  See also the @ref di_sec for information on key handling, and @ref
+ * td_sec for detection of Tamper Events.  Only the random functions are
+ * available from user space on this platform.
+ *
+ * @section usr_ctx The User Context
+ *
+ * The User Context Object (#fsl_shw_uco_t) controls the interaction between
+ * the user program and the API.  It is initialized as part of user
+ * registration (#fsl_shw_register_user()), and is part of every interaction
+ * thereafter.
+ *
+ * @section pf_sec Platform Capabilities
+ *
+ * Since this API is not tied to one specific type of hardware or even one
+ * given version of a given type of hardware, the platform capabilities object
+ * could be used by a portable program to make choices about using software
+ * instead of hardware for certain operations.
+ *
+ * See the #fsl_shw_pco_t, returned by #fsl_shw_get_capabilities().
+ *
+ * @ref pcoops are provided to query its contents.
+ *
+ *
+ * @section sym_sec Symmetric-Key Encryption and Decryption
+ *
+ * Symmetric-Key encryption support is provided for the block cipher algorithms
+ * AES, DES, and Triple DES.  Modes supported are #FSL_SYM_MODE_ECB,
+ * #FSL_SYM_MODE_CBC, and #FSL_SYM_MODE_CTR, though not necessarily all modes
+ * for all algorithms.  There is also support for the stream cipher algorithm
+ * commonly known as ARC4.
+ *
+ * Encryption and decryption are performed by using the functions
+ * #fsl_shw_symmetric_encrypt() and #fsl_shw_symmetric_decrypt(), respectively.
+ * There are two objects which provide information about the operation of these
+ * functions.  They are the #fsl_shw_sko_t, to provide key and algorithm
+ * information; and the #fsl_shw_scco_t, to provide (and store) initial context
+ * or counter value information.
+ *
+ * CCM is not supported by these functions.  For information CCM support, see
+ * @ref cmb_sec.
+ *
+ *
+ * @section hash_sec Cryptographic Hashing
+ *
+ * Hashing is performed by fsl_shw_hash(). Control of the function is through
+ * flags in the #fsl_shw_hco_t.  The algorithms which are
+ * supported are listed in #fsl_shw_hash_alg_t.
+ *
+ * The hashing function works on octet streams.  If a user application needs to
+ * hash a bitstream, it will need to do its own padding of the last block.
+ *
+ *
+ * @section hmac_sec Hashed Message Authentication Codes
+ *
+ * An HMAC is a method of combining a hash and a key so that a message cannot
+ * be faked by a third party.
+ *
+ * The #fsl_shw_hmac() can be used by itself for one-shot or multi-step
+ * operations, or in combination with #fsl_shw_hmac_precompute() to provide the
+ * ability to compute and save the beginning hashes from a key one time, and
+ * then use #fsl_shw_hmac() to calculate an HMAC on each message as it is
+ * processed.
+ *
+ * The maximum key length which is directly supported by this API is 64 octets.
+ * If a longer key size is needed for HMAC, the user will have to hash the key
+ * and present the digest value as the key to be used by the HMAC functions.
+ *
+ *
+ * @section rnd_sec Random Numbers
+ *
+ * Support is available for acquiring random values from a
+ * cryptographically-strong random number generator.  See
+ * #fsl_shw_get_random().  The function #fsl_shw_add_entropy() may be used to
+ * add entropy to the random number generator.
+ *
+ *
+ * @section cmb_sec Combined Cipher and Authentication
+ *
+ * Some schemes require that messages be encrypted and that they also have an
+ * authentication code associated with the message. The function
+ * #fsl_shw_gen_encrypt() will generate the authentication code and encrypt the
+ * message.
+ *
+ * Upon receipt of such a message, the message must be decrypted and the
+ * authentication code validated.  The function
+ * #fsl_shw_auth_decrypt() will perform these steps.
+ *
+ * Only AES-CCM is supported.
+ *
+ *
+ * @section wrap_sec Wrapped Keys
+ *
+ * On platforms with a Secure Memory, the function #fsl_shw_establish_key() can
+ * be used to place a key into the System Keystore.  This key then can be used
+ * directly by the cryptographic hardware.  It later then be wrapped
+ * (cryptographically obscured) by #fsl_shw_extract_key() and stored for later
+ * use.  If a software key (#FSL_SKO_KEY_SW_KEY) was established, then its
+ * value can be retrieved with a call to #fsl_shw_read_key().
+ *
+ * The wrapping and unwrapping functions provide security against unauthorized
+ * use and detection of tampering.
+ *
+ * The functions can also be used with a User Keystore.
+ *
+ * @section smalloc_sec Secure Memory Allocation
+ *
+ * On platforms with multiple partitions of Secure Memory, the function
+ * #fsl_shw_smalloc() can be used to acquire a partition for private use.  The
+ * function #fsl_shw_diminish_perms() can then be used to revoke specific
+ * permissions on the partition, and #fsl_shw_sfree() can be used to release the
+ * partition.
+ *
+ * @section keystore_sec User Keystore
+ *
+ * User Keystore functionality is defined in fsl_shw_keystore.h.  See @ref
+ * user_keystore for details.  This is not supported on platforms without SCC2.
+ *
+ * @section di_sec Hardware key-select extensions - DryIce
+ *
+ * Some platforms have a component called DryIce which allows the software to
+ * control which key will be used by the secure memory encryption hardware.
+ * The choices are the secret per-chip Fused (IIM) Key, an unknown, hardware-
+ * generated Random Key, a software-written Programmed Key, or the IIM Key in
+ * combination with one of the others.  #fsl_shw_pco_check_pk_supported() can
+ * be used to determine whether this feature is available on the platform.
+ * The rest of this section will explain the symmetric ciphering and key
+ * operations which are available on such a platform.
+ *
+ * The function #fsl_shw_sko_init_pf_key() will set up a Secret Key Object to
+ * refer to one of the system's platform keys.  All keys which reference a
+ * platform key must use this initialization function, including a user-
+ * provided key value.  Keys which are intended for software encryption must
+ * use #fsl_shw_sko_init().
+ *
+ * To change the setting of the Programmed Key of the DryIce module,
+ * #fsl_shw_establish_key() must be called with a platform key object of type
+ * #FSL_SHW_PF_KEY_PRG or #FSL_SHW_PF_KEY_IIM_PRG.  The key will be go
+ * into the PK register of DryIce and not to the keystore.  Any symmetric
+ * operation which references either #FSL_SHW_PF_KEY_PRG or
+ * #FSL_SHW_PF_KEY_IIM_PRG will use the current PK value (possibly modified by
+ * the secret fused IIM key).  Before the Flatform Key can be changed, a call to
+ * #fsl_shw_release_key() or #fsl_shw_extract_key() must be made.  Neither
+ * function will change the value in the PK registers, and further ciphering
+ * can take place.
+ *
+ * When #fsl_shw_establish_key() is called to change the PK value, a plaintext
+ * key can be passed in with the #FSL_KEY_WRAP_ACCEPT argument or a previously
+ * wrapped key can be passed in with the #FSL_KEY_WRAP_UNWRAP argument.  If
+ * #FSL_KEY_WRAP_CREATE is passed in, then a random value will be loaded into
+ * the PK register.  The PK value can be wrapped by a call to
+ * #fsl_shw_extract_key() for later use with the #FSL_KEY_WRAP_UNWRAP argument.
+ *
+ * As an alternative to using only the fused key for @ref wrap_sec,
+ * #fsl_shw_uco_set_wrap_key() can be used to select either the random key or
+ * the random key with the fused key as the key which will be used to protect
+ * the one-time value used to wrap the key.  This allows for these
+ * wrapped keys to be dependent upon and therefore unrecoverable after a tamper
+ * event causes the erasure of the DryIce Random Key register.
+ *
+ * The software can request that the hardware generate a (new) Random Key for
+ * DryIce by calling #fsl_shw_gen_random_pf_key().
+ *
+ *
+ * @section td_sec Device Tamper-Detection
+ *
+ * Some platforms have a component which can detect certain types of tampering
+ * with the hardware.  #fsl_shw_read_tamper_event() API will allow the
+ * retrieval of the type of event which caused a tamper-detection failure.
+ *
+ */
+
+/*! @defgroup glossary Glossary
+ *
+ * @li @b AES - Advanced Encryption Standard - An NIST-created block cipher
+ *          originally knowns as Rijndael.
+ * @li @b ARC4 - ARCFOUR - An S-Box-based OFB mode stream cipher.
+ * @li @b CBC - Cipher-Block Chaining - Each encrypted block is XORed with the
+ *          result of the previous block's encryption.
+ * @li @b CCM - A way of combining CBC and CTR to perform cipher and
+ *        authentication.
+ * @li @b ciphertext - @a plaintext which has been encrypted in some fashion.
+ * @li @b context - Information on the state of a cryptographic operation,
+ *        excluding any key.  This could include IV, Counter Value, or SBox.
+ * @li @b CTR - A mode where a counter value is encrypted and then XORed with
+ *        the data.  After each block, the counter value is incremented.
+ * @li @b DES - Data Encryption Standard - An 8-octet-block cipher.
+ * @li @b ECB - Electronic Codebook - A straight encryption/decryption of the
+ *        data.
+ * @li @b hash - A cryptographically strong one-way function performed on data.
+ * @li @b HMAC - Hashed Message Authentication Code - A key-dependent one-way
+ *        hash result, used to verify authenticity of a message.  The equation
+ *        for an HMAC is hash((K + A) || hash((K + B) || msg)), where K is the
+ *        key, A is the constant for the outer hash, B is the constant for the
+ *        inner hash, and hash is the hashing function (MD5, SHA256, etc).
+ * @li @b IPAD - In an HMAC operation, the context generated by XORing the key
+ *        with a constant and then hashing that value as the first block of the
+ *        inner hash.
+ * @li @b IV - An "Initial Vector" or @a context for modes like CBC.
+ * @li @b MAC - A Message Authentication Code.  HMAC, hashing, and CCM all
+ *        produce a MAC.
+ * @li @b mode  - A way of using a cryptographic algorithm.  See ECB, CBC, etc.
+ * @li @b MD5 - Message Digest 5 - A one-way hash function.
+ * @li @b plaintext - Data which has not been encrypted, or has been decrypted
+ *        from @a ciphertext.
+ * @li @b OPAD - In an HMAC operation, the context generated by XORing the key
+ *        with a constant and then hashing that value as the first block of the
+ *        outer hash.
+ * @li @b SHA - Secure Hash Algorithm - A one-way hash function.
+ * @li @b TDES - AKA @b 3DES - Triple Data Encryption Standard - A method of
+ *        using two or three keys and DES to perform three operations (encrypt
+ *        decrypt encrypt) to create a new algorithm.
+ * @li @b XOR - Exclusive-OR.  A Boolean arithmetic function.
+ * @li @b Wrapped value - A (key) which has been encrypted into an opaque datum
+ *        which cannot be unwrapped (decrypted) for use except by an authorized
+ *        user.  Once created, the key is never visible, but may be used for
+ *        other cryptographic operations.
+ */
+
+#ifndef FSL_SHW_H
+#define FSL_SHW_H
+
+/* Set FSL_HAVE_* flags */
+
+#include "fsl_platform.h"
+
+#ifndef API_DOC
+
+#if defined(FSL_HAVE_SAHARA2) || defined(FSL_HAVE_SAHARA4)
+
+#include "sahara.h"
+
+#else
+
+#if defined(FSL_HAVE_RNGA) || defined(FSL_HAVE_RNGB) || defined(FSL_HAVE_RNGC)
+
+#include "rng_driver.h"
+
+#else
+
+#error FSL_SHW_API_platform_not_recognized
+
+#endif
+
+#endif				/* HAVE SAHARA */
+
+#else				/* API_DOC */
+
+#include <inttypes.h>		/* for uint32_t, etc. */
+#include <stdio.h>		/* Mainly for definition of NULL !! */
+
+/* These groups will appear in the order in which they are defined. */
+
+/*!
+ * @defgroup strgrp Objects
+ *
+ * These objects are used to pass information into and out of the API.  Through
+ * flags and other settings, they control the behavior of the @ref opfuns.
+ *
+ * They are manipulated and queried by use of the various access functions.
+ * There are different sets defined for each object.  See @ref objman.
+ */
+
+/*!
+ * @defgroup consgrp Enumerations and other Constants
+ *
+ * This collection of symbols comprise the values which can be passed into
+ * various functions to control how the API will work.
+ */
+
+/*! @defgroup opfuns Operational Functions
+ *
+ * These functions request that the underlying hardware perform cryptographic
+ * operations.  They are the heart of the API.
+ */
+
+/******  Organization the Object Operations under one group ! **********/
+/*! @defgroup objman Object-Manipulation Operations
+ *
+ */
+/*! @addtogroup objman
+    @{ */
+/*!
+ * @defgroup pcoops Platform Context Object Operations
+ *
+ * The Platform Context object is "read-only", so only query operations are
+ * provided for it.  It is returned by the #fsl_shw_get_capabilities()
+ * function.
+ */
+
+/*! @defgroup ucoops User Context Operations
+ *
+ * These operations should be the only access to the #fsl_shw_uco_t
+ * type/struct, as the internal members of the object are subject to change.
+ * The #fsl_shw_uco_init() function must be called before any other use of the
+ * object.
+ */
+
+/*!
+ * @defgroup rops Result Object Operations
+ *
+ * As the Result Object contains the result of one of the @ref opfuns.  The
+ * manipulations provided are query-only.  No initialization is needed for this
+ * object.
+ */
+
+/*!
+ * @defgroup skoops Secret Key Object Operations
+ *
+ * These operations should be the only access to the #fsl_shw_sko_t
+ * type/struct, as the internal members of that object are subject to change.
+ */
+
+/*!
+ * @defgroup ksoops Keystore Object Operations
+ *
+ * These operations should be the only access to the #fsl_shw_kso_t
+ * type/struct, as the internal members of that object are subject to change.
+ */
+
+/*!
+ * @defgroup hcops Hash Context Object Operations
+ *
+ * These operations should be the only access to the #fsl_shw_hco_t
+ * type/struct, as the internal members of that object are subject to change.
+ */
+
+/*!
+ * @defgroup hmcops HMAC Context Object Operations
+ *
+ * These operations should be the only access to the #fsl_shw_hmco_t
+ * type/struct, as the internal members of that object are subject to change.
+ */
+
+/*!
+ * @defgroup sccops Symmetric Cipher Context Operations
+ *
+ * These operations should be the only access to the #fsl_shw_scco_t
+ * type/struct, as the internal members of that object are subject to change
+ */
+
+/*! @defgroup accoops Authentication-Cipher Context Object Operations
+ *
+ * These functions operate on a #fsl_shw_acco_t.  Their purpose is to set
+ * flags, fields, etc., in order to control the operation of
+ * #fsl_shw_gen_encrypt() and #fsl_shw_auth_decrypt().
+ */
+
+	 /* @} *//************ END GROUPING of Object Manipulations *****************/
+
+/*! @defgroup miscfuns Miscellaneous Functions
+ *
+ * These functions are neither @ref opfuns nor @ref objman.  Their behavior
+ * does not depend upon the flags in the #fsl_shw_uco_t, yet they may involve
+ * more interaction with the library and the kernel than simply querying an
+ * object.
+ */
+
+/******************************************************************************
+ * Enumerations
+ *****************************************************************************/
+/*! @addtogroup consgrp
+    @{ */
+
+/*!
+ * Flags for the state of the User Context Object (#fsl_shw_uco_t).
+ *
+ * These flags describe how the @ref opfuns will operate.
+ */
+typedef enum fsl_shw_user_ctx_flags_t {
+	/*!
+	 * API will block the caller until operation completes.  The result will be
+	 * available in the return code.  If this is not set, user will have to get
+	 * results using #fsl_shw_get_results().
+	 */
+	FSL_UCO_BLOCKING_MODE,
+	/*!
+	 * User wants callback (at the function specified with
+	 * #fsl_shw_uco_set_callback()) when the operation completes.  This flag is
+	 * valid only if #FSL_UCO_BLOCKING_MODE is not set.
+	 */
+	FSL_UCO_CALLBACK_MODE,
+	/*! Do not free descriptor chain after driver (adaptor) finishes */
+	FSL_UCO_SAVE_DESC_CHAIN,
+	/*!
+	 * User has made at least one request with callbacks requested, so API is
+	 * ready to handle others.
+	 */
+	FSL_UCO_CALLBACK_SETUP_COMPLETE,
+	/*!
+	 * (virtual) pointer to descriptor chain is completely linked with physical
+	 * (DMA) addresses, ready for the hardware.  This flag should not be used
+	 * by FSL SHW API programs.
+	 */
+	FSL_UCO_CHAIN_PREPHYSICALIZED,
+	/*!
+	 * The user has changed the context but the changes have not been copied to
+	 * the kernel driver.
+	 */
+	FSL_UCO_CONTEXT_CHANGED,
+	/*! Internal Use.  This context belongs to a user-mode API user. */
+	FSL_UCO_USERMODE_USER,
+} fsl_shw_user_ctx_flags_t;
+
+/*!
+ * Return code for FSL_SHW library.
+ *
+ * These codes may be returned from a function call.  In non-blocking mode,
+ * they will appear as the status in a Result Object.
+ */
+typedef enum fsl_shw_return_t {
+	/*!
+	 * No error.  As a function return code in Non-blocking mode, this may
+	 * simply mean that the operation was accepted for eventual execution.
+	 */
+	FSL_RETURN_OK_S = 0,
+	/*! Failure for non-specific reason. */
+	FSL_RETURN_ERROR_S,
+	/*!
+	 * Operation failed because some resource was not able to be allocated.
+	 */
+	FSL_RETURN_NO_RESOURCE_S,
+	/*! Crypto algorithm unrecognized or improper. */
+	FSL_RETURN_BAD_ALGORITHM_S,
+	/*! Crypto mode unrecognized or improper. */
+	FSL_RETURN_BAD_MODE_S,
+	/*! Flag setting unrecognized or inconsistent. */
+	FSL_RETURN_BAD_FLAG_S,
+	/*! Improper or unsupported key length for algorithm. */
+	FSL_RETURN_BAD_KEY_LENGTH_S,
+	/*! Improper parity in a (DES, TDES) key. */
+	FSL_RETURN_BAD_KEY_PARITY_S,
+	/*!
+	 * Improper or unsupported data length for algorithm or internal buffer.
+	 */
+	FSL_RETURN_BAD_DATA_LENGTH_S,
+	/*! Authentication / Integrity Check code check failed. */
+	FSL_RETURN_AUTH_FAILED_S,
+	/*! A memory error occurred. */
+	FSL_RETURN_MEMORY_ERROR_S,
+	/*! An error internal to the hardware occurred. */
+	FSL_RETURN_INTERNAL_ERROR_S,
+	/*! ECC detected Point at Infinity */
+	FSL_RETURN_POINT_AT_INFINITY_S,
+	/*! ECC detected No Point at Infinity */
+	FSL_RETURN_POINT_NOT_AT_INFINITY_S,
+	/*! GCD is One */
+	FSL_RETURN_GCD_IS_ONE_S,
+	/*! GCD is not One */
+	FSL_RETURN_GCD_IS_NOT_ONE_S,
+	/*! Candidate is Prime */
+	FSL_RETURN_PRIME_S,
+	/*! Candidate is not Prime */
+	FSL_RETURN_NOT_PRIME_S,
+	/*! N register loaded improperly with even value */
+	FSL_RETURN_EVEN_MODULUS_ERROR_S,
+	/*! Divisor is zero. */
+	FSL_RETURN_DIVIDE_BY_ZERO_ERROR_S,
+	/*! Bad Exponent or Scalar value for Point Multiply */
+	FSL_RETURN_BAD_EXPONENT_ERROR_S,
+	/*! RNG hardware problem. */
+	FSL_RETURN_OSCILLATOR_ERROR_S,
+	/*! RNG hardware problem. */
+	FSL_RETURN_STATISTICS_ERROR_S,
+} fsl_shw_return_t;
+
+/*!
+ * Algorithm Identifier.
+ *
+ * Selection of algorithm will determine how large the block size of the
+ * algorithm is.   Context size is the same length unless otherwise specified.
+ * Selection of algorithm also affects the allowable key length.
+ */
+typedef enum fsl_shw_key_alg_t {
+	FSL_KEY_ALG_HMAC,	/*!< Key will be used to perform an HMAC.  Key
+				   size is 1 to 64 octets.  Block size is 64
+				   octets. */
+	FSL_KEY_ALG_AES,	/*!< Advanced Encryption Standard (Rijndael).
+				   Block size is 16 octets.  Key size is 16
+				   octets.  (The single choice of key size is a
+				   Sahara platform limitation.) */
+	FSL_KEY_ALG_DES,	/*!< Data Encryption Standard.  Block size is
+				   8 octets.  Key size is 8 octets. */
+	FSL_KEY_ALG_TDES,	/*!< 2- or 3-key Triple DES.  Block size is 8
+				   octets.  Key size is 16 octets for 2-key
+				   Triple DES, and 24 octets for 3-key. */
+	FSL_KEY_ALG_ARC4	/*!< ARC4.  No block size.  Context size is 259
+				   octets.  Allowed key size is 1-16 octets.
+				   (The choices for key size are a Sahara
+				   platform limitation.) */
+} fsl_shw_key_alg_t;
+
+/*!
+ * Mode selector for Symmetric Ciphers.
+ *
+ * The selection of mode determines how a cryptographic algorithm will be
+ * used to process the plaintext or ciphertext.
+ *
+ * For all modes which are run block-by-block (that is, all but
+ * #FSL_SYM_MODE_STREAM), any partial operations must be performed on a text
+ * length which is multiple of the block size.  Except for #FSL_SYM_MODE_CTR,
+ * these block-by-block algorithms must also be passed a total number of octets
+ * which is a multiple of the block size.
+ *
+ * In modes which require that the total number of octets of data be a multiple
+ * of the block size (#FSL_SYM_MODE_ECB and #FSL_SYM_MODE_CBC), and the user
+ * has a total number of octets which are not a multiple of the block size, the
+ * user must perform any necessary padding to get to the correct data length.
+ */
+typedef enum fsl_shw_sym_mode_t {
+	/*!
+	 * Stream.  There is no associated block size.  Any request to process data
+	 * may be of any length.  This mode is only for ARC4 operations, and is
+	 * also the only mode used for ARC4.
+	 */
+	FSL_SYM_MODE_STREAM,
+
+	/*!
+	 * Electronic Codebook.  Each block of data is encrypted/decrypted.  The
+	 * length of the data stream must be a multiple of the block size.  This
+	 * mode may be used for DES, 3DES, and AES.  The block size is determined
+	 * by the algorithm.
+	 */
+	FSL_SYM_MODE_ECB,
+	/*!
+	 * Cipher-Block Chaining.  Each block of data is encrypted/decrypted and
+	 * then "chained" with the previous block by an XOR function.  Requires
+	 * context to start the XOR (previous block).  This mode may be used for
+	 * DES, 3DES, and AES.  The block size is determined by the algorithm.
+	 */
+	FSL_SYM_MODE_CBC,
+	/*!
+	 * Counter.  The counter is encrypted, then XORed with a block of data.
+	 * The counter is then incremented (using modulus arithmetic) for the next
+	 * block. The final operation may be non-multiple of block size.  This mode
+	 * may be used for AES.  The block size is determined by the algorithm.
+	 */
+	FSL_SYM_MODE_CTR,
+} fsl_shw_sym_mode_t;
+
+/*!
+ * Algorithm selector for Cryptographic Hash functions.
+ *
+ * Selection of algorithm determines how large the context and digest will be.
+ * Context is the same size as the digest (resulting hash), unless otherwise
+ * specified.
+ */
+typedef enum fsl_shw_hash_alg_t {
+	FSL_HASH_ALG_MD5,	/*!< MD5 algorithm.  Digest is 16 octets. */
+	FSL_HASH_ALG_SHA1,	/*!< SHA-1 (aka SHA or SHA-160) algorithm.
+				   Digest is 20 octets. */
+	FSL_HASH_ALG_SHA224,	/*!< SHA-224 algorithm.  Digest is 28 octets,
+				   though context is 32 octets. */
+	FSL_HASH_ALG_SHA256	/*!< SHA-256 algorithm.  Digest is 32
+				   octets. */
+} fsl_shw_hash_alg_t;
+
+/*!
+ * The type of Authentication-Cipher function which will be performed.
+ */
+typedef enum fsl_shw_acc_mode_t {
+	/*!
+	 * CBC-MAC for Counter.  Requires context and modulus.  Final operation may
+	 * be non-multiple of block size.  This mode may be used for AES.
+	 */
+	FSL_ACC_MODE_CCM,
+	/*!
+	 * SSL mode.  Not supported.  Combines HMAC and encrypt (or decrypt).
+	 * Needs one key object for encryption, another for the HMAC.  The usual
+	 * hashing and symmetric encryption algorithms are supported.
+	 */
+	FSL_ACC_MODE_SSL,
+} fsl_shw_acc_mode_t;
+
+/*!
+ * The operation which controls the behavior of #fsl_shw_establish_key().
+ *
+ * These values are passed to #fsl_shw_establish_key().
+ */
+typedef enum fsl_shw_key_wrap_t {
+	FSL_KEY_WRAP_CREATE,	/*!< Generate a key from random values. */
+	FSL_KEY_WRAP_ACCEPT,	/*!< Use the provided clear key. */
+	FSL_KEY_WRAP_UNWRAP	/*!< Unwrap a previously wrapped key. */
+} fsl_shw_key_wrap_t;
+
+/* REQ-S2LRD-PINTFC-COA-HCO-001 */
+/*!
+ * Flags which control a Hash operation.
+ *
+ *  These may be combined by ORing them together.  See #fsl_shw_hco_set_flags()
+ * and #fsl_shw_hco_clear_flags().
+ */
+typedef enum fsl_shw_hash_ctx_flags_t {
+	FSL_HASH_FLAGS_INIT = 1,	/*!< Context is empty.  Hash is started
+					   from scratch, with a message-processed
+					   count of zero. */
+	FSL_HASH_FLAGS_SAVE = 2,	/*!< Retrieve context from hardware after
+					   hashing.  If used with the
+					   #FSL_HASH_FLAGS_FINALIZE flag, the final
+					   digest value will be saved in the
+					   object. */
+	FSL_HASH_FLAGS_LOAD = 4,	/*!< Place context into hardware before
+					   hashing. */
+	FSL_HASH_FLAGS_FINALIZE = 8,	/*!< PAD message and perform final digest
+					   operation.  If user message is
+					   pre-padded, this flag should not be
+					   used. */
+} fsl_shw_hash_ctx_flags_t;
+
+/*!
+ * Flags which control an HMAC operation.
+ *
+ * These may be combined by ORing them together.  See #fsl_shw_hmco_set_flags()
+ * and #fsl_shw_hmco_clear_flags().
+ */
+typedef enum fsl_shw_hmac_ctx_flags_t {
+	FSL_HMAC_FLAGS_INIT = 1,	/*!< Message context is empty.  HMAC is
+					   started from scratch (with key) or from
+					   precompute of inner hash, depending on
+					   whether
+					   #FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT is
+					   set. */
+	FSL_HMAC_FLAGS_SAVE = 2,	/*!< Retrieve ongoing context from hardware
+					   after hashing.  If used with the
+					   #FSL_HMAC_FLAGS_FINALIZE flag, the final
+					   digest value (HMAC) will be saved in the
+					   object. */
+	FSL_HMAC_FLAGS_LOAD = 4,	/*!< Place ongoing context into hardware
+					   before hashing. */
+	FSL_HMAC_FLAGS_FINALIZE = 8,	/*!< PAD message and perform final HMAC
+					   operations of inner and outer hashes. */
+	FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT = 16	/*!< This means that the context
+						   contains precomputed inner and outer
+						   hash values. */
+} fsl_shw_hmac_ctx_flags_t;
+
+/*!
+ * Flags to control use of the #fsl_shw_scco_t.
+ *
+ * These may be ORed together to get the desired effect.
+ * See #fsl_shw_scco_set_flags() and #fsl_shw_scco_clear_flags()
+ */
+typedef enum fsl_shw_sym_ctx_flags_t {
+	/*!
+	 * Context is empty.  In ARC4, this means that the S-Box needs to be
+	 * generated from the key.  In #FSL_SYM_MODE_CBC mode, this allows an IV of
+	 * zero to be specified.  In #FSL_SYM_MODE_CTR mode, it means that an
+	 * initial CTR value of zero is desired.
+	 */
+	FSL_SYM_CTX_INIT = 1,
+	/*!
+	 * Load context from object into hardware before running cipher.  In
+	 * #FSL_SYM_MODE_CTR mode, this would refer to the Counter Value.
+	 */
+	FSL_SYM_CTX_LOAD = 2,
+	/*!
+	 * Save context from hardware into object after running cipher.  In
+	 * #FSL_SYM_MODE_CTR mode, this would refer to the Counter Value.
+	 */
+	FSL_SYM_CTX_SAVE = 4,
+	/*!
+	 * Context (SBox) is to be unwrapped and wrapped on each use.
+	 * This flag is unsupported.
+	 * */
+	FSL_SYM_CTX_PROTECT = 8,
+} fsl_shw_sym_ctx_flags_t;
+
+/*!
+ * Flags which describe the state of the #fsl_shw_sko_t.
+ *
+ * These may be ORed together to get the desired effect.
+ * See #fsl_shw_sko_set_flags() and #fsl_shw_sko_clear_flags()
+ */
+typedef enum fsl_shw_key_flags_t {
+	FSL_SKO_KEY_IGNORE_PARITY = 1,	/*!< If algorithm is DES or 3DES, do not
+					   validate the key parity bits. */
+	FSL_SKO_KEY_PRESENT = 2,	/*!< Clear key is present in the object. */
+	FSL_SKO_KEY_ESTABLISHED = 4,	/*!< Key has been established for use.  This
+					   feature is not available for all
+					   platforms, nor for all algorithms and
+					   modes. */
+	FSL_SKO_KEY_SW_KEY = 8,	/*!< This key is for software use, and can
+				   be copied out of a keystore by its owner.
+				   The default is that they key is available
+				   only for hardware (or security driver)
+				   use. */
+} fsl_shw_key_flags_t;
+
+/*!
+ * Type of value which is associated with an established key.
+ */
+typedef uint64_t key_userid_t;
+
+/*!
+ * Flags which describe the state of the #fsl_shw_acco_t.
+ *
+ * The @a FSL_ACCO_CTX_INIT and @a FSL_ACCO_CTX_FINALIZE flags, when used
+ * together, provide for a one-shot operation.
+ */
+typedef enum fsl_shw_auth_ctx_flags_t {
+	FSL_ACCO_CTX_INIT = 1,	/*!< Initialize Context(s) */
+	FSL_ACCO_CTX_LOAD = 2,	/*!< Load intermediate context(s).
+				   This flag is unsupported. */
+	FSL_ACCO_CTX_SAVE = 4,	/*!< Save intermediate context(s).
+				   This flag is unsupported. */
+	FSL_ACCO_CTX_FINALIZE = 8,	/*!< Create MAC during this operation. */
+	FSL_ACCO_NIST_CCM = 16,	/*!< Formatting of CCM input data is
+				   performed by calls to
+				   #fsl_shw_ccm_nist_format_ctr_and_iv() and
+				   #fsl_shw_ccm_nist_update_ctr_and_iv().  */
+} fsl_shw_auth_ctx_flags_t;
+
+/*!
+ *  Modulus Selector for CTR modes.
+ *
+ * The incrementing of the Counter value may be modified by a modulus.  If no
+ * modulus is needed or desired for AES, use #FSL_CTR_MOD_128.
+ */
+typedef enum fsl_shw_ctr_mod_t {
+	FSL_CTR_MOD_8,		/*!< Run counter with modulus of 2^8. */
+	FSL_CTR_MOD_16,		/*!< Run counter with modulus of 2^16. */
+	FSL_CTR_MOD_24,		/*!< Run counter with modulus of 2^24. */
+	FSL_CTR_MOD_32,		/*!< Run counter with modulus of 2^32. */
+	FSL_CTR_MOD_40,		/*!< Run counter with modulus of 2^40. */
+	FSL_CTR_MOD_48,		/*!< Run counter with modulus of 2^48. */
+	FSL_CTR_MOD_56,		/*!< Run counter with modulus of 2^56. */
+	FSL_CTR_MOD_64,		/*!< Run counter with modulus of 2^64. */
+	FSL_CTR_MOD_72,		/*!< Run counter with modulus of 2^72. */
+	FSL_CTR_MOD_80,		/*!< Run counter with modulus of 2^80. */
+	FSL_CTR_MOD_88,		/*!< Run counter with modulus of 2^88. */
+	FSL_CTR_MOD_96,		/*!< Run counter with modulus of 2^96. */
+	FSL_CTR_MOD_104,	/*!< Run counter with modulus of 2^104. */
+	FSL_CTR_MOD_112,	/*!< Run counter with modulus of 2^112. */
+	FSL_CTR_MOD_120,	/*!< Run counter with modulus of 2^120. */
+	FSL_CTR_MOD_128		/*!< Run counter with modulus of 2^128. */
+} fsl_shw_ctr_mod_t;
+
+/*!
+ * Permissions flags for Secure Partitions
+ *
+ * They currently map directly to the SCC2 hardware values, but this is not
+ * guarinteed behavior.
+ */
+typedef enum fsl_shw_permission_t {
+/*! SCM Access Permission: Do not zeroize/deallocate partition on SMN Fail state */
+	FSL_PERM_NO_ZEROIZE,
+/*! SCM Access Permission: Enforce trusted key read in  */
+	FSL_PERM_TRUSTED_KEY_READ,
+/*! SCM Access Permission: Ignore Supervisor/User mode in permission determination */
+	FSL_PERM_HD_S,
+/*! SCM Access Permission: Allow Read Access to  Host Domain */
+	FSL_PERM_HD_R,
+/*! SCM Access Permission: Allow Write Access to  Host Domain */
+	FSL_PERM_HD_W,
+/*! SCM Access Permission: Allow Execute Access to  Host Domain */
+	FSL_PERM_HD_X,
+/*! SCM Access Permission: Allow Read Access to Trusted Host Domain */
+	FSL_PERM_TH_R,
+/*! SCM Access Permission: Allow Write Access to Trusted Host Domain */
+	FSL_PERM_TH_W,
+/*! SCM Access Permission: Allow Read Access to Other/World Domain */
+	FSL_PERM_OT_R,
+/*! SCM Access Permission: Allow Write Access to Other/World Domain */
+	FSL_PERM_OT_W,
+/*! SCM Access Permission: Allow Execute Access to Other/World Domain */
+	FSL_PERM_OT_X,
+} fsl_shw_permission_t;
+
+/*!
+ * Select the cypher mode to use for partition cover/uncover operations.
+ *
+ * They currently map directly to the values used in the SCC2 driver, but this
+ * is not guarinteed behavior.
+ */
+typedef enum fsl_shw_cypher_mode_t {
+	FSL_SHW_CYPHER_MODE_ECB,	/*!< ECB mode */
+	FSL_SHW_CYPHER_MODE_CBC,	/*!< CBC mode */
+} fsl_shw_cypher_mode_t;
+
+/*!
+ * Which platform key should be presented for cryptographic use.
+ */
+typedef enum fsl_shw_pf_key_t {
+	FSL_SHW_PF_KEY_IIM,	/*!< Present fused IIM key */
+	FSL_SHW_PF_KEY_PRG,	/*!< Present Program key */
+	FSL_SHW_PF_KEY_IIM_PRG,	/*!< Present IIM ^ Program key */
+	FSL_SHW_PF_KEY_IIM_RND,	/*!< Present Random key */
+	FSL_SHW_PF_KEY_RND,	/*!< Present IIM ^ Random key */
+} fsl_shw_pf_key_t;
+
+/*!
+ * The various security tamper events
+ */
+typedef enum fsl_shw_tamper_t {
+	FSL_SHW_TAMPER_NONE,	/*!< No error detected */
+	FSL_SHW_TAMPER_WTD,	/*!< wire-mesh tampering det */
+	FSL_SHW_TAMPER_ETBD,	/*!< ext tampering det: input B */
+	FSL_SHW_TAMPER_ETAD,	/*!< ext tampering det: input A */
+	FSL_SHW_TAMPER_EBD,	/*!< external boot detected */
+	FSL_SHW_TAMPER_SAD,	/*!< security alarm detected */
+	FSL_SHW_TAMPER_TTD,	/*!< temperature tampering det */
+	FSL_SHW_TAMPER_CTD,	/*!< clock tampering det */
+	FSL_SHW_TAMPER_VTD,	/*!< voltage tampering det */
+	FSL_SHW_TAMPER_MCO,	/*!< monotonic counter overflow */
+	FSL_SHW_TAMPER_TCO,	/*!< time counter overflow */
+} fsl_shw_tamper_t;
+
+/*! @} *//* consgrp */
+
+/******************************************************************************
+ * Data Structures
+ *****************************************************************************/
+/*! @addtogroup strgrp
+    @{ */
+
+/* REQ-S2LRD-PINTFC-COA-IBO-001 */
+/*!
+ * Application Initialization Object
+ *
+ * This object, the operations on it, and its interaction with the driver are
+ * TBD.
+ */
+typedef struct fsl_sho_ibo_t {
+} fsl_sho_ibo_t;
+
+/* REQ-S2LRD-PINTFC-COA-UCO-001 */
+/*!
+ * User Context Object
+ *
+ * This object must be initialized by a call to #fsl_shw_uco_init().  It must
+ * then be passed to #fsl_shw_register_user() before it can be used in any
+ * calls besides those in @ref ucoops.
+ *
+ * It contains the user's configuration for the API, for instance whether an
+ * operation should block, or instead should call back the user upon completion
+ * of the operation.
+ *
+ * See @ref ucoops for further information.
+ */
+typedef struct fsl_shw_uco_t {	/* fsl_shw_user_context_object */
+} fsl_shw_uco_t;
+
+/* REQ-S2LRD-PINTFC-API-GEN-006  ??  */
+/*!
+ * Result Object
+ *
+ * This object will contain success and failure information about a specific
+ * cryptographic request which has been made.
+ *
+ * No direct access to its members should be made by programs.  Instead, the
+ * object should be manipulated using the provided functions.  See @ref rops.
+ */
+typedef struct fsl_shw_result_t {	/* fsl_shw_result */
+} fsl_shw_result_t;
+
+/*!
+ * Keystore Object
+ *
+ * This object holds the context of a user keystore, including the functions
+ * that define the interface and pointers to where the key data is stored.  The
+ * user must supply a set of functions to handle keystore management, including
+ * slot allocation, deallocation, etc.  A default keystore manager is provided
+ * as part of the API.
+ * 
+ * No direct access to its members should be made by programs.  Instead, the
+ * object should be manipulated using the provided functions.  See @ref ksoops.
+ */
+typedef struct fsl_shw_kso_t {	/* fsl_shw_keystore_object */
+} fsl_shw_kso_t;
+
+/* REQ-S2LRD-PINTFC-COA-SKO-001 */
+/*!
+ * Secret Key Object
+ *
+ * This object contains a key for a cryptographic operation, and information
+ * about its current state, its intended usage, etc.  It may instead contain
+ * information about a protected key, or an indication to use a platform-
+ * specific secret key.
+ *
+ * No direct access to its members should be made by programs.  Instead, the
+ * object should be manipulated using the provided functions.  See @ref skoops.
+ */
+typedef struct fsl_shw_sko_t {	/* fsl_shw_secret_key_object */
+} fsl_shw_sko_t;
+
+/* REQ-S2LRD-PINTFC-COA-CO-001 */
+/*!
+ * Platform Capabilities Object
+ *
+ * This object will contain information about the cryptographic features of the
+ * platform which the program is running on.
+ *
+ * No direct access to its members should be made by programs.  Instead, the
+ * object should be manipulated using the provided functions.
+ *
+ * See @ref pcoops.
+ */
+typedef struct fsl_shw_pco_t {	/* fsl_shw_platform_capabilities_object */
+} fsl_shw_pco_t;
+
+/* REQ-S2LRD-PINTFC-COA-HCO-001 */
+/*!
+ * Hash Context Object
+ *
+ * This object contains information to control hashing functions.
+
+ * No direct access to its members should be made by programs.  Instead, the
+ * object should be manipulated using the provided functions.  See @ref hcops.
+ */
+typedef struct fsl_shw_hco_t {	/* fsl_shw_hash_context_object */
+} fsl_shw_hco_t;
+
+/*!
+ * HMAC Context Object
+ *
+ * This object contains information to control HMAC functions.
+
+ * No direct access to its members should be made by programs.  Instead, the
+ * object should be manipulated using the provided functions.  See @ref hmcops.
+ */
+typedef struct fsl_shw_hmco_t {	/* fsl_shw_hmac_context_object */
+} fsl_shw_hmco_t;
+
+/* REQ-S2LRD-PINTFC-COA-SCCO-001 */
+/*!
+ * Symmetric Cipher Context Object
+ *
+ * This object contains information to control Symmetric Ciphering encrypt and
+ * decrypt functions in #FSL_SYM_MODE_STREAM (ARC4), #FSL_SYM_MODE_ECB,
+ * #FSL_SYM_MODE_CBC, and #FSL_SYM_MODE_CTR modes and the
+ * #fsl_shw_symmetric_encrypt() and #fsl_shw_symmetric_decrypt() functions.
+ * CCM mode is controlled with the #fsl_shw_acco_t object.
+ *
+ * No direct access to its members should be made by programs.  Instead, the
+ * object should be manipulated using the provided functions.  See @ref sccops.
+ */
+typedef struct fsl_shw_scco_t {	/* fsl_shw_symmetric_cipher_context_object */
+} fsl_shw_scco_t;
+
+/*!
+ * Authenticate-Cipher Context Object
+
+ * An object for controlling the function of, and holding information about,
+ * data for the authenticate-cipher functions, #fsl_shw_gen_encrypt() and
+ * #fsl_shw_auth_decrypt().
+ *
+ * No direct access to its members should be made by programs.  Instead, the
+ * object should be manipulated using the provided functions.  See @ref
+ * accoops.
+ */
+typedef struct fsl_shw_acco_t {	/* fsl_shw_authenticate_cipher_context_object */
+} fsl_shw_acco_t;
+	  /*! @} *//* strgrp */
+
+/******************************************************************************
+ * Access Macros for Objects
+ *****************************************************************************/
+/*! @addtogroup pcoops
+    @{ */
+
+/*!
+ * Get FSL SHW API version
+ *
+ * @param      pc_info   The Platform Capabilities Object to query.
+ * @param[out] major     A pointer to where the major version
+ *                       of the API is to be stored.
+ * @param[out] minor     A pointer to where the minor version
+ *                       of the API is to be stored.
+ */
+void fsl_shw_pco_get_version(const fsl_shw_pco_t * pc_info,
+			     uint32_t * major, uint32_t * minor);
+
+/*!
+ * Get underlying driver version.
+ *
+ * @param      pc_info   The Platform Capabilities Object to query.
+ * @param[out] major     A pointer to where the major version
+ *                       of the driver is to be stored.
+ * @param[out] minor     A pointer to where the minor version
+ *                       of the driver is to be stored.
+ */
+void fsl_shw_pco_get_driver_version(const fsl_shw_pco_t * pc_info,
+				    uint32_t * major, uint32_t * minor);
+
+/*!
+ * Get list of symmetric algorithms supported.
+ *
+ * @param pc_info   The Platform Capabilities Object to query.
+ * @param[out] algorithms A pointer to where to store the location of
+ *                        the list of algorithms.
+ * @param[out] algorithm_count A pointer to where to store the number of
+ *                             algorithms in the list at @a algorithms.
+ */
+void fsl_shw_pco_get_sym_algorithms(const fsl_shw_pco_t * pc_info,
+				    fsl_shw_key_alg_t * algorithms[],
+				    uint8_t * algorithm_count);
+
+/*!
+ * Get list of symmetric modes supported.
+ *
+ * @param pc_info         The Platform Capabilities Object to query.
+ * @param[out] modes      A pointer to where to store the location of
+ *                        the list of modes.
+ * @param[out] mode_count A pointer to where to store the number of
+ *                        algorithms in the list at @a modes.
+ */
+void fsl_shw_pco_get_sym_modes(const fsl_shw_pco_t * pc_info,
+			       fsl_shw_sym_mode_t * modes[],
+			       uint8_t * mode_count);
+
+/*!
+ * Get list of hash algorithms supported.
+ *
+ * @param pc_info         The Platform Capabilities Object to query.
+ * @param[out] algorithms A pointer which will be set to the list of
+ *                        algorithms.
+ * @param[out] algorithm_count The number of algorithms in the list at @a
+ *                             algorithms.
+ */
+void fsl_shw_pco_get_hash_algorithms(const fsl_shw_pco_t * pc_info,
+				     fsl_shw_hash_alg_t * algorithms[],
+				     uint8_t * algorithm_count);
+
+/*!
+ * Determine whether the combination of a given symmetric algorithm and a given
+ * mode is supported.
+ *
+ * @param pc_info    The Platform Capabilities Object to query.
+ * @param algorithm  A Symmetric Cipher algorithm.
+ * @param mode       A Symmetric Cipher mode.
+ *
+ * @return 0 if combination is not supported, non-zero if supported.
+ */
+int fsl_shw_pco_check_sym_supported(const fsl_shw_pco_t * pc_info,
+				    fsl_shw_key_alg_t algorithm,
+				    fsl_shw_sym_mode_t mode);
+
+/*!
+ * Determine whether a given Encryption-Authentication mode is supported.
+ *
+ * @param pc_info   The Platform Capabilities Object to query.
+ * @param mode       The Authentication mode.
+ *
+ * @return 0 if mode is not supported, non-zero if supported.
+ */
+int fsl_shw_pco_check_auth_supported(const fsl_shw_pco_t * pc_info,
+				     fsl_shw_acc_mode_t mode);
+
+/*!
+ * Determine whether Black Keys (key establishment / wrapping) is supported.
+ *
+ * @param pc_info  The Platform Capabilities Object to query.
+ *
+ * @return 0 if wrapping is not supported, non-zero if supported.
+ */
+int fsl_shw_pco_check_black_key_supported(const fsl_shw_pco_t * pc_info);
+
+/*!
+ * Get FSL SHW SCC driver version
+ *
+ * @param      pc_info   The Platform Capabilities Object to query.
+ * @param[out] major     A pointer to where the major version
+ *                       of the SCC driver is to be stored.
+ * @param[out] minor     A pointer to where the minor version
+ *                       of the SCC driver is to be stored.
+ */
+void fsl_shw_pco_get_scc_driver_version(const fsl_shw_pco_t * pc_info,
+					uint32_t * major, uint32_t * minor);
+
+/*!
+ * Get SCM hardware version
+ *
+ * @param      pc_info   The Platform Capabilities Object to query.
+ * @return               The SCM hardware version
+ */
+uint32_t fsl_shw_pco_get_scm_version(const fsl_shw_pco_t * pc_info);
+
+/*!
+ * Get SMN hardware version
+ *
+ * @param      pc_info   The Platform Capabilities Object to query.
+ * @return               The SMN hardware version
+ */
+uint32_t fsl_shw_pco_get_smn_version(const fsl_shw_pco_t * pc_info);
+
+/*!
+ * Get the size of an SCM block, in bytes
+ *
+ * @param      pc_info   The Platform Capabilities Object to query.
+ * @return               The size of an SCM block, in bytes.
+ */
+uint32_t fsl_shw_pco_get_scm_block_size(const fsl_shw_pco_t * pc_info);
+
+/*!
+ * Get size of Black and Red RAM memory
+ *
+ * @param      pc_info     The Platform Capabilities Object to query.
+ * @param[out] black_size  A pointer to where the size of the Black RAM, in
+ *                         blocks, is to be placed.
+ * @param[out] red_size    A pointer to where the size of the Red RAM, in 
+ *                         blocks, is to be placed.
+ */
+void fsl_shw_pco_get_smn_size(const fsl_shw_pco_t * pc_info,
+			      uint32_t * black_size, uint32_t * red_size);
+
+/*!
+ * Determine whether Secure Partitions are supported
+ *
+ * @param pc_info          The Platform Capabilities Object to query.
+ *
+ * @return 0 if secure partitions are not supported, non-zero if supported.
+ */
+int fsl_shw_pco_check_spo_supported(const fsl_shw_pco_t * pc_info);
+
+/*!
+ * Get the size of a Secure Partitions
+ *
+ * @param pc_info          The Platform Capabilities Object to query.
+ * 
+ * @return Partition size, in bytes.  0 if Secure Partitions not supported.
+ */
+uint32_t fsl_shw_pco_get_spo_size_bytes(const fsl_shw_pco_t * pc_info);
+
+/*!
+ * Get the number of Secure Partitions on this platform
+ *
+ * @param pc_info          The Platform Capabilities Object to query.
+ * 
+ * @return Number of partitions. 0 if Secure Partitions not supported.  Note
+ *         that this returns the total number of partitions, though
+ *         not all may be available to the user.
+ */
+uint32_t fsl_shw_pco_get_spo_count(const fsl_shw_pco_t * pc_info);
+
+/*!
+ * Determine whether Platform Key features are available
+ *
+ * @param pc_info          The Platform Capabilities Object to query.
+ *
+ * @return  1 if Programmed Key features are available, otherwise zero.
+ */
+int fsl_shw_pco_check_pk_supported(const fsl_shw_pco_t * pc_info);
+
+/*!
+  * Determine whether Software Key features are available
+  *
+  * @param pc_info 		 The Platform Capabilities Object to query.
+  *
+  * @return  1 if Software key features are available, otherwise zero.
+  */
+int fsl_shw_pco_check_sw_keys_supported(const fsl_shw_pco_t * pc_info);
+
+/*! @} *//* pcoops */
+
+/*! @addtogroup ucoops
+    @{ */
+
+/*!
+ * Initialize a User Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  It sets the User Context Object to initial values, and set the size
+ * of the results pool.  The mode will be set to a default of
+ * #FSL_UCO_BLOCKING_MODE.
+ *
+ * When using non-blocking operations, this sets the maximum number of
+ * operations which can be outstanding.  This number includes the counts of
+ * operations waiting to start, operation(s) being performed, and results which
+ * have not been retrieved.
+ *
+ * Changes to this value are ignored once user registration has completed.  It
+ * should be set to 1 if only blocking operations will ever be performed.
+ *
+ * @param user_ctx     The User Context object to operate on.
+ * @param pool_size    The maximum number of operations which can be
+ *                     outstanding.
+ */
+void fsl_shw_uco_init(fsl_shw_uco_t * user_ctx, uint16_t pool_size);
+
+/*!
+ * Set the User Reference for the User Context.
+ *
+ * @param user_ctx     The User Context object to operate on.
+ * @param reference    A value which will be passed back with a result.
+ */
+void fsl_shw_uco_set_reference(fsl_shw_uco_t * user_ctx, uint32_t reference);
+
+/*!
+ * Set the callback routine for the User Context.
+ *
+ * Note that the callback routine may be called when no results are available,
+ * and possibly even when no requests are outstanding.
+ *
+ *
+ * @param user_ctx     The User Context object to operate on.
+ * @param callback_fn  The function the API will invoke when an operation
+ *                     completes.
+ */
+void fsl_shw_uco_set_callback(fsl_shw_uco_t * user_ctx,
+			      void (*callback_fn) (fsl_shw_uco_t * uco));
+
+/*!
+ * Set flags in the User Context.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param user_ctx     The User Context object to operate on.
+ * @param flags        ORed values from #fsl_shw_user_ctx_flags_t.
+ */
+void fsl_shw_uco_set_flags(fsl_shw_uco_t * user_ctx, uint32_t flags);
+
+/*!
+ * Clear flags in the User Context.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param user_ctx     The User Context object to operate on.
+ * @param flags        ORed values from #fsl_shw_user_ctx_flags_t.
+ */
+void fsl_shw_uco_clear_flags(fsl_shw_uco_t * user_ctx, uint32_t flags);
+
+/*!
+ * Select a key for the key-wrap key for key wrapping/unwrapping
+ *
+ * Without a call to this function, default is FSL_SHW_PF_KEY_IIM.  The wrap
+ * key is used to encrypt and decrypt the per-key random secret which is used
+ * to calculate the key which will encrypt/decrypt the user's key.
+ *
+ * @param user_ctx     The User Context object to operate on.
+ * @param pf_key       Which key to use.  Valid choices are
+ *                     #FSL_SHW_PF_KEY_IIM, #FSL_SHW_PF_KEY_RND, and
+ *                     #FSL_SHW_PF_KEY_IIM_RND.
+ */
+void fsl_shw_uco_set_wrap_key(fsl_shw_uco_t * user_ctx,
+			      fsl_shw_pf_key_t pf_key);
+
+	  /*! @} *//* ucoops */
+
+/*! @addtogroup rops
+    @{ */
+
+/*!
+ * Retrieve the status code from a Result Object.
+ *
+ * @param result   The result object to query.
+ *
+ * @return The status of the request.
+ */
+fsl_shw_return_t fsl_shw_ro_get_status(fsl_shw_result_t * result);
+
+/*!
+ * Retrieve the reference value from a Result Object.
+ *
+ * @param result   The result object to query.
+ *
+ * @return The reference associated with the request.
+ */
+uint32_t fsl_shw_ro_get_reference(fsl_shw_result_t * result);
+
+	 /* @} *//* rops */
+
+/*! @addtogroup skoops
+    @{ */
+
+/*!
+ * Initialize a Secret Key Object.
+ *
+ * This function or #fsl_shw_sko_init_pf_key() must be called before performing
+ * any other operation with the Object.
+ *
+ * @param key_info  The Secret Key Object to be initialized.
+ * @param algorithm DES, AES, etc.
+ *
+ */
+void fsl_shw_sko_init(fsl_shw_sko_t * key_info, fsl_shw_key_alg_t algorithm);
+
+/*!
+ * Initialize a Secret Key Object to use a Platform Key register.
+ *
+ * This function or #fsl_shw_sko_init() must be called before performing any
+ * other operation with the Object.  #fsl_shw_sko_set_key() does not work on
+ * a key object initialized in this way.
+ *
+ * If this function is used to initialize the key object, but no key is
+ * established with the key object, then the object will refer strictly to the
+ * key value specified by the @c pf_key selection.
+ *
+ * If the pf key is #FSL_SHW_PF_KEY_PRG or #FSL_SHW_PF_KEY_IIM_PRG, then the
+ * key object may be used with #fsl_shw_establish_key() to change the Program
+ * Key value.  When the pf key is neither #FSL_SHW_PF_KEY_PRG nor
+ * #FSL_SHW_PF_KEY_IIM_PRG, it is an error to call #fsl_shw_establish_key().
+ *
+ * @param key_info     The Secret Key Object to be initialized.
+ * @param algorithm    DES, AES, etc.
+ * @param pf_key       Which platform key is referenced.
+ */
+void fsl_shw_sko_init_pf_key(fsl_shw_sko_t * key_info,
+			     fsl_shw_key_alg_t algorithm,
+			     fsl_shw_pf_key_t pf_key);
+
+/*!
+ * Store a cleartext key in the key object.
+ *
+ * This has the side effect of setting the #FSL_SKO_KEY_PRESENT flag. It should
+ * not be used if there is a key established with the key object.  If there is,
+ * a call to #fsl_shw_release_key() should be made first.
+ *
+ * @param key_object   A variable of type #fsl_shw_sko_t.
+ * @param key          A pointer to the beginning of the key.
+ * @param key_length   The length, in octets, of the key.  The value should be
+ *                     appropriate to the key size supported by the algorithm.
+ *                     64 octets is the absolute maximum value allowed for this
+ *                     call.
+ */
+void fsl_shw_sko_set_key(fsl_shw_sko_t * key_object,
+			 const uint8_t * key, uint16_t key_length);
+
+/*!
+ * Set a size for the key.
+ *
+ * This function would normally be used when the user wants the key to be
+ * generated from a random source.
+ *
+ * @param key_object   A variable of type #fsl_shw_sko_t.
+ * @param key_length   The length, in octets, of the key.  The value should be
+ *                     appropriate to the key size supported by the algorithm.
+ *                     64 octets is the absolute maximum value allowed for this
+ *                     call.
+ */
+void fsl_shw_sko_set_key_length(fsl_shw_sko_t * key_object,
+				uint16_t key_length);
+
+/*!
+ * Set the User ID associated with the key.
+ *
+ * @param key_object   A variable of type #fsl_shw_sko_t.
+ * @param userid       The User ID to identify authorized users of the key.
+ */
+void fsl_shw_sko_set_user_id(fsl_shw_sko_t * key_object, key_userid_t userid);
+
+/*!
+ * Set the keystore that the key will be stored in.
+ *
+ * @param key_object    A variable of type #fsl_shw_sko_t.
+ * @param keystore      The keystore to place the key in.  This is a variable of
+ *                      type #fsl_shw_kso_t.
+ */
+void fsl_shw_sko_set_keystore(fsl_shw_sko_t * key_object,
+			      fsl_shw_kso_t * keystore);
+
+/*!
+ * Set the establish key handle into a key object.
+ *
+ * The @a userid field will be used to validate the access to the unwrapped
+ * key.  This feature is not available for all platforms, nor for all
+ * algorithms and modes.
+ *
+ * The #FSL_SKO_KEY_ESTABLISHED will be set (and the #FSL_SKO_KEY_PRESENT
+ * flag will be cleared).
+ *
+ * @param key_object   A variable of type #fsl_shw_sko_t.
+ * @param userid       The User ID to verify this user is an authorized user of
+ *                     the key.
+ * @param handle       A @a handle from #fsl_shw_sko_get_established_info.
+ */
+void fsl_shw_sko_set_established_info(fsl_shw_sko_t * key_object,
+				      key_userid_t userid, uint32_t handle);
+
+/*!
+ * Extract the algorithm from a key object.
+ *
+ * @param      key_info  The Key Object to be queried.
+ * @param[out] algorithm A pointer to the location to store the algorithm.
+ */
+void fsl_shw_sko_get_algorithm(const fsl_shw_sko_t * key_info,
+			       fsl_shw_key_alg_t * algorithm);
+
+/*!
+ * Retrieve the cleartext key from a key object that is stored in a user
+ * keystore.
+ *
+ * @param      skobject     The Key Object to be queried.
+ * @param[out] skkey        A pointer to the location to store the key.  NULL
+ *                          if the key is not stored in a user keystore.
+ */
+void fsl_shw_sko_get_key(const fsl_shw_sko_t * skobject, void *skkey);
+
+/*!
+ * Retrieve the established-key handle from a key object.
+ *
+ * @param key_object   A variable of type #fsl_shw_sko_t.
+ * @param handle       The location to store the @a handle of the unwrapped
+ *                     key.
+ */
+void fsl_shw_sko_get_established_info(fsl_shw_sko_t * key_object,
+				      uint32_t * handle);
+
+/*!
+ * Determine the size of a wrapped key based upon the cleartext key's length.
+ *
+ * This function can be used to calculate the number of octets that
+ * #fsl_shw_extract_key() will write into the location at @a covered_key.
+ *
+ * If zero is returned at @a length, this means that the key length in
+ * @a key_info is not supported.
+ *
+ * @param      key_info         Information about a key to be wrapped.
+ * @param      length           Location to store the length of a wrapped
+ *                              version of the key in @a key_info.
+ */
+void fsl_shw_sko_calculate_wrapped_size(const fsl_shw_sko_t * key_info,
+					uint32_t * length);
+
+/*!
+ * Set some flags in the key object.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param key_object   A variable of type #fsl_shw_sko_t.
+ * @param flags        (One or more) ORed members of #fsl_shw_key_flags_t which
+ *                     are to be set.
+ */
+void fsl_shw_sko_set_flags(fsl_shw_sko_t * key_object, uint32_t flags);
+
+/*!
+ * Clear some flags in the key object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param key_object   A variable of type #fsl_shw_sko_t.
+ * @param flags        (One or more) ORed members of #fsl_shw_key_flags_t which
+ *                      are to be reset.
+ */
+void fsl_shw_sko_clear_flags(fsl_shw_sko_t * key_object, uint32_t flags);
+
+	  /*! @} *//* end skoops */
+
+/*****************************************************************************/
+
+/*! @addtogroup hcops
+    @{ */
+
+/*****************************************************************************/
+/* REQ-S2LRD-PINTFC-API-BASIC-HASH-004 - partially */
+/*!
+ * Initialize a Hash Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  It sets the current message length and hash algorithm in the hash
+ * context object.
+ *
+ * @param      hash_ctx  The hash context to operate upon.
+ * @param      algorithm The hash algorithm to be used (#FSL_HASH_ALG_MD5,
+ *                       #FSL_HASH_ALG_SHA256, etc).
+ *
+ */
+void fsl_shw_hco_init(fsl_shw_hco_t * hash_ctx, fsl_shw_hash_alg_t algorithm);
+
+/*****************************************************************************/
+/* REQ-S2LRD-PINTFC-API-BASIC-HASH-001 */
+/* REQ-S2LRD-PINTFC-API-BASIC-HASH-002 */
+/*!
+ * Get the current hash value and message length from the hash context object.
+ *
+ * The algorithm must have already been specified.  See #fsl_shw_hco_init().
+ *
+ * @param      hash_ctx  The hash context to query.
+ * @param[out] digest    Pointer to the location of @a length octets where to
+ *                       store a copy of the current value of the digest.
+ * @param      length    Number of octets of hash value to copy.
+ * @param[out] msg_length Pointer to the location to store the number of octets
+ *                        already hashed.
+ */
+void fsl_shw_hco_get_digest(const fsl_shw_hco_t * hash_ctx, uint8_t * digest,
+			    uint8_t length, uint32_t * msg_length);
+
+/*****************************************************************************/
+/* REQ-S2LRD-PINTFC-API-BASIC-HASH-002 - partially */
+/*!
+ * Get the hash algorithm from the hash context object.
+ *
+ * @param      hash_ctx  The hash context to query.
+ * @param[out] algorithm Pointer to where the algorithm is to be stored.
+ */
+void fsl_shw_hco_get_info(const fsl_shw_hco_t * hash_ctx,
+			  fsl_shw_hash_alg_t * algorithm);
+
+/*****************************************************************************/
+/* REQ-S2LRD-PINTFC-API-BASIC-HASH-003 */
+/* REQ-S2LRD-PINTFC-API-BASIC-HASH-004 */
+/*!
+ * Set the current hash value and message length in the hash context object.
+ *
+ * The algorithm must have already been specified.  See #fsl_shw_hco_init().
+ *
+ * @param      hash_ctx  The hash context to operate upon.
+ * @param      context   Pointer to buffer of appropriate length to copy into
+ *                       the hash context object.
+ * @param      msg_length The number of octets of the message which have
+ *                        already been hashed.
+ *
+ */
+void fsl_shw_hco_set_digest(fsl_shw_hco_t * hash_ctx, const uint8_t * context,
+			    uint32_t msg_length);
+
+/*!
+ * Set flags in a Hash Context Object.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hash_ctx   The hash context to be operated on.
+ * @param flags      The flags to be set in the context.  These can be ORed
+ *                   members of #fsl_shw_hash_ctx_flags_t.
+ */
+void fsl_shw_hco_set_flags(fsl_shw_hco_t * hash_ctx, uint32_t flags);
+
+/*!
+ * Clear flags in a Hash Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hash_ctx   The hash context to be operated on.
+ * @param flags      The flags to be reset in the context.  These can be ORed
+ *                   members of #fsl_shw_hash_ctx_flags_t.
+ */
+void fsl_shw_hco_clear_flags(fsl_shw_hco_t * hash_ctx, uint32_t flags);
+
+	  /*! @} *//* end hcops */
+
+/*****************************************************************************/
+
+/*! @addtogroup hmcops
+    @{ */
+
+/*!
+ * Initialize an HMAC Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  It sets the current message length and hash algorithm in the HMAC
+ * context object.
+ *
+ * @param      hmac_ctx  The HMAC context to operate upon.
+ * @param      algorithm The hash algorithm to be used (#FSL_HASH_ALG_MD5,
+ *                       #FSL_HASH_ALG_SHA256, etc).
+ *
+ */
+void fsl_shw_hmco_init(fsl_shw_hmco_t * hmac_ctx, fsl_shw_hash_alg_t algorithm);
+
+/*!
+ * Set flags in an HMAC Context Object.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hmac_ctx   The HMAC context to be operated on.
+ * @param flags      The flags to be set in the context.  These can be ORed
+ *                   members of #fsl_shw_hmac_ctx_flags_t.
+ */
+void fsl_shw_hmco_set_flags(fsl_shw_hmco_t * hmac_ctx, uint32_t flags);
+
+/*!
+ * Clear flags in an HMAC Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hmac_ctx   The HMAC context to be operated on.
+ * @param flags      The flags to be reset in the context.  These can be ORed
+ *                   members of #fsl_shw_hmac_ctx_flags_t.
+ */
+void fsl_shw_hmco_clear_flags(fsl_shw_hmco_t * hmac_ctx, uint32_t flags);
+
+/*! @} */
+
+/*****************************************************************************/
+
+/*! @addtogroup sccops
+    @{ */
+
+/*!
+ * Initialize a Symmetric Cipher Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  This will set the @a mode and @a algorithm and initialize the
+ * Object.
+ *
+ * @param sym_ctx   The context object to operate on.
+ * @param algorithm The cipher algorithm this context will be used with.
+ * @param mode      #FSL_SYM_MODE_CBC, #FSL_SYM_MODE_ECB, etc.
+ *
+ */
+void fsl_shw_scco_init(fsl_shw_scco_t * sym_ctx,
+		       fsl_shw_key_alg_t algorithm, fsl_shw_sym_mode_t mode);
+
+/*!
+ * Set the flags for a Symmetric Cipher Context.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param sym_ctx  The context object to operate on.
+ * @param flags    The flags to reset (one or more values from
+ *                 #fsl_shw_sym_ctx_flags_t ORed together).
+ *
+ */
+void fsl_shw_scco_set_flags(fsl_shw_scco_t * sym_ctx, uint32_t flags);
+
+/*!
+ * Clear some flags in a Symmetric Cipher Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param sym_ctx  The context object to operate on.
+ * @param flags    The flags to reset (one or more values from
+ *                 #fsl_shw_sym_ctx_flags_t ORed together).
+ *
+ */
+void fsl_shw_scco_clear_flags(fsl_shw_scco_t * sym_ctx, uint32_t flags);
+
+/*!
+ * Set the Context (IV) for a Symmetric Cipher Context.
+ *
+ * This is to set the context/IV for #FSL_SYM_MODE_CBC mode, or to set the
+ * context (the S-Box and pointers) for ARC4.  The full context size will
+ * be copied.
+ *
+ * @param sym_ctx  The context object to operate on.
+ * @param context  A pointer to the buffer which contains the context.
+ *
+ */
+void fsl_shw_scco_set_context(fsl_shw_scco_t * sym_ctx, uint8_t * context);
+
+/*!
+ * Get the Context for a Symmetric Cipher Context.
+ *
+ * This is to retrieve the context/IV for #FSL_SYM_MODE_CBC mode, or to
+ * retrieve context (the S-Box and pointers) for ARC4.  The full context
+ * will be copied.
+ *
+ * @param      sym_ctx  The context object to operate on.
+ * @param[out] context  Pointer to location where context will be stored.
+ */
+void fsl_shw_scco_get_context(const fsl_shw_scco_t * sym_ctx,
+			      uint8_t * context);
+
+/*!
+ * Set the Counter Value for a Symmetric Cipher Context.
+ *
+ * This will set the Counter Value for CTR mode.
+ *
+ * @param sym_ctx  The context object to operate on.
+ * @param counter  The starting counter value.  The number of octets.
+ *                 copied will be the block size for the algorithm.
+ * @param modulus  The modulus for controlling the incrementing of the counter.
+ *
+ */
+void fsl_shw_scco_set_counter_info(fsl_shw_scco_t * sym_ctx,
+				   const uint8_t * counter,
+				   fsl_shw_ctr_mod_t modulus);
+
+/*!
+ * Get the Counter Value for a Symmetric Cipher Context.
+ *
+ * This will retrieve the Counter Value is for CTR mode.
+ *
+ * @param sym_ctx         The context object to query.
+ * @param[out] counter    Pointer to location to store the current counter
+ *                        value.  The number of octets copied will be the
+ *                        block size for the algorithm.
+ * @param[out] modulus    Pointer to location to store the modulus.
+ *
+ */
+void fsl_shw_scco_get_counter_info(const fsl_shw_scco_t * sym_ctx,
+				   uint8_t * counter,
+				   fsl_shw_ctr_mod_t * modulus);
+
+	  /*! @} *//* end sccops */
+
+/*****************************************************************************/
+
+/*! @addtogroup accoops
+    @{ */
+
+/*!
+ * Initialize a Authentication-Cipher Context.
+ *
+ * @param auth_object  Pointer to object to operate on.
+ * @param mode         The mode for this object (only #FSL_ACC_MODE_CCM
+ *                     supported).
+ */
+void fsl_shw_acco_init(fsl_shw_acco_t * auth_object, fsl_shw_acc_mode_t mode);
+
+/*!
+ * Set the flags for a Authentication-Cipher Context.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param auth_object  Pointer to object to operate on.
+ * @param flags        The flags to set (one or more from
+ *                     #fsl_shw_auth_ctx_flags_t ORed together).
+ *
+ */
+void fsl_shw_acco_set_flags(fsl_shw_acco_t * auth_object, uint32_t flags);
+
+/*!
+ * Clear some flags in a Authentication-Cipher Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param auth_object  Pointer to object to operate on.
+ * @param flags        The flags to reset (one or more from
+ *                     #fsl_shw_auth_ctx_flags_t ORed together).
+ *
+ */
+void fsl_shw_acco_clear_flags(fsl_shw_acco_t * auth_object, uint32_t flags);
+
+/*!
+ * Set up the Authentication-Cipher Object for CCM mode.
+ *
+ * This will set the @a auth_object for CCM mode and save the @a ctr,
+ * and @a mac_length.  This function can be called instead of
+ * #fsl_shw_acco_init().
+ *
+ * The parameter @a ctr is Counter Block 0, (counter value 0), which is for the
+ * MAC.
+ *
+ * @param auth_object  Pointer to object to operate on.
+ * @param algorithm    Cipher algorithm.  Only AES is supported.
+ * @param ctr          The initial counter value.
+ * @param mac_length   The number of octets used for the MAC.  Valid values are
+ *                     4, 6, 8, 10, 12, 14, and 16.
+ */
+void fsl_shw_acco_set_ccm(fsl_shw_acco_t * auth_object,
+			  fsl_shw_key_alg_t algorithm,
+			  const uint8_t * ctr, uint8_t mac_length);
+
+/*!
+ * Format the First Block (IV) & Initial Counter Value per NIST CCM.
+ *
+ * This function will also set the IV and CTR values per Appendix A of NIST
+ * Special Publication 800-38C (May 2004).  It will also perform the
+ * #fsl_shw_acco_set_ccm() operation with information derived from this set of
+ * parameters.
+ *
+ * Note this function assumes the algorithm is AES.  It initializes the
+ * @a auth_object by setting the mode to #FSL_ACC_MODE_CCM and setting the
+ * flags to be #FSL_ACCO_NIST_CCM.
+ *
+ * @param auth_object  Pointer to object to operate on.
+ * @param t_length     The number of octets used for the MAC.  Valid values are
+ *                     4, 6, 8, 10, 12, 14, and 16.
+ * @param ad_length    Number of octets of Associated Data (may be zero).
+ * @param q_length     A value for the size of the length of @a q field.  Valid
+ *                     values are 1-8.
+ * @param n            The Nonce (packet number or other changing value). Must
+ *                     be (15 - @a q_length) octets long.
+ * @param q            The value of Q (size of the payload in octets).
+ *
+ */
+void fsl_shw_ccm_nist_format_ctr_and_iv(fsl_shw_acco_t * auth_object,
+					uint8_t t_length,
+					uint32_t ad_length,
+					uint8_t q_length,
+					const uint8_t * n, uint32_t q);
+
+/*!
+ * Update the First Block (IV) & Initial Counter Value per NIST CCM.
+ *
+ * This function will set the IV and CTR values per Appendix A of NIST Special
+ * Publication 800-38C (May 2004).
+ *
+ * Note this function assumes that #fsl_shw_ccm_nist_format_ctr_and_iv() has
+ * previously been called on the @a auth_object.
+ *
+ * @param auth_object  Pointer to object to operate on.
+ * @param n             The Nonce (packet number or other changing value). Must
+ *                      be (15 - @a q_length) octets long.
+ * @param q             The value of Q (size of the payload in octets).
+ *
+ */
+void fsl_shw_ccm_nist_update_ctr_and_iv(fsl_shw_acco_t * auth_object,
+					const uint8_t * n, uint32_t q);
+
+	 /* @} *//* accoops */
+
+/******************************************************************************
+ * Library functions
+ *****************************************************************************/
+
+/*! @addtogroup miscfuns
+    @{ */
+
+/* REQ-S2LRD-PINTFC-API-GEN-003 */
+/*!
+ * Determine the hardware security capabilities of this platform.
+ *
+ * Though a user context object is passed into this function, it will always
+ * act in a non-blocking manner.
+ *
+ * @param  user_ctx   The user context which will be used for the query.
+ *
+ * @return  A pointer to the capabilities object.
+ */
+extern fsl_shw_pco_t *fsl_shw_get_capabilities(fsl_shw_uco_t * user_ctx);
+
+/* REQ-S2LRD-PINTFC-API-GEN-004 */
+/*!
+ * Create an association between the user and the provider of the API.
+ *
+ * @param  user_ctx   The user context which will be used for this association.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_register_user(fsl_shw_uco_t * user_ctx);
+
+/* REQ-S2LRD-PINTFC-API-GEN-005 */
+/*!
+ * Destroy the association between the user and the provider of the API.
+ *
+ * @param  user_ctx   The user context which is no longer needed.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_deregister_user(fsl_shw_uco_t * user_ctx);
+
+/* REQ-S2LRD-PINTFC-API-GEN-006 */
+/*!
+ * Retrieve results from earlier operations.
+ *
+ * @param         user_ctx     The user's context.
+ * @param         result_size  The number of array elements of @a results.
+ * @param[in,out] results      Pointer to first of the (array of) locations to
+ *                             store results.
+ * @param[out]    result_count Pointer to store the number of results which
+ *                             were returned.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_get_results(fsl_shw_uco_t * user_ctx,
+					    uint16_t result_size,
+					    fsl_shw_result_t results[],
+					    uint16_t * result_count);
+
+/*!
+ * Allocate a block of secure memory
+ *
+ * @param       user_ctx        User context
+ * @param       size            Memory size (octets).  Note: currently only
+ *                              supports only single-partition sized blocks.
+ * @param       UMID            User Mode ID to use when registering the
+ *                              partition.
+ * @param       permissions     Permissions to initialize the partition with.
+ *                              Can be made by ORing flags from the
+ *                              #fsl_shw_permission_t.
+ *
+ * @return                      Address of the allocated memory.  NULL if the
+ *                              call was not successful.
+ */
+extern void *fsl_shw_smalloc(fsl_shw_uco_t * user_ctx,
+			     uint32_t size,
+			     const uint8_t * UMID, uint32_t permissions);
+
+/*!
+ * Free a block of secure memory that was allocated with #fsl_shw_smalloc
+ *
+ * @param       user_ctx        User context
+ * @param       address         Address of the block of secure memory to be
+ *                              released.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_sfree(fsl_shw_uco_t * user_ctx, void *address);
+
+/*!
+ * Diminish the permissions of a block of secure memory.  Note that permissions
+ * can only be revoked.
+ *
+ * @param       user_ctx        User context
+ * @param       address         Base address of the secure memory to work with
+ * @param       permissions     Permissions to initialize the partition with.
+ *                              Can be made by ORing flags from the
+ *                              #fsl_shw_permission_t.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_diminish_perms(fsl_shw_uco_t * user_ctx,
+					       void *address,
+					       uint32_t permissions);
+
+/*!
+ * @brief   Encrypt a region of secure memory using the hardware secret key
+ *
+ * @param       user_ctx        User context
+ * @param       partition_base  Base address of the partition
+ * @param       offset_bytes    Offset of data from the partition base
+ * @param       byte_count      Length of the data to encrypt
+ * @param       black_data      Location to store the encrypted data
+ * @param       IV              IV to use for the encryption routine
+ * @param       cypher_mode     Cyphering mode to use, specified by type
+ *                              #fsl_shw_cypher_mode_t
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t
+do_scc_encrypt_region(fsl_shw_uco_t * user_ctx,
+		      void *partition_base, uint32_t offset_bytes,
+		      uint32_t byte_count, uint8_t * black_data,
+		      uint32_t * IV, fsl_shw_cypher_mode_t cypher_mode);
+
+/*!
+ * @brief   Decrypt a region of secure memory using the hardware secret key
+ *
+ * @param       user_ctx        User context
+ * @param       partition_base  Base address of the partition
+ * @param       offset_bytes    Offset of data from the partition base
+ * @param       byte_count      Length of the data to encrypt
+ * @param       black_data      Location to store the encrypted data
+ * @param       IV              IV to use for the encryption routine
+ * @param       cypher_mode     Cyphering mode to use, specified by type
+ *                              #fsl_shw_cypher_mode_t
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t
+do_scc_decrypt_region(fsl_shw_uco_t * user_ctx,
+		      void *partition_base, uint32_t offset_bytes,
+		      uint32_t byte_count, const uint8_t * black_data,
+		      uint32_t * IV, fsl_shw_cypher_mode_t cypher_mode);
+
+	  /*! @} *//* miscfuns */
+
+/*! @addtogroup opfuns
+    @{ */
+
+/* REQ-S2LRD-PINTFC-API-BASIC-SYM-002 */
+/* PINTFC-API-BASIC-SYM-ARC4-001 */
+/* PINTFC-API-BASIC-SYM-ARC4-002 */
+/*!
+ * Encrypt a stream of data with a symmetric-key algorithm.
+ *
+ * In ARC4, and also in #FSL_SYM_MODE_CBC and #FSL_SYM_MODE_CTR modes, the
+ * flags of the @a sym_ctx object will control part of the operation of this
+ * function.  The #FSL_SYM_CTX_INIT flag means that there is no context info in
+ * the object.  The #FSL_SYM_CTX_LOAD means to use information in the
+ * @a sym_ctx at the start of the operation, and the #FSL_SYM_CTX_SAVE flag
+ * means to update the object's context information after the operation has
+ * been performed.
+ *
+ * All of the data for an operation can be run through at once using the
+ * #FSL_SYM_CTX_INIT or #FSL_SYM_CTX_LOAD flags, as appropriate, and then using
+ * a @a length for the whole of the data.
+ *
+ * If a #FSL_SYM_CTX_SAVE flag were added, an additional call to the function
+ * would "pick up" where the previous call left off, allowing the user to
+ * perform the larger function in smaller steps.
+ *
+ * In #FSL_SYM_MODE_CBC and #FSL_SYM_MODE_ECB modes, the @a length must always
+ * be a multiple of the block size for the algorithm being used.  For proper
+ * operation in #FSL_SYM_MODE_CTR mode, the @a length must be a multiple of the
+ * block size until the last operation on the total octet stream.
+ *
+ * Some users of ARC4 may want to compute the context (S-Box and pointers) from
+ * the key before any data is available.  This may be done by running this
+ * function with a @a length of zero, with the init & save flags flags on in
+ * the @a sym_ctx.  Subsequent operations would then run as normal with the
+ * load and save flags.  Note that they key object is still required.
+ *
+ * @param         user_ctx  A user context from #fsl_shw_register_user().
+ * @param         key_info  Key and algorithm  being used for this operation.
+ * @param[in,out] sym_ctx   Info on cipher mode, state of the cipher.
+ * @param         length   Length, in octets, of the pt (and ct).
+ * @param         pt       pointer to plaintext to be encrypted.
+ * @param[out]    ct       pointer to where to store the resulting ciphertext.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ *
+ */
+extern fsl_shw_return_t fsl_shw_symmetric_encrypt(fsl_shw_uco_t * user_ctx,
+						  fsl_shw_sko_t * key_info,
+						  fsl_shw_scco_t * sym_ctx,
+						  uint32_t length,
+						  const uint8_t * pt,
+						  uint8_t * ct);
+
+/* PINTFC-API-BASIC-SYM-002 */
+/* PINTFC-API-BASIC-SYM-ARC4-001 */
+/* PINTFC-API-BASIC-SYM-ARC4-002 */
+/*!
+ * Decrypt a stream of data with a symmetric-key algorithm.
+ *
+ * In ARC4, and also in #FSL_SYM_MODE_CBC and #FSL_SYM_MODE_CTR modes, the
+ * flags of the @a sym_ctx object will control part of the operation of this
+ * function.  The #FSL_SYM_CTX_INIT flag means that there is no context info in
+ * the object.  The #FSL_SYM_CTX_LOAD means to use information in the
+ * @a sym_ctx at the start of the operation, and the #FSL_SYM_CTX_SAVE flag
+ * means to update the object's context information after the operation has
+ * been performed.
+ *
+ * All of the data for an operation can be run through at once using the
+ * #FSL_SYM_CTX_INIT or #FSL_SYM_CTX_LOAD flags, as appropriate, and then using
+ * a @a length for the whole of the data.
+ *
+ * If a #FSL_SYM_CTX_SAVE flag were added, an additional call to the function
+ * would "pick up" where the previous call left off, allowing the user to
+ * perform the larger function in smaller steps.
+ *
+ * In #FSL_SYM_MODE_CBC and #FSL_SYM_MODE_ECB modes, the @a length must always
+ * be a multiple of the block size for the algorithm being used.  For proper
+ * operation in #FSL_SYM_MODE_CTR mode, the @a length must be a multiple of the
+ * block size until the last operation on the total octet stream.
+ *
+ * Some users of ARC4 may want to compute the context (S-Box and pointers) from
+ * the key before any data is available.  This may be done by running this
+ * function with a @a length of zero, with the #FSL_SYM_CTX_INIT &
+ * #FSL_SYM_CTX_SAVE flags on in the @a sym_ctx.  Subsequent operations would
+ * then run as normal with the load & save flags.  Note that they key object is
+ * still required.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param      key_info The key and algorithm being used in this operation.
+ * @param[in,out] sym_ctx Info on cipher mode, state of the cipher.
+ * @param      length   Length, in octets, of the ct (and pt).
+ * @param      ct       pointer to ciphertext to be decrypted.
+ * @param[out] pt       pointer to where to store the resulting plaintext.
+ *
+ * @return    A return code of type #fsl_shw_return_t
+ *
+ */
+extern fsl_shw_return_t fsl_shw_symmetric_decrypt(fsl_shw_uco_t * user_ctx,
+						  fsl_shw_sko_t * key_info,
+						  fsl_shw_scco_t * sym_ctx,
+						  uint32_t length,
+						  const uint8_t * ct,
+						  uint8_t * pt);
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HASH-005 */
+/*!
+ * Hash a stream of data with a cryptographic hash algorithm.
+ *
+ * The flags in the @a hash_ctx control the operation of this function.
+ *
+ * Hashing functions work on 64 octets of message at a time.  Therefore, when
+ * any partial hashing of a long message is performed, the message @a length of
+ * each segment must be a multiple of 64.  When ready to
+ * #FSL_HASH_FLAGS_FINALIZE the hash, the @a length may be any value.
+ *
+ * With the #FSL_HASH_FLAGS_INIT and #FSL_HASH_FLAGS_FINALIZE flags on, a
+ * one-shot complete hash, including padding, will be performed.  The @a length
+ * may be any value.
+ *
+ * The first octets of a data stream can be hashed by setting the
+ * #FSL_HASH_FLAGS_INIT and #FSL_HASH_FLAGS_SAVE flags.  The @a length must be
+ * a multiple of 64.
+ *
+ * The flag #FSL_HASH_FLAGS_LOAD is used to load a context previously saved by
+ * #FSL_HASH_FLAGS_SAVE.  The two in combination will allow a (multiple-of-64
+ * octets) 'middle sequence' of the data stream to be hashed with the
+ * beginning.  The @a length must again be a multiple of 64.
+ *
+ * Since the flag #FSL_HASH_FLAGS_LOAD is used to load a context previously
+ * saved by #FSL_HASH_FLAGS_SAVE, the #FSL_HASH_FLAGS_LOAD and
+ * #FSL_HASH_FLAGS_FINALIZE flags, used together, can be used to finish the
+ * stream.  The @a length may be any value.
+ *
+ * If the user program wants to do the padding for the hash, it can leave off
+ * the #FSL_HASH_FLAGS_FINALIZE flag.  The @a length must then be a multiple of
+ * 64 octets.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param[in,out] hash_ctx Hashing algorithm and state of the cipher.
+ * @param      msg       Pointer to the data to be hashed.
+ * @param      length    Length, in octets, of the @a msg.
+ * @param[out] result    If not null, pointer to where to store the hash
+ *                       digest.
+ * @param      result_len Number of octets to store in @a result.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_hash(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_hco_t * hash_ctx,
+				     const uint8_t * msg,
+				     uint32_t length,
+				     uint8_t * result, uint32_t result_len);
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HMAC-001 */
+/*!
+ * Precompute the Key hashes for an HMAC operation.
+ *
+ * This function may be used to calculate the inner and outer precomputes,
+ * which are the hash contexts resulting from hashing the XORed key for the
+ * 'inner hash' and the 'outer hash', respectively, of the HMAC function.
+ *
+ * After execution of this function, the @a hmac_ctx will contain the
+ * precomputed inner and outer contexts, so that they may be used by
+ * #fsl_shw_hmac().  The flags of @a hmac_ctx will be updated with
+ * #FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT to mark their presence.  In addition, the
+ * #FSL_HMAC_FLAGS_INIT flag will be set.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param      key_info  The key being used in this operation.  Key must be
+ *                       1 to 64 octets long.
+ * @param[in,out] hmac_ctx The context which controls, by its flags and
+ *                         algorithm, the operation of this function.
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_hmac_precompute(fsl_shw_uco_t * user_ctx,
+						fsl_shw_sko_t * key_info,
+						fsl_shw_hmco_t * hmac_ctx);
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HMAC-002 */
+/*!
+ * Continue, finalize, or one-shot an HMAC operation.
+ *
+ * There are a number of ways to use this function.  The flags in the
+ * @a hmac_ctx object will determine what operations occur.
+ *
+ * If #FSL_HMAC_FLAGS_INIT is set, then the hash will be started either from
+ * the @a key_info, or from the precomputed inner hash value in the
+ * @a hmac_ctx, depending on the value of #FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT.
+ *
+ * If, instead, #FSL_HMAC_FLAGS_LOAD is set, then the hash will be continued
+ * from the ongoing inner hash computation in the @a hmac_ctx.
+ *
+ * If #FSL_HMAC_FLAGS_FINALIZE are set, then the @a msg will be padded, hashed,
+ * the outer hash will be performed, and the @a result will be generated.
+ *
+ * If the #FSL_HMAC_FLAGS_SAVE flag is set, then the (ongoing or final) digest
+ * value will be stored in the ongoing inner hash computation field of the @a
+ * hmac_ctx.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param key_info       If #FSL_HMAC_FLAGS_INIT is set in the @a hmac_ctx,
+ *                       this is the key being used in this operation, and the
+ *                       IPAD.  If #FSL_HMAC_FLAGS_INIT is set in the @a
+ *                       hmac_ctx and @a key_info is NULL, then
+ *                       #fsl_shw_hmac_precompute() has been used to populate
+ *                       the @a inner_precompute and @a outer_precompute
+ *                       contexts.  If #FSL_HMAC_FLAGS_INIT is not set, this
+ *                       parameter is ignored.
+
+ * @param[in,out] hmac_ctx The context which controls, by its flags and
+ *                       algorithm, the operation of this function.
+ * @param      msg               Pointer to the message to be hashed.
+ * @param      length            Length, in octets, of the @a msg.
+ * @param[out] result            Pointer, of @a result_len octets, to where to
+ *                               store the HMAC.
+ * @param      result_len        Length of @a result buffer.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_hmac(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info,
+				     fsl_shw_hmco_t * hmac_ctx,
+				     const uint8_t * msg,
+				     uint32_t length,
+				     uint8_t * result, uint32_t result_len);
+
+/* REQ-S2LRD-PINTFC-API-BASIC-RNG-002 */
+/*!
+ * Get random data.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param      length    The number of octets of @a data being requested.
+ * @param[out] data      A pointer to a location of @a length octets to where
+ *                       random data will be returned.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_get_random(fsl_shw_uco_t * user_ctx,
+					   uint32_t length, uint8_t * data);
+
+/* REQ-S2LRD-PINTFC-API-BASIC-RNG-002 */
+/*!
+ * Add entropy to random number generator.
+ *
+ * @param      user_ctx  A user context from #fsl_shw_register_user().
+ * @param      length    Number of bytes at @a data.
+ * @param      data      Entropy to add to random number generator.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_add_entropy(fsl_shw_uco_t * user_ctx,
+					    uint32_t length, uint8_t * data);
+
+/*!
+ * Perform Generation-Encryption by doing a Cipher and a Hash.
+ *
+ * Generate the authentication value @a auth_value as well as encrypt the @a
+ * payload into @a ct (the ciphertext).  This is a one-shot function, so all of
+ * the @a auth_data and the total message @a payload must passed in one call.
+ * This also means that the flags in the @a auth_ctx must be #FSL_ACCO_CTX_INIT
+ * and #FSL_ACCO_CTX_FINALIZE.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      auth_ctx         Controlling object for Authenticate-decrypt.
+ * @param      cipher_key_info  The key being used for the cipher part of this
+ *                              operation.  In CCM mode, this key is used for
+ *                              both parts.
+ * @param      auth_key_info    The key being used for the authentication part
+ *                              of this operation.  In CCM mode, this key is
+ *                              ignored and may be NULL.
+ * @param      auth_data_length Length, in octets, of @a auth_data.
+ * @param      auth_data        Data to be authenticated but not encrypted.
+ * @param      payload_length   Length, in octets, of @a payload.
+ * @param      payload          Pointer to the plaintext to be encrypted.
+ * @param[out] ct               Pointer to the where the encrypted @a payload
+ *                              will be stored.  Must be @a payload_length
+ *                              octets long.
+ * @param[out] auth_value       Pointer to where the generated authentication
+ *                              field will be stored. Must be as many octets as
+ *                              indicated by MAC length in the @a function_ctx.
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_gen_encrypt(fsl_shw_uco_t * user_ctx,
+					    fsl_shw_acco_t * auth_ctx,
+					    fsl_shw_sko_t * cipher_key_info,
+					    fsl_shw_sko_t * auth_key_info,
+					    uint32_t auth_data_length,
+					    const uint8_t * auth_data,
+					    uint32_t payload_length,
+					    const uint8_t * payload,
+					    uint8_t * ct, uint8_t * auth_value);
+
+/*!
+ * Perform Authentication-Decryption in Cipher + Hash.
+ *
+ * This function will perform a one-shot decryption of a data stream as well as
+ * authenticate the authentication value.  This is a one-shot function, so all
+ * of the @a auth_data and the total message @a payload must passed in one
+ * call.  This also means that the flags in the @a auth_ctx must be
+ * #FSL_ACCO_CTX_INIT and #FSL_ACCO_CTX_FINALIZE.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      auth_ctx         Controlling object for Authenticate-decrypt.
+ * @param      cipher_key_info  The key being used for the cipher part of this
+ *                              operation.  In CCM mode, this key is used for
+ *                              both parts.
+ * @param      auth_key_info    The key being used for the authentication part
+ *                              of this operation.  In CCM mode, this key is
+ *                              ignored and may be NULL.
+ * @param      auth_data_length Length, in octets, of @a auth_data.
+ * @param      auth_data        Data to be authenticated but not decrypted.
+ * @param      payload_length   Length, in octets, of @a ct and @a pt.
+ * @param      ct               Pointer to the encrypted input stream.
+ * @param      auth_value       The (encrypted) authentication value which will
+ *                              be authenticated.  This is the same data as the
+ *                              (output) @a auth_value argument to
+ *                              #fsl_shw_gen_encrypt().
+ * @param[out] payload          Pointer to where the plaintext resulting from
+ *                              the decryption will be stored.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_auth_decrypt(fsl_shw_uco_t * user_ctx,
+					     fsl_shw_acco_t * auth_ctx,
+					     fsl_shw_sko_t * cipher_key_info,
+					     fsl_shw_sko_t * auth_key_info,
+					     uint32_t auth_data_length,
+					     const uint8_t * auth_data,
+					     uint32_t payload_length,
+					     const uint8_t * ct,
+					     const uint8_t * auth_value,
+					     uint8_t * payload);
+
+/*!
+ * Establish the key in a protected location, which can be the system keystore,
+ * user keystore, or (on platforms that support it) as a Platform Key.
+ *
+ * By default, keys initialized with #fsl_shw_sko_init() will be placed into
+ * the system keystore.  The user can cause the key to be established in a
+ * user keystore by first calling #fsl_shw_sko_set_keystore() on the key.
+ * Normally, keys in the system keystore can only be used for hardware
+ * encrypt or decrypt operations, however if the #FSL_SKO_KEY_SW_KEY flag is
+ * applied using #fsl_shw_sko_set_flags(), the key will be established as a
+ * software key, which can then be read out using #fsl_shw_read_key().
+ *
+ * Keys initialized with #fsl_shw_sko_init_pf_key() are established as a
+ * Platform Key.  Their use is covered in @ref di_sec.
+ *
+ * This function only needs to be used when unwrapping a key, setting up a key
+ * which could be wrapped with a later call to #fsl_shw_extract_key(), or
+ * setting up a key as a Platform Key.  Normal cleartext keys can simply be
+ * placed into #fsl_shw_sko_t key objects with #fsl_shw_sko_set_key() and used
+ * directly.
+ *
+ * The maximum key size supported for wrapped/unwrapped keys is 32 octets.
+ * (This is the maximum reasonable key length on Sahara - 32 octets for an HMAC
+ * key based on SHA-256.)  The key size is determined by the @a key_info.  The
+ * expected length of @a key can be determined by
+ * #fsl_shw_sko_calculate_wrapped_size()
+ *
+ * The protected key will not be available for use until this operation
+ * successfully completes.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param[in,out] key_info      The information about the key to be which will
+ *                              be established.  In the create case, the key
+ *                              length must be set.
+ * @param      establish_type   How @a key will be interpreted to establish a
+ *                              key for use.
+ * @param key                   If @a establish_type is #FSL_KEY_WRAP_UNWRAP,
+ *                              this is the location of a wrapped key.  If
+ *                              @a establish_type is #FSL_KEY_WRAP_CREATE, this
+ *                              parameter can be @a NULL.  If @a establish_type
+ *                              is #FSL_KEY_WRAP_ACCEPT, this is the location
+ *                              of a plaintext key.
+ */
+extern fsl_shw_return_t fsl_shw_establish_key(fsl_shw_uco_t * user_ctx,
+					      fsl_shw_sko_t * key_info,
+					      fsl_shw_key_wrap_t establish_type,
+					      const uint8_t * key);
+
+/*!
+ * Read the key value from a key object.
+ *
+ * Only a key marked as a software key (#FSL_SKO_KEY_SW_KEY) can be read with
+ * this call.  It has no effect on the status of the key store.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      key_info         The referenced key.
+ * @param[out] key              The location to store the key value.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_read_key(fsl_shw_uco_t * user_ctx,
+					 fsl_shw_sko_t * key_info,
+					 uint8_t * key);
+
+/*!
+ * Wrap a key and retrieve the wrapped value.
+ *
+ * A wrapped key is a key that has been cryptographically obscured.  It is
+ * only able to be used with keys that have been established by 
+ * #fsl_shw_establish_key().
+ *
+ * For keys established in the system or user keystore, this function will
+ * also release the key (see #fsl_shw_release_key()) so that it must be re-
+ * established before reuse.  This function will not release keys that are
+ * established as a Platform Key, so a call to #fsl_shw_release_key() is
+ * necessary to release those keys.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      key_info         The information about the key to be deleted.
+ * @param[out] covered_key      The location to store the wrapped key.
+ *                              (This size is based upon the maximum key size
+ *                              of 32 octets).
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_extract_key(fsl_shw_uco_t * user_ctx,
+					    fsl_shw_sko_t * key_info,
+					    uint8_t * covered_key);
+
+/*!
+ * De-establish a key so that it can no longer be accessed.
+ *
+ * The key will need to be re-established before it can again be used.
+ *
+ * This feature is not available for all platforms, nor for all algorithms and
+ * modes.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ * @param      key_info         The information about the key to be deleted.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_release_key(fsl_shw_uco_t * user_ctx,
+					    fsl_shw_sko_t * key_info);
+
+/*!
+ * Cause the hardware to create a new random key for use by the secure memory
+ * encryption hardware.
+ *
+ * Have the hardware use the secure hardware random number generator to load a
+ * new secret key into the system's Random Key register.
+ *
+ * @param      user_ctx         A user context from #fsl_shw_register_user().
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_gen_random_pf_key(fsl_shw_uco_t * user_ctx);
+
+/*!
+ * Retrieve the detected tamper event.
+ *
+ * Note that if more than one event was detected, this routine will only ever
+ * return one of them.
+ *
+ * @param[in]  user_ctx         A user context from #fsl_shw_register_user().
+ * @param[out] tamperp          Location to store the tamper information.
+ * @param[out] timestampp       Locate to store timestamp from hardwhare when
+ *                              an event was detected.
+ *
+ *
+ * @return    A return code of type #fsl_shw_return_t (for instance, if the platform
+ *            is not in a fail state.
+ */
+extern fsl_shw_return_t fsl_shw_read_tamper_event(fsl_shw_uco_t * user_ctx,
+						  fsl_shw_tamper_t * tamperp,
+						  uint64_t * timestampp);
+
+/*! @} *//* opfuns */
+
+/* Insert example code into the API documentation. */
+
+/*!
+ * @example apitest.c
+ */
+
+/*!
+ * @example sym.c
+ */
+
+/*!
+ * @example rand.c
+ */
+
+/*!
+ * @example hash.c
+ */
+
+/*!
+ * @example hmac1.c
+ */
+
+/*!
+ * @example hmac2.c
+ */
+
+/*!
+ * @example gen_encrypt.c
+ */
+
+/*!
+ * @example auth_decrypt.c
+ */
+
+/*!
+ * @example wrapped_key.c
+ */
+
+/*!
+ * @example smalloc.c
+ */
+
+/*!
+ * @example user_keystore.c
+ */
+
+/*!
+ * @example dryice.c
+ */
+
+#endif				/* API_DOC */
+
+#endif				/* FSL_SHW_H */
diff --git a/drivers/mxc/security/sahara2/include/fsl_shw_keystore.h b/drivers/mxc/security/sahara2/include/fsl_shw_keystore.h
new file mode 100644
index 000000000000..837d01006086
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/fsl_shw_keystore.h
@@ -0,0 +1,475 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+
+#ifndef FSL_SHW_KEYSTORE_H
+#define FSL_SHW_KEYSTORE_H
+
+/*!
+ * @file fsl_shw_keystore.h
+ *
+ * @brief Definition of the User Keystore API.
+ *
+ */
+
+/*! \page user_keystore User Keystore API
+ *
+ * Definition of the User Keystore API.
+ *
+ * On platforms with multiple partitions of Secure Memory, the Keystore Object 
+ * (#fsl_shw_kso_t) is provided to allow users to manage a private keystore for
+ * use in software cryptographic routines.  The user can define a custom set of
+ * methods for managing their keystore, or use a default keystore handler.  The
+ * keystore is established by #fsl_shw_establish_keystore(), and released by
+ * #fsl_shw_release_keystore().  The intent of this design is to make the
+ * keystore implementation as flexible as possible.
+ *
+ * See @ref keystore_api for the generic keystore API, and @ref
+ * default_keystore for the default keystore implementation.
+ *
+ */
+
+/*!
+ * @defgroup keystore_api User Keystore API
+ *
+ * Keystore API
+ *
+ * These functions define the generic keystore API, which can be used in 
+ * conjunction with a keystore implementation backend to support a user
+ * keystore.
+ */
+
+/*!
+ * @defgroup default_keystore Default Keystore Implementation
+ *
+ * Default Keystore Implementation
+ *
+ * These functions define the default keystore implementation, which is used
+ * for the system keystore and for user keystores initialized by 
+ * #fsl_shw_init_keystore_default().  They can be used as-is or as a reference
+ * for creating a custom keystore handler.  It uses an entire Secure Memory
+ * partition, divided in to equal slots of length #KEYSTORE_SLOT_SIZE.  These
+ * functions are not intended to be used directly-  all user interaction with
+ * the keystore should be through the @ref keystore_api and the Wrapped Key
+ * interface.
+ *
+ * The current implementation is designed to work with both SCC and SCC2.
+ * Differences between the two versions are noted below.
+ */
+
+/*! @addtogroup keystore_api
+    @{ */
+
+#ifndef KEYSTORE_SLOT_SIZE
+/*! Size of each key slot, in octets.  This sets an upper bound on the size
+ * of a key that can placed in the keystore.
+ */
+#define KEYSTORE_SLOT_SIZE 32
+#endif
+
+/*!
+ * Initialize a Keystore Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object. It allows the user to associate a custom keystore interface by
+ * specifying the correct set of functions that will be used to perform actions
+ * on the keystore object.  To use the default keystore handler, the function
+ * #fsl_shw_init_keystore_default() can be used instead.
+ *
+ * @param keystore      The Keystore object to operate on.
+ * @param data_init     Keystore initialization function.  This function is
+ *                      responsible for initializing the keystore.  A
+ *                      user-defined object can be assigned to the user_data
+ *                      pointer, and will be passed to any function acting on
+ *                      that keystore.  It is called during
+ *                      #fsl_shw_establish_keystore().
+ * @param data_cleanup  Keystore cleanup function.  This function cleans up
+ *                      any data structures associated with the keyboard.  It
+ *                      is called by #fsl_shw_release_keystore().
+ * @param slot_alloc    Slot allocation function.  This function allocates a
+ *                      key slot, potentially based on size and owner id.  It
+ *                      is called by #fsl_shw_establish_key().
+ * @param slot_dealloc  Slot deallocation function.
+ * @param slot_verify_access Function to verify that a given Owner ID
+ *                           credential matches the given slot.
+ * @param slot_get_address   For SCC2: Get the virtual address (kernel or
+ *                           userspace) of the data stored in the slot.
+ *                           For SCC: Get the physical address of the data
+ *                           stored in the slot.
+ * @param slot_get_base      For SCC2: Get the (virtual) base address of the
+ *                           partition that the slot is located on.
+ *                           For SCC: Not implemented.
+ * @param slot_get_offset    For SCC2: Get the offset from the start of the
+ *                           partition that the slot data is located at (in
+ *                           octets)
+ *                           For SCC: Not implemented.
+ * @param slot_get_slot_size Get the size of the key slot, in octets.
+ */
+extern void fsl_shw_init_keystore(fsl_shw_kso_t * keystore,
+				  fsl_shw_return_t(*data_init) (fsl_shw_uco_t *
+								user_ctx,
+								void
+								**user_data),
+				  void (*data_cleanup) (fsl_shw_uco_t *
+							user_ctx,
+							void **user_data),
+				  fsl_shw_return_t(*slot_alloc) (void
+								 *user_data,
+								 uint32_t size,
+								 uint64_t
+								 owner_id,
+								 uint32_t *
+								 slot),
+				  fsl_shw_return_t(*slot_dealloc) (void
+								   *user_data,
+								   uint64_t
+								   owner_id,
+								   uint32_t
+								   slot),
+				  fsl_shw_return_t(*slot_verify_access) (void
+									 *user_data,
+									 uint64_t
+									 owner_id,
+									 uint32_t
+									 slot),
+				  void *(*slot_get_address) (void *user_data,
+							     uint32_t handle),
+				  uint32_t(*slot_get_base) (void *user_data,
+							    uint32_t handle),
+				  uint32_t(*slot_get_offset) (void *user_data,
+							      uint32_t handle),
+				  uint32_t(*slot_get_slot_size) (void
+								 *user_data,
+								 uint32_t
+								 handle));
+
+/*!
+ * Initialize a Keystore Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object. It sets the user keystore object up to use the default keystore
+ * handler.  If a custom keystore handler is desired, the function
+ * #fsl_shw_init_keystore() can be used instead.
+ *
+ * @param keystore      The Keystore object to operate on.
+ */
+extern void fsl_shw_init_keystore_default(fsl_shw_kso_t * keystore);
+
+/*!
+ * Establish a Keystore Object.
+ *
+ * This function establishes a keystore object that has been set up by a call
+ * to #fsl_shw_init_keystore().  It is a wrapper for the user-defined
+ * data_init() function, which is specified during keystore initialization.
+ *
+ * @param user_ctx      The user context that this keystore should be attached
+ *                      to
+ * @param keystore      The Keystore object to operate on.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t fsl_shw_establish_keystore(fsl_shw_uco_t * user_ctx,
+						   fsl_shw_kso_t * keystore);
+
+/*!
+ * Release a Keystore Object.
+ *
+ * This function releases an established keystore object.  It is a wrapper for
+ * the user-defined data_cleanup() function, which is specified during keystore
+ * initialization.
+ *
+ * @param user_ctx      The user context that this keystore should be attached
+ *                      to.
+ * @param keystore      The Keystore object to operate on.
+ */
+extern void fsl_shw_release_keystore(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_kso_t * keystore);
+
+/*!
+ * Allocate a slot in the Keystore.
+ *
+ * This function attempts to allocate a slot to hold a key in the keystore.  It
+ * is called by #fsl_shw_establish_key() when establishing a Secure Key Object,
+ * if the key has been flagged to be stored in a user keystore by the
+ * #fsl_shw_sko_set_keystore() function.  It is a wrapper for the
+ * implementation-specific function slot_alloc().
+ *
+ * @param keystore      The Keystore object to operate on.
+ * @param[in] size      Size of the key to be stored (octets).
+ * @param[in] owner_id  ID of the key owner.
+ * @param[out] slot     If successful, assigned slot ID
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t keystore_slot_alloc(fsl_shw_kso_t * keystore,
+					    uint32_t size,
+					    uint64_t owner_id, uint32_t * slot);
+
+/*!
+ * Deallocate a slot in the Keystore.
+ *
+ * This function attempts to allocate a slot to hold a key in the keystore.
+ * It is called by #fsl_shw_extract_key() and #fsl_shw_release_key() when the
+ * key that it contains is to be released.  It is a wrapper for the
+ * implmentation-specific function slot_dealloc().
+ 
+ * @param keystore      The Keystore object to operate on.
+ * @param[in] owner_id  ID of the key owner.
+ * @param[in] slot     If successful, assigned slot ID.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t keystore_slot_dealloc(fsl_shw_kso_t * keystore,
+					      uint64_t owner_id, uint32_t slot);
+
+/*!
+ * Load cleartext key data into a key slot
+ *
+ * This function loads a key slot with cleartext data.
+ *
+ * @param keystore      The Keystore object to operate on.
+ * @param[in] owner_id  ID of the key owner.
+ * @param[in] slot      If successful, assigned slot ID.
+ * @param[in] key_data  Pointer to the location of the cleartext key data.
+ * @param[in] key_length Length of the key data (octets).
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t
+keystore_slot_load(fsl_shw_kso_t * keystore, uint64_t owner_id, uint32_t slot,
+		   const uint8_t * key_data, uint32_t key_length);
+
+/*!
+ * Read cleartext key data from a key slot
+ *
+ * This function returns the key in a key slot.
+ *
+ * @param keystore      The Keystore object to operate on.
+ * @param[in]  owner_id   ID of the key owner.
+ * @param[in]  slot       ID of slot where key resides.
+ * @param[in]  key_length Length of the key data (octets).
+ * @param[out] key_data   Pointer to the location of the cleartext key data.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t
+keystore_slot_read(fsl_shw_kso_t * keystore, uint64_t owner_id, uint32_t slot,
+		   uint32_t key_length, uint8_t * key_data);
+
+/*!
+ * Encrypt a keyslot
+ *
+ * This function encrypts a key using the hardware secret key.
+ *
+ * @param user_ctx      User context
+ * @param keystore      The Keystore object to operate on.
+ * @param[in] owner_id  ID of the key owner.
+ * @param[in] slot      Slot ID of the key to encrypt.
+ * @param[in] length    Length of the key
+ * @param[out] destination  Pointer to the location where the encrypted data
+ *                          is to be stored.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t
+keystore_slot_encrypt(fsl_shw_uco_t * user_ctx,
+		      fsl_shw_kso_t * keystore, uint64_t owner_id,
+		      uint32_t slot, uint32_t length, uint8_t * destination);
+
+/*!
+ * Decrypt a keyslot
+ *
+ * This function decrypts a key using the hardware secret key.
+ *
+ * @param user_ctx      User context
+ * @param keystore      The Keystore object to operate on.
+ * @param[in] owner_id  ID of the key owner.
+ * @param[in] slot      Slot ID of the key to encrypt.
+ * @param[in] length    Length of the key
+ * @param[in] source    Pointer to the location where the encrypted data
+ *                      is stored.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+extern fsl_shw_return_t
+keystore_slot_decrypt(fsl_shw_uco_t * user_ctx,
+		      fsl_shw_kso_t * keystore, uint64_t owner_id,
+		      uint32_t slot, uint32_t length, const uint8_t * source);
+
+/* @} */
+
+/*! @addtogroup default_keystore
+    @{ */
+
+/*!
+ * Data structure to hold per-slot information
+ */
+typedef struct keystore_data_slot_info_t {
+	uint8_t allocated;	/*!< Track slot assignments */
+	uint64_t owner;		/*!< Owner IDs */
+	uint32_t key_length;	/*!< Size of the key */
+} keystore_data_slot_info_t;
+
+/*!
+ * Data structure to hold keystore information.
+ */
+typedef struct keystore_data_t {
+	void *base_address;	/*!< Base of the Secure Partition */
+	uint32_t slot_count;	/*!< Number of slots in the keystore */
+	struct keystore_data_slot_info_t *slot;	/*!< Per-slot information */
+} keystore_data_t;
+
+/*!
+ * Default keystore initialization routine.
+ *
+ * This function acquires a Secure Partition Object to store the keystore,
+ * divides it into slots of length #KEYSTORE_SLOT_SIZE, and builds a data
+ * structure to hold key information.
+ *
+ * @param user_ctx       User context
+ * @param[out] user_data Pointer to the location where the keystore data
+ *                       structure is to be stored.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t shw_kso_init_data(fsl_shw_uco_t * user_ctx, void **user_data);
+
+/*!
+ * Default keystore cleanup routine.
+ *
+ * This function releases the Secure Partition Object and the memory holding
+ * the keystore data structure, that obtained by the shw_kso_init_data
+ * function. 
+ *
+ * @param user_ctx          User context
+ * @param[in,out] user_data Pointer to the location where the keystore data
+ *                          structure is stored.
+ */
+void shw_kso_cleanup_data(fsl_shw_uco_t * user_ctx, void **user_data);
+
+/*!
+ * Default keystore slot access verification
+ *
+ * This function compares the supplied Owner ID to the registered owner of
+ * the key slot, to see if the supplied ID is correct.
+ *
+ * @param[in] user_data Pointer to the location where the keystore data
+ *                      structure stored.
+ * @param[in] owner_id  Owner ID supplied as a credential.
+ * @param[in] slot      Requested slot
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t shw_slot_verify_access(void *user_data, uint64_t owner_id,
+					uint32_t slot);
+
+/*!
+ * Default keystore slot allocation
+ *
+ * This function first checks that the requested size is equal to or less than
+ * the maximum keystore slot size.  If so, it searches the keystore for a free
+ * key slot, and if found, marks it as used and returns a slot reference to the
+ * user.
+ *
+ * @param[in] user_data Pointer to the location where the keystore data
+ *                      structure stored.
+ * @param[in] size      Size of the key data that will be stored in this slot
+ *                      (octets)
+ * @param[in] owner_id  Owner ID supplied as a credential.
+ * @param[out] slot     Requested slot
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t shw_slot_alloc(void *user_data, uint32_t size,
+				uint64_t owner_id, uint32_t * slot);
+
+/*!
+ * Default keystore slot deallocation
+ *
+ * This function releases the given key slot in the keystore, making it
+ * available to store a new key.
+ *
+ * @param[in] user_data Pointer to the location where the keystore data
+ *                      structure stored.
+ * @param[in] owner_id  Owner ID supplied as a credential.
+ * @param[in] slot      Requested slot
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t shw_slot_dealloc(void *user_data,
+				  uint64_t owner_id, uint32_t slot);
+
+/*!
+ * Default keystore slot address lookup
+ *
+ * This function calculates the address where the key data is stored.
+ *
+ * @param[in] user_data Pointer to the location where the keystore data
+ *                      structure stored.
+ * @param[in] slot      Requested slot
+ *
+ * @return    SCC2: Virtual address (kernel or userspace) of the key data.
+ *            SCC: Physical address of the key data.
+ */
+void *shw_slot_get_address(void *user_data, uint32_t slot);
+
+/*!
+ * Default keystore slot base address lookup
+ *
+ * This function calculates the base address of the Secure Partition on which
+ * the key data is located.  For the reference design, only one Secure
+ * Partition is used per Keystore, however in general, any number may be used.
+ *
+ * @param[in] user_data Pointer to the location where the keystore data
+ *                      structure stored.
+ * @param[in] slot      Requested slot
+ *
+ * @return    SCC2: Secure Partition virtual (kernel or userspace) base address.
+ *            SCC: Secure Partition physical base address.
+ */
+uint32_t shw_slot_get_base(void *user_data, uint32_t slot);
+
+/*!
+ * Default keystore slot offset lookup
+ *
+ * This function calculates the offset from the base of the Secure Partition
+ * where the key data is located.
+ *
+ * @param[in] user_data Pointer to the location where the keystore data
+ *                      structure stored.
+ * @param[in] slot      Requested slot
+ *
+ * @return    SCC2: Key data offset (octets)
+ *            SCC: Not implemented
+ */
+uint32_t shw_slot_get_offset(void *user_data, uint32_t slot);
+
+/*!
+ * Default keystore slot offset lookup
+ *
+ * This function returns the size of the given key slot.  In the reference
+ * implementation, all key slots are of the same size, however in general,
+ * the keystore slot sizes can be made variable.
+ *
+ * @param[in] user_data Pointer to the location where the keystore data
+ *                      structure stored.
+ * @param[in] slot      Requested slot
+ *
+ * @return    SCC2: Keystore slot size.
+ *            SCC: Not implemented
+ */
+uint32_t shw_slot_get_slot_size(void *user_data, uint32_t slot);
+
+/* @} */
+
+#endif /* FSL_SHW_KEYSTORE_H */
diff --git a/drivers/mxc/security/sahara2/include/linux_port.h b/drivers/mxc/security/sahara2/include/linux_port.h
new file mode 100644
index 000000000000..880658f3ad90
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/linux_port.h
@@ -0,0 +1,1804 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file linux_port.h
+ *
+ * OS_PORT ported to Linux (2.6.9+ for now)
+ *
+ */
+
+ /*!
+  * @if USE_MAINPAGE
+  * @mainpage ==Linux version of== Generic OS API for STC Drivers
+  * @endif
+  *
+  * @section intro_sec Introduction
+  *
+  * This API / kernel programming environment blah blah.
+  *
+  * See @ref dkops "Driver-to-Kernel Operations" as a good place to start.
+  */
+
+#ifndef LINUX_PORT_H
+#define LINUX_PORT_H
+
+#define PORTABLE_OS_VERSION 101
+
+/* Linux Kernel Includes */
+#include <linux/version.h>	/* Current version Linux kernel */
+
+#if defined(CONFIG_MODVERSIONS) && ! defined(MODVERSIONS)
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+#include <linux/modversions.h>
+#endif
+#define MODVERSIONS
+#endif
+/*!
+ * __NO_VERSION__ defined due to Kernel module possibly spanning multiple
+ * files.
+ */
+#define __NO_VERSION__
+
+#include <linux/module.h>	/* Basic support for loadable modules,
+				   printk */
+#include <linux/init.h>		/* module_init, module_exit */
+#include <linux/kernel.h>	/* General kernel system calls */
+#include <linux/sched.h>	/* for interrupt.h */
+#include <linux/fs.h>		/* for inode */
+#include <linux/random.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>		/* kmalloc */
+
+#include <stdarg.h>
+
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+#include <linux/device.h>  /* used in dynamic power management */
+#else
+#include <linux/platform_device.h>  /* used in dynamic power management */
+#endif
+
+#include <linux/dmapool.h>
+#include <linux/dma-mapping.h>
+
+#include <linux/clk.h>         /* clock en/disable for DPM */
+
+#include <linux/dmapool.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/uaccess.h>            /* copy_to_user(), copy_from_user() */
+#include <asm/io.h>                 /* ioremap() */
+#include <asm/irq.h>
+#include <asm/cacheflush.h>
+
+#ifndef TRUE
+/*! Useful symbol for unsigned values used as flags. */
+#define TRUE 1
+#endif
+
+#ifndef FALSE
+/*! Useful symbol for unsigned values used as flags. */
+#define FALSE 0
+#endif
+
+/* These symbols are defined in Linux 2.6 and later.  Include here for minimal
+ * support of 2.4 kernel.
+ **/
+#if !defined(LINUX_VERSION_CODE) || LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+/*!
+ * Symbol defined somewhere in 2.5/2.6.  It is the return signature of an ISR.
+ */
+#define irqreturn_t void
+/*! Possible return value of 'modern' ISR routine. */
+#define IRQ_HANDLED
+/*! Method of generating value of 'modern' ISR routine. */
+#define IRQ_RETVAL(x)
+#endif
+
+/*!
+ * Type used for registering and deregistering interrupts.
+ */
+typedef int os_interrupt_id_t;
+
+/*!
+ * Type used as handle for a process
+ *
+ * See #os_get_process_handle() and #os_send_signal().
+ */
+/*
+ * The following should be defined this way, but it gets compiler errors
+ * on the current tool chain.
+ *
+ * typedef task_t *os_process_handle_t;
+ */
+
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+typedef task_t *os_process_handle_t;
+#else
+typedef struct task_struct *os_process_handle_t;
+#endif
+
+/*!
+ * Generic return code for functions which need such a thing.
+ *
+ * No knowledge should be assumed of the value of any of these symbols except
+ * that @c OS_ERROR_OK_S is guaranteed to be zero.
+ */
+typedef enum {
+	OS_ERROR_OK_S = 0,	/*!< Success  */
+	OS_ERROR_FAIL_S = -EIO,	/*!< Generic driver failure */
+	OS_ERROR_NO_MEMORY_S = -ENOMEM,	/*!< Failure to acquire/use memory  */
+	OS_ERROR_BAD_ADDRESS_S = -EFAULT,	/*!< Bad address  */
+	OS_ERROR_BAD_ARG_S = -EINVAL,	/*!< Bad input argument  */
+} os_error_code;
+
+/*!
+ * Handle to a lock.
+ */
+#ifdef CONFIG_PREEMPT_RT
+typedef raw_spinlock_t *os_lock_t;
+#else
+typedef spinlock_t *os_lock_t;
+#endif
+
+/*!
+ * Context while locking.
+ */
+typedef unsigned long os_lock_context_t;
+
+/*!
+ * Declare a wait object for sleeping/waking processes.
+ */
+#define OS_WAIT_OBJECT(name)                                            \
+    DECLARE_WAIT_QUEUE_HEAD(name##_qh)
+
+/*!
+ * Driver registration handle
+ *
+ * Used with #os_driver_init_registration(), #os_driver_add_registration(),
+ * and #os_driver_complete_registration().
+ */
+typedef struct {
+	unsigned reg_complete;	/*!< TRUE if next inits succeeded. */
+	dev_t dev;		/*!< dev_t for register_chrdev() */
+	struct file_operations fops;	/*!< struct for register_chrdev() */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13)
+	struct class_simple *cs;	/*!< results of class_simple_create() */
+#else
+	struct class *cs;	/*!< results of class_create() */
+#endif
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,26)
+	struct class_device *cd;	/*!< Result of class_device_create() */
+#else
+	struct device *cd;	/*!< Result of device_create() */
+#endif
+	unsigned power_complete;	/*!< TRUE if next inits succeeded */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+	struct device_driver dd;	/*!< struct for register_driver() */
+#else
+	struct platform_driver dd;	/*!< struct for register_driver() */
+#endif
+	struct platform_device pd;	/*!< struct for platform_register_device() */
+} os_driver_reg_t;
+
+/*
+ *  Function types which can be associated with driver entry points.
+ *
+ *  Note that init and shutdown are absent.
+ */
+/*! @{ */
+/*! Keyword for registering open() operation handler. */
+#define OS_FN_OPEN open
+/*! Keyword for registering close() operation handler. */
+#define OS_FN_CLOSE release
+/*! Keyword for registering read() operation handler. */
+#define OS_FN_READ read
+/*! Keyword for registering write() operation handler. */
+#define OS_FN_WRITE write
+/*! Keyword for registering ioctl() operation handler. */
+#define OS_FN_IOCTL ioctl
+/*! Keyword for registering mmap() operation handler. */
+#define OS_FN_MMAP mmap
+/*! @} */
+
+/*!
+ * Function signature for the portable interrupt handler
+ *
+ * While it would be nice to know which interrupt is being serviced, the
+ * Least Common Denominator rule says that no arguments get passed in.
+ *
+ * @return  Zero if not handled, non-zero if handled.
+ */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19)
+typedef int (*os_interrupt_handler_t) (int, void *, struct pt_regs *);
+#else
+typedef int (*os_interrupt_handler_t) (int, void *);
+#endif
+
+/*!
+ * @defgroup dkops Driver-to-Kernel Operations
+ *
+ * These are the operations which drivers should call to get the OS to perform
+ * services.
+ */
+
+/*! @addtogroup dkops */
+/*! @{ */
+
+/*!
+ * Register an interrupt handler.
+ *
+ * @param driver_name    The name of the driver
+ * @param interrupt_id   The interrupt line to monitor (type
+ *                       #os_interrupt_id_t)
+ * @param function       The function to be called to handle an interrupt
+ *
+ * @return #os_error_code
+ */
+#define os_register_interrupt(driver_name, interrupt_id, function)            \
+     request_irq(interrupt_id, function, 0, driver_name, NULL)
+
+/*!
+ * Deregister an interrupt handler.
+ *
+ * @param interrupt_id   The interrupt line to stop monitoring
+ *
+ * @return #os_error_code
+ */
+#define os_deregister_interrupt(interrupt_id)                                 \
+     free_irq(interrupt_id, NULL)
+
+/*!
+ * INTERNAL implementation of os_driver_init_registration()
+ *
+ * @return An os error code.
+ */
+inline static int os_drv_do_init_reg(os_driver_reg_t * handle)
+{
+	memset(handle, 0, sizeof(*handle));
+	handle->fops.owner = THIS_MODULE;
+	handle->power_complete = FALSE;
+	handle->reg_complete = FALSE;
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+	handle->dd.name = NULL;
+#else
+	handle->dd.driver.name = NULL;
+#endif
+
+	return OS_ERROR_OK_S;
+}
+
+/*!
+ * Initialize driver registration.
+ *
+ * If the driver handles open(), close(), ioctl(), read(), write(), or mmap()
+ * calls, then it needs to register their location with the kernel so that they
+ * get associated with the device.
+ *
+ * @param handle  The handle object to be used with this registration.  The
+ *                object must live (be in memory somewhere) at least until
+ *                os_driver_remove_registration() is called.
+ *
+ * @return A handle for further driver registration, or NULL if failed.
+ */
+#define os_driver_init_registration(handle)                                   \
+    os_drv_do_init_reg(&handle)
+
+/*!
+ * Add a function registration to driver registration.
+ *
+ * @param handle    A handle initialized by #os_driver_init_registration().
+ * @param name      Which function is being supported.
+ * @param function  The result of a call to a @c _REF version of one of the
+ *                  driver function signature macros
+ * @return void
+ */
+#define os_driver_add_registration(handle, name, function)                    \
+    do {handle.fops.name = (void*)(function); } while (0)
+
+/*!
+ * Record 'power suspend' function for the device.
+ *
+ * @param handle    A handle initialized by #os_driver_init_registration().
+ * @param function  Name of function to call on power suspend request
+ *
+ * Status: Provisional
+ *
+ * @return void
+ */
+#define os_driver_register_power_suspend(handle, function)                    \
+   handle.dd.suspend = function
+
+/*!
+ * Record 'power resume' function for the device.
+ *
+ * @param handle    A handle initialized by #os_driver_init_registration().
+ * @param function  Name of function to call on power resume request
+ *
+ * Status: Provisional
+ *
+ * @return void
+ */
+#define os_driver_register_resume(handle, function)                          \
+   handle.dd.resume = function
+
+/*!
+ * INTERNAL function of the Linux port of the OS API.   Implements the
+ * os_driver_complete_registration() function.
+ *
+ * @param handle    The handle used with #os_driver_init_registration().
+ * @param major      The major device number to be associated with the driver.
+ *                   If this value is zero, a major number may be assigned.
+ *                   See #os_driver_get_major() to determine final value.
+ *                   #os_driver_remove_registration().
+ * @param driver_name The driver name.  Can be used as part of 'device node'
+ *                   name on platforms which support such a feature.
+ *
+ * @return  An error code
+ */
+inline static int os_drv_do_reg(os_driver_reg_t * handle,
+				unsigned major, char *driver_name)
+{
+	os_error_code code = OS_ERROR_NO_MEMORY_S;
+	char *name = kmalloc(strlen(driver_name) + 1, 0);
+
+	if (name != NULL) {
+		memcpy(name, driver_name, strlen(driver_name) + 1);
+		code = OS_ERROR_OK_S;	/* OK so far */
+		/* If any chardev/POSIX routines were added, then do chrdev part */
+		if (handle->fops.open || handle->fops.release
+		    || handle->fops.read || handle->fops.write
+		    || handle->fops.ioctl || handle->fops.mmap) {
+
+			printk("ioctl pointer: %p.  mmap pointer: %p\n",
+			       handle->fops.ioctl, handle->fops.mmap);
+
+			/* this method is depricated, see:
+			 * http://lwn.net/Articles/126808/
+			 */
+			code =
+			    register_chrdev(major, driver_name, &handle->fops);
+
+			/* instead something like this: */
+#if 0
+			handle->dev = MKDEV(major, 0);
+			code =
+			    register_chrdev_region(handle->dev, 1, driver_name);
+			if (code < 0) {
+				code = OS_ERROR_FAIL_S;
+			} else {
+				cdev_init(&handle->cdev, &handle->fops);
+				code = cdev_add(&handle->cdev, major, 1);
+			}
+#endif
+
+			if (code < 0) {
+				code = OS_ERROR_FAIL_S;
+			} else {
+				if (code != 0) {
+					/* Zero was passed in for major; code is actual value */
+					handle->dev = MKDEV(code, 0);
+				} else {
+					handle->dev = MKDEV(major, 0);
+				}
+				code = OS_ERROR_OK_S;
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13)
+				handle->cs =
+				    class_simple_create(THIS_MODULE,
+							driver_name);
+				if (IS_ERR(handle->cs)) {
+					code = (os_error_code) handle->cs;
+					handle->cs = NULL;
+				} else {
+					handle->cd =
+					    class_simple_device_add(handle->cs,
+								    handle->dev,
+								    NULL,
+								    driver_name);
+					if (IS_ERR(handle->cd)) {
+						class_simple_device_remove
+						    (handle->dev);
+						unregister_chrdev(MAJOR
+								  (handle->dev),
+								  driver_name);
+						code =
+						    (os_error_code) handle->cs;
+						handle->cs = NULL;
+					} else {
+						handle->reg_complete = TRUE;
+					}
+				}
+#else
+				handle->cs =
+				    class_create(THIS_MODULE, driver_name);
+				if (IS_ERR(handle->cs)) {
+					code = (os_error_code) handle->cs;
+					handle->cs = NULL;
+				} else {
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,26)
+					handle->cd =
+					    class_device_create(handle->cs,
+								NULL,
+								handle->dev,
+								NULL,
+								driver_name);
+#else
+					handle->cd =
+					    device_create(handle->cs, NULL,
+							  handle->dev, NULL,
+							  driver_name);
+#endif
+					if (IS_ERR(handle->cd)) {
+						class_destroy(handle->cs);
+						unregister_chrdev(MAJOR
+								  (handle->dev),
+								  driver_name);
+						code =
+						    (os_error_code) handle->cs;
+						handle->cs = NULL;
+					} else {
+						handle->reg_complete = TRUE;
+					}
+				}
+#endif
+			}
+		}
+		/* ... fops routine registered */
+		/* Handle power management fns through separate interface */
+		if ((code == OS_ERROR_OK_S) &&
+		    (handle->dd.suspend || handle->dd.resume)) {
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+			handle->dd.name = name;
+			handle->dd.bus = &platform_bus_type;
+			code = driver_register(&handle->dd);
+#else
+			handle->dd.driver.name = name;
+			handle->dd.driver.bus = &platform_bus_type;
+			code = driver_register(&handle->dd.driver);
+#endif
+			if (code == OS_ERROR_OK_S) {
+				handle->pd.name = name;
+				handle->pd.id = 0;
+				code = platform_device_register(&handle->pd);
+				if (code != OS_ERROR_OK_S) {
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+					driver_unregister(&handle->dd);
+#else
+					driver_unregister(&handle->dd.driver);
+#endif
+				} else {
+					handle->power_complete = TRUE;
+				}
+			}
+		}		/* ... suspend or resume */
+	}			/* name != NULL */
+	return code;
+}
+
+/*!
+ * Finalize the driver registration with the kernel.
+ *
+ * Upon return from this call, the driver may begin receiving calls at the
+ * defined entry points.
+ *
+ * @param handle    The handle used with #os_driver_init_registration().
+ * @param major      The major device number to be associated with the driver.
+ *                   If this value is zero, a major number may be assigned.
+ *                   See #os_driver_get_major() to determine final value.
+ *                   #os_driver_remove_registration().
+ * @param driver_name The driver name.  Can be used as part of 'device node'
+ *                   name on platforms which support such a feature.
+ *
+ * @return  An error code
+ */
+#define os_driver_complete_registration(handle, major, driver_name)           \
+   os_drv_do_reg(&handle, major, driver_name)
+
+/*!
+ * Get driver Major Number from handle after a successful registration.
+ *
+ * @param   handle  A handle which has completed registration.
+ *
+ * @return The major number (if any) associated with the handle.
+ */
+#define os_driver_get_major(handle)                                           \
+    (handle.reg_complete ? MAJOR(handle.dev) : -1)
+
+/*!
+ * INTERNAL implemention of os_driver_remove_registration.
+ *
+ * @param handle    A handle initialized by #os_driver_init_registration().
+ *
+ * @return  An error code.
+ */
+inline static int os_drv_rmv_reg(os_driver_reg_t * handle)
+{
+	if (handle->reg_complete) {
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13)
+		if (handle->cd != NULL) {
+			class_simple_device_remove(handle->dev);
+			handle->cd = NULL;
+		}
+		if (handle->cs != NULL) {
+			class_simple_destroy(handle->cs);
+			handle->cs = NULL;
+		}
+		unregister_chrdev(MAJOR(handle->dev), handle->dd.name);
+#else
+		if (handle->cd != NULL) {
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,26)
+			class_device_destroy(handle->cs, handle->dev);
+#else
+			device_destroy(handle->cs, handle->dev);
+#endif
+			handle->cd = NULL;
+		}
+		if (handle->cs != NULL) {
+			class_destroy(handle->cs);
+			handle->cs = NULL;
+		}
+		unregister_chrdev(MAJOR(handle->dev), handle->dd.driver.name);
+#endif
+		handle->reg_complete = FALSE;
+	}
+	if (handle->power_complete) {
+		platform_device_unregister(&handle->pd);
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+		driver_unregister(&handle->dd);
+#else
+		driver_unregister(&handle->dd.driver);
+#endif
+		handle->power_complete = FALSE;
+	}
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+	if (handle->dd.name != NULL) {
+		kfree(handle->dd.name);
+		handle->dd.name = NULL;
+	}
+#else
+	if (handle->dd.driver.name != NULL) {
+		kfree(handle->dd.driver.name);
+		handle->dd.driver.name = NULL;
+	}
+#endif
+	return OS_ERROR_OK_S;
+}
+
+/*!
+ * Remove the driver's registration with the kernel.
+ *
+ * Upon return from this call, the driver not receive any more calls at the
+ * defined entry points (other than ISR and shutdown).
+ *
+ * @param handle    A handle initialized by #os_driver_init_registration().
+ *
+ * @return  An error code.
+ */
+#define os_driver_remove_registration(handle)                                 \
+   os_drv_rmv_reg(&handle)
+
+/*!
+ * Register a driver with the Linux Device Model.
+ *
+ * @param   driver_information  The device_driver structure information
+ *
+ * @return  An error code.
+ *
+ * Status: denigrated in favor of #os_driver_complete_registration()
+ */
+#define os_register_to_driver(driver_information)                             \
+              driver_register(driver_information)
+
+/*!
+ * Unregister a driver from the Linux Device Model
+ *
+ * this routine unregisters from the Linux Device Model
+ *
+ * @param   driver_information  The device_driver structure information
+ *
+ * @return  An error code.
+ *
+ * Status: Denigrated. See #os_register_to_driver().
+ */
+#define os_unregister_from_driver(driver_information)                         \
+                driver_unregister(driver_information)
+
+/*!
+ * register a device to a driver
+ *
+ * this routine registers a drivers devices to the Linux Device Model
+ *
+ * @param   device_information  The platform_device structure information
+ *
+ * @return  An error code.
+ *
+ * Status: denigrated in favor of #os_driver_complete_registration()
+ */
+#define os_register_a_device(device_information)                              \
+    platform_device_register(device_information)
+
+/*!
+ * unregister a device from a driver
+ *
+ * this routine unregisters a drivers devices from the Linux Device Model
+ *
+ * @param   device_information  The platform_device structure information
+ *
+ * @return  An error code.
+ *
+ * Status: Denigrated.  See #os_register_a_device().
+ */
+#define os_unregister_a_device(device_information)                            \
+    platform_device_unregister(device_information)
+
+/*!
+ * Print a message to console / into log file.  After the @c msg argument a
+ * number of printf-style arguments may be added.  Types should be limited to
+ * printf string, char, octal, decimal, and hexadecimal types.  (This excludes
+ * pointers, and floating point).
+ *
+ * @param  msg    The main text of the message to be logged
+ * @param  s      The printf-style arguments which go with msg, if any
+ *
+ * @return (void)
+ */
+#define os_printk(...)                                                        \
+    (void) printk(__VA_ARGS__)
+
+/*!
+ * Prepare a task to execute the given function.  This should only be done once
+ * per function,, during the driver's initialization routine.
+ *
+ * @param task_fn   Name of the OS_DEV_TASK() function to be created.
+ *
+ * @return an OS ERROR code.
+ */
+#define os_create_task(function_name)                                         \
+    OS_ERROR_OK_S
+
+/*!
+ * Schedule execution of a task.
+ *
+ * @param function_name   The function associated with the task.
+ *
+ * @return (void)
+ */
+#define os_dev_schedule_task(function_name)                                   \
+    tasklet_schedule(&(function_name ## let))
+
+/*!
+ * Make sure that task is no longer running and will no longer run.
+ *
+ * This function will not return until both are true.  This is useful when
+ * shutting down a driver.
+ */
+#define os_dev_stop_task(function_name)                                       \
+do {                                                                          \
+    tasklet_disable(&(function_name ## let));                                 \
+    tasklet_kill(&(function_name ## let));                                    \
+} while (0)
+
+/*!
+ * Allocate some kernel memory
+ *
+ * @param amount   Number of 8-bit bytes to allocate
+ * @param flags    Some indication of purpose of memory (needs definition)
+ *
+ * @return  Pointer to allocated memory, or NULL if failed.
+ */
+#define os_alloc_memory(amount, flags)                                        \
+    (void*)kmalloc(amount, flags)
+
+/*!
+ * Free some kernel memory
+ *
+ * @param location  The beginning of the region to be freed.
+ *
+ * Do some OSes have separate free() functions which should be
+ * distinguished by passing in @c flags here, too? Don't some also require the
+ * size of the buffer being freed?
+ */
+#define os_free_memory(location)                                              \
+    kfree(location)
+
+/*!
+ * Allocate cache-coherent memory
+ *
+ * @param       amount     Number of bytes to allocate
+ * @param[out]  dma_addrp  Location to store physical address of allocated
+ *                         memory.
+ * @param       flags      Some indication of purpose of memory (needs
+ *                         definition).
+ *
+ * @return (virtual space) pointer to allocated memory, or NULL if failed.
+ *
+ */
+#define os_alloc_coherent(amount, dma_addrp, flags)                           \
+    (void*)dma_alloc_coherent(NULL, amount, dma_addrp, flags)
+
+/*!
+ * Free cache-coherent memory
+ *
+ * @param       size       Number of bytes which were allocated.
+ * @param       virt_addr  Virtual(kernel) address of memory.to be freed, as
+ *                         returned by #os_alloc_coherent().
+ * @param       dma_addr   Physical address of memory.to be freed, as returned
+ *                         by #os_alloc_coherent().
+ *
+ * @return void
+ *
+ */
+#define os_free_coherent(size, virt_addr, dma_addr)                           \
+    dma_free_coherent(NULL, size, virt_addr, dma_addr
+
+/*!
+ * Map an I/O space into kernel memory space
+ *
+ * @param start       The starting address of the (physical / io space) region
+ * @param range_bytes The number of bytes to map
+ *
+ * @return A pointer to the mapped area, or NULL on failure
+ */
+#define os_map_device(start, range_bytes)                                     \
+    (void*)ioremap_nocache((start), range_bytes)
+
+/*!
+ * Unmap an I/O space from kernel memory space
+ *
+ * @param start       The starting address of the (virtual) region
+ * @param range_bytes The number of bytes to unmap
+ *
+ * @return None
+ */
+#define os_unmap_device(start, range_bytes)                                   \
+    iounmap((void*)(start))
+
+/*!
+ * Copy data from Kernel space to User space
+ *
+ * @param to   The target location in user memory
+ * @param from The source location in kernel memory
+ * @param size The number of bytes to be copied
+ *
+ * @return #os_error_code
+ */
+#define os_copy_to_user(to, from, size)                                       \
+    ((copy_to_user(to, from, size) == 0) ? 0 : OS_ERROR_BAD_ADDRESS_S)
+
+/*!
+ * Copy data from User space to Kernel space
+ *
+ * @param to   The target location in kernel memory
+ * @param from The source location in user memory
+ * @param size The number of bytes to be copied
+ *
+ * @return #os_error_code
+ */
+#define os_copy_from_user(to, from, size)                                     \
+    ((copy_from_user(to, from, size) == 0) ? 0 : OS_ERROR_BAD_ADDRESS_S)
+
+/*!
+ * Read a 8-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @return                  The value in the register
+ */
+#define os_read8(register_address)                                            \
+    __raw_readb(register_address)
+
+/*!
+ * Write a 8-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @param value             The value to write into the register
+ */
+#define os_write8(register_address, value)                                    \
+    __raw_writeb(value, register_address)
+
+/*!
+ * Read a 16-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @return                  The value in the register
+ */
+#define os_read16(register_address)                                           \
+    __raw_readw(register_address)
+
+/*!
+ * Write a 16-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @param value             The value to write into the register
+ */
+#define os_write16(register_address, value)                                   \
+    __raw_writew(value, (uint32_t*)(register_address))
+
+/*!
+ * Read a 32-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @return                  The value in the register
+ */
+#define os_read32(register_address)                                           \
+    __raw_readl((uint32_t*)(register_address))
+
+/*!
+ * Write a 32-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @param value             The value to write into the register
+ */
+#define os_write32(register_address, value)                                   \
+    __raw_writel(value, register_address)
+
+/*!
+ * Read a 64-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @return                  The value in the register
+ */
+#define os_read64(register_address)                                           \
+     ERROR_UNIMPLEMENTED
+
+/*!
+ * Write a 64-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @param value             The value to write into the register
+ */
+#define os_write64(register_address, value)                                   \
+    ERROR_UNIMPLEMENTED
+
+/*!
+ * Delay some number of microseconds
+ *
+ * Note that this is a busy-loop, not a suspension of the task/process.
+ *
+ * @param  msecs   The number of microseconds to delay
+ *
+ * @return void
+ */
+#define os_mdelay mdelay
+
+/*!
+ * Calculate virtual address from physical address
+ *
+ * @param pa    Physical address
+ *
+ * @return virtual address
+ *
+ * @note this assumes that addresses are 32 bits wide
+ */
+#define os_va __va
+
+/*!
+ * Calculate physical address from virtual address
+ *
+ *
+ * @param va    Virtual address
+ *
+ * @return physical address
+ *
+ * @note this assumes that addresses are 32 bits wide
+ */
+#define os_pa __pa
+
+#ifdef CONFIG_PREEMPT_RT
+/*!
+ * Allocate and initialize a lock, returning a lock handle.
+ *
+ * The lock state will be initialized to 'unlocked'.
+ *
+ * @return A lock handle, or NULL if an error occurred.
+ */
+inline static os_lock_t os_lock_alloc_init(void)
+{
+	raw_spinlock_t *lockp;
+	lockp = (raw_spinlock_t *) kmalloc(sizeof(raw_spinlock_t), 0);
+	if (lockp) {
+		_raw_spin_lock_init(lockp);
+	} else {
+		printk("OS: lock init failed\n");
+	}
+
+	return lockp;
+}
+#else
+/*!
+ * Allocate and initialize a lock, returning a lock handle.
+ *
+ * The lock state will be initialized to 'unlocked'.
+ *
+ * @return A lock handle, or NULL if an error occurred.
+ */
+inline static os_lock_t os_lock_alloc_init(void)
+{
+	spinlock_t *lockp;
+	lockp = (spinlock_t *) kmalloc(sizeof(spinlock_t), 0);
+	if (lockp) {
+		spin_lock_init(lockp);
+	} else {
+		printk("OS: lock init failed\n");
+	}
+
+	return lockp;
+}
+#endif				/* CONFIG_PREEMPT_RT */
+
+/*!
+ * Acquire a lock.
+ *
+ * This function should only be called from an interrupt service routine.
+ *
+ * @param   lock_handle  A handle to the lock to acquire.
+ *
+ * @return void
+ */
+#define os_lock(lock_handle)                                              \
+   spin_lock(lock_handle)
+
+/*!
+ * Unlock a lock.  Lock must have been acquired by #os_lock().
+ *
+ * @param   lock_handle  A handle to the lock to unlock.
+ *
+ * @return void
+ */
+#define os_unlock(lock_handle)                                            \
+   spin_unlock(lock_handle)
+
+/*!
+ * Acquire a lock in non-ISR context
+ *
+ * This function will spin until the lock is available.
+ *
+ * @param lock_handle  A handle of the lock to acquire.
+ * @param context      Place to save the before-lock context
+ *
+ * @return void
+ */
+#define os_lock_save_context(lock_handle, context)                        \
+    spin_lock_irqsave(lock_handle, context)
+
+/*!
+ * Release a lock in non-ISR context
+ *
+ * @param lock_handle  A handle of the lock to release.
+ * @param context      Place where before-lock context was saved.
+ *
+ * @return void
+ */
+#define os_unlock_restore_context(lock_handle, context)                  \
+    spin_unlock_irqrestore(lock_handle, context)
+
+/*!
+ * Deallocate a lock handle.
+ *
+ * @param lock_handle   An #os_lock_t that has been allocated.
+ *
+ * @return void
+ */
+#define os_lock_deallocate(lock_handle)                                   \
+    kfree(lock_handle)
+
+/*!
+ * Determine process handle
+ *
+ * The process handle of the current user is returned.
+ *
+ * @return   A handle on the current process.
+ */
+#define os_get_process_handle()                                           \
+    current
+
+/*!
+ * Send a signal to a process
+ *
+ * @param  proc   A handle to the target process.
+ * @param  sig    The POSIX signal to send to that process.
+ */
+#define os_send_signal(proc, sig)                                         \
+    send_sig(sig, proc, 0);
+
+/*!
+ * Get some random bytes
+ *
+ * @param buf    The location to store the random data.
+ * @param count  The number of bytes to store.
+ *
+ * @return  void
+ */
+#define os_get_random_bytes(buf, count)                                   \
+    get_random_bytes(buf, count)
+
+/*!
+ * Go to sleep on an object.
+ *
+ * @param object    The object on which to sleep
+ * @param condition An expression to check for sleep completion.  Must be
+ *                  coded so that it can be referenced more than once inside
+ *                  macro, i.e., no ++ or other modifying expressions.
+ * @param atomic    Non-zero if sleep must not return until condition.
+ *
+ * @return error code -- OK or sleep interrupted??
+ */
+#define os_sleep(object, condition, atomic)                             \
+({                                                                      \
+    DEFINE_WAIT(_waitentry_);                                           \
+    os_error_code code = OS_ERROR_OK_S;                                 \
+                                                                        \
+    while (!(condition)) {                                              \
+        prepare_to_wait(&(object##_qh), &_waitentry_,                   \
+                        atomic ? 0 : TASK_INTERRUPTIBLE);               \
+        if (!(condition)) {                                             \
+            schedule();                                                 \
+        }                                                               \
+                                                                        \
+        finish_wait(&(object##_qh), &_waitentry_);                      \
+                                                                        \
+        if (!atomic && signal_pending(current)) {                       \
+            code = OS_ERROR_FAIL_S; /* NEED SOMETHING BETTER */         \
+            break;                                                      \
+        }                                                               \
+    };                                                                  \
+                                                                        \
+    code;                                                               \
+})
+
+/*!
+ * Wake up whatever is sleeping on sleep object
+ *
+ * @param object  The object on which things might be sleeping
+ *
+ * @return none
+ */
+#define os_wake_sleepers(object)                                        \
+    wake_up_interruptible(&(object##_qh));
+
+	  /*! @} *//* dkops */
+
+/******************************************************************************
+ *  Function signature-generating macros
+ *****************************************************************************/
+
+/*!
+ * @defgroup drsigs Driver Signatures
+ *
+ * These macros will define the entry point signatures for interrupt handlers;
+ * driver initialization and shutdown; device open/close; etc.
+ *
+ * There are two versions of each macro for a given Driver Entry Point.  The
+ * first version is used to define a function and its implementation in the
+ * driver.c file, e.g. #OS_DEV_INIT().
+ *
+ * The second form is used whenever a forward declaration (prototype) is
+ * needed.  It has the letters @c _DCL appended to the name of the defintion
+ * function, and takes only the first two arguments (driver_name and
+ * function_name).  These are not otherwise mentioned in this documenation.
+ *
+ * There is a third form used when a reference to a function is required, for
+ * instance when passing the routine as a pointer to a function.  It has the
+ * letters @c _REF appended to it, and takes only the first two arguments
+ * (driver_name and function_name).  These functions are not otherwise
+ * mentioned in this documentation.
+ *
+ * (Note that these two extra forms are required because of the
+ * possibility/likelihood of having a 'wrapper function' which invokes the
+ * generic function with expected arguments.  An alternative would be to have a
+ * generic function which isn't able to get at any arguments directly, but
+ * would be equipped with macros which could get at information passed in.
+ *
+ * Example:
+ *
+ * (in a header file)
+ * @code
+ * OS_DEV_INIT_DCL(widget, widget_init);
+ * @endcode
+ *
+ * (in an implementation file)
+ * @code
+ * OS_DEV_INIT(widget, widget_init)
+ * {
+ *     os_dev_init_return(TRUE);
+ * }
+ * @endcode
+ *
+ */
+
+/*! @addtogroup drsigs */
+/*! @{ */
+
+/*!
+ * Define a function which will handle device initialization
+ *
+ * This is tne driver initialization routine.  This is normally where the
+ * part would be initialized; queues, locks, interrupts handlers defined;
+ * long-term dynamic memory allocated for driver use; etc.
+ *
+ * @param function_name   The name of the portable initialization function.
+ *
+ * @return  A call to #os_dev_init_return()
+ *
+ */
+#define OS_DEV_INIT(function_name)                                            \
+module_init(function_name);                                                   \
+static int __init function_name (void)
+
+/*! Make declaration for driver init function.
+ * @param function_name foo
+ */
+#define OS_DEV_INIT_DCL(function_name)                                        \
+static int __init function_name (void);
+
+/*!
+ * Generate a function reference to the driver's init function.
+ * @param function_name   Name of the OS_DEV_INIT() function.
+ *
+ * @return A function pointer.
+ */
+#define OS_DEV_INIT_REF(function_name)                                        \
+function_name
+
+/*!
+ * Define a function which will handle device shutdown
+ *
+ * This is the inverse of the #OS_DEV_INIT() routine.
+ *
+ * @param function_name   The name of the portable driver shutdown routine.
+ *
+ * @return  A call to #os_dev_shutdown_return()
+ *
+ */
+#define OS_DEV_SHUTDOWN(function_name)                                        \
+module_exit(function_name);                                                   \
+static void function_name(void)
+
+/*!
+ * Generate a function reference to the driver's shutdown function.
+ * @param function_name   Name of the OS_DEV_HUSTDOWN() function.
+ *
+ * @return A function pointer.
+ */
+#define OS_DEV_SHUTDOWN_DCL(function_name)                                    \
+static void function_name(void);
+
+/*!
+ * Generate a reference to driver's shutdown function
+ * @param function_name   Name of the OS_DEV_HUSTDOWN() function.
+*/
+
+#define OS_DEV_SHUTDOWN_REF(function_name)                                    \
+function_name
+
+/*!
+ * Define a function which will open the device for a user.
+ *
+ * @param function_name The name of the driver open() function
+ *
+ * @return  A call to #os_dev_open_return()
+ */
+#define OS_DEV_OPEN(function_name)                                            \
+static int function_name(struct inode* inode_p_, struct file* file_p_)
+
+/*!
+ * Declare prototype for an open() function.
+ *
+ * @param function_name The name of the OS_DEV_OPEN() function.
+ */
+#define OS_DEV_OPEN_DCL(function_name)                                        \
+OS_DEV_OPEN(function_name);
+
+/*!
+ * Generate a function reference to the driver's open() function.
+ * @param function_name   Name of the OS_DEV_OPEN() function.
+ *
+ * @return A function pointer.
+ */
+#define OS_DEV_OPEN_REF(function_name)                                        \
+function_name
+
+/*!
+ * Define a function which will handle a user's ioctl() request
+ *
+ * @param function_name The name of the driver ioctl() function
+ *
+ * @return A call to #os_dev_ioctl_return()
+ */
+#define OS_DEV_IOCTL(function_name)                                           \
+static int function_name(struct inode* inode_p_, struct file* file_p_,        \
+                     unsigned int cmd_, unsigned long data_)
+
+/*! Boo. */
+#define OS_DEV_IOCTL_DCL(function_name)                                       \
+OS_DEV_IOCTL(function_name);
+
+/*!
+ * Generate a function reference to the driver's ioctl() function.
+ * @param function_name   Name of the OS_DEV_IOCTL() function.
+ *
+ * @return A function pointer.
+ */
+#define OS_DEV_IOCTL_REF(function_name)                                       \
+function_name
+
+/*!
+ * Define a function which will handle a user's mmap() request
+ *
+ * @param function_name The name of the driver mmap() function
+ *
+ * @return A call to #os_dev_ioctl_return()
+ */
+#define OS_DEV_MMAP(function_name)                                            \
+int function_name(struct file* file_p_, struct vm_area_struct* vma_)
+
+#define OS_DEV_MMAP_DCL(function_name)                                        \
+OS_DEV_MMAP(function_name);
+
+#define OS_DEV_MMAP_REF(function_name)                                        \
+function_name
+
+/* Retrieve the context to the memory structure that is to be MMAPed */
+#define os_mmap_memory_ctx() (vma_)
+
+/* Determine the size of the requested MMAP region*/
+#define os_mmap_memory_size() (vma_->vm_end - vma_->vm_start)
+
+/* Determine the base address of the requested MMAP region*/
+#define os_mmap_user_base() (vma_->vm_start)
+
+/*!
+ * Declare prototype for an read() function.
+ *
+ * @param function_name The name of the driver read function.
+ */
+#define OS_DEV_READ_DCL(function_name)                                        \
+OS_DEV_READ(function_name);
+
+/*!
+ * Generate a function reference to the driver's read() routine
+ * @param function_name   Name of the OS_DEV_READ() function.
+ *
+ * @return A function pointer.
+ */
+#define OS_DEV_READ_REF(function_name)                                        \
+function_name
+
+/*!
+ * Define a function which will handle a user's write() request
+ *
+ * @param function_name The name of the driver write() function
+ *
+ * @return A call to #os_dev_write_return()
+ */
+#define OS_DEV_WRITE(function_name)                                           \
+static ssize_t function_name(struct file* file_p_, char* user_buffer_,        \
+                     size_t count_bytes_, loff_t* file_position_)
+
+/*!
+ * Declare prototype for an write() function.
+ *
+ * @param function_name The name of the driver write function.
+ */
+#define OS_DEV_WRITE_DCL(function_name)                                       \
+OS_DEV_WRITE(function_name);
+
+/*!
+ * Generate a function reference to the driver's write() routine
+ * @param function_name   Name of the OS_DEV_WRITE() function.
+ *
+ * @return A function pointer.
+ */
+#define OS_DEV_WRITE_REF(function_name)                                       \
+function_name
+
+/*!
+ * Define a function which will close the device - opposite of OS_DEV_OPEN()
+ *
+ * @param function_name The name of the driver close() function
+ *
+ * @return A call to #os_dev_close_return()
+ */
+#define OS_DEV_CLOSE(function_name)                                           \
+static int function_name(struct inode* inode_p_, struct file* file_p_)
+
+/*!
+ * Declare prototype for an close() function
+ *
+ * @param function_name The name of the driver close() function.
+ */
+#define OS_DEV_CLOSE_DCL(function_name)                                       \
+OS_DEV_CLOSE(function_name);
+
+/*!
+ * Generate a function reference to the driver's close function.
+ * @param function_name   Name of the OS_DEV_CLOSE() function.
+ *
+ * @return A function pointer.
+ */
+#define OS_DEV_CLOSE_REF(function_name)                                       \
+function_name
+
+/*!
+ * Define a function which will handle an interrupt
+ *
+ * No arguments are available to the generic function.  It must not invoke any
+ * OS functions which are illegal in a ISR.  It gets no parameters, and must
+ * have a call to #os_dev_isr_return() instead of any/all return statements.
+ *
+ * Example:
+ * @code
+ * OS_DEV_ISR(widget)
+ * {
+ *     os_dev_isr_return(1);
+ * }
+ * @endcode
+ *
+ * @param function_name The name of the driver ISR function
+ *
+ * @return   A call to #os_dev_isr_return()
+ */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19)
+#define OS_DEV_ISR(function_name)                                             \
+static irqreturn_t function_name(int N1_, void* N2_, struct pt_regs* N3_)
+#else
+#define OS_DEV_ISR(function_name)                                             \
+static irqreturn_t function_name(int N1_, void* N2_)
+#endif
+
+/*!
+ * Declare prototype for an ISR function.
+ *
+ * @param function_name The name of the driver ISR function.
+ */
+#define OS_DEV_ISR_DCL(function_name)                                         \
+OS_DEV_ISR(function_name);
+
+/*!
+ * Generate a function reference to the driver's interrupt service routine
+ * @param function_name   Name of the OS_DEV_ISR() function.
+ *
+ * @return A function pointer.
+ */
+#define OS_DEV_ISR_REF(function_name)                                         \
+function_name
+
+/*!
+ * Define a function which will operate as a background task / bottom half.
+ *
+ * Tasklet stuff isn't strictly limited to 'Device drivers', but leave it
+ * this namespace anyway.
+ *
+ * @param function_name The name of this background task function
+ *
+ * @return A call to #os_dev_task_return()
+ */
+#define OS_DEV_TASK(function_name)                                            \
+static void function_name(unsigned long data_)
+
+/*!
+ * Declare prototype for a background task / bottom half function
+ *
+ * @param function_name The name of this background task function
+ */
+#define OS_DEV_TASK_DCL(function_name)                                        \
+OS_DEV_TASK(function_name);                                                   \
+DECLARE_TASKLET(function_name ## let, function_name, 0);
+
+/*!
+ * Generate a reference to an #OS_DEV_TASK() function
+ *
+ * @param function_name   The name of the task being referenced.
+ */
+#define OS_DEV_TASK_REF(function_name)                                        \
+    (function_name ## let)
+
+	  /*! @} *//* drsigs */
+
+/*****************************************************************************
+ *  Functions/Macros for returning values from Driver Signature routines
+ *****************************************************************************/
+
+/*!
+ * Return from the #OS_DEV_INIT() function
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+#define os_dev_init_return(code)                                             \
+    return code
+
+/*!
+ * Return from the #OS_DEV_SHUTDOWN() function
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+#define os_dev_shutdown_return(code)                                         \
+    return
+
+/*!
+ * Return from the #OS_DEV_ISR() function
+ *
+ * The function should verify that it really was supposed to be called,
+ * and that its device needed attention, in order to properly set the
+ * return code.
+ *
+ * @param code    non-zero if interrupt handled, zero otherwise.
+ *
+ */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19)
+#define os_dev_isr_return(code)                                              \
+do {                                                                         \
+    /* Unused warnings */                                                    \
+    (void)N1_;                                                               \
+    (void)N2_;                                                               \
+    (void)N3_;                                                               \
+                                                                             \
+    return IRQ_RETVAL(code);                                                 \
+} while (0)
+#else
+#define os_dev_isr_return(code)                                              \
+do {                                                                         \
+    /* Unused warnings */                                                    \
+    (void)N1_;                                                               \
+    (void)N2_;                                                               \
+                                                                             \
+    return IRQ_RETVAL(code);                                                 \
+} while (0)
+#endif
+
+/*!
+ * Return from the #OS_DEV_OPEN() function
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+#define os_dev_open_return(code)                                             \
+do {                                                                         \
+    int retcode = code;                                                      \
+                                                                             \
+    /* get rid of 'unused parameter' warnings */                             \
+    (void)inode_p_;                                                          \
+    (void)file_p_;                                                           \
+                                                                             \
+    return retcode;                                                          \
+} while (0)
+
+/*!
+ * Return from the #OS_DEV_IOCTL() function
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+#define os_dev_ioctl_return(code)                                            \
+do {                                                                         \
+    int retcode = code;                                                      \
+                                                                             \
+    /* get rid of 'unused parameter' warnings */                             \
+    (void)inode_p_;                                                          \
+    (void)file_p_;                                                           \
+    (void)cmd_;                                                              \
+    (void)data_;                                                             \
+                                                                             \
+    return retcode;                                                          \
+} while (0)
+
+/*!
+ * Return from the #OS_DEV_READ() function
+ *
+ * @param code    Number of bytes read, or an error code to report failure.
+ *
+ */
+#define os_dev_read_return(code)                                             \
+do {                                                                         \
+    ssize_t retcode = code;                                                  \
+                                                                             \
+    /* get rid of 'unused parameter' warnings */                             \
+    (void)file_p_;                                                           \
+    (void)user_buffer_;                                                      \
+    (void)count_bytes_;                                                      \
+    (void)file_position_;                                                    \
+                                                                             \
+    return retcode;                                                          \
+} while (0)
+
+/*!
+ * Return from the #OS_DEV_WRITE() function
+ *
+ * @param code    Number of bytes written, or an error code to report failure.
+ *
+ */
+#define os_dev_write_return(code)                                            \
+do {                                                                         \
+    ssize_t retcode = code;                                                  \
+                                                                             \
+    /* get rid of 'unused parameter' warnings */                             \
+    (void)file_p_;                                                           \
+    (void)user_buffer_;                                                      \
+    (void)count_bytes_;                                                      \
+    (void)file_position_;                                                    \
+                                                                             \
+    return retcode;                                                          \
+} while (0)
+
+/*!
+ * Return from the #OS_DEV_CLOSE() function
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+#define os_dev_close_return(code)                                            \
+do {                                                                         \
+    ssize_t retcode = code;                                                  \
+                                                                             \
+    /* get rid of 'unused parameter' warnings */                             \
+    (void)inode_p_;                                                          \
+    (void)file_p_;                                                           \
+                                                                             \
+    return retcode;                                                          \
+} while (0)
+
+/*!
+ * Start the #OS_DEV_TASK() function
+ *
+ * In some implementations, this could be turned into a label for
+ * the os_dev_task_return() call.
+ *
+ * @return none
+ */
+#define os_dev_task_begin()
+
+/*!
+ * Return from the #OS_DEV_TASK() function
+ *
+ * In some implementations, this could be turned into a sleep followed
+ * by a jump back to the os_dev_task_begin() call.
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+#define os_dev_task_return(code)                                             \
+do {                                                                         \
+    /* Unused warnings */                                                    \
+    (void)data_;                                                             \
+                                                                             \
+    return;                                                                  \
+} while (0)
+
+/*****************************************************************************
+ *  Functions/Macros for accessing arguments from Driver Signature routines
+ *****************************************************************************/
+
+/*! @defgroup drsigargs Functions for Getting Arguments in Signature functions
+ *
+ */
+/* @addtogroup @drsigargs */
+/*! @{ */
+/*!
+ * Used in #OS_DEV_OPEN(), #OS_DEV_CLOSE(), #OS_DEV_IOCTL(), #OS_DEV_READ() and
+ * #OS_DEV_WRITE() routines to check whether user is requesting read
+ * (permission)
+ */
+#define os_dev_is_flag_read()                                                 \
+   (file_p_->f_mode & FMODE_READ)
+
+/*!
+ * Used in #OS_DEV_OPEN(), #OS_DEV_CLOSE(), #OS_DEV_IOCTL(), #OS_DEV_READ() and
+ * #OS_DEV_WRITE() routines to check whether user is requesting write
+ * (permission)
+ */
+#define os_dev_is_flag_write()                                                \
+   (file_p_->f_mode & FMODE_WRITE)
+
+/*!
+ * Used in #OS_DEV_OPEN(), #OS_DEV_CLOSE(), #OS_DEV_IOCTL(), #OS_DEV_READ() and
+ * #OS_DEV_WRITE() routines to check whether user is requesting non-blocking
+ * I/O.
+ */
+#define os_dev_is_flag_nonblock()                                             \
+   (file_p_->f_flags & (O_NONBLOCK | O_NDELAY))
+
+/*!
+ * Used in #OS_DEV_OPEN() and #OS_DEV_CLOSE() to determine major device being
+ * accessed.
+ */
+#define os_dev_get_major()                                                    \
+    (imajor(inode_p_))
+
+/*!
+ * Used in #OS_DEV_OPEN() and #OS_DEV_CLOSE() to determine minor device being
+ * accessed.
+ */
+#define os_dev_get_minor()                                                    \
+    (iminor(inode_p_))
+
+/*!
+ * Used in #OS_DEV_IOCTL() to determine which operation the user wants
+ * performed.
+ *
+ * @return Value of the operation.
+ */
+#define os_dev_get_ioctl_op()                                                 \
+    (cmd_)
+
+/*!
+ * Used in #OS_DEV_IOCTL() to return the associated argument for the desired
+ * operation.
+ *
+ * @return    A value which can be cast to a struct pointer or used as
+ *            int/long.
+ */
+#define os_dev_get_ioctl_arg()                                                \
+    (data_)
+
+/*!
+ * Used in OS_DEV_READ() and OS_DEV_WRITE() routines to access the requested
+ * byte count.
+ *
+ * @return  (unsigned) a count of bytes
+ */
+#define os_dev_get_count()                                                    \
+    ((unsigned)count_bytes_)
+
+/*!
+ * Used in OS_DEV_READ() and OS_DEV_WRITE() routines to return the pointer
+ * byte count.
+ *
+ * @return   char* pointer to user buffer
+ */
+#define os_dev_get_user_buffer()                                              \
+    ((void*)user_buffer_)
+
+/*!
+ * Used in OS_DEV_READ(), OS_DEV_WRITE(), and OS_DEV_IOCTL() routines to
+ * get the POSIX flags field for the associated open file).
+ *
+ * @return The flags associated with the file.
+ */
+#define os_dev_get_file_flags()                                               \
+    (file_p_->f_flags)
+
+/*!
+ * Set the driver's private structure associated with this file/open.
+ *
+ * Generally used during #OS_DEV_OPEN().  See #os_dev_get_user_private().
+ *
+ * @param  struct_p   The driver data structure to associate with this user.
+ */
+#define os_dev_set_user_private(struct_p)                                     \
+    file_p_->private_data = (void*)(struct_p)
+
+/*!
+ * Get the driver's private structure associated with this file.
+ *
+ * May be used during #OS_DEV_OPEN(), #OS_DEV_READ(), #OS_DEV_WRITE(),
+ * #OS_DEV_IOCTL(), and #OS_DEV_CLOSE().  See #os_dev_set_user_private().
+ *
+ * @return   The driver data structure to associate with this user.
+ */
+#define os_dev_get_user_private()                                             \
+    ((void*)file_p_->private_data)
+
+/*!
+ * Get the IRQ associated with this call to the #OS_DEV_ISR() function.
+ *
+ * @return  The IRQ (integer) interrupt number.
+ */
+#define os_dev_get_irq()                                                      \
+    N1_
+
+	   /*! @} *//* drsigargs */
+
+/*!
+ * @defgroup cacheops Cache Operations
+ *
+ * These functions are for synchronizing processor cache with RAM.
+ */
+/*! @addtogroup cacheops */
+/*! @{ */
+
+/*!
+ * Flush and invalidate all cache lines.
+ */
+#if 0
+#define os_flush_cache_all()                                              \
+    flush_cache_all()
+#else
+/* Call ARM fn directly, in case L2cache=on3 not set */
+#define os_flush_cache_all()                                              \
+    v6_flush_kern_cache_all_L2()
+
+/*!
+ * ARM-routine to flush all cache.  Defined here, because it exists in no
+ * easy-access header file.  ARM-11 with L210 cache only!
+ */
+extern void v6_flush_kern_cache_all_L2(void);
+#endif
+
+/*
+ *  These macros are using part of the Linux DMA API.  They rely on the
+ *  map function to do nothing more than the equivalent clean/inv/flush
+ *  operation at the time of the mapping, and do nothing at an unmapping
+ *  call, which the Sahara driver code will never invoke.
+ */
+
+/*!
+ * Clean a range of addresses from the cache.  That is, write updates back
+ * to (RAM, next layer).
+ *
+ * @param  start    Starting virtual address
+ * @param  len      Number of bytes to flush
+ *
+ * @return void
+ */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)
+#define os_cache_clean_range(start,len)                                   \
+    dma_map_single(NULL, (void*)start, len, DMA_TO_DEVICE)
+#else
+#define os_cache_clean_range(start,len)                                   \
+{                                                                         \
+    void *s = (void*)start;                                               \
+    void *e = s + len;                                                    \
+    dmac_clean_range(s, e);                                               \
+    outer_clean_range(__pa(s), __pa(e));                                  \
+}
+#endif
+
+/*!
+ * Invalidate a range of addresses in the cache
+ *
+ * @param  start    Starting virtual address
+ * @param  len      Number of bytes to flush
+ *
+ * @return void
+ */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)
+#define os_cache_inv_range(start,len)                                     \
+    dma_map_single(NULL, (void*)start, len, DMA_FROM_DEVICE)
+#else
+#define os_cache_inv_range(start,len)                                     \
+{                                                                         \
+    void *s = (void*)start;                                               \
+    void *e = s + len;                                                    \
+    dmac_inv_range(s, e);                                                 \
+    outer_inv_range(__pa(s), __pa(e));                                    \
+}
+#endif
+
+/*!
+ * Flush a range of addresses from the cache.  That is, perform clean
+ * and invalidate
+ *
+ * @param  start    Starting virtual address
+ * @param  len      Number of bytes to flush
+ *
+ * @return void
+ */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)
+#define os_cache_flush_range(start,len)                                   \
+    dma_map_single(NULL, (void*)start, len, DMA_BIDIRECTIONAL)
+#else
+#define os_cache_flush_range(start,len)                                   \
+{                                                                         \
+    void *s = (void*)start;                                               \
+    void *e = s + len;                                                    \
+    dmac_flush_range(s, e);                                               \
+    outer_flush_range(__pa(s), __pa(e));                                  \
+}
+#endif
+
+	  /*! @} *//* cacheops */
+
+#endif				/* LINUX_PORT_H */
diff --git a/drivers/mxc/security/sahara2/include/platform_abstractions.h b/drivers/mxc/security/sahara2/include/platform_abstractions.h
new file mode 100644
index 000000000000..c2f9c489eb0b
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/platform_abstractions.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/*!
+ * @file platform_abstractions.h
+ */
diff --git a/drivers/mxc/security/sahara2/include/portable_os.h b/drivers/mxc/security/sahara2/include/portable_os.h
new file mode 100644
index 000000000000..e904b3222cbb
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/portable_os.h
@@ -0,0 +1,1453 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef PORTABLE_OS_H
+#define PORTABLE_OS_H
+
+/***************************************************************************/
+
+/*
+ * Add support for your target OS by checking appropriate flags and then
+ * including the appropriate file.  Don't forget to document the conditions
+ * in the later documentation section at the beginning of the
+ * DOXYGEN_PORTABLE_OS_DOC.
+ */
+
+#if defined(LINUX_KERNEL)
+
+#include "linux_port.h"
+
+#elif defined(PORTABLE_OS)
+
+#include "check_portability.h"
+
+#else
+
+#error Target OS unsupported or unspecified
+
+#endif
+
+
+/***************************************************************************/
+
+/*!
+ * @file portable_os.h
+ *
+ * This file should be included by portable driver code in order to gain access
+ * to the OS-specific header files.  It is the only OS-related header file that
+ * the writer of a portable driver should need.
+ *
+ * This file also contains the documentation for the common API.
+ *
+ * Begin reading the documentation for this file at the @ref index "main page".
+ *
+ */
+
+/*!
+ * @if USE_MAINPAGE
+ * @mainpage Generic OS API for STC Drivers
+ * @endif
+ *
+ * @section intro_sec Introduction
+ *
+ * This defines the API / kernel programming environment for portable drivers.
+ *
+ * This API is broken up into several functional areas.  It greatly limits the
+ * choices of a device-driver author, but at the same time should allow for
+ * greater portability of the resulting code.
+ *
+ * Each kernel-to-driver function (initialization function, interrupt service
+ * routine, etc.) has a 'portable signature' which must be used, and a specific
+ * function which must be called to generate the return statement.  There is
+ * one exception, a background task or "bottom half" routine, which instead has
+ * a specific structure which must be followed.  These signatures and function
+ * definitions are found in @ref drsigs.
+ *
+ * None of these kernel-to-driver functions seem to get any arguments passed to
+ * them.  Instead, there are @ref drsigargs which allow one of these functions
+ * to get at fairly generic parts of its calling arguments, if there are any.
+ *
+ * Almost every driver will have some need to call the operating system
+ * @ref dkops is the list of services which are available to the driver.
+ *
+ *
+ * @subsection warn_sec Warning
+ *
+ * The specifics of the types, values of the various enumerations
+ * (unless specifically stated, like = 0), etc., are only here for illustrative
+ * purposes.  No attempts should be made to make use of any specific knowledge
+ * gleaned from this documentation.  These types are only meant to be passed in
+ * and out of the API, and their contents are to be handled only by the
+ * provided OS-specific functions.
+ *
+ * Also, note that the function may be provided as macros in some
+ * implementations, or vice versa.
+ *
+ *
+ * @section dev_sec Writing a Portable Driver
+ *
+ * First off, writing a portable driver means calling no function in an OS
+ * except what is available through this header file.
+ *
+ * Secondly, all OS-called functions in your driver must be invoked and
+ * referenced through the signature routines.
+ *
+ * Thirdly, there might be some rules which you can get away with ignoring or
+ * violating on one OS, yet will cause your code not to be portable to a
+ * different OS.
+ *
+ *
+ * @section limit_sec Limitations
+ *
+ * This API is not expected to handle every type of driver which may be found
+ * in an operating system.  For example, it will not be able to handle the
+ * usual design for something like a UART driver, where there are multiple
+ * logical devices to access, because the design usually calls for some sort of
+ * indication to the #OS_DEV_TASK() function or OS_DEV_ISR() to indicate which
+ * channel is to be serviced by that instance of the task/function.  This sort
+ * of argument is missing in this API for functions like os_dev_schedule_task() and
+ * os_register_interrupt().
+ *
+ *
+ * @section port_guide Porting Guidelines
+ *
+ * This section is intended for a developer who needs to port the header file
+ * to an operating system which is not yet supported.
+ *
+ * This interface allows for a lot of flexibility when it comes to porting to
+ * an operating systems device driver interface.  There are three main areas to
+ * examine:  The use of Driver Routine Signatures, the use of Driver Argument
+ * Access functions, the Calls to Kernel Functions, and Data Types.
+ *
+ *
+ * @subsection port_sig Porting Driver Routine Signatures
+ *
+ * The three macros for each function (e.g. #OS_DEV_INIT(), #OS_DEV_INIT_DCL(),
+ * and #OS_DEV_INIT_REF()) allow the flexibility of having a 'wrapper' function
+ * with the OS-required signature, which would then call the user's function
+ * with some different signature.
+ *
+ * The first form would lay down the wrapper function, followed by the
+ * signature for the user function.  The second form would lay down just the
+ * signatures for both functions, and the last function would reference the
+ * wrapper function, since that is the interface function called by the OS.
+ *
+ * Note that the driver author has no visibility at all to the signature of the
+ * routines.  The author can access arguments only through a limited set of
+ * functions, and must return via another function.
+ *
+ * The Return Functions allow a lot of flexibility in converting the return
+ * value, or not returning a value at all.  These will likely be implemented as
+ * macros.
+ *
+ *
+ * @subsection port_arg Porting Driver Argument Access Functions
+ *
+ * The signatures defined by the guide will usually be replaced with macro
+ * definitions.
+ *
+ *
+ * @subsection port_dki Porting Calls to Kernel Functions
+ *
+ * The signatures defined by the guide may be replaced with macro definitions,
+ * if that makes more sense.
+ *
+ * Implementors are free to ignore arguments which are not applicable to their
+ * OS.
+ *
+ * @subsection port_datatypes Porting Data Types
+ *
+ *
+ */
+
+/***************************************************************************
+ * Compile flags
+ **************************************************************************/
+
+/*
+ * This compile flag should only be turned on when running doxygen to generate
+ * the API documentation.
+ */
+#ifdef DOXYGEN_PORTABLE_OS_DOC
+
+/*!
+ * @todo module_init()/module_cleanup() for Linux need to be added to OS
+ * abstractions.  Also need EXPORT_SYMBOL() equivalent??
+ *
+ */
+
+/* Drop OS differentation documentation here */
+
+/*!
+ * \#define this flag to build your driver as a Linux driver
+ */
+#define LINUX
+
+/* end OS differentation documentation */
+
+/*!
+ * Symbol to give version number of the implementation file.  Earliest
+ * definition is in version 1.1, with value 101 (to mean version 1.1)
+ */
+#define PORTABLE_OS_VERSION 101
+
+/*
+ * NOTICE: The following definitions (the rest of the file) are not meant ever
+ * to be compiled.  Instead, they are the documentation for the portable OS
+ * API, to be used by driver writers.
+ *
+ * Each individual OS port will define each of these types, functions, or
+ * macros as appropriate to the target OS. This is why they are under the
+ * DOXYGEN_PORTABLE_OS_DOC flag.
+ */
+
+/***************************************************************************
+ * Type definitions
+ **************************************************************************/
+
+/*!
+ * Type used for registering and deregistering interrupts.
+ *
+ * This is typically an interrupt channel number.
+ */
+typedef int os_interrupt_id_t;
+
+/*!
+ * Type used as handle for a process
+ *
+ * See #os_get_process_handle() and #os_send_signal().
+ */
+typedef int os_process_handle_t;
+
+/*!
+ * Generic return code for functions which need such a thing.
+ *
+ * No knowledge should be assumed of the value of any of these symbols except
+ * that @c OS_ERROR_OK_S is guaranteed to be zero.
+ *
+ * @todo Any other named values? What about (-EAGAIN?  -ERESTARTSYS? Are they
+ * too Linux/Unix-specific read()/write() return values) ?
+ */
+typedef enum {
+	OS_ERROR_OK_S = 0,	/*!< Success  */
+	OS_ERROR_FAIL_S,	/*!< Generic driver failure */
+	OS_ERROR_NO_MEMORY_S,	/*!< Failure to acquire/use memory  */
+	OS_ERROR_BAD_ADDRESS_S,	/*!< Bad address  */
+	OS_ERROR_BAD_ARG_S	/*!< Bad input argument */
+} os_error_code;
+
+/*!
+ * Handle to a lock.
+ */
+typedef int *os_lock_t;
+
+/*!
+ * Context while locking.
+ */
+typedef int os_lock_context_t;
+
+/*!
+ * An object which can be slept on and later used to wake any/all sleepers.
+ */
+typedef int os_sleep_object_t;
+
+/*!
+ * Driver registration handle
+ */
+typedef void *os_driver_reg_t;
+
+/*!
+ * Function signature for an #OS_DEV_INIT() function.
+ *
+ * @return  A call to os_dev_init_return() function.
+ */
+typedef void (*os_init_function_t) (void);
+
+/*!
+ * Function signature for an #OS_DEV_SHUTDOWN() function.
+ *
+ * @return  A call to os_dev_shutdown_return() function.
+ */
+typedef void (*os_shutdown_function_t) (void);
+
+/*!
+ * Function signature for a user-driver function.
+ *
+ * @return  A call to the appropriate os_dev_xxx_return() function.
+ */
+typedef void (*os_user_function_t) (void);
+
+/*!
+ * Function signature for the portable interrupt handler
+ *
+ * While it would be nice to know which interrupt is being serviced, the
+ * Least Common Denominator rule says that no arguments get passed in.
+ *
+ * @return  A call to #os_dev_isr_return()
+ */
+typedef void (*os_interrupt_handler_t) (void);
+
+/*!
+ * Function signature for a task function
+ *
+ * Many task function definitions get some sort of generic argument so that the
+ * same function can run across many (queues, channels, ...) as separate task
+ * instances.  This has been left out of this API.
+ *
+ * This function must be structured as documented by #OS_DEV_TASK().
+ *
+ */
+typedef void (*os_task_fn_t) (void);
+
+/*!
+ *  Function types which can be associated with driver entry points.  These are
+ *  used in os_driver_add_registration().
+ *
+ *  Note that init and shutdown are absent.
+ */
+typedef enum {
+	OS_FN_OPEN,		/*!< open() operation handler. */
+	OS_FN_CLOSE,		/*!< close() operation handler. */
+	OS_FN_READ,		/*!< read() operation handler. */
+	OS_FN_WRITE,		/*!< write() operation handler. */
+	OS_FN_IOCTL,		/*!< ioctl() operation handler. */
+	OS_FN_MMAP		/*!< mmap() operation handler. */
+} os_driver_fn_t;
+
+/***************************************************************************
+ * Driver-to-Kernel Operations
+ **************************************************************************/
+
+/*!
+ * @defgroup dkops Driver-to-Kernel Operations
+ *
+ * These are the operations which drivers should call to get the OS to perform
+ * services.
+ */
+
+/*! @addtogroup dkops */
+/*! @{ */
+
+/*!
+ * Register an interrupt handler.
+ *
+ * @param driver_name    The name of the driver
+ * @param interrupt_id   The interrupt line to monitor (type
+ *                       #os_interrupt_id_t)
+ * @param function       The function to be called to handle an interrupt
+ *
+ * @return #os_error_code
+ */
+os_error_code os_register_interrupt(char *driver_name,
+				    os_interrupt_id_t interrupt_id,
+				    os_interrupt_handler_t function);
+
+/*!
+ * Deregister an interrupt handler.
+ *
+ * @param interrupt_id   The interrupt line to stop monitoring
+ *
+ * @return #os_error_code
+ */
+os_error_code os_deregister_interrupt(os_interrupt_id_t interrupt_id);
+
+/*!
+ * Initialize driver registration.
+ *
+ * If the driver handles open(), close(), ioctl(), read(), write(), or mmap()
+ * calls, then it needs to register their location with the kernel so that they
+ * get associated with the device.
+ *
+ * @param handle  The handle object to be used with this registration.  The
+ *                object must live (be in memory somewhere) at least until
+ *                os_driver_remove_registration() is called.
+ *
+ * @return An os error code.
+ */
+os_error_code os_driver_init_registration(os_driver_reg_t handle);
+
+/*!
+ * Add a function registration to driver registration.
+ *
+ * @param handle    The handle used with #os_driver_init_registration().
+ * @param name      Which function is being supported.
+ * @param function  The result of a call to a @c _REF version of one of the
+ *                  driver function signature macros
+ *                  driver function signature macros
+ * @return void
+ */
+void os_driver_add_registration(os_driver_reg_t handle, os_driver_fn_t name,
+				void *function);
+
+/*!
+ * Finalize the driver registration with the kernel.
+ *
+ * Upon return from this call, the driver may begin receiving calls at the
+ * defined entry points.
+ *
+ * @param handle    The handle used with #os_driver_init_registration().
+ * @param major      The major device number to be associated with the driver.
+ *                   If this value is zero, a major number may be assigned.
+ *                   See #os_driver_get_major() to determine final value.
+ *                   #os_driver_remove_registration().
+ * @param driver_name The driver name.  Can be used as part of 'device node'
+ *                   name on platforms which support such a feature.
+ *
+ * @return  An error code
+ */
+os_error_code os_driver_complete_registration(os_driver_reg_t handle,
+					      int major, char *driver_name);
+
+/*!
+ * Get driver Major Number from handle after a successful registration.
+ *
+ * @param   handle  A handle which has completed registration.
+ *
+ * @return The major number (if any) associated with the handle.
+ */
+uint32_t os_driver_get_major(os_driver_reg_t handle);
+
+/*!
+ * Remove the driver's registration with the kernel.
+ *
+ * Upon return from this call, the driver not receive any more calls at the
+ * defined entry points (other than ISR and shutdown).
+ *
+ * @param major       The major device number to be associated with the driver.
+ * @param driver_name The driver name
+ *
+ * @return  An error code.
+ */
+os_error_code os_driver_remove_registration(int major, char *driver_name);
+
+/*!
+ * Print a message to console / into log file.  After the @c msg argument a
+ * number of printf-style arguments may be added.  Types should be limited to
+ * printf string, char, octal, decimal, and hexadecimal types.  (This excludes
+ * pointers, and floating point).
+ *
+ * @param  msg  The message to print to console / system log
+ *
+ * @return (void)
+ */
+void os_printk(char *msg, ...);
+
+/*!
+ * Allocate some kernel memory
+ *
+ * @param amount   Number of 8-bit bytes to allocate
+ * @param flags    Some indication of purpose of memory (needs definition)
+ *
+ * @return  Pointer to allocated memory, or NULL if failed.
+ */
+void *os_alloc_memory(unsigned amount, int flags);
+
+/*!
+ * Free some kernel memory
+ *
+ * @param location  The beginning of the region to be freed.
+ *
+ * Do some OSes have separate free() functions which should be
+ * distinguished by passing in @c flags here, too? Don't some also require the
+ * size of the buffer being freed?  Perhaps separate routines for each
+ * alloc/free pair (DMAable, etc.)?
+ */
+void os_free_memory(void *location);
+
+/*!
+ * Allocate cache-coherent memory
+ *
+ * @param       amount     Number of bytes to allocate
+ * @param[out]  dma_addrp  Location to store physical address of allocated
+ *                         memory.
+ * @param       flags      Some indication of purpose of memory (needs
+ *                         definition).
+ *
+ * @return (virtual space) pointer to allocated memory, or NULL if failed.
+ *
+ */
+void *os_alloc_coherent(unsigned amount, uint32_t * dma_addrp, int flags);
+
+/*!
+ * Free cache-coherent memory
+ *
+ * @param       size       Number of bytes which were allocated.
+ * @param[out]  virt_addr  Virtual(kernel) address of memory.to be freed, as
+ *                         returned by #os_alloc_coherent().
+ * @param[out]  dma_addr   Physical address of memory.to be freed, as returned
+ *                         by #os_alloc_coherent().
+ *
+ * @return void
+ *
+ */
+void os_free_coherent(unsigned size, void *virt_addr, uint32_t dma_addr);
+
+/*!
+ * Map an I/O space into kernel memory space
+ *
+ * @param start       The starting address of the (physical / io space) region
+ * @param range_bytes The number of bytes to map
+ *
+ * @return A pointer to the mapped area, or NULL on failure
+ */
+void *os_map_device(uint32_t start, unsigned range_bytes);
+
+/*!
+ * Unmap an I/O space from kernel memory space
+ *
+ * @param start       The starting address of the (virtual) region
+ * @param range_bytes The number of bytes to unmap
+ *
+ * @return None
+ */
+void os_unmap_device(void *start, unsigned range_bytes);
+
+/*!
+ * Copy data from Kernel space to User space
+ *
+ * @param to   The target location in user memory
+ * @param from The source location in kernel memory
+ * @param size The number of bytes to be copied
+ *
+ * @return #os_error_code
+ */
+os_error_code os_copy_to_user(void *to, void *from, unsigned size);
+
+/*!
+ * Copy data from User space to Kernel space
+ *
+ * @param to   The target location in kernel memory
+ * @param from The source location in user memory
+ * @param size The number of bytes to be copied
+ *
+ * @return #os_error_code
+ */
+os_error_code os_copy_from_user(void *to, void *from, unsigned size);
+
+/*!
+ * Read an 8-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @return                  The value in the register
+ */
+uint8_t os_read8(uint8_t * register_address);
+
+/*!
+ * Write an 8-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @param value             The value to write into the register
+ */
+void os_write8(uint8_t * register_address, uint8_t value);
+
+/*!
+ * Read a 16-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @return                  The value in the register
+ */
+uint16_t os_read16(uint16_t * register_address);
+
+/*!
+ * Write a 16-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @param value             The value to write into the register
+ */
+void os_write16(uint16_t * register_address, uint16_t value);
+
+/*!
+ * Read a 32-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @return                  The value in the register
+ */
+uint32_t os_read32(uint32_t * register_address);
+
+/*!
+ * Write a 32-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @param value             The value to write into the register
+ */
+void os_write32(uint32_t * register_address, uint32_t value);
+
+/*!
+ * Read a 64-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @return                  The value in the register
+ */
+uint64_t os_read64(uint64_t * register_address);
+
+/*!
+ * Write a 64-bit device register
+ *
+ * @param register_address  The (bus) address of the register to write to
+ * @param value             The value to write into the register
+ */
+void os_write64(uint64_t * register_address, uint64_t value);
+
+/*!
+ * Prepare a task to execute the given function.  This should only be done once
+ * per task, during the driver's initialization routine.
+ *
+ * @param task_fn   Name of the OS_DEV_TASK() function to be created.
+ *
+ * @return an OS ERROR code.
+ */
+os_error os_create_task(os_task_fn_t * task_fn);
+
+/*!
+ * Run the task associated with an #OS_DEV_TASK() function
+ *
+ * The task will begin execution sometime after or during this call.
+ *
+ * @param task_fn   Name of the OS_DEV_TASK() function to be scheduled.
+ *
+ * @return void
+ */
+void os_dev_schedule_task(os_task_fn_t * task_fn);
+
+/*!
+ * Make sure that task is no longer running and will no longer run.
+ *
+ * This function will not return until both are true.  This is useful when
+ * shutting down a driver.
+ *
+ * @param task_fn   Name of the OS_DEV_TASK() funciton to be stopped.
+ *
+ */
+void os_stop_task(os_task_fn_t * task_fn);
+
+/*!
+ * Delay some number of microseconds
+ *
+ * Note that this is a busy-loop, not a suspension of the task/process.
+ *
+ * @param  msecs   The number of microseconds to delay
+ *
+ * @return void
+ */
+void os_mdelay(unsigned long msecs);
+
+/*!
+ * Calculate virtual address from physical address
+ *
+ * @param pa    Physical address
+ *
+ * @return virtual address
+ *
+ * @note this assumes that addresses are 32 bits wide
+ */
+void *os_va(uint32_t pa);
+
+/*!
+ * Calculate physical address from virtual address
+ *
+ *
+ * @param va    Virtual address
+ *
+ * @return physical address
+ *
+ * @note this assumes that addresses are 32 bits wide
+ */
+uint32_t os_pa(void *va);
+
+/*!
+ * Allocate and initialize a lock, returning a lock handle.
+ *
+ * The lock state will be initialized to 'unlocked'.
+ *
+ * @return A lock handle, or NULL if an error occurred.
+ */
+os_lock_t os_lock_alloc_init(void);
+
+/*!
+ * Acquire a lock.
+ *
+ * This function should only be called from an interrupt service routine.
+ *
+ * @param   lock_handle  A handle to the lock to acquire.
+ *
+ * @return void
+ */
+void os_lock(os_lock_t lock_handle);
+
+/*!
+ * Unlock a lock.  Lock must have been acquired by #os_lock().
+ *
+ * @param   lock_handle  A handle to the lock to unlock.
+ *
+ * @return void
+ */
+void os_unlock(os_lock_t lock_handle);
+
+/*!
+ * Acquire a lock in non-ISR context
+ *
+ * This function will spin until the lock is available.
+ *
+ * @param lock_handle  A handle of the lock to acquire.
+ * @param context      Place to save the before-lock context
+ *
+ * @return void
+ */
+void os_lock_save_context(os_lock_t lock_handle, os_lock_context_t context);
+
+/*!
+ * Release a lock in non-ISR context
+ *
+ * @param lock_handle  A handle of the lock to release.
+ * @param context      Place where before-lock context was saved.
+ *
+ * @return void
+ */
+void os_unlock_restore_context(os_lock_t lock_handle,
+			       os_lock_context_t context);
+
+/*!
+ * Deallocate a lock handle.
+ *
+ * @param lock_handle   An #os_lock_t that has been allocated.
+ *
+ * @return void
+ */
+void os_lock_deallocate(os_lock_t lock_handle);
+
+/*!
+ * Determine process handle
+ *
+ * The process handle of the current user is returned.
+ *
+ * @return   A handle on the current process.
+ */
+os_process_handle_t os_get_process_handle();
+
+/*!
+ * Send a signal to a process
+ *
+ * @param  proc   A handle to the target process.
+ * @param  sig    The POSIX signal to send to that process.
+ */
+void os_send_signal(os_process_handle_t proc, int sig);
+
+/*!
+ * Get some random bytes
+ *
+ * @param buf    The location to store the random data.
+ * @param count  The number of bytes to store.
+ *
+ * @return  void
+ */
+void os_get_random_bytes(void *buf, unsigned count);
+
+/*!
+ * Go to sleep on an object.
+ *
+ * Example: code = os_sleep(my_queue, available_count == 0, 0);
+ *
+ * @param object    The object on which to sleep
+ * @param condition An expression to check for sleep completion.  Must be
+ *                  coded so that it can be referenced more than once inside
+ *                  macro, i.e., no ++ or other modifying expressions.
+ * @param atomic    Non-zero if sleep must not return until condition.
+ *
+ * @return error code -- OK or sleep interrupted??
+ */
+os_error_code os_sleep(os_sleep_object_t object, unsigned condition,
+		       unsigned atomic);
+
+/*!
+ * Wake up whatever is sleeping on sleep object
+ *
+ * @param object  The object on which things might be sleeping
+ *
+ * @return none
+ */
+void os_wake_sleepers(os_sleep_object_t object);
+
+	  /*! @} *//* dkops */
+
+/*****************************************************************************
+ *  Function-signature-generating macros
+ *****************************************************************************/
+
+/*!
+ * @defgroup drsigs Driver Function Signatures
+ *
+ * These macros will define the entry point signatures for interrupt handlers;
+ * driver initialization and shutdown; device open/close; etc.  They are to be
+ * used whenever the Kernel will call into the Driver.  They are not
+ * appropriate for driver calls to other routines in the driver.
+ *
+ * There are three versions of each macro for a given Driver Entry Point.  The
+ * first version is used to define a function and its implementation in the
+ * driver.c file, e.g. #OS_DEV_INIT().
+ *
+ * The second form is used whenever a forward declaration (prototype) is
+ * needed.  It has the letters @c _DCL appended to the name of the definition
+ * function.  These are not otherwise mentioned in this documenation.
+ *
+ * There is a third form used when a reference to a function is required, for
+ * instance when passing the routine as a pointer to a function.  It has the
+ * letters @c _REF appended to the name of the definition function
+ * (e.g. DEV_IOCTL_REF).
+ *
+ * Note that these two extra forms are required because of the possibility of
+ * having an invisible 'wrapper function' created by the os-specific header
+ * file which would need to be invoked by the operating system, and which in
+ * turn would invoke the generic function.
+ *
+ * Example:
+ *
+ * (in a header file)
+ * @code
+ * OS_DEV_INIT_DCL(widget_init);
+ * OS_DEV_ISR_DCL(widget_isr);
+ * @endcode
+ *
+ * (in an implementation file)
+ * @code
+ * OS_DEV_INIT(widget, widget_init)
+ * {
+ *
+ *     os_register_interrupt("widget", WIDGET_IRQ, OS_DEV_ISR_REF(widget_isr));
+ *
+ *     os_dev_init_return(OS_RETURN_NO_ERROR_S);
+ * }
+ *
+ * OS_DEV_ISR(widget_isr)
+ * {
+ *     os_dev_isr_return(TRUE);
+ * }
+ * @endcode
+ */
+
+/*! @addtogroup drsigs */
+/*! @{ */
+
+/*!
+ * Define a function which will handle device initialization
+ *
+ * This is tne driver initialization routine.  This is normally where the
+ * part would be initialized; queues, locks, interrupts handlers defined;
+ * long-term dynamic memory allocated for driver use; etc.
+ *
+ * @param function_name   The name of the portable initialization function.
+ *
+ * @return  A call to #os_dev_init_return()
+ *
+ */
+#define OS_DEV_INIT(function_name)
+
+/*!
+ * Define a function which will handle device shutdown
+ *
+ * This is the reverse of the #OS_DEV_INIT() routine.
+ *
+ * @param function_name   The name of the portable driver shutdown routine.
+ *
+ * @return  A call to #os_dev_shutdown_return()
+ */
+#define OS_DEV_SHUTDOWN(function_name)
+
+/*!
+ * Define a function which will open the device for a user.
+ *
+ * @param function_name The name of the driver open() function
+ *
+ * @return A call to #os_dev_open_return()
+ */
+#define OS_DEV_OPEN(function_name)
+
+/*!
+ * Define a function which will handle a user's ioctl() request
+ *
+ * @param function_name The name of the driver ioctl() function
+ *
+ * @return A call to #os_dev_ioctl_return()
+ */
+#define OS_DEV_IOCTL(function_name)
+
+/*!
+ * Define a function which will handle a user's read() request
+ *
+ * @param function_name The name of the driver read() function
+ *
+ * @return A call to #os_dev_read_return()
+ */
+#define OS_DEV_READ(function_name)
+
+/*!
+ * Define a function which will handle a user's write() request
+ *
+ * @param function_name The name of the driver write() function
+ *
+ * @return A call to #os_dev_write_return()
+ */
+#define OS_DEV_WRITE(function_name)
+
+/*!
+ * Define a function which will handle a user's mmap() request
+ *
+ * The mmap() function requests the driver to map some memory into user space.
+ *
+ * @todo Determine what support functions are needed for mmap() handling.
+ *
+ * @param function_name The name of the driver mmap() function
+ *
+ * @return A call to #os_dev_mmap_return()
+ */
+#define OS_DEV_MMAP(function_name)
+
+/*!
+ * Define a function which will close the device - opposite of OS_DEV_OPEN()
+ *
+ * @param function_name The name of the driver close() function
+ *
+ * @return A call to #os_dev_close_return()
+ */
+#define OS_DEV_CLOSE(function_name)
+
+/*!
+ * Define a function which will handle an interrupt
+ *
+ * No arguments are available to the generic function.  It must not invoke any
+ * OS functions which are illegal in a ISR.  It gets no parameters, and must
+ * have a call to #os_dev_isr_return() instead of any/all return statements.
+ *
+ * Example:
+ * @code
+ * OS_DEV_ISR(widget, widget_isr, WIDGET_IRQ_NUMBER)
+ * {
+ *     os_dev_isr_return(1);
+ * }
+ * @endcode
+ *
+ * @param function_name The name of the driver ISR function
+ *
+ * @return   A call to #os_dev_isr_return()
+ */
+#define OS_DEV_ISR(function_name)
+
+/*!
+ * Define a function which will operate as a background task / bottom half.
+ *
+ * The function implementation must be structured in the following manner:
+ * @code
+ * OS_DEV_TASK(widget_task)
+ * {
+ *     OS_DEV_TASK_SETUP(widget_task);
+ *
+ *     while OS_DEV_TASK_CONDITION(widget_task) }
+ *
+ *     };
+ * }
+ * @endcode
+ *
+ * @todo In some systems the OS_DEV_TASK_CONDITION() will be an action which
+ * will cause the task to sleep on some event triggered by os_run_task().  In
+ * others, the macro will reference a variable laid down by
+ * OS_DEV_TASK_SETUP() to make sure that the loop is only performed once.
+ *
+ * @param function_name The name of this background task function
+ */
+#define OS_DEV_TASK(function_name)
+
+	  /*! @} *//* drsigs */
+
+/*! @defgroup dclsigs Routines to declare Driver Signature routines
+ *
+ * These macros drop prototypes suitable for forward-declaration of
+ * @ref drsigs "function signatures".
+ */
+
+/*! @addtogroup dclsigs */
+/*! @{ */
+
+/*!
+ * Declare prototype for the device initialization function
+ *
+ * @param function_name   The name of the portable initialization function.
+ */
+#define OS_DEV_INIT_DCL(function_name)
+
+/*!
+ * Declare prototype for the device shutdown function
+ *
+ * @param function_name   The name of the portable driver shutdown routine.
+ *
+ * @return  A call to #os_dev_shutdown_return()
+ */
+#define OS_DEV_SHUTDOWN_DCL(function_name)
+
+/*!
+ * Declare prototype for the open() function.
+ *
+ * @param function_name The name of the driver open() function
+ *
+ * @return A call to #os_dev_open_return()
+ */
+#define OS_DEV_OPEN_DCL(function_name)
+
+/*!
+ * Declare prototype for the user's ioctl() request function
+ *
+ * @param function_name The name of the driver ioctl() function
+ *
+ * @return A call to #os_dev_ioctl_return()
+ */
+#define OS_DEV_IOCTL_DCL(function_name)
+
+/*!
+ * Declare prototype for the function a user's read() request
+ *
+ * @param function_name The name of the driver read() function
+ */
+#define OS_DEV_READ_DCL(function_name)
+
+/*!
+ * Declare prototype for the user's write() request function
+ *
+ * @param function_name The name of the driver write() function
+ */
+#define OS_DEV_WRITE_DCL(function_name)
+
+/*!
+ * Declare prototype for the user's mmap() request function
+ *
+ * @param function_name The name of the driver mmap() function
+ */
+#define OS_DEV_MMAP_DCL(function_name)
+
+/*!
+ * Declare prototype for the close function
+ *
+ * @param function_name The name of the driver close() function
+ *
+ * @return A call to #os_dev_close_return()
+ */
+#define OS_DEV_CLOSE_DCL(function_name)
+
+/*!
+ * Declare prototype for the interrupt handling function
+ *
+ * @param function_name The name of the driver ISR function
+ */
+#define OS_DEV_ISR_DCL(function_name)
+
+/*!
+ * Declare prototype for a background task / bottom half function
+ *
+ * @param function_name The name of this background task function
+ */
+#define OS_DEV_TASK_DCL(function_name)
+
+	  /*! @} *//* dclsigs */
+
+/*****************************************************************************
+ *  Functions for Returning Values from Driver Signature routines
+ *****************************************************************************/
+
+/*!
+ * @defgroup retfns Functions to Return Values from Driver Signature routines
+ */
+
+/*! @addtogroup retfns */
+/*! @{ */
+
+/*!
+ * Return from the #OS_DEV_INIT() function
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+void os_dev_init_return(os_error_code code);
+
+/*!
+ * Return from the #OS_DEV_SHUTDOWN() function
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+void os_dev_shutdown_return(os_error_code code);
+
+/*!
+ * Return from the #OS_DEV_ISR() function
+ *
+ * The function should verify that it really was supposed to be called,
+ * and that its device needed attention, in order to properly set the
+ * return code.
+ *
+ * @param code    non-zero if interrupt handled, zero otherwise.
+ *
+ */
+void os_dev_isr_return(int code);
+
+/*!
+ * Return from the #OS_DEV_OPEN() function
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+void os_dev_open_return(os_error_code code);
+
+/*!
+ * Return from the #OS_DEV_IOCTL() function
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+void os_dev_ioctl_return(os_error_code code);
+
+/*!
+ * Return from the #OS_DEV_READ() function
+ *
+ * @param code    Number of bytes read, or an error code to report failure.
+ *
+ */
+void os_dev_read_return(os_error_code code);
+
+/*!
+ * Return from the #OS_DEV_WRITE() function
+ *
+ * @param code    Number of bytes written, or an error code to report failure.
+ *
+ */
+void os_dev_write_return(os_error_code code);
+
+/*!
+ * Return from the #OS_DEV_MMAP() function
+ *
+ * @param code    Number of bytes written, or an error code to report failure.
+ *
+ */
+void os_dev_mmap_return(os_error_code code);
+
+/*!
+ * Return from the #OS_DEV_CLOSE() function
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+void os_dev_close_return(os_error_code code);
+
+/*!
+ * Start the #OS_DEV_TASK() function
+ *
+ * In some implementations, this could be turned into a label for
+ * the os_dev_task_return() call.
+ *
+ * For a more portable interface, should this take the sleep object as an
+ * argument???
+ *
+ * @return none
+ */
+void os_dev_task_begin(void);
+
+/*!
+ * Return from the #OS_DEV_TASK() function
+ *
+ * In some implementations, this could be turned into a sleep followed
+ * by a jump back to the os_dev_task_begin() call.
+ *
+ * @param code    An error code to report success or failure.
+ *
+ */
+void os_dev_task_return(os_error_code code);
+
+	  /*! @} *//* retfns */
+
+/*****************************************************************************
+ *  Functions/Macros for accessing arguments from Driver Signature routines
+ *****************************************************************************/
+
+/*! @defgroup drsigargs Functions for Getting Arguments in Signature functions
+ *
+ */
+/* @addtogroup @drsigargs */
+/*! @{ */
+
+/*!
+ * Check whether user is requesting read (permission) on the file/device.
+ * Usable in #OS_DEV_OPEN(), #OS_DEV_CLOSE(), #OS_DEV_IOCTL(), #OS_DEV_READ()
+ * and #OS_DEV_WRITE() routines.
+ */
+int os_dev_is_flag_read(void);
+
+/*!
+ * Check whether user is requesting write (permission) on the file/device.
+ * Usable in #OS_DEV_OPEN(), #OS_DEV_CLOSE(), #OS_DEV_IOCTL(), #OS_DEV_READ()
+ * and #OS_DEV_WRITE() routines.
+ */
+int os_dev_is_flag_write(void);
+
+/*!
+ * Check whether user is requesting non-blocking I/O.  Usable in
+ * #OS_DEV_OPEN(), #OS_DEV_CLOSE(), #OS_DEV_IOCTL(), #OS_DEV_READ() and
+ * #OS_DEV_WRITE() routines.
+ *
+ * @todo Specify required behavior when nonblock is requested and (sufficient?)
+ * data are not available to fulfill the request.
+ *
+ */
+int os_dev_is_flag_nonblock(void);
+
+/*!
+ * Determine which major device is being accessed.  Usable in #OS_DEV_OPEN()
+ * and #OS_DEV_CLOSE().
+ */
+int os_dev_get_major(void);
+
+/*!
+ * Determine which minor device is being accessed.  Usable in #OS_DEV_OPEN()
+ * and #OS_DEV_CLOSE().
+ */
+int os_dev_get_minor(void);
+
+/*!
+ * Determine which operation the user wants performed.  Usable in
+ * #OS_DEV_IOCTL().
+ *
+ * @return  Value of the operation.
+ *
+ * @todo Define some generic way to define the individual operations.
+ */
+unsigned os_dev_get_ioctl_op(void);
+
+/*!
+ * Retrieve the associated argument for the desired operation.  Usable in
+ * #OS_DEV_IOCTL().
+ *
+ * @return    A value which can be cast to a struct pointer or used as
+ *            int/long.
+ */
+os_dev_ioctl_arg_t os_dev_get_ioctl_arg(void);
+
+/*!
+ * Determine the requested byte count. This should be the size of buffer at
+ * #os_dev_get_user_buffer(). Usable in OS_DEV_READ() and OS_DEV_WRITE()
+ * routines.
+ *
+ * @return   A count of bytes
+ */
+unsigned os_dev_get_count(void);
+
+/*!
+ * Get the pointer to the user's data buffer. Usable in OS_DEV_READ(),
+ * OS_DEV_WRITE(), and OS_DEV_MMAP() routines.
+ *
+ * @return   Pointer to user buffer (in user space).  See #os_copy_to_user()
+ *           and #os_copy_from_user().
+ */
+void *os_dev_get_user_buffer(void);
+
+/*!
+ * Get the POSIX flags field for the associated open file.  Usable in
+ * OS_DEV_READ(), OS_DEV_WRITE(), and OS_DEV_IOCTL() routines.
+ *
+ * @return The flags associated with the file.
+ */
+unsigned os_dev_get_file_flags(void);
+
+/*!
+ * Set the driver's private structure associated with this file/open.
+ *
+ * Generally used during #OS_DEV_OPEN().  May also be used during
+ * #OS_DEV_READ(), #OS_DEV_WRITE(), #OS_DEV_IOCTL(), #OS_DEV_MMAP(), and
+ * #OS_DEV_CLOSE().  See also #os_dev_get_user_private().
+ *
+ * @param  struct_p   The driver data structure to associate with this user.
+ */
+void os_dev_set_user_private(void *struct_p);
+
+/*!
+ * Get the driver's private structure associated with this file.
+ *
+ * May be used during #OS_DEV_OPEN(), #OS_DEV_READ(), #OS_DEV_WRITE(),
+ * #OS_DEV_IOCTL(), #OS_DEV_MMAP(), and #OS_DEV_CLOSE().  See
+ * also #os_dev_set_user_private().
+ *
+ * @return   The driver data structure to associate with this user.
+ */
+void *os_dev_get_user_private(void);
+
+/*!
+ * Get the IRQ associated with this call to the #OS_DEV_ISR() function.
+ *
+ * @return  The IRQ (integer) interrupt number.
+ */
+int os_dev_get_irq(void);
+
+	  /*! @} *//* drsigargs */
+
+/*****************************************************************************
+ *  Functions for Generating References to Driver Routines
+ *****************************************************************************/
+
+/*!
+ * @defgroup drref Functions for Generating References to Driver Routines
+ *
+ * These functions will most likely be implemented as macros.  They are a
+ * necessary part of the portable API to guarantee portability.  The @c symbol
+ * type in here is the same symbol passed to the associated
+ * signature-generating macro.
+ *
+ * These macros must be used whenever referring to a
+ * @ref drsigs "driver signature function", for instance when storing or
+ * passing a pointer to the function.
+ */
+
+/*! @addtogroup drref */
+/*! @{ */
+
+/*!
+ * Generate a reference to an #OS_DEV_INIT() function
+ *
+ * @param function_name   The name of the init function being referenced.
+ *
+ * @return A reference to the function
+ */
+os_init_function_t OS_DEV_INIT_REF(symbol function_name);
+
+/*!
+ * Generate a reference to an #OS_DEV_SHUTDOWN() function
+ *
+ * @param function_name   The name of the shutdown function being referenced.
+ *
+ * @return A reference to the function
+ */
+os_shutdown_function_t OS_DEV_SHUTDOWN_REF(symbol function_name);
+
+/*!
+ * Generate a reference to an #OS_DEV_OPEN() function
+ *
+ * @param function_name   The name of the open function being referenced.
+ *
+ * @return A reference to the function
+ */
+os_user_function_t OS_DEV_OPEN_REF(symbol function_name);
+
+/*!
+ * Generate a reference to an #OS_DEV_CLOSE() function
+ *
+ * @param function_name   The name of the close function being referenced.
+ *
+ * @return A reference to the function
+ */
+os_user_function_t OS_DEV_CLOSE_REF(symbol function_name);
+
+/*!
+ * Generate a reference to an #OS_DEV_READ() function
+ *
+ * @param function_name   The name of the read function being referenced.
+ *
+ * @return A reference to the function
+ */
+os_user_function_t OS_DEV_READ_REF(symbol function_name);
+
+/*!
+ * Generate a reference to an #OS_DEV_WRITE() function
+ *
+ * @param function_name   The name of the write function being referenced.
+ *
+ * @return A reference to the function
+ */
+os_user_function_t OS_DEV_WRITE_REF(symbol function_name);
+
+/*!
+ * Generate a reference to an #OS_DEV_IOCTL() function
+ *
+ * @param function_name   The name of the ioctl function being referenced.
+ *
+ * @return A reference to the function
+ */
+os_user_function_t OS_DEV_IOCTL_REF(symbol function_name);
+
+/*!
+ * Generate a reference to an #OS_DEV_MMAP() function
+ *
+ * @param function_name   The name of the mmap function being referenced.
+ *
+ * @return A reference to the function
+ */
+os_user_function_t OS_DEV_MMAP_REF(symbol function_name);
+
+/*!
+ * Generate a reference to an #OS_DEV_ISR() function
+ *
+ * @param function_name   The name of the isr being referenced.
+ *
+ * @return a reference to the function
+ */
+os_interrupt_handler_t OS_DEV_ISR_REF(symbol function_name);
+
+	  /*! @} *//* drref */
+
+/*!
+ * Flush and invalidate all cache lines.
+ */
+void os_flush_cache_all(void);
+
+/*!
+ * Flush a range of addresses from the cache
+ *
+ * @param  start    Starting virtual address
+ * @param  len      Number of bytes to flush
+ */
+void os_cache_flush_range(void *start, uint32_t len);
+
+/*!
+ * Invalidate a range of addresses in the cache
+ *
+ * @param  start    Starting virtual address
+ * @param  len      Number of bytes to flush
+ */
+void os_cache_inv_range(void *start, uint32_t len);
+
+/*!
+ * Clean a range of addresses from the cache
+ *
+ * @param  start    Starting virtual address
+ * @param  len      Number of bytes to flush
+ */
+void os_cache_clean_range(void *start, uint32_t len);
+
+/*!
+  * @example widget.h
+  */
+
+/*!
+  * @example widget.c
+  */
+
+/*!
+  * @example rng_driver.h
+  */
+
+/*!
+  * @example rng_driver.c
+  */
+
+/*!
+  * @example shw_driver.h
+  */
+
+/*!
+  * @example shw_driver.c
+  */
+
+#endif				/* DOXYGEN_PORTABLE_OS_DOC */
+
+#endif				/* PORTABLE_OS_H */
diff --git a/drivers/mxc/security/sahara2/include/sah_driver_common.h b/drivers/mxc/security/sahara2/include/sah_driver_common.h
new file mode 100644
index 000000000000..7cfd32a30352
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/sah_driver_common.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/**
+* @file sah_driver_common.h
+*
+* @brief Provides types and defined values for use in the Driver Interface.
+*
+*/
+
+#ifndef SAH_DRIVER_COMMON_H
+#define SAH_DRIVER_COMMON_H
+
+#include "fsl_platform.h"
+#include <sahara.h>
+#include <adaptor.h>
+
+/** This specifies the permissions for the device file. It is equivalent to
+ * chmod 666.
+ */
+#define SAHARA_DEVICE_MODE S_IFCHR | S_IRUGO | S_IWUGO
+
+/**
+* The status of entries in the Queue.
+*
+******************************************************************************/
+typedef enum
+{
+    /** This state indicates that the entry is in the queue and awaits
+     *  execution on SAHARA. */
+    SAH_STATE_PENDING,
+    /** This state indicates that the entry has been written to the SAHARA
+     *  DAR. */
+    SAH_STATE_ON_SAHARA,
+    /** This state indicates that the entry is off of SAHARA, and is awaiting
+        post-processing. */
+    SAH_STATE_OFF_SAHARA,
+    /** This state indicates that the entry is successfully executed on SAHARA,
+        and it is finished with post-processing. */
+    SAH_STATE_COMPLETE,
+    /** This state indicates that the entry caused an error or fault on SAHARA,
+     *  and it is finished with post-processing. */
+    SAH_STATE_FAILED,
+    /** This state indicates that the entry was reset via the Reset IO
+     *  Control, and it is finished with post-processing. */
+    SAH_STATE_RESET,
+    /** This state indicates that the entry was signalled from user-space and
+     *  either in the DAR, IDAR or has finished executing pending Bottom Half
+     *  processing. */
+   SAH_STATE_IGNORE,
+    /** This state indicates that the entry was signalled from user-space and
+     *  has been processed by the bottom half. */
+   SAH_STATE_IGNORED
+} sah_Queue_Status;
+
+/* any of these conditions being true indicates the descriptor's processing
+ * is complete */
+#define SAH_DESC_PROCESSED(status)                             \
+                        (((status) == SAH_STATE_COMPLETE) ||   \
+                         ((status) == SAH_STATE_FAILED  ) ||   \
+                         ((status) == SAH_STATE_RESET   ))
+
+extern os_lock_t desc_queue_lock;
+
+extern uint32_t dar_count;
+extern uint32_t interrupt_count;
+extern uint32_t done1done2_count;
+extern uint32_t done1busy2_count;
+extern uint32_t done1_count;
+
+#ifdef FSL_HAVE_SCC2
+extern void *lookup_user_partition(fsl_shw_uco_t * user_ctx,
+				   uint32_t user_base);
+#endif
+
+int sah_get_results_pointers(fsl_shw_uco_t* user_ctx, uint32_t arg);
+fsl_shw_return_t sah_get_results_from_pool(volatile fsl_shw_uco_t* user_ctx,
+                                           sah_results *arg);
+fsl_shw_return_t sah_handle_registration(fsl_shw_uco_t *user_cts);
+fsl_shw_return_t sah_handle_deregistration(fsl_shw_uco_t *user_cts);
+
+int sah_Queue_Manager_Count_Entries(int ignore_state, sah_Queue_Status state);
+unsigned long sah_Handle_Poll(sah_Head_Desc *entry);
+
+#ifdef DIAG_DRV_IF
+/******************************************************************************
+* Descriptor and Link dumping functions.
+******************************************************************************/
+void sah_Dump_Chain(const sah_Desc *chain, dma_addr_t addr);
+#endif /* DIAG_DRV_IF */
+
+#endif  /* SAH_DRIVER_COMMON_H */
diff --git a/drivers/mxc/security/sahara2/include/sah_hardware_interface.h b/drivers/mxc/security/sahara2/include/sah_hardware_interface.h
new file mode 100644
index 000000000000..0933346fc223
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/sah_hardware_interface.h
@@ -0,0 +1,99 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/**
+* @file sah_hardware_interface.h
+*
+* @brief Provides an interface to the SAHARA hardware registers.
+*
+*/
+
+#ifndef SAH_HARDWARE_INTERFACE_H
+#define SAH_HARDWARE_INTERFACE_H
+
+#include <sah_driver_common.h>
+#include <sah_status_manager.h>
+
+/* These values can be used with sah_HW_Write_Control(). */
+#ifdef SAHARA1
+/** Define platform as Little-Endian */
+#define CTRL_LITTLE_END     0x00000002
+/** Bit to cause endian change in transfers */
+#define CTRL_INT_EN         0x00000004
+/** Set High Assurance mode */
+#define CTRL_HA             0x00000008
+#else
+/** Bit to cause byte swapping in (data?) transfers */
+#define CTRL_BYTE_SWAP      0x00000001
+/** Bit to cause halfword swapping in (data?) transfers */
+#define CTRL_HALFWORD_SWAP  0x00000002
+/** Bit to cause endian change in (data?) transfers */
+#define CTRL_INT_EN         0x00000010
+/** Set High Assurance mode */
+#define CTRL_HA             0x00000020
+/** Disable High Assurance */
+#define CTRL_HA_DISABLE     0x00000040
+/** Reseed the RNG CHA */
+#define CTRL_RNG_RESEED     0x00000080
+#endif
+
+
+/* These values can be used with sah_HW_Write_Command(). */
+/** Reset the Sahara */
+#define CMD_RESET           0x00000001
+/** Set Sahara into Batch mode. */
+#define CMD_BATCH           0x00010000
+/** Clear the Sahara interrupt */
+#define CMD_CLR_INT_BIT     0x00000100
+/** Clear the Sahara error */
+#define CMD_CLR_ERROR_BIT   0x00000200
+
+
+/** error status register contains error */
+#define STATUS_ERROR 0x00000010
+
+/** Op status register contains op status */
+#define OP_STATUS 0x00000020
+
+
+/* High Level functions */
+int                sah_HW_Reset(void);
+fsl_shw_return_t   sah_HW_Set_HA(void);
+sah_Execute_Status sah_Wait_On_Sahara(void);
+
+/* Low Level functions */
+uint32_t    sah_HW_Read_Version(void);
+uint32_t    sah_HW_Read_Control(void);
+uint32_t    sah_HW_Read_Status(void);
+uint32_t    sah_HW_Read_Error_Status(void);
+uint32_t    sah_HW_Read_Op_Status(void);
+uint32_t    sah_HW_Read_DAR(void);
+uint32_t    sah_HW_Read_CDAR(void);
+uint32_t    sah_HW_Read_IDAR(void);
+uint32_t    sah_HW_Read_Fault_Address(void);
+uint32_t    sah_HW_Read_MM_Status(void);
+uint32_t    sah_HW_Read_Config(void);
+void        sah_HW_Write_Command(uint32_t command);
+void        sah_HW_Write_Control(uint32_t control);
+void        sah_HW_Write_DAR(uint32_t pointer);
+void        sah_HW_Write_Config(uint32_t configuration);
+
+#if defined DIAG_DRV_IF || defined(DO_DBG)
+
+void sah_Dump_Words(const char *prefix, const unsigned *data, dma_addr_t addr,
+		    unsigned length);
+#endif
+
+#endif  /* SAH_HARDWARE_INTERFACE_H */
+
+/* End of sah_hardware_interface.c */
diff --git a/drivers/mxc/security/sahara2/include/sah_interrupt_handler.h b/drivers/mxc/security/sahara2/include/sah_interrupt_handler.h
new file mode 100644
index 000000000000..8eb8690ff093
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/sah_interrupt_handler.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/**
+* @file sah_interrupt_handler.h
+*
+* @brief Provides a hardware interrupt handling mechanism for device driver.
+*
+*/
+/******************************************************************************
+*
+* CAUTION:
+*
+* MODIFICATION HISTORY:
+*
+* Date      Person     Change
+* 30/07/03  MW         Initial Creation
+*******************************************************************
+*/
+
+#ifndef SAH_INTERRUPT_HANDLER_H
+#define SAH_INTERRUPT_HANDLER_H
+
+#include <sah_driver_common.h>
+
+/******************************************************************************
+* External function declarations
+******************************************************************************/
+int sah_Intr_Init (wait_queue_head_t *wait_queue);
+void sah_Intr_Release (void);
+
+#endif  /* SAH_INTERRUPT_HANDLER_H */
diff --git a/drivers/mxc/security/sahara2/include/sah_kernel.h b/drivers/mxc/security/sahara2/include/sah_kernel.h
new file mode 100644
index 000000000000..42013272f610
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/sah_kernel.h
@@ -0,0 +1,113 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*
+* @file sah_kernel.h
+*
+* @brief Provides definitions for items that user-space and kernel-space share.
+*/
+/******************************************************************************
+*
+* This file needs to be PORTED to a non-Linux platform
+*/
+
+#ifndef SAH_KERNEL_H
+#define SAH_KERNEL_H
+
+#if defined(__KERNEL__)
+
+#if defined(CONFIG_ARCH_MXC91321) || defined(CONFIG_ARCH_MXC91231)     \
+    || defined(CONFIG_ARCH_MX27)  || defined(CONFIG_ARCH_MXC92323)
+#include <mach/hardware.h>
+#define SAHA_BASE_ADDR SAHARA_BASE_ADDR
+#define SAHARA_IRQ     MXC_INT_SAHARA
+#elif defined(CONFIG_ARCH_MX51)
+#include <mach/hardware.h>
+#define SAHA_BASE_ADDR SAHARA_BASE_ADDR
+#define SAHARA_IRQ     MXC_INT_SAHARA_H0
+#else
+#include <mach/mx2.h>
+#endif
+
+#endif				/* KERNEL */
+
+/* IO Controls */
+/* The magic number 'k' is reserved for the SPARC architecture. (See <kernel
+ * source root>/Documentation/ioctl-number.txt.
+ *
+ * Note: Numbers 8-13 were used in a previous version of the API and should
+ *       be avoided.
+ */
+#define SAH_IOC_MAGIC        'k'
+#define SAHARA_HWRESET       _IO(SAH_IOC_MAGIC, 0)
+#define SAHARA_SET_HA        _IO(SAH_IOC_MAGIC, 1)
+#define SAHARA_CHK_TEST_MODE _IOR(SAH_IOC_MAGIC,2, int)
+#define SAHARA_DAR           _IO(SAH_IOC_MAGIC, 3)
+#define SAHARA_GET_RESULTS   _IO(SAH_IOC_MAGIC, 4)
+#define SAHARA_REGISTER      _IO(SAH_IOC_MAGIC, 5)
+#define SAHARA_DEREGISTER    _IO(SAH_IOC_MAGIC, 6)
+/*                                              7 */
+/*                                              8 */
+/*                                              9 */
+/*                                             10 */
+/*                                             11 */
+/*                                             12 */
+/*                                             13 */
+
+#define SAHARA_SCC_DROP_PERMS _IOWR(SAH_IOC_MAGIC, 14, scc_partition_info_t)
+#define SAHARA_SCC_SFREE     _IOWR(SAH_IOC_MAGIC, 15, scc_partition_info_t)
+
+#define SAHARA_SK_ALLOC     _IOWR(SAH_IOC_MAGIC, 16, scc_slot_t)
+#define SAHARA_SK_DEALLOC   _IOWR(SAH_IOC_MAGIC, 17, scc_slot_t)
+#define SAHARA_SK_LOAD      _IOWR(SAH_IOC_MAGIC, 18, scc_slot_t)
+#define SAHARA_SK_UNLOAD    _IOWR(SAH_IOC_MAGIC, 19, scc_slot_t)
+#define SAHARA_SK_SLOT_ENC  _IOWR(SAH_IOC_MAGIC, 20, scc_slot_t)
+#define SAHARA_SK_SLOT_DEC  _IOWR(SAH_IOC_MAGIC, 21, scc_slot_t)
+
+#define SAHARA_SCC_ENCRYPT  _IOWR(SAH_IOC_MAGIC, 22, scc_region_t)
+#define SAHARA_SCC_DECRYPT  _IOWR(SAH_IOC_MAGIC, 23, scc_region_t)
+#define SAHARA_GET_CAPS     _IOWR(SAH_IOC_MAGIC, 24, fsl_shw_pco_t)
+
+#define SAHARA_SCC_SSTATUS  _IOWR(SAH_IOC_MAGIC, 25, scc_partition_info_t)
+
+#define SAHARA_SK_READ      _IOWR(SAH_IOC_MAGIC, 29, scc_slot_t)
+
+/*! This is the name that will appear in /proc/interrupts */
+#define SAHARA_NAME         "SAHARA"
+
+/*!
+ * SAHARA IRQ number. See page 9-239 of TLICS - Motorola Semiconductors H.K.
+ * TAHITI-Lite IC Specification, Rev 1.1, Feb 2003.
+ *
+ * TAHITI has two blocks of 32 interrupts. The SAHARA IRQ is number 27
+ * in the second block. This means that the SAHARA IRQ is 27 + 32 = 59.
+ */
+#ifndef SAHARA_IRQ
+#define SAHARA_IRQ          (27+32)
+#endif
+
+/*!
+ * Device file definition. The #ifndef is here to support the unit test code
+ * by allowing it to specify a different test device.
+ */
+#ifndef SAHARA_DEVICE_SHORT
+#define SAHARA_DEVICE_SHORT "sahara"
+#endif
+
+#ifndef SAHARA_DEVICE
+#define SAHARA_DEVICE       "/dev/"SAHARA_DEVICE_SHORT
+#endif
+
+#endif				/* SAH_KERNEL_H */
+
+/* End of sah_kernel.h */
diff --git a/drivers/mxc/security/sahara2/include/sah_memory_mapper.h b/drivers/mxc/security/sahara2/include/sah_memory_mapper.h
new file mode 100644
index 000000000000..838ce47cbf85
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/sah_memory_mapper.h
@@ -0,0 +1,79 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/**
+* @file sah_memory_mapper.h
+*
+* @brief Re-creates SAHARA data structures in Kernel memory such that they are
+*       suitable for DMA.
+*
+*/
+
+#ifndef SAH_MEMORY_MAPPER_H
+#define SAH_MEMORY_MAPPER_H
+
+#include <sah_driver_common.h>
+#include <sah_queue_manager.h>
+
+
+/******************************************************************************
+* External function declarations
+******************************************************************************/
+sah_Head_Desc *sah_Copy_Descriptors(fsl_shw_uco_t * user_ctx,
+					sah_Head_Desc * desc);
+
+sah_Link *sah_Copy_Links(fsl_shw_uco_t * user_ctx, sah_Link * ptr);
+
+sah_Head_Desc *sah_Physicalise_Descriptors(sah_Head_Desc * desc);
+
+sah_Link *sah_Physicalise_Links (sah_Link *ptr);
+
+sah_Head_Desc *sah_DePhysicalise_Descriptors (sah_Head_Desc *desc);
+
+sah_Link *sah_DePhysicalise_Links (sah_Link *ptr);
+
+sah_Link *sah_Make_Links(fsl_shw_uco_t * user_ctx,
+			 sah_Link * ptr, sah_Link ** tail);
+
+
+void sah_Destroy_Descriptors (sah_Head_Desc *desc);
+
+void sah_Destroy_Links (sah_Link *link);
+
+void sah_Free_Chained_Descriptors (sah_Head_Desc *desc);
+
+void sah_Free_Chained_Links (sah_Link *link);
+
+int sah_Init_Mem_Map (void);
+
+void sah_Stop_Mem_Map (void);
+
+int sah_Block_Add_Page (int big);
+
+sah_Desc *sah_Alloc_Descriptor (void);
+sah_Head_Desc *sah_Alloc_Head_Descriptor (void);
+void sah_Free_Descriptor (sah_Desc *desc);
+void sah_Free_Head_Descriptor (sah_Head_Desc *desc);
+sah_Link *sah_Alloc_Link (void);
+void sah_Free_Link (sah_Link *link);
+
+void *wire_user_memory(void *address, uint32_t length, void **page_ctx);
+void unwire_user_memory(void **page_ctx);
+
+os_error_code map_user_memory(struct vm_area_struct *vma,
+			      uint32_t physical_addr, uint32_t size);
+os_error_code unmap_user_memory(uint32_t user_addr, uint32_t size);
+
+#endif  /* SAH_MEMORY_MAPPER_H */
+
+/* End of sah_memory_mapper.h */
diff --git a/drivers/mxc/security/sahara2/include/sah_queue_manager.h b/drivers/mxc/security/sahara2/include/sah_queue_manager.h
new file mode 100644
index 000000000000..2dde5883e193
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/sah_queue_manager.h
@@ -0,0 +1,63 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/**
+* @file sah_queue_manager.h
+*
+* @brief This file definitions for the Queue Manager.
+
+* The Queue Manager manages additions and removal from the queue and updates
+* the status of queue entries. It also calls sah_HW_* functions to interact
+* with the hardware.
+*
+*/
+
+#ifndef SAH_QUEUE_MANAGER_H
+#define SAH_QUEUE_MANAGER_H
+
+#include <sah_driver_common.h>
+#include <sah_status_manager.h>
+
+
+/*************************
+* Queue Manager Functions
+*************************/
+fsl_shw_return_t sah_Queue_Manager_Init(void);
+void             sah_Queue_Manager_Close(void);
+void             sah_Queue_Manager_Reset_Entries(void);
+void             sah_Queue_Manager_Append_Entry(sah_Head_Desc *entry);
+void             sah_Queue_Manager_Remove_Entry(sah_Head_Desc *entry);
+
+
+/*************************
+* Queue Functions
+*************************/
+sah_Queue *sah_Queue_Construct(void);
+void      sah_Queue_Destroy(sah_Queue *this);
+void      sah_Queue_Append_Entry(sah_Queue *this, sah_Head_Desc *entry);
+void      sah_Queue_Remove_Entry(sah_Queue *this);
+void      sah_Queue_Remove_Any_Entry(sah_Queue *this, sah_Head_Desc *entry);
+void      sah_postprocess_queue(unsigned long reset_flag);
+
+
+/*************************
+* Misc Releated Functions
+*************************/
+
+int              sah_blocking_mode(struct sah_Head_Desc *entry);
+fsl_shw_return_t sah_convert_error_status(uint32_t error_status);
+
+
+#endif  /* SAH_QUEUE_MANAGER_H */
+
+/* End of sah_queue_manager.h */
diff --git a/drivers/mxc/security/sahara2/include/sah_status_manager.h b/drivers/mxc/security/sahara2/include/sah_status_manager.h
new file mode 100644
index 000000000000..e38731bf4835
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/sah_status_manager.h
@@ -0,0 +1,228 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/**
+* @file sah_status_manager.h
+*
+* @brief SAHARA Status Manager Types and Function Prototypes
+*
+* @author Stuart Holloway (SH)
+*
+*/
+
+#ifndef STATUS_MANAGER_H
+#define STATUS_MANAGER_H
+#include "sah_driver_common.h"
+#include "sahara.h"
+
+
+/******************************************************************************
+* User defined data types
+******************************************************************************/
+/**
+******************************************************************************
+* sah_Execute_Status
+* Types read from SAHARA Status Register, with additional state for Op Status
+******************************************************************************/
+typedef enum sah_Execute_Status
+{
+    /** Sahara is Idle. */
+    SAH_EXEC_IDLE = 0,
+    /** SAHARA is busy performing a resest or processing a decriptor chain. */
+    SAH_EXEC_BUSY = 1,
+    /** An error occurred while SAHARA executed the first descriptor. */
+    SAH_EXEC_ERROR1 = 2,
+    /** SAHARA has failed internally. */
+    SAH_EXEC_FAULT = 3,
+    /** SAHARA has finished processing a descriptor chain and is idle. */
+    SAH_EXEC_DONE1 = 4,
+    /** SAHARA has finished processing a descriptor chain, and is processing a
+     *   second chain.  */
+    SAH_EXEC_DONE1_BUSY2 = 5,
+    /** SAHARA has finished processing a descriptor chain, but has generated an
+     *   error while processing a second descriptor chain. */
+    SAH_EXEC_DONE1_ERROR2 = 6,
+    /** SAHARA has finished two descriptors. */
+    SAH_EXEC_DONE1_DONE2 = 7,
+    /** SAHARA has stopped, and first descriptor has Op Status, not Err  */
+    SAH_EXEC_OPSTAT1 = 0x20,
+} sah_Execute_Status;
+
+/**
+ * When this bit is on in a #sah_Execute_Status, it means that DONE1 is true.
+ */
+#define SAH_EXEC_DONE1_BIT  4
+
+/**
+ * Bits which make up the Sahara State
+ */
+#define SAH_EXEC_STATE_MASK 0x00000007
+
+/**
+*******************************************************************************
+* sah_Execute_Error
+* Types read from SAHARA Error Status Register
+******************************************************************************/
+typedef enum sah_Execute_Error
+{
+    /** No Error */
+    SAH_ERR_NONE = 0,
+    /** Header is not valid. */
+    SAH_ERR_HEADER = 1,
+    /** Descriptor length is not correct. */
+    SAH_ERR_DESC_LENGTH = 2,
+    /** Length or pointer field is zero while the other is non-zero.  */
+    SAH_ERR_DESC_POINTER = 3,
+    /** Length of the link is not a multiple of 4 and is not the last link */
+    SAH_ERR_LINK_LENGTH = 4,
+    /** The data pointer in a link is zero */
+    SAH_ERR_LINK_POINTER = 5,
+    /** Input Buffer reported an overflow */
+    SAH_ERR_INPUT_BUFFER = 6,
+    /** Output Buffer reported an underflow */
+    SAH_ERR_OUTPUT_BUFFER = 7,
+    /** Incorrect data in output buffer after CHA's has signalled 'done'. */
+    SAH_ERR_OUTPUT_BUFFER_STARVATION = 8,
+    /** Internal Hardware Failure. */
+    SAH_ERR_INTERNAL_STATE = 9,
+    /** Current Descriptor was not legal, but cause is unknown. */
+    SAH_ERR_GENERAL_DESCRIPTOR = 10,
+    /** Reserved pointer fields have been set to 1. */
+    SAH_ERR_RESERVED_FIELDS = 11,
+    /** Descriptor address error */
+    SAH_ERR_DESCRIPTOR_ADDRESS = 12,
+    /** Link address error */
+    SAH_ERR_LINK_ADDRESS = 13,
+    /** Processing error in CHA module */
+    SAH_ERR_CHA = 14,
+    /** Processing error during DMA */
+    SAH_ERR_DMA = 15
+} sah_Execute_Error;
+
+
+/**
+*******************************************************************************
+* sah_CHA_Error_Source
+* Types read from SAHARA Error Status Register for CHA Error Source
+*
+******************************************************************************/
+typedef enum sah_CHA_Error_Source
+{
+    /** No Error indicated in Source CHA Error. */
+    SAH_CHA_NO_ERROR = 0,
+    /** Error in SKHA module. */
+    SAH_CHA_SKHA_ERROR = 1,
+    /** Error in MDHA module. */
+    SAH_CHA_MDHA_ERROR = 2,
+    /** Error in RNG module.  */
+    SAH_CHA_RNG_ERROR = 3,
+    /** Error in PKHA module.  */
+    SAH_CHA_PKHA_ERROR = 4,
+} sah_CHA_Error_Source;
+
+/**
+******************************************************************************
+* sah_CHA_Error_Status
+* Types read from SAHARA Error Status Register for CHA Error Status
+*
+******************************************************************************/
+typedef enum sah_CHA_Error_Status
+{
+    /** No CHA error detected */
+    SAH_CHA_NO_ERR = 0x000,
+    /** Non-empty input buffer when complete. */
+    SAH_CHA_IP_BUF = 0x001,
+    /** Illegal Address Error. */
+    SAH_CHA_ADD_ERR = 0x002,
+    /** Illegal Mode Error.  */
+    SAH_CHA_MODE_ERR = 0x004,
+    /** Illegal Data Size Error. */
+    SAH_CHA_DATA_SIZE_ERR = 0x008,
+    /** Illegal Key Size Error. */
+    SAH_CHA_KEY_SIZE_ERR = 0x010,
+    /** Mode/Context/Key written during processing. */
+    SAH_CHA_PROC_ERR = 0x020,
+    /** Context Read During Processing. */
+    SAH_CHA_CTX_READ_ERR = 0x040,
+    /** Internal Hardware Error. */
+    SAH_CHA_INTERNAL_HW_ERR = 0x080,
+    /** Input Buffer not enabled or underflow. */
+    SAH_CHA_IP_BUFF_ERR = 0x100,
+    /** Output Buffer not enabled or overflow. */
+    SAH_CHA_OP_BUFF_ERR = 0x200,
+    /** DES key parity error (SKHA) */
+    SAH_CHA_DES_KEY_ERR = 0x400,
+    /** Reserved error code. */
+    SAH_CHA_RES = 0x800
+} sah_CHA_Error_Status;
+
+/**
+*****************************************************************************
+* sah_DMA_Error_Status
+* Types read from SAHARA Error Status Register for DMA Error Status
+******************************************************************************/
+typedef enum sah_DMA_Error_Status
+{
+    /** No DMA Error Code. */
+    SAH_DMA_NO_ERR = 0,
+    /** AHB terminated a bus cycle with an error. */
+    SAH_DMA_AHB_ERR = 2,
+    /** Internal IP bus cycle was terminated with an error termination. */
+    SAH_DMA_IP_ERR = 4,
+    /** Parity error detected on DMA command. */
+    SAH_DMA_PARITY_ERR = 6,
+    /** DMA was requested to cross a 256 byte internal address boundary. */
+    SAH_DMA_BOUNDRY_ERR = 8,
+    /** DMA controller is busy */
+    SAH_DMA_BUSY_ERR = 10,
+    /** Memory Bounds Error */
+    SAH_DMA_RESERVED_ERR = 12,
+    /** Internal DMA hardware error detected */
+    SAH_DMA_INT_ERR = 14
+} sah_DMA_Error_Status;
+
+/**
+*****************************************************************************
+* sah_DMA_Error_Size
+* Types read from SAHARA Error Status Register for DMA Error Size
+*
+******************************************************************************/
+typedef enum sah_DMA_Error_Size
+{
+    /** Error during Byte transfer.  */
+    SAH_DMA_SIZE_BYTE = 0,
+    /** Error during Half-word transfer. */
+    SAH_DMA_SIZE_HALF_WORD = 1,
+    /** Error during Word transfer. */
+    SAH_DMA_SIZE_WORD = 2,
+    /** Reserved DMA word size. */
+    SAH_DMA_SIZE_RES = 3
+} sah_DMA_Error_Size;
+
+
+
+
+extern bool sah_dpm_flag;
+
+/*************************
+* Status Manager Functions
+*************************/
+
+unsigned long sah_Handle_Interrupt(sah_Execute_Status hw_status);
+sah_Head_Desc *sah_Find_With_State (sah_Queue_Status status);
+int sah_dpm_init(void);
+void sah_dpm_close(void);
+void sah_Queue_Manager_Prime (sah_Head_Desc *entry);
+
+
+#endif  /* STATUS_MANAGER_H */
diff --git a/drivers/mxc/security/sahara2/include/sahara.h b/drivers/mxc/security/sahara2/include/sahara.h
new file mode 100644
index 000000000000..da2352780efd
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/sahara.h
@@ -0,0 +1,2266 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file sahara.h
+ *
+ * File which implements the FSL_SHW API when used on Sahara
+ */
+/*!
+ * @if USE_MAINPAGE
+ * @mainpage Sahara2 implemtation of FSL Security Hardware API
+ * @endif
+ *
+ */
+
+#define _DIAG_DRV_IF
+#define _DIAG_SECURITY_FUNC
+#define _DIAG_ADAPTOR
+
+#ifndef SAHARA2_API_H
+#define SAHARA2_API_H
+
+#ifdef DIAG_SECURITY_FUNC
+#include <diagnostic.h>
+#endif				/* DIAG_SECURITY_FUNC */
+
+/* This is a Linux flag... ? */
+#ifndef __KERNEL__
+#include <inttypes.h>
+#include <stdlib.h>
+#include <memory.h>
+#else
+#include "portable_os.h"
+#endif
+
+/* This definition may need a new name, and needs to go somewhere which
+ * can determine platform, kernel vs. user, os, etc.
+ */
+#define copy_bytes(out, in, len) memcpy(out, in, len)
+
+/* Does this belong here? */
+#ifndef SAHARA_DEVICE
+#define SAHARA_DEVICE "/dev/sahara"
+#endif
+
+/*!
+*******************************************************************************
+* @defgroup lnkflags Link Flags
+*
+* @brief Flags to show information about link data and link segments
+*
+******************************************************************************/
+/*! @addtogroup lnkflags
+ * @{
+ */
+
+/*!
+*******************************************************************************
+* This flag indicates that the data in a link is owned by the security
+* function component and this memory will be freed by the security function
+* component. To be used as part of the flag field of the sah_Link structure.
+******************************************************************************/
+#define SAH_OWNS_LINK_DATA          0x01
+
+/*!
+*******************************************************************************
+* The data in a link is not owned by the security function component and
+* therefore it will not attempt to free this memory. To be used as part of the
+* flag field of the sah_Link structure.
+******************************************************************************/
+#define SAH_USES_LINK_DATA          0x02
+
+/*!
+*******************************************************************************
+* The data in this link will change when the descriptor gets executed.
+******************************************************************************/
+#define SAH_OUTPUT_LINK             0x04
+
+/*!
+*******************************************************************************
+* The ptr and length in this link are really 'established key' info.  They
+* are to be converted to ptr/length before putting on request queue.
+******************************************************************************/
+#define SAH_KEY_IS_HIDDEN           0x08
+
+/*!
+*******************************************************************************
+* The link structure has been appended to the previous one by the driver.  It
+* needs to be removed before leaving the driver (and returning to API).
+******************************************************************************/
+#define SAH_REWORKED_LINK           0x10
+
+/*!
+*******************************************************************************
+* The length and data fields of this link contain the slot and user id
+* used to access the SCC stored key
+******************************************************************************/
+#define SAH_STORED_KEY_INFO         0x20
+
+/*!
+*******************************************************************************
+* The Data field points to a physical address, and does not need to be
+* processed by the driver.  Honored only in Kernel API.
+******************************************************************************/
+#define SAH_PREPHYS_DATA            0x40
+
+/*!
+*******************************************************************************
+* The link was inserted during the Physicalise procedure.  It is tagged so
+* it can be removed during DePhysicalise, thereby returning to the caller an
+* intact chain.
+******************************************************************************/
+#define SAH_LINK_INSERTED_LINK      0x80
+
+/*!
+*******************************************************************************
+* The Data field points to the location of the key, which is in a secure
+* partition held by the user.  The memory address needs to be converted to
+* kernel space manually, by looking through the partitions that the user holds.
+******************************************************************************/
+#define SAH_IN_USER_KEYSTORE       0x100
+
+/*!
+*******************************************************************************
+* sah_Link_Flags
+*
+* Type to be used for flags associated with a Link in security function.
+* These flags are used internally by the security function component only.
+*
+* Values defined at @ref lnkflags
+*
+* @brief typedef for flags field of sah_Link
+******************************************************************************/
+typedef uint32_t sah_Link_Flags;
+
+/*
+*******************************************************************************
+* Security Parameters Related Structures
+*
+* All of structures associated with API parameters
+*
+******************************************************************************/
+
+/*
+*******************************************************************************
+* Common Types
+*
+* All of structures used across several classes of crytography
+******************************************************************************/
+
+/*!
+*******************************************************************************
+* @brief Indefinite precision integer used for security operations on SAHARA
+* accelerator. The data will always be in little Endian format.
+******************************************************************************/
+typedef uint8_t *sah_Int;
+
+/*!
+*******************************************************************************
+* @brief Byte array used for block cipher and hash digest/MAC operations on
+* SAHARA accelerator. The Endian format will be as specified by the function
+* using the sah_Oct_Str.
+******************************************************************************/
+typedef uint8_t *sah_Oct_Str;
+
+/*!
+ * A queue of descriptor heads -- used to hold requests waiting for user to
+ * pick up the results. */
+typedef struct sah_Queue {
+	int count;		/*!< # entries in queue  */
+	struct sah_Head_Desc *head;	/*!< first entry in queue  */
+	struct sah_Head_Desc *tail;	/*!< last entry in queue   */
+} sah_Queue;
+
+/******************************************************************************
+ * Enumerations
+ *****************************************************************************/
+/*!
+ * Flags for the state of the User Context Object (#fsl_shw_uco_t).
+ */
+typedef enum fsl_shw_user_ctx_flags_t {
+	/*!
+	 * API will block the caller until operation completes.  The result will be
+	 * available in the return code.  If this is not set, user will have to get
+	 * results using #fsl_shw_get_results().
+	 */
+	FSL_UCO_BLOCKING_MODE = 0x01,
+	/*!
+	 * User wants callback (at the function specified with
+	 * #fsl_shw_uco_set_callback()) when the operation completes.  This flag is
+	 * valid only if #FSL_UCO_BLOCKING_MODE is not set.
+	 */
+	FSL_UCO_CALLBACK_MODE = 0x02,
+	/*! Do not free descriptor chain after driver (adaptor) finishes */
+	FSL_UCO_SAVE_DESC_CHAIN = 0x04,
+	/*!
+	 * User has made at least one request with callbacks requested, so API is
+	 * ready to handle others.
+	 */
+	FSL_UCO_CALLBACK_SETUP_COMPLETE = 0x08,
+	/*!
+	 * (virtual) pointer to descriptor chain is completely linked with physical
+	 * (DMA) addresses, ready for the hardware.  This flag should not be used
+	 * by FSL SHW API programs.
+	 */
+	FSL_UCO_CHAIN_PREPHYSICALIZED = 0x10,
+	/*!
+	 * The user has changed the context but the changes have not been copied to
+	 * the kernel driver.
+	 */
+	FSL_UCO_CONTEXT_CHANGED = 0x20,
+	/*! Internal Use.  This context belongs to a user-mode API user. */
+	FSL_UCO_USERMODE_USER = 0x40,
+} fsl_shw_user_ctx_flags_t;
+
+/*!
+ * Return code for FSL_SHW library.
+ *
+ * These codes may be returned from a function call.  In non-blocking mode,
+ * they will appear as the status in a Result Object.
+ */
+typedef enum fsl_shw_return_t {
+	/*!
+	 * No error.  As a function return code in Non-blocking mode, this may
+	 * simply mean that the operation was accepted for eventual execution.
+	 */
+	FSL_RETURN_OK_S = 0,
+	/*! Failure for non-specific reason. */
+	FSL_RETURN_ERROR_S,
+	/*!
+	 * Operation failed because some resource was not able to be allocated.
+	 */
+	FSL_RETURN_NO_RESOURCE_S,
+	/*! Crypto algorithm unrecognized or improper. */
+	FSL_RETURN_BAD_ALGORITHM_S,
+	/*! Crypto mode unrecognized or improper. */
+	FSL_RETURN_BAD_MODE_S,
+	/*! Flag setting unrecognized or inconsistent. */
+	FSL_RETURN_BAD_FLAG_S,
+	/*! Improper or unsupported key length for algorithm. */
+	FSL_RETURN_BAD_KEY_LENGTH_S,
+	/*! Improper parity in a (DES, TDES) key. */
+	FSL_RETURN_BAD_KEY_PARITY_S,
+	/*!
+	 * Improper or unsupported data length for algorithm or internal buffer.
+	 */
+	FSL_RETURN_BAD_DATA_LENGTH_S,
+	/*! Authentication / Integrity Check code check failed. */
+	FSL_RETURN_AUTH_FAILED_S,
+	/*! A memory error occurred. */
+	FSL_RETURN_MEMORY_ERROR_S,
+	/*! An error internal to the hardware occurred. */
+	FSL_RETURN_INTERNAL_ERROR_S,
+	/*! ECC detected Point at Infinity */
+	FSL_RETURN_POINT_AT_INFINITY_S,
+	/*! ECC detected No Point at Infinity */
+	FSL_RETURN_POINT_NOT_AT_INFINITY_S,
+	/*! GCD is One */
+	FSL_RETURN_GCD_IS_ONE_S,
+	/*! GCD is not One */
+	FSL_RETURN_GCD_IS_NOT_ONE_S,
+	/*! Candidate is Prime */
+	FSL_RETURN_PRIME_S,
+	/*! Candidate is not Prime */
+	FSL_RETURN_NOT_PRIME_S,
+	/*! N register loaded improperly with even value */
+	FSL_RETURN_EVEN_MODULUS_ERROR_S,
+	/*! Divisor is zero. */
+	FSL_RETURN_DIVIDE_BY_ZERO_ERROR_S,
+	/*! Bad Exponent or Scalar value for Point Multiply */
+	FSL_RETURN_BAD_EXPONENT_ERROR_S,
+	/*! RNG hardware problem. */
+	FSL_RETURN_OSCILLATOR_ERROR_S,
+	/*! RNG hardware problem. */
+	FSL_RETURN_STATISTICS_ERROR_S,
+} fsl_shw_return_t;
+
+/*!
+ * Algorithm Identifier.
+ *
+ * Selection of algorithm will determine how large the block size of the
+ * algorithm is.   Context size is the same length unless otherwise specified.
+ * Selection of algorithm also affects the allowable key length.
+ */
+typedef enum fsl_shw_key_alg_t {
+	/*!
+	 * Key will be used to perform an HMAC.  Key size is 1 to 64 octets.  Block
+	 * size is 64 octets.
+	 */
+	FSL_KEY_ALG_HMAC,
+	/*!
+	 * Advanced Encryption Standard (Rijndael).  Block size is 16 octets.  Key
+	 * size is 16 octets.  (The single choice of key size is a Sahara platform
+	 * limitation.)
+	 */
+	FSL_KEY_ALG_AES,
+	/*!
+	 * Data Encryption Standard.  Block size is 8 octets.  Key size is 8
+	 * octets.
+	 */
+	FSL_KEY_ALG_DES,
+	/*!
+	 * 2- or 3-key Triple DES.  Block size is 8 octets.  Key size is 16 octets
+	 * for 2-key Triple DES, and 24 octets for 3-key.
+	 */
+	FSL_KEY_ALG_TDES,
+	/*!
+	 * ARC4.  No block size.  Context size is 259 octets.  Allowed key size is
+	 * 1-16 octets.  (The choices for key size are a Sahara platform
+	 * limitation.)
+	 */
+	FSL_KEY_ALG_ARC4,
+	/*!
+	 * Private key of a public-private key-pair.  Max is 512 bits...
+	 */
+	FSL_KEY_PK_PRIVATE,
+} fsl_shw_key_alg_t;
+
+/*!
+ * Mode selector for Symmetric Ciphers.
+ *
+ * The selection of mode determines how a cryptographic algorithm will be
+ * used to process the plaintext or ciphertext.
+ *
+ * For all modes which are run block-by-block (that is, all but
+ * #FSL_SYM_MODE_STREAM), any partial operations must be performed on a text
+ * length which is multiple of the block size.  Except for #FSL_SYM_MODE_CTR,
+ * these block-by-block algorithms must also be passed a total number of octets
+ * which is a multiple of the block size.
+ *
+ * In modes which require that the total number of octets of data be a multiple
+ * of the block size (#FSL_SYM_MODE_ECB and #FSL_SYM_MODE_CBC), and the user
+ * has a total number of octets which are not a multiple of the block size, the
+ * user must perform any necessary padding to get to the correct data length.
+ */
+typedef enum fsl_shw_sym_mode_t {
+	/*!
+	 * Stream.  There is no associated block size.  Any request to process data
+	 * may be of any length.  This mode is only for ARC4 operations, and is
+	 * also the only mode used for ARC4.
+	 */
+	FSL_SYM_MODE_STREAM,
+
+	/*!
+	 * Electronic Codebook.  Each block of data is encrypted/decrypted.  The
+	 * length of the data stream must be a multiple of the block size.  This
+	 * mode may be used for DES, 3DES, and AES.  The block size is determined
+	 * by the algorithm.
+	 */
+	FSL_SYM_MODE_ECB,
+	/*!
+	 * Cipher-Block Chaining.  Each block of data is encrypted/decrypted and
+	 * then "chained" with the previous block by an XOR function.  Requires
+	 * context to start the XOR (previous block).  This mode may be used for
+	 * DES, 3DES, and AES.  The block size is determined by the algorithm.
+	 */
+	FSL_SYM_MODE_CBC,
+	/*!
+	 * Counter.  The counter is encrypted, then XORed with a block of data.
+	 * The counter is then incremented (using modulus arithmetic) for the next
+	 * block. The final operation may be non-multiple of block size.  This mode
+	 * may be used for AES.  The block size is determined by the algorithm.
+	 */
+	FSL_SYM_MODE_CTR,
+} fsl_shw_sym_mode_t;
+
+/*!
+ * Algorithm selector for Cryptographic Hash functions.
+ *
+ * Selection of algorithm determines how large the context and digest will be.
+ * Context is the same size as the digest (resulting hash), unless otherwise
+ * specified.
+ */
+typedef enum fsl_shw_hash_alg_t {
+	/*! MD5 algorithm.  Digest is 16 octets. */
+	FSL_HASH_ALG_MD5,
+	/*! SHA-1 (aka SHA or SHA-160) algorithm. Digest is 20 octets. */
+	FSL_HASH_ALG_SHA1,
+	/*!
+	 * SHA-224 algorithm.  Digest is 28 octets, though context is 32 octets.
+	 */
+	FSL_HASH_ALG_SHA224,
+	/*! SHA-256 algorithm.  Digest is 32 octets. */
+	FSL_HASH_ALG_SHA256
+} fsl_shw_hash_alg_t;
+
+/*!
+ * The type of Authentication-Cipher function which will be performed.
+ */
+typedef enum fsl_shw_acc_mode_t {
+	/*!
+	 * CBC-MAC for Counter.  Requires context and modulus.  Final operation may
+	 * be non-multiple of block size.  This mode may be used for AES.
+	 */
+	FSL_ACC_MODE_CCM,
+	/*!
+	 * SSL mode.  Not supported.  Combines HMAC and encrypt (or decrypt).
+	 * Needs one key object for encryption, another for the HMAC.  The usual
+	 * hashing and symmetric encryption algorithms are supported.
+	 */
+	FSL_ACC_MODE_SSL,
+} fsl_shw_acc_mode_t;
+
+/* REQ-S2LRD-PINTFC-COA-HCO-001 */
+/*!
+ * Flags which control a Hash operation.
+ */
+typedef enum fsl_shw_hash_ctx_flags_t {
+	/*!
+	 * Context is empty.  Hash is started from scratch, with a
+	 * message-processed count of zero.
+	 */
+	FSL_HASH_FLAGS_INIT = 0x01,
+	/*!
+	 *  Retrieve context from hardware after hashing.  If used with the
+	 *  #FSL_HASH_FLAGS_FINALIZE flag, the final digest value will be saved in
+	 *  the object.
+	 */
+	FSL_HASH_FLAGS_SAVE = 0x02,
+	/*! Place context into hardware before hashing. */
+	FSL_HASH_FLAGS_LOAD = 0x04,
+	/*!
+	 * PAD message and perform final digest operation.  If user message is
+	 * pre-padded, this flag should not be used.
+	 */
+	FSL_HASH_FLAGS_FINALIZE = 0x08,
+} fsl_shw_hash_ctx_flags_t;
+
+/*!
+ * Flags which control an HMAC operation.
+ *
+ * These may be combined by ORing them together.  See #fsl_shw_hmco_set_flags()
+ * and #fsl_shw_hmco_clear_flags().
+ */
+typedef enum fsl_shw_hmac_ctx_flags_t {
+	/*!
+	 * Message context is empty.  HMAC is started from scratch (with key) or
+	 * from precompute of inner hash, depending on whether
+	 * #FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT is set.
+	 */
+	FSL_HMAC_FLAGS_INIT = 1,
+	/*!
+	 * Retrieve ongoing context from hardware after hashing.  If used with the
+	 * #FSL_HMAC_FLAGS_FINALIZE flag, the final digest value (HMAC) will be
+	 * saved in the object.
+	 */
+	FSL_HMAC_FLAGS_SAVE = 2,
+	/*! Place ongoing context into hardware before hashing. */
+	FSL_HMAC_FLAGS_LOAD = 4,
+	/*!
+	 * PAD message and perform final HMAC operations of inner and outer
+	 * hashes.
+	 */
+	FSL_HMAC_FLAGS_FINALIZE = 8,
+	/*!
+	 * This means that the context contains precomputed inner and outer hash
+	 * values.
+	 */
+	FSL_HMAC_FLAGS_PRECOMPUTES_PRESENT = 16,
+} fsl_shw_hmac_ctx_flags_t;
+
+/*!
+ * Flags to control use of the #fsl_shw_scco_t.
+ *
+ * These may be ORed together to get the desired effect.
+ * See #fsl_shw_scco_set_flags() and #fsl_shw_scco_clear_flags()
+ */
+typedef enum fsl_shw_sym_ctx_flags_t {
+	/*!
+	 * Context is empty.  In ARC4, this means that the S-Box needs to be
+	 * generated from the key.  In #FSL_SYM_MODE_CBC mode, this allows an IV of
+	 * zero to be specified.  In #FSL_SYM_MODE_CTR mode, it means that an
+	 * initial CTR value of zero is desired.
+	 */
+	FSL_SYM_CTX_INIT = 1,
+	/*!
+	 * Load context from object into hardware before running cipher.  In
+	 * #FSL_SYM_MODE_CTR mode, this would refer to the Counter Value.
+	 */
+	FSL_SYM_CTX_LOAD = 2,
+	/*!
+	 * Save context from hardware into object after running cipher.  In
+	 * #FSL_SYM_MODE_CTR mode, this would refer to the Counter Value.
+	 */
+	FSL_SYM_CTX_SAVE = 4,
+	/*!
+	 * Context (SBox) is to be unwrapped and wrapped on each use.
+	 * This flag is unsupported.
+	 * */
+	FSL_SYM_CTX_PROTECT = 8,
+} fsl_shw_sym_ctx_flags_t;
+
+/*!
+ * Flags which describe the state of the #fsl_shw_sko_t.
+ *
+ * These may be ORed together to get the desired effect.
+ * See #fsl_shw_sko_set_flags() and #fsl_shw_sko_clear_flags()
+ */
+typedef enum fsl_shw_key_flags_t {
+	/*! If algorithm is DES or 3DES, do not validate the key parity bits. */
+	FSL_SKO_KEY_IGNORE_PARITY = 1,
+	/*! Clear key is present in the object. */
+	FSL_SKO_KEY_PRESENT = 2,
+	/*!
+	 * Key has been established for use.  This feature is not available for all
+	 * platforms, nor for all algorithms and modes.
+	 */
+	FSL_SKO_KEY_ESTABLISHED = 4,
+	/*!
+	 * Key intended for user (software) use; can be read cleartext from the
+	 * keystore.
+	 */
+	FSL_SKO_KEY_SW_KEY = 8,
+} fsl_shw_key_flags_t;
+
+/*!
+ * Type of value which is associated with an established key.
+ */
+typedef uint64_t key_userid_t;
+
+/*!
+ * Flags which describe the state of the #fsl_shw_acco_t.
+ *
+ * The @a FSL_ACCO_CTX_INIT and @a FSL_ACCO_CTX_FINALIZE flags, when used
+ * together, provide for a one-shot operation.
+ */
+typedef enum fsl_shw_auth_ctx_flags_t {
+	/*! Initialize Context(s) */
+	FSL_ACCO_CTX_INIT = 1,
+	/*! Load intermediate context(s). This flag is unsupported. */
+	FSL_ACCO_CTX_LOAD = 2,
+	/*! Save intermediate context(s). This flag is unsupported. */
+	FSL_ACCO_CTX_SAVE = 4,
+	/*! Create MAC during this operation. */
+	FSL_ACCO_CTX_FINALIZE = 8,
+	/*!
+	 * Formatting of CCM input data is performed by calls to
+	 * #fsl_shw_ccm_nist_format_ctr_and_iv() and
+	 * #fsl_shw_ccm_nist_update_ctr_and_iv().
+	 */
+	FSL_ACCO_NIST_CCM = 0x10,
+} fsl_shw_auth_ctx_flags_t;
+
+/*!
+ * The operation which controls the behavior of #fsl_shw_establish_key().
+ *
+ * These values are passed to #fsl_shw_establish_key().
+ */
+typedef enum fsl_shw_key_wrap_t {
+	/*! Generate a key from random values. */
+	FSL_KEY_WRAP_CREATE,
+	/*! Use the provided clear key. */
+	FSL_KEY_WRAP_ACCEPT,
+	/*! Unwrap a previously wrapped key. */
+	FSL_KEY_WRAP_UNWRAP
+} fsl_shw_key_wrap_t;
+
+/*!
+ *  Modulus Selector for CTR modes.
+ *
+ * The incrementing of the Counter value may be modified by a modulus.  If no
+ * modulus is needed or desired for AES, use #FSL_CTR_MOD_128.
+ */
+typedef enum fsl_shw_ctr_mod_t {
+	FSL_CTR_MOD_8,		/*!< Run counter with modulus of 2^8. */
+	FSL_CTR_MOD_16,		/*!< Run counter with modulus of 2^16. */
+	FSL_CTR_MOD_24,		/*!< Run counter with modulus of 2^24. */
+	FSL_CTR_MOD_32,		/*!< Run counter with modulus of 2^32. */
+	FSL_CTR_MOD_40,		/*!< Run counter with modulus of 2^40. */
+	FSL_CTR_MOD_48,		/*!< Run counter with modulus of 2^48. */
+	FSL_CTR_MOD_56,		/*!< Run counter with modulus of 2^56. */
+	FSL_CTR_MOD_64,		/*!< Run counter with modulus of 2^64. */
+	FSL_CTR_MOD_72,		/*!< Run counter with modulus of 2^72. */
+	FSL_CTR_MOD_80,		/*!< Run counter with modulus of 2^80. */
+	FSL_CTR_MOD_88,		/*!< Run counter with modulus of 2^88. */
+	FSL_CTR_MOD_96,		/*!< Run counter with modulus of 2^96. */
+	FSL_CTR_MOD_104,	/*!< Run counter with modulus of 2^104. */
+	FSL_CTR_MOD_112,	/*!< Run counter with modulus of 2^112. */
+	FSL_CTR_MOD_120,	/*!< Run counter with modulus of 2^120. */
+	FSL_CTR_MOD_128		/*!< Run counter with modulus of 2^128. */
+} fsl_shw_ctr_mod_t;
+
+/*!
+ * Permissions flags for Secure Partitions
+ */
+typedef enum fsl_shw_permission_t {
+/*! SCM Access Permission: Do not zeroize/deallocate partition on SMN Fail state */
+	FSL_PERM_NO_ZEROIZE = 0x80000000,
+/*! SCM Access Permission: Enforce trusted key read in  */
+	FSL_PERM_TRUSTED_KEY_READ = 0x40000000,
+/*! SCM Access Permission: Ignore Supervisor/User mode in permission determination */
+	FSL_PERM_HD_S = 0x00000800,
+/*! SCM Access Permission: Allow Read Access to  Host Domain */
+	FSL_PERM_HD_R = 0x00000400,
+/*! SCM Access Permission: Allow Write Access to  Host Domain */
+	FSL_PERM_HD_W = 0x00000200,
+/*! SCM Access Permission: Allow Execute Access to  Host Domain */
+	FSL_PERM_HD_X = 0x00000100,
+/*! SCM Access Permission: Allow Read Access to Trusted Host Domain */
+	FSL_PERM_TH_R = 0x00000040,
+/*! SCM Access Permission: Allow Write Access to Trusted Host Domain */
+	FSL_PERM_TH_W = 0x00000020,
+/*! SCM Access Permission: Allow Read Access to Other/World Domain */
+	FSL_PERM_OT_R = 0x00000004,
+/*! SCM Access Permission: Allow Write Access to Other/World Domain */
+	FSL_PERM_OT_W = 0x00000002,
+/*! SCM Access Permission: Allow Execute Access to Other/World Domain */
+	FSL_PERM_OT_X = 0x00000001,
+} fsl_shw_permission_t;
+
+typedef enum fsl_shw_cypher_mode_t {
+	FSL_SHW_CYPHER_MODE_ECB = 1,	/*!< ECB mode */
+	FSL_SHW_CYPHER_MODE_CBC = 2,	/*!< CBC mode */
+} fsl_shw_cypher_mode_t;
+
+typedef enum fsl_shw_pf_key_t {
+	FSL_SHW_PF_KEY_IIM,	/*!< Present fused IIM key */
+	FSL_SHW_PF_KEY_PRG,	/*!< Present Program key */
+	FSL_SHW_PF_KEY_IIM_PRG,	/*!< Present IIM ^ Program key */
+	FSL_SHW_PF_KEY_IIM_RND,	/*!< Present Random key */
+	FSL_SHW_PF_KEY_RND,	/*!< Present IIM ^ Random key */
+} fsl_shw_pf_key_t;
+
+typedef enum fsl_shw_tamper_t {
+	FSL_SHW_TAMPER_NONE,	/*!< No error detected */
+	FSL_SHW_TAMPER_WTD,	/*!< wire-mesh tampering det */
+	FSL_SHW_TAMPER_ETBD,	/*!< ext tampering det: input B */
+	FSL_SHW_TAMPER_ETAD,	/*!< ext tampering det: input A */
+	FSL_SHW_TAMPER_EBD,	/*!< external boot detected */
+	FSL_SHW_TAMPER_SAD,	/*!< security alarm detected */
+	FSL_SHW_TAMPER_TTD,	/*!< temperature tampering det */
+	FSL_SHW_TAMPER_CTD,	/*!< clock tampering det */
+	FSL_SHW_TAMPER_VTD,	/*!< voltage tampering det */
+	FSL_SHW_TAMPER_MCO,	/*!< monotonic counter overflow */
+	FSL_SHW_TAMPER_TCO,	/*!< time counter overflow */
+} fsl_shw_tamper_t;
+
+/******************************************************************************
+ * Data Structures
+ *****************************************************************************/
+
+/*!
+ *
+ * @brief Structure type for descriptors
+ *
+ * The first five fields are passed to the hardware.
+ *
+ *****************************************************************************/
+#ifndef USE_NEW_PTRS		/* Experimental */
+
+typedef struct sah_Desc {
+	uint32_t header;	/*!< descriptor header value */
+	uint32_t len1;		/*!< number of data bytes in 'ptr1' buffer */
+	void *ptr1;		/*!< pointer to first sah_Link structure */
+	uint32_t len2;		/*!< number of data bytes in 'ptr2' buffer */
+	void *ptr2;		/*!< pointer to second sah_Link structure */
+	struct sah_Desc *next;	/*!< pointer to next descriptor */
+#ifdef __KERNEL__		/* This needs a better test */
+	/* These two must be last.  See sah_Copy_Descriptors */
+	struct sah_Desc *virt_addr;	/*!< Virtual (kernel) address of this
+					   descriptor. */
+	dma_addr_t dma_addr;	/*!< Physical (bus) address of this
+				   descriptor.  */
+	void *original_ptr1;	/*!< user's pointer to ptr1 */
+	void *original_ptr2;	/*!< user's pointer to ptr2 */
+	struct sah_Desc *original_next;	/*!< user's pointer to next */
+#endif
+} sah_Desc;
+
+#else
+
+typedef struct sah_Desc {
+	uint32_t header;	/*!< descriptor header value */
+	uint32_t len1;		/*!< number of data bytes in 'ptr1' buffer */
+	uint32_t hw_ptr1;	/*!< pointer to first sah_Link structure */
+	uint32_t len2;		/*!< number of data bytes in 'ptr2' buffer */
+	uint32_t hw_ptr2;	/*!< pointer to second sah_Link structure */
+	uint32_t hw_next;	/*!< pointer to next descriptor */
+	struct sah_Link *ptr1;	/*!< (virtual) pointer to first sah_Link structure */
+	struct sah_Link *ptr2;	/*!< (virtual) pointer to first sah_Link structure */
+	struct sah_Desc *next;	/*!< (virtual) pointer to next descriptor */
+#ifdef __KERNEL__		/* This needs a better test */
+	/* These two must be last.  See sah_Copy_Descriptors */
+	struct sah_Desc *virt_addr;	/*!< Virtual (kernel) address of this
+					   descriptor. */
+	dma_addr_t dma_addr;	/*!< Physical (bus) address of this
+				   descriptor.  */
+#endif
+} sah_Desc;
+
+#endif
+
+/*!
+*******************************************************************************
+* @brief The first descriptor in a chain
+******************************************************************************/
+typedef struct sah_Head_Desc {
+	sah_Desc desc;		/*!< whole struct - must be first */
+	struct fsl_shw_uco_t *user_info;	/*!< where result pool lives */
+	uint32_t user_ref;	/*!< at time of request */
+	uint32_t uco_flags;	/*!< at time of request */
+	uint32_t status;	/*!<  Status of queue entry */
+	uint32_t error_status;	/*!< If error, register from Sahara */
+	uint32_t fault_address;	/*!< If error, register from Sahara */
+	uint32_t op_status;	/*!< If error, register from Sahara */
+	fsl_shw_return_t result;	/*!< Result of running descriptor  */
+	struct sah_Head_Desc *next;	/*!< Next in queue  */
+	struct sah_Head_Desc *prev;	/*!< previous in queue  */
+	struct sah_Head_Desc *user_desc;	/*!< For API async get_results */
+	void *out1_ptr;		/*!< For async post-processing  */
+	void *out2_ptr;		/*!< For async post-processing  */
+	uint32_t out_len;	/*!< For async post-processing  */
+} sah_Head_Desc;
+
+/*!
+ * @brief Structure type for links
+ *
+ * The first three fields are used by hardware.
+ *****************************************************************************/
+#ifndef USE_NEW_PTRS
+
+typedef struct sah_Link {
+	size_t len;		/*!< len of 'data' buffer in bytes */
+	uint8_t *data;		/*!< buffer to store data */
+	struct sah_Link *next;	/*!< pointer to the next sah_Link storing
+				 * data */
+	sah_Link_Flags flags;	/*!< indicates the component that created the
+				 * data buffer. Security Function internal
+				 * information */
+	key_userid_t ownerid;	/*!< Auth code for established key */
+	uint32_t slot;		/*!< Location of the the established key */
+#ifdef __KERNEL__		/* This needs a better test */
+	/* These two elements must be last.  See sah_Copy_Links() */
+	struct sah_Link *virt_addr;
+	dma_addr_t dma_addr;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
+	struct page *vm_info;
+#endif
+	uint8_t *original_data;	/*!< user's version of data pointer */
+	struct sah_Link *original_next;	/*!< user's version of next pointer */
+#ifdef SAH_COPY_DATA
+	uint8_t *copy_data;	/*!< Virtual address of acquired buffer */
+#endif
+#endif				/* kernel-only */
+} sah_Link;
+
+#else
+
+typedef struct sah_Link {
+	/*! len of 'data' buffer in bytes */
+	size_t len;
+	/*! buffer to store data */
+	uint32_t hw_data;
+	/*! Physical address */
+	uint32_t hw_next;
+	/*!
+	 * indicates the component that created the data buffer. Security Function
+	 * internal information
+	 */
+	sah_Link_Flags flags;
+	/*! (virtual) pointer to data */
+	uint8_t *data;
+	/*! (virtual) pointer to the next sah_Link storing data */
+	struct sah_Link *next;
+	/*! Auth code for established key */
+	key_userid_t ownerid;
+	/*! Location of the the established key */
+	uint32_t slot;
+#ifdef __KERNEL__		/* This needs a better test */
+	/* These two elements must be last.  See sah_Copy_Links() */
+	struct sah_Link *virt_addr;
+	dma_addr_t dma_addr;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
+	struct page *vm_info;
+#endif
+#endif				/* kernel-only */
+} sah_Link;
+
+#endif
+
+/*!
+ * Initialization Object
+ */
+typedef struct fsl_sho_ibo_t {
+} fsl_sho_ibo_t;
+
+/* Imported from Sahara1 driver -- is it needed forever? */
+/*!
+*******************************************************************************
+* FIELDS
+*
+*   void * ref - parameter to be passed into the memory function calls
+*
+*   void * (*malloc)(void *ref, size_t n) - pointer to user's malloc function
+*
+*   void   (*free)(void *ref, void *ptr)  - pointer to user's free function
+*
+*   void * (*memcpy)(void *ref, void *dest, const void *src, size_t n) -
+*                                         pointer to user's memcpy function
+*
+*   void * (*memset)(void *ref, void *ptr, int ch, size_t n) - pointer to
+*                                         user's memset function
+*
+* @brief Structure for API memory utilities
+******************************************************************************/
+typedef struct sah_Mem_Util {
+	/*! Who knows.  Vestigial. */
+	void *mu_ref;
+	/*! Acquire buffer of size n bytes */
+	void *(*mu_malloc) (void *ref, size_t n);
+	/*! Acquire a sah_Head_Desc */
+	sah_Head_Desc *(*mu_alloc_head_desc) (void *ref);
+	/* Acquire a sah_Desc */
+	sah_Desc *(*mu_alloc_desc) (void *ref);
+	/* Acquire a sah_Link */
+	sah_Link *(*mu_alloc_link) (void *ref);
+	/*! Free buffer at ptr */
+	void (*mu_free) (void *ref, void *ptr);
+	/*! Free sah_Head_Desc at ptr */
+	void (*mu_free_head_desc) (void *ref, sah_Head_Desc * ptr);
+	/*! Free sah_Desc at ptr */
+	void (*mu_free_desc) (void *ref, sah_Desc * ptr);
+	/*! Free sah_Link at ptr */
+	void (*mu_free_link) (void *ref, sah_Link * ptr);
+	/*! Funciton which will copy n bytes from src to dest  */
+	void *(*mu_memcpy) (void *ref, void *dest, const void *src, size_t n);
+	/*! Set all n bytes of ptr to ch */
+	void *(*mu_memset) (void *ref, void *ptr, int ch, size_t n);
+} sah_Mem_Util;
+
+/*!
+ * Secure Partition information
+ *
+ * This holds the context to a single secure partition owned by the user.  It
+ * is only available in the kernel version of the User Context Object.
+ */
+typedef struct fsl_shw_spo_t {
+	uint32_t user_base;	/*!< Base address (user virtual) */
+	void *kernel_base;	/*!< Base address (kernel virtual) */
+	struct fsl_shw_spo_t *next;	/*!< Pointer to the next partition
+					   owned by the user.  NULL if this
+					   is the last partition. */
+} fsl_shw_spo_t;
+
+/* REQ-S2LRD-PINTFC-COA-UCO-001 */
+/*!
+ * User Context Object
+ */
+typedef struct fsl_shw_uco_t {
+	int sahara_openfd;	/*!< this should be kernel-only?? */
+	sah_Mem_Util *mem_util;	/*!< Memory utility fns  */
+	uint32_t user_ref;	/*!< User's reference */
+	void (*callback) (struct fsl_shw_uco_t * uco);	/*!< User's callback fn  */
+	uint32_t flags;		/*!< from fsl_shw_user_ctx_flags_t */
+	unsigned pool_size;	/*!< maximum size of user pool */
+#ifdef __KERNEL__
+	sah_Queue result_pool;	/*!< where non-blocking results go */
+	os_process_handle_t process;	/*!< remember for signalling User mode */
+	fsl_shw_spo_t *partition;	/*!< chain of secure partitions owned by
+					   the user */
+#else
+	struct fsl_shw_uco_t *next;	/*!< To allow user-mode chaining of contexts,
+					   for signalling.  */
+#endif
+} fsl_shw_uco_t;
+
+/* REQ-S2LRD-PINTFC-API-GEN-006  ??  */
+/*!
+ * Result object
+ */
+typedef struct fsl_shw_result_t {
+	uint32_t user_ref;
+	fsl_shw_return_t code;
+	uint32_t detail1;
+	uint32_t detail2;
+	sah_Head_Desc *user_desc;
+} fsl_shw_result_t;
+
+/*!
+ * Keystore Object
+ */
+typedef struct fsl_shw_kso_t {
+#ifdef __KERNEL__
+	os_lock_t lock;		/*!< Pointer to lock that controls access to
+				   the keystore. */
+#endif
+	void *user_data;	/*!< Pointer to user structure that handles
+				   the internals of the keystore. */
+	 fsl_shw_return_t(*data_init) (fsl_shw_uco_t * user_ctx,
+				       void **user_data);
+	void (*data_cleanup) (fsl_shw_uco_t * user_ctx, void **user_data);
+	 fsl_shw_return_t(*slot_verify_access) (void *user_data,
+						uint64_t owner_id,
+						uint32_t slot);
+	 fsl_shw_return_t(*slot_alloc) (void *user_data, uint32_t size_bytes,
+					uint64_t owner_id, uint32_t * slot);
+	 fsl_shw_return_t(*slot_dealloc) (void *user_data, uint64_t owner_id,
+					  uint32_t slot);
+	void *(*slot_get_address) (void *user_data, uint32_t slot);
+	 uint32_t(*slot_get_base) (void *user_data, uint32_t slot);
+	 uint32_t(*slot_get_offset) (void *user_data, uint32_t slot);
+	 uint32_t(*slot_get_slot_size) (void *user_data, uint32_t slot);
+} fsl_shw_kso_t;
+
+/* REQ-S2LRD-PINTFC-COA-SKO-001 */
+/*!
+ * Secret Key Context Object
+ */
+typedef struct fsl_shw_sko_t {
+	uint32_t flags;
+	fsl_shw_key_alg_t algorithm;
+	key_userid_t userid;
+	uint32_t handle;
+	uint16_t key_length;
+	uint8_t key[64];
+	struct fsl_shw_kso_t *keystore;	/*!< If present, key is in keystore */
+} fsl_shw_sko_t;
+
+/* REQ-S2LRD-PINTFC-COA-CO-001 */
+/*!
+ * @brief Platform Capability Object
+ */
+typedef struct fsl_shw_pco_t {	/* Consider turning these constants into symbols */
+	int api_major;
+	int api_minor;
+	int driver_major;
+	int driver_minor;
+	fsl_shw_key_alg_t sym_algorithms[4];
+	fsl_shw_sym_mode_t sym_modes[4];
+	fsl_shw_hash_alg_t hash_algorithms[4];
+	uint8_t sym_support[5][4];	/* indexed by key alg then mode */
+
+	int scc_driver_major;
+	int scc_driver_minor;
+	int scm_version;	/*!< Version from SCM Configuration register */
+	int smn_version;	/*!< Version from SMN Status register */
+	int block_size_bytes;	/*!< Number of bytes per block of RAM; also
+				   block size of the crypto algorithm. */
+	union {
+		struct {
+			int black_ram_size_blocks;	/*!< Number of blocks of Black RAM */
+			int red_ram_size_blocks;	/*!< Number of blocks of Red RAM */
+		} scc_info;
+		struct {
+			int partition_size_bytes;	/*!< Number of bytes in each partition */
+			int partition_count;	/*!< Number of partitions on this platform */
+		} scc2_info;
+	};
+} fsl_shw_pco_t;
+
+/* REQ-S2LRD-PINTFC-COA-HCO-001 */
+/*!
+ * Hash Context Object
+ */
+typedef struct fsl_shw_hco_t {	/* fsl_shw_hash_context_object */
+	fsl_shw_hash_alg_t algorithm;
+	uint32_t flags;
+	uint8_t digest_length;	/* in bytes */
+	uint8_t context_length;	/* in bytes */
+	uint8_t context_register_length;	/* in bytes */
+	uint32_t context[9];	/* largest digest + msg size */
+} fsl_shw_hco_t;
+
+/*!
+ * HMAC Context Object
+ */
+typedef struct fsl_shw_hmco_t {	/* fsl_shw_hmac_context_object */
+	fsl_shw_hash_alg_t algorithm;
+	uint32_t flags;
+	uint8_t digest_length;	/*!< in bytes */
+	uint8_t context_length;	/*!< in bytes */
+	uint8_t context_register_length;	/*!< in bytes */
+	uint32_t ongoing_context[9];	/*!< largest digest + msg
+					   size */
+	uint32_t inner_precompute[9];	/*!< largest digest + msg
+					   size */
+	uint32_t outer_precompute[9];	/*!< largest digest + msg
+					   size */
+} fsl_shw_hmco_t;
+
+/* REQ-S2LRD-PINTFC-COA-SCCO-001 */
+/*!
+ * Symmetric Crypto Context Object Context Object
+ */
+typedef struct fsl_shw_scco_t {
+	uint32_t flags;
+	unsigned block_size_bytes;	/* double duty block&ctx size */
+	fsl_shw_sym_mode_t mode;
+	/* Could put modulus plus 16-octet context in union with arc4
+	   sbox+ptrs... */
+	fsl_shw_ctr_mod_t modulus_exp;
+	uint8_t context[259];
+} fsl_shw_scco_t;
+
+/*!
+ * Authenticate-Cipher Context Object
+
+ * An object for controlling the function of, and holding information about,
+ * data for the authenticate-cipher functions, #fsl_shw_gen_encrypt() and
+ * #fsl_shw_auth_decrypt().
+ */
+typedef struct fsl_shw_acco_t {
+	uint32_t flags;		/*!< See #fsl_shw_auth_ctx_flags_t for
+				   meanings */
+	fsl_shw_acc_mode_t mode;	/*!< CCM only */
+	uint8_t mac_length;	/*!< User's value for length  */
+	unsigned q_length;	/*!< NIST parameter - */
+	fsl_shw_scco_t cipher_ctx_info;	/*!< For running
+					   encrypt/decrypt. */
+	union {
+		fsl_shw_scco_t CCM_ctx_info;	/*!< For running the CBC in
+						   AES-CCM.  */
+		fsl_shw_hco_t hash_ctx_info;	/*!< For running the hash */
+	} auth_info;		/*!< "auth" info struct  */
+	uint8_t unencrypted_mac[16];	/*!< max block size... */
+} fsl_shw_acco_t;
+
+/*!
+ *  Used by Sahara API to retrieve completed non-blocking results.
+ */
+typedef struct sah_results {
+	unsigned requested;	/*!< number of results requested */
+	unsigned *actual;	/*!< number of results obtained */
+	fsl_shw_result_t *results;	/*!< pointer to memory to hold results */
+} sah_results;
+
+/*!
+ * @typedef scc_partition_status_t
+ */
+/*! Partition status information. */
+typedef enum fsl_shw_partition_status_t {
+	FSL_PART_S_UNUSABLE,	/*!< Partition not implemented */
+	FSL_PART_S_UNAVAILABLE,	/*!< Partition owned by other host */
+	FSL_PART_S_AVAILABLE,	/*!< Partition available */
+	FSL_PART_S_ALLOCATED,	/*!< Partition owned by host but not engaged
+				 */
+	FSL_PART_S_ENGAGED,	/*!< Partition owned by host and engaged */
+} fsl_shw_partition_status_t;
+
+/******************************************************************************
+ * Access Macros for Objects
+ *****************************************************************************/
+/*!
+ * Get FSL SHW API version
+ *
+ * @param      pcobject  The Platform Capababilities Object to query.
+ * @param[out] pcmajor   A pointer to where the major version
+ *                       of the API is to be stored.
+ * @param[out] pcminor   A pointer to where the minor version
+ *                       of the API is to be stored.
+ */
+#define fsl_shw_pco_get_version(pcobject, pcmajor, pcminor)                   \
+{                                                                             \
+    *(pcmajor) = (pcobject)->api_major;                                       \
+    *(pcminor) = (pcobject)->api_minor;                                       \
+}
+
+/*!
+ * Get underlying driver version.
+ *
+ * @param      pcobject  The Platform Capababilities Object to query.
+ * @param[out] pcmajor   A pointer to where the major version
+ *                       of the driver is to be stored.
+ * @param[out] pcminor   A pointer to where the minor version
+ *                       of the driver is to be stored.
+ */
+#define fsl_shw_pco_get_driver_version(pcobject, pcmajor, pcminor)            \
+{                                                                             \
+    *(pcmajor) = (pcobject)->driver_major;                                    \
+    *(pcminor) = (pcobject)->driver_minor;                                    \
+}
+
+/*!
+ * Get list of symmetric algorithms supported.
+ *
+ * @param pcobject           The Platform Capababilities Object to query.
+ * @param[out] pcalgorithms  A pointer to where to store the location of
+ *                           the list of algorithms.
+ * @param[out] pcacount      A pointer to where to store the number of
+ *                           algorithms in the list at @a algorithms.
+ */
+#define fsl_shw_pco_get_sym_algorithms(pcobject, pcalgorithms, pcacount)      \
+{                                                                             \
+    *(pcalgorithms) = (pcobject)->sym_algorithms;                             \
+    *(pcacount) = sizeof((pcobject)->sym_algorithms)/4;                       \
+}
+
+/*!
+ * Get list of symmetric modes supported.
+ *
+ * @param pcobject        The Platform Capababilities Object to query.
+ * @param[out] gsmodes    A pointer to where to store the location of
+ *                        the list of modes.
+ * @param[out] gsacount   A pointer to where to store the number of
+ *                        algorithms in the list at @a modes.
+ */
+#define fsl_shw_pco_get_sym_modes(pcobject, gsmodes, gsacount)                \
+{                                                                             \
+    *(gsmodes) = (pcobject)->sym_modes;                                       \
+    *(gsacount) = sizeof((pcobject)->sym_modes)/4;                            \
+}
+
+/*!
+ * Get list of hash algorithms supported.
+ *
+ * @param pcobject           The Platform Capababilities Object to query.
+ * @param[out] gsalgorithms  A pointer which will be set to the list of
+ *                           algorithms.
+ * @param[out] gsacount      The number of algorithms in the list at @a
+ *                           algorithms.
+ */
+#define fsl_shw_pco_get_hash_algorithms(pcobject, gsalgorithms, gsacount)     \
+{                                                                             \
+    *(gsalgorithms) = (pcobject)->hash_algorithms;                            \
+    *(gsacount) = sizeof((pcobject)->hash_algorithms)/4;                      \
+}
+
+/*!
+ * Determine whether the combination of a given symmetric algorithm and a given
+ * mode is supported.
+ *
+ * @param pcobject   The Platform Capababilities Object to query.
+ * @param pcalg      A Symmetric Cipher algorithm.
+ * @param pcmode     A Symmetric Cipher mode.
+ *
+ * @return 0 if combination is not supported, non-zero if supported.
+ */
+#define fsl_shw_pco_check_sym_supported(pcobject, pcalg, pcmode)              \
+    ((pcobject)->sym_support[pcalg][pcmode])
+
+/*!
+ * Determine whether a given Encryption-Authentication mode is supported.
+ *
+ * @param pcobject  The Platform Capababilities Object to query.
+ * @param pcmode    The Authentication mode.
+ *
+ * @return 0 if mode is not supported, non-zero if supported.
+ */
+#define fsl_shw_pco_check_auth_supported(pcobject, pcmode)                    \
+    ((pcmode == FSL_ACC_MODE_CCM) ? 1 : 0)
+
+/*!
+ * Determine whether Black Keys (key establishment / wrapping) is supported.
+ *
+ * @param pcobject  The Platform Capababilities Object to query.
+ *
+ * @return 0 if wrapping is not supported, non-zero if supported.
+ */
+#define fsl_shw_pco_check_black_key_supported(pcobject)                     \
+    1
+
+/*!
+ * Determine whether Programmed Key features are available
+ *
+ * @param pc_info          The Platform Capabilities Object to query.
+ *
+ * @return  1 if Programmed Key features are available, otherwise zero.
+ */
+#define fsl_shw_pco_check_pk_supported(pcobject)        \
+    0
+
+/*!
+ * Determine whether Software Key features are available
+ *
+ * @param pc_info          The Platform Capabilities Object to query.
+ *
+ * @return  1 if Software key features are available, otherwise zero.
+ */
+#define fsl_shw_pco_check_sw_keys_supported(pcobject)        \
+    0
+
+/*!
+ * Get FSL SHW SCC driver version
+ *
+ * @param      pcobject  The Platform Capabilities Object to query.
+ * @param[out] pcmajor   A pointer to where the major version
+ *                       of the SCC driver is to be stored.
+ * @param[out] pcminor   A pointer to where the minor version
+ *                       of the SCC driver is to be stored.
+ */
+#define fsl_shw_pco_get_scc_driver_version(pcobject, pcmajor, pcminor)        \
+{                                                                             \
+    *(pcmajor) = (pcobject)->scc_driver_major;                                \
+    *(pcminor) = (pcobject)->scc_driver_minor;                                \
+}
+
+/*!
+ * Get SCM hardware version
+ *
+ * @param      pcobject  The Platform Capabilities Object to query.
+ * @return               The SCM hardware version
+ */
+#define fsl_shw_pco_get_scm_version(pcobject)                                 \
+    ((pcobject)->scm_version)
+
+/*!
+ * Get SMN hardware version
+ *
+ * @param      pcobject  The Platform Capabilities Object to query.
+ * @return               The SMN hardware version
+ */
+#define fsl_shw_pco_get_smn_version(pcobject)                                 \
+    ((pcobject)->smn_version)
+
+/*!
+ * Get the size of an SCM block, in bytes
+ *
+ * @param      pcobject  The Platform Capabilities Object to query.
+ * @return               The size of an SCM block, in bytes.
+ */
+#define fsl_shw_pco_get_scm_block_size(pcobject)                              \
+    ((pcobject)->block_size_bytes)
+
+/*!
+ * Get size of Black and Red RAM memory
+ *
+ * @param      pcobject    The Platform Capabilities Object to query.
+ * @param[out] black_size  A pointer to where the size of the Black RAM, in
+ *                         blocks, is to be placed.
+ * @param[out] red_size    A pointer to where the size of the Red RAM, in 
+ *                         blocks, is to be placed.
+ */
+#define fsl_shw_pco_get_smn_size(pcobject, black_size, red_size)              \
+{                                                                             \
+    if ((pcobject)->scm_version == 1) {                                       \
+        *(black_size) = (pcobject)->scc_info.black_ram_size_blocks;           \
+        *(red_size)   = (pcobject)->scc_info.red_ram_size_blocks;             \
+    } else {                                                                  \
+        *(black_size) = 0;                                                    \
+        *(red_size)   = 0;                                                    \
+    }                                                                         \
+}
+
+/*!
+ * Determine whether Secure Partitions are supported
+ *
+ * @param pcobject   The Platform Capabilities Object to query.
+ *
+ * @return 0 if secure partitions are not supported, non-zero if supported.
+ */
+#define fsl_shw_pco_check_spo_supported(pcobject)                           \
+    ((pcobject)->scm_version == 2)
+
+/*!
+ * Get the size of a Secure Partitions
+ *
+ * @param pcobject   The Platform Capabilities Object to query.
+ * 
+ * @return Partition size, in bytes.  0 if Secure Partitions not supported.
+ */
+#define fsl_shw_pco_get_spo_size_bytes(pcobject)                            \
+    (((pcobject)->scm_version == 2) ?                                       \
+        ((pcobject)->scc2_info.partition_size_bytes) : 0 )
+
+/*!
+ * Get the number of Secure Partitions on this platform
+ *
+ * @param pcobject   The Platform Capabilities Object to query.
+ * 
+ * @return Number of partitions. 0 if Secure Paritions not supported.  Note
+ *         that this returns the total number of partitions, not all may be
+ *         available to the user.
+ */
+#define fsl_shw_pco_get_spo_count(pcobject)                                 \
+    (((pcobject)->scm_version == 2) ?                                       \
+        ((pcobject)->scc2_info.partition_count) : 0 )
+
+/*!
+ * Initialize a User Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  It sets the User Context Object to initial values, and set the size
+ * of the results pool.  The mode will be set to a default of
+ * #FSL_UCO_BLOCKING_MODE.
+ *
+ * When using non-blocking operations, this sets the maximum number of
+ * operations which can be outstanding.  This number includes the counts of
+ * operations waiting to start, operation(s) being performed, and results which
+ * have not been retrieved.
+ *
+ * Changes to this value are ignored once user registration has completed.  It
+ * should be set to 1 if only blocking operations will ever be performed.
+ *
+ * @param ucontext     The User Context object to operate on.
+ * @param usize        The maximum number of operations which can be
+ *                     outstanding.
+ */
+#ifdef __KERNEL__
+#define fsl_shw_uco_init(ucontext, usize)                                     \
+{                                                                             \
+      (ucontext)->pool_size = usize;                                          \
+      (ucontext)->flags = FSL_UCO_BLOCKING_MODE;                              \
+      (ucontext)->sahara_openfd = -1;                                         \
+      (ucontext)->mem_util = NULL;                                            \
+      (ucontext)->partition = NULL;                                           \
+      (ucontext)->callback = NULL;                                            \
+}
+#else
+#define fsl_shw_uco_init(ucontext, usize)                                     \
+{                                                                             \
+      (ucontext)->pool_size = usize;                                          \
+      (ucontext)->flags = FSL_UCO_BLOCKING_MODE;                              \
+      (ucontext)->sahara_openfd = -1;                                         \
+      (ucontext)->mem_util = NULL;                                            \
+      (ucontext)->callback = NULL;                                            \
+}
+#endif
+
+/*!
+ * Set the User Reference for the User Context.
+ *
+ * @param ucontext     The User Context object to operate on.
+ * @param uref         A value which will be passed back with a result.
+ */
+#define fsl_shw_uco_set_reference(ucontext, uref)                             \
+      (ucontext)->user_ref = uref
+
+/*!
+ * Set the User Reference for the User Context.
+ *
+ * @param ucontext     The User Context object to operate on.
+ * @param ucallback    The function the API will invoke when an operation
+ *                     completes.
+ */
+#define fsl_shw_uco_set_callback(ucontext, ucallback)                         \
+      (ucontext)->callback = ucallback
+
+/*!
+ * Set flags in the User Context.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param ucontext     The User Context object to operate on.
+ * @param uflags       ORed values from #fsl_shw_user_ctx_flags_t.
+ */
+#define fsl_shw_uco_set_flags(ucontext, uflags)                               \
+      (ucontext)->flags |= (uflags)
+
+/*!
+ * Clear flags in the User Context.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param ucontext     The User Context object to operate on.
+ * @param uflags       ORed values from #fsl_shw_user_ctx_flags_t.
+ */
+#define fsl_shw_uco_clear_flags(ucontext, uflags)                             \
+      (ucontext)->flags &= ~(uflags)
+
+/*!
+ * Retrieve the reference value from a Result Object.
+ *
+ * @param robject  The result object to query.
+ *
+ * @return The reference associated with the request.
+ */
+#define fsl_shw_ro_get_reference(robject)                                    \
+       (robject)->user_ref
+
+/*!
+ * Retrieve the status code from a Result Object.
+ *
+ * @param robject  The result object to query.
+ *
+ * @return The status of the request.
+ */
+#define fsl_shw_ro_get_status(robject)                                       \
+       (robject)->code
+
+/*!
+ * Initialize a Secret Key Object.
+ *
+ * This function must be called before performing any other operation with
+ * the Object.
+ *
+ * @param skobject     The Secret Key Object to be initialized.
+ * @param skalgorithm  DES, AES, etc.
+ *
+ */
+#define fsl_shw_sko_init(skobject,skalgorithm)                               \
+{                                                                            \
+       (skobject)->algorithm = skalgorithm;                                  \
+       (skobject)->flags = 0;                                                \
+       (skobject)->keystore = NULL;                                          \
+}
+
+/*!
+ * Initialize a Secret Key Object to use a Platform Key register.
+ *
+ * This function must be called before performing any other operation with
+ * the Object.  INVALID on this platform.
+ *
+ * @param skobject     The Secret Key Object to be initialized.
+ * @param skalgorithm  DES, AES, etc.
+ * @param skhwkey      one of the fsl_shw_pf_key_t values.
+ *
+ */
+#define fsl_shw_sko_init_pf_key(skobject,skalgorithm,skhwkey)       \
+{                                                                   \
+    (skobject)->algorithm = -1;                                         \
+    (skobject)->flags = -1;                                             \
+    (skobject)->keystore = NULL;                                        \
+}
+
+/*!
+ * Store a cleartext key in the key object.
+ *
+ * This has the side effect of setting the #FSL_SKO_KEY_PRESENT flag and
+ * resetting the #FSL_SKO_KEY_ESTABLISHED flag.
+ *
+ * @param skobject     A variable of type #fsl_shw_sko_t.
+ * @param skkey        A pointer to the beginning of the key.
+ * @param skkeylen     The length, in octets, of the key.  The value should be
+ *                     appropriate to the key size supported by the algorithm.
+ *                     64 octets is the absolute maximum value allowed for this
+ *                     call.
+ */
+#define fsl_shw_sko_set_key(skobject, skkey, skkeylen)                       \
+{                                                                            \
+       (skobject)->key_length = skkeylen;                                    \
+       copy_bytes((skobject)->key, skkey, skkeylen);                         \
+       (skobject)->flags |= FSL_SKO_KEY_PRESENT;                             \
+       (skobject)->flags &= ~FSL_SKO_KEY_ESTABLISHED;                        \
+}
+
+/*!
+ * Set a size for the key.
+ *
+ * This function would normally be used when the user wants the key to be
+ * generated from a random source.
+ *
+ * @param skobject   A variable of type #fsl_shw_sko_t.
+ * @param skkeylen   The length, in octets, of the key.  The value should be
+ *                   appropriate to the key size supported by the algorithm.
+ *                   64 octets is the absolute maximum value allowed for this
+ *                   call.
+ */
+#define fsl_shw_sko_set_key_length(skobject, skkeylen)                       \
+       (skobject)->key_length = skkeylen;
+
+/*!
+ * Set the User ID associated with the key.
+ *
+ * @param skobject   A variable of type #fsl_shw_sko_t.
+ * @param skuserid   The User ID to identify authorized users of the key.
+ */
+#define fsl_shw_sko_set_user_id(skobject, skuserid)                           \
+       (skobject)->userid = (skuserid)
+
+/*!
+ * Establish a user Keystore to hold the key.
+ */
+#define fsl_shw_sko_set_keystore(skobject, user_keystore)                     \
+       (skobject)->keystore = (user_keystore)
+
+/*!
+ * Set the establish key handle into a key object.
+ *
+ * The @a userid field will be used to validate the access to the unwrapped
+ * key.  This feature is not available for all platforms, nor for all
+ * algorithms and modes.
+ *
+ * The #FSL_SKO_KEY_ESTABLISHED will be set (and the #FSL_SKO_KEY_PRESENT flag
+ * will be cleared).
+ *
+ * @param skobject   A variable of type #fsl_shw_sko_t.
+ * @param skuserid   The User ID to verify this user is an authorized user of
+ *                   the key.
+ * @param skhandle   A @a handle from #fsl_shw_sko_get_established_info.
+ */
+#define fsl_shw_sko_set_established_info(skobject, skuserid, skhandle)        \
+{                                                                             \
+       (skobject)->userid = (skuserid);                                       \
+       (skobject)->handle = (skhandle);                                       \
+       (skobject)->flags |= FSL_SKO_KEY_ESTABLISHED;                          \
+       (skobject)->flags &=                                                   \
+                       ~(FSL_SKO_KEY_PRESENT);   \
+}
+
+/*!
+ * Retrieve the established-key handle from a key object.
+ *
+ * @param skobject   A variable of type #fsl_shw_sko_t.
+ * @param skhandle   The location to store the @a handle of the unwrapped
+ *                   key.
+ */
+#define fsl_shw_sko_get_established_info(skobject, skhandle)                  \
+       *(skhandle) = (skobject)->handle
+
+/*!
+ * Extract the algorithm from a key object.
+ *
+ * @param      skobject     The Key Object to be queried.
+ * @param[out] skalgorithm  A pointer to the location to store the algorithm.
+ */
+#define fsl_shw_sko_get_algorithm(skobject, skalgorithm)                      \
+       *(skalgorithm) = (skobject)->algorithm
+
+/*!
+ * Retrieve the cleartext key from a key object that is stored in a user
+ * keystore.
+ *
+ * @param      skobject     The Key Object to be queried.
+ * @param[out] skkey        A pointer to the location to store the key.  NULL
+ *                          if the key is not stored in a user keystore.
+ */
+#define fsl_shw_sko_get_key(skobject, skkey)                                  \
+{                                                                             \
+    fsl_shw_kso_t* keystore = (skobject)->keystore;                           \
+    if (keystore != NULL) {                                                   \
+        *(skkey) = keystore->slot_get_address(keystore->user_data,            \
+                                              (skobject)->handle);            \
+    } else {                                                                  \
+        *(skkey) = NULL;                                                      \
+    }                                                                         \
+}
+
+/*!
+ * Determine the size of a wrapped key based upon the cleartext key's length.
+ *
+ * This function can be used to calculate the number of octets that
+ * #fsl_shw_extract_key() will write into the location at @a covered_key.
+ *
+ * If zero is returned at @a length, this means that the key length in
+ * @a key_info is not supported.
+ *
+ * @param      wkeyinfo         Information about a key to be wrapped.
+ * @param      wkeylen          Location to store the length of a wrapped
+ *                              version of the key in @a key_info.
+ */
+#define fsl_shw_sko_calculate_wrapped_size(wkeyinfo, wkeylen)           \
+{                                                                       \
+    register fsl_shw_sko_t* kp = wkeyinfo;                              \
+    register uint32_t kl = kp->key_length;                              \
+    int key_blocks = (kl + 15) / 16;                                    \
+    int base_size = 35; /* ICV + T' + ALG + LEN + FLAGS */              \
+                                                                        \
+    *(wkeylen) = base_size + 16 * key_blocks;                           \
+}
+
+/*!
+ * Set some flags in the key object.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param skobject     A variable of type #fsl_shw_sko_t.
+ * @param skflags      (One or more) ORed members of #fsl_shw_key_flags_t which
+ *                     are to be set.
+ */
+#define fsl_shw_sko_set_flags(skobject, skflags)                              \
+      (skobject)->flags |= (skflags)
+
+/*!
+ * Clear some flags in the key object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param skobject      A variable of type #fsl_shw_sko_t.
+ * @param skflags       (One or more) ORed members of #fsl_shw_key_flags_t
+ *                      which are to be reset.
+ */
+#define fsl_shw_sko_clear_flags(skobject, skflags)                            \
+      (skobject)->flags &= ~(skflags)
+
+/*!
+ * Initialize a Hash Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  It sets the current message length and hash algorithm in the hash
+ * context object.
+ *
+ * @param      hcobject    The hash context to operate upon.
+ * @param      hcalgorithm The hash algorithm to be used (#FSL_HASH_ALG_MD5,
+ *                         #FSL_HASH_ALG_SHA256, etc).
+ *
+ */
+#define fsl_shw_hco_init(hcobject, hcalgorithm)                               \
+{                                                                             \
+     (hcobject)->algorithm = hcalgorithm;                                     \
+     (hcobject)->flags = 0;                                                   \
+     switch (hcalgorithm) {                                                   \
+     case FSL_HASH_ALG_MD5:                                                   \
+         (hcobject)->digest_length = 16;                                      \
+         (hcobject)->context_length = 16;                                     \
+         (hcobject)->context_register_length = 24;                            \
+         break;                                                               \
+     case FSL_HASH_ALG_SHA1:                                                  \
+         (hcobject)->digest_length = 20;                                      \
+         (hcobject)->context_length = 20;                                     \
+         (hcobject)->context_register_length = 24;                            \
+         break;                                                               \
+     case FSL_HASH_ALG_SHA224:                                                \
+         (hcobject)->digest_length = 28;                                      \
+         (hcobject)->context_length = 32;                                     \
+         (hcobject)->context_register_length = 36;                            \
+         break;                                                               \
+     case FSL_HASH_ALG_SHA256:                                                \
+         (hcobject)->digest_length = 32;                                      \
+         (hcobject)->context_length = 32;                                     \
+         (hcobject)->context_register_length = 36;                            \
+         break;                                                               \
+     default:                                                                 \
+         /* error ! */                                                        \
+         (hcobject)->digest_length = 1;                                       \
+         (hcobject)->context_length = 1;                                      \
+         (hcobject)->context_register_length = 1;                             \
+         break;                                                               \
+     }                                                                        \
+}
+
+/*!
+ * Get the current hash value and message length from the hash context object.
+ *
+ * The algorithm must have already been specified.  See #fsl_shw_hco_init().
+ *
+ * @param      hcobject   The hash context to query.
+ * @param[out] hccontext  Pointer to the location of @a length octets where to
+ *                        store a copy of the current value of the digest.
+ * @param      hcclength  Number of octets of hash value to copy.
+ * @param[out] hcmsglen   Pointer to the location to store the number of octets
+ *                        already hashed.
+ */
+#define fsl_shw_hco_get_digest(hcobject, hccontext, hcclength, hcmsglen)      \
+{                                                                             \
+     copy_bytes(hccontext, (hcobject)->context, hcclength);                   \
+         if ((hcobject)->algorithm == FSL_HASH_ALG_SHA224                     \
+             || (hcobject)->algorithm == FSL_HASH_ALG_SHA256) {               \
+             *(hcmsglen) = (hcobject)->context[8];                            \
+         } else {                                                             \
+             *(hcmsglen) = (hcobject)->context[5];                            \
+         }                                                                    \
+}
+
+/*!
+ * Get the hash algorithm from the hash context object.
+ *
+ * @param      hcobject    The hash context to query.
+ * @param[out] hcalgorithm Pointer to where the algorithm is to be stored.
+ */
+#define fsl_shw_hco_get_info(hcobject, hcalgorithm)                           \
+{                                                                             \
+     *(hcalgorithm) = (hcobject)->algorithm;                                  \
+}
+
+/*!
+ * Set the current hash value and message length in the hash context object.
+ *
+ * The algorithm must have already been specified.  See #fsl_shw_hco_init().
+ *
+ * @param      hcobject  The hash context to operate upon.
+ * @param      hccontext Pointer to buffer of appropriate length to copy into
+ *                       the hash context object.
+ * @param      hcmsglen  The number of octets of the message which have
+ *                        already been hashed.
+ *
+ */
+#define fsl_shw_hco_set_digest(hcobject, hccontext, hcmsglen)                 \
+{                                                                             \
+     copy_bytes((hcobject)->context, hccontext, (hcobject)->context_length);  \
+     if (((hcobject)->algorithm == FSL_HASH_ALG_SHA224)                       \
+         || ((hcobject)->algorithm == FSL_HASH_ALG_SHA256)) {                 \
+         (hcobject)->context[8] = hcmsglen;                                   \
+     } else {                                                                 \
+         (hcobject)->context[5] = hcmsglen;                                   \
+     }                                                                        \
+}
+
+/*!
+ * Set flags in a Hash Context Object.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hcobject   The hash context to be operated on.
+ * @param hcflags    The flags to be set in the context.  These can be ORed
+ *                   members of #fsl_shw_hash_ctx_flags_t.
+ */
+#define fsl_shw_hco_set_flags(hcobject, hcflags)                              \
+      (hcobject)->flags |= (hcflags)
+
+/*!
+ * Clear flags in a Hash Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hcobject   The hash context to be operated on.
+ * @param hcflags    The flags to be reset in the context.  These can be ORed
+ *                   members of #fsl_shw_hash_ctx_flags_t.
+ */
+#define fsl_shw_hco_clear_flags(hcobject, hcflags)                            \
+      (hcobject)->flags &= ~(hcflags)
+
+/*!
+ * Initialize an HMAC Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  It sets the current message length and hash algorithm in the HMAC
+ * context object.
+ *
+ * @param      hcobject    The HMAC context to operate upon.
+ * @param      hcalgorithm The hash algorithm to be used (#FSL_HASH_ALG_MD5,
+ *                         #FSL_HASH_ALG_SHA256, etc).
+ *
+ */
+#define fsl_shw_hmco_init(hcobject, hcalgorithm)                              \
+    fsl_shw_hco_init(hcobject, hcalgorithm)
+
+/*!
+ * Set flags in an HMAC Context Object.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hcobject   The HMAC context to be operated on.
+ * @param hcflags    The flags to be set in the context.  These can be ORed
+ *                   members of #fsl_shw_hmac_ctx_flags_t.
+ */
+#define fsl_shw_hmco_set_flags(hcobject, hcflags)                             \
+      (hcobject)->flags |= (hcflags)
+
+/*!
+ * Clear flags in an HMAC Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param hcobject   The HMAC context to be operated on.
+ * @param hcflags    The flags to be reset in the context.  These can be ORed
+ *                   members of #fsl_shw_hmac_ctx_flags_t.
+ */
+#define fsl_shw_hmco_clear_flags(hcobject, hcflags)                           \
+      (hcobject)->flags &= ~(hcflags)
+
+/*!
+ * Initialize a Symmetric Cipher Context Object.
+ *
+ * This function must be called before performing any other operation with the
+ * Object.  This will set the @a mode and @a algorithm and initialize the
+ * Object.
+ *
+ * @param scobject  The context object to operate on.
+ * @param scalg     The cipher algorithm this context will be used with.
+ * @param scmode    #FSL_SYM_MODE_CBC, #FSL_SYM_MODE_ECB, etc.
+ *
+ */
+#define fsl_shw_scco_init(scobject, scalg, scmode)                            \
+{                                                                             \
+      register uint32_t bsb;   /* block-size bytes */                         \
+                                                                              \
+      switch (scalg) {                                                        \
+      case FSL_KEY_ALG_AES:                                                   \
+          bsb = 16;                                                           \
+          break;                                                              \
+      case FSL_KEY_ALG_DES:                                                   \
+          /* fall through */                                                  \
+      case FSL_KEY_ALG_TDES:                                                  \
+          bsb = 8;                                                            \
+          break;                                                              \
+      case FSL_KEY_ALG_ARC4:                                                  \
+          bsb = 259;                                                          \
+          break;                                                              \
+      case FSL_KEY_ALG_HMAC:                                                  \
+          bsb = 1;  /* meaningless */                                         \
+          break;                                                              \
+      default:                                                                \
+          bsb = 00;                                                           \
+      }                                                                       \
+      (scobject)->block_size_bytes = bsb;                                     \
+      (scobject)->mode = scmode;                                              \
+      (scobject)->flags = 0;                                                  \
+}
+
+/*!
+ * Set the flags for a Symmetric Cipher Context.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param scobject The context object to operate on.
+ * @param scflags  The flags to reset (one or more values from
+ *                 #fsl_shw_sym_ctx_flags_t ORed together).
+ *
+ */
+#define fsl_shw_scco_set_flags(scobject, scflags)                             \
+       (scobject)->flags |= (scflags)
+
+/*!
+ * Clear some flags in a Symmetric Cipher Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param scobject The context object to operate on.
+ * @param scflags  The flags to reset (one or more values from
+ *                 #fsl_shw_sym_ctx_flags_t ORed together).
+ *
+ */
+#define fsl_shw_scco_clear_flags(scobject, scflags)                           \
+       (scobject)->flags &= ~(scflags)
+
+/*!
+ * Set the Context (IV) for a Symmetric Cipher Context.
+ *
+ * This is to set the context/IV for #FSL_SYM_MODE_CBC mode, or to set the
+ * context (the S-Box and pointers) for ARC4.  The full context size will
+ * be copied.
+ *
+ * @param scobject  The context object to operate on.
+ * @param sccontext A pointer to the buffer which contains the context.
+ *
+ */
+#define fsl_shw_scco_set_context(scobject, sccontext)                         \
+       copy_bytes((scobject)->context, sccontext,                             \
+                  (scobject)->block_size_bytes)
+
+/*!
+ * Get the Context for a Symmetric Cipher Context.
+ *
+ * This is to retrieve the context/IV for #FSL_SYM_MODE_CBC mode, or to
+ * retrieve context (the S-Box and pointers) for ARC4.  The full context
+ * will be copied.
+ *
+ * @param      scobject  The context object to operate on.
+ * @param[out] sccontext Pointer to location where context will be stored.
+ */
+#define fsl_shw_scco_get_context(scobject, sccontext)                         \
+       copy_bytes(sccontext, (scobject)->context, (scobject)->block_size_bytes)
+
+/*!
+ * Set the Counter Value for a Symmetric Cipher Context.
+ *
+ * This will set the Counter Value for CTR mode.
+ *
+ * @param scobject  The context object to operate on.
+ * @param sccounter The starting counter value.  The number of octets.
+ *                  copied will be the block size for the algorithm.
+ * @param scmodulus The modulus for controlling the incrementing of the
+ *                  counter.
+ *
+ */
+#define fsl_shw_scco_set_counter_info(scobject, sccounter, scmodulus)        \
+       {                                                                     \
+           if ((sccounter) != NULL) {                                        \
+               copy_bytes((scobject)->context, sccounter,                    \
+                          (scobject)->block_size_bytes);                     \
+           }                                                                 \
+           (scobject)->modulus_exp = scmodulus;                              \
+       }
+
+/*!
+ * Get the Counter Value for a Symmetric Cipher Context.
+ *
+ * This will retrieve the Counter Value is for CTR mode.
+ *
+ * @param     scobject    The context object to query.
+ * @param[out] sccounter  Pointer to location to store the current counter
+ *                        value.  The number of octets copied will be the
+ *                        block size for the algorithm.
+ * @param[out] scmodulus  Pointer to location to store the modulus.
+ *
+ */
+#define fsl_shw_scco_get_counter_info(scobject, sccounter, scmodulus)        \
+       {                                                                     \
+           if ((sccounter) != NULL) {                                        \
+               copy_bytes(sccounter, (scobject)->context,                    \
+                          (scobject)->block_size_bytes);                     \
+           }                                                                 \
+           if ((scmodulus) != NULL) {                                        \
+               *(scmodulus) = (scobject)->modulus_exp;                       \
+           }                                                                 \
+       }
+
+/*!
+ * Initialize a Authentication-Cipher Context.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param acmode    The mode for this object (only #FSL_ACC_MODE_CCM
+ *                  supported).
+ */
+#define fsl_shw_acco_init(acobject, acmode)                                   \
+   {                                                                          \
+       (acobject)->flags = 0;                                                 \
+       (acobject)->mode = (acmode);                                           \
+   }
+
+/*!
+ * Set the flags for a Authentication-Cipher Context.
+ *
+ * Turns on the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param acflags   The flags to set (one or more from
+ *                  #fsl_shw_auth_ctx_flags_t ORed together).
+ *
+ */
+#define fsl_shw_acco_set_flags(acobject, acflags)                             \
+       (acobject)->flags |= (acflags)
+
+/*!
+ * Clear some flags in a Authentication-Cipher Context Object.
+ *
+ * Turns off the flags specified in @a flags.  Other flags are untouched.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param acflags   The flags to reset (one or more from
+ *                  #fsl_shw_auth_ctx_flags_t ORed together).
+ *
+ */
+#define fsl_shw_acco_clear_flags(acobject, acflags)                           \
+       (acobject)->flags &= ~(acflags)
+
+/*!
+ * Set up the Authentication-Cipher Object for CCM mode.
+ *
+ * This will set the @a auth_object for CCM mode and save the @a ctr,
+ * and @a mac_length.  This function can be called instead of
+ * #fsl_shw_acco_init().
+ *
+ * The paramater @a ctr is Counter Block 0, (counter value 0), which is for the
+ * MAC.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param acalg     Cipher algorithm.  Only AES is supported.
+ * @param accounter The initial counter value.
+ * @param acmaclen  The number of octets used for the MAC.  Valid values are
+ *                  4, 6, 8, 10, 12, 14, and 16.
+ */
+/* Do we need to stash the +1 value of the CTR somewhere? */
+#define fsl_shw_acco_set_ccm(acobject, acalg, accounter, acmaclen)            \
+{                                                                             \
+      (acobject)->flags = 0;                                                  \
+      (acobject)->mode = FSL_ACC_MODE_CCM;                                    \
+      (acobject)->auth_info.CCM_ctx_info.block_size_bytes = 16;               \
+      (acobject)->cipher_ctx_info.block_size_bytes = 16;                      \
+      (acobject)->mac_length = acmaclen;                                      \
+      fsl_shw_scco_set_counter_info(&(acobject)->cipher_ctx_info, accounter,  \
+            FSL_CTR_MOD_128);                                                 \
+}
+
+/*!
+ * Format the First Block (IV) & Initial Counter Value per NIST CCM.
+ *
+ * This function will also set the IV and CTR values per Appendix A of NIST
+ * Special Publication 800-38C (May 2004).  It will also perform the
+ * #fsl_shw_acco_set_ccm() operation with information derived from this set of
+ * parameters.
+ *
+ * Note this function assumes the algorithm is AES.  It initializes the
+ * @a auth_object by setting the mode to #FSL_ACC_MODE_CCM and setting the
+ * flags to be #FSL_ACCO_NIST_CCM.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param act       The number of octets used for the MAC.  Valid values are
+ *                  4, 6, 8, 10, 12, 14, and 16.
+ * @param acad      Number of octets of Associated Data (may be zero).
+ * @param acq       A value for the size of the length of @a q field.  Valid
+ *                  values are 1-8.
+ * @param acN       The Nonce (packet number or other changing value). Must
+ *                  be (15 - @a q_length) octets long.
+ * @param acQ       The value of Q (size of the payload in octets).
+ *
+ */
+/* Do we need to stash the +1 value of the CTR somewhere? */
+#define fsl_shw_ccm_nist_format_ctr_and_iv(acobject, act, acad, acq, acN, acQ)\
+    {                                                                         \
+        uint64_t Q = acQ;                                                     \
+        uint8_t bflag = ((acad)?0x40:0) | ((((act)-2)/2)<<3) | ((acq)-1);     \
+        unsigned i;                                                           \
+        uint8_t* qptr = (acobject)->auth_info.CCM_ctx_info.context + 15;      \
+        (acobject)->auth_info.CCM_ctx_info.block_size_bytes = 16;             \
+        (acobject)->cipher_ctx_info.block_size_bytes = 16;                    \
+        (acobject)->mode = FSL_ACC_MODE_CCM;                                  \
+        (acobject)->flags  = FSL_ACCO_NIST_CCM;                               \
+                                                                              \
+        /* Store away the MAC length (after calculating actual value */       \
+        (acobject)->mac_length = (act);                                       \
+        /* Set Flag field in Block 0 */                                       \
+        *((acobject)->auth_info.CCM_ctx_info.context) = bflag;                \
+        /* Set Nonce field in Block 0 */                                      \
+        copy_bytes((acobject)->auth_info.CCM_ctx_info.context+1, acN,         \
+                   15-(acq));                                                 \
+        /* Set Flag field in ctr */                                           \
+        *((acobject)->cipher_ctx_info.context) = (acq)-1;                     \
+        /* Update the Q (payload length) field of Block0 */                   \
+        (acobject)->q_length = acq;                                           \
+        for (i = 0; i < (acq); i++) {                                         \
+            *qptr-- = Q & 0xFF;                                               \
+            Q >>= 8;                                                          \
+        }                                                                     \
+        /* Set the Nonce field of the ctr */                                  \
+        copy_bytes((acobject)->cipher_ctx_info.context+1, acN, 15-(acq));     \
+        /* Clear the block counter field of the ctr */                        \
+        memset((acobject)->cipher_ctx_info.context+16-(acq), 0, (acq)+1);     \
+     }
+
+/*!
+ * Update the First Block (IV) & Initial Counter Value per NIST CCM.
+ *
+ * This function will set the IV and CTR values per Appendix A of NIST Special
+ * Publication 800-38C (May 2004).
+ *
+ * Note this function assumes that #fsl_shw_ccm_nist_format_ctr_and_iv() has
+ * previously been called on the @a auth_object.
+ *
+ * @param acobject  Pointer to object to operate on.
+ * @param acN       The Nonce (packet number or other changing value). Must
+ *                  be (15 - @a q_length) octets long.
+ * @param acQ       The value of Q (size of the payload in octets).
+ *
+ */
+/* Do we need to stash the +1 value of the CTR somewhere? */
+#define fsl_shw_ccm_nist_update_ctr_and_iv(acobject, acN, acQ)                \
+     {                                                                        \
+        uint64_t Q = acQ;                                                     \
+        unsigned i;                                                           \
+        uint8_t* qptr = (acobject)->auth_info.CCM_ctx_info.context + 15;      \
+                                                                              \
+        /* Update the Nonce field field of Block0 */                          \
+        copy_bytes((acobject)->auth_info.CCM_ctx_info.context+1, acN,         \
+               15 - (acobject)->q_length);                                    \
+        /* Update the Q (payload length) field of Block0 */                   \
+        for (i = 0; i < (acobject)->q_length; i++) {                          \
+            *qptr-- = Q & 0xFF;                                               \
+            Q >>= 8;                                                          \
+        }                                                                     \
+        /* Update the Nonce field of the ctr */                               \
+        copy_bytes((acobject)->cipher_ctx_info.context+1, acN,                \
+               15 - (acobject)->q_length);                                    \
+     }
+
+/******************************************************************************
+ * Library functions
+ *****************************************************************************/
+/* REQ-S2LRD-PINTFC-API-GEN-003 */
+extern fsl_shw_pco_t *fsl_shw_get_capabilities(fsl_shw_uco_t * user_ctx);
+
+/* REQ-S2LRD-PINTFC-API-GEN-004 */
+extern fsl_shw_return_t fsl_shw_register_user(fsl_shw_uco_t * user_ctx);
+
+/* REQ-S2LRD-PINTFC-API-GEN-005 */
+extern fsl_shw_return_t fsl_shw_deregister_user(fsl_shw_uco_t * user_ctx);
+
+/* REQ-S2LRD-PINTFC-API-GEN-006 */
+extern fsl_shw_return_t fsl_shw_get_results(fsl_shw_uco_t * user_ctx,
+					    unsigned result_size,
+					    fsl_shw_result_t results[],
+					    unsigned *result_count);
+
+extern fsl_shw_return_t fsl_shw_establish_key(fsl_shw_uco_t * user_ctx,
+					      fsl_shw_sko_t * key_info,
+					      fsl_shw_key_wrap_t establish_type,
+					      const uint8_t * key);
+
+extern fsl_shw_return_t fsl_shw_extract_key(fsl_shw_uco_t * user_ctx,
+					    fsl_shw_sko_t * key_info,
+					    uint8_t * covered_key);
+
+extern fsl_shw_return_t fsl_shw_release_key(fsl_shw_uco_t * user_ctx,
+					    fsl_shw_sko_t * key_info);
+
+extern void *fsl_shw_smalloc(fsl_shw_uco_t * user_ctx,
+			     uint32_t size,
+			     const uint8_t * UMID, uint32_t permissions);
+
+extern fsl_shw_return_t fsl_shw_sfree(fsl_shw_uco_t * user_ctx, void *address);
+
+extern fsl_shw_return_t fsl_shw_sstatus(fsl_shw_uco_t * user_ctx,
+					void *address,
+					fsl_shw_partition_status_t * status);
+
+extern fsl_shw_return_t fsl_shw_diminish_perms(fsl_shw_uco_t * user_ctx,
+					       void *address,
+					       uint32_t permissions);
+
+extern fsl_shw_return_t do_scc_engage_partition(fsl_shw_uco_t * user_ctx,
+						void *address,
+						const uint8_t * UMID,
+						uint32_t permissions);
+
+extern fsl_shw_return_t do_system_keystore_slot_alloc(fsl_shw_uco_t * user_ctx,
+						      uint32_t key_lenth,
+						      uint64_t ownerid,
+						      uint32_t * slot);
+
+extern fsl_shw_return_t do_system_keystore_slot_dealloc(fsl_shw_uco_t *
+							user_ctx,
+							uint64_t ownerid,
+							uint32_t slot);
+
+extern fsl_shw_return_t do_system_keystore_slot_load(fsl_shw_uco_t * user_ctx,
+						     uint64_t ownerid,
+						     uint32_t slot,
+						     const uint8_t * key,
+						     uint32_t key_length);
+
+extern fsl_shw_return_t do_system_keystore_slot_read(fsl_shw_uco_t * user_ctx,
+						     uint64_t ownerid,
+						     uint32_t slot,
+						     uint32_t key_length,
+						     const uint8_t * key);
+
+extern fsl_shw_return_t do_system_keystore_slot_encrypt(fsl_shw_uco_t *
+							user_ctx,
+							uint64_t ownerid,
+							uint32_t slot,
+							uint32_t key_length,
+							uint8_t * black_data);
+
+extern fsl_shw_return_t do_system_keystore_slot_decrypt(fsl_shw_uco_t *
+							user_ctx,
+							uint64_t ownerid,
+							uint32_t slot,
+							uint32_t key_length,
+							const uint8_t *
+							black_data);
+
+extern fsl_shw_return_t
+do_scc_encrypt_region(fsl_shw_uco_t * user_ctx,
+		      void *partition_base, uint32_t offset_bytes,
+		      uint32_t byte_count, uint8_t * black_data,
+		      uint32_t * IV, fsl_shw_cypher_mode_t cypher_mode);
+
+extern fsl_shw_return_t
+do_scc_decrypt_region(fsl_shw_uco_t * user_ctx,
+		      void *partition_base, uint32_t offset_bytes,
+		      uint32_t byte_count, const uint8_t * black_data,
+		      uint32_t * IV, fsl_shw_cypher_mode_t cypher_mode);
+
+extern fsl_shw_return_t
+system_keystore_get_slot_info(uint64_t owner_id, uint32_t slot,
+			      uint32_t * address, uint32_t * slot_size_bytes);
+
+/* REQ-S2LRD-PINTFC-API-BASIC-SYM-002 */
+/* PINTFC-API-BASIC-SYM-ARC4-001 */
+/* PINTFC-API-BASIC-SYM-ARC4-002 */
+extern fsl_shw_return_t fsl_shw_symmetric_encrypt(fsl_shw_uco_t * user_ctx,
+						  fsl_shw_sko_t * key_info,
+						  fsl_shw_scco_t * sym_ctx,
+						  uint32_t length,
+						  const uint8_t * pt,
+						  uint8_t * ct);
+
+/* PINTFC-API-BASIC-SYM-002 */
+/* PINTFC-API-BASIC-SYM-ARC4-001 */
+/* PINTFC-API-BASIC-SYM-ARC4-002 */
+extern fsl_shw_return_t fsl_shw_symmetric_decrypt(fsl_shw_uco_t * user_ctx,
+						  fsl_shw_sko_t * key_info,
+						  fsl_shw_scco_t * sym_ctx,
+						  uint32_t length,
+						  const uint8_t * ct,
+						  uint8_t * pt);
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HASH-005 */
+extern fsl_shw_return_t fsl_shw_hash(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_hco_t * hash_ctx,
+				     const uint8_t * msg,
+				     uint32_t length,
+				     uint8_t * result, uint32_t result_len);
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HMAC-001 */
+extern fsl_shw_return_t fsl_shw_hmac_precompute(fsl_shw_uco_t * user_ctx,
+						fsl_shw_sko_t * key_info,
+						fsl_shw_hmco_t * hmac_ctx);
+
+/* REQ-S2LRD-PINTFC-API-BASIC-HMAC-002 */
+extern fsl_shw_return_t fsl_shw_hmac(fsl_shw_uco_t * user_ctx,
+				     fsl_shw_sko_t * key_info,
+				     fsl_shw_hmco_t * hmac_ctx,
+				     const uint8_t * msg,
+				     uint32_t length,
+				     uint8_t * result, uint32_t result_len);
+
+/* REQ-S2LRD-PINTFC-API-BASIC-RNG-002 */
+extern fsl_shw_return_t fsl_shw_get_random(fsl_shw_uco_t * user_ctx,
+					   uint32_t length, uint8_t * data);
+
+extern fsl_shw_return_t fsl_shw_add_entropy(fsl_shw_uco_t * user_ctx,
+					    uint32_t length, uint8_t * data);
+
+extern fsl_shw_return_t fsl_shw_gen_encrypt(fsl_shw_uco_t * user_ctx,
+					    fsl_shw_acco_t * auth_ctx,
+					    fsl_shw_sko_t * cipher_key_info,
+					    fsl_shw_sko_t * auth_key_info,
+					    uint32_t auth_data_length,
+					    const uint8_t * auth_data,
+					    uint32_t payload_length,
+					    const uint8_t * payload,
+					    uint8_t * ct, uint8_t * auth_value);
+
+extern fsl_shw_return_t fsl_shw_auth_decrypt(fsl_shw_uco_t * user_ctx,
+					     fsl_shw_acco_t * auth_ctx,
+					     fsl_shw_sko_t * cipher_key_info,
+					     fsl_shw_sko_t * auth_key_info,
+					     uint32_t auth_data_length,
+					     const uint8_t * auth_data,
+					     uint32_t payload_length,
+					     const uint8_t * ct,
+					     const uint8_t * auth_value,
+					     uint8_t * payload);
+
+extern fsl_shw_return_t fsl_shw_read_key(fsl_shw_uco_t * user_ctx,
+						fsl_shw_sko_t * key_info,
+						uint8_t * key);
+
+static inline fsl_shw_return_t fsl_shw_gen_random_pf_key(fsl_shw_uco_t *
+							 user_ctx)
+{
+	(void)user_ctx;
+
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+static inline fsl_shw_return_t fsl_shw_read_tamper_event(fsl_shw_uco_t *
+							 user_ctx,
+							 fsl_shw_tamper_t *
+							 tamperp,
+							 uint64_t * timestampp)
+{
+	(void)user_ctx;
+	(void)tamperp;
+	(void)timestampp;
+
+	return FSL_RETURN_NO_RESOURCE_S;
+}
+
+fsl_shw_return_t sah_Append_Desc(const sah_Mem_Util * mu,
+				 sah_Head_Desc ** desc_head,
+				 const uint32_t header,
+				 sah_Link * link1, sah_Link * link2);
+
+/* Utility Function leftover from sahara1 API */
+void sah_Descriptor_Chain_Destroy(const sah_Mem_Util * mu,
+				  sah_Head_Desc ** desc_chain);
+
+/* Utility Function leftover from sahara1 API */
+fsl_shw_return_t sah_Descriptor_Chain_Execute(sah_Head_Desc * desc_chain,
+					      fsl_shw_uco_t * user_ctx);
+
+fsl_shw_return_t sah_Append_Link(const sah_Mem_Util * mu,
+				 sah_Link * link,
+				 uint8_t * p,
+				 const size_t length,
+				 const sah_Link_Flags flags);
+
+fsl_shw_return_t sah_Create_Link(const sah_Mem_Util * mu,
+				 sah_Link ** link,
+				 uint8_t * p,
+				 const size_t length,
+				 const sah_Link_Flags flags);
+
+fsl_shw_return_t sah_Create_Key_Link(const sah_Mem_Util * mu,
+				     sah_Link ** link,
+				     fsl_shw_sko_t * key_info);
+
+void sah_Destroy_Link(const sah_Mem_Util * mu, sah_Link * link);
+
+void sah_Postprocess_Results(fsl_shw_uco_t * user_ctx,
+			     sah_results * result_info);
+
+#endif				/* SAHARA2_API_H */
+
diff --git a/drivers/mxc/security/sahara2/include/sahara2_kernel.h b/drivers/mxc/security/sahara2/include/sahara2_kernel.h
new file mode 100644
index 000000000000..b833f0ab8f56
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/sahara2_kernel.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#define DRIVER_NAME sahara2
+
+#define SAHARA_MAJOR_NODE 78
+
+#include "portable_os.h"
+
+#include "platform_abstractions.h"
+
+/* Forward-declare prototypes using signature macros */
+
+OS_DEV_ISR_DCL(sahara2_isr);
+
+OS_DEV_INIT_DCL(sahara2_init);
+
+OS_DEV_SHUTDOWN_DCL(sahara2_shutdown);
+
+OS_DEV_OPEN_DCL(sahara2_open);
+
+OS_DEV_CLOSE_DCL(sahara2_release);
+
+OS_DEV_IOCTL_DCL(sahara2_ioctl);
+
+struct sahara2_kernel_user {
+	void *command_ring[32];
+};
+
+struct sahara2_sym_arg {
+	char *key;
+	unsigned key_len;
+};
+
+/*! These need to be added to Linux / OS abstractions */
+/*
+module_init(OS_DEV_INIT_REF(sahara2_init));
+module_cleanup(OS_DEV_SHUTDOWN_REF(sahara2_shutdown));
+*/
diff --git a/drivers/mxc/security/sahara2/include/sf_util.h b/drivers/mxc/security/sahara2/include/sf_util.h
new file mode 100644
index 000000000000..c0af0c96ff02
--- /dev/null
+++ b/drivers/mxc/security/sahara2/include/sf_util.h
@@ -0,0 +1,466 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+* @file sf_util.h
+*
+* @brief Header for Sahara Descriptor-chain building Functions
+*/
+#ifndef SF_UTIL_H
+#define SF_UTIL_H
+
+#include <fsl_platform.h>
+#include <sahara.h>
+
+/*! Header value for Sahara Descriptor  1 */
+#define SAH_HDR_SKHA_SET_MODE_IV_KEY  0x10880000
+/*! Header value for Sahara Descriptor  2 */
+#define SAH_HDR_SKHA_SET_MODE_ENC_DEC 0x108D0000
+/*! Header value for Sahara Descriptor  4 */
+#define SAH_HDR_SKHA_ENC_DEC          0x90850000
+/*! Header value for Sahara Descriptor  5 */
+#define SAH_HDR_SKHA_READ_CONTEXT_IV  0x10820000
+/*! Header value for Sahara Descriptor  6 */
+#define SAH_HDR_MDHA_SET_MODE_MD_KEY  0x20880000
+/*! Header value for Sahara Descriptor  8 */
+#define SAH_HDR_MDHA_SET_MODE_HASH    0x208D0000
+/*! Header value for Sahara Descriptor  10 */
+#define SAH_HDR_MDHA_HASH             0xA0850000
+/*! Header value for Sahara Descriptor  11 */
+#define SAH_HDR_MDHA_STORE_DIGEST     0x20820000
+/*! Header value for Sahara Descriptor  18 */
+#define SAH_HDR_RNG_GENERATE          0x308C0000
+/*! Header value for Sahara Descriptor 19 */
+#define SAH_HDR_PKHA_LD_N_E           0xC0800000
+/*! Header value for Sahara Descriptor 20 */
+#define SAH_HDR_PKHA_LD_A_EX_ST_B     0x408D0000
+/*! Header value for Sahara Descriptor 21 */
+#define SAH_HDR_PKHA_LD_N_EX_ST_B     0x408E0000
+/*! Header value for Sahara Descriptor 22 */
+#define SAH_HDR_PKHA_LD_A_B           0xC0830000
+/*! Header value for Sahara Descriptor 23 */
+#define SAH_HDR_PKHA_LD_A0_A1         0x40840000
+/*! Header value for Sahara Descriptor 24 */
+#define SAH_HDR_PKHA_LD_A2_A3         0xC0850000
+/*! Header value for Sahara Descriptor 25 */
+#define SAH_HDR_PKHA_LD_B0_B1         0xC0860000
+/*! Header value for Sahara Descriptor 26 */
+#define SAH_HDR_PKHA_LD_B2_B3         0x40870000
+/*! Header value for Sahara Descriptor 27 */
+#define SAH_HDR_PKHA_ST_A_B           0x40820000
+/*! Header value for Sahara Descriptor 28 */
+#define SAH_HDR_PKHA_ST_A0_A1         0x40880000
+/*! Header value for Sahara Descriptor 29 */
+#define SAH_HDR_PKHA_ST_A2_A3         0xC0890000
+/*! Header value for Sahara Descriptor 30 */
+#define SAH_HDR_PKHA_ST_B0_B1         0xC08A0000
+/*! Header value for Sahara Descriptor 31 */
+#define SAH_HDR_PKHA_ST_B2_B3         0x408B0000
+/*! Header value for Sahara Descriptor 32 */
+#define SAH_HDR_PKHA_EX_ST_B1         0xC08C0000
+/*! Header value for Sahara Descriptor 33 */
+#define SAH_HDR_ARC4_SET_MODE_SBOX    0x90890000
+/*! Header value for Sahara Descriptor 34 */
+#define SAH_HDR_ARC4_READ_SBOX        0x90860000
+/*! Header value for Sahara Descriptor 35 */
+#define SAH_HDR_ARC4_SET_MODE_KEY     0x90830000
+/*! Header value for Sahara Descriptor 36 */
+#define SAH_HDR_PKHA_LD_A3_B0         0x40810000
+/*! Header value for Sahara Descriptor 37 */
+#define SAH_HDR_PKHA_ST_B1_B2         0xC08F0000
+/*! Header value for Sahara Descriptor 38 */
+#define SAH_HDR_SKHA_CBC_ICV          0x10840000
+/*! Header value for Sahara Descriptor 39 */
+#define SAH_HDR_MDHA_ICV_CHECK        0xA08A0000
+
+/*! Header bit indicating "Link-List optimization" */
+#define SAH_HDR_LLO                   0x01000000
+
+#define SAH_SF_DCLS                                                         \
+    fsl_shw_return_t ret;                                                   \
+    unsigned sf_executed = 0;                                               \
+    sah_Head_Desc* desc_chain = NULL;                                       \
+    uint32_t header
+
+#define SAH_SF_USER_CHECK()                                                 \
+do {                                                                        \
+    ret = sah_validate_uco(user_ctx);                                       \
+    if (ret != FSL_RETURN_OK_S) {                                           \
+        goto out;                                                           \
+    }                                                                       \
+} while (0)
+
+#define SAH_SF_EXECUTE()                                                    \
+do {                                                                        \
+    sf_executed = 1;                                                        \
+    ret = sah_Descriptor_Chain_Execute(desc_chain, user_ctx);               \
+} while (0)
+
+#define SAH_SF_DESC_CLEAN()                                                 \
+do {                                                                        \
+    if (!sf_executed || (user_ctx->flags & FSL_UCO_BLOCKING_MODE)) {        \
+        sah_Descriptor_Chain_Destroy(user_ctx->mem_util, &desc_chain);      \
+    }                                                                       \
+    (void) header;                                                          \
+} while (0)
+
+/*! Add Descriptor with two inputs */
+#define DESC_IN_IN(hdr, len1, ptr1, len2, ptr2)                             \
+{                                                                           \
+    ret = sah_add_two_in_desc(hdr, ptr1, len1, ptr2, len2,                  \
+                              user_ctx->mem_util, &desc_chain);             \
+    if (ret != FSL_RETURN_OK_S) {                                           \
+        goto out;                                                           \
+    }                                                                       \
+}
+
+/*! Add Descriptor with two vectors */
+#define DESC_D_D(hdr, len1, ptr1, len2, ptr2)                               \
+{                                                                           \
+    ret = sah_add_two_d_desc(hdr, ptr1, len1, ptr2, len2,                   \
+                              user_ctx->mem_util, &desc_chain);             \
+    if (ret != FSL_RETURN_OK_S) {                                           \
+        goto out;                                                           \
+    }                                                                       \
+}
+
+/*! Add Descriptor with input and a key */
+#define DESC_IN_KEY(hdr, len1, ptr1, key2)                                  \
+{                                                                           \
+    ret = sah_add_in_key_desc(hdr, ptr1, len1, key2,                        \
+                              user_ctx->mem_util, &desc_chain);             \
+    if (ret != FSL_RETURN_OK_S) {                                           \
+        goto out;                                                           \
+    }                                                                       \
+}
+
+/*! Add Descriptor with input and an output */
+#define DESC_IN_OUT(hdr, len1, ptr1, len2, ptr2)                            \
+{                                                                           \
+    ret = sah_add_in_out_desc(hdr, ptr1, len1, ptr2, len2,                  \
+                              user_ctx->mem_util, &desc_chain);             \
+    if (ret != FSL_RETURN_OK_S) {                                           \
+        goto out;                                                           \
+    }                                                                       \
+}
+
+/*! Add Descriptor with input and a key output */
+#define DESC_IN_KEYOUT(hdr, len1, ptr1, key2)                               \
+{                                                                           \
+    ret = sah_add_in_keyout_desc(hdr, ptr1, len1, key2,                     \
+                              user_ctx->mem_util, &desc_chain);             \
+    if (ret != FSL_RETURN_OK_S) {                                           \
+        goto out;                                                           \
+    }                                                                       \
+}
+
+/*! Add Descriptor with a key and an output */
+#define DESC_KEY_OUT(hdr, key1, len2, ptr2)                                 \
+{                                                                           \
+    ret = sah_add_key_out_desc(hdr, key1, ptr2, len2,                       \
+                              user_ctx->mem_util, &desc_chain);             \
+    if (ret != FSL_RETURN_OK_S) {                                           \
+        goto out;                                                           \
+    }                                                                       \
+}
+
+/*! Add Descriptor with two outputs */
+#define DESC_OUT_OUT(hdr, len1, ptr1, len2, ptr2)                           \
+{                                                                           \
+    ret = sah_add_two_out_desc(hdr, ptr1, len1, ptr2, len2,                 \
+                              user_ctx->mem_util, &desc_chain);             \
+    if (ret != FSL_RETURN_OK_S) {                                           \
+        goto out;                                                           \
+    }                                                                       \
+}
+
+/*! Add Descriptor with output then input pointers */
+#define DESC_OUT_IN(hdr, len1, ptr1, len2, ptr2)                            \
+{                                                                           \
+    ret = sah_add_out_in_desc(hdr, ptr1, len1, ptr2, len2,                  \
+                              user_ctx->mem_util, &desc_chain);             \
+    if (ret != FSL_RETURN_OK_S) {                                           \
+        goto out;                                                           \
+    }                                                                       \
+}
+
+#ifdef SAH_SF_DEBUG
+/*! Add Descriptor with two outputs */
+#define DBG_DESC(hdr, len1, ptr1, len2, ptr2)                               \
+{                                                                           \
+    ret = sah_add_two_out_desc(hdr, ptr1, len1, ptr2, len2,                 \
+                              user_ctx->mem_util, &desc_chain);             \
+    if (ret != FSL_RETURN_OK_S) {                                           \
+        goto out;                                                           \
+    }                                                                       \
+}
+#else
+#define DBG_DESC(hdr, len1, ptr1, len2, ptr2)
+#endif
+
+#ifdef __KERNEL__
+#define DESC_DBG_ON  ({console_loglevel = 8;})
+#define DESC_DBG_OFF ({console_loglevel = 7;})
+#else
+#define DESC_DBG_ON  system("echo 8 > /proc/sys/kernel/printk")
+#define DESC_DBG_OFF system("echo 7 > /proc/sys/kernel/printk")
+#endif
+
+#define DESC_TEMP_ALLOC(size)                                               \
+({                                                                          \
+    uint8_t* ptr;                                                           \
+    ptr = user_ctx->mem_util->mu_malloc(user_ctx->mem_util->mu_ref,         \
+                                                   size);                   \
+    if (ptr == NULL) {                                                      \
+        ret = FSL_RETURN_NO_RESOURCE_S;                                     \
+        goto out;                                                           \
+    }                                                                       \
+                                                                            \
+    ptr;                                                                    \
+})
+
+#define DESC_TEMP_FREE(ptr)                                                 \
+({                                                                          \
+    if ((ptr != NULL) &&                                                    \
+        (!sf_executed || (user_ctx->flags & FSL_UCO_BLOCKING_MODE))) {      \
+            user_ctx->mem_util->                                            \
+                mu_free(user_ctx->mem_util->mu_ref, ptr);                   \
+            ptr = NULL;                                                     \
+    }                                                                       \
+})
+
+/* Temporary implementation.  This needs to be in internal/secure RAM */
+#define DESC_TEMP_SECURE_ALLOC(size)                                        \
+({                                                                          \
+    uint8_t* ptr;                                                           \
+    ptr = user_ctx->mem_util->mu_malloc(user_ctx->mem_util->mu_ref,         \
+                                                   size);                   \
+    if (ptr == NULL) {                                                      \
+        ret = FSL_RETURN_NO_RESOURCE_S;                                     \
+        goto out;                                                           \
+    }                                                                       \
+                                                                            \
+    ptr;                                                                    \
+})
+
+#define DESC_TEMP_SECURE_FREE(ptr, size)                                    \
+({                                                                          \
+    if ((ptr != NULL) &&                                                    \
+        (!sf_executed || (user_ctx->flags & FSL_UCO_BLOCKING_MODE))) {      \
+            user_ctx->mem_util->mu_memset(user_ctx->mem_util->mu_ref,       \
+                                          ptr, 0, size);                    \
+                                                                            \
+            user_ctx->mem_util->                                            \
+                mu_free(user_ctx->mem_util->mu_ref, ptr);                   \
+            ptr = NULL;                                                     \
+    }                                                                       \
+})
+
+extern const uint32_t sah_insert_mdha_algorithm[];
+
+/*! @defgroup mdhaflags MDHA Mode Register Values
+ *
+ * These are bit fields and combinations of bit fields for setting the Mode
+ * Register portion of a Sahara Descriptor Header field.
+ *
+ * The parity bit has been set to ensure that these values have even parity,
+ * therefore using an Exclusive-OR operation against an existing header will
+ * preserve its parity.
+ *
+ * @addtogroup mdhaflags
+   @{
+ */
+#define sah_insert_mdha_icv_check 0x80001000
+#define sah_insert_mdha_ssl       0x80000400
+#define sah_insert_mdha_mac_full  0x80000200
+#define sah_insert_mdha_opad      0x80000080
+#define sah_insert_mdha_ipad      0x80000040
+#define sah_insert_mdha_init      0x80000020
+#define sah_insert_mdha_hmac      0x80000008
+#define sah_insert_mdha_pdata     0x80000004
+#define sah_insert_mdha_algorithm_sha224  0x00000003
+#define sah_insert_mdha_algorithm_sha256  0x80000002
+#define sah_insert_mdha_algorithm_md5     0x80000001
+#define sah_insert_mdha_algorithm_sha1    0x00000000
+/*! @} */
+
+extern const uint32_t sah_insert_skha_algorithm[];
+extern const uint32_t sah_insert_skha_mode[];
+extern const uint32_t sah_insert_skha_modulus[];
+
+/*! @defgroup skhaflags SKHA Mode Register Values
+ *
+ * These are bit fields and combinations of bit fields for setting the Mode
+ * Register portion of a Sahara Descriptor Header field.
+ *
+ * The parity bit has been set to ensure that these values have even parity,
+ * therefore using an Exclusive-OR operation against an existing header will
+ * preserve its parity.
+ *
+ * @addtogroup skhaflags
+   @{
+ */
+/*! */
+#define sah_insert_skha_modulus_128    0x00001e00
+#define sah_insert_skha_no_key_parity  0x80000100
+#define sah_insert_skha_ctr_last_block 0x80000020
+#define sah_insert_skha_suppress_cbc   0x80000020
+#define sah_insert_skha_no_permute     0x80000020
+#define sah_insert_skha_algorithm_arc4 0x00000003
+#define sah_insert_skha_algorithm_tdes 0x80000002
+#define sah_insert_skha_algorithm_des  0x80000001
+#define sah_insert_skha_algorithm_aes  0x00000000
+#define sah_insert_skha_aux0           0x80000020
+#define sah_insert_skha_mode_ctr       0x00000018
+#define sah_insert_skha_mode_ccm       0x80000010
+#define sah_insert_skha_mode_cbc       0x80000008
+#define sah_insert_skha_mode_ecb       0x00000000
+#define sah_insert_skha_encrypt        0x80000004
+#define sah_insert_skha_decrypt        0x00000000
+/*! @} */
+
+/*! @defgroup rngflags RNG Mode Register Values
+ *
+ */
+/*! */
+#define sah_insert_rng_gen_seed        0x80000001
+
+/*! @} */
+
+/*! @defgroup pkhaflags PKHA Mode Register Values
+ *
+ */
+/*! */
+#define sah_insert_pkha_soft_err_false         0x80000200
+#define sah_insert_pkha_soft_err_true          0x80000100
+
+#define sah_insert_pkha_rtn_clr_mem            0x80000001
+#define sah_insert_pkha_rtn_clr_eram           0x80000002
+#define sah_insert_pkha_rtn_mod_exp            0x00000003
+#define sah_insert_pkha_rtn_mod_r2modn         0x80000004
+#define sah_insert_pkha_rtn_mod_rrmodp         0x00000005
+#define sah_insert_pkha_rtn_ec_fp_aff_ptmul    0x00000006
+#define sah_insert_pkha_rtn_ec_f2m_aff_ptmul   0x80000007
+#define sah_insert_pkha_rtn_ec_fp_proj_ptmul   0x80000008
+#define sah_insert_pkha_rtn_ec_f2m_proj_ptmul  0x00000009
+#define sah_insert_pkha_rtn_ec_fp_add          0x0000000A
+#define sah_insert_pkha_rtn_ec_fp_double       0x8000000B
+#define sah_insert_pkha_rtn_ec_f2m_add         0x0000000C
+#define sah_insert_pkha_rtn_ec_f2m_double      0x8000000D
+#define sah_insert_pkha_rtn_f2m_r2modn         0x8000000E
+#define sah_insert_pkha_rtn_f2m_inv            0x0000000F
+#define sah_insert_pkha_rtn_mod_inv            0x80000010
+#define sah_insert_pkha_rtn_rsa_sstep          0x00000011
+#define sah_insert_pkha_rtn_mod_emodn          0x00000012
+#define sah_insert_pkha_rtn_f2m_emodn          0x80000013
+#define sah_insert_pkha_rtn_ec_fp_ptmul        0x00000014
+#define sah_insert_pkha_rtn_ec_f2m_ptmul       0x80000015
+#define sah_insert_pkha_rtn_f2m_gcd            0x80000016
+#define sah_insert_pkha_rtn_mod_gcd            0x00000017
+#define sah_insert_pkha_rtn_f2m_dbl_aff        0x00000018
+#define sah_insert_pkha_rtn_fp_dbl_aff         0x80000019
+#define sah_insert_pkha_rtn_f2m_add_aff        0x8000001A
+#define sah_insert_pkha_rtn_fp_add_aff         0x0000001B
+#define sah_insert_pkha_rtn_f2m_exp            0x8000001C
+#define sah_insert_pkha_rtn_mod_exp_teq        0x0000001D
+#define sah_insert_pkha_rtn_rsa_sstep_teq      0x0000001E
+#define sah_insert_pkha_rtn_f2m_multn          0x8000001F
+#define sah_insert_pkha_rtn_mod_multn          0x80000020
+#define sah_insert_pkha_rtn_mod_add            0x00000021
+#define sah_insert_pkha_rtn_mod_sub            0x00000022
+#define sah_insert_pkha_rtn_mod_mult1_mont     0x80000023
+#define sah_insert_pkha_rtn_mod_mult2_deconv   0x00000024
+#define sah_insert_pkha_rtn_f2m_add            0x80000025
+#define sah_insert_pkha_rtn_f2m_mult1_mont     0x80000026
+#define sah_insert_pkha_rtn_f2m_mult2_deconv   0x00000027
+#define sah_insert_pkha_rtn_miller_rabin       0x00000028
+#define sah_insert_pkha_rtn_mod_amodn          0x00000029
+#define sah_insert_pkha_rtn_f2m_amodn          0x8000002A
+/*! @} */
+
+/*! Add a descriptor with two input pointers */
+fsl_shw_return_t sah_add_two_in_desc(uint32_t header,
+				     const uint8_t * in1,
+				     uint32_t in1_length,
+				     const uint8_t * in2,
+				     uint32_t in2_length,
+				     const sah_Mem_Util * mu,
+				     sah_Head_Desc ** desc_chain);
+
+/*! Add a descriptor with two 'data' pointers */
+fsl_shw_return_t sah_add_two_d_desc(uint32_t header,
+				    const uint8_t * in1,
+				    uint32_t in1_length,
+				    const uint8_t * in2,
+				    uint32_t in2_length,
+				    const sah_Mem_Util * mu,
+				    sah_Head_Desc ** desc_chain);
+
+/*! Add a descriptor with an input and key pointer */
+fsl_shw_return_t sah_add_in_key_desc(uint32_t header,
+				     const uint8_t * in1,
+				     uint32_t in1_length,
+				     fsl_shw_sko_t * key_info,
+				     const sah_Mem_Util * mu,
+				     sah_Head_Desc ** desc_chain);
+
+/*! Add a descriptor with two key pointers */
+fsl_shw_return_t sah_add_key_key_desc(uint32_t header,
+				      fsl_shw_sko_t * key_info1,
+				      fsl_shw_sko_t * key_info2,
+				      const sah_Mem_Util * mu,
+				      sah_Head_Desc ** desc_chain);
+
+/*! Add a descriptor with two output pointers */
+fsl_shw_return_t sah_add_two_out_desc(uint32_t header,
+				      uint8_t * out1,
+				      uint32_t out1_length,
+				      uint8_t * out2,
+				      uint32_t out2_length,
+				      const sah_Mem_Util * mu,
+				      sah_Head_Desc ** desc_chain);
+
+/*! Add a descriptor with an input and output pointer */
+fsl_shw_return_t sah_add_in_out_desc(uint32_t header,
+				     const uint8_t * in, uint32_t in_length,
+				     uint8_t * out, uint32_t out_length,
+				     const sah_Mem_Util * mu,
+				     sah_Head_Desc ** desc_chain);
+
+/*! Add a descriptor with an input and key output pointer */
+fsl_shw_return_t sah_add_in_keyout_desc(uint32_t header,
+					const uint8_t * in, uint32_t in_length,
+					fsl_shw_sko_t * key_info,
+					const sah_Mem_Util * mu,
+					sah_Head_Desc ** desc_chain);
+
+/*! Add a descriptor with a key and an output pointer */
+fsl_shw_return_t sah_add_key_out_desc(uint32_t header,
+					  const fsl_shw_sko_t * key_info,
+				      uint8_t * out, uint32_t out_length,
+				      const sah_Mem_Util * mu,
+				      sah_Head_Desc ** desc_chain);
+
+/*! Add a descriptor with an output and input pointer */
+fsl_shw_return_t sah_add_out_in_desc(uint32_t header,
+				     uint8_t * out, uint32_t out_length,
+				     const uint8_t * in, uint32_t in_length,
+				     const sah_Mem_Util * mu,
+				     sah_Head_Desc ** desc_chain);
+
+/*! Verify that supplied User Context Object is valid */
+fsl_shw_return_t sah_validate_uco(fsl_shw_uco_t * uco);
+
+#endif				/* SF_UTIL_H */
+
+/* End of sf_util.h */
diff --git a/drivers/mxc/security/sahara2/km_adaptor.c b/drivers/mxc/security/sahara2/km_adaptor.c
new file mode 100644
index 000000000000..2cec1b8d0f98
--- /dev/null
+++ b/drivers/mxc/security/sahara2/km_adaptor.c
@@ -0,0 +1,849 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+* @file km_adaptor.c
+*
+* @brief The Adaptor component provides an interface to the
+*        driver for a kernel user.
+*/
+
+#include <adaptor.h>
+#include <sf_util.h>
+#include <sah_queue_manager.h>
+#include <sah_memory_mapper.h>
+#include <fsl_shw_keystore.h>
+#ifdef FSL_HAVE_SCC
+#include <linux/mxc_scc_driver.h>
+#elif defined (FSL_HAVE_SCC2)
+#include <linux/mxc_scc2_driver.h>
+#endif
+
+
+EXPORT_SYMBOL(adaptor_Exec_Descriptor_Chain);
+EXPORT_SYMBOL(sah_register);
+EXPORT_SYMBOL(sah_deregister);
+EXPORT_SYMBOL(sah_get_results);
+EXPORT_SYMBOL(fsl_shw_smalloc);
+EXPORT_SYMBOL(fsl_shw_sfree);
+EXPORT_SYMBOL(fsl_shw_sstatus);
+EXPORT_SYMBOL(fsl_shw_diminish_perms);
+EXPORT_SYMBOL(do_scc_encrypt_region);
+EXPORT_SYMBOL(do_scc_decrypt_region);
+EXPORT_SYMBOL(do_system_keystore_slot_alloc);
+EXPORT_SYMBOL(do_system_keystore_slot_dealloc);
+EXPORT_SYMBOL(do_system_keystore_slot_load);
+EXPORT_SYMBOL(do_system_keystore_slot_read);
+EXPORT_SYMBOL(do_system_keystore_slot_encrypt);
+EXPORT_SYMBOL(do_system_keystore_slot_decrypt);
+
+
+#if defined(DIAG_DRV_IF) || defined(DIAG_MEM) || defined(DIAG_ADAPTOR)
+#include <diagnostic.h>
+#endif
+
+#if defined(DIAG_DRV_IF) || defined(DIAG_MEM) || defined(DIAG_ADAPTOR)
+#define MAX_DUMP 16
+
+#define DIAG_MSG_SIZE   300
+static char Diag_msg[DIAG_MSG_SIZE];
+#endif
+
+/* This is the wait queue to this mode of driver */
+DECLARE_WAIT_QUEUE_HEAD(Wait_queue_km);
+
+/*! This matches Sahara2 capabilities... */
+fsl_shw_pco_t sahara2_capabilities = {
+	1, 3,			/* api version number - major & minor */
+	1, 6,			/* driver version number - major & minor */
+	{
+	 FSL_KEY_ALG_AES,
+	 FSL_KEY_ALG_DES,
+	 FSL_KEY_ALG_TDES,
+	 FSL_KEY_ALG_ARC4},
+	{
+	 FSL_SYM_MODE_STREAM,
+	 FSL_SYM_MODE_ECB,
+	 FSL_SYM_MODE_CBC,
+	 FSL_SYM_MODE_CTR},
+	{
+	 FSL_HASH_ALG_MD5,
+	 FSL_HASH_ALG_SHA1,
+	 FSL_HASH_ALG_SHA224,
+	 FSL_HASH_ALG_SHA256},
+	/*
+	 * The following table must be set to handle all values of key algorithm
+	 * and sym mode, and be in the correct order..
+	 */
+	{			/* Stream, ECB, CBC, CTR */
+	 {0, 0, 0, 0},		/* HMAC */
+	 {0, 1, 1, 1},		/* AES  */
+	 {0, 1, 1, 0},		/* DES */
+	 {0, 1, 1, 0},		/* 3DES */
+	 {1, 0, 0, 0}		/* ARC4 */
+	 },
+	0, 0,
+	0, 0, 0,
+	{{0, 0}}
+};
+
+#ifdef DIAG_ADAPTOR
+void km_Dump_Chain(const sah_Desc * chain);
+
+void km_Dump_Region(const char *prefix, const unsigned char *data,
+		    unsigned length);
+
+static void km_Dump_Link(const char *prefix, const sah_Link * link);
+
+void km_Dump_Words(const char *prefix, const unsigned *data, unsigned length);
+#endif
+
+/**** Memory routines ****/
+
+static void *my_malloc(void *ref, size_t n)
+{
+	register void *mem;
+
+#ifndef DIAG_MEM_ERRORS
+	mem = os_alloc_memory(n, GFP_KERNEL);
+
+#else
+	{
+		uint32_t rand;
+		/* are we feeling lucky ? */
+		os_get_random_bytes(&rand, sizeof(rand));
+		if ((rand % DIAG_MEM_CONST) == 0) {
+			mem = 0;
+		} else {
+			mem = os_alloc_memory(n, GFP_ATOMIC);
+		}
+	}
+#endif				/* DIAG_MEM_ERRORS */
+
+#ifdef DIAG_MEM
+	sprintf(Diag_msg, "API kmalloc: %p for %d\n", mem, n);
+	LOG_KDIAG(Diag_msg);
+#endif
+	ref = 0;		/* unused param warning */
+	return mem;
+}
+
+static sah_Head_Desc *my_alloc_head_desc(void *ref)
+{
+	register sah_Head_Desc *ptr;
+
+#ifndef DIAG_MEM_ERRORS
+	ptr = sah_Alloc_Head_Descriptor();
+
+#else
+	{
+		uint32_t rand;
+		/* are we feeling lucky ? */
+		os_get_random_bytes(&rand, sizeof(rand));
+		if ((rand % DIAG_MEM_CONST) == 0) {
+			ptr = 0;
+		} else {
+			ptr = sah_Alloc_Head_Descriptor();
+		}
+	}
+#endif
+	ref = 0;
+	return ptr;
+}
+
+static sah_Desc *my_alloc_desc(void *ref)
+{
+	register sah_Desc *ptr;
+
+#ifndef DIAG_MEM_ERRORS
+	ptr = sah_Alloc_Descriptor();
+
+#else
+	{
+		uint32_t rand;
+		/* are we feeling lucky ? */
+		os_get_random_bytes(&rand, sizeof(rand));
+		if ((rand % DIAG_MEM_CONST) == 0) {
+			ptr = 0;
+		} else {
+			ptr = sah_Alloc_Descriptor();
+		}
+	}
+#endif
+	ref = 0;
+	return ptr;
+}
+
+static sah_Link *my_alloc_link(void *ref)
+{
+	register sah_Link *ptr;
+
+#ifndef DIAG_MEM_ERRORS
+	ptr = sah_Alloc_Link();
+
+#else
+	{
+		uint32_t rand;
+		/* are we feeling lucky ? */
+		os_get_random_bytes(&rand, sizeof(rand));
+		if ((rand % DIAG_MEM_CONST) == 0) {
+			ptr = 0;
+		} else {
+			ptr = sah_Alloc_Link();
+		}
+	}
+#endif
+	ref = 0;
+	return ptr;
+}
+
+static void my_free(void *ref, void *ptr)
+{
+	ref = 0;		/* unused param warning */
+#ifdef DIAG_MEM
+	sprintf(Diag_msg, "API kfree: %p\n", ptr);
+	LOG_KDIAG(Diag_msg);
+#endif
+	os_free_memory(ptr);
+}
+
+static void my_free_head_desc(void *ref, sah_Head_Desc * ptr)
+{
+	sah_Free_Head_Descriptor(ptr);
+}
+
+static void my_free_desc(void *ref, sah_Desc * ptr)
+{
+	sah_Free_Descriptor(ptr);
+}
+
+static void my_free_link(void *ref, sah_Link * ptr)
+{
+	sah_Free_Link(ptr);
+}
+
+static void *my_memcpy(void *ref, void *dest, const void *src, size_t n)
+{
+	ref = 0;		/* unused param warning */
+	return memcpy(dest, src, n);
+}
+
+static void *my_memset(void *ref, void *ptr, int ch, size_t n)
+{
+	ref = 0;		/* unused param warning */
+	return memset(ptr, ch, n);
+}
+
+/*! Standard memory manipulation routines for kernel API. */
+static sah_Mem_Util std_kernelmode_mem_util = {
+	.mu_ref = 0,
+	.mu_malloc = my_malloc,
+	.mu_alloc_head_desc = my_alloc_head_desc,
+	.mu_alloc_desc = my_alloc_desc,
+	.mu_alloc_link = my_alloc_link,
+	.mu_free = my_free,
+	.mu_free_head_desc = my_free_head_desc,
+	.mu_free_desc = my_free_desc,
+	.mu_free_link = my_free_link,
+	.mu_memcpy = my_memcpy,
+	.mu_memset = my_memset
+};
+
+fsl_shw_return_t get_capabilities(fsl_shw_uco_t * user_ctx,
+				  fsl_shw_pco_t * capabilities)
+{
+	scc_config_t *scc_capabilities;
+
+	/* Fill in the Sahara2 capabilities. */
+	memcpy(capabilities, &sahara2_capabilities, sizeof(fsl_shw_pco_t));
+
+	/* Fill in the SCC portion of the capabilities object */
+	scc_capabilities = scc_get_configuration();
+	capabilities->scc_driver_major = scc_capabilities->driver_major_version;
+	capabilities->scc_driver_minor = scc_capabilities->driver_minor_version;
+	capabilities->scm_version = scc_capabilities->scm_version;
+	capabilities->smn_version = scc_capabilities->smn_version;
+	capabilities->block_size_bytes = scc_capabilities->block_size_bytes;
+
+#ifdef FSL_HAVE_SCC
+	capabilities->scc_info.black_ram_size_blocks =
+	    scc_capabilities->black_ram_size_blocks;
+	capabilities->scc_info.red_ram_size_blocks =
+	    scc_capabilities->red_ram_size_blocks;
+#elif defined(FSL_HAVE_SCC2)
+	capabilities->scc2_info.partition_size_bytes =
+	    scc_capabilities->partition_size_bytes;
+	capabilities->scc2_info.partition_count =
+	    scc_capabilities->partition_count;
+#endif
+
+	return FSL_RETURN_OK_S;
+}
+
+/*!
+ * Sends a request to register this user
+ *
+ * @brief    Sends a request to register this user
+ *
+ * @param[in,out] user_ctx  part of the structure contains input parameters and
+ *                          part is filled in by the driver
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_register(fsl_shw_uco_t * user_ctx)
+{
+	fsl_shw_return_t status;
+
+	/* this field is used in user mode to indicate a file open has occured.
+	 * it is used here, in kernel mode, to indicate that the uco is registered
+	 */
+	user_ctx->sahara_openfd = 0;	/* set to 'registered' */
+	user_ctx->mem_util = &std_kernelmode_mem_util;
+
+	/* check that uco is valid */
+	status = sah_validate_uco(user_ctx);
+
+	/*  If life is good, register this user */
+	if (status == FSL_RETURN_OK_S) {
+		status = sah_handle_registration(user_ctx);
+	}
+
+	if (status != FSL_RETURN_OK_S) {
+		user_ctx->sahara_openfd = -1;	/* set to 'not registered' */
+	}
+
+	return status;
+}
+
+/*!
+ * Sends a request to deregister this user
+ *
+ * @brief    Sends a request to deregister this user
+ *
+ * @param[in,out]  user_ctx   Info on user being deregistered.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_deregister(fsl_shw_uco_t * user_ctx)
+{
+	fsl_shw_return_t status = FSL_RETURN_OK_S;
+
+	if (user_ctx->sahara_openfd == 0) {
+		status = sah_handle_deregistration(user_ctx);
+		user_ctx->sahara_openfd = -1;	/* set to 'no registered */
+	}
+
+	return status;
+}
+
+/*!
+ * Sends a request to get results for this user
+ *
+ * @brief    Sends a request to get results for this user
+ *
+ * @param[in,out] arg      Pointer to structure to collect results
+ * @param         uco      User's context
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_get_results(sah_results * arg, fsl_shw_uco_t * uco)
+{
+	fsl_shw_return_t code = sah_get_results_from_pool(uco, arg);
+
+	if ((code == FSL_RETURN_OK_S) && (arg->actual != 0)) {
+		sah_Postprocess_Results(uco, arg);
+	}
+
+	return code;
+}
+
+/*!
+ * This function writes the Descriptor Chain to the kernel driver.
+ *
+ * @brief    Writes the Descriptor Chain to the kernel driver.
+ *
+ * @param    dar   A pointer to a Descriptor Chain of type sah_Head_Desc
+ * @param    uco   The user context object
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t adaptor_Exec_Descriptor_Chain(sah_Head_Desc * dar,
+					       fsl_shw_uco_t * uco)
+{
+	sah_Head_Desc *kernel_space_desc = NULL;
+	fsl_shw_return_t code = FSL_RETURN_OK_S;
+	int os_error_code = 0;
+	unsigned blocking_mode = dar->uco_flags & FSL_UCO_BLOCKING_MODE;
+
+#ifdef DIAG_ADAPTOR
+	km_Dump_Chain(&dar->desc);
+#endif
+
+	dar->user_info = uco;
+	dar->user_desc = dar;
+
+	/* This code has been shamelessly copied from sah_driver_interface.c */
+	/* It needs to be moved somewhere common ... */
+	kernel_space_desc = sah_Physicalise_Descriptors(dar);
+
+	if (kernel_space_desc == NULL) {
+		/* We may have failed due to a -EFAULT as well, but we will return
+		 * -ENOMEM since either way it is a memory related failure. */
+		code = FSL_RETURN_NO_RESOURCE_S;
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("sah_Physicalise_Descriptors() failed\n");
+#endif
+	} else {
+		if (blocking_mode) {
+#ifdef SAHARA_POLL_MODE
+			os_error_code = sah_Handle_Poll(dar);
+#else
+			os_error_code = sah_blocking_mode(dar);
+#endif
+			if (os_error_code != 0) {
+				code = FSL_RETURN_ERROR_S;
+			} else {	/* status of actual operation */
+				code = dar->result;
+			}
+		} else {
+#ifdef SAHARA_POLL_MODE
+			sah_Handle_Poll(dar);
+#else
+			/* just put someting in the DAR */
+			sah_Queue_Manager_Append_Entry(dar);
+#endif				/* SAHARA_POLL_MODE */
+		}
+	}
+
+	return code;
+}
+
+
+/* System keystore context, defined in sah_driver_interface.c */
+extern fsl_shw_kso_t system_keystore;
+
+fsl_shw_return_t do_system_keystore_slot_alloc(fsl_shw_uco_t * user_ctx,
+					       uint32_t key_length,
+					       uint64_t ownerid,
+					       uint32_t * slot)
+{
+	(void)user_ctx;
+	return keystore_slot_alloc(&system_keystore, key_length, ownerid, slot);
+}
+
+fsl_shw_return_t do_system_keystore_slot_dealloc(fsl_shw_uco_t * user_ctx,
+						 uint64_t ownerid,
+						 uint32_t slot)
+{
+	(void)user_ctx;
+	return keystore_slot_dealloc(&system_keystore, ownerid, slot);
+}
+
+fsl_shw_return_t do_system_keystore_slot_load(fsl_shw_uco_t * user_ctx,
+					      uint64_t ownerid,
+					      uint32_t slot,
+					      const uint8_t * key,
+					      uint32_t key_length)
+{
+	(void)user_ctx;
+	return keystore_slot_load(&system_keystore, ownerid, slot,
+				  (void *)key, key_length);
+}
+
+fsl_shw_return_t do_system_keystore_slot_read(fsl_shw_uco_t * user_ctx,
+					      uint64_t ownerid,
+					      uint32_t slot,
+					      uint32_t key_length,
+					      const uint8_t * key)
+{
+	(void)user_ctx;
+	return keystore_slot_read(&system_keystore, ownerid, slot,
+				  key_length, (void *)key);
+}
+
+fsl_shw_return_t do_system_keystore_slot_encrypt(fsl_shw_uco_t * user_ctx,
+						 uint64_t ownerid,
+						 uint32_t slot,
+						 uint32_t key_length,
+						 uint8_t * black_data)
+{
+	(void)user_ctx;
+	return keystore_slot_encrypt(NULL, &system_keystore, ownerid,
+				     slot, key_length, black_data);
+}
+
+fsl_shw_return_t do_system_keystore_slot_decrypt(fsl_shw_uco_t * user_ctx,
+						 uint64_t ownerid,
+						 uint32_t slot,
+						 uint32_t key_length,
+						 const uint8_t * black_data)
+{
+	(void)user_ctx;
+	return keystore_slot_decrypt(NULL, &system_keystore, ownerid,
+				     slot, key_length, black_data);
+}
+
+void *fsl_shw_smalloc(fsl_shw_uco_t * user_ctx,
+		      uint32_t size, const uint8_t * UMID, uint32_t permissions)
+{
+#ifdef FSL_HAVE_SCC2
+	int part_no;
+	void *part_base;
+	uint32_t part_phys;
+	scc_config_t *scc_configuration;
+
+	/* Check that the memory size requested is correct */
+	scc_configuration = scc_get_configuration();
+	if (size != scc_configuration->partition_size_bytes) {
+		return NULL;
+	}
+
+	/* Attempt to grab a partition. */
+	if (scc_allocate_partition(0, &part_no, &part_base, &part_phys)
+	    != SCC_RET_OK) {
+		return NULL;
+	}
+	printk(KERN_ALERT "In fsh_shw_smalloc (km): partition_base:%p "
+	       "partition_base_phys: %p\n", part_base, (void *)part_phys);
+
+	/* these bits should be in a separate function */
+	printk(KERN_ALERT "writing UMID and MAP to secure the partition\n");
+
+	scc_engage_partition(part_base, UMID, permissions);
+
+	(void)user_ctx;		/* unused param warning */
+
+	return part_base;
+#else				/* FSL_HAVE_SCC2 */
+	(void)user_ctx;
+	(void)size;
+	(void)UMID;
+	(void)permissions;
+	return NULL;
+#endif				/* FSL_HAVE_SCC2 */
+
+}
+
+fsl_shw_return_t fsl_shw_sfree(fsl_shw_uco_t * user_ctx, void *address)
+{
+	(void)user_ctx;
+
+#ifdef FSL_HAVE_SCC2
+	if (scc_release_partition(address) == SCC_RET_OK) {
+		return FSL_RETURN_OK_S;
+	}
+#endif
+
+	return FSL_RETURN_ERROR_S;
+}
+
+fsl_shw_return_t fsl_shw_sstatus(fsl_shw_uco_t * user_ctx,
+				 void *address,
+				 fsl_shw_partition_status_t * status)
+{
+	(void)user_ctx;
+
+#ifdef FSL_HAVE_SCC2
+	*status = scc_partition_status(address);
+	return FSL_RETURN_OK_S;
+#endif
+
+	return FSL_RETURN_ERROR_S;
+}
+
+/* Diminish permissions on some secure memory */
+fsl_shw_return_t fsl_shw_diminish_perms(fsl_shw_uco_t * user_ctx,
+					void *address, uint32_t permissions)
+{
+
+	(void)user_ctx;		/* unused parameter warning */
+
+#ifdef FSL_HAVE_SCC2
+	if (scc_diminish_permissions(address, permissions) == SCC_RET_OK) {
+		return FSL_RETURN_OK_S;
+	}
+#endif
+	return FSL_RETURN_ERROR_S;
+}
+
+/*
+ * partition_base - physical address of the partition
+ * offset - offset, in blocks, of the data from the start of the partition
+ * length - length, in bytes, of the data to be encrypted (multiple of 4)
+ * black_data - virtual address that the encrypted data should be stored at
+ * Note that this virtual address must be translatable using the __virt_to_phys
+ * macro; ie, it can't be a specially mapped address.  To do encryption with those
+ * addresses, use the scc_encrypt_region function directly.  This is to make
+ * this function compatible with the user mode declaration, which does not know
+ * the physical addresses of the data it is using.
+ */
+fsl_shw_return_t
+do_scc_encrypt_region(fsl_shw_uco_t * user_ctx,
+		      void *partition_base, uint32_t offset_bytes,
+		      uint32_t byte_count, uint8_t * black_data,
+		      uint32_t * IV, fsl_shw_cypher_mode_t cypher_mode)
+{
+	scc_return_t scc_ret;
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+
+#ifdef FSL_HAVE_SCC2
+
+#ifdef DIAG_ADAPTOR
+	uint32_t *owner_32 = (uint32_t *) & (owner_id);
+
+	LOG_KDIAG_ARGS
+	    ("partition base: %p, offset: %i, count: %i, black data: %p\n",
+	     partition_base, offset_bytes, byte_count, (void *)black_data);
+#endif
+	(void)user_ctx;
+
+	os_cache_flush_range(black_data, byte_count);
+
+	scc_ret =
+	    scc_encrypt_region((uint32_t) partition_base, offset_bytes,
+			       byte_count, __virt_to_phys(black_data), IV,
+			       cypher_mode);
+
+	if (scc_ret == SCC_RET_OK) {
+		retval = FSL_RETURN_OK_S;
+	} else {
+		retval = FSL_RETURN_ERROR_S;
+	}
+
+	/* The SCC2 DMA engine should have written to the black ram, so we need to
+	 * invalidate that region of memory.  Note that the red ram is not an
+	 * because it is mapped with the cache disabled.
+	 */
+	os_cache_inv_range(black_data, byte_count);
+
+#else
+	(void)scc_ret;
+#endif				/* FSL_HAVE_SCC2 */
+
+	return retval;
+}
+
+/*!
+ * Call the proper function to decrypt a region of encrypted secure memory
+ *
+ * @brief 
+ *
+ * @param   user_ctx        User context of the partition owner (NULL in kernel)
+ * @param   partition_base  Base address (physical) of the partition
+ * @param   offset_bytes    Offset from base address that the decrypted data
+ *                          shall be placed
+ * @param   byte_count      Length of the message (bytes)
+ * @param   black_data      Pointer to where the encrypted data is stored
+ * @param   owner_id    
+ *
+ * @return  status
+ */
+
+fsl_shw_return_t
+do_scc_decrypt_region(fsl_shw_uco_t * user_ctx,
+		      void *partition_base, uint32_t offset_bytes,
+		      uint32_t byte_count, const uint8_t * black_data,
+		      uint32_t * IV, fsl_shw_cypher_mode_t cypher_mode)
+{
+	scc_return_t scc_ret;
+	fsl_shw_return_t retval = FSL_RETURN_ERROR_S;
+
+#ifdef FSL_HAVE_SCC2
+
+#ifdef DIAG_ADAPTOR
+	uint32_t *owner_32 = (uint32_t *) & (owner_id);
+
+	LOG_KDIAG_ARGS
+	    ("partition base: %p, offset: %i, count: %i, black data: %p\n",
+	     partition_base, offset_bytes, byte_count, (void *)black_data);
+#endif
+
+	(void)user_ctx;
+
+	/* The SCC2 DMA engine will be reading from the black ram, so we need to
+	 * make sure that the data is pushed out of the cache.  Note that the red
+	 * ram is not an issue because it is mapped with the cache disabled.
+	 */
+	os_cache_flush_range(black_data, byte_count);
+
+	scc_ret =
+	    scc_decrypt_region((uint32_t) partition_base, offset_bytes,
+			       byte_count,
+			       (uint8_t *) __virt_to_phys(black_data), IV,
+			       cypher_mode);
+
+	if (scc_ret == SCC_RET_OK) {
+		retval = FSL_RETURN_OK_S;
+	} else {
+		retval = FSL_RETURN_ERROR_S;
+	}
+
+#else
+	(void)scc_ret;
+#endif				/* FSL_HAVE_SCC2 */
+
+	return retval;
+}
+
+#ifdef DIAG_ADAPTOR
+/*!
+ * Dump chain of descriptors to the log.
+ *
+ * @brief Dump descriptor chain
+ *
+ * @param    chain     Kernel virtual address of start of chain of descriptors
+ *
+ * @return   void
+ */
+void km_Dump_Chain(const sah_Desc * chain)
+{
+	while (chain != NULL) {
+		km_Dump_Words("Desc", (unsigned *)chain,
+			      6 /*sizeof(*chain)/sizeof(unsigned) */ );
+		/* place this definition elsewhere */
+		if (chain->ptr1) {
+			if (chain->header & SAH_HDR_LLO) {
+				km_Dump_Region(" Data1", chain->ptr1,
+					       chain->len1);
+			} else {
+				km_Dump_Link(" Link1", chain->ptr1);
+			}
+		}
+		if (chain->ptr2) {
+			if (chain->header & SAH_HDR_LLO) {
+				km_Dump_Region(" Data2", chain->ptr2,
+					       chain->len2);
+			} else {
+				km_Dump_Link(" Link2", chain->ptr2);
+			}
+		}
+
+		chain = chain->next;
+	}
+}
+
+/*!
+ * Dump chain of links to the log.
+ *
+ * @brief Dump chain of links
+ *
+ * @param    prefix    Text to put in front of dumped data
+ * @param    link      Kernel virtual address of start of chain of links
+ *
+ * @return   void
+ */
+static void km_Dump_Link(const char *prefix, const sah_Link * link)
+{
+	while (link != NULL) {
+		km_Dump_Words(prefix, (unsigned *)link,
+			      3 /* # words in h/w link */ );
+		if (link->flags & SAH_STORED_KEY_INFO) {
+#ifdef CAN_DUMP_SCC_DATA
+			uint32_t len;
+#endif
+
+#ifdef CAN_DUMP_SCC_DATA
+			{
+				char buf[50];
+
+				scc_get_slot_info(link->ownerid, link->slot, (uint32_t *) & link->data,	/* RED key address */
+						  &len);	/* key length */
+				sprintf(buf, "  SCC slot %d: ", link->slot);
+				km_Dump_Words(buf,
+					      (void *)IO_ADDRESS((uint32_t)
+								 link->data),
+					      link->len / 4);
+			}
+#else
+			sprintf(Diag_msg, "  SCC slot %d", link->slot);
+			LOG_KDIAG(Diag_msg);
+#endif
+		} else if (link->data != NULL) {
+			km_Dump_Region("  Data", link->data, link->len);
+		}
+
+		link = link->next;
+	}
+}
+
+/*!
+ * Dump given region of data to the log.
+ *
+ * @brief Dump data
+ *
+ * @param    prefix    Text to put in front of dumped data
+ * @param    data      Kernel virtual address of start of region to dump
+ * @param    length    Amount of data to dump
+ *
+ * @return   void
+*/
+void km_Dump_Region(const char *prefix, const unsigned char *data,
+		    unsigned length)
+{
+	unsigned count;
+	char *output;
+	unsigned data_len;
+
+	sprintf(Diag_msg, "%s (%08X,%u):", prefix, (uint32_t) data, length);
+
+	/* Restrict amount of data to dump */
+	if (length > MAX_DUMP) {
+		data_len = MAX_DUMP;
+	} else {
+		data_len = length;
+	}
+
+	/* We've already printed some text in output buffer, skip over it */
+	output = Diag_msg + strlen(Diag_msg);
+
+	for (count = 0; count < data_len; count++) {
+		if (count % 4 == 0) {
+			*output++ = ' ';
+		}
+		sprintf(output, "%02X", *data++);
+		output += 2;
+	}
+
+	LOG_KDIAG(Diag_msg);
+}
+
+/*!
+ * Dump given wors of data to the log.
+ *
+ * @brief Dump data
+ *
+ * @param    prefix       Text to put in front of dumped data
+ * @param    data         Kernel virtual address of start of region to dump
+ * @param    word_count   Amount of data to dump
+ *
+ * @return   void
+*/
+void km_Dump_Words(const char *prefix, const unsigned *data,
+		   unsigned word_count)
+{
+	char *output;
+
+	sprintf(Diag_msg, "%s (%08X,%uw): ", prefix, (uint32_t) data,
+		word_count);
+
+	/* We've already printed some text in output buffer, skip over it */
+	output = Diag_msg + strlen(Diag_msg);
+
+	while (word_count--) {
+		sprintf(output, "%08X ", *data++);
+		output += 9;
+	}
+
+	LOG_KDIAG(Diag_msg);
+}
+#endif
diff --git a/drivers/mxc/security/sahara2/sah_driver_interface.c b/drivers/mxc/security/sahara2/sah_driver_interface.c
new file mode 100644
index 000000000000..d34e3ed90af8
--- /dev/null
+++ b/drivers/mxc/security/sahara2/sah_driver_interface.c
@@ -0,0 +1,2179 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+* @file sah_driver_interface.c
+*
+* @brief Provides a Linux Kernel Module interface to the SAHARA h/w device.
+*
+*/
+
+/* SAHARA Includes */
+#include <sah_driver_common.h>
+#include <sah_kernel.h>
+#include <sah_memory_mapper.h>
+#include <sah_queue_manager.h>
+#include <sah_status_manager.h>
+#include <sah_interrupt_handler.h>
+#include <sah_hardware_interface.h>
+#include <fsl_shw_keystore.h>
+#include <adaptor.h>
+#ifdef FSL_HAVE_SCC
+#include <linux/mxc_scc_driver.h>
+#else
+#include <linux/mxc_scc2_driver.h>
+#endif
+
+#ifdef DIAG_DRV_IF
+#include <diagnostic.h>
+#endif
+
+#if defined(CONFIG_DEVFS_FS) && (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
+#include <linux/devfs_fs_kernel.h>
+#else
+#include <linux/proc_fs.h>
+#endif
+
+#include <mach/spba.h>
+
+#ifdef PERF_TEST
+#define interruptible_sleep_on(x) sah_Handle_Interrupt()
+#endif
+
+#define TEST_MODE_OFF 1
+#define TEST_MODE_ON 2
+
+/*! Version register on first deployments */
+#define SAHARA_VERSION2  2
+/*! Version register on MX27 */
+#define SAHARA_VERSION3  3
+/*! Version register on MXC92323 */
+#define SAHARA_VERSION4  4
+
+/******************************************************************************
+* Module function declarations
+******************************************************************************/
+
+OS_DEV_INIT_DCL(sah_init);
+OS_DEV_SHUTDOWN_DCL(sah_cleanup);
+OS_DEV_OPEN_DCL(sah_open);
+OS_DEV_CLOSE_DCL(sah_release);
+OS_DEV_IOCTL_DCL(sah_ioctl);
+OS_DEV_MMAP_DCL(sah_mmap);
+
+static os_error_code sah_handle_get_capabilities(fsl_shw_uco_t* user_ctx,
+                                                 uint32_t info);
+
+static void sah_user_callback(fsl_shw_uco_t * user_ctx);
+static os_error_code sah_handle_scc_sfree(fsl_shw_uco_t* user_ctx,
+                                          uint32_t info);
+static os_error_code sah_handle_scc_sstatus(fsl_shw_uco_t* user_ctx,
+                                          uint32_t info);
+static os_error_code sah_handle_scc_drop_perms(fsl_shw_uco_t* user_ctx,
+                                               uint32_t info);
+static os_error_code sah_handle_scc_encrypt(fsl_shw_uco_t* user_ctx,
+                                            uint32_t info);
+static os_error_code sah_handle_scc_decrypt(fsl_shw_uco_t* user_ctx,
+                                            uint32_t info);
+
+#ifdef FSL_HAVE_SCC2
+static fsl_shw_return_t register_user_partition(fsl_shw_uco_t * user_ctx,
+						uint32_t user_base,
+						void *kernel_base);
+static fsl_shw_return_t deregister_user_partition(fsl_shw_uco_t * user_ctx,
+						  uint32_t user_base);
+#endif
+
+static os_error_code sah_handle_sk_slot_alloc(uint32_t info);
+static os_error_code sah_handle_sk_slot_dealloc(uint32_t info);
+static os_error_code sah_handle_sk_slot_load(uint32_t info);
+static os_error_code sah_handle_sk_slot_read(uint32_t info);
+static os_error_code sah_handle_sk_slot_decrypt(uint32_t info);
+static os_error_code sah_handle_sk_slot_encrypt(uint32_t info);
+
+/*! Boolean flag for whether interrupt handler needs to be released on exit */
+static unsigned interrupt_registered;
+
+static int handle_sah_ioctl_dar(fsl_shw_uco_t * filp, uint32_t user_space_desc);
+
+#if !defined(CONFIG_DEVFS_FS) || (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+static int sah_read_procfs(char *buf,
+			   char **start,
+			   off_t offset, int count, int *eof, void *data);
+
+static int sah_write_procfs(struct file *file, const char __user * buffer,
+			    unsigned long count, void *data);
+#endif
+
+#if defined(CONFIG_DEVFS_FS) && (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
+
+/* This is a handle to the sahara DEVFS entry. */
+static devfs_handle_t Sahara_devfs_handle;
+
+#else
+
+/* Major number assigned to our device driver */
+static int Major;
+
+/* This is a handle to the sahara PROCFS entry */
+static struct proc_dir_entry *Sahara_procfs_handle;
+
+#endif
+
+uint32_t sah_hw_version;
+
+/* This is the wait queue to this driver.  Linux declaration. */
+DECLARE_WAIT_QUEUE_HEAD(Wait_queue);
+
+/* This is a global variable that is used to track how many times the device
+ * has been opened simultaneously. */
+#ifdef DIAG_DRV_IF
+static int Device_in_use = 0;
+#endif
+
+/* This is the system keystore object */
+fsl_shw_kso_t system_keystore;
+
+/*!
+ * OS-dependent handle used for registering user interface of a driver.
+ */
+static os_driver_reg_t reg_handle;
+
+#ifdef DIAG_DRV_IF
+/* This is for sprintf() to use when constructing output. */
+#define DIAG_MSG_SIZE   1024
+static char Diag_msg[DIAG_MSG_SIZE];
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 18))
+/** Pointer to Sahara clock information.  Initialized during os_dev_init(). */
+static struct clk *sah_clk;
+#endif
+
+/*!
+*******************************************************************************
+* This function gets called when the module is inserted (insmod) into  the
+* running kernel.
+*
+* @brief     SAHARA device initialisation function.
+*
+* @return   0 on success
+* @return   -EBUSY if the device or proc file entry cannot be created.
+* @return   OS_ERROR_NO_MEMORY_S if kernel memory could not be allocated.
+* @return   OS_ERROR_FAIL_S if initialisation of proc entry failed
+*/
+OS_DEV_INIT(sah_init)
+{
+	/* Status variable */
+	int os_error_code = 0;
+
+	interrupt_registered = 0;
+
+	/* Enable the SAHARA Clocks */
+#ifdef DIAG_DRV_IF
+	LOG_KDIAG("SAHARA : Enabling the IPG and AHB clocks\n")
+#endif				/*DIAG_DRV_IF */
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,18))
+	    mxc_clks_enable(SAHARA2_CLK);
+#else
+	{
+		sah_clk = clk_get(NULL, "sahara_clk");
+		if (sah_clk != ERR_PTR(ENOENT))
+			clk_enable(sah_clk);
+	}
+#endif
+
+	/* Check for SPBA need */
+#if defined(CONFIG_ARCH_MXC91231) || defined(CONFIG_ARCH_MXC91321)
+	/* This needs to be a PLATFORM abstraction */
+	if (spba_take_ownership(SPBA_SAHARA, SPBA_MASTER_A)) {
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("Sahara driver could not take ownership of Sahara\n");
+#endif
+		os_error_code = OS_ERROR_FAIL_S;
+	}
+#endif				/* SPBA */
+
+	if (os_error_code == OS_ERROR_OK_S) {
+		sah_hw_version = sah_HW_Read_Version();
+		os_printk("Sahara HW Version is 0x%08x\n", sah_hw_version);
+
+		/* verify code and hardware are version compatible */
+		if ((sah_hw_version != SAHARA_VERSION2)
+		    && (sah_hw_version != SAHARA_VERSION3)) {
+			if (((sah_hw_version >> 8) & 0xff) != SAHARA_VERSION4) {
+				os_printk
+				    ("Sahara HW Version was not expected value.\n");
+				os_error_code = OS_ERROR_FAIL_S;
+			}
+		}
+	}
+
+	if (os_error_code == OS_ERROR_OK_S) {
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("Calling sah_Init_Mem_Map to initialise "
+			  "memory subsystem.");
+#endif
+		/* Do any memory-routine initialization */
+		os_error_code = sah_Init_Mem_Map();
+	}
+
+	if (os_error_code == OS_ERROR_OK_S) {
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("Calling sah_HW_Reset() to Initialise the Hardware.");
+#endif
+		/* Initialise the hardware */
+		os_error_code = sah_HW_Reset();
+		if (os_error_code != OS_ERROR_OK_S) {
+			os_printk
+			    ("sah_HW_Reset() failed to Initialise the Hardware.\n");
+		}
+
+	}
+
+	if (os_error_code == OS_ERROR_OK_S) {
+#if defined(CONFIG_DEVFS_FS) && (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
+		/* Register the DEVFS entry */
+		Sahara_devfs_handle = devfs_register(NULL,
+						     SAHARA_DEVICE_SHORT,
+						     DEVFS_FL_AUTO_DEVNUM,
+						     0, 0,
+						     SAHARA_DEVICE_MODE,
+						     &Fops, NULL);
+		if (Sahara_devfs_handle == NULL) {
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG
+			    ("Registering the DEVFS character device failed.");
+#endif				/* DIAG_DRV_IF */
+			os_error_code = -EBUSY;
+		}
+#else				/* CONFIG_DEVFS_FS */
+		/* Create the PROCFS entry. This is used to report the assigned device
+		 * major number back to user-space. */
+#if 1
+		Sahara_procfs_handle = create_proc_entry(SAHARA_DEVICE_SHORT, 0700,	/* default mode */
+							 NULL);	/* parent dir */
+		if (Sahara_procfs_handle == NULL) {
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("Registering the PROCFS interface failed.");
+#endif				/* DIAG_DRV_IF */
+			os_error_code = OS_ERROR_FAIL_S;
+		} else {
+			Sahara_procfs_handle->nlink = 1;
+			Sahara_procfs_handle->data = 0;
+			Sahara_procfs_handle->read_proc = sah_read_procfs;
+			Sahara_procfs_handle->write_proc = sah_write_procfs;
+		}
+#endif				/* #if 1 */
+	}
+
+	if (os_error_code == OS_ERROR_OK_S) {
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG
+		    ("Calling sah_Queue_Manager_Init() to Initialise the Queue "
+		     "Manager.");
+#endif
+		/* Initialise the Queue Manager */
+		if (sah_Queue_Manager_Init() != FSL_RETURN_OK_S) {
+			os_error_code = -ENOMEM;
+		}
+	}
+#ifndef SAHARA_POLL_MODE
+	if (os_error_code == OS_ERROR_OK_S) {
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("Calling sah_Intr_Init() to Initialise the Interrupt "
+			  "Handler.");
+#endif
+		/* Initialise the Interrupt Handler */
+		os_error_code = sah_Intr_Init(&Wait_queue);
+		if (os_error_code == OS_ERROR_OK_S) {
+			interrupt_registered = 1;
+		}
+	}
+#endif				/* ifndef SAHARA_POLL_MODE */
+
+#ifdef SAHARA_POWER_MANAGEMENT
+	if (os_error_code == OS_ERROR_OK_S) {
+		/* set up dynamic power management (dmp) */
+		os_error_code = sah_dpm_init();
+	}
+#endif
+
+	if (os_error_code == OS_ERROR_OK_S) {
+		os_driver_init_registration(reg_handle);
+		os_driver_add_registration(reg_handle, OS_FN_OPEN,
+					   OS_DEV_OPEN_REF(sah_open));
+		os_driver_add_registration(reg_handle, OS_FN_IOCTL,
+					   OS_DEV_IOCTL_REF(sah_ioctl));
+		os_driver_add_registration(reg_handle, OS_FN_CLOSE,
+					   OS_DEV_CLOSE_REF(sah_release));
+		os_driver_add_registration(reg_handle, OS_FN_MMAP,
+					   OS_DEV_MMAP_REF(sah_mmap));
+
+		os_error_code =
+		    os_driver_complete_registration(reg_handle, Major,
+						    "sahara");
+
+		if (os_error_code < OS_ERROR_OK_S) {
+#ifdef DIAG_DRV_IF
+			snprintf(Diag_msg, DIAG_MSG_SIZE,
+				 "Registering the regular "
+				 "character device failed with error code: %d\n",
+				 os_error_code);
+			LOG_KDIAG(Diag_msg);
+#endif
+		}
+	}
+#endif				/* CONFIG_DEVFS_FS */
+
+	if (os_error_code == OS_ERROR_OK_S) {
+		/* set up the system keystore, using the default keystore handler */
+		fsl_shw_init_keystore_default(&system_keystore);
+
+		if (fsl_shw_establish_keystore(NULL, &system_keystore)
+		    == FSL_RETURN_OK_S) {
+			os_error_code = OS_ERROR_OK_S;
+		} else {
+			os_error_code = OS_ERROR_FAIL_S;
+		}
+
+		if (os_error_code != OS_ERROR_OK_S) {
+#ifdef DIAG_DRV_IF
+			snprintf(Diag_msg, DIAG_MSG_SIZE,
+				 "Registering the system keystore "
+				 "failed with error code: %d\n", os_error_code);
+			LOG_KDIAG(Diag_msg);
+#endif
+		}
+	}
+
+	if (os_error_code != OS_ERROR_OK_S) {
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
+		cleanup_module();
+#else
+		sah_cleanup();
+#endif
+	}
+#ifdef DIAG_DRV_IF
+	else {
+		LOG_KDIAG_ARGS("Sahara major node is %d\n", Major);
+	}
+#endif
+
+/* Disabling the Clock after the driver has been registered fine.
+    This is done to save power when Sahara is not in use.*/
+#ifdef DIAG_DRV_IF
+	LOG_KDIAG("SAHARA : Disabling the clocks\n")
+#endif				/* DIAG_DRV_IF */
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+		mxc_clks_disable(SAHARA2_CLK);
+#else
+	{
+		if (sah_clk != ERR_PTR(ENOENT))
+			clk_disable(sah_clk);
+	}
+#endif
+
+	os_dev_init_return(os_error_code);
+}
+
+/*!
+*******************************************************************************
+* This function gets called when the module is removed (rmmod) from the running
+* kernel.
+*
+* @brief     SAHARA device clean-up function.
+*
+* @return   void
+*/
+OS_DEV_SHUTDOWN(sah_cleanup)
+{
+	int ret_val = 0;
+
+	printk(KERN_ALERT "Sahara going into cleanup\n");
+
+	/* clear out the system keystore */
+	fsl_shw_release_keystore(NULL, &system_keystore);
+
+	/* Unregister the device */
+#if defined(CONFIG_DEVFS_FS) && (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
+	devfs_unregister(Sahara_devfs_handle);
+#else
+
+	if (Sahara_procfs_handle != NULL) {
+		remove_proc_entry(SAHARA_DEVICE_SHORT, NULL);
+	}
+
+	if (Major >= 0) {
+		ret_val = os_driver_remove_registration(reg_handle);
+	}
+#ifdef DIAG_DRV_IF
+	if (ret_val < 0) {
+		snprintf(Diag_msg, DIAG_MSG_SIZE, "Error while attempting to "
+			 "unregister the device: %d\n", ret_val);
+		LOG_KDIAG(Diag_msg);
+	}
+#endif
+
+#endif				/* CONFIG_DEVFS_FS */
+	sah_Queue_Manager_Close();
+
+#ifndef SAHARA_POLL_MODE
+	if (interrupt_registered) {
+		sah_Intr_Release();
+		interrupt_registered = 0;
+	}
+#endif
+	sah_Stop_Mem_Map();
+#ifdef SAHARA_POWER_MANAGEMENT
+	sah_dpm_close();
+#endif
+
+#ifdef DIAG_DRV_IF
+	LOG_KDIAG("SAHARA : Disabling the clocks\n")
+#endif				/* DIAG_DRV_IF */
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,18))
+	    mxc_clks_disable(SAHARA2_CLK);
+#else
+	{
+		if (sah_clk != ERR_PTR(ENOENT))
+			clk_disable(sah_clk);
+		clk_put(sah_clk);
+	}
+#endif
+
+	os_dev_shutdown_return(OS_ERROR_OK_S);
+}
+
+/*!
+*******************************************************************************
+* This function simply increments the module usage count.
+*
+* @brief     SAHARA device open function.
+*
+* @param    inode Part of the kernel prototype.
+* @param    file Part of the kernel prototype.
+*
+* @return   0 - Always returns 0 since any number of calls to this function are
+*               allowed.
+*
+*/
+OS_DEV_OPEN(sah_open)
+{
+
+#if defined(LINUX_VERSION) && (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10))
+	MOD_INC_USE_COUNT;
+#endif
+
+#ifdef DIAG_DRV_IF
+	Device_in_use++;
+	snprintf(Diag_msg, DIAG_MSG_SIZE,
+		 "Incrementing module use count to: %d ", Device_in_use);
+	LOG_KDIAG(Diag_msg);
+#endif
+
+	os_dev_set_user_private(NULL);
+
+	/* Return 0 to indicate success */
+	os_dev_open_return(0);
+}
+
+/*!
+*******************************************************************************
+* This function simply decrements the module usage count.
+*
+* @brief     SAHARA device release function.
+*
+* @param    inode   Part of the kernel prototype.
+* @param    file    Part of the kernel prototype.
+*
+* @return   0 - Always returns 0 since this function does not fail.
+*/
+OS_DEV_CLOSE(sah_release)
+{
+	fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+#if defined(LINUX_VERSION) && (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10))
+	MOD_DEC_USE_COUNT;
+#endif
+
+#ifdef DIAG_DRV_IF
+	Device_in_use--;
+	snprintf(Diag_msg, DIAG_MSG_SIZE,
+		 "Decrementing module use count to: %d ", Device_in_use);
+	LOG_KDIAG(Diag_msg);
+#endif
+
+	if (user_ctx != NULL) {
+		sah_handle_deregistration(user_ctx);
+		os_free_memory(user_ctx);
+		os_dev_set_user_private(NULL);
+	}
+
+	/* Return 0 to indicate success */
+	os_dev_close_return(OS_ERROR_OK_S);
+}
+
+/*!
+*******************************************************************************
+* This function provides the IO Controls for the SAHARA driver. Three IO
+* Controls are supported:
+*
+*   SAHARA_HWRESET and
+*   SAHARA_SET_HA
+*   SAHARA_CHK_TEST_MODE
+*
+* @brief SAHARA device IO Control function.
+*
+* @param    inode   Part of the kernel prototype.
+* @param    filp    Part of the kernel prototype.
+* @param    cmd     Part of the kernel prototype.
+* @param    arg     Part of the kernel prototype.
+*
+* @return   0 on success
+* @return   -EBUSY if the HA bit could not be set due to busy hardware.
+* @return   -ENOTTY if an unsupported IOCTL was attempted on the device.
+* @return   -EFAULT if put_user() fails
+*/
+OS_DEV_IOCTL(sah_ioctl)
+{
+	int status = 0;
+	int test_mode;
+
+	switch (os_dev_get_ioctl_op()) {
+	case SAHARA_HWRESET:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_HWRESET IOCTL.");
+#endif
+		/* We need to reset the hardware. */
+		sah_HW_Reset();
+
+		/* Mark all the entries in the Queue Manager's queue with state
+		 * SAH_STATE_RESET.
+		 */
+		sah_Queue_Manager_Reset_Entries();
+
+		/* Wake up all sleeping write() calls. */
+		wake_up_interruptible(&Wait_queue);
+		break;
+#ifdef SAHARA_HA_ENABLED
+	case SAHARA_SET_HA:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SET_HA IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		if (sah_HW_Set_HA() == ERR_INTERNAL) {
+			status = -EBUSY;
+		}
+		break;
+#endif				/* SAHARA_HA_ENABLED */
+
+	case SAHARA_CHK_TEST_MODE:
+		/* load test_mode */
+		test_mode = TEST_MODE_OFF;
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_CHECK_TEST_MODE IOCTL.");
+		test_mode = TEST_MODE_ON;
+#endif				/* DIAG_DRV_IF */
+#if defined(KERNEL_TEST) || defined(PERF_TEST)
+		test_mode = TEST_MODE_ON;
+#endif				/* KERNEL_TEST || PERF_TEST */
+		/* copy test_mode back to user space.  put_user() is Linux fn */
+		/* compiler warning `register': no problem found so ignored */
+		status = put_user(test_mode, (int *)os_dev_get_ioctl_arg());
+		break;
+
+	case SAHARA_DAR:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_DAR IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		{
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+			if (user_ctx != NULL) {
+				status =
+				    handle_sah_ioctl_dar(user_ctx,
+							 os_dev_get_ioctl_arg
+							 ());
+			} else {
+				status = OS_ERROR_FAIL_S;
+			}
+
+		}
+		break;
+
+	case SAHARA_GET_RESULTS:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_GET_RESULTS IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		{
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+			if (user_ctx != NULL) {
+				status =
+				    sah_get_results_pointers(user_ctx,
+							     os_dev_get_ioctl_arg
+							     ());
+			} else {
+				status = OS_ERROR_FAIL_S;
+			}
+		}
+		break;
+
+	case SAHARA_REGISTER:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_REGISTER IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		{
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+			if (user_ctx != NULL) {
+				status = OS_ERROR_FAIL_S;	/* already registered */
+			} else {
+				user_ctx =
+				    os_alloc_memory(sizeof(fsl_shw_uco_t),
+						    GFP_KERNEL);
+				if (user_ctx == NULL) {
+					status = OS_ERROR_NO_MEMORY_S;
+				} else {
+					/* Copy UCO from user, but only as big as the common UCO */
+					if (os_copy_from_user(user_ctx,
+							      (void *)
+							      os_dev_get_ioctl_arg
+							      (),
+							      offsetof
+							      (fsl_shw_uco_t,
+							       result_pool))) {
+						status = OS_ERROR_FAIL_S;
+					} else {
+						os_dev_set_user_private
+						    (user_ctx);
+						status =
+						    sah_handle_registration
+						    (user_ctx);
+					}
+				}
+			}
+		}
+		break;
+
+		/* This ioctl cmd should disappear in favor of a close() routine.  */
+	case SAHARA_DEREGISTER:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_DEREGISTER IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		{
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+			if (user_ctx == NULL) {
+				status = OS_ERROR_FAIL_S;
+			} else {
+				status = sah_handle_deregistration(user_ctx);
+				os_free_memory(user_ctx);
+				os_dev_set_user_private(NULL);
+			}
+		}
+		break;
+	case SAHARA_SCC_DROP_PERMS:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SCC_DROP_PERMS IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		{
+			/* drop permissions on the specified partition */
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+			status =
+			    sah_handle_scc_drop_perms(user_ctx,
+						      os_dev_get_ioctl_arg());
+		}
+		break;
+
+	case SAHARA_SCC_SFREE:
+		/* Unmap the specified partition from the users space, and then
+		 * free it for use by someone else.
+		 */
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SCC_SFREE IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		{
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+			status =
+			    sah_handle_scc_sfree(user_ctx,
+						 os_dev_get_ioctl_arg());
+		}
+		break;
+
+	case SAHARA_SCC_SSTATUS:
+		/* Unmap the specified partition from the users space, and then
+		 * free it for use by someone else.
+		 */
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SCC_SSTATUS IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		{
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+			status =
+			    sah_handle_scc_sstatus(user_ctx,
+						   os_dev_get_ioctl_arg());
+		}
+		break;
+
+	case SAHARA_SCC_ENCRYPT:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SCC_ENCRYPT IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		{
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+			status =
+			    sah_handle_scc_encrypt(user_ctx,
+						   os_dev_get_ioctl_arg());
+		}
+		break;
+
+	case SAHARA_SCC_DECRYPT:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SCC_DECRYPT IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		{
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+			status =
+			    sah_handle_scc_decrypt(user_ctx,
+						   os_dev_get_ioctl_arg());
+		}
+		break;
+
+	case SAHARA_SK_ALLOC:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SK_ALLOC IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		status = sah_handle_sk_slot_alloc(os_dev_get_ioctl_arg());
+		break;
+
+	case SAHARA_SK_DEALLOC:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SK_DEALLOC IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		status = sah_handle_sk_slot_dealloc(os_dev_get_ioctl_arg());
+		break;
+
+	case SAHARA_SK_LOAD:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SK_LOAD IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		status = sah_handle_sk_slot_load(os_dev_get_ioctl_arg());
+		break;
+	case SAHARA_SK_READ:
+#ifdef DIAG_DRV_IF
+				LOG_KDIAG("SAHARA_SK_READ IOCTL.");
+#endif				/* DIAG_DRV_IF */
+				status = sah_handle_sk_slot_read(os_dev_get_ioctl_arg());
+				break;
+
+	case SAHARA_SK_SLOT_DEC:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SK_SLOT_DECRYPT IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		status = sah_handle_sk_slot_decrypt(os_dev_get_ioctl_arg());
+		break;
+
+	case SAHARA_SK_SLOT_ENC:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_SK_SLOT_ENCRYPT IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		status = sah_handle_sk_slot_encrypt(os_dev_get_ioctl_arg());
+		break;
+	case SAHARA_GET_CAPS:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA_GET_CAPS IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		{
+			fsl_shw_uco_t *user_ctx = os_dev_get_user_private();
+
+			status =
+			    sah_handle_get_capabilities(user_ctx,
+							os_dev_get_ioctl_arg());
+		}
+		break;
+
+	default:
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("Unknown SAHARA IOCTL.");
+#endif				/* DIAG_DRV_IF */
+		status = OS_ERROR_FAIL_S;
+	}
+
+	os_dev_ioctl_return(status);
+}
+
+/* Fill in the user's capabilities structure */
+static os_error_code sah_handle_get_capabilities(fsl_shw_uco_t * user_ctx,
+						 uint32_t info)
+{
+	os_error_code status = OS_ERROR_FAIL_S;
+	fsl_shw_pco_t capabilities;
+
+	status = os_copy_from_user(&capabilities, (void *)info,
+				   sizeof(fsl_shw_pco_t));
+
+	if (status != OS_ERROR_OK_S) {
+		goto out;
+	}
+
+	if (get_capabilities(user_ctx, &capabilities) == FSL_RETURN_OK_S) {
+		status = os_copy_to_user((void *)info, &capabilities,
+					 sizeof(fsl_shw_pco_t));
+	}
+
+      out:
+	return status;
+}
+
+#ifdef FSL_HAVE_SCC2
+
+/* Find the kernel-mode address of the partition.
+ * This can then be passed to the SCC functions.
+ */
+void *lookup_user_partition(fsl_shw_uco_t * user_ctx, uint32_t user_base)
+{
+	/* search through the partition chain to find one that matches the user base
+	 * address.
+	 */
+	fsl_shw_spo_t *curr = (fsl_shw_spo_t *) user_ctx->partition;
+
+	while (curr != NULL) {
+		if (curr->user_base == user_base) {
+			return curr->kernel_base;
+		}
+		curr = (fsl_shw_spo_t *) curr->next;
+	}
+	return NULL;
+}
+
+/* user_base: userspace base address of the partition
+ * kernel_base: kernel mode base address of the partition
+ */
+static fsl_shw_return_t register_user_partition(fsl_shw_uco_t * user_ctx,
+						uint32_t user_base,
+						void *kernel_base)
+{
+	fsl_shw_spo_t *partition_info;
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+
+	if (user_ctx == NULL) {
+		goto out;
+	}
+
+	partition_info = os_alloc_memory(sizeof(fsl_shw_spo_t), GFP_KERNEL);
+
+	if (partition_info == NULL) {
+		goto out;
+	}
+
+	/* stuff the partition info, then put it at the front of the chain */
+	partition_info->user_base = user_base;
+	partition_info->kernel_base = kernel_base;
+	partition_info->next = user_ctx->partition;
+
+	user_ctx->partition = (struct fsl_shw_spo_t *)partition_info;
+
+#ifdef DIAG_DRV_IF
+	LOG_KDIAG_ARGS
+	    ("partition with user_base=%p, kernel_base=%p registered.",
+	     (void *)user_base, kernel_base);
+#endif
+
+	ret = FSL_RETURN_OK_S;
+
+      out:
+
+	return ret;
+}
+
+/* if the partition is in the users list, remove it */
+static fsl_shw_return_t deregister_user_partition(fsl_shw_uco_t * user_ctx,
+						  uint32_t user_base)
+{
+	fsl_shw_spo_t *curr = (fsl_shw_spo_t *) user_ctx->partition;
+	fsl_shw_spo_t *last = (fsl_shw_spo_t *) user_ctx->partition;
+
+	while (curr != NULL) {
+		if (curr->user_base == user_base) {
+
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG_ARGS
+			    ("deregister_user_partition: partition with "
+			     "user_base=%p, kernel_base=%p deregistered.\n",
+			     (void *)curr->user_base, curr->kernel_base);
+#endif
+
+			if (last == curr) {
+				user_ctx->partition = curr->next;
+				os_free_memory(curr);
+				return FSL_RETURN_OK_S;
+			} else {
+				last->next = curr->next;
+				os_free_memory(curr);
+				return FSL_RETURN_OK_S;
+			}
+		}
+		last = curr;
+		curr = (fsl_shw_spo_t *) curr->next;
+	}
+
+	return FSL_RETURN_ERROR_S;
+}
+
+#endif				/* FSL_HAVE_SCC2 */
+
+static os_error_code sah_handle_scc_drop_perms(fsl_shw_uco_t * user_ctx,
+					       uint32_t info)
+{
+	os_error_code status = OS_ERROR_NO_MEMORY_S;
+#ifdef FSL_HAVE_SCC2
+	scc_return_t scc_ret;
+	scc_partition_info_t partition_info;
+	void *kernel_base;
+
+	status =
+	    os_copy_from_user(&partition_info, (void *)info,
+			      sizeof(partition_info));
+
+	if (status != OS_ERROR_OK_S) {
+		goto out;
+	}
+
+	/* validate that the user owns this partition, and look up its handle */
+	kernel_base = lookup_user_partition(user_ctx, partition_info.user_base);
+
+	if (kernel_base == NULL) {
+		status = OS_ERROR_FAIL_S;
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("_scc_drop_perms(): failed to find partition\n");
+#endif
+		goto out;
+	}
+
+	/* call scc driver to perform the drop */
+	scc_ret = scc_diminish_permissions(kernel_base,
+					   partition_info.permissions);
+	if (scc_ret == SCC_RET_OK) {
+		status = OS_ERROR_OK_S;
+	} else {
+		status = OS_ERROR_FAIL_S;
+	}
+
+      out:
+#endif				/* FSL_HAVE_SCC2 */
+	return status;
+}
+
+static os_error_code sah_handle_scc_sfree(fsl_shw_uco_t * user_ctx,
+					  uint32_t info)
+{
+	os_error_code status = OS_ERROR_NO_MEMORY_S;
+#ifdef FSL_HAVE_SCC2
+	{
+		scc_partition_info_t partition_info;
+		void *kernel_base;
+		int ret;
+
+		status =
+		    os_copy_from_user(&partition_info, (void *)info,
+				      sizeof(partition_info));
+
+		/* check that the copy was successful */
+		if (status != OS_ERROR_OK_S) {
+			goto out;
+		}
+
+		/* validate that the user owns this partition, and look up its handle */
+		kernel_base =
+		    lookup_user_partition(user_ctx, partition_info.user_base);
+
+		if (kernel_base == NULL) {
+			status = OS_ERROR_FAIL_S;
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("failed to find partition\n");
+#endif				/*DIAG_DRV_IF */
+			goto out;
+		}
+
+		/* Unmap the memory region (see sys_munmap in mmap.c) */
+		ret = unmap_user_memory(partition_info.user_base, 8192);
+
+		/* If the memory was successfully released */
+		if (ret == OS_ERROR_OK_S) {
+
+			/* release the partition */
+			scc_release_partition(kernel_base);
+
+			/* and remove it from the users context */
+			deregister_user_partition(user_ctx,
+						  partition_info.user_base);
+
+			status = OS_ERROR_OK_S;
+		}
+	}
+      out:
+#endif				/* FSL_HAVE_SCC2 */
+	return status;
+}
+
+static os_error_code sah_handle_scc_sstatus(fsl_shw_uco_t * user_ctx,
+					    uint32_t info)
+{
+	os_error_code status = OS_ERROR_NO_MEMORY_S;
+#ifdef FSL_HAVE_SCC2
+	{
+		scc_partition_info_t partition_info;
+		void *kernel_base;
+
+		status =
+		    os_copy_from_user(&partition_info, (void *)info,
+				      sizeof(partition_info));
+
+		/* check that the copy was successful */
+		if (status != OS_ERROR_OK_S) {
+			goto out;
+		}
+
+		/* validate that the user owns this partition, and look up its handle */
+		kernel_base =
+		    lookup_user_partition(user_ctx, partition_info.user_base);
+
+		if (kernel_base == NULL) {
+			status = OS_ERROR_FAIL_S;
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("failed to find partition\n");
+#endif				/*DIAG_DRV_IF */
+			goto out;
+		}
+
+		partition_info.status = scc_partition_status(kernel_base);
+
+		status =
+		    os_copy_to_user((void *)info, &partition_info,
+				    sizeof(partition_info));
+	}
+      out:
+#endif				/* FSL_HAVE_SCC2 */
+	return status;
+}
+
+static os_error_code sah_handle_scc_encrypt(fsl_shw_uco_t * user_ctx,
+					    uint32_t info)
+{
+	os_error_code os_err = OS_ERROR_FAIL_S;
+#ifdef FSL_HAVE_SCC2
+	{
+		fsl_shw_return_t retval;
+		scc_region_t region_info;
+		void *page_ctx = NULL;
+		void *black_addr = NULL;
+		void *partition_base = NULL;
+		scc_config_t *scc_configuration;
+
+		os_err =
+		    os_copy_from_user(&region_info, (void *)info,
+				      sizeof(region_info));
+
+		if (os_err != OS_ERROR_OK_S) {
+			goto out;
+		}
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG_ARGS
+		    ("partition_base: %p, offset: %i, length: %i, black data: %p",
+		     (void *)region_info.partition_base, region_info.offset,
+		     region_info.length, (void *)region_info.black_data);
+#endif
+
+		/* validate that the user owns this partition, and look up its handle */
+		partition_base = lookup_user_partition(user_ctx,
+						       region_info.
+						       partition_base);
+
+		if (partition_base == NULL) {
+			retval = FSL_RETURN_ERROR_S;
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("failed to find secure partition\n");
+#endif
+			goto out;
+		}
+
+		/* Check that the memory size requested is correct */
+		scc_configuration = scc_get_configuration();
+		if (region_info.offset + region_info.length >
+		    scc_configuration->partition_size_bytes) {
+			retval = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+
+		/* wire down black data */
+		black_addr = wire_user_memory(region_info.black_data,
+					      region_info.length, &page_ctx);
+
+		if (black_addr == NULL) {
+			retval = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+
+		retval =
+		    do_scc_encrypt_region(NULL, partition_base,
+					  region_info.offset,
+					  region_info.length, black_addr,
+					  region_info.IV,
+					  region_info.cypher_mode);
+
+		/* release black data */
+		unwire_user_memory(&page_ctx);
+
+	      out:
+		if (os_err == OS_ERROR_OK_S) {
+			/* Return error code */
+			region_info.code = retval;
+			os_err =
+			    os_copy_to_user((void *)info, &region_info,
+					    sizeof(region_info));
+		}
+	}
+
+#endif
+	return os_err;
+}
+
+static os_error_code sah_handle_scc_decrypt(fsl_shw_uco_t * user_ctx,
+					    uint32_t info)
+{
+	os_error_code os_err = OS_ERROR_FAIL_S;
+#ifdef FSL_HAVE_SCC2
+	{
+		fsl_shw_return_t retval;
+		scc_region_t region_info;
+		void *page_ctx = NULL;
+		void *black_addr;
+		void *partition_base;
+		scc_config_t *scc_configuration;
+
+		os_err =
+		    os_copy_from_user(&region_info, (void *)info,
+				      sizeof(region_info));
+
+		if (os_err != OS_ERROR_OK_S) {
+			goto out;
+		}
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG_ARGS
+		    ("partition_base: %p, offset: %i, length: %i, black data: %p",
+		     (void *)region_info.partition_base, region_info.offset,
+		     region_info.length, (void *)region_info.black_data);
+#endif
+
+		/* validate that the user owns this partition, and look up its handle */
+		partition_base = lookup_user_partition(user_ctx,
+						       region_info.
+						       partition_base);
+
+		if (partition_base == NULL) {
+			retval = FSL_RETURN_ERROR_S;
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("failed to find partition\n");
+#endif
+			goto out;
+		}
+
+		/* Check that the memory size requested is correct */
+		scc_configuration = scc_get_configuration();
+		if (region_info.offset + region_info.length >
+		    scc_configuration->partition_size_bytes) {
+			retval = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+
+		/* wire down black data */
+		black_addr = wire_user_memory(region_info.black_data,
+					      region_info.length, &page_ctx);
+
+		if (black_addr == NULL) {
+			retval = FSL_RETURN_ERROR_S;
+			goto out;
+		}
+
+		retval =
+		    do_scc_decrypt_region(NULL, partition_base,
+					  region_info.offset,
+					  region_info.length, black_addr,
+					  region_info.IV,
+					  region_info.cypher_mode);
+
+		/* release black data */
+		unwire_user_memory(&page_ctx);
+
+	      out:
+		if (os_err == OS_ERROR_OK_S) {
+			/* Return error code */
+			region_info.code = retval;
+			os_err =
+			    os_copy_to_user((void *)info, &region_info,
+					    sizeof(region_info));
+		}
+	}
+
+#endif				/* FSL_HAVE_SCC2 */
+	return os_err;
+}
+
+/*****************************************************************************/
+/* fn get_user_smid()                                                        */
+/*****************************************************************************/
+uint32_t get_user_smid(void *proc)
+{
+	/*
+	 * A real implementation would have some way to handle signed applications
+	 * which wouild be assigned distinct SMIDs.  For the reference
+	 * implementation, we show where this would be determined (here), but
+	 * always provide a fixed answer, thus not separating users at all.
+	 */
+
+	return 0x42eaae42;
+}
+
+/*!
+*******************************************************************************
+* This function implements the smalloc() function for userspace programs, by
+* making a call to the SCC2 mmap() function that acquires a region of secure
+* memory on behalf of the user, and then maps it into the users memory space.
+* Currently, the only memory size supported is that of a single SCC2 partition.
+* Requests for other sized memory regions will fail.
+*/
+OS_DEV_MMAP(sah_mmap)
+{
+	os_error_code status = OS_ERROR_NO_MEMORY_S;
+
+#ifdef FSL_HAVE_SCC2
+	{
+		scc_return_t scc_ret;
+		fsl_shw_return_t fsl_ret;
+		uint32_t partition_registered = FALSE;
+
+		uint32_t user_base;
+		void *partition_base;
+		uint32_t smid;
+		scc_config_t *scc_configuration;
+
+		int part_no = -1;
+		uint32_t part_phys;
+
+		fsl_shw_uco_t *user_ctx =
+		    (fsl_shw_uco_t *) os_dev_get_user_private();
+
+		/* Make sure that the user context is valid */
+		if (user_ctx == NULL) {
+			user_ctx =
+			    os_alloc_memory(sizeof(*user_ctx), GFP_KERNEL);
+
+			if (user_ctx == NULL) {
+				status = OS_ERROR_NO_MEMORY_S;
+				goto out;
+			}
+
+			sah_handle_registration(user_ctx);
+			os_dev_set_user_private(user_ctx);
+		}
+
+		/* Determine the size of a secure partition */
+		scc_configuration = scc_get_configuration();
+
+		/* Check that the memory size requested is equal to the partition
+		 * size, and that the requested destination is on a page boundary.
+		 */
+		if (((os_mmap_user_base() % PAGE_SIZE) != 0) ||
+		    (os_mmap_memory_size() !=
+		     scc_configuration->partition_size_bytes)) {
+			status = OS_ERROR_BAD_ARG_S;
+			goto out;
+		}
+
+		/* Retrieve the SMID associated with the user */
+		smid = get_user_smid(user_ctx->process);
+
+		/* Attempt to allocate a secure partition */
+		scc_ret =
+		    scc_allocate_partition(smid, &part_no, &partition_base,
+					   &part_phys);
+		if (scc_ret != SCC_RET_OK) {
+			pr_debug
+			    ("SCC mmap() request failed to allocate partition;"
+			     " error %d\n", status);
+			status = OS_ERROR_FAIL_S;
+			goto out;
+		}
+
+		pr_debug("scc_mmap() acquired partition %d at %08x\n",
+			 part_no, part_phys);
+
+		/* Record partition info in the user context */
+		user_base = os_mmap_user_base();
+		fsl_ret =
+		    register_user_partition(user_ctx, user_base,
+					    partition_base);
+
+		if (fsl_ret != FSL_RETURN_OK_S) {
+			pr_debug
+			    ("SCC mmap() request failed to register partition with user"
+			     " context, error: %d\n", fsl_ret);
+			status = OS_ERROR_FAIL_S;
+		}
+
+		partition_registered = TRUE;
+
+		status = map_user_memory(os_mmap_memory_ctx(), part_phys,
+					 os_mmap_memory_size());
+
+#ifdef SHW_DEBUG
+		if (status == OS_ERROR_OK_S) {
+			LOG_KDIAG_ARGS
+			    ("Partition allocated: user_base=%p, partition_base=%p.",
+			     (void *)user_base, partition_base);
+		}
+#endif
+
+	      out:
+		/* If there is an error it has to be handled here */
+		if (status != OS_ERROR_OK_S) {
+			/* if the partition was registered with the user, unregister it. */
+			if (partition_registered == TRUE) {
+				deregister_user_partition(user_ctx, user_base);
+			}
+
+			/* if the partition was allocated, deallocate it */
+			if (partition_base != NULL) {
+				scc_release_partition(partition_base);
+			}
+		}
+	}
+#endif				/* FSL_HAVE_SCC2 */
+
+	return status;
+}
+
+/* Find the physical address of a key stored in the system keystore */
+fsl_shw_return_t
+system_keystore_get_slot_info(uint64_t owner_id, uint32_t slot,
+			      uint32_t * address, uint32_t * slot_size_bytes)
+{
+	fsl_shw_return_t retval;
+	void *kernel_address;
+
+	/* First verify that the key access is valid */
+	retval = system_keystore.slot_verify_access(system_keystore.user_data,
+						    owner_id, slot);
+
+	if (retval != FSL_RETURN_OK_S) {
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("verification failed");
+#endif
+		return retval;
+	}
+
+	if (address != NULL) {
+#ifdef FSL_HAVE_SCC2
+		kernel_address =
+		    system_keystore.slot_get_address(system_keystore.user_data,
+						     slot);
+		(*address) = scc_virt_to_phys(kernel_address);
+#else
+		kernel_address =
+		    system_keystore.slot_get_address((void *)&owner_id, slot);
+		(*address) = (uint32_t) kernel_address;
+#endif
+	}
+
+	if (slot_size_bytes != NULL) {
+#ifdef FSL_HAVE_SCC2
+		*slot_size_bytes =
+		    system_keystore.slot_get_slot_size(system_keystore.
+						       user_data, slot);
+#else
+		*slot_size_bytes =
+		    system_keystore.slot_get_slot_size((void *)&owner_id, slot);
+#endif
+	}
+
+	return retval;
+}
+
+static os_error_code sah_handle_sk_slot_alloc(uint32_t info)
+{
+	scc_slot_t slot_info;
+	os_error_code os_err;
+	scc_return_t scc_ret;
+
+	os_err = os_copy_from_user(&slot_info, (void *)info, sizeof(slot_info));
+	if (os_err == OS_ERROR_OK_S) {
+		scc_ret = keystore_slot_alloc(&system_keystore,
+					      slot_info.key_length,
+					      slot_info.ownerid,
+					      &slot_info.slot);
+		if (scc_ret == SCC_RET_OK) {
+			slot_info.code = FSL_RETURN_OK_S;
+		} else if (scc_ret == SCC_RET_INSUFFICIENT_SPACE) {
+			slot_info.code = FSL_RETURN_NO_RESOURCE_S;
+		} else {
+			slot_info.code = FSL_RETURN_ERROR_S;
+		}
+
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG_ARGS("key length: %i, handle: %i\n",
+			       slot_info.key_length, slot_info.slot);
+#endif
+
+		/* Return error code and slot info */
+		os_err =
+		    os_copy_to_user((void *)info, &slot_info,
+				    sizeof(slot_info));
+
+		if (os_err != OS_ERROR_OK_S) {
+			(void)keystore_slot_dealloc(&system_keystore,
+						    slot_info.ownerid,
+						    slot_info.slot);
+		}
+	}
+
+	return os_err;
+}
+
+static os_error_code sah_handle_sk_slot_dealloc(uint32_t info)
+{
+	fsl_shw_return_t ret = FSL_RETURN_INTERNAL_ERROR_S;
+	scc_slot_t slot_info;
+	os_error_code os_err;
+	scc_return_t scc_ret;
+
+	os_err = os_copy_from_user(&slot_info, (void *)info, sizeof(slot_info));
+
+	if (os_err == OS_ERROR_OK_S) {
+		scc_ret = keystore_slot_dealloc(&system_keystore,
+						slot_info.ownerid,
+						slot_info.slot);
+
+		if (scc_ret == SCC_RET_OK) {
+			ret = FSL_RETURN_OK_S;
+		} else {
+			ret = FSL_RETURN_ERROR_S;
+		}
+		slot_info.code = ret;
+
+		os_err =
+		    os_copy_to_user((void *)info, &slot_info,
+				    sizeof(slot_info));
+	}
+
+	return os_err;
+}
+
+static os_error_code sah_handle_sk_slot_load(uint32_t info)
+{
+	fsl_shw_return_t ret = FSL_RETURN_INTERNAL_ERROR_S;
+	scc_slot_t slot_info;
+	os_error_code os_err;
+	uint8_t *key = NULL;
+
+	os_err = os_copy_from_user(&slot_info, (void *)info, sizeof(slot_info));
+
+	if (os_err == OS_ERROR_OK_S) {
+		/* Allow slop in alloc in case we are rounding up to word multiple */
+		key = os_alloc_memory(slot_info.key_length + 3, GFP_KERNEL);
+		if (key == NULL) {
+			ret = FSL_RETURN_NO_RESOURCE_S;
+			os_err = OS_ERROR_NO_MEMORY_S;
+		} else {
+			os_err = os_copy_from_user(key, slot_info.key,
+						   slot_info.key_length);
+		}
+	}
+
+	if (os_err == OS_ERROR_OK_S) {
+		unsigned key_length = slot_info.key_length;
+
+		/* Round up if necessary, as SCC call wants a multiple of 32-bit
+		 * values for the full object being loaded. */
+		if ((key_length & 3) != 0) {
+			key_length += 4 - (key_length & 3);
+		}
+		ret = keystore_slot_load(&system_keystore,
+					 slot_info.ownerid, slot_info.slot, key,
+					 key_length);
+
+		slot_info.code = ret;
+		os_err =
+		    os_copy_to_user((void *)info, &slot_info,
+				    sizeof(slot_info));
+	}
+
+	if (key != NULL) {
+		memset(key, 0, slot_info.key_length);
+		os_free_memory(key);
+	}
+
+	return os_err;
+}
+
+static os_error_code sah_handle_sk_slot_read(uint32_t info)
+{
+	fsl_shw_return_t ret = FSL_RETURN_INTERNAL_ERROR_S;
+	scc_slot_t slot_info;
+	os_error_code os_err;
+	uint8_t *key = NULL;
+
+	os_err = os_copy_from_user(&slot_info, (void *)info, sizeof(slot_info));
+
+	if (os_err == OS_ERROR_OK_S) {
+
+		/* This operation is not allowed for user keys */
+		slot_info.code = FSL_RETURN_NO_RESOURCE_S;
+		os_err =
+		    os_copy_to_user((void *)info, &slot_info,
+				    sizeof(slot_info));
+
+		return os_err;
+	}
+
+	if (os_err == OS_ERROR_OK_S) {
+		/* Allow slop in alloc in case we are rounding up to word multiple */
+		key = os_alloc_memory(slot_info.key_length + 3, GFP_KERNEL);
+		if (key == NULL) {
+			ret = FSL_RETURN_NO_RESOURCE_S;
+			os_err = OS_ERROR_NO_MEMORY_S;
+		}
+	}
+
+	if (os_err == OS_ERROR_OK_S) {
+		unsigned key_length = slot_info.key_length;
+
+		/* @bug Do some PERMISSIONS checking - make sure this is SW key */
+
+		/* Round up if necessary, as SCC call wants a multiple of 32-bit
+		 * values for the full object being loaded. */
+		if ((key_length & 3) != 0) {
+			key_length += 4 - (key_length & 3);
+		}
+		ret = keystore_slot_read(&system_keystore,
+					 slot_info.ownerid, slot_info.slot,
+					 key_length, key);
+
+		/* @bug do some error checking */
+
+		/* Send key back to user */
+		os_err = os_copy_to_user(slot_info.key, key,
+					 slot_info.key_length);
+
+		slot_info.code = ret;
+		os_err =
+		    os_copy_to_user((void *)info, &slot_info,
+				    sizeof(slot_info));
+	}
+
+	if (key != NULL) {
+		memset(key, 0, slot_info.key_length);
+		os_free_memory(key);
+	}
+
+	return os_err;
+}
+
+static os_error_code sah_handle_sk_slot_encrypt(uint32_t info)
+{
+	fsl_shw_return_t ret = FSL_RETURN_INTERNAL_ERROR_S;
+	scc_slot_t slot_info;
+	os_error_code os_err;
+	scc_return_t scc_ret;
+	uint8_t *key = NULL;
+
+	os_err = os_copy_from_user(&slot_info, (void *)info, sizeof(slot_info));
+
+	if (os_err == OS_ERROR_OK_S) {
+		key = os_alloc_memory(slot_info.key_length, GFP_KERNEL);
+		if (key == NULL) {
+			ret = FSL_RETURN_NO_RESOURCE_S;
+		}
+	}
+
+	if (key != NULL) {
+
+		scc_ret = keystore_slot_encrypt(NULL, &system_keystore,
+						slot_info.ownerid,
+						slot_info.slot,
+						slot_info.key_length, key);
+
+		if (scc_ret != SCC_RET_OK) {
+			ret = FSL_RETURN_ERROR_S;
+		} else {
+			os_err =
+			    os_copy_to_user(slot_info.key, key,
+					    slot_info.key_length);
+			if (os_err != OS_ERROR_OK_S) {
+				ret = FSL_RETURN_INTERNAL_ERROR_S;
+			} else {
+				ret = FSL_RETURN_OK_S;
+			}
+		}
+
+		slot_info.code = ret;
+		os_err =
+		    os_copy_to_user((void *)info, &slot_info,
+				    sizeof(slot_info));
+
+		memset(key, 0, slot_info.key_length);
+		os_free_memory(key);
+	}
+
+	return os_err;
+}
+
+static os_error_code sah_handle_sk_slot_decrypt(uint32_t info)
+{
+	fsl_shw_return_t ret = FSL_RETURN_INTERNAL_ERROR_S;
+	scc_slot_t slot_info;	/*!< decrypt request fields */
+	os_error_code os_err;
+	scc_return_t scc_ret;
+	uint8_t *key = NULL;
+
+	os_err = os_copy_from_user(&slot_info, (void *)info, sizeof(slot_info));
+
+	if (os_err == OS_ERROR_OK_S) {
+		key = os_alloc_memory(slot_info.key_length, GFP_KERNEL);
+		if (key == NULL) {
+			ret = FSL_RETURN_NO_RESOURCE_S;
+			os_err = OS_ERROR_OK_S;
+		} else {
+			os_err = os_copy_from_user(key, slot_info.key,
+						   slot_info.key_length);
+		}
+	}
+
+	if (os_err == OS_ERROR_OK_S) {
+		scc_ret = keystore_slot_decrypt(NULL, &system_keystore,
+						slot_info.ownerid,
+						slot_info.slot,
+						slot_info.key_length, key);
+		if (scc_ret == SCC_RET_OK) {
+			ret = FSL_RETURN_OK_S;
+		} else {
+			ret = FSL_RETURN_ERROR_S;
+		}
+
+		slot_info.code = ret;
+		os_err =
+		    os_copy_to_user((void *)info, &slot_info,
+				    sizeof(slot_info));
+	}
+
+	if (key != NULL) {
+		memset(key, 0, slot_info.key_length);
+		os_free_memory(key);
+	}
+
+	return os_err;
+}
+
+/*!
+ * Register a user
+ *
+ * @brief Register a user
+  *
+ * @param  user_ctx  information about this user
+ *
+ * @return   status code
+ */
+fsl_shw_return_t sah_handle_registration(fsl_shw_uco_t * user_ctx)
+{
+	/* Initialize the user's result pool (like sah_Queue_Construct() */
+	user_ctx->result_pool.head = NULL;
+	user_ctx->result_pool.tail = NULL;
+	user_ctx->result_pool.count = 0;
+
+	/* initialize the user's partition chain */
+	user_ctx->partition = NULL;
+
+	return FSL_RETURN_OK_S;
+}
+
+/*!
+ * Deregister a user
+ *
+ * @brief Deregister a user
+ *
+ * @param  user_ctx  information about this user
+ *
+ * @return   status code
+ */
+fsl_shw_return_t sah_handle_deregistration(fsl_shw_uco_t * user_ctx)
+{
+	/* NOTE:
+	 * This will release any secure partitions that are held by the user.
+	 * Encryption keys that were placed in the system keystore by the user
+	 * should not be removed here, because they might have been shared with
+	 * another process.  The user must be careful to release any that are no
+	 * longer in use.
+	 */
+	fsl_shw_return_t ret = FSL_RETURN_OK_S;
+
+#ifdef FSL_HAVE_SCC2
+	fsl_shw_spo_t *partition;
+	struct mm_struct *mm = current->mm;
+
+	while ((user_ctx->partition != NULL) && (ret == FSL_RETURN_OK_S)) {
+
+		partition = user_ctx->partition;
+
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG_ARGS
+		    ("Found an abandoned secure partition at %p, releasing",
+		     partition);
+#endif
+
+		/* It appears that current->mm is not valid if this is called from a
+		 * close routine (perhaps only if the program raised an exception that
+		 * caused it to close?)  If that is the case, then still free the 
+		 * partition, but do not remove it from the memory space (dangerous?)
+		 */
+
+		if (mm == NULL) {
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG
+			    ("Warning: no mm structure found, not unmapping "
+			     "partition from user memory\n");
+#endif
+		} else {
+			/* Unmap the memory region (see sys_munmap in mmap.c) */
+			/* Note that this assumes a single memory partition */
+			unmap_user_memory(partition->user_base, 8192);
+		}
+
+		/* If the memory was successfully released */
+		if (ret == OS_ERROR_OK_S) {
+			/* release the partition */
+			scc_release_partition(partition->kernel_base);
+
+			/* and remove it from the users context */
+			deregister_user_partition(user_ctx,
+						  partition->user_base);
+
+			ret = FSL_RETURN_OK_S;
+		} else {
+			ret = FSL_RETURN_ERROR_S;
+
+			goto out;
+		}
+	}
+      out:
+#endif				/* FSL_HAVE_SCC2 */
+
+	return ret;
+}
+
+/*!
+ * Sets up memory to extract results from results pool
+ *
+ * @brief Sets up memory to extract results from results pool
+ *
+ * @param  user_ctx  information about this user
+ * @param[in,out]   arg contains input parameters and fields that the driver
+ *                   fills in
+ *
+ * @return   os error code or 0 on success
+ */
+int sah_get_results_pointers(fsl_shw_uco_t * user_ctx, uint32_t arg)
+{
+	sah_results results_arg;	/* kernel mode usable version of 'arg' */
+	fsl_shw_result_t *user_results;	/* user mode address of results */
+	unsigned *user_actual;	/* user mode address of actual number of results */
+	unsigned actual;	/* local memory of actual number of results */
+	int ret_val = OS_ERROR_FAIL_S;
+	sah_Head_Desc *finished_request;
+	unsigned int loop;
+
+	/* copy structure from user to kernel space */
+	if (!os_copy_from_user(&results_arg, (void *)arg, sizeof(sah_results))) {
+		/* save user space pointers */
+		user_actual = results_arg.actual;	/* where count goes */
+		user_results = results_arg.results;	/* where results goe */
+
+		/* Set pointer for actual value to temporary kernel memory location */
+		results_arg.actual = &actual;
+
+		/* Allocate kernel memory to hold temporary copy of the results */
+		results_arg.results =
+		    os_alloc_memory(sizeof(fsl_shw_result_t) *
+				    results_arg.requested, GFP_KERNEL);
+
+		/* if memory allocated, continue */
+		if (results_arg.results == NULL) {
+			ret_val = OS_ERROR_NO_MEMORY_S;
+		} else {
+			fsl_shw_return_t get_status;
+
+			/* get the results */
+			get_status =
+			    sah_get_results_from_pool(user_ctx, &results_arg);
+
+			/* free the copy of the user space descriptor chain */
+			for (loop = 0; loop < actual; ++loop) {
+				/* get sah_Head_Desc from results and put user address into
+				 * the return structure */
+				finished_request =
+				    results_arg.results[loop].user_desc;
+				results_arg.results[loop].user_desc =
+				    finished_request->user_desc;
+				/* return the descriptor chain memory to the block free pool */
+				sah_Free_Chained_Descriptors(finished_request);
+			}
+
+			/* if no errors, copy results and then the actual number of results
+			 * back to user space
+			 */
+			if (get_status == FSL_RETURN_OK_S) {
+				if (os_copy_to_user
+				    (user_results, results_arg.results,
+				     actual * sizeof(fsl_shw_result_t))
+				    || os_copy_to_user(user_actual, &actual,
+						       sizeof(user_actual))) {
+					ret_val = OS_ERROR_FAIL_S;
+				} else {
+					ret_val = 0;	/* no error */
+				}
+			}
+			/* free the allocated memory */
+			os_free_memory(results_arg.results);
+		}
+	}
+
+	return ret_val;
+}
+
+/*!
+ * Extracts results from results pool
+ *
+ * @brief Extract results from results pool
+ *
+ * @param           user_ctx  information about this user
+ * @param[in,out]   arg       contains input parameters and fields that the
+ *                            driver fills in
+ *
+ * @return   status code
+ */
+fsl_shw_return_t sah_get_results_from_pool(volatile fsl_shw_uco_t * user_ctx,
+					   sah_results * arg)
+{
+	sah_Head_Desc *finished_request;
+	unsigned int loop = 0;
+	os_lock_context_t int_flags;
+
+	/* Get the number of results requested, up to total number of results
+	 * available
+	 */
+	do {
+		/* Protect state of user's result pool until we have retrieved and
+		 * remove the first entry, or determined that the pool is empty. */
+		os_lock_save_context(desc_queue_lock, int_flags);
+		finished_request = user_ctx->result_pool.head;
+
+		if (finished_request != NULL) {
+			sah_Queue_Remove_Entry((sah_Queue *) & user_ctx->
+					       result_pool);
+			os_unlock_restore_context(desc_queue_lock, int_flags);
+
+			/* Prepare to free. */
+			(void)sah_DePhysicalise_Descriptors(finished_request);
+
+			arg->results[loop].user_ref =
+			    finished_request->user_ref;
+			arg->results[loop].code = finished_request->result;
+			arg->results[loop].detail1 =
+			    finished_request->fault_address;
+			arg->results[loop].detail2 = 0;
+			arg->results[loop].user_desc = finished_request;
+
+			loop++;
+		} else {	/* finished_request is NULL */
+			/* pool is empty */
+			os_unlock_restore_context(desc_queue_lock, int_flags);
+		}
+
+	} while ((loop < arg->requested) && (finished_request != NULL));
+
+	/* record number of results actually obtained */
+	*arg->actual = loop;
+
+	return FSL_RETURN_OK_S;
+}
+
+/*!
+ * Converts descriptor chain to kernel space (from user space) and submits
+ * chain to Sahara for processing
+ *
+ * @brief Submits converted descriptor chain to sahara
+ *
+ * @param  user_ctx        Pointer to Kernel version of user's ctx
+ * @param  user_space_desc user space address of descriptor chain that is
+ *                         in user space
+ *
+ * @return   OS status code
+ */
+static int handle_sah_ioctl_dar(fsl_shw_uco_t * user_ctx,
+				uint32_t user_space_desc)
+{
+	int os_error_code = OS_ERROR_FAIL_S;
+	sah_Head_Desc *desc_chain_head;	/* chain in kernel - virtual address */
+
+	/* This will re-create the linked list so that the SAHARA hardware can
+	 * DMA on it.
+	 */
+	desc_chain_head = sah_Copy_Descriptors(user_ctx,
+					       (sah_Head_Desc *)
+					       user_space_desc);
+
+	if (desc_chain_head == NULL) {
+		/* We may have failed due to a -EFAULT as well, but we will return
+		 * OS_ERROR_NO_MEMORY_S since either way it is a memory related
+		 * failure.
+		 */
+		os_error_code = OS_ERROR_NO_MEMORY_S;
+	} else {
+		fsl_shw_return_t stat;
+
+		desc_chain_head->user_info = user_ctx;
+		desc_chain_head->user_desc = (sah_Head_Desc *) user_space_desc;
+
+		if (desc_chain_head->uco_flags & FSL_UCO_BLOCKING_MODE) {
+#ifdef SAHARA_POLL_MODE
+			sah_Handle_Poll(desc_chain_head);
+#else
+			sah_blocking_mode(desc_chain_head);
+#endif
+			stat = desc_chain_head->result;
+			/* return the descriptor chain memory to the block free pool */
+			sah_Free_Chained_Descriptors(desc_chain_head);
+			/* Tell user how the call turned out */
+
+			/* Copy 'result' back up to the result member.
+			 *
+			 * The dereference of the different member will cause correct the
+			 * arithmetic to occur on the user-space address because of the
+			 * missing dma/bus locations in the user mode version of the
+			 * sah_Desc structure. */
+			os_error_code =
+			    os_copy_to_user((void *)(user_space_desc
+						     + offsetof(sah_Head_Desc,
+								uco_flags)),
+					    &stat, sizeof(fsl_shw_return_t));
+
+		} else {	/* not blocking mode - queue and forget */
+
+			if (desc_chain_head->uco_flags & FSL_UCO_CALLBACK_MODE) {
+				user_ctx->process = os_get_process_handle();
+				user_ctx->callback = sah_user_callback;
+			}
+#ifdef SAHARA_POLL_MODE
+			/* will put results in result pool */
+			sah_Handle_Poll(desc_chain_head);
+#else
+			/* just put someting in the DAR */
+			sah_Queue_Manager_Append_Entry(desc_chain_head);
+#endif
+			/* assume all went well */
+			os_error_code = OS_ERROR_OK_S;
+		}
+	}
+
+	return os_error_code;
+}
+
+static void sah_user_callback(fsl_shw_uco_t * user_ctx)
+{
+	os_send_signal(user_ctx->process, SIGUSR2);
+}
+
+/*!
+ * This function is called when a thread attempts to read from the /proc/sahara
+ * file.  Upon read, statistics and information about the state of the driver
+ * are returned in nthe supplied buffer.
+ *
+ * @brief     SAHARA PROCFS read function.
+ *
+ * @param    buf     Anything written to this buffer will be returned to the
+ *                   user-space process that is reading from this proc entry.
+ * @param    start   Part of the kernel prototype.
+ * @param    offset  Part of the kernel prototype.
+ * @param    count   The size of the buf argument.
+ * @param    eof     An integer which is set to one to tell the user-space
+ *                   process that there is no more data to read.
+ * @param    data    Part of the kernel prototype.
+ *
+ * @return   The number of bytes written to the proc entry.
+ */
+#if !defined(CONFIG_DEVFS_FS) || (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+static int sah_read_procfs(char *buf,
+			   char **start,
+			   off_t offset, int count, int *eof, void *data)
+{
+	int output_bytes = 0;
+	int in_queue_count = 0;
+	os_lock_context_t lock_context;
+
+	os_lock_save_context(desc_queue_lock, lock_context);
+	in_queue_count = sah_Queue_Manager_Count_Entries(TRUE, 0);
+	os_unlock_restore_context(desc_queue_lock, lock_context);
+	output_bytes += snprintf(buf, count - output_bytes, "queued: %d\n",
+				 in_queue_count);
+	output_bytes += snprintf(buf + output_bytes, count - output_bytes,
+				 "Descriptors: %d, "
+				 "Interrupts %d (%d Done1Done2, %d Done1Busy2, "
+				 " %d Done1)\n",
+				 dar_count, interrupt_count, done1done2_count,
+				 done1busy2_count, done1_count);
+	output_bytes += snprintf(buf + output_bytes, count - output_bytes,
+				 "Control: %08x\n", sah_HW_Read_Control());
+#if !defined(FSL_HAVE_SAHARA4) || defined(SAHARA4_NO_USE_SQUIB)
+	output_bytes += snprintf(buf + output_bytes, count - output_bytes,
+				 "IDAR: %08x; CDAR: %08x\n",
+				 sah_HW_Read_IDAR(), sah_HW_Read_CDAR());
+#endif
+#ifdef DIAG_DRV_STATUS
+	output_bytes += snprintf(buf + output_bytes, count - output_bytes,
+				 "Status: %08x; Error Status: %08x; Op Status: %08x\n",
+				 sah_HW_Read_Status(),
+				 sah_HW_Read_Error_Status(),
+				 sah_HW_Read_Op_Status());
+#endif
+#ifdef FSL_HAVE_SAHARA4
+	output_bytes += snprintf(buf + output_bytes, count - output_bytes,
+				 "MMStat: %08x; Config: %08x\n",
+				 sah_HW_Read_MM_Status(), sah_HW_Read_Config());
+#endif
+
+	/* Signal the end of the file */
+	*eof = 1;
+
+	/* To get rid of the unused parameter warnings */
+	(void)start;
+	(void)data;
+	(void)offset;
+
+	return output_bytes;
+}
+
+static int sah_write_procfs(struct file *file, const char __user * buffer,
+			    unsigned long count, void *data)
+{
+
+	/* Any write to this file will reset all counts. */
+	dar_count = interrupt_count = done1done2_count =
+	    done1busy2_count = done1_count = 0;
+
+	(void)file;
+	(void)buffer;
+	(void)data;
+
+	return count;
+}
+
+#endif
+
+#ifndef SAHARA_POLL_MODE
+/*!
+ * Block user call until processing is complete.
+ *
+ * @param entry    The user's request.
+ *
+ * @return   An OS error code, or 0 if no error
+ */
+int sah_blocking_mode(sah_Head_Desc * entry)
+{
+	int os_error_code = 0;
+	sah_Queue_Status status;
+
+	/* queue entry, put something in the DAR, if nothing is there currently */
+	sah_Queue_Manager_Append_Entry(entry);
+
+	/* get this descriptor chain's current status */
+	status = ((volatile sah_Head_Desc *)entry)->status;
+
+	while (!SAH_DESC_PROCESSED(status)) {
+		extern sah_Queue *main_queue;
+
+		DEFINE_WAIT(sahara_wait);	/* create a wait queue entry. Linux */
+
+		/* enter the wait queue entry into the queue */
+		prepare_to_wait(&Wait_queue, &sahara_wait, TASK_INTERRUPTIBLE);
+
+		/* check if this entry has been processed */
+		status = ((volatile sah_Head_Desc *)entry)->status;
+
+		if (!SAH_DESC_PROCESSED(status)) {
+			/* go to sleep - Linux */
+			schedule();
+		}
+
+		/* un-queue the 'prepare to wait' queue? - Linux */
+		finish_wait(&Wait_queue, &sahara_wait);
+
+		/* signal belongs to this thread? */
+		if (signal_pending(current)) {	/* Linux */
+			os_lock_context_t lock_flags;
+
+			/* don't allow access during this check and operation */
+			os_lock_save_context(desc_queue_lock, lock_flags);
+			status = ((volatile sah_Head_Desc *)entry)->status;
+			if (status == SAH_STATE_PENDING) {
+				sah_Queue_Remove_Any_Entry(main_queue, entry);
+				entry->result = FSL_RETURN_INTERNAL_ERROR_S;
+				((volatile sah_Head_Desc *)entry)->status =
+				    SAH_STATE_FAILED;
+			}
+			os_unlock_restore_context(desc_queue_lock, lock_flags);
+		}
+
+		status = ((volatile sah_Head_Desc *)entry)->status;
+	}			/* while ... */
+
+	/* Do this so that caller can free */
+	(void)sah_DePhysicalise_Descriptors(entry);
+
+	return os_error_code;
+}
+
+/*!
+ * If interrupt does not return in a reasonable time, time out, trigger
+ * interrupt, and continue with process
+ *
+ * @param    data   ignored
+ */
+void sahara_timeout_handler(unsigned long data)
+{
+	/* Sahara has not issuing an interrupt, so timed out */
+#ifdef DIAG_DRV_IF
+	LOG_KDIAG("Sahara HW did not respond.  Resetting.\n");
+#endif
+	/* assume hardware needs resetting */
+	sah_Handle_Interrupt(SAH_EXEC_FAULT);
+	/* wake up sleeping thread to try again */
+	wake_up_interruptible(&Wait_queue);
+}
+
+#endif				/* ifndef SAHARA_POLL_MODE */
+
+/* End of sah_driver_interface.c */
diff --git a/drivers/mxc/security/sahara2/sah_hardware_interface.c b/drivers/mxc/security/sahara2/sah_hardware_interface.c
new file mode 100644
index 000000000000..cf5065379d3d
--- /dev/null
+++ b/drivers/mxc/security/sahara2/sah_hardware_interface.c
@@ -0,0 +1,854 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file sah_hardware_interface.c
+ *
+ * @brief Provides an interface to the SAHARA hardware registers.
+ *
+ */
+
+/* SAHARA Includes */
+#include <sah_driver_common.h>
+#include <sah_hardware_interface.h>
+#include <sah_memory_mapper.h>
+#include <sah_kernel.h>
+
+#if defined DIAG_DRV_IF || defined(DO_DBG)
+#include <diagnostic.h>
+#ifndef LOG_KDIAG
+#define LOG_KDIAG(x) os_printk("%s\n", x)
+#endif
+
+static void sah_Dump_Link(const char *prefix, const sah_Link * link,
+			  dma_addr_t addr);
+
+/* This is for sprintf() to use when constructing output. */
+#define DIAG_MSG_SIZE   1024
+/* was 200 */
+#define MAX_DUMP        200
+static char Diag_msg[DIAG_MSG_SIZE];
+
+#endif				/* DIAG_DRV_IF */
+
+/*!
+ * Number of descriptors sent to Sahara.  This value should only be updated
+ * with the main queue lock held.
+ */
+uint32_t dar_count;
+
+/*! The "link-list optimize" bit in the Header of a Descriptor */
+#define SAH_HDR_LLO 0x01000000
+
+/* IO_ADDRESS() is Linux macro -- need portable equivalent */
+#define SAHARA_BASE_ADDRESS             IO_ADDRESS(SAHA_BASE_ADDR)
+#define SAHARA_VERSION_REGISTER_OFFSET  0x000
+#define SAHARA_DAR_REGISTER_OFFSET      0x004
+#define SAHARA_CONTROL_REGISTER_OFFSET  0x008
+#define SAHARA_COMMAND_REGISTER_OFFSET  0x00C
+#define SAHARA_STATUS_REGISTER_OFFSET   0x010
+#define SAHARA_ESTATUS_REGISTER_OFFSET  0x014
+#define SAHARA_FLT_ADD_REGISTER_OFFSET  0x018
+#define SAHARA_CDAR_REGISTER_OFFSET     0x01C
+#define SAHARA_IDAR_REGISTER_OFFSET     0x020
+#define SAHARA_OSTATUS_REGISTER_OFFSET  0x028
+#define SAHARA_CONFIG_REGISTER_OFFSET   0x02C
+#define SAHARA_MM_STAT_REGISTER_OFFSET  0x030
+
+/*! Register within Sahara which contains hardware version. (1 or 2).  */
+#define SAHARA_VERSION_REGISTER (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_VERSION_REGISTER_OFFSET)
+
+/*! Register within Sahara which is used to provide new work to the block. */
+#define SAHARA_DAR_REGISTER     (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_DAR_REGISTER_OFFSET)
+
+/*! Register with Sahara which is used for configuration. */
+#define SAHARA_CONTROL_REGISTER (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_CONTROL_REGISTER_OFFSET)
+
+/*! Register with Sahara which is used for changing status. */
+#define SAHARA_COMMAND_REGISTER (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_COMMAND_REGISTER_OFFSET)
+
+/*! Register with Sahara which is contains status and state. */
+#define SAHARA_STATUS_REGISTER  (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_STATUS_REGISTER_OFFSET)
+
+/*! Register with Sahara which is contains error status information. */
+#define SAHARA_ESTATUS_REGISTER (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_ESTATUS_REGISTER_OFFSET)
+
+/*! Register with Sahara which is contains faulting address information. */
+#define SAHARA_FLT_ADD_REGISTER (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_FLT_ADD_REGISTER_OFFSET)
+
+/*! Register with Sahara which is contains current descriptor address. */
+#define SAHARA_CDAR_REGISTER    (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_CDAR_REGISTER_OFFSET)
+
+/*! Register with Sahara which is contains initial descriptor address (of a
+    chain). */
+#define SAHARA_IDAR_REGISTER    (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_IDAR_REGISTER_OFFSET)
+
+/*! Register with Sahara which is contains op status information. */
+#define SAHARA_OSTATUS_REGISTER (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_OSTATUS_REGISTER_OFFSET)
+
+/*! Register with Sahara which is contains configuration information. */
+#define SAHARA_CONFIG_REGISTER (SAHARA_BASE_ADDRESS + \
+                                SAHARA_CONFIG_REGISTER_OFFSET)
+
+/*! Register with Sahara which is contains configuration information. */
+#define SAHARA_MM_STAT_REGISTER (SAHARA_BASE_ADDRESS + \
+                                 SAHARA_MM_STAT_REGISTER_OFFSET)
+
+/* Local Functions */
+#if defined DIAG_DRV_IF || defined DO_DBG
+void sah_Dump_Region(const char *prefix, const unsigned char *data,
+		     dma_addr_t addr, unsigned length);
+
+#endif				/* DIAG_DRV_IF */
+
+/* time out value when polling SAHARA status register for completion */
+static uint32_t sah_poll_timeout = 0xFFFFFFFF;
+
+/*!
+ * Polls Sahara to determine when its current operation is complete
+ *
+ * @return   last value found in Sahara's status register
+ */
+sah_Execute_Status sah_Wait_On_Sahara()
+{
+	uint32_t count = 0;	/* ensure we don't get stuck in the loop forever */
+	sah_Execute_Status status;	/* Sahara's status register */
+	uint32_t stat_reg;
+
+	pr_debug("Entered sah_Wait_On_Sahara\n");
+
+	do {
+		/* get current status register from Sahara */
+		stat_reg = sah_HW_Read_Status();
+		status = stat_reg & SAH_EXEC_STATE_MASK;
+
+		/* timeout if SAHARA takes too long to complete */
+		if (++count == sah_poll_timeout) {
+			status = SAH_EXEC_FAULT;
+			printk("sah_Wait_On_Sahara timed out\n");
+		}
+
+		/* stay in loop as long as Sahara is still busy */
+	} while ((status == SAH_EXEC_BUSY) || (status == SAH_EXEC_DONE1_BUSY2));
+
+	if (status == SAH_EXEC_ERROR1) {
+		if (stat_reg & OP_STATUS) {
+			status = SAH_EXEC_OPSTAT1;
+		}
+	}
+
+	return status;
+}				/* sah_Wait_on_Sahara() */
+
+/*!
+ * This function resets the SAHARA hardware. The following operations are
+ * performed:
+ *   1. Resets SAHARA.
+ *   2. Requests BATCH mode.
+ *   3. Enables interrupts.
+ *   4. Requests Little Endian mode.
+ *
+ * @brief     SAHARA hardware reset function.
+ *
+ * @return   void
+ */
+int sah_HW_Reset(void)
+{
+	sah_Execute_Status sah_state;
+	int status;		/* this is the value to return to the calling routine */
+	uint32_t saha_control = 0;
+
+#ifndef USE_3WORD_BURST
+#ifdef FSL_HAVE_SAHARA2
+	saha_control |= (8 << 16);	/* Allow 8-word burst */
+#endif
+#else
+/***************** HARDWARE BUG WORK AROUND ******************/
+/* A burst size of > 4 can cause Sahara DMA to issue invalid AHB transactions
+ * when crossing 1KB boundaries.  By limiting the 'burst size' to 3, these
+ * invalid transactions will not be generated, but Sahara will still transfer
+ * data more efficiently than if the burst size were set to 1.
+ */
+	saha_control |= (3 << 16);	/* Limit DMA burst size. For versions 2/3 */
+#endif				/* USE_3WORD_BURST */
+
+#ifdef DIAG_DRV_IF
+	snprintf(Diag_msg, DIAG_MSG_SIZE,
+		 "Address of SAHARA_BASE_ADDRESS = 0x%08x\n",
+		 SAHARA_BASE_ADDRESS);
+	LOG_KDIAG(Diag_msg);
+	snprintf(Diag_msg, DIAG_MSG_SIZE,
+		 "Sahara Status register before reset: %08x",
+		 sah_HW_Read_Status());
+	LOG_KDIAG(Diag_msg);
+#endif
+
+	/* Write the Reset & BATCH mode command to the SAHARA Command register. */
+	sah_HW_Write_Command(CMD_BATCH | CMD_RESET);
+#ifdef SAHARA4_NO_USE_SQUIB
+	{
+		uint32_t cfg = sah_HW_Read_Config();
+		cfg &= ~0x10000;
+		sah_HW_Write_Config(cfg);
+	}
+#endif
+
+	sah_poll_timeout = 0x0FFFFFFF;
+	sah_state = sah_Wait_On_Sahara();
+#ifdef DIAG_DRV_IF
+	snprintf(Diag_msg, DIAG_MSG_SIZE,
+		 "Sahara Status register after reset: %08x",
+		 sah_HW_Read_Status());
+	LOG_KDIAG(Diag_msg);
+#endif
+	/* on reset completion, check that Sahara is in the idle state */
+	status = (sah_state == SAH_EXEC_IDLE) ? 0 : OS_ERROR_FAIL_S;
+
+	/* Set initial value out of reset */
+	sah_HW_Write_Control(saha_control);
+
+#ifndef NO_RESEED_WORKAROUND
+/***************** HARDWARE BUG WORK AROUND ******************/
+/* In order to set the 'auto reseed' bit, must first acquire a random value. */
+	/*
+	 * to solve a hardware bug, a random number must be generated before
+	 * the 'RNG Auto Reseed' bit can be set. So this generates a random
+	 * number that is thrown away.
+	 *
+	 * Note that the interrupt bit has not been set at this point so
+	 * the result can be polled.
+	 */
+#ifdef DIAG_DRV_IF
+	LOG_KDIAG("Create and submit Random Number Descriptor");
+#endif
+
+	if (status == OS_ERROR_OK_S) {
+		/* place to put random number */
+		volatile uint32_t *random_data_ptr;
+		sah_Head_Desc *random_desc;
+		dma_addr_t desc_dma;
+		dma_addr_t rand_dma;
+		const int rnd_cnt = 3;	/* how many random 32-bit values to get */
+
+		/* Get space for data -- assume at least 32-bit aligned! */
+		random_data_ptr = os_alloc_memory(rnd_cnt * sizeof(uint32_t),
+						  GFP_ATOMIC);
+
+		random_desc = sah_Alloc_Head_Descriptor();
+
+		if ((random_data_ptr == NULL) || (random_desc == NULL)) {
+			status = OS_ERROR_FAIL_S;
+		} else {
+			int i;
+
+			/* Clear out values */
+			for (i = 0; i < rnd_cnt; i++) {
+				random_data_ptr[i] = 0;
+			}
+
+			rand_dma = os_pa(random_data_ptr);
+
+			random_desc->desc.header = 0xB18C0000;	/* LLO get random number */
+			random_desc->desc.len1 =
+			    rnd_cnt * sizeof(*random_data_ptr);
+			random_desc->desc.ptr1 = (void *)rand_dma;
+			random_desc->desc.original_ptr1 =
+			    (void *)random_data_ptr;
+
+			random_desc->desc.len2 = 0;	/* not used */
+			random_desc->desc.ptr2 = 0;	/* not used */
+
+			random_desc->desc.next = 0;	/* chain terminates here */
+			random_desc->desc.original_next = 0;	/* chain terminates here */
+
+			desc_dma = random_desc->desc.dma_addr;
+
+			/* Force in-cache data out to RAM */
+			os_cache_clean_range(random_data_ptr,
+					     rnd_cnt *
+					     sizeof(*random_data_ptr));
+
+			/* pass descriptor to Sahara */
+			sah_HW_Write_DAR(desc_dma);
+
+			/*
+			 * Wait for RNG to complete (interrupts are disabled at this point
+			 * due to sahara being reset previously) then check for error
+			 */
+			sah_state = sah_Wait_On_Sahara();
+			/* Force CPU to ignore in-cache and reload from RAM */
+			os_cache_inv_range(random_data_ptr,
+					   rnd_cnt * sizeof(*random_data_ptr));
+
+			/* if it didn't move to done state, an error occured */
+			if (
+#ifndef SUBMIT_MULTIPLE_DARS
+				   (sah_state != SAH_EXEC_IDLE) &&
+#endif
+				   (sah_state != SAH_EXEC_DONE1)
+			    ) {
+				status = OS_ERROR_FAIL_S;
+				os_printk
+				    ("(sahara) Failure: state is %08x; random_data is"
+				     " %08x\n", sah_state, *random_data_ptr);
+				os_printk
+				    ("(sahara) CDAR: %08x, IDAR: %08x, FADR: %08x,"
+				     " ESTAT: %08x\n", sah_HW_Read_CDAR(),
+				     sah_HW_Read_IDAR(),
+				     sah_HW_Read_Fault_Address(),
+				     sah_HW_Read_Error_Status());
+			} else {
+				int i;
+				int seen_rand = 0;
+
+				for (i = 0; i < rnd_cnt; i++) {
+					if (*random_data_ptr != 0) {
+						seen_rand = 1;
+						break;
+					}
+				}
+				if (!seen_rand) {
+					status = OS_ERROR_FAIL_S;
+					os_printk
+					    ("(sahara) Error:  Random number is zero!\n");
+				}
+			}
+		}
+
+		if (random_data_ptr) {
+			os_free_memory((void *)random_data_ptr);
+		}
+		if (random_desc) {
+			sah_Free_Head_Descriptor(random_desc);
+		}
+	}
+/***************** END HARDWARE BUG WORK AROUND ******************/
+#endif
+
+	if (status == 0) {
+#ifdef FSL_HAVE_SAHARA2
+		saha_control |= CTRL_RNG_RESEED;
+#endif
+
+#ifndef SAHARA_POLL_MODE
+		saha_control |= CTRL_INT_EN;	/* enable interrupts */
+#else
+		sah_poll_timeout = SAHARA_POLL_MODE_TIMEOUT;
+#endif
+
+#ifdef DIAG_DRV_IF
+		snprintf(Diag_msg, DIAG_MSG_SIZE,
+			 "Setting up Sahara's Control Register: %08x\n",
+			 saha_control);
+		LOG_KDIAG(Diag_msg);
+#endif
+
+		/* Rewrite the setup to the SAHARA Control register */
+		sah_HW_Write_Control(saha_control);
+#ifdef DIAG_DRV_IF
+		snprintf(Diag_msg, DIAG_MSG_SIZE,
+			 "Sahara Status register after control write: %08x",
+			 sah_HW_Read_Status());
+		LOG_KDIAG(Diag_msg);
+#endif
+
+#ifdef FSL_HAVE_SAHARA4
+		{
+			uint32_t cfg = sah_HW_Read_Config();
+			sah_HW_Write_Config(cfg | 0x100);	/* Add RNG auto-reseed */
+		}
+#endif
+	} else {
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("Reset failed\n");
+#endif
+	}
+
+	return status;
+}				/* sah_HW_Reset() */
+
+/*!
+ * This function enables High Assurance mode.
+ *
+ * @brief     SAHARA hardware enable High Assurance mode.
+ *
+ * @return   FSL_RETURN_OK_S             - if HA was set successfully
+ * @return   FSL_RETURN_INTERNAL_ERROR_S - if HA was not set due to SAHARA
+ *                                         being busy.
+ */
+fsl_shw_return_t sah_HW_Set_HA(void)
+{
+	/* This is the value to write to the register */
+	uint32_t value;
+
+	/* Read from the control register. */
+	value = sah_HW_Read_Control();
+
+	/* Set the HA bit */
+	value |= CTRL_HA;
+
+	/* Write to the control register. */
+	sah_HW_Write_Control(value);
+
+	/* Read from the control register. */
+	value = sah_HW_Read_Control();
+
+	return (value & CTRL_HA) ? FSL_RETURN_OK_S :
+	    FSL_RETURN_INTERNAL_ERROR_S;
+}
+
+/*!
+ * This function reads the SAHARA hardware Version Register.
+ *
+ * @brief     Read SAHARA hardware Version Register.
+ *
+ * @return   uint32_t Register value.
+ */
+uint32_t sah_HW_Read_Version(void)
+{
+	return os_read32(SAHARA_VERSION_REGISTER);
+}
+
+/*!
+ * This function reads the SAHARA hardware Control Register.
+ *
+ * @brief     Read SAHARA hardware Control Register.
+ *
+ * @return   uint32_t Register value.
+ */
+uint32_t sah_HW_Read_Control(void)
+{
+	return os_read32(SAHARA_CONTROL_REGISTER);
+}
+
+/*!
+ * This function reads the SAHARA hardware Status Register.
+ *
+ * @brief     Read SAHARA hardware Status Register.
+ *
+ * @return   uint32_t Register value.
+ */
+uint32_t sah_HW_Read_Status(void)
+{
+	return os_read32(SAHARA_STATUS_REGISTER);
+}
+
+/*!
+ * This function reads the SAHARA hardware Error Status Register.
+ *
+ * @brief     Read SAHARA hardware Error Status Register.
+ *
+ * @return   uint32_t Error Status value.
+ */
+uint32_t sah_HW_Read_Error_Status(void)
+{
+	return os_read32(SAHARA_ESTATUS_REGISTER);
+}
+
+/*!
+ * This function reads the SAHARA hardware Op Status Register.
+ *
+ * @brief     Read SAHARA hardware Op Status Register.
+ *
+ * @return   uint32_t Op Status value.
+ */
+uint32_t sah_HW_Read_Op_Status(void)
+{
+	return os_read32(SAHARA_OSTATUS_REGISTER);
+}
+
+/*!
+ * This function reads the SAHARA hardware Descriptor Address Register.
+ *
+ * @brief     Read SAHARA hardware DAR Register.
+ *
+ * @return   uint32_t DAR value.
+ */
+uint32_t sah_HW_Read_DAR(void)
+{
+	return os_read32(SAHARA_DAR_REGISTER);
+}
+
+/*!
+ * This function reads the SAHARA hardware Current Descriptor Address Register.
+ *
+ * @brief     Read SAHARA hardware CDAR Register.
+ *
+ * @return   uint32_t CDAR value.
+ */
+uint32_t sah_HW_Read_CDAR(void)
+{
+	return os_read32(SAHARA_CDAR_REGISTER);
+}
+
+/*!
+ * This function reads the SAHARA hardware Initial Descriptor Address Register.
+ *
+ * @brief     Read SAHARA hardware IDAR Register.
+ *
+ * @return   uint32_t IDAR value.
+ */
+uint32_t sah_HW_Read_IDAR(void)
+{
+	return os_read32(SAHARA_IDAR_REGISTER);
+}
+
+/*!
+ * This function reads the SAHARA hardware Fault Address Register.
+ *
+ * @brief     Read SAHARA Fault Address Register.
+ *
+ * @return   uint32_t Fault Address value.
+ */
+uint32_t sah_HW_Read_Fault_Address(void)
+{
+	return os_read32(SAHARA_FLT_ADD_REGISTER);
+}
+
+/*!
+ * This function reads the SAHARA hardware Multiple Master Status Register.
+ *
+ * @brief     Read SAHARA hardware MM Stat Register.
+ *
+ * @return   uint32_t MM Stat value.
+ */
+uint32_t sah_HW_Read_MM_Status(void)
+{
+	return os_read32(SAHARA_MM_STAT_REGISTER);
+}
+
+/*!
+ * This function reads the SAHARA hardware Configuration Register.
+ *
+ * @brief     Read SAHARA Configuration Register.
+ *
+ * @return   uint32_t Configuration value.
+ */
+uint32_t sah_HW_Read_Config(void)
+{
+	return os_read32(SAHARA_CONFIG_REGISTER);
+}
+
+/*!
+ * This function writes a command to the SAHARA hardware Command Register.
+ *
+ * @brief     Write to SAHARA hardware Command Register.
+ *
+ * @param    command     An unsigned 32bit command value.
+ *
+ * @return   void
+ */
+void sah_HW_Write_Command(uint32_t command)
+{
+	os_write32(SAHARA_COMMAND_REGISTER, command);
+}
+
+/*!
+ * This function writes a control value to the SAHARA hardware Control
+ * Register.
+ *
+ * @brief     Write to SAHARA hardware Control Register.
+ *
+ * @param    control     An unsigned 32bit control value.
+ *
+ * @return   void
+ */
+void sah_HW_Write_Control(uint32_t control)
+{
+	os_write32(SAHARA_CONTROL_REGISTER, control);
+}
+
+/*!
+ * This function writes a configuration value to the SAHARA hardware Configuration
+ * Register.
+ *
+ * @brief     Write to SAHARA hardware Configuration Register.
+ *
+ * @param    configuration     An unsigned 32bit configuration value.
+ *
+ * @return   void
+ */
+void sah_HW_Write_Config(uint32_t configuration)
+{
+	os_write32(SAHARA_CONFIG_REGISTER, configuration);
+}
+
+/*!
+ * This function writes a descriptor address to the SAHARA Descriptor Address
+ * Register.
+ *
+ * @brief     Write to SAHARA Descriptor Address Register.
+ *
+ * @param    pointer     An unsigned 32bit descriptor address value.
+ *
+ * @return   void
+ */
+void sah_HW_Write_DAR(uint32_t pointer)
+{
+	os_write32(SAHARA_DAR_REGISTER, pointer);
+	dar_count++;
+}
+
+#if defined DIAG_DRV_IF || defined DO_DBG
+
+static char *interpret_header(uint32_t header)
+{
+	unsigned desc_type = ((header >> 24) & 0x70) | ((header >> 16) & 0xF);
+
+	switch (desc_type) {
+	case 0x12:
+		return "5/SKHA_ST_CTX";
+	case 0x13:
+		return "35/SKHA_LD_MODE_KEY";
+	case 0x14:
+		return "38/SKHA_LD_MODE_IN_CPHR_ST_CTX";
+	case 0x15:
+		return "4/SKHA_IN_CPHR_OUT";
+	case 0x16:
+		return "34/SKHA_ST_SBOX";
+	case 0x18:
+		return "1/SKHA_LD_MODE_IV_KEY";
+	case 0x19:
+		return "33/SKHA_ST_SBOX";
+	case 0x1D:
+		return "2/SKHA_LD_MODE_IN_CPHR_OUT";
+	case 0x22:
+		return "11/MDHA_ST_MD";
+	case 0x25:
+		return "10/MDHA_HASH_ST_MD";
+	case 0x28:
+		return "6/MDHA_LD_MODE_MD_KEY";
+	case 0x2A:
+		return "39/MDHA_ICV";
+	case 0x2D:
+		return "8/MDHA_LD_MODE_HASH_ST_MD";
+	case 0x3C:
+		return "18/RNG_GEN";
+	case 0x40:
+		return "19/PKHA_LD_N_E";
+	case 0x41:
+		return "36/PKHA_LD_A3_B0";
+	case 0x42:
+		return "27/PKHA_ST_A_B";
+	case 0x43:
+		return "22/PKHA_LD_A_B";
+	case 0x44:
+		return "23/PKHA_LD_A0_A1";
+	case 0x45:
+		return "24/PKHA_LD_A2_A3";
+	case 0x46:
+		return "25/PKHA_LD_B0_B1";
+	case 0x47:
+		return "26/PKHA_LD_B2_B3";
+	case 0x48:
+		return "28/PKHA_ST_A0_A1";
+	case 0x49:
+		return "29/PKHA_ST_A2_A3";
+	case 0x4A:
+		return "30/PKHA_ST_B0_B1";
+	case 0x4B:
+		return "31/PKHA_ST_B2_B3";
+	case 0x4C:
+		return "32/PKHA_EX_ST_B1";
+	case 0x4D:
+		return "20/PKHA_LD_A_EX_ST_B";
+	case 0x4E:
+		return "21/PKHA_LD_N_EX_ST_B";
+	case 0x4F:
+		return "37/PKHA_ST_B1_B2";
+	default:
+		return "??/UNKNOWN";
+	}
+}				/* cvt_desc_name() */
+
+/*!
+ * Dump chain of descriptors to the log.
+ *
+ * @brief Dump descriptor chain
+ *
+ * @param    chain     Kernel virtual address of start of chain of descriptors
+ *
+ * @return   void
+ */
+void sah_Dump_Chain(const sah_Desc * chain, dma_addr_t addr)
+{
+	int desc_no = 1;
+
+	pr_debug("Chain for Sahara\n");
+
+	while (chain != NULL) {
+		char desc_name[50];
+
+		sprintf(desc_name, "Desc %02d (%s)\n" KERN_DEBUG "Desc  ",
+			desc_no++, interpret_header(chain->header));
+
+		sah_Dump_Words(desc_name, (unsigned *)chain, addr,
+			       6 /* #words in h/w link */ );
+		if (chain->original_ptr1) {
+			if (chain->header & SAH_HDR_LLO) {
+				sah_Dump_Region(" Data1",
+						(unsigned char *)chain->
+						original_ptr1,
+						(dma_addr_t) chain->ptr1,
+						chain->len1);
+			} else {
+				sah_Dump_Link(" Link1", chain->original_ptr1,
+					      (dma_addr_t) chain->ptr1);
+			}
+		}
+		if (chain->ptr2) {
+			if (chain->header & SAH_HDR_LLO) {
+				sah_Dump_Region(" Data2",
+						(unsigned char *)chain->
+						original_ptr2,
+						(dma_addr_t) chain->ptr2,
+						chain->len2);
+			} else {
+				sah_Dump_Link(" Link2", chain->original_ptr2,
+					      (dma_addr_t) chain->ptr2);
+			}
+		}
+
+		addr = (dma_addr_t) chain->next;
+		chain = (chain->next) ? (chain->original_next) : NULL;
+	}
+}
+
+/*!
+ * Dump chain of links to the log.
+ *
+ * @brief Dump chain of links
+ *
+ * @param    prefix    Text to put in front of dumped data
+ * @param    link      Kernel virtual address of start of chain of links
+ *
+ * @return   void
+ */
+static void sah_Dump_Link(const char *prefix, const sah_Link * link,
+			  dma_addr_t addr)
+{
+#ifdef DUMP_SCC_DATA
+	extern uint8_t *sahara_partition_base;
+	extern dma_addr_t sahara_partition_phys;
+#endif
+
+	while (link != NULL) {
+		sah_Dump_Words(prefix, (unsigned *)link, addr,
+			       3 /* # words in h/w link */ );
+		if (link->flags & SAH_STORED_KEY_INFO) {
+#ifdef SAH_DUMP_DATA
+#ifdef DUMP_SCC_DATA
+			sah_Dump_Region("  Data",
+					(uint8_t *) link->data -
+					(uint8_t *) sahara_partition_phys +
+					sahara_partition_base,
+					(dma_addr_t) link->data, link->len);
+#else
+			pr_debug("  Key Slot %d\n", link->slot);
+#endif
+#endif
+		} else {
+#ifdef SAH_DUMP_DATA
+			sah_Dump_Region("  Data", link->original_data,
+					(dma_addr_t) link->data, link->len);
+#endif
+		}
+		addr = (dma_addr_t) link->next;
+		link = link->original_next;
+	}
+}
+
+/*!
+ * Dump given region of data to the log.
+ *
+ * @brief Dump data
+ *
+ * @param    prefix    Text to put in front of dumped data
+ * @param    data      Kernel virtual address of start of region to dump
+ * @param    length    Amount of data to dump
+ *
+ * @return   void
+ */
+void sah_Dump_Region(const char *prefix, const unsigned char *data,
+		     dma_addr_t addr, unsigned length)
+{
+	unsigned count;
+	char *output;
+	unsigned data_len;
+
+	sprintf(Diag_msg, "%s (%08X,%u):", prefix, addr, length);
+
+	/* Restrict amount of data to dump */
+	if (length > MAX_DUMP) {
+		data_len = MAX_DUMP;
+	} else {
+		data_len = length;
+	}
+
+	/* We've already printed some text in output buffer, skip over it */
+	output = Diag_msg + strlen(Diag_msg);
+
+	for (count = 0; count < data_len; count++) {
+		if ((count % 4) == 0) {
+			*output++ = ' ';
+		}
+		sprintf(output, "%02X", *data++);
+		output += 2;
+	}
+
+	pr_debug("%s\n", Diag_msg);
+}
+
+/*!
+ * Dump given word of data to the log.
+ *
+ * @brief Dump data
+ *
+ * @param    prefix       Text to put in front of dumped data
+ * @param    data         Kernel virtual address of start of region to dump
+ * @param    word_count   Amount of data to dump
+ *
+ * @return   void
+ */
+void sah_Dump_Words(const char *prefix, const unsigned *data, dma_addr_t addr,
+		    unsigned word_count)
+{
+	char *output;
+
+	sprintf(Diag_msg, "%s (%08X,%uw): ", prefix, addr, word_count);
+
+	/* We've already printed some text in output buffer, skip over it */
+	output = Diag_msg + strlen(Diag_msg);
+
+	while (word_count--) {
+		sprintf(output, "%08X ", *data++);
+		output += 9;
+	}
+
+	pr_debug("%s\n", Diag_msg);
+	
+}
+
+#endif				/* DIAG_DRV_IF */
+
+/* End of sah_hardware_interface.c */
diff --git a/drivers/mxc/security/sahara2/sah_interrupt_handler.c b/drivers/mxc/security/sahara2/sah_interrupt_handler.c
new file mode 100644
index 000000000000..3d70fe8d2c64
--- /dev/null
+++ b/drivers/mxc/security/sahara2/sah_interrupt_handler.c
@@ -0,0 +1,216 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+* @file sah_interrupt_handler.c
+*
+* @brief Provides a hardware interrupt handling mechanism for device driver.
+*
+* This file needs to be ported for a non-Linux OS.
+*
+* It gets a call at #sah_Intr_Init() during initialization.
+*
+* #sah_Intr_Top_Half() is intended to be the Interrupt Service Routine.  It
+* calls a portable function in another file to process the Sahara status.
+*
+* #sah_Intr_Bottom_Half() is a 'background' task scheduled by the top half to
+* take care of the expensive tasks of the interrupt processing.
+*
+* The driver shutdown code calls #sah_Intr_Release().
+*
+*/
+
+#include <portable_os.h>
+
+/* SAHARA Includes */
+#include <sah_kernel.h>
+#include <sah_interrupt_handler.h>
+#include <sah_status_manager.h>
+#include <sah_hardware_interface.h>
+#include <sah_queue_manager.h>
+
+/*Enable this flag for debugging*/
+#if 0
+#define DIAG_DRV_INTERRUPT
+#endif
+
+#ifdef DIAG_DRV_INTERRUPT
+#include <diagnostic.h>
+#endif
+
+/*!
+ * Number of interrupts received.  This value should only be updated during
+ * interrupt processing.
+ */
+uint32_t interrupt_count;
+
+#ifndef SAHARA_POLL_MODE
+
+#if !defined(LINUX_VERSION_CODE) || LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+#define irqreturn_t void
+#define IRQ_HANDLED
+#define IRQ_RETVAL(x)
+#endif
+
+/* Internal Prototypes */
+static irqreturn_t sah_Intr_Top_Half(int irq, void *dev_id);
+
+#ifdef KERNEL_TEST
+extern void (*SAHARA_INT_PTR) (int, void *);
+#endif
+
+unsigned long reset_flag;
+static void sah_Intr_Bottom_Half(unsigned long reset_flag);
+
+/* This is the Bottom Half Task, (reset flag set to false) */
+DECLARE_TASKLET(BH_task, sah_Intr_Bottom_Half, (unsigned long)&reset_flag);
+
+/*! This is set by the Initialisation function */
+wait_queue_head_t *int_queue = NULL;
+
+/*!
+*******************************************************************************
+* This function registers the Top Half of the interrupt handler with the Kernel
+* and the SAHARA IRQ number.
+*
+* @brief     SAHARA Interrupt Handler Initialisation
+*
+* @param    wait_queue Pointer to the wait queue used by driver interface
+*
+* @return   int A return of Zero indicates successful initialisation.
+*/
+/******************************************************************************
+*
+* CAUTION: NONE
+*
+* MODIFICATION HISTORY:
+*
+* Date         Person     Change
+* 30/07/2003   MW         Initial Creation
+******************************************************************************/
+int sah_Intr_Init(wait_queue_head_t * wait_queue)
+{
+
+#ifdef DIAG_DRV_INTERRUPT
+	char err_string[200];
+#endif
+
+	int result;
+
+#ifdef KERNEL_TEST
+	SAHARA_INT_PTR = sah_Intr_Top_Half;
+#endif
+
+	/* Set queue used by the interrupt handler to match the driver interface */
+	int_queue = wait_queue;
+
+	/* Request use of the Interrupt line. */
+	result = request_irq(SAHARA_IRQ,
+			     sah_Intr_Top_Half, 0, SAHARA_NAME, NULL);
+
+#ifdef DIAG_DRV_INTERRUPT
+	if (result != 0) {
+		sprintf(err_string, "Cannot use SAHARA interrupt line %d. "
+			"request_irq() return code is %i.", SAHARA_IRQ, result);
+		LOG_KDIAG(err_string);
+	} else {
+		sprintf(err_string,
+			"SAHARA driver registered for interrupt %d. ",
+			SAHARA_IRQ);
+		LOG_KDIAG(err_string);
+	}
+#endif
+
+	return result;
+}
+
+/*!
+*******************************************************************************
+* This function releases the Top Half of the interrupt handler. The driver will
+* not receive any more interrupts after calling this functions.
+*
+* @brief     SAHARA Interrupt Handler Release
+*
+* @return   void
+*/
+/******************************************************************************
+*
+* CAUTION: NONE
+*
+* MODIFICATION HISTORY:
+*
+* Date         Person     Change
+* 30/07/2003   MW         Initial Creation
+******************************************************************************/
+void sah_Intr_Release(void)
+{
+	/* Release the Interrupt. */
+	free_irq(SAHARA_IRQ, NULL);
+}
+
+/*!
+*******************************************************************************
+* This function is the Top Half of the interrupt handler.  It updates the
+* status of any finished descriptor chains and then tries to add any pending
+* requests into the hardware.  It then queues the bottom half to complete
+* operations on the finished chains.
+*
+* @brief     SAHARA Interrupt Handler Top Half
+*
+* @param    irq     Part of the kernel prototype.
+* @param    dev_id  Part of the kernel prototype.
+*
+* @return   An IRQ_RETVAL() -- non-zero to that function means 'handled'
+*/
+static irqreturn_t sah_Intr_Top_Half(int irq, void *dev_id)
+{
+#if defined(DIAG_DRV_INTERRUPT) && defined(DIAG_DURING_INTERRUPT)
+	LOG_KDIAG("Top half of Sahara's interrupt handler called.");
+#endif
+
+	interrupt_count++;
+	reset_flag = sah_Handle_Interrupt(sah_HW_Read_Status());
+
+	/* Schedule the Bottom Half of the Interrupt. */
+	tasklet_schedule(&BH_task);
+
+	/* To get rid of the unused parameter warnings. */
+	irq = 0;
+	dev_id = NULL;
+	return IRQ_RETVAL(1);
+}
+
+/*!
+*******************************************************************************
+* This function is the Bottom Half of the interrupt handler.  It calls
+* #sah_postprocess_queue() to complete the processing of the Descriptor Chains
+* which were finished by the hardware.
+*
+* @brief     SAHARA Interrupt Handler Bottom Half
+*
+* @param    data   Part of the kernel prototype.
+*
+* @return   void
+*/
+static void sah_Intr_Bottom_Half(unsigned long reset_flag)
+{
+#if defined(DIAG_DRV_INTERRUPT) && defined(DIAG_DURING_INTERRUPT)
+	LOG_KDIAG("Bottom half of Sahara's interrupt handler called.");
+#endif
+	sah_postprocess_queue(*(unsigned long *)reset_flag);
+
+	return;
+}
+
+/* end of sah_interrupt_handler.c */
+#endif				/* ifndef SAHARA_POLL_MODE */
diff --git a/drivers/mxc/security/sahara2/sah_memory_mapper.c b/drivers/mxc/security/sahara2/sah_memory_mapper.c
new file mode 100644
index 000000000000..d239a34368f0
--- /dev/null
+++ b/drivers/mxc/security/sahara2/sah_memory_mapper.c
@@ -0,0 +1,2349 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+* @file sah_memory_mapper.c
+*
+* @brief Re-creates SAHARA data structures in Kernel memory such that they are
+*       suitable for DMA.  Provides support for kernel API.
+*
+* This file needs to be ported.
+*
+* The memory mapper gets a call at #sah_Init_Mem_Map() during driver
+* initialization.
+*
+* The routine #sah_Copy_Descriptors() is used to bring descriptor chains from
+* user memory down to kernel memory, relink using physical addresses, and make
+* sure that all user data will be accessible by the Sahara DMA.
+* #sah_Destroy_Descriptors() does the inverse.
+*
+* The #sah_Alloc_Block(), #sah_Free_Block(), and #sah_Block_Add_Page() routines
+* implement a cache of free blocks used when allocating descriptors and links
+* within the kernel.
+*
+* The memory mapper gets a call at #sah_Stop_Mem_Map() during driver shutdown.
+*
+*/
+
+#include <sah_driver_common.h>
+#include <sah_kernel.h>
+#include <sah_queue_manager.h>
+#include <sah_memory_mapper.h>
+#ifdef FSL_HAVE_SCC2
+#include <linux/mxc_scc2_driver.h>
+#else
+#include <linux/mxc_scc_driver.h>
+#endif
+
+#if defined(DIAG_DRV_IF) || defined(DIAG_MEM) || defined(DO_DBG)
+#include <diagnostic.h>
+#include <sah_hardware_interface.h>
+#endif
+
+#include <linux/mm.h>		/* get_user_pages() */
+#include <linux/pagemap.h>
+#include <linux/dmapool.h>
+
+#include <linux/slab.h>
+#include <linux/highmem.h>
+
+#if defined(DIAG_MEM) || defined(DIAG_DRV_IF)
+#define DIAG_MSG_SIZE   1024
+static char Diag_msg[DIAG_MSG_SIZE];
+#endif
+
+#ifdef LINUX_VERSION_CODE
+#define FLUSH_SPECIFIC_DATA_ONLY
+#else
+#define SELF_MANAGED_POOL
+#endif
+
+#if defined(LINUX_VERSION_CODE)
+EXPORT_SYMBOL(sah_Alloc_Link);
+EXPORT_SYMBOL(sah_Free_Link);
+EXPORT_SYMBOL(sah_Alloc_Descriptor);
+EXPORT_SYMBOL(sah_Free_Descriptor);
+EXPORT_SYMBOL(sah_Alloc_Head_Descriptor);
+EXPORT_SYMBOL(sah_Free_Head_Descriptor);
+EXPORT_SYMBOL(sah_Physicalise_Descriptors);
+EXPORT_SYMBOL(sah_DePhysicalise_Descriptors);
+#endif
+
+/* Determine if L2 cache support should be built in. */
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))
+#ifdef CONFIG_OUTER_CACHE
+#define HAS_L2_CACHE
+#endif
+#else
+#ifdef CONFIG_CPU_CACHE_L210
+#define HAS_L2_CACHE
+#endif
+#endif
+
+/* Number of bytes the hardware uses out of sah_Link and sah_*Desc structs */
+#define SAH_HW_LINK_LEN  1
+#define SAH_HW_DESC_LEN  24
+
+/* Macros for Descriptors */
+#define SAH_LLO_BIT 0x01000000
+#define sah_Desc_Get_LLO(desc)          (desc->header & SAH_LLO_BIT)
+#define sah_Desc_Set_Header(desc, h)    (desc->header = (h))
+
+#define sah_Desc_Get_Next(desc)     (desc->next)
+#define sah_Desc_Set_Next(desc, n)  (desc->next = (n))
+
+#define sah_Desc_Get_Ptr1(desc)     (desc->ptr1)
+#define sah_Desc_Get_Ptr2(desc)     (desc->ptr2)
+#define sah_Desc_Set_Ptr1(desc,p1)  (desc->ptr1 = (p1))
+#define sah_Desc_Set_Ptr2(desc,p2)  (desc->ptr2 = (p2))
+
+#define sah_Desc_Get_Len1(desc)     (desc->len1)
+#define sah_Desc_Get_Len2(desc)     (desc->len2)
+#define sah_Desc_Set_Len1(desc,l1)  (desc->len1 = (l1))
+#define sah_Desc_Set_Len2(desc,l2)  (desc->len2 = (l2))
+
+/* Macros for Links */
+#define sah_Link_Get_Next(link)     (link->next)
+#define sah_Link_Set_Next(link, n)  (link->next = (n))
+
+#define sah_Link_Get_Data(link)     (link->data)
+#define sah_Link_Set_Data(link,d)   (link->data = (d))
+
+#define sah_Link_Get_Len(link)      (link->len)
+#define sah_Link_Set_Len(link, l)   (link->len = (l))
+
+#define sah_Link_Get_Flags(link)    (link->flags)
+
+/* Memory block defines */
+/* Warning!  This assumes that kernel version of sah_Link
+ * is larger than kernel version of sah_Desc.
+ */
+#define MEM_BLOCK_SIZE sizeof(sah_Link)
+
+/*! Structure for  link/descriptor memory blocks in internal pool */
+typedef struct mem_block {
+	uint8_t data[MEM_BLOCK_SIZE];	/*!< the actual buffer area  */
+	struct mem_block *next;	/*!< next block when in free chain */
+	dma_addr_t dma_addr;	/*!< physical address of @a data  */
+} Mem_Block;
+
+#define MEM_BLOCK_ENTRIES  (PAGE_SIZE / sizeof(Mem_Block))
+
+#define MEM_BIG_BLOCK_SIZE     sizeof(sah_Head_Desc)
+
+/*! Structure for head descriptor memory blocks in internal pool */
+typedef struct mem_big_block {
+	uint8_t data[MEM_BIG_BLOCK_SIZE];	/*!< the actual buffer area  */
+	struct mem_big_block *next;	/*!< next block when in free chain */
+	uint32_t dma_addr;	/*!< physical address of @a data  */
+} Mem_Big_Block;
+
+#define MEM_BIG_BLOCK_ENTRIES  (PAGE_SIZE / sizeof(Mem_Big_Block))
+
+/* Shared variables */
+
+/*!
+ * Lock to protect the memory chain composed of #block_free_head and
+ * #block_free_tail.
+ */
+static os_lock_t mem_lock;
+
+#ifndef SELF_MANAGED_POOL
+static struct dma_pool *big_dma_pool = NULL;
+static struct dma_pool *small_dma_pool = NULL;
+#endif
+
+#ifdef SELF_MANAGED_POOL
+/*!
+ * Memory block free pool - pointer to first block.  Chain is protected by
+ * #mem_lock.
+ */
+static Mem_Block *block_free_head = NULL;
+/*!
+ * Memory block free pool - pointer to last block.  Chain is protected by
+ * #mem_lock.
+ */
+static Mem_Block *block_free_tail = NULL;
+/*!
+ * Memory block free pool - pointer to first block.  Chain is protected by
+ * #mem_lock.
+ */
+static Mem_Big_Block *big_block_free_head = NULL;
+/*!
+ * Memory block free pool - pointer to last block.  Chain is protected by
+ * #mem_lock.
+a */
+static Mem_Big_Block *big_block_free_tail = NULL;
+#endif				/* SELF_MANAGED_POOL */
+
+static Mem_Block *sah_Alloc_Block(void);
+static void sah_Free_Block(Mem_Block * block);
+static Mem_Big_Block *sah_Alloc_Big_Block(void);
+static void sah_Free_Big_Block(Mem_Big_Block * block);
+#ifdef SELF_MANAGED_POOL
+static void sah_Append_Block(Mem_Block * block);
+static void sah_Append_Big_Block(Mem_Big_Block * block);
+#endif				/* SELF_MANAGED_POOL */
+
+/* Page context structure.  Used by wire_user_memory and unwire_user_memory */
+typedef struct page_ctx_t {
+	uint32_t count;
+	struct page **local_pages;
+} page_ctx_t;
+
+/*!
+*******************************************************************************
+* Map and wire down a region of user memory.
+*
+*
+* @param    address     Userspace address of the memory to wire
+* @param    length      Length of the memory region to wire
+* @param    page_ctx    Page context, to be passed to unwire_user_memory
+*
+* @return   (if successful) Kernel virtual address of the wired pages
+*/
+void *wire_user_memory(void *address, uint32_t length, void **page_ctx)
+{
+	void *kernel_black_addr = NULL;
+	int result = -1;
+	int page_index = 0;
+	page_ctx_t *page_context;
+	int nr_pages = 0;
+	unsigned long start_page;
+	fsl_shw_return_t status;
+
+	/* Determine the number of pages being used for this link */
+	nr_pages = (((unsigned long)(address) & ~PAGE_MASK)
+		    + length + ~PAGE_MASK) >> PAGE_SHIFT;
+
+	start_page = (unsigned long)(address) & PAGE_MASK;
+
+	/* Allocate some memory to keep track of the wired user pages, so that
+	 * they can be deallocated later.  The block of memory will contain both
+	 * the structure and the array of pages.
+	 */
+	page_context = kmalloc(sizeof(page_ctx_t)
+			       + nr_pages * sizeof(struct page *), GFP_KERNEL);
+
+	if (page_context == NULL) {
+		status = FSL_RETURN_NO_RESOURCE_S;	/* no memory! */
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("kmalloc() failed.");
+#endif
+		return NULL;
+	}
+
+	/* Set the page pointer to point to the allocated region of memory */
+	page_context->local_pages = (void *)page_context + sizeof(page_ctx_t);
+
+#ifdef DIAG_DRV_IF
+	LOG_KDIAG_ARGS("page_context at: %p, local_pages at: %p",
+		       (void *)page_context,
+		       (void *)(page_context->local_pages));
+#endif
+
+	/* Wire down the pages from user space */
+	down_read(&current->mm->mmap_sem);
+	result = get_user_pages(current, current->mm,
+				start_page, nr_pages, WRITE, 0 /* noforce */ ,
+				(page_context->local_pages), NULL);
+	up_read(&current->mm->mmap_sem);
+
+	if (result < nr_pages) {
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("get_user_pages() failed.");
+#endif
+		if (result > 0) {
+			for (page_index = 0; page_index < result; page_index++) {
+				page_cache_release((page_context->
+						    local_pages[page_index]));
+			}
+
+			kfree(page_context);
+		}
+		return NULL;
+	}
+
+	kernel_black_addr = page_address(page_context->local_pages[0]) +
+	    ((unsigned long)address & ~PAGE_MASK);
+
+	page_context->count = nr_pages;
+	*page_ctx = page_context;
+
+	return kernel_black_addr;
+}
+
+/*!
+*******************************************************************************
+* Release and unmap a region of user memory.
+*
+* @param    page_ctx    Page context from wire_user_memory
+*/
+void unwire_user_memory(void **page_ctx)
+{
+	int page_index = 0;
+	struct page_ctx_t *page_context = *page_ctx;
+
+#ifdef DIAG_DRV_IF
+	LOG_KDIAG_ARGS("page_context at: %p, first page at:%p, count: %i",
+		       (void *)page_context,
+		       (void *)(page_context->local_pages),
+		       page_context->count);
+#endif
+
+	if ((page_context != NULL) && (page_context->local_pages != NULL)) {
+		for (page_index = 0; page_index < page_context->count;
+		     page_index++) {
+			page_cache_release(page_context->
+					   local_pages[page_index]);
+		}
+
+		kfree(page_context);
+		*page_ctx = NULL;
+	}
+}
+
+/*!
+*******************************************************************************
+* Map some physical memory into a users memory space
+*
+* @param    vma             Memory structure to map to
+* @param    physical_addr   Physical address of the memory to be mapped in
+* @param    size            Size of the memory to map (bytes)
+*
+* @return 
+*/
+os_error_code
+map_user_memory(struct vm_area_struct *vma, uint32_t physical_addr,
+		uint32_t size)
+{
+	os_error_code retval;
+
+	/* Map the acquired partition into the user's memory space */
+	vma->vm_end = vma->vm_start + size;
+
+	/* set cache policy to uncached so that each write of the UMID and
+	 * permissions get directly to the SCC2 in order to engage it
+	 * properly.  Once the permissions have been written, it may be
+	 * useful to provide a service for the user to request a different
+	 * cache policy
+	 */
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	/* Make sure that the user cannot fork() a child which will inherit
+	 * this mapping, as it creates a security hole.  Likewise, do not
+	 * allow the user to 'expand' his mapping beyond this partition.
+	 */
+	vma->vm_flags |= VM_IO | VM_RESERVED | VM_DONTCOPY | VM_DONTEXPAND;
+
+	retval = remap_pfn_range(vma,
+				 vma->vm_start,
+				 __phys_to_pfn(physical_addr),
+				 size, vma->vm_page_prot);
+
+	return retval;
+}
+
+/*!
+*******************************************************************************
+* Remove some memory from a user's memory space
+*
+* @param    user_addr       Userspace address of the memory to be unmapped
+* @param    size            Size of the memory to map (bytes)
+*
+* @return 
+*/
+os_error_code unmap_user_memory(uint32_t user_addr, uint32_t size)
+{
+	os_error_code retval;
+	struct mm_struct *mm = current->mm;
+
+	/* Unmap the memory region (see sys_munmap in mmap.c) */
+	down_write(&mm->mmap_sem);
+	retval = do_munmap(mm, (unsigned long)user_addr, size);
+	up_write(&mm->mmap_sem);
+
+	return retval;
+}
+
+/*!
+*******************************************************************************
+* Free descriptor back to free pool
+*
+* @brief Free descriptor
+*
+* @param    desc        A descriptor allocated with sah_Alloc_Descriptor().
+*
+* @return none
+*
+*/
+void sah_Free_Descriptor(sah_Desc * desc)
+{
+	memset(desc, 0x45, sizeof(*desc));
+	sah_Free_Block((Mem_Block *) desc);
+}
+
+/*!
+*******************************************************************************
+* Free Head descriptor back to free pool
+*
+* @brief Free Head descriptor
+*
+* @param    desc  A Head descriptor allocated with sah_Alloc_Head_Descriptor().
+*
+* @return none
+*
+*/
+void sah_Free_Head_Descriptor(sah_Head_Desc * desc)
+{
+	memset(desc, 0x43, sizeof(*desc));
+	sah_Free_Big_Block((Mem_Big_Block *) desc);
+}
+
+/*!
+*******************************************************************************
+* Free link back to free pool
+*
+* @brief Free link
+*
+* @param    link        A link allocated with sah_Alloc_Link().
+*
+* @return none
+*
+*/
+void sah_Free_Link(sah_Link * link)
+{
+	memset(link, 0x41, sizeof(*link));
+	sah_Free_Block((Mem_Block *) link);
+}
+
+/*!
+*******************************************************************************
+* This function runs through a descriptor chain pointed to by a user-space
+* address. It duplicates each descriptor in Kernel space memory and calls
+* sah_Copy_Links() to handle any links attached to the descriptors. This
+* function cleans-up everything that it created in the case of a failure.
+*
+* @brief Kernel Descriptor Chain Copier
+*
+* @param    fsl_shw_uco_t  The user context to act under
+* @param    user_head_desc A Head Descriptor pointer from user-space.
+*
+* @return   sah_Head_Desc *  - A virtual address of the first descriptor in the
+*                              chain.
+* @return   NULL        - If there was some error.
+*
+*/
+sah_Head_Desc *sah_Copy_Descriptors(fsl_shw_uco_t * user_ctx,
+				sah_Head_Desc * user_head_desc)
+{
+	sah_Desc *curr_desc = NULL;
+	sah_Desc *prev_desc = NULL;
+	sah_Desc *next_desc = NULL;
+	sah_Head_Desc *head_desc = NULL;
+	sah_Desc *user_desc = NULL;
+	unsigned long result;
+
+	/* Internal status variable to be used in this function */
+	fsl_shw_return_t status = FSL_RETURN_OK_S;
+	sah_Head_Desc *ret_val = NULL;
+
+	/* This will be set to True when we have finished processing our
+	 * descriptor chain.
+	 */
+	int drv_if_done = FALSE;
+	int is_this_the_head = TRUE;
+
+	do {
+		/* Allocate memory for this descriptor */
+		if (is_this_the_head) {
+			head_desc =
+			    (sah_Head_Desc *) sah_Alloc_Head_Descriptor();
+
+#ifdef DIAG_MEM
+			sprintf(Diag_msg,
+				"Alloc_Head_Descriptor returned %p\n",
+				head_desc);
+			LOG_KDIAG(Diag_msg);
+#endif
+			if (head_desc == NULL) {
+#ifdef DIAG_DRV_IF
+				LOG_KDIAG
+				    ("sah_Alloc_Head_Descriptor() failed.");
+#endif
+				drv_if_done = TRUE;
+				status = FSL_RETURN_NO_RESOURCE_S;
+			} else {
+				void *virt_addr = head_desc->desc.virt_addr;
+				dma_addr_t dma_addr = head_desc->desc.dma_addr;
+
+				/* Copy the head descriptor from user-space */
+				/* Instead of copying the whole structure,
+				 * unneeded bits at the end are left off.
+				 * The user space version is missing virt/dma addrs, which
+				 * means that the copy will be off for flags... */
+				result = copy_from_user(head_desc,
+							user_head_desc,
+							(sizeof(*head_desc) -
+							 sizeof(head_desc->desc.
+								dma_addr) -
+							 sizeof(head_desc->desc.
+								virt_addr) -
+							 sizeof(head_desc->desc.
+								original_ptr1) -
+/*                                sizeof(head_desc->desc.original_ptr2) -
+                                sizeof(head_desc->status) -
+                                sizeof(head_desc->error_status) -
+                                sizeof(head_desc->fault_address) -
+                                sizeof(head_desc->current_dar) -
+                                sizeof(head_desc->result) -
+                                sizeof(head_desc->next) -
+                                sizeof(head_desc->prev) -
+                                sizeof(head_desc->user_desc) -
+*/ sizeof(head_desc->out1_ptr) -
+							 sizeof(head_desc->
+								out2_ptr) -
+							 sizeof(head_desc->
+								out_len)));
+				/* there really isn't a 'next' descriptor at this point, so
+				 * set that pointer to NULL, but remember it for if/when there
+				 * is a next */
+				next_desc = head_desc->desc.next;
+				head_desc->desc.next = NULL;
+
+				if (result != 0) {
+#ifdef DIAG_DRV_IF
+					LOG_KDIAG("copy_from_user() failed.");
+#endif
+					drv_if_done = TRUE;
+					status = FSL_RETURN_INTERNAL_ERROR_S;
+					/* when destroying the descriptor, skip these links.
+					 * They've not been copied down, so don't exist */
+					head_desc->desc.ptr1 = NULL;
+					head_desc->desc.ptr2 = NULL;
+
+				} else {
+					/* The kernel DESC has five more words than user DESC, so
+					 * the missing values are in the middle of the HEAD DESC,
+					 * causing values after the missing ones to be at different
+					 * offsets in kernel and user space.
+					 *
+					 * Patch up the problem by moving field two spots.
+					 * This assumes sizeof(pointer) == sizeof(uint32_t).
+					 * Note that 'user_info' is not needed, so not copied.
+					 */
+					head_desc->user_ref =
+					    (uint32_t) head_desc->desc.dma_addr;
+					head_desc->uco_flags =
+					    (uint32_t) head_desc->desc.
+					    original_ptr1;
+#ifdef DIAG_DRV_IF
+					LOG_KDIAG_ARGS(
+						"User flags: %x; User Reference: %x",
+						head_desc->uco_flags,
+						head_desc->user_ref);
+#endif
+					/* These values were destroyed by the copy. */
+					head_desc->desc.virt_addr = virt_addr;
+					head_desc->desc.dma_addr = dma_addr;
+
+					/* ensure that the save descriptor chain bit is not set.
+					 * the copy of the user space descriptor chain should
+					 * always be deleted */
+					head_desc->uco_flags &=
+					    ~FSL_UCO_SAVE_DESC_CHAIN;
+
+					curr_desc = (sah_Desc *) head_desc;
+					is_this_the_head = FALSE;
+				}
+			}
+		} else {	/* not head */
+			curr_desc = sah_Alloc_Descriptor();
+#ifdef DIAG_MEM
+			LOG_KDIAG_ARGS("Alloc_Descriptor returned %p\n",
+				curr_desc);
+#endif
+			if (curr_desc == NULL) {
+#ifdef DIAG_DRV_IF
+				LOG_KDIAG("sah_Alloc_Descriptor() failed.");
+#endif
+				drv_if_done = TRUE;
+				status = FSL_RETURN_NO_RESOURCE_S;
+			} else {
+				/* need to update the previous descriptors' next field to
+				 * pointer to the current descriptor. */
+				prev_desc->original_next = curr_desc;
+				prev_desc->next =
+				    (sah_Desc *) curr_desc->dma_addr;
+
+				/* Copy the current descriptor from user-space */
+				/* The virtual address and DMA address part of the sah_Desc
+				 * struct are not copied to user space */
+				result = copy_from_user(curr_desc, user_desc, (sizeof(sah_Desc) - sizeof(dma_addr_t) -	/* dma_addr */
+									       sizeof(uint32_t) -	/* virt_addr */
+									       sizeof(void *) -	/* original_ptr1 */
+									       sizeof(void *) -	/* original_ptr2 */
+									       sizeof(sah_Desc **)));	/* original_next */
+				/* there really isn't a 'next' descriptor at this point, so
+				 * set that pointer to NULL, but remember it for if/when there
+				 * is a next */
+				next_desc = curr_desc->next;
+				curr_desc->next = NULL;
+				curr_desc->original_next = NULL;
+
+				if (result != 0) {
+#ifdef DIAG_DRV_IF
+					LOG_KDIAG("copy_from_user() failed.");
+#endif
+					drv_if_done = TRUE;
+					status = FSL_RETURN_INTERNAL_ERROR_S;
+					/* when destroying the descriptor chain, skip these links.
+					 * They've not been copied down, so don't exist */
+					curr_desc->ptr1 = NULL;
+					curr_desc->ptr2 = NULL;
+				}
+			}
+		}		/* end if (is_this_the_head) */
+
+		if (status == FSL_RETURN_OK_S) {
+			if (!(curr_desc->header & SAH_LLO_BIT)) {
+				/* One or both pointer fields being NULL is a valid
+				 * configuration. */
+				if (curr_desc->ptr1 == NULL) {
+					curr_desc->original_ptr1 = NULL;
+				} else {
+					/* pointer fields point to sah_Link structures */
+					curr_desc->original_ptr1 =
+					    sah_Copy_Links(user_ctx, curr_desc->ptr1);
+					if (curr_desc->original_ptr1 == NULL) {
+						/* This descriptor and any links created successfully
+						 * are cleaned-up at the bottom of this function. */
+						drv_if_done = TRUE;
+						status =
+						    FSL_RETURN_INTERNAL_ERROR_S;
+						/* mark that link 2 doesn't exist */
+						curr_desc->ptr2 = NULL;
+#ifdef DIAG_DRV_IF
+						LOG_KDIAG
+						    ("sah_Copy_Links() failed.");
+#endif
+					} else {
+						curr_desc->ptr1 = (void *)
+						    ((sah_Link *) curr_desc->
+						     original_ptr1)->dma_addr;
+					}
+				}
+
+				if (status == FSL_RETURN_OK_S) {
+					if (curr_desc->ptr2 == NULL) {
+						curr_desc->original_ptr2 = NULL;
+					} else {
+						/* pointer fields point to sah_Link structures */
+						curr_desc->original_ptr2 =
+						    sah_Copy_Links(user_ctx, curr_desc->ptr2);
+						if (curr_desc->original_ptr2 ==
+						    NULL) {
+							/* This descriptor and any links created
+							 * successfully are cleaned-up at the bottom of
+							 * this function. */
+							drv_if_done = TRUE;
+							status =
+							    FSL_RETURN_INTERNAL_ERROR_S;
+#ifdef DIAG_DRV_IF
+							LOG_KDIAG
+							    ("sah_Copy_Links() failed.");
+#endif
+						} else {
+							curr_desc->ptr2 =
+							    (void
+							     *)(((sah_Link *)
+								 curr_desc->
+								 original_ptr2)
+								->dma_addr);
+						}
+					}
+				}
+			} else {
+				/* Pointer fields point directly to user buffers. We don't
+				 * support this mode.
+				 */
+#ifdef DIAG_DRV_IF
+				LOG_KDIAG
+				    ("The LLO bit in the Descriptor Header field was "
+				     "set. This an invalid configuration.");
+#endif
+				drv_if_done = TRUE;
+				status = FSL_RETURN_INTERNAL_ERROR_S;
+			}
+		}
+
+		if (status == FSL_RETURN_OK_S) {
+			user_desc = next_desc;
+			prev_desc = curr_desc;
+			if (user_desc == NULL) {
+				/* We have reached the end our our descriptor chain */
+				drv_if_done = TRUE;
+			}
+		}
+
+	} while (drv_if_done == FALSE);
+
+	if (status != FSL_RETURN_OK_S) {
+		/* Clean-up if failed */
+		if (head_desc != NULL) {
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("Error!  Calling destroy descriptors!\n");
+#endif
+			sah_Destroy_Descriptors(head_desc);
+		}
+		ret_val = NULL;
+	} else {
+		/* Flush the caches */
+#ifndef FLUSH_SPECIFIC_DATA_ONLY
+		os_flush_cache_all();
+#endif
+
+		/* Success. Return the DMA'able head descriptor. */
+		ret_val = head_desc;
+
+	}
+
+	return ret_val;
+}				/* sah_Copy_Descriptors() */
+
+/*!
+*******************************************************************************
+* This function runs through a sah_Link chain pointed to by a kernel-space
+* address.  It computes the physical address for each pointer, and converts
+* the chain to use these physical addresses.
+*
+******
+*   This function needs to return some indication that the chain could not be
+*   converted.  It also needs to back out any conversion already taken place on
+*   this chain of links.
+*
+*   Then, of course, sah_Physicalise_Descriptors() will need to recognize that
+*   an error occured, and then be able to back out any physicalization of the
+*   chain which had taken place up to that point!
+******
+*
+* @brief Convert kernel Link chain
+*
+* @param    first_link   A sah_Link pointer from kernel space; must not be
+*                        NULL, so error case can be distinguished.
+*
+* @return   sah_Link *   A dma'able address of the first descriptor in the
+*                         chain.
+* @return   NULL         If Link chain could not be physicalised, i.e. ERROR
+*
+*/
+sah_Link *sah_Physicalise_Links(sah_Link * first_link)
+{
+	sah_Link *link = first_link;
+
+	while (link != NULL) {
+#ifdef DO_DBG
+		sah_Dump_Words("Link", (unsigned *)link, link->dma_addr, 3);
+#endif
+		link->vm_info = NULL;
+
+		/* need to retrieve stored key? */
+		if (link->flags & SAH_STORED_KEY_INFO) {
+			uint32_t max_len = 0;	/* max slot length */
+			fsl_shw_return_t ret_status;
+
+			/* get length and physical address of stored key */
+			ret_status = system_keystore_get_slot_info(link->ownerid, link->slot, (uint32_t *) & link->data,	/* RED key address */
+						       &max_len);
+			if ((ret_status != FSL_RETURN_OK_S) || (link->len > max_len)) {
+				/* trying to illegally/incorrectly access a key. Cause the
+				 * error status register to show a Link Length Error by
+				 * putting a zero in the links length. */
+				link->len = 0;	/* Cause error.  Somebody is up to no good. */
+			}
+		} else if (link->flags & SAH_IN_USER_KEYSTORE) {
+
+#ifdef FSL_HAVE_SCC2
+			/* The data field points to the virtual address of the key.  Convert
+			 * this to a physical address by modifying the address based
+			 * on where the secure memory was mapped to the kernel.  Note: In
+			 * kernel mode, no attempt is made to track or control who owns what
+			 * memory partition.
+			 */
+			link->data = (uint8_t *) scc_virt_to_phys(link->data);
+
+			/* Do bounds checking to ensure that the user is not overstepping
+			 * the bounds of their partition.  This is a simple implementation
+			 * that assumes the user only owns one partition.  It only checks
+			 * to see if the address of the last byte of data steps over a
+			 * page boundary.
+			 */
+
+#ifdef DO_DBG
+			LOG_KDIAG_ARGS("start page: %08x, end page: %08x"
+				       "first addr: %p, last addr: %p, len; %i",
+				       ((uint32_t) (link->data) >> PAGE_SHIFT),
+				       (((uint32_t) link->data +
+					 link->len) >> PAGE_SHIFT), link->data,
+				       link->data + link->len, link->len);
+#endif
+
+			if ((((uint32_t) link->data +
+			      link->len) >> PAGE_SHIFT) !=
+			    ((uint32_t) link->data >> PAGE_SHIFT)) {
+				link->len = 0;	/* Cause error.  Somebody is up to no good. */
+			}
+#else				/* FSL_HAVE_SCC2 */
+
+			/* User keystores are not valid on non-SCC2 platforms */
+			link->len = 0;	/* Cause error.  Somebody is up to no good. */
+
+#endif				/* FSL_HAVE_SCC2 */
+
+		} else {
+			if (!(link->flags & SAH_PREPHYS_DATA)) {
+				link->original_data = link->data;
+
+				/* All pointers are virtual right now */
+				link->data = (void *)os_pa(link->data);
+#ifdef DO_DBG
+				os_printk("%sput: %p (%d)\n",
+					  (link->
+					   flags & SAH_OUTPUT_LINK) ? "out" :
+					  "in", link->data, link->len);
+#endif
+
+				if (link->flags & SAH_OUTPUT_LINK) {
+					/* clean and invalidate */
+					os_cache_flush_range(link->
+							     original_data,
+							     link->len);
+				} else {
+					os_cache_clean_range(link->original_data,
+							     link->len);
+				}
+			}	/* not prephys */
+		}		/* else not key reference */
+
+#if defined(NO_OUTPUT_1K_CROSSING) || defined(NO_1K_CROSSING)
+		if (
+#ifdef NO_OUTPUT_1K_CROSSING
+			   /* Insert extra link if 1k boundary on output pointer
+			    * crossed not at an 8-word boundary */
+			   (link->flags & SAH_OUTPUT_LINK) &&
+			   (((uint32_t) link->data % 32) != 0) &&
+#endif
+			   ((((uint32_t) link->data & 1023) + link->len) >
+			    1024)) {
+			uint32_t full_length = link->len;
+			sah_Link *new_link = sah_Alloc_Link();
+			link->len = 1024 - ((uint32_t) link->data % 1024);
+			new_link->len = full_length - link->len;
+			new_link->data = link->data + link->len;
+			new_link->original_data =
+			    link->original_data + link->len;
+			new_link->flags = link->flags & ~(SAH_OWNS_LINK_DATA);
+			new_link->flags |= SAH_LINK_INSERTED_LINK;
+			new_link->next = link->next;
+
+			link->next = (sah_Link *) new_link->dma_addr;
+			link->original_next = new_link;
+			link = new_link;
+		}
+#endif				/* ALLOW_OUTPUT_1K_CROSSING */
+
+		link->original_next = link->next;
+		if (link->next != NULL) {
+			link->next = (sah_Link *) link->next->dma_addr;
+		}
+#ifdef DO_DBG
+		sah_Dump_Words("Link", link, link->dma_addr, 3);
+#endif
+
+		link = link->original_next;
+	}
+
+	return (sah_Link *) first_link->dma_addr;
+}				/* sah_Physicalise_Links */
+
+/*!
+ * Run through descriptors and links created by KM-API and set the
+ * dma addresses and 'do not free' flags.
+ *
+ * @param first_desc  KERNEL VIRTUAL address of first descriptor in chain.
+ *
+ * Warning!  This ONLY works without LLO flags in headers!!!
+ *
+ * @return Virtual address of @a first_desc.
+ * @return NULL if Descriptor Chain could not be physicalised
+ */
+sah_Head_Desc *sah_Physicalise_Descriptors(sah_Head_Desc * first_desc)
+{
+	sah_Desc *desc = &first_desc->desc;
+
+	if (!(first_desc->uco_flags & FSL_UCO_CHAIN_PREPHYSICALIZED)) {
+		while (desc != NULL) {
+			sah_Desc *next_desc;
+
+#ifdef DO_DBG
+
+			sah_Dump_Words("Desc", (unsigned *)desc, desc->dma_addr, 6);
+#endif
+
+			desc->original_ptr1 = desc->ptr1;
+			if (desc->ptr1 != NULL) {
+				if ((desc->ptr1 =
+				     sah_Physicalise_Links(desc->ptr1)) ==
+				    NULL) {
+					/* Clean up ... */
+					sah_DePhysicalise_Descriptors
+					    (first_desc);
+					first_desc = NULL;
+					break;
+				}
+			}
+			desc->original_ptr2 = desc->ptr2;
+			if (desc->ptr2 != NULL) {
+				if ((desc->ptr2 =
+				     sah_Physicalise_Links(desc->ptr2)) ==
+				    NULL) {
+					/* Clean up ... */
+					sah_DePhysicalise_Descriptors
+					    (first_desc);
+					first_desc = NULL;
+					break;
+				}
+			}
+
+			desc->original_next = desc->next;
+			next_desc = desc->next;	/* save for bottom of while loop */
+			if (desc->next != NULL) {
+				desc->next = (sah_Desc *) desc->next->dma_addr;
+			}
+
+			desc = next_desc;
+		}
+	}
+	/* not prephysicalized */
+#ifdef DO_DBG
+	os_printk("Physicalise finished\n");
+#endif
+
+	return first_desc;
+}				/* sah_Physicalise_Descriptors() */
+
+/*!
+*******************************************************************************
+* This function runs through a sah_Link chain pointed to by a physical address.
+* It computes the virtual address for each pointer
+*
+* @brief Convert physical Link chain
+*
+* @param    first_link   A kernel address of a sah_Link
+*
+* @return   sah_Link *  A kernal address for the link chain of @c first_link
+* @return   NULL        If there was some error.
+*
+* @post  All links will be chained together by original virtual addresses,
+*        data pointers will point to virtual addresses.  Appropriate cache
+*        lines will be flushed, memory unwired, etc.
+*/
+sah_Link *sah_DePhysicalise_Links(sah_Link * first_link)
+{
+	sah_Link *link = first_link;
+	sah_Link *prev_link = NULL;
+
+	/* Loop on virtual link pointer */
+	while (link != NULL) {
+
+#ifdef DO_DBG
+		sah_Dump_Words("Link", (unsigned *)link, link->dma_addr, 3);
+#endif
+
+		/* if this references stored keys, don't want to dephysicalize them */
+		if (!(link->flags & SAH_STORED_KEY_INFO)
+		    && !(link->flags & SAH_PREPHYS_DATA)
+		    && !(link->flags & SAH_IN_USER_KEYSTORE)) {
+
+			/* */
+			if (link->flags & SAH_OUTPUT_LINK) {
+				os_cache_inv_range(link->original_data,
+						   link->len);
+			}
+
+			/* determine if there is a page in user space associated with this
+			 * link */
+			if (link->vm_info != NULL) {
+				/* check that this isn't reserved and contains output */
+				if (!PageReserved(link->vm_info) &&
+				    (link->flags & SAH_OUTPUT_LINK)) {
+
+					/* Mark to force page eventually to backing store */
+					SetPageDirty(link->vm_info);
+				}
+
+				/* Untie this page from physical memory */
+				page_cache_release(link->vm_info);
+			} else {
+				/* kernel-mode data */
+#ifdef DO_DBG
+				os_printk("%sput: %p (%d)\n",
+					  (link->
+					   flags & SAH_OUTPUT_LINK) ? "out" :
+					  "in", link->original_data, link->len);
+#endif
+			}
+			link->data = link->original_data;
+		}
+#ifndef ALLOW_OUTPUT_1K_CROSSING
+		if (link->flags & SAH_LINK_INSERTED_LINK) {
+			/* Reconsolidate data by merging this link with previous */
+			prev_link->len += link->len;
+			prev_link->next = link->next;
+			prev_link->original_next = link->original_next;
+			sah_Free_Link(link);
+			link = prev_link;
+
+		}
+#endif
+
+		if (link->next != NULL) {
+			link->next = link->original_next;
+		}
+		prev_link = link;
+		link = link->next;
+	}
+
+	return first_link;
+}				/* sah_DePhysicalise_Links() */
+
+/*!
+ * Run through descriptors and links that have been Physicalised
+ * (sah_Physicalise_Descriptors function) and set the dma addresses back
+ * to KM virtual addresses
+ *
+ * @param first_desc Kernel virtual address of first descriptor in chain.
+ *
+ * Warning!  This ONLY works without LLO flags in headers!!!
+ */
+sah_Head_Desc *sah_DePhysicalise_Descriptors(sah_Head_Desc * first_desc)
+{
+	sah_Desc *desc = &first_desc->desc;
+
+	if (!(first_desc->uco_flags & FSL_UCO_CHAIN_PREPHYSICALIZED)) {
+		while (desc != NULL) {
+#ifdef DO_DBG
+			sah_Dump_Words("Desc", (unsigned *)desc, desc->dma_addr, 6);
+#endif
+
+			if (desc->ptr1 != NULL) {
+				desc->ptr1 =
+				    sah_DePhysicalise_Links(desc->
+							    original_ptr1);
+			}
+			if (desc->ptr2 != NULL) {
+				desc->ptr2 =
+				    sah_DePhysicalise_Links(desc->
+							    original_ptr2);
+			}
+			if (desc->next != NULL) {
+				desc->next = desc->original_next;
+			}
+			desc = desc->next;
+		}
+	}
+	/* not prephysicalized */
+	return first_desc;
+}				/* sah_DePhysicalise_Descriptors() */
+
+/*!
+*******************************************************************************
+* This walks through a SAHARA descriptor chain and free()'s everything
+* that is not NULL. Finally it also unmaps all of the physical memory and
+* frees the kiobuf_list Queue.
+*
+* @brief Kernel Descriptor Chain Destructor
+*
+* @param    head_desc   A Descriptor pointer from kernel-space.
+*
+* @return   void
+*
+*/
+void sah_Free_Chained_Descriptors(sah_Head_Desc * head_desc)
+{
+	sah_Desc *desc = NULL;
+	sah_Desc *next_desc = NULL;
+	int this_is_head = 1;
+
+	desc = &head_desc->desc;
+
+	while (desc != NULL) {
+
+		sah_Free_Chained_Links(desc->ptr1);
+		sah_Free_Chained_Links(desc->ptr2);
+
+		/* Get a bus pointer to the next Descriptor */
+		next_desc = desc->next;
+
+		/* Zero the header and Length fields for security reasons. */
+		desc->header = 0;
+		desc->len1 = 0;
+		desc->len2 = 0;
+
+		if (this_is_head) {
+			sah_Free_Head_Descriptor(head_desc);
+			this_is_head = 0;
+#ifdef DIAG_MEM
+			sprintf(Diag_msg, "Free_Head_Descriptor: %p\n",
+				head_desc);
+			LOG_KDIAG(Diag_msg);
+#endif
+		} else {
+			/* free this descriptor */
+			sah_Free_Descriptor(desc);
+#ifdef DIAG_MEM
+			sprintf(Diag_msg, "Free_Descriptor: %p\n", desc);
+			LOG_KDIAG(Diag_msg);
+#endif
+		}
+
+		/* Look at the next Descriptor */
+		desc = next_desc;
+	}
+}				/* sah_Free_Chained_Descriptors() */
+
+/*!
+*******************************************************************************
+* This walks through a SAHARA link chain and frees everything that is
+* not NULL, excluding user-space buffers.
+*
+* @brief Kernel Link Chain Destructor
+*
+* @param    link    A Link pointer from kernel-space. This is in bus address
+*                   space.
+*
+* @return   void
+*
+*/
+void sah_Free_Chained_Links(sah_Link * link)
+{
+	sah_Link *next_link = NULL;
+
+	while (link != NULL) {
+		/* Get a bus pointer to the next Link */
+		next_link = link->next;
+
+		/* Zero some fields for security reasons. */
+		link->data = NULL;
+		link->len = 0;
+		link->ownerid = 0;
+
+		/* Free this Link */
+#ifdef DIAG_MEM
+		sprintf(Diag_msg, "Free_Link: %p(->%p)\n", link, link->next);
+		LOG_KDIAG(Diag_msg);
+#endif
+		sah_Free_Link(link);
+
+		/* Move on to the next Link */
+		link = next_link;
+	}
+}
+
+/*!
+*******************************************************************************
+* This function runs through a link chain pointed to by a user-space
+* address. It makes a temporary kernel-space copy of each link in the
+* chain and calls sah_Make_Links() to create a set of kernel-side links
+* to replace it.
+*
+* @brief Kernel Link Chain Copier
+*
+* @param    ptr         A link pointer from user-space.
+*
+* @return   sah_Link *  - The virtual address of the first link in the
+*                         chain.
+* @return   NULL        - If there was some error.
+*/
+sah_Link *sah_Copy_Links(fsl_shw_uco_t * user_ctx, sah_Link * ptr)
+{
+	sah_Link *head_link = NULL;
+	sah_Link *new_head_link = NULL;
+	sah_Link *new_tail_link = NULL;
+	sah_Link *prev_tail_link = NULL;
+	sah_Link *user_link = ptr;
+	sah_Link link_copy;
+	int link_data_length = 0;
+
+	/* Internal status variable to be used in this function */
+	fsl_shw_return_t status = FSL_RETURN_OK_S;
+	sah_Link *ret_val = NULL;
+
+	/* This will be set to True when we have finished processing our
+	 * link chain. */
+	int drv_if_done = FALSE;
+	int is_this_the_head = TRUE;
+	int result;
+
+	/* transfer all links, on this link chain, from user space */
+	while (drv_if_done == FALSE) {
+		/* Copy the current link from user-space. The virtual address, DMA
+		 * address, and vm_info fields of the sah_Link struct are not part
+		 * of the user-space structure. They must be the last elements and
+		 * should not be copied. */
+		result = copy_from_user(&link_copy,
+					user_link, (sizeof(sah_Link) -
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
+						    sizeof(struct page *) -	/* vm_info */
+#endif
+						    sizeof(dma_addr_t) -	/* dma_addr */
+						    sizeof(uint32_t) -	/* virt_addr */
+						    sizeof(uint8_t *) -	/* original_data */
+						    sizeof(sah_Link *)));	/* original_next */
+
+		if (result != 0) {
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("copy_from_user() failed.");
+#endif
+			drv_if_done = TRUE;
+			status = FSL_RETURN_INTERNAL_ERROR_S;
+		}
+
+		if (status == FSL_RETURN_OK_S) {
+			/* This will create new links which can be used to replace tmp_link
+			 * in the chain. This will return a new head and tail link. */
+			link_data_length = link_data_length + link_copy.len;
+			new_head_link =
+			    sah_Make_Links(user_ctx, &link_copy, &new_tail_link);
+
+			if (new_head_link == NULL) {
+				/* If we ran out of memory or a user pointer was invalid */
+				drv_if_done = TRUE;
+				status = FSL_RETURN_INTERNAL_ERROR_S;
+#ifdef DIAG_DRV_IF
+				LOG_KDIAG("sah_Make_Links() failed.");
+#endif
+			} else {
+				if (is_this_the_head == TRUE) {
+					/* Keep a reference to the head link */
+					head_link = new_head_link;
+					is_this_the_head = FALSE;
+				} else {
+					/* Need to update the previous links' next field to point
+					 * to the current link. */
+					prev_tail_link->next =
+					    (void *)new_head_link->dma_addr;
+					prev_tail_link->original_next =
+					    new_head_link;
+				}
+			}
+		}
+
+		if (status == FSL_RETURN_OK_S) {
+			/* Get to the next link in the chain. */
+			user_link = link_copy.next;
+			prev_tail_link = new_tail_link;
+
+			/* Check if the end of the link chain was reached (TRUE) or if
+			 * there is another linked to this one (FALSE) */
+			drv_if_done = (user_link == NULL) ? TRUE : FALSE;
+		}
+	}			/* end while */
+
+	if (status != FSL_RETURN_OK_S) {
+		ret_val = NULL;
+		/* There could be clean-up to do here because we may have made some
+		 * successful iterations through the while loop and as a result, the
+		 * links created by sah_Make_Links() need to be destroyed.
+		 */
+		if (head_link != NULL) {
+			/* Failed somewhere in the while loop and need to clean-up. */
+			sah_Destroy_Links(head_link);
+		}
+	} else {
+		/* Success. Return the head link. */
+		ret_val = head_link;
+	}
+
+	return ret_val;
+}				/* sah_Copy_Links() */
+
+/*!
+*******************************************************************************
+* This function takes an input link pointed to by a user-space address
+* and returns a chain of links that span the physical pages pointed
+* to by the input link.
+*
+* @brief Kernel Link Chain Constructor
+*
+* @param    ptr         A link pointer from user-space.
+* @param    tail        The address of a link pointer. This is used to return
+*                       the tail link created by this function.
+*
+* @return   sah_Link *  - A virtual address of the first link in the
+*                         chain.
+* @return   NULL        - If there was some error.
+*
+*/
+sah_Link *sah_Make_Links(fsl_shw_uco_t * user_ctx, 
+							sah_Link * ptr, sah_Link ** tail)
+{
+	int result = -1;
+	int page_index = 0;
+	fsl_shw_return_t status = FSL_RETURN_OK_S;
+	int is_this_the_head = TRUE;
+	void *buffer_start = NULL;
+	sah_Link *link = NULL;
+	sah_Link *prev_link = NULL;
+	sah_Link *head_link = NULL;
+	sah_Link *ret_val = NULL;
+	int buffer_length = 0;
+	struct page **local_pages = NULL;
+	int nr_pages = 0;
+	int write = (sah_Link_Get_Flags(ptr) & SAH_OUTPUT_LINK) ? WRITE : READ;
+
+	/* need to retrieve stored key? */
+	if (ptr->flags & SAH_STORED_KEY_INFO) {
+		fsl_shw_return_t ret_status;
+
+		/* allocate space for this link */
+		link = sah_Alloc_Link();
+#ifdef DIAG_MEM
+		sprintf(Diag_msg, "Alloc_Link returned %p/%p\n", link,
+			(void *)link->dma_addr);
+		LOG_KDIAG(Diag_msg);
+#endif
+
+		if (link == NULL) {
+			status = FSL_RETURN_NO_RESOURCE_S;
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("sah_Alloc_Link() failed!");
+#endif
+			return link;
+		} else {
+			uint32_t max_len = 0;	/* max slot length */
+
+			/* get length and physical address of stored key */
+			ret_status = system_keystore_get_slot_info(ptr->ownerid, ptr->slot, (uint32_t *) & link->data,	/* RED key address */
+						      &max_len);
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG_ARGS
+			("ret_status==SCC_RET_OK? %s.  slot: %i. data: %p"
+			 ". len: %i, key length: %i",
+		 	(ret_status == FSL_RETURN_OK_S ? "yes" : "no"),
+			 ptr->slot, link->data, max_len, ptr->len);
+#endif
+
+			if ((ret_status == FSL_RETURN_OK_S) && (ptr->len <= max_len)) {
+				/* finish populating the link */
+				link->len = ptr->len;
+				link->flags = ptr->flags & ~SAH_PREPHYS_DATA;
+				*tail = link;
+			} else {
+#ifdef DIAG_DRV_IF
+				if (ret_status == FSL_RETURN_OK_S) {
+					LOG_KDIAG
+					    ("SCC sah_Link key slot reference is too long");
+				} else {
+					LOG_KDIAG
+					    ("SCC sah_Link slot slot reference is invalid");
+				}
+#endif
+				sah_Free_Link(link);
+				status = FSL_RETURN_INTERNAL_ERROR_S;
+				return NULL;
+			}
+			return link;
+		}
+	}	else if (ptr->flags & SAH_IN_USER_KEYSTORE) {
+
+#ifdef FSL_HAVE_SCC2
+
+		void *kernel_base;
+
+		/* allocate space for this link */
+		link = sah_Alloc_Link();
+#ifdef DIAG_MEM
+		sprintf(Diag_msg, "Alloc_Link returned %p/%p\n", link,
+			(void *)link->dma_addr);
+		LOG_KDIAG(Diag_msg);
+#endif				/* DIAG_MEM */
+
+		if (link == NULL) {
+			status = FSL_RETURN_NO_RESOURCE_S;
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("sah_Alloc_Link() failed!");
+#endif
+			return link;
+		} else {
+			/* link->data points to the virtual address of the key data, however
+			 * this memory does not need to be locked down.
+			 */
+			kernel_base = lookup_user_partition(user_ctx,
+							    (uint32_t) ptr->
+							    data & PAGE_MASK);
+
+			link->data = (uint8_t *) scc_virt_to_phys(kernel_base +
+								  ((unsigned
+								    long)ptr->
+								   data &
+								   ~PAGE_MASK));
+
+			/* Do bounds checking to ensure that the user is not overstepping
+			 * the bounds of their partition.  This is a simple implementation
+			 * that assumes the user only owns one partition.  It only checks
+			 * to see if the address of the last byte of data steps over a
+			 * page boundary.
+			 */
+			if ((kernel_base != NULL) &&
+			    ((((uint32_t) link->data +
+			       link->len) >> PAGE_SHIFT) ==
+			     ((uint32_t) link->data >> PAGE_SHIFT))) {
+				/* finish populating the link */
+				link->len = ptr->len;
+				link->flags = ptr->flags & ~SAH_PREPHYS_DATA;
+				*tail = link;
+			} else {
+#ifdef DIAG_DRV_IF
+				if (kernel_base != NULL) {
+					LOG_KDIAG
+					    ("SCC sah_Link key slot reference is too long");
+				} else {
+					LOG_KDIAG
+					    ("SCC sah_Link slot slot reference is invalid");
+				}
+#endif
+				sah_Free_Link(link);
+				status = FSL_RETURN_INTERNAL_ERROR_S;
+				return NULL;
+			}
+			return link;
+		}
+
+#else				/* FSL_HAVE_SCC2 */
+
+		return NULL;
+
+#endif				/* FSL_HAVE_SCC2 */
+	}
+	
+	if (ptr->data == NULL) {
+		/* The user buffer must not be NULL because map_user_kiobuf() cannot
+		 * handle NULL pointer input.
+		 */
+		status = FSL_RETURN_BAD_DATA_LENGTH_S;
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("sah_Link data pointer is NULL.");
+#endif
+	}
+
+	if (status == FSL_RETURN_OK_S) {
+		unsigned long start_page = (unsigned long)ptr->data & PAGE_MASK;
+
+		/* determine number of pages being used for this link */
+		nr_pages = (((unsigned long)(ptr->data) & ~PAGE_MASK)
+			    + ptr->len + ~PAGE_MASK) >> PAGE_SHIFT;
+
+		/* ptr contains all the 'user space' information, add the pages
+		 * to it also just so everything is in one place */
+		local_pages =
+		    kmalloc(nr_pages * sizeof(struct page *), GFP_KERNEL);
+
+		if (local_pages == NULL) {
+			status = FSL_RETURN_NO_RESOURCE_S;	/* no memory! */
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("kmalloc() failed.");
+#endif
+		} else {
+			/* get the actual pages being used in 'user space' */
+
+			down_read(&current->mm->mmap_sem);
+			result = get_user_pages(current, current->mm,
+						start_page, nr_pages,
+						write, 0 /* noforce */ ,
+						local_pages, NULL);
+			up_read(&current->mm->mmap_sem);
+
+			if (result < nr_pages) {
+#ifdef DIAG_DRV_IF
+				LOG_KDIAG("get_user_pages() failed.");
+#endif
+				if (result > 0) {
+					for (page_index = 0;
+					     page_index < result;
+					     page_index++) {
+						page_cache_release(local_pages
+								   [page_index]);
+					}
+				}
+				status = FSL_RETURN_INTERNAL_ERROR_S;
+			}
+		}
+	}
+
+	/* Now we can walk through the list of pages in the buffer */
+	if (status == FSL_RETURN_OK_S) {
+
+#if defined(FLUSH_SPECIFIC_DATA_ONLY) && !defined(HAS_L2_CACHE)
+		/*
+		 * Now that pages are wired, clear user data from cache lines.  When
+		 * there is just an L1 cache, clean based on user virtual for ARM.
+		 */
+		if (write == WRITE) {
+			os_cache_flush_range(ptr->data, ptr->len);
+		} else {
+			os_cache_clean_range(ptr->data, ptr->len);
+		}
+#endif
+
+		for (page_index = 0; page_index < nr_pages; page_index++) {
+			/* Allocate a new link structure */
+			link = sah_Alloc_Link();
+#ifdef DIAG_MEM
+			sprintf(Diag_msg, "Alloc_Link returned %p/%p\n", link,
+				(void *)link->dma_addr);
+			LOG_KDIAG(Diag_msg);
+#endif
+			if (link == NULL) {
+#ifdef DIAG_DRV_IF
+				LOG_KDIAG("sah_Alloc_Link() failed.");
+#endif
+				status = FSL_RETURN_NO_RESOURCE_S;
+
+				/* need to free the rest of the pages. Destroy_Links will take
+				 * care of the ones already assigned to a link */
+				for (; page_index < nr_pages; page_index++) {
+					page_cache_release(local_pages
+							   [page_index]);
+				}
+				break;	/* exit 'for page_index' loop */
+			}
+
+			if (status == FSL_RETURN_OK_S) {
+				if (is_this_the_head == TRUE) {
+					/* keep a reference to the head link */
+					head_link = link;
+					/* remember that we have seen the head link */
+					is_this_the_head = FALSE;
+				} else {
+					/* If this is not the head link then set the previous
+					 * link's next pointer to point to this link */
+					prev_link->original_next = link;
+					prev_link->next =
+					    (sah_Link *) link->dma_addr;
+				}
+
+				buffer_start =
+				    page_address(local_pages[page_index]);
+
+				if (page_index == 0) {
+					/* If this is the first page, there might be an
+					 * offset. We need to increment the address by this offset
+					 * so we don't just get the start of the page.
+					 */
+					buffer_start +=
+					    (unsigned long)
+					    sah_Link_Get_Data(ptr)
+					    & ~PAGE_MASK;
+					buffer_length = PAGE_SIZE
+					    -
+					    ((unsigned long)
+					     sah_Link_Get_Data(ptr)
+					     & ~PAGE_MASK);
+				} else {
+					buffer_length = PAGE_SIZE;
+				}
+
+				if (page_index == nr_pages - 1) {
+					/* if this is the last page, we need to adjust
+					 * the buffer_length to account for the last page being
+					 * partially used.
+					 */
+					buffer_length -=
+					    nr_pages * PAGE_SIZE -
+					    sah_Link_Get_Len(ptr) -
+					    ((unsigned long)
+					     sah_Link_Get_Data(ptr) &
+					     ~PAGE_MASK);
+				}
+#if defined(FLUSH_SPECIFIC_DATA_ONLY) && defined(HAS_L2_CACHE)
+				/*
+				 * When there is an L2 cache, clean based on kernel
+				 * virtual..
+				 */
+				if (write == WRITE) {
+					os_cache_flush_range(buffer_start,
+							     buffer_length);
+				} else {
+					os_cache_clean_range(buffer_start,
+							     buffer_length);
+				}
+#endif
+
+				/* Fill in link information */
+				link->len = buffer_length;
+#if !defined(HAS_L2_CACHE)
+				/* use original virtual */
+				link->original_data = ptr->data;
+#else
+				/* use kernel virtual */
+				link->original_data = buffer_start;
+#endif
+				link->data = (void *)os_pa(buffer_start);
+				link->flags = ptr->flags & ~SAH_PREPHYS_DATA;
+				link->vm_info = local_pages[page_index];
+				prev_link = link;
+
+#if defined(NO_OUTPUT_1K_CROSSING) || defined(NO_1K_CROSSING)
+				if (
+#ifdef NO_OUTPUT_1K_CROSSING
+					   /* Insert extra link if 1k boundary on output pointer
+					    * crossed not at an 8-word boundary */
+					   (link->flags & SAH_OUTPUT_LINK) &&
+					   (((uint32_t) buffer_start % 32) != 0)
+					   &&
+#endif
+					   ((((uint32_t) buffer_start & 1023) +
+					     buffer_length) > 1024)) {
+
+					/* Shorten current link to 1k boundary */
+					link->len =
+					    1024 -
+					    ((uint32_t) buffer_start % 1024);
+
+					/* Get new link to follow it */
+					link = sah_Alloc_Link();
+					prev_link->len =
+					    1024 -
+					    ((uint32_t) buffer_start % 1024);
+					prev_link->original_next = link;
+					prev_link->next =
+					    (sah_Link *) link->dma_addr;
+					buffer_length -= prev_link->len;
+					buffer_start += prev_link->len;
+
+#if !defined(HAS_L2_CACHE)
+					/* use original virtual */
+					link->original_data = ptr->data;
+#else
+					/* use kernel virtual */
+					link->original_data = buffer_start;
+#endif
+					link->data =
+					    (void *)os_pa(buffer_start);
+					link->vm_info = prev_link->vm_info;
+					prev_link->vm_info = NULL;	/* delay release */
+					link->flags = ptr->flags;
+					link->len = buffer_length;
+					prev_link = link;
+				}	/* while link would cross 1K boundary */
+#endif				/* 1K_CROSSING */
+			}
+		}		/* for each page */
+	}
+
+	if (local_pages != NULL) {
+		kfree(local_pages);
+	}
+
+	if (status != FSL_RETURN_OK_S) {
+		/* De-allocated any links created, this routine first looks if
+		 * head_link is NULL */
+		sah_Destroy_Links(head_link);
+
+		/* Clean-up of the KIOBUF will occur in the * sah_Copy_Descriptors()
+		 * function.
+		 * Clean-up of the Queue entry must occur in the function called
+		 * sah_Copy_Descriptors().
+		 */
+	} else {
+
+		/* Success. Return the head link. */
+		ret_val = head_link;
+		link->original_next = NULL;
+		/* return the tail link as well */
+		*tail = link;
+	}
+
+	return ret_val;
+}				/* sah_Make_Links() */
+
+/*!
+*******************************************************************************
+* This walks through a SAHARA descriptor chain and frees everything
+* that is not NULL. Finally it also unmaps all of the physical memory and
+* frees the kiobuf_list Queue.
+*
+* @brief Kernel Descriptor Chain Destructor
+*
+* @param    desc        A Descriptor pointer from kernel-space. This should be
+*                       in bus address space.
+*
+* @return   void
+*
+*/
+void sah_Destroy_Descriptors(sah_Head_Desc * head_desc)
+{
+	sah_Desc *this_desc = (sah_Desc *) head_desc;
+	sah_Desc *next_desc = NULL;
+	int this_is_head = 1;
+
+	/*
+	 * Flush the D-cache. This flush is here because the hardware has finished
+	 * processing this descriptor and probably has changed the contents of
+	 * some linked user buffers as a result.  This flush will enable
+	 * user-space applications to see the correct data rather than the
+	 * out-of-date cached version.
+	 */
+#ifndef FLUSH_SPECIFIC_DATA_ONLY
+	os_flush_cache_all();
+#endif
+
+	head_desc = (sah_Head_Desc *) this_desc->virt_addr;
+
+	while (this_desc != NULL) {
+		if (this_desc->ptr1 != NULL) {
+			sah_Destroy_Links(this_desc->original_ptr1
+					  ? this_desc->
+					  original_ptr1 : this_desc->ptr1);
+		}
+		if (this_desc->ptr2 != NULL) {
+			sah_Destroy_Links(this_desc->original_ptr2
+					  ? this_desc->
+					  original_ptr2 : this_desc->ptr2);
+		}
+
+		/* Get a bus pointer to the next Descriptor */
+		next_desc = (this_desc->original_next
+			     ? this_desc->original_next : this_desc->next);
+
+		/* Zero the header and Length fields for security reasons. */
+		this_desc->header = 0;
+		this_desc->len1 = 0;
+		this_desc->len2 = 0;
+
+		if (this_is_head) {
+			sah_Free_Head_Descriptor(head_desc);
+#ifdef DIAG_MEM
+			sprintf(Diag_msg, "Free_Head_Descriptor: %p\n",
+				head_desc);
+			LOG_KDIAG(Diag_msg);
+#endif
+			this_is_head = 0;
+		} else {
+			/* free this descriptor */
+			sah_Free_Descriptor(this_desc);
+#ifdef DIAG_MEM
+			sprintf(Diag_msg, "Free_Descriptor: %p\n", this_desc);
+			LOG_KDIAG(Diag_msg);
+#endif
+		}
+
+		/* Set up for next round. */
+		this_desc = (sah_Desc *) next_desc;
+	}
+}
+
+/*!
+*******************************************************************************
+* This walks through a SAHARA link chain and frees everything that is
+* not NULL excluding user-space buffers.
+*
+* @brief Kernel Link Chain Destructor
+*
+* @param    link    A Link pointer from kernel-space.
+*
+* @return   void
+*
+*/
+void sah_Destroy_Links(sah_Link * link)
+{
+	sah_Link *this_link = link;
+	sah_Link *next_link = NULL;
+
+	while (this_link != NULL) {
+
+		/* if this link indicates an associated page, process it */
+		if (this_link->vm_info != NULL) {
+			/* since this function is only called from the routine that
+			 * creates a kernel copy of the user space descriptor chain,
+			 * there are no pages to dirty. All that is needed is to release
+			 * the page from cache */
+			page_cache_release(this_link->vm_info);
+		}
+
+		/* Get a bus pointer to the next Link */
+		next_link = (this_link->original_next
+			     ? this_link->original_next : this_link->next);
+
+		/* Zero the Pointer and Length fields for security reasons. */
+		this_link->data = NULL;
+		this_link->len = 0;
+
+		/* Free this Link */
+		sah_Free_Link(this_link);
+#ifdef DIAG_MEM
+		sprintf(Diag_msg, "Free_Link: %p\n", this_link);
+		LOG_KDIAG(Diag_msg);
+#endif
+
+		/* Look at the next Link */
+		this_link = next_link;
+	}
+}
+
+/*!
+*******************************************************************************
+* @brief Initialize memory manager/mapper.
+*
+* In 2.4, this function also allocates a kiovec to be used when mapping user
+* data to kernel space
+*
+* @return 0 for success, OS error code on failure
+*
+*/
+int sah_Init_Mem_Map(void)
+{
+	int ret = OS_ERROR_FAIL_S;
+
+	mem_lock = os_lock_alloc_init();
+
+	/*
+	 * If one of these fails, change the calculation in the #define earlier in
+	 * the file to be the other one.
+	 */
+	if (sizeof(sah_Link) > MEM_BLOCK_SIZE) {
+		os_printk("Sahara Driver: sah_Link structure is too large\n");
+	} else if (sizeof(sah_Desc) > MEM_BLOCK_SIZE) {
+		os_printk("Sahara Driver: sah_Desc structure is too large\n");
+	} else {
+		ret = OS_ERROR_OK_S;
+	}
+
+#ifndef SELF_MANAGED_POOL
+
+	big_dma_pool = dma_pool_create("sah_big_blocks", NULL,
+				       sizeof(Mem_Big_Block), sizeof(uint32_t),
+				       PAGE_SIZE);
+	small_dma_pool = dma_pool_create("sah_small_blocks", NULL,
+					 sizeof(Mem_Block), sizeof(uint32_t),
+					 PAGE_SIZE);
+#else
+
+#endif
+	return ret;
+}
+
+/*!
+*******************************************************************************
+* @brief Clean up memory manager/mapper.
+*
+* In 2.4, this function also frees the kiovec used when mapping user data to
+* kernel space.
+*
+* @return none
+*
+*/
+void sah_Stop_Mem_Map(void)
+{
+	os_lock_deallocate(mem_lock);
+
+#ifndef SELF_MANAGED_POOL
+	if (big_dma_pool != NULL) {
+		dma_pool_destroy(big_dma_pool);
+	}
+	if (small_dma_pool != NULL) {
+		dma_pool_destroy(small_dma_pool);
+	}
+#endif
+}
+
+/*!
+*******************************************************************************
+* Allocate Head descriptor from free pool.
+*
+* @brief Allocate Head descriptor
+*
+* @return sah_Head_Desc Free descriptor, NULL if no free descriptors available.
+*
+*/
+sah_Head_Desc *sah_Alloc_Head_Descriptor(void)
+{
+	Mem_Big_Block *block;
+	sah_Head_Desc *desc;
+
+	block = sah_Alloc_Big_Block();
+	if (block != NULL) {
+		/* initialize everything */
+		desc = (sah_Head_Desc *) block->data;
+
+		desc->desc.virt_addr = (sah_Desc *) desc;
+		desc->desc.dma_addr = block->dma_addr;
+		desc->desc.original_ptr1 = NULL;
+		desc->desc.original_ptr2 = NULL;
+		desc->desc.original_next = NULL;
+
+		desc->desc.ptr1 = NULL;
+		desc->desc.ptr2 = NULL;
+		desc->desc.next = NULL;
+	} else {
+		desc = NULL;
+	}
+
+	return desc;
+}
+
+/*!
+*******************************************************************************
+* Allocate descriptor from free pool.
+*
+* @brief Allocate descriptor
+*
+* @return sah_Desc Free descriptor, NULL if no free descriptors available.
+*
+*/
+sah_Desc *sah_Alloc_Descriptor(void)
+{
+	Mem_Block *block;
+	sah_Desc *desc;
+
+	block = sah_Alloc_Block();
+	if (block != NULL) {
+		/* initialize everything */
+		desc = (sah_Desc *) block->data;
+
+		desc->virt_addr = desc;
+		desc->dma_addr = block->dma_addr;
+		desc->original_ptr1 = NULL;
+		desc->original_ptr2 = NULL;
+		desc->original_next = NULL;
+
+		desc->ptr1 = NULL;
+		desc->ptr2 = NULL;
+		desc->next = NULL;
+	} else {
+		desc = NULL;
+	}
+
+	return (desc);
+}
+
+/*!
+*******************************************************************************
+* Allocate link from free pool.
+*
+* @brief Allocate link
+*
+* @return sah_Link Free link, NULL if no free links available.
+*
+*/
+sah_Link *sah_Alloc_Link(void)
+{
+	Mem_Block *block;
+	sah_Link *link;
+
+	block = sah_Alloc_Block();
+	if (block != NULL) {
+		/* initialize everything */
+		link = (sah_Link *) block->data;
+
+		link->virt_addr = link;
+		link->original_next = NULL;
+		link->original_data = NULL;
+		/* information found in allocated block */
+		link->dma_addr = block->dma_addr;
+
+		/* Sahara link fields */
+		link->len = 0;
+		link->data = NULL;
+		link->next = NULL;
+
+		/* driver required fields */
+		link->flags = 0;
+		link->vm_info = NULL;
+	} else {
+		link = NULL;
+	}
+
+	return link;
+}
+
+#ifdef SELF_MANAGED_POOL
+/*!
+*******************************************************************************
+* Add a new page to end of block free pool. This will allocate one page and
+* fill the pool with entries, appending to the end.
+*
+* @brief Add page of blocks to block free pool.
+*
+* @pre This function must be called with the #mem_lock held.
+*
+* @param   big    0        - make blocks big enough for sah_Desc
+*                 non-zero - make blocks big enough for sah_Head_Desc
+*
+* @return int TRUE if blocks added succeesfully, FALSE otherwise
+*
+*/
+int sah_Block_Add_Page(int big)
+{
+	void *page;
+	int success;
+	dma_addr_t dma_addr;
+	unsigned block_index;
+	uint32_t dma_offset;
+	unsigned block_entries =
+	    big ? MEM_BIG_BLOCK_ENTRIES : MEM_BLOCK_ENTRIES;
+	unsigned block_size = big ? sizeof(Mem_Big_Block) : sizeof(Mem_Block);
+	void *block;
+
+	/* Allocate page of memory */
+#ifndef USE_COHERENT_MEMORY
+	page = os_alloc_memory(PAGE_SIZE, GFP_ATOMIC | __GFP_DMA);
+	dma_addr = os_pa(page);
+#else
+	page = os_alloc_coherent(PAGE_SIZE, &dma_addr, GFP_ATOMIC);
+#endif
+	if (page != NULL) {
+		/*
+		 * Find the difference between the virtual address and the DMA
+		 * address of the page. This is used later to determine the DMA
+		 * address of each individual block.
+		 */
+		dma_offset = page - (void *)dma_addr;
+
+		/* Split page into blocks and add to free pool */
+		block = page;
+		for (block_index = 0; block_index < block_entries;
+		     block_index++) {
+			if (big) {
+				register Mem_Big_Block *blockp = block;
+				blockp->dma_addr =
+				    (uint32_t) (block - dma_offset);
+				sah_Append_Big_Block(blockp);
+			} else {
+				register Mem_Block *blockp = block;
+				blockp->dma_addr =
+				    (uint32_t) (block - dma_offset);
+				/* sah_Append_Block must be protected with spin locks. This is
+				 * done in sah_Alloc_Block(), which calls
+				 * sah_Block_Add_Page() */
+				sah_Append_Block(blockp);
+			}
+			block += block_size;
+		}
+		success = TRUE;
+#ifdef DIAG_MEM
+		LOG_KDIAG("Succeeded in allocating new page");
+#endif
+	} else {
+		success = FALSE;
+#ifdef DIAG_MEM
+		LOG_KDIAG("Failed in allocating new page");
+#endif
+	}
+
+	return success;
+}
+#endif				/* SELF_MANAGED_POOL */
+
+#ifdef SELF_MANAGED_POOL
+/*!
+*******************************************************************************
+* Allocate block from free pool. A block is large enough to fit either a link
+* or descriptor.
+*
+* @brief Allocate memory block
+*
+* @return Mem_Block Free block, NULL if no free blocks available.
+*
+*/
+static Mem_Big_Block *sah_Alloc_Big_Block(void)
+{
+	Mem_Big_Block *block;
+	os_lock_context_t lock_flags;
+
+	os_lock_save_context(mem_lock, lock_flags);
+
+	/* If the pool is empty, try to allocate more entries */
+	if (big_block_free_head == NULL) {
+		(void)sah_Block_Add_Page(1);
+	}
+
+	/* Check that the pool now has some free entries */
+	if (big_block_free_head != NULL) {
+		/* Return the head of the free pool */
+		block = big_block_free_head;
+
+		big_block_free_head = big_block_free_head->next;
+		if (big_block_free_head == NULL) {
+			/* Allocated last entry in pool */
+			big_block_free_tail = NULL;
+		}
+	} else {
+		block = NULL;
+	}
+	os_unlock_restore_context(mem_lock, lock_flags);
+
+	return block;
+}
+#else
+/*!
+*******************************************************************************
+* Allocate block from free pool. A block is large enough to fit either a link
+* or descriptor.
+*
+* @brief Allocate memory block
+*
+* @return Mem_Block Free block, NULL if no free blocks available.
+*
+*/
+static Mem_Big_Block *sah_Alloc_Big_Block(void)
+{
+	dma_addr_t dma_addr;
+	Mem_Big_Block *block =
+	    dma_pool_alloc(big_dma_pool, GFP_ATOMIC, &dma_addr);
+
+	if (block == NULL) {
+	} else {
+		block->dma_addr = dma_addr;
+	}
+
+	return block;
+}
+#endif
+
+#ifdef SELF_MANAGED_POOL
+/*!
+*******************************************************************************
+* Allocate block from free pool. A block is large enough to fit either a link
+* or descriptor.
+*
+* @brief Allocate memory block
+*
+* @return Mem_Block Free block, NULL if no free blocks available.
+*
+*/
+/******************************************************************************
+*
+* MODIFICATION HISTORY:
+*
+* Date         Person     Change
+* 31/10/2003   RWK        PR52734 - Implement functions to allocate
+*                                   descriptors and links. Replace
+*                                   consistent_alloc() calls. Initial creation.
+*
+******************************************************************************/
+static Mem_Block *sah_Alloc_Block(void)
+{
+	Mem_Block *block;
+	os_lock_context_t lock_flags;
+
+	os_lock_save_context(mem_lock, lock_flags);
+
+	/* If the pool is empty, try to allocate more entries */
+	if (block_free_head == NULL) {
+		(void)sah_Block_Add_Page(0);
+	}
+
+	/* Check that the pool now has some free entries */
+	if (block_free_head != NULL) {
+		/* Return the head of the free pool */
+		block = block_free_head;
+
+		block_free_head = block_free_head->next;
+		if (block_free_head == NULL) {
+			/* Allocated last entry in pool */
+			block_free_tail = NULL;
+		}
+	} else {
+		block = NULL;
+	}
+	os_unlock_restore_context(mem_lock, lock_flags);
+
+	return block;
+}
+#else
+/*!
+*******************************************************************************
+* Allocate block from free pool. A block is large enough to fit either a link
+* or descriptor.
+*
+* @brief Allocate memory block
+*
+* @return Mem_Block Free block, NULL if no free blocks available.
+*
+*/
+/******************************************************************************
+*
+* MODIFICATION HISTORY:
+*
+* Date         Person     Change
+* 31/10/2003   RWK        PR52734 - Implement functions to allocate
+*                                   descriptors and links. Replace
+*                                   consistent_alloc() calls. Initial creation.
+*
+******************************************************************************/
+static Mem_Block *sah_Alloc_Block(void)
+{
+
+	dma_addr_t dma_addr;
+	Mem_Block *block =
+	    dma_pool_alloc(small_dma_pool, GFP_ATOMIC, &dma_addr);
+
+	if (block == NULL) {
+	} else {
+		block->dma_addr = dma_addr;
+	}
+
+	return block;
+}
+#endif
+
+#ifdef SELF_MANAGED_POOL
+/*!
+*******************************************************************************
+* Free memory block back to free pool
+*
+* @brief Free memory block
+*
+* @param    block       A block allocated with sah_Alloc_Block().
+*
+* @return none
+*
+*/
+static void sah_Free_Block(Mem_Block * block)
+{
+	os_lock_context_t lock_flags;
+
+	os_lock_save_context(mem_lock, lock_flags);
+	sah_Append_Block(block);
+	os_unlock_restore_context(mem_lock, lock_flags);
+}
+#else
+/*!
+*******************************************************************************
+* Free memory block back to free pool
+*
+* @brief Free memory block
+*
+* @param    block       A block allocated with sah_Alloc_Block().
+*
+* @return none
+*
+*/
+static void sah_Free_Block(Mem_Block * block)
+{
+	dma_pool_free(small_dma_pool, block, block->dma_addr);
+}
+#endif
+
+#ifdef SELF_MANAGED_POOL
+/*!
+*******************************************************************************
+* Free memory block back to free pool
+*
+* @brief Free memory block
+*
+* @param    block       A block allocated with sah_Alloc_Block().
+*
+* @return none
+*
+*/
+static void sah_Free_Big_Block(Mem_Big_Block * block)
+{
+	os_lock_context_t lock_flags;
+
+	os_lock_save_context(mem_lock, lock_flags);
+	sah_Append_Big_Block(block);
+	os_unlock_restore_context(mem_lock, lock_flags);
+}
+#else
+/*!
+*******************************************************************************
+* Free memory block back to free pool
+*
+* @brief Free memory block
+*
+* @param    block       A block allocated with sah_Alloc_Block().
+*
+* @return none
+*
+*/
+static void sah_Free_Big_Block(Mem_Big_Block * block)
+{
+	dma_pool_free(big_dma_pool, block, block->dma_addr);
+}
+#endif
+
+#ifdef SELF_MANAGED_POOL
+/*!
+*******************************************************************************
+* Append memory block to end of free pool.
+*
+* @param    block       A block entry
+*
+* @return none
+*
+* @pre This function must be called with the #mem_lock held.
+*
+* @brief Append memory block to free pool
+*/
+static void sah_Append_Big_Block(Mem_Big_Block * block)
+{
+
+	/* Initialise block */
+	block->next = NULL;
+
+	/* Remember that block may be the first in the pool */
+	if (big_block_free_tail != NULL) {
+		big_block_free_tail->next = block;
+	} else {
+		/* Pool is empty */
+		big_block_free_head = block;
+	}
+
+	big_block_free_tail = block;
+}
+
+/*!
+*******************************************************************************
+* Append memory block to end of free pool.
+*
+* @brief Append memory block to free pool
+*
+* @param    block       A block entry
+*
+* @return none
+*
+* @pre #mem_lock must be held
+*
+*/
+static void sah_Append_Block(Mem_Block * block)
+{
+
+	/* Initialise block */
+	block->next = NULL;
+
+	/* Remember that block may be the first in the pool */
+	if (block_free_tail != NULL) {
+		block_free_tail->next = block;
+	} else {
+		/* Pool is empty */
+		block_free_head = block;
+	}
+
+	block_free_tail = block;
+}
+#endif				/* SELF_MANAGED_POOL */
+
+/* End of sah_memory_mapper.c */
diff --git a/drivers/mxc/security/sahara2/sah_queue.c b/drivers/mxc/security/sahara2/sah_queue.c
new file mode 100644
index 000000000000..0f3e56e4c254
--- /dev/null
+++ b/drivers/mxc/security/sahara2/sah_queue.c
@@ -0,0 +1,249 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+* @file sah_queue.c
+*
+* @brief This file provides a FIFO Queue implementation.
+*
+*/
+/******************************************************************************
+*
+* CAUTION:
+*******************************************************************
+*/
+
+/* SAHARA Includes */
+#include <sah_queue_manager.h>
+#ifdef DIAG_DRV_QUEUE
+#include <diagnostic.h>
+#endif
+
+/******************************************************************************
+* Queue Functions
+******************************************************************************/
+
+/*!
+*******************************************************************************
+* This function constructs a new sah_Queue.
+*
+* @brief     sah_Queue Constructor
+*
+* @return   A pointer to a newly allocated sah_Queue.
+* @return   NULL if allocation of memory failed.
+*/
+/******************************************************************************
+*
+* CAUTION:  This function may sleep in low-memory situations, as it uses
+*           kmalloc ( ..., GFP_KERNEL).
+******************************************************************************/
+sah_Queue *sah_Queue_Construct(void)
+{
+	sah_Queue *q = (sah_Queue *) os_alloc_memory(sizeof(sah_Queue),
+						     GFP_KERNEL);
+
+	if (q != NULL) {
+		/* Initialise the queue to an empty state. */
+		q->head = NULL;
+		q->tail = NULL;
+		q->count = 0;
+	}
+#ifdef DIAG_DRV_QUEUE
+	else {
+		LOG_KDIAG("kmalloc() failed.");
+	}
+#endif
+
+	return q;
+}
+
+/*!
+*******************************************************************************
+* This function destroys a sah_Queue.
+*
+* @brief     sah_Queue Destructor
+*
+* @param    q    A pointer to a sah_Queue.
+*
+* @return   void
+*/
+/******************************************************************************
+*
+* CAUTION:  This function does not free any queue entries.
+*
+******************************************************************************/
+void sah_Queue_Destroy(sah_Queue * q)
+{
+#ifdef DIAG_DRV_QUEUE
+	if (q == NULL) {
+		LOG_KDIAG("Trying to kfree() a NULL pointer.");
+	} else {
+		if (q->count != 0) {
+			LOG_KDIAG
+			    ("Trying to destroy a queue that is not empty.");
+		}
+	}
+#endif
+
+	if (q != NULL) {
+		os_free_memory(q);
+		q = NULL;
+	}
+}
+
+/*!
+*******************************************************************************
+* This function appends a sah_Head_Desc to the tail of a sah_Queue.
+*
+* @brief     Appends a sah_Head_Desc to a sah_Queue.
+*
+* @param    q       A pointer to a sah_Queue to append to.
+* @param    entry   A pointer to a sah_Head_Desc to append.
+*
+* @pre   The #desc_queue_lock must be held before calling this function.
+*
+* @return   void
+*/
+/******************************************************************************
+*
+* CAUTION: NONE
+******************************************************************************/
+void sah_Queue_Append_Entry(sah_Queue * q, sah_Head_Desc * entry)
+{
+	sah_Head_Desc *tail_entry = NULL;
+
+	if ((q == NULL) || (entry == NULL)) {
+#ifdef DIAG_DRV_QUEUE
+		LOG_KDIAG("Null pointer input.");
+#endif
+		return;
+	}
+
+	if (q->count == 0) {
+		/* The queue is empty */
+		q->head = entry;
+		q->tail = entry;
+		entry->next = NULL;
+		entry->prev = NULL;
+	} else {
+		/* The queue is not empty */
+		tail_entry = q->tail;
+		tail_entry->next = entry;
+		entry->next = NULL;
+		entry->prev = tail_entry;
+		q->tail = entry;
+	}
+	q->count++;
+}
+
+/*!
+*******************************************************************************
+* This function a removes a sah_Head_Desc from the head of a sah_Queue.
+*
+* @brief     Removes a sah_Head_Desc from a the head of a sah_Queue.
+*
+* @param    q    A pointer to a sah_Queue to remove from.
+*
+* @pre   The #desc_queue_lock must be held before calling this function.
+*
+* @return   void
+*/
+/******************************************************************************
+*
+* CAUTION:  This does not kfree() the entry.
+******************************************************************************/
+void sah_Queue_Remove_Entry(sah_Queue * q)
+{
+	sah_Queue_Remove_Any_Entry(q, q->head);
+}
+
+/*!
+*******************************************************************************
+* This function a removes a sah_Head_Desc from anywhere in a sah_Queue.
+*
+* @brief     Removes a sah_Head_Desc from anywhere in a sah_Queue.
+*
+* @param    qq      A pointer to a sah_Queue to remove from.
+* @param    entry   A pointer to a sah_Head_Desc to remove.
+*
+* @pre   The #desc_queue_lock must be held before calling this function.
+*
+* @return   void
+*/
+/******************************************************************************
+*
+* CAUTION:  This does not kfree() the entry. Does not check to see if the entry
+*           actually belongs to the queue.
+******************************************************************************/
+void sah_Queue_Remove_Any_Entry(sah_Queue * q, sah_Head_Desc * entry)
+{
+	sah_Head_Desc *prev_entry = NULL;
+	sah_Head_Desc *next_entry = NULL;
+
+	if ((q == NULL) || (entry == NULL)) {
+#if defined DIAG_DRV_QUEUE && defined DIAG_DURING_INTERRUPT
+		LOG_KDIAG("Null pointer input.");
+#endif
+		return;
+	}
+
+	if (q->count == 1) {
+		/* If q is the only entry in the queue. */
+		q->tail = NULL;
+		q->head = NULL;
+		q->count = 0;
+	} else if (q->count > 1) {
+		/* There are 2 or more entries in the queue. */
+
+#if defined DIAG_DRV_QUEUE && defined DIAG_DURING_INTERRUPT
+		if ((entry->next == NULL) && (entry->prev == NULL)) {
+			LOG_KDIAG
+			    ("Queue is not empty yet both next and prev pointers"
+			     " are NULL");
+		}
+#endif
+
+		if (entry->next == NULL) {
+			/* If this is the end of the queue */
+			prev_entry = entry->prev;
+			prev_entry->next = NULL;
+			q->tail = prev_entry;
+		} else if (entry->prev == NULL) {
+			/* If this is the head of the queue */
+			next_entry = entry->next;
+			next_entry->prev = NULL;
+			q->head = next_entry;
+		} else {
+			/* If this is somewhere in the middle of the queue */
+			prev_entry = entry->prev;
+			next_entry = entry->next;
+			prev_entry->next = next_entry;
+			next_entry->prev = prev_entry;
+		}
+		q->count--;
+	}
+	/*
+	 * Otherwise we are removing an entry from an empty queue.
+	 * Don't do anything in the product code
+	 */
+#if defined DIAG_DRV_QUEUE && defined DIAG_DURING_INTERRUPT
+	else {
+		LOG_KDIAG("Trying to remove an entry from an empty queue.");
+	}
+#endif
+
+	entry->next = NULL;
+	entry->prev = NULL;
+}
+
+/* End of sah_queue.c */
diff --git a/drivers/mxc/security/sahara2/sah_queue_manager.c b/drivers/mxc/security/sahara2/sah_queue_manager.c
new file mode 100644
index 000000000000..1602c7043a13
--- /dev/null
+++ b/drivers/mxc/security/sahara2/sah_queue_manager.c
@@ -0,0 +1,1050 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file sah_queue_manager.c
+ *
+ * @brief This file provides a Queue Manager implementation.
+ *
+ * The Queue Manager manages additions and removal from the queue and updates
+ * the status of queue entries. It also calls sah_HW_* functions to interract
+ * with the hardware.
+*/
+
+#include "portable_os.h"
+
+/* SAHARA Includes */
+#include <sah_driver_common.h>
+#include <sah_queue_manager.h>
+#include <sah_status_manager.h>
+#include <sah_hardware_interface.h>
+#if defined(DIAG_DRV_QUEUE) || defined(DIAG_DRV_STATUS)
+#include <diagnostic.h>
+#endif
+#include <sah_memory_mapper.h>
+
+#ifdef DIAG_DRV_STATUS
+
+#define FSL_INVALID_RETURN 13
+#define MAX_RETURN_STRING_LEN 22
+#endif
+
+/* Defines for parsing value from Error Status register */
+#define SAH_STATUS_MASK 0x07
+#define SAH_ERROR_MASK 0x0F
+#define SAH_CHA_ERR_SOURCE_MASK 0x07
+#define SAH_CHA_ERR_STATUS_MASK 0x0FFF
+#define SAH_DMA_ERR_STATUS_MASK 0x0F
+#define SAH_DMA_ERR_SIZE_MASK 0x03
+#define SAH_DMA_ERR_DIR_MASK 0x01
+
+#define SHA_ERROR_STATUS_CONTINUE 0xFFFFFFFF
+#define SHA_CHA_ERROR_STATUS_DONE 0xFFFFFFFF
+
+/* this maps the error status register's error source 4 bit field to the API
+ * return values. A 0xFFFFFFFF indicates additional fields must be checked to
+ * determine an appropriate return value */
+static sah_Execute_Error sah_Execute_Error_Array[] = {
+	FSL_RETURN_ERROR_S,	/* SAH_ERR_NONE */
+	FSL_RETURN_BAD_FLAG_S,	/* SAH_ERR_HEADER */
+	FSL_RETURN_BAD_DATA_LENGTH_S,	/* SAH_ERR_DESC_LENGTH */
+	FSL_RETURN_BAD_DATA_LENGTH_S,	/* SAH_ERR_DESC_POINTER */
+	FSL_RETURN_BAD_DATA_LENGTH_S,	/* SAH_ERR_LINK_LENGTH */
+	FSL_RETURN_BAD_DATA_LENGTH_S,	/* SAH_ERR_LINK_POINTER */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_ERR_INPUT_BUFFER */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_ERR_OUTPUT_BUFFER */
+	FSL_RETURN_BAD_DATA_LENGTH_S,	/* SAH_ERR_OUTPUT_BUFFER_STARVATION */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_ERR_INTERNAL_STATE */
+	FSL_RETURN_ERROR_S,	/* SAH_ERR_GENERAL_DESCRIPTOR */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_ERR_RESERVED_FIELDS */
+	FSL_RETURN_MEMORY_ERROR_S,	/* SAH_ERR_DESCRIPTOR_ADDRESS */
+	FSL_RETURN_MEMORY_ERROR_S,	/* SAH_ERR_LINK_ADDRESS */
+	SHA_ERROR_STATUS_CONTINUE,	/* SAH_ERR_CHA */
+	SHA_ERROR_STATUS_CONTINUE	/* SAH_ERR_DMA */
+};
+
+static sah_DMA_Error_Status sah_DMA_Error_Status_Array[] = {
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_DMA_NO_ERR */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_DMA_AHB_ERR */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_DMA_IP_ERR */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_DMA_PARITY_ERR */
+	FSL_RETURN_BAD_DATA_LENGTH_S,	/* SAH_DMA_BOUNDRY_ERR */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_DMA_BUSY_ERR */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_DMA_RESERVED_ERR */
+	FSL_RETURN_INTERNAL_ERROR_S	/* SAH_DMA_INT_ERR */
+};
+
+static sah_CHA_Error_Status sah_CHA_Error_Status_Array[] = {
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_CHA_NO_ERR */
+	FSL_RETURN_BAD_DATA_LENGTH_S,	/* SAH_CHA_IP_BUF */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_CHA_ADD_ERR */
+	FSL_RETURN_BAD_MODE_S,	/* SAH_CHA_MODE_ERR */
+	FSL_RETURN_BAD_DATA_LENGTH_S,	/* SAH_CHA_DATA_SIZE_ERR */
+	FSL_RETURN_BAD_KEY_LENGTH_S,	/* SAH_CHA_KEY_SIZE_ERR */
+	FSL_RETURN_BAD_MODE_S,	/* SAH_CHA_PROC_ERR */
+	FSL_RETURN_ERROR_S,	/* SAH_CHA_CTX_READ_ERR */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_CHA_INTERNAL_HW_ERR */
+	FSL_RETURN_MEMORY_ERROR_S,	/* SAH_CHA_IP_BUFF_ERR */
+	FSL_RETURN_MEMORY_ERROR_S,	/* SAH_CHA_OP_BUFF_ERR */
+	FSL_RETURN_BAD_KEY_PARITY_S,	/* SAH_CHA_DES_KEY_ERR */
+	FSL_RETURN_INTERNAL_ERROR_S,	/* SAH_CHA_RES */
+};
+
+#ifdef DIAG_DRV_STATUS
+
+char sah_return_text[FSL_INVALID_RETURN][MAX_RETURN_STRING_LEN] = {
+	"No error",		/* FSL_RETURN_OK_S */
+	"Error",		/* FSL_RETURN_ERROR_S */
+	"No resource",		/* FSL_RETURN_NO_RESOURCE_S */
+	"Bad algorithm",	/* FSL_RETURN_BAD_ALGORITHM_S */
+	"Bad mode",		/* FSL_RETURN_BAD_MODE_S */
+	"Bad flag",		/* FSL_RETURN_BAD_FLAG_S */
+	"Bad key length",	/* FSL_RETURN_BAD_KEY_LENGTH_S */
+	"Bad key parity",	/* FSL_RETURN_BAD_KEY_PARITY_S */
+	"Bad data length",	/* FSL_RETURN_BAD_DATA_LENGTH_S */
+	"Authentication failed",	/* FSL_RETURN_AUTH_FAILED_S */
+	"Memory error",		/* FSL_RETURN_MEMORY_ERROR_S */
+	"Internal error",	/* FSL_RETURN_INTERNAL_ERROR_S */
+	"unknown value",	/* default */
+};
+
+#endif				/* DIAG_DRV_STATUS */
+
+/*!
+ * This lock must be held while performing any queuing or unqueuing functions,
+ * including reading the first pointer on the queue.  It also protects reading
+ * and writing the Sahara DAR register.  It must be held during a read-write
+ * operation on the DAR so that the 'test-and-set' is atomic.
+ */
+os_lock_t desc_queue_lock;
+
+/*! This is the main queue for the driver. This is shared between all threads
+ * and is not protected by mutexes since the kernel is non-preemptable. */
+sah_Queue *main_queue = NULL;
+
+/* Internal Prototypes */
+sah_Head_Desc *sah_Find_With_State(sah_Queue_Status state);
+
+#ifdef DIAG_DRV_STATUS
+void sah_Log_Error(uint32_t descriptor, uint32_t error, uint32_t fault_address);
+#endif
+
+extern wait_queue_head_t *int_queue;
+
+/*!
+ * This function initialises the Queue Manager
+ *
+ * @brief     Initialise the Queue Manager
+ *
+ * @return   FSL_RETURN_OK_S on success; FSL_RETURN_MEMORY_ERROR_S if not
+ */
+fsl_shw_return_t sah_Queue_Manager_Init(void)
+{
+	fsl_shw_return_t ret_val = FSL_RETURN_OK_S;
+
+	desc_queue_lock = os_lock_alloc_init();
+
+	if (main_queue == NULL) {
+		/* Construct the main queue. */
+		main_queue = sah_Queue_Construct();
+
+		if (main_queue == NULL) {
+			ret_val = FSL_RETURN_MEMORY_ERROR_S;
+		}
+	} else {
+#ifdef DIAG_DRV_QUEUE
+		LOG_KDIAG
+		    ("Trying to initialise the queue manager more than once.");
+#endif
+	}
+
+	return ret_val;
+}
+
+/*!
+ * This function closes the Queue Manager
+ *
+ * @brief     Close the Queue Manager
+ *
+ * @return   void
+ */
+void sah_Queue_Manager_Close(void)
+{
+#ifdef DIAG_DRV_QUEUE
+	if (main_queue && main_queue->count != 0) {
+		LOG_KDIAG
+		    ("Trying to close the main queue when it is not empty.");
+	}
+#endif
+
+	if (main_queue) {
+		/* There is no error checking here because there is no way to handle
+		   it. */
+		sah_Queue_Destroy(main_queue);
+		main_queue = NULL;
+	}
+}
+
+/*!
+ * Count the number of entries on the Queue Manager's queue
+ *
+ * @param ignore_state  If non-zero, the @a state parameter is ignored.
+ *                      If zero, only entries matching @a state are counted.
+ * @param state         State of entry to match for counting.
+ *
+ * @return        Number of entries which matched criteria
+ */
+int sah_Queue_Manager_Count_Entries(int ignore_state, sah_Queue_Status state)
+{
+	int count = 0;
+	sah_Head_Desc *current_entry;
+
+	/* Start at the head */
+	current_entry = main_queue->head;
+	while (current_entry != NULL) {
+		if (ignore_state || (current_entry->status == state)) {
+			count++;
+		}
+		/* Jump to the next entry. */
+		current_entry = current_entry->next;
+	}
+
+	return count;
+}
+
+/*!
+ * This function removes an entry from the Queue Manager's queue. The entry to
+ * be removed can be anywhere in the queue.
+ *
+ * @brief     Remove an entry from the Queue Manager's queue.
+ *
+ * @param    entry   A pointer to a sah_Head_Desc to remove from the Queue
+ *                   Manager's queue.
+ *
+ * @pre   The #desc_queue_lock must be held before calling this function.
+ *
+ * @return   void
+ */
+void sah_Queue_Manager_Remove_Entry(sah_Head_Desc * entry)
+{
+	if (entry == NULL) {
+#ifdef DIAG_DRV_QUEUE
+		LOG_KDIAG("NULL pointer input.");
+#endif
+	} else {
+		sah_Queue_Remove_Any_Entry(main_queue, entry);
+	}
+}
+
+/*!
+ * This function appends an entry to the Queue Managers queue. It primes SAHARA
+ * if this entry is the first PENDING entry in the Queue Manager's Queue.
+ *
+ * @brief     Appends an entry to the Queue Manager's queue.
+ *
+ * @param    entry   A pointer to a sah_Head_Desc to append to the Queue
+ *                   Manager's queue.
+ *
+ * @pre   The #desc_queue_lock may not may be held when calling this function.
+ *
+ * @return   void
+ */
+void sah_Queue_Manager_Append_Entry(sah_Head_Desc * entry)
+{
+	sah_Head_Desc *current_entry;
+	os_lock_context_t int_flags;
+
+#ifdef DIAG_DRV_QUEUE
+	if (entry == NULL) {
+		LOG_KDIAG("NULL pointer input.");
+	}
+#endif
+	entry->status = SAH_STATE_PENDING;
+	os_lock_save_context(desc_queue_lock, int_flags);
+	sah_Queue_Append_Entry(main_queue, entry);
+
+	/* Prime SAHARA if the operation that was just appended is the only PENDING
+	 * operation in the queue.
+	 */
+	current_entry = sah_Find_With_State(SAH_STATE_PENDING);
+	if (current_entry != NULL) {
+		if (current_entry == entry) {
+			sah_Queue_Manager_Prime(entry);
+		}
+	}
+
+	os_unlock_restore_context(desc_queue_lock, int_flags);
+}
+
+/*!
+ * This function marks all entries in the Queue Manager's queue with state
+ * SAH_STATE_RESET.
+ *
+ * @brief     Mark all entries with state SAH_STATE_RESET
+ *
+ * @return   void
+ *
+ * @note This feature needs re-visiting
+ */
+void sah_Queue_Manager_Reset_Entries(void)
+{
+	sah_Head_Desc *current_entry = NULL;
+
+	/* Start at the head */
+	current_entry = main_queue->head;
+
+	while (current_entry != NULL) {
+		/* Set the state. */
+		current_entry->status = SAH_STATE_RESET;
+		/* Jump to the next entry. */
+		current_entry = current_entry->next;
+	}
+}
+
+/*!
+ * This function primes SAHARA for the first time or after the queue becomes
+ * empty. Queue lock must have been set by the caller of this routine.
+ *
+ * @brief    Prime SAHARA.
+ *
+ * @param    entry   A pointer to a sah_Head_Desc to Prime SAHARA with.
+ *
+ * @return   void
+ */
+void sah_Queue_Manager_Prime(sah_Head_Desc * entry)
+{
+#ifdef DIAG_DRV_QUEUE
+	LOG_KDIAG("Priming SAHARA");
+	if (entry == NULL) {
+		LOG_KDIAG("Trying to prime SAHARA with a NULL entry pointer.");
+	}
+#endif
+
+#ifndef SUBMIT_MULTIPLE_DARS
+	/* BUG FIX: state machine can transition from Done1 Busy2 directly
+	 * to Idle. To fix that problem, only one DAR is being allowed on
+	 * SAHARA at a time */
+	if (sah_Find_With_State(SAH_STATE_ON_SAHARA) != NULL) {
+		return;
+	}
+#endif
+
+#ifdef SAHARA_POWER_MANAGEMENT
+	/* check that dynamic power management is not asserted */
+    if (!sah_dpm_flag) {
+#endif
+
+	/* Enable the SAHARA Clocks */
+#ifdef DIAG_DRV_IF
+			LOG_KDIAG("SAHARA : Enabling the IPG and AHB clocks\n")
+#endif				/*DIAG_DRV_IF */
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+		mxc_clks_enable(SAHARA2_CLK);
+#else
+		{
+			struct clk *clk = clk_get(NULL, "sahara_clk");
+			if (clk != ERR_PTR(ENOENT))
+				clk_enable(clk);
+			clk_put(clk);
+		}
+#endif
+
+		/* Make sure nothing is in the DAR */
+		if (sah_HW_Read_DAR() == 0) {
+#if defined(DIAG_DRV_IF)
+			sah_Dump_Chain(&entry->desc, entry->desc.dma_addr);
+#endif				/* DIAG_DRV_IF */
+
+			sah_HW_Write_DAR((entry->desc.dma_addr));
+			entry->status = SAH_STATE_ON_SAHARA;
+		}
+#ifdef DIAG_DRV_QUEUE
+		else {
+			LOG_KDIAG("DAR should be empty when Priming SAHARA");
+		}
+#endif
+#ifdef SAHARA_POWER_MANAGEMENT
+	}
+#endif
+}
+
+#ifndef SAHARA_POLL_MODE
+
+/*!
+ * Reset SAHARA, then load the next descriptor on it, if one exists
+ */
+void sah_reset_sahara_request(void)
+{
+	sah_Head_Desc *desc;
+	os_lock_context_t lock_flags;
+
+#ifdef DIAG_DRV_STATUS
+	LOG_KDIAG("Sahara required reset from tasklet, replace chip");
+#endif
+	sah_HW_Reset();
+
+	/* Now stick in a waiting request */
+	os_lock_save_context(desc_queue_lock, lock_flags);
+	if ((desc = sah_Find_With_State(SAH_STATE_PENDING))) {
+		sah_Queue_Manager_Prime(desc);
+	}
+	os_unlock_restore_context(desc_queue_lock, lock_flags);
+}
+
+/*!
+ * Post-process a descriptor chain after the hardware has finished with it.
+ *
+ * The status of the descriptor could also be checked.  (for FATAL or IGNORED).
+ *
+ * @param     desc_head   The finished chain
+ * @param     error       A boolean to mark whether hardware reported error
+ *
+ * @pre   The #desc_queue_lock may not be held when calling this function.
+ */
+void sah_process_finished_request(sah_Head_Desc * desc_head, unsigned error)
+{
+	os_lock_context_t lock_flags;
+
+	if (!error) {
+		desc_head->result = FSL_RETURN_OK_S;
+	} else if (desc_head->error_status == -1) {
+		/* Disaster!  Sahara has faulted */
+		desc_head->result = FSL_RETURN_ERROR_S;
+	} else {
+		/* translate from SAHARA error status to fsl_shw return values */
+		desc_head->result =
+		    sah_convert_error_status(desc_head->error_status);
+#ifdef DIAG_DRV_STATUS
+		sah_Log_Error(desc_head->current_dar, desc_head->error_status,
+			      desc_head->fault_address);
+#endif
+	}
+
+	/* Show that the request has been processd */
+	desc_head->status = error ? SAH_STATE_FAILED : SAH_STATE_COMPLETE;
+
+	if (desc_head->uco_flags & FSL_UCO_BLOCKING_MODE) {
+
+		/* Wake up all processes on Sahara queue */
+		wake_up_interruptible(int_queue);
+
+	} else {
+		os_lock_save_context(desc_queue_lock, lock_flags);
+		sah_Queue_Append_Entry(&desc_head->user_info->result_pool,
+				       desc_head);
+		os_unlock_restore_context(desc_queue_lock, lock_flags);
+
+		/* perform callback */
+		if (desc_head->uco_flags & FSL_UCO_CALLBACK_MODE) {
+			desc_head->user_info->callback(desc_head->user_info);
+		}
+	}
+}				/* sah_process_finished_request */
+
+/*! Called from bottom half.
+ *
+ * @pre   The #desc_queue_lock may not be held when calling this function.
+ */
+void sah_postprocess_queue(unsigned long reset_flag)
+{
+
+	/* if SAHARA needs to be reset, do it here. This starts a descriptor chain
+	 * if one is ready also */
+	if (reset_flag) {
+		sah_reset_sahara_request();
+	}
+
+	/* now handle the descriptor chain(s) that has/have completed */
+	do {
+		sah_Head_Desc *first_entry;
+		os_lock_context_t lock_flags;
+
+		os_lock_save_context(desc_queue_lock, lock_flags);
+
+		first_entry = main_queue->head;
+		if ((first_entry != NULL) &&
+		    (first_entry->status == SAH_STATE_OFF_SAHARA)) {
+			sah_Queue_Remove_Entry(main_queue);
+			os_unlock_restore_context(desc_queue_lock, lock_flags);
+
+			sah_process_finished_request(first_entry,
+						     (first_entry->
+						      error_status != 0));
+		} else {
+			os_unlock_restore_context(desc_queue_lock, lock_flags);
+			break;
+		}
+	} while (1);
+
+	return;
+}
+
+#endif				/* ifndef SAHARA_POLL_MODE */
+
+/*!
+ * This is a helper function for Queue Manager. This function finds the first
+ * entry in the Queue Manager's queue whose state matches the given input
+ * state. This function starts at the head of the queue and works towards the
+ * tail. If a matching entry was found, the address of the entry is returned.
+ *
+ * @brief     Handle the IDLE state.
+ *
+ * @param    state   A sah_Queue_Status value.
+ *
+ * @pre   The #desc_queue_lock must be held before calling this function.
+ *
+ * @return   A pointer to a sah_Head_Desc that matches the given state.
+ * @return   NULL otherwise.
+ */
+sah_Head_Desc *sah_Find_With_State(sah_Queue_Status state)
+{
+	sah_Head_Desc *current_entry = NULL;
+	sah_Head_Desc *ret_val = NULL;
+	int done_looping = FALSE;
+
+	/* Start at the head */
+	current_entry = main_queue->head;
+
+	while ((current_entry != NULL) && (done_looping == FALSE)) {
+		if (current_entry->status == state) {
+			done_looping = TRUE;
+			ret_val = current_entry;
+		}
+		/* Jump to the next entry. */
+		current_entry = current_entry->next;
+	}
+
+	return ret_val;
+}				/* sah_postprocess_queue */
+
+/*!
+ * Process the value from the Sahara error status register and convert it into
+ * an FSL SHW API error code.
+ *
+ * Warning, this routine must only be called if an error exists.
+ *
+ * @param error_status   The value from the error status register.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_convert_error_status(uint32_t error_status)
+{
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;	/* catchall */
+	uint8_t error_source;
+	uint8_t DMA_error_status;
+	uint8_t DMA_error_size;
+
+	/* get the error source from the error status register */
+	error_source = error_status & SAH_ERROR_MASK;
+
+	/* array size is maximum allowed by mask, so no boundary checking is
+	 * needed here */
+	ret = sah_Execute_Error_Array[error_source];
+
+	/* is this one that needs additional fields checked to determine the
+	 * error condition? */
+	if (ret == SHA_ERROR_STATUS_CONTINUE) {
+		/* check the DMA fields */
+		if (error_source == SAH_ERR_DMA) {
+			/* get the DMA transfer error size. If this indicates that no
+			 * error was detected, something is seriously wrong */
+			DMA_error_size =
+			    (error_status >> 9) & SAH_DMA_ERR_SIZE_MASK;
+			if (DMA_error_size == SAH_DMA_NO_ERR) {
+				ret = FSL_RETURN_INTERNAL_ERROR_S;
+			} else {
+				/* get DMA error status */
+				DMA_error_status = (error_status >> 12) &
+				    SAH_DMA_ERR_STATUS_MASK;
+
+				/* the DMA error bits cover all the even numbers. By dividing
+				 * by 2 it can be used as an index into the error array */
+				ret =
+				    sah_DMA_Error_Status_Array[DMA_error_status
+							       >> 1];
+			}
+		} else {	/* not SAH_ERR_DMA, so must be SAH_ERR_CHA */
+			uint16_t CHA_error_status;
+			uint8_t CHA_error_source;
+
+			/* get CHA Error Source. If this indicates that no error was
+			 * detected, something is seriously wrong */
+			CHA_error_source =
+			    (error_status >> 28) & SAH_CHA_ERR_SOURCE_MASK;
+			if (CHA_error_source == SAH_CHA_NO_ERROR) {
+				ret = FSL_RETURN_INTERNAL_ERROR_S;
+			} else {
+				uint32_t mask = 1;
+				uint32_t count = 0;
+
+				/* get CHA Error Status */
+				CHA_error_status = (error_status >> 16) &
+				    SAH_CHA_ERR_STATUS_MASK;
+
+				/* If more than one bit is set (which shouldn't happen), only
+				 * the first will be captured */
+				if (CHA_error_status != 0) {
+					count = 1;
+					while (CHA_error_status != mask) {
+						++count;
+						mask <<= 1;
+					}
+				}
+
+				ret = sah_CHA_Error_Status_Array[count];
+			}
+		}
+	}
+
+	return ret;
+}
+
+fsl_shw_return_t sah_convert_op_status(uint32_t op_status)
+{
+	unsigned op_source = (op_status >> 28) & 0x7;
+	unsigned op_detail = op_status & 0x3f;
+	fsl_shw_return_t ret = FSL_RETURN_ERROR_S;
+
+	switch (op_source) {
+	case 1:		/* SKHA */
+		/* Can't this have "ICV" error from CCM ?? */
+		break;
+	case 2:		/* MDHA */
+		if (op_detail == 1) {
+			ret = FSL_RETURN_AUTH_FAILED_S;
+		}
+		break;
+	case 3:		/* RNGA */
+		/* Self-test and Compare errors... what to do? */
+		break;
+	case 4:		/* PKHA */
+		switch (op_detail) {
+		case 0x01:
+			ret = FSL_RETURN_PRIME_S;
+			break;
+		case 0x02:
+			ret = FSL_RETURN_NOT_PRIME_S;
+			break;
+		case 0x04:
+			ret = FSL_RETURN_POINT_AT_INFINITY_S;
+			break;
+		case 0x08:
+			ret = FSL_RETURN_POINT_NOT_AT_INFINITY_S;
+			break;
+		case 0x10:
+			ret = FSL_RETURN_GCD_IS_ONE_S;
+			break;
+		case 0x20:
+			ret = FSL_RETURN_GCD_IS_NOT_ONE_S;
+			break;
+		default:
+			break;
+		}
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+#ifdef DIAG_DRV_STATUS
+
+/*!
+ * This function logs the diagnostic information for the given error and
+ * descriptor address.  Only used for diagnostic purposes.
+ *
+ * @brief    (debug only) Log a description of hardware-detected error.
+ *
+ * @param    descriptor    The descriptor address that caused the error
+ * @param    error         The SAHARA error code
+ * @param    fault_address Value from the Fault address register
+ *
+ * @return   void
+ */
+void sah_Log_Error(uint32_t descriptor, uint32_t error, uint32_t fault_address)
+{
+	char *source_text;	/* verbose error source from register */
+	char *address;		/* string buffer for descriptor address */
+	char *error_log;	/* the complete logging message */
+	char *cha_log = NULL;	/* string buffer for descriptor address */
+	char *dma_log = NULL;	/* string buffer for descriptor address */
+
+	uint16_t cha_error = 0;
+	uint16_t dma_error = 0;
+
+	uint8_t error_source;
+	sah_Execute_Error return_code;
+
+	/* log error code and descriptor address */
+	error_source = error & SAH_ERROR_MASK;
+	return_code = sah_Execute_Error_Array[error_source];
+
+	source_text = os_alloc_memory(64, GFP_KERNEL);
+
+	switch (error_source) {
+	case SAH_ERR_HEADER:
+		sprintf(source_text, "%s", "Header is not valid");
+		break;
+
+	case SAH_ERR_DESC_LENGTH:
+		sprintf(source_text, "%s",
+			"Descriptor length not equal to sum of link lengths");
+		break;
+
+	case SAH_ERR_DESC_POINTER:
+		sprintf(source_text, "%s", "Length or pointer "
+			"field is zero while the other is non-zero");
+		break;
+
+	case SAH_ERR_LINK_LENGTH:
+		/* note that the Sahara Block Guide 2.7 has an invalid explaination
+		 * of this. It only happens when a link length is zero */
+		sprintf(source_text, "%s", "A data length is a link is zero");
+		break;
+
+	case SAH_ERR_LINK_POINTER:
+		sprintf(source_text, "%s",
+			"The data pointer in a link is zero");
+		break;
+
+	case SAH_ERR_INPUT_BUFFER:
+		sprintf(source_text, "%s", "Input Buffer reported an overflow");
+		break;
+
+	case SAH_ERR_OUTPUT_BUFFER:
+		sprintf(source_text, "%s",
+			"Output Buffer reported an underflow");
+		break;
+
+	case SAH_ERR_OUTPUT_BUFFER_STARVATION:
+		sprintf(source_text, "%s", "Incorrect data in output "
+			"buffer after CHA has signalled 'done'");
+		break;
+
+	case SAH_ERR_INTERNAL_STATE:
+		sprintf(source_text, "%s", "Internal Hardware Failure");
+		break;
+
+	case SAH_ERR_GENERAL_DESCRIPTOR:
+		sprintf(source_text, "%s",
+			"Current Descriptor was not legal, but cause is unknown");
+		break;
+
+	case SAH_ERR_RESERVED_FIELDS:
+		sprintf(source_text, "%s",
+			"Reserved pointer field is non-zero");
+		break;
+
+	case SAH_ERR_DESCRIPTOR_ADDRESS:
+		sprintf(source_text, "%s",
+			"Descriptor address not word aligned");
+		break;
+
+	case SAH_ERR_LINK_ADDRESS:
+		sprintf(source_text, "%s", "Link address not word aligned");
+		break;
+
+	case SAH_ERR_CHA:
+		sprintf(source_text, "%s", "CHA Error");
+		{
+			char *cha_module = os_alloc_memory(5, GFP_KERNEL);
+			char *cha_text = os_alloc_memory(45, GFP_KERNEL);
+
+			cha_error = (error >> 28) & SAH_CHA_ERR_SOURCE_MASK;
+
+			switch (cha_error) {
+			case SAH_CHA_SKHA_ERROR:
+				sprintf(cha_module, "%s", "SKHA");
+				break;
+
+			case SAH_CHA_MDHA_ERROR:
+				sprintf(cha_module, "%s", "MDHA");
+				break;
+
+			case SAH_CHA_RNG_ERROR:
+				sprintf(cha_module, "%s", "RNG ");
+				break;
+
+			case SAH_CHA_PKHA_ERROR:
+				sprintf(cha_module, "%s", "PKHA");
+				break;
+
+			case SAH_CHA_NO_ERROR:
+				/* can't happen */
+				/* no break */
+			default:
+				sprintf(cha_module, "%s", "????");
+				break;
+			}
+
+			cha_error = (error >> 16) & SAH_CHA_ERR_STATUS_MASK;
+
+			/* Log CHA Error Status */
+			switch (cha_error) {
+			case SAH_CHA_IP_BUF:
+				sprintf(cha_text, "%s",
+					"Non-empty input buffer when done");
+				break;
+
+			case SAH_CHA_ADD_ERR:
+				sprintf(cha_text, "%s", "Illegal address");
+				break;
+
+			case SAH_CHA_MODE_ERR:
+				sprintf(cha_text, "%s", "Illegal mode");
+				break;
+
+			case SAH_CHA_DATA_SIZE_ERR:
+				sprintf(cha_text, "%s", "Illegal data size");
+				break;
+
+			case SAH_CHA_KEY_SIZE_ERR:
+				sprintf(cha_text, "%s", "Illegal key size");
+				break;
+
+			case SAH_CHA_PROC_ERR:
+				sprintf(cha_text, "%s",
+					"Mode/Context/Key written during processing");
+				break;
+
+			case SAH_CHA_CTX_READ_ERR:
+				sprintf(cha_text, "%s",
+					"Context read during processing");
+				break;
+
+			case SAH_CHA_INTERNAL_HW_ERR:
+				sprintf(cha_text, "%s", "Internal hardware");
+				break;
+
+			case SAH_CHA_IP_BUFF_ERR:
+				sprintf(cha_text, "%s",
+					"Input buffer not enabled or underflow");
+				break;
+
+			case SAH_CHA_OP_BUFF_ERR:
+				sprintf(cha_text, "%s",
+					"Output buffer not enabled or overflow");
+				break;
+
+			case SAH_CHA_DES_KEY_ERR:
+				sprintf(cha_text, "%s", "DES key parity error");
+				break;
+
+			case SAH_CHA_RES:
+				sprintf(cha_text, "%s", "Reserved");
+				break;
+
+			case SAH_CHA_NO_ERR:
+				/* can't happen */
+				/* no break */
+			default:
+				sprintf(cha_text, "%s", "Unknown error");
+				break;
+			}
+
+			cha_log = os_alloc_memory(90, GFP_KERNEL);
+			sprintf(cha_log,
+				"  Module %s encountered the error: %s.",
+				cha_module, cha_text);
+
+			os_free_memory(cha_module);
+			os_free_memory(cha_text);
+
+			{
+				uint32_t mask = 1;
+				uint32_t count = 0;
+
+				if (cha_error != 0) {
+					count = 1;
+					while (cha_error != mask) {
+						++count;
+						mask <<= 1;
+					}
+				}
+
+				return_code = sah_CHA_Error_Status_Array[count];
+			}
+			cha_error = 1;
+		}
+		break;
+
+	case SAH_ERR_DMA:
+		sprintf(source_text, "%s", "DMA Error");
+		{
+			char *dma_direction = os_alloc_memory(6, GFP_KERNEL);
+			char *dma_size = os_alloc_memory(14, GFP_KERNEL);
+			char *dma_text = os_alloc_memory(250, GFP_KERNEL);
+
+			if ((dma_direction == NULL) || (dma_size == NULL) ||
+			    (dma_text == NULL)) {
+				LOG_KDIAG
+				    ("No memory allocated for DMA debug messages\n");
+			}
+
+			/* log DMA error direction */
+			sprintf(dma_direction, "%s",
+				(((error >> 8) & SAH_DMA_ERR_DIR_MASK) == 1) ?
+				"read" : "write");
+
+			/* log the size of the DMA transfer error */
+			dma_error = (error >> 9) & SAH_DMA_ERR_SIZE_MASK;
+			switch (dma_error) {
+			case SAH_DMA_SIZE_BYTE:
+				sprintf(dma_size, "%s", "byte");
+				break;
+
+			case SAH_DMA_SIZE_HALF_WORD:
+				sprintf(dma_size, "%s", "half-word");
+				break;
+
+			case SAH_DMA_SIZE_WORD:
+				sprintf(dma_size, "%s", "word");
+				break;
+
+			case SAH_DMA_SIZE_RES:
+				sprintf(dma_size, "%s", "reserved size");
+				break;
+
+			default:
+				sprintf(dma_size, "%s", "unknown size");
+				break;
+			}
+
+			/* log DMA error status */
+			dma_error = (error >> 12) & SAH_DMA_ERR_STATUS_MASK;
+			switch (dma_error) {
+			case SAH_DMA_NO_ERR:
+				sprintf(dma_text, "%s", "No DMA Error Code");
+				break;
+
+			case SAH_DMA_AHB_ERR:
+				sprintf(dma_text, "%s",
+					"AHB terminated a bus cycle with an error");
+				break;
+
+			case SAH_DMA_IP_ERR:
+				sprintf(dma_text, "%s",
+					"Internal IP bus cycle was terminated with an "
+					"error termination. This would likely be "
+					"caused by a descriptor length being too "
+					"large, and thus accessing an illegal "
+					"internal address. Verify the length field "
+					"of the current descriptor");
+				break;
+
+			case SAH_DMA_PARITY_ERR:
+				sprintf(dma_text, "%s",
+					"Parity error detected on DMA command from "
+					"Descriptor Decoder. Cause is likely to be "
+					"internal hardware fault");
+				break;
+
+			case SAH_DMA_BOUNDRY_ERR:
+				sprintf(dma_text, "%s",
+					"DMA was requested to cross a 256 byte "
+					"internal address boundary. Cause is likely a "
+					"descriptor length being too large, thus "
+					"accessing two different internal hardware "
+					"blocks");
+				break;
+
+			case SAH_DMA_BUSY_ERR:
+				sprintf(dma_text, "%s",
+					"Descriptor Decoder has made a DMA request "
+					"while the DMA controller is busy. Cause is "
+					"likely due to hardware fault");
+				break;
+
+			case SAH_DMA_RESERVED_ERR:
+				sprintf(dma_text, "%s", "Reserved");
+				break;
+
+			case SAH_DMA_INT_ERR:
+				sprintf(dma_text, "%s",
+					"Internal DMA hardware error detected. The "
+					"DMA controller has detected an internal "
+					"condition which should never occur");
+				break;
+
+			default:
+				sprintf(dma_text, "%s",
+					"Unknown DMA Error Status Code");
+				break;
+			}
+
+			return_code =
+			    sah_DMA_Error_Status_Array[dma_error >> 1];
+			dma_error = 1;
+
+			dma_log = os_alloc_memory(320, GFP_KERNEL);
+			sprintf(dma_log,
+				"  Occurred during a %s operation of a %s transfer: %s.",
+				dma_direction, dma_size, dma_text);
+
+			os_free_memory(dma_direction);
+			os_free_memory(dma_size);
+			os_free_memory(dma_text);
+		}
+		break;
+
+	case SAH_ERR_NONE:
+	default:
+		sprintf(source_text, "%s", "Unknown Error Code");
+		break;
+	}
+
+	address = os_alloc_memory(35, GFP_KERNEL);
+
+	/* convert error & descriptor address to strings */
+	if (dma_error) {
+		sprintf(address, "Fault address is 0x%08x", fault_address);
+	} else {
+		sprintf(address, "Descriptor bus address is 0x%08x",
+			descriptor);
+	}
+
+	if (return_code > FSL_INVALID_RETURN) {
+		return_code = FSL_INVALID_RETURN;
+	}
+
+	error_log = os_alloc_memory(250, GFP_KERNEL);
+
+	/* construct final log message */
+	sprintf(error_log, "Error source = 0x%08x. Return = %s. %s. %s.",
+		error, sah_return_text[return_code], address, source_text);
+
+	os_free_memory(source_text);
+	os_free_memory(address);
+
+	/* log standard messages */
+	LOG_KDIAG(error_log);
+	os_free_memory(error_log);
+
+	/* add additional information if available */
+	if (cha_error) {
+		LOG_KDIAG(cha_log);
+		os_free_memory(cha_log);
+	}
+
+	if (dma_error) {
+		LOG_KDIAG(dma_log);
+		os_free_memory(dma_log);
+	}
+
+	return;
+}				/* sah_Log_Error */
+
+#endif				/* DIAG_DRV_STATUS */
+
+/* End of sah_queue_manager.c */
diff --git a/drivers/mxc/security/sahara2/sah_status_manager.c b/drivers/mxc/security/sahara2/sah_status_manager.c
new file mode 100644
index 000000000000..d82facb600a6
--- /dev/null
+++ b/drivers/mxc/security/sahara2/sah_status_manager.c
@@ -0,0 +1,734 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+* @file sah_status_manager.c
+*
+* @brief Status Manager Function
+*
+* This file contains the function which processes the Sahara status register
+* during an interrupt.
+*
+* This file does not need porting.
+*/
+
+#include "portable_os.h"
+
+#include <sah_status_manager.h>
+#include <sah_hardware_interface.h>
+#include <sah_queue_manager.h>
+#include <sah_memory_mapper.h>
+#include <sah_kernel.h>
+
+#if defined(DIAG_DRV_INTERRUPT) && defined(DIAG_DURING_INTERRUPT)
+#include <diagnostic.h>
+#endif
+
+/*! Compile-time flag to count various interrupt types. */
+#define DIAG_INT_COUNT
+
+/*!
+ * Number of interrupts processed with Done1Done2 status.  Updates to this
+ * value should only be done in interrupt processing.
+ */
+uint32_t done1_count;
+
+/*!
+ * Number of interrupts processed with Done1Busy2 status.  Updates to this
+ * value should only be done in interrupt processing.
+ */
+uint32_t done1busy2_count;
+
+/*!
+ * Number of interrupts processed with Done1Done2 status.  Updates to this
+ * value should only be done in interrupt processing.
+ */
+uint32_t done1done2_count;
+
+/*!
+ * the dynameic power management flag is false when power management is not
+ * asserted and true when dpm is.
+ */
+#ifdef SAHARA_POWER_MANAGEMENT
+bool sah_dpm_flag = FALSE;
+
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+static int sah_dpm_suspend(struct device *dev, uint32_t state, uint32_t level);
+static int sah_dpm_resume(struct device *dev, uint32_t level);
+#else
+static int sah_dpm_suspend(struct platform_device *dev, pm_message_t state);
+static int sah_dpm_resume(struct platform_device *dev);
+#endif
+#endif
+
+#ifndef SAHARA_POLL_MODE
+/*!
+*******************************************************************************
+* This functionx processes the status register of the Sahara, updates the state
+* of the finished queue entry, and then tries to find more work for Sahara to
+* do.
+*
+* @brief    The bulk of the interrupt handling code.
+*
+* @param   hw_status     The status register of Sahara at time of interrupt.
+*                        The Clear interrupt bit is already handled by this
+*                        register read prior to entry into this function.
+* @return   void
+*/
+unsigned long sah_Handle_Interrupt(sah_Execute_Status hw_status)
+{
+	unsigned long reset_flag = 0;	/* assume no SAHARA reset needed */
+	os_lock_context_t lock_flags;
+	sah_Head_Desc *current_entry;
+
+	/* HW status at time of interrupt */
+	sah_Execute_Status state = hw_status & SAH_EXEC_STATE_MASK;
+
+	do {
+		uint32_t dar;
+
+#ifdef DIAG_INT_COUNT
+		if (state == SAH_EXEC_DONE1) {
+			done1_count++;
+		} else if (state == SAH_EXEC_DONE1_BUSY2) {
+			done1busy2_count++;
+		} else if (state == SAH_EXEC_DONE1_DONE2) {
+			done1done2_count++;
+		}
+#endif
+
+		/* if the first entry on sahara has completed... */
+		if ((state & SAH_EXEC_DONE1_BIT) ||
+		    (state == SAH_EXEC_ERROR1)) {
+			/* lock queue while searching */
+			os_lock_save_context(desc_queue_lock, lock_flags);
+			current_entry =
+			    sah_Find_With_State(SAH_STATE_ON_SAHARA);
+			os_unlock_restore_context(desc_queue_lock, lock_flags);
+
+			/* an active descriptor was not found */
+			if (current_entry == NULL) {
+				/* change state to avoid an infinite loop (possible if
+				 * state is SAH_EXEC_DONE1_BUSY2 first time into loop) */
+				hw_status = SAH_EXEC_IDLE;
+#if defined(DIAG_DRV_INTERRUPT) && defined(DIAG_DURING_INTERRUPT)
+				LOG_KDIAG
+				    ("Interrupt received with nothing on queue.");
+#endif
+			} else {
+				/* SAHARA has completed its work on this descriptor chain */
+				current_entry->status = SAH_STATE_OFF_SAHARA;
+
+				if (state == SAH_EXEC_ERROR1) {
+				   if (hw_status & STATUS_ERROR) {
+					  /* Gather extra diagnostic information */
+					  current_entry->fault_address =
+					    sah_HW_Read_Fault_Address();
+					  /* Read this last - it clears the error */
+					  current_entry->error_status =
+					    sah_HW_Read_Error_Status();
+					  current_entry->op_status = 0;
+#ifdef FSL_HAVE_SAHARA4
+				   } else {
+					  current_entry->op_status =
+							sah_HW_Read_Op_Status();
+					  current_entry->error_status = 0;
+#endif
+				   }
+
+				} else {
+					/* indicate that no errors were found with descriptor
+					 * chain 1 */
+					current_entry->error_status = 0;
+					current_entry->op_status = 0;
+
+					/* is there a second, successfully, completed descriptor
+					 * chain? (done1/error2 processing is handled later) */
+					if (state == SAH_EXEC_DONE1_DONE2) {
+						os_lock_save_context
+						    (desc_queue_lock,
+						     lock_flags);
+						current_entry =
+						    sah_Find_With_State
+						    (SAH_STATE_ON_SAHARA);
+						os_unlock_restore_context
+						    (desc_queue_lock,
+						     lock_flags);
+
+						if (current_entry == NULL) {
+#if defined(DIAG_DRV_INTERRUPT) && defined(DIAG_DURING_INTERRUPT)
+							LOG_KDIAG
+							    ("Done1_Done2 Interrupt received with "
+							     "one entry on queue.");
+#endif
+						} else {
+							/* indicate no errors in descriptor chain 2 */
+							current_entry->
+							    error_status = 0;
+							current_entry->status =
+							    SAH_STATE_OFF_SAHARA;
+						}
+					}
+				}
+			}
+
+#ifdef SAHARA_POWER_MANAGEMENT
+			/* check dynamic power management is not asserted */
+			if (!sah_dpm_flag) {
+#endif
+				do {
+					/* protect DAR and main_queue */
+					os_lock_save_context(desc_queue_lock,
+							     lock_flags);
+					dar = sah_HW_Read_DAR();
+					/* check if SAHARA has space for another descriptor. SAHARA
+					 * only accepts up to the DAR queue size number of DAR
+					 * entries, after that 'dar' will not be zero until the
+					 * pending interrupt is serviced */
+					if (dar == 0) {
+						current_entry =
+						    sah_Find_With_State
+						    (SAH_STATE_PENDING);
+						if (current_entry != NULL) {
+#ifndef SUBMIT_MULTIPLE_DARS
+							/* BUG FIX: state machine can transition from Done1
+							 * Busy2 directly to Idle. To fix that problem,
+							 * only one DAR is being allowed on SAHARA at a
+							 * time.  If a high level interrupt has happened,
+							 * there could * be an active descriptor chain */
+							if (sah_Find_With_State
+							    (SAH_STATE_ON_SAHARA)
+							    == NULL) {
+#endif
+#if defined(DIAG_DRV_IF) && defined(DIAG_DURING_INTERRUPT)
+								sah_Dump_Chain
+								    (&current_entry->
+								     desc,
+								     current_entry->
+								     desc.
+								     dma_addr);
+#endif				/* DIAG_DRV_IF */
+								sah_HW_Write_DAR
+								    (current_entry->
+								     desc.
+								     dma_addr);
+								current_entry->
+								    status =
+								    SAH_STATE_ON_SAHARA;
+#ifndef SUBMIT_MULTIPLE_DARS
+							}
+							current_entry = NULL;	/* exit loop */
+#endif
+						}
+					}
+					os_unlock_restore_context
+					    (desc_queue_lock, lock_flags);
+				} while ((dar == 0) && (current_entry != NULL));
+#ifdef SAHARA_POWER_MANAGEMENT
+			}	/* sah_device_power_manager */
+#endif
+		} else {
+			if (state == SAH_EXEC_FAULT) {
+				sah_Head_Desc *previous_entry;	/* point to chain 1 */
+				/* Address of request when fault occured */
+				uint32_t bad_dar = sah_HW_Read_IDAR();
+
+				reset_flag = 1;	/* SAHARA needs to be reset */
+
+				/* get first of possible two descriptor chain that was
+				 * on SAHARA */
+				os_lock_save_context(desc_queue_lock,
+						     lock_flags);
+				previous_entry =
+				    sah_Find_With_State(SAH_STATE_ON_SAHARA);
+				os_unlock_restore_context(desc_queue_lock,
+							  lock_flags);
+
+				/* if it exists, continue processing the fault */
+				if (previous_entry) {
+					/* assume this chain didn't complete correctly */
+					previous_entry->error_status = -1;
+					previous_entry->status =
+					    SAH_STATE_OFF_SAHARA;
+
+					/* get the second descriptor chain */
+					os_lock_save_context(desc_queue_lock,
+							     lock_flags);
+					current_entry =
+					    sah_Find_With_State
+					    (SAH_STATE_ON_SAHARA);
+					os_unlock_restore_context
+					    (desc_queue_lock, lock_flags);
+
+					/* if it exists, continue processing both chains */
+					if (current_entry) {
+						/* assume this chain didn't complete correctly */
+						current_entry->error_status =
+						    -1;
+						current_entry->status =
+						    SAH_STATE_OFF_SAHARA;
+
+						/* now see if either can be identified as the one
+						 * in progress when the fault occured */
+						if (current_entry->desc.
+						    dma_addr == bad_dar) {
+							/* the second descriptor chain was active when the
+							 * fault occured, so the first descriptor chain
+							 * was successfull */
+							previous_entry->
+							    error_status = 0;
+						} else {
+							if (previous_entry->
+							    desc.dma_addr ==
+							    bad_dar) {
+								/* if the first chain was in progress when the
+								 * fault occured, the second has not yet been
+								 * touched, so reset it to PENDING */
+								current_entry->
+								    status =
+								    SAH_STATE_PENDING;
+							}
+						}
+					}
+				}
+#if defined(DIAG_DRV_INTERRUPT) && defined(DIAG_DURING_INTERRUPT)
+			} else {
+				/* shouldn't ever get here */
+				if (state == SAH_EXEC_BUSY) {
+					LOG_KDIAG
+					    ("Got Sahara interrupt in Busy state");
+				} else {
+					if (state == SAH_EXEC_IDLE) {
+						LOG_KDIAG
+						    ("Got Sahara interrupt in Idle state");
+					} else {
+						LOG_KDIAG
+						    ("Got Sahara interrupt in unknown state");
+					}
+				}
+#endif
+			}
+		}
+
+		/* haven't handled the done1/error2 (the error 2 part), so setup to
+		 * do that now. Otherwise, exit loop */
+		state = (state == SAH_EXEC_DONE1_ERROR2) ?
+		    SAH_EXEC_ERROR1 : SAH_EXEC_IDLE;
+
+		/* Keep going while further status is available. */
+	} while (state == SAH_EXEC_ERROR1);
+
+    /* Disabling Sahara Clock only if the hardware is in idle state and
+	    the DAR queue is empty.*/
+	os_lock_save_context(desc_queue_lock, lock_flags);
+	current_entry = sah_Find_With_State(SAH_STATE_ON_SAHARA);
+	os_unlock_restore_context(desc_queue_lock, lock_flags);
+
+	if ((current_entry == NULL) && (state == SAH_EXEC_IDLE)) {
+
+#ifdef DIAG_DRV_IF
+		LOG_KDIAG("SAHARA : Disabling the clocks\n")
+#endif				/* DIAG_DRV_IF */
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+		mxc_clks_disable(SAHARA2_CLK);
+#else
+		{
+			struct clk *clk = clk_get(NULL, "sahara_clk");
+			if (clk != ERR_PTR(ENOENT))
+				clk_disable(clk);
+			clk_put(clk);
+		}
+#endif
+
+	}
+
+	return reset_flag;
+}
+
+#endif				/* ifndef SAHARA_POLL_MODE */
+
+#ifdef SAHARA_POLL_MODE
+/*!
+*******************************************************************************
+* Submits descriptor chain to SAHARA, polls on SAHARA for completion, process
+* results, and dephysicalizes chain
+*
+* @brief     Handle poll mode.
+*
+* @param    entry   Virtual address of a physicalized chain
+*
+* @return   0      this function is always successful
+*/
+
+unsigned long sah_Handle_Poll(sah_Head_Desc * entry)
+{
+	sah_Execute_Status hw_status;	/* Sahara's status register */
+	os_lock_context_t lock_flags;
+
+	/* lock SARAHA */
+	os_lock_save_context(desc_queue_lock, lock_flags);
+
+#ifdef SAHARA_POWER_MANAGEMENT
+	/* check if the dynamic power management is asserted */
+	if (sah_dpm_flag) {
+		/* return that request failed to be processed */
+		entry->result = FSL_RETURN_ERROR_S;
+		entry->fault_address = 0xBAD;
+		entry->op_status= 0xBAD;
+		entry->error_status = 0xBAD;
+	} else {
+#endif				/* SAHARA_POWER_MANAGEMENT */
+
+#if defined(DIAG_DRV_IF)
+		sah_Dump_Chain(&entry->desc, entry->desc.dma_addr);
+#endif				/* DIAG_DRV_IF */
+		/* Nothing can be in the dar if we got the lock */
+		sah_HW_Write_DAR((uint32_t) (entry->desc.dma_addr));
+
+		/* Wait for SAHARA to finish with this entry */
+		hw_status = sah_Wait_On_Sahara();
+
+		/* if entry completed successfully, mark it as such */
+	/**** HARDWARE ERROR WORK AROUND (hw_status == SAH_EXEC_IDLE) *****/
+		if (
+#ifndef SUBMIT_MULTIPLE_DARS
+			   (hw_status == SAH_EXEC_IDLE) || (hw_status == SAH_EXEC_DONE1_BUSY2) ||	/* should not happen */
+#endif
+			   (hw_status == SAH_EXEC_DONE1)
+		    ) {
+			entry->error_status = 0;
+			entry->result = FSL_RETURN_OK_S;
+		} else {
+			/* SAHARA is reporting an error with entry */
+			if (hw_status == SAH_EXEC_ERROR1) {
+				/* Gather extra diagnostic information */
+				entry->fault_address =
+				    sah_HW_Read_Fault_Address();
+				/* Read this register last - it clears the error */
+				entry->error_status =
+				    sah_HW_Read_Error_Status();
+				entry->op_status = 0;
+				/* translate from SAHARA error status to fsl_shw return values */
+				entry->result =
+				    sah_convert_error_status(entry->
+							     error_status);
+#ifdef DIAG_DRV_STATUS
+				sah_Log_Error(entry->op_status,
+					      entry->error_status,
+					      entry->fault_address);
+#endif
+			} else if (hw_status == SAH_EXEC_OPSTAT1) {
+				entry->op_status = sah_HW_Read_Op_Status();
+				entry->error_status = 0;
+				entry->result =
+				    sah_convert_op_status(op_status);
+			} else {
+				/* SAHARA entered FAULT state (or something bazaar has
+				 * happened) */
+				pr_debug
+					("Sahara: hw_status = 0x%x; Stat: 0x%08x; IDAR: 0x%08x; "
+					 "CDAR: 0x%08x; FltAdr: 0x%08x; Estat: 0x%08x\n",
+					 hw_status, sah_HW_Read_Status(),
+					 sah_HW_Read_IDAR(), sah_HW_Read_CDAR(),
+					 sah_HW_Read_Fault_Address(),
+					 sah_HW_Read_Error_Status());
+#ifdef DIAG_DRV_IF
+				{
+					int old_level = console_loglevel;
+					console_loglevel = 8;
+					sah_Dump_Chain(&(entry->desc),
+							   entry->desc.dma_addr);
+					console_loglevel = old_level;
+				}
+#endif
+				 
+				entry->error_status = -1;
+				entry->result = FSL_RETURN_ERROR_S;
+				sah_HW_Reset();
+			}
+		}
+#ifdef SAHARA_POWER_MANAGEMENT
+	}
+#endif
+
+	if (!(entry->uco_flags & FSL_UCO_BLOCKING_MODE)) {
+		/* put it in results pool to allow get_results to work */
+		sah_Queue_Append_Entry(&entry->user_info->result_pool, entry);
+		if (entry->uco_flags & FSL_UCO_CALLBACK_MODE) {
+			/* invoke callback */
+			entry->user_info->callback(entry->user_info);
+		}
+	} else {
+		/* convert the descriptor link back to virtual memory, mark dirty pages
+		 * if they are from user mode, and release the page cache for user
+		 * pages
+		 */
+		entry = sah_DePhysicalise_Descriptors(entry);
+	}
+
+	os_unlock_restore_context(desc_queue_lock, lock_flags);
+
+	return 0;
+}
+
+#endif				/* SAHARA_POLL_MODE */
+
+/******************************************************************************
+* The following is the implementation of the Dynamic Power Management
+* functionality.
+******************************************************************************/
+#ifdef SAHARA_POWER_MANAGEMENT
+
+static bool sah_dpm_init_flag;
+
+/* dynamic power management information for the sahara driver */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+static struct device_driver sah_dpm_driver = {
+	.name = "sahara_",
+	.bus = &platform_bus_type,
+#else
+static struct platform_driver sah_dpm_driver = {
+	.driver.name = "sahara_",
+	.driver.bus = &platform_bus_type,
+#endif
+	.suspend = sah_dpm_suspend,
+	.resume = sah_dpm_resume
+};
+
+/* dynamic power management information for the sahara HW device */
+static struct platform_device sah_dpm_device = {
+	.name = "sahara_",
+	.id = 1,
+};
+
+/*!
+*******************************************************************************
+* Initilaizes the dynamic power managment functionality
+*
+* @brief    Initialization of the Dynamic Power Management functionality
+*
+* @return   0 = success; failed otherwise
+*/
+int sah_dpm_init()
+{
+	int status;
+
+	/* dpm is not asserted */
+	sah_dpm_flag = FALSE;
+
+	/* register the driver to the kernel */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+	status = os_register_to_driver(&sah_dpm_driver);
+#else
+	status = os_register_to_driver(&sah_dpm_driver.driver);
+#endif
+
+	if (status == 0) {
+		/* register a single sahara chip */
+		/*status = platform_device_register(&sah_dpm_device); */
+		status = os_register_a_device(&sah_dpm_device);
+
+		/* if something went awry, unregister the driver */
+		if (status != 0) {
+			/*driver_unregister(&sah_dpm_driver); */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+			os_unregister_from_driver(&sah_dpm_driver);
+#else
+			os_unregister_from_driver(&sah_dpm_driver.driver);
+#endif
+			sah_dpm_init_flag = FALSE;
+		} else {
+			/* if everything went okay, flag that life is good */
+			sah_dpm_init_flag = TRUE;
+		}
+	}
+
+	/* let the kernel know how it went */
+	return status;
+
+}
+
+/*!
+*******************************************************************************
+* Unregister the dynamic power managment functionality
+*
+* @brief    Unregister the Dynamic Power Management functionality
+*
+*/
+void sah_dpm_close()
+{
+	/* if dynamic power management was initilaized, kill it */
+	if (sah_dpm_init_flag == TRUE) {
+		/*driver_unregister(&sah_dpm_driver); */
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+		os_unregister_from_driver(&sah_dpm_driver);
+#else
+		os_unregister_from_driver(&sah_dpm_driver.driver);
+#endif
+		/*platform_device_register(&sah_dpm_device); */
+		os_unregister_a_device(&sah_dpm_device);
+	}
+}
+
+/*!
+*******************************************************************************
+* Callback routine defined by the Linux Device Model / Dynamic Power management
+* extension. It sets a global flag to disallow the driver to enter queued items
+* into Sahara's DAR.
+*
+* It allows the current active descriptor chains to complete before it returns
+*
+* @brief   Suspends the driver
+*
+* @param   dev   contains device information
+* @param   state contains state information
+* @param   level  level of shutdown
+*
+* @return   0 = success; failed otherwise
+*/
+
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+static int sah_dpm_suspend(struct device *dev, uint32_t state, uint32_t level)
+#else
+static int sah_dpm_suspend(struct platform_device *dev, pm_message_t state)
+#endif
+{
+	sah_Head_Desc *entry = NULL;
+	os_lock_context_t lock_flags;
+
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+	switch (level) {
+	case SUSPEND_DISABLE:
+		/* Assert dynamic power management. This stops the driver from
+		 * entering queued requests to Sahara */
+		sah_dpm_flag = TRUE;
+		break;
+
+	case SUSPEND_SAVE_STATE:
+		break;
+
+	case SUSPEND_POWER_DOWN:
+		/* hopefully between the DISABLE call and this one, the outstanding
+		 * work Sahara was doing complete. this checks (and waits) for
+		 * those entries that were already active on Sahara to complete */
+		/* lock queue while searching */
+		os_lock_save_context(desc_queue_lock, lock_flags);
+		do {
+			entry = sah_Find_With_State(SAH_STATE_ON_SAHARA);
+		} while (entry != NULL);
+		os_unlock_restore_context(desc_queue_lock, lock_flags);
+
+		/* now we kill the clock so the control circuitry isn't sucking
+		 * any power */
+		mxc_clks_disable(SAHARA2_CLK);
+		break;
+	}
+#else
+	/* Assert dynamic power management. This stops the driver from
+	 * entering queued requests to Sahara */
+	sah_dpm_flag = TRUE;
+
+	/* Now wait for any outstanding work Sahara was doing to complete.
+	 * this checks (and waits) for
+	 * those entries that were already active on Sahara to complete */
+	do {
+		/* lock queue while searching */
+		os_lock_save_context(desc_queue_lock, lock_flags);
+		entry = sah_Find_With_State(SAH_STATE_ON_SAHARA);
+		os_unlock_restore_context(desc_queue_lock, lock_flags);
+	} while (entry != NULL);
+
+	/* now we kill the clock so the control circuitry isn't sucking
+	 * any power */
+	{
+		struct clk *clk = clk_get(NULL, "sahara_clk");
+		if (clk != ERR_PTR(ENOENT)) {
+			clk_disable(clk);
+		}
+	}
+#endif
+
+	return 0;
+}
+
+/*!
+*******************************************************************************
+* Callback routine defined by the Linux Device Model / Dynamic Power management
+* extension. It cleears a global flag to allow the driver to enter queued items
+* into Sahara's DAR.
+*
+* It primes the mechanism to start depleting the queue
+*
+* @brief   Resumes the driver
+*
+* @param   dev    contains device information
+* @param   level  level of resumption
+*
+* @return   0 = success; failed otherwise
+*/
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+static int sah_dpm_resume(struct device *dev, uint32_t level)
+#else
+static int sah_dpm_resume(struct platform_device *dev)
+#endif
+{
+	sah_Head_Desc *entry = NULL;
+	os_lock_context_t lock_flags;
+
+#if  LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
+	switch (level) {
+	case RESUME_POWER_ON:
+		/* enable Sahara's clock */
+		mxc_clks_enable(SAHARA2_CLK);
+		break;
+
+	case RESUME_RESTORE_STATE:
+		break;
+
+	case RESUME_ENABLE:
+		/* Disable dynamic power managment. This allows the driver to put
+		 * entries into Sahara's DAR */
+		sah_dpm_flag = FALSE;
+
+		/* find a pending entry to prime the pump */
+		os_lock_save_context(desc_queue_lock, lock_flags);
+		entry = sah_Find_With_State(SAH_STATE_PENDING);
+		if (entry != NULL) {
+			sah_Queue_Manager_Prime(entry);
+		}
+		os_unlock_restore_context(desc_queue_lock, lock_flags);
+		break;
+	}
+#else
+	{
+		/* enable Sahara's clock */
+		struct clk *clk = clk_get(NULL, "sahara_clk");
+
+		if (clk != ERR_PTR(ENOENT)) {
+			clk_enable(clk);
+		}
+	}
+	sah_dpm_flag = FALSE;
+
+	/* find a pending entry to prime the pump */
+	os_lock_save_context(desc_queue_lock, lock_flags);
+	entry = sah_Find_With_State(SAH_STATE_PENDING);
+	if (entry != NULL) {
+		sah_Queue_Manager_Prime(entry);
+	}
+	os_unlock_restore_context(desc_queue_lock, lock_flags);
+#endif
+	return 0;
+}
+
+#endif				/* SAHARA_POWER_MANAGEMENT */
diff --git a/drivers/mxc/security/sahara2/sf_util.c b/drivers/mxc/security/sahara2/sf_util.c
new file mode 100644
index 000000000000..c813b7b22338
--- /dev/null
+++ b/drivers/mxc/security/sahara2/sf_util.c
@@ -0,0 +1,1390 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/**
+* @file sf_util.c
+*
+* @brief Security Functions component API - Utility functions
+*
+* These are the 'Sahara api' functions which are used by the higher-level
+* FSL SHW API to build and then execute descriptor chains.
+*/
+
+
+#include "sf_util.h"
+#include <adaptor.h>
+
+#ifdef DIAG_SECURITY_FUNC
+#include <diagnostic.h>
+#endif /*DIAG_SECURITY_FUNC*/
+
+
+#ifdef __KERNEL__
+EXPORT_SYMBOL(sah_Append_Desc);
+EXPORT_SYMBOL(sah_Append_Link);
+EXPORT_SYMBOL(sah_Create_Link);
+EXPORT_SYMBOL(sah_Create_Key_Link);
+EXPORT_SYMBOL(sah_Destroy_Link);
+EXPORT_SYMBOL(sah_Descriptor_Chain_Execute);
+EXPORT_SYMBOL(sah_insert_mdha_algorithm);
+EXPORT_SYMBOL(sah_insert_skha_algorithm);
+EXPORT_SYMBOL(sah_insert_skha_mode);
+EXPORT_SYMBOL(sah_insert_skha_modulus);
+EXPORT_SYMBOL(sah_Descriptor_Chain_Destroy);
+EXPORT_SYMBOL(sah_add_two_in_desc);
+EXPORT_SYMBOL(sah_add_in_key_desc);
+EXPORT_SYMBOL(sah_add_two_out_desc);
+EXPORT_SYMBOL(sah_add_in_out_desc);
+EXPORT_SYMBOL(sah_add_key_out_desc);
+#endif
+
+#ifdef DEBUG_REWORK
+#ifndef __KERNEL__
+#include <stdio.h>
+#define os_printk printf
+#endif
+#endif
+
+/**
+ * Convert fsl_shw_hash_alg_t to mdha mode bits.
+ *
+ * Index must be maintained in order of fsl_shw_hash_alg_t enumeration!!!
+ */
+const uint32_t sah_insert_mdha_algorithm[] =
+{
+    [FSL_HASH_ALG_MD5] = sah_insert_mdha_algorithm_md5,
+    [FSL_HASH_ALG_SHA1] = sah_insert_mdha_algorithm_sha1,
+    [FSL_HASH_ALG_SHA224] = sah_insert_mdha_algorithm_sha224,
+    [FSL_HASH_ALG_SHA256] = sah_insert_mdha_algorithm_sha256,
+};
+
+/**
+ * Header bits for Algorithm field of SKHA header
+ *
+ * Index value must be kept in sync with fsl_shw_key_alg_t
+ */
+const uint32_t sah_insert_skha_algorithm[] =
+{
+    [FSL_KEY_ALG_HMAC] = 0x00000040,
+    [FSL_KEY_ALG_AES] = sah_insert_skha_algorithm_aes,
+    [FSL_KEY_ALG_DES] = sah_insert_skha_algorithm_des,
+    [FSL_KEY_ALG_TDES] = sah_insert_skha_algorithm_tdes,
+    [FSL_KEY_ALG_ARC4] = sah_insert_skha_algorithm_arc4,
+};
+
+
+/**
+ * Header bits for MODE field of SKHA header
+ *
+ * Index value must be kept in sync with fsl_shw_sym_mod_t
+ */
+const uint32_t sah_insert_skha_mode[] =
+{
+    [FSL_SYM_MODE_STREAM] = sah_insert_skha_mode_ecb,
+    [FSL_SYM_MODE_ECB] = sah_insert_skha_mode_ecb,
+    [FSL_SYM_MODE_CBC] = sah_insert_skha_mode_cbc,
+    [FSL_SYM_MODE_CTR] = sah_insert_skha_mode_ctr,
+};
+
+
+/**
+ * Header bits to set CTR modulus size.  These have parity
+ * included to allow XOR insertion of values.
+ *
+ * @note Must be kept in sync with fsl_shw_ctr_mod_t
+ */
+const uint32_t sah_insert_skha_modulus[] =
+{
+    [FSL_CTR_MOD_8] = 0x00000000, /**< 2**8 */
+    [FSL_CTR_MOD_16] = 0x80000200, /**< 2**16 */
+    [FSL_CTR_MOD_24] = 0x80000400, /**< 2**24 */
+    [FSL_CTR_MOD_32] = 0x00000600, /**< 2**32 */
+    [FSL_CTR_MOD_40] = 0x80000800, /**< 2**40 */
+    [FSL_CTR_MOD_48] = 0x00000a00, /**< 2**48 */
+    [FSL_CTR_MOD_56] = 0x00000c00, /**< 2**56 */
+    [FSL_CTR_MOD_64] = 0x80000e00, /**< 2**64 */
+    [FSL_CTR_MOD_72] = 0x80001000, /**< 2**72 */
+    [FSL_CTR_MOD_80] = 0x00001200, /**< 2**80 */
+    [FSL_CTR_MOD_88] = 0x00001400, /**< 2**88 */
+    [FSL_CTR_MOD_96] = 0x80001600, /**< 2**96 */
+    [FSL_CTR_MOD_104] = 0x00001800, /**< 2**104 */
+    [FSL_CTR_MOD_112] = 0x80001a00, /**< 2**112 */
+    [FSL_CTR_MOD_120] = 0x80001c00, /**< 2**120 */
+    [FSL_CTR_MOD_128] = 0x00001e00 /**< 2**128 */
+};
+
+
+/******************************************************************************
+* Internal function declarations
+******************************************************************************/
+static fsl_shw_return_t sah_Create_Desc(
+    const sah_Mem_Util *mu,
+    sah_Desc ** desc,
+    int head,
+    uint32_t header,
+    sah_Link * link1,
+    sah_Link * link2);
+
+
+/**
+ * Create a descriptor chain using the the header and links passed in as
+ * parameters. The newly created descriptor will be added to the end of
+ * the descriptor chain passed.
+ *
+ * If @a desc_head points to a NULL value, then a sah_Head_Desc will be created
+ * as the first descriptor.  Otherwise a sah_Desc will be created and appended.
+ *
+ * @pre
+ *
+ * - None
+ *
+ * @post
+ *
+ * - A descriptor has been created from the header, link1 and link2.
+ *
+ * - The newly created descriptor has been appended to the end of
+ *   desc_head, or its location stored into the location pointed to by
+ *   @a desc_head.
+ *
+ * - On allocation failure, @a link1 and @a link2 will be destroyed., and
+ *   @a desc_head will be untouched.
+ *
+ * @brief   Create and append descriptor chain, inserting header and links
+ *          pointing to link1 and link2
+ *
+ * @param  mu Memory functions
+ * @param  header Value of descriptor header to be added
+ * @param  desc_head Pointer to head of descriptor chain to append new desc
+ * @param  link1 Pointer to sah_Link 1 (or NULL)
+ * @param  link2 Pointer to sah_Link 2 (or NULL)
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_Append_Desc(
+    const sah_Mem_Util *mu,
+    sah_Head_Desc **desc_head,
+    const uint32_t header,
+    sah_Link *link1,
+    sah_Link *link2)
+{
+    fsl_shw_return_t status;
+    sah_Desc         *desc;
+    sah_Desc         *desc_ptr;
+
+
+    status = sah_Create_Desc(mu, (sah_Desc**)&desc, (*desc_head == NULL),
+                             header, link1, link2);
+    /* append newly created descriptor to end of current chain */
+    if (status == FSL_RETURN_OK_S) {
+        if (*desc_head == NULL) {
+            (*desc_head) = (sah_Head_Desc*)desc;
+            (*desc_head)->out1_ptr = NULL;
+            (*desc_head)->out2_ptr = NULL;
+
+        } else {
+            desc_ptr = (sah_Desc*)*desc_head;
+            while (desc_ptr->next != NULL) {
+                desc_ptr = desc_ptr->next;
+            }
+            desc_ptr->next = desc;
+        }
+    }
+
+    return status;
+}
+
+
+/**
+ * Releases the memory allocated by the Security Function library for
+ * descriptors, links and any internally allocated memory referenced in the
+ * given chain. Note that memory allocated by user applications is not
+ * released.
+ *
+ * @post The @a desc_head pointer will be set to NULL to prevent further use.
+ *
+ * @brief   Destroy a descriptor chain and free memory of associated links
+ *
+ * @param  mu Memory functions
+ * @param  desc_head Pointer to head of descriptor chain to be freed
+ *
+ * @return none
+ */
+void sah_Descriptor_Chain_Destroy (
+    const sah_Mem_Util *mu,
+    sah_Head_Desc **desc_head)
+{
+    sah_Desc *desc_ptr = &(*desc_head)->desc;
+    sah_Head_Desc *desc_head_ptr = (sah_Head_Desc *)desc_ptr;
+
+    while (desc_ptr != NULL) {
+        register sah_Desc *next_desc_ptr;
+
+        if (desc_ptr->ptr1 != NULL) {
+            sah_Destroy_Link(mu, desc_ptr->ptr1);
+        }
+        if (desc_ptr->ptr2 != NULL) {
+            sah_Destroy_Link(mu, desc_ptr->ptr2);
+        }
+
+        next_desc_ptr = desc_ptr->next;
+
+        /* Be sure to free head descriptor as such */
+        if (desc_ptr == (sah_Desc*)desc_head_ptr) {
+            mu->mu_free_head_desc(mu->mu_ref, desc_head_ptr);
+        } else {
+            mu->mu_free_desc(mu->mu_ref, desc_ptr);
+        }
+
+        desc_ptr = next_desc_ptr;
+    }
+
+    *desc_head = NULL;
+}
+
+
+#ifndef NO_INPUT_WORKAROUND
+/**
+ * Reworks the link chain
+ *
+ * @brief   Reworks the link chain
+ *
+ * @param  mu    Memory functions
+ * @param  link  Pointer to head of link chain to be reworked
+ *
+ * @return none
+ */
+static fsl_shw_return_t sah_rework_link_chain(
+        const sah_Mem_Util *mu,
+        sah_Link* link)
+{
+    fsl_shw_return_t status = FSL_RETURN_OK_S;
+    int found_potential_problem = 0;
+    uint32_t total_data = 0;
+#ifdef DEBUG_REWORK
+    sah_Link* first_link = link;
+#endif
+
+    if ((link->flags & SAH_OUTPUT_LINK)) {
+        return status;
+    }
+
+    while (link != NULL) {
+        total_data += link->len;
+
+    /* Only non-key Input Links are affected by the DMA flush-to-FIFO
+     * problem */
+
+        /* If have seen problem and at end of chain... */
+        if (found_potential_problem && (link->next == NULL) &&
+                                         (total_data > 16)) {
+            /* insert new 1-byte link */
+            sah_Link* new_tail_link = mu->mu_alloc_link(mu->mu_ref);
+            if (new_tail_link == NULL) {
+                status = FSL_RETURN_NO_RESOURCE_S;
+            } else {
+#ifdef DEBUG_REWORK
+                sah_Link* dump_link = first_link;
+                while (dump_link != NULL) {
+                    uint32_t i;
+                    unsigned bytes_to_dump = (dump_link->len > 32) ?
+                                              32 : dump_link->len;
+                    os_printk("(rework)Link %p: %p/%u/%p\n", dump_link,
+                              dump_link->data, dump_link->len,
+                              dump_link->next);
+                    if (!(dump_link->flags & SAH_STORED_KEY_INFO)) {
+                        os_printk("(rework)Data %p: ", dump_link->data);
+                        for (i = 0; i < bytes_to_dump; i++) {
+                            os_printk("%02X ", dump_link->data[i]);
+                        }
+                        os_printk("\n");
+                    }
+                    else {
+                        os_printk("rework)Data %p: Red key data\n", dump_link);
+                    }
+                    dump_link = dump_link->next;
+                }
+#endif
+                link->len--;
+                link->next = new_tail_link;
+                new_tail_link->len = 1;
+                new_tail_link->data = link->data+link->len;
+                new_tail_link->flags = link->flags & ~(SAH_OWNS_LINK_DATA);
+                new_tail_link->next = NULL;
+                link = new_tail_link;
+#ifdef DEBUG_REWORK
+                os_printk("(rework)New link chain:\n");
+                dump_link = first_link;
+                while (dump_link != NULL) {
+                    uint32_t i;
+                    unsigned bytes_to_dump = (dump_link->len > 32) ?
+                                              32 : dump_link->len;
+
+                    os_printk("Link %p: %p/%u/%p\n", dump_link,
+                              dump_link->data, dump_link->len,
+                              dump_link->next);
+                    if (!(dump_link->flags & SAH_STORED_KEY_INFO)) {
+                        os_printk("Data %p: ", dump_link->data);
+                        for (i = 0; i < bytes_to_dump; i++) {
+                            os_printk("%02X ", dump_link->data[i]);
+                        }
+                        os_printk("\n");
+                    }
+                    else {
+                        os_printk("Data %p: Red key data\n", dump_link);
+                    }
+                    dump_link = dump_link->next;
+                }
+#endif
+            }
+        } else if ((link->len % 4) || ((uint32_t)link->data % 4)) {
+            found_potential_problem = 1;
+        }
+
+        link = link->next;
+    }
+
+    return status;
+}
+
+
+/**
+ * Rework links to avoid H/W bug
+ *
+ * @param head   Beginning of descriptor chain
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static fsl_shw_return_t sah_rework_links(
+        const sah_Mem_Util *mu,
+        sah_Head_Desc *head)
+{
+    fsl_shw_return_t status = FSL_RETURN_OK_S;
+    sah_Desc* desc = &head->desc;
+
+    while ((status == FSL_RETURN_OK_S) && (desc != NULL)) {
+        if (desc->header & SAH_HDR_LLO) {
+            status = FSL_RETURN_ERROR_S;
+            break;
+        }
+        if (desc->ptr1 != NULL) {
+            status = sah_rework_link_chain(mu, desc->ptr1);
+        }
+        if ((status == FSL_RETURN_OK_S) && (desc->ptr2 != NULL)) {
+            status = sah_rework_link_chain(mu, desc->ptr2);
+        }
+        desc = desc->next;
+    }
+
+    return status;
+}
+#endif /* NO_INPUT_WORKAROUND */
+
+
+/**
+ * Send a descriptor chain to the SAHARA driver for processing.
+ *
+ * Note that SAHARA will read the input data from and write the output data
+ * directly to the locations indicated during construction of the chain.
+ *
+ * @pre
+ *
+ * - None
+ *
+ * @post
+ *
+ * - @a head will have been executed on SAHARA
+ * - @a head Will be freed unless a SAVE flag is set
+ *
+ * @brief   Execute a descriptor chain
+ *
+ * @param  head      Pointer to head of descriptor chain to be executed
+ * @param  user_ctx  The user context on which to execute the descriptor chain.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_Descriptor_Chain_Execute(
+    sah_Head_Desc *head,
+    fsl_shw_uco_t *user_ctx)
+{
+    fsl_shw_return_t status;
+
+	/* Check for null pointer or non-multiple-of-four value */
+    if ((head == NULL) || ((uint32_t)head & 0x3)) {
+        status = FSL_RETURN_ERROR_S;
+        goto out;
+    }
+
+#ifndef NO_INPUT_WORKAROUND
+    status = sah_rework_links(user_ctx->mem_util, head);
+    if (status != FSL_RETURN_OK_S) {
+        goto out;
+    }
+#endif
+
+    /* complete the information in the descriptor chain head node */
+    head->user_ref = user_ctx->user_ref;
+    head->uco_flags = user_ctx->flags;
+    head->next = NULL; /* driver will use this to link chain heads */
+
+    status = adaptor_Exec_Descriptor_Chain(head, user_ctx);
+
+#ifdef DIAG_SECURITY_FUNC
+	 if (status == FSL_RETURN_OK_S)
+		 LOG_DIAG("after exec desc chain: status is ok\n");
+	 else
+		 LOG_DIAG("after exec desc chain: status is not ok\n");
+#endif
+
+ out:
+    return status;
+}
+
+
+/**
+ * Create Link
+ *
+ * @brief   Allocate Memory for Link structure and populate using input
+ *          parameters
+ *
+ * @post    On allocation failure, @a p will be freed if #SAH_OWNS_LINK_DATA is
+ *          p set in @a flags.
+
+ * @param  mu Memory functions
+ * @param  link Pointer to link to be created
+ * @param  p Pointer to data to use in link
+ * @param  length Length of buffer 'p' in bytes
+ * @param  flags Indicates whether memory has been allocated by the calling
+ *         function or the security function
+ *
+ * @return FSL_RETURN_OK_S or FSL_RETURN_NO_RESOURCE_S
+ */
+fsl_shw_return_t sah_Create_Link(
+    const sah_Mem_Util *mu,
+    sah_Link **link,
+    uint8_t *p,
+    const size_t length,
+    const sah_Link_Flags flags)
+{
+
+#ifdef DIAG_SECURITY_FUNC
+
+    char diag[50];
+#endif /*DIAG_SECURITY_FUNC_UGLY*/
+    fsl_shw_return_t status = FSL_RETURN_NO_RESOURCE_S;
+
+
+    *link = mu->mu_alloc_link(mu->mu_ref);
+
+    /* populate link if memory allocation successful */
+    if (*link != NULL) {
+        (*link)->len = length;
+        (*link)->data = p;
+        (*link)->next = NULL;
+        (*link)->flags = flags;
+        status = FSL_RETURN_OK_S;
+
+#ifdef DIAG_SECURITY_FUNC
+
+        LOG_DIAG("Created Link");
+        LOG_DIAG("------------");
+        sprintf(diag," address       = 0x%x", (int) *link);
+        LOG_DIAG(diag);
+        sprintf(diag," link->len     = %d",(*link)->len);
+        LOG_DIAG(diag);
+        sprintf(diag," link->data    = 0x%x",(int) (*link)->data);
+        LOG_DIAG(diag);
+        sprintf(diag," link->flags   = 0x%x",(*link)->flags);
+        LOG_DIAG(diag);
+        LOG_DIAG(" link->next    = NULL");
+#endif /*DIAG_SECURITY_FUNC_UGLY*/
+
+    } else {
+#ifdef DIAG_SECURITY_FUNC
+        LOG_DIAG("Allocation of memory for sah_Link failed!\n");
+#endif /*DIAG_SECURITY_FUNC*/
+
+        /* Free memory previously allocated by the security function layer for
+        link data. Note that the memory being pointed to will be zeroed before
+        being freed, for security reasons. */
+        if (flags & SAH_OWNS_LINK_DATA) {
+            mu->mu_memset(mu->mu_ref, p, 0x00, length);
+            mu->mu_free(mu->mu_ref, p);
+        }
+    }
+
+    return status;
+}
+
+
+/**
+ * Create Key Link
+ *
+ * @brief   Allocate Memory for Link structure and populate using key info
+ *          object
+ *
+ * @param  mu Memory functions
+ * @param  link Pointer to store address of link to be created
+ * @param  key_info Pointer to Key Info object to be referenced
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_Create_Key_Link(
+    const sah_Mem_Util *mu,
+    sah_Link **link,
+    fsl_shw_sko_t *key_info)
+{
+#ifdef DIAG_SECURITY_FUNC_UGLY
+    char diag[50];
+#endif /*DIAG_SECURITY_FUNC_UGLY*/
+    fsl_shw_return_t status = FSL_RETURN_NO_RESOURCE_S;
+    sah_Link_Flags flags = 0;
+
+
+    *link = mu->mu_alloc_link(mu->mu_ref);
+
+    /* populate link if memory allocation successful */
+    if (*link != NULL) {
+        (*link)->len = key_info->key_length;
+
+        if (key_info->flags & FSL_SKO_KEY_PRESENT) {
+            (*link)->data = key_info->key;
+            status = FSL_RETURN_OK_S;
+        } else {
+            if (key_info->flags & FSL_SKO_KEY_ESTABLISHED) {
+				
+				if (key_info->keystore == NULL) {
+					/* System Keystore */
+                	(*link)->slot = key_info->handle;
+                	(*link)->ownerid = key_info->userid;
+                	(*link)->data = 0;
+                	flags |= SAH_STORED_KEY_INFO;
+                	status = FSL_RETURN_OK_S;
+				} else {
+#ifdef FSL_HAVE_SCC2
+					/* User Keystore */
+					fsl_shw_kso_t *keystore = key_info->keystore;
+					/* Note: the key data is stored here, but the address has to
+					 * be converted to a partition and offset in the kernel.
+					 * This will be calculated in kernel space, based on the
+					 * list of partitions held by the users context.
+					 */
+					(*link)->data =	
+						keystore->slot_get_address(keystore->user_data,
+												 key_info->handle);
+				
+					flags |= SAH_IN_USER_KEYSTORE;
+					status = FSL_RETURN_OK_S;
+#else
+					/* User keystores only supported in SCC2 */
+					status = FSL_RETURN_BAD_FLAG_S;
+#endif				/* FSL_HAVE_SCC2 */
+				
+				}
+            } else {
+                /* the flag is bad. Should never get here */
+                status = FSL_RETURN_BAD_FLAG_S;
+            }
+        }
+
+        (*link)->next = NULL;
+        (*link)->flags = flags;
+
+#ifdef DIAG_SECURITY_FUNC_UGLY
+        if (status == FSL_RETURN_OK_S) {
+            LOG_DIAG("Created Link");
+            LOG_DIAG("------------");
+            sprintf(diag," address       = 0x%x", (int) *link);
+            LOG_DIAG(diag);
+            sprintf(diag," link->len     = %d", (*link)->len);
+            LOG_DIAG(diag);
+            if (key_info->flags & FSL_SKO_KEY_ESTABLISHED) {
+                sprintf(diag," link->slot    = 0x%x", (*link)->slot);
+                LOG_DIAG(diag);
+            } else {
+                sprintf(diag," link->data    = 0x%x", (int)(*link)->data);
+                LOG_DIAG(diag);
+            }
+            sprintf(diag," link->flags   = 0x%x", (*link)->flags);
+            LOG_DIAG(diag);
+            LOG_DIAG(" link->next    = NULL");
+        }
+#endif /*DIAG_SECURITY_FUNC_UGLY*/
+
+        if (status == FSL_RETURN_BAD_FLAG_S) {
+            mu->mu_free_link(mu->mu_ref, *link);
+            *link = NULL;
+#ifdef DIAG_SECURITY_FUNC
+            LOG_DIAG("Creation of sah_Key_Link failed due to bad key flag!\n");
+#endif /*DIAG_SECURITY_FUNC*/
+        }
+
+    } else {
+#ifdef DIAG_SECURITY_FUNC
+        LOG_DIAG("Allocation of memory for sah_Key_Link failed!\n");
+#endif /*DIAG_SECURITY_FUNC*/
+    }
+
+    return status;
+}
+
+
+/**
+ * Append Link
+ *
+ * @brief   Allocate Memory for Link structure and append it to the end of
+ *          the link chain.
+ *
+ * @post    On allocation failure, @a p will be freed if #SAH_OWNS_LINK_DATA is
+ *          p set in @a flags.
+ *
+ * @param  mu Memory functions
+ * @param  link_head Pointer to (head of) existing link chain
+ * @param  p Pointer to data to use in link
+ * @param  length Length of buffer 'p' in bytes
+ * @param  flags Indicates whether memory has been allocated by the calling
+ *         function or the security function
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_Append_Link(
+    const sah_Mem_Util *mu,
+    sah_Link *link_head,
+    uint8_t *p,
+    const size_t length,
+    const sah_Link_Flags flags)
+{
+    sah_Link* new_link;
+    fsl_shw_return_t status;
+
+
+    status = sah_Create_Link(mu, &new_link, p, length, flags);
+
+    if (status == FSL_RETURN_OK_S) {
+        /* chase for the tail */
+        while (link_head->next != NULL) {
+            link_head = link_head->next;
+        }
+
+        /* and add new tail */
+        link_head->next = new_link;
+    }
+
+    return status;
+}
+
+
+/**
+ * Create and populate a single descriptor
+ *
+ * The pointer and length fields will be be set based on the chains passed in
+ * as @a link1 and @a link2.
+ *
+ * @param  mu         Memory utility suite
+ * @param  desc       Location to store pointer of new descriptor
+ * @param  head_desc  Non-zero if this will be first in chain; zero otherwise
+ * @param  header     The Sahara header value to store in the descriptor
+ * @param  link1      A value (or NULL) for the first ptr
+ * @param  link2      A value (or NULL) for the second ptr
+ *
+ * @post  If allocation succeeded, the @a link1 and @link2 will be linked into
+ *        the descriptor.  If allocation failed, those link structues will be
+ *        freed, and the @a desc will be unchanged.
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+static fsl_shw_return_t sah_Create_Desc(
+    const sah_Mem_Util *mu,
+    sah_Desc ** desc,
+    int head_desc,
+    uint32_t header,
+    sah_Link * link1,
+    sah_Link * link2)
+{
+    fsl_shw_return_t status = FSL_RETURN_NO_RESOURCE_S;
+#ifdef DIAG_SECURITY_FUNC_UGLY
+    char diag[50];
+#endif /*DIAG_SECURITY_FUNC_UGLY*/
+
+
+    if (head_desc != 0) {
+        *desc = (sah_Desc *)mu->mu_alloc_head_desc(mu->mu_ref);
+    } else {
+        *desc = mu->mu_alloc_desc(mu->mu_ref);
+    }
+
+    /* populate descriptor if memory allocation successful */
+    if ((*desc) != NULL) {
+        sah_Link* temp_link;
+
+        status = FSL_RETURN_OK_S;
+        (*desc)->header = header;
+
+        temp_link = (*desc)->ptr1 = link1;
+        (*desc)->len1 = 0;
+        while (temp_link != NULL) {
+            (*desc)->len1 += temp_link->len;
+            temp_link = temp_link->next;
+        }
+
+        temp_link = (*desc)->ptr2 = link2;
+        (*desc)->len2 = 0;
+        while (temp_link != NULL) {
+            (*desc)->len2 += temp_link->len;
+            temp_link = temp_link->next;
+        }
+
+        (*desc)->next = NULL;
+
+#ifdef DIAG_SECURITY_FUNC_UGLY
+        LOG_DIAG("Created Desc");
+        LOG_DIAG("------------");
+        sprintf(diag," address       = 0x%x",(int) *desc);
+        LOG_DIAG(diag);
+        sprintf(diag," desc->header  = 0x%x",(*desc)->header);
+        LOG_DIAG(diag);
+        sprintf(diag," desc->len1    = %d",(*desc)->len1);
+        LOG_DIAG(diag);
+        sprintf(diag," desc->ptr1    = 0x%x",(int) ((*desc)->ptr1));
+        LOG_DIAG(diag);
+        sprintf(diag," desc->len2    = %d",(*desc)->len2);
+        LOG_DIAG(diag);
+        sprintf(diag," desc->ptr2    = 0x%x",(int) ((*desc)->ptr2));
+        LOG_DIAG(diag);
+        sprintf(diag," desc->next    = 0x%x",(int) ((*desc)->next));
+        LOG_DIAG(diag);
+#endif /*DIAG_SECURITY_FUNC_UGLY*/
+    } else {
+#ifdef DIAG_SECURITY_FUNC
+        LOG_DIAG("Allocation of memory for sah_Desc failed!\n");
+#endif /*DIAG_SECURITY_FUNC*/
+
+        /* Destroy the links, otherwise the memory allocated by the Security
+        Function layer for the links (and possibly the data within the links)
+        will be lost */
+        if (link1 != NULL) {
+            sah_Destroy_Link(mu, link1);
+        }
+        if (link2 != NULL) {
+            sah_Destroy_Link(mu, link2);
+        }
+    }
+
+    return status;
+}
+
+
+/**
+ * Destroy a link (chain) and free memory
+ *
+ * @param mu   memory utility suite
+ * @param link The Link to destroy
+ *
+ * @return none
+ */
+void sah_Destroy_Link(
+    const sah_Mem_Util *mu,
+    sah_Link * link)
+{
+
+    while (link != NULL) {
+        sah_Link * next_link = link->next;
+
+        if (link->flags & SAH_OWNS_LINK_DATA) {
+            /* zero data for security purposes */
+            mu->mu_memset(mu->mu_ref, link->data, 0x00, link->len);
+            mu->mu_free(mu->mu_ref, link->data);
+        }
+
+        link->data = NULL;
+        link->next = NULL;
+        mu->mu_free_link(mu->mu_ref, link);
+
+        link = next_link;
+    }
+}
+
+
+/**
+ * Add descriptor where both links are inputs.
+ *
+ * @param         header     The Sahara header value for the descriptor.
+ * @param         in1        The first input buffer (or NULL)
+ * @param         in1_length Size of @a in1
+ * @param[out]    in2        The second input buffer (or NULL)
+ * @param         in2_length Size of @a in2
+ * @param         mu         Memory functions
+ * @param[in,out] desc_chain Chain to start or append to
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_add_two_in_desc(uint32_t header,
+                                 const uint8_t* in1,
+                                 uint32_t in1_length,
+                                 const uint8_t* in2,
+                                 uint32_t in2_length,
+                                 const sah_Mem_Util* mu,
+                                 sah_Head_Desc** desc_chain)
+{
+    fsl_shw_return_t status = FSL_RETURN_OK_S;
+    sah_Link* link1 = NULL;
+    sah_Link* link2 = NULL;
+
+	if (in1 != NULL) {
+		status = sah_Create_Link(mu, &link1,
+			(sah_Oct_Str) in1, in1_length, SAH_USES_LINK_DATA);
+    }
+
+    if ( (in2 != NULL) && (status == FSL_RETURN_OK_S) ) {
+        status = sah_Create_Link(mu, &link2,
+                                 (sah_Oct_Str) in2, in2_length,
+                                 SAH_USES_LINK_DATA);
+    }
+
+    if (status != FSL_RETURN_OK_S) {
+        if (link1 != NULL) {
+            sah_Destroy_Link(mu, link1);
+        }
+        if (link2 != NULL) {
+            sah_Destroy_Link(mu, link2);
+        }
+    } else {
+        status = sah_Append_Desc(mu, desc_chain, header, link1, link2);
+    }
+
+    return status;
+}
+
+/*!
+ * Add descriptor where neither link needs sync
+ *
+ * @param         header     The Sahara header value for the descriptor.
+ * @param         in1        The first input buffer (or NULL)
+ * @param         in1_length Size of @a in1
+ * @param[out]    in2        The second input buffer (or NULL)
+ * @param         in2_length Size of @a in2
+ * @param         mu         Memory functions
+ * @param[in,out] desc_chain Chain to start or append to
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_add_two_d_desc(uint32_t header,
+				    const uint8_t * in1,
+				    uint32_t in1_length,
+				    const uint8_t * in2,
+				    uint32_t in2_length,
+				    const sah_Mem_Util * mu,
+				    sah_Head_Desc ** desc_chain)
+{
+	fsl_shw_return_t status = FSL_RETURN_OK_S;
+	sah_Link *link1 = NULL;
+	sah_Link *link2 = NULL;
+
+	printk("Entering sah_add_two_d_desc \n");
+
+	if (in1 != NULL) {
+		status = sah_Create_Link(mu, &link1,
+					 (sah_Oct_Str) in1, in1_length,
+					 SAH_USES_LINK_DATA);
+	}
+
+	if ((in2 != NULL) && (status == FSL_RETURN_OK_S)) {
+		status = sah_Create_Link(mu, &link2,
+					 (sah_Oct_Str) in2, in2_length,
+					 SAH_USES_LINK_DATA);
+	}
+
+	if (status != FSL_RETURN_OK_S) {
+		if (link1 != NULL) {
+			sah_Destroy_Link(mu, link1);
+		}
+		if (link2 != NULL) {
+			sah_Destroy_Link(mu, link2);
+		}
+	} else {
+		status = sah_Append_Desc(mu, desc_chain, header, link1, link2);
+	}
+
+	return status;
+}				/* sah_add_two_d_desc() */
+
+/**
+ * Add descriptor where both links are inputs, the second one being a key.
+ *
+ * @param         header     The Sahara header value for the descriptor.
+ * @param         in1        The first input buffer (or NULL)
+ * @param         in1_length Size of @a in1
+ * @param[out]    in2        The second input buffer (or NULL)
+ * @param         in2_length Size of @a in2
+ * @param         mu         Memory functions
+ * @param[in,out] desc_chain Chain to start or append to
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_add_in_key_desc(uint32_t header,
+                                 const uint8_t* in1,
+                                 uint32_t in1_length,
+                                 fsl_shw_sko_t* key_info,
+                                 const sah_Mem_Util* mu,
+                                 sah_Head_Desc** desc_chain)
+{
+    fsl_shw_return_t status = FSL_RETURN_OK_S;
+    sah_Link         *link1 = NULL;
+    sah_Link         *link2 = NULL;
+
+	if (in1 != NULL) {
+        status = sah_Create_Link(mu, &link1,
+                                 (sah_Oct_Str) in1, in1_length,
+                                 SAH_USES_LINK_DATA);
+    }
+
+	if (status != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+    status = sah_Create_Key_Link(mu, &link2, key_info);
+
+	
+	if (status != FSL_RETURN_OK_S) {
+		goto out;
+	}
+
+	status = sah_Append_Desc(mu, desc_chain, header, link1, link2);
+	
+out:
+    if (status != FSL_RETURN_OK_S) {
+        if (link1 != NULL) {
+            sah_Destroy_Link(mu, link1);
+        }
+        if (link2 != NULL) {
+            sah_Destroy_Link(mu, link2);
+        }
+    }
+
+    return status;
+}
+
+
+/**
+ * Create two links using keys allocated in the scc
+ *
+ * @param         header     The Sahara header value for the descriptor.
+ * @param         in1        The first input buffer (or NULL)
+ * @param         in1_length Size of @a in1
+ * @param[out]    in2        The second input buffer (or NULL)
+ * @param         in2_length Size of @a in2
+ * @param         mu         Memory functions
+ * @param[in,out] desc_chain Chain to start or append to
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_add_key_key_desc(uint32_t header,
+                                  fsl_shw_sko_t *key_info1,
+                                  fsl_shw_sko_t *key_info2,
+                                  const sah_Mem_Util *mu,
+                                  sah_Head_Desc **desc_chain)
+{
+    fsl_shw_return_t status;
+    sah_Link         *link1 = NULL;
+    sah_Link         *link2 = NULL;
+
+
+    status = sah_Create_Key_Link(mu, &link1, key_info1);
+
+    if (status == FSL_RETURN_OK_S) {
+        status = sah_Create_Key_Link(mu, &link2, key_info2);
+    }
+
+    if (status != FSL_RETURN_OK_S) {
+        if (link1 != NULL) {
+            sah_Destroy_Link(mu, link1);
+        }
+        if (link2 != NULL) {
+            sah_Destroy_Link(mu, link2);
+        }
+    } else {
+        status = sah_Append_Desc(mu, desc_chain, header, link1, link2);
+    }
+
+    return status;
+}
+
+
+/**
+ * Add descriptor where first link is input, the second is a changing key
+ *
+ * @param         header     The Sahara header value for the descriptor.
+ * @param         in1        The first input buffer (or NULL)
+ * @param         in1_length Size of @a in1
+ * @param[out]    in2        The key for output
+ * @param         mu         Memory functions
+ * @param[in,out] desc_chain Chain to start or append to
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_add_in_keyout_desc(uint32_t header,
+                                 const uint8_t* in1,
+                                 uint32_t in1_length,
+                                 fsl_shw_sko_t* key_info,
+                                 const sah_Mem_Util* mu,
+                                 sah_Head_Desc** desc_chain)
+{
+    fsl_shw_return_t status = FSL_RETURN_OK_S;
+    sah_Link         *link1 = NULL;
+    sah_Link         *link2 = NULL;
+
+	if (in1 != NULL) {
+        status = sah_Create_Link(mu, &link1,
+                                 (sah_Oct_Str) in1, in1_length,
+                                 SAH_USES_LINK_DATA);
+    }
+
+    if (status != FSL_RETURN_OK_S) {
+		goto out;
+    }	
+
+	status = sah_Create_Key_Link(mu, &link2, key_info);
+    
+	if (status != FSL_RETURN_OK_S) {
+		goto out;
+	}   
+
+link2->flags |= SAH_OUTPUT_LINK;	/* mark key for output */
+status = sah_Append_Desc(mu, desc_chain, header, link1, link2);
+	
+out:
+
+    if (status != FSL_RETURN_OK_S) {
+        if (link1 != NULL) {
+            sah_Destroy_Link(mu, link1);
+        }
+        if (link2 != NULL) {
+            sah_Destroy_Link(mu, link2);
+        }
+    } 
+
+    return status;
+}
+
+/**
+ * Add descriptor where both links are outputs.
+ *
+ * @param         header      The Sahara header value for the descriptor.
+ * @param         out1        The first output buffer (or NULL)
+ * @param         out1_length Size of @a out1
+ * @param[out]    out2        The second output buffer (or NULL)
+ * @param         out2_length Size of @a out2
+ * @param         mu          Memory functions
+ * @param[in,out] desc_chain  Chain to start or append to
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_add_two_out_desc(uint32_t header,
+                                      uint8_t* out1,
+                                      uint32_t out1_length,
+                                      uint8_t* out2,
+                                      uint32_t out2_length,
+                                      const sah_Mem_Util* mu,
+                                      sah_Head_Desc** desc_chain)
+{
+    fsl_shw_return_t status = FSL_RETURN_OK_S;
+    sah_Link         *link1 = NULL;
+    sah_Link         *link2 = NULL;
+
+
+	if (out1 != NULL) {
+		status = sah_Create_Link(mu, &link1,
+			(sah_Oct_Str) out1, out1_length,
+			SAH_OUTPUT_LINK | SAH_USES_LINK_DATA);
+    }
+
+    if ( (out2 != NULL) && (status == FSL_RETURN_OK_S) ) {
+        status = sah_Create_Link(mu, &link2,
+                                 (sah_Oct_Str) out2, out2_length,
+                                 SAH_OUTPUT_LINK |
+                                 SAH_USES_LINK_DATA);
+    }
+
+    if (status != FSL_RETURN_OK_S) {
+        if (link1 != NULL) {
+            sah_Destroy_Link(mu, link1);
+        }
+        if (link2 != NULL) {
+            sah_Destroy_Link(mu, link2);
+        }
+    } else {
+        status = sah_Append_Desc(mu, desc_chain, header, link1, link2);
+    }
+
+    return status;
+}
+
+
+/**
+ * Add descriptor where first link is output, second is output
+ *
+ * @param         header      The Sahara header value for the descriptor.
+ * @param         out1        The first output buffer (or NULL)
+ * @param         out1_length Size of @a out1
+ * @param[out]    in2         The input buffer (or NULL)
+ * @param         in2_length  Size of @a in2
+ * @param         mu          Memory functions
+ * @param[in,out] desc_chain  Chain to start or append to
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_add_out_in_desc(uint32_t header,
+                                     uint8_t* out1,
+                                     uint32_t out1_length,
+                                     const uint8_t* in2,
+                                     uint32_t in2_length,
+                                     const sah_Mem_Util* mu,
+                                     sah_Head_Desc** desc_chain)
+{
+    fsl_shw_return_t status = FSL_RETURN_OK_S;
+    sah_Link         *link1 = NULL;
+    sah_Link         *link2 = NULL;
+
+
+    if (out1 != NULL) {
+        status = sah_Create_Link(mu, &link1,
+                                 (sah_Oct_Str) out1, out1_length,
+                                 SAH_OUTPUT_LINK |
+                                 SAH_USES_LINK_DATA);
+    }
+
+    if ( (in2 != NULL) && (status == FSL_RETURN_OK_S) ) {
+        status = sah_Create_Link(mu, &link2,
+                                 (sah_Oct_Str) in2, in2_length,
+                                 SAH_USES_LINK_DATA);
+    }
+
+    if (status != FSL_RETURN_OK_S) {
+        if (link1 != NULL) {
+            sah_Destroy_Link(mu, link1);
+        }
+        if (link2 != NULL) {
+            sah_Destroy_Link(mu, link2);
+        }
+    } else {
+        status = sah_Append_Desc(mu, desc_chain, header, link1, link2);
+    }
+
+    return status;
+}
+
+
+/**
+ * Add descriptor where link1 is input buffer, link2 is output buffer.
+ *
+ * @param         header     The Sahara header value for the descriptor.
+ * @param         in         The input buffer
+ * @param         in_length  Size of the input buffer
+ * @param[out]    out        The output buffer
+ * @param         out_length Size of the output buffer
+ * @param         mu         Memory functions
+ * @param[in,out] desc_chain Chain to start or append to
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_add_in_out_desc(uint32_t header,
+                                 const uint8_t* in, uint32_t in_length,
+                                 uint8_t* out, uint32_t out_length,
+                                 const sah_Mem_Util* mu,
+                                 sah_Head_Desc** desc_chain)
+{
+    fsl_shw_return_t status = FSL_RETURN_OK_S;
+    sah_Link         *link1 = NULL;
+    sah_Link         *link2 = NULL;
+
+	if (in != NULL) {
+        status = sah_Create_Link(mu, &link1,
+                                 (sah_Oct_Str) in, in_length,
+                                 SAH_USES_LINK_DATA);
+    }
+
+    if ((status == FSL_RETURN_OK_S) && (out != NULL))  {
+        status = sah_Create_Link(mu, &link2,
+                                 (sah_Oct_Str) out, out_length,
+                                 SAH_OUTPUT_LINK |
+                                 SAH_USES_LINK_DATA);
+    }
+
+    if (status != FSL_RETURN_OK_S) {
+        if (link1 != NULL) {
+            sah_Destroy_Link(mu, link1);
+        }
+        if (link2 != NULL) {
+            sah_Destroy_Link(mu, link2);
+        }
+    } else {
+        status = sah_Append_Desc(mu, desc_chain, header, link1, link2);
+    }
+
+    return status;
+}
+
+
+/**
+ * Add descriptor where link1 is a key, link2 is output buffer.
+ *
+ * @param         header     The Sahara header value for the descriptor.
+ * @param         key_info   Key information
+ * @param[out]    out        The output buffer
+ * @param         out_length Size of the output buffer
+ * @param         mu         Memory functions
+ * @param[in,out] desc_chain Chain to start or append to
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_add_key_out_desc(uint32_t header,
+								  const fsl_shw_sko_t *key_info,
+                                  uint8_t* out, uint32_t out_length,
+                                  const sah_Mem_Util *mu,
+                                  sah_Head_Desc **desc_chain)
+{
+    fsl_shw_return_t status;
+    sah_Link         *link1 = NULL;
+    sah_Link         *link2 = NULL;
+
+	status = sah_Create_Key_Link(mu, &link1, (fsl_shw_sko_t *) key_info);
+	if (status != FSL_RETURN_OK_S) {
+		goto out;
+	}
+    
+
+    if (out != NULL)  {
+        status = sah_Create_Link(mu, &link2,
+                                 (sah_Oct_Str) out, out_length,
+                                 SAH_OUTPUT_LINK |
+                                 SAH_USES_LINK_DATA);
+		if (status != FSL_RETURN_OK_S) {
+			goto out;
+	   }
+    }
+status = sah_Append_Desc(mu, desc_chain, header, link1, link2);
+	
+out:
+    if (status != FSL_RETURN_OK_S) {
+        if (link1 != NULL) {
+            sah_Destroy_Link(mu, link1);
+        }
+        if (link2 != NULL) {
+            sah_Destroy_Link(mu, link2);
+        }
+    }
+
+    return status;
+}
+
+
+/**
+ * Sanity checks the user context object fields to ensure that they make some
+ * sense before passing the uco as a parameter
+ *
+ * @brief Verify the user context object
+ *
+ * @param  uco  user context object
+ *
+ * @return    A return code of type #fsl_shw_return_t.
+ */
+fsl_shw_return_t sah_validate_uco(fsl_shw_uco_t *uco)
+{
+    fsl_shw_return_t status = FSL_RETURN_OK_S;
+
+
+    /* check if file is opened */
+    if (uco->sahara_openfd < 0) {
+        status = FSL_RETURN_NO_RESOURCE_S;
+    } else {
+        /* check flag combination: the only invalid setting of the
+         * blocking and callback flags is blocking with callback. So check
+         * for that
+         */
+        if ((uco->flags & (FSL_UCO_BLOCKING_MODE | FSL_UCO_CALLBACK_MODE)) ==
+                         (FSL_UCO_BLOCKING_MODE | FSL_UCO_CALLBACK_MODE)) {
+            status = FSL_RETURN_BAD_FLAG_S;
+        } else {
+            /* check that memory utilities have been attached */
+            if (uco->mem_util == NULL) {
+                status = FSL_RETURN_MEMORY_ERROR_S;
+            } else {
+                /* must have pool of at least 1, even for blocking mode */
+                if (uco->pool_size == 0) {
+                    status = FSL_RETURN_ERROR_S;
+                } else {
+                    /* if callback flag is set, it better have a callback
+                     * routine */
+                    if (uco->flags & FSL_UCO_CALLBACK_MODE) {
+                        if (uco->callback == NULL) {
+                            status = FSL_RETURN_INTERNAL_ERROR_S;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    return status;
+}
+
+
+/**
+ * Perform any post-processing on non-blocking results.
+ *
+ * For instance, free descriptor chains, compare authentication codes, ...
+ *
+ * @param  user_ctx     User context object
+ * @param  result_info  A set of results
+ */
+void sah_Postprocess_Results(fsl_shw_uco_t* user_ctx, sah_results* result_info)
+{
+    unsigned i;
+
+    /* for each result returned */
+    for (i = 0; i < *result_info->actual; i++) {
+        sah_Head_Desc* desc = result_info->results[i].user_desc;
+        uint8_t* out1 = desc->out1_ptr;
+        uint8_t* out2 = desc->out2_ptr;
+        uint32_t len = desc->out_len;
+
+        /*
+         * For now, tne only post-processing besides freeing the
+         * chain is the need to check the auth code for fsl_shw_auth_decrypt().
+         *
+         * If other uses are required in the future, this code will probably
+         * need a flag in the sah_Head_Desc (or a function pointer!) to
+         * determine what needs to be done.
+         */
+        if ((out1 != NULL) && (out2 != NULL)) {
+            unsigned j;
+            for (j = 0; j < len; j++) {
+                if (out1[j] != out2[j]) {
+                    /* Problem detected.  Change result. */
+                    result_info->results[i].code = FSL_RETURN_AUTH_FAILED_S;
+                    break;
+                }
+            }
+            /* free allocated 'calced_auth' */
+            user_ctx->mem_util->
+                mu_free(user_ctx->mem_util->mu_ref, out1);
+        }
+
+        /* Free the API-created chain, unless tagged as not-from-API */
+        if (! (desc->uco_flags & FSL_UCO_SAVE_DESC_CHAIN)) {
+            sah_Descriptor_Chain_Destroy(user_ctx->mem_util, &desc);
+        }
+    }
+}
+
+
+/* End of sf_util.c */
diff --git a/drivers/mxc/security/scc2_driver.c b/drivers/mxc/security/scc2_driver.c
new file mode 100644
index 000000000000..6cd837f77806
--- /dev/null
+++ b/drivers/mxc/security/scc2_driver.c
@@ -0,0 +1,2306 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*! @file scc2_driver.c
+ *
+ * This is the driver code for the Security Controller version 2 (SCC2).  It's
+ * interaction with the Linux kernel is from calls to #scc_init() when the
+ * driver is loaded, and #scc_cleanup() should the driver be unloaded.  The
+ * driver uses locking and (task-sleep/task-wakeup) functions from the kernel.
+ * It also registers itself to handle the interrupt line(s) from the SCC.  New
+ * to this version of the driver is an interface providing access to the secure
+ * partitions.  This is in turn exposed to the API user through the
+ * fsl_shw_smalloc() series of functions.  Other drivers in the kernel may use
+ * the remaining API functions to get at the services of the SCC.  The main
+ * service provided is the Secure Memory, which allows encoding and decoding of
+ * secrets with a per-chip secret key.
+ *
+ * The SCC is single-threaded, and so is this module.  When the scc_crypt()
+ * routine is called, it will lock out other accesses to the function.  If
+ * another task is already in the module, the subsequent caller will spin on a
+ * lock waiting for the other access to finish.
+ *
+ * Note that long crypto operations could cause a task to spin for a while,
+ * preventing other kernel work (other than interrupt processing) to get done.
+ *
+ * The external (kernel module) interface is through the following functions:
+ * @li scc_get_configuration() @li scc_crypt() @li scc_zeroize_memories() @li
+ * scc_monitor_security_failure() @li scc_stop_monitoring_security_failure()
+ * @li scc_set_sw_alarm() @li scc_read_register() @li scc_write_register() @li
+ * scc_allocate_partition() @li scc_initialize_partition @li
+ * scc_release_partition() @li scc_diminish_permissions @li
+ * scc_encrypt_region() @li scc_decrypt_region() @li scc_virt_to_phys
+ *
+ * All other functions are internal to the driver.
+ */
+
+#include "scc2_internals.h"
+#include "sahara2/include/portable_os.h"
+#include <linux/delay.h>
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,18))
+
+#include <linux/device.h>
+#include <mach/clock.h>
+#include <linux/device.h>
+
+#else
+
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+
+#endif
+
+#include <linux/dmapool.h>
+
+/**
+ * This is the set of errors which signal that access to the SCM RAM has
+ * failed or will fail.
+ */
+#define SCM_ACCESS_ERRORS                                                  \
+       (SCM_ERRSTAT_ILM | SCM_ERRSTAT_SUP | SCM_ERRSTAT_ERC_MASK)
+
+/******************************************************************************
+ *
+ *  Global / Static Variables
+ *
+ *****************************************************************************/
+
+#ifdef SCC_REGISTER_DEBUG
+
+#define REG_PRINT_BUFFER_SIZE 200
+
+static char reg_print_buffer[REG_PRINT_BUFFER_SIZE];
+
+typedef char *(*reg_print_routine_t) (uint32_t value, char *print_buffer,
+				      int buf_size);
+
+#endif
+
+/**
+ * This is type void* so that a) it cannot directly be dereferenced,
+ * and b) pointer arithmetic on it will function in a 'normal way' for
+ * the offsets in scc_defines.h
+ *
+ * scc_base is the location in the iomap where the SCC's registers
+ * (and memory) start.
+ *
+ * The referenced data is declared volatile so that the compiler will
+ * not make any assumptions about the value of registers in the SCC,
+ * and thus will always reload the register into CPU memory before
+ * using it (i.e. wherever it is referenced in the driver).
+ *
+ * This value should only be referenced by the #SCC_READ_REGISTER and
+ * #SCC_WRITE_REGISTER macros and their ilk.  All dereferences must be
+ * 32 bits wide.
+ */
+static volatile void *scc_base;
+
+/** Array to hold function pointers registered by
+    #scc_monitor_security_failure() and processed by
+    #scc_perform_callbacks() */
+static void (*scc_callbacks[SCC_CALLBACK_SIZE]) (void);
+/*SCC need IRAM's base address but use only the partitions allocated for it.*/
+uint32_t scm_ram_phys_base = IRAM_BASE_ADDR;
+
+void *scm_ram_base = NULL;
+
+/** Calculated once for quick reference to size of the unreserved space in
+ *  RAM in SCM.
+ */
+uint32_t scm_memory_size_bytes;
+
+/** Structure returned by #scc_get_configuration() */
+static scc_config_t scc_configuration = {
+	.driver_major_version = SCC_DRIVER_MAJOR_VERSION,
+	.driver_minor_version = SCC_DRIVER_MINOR_VERSION_2,
+	.scm_version = -1,
+	.smn_version = -1,
+	.block_size_bytes = -1,
+	.partition_size_bytes = -1,
+	.partition_count = -1,
+};
+
+/** Internal flag to know whether SCC is in Failed state (and thus many
+ *  registers are unavailable).  Once it goes failed, it never leaves it. */
+static volatile enum scc_status scc_availability = SCC_STATUS_INITIAL;
+
+/** Flag to say whether interrupt handler has been registered for
+ * SMN interrupt */
+static int smn_irq_set = 0;
+
+/** Flag to say whether interrupt handler has been registered for
+ * SCM interrupt */
+static int scm_irq_set = 0;
+
+/** This lock protects the #scc_callbacks list as well as the @c
+ * callbacks_performed flag in #scc_perform_callbacks.  Since the data this
+ * protects may be read or written from either interrupt or base level, all
+ * operations should use the irqsave/irqrestore or similar to make sure that
+ * interrupts are inhibited when locking from base level.
+ */
+static os_lock_t scc_callbacks_lock = NULL;
+
+/**
+ * Ownership of this lock prevents conflicts on the crypto operation in the
+ * SCC.
+ */
+static os_lock_t scc_crypto_lock = NULL;
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,18))
+/** Pointer to SCC's clock information.  Initialized during scc_init(). */
+static struct clk *scc_clk = NULL;
+#endif
+
+/** The lookup table for an 8-bit value.  Calculated once
+ * by #scc_init_ccitt_crc().
+ */
+static uint16_t scc_crc_lookup_table[256];
+
+/******************************************************************************
+ *
+ *  Function Implementations - Externally Accessible
+ *
+ *****************************************************************************/
+
+/**
+ * Allocate a partition of secure memory
+ *
+ * @param       smid_value  Value to use for the SMID register.  Must be 0 for
+ *                          kernel mode access.
+ * @param[out]  part_no     (If successful) Assigned partition number.
+ * @param[out]  part_base   Kernel virtual address of the partition.
+ * @param[out]  part_phys   Physical address of the partition.
+ *
+ * @return
+ */
+scc_return_t scc_allocate_partition(uint32_t smid_value,
+				    int *part_no,
+				    void **part_base, uint32_t *part_phys)
+{
+	uint32_t i;
+	os_lock_context_t irq_flags = 0;	/* for IRQ save/restore */
+	int local_part;
+	scc_return_t retval = SCC_RET_FAIL;
+	void *base_addr = NULL;
+	uint32_t reg_value;
+
+	local_part = -1;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+	if (scc_availability == SCC_STATUS_UNIMPLEMENTED) {
+		goto out;
+	}
+
+	/* ACQUIRE LOCK to prevent others from using crypto or acquiring a
+	 * partition.  Note that crypto operations could take a long time, so the
+	 * calling process could potentially spin for some time.
+	 */
+	os_lock_save_context(scc_crypto_lock, irq_flags);
+
+	do {
+		/* Find current state of partition ownership */
+		reg_value = SCC_READ_REGISTER(SCM_PART_OWNERS_REG);
+
+		/* Search for a free one */
+		for (i = 0; i < scc_configuration.partition_count; i++) {
+			if (((reg_value >> (SCM_POWN_SHIFT * i))
+			     & SCM_POWN_MASK) == SCM_POWN_PART_FREE) {
+				break;	/* found a free one */
+			}
+		}
+		if (i == local_part) {
+			/* found this one last time, and failed to allocated it */
+			pr_debug(KERN_ERR "Partition %d cannot be allocated\n",
+				 i);
+			goto out;
+		}
+		if (i >= scc_configuration.partition_count) {
+			retval = SCC_RET_INSUFFICIENT_SPACE;	/* all used up */
+			goto out;
+		}
+
+		pr_debug
+		    ("SCC2: Attempting to allocate partition %i, owners:%08x\n",
+		     i, SCC_READ_REGISTER(SCM_PART_OWNERS_REG));
+
+		local_part = i;
+		/* Store SMID to grab a partition */
+		SCC_WRITE_REGISTER(SCM_SMID0_REG +
+				   SCM_SMID_WIDTH * (local_part), smid_value);
+		mdelay(2);
+
+		/* Now make sure it is ours... ? */
+		reg_value = SCC_READ_REGISTER(SCM_PART_OWNERS_REG);
+
+		if (((reg_value >> (SCM_POWN_SHIFT * (local_part)))
+		     & SCM_POWN_MASK) != SCM_POWN_PART_OWNED) {
+			continue;	/* try for another */
+		}
+		base_addr = scm_ram_base +
+		    (local_part * scc_configuration.partition_size_bytes);
+		break;
+	} while (1);
+
+out:
+
+	/* Free the lock */
+	os_unlock_restore_context(scc_callbacks_lock, irq_flags);
+
+	/* If the base address was assigned, then a partition was successfully
+	 * acquired.
+	 */
+	if (base_addr != NULL) {
+		pr_debug("SCC2 Part owners: %08x, engaged: %08x\n",
+			 reg_value, SCC_READ_REGISTER(SCM_PART_ENGAGED_REG));
+		pr_debug("SCC2 MAP for part %d: %08x\n",
+			 local_part,
+			 SCC_READ_REGISTER(SCM_ACC0_REG + 8 * local_part));
+
+		/* Copy the partition information to the data structures passed by the
+		 * user.
+		 */
+		*part_no = local_part;
+		*part_base = base_addr;
+		*part_phys = (uint32_t) scm_ram_phys_base
+		    + (local_part * scc_configuration.partition_size_bytes);
+		retval = SCC_RET_OK;
+
+		pr_debug
+		    ("SCC2 partition engaged.  Kernel address: %p.  Physical "
+		     "address: %p, pfn: %08x\n", *part_base, (void *)*part_phys,
+		     __phys_to_pfn(*part_phys));
+	}
+
+	return retval;
+}				/* allocate_partition() */
+
+/**
+ * Release a partition of secure memory
+ *
+ * @param   part_base   Kernel virtual address of the partition to be released.
+ *
+ * @return  SCC_RET_OK if successful.
+ */
+scc_return_t scc_release_partition(void *part_base)
+{
+	uint32_t partition_no;
+
+	if (part_base == NULL) {
+		return SCC_RET_FAIL;
+	}
+
+	/* Ensure that this is a proper partition location */
+	partition_no = SCM_PART_NUMBER((uint32_t) part_base);
+
+	pr_debug("SCC2: Attempting to release partition %i, owners:%08x\n",
+		 partition_no, SCC_READ_REGISTER(SCM_PART_OWNERS_REG));
+
+	/* check that the partition is ours to de-establish */
+	if (!host_owns_partition(partition_no)) {
+		return SCC_RET_FAIL;
+	}
+
+	/* TODO: The state of the zeroize engine (SRS field in the Command Status
+	 * Register) should be examined before issuing the zeroize command here.
+	 * To make the driver thread-safe, a lock should be taken out before
+	 * issuing the check and released after the zeroize command has been
+	 * issued.
+	 */
+
+	/* Zero the partition to release it */
+	scc_write_register(SCM_ZCMD_REG,
+			   (partition_no << SCM_ZCMD_PART_SHIFT) |
+			   (ZCMD_DEALLOC_PART << SCM_ZCMD_CCMD_SHIFT));
+	mdelay(2);
+
+	pr_debug("SCC2: done releasing partition %i, owners:%08x\n",
+		 partition_no, SCC_READ_REGISTER(SCM_PART_OWNERS_REG));
+
+	/* Check that the de-assignment went correctly */
+	if (host_owns_partition(partition_no)) {
+		return SCC_RET_FAIL;
+	}
+
+	return SCC_RET_OK;
+}
+
+/**
+ * Diminish the permissions on a partition of secure memory
+ *
+ * @param   part_base     Kernel virtual address of the partition.
+ * @param   permissions   ORed values of the type SCM_PERM_* which will be used as
+ *                        initial partition permissions.  SHW API users should use
+ *                        the FSL_PERM_* definitions instead.
+ *
+ * @return  SCC_RET_OK if successful.
+ */
+scc_return_t scc_diminish_permissions(void *part_base, uint32_t permissions)
+{
+	uint32_t partition_no;
+	uint32_t permissions_requested;
+	permissions_requested = permissions;
+
+	/* ensure that this is a proper partition location */
+	partition_no = SCM_PART_NUMBER((uint32_t) part_base);
+
+	/* invert the permissions, masking out unused bits */
+	permissions = (~permissions) & SCM_PERM_MASK;
+
+	/* attempt to diminish the permissions */
+	scc_write_register(SCM_ACC0_REG + 8 * partition_no, permissions);
+	mdelay(2);
+
+	/* Reading it back puts it into the original form */
+	permissions = SCC_READ_REGISTER(SCM_ACC0_REG + 8 * partition_no);
+	if (permissions == permissions_requested) {
+		pr_debug("scc_partition_diminish_perms: successful\n");
+		pr_debug("scc_partition_diminish_perms: successful\n");
+		return SCC_RET_OK;
+	}
+	pr_debug("scc_partition_diminish_perms: not successful\n");
+
+	return SCC_RET_FAIL;
+}
+
+extern scc_partition_status_t scc_partition_status(void *part_base)
+{
+	uint32_t part_no;
+	uint32_t part_owner;
+
+	/* Determine the partition number from the address */
+	part_no = SCM_PART_NUMBER((uint32_t) part_base);
+
+	/* Check if the partition is implemented */
+	if (part_no >= scc_configuration.partition_count) {
+		return SCC_PART_S_UNUSABLE;
+	}
+
+	/* Determine the value of the partition owners register */
+	part_owner = (SCC_READ_REGISTER(SCM_PART_OWNERS_REG)
+		      >> (part_no * SCM_POWN_SHIFT)) & SCM_POWN_MASK;
+
+	switch (part_owner) {
+	case SCM_POWN_PART_OTHER:
+		return SCC_PART_S_UNAVAILABLE;
+		break;
+	case SCM_POWN_PART_FREE:
+		return SCC_PART_S_AVAILABLE;
+		break;
+	case SCM_POWN_PART_OWNED:
+		/* could be allocated or engaged*/
+		if (partition_engaged(part_no)) {
+			return SCC_PART_S_ENGAGED;
+		} else {
+			return SCC_PART_S_ALLOCATED;
+		}
+		break;
+	case SCM_POWN_PART_UNUSABLE:
+	default:
+		return SCC_PART_S_UNUSABLE;
+		break;
+	}
+}
+
+/**
+ * Calculate the physical address from the kernel virtual address.
+ *
+ * @param   address Kernel virtual address of data in an Secure Partition.
+ * @return  Physical address of said data.
+ */
+uint32_t scc_virt_to_phys(void *address)
+{
+	return (uint32_t) address - (uint32_t) scm_ram_base
+	    + (uint32_t) scm_ram_phys_base;
+}
+
+/**
+ * Engage partition of secure memory
+ *
+ * @param part_base (kernel) Virtual
+ * @param UMID NULL, or 16-byte UMID for partition security
+ * @param permissions ORed values from fsl_shw_permission_t which
+ * will be used as initial partiition permissions.
+ *
+ * @return SCC_RET_OK if successful.
+ */
+
+scc_return_t
+scc_engage_partition(void *part_base,
+		     const uint8_t *UMID, uint32_t permissions)
+{
+	uint32_t partition_no;
+	uint8_t *UMID_base = part_base + 0x10;
+	uint32_t *MAP_base = part_base;
+	uint8_t i;
+
+	partition_no = SCM_PART_NUMBER((uint32_t) part_base);
+
+	if (!host_owns_partition(partition_no) ||
+	    partition_engaged(partition_no) ||
+	    !(SCC_READ_REGISTER(SCM_SMID0_REG + (partition_no * 8)) == 0)) {
+
+		return SCC_RET_FAIL;
+	}
+
+	if (UMID != NULL) {
+		for (i = 0; i < 16; i++) {
+			UMID_base[i] = UMID[i];
+		}
+	}
+
+	MAP_base[0] = permissions;
+
+	udelay(20);
+
+	/* Check that the partition was engaged correctly, and that it has the
+	 * proper permissions.
+	 */
+
+	if ((!partition_engaged(partition_no)) ||
+	    (permissions !=
+	     SCC_READ_REGISTER(SCM_ACC0_REG + 8 * partition_no))) {
+		return SCC_RET_FAIL;
+	}
+
+	return SCC_RET_OK;
+}
+
+/*****************************************************************************/
+/* fn scc_init()                                                             */
+/*****************************************************************************/
+/**
+ *  Initialize the driver at boot time or module load time.
+ *
+ *  Register with the kernel as the interrupt handler for the SCC interrupt
+ *  line(s).
+ *
+ *  Map the SCC's register space into the driver's memory space.
+ *
+ *  Query the SCC for its configuration and status.  Save the configuration in
+ *  #scc_configuration and save the status in #scc_availability.  Called by the
+ *  kernel.
+ *
+ *  Do any locking/wait queue initialization which may be necessary.
+ *
+ *  The availability fuse may be checked, depending on platform.
+ */
+static int scc_init(void)
+{
+	uint32_t smn_status;
+	int i;
+	int return_value = -EIO;	/* assume error */
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+
+		/* Set this until we get an initial reading */
+		scc_availability = SCC_STATUS_CHECKING;
+
+		/* Initialize the constant for the CRC function */
+		scc_init_ccitt_crc();
+
+		/* initialize the callback table */
+		for (i = 0; i < SCC_CALLBACK_SIZE; i++) {
+			scc_callbacks[i] = 0;
+		}
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,18))
+		mxc_clks_enable(SCC_CLK);
+#else
+		scc_clk = clk_get(NULL, "scc_clk");
+		if (scc_clk != ERR_PTR(ENOENT)) {
+			clk_enable(scc_clk);
+		}
+#endif
+
+		/* Set up the hardware access locks */
+		scc_callbacks_lock = os_lock_alloc_init();
+		scc_crypto_lock = os_lock_alloc_init();
+		if (scc_callbacks_lock == NULL || scc_crypto_lock == NULL) {
+			os_printk(KERN_ERR
+				  "SCC2: Failed to allocate context locks.  Exiting.\n");
+			goto out;
+		}
+
+		/* See whether there is an SCC available */
+		if (0 && !SCC_ENABLED()) {
+			os_printk(KERN_ERR
+				  "SCC2: Fuse for SCC is set to disabled.  Exiting.\n");
+			goto out;
+		}
+		/* Map the SCC (SCM and SMN) memory on the internal bus into
+		   kernel address space */
+		scc_base = (void *)IO_ADDRESS(SCC_BASE);
+		if (scc_base == NULL) {
+			os_printk(KERN_ERR
+				  "SCC2: Register mapping failed.  Exiting.\n");
+			goto out;
+		}
+
+		/* If that worked, we can try to use the SCC */
+		/* Get SCM into 'clean' condition w/interrupts cleared &
+		   disabled */
+		SCC_WRITE_REGISTER(SCM_INT_CTL_REG, 0);
+
+		/* Clear error status register */
+		(void)SCC_READ_REGISTER(SCM_ERR_STATUS_REG);
+
+		/*
+		 * There is an SCC.  Determine its current state.  Side effect
+		 * is to populate scc_config and scc_availability
+		 */
+		smn_status = scc_grab_config_values();
+
+		/* Try to set up interrupt handler(s) */
+		if (scc_availability != SCC_STATUS_OK) {
+			goto out;
+		}
+
+		if (cpu_is_mx51_rev(CHIP_REV_2_0) < 0)
+			scm_ram_phys_base += 0x8000;
+
+		scm_ram_base = (void *)ioremap_nocache(scm_ram_phys_base,
+						       scc_configuration.
+						       partition_count *
+						       scc_configuration.
+						       partition_size_bytes);
+		if (scm_ram_base == NULL) {
+			os_printk(KERN_ERR
+				  "SCC2: RAM failed to remap: %p for %d bytes\n",
+				  (void *)scm_ram_phys_base,
+				  scc_configuration.partition_count *
+				  scc_configuration.partition_size_bytes);
+			goto out;
+		}
+		pr_debug("SCC2: RAM at Physical %p / Virtual %p\n",
+			 (void *)scm_ram_phys_base, scm_ram_base);
+
+		pr_debug("Secure Partition Table: Found %i partitions\n",
+			 scc_configuration.partition_count);
+
+		if (setup_interrupt_handling() != 0) {
+			unsigned err_cond;
+	    /**
+		* The error could be only that the SCM interrupt was
+		* not set up.  This interrupt is always masked, so
+		* that is not an issue.
+		* The SMN's interrupt may be shared on that line, it
+		* may be separate, or it may not be wired.  Do what
+		* is necessary to check its status.
+		* Although the driver is coded for possibility of not
+		* having SMN interrupt, the fact that there is one
+		* means it should be available and used.
+		*/
+#ifdef USE_SMN_INTERRUPT
+			err_cond = !smn_irq_set;	/* Separate. Check SMN binding */
+#elif !defined(NO_SMN_INTERRUPT)
+			err_cond = !scm_irq_set;	/* Shared. Check SCM binding */
+#else
+			err_cond = FALSE;	/*  SMN not wired at all.  Ignore. */
+#endif
+			if (err_cond) {
+				/* setup was not able to set up SMN interrupt */
+				scc_availability = SCC_STATUS_UNIMPLEMENTED;
+				goto out;
+			}
+		}
+
+		/* interrupt handling returned non-zero */
+		/* Get SMN into 'clean' condition w/interrupts cleared &
+		   enabled */
+		SCC_WRITE_REGISTER(SMN_COMMAND_REG,
+				   SMN_COMMAND_CLEAR_INTERRUPT
+				   | SMN_COMMAND_ENABLE_INTERRUPT);
+
+	      out:
+		/*
+		 * If status is SCC_STATUS_UNIMPLEMENTED or is still
+		 * SCC_STATUS_CHECKING, could be leaving here with the driver partially
+		 * initialized.  In either case, cleanup (which will mark the SCC as
+		 * UNIMPLEMENTED).
+		 */
+		if (scc_availability == SCC_STATUS_CHECKING ||
+		    scc_availability == SCC_STATUS_UNIMPLEMENTED) {
+			scc_cleanup();
+		} else {
+			return_value = 0;	/* All is well */
+		}
+	}
+	/* ! STATUS_INITIAL */
+	os_printk(KERN_ALERT "SCC2: Driver Status is %s\n",
+		  (scc_availability == SCC_STATUS_INITIAL) ? "INITIAL" :
+		  (scc_availability == SCC_STATUS_CHECKING) ? "CHECKING" :
+		  (scc_availability ==
+		   SCC_STATUS_UNIMPLEMENTED) ? "UNIMPLEMENTED"
+		  : (scc_availability ==
+		     SCC_STATUS_OK) ? "OK" : (scc_availability ==
+					      SCC_STATUS_FAILED) ? "FAILED" :
+		  "UNKNOWN");
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+			mxc_clks_disable(SCC_CLK);
+#else
+			if (scc_clk != ERR_PTR(ENOENT))
+				clk_disable(scc_clk);
+#endif
+
+	return return_value;
+}				/* scc_init */
+
+/*****************************************************************************/
+/* fn scc_cleanup()                                                          */
+/*****************************************************************************/
+/**
+ * Perform cleanup before driver/module is unloaded by setting the machine
+ * state close to what it was when the driver was loaded.  This function is
+ * called when the kernel is shutting down or when this driver is being
+ * unloaded.
+ *
+ * A driver like this should probably never be unloaded, especially if there
+ * are other module relying upon the callback feature for monitoring the SCC
+ * status.
+ *
+ * In any case, cleanup the callback table (by clearing out all of the
+ * pointers).  Deregister the interrupt handler(s).  Unmap SCC registers.
+ *
+ * Note that this will not release any partitions that have been allocated.
+ *
+ */
+static void scc_cleanup(void)
+{
+	int i;
+
+    /******************************************************/
+
+	/* Mark the driver / SCC as unusable. */
+	scc_availability = SCC_STATUS_UNIMPLEMENTED;
+
+	/* Clear out callback table */
+	for (i = 0; i < SCC_CALLBACK_SIZE; i++) {
+		scc_callbacks[i] = 0;
+	}
+
+	/* If SCC has been mapped in, clean it up and unmap it */
+	if (scc_base) {
+		/* For the SCM, disable interrupts. */
+		SCC_WRITE_REGISTER(SCM_INT_CTL_REG, 0);
+
+		/* For the SMN, clear and disable interrupts */
+		SCC_WRITE_REGISTER(SMN_COMMAND_REG,
+				   SMN_COMMAND_CLEAR_INTERRUPT);
+	}
+
+	/* Now that interrupts cannot occur, disassociate driver from the interrupt
+	 * lines.
+	 */
+
+	/* Deregister SCM interrupt handler */
+	if (scm_irq_set) {
+		os_deregister_interrupt(INT_SCC_SCM);
+	}
+
+	/* Deregister SMN interrupt handler */
+	if (smn_irq_set) {
+#ifdef USE_SMN_INTERRUPT
+		os_deregister_interrupt(INT_SCC_SMN);
+#endif
+	}
+
+	/* Finally, release the mapped memory */
+	iounmap(scm_ram_base);
+
+	if (scc_callbacks_lock != NULL)
+		os_lock_deallocate(scc_callbacks_lock);
+
+	if (scc_crypto_lock != NULL)
+		os_lock_deallocate(scc_crypto_lock);
+
+    /*Disabling SCC Clock*/
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+			mxc_clks_disable(SCC_CLK);
+#else
+			if (scc_clk != ERR_PTR(ENOENT))
+				clk_disable(scc_clk);
+			clk_put(scc_clk);
+#endif
+	pr_debug("SCC2 driver cleaned up.\n");
+
+}				/* scc_cleanup */
+
+/*****************************************************************************/
+/* fn scc_get_configuration()                                                */
+/*****************************************************************************/
+scc_config_t *scc_get_configuration(void)
+{
+	/*
+	 * If some other driver calls scc before the kernel does, make sure that
+	 * this driver's initialization is performed.
+	 */
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+    /**
+     * If there is no SCC, yet the driver exists, the value -1 will be in
+     * the #scc_config_t fields for other than the driver versions.
+     */
+	return &scc_configuration;
+}				/* scc_get_configuration */
+
+/*****************************************************************************/
+/* fn scc_zeroize_memories()                                                 */
+/*****************************************************************************/
+scc_return_t scc_zeroize_memories(void)
+{
+	scc_return_t return_status = SCC_RET_FAIL;
+
+	return return_status;
+}				/* scc_zeroize_memories */
+
+/*****************************************************************************/
+/* fn scc_set_sw_alarm()                                                     */
+/*****************************************************************************/
+void scc_set_sw_alarm(void)
+{
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	/* Update scc_availability based on current SMN status.  This might
+	 * perform callbacks.
+	 */
+	(void)scc_update_state();
+
+	/* if everything is OK, make it fail */
+	if (scc_availability == SCC_STATUS_OK) {
+
+		/* sound the alarm (and disable SMN interrupts */
+		SCC_WRITE_REGISTER(SMN_COMMAND_REG,
+				   SMN_COMMAND_SET_SOFTWARE_ALARM);
+
+		scc_availability = SCC_STATUS_FAILED;	/* Remember what we've done */
+
+		/* In case SMN interrupt is not available, tell the world */
+		scc_perform_callbacks();
+	}
+
+	return;
+}				/* scc_set_sw_alarm */
+
+/*****************************************************************************/
+/* fn scc_monitor_security_failure()                                         */
+/*****************************************************************************/
+scc_return_t scc_monitor_security_failure(void callback_func(void))
+{
+	int i;
+	os_lock_context_t irq_flags;	/* for IRQ save/restore */
+	scc_return_t return_status = SCC_RET_TOO_MANY_FUNCTIONS;
+	int function_stored = FALSE;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	/* Acquire lock of callbacks table.  Could be spin_lock_irq() if this
+	 * routine were just called from base (not interrupt) level
+	 */
+	os_lock_save_context(scc_callbacks_lock, irq_flags);
+
+	/* Search through table looking for empty slot */
+	for (i = 0; i < SCC_CALLBACK_SIZE; i++) {
+		if (scc_callbacks[i] == callback_func) {
+			if (function_stored) {
+				/* Saved duplicate earlier.  Clear this later one. */
+				scc_callbacks[i] = NULL;
+			}
+			/* Exactly one copy is now stored */
+			return_status = SCC_RET_OK;
+			break;
+		} else if (scc_callbacks[i] == NULL && !function_stored) {
+			/* Found open slot.  Save it and remember */
+			scc_callbacks[i] = callback_func;
+			return_status = SCC_RET_OK;
+			function_stored = TRUE;
+		}
+	}
+
+	/* Free the lock */
+	os_unlock_restore_context(scc_callbacks_lock, irq_flags);
+
+	return return_status;
+}				/* scc_monitor_security_failure */
+
+/*****************************************************************************/
+/* fn scc_stop_monitoring_security_failure()                                 */
+/*****************************************************************************/
+void scc_stop_monitoring_security_failure(void callback_func(void))
+{
+	os_lock_context_t irq_flags;	/* for IRQ save/restore */
+	int i;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	/* Acquire lock of callbacks table.  Could be spin_lock_irq() if this
+	 * routine were just called from base (not interrupt) level
+	 */
+	os_lock_save_context(scc_callbacks_lock, irq_flags);
+
+	/* Search every entry of the table for this function */
+	for (i = 0; i < SCC_CALLBACK_SIZE; i++) {
+		if (scc_callbacks[i] == callback_func) {
+			scc_callbacks[i] = NULL;	/* found instance - clear it out */
+			break;
+		}
+	}
+
+	/* Free the lock */
+	os_unlock_restore_context(scc_callbacks_lock, irq_flags);
+
+	return;
+}				/* scc_stop_monitoring_security_failure */
+
+/*****************************************************************************/
+/* fn scc_read_register()                                                    */
+/*****************************************************************************/
+scc_return_t scc_read_register(int register_offset, uint32_t * value)
+{
+	scc_return_t return_status = SCC_RET_FAIL;
+	uint32_t smn_status;
+	uint32_t scm_status;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	/* First layer of protection -- completely unaccessible SCC */
+	if (scc_availability != SCC_STATUS_UNIMPLEMENTED) {
+
+		/* Second layer -- that offset is valid */
+		if (register_offset != SMN_BB_DEC_REG &&	/* write only! */
+		    check_register_offset(register_offset) == SCC_RET_OK) {
+
+			/* Get current status / update local state */
+			smn_status = scc_update_state();
+			scm_status = SCC_READ_REGISTER(SCM_STATUS_REG);
+
+			/*
+			 * Third layer - verify that the register being requested is
+			 * available in the current state of the SCC.
+			 */
+			if ((return_status =
+			     check_register_accessible(register_offset,
+						       smn_status,
+						       scm_status)) ==
+			    SCC_RET_OK) {
+				*value = SCC_READ_REGISTER(register_offset);
+			}
+		}
+	}
+
+	return return_status;
+}				/* scc_read_register */
+
+/*****************************************************************************/
+/* fn scc_write_register()                                                   */
+/*****************************************************************************/
+scc_return_t scc_write_register(int register_offset, uint32_t value)
+{
+	scc_return_t return_status = SCC_RET_FAIL;
+	uint32_t smn_status;
+	uint32_t scm_status;
+
+	if (scc_availability == SCC_STATUS_INITIAL) {
+		scc_init();
+	}
+
+	/* First layer of protection -- completely unaccessible SCC */
+	if (scc_availability != SCC_STATUS_UNIMPLEMENTED) {
+
+		/* Second layer -- that offset is valid */
+		if (!((register_offset == SCM_STATUS_REG) ||	/* These registers are */
+		      (register_offset == SCM_VERSION_REG) ||	/*  Read Only */
+		      (register_offset == SMN_BB_CNT_REG) ||
+		      (register_offset == SMN_TIMER_REG)) &&
+		    check_register_offset(register_offset) == SCC_RET_OK) {
+
+			/* Get current status / update local state */
+			smn_status = scc_update_state();
+			scm_status = SCC_READ_REGISTER(SCM_STATUS_REG);
+
+			/*
+			 * Third layer - verify that the register being requested is
+			 * available in the current state of the SCC.
+			 */
+			if (check_register_accessible
+			    (register_offset, smn_status, scm_status) == 0) {
+				SCC_WRITE_REGISTER(register_offset, value);
+				return_status = SCC_RET_OK;
+			}
+		}
+	}
+
+	return return_status;
+}				/* scc_write_register() */
+
+/******************************************************************************
+ *
+ *  Function Implementations - Internal
+ *
+ *****************************************************************************/
+
+/*****************************************************************************/
+/* fn scc_irq()                                                              */
+/*****************************************************************************/
+/**
+ * This is the interrupt handler for the SCC.
+ *
+ * This function checks the SMN Status register to see whether it
+ * generated the interrupt, then it checks the SCM Status register to
+ * see whether it needs attention.
+ *
+ * If an SMN Interrupt is active, then the SCC state set to failure, and
+ * #scc_perform_callbacks() is invoked to notify any interested parties.
+ *
+ * The SCM Interrupt should be masked, as this driver uses polling to determine
+ * when the SCM has completed a crypto or zeroing operation.  Therefore, if the
+ * interrupt is active, the driver will just clear the interrupt and (re)mask.
+ */
+OS_DEV_ISR(scc_irq)
+{
+	uint32_t smn_status;
+	uint32_t scm_status;
+	int handled = 0;	/* assume interrupt isn't from SMN */
+#if defined(USE_SMN_INTERRUPT)
+	int smn_irq = INT_SCC_SMN;	/* SMN interrupt is on a line by itself */
+#elif defined (NO_SMN_INTERRUPT)
+	int smn_irq = -1;	/* not wired to CPU at all */
+#else
+	int smn_irq = INT_SCC_SCM;	/* SMN interrupt shares a line with SCM */
+#endif
+
+	/* Update current state... This will perform callbacks... */
+	smn_status = scc_update_state();
+
+	/* SMN is on its own interrupt line.  Verify the IRQ was triggered
+	 * before clearing the interrupt and marking it handled.  */
+	if ((os_dev_get_irq() == smn_irq) &&
+	    (smn_status & SMN_STATUS_SMN_STATUS_IRQ)) {
+		SCC_WRITE_REGISTER(SMN_COMMAND_REG,
+				   SMN_COMMAND_CLEAR_INTERRUPT);
+		handled++;	/* tell kernel that interrupt was handled */
+	}
+
+	/* Check on the health of the SCM */
+	scm_status = SCC_READ_REGISTER(SCM_STATUS_REG);
+
+	/* The driver masks interrupts, so this should never happen. */
+	if (os_dev_get_irq() == INT_SCC_SCM) {
+		/* but if it does, try to prevent it in the future */
+		SCC_WRITE_REGISTER(SCM_INT_CTL_REG, 0);
+		handled++;
+	}
+
+	/* Any non-zero value of handled lets kernel know we got something */
+	os_dev_isr_return(handled);
+}
+
+/*****************************************************************************/
+/* fn scc_perform_callbacks()                                                */
+/*****************************************************************************/
+/** Perform callbacks registered by #scc_monitor_security_failure().
+ *
+ *  Make sure callbacks only happen once...  Since there may be some reason why
+ *  the interrupt isn't generated, this routine could be called from base(task)
+ *  level.
+ *
+ *  One at a time, go through #scc_callbacks[] and call any non-null pointers.
+ */
+static void scc_perform_callbacks(void)
+{
+	static int callbacks_performed = 0;
+	unsigned long irq_flags;	/* for IRQ save/restore */
+	int i;
+
+	/* Acquire lock of callbacks table and callbacks_performed flag */
+	os_lock_save_context(scc_callbacks_lock, irq_flags);
+
+	if (!callbacks_performed) {
+		callbacks_performed = 1;
+
+		/* Loop over all of the entries in the table */
+		for (i = 0; i < SCC_CALLBACK_SIZE; i++) {
+			/* If not null, ... */
+			if (scc_callbacks[i]) {
+				scc_callbacks[i] ();	/* invoke the callback routine */
+			}
+		}
+	}
+
+	os_unlock_restore_context(scc_callbacks_lock, irq_flags);
+
+	return;
+}
+
+/*****************************************************************************/
+/* fn scc_update_state()                                                     */
+/*****************************************************************************/
+/**
+ * Make certain SCC is still running.
+ *
+ * Side effect is to update #scc_availability and, if the state goes to failed,
+ * run #scc_perform_callbacks().
+ *
+ * (If #SCC_BRINGUP is defined, bring SCC to secure state if it is found to be
+ * in health check state)
+ *
+ * @return Current value of #SMN_STATUS_REG register.
+ */
+static uint32_t scc_update_state(void)
+{
+	uint32_t smn_status_register = SMN_STATE_FAIL;
+	int smn_state;
+
+	/* if FAIL or UNIMPLEMENTED, don't bother */
+	if (scc_availability == SCC_STATUS_CHECKING ||
+	    scc_availability == SCC_STATUS_OK) {
+
+		smn_status_register = SCC_READ_REGISTER(SMN_STATUS_REG);
+		smn_state = smn_status_register & SMN_STATUS_STATE_MASK;
+
+#ifdef SCC_BRINGUP
+		/* If in Health Check while booting, try to 'bringup' to Secure mode */
+		if (scc_availability == SCC_STATUS_CHECKING &&
+		    smn_state == SMN_STATE_HEALTH_CHECK) {
+			/* Code up a simple algorithm for the ASC */
+			SCC_WRITE_REGISTER(SMN_SEQ_START_REG, 0xaaaa);
+			SCC_WRITE_REGISTER(SMN_SEQ_END_REG, 0x5555);
+			SCC_WRITE_REGISTER(SMN_SEQ_CHECK_REG, 0x5555);
+			/* State should be SECURE now */
+			smn_status_register = SCC_READ_REGISTER(SMN_STATUS);
+			smn_state = smn_status_register & SMN_STATUS_STATE_MASK;
+		}
+#endif
+
+		/*
+		 * State should be SECURE or NON_SECURE for operation of the part.  If
+		 * FAIL, mark failed (i.e. limited access to registers).  Any other
+		 * state, mark unimplemented, as the SCC is unuseable.
+		 */
+		if (smn_state == SMN_STATE_SECURE
+		    || smn_state == SMN_STATE_NON_SECURE) {
+			/* Healthy */
+			scc_availability = SCC_STATUS_OK;
+		} else if (smn_state == SMN_STATE_FAIL) {
+			scc_availability = SCC_STATUS_FAILED;	/* uh oh - unhealthy */
+			scc_perform_callbacks();
+			os_printk(KERN_ERR "SCC2: SCC went into FAILED mode\n");
+		} else {
+			/* START, ZEROIZE RAM, HEALTH CHECK, or unknown */
+			scc_availability = SCC_STATUS_UNIMPLEMENTED;	/* unuseable */
+			os_printk(KERN_ERR
+				  "SCC2: SCC declared UNIMPLEMENTED\n");
+		}
+	}
+	/* if availability is initial or ok */
+	return smn_status_register;
+}
+
+/*****************************************************************************/
+/* fn scc_init_ccitt_crc()                                                   */
+/*****************************************************************************/
+/**
+ * Populate the partial CRC lookup table.
+ *
+ * @return   none
+ *
+ */
+static void scc_init_ccitt_crc(void)
+{
+	int dividend;		/* index for lookup table */
+	uint16_t remainder;	/* partial value for a given dividend */
+	int bit;		/* index into bits of a byte */
+
+	/*
+	 * Compute the remainder of each possible dividend.
+	 */
+	for (dividend = 0; dividend < 256; ++dividend) {
+		/*
+		 * Start with the dividend followed by zeros.
+		 */
+		remainder = dividend << (8);
+
+		/*
+		 * Perform modulo-2 division, a bit at a time.
+		 */
+		for (bit = 8; bit > 0; --bit) {
+			/*
+			 * Try to divide the current data bit.
+			 */
+			if (remainder & 0x8000) {
+				remainder = (remainder << 1) ^ CRC_POLYNOMIAL;
+			} else {
+				remainder = (remainder << 1);
+			}
+		}
+
+		/*
+		 * Store the result into the table.
+		 */
+		scc_crc_lookup_table[dividend] = remainder;
+	}
+
+}				/* scc_init_ccitt_crc() */
+
+/*****************************************************************************/
+/* fn grab_config_values()                                                   */
+/*****************************************************************************/
+/**
+ * grab_config_values() will read the SCM Configuration and SMN Status
+ * registers and store away version and size information for later use.
+ *
+ * @return The current value of the SMN Status register.
+ */
+static uint32_t scc_grab_config_values(void)
+{
+	uint32_t scm_version_register;
+	uint32_t smn_status_register = SMN_STATE_FAIL;
+
+	if (scc_availability != SCC_STATUS_CHECKING) {
+		goto out;
+	}
+	scm_version_register = SCC_READ_REGISTER(SCM_VERSION_REG);
+	pr_debug("SCC2 Driver: SCM version is 0x%08x\n", scm_version_register);
+
+	/* Get SMN status and update scc_availability */
+	smn_status_register = scc_update_state();
+	pr_debug("SCC2 Driver: SMN status is 0x%08x\n", smn_status_register);
+
+	/* save sizes and versions information for later use */
+	scc_configuration.block_size_bytes = 16;	/* BPCP ? */
+	scc_configuration.partition_count =
+	    1 + ((scm_version_register & SCM_VER_NP_MASK) >> SCM_VER_NP_SHIFT);
+	scc_configuration.partition_size_bytes =
+	    1 << ((scm_version_register & SCM_VER_BPP_MASK) >>
+		  SCM_VER_BPP_SHIFT);
+	scc_configuration.scm_version =
+	    (scm_version_register & SCM_VER_MAJ_MASK) >> SCM_VER_MAJ_SHIFT;
+	scc_configuration.smn_version =
+	    (smn_status_register & SMN_STATUS_VERSION_ID_MASK)
+	    >> SMN_STATUS_VERSION_ID_SHIFT;
+	if (scc_configuration.scm_version != SCM_MAJOR_VERSION_2) {
+		scc_availability = SCC_STATUS_UNIMPLEMENTED;	/* Unknown version */
+	}
+
+      out:
+	return smn_status_register;
+}				/* grab_config_values */
+
+/*****************************************************************************/
+/* fn setup_interrupt_handling()                                             */
+/*****************************************************************************/
+/**
+ * Register the SCM and SMN interrupt handlers.
+ *
+ * Called from #scc_init()
+ *
+ * @return 0 on success
+ */
+static int setup_interrupt_handling(void)
+{
+	int smn_error_code = -1;
+	int scm_error_code = -1;
+
+	/* Disnable SCM interrupts */
+	SCC_WRITE_REGISTER(SCM_INT_CTL_REG, 0);
+
+#ifdef USE_SMN_INTERRUPT
+	/* Install interrupt service routine for SMN. */
+	smn_error_code = os_register_interrupt(SCC_DRIVER_NAME,
+					       INT_SCC_SMN, scc_irq);
+	if (smn_error_code != 0) {
+		os_printk(KERN_ERR
+			  "SCC2 Driver: Error installing SMN Interrupt Handler: %d\n",
+			  smn_error_code);
+	} else {
+		smn_irq_set = 1;	/* remember this for cleanup */
+		/* Enable SMN interrupts */
+		SCC_WRITE_REGISTER(SMN_COMMAND_REG,
+				   SMN_COMMAND_CLEAR_INTERRUPT |
+				   SMN_COMMAND_ENABLE_INTERRUPT);
+	}
+#else
+	smn_error_code = 0;	/* no problems... will handle later */
+#endif
+
+	/*
+	 * Install interrupt service routine for SCM (or both together).
+	 */
+	scm_error_code = os_register_interrupt(SCC_DRIVER_NAME,
+					       INT_SCC_SCM, scc_irq);
+	if (scm_error_code != 0) {
+#ifndef MXC
+		os_printk(KERN_ERR
+			  "SCC2 Driver: Error installing SCM Interrupt Handler: %d\n",
+			  scm_error_code);
+#else
+		os_printk(KERN_ERR
+			  "SCC2 Driver: Error installing SCC Interrupt Handler: %d\n",
+			  scm_error_code);
+#endif
+	} else {
+		scm_irq_set = 1;	/* remember this for cleanup */
+#if defined(USE_SMN_INTERRUPT) && !defined(NO_SMN_INTERRUPT)
+		/* Enable SMN interrupts */
+		SCC_WRITE_REGISTER(SMN_COMMAND_REG,
+				   SMN_COMMAND_CLEAR_INTERRUPT |
+				   SMN_COMMAND_ENABLE_INTERRUPT);
+#endif
+	}
+
+	/* Return an error if one was encountered */
+	return scm_error_code ? scm_error_code : smn_error_code;
+}				/* setup_interrupt_handling */
+
+/*****************************************************************************/
+/* fn scc_do_crypto()                                                        */
+/*****************************************************************************/
+/** Have the SCM perform the crypto function.
+ *
+ * Set up length register, and the store @c scm_control into control register
+ * to kick off the operation.  Wait for completion, gather status, clear
+ * interrupt / status.
+ *
+ * @param byte_count  number of bytes to perform in this operation
+ * @param scm_command Bit values to be set in @c SCM_CCMD_REG register
+ *
+ * @return 0 on success, value of #SCM_ERR_STATUS_REG on failure
+ */
+static uint32_t scc_do_crypto(int byte_count, uint32_t scm_command)
+{
+	int block_count = byte_count / SCC_BLOCK_SIZE_BYTES();
+	uint32_t crypto_status;
+	scc_return_t ret;
+
+	/* This seems to be necessary in order to allow subsequent cipher
+	 * operations to succeed when a partition is deallocated/reallocated!
+	 */
+	(void)SCC_READ_REGISTER(SCM_STATUS_REG);
+
+	/* In length register, 0 means 1, etc. */
+	scm_command |= (block_count - 1) << SCM_CCMD_LENGTH_SHIFT;
+
+	/* set modes and kick off the operation */
+	SCC_WRITE_REGISTER(SCM_CCMD_REG, scm_command);
+
+	ret = scc_wait_completion(&crypto_status);
+
+	/* Only done bit should be on */
+	if (crypto_status & SCM_STATUS_ERR) {
+		/* Replace with error status instead */
+		crypto_status = SCC_READ_REGISTER(SCM_ERR_STATUS_REG);
+		pr_debug("SCM Failure: 0x%x\n", crypto_status);
+		if (crypto_status == 0) {
+			/* That came up 0.  Turn on arbitrary bit to signal error. */
+			crypto_status = SCM_ERRSTAT_ILM;
+		}
+	} else {
+		crypto_status = 0;
+	}
+	pr_debug("SCC2: Done waiting.\n");
+
+	return crypto_status;
+}
+
+/**
+ * Encrypt a region of secure memory.
+ *
+ * @param   part_base    Kernel virtual address of the partition.
+ * @param   offset_bytes Offset from the start of the partition to the plaintext
+ *                       data.
+ * @param   byte_count   Length of the region (octets).
+ * @param   black_data   Physical location to store the encrypted data.
+ * @param   IV           Value to use for the IV.
+ * @param   cypher_mode  Cyphering mode to use, specified by type
+ *                       #scc_cypher_mode_t
+ *
+ * @return  SCC_RET_OK if successful.
+ */
+scc_return_t
+scc_encrypt_region(uint32_t part_base, uint32_t offset_bytes,
+		   uint32_t byte_count, uint8_t *black_data,
+		   uint32_t *IV, scc_cypher_mode_t cypher_mode)
+{
+	os_lock_context_t irq_flags;	/* for IRQ save/restore */
+	scc_return_t status = SCC_RET_OK;
+	uint32_t crypto_status;
+	uint32_t scm_command;
+	int offset_blocks = offset_bytes / SCC_BLOCK_SIZE_BYTES();
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+				mxc_clks_enable(SCC_CLK);
+#else
+				if (scc_clk != ERR_PTR(ENOENT))
+					clk_enable(scc_clk);
+#endif
+
+	scm_command = ((offset_blocks << SCM_CCMD_OFFSET_SHIFT) |
+		       (SCM_PART_NUMBER(part_base) << SCM_CCMD_PART_SHIFT));
+
+	switch (cypher_mode) {
+	case SCC_CYPHER_MODE_CBC:
+		scm_command |= SCM_CCMD_AES_ENC_CBC;
+		break;
+	case SCC_CYPHER_MODE_ECB:
+		scm_command |= SCM_CCMD_AES_ENC_ECB;
+		break;
+	default:
+		status = SCC_RET_FAIL;
+		break;
+	}
+
+	pr_debug("Received encrypt request.  SCM_C_BLACK_ST_REG: %p, "
+		 "scm_Command: %08x, length: %i (part_base: %08x, "
+		 "offset: %i)\n",
+		 black_data, scm_command, byte_count, part_base, offset_blocks);
+
+	if (status != SCC_RET_OK)
+		goto out;
+
+	/* ACQUIRE LOCK to prevent others from using crypto or releasing slot */
+	os_lock_save_context(scc_crypto_lock, irq_flags);
+
+	if (status == SCC_RET_OK) {
+		SCC_WRITE_REGISTER(SCM_C_BLACK_ST_REG, (uint32_t) black_data);
+
+		/* Only write the IV if it will actually be used */
+		if (cypher_mode == SCC_CYPHER_MODE_CBC) {
+			/* Write the IV register */
+			SCC_WRITE_REGISTER(SCM_AES_CBC_IV0_REG, *(IV));
+			SCC_WRITE_REGISTER(SCM_AES_CBC_IV1_REG, *(IV + 1));
+			SCC_WRITE_REGISTER(SCM_AES_CBC_IV2_REG, *(IV + 2));
+			SCC_WRITE_REGISTER(SCM_AES_CBC_IV3_REG, *(IV + 3));
+		}
+
+		/* Set modes and kick off the encryption */
+		crypto_status = scc_do_crypto(byte_count, scm_command);
+
+		if (crypto_status != 0) {
+			pr_debug("SCM encrypt red crypto failure: 0x%x\n",
+				 crypto_status);
+		} else {
+			status = SCC_RET_OK;
+			pr_debug("SCC2: Encrypted %d bytes\n", byte_count);
+		}
+	}
+
+	os_unlock_restore_context(scc_crypto_lock, irq_flags);
+
+out:
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+					mxc_clks_disable(SCC_CLK);
+#else
+					if (scc_clk != ERR_PTR(ENOENT))
+						clk_disable(scc_clk);
+#endif
+
+	return status;
+}
+
+/* Decrypt a region into secure memory
+ *
+ * @param   part_base    Kernel virtual address of the partition.
+ * @param   offset_bytes Offset from the start of the partition to store the
+ *                       plaintext data.
+ * @param   byte_counts  Length of the region (octets).
+ * @param   black_data   Physical location of the encrypted data.
+ * @param   IV           Value to use for the IV.
+ * @param   cypher_mode  Cyphering mode to use, specified by type
+ *                       #scc_cypher_mode_t
+ *
+ * @return  SCC_RET_OK if successful.
+ */
+scc_return_t
+scc_decrypt_region(uint32_t part_base, uint32_t offset_bytes,
+		   uint32_t byte_count, uint8_t *black_data,
+		   uint32_t *IV, scc_cypher_mode_t cypher_mode)
+{
+	os_lock_context_t irq_flags;	/* for IRQ save/restore */
+	scc_return_t status = SCC_RET_OK;
+	uint32_t crypto_status;
+	uint32_t scm_command;
+	int offset_blocks = offset_bytes / SCC_BLOCK_SIZE_BYTES();
+
+    /*Enabling SCC clock.*/
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+			mxc_clks_enable(SCC_CLK);
+#else
+			if (scc_clk != ERR_PTR(ENOENT))
+				clk_enable(scc_clk);
+#endif
+	scm_command = ((offset_blocks << SCM_CCMD_OFFSET_SHIFT) |
+		       (SCM_PART_NUMBER(part_base) << SCM_CCMD_PART_SHIFT));
+
+	switch (cypher_mode) {
+	case SCC_CYPHER_MODE_CBC:
+		scm_command |= SCM_CCMD_AES_DEC_CBC;
+		break;
+	case SCC_CYPHER_MODE_ECB:
+		scm_command |= SCM_CCMD_AES_DEC_ECB;
+		break;
+	default:
+		status = SCC_RET_FAIL;
+		break;
+	}
+
+	pr_debug("Received decrypt request.  SCM_C_BLACK_ST_REG: %p, "
+		 "scm_Command: %08x, length: %i (part_base: %08x, "
+		 "offset: %i)\n",
+		 black_data, scm_command, byte_count, part_base, offset_blocks);
+
+	if (status != SCC_RET_OK)
+		goto out;
+
+	/* ACQUIRE LOCK to prevent others from using crypto or releasing slot */
+	os_lock_save_context(scc_crypto_lock, irq_flags);
+
+	if (status == SCC_RET_OK) {
+		status = SCC_RET_FAIL;	/* reset expectations */
+		SCC_WRITE_REGISTER(SCM_C_BLACK_ST_REG, (uint32_t) black_data);
+
+		/* Write the IV register */
+		SCC_WRITE_REGISTER(SCM_AES_CBC_IV0_REG, *(IV));
+		SCC_WRITE_REGISTER(SCM_AES_CBC_IV1_REG, *(IV + 1));
+		SCC_WRITE_REGISTER(SCM_AES_CBC_IV2_REG, *(IV + 2));
+		SCC_WRITE_REGISTER(SCM_AES_CBC_IV3_REG, *(IV + 3));
+
+		/* Set modes and kick off the decryption */
+		crypto_status = scc_do_crypto(byte_count, scm_command);
+
+		if (crypto_status != 0) {
+			pr_debug("SCM decrypt black crypto failure: 0x%x\n",
+				 crypto_status);
+		} else {
+			status = SCC_RET_OK;
+			pr_debug("SCC2: Decrypted %d bytes\n", byte_count);
+		}
+	}
+
+	os_unlock_restore_context(scc_crypto_lock, irq_flags);
+out:
+    /*Disabling the Clock when the driver is not in use.*/
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+			mxc_clks_disable(SCC_CLK);
+#else
+			if (scc_clk != ERR_PTR(ENOENT))
+				clk_disable(scc_clk);
+#endif
+	return status;
+}
+
+/*****************************************************************************/
+/* fn host_owns_partition()                                                  */
+/*****************************************************************************/
+/**
+ * Determine if the host owns a given partition.
+ *
+ * @internal
+ *
+ * @param part_no       Partition number to query
+ *
+ * @return TRUE if the host owns the partition, FALSE otherwise.
+ */
+
+static uint32_t host_owns_partition(uint32_t part_no)
+{
+	uint32_t value;
+
+	if (part_no < scc_configuration.partition_count) {
+
+		/* Check the partition owners register */
+		value = SCC_READ_REGISTER(SCM_PART_OWNERS_REG);
+		if (((value >> (part_no * SCM_POWN_SHIFT)) & SCM_POWN_MASK)
+		    == SCM_POWN_PART_OWNED)
+			return TRUE;
+	}
+	return FALSE;
+}
+
+/*****************************************************************************/
+/* fn partition_engaged()                                                    */
+/*****************************************************************************/
+/**
+ * Determine if the given partition is engaged.
+ *
+ * @internal
+ *
+ * @param part_no       Partition number to query
+ *
+ * @return TRUE if the partition is engaged, FALSE otherwise.
+ */
+
+static uint32_t partition_engaged(uint32_t part_no)
+{
+	uint32_t value;
+
+	if (part_no < scc_configuration.partition_count) {
+
+		/* Check the partition engaged register */
+		value = SCC_READ_REGISTER(SCM_PART_ENGAGED_REG);
+		if (((value >> (part_no * SCM_PENG_SHIFT)) & 0x1)
+		    == SCM_PENG_ENGAGED)
+			return TRUE;
+	}
+	return FALSE;
+}
+
+/*****************************************************************************/
+/* fn scc_wait_completion()                                                  */
+/*****************************************************************************/
+/**
+ * Poll looking for end-of-cipher indication. Only used
+ * if @c SCC_SCM_SLEEP is not defined.
+ *
+ * @internal
+ *
+ * On a Tahiti, crypto under 230 or so bytes is done after the first loop, all
+ * the way up to five sets of spins for 1024 bytes.  (8- and 16-byte functions
+ * are done when we first look.  Zeroizing takes one pass around.
+ *
+ * @param   scm_status      Address of the SCM_STATUS register
+ *
+ * @return  A return code of type #scc_return_t
+ */
+static scc_return_t scc_wait_completion(uint32_t * scm_status)
+{
+	scc_return_t ret;
+	int done;
+	int i = 0;
+
+	/* check for completion by polling */
+	do {
+		done = is_cipher_done(scm_status);
+		if (done)
+			break;
+		/* TODO: shorten this delay */
+		udelay(1000);
+	} while (i++ < SCC_CIPHER_MAX_POLL_COUNT);
+
+	pr_debug("SCC2: Polled DONE %d times\n", i);
+	if (!done) {
+		ret = SCC_RET_FAIL;
+	}
+
+	return ret;
+}				/* scc_wait_completion() */
+
+/*****************************************************************************/
+/* fn is_cipher_done()                                                       */
+/*****************************************************************************/
+/**
+ * This function returns non-zero if SCM Status register indicates
+ * that a cipher has terminated or some other interrupt-generating
+ * condition has occurred.
+ *
+ * @param scm_status    Address of the SCM STATUS register
+ *
+ * @return  0 if cipher operations are finished
+ */
+static int is_cipher_done(uint32_t * scm_status)
+{
+	register unsigned status;
+	register int cipher_done;
+
+	*scm_status = SCC_READ_REGISTER(SCM_STATUS_REG);
+	status = (*scm_status & SCM_STATUS_SRS_MASK) >> SCM_STATUS_SRS_SHIFT;
+
+	/*
+	 * Done when SCM is not in 'currently performing a function' states.
+	 */
+	cipher_done = ((status != SCM_STATUS_SRS_ZBUSY)
+		       && (status != SCM_STATUS_SRS_CBUSY)
+		       && (status != SCM_STATUS_SRS_ABUSY));
+
+	return cipher_done;
+}				/* is_cipher_done() */
+
+/*****************************************************************************/
+/* fn offset_within_smn()                                                    */
+/*****************************************************************************/
+/*!
+ *  Check that the offset is with the bounds of the SMN register set.
+ *
+ *  @param[in]  register_offset    register offset of SMN.
+ *
+ *  @return   1 if true, 0 if false (not within SMN)
+ */
+static inline int offset_within_smn(uint32_t register_offset)
+{
+	return ((register_offset >= SMN_STATUS_REG)
+		&& (register_offset <= SMN_HAC_REG));
+}
+
+/*****************************************************************************/
+/* fn offset_within_scm()                                                    */
+/*****************************************************************************/
+/*!
+ *  Check that the offset is with the bounds of the SCM register set.
+ *
+ *  @param[in]  register_offset    Register offset of SCM
+ *
+ *  @return   1 if true, 0 if false (not within SCM)
+ */
+static inline int offset_within_scm(uint32_t register_offset)
+{
+	return 1;		/* (register_offset >= SCM_RED_START)
+				   && (register_offset < scm_highest_memory_address); */
+/* Although this would cause trouble for zeroize testing, this change would
+ * close a security hole which currently allows any kernel program to access
+ * any location in RED RAM.  Perhaps enforce in non-SCC_DEBUG compiles?
+ && (register_offset <= SCM_INIT_VECTOR_1); */
+}
+
+/*****************************************************************************/
+/* fn check_register_accessible()                                            */
+/*****************************************************************************/
+/**
+ *  Given the current SCM and SMN status, verify that access to the requested
+ *  register should be OK.
+ *
+ *  @param[in]   register_offset  register offset within SCC
+ *  @param[in]   smn_status  recent value from #SMN_STATUS_REG
+ *  @param[in]   scm_status  recent value from #SCM_STATUS_REG
+ *
+ *  @return   #SCC_RET_OK if ok, #SCC_RET_FAIL if not
+ */
+static scc_return_t
+check_register_accessible(uint32_t register_offset, uint32_t smn_status,
+			  uint32_t scm_status)
+{
+	int error_code = SCC_RET_FAIL;
+
+	/* Verify that the register offset passed in is not among the verboten set
+	 * if the SMN is in Fail mode.
+	 */
+	if (offset_within_smn(register_offset)) {
+		if ((smn_status & SMN_STATUS_STATE_MASK) == SMN_STATE_FAIL) {
+			if (!((register_offset == SMN_STATUS_REG) ||
+			      (register_offset == SMN_COMMAND_REG) ||
+			      (register_offset == SMN_SEC_VIO_REG))) {
+				pr_debug
+				    ("SCC2 Driver: Note: Security State is in FAIL state.\n");
+			} /* register not a safe one */
+			else {
+				/* SMN is in  FAIL, but register is a safe one */
+				error_code = SCC_RET_OK;
+			}
+		} /* State is FAIL */
+		else {
+			/* State is not fail.  All registers accessible. */
+			error_code = SCC_RET_OK;
+		}
+	}
+	/* offset within SMN */
+	/*  Not SCM register.  Check for SCM busy. */
+	else if (offset_within_scm(register_offset)) {
+		/* This is the 'cannot access' condition in the SCM */
+		if (0		/* (scm_status & SCM_STATUS_BUSY) */
+		    /* these are always available  - rest fail on busy */
+		    && !((register_offset == SCM_STATUS_REG) ||
+			 (register_offset == SCM_ERR_STATUS_REG) ||
+			 (register_offset == SCM_INT_CTL_REG) ||
+			 (register_offset == SCM_VERSION_REG))) {
+			pr_debug
+			    ("SCC2 Driver: Note: Secure Memory is in BUSY state.\n");
+		} /* status is busy & register inaccessible */
+		else {
+			error_code = SCC_RET_OK;
+		}
+	}
+	/* offset within SCM */
+	return error_code;
+
+}				/* check_register_accessible() */
+
+/*****************************************************************************/
+/* fn check_register_offset()                                                */
+/*****************************************************************************/
+/**
+ *  Check that the offset is with the bounds of the SCC register set.
+ *
+ *  @param[in]  register_offset    register offset of SMN.
+ *
+ * #SCC_RET_OK if ok, #SCC_RET_FAIL if not
+ */
+static scc_return_t check_register_offset(uint32_t register_offset)
+{
+	int return_value = SCC_RET_FAIL;
+
+	/* Is it valid word offset ? */
+	if (SCC_BYTE_OFFSET(register_offset) == 0) {
+		/* Yes. Is register within SCM? */
+		if (offset_within_scm(register_offset)) {
+			return_value = SCC_RET_OK;	/* yes, all ok */
+		}
+		/* Not in SCM.  Now look within the SMN */
+		else if (offset_within_smn(register_offset)) {
+			return_value = SCC_RET_OK;	/* yes, all ok */
+		}
+	}
+
+	return return_value;
+}
+
+#ifdef SCC_REGISTER_DEBUG
+
+/**
+ * Names of the SCC Registers, indexed by register number
+ */
+static char *scc_regnames[] = {
+	"SCM_VERSION_REG",
+	"0x04",
+	"SCM_INT_CTL_REG",
+	"SCM_STATUS_REG",
+	"SCM_ERR_STATUS_REG",
+	"SCM_FAULT_ADR_REG",
+	"SCM_PART_OWNERS_REG",
+	"SCM_PART_ENGAGED_REG",
+	"SCM_UNIQUE_ID0_REG",
+	"SCM_UNIQUE_ID1_REG",
+	"SCM_UNIQUE_ID2_REG",
+	"SCM_UNIQUE_ID3_REG",
+	"0x30",
+	"0x34",
+	"0x38",
+	"0x3C",
+	"0x40",
+	"0x44",
+	"0x48",
+	"0x4C",
+	"SCM_ZCMD_REG",
+	"SCM_CCMD_REG",
+	"SCM_C_BLACK_ST_REG",
+	"SCM_DBG_STATUS_REG",
+	"SCM_AES_CBC_IV0_REG",
+	"SCM_AES_CBC_IV1_REG",
+	"SCM_AES_CBC_IV2_REG",
+	"SCM_AES_CBC_IV3_REG",
+	"0x70",
+	"0x74",
+	"0x78",
+	"0x7C",
+	"SCM_SMID0_REG",
+	"SCM_ACC0_REG",
+	"SCM_SMID1_REG",
+	"SCM_ACC1_REG",
+	"SCM_SMID2_REG",
+	"SCM_ACC2_REG",
+	"SCM_SMID3_REG",
+	"SCM_ACC3_REG",
+	"SCM_SMID4_REG",
+	"SCM_ACC4_REG",
+	"SCM_SMID5_REG",
+	"SCM_ACC5_REG",
+	"SCM_SMID6_REG",
+	"SCM_ACC6_REG",
+	"SCM_SMID7_REG",
+	"SCM_ACC7_REG",
+	"SCM_SMID8_REG",
+	"SCM_ACC8_REG",
+	"SCM_SMID9_REG",
+	"SCM_ACC9_REG",
+	"SCM_SMID10_REG",
+	"SCM_ACC10_REG",
+	"SCM_SMID11_REG",
+	"SCM_ACC11_REG",
+	"SCM_SMID12_REG",
+	"SCM_ACC12_REG",
+	"SCM_SMID13_REG",
+	"SCM_ACC13_REG",
+	"SCM_SMID14_REG",
+	"SCM_ACC14_REG",
+	"SCM_SMID15_REG",
+	"SCM_ACC15_REG",
+	"SMN_STATUS_REG",
+	"SMN_COMMAND_REG",
+	"SMN_SEQ_START_REG",
+	"SMN_SEQ_END_REG",
+	"SMN_SEQ_CHECK_REG",
+	"SMN_BB_CNT_REG",
+	"SMN_BB_INC_REG",
+	"SMN_BB_DEC_REG",
+	"SMN_COMPARE_REG",
+	"SMN_PT_CHK_REG",
+	"SMN_CT_CHK_REG",
+	"SMN_TIMER_IV_REG",
+	"SMN_TIMER_CTL_REG",
+	"SMN_SEC_VIO_REG",
+	"SMN_TIMER_REG",
+	"SMN_HAC_REG"
+};
+
+/**
+ * Names of the Secure RAM States
+ */
+static char *srs_names[] = {
+	"SRS_Reset",
+	"SRS_All_Ready",
+	"SRS_ZeroizeBusy",
+	"SRS_CipherBusy",
+	"SRS_AllBusy",
+	"SRS_ZeroizeDoneCipherReady",
+	"SRS_CipherDoneZeroizeReady",
+	"SRS_ZeroizeDoneCipherBusy",
+	"SRS_CipherDoneZeroizeBusy",
+	"SRS_UNKNOWN_STATE_9",
+	"SRS_TransitionalA",
+	"SRS_TransitionalB",
+	"SRS_TransitionalC",
+	"SRS_TransitionalD",
+	"SRS_AllDone",
+	"SRS_UNKNOWN_STATE_E",
+	"SRS_FAIL"
+};
+
+/**
+ * Create a text interpretation of the SCM Version Register
+ *
+ * @param      value        The value of the register
+ * @param[out] print_buffer Place to store the interpretation
+ * @param      buf_size     Number of bytes available at print_buffer
+ *
+ * @return The print_buffer
+ */
+static
+char *scm_print_version_reg(uint32_t value, char *print_buffer, int buf_size)
+{
+	snprintf(print_buffer, buf_size,
+		 "Bpp: %u, Bpcb: %u, np: %u, maj: %u, min: %u",
+		 (value & SCM_VER_BPP_MASK) >> SCM_VER_BPP_SHIFT,
+		 ((value & SCM_VER_BPCB_MASK) >> SCM_VER_BPCB_SHIFT) + 1,
+		 ((value & SCM_VER_NP_MASK) >> SCM_VER_NP_SHIFT) + 1,
+		 (value & SCM_VER_MAJ_MASK) >> SCM_VER_MAJ_SHIFT,
+		 (value & SCM_VER_MIN_MASK) >> SCM_VER_MIN_SHIFT);
+
+	return print_buffer;
+}
+
+/**
+ * Create a text interpretation of the SCM Status Register
+ *
+ * @param      value        The value of the register
+ * @param[out] print_buffer Place to store the interpretation
+ * @param      buf_size     Number of bytes available at print_buffer
+ *
+ * @return The print_buffer
+ */
+static
+char *scm_print_status_reg(uint32_t value, char *print_buffer, int buf_size)
+{
+
+	snprintf(print_buffer, buf_size, "%s%s%s%s%s%s%s%s%s%s%s%s%s",
+		 (value & SCM_STATUS_KST_DEFAULT_KEY) ? "KST_DefaultKey " : "",
+		 /* reserved */
+		 (value & SCM_STATUS_KST_WRONG_KEY) ? "KST_WrongKey " : "",
+		 (value & SCM_STATUS_KST_BAD_KEY) ? "KST_BadKey " : "",
+		 (value & SCM_STATUS_ERR) ? "Error " : "",
+		 (value & SCM_STATUS_MSS_FAIL) ? "MSS_FailState " : "",
+		 (value & SCM_STATUS_MSS_SEC) ? "MSS_SecureState " : "",
+		 (value & SCM_STATUS_RSS_FAIL) ? "RSS_FailState " : "",
+		 (value & SCM_STATUS_RSS_SEC) ? "RSS_SecureState " : "",
+		 (value & SCM_STATUS_RSS_INIT) ? "RSS_Initializing " : "",
+		 (value & SCM_STATUS_UNV) ? "UID_Invalid " : "",
+		 (value & SCM_STATUS_BIG) ? "BigEndian " : "",
+		 (value & SCM_STATUS_USK) ? "SecretKey " : "",
+		 srs_names[(value & SCM_STATUS_SRS_MASK) >>
+			   SCM_STATUS_SRS_SHIFT]);
+
+	return print_buffer;
+}
+
+/**
+ * Names of the SCM Error Codes
+ */
+static
+char *scm_err_code[] = {
+	"Unknown_0",
+	"UnknownAddress",
+	"UnknownCommand",
+	"ReadPermErr",
+	"WritePermErr",
+	"DMAErr",
+	"EncBlockLenOvfl",
+	"KeyNotEngaged",
+	"ZeroizeCmdQOvfl",
+	"CipherCmdQOvfl",
+	"ProcessIntr",
+	"WrongKey",
+	"DeviceBusy",
+	"DMAUnalignedAddr",
+	"Unknown_E",
+	"Unknown_F",
+};
+
+/**
+ * Names of the SMN States
+ */
+static char *smn_state_name[] = {
+	"Start",
+	"Invalid_01",
+	"Invalid_02",
+	"Invalid_03",
+	"Zeroizing_04",
+	"Zeroizing",
+	"HealthCheck",
+	"HealthCheck_07",
+	"Invalid_08",
+	"Fail",
+	"Secure",
+	"Invalid_0B",
+	"NonSecure",
+	"Invalid_0D",
+	"Invalid_0E",
+	"Invalid_0F",
+	"Invalid_10",
+	"Invalid_11",
+	"Invalid_12",
+	"Invalid_13",
+	"Invalid_14",
+	"Invalid_15",
+	"Invalid_16",
+	"Invalid_17",
+	"Invalid_18",
+	"FailHard",
+	"Invalid_1A",
+	"Invalid_1B",
+	"Invalid_1C",
+	"Invalid_1D",
+	"Invalid_1E",
+	"Invalid_1F"
+};
+
+/**
+ * Create a text interpretation of the SCM Error Status Register
+ *
+ * @param      value        The value of the register
+ * @param[out] print_buffer Place to store the interpretation
+ * @param      buf_size     Number of bytes available at print_buffer
+ *
+ * @return The print_buffer
+ */
+static
+char *scm_print_err_status_reg(uint32_t value, char *print_buffer, int buf_size)
+{
+	snprintf(print_buffer, buf_size,
+		 "MID: 0x%x, %s%s ErrorCode: %s, SMSState: %s, SCMState: %s",
+		 (value & SCM_ERRSTAT_MID_MASK) >> SCM_ERRSTAT_MID_SHIFT,
+		 (value & SCM_ERRSTAT_ILM) ? "ILM, " : "",
+		 (value & SCM_ERRSTAT_SUP) ? "SUP, " : "",
+		 scm_err_code[(value & SCM_ERRSTAT_ERC_MASK) >>
+			      SCM_ERRSTAT_ERC_SHIFT],
+		 smn_state_name[(value & SCM_ERRSTAT_SMS_MASK) >>
+				SCM_ERRSTAT_SMS_SHIFT],
+		 srs_names[(value & SCM_ERRSTAT_SRS_MASK) >>
+			   SCM_ERRSTAT_SRS_SHIFT]);
+	return print_buffer;
+}
+
+/**
+ * Create a text interpretation of the SCM Zeroize Command Register
+ *
+ * @param      value        The value of the register
+ * @param[out] print_buffer Place to store the interpretation
+ * @param      buf_size     Number of bytes available at print_buffer
+ *
+ * @return The print_buffer
+ */
+static
+char *scm_print_zcmd_reg(uint32_t value, char *print_buffer, int buf_size)
+{
+	unsigned cmd = (value & SCM_ZCMD_CCMD_MASK) >> SCM_CCMD_CCMD_SHIFT;
+
+	snprintf(print_buffer, buf_size, "%s %u",
+		 (cmd ==
+		  ZCMD_DEALLOC_PART) ? "DeallocPartition" :
+		 "(unknown function)",
+		 (value & SCM_ZCMD_PART_MASK) >> SCM_ZCMD_PART_SHIFT);
+
+	return print_buffer;
+}
+
+/**
+ * Create a text interpretation of the SCM Cipher Command Register
+ *
+ * @param      value        The value of the register
+ * @param[out] print_buffer Place to store the interpretation
+ * @param      buf_size     Number of bytes available at print_buffer
+ *
+ * @return The print_buffer
+ */
+static
+char *scm_print_ccmd_reg(uint32_t value, char *print_buffer, int buf_size)
+{
+	unsigned cmd = (value & SCM_CCMD_CCMD_MASK) >> SCM_CCMD_CCMD_SHIFT;
+
+	snprintf(print_buffer, buf_size,
+		 "%s %u bytes, %s offset 0x%x, in partition %u",
+		 (cmd == SCM_CCMD_AES_DEC_ECB) ? "ECB Decrypt" : (cmd ==
+								  SCM_CCMD_AES_ENC_ECB)
+		 ? "ECB Encrypt" : (cmd ==
+				    SCM_CCMD_AES_DEC_CBC) ? "CBC Decrypt" : (cmd
+									     ==
+									     SCM_CCMD_AES_ENC_CBC)
+		 ? "CBC Encrypt" : "(unknown function)",
+		 16 +
+		 16 * ((value & SCM_CCMD_LENGTH_MASK) >> SCM_CCMD_LENGTH_SHIFT),
+		 ((cmd == SCM_CCMD_AES_ENC_CBC)
+		  || (cmd == SCM_CCMD_AES_ENC_ECB)) ? "at" : "to",
+		 16 * ((value & SCM_CCMD_OFFSET_MASK) >> SCM_CCMD_OFFSET_SHIFT),
+		 (value & SCM_CCMD_PART_MASK) >> SCM_CCMD_PART_SHIFT);
+
+	return print_buffer;
+}
+
+/**
+ * Create a text interpretation of an SCM Access Permissions Register
+ *
+ * @param      value        The value of the register
+ * @param[out] print_buffer Place to store the interpretation
+ * @param      buf_size     Number of bytes available at print_buffer
+ *
+ * @return The print_buffer
+ */
+static
+char *scm_print_acc_reg(uint32_t value, char *print_buffer, int buf_size)
+{
+	snprintf(print_buffer, buf_size, "%s%s%s%s%s%s%s%s%s%s",
+		 (value & SCM_PERM_NO_ZEROIZE) ? "NO_ZERO " : "",
+		 (value & SCM_PERM_HD_SUP_DISABLE) ? "SUP_DIS " : "",
+		 (value & SCM_PERM_HD_READ) ? "HD_RD " : "",
+		 (value & SCM_PERM_HD_WRITE) ? "HD_WR " : "",
+		 (value & SCM_PERM_HD_EXECUTE) ? "HD_EX " : "",
+		 (value & SCM_PERM_TH_READ) ? "TH_RD " : "",
+		 (value & SCM_PERM_TH_WRITE) ? "TH_WR " : "",
+		 (value & SCM_PERM_OT_READ) ? "OT_RD " : "",
+		 (value & SCM_PERM_OT_WRITE) ? "OT_WR " : "",
+		 (value & SCM_PERM_OT_EXECUTE) ? "OT_EX" : "");
+
+	return print_buffer;
+}
+
+/**
+ * Create a text interpretation of the SCM Partitions Engaged Register
+ *
+ * @param      value        The value of the register
+ * @param[out] print_buffer Place to store the interpretation
+ * @param      buf_size     Number of bytes available at print_buffer
+ *
+ * @return The print_buffer
+ */
+static
+char *scm_print_part_eng_reg(uint32_t value, char *print_buffer, int buf_size)
+{
+	snprintf(print_buffer, buf_size, "%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s",
+		 (value & 0x8000) ? "15 " : "",
+		 (value & 0x4000) ? "14 " : "",
+		 (value & 0x2000) ? "13 " : "",
+		 (value & 0x1000) ? "12 " : "",
+		 (value & 0x0800) ? "11 " : "",
+		 (value & 0x0400) ? "10 " : "",
+		 (value & 0x0200) ? "9 " : "",
+		 (value & 0x0100) ? "8 " : "",
+		 (value & 0x0080) ? "7 " : "",
+		 (value & 0x0040) ? "6 " : "",
+		 (value & 0x0020) ? "5 " : "",
+		 (value & 0x0010) ? "4 " : "",
+		 (value & 0x0008) ? "3 " : "",
+		 (value & 0x0004) ? "2 " : "",
+		 (value & 0x0002) ? "1 " : "", (value & 0x0001) ? "0" : "");
+
+	return print_buffer;
+}
+
+/**
+ * Create a text interpretation of the SMN Status Register
+ *
+ * @param      value        The value of the register
+ * @param[out] print_buffer Place to store the interpretation
+ * @param      buf_size     Number of bytes available at print_buffer
+ *
+ * @return The print_buffer
+ */
+static
+char *smn_print_status_reg(uint32_t value, char *print_buffer, int buf_size)
+{
+	snprintf(print_buffer, buf_size,
+		 "Version %d %s%s%s%s%s%s%s%s%s%s%s%s%s",
+		 (value & SMN_STATUS_VERSION_ID_MASK) >>
+		 SMN_STATUS_VERSION_ID_SHIFT,
+		 (value & SMN_STATUS_ILLEGAL_MASTER) ? "IllMaster " : "",
+		 (value & SMN_STATUS_SCAN_EXIT) ? "ScanExit " : "",
+		 (value & SMN_STATUS_PERIP_INIT) ? "PeripInit " : "",
+		 (value & SMN_STATUS_SMN_ERROR) ? "SMNError " : "",
+		 (value & SMN_STATUS_SOFTWARE_ALARM) ? "SWAlarm " : "",
+		 (value & SMN_STATUS_TIMER_ERROR) ? "TimerErr " : "",
+		 (value & SMN_STATUS_PC_ERROR) ? "PTCTErr " : "",
+		 (value & SMN_STATUS_BITBANK_ERROR) ? "BitbankErr " : "",
+		 (value & SMN_STATUS_ASC_ERROR) ? "ASCErr " : "",
+		 (value & SMN_STATUS_SECURITY_POLICY_ERROR) ? "SecPlcyErr " :
+		 "",
+		 (value & SMN_STATUS_SEC_VIO_ACTIVE_ERROR) ? "SecVioAct " : "",
+		 (value & SMN_STATUS_INTERNAL_BOOT) ? "IntBoot " : "",
+		 smn_state_name[(value & SMN_STATUS_STATE_MASK) >>
+				SMN_STATUS_STATE_SHIFT]);
+
+	return print_buffer;
+}
+
+/**
+ * The array, indexed by register number (byte-offset / 4), of print routines
+ * for the SCC (SCM and SMN) registers.
+ */
+static reg_print_routine_t reg_printers[] = {
+	scm_print_version_reg,
+	NULL,			/* 0x04 */
+	NULL,			/* SCM_INT_CTL_REG */
+	scm_print_status_reg,
+	scm_print_err_status_reg,
+	NULL,			/* SCM_FAULT_ADR_REG */
+	NULL,			/* SCM_PART_OWNERS_REG */
+	scm_print_part_eng_reg,
+	NULL,			/* SCM_UNIQUE_ID0_REG */
+	NULL,			/* SCM_UNIQUE_ID1_REG */
+	NULL,			/* SCM_UNIQUE_ID2_REG */
+	NULL,			/* SCM_UNIQUE_ID3_REG */
+	NULL,			/* 0x30 */
+	NULL,			/* 0x34 */
+	NULL,			/* 0x38 */
+	NULL,			/* 0x3C */
+	NULL,			/* 0x40 */
+	NULL,			/* 0x44 */
+	NULL,			/* 0x48 */
+	NULL,			/* 0x4C */
+	scm_print_zcmd_reg,
+	scm_print_ccmd_reg,
+	NULL,			/* SCM_C_BLACK_ST_REG */
+	NULL,			/* SCM_DBG_STATUS_REG */
+	NULL,			/* SCM_AES_CBC_IV0_REG */
+	NULL,			/* SCM_AES_CBC_IV1_REG */
+	NULL,			/* SCM_AES_CBC_IV2_REG */
+	NULL,			/* SCM_AES_CBC_IV3_REG */
+	NULL,			/* 0x70 */
+	NULL,			/* 0x74 */
+	NULL,			/* 0x78 */
+	NULL,			/* 0x7C */
+	NULL,			/* SCM_SMID0_REG */
+	scm_print_acc_reg,	/* ACC0 */
+	NULL,			/* SCM_SMID1_REG */
+	scm_print_acc_reg,	/* ACC1 */
+	NULL,			/* SCM_SMID2_REG */
+	scm_print_acc_reg,	/* ACC2 */
+	NULL,			/* SCM_SMID3_REG */
+	scm_print_acc_reg,	/* ACC3 */
+	NULL,			/* SCM_SMID4_REG */
+	scm_print_acc_reg,	/* ACC4 */
+	NULL,			/* SCM_SMID5_REG */
+	scm_print_acc_reg,	/* ACC5 */
+	NULL,			/* SCM_SMID6_REG */
+	scm_print_acc_reg,	/* ACC6 */
+	NULL,			/* SCM_SMID7_REG */
+	scm_print_acc_reg,	/* ACC7 */
+	NULL,			/* SCM_SMID8_REG */
+	scm_print_acc_reg,	/* ACC8 */
+	NULL,			/* SCM_SMID9_REG */
+	scm_print_acc_reg,	/* ACC9 */
+	NULL,			/* SCM_SMID10_REG */
+	scm_print_acc_reg,	/* ACC10 */
+	NULL,			/* SCM_SMID11_REG */
+	scm_print_acc_reg,	/* ACC11 */
+	NULL,			/* SCM_SMID12_REG */
+	scm_print_acc_reg,	/* ACC12 */
+	NULL,			/* SCM_SMID13_REG */
+	scm_print_acc_reg,	/* ACC13 */
+	NULL,			/* SCM_SMID14_REG */
+	scm_print_acc_reg,	/* ACC14 */
+	NULL,			/* SCM_SMID15_REG */
+	scm_print_acc_reg,	/* ACC15 */
+	smn_print_status_reg,
+	NULL,			/* SMN_COMMAND_REG */
+	NULL,			/* SMN_SEQ_START_REG */
+	NULL,			/* SMN_SEQ_END_REG */
+	NULL,			/* SMN_SEQ_CHECK_REG */
+	NULL,			/* SMN_BB_CNT_REG */
+	NULL,			/* SMN_BB_INC_REG */
+	NULL,			/* SMN_BB_DEC_REG */
+	NULL,			/* SMN_COMPARE_REG */
+	NULL,			/* SMN_PT_CHK_REG */
+	NULL,			/* SMN_CT_CHK_REG */
+	NULL,			/* SMN_TIMER_IV_REG */
+	NULL,			/* SMN_TIMER_CTL_REG */
+	NULL,			/* SMN_SEC_VIO_REG */
+	NULL,			/* SMN_TIMER_REG */
+	NULL,			/* SMN_HAC_REG */
+};
+
+/*****************************************************************************/
+/* fn dbg_scc_read_register()                                                */
+/*****************************************************************************/
+/**
+ * Noisily read a 32-bit value to an SCC register.
+ * @param offset        The address of the register to read.
+ *
+ * @return  The register value
+ * */
+uint32_t dbg_scc_read_register(uint32_t offset)
+{
+	uint32_t value;
+	char *regname = scc_regnames[offset / 4];
+
+	value = __raw_readl(scc_base + offset);
+	pr_debug("SCC2 RD: 0x%03x : 0x%08x (%s) %s\n", offset, value, regname,
+		 reg_printers[offset / 4]
+		 ? reg_printers[offset / 4] (value, reg_print_buffer,
+					     REG_PRINT_BUFFER_SIZE)
+		 : "");
+
+	return value;
+}
+
+/*****************************************************************************/
+/* fn dbg_scc_write_register()                                               */
+/*****************************************************************************/
+/*
+ * Noisily read a 32-bit value to an SCC register.
+ * @param offset        The address of the register to written.
+ *
+ * @param value         The new register value
+ */
+void dbg_scc_write_register(uint32_t offset, uint32_t value)
+{
+	char *regname = scc_regnames[offset / 4];
+
+	pr_debug("SCC2 WR: 0x%03x : 0x%08x (%s) %s\n", offset, value, regname,
+		 reg_printers[offset / 4]
+		 ? reg_printers[offset / 4] (value, reg_print_buffer,
+					     REG_PRINT_BUFFER_SIZE)
+		 : "");
+	(void)__raw_writel(value, scc_base + offset);
+
+}
+
+#endif				/* SCC_REGISTER_DEBUG */
diff --git a/drivers/mxc/security/scc2_internals.h b/drivers/mxc/security/scc2_internals.h
new file mode 100644
index 000000000000..d91ac45f62bd
--- /dev/null
+++ b/drivers/mxc/security/scc2_internals.h
@@ -0,0 +1,527 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef SCC_INTERNALS_H
+#define SCC_INTERNALS_H
+
+/** @file scc2_internals.h
+ *
+ * @brief This is intended to be the file which contains most or all of the
+ * code or changes need to port the driver.  It also includes other definitions
+ * needed by the driver.
+ *
+ *  This header file should only ever be included by scc2_driver.c
+ *
+ *  Compile-time flags minimally needed:
+ *
+ *  @li Some sort of platform flag.  Currently TAHITI and MXC are understood.
+ *  @li Some start-of-SCC consideration, such as SCC_BASE_ADDR
+ *
+ *  Some changes which could be made when porting this driver:
+ *  #SCC_SPIN_COUNT
+ *
+ */
+
+#include <linux/version.h>          /* Current version Linux kernel */
+#include <linux/module.h>           /* Basic support for loadable modules,
+                                       printk */
+#include <linux/init.h>             /* module_init, module_exit */
+#include <linux/kernel.h>           /* General kernel system calls */
+#include <linux/sched.h>            /* for interrupt.h */
+#include <linux/spinlock.h>
+
+#include <linux/io.h>               /* ioremap() */
+#include <linux/interrupt.h>        /* IRQ / interrupt definitions */
+
+
+#include <linux/mxc_scc2_driver.h>
+
+#if defined(MXC)
+
+#include <mach/iim.h>
+#include <mach/mxc_scc.h>
+
+
+/**
+ * This macro is used to determine whether the SCC is enabled/available
+ * on the platform.  This macro may need to be ported.
+ */
+#define SCC_FUSE IO_ADDRESS(IIM_BASE_ADDR + MXC_IIMHWV1)
+#define SCC_ENABLED() ((SCC_FUSE & MXC_IIMHWV1_SCC_DISABLE) == 0)
+
+#else  /* neither TAHITI nor MXC */
+
+#error Do not understand target architecture
+
+#endif /* TAHITI */
+/**
+ * Define the number of Stored Keys which the SCC driver will make available.
+ * Value shall be from 0 to 20.  Default is zero (0).
+ */
+/*#define SCC_KEY_SLOTS   20*/
+
+
+/* Temporarily define compile-time flags to make Doxygen happy. */
+#ifdef DOXYGEN_HACK
+/** @addtogroup scccompileflags */
+/** @{ */
+
+
+/** @def NO_SMN_INTERRUPT
+ * The SMN interrupt is not wired to the CPU at all.
+ */
+#define NO_SMN_INTERRUPT
+
+
+/**
+ * Register an interrupt handler for the SMN as well as
+ * the SCM.  In some implementations, the SMN is not connected at all (see
+ * #NO_SMN_INTERRUPT), and in others, it is on the same interrupt line as the
+ * SCM.  When defining this flag, the SMN interrupt should be on a separate
+ * line from the SCM interrupt.
+ */
+
+#define USE_SMN_INTERRUPT
+
+
+/**
+ * Turn on generation of run-time operational, debug, and error messages
+ */
+#define SCC_DEBUG
+
+
+/**
+ * Turn on generation of run-time logging of access to the SCM and SMN
+ * registers.
+ */
+#define SCC_REGISTER_DEBUG
+
+
+/**
+ * Turn on generation of run-time logging of access to the SCM Red and
+ * Black memories.  Will only work if #SCC_REGISTER_DEBUG is also defined.
+ */
+#define SCC_RAM_DEBUG
+
+
+/**
+ *  If the driver finds the SCC in HEALTH_CHECK state, go ahead and
+ *  run a quick ASC to bring it to SECURE state.
+ */
+#define SCC_BRINGUP
+
+
+/**
+ * Expected to come from platform header files or compile command line.
+ * This symbol must be the address of the SCC
+ */
+#define SCC_BASE
+
+/**
+ *  This must be the interrupt line number of the SCM interrupt.
+ */
+#define INT_SCM
+
+/**
+ *  if #USE_SMN_INTERRUPT is defined, this must be the interrupt line number of
+ *  the SMN interrupt.
+ */
+#define INT_SMN
+
+/**
+ * Define the number of Stored Keys which the SCC driver will make available.
+ * Value shall be from 0 to 20.  Default is zero (0).
+ */
+#define SCC_KEY_SLOTS
+
+/**
+ * Make sure that this flag is defined if compiling for a Little-Endian
+ * platform.  Linux Kernel builds provide this flag.
+ */
+#define __LITTLE_ENDIAN
+
+/**
+ * Make sure that this flag is defined if compiling for a Big-Endian platform.
+ * Linux Kernel builds provide this flag.
+ */
+#define __BIG_ENDIAN
+
+/**
+ * Read a 32-bit register value from a 'peripheral'.  Standard Linux/Unix
+ * macro.
+ *
+ * @param offset   Bus address of register to be read
+ *
+ * @return The value of the register
+ */
+#define readl(offset)
+
+
+/**
+ * Write a 32-bit value to a register in a 'peripheral'.  Standard Linux/Unix
+ * macro.
+ *
+ * @param value    The 32-bit value to store
+ * @param offset   Bus address of register to be written
+ *
+ * return (none)
+ */
+#define writel(value,offset)
+
+
+/** @} */  /* end group scccompileflags */
+
+#endif  /* DOXYGEN_HACK */
+
+
+#ifndef SCC_KEY_SLOTS
+#define SCC_KEY_SLOTS 0
+
+#else
+
+#if (SCC_KEY_SLOTS < 0) || (SCC_KEY_SLOTS > 20)
+#error Bad value for SCC_KEY_SLOTS
+#endif
+
+#endif
+
+
+/**
+ * Maximum length of key/secret value which can be stored in SCC.
+ */
+#define SCC_MAX_KEY_SIZE 256
+
+
+/**
+ * This is the size, in bytes, of each key slot, and therefore the maximum size
+ * of the wrapped key.
+ */
+#define SCC_KEY_SLOT_SIZE 32
+
+
+/* These come for free with Linux, but may need to be set in a port. */
+#ifndef __BIG_ENDIAN
+#ifndef __LITTLE_ENDIAN
+#error One of __LITTLE_ENDIAN or __BIG_ENDIAN must be #defined
+#endif
+#else
+#ifdef __LITTLE_ENDIAN
+#error Exactly one of __LITTLE_ENDIAN or __BIG_ENDIAN must be #defined
+#endif
+#endif
+
+
+#ifndef SCC_CALLBACK_SIZE
+/** The number of function pointers which can be stored in #scc_callbacks.
+ *  Defaults to 4, can be overridden with compile-line argument.
+ */
+#define SCC_CALLBACK_SIZE 4
+#endif
+
+
+/** Initial CRC value for CCITT-CRC calculation. */
+#define CRC_CCITT_START 0xFFFF
+
+
+#ifdef TAHITI
+
+/**
+ * The SCC_BASE has to be SMN_BASE_ADDR on TAHITI, as the banks of
+ * registers are swapped in place.
+ */
+#define SCC_BASE       SMN_BASE_ADDR
+
+
+/** The interrupt number for the SCC (SCM only!) on Tahiti */
+#define INT_SCC_SCM       62
+
+
+/** Tahiti does not have the SMN interrupt wired to the CPU.  */
+#define NO_SMN_INTERRUPT
+
+
+#endif /* TAHITI */
+
+
+/** Number of times to spin between polling of SCC while waiting for cipher
+ *  or zeroizing function to complete. See also #SCC_CIPHER_MAX_POLL_COUNT. */
+#define SCC_SPIN_COUNT 1000
+
+
+/** Number of times to polling SCC while waiting for cipher
+ *  or zeroizing function to complete.  See also #SCC_SPIN_COUNT.  */
+#define SCC_CIPHER_MAX_POLL_COUNT 100
+
+
+/**
+ * @def SCC_READ_REGISTER
+ * Read a 32-bit value from an SCC register.  Macro which depends upon
+ * #scc_base.  Linux readl()/writel() macros operate on 32-bit quantities, as
+ * do SCC register reads/writes.
+ *
+ * @param     offset  Register offset within SCC.
+ *
+ * @return    The value from the SCC's register.
+ */
+#ifndef SCC_REGISTER_DEBUG
+#define SCC_READ_REGISTER(offset) __raw_readl(scc_base+(offset))
+#else
+#define SCC_READ_REGISTER(offset) dbg_scc_read_register(offset)
+#endif
+
+
+/**
+ * Write a 32-bit value to an SCC register.  Macro depends upon #scc_base.
+ * Linux readl()/writel() macros operate on 32-bit quantities, as do SCC
+ * register reads/writes.
+ *
+ * @param   offset  Register offset within SCC.
+ * @param   value   32-bit value to store into the register
+ *
+ * @return   (void)
+ */
+#ifndef SCC_REGISTER_DEBUG
+#define SCC_WRITE_REGISTER(offset,value) \
+	(void)__raw_writel(value, scc_base+(offset))
+#else
+#define SCC_WRITE_REGISTER(offset,value) \
+	dbg_scc_write_register(offset, value)
+#endif
+
+/**
+ * Calculate the physical address of a partition from the partition number.
+ */
+#define SCM_PART_PHYS_ADDRESS(part)                                     \
+ ((uint32_t)scm_ram_phys_base + (part*scc_configuration.partition_size_bytes))
+
+/**
+ * Calculate the kernel virtual address of a partition from the partition number.
+ */
+#define SCM_PART_ADDRESS(part)                                          \
+ (scm_ram_base + (part*scc_configuration.partition_size_bytes))
+
+/**
+ * Calculate the partition number from the kernel virtual address.
+ */
+#define SCM_PART_NUMBER(address)                                        \
+ ((address - (uint32_t)scm_ram_base)/scc_configuration.partition_size_bytes)
+
+/**
+ * Calculates the byte offset into a word
+ *  @param   bp  The byte (char*) pointer
+ *  @return      The offset (0, 1, 2, or 3)
+ */
+#define SCC_BYTE_OFFSET(bp) ((uint32_t)(bp) % sizeof(uint32_t))
+
+
+/**
+ * Converts (by rounding down) a byte pointer into a word pointer
+ *  @param  bp  The byte (char*) pointer
+ *  @return     The word (uint32_t) as though it were an aligned (uint32_t*)
+ */
+#define SCC_WORD_PTR(bp) (((uint32_t)(bp)) & ~(sizeof(uint32_t)-1))
+
+
+/**
+ * Determine number of bytes in an SCC block
+ *
+ * @return Bytes / block
+ */
+#define SCC_BLOCK_SIZE_BYTES() scc_configuration.block_size_bytes
+
+
+/**
+ * Maximum number of additional bytes which may be added in CRC+padding mode.
+ */
+#define PADDING_BUFFER_MAX_BYTES (CRC_SIZE_BYTES + sizeof(scc_block_padding))
+
+/**
+ *  Shorthand (clearer, anyway) for number of bytes in a CRC.
+ */
+#define CRC_SIZE_BYTES (sizeof(crc_t))
+
+/**
+ * The polynomial used in CCITT-CRC calculation
+ */
+#define CRC_POLYNOMIAL 0x1021
+
+/**
+ * Calculate CRC on one byte of data
+ *
+ * @param[in,out] running_crc  A value of type crc_t where CRC is kept. This
+ *                             must be an rvalue and an lvalue.
+ * @param[in]     byte_value   The byte (uint8_t, char) to be put in the CRC
+ *
+ * @return      none
+ */
+#define CALC_CRC(byte_value,running_crc)  {                                  \
+    uint8_t data;                                                            \
+    data = (0xff&(byte_value)) ^ (running_crc >> 8);                         \
+    running_crc = scc_crc_lookup_table[data] ^ (running_crc << 8);           \
+}
+
+/** Value of 'beginning of padding' marker in driver-provided padding */
+#define SCC_DRIVER_PAD_CHAR  0x80
+
+
+/** Name of the driver.  Used (on Linux, anyway) when registering interrupts */
+#define SCC_DRIVER_NAME "scc"
+
+
+/* Port -- these symbols are defined in Linux 2.6 and later.  They are defined
+ * here for backwards compatibility because this started life as a 2.4
+ * driver, and as a guide to portation to other platforms.
+ */
+
+#if !defined(LINUX_VERSION_CODE) || LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+
+#define irqreturn_t void        /* Return type of an interrupt handler */
+
+#define IRQ_HANDLED /* Would be '1' for handled -- as in return IRQ_HANDLED; */
+
+#define IRQ_NONE   /* would be '0' for not handled -- as in return IRQ_NONE; */
+
+#define IRQ_RETVAL(x) /* Return x==0 (not handled) or non-zero (handled) */
+
+#endif /* LINUX earlier than 2.5 */
+
+
+/* These are nice to have around */
+#ifndef FALSE
+#define FALSE 0
+#endif
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+
+/** Provide a typedef for the CRC which can be used in encrypt/decrypt */
+typedef uint16_t crc_t;
+
+
+/** Gives high-level view of state of the SCC */
+enum scc_status {
+    SCC_STATUS_INITIAL,         /**< State of driver before ever checking */
+    SCC_STATUS_CHECKING,        /**< Transient state while driver loading  */
+    SCC_STATUS_UNIMPLEMENTED,   /**< SCC is non-existent or unuseable  */
+    SCC_STATUS_OK,              /**< SCC is in Secure or Default state  */
+    SCC_STATUS_FAILED           /**< In Failed state */
+};
+
+/**
+ * Information about a key slot.
+ */
+struct scc_key_slot
+{
+    uint64_t   owner_id;        /**< Access control value. */
+    uint32_t   length;          /**< Length of value in slot. */
+    uint32_t   offset;          /**< Offset of value from start of RAM. */
+    uint32_t   status;          /**< 0 = unassigned, 1 = assigned. */
+    uint32_t   part_ctl;        /**< for the CCMD register  */
+};
+
+/* Forward-declare a number routines which are not part of user api */
+static int scc_init(void);
+static void scc_cleanup(void);
+
+/* Forward defines of internal functions */
+OS_DEV_ISR(scc_irq);
+/*static irqreturn_t scc_irq(int irq, void *dev_id);*/
+/** Perform callbacks registered by #scc_monitor_security_failure().
+ *
+ *  Make sure callbacks only happen once...  Since there may be some reason why
+ *  the interrupt isn't generated, this routine could be called from base(task)
+ *  level.
+ *
+ *  One at a time, go through #scc_callbacks[] and call any non-null pointers.
+ */
+static void scc_perform_callbacks(void);
+/*static uint32_t copy_to_scc(const uint8_t* from, uint32_t to, unsigned long count_bytes, uint16_t* crc);
+static uint32_t copy_from_scc(const uint32_t from, uint8_t* to,unsigned long count_bytes, uint16_t* crc);
+static scc_return_t scc_strip_padding(uint8_t* from,unsigned* count_bytes_stripped);*/
+static uint32_t scc_update_state(void);
+static void scc_init_ccitt_crc(void);
+static uint32_t scc_grab_config_values(void);
+static int setup_interrupt_handling(void);
+/**
+ * Perform an encryption on the input.  If @c verify_crc is true, a CRC must be
+ * calculated on the plaintext, and appended, with padding, before computing
+ * the ciphertext.
+ *
+ * @param[in]     count_in_bytes  Count of bytes of plaintext
+ * @param[in]     data_in         Pointer to the plaintext
+ * @param[in]     scm_control     Bit values for the SCM_CONTROL register
+ * @param[in,out] data_out        Pointer for storing ciphertext
+ * @param[in]     add_crc         Flag for computing CRC - 0 no, else yes
+ * @param[in,out] count_out_bytes Number of bytes available at @c data_out
+ */
+/*static scc_return_t scc_encrypt(uint32_t count_in_bytes, uint8_t* data_in, uint32_t scm_control, uint8_t* data_out,int add_crc, unsigned long* count_out_bytes);*/
+/**
+ * Perform a decryption on the input.  If @c verify_crc is true, the last block
+ * (maybe the two last blocks) is special - it should contain a CRC and
+ * padding.  These must be stripped and verified.
+ *
+ * @param[in]     count_in_bytes  Count of bytes of ciphertext
+ * @param[in]     data_in         Pointer to the ciphertext
+ * @param[in]     scm_control     Bit values for the SCM_CONTROL register
+ * @param[in,out] data_out        Pointer for storing plaintext
+ * @param[in]     verify_crc      Flag for running CRC - 0 no, else yes
+ * @param[in,out] count_out_bytes Number of bytes available at @c data_out
+
+ */
+/*static scc_return_t scc_decrypt(uint32_t count_in_bytes, uint8_t* data_in, uint32_t scm_control, uint8_t* data_out, int verify_crc, unsigned long* count_out_bytes);*/
+static uint32_t host_owns_partition(uint32_t part_no);
+static uint32_t partition_engaged(uint32_t part_no);
+
+static scc_return_t scc_wait_completion(uint32_t* scm_status);
+static int is_cipher_done(uint32_t* scm_status);
+static scc_return_t check_register_accessible (uint32_t offset,
+                                               uint32_t smn_status,
+                                               uint32_t scm_status);
+static scc_return_t check_register_offset(uint32_t offset);
+/*uint8_t make_vpu_partition(void);*/
+
+#ifdef SCC_REGISTER_DEBUG
+static uint32_t dbg_scc_read_register(uint32_t offset);
+static void dbg_scc_write_register(uint32_t offset, uint32_t value);
+#endif
+
+
+/* For Linux kernel, export the API functions to other kernel modules */
+EXPORT_SYMBOL(scc_get_configuration);
+EXPORT_SYMBOL(scc_zeroize_memories);
+/*EXPORT_SYMBOL(scc_crypt);*/
+EXPORT_SYMBOL(scc_set_sw_alarm);
+EXPORT_SYMBOL(scc_monitor_security_failure);
+EXPORT_SYMBOL(scc_stop_monitoring_security_failure);
+EXPORT_SYMBOL(scc_read_register);
+EXPORT_SYMBOL(scc_write_register);
+EXPORT_SYMBOL(scc_allocate_partition);
+EXPORT_SYMBOL(scc_engage_partition);
+EXPORT_SYMBOL(scc_release_partition);
+EXPORT_SYMBOL(scc_diminish_permissions);
+EXPORT_SYMBOL(scc_encrypt_region);
+EXPORT_SYMBOL(scc_decrypt_region);
+/*EXPORT_SYMBOL(make_vpu_partition);*/
+/* Tell Linux where to invoke driver at boot/module load time */
+module_init(scc_init);
+/* Tell Linux where to invoke driver on module unload  */
+module_exit(scc_cleanup);
+
+
+/* Tell Linux this is not GPL code */
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Device Driver for SCC (SMN/SCM)");
+
+
+#endif /* SCC_INTERNALS_H */
diff --git a/drivers/mxc/ssi/Kconfig b/drivers/mxc/ssi/Kconfig
new file mode 100644
index 000000000000..4cb581c2f945
--- /dev/null
+++ b/drivers/mxc/ssi/Kconfig
@@ -0,0 +1,12 @@
+#
+# SPI device configuration
+#
+
+menu "MXC SSI support"
+
+config MXC_SSI
+	tristate "SSI support"
+	---help---
+	  Say Y to get the SSI services API available on MXC platform.
+
+endmenu
diff --git a/drivers/mxc/ssi/Makefile b/drivers/mxc/ssi/Makefile
new file mode 100644
index 000000000000..f9bb4419fc19
--- /dev/null
+++ b/drivers/mxc/ssi/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for the kernel SSI device drivers.
+#
+
+obj-$(CONFIG_MXC_SSI) 		+= ssimod.o
+
+ssimod-objs := ssi.o
diff --git a/drivers/mxc/ssi/registers.h b/drivers/mxc/ssi/registers.h
new file mode 100644
index 000000000000..ba98d96c5274
--- /dev/null
+++ b/drivers/mxc/ssi/registers.h
@@ -0,0 +1,208 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+ /*!
+  * @file ../ssi/registers.h
+  * @brief This header file contains SSI driver low level definition to access module registers.
+  *
+  * @ingroup SSI
+  */
+
+#ifndef __MXC_SSI_REGISTERS_H__
+#define __MXC_SSI_REGISTERS_H__
+
+/*!
+ * This include to define bool type, false and true definitions.
+ */
+#include <mach/hardware.h>
+
+#define SPBA_CPU_SSI            0x07
+
+#define    MXC_SSISTX0		0x00
+#define    MXC_SSISTX1		0x04
+#define    MXC_SSISRX0		0x08
+#define    MXC_SSISRX1   	0x0C
+#define    MXC_SSISCR           0x10
+#define    MXC_SSISISR          0x14
+#define    MXC_SSISIER   	0x18
+#define    MXC_SSISTCR   	0x1C
+#define    MXC_SSISRCR   	0x20
+#define    MXC_SSISTCCR  	0x24
+#define    MXC_SSISRCCR  	0x28
+#define    MXC_SSISFCSR  	0x2C
+#define    MXC_SSISTR           0x30
+#define    MXC_SSISOR           0x34
+#define    MXC_SSISACNT  	0x38
+#define    MXC_SSISACADD 	0x3C
+#define    MXC_SSISACDAT 	0x40
+#define    MXC_SSISATAG  	0x44
+#define    MXC_SSISTMSK  	0x48
+#define    MXC_SSISRMSK  	0x4C
+
+/* MXC 91221 only */
+#define    MXC_SSISACCST  	0x50
+#define    MXC_SSISACCEN  	0x54
+#define    MXC_SSISACCDIS  	0x58
+
+/*! SSI1 registers offset*/
+#define    MXC_SSI1STX0		0x00
+#define    MXC_SSI1STX1		0x04
+#define    MXC_SSI1SRX0		0x08
+#define    MXC_SSI1SRX1   	0x0C
+#define    MXC_SSI1SCR          0x10
+#define    MXC_SSI1SISR         0x14
+#define    MXC_SSI1SIER   	0x18
+#define    MXC_SSI1STCR   	0x1C
+#define    MXC_SSI1SRCR   	0x20
+#define    MXC_SSI1STCCR  	0x24
+#define    MXC_SSI1SRCCR  	0x28
+#define    MXC_SSI1SFCSR  	0x2C
+#define    MXC_SSI1STR          0x30
+#define    MXC_SSI1SOR          0x34
+#define    MXC_SSI1SACNT  	0x38
+#define    MXC_SSI1SACADD 	0x3C
+#define    MXC_SSI1SACDAT 	0x40
+#define    MXC_SSI1SATAG  	0x44
+#define    MXC_SSI1STMSK  	0x48
+#define    MXC_SSI1SRMSK  	0x4C
+
+/* MXC91221 only */
+
+#define    MXC_SSISACCST  	0x50
+#define    MXC_SSISACCEN  	0x54
+#define    MXC_SSISACCDIS  	0x58
+
+/* Not on MXC91221 */
+/*! SSI2 registers offset*/
+#define    MXC_SSI2STX0   	0x00
+#define    MXC_SSI2STX1         0x04
+#define    MXC_SSI2SRX0    	0x08
+#define    MXC_SSI2SRX1         0x0C
+#define    MXC_SSI2SCR          0x10
+#define    MXC_SSI2SISR         0x14
+#define    MXC_SSI2SIER         0x18
+#define    MXC_SSI2STCR         0x1C
+#define    MXC_SSI2SRCR         0x20
+#define    MXC_SSI2STCCR  	0x24
+#define    MXC_SSI2SRCCR   	0x28
+#define    MXC_SSI2SFCSR        0x2C
+#define    MXC_SSI2STR    	0x30
+#define    MXC_SSI2SOR    	0x34
+#define    MXC_SSI2SACNT  	0x38
+#define    MXC_SSI2SACADD 	0x3C
+#define    MXC_SSI2SACDAT 	0x40
+#define    MXC_SSI2SATAG  	0x44
+#define    MXC_SSI2STMSK  	0x48
+#define    MXC_SSI2SRMSK  	0x4C
+
+/*!
+ * SCR Register bit shift definitions
+ */
+#define SSI_ENABLE_SHIFT            0
+#define SSI_TRANSMIT_ENABLE_SHIFT   1
+#define SSI_RECEIVE_ENABLE_SHIFT    2
+#define SSI_NETWORK_MODE_SHIFT      3
+#define SSI_SYNCHRONOUS_MODE_SHIFT  4
+#define SSI_I2S_MODE_SHIFT          5
+#define SSI_SYSTEM_CLOCK_SHIFT      7
+#define SSI_TWO_CHANNEL_SHIFT       8
+#define SSI_CLOCK_IDLE_SHIFT        9
+
+/* MXC91221 only*/
+#define SSI_TX_FRAME_CLOCK_DISABLE_SHIFT        10
+#define SSI_RX_FRAME_CLOCK_DISABLE_SHIFT        11
+
+/*!
+ * STCR & SRCR Registers bit shift definitions
+ */
+#define SSI_EARLY_FRAME_SYNC_SHIFT    0
+#define SSI_FRAME_SYNC_LENGTH_SHIFT   1
+#define SSI_FRAME_SYNC_INVERT_SHIFT   2
+#define SSI_CLOCK_POLARITY_SHIFT      3
+#define SSI_SHIFT_DIRECTION_SHIFT     4
+#define SSI_CLOCK_DIRECTION_SHIFT     5
+#define SSI_FRAME_DIRECTION_SHIFT     6
+#define SSI_FIFO_ENABLE_0_SHIFT       7
+#define SSI_FIFO_ENABLE_1_SHIFT       8
+#define SSI_BIT_0_SHIFT               9
+
+/* MXC91221 only*/
+#define SSI_TX_FRAME_CLOCK_DISABLE_SHIFT        10
+#define SSI_RX_DATA_EXTENSION_SHIFT   10	/*SRCR only */
+/*!
+ * STCCR & SRCCR Registers bit shift definitions
+ */
+#define SSI_PRESCALER_MODULUS_SHIFT          0
+#define SSI_FRAME_RATE_DIVIDER_SHIFT         8
+#define SSI_WORD_LENGTH_SHIFT               13
+#define SSI_PRESCALER_RANGE_SHIFT           17
+#define SSI_DIVIDE_BY_TWO_SHIFT             18
+#define SSI_FRAME_DIVIDER_MASK              31
+#define SSI_MIN_FRAME_DIVIDER_RATIO          1
+#define SSI_MAX_FRAME_DIVIDER_RATIO         32
+#define SSI_PRESCALER_MODULUS_MASK         255
+#define SSI_MIN_PRESCALER_MODULUS_RATIO      1
+#define SSI_MAX_PRESCALER_MODULUS_RATIO    256
+#define SSI_WORD_LENGTH_MASK                15
+
+#define SSI_IRQ_STATUS_NUMBER        25
+
+/*!
+ * SFCSR Register bit shift definitions
+ */
+#define SSI_RX_FIFO_1_COUNT_SHIFT       28
+#define SSI_TX_FIFO_1_COUNT_SHIFT       24
+#define SSI_RX_FIFO_1_WATERMARK_SHIFT   20
+#define SSI_TX_FIFO_1_WATERMARK_SHIFT   16
+#define SSI_RX_FIFO_0_COUNT_SHIFT       12
+#define SSI_TX_FIFO_0_COUNT_SHIFT        8
+#define SSI_RX_FIFO_0_WATERMARK_SHIFT    4
+#define SSI_TX_FIFO_0_WATERMARK_SHIFT    0
+#define SSI_MIN_FIFO_WATERMARK           0
+#define SSI_MAX_FIFO_WATERMARK           8
+
+/*!
+ * SSI Option Register (SOR) bit shift definitions
+ */
+#define SSI_FRAME_SYN_RESET_SHIFT        0
+#define SSI_WAIT_SHIFT                   1
+#define SSI_INIT_SHIFT                   3
+#define SSI_TRANSMITTER_CLEAR_SHIFT      4
+#define SSI_RECEIVER_CLEAR_SHIFT         5
+#define SSI_CLOCK_OFF_SHIFT              6
+#define SSI_WAIT_STATE_MASK            0x3
+
+/*!
+ * SSI AC97 Control Register (SACNT) bit shift definitions
+ */
+#define AC97_MODE_ENABLE_SHIFT           0
+#define AC97_VARIABLE_OPERATION_SHIFT    1
+#define AC97_TAG_IN_FIFO_SHIFT           2
+#define AC97_READ_COMMAND_SHIFT          3
+#define AC97_WRITE_COMMAND_SHIFT         4
+#define AC97_FRAME_RATE_DIVIDER_SHIFT    5
+#define AC97_FRAME_RATE_MASK          0x3F
+
+/*!
+ * SSI Test Register (STR) bit shift definitions
+ */
+#define SSI_TEST_MODE_SHIFT               15
+#define SSI_RCK2TCK_SHIFT                 14
+#define SSI_RFS2TFS_SHIFT                 13
+#define SSI_RXSTATE_SHIFT                 8
+#define SSI_TXD2RXD_SHIFT                 7
+#define SSI_TCK2RCK_SHIFT                 6
+#define SSI_TFS2RFS_SHIFT                 5
+#define SSI_TXSTATE_SHIFT                 0
+
+#endif				/* __MXC_SSI_REGISTERS_H__ */
diff --git a/drivers/mxc/ssi/ssi.c b/drivers/mxc/ssi/ssi.c
new file mode 100644
index 000000000000..c6b8ad5b4758
--- /dev/null
+++ b/drivers/mxc/ssi/ssi.c
@@ -0,0 +1,1239 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ssi.c
+ * @brief This file contains the implementation of the SSI driver main services
+ *
+ *
+ * @ingroup SSI
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <asm/uaccess.h>
+#include <mach/clock.h>
+
+#include "registers.h"
+#include "ssi.h"
+
+static spinlock_t ssi_lock;
+struct mxc_audio_platform_data *ssi_platform_data;
+
+EXPORT_SYMBOL(ssi_ac97_frame_rate_divider);
+EXPORT_SYMBOL(ssi_ac97_get_command_address_register);
+EXPORT_SYMBOL(ssi_ac97_get_command_data_register);
+EXPORT_SYMBOL(ssi_ac97_get_tag_register);
+EXPORT_SYMBOL(ssi_ac97_mode_enable);
+EXPORT_SYMBOL(ssi_ac97_tag_in_fifo);
+EXPORT_SYMBOL(ssi_ac97_read_command);
+EXPORT_SYMBOL(ssi_ac97_set_command_address_register);
+EXPORT_SYMBOL(ssi_ac97_set_command_data_register);
+EXPORT_SYMBOL(ssi_ac97_set_tag_register);
+EXPORT_SYMBOL(ssi_ac97_variable_mode);
+EXPORT_SYMBOL(ssi_ac97_write_command);
+EXPORT_SYMBOL(ssi_clock_idle_state);
+EXPORT_SYMBOL(ssi_clock_off);
+EXPORT_SYMBOL(ssi_enable);
+EXPORT_SYMBOL(ssi_get_data);
+EXPORT_SYMBOL(ssi_get_status);
+EXPORT_SYMBOL(ssi_i2s_mode);
+EXPORT_SYMBOL(ssi_interrupt_disable);
+EXPORT_SYMBOL(ssi_interrupt_enable);
+EXPORT_SYMBOL(ssi_network_mode);
+EXPORT_SYMBOL(ssi_receive_enable);
+EXPORT_SYMBOL(ssi_rx_bit0);
+EXPORT_SYMBOL(ssi_rx_clock_direction);
+EXPORT_SYMBOL(ssi_rx_clock_divide_by_two);
+EXPORT_SYMBOL(ssi_rx_clock_polarity);
+EXPORT_SYMBOL(ssi_rx_clock_prescaler);
+EXPORT_SYMBOL(ssi_rx_early_frame_sync);
+EXPORT_SYMBOL(ssi_rx_fifo_counter);
+EXPORT_SYMBOL(ssi_rx_fifo_enable);
+EXPORT_SYMBOL(ssi_rx_fifo_full_watermark);
+EXPORT_SYMBOL(ssi_rx_flush_fifo);
+EXPORT_SYMBOL(ssi_rx_frame_direction);
+EXPORT_SYMBOL(ssi_rx_frame_rate);
+EXPORT_SYMBOL(ssi_rx_frame_sync_active);
+EXPORT_SYMBOL(ssi_rx_frame_sync_length);
+EXPORT_SYMBOL(ssi_rx_mask_time_slot);
+EXPORT_SYMBOL(ssi_rx_prescaler_modulus);
+EXPORT_SYMBOL(ssi_rx_shift_direction);
+EXPORT_SYMBOL(ssi_rx_word_length);
+EXPORT_SYMBOL(ssi_set_data);
+EXPORT_SYMBOL(ssi_set_wait_states);
+EXPORT_SYMBOL(ssi_synchronous_mode);
+EXPORT_SYMBOL(ssi_system_clock);
+EXPORT_SYMBOL(ssi_transmit_enable);
+EXPORT_SYMBOL(ssi_two_channel_mode);
+EXPORT_SYMBOL(ssi_tx_bit0);
+EXPORT_SYMBOL(ssi_tx_clock_direction);
+EXPORT_SYMBOL(ssi_tx_clock_divide_by_two);
+EXPORT_SYMBOL(ssi_tx_clock_polarity);
+EXPORT_SYMBOL(ssi_tx_clock_prescaler);
+EXPORT_SYMBOL(ssi_tx_early_frame_sync);
+EXPORT_SYMBOL(ssi_tx_fifo_counter);
+EXPORT_SYMBOL(ssi_tx_fifo_empty_watermark);
+EXPORT_SYMBOL(ssi_tx_fifo_enable);
+EXPORT_SYMBOL(ssi_tx_flush_fifo);
+EXPORT_SYMBOL(ssi_tx_frame_direction);
+EXPORT_SYMBOL(ssi_tx_frame_rate);
+EXPORT_SYMBOL(ssi_tx_frame_sync_active);
+EXPORT_SYMBOL(ssi_tx_frame_sync_length);
+EXPORT_SYMBOL(ssi_tx_mask_time_slot);
+EXPORT_SYMBOL(ssi_tx_prescaler_modulus);
+EXPORT_SYMBOL(ssi_tx_shift_direction);
+EXPORT_SYMBOL(ssi_tx_word_length);
+EXPORT_SYMBOL(get_ssi_fifo_addr);
+
+struct resource *res;
+void *base_addr_1;
+void *base_addr_2;
+
+unsigned int get_ssi_fifo_addr(unsigned int ssi, int direction)
+{
+	unsigned int fifo_addr;
+	if (direction == 1) {
+		if (ssi_platform_data->ssi_num == 2) {
+			fifo_addr =
+			    (ssi ==
+			     SSI1) ? (int)(base_addr_1 +
+					   MXC_SSI1STX0) : (int)(base_addr_2 +
+								 MXC_SSI2STX0);
+		} else {
+			fifo_addr = (int)(base_addr_1 + MXC_SSI1STX0);
+		}
+	} else {
+		fifo_addr = (int)(base_addr_1 + MXC_SSI1SRX0);
+	}
+	return fifo_addr;
+}
+
+unsigned int get_ssi_base_addr(unsigned int ssi)
+{
+	int base_addr;
+	if (ssi_platform_data->ssi_num == 2) {
+		base_addr =
+		    (ssi ==
+		     SSI1) ? IO_ADDRESS((int)base_addr_1) : IO_ADDRESS((int)
+								       base_addr_2);
+	} else {
+		base_addr = IO_ADDRESS((int)base_addr_1);
+	}
+	return base_addr;
+}
+
+void set_register_bits(unsigned int mask, unsigned int data,
+		       unsigned int offset, unsigned int ssi)
+{
+	volatile unsigned long reg = 0;
+	unsigned int base_addr = 0;
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&ssi_lock, flags);
+	base_addr = get_ssi_base_addr(ssi);
+	reg = __raw_readl(base_addr + offset);
+	reg = (reg & (~mask)) | data;
+	__raw_writel(reg, base_addr + offset);
+	spin_unlock_irqrestore(&ssi_lock, flags);
+}
+
+unsigned long getreg_value(unsigned int offset, unsigned int ssi)
+{
+	volatile unsigned long reg = 0;
+	unsigned int base_addr = 0;
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&ssi_lock, flags);
+	base_addr = get_ssi_base_addr(ssi);
+	reg = __raw_readl(base_addr + offset);
+	spin_unlock_irqrestore(&ssi_lock, flags);
+
+	return reg;
+}
+
+void set_register(unsigned int data, unsigned int offset, unsigned int ssi)
+{
+	unsigned int base_addr = 0;
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&ssi_lock, flags);
+	base_addr = get_ssi_base_addr(ssi);
+	__raw_writel(data, base_addr + offset);
+	spin_unlock_irqrestore(&ssi_lock, flags);
+
+}
+
+/*!
+ * This function controls the AC97 frame rate divider.
+ *
+ * @param        module               the module number
+ * @param        frame_rate_divider   the AC97 frame rate divider
+ */
+void ssi_ac97_frame_rate_divider(ssi_mod module,
+				 unsigned char frame_rate_divider)
+{
+	unsigned int reg = 0;
+
+	reg = getreg_value(MXC_SSISACNT, module);
+	reg |= ((frame_rate_divider & AC97_FRAME_RATE_MASK)
+		<< AC97_FRAME_RATE_DIVIDER_SHIFT);
+	set_register(reg, MXC_SSISACNT, module);
+}
+
+/*!
+ * This function gets the AC97 command address register.
+ *
+ * @param        module        the module number
+ * @return       This function returns the command address slot information.
+ */
+unsigned int ssi_ac97_get_command_address_register(ssi_mod module)
+{
+	return getreg_value(MXC_SSISACADD, module);
+}
+
+/*!
+ * This function gets the AC97 command data register.
+ *
+ * @param        module        the module number
+ * @return       This function returns the command data slot information.
+ */
+unsigned int ssi_ac97_get_command_data_register(ssi_mod module)
+{
+	return getreg_value(MXC_SSISACDAT, module);
+}
+
+/*!
+ * This function gets the AC97 tag register.
+ *
+ * @param        module        the module number
+ * @return       This function returns the tag information.
+ */
+unsigned int ssi_ac97_get_tag_register(ssi_mod module)
+{
+	return getreg_value(MXC_SSISATAG, module);
+}
+
+/*!
+ * This function controls the AC97 mode.
+ *
+ * @param        module        the module number
+ * @param        state         the AC97 mode state (enabled or disabled)
+ */
+void ssi_ac97_mode_enable(ssi_mod module, bool state)
+{
+	unsigned int reg = 0;
+
+	reg = getreg_value(MXC_SSISACNT, module);
+	if (state == true) {
+		reg |= (1 << AC97_MODE_ENABLE_SHIFT);
+	} else {
+		reg &= ~(1 << AC97_MODE_ENABLE_SHIFT);
+	}
+
+	set_register(reg, MXC_SSISACNT, module);
+}
+
+/*!
+ * This function controls the AC97 tag in FIFO behavior.
+ *
+ * @param        module        the module number
+ * @param        state         the tag in fifo behavior (Tag info stored in Rx FIFO 0 if true,
+ * Tag info stored in SATAG register otherwise)
+ */
+void ssi_ac97_tag_in_fifo(ssi_mod module, bool state)
+{
+	unsigned int reg = 0;
+
+	reg = getreg_value(MXC_SSISACNT, module);
+	if (state == true) {
+		reg |= (1 << AC97_TAG_IN_FIFO_SHIFT);
+	} else {
+		reg &= ~(1 << AC97_TAG_IN_FIFO_SHIFT);
+	}
+
+	set_register(reg, MXC_SSISACNT, module);
+}
+
+/*!
+ * This function controls the AC97 read command.
+ *
+ * @param        module        the module number
+ * @param        state         the next AC97 command is a read command or not
+ */
+void ssi_ac97_read_command(ssi_mod module, bool state)
+{
+	unsigned int reg = 0;
+
+	reg = getreg_value(MXC_SSISACNT, module);
+	if (state == true) {
+		reg |= (1 << AC97_READ_COMMAND_SHIFT);
+	} else {
+		reg &= ~(1 << AC97_READ_COMMAND_SHIFT);
+	}
+
+	set_register(reg, MXC_SSISACNT, module);
+}
+
+/*!
+ * This function sets the AC97 command address register.
+ *
+ * @param        module        the module number
+ * @param        address       the command address slot information
+ */
+void ssi_ac97_set_command_address_register(ssi_mod module, unsigned int address)
+{
+	set_register(address, MXC_SSISACADD, module);
+}
+
+/*!
+ * This function sets the AC97 command data register.
+ *
+ * @param        module        the module number
+ * @param        data          the command data slot information
+ */
+void ssi_ac97_set_command_data_register(ssi_mod module, unsigned int data)
+{
+	set_register(data, MXC_SSISACDAT, module);
+}
+
+/*!
+ * This function sets the AC97 tag register.
+ *
+ * @param        module        the module number
+ * @param        tag           the tag information
+ */
+void ssi_ac97_set_tag_register(ssi_mod module, unsigned int tag)
+{
+	set_register(tag, MXC_SSISATAG, module);
+}
+
+/*!
+ * This function controls the AC97 variable mode.
+ *
+ * @param        module        the module number
+ * @param        state         the AC97 variable mode state (enabled or disabled)
+ */ void ssi_ac97_variable_mode(ssi_mod module, bool state)
+{
+	unsigned int reg = 0;
+
+	reg = getreg_value(MXC_SSISACNT, module);
+	if (state == true) {
+		reg |= (1 << AC97_VARIABLE_OPERATION_SHIFT);
+	} else {
+		reg &= ~(1 << AC97_VARIABLE_OPERATION_SHIFT);
+	}
+
+	set_register(reg, MXC_SSISACNT, module);
+}
+
+/*!
+ * This function controls the AC97 write command.
+ *
+ * @param        module        the module number
+ * @param        state         the next AC97 command is a write command or not
+ */
+void ssi_ac97_write_command(ssi_mod module, bool state)
+{
+	unsigned int reg = 0;
+
+	reg = getreg_value(MXC_SSISACNT, module);
+	if (state == true) {
+		reg |= (1 << AC97_WRITE_COMMAND_SHIFT);
+	} else {
+		reg &= ~(1 << AC97_WRITE_COMMAND_SHIFT);
+	}
+
+	set_register(reg, MXC_SSISACNT, module);
+}
+
+/*!
+ * This function controls the idle state of the transmit clock port during SSI internal gated mode.
+ *
+ * @param        module        the module number
+ * @param        state         the clock idle state
+ */
+void ssi_clock_idle_state(ssi_mod module, idle_state state)
+{
+	set_register_bits(1 << SSI_CLOCK_IDLE_SHIFT,
+			  state << SSI_CLOCK_IDLE_SHIFT, MXC_SSISCR, module);
+}
+
+/*!
+ * This function turns off/on the ccm_ssi_clk to reduce power consumption.
+ *
+ * @param        module        the module number
+ * @param        state         the state for ccm_ssi_clk (true: turn off, else:turn on)
+ */
+void ssi_clock_off(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_CLOCK_OFF_SHIFT,
+			  state << SSI_CLOCK_OFF_SHIFT, MXC_SSISOR, module);
+}
+
+/*!
+ * This function enables/disables the SSI module.
+ *
+ * @param        module        the module number
+ * @param        state         the state for SSI module
+ */
+void ssi_enable(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_ENABLE_SHIFT, state << SSI_ENABLE_SHIFT,
+			  MXC_SSISCR, module);
+}
+
+/*!
+ * This function gets the data word in the Receive FIFO of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        fifo          the Receive FIFO to read
+ * @return       This function returns the read data.
+ */
+unsigned int ssi_get_data(ssi_mod module, fifo_nb fifo)
+{
+	unsigned int result = 0;
+
+	if (ssi_fifo_0 == fifo) {
+		result = getreg_value(MXC_SSISRX0, module);
+	} else {
+		result = getreg_value(MXC_SSISRX1, module);
+	}
+
+	return result;
+}
+
+/*!
+ * This function returns the status of the SSI module (SISR register) as a combination of status.
+ *
+ * @param        module        the module number
+ * @return       This function returns the status of the SSI module
+ */
+ssi_status_enable_mask ssi_get_status(ssi_mod module)
+{
+	unsigned int result;
+
+	result = getreg_value(MXC_SSISISR, module);
+	result &= ((1 << SSI_IRQ_STATUS_NUMBER) - 1);
+
+	return (ssi_status_enable_mask) result;
+}
+
+/*!
+ * This function selects the I2S mode of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        mode          the I2S mode
+ */
+void ssi_i2s_mode(ssi_mod module, mode_i2s mode)
+{
+	set_register_bits(3 << SSI_I2S_MODE_SHIFT, mode << SSI_I2S_MODE_SHIFT,
+			  MXC_SSISCR, module);
+}
+
+/*!
+ * This function disables the interrupts of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        mask          the mask of the interrupts to disable
+ */
+void ssi_interrupt_disable(ssi_mod module, ssi_status_enable_mask mask)
+{
+	set_register_bits(mask, 0, MXC_SSISIER, module);
+}
+
+/*!
+ * This function enables the interrupts of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        mask          the mask of the interrupts to enable
+ */
+void ssi_interrupt_enable(ssi_mod module, ssi_status_enable_mask mask)
+{
+	set_register_bits(0, mask, MXC_SSISIER, module);
+}
+
+/*!
+ * This function enables/disables the network mode of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        state         the network mode state
+ */
+void ssi_network_mode(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_NETWORK_MODE_SHIFT,
+			  state << SSI_NETWORK_MODE_SHIFT, MXC_SSISCR, module);
+}
+
+/*!
+ * This function enables/disables the receive section of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        state         the receive section state
+ */
+void ssi_receive_enable(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_RECEIVE_ENABLE_SHIFT,
+			  state << SSI_RECEIVE_ENABLE_SHIFT, MXC_SSISCR,
+			  module);
+}
+
+/*!
+ * This function configures the SSI module to receive data word at bit position 0 or 23 in the Receive shift register.
+ *
+ * @param        module        the module number
+ * @param        state         the state to receive at bit 0
+ */
+void ssi_rx_bit0(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_BIT_0_SHIFT, state << SSI_BIT_0_SHIFT,
+			  MXC_SSISRCR, module);
+}
+
+/*!
+ * This function controls the source of the clock signal used to clock the Receive shift register.
+ *
+ * @param        module        the module number
+ * @param        direction     the clock signal direction
+ */
+void ssi_rx_clock_direction(ssi_mod module, ssi_tx_rx_direction direction)
+{
+	set_register_bits(1 << SSI_CLOCK_DIRECTION_SHIFT,
+			  direction << SSI_CLOCK_DIRECTION_SHIFT, MXC_SSISRCR,
+			  module);
+}
+
+/*!
+ * This function configures the divide-by-two divider of the SSI module for the receive section.
+ *
+ * @param        module        the module number
+ * @param        state         the divider state
+ */
+void ssi_rx_clock_divide_by_two(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_DIVIDE_BY_TWO_SHIFT,
+			  state << SSI_DIVIDE_BY_TWO_SHIFT, MXC_SSISRCCR,
+			  module);
+}
+
+/*!
+ * This function controls which bit clock edge is used to clock in data.
+ *
+ * @param        module        the module number
+ * @param        polarity      the clock polarity
+ */
+void ssi_rx_clock_polarity(ssi_mod module, ssi_tx_rx_clock_polarity polarity)
+{
+	set_register_bits(1 << SSI_CLOCK_POLARITY_SHIFT,
+			  polarity << SSI_CLOCK_POLARITY_SHIFT, MXC_SSISRCR,
+			  module);
+}
+
+/*!
+ * This function configures a fixed divide-by-eight clock prescaler divider of the SSI module in series with the variable prescaler for the receive section.
+ *
+ * @param        module        the module number
+ * @param        state         the prescaler state
+ */
+void ssi_rx_clock_prescaler(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_PRESCALER_RANGE_SHIFT,
+			  state << SSI_PRESCALER_RANGE_SHIFT,
+			  MXC_SSISRCCR, module);
+}
+
+/*!
+ * This function controls the early frame sync configuration.
+ *
+ * @param        module        the module number
+ * @param        early         the early frame sync configuration
+ */
+void ssi_rx_early_frame_sync(ssi_mod module, ssi_tx_rx_early_frame_sync early)
+{
+	set_register_bits(1 << SSI_EARLY_FRAME_SYNC_SHIFT,
+			  early << SSI_EARLY_FRAME_SYNC_SHIFT,
+			  MXC_SSISRCR, module);
+}
+
+/*!
+ * This function gets the number of data words in the Receive FIFO.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo
+ * @return       This function returns the number of words in the Rx FIFO.
+ */
+unsigned char ssi_rx_fifo_counter(ssi_mod module, fifo_nb fifo)
+{
+	unsigned char result;
+	result = 0;
+
+	if (ssi_fifo_0 == fifo) {
+		result = getreg_value(MXC_SSISFCSR, module);
+		result &= (0xF << SSI_RX_FIFO_0_COUNT_SHIFT);
+		result = result >> SSI_RX_FIFO_0_COUNT_SHIFT;
+	} else {
+		result = getreg_value(MXC_SSISFCSR, module);
+		result &= (0xF << SSI_RX_FIFO_1_COUNT_SHIFT);
+		result = result >> SSI_RX_FIFO_1_COUNT_SHIFT;
+	}
+
+	return result;
+}
+
+/*!
+ * This function enables the Receive FIFO.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo to enable
+ * @param        enable        the state of the fifo, enabled or disabled
+ */
+
+void ssi_rx_fifo_enable(ssi_mod module, fifo_nb fifo, bool enable)
+{
+	volatile unsigned int reg;
+
+	reg = getreg_value(MXC_SSISRCR, module);
+	if (enable == true) {
+		reg |= ((1 + fifo) << SSI_FIFO_ENABLE_0_SHIFT);
+	} else {
+		reg &= ~((1 + fifo) << SSI_FIFO_ENABLE_0_SHIFT);
+	}
+
+	set_register(reg, MXC_SSISRCR, module);
+}
+
+/*!
+ * This function controls the threshold at which the RFFx flag will be set.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo to enable
+ * @param        watermark     the watermark
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_rx_fifo_full_watermark(ssi_mod module,
+			       fifo_nb fifo, unsigned char watermark)
+{
+	int result = -1;
+	result = -1;
+
+	if ((watermark > SSI_MIN_FIFO_WATERMARK) &&
+	    (watermark <= SSI_MAX_FIFO_WATERMARK)) {
+		if (ssi_fifo_0 == fifo) {
+			set_register_bits(0xf << SSI_RX_FIFO_0_WATERMARK_SHIFT,
+					  watermark <<
+					  SSI_RX_FIFO_0_WATERMARK_SHIFT,
+					  MXC_SSISFCSR, module);
+		} else {
+			set_register_bits(0xf << SSI_RX_FIFO_1_WATERMARK_SHIFT,
+					  watermark <<
+					  SSI_RX_FIFO_1_WATERMARK_SHIFT,
+					  MXC_SSISFCSR, module);
+		}
+
+		result = 0;
+	}
+
+	return result;
+}
+
+/*!
+ * This function flushes the Receive FIFOs.
+ *
+ * @param        module        the module number
+ */
+void ssi_rx_flush_fifo(ssi_mod module)
+{
+	set_register_bits(0, 1 << SSI_RECEIVER_CLEAR_SHIFT, MXC_SSISOR, module);
+}
+
+/*!
+ * This function controls the direction of the Frame Sync signal for the receive section.
+ *
+ * @param        module        the module number
+ * @param        direction     the Frame Sync signal direction
+ */
+void ssi_rx_frame_direction(ssi_mod module, ssi_tx_rx_direction direction)
+{
+	set_register_bits(1 << SSI_FRAME_DIRECTION_SHIFT,
+			  direction << SSI_FRAME_DIRECTION_SHIFT,
+			  MXC_SSISRCR, module);
+}
+
+/*!
+ * This function configures the Receive frame rate divider for the receive section.
+ *
+ * @param        module        the module number
+ * @param        ratio         the divide ratio from 1 to 32
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_rx_frame_rate(ssi_mod module, unsigned char ratio)
+{
+	int result = -1;
+
+	if ((ratio >= SSI_MIN_FRAME_DIVIDER_RATIO) &&
+	    (ratio <= SSI_MAX_FRAME_DIVIDER_RATIO)) {
+		set_register_bits(SSI_FRAME_DIVIDER_MASK <<
+				  SSI_FRAME_RATE_DIVIDER_SHIFT,
+				  (ratio - 1) << SSI_FRAME_RATE_DIVIDER_SHIFT,
+				  MXC_SSISRCCR, module);
+		result = 0;
+	}
+
+	return result;
+}
+
+/*!
+ * This function controls the Frame Sync active polarity for the receive section.
+ *
+ * @param        module        the module number
+ * @param        active        the Frame Sync active polarity
+ */
+void ssi_rx_frame_sync_active(ssi_mod module,
+			      ssi_tx_rx_frame_sync_active active)
+{
+	set_register_bits(1 << SSI_FRAME_SYNC_INVERT_SHIFT,
+			  active << SSI_FRAME_SYNC_INVERT_SHIFT,
+			  MXC_SSISRCR, module);
+}
+
+/*!
+ * This function controls the Frame Sync length (one word or one bit long) for the receive section.
+ *
+ * @param        module        the module number
+ * @param        length        the Frame Sync length
+ */
+void ssi_rx_frame_sync_length(ssi_mod module,
+			      ssi_tx_rx_frame_sync_length length)
+{
+	set_register_bits(1 << SSI_FRAME_SYNC_LENGTH_SHIFT,
+			  length << SSI_FRAME_SYNC_LENGTH_SHIFT,
+			  MXC_SSISRCR, module);
+}
+
+/*!
+ * This function configures the time slot(s) to mask for the receive section.
+ *
+ * @param        module        the module number
+ * @param        mask          the mask to indicate the time slot(s) masked
+ */
+void ssi_rx_mask_time_slot(ssi_mod module, unsigned int mask)
+{
+	set_register_bits(0xFFFFFFFF, mask, MXC_SSISRMSK, module);
+}
+
+/*!
+ * This function configures the Prescale divider for the receive section.
+ *
+ * @param        module        the module number
+ * @param        divider       the divide ratio from 1 to  256
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_rx_prescaler_modulus(ssi_mod module, unsigned int divider)
+{
+	int result = -1;
+
+	if ((divider >= SSI_MIN_PRESCALER_MODULUS_RATIO) &&
+	    (divider <= SSI_MAX_PRESCALER_MODULUS_RATIO)) {
+
+		set_register_bits(SSI_PRESCALER_MODULUS_MASK <<
+				  SSI_PRESCALER_MODULUS_SHIFT,
+				  (divider - 1) << SSI_PRESCALER_MODULUS_SHIFT,
+				  MXC_SSISRCCR, module);
+		result = 0;
+	}
+
+	return result;
+}
+
+/*!
+ * This function controls whether the MSB or LSB will be received first in a sample.
+ *
+ * @param        module        the module number
+ * @param        direction     the shift direction
+ */
+void ssi_rx_shift_direction(ssi_mod module, ssi_tx_rx_shift_direction direction)
+{
+	set_register_bits(1 << SSI_SHIFT_DIRECTION_SHIFT,
+			  direction << SSI_SHIFT_DIRECTION_SHIFT,
+			  MXC_SSISRCR, module);
+}
+
+/*!
+ * This function configures the Receive word length.
+ *
+ * @param        module        the module number
+ * @param        length        the word length
+ */
+void ssi_rx_word_length(ssi_mod module, ssi_word_length length)
+{
+	set_register_bits(SSI_WORD_LENGTH_MASK << SSI_WORD_LENGTH_SHIFT,
+			  length << SSI_WORD_LENGTH_SHIFT,
+			  MXC_SSISRCCR, module);
+}
+
+/*!
+ * This function sets the data word in the Transmit FIFO of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        fifo          the FIFO number
+ * @param        data          the data to load in the FIFO
+ */
+
+void ssi_set_data(ssi_mod module, fifo_nb fifo, unsigned int data)
+{
+	if (ssi_fifo_0 == fifo) {
+		set_register(data, MXC_SSISTX0, module);
+	} else {
+		set_register(data, MXC_SSISTX1, module);
+	}
+}
+
+/*!
+ * This function controls the number of wait states between the core and SSI.
+ *
+ * @param        module        the module number
+ * @param        wait          the number of wait state(s)
+ */
+void ssi_set_wait_states(ssi_mod module, ssi_wait_states wait)
+{
+	set_register_bits(SSI_WAIT_STATE_MASK << SSI_WAIT_SHIFT,
+			  wait << SSI_WAIT_SHIFT, MXC_SSISOR, module);
+}
+
+/*!
+ * This function enables/disables the synchronous mode of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        state         the synchronous mode state
+ */
+void ssi_synchronous_mode(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_SYNCHRONOUS_MODE_SHIFT,
+			  state << SSI_SYNCHRONOUS_MODE_SHIFT,
+			  MXC_SSISCR, module);
+}
+
+/*!
+ * This function allows the SSI module to output the SYS_CLK at the SRCK port.
+ *
+ * @param        module        the module number
+ * @param        state         the system clock state
+ */
+void ssi_system_clock(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_SYSTEM_CLOCK_SHIFT,
+			  state << SSI_SYSTEM_CLOCK_SHIFT, MXC_SSISCR, module);
+}
+
+/*!
+ * This function enables/disables the transmit section of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        state         the transmit section state
+ */
+void ssi_transmit_enable(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_TRANSMIT_ENABLE_SHIFT,
+			  state << SSI_TRANSMIT_ENABLE_SHIFT,
+			  MXC_SSISCR, module);
+}
+
+/*!
+ * This function allows the SSI module to operate in the two channel mode.
+ *
+ * @param        module        the module number
+ * @param        state         the two channel mode state
+ */
+void ssi_two_channel_mode(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_TWO_CHANNEL_SHIFT,
+			  state << SSI_TWO_CHANNEL_SHIFT, MXC_SSISCR, module);
+}
+
+/*!
+ * This function configures the SSI module to transmit data word from bit position 0 or 23 in the Transmit shift register.
+ *
+ * @param        module        the module number
+ * @param        state         the transmit from bit 0 state
+ */
+void ssi_tx_bit0(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_BIT_0_SHIFT,
+			  state << SSI_BIT_0_SHIFT, MXC_SSISTCR, module);
+}
+
+/*!
+ * This function controls the direction of the clock signal used to clock the Transmit shift register.
+ *
+ * @param        module        the module number
+ * @param        direction     the clock signal direction
+ */
+void ssi_tx_clock_direction(ssi_mod module, ssi_tx_rx_direction direction)
+{
+	set_register_bits(1 << SSI_CLOCK_DIRECTION_SHIFT,
+			  direction << SSI_CLOCK_DIRECTION_SHIFT,
+			  MXC_SSISTCR, module);
+}
+
+/*!
+ * This function configures the divide-by-two divider of the SSI module for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        state         the divider state
+ */
+void ssi_tx_clock_divide_by_two(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_DIVIDE_BY_TWO_SHIFT,
+			  state << SSI_DIVIDE_BY_TWO_SHIFT,
+			  MXC_SSISTCCR, module);
+}
+
+/*!
+ * This function controls which bit clock edge is used to clock out data.
+ *
+ * @param        module        the module number
+ * @param        polarity      the clock polarity
+ */
+void ssi_tx_clock_polarity(ssi_mod module, ssi_tx_rx_clock_polarity polarity)
+{
+	set_register_bits(1 << SSI_CLOCK_POLARITY_SHIFT,
+			  polarity << SSI_CLOCK_POLARITY_SHIFT,
+			  MXC_SSISTCR, module);
+}
+
+/*!
+ * This function configures a fixed divide-by-eight clock prescaler divider of the SSI module in series with the variable prescaler for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        state         the prescaler state
+ */
+void ssi_tx_clock_prescaler(ssi_mod module, bool state)
+{
+	set_register_bits(1 << SSI_PRESCALER_RANGE_SHIFT,
+			  state << SSI_PRESCALER_RANGE_SHIFT,
+			  MXC_SSISTCCR, module);
+}
+
+/*!
+ * This function controls the early frame sync configuration for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        early         the early frame sync configuration
+ */
+void ssi_tx_early_frame_sync(ssi_mod module, ssi_tx_rx_early_frame_sync early)
+{
+	set_register_bits(1 << SSI_EARLY_FRAME_SYNC_SHIFT,
+			  early << SSI_EARLY_FRAME_SYNC_SHIFT,
+			  MXC_SSISTCR, module);
+}
+
+/*!
+ * This function gets the number of data words in the Transmit FIFO.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo
+ * @return       This function returns the number of words in the Tx FIFO.
+ */
+unsigned char ssi_tx_fifo_counter(ssi_mod module, fifo_nb fifo)
+{
+	unsigned char result = 0;
+
+	if (ssi_fifo_0 == fifo) {
+		result = getreg_value(MXC_SSISFCSR, module);
+		result &= (0xF << SSI_TX_FIFO_0_COUNT_SHIFT);
+		result >>= SSI_TX_FIFO_0_COUNT_SHIFT;
+	} else {
+		result = getreg_value(MXC_SSISFCSR, module);
+		result &= (0xF << SSI_TX_FIFO_1_COUNT_SHIFT);
+		result >>= SSI_TX_FIFO_1_COUNT_SHIFT;
+	}
+
+	return result;
+}
+
+/*!
+ * This function controls the threshold at which the TFEx flag will be set.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo to enable
+ * @param        watermark     the watermark
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_tx_fifo_empty_watermark(ssi_mod module,
+				fifo_nb fifo, unsigned char watermark)
+{
+	int result = -1;
+
+	if ((watermark > SSI_MIN_FIFO_WATERMARK) &&
+	    (watermark <= SSI_MAX_FIFO_WATERMARK)) {
+		if (ssi_fifo_0 == fifo) {
+			set_register_bits(0xf << SSI_TX_FIFO_0_WATERMARK_SHIFT,
+					  watermark <<
+					  SSI_TX_FIFO_0_WATERMARK_SHIFT,
+					  MXC_SSISFCSR, module);
+		} else {
+			set_register_bits(0xf << SSI_TX_FIFO_1_WATERMARK_SHIFT,
+					  watermark <<
+					  SSI_TX_FIFO_1_WATERMARK_SHIFT,
+					  MXC_SSISFCSR, module);
+		}
+
+		result = 0;
+	}
+
+	return result;
+}
+
+/*!
+ * This function enables the Transmit FIFO.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo to enable
+ * @param        enable        the state of the fifo, enabled or disabled
+ */
+
+void ssi_tx_fifo_enable(ssi_mod module, fifo_nb fifo, bool enable)
+{
+	unsigned int reg;
+
+	reg = getreg_value(MXC_SSISTCR, module);
+	if (enable == true) {
+		reg |= ((1 + fifo) << SSI_FIFO_ENABLE_0_SHIFT);
+	} else {
+		reg &= ~((1 + fifo) << SSI_FIFO_ENABLE_0_SHIFT);
+	}
+
+	set_register(reg, MXC_SSISTCR, module);
+}
+
+/*!
+ * This function flushes the Transmit FIFOs.
+ *
+ * @param        module        the module number
+ */
+void ssi_tx_flush_fifo(ssi_mod module)
+{
+	set_register_bits(0, 1 << SSI_TRANSMITTER_CLEAR_SHIFT,
+			  MXC_SSISOR, module);
+}
+
+/*!
+ * This function controls the direction of the Frame Sync signal for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        direction     the Frame Sync signal direction
+ */
+void ssi_tx_frame_direction(ssi_mod module, ssi_tx_rx_direction direction)
+{
+	set_register_bits(1 << SSI_FRAME_DIRECTION_SHIFT,
+			  direction << SSI_FRAME_DIRECTION_SHIFT,
+			  MXC_SSISTCR, module);
+}
+
+/*!
+ * This function configures the Transmit frame rate divider.
+ *
+ * @param        module        the module number
+ * @param        ratio         the divide ratio from 1 to 32
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_tx_frame_rate(ssi_mod module, unsigned char ratio)
+{
+	int result = -1;
+
+	if ((ratio >= SSI_MIN_FRAME_DIVIDER_RATIO) &&
+	    (ratio <= SSI_MAX_FRAME_DIVIDER_RATIO)) {
+
+		set_register_bits(SSI_FRAME_DIVIDER_MASK <<
+				  SSI_FRAME_RATE_DIVIDER_SHIFT,
+				  (ratio - 1) << SSI_FRAME_RATE_DIVIDER_SHIFT,
+				  MXC_SSISTCCR, module);
+		result = 0;
+	}
+
+	return result;
+}
+
+/*!
+ * This function controls the Frame Sync active polarity for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        active        the Frame Sync active polarity
+ */
+void ssi_tx_frame_sync_active(ssi_mod module,
+			      ssi_tx_rx_frame_sync_active active)
+{
+	set_register_bits(1 << SSI_FRAME_SYNC_INVERT_SHIFT,
+			  active << SSI_FRAME_SYNC_INVERT_SHIFT,
+			  MXC_SSISTCR, module);
+}
+
+/*!
+ * This function controls the Frame Sync length (one word or one bit long) for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        length        the Frame Sync length
+ */
+void ssi_tx_frame_sync_length(ssi_mod module,
+			      ssi_tx_rx_frame_sync_length length)
+{
+	set_register_bits(1 << SSI_FRAME_SYNC_LENGTH_SHIFT,
+			  length << SSI_FRAME_SYNC_LENGTH_SHIFT,
+			  MXC_SSISTCR, module);
+}
+
+/*!
+ * This function configures the time slot(s) to mask for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        mask          the mask to indicate the time slot(s) masked
+ */
+void ssi_tx_mask_time_slot(ssi_mod module, unsigned int mask)
+{
+	set_register_bits(0xFFFFFFFF, mask, MXC_SSISTMSK, module);
+}
+
+/*!
+ * This function configures the Prescale divider for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        divider       the divide ratio from 1 to  256
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_tx_prescaler_modulus(ssi_mod module, unsigned int divider)
+{
+	int result = -1;
+
+	if ((divider >= SSI_MIN_PRESCALER_MODULUS_RATIO) &&
+	    (divider <= SSI_MAX_PRESCALER_MODULUS_RATIO)) {
+
+		set_register_bits(SSI_PRESCALER_MODULUS_MASK <<
+				  SSI_PRESCALER_MODULUS_SHIFT,
+				  (divider - 1) << SSI_PRESCALER_MODULUS_SHIFT,
+				  MXC_SSISTCCR, module);
+		result = 0;
+	}
+
+	return result;
+}
+
+/*!
+ * This function controls whether the MSB or LSB will be transmitted first in a sample.
+ *
+ * @param        module        the module number
+ * @param        direction     the shift direction
+ */
+void ssi_tx_shift_direction(ssi_mod module, ssi_tx_rx_shift_direction direction)
+{
+	set_register_bits(1 << SSI_SHIFT_DIRECTION_SHIFT,
+			  direction << SSI_SHIFT_DIRECTION_SHIFT,
+			  MXC_SSISTCR, module);
+}
+
+/*!
+ * This function configures the Transmit word length.
+ *
+ * @param        module        the module number
+ * @param        length        the word length
+ */
+void ssi_tx_word_length(ssi_mod module, ssi_word_length length)
+{
+	set_register_bits(SSI_WORD_LENGTH_MASK << SSI_WORD_LENGTH_SHIFT,
+			  length << SSI_WORD_LENGTH_SHIFT,
+			  MXC_SSISTCCR, module);
+}
+
+/*!
+ * This function initializes the driver in terms of memory of the soundcard
+ * and some basic HW clock settings.
+ *
+ * @return              0 on success, -1 otherwise.
+ */
+static int __init ssi_probe(struct platform_device *pdev)
+{
+	int ret = -1;
+	ssi_platform_data =
+	    (struct mxc_audio_platform_data *)pdev->dev.platform_data;
+	if (!ssi_platform_data) {
+		dev_err(&pdev->dev, "can't get the platform data for SSI\n");
+		return -EINVAL;
+	}
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (!res) {
+		dev_err(&pdev->dev, "can't get platform resource -  SSI\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	if (pdev->id == 0) {
+		base_addr_1 = (void *)res->start;
+	} else if (pdev->id == 1) {
+		base_addr_2 = (void *)res->start;
+	}
+
+	printk(KERN_INFO "SSI %d module loaded successfully \n", pdev->id + 1);
+
+	return 0;
+      err:
+	return -1;
+
+}
+
+static int ssi_remove(struct platform_device *dev)
+{
+	return 0;
+}
+
+static struct platform_driver mxc_ssi_driver = {
+	.probe = ssi_probe,
+	.remove = ssi_remove,
+	.driver = {
+		   .name = "mxc_ssi",
+		   },
+};
+
+/*!
+ * This function implements the init function of the SSI device.
+ * This function is called when the module is loaded.
+ *
+ * @return       This function returns 0.
+ */
+static int __init ssi_init(void)
+{
+	spin_lock_init(&ssi_lock);
+	return platform_driver_register(&mxc_ssi_driver);
+
+}
+
+/*!
+ * This function implements the exit function of the SPI device.
+ * This function is called when the module is unloaded.
+ *
+ */
+static void __exit ssi_exit(void)
+{
+	platform_driver_unregister(&mxc_ssi_driver);
+	printk(KERN_INFO "SSI module unloaded successfully\n");
+}
+
+module_init(ssi_init);
+module_exit(ssi_exit);
+
+MODULE_DESCRIPTION("SSI char device driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mxc/ssi/ssi.h b/drivers/mxc/ssi/ssi.h
new file mode 100644
index 000000000000..22032b6616fa
--- /dev/null
+++ b/drivers/mxc/ssi/ssi.h
@@ -0,0 +1,574 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+ /*!
+  * @defgroup SSI Synchronous Serial Interface (SSI) Driver
+  */
+
+ /*!
+  * @file ssi.h
+  * @brief This header file contains SSI driver functions prototypes.
+  *
+  * @ingroup SSI
+  */
+
+#ifndef __MXC_SSI_H__
+#define __MXC_SSI_H__
+
+#include "ssi_types.h"
+
+/*!
+ * This function gets the SSI fifo address.
+ *
+ * @param        ssi                 ssi number
+ * @param        direction           To indicate playback / recording
+ * @return       This function returns the SSI fifo address.
+ */
+unsigned int get_ssi_fifo_addr(unsigned int ssi, int direction);
+
+/*!
+ * This function controls the AC97 frame rate divider.
+ *
+ * @param        module               the module number
+ * @param        frame_rate_divider   the AC97 frame rate divider
+ */
+void ssi_ac97_frame_rate_divider(ssi_mod module,
+				 unsigned char frame_rate_divider);
+
+/*!
+ * This function gets the AC97 command address register.
+ *
+ * @param        module        the module number
+ * @return       This function returns the command address slot information.
+ */
+unsigned int ssi_ac97_get_command_address_register(ssi_mod module);
+
+/*!
+ * This function gets the AC97 command data register.
+ *
+ * @param        module        the module number
+ * @return       This function returns the command data slot information.
+ */
+unsigned int ssi_ac97_get_command_data_register(ssi_mod module);
+
+/*!
+ * This function gets the AC97 tag register.
+ *
+ * @param        module        the module number
+ * @return       This function returns the tag information.
+ */
+unsigned int ssi_ac97_get_tag_register(ssi_mod module);
+
+/*!
+ * This function controls the AC97 mode.
+ *
+ * @param        module        the module number
+ * @param        state         the AC97 mode state (enabled or disabled)
+ */
+void ssi_ac97_mode_enable(ssi_mod module, bool state);
+
+/*!
+ * This function controls the AC97 tag in FIFO behavior.
+ *
+ * @param        module        the module number
+ * @param        state         the tag in fifo behavior (Tag info stored in Rx FIFO 0 if TRUE,
+ * Tag info stored in SATAG register otherwise)
+ */
+void ssi_ac97_tag_in_fifo(ssi_mod module, bool state);
+
+/*!
+ * This function controls the AC97 read command.
+ *
+ * @param        module        the module number
+ * @param        state         the next AC97 command is a read command or not
+ */
+void ssi_ac97_read_command(ssi_mod module, bool state);
+
+/*!
+ * This function sets the AC97 command address register.
+ *
+ * @param        module        the module number
+ * @param        address       the command address slot information
+ */
+void ssi_ac97_set_command_address_register(ssi_mod module,
+					   unsigned int address);
+
+/*!
+ * This function sets the AC97 command data register.
+ *
+ * @param        module        the module number
+ * @param        data          the command data slot information
+ */
+void ssi_ac97_set_command_data_register(ssi_mod module, unsigned int data);
+
+/*!
+ * This function sets the AC97 tag register.
+ *
+ * @param        module        the module number
+ * @param        tag           the tag information
+ */
+void ssi_ac97_set_tag_register(ssi_mod module, unsigned int tag);
+
+/*!
+ * This function controls the AC97 variable mode.
+ *
+ * @param        module        the module number
+ * @param        state         the AC97 variable mode state (enabled or disabled)
+ */
+void ssi_ac97_variable_mode(ssi_mod module, bool state);
+
+/*!
+ * This function controls the AC97 write command.
+ *
+ * @param        module        the module number
+ * @param        state         the next AC97 command is a write command or not
+ */
+void ssi_ac97_write_command(ssi_mod module, bool state);
+
+/*!
+ * This function controls the idle state of the transmit clock port during SSI internal gated mode.
+ *
+ * @param        module        the module number
+ * @param        state         the clock idle state
+ */
+void ssi_clock_idle_state(ssi_mod module, idle_state state);
+
+/*!
+ * This function turns off/on the ccm_ssi_clk to reduce power consumption.
+ *
+ * @param        module        the module number
+ * @param        state         the state for ccm_ssi_clk (true: turn off, else:turn on)
+ */
+void ssi_clock_off(ssi_mod module, bool state);
+
+/*!
+ * This function enables/disables the SSI module.
+ *
+ * @param        module        the module number
+ * @param        state         the state for SSI module
+ */
+void ssi_enable(ssi_mod module, bool state);
+
+/*!
+ * This function gets the data word in the Receive FIFO of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        fifo          the Receive FIFO to read
+ * @return       This function returns the read data.
+ */
+unsigned int ssi_get_data(ssi_mod module, fifo_nb fifo);
+
+/*!
+ * This function returns the status of the SSI module (SISR register) as a combination of status.
+ *
+ * @param        module        the module number
+ * @return       This function returns the status of the SSI module.
+ */
+ssi_status_enable_mask ssi_get_status(ssi_mod module);
+
+/*!
+ * This function selects the I2S mode of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        mode          the I2S mode
+ */
+void ssi_i2s_mode(ssi_mod module, mode_i2s mode);
+
+/*!
+ * This function disables the interrupts of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        mask          the mask of the interrupts to disable
+ */
+void ssi_interrupt_disable(ssi_mod module, ssi_status_enable_mask mask);
+
+/*!
+ * This function enables the interrupts of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        mask          the mask of the interrupts to enable
+ */
+void ssi_interrupt_enable(ssi_mod module, ssi_status_enable_mask mask);
+
+/*!
+ * This function enables/disables the network mode of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        state         the network mode state
+ */
+void ssi_network_mode(ssi_mod module, bool state);
+
+/*!
+ * This function enables/disables the receive section of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        state         the receive section state
+ */
+void ssi_receive_enable(ssi_mod module, bool state);
+
+/*!
+ * This function configures the SSI module to receive data word at bit position 0 or 23 in the Receive shift register.
+ *
+ * @param        module        the module number
+ * @param        state         the state to receive at bit 0
+ */
+void ssi_rx_bit0(ssi_mod module, bool state);
+
+/*!
+ * This function controls the source of the clock signal used to clock the Receive shift register.
+ *
+ * @param        module        the module number
+ * @param        direction     the clock signal direction
+ */
+void ssi_rx_clock_direction(ssi_mod module, ssi_tx_rx_direction direction);
+
+/*!
+ * This function configures the divide-by-two divider of the SSI module for the receive section.
+ *
+ * @param        module        the module number
+ * @param        state         the divider state
+ */
+void ssi_rx_clock_divide_by_two(ssi_mod module, bool state);
+
+/*!
+ * This function controls which bit clock edge is used to clock in data.
+ *
+ * @param        module        the module number
+ * @param        polarity      the clock polarity
+ */
+void ssi_rx_clock_polarity(ssi_mod module, ssi_tx_rx_clock_polarity polarity);
+
+/*!
+ * This function configures a fixed divide-by-eight clock prescaler divider of the SSI module in series with the variable prescaler for the receive section.
+ *
+ * @param        module        the module number
+ * @param        state         the prescaler state
+ */
+void ssi_rx_clock_prescaler(ssi_mod module, bool state);
+
+/*!
+ * This function controls the early frame sync configuration.
+ *
+ * @param        module        the module number
+ * @param        early         the early frame sync configuration
+ */
+void ssi_rx_early_frame_sync(ssi_mod module, ssi_tx_rx_early_frame_sync early);
+
+/*!
+ * This function gets the number of data words in the Receive FIFO.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo
+ * @return       This function returns the number of words in the Rx FIFO.
+ */
+unsigned char ssi_rx_fifo_counter(ssi_mod module, fifo_nb fifo);
+
+/*!
+ * This function enables the Receive FIFO.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo to enable
+ * @param        enabled       the state of the fifo, enabled or disabled
+ */
+void ssi_rx_fifo_enable(ssi_mod module, fifo_nb fifo, bool enabled);
+
+/*!
+ * This function controls the threshold at which the RFFx flag will be set.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo to enable
+ * @param        watermark     the watermark
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_rx_fifo_full_watermark(ssi_mod module,
+			       fifo_nb fifo, unsigned char watermark);
+
+/*!
+ * This function flushes the Receive FIFOs.
+ *
+ * @param        module        the module number
+ */
+void ssi_rx_flush_fifo(ssi_mod module);
+
+/*!
+ * This function controls the direction of the Frame Sync signal for the receive section.
+ *
+ * @param        module        the module number
+ * @param        direction     the Frame Sync signal direction
+ */
+void ssi_rx_frame_direction(ssi_mod module, ssi_tx_rx_direction direction);
+
+/*!
+ * This function configures the Receive frame rate divider for the receive section.
+ *
+ * @param        module        the module number
+ * @param        ratio         the divide ratio from 1 to 32
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_rx_frame_rate(ssi_mod module, unsigned char ratio);
+
+/*!
+ * This function controls the Frame Sync active polarity for the receive section.
+ *
+ * @param        module        the module number
+ * @param        active        the Frame Sync active polarity
+ */
+void ssi_rx_frame_sync_active(ssi_mod module,
+			      ssi_tx_rx_frame_sync_active active);
+
+/*!
+ * This function controls the Frame Sync length (one word or one bit long) for the receive section.
+ *
+ * @param        module        the module number
+ * @param        length        the Frame Sync length
+ */
+void ssi_rx_frame_sync_length(ssi_mod module,
+			      ssi_tx_rx_frame_sync_length length);
+
+/*!
+ * This function configures the time slot(s) to mask for the receive section.
+ *
+ * @param        module        the module number
+ * @param        mask          the mask to indicate the time slot(s) masked
+ */
+void ssi_rx_mask_time_slot(ssi_mod module, unsigned int mask);
+
+/*!
+ * This function configures the Prescale divider for the receive section.
+ *
+ * @param        module        the module number
+ * @param        divider       the divide ratio from 1 to  256
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_rx_prescaler_modulus(ssi_mod module, unsigned int divider);
+
+/*!
+ * This function controls whether the MSB or LSB will be received first in a sample.
+ *
+ * @param        module        the module number
+ * @param        direction     the shift direction
+ */
+void ssi_rx_shift_direction(ssi_mod module,
+			    ssi_tx_rx_shift_direction direction);
+
+/*!
+ * This function configures the Receive word length.
+ *
+ * @param        module        the module number
+ * @param        length        the word length
+ */
+void ssi_rx_word_length(ssi_mod module, ssi_word_length length);
+
+/*!
+ * This function sets the data word in the Transmit FIFO of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        fifo          the FIFO number
+ * @param        data          the data to load in the FIFO
+ */
+void ssi_set_data(ssi_mod module, fifo_nb fifo, unsigned int data);
+
+/*!
+ * This function controls the number of wait states between the core and SSI.
+ *
+ * @param        module        the module number
+ * @param        wait          the number of wait state(s)
+ */
+void ssi_set_wait_states(ssi_mod module, ssi_wait_states wait);
+
+/*!
+ * This function enables/disables the synchronous mode of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        state         the synchronous mode state
+ */
+void ssi_synchronous_mode(ssi_mod module, bool state);
+
+/*!
+ * This function allows the SSI module to output the SYS_CLK at the SRCK port.
+ *
+ * @param        module        the module number
+ * @param        state         the system clock state
+ */
+void ssi_system_clock(ssi_mod module, bool state);
+
+/*!
+ * This function enables/disables the transmit section of the SSI module.
+ *
+ * @param        module        the module number
+ * @param        state         the transmit section state
+ */
+void ssi_transmit_enable(ssi_mod module, bool state);
+
+/*!
+ * This function allows the SSI module to operate in the two channel mode.
+ *
+ * @param        module        the module number
+ * @param        state         the two channel mode state
+ */
+void ssi_two_channel_mode(ssi_mod module, bool state);
+
+/*!
+ * This function configures the SSI module to transmit data word from bit position 0 or 23 in the Transmit shift register.
+ *
+ * @param        module        the module number
+ * @param        state         the transmit from bit 0 state
+ */
+void ssi_tx_bit0(ssi_mod module, bool state);
+
+/*!
+ * This function controls the direction of the clock signal used to clock the Transmit shift register.
+ *
+ * @param        module        the module number
+ * @param        direction     the clock signal direction
+ */
+void ssi_tx_clock_direction(ssi_mod module, ssi_tx_rx_direction direction);
+
+/*!
+ * This function configures the divide-by-two divider of the SSI module for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        state         the divider state
+ */
+void ssi_tx_clock_divide_by_two(ssi_mod module, bool state);
+
+/*!
+ * This function controls which bit clock edge is used to clock out data.
+ *
+ * @param        module        the module number
+ * @param        polarity      the clock polarity
+ */
+void ssi_tx_clock_polarity(ssi_mod module, ssi_tx_rx_clock_polarity polarity);
+
+/*!
+ * This function configures a fixed divide-by-eight clock prescaler divider of the SSI module in series with the variable prescaler for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        state         the prescaler state
+ */
+void ssi_tx_clock_prescaler(ssi_mod module, bool state);
+
+/*!
+ * This function controls the early frame sync configuration for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        early         the early frame sync configuration
+ */
+void ssi_tx_early_frame_sync(ssi_mod module, ssi_tx_rx_early_frame_sync early);
+
+/*!
+ * This function gets the number of data words in the Transmit FIFO.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo
+ * @return       This function returns the number of words in the Tx FIFO.
+ */
+unsigned char ssi_tx_fifo_counter(ssi_mod module, fifo_nb fifo);
+
+/*!
+ * This function controls the threshold at which the TFEx flag will be set.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo to enable
+ * @param        watermark     the watermark
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_tx_fifo_empty_watermark(ssi_mod module, fifo_nb fifo,
+				unsigned char watermark);
+
+/*!
+ * This function enables the Transmit FIFO.
+ *
+ * @param        module        the module number
+ * @param        fifo          the fifo to enable
+ * @param        enable        the state of the FIFO, enabled or disabled
+ */
+void ssi_tx_fifo_enable(ssi_mod module, fifo_nb fifo, bool enable);
+
+/*!
+ * This function flushes the Transmit FIFOs.
+ *
+ * @param        module        the module number
+ */
+void ssi_tx_flush_fifo(ssi_mod module);
+
+/*!
+ * This function controls the direction of the Frame Sync signal for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        direction     the Frame Sync signal direction
+ */
+void ssi_tx_frame_direction(ssi_mod module, ssi_tx_rx_direction direction);
+
+/*!
+ * This function configures the Transmit frame rate divider.
+ *
+ * @param        module        the module number
+ * @param        ratio         the divide ratio from 1 to 32
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_tx_frame_rate(ssi_mod module, unsigned char ratio);
+
+/*!
+ * This function controls the Frame Sync active polarity for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        active        the Frame Sync active polarity
+ */
+void ssi_tx_frame_sync_active(ssi_mod module,
+			      ssi_tx_rx_frame_sync_active active);
+
+/*!
+ * This function controls the Frame Sync length (one word or one bit long) for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        length        the Frame Sync length
+ */
+void ssi_tx_frame_sync_length(ssi_mod module,
+			      ssi_tx_rx_frame_sync_length length);
+
+/*!
+ * This function configures the time slot(s) to mask for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        mask          the mask to indicate the time slot(s) masked
+ */
+void ssi_tx_mask_time_slot(ssi_mod module, unsigned int mask);
+
+/*!
+ * This function configures the Prescale divider for the transmit section.
+ *
+ * @param        module        the module number
+ * @param        divider       the divide ratio from 1 to  256
+ * @return       This function returns the result of the operation (0 if successful, -1 otherwise).
+ */
+int ssi_tx_prescaler_modulus(ssi_mod module, unsigned int divider);
+
+/*!
+ * This function controls whether the MSB or LSB will be transmited first in a sample.
+ *
+ * @param        module        the module number
+ * @param        direction     the shift direction
+ */
+void ssi_tx_shift_direction(ssi_mod module,
+			    ssi_tx_rx_shift_direction direction);
+
+/*!
+ * This function configures the Transmit word length.
+ *
+ * @param        module        the module number
+ * @param        length        the word length
+ */
+void ssi_tx_word_length(ssi_mod module, ssi_word_length length);
+
+#endif				/* __MXC_SSI_H__ */
diff --git a/drivers/mxc/ssi/ssi_types.h b/drivers/mxc/ssi/ssi_types.h
new file mode 100644
index 000000000000..015e65a6d2d2
--- /dev/null
+++ b/drivers/mxc/ssi/ssi_types.h
@@ -0,0 +1,367 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+ /*!
+  * @file ssi_types.h
+  * @brief This header file contains SSI types.
+  *
+  * @ingroup SSI
+  */
+
+#ifndef __MXC_SSI_TYPES_H__
+#define __MXC_SSI_TYPES_H__
+
+/*!
+ * This enumeration describes the FIFO number.
+ */
+typedef enum {
+	/*!
+	 * FIFO 0
+	 */
+	ssi_fifo_0 = 0,
+	/*!
+	 * FIFO 1
+	 */
+	ssi_fifo_1 = 1
+} fifo_nb;
+
+/*!
+ * This enumeration describes the clock idle state.
+ */
+typedef enum {
+	/*!
+	 * Clock idle state is 1
+	 */
+	clock_idle_state_1 = 0,
+	/*!
+	 * Clock idle state is 0
+	 */
+	clock_idle_state_0 = 1
+} idle_state;
+
+/*!
+ * This enumeration describes I2S mode.
+ */
+typedef enum {
+	/*!
+	 * Normal mode
+	 */
+	i2s_normal = 0,
+	/*!
+	 * Master mode
+	 */
+	i2s_master = 1,
+	/*!
+	 * Slave mode
+	 */
+	i2s_slave = 2
+} mode_i2s;
+
+/*!
+ * This enumeration describes index for both SSI1 and SSI2 modules.
+ */
+typedef enum {
+	/*!
+	 * SSI1 index
+	 */
+	SSI1 = 0,
+	/*!
+	 * SSI2 index not present on MXC 91221 and MXC91311
+	 */
+	SSI2 = 1
+} ssi_mod;
+
+/*!
+ * This enumeration describes the status/enable bits for interrupt source of the SSI module.
+ */
+typedef enum {
+	/*!
+	 * SSI Transmit FIFO 0 empty bit
+	 */
+	ssi_tx_fifo_0_empty = 0x00000001,
+	/*!
+	 * SSI Transmit FIFO 1 empty bit
+	 */
+	ssi_tx_fifo_1_empty = 0x00000002,
+	/*!
+	 * SSI Receive FIFO 0 full bit
+	 */
+	ssi_rx_fifo_0_full = 0x00000004,
+	/*!
+	 * SSI Receive FIFO 1 full bit
+	 */
+	ssi_rx_fifo_1_full = 0x00000008,
+	/*!
+	 * SSI Receive Last Time Slot bit
+	 */
+	ssi_rls = 0x00000010,
+	/*!
+	 * SSI Transmit Last Time Slot bit
+	 */
+	ssi_tls = 0x00000020,
+	/*!
+	 * SSI Receive Frame Sync bit
+	 */
+	ssi_rfs = 0x00000040,
+	/*!
+	 * SSI Transmit Frame Sync bit
+	 */
+	ssi_tfs = 0x00000080,
+	/*!
+	 * SSI Transmitter underrun 0 bit
+	 */
+	ssi_transmitter_underrun_0 = 0x00000100,
+	/*!
+	 * SSI Transmitter underrun 1 bit
+	 */
+	ssi_transmitter_underrun_1 = 0x00000200,
+	/*!
+	 * SSI Receiver overrun 0 bit
+	 */
+	ssi_receiver_overrun_0 = 0x00000400,
+	/*!
+	 * SSI Receiver overrun 1 bit
+	 */
+	ssi_receiver_overrun_1 = 0x00000800,
+	/*!
+	 * SSI Transmit Data register empty 0 bit
+	 */
+	ssi_tx_data_reg_empty_0 = 0x00001000,
+	/*!
+	 * SSI Transmit Data register empty 1 bit
+	 */
+	ssi_tx_data_reg_empty_1 = 0x00002000,
+
+	/*!
+	 * SSI Receive Data Ready 0 bit
+	 */
+	ssi_rx_data_ready_0 = 0x00004000,
+	/*!
+	 * SSI Receive Data Ready 1 bit
+	 */
+	ssi_rx_data_ready_1 = 0x00008000,
+	/*!
+	 * SSI Receive tag updated bit
+	 */
+	ssi_rx_tag_updated = 0x00010000,
+	/*!
+	 * SSI Command data register updated bit
+	 */
+	ssi_cmd_data_reg_updated = 0x00020000,
+	/*!
+	 * SSI Command address register updated bit
+	 */
+	ssi_cmd_address_reg_updated = 0x00040000,
+	/*!
+	 * SSI Transmit interrupt enable bit
+	 */
+	ssi_tx_interrupt_enable = 0x00080000,
+	/*!
+	 * SSI Transmit DMA enable bit
+	 */
+	ssi_tx_dma_interrupt_enable = 0x00100000,
+	/*!
+	 * SSI Receive interrupt enable bit
+	 */
+	ssi_rx_interrupt_enable = 0x00200000,
+	/*!
+	 * SSI Receive DMA enable bit
+	 */
+	ssi_rx_dma_interrupt_enable = 0x00400000,
+	/*!
+	 * SSI Tx frame complete enable bit on MXC91221 & MXC91311 only
+	 */
+	ssi_tx_frame_complete = 0x00800000,
+	/*!
+	 * SSI Rx frame complete on MXC91221 & MXC91311 only
+	 */
+	ssi_rx_frame_complete = 0x001000000
+} ssi_status_enable_mask;
+
+/*!
+ * This enumeration describes the clock edge to clock in or clock out data.
+ */
+typedef enum {
+	/*!
+	 * Clock on rising edge
+	 */
+	ssi_clock_on_rising_edge = 0,
+	/*!
+	 * Clock on falling edge
+	 */
+	ssi_clock_on_falling_edge = 1
+} ssi_tx_rx_clock_polarity;
+
+/*!
+ * This enumeration describes the clock direction.
+ */
+typedef enum {
+	/*!
+	 * Clock is external
+	 */
+	ssi_tx_rx_externally = 0,
+	/*!
+	 * Clock is generated internally
+	 */
+	ssi_tx_rx_internally = 1
+} ssi_tx_rx_direction;
+
+/*!
+ * This enumeration describes the early frame sync behavior.
+ */
+typedef enum {
+	/*!
+	 * Frame Sync starts on the first data bit
+	 */
+	ssi_frame_sync_first_bit = 0,
+	/*!
+	 * Frame Sync starts one bit before the first data bit
+	 */
+	ssi_frame_sync_one_bit_before = 1
+} ssi_tx_rx_early_frame_sync;
+
+/*!
+ * This enumeration describes the Frame Sync active value.
+ */
+typedef enum {
+	/*!
+	 * Frame Sync is active when high
+	 */
+	ssi_frame_sync_active_high = 0,
+	/*!
+	 * Frame Sync is active when low
+	 */
+	ssi_frame_sync_active_low = 1
+} ssi_tx_rx_frame_sync_active;
+
+/*!
+ * This enumeration describes the Frame Sync active length.
+ */
+typedef enum {
+	/*!
+	 * Frame Sync is active when high
+	 */
+	ssi_frame_sync_one_word = 0,
+	/*!
+	 * Frame Sync is active when low
+	 */
+	ssi_frame_sync_one_bit = 1
+} ssi_tx_rx_frame_sync_length;
+
+/*!
+ * This enumeration describes the Tx/Rx frame shift direction.
+ */
+typedef enum {
+	/*!
+	 * MSB first
+	 */
+	ssi_msb_first = 0,
+	/*!
+	 * LSB first
+	 */
+	ssi_lsb_first = 1
+} ssi_tx_rx_shift_direction;
+
+/*!
+ * This enumeration describes the wait state number.
+ */
+typedef enum {
+	/*!
+	 * 0 wait state
+	 */
+	ssi_waitstates0 = 0x0,
+	/*!
+	 * 1 wait state
+	 */
+	ssi_waitstates1 = 0x1,
+	/*!
+	 * 2 wait states
+	 */
+	ssi_waitstates2 = 0x2,
+	/*!
+	 * 3 wait states
+	 */
+	ssi_waitstates3 = 0x3
+} ssi_wait_states;
+
+/*!
+ * This enumeration describes the word length.
+ */
+typedef enum {
+	/*!
+	 * 2 bits long
+	 */
+	ssi_2_bits = 0x0,
+	/*!
+	 * 4 bits long
+	 */
+	ssi_4_bits = 0x1,
+	/*!
+	 * 6 bits long
+	 */
+	ssi_6_bits = 0x2,
+	/*!
+	 * 8 bits long
+	 */
+	ssi_8_bits = 0x3,
+	/*!
+	 * 10 bits long
+	 */
+	ssi_10_bits = 0x4,
+	/*!
+	 * 12 bits long
+	 */
+	ssi_12_bits = 0x5,
+	/*!
+	 * 14 bits long
+	 */
+	ssi_14_bits = 0x6,
+	/*!
+	 * 16 bits long
+	 */
+	ssi_16_bits = 0x7,
+	/*!
+	 * 18 bits long
+	 */
+	ssi_18_bits = 0x8,
+	/*!
+	 * 20 bits long
+	 */
+	ssi_20_bits = 0x9,
+	/*!
+	 * 22 bits long
+	 */
+	ssi_22_bits = 0xA,
+	/*!
+	 * 24 bits long
+	 */
+	ssi_24_bits = 0xB,
+	/*!
+	 * 26 bits long
+	 */
+	ssi_26_bits = 0xC,
+	/*!
+	 * 28 bits long
+	 */
+	ssi_28_bits = 0xD,
+	/*!
+	 * 30 bits long
+	 */
+	ssi_30_bits = 0xE,
+	/*!
+	 * 32 bits long
+	 */
+	ssi_32_bits = 0xF
+} ssi_word_length;
+
+#endif				/* __MXC_SSI_TYPES_H__ */
diff --git a/drivers/mxc/vpu/Kconfig b/drivers/mxc/vpu/Kconfig
new file mode 100644
index 000000000000..e41974591b86
--- /dev/null
+++ b/drivers/mxc/vpu/Kconfig
@@ -0,0 +1,30 @@
+#
+# Codec configuration
+#
+
+menu "MXC VPU(Video Processing Unit) support"
+
+config MXC_VPU
+	  tristate "Support for MXC VPU(Video Processing Unit)"
+	  depends on (ARCH_MX3 || ARCH_MX27 || ARCH_MXC92323 || ARCH_MX37 || ARCH_MX51)
+	  default y
+	---help---
+	  The VPU codec device provides codec function for H.264/MPEG4/H.263,
+	  as well as MPEG2/VC-1/DivX on some platforms.
+
+config MXC_VPU_IRAM
+	  tristate "Use IRAM as temporary buffer for VPU to enhance performace"
+	  depends on (ARCH_MX37 || ARCH_MX51)
+	  default y
+	---help---
+	  The VPU can use internal RAM as temporary buffer to save external
+	  memroy bandwith, thus to enhance video performance.
+
+config MXC_VPU_DEBUG
+	bool "MXC VPU debugging"
+	depends on MXC_VPU != n
+	help
+	  This is an option for the developers; most people should
+	  say N here.  This enables MXC VPU driver debugging.
+
+endmenu
diff --git a/drivers/mxc/vpu/Makefile b/drivers/mxc/vpu/Makefile
new file mode 100644
index 000000000000..88e8f2c084a0
--- /dev/null
+++ b/drivers/mxc/vpu/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for the VPU drivers.
+#
+
+obj-$(CONFIG_MXC_VPU)                  += vpu.o
+vpu-objs	:= mxc_vpu.o mxc_vl2cc.o
+
+ifeq ($(CONFIG_MXC_VPU_DEBUG),y)
+EXTRA_CFLAGS += -DDEBUG
+endif
diff --git a/drivers/mxc/vpu/mxc_vl2cc.c b/drivers/mxc/vpu/mxc_vl2cc.c
new file mode 100644
index 000000000000..acc40dd01689
--- /dev/null
+++ b/drivers/mxc/vpu/mxc_vl2cc.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_vl2cc.c
+ *
+ * @brief VL2CC initialization and flush operation implementation
+ *
+ * @ingroup VL2CC
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <mach/hardware.h>
+#include <asm/io.h>
+
+#define VL2CC_CTRL_OFFSET	(0x100)
+#define VL2CC_AUXCTRL_OFFSET	(0x104)
+#define VL2CC_INVWAY_OFFSET	(0x77C)
+#define VL2CC_CLEANWAY_OFFSET	(0x7BC)
+
+/*! VL2CC clock handle. */
+static struct clk *vl2cc_clk;
+static u32 *vl2cc_base;
+
+/*!
+ * Initialization function of VL2CC. Remap the VL2CC base address.
+ *
+ * @return status  0 success.
+ */
+int vl2cc_init(u32 vl2cc_hw_base)
+{
+	vl2cc_base = ioremap(vl2cc_hw_base, SZ_8K - 1);
+	if (vl2cc_base == NULL) {
+		printk(KERN_INFO "vl2cc: Unable to ioremap\n");
+		return -ENOMEM;
+	}
+
+	vl2cc_clk = clk_get(NULL, "vl2cc_clk");
+	if (IS_ERR(vl2cc_clk)) {
+		printk(KERN_INFO "vl2cc: Unable to get clock\n");
+		iounmap(vl2cc_base);
+		return -EIO;
+	}
+
+	printk(KERN_INFO "VL2CC initialized\n");
+	return 0;
+}
+
+/*!
+ * Enable VL2CC hardware
+ */
+void vl2cc_enable(void)
+{
+	volatile u32 reg;
+
+	clk_enable(vl2cc_clk);
+
+	/* Disable VL2CC */
+	reg = __raw_readl(vl2cc_base + VL2CC_CTRL_OFFSET);
+	reg &= 0xFFFFFFFE;
+	__raw_writel(reg, vl2cc_base + VL2CC_CTRL_OFFSET);
+
+	/* Set the latency for data RAM reads, data RAM writes, tag RAM and
+	 * dirty RAM to 1 cycle - write 0x0 to AUX CTRL [11:0] and also
+	 * configure the number of ways to 8 - write 8 to AUX CTRL [16:13]
+	 */
+	reg = __raw_readl(vl2cc_base + VL2CC_AUXCTRL_OFFSET);
+	reg &= 0xFFFE1000;	/* Clear [16:13] too */
+	reg |= (0x8 << 13);	/* [16:13] = 8; */
+	__raw_writel(reg, vl2cc_base + VL2CC_AUXCTRL_OFFSET);
+
+	/* Invalidate the VL2CC ways - write 0xff to INV BY WAY and poll the
+	 * register until its value is 0x0
+	 */
+	__raw_writel(0xff, vl2cc_base + VL2CC_INVWAY_OFFSET);
+	while (__raw_readl(vl2cc_base + VL2CC_INVWAY_OFFSET) != 0x0) ;
+
+	/* Enable VL2CC */
+	reg = __raw_readl(vl2cc_base + VL2CC_CTRL_OFFSET);
+	reg |= 0x1;
+	__raw_writel(reg, vl2cc_base + VL2CC_CTRL_OFFSET);
+}
+
+/*!
+ * Flush VL2CC
+ */
+void vl2cc_flush(void)
+{
+	__raw_writel(0xff, vl2cc_base + VL2CC_CLEANWAY_OFFSET);
+	while (__raw_readl(vl2cc_base + VL2CC_CLEANWAY_OFFSET) != 0x0) ;
+}
+
+/*!
+ * Disable VL2CC
+ */
+void vl2cc_disable(void)
+{
+	__raw_writel(0, vl2cc_base + VL2CC_CTRL_OFFSET);
+	clk_disable(vl2cc_clk);
+}
+
+/*!
+ * Cleanup VL2CC
+ */
+void vl2cc_cleanup(void)
+{
+	clk_put(vl2cc_clk);
+	iounmap(vl2cc_base);
+}
diff --git a/drivers/mxc/vpu/mxc_vpu.c b/drivers/mxc/vpu/mxc_vpu.c
new file mode 100644
index 000000000000..ffc31b5ba7fa
--- /dev/null
+++ b/drivers/mxc/vpu/mxc_vpu.c
@@ -0,0 +1,847 @@
+/*
+ * Copyright 2006-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_vpu.c
+ *
+ * @brief VPU system initialization and file operation implementation
+ *
+ * @ingroup VPU
+ */
+
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/interrupt.h>
+#include <linux/autoconf.h>
+#include <linux/ioport.h>
+#include <linux/stat.h>
+#include <linux/platform_device.h>
+#include <linux/kdev_t.h>
+#include <linux/dma-mapping.h>
+#include <linux/wait.h>
+#include <linux/list.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+
+#include <asm/uaccess.h>
+#include <asm/io.h>
+#include <asm/sizes.h>
+#include <asm/dma-mapping.h>
+#include <mach/hardware.h>
+#include <mach/clock.h>
+
+#include <mach/mxc_vpu.h>
+
+struct vpu_priv {
+	struct fasync_struct *async_queue;
+};
+
+/* To track the allocated memory buffer */
+typedef struct memalloc_record {
+	struct list_head list;
+	struct vpu_mem_desc mem;
+} memalloc_record;
+
+struct iram_setting {
+	u32 start;
+	u32 end;
+};
+
+static DEFINE_SPINLOCK(vpu_lock);
+static LIST_HEAD(head);
+
+static int vpu_major = 0;
+static int vpu_clk_usercount;
+static struct class *vpu_class;
+static struct vpu_priv vpu_data;
+static u8 open_count = 0;
+static struct clk *vpu_clk;
+static struct vpu_mem_desc bitwork_mem = { 0 };
+static struct vpu_mem_desc pic_para_mem = { 0 };
+static struct vpu_mem_desc user_data_mem = { 0 };
+static struct vpu_mem_desc share_mem = { 0 };
+
+/* IRAM setting */
+static struct iram_setting iram;
+/* store SRC base addr */
+static u32 src_base_addr;
+
+/* implement the blocking ioctl */
+static int codec_done = 0;
+static wait_queue_head_t vpu_queue;
+
+static u32 workctrl_regsave[6];
+static u32 rd_ptr_regsave[4];
+static u32 wr_ptr_regsave[4];
+static u32 dis_flag_regsave[4];
+
+#define	READ_REG(x)	__raw_readl(IO_ADDRESS(VPU_BASE_ADDR+(x)))
+#define	WRITE_REG(val, x)					\
+		__raw_writel((val), IO_ADDRESS(VPU_BASE_ADDR+(x)))
+
+#define	SAVE_WORK_REGS	do {					\
+	int i;							\
+	for (i = 0; i < ARRAY_SIZE(workctrl_regsave)/2; i++)	\
+		workctrl_regsave[i] = READ_REG(BIT_WORK_CTRL_BUF_REG(i));\
+} while (0)
+#define	RESTORE_WORK_REGS	do {				\
+	int i;							\
+	for (i = 0; i < ARRAY_SIZE(workctrl_regsave)/2; i++)	\
+		WRITE_REG(workctrl_regsave[i], BIT_WORK_CTRL_BUF_REG(i));\
+} while (0)
+#define	SAVE_CTRL_REGS	do {					\
+	int i;							\
+	for (i = ARRAY_SIZE(workctrl_regsave)/2;		\
+			i < ARRAY_SIZE(workctrl_regsave); i++)		\
+		workctrl_regsave[i] = READ_REG(BIT_WORK_CTRL_BUF_REG(i));\
+} while (0)
+#define	RESTORE_CTRL_REGS	do {				\
+	int i;							\
+	for (i = ARRAY_SIZE(workctrl_regsave)/2;		\
+			i < ARRAY_SIZE(workctrl_regsave); i++)		\
+		WRITE_REG(workctrl_regsave[i], BIT_WORK_CTRL_BUF_REG(i));\
+} while (0)
+#define	SAVE_RDWR_PTR_REGS	do {					\
+	int i;								\
+	for (i = 0; i < ARRAY_SIZE(rd_ptr_regsave); i++)		\
+		rd_ptr_regsave[i] = READ_REG(BIT_RD_PTR_REG(i));	\
+	for (i = 0; i < ARRAY_SIZE(wr_ptr_regsave); i++)		\
+		wr_ptr_regsave[i] = READ_REG(BIT_WR_PTR_REG(i));	\
+} while (0)
+#define	RESTORE_RDWR_PTR_REGS	do {					\
+	int i;								\
+	for (i = 0; i < ARRAY_SIZE(rd_ptr_regsave); i++)		\
+		WRITE_REG(rd_ptr_regsave[i], BIT_RD_PTR_REG(i));	\
+	for (i = 0; i < ARRAY_SIZE(wr_ptr_regsave); i++)		\
+		WRITE_REG(wr_ptr_regsave[i], BIT_WR_PTR_REG(i));	\
+} while (0)
+#define	SAVE_DIS_FLAG_REGS	do {					\
+	int i;								\
+	for (i = 0; i < ARRAY_SIZE(dis_flag_regsave); i++)		\
+		dis_flag_regsave[i] = READ_REG(BIT_FRM_DIS_FLG_REG(i));	\
+} while (0)
+#define	RESTORE_DIS_FLAG_REGS	do {					\
+	int i;								\
+	for (i = 0; i < ARRAY_SIZE(dis_flag_regsave); i++)		\
+		WRITE_REG(dis_flag_regsave[i], BIT_FRM_DIS_FLG_REG(i));	\
+} while (0)
+
+/*!
+ * Private function to alloc dma buffer
+ * @return status  0 success.
+ */
+static int vpu_alloc_dma_buffer(struct vpu_mem_desc *mem)
+{
+	mem->cpu_addr = (unsigned long)
+	    dma_alloc_coherent(NULL, PAGE_ALIGN(mem->size),
+			       (dma_addr_t *) (&mem->phy_addr),
+			       GFP_DMA | GFP_KERNEL);
+	pr_debug("[ALLOC] mem alloc cpu_addr = 0x%x\n", mem->cpu_addr);
+	if ((void *)(mem->cpu_addr) == NULL) {
+		printk(KERN_ERR "Physical memory allocation error!\n");
+		return -1;
+	}
+	return 0;
+}
+
+/*!
+ * Private function to free dma buffer
+ */
+static void vpu_free_dma_buffer(struct vpu_mem_desc *mem)
+{
+	if (mem->cpu_addr != 0) {
+		dma_free_coherent(0, PAGE_ALIGN(mem->size),
+				  (void *)mem->cpu_addr, mem->phy_addr);
+	}
+}
+
+/*!
+ * Private function to free buffers
+ * @return status  0 success.
+ */
+static int vpu_free_buffers(void)
+{
+	struct memalloc_record *rec, *n;
+	struct vpu_mem_desc mem;
+
+	list_for_each_entry_safe(rec, n, &head, list) {
+		mem = rec->mem;
+		if (mem.cpu_addr != 0) {
+			vpu_free_dma_buffer(&mem);
+			pr_debug("[FREE] freed paddr=0x%08X\n", mem.phy_addr);
+			/* delete from list */
+			list_del(&rec->list);
+			kfree(rec);
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief vpu interrupt handler
+ */
+static irqreturn_t vpu_irq_handler(int irq, void *dev_id)
+{
+	struct vpu_priv *dev = dev_id;
+
+	READ_REG(BIT_INT_STATUS);
+	WRITE_REG(0x1, BIT_INT_CLEAR);
+
+	if (dev->async_queue)
+		kill_fasync(&dev->async_queue, SIGIO, POLL_IN);
+
+	/*
+	 * Clock is gated on when dec/enc started, gate it off when
+	 * interrupt is received.
+	 */
+	clk_disable(vpu_clk);
+
+	codec_done = 1;
+	wake_up_interruptible(&vpu_queue);
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * @brief open function for vpu file operation
+ *
+ * @return  0 on success or negative error code on error
+ */
+static int vpu_open(struct inode *inode, struct file *filp)
+{
+	spin_lock(&vpu_lock);
+	if ((open_count++ == 0) && cpu_is_mx32())
+		vl2cc_enable();
+	filp->private_data = (void *)(&vpu_data);
+	spin_unlock(&vpu_lock);
+	return 0;
+}
+
+/*!
+ * @brief IO ctrl function for vpu file operation
+ * @param cmd IO ctrl command
+ * @return  0 on success or negative error code on error
+ */
+static int vpu_ioctl(struct inode *inode, struct file *filp, u_int cmd,
+		     u_long arg)
+{
+	int ret = 0;
+
+	switch (cmd) {
+	case VPU_IOC_PHYMEM_ALLOC:
+		{
+			struct memalloc_record *rec;
+
+			rec = kzalloc(sizeof(*rec), GFP_KERNEL);
+			if (!rec)
+				return -ENOMEM;
+
+			ret = copy_from_user(&(rec->mem),
+					     (struct vpu_mem_desc *)arg,
+					     sizeof(struct vpu_mem_desc));
+			if (ret) {
+				kfree(rec);
+				return -EFAULT;
+			}
+
+			pr_debug("[ALLOC] mem alloc size = 0x%x\n",
+				 rec->mem.size);
+
+			ret = vpu_alloc_dma_buffer(&(rec->mem));
+			if (ret == -1) {
+				kfree(rec);
+				printk(KERN_ERR
+				       "Physical memory allocation error!\n");
+				break;
+			}
+			ret = copy_to_user((void __user *)arg, &(rec->mem),
+					   sizeof(struct vpu_mem_desc));
+			if (ret) {
+				kfree(rec);
+				ret = -EFAULT;
+				break;
+			}
+
+			spin_lock(&vpu_lock);
+			list_add(&rec->list, &head);
+			spin_unlock(&vpu_lock);
+
+			break;
+		}
+	case VPU_IOC_PHYMEM_FREE:
+		{
+			struct memalloc_record *rec, *n;
+			struct vpu_mem_desc vpu_mem;
+
+			ret = copy_from_user(&vpu_mem,
+					     (struct vpu_mem_desc *)arg,
+					     sizeof(struct vpu_mem_desc));
+			if (ret)
+				return -EACCES;
+
+			pr_debug("[FREE] mem freed cpu_addr = 0x%x\n",
+				 vpu_mem.cpu_addr);
+			if ((void *)vpu_mem.cpu_addr != NULL) {
+				vpu_free_dma_buffer(&vpu_mem);
+			}
+
+			spin_lock(&vpu_lock);
+			list_for_each_entry_safe(rec, n, &head, list) {
+				if (rec->mem.cpu_addr == vpu_mem.cpu_addr) {
+					/* delete from list */
+					list_del(&rec->list);
+					kfree(rec);
+					break;
+				}
+			}
+			spin_unlock(&vpu_lock);
+
+			break;
+		}
+	case VPU_IOC_WAIT4INT:
+		{
+			u_long timeout = (u_long) arg;
+			if (!wait_event_interruptible_timeout
+			    (vpu_queue, codec_done != 0,
+			     msecs_to_jiffies(timeout))) {
+				printk(KERN_WARNING "VPU blocking: timeout.\n");
+				ret = -ETIME;
+			} else if (signal_pending(current)) {
+				printk(KERN_WARNING
+				       "VPU interrupt received.\n");
+				ret = -ERESTARTSYS;
+			} else
+				codec_done = 0;
+			break;
+		}
+	case VPU_IOC_VL2CC_FLUSH:
+		if (cpu_is_mx32()) {
+			vl2cc_flush();
+		}
+		break;
+	case VPU_IOC_IRAM_SETTING:
+		{
+			ret = copy_to_user((void __user *)arg, &iram,
+					   sizeof(struct iram_setting));
+			if (ret)
+				ret = -EFAULT;
+
+			break;
+		}
+	case VPU_IOC_CLKGATE_SETTING:
+		{
+			u32 clkgate_en;
+
+			if (get_user(clkgate_en, (u32 __user *) arg))
+				return -EFAULT;
+
+			if (clkgate_en) {
+				clk_enable(vpu_clk);
+			} else {
+				clk_disable(vpu_clk);
+			}
+
+			break;
+		}
+	case VPU_IOC_GET_SHARE_MEM:
+		{
+			spin_lock(&vpu_lock);
+			if (share_mem.cpu_addr != 0) {
+				ret = copy_to_user((void __user *)arg,
+						   &share_mem,
+						   sizeof(struct vpu_mem_desc));
+				spin_unlock(&vpu_lock);
+				break;
+			} else {
+				if (copy_from_user(&share_mem,
+						   (struct vpu_mem_desc *)arg,
+						 sizeof(struct vpu_mem_desc))) {
+					spin_unlock(&vpu_lock);
+					return -EFAULT;
+				}
+				if (vpu_alloc_dma_buffer(&share_mem) == -1)
+					ret = -EFAULT;
+				else {
+					if (copy_to_user((void __user *)arg,
+							 &share_mem,
+							 sizeof(struct
+								vpu_mem_desc)))
+						ret = -EFAULT;
+				}
+			}
+			spin_unlock(&vpu_lock);
+			break;
+		}
+	case VPU_IOC_GET_WORK_ADDR:
+		{
+			if (bitwork_mem.cpu_addr != 0) {
+				ret =
+				    copy_to_user((void __user *)arg,
+						 &bitwork_mem,
+						 sizeof(struct vpu_mem_desc));
+				break;
+			} else {
+				if (copy_from_user(&bitwork_mem,
+						   (struct vpu_mem_desc *)arg,
+						   sizeof(struct vpu_mem_desc)))
+					return -EFAULT;
+
+				if (vpu_alloc_dma_buffer(&bitwork_mem) == -1)
+					ret = -EFAULT;
+				else if (copy_to_user((void __user *)arg,
+						      &bitwork_mem,
+						      sizeof(struct
+							     vpu_mem_desc)))
+					ret = -EFAULT;
+			}
+			break;
+		}
+	case VPU_IOC_GET_PIC_PARA_ADDR:
+		{
+			if (pic_para_mem.cpu_addr != 0) {
+				ret =
+				    copy_to_user((void __user *)arg,
+						 &pic_para_mem,
+						 sizeof(struct vpu_mem_desc));
+				break;
+			} else {
+				if (copy_from_user(&pic_para_mem,
+						   (struct vpu_mem_desc *)arg,
+						   sizeof(struct vpu_mem_desc)))
+					return -EFAULT;
+
+				if (vpu_alloc_dma_buffer(&pic_para_mem) == -1)
+					ret = -EFAULT;
+				else if (copy_to_user((void __user *)arg,
+						      &pic_para_mem,
+						      sizeof(struct
+							     vpu_mem_desc)))
+					ret = -EFAULT;
+			}
+			break;
+		}
+	case VPU_IOC_GET_USER_DATA_ADDR:
+		{
+			if (user_data_mem.cpu_addr != 0) {
+				ret =
+				    copy_to_user((void __user *)arg,
+						 &user_data_mem,
+						 sizeof(struct vpu_mem_desc));
+				break;
+			} else {
+				if (copy_from_user(&user_data_mem,
+						   (struct vpu_mem_desc *)arg,
+						   sizeof(struct vpu_mem_desc)))
+					return -EFAULT;
+
+				if (vpu_alloc_dma_buffer(&user_data_mem) == -1)
+					ret = -EFAULT;
+				else if (copy_to_user((void __user *)arg,
+						      &user_data_mem,
+						      sizeof(struct
+							     vpu_mem_desc)))
+					ret = -EFAULT;
+			}
+			break;
+		}
+	case VPU_IOC_SYS_SW_RESET:
+		{
+			if (cpu_is_mx37() || cpu_is_mx51()) {
+				u32 reg;
+
+				reg = __raw_readl(src_base_addr);
+				reg |= 0x02;	/* SW_VPU_RST_BIT */
+				__raw_writel(reg, src_base_addr);
+				while (__raw_readl(src_base_addr) & 0x02)
+					;
+			}
+			break;
+		}
+	case VPU_IOC_REG_DUMP:
+		break;
+	case VPU_IOC_PHYMEM_DUMP:
+		break;
+	default:
+		{
+			printk(KERN_ERR "No such IOCTL, cmd is %d\n", cmd);
+			break;
+		}
+	}
+	return ret;
+}
+
+/*!
+ * @brief Release function for vpu file operation
+ * @return  0 on success or negative error code on error
+ */
+static int vpu_release(struct inode *inode, struct file *filp)
+{
+	spin_lock(&vpu_lock);
+	if (open_count > 0 && !(--open_count)) {
+		vpu_free_buffers();
+
+		if (cpu_is_mx32())
+			vl2cc_disable();
+
+		/* Free shared memory when vpu device is idle */
+		vpu_free_dma_buffer(&share_mem);
+		share_mem.cpu_addr = 0;
+	}
+	spin_unlock(&vpu_lock);
+
+	return 0;
+}
+
+/*!
+ * @brief fasync function for vpu file operation
+ * @return  0 on success or negative error code on error
+ */
+static int vpu_fasync(int fd, struct file *filp, int mode)
+{
+	struct vpu_priv *dev = (struct vpu_priv *)filp->private_data;
+	return fasync_helper(fd, filp, mode, &dev->async_queue);
+}
+
+/*!
+ * @brief memory map function of harware registers for vpu file operation
+ * @return  0 on success or negative error code on error
+ */
+static int vpu_map_hwregs(struct file *fp, struct vm_area_struct *vm)
+{
+	unsigned long pfn;
+
+	vm->vm_flags |= VM_IO | VM_RESERVED;
+	vm->vm_page_prot = pgprot_noncached(vm->vm_page_prot);
+	pfn = VPU_BASE_ADDR >> PAGE_SHIFT;
+	pr_debug("size=0x%x,  page no.=0x%x\n",
+		 (int)(vm->vm_end - vm->vm_start), (int)pfn);
+	return remap_pfn_range(vm, vm->vm_start, pfn, vm->vm_end - vm->vm_start,
+			       vm->vm_page_prot) ? -EAGAIN : 0;
+}
+
+/*!
+ * @brief memory map function of memory for vpu file operation
+ * @return  0 on success or negative error code on error
+ */
+static int vpu_map_mem(struct file *fp, struct vm_area_struct *vm)
+{
+	int request_size;
+	request_size = vm->vm_end - vm->vm_start;
+
+	pr_debug(" start=0x%x, pgoff=0x%x, size=0x%x\n",
+		 (unsigned int)(vm->vm_start), (unsigned int)(vm->vm_pgoff),
+		 request_size);
+
+	vm->vm_flags |= VM_IO | VM_RESERVED;
+	vm->vm_page_prot = pgprot_noncached(vm->vm_page_prot);
+
+	return remap_pfn_range(vm, vm->vm_start, vm->vm_pgoff,
+			       request_size, vm->vm_page_prot) ? -EAGAIN : 0;
+
+}
+
+/*!
+ * @brief memory map interface for vpu file operation
+ * @return  0 on success or negative error code on error
+ */
+static int vpu_mmap(struct file *fp, struct vm_area_struct *vm)
+{
+	if (vm->vm_pgoff)
+		return vpu_map_mem(fp, vm);
+	else
+		return vpu_map_hwregs(fp, vm);
+}
+
+struct file_operations vpu_fops = {
+	.owner = THIS_MODULE,
+	.open = vpu_open,
+	.ioctl = vpu_ioctl,
+	.release = vpu_release,
+	.fasync = vpu_fasync,
+	.mmap = vpu_mmap,
+};
+
+/*!
+ * This function is called by the driver framework to initialize the vpu device.
+ * @param   dev The device structure for the vpu passed in by the framework.
+ * @return   0 on success or negative error code on error
+ */
+static int vpu_dev_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct device *temp_class;
+	struct resource *res;
+
+	if (cpu_is_mx32()) {
+		/* Obtain VL2CC base address */
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+		if (!res) {
+			printk(KERN_ERR "vpu: unable to get VL2CC base\n");
+			return -ENODEV;
+		}
+
+		err = vl2cc_init(res->start);
+		if (err != 0)
+			return err;
+	}
+
+	if (cpu_is_mx37() || cpu_is_mx51()) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+		if (!res) {
+			printk(KERN_ERR "vpu: unable to get VPU IRAM base\n");
+			return -ENODEV;
+		}
+		iram.start = res->start;
+		iram.end = res->end;
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+		if (!res) {
+			printk(KERN_ERR "vpu: unable to get src base addr\n");
+			return -ENODEV;
+		}
+		src_base_addr = res->start;
+	}
+
+	vpu_major = register_chrdev(vpu_major, "mxc_vpu", &vpu_fops);
+	if (vpu_major < 0) {
+		printk(KERN_ERR "vpu: unable to get a major for VPU\n");
+		err = -EBUSY;
+		goto error;
+	}
+
+	vpu_class = class_create(THIS_MODULE, "mxc_vpu");
+	if (IS_ERR(vpu_class)) {
+		err = PTR_ERR(vpu_class);
+		goto err_out_chrdev;
+	}
+
+	temp_class = device_create(vpu_class, NULL, MKDEV(vpu_major, 0),
+				   NULL, "mxc_vpu");
+	if (IS_ERR(temp_class)) {
+		err = PTR_ERR(temp_class);
+		goto err_out_class;
+	}
+
+	vpu_clk = clk_get(&pdev->dev, "vpu_clk");
+	if (IS_ERR(vpu_clk)) {
+		err = -ENOENT;
+		goto err_out_class;
+	}
+
+	err = request_irq(MXC_INT_VPU, vpu_irq_handler, 0, "VPU_CODEC_IRQ",
+			  (void *)(&vpu_data));
+	if (err)
+		goto err_out_class;
+
+	printk(KERN_INFO "VPU initialized\n");
+	goto out;
+
+      err_out_class:
+	device_destroy(vpu_class, MKDEV(vpu_major, 0));
+	class_destroy(vpu_class);
+      err_out_chrdev:
+	unregister_chrdev(vpu_major, "mxc_vpu");
+      error:
+	if (cpu_is_mx32()) {
+		vl2cc_cleanup();
+	}
+      out:
+	return err;
+}
+
+#ifdef CONFIG_PM
+static int vpu_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int i;
+	unsigned long timeout;
+
+	/* Wait for vpu go to idle state, suspect vpu cannot be changed
+	   to idle state after about 1 sec */
+	if (open_count > 0) {
+		timeout = jiffies + HZ;
+		clk_enable(vpu_clk);
+		while (READ_REG(BIT_BUSY_FLAG)) {
+			msleep(1);
+			if (time_after(jiffies, timeout))
+				goto out;
+		}
+		clk_disable(vpu_clk);
+	}
+
+	/* Make sure clock is disabled before suspend */
+	vpu_clk_usercount = clk_get_usecount(vpu_clk);
+	for (i = 0; i < vpu_clk_usercount; i++)
+		clk_disable(vpu_clk);
+
+	clk_enable(vpu_clk);
+	if (bitwork_mem.cpu_addr != 0) {
+		SAVE_WORK_REGS;
+		SAVE_CTRL_REGS;
+		SAVE_RDWR_PTR_REGS;
+		SAVE_DIS_FLAG_REGS;
+
+		WRITE_REG(0x1, BIT_BUSY_FLAG);
+		WRITE_REG(VPU_SLEEP_REG_VALUE, BIT_RUN_COMMAND);
+		while (READ_REG(BIT_BUSY_FLAG)) ;
+	}
+
+	clk_disable(vpu_clk);
+
+	if (cpu_is_mx37() || cpu_is_mx51())
+		mxc_pg_enable(pdev);
+
+	return 0;
+
+out:
+	clk_disable(vpu_clk);
+	return -EAGAIN;
+
+}
+
+static int vpu_resume(struct platform_device *pdev)
+{
+	int i;
+
+	if (cpu_is_mx37() || cpu_is_mx51())
+		mxc_pg_disable(pdev);
+
+	clk_enable(vpu_clk);
+
+	if (bitwork_mem.cpu_addr != 0) {
+		u32 *p = (u32 *) bitwork_mem.cpu_addr;
+		u32 data;
+		u16 data_hi;
+		u16 data_lo;
+
+		RESTORE_WORK_REGS;
+
+		WRITE_REG(0x0, BIT_RESET_CTRL);
+		WRITE_REG(0x0, BIT_CODE_RUN);
+
+		/*
+		 * Re-load boot code, from the codebuffer in external RAM.
+		 * Thankfully, we only need 4096 bytes, same for all platforms.
+		 */
+		if (cpu_is_mx51()) {
+			for (i = 0; i < 2048; i += 4) {
+				data = p[(i / 2) + 1];
+				data_hi = (data >> 16) & 0xFFFF;
+				data_lo = data & 0xFFFF;
+				WRITE_REG((i << 16) | data_hi, BIT_CODE_DOWN);
+				WRITE_REG(((i + 1) << 16) | data_lo,
+					  BIT_CODE_DOWN);
+
+				data = p[i / 2];
+				data_hi = (data >> 16) & 0xFFFF;
+				data_lo = data & 0xFFFF;
+				WRITE_REG(((i + 2) << 16) | data_hi,
+					  BIT_CODE_DOWN);
+				WRITE_REG(((i + 3) << 16) | data_lo,
+					  BIT_CODE_DOWN);
+			}
+		} else {
+			for (i = 0; i < 2048; i += 2) {
+				if (cpu_is_mx37())
+					data = swab32(p[i / 2]);
+				else
+					data = p[i / 2];
+				data_hi = (data >> 16) & 0xFFFF;
+				data_lo = data & 0xFFFF;
+
+				WRITE_REG((i << 16) | data_hi, BIT_CODE_DOWN);
+				WRITE_REG(((i + 1) << 16) | data_lo,
+					  BIT_CODE_DOWN);
+			}
+		}
+
+		RESTORE_CTRL_REGS;
+
+		WRITE_REG(BITVAL_PIC_RUN, BIT_INT_ENABLE);
+
+		WRITE_REG(0x1, BIT_BUSY_FLAG);
+		WRITE_REG(0x1, BIT_CODE_RUN);
+		while (READ_REG(BIT_BUSY_FLAG)) ;
+
+		RESTORE_RDWR_PTR_REGS;
+		RESTORE_DIS_FLAG_REGS;
+
+		WRITE_REG(0x1, BIT_BUSY_FLAG);
+		WRITE_REG(VPU_WAKE_REG_VALUE, BIT_RUN_COMMAND);
+		while (READ_REG(BIT_BUSY_FLAG)) ;
+	}
+
+	clk_disable(vpu_clk);
+
+	/* Recover vpu clock */
+	for (i = 0; i < vpu_clk_usercount; i++)
+		clk_enable(vpu_clk);
+
+	return 0;
+}
+#else
+#define	vpu_suspend	NULL
+#define	vpu_resume	NULL
+#endif				/* !CONFIG_PM */
+
+/*! Driver definition
+ *
+ */
+static struct platform_driver mxcvpu_driver = {
+	.driver = {
+		   .name = "mxc_vpu",
+		   },
+	.probe = vpu_dev_probe,
+	.suspend = vpu_suspend,
+	.resume = vpu_resume,
+};
+
+static int __init vpu_init(void)
+{
+	int ret = platform_driver_register(&mxcvpu_driver);
+
+	init_waitqueue_head(&vpu_queue);
+
+	return ret;
+}
+
+static void __exit vpu_exit(void)
+{
+	free_irq(MXC_INT_VPU, (void *)(&vpu_data));
+	if (vpu_major > 0) {
+		device_destroy(vpu_class, MKDEV(vpu_major, 0));
+		class_destroy(vpu_class);
+		unregister_chrdev(vpu_major, "mxc_vpu");
+		vpu_major = 0;
+	}
+
+	if (cpu_is_mx32()) {
+		vl2cc_cleanup();
+	}
+
+	vpu_free_dma_buffer(&bitwork_mem);
+	vpu_free_dma_buffer(&pic_para_mem);
+	vpu_free_dma_buffer(&user_data_mem);
+
+	clk_put(vpu_clk);
+
+	platform_driver_unregister(&mxcvpu_driver);
+	return;
+}
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Linux VPU driver for Freescale i.MX/MXC");
+MODULE_LICENSE("GPL");
+
+module_init(vpu_init);
+module_exit(vpu_exit);
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 231eeaf1d552..601215803b79 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -962,6 +962,17 @@ config ENC28J60_WRITEVERIFY
 	  Enable the verify after the buffer write useful for debugging purpose.
 	  If unsure, say N.
 
+config NS9XXX_ETH
+	tristate "Digi NS9xxx ethernet support"
+	depends on ARM && ARCH_NS9XXX
+	depends on PHYLIB
+	depends on CRC32
+	help
+	  If you have a Digi NS9xxx based system and wish to use its ethernet
+	  port.
+	  If you choose to build this driver as a module, it will be called
+	  ns9xxx-eth.
+
 config SMC911X
 	tristate "SMSC LAN911[5678] support"
 	select CRC32
@@ -970,14 +981,35 @@ config SMC911X
 	help
 	  This is a driver for SMSC's LAN911x series of Ethernet chipsets
 	  including the new LAN9115, LAN9116, LAN9117, and LAN9118.
-	  Say Y if you want it compiled into the kernel, 
+	  Say Y if you want it compiled into the kernel,
 	  and read the Ethernet-HOWTO, available from
 	  <http://www.linuxdoc.org/docs.html#howto>.
 
-	  This driver is also available as a module. The module will be 
-	  called smc911x.  If you want to compile it as a module, say M 
+	  This driver is also available as a module. The module will be
+	  called smc911x.  If you want to compile it as a module, say M
 	  here and read <file:Documentation/kbuild/modules.txt>
 
+config SMSC911X
+	tristate "SMSC LAN911x/LAN921x families embedded ethernet support"
+	depends on NET_ETHERNET
+	select CRC32
+	select MII
+	---help---
+	  Say Y here if you want support for SMSC LAN911x and LAN921x families
+	  of ethernet controllers.
+	  To compile this driver as a module, choose M here and read
+	  <file:Documentation/networking/net-modules.txt>. The module
+	  will be called smsc911x.
+
+config SMSC9118
+	tristate "SMSC LAN9218 support"
+	depends on NET_ETHERNET && (MACH_CC9M2443JS || MACH_CCW9M2443JS || MACH_CCWMX51JS || MACH_CCMX51JS)
+	select CRC32
+	select MII
+	---help---
+	  Say Y here if you want support for SMSC LAN921x families
+	  of ethernet controllers.
+
 config NET_VENDOR_RACAL
 	bool "Racal-Interlan (Micom) NI cards"
 	depends on ISA
@@ -1393,7 +1425,7 @@ config FORCEDETH_NAPI
 config CS89x0
 	tristate "CS89x0 support"
 	depends on NET_ETHERNET && (ISA || EISA || MACH_IXDP2351 \
-		|| ARCH_IXDP2X01 || ARCH_PNX010X || MACH_MX31ADS)
+		|| ARCH_IXDP2X01 || ARCH_PNX010X || ARCH_MXC)
 	---help---
 	  Support for CS89x0 chipset based Ethernet cards. If you have a
 	  network (Ethernet) card of this type, say Y and read the
@@ -1407,7 +1439,7 @@ config CS89x0
 config CS89x0_NONISA_IRQ
 	def_bool y
 	depends on CS89x0 != n
-	depends on MACH_IXDP2351 || ARCH_IXDP2X01 || ARCH_PNX010X || MACH_MX31ADS
+	depends on MACH_IXDP2351 || ARCH_IXDP2X01 || ARCH_PNX010X || ARCH_MXC
 
 config TC35815
 	tristate "TOSHIBA TC35815 Ethernet support"
@@ -1433,9 +1465,9 @@ config E100
 	select MII
 	---help---
 	  This driver supports Intel(R) PRO/100 family of adapters.
-	  To verify that your adapter is supported, find the board ID number 
-	  on the adapter. Look for a label that has a barcode and a number 
-	  in the format 123456-001 (six digits hyphen three digits). 
+	  To verify that your adapter is supported, find the board ID number
+	  on the adapter. Look for a label that has a barcode and a number
+	  in the format 123456-001 (six digits hyphen three digits).
 
 	  Use the above information and the Adapter & Driver ID Guide at:
 
@@ -1447,7 +1479,7 @@ config E100
 
 	  <http://appsr.intel.com/scripts-df/support_intel.asp>
 
-	  More specific information on configuring the driver is in 
+	  More specific information on configuring the driver is in
 	  <file:Documentation/networking/e100.txt>.
 
 	  To compile this driver as a module, choose M here. The module
@@ -1810,11 +1842,11 @@ config 68360_ENET
 	  the Motorola 68360 processor.
 
 config FEC
-	bool "FEC ethernet controller (of ColdFire CPUs)"
-	depends on M523x || M527x || M5272 || M528x || M520x
+	tristate "FEC ethernet controller"
+	depends on M523x || M527x || M5272 || M528x || M520x || ARCH_MX27 || ARCH_MX37 || ARCH_MX35 || ARCH_MX51 || ARCH_MX25
 	help
 	  Say Y here if you want to use the built-in 10/100 Fast ethernet
-	  controller on some Motorola ColdFire processors.
+	  controller on some Motorola/Freescale processors.
 
 config FEC2
 	bool "Second FEC ethernet controller (on some ColdFire CPUs)"
@@ -1935,7 +1967,7 @@ config E1000
 	depends on PCI
 	---help---
 	  This driver supports Intel(R) PRO/1000 gigabit ethernet family of
-	  adapters.  For more information on how to identify your adapter, go 
+	  adapters.  For more information on how to identify your adapter, go
 	  to the Adapter & Driver ID Guide at:
 
 	  <http://support.intel.com/support/network/adapter/pro100/21397.htm>
@@ -1945,7 +1977,7 @@ config E1000
 
 	  <http://support.intel.com>
 
-	  More specific information on configuring the driver is in 
+	  More specific information on configuring the driver is in
 	  <file:Documentation/networking/e1000.txt>.
 
 	  To compile this driver as a module, choose M here. The module
@@ -2122,7 +2154,7 @@ config SKGE
 	  and related Gigabit Ethernet adapters. It is a new smaller driver
 	  with better performance and more complete ethtool support.
 
-	  It does not support the link failover and network management 
+	  It does not support the link failover and network management
 	  features that "portable" vendor supplied sk98lin driver does.
 
 	  This driver supports adapters based on the original Yukon chipset:
@@ -2466,7 +2498,7 @@ config IXGB
 
 	  <http://support.intel.com>
 
-	  More specific information on configuring the driver is in 
+	  More specific information on configuring the driver is in
 	  <file:Documentation/networking/ixgb.txt>.
 
 	  To compile this driver as a module, choose M here. The module
@@ -2476,8 +2508,8 @@ config S2IO
 	tristate "S2IO 10Gbe XFrame NIC"
 	depends on PCI
 	---help---
-	  This driver supports the 10Gbe XFrame NIC of S2IO. 
-	  More specific information on configuring the driver is in 
+	  This driver supports the 10Gbe XFrame NIC of S2IO.
+	  More specific information on configuring the driver is in
 	  <file:Documentation/networking/s2io.txt>.
 
 config MYRI10GE
@@ -2897,9 +2929,9 @@ config PPPOE
 	  Support for PPP over Ethernet.
 
 	  This driver requires the latest version of pppd from the CVS
-	  repository at cvs.samba.org.  Alternatively, see the 
+	  repository at cvs.samba.org.  Alternatively, see the
 	  RoaringPenguin package (<http://www.roaringpenguin.com/pppoe>)
-	  which contains instruction on how to use this driver (under 
+	  which contains instruction on how to use this driver (under
 	  the heading "Kernel mode PPPoE").
 
 config PPPOATM
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index 017383ad5ec6..c7cf052d08cf 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -220,18 +220,21 @@ obj-$(CONFIG_S2IO) += s2io.o
 obj-$(CONFIG_MYRI10GE) += myri10ge/
 obj-$(CONFIG_SMC91X) += smc91x.o
 obj-$(CONFIG_SMC911X) += smc911x.o
+obj-$(CONFIG_SMSC911X) += smsc911x.o
 obj-$(CONFIG_BFIN_MAC) += bfin_mac.o
 obj-$(CONFIG_DM9000) += dm9000.o
 obj-$(CONFIG_PASEMI_MAC) += pasemi_mac_driver.o
 pasemi_mac_driver-objs := pasemi_mac.o pasemi_mac_ethtool.o
 obj-$(CONFIG_MLX4_CORE) += mlx4/
 obj-$(CONFIG_ENC28J60) += enc28j60.o
+obj-$(CONFIG_NS9XXX_ETH) += ns9xxx-eth.o
 
 obj-$(CONFIG_XTENSA_XT2000_SONIC) += xtsonic.o
 
 obj-$(CONFIG_MACB) += macb.o
 
 obj-$(CONFIG_ARM) += arm/
+obj-$(CONFIG_SMSC9118) += smsc9118/
 obj-$(CONFIG_DEV_APPLETALK) += appletalk/
 obj-$(CONFIG_TR) += tokenring/
 obj-$(CONFIG_WAN) += wan/
diff --git a/drivers/net/can/Kconfig b/drivers/net/can/Kconfig
index 57def0d57371..3ac5a12a0c8b 100644
--- a/drivers/net/can/Kconfig
+++ b/drivers/net/can/Kconfig
@@ -12,6 +12,164 @@ config CAN_VCAN
 	  This driver can also be built as a module.  If so, the module
 	  will be called vcan.
 
+config CAN_SLCAN
+	tristate "Serial / USB serial CAN Adaptors (slcan)"
+	depends on CAN && EXPERIMENTAL
+	default N
+	---help---
+	  CAN driver for several 'low cost' CAN interfaces that are attached
+	  via serial lines or via USB-to-serial adapters using the LAWICEL
+	  ASCII protocol. The driver implements the tty linediscipline N_SLCAN.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called slcan.
+
+config CAN_OLD_DRIVERS
+	tristate "Prompt for old CAN drivers (e.g. no sysfs support)"
+	depends on CAN
+	default N
+	---help---
+	  The old drivers do not support sysfs nor proper platform device
+	  support. Some of the old drivers might only be configured by
+	  module commandline options.
+
+if CAN_OLD_DRIVERS
+source "drivers/net/can/old/Kconfig"
+endif
+
+config CAN_DEV
+	tristate "Prompt for platform CAN drivers with sysfs support"
+	depends on CAN && SYSFS
+	default Y
+	---help---
+	  Enables the common framework for platform CAN drivers with sysfs
+	  support. This is the standard library for CAN drivers.
+	  If unsure, say Y.
+
+config CAN_CALC_BITTIMING
+	bool "CAN bit-timing calculation"
+	depends on CAN_DEV
+	default Y
+	---help---
+	  If enabled, CAN bit-timing parameters will be calculated for the
+	  bit-rate specified via SYSFS file "bitrate" when the device gets
+	  started. This works fine for the most common CAN controllers
+	  with standard bit-rates but may fail for exotic bit-rates or CAN
+	  source clock frequencies. Disabling saves some space, but then the
+	  bit-timing parameters must be specified directly using the SYSFS
+	  files "tq", "prop_seg", "phase_seg1", "phase_seg2" and "sjw".
+	  If unsure, say Y.
+
+config CAN_SJA1000
+	depends on CAN_DEV
+	tristate "Philips SJA1000"
+	---help---
+	  The SJA1000 is one of the top CAN controllers out there. As it
+	  has a multiplexed interface it fits directly to 8051
+	  microcontrollers or into the PC I/O port space. The SJA1000
+	  is a full CAN controller, with shadow registers for RX and TX.
+	  It can send and receive any kinds of CAN frames (SFF/EFF/RTR)
+	  with a single (simple) filter setup.
+
+	  This driver will use the new device interface.
+
+config CAN_SJA1000_PLATFORM
+	depends on CAN_SJA1000
+	tristate "generic Platform Bus based SJA1000 driver"
+	---help---
+	  This driver adds support for the SJA1000 chips connected to
+	  the "platform bus" (Linux abstraction for directly to the
+	  processor attached devices).	Which can be found on various
+	  boards from Phytec (http://www.phytec.de) like the PCM027,
+	  PCM038.
+
+config CAN_SJA1000_OF_PLATFORM
+	depends on CAN_SJA1000 && PPC_OF
+	tristate "generic OF Platform Bus based SJA1000 driver"
+	---help---
+	  This driver adds support for the SJA1000 chips connected to
+	  the OpenFirmware "platform bus" found on embedded systems with
+	  OpenFirmware bindings, e.g. if you have a PowerPC based system
+	  you should enable this option.
+
+config CAN_EMS_PCI
+	tristate "EMS CPC-PCI and CPC-PCIe Card"
+	depends on PCI && CAN_SJA1000
+	---help---
+	  This driver is for the one or two channel CPC-PCI and CPC-PCIe
+	  cards from EMS Dr. Thomas Wuensche (http://www.ems-wuensche.de).
+
+config CAN_EMS_PCMCIA
+	tristate "EMS CPC-CARD Card"
+	depends on PCMCIA && CAN_SJA1000
+	---help---
+	  This driver is for the one or two channel CPC-CARD cards from
+	  EMS Dr. Thomas Wuensche (http://www.ems-wuensche.de).
+
+config CAN_IXXAT_PCI
+	tristate "IXXAT PCI Card"
+	depends on PCI && CAN_SJA1000
+	---help---
+	  This driver is for the IXXAT PC-I 04/PCI card (1 or 2 channel)
+	  from the IXXAT Automation GmbH (http://www.ixxat.de).
+
+config CAN_PEAK_PCI
+	tristate "PEAK PCAN PCI Card"
+	depends on PCI && CAN_SJA1000
+	---help---
+	  This driver is for the PCAN PCI, the PC-PCI CAN plug-in card (1 or
+	  2 channel) from PEAK Systems (http://www.peak-system.com).
+
+config CAN_PIPCAN
+	depends on CAN_SJA1000
+	tristate "MPL PIPCAN CAN module driver (SJA1000)"
+	---help---
+	  This driver adds support for the PIPCAN module used on some SBC
+	  boards from MPL AG (http://www.mpl.ch).
+
+config CAN_KVASER_PCI
+	tristate "Kvaser PCIcanx and Kvaser PCIcan PCI Cards"
+	depends on PCI && CAN_SJA1000
+	---help---
+	  This driver is for the the PCIcanx and PCIcan cards (1, 2 or
+	  4 channel) from Kvaser (http://www.kvaser.com).
+
+config CAN_SOFTING
+	tristate "Softing Gmbh CAN generic support"
+	depends on CAN_DEV
+	---help---
+	  generic softing CAN cards
+
+config CAN_SOFTING_CS
+	tristate "Softing CAN pcmcia cards"
+	depends on CAN_SOFTING && PCMCIA
+
+config CAN_MSCAN
+	depends on CAN_DEV && (PPC || M68K || M68KNOMMU)
+	tristate "Support for a Freescale MSCAN based chips"
+	---help---
+	  The Motorola Scalable Controller Area Network (MSCAN) definition
+	  is based on the MSCAN12 definition which is the specific
+	  implementation of the Motorola Scalable CAN concept targeted for
+	  the Motorola MC68HC12 Microcontroller Family.
+
+config CAN_MPC52XX
+	tristate "Freescale MPC5200 onboard CAN controller"
+	depends on CAN_MSCAN && (PPC_MPC52xx || PPC_52xx)
+	default LITE5200
+	---help---
+	  If you say yes here you get support for Freescale MPC5200
+	  onboard dualCAN controller.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called mpc52xx_can.
+
+config CAN_MCP251X
+	tristate "Microchip MCP251x SPI CAN controllers"
+	depends on CAN_DEV && SPI
+	---help---
+	  Driver for the Microchip MCP251x SPI CAN controllers.
+
 config CAN_DEBUG_DEVICES
 	bool "CAN devices debugging messages"
 	depends on CAN
@@ -22,4 +180,13 @@ config CAN_DEBUG_DEVICES
 	  a problem with CAN support and want to see more of what is going
 	  on.
 
+config CAN_FLEXCAN
+        tristate "Freescale FlexCAN"
+        depends on CAN && (ARCH_MX25 || ARCH_MX35)
+        default m
+        ---help---
+          This select the support of Freescale CAN(FlexCAN).
+          This driver can also be built as a module.
+          If unsure, say N.
+
 endmenu
diff --git a/drivers/net/can/Makefile b/drivers/net/can/Makefile
index c4bead705cd9..60c7ecca200d 100644
--- a/drivers/net/can/Makefile
+++ b/drivers/net/can/Makefile
@@ -1,5 +1,49 @@
 #
-#  Makefile for the Linux Controller Area Network drivers.
 #
 
+
+ifeq ($(KERNELRELEASE),)
+
+KERNELDIR := /lib/modules/$(shell uname -r)/build
+PWD       := $(shell pwd)
+TOPDIR    := $(PWD)/../../..
+
+export CONFIG_CAN_VCAN=m
+export CONFIG_CAN_DEV=m
+#export CONFIG_CAN_SJA1000_OLD=m
+#export CONFIG_CAN_I82527_OLD=m
+export CONFIG_CAN_SJA1000=m
+export CONFIG_CAN_SJA1000_PLATFORM=m
+export CONFIG_CAN_EMS_PCI=m
+export CONFIG_CAN_EMS_PCMCIA=m
+export CONFIG_CAN_PIPCAN=m
+export CONFIG_CAN_SOFTING=m
+export CONFIG_CAN_SOFTING_CS=m
+export CONFIG_CAN_MCP251X=m
+
+modules modules_install clean:
+	$(MAKE) -C $(KERNELDIR) M=$(PWD) $@ TOPDIR=$(TOPDIR)
+
+else
+
+-include $(TOPDIR)/Makefile.common
+
 obj-$(CONFIG_CAN_VCAN)		+= vcan.o
+obj-$(CONFIG_CAN_SLCAN)		+= slcan.o
+
+obj-$(CONFIG_CAN_DEV)		+= can-dev.o
+can-dev-y			:= dev.o sysfs.o
+
+obj-$(CONFIG_CAN_SJA1000)	+= sja1000/
+obj-$(CONFIG_CAN_SOFTING)	+= softing/
+obj-$(CONFIG_CAN_MSCAN)		+= mscan/
+obj-$(CONFIG_CAN_SJA1000_OLD)	+= old/sja1000/
+obj-$(CONFIG_CAN_I82527_OLD)	+= old/i82527/
+obj-$(CONFIG_CAN_MSCAN_OLD)	+= old/mscan/
+obj-$(CONFIG_CAN_CCAN_OLD)	+= old/ccan/
+obj-$(CONFIG_CAN_MCP251X)	+= mcp251x.o
+
+ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
+
+endif
+obj-$(CONFIG_CAN_FLEXCAN)	+= flexcan/
diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
new file mode 100644
index 000000000000..faa75d14cf29
--- /dev/null
+++ b/drivers/net/can/dev.c
@@ -0,0 +1,528 @@
+/*
+ * Copyright (C) 2005 Marc Kleine-Budde, Pengutronix
+ * Copyright (C) 2006 Andrey Volkov, Varma Electronics
+ * Copyright (C) 2008 Wolfgang Grandegger <wg@grandegger.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <net/rtnetlink.h>
+
+#include "sysfs.h"
+
+#define MOD_DESC "CAN device driver interface"
+
+MODULE_DESCRIPTION(MOD_DESC);
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Wolfgang Grandegger <wg@grandegger.com>");
+
+#ifdef CONFIG_CAN_CALC_BITTIMING
+#define CAN_CALC_MAX_ERROR 50 /* in one-tenth of a percent */
+
+/*
+ * Bit-timing calculation derived from:
+ *
+ * Code based on LinCAN sources and H8S2638 project
+ * Copyright 2004-2006 Pavel Pisa - DCE FELK CVUT cz
+ * Copyright 2005      Stanislav Marek
+ * email: pisa@cmp.felk.cvut.cz
+ */
+static int can_update_spt(const struct can_bittiming_const *btc,
+			  int sampl_pt, int tseg, int *tseg1, int *tseg2)
+{
+	*tseg2 = tseg + 1 - (sampl_pt * (tseg + 1)) / 1000;
+	if (*tseg2 < btc->tseg2_min)
+		*tseg2 = btc->tseg2_min;
+	if (*tseg2 > btc->tseg2_max)
+		*tseg2 = btc->tseg2_max;
+	*tseg1 = tseg - *tseg2;
+	if (*tseg1 > btc->tseg1_max) {
+		*tseg1 = btc->tseg1_max;
+		*tseg2 = tseg - *tseg1;
+	}
+	return 1000 * (tseg + 1 - *tseg2) / (tseg + 1);
+}
+
+static int can_calc_bittiming(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	struct can_bittiming *bt = &priv->bittiming;
+	const struct can_bittiming_const *btc = priv->bittiming_const;
+	long rate, best_rate = 0;
+	long best_error = 1000000000, error = 0;
+	int best_tseg = 0, best_brp = 0, brp = 0;
+	int tsegall, tseg = 0, tseg1 = 0, tseg2 = 0;
+	int spt_error = 1000, spt = 0, sampl_pt;
+	u64 v64;
+
+	if (!priv->bittiming_const)
+		return -ENOTSUPP;
+
+	/* Use CIA recommended sample points */
+	if (bt->sample_point) {
+		sampl_pt = bt->sample_point;
+	} else {
+		if (bt->bitrate > 800000)
+			sampl_pt = 750;
+		else if (bt->bitrate > 500000)
+			sampl_pt = 800;
+		else
+			sampl_pt = 875;
+	}
+
+	/* tseg even = round down, odd = round up */
+	for (tseg = (btc->tseg1_max + btc->tseg2_max) * 2 + 1;
+	     tseg >= (btc->tseg1_min + btc->tseg2_min) * 2; tseg--) {
+		tsegall = 1 + tseg / 2;
+		/* Compute all possible tseg choices (tseg=tseg1+tseg2) */
+		brp = bt->clock / (tsegall * bt->bitrate) + tseg % 2;
+		/* chose brp step which is possible in system */
+		brp = (brp / btc->brp_inc) * btc->brp_inc;
+		if ((brp < btc->brp_min) || (brp > btc->brp_max))
+			continue;
+		rate = bt->clock / (brp * tsegall);
+		error = bt->bitrate - rate;
+		/* tseg brp biterror */
+		if (error < 0)
+			error = -error;
+		if (error > best_error)
+			continue;
+		best_error = error;
+		if (error == 0) {
+			spt = can_update_spt(btc, sampl_pt, tseg / 2,
+					     &tseg1, &tseg2);
+			error = sampl_pt - spt;
+			if (error < 0)
+				error = -error;
+			if (error > spt_error)
+				continue;
+			spt_error = error;
+		}
+		best_tseg = tseg / 2;
+		best_brp = brp;
+		best_rate = rate;
+		if (error == 0)
+			break;
+	}
+
+	if (best_error) {
+		/* Error in one-tenth of a percent */
+		error = (best_error * 1000) / bt->bitrate;
+		if (error > CAN_CALC_MAX_ERROR) {
+			dev_err(ND2D(dev), "bitrate error %ld.%ld%% too high\n",
+				error / 10, error % 10);
+			return -EDOM;
+		} else {
+			dev_warn(ND2D(dev), "bitrate error %ld.%ld%%\n",
+				 error / 10, error % 10);
+		}
+	}
+
+	spt = can_update_spt(btc, sampl_pt, best_tseg, &tseg1, &tseg2);
+
+	v64 = (u64)best_brp * 1000000000UL;
+	do_div(v64, bt->clock);
+	bt->tq = (u32)v64;
+	bt->prop_seg = tseg1 / 2;
+	bt->phase_seg1 = tseg1 - bt->prop_seg;
+	bt->phase_seg2 = tseg2;
+	bt->sjw = 1;
+	bt->brp = best_brp;
+
+	return 0;
+}
+#else /* !CONFIG_CAN_CALC_BITTIMING */
+static int can_calc_bittiming(struct net_device *dev)
+{
+	dev_err(ND2D(dev), "bit-timing calculation not available\n");
+	return -EINVAL;
+}
+#endif /* CONFIG_CAN_CALC_BITTIMING */
+
+int can_sample_point(struct can_bittiming *bt)
+{
+	return ((bt->prop_seg + bt->phase_seg1 + 1) * 1000) /
+		(bt->prop_seg + bt->phase_seg1 + bt->phase_seg2 + 1);
+}
+
+static int can_fixup_bittiming(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	struct can_bittiming *bt = &priv->bittiming;
+	const struct can_bittiming_const *btc = priv->bittiming_const;
+	int tseg1, alltseg;
+	u32 bitrate;
+	u64 brp64;
+
+	if (!priv->bittiming_const)
+		return -ENOTSUPP;
+
+	tseg1 = bt->prop_seg + bt->phase_seg1;
+	if (bt->sjw > btc->sjw_max ||
+	    tseg1 < btc->tseg1_min || tseg1 > btc->tseg1_max ||
+	    bt->phase_seg2 < btc->tseg2_min || bt->phase_seg2 > btc->tseg2_max)
+		return -EINVAL;
+
+	brp64 = (u64)bt->clock * (u64)bt->tq;
+	if (btc->brp_inc > 1)
+		do_div(brp64, btc->brp_inc);
+	brp64 += 500000000UL - 1;
+	do_div(brp64, 1000000000UL); /* the practicable BRP */
+	if (btc->brp_inc > 1)
+		brp64 *= btc->brp_inc;
+	bt->brp = (u32)brp64;
+
+	if (bt->brp < btc->brp_min || bt->brp > btc->brp_max)
+		return -EINVAL;
+
+	alltseg = bt->prop_seg + bt->phase_seg1 + bt->phase_seg2 + 1;
+	bitrate = bt->clock / (bt->brp * alltseg);
+	bt->bitrate = bitrate;
+
+	return 0;
+}
+
+/*
+ * Set CAN bit-timing for the device
+ *
+ * This functions should be called in the open function of the device
+ * driver to determine, check and set appropriate bit-timing parameters.
+ */
+int can_set_bittiming(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	int err;
+
+	/* Check if bit-timing parameters have been pre-defined */
+	if (!priv->bittiming.tq && !priv->bittiming.bitrate) {
+		dev_err(ND2D(dev), "bit-timing not yet defined\n");
+		return -EINVAL;
+	}
+
+	/* Check if the CAN device has bit-timing parameters */
+	if (priv->bittiming_const) {
+
+		/* Check if bit-timing parameters have already been set */
+		if (priv->bittiming.tq && priv->bittiming.bitrate)
+			return 0;
+
+		/* Non-expert mode? Check if the bitrate has been pre-defined */
+		if (!priv->bittiming.tq)
+			/* Determine bit-timing parameters */
+			err = can_calc_bittiming(dev);
+		else
+			/* Check bit-timing params and calculate proper brp */
+			err = can_fixup_bittiming(dev);
+		if (err)
+			return err;
+	}
+
+	if (priv->do_set_bittiming) {
+		/* Finally, set the bit-timing registers */
+		err = priv->do_set_bittiming(dev);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(can_set_bittiming);
+
+static void can_setup(struct net_device *dev)
+{
+	dev->type = ARPHRD_CAN;
+	dev->mtu = sizeof(struct can_frame);
+	dev->hard_header_len = 0;
+	dev->addr_len = 0;
+	dev->tx_queue_len = 10;
+
+	/* New-style flags. */
+	dev->flags = IFF_NOARP;
+	dev->features = NETIF_F_NO_CSUM;
+}
+
+/*
+ * Allocate and setup space for the CAN network device
+ */
+struct net_device *alloc_candev(int sizeof_priv)
+{
+	struct net_device *dev;
+	struct can_priv *priv;
+
+	dev = alloc_netdev(sizeof_priv, "can%d", can_setup);
+	if (!dev)
+		return NULL;
+
+	priv = netdev_priv(dev);
+
+	priv->state = CAN_STATE_STOPPED;
+	spin_lock_init(&priv->irq_lock);
+
+	init_timer(&priv->timer);
+	priv->timer.expires = 0;
+
+	return dev;
+}
+EXPORT_SYMBOL_GPL(alloc_candev);
+
+/*
+ * Allocate space of the CAN network device
+ */
+void free_candev(struct net_device *dev)
+{
+	free_netdev(dev);
+}
+EXPORT_SYMBOL_GPL(free_candev);
+
+/*
+ * Register the CAN network device
+ */
+int register_candev(struct net_device *dev)
+{
+	int err;
+
+	err = register_netdev(dev);
+	if (err)
+		return err;
+
+	can_create_sysfs(dev);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(register_candev);
+
+/*
+ * Unregister the CAN network device
+ */
+void unregister_candev(struct net_device *dev)
+{
+	can_remove_sysfs(dev);
+	unregister_netdev(dev);
+}
+EXPORT_SYMBOL_GPL(unregister_candev);
+
+/*
+ * Local echo of CAN messages
+ *
+ * CAN network devices *should* support a local echo functionality
+ * (see Documentation/networking/can.txt). To test the handling of CAN
+ * interfaces that do not support the local echo both driver types are
+ * implemented. In the case that the driver does not support the echo
+ * the IFF_ECHO remains clear in dev->flags. This causes the PF_CAN core
+ * to perform the echo as a fallback solution.
+ */
+
+static void can_flush_echo_skb(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	int i;
+
+	for (i = 0; i < CAN_ECHO_SKB_MAX; i++) {
+		if (priv->echo_skb[i]) {
+			kfree_skb(priv->echo_skb[i]);
+			priv->echo_skb[i] = NULL;
+			stats->tx_dropped++;
+			stats->tx_aborted_errors++;
+		}
+	}
+}
+
+/*
+ * Put the skb on the stack to be looped backed locally lateron
+ *
+ * The function is typically called in the start_xmit function
+ * of the device driver.
+ */
+void can_put_echo_skb(struct sk_buff *skb, struct net_device *dev, int idx)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	/* set flag whether this packet has to be looped back */
+	if (!(dev->flags & IFF_ECHO) || skb->pkt_type != PACKET_LOOPBACK) {
+		kfree_skb(skb);
+		return;
+	}
+
+	if (!priv->echo_skb[idx]) {
+		struct sock *srcsk = skb->sk;
+
+		if (atomic_read(&skb->users) != 1) {
+			struct sk_buff *old_skb = skb;
+
+			skb = skb_clone(old_skb, GFP_ATOMIC);
+			kfree_skb(old_skb);
+			if (!skb)
+				return;
+		} else
+			skb_orphan(skb);
+
+		skb->sk = srcsk;
+
+		/* make settings for echo to reduce code in irq context */
+		skb->protocol = htons(ETH_P_CAN);
+		skb->pkt_type = PACKET_BROADCAST;
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+		skb->dev = dev;
+
+		/* save this skb for tx interrupt echo handling */
+		priv->echo_skb[idx] = skb;
+	} else {
+		/* locking problem with netif_stop_queue() ?? */
+		printk(KERN_ERR "%s: %s: BUG! echo_skb is occupied!\n",
+		       dev->name, __func__);
+		kfree_skb(skb);
+	}
+}
+EXPORT_SYMBOL_GPL(can_put_echo_skb);
+
+/*
+ * Get the skb from the stack and loop it back locally
+ *
+ * The function is typically called when the TX done interrupt
+ * is handled in the device driver.
+ */
+void can_get_echo_skb(struct net_device *dev, int idx)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	if ((dev->flags & IFF_ECHO) && priv->echo_skb[idx]) {
+		netif_rx(priv->echo_skb[idx]);
+		priv->echo_skb[idx] = NULL;
+	}
+}
+EXPORT_SYMBOL_GPL(can_get_echo_skb);
+
+/*
+ * CAN device restart for bus-off recovery
+ */
+int can_restart_now(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct sk_buff *skb;
+	struct can_frame *cf;
+	int err;
+
+	if (netif_carrier_ok(dev))
+		netif_carrier_off(dev);
+
+	/* Cancel restart in progress */
+	if (priv->timer.expires) {
+		del_timer(&priv->timer);
+		priv->timer.expires = 0; /* mark inactive timer */
+	}
+
+	can_flush_echo_skb(dev);
+
+	err = priv->do_set_mode(dev, CAN_MODE_START);
+	if (err)
+		return err;
+
+	netif_carrier_on(dev);
+
+	dev_dbg(ND2D(dev), "restarted\n");
+	priv->can_stats.restarts++;
+
+	/* send restart message upstream */
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (skb == NULL)
+		return -ENOMEM;
+	skb->dev = dev;
+	skb->protocol = htons(ETH_P_CAN);
+	cf = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
+	memset(cf, 0, sizeof(struct can_frame));
+	cf->can_id = CAN_ERR_FLAG | CAN_ERR_RESTARTED;
+	cf->can_dlc = CAN_ERR_DLC;
+
+	netif_rx(skb);
+
+	dev->last_rx = jiffies;
+	stats->rx_packets++;
+	stats->rx_bytes += cf->can_dlc;
+
+	return 0;
+}
+
+static void can_restart_after(unsigned long data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	struct can_priv *priv = netdev_priv(dev);
+
+	priv->timer.expires = 0; /* mark inactive timer */
+	can_restart_now(dev);
+}
+
+/*
+ * CAN bus-off
+ *
+ * This functions should be called when the device goes bus-off to
+ * tell the netif layer that no more packets can be sent or received.
+ * If enabled, a timer is started to trigger bus-off recovery.
+ */
+void can_bus_off(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	dev_dbg(ND2D(dev), "bus-off\n");
+
+	netif_carrier_off(dev);
+
+	if (priv->restart_ms > 0 && !priv->timer.expires) {
+
+		priv->timer.function = can_restart_after;
+		priv->timer.data = (unsigned long)dev;
+		priv->timer.expires =
+			jiffies + (priv->restart_ms * HZ) / 1000;
+		add_timer(&priv->timer);
+	}
+}
+EXPORT_SYMBOL_GPL(can_bus_off);
+
+/*
+ * Cleanup function before the device gets closed.
+ *
+ * This functions should be called in the close function of the device
+ * driver.
+ */
+void can_close_cleanup(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	if (priv->timer.expires) {
+		del_timer(&priv->timer);
+		priv->timer.expires = 0;
+	}
+
+	can_flush_echo_skb(dev);
+}
+EXPORT_SYMBOL_GPL(can_close_cleanup);
+
+static __init int can_dev_init(void)
+{
+	printk(KERN_INFO MOD_DESC "\n");
+
+	return 0;
+}
+module_init(can_dev_init);
+
+static __exit void can_dev_exit(void)
+{
+}
+module_exit(can_dev_exit);
diff --git a/drivers/net/can/flexcan/Makefile b/drivers/net/can/flexcan/Makefile
new file mode 100644
index 000000000000..b2dbb4fb2793
--- /dev/null
+++ b/drivers/net/can/flexcan/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_CAN_FLEXCAN)	+= flexcan.o
+
+flexcan-y := dev.o drv.o mbm.o
diff --git a/drivers/net/can/flexcan/dev.c b/drivers/net/can/flexcan/dev.c
new file mode 100644
index 000000000000..f2040c135a89
--- /dev/null
+++ b/drivers/net/can/flexcan/dev.c
@@ -0,0 +1,619 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file dev.c
+ *
+ * @brief Driver for Freescale CAN Controller FlexCAN.
+ *
+ * @ingroup can
+ */
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/unistd.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/netdevice.h>
+#include <linux/spinlock.h>
+#include <linux/device.h>
+
+#include <linux/module.h>
+#include "flexcan.h"
+
+enum {
+	FLEXCAN_ATTR_STATE = 0,
+	FLEXCAN_ATTR_BITRATE,
+	FLEXCAN_ATTR_BR_PRESDIV,
+	FLEXCAN_ATTR_BR_RJW,
+	FLEXCAN_ATTR_BR_PROPSEG,
+	FLEXCAN_ATTR_BR_PSEG1,
+	FLEXCAN_ATTR_BR_PSEG2,
+	FLEXCAN_ATTR_BR_CLKSRC,
+	FLEXCAN_ATTR_MAXMB,
+	FLEXCAN_ATTR_XMIT_MAXMB,
+	FLEXCAN_ATTR_FIFO,
+	FLEXCAN_ATTR_WAKEUP,
+	FLEXCAN_ATTR_SRX_DIS,
+	FLEXCAN_ATTR_WAK_SRC,
+	FLEXCAN_ATTR_BCC,
+	FLEXCAN_ATTR_LOCAL_PRIORITY,
+	FLEXCAN_ATTR_ABORT,
+	FLEXCAN_ATTR_LOOPBACK,
+	FLEXCAN_ATTR_SMP,
+	FLEXCAN_ATTR_BOFF_REC,
+	FLEXCAN_ATTR_TSYN,
+	FLEXCAN_ATTR_LISTEN,
+	FLEXCAN_ATTR_EXTEND_MSG,
+	FLEXCAN_ATTR_STANDARD_MSG,
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+	FLEXCAN_ATTR_DUMP_REG,
+	FLEXCAN_ATTR_DUMP_XMIT_MB,
+	FLEXCAN_ATTR_DUMP_RX_MB,
+#endif
+	FLEXCAN_ATTR_MAX
+};
+
+static ssize_t flexcan_show_attr(struct device *dev,
+				 struct device_attribute *attr, char *buf);
+static ssize_t flexcan_set_attr(struct device *dev,
+				struct device_attribute *attr, const char *buf,
+				size_t count);
+
+static struct device_attribute flexcan_dev_attr[FLEXCAN_ATTR_MAX] = {
+	[FLEXCAN_ATTR_STATE] = __ATTR(state, 0444, flexcan_show_attr, NULL),
+	[FLEXCAN_ATTR_BITRATE] =
+	    __ATTR(bitrate, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_BR_PRESDIV] =
+	    __ATTR(br_presdiv, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_BR_RJW] =
+	    __ATTR(br_rjw, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_BR_PROPSEG] =
+	    __ATTR(br_propseg, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_BR_PSEG1] =
+	    __ATTR(br_pseg1, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_BR_PSEG2] =
+	    __ATTR(br_pseg2, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_BR_CLKSRC] =
+	    __ATTR(br_clksrc, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_MAXMB] =
+	    __ATTR(maxmb, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_XMIT_MAXMB] =
+	    __ATTR(xmit_maxmb, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_FIFO] =
+	    __ATTR(fifo, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_WAKEUP] =
+	    __ATTR(wakeup, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_SRX_DIS] =
+	    __ATTR(srx_dis, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_WAK_SRC] =
+	    __ATTR(wak_src, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_BCC] =
+	    __ATTR(bcc, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_LOCAL_PRIORITY] =
+	    __ATTR(local_priority, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_ABORT] =
+	    __ATTR(abort, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_LOOPBACK] =
+	    __ATTR(loopback, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_SMP] =
+	    __ATTR(smp, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_BOFF_REC] =
+	    __ATTR(boff_rec, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_TSYN] =
+	    __ATTR(tsyn, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_LISTEN] =
+	    __ATTR(listen, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_EXTEND_MSG] =
+	    __ATTR(ext_msg, 0644, flexcan_show_attr, flexcan_set_attr),
+	[FLEXCAN_ATTR_STANDARD_MSG] =
+	    __ATTR(std_msg, 0644, flexcan_show_attr, flexcan_set_attr),
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+	[FLEXCAN_ATTR_DUMP_REG] =
+	    __ATTR(dump_reg, 0444, flexcan_show_attr, NULL),
+	[FLEXCAN_ATTR_DUMP_XMIT_MB] =
+	    __ATTR(dump_xmit_mb, 0444, flexcan_show_attr, NULL),
+	[FLEXCAN_ATTR_DUMP_RX_MB] =
+	    __ATTR(dump_rx_mb, 0444, flexcan_show_attr, NULL),
+#endif
+};
+
+static void flexcan_set_bitrate(struct flexcan_device *flexcan, int bitrate)
+{
+	/* TODO:: implement in future
+	 * based on the bitrate to get the timing of
+	 * presdiv, pseg1, pseg2, propseg
+	 */
+}
+
+static void flexcan_update_bitrate(struct flexcan_device *flexcan)
+{
+	int rate, div;
+
+	if (flexcan->br_clksrc)
+		rate = clk_get_rate(flexcan->clk);
+	else {
+		struct clk *clk;
+		clk = clk_get(NULL, "ckih");
+		if (!clk)
+			return;
+		rate = clk_get_rate(clk);
+		clk_put(clk);
+	}
+	if (!rate)
+		return;
+
+	div = (flexcan->br_presdiv + 1);
+	div *=
+	    (flexcan->br_propseg + flexcan->br_pseg1 + flexcan->br_pseg2 + 4);
+	flexcan->bitrate = (rate + div - 1) / div;
+}
+
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+static int flexcan_dump_reg(struct flexcan_device *flexcan, char *buf)
+{
+	int ret = 0;
+	unsigned int reg;
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR);
+	ret += sprintf(buf + ret, "MCR::0x%x\n", reg);
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_CTRL);
+	ret += sprintf(buf + ret, "CTRL::0x%x\n", reg);
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_RXGMASK);
+	ret += sprintf(buf + ret, "RXGMASK::0x%x\n", reg);
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_RX14MASK);
+	ret += sprintf(buf + ret, "RX14MASK::0x%x\n", reg);
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_RX15MASK);
+	ret += sprintf(buf + ret, "RX15MASK::0x%x\n", reg);
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_ECR);
+	ret += sprintf(buf + ret, "ECR::0x%x\n", reg);
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_ESR);
+	ret += sprintf(buf + ret, "ESR::0x%x\n", reg);
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_IMASK2);
+	ret += sprintf(buf + ret, "IMASK2::0x%x\n", reg);
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_IMASK1);
+	ret += sprintf(buf + ret, "IMASK1::0x%x\n", reg);
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_IFLAG2);
+	ret += sprintf(buf + ret, "IFLAG2::0x%x\n", reg);
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_IFLAG1);
+	ret += sprintf(buf + ret, "IFLAG1::0x%x\n", reg);
+	return ret;
+}
+
+static int flexcan_dump_xmit_mb(struct flexcan_device *flexcan, char *buf)
+{
+	int ret = 0, i;
+	i = flexcan->xmit_maxmb + 1;
+	for (; i <= flexcan->maxmb; i++)
+		ret +=
+		    sprintf(buf + ret,
+			    "mb[%d]::CS:0x%x ID:0x%x DATA[1~2]:0x%02x,0x%02x\n",
+			    i, flexcan->hwmb[i].mb_cs.data,
+			    flexcan->hwmb[i].mb_id, flexcan->hwmb[i].mb_data[1],
+			    flexcan->hwmb[i].mb_data[2]);
+	return ret;
+}
+
+static int flexcan_dump_rx_mb(struct flexcan_device *flexcan, char *buf)
+{
+	int ret = 0, i;
+	for (i = 0; i <= flexcan->xmit_maxmb; i++)
+		ret +=
+		    sprintf(buf + ret,
+			    "mb[%d]::CS:0x%x ID:0x%x DATA[1~2]:0x%02x,0x%02x\n",
+			    i, flexcan->hwmb[i].mb_cs.data,
+			    flexcan->hwmb[i].mb_id, flexcan->hwmb[i].mb_data[1],
+			    flexcan->hwmb[i].mb_data[2]);
+	return ret;
+}
+#endif
+
+static ssize_t flexcan_show_state(struct net_device *net, char *buf)
+{
+	int ret, esr;
+	struct flexcan_device *flexcan = netdev_priv(net);
+	ret = sprintf(buf, "%s::", netif_running(net) ? "Start" : "Stop");
+	if (netif_carrier_ok(net)) {
+		esr = __raw_readl(flexcan->io_base + CAN_HW_REG_ESR);
+		switch ((esr & __ESR_FLT_CONF_MASK) >> __ESR_FLT_CONF_OFF) {
+		case 0:
+			ret += sprintf(buf + ret, "normal\n");
+			break;
+		case 1:
+			ret += sprintf(buf + ret, "error passive\n");
+			break;
+		default:
+			ret += sprintf(buf + ret, "bus off\n");
+		}
+	} else
+		ret += sprintf(buf + ret, "bus off\n");
+	return ret;
+}
+
+static ssize_t flexcan_show_attr(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	int attr_id;
+	struct net_device *net;
+	struct flexcan_device *flexcan;
+
+	net = dev_get_drvdata(dev);
+	BUG_ON(!net);
+	flexcan = netdev_priv(net);
+	BUG_ON(!flexcan);
+
+	attr_id = attr - flexcan_dev_attr;
+	switch (attr_id) {
+	case FLEXCAN_ATTR_STATE:
+		return flexcan_show_state(net, buf);
+	case FLEXCAN_ATTR_BITRATE:
+		return sprintf(buf, "%d\n", flexcan->bitrate);
+	case FLEXCAN_ATTR_BR_PRESDIV:
+		return sprintf(buf, "%d\n", flexcan->br_presdiv + 1);
+	case FLEXCAN_ATTR_BR_RJW:
+		return sprintf(buf, "%d\n", flexcan->br_rjw);
+	case FLEXCAN_ATTR_BR_PROPSEG:
+		return sprintf(buf, "%d\n", flexcan->br_propseg + 1);
+	case FLEXCAN_ATTR_BR_PSEG1:
+		return sprintf(buf, "%d\n", flexcan->br_pseg1 + 1);
+	case FLEXCAN_ATTR_BR_PSEG2:
+		return sprintf(buf, "%d\n", flexcan->br_pseg2 + 1);
+	case FLEXCAN_ATTR_BR_CLKSRC:
+		return sprintf(buf, "%s\n", flexcan->br_clksrc ? "bus" : "osc");
+	case FLEXCAN_ATTR_MAXMB:
+		return sprintf(buf, "%d\n", flexcan->maxmb + 1);
+	case FLEXCAN_ATTR_XMIT_MAXMB:
+		return sprintf(buf, "%d\n", flexcan->xmit_maxmb + 1);
+	case FLEXCAN_ATTR_FIFO:
+		return sprintf(buf, "%d\n", flexcan->fifo);
+	case FLEXCAN_ATTR_WAKEUP:
+		return sprintf(buf, "%d\n", flexcan->wakeup);
+	case FLEXCAN_ATTR_SRX_DIS:
+		return sprintf(buf, "%d\n", flexcan->srx_dis);
+	case FLEXCAN_ATTR_WAK_SRC:
+		return sprintf(buf, "%d\n", flexcan->wak_src);
+	case FLEXCAN_ATTR_BCC:
+		return sprintf(buf, "%d\n", flexcan->bcc);
+	case FLEXCAN_ATTR_LOCAL_PRIORITY:
+		return sprintf(buf, "%d\n", flexcan->lprio);
+	case FLEXCAN_ATTR_ABORT:
+		return sprintf(buf, "%d\n", flexcan->abort);
+	case FLEXCAN_ATTR_LOOPBACK:
+		return sprintf(buf, "%d\n", flexcan->loopback);
+	case FLEXCAN_ATTR_SMP:
+		return sprintf(buf, "%d\n", flexcan->smp);
+	case FLEXCAN_ATTR_BOFF_REC:
+		return sprintf(buf, "%d\n", flexcan->boff_rec);
+	case FLEXCAN_ATTR_TSYN:
+		return sprintf(buf, "%d\n", flexcan->tsyn);
+	case FLEXCAN_ATTR_LISTEN:
+		return sprintf(buf, "%d\n", flexcan->listen);
+	case FLEXCAN_ATTR_EXTEND_MSG:
+		return sprintf(buf, "%d\n", flexcan->ext_msg);
+	case FLEXCAN_ATTR_STANDARD_MSG:
+		return sprintf(buf, "%d\n", flexcan->std_msg);
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+	case FLEXCAN_ATTR_DUMP_REG:
+		return flexcan_dump_reg(flexcan, buf);
+	case FLEXCAN_ATTR_DUMP_XMIT_MB:
+		return flexcan_dump_xmit_mb(flexcan, buf);
+	case FLEXCAN_ATTR_DUMP_RX_MB:
+		return flexcan_dump_rx_mb(flexcan, buf);
+#endif
+	default:
+		return sprintf(buf, "%s:%p->%p\n", __func__, flexcan_dev_attr,
+			       attr);
+	}
+}
+
+static ssize_t flexcan_set_attr(struct device *dev,
+				struct device_attribute *attr, const char *buf,
+				size_t count)
+{
+	int attr_id, tmp;
+	struct net_device *net;
+	struct flexcan_device *flexcan;
+
+	net = dev_get_drvdata(dev);
+	BUG_ON(!net);
+	flexcan = netdev_priv(net);
+	BUG_ON(!flexcan);
+
+	attr_id = attr - flexcan_dev_attr;
+
+	if (mutex_lock_interruptible(&flexcan->mutex))
+		return count;
+
+	if (netif_running(net))
+		goto set_finish;
+
+	if (attr_id == FLEXCAN_ATTR_BR_CLKSRC) {
+		if (!strcasecmp(buf, "bus"))
+			flexcan->br_clksrc = 1;
+		else if (!strcasecmp(buf, "osc"))
+			flexcan->br_clksrc = 0;
+		goto set_finish;
+	}
+
+	tmp = simple_strtoul(buf, NULL, 0);
+	switch (attr_id) {
+	case FLEXCAN_ATTR_BITRATE:
+		flexcan_set_bitrate(flexcan, tmp);
+		break;
+	case FLEXCAN_ATTR_BR_PRESDIV:
+		if ((tmp > 0) && (tmp <= FLEXCAN_MAX_PRESDIV)) {
+			flexcan->br_presdiv = tmp - 1;
+			flexcan_update_bitrate(flexcan);
+		}
+		break;
+	case FLEXCAN_ATTR_BR_RJW:
+		if ((tmp > 0) && (tmp <= FLEXCAN_MAX_RJW))
+			flexcan->br_rjw = tmp - 1;
+		break;
+	case FLEXCAN_ATTR_BR_PROPSEG:
+		if ((tmp > 0) && (tmp <= FLEXCAN_MAX_PROPSEG)) {
+			flexcan->br_propseg = tmp - 1;
+			flexcan_update_bitrate(flexcan);
+		}
+		break;
+	case FLEXCAN_ATTR_BR_PSEG1:
+		if ((tmp > 0) && (tmp <= FLEXCAN_MAX_PSEG1)) {
+			flexcan->br_pseg1 = tmp - 1;
+			flexcan_update_bitrate(flexcan);
+		}
+		break;
+	case FLEXCAN_ATTR_BR_PSEG2:
+		if ((tmp > 0) && (tmp <= FLEXCAN_MAX_PSEG2)) {
+			flexcan->br_pseg2 = tmp - 1;
+			flexcan_update_bitrate(flexcan);
+		}
+		break;
+	case FLEXCAN_ATTR_MAXMB:
+		if ((tmp > 0) && (tmp <= FLEXCAN_MAX_MB)) {
+			if (flexcan->maxmb != (tmp - 1)) {
+				flexcan->maxmb = tmp - 1;
+				if (flexcan->xmit_maxmb < flexcan->maxmb)
+					flexcan->xmit_maxmb = flexcan->maxmb;
+			}
+		}
+		break;
+	case FLEXCAN_ATTR_XMIT_MAXMB:
+		if ((tmp > 0) && (tmp <= (flexcan->maxmb + 1))) {
+			if (flexcan->xmit_maxmb != (tmp - 1))
+				flexcan->xmit_maxmb = tmp - 1;
+		}
+		break;
+	case FLEXCAN_ATTR_FIFO:
+		flexcan->fifo = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_WAKEUP:
+		flexcan->wakeup = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_SRX_DIS:
+		flexcan->srx_dis = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_WAK_SRC:
+		flexcan->wak_src = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_BCC:
+		flexcan->bcc = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_LOCAL_PRIORITY:
+		flexcan->lprio = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_ABORT:
+		flexcan->abort = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_LOOPBACK:
+		flexcan->loopback = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_SMP:
+		flexcan->smp = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_BOFF_REC:
+		flexcan->boff_rec = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_TSYN:
+		flexcan->tsyn = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_LISTEN:
+		flexcan->listen = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_EXTEND_MSG:
+		flexcan->ext_msg = tmp ? 1 : 0;
+		break;
+	case FLEXCAN_ATTR_STANDARD_MSG:
+		flexcan->std_msg = tmp ? 1 : 0;
+		break;
+	}
+      set_finish:
+	mutex_unlock(&flexcan->mutex);
+	return count;
+}
+
+static void flexcan_device_default(struct flexcan_device *dev)
+{
+	dev->br_clksrc = 1;
+	dev->br_rjw = 2;
+	dev->br_presdiv = 6;
+	dev->br_propseg = 4;
+	dev->br_pseg1 = 4;
+	dev->br_pseg2 = 7;
+
+	dev->bcc = 1;
+	dev->srx_dis = 1;
+	dev->smp = 1;
+	dev->boff_rec = 1;
+
+	dev->maxmb = FLEXCAN_MAX_MB - 1;
+	dev->xmit_maxmb = (FLEXCAN_MAX_MB >> 1) - 1;
+	dev->xmit_mb = dev->maxmb - dev->xmit_maxmb;
+
+	dev->ext_msg = 1;
+	dev->std_msg = 1;
+}
+
+static int flexcan_device_attach(struct flexcan_device *flexcan)
+{
+	int ret;
+	struct resource *res;
+	struct platform_device *pdev = flexcan->dev;
+	struct flexcan_platform_data *plat_data = (pdev->dev).platform_data;
+
+	res = platform_get_resource(flexcan->dev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	flexcan->io_base = ioremap(res->start, res->end - res->start + 1);
+	if (!flexcan->io_base)
+		return -ENOMEM;
+
+	flexcan->irq = platform_get_irq(flexcan->dev, 0);
+	if (!flexcan->irq) {
+		ret = -ENODEV;
+		goto no_irq_err;
+	}
+
+	ret = -EINVAL;
+	if (plat_data) {
+		if (plat_data->core_reg) {
+			flexcan->core_reg = regulator_get(&pdev->dev,
+							  plat_data->core_reg);
+			if (!flexcan->core_reg)
+				goto plat_err;
+		}
+
+		if (plat_data->io_reg) {
+			flexcan->io_reg = regulator_get(&pdev->dev,
+							plat_data->io_reg);
+			if (!flexcan->io_reg)
+				goto plat_err;
+		}
+	}
+	flexcan->clk = clk_get(&(flexcan->dev)->dev, "can_clk");
+	flexcan->hwmb = (struct can_hw_mb *)(flexcan->io_base + CAN_MB_BASE);
+	flexcan->rx_mask = (unsigned int *)(flexcan->io_base + CAN_RXMASK_BASE);
+
+	return 0;
+      plat_err:
+	if (flexcan->core_reg) {
+		regulator_put(flexcan->core_reg);
+		flexcan->core_reg = NULL;
+	}
+      no_irq_err:
+	if (flexcan->io_base)
+		iounmap(flexcan->io_base);
+	return ret;
+}
+
+static void flexcan_device_detach(struct flexcan_device *flexcan)
+{
+	struct platform_device *pdev = flexcan->dev;
+	if (flexcan->clk) {
+		clk_put(flexcan->clk);
+		flexcan->clk = NULL;
+	}
+
+	if (flexcan->io_reg) {
+		regulator_put(flexcan->io_reg);
+		flexcan->io_reg = NULL;
+	}
+
+	if (flexcan->core_reg) {
+		regulator_put(flexcan->core_reg);
+		flexcan->core_reg = NULL;
+	}
+
+	if (flexcan->io_base)
+		iounmap(flexcan->io_base);
+}
+
+/*!
+ * @brief The function allocates can device.
+ *
+ * @param pdev	the pointer of platform device.
+ * @param setup	the initial function pointer of network device.
+ *
+ * @return none
+ */
+struct net_device *flexcan_device_alloc(struct platform_device *pdev,
+					void (*setup) (struct net_device *dev))
+{
+	struct flexcan_device *flexcan;
+	struct net_device *net;
+	int i, num;
+
+	net = alloc_netdev(sizeof(*flexcan), "can%d", setup);
+	if (net == NULL) {
+		printk(KERN_ERR "Allocate netdevice for FlexCAN fail!\n");
+		return NULL;
+	}
+	flexcan = netdev_priv(net);
+	memset(flexcan, 0, sizeof(*flexcan));
+
+	mutex_init(&flexcan->mutex);
+	init_timer(&flexcan->timer);
+
+	flexcan->dev = pdev;
+	if (flexcan_device_attach(flexcan)) {
+		printk(KERN_ERR "Attach FlexCAN fail!\n");
+		free_netdev(net);
+		return NULL;
+	}
+	flexcan_device_default(flexcan);
+	flexcan_update_bitrate(flexcan);
+
+	num = ARRAY_SIZE(flexcan_dev_attr);
+
+	for (i = 0; i < num; i++) {
+		if (device_create_file(&pdev->dev, flexcan_dev_attr + i)) {
+			printk(KERN_ERR "Create attribute file fail!\n");
+			break;
+		}
+	}
+
+	if (i != num) {
+		for (; i >= 0; i--)
+			device_remove_file(&pdev->dev, flexcan_dev_attr + i);
+		free_netdev(net);
+		return NULL;
+	}
+	dev_set_drvdata(&pdev->dev, net);
+	return net;
+}
+
+/*!
+ * @brief The function frees can device.
+ *
+ * @param pdev	the pointer of platform device.
+ *
+ * @return none
+ */
+void flexcan_device_free(struct platform_device *pdev)
+{
+	struct net_device *net;
+	struct flexcan_device *flexcan;
+	int i, num;
+	net = (struct net_device *)dev_get_drvdata(&pdev->dev);
+
+	unregister_netdev(net);
+	flexcan = netdev_priv(net);
+	del_timer(&flexcan->timer);
+
+	num = ARRAY_SIZE(flexcan_dev_attr);
+
+	for (i = 0; i < num; i++)
+		device_remove_file(&pdev->dev, flexcan_dev_attr + i);
+
+	flexcan_device_detach(netdev_priv(net));
+	free_netdev(net);
+}
diff --git a/drivers/net/can/flexcan/drv.c b/drivers/net/can/flexcan/drv.c
new file mode 100644
index 000000000000..1eaeac50d6ee
--- /dev/null
+++ b/drivers/net/can/flexcan/drv.c
@@ -0,0 +1,624 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drv.c
+ *
+ * @brief Driver for Freescale CAN Controller FlexCAN.
+ *
+ * @ingroup can
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/clk.h>
+
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <mach/hardware.h>
+#include "flexcan.h"
+
+static void flexcan_hw_start(struct flexcan_device *flexcan)
+{
+	unsigned int reg;
+	if ((flexcan->maxmb + 1) > 32) {
+		__raw_writel(0xFFFFFFFF, flexcan->io_base + CAN_HW_REG_IMASK1);
+		reg = (1 << (flexcan->maxmb - 31)) - 1;
+		__raw_writel(reg, flexcan->io_base + CAN_HW_REG_IMASK2);
+	} else {
+		reg = (1 << (flexcan->maxmb + 1)) - 1;
+		__raw_writel(reg, flexcan->io_base + CAN_HW_REG_IMASK1);
+		__raw_writel(0, flexcan->io_base + CAN_HW_REG_IMASK2);
+	}
+
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR) & (~__MCR_HALT);
+	__raw_writel(reg, flexcan->io_base + CAN_HW_REG_MCR);
+}
+
+static void flexcan_hw_stop(struct flexcan_device *flexcan)
+{
+	unsigned int reg;
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR);
+	__raw_writel(reg | __MCR_HALT, flexcan->io_base + CAN_HW_REG_MCR);
+}
+
+static int flexcan_hw_reset(struct flexcan_device *flexcan)
+{
+	unsigned int reg;
+	int timeout = 100000;
+
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR);
+	__raw_writel(reg | __MCR_MDIS, flexcan->io_base + CAN_HW_REG_MCR);
+
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_CTRL);
+	if (flexcan->br_clksrc)
+		reg |= __CTRL_CLK_SRC;
+	else
+		reg &= ~__CTRL_CLK_SRC;
+	__raw_writel(reg, flexcan->io_base + CAN_HW_REG_CTRL);
+
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR) & (~__MCR_MDIS);
+	__raw_writel(reg, flexcan->io_base + CAN_HW_REG_MCR);
+	reg |= __MCR_SOFT_RST;
+	__raw_writel(reg, flexcan->io_base + CAN_HW_REG_MCR);
+
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR);
+	while (reg & __MCR_SOFT_RST) {
+		if (--timeout <= 0) {
+			printk(KERN_ERR "Flexcan software Reset Timeouted\n");
+			return -1;
+		}
+		udelay(10);
+		reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR);
+	}
+	return 0;
+}
+
+static inline void flexcan_mcr_setup(struct flexcan_device *flexcan)
+{
+	unsigned int reg;
+
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR);
+	reg &= ~(__MCR_MAX_MB_MASK | __MCR_WAK_MSK | __MCR_MAX_IDAM_MASK);
+
+	if (flexcan->fifo)
+		reg |= __MCR_FEN;
+	else
+		reg &= ~__MCR_FEN;
+
+	if (flexcan->wakeup)
+		reg |= __MCR_SLF_WAK | __MCR_WAK_MSK;
+	else
+		reg &= ~(__MCR_SLF_WAK | __MCR_WAK_MSK);
+
+	if (flexcan->wak_src)
+		reg |= __MCR_WAK_SRC;
+	else
+		reg &= ~__MCR_WAK_SRC;
+
+	if (flexcan->srx_dis)
+		reg |= __MCR_SRX_DIS;
+	else
+		reg &= ~__MCR_SRX_DIS;
+
+	if (flexcan->bcc)
+		reg |= __MCR_BCC;
+	else
+		reg &= ~__MCR_BCC;
+
+	if (flexcan->lprio)
+		reg |= __MCR_LPRIO_EN;
+	else
+		reg &= ~__MCR_LPRIO_EN;
+
+	if (flexcan->abort)
+		reg |= __MCR_AEN;
+	else
+		reg &= ~__MCR_AEN;
+
+	reg |= (flexcan->maxmb << __MCR_MAX_MB_OFFSET);
+	reg |= __MCR_DOZE | __MCR_MAX_IDAM_C;
+	__raw_writel(reg, flexcan->io_base + CAN_HW_REG_MCR);
+}
+
+static inline void flexcan_ctrl_setup(struct flexcan_device *flexcan)
+{
+	unsigned int reg;
+
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_CTRL);
+	reg &= ~(__CTRL_PRESDIV_MASK | __CTRL_RJW_MASK | __CTRL_PSEG1_MASK |
+		 __CTRL_PSEG2_MASK | __CTRL_PROPSEG_MASK);
+
+	if (flexcan->loopback)
+		reg |= __CTRL_LPB;
+	else
+		reg &= ~__CTRL_LPB;
+
+	if (flexcan->smp)
+		reg |= __CTRL_SMP;
+	else
+		reg &= ~__CTRL_SMP;
+
+	if (flexcan->boff_rec)
+		reg |= __CTRL_BOFF_REC;
+	else
+		reg &= ~__CTRL_BOFF_REC;
+
+	if (flexcan->tsyn)
+		reg |= __CTRL_TSYN;
+	else
+		reg &= ~__CTRL_TSYN;
+
+	if (flexcan->listen)
+		reg |= __CTRL_LOM;
+	else
+		reg &= ~__CTRL_LOM;
+
+	reg |= (flexcan->br_presdiv << __CTRL_PRESDIV_OFFSET) |
+	    (flexcan->br_rjw << __CTRL_RJW_OFFSET) |
+	    (flexcan->br_pseg1 << __CTRL_PSEG1_OFFSET) |
+	    (flexcan->br_pseg2 << __CTRL_PSEG2_OFFSET) |
+	    (flexcan->br_propseg << __CTRL_PROPSEG_OFFSET);
+
+	reg &= ~__CTRL_LBUF;
+
+	reg |= __CTRL_TWRN_MSK | __CTRL_RWRN_MSK | __CTRL_BOFF_MSK |
+	    __CTRL_ERR_MSK;
+
+	__raw_writel(reg, flexcan->io_base + CAN_HW_REG_CTRL);
+}
+
+static int flexcan_hw_restart(struct net_device *dev)
+{
+	unsigned int reg;
+	struct flexcan_device *flexcan = netdev_priv(dev);
+
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR);
+	if (reg & __MCR_SOFT_RST)
+		return 1;
+
+	flexcan_mcr_setup(flexcan);
+
+	__raw_writel(0, flexcan->io_base + CAN_HW_REG_IMASK2);
+	__raw_writel(0, flexcan->io_base + CAN_HW_REG_IMASK1);
+
+	__raw_writel(0xFFFFFFFF, flexcan->io_base + CAN_HW_REG_IFLAG2);
+	__raw_writel(0xFFFFFFFF, flexcan->io_base + CAN_HW_REG_IFLAG1);
+
+	__raw_writel(0, flexcan->io_base + CAN_HW_REG_ECR);
+
+	flexcan_mbm_init(flexcan);
+	netif_carrier_on(dev);
+	flexcan_hw_start(flexcan);
+
+	if (netif_queue_stopped(dev))
+		netif_start_queue(dev);
+
+	return 0;
+}
+
+static void flexcan_hw_watch(unsigned long data)
+{
+	unsigned int reg, ecr;
+	struct net_device *dev = (struct net_device *)data;
+	struct flexcan_device *flexcan = dev ? netdev_priv(dev) : NULL;
+
+	BUG_ON(!flexcan);
+
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR);
+	if (reg & __MCR_MDIS) {
+		if (flexcan_hw_restart(dev))
+			mod_timer(&flexcan->timer, HZ / 20);
+		return;
+	}
+	ecr = __raw_readl(flexcan->io_base + CAN_HW_REG_ECR);
+	if (flexcan->boff_rec) {
+		if (((reg & __ESR_FLT_CONF_MASK) >> __ESR_FLT_CONF_OFF) > 1) {
+			reg |= __MCR_SOFT_RST;
+			__raw_writel(reg, flexcan->io_base + CAN_HW_REG_MCR);
+			mod_timer(&flexcan->timer, HZ / 20);
+			return;
+		}
+		netif_carrier_on(dev);
+	}
+}
+
+static void flexcan_hw_busoff(struct net_device *dev)
+{
+	struct flexcan_device *flexcan = netdev_priv(dev);
+	unsigned int reg;
+
+	netif_carrier_off(dev);
+
+	flexcan->timer.function = flexcan_hw_watch;
+	flexcan->timer.data = (unsigned long)dev;
+
+	if (flexcan->boff_rec) {
+		mod_timer(&flexcan->timer, HZ / 10);
+		return;
+	}
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_MCR);
+	__raw_writel(reg | __MCR_SOFT_RST, flexcan->io_base + CAN_HW_REG_MCR);
+	mod_timer(&flexcan->timer, HZ / 20);
+}
+
+static int flexcan_hw_open(struct flexcan_device *flexcan)
+{
+	if (flexcan_hw_reset(flexcan))
+		return -EFAULT;
+
+	flexcan_mcr_setup(flexcan);
+	flexcan_ctrl_setup(flexcan);
+
+	__raw_writel(0, flexcan->io_base + CAN_HW_REG_IMASK2);
+	__raw_writel(0, flexcan->io_base + CAN_HW_REG_IMASK1);
+
+	__raw_writel(0xFFFFFFFF, flexcan->io_base + CAN_HW_REG_IFLAG2);
+	__raw_writel(0xFFFFFFFF, flexcan->io_base + CAN_HW_REG_IFLAG1);
+
+	__raw_writel(0, flexcan->io_base + CAN_HW_REG_ECR);
+	return 0;
+}
+
+static void flexcan_err_handler(struct net_device *dev)
+{
+	struct flexcan_device *flexcan = netdev_priv(dev);
+	struct sk_buff *skb;
+	struct can_frame *frame;
+	unsigned int esr, ecr;
+
+	esr = __raw_readl(flexcan->io_base + CAN_HW_REG_ESR);
+	__raw_writel(esr & __ESR_INTERRUPTS, flexcan->io_base + CAN_HW_REG_ESR);
+
+	if (esr & __ESR_WAK_INT)
+		return;
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (!skb) {
+		printk(KERN_ERR "%s: allocates skb fail in\n", __func__);
+		return;
+	}
+	frame = (struct can_frame *)skb_put(skb, sizeof(*frame));
+	memset(frame, 0, sizeof(*frame));
+	frame->can_id = CAN_ERR_FLAG | CAN_ERR_CRTL;
+	frame->can_dlc = CAN_ERR_DLC;
+
+	if (esr & __ESR_TWRN_INT)
+		frame->data[1] |= CAN_ERR_CRTL_TX_WARNING;
+
+	if (esr & __ESR_RWRN_INT)
+		frame->data[1] |= CAN_ERR_CRTL_RX_WARNING;
+
+	if (esr & __ESR_BOFF_INT)
+		frame->can_id |= CAN_ERR_BUSOFF;
+
+	if (esr & __ESR_ERR_INT) {
+		if (esr & __ESR_BIT1_ERR)
+			frame->data[2] |= CAN_ERR_PROT_BIT1;
+
+		if (esr & __ESR_BIT0_ERR)
+			frame->data[2] |= CAN_ERR_PROT_BIT0;
+
+		if (esr & __ESR_ACK_ERR)
+			frame->can_id |= CAN_ERR_ACK;
+
+		/*TODO:// if (esr & __ESR_CRC_ERR) */
+
+		if (esr & __ESR_FRM_ERR)
+			frame->data[2] |= CAN_ERR_PROT_FORM;
+
+		if (esr & __ESR_STF_ERR)
+			frame->data[2] |= CAN_ERR_PROT_STUFF;
+
+		ecr = __raw_readl(flexcan->io_base + CAN_HW_REG_ECR);
+		switch ((esr & __ESR_FLT_CONF_MASK) >> __ESR_FLT_CONF_OFF) {
+		case 0:
+			if (__ECR_TX_ERR_COUNTER(ecr) >= __ECR_ACTIVE_THRESHOLD)
+				frame->data[1] |= CAN_ERR_CRTL_TX_WARNING;
+			if (__ECR_RX_ERR_COUNTER(ecr) >= __ECR_ACTIVE_THRESHOLD)
+				frame->data[1] |= CAN_ERR_CRTL_RX_WARNING;
+			break;
+		case 1:
+			if (__ECR_TX_ERR_COUNTER(ecr) >=
+			    __ECR_PASSIVE_THRESHOLD)
+				frame->data[1] |= CAN_ERR_CRTL_TX_PASSIVE;
+
+			if (__ECR_RX_ERR_COUNTER(ecr) >=
+			    __ECR_PASSIVE_THRESHOLD)
+				frame->data[1] |= CAN_ERR_CRTL_RX_PASSIVE;
+			break;
+		default:
+			frame->can_id |= CAN_ERR_BUSOFF;
+		}
+	}
+
+	if (frame->can_id & CAN_ERR_BUSOFF)
+		flexcan_hw_busoff(dev);
+
+	skb->dev = dev;
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	netif_receive_skb(skb);
+}
+
+static irqreturn_t flexcan_irq_handler(int irq, void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	struct flexcan_device *flexcan = dev ? netdev_priv(dev) : NULL;
+	unsigned int reg;
+
+	BUG_ON(!flexcan);
+
+	reg = __raw_readl(flexcan->io_base + CAN_HW_REG_ESR);
+	if (reg & __ESR_INTERRUPTS) {
+		flexcan_err_handler(dev);
+		return IRQ_HANDLED;
+	}
+
+	flexcan_mbm_isr(dev);
+	return IRQ_HANDLED;
+}
+
+static int flexcan_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct can_frame *frame = (struct can_frame *)skb->data;
+	struct flexcan_device *flexcan = netdev_priv(dev);
+	struct net_device_stats *stats = dev->get_stats(dev);
+
+	BUG_ON(!flexcan);
+
+	if (frame->can_dlc > 8)
+		return -EINVAL;
+
+	if (!flexcan_mbm_xmit(flexcan, frame)) {
+		dev_kfree_skb(skb);
+		stats->tx_bytes += frame->can_dlc;
+		stats->tx_packets++;
+		dev->trans_start = jiffies;
+		return NETDEV_TX_OK;
+	}
+	netif_stop_queue(dev);
+	return NETDEV_TX_BUSY;
+}
+
+static int flexcan_open(struct net_device *dev)
+{
+	struct flexcan_device *flexcan;
+	struct platform_device *pdev;
+	struct flexcan_platform_data *plat_data;
+
+	flexcan = netdev_priv(dev);
+	BUG_ON(!flexcan);
+
+	pdev = flexcan->dev;
+	plat_data = (pdev->dev).platform_data;
+	if (plat_data && plat_data->active)
+		plat_data->active(pdev->id);
+
+	if (flexcan->clk)
+		if (clk_enable(flexcan->clk))
+			goto clk_err;
+
+	if (flexcan->core_reg)
+		if (regulator_enable(flexcan->core_reg))
+			goto core_reg_err;
+
+	if (flexcan->io_reg)
+		if (regulator_enable(flexcan->io_reg))
+			goto io_reg_err;
+
+	if (plat_data && plat_data->xcvr_enable)
+		plat_data->xcvr_enable(pdev->id, 1);
+
+	if (request_irq(flexcan->irq, flexcan_irq_handler, IRQF_SAMPLE_RANDOM,
+			dev->name, dev))
+		goto irq_err;
+
+	if (flexcan_hw_open(flexcan))
+		goto open_err;
+
+	flexcan_mbm_init(flexcan);
+	netif_carrier_on(dev);
+	flexcan_hw_start(flexcan);
+	return 0;
+      open_err:
+	free_irq(flexcan->irq, dev);
+      irq_err:
+	if (plat_data && plat_data->xcvr_enable)
+		plat_data->xcvr_enable(pdev->id, 0);
+
+	if (flexcan->io_reg)
+		regulator_disable(flexcan->io_reg);
+      io_reg_err:
+	if (flexcan->core_reg)
+		regulator_disable(flexcan->core_reg);
+      core_reg_err:
+	if (flexcan->clk)
+		clk_disable(flexcan->clk);
+      clk_err:
+	if (plat_data && plat_data->inactive)
+		plat_data->inactive(pdev->id);
+	return -ENODEV;
+}
+
+static int flexcan_stop(struct net_device *dev)
+{
+	struct flexcan_device *flexcan;
+	struct platform_device *pdev;
+	struct flexcan_platform_data *plat_data;
+
+	flexcan = netdev_priv(dev);
+
+	BUG_ON(!flexcan);
+
+	pdev = flexcan->dev;
+	plat_data = (pdev->dev).platform_data;
+
+	flexcan_hw_stop(flexcan);
+
+	free_irq(flexcan->irq, dev);
+
+	if (plat_data && plat_data->xcvr_enable)
+		plat_data->xcvr_enable(pdev->id, 0);
+
+	if (flexcan->io_reg)
+		regulator_disable(flexcan->io_reg);
+	if (flexcan->core_reg)
+		regulator_disable(flexcan->core_reg);
+	if (flexcan->clk)
+		clk_disable(flexcan->clk);
+	if (plat_data && plat_data->inactive)
+		plat_data->inactive(pdev->id);
+	return 0;
+}
+
+static void flexcan_setup(struct net_device *dev)
+{
+	dev->type = ARPHRD_CAN;
+	dev->mtu = sizeof(struct can_frame);
+	dev->hard_header_len = 0;
+	dev->addr_len = 0;
+	dev->tx_queue_len = FLEXCAN_MAX_MB;
+	dev->flags = IFF_NOARP;
+	dev->features = NETIF_F_NO_CSUM;
+
+	dev->open = flexcan_open;
+	dev->stop = flexcan_stop;
+	dev->hard_start_xmit = flexcan_start_xmit;
+}
+
+static int flexcan_probe(struct platform_device *pdev)
+{
+	struct net_device *net;
+	net = flexcan_device_alloc(pdev, flexcan_setup);
+	if (!net)
+		return -ENOMEM;
+
+	if (register_netdev(net)) {
+		flexcan_device_free(pdev);
+		return -ENODEV;
+	}
+	return 0;
+}
+
+static int flexcan_remove(struct platform_device *pdev)
+{
+	flexcan_device_free(pdev);
+	return 0;
+}
+
+static int flexcan_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct net_device *net;
+	struct flexcan_device *flexcan;
+	struct flexcan_platform_data *plat_data;
+	net = (struct net_device *)dev_get_drvdata(&pdev->dev);
+	flexcan = netdev_priv(net);
+
+	BUG_ON(!flexcan);
+
+	if (!(net->flags & IFF_UP))
+		return 0;
+	if (flexcan->wakeup)
+		set_irq_wake(flexcan->irq, 1);
+	else {
+		plat_data = (pdev->dev).platform_data;
+
+		if (plat_data && plat_data->xcvr_enable)
+			plat_data->xcvr_enable(pdev->id, 0);
+
+		if (flexcan->io_reg)
+			regulator_disable(flexcan->io_reg);
+		if (flexcan->core_reg)
+			regulator_disable(flexcan->core_reg);
+		if (flexcan->clk)
+			clk_disable(flexcan->clk);
+		if (plat_data && plat_data->inactive)
+			plat_data->inactive(pdev->id);
+	}
+	return 0;
+}
+
+static int flexcan_resume(struct platform_device *pdev)
+{
+	struct net_device *net;
+	struct flexcan_device *flexcan;
+	struct flexcan_platform_data *plat_data;
+	net = (struct net_device *)dev_get_drvdata(&pdev->dev);
+	flexcan = netdev_priv(net);
+
+	BUG_ON(!flexcan);
+
+	if (!(net->flags & IFF_UP))
+		return 0;
+
+	if (flexcan->wakeup)
+		set_irq_wake(flexcan->irq, 0);
+	else {
+		plat_data = (pdev->dev).platform_data;
+		if (plat_data && plat_data->active)
+			plat_data->active(pdev->id);
+
+		if (flexcan->clk) {
+			if (clk_enable(flexcan->clk))
+				printk(KERN_ERR "%s:enable clock fail\n",
+				       __func__);
+		}
+
+		if (flexcan->core_reg) {
+			if (regulator_enable(flexcan->core_reg))
+				printk(KERN_ERR "%s:enable core voltage\n",
+				       __func__);
+		}
+		if (flexcan->io_reg) {
+			if (regulator_enable(flexcan->io_reg))
+				printk(KERN_ERR "%s:enable io voltage\n",
+				       __func__);
+		}
+
+		if (plat_data && plat_data->xcvr_enable)
+			plat_data->xcvr_enable(pdev->id, 1);
+	}
+	return 0;
+}
+
+static struct platform_driver flexcan_driver = {
+	.driver = {
+		   .name = FLEXCAN_DEVICE_NAME,
+		   },
+	.probe = flexcan_probe,
+	.remove = flexcan_remove,
+	.suspend = flexcan_suspend,
+	.resume = flexcan_resume,
+};
+
+static __init int flexcan_init(void)
+{
+	pr_info("Freescale FlexCAN Driver \n");
+	return platform_driver_register(&flexcan_driver);
+}
+
+static __exit void flexcan_exit(void)
+{
+	return platform_driver_unregister(&flexcan_driver);
+}
+
+module_init(flexcan_init);
+module_exit(flexcan_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/can/flexcan/flexcan.h b/drivers/net/can/flexcan/flexcan.h
new file mode 100644
index 000000000000..d19cc1ee0620
--- /dev/null
+++ b/drivers/net/can/flexcan/flexcan.h
@@ -0,0 +1,223 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file flexcan.h
+ *
+ * @brief FlexCan definitions.
+ *
+ * @ingroup can
+ */
+
+#ifndef __CAN_FLEXCAN_H__
+#define __CAN_FLEXCAN_H__
+
+#include <linux/list.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/clk.h>
+#include <linux/can.h>
+#include <linux/can/core.h>
+#include <linux/can/error.h>
+
+#define FLEXCAN_DEVICE_NAME	"FlexCAN"
+
+struct can_mb_cs {
+	unsigned int time_stamp:16;
+	unsigned int length:4;
+	unsigned int rtr:1;
+	unsigned int ide:1;
+	unsigned int srr:1;
+	unsigned int nouse1:1;
+	unsigned int code:4;
+	unsigned int nouse2:4;
+};
+
+#define CAN_MB_RX_INACTIVE	0x0
+#define CAN_MB_RX_EMPTY		0x4
+#define CAN_MB_RX_FULL		0x2
+#define CAN_MB_RX_OVERRUN	0x6
+#define CAN_MB_RX_BUSY		0x1
+
+#define CAN_MB_TX_INACTIVE	0x8
+#define CAN_MB_TX_ABORT		0x9
+#define CAN_MB_TX_ONCE		0xC
+#define CAN_MB_TX_REMOTE	0xA
+
+struct can_hw_mb {
+	union {
+		struct can_mb_cs cs;
+		unsigned int data;
+	} mb_cs;
+	unsigned int mb_id;
+	unsigned char mb_data[8];
+};
+
+#define CAN_HW_REG_MCR		0x00
+#define CAN_HW_REG_CTRL		0x04
+#define CAN_HW_REG_TIMER	0x08
+#define CAN_HW_REG_RXGMASK	0x10
+#define CAN_HW_REG_RX14MASK	0x14
+#define CAN_HW_REG_RX15MASK	0x18
+#define CAN_HW_REG_ECR		0x1C
+#define CAN_HW_REG_ESR		0x20
+#define CAN_HW_REG_IMASK2	0x24
+#define CAN_HW_REG_IMASK1	0x28
+#define CAN_HW_REG_IFLAG2	0x2C
+#define CAN_HW_REG_IFLAG1	0x30
+
+#define CAN_MB_BASE	0x0080
+#define CAN_RXMASK_BASE	0x0880
+#define CAN_FIFO_BASE	0xE0
+
+#define __MCR_MDIS		(1 << 31)
+#define __MCR_FRZ		(1 << 30)
+#define __MCR_FEN		(1 << 29)
+#define __MCR_HALT		(1 << 28)
+#define __MCR_NOTRDY		(1 << 27)
+#define __MCR_WAK_MSK		(1 << 26)
+#define __MCR_SOFT_RST		(1 << 25)
+#define __MCR_FRZ_ACK		(1 << 24)
+#define __MCR_SLF_WAK		(1 << 22)
+#define __MCR_WRN_EN		(1 << 21)
+#define __MCR_LPM_ACK		(1 << 20)
+#define __MCR_WAK_SRC		(1 << 19)
+#define __MCR_DOZE		(1 << 18)
+#define __MCR_SRX_DIS		(1 << 17)
+#define __MCR_BCC		(1 << 16)
+#define __MCR_LPRIO_EN		(1 << 13)
+#define __MCR_AEN		(1 << 12)
+#define __MCR_MAX_IDAM_OFFSET 	8
+#define __MCR_MAX_IDAM_MASK 	(0x3 << __MCR_MAX_IDAM_OFFSET)
+#define __MCR_MAX_IDAM_A	(0x0 << __MCR_MAX_IDAM_OFFSET)
+#define __MCR_MAX_IDAM_B	(0x1 << __MCR_MAX_IDAM_OFFSET)
+#define __MCR_MAX_IDAM_C	(0x2 << __MCR_MAX_IDAM_OFFSET)
+#define __MCR_MAX_IDAM_D	(0x3 << __MCR_MAX_IDAM_OFFSET)
+#define __MCR_MAX_MB_OFFSET 	0
+#define __MCR_MAX_MB_MASK 	(0x3F)
+
+#define __CTRL_PRESDIV_OFFSET	24
+#define __CTRL_PRESDIV_MASK	(0xFF << __CTRL_PRESDIV_OFFSET)
+#define __CTRL_RJW_OFFSET	22
+#define __CTRL_RJW_MASK		(0x3 << __CTRL_RJW_OFFSET)
+#define __CTRL_PSEG1_OFFSET	19
+#define __CTRL_PSEG1_MASK	(0x7 << __CTRL_PSEG1_OFFSET)
+#define __CTRL_PSEG2_OFFSET	16
+#define __CTRL_PSEG2_MASK	(0x7 << __CTRL_PSEG2_OFFSET)
+#define __CTRL_BOFF_MSK		(0x1 << 15)
+#define __CTRL_ERR_MSK		(0x1 << 14)
+#define __CTRL_CLK_SRC		(0x1 << 13)
+#define __CTRL_LPB		(0x1 << 12)
+#define __CTRL_TWRN_MSK		(0x1 << 11)
+#define __CTRL_RWRN_MSK		(0x1 << 10)
+#define __CTRL_SMP		(0x1 << 7)
+#define __CTRL_BOFF_REC		(0x1 << 6)
+#define __CTRL_TSYN		(0x1 << 5)
+#define __CTRL_LBUF		(0x1 << 4)
+#define __CTRL_LOM		(0x1 << 3)
+#define __CTRL_PROPSEG_OFFSET	0
+#define __CTRL_PROPSEG_MASK	(0x7)
+
+#define __ECR_TX_ERR_COUNTER(x) ((x) & 0xFF)
+#define __ECR_RX_ERR_COUNTER(x) (((x) >> 8) & 0xFF)
+#define __ECR_PASSIVE_THRESHOLD	128
+#define __ECR_ACTIVE_THRESHOLD	96
+
+#define __ESR_TWRN_INT		(0x1 << 17)
+#define __ESR_RWRN_INT		(0x1 << 16)
+#define __ESR_BIT1_ERR		(0x1 << 15)
+#define __ESR_BIT0_ERR		(0x1 << 14)
+#define __ESR_ACK_ERR		(0x1 << 13)
+#define __ESR_CRC_ERR		(0x1 << 12)
+#define __ESR_FRM_ERR		(0x1 << 11)
+#define __ESR_STF_ERR		(0x1 << 10)
+#define __ESR_TX_WRN		(0x1 << 9)
+#define __ESR_RX_WRN		(0x1 << 8)
+#define __ESR_IDLE		(0x1 << 7)
+#define __ESR_TXRX		(0x1 << 6)
+#define __ESR_FLT_CONF_OFF	4
+#define __ESR_FLT_CONF_MASK	(0x3 << __ESR_FLT_CONF_OFF)
+#define __ESR_BOFF_INT		(0x1 << 2)
+#define __ESR_ERR_INT		(0x1 << 1)
+#define __ESR_WAK_INT		(0x1)
+
+#define __ESR_INTERRUPTS	(__ESR_WAK_INT | __ESR_ERR_INT | \
+				__ESR_BOFF_INT | __ESR_TWRN_INT | \
+				__ESR_RWRN_INT)
+
+#define __FIFO_OV_INT		0x0080
+#define __FIFO_WARN_INT		0x0040
+#define __FIFO_RDY_INT		0x0020
+
+struct flexcan_device {
+	struct mutex mutex;
+	void *io_base;
+	struct can_hw_mb *hwmb;
+	unsigned int *rx_mask;
+	unsigned int xmit_mb;
+	unsigned int bitrate;
+	/* word 1 */
+	unsigned int br_presdiv:8;
+	unsigned int br_rjw:2;
+	unsigned int br_propseg:3;
+	unsigned int br_pseg1:3;
+	unsigned int br_pseg2:3;
+	unsigned int maxmb:6;
+	unsigned int xmit_maxmb:6;
+	unsigned int wd1_resv:1;
+
+	/* word 2 */
+	unsigned int fifo:1;
+	unsigned int wakeup:1;
+	unsigned int srx_dis:1;
+	unsigned int wak_src:1;
+	unsigned int bcc:1;
+	unsigned int lprio:1;
+	unsigned int abort:1;
+	unsigned int br_clksrc:1;
+	unsigned int loopback:1;
+	unsigned int smp:1;
+	unsigned int boff_rec:1;
+	unsigned int tsyn:1;
+	unsigned int listen:1;
+
+	unsigned int ext_msg:1;
+	unsigned int std_msg:1;
+
+	struct timer_list timer;
+	struct platform_device *dev;
+	struct regulator *core_reg;
+	struct regulator *io_reg;
+	struct clk *clk;
+	int irq;
+};
+
+#define FLEXCAN_MAX_FIFO_MB	8
+#define FLEXCAN_MAX_MB		64
+#define FLEXCAN_MAX_PRESDIV	256
+#define FLEXCAN_MAX_RJW		4
+#define FLEXCAN_MAX_PSEG1	8
+#define FLEXCAN_MAX_PSEG2	8
+#define FLEXCAN_MAX_PROPSEG	8
+#define FLEXCAN_MAX_BITRATE	1000000
+
+extern struct net_device *flexcan_device_alloc(struct platform_device *pdev,
+					       void (*setup) (struct net_device
+							      *dev));
+extern void flexcan_device_free(struct platform_device *pdev);
+
+extern void flexcan_mbm_init(struct flexcan_device *flexcan);
+extern void flexcan_mbm_isr(struct net_device *dev);
+extern int flexcan_mbm_xmit(struct flexcan_device *flexcan,
+			    struct can_frame *frame);
+#endif				/* __CAN_FLEXCAN_H__ */
diff --git a/drivers/net/can/flexcan/mbm.c b/drivers/net/can/flexcan/mbm.c
new file mode 100644
index 000000000000..ca1300503e02
--- /dev/null
+++ b/drivers/net/can/flexcan/mbm.c
@@ -0,0 +1,347 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mbm.c
+ *
+ * @brief Driver for Freescale CAN Controller FlexCAN.
+ *
+ * @ingroup can
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/platform_device.h>
+
+#include <asm/io.h>
+#include <asm/irq.h>
+#include "flexcan.h"
+
+#define flexcan_swab32(x)	\
+	(((x) << 24) | ((x) >> 24) |\
+		(((x) & (__u32)0x0000ff00UL) << 8) |\
+		(((x) & (__u32)0x00ff0000UL) >> 8))
+
+static inline void flexcan_memcpy(void *dst, void *src, int len)
+{
+	int i;
+	unsigned int *d = (unsigned int *)dst, *s = (unsigned int *)src;
+	len = (len + 3) >> 2;
+	for (i = 0; i < len; i++, s++, d++)
+		*d = flexcan_swab32(*s);
+}
+
+static void flexcan_mb_bottom(struct net_device *dev, int index)
+{
+	struct flexcan_device *flexcan = netdev_priv(dev);
+	struct net_device_stats *stats = dev->get_stats(dev);
+	struct can_hw_mb *hwmb;
+	struct can_frame *frame;
+	struct sk_buff *skb;
+	unsigned int tmp;
+
+	hwmb = flexcan->hwmb + index;
+	if (flexcan->fifo || (index >= (flexcan->maxmb - flexcan->xmit_maxmb))) {
+		if (hwmb->mb_cs.cs.code == CAN_MB_TX_ABORT)
+			hwmb->mb_cs.cs.code = CAN_MB_TX_INACTIVE;
+
+		if (hwmb->mb_cs.cs.code & CAN_MB_TX_INACTIVE) {
+			if (netif_queue_stopped(dev))
+				netif_start_queue(dev);
+			return;
+		}
+	}
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (skb) {
+		frame = (struct can_frame *)skb_put(skb, sizeof(*frame));
+		memset(frame, 0, sizeof(*frame));
+		if (hwmb->mb_cs.cs.ide)
+			frame->can_id =
+			    (hwmb->mb_id & CAN_EFF_MASK) | CAN_EFF_FLAG;
+		else
+			frame->can_id = (hwmb->mb_id >> 18) & CAN_SFF_MASK;
+
+		if (hwmb->mb_cs.cs.rtr)
+			frame->can_id |= CAN_RTR_FLAG;
+
+		frame->can_dlc = hwmb->mb_cs.cs.length;
+
+		if (frame->can_dlc && frame->can_dlc)
+			flexcan_memcpy(frame->data, hwmb->mb_data,
+				       frame->can_dlc);
+
+		if (flexcan->fifo
+		    || (index >= (flexcan->maxmb - flexcan->xmit_maxmb))) {
+			hwmb->mb_cs.cs.code = CAN_MB_TX_INACTIVE;
+			if (netif_queue_stopped(dev))
+				netif_start_queue(dev);
+		}
+
+		tmp = __raw_readl(flexcan->io_base + CAN_HW_REG_TIMER);
+
+		dev->last_rx = jiffies;
+		stats->rx_packets++;
+		stats->rx_bytes += frame->can_dlc;
+
+		skb->dev = dev;
+		skb->protocol = __constant_htons(ETH_P_CAN);
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+		netif_rx(skb);
+	} else {
+		tmp = hwmb->mb_cs.data;
+		tmp = hwmb->mb_id;
+		tmp = hwmb->mb_data[0];
+		if (flexcan->fifo
+		    || (index >= (flexcan->maxmb - flexcan->xmit_maxmb))) {
+
+			hwmb->mb_cs.cs.code = CAN_MB_TX_INACTIVE;
+			if (netif_queue_stopped(dev))
+				netif_start_queue(dev);
+		}
+		tmp = __raw_readl(flexcan->io_base + CAN_HW_REG_TIMER);
+		stats->rx_dropped++;
+	}
+}
+
+static void flexcan_fifo_isr(struct net_device *dev, unsigned int iflag1)
+{
+	struct flexcan_device *flexcan = dev ? netdev_priv(dev) : NULL;
+	struct net_device_stats *stats = dev->get_stats(dev);
+	struct sk_buff *skb;
+	struct can_hw_mb *hwmb = flexcan->hwmb;
+	struct can_frame *frame;
+	unsigned int tmp;
+
+	if (iflag1 & __FIFO_RDY_INT) {
+		skb = dev_alloc_skb(sizeof(struct can_frame));
+		if (skb) {
+			frame =
+			    (struct can_frame *)skb_put(skb, sizeof(*frame));
+			memset(frame, 0, sizeof(*frame));
+			if (hwmb->mb_cs.cs.ide)
+				frame->can_id =
+				    (hwmb->mb_id & CAN_EFF_MASK) | CAN_EFF_FLAG;
+			else
+				frame->can_id =
+				    (hwmb->mb_id >> 18) & CAN_SFF_MASK;
+
+			if (hwmb->mb_cs.cs.rtr)
+				frame->can_id |= CAN_RTR_FLAG;
+
+			frame->can_dlc = hwmb->mb_cs.cs.length;
+
+			if (frame->can_dlc && (frame->can_dlc <= 8))
+				flexcan_memcpy(frame->data, hwmb->mb_data,
+					       frame->can_dlc);
+			tmp = __raw_readl(flexcan->io_base + CAN_HW_REG_TIMER);
+
+			dev->last_rx = jiffies;
+
+			stats->rx_packets++;
+			stats->rx_bytes += frame->can_dlc;
+
+			skb->dev = dev;
+			skb->protocol = __constant_htons(ETH_P_CAN);
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
+			netif_rx(skb);
+		} else {
+			tmp = hwmb->mb_cs.data;
+			tmp = hwmb->mb_id;
+			tmp = hwmb->mb_data[0];
+			tmp = __raw_readl(flexcan->io_base + CAN_HW_REG_TIMER);
+		}
+	}
+
+	if (iflag1 & (__FIFO_OV_INT | __FIFO_WARN_INT)) {
+		skb = dev_alloc_skb(sizeof(struct can_frame));
+		if (skb) {
+			frame =
+			    (struct can_frame *)skb_put(skb, sizeof(*frame));
+			memset(frame, 0, sizeof(*frame));
+			frame->can_id = CAN_ERR_FLAG | CAN_ERR_CRTL;
+			frame->can_dlc = CAN_ERR_DLC;
+			if (iflag1 & __FIFO_WARN_INT)
+				frame->data[1] |=
+				    CAN_ERR_CRTL_TX_WARNING |
+				    CAN_ERR_CRTL_RX_WARNING;
+			if (iflag1 & __FIFO_OV_INT)
+				frame->data[1] |= CAN_ERR_CRTL_RX_OVERFLOW;
+
+			skb->dev = dev;
+			skb->protocol = __constant_htons(ETH_P_CAN);
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
+			netif_rx(skb);
+		}
+	}
+}
+
+/*!
+ * @brief The function call by CAN ISR to handle mb events.
+ *
+ * @param dev		the pointer of network device.
+ *
+ * @return none
+ */
+void flexcan_mbm_isr(struct net_device *dev)
+{
+	int i, iflag1, iflag2, maxmb;
+	struct flexcan_device *flexcan = dev ? netdev_priv(dev) : NULL;
+
+	if (flexcan->maxmb > 31) {
+		maxmb = flexcan->maxmb + 1 - 32;
+		iflag1 = __raw_readl(flexcan->io_base + CAN_HW_REG_IFLAG1) &
+		    __raw_readl(flexcan->io_base + CAN_HW_REG_IMASK1);
+		iflag2 = __raw_readl(flexcan->io_base + CAN_HW_REG_IFLAG2) &
+		    __raw_readl(flexcan->io_base + CAN_HW_REG_IMASK2);
+		iflag2 &= (1 << maxmb) - 1;
+		maxmb = 32;
+	} else {
+		maxmb = flexcan->maxmb + 1;
+		iflag1 = __raw_readl(flexcan->io_base + CAN_HW_REG_IFLAG1) &
+		    __raw_readl(flexcan->io_base + CAN_HW_REG_IMASK1);
+		iflag1 &= (1 << maxmb) - 1;
+		iflag2 = 0;
+	}
+
+	__raw_writel(iflag1, flexcan->io_base + CAN_HW_REG_IFLAG1);
+	__raw_writel(iflag2, flexcan->io_base + CAN_HW_REG_IFLAG2);
+
+	if (flexcan->fifo) {
+		flexcan_fifo_isr(dev, iflag1);
+		iflag1 &= 0xFFFFFF00;
+	}
+	for (i = 0; iflag1 && (i < maxmb); i++) {
+		if (iflag1 & (1 << i)) {
+			iflag1 &= ~(1 << i);
+			flexcan_mb_bottom(dev, i);
+		}
+	}
+
+	for (i = maxmb; iflag2 && (i <= flexcan->maxmb); i++) {
+		if (iflag2 & (1 << (i - 32))) {
+			iflag2 &= ~(1 << (i - 32));
+			flexcan_mb_bottom(dev, i);
+		}
+	}
+}
+
+/*!
+ * @brief function to xmit message buffer
+ *
+ * @param flexcan	the pointer of can hardware device.
+ * @param frame		the pointer of can message frame.
+ *
+ * @return	Returns 0 if xmit is success. otherwise returns non-zero.
+ */
+int flexcan_mbm_xmit(struct flexcan_device *flexcan, struct can_frame *frame)
+{
+	int i = flexcan->xmit_mb;
+	struct can_hw_mb *hwmb = flexcan->hwmb;
+
+	do {
+		if (hwmb[i].mb_cs.cs.code == CAN_MB_TX_INACTIVE)
+			break;
+		if ((++i) > flexcan->maxmb) {
+			if (flexcan->fifo)
+				i = FLEXCAN_MAX_FIFO_MB;
+			else
+				i = flexcan->xmit_maxmb + 1;
+		}
+		if (i == flexcan->xmit_mb)
+			return -1;
+	} while (1);
+
+	flexcan->xmit_mb = i + 1;
+	if (flexcan->xmit_mb > flexcan->maxmb) {
+		if (flexcan->fifo)
+			flexcan->xmit_mb = FLEXCAN_MAX_FIFO_MB;
+		else
+			flexcan->xmit_mb = flexcan->xmit_maxmb + 1;
+	}
+
+	if (frame->can_id & CAN_RTR_FLAG)
+		hwmb[i].mb_cs.cs.rtr = 1;
+	else
+		hwmb[i].mb_cs.cs.rtr = 0;
+
+	if (frame->can_id & CAN_EFF_FLAG) {
+		hwmb[i].mb_cs.cs.ide = 1;
+		hwmb[i].mb_cs.cs.srr = 1;
+		hwmb[i].mb_id = frame->can_id & CAN_EFF_MASK;
+	} else {
+		hwmb[i].mb_cs.cs.ide = 0;
+		hwmb[i].mb_id = (frame->can_id & CAN_SFF_MASK) << 18;
+	}
+
+	hwmb[i].mb_cs.cs.length = frame->can_dlc;
+	flexcan_memcpy(hwmb[i].mb_data, frame->data, frame->can_dlc);
+	hwmb[i].mb_cs.cs.code = CAN_MB_TX_ONCE;
+	return 0;
+}
+
+/*!
+ * @brief function to initial message buffer
+ *
+ * @param flexcan	the pointer of can hardware device.
+ *
+ * @return	none
+ */
+void flexcan_mbm_init(struct flexcan_device *flexcan)
+{
+	struct can_hw_mb *hwmb;
+	int rx_mb, i;
+
+	/* Set global mask to receive all messages */
+	__raw_writel(0, flexcan->io_base + CAN_HW_REG_RXGMASK);
+	__raw_writel(0, flexcan->io_base + CAN_HW_REG_RX14MASK);
+	__raw_writel(0, flexcan->io_base + CAN_HW_REG_RX15MASK);
+
+	memset(flexcan->hwmb, 0, sizeof(*hwmb) * FLEXCAN_MAX_MB);
+	/* Set individual mask to receive all messages */
+	memset(flexcan->rx_mask, 0, sizeof(unsigned int) * FLEXCAN_MAX_MB);
+
+	if (flexcan->fifo)
+		rx_mb = FLEXCAN_MAX_FIFO_MB;
+	else
+		rx_mb = flexcan->maxmb - flexcan->xmit_maxmb;
+
+	hwmb = flexcan->hwmb;
+	if (flexcan->fifo) {
+		unsigned long *id_table = flexcan->io_base + CAN_FIFO_BASE;
+		for (i = 0; i < rx_mb; i++)
+			id_table[i] = 0;
+	} else {
+		for (i = 0; i < rx_mb; i++) {
+			hwmb[i].mb_cs.cs.code = CAN_MB_RX_EMPTY;
+			/*
+			 * IDE bit can not control by mask registers
+			 * So set message buffer to receive extend
+			 * or standard message.
+			 */
+			if (flexcan->ext_msg && flexcan->std_msg)
+				hwmb[i].mb_cs.cs.ide = i & 1;
+			else {
+				if (flexcan->ext_msg)
+					hwmb[i].mb_cs.cs.ide = 1;
+			}
+		}
+	}
+
+	for (; i <= flexcan->maxmb; i++)
+		hwmb[i].mb_cs.cs.code = CAN_MB_TX_INACTIVE;
+
+	flexcan->xmit_mb = rx_mb;
+}
diff --git a/drivers/net/can/mcp251x.c b/drivers/net/can/mcp251x.c
new file mode 100644
index 000000000000..1dcda35e0d7d
--- /dev/null
+++ b/drivers/net/can/mcp251x.c
@@ -0,0 +1,1239 @@
+/*
+ *
+ * CAN bus driver for Microchip 251x CAN Controller with SPI Interface
+ *
+ * MCP2510 support and bug fixes by Christian Pellegrin
+ * <chripell@evolware.org>
+ *
+ * Copyright 2007 Raymarine UK, Ltd. All Rights Reserved.
+ * Written under contract by:
+ *   Chris Elston, Katalix Systems, Ltd.
+ *
+ * Based on Microchip MCP251x CAN controller driver written by
+ * David Vrabel, Copyright 2006 Arcom Control Systems Ltd.
+ *
+ * Based on CAN bus driver for the CCAN controller written by
+ * - Sascha Hauer, Marc Kleine-Budde, Pengutronix
+ * - Simon Kallweit, intefo AG
+ * Copyright 2007
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *
+ *
+ * Your platform definition file should specify something like:
+ *
+ * static struct mcp251x_platform_data mcp251x_info = {
+ *         .oscillator_frequency = 8000000,
+ *         .board_specific_setup = &mcp251x_setup,
+ *         .model = CAN_MCP251X_MCP2510,
+ *         .power_enable = mcp251x_power_enable,
+ *         .transceiver_enable = NULL,
+ * };
+ *
+ * static struct spi_board_info spi_board_info[] = {
+ *         {
+ *                 .modalias      = "mcp251x",
+ *                 .platform_data = &mcp251x_info,
+ *                 .irq           = IRQ_EINT13,
+ *                 .max_speed_hz  = 2*1000*1000,
+ *                 .chip_select   = 2,
+ *         },
+ * };
+ *
+ * Please see mcp251x.h for a description of the fields in
+ * struct mcp251x_platform_data.
+ *
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/netdevice.h>
+#include <linux/can.h>
+#include <linux/spi/spi.h>
+#include <linux/can/dev.h>
+#include <linux/can/core.h>
+#include <linux/if_arp.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/completion.h>
+#include <linux/freezer.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/can/platform/mcp251x.h>
+
+/* SPI interface instruction set */
+#define INSTRUCTION_WRITE		0x02
+#define INSTRUCTION_READ		0x03
+#define INSTRUCTION_BIT_MODIFY	0x05
+#define INSTRUCTION_LOAD_TXB(n)	(0x40 + 2 * (n))
+#define INSTRUCTION_READ_RXB(n)	(((n) == 0) ? 0x90 : 0x94)
+#define INSTRUCTION_RESET		0xC0
+
+/* MPC251x registers */
+#define CANSTAT	      0x0e
+#define CANCTRL	      0x0f
+#  define CANCTRL_REQOP_MASK	    0xe0
+#  define CANCTRL_REQOP_CONF	    0x80
+#  define CANCTRL_REQOP_LISTEN_ONLY 0x60
+#  define CANCTRL_REQOP_LOOPBACK    0x40
+#  define CANCTRL_REQOP_SLEEP	    0x20
+#  define CANCTRL_REQOP_NORMAL	    0x00
+#  define CANCTRL_OSM		    0x08
+#  define CANCTRL_ABAT		    0x10
+#define TEC	      0x1c
+#define REC	      0x1d
+#define CNF1	      0x2a
+#define CNF2	      0x29
+#  define CNF2_BTLMODE	0x80
+#define CNF3	      0x28
+#  define CNF3_SOF	0x08
+#  define CNF3_WAKFIL	0x04
+#  define CNF3_PHSEG2_MASK 0x07
+#define CANINTE	      0x2b
+#  define CANINTE_MERRE 0x80
+#  define CANINTE_WAKIE 0x40
+#  define CANINTE_ERRIE 0x20
+#  define CANINTE_TX2IE 0x10
+#  define CANINTE_TX1IE 0x08
+#  define CANINTE_TX0IE 0x04
+#  define CANINTE_RX1IE 0x02
+#  define CANINTE_RX0IE 0x01
+#define CANINTF	      0x2c
+#  define CANINTF_MERRF 0x80
+#  define CANINTF_WAKIF 0x40
+#  define CANINTF_ERRIF 0x20
+#  define CANINTF_TX2IF 0x10
+#  define CANINTF_TX1IF 0x08
+#  define CANINTF_TX0IF 0x04
+#  define CANINTF_RX1IF 0x02
+#  define CANINTF_RX0IF 0x01
+#define EFLG	      0x2d
+#  define EFLG_EWARN	0x01
+#  define EFLG_RXWAR	0x02
+#  define EFLG_TXWAR	0x04
+#  define EFLG_RXEP	0x08
+#  define EFLG_TXEP	0x10
+#  define EFLG_TXBO	0x20
+#  define EFLG_RX0OVR	0x40
+#  define EFLG_RX1OVR	0x80
+#define TXBCTRL(n)  ((n * 0x10) + 0x30)
+#  define TXBCTRL_ABTF	0x40
+#  define TXBCTRL_MLOA	0x20
+#  define TXBCTRL_TXERR 0x10
+#  define TXBCTRL_TXREQ 0x08
+#define RXBCTRL(n)  ((n * 0x10) + 0x60)
+#  define RXBCTRL_BUKT	 0x04
+#  define RXBCTRL_RXM0	 0x20
+#  define RXBCTRL_RXM1	 0x40
+
+/* Buffer size required for the largest SPI transfer (i.e., reading a
+ * frame). */
+#define CAN_FRAME_MAX_DATA_LEN	8
+#define SPI_TRANSFER_BUF_LEN	(2*(6 + CAN_FRAME_MAX_DATA_LEN))
+#define CAN_FRAME_MAX_BITS	128
+
+#define DEVICE_NAME "mcp251x"
+
+static int mcp251x_enable_dma; /* Enable SPI DMA. Default: 0 (Off) */
+module_param(mcp251x_enable_dma, int, S_IRUGO);
+MODULE_PARM_DESC(mcp251x_enable_dma, "Enable SPI DMA. Default: 0 (Off)");
+
+static struct can_bittiming_const mcp251x_bittiming_const = {
+	.tseg1_min = 3,
+	.tseg1_max = 16,
+	.tseg2_min = 2,
+	.tseg2_max = 8,
+	.sjw_max = 4,
+	.brp_min = 1,
+	.brp_max = 64,
+	.brp_inc = 1,
+};
+
+struct mcp251x_priv {
+	struct can_priv	   can;
+	struct net_device *net;
+	struct spi_device *spi;
+
+	struct mutex spi_lock; /* SPI buffer lock */
+	u8 *spi_tx_buf;
+	u8 *spi_rx_buf;
+	dma_addr_t spi_tx_dma;
+	dma_addr_t spi_rx_dma;
+
+	struct sk_buff *tx_skb;
+	struct workqueue_struct *wq;
+	struct work_struct tx_work;
+	struct work_struct irq_work;
+	struct completion awake;
+	int wake;
+	int force_quit;
+	int after_suspend;
+#define AFTER_SUSPEND_UP 1
+#define AFTER_SUSPEND_DOWN 2
+#define AFTER_SUSPEND_POWER 4
+	int restart_tx;
+};
+
+static u8 mcp251x_read_reg(struct spi_device *spi, uint8_t reg)
+{
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct spi_transfer t = {
+		.tx_buf = priv->spi_tx_buf,
+		.rx_buf = priv->spi_rx_buf,
+		.len = 3,
+		.cs_change = 0,
+	};
+	struct spi_message m;
+	u8 val = 0;
+	int ret;
+
+	mutex_lock(&priv->spi_lock);
+
+	priv->spi_tx_buf[0] = INSTRUCTION_READ;
+	priv->spi_tx_buf[1] = reg;
+
+	spi_message_init(&m);
+
+	if (mcp251x_enable_dma) {
+		t.tx_dma = priv->spi_tx_dma;
+		t.rx_dma = priv->spi_rx_dma;
+		m.is_dma_mapped = 1;
+	}
+
+	spi_message_add_tail(&t, &m);
+
+	ret = spi_sync(spi, &m);
+	if (ret < 0)
+		dev_dbg(&spi->dev, "%s: failed: ret = %d\n", __func__, ret);
+	else
+		val = priv->spi_rx_buf[2];
+
+	mutex_unlock(&priv->spi_lock);
+
+	dev_dbg(&spi->dev, "%s: read %02x = %02x\n", __func__, reg, val);
+	return val;
+}
+
+static void mcp251x_write_reg(struct spi_device *spi, u8 reg, uint8_t val)
+{
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct spi_transfer t = {
+		.tx_buf = priv->spi_tx_buf,
+		.rx_buf = priv->spi_rx_buf,
+		.len = 3,
+		.cs_change = 0,
+	};
+	struct spi_message m;
+	int ret;
+
+	mutex_lock(&priv->spi_lock);
+
+	priv->spi_tx_buf[0] = INSTRUCTION_WRITE;
+	priv->spi_tx_buf[1] = reg;
+	priv->spi_tx_buf[2] = val;
+
+	spi_message_init(&m);
+
+	if (mcp251x_enable_dma) {
+		t.tx_dma = priv->spi_tx_dma;
+		t.rx_dma = priv->spi_rx_dma;
+		m.is_dma_mapped = 1;
+	}
+
+	spi_message_add_tail(&t, &m);
+
+	ret = spi_sync(spi, &m);
+
+	mutex_unlock(&priv->spi_lock);
+
+	if (ret < 0)
+		dev_dbg(&spi->dev, "%s: failed\n", __func__);
+}
+
+static void mcp251x_write_bits(struct spi_device *spi, u8 reg,
+			       u8 mask, uint8_t val)
+{
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct spi_transfer t = {
+		.tx_buf = priv->spi_tx_buf,
+		.rx_buf = priv->spi_rx_buf,
+		.len = 4,
+		.cs_change = 0,
+	};
+	struct spi_message m;
+	int ret;
+
+	mutex_lock(&priv->spi_lock);
+
+	priv->spi_tx_buf[0] = INSTRUCTION_BIT_MODIFY;
+	priv->spi_tx_buf[1] = reg;
+	priv->spi_tx_buf[2] = mask;
+	priv->spi_tx_buf[3] = val;
+
+	spi_message_init(&m);
+
+	if (mcp251x_enable_dma) {
+		t.tx_dma = priv->spi_tx_dma;
+		t.rx_dma = priv->spi_rx_dma;
+		m.is_dma_mapped = 1;
+	}
+
+	spi_message_add_tail(&t, &m);
+
+	ret = spi_sync(spi, &m);
+
+	mutex_unlock(&priv->spi_lock);
+
+	if (ret < 0)
+		dev_dbg(&spi->dev, "%s: failed\n", __func__);
+}
+
+static int mcp251x_hw_tx(struct spi_device *spi, struct can_frame *frame,
+			  int tx_buf_idx)
+{
+	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	u32 sid, eid, exide, rtr;
+
+	dev_dbg(&spi->dev, "%s\n", __func__);
+
+	exide = (frame->can_id & CAN_EFF_FLAG) ? 1 : 0; /* Extended ID Enable */
+	if (exide)
+		sid = (frame->can_id & CAN_EFF_MASK) >> 18;
+	else
+		sid = frame->can_id & CAN_SFF_MASK; /* Standard ID */
+	eid = frame->can_id & CAN_EFF_MASK; /* Extended ID */
+	rtr = (frame->can_id & CAN_RTR_FLAG) ? 1 : 0; /* Remote transmission */
+
+	if (pdata->model == CAN_MCP251X_MCP2510) {
+		int i;
+
+		mcp251x_write_reg(spi, TXBCTRL(tx_buf_idx) + 1, sid >> 3);
+		mcp251x_write_reg(spi, TXBCTRL(tx_buf_idx) + 2,
+				  ((sid & 7) << 5) | (exide << 3) |
+				  ((eid >> 16) & 3));
+		mcp251x_write_reg(spi, TXBCTRL(tx_buf_idx) + 3,
+				  (eid >> 8) & 0xff);
+		mcp251x_write_reg(spi, TXBCTRL(tx_buf_idx) + 4, eid & 0xff);
+		mcp251x_write_reg(spi, TXBCTRL(tx_buf_idx) + 5,
+				  (rtr << 6) | frame->can_dlc);
+
+		for (i = 0; i < frame->can_dlc ; i++) {
+			mcp251x_write_reg(spi, TXBCTRL(tx_buf_idx) + 6 + i,
+					  frame->data[i]);
+		}
+	} else {
+		struct spi_transfer t = {
+			.tx_buf = priv->spi_tx_buf,
+			.rx_buf = priv->spi_rx_buf,
+			.cs_change = 0,
+			.len = 6 + CAN_FRAME_MAX_DATA_LEN,
+		};
+		struct spi_message m;
+		int ret;
+		u8 *tx_buf = priv->spi_tx_buf;
+
+		mutex_lock(&priv->spi_lock);
+
+		tx_buf[0] = INSTRUCTION_LOAD_TXB(tx_buf_idx);
+		tx_buf[1] = sid >> 3;
+		tx_buf[2] = ((sid & 7) << 5) | (exide << 3) |
+		  ((eid >> 16) & 3);
+		tx_buf[3] = (eid >> 8) & 0xff;
+		tx_buf[4] = eid & 0xff;
+		tx_buf[5] = (rtr << 6) | frame->can_dlc;
+
+		memcpy(tx_buf + 6, frame->data, frame->can_dlc);
+
+		spi_message_init(&m);
+
+		if (mcp251x_enable_dma) {
+			t.tx_dma = priv->spi_tx_dma;
+			t.rx_dma = priv->spi_rx_dma;
+			m.is_dma_mapped = 1;
+		}
+
+		spi_message_add_tail(&t, &m);
+
+		ret = spi_sync(spi, &m);
+
+		mutex_unlock(&priv->spi_lock);
+
+		if (ret < 0) {
+			dev_dbg(&spi->dev, "%s: failed: ret = %d\n", __func__,
+				ret);
+			return -1;
+		}
+	}
+	mcp251x_write_reg(spi, TXBCTRL(tx_buf_idx), TXBCTRL_TXREQ);
+	return 0;
+}
+
+static void mcp251x_hw_rx(struct spi_device *spi, int buf_idx)
+{
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
+	struct sk_buff *skb;
+	struct can_frame *frame;
+
+	dev_dbg(&spi->dev, "%s\n", __func__);
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (!skb) {
+		dev_dbg(&spi->dev, "%s: out of memory for Rx'd frame\n",
+			__func__);
+		priv->net->stats.rx_dropped++;
+		return;
+	}
+	skb->dev = priv->net;
+	frame = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
+
+	if (pdata->model == CAN_MCP251X_MCP2510) {
+		int i;
+		u8 rx_buf[6];
+
+		rx_buf[1] = mcp251x_read_reg(spi, RXBCTRL(buf_idx) + 1);
+		rx_buf[2] = mcp251x_read_reg(spi, RXBCTRL(buf_idx) + 2);
+		rx_buf[3] = mcp251x_read_reg(spi, RXBCTRL(buf_idx) + 3);
+		rx_buf[4] = mcp251x_read_reg(spi, RXBCTRL(buf_idx) + 4);
+		rx_buf[5] = mcp251x_read_reg(spi, RXBCTRL(buf_idx) + 5);
+
+		if ((rx_buf[2] >> 3) & 0x1) {
+			/* Extended ID format */
+			frame->can_id = CAN_EFF_FLAG;
+			frame->can_id |= ((rx_buf[2] & 3) << 16) |
+			  (rx_buf[3] << 8) | rx_buf[4] |
+			  (((rx_buf[1] << 3) | (rx_buf[2] >> 5)) << 18);
+		} else {
+			/* Standard ID format */
+			frame->can_id = (rx_buf[1] << 3) | (rx_buf[2] >> 5);
+		}
+
+		if ((rx_buf[5] >> 6) & 0x1) {
+			/* Remote transmission request */
+			frame->can_id |= CAN_RTR_FLAG;
+		}
+
+		/* Data length */
+		frame->can_dlc = rx_buf[5] & 0x0f;
+		if (frame->can_dlc > 8) {
+			dev_warn(&spi->dev, "invalid frame recevied\n");
+			priv->net->stats.rx_errors++;
+			dev_kfree_skb(skb);
+			return;
+		}
+
+		for (i = 0; i < frame->can_dlc; i++) {
+			frame->data[i] = mcp251x_read_reg(spi,
+							  RXBCTRL(buf_idx) +
+							  6 + i);
+		}
+	} else {
+		struct spi_transfer t = {
+			.tx_buf = priv->spi_tx_buf,
+			.rx_buf = priv->spi_rx_buf,
+			.cs_change = 0,
+			.len = 14, /* RX buffer: RXBnCTRL to RXBnD7 */
+		};
+		struct spi_message m;
+		int ret;
+		u8 *tx_buf = priv->spi_tx_buf;
+		u8 *rx_buf = priv->spi_rx_buf;
+
+		mutex_lock(&priv->spi_lock);
+
+		tx_buf[0] = INSTRUCTION_READ_RXB(buf_idx);
+
+		spi_message_init(&m);
+
+		if (mcp251x_enable_dma) {
+			t.tx_dma = priv->spi_tx_dma;
+			t.rx_dma = priv->spi_rx_dma;
+			m.is_dma_mapped = 1;
+		}
+
+		spi_message_add_tail(&t, &m);
+
+		ret = spi_sync(spi, &m);
+
+		if (ret < 0) {
+			dev_dbg(&spi->dev, "%s: failed: ret = %d\n",
+				__func__, ret);
+			priv->net->stats.rx_errors++;
+			mutex_unlock(&priv->spi_lock);
+			return;
+		}
+
+		if ((rx_buf[2] >> 3) & 0x1) {
+			/* Extended ID format */
+			frame->can_id = CAN_EFF_FLAG;
+			frame->can_id |= ((rx_buf[2] & 3) << 16) |
+			  (rx_buf[3] << 8) | rx_buf[4] |
+			  (((rx_buf[1] << 3) | (rx_buf[2] >> 5)) << 18);
+		} else {
+			/* Standard ID format */
+			frame->can_id = (rx_buf[1] << 3) | (rx_buf[2] >> 5);
+		}
+
+		if ((rx_buf[5] >> 6) & 0x1) {
+			/* Remote transmission request */
+			frame->can_id |= CAN_RTR_FLAG;
+		}
+
+		/* Data length */
+		frame->can_dlc = rx_buf[5] & 0x0f;
+		if (frame->can_dlc > 8) {
+			dev_warn(&spi->dev, "invalid frame recevied\n");
+			priv->net->stats.rx_errors++;
+			dev_kfree_skb(skb);
+			mutex_unlock(&priv->spi_lock);
+			return;
+		}
+
+		memcpy(frame->data, rx_buf + 6, CAN_FRAME_MAX_DATA_LEN);
+
+		mutex_unlock(&priv->spi_lock);
+	}
+
+	priv->net->stats.rx_packets++;
+	priv->net->stats.rx_bytes += frame->can_dlc;
+
+	skb->protocol = __constant_htons(ETH_P_CAN);
+	skb->pkt_type = PACKET_BROADCAST;
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	netif_rx(skb);
+}
+
+static void mcp251x_hw_sleep(struct spi_device *spi)
+{
+	mcp251x_write_reg(spi, CANCTRL, CANCTRL_REQOP_SLEEP);
+}
+
+static void mcp251x_hw_wakeup(struct spi_device *spi)
+{
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+
+	priv->wake = 1;
+
+	/* Can only wake up by generating a wake-up interrupt. */
+	mcp251x_write_bits(spi, CANINTE, CANINTE_WAKIE, CANINTE_WAKIE);
+	mcp251x_write_bits(spi, CANINTF, CANINTF_WAKIF, CANINTF_WAKIF);
+
+	/* Wait until the device is awake */
+	if (!wait_for_completion_timeout(&priv->awake, HZ))
+		dev_err(&spi->dev, "MCP251x didn't wake-up\n");
+}
+
+static int mcp251x_hard_start_xmit(struct sk_buff *skb, struct net_device *net)
+{
+	struct mcp251x_priv *priv = netdev_priv(net);
+	struct spi_device *spi = priv->spi;
+
+	dev_dbg(&spi->dev, "%s\n", __func__);
+
+	if (priv->tx_skb) {
+		dev_warn(&spi->dev, "hard_xmit called with not null tx_skb\n");
+		return NETDEV_TX_BUSY;
+	}
+
+	if (skb->len != sizeof(struct can_frame)) {
+		dev_dbg(&spi->dev, "dropping packet - bad length\n");
+		dev_kfree_skb(skb);
+		net->stats.tx_dropped++;
+		return 0;
+	}
+
+	netif_stop_queue(net);
+	priv->tx_skb = skb;
+	net->trans_start = jiffies;
+	queue_work(priv->wq, &priv->tx_work);
+
+	return NETDEV_TX_OK;
+}
+
+static int mcp251x_do_set_mode(struct net_device *net, enum can_mode mode)
+{
+	struct mcp251x_priv *priv = netdev_priv(net);
+	struct spi_device *spi = priv->spi;
+
+	dev_dbg(&spi->dev, "%s (unimplemented)\n", __func__);
+
+	switch (mode) {
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+static void mcp251x_set_normal_mode(struct spi_device *spi)
+{
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	unsigned long timeout;
+
+	/* Enable interrupts */
+	mcp251x_write_reg(spi, CANINTE,
+		CANINTE_ERRIE | CANINTE_TX2IE | CANINTE_TX1IE |
+		CANINTE_TX0IE | CANINTE_RX1IE | CANINTE_RX0IE);
+
+	if (priv->can.ctrlmode & CAN_CTRLMODE_LOOPBACK) {
+		/* Put device into loopback mode */
+		mcp251x_write_reg(spi, CANCTRL, CANCTRL_REQOP_LOOPBACK);
+	} else {
+		/* Put device into normal mode */
+		mcp251x_write_reg(spi, CANCTRL, CANCTRL_REQOP_NORMAL);
+
+		/* Wait for the device to enter normal mode */
+		timeout = jiffies + HZ;
+		while (mcp251x_read_reg(spi, CANSTAT) & 0xE0) {
+			udelay(10);
+			if (time_after(jiffies, timeout)) {
+				dev_err(&spi->dev, "MCP251x didn't"
+					" enter in normal mode\n");
+				break;
+			}
+		}
+	}
+}
+
+static int mcp251x_do_set_bittiming(struct net_device *net)
+{
+	struct mcp251x_priv *priv = netdev_priv(net);
+	struct can_bittiming *bt = &priv->can.bittiming;
+	struct spi_device *spi = priv->spi;
+	u8 state;
+
+	dev_dbg(&spi->dev, "%s: BRP = %d, PropSeg = %d, PS1 = %d,"
+		" PS2 = %d, SJW = %d\n", __func__, bt->brp,
+		bt->prop_seg, bt->phase_seg1, bt->phase_seg2,
+		bt->sjw);
+
+	/* Store original mode and set mode to config */
+	state = mcp251x_read_reg(spi, CANCTRL);
+	state = mcp251x_read_reg(spi, CANSTAT) & CANCTRL_REQOP_MASK;
+	mcp251x_write_bits(spi, CANCTRL, CANCTRL_REQOP_MASK,
+			   CANCTRL_REQOP_CONF);
+
+	mcp251x_write_reg(spi, CNF1, ((bt->sjw - 1) << 6) | (bt->brp - 1));
+	mcp251x_write_reg(spi, CNF2, CNF2_BTLMODE |
+			  ((bt->phase_seg1 - 1) << 3) |
+			  (bt->prop_seg - 1));
+	mcp251x_write_bits(spi, CNF3, CNF3_PHSEG2_MASK,
+			   (bt->phase_seg2 - 1));
+
+	/* Restore original state */
+	mcp251x_write_bits(spi, CANCTRL, CANCTRL_REQOP_MASK, state);
+
+	return 0;
+}
+
+static void mcp251x_setup(struct net_device *net, struct mcp251x_priv *priv,
+			  struct spi_device *spi)
+{
+	int ret;
+
+	/* Set initial baudrate. Make sure that registers are updated
+	   always by explicitly calling mcp251x_do_set_bittiming */
+	ret = can_set_bittiming(net);
+	if (ret)
+		dev_err(&spi->dev, "unable to set initial baudrate!\n");
+	else
+		mcp251x_do_set_bittiming(net);
+
+	/* Enable RX0->RX1 buffer roll over and disable filters */
+	mcp251x_write_bits(spi, RXBCTRL(0),
+			   RXBCTRL_BUKT | RXBCTRL_RXM0 | RXBCTRL_RXM1,
+			   RXBCTRL_BUKT | RXBCTRL_RXM0 | RXBCTRL_RXM1);
+	mcp251x_write_bits(spi, RXBCTRL(1),
+			   RXBCTRL_RXM0 | RXBCTRL_RXM1,
+			   RXBCTRL_RXM0 | RXBCTRL_RXM1);
+
+	dev_dbg(&spi->dev, "%s RXBCTL 0 and 1: %02x %02x\n", __func__,
+		mcp251x_read_reg(spi, RXBCTRL(0)),
+		mcp251x_read_reg(spi, RXBCTRL(1)));
+}
+
+static void mcp251x_hw_reset(struct spi_device *spi)
+{
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	int ret;
+
+	mutex_lock(&priv->spi_lock);
+
+	priv->spi_tx_buf[0] = INSTRUCTION_RESET;
+
+	ret = spi_write(spi, priv->spi_tx_buf, 1);
+
+	mutex_unlock(&priv->spi_lock);
+
+	if (ret < 0)
+		dev_dbg(&spi->dev, "%s: failed: ret = %d\n", __func__, ret);
+	/* wait for reset to finish */
+	mdelay(10);
+}
+
+static int mcp251x_hw_probe(struct spi_device *spi)
+{
+	int st1, st2;
+
+	mcp251x_hw_reset(spi);
+
+	st1 = mcp251x_read_reg(spi, CANSTAT) & 0xEE;
+	st2 = mcp251x_read_reg(spi, CANCTRL) & 0x17;
+
+	dev_dbg(&spi->dev, "%s: 0x%02x - 0x%02x\n", __func__,
+		st1, st2);
+
+	/* check for power up default values */
+	return (st1 == 0x80 && st2 == 0x07) ? 1 : 0;
+}
+
+static int mcp251x_open(struct net_device *net)
+{
+	struct mcp251x_priv *priv = netdev_priv(net);
+	struct spi_device *spi = priv->spi;
+	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
+
+	dev_dbg(&spi->dev, "%s\n", __func__);
+
+	if (pdata->transceiver_enable)
+		pdata->transceiver_enable(1);
+
+	priv->force_quit = 0;
+	priv->tx_skb = NULL;
+	enable_irq(spi->irq);
+	mcp251x_hw_wakeup(spi);
+	mcp251x_hw_reset(spi);
+	mcp251x_setup(net, priv, spi);
+	mcp251x_set_normal_mode(spi);
+	netif_wake_queue(net);
+
+	return 0;
+}
+
+static int mcp251x_stop(struct net_device *net)
+{
+	struct mcp251x_priv *priv = netdev_priv(net);
+	struct spi_device *spi = priv->spi;
+	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
+
+	dev_dbg(&spi->dev, "%s\n", __func__);
+
+	/* Disable and clear pending interrupts */
+	mcp251x_write_reg(spi, CANINTE, 0x00);
+	mcp251x_write_reg(spi, CANINTF, 0x00);
+
+	priv->force_quit = 1;
+	disable_irq(spi->irq);
+	flush_workqueue(priv->wq);
+
+	mcp251x_write_reg(spi, TXBCTRL(0), 0);
+	if (priv->tx_skb) {
+		net->stats.tx_errors++;
+		dev_kfree_skb(priv->tx_skb);
+		priv->tx_skb = NULL;
+	}
+
+	mcp251x_hw_sleep(spi);
+
+	if (pdata->transceiver_enable)
+		pdata->transceiver_enable(0);
+
+	return 0;
+}
+
+static int mcp251x_do_get_state(struct net_device *net, enum can_state	*state)
+{
+	struct mcp251x_priv *priv = netdev_priv(net);
+	struct spi_device *spi = priv->spi;
+	u8 eflag;
+
+	eflag = mcp251x_read_reg(spi, EFLG);
+
+	if (eflag & EFLG_TXBO)
+		*state = CAN_STATE_BUS_OFF;
+	else if (eflag & (EFLG_RXEP | EFLG_TXEP))
+		*state = CAN_STATE_BUS_PASSIVE;
+	else if (eflag & EFLG_EWARN)
+		*state = CAN_STATE_BUS_WARNING;
+	else
+		*state = CAN_STATE_ACTIVE;
+
+	return 0;
+}
+
+static void mcp251x_tx_work_handler(struct work_struct *ws)
+{
+	struct mcp251x_priv *priv = container_of(ws, struct mcp251x_priv,
+						 tx_work);
+	struct spi_device *spi = priv->spi;
+	struct can_frame *frame;
+
+	dev_dbg(&spi->dev, "%s\n", __func__);
+
+	if (priv->tx_skb) {
+		frame = (struct can_frame *)priv->tx_skb->data;
+		if (frame->can_dlc > CAN_FRAME_MAX_DATA_LEN)
+			frame->can_dlc = CAN_FRAME_MAX_DATA_LEN;
+		mcp251x_hw_tx(spi, frame, 0);
+	}
+}
+
+static void mcp251x_irq_work_handler(struct work_struct *ws)
+{
+	struct mcp251x_priv *priv = container_of(ws, struct mcp251x_priv,
+						 irq_work);
+	struct spi_device *spi = priv->spi;
+	struct net_device *net = priv->net;
+	u8 intf;
+	u8 txbnctrl;
+
+	if (priv->after_suspend) {
+		/* Wait whilst the device wakes up */
+		mdelay(10);
+		mcp251x_hw_reset(spi);
+		mcp251x_setup(net, priv, spi);
+		if (priv->after_suspend & AFTER_SUSPEND_UP) {
+			netif_device_attach(net);
+			/* clear since we lost tx buffer */
+			if (priv->tx_skb) {
+				net->stats.tx_errors++;
+				dev_kfree_skb(priv->tx_skb);
+				priv->tx_skb = NULL;
+				netif_wake_queue(net);
+			}
+			mcp251x_set_normal_mode(spi);
+		} else
+			mcp251x_hw_sleep(spi);
+		priv->after_suspend = 0;
+		return;
+	}
+
+	while (!priv->force_quit && !freezing(current)) {
+		if (priv->restart_tx) {
+			priv->restart_tx = 0;
+			dev_warn(&spi->dev,
+				 "timeout in txing a packet, restarting\n");
+			mcp251x_write_reg(spi, TXBCTRL(0), 0);
+			if (priv->tx_skb) {
+				net->stats.tx_errors++;
+				dev_kfree_skb(priv->tx_skb);
+				priv->tx_skb = NULL;
+			}
+			netif_wake_queue(net);
+		}
+
+		if (priv->wake) {
+			/* Wait whilst the device wakes up */
+			mdelay(10);
+			priv->wake = 0;
+		}
+
+		intf = mcp251x_read_reg(spi, CANINTF);
+		if (intf == 0x00)
+			break;
+		mcp251x_write_bits(spi, CANINTF, intf, 0x00);
+
+		dev_dbg(&spi->dev, "interrupt:%s%s%s%s%s%s%s%s\n",
+			(intf & CANINTF_MERRF) ? " MERR" : "",
+			(intf & CANINTF_WAKIF) ? " WAK" : "",
+			(intf & CANINTF_ERRIF) ? " ERR" : "",
+			(intf & CANINTF_TX2IF) ? " TX2" : "",
+			(intf & CANINTF_TX1IF) ? " TX1" : "",
+			(intf & CANINTF_TX0IF) ? " TX0" : "",
+			(intf & CANINTF_RX1IF) ? " RX1" : "",
+			(intf & CANINTF_RX0IF) ? " RX0" : "");
+
+		if (intf & CANINTF_WAKIF)
+			complete(&priv->awake);
+
+		if (intf & CANINTF_MERRF) {
+			/* if there are no pending Tx buffers, restart queue */
+			txbnctrl = mcp251x_read_reg(spi, TXBCTRL(0));
+			if (!(txbnctrl & TXBCTRL_TXREQ)) {
+				if (priv->tx_skb) {
+					net->stats.tx_errors++;
+					dev_kfree_skb(priv->tx_skb);
+					priv->tx_skb = NULL;
+				}
+				netif_wake_queue(net);
+			}
+		}
+
+		if (intf & CANINTF_ERRIF) {
+			struct sk_buff *skb;
+			struct can_frame *frame = NULL;
+			u8 eflag = mcp251x_read_reg(spi, EFLG);
+
+			dev_dbg(&spi->dev, "EFLG = 0x%02x\n", eflag);
+
+			/* Create error frame */
+			skb = dev_alloc_skb(sizeof(struct can_frame));
+			if (skb) {
+				frame = (struct can_frame *)
+					skb_put(skb, sizeof(struct can_frame));
+				*(unsigned long long *)&frame->data = 0ULL;
+				frame->can_id = CAN_ERR_FLAG;
+				frame->can_dlc = CAN_ERR_DLC;
+
+				skb->dev = net;
+				skb->protocol = __constant_htons(ETH_P_CAN);
+				skb->pkt_type = PACKET_BROADCAST;
+				skb->ip_summed = CHECKSUM_UNNECESSARY;
+
+				/* Set error frame flags based on bus state */
+				if (eflag & EFLG_TXBO) {
+					frame->can_id |= CAN_ERR_BUSOFF;
+				} else if (eflag & EFLG_TXEP) {
+					frame->can_id |= CAN_ERR_CRTL;
+					frame->data[1] |=
+					  CAN_ERR_CRTL_TX_PASSIVE;
+				} else if (eflag & EFLG_RXEP) {
+					frame->can_id |= CAN_ERR_CRTL;
+					frame->data[1] |=
+					  CAN_ERR_CRTL_RX_PASSIVE;
+				} else if (eflag & EFLG_TXWAR) {
+					frame->can_id |= CAN_ERR_CRTL;
+					frame->data[1] |=
+					  CAN_ERR_CRTL_TX_WARNING;
+				} else if (eflag & EFLG_RXWAR) {
+					frame->can_id |= CAN_ERR_CRTL;
+					frame->data[1] |=
+					  CAN_ERR_CRTL_RX_WARNING;
+				}
+			}
+
+			if (eflag & (EFLG_RX0OVR | EFLG_RX1OVR)) {
+				if (eflag & EFLG_RX0OVR)
+					net->stats.rx_over_errors++;
+				if (eflag & EFLG_RX1OVR)
+					net->stats.rx_over_errors++;
+				if (frame) {
+					frame->can_id |= CAN_ERR_CRTL;
+					frame->data[1] =
+					  CAN_ERR_CRTL_RX_OVERFLOW;
+				}
+			}
+			mcp251x_write_reg(spi, EFLG, 0x00);
+
+			if (skb)
+				netif_rx(skb);
+		}
+
+		if (intf & (CANINTF_TX2IF | CANINTF_TX1IF | CANINTF_TX0IF)) {
+			if (priv->tx_skb) {
+				net->stats.tx_packets++;
+				net->stats.tx_bytes +=
+					((struct can_frame *)
+					 (priv->tx_skb->data))->can_dlc;
+				dev_kfree_skb(priv->tx_skb);
+				priv->tx_skb = NULL;
+			}
+			netif_wake_queue(net);
+		}
+
+		if (intf & CANINTF_RX0IF)
+			mcp251x_hw_rx(spi, 0);
+
+		if (intf & CANINTF_RX1IF)
+			mcp251x_hw_rx(spi, 1);
+
+	}
+
+	mcp251x_read_reg(spi, CANSTAT);
+
+	dev_dbg(&spi->dev, "interrupt ended\n");
+}
+
+static irqreturn_t mcp251x_can_isr(int irq, void *dev_id)
+{
+	struct net_device *net = (struct net_device *)dev_id;
+	struct mcp251x_priv *priv = netdev_priv(net);
+
+	dev_dbg(&priv->spi->dev, "%s: irq\n", __func__);
+	/* Schedule bottom half */
+	if (!work_pending(&priv->irq_work))
+		queue_work(priv->wq, &priv->irq_work);
+
+	return IRQ_HANDLED;
+}
+
+static void mcp251x_tx_timeout(struct net_device *net)
+{
+	struct mcp251x_priv *priv = netdev_priv(net);
+
+	priv->restart_tx = 1;
+	queue_work(priv->wq, &priv->irq_work);
+}
+
+static struct net_device *alloc_mcp251x_netdev(int sizeof_priv)
+{
+	struct net_device *net;
+	struct mcp251x_priv *priv;
+
+	net = alloc_candev(sizeof_priv);
+	if (!net)
+		return NULL;
+
+	priv = netdev_priv(net);
+
+	net->open		= mcp251x_open;
+	net->stop		= mcp251x_stop;
+	net->hard_start_xmit	= mcp251x_hard_start_xmit;
+	net->tx_timeout		= mcp251x_tx_timeout;
+	net->watchdog_timeo	= HZ;
+
+	priv->can.bittiming_const = &mcp251x_bittiming_const;
+	priv->can.do_get_state	  = mcp251x_do_get_state;
+	priv->can.do_set_mode	  = mcp251x_do_set_mode;
+
+	priv->net = net;
+
+	return net;
+}
+
+static int __devinit mcp251x_can_probe(struct spi_device *spi)
+{
+	struct net_device *net;
+	struct mcp251x_priv *priv;
+	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
+	int ret = -ENODEV;
+
+	if (!pdata) {
+		/* Platform data is required for osc freq */
+		goto error_out;
+	}
+
+	/* Allocate can/net device */
+	net = alloc_mcp251x_netdev(sizeof(struct mcp251x_priv));
+	if (!net) {
+		ret = -ENOMEM;
+		goto error_alloc;
+	}
+
+	priv = netdev_priv(net);
+	dev_set_drvdata(&spi->dev, priv);
+
+	priv->spi = spi;
+	mutex_init(&priv->spi_lock);
+
+	priv->can.bittiming.clock = pdata->oscillator_frequency / 2;
+
+	/* If requested, allocate DMA buffers */
+	if (mcp251x_enable_dma) {
+		spi->dev.coherent_dma_mask = DMA_32BIT_MASK;
+
+		/* Minimum coherent DMA allocation is PAGE_SIZE, so allocate
+		   that much and share it between Tx and Rx DMA buffers. */
+		priv->spi_tx_buf = dma_alloc_coherent(&spi->dev,
+			PAGE_SIZE, &priv->spi_tx_dma, GFP_DMA);
+
+		if (priv->spi_tx_buf) {
+			priv->spi_rx_buf = (u8 *)(priv->spi_tx_buf +
+				(PAGE_SIZE / 2));
+			priv->spi_rx_dma = (dma_addr_t)(priv->spi_tx_dma +
+				(PAGE_SIZE / 2));
+		} else {
+			/* Fall back to non-DMA */
+			mcp251x_enable_dma = 0;
+		}
+	}
+
+	/* Allocate non-DMA buffers */
+	if (!mcp251x_enable_dma) {
+		priv->spi_tx_buf = kmalloc(SPI_TRANSFER_BUF_LEN, GFP_KERNEL);
+		if (!priv->spi_tx_buf) {
+			ret = -ENOMEM;
+			goto error_tx_buf;
+		}
+		priv->spi_rx_buf = kmalloc(SPI_TRANSFER_BUF_LEN, GFP_KERNEL);
+		if (!priv->spi_tx_buf) {
+			ret = -ENOMEM;
+			goto error_rx_buf;
+		}
+	}
+
+	if (pdata->power_enable)
+		pdata->power_enable(1);
+
+	/* Call out to platform specific setup */
+	if (pdata->board_specific_setup)
+		pdata->board_specific_setup(spi);
+
+	SET_NETDEV_DEV(net, &spi->dev);
+
+	priv->wq = create_freezeable_workqueue("mcp251x_wq");
+
+	INIT_WORK(&priv->tx_work, mcp251x_tx_work_handler);
+	INIT_WORK(&priv->irq_work, mcp251x_irq_work_handler);
+
+	init_completion(&priv->awake);
+
+	/* Configure the SPI bus */
+	spi->mode = SPI_MODE_0;
+	spi->bits_per_word = 8;
+	spi_setup(spi);
+
+	/* Register IRQ */
+	if (request_irq(spi->irq, mcp251x_can_isr,
+			IRQF_TRIGGER_FALLING, DEVICE_NAME, net) < 0) {
+		dev_err(&spi->dev, "failed to acquire irq %d\n", spi->irq);
+		goto error_irq;
+	}
+	disable_irq(spi->irq);
+
+	if (!mcp251x_hw_probe(spi)) {
+		dev_info(&spi->dev, "Probe failed\n");
+		goto error_probe;
+	}
+	mcp251x_hw_sleep(spi);
+
+	if (pdata->transceiver_enable)
+		pdata->transceiver_enable(0);
+
+	ret = register_candev(net);
+	if (ret >= 0) {
+		dev_info(&spi->dev, "probed\n");
+		return ret;
+	}
+error_probe:
+	free_irq(spi->irq, net);
+error_irq:
+	if (!mcp251x_enable_dma)
+		kfree(priv->spi_rx_buf);
+error_rx_buf:
+	if (!mcp251x_enable_dma)
+		kfree(priv->spi_tx_buf);
+error_tx_buf:
+	free_candev(net);
+	if (mcp251x_enable_dma) {
+		dma_free_coherent(&spi->dev, PAGE_SIZE,
+			priv->spi_tx_buf, priv->spi_tx_dma);
+	}
+error_alloc:
+	dev_err(&spi->dev, "probe failed\n");
+error_out:
+	return ret;
+}
+
+static int __devexit mcp251x_can_remove(struct spi_device *spi)
+{
+	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct net_device *net = priv->net;
+
+	unregister_candev(net);
+	free_candev(net);
+
+	free_irq(spi->irq, net);
+	priv->force_quit = 1;
+	flush_workqueue(priv->wq);
+	destroy_workqueue(priv->wq);
+
+	if (mcp251x_enable_dma) {
+		dma_free_coherent(&spi->dev, PAGE_SIZE,
+			priv->spi_tx_buf, priv->spi_tx_dma);
+	} else {
+		kfree(priv->spi_tx_buf);
+		kfree(priv->spi_rx_buf);
+	}
+
+	if (pdata->power_enable)
+		pdata->power_enable(0);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int mcp251x_can_suspend(struct spi_device *spi, pm_message_t state)
+{
+	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct net_device *net = priv->net;
+
+	if (netif_running(net)) {
+		netif_device_detach(net);
+
+		mcp251x_hw_sleep(spi);
+		if (pdata->transceiver_enable)
+			pdata->transceiver_enable(0);
+		priv->after_suspend = AFTER_SUSPEND_UP;
+	} else
+		priv->after_suspend = AFTER_SUSPEND_DOWN;
+
+	if (pdata->power_enable) {
+		pdata->power_enable(0);
+		priv->after_suspend |= AFTER_SUSPEND_POWER;
+	}
+
+	return 0;
+}
+
+static int mcp251x_can_resume(struct spi_device *spi)
+{
+	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
+	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+
+	if (priv->after_suspend & AFTER_SUSPEND_POWER) {
+		pdata->power_enable(1);
+		queue_work(priv->wq, &priv->irq_work);
+	} else {
+		if (priv->after_suspend & AFTER_SUSPEND_UP) {
+			if (pdata->transceiver_enable)
+				pdata->transceiver_enable(1);
+			queue_work(priv->wq, &priv->irq_work);
+		} else
+			priv->after_suspend = 0;
+	}
+	return 0;
+}
+#else
+#define mcp251x_can_suspend NULL
+#define mcp251x_can_resume NULL
+#endif
+
+static struct spi_driver mcp251x_can_driver = {
+	.driver = {
+		.name		= DEVICE_NAME,
+		.bus		= &spi_bus_type,
+		.owner		= THIS_MODULE,
+	},
+
+	.probe		= mcp251x_can_probe,
+	.remove		= __devexit_p(mcp251x_can_remove),
+	.suspend	= mcp251x_can_suspend,
+	.resume		= mcp251x_can_resume,
+};
+
+static int __init mcp251x_can_init(void)
+{
+	return spi_register_driver(&mcp251x_can_driver);
+}
+
+static void __exit mcp251x_can_exit(void)
+{
+	spi_unregister_driver(&mcp251x_can_driver);
+}
+
+module_init(mcp251x_can_init);
+module_exit(mcp251x_can_exit);
+
+MODULE_AUTHOR("Chris Elston <celston@katalix.com>, "
+	      "Christian Pellegrin <chripell@evolware.org>");
+MODULE_DESCRIPTION("Microchip 251x CAN driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/can/mscan/Makefile b/drivers/net/can/mscan/Makefile
new file mode 100644
index 000000000000..9f61b13db90e
--- /dev/null
+++ b/drivers/net/can/mscan/Makefile
@@ -0,0 +1,23 @@
+#
+#
+
+ifeq ($(KERNELRELEASE),)
+
+KERNELDIR := /lib/modules/$(shell uname -r)/build
+PWD       := $(shell pwd)
+TOPDIR    := $(PWD)/../../../..
+
+modules modules_install clean:
+	$(MAKE) -C $(KERNELDIR) M=$(PWD) $@ TOPDIR=$(TOPDIR)
+
+else
+
+-include $(TOPDIR)/Makefile.common
+
+obj-$(CONFIG_CAN_MPC52XX)	+= mscan-mpc52xx.o
+
+mscan-mpc52xx-objs	:= mscan.o mpc52xx_can.o
+
+ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
+
+endif
diff --git a/drivers/net/can/mscan/mpc52xx_can.c b/drivers/net/can/mscan/mpc52xx_can.c
new file mode 100644
index 000000000000..8878a5901aa5
--- /dev/null
+++ b/drivers/net/can/mscan/mpc52xx_can.c
@@ -0,0 +1,293 @@
+/*
+ * CAN bus driver for the Freescale MPC52xx embedded CPU.
+ *
+ * Copyright (C) 2004-2005 Andrey Volkov <avolkov@varma-el.com>,
+ *                         Varma Electronics Oy
+ * Copyright (C) 2008-2009 Wolfgang Grandegger <wg@grandegger.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/netdevice.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/of_platform.h>
+#include <sysdev/fsl_soc.h>
+#include <linux/io.h>
+#include <asm/mpc52xx.h>
+
+#include "mscan.h"
+
+
+#define DRV_NAME "mpc52xx_can"
+
+static struct of_device_id mpc52xx_cdm_ids[] __devinitdata = {
+	{ .compatible = "fsl,mpc5200-cdm", },
+	{ .compatible = "fsl,mpc5200b-cdm", },
+	{}
+};
+
+/*
+ * Get the frequency of the external oscillator clock connected
+ * to the SYS_XTAL_IN pin, or retrun 0 if it cannot be determined.
+ */
+static unsigned int  __devinit mpc52xx_can_xtal_freq(struct device_node *np)
+{
+	struct mpc52xx_cdm  __iomem *cdm;
+	struct device_node *np_cdm;
+	unsigned int freq;
+	u32 val;
+
+	freq = mpc52xx_find_ipb_freq(np);
+	if (!freq)
+		return 0;
+
+	/*
+	 * Detemine SYS_XTAL_IN frequency from the clock domain settings
+	 */
+	np_cdm = of_find_matching_node(NULL, mpc52xx_cdm_ids);
+	cdm = of_iomap(np_cdm, 0);
+	of_node_put(np_cdm);
+	if (!np_cdm) {
+		printk(KERN_ERR "%s() failed abnormally\n", __func__);
+		return 0;
+	}
+
+	if (in_8(&cdm->ipb_clk_sel) & 0x1)
+		freq *= 2;
+	val  = in_be32(&cdm->rstcfg);
+	if (val & (1 << 5))
+		freq *= 8;
+	else
+		freq *= 4;
+	if (val & (1 << 6))
+		freq /= 12;
+	else
+		freq /= 16;
+
+	iounmap(cdm);
+
+	return freq;
+}
+
+/*
+ * Get frequency of the MSCAN clock source
+ *
+ * Either the oscillator clock (SYS_XTAL_IN) or the IP bus clock (IP_CLK)
+ * can be selected. According to the MPC5200 user's manual, the oscillator
+ * clock is the better choice as it has less jitter but due to a hardware
+ * bug, it can not be selected for the old MPC5200 Rev. A chips.
+ */
+
+static unsigned int  __devinit mpc52xx_can_clock_freq(struct device_node *np,
+						      int clock_src)
+{
+	unsigned int pvr;
+
+	pvr = mfspr(SPRN_PVR);
+
+	if (clock_src == MSCAN_CLKSRC_BUS || pvr == 0x80822011)
+		return mpc52xx_find_ipb_freq(np);
+
+	return mpc52xx_can_xtal_freq(np);
+}
+
+static int __devinit mpc52xx_can_probe(struct of_device *ofdev,
+				       const struct of_device_id *id)
+{
+	struct device_node *np = ofdev->node;
+	struct net_device *dev;
+	struct can_priv *priv;
+	struct resource res;
+	void __iomem *base;
+	int err, irq, res_size, clock_src;
+
+	err = of_address_to_resource(np, 0, &res);
+	if (err) {
+		dev_err(&ofdev->dev, "invalid address\n");
+		return err;
+	}
+
+	res_size = res.end - res.start + 1;
+
+	if (!request_mem_region(res.start, res_size, DRV_NAME)) {
+		dev_err(&ofdev->dev, "couldn't request %#x..%#x\n",
+			res.start, res.end);
+		return -EBUSY;
+	}
+
+	base = ioremap_nocache(res.start, res_size);
+	if (!base) {
+		dev_err(&ofdev->dev, "couldn't ioremap %#x..%#x\n",
+			res.start, res.end);
+		err = -ENOMEM;
+		goto exit_release_mem;
+	}
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (irq == NO_IRQ) {
+		dev_err(&ofdev->dev, "no irq found\n");
+		err = -ENODEV;
+		goto exit_unmap_mem;
+	}
+
+	dev = alloc_mscandev();
+	if (!dev) {
+		err = -ENOMEM;
+		goto exit_dispose_irq;
+	}
+
+	dev->base_addr = (unsigned long)base;
+	dev->irq = irq;
+
+	priv = netdev_priv(dev);
+
+	/*
+	 * Either the oscillator clock (SYS_XTAL_IN) or the IP bus clock
+	 * (IP_CLK) can be selected as MSCAN clock source. According to
+	 * the MPC5200 user's manual, the oscillator clock is the better
+	 * choice as it has less jitter. For this reason, it is selected
+	 * by default.
+	 */
+	if (of_get_property(np, "clock-ipb", NULL))
+		clock_src = MSCAN_CLKSRC_BUS;
+	else
+		clock_src = MSCAN_CLKSRC_XTAL;
+	priv->bittiming.clock = mpc52xx_can_clock_freq(np, clock_src);
+	if (!priv->bittiming.clock) {
+		dev_err(&ofdev->dev, "couldn't get MSCAN clock frequency\n");
+		err = -ENODEV;
+		goto exit_free_mscan;
+	}
+
+	SET_NETDEV_DEV(dev, &ofdev->dev);
+
+	err = register_mscandev(dev, clock_src);
+	if (err) {
+		dev_err(&ofdev->dev, "registering %s failed (err=%d)\n",
+			DRV_NAME, err);
+		goto exit_free_mscan;
+	}
+
+	dev_set_drvdata(&ofdev->dev, dev);
+
+	dev_info(&ofdev->dev, "MSCAN at 0x%lx, irq %d, clock %d Hz\n",
+		 dev->base_addr, dev->irq, priv->bittiming.clock);
+
+	return 0;
+
+exit_free_mscan:
+	free_candev(dev);
+exit_dispose_irq:
+	irq_dispose_mapping(irq);
+exit_unmap_mem:
+	iounmap(base);
+exit_release_mem:
+	release_mem_region(res.start, res_size);
+
+	return err;
+}
+
+static int __devexit mpc52xx_can_remove(struct of_device *ofdev)
+{
+	struct net_device *dev = dev_get_drvdata(&ofdev->dev);
+	struct device_node *np = ofdev->node;
+	struct resource res;
+
+	dev_set_drvdata(&ofdev->dev, NULL);
+
+	unregister_mscandev(dev);
+	iounmap((void __iomem *)dev->base_addr);
+	irq_dispose_mapping(dev->irq);
+	free_candev(dev);
+
+	of_address_to_resource(np, 0, &res);
+	release_mem_region(res.start, res.end - res.start + 1);
+
+	return 0;
+}
+
+static struct mscan_regs saved_regs;
+static int mpc52xx_can_suspend(struct of_device *ofdev, pm_message_t state)
+{
+	struct net_device *dev = dev_get_drvdata(&ofdev->dev);
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+
+	_memcpy_fromio(&saved_regs, regs, sizeof(*regs));
+
+	return 0;
+}
+
+static int mpc52xx_can_resume(struct of_device *ofdev)
+{
+	struct net_device *dev = dev_get_drvdata(&ofdev->dev);
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+
+	regs->canctl0 |= MSCAN_INITRQ;
+	while ((regs->canctl1 & MSCAN_INITAK) == 0)
+		udelay(10);
+
+	regs->canctl1 = saved_regs.canctl1;
+	regs->canbtr0 = saved_regs.canbtr0;
+	regs->canbtr1 = saved_regs.canbtr1;
+	regs->canidac = saved_regs.canidac;
+
+	/* restore masks, buffers etc. */
+	_memcpy_toio(&regs->canidar1_0, (void *)&saved_regs.canidar1_0,
+		     sizeof(*regs) - offsetof(struct mscan_regs, canidar1_0));
+
+	regs->canctl0 &= ~MSCAN_INITRQ;
+	regs->cantbsel = saved_regs.cantbsel;
+	regs->canrier = saved_regs.canrier;
+	regs->cantier = saved_regs.cantier;
+	regs->canctl0 = saved_regs.canctl0;
+
+	return 0;
+}
+
+static struct of_device_id __devinitdata mpc52xx_can_table[] = {
+	{.compatible = "fsl,mpc5200-mscan"},
+	{.compatible = "fsl,mpc5200b-mscan"},
+	{},
+};
+
+static struct of_platform_driver mpc52xx_can_driver = {
+	.owner = THIS_MODULE,
+	.name = "mpc52xx_can",
+	.probe = mpc52xx_can_probe,
+	.remove = __devexit_p(mpc52xx_can_remove),
+	.suspend = mpc52xx_can_suspend,
+	.resume = mpc52xx_can_resume,
+	.match_table = mpc52xx_can_table,
+};
+
+static int __init mpc52xx_can_init(void)
+{
+	return of_register_platform_driver(&mpc52xx_can_driver);
+}
+module_init(mpc52xx_can_init);
+
+static void __exit mpc52xx_can_exit(void)
+{
+	return of_unregister_platform_driver(&mpc52xx_can_driver);
+};
+module_exit(mpc52xx_can_exit);
+
+MODULE_AUTHOR("Wolfgang Grandegger <wg@grandegger.com>");
+MODULE_DESCRIPTION("Freescale MPC5200 CAN driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/can/mscan/mscan.c b/drivers/net/can/mscan/mscan.c
new file mode 100644
index 000000000000..fe42b9e7dfb7
--- /dev/null
+++ b/drivers/net/can/mscan/mscan.c
@@ -0,0 +1,679 @@
+/*
+ * CAN bus driver for the alone generic (as possible as) MSCAN controller.
+ *
+ * Copyright (C) 2005-2006 Andrey Volkov <avolkov@varma-el.com>,
+ *                         Varma Electronics Oy
+ * Copyright (C) 2008-2009 Wolfgang Grandegger <wg@grandegger.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/list.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/can/error.h>
+#include <linux/io.h>
+
+#include "mscan.h"
+
+#define MSCAN_NORMAL_MODE	0
+#define MSCAN_SLEEP_MODE	MSCAN_SLPRQ
+#define MSCAN_INIT_MODE		(MSCAN_INITRQ | MSCAN_SLPRQ)
+#define MSCAN_POWEROFF_MODE	(MSCAN_CSWAI | MSCAN_SLPRQ)
+#define MSCAN_SET_MODE_RETRIES	255
+
+#define BTR0_BRP_MASK		0x3f
+#define BTR0_SJW_SHIFT		6
+#define BTR0_SJW_MASK		(0x3 << BTR0_SJW_SHIFT)
+
+#define BTR1_TSEG1_MASK 	0xf
+#define BTR1_TSEG2_SHIFT	4
+#define BTR1_TSEG2_MASK 	(0x7 << BTR1_TSEG2_SHIFT)
+#define BTR1_SAM_SHIFT  	7
+
+#define BTR0_SET_BRP(brp)	(((brp) - 1) & BTR0_BRP_MASK)
+#define BTR0_SET_SJW(sjw)	((((sjw) - 1) << BTR0_SJW_SHIFT) & \
+				 BTR0_SJW_MASK)
+
+#define BTR1_SET_TSEG1(tseg1)	(((tseg1) - 1) &  BTR1_TSEG1_MASK)
+#define BTR1_SET_TSEG2(tseg2)	((((tseg2) - 1) << BTR1_TSEG2_SHIFT) & \
+				 BTR1_TSEG2_MASK)
+#define BTR1_SET_SAM(sam)	(((sam) & 1) << BTR1_SAM_SHIFT)
+
+static struct can_bittiming_const mscan_bittiming_const = {
+	.tseg1_min = 4,
+	.tseg1_max = 16,
+	.tseg2_min = 2,
+	.tseg2_max = 8,
+	.sjw_max = 4,
+	.brp_min = 1,
+	.brp_max = 64,
+	.brp_inc = 1,
+};
+
+struct mscan_state {
+	u8 mode;
+	u8 canrier;
+	u8 cantier;
+};
+
+#define TX_QUEUE_SIZE	3
+
+struct tx_queue_entry {
+	struct list_head list;
+	u8 mask;
+	u8 id;
+};
+
+struct mscan_priv {
+	struct can_priv can;
+	long open_time;
+	unsigned long flags;
+	u8 shadow_statflg;
+	u8 shadow_canrier;
+	u8 cur_pri;
+	u8 tx_active;
+
+	struct list_head tx_head;
+	struct tx_queue_entry tx_queue[TX_QUEUE_SIZE];
+	struct napi_struct napi;
+	struct net_device *dev;
+};
+
+#define F_RX_PROGRESS	0
+#define F_TX_PROGRESS	1
+#define F_TX_WAIT_ALL	2
+
+static enum can_state state_map[] = {
+	CAN_STATE_ACTIVE,
+	CAN_STATE_BUS_WARNING,
+	CAN_STATE_BUS_PASSIVE,
+	CAN_STATE_BUS_OFF
+};
+
+static int mscan_set_mode(struct net_device *dev, u8 mode)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	struct mscan_priv *priv = netdev_priv(dev);
+	int ret = 0;
+	int i;
+	u8 canctl1;
+
+	if (mode != MSCAN_NORMAL_MODE) {
+
+		if (priv->tx_active) {
+			/* Abort transfers before going to sleep */
+			out_8(&regs->cantier, 0);
+			out_8(&regs->cantarq, priv->tx_active);
+			out_8(&regs->cantier, priv->tx_active);
+		}
+
+		canctl1 = in_8(&regs->canctl1);
+		if ((mode & MSCAN_SLPRQ) && (canctl1 & MSCAN_SLPAK) == 0) {
+			out_8(&regs->canctl0,
+			      in_8(&regs->canctl0) | MSCAN_SLPRQ);
+			for (i = 0; i < MSCAN_SET_MODE_RETRIES; i++) {
+				if (in_8(&regs->canctl1) & MSCAN_SLPAK)
+					break;
+				udelay(100);
+			}
+			if (i >= MSCAN_SET_MODE_RETRIES)
+				ret = -ENODEV;
+		}
+		if (!ret)
+			priv->can.state = CAN_STATE_SLEEPING;
+
+		if (!ret && (mode & MSCAN_INITRQ)
+		    && (canctl1 & MSCAN_INITAK) == 0) {
+			out_8(&regs->canctl0,
+			      in_8(&regs->canctl0) | MSCAN_INITRQ);
+			for (i = 0; i < MSCAN_SET_MODE_RETRIES; i++) {
+				if (in_8(&regs->canctl1) & MSCAN_INITAK)
+					break;
+			}
+			if (i >= MSCAN_SET_MODE_RETRIES)
+				ret = -ENODEV;
+		}
+		if (!ret)
+			priv->can.state = CAN_STATE_STOPPED;
+
+		if (!ret && (mode & MSCAN_CSWAI))
+			out_8(&regs->canctl0,
+			      in_8(&regs->canctl0) | MSCAN_CSWAI);
+
+	} else {
+		canctl1 = in_8(&regs->canctl1);
+		if (canctl1 & (MSCAN_SLPAK | MSCAN_INITAK)) {
+			out_8(&regs->canctl0, in_8(&regs->canctl0) &
+			      ~(MSCAN_SLPRQ | MSCAN_INITRQ));
+			for (i = 0; i < MSCAN_SET_MODE_RETRIES; i++) {
+				canctl1 = in_8(&regs->canctl1);
+				if (!(canctl1 & (MSCAN_INITAK | MSCAN_SLPAK)))
+					break;
+			}
+			if (i >= MSCAN_SET_MODE_RETRIES)
+				ret = -ENODEV;
+			else
+				priv->can.state = CAN_STATE_ACTIVE;
+		}
+	}
+	return ret;
+}
+
+static int mscan_start(struct net_device *dev)
+{
+	struct mscan_priv *priv = netdev_priv(dev);
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	u8 canrflg;
+	int err;
+
+	out_8(&regs->canrier, 0);
+
+	INIT_LIST_HEAD(&priv->tx_head);
+	priv->cur_pri = 0;
+	priv->tx_active = 0;
+	priv->shadow_canrier = 0;
+	priv->flags = 0;
+
+	err = mscan_set_mode(dev, MSCAN_NORMAL_MODE);
+	if (err)
+		return err;
+
+	canrflg = in_8(&regs->canrflg);
+	priv->shadow_statflg = canrflg & MSCAN_STAT_MSK;
+	priv->can.state = state_map[max(MSCAN_STATE_RX(canrflg),
+				    MSCAN_STATE_TX(canrflg))];
+	out_8(&regs->cantier, 0);
+
+	/* Enable receive interrupts. */
+	out_8(&regs->canrier, MSCAN_OVRIE | MSCAN_RXFIE | MSCAN_CSCIE |
+	      MSCAN_RSTATE1 | MSCAN_RSTATE0 | MSCAN_TSTATE1 | MSCAN_TSTATE0);
+
+	return 0;
+}
+
+static int mscan_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct can_frame *frame = (struct can_frame *)skb->data;
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	struct mscan_priv *priv = netdev_priv(dev);
+	int i, rtr, buf_id;
+	u32 can_id;
+
+	if (frame->can_dlc > 8)
+		return -EINVAL;
+
+	out_8(&regs->cantier, 0);
+
+	i = ~priv->tx_active & MSCAN_TXE;
+	buf_id = ffs(i) - 1;
+	switch (hweight8(i)) {
+	case 0:
+		netif_stop_queue(dev);
+		dev_err(ND2D(dev), "BUG! Tx Ring full when queue awake!\n");
+		return NETDEV_TX_BUSY;
+	case 1:
+		/* if buf_id < 3, then current frame will be send out of order,
+		   since  buffer with lower id have higher priority (hell..) */
+		if (buf_id < 3)
+			priv->cur_pri++;
+		if (priv->cur_pri == 0xff)
+			set_bit(F_TX_WAIT_ALL, &priv->flags);
+		netif_stop_queue(dev);
+	case 2:
+		set_bit(F_TX_PROGRESS, &priv->flags);
+	}
+	out_8(&regs->cantbsel, i);
+
+	rtr = frame->can_id & CAN_RTR_FLAG;
+
+	if (frame->can_id & CAN_EFF_FLAG) {
+		can_id = (frame->can_id & CAN_EFF_MASK) << 1;
+		if (rtr)
+			can_id |= 1;
+		out_be16(&regs->tx.idr3_2, can_id);
+
+		can_id >>= 16;
+		can_id = (can_id & 0x7) | ((can_id << 2) & 0xffe0) | (3 << 3);
+	} else {
+		can_id = (frame->can_id & CAN_SFF_MASK) << 5;
+		if (rtr)
+			can_id |= 1 << 4;
+	}
+	out_be16(&regs->tx.idr1_0, can_id);
+
+	if (!rtr) {
+		void __iomem *data = &regs->tx.dsr1_0;
+		u16 *payload = (u16 *) frame->data;
+		/*Its safe to write into dsr[dlc+1] */
+		for (i = 0; i < (frame->can_dlc + 1) / 2; i++) {
+			out_be16(data, *payload++);
+			data += 2 + _MSCAN_RESERVED_DSR_SIZE;
+		}
+	}
+
+	out_8(&regs->tx.dlr, frame->can_dlc);
+	out_8(&regs->tx.tbpr, priv->cur_pri);
+
+	/* Start transmission. */
+	out_8(&regs->cantflg, 1 << buf_id);
+
+	if (!test_bit(F_TX_PROGRESS, &priv->flags))
+		dev->trans_start = jiffies;
+
+	list_add_tail(&priv->tx_queue[buf_id].list, &priv->tx_head);
+
+	can_put_echo_skb(skb, dev, buf_id);
+
+	/* Enable interrupt. */
+	priv->tx_active |= 1 << buf_id;
+	out_8(&regs->cantier, priv->tx_active);
+
+	return NETDEV_TX_OK;
+}
+
+static inline int check_set_state(struct net_device *dev, u8 canrflg)
+{
+	struct mscan_priv *priv = netdev_priv(dev);
+	enum can_state state;
+	int ret = 0;
+
+	if (!(canrflg & MSCAN_CSCIF) || priv->can.state > CAN_STATE_BUS_OFF)
+		return 0;
+
+	state = state_map[max(MSCAN_STATE_RX(canrflg),
+			      MSCAN_STATE_TX(canrflg))];
+	if (priv->can.state < state)
+		ret = 1;
+	if (state == CAN_STATE_BUS_OFF)
+		can_bus_off(dev);
+	priv->can.state = state;
+	return ret;
+}
+
+static int mscan_rx_poll(struct napi_struct *napi, int quota)
+{
+	struct mscan_priv *priv = container_of(napi, struct mscan_priv, napi);
+	struct net_device *dev = priv->dev;
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	struct net_device_stats *stats = &dev->stats;
+	int npackets = 0;
+	int ret = 1;
+	struct sk_buff *skb;
+	struct can_frame *frame;
+	u32 can_id;
+	u8 canrflg;
+	int i;
+
+	while (npackets < quota && ((canrflg = in_8(&regs->canrflg)) &
+				    (MSCAN_RXF | MSCAN_ERR_IF))) {
+
+		skb = dev_alloc_skb(sizeof(struct can_frame));
+		if (!skb) {
+			if (printk_ratelimit())
+				dev_notice(ND2D(dev), "packet dropped\n");
+			stats->rx_dropped++;
+			out_8(&regs->canrflg, canrflg);
+			continue;
+		}
+
+		frame = (struct can_frame *)skb_put(skb, sizeof(*frame));
+		memset(frame, 0, sizeof(*frame));
+
+		if (canrflg & MSCAN_RXF) {
+			can_id = in_be16(&regs->rx.idr1_0);
+			if (can_id & (1 << 3)) {
+				frame->can_id = CAN_EFF_FLAG;
+				can_id = ((can_id << 16) |
+					  in_be16(&regs->rx.idr3_2));
+				can_id = ((can_id & 0xffe00000) |
+					  ((can_id & 0x7ffff) << 2)) >> 2;
+			} else {
+				can_id >>= 4;
+				frame->can_id = 0;
+			}
+
+			frame->can_id |= can_id >> 1;
+			if (can_id & 1)
+				frame->can_id |= CAN_RTR_FLAG;
+			frame->can_dlc = in_8(&regs->rx.dlr) & 0xf;
+
+			if (!(frame->can_id & CAN_RTR_FLAG)) {
+				void __iomem *data = &regs->rx.dsr1_0;
+				u16 *payload = (u16 *) frame->data;
+				for (i = 0; i < (frame->can_dlc + 1) / 2; i++) {
+					*payload++ = in_be16(data);
+					data += 2 + _MSCAN_RESERVED_DSR_SIZE;
+				}
+			}
+
+			out_8(&regs->canrflg, MSCAN_RXF);
+			dev->last_rx = jiffies;
+			stats->rx_packets++;
+			stats->rx_bytes += frame->can_dlc;
+		} else if (canrflg & MSCAN_ERR_IF) {
+			dev_dbg(ND2D(dev), "error interrupt (canrflg=%#x)\n",
+				canrflg);
+			frame->can_id = CAN_ERR_FLAG;
+
+			if (canrflg & MSCAN_OVRIF) {
+				frame->can_id |= CAN_ERR_CRTL;
+				frame->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+				stats->rx_over_errors++;
+			} else
+				frame->data[1] = 0;
+
+			if (check_set_state(dev, canrflg)) {
+				frame->can_id |= CAN_ERR_CRTL;
+				switch (priv->can.state) {
+				case CAN_STATE_BUS_WARNING:
+					if ((priv->shadow_statflg &
+					     MSCAN_RSTAT_MSK) <
+					    (canrflg & MSCAN_RSTAT_MSK))
+						frame->data[1] |=
+						    CAN_ERR_CRTL_RX_WARNING;
+
+					if ((priv->shadow_statflg &
+					     MSCAN_TSTAT_MSK) <
+					    (canrflg & MSCAN_TSTAT_MSK))
+						frame->data[1] |=
+							CAN_ERR_CRTL_TX_WARNING;
+					break;
+				case CAN_STATE_BUS_PASSIVE:
+					frame->data[1] |=
+					    CAN_ERR_CRTL_RX_PASSIVE;
+					break;
+				case CAN_STATE_BUS_OFF:
+					frame->can_id |= CAN_ERR_BUSOFF;
+					frame->can_id &= ~CAN_ERR_CRTL;
+					break;
+				default:
+					break;
+				}
+			}
+			priv->shadow_statflg = canrflg & MSCAN_STAT_MSK;
+			frame->can_dlc = CAN_ERR_DLC;
+			out_8(&regs->canrflg, MSCAN_ERR_IF);
+		}
+
+		npackets++;
+		skb->dev = dev;
+		skb->protocol = __constant_htons(ETH_P_CAN);
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+		netif_receive_skb(skb);
+	}
+
+	if (!(in_8(&regs->canrflg) & (MSCAN_RXF | MSCAN_ERR_IF))) {
+		netif_rx_complete(dev, &priv->napi);
+		clear_bit(F_RX_PROGRESS, &priv->flags);
+		if (priv->can.state < CAN_STATE_BUS_OFF)
+			out_8(&regs->canrier, priv->shadow_canrier);
+		ret = 0;
+	}
+	return ret;
+}
+
+static irqreturn_t mscan_isr(int irq, void *dev_id)
+{
+	struct net_device *dev = (struct net_device *)dev_id;
+	struct mscan_priv *priv = netdev_priv(dev);
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	struct net_device_stats *stats = &dev->stats;
+	u8 cantier, cantflg, canrflg;
+	irqreturn_t ret = IRQ_NONE;
+
+	cantier = in_8(&regs->cantier) & MSCAN_TXE;
+	cantflg = in_8(&regs->cantflg) & cantier;
+
+	if (cantier && cantflg) {
+
+		struct list_head *tmp, *pos;
+
+		list_for_each_safe(pos, tmp, &priv->tx_head) {
+			struct tx_queue_entry *entry =
+			    list_entry(pos, struct tx_queue_entry, list);
+			u8 mask = entry->mask;
+
+			if (!(cantflg & mask))
+				continue;
+
+			out_8(&regs->cantbsel, mask);
+			stats->tx_bytes += in_8(&regs->tx.dlr);
+			stats->tx_packets++;
+			can_get_echo_skb(dev, entry->id);
+			priv->tx_active &= ~mask;
+			list_del(pos);
+		}
+
+		if (list_empty(&priv->tx_head)) {
+			clear_bit(F_TX_WAIT_ALL, &priv->flags);
+			clear_bit(F_TX_PROGRESS, &priv->flags);
+			priv->cur_pri = 0;
+		} else
+			dev->trans_start = jiffies;
+
+		if (!test_bit(F_TX_WAIT_ALL, &priv->flags))
+			netif_wake_queue(dev);
+
+		out_8(&regs->cantier, priv->tx_active);
+		ret = IRQ_HANDLED;
+	}
+
+	canrflg = in_8(&regs->canrflg);
+	if ((canrflg & ~MSCAN_STAT_MSK) &&
+	    !test_and_set_bit(F_RX_PROGRESS, &priv->flags)) {
+		if (canrflg & ~MSCAN_STAT_MSK) {
+			priv->shadow_canrier = in_8(&regs->canrier);
+			out_8(&regs->canrier, 0);
+			netif_rx_schedule(dev, &priv->napi);
+			ret = IRQ_HANDLED;
+		} else
+			clear_bit(F_RX_PROGRESS, &priv->flags);
+	}
+	return ret;
+}
+
+static int mscan_do_set_mode(struct net_device *dev, enum can_mode mode)
+{
+
+	struct mscan_priv *priv = netdev_priv(dev);
+	int ret = 0;
+
+	if (!priv->open_time)
+		return -EINVAL;
+
+	switch (mode) {
+	case CAN_MODE_SLEEP:
+	case CAN_MODE_STOP:
+		netif_stop_queue(dev);
+		mscan_set_mode(dev,
+			       (mode ==
+				CAN_MODE_STOP) ? MSCAN_INIT_MODE :
+			       MSCAN_SLEEP_MODE);
+		break;
+	case CAN_MODE_START:
+		if (priv->can.state <= CAN_STATE_BUS_OFF)
+			mscan_set_mode(dev, MSCAN_INIT_MODE);
+		ret = mscan_start(dev);
+		if (ret)
+			break;
+		if (netif_queue_stopped(dev))
+			netif_wake_queue(dev);
+		break;
+
+	default:
+		ret = -EOPNOTSUPP;
+		break;
+	}
+	return ret;
+}
+
+static int mscan_do_set_bittiming(struct net_device *dev)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	struct mscan_priv *priv = netdev_priv(dev);
+	struct can_bittiming *bt = &priv->can.bittiming;
+	u8 btr0, btr1;
+
+	btr0 = BTR0_SET_BRP(bt->brp) | BTR0_SET_SJW(bt->sjw);
+	btr1 = (BTR1_SET_TSEG1(bt->prop_seg + bt->phase_seg1) |
+		BTR1_SET_TSEG2(bt->phase_seg2) |
+		BTR1_SET_SAM(priv->can.ctrlmode & CAN_CTRLMODE_3_SAMPLES));
+
+	dev_info(ND2D(dev), "setting BTR0=0x%02x BTR1=0x%02x\n", btr0, btr1);
+
+	out_8(&regs->canbtr0, btr0);
+	out_8(&regs->canbtr1, btr1);
+
+	return 0;
+}
+
+static int mscan_open(struct net_device *dev)
+{
+	int ret;
+	struct mscan_priv *priv = netdev_priv(dev);
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+
+	/* determine and set bittime */
+	ret = can_set_bittiming(dev);
+	if (ret)
+		return ret;
+
+	napi_enable(&priv->napi);
+	ret = request_irq(dev->irq, mscan_isr, 0, dev->name, dev);
+
+	if (ret < 0) {
+		napi_disable(&priv->napi);
+		printk(KERN_ERR "%s - failed to attach interrupt\n",
+		       dev->name);
+		return ret;
+	}
+
+	priv->open_time = jiffies;
+
+	out_8(&regs->canctl1, in_8(&regs->canctl1) & ~MSCAN_LISTEN);
+
+	ret = mscan_start(dev);
+	if (ret)
+		return ret;
+
+	netif_start_queue(dev);
+
+	return 0;
+}
+
+static int mscan_close(struct net_device *dev)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	struct mscan_priv *priv = netdev_priv(dev);
+
+	napi_disable(&priv->napi);
+
+	out_8(&regs->cantier, 0);
+	out_8(&regs->canrier, 0);
+	free_irq(dev->irq, dev);
+	mscan_set_mode(dev, MSCAN_INIT_MODE);
+	can_close_cleanup(dev);
+	netif_stop_queue(dev);
+	priv->open_time = 0;
+
+	return 0;
+}
+
+int register_mscandev(struct net_device *dev, int clock_src)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	u8 ctl1;
+
+	ctl1 = in_8(&regs->canctl1);
+	if (clock_src)
+		ctl1 |= MSCAN_CLKSRC;
+	else
+		ctl1 &= ~MSCAN_CLKSRC;
+
+	ctl1 |= MSCAN_CANE;
+	out_8(&regs->canctl1, ctl1);
+	udelay(100);
+
+	/* acceptance mask/acceptance code (accept everything) */
+	out_be16(&regs->canidar1_0, 0);
+	out_be16(&regs->canidar3_2, 0);
+	out_be16(&regs->canidar5_4, 0);
+	out_be16(&regs->canidar7_6, 0);
+
+	out_be16(&regs->canidmr1_0, 0xffff);
+	out_be16(&regs->canidmr3_2, 0xffff);
+	out_be16(&regs->canidmr5_4, 0xffff);
+	out_be16(&regs->canidmr7_6, 0xffff);
+	/* Two 32 bit Acceptance Filters */
+	out_8(&regs->canidac, MSCAN_AF_32BIT);
+
+	mscan_set_mode(dev, MSCAN_INIT_MODE);
+
+	return register_candev(dev);
+}
+EXPORT_SYMBOL_GPL(register_mscandev);
+
+void unregister_mscandev(struct net_device *dev)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	mscan_set_mode(dev, MSCAN_INIT_MODE);
+	out_8(&regs->canctl1, in_8(&regs->canctl1) & ~MSCAN_CANE);
+	unregister_candev(dev);
+}
+EXPORT_SYMBOL_GPL(unregister_mscandev);
+
+struct net_device *alloc_mscandev(void)
+{
+	struct net_device *dev;
+	struct mscan_priv *priv;
+	int i;
+
+	dev = alloc_candev(sizeof(struct mscan_priv));
+	if (!dev)
+		return NULL;
+	priv = netdev_priv(dev);
+
+	dev->open = mscan_open;
+	dev->stop = mscan_close;
+	dev->hard_start_xmit = mscan_start_xmit;
+
+	dev->flags |= IFF_ECHO;	/* we support local echo */
+
+	priv->dev = dev;
+	netif_napi_add(dev, &priv->napi, mscan_rx_poll, 8);
+
+	priv->can.bittiming_const = &mscan_bittiming_const;
+	priv->can.do_set_bittiming = mscan_do_set_bittiming;
+	priv->can.do_set_mode = mscan_do_set_mode;
+
+	for (i = 0; i < TX_QUEUE_SIZE; i++) {
+		priv->tx_queue[i].id = i;
+		priv->tx_queue[i].mask = 1 << i;
+	}
+
+	return dev;
+}
+EXPORT_SYMBOL_GPL(alloc_mscandev);
+
+MODULE_AUTHOR("Andrey Volkov <avolkov@varma-el.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CAN port driver for a MSCAN based chips");
diff --git a/drivers/net/can/mscan/mscan.h b/drivers/net/can/mscan/mscan.h
new file mode 100644
index 000000000000..81a2eb5a19e5
--- /dev/null
+++ b/drivers/net/can/mscan/mscan.h
@@ -0,0 +1,237 @@
+/*
+ * Definitions of consts/structs to drive the Freescale MSCAN.
+ *
+ * Copyright (C) 2005-2006 Andrey Volkov <avolkov@varma-el.com>,
+ *                         Varma Electronics Oy
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef __MSCAN_H__
+#define __MSCAN_H__
+
+#include <linux/types.h>
+
+/* MSCAN control register 0 (CANCTL0) bits */
+#define MSCAN_RXFRM		0x80
+#define MSCAN_RXACT		0x40
+#define MSCAN_CSWAI		0x20
+#define MSCAN_SYNCH		0x10
+#define MSCAN_TIME		0x08
+#define MSCAN_WUPE		0x04
+#define MSCAN_SLPRQ		0x02
+#define MSCAN_INITRQ		0x01
+
+/* MSCAN control register 1 (CANCTL1) bits */
+#define MSCAN_CANE		0x80
+#define MSCAN_CLKSRC		0x40
+#define MSCAN_LOOPB		0x20
+#define MSCAN_LISTEN		0x10
+#define MSCAN_WUPM		0x04
+#define MSCAN_SLPAK		0x02
+#define MSCAN_INITAK		0x01
+
+/* Use the MPC5200 MSCAN variant? */
+#ifdef CONFIG_PPC
+#define MSCAN_FOR_MPC5200
+#endif
+
+#ifdef MSCAN_FOR_MPC5200
+#define MSCAN_CLKSRC_BUS	0
+#define MSCAN_CLKSRC_XTAL	MSCAN_CLKSRC
+#else
+#define MSCAN_CLKSRC_BUS	MSCAN_CLKSRC
+#define MSCAN_CLKSRC_XTAL	0
+#endif
+
+/* MSCAN receiver flag register (CANRFLG) bits */
+#define MSCAN_WUPIF		0x80
+#define MSCAN_CSCIF		0x40
+#define MSCAN_RSTAT1		0x20
+#define MSCAN_RSTAT0		0x10
+#define MSCAN_TSTAT1		0x08
+#define MSCAN_TSTAT0		0x04
+#define MSCAN_OVRIF		0x02
+#define MSCAN_RXF		0x01
+#define MSCAN_ERR_IF 		(MSCAN_OVRIF | MSCAN_CSCIF)
+#define MSCAN_RSTAT_MSK		(MSCAN_RSTAT1 | MSCAN_RSTAT0)
+#define MSCAN_TSTAT_MSK		(MSCAN_TSTAT1 | MSCAN_TSTAT0)
+#define MSCAN_STAT_MSK		(MSCAN_RSTAT_MSK | MSCAN_TSTAT_MSK)
+
+#define MSCAN_STATE_BUS_OFF	(MSCAN_RSTAT1 | MSCAN_RSTAT0 | \
+				 MSCAN_TSTAT1 | MSCAN_TSTAT0)
+#define MSCAN_STATE_TX(canrflg)	(((canrflg)&MSCAN_TSTAT_MSK)>>2)
+#define MSCAN_STATE_RX(canrflg)	(((canrflg)&MSCAN_RSTAT_MSK)>>4)
+#define MSCAN_STATE_ACTIVE	0
+#define MSCAN_STATE_WARNING	1
+#define MSCAN_STATE_PASSIVE	2
+#define MSCAN_STATE_BUSOFF	3
+
+/* MSCAN receiver interrupt enable register (CANRIER) bits */
+#define MSCAN_WUPIE		0x80
+#define MSCAN_CSCIE		0x40
+#define MSCAN_RSTATE1		0x20
+#define MSCAN_RSTATE0		0x10
+#define MSCAN_TSTATE1		0x08
+#define MSCAN_TSTATE0		0x04
+#define MSCAN_OVRIE		0x02
+#define MSCAN_RXFIE		0x01
+
+/* MSCAN transmitter flag register (CANTFLG) bits */
+#define MSCAN_TXE2		0x04
+#define MSCAN_TXE1		0x02
+#define MSCAN_TXE0		0x01
+#define MSCAN_TXE		(MSCAN_TXE2 | MSCAN_TXE1 | MSCAN_TXE0)
+
+/* MSCAN transmitter interrupt enable register (CANTIER) bits */
+#define MSCAN_TXIE2		0x04
+#define MSCAN_TXIE1		0x02
+#define MSCAN_TXIE0		0x01
+#define MSCAN_TXIE		(MSCAN_TXIE2 | MSCAN_TXIE1 | MSCAN_TXIE0)
+
+/* MSCAN transmitter message abort request (CANTARQ) bits */
+#define MSCAN_ABTRQ2		0x04
+#define MSCAN_ABTRQ1		0x02
+#define MSCAN_ABTRQ0		0x01
+
+/* MSCAN transmitter message abort ack (CANTAAK) bits */
+#define MSCAN_ABTAK2		0x04
+#define MSCAN_ABTAK1		0x02
+#define MSCAN_ABTAK0		0x01
+
+/* MSCAN transmit buffer selection (CANTBSEL) bits */
+#define MSCAN_TX2		0x04
+#define MSCAN_TX1		0x02
+#define MSCAN_TX0		0x01
+
+/* MSCAN ID acceptance control register (CANIDAC) bits */
+#define MSCAN_IDAM1		0x20
+#define MSCAN_IDAM0		0x10
+#define MSCAN_IDHIT2		0x04
+#define MSCAN_IDHIT1		0x02
+#define MSCAN_IDHIT0		0x01
+
+#define MSCAN_AF_32BIT		0x00
+#define MSCAN_AF_16BIT		MSCAN_IDAM0
+#define MSCAN_AF_8BIT		MSCAN_IDAM1
+#define MSCAN_AF_CLOSED		(MSCAN_IDAM0|MSCAN_IDAM1)
+#define MSCAN_AF_MASK		(~(MSCAN_IDAM0|MSCAN_IDAM1))
+
+/* MSCAN Miscellaneous Register (CANMISC) bits */
+#define MSCAN_BOHOLD		0x01
+
+#ifdef MSCAN_FOR_MPC5200
+#define _MSCAN_RESERVED_(n, num) u8 _res##n[num]
+#define _MSCAN_RESERVED_DSR_SIZE	2
+#else
+#define _MSCAN_RESERVED_(n, num)
+#define _MSCAN_RESERVED_DSR_SIZE	0
+#endif
+
+/* Structure of the hardware registers */
+struct mscan_regs {
+	/* (see doco S12MSCANV3/D)		  MPC5200    MSCAN */
+	u8 canctl0;				/* + 0x00     0x00 */
+	u8 canctl1;				/* + 0x01     0x01 */
+	_MSCAN_RESERVED_(1, 2);			/* + 0x02          */
+	u8 canbtr0;				/* + 0x04     0x02 */
+	u8 canbtr1;				/* + 0x05     0x03 */
+	_MSCAN_RESERVED_(2, 2);			/* + 0x06          */
+	u8 canrflg;				/* + 0x08     0x04 */
+	u8 canrier;				/* + 0x09     0x05 */
+	_MSCAN_RESERVED_(3, 2);			/* + 0x0a          */
+	u8 cantflg;				/* + 0x0c     0x06 */
+	u8 cantier;				/* + 0x0d     0x07 */
+	_MSCAN_RESERVED_(4, 2);			/* + 0x0e          */
+	u8 cantarq;				/* + 0x10     0x08 */
+	u8 cantaak;				/* + 0x11     0x09 */
+	_MSCAN_RESERVED_(5, 2);			/* + 0x12          */
+	u8 cantbsel;				/* + 0x14     0x0a */
+	u8 canidac;				/* + 0x15     0x0b */
+	u8 reserved;				/* + 0x16     0x0c */
+	_MSCAN_RESERVED_(6, 5);			/* + 0x17          */
+#ifndef MSCAN_FOR_MPC5200
+	u8 canmisc;				/*            0x0d */
+#endif
+	u8 canrxerr;				/* + 0x1c     0x0e */
+	u8 cantxerr;				/* + 0x1d     0x0f */
+	_MSCAN_RESERVED_(7, 2);			/* + 0x1e          */
+	u16 canidar1_0;				/* + 0x20     0x10 */
+	_MSCAN_RESERVED_(8, 2);			/* + 0x22          */
+	u16 canidar3_2;				/* + 0x24     0x12 */
+	_MSCAN_RESERVED_(9, 2);			/* + 0x26          */
+	u16 canidmr1_0;				/* + 0x28     0x14 */
+	_MSCAN_RESERVED_(10, 2);		/* + 0x2a          */
+	u16 canidmr3_2;				/* + 0x2c     0x16 */
+	_MSCAN_RESERVED_(11, 2);		/* + 0x2e          */
+	u16 canidar5_4;				/* + 0x30     0x18 */
+	_MSCAN_RESERVED_(12, 2);		/* + 0x32          */
+	u16 canidar7_6;				/* + 0x34     0x1a */
+	_MSCAN_RESERVED_(13, 2);		/* + 0x36          */
+	u16 canidmr5_4;				/* + 0x38     0x1c */
+	_MSCAN_RESERVED_(14, 2);		/* + 0x3a          */
+	u16 canidmr7_6;				/* + 0x3c     0x1e */
+	_MSCAN_RESERVED_(15, 2);		/* + 0x3e          */
+	struct {
+		u16 idr1_0;			/* + 0x40     0x20 */
+		 _MSCAN_RESERVED_(16, 2);	/* + 0x42          */
+		u16 idr3_2;			/* + 0x44     0x22 */
+		 _MSCAN_RESERVED_(17, 2);	/* + 0x46          */
+		u16 dsr1_0;			/* + 0x48     0x24 */
+		 _MSCAN_RESERVED_(18, 2);	/* + 0x4a          */
+		u16 dsr3_2;			/* + 0x4c     0x26 */
+		 _MSCAN_RESERVED_(19, 2);	/* + 0x4e          */
+		u16 dsr5_4;			/* + 0x50     0x28 */
+		 _MSCAN_RESERVED_(20, 2);	/* + 0x52          */
+		u16 dsr7_6;			/* + 0x54     0x2a */
+		 _MSCAN_RESERVED_(21, 2);	/* + 0x56          */
+		u8 dlr;				/* + 0x58     0x2c */
+		 u8:8;				/* + 0x59     0x2d */
+		 _MSCAN_RESERVED_(22, 2);	/* + 0x5a          */
+		u16 time;			/* + 0x5c     0x2e */
+	} rx;
+	 _MSCAN_RESERVED_(23, 2);		/* + 0x5e          */
+	struct {
+		u16 idr1_0;			/* + 0x60     0x30 */
+		 _MSCAN_RESERVED_(24, 2);	/* + 0x62          */
+		u16 idr3_2;			/* + 0x64     0x32 */
+		 _MSCAN_RESERVED_(25, 2);	/* + 0x66          */
+		u16 dsr1_0;			/* + 0x68     0x34 */
+		 _MSCAN_RESERVED_(26, 2);	/* + 0x6a          */
+		u16 dsr3_2;			/* + 0x6c     0x36 */
+		 _MSCAN_RESERVED_(27, 2);	/* + 0x6e          */
+		u16 dsr5_4;			/* + 0x70     0x38 */
+		 _MSCAN_RESERVED_(28, 2);	/* + 0x72          */
+		u16 dsr7_6;			/* + 0x74     0x3a */
+		 _MSCAN_RESERVED_(29, 2);	/* + 0x76          */
+		u8 dlr;				/* + 0x78     0x3c */
+		u8 tbpr;			/* + 0x79     0x3d */
+		 _MSCAN_RESERVED_(30, 2);	/* + 0x7a          */
+		u16 time;			/* + 0x7c     0x3e */
+	} tx;
+	 _MSCAN_RESERVED_(31, 2);		/* + 0x7e          */
+} __attribute__ ((packed));
+
+#undef _MSCAN_RESERVED_
+#define MSCAN_REGION 	sizeof(struct mscan)
+
+struct net_device *alloc_mscandev(void);
+/* clock_src:
+ *	1 = The MSCAN clock source is the onchip Bus Clock.
+ *	0 = The MSCAN clock source is the chip Oscillator Clock.
+ */
+extern int register_mscandev(struct net_device *dev, int clock_src);
+extern void unregister_mscandev(struct net_device *dev);
+
+#endif /* __MSCAN_H__ */
diff --git a/drivers/net/can/old/Kconfig b/drivers/net/can/old/Kconfig
new file mode 100644
index 000000000000..4b8567a20d03
--- /dev/null
+++ b/drivers/net/can/old/Kconfig
@@ -0,0 +1,67 @@
+
+config CAN_SJA1000_OLD
+	depends on CAN_OLD_DRIVERS
+	tristate "Philips SJA1000 (old)"
+	---help---
+	  The SJA1000 is one of the top CAN controllers out there. As it
+	  has a multiplexed interface it fits directly to 8051
+	  microcontrollers or into the PC I/O port space. The SJA1000
+	  is a full CAN controller, with shadow registers for RX and TX.
+	  It can send and receive any kinds of CAN frames (SFF/EFF/RTR)
+	  with a single (simple) filter setup.
+	  REMARK: This is the 'old' driver originally written by Matthias
+	  Brukner and Oliver Hartkopp which uses a non-standard hardware
+	  abstaction layer (HAL) inspired by the OCAN driver.
+
+config CAN_I82527_OLD
+	depends on CAN_OLD_DRIVERS
+	tristate "Intel 82527 (old)"
+	---help---
+	  The i82527 is a complex CAN controller that can handle RTR
+	  frame replies on it's own. This feature (and diffent RX filters)
+	  lead to an amount of 15 message objects (for RX & TX). Message
+	  object 15 has (as only) a shadow register for a reliable
+	  receiption of EFF or(!) SFF frames at high CAN traffic.
+	  This driver can send each type of CAN frames (SFF/EFF/RTR).
+	  Using 4 message objects it can also receive each type of CAN
+	  frames. But due to the onchip filter matching trigger method
+	  it is not possible to determine the received RTR CAN-ID.
+	  The reliable message object 15 receives SFF frames by default.
+	  This message object 15 usage maybe changed with the mo15 param.
+	  REMARK: This is the 'old' driver originally written by Oliver
+	  Hartkopp which uses a non-standard hardware abstaction layer (HAL)
+	  inspired by the OCAN driver. http://ar.linux.it/software/#ocan
+
+config CAN_MSCAN_OLD
+	depends on CAN_OLD_DRIVERS && (PPC || M68K || M68KNOMMU)
+	tristate "Support for a Freescale MSCAN based chips (old)"
+	---help---
+	  The Motorola Scalable Controller Area Network (MSCAN) definition
+	  is based on the MSCAN12 definition which is the specific
+	  implementation of the Motorola Scalable CAN concept targeted for
+	  the Motorola MC68HC12 Microcontroller Family.
+
+config CAN_MPC52XX_OLD
+	tristate "Freescale MPC5200 onboard CAN controller (old)"
+	depends on CAN_MSCAN_OLD && (PPC_MPC52xx || PPC_52xx)
+	default LITE5200
+	---help---
+	  If you say yes here you get support for Freescale MPC5200
+	  onboard dualCAN controller.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called mpc52xx_can.
+
+config CAN_CCAN_OLD
+	depends on CAN_OLD_DRIVERS
+	tristate "Bosch CCAN driver (old)"
+	---help---
+	  This is a driver for the Bosch CCAN controller found for example
+	  on the hynix h7202 chip.
+
+config CAN_H7202_OLD
+	tristate "Hynix H7202 onboard CAN controller (old)"
+	depends on CAN_CCAN_OLD
+	---help---
+	  This is a driver for the hynix h7202 can controller.
+
diff --git a/drivers/net/can/old/ccan/Makefile b/drivers/net/can/old/ccan/Makefile
new file mode 100644
index 000000000000..c3db1800e6d7
--- /dev/null
+++ b/drivers/net/can/old/ccan/Makefile
@@ -0,0 +1,20 @@
+#
+#
+
+ifeq ($(KERNELRELEASE),)
+
+KERNELDIR := /lib/modules/$(shell uname -r)/build
+PWD       := $(shell pwd)
+TOPDIR    := $(PWD)/../../../..
+
+modules modules_install clean:
+	$(MAKE) -C $(KERNELDIR) M=$(PWD) $@ TOPDIR=$(TOPDIR)
+
+else
+
+-include $(TOPDIR)/Makefile.common
+
+obj-$(CONFIG_CAN_CCAN)		+= ccan.o
+obj-$(CONFIG_CAN_H7202)		+= h7202_can.o
+
+endif
diff --git a/drivers/net/can/old/ccan/ccan.c b/drivers/net/can/old/ccan/ccan.c
new file mode 100644
index 000000000000..6a908a14eee4
--- /dev/null
+++ b/drivers/net/can/old/ccan/ccan.c
@@ -0,0 +1,557 @@
+/*
+ * drivers/can/c_can.c
+ *
+ * Copyright (C) 2007
+ *
+ * - Sascha Hauer, Marc Kleine-Budde, Pengutronix
+ * - Simon Kallweit, intefo AG
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+#define DBG(args...) printk(args)
+#else
+#define DBG(args...)
+#endif
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/can.h>
+#include <linux/list.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/io.h>
+
+#include <linux/can/dev.h>
+#include <linux/can/error.h>
+#include "ccan.h"
+
+static u32 ccan_read_reg32(struct net_device *dev, enum c_regs reg)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+
+	u32 val = priv->read_reg(dev, reg);
+	val |= ((u32) priv->read_reg(dev, reg + 2)) << 16;
+
+	return val;
+}
+
+static void ccan_write_reg32(struct net_device *dev, enum c_regs reg, u32 val)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+
+	priv->write_reg(dev, reg, val & 0xffff);
+	priv->write_reg(dev, reg + 2, val >> 16);
+}
+
+static inline void ccan_object_get(struct net_device *dev,
+				   int iface, int objno, int mask)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+
+	priv->write_reg(dev, CAN_IF_COMM(iface), mask);
+	priv->write_reg(dev, CAN_IF_COMR(iface), objno + 1);
+	while (priv->read_reg(dev, CAN_IF_COMR(iface)) & IF_COMR_BUSY)
+		DBG("busy\n");
+}
+
+static inline void ccan_object_put(struct net_device *dev,
+				   int iface, int objno, int mask)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+
+	priv->write_reg(dev, CAN_IF_COMM(iface), IF_COMM_WR | mask);
+	priv->write_reg(dev, CAN_IF_COMR(iface), objno + 1);
+	while (priv->read_reg(dev, CAN_IF_COMR(iface)) & IF_COMR_BUSY)
+		DBG("busy\n");
+}
+
+static int ccan_write_object(struct net_device *dev,
+			     int iface, struct can_frame *frame, int objno)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+	unsigned int val;
+
+	if (frame->can_id & CAN_EFF_FLAG)
+		val = IF_ARB_MSGXTD | (frame->can_id & CAN_EFF_MASK);
+	else
+		val = ((frame->can_id & CAN_SFF_MASK) << 18);
+
+	if (!(frame->can_id & CAN_RTR_FLAG))
+		val |=  IF_ARB_TRANSMIT;
+
+	val |=  IF_ARB_MSGVAL;
+	ccan_write_reg32(dev, CAN_IF_ARB(iface), val);
+
+	memcpy(&val, &frame->data[0], 4);
+	ccan_write_reg32(dev, CAN_IF_DATAA(iface), val);
+	memcpy(&val, &frame->data[4], 4);
+	ccan_write_reg32(dev, CAN_IF_DATAB(iface), val);
+	priv->write_reg(dev, CAN_IF_MCONT(iface),
+			IF_MCONT_TXIE |	IF_MCONT_TXRQST | IF_MCONT_EOB |
+			(frame->can_dlc & 0xf));
+
+	ccan_object_put(dev, 0, objno, IF_COMM_ALL);
+
+	return 0;
+}
+
+static int ccan_read_object(struct net_device *dev, int iface, int objno)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+	unsigned int val, ctrl, data;
+	struct sk_buff *skb;
+	struct can_frame *frame;
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	skb->dev = dev;
+
+	ccan_object_get(dev, 0, objno, IF_COMM_ALL & ~IF_COMM_TXRQST);
+#ifdef CCAN_DEBUG
+	priv->bufstat[objno]++;
+#endif
+	frame = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
+
+	ctrl = priv->read_reg(dev, CAN_IF_MCONT(iface));
+
+	if (ctrl & IF_MCONT_MSGLST) {
+		priv->can.net_stats.rx_errors++;
+		DBG("%s: msg lost in buffer %d\n", __func__, objno);
+	}
+
+	frame->can_dlc = ctrl & 0xf;
+
+	val = ccan_read_reg32(dev, CAN_IF_ARB(iface));
+
+	data = ccan_read_reg32(dev, CAN_IF_DATAA(iface));
+	memcpy(&frame->data[0], &data, 4);
+	data = ccan_read_reg32(dev, CAN_IF_DATAB(iface));
+	memcpy(&frame->data[4], &data, 4);
+
+	if (val & IF_ARB_MSGXTD)
+		frame->can_id = (val & CAN_EFF_MASK) | CAN_EFF_FLAG;
+	else
+		frame->can_id = (val >> 18) & CAN_SFF_MASK;
+
+	if (val & IF_ARB_TRANSMIT)
+		frame->can_id |= CAN_RTR_FLAG;
+
+	priv->write_reg(dev, CAN_IF_MCONT(iface), ctrl &
+			~(IF_MCONT_MSGLST | IF_MCONT_INTPND | IF_MCONT_NEWDAT));
+
+	ccan_object_put(dev, 0, objno, IF_COMM_CONTROL);
+
+	skb->protocol = __constant_htons(ETH_P_CAN);
+	netif_rx(skb);
+
+	priv->can.net_stats.rx_packets++;
+	priv->can.net_stats.rx_bytes += frame->can_dlc;
+
+	return 0;
+}
+
+static int ccan_setup_receive_object(struct net_device *dev, int iface,
+				     int objno, unsigned int mask,
+				     unsigned int id, unsigned int mcont)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+
+	ccan_write_reg32(dev, CAN_IF_MASK(iface), mask);
+	ccan_write_reg32(dev, CAN_IF_ARB(iface), IF_ARB_MSGVAL | id);
+
+	priv->write_reg(dev, CAN_IF_MCONT(iface), mcont);
+
+	ccan_object_put(dev, 0, objno, IF_COMM_ALL & ~IF_COMM_TXRQST);
+
+	DBG("%s: obj no %d msgval: 0x%08x\n", __func__,
+		objno, ccan_read_reg32(dev, CAN_MSGVAL));
+
+	return 0;
+}
+
+static int ccan_inval_object(struct net_device *dev, int iface, int objno)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+
+	ccan_write_reg32(dev, CAN_IF_ARB(iface), 0);
+	priv->write_reg(dev, CAN_IF_MCONT(iface), 0);
+	ccan_object_put(dev, 0, objno, IF_COMM_ARB | IF_COMM_CONTROL);
+
+	DBG("%s: obj no %d msgval: 0x%08x\n", __func__,
+		objno, ccan_read_reg32(dev, CAN_MSGVAL));
+
+	return 0;
+}
+
+static int ccan_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+	struct can_frame *frame = (struct can_frame *)skb->data;
+
+	spin_lock_irq(&priv->can.irq_lock);
+
+	ccan_write_object(dev, 0, frame, priv->tx_object);
+#ifdef CCAN_DEBUG
+	priv->bufstat[priv->tx_object]++;
+#endif
+	priv->tx_object++;
+	if (priv->tx_object > 5)
+		netif_stop_queue(dev);
+
+	spin_unlock_irq(&priv->can.irq_lock);
+
+	priv->can.net_stats.tx_packets++;
+	priv->can.net_stats.tx_bytes += frame->can_dlc;
+
+	dev->trans_start = jiffies;
+	dev_kfree_skb(skb);
+
+	return 0;
+}
+
+static void ccan_tx_timeout(struct net_device *dev)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+
+	priv->can.net_stats.tx_errors++;
+}
+
+static int ccan_set_bittime(struct net_device *dev, struct can_bittime *br)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+	unsigned int reg_timing, ctrl_save;
+	u8 brp, sjw, tseg1, tseg2;
+
+	if (br->type != CAN_BITTIME_STD)
+		return -EINVAL;
+
+	brp = br->std.brp - 1;
+	sjw = br->std.sjw - 1;
+	tseg1 = br->std.prop_seg + br->std.phase_seg1 - 1;
+	tseg2 = br->std.phase_seg2 - 1;
+
+	reg_timing = (brp & BTR_BRP_MASK) |
+		     ((sjw << BTR_SJW_SHIFT) & BTR_SJW_MASK) |
+		     ((tseg1 << BTR_TSEG1_SHIFT) & BTR_TSEG1_MASK) |
+		     ((tseg2 << BTR_TSEG2_SHIFT) & BTR_TSEG2_MASK);
+
+	DBG("%s: brp = %d sjw = %d seg1 = %d seg2 = %d\n", __func__,
+		brp, sjw, tseg1, tseg2);
+	DBG("%s: setting BTR to %04x\n", __func__, reg_timing);
+
+	spin_lock_irq(&priv->can.irq_lock);
+
+	ctrl_save = priv->read_reg(dev, CAN_CONTROL);
+	priv->write_reg(dev, CAN_CONTROL,
+			ctrl_save | CONTROL_CCE | CONTROL_INIT);
+	priv->write_reg(dev, CAN_BTR, reg_timing);
+	priv->write_reg(dev, CAN_CONTROL, ctrl_save);
+
+	spin_unlock_irq(&priv->can.irq_lock);
+
+	return 0;
+}
+
+static int ccan_set_mode(struct net_device *dev, enum can_mode mode)
+{
+	switch (mode) {
+	case CAN_MODE_START:
+		DBG("%s: CAN_MODE_START requested\n", __func__);
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+static int ccan_get_state(struct net_device *dev, enum can_state *state)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+	u32 reg_status;
+#ifdef CCAN_DEBUG
+	int i;
+#endif
+
+	reg_status = priv->read_reg(dev, CAN_STATUS);
+
+	if (reg_status & STATUS_EPASS)
+		*state = CAN_STATE_BUS_PASSIVE;
+	else if (reg_status & STATUS_EWARN)
+		*state = CAN_STATE_BUS_WARNING;
+	else if (reg_status & STATUS_BOFF)
+		*state = CAN_STATE_BUS_OFF;
+	else
+		*state = CAN_STATE_ACTIVE;
+#ifdef CCAN_DEBUG
+	DBG("buffer statistic:\n");
+	for (i = 0; i <= MAX_OBJECT; i++)
+		DBG("%d: %d\n", i, priv->bufstat[i]);
+#endif
+	return 0;
+}
+
+static int ccan_do_status_irq(struct net_device *dev)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+	int status, diff;
+
+	status = priv->read_reg(dev, CAN_STATUS);
+	status &= ~(STATUS_TXOK | STATUS_RXOK);
+	diff = status ^ priv->last_status;
+
+	if (diff & STATUS_EPASS) {
+		if (status & STATUS_EPASS)
+			dev_info(ND2D(dev), "entered error passive state\n");
+		else
+			dev_info(ND2D(dev), "left error passive state\n");
+	}
+	if (diff & STATUS_EWARN) {
+		if (status & STATUS_EWARN)
+			dev_info(ND2D(dev), "entered error warning state\n");
+		else
+			dev_info(ND2D(dev), "left error warning state\n");
+	}
+	if (diff & STATUS_BOFF) {
+		if (status & STATUS_BOFF)
+			dev_info(ND2D(dev), "entered busoff state\n");
+		else
+			dev_info(ND2D(dev), "left busoff state\n");
+	}
+
+	if (diff & STATUS_LEC_MASK) {
+		switch (status & STATUS_LEC_MASK) {
+		case LEC_STUFF_ERROR:
+			dev_info(ND2D(dev), "suffing error\n");
+			break;
+		case LEC_FORM_ERROR:
+			dev_info(ND2D(dev), "form error\n");
+			break;
+		case LEC_ACK_ERROR:
+			dev_info(ND2D(dev), "ack error\n");
+			break;
+		case LEC_BIT1_ERROR:
+			dev_info(ND2D(dev), "bit1 error\n");
+			break;
+		}
+	}
+
+	priv->write_reg(dev, CAN_STATUS, 0);
+	priv->last_status = status;
+
+	return diff ? 1 : 0;
+}
+
+static void ccan_do_object_irq(struct net_device *dev, u16 irqstatus)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+	int i;
+	u32 val;
+
+	if (irqstatus > MAX_TRANSMIT_OBJECT) {
+		val = ccan_read_reg32(dev, CAN_NEWDAT);
+		while (val & RECEIVE_OBJECT_BITS) {
+			for (i = MAX_TRANSMIT_OBJECT + 1; i <= MAX_OBJECT; i++)
+				if (val & (1<<i))
+					ccan_read_object(dev, 0, i);
+			val = ccan_read_reg32(dev, CAN_NEWDAT);
+		}
+	} else {
+		ccan_inval_object(dev, 0, irqstatus - 1);
+		val = ccan_read_reg32(dev, CAN_TXRQST);
+		if (!val) {
+			priv->tx_object = 0;
+			netif_wake_queue(dev);
+		}
+	}
+}
+
+static void do_statuspoll(struct work_struct *work)
+{
+	struct ccan_priv *priv = container_of(((struct delayed_work *) work),
+					      struct ccan_priv, work);
+
+	priv->write_reg(priv->dev, CAN_CONTROL,
+			CONTROL_SIE | CONTROL_EIE | CONTROL_IE);
+}
+
+static irqreturn_t ccan_isr(int irq, void *dev_id)
+{
+	struct net_device *dev = (struct net_device *) dev_id;
+	struct ccan_priv *priv = netdev_priv(dev);
+	u16 irqstatus;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->can.irq_lock, flags);
+
+	irqstatus = priv->read_reg(dev, CAN_IR);
+	while (irqstatus) {
+		if (irqstatus == 0x8000) {
+			if (ccan_do_status_irq(dev)) {
+				/* The c_can core tends to flood us with
+				 * interrupts when certain error states don't
+				 * disappear. Disable interrupts and see if it's
+				 * getting better later. This is at least the
+				 * case on the Magnachip h7202.
+				 */
+				priv->write_reg(dev, CAN_CONTROL, CONTROL_EIE |
+						CONTROL_IE);
+				schedule_delayed_work(&priv->work, HZ / 10);
+				goto exit;
+			}
+		} else {
+			ccan_do_object_irq(dev, irqstatus);
+		}
+		irqstatus = priv->read_reg(dev, CAN_IR);
+	}
+
+exit:
+	spin_unlock_irqrestore(&priv->can.irq_lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static int ccan_open(struct net_device *dev)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+
+	if (request_irq(dev->irq, &ccan_isr, 0, dev->name, dev)) {
+		dev_err(ND2D(dev), "failed to attach interrupt\n");
+		return -EAGAIN;
+	}
+
+	priv->write_reg(dev, CAN_CONTROL,
+			CONTROL_EIE | CONTROL_SIE | CONTROL_IE);
+
+	netif_wake_queue(dev);
+
+	return 0;
+}
+
+static int ccan_stop(struct net_device *dev)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+	unsigned long flags;
+
+	netif_stop_queue(dev);
+
+	cancel_delayed_work(&priv->work);
+	flush_scheduled_work();
+
+	/* mask all IRQs */
+	spin_lock_irqsave(&priv->can.irq_lock, flags);
+	priv->write_reg(dev, CAN_CONTROL, 0);
+	spin_unlock_irqrestore(&priv->can.irq_lock, flags);
+
+	free_irq(dev->irq, dev);
+
+	return 0;
+}
+
+static int ccan_chip_config(struct net_device *dev)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+	int i;
+
+	/* setup message objects */
+	for (i = 0; i <= MAX_OBJECT; i++)
+		ccan_inval_object(dev, 0, i);
+
+	for (i = MAX_TRANSMIT_OBJECT + 1; i < MAX_OBJECT; i++)
+		ccan_setup_receive_object(dev, 0, i, 0, 0,
+					  IF_MCONT_RXIE | IF_MCONT_UMASK);
+
+	ccan_setup_receive_object(dev, 0, MAX_OBJECT, 0, 0, IF_MCONT_EOB |
+				  IF_MCONT_RXIE | IF_MCONT_UMASK);
+
+#ifdef CCAN_DEBUG
+	for (i = 0; i <= MAX_OBJECT; i++)
+		priv->bufstat[i] = 0;
+#endif
+
+	return 0;
+}
+
+struct net_device *alloc_ccandev(int sizeof_priv)
+{
+	struct net_device *dev;
+	struct ccan_priv *priv;
+
+	dev = alloc_candev(sizeof_priv);
+	if (!dev)
+		return NULL;
+
+	priv = netdev_priv(dev);
+
+	dev->open = ccan_open;
+	dev->stop = ccan_stop;
+	dev->hard_start_xmit = ccan_hard_start_xmit;
+	dev->tx_timeout = ccan_tx_timeout;
+
+	priv->can.bitrate = 500000;
+
+	priv->can.do_set_bittime = ccan_set_bittime;
+	priv->can.do_get_state = ccan_get_state;
+	priv->can.do_set_mode = ccan_set_mode;
+
+	priv->dev = dev;
+	priv->tx_object = 0;
+
+	return dev;
+}
+EXPORT_SYMBOL(alloc_ccandev);
+
+void free_ccandev(struct net_device *dev)
+{
+	free_candev(dev);
+}
+EXPORT_SYMBOL(free_ccandev);
+
+int register_ccandev(struct net_device *dev)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+
+	ccan_set_mode(dev, CAN_MODE_START);
+
+	ccan_chip_config(dev);
+	INIT_DELAYED_WORK(&priv->work, do_statuspoll);
+
+	return register_netdev(dev);
+}
+EXPORT_SYMBOL(register_ccandev);
+
+void unregister_ccandev(struct net_device *dev)
+{
+	struct ccan_priv *priv = netdev_priv(dev);
+
+	ccan_set_mode(dev, CAN_MODE_START);
+
+	cancel_delayed_work(&priv->work);
+	flush_scheduled_work();
+
+	unregister_netdev(dev);
+}
+EXPORT_SYMBOL(unregister_ccandev);
+
+
+MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
+MODULE_AUTHOR("Simon Kallweit <simon.kallweit@intefo.ch>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CAN port driver for C_CAN based chips");
diff --git a/drivers/net/can/old/ccan/ccan.h b/drivers/net/can/old/ccan/ccan.h
new file mode 100644
index 000000000000..b7a5460195e1
--- /dev/null
+++ b/drivers/net/can/old/ccan/ccan.h
@@ -0,0 +1,140 @@
+/*
+ * drivers/can/c_can.h
+ *
+ * Copyright (C) 2007
+ *
+ * - Sascha Hauer, Marc Kleine-Budde, Pengutronix
+ * - Simon Kallweit, intefo AG
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef __CCAN_H__
+#define __CCAN_H__
+
+#include <linux/can.h>
+#include <linux/platform_device.h>
+
+#undef CCAN_DEBUG
+
+enum c_regs {
+	CAN_CONTROL = 0x00,
+	CAN_STATUS  = 0x02,
+	CAN_ERROR   = 0x04,
+	CAN_BTR     = 0x06,
+	CAN_IR      = 0x08,
+	CAN_TEST    = 0x0a,
+	CAN_BRP_EXT = 0x0c,
+	CAN_IF1     = 0x10,
+	CAN_IF2     = 0x40,
+	CAN_TXRQST  = 0x80, /* 32bit */
+	CAN_NEWDAT  = 0x90, /* 32bit */
+	CAN_INTPND  = 0xa0, /* 32bit */
+	CAN_MSGVAL  = 0xb0, /* 32bit */
+};
+
+#define CAN_IF_COMR(x)   (CAN_IF1 + (x) * 0x30 + 0x00)
+#define CAN_IF_COMM(x)   (CAN_IF1 + (x) * 0x30 + 0x02)
+#define CAN_IF_MASK(x)   (CAN_IF1 + (x) * 0x30 + 0x04)  /* 32bit */
+#define CAN_IF_ARB(x)    (CAN_IF1 + (x) * 0x30 + 0x08)  /* 32bit */
+#define CAN_IF_MCONT(x)  (CAN_IF1 + (x) * 0x30 + 0x0c)
+#define CAN_IF_DATAA(x)  (CAN_IF1 + (x) * 0x30 + 0x0e)  /* 32bit */
+#define CAN_IF_DATAB(x)  (CAN_IF1 + (x) * 0x30 + 0x12)  /* 32bit */
+
+#define CONTROL_TEST (1<<7)
+#define CONTROL_CCE  (1<<6)
+#define CONTROL_DAR  (1<<5)
+#define CONTROL_EIE  (1<<3)
+#define CONTROL_SIE  (1<<2)
+#define CONTROL_IE   (1<<1)
+#define CONTROL_INIT (1<<0)
+
+#define TEST_RX     (1<<7)
+#define TEST_TX1    (1<<6)
+#define TEST_TX2    (1<<5)
+#define TEST_LBACK  (1<<4)
+#define TEST_SILENT (1<<3)
+#define TEST_BASIC  (1<<2)
+
+#define STATUS_BOFF     (1<<7)
+#define STATUS_EWARN    (1<<6)
+#define STATUS_EPASS    (1<<5)
+#define STATUS_RXOK     (1<<4)
+#define STATUS_TXOK     (1<<3)
+#define STATUS_LEC_MASK (1<<2)
+#define LEC_STUFF_ERROR 1
+#define LEC_FORM_ERROR  2
+#define LEC_ACK_ERROR   3
+#define LEC_BIT1_ERROR  4
+
+#define BTR_BRP_MASK	0x3f
+#define BTR_BRP_SHIFT	0
+#define BTR_SJW_SHIFT	6
+#define BTR_SJW_MASK	(0x3 << BTR_SJW_SHIFT)
+#define BTR_TSEG1_SHIFT	8
+#define BTR_TSEG1_MASK	(0xf << BTR_TSEG1_SHIFT)
+#define BTR_TSEG2_SHIFT	12
+#define BTR_TSEG2_MASK	(0x7 << BTR_TSEG2_SHIFT)
+
+#define IF_COMR_BUSY (1<<15)
+
+#define IF_COMM_WR          (1<<7)
+#define IF_COMM_MASK        (1<<6)
+#define IF_COMM_ARB         (1<<5)
+#define IF_COMM_CONTROL     (1<<4)
+#define IF_COMM_CLR_INT_PND (1<<3)
+#define IF_COMM_TXRQST      (1<<2)
+#define IF_COMM_DATAA       (1<<1)
+#define IF_COMM_DATAB       (1<<0)
+
+#define IF_COMM_ALL (IF_COMM_MASK | IF_COMM_ARB | IF_COMM_CONTROL | \
+		     IF_COMM_TXRQST | IF_COMM_DATAA | IF_COMM_DATAB)
+
+#define IF_ARB_MSGVAL   (1<<31)
+#define IF_ARB_MSGXTD   (1<<30)
+#define IF_ARB_TRANSMIT (1<<29)
+
+#define IF_MCONT_NEWDAT (1<<15)
+#define IF_MCONT_MSGLST (1<<14)
+#define IF_MCONT_INTPND (1<<13)
+#define IF_MCONT_UMASK  (1<<12)
+#define IF_MCONT_TXIE   (1<<11)
+#define IF_MCONT_RXIE   (1<<10)
+#define IF_MCONT_RMTEN  (1<<9)
+#define IF_MCONT_TXRQST (1<<8)
+#define IF_MCONT_EOB    (1<<7)
+
+#define MAX_OBJECT 31
+#define MAX_TRANSMIT_OBJECT 15
+#define RECEIVE_OBJECT_BITS 0xffff0000
+
+struct ccan_priv {
+	struct can_priv can;
+	struct net_device *dev;
+	int tx_object;
+	int last_status;
+	struct delayed_work work;
+	u16 (*read_reg)(struct net_device *dev, enum c_regs reg);
+	void (*write_reg)(struct net_device *dev, enum c_regs reg, u16 val);
+#ifdef CCAN_DEBUG
+	unsigned int bufstat[MAX_OBJECT + 1];
+#endif
+};
+
+extern struct net_device *alloc_ccandev(int sizeof_priv);
+extern void free_ccandev(struct net_device *dev);
+extern int register_ccandev(struct net_device *dev);
+extern void unregister_ccandev(struct net_device *dev);
+
+#endif /* __CCAN_H__ */
diff --git a/drivers/net/can/old/ccan/h7202_can.c b/drivers/net/can/old/ccan/h7202_can.c
new file mode 100644
index 000000000000..7dcc3e749248
--- /dev/null
+++ b/drivers/net/can/old/ccan/h7202_can.c
@@ -0,0 +1,199 @@
+/*
+ * drivers/can/h7202_can.c
+ *
+ * Copyright (C) 2007
+ *
+ * - Sascha Hauer, Marc Kleine-Budde, Pengutronix
+ * - Simon Kallweit, intefo AG
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/netdevice.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/io.h>
+#include <asm/hardware.h>
+
+#include "ccan.h"
+
+#define DRV_NAME      "h7202can"
+#define DELAY         5
+#define CAN_ENABLE    0x0e
+
+static u16 h7202can_read_reg(struct net_device *dev, enum c_regs reg)
+{
+	u16 val;
+	volatile int i;
+
+	/* The big kernel lock is used to prevent any other AMBA devices from
+	 * interfering with the current register read operation. The register
+	 * is read twice because of braindamaged hynix cpu.
+	 */
+	lock_kernel();
+	val = inw(dev->base_addr + (reg<<1));
+	for (i = 0; i < DELAY; i++);
+	val = inw(dev->base_addr + (reg<<1));
+	for (i = 0; i < DELAY; i++);
+	unlock_kernel();
+
+	return val;
+}
+
+static void h7202can_write_reg(struct net_device *dev, enum c_regs reg, u16 val)
+{
+	volatile int i;
+
+	lock_kernel();
+	outw(val, dev->base_addr + (reg<<1));
+	for (i = 0; i < DELAY; i++);
+	unlock_kernel();
+}
+
+static int h7202can_drv_probe(struct platform_device *pdev)
+{
+	struct net_device *dev;
+	struct ccan_priv *priv;
+	struct resource *mem;
+	u32 mem_size;
+	int ret = -ENODEV;
+
+	dev = alloc_ccandev(sizeof(struct ccan_priv));
+	if (!dev)
+		return -ENOMEM;
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dev->irq = platform_get_irq(pdev, 0);
+	if (!mem || !dev->irq)
+		goto req_error;
+
+	mem_size = mem->end - mem->start + 1;
+	if (!request_mem_region(mem->start, mem_size, pdev->dev.driver->name)) {
+		dev_err(&pdev->dev, "resource unavailable\n");
+		goto req_error;
+	}
+
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	dev->base_addr = (unsigned long)ioremap_nocache(mem->start, mem_size);
+
+	if (!dev->base_addr) {
+		dev_err(&pdev->dev, "failed to map can port\n");
+		ret = -ENOMEM;
+		goto fail_map;
+	}
+
+	priv = netdev_priv(dev);
+	priv->can.can_sys_clock = 8000000;
+	priv->read_reg = h7202can_read_reg;
+	priv->write_reg = h7202can_write_reg;
+
+	platform_set_drvdata(pdev, dev);
+
+	/* configure ports */
+	switch (mem->start) {
+	case CAN0_PHYS:
+		CPU_REG(GPIO_C_VIRT, GPIO_EN) &= ~(3<<1);
+		CPU_REG(GPIO_C_VIRT, GPIO_DIR) &= ~(1<<1);
+		CPU_REG(GPIO_C_VIRT, GPIO_DIR) |= (1<<2);
+		break;
+	case CAN1_PHYS:
+		CPU_REG(GPIO_E_VIRT, GPIO_EN) &= ~(3<<16);
+		CPU_REG(GPIO_E_VIRT, GPIO_DIR) |= (1<<16);
+		CPU_REG(GPIO_E_VIRT, GPIO_DIR) &= ~(1<<17);
+		break;
+	}
+
+	/* enable can */
+	h7202can_write_reg(dev, CAN_ENABLE, 1);
+
+	ret = register_ccandev(dev);
+	if (ret >= 0) {
+		dev_info(&pdev->dev, "probe for a port 0x%lX done\n",
+			 dev->base_addr);
+		return ret;
+	}
+
+	iounmap((unsigned long *)dev->base_addr);
+fail_map:
+	release_mem_region(mem->start, mem_size);
+req_error:
+	free_ccandev(dev);
+	dev_err(&pdev->dev, "probe failed\n");
+	return ret;
+}
+
+static int h7202can_drv_remove(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct resource *mem;
+
+	platform_set_drvdata(pdev, NULL);
+	unregister_ccandev(dev);
+
+	iounmap((volatile void __iomem *)(dev->base_addr));
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(mem->start, mem->end - mem->start + 1);
+	free_ccandev(dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int h7202can_drv_suspend(struct platform_device *pdev,
+				pm_message_t state)
+{
+	return 0;
+}
+
+static int h7202can_drv_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+static struct platform_driver h7202can_driver = {
+	.driver		= {
+		.name		= DRV_NAME,
+	},
+	.probe		= h7202can_drv_probe,
+	.remove		= h7202can_drv_remove,
+#ifdef CONFIG_PM
+	.suspend	= h7202can_drv_suspend,
+	.resume		= h7202can_drv_resume,
+#endif	/* CONFIG_PM */
+};
+
+static int __init h7202can_init(void)
+{
+	printk(KERN_INFO "%s initializing\n", h7202can_driver.driver.name);
+	return platform_driver_register(&h7202can_driver);
+}
+
+static void __exit h7202can_cleanup(void)
+{
+	platform_driver_unregister(&h7202can_driver);
+	printk(KERN_INFO "%s unloaded\n", h7202can_driver.driver.name);
+}
+
+module_init(h7202can_init);
+module_exit(h7202can_cleanup);
+
+MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
+MODULE_AUTHOR("Simon Kallweit <simon.kallweit@intefo.ch>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CAN port driver Hynix H7202 processor");
diff --git a/drivers/net/can/old/hal/c200.c b/drivers/net/can/old/hal/c200.c
new file mode 100644
index 000000000000..47ea74ca51b0
--- /dev/null
+++ b/drivers/net/can/old/hal/c200.c
@@ -0,0 +1,206 @@
+/*
+ * c200.c - low cost parallelport CAN adaptor hardware abstraction layer
+ *          ( direct register access without parport subsystem support )
+ *
+ *          CAN200 project homepage http://private.addcom.de/horo/can200
+ *
+ *          This hal is based on a patch from Uwe Bonnes.
+ *
+ * Inspired by the OCAN driver http://ar.linux.it/software/#ocan
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/netdevice.h>
+#include <linux/ioport.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include "hal.h"
+
+/* init the HAL - call at driver module init */
+int hal_init(void) { return 0; }
+
+/* exit the HAL - call at driver module exit */
+int hal_exit(void) { return 0; }
+
+/* get name of this CAN HAL */
+char *hal_name(void) { return "c200"; }
+
+/* fill arrays base[] and irq[] with HAL specific defaults */
+void hal_use_defaults(void)
+{
+	extern unsigned long base[];
+	extern unsigned int  irq[];
+
+	base[0]		= 0x378UL;
+	irq[0]		= 7;
+}
+
+#define ECR_REGS_OFFSET 0x400
+#define ECR_CTRL_OFFSET (ECR_REGS_OFFSET + 2)
+
+static u8 ecr_crtl_save;
+
+/* request controller register access space */
+int hal_request_region(int dev_num,
+		       unsigned int num_regs,
+		       char *drv_name)
+{
+	extern unsigned long base[];
+	extern unsigned long rbase[];
+
+	/* set for device base_addr */
+	rbase[dev_num] = base[dev_num];
+
+	/* grab ECR control registers and set parport to 'byte mode' */
+	if (request_region(rbase[dev_num] + ECR_REGS_OFFSET, 3, drv_name)) {
+
+		ecr_crtl_save = inb(rbase[dev_num] + ECR_CTRL_OFFSET);
+
+		outb((ecr_crtl_save & 0x1F) | 0x20,
+		     rbase[dev_num] + ECR_CTRL_OFFSET);
+	} else
+		return 0;
+
+	if (request_region(rbase[dev_num], 4, drv_name))
+		return 1;
+
+	release_region(rbase[dev_num] + ECR_REGS_OFFSET, 3);
+
+	return 0;
+}
+
+/* release controller register access space */
+void hal_release_region(int dev_num,
+			unsigned int num_regs)
+{
+	extern unsigned long base[];
+
+	release_region(base[dev_num], 4);
+
+	/* restore original ECR control register value */
+	outb(ecr_crtl_save, base[dev_num] + ECR_CTRL_OFFSET);
+	release_region(base[dev_num] + ECR_REGS_OFFSET, 3);
+}
+
+/* enable non controller hardware (e.g. irq routing, etc.) */
+int hw_attach(int dev_num)
+{
+	extern unsigned long rbase[];
+	unsigned long pc = rbase[dev_num] + 2;
+
+	/* enable irq */
+	outb(inb(pc) | 0x10, pc);
+
+	return 0;
+}
+
+/* disable non controller hardware (e.g. irq routing, etc.) */
+int hw_detach(int dev_num)
+{
+	extern unsigned long rbase[];
+	unsigned long pc = rbase[dev_num] + 2;
+
+	/* disable irq */
+	outb(inb(pc) & ~0x10, pc);
+
+	return 0;
+}
+
+/* reset controller hardware (with specific non controller hardware) */
+int hw_reset_dev(int dev_num) { return 0; }
+
+#define WRITEP		0x01 /* inverted at port  */
+#define DATASTB		0x02 /* inverted at port and at device*/
+#define ADDRSTB		0x08 /* inverted at port and at device*/
+#define PORTREAD	0x20
+
+static DEFINE_SPINLOCK(c200_lock);
+
+/* read from controller register */
+u8 hw_readreg(unsigned long base, int reg)
+{
+	unsigned long pa = base;
+	unsigned long pc = pa + 2;
+	unsigned long flags;
+	u8 irqstatus = (inb(pc) & 0x10) | 0x04;
+	u8 val;
+
+	spin_lock_irqsave(&c200_lock, flags);
+
+	outb(irqstatus | ADDRSTB, pc);
+	outb((reg & 0x1F) | 0x80, pa);
+	outb(irqstatus, pc);
+	outb(irqstatus | PORTREAD, pc);
+	outb(irqstatus | DATASTB | PORTREAD, pc);
+	val = inb(pa);
+	outb(irqstatus, pc);
+
+	spin_unlock_irqrestore(&c200_lock, flags);
+
+	return val;
+}
+
+/* write to controller register */
+void hw_writereg(unsigned long base, int reg, u8 val)
+{
+	unsigned long pa = base;
+	unsigned long pc = pa + 2;
+	unsigned long flags;
+	u8 irqstatus = (inb(pc) & 0x10) | 0x04;
+
+	spin_lock_irqsave(&c200_lock, flags);
+
+	outb(irqstatus | ADDRSTB, pc);
+	outb(reg & 0x1F, pa);
+	outb(irqstatus, pc);
+	outb(irqstatus | WRITEP, pc);
+	outb(irqstatus | DATASTB | WRITEP, pc);
+	outb(val, pa);
+	outb(irqstatus, pc);
+
+	spin_unlock_irqrestore(&c200_lock, flags);
+}
+
+/* hardware specific work to do at start of irq handler */
+void hw_preirq(struct net_device *dev) { return; }
+
+/* hardware specific work to do at end of irq handler */
+void hw_postirq(struct net_device *dev) { return; }
diff --git a/drivers/net/can/old/hal/esdio.c b/drivers/net/can/old/hal/esdio.c
new file mode 100644
index 000000000000..a0132e2ed545
--- /dev/null
+++ b/drivers/net/can/old/hal/esdio.c
@@ -0,0 +1,186 @@
+/*
+ * esdio.c - multiplex register access CAN hardware abstraction layer
+ *           for the esd 3xCAN pc104 board
+ *           http://www.esd-electronics.de/products/CAN/can-pc104-200_e.htm
+ *
+ * Inspired by the OCAN driver http://ar.linux.it/software/#ocan
+ *
+ * Copyright (c) 2007 Fraunhofer FOKUS
+ *
+ * Provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ * History:
+ * 2007-05-22 Bjoern Riemer: initial release
+ */
+
+#include <linux/netdevice.h>
+#include <linux/ioport.h>
+#include <asm/io.h>
+#include "hal.h"
+
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+#define DBG(args...) printk(args)
+#else
+#define DBG(args...)
+#endif
+
+//#define DBG(args...)   printk(args)
+
+int esd_ale_offset = 1;	//default for the sja1000 chip
+int esd_cs_offset = 0;	//default for the sja1000 chip
+
+/* init the HAL - call at driver module init */
+int hal_init(void) { return 0; }
+
+/* exit the HAL - call at driver module exit */
+int hal_exit(void) { return 0; }
+
+/* get name of this CAN HAL */
+char *hal_name(void) { return "esdio"; }
+
+/* fill arrays base[] and irq[] with HAL specific defaults */
+void hal_use_defaults(void)
+{
+	extern unsigned long base[];
+	extern unsigned int  irq[];
+
+	base[0]		= 0x1e8UL;
+	irq[0]		= 5;
+}
+
+/* request controller register access space */
+int hal_request_region(int dev_num,
+		       unsigned int num_regs,
+		       char *drv_name)
+{
+	extern unsigned long base[];
+	extern unsigned long rbase[];
+	
+	if (!memcmp(drv_name,"i82527-esdio",sizeof("i82527-esdio"))){
+		esd_ale_offset = 7; 
+		esd_cs_offset = 4;
+	} else if (!memcmp(drv_name,"sja1000-esdio",sizeof("sja1000-esdio"))){
+		esd_ale_offset = 1;
+		esd_cs_offset = 0;
+	}
+	
+	/* set for device base_addr */
+	rbase[dev_num] = base[dev_num];
+
+	/* ignore num_regs and create the 2 register region: */
+	/* address register = base + esd_ale_offset          */
+	/* data register    = base + esd_cs_offset           */
+	if (request_region(base[dev_num] + esd_ale_offset, 1, drv_name)){
+		if (request_region(base[dev_num] + esd_cs_offset, 1,drv_name)){
+			return 1;
+		} else {
+			release_region(base[dev_num]+esd_ale_offset, 1);
+			return 0; // error
+		}
+	}
+
+	return 0; // error 
+}
+
+/* release controller register access space */
+void hal_release_region(int dev_num,
+			unsigned int num_regs)
+{
+	extern unsigned long base[];
+
+	/* ignore num_regs and create the 2 register region: */
+	/* address register = base + esd_ale_offset          */
+	/* data register    = base + esd_cs_offset           */
+	release_region(base[dev_num] + esd_cs_offset, 1);
+	release_region(base[dev_num] + esd_ale_offset, 1);
+}
+
+/* enable non controller hardware (e.g. irq routing, etc.) */
+int hw_attach(int dev_num)
+{
+	int i, stat, i1;
+	extern unsigned long base[];
+	extern unsigned int  irq[];
+	 
+	i1 = irq[dev_num]; //get IRQ number
+	DBG(KERN_INFO "esdio.c: enabling IRQ %d for dev_num %d\n",i1,dev_num);
+	
+	for (i=0; i<4; i++){
+		stat=i; // bit 0,1 selects the latch bit to write
+		if (i1 & 0x01){
+			stat |= 0x80; //bit7 carrys the value of the latch bit
+		}
+		outb(stat,base[dev_num]+3);
+		i1 = i1>>1;
+	}
+
+	outb(0x87,base[dev_num]+3); //enable irq selection
+	outb(0x86,base[dev_num]+3); //enable irq tristate buffer
+
+	return 1; 
+}
+
+/* disable non controller hardware (e.g. irq routing, etc.) */
+int hw_detach(int dev_num)
+{
+	int i;
+	extern unsigned long base[];
+	
+	DBG(KERN_INFO "esdio.c: diabling IRQ for dev_num %d\n",dev_num);
+	
+	outb(0x07,base[dev_num]+3); //disable irq selection
+	outb(0x06,base[dev_num]+3); //disable irq tristate buffer
+	
+	for (i=0; i<4; i++)
+		outb(i,base[dev_num]+3);
+
+	return 1;
+}
+
+/* reset controller hardware (with specific non controller hardware) */
+int hw_reset_dev(int dev_num) {	return 0; }
+
+/* read from controller register */
+u8 hw_readreg(unsigned long base, int reg) {	
+	
+	outb(reg, base + esd_ale_offset);	/* address */
+	return inb(base + esd_cs_offset);	/* data */
+}
+
+/* write to controller register */
+void hw_writereg(unsigned long base, int reg, u8 val) {
+	
+	outb(reg, base + esd_ale_offset);	/* address */
+	outb(val, base + esd_cs_offset);	/* data */
+}
+
+/* hardware specific work to do at start of irq handler */
+void hw_preirq(struct net_device *dev) { return; }
+
+/* hardware specific work to do at end of irq handler */
+void hw_postirq(struct net_device *dev) {	
+
+	outb(0x86,dev->base_addr+3); //enable irq tristate buffer 
+	return; 
+}
diff --git a/drivers/net/can/old/hal/gw2.c b/drivers/net/can/old/hal/gw2.c
new file mode 100644
index 000000000000..893b983e2ec2
--- /dev/null
+++ b/drivers/net/can/old/hal/gw2.c
@@ -0,0 +1,161 @@
+/*
+ * gw2.c - Trajet GW2 register access CAN hardware abstraction layer
+ *
+ * Inspired by the OCAN driver http://ar.linux.it/software/#ocan
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/netdevice.h>
+#include <linux/ioport.h>
+#include <asm/io.h>
+#include "hal.h"
+
+#define ADDR_GAP 1
+
+/* init the HAL - call at driver module init */
+int hal_init(void) { return 0; }
+
+/* exit the HAL - call at driver module exit */
+int hal_exit(void) { return 0; }
+
+/* get name of this CAN HAL */
+char *hal_name(void) { return "gw2"; }
+
+/* fill arrays base[] and irq[] with HAL specific defaults */
+void hal_use_defaults(void)
+{
+	extern unsigned long base[];
+	extern unsigned int  irq[];
+	extern unsigned int  speed[];
+
+	base[0]		= 0xF0100200UL;
+	irq[0]		= 26;
+	speed[0]	= 500;
+
+	base[1]		= 0xF0100300UL;
+	irq[1]		= 26;
+	speed[1]	= 100;
+
+	base[2]		= 0xF0100400UL;
+	irq[2]		= 26;
+	speed[2]	= 100;
+
+	base[3]		= 0xF0100500UL;
+	irq[3]		= 26;
+	speed[3]	= 500;
+}
+
+/* request controller register access space */
+int hal_request_region(int dev_num,
+		       unsigned int num_regs,
+		       char *drv_name)
+{
+	extern unsigned long base[];
+	extern unsigned long rbase[];
+
+	unsigned int gw2_regs = num_regs * (ADDR_GAP + 1);
+
+	/* creating the region for IOMEM is pretty easy */
+	if (!request_mem_region(base[dev_num], gw2_regs, drv_name))
+		return 0; /* failed */
+
+	/* set device base_addr */
+	rbase[dev_num] = (unsigned long)ioremap(base[dev_num], gw2_regs);
+
+	if (rbase[dev_num])
+		return 1; /* success */
+
+	/* cleanup due to failed ioremap() */
+	release_mem_region(base[dev_num], gw2_regs);
+	return 0; /* failed */
+}
+
+/* release controller register access space */
+void hal_release_region(int dev_num,
+			unsigned int num_regs)
+{
+	extern unsigned long base[];
+	extern unsigned long rbase[];
+
+	unsigned int gw2_regs = num_regs * (ADDR_GAP + 1);
+
+	iounmap((void *)rbase[dev_num]);
+	release_mem_region(base[dev_num], gw2_regs);
+}
+
+/* enable non controller hardware (e.g. irq routing, etc.) */
+int hw_attach(int dev_num) { return 0; }
+
+/* disable non controller hardware (e.g. irq routing, etc.) */
+int hw_detach(int dev_num) { return 0; }
+
+/* reset controller hardware (with specific non controller hardware) */
+int hw_reset_dev(int dev_num) { return 0; }
+
+/* read from controller register */
+u8 hw_readreg(unsigned long base, int reg) {
+
+	static u8 val;
+	void __iomem *addr = (void __iomem *)base +
+		reg * (ADDR_GAP + 1) + ADDR_GAP;
+
+	val = (u8)readw(addr);
+	rmb();
+
+        return val;
+}
+
+/* write to controller register */
+void hw_writereg(unsigned long base, int reg, u8 val) {
+
+	void __iomem *addr = (void __iomem *)base +
+		reg * (ADDR_GAP + 1) + ADDR_GAP;
+
+	writew(val, addr);
+	wmb();
+}
+
+/* hardware specific work to do at start of irq handler */
+void hw_preirq(struct net_device *dev) { return; }
+
+/* hardware specific work to do at end of irq handler */
+void hw_postirq(struct net_device *dev) { return; }
+
diff --git a/drivers/net/can/old/hal/hal.h b/drivers/net/can/old/hal/hal.h
new file mode 100644
index 000000000000..1c59b54cdb6a
--- /dev/null
+++ b/drivers/net/can/old/hal/hal.h
@@ -0,0 +1,99 @@
+/*
+ * hal.h - definitions for CAN controller hardware abstraction layer
+ *
+ * Inspired by the OCAN driver http://ar.linux.it/software/#ocan
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#ifndef CAN_HAL_H
+#define CAN_HAL_H
+
+#include <linux/types.h>
+#include <linux/netdevice.h>
+
+/* Number of supported CAN devices for each HAL (default) */
+#define MAXDEV 8
+
+/* general function prototypes for CAN HAL */
+
+/* init the HAL - call at driver module init */
+int hal_init(void);
+
+/* exit the HAL - call at driver module exit */
+int hal_exit(void);
+
+/* get name of this CAN HAL */
+char *hal_name(void);
+
+/* fill arrays base[] and irq[] with HAL specific defaults */
+void hal_use_defaults(void);
+
+/* request controller register access space */
+int hal_request_region(int dev_num,
+		       unsigned int num_regs,
+		       char *drv_name);
+
+/* release controller register access space */
+void hal_release_region(int dev_num,
+			unsigned int num_regs);
+
+/* enable non controller hardware (e.g. irq routing, etc.) */
+int hw_attach(int dev_num);
+
+/* disable non controller hardware (e.g. irq routing, etc.) */
+int hw_detach(int dev_num);
+
+/* reset controller hardware (with specific non controller hardware) */
+int hw_reset_dev(int dev_num);
+
+/* read from controller register */
+u8 hw_readreg(unsigned long base, int reg);
+
+/* write to controller register */
+void hw_writereg(unsigned long base, int reg, u8 val);
+
+/* hardware specific work to do at start of irq handler */
+void hw_preirq(struct net_device *dev);
+
+/* hardware specific work to do at end of irq handler */
+void hw_postirq(struct net_device *dev);
+
+#endif /* CAN_HAL_H */
diff --git a/drivers/net/can/old/hal/io.c b/drivers/net/can/old/hal/io.c
new file mode 100644
index 000000000000..85e3b1edc1bc
--- /dev/null
+++ b/drivers/net/can/old/hal/io.c
@@ -0,0 +1,123 @@
+/*
+ * io.c - linear register access CAN hardware abstraction layer
+ *
+ * Inspired by the OCAN driver http://ar.linux.it/software/#ocan
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/netdevice.h>
+#include <linux/ioport.h>
+#include <asm/io.h>
+#include "hal.h"
+
+/* init the HAL - call at driver module init */
+int hal_init(void) { return 0; }
+
+/* exit the HAL - call at driver module exit */
+int hal_exit(void) { return 0; }
+
+/* get name of this CAN HAL */
+char *hal_name(void) { return "io"; }
+
+/* fill arrays base[] and irq[] with HAL specific defaults */
+void hal_use_defaults(void)
+{
+	extern unsigned long base[];
+	extern unsigned int  irq[];
+
+	base[0]		= 0x2C0UL;
+	irq[0]		= 10;
+
+	base[1]		= 0x320UL;
+	irq[1]		= 5;
+}
+
+/* request controller register access space */
+int hal_request_region(int dev_num,
+		       unsigned int num_regs,
+		       char *drv_name)
+{
+	extern unsigned long base[];
+	extern unsigned long rbase[];
+
+	/* set for device base_addr */
+	rbase[dev_num] = base[dev_num];
+
+	/* creating the region for IO is pretty easy */
+	return (request_region(base[dev_num], num_regs, drv_name))? 1 : 0;
+}
+
+/* release controller register access space */
+void hal_release_region(int dev_num,
+			unsigned int num_regs)
+{
+	extern unsigned long base[];
+
+	release_region(base[dev_num], num_regs);
+}
+
+/* enable non controller hardware (e.g. irq routing, etc.) */
+int hw_attach(int dev_num) { return 0; }
+
+/* disable non controller hardware (e.g. irq routing, etc.) */
+int hw_detach(int dev_num) { return 0; }
+
+/* reset controller hardware (with specific non controller hardware) */
+int hw_reset_dev(int dev_num) { return 0; }
+
+/* read from controller register */
+u8 hw_readreg(unsigned long base, int reg) {
+
+	return inb(base + reg);
+}
+
+/* write to controller register */
+void hw_writereg(unsigned long base, int reg, u8 val) {
+
+	outb(val, base + reg);
+}
+
+/* hardware specific work to do at start of irq handler */
+void hw_preirq(struct net_device *dev) { return; }
+
+/* hardware specific work to do at end of irq handler */
+void hw_postirq(struct net_device *dev) { return; }
+
diff --git a/drivers/net/can/old/hal/iomem.c b/drivers/net/can/old/hal/iomem.c
new file mode 100644
index 000000000000..6a89a9cc2fe0
--- /dev/null
+++ b/drivers/net/can/old/hal/iomem.c
@@ -0,0 +1,142 @@
+/*
+ * iomem.c - linear register access CAN hardware abstraction layer
+ *
+ * Inspired by the OCAN driver http://ar.linux.it/software/#ocan
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/netdevice.h>
+#include <linux/ioport.h>
+#include <asm/io.h>
+#include "hal.h"
+
+/* init the HAL - call at driver module init */
+int hal_init(void) { return 0; }
+
+/* exit the HAL - call at driver module exit */
+int hal_exit(void) { return 0; }
+
+/* get name of this CAN HAL */
+char *hal_name(void) { return "iomem"; }
+
+/* fill arrays base[] and irq[] with HAL specific defaults */
+void hal_use_defaults(void)
+{
+	extern unsigned long base[];
+	extern unsigned int  irq[];
+
+	base[0]		= 0xd8000UL;
+	irq[0]		= 5;
+
+	base[1]		= 0xd8100UL;
+	irq[1]		= 15;
+}
+
+/* request controller register access space */
+int hal_request_region(int dev_num,
+		       unsigned int num_regs,
+		       char *drv_name)
+{
+	extern unsigned long base[];
+	extern unsigned long rbase[];
+
+	/* creating the region for IOMEM is pretty easy */
+	if (!request_mem_region(base[dev_num], num_regs, drv_name))
+		return 0; /* failed */
+
+	/* set device base_addr */
+	rbase[dev_num] = (unsigned long)ioremap(base[dev_num], num_regs);
+
+	if (rbase[dev_num])
+		return 1; /* success */
+
+	/* cleanup due to failed ioremap() */
+	release_mem_region(base[dev_num], num_regs);
+	return 0; /* failed */
+}
+
+/* release controller register access space */
+void hal_release_region(int dev_num,
+			unsigned int num_regs)
+{
+	extern unsigned long base[];
+	extern unsigned long rbase[];
+
+	iounmap((void *)rbase[dev_num]);
+	release_mem_region(base[dev_num], num_regs);
+}
+
+/* enable non controller hardware (e.g. irq routing, etc.) */
+int hw_attach(int dev_num) { return 0; }
+
+/* disable non controller hardware (e.g. irq routing, etc.) */
+int hw_detach(int dev_num) { return 0; }
+
+/* reset controller hardware (with specific non controller hardware) */
+int hw_reset_dev(int dev_num) { return 0; }
+
+/* read from controller register */
+u8 hw_readreg(unsigned long base, int reg) {
+
+	static u8 val;
+	void __iomem *addr = (void __iomem *)base + reg;
+
+	val = (u8)readb(addr);
+	rmb();
+
+        return val;
+}
+
+/* write to controller register */
+void hw_writereg(unsigned long base, int reg, u8 val) {
+
+	void __iomem *addr = (void __iomem *)base + reg;
+
+	writeb(val, addr);
+	wmb();
+}
+
+/* hardware specific work to do at start of irq handler */
+void hw_preirq(struct net_device *dev) { return; }
+
+/* hardware specific work to do at end of irq handler */
+void hw_postirq(struct net_device *dev) { return; }
+
diff --git a/drivers/net/can/old/hal/iomux.c b/drivers/net/can/old/hal/iomux.c
new file mode 100644
index 000000000000..a4e8fba82ddd
--- /dev/null
+++ b/drivers/net/can/old/hal/iomux.c
@@ -0,0 +1,125 @@
+/*
+ * iomux.c - multiplex register access CAN hardware abstraction layer
+ *
+ * Inspired by the OCAN driver http://ar.linux.it/software/#ocan
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/netdevice.h>
+#include <linux/ioport.h>
+#include <asm/io.h>
+#include "hal.h"
+
+/* init the HAL - call at driver module init */
+int hal_init(void) { return 0; }
+
+/* exit the HAL - call at driver module exit */
+int hal_exit(void) { return 0; }
+
+/* get name of this CAN HAL */
+char *hal_name(void) { return "iomux"; }
+
+/* fill arrays base[] and irq[] with HAL specific defaults */
+void hal_use_defaults(void)
+{
+	extern unsigned long base[];
+	extern unsigned int  irq[];
+
+	base[0]		= 0x300UL;
+	irq[0]		= 5;
+}
+
+/* request controller register access space */
+int hal_request_region(int dev_num,
+		       unsigned int num_regs,
+		       char *drv_name)
+{
+	extern unsigned long base[];
+	extern unsigned long rbase[];
+
+	/* set for device base_addr */
+	rbase[dev_num] = base[dev_num];
+
+	/* ignore num_regs and create the 2 register region:  */
+	/* address register = base / data register = base + 1 */
+	return (request_region(base[dev_num], 2, drv_name))? 1 : 0;
+}
+
+/* release controller register access space */
+void hal_release_region(int dev_num,
+			unsigned int num_regs)
+{
+	extern unsigned long base[];
+
+	/* ignore num_regs and create the 2 register region:  */
+	/* address register = base / data register = base + 1 */
+	release_region(base[dev_num], 2);
+}
+
+/* enable non controller hardware (e.g. irq routing, etc.) */
+int hw_attach(int dev_num) { return 0; }
+
+/* disable non controller hardware (e.g. irq routing, etc.) */
+int hw_detach(int dev_num) { return 0; }
+
+/* reset controller hardware (with specific non controller hardware) */
+int hw_reset_dev(int dev_num) { return 0; }
+
+/* read from controller register */
+u8 hw_readreg(unsigned long base, int reg) {
+
+	outb(reg, base);	/* address */
+	return inb(base + 1);	/* data */
+}
+
+/* write to controller register */
+void hw_writereg(unsigned long base, int reg, u8 val) {
+
+	outb(reg, base);	/* address */
+	outb(val, base + 1);	/* data */
+}
+
+/* hardware specific work to do at start of irq handler */
+void hw_preirq(struct net_device *dev) { return; }
+
+/* hardware specific work to do at end of irq handler */
+void hw_postirq(struct net_device *dev) { return; }
+
diff --git a/drivers/net/can/old/hal/pc7io.c b/drivers/net/can/old/hal/pc7io.c
new file mode 100644
index 000000000000..b223efd25f9f
--- /dev/null
+++ b/drivers/net/can/old/hal/pc7io.c
@@ -0,0 +1,126 @@
+/*
+ * pc7io.c - linear register access CAN hardware abstraction layer
+ *
+ * Inspired by the OCAN driver http://ar.linux.it/software/#ocan
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/netdevice.h>
+#include <linux/ioport.h>
+#include <asm/io.h>
+#include "hal.h"
+
+/* init the HAL - call at driver module init */
+int hal_init(void) { return 0; }
+
+/* exit the HAL - call at driver module exit */
+int hal_exit(void) { return 0; }
+
+/* get name of this CAN HAL */
+char *hal_name(void) { return "pc7io"; }
+
+/* fill arrays base[] and irq[] with HAL specific defaults */
+void hal_use_defaults(void)
+{
+	extern unsigned long base[];
+	extern unsigned int  irq[];
+
+	base[0]		= 0x1000UL;
+	irq[0]		= 9;
+}
+
+/* request controller register access space */
+int hal_request_region(int dev_num,
+		       unsigned int num_regs,
+		       char *drv_name)
+{
+	extern unsigned long base[];
+	extern unsigned long rbase[];
+
+	/* set for device base_addr */
+	rbase[dev_num] = base[dev_num];
+
+	/* creating the region for IO is pretty easy */
+	return (request_region(base[dev_num], num_regs, drv_name))? 1 : 0;
+}
+
+/* release controller register access space */
+void hal_release_region(int dev_num,
+			unsigned int num_regs)
+{
+	extern unsigned long base[];
+
+	release_region(base[dev_num], num_regs);
+}
+
+/* enable non controller hardware (e.g. irq routing, etc.) */
+int hw_attach(int dev_num) {
+
+	/* Unlock special function register */
+	outb(5, 0x169);
+
+	return 0;
+}
+
+/* disable non controller hardware (e.g. irq routing, etc.) */
+int hw_detach(int dev_num) { return 0; }
+
+/* reset controller hardware (with specific non controller hardware) */
+int hw_reset_dev(int dev_num) { return 0; }
+
+/* read from controller register */
+u8 hw_readreg(unsigned long base, int reg) {
+
+	return inb(base + reg);
+}
+
+/* write to controller register */
+void hw_writereg(unsigned long base, int reg, u8 val) {
+
+	outb(val, base + reg);
+}
+
+/* hardware specific work to do at start of irq handler */
+void hw_preirq(struct net_device *dev) { return; }
+
+/* hardware specific work to do at end of irq handler */
+void hw_postirq(struct net_device *dev) { return; }
+
diff --git a/drivers/net/can/old/i82527/Makefile b/drivers/net/can/old/i82527/Makefile
new file mode 100644
index 000000000000..eb025a8a10c5
--- /dev/null
+++ b/drivers/net/can/old/i82527/Makefile
@@ -0,0 +1,24 @@
+#
+#
+
+ifeq ($(KERNELRELEASE),)
+
+KERNELDIR := /lib/modules/$(shell uname -r)/build
+PWD       := $(shell pwd)
+TOPDIR    := $(PWD)/../../../../..
+
+modules modules_install clean:
+	$(MAKE) -C $(KERNELDIR) M=$(PWD) $@ TOPDIR=$(TOPDIR)
+
+else
+
+-include $(TOPDIR)/Makefile.common
+EXTRA_CFLAGS += -I$(TOPDIR)/drivers/net/can/old/hal
+
+obj-m := i82527-pc7io.o i82527-iomem.o i82527-esdio.o
+
+i82527-pc7io-objs := i82527.o proc.o ../hal/pc7io.o
+i82527-iomem-objs := i82527.o proc.o ../hal/iomem.o
+i82527-esdio-objs := i82527.o proc.o ../hal/esdio.o
+
+endif
diff --git a/drivers/net/can/old/i82527/i82527.c b/drivers/net/can/old/i82527/i82527.c
new file mode 100644
index 000000000000..d45c4fab22de
--- /dev/null
+++ b/drivers/net/can/old/i82527/i82527.c
@@ -0,0 +1,1251 @@
+/*
+ * i82527.c -  Intel I82527 network device driver
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/fcntl.h>
+#include <linux/interrupt.h>
+#include <linux/ptrace.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/skbuff.h>
+
+#include <linux/can.h>
+#include <linux/can/ioctl.h> /* for struct can_device_stats */
+#include "hal.h"
+#include "i82527.h"
+
+MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("LLCF/socketcan '" CHIP_NAME "' network device driver");
+
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+#define DBG(args...)   ((priv->debug > 0) ? printk(args) : 0)
+/* logging in interrupt context! */
+#define iDBG(args...)  ((priv->debug > 1) ? printk(args) : 0)
+#define iiDBG(args...) ((priv->debug > 2) ? printk(args) : 0)
+#else
+#define DBG(args...)
+#define iDBG(args...)
+#define iiDBG(args...)
+#endif
+
+char drv_name[DRV_NAME_LEN] = "undefined";
+
+/* driver and version information */
+static const char *drv_version	= "0.0.4";
+static const char *drv_reldate	= "2007-08-03";
+
+static const canid_t rxobjflags[] = {0, CAN_EFF_FLAG,
+				     CAN_RTR_FLAG, CAN_RTR_FLAG | CAN_EFF_FLAG,
+				     0, CAN_EFF_FLAG}; 
+#define RXOBJBASE 10
+
+/* array of all can chips */
+struct net_device *can_dev[MAXDEV];
+
+/* module parameters */
+unsigned long base[MAXDEV]	= { 0 }; /* hardware address */
+unsigned long rbase[MAXDEV]	= { 0 }; /* (remapped) device address */
+unsigned int  irq[MAXDEV]	= { 0 };
+
+unsigned int speed[MAXDEV]	= { DEFAULT_SPEED, DEFAULT_SPEED };
+unsigned int btr[MAXDEV]	= { 0 };
+unsigned int bcr[MAXDEV]	= { 0 }; /* bus configuration register */
+unsigned int cdv[MAXDEV]	= { 0 }; /* CLKOUT clock divider */
+unsigned int mo15[MAXDEV]	= { MO15_DEFLT, MO15_DEFLT }; /* msg obj 15 */
+
+static int rx_probe[MAXDEV]	= { 0 };
+static int clk			= DEFAULT_HW_CLK;
+static int force_dmc		= DEFAULT_FORCE_DMC;
+static int irq_mode		= DEFAULT_IRQ_MODE;
+static int debug		= 0;
+static int restart_ms		= 100;
+
+static int base_n;
+static int irq_n;
+static int speed_n;
+static int btr_n;
+static int bcr_n;
+static int cdv_n;
+static int mo15_n;
+static int rx_probe_n;
+
+static u8 dsc; /* devide system clock */
+static u8 dmc; /* devide memory clock */
+static unsigned long irqflags; /* for shared / disabled local interrupts */
+
+module_param_array(base, int, &base_n, 0);
+module_param_array(irq, int, &irq_n, 0);
+module_param_array(speed, int, &speed_n, 0);
+module_param_array(btr, int, &btr_n, 0);
+module_param_array(bcr, int, &bcr_n, 0);
+module_param_array(cdv, int, &cdv_n, 0);
+module_param_array(mo15, int, &mo15_n, 0);
+module_param_array(rx_probe, int, &rx_probe_n, 0);
+
+module_param(clk, int, 0);
+module_param(force_dmc, int, 0);
+module_param(irq_mode, int, 0);
+module_param(debug, int, 0);
+module_param(restart_ms, int, 0);
+
+MODULE_PARM_DESC(base, "CAN controller base address");
+MODULE_PARM_DESC(irq, "CAN controller interrupt");
+MODULE_PARM_DESC(speed, "CAN bus bitrate");
+MODULE_PARM_DESC(btr, "Bit Timing Register value 0x<btr0><btr1>, e.g. 0x4014");
+MODULE_PARM_DESC(bcr, "i82527 bus configuration register value (default: 0)");
+MODULE_PARM_DESC(cdv, "clockout devider value (0-14) (default: 0)");
+MODULE_PARM_DESC(mo15, "rx message object 15 usage. 0:none 1:sff(default) 2:eff");
+MODULE_PARM_DESC(rx_probe, "switch to trx mode after correct msg receiption. (default off)");
+
+MODULE_PARM_DESC(clk, "CAN controller chip clock (default: 16MHz)");
+MODULE_PARM_DESC(force_dmc, "set i82527 DMC bit (default: calculate from clk)"); 
+MODULE_PARM_DESC(irq_mode, "specify irq setup bits (1:shared 2:disable local irqs while processing) (default: 1)"); 
+MODULE_PARM_DESC(debug, "set debug mask (default: 0)");
+MODULE_PARM_DESC(restart_ms, "restart chip on heavy bus errors / bus off after x ms (default 100ms)");
+
+/* function declarations */
+
+static void chipset_init(struct net_device *dev, int wake);
+static void chipset_init_rx(struct net_device *dev);
+static void chipset_init_trx(struct net_device *dev);
+static void can_netdev_setup(struct net_device *dev);
+static struct net_device* can_create_netdev(int dev_num, int hw_regs);
+static int  can_set_drv_name(void);
+int set_reset_mode(struct net_device *dev);
+
+static int i82527_probe_chip(unsigned long base)
+{
+	// Check if hardware reset is still inactive OR
+	// maybe there is no chip in this address space
+	if (CANin(base, cpuInterfaceReg) & iCPU_RST) {
+		printk(KERN_INFO "%s: probing @ 0x%lX failed (reset)\n",
+		       drv_name, base);
+		return 0;
+	}
+
+	// Write test pattern
+	CANout(base, message1Reg.dataReg[1], 0x25);
+	CANout(base, message2Reg.dataReg[3], 0x52);
+	CANout(base, message10Reg.dataReg[6], 0xc3);
+
+	// Read back test pattern
+	if ((CANin(base, message1Reg.dataReg[1]) != 0x25 ) ||
+	    (CANin(base, message2Reg.dataReg[3]) != 0x52 ) ||
+	    (CANin(base, message10Reg.dataReg[6]) != 0xc3 )) {
+		printk(KERN_INFO "%s: probing @ 0x%lX failed (pattern)\n",
+		       drv_name, base);
+		return 0;
+	}
+
+	return 1;
+}
+
+/*
+ * set baud rate divisor values
+ */
+static void set_btr(struct net_device *dev, int btr0, int btr1)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	unsigned long base = dev->base_addr;
+
+	/* no bla bla when restarting the device */
+	if (priv->state == STATE_UNINITIALIZED)
+		printk(KERN_INFO "%s: setting BTR0=%02X BTR1=%02X\n",
+		       dev->name, btr0, btr1);
+
+	CANout(base, bitTiming0Reg, btr0);
+	CANout(base, bitTiming1Reg, btr1);
+}
+
+/*
+ * calculate baud rate divisor values
+ */
+static void set_baud(struct net_device *dev, int baud, int clock)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	int error;
+	int brp;
+	int tseg;
+	int tseg1 = 0;
+	int tseg2 = 0;
+
+	int best_error = 1000000000;
+	int best_tseg = 0;
+	int best_brp = 0;
+	int best_baud = 0;
+
+	int SAM = (baud > 100000 ? 0 : 1);
+
+	if (dsc) /* devide system clock */
+		clock >>= 1; /* calculate BTR with this value */
+
+	for (tseg = (0 + 0 + 2) * 2;
+	     tseg <= (MAX_TSEG2 + MAX_TSEG1 + 2) * 2 + 1;
+	     tseg++) {
+		brp = clock / ((1 + tseg / 2) * baud) + tseg % 2;
+		if ((brp > 0) && (brp <= 64)) {
+			error = baud - clock / (brp * (1 + tseg / 2));
+			if (error < 0) {
+				error = -error;
+			}
+			if (error <= best_error) {
+				best_error = error;
+				best_tseg = tseg / 2;
+				best_brp = brp - 1;
+				best_baud = clock / (brp * (1 + tseg / 2));
+			}
+		}
+	}
+	if (best_error && (baud / best_error < 10)) {
+		printk("%s: unable to set baud rate %d (ext clock %dHz)\n",
+		       dev->name, baud, clock * 2);
+		return;
+//		return -EINVAL;
+	}
+	tseg2 = best_tseg - (SAMPLE_POINT * (best_tseg + 1)) / 100;
+	if (tseg2 < 0) {
+		tseg2 = 0;
+	} else if (tseg2 > MAX_TSEG2) {
+		tseg2 = MAX_TSEG2;
+	}
+	tseg1 = best_tseg - tseg2 - 2;
+	if (tseg1 > MAX_TSEG1) {
+		tseg1 = MAX_TSEG1;
+		tseg2 = best_tseg - tseg1 - 2;
+	}
+
+	priv->btr = ((best_brp | JUMPWIDTH)<<8) + 
+		((SAM << 7) | (tseg2 << 4) | tseg1);
+
+	printk(KERN_INFO "%s: calculated best baudrate: %d / btr is 0x%04X\n",
+	       dev->name, best_baud, priv->btr);
+
+	set_btr(dev, (priv->btr>>8) & 0xFF, priv->btr & 0xFF);
+//	set_btr(dev, best_brp | JUMPWIDTH, (SAM << 7) | (tseg2 << 4) | tseg1);
+}
+
+static inline int obj2rxo(int obj)
+{
+	/* obj4 = obj15 SFF, obj5 = obj15 EFF */ 
+	if (obj < 4)
+		return RXOBJBASE + obj;
+	else
+		return 15;
+}
+
+void enable_rx_obj(unsigned long base, int obj)
+{
+	u8 mcfg = 0;
+	int rxo = obj2rxo(obj);
+
+	// Configure message object for receiption
+	if (rxobjflags[obj] & CAN_EFF_FLAG)
+		mcfg = MCFG_XTD;
+
+	if (rxobjflags[obj] & CAN_RTR_FLAG) {
+		CANout(base, msgArr[rxo].messageReg.messageConfigReg,
+		       mcfg | MCFG_DIR);
+		CANout(base, msgArr[rxo].messageReg.msgCtrl0Reg,
+		       MVAL_SET | TXIE_RES | RXIE_SET | INTPD_RES);
+		CANout(base, msgArr[rxo].messageReg.msgCtrl1Reg,
+		       NEWD_RES | CPUU_SET | TXRQ_RES | RMPD_RES);
+	} else {
+		CANout(base, msgArr[rxo].messageReg.messageConfigReg, mcfg);
+		CANout(base, msgArr[rxo].messageReg.msgCtrl0Reg,
+		       MVAL_SET | TXIE_RES | RXIE_SET | INTPD_RES);
+		CANout(base, msgArr[rxo].messageReg.msgCtrl1Reg,
+		       NEWD_RES | MLST_RES | TXRQ_RES | RMPD_RES);
+	}
+}
+
+void disable_rx_obj(unsigned long base, int obj)
+{
+	int rxo = obj2rxo(obj);
+
+	CANout(base, msgArr[rxo].messageReg.msgCtrl1Reg,
+	       NEWD_RES | MLST_RES | TXRQ_RES | RMPD_RES);
+	CANout(base, msgArr[rxo].messageReg.msgCtrl0Reg,
+	       MVAL_RES | TXIE_RES | RXIE_RES | INTPD_RES);
+}
+
+int set_reset_mode(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	unsigned long base = dev->base_addr;
+
+	// Configure cpu interface
+	CANout(base, cpuInterfaceReg,(dsc | dmc | iCPU_CEN));
+
+	// Enable configuration and puts chip in bus-off, disable interrupts
+	CANout(base, controlReg, iCTL_CCE | iCTL_INI);
+
+	// Clear interrupts
+	CANin(base, interruptReg);
+
+	// Clear status register
+	CANout(base, statusReg, 0);
+
+	// Clear message objects for receiption
+	if (priv->mo15 == MO15_SFF)
+		disable_rx_obj(base, 4); /* rx via obj15 SFF */
+	else
+		disable_rx_obj(base, 0); /* rx via obj10 SFF */
+
+	if (priv->mo15 == MO15_EFF)
+		disable_rx_obj(base, 5); /* rx via obj15 EFF */
+	else
+		disable_rx_obj(base, 1); /* rx via obj11 EFF */
+
+	disable_rx_obj(base, 2);
+	disable_rx_obj(base, 3);
+
+	// Clear message object for send
+	CANout(base, message1Reg.msgCtrl1Reg,
+	       RMPD_RES | TXRQ_RES | CPUU_RES | NEWD_RES);
+	CANout(base, message1Reg.msgCtrl0Reg,
+	       MVAL_RES | TXIE_RES | RXIE_RES | INTPD_RES);
+
+	DBG(KERN_INFO "%s: %s: CtrlReg 0x%x CPUifReg 0x%x\n",
+	    dev->name, __FUNCTION__,
+	    CANin(base, controlReg), CANin(base, cpuInterfaceReg));
+
+	return 0;
+}
+
+static int set_normal_mode(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	unsigned long base = dev->base_addr;
+
+	// Clear interrupts
+	CANin(base, interruptReg);
+
+	// Clear status register
+	CANout(base, statusReg, 0);
+
+	// Configure message objects for receiption
+	if (priv->mo15 == MO15_SFF) {
+		enable_rx_obj(base, 4); /* rx via obj15 SFF */
+		printk(KERN_INFO "%s: %s: using msg object 15 for "
+		       "SFF receiption.\n",
+		       dev->name, CHIP_NAME);
+	} else
+		enable_rx_obj(base, 0); /* rx via obj10 SFF */
+
+	if (priv->mo15 == MO15_EFF) {
+		enable_rx_obj(base, 5); /* rx via obj15 EFF */
+		printk(KERN_INFO "%s: %s: using msg object 15 for "
+		       "EFF receiption.\n",
+		       dev->name, CHIP_NAME);
+	} else
+		enable_rx_obj(base, 1); /* rx via obj11 EFF */
+
+	enable_rx_obj(base, 2);
+	enable_rx_obj(base, 3);
+
+	// Clear message object for send
+	CANout(base, message1Reg.msgCtrl1Reg,
+	       RMPD_RES | TXRQ_RES | CPUU_RES | NEWD_RES);
+	CANout(base, message1Reg.msgCtrl0Reg,
+	       MVAL_RES | TXIE_RES | RXIE_RES | INTPD_RES);
+
+	return 0;
+}
+
+static int set_listen_mode(struct net_device *dev)
+{
+	return set_normal_mode(dev); /* for now */
+}
+
+/*
+ * Clear and invalidate message objects
+ */
+int i82527_clear_msg_objects(unsigned long base)
+{
+    int i;
+    int id;
+    int data;
+
+    for (i = 1; i <= 15; i++) {
+	    CANout(base, msgArr[i].messageReg.msgCtrl0Reg,
+		   INTPD_UNC | RXIE_RES | TXIE_RES | MVAL_RES);
+	    CANout(base, msgArr[i].messageReg.msgCtrl0Reg,
+		   INTPD_RES | RXIE_RES | TXIE_RES | MVAL_RES);
+	    CANout(base, msgArr[i].messageReg.msgCtrl1Reg,
+		   NEWD_RES | MLST_RES | TXRQ_RES | RMPD_RES);
+	    for (data = 0; data < 8; data++)
+		    CANout(base, msgArr[i].messageReg.dataReg[data], 0);
+	    for (id = 0; id < 4; id++)
+		    CANout(base, msgArr[i].messageReg.idReg[id], 0);
+	    CANout(base, msgArr[i].messageReg.messageConfigReg, 0);
+    }
+
+    return 0;
+}
+
+/*
+ * initialize I82527 chip:
+ *   - reset chip
+ *   - set output mode
+ *   - set baudrate
+ *   - enable interrupts
+ *   - start operating mode
+ */
+static void chipset_init_regs(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	unsigned long base = dev->base_addr;
+
+	// Enable configuration and puts chip in bus-off, disable interrupts
+	CANout(base, controlReg, (iCTL_CCE | iCTL_INI));
+
+	// Set CLKOUT devider and slew rates is was done in i82527_init_module
+
+	// Bus configuration was done in i82527_init_module
+
+	// Clear interrupts
+	CANin(base, interruptReg);
+
+	// Clear status register
+	CANout(base, statusReg, 0);
+
+	i82527_clear_msg_objects(base);
+
+	// Set all global ID masks to "don't care"
+	CANout(base, globalMaskStandardReg[0], 0);	
+	CANout(base, globalMaskStandardReg[1], 0);
+	CANout(base, globalMaskExtendedReg[0], 0);
+	CANout(base, globalMaskExtendedReg[1], 0);
+	CANout(base, globalMaskExtendedReg[2], 0);
+	CANout(base, globalMaskExtendedReg[3], 0);
+
+	DBG(KERN_INFO "%s: %s: CtrlReg 0x%x CPUifReg 0x%x\n",
+	    dev->name, __FUNCTION__,
+	    CANin(base, controlReg), CANin(base, cpuInterfaceReg));
+
+	// Note: At this stage the CAN ship is still in bus-off condition
+	// and must be started using StartChip()
+
+	/* set baudrate */
+	if (priv->btr) { /* no calculation when btr is provided */
+		set_btr(dev, (priv->btr>>8) & 0xFF, priv->btr & 0xFF);
+	} else {
+		if (priv->speed == 0) {
+			priv->speed = DEFAULT_SPEED;
+		}
+		set_baud(dev, priv->speed * 1000, priv->clock);
+	}
+
+}
+
+static void chipset_init(struct net_device *dev, int wake)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	if (priv->rx_probe)
+		chipset_init_rx(dev); /* wait for valid reception first */
+	else
+		chipset_init_trx(dev);
+
+	if ((wake) && netif_queue_stopped(dev))
+		netif_wake_queue(dev);
+}
+
+static void chipset_init_rx(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	unsigned long base    = dev->base_addr;
+
+	iDBG(KERN_INFO "%s: %s()\n", dev->name, __FUNCTION__);
+
+	/* set chip into reset mode */
+	set_reset_mode(dev);
+
+	/* set registers */
+	chipset_init_regs(dev);
+
+	/* automatic bit rate detection */
+	set_listen_mode(dev);
+
+	priv->state = STATE_PROBE;
+
+	// Clear bus-off, Interrupts only for errors, not for status change
+	CANout(base, controlReg, iCTL_IE | iCTL_EIE);
+
+	DBG(KERN_INFO "%s: %s: CtrlReg 0x%x CPUifReg 0x%x\n",
+	    dev->name, __FUNCTION__,
+	    CANin(base, controlReg), CANin(base, cpuInterfaceReg));
+}
+
+static void chipset_init_trx(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	unsigned long base    = dev->base_addr;
+
+	iDBG(KERN_INFO "%s: %s()\n", dev->name, __FUNCTION__);
+
+	/* set chip into reset mode */
+	set_reset_mode(dev);
+
+	/* set registers */
+	chipset_init_regs(dev);
+
+	/* leave reset mode */
+	set_normal_mode(dev);
+
+	priv->state = STATE_ACTIVE;
+
+	// Clear bus-off, Interrupts only for errors, not for status change
+	CANout(base, controlReg, iCTL_IE | iCTL_EIE);
+
+	DBG(KERN_INFO "%s: %s: CtrlReg 0x%x CPUifReg 0x%x\n",
+	    dev->name, __FUNCTION__,
+	    CANin(base, controlReg), CANin(base, cpuInterfaceReg));
+}
+
+/*
+ * transmit a CAN message
+ * message layout in the sk_buff should be like this:
+ * xx xx xx xx  ll   00 11 22 33 44 55 66 77
+ * [  can-id ] [len] [can data (up to 8 bytes]
+ */
+static int can_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct net_device_stats *stats = &dev->stats;
+	struct can_frame *cf	= (struct can_frame*)skb->data;
+	unsigned long base	= dev->base_addr;
+	uint8_t	dlc;
+	uint8_t	rtr;
+	canid_t	id;
+	int	i;
+
+	if ((CANin(base, message1Reg.msgCtrl1Reg) & TXRQ_UNC) == TXRQ_SET) {
+		printk(KERN_ERR "%s: %s: TX register is occupied!\n",
+		       dev->name, drv_name);
+		return 0;
+	}
+
+	netif_stop_queue(dev);
+
+	dlc = cf->can_dlc;
+	id  = cf->can_id;
+
+	if ( cf->can_id & CAN_RTR_FLAG )
+		rtr = 0;
+	else
+		rtr = MCFG_DIR;
+
+	CANout(base, message1Reg.msgCtrl1Reg,
+	       RMPD_RES | TXRQ_RES | CPUU_SET | NEWD_RES);
+	CANout(base, message1Reg.msgCtrl0Reg,
+	       MVAL_SET | TXIE_SET | RXIE_RES | INTPD_RES);
+
+	if (id & CAN_EFF_FLAG) {
+		id &= CAN_EFF_MASK;
+		CANout(base, message1Reg.messageConfigReg,
+		       (dlc << 4) + rtr + MCFG_XTD);
+		CANout(base, message1Reg.idReg[3], (id << 3) & 0xFFU);
+		CANout(base, message1Reg.idReg[2], (id >> 5) & 0xFFU);
+		CANout(base, message1Reg.idReg[1], (id >> 13) & 0xFFU);
+		CANout(base, message1Reg.idReg[0], (id >> 21) & 0xFFU);
+	}
+	else {
+		id &= CAN_SFF_MASK;
+		CANout(base, message1Reg.messageConfigReg,
+		       ( dlc << 4 ) + rtr);
+		CANout(base, message1Reg.idReg[0], (id >> 3) & 0xFFU);
+		CANout(base, message1Reg.idReg[1], (id << 5) & 0xFFU);
+	}
+
+	dlc &= 0x0f; //restore length only
+	for ( i=0; i < dlc; i++ ) {
+		CANout(base, message1Reg.dataReg[i],
+		       cf->data[i]);
+	}
+
+	CANout(base, message1Reg.msgCtrl1Reg,
+	       (RMPD_RES | TXRQ_SET | CPUU_RES | NEWD_UNC));
+
+	// HM: We had some cases of repeated IRQs
+	// so make sure the INT is acknowledged
+	// I know it's already further up, but doing again fixed the issue
+	CANout(base, message1Reg.msgCtrl0Reg,
+	       (MVAL_UNC | TXIE_UNC | RXIE_UNC | INTPD_RES));
+
+	stats->tx_bytes += dlc;
+
+	dev->trans_start = jiffies;
+
+	kfree_skb(skb);
+
+	return 0;
+}
+
+static void can_tx_timeout(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+
+	stats->tx_errors++;
+
+	/* do not conflict with e.g. bus error handling */
+	if (!(priv->timer.expires)){ /* no restart on the run */
+		chipset_init_trx(dev); /* no tx queue wakeup */
+		netif_wake_queue(dev); /* wakeup here */
+	}
+	else
+		DBG(KERN_INFO "%s: %s: can_restart_dev already active.\n",
+		    dev->name, __FUNCTION__ );
+
+}
+
+# if 0
+static void can_restart_on(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	if (!(priv->timer.expires)){ /* no restart on the run */
+
+		set_reset_mode(dev);
+
+		priv->timer.function = can_restart_dev;
+		priv->timer.data = (unsigned long) dev;
+
+		/* restart chip on persistent error in <xxx> ms */
+		priv->timer.expires = jiffies + (priv->restart_ms * HZ) / 1000;
+		add_timer(&priv->timer);
+
+		iDBG(KERN_INFO "%s: %s start (%ld)\n",
+		     dev->name, __FUNCTION__ , jiffies);
+	} else
+		iDBG(KERN_INFO "%s: %s already (%ld)\n",
+		     dev->name, __FUNCTION__ , jiffies);
+}
+
+static void can_restart_dev(unsigned long data)
+{
+	struct net_device *dev = (struct net_device*) data;
+	struct can_priv *priv = netdev_priv(dev);
+
+	DBG(KERN_INFO "%s: can_restart_dev (%ld)\n",
+	    dev->name, jiffies);
+
+	/* mark inactive timer */
+	priv->timer.expires = 0;
+
+	if (priv->state != STATE_UNINITIALIZED) {
+
+		/* count number of restarts */
+		priv->can_stats.restarts++;
+
+		chipset_init(dev, 1);
+	}
+}
+#endif
+
+#if 0
+/* the timerless version */
+
+static void can_restart_now(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	if (priv->state != STATE_UNINITIALIZED) {
+
+		/* count number of restarts */
+		priv->can_stats.restarts++;
+
+		chipset_init(dev, 1);
+	}
+}
+#endif
+
+/*
+ * Subroutine of ISR for RX interrupts.
+ *
+ */
+static void can_rx(struct net_device *dev, int obj)
+{
+	struct net_device_stats *stats = &dev->stats;
+	unsigned long base = dev->base_addr;
+	struct can_frame *cf;
+	struct sk_buff	*skb;
+	uint8_t msgctlreg;
+	uint8_t ctl1reg;
+	canid_t	id;
+	uint8_t	dlc;
+	int	i;
+	int	rxo = obj2rxo(obj);
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (skb == NULL) {
+		return;
+	}
+	skb->dev = dev;
+	skb->protocol = htons(ETH_P_CAN);
+	skb->pkt_type = PACKET_BROADCAST;
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+
+	ctl1reg = CANin(base, msgArr[rxo].messageReg.msgCtrl1Reg);
+	msgctlreg = CANin(base, msgArr[rxo].messageReg.messageConfigReg);
+
+	if( msgctlreg & MCFG_XTD ) {
+		id = CANin(base, msgArr[rxo].messageReg.idReg[3])
+			| (CANin(base, msgArr[rxo].messageReg.idReg[2]) << 8)
+			| (CANin(base, msgArr[rxo].messageReg.idReg[1]) << 16)
+			| (CANin(base, msgArr[rxo].messageReg.idReg[0]) << 24);
+		id >>= 3;
+		id |= CAN_EFF_FLAG;
+	} else {
+		id = CANin(base, msgArr[rxo].messageReg.idReg[1])
+			|(CANin(base, msgArr[rxo].messageReg.idReg[0]) << 8);
+		id >>= 5;
+	}
+
+	if (ctl1reg & RMPD_SET) {
+		id |= CAN_RTR_FLAG;
+	}
+
+	msgctlreg  &= 0xf0;/* strip length code */
+	dlc  = msgctlreg >> 4;
+	dlc %= 9;	/* limit count to 8 bytes */
+
+	cf = (struct can_frame*)skb_put(skb, sizeof(struct can_frame));
+	memset(cf, 0, sizeof(struct can_frame));
+	cf->can_id    = id;
+	cf->can_dlc   = dlc;
+	for (i = 0; i < dlc; i++) {
+		cf->data[i] = CANin(base, msgArr[rxo].messageReg.dataReg[i]);
+	}
+
+	// Make the chip ready to receive the next message
+	enable_rx_obj(base, obj);
+
+	netif_rx(skb);
+
+	dev->last_rx = jiffies;
+	stats->rx_packets++;
+	stats->rx_bytes += dlc;
+}
+
+/*
+ * I82527 interrupt handler
+ */
+static irqreturn_t can_interrupt(int irq, void *dev_id)
+{
+	struct net_device *dev	= (struct net_device*)dev_id;
+	struct can_priv *priv	= netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	unsigned long base	= dev->base_addr;
+	uint8_t irqreg;
+	uint8_t lastIrqreg;
+	int n = 0;
+
+	hw_preirq(dev);
+
+	iiDBG(KERN_INFO "%s: interrupt\n", dev->name);
+
+	if (priv->state == STATE_UNINITIALIZED) {
+		printk(KERN_ERR "%s: %s: uninitialized controller!\n",
+		       dev->name, __FUNCTION__);
+		//chipset_init(dev, 1); /* should be possible at this stage */
+		return IRQ_NONE;
+	}
+
+	if (priv->state == STATE_RESET_MODE) {
+		iiDBG(KERN_ERR "%s: %s: controller is in reset mode!\n",
+		      dev->name, __FUNCTION__);
+		return IRQ_NONE;
+	}
+
+     
+	// Read the highest pending interrupt request
+	irqreg = CANin(base, interruptReg);
+	lastIrqreg = irqreg;
+    
+	while ( irqreg ) {
+		n++;
+		switch (irqreg)	{
+
+		case 1: // Status register
+		{
+			uint8_t status;
+
+			// Read the STATUS reg
+			status = CANin(base, statusReg);
+			CANout (base, statusReg, 0);
+
+			if ( status & iSTAT_RXOK ) {
+				// Intel: Software must clear this bit in ISR
+				CANout (base, statusReg, status & ~iSTAT_RXOK);
+			}
+			if ( status & iSTAT_TXOK ) {
+				// Intel: Software must clear this bit in ISR
+				CANout (base, statusReg, status & ~iSTAT_TXOK);
+			}
+			if ( status & iSTAT_WARN ) {
+				// Note: status bit is read-only, don't clear
+				/* error warning interrupt */
+				iDBG(KERN_INFO "%s: error warning\n",
+				     dev->name);
+				priv->can_stats.error_warning++;
+			}
+			if ( status & iSTAT_BOFF ) {
+				uint8_t flags;
+
+				// Note: status bit is read-only, don't clear
+
+				priv->can_stats.bus_error++;
+
+				// Clear init flag and reenable interrupts
+				flags = CANin(base, controlReg) |
+					( iCTL_IE | iCTL_EIE );
+
+				flags &= ~iCTL_INI; // Reset init flag
+				CANout(base, controlReg, flags);
+			}
+		}
+		break;
+
+		case 0x2: // Receiption, message object 15
+		{
+			uint8_t ctl1reg;
+
+			ctl1reg = CANin(base, message15Reg.msgCtrl1Reg);
+			while (ctl1reg & NEWD_SET) {
+				if (ctl1reg & MLST_SET)
+					priv->can_stats.data_overrun++;
+
+				if (priv->mo15 == MO15_SFF)
+					can_rx(dev, 4); /* rx via obj15 SFF */
+				else
+					can_rx(dev, 5); /* rx via obj15 EFF */
+
+				ctl1reg = CANin(base, message15Reg.msgCtrl1Reg);
+			}
+
+			if (priv->state == STATE_PROBE) {
+				/* valid RX -> switch to trx-mode */
+				chipset_init_trx(dev); /* no tx queue wakeup */
+				break; /* check again after init controller */
+			}
+		}
+		break;
+
+		case 0xC: // Receiption, message object 10
+		case 0xD: // Receiption, message object 11
+		{
+			int obj = irqreg - 0xC;
+			int rxo = obj2rxo(obj);
+			uint8_t ctl1reg;
+			ctl1reg = CANin(base, msgArr[rxo].messageReg.msgCtrl1Reg);
+			while (ctl1reg & NEWD_SET) {
+				if (ctl1reg & MLST_SET)
+					priv->can_stats.data_overrun++;
+				CANout(base, msgArr[rxo].messageReg.msgCtrl1Reg,
+				       NEWD_RES | MLST_RES | TXRQ_UNC | RMPD_UNC);
+				can_rx(dev, obj);
+				ctl1reg = CANin(base,
+						msgArr[rxo].messageReg.msgCtrl1Reg);
+			}
+
+			if (priv->state == STATE_PROBE) {
+				/* valid RX -> switch to trx-mode */
+				chipset_init_trx(dev); /* no tx queue wakeup */
+				break; /* check again after init controller */
+			}
+		}
+		break;
+
+		case 0xE: // Receiption, message object 12 (RTR)
+		case 0xF: // Receiption, message object 13 (RTR)
+		{
+			int obj = irqreg - 0xC;
+			int rxo = obj2rxo(obj);
+			uint8_t ctl0reg;
+			ctl0reg = CANin(base, msgArr[rxo].messageReg.msgCtrl0Reg);
+			while (ctl0reg & INTPD_SET) {
+				can_rx(dev, obj);
+				ctl0reg = CANin(base, msgArr[rxo].messageReg.msgCtrl0Reg);
+			}
+
+			if (priv->state == STATE_PROBE) {
+				/* valid RX -> switch to trx-mode */
+				chipset_init_trx(dev); /* no tx queue wakeup */
+				break; /* check again after init controller */
+			}
+		}
+		break;
+
+		case 3: // Message object 1 (our write object)
+			/* transmission complete interrupt */
+
+			// Nothing more to send, switch off interrupts
+			CANout(base, message1Reg.msgCtrl0Reg,
+			       (MVAL_RES | TXIE_RES | RXIE_RES | INTPD_RES));
+			// We had some cases of repeated IRQ
+			// so make sure the INT is acknowledged
+			CANout(base, message1Reg.msgCtrl0Reg,
+			       (MVAL_UNC | TXIE_UNC | RXIE_UNC | INTPD_RES));
+
+			stats->tx_packets++;
+			netif_wake_queue(dev);
+			break;
+
+		default: // Unexpected
+			iDBG(KERN_INFO "%s: Unexpected i82527 interrupt: "
+			     "irqreq=0x%X\n", dev->name, irqreg);
+			break;
+		}
+
+		// Get irq status again for next loop iteration
+		irqreg = CANin(base, interruptReg);
+		if (irqreg == lastIrqreg)
+			iDBG(KERN_INFO "%s: i82527 interrupt repeated: "
+			     "irqreq=0x%X\n", dev->name, irqreg);
+
+		lastIrqreg = irqreg;
+	} /* end while (irqreq) */
+
+	if (n > 1) {
+		iDBG(KERN_INFO "%s: handled %d IRQs\n", dev->name, n);
+	}
+
+	hw_postirq(dev);
+
+	return n == 0 ? IRQ_NONE : IRQ_HANDLED;
+}
+
+/*
+ * initialize CAN bus driver
+ */
+static int can_open(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	/* set chip into reset mode */
+	set_reset_mode(dev);
+
+	priv->state = STATE_UNINITIALIZED;
+
+	/* register interrupt handler */
+	if (request_irq(dev->irq, &can_interrupt, irqflags,
+			dev->name, (void*)dev))
+		return -EAGAIN;
+
+	/* init chip */
+	chipset_init(dev, 0);
+	priv->open_time = jiffies;
+
+	netif_start_queue(dev);
+
+	return 0;
+}
+
+/*
+ * stop CAN bus activity
+ */
+static int can_close(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	/* set chip into reset mode */
+	set_reset_mode(dev);
+
+	priv->open_time = 0;
+
+	if (priv->timer.expires) {
+		del_timer(&priv->timer);
+		priv->timer.expires = 0;
+	}
+
+	free_irq(dev->irq, (void*)dev);
+	priv->state = STATE_UNINITIALIZED;
+
+	netif_stop_queue(dev);
+
+	return 0;
+}
+
+void can_netdev_setup(struct net_device *dev)
+{
+	/* Fill in the the fields of the device structure
+	   with CAN netdev generic values */
+
+	dev->change_mtu			= NULL;
+	dev->set_mac_address		= NULL;
+	dev->header_ops			= NULL;
+
+	dev->type			= ARPHRD_CAN;
+	dev->hard_header_len		= 0;
+	dev->mtu			= sizeof(struct can_frame);
+	dev->addr_len			= 0;
+	dev->tx_queue_len		= 10;
+
+	dev->flags			= IFF_NOARP;
+	dev->features			= NETIF_F_NO_CSUM;
+
+	dev->open			= can_open;
+	dev->stop			= can_close;
+	dev->hard_start_xmit		= can_start_xmit;
+
+	dev->tx_timeout			= can_tx_timeout;
+	dev->watchdog_timeo		= TX_TIMEOUT;
+}
+
+static struct net_device* can_create_netdev(int dev_num, int hw_regs)
+{
+	struct net_device	*dev;
+	struct can_priv		*priv;
+
+	const char mo15mode [3][6] = {"none", "sff", "eff"};
+
+	if (!(dev = alloc_netdev(sizeof(struct can_priv), CAN_NETDEV_NAME,
+				 can_netdev_setup))) {
+		printk(KERN_ERR "%s: out of memory\n", CHIP_NAME);
+		return NULL;
+	}
+
+	printk(KERN_INFO "%s: base 0x%lX / irq %d / speed %d / "
+	       "btr 0x%X / rx_probe %d / mo15 %s\n",
+	       drv_name, rbase[dev_num], irq[dev_num],
+	       speed[dev_num], btr[dev_num], rx_probe[dev_num],
+	       mo15mode[mo15[dev_num]]);
+
+	/* fill net_device structure */
+
+	priv             = netdev_priv(dev);
+
+	dev->irq         = irq[dev_num];
+	dev->base_addr   = rbase[dev_num];
+
+	priv->speed      = speed[dev_num];
+	priv->btr        = btr[dev_num];
+	priv->rx_probe   = rx_probe[dev_num];
+	priv->mo15       = mo15[dev_num];
+	priv->clock      = clk;
+	priv->hw_regs    = hw_regs;
+	priv->restart_ms = restart_ms;
+	priv->debug      = debug;
+
+	init_timer(&priv->timer);
+	priv->timer.expires = 0;
+
+	if (register_netdev(dev)) {
+		printk(KERN_INFO "%s: register netdev failed\n", CHIP_NAME);
+		free_netdev(dev);
+		return NULL;
+	}
+
+	return dev;
+}
+
+int can_set_drv_name(void)
+{
+	char *hname = hal_name();
+
+	if (strlen(CHIP_NAME) + strlen(hname) >= DRV_NAME_LEN-1) {
+		printk(KERN_ERR "%s: driver name too long!\n", CHIP_NAME);
+		return -EINVAL;
+	}
+	sprintf(drv_name, "%s-%s", CHIP_NAME, hname);
+	return 0;
+}
+
+static void i82527_exit_module(void)
+{
+	int i, ret;
+
+	for (i = 0; i < MAXDEV; i++) {
+		if (can_dev[i] != NULL) {
+			struct can_priv *priv = netdev_priv(can_dev[i]);
+			unregister_netdev(can_dev[i]);
+			del_timer(&priv->timer);
+			hw_detach(i);
+			hal_release_region(i, I82527_IO_SIZE);
+			free_netdev(can_dev[i]);
+		}
+	}
+	can_proc_remove(drv_name);
+
+	if ((ret = hal_exit()))
+		printk(KERN_INFO "%s: hal_exit error %d.\n", drv_name, ret);
+}
+
+static __init int i82527_init_module(void)
+{
+	int i, ret;
+	struct net_device *dev;
+
+	if ((sizeof(canmessage_t) != 15) || (sizeof(canregs_t) != 256)) {
+		printk(KERN_WARNING "%s sizes: canmessage_t %d canregs_t %d\n",
+		       CHIP_NAME, (int)sizeof(canmessage_t),
+		       (int)sizeof(canregs_t));
+		return -EBUSY;
+	}
+
+	if ((ret = hal_init()))
+		return ret;
+
+	if ((ret = can_set_drv_name()))
+		return ret;
+
+	if (clk < 1000 ) /* MHz command line value */
+		clk *= 1000000;
+
+	if (clk < 1000000 ) /* kHz command line value */
+		clk *= 1000;
+
+	printk(KERN_INFO "%s driver v%s (%s)\n",
+	       drv_name, drv_version, drv_reldate);
+	printk(KERN_INFO "%s - options [clk %d.%06d MHz] [restart_ms %dms]"
+	       " [debug %d]\n",
+	       drv_name, clk/1000000, clk%1000000, restart_ms, debug);
+	printk(KERN_INFO "%s - options [force_dmc %d] [irq_mode %d]\n",
+	       drv_name, force_dmc, irq_mode);
+
+	if (!base[0]) {
+		printk(KERN_INFO "%s: loading defaults.\n", drv_name);
+		hal_use_defaults();
+	}
+		
+	/* to ensure the proper access to the i82527 registers */
+	/* the timing dependend settings have to be done first */
+	if (clk > 10000000)
+		dsc = iCPU_DSC; /* devide system clock => MCLK is 8MHz save */
+	else if (clk > 8000000) /* 8MHz < clk <= 10MHz */
+		dmc = iCPU_DMC; /* devide memory clock */
+
+	/* devide memory clock even if it's not needed (regarding the spec) */
+	if (force_dmc)
+		dmc = iCPU_DMC;
+
+	if (irq_mode & IRQ_MODE_SHARED)
+		irqflags |= IRQF_SHARED;
+	if (irq_mode & IRQ_MODE_DISABLE_LOCAL_IRQS)
+		irqflags |= IRQF_DISABLED;
+
+	for (i = 0; base[i]; i++) {
+		int clkout;
+		u8 clockdiv;
+
+		printk(KERN_DEBUG "%s: checking for %s on address 0x%lX ...\n",
+		       drv_name, CHIP_NAME, base[i]);
+
+		if (!hal_request_region(i, I82527_IO_SIZE, drv_name)) {
+			printk(KERN_ERR "%s: memory already in use\n",
+			       drv_name);
+			i82527_exit_module();
+			return -EBUSY;
+		}
+
+		hw_attach(i);
+		hw_reset_dev(i);
+
+		// Enable configuration, put chip in bus-off, disable ints
+		CANout(rbase[i], controlReg, iCTL_CCE | iCTL_INI);
+
+		// Configure cpu interface / CLKOUT disable
+		CANout(rbase[i], cpuInterfaceReg,(dsc | dmc));
+
+		if (!i82527_probe_chip(rbase[i])) {
+			printk(KERN_ERR "%s: probably missing controller"
+			       " hardware\n", drv_name);
+			hw_detach(i);
+			hal_release_region(i, I82527_IO_SIZE);
+			i82527_exit_module();
+			return -ENODEV;
+		}
+
+		/* CLKOUT devider and slew rate calculation */
+		if ((cdv[i] < 0) || (cdv[i] > 14)) {
+			printk(KERN_WARNING "%s: adjusted cdv[%d]=%d to 0.\n",
+			       drv_name, i, cdv[i]);
+			cdv[i] = 0;
+		}
+
+		clkout = clk / (cdv[i] + 1); /* CLKOUT frequency */
+		clockdiv = (u8)cdv[i]; /* devider value (see i82527 spec) */
+
+		if (clkout <= 16000000) {
+			clockdiv |= iCLK_SL1;
+			if (clkout <= 8000000)
+				clockdiv |= iCLK_SL0;
+		} else if (clkout <= 24000000)
+				clockdiv |= iCLK_SL0;
+
+		// Set CLKOUT devider and slew rates
+		CANout(rbase[i], clkOutReg, clockdiv);
+
+		// Configure cpu interface / CLKOUT enable
+		CANout(rbase[i], cpuInterfaceReg,(dsc | dmc | iCPU_CEN));
+
+		CANout(rbase[i], busConfigReg, bcr[i]);
+
+		dev = can_create_netdev(i, I82527_IO_SIZE);
+
+		if (dev != NULL) {
+			can_dev[i] = dev;
+			set_reset_mode(dev);
+			can_proc_create(drv_name);
+		} else {
+			can_dev[i] = NULL;
+			hw_detach(i);
+			hal_release_region(i, I82527_IO_SIZE);
+		}
+	}
+	return 0;
+}
+
+module_init(i82527_init_module);
+module_exit(i82527_exit_module);
+
diff --git a/drivers/net/can/old/i82527/i82527.h b/drivers/net/can/old/i82527/i82527.h
new file mode 100644
index 000000000000..0484b21406f9
--- /dev/null
+++ b/drivers/net/can/old/i82527/i82527.h
@@ -0,0 +1,300 @@
+/*
+ * i82527.h -  Intel I82527 network device driver
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * Original version Written by Arnaud Westenberg email:arnaud@wanadoo.nl
+ * This software is released under the GPL-License.
+ *
+ * Major Refactoring and Integration into can4linux version 3.1 by
+ * Henrik W Maier of FOCUS Software Engineering Pty Ltd <www.focus-sw.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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#ifndef I82527_H
+#define I82527_H
+
+#define I82527_IO_SIZE 0x100
+
+#define CHIP_NAME	"i82527"
+
+#define DRV_NAME_LEN	30 /* for "<chip_name>-<hal_name>" */
+
+#define PROCBASE          "driver" /* /proc/ ... */
+
+#define DEFAULT_HW_CLK	16000000
+#define DEFAULT_SPEED	500 /* kBit/s */
+#define DEFAULT_FORCE_DMC 0 /* for critical register access, e.g. ser1274 */
+
+#define IRQ_MODE_SHARED 1 /* enable shared interrupts */
+#define IRQ_MODE_DISABLE_LOCAL_IRQS 2 /* when processing the irq handler */
+#define DEFAULT_IRQ_MODE IRQ_MODE_SHARED
+
+/* The message object 15 has a shadow register for reliable data receiption  */
+/* under heavy bus load. Therefore it makes sense to use this message object */
+/* (mo15) for the needed use case. The frame type (EFF/SFF) for the mo15 can */
+/* be defined on the module command line. The default is 11 bit SFF format.  */
+
+#define MO15_NONE 0
+#define MO15_SFF  1
+#define MO15_EFF  2
+
+#define MO15_DEFLT MO15_SFF /* the default */
+
+#define CAN_NETDEV_NAME	"can%d"
+
+#define TX_TIMEOUT      (50*HZ/1000) /* 50ms */ 
+#define RESTART_MS      100  /* restart chip on persistent errors in 100ms */
+#define MAX_BUS_ERRORS  200  /* prevent from flooding bus error interrupts */
+
+/* bus timing */
+#define MAX_TSEG1	15
+#define MAX_TSEG2	 7
+#define SAMPLE_POINT	62
+#define JUMPWIDTH     0x40
+
+typedef struct canmessage {
+	uint8_t	msgCtrl0Reg;	
+	uint8_t	msgCtrl1Reg;	
+	uint8_t	idReg[4];
+	uint8_t	messageConfigReg;
+	uint8_t	dataReg[8];	
+} canmessage_t; // __attribute__ ((packed));
+
+typedef struct canregs {
+  union
+  {
+    struct
+    {
+      canmessage_t messageReg;
+      uint8_t someOtherReg; // padding
+    } msgArr[16];
+    struct
+    {
+      uint8_t      controlReg;               // Control Register
+      uint8_t      statusReg;                // Status Register
+      uint8_t      cpuInterfaceReg;          // CPU Interface Register
+      uint8_t      reserved1Reg;
+      uint8_t      highSpeedReadReg[2];      // High Speed Read
+      uint8_t      globalMaskStandardReg[2]; // Standard Global Mask byte 0
+      uint8_t      globalMaskExtendedReg[4]; // Extended Global Mask bytes
+      uint8_t      message15MaskReg[4];      // Message 15 Mask bytes
+      canmessage_t message1Reg;
+      uint8_t      clkOutReg;                // Clock Out Register
+      canmessage_t message2Reg;
+      uint8_t      busConfigReg;             // Bus Configuration Register
+      canmessage_t message3Reg;
+      uint8_t      bitTiming0Reg;            // Bit Timing Register byte 0
+      canmessage_t message4Reg;
+      uint8_t      bitTiming1Reg;            // Bit Timing Register byte 1
+      canmessage_t message5Reg;
+      uint8_t      interruptReg;             // Interrupt Register
+      canmessage_t message6Reg;
+      uint8_t      reserved2Reg;
+      canmessage_t message7Reg;
+      uint8_t      reserved3Reg;
+      canmessage_t message8Reg;
+      uint8_t      reserved4Reg;
+      canmessage_t message9Reg;
+      uint8_t      p1ConfReg;
+      canmessage_t message10Reg;
+      uint8_t      p2ConfReg;
+      canmessage_t message11Reg;
+      uint8_t      p1InReg;
+      canmessage_t message12Reg;
+      uint8_t      p2InReg;
+      canmessage_t message13Reg;
+      uint8_t      p1OutReg;
+      canmessage_t message14Reg;
+      uint8_t      p2OutReg;
+      canmessage_t message15Reg;
+      uint8_t      serialResetAddressReg;
+    };
+  };
+} canregs_t; // __attribute__ ((packed));
+
+/* Control Register (0x00) */
+enum i82527_iCTL {
+	iCTL_INI = 1,		// Initialization
+	iCTL_IE  = 1<<1,	// Interrupt Enable
+	iCTL_SIE = 1<<2,	// Status Interrupt Enable
+	iCTL_EIE = 1<<3,	// Error Interrupt Enable
+	iCTL_CCE = 1<<6		// Change Configuration Enable
+};
+
+/* Status Register (0x01) */
+enum i82527_iSTAT {
+	iSTAT_TXOK = 1<<3,	// Transmit Message Successfully
+	iSTAT_RXOK = 1<<4,	// Receive Message Successfully
+	iSTAT_WAKE = 1<<5,	// Wake Up Status
+	iSTAT_WARN = 1<<6,	// Warning Status
+	iSTAT_BOFF = 1<<7	// Bus Off Status
+};
+
+/* CPU Interface Register (0x02) */
+enum i82527_iCPU {
+	iCPU_CEN = 1,		// Clock Out Enable
+	iCPU_MUX = 1<<2,	// Multiplex
+	iCPU_SLP = 1<<3,	// Sleep
+	iCPU_PWD = 1<<4,	// Power Down Mode
+	iCPU_DMC = 1<<5,	// Divide Memory Clock
+	iCPU_DSC = 1<<6,	// Divide System Clock
+	iCPU_RST = 1<<7,	// Hardware Reset Status
+};
+
+/* Clock Out Register (0x1f) */
+enum i82527_iCLK {
+	iCLK_CD0 = 1,		// Clock Divider bit 0
+	iCLK_CD1 = 1<<1,
+	iCLK_CD2 = 1<<2,
+	iCLK_CD3 = 1<<3,
+	iCLK_SL0 = 1<<4,	// Slew Rate bit 0
+	iCLK_SL1 = 1<<5
+};
+
+/* Bus Configuration Register (0x2f) */
+enum i82527_iBUS {
+	iBUS_DR0 = 1,		// Disconnect RX0 Input
+	iBUS_DR1 = 1<<1,	// Disconnect RX1 Input
+	iBUS_DT1 = 1<<3,	// Disconnect TX1 Output
+	iBUS_POL = 1<<5,	// Polarity
+	iBUS_CBY = 1<<6		// Comparator Bypass
+};
+
+#define RESET 1			// Bit Pair Reset Status
+#define SET 2			// Bit Pair Set Status
+#define UNCHANGED 3		// Bit Pair Unchanged
+
+/* Message Control Register 0 (Base Address + 0x0) */
+enum i82527_iMSGCTL0 {
+	INTPD_SET = SET,		// Interrupt pending
+	INTPD_RES = RESET,		// No Interrupt pending
+	INTPD_UNC = UNCHANGED,
+	RXIE_SET  = SET<<2,		// Receive Interrupt Enable
+	RXIE_RES  = RESET<<2,		// Receive Interrupt Disable
+	RXIE_UNC  = UNCHANGED<<2,
+	TXIE_SET  = SET<<4,		// Transmit Interrupt Enable
+	TXIE_RES  = RESET<<4,		// Transmit Interrupt Disable
+	TXIE_UNC  = UNCHANGED<<4,
+	MVAL_SET  = SET<<6,		// Message Valid
+	MVAL_RES  = RESET<<6,		// Message Invalid
+	MVAL_UNC  = UNCHANGED<<6
+};
+
+/* Message Control Register 1 (Base Address + 0x01) */
+enum i82527_iMSGCTL1 {
+	NEWD_SET = SET,			// New Data
+	NEWD_RES = RESET,		// No New Data
+	NEWD_UNC = UNCHANGED,
+	MLST_SET = SET<<2,		// Message Lost
+	MLST_RES = RESET<<2,		// No Message Lost
+	MLST_UNC = UNCHANGED<<2,
+	CPUU_SET = SET<<2,		// CPU Updating
+	CPUU_RES = RESET<<2,		// No CPU Updating
+	CPUU_UNC = UNCHANGED<<2,
+	TXRQ_SET = SET<<4,		// Transmission Request
+	TXRQ_RES = RESET<<4,		// No Transmission Request
+	TXRQ_UNC = UNCHANGED<<4,
+	RMPD_SET = SET<<6,		// Remote Request Pending
+	RMPD_RES = RESET<<6,		// No Remote Request Pending
+	RMPD_UNC = UNCHANGED<<6
+};
+
+/* Message Configuration Register (Base Address + 0x06) */
+enum i82527_iMSGCFG {
+	MCFG_XTD = 1<<2,		// Extended Identifier
+	MCFG_DIR = 1<<3			// Direction is Transmit
+};
+
+#undef IOPRINT
+#undef IODEBUG
+
+#ifdef IOPRINT
+#define CANout(base,adr,v) \
+	printk("CANout: (%lx+%x)=%x\n", base,\
+					(int)(long)&((canregs_t *)0)->adr,v)
+
+#define CANin(base,adr) \
+	printk("CANin: (%lx+%x)\n", base, (int)(long)&((canregs_t *)0)->adr)
+
+#else /* IOPRINT */
+
+#ifdef IODEBUG
+#define CANout(base,adr,v)      \
+	(printk("CANout: (%lx+%x)=%x\n", base,\
+		(int)(long)&((canregs_t *)0)->adr,v),\
+		hw_writereg(base, (int)(long)&((canregs_t *)0)->adr, v))
+#else
+#define CANout(base,adr,v) hw_writereg(base,\
+					(int)(long)&((canregs_t *)0)->adr, v)
+#endif
+
+#define CANin(base,adr)	hw_readreg(base, (int)(long)&((canregs_t *)0)->adr)
+
+#endif /* IOPRINT */
+
+/* CAN private data structure */
+
+struct can_priv {
+	struct can_device_stats	can_stats;
+	long			open_time;
+	int			clock;
+	int			hw_regs;
+	int			restart_ms;
+	int			debug;
+	int			speed;
+	int			btr;
+	int			rx_probe;
+	int			mo15;
+	struct timer_list       timer;
+	int			state;
+};
+
+#define STATE_UNINITIALIZED	0
+#define STATE_PROBE		1
+#define STATE_ACTIVE		2
+#define STATE_ERROR_ACTIVE	3
+#define STATE_ERROR_PASSIVE	4
+#define STATE_BUS_OFF		5
+#define STATE_RESET_MODE	6
+
+void can_proc_create(const char *drv_name);
+void can_proc_remove(const char *drv_name);
+
+#endif /* I82527_H */
diff --git a/drivers/net/can/old/i82527/proc.c b/drivers/net/can/old/i82527/proc.c
new file mode 100644
index 000000000000..e11628fe387c
--- /dev/null
+++ b/drivers/net/can/old/i82527/proc.c
@@ -0,0 +1,209 @@
+/*
+ * proc.c -  proc file system functions for I82527 CAN driver.
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/proc_fs.h>
+#include <linux/netdevice.h>
+
+#include <linux/can.h>
+#include <linux/can/ioctl.h>
+#include "i82527.h"
+#include "hal.h"
+
+extern struct net_device *can_dev[];
+
+static struct proc_dir_entry *pde       = NULL;
+static struct proc_dir_entry *pde_regs  = NULL;
+static struct proc_dir_entry *pde_reset = NULL;
+
+static int can_proc_read_stats(char *page, char **start, off_t off,
+			       int count, int *eof, void *data)
+{
+	int len = 0;
+	int i;
+
+	len += snprintf(page + len, PAGE_SIZE - len,
+			"CAN bus device statistics:\n");
+	len += snprintf(page + len, PAGE_SIZE - len,
+			"       errwarn  overrun   wakeup   buserr   "
+			"errpass  arbitr   restarts clock        baud\n");
+	for (i = 0; (i < MAXDEV) && (len < PAGE_SIZE - 200); i++) {
+		if (can_dev[i]) {
+			struct net_device *dev = can_dev[i];
+			struct can_priv *priv  = netdev_priv(dev);
+			len += snprintf(page + len, PAGE_SIZE - len,
+					"%s: %8d %8d %8d %8d %8d "
+					"%8d %8d %10d %8d\n", dev->name,
+					priv->can_stats.error_warning,
+					priv->can_stats.data_overrun,
+					priv->can_stats.wakeup,
+					priv->can_stats.bus_error,
+					priv->can_stats.error_passive,
+					priv->can_stats.arbitration_lost,
+					priv->can_stats.restarts,
+					priv->clock,
+					priv->speed
+				);
+
+		}
+	}
+
+	*eof = 1;
+	return len;
+}
+
+
+static int can_proc_dump_regs(char *page, int len, struct net_device *dev)
+{
+	int r,s;
+	struct can_priv	*priv = netdev_priv(dev);
+	int regs = priv->hw_regs;
+
+	len += snprintf(page + len, PAGE_SIZE - len,
+			"%s registers:\n", dev->name);
+
+	for (r = 0; r < regs; r += 0x10) {
+		len += snprintf(page + len, PAGE_SIZE - len, "%02X: ", r);
+		for (s = 0; s < 0x10; s++) {
+			if (r+s < regs)
+				len += snprintf(page + len, PAGE_SIZE-len,
+						"%02X ",
+						hw_readreg(dev->base_addr,
+							   r+s));
+		}
+		len += snprintf(page + len, PAGE_SIZE - len, "\n");
+	}
+
+        return len;
+}
+
+static int can_proc_read_regs(char *page, char **start, off_t off,
+				  int count, int *eof, void *data)
+{
+	int len = 0;
+	int i;
+
+	for (i = 0; (i < MAXDEV) && (len < PAGE_SIZE - 200); i++) {
+		if (can_dev[i])
+			len = can_proc_dump_regs(page, len, can_dev[i]);
+	}
+
+	*eof = 1;
+	return len;
+}
+
+static int can_proc_read_reset(char *page, char **start, off_t off,
+				   int count, int *eof, void *data)
+{
+	int len = 0;
+	struct net_device *dev;
+	int i;
+	struct can_priv   *priv;
+
+	len += snprintf(page + len, PAGE_SIZE - len, "resetting ");
+	for (i = 0; (i < MAXDEV) && (len < PAGE_SIZE - 200); i++) {
+		if (can_dev[i]) {
+			dev = can_dev[i];
+			priv = netdev_priv(can_dev[i]);
+			if ((priv->state != STATE_UNINITIALIZED)
+			    && (priv->state != STATE_RESET_MODE)) {
+				len += snprintf(page + len, PAGE_SIZE - len,
+						"%s ", dev->name);
+				dev->stop(dev);
+				dev->open(dev);
+				/* count number of restarts */
+				priv->can_stats.restarts++;
+
+			} else {
+				len += snprintf(page + len, PAGE_SIZE - len,
+						"(%s|%d) ", dev->name,
+						priv->state);
+			}
+		}
+	}
+
+	len += snprintf(page + len, PAGE_SIZE - len, "done\n");
+
+	*eof = 1;
+	return len;
+}
+
+void can_proc_create(const char *drv_name)
+{
+	char fname[256];
+
+	if (pde == NULL) {
+		sprintf(fname, PROCBASE "/%s_stats", drv_name);
+		pde = create_proc_read_entry(fname, 0644, NULL,
+					     can_proc_read_stats, NULL);
+	}
+	if (pde_regs == NULL) {
+		sprintf(fname, PROCBASE "/%s_regs", drv_name);
+		pde_regs = create_proc_read_entry(fname, 0644, NULL,
+						  can_proc_read_regs, NULL);
+	}
+	if (pde_reset == NULL) {
+		sprintf(fname, PROCBASE "/%s_reset", drv_name);
+		pde_reset = create_proc_read_entry(fname, 0644, NULL,
+						   can_proc_read_reset, NULL);
+	}
+}
+
+void can_proc_remove(const char *drv_name)
+{
+	char fname[256];
+
+	if (pde) {
+		sprintf(fname, PROCBASE "/%s_stats", drv_name);
+		remove_proc_entry(fname, NULL);
+	}
+	if (pde_regs) {
+		sprintf(fname, PROCBASE "/%s_regs", drv_name);
+		remove_proc_entry(fname, NULL);
+	}
+	if (pde_reset) {
+		sprintf(fname, PROCBASE "/%s_reset", drv_name);
+		remove_proc_entry(fname, NULL);
+	}
+}
diff --git a/drivers/net/can/old/mscan/Makefile b/drivers/net/can/old/mscan/Makefile
new file mode 100644
index 000000000000..854012e9174d
--- /dev/null
+++ b/drivers/net/can/old/mscan/Makefile
@@ -0,0 +1,21 @@
+#
+#
+
+ifeq ($(KERNELRELEASE),)
+
+KERNELDIR := /lib/modules/$(shell uname -r)/build
+PWD       := $(shell pwd)
+TOPDIR    := $(PWD)/../../../..
+
+modules modules_install clean:
+	$(MAKE) -C $(KERNELDIR) M=$(PWD) $@ TOPDIR=$(TOPDIR)
+
+else
+
+-include $(TOPDIR)/Makefile.common
+
+obj-$(CONFIG_CAN_MPC52XX_OLD)	+= mscan-mpc52xx-old.o
+
+mscan-mpc52xx-old-objs	:= mscan.o mpc52xx_can.o
+
+endif
diff --git a/drivers/net/can/old/mscan/mpc52xx_can.c b/drivers/net/can/old/mscan/mpc52xx_can.c
new file mode 100644
index 000000000000..da1278fa630a
--- /dev/null
+++ b/drivers/net/can/old/mscan/mpc52xx_can.c
@@ -0,0 +1,248 @@
+/*
+ * DESCRIPTION:
+ *  CAN bus driver for the Freescale MPC52xx embedded CPU.
+ *
+ * AUTHOR:
+ *  Andrey Volkov <avolkov@varma-el.com>
+ *
+ * COPYRIGHT:
+ *  2004-2005, Varma Electronics Oy
+ *
+ * LICENCE:
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * HISTORY:
+ *	 2005-02-03 created
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/netdevice.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <asm/io.h>
+#include <asm/mpc52xx.h>
+
+#include "mscan.h"
+
+
+#define PDEV_MAX 2
+
+struct platform_device *pdev[PDEV_MAX];
+
+static int __devinit mpc52xx_can_probe(struct platform_device *pdev)
+{
+	struct resource *mem;
+	struct net_device *dev;
+	struct mscan_platform_data *pdata = pdev->dev.platform_data;
+	struct can_priv *can;
+	u32 mem_size;
+	int ret = -ENODEV;
+
+	if (!pdata)
+		return ret;
+
+	dev = alloc_mscandev();
+	if (!dev)
+		return -ENOMEM;
+	can = netdev_priv(dev);
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dev->irq = platform_get_irq(pdev, 0);
+	if (!mem || !dev->irq)
+		goto req_error;
+
+	mem_size = mem->end - mem->start + 1;
+	if (!request_mem_region(mem->start, mem_size, pdev->dev.driver->name)) {
+		dev_err(&pdev->dev, "resource unavailable\n");
+		goto req_error;
+	}
+
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	dev->base_addr = (unsigned long)ioremap_nocache(mem->start, mem_size);
+
+	if (!dev->base_addr) {
+		dev_err(&pdev->dev, "failed to map can port\n");
+		ret = -ENOMEM;
+		goto fail_map;
+	}
+
+	can->can_sys_clock = pdata->clock_frq;
+
+	platform_set_drvdata(pdev, dev);
+
+	ret = register_mscandev(dev, pdata->clock_src);
+	if (ret >= 0) {
+		dev_info(&pdev->dev, "probe for a port 0x%lX done\n",
+			 dev->base_addr);
+		return ret;
+	}
+
+	iounmap((unsigned long *)dev->base_addr);
+      fail_map:
+	release_mem_region(mem->start, mem_size);
+      req_error:
+	free_candev(dev);
+	dev_err(&pdev->dev, "probe failed\n");
+	return ret;
+}
+
+static int __devexit mpc52xx_can_remove(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct resource *mem;
+
+	platform_set_drvdata(pdev, NULL);
+	unregister_mscandev(dev);
+
+	iounmap((volatile void __iomem *)dev->base_addr);
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(mem->start, mem->end - mem->start + 1);
+	free_candev(dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static struct mscan_regs saved_regs;
+static int mpc52xx_can_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+
+	_memcpy_fromio(&saved_regs, regs, sizeof(*regs));
+
+	return 0;
+}
+
+static int mpc52xx_can_resume(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+
+	regs->canctl0 |= MSCAN_INITRQ;
+	while ((regs->canctl1 & MSCAN_INITAK) == 0)
+		udelay(10);
+
+	regs->canctl1 = saved_regs.canctl1;
+	regs->canbtr0 = saved_regs.canbtr0;
+	regs->canbtr1 = saved_regs.canbtr1;
+	regs->canidac = saved_regs.canidac;
+
+	/* restore masks, buffers etc. */
+	_memcpy_toio(&regs->canidar1_0, (void *)&saved_regs.canidar1_0,
+		     sizeof(*regs) - offsetof(struct mscan_regs, canidar1_0));
+
+	regs->canctl0 &= ~MSCAN_INITRQ;
+	regs->cantbsel = saved_regs.cantbsel;
+	regs->canrier = saved_regs.canrier;
+	regs->cantier = saved_regs.cantier;
+	regs->canctl0 = saved_regs.canctl0;
+
+	return 0;
+}
+#endif
+
+static struct platform_driver mpc52xx_can_driver = {
+	.driver = {
+		   .name = "mpc52xx-mscan",
+		   },
+	.probe = mpc52xx_can_probe,
+	.remove = __devexit_p(mpc52xx_can_remove),
+#ifdef CONFIG_PM
+	.suspend = mpc52xx_can_suspend,
+	.resume = mpc52xx_can_resume,
+#endif
+};
+
+#ifdef CONFIG_PPC_MERGE
+static int __init mpc52xx_of_to_pdev(void)
+{
+	struct device_node *np = NULL;
+	unsigned int i;
+	int err = -ENODEV;
+
+	for (i = 0;
+	     (np = of_find_compatible_node(np, NULL, "fsl,mpc5200-mscan"));
+	     i++) {
+		struct resource r[2] = { };
+		struct mscan_platform_data pdata;
+
+		if (i >= PDEV_MAX) {
+			printk(KERN_WARNING "%s: increase PDEV_MAX for more "
+			       "than %i devices\n", __func__, PDEV_MAX);
+			break;
+		}
+
+		err = of_address_to_resource(np, 0, &r[0]);
+		if (err)
+			break;
+
+		of_irq_to_resource(np, 0, &r[1]);
+
+		pdev[i] =
+		    platform_device_register_simple("mpc52xx-mscan", i, r, 2);
+		if (IS_ERR(pdev[i])) {
+			err = PTR_ERR(pdev[i]);
+			break;
+		}
+
+		pdata.clock_src = MSCAN_CLKSRC_BUS;
+		pdata.clock_frq = mpc52xx_find_ipb_freq(np);
+		err = platform_device_add_data(pdev[i], &pdata, sizeof(pdata));
+		if (err)
+			break;
+	}
+	return err;
+}
+#else
+#define mscan_of_to_pdev()
+#endif
+
+int __init mpc52xx_can_init(void)
+{
+	printk(KERN_WARNING
+	       "This %s driver is DEPRECATED, please switch!\n",
+	       mpc52xx_can_driver.driver.name);
+#ifdef CONFIG_PPC_MERGE
+	int err = mpc52xx_of_to_pdev();
+
+	if (err) {
+		printk(KERN_ERR "%s init failed with err=%d\n",
+		       mpc52xx_can_driver.driver.name, err);
+		return err;
+	}
+#endif
+	return platform_driver_register(&mpc52xx_can_driver);
+}
+
+void __exit mpc52xx_can_exit(void)
+{
+	int i;
+	platform_driver_unregister(&mpc52xx_can_driver);
+	for (i = 0; i < PDEV_MAX; i++)
+		platform_device_unregister(pdev[i]);
+	printk(KERN_INFO "%s unloaded\n", mpc52xx_can_driver.driver.name);
+}
+
+module_init(mpc52xx_can_init);
+module_exit(mpc52xx_can_exit);
+
+MODULE_AUTHOR("Andrey Volkov <avolkov@varma-el.com>");
+MODULE_DESCRIPTION("Freescale MPC5200 CAN driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/can/old/mscan/mscan.c b/drivers/net/can/old/mscan/mscan.c
new file mode 100644
index 000000000000..05afeae1e2fe
--- /dev/null
+++ b/drivers/net/can/old/mscan/mscan.c
@@ -0,0 +1,708 @@
+/*
+ * mscan.c
+ *
+ * DESCRIPTION:
+ *  CAN bus driver for the alone generic (as possible as) MSCAN controller.
+ *
+ * AUTHOR:
+ *  Andrey Volkov <avolkov@varma-el.com>
+ *
+ * COPYRIGHT:
+ *  2005-2006, Varma Electronics Oy
+ *
+ * LICENCE:
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/can.h>
+#include <linux/list.h>
+#include <asm/io.h>
+
+#include <linux/can/dev.h>
+#include <linux/can/error.h>
+#include "mscan.h"
+
+#define MSCAN_NORMAL_MODE	0
+#define MSCAN_SLEEP_MODE	MSCAN_SLPRQ
+#define MSCAN_INIT_MODE		(MSCAN_INITRQ | MSCAN_SLPRQ)
+#define MSCAN_POWEROFF_MODE	(MSCAN_CSWAI | MSCAN_SLPRQ)
+#define MSCAN_SET_MODE_RETRIES	255
+
+
+#define BTR0_BRP_MASK		0x3f
+#define BTR0_SJW_SHIFT		6
+#define BTR0_SJW_MASK		(0x3 << BTR0_SJW_SHIFT)
+
+#define BTR1_TSEG1_MASK 	0xf
+#define BTR1_TSEG2_SHIFT	4
+#define BTR1_TSEG2_MASK 	(0x7 << BTR1_TSEG2_SHIFT)
+#define BTR1_SAM_SHIFT  	7
+
+#define BTR0_SET_BRP(brp)	(((brp) - 1) & BTR0_BRP_MASK)
+#define BTR0_SET_SJW(sjw)	((((sjw) - 1) << BTR0_SJW_SHIFT) & \
+				 BTR0_SJW_MASK)
+
+#define BTR1_SET_TSEG1(tseg1)	(((tseg1) - 1) &  BTR1_TSEG1_MASK)
+#define BTR1_SET_TSEG2(tseg2)	((((tseg2) - 1) << BTR1_TSEG2_SHIFT) & \
+				 BTR1_TSEG2_MASK)
+#define BTR1_SET_SAM(sam)	(((sam) & 1) << BTR1_SAM_SHIFT)
+
+struct mscan_state {
+	u8 mode;
+	u8 canrier;
+	u8 cantier;
+};
+
+#define TX_QUEUE_SIZE	3
+
+typedef struct {
+	struct list_head list;
+	u8 mask;
+} tx_queue_entry_t;
+
+struct mscan_priv {
+	struct can_priv can;
+	volatile unsigned long flags;
+	u8 shadow_statflg;
+	u8 shadow_canrier;
+	u8 cur_pri;
+	u8 tx_active;
+
+	struct list_head tx_head;
+	tx_queue_entry_t tx_queue[TX_QUEUE_SIZE];
+	struct napi_struct napi;
+	struct net_device *dev;
+};
+
+#define F_RX_PROGRESS	0
+#define F_TX_PROGRESS	1
+#define F_TX_WAIT_ALL	2
+
+static int mscan_set_mode(struct net_device *dev, u8 mode)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	int ret = 0;
+	int i;
+	u8 canctl1;
+
+	if (mode != MSCAN_NORMAL_MODE) {
+		canctl1 = in_8(&regs->canctl1);
+		if ((mode & MSCAN_SLPRQ) && (canctl1 & MSCAN_SLPAK) == 0) {
+			out_8(&regs->canctl0,
+			      in_8(&regs->canctl0) | MSCAN_SLPRQ);
+			for (i = 0; i < MSCAN_SET_MODE_RETRIES; i++) {
+				if (in_8(&regs->canctl1) & MSCAN_SLPAK)
+					break;
+				udelay(100);
+			}
+			if (i >= MSCAN_SET_MODE_RETRIES)
+				ret = -ENODEV;
+		}
+
+		if (!ret && (mode & MSCAN_INITRQ)
+		    && (canctl1 & MSCAN_INITAK) == 0) {
+			out_8(&regs->canctl0,
+			      in_8(&regs->canctl0) | MSCAN_INITRQ);
+			for (i = 0; i < MSCAN_SET_MODE_RETRIES; i++) {
+				if (in_8(&regs->canctl1) & MSCAN_INITAK)
+					break;
+			}
+			if (i >= MSCAN_SET_MODE_RETRIES)
+				ret = -ENODEV;
+		}
+
+		if (!ret && (mode & MSCAN_CSWAI))
+			out_8(&regs->canctl0,
+			      in_8(&regs->canctl0) | MSCAN_CSWAI);
+
+	} else {
+		canctl1 = in_8(&regs->canctl1);
+		if (canctl1 & (MSCAN_SLPAK | MSCAN_INITAK)) {
+			out_8(&regs->canctl0, in_8(&regs->canctl0) &
+			      ~(MSCAN_SLPRQ | MSCAN_INITRQ));
+			for (i = 0; i < MSCAN_SET_MODE_RETRIES; i++) {
+				canctl1 = in_8(&regs->canctl1);
+				if (!(canctl1 & (MSCAN_INITAK | MSCAN_SLPAK)))
+					break;
+			}
+			if (i >= MSCAN_SET_MODE_RETRIES)
+				ret = -ENODEV;
+		}
+	}
+	return ret;
+}
+
+static void mscan_push_state(struct net_device *dev, struct mscan_state *state)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+
+	state->mode = in_8(&regs->canctl0) & (MSCAN_SLPRQ | MSCAN_INITRQ |
+					      MSCAN_CSWAI);
+	state->canrier = in_8(&regs->canrier);
+	state->cantier = in_8(&regs->cantier);
+}
+
+static int mscan_pop_state(struct net_device *dev, struct mscan_state *state)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+
+	int ret;
+	ret = mscan_set_mode(dev, state->mode);
+	if (!ret) {
+		out_8(&regs->canrier, state->canrier);
+		out_8(&regs->cantier, state->cantier);
+	}
+	return ret;
+}
+
+static int mscan_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct can_frame *frame = (struct can_frame *)skb->data;
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	struct mscan_priv *priv = netdev_priv(dev);
+
+	int i, rtr, buf_id;
+	u32 can_id;
+
+	if (frame->can_dlc > 8)
+		return -EINVAL;
+
+	dev_dbg(ND2D(dev), "%s\n", __FUNCTION__);
+	out_8(&regs->cantier, 0);
+
+	i = ~priv->tx_active & MSCAN_TXE;
+	buf_id = ffs(i) - 1;
+	switch (hweight8(i)) {
+	case 0:
+		netif_stop_queue(dev);
+		dev_err(ND2D(dev), "BUG! Tx Ring full when queue awake!\n");
+		return NETDEV_TX_BUSY;
+	case 1:
+		/* if buf_id < 3, then current frame will be send out of order,
+		   since  buffer with lower id have higher priority (hell..) */
+		if (buf_id < 3)
+			priv->cur_pri++;
+		if (priv->cur_pri == 0xff)
+			set_bit(F_TX_WAIT_ALL, &priv->flags);
+		netif_stop_queue(dev);
+	case 2:
+		set_bit(F_TX_PROGRESS, &priv->flags);
+	}
+	out_8(&regs->cantbsel, i);
+
+	rtr = frame->can_id & CAN_RTR_FLAG;
+
+	if (frame->can_id & CAN_EFF_FLAG) {
+		dev_dbg(ND2D(dev), "sending extended frame\n");
+
+		can_id = (frame->can_id & CAN_EFF_MASK) << 1;
+		if (rtr)
+			can_id |= 1;
+		out_be16(&regs->tx.idr3_2, can_id);
+
+		can_id >>= 16;
+		can_id = (can_id & 0x7) | ((can_id << 2) & 0xffe0) | (3 << 3);
+	} else {
+		dev_dbg(ND2D(dev), "sending standard frame\n");
+		can_id = (frame->can_id & CAN_SFF_MASK) << 5;
+		if (rtr)
+			can_id |= 1 << 4;
+	}
+	out_be16(&regs->tx.idr1_0, can_id);
+
+	if (!rtr) {
+		volatile void __iomem *data = &regs->tx.dsr1_0;
+		u16 *payload = (u16 *) frame->data;
+		/*Its safe to write into dsr[dlc+1] */
+		for (i = 0; i < (frame->can_dlc + 1) / 2; i++) {
+			out_be16(data, *payload++);
+			data += 2 + _MSCAN_RESERVED_DSR_SIZE;
+		}
+	}
+
+	out_8(&regs->tx.dlr, frame->can_dlc);
+	out_8(&regs->tx.tbpr, priv->cur_pri);
+
+	/* Start transmission. */
+	out_8(&regs->cantflg, 1 << buf_id);
+
+	if (!test_bit(F_TX_PROGRESS, &priv->flags))
+		dev->trans_start = jiffies;
+
+	list_add_tail(&priv->tx_queue[buf_id].list, &priv->tx_head);
+
+	kfree_skb(skb);
+
+	/* Enable interrupt. */
+	priv->tx_active |= 1 << buf_id;
+	out_8(&regs->cantier, priv->tx_active);
+
+	return NETDEV_TX_OK;
+}
+
+static void mscan_tx_timeout(struct net_device *dev)
+{
+	struct sk_buff *skb;
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	struct mscan_priv *priv = netdev_priv(dev);
+	struct can_frame *frame;
+	u8 mask;
+
+	printk("%s\n", __FUNCTION__);
+
+	out_8(&regs->cantier, 0);
+
+	mask = list_entry(priv->tx_head.next, tx_queue_entry_t, list)->mask;
+	dev->trans_start = jiffies;
+	out_8(&regs->cantarq, mask);
+	out_8(&regs->cantier, priv->tx_active);
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (!skb) {
+		if (printk_ratelimit())
+			dev_notice(ND2D(dev), "TIMEOUT packet dropped\n");
+		return;
+	}
+	frame = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
+
+	frame->can_id = CAN_ERR_FLAG | CAN_ERR_TX_TIMEOUT;
+	frame->can_dlc = CAN_ERR_DLC;
+
+	skb->dev = dev;
+	skb->protocol = __constant_htons(ETH_P_CAN);
+	skb->pkt_type = PACKET_BROADCAST;
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+
+	netif_rx(skb);
+
+}
+
+static can_state_t state_map[] = {
+	CAN_STATE_ACTIVE,
+	CAN_STATE_BUS_WARNING,
+	CAN_STATE_BUS_PASSIVE,
+	CAN_STATE_BUS_OFF
+};
+
+static inline int check_set_state(struct net_device *dev, u8 canrflg)
+{
+	struct mscan_priv *priv = netdev_priv(dev);
+	can_state_t state;
+	int ret = 0;
+
+	if (!(canrflg & MSCAN_CSCIF) || priv->can.state > CAN_STATE_BUS_OFF)
+		return 0;
+
+	state =
+	    state_map[max(MSCAN_STATE_RX(canrflg), MSCAN_STATE_TX(canrflg))];
+	if (priv->can.state < state)
+		ret = 1;
+	if (state == CAN_STATE_BUS_OFF)
+		netif_carrier_off(dev);
+	else if (priv->can.state == CAN_STATE_BUS_OFF
+		 && state != CAN_STATE_BUS_OFF)
+		netif_carrier_on(dev);
+	priv->can.state = state;
+	return ret;
+}
+
+static int mscan_rx_poll(struct napi_struct *napi, int quota)
+{
+	struct mscan_priv *priv = container_of(napi, struct mscan_priv, napi);
+	struct net_device *dev = priv->dev;
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	struct net_device_stats *stats = dev->get_stats(dev);
+	int npackets = 0;
+	int ret = 1;
+	struct sk_buff *skb;
+	struct can_frame *frame;
+	u32 can_id;
+	u8 canrflg;
+	int i;
+
+	while (npackets < quota && ((canrflg = in_8(&regs->canrflg)) &
+				    (MSCAN_RXF | MSCAN_ERR_IF))) {
+
+		skb = dev_alloc_skb(sizeof(struct can_frame));
+		if (!skb) {
+			if (printk_ratelimit())
+				dev_notice(ND2D(dev), "packet dropped\n");
+			stats->rx_dropped++;
+			out_8(&regs->canrflg, canrflg);
+			continue;
+		}
+
+		frame = (struct can_frame *)skb_put(skb,
+						    sizeof(struct can_frame));
+
+		if (canrflg & MSCAN_RXF) {
+			can_id = in_be16(&regs->rx.idr1_0);
+			if (can_id & (1 << 3)) {
+				frame->can_id = CAN_EFF_FLAG;
+				can_id = ((can_id << 16) |
+					  in_be16(&regs->rx.idr3_2));
+				can_id = ((can_id & 0xffe00000) |
+					  ((can_id & 0x7ffff) << 2)) >> 2;
+			} else {
+				can_id >>= 4;
+				frame->can_id = 0;
+			}
+
+			frame->can_id |= can_id >> 1;
+			if (can_id & 1)
+				frame->can_id |= CAN_RTR_FLAG;
+			frame->can_dlc = in_8(&regs->rx.dlr) & 0xf;
+
+			if (!(frame->can_id & CAN_RTR_FLAG)) {
+				volatile void __iomem *data = &regs->rx.dsr1_0;
+				u16 *payload = (u16 *) frame->data;
+				for (i = 0; i < (frame->can_dlc + 1) / 2; i++) {
+					*payload++ = in_be16(data);
+					data += 2 + _MSCAN_RESERVED_DSR_SIZE;
+				}
+			}
+
+			dev_dbg(ND2D(dev),
+				"received pkt: id: %u dlc: %u data: ",
+				frame->can_id, frame->can_dlc);
+#ifdef DEBUG
+			for (i = 0;
+			     i < frame->can_dlc && !(frame->can_id &
+						     CAN_FLAG_RTR); i++)
+				printk("%2x ", frame->data[i]);
+			printk("\n");
+#endif
+
+			out_8(&regs->canrflg, MSCAN_RXF);
+			dev->last_rx = jiffies;
+			stats->rx_packets++;
+			stats->rx_bytes += frame->can_dlc;
+		} else if (canrflg & MSCAN_ERR_IF) {
+			frame->can_id = CAN_ERR_FLAG;
+
+			if (canrflg & MSCAN_OVRIF) {
+				frame->can_id |= CAN_ERR_CRTL;
+				frame->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+				stats->rx_over_errors++;
+			} else
+				frame->data[1] = 0;
+
+			if (check_set_state(dev, canrflg)) {
+				frame->can_id |= CAN_ERR_CRTL;
+				switch (priv->can.state) {
+				case CAN_STATE_BUS_WARNING:
+					if ((priv->shadow_statflg &
+					     MSCAN_RSTAT_MSK) <
+					    (canrflg & MSCAN_RSTAT_MSK))
+						frame->data[1] |=
+						    CAN_ERR_CRTL_RX_WARNING;
+
+					if ((priv->shadow_statflg &
+					     MSCAN_TSTAT_MSK) <
+					    (canrflg & MSCAN_TSTAT_MSK))
+						frame->data[1] |=
+							CAN_ERR_CRTL_TX_WARNING;
+					break;
+				case CAN_STATE_BUS_PASSIVE:
+					frame->data[1] |=
+					    CAN_ERR_CRTL_RX_PASSIVE;
+					break;
+				case CAN_STATE_BUS_OFF:
+					frame->can_id |= CAN_ERR_BUSOFF;
+					frame->can_id &= ~CAN_ERR_CRTL;
+					break;
+				default:
+					break;
+				}
+			}
+			priv->shadow_statflg = canrflg & MSCAN_STAT_MSK;
+			frame->can_dlc = CAN_ERR_DLC;
+			out_8(&regs->canrflg, MSCAN_ERR_IF);
+		}
+
+		npackets++;
+		skb->dev = dev;
+		skb->protocol = __constant_htons(ETH_P_CAN);
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+		netif_receive_skb(skb);
+	}
+
+	if (!(in_8(&regs->canrflg) & (MSCAN_RXF | MSCAN_ERR_IF))) {
+		netif_rx_complete(dev, &priv->napi);
+		clear_bit(F_RX_PROGRESS, &priv->flags);
+		out_8(&regs->canrier,
+		      in_8(&regs->canrier) | MSCAN_ERR_IF | MSCAN_RXFIE);
+		ret = 0;
+	}
+	return ret;
+}
+
+
+static irqreturn_t mscan_isr(int irq, void *dev_id)
+{
+	struct net_device *dev = (struct net_device *)dev_id;
+	struct mscan_priv *priv = netdev_priv(dev);
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	struct net_device_stats *stats = dev->get_stats(dev);
+	u8 cantier, cantflg, canrflg;
+	irqreturn_t ret = IRQ_NONE;
+
+	if ((cantier = in_8(&regs->cantier) & MSCAN_TXE) &&
+	    (cantflg = in_8(&regs->cantflg) & cantier)) {
+		struct list_head *tmp, *pos;
+
+		list_for_each_safe(pos, tmp, &priv->tx_head) {
+			tx_queue_entry_t *entry =
+			    list_entry(pos, tx_queue_entry_t, list);
+			u8 mask = entry->mask;
+
+			if (!(cantflg & mask))
+				continue;
+
+			if (in_8(&regs->cantaak) & mask) {
+				stats->tx_dropped++;
+				stats->tx_aborted_errors++;
+			} else {
+				out_8(&regs->cantbsel, mask);
+				stats->tx_bytes +=
+				    in_8(&regs->tx.dlr);
+				stats->tx_packets++;
+			}
+			priv->tx_active &= ~mask;
+			list_del(pos);
+		}
+
+		if (list_empty(&priv->tx_head)) {
+			clear_bit(F_TX_WAIT_ALL, &priv->flags);
+			clear_bit(F_TX_PROGRESS, &priv->flags);
+			priv->cur_pri = 0;
+		} else
+			dev->trans_start = jiffies;
+
+		if (!test_bit(F_TX_WAIT_ALL, &priv->flags))
+			netif_wake_queue(dev);
+
+		out_8(&regs->cantier, priv->tx_active);
+		ret = IRQ_HANDLED;
+	}
+
+	if ((((canrflg = in_8(&regs->canrflg)) & ~MSCAN_STAT_MSK)) &&
+	    !test_and_set_bit(F_RX_PROGRESS, &priv->flags)) {
+		if (check_set_state(dev, canrflg)) {
+			out_8(&regs->canrflg, MSCAN_CSCIF);
+			ret = IRQ_HANDLED;
+		}
+		if (canrflg & ~MSCAN_STAT_MSK) {
+			priv->shadow_canrier = in_8(&regs->canrier);
+			out_8(&regs->canrier, 0);
+			netif_rx_schedule(dev, &priv->napi);
+			ret = IRQ_HANDLED;
+		} else
+			clear_bit(F_RX_PROGRESS, &priv->flags);
+	}
+	return ret;
+}
+
+static int mscan_do_set_mode(struct net_device *dev, can_mode_t mode)
+{
+	switch (mode) {
+	case CAN_MODE_SLEEP:
+	case CAN_MODE_STOP:
+		netif_stop_queue(dev);
+		mscan_set_mode(dev,
+			       (mode ==
+				CAN_MODE_STOP) ? MSCAN_INIT_MODE :
+			       MSCAN_SLEEP_MODE);
+		break;
+	case CAN_MODE_START:
+		printk("%s: CAN_MODE_START requested\n", __FUNCTION__);
+		mscan_set_mode(dev, MSCAN_NORMAL_MODE);
+		netif_wake_queue(dev);
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+static int mscan_do_set_bit_time(struct net_device *dev,
+				 struct can_bittime *bt)
+{
+	struct mscan_priv *priv = netdev_priv(dev);
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	int ret = 0;
+	u8 reg;
+	struct mscan_state state;
+
+	if (bt->type != CAN_BITTIME_STD)
+		return -EINVAL;
+
+	spin_lock_irq(&priv->can.irq_lock);
+
+	mscan_push_state(dev, &state);
+	ret = mscan_set_mode(dev, MSCAN_INIT_MODE);
+	if (!ret) {
+		reg = BTR0_SET_BRP(bt->std.brp) | BTR0_SET_SJW(bt->std.sjw);
+		out_8(&regs->canbtr0, reg);
+
+		reg = (BTR1_SET_TSEG1(bt->std.prop_seg + bt->std.phase_seg1) |
+		       BTR1_SET_TSEG2(bt->std.phase_seg2) |
+		       BTR1_SET_SAM(bt->std.sam));
+		out_8(&regs->canbtr1, reg);
+
+		ret = mscan_pop_state(dev, &state);
+	}
+
+	spin_unlock_irq(&priv->can.irq_lock);
+	return ret;
+}
+
+static int mscan_open(struct net_device *dev)
+{
+	int ret;
+	struct mscan_priv *priv = netdev_priv(dev);
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+
+	napi_enable(&priv->napi);
+	ret = request_irq(dev->irq, mscan_isr, IRQF_SHARED, dev->name, dev);
+
+	if (ret  < 0) {
+		napi_disable(&priv->napi);
+		printk(KERN_ERR "%s - failed to attach interrupt\n",
+		       dev->name);
+		return ret;
+	}
+
+	INIT_LIST_HEAD(&priv->tx_head);
+	/* acceptance mask/acceptance code (accept everything) */
+	out_be16(&regs->canidar1_0, 0);
+	out_be16(&regs->canidar3_2, 0);
+	out_be16(&regs->canidar5_4, 0);
+	out_be16(&regs->canidar7_6, 0);
+
+	out_be16(&regs->canidmr1_0, 0xffff);
+	out_be16(&regs->canidmr3_2, 0xffff);
+	out_be16(&regs->canidmr5_4, 0xffff);
+	out_be16(&regs->canidmr7_6, 0xffff);
+	/* Two 32 bit Acceptance Filters */
+	out_8(&regs->canidac, MSCAN_AF_32BIT);
+
+	out_8(&regs->canctl1, in_8(&regs->canctl1) & ~MSCAN_LISTEN);
+	mscan_set_mode(dev, MSCAN_NORMAL_MODE);
+
+	priv->shadow_statflg = in_8(&regs->canrflg) & MSCAN_STAT_MSK;
+	priv->cur_pri = 0;
+	priv->tx_active = 0;
+
+	out_8(&regs->cantier, 0);
+	/* Enable receive interrupts. */
+	out_8(&regs->canrier, MSCAN_OVRIE | MSCAN_RXFIE | MSCAN_CSCIE |
+	      MSCAN_RSTATE1 | MSCAN_RSTATE0 | MSCAN_TSTATE1 | MSCAN_TSTATE0);
+
+	netif_start_queue(dev);
+
+	return 0;
+}
+
+static int mscan_close(struct net_device *dev)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+
+	netif_stop_queue(dev);
+
+	/* disable interrupts */
+	out_8(&regs->cantier, 0);
+	out_8(&regs->canrier, 0);
+	free_irq(dev->irq, dev);
+
+	mscan_set_mode(dev, MSCAN_INIT_MODE);
+	return 0;
+}
+
+int register_mscandev(struct net_device *dev, int clock_src)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	u8 ctl1;
+
+	ctl1 = in_8(&regs->canctl1);
+	if (clock_src)
+		ctl1 |= MSCAN_CLKSRC;
+	else
+		ctl1 &= ~MSCAN_CLKSRC;
+
+	ctl1 |= MSCAN_CANE;
+	out_8(&regs->canctl1, ctl1);
+	udelay(100);
+
+	mscan_set_mode(dev, MSCAN_INIT_MODE);
+
+	return register_netdev(dev);
+}
+
+EXPORT_SYMBOL(register_mscandev);
+
+void unregister_mscandev(struct net_device *dev)
+{
+	struct mscan_regs *regs = (struct mscan_regs *)dev->base_addr;
+	mscan_set_mode(dev, MSCAN_INIT_MODE);
+	out_8(&regs->canctl1, in_8(&regs->canctl1) & ~MSCAN_CANE);
+	unregister_netdev(dev);
+}
+
+EXPORT_SYMBOL(unregister_mscandev);
+
+struct net_device *alloc_mscandev(void)
+{
+	struct net_device *dev;
+	struct mscan_priv *priv;
+	int i;
+
+	dev = alloc_candev(sizeof(struct mscan_priv));
+	if (!dev)
+		return NULL;
+	priv = netdev_priv(dev);
+
+	dev->watchdog_timeo = MSCAN_WATCHDOG_TIMEOUT;
+	dev->open = mscan_open;
+	dev->stop = mscan_close;
+	dev->hard_start_xmit = mscan_hard_start_xmit;
+	dev->tx_timeout = mscan_tx_timeout;
+
+	priv->dev = dev;
+	netif_napi_add(dev, &priv->napi, mscan_rx_poll, 8);
+
+	priv->can.do_set_bittime = mscan_do_set_bit_time;
+	priv->can.do_set_mode = mscan_do_set_mode;
+
+	for (i = 0; i < TX_QUEUE_SIZE; i++)
+		priv->tx_queue[i].mask = 1 << i;
+
+	return dev;
+}
+
+EXPORT_SYMBOL(alloc_mscandev);
+
+MODULE_AUTHOR("Andrey Volkov <avolkov@varma-el.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CAN port driver for a mscan based chips");
diff --git a/drivers/net/can/old/mscan/mscan.h b/drivers/net/can/old/mscan/mscan.h
new file mode 100644
index 000000000000..807ec3fae437
--- /dev/null
+++ b/drivers/net/can/old/mscan/mscan.h
@@ -0,0 +1,247 @@
+/*
+ * DESCRIPTION:
+ *  Definitions of consts/structs to drive the Freescale MSCAN.
+ *
+ * AUTHOR:
+ *  Andrey Volkov <avolkov@varma-el.com>
+ *
+ * COPYRIGHT:
+ *  2004-2006, Varma Electronics Oy
+ *
+ * LICENCE:
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#ifndef __MSCAN_H__
+#define __MSCAN_H__
+
+#include <linux/autoconf.h>
+#include <asm/types.h>
+
+/* MSCAN control register 0 (CANCTL0) bits */
+#define MSCAN_RXFRM		0x80
+#define MSCAN_RXACT		0x40
+#define MSCAN_CSWAI		0x20
+#define MSCAN_SYNCH		0x10
+#define MSCAN_TIME		0x08
+#define MSCAN_WUPE		0x04
+#define MSCAN_SLPRQ		0x02
+#define MSCAN_INITRQ		0x01
+
+/* MSCAN control register 1 (CANCTL1) bits */
+#define MSCAN_CANE		0x80
+#define MSCAN_CLKSRC		0x40
+#define MSCAN_LOOPB		0x20
+#define MSCAN_LISTEN		0x10
+#define MSCAN_WUPM		0x04
+#define MSCAN_SLPAK		0x02
+#define MSCAN_INITAK		0x01
+
+#ifdef	CONFIG_PPC_MPC52xx
+#define MSCAN_CLKSRC_BUS	0
+#define MSCAN_CLKSRC_XTAL	MSCAN_CLKSRC
+#else
+#define MSCAN_CLKSRC_BUS	MSCAN_CLKSRC
+#define MSCAN_CLKSRC_XTAL	0
+#endif
+
+/* MSCAN receiver flag register (CANRFLG) bits */
+#define MSCAN_WUPIF		0x80
+#define MSCAN_CSCIF		0x40
+#define MSCAN_RSTAT1		0x20
+#define MSCAN_RSTAT0		0x10
+#define MSCAN_TSTAT1		0x08
+#define MSCAN_TSTAT0		0x04
+#define MSCAN_OVRIF		0x02
+#define MSCAN_RXF		0x01
+#define MSCAN_ERR_IF 		(MSCAN_OVRIF | MSCAN_CSCIF)
+#define MSCAN_RSTAT_MSK		(MSCAN_RSTAT1 | MSCAN_RSTAT0)
+#define MSCAN_TSTAT_MSK		(MSCAN_TSTAT1 | MSCAN_TSTAT0)
+#define MSCAN_STAT_MSK		(MSCAN_RSTAT_MSK | MSCAN_TSTAT_MSK)
+
+#define MSCAN_STATE_BUS_OFF	(MSCAN_RSTAT1 | MSCAN_RSTAT0 | \
+				 MSCAN_TSTAT1 | MSCAN_TSTAT0)
+#define MSCAN_STATE_TX(canrflg)	(((canrflg)&MSCAN_TSTAT_MSK)>>2)
+#define MSCAN_STATE_RX(canrflg)	(((canrflg)&MSCAN_RSTAT_MSK)>>4)
+#define MSCAN_STATE_ACTIVE	0
+#define MSCAN_STATE_WARNING	1
+#define MSCAN_STATE_PASSIVE	2
+#define MSCAN_STATE_BUSOFF	3
+
+/* MSCAN receiver interrupt enable register (CANRIER) bits */
+#define MSCAN_WUPIE		0x80
+#define MSCAN_CSCIE		0x40
+#define MSCAN_RSTATE1		0x20
+#define MSCAN_RSTATE0		0x10
+#define MSCAN_TSTATE1		0x08
+#define MSCAN_TSTATE0		0x04
+#define MSCAN_OVRIE		0x02
+#define MSCAN_RXFIE		0x01
+
+/* MSCAN transmitter flag register (CANTFLG) bits */
+#define MSCAN_TXE2		0x04
+#define MSCAN_TXE1		0x02
+#define MSCAN_TXE0		0x01
+#define MSCAN_TXE		(MSCAN_TXE2 | MSCAN_TXE1 | MSCAN_TXE0)
+
+/* MSCAN transmitter interrupt enable register (CANTIER) bits */
+#define MSCAN_TXIE2		0x04
+#define MSCAN_TXIE1		0x02
+#define MSCAN_TXIE0		0x01
+#define MSCAN_TXIE		(MSCAN_TXIE2 | MSCAN_TXIE1 | MSCAN_TXIE0)
+
+/* MSCAN transmitter message abort request (CANTARQ) bits */
+#define MSCAN_ABTRQ2		0x04
+#define MSCAN_ABTRQ1		0x02
+#define MSCAN_ABTRQ0		0x01
+
+/* MSCAN transmitter message abort ack (CANTAAK) bits */
+#define MSCAN_ABTAK2		0x04
+#define MSCAN_ABTAK1		0x02
+#define MSCAN_ABTAK0		0x01
+
+/* MSCAN transmit buffer selection (CANTBSEL) bits */
+#define MSCAN_TX2		0x04
+#define MSCAN_TX1		0x02
+#define MSCAN_TX0		0x01
+
+/* MSCAN ID acceptance control register (CANIDAC) bits */
+#define MSCAN_IDAM1		0x20
+#define MSCAN_IDAM0		0x10
+#define MSCAN_IDHIT2		0x04
+#define MSCAN_IDHIT1		0x02
+#define MSCAN_IDHIT0		0x01
+
+#define MSCAN_AF_32BIT		0x00
+#define MSCAN_AF_16BIT		MSCAN_IDAM0
+#define MSCAN_AF_8BIT		MSCAN_IDAM1
+#define MSCAN_AF_CLOSED		(MSCAN_IDAM0|MSCAN_IDAM1)
+#define MSCAN_AF_MASK		(~(MSCAN_IDAM0|MSCAN_IDAM1))
+
+/* MSCAN Miscellaneous Register (CANMISC) bits */
+#define MSCAN_BOHOLD		0x01
+
+#ifdef	CONFIG_PPC_MPC52xx
+#define _MSCAN_RESERVED_(n,num)	u8	_res##n[num]
+#define _MSCAN_RESERVED_DSR_SIZE	2
+#else
+#define _MSCAN_RESERVED_(n,num)
+#define _MSCAN_RESERVED_DSR_SIZE	0
+#endif
+
+/* Structure of the hardware registers */
+struct mscan_regs {
+	/* (see doco S12MSCANV3/D)		  MPC5200    MSCAN */
+	u8 canctl0;				/* + 0x00     0x00 */
+	u8 canctl1;				/* + 0x01     0x01 */
+	_MSCAN_RESERVED_(1, 2);			/* + 0x02          */
+	u8 canbtr0;				/* + 0x04     0x02 */
+	u8 canbtr1;				/* + 0x05     0x03 */
+	_MSCAN_RESERVED_(2, 2);			/* + 0x06          */
+	u8 canrflg;				/* + 0x08     0x04 */
+	u8 canrier;				/* + 0x09     0x05 */
+	_MSCAN_RESERVED_(3, 2);			/* + 0x0a          */
+	u8 cantflg;				/* + 0x0c     0x06 */
+	u8 cantier;				/* + 0x0d     0x07 */
+	_MSCAN_RESERVED_(4, 2);			/* + 0x0e          */
+	u8 cantarq;				/* + 0x10     0x08 */
+	u8 cantaak;				/* + 0x11     0x09 */
+	_MSCAN_RESERVED_(5, 2);			/* + 0x12          */
+	u8 cantbsel;				/* + 0x14     0x0a */
+	u8 canidac;				/* + 0x15     0x0b */
+	u8 reserved;				/* + 0x16     0x0c */
+	_MSCAN_RESERVED_(6, 5);			/* + 0x17          */
+#ifndef CONFIG_PPC_MPC52xx
+	u8 canmisc;				/*            0x0d */
+#endif
+	u8 canrxerr;				/* + 0x1c     0x0e */
+	u8 cantxerr;				/* + 0x1d     0x0f */
+	_MSCAN_RESERVED_(7, 2);			/* + 0x1e          */
+	u16 canidar1_0;				/* + 0x20     0x10 */
+	_MSCAN_RESERVED_(8, 2);			/* + 0x22          */
+	u16 canidar3_2;				/* + 0x24     0x12 */
+	_MSCAN_RESERVED_(9, 2);			/* + 0x26          */
+	u16 canidmr1_0;				/* + 0x28     0x14 */
+	_MSCAN_RESERVED_(10, 2);		/* + 0x2a          */
+	u16 canidmr3_2;				/* + 0x2c     0x16 */
+	_MSCAN_RESERVED_(11, 2);		/* + 0x2e          */
+	u16 canidar5_4;				/* + 0x30     0x18 */
+	_MSCAN_RESERVED_(12, 2);		/* + 0x32          */
+	u16 canidar7_6;				/* + 0x34     0x1a */
+	_MSCAN_RESERVED_(13, 2);		/* + 0x36          */
+	u16 canidmr5_4;				/* + 0x38     0x1c */
+	_MSCAN_RESERVED_(14, 2);		/* + 0x3a          */
+	u16 canidmr7_6;				/* + 0x3c     0x1e */
+	_MSCAN_RESERVED_(15, 2);		/* + 0x3e          */
+	struct {
+		u16 idr1_0;			/* + 0x40     0x20 */
+		 _MSCAN_RESERVED_(16, 2);	/* + 0x42          */
+		u16 idr3_2;			/* + 0x44     0x22 */
+		 _MSCAN_RESERVED_(17, 2);	/* + 0x46          */
+		u16 dsr1_0;			/* + 0x48     0x24 */
+		 _MSCAN_RESERVED_(18, 2);	/* + 0x4a          */
+		u16 dsr3_2;			/* + 0x4c     0x26 */
+		 _MSCAN_RESERVED_(19, 2);	/* + 0x4e          */
+		u16 dsr5_4;			/* + 0x50     0x28 */
+		 _MSCAN_RESERVED_(20, 2);	/* + 0x52          */
+		u16 dsr7_6;			/* + 0x54     0x2a */
+		 _MSCAN_RESERVED_(21, 2);	/* + 0x56          */
+		u8 dlr;				/* + 0x58     0x2c */
+		 u8:8;				/* + 0x59     0x2d */
+		 _MSCAN_RESERVED_(22, 2);	/* + 0x5a          */
+		u16 time;			/* + 0x5c     0x2e */
+	} rx;
+	 _MSCAN_RESERVED_(23, 2);		/* + 0x5e          */
+	struct {
+		u16 idr1_0;			/* + 0x60     0x30 */
+		 _MSCAN_RESERVED_(24, 2);	/* + 0x62          */
+		u16 idr3_2;			/* + 0x64     0x32 */
+		 _MSCAN_RESERVED_(25, 2);	/* + 0x66          */
+		u16 dsr1_0;			/* + 0x68     0x34 */
+		 _MSCAN_RESERVED_(26, 2);	/* + 0x6a          */
+		u16 dsr3_2;			/* + 0x6c     0x36 */
+		 _MSCAN_RESERVED_(27, 2);	/* + 0x6e          */
+		u16 dsr5_4;			/* + 0x70     0x38 */
+		 _MSCAN_RESERVED_(28, 2);	/* + 0x72          */
+		u16 dsr7_6;			/* + 0x74     0x3a */
+		 _MSCAN_RESERVED_(29, 2);	/* + 0x76          */
+		u8 dlr;				/* + 0x78     0x3c */
+		u8 tbpr;			/* + 0x79     0x3d */
+		 _MSCAN_RESERVED_(30, 2);	/* + 0x7a          */
+		u16 time;			/* + 0x7c     0x3e */
+	} tx;
+	 _MSCAN_RESERVED_(31, 2);		/* + 0x7e          */
+} __attribute__ ((packed));
+
+#undef _MSCAN_RESERVED_
+#define MSCAN_REGION 	sizeof(struct mscan)
+
+#define MSCAN_WATCHDOG_TIMEOUT	((500*HZ)/1000)
+
+struct mscan_platform_data {
+	u8 clock_src;		/* MSCAN_CLKSRC_BUS or MSCAN_CLKSRC_XTAL */
+	u32 clock_frq;		/* can ref. clock, in Hz */
+};
+
+struct net_device *alloc_mscandev(void);
+/* @clock_src:
+	1 = The MSCAN clock source is the onchip Bus Clock.
+	0 = The MSCAN clock source is the chip Oscillator Clock.
+*/
+extern int register_mscandev(struct net_device *dev, int clock_src);
+extern void unregister_mscandev(struct net_device *dev);
+
+#endif				/* __MSCAN_H__ */
diff --git a/drivers/net/can/old/sja1000/Makefile b/drivers/net/can/old/sja1000/Makefile
new file mode 100644
index 000000000000..cc8f1f917e99
--- /dev/null
+++ b/drivers/net/can/old/sja1000/Makefile
@@ -0,0 +1,27 @@
+#
+#
+
+ifeq ($(KERNELRELEASE),)
+
+KERNELDIR := /lib/modules/$(shell uname -r)/build
+PWD       := $(shell pwd)
+TOPDIR    := $(PWD)/../../../../..
+
+modules modules_install clean:
+	$(MAKE) -C $(KERNELDIR) M=$(PWD) $@ TOPDIR=$(TOPDIR)
+
+else
+
+-include $(TOPDIR)/Makefile.common
+EXTRA_CFLAGS += -I$(TOPDIR)/drivers/net/can/old/hal
+
+obj-m := sja1000-io.o sja1000-iomem.o sja1000-iomux.o sja1000-gw2.o sja1000-esdio.o sja1000-c200.o
+
+sja1000-io-objs    := sja1000.o proc.o ../hal/io.o
+sja1000-iomem-objs := sja1000.o proc.o ../hal/iomem.o
+sja1000-iomux-objs := sja1000.o proc.o ../hal/iomux.o
+sja1000-gw2-objs   := sja1000.o proc.o ../hal/gw2.o
+sja1000-esdio-objs := sja1000.o proc.o ../hal/esdio.o
+sja1000-c200-objs  := sja1000.o proc.o ../hal/c200.o
+
+endif
diff --git a/drivers/net/can/old/sja1000/proc.c b/drivers/net/can/old/sja1000/proc.c
new file mode 100644
index 000000000000..b98aa6180784
--- /dev/null
+++ b/drivers/net/can/old/sja1000/proc.c
@@ -0,0 +1,230 @@
+/*
+ * proc.c -  proc file system functions for SJA1000 CAN driver.
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/proc_fs.h>
+#include <linux/netdevice.h>
+
+#include <linux/can.h>
+#include <linux/can/ioctl.h>
+#include "sja1000.h"
+#include "hal.h"
+
+extern struct net_device *can_dev[];
+
+static struct proc_dir_entry *pde       = NULL;
+static struct proc_dir_entry *pde_regs  = NULL;
+static struct proc_dir_entry *pde_reset = NULL;
+
+static int can_proc_read_stats(char *page, char **start, off_t off,
+			       int count, int *eof, void *data)
+{
+	int len = 0;
+	int i;
+
+	len += snprintf(page + len, PAGE_SIZE - len,
+			"CAN bus device statistics:\n");
+	len += snprintf(page + len, PAGE_SIZE - len,
+			"       errwarn  overrun   wakeup   buserr   "
+			"errpass  arbitr   restarts clock        baud\n");
+	for (i = 0; (i < MAXDEV) && (len < PAGE_SIZE - 200); i++) {
+		if (can_dev[i]) {
+			struct net_device *dev = can_dev[i];
+			struct can_priv *priv  = netdev_priv(dev);
+#ifdef SJA1000_H
+			u8 stat = hw_readreg(dev->base_addr, REG_SR);
+
+			if (stat & 0x80) {
+				len += snprintf(page + len, PAGE_SIZE - len,
+						"%s: bus status: "
+						"BUS OFF, ", dev->name);
+			} else if (stat & 0x40) {
+				len += snprintf(page + len, PAGE_SIZE - len,
+						"%s: bus status: ERROR "
+						"PASSIVE, ", dev->name);
+			} else {
+				len += snprintf(page + len, PAGE_SIZE - len,
+						"%s: bus status: OK, ",
+						dev->name);
+			}
+			len += snprintf(page + len, PAGE_SIZE - len,
+					"RXERR: %d, TXERR: %d\n",
+					hw_readreg(dev->base_addr, REG_RXERR),
+					hw_readreg(dev->base_addr, REG_TXERR));
+#endif
+			len += snprintf(page + len, PAGE_SIZE - len,
+					"%s: %8d %8d %8d %8d %8d "
+					"%8d %8d %10d %8d\n", dev->name,
+					priv->can_stats.error_warning,
+					priv->can_stats.data_overrun,
+					priv->can_stats.wakeup,
+					priv->can_stats.bus_error,
+					priv->can_stats.error_passive,
+					priv->can_stats.arbitration_lost,
+					priv->can_stats.restarts,
+					priv->clock,
+					priv->speed
+				);
+
+		}
+	}
+
+	*eof = 1;
+	return len;
+}
+
+
+static int can_proc_dump_regs(char *page, int len, struct net_device *dev)
+{
+	int r,s;
+	struct can_priv	*priv = netdev_priv(dev);
+	int regs = priv->hw_regs;
+
+	len += snprintf(page + len, PAGE_SIZE - len,
+			"%s registers:\n", dev->name);
+
+	for (r = 0; r < regs; r += 0x10) {
+		len += snprintf(page + len, PAGE_SIZE - len, "%02X: ", r);
+		for (s = 0; s < 0x10; s++) {
+			if (r+s < regs)
+				len += snprintf(page + len, PAGE_SIZE-len,
+						"%02X ",
+						hw_readreg(dev->base_addr,
+							   r+s));
+		}
+		len += snprintf(page + len, PAGE_SIZE - len, "\n");
+	}
+
+        return len;
+}
+
+static int can_proc_read_regs(char *page, char **start, off_t off,
+				  int count, int *eof, void *data)
+{
+	int len = 0;
+	int i;
+
+	for (i = 0; (i < MAXDEV) && (len < PAGE_SIZE - 200); i++) {
+		if (can_dev[i])
+			len = can_proc_dump_regs(page, len, can_dev[i]);
+	}
+
+	*eof = 1;
+	return len;
+}
+
+static int can_proc_read_reset(char *page, char **start, off_t off,
+				   int count, int *eof, void *data)
+{
+	int len = 0;
+	struct net_device *dev;
+	int i;
+	struct can_priv   *priv;
+
+	len += snprintf(page + len, PAGE_SIZE - len, "resetting ");
+	for (i = 0; (i < MAXDEV) && (len < PAGE_SIZE - 200); i++) {
+		if (can_dev[i]) {
+			dev = can_dev[i];
+			priv = netdev_priv(can_dev[i]);
+			if ((priv->state != STATE_UNINITIALIZED)
+			    && (priv->state != STATE_RESET_MODE)) {
+				len += snprintf(page + len, PAGE_SIZE - len,
+						"%s ", dev->name);
+				dev->stop(dev);
+				dev->open(dev);
+				/* count number of restarts */
+				priv->can_stats.restarts++;
+
+			} else {
+				len += snprintf(page + len, PAGE_SIZE - len,
+						"(%s|%d) ", dev->name,
+						priv->state);
+			}
+		}
+	}
+
+	len += snprintf(page + len, PAGE_SIZE - len, "done\n");
+
+	*eof = 1;
+	return len;
+}
+
+void can_proc_create(const char *drv_name)
+{
+	char fname[256];
+
+	if (pde == NULL) {
+		sprintf(fname, PROCBASE "/%s_stats", drv_name);
+		pde = create_proc_read_entry(fname, 0644, NULL,
+					     can_proc_read_stats, NULL);
+	}
+	if (pde_regs == NULL) {
+		sprintf(fname, PROCBASE "/%s_regs", drv_name);
+		pde_regs = create_proc_read_entry(fname, 0644, NULL,
+						  can_proc_read_regs, NULL);
+	}
+	if (pde_reset == NULL) {
+		sprintf(fname, PROCBASE "/%s_reset", drv_name);
+		pde_reset = create_proc_read_entry(fname, 0644, NULL,
+						   can_proc_read_reset, NULL);
+	}
+}
+
+void can_proc_remove(const char *drv_name)
+{
+	char fname[256];
+
+	if (pde) {
+		sprintf(fname, PROCBASE "/%s_stats", drv_name);
+		remove_proc_entry(fname, NULL);
+	}
+	if (pde_regs) {
+		sprintf(fname, PROCBASE "/%s_regs", drv_name);
+		remove_proc_entry(fname, NULL);
+	}
+	if (pde_reset) {
+		sprintf(fname, PROCBASE "/%s_reset", drv_name);
+		remove_proc_entry(fname, NULL);
+	}
+}
diff --git a/drivers/net/can/old/sja1000/sja1000.c b/drivers/net/can/old/sja1000/sja1000.c
new file mode 100644
index 000000000000..8d7185a2962b
--- /dev/null
+++ b/drivers/net/can/old/sja1000/sja1000.c
@@ -0,0 +1,1140 @@
+/*
+ * sja1000.c -  Philips SJA1000 network device driver
+ *
+ * Copyright (c) 2003 Matthias Brukner, Trajet Gmbh, Rebenring 33,
+ * 38106 Braunschweig, GERMANY
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/fcntl.h>
+#include <linux/interrupt.h>
+#include <linux/ptrace.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/skbuff.h>
+
+#include <linux/can.h>
+#include <linux/can/ioctl.h> /* for struct can_device_stats */
+#include "sja1000.h"
+#include "hal.h"
+
+MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("LLCF/socketcan '" CHIP_NAME "' network device driver");
+
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+#define DBG(args...)   ((priv->debug > 0) ? printk(args) : 0)
+/* logging in interrupt context! */
+#define iDBG(args...)  ((priv->debug > 1) ? printk(args) : 0)
+#define iiDBG(args...) ((priv->debug > 2) ? printk(args) : 0)
+#else
+#define DBG(args...)
+#define iDBG(args...)
+#define iiDBG(args...)
+#endif
+
+char drv_name[DRV_NAME_LEN] = "undefined";
+
+/* driver and version information */
+static const char *drv_version	= "0.1.1";
+static const char *drv_reldate	= "2007-04-13";
+
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+static const char *ecc_errors[] = {
+	NULL,
+	NULL,
+	"ID.28 to ID.28",
+	"start of frame",
+	"bit SRTR",
+	"bit IDE",
+	"ID.20 to ID.18",
+	"ID.17 to ID.13",
+	"CRC sequence",
+	"reserved bit 0",
+	"data field",
+	"data length code",
+	"bit RTR",
+	"reserved bit 1",
+	"ID.4 to ID.0",
+	"ID.12 to ID.5",
+	NULL,
+	"active error flag",
+	"intermission",
+	"tolerate dominant bits",
+	NULL,
+	NULL,
+	"passive error flag",
+	"error delimiter",
+	"CRC delimiter",
+	"acknowledge slot",
+	"end of frame",
+	"acknowledge delimiter",
+	"overload flag",
+	NULL,
+	NULL,
+	NULL
+};
+
+static const char *ecc_types[] = {
+	"bit error",
+	"form error",
+	"stuff error",
+	"other type of error"
+};
+#endif
+
+/* array of all can chips */
+struct net_device *can_dev[MAXDEV];
+
+/* module parameters */
+unsigned long base[MAXDEV]	= { 0 }; /* hardware address */
+unsigned long rbase[MAXDEV]	= { 0 }; /* (remapped) device address */
+unsigned int  irq[MAXDEV]	= { 0 };
+
+unsigned int speed[MAXDEV]	= { DEFAULT_SPEED, DEFAULT_SPEED };
+unsigned int btr[MAXDEV]	= { 0 };
+
+static int rx_probe[MAXDEV]	= { 0 };
+static int clk			= DEFAULT_HW_CLK;
+static int debug		= 0;
+static int restart_ms		= 100;
+static int echo			= 1;
+
+static int base_n;
+static int irq_n;
+static int speed_n;
+static int btr_n;
+static int rx_probe_n;
+
+module_param_array(base, int, &base_n, 0);
+module_param_array(irq, int, &irq_n, 0);
+module_param_array(speed, int, &speed_n, 0);
+module_param_array(btr, int, &btr_n, 0);
+module_param_array(rx_probe, int, &rx_probe_n, 0);
+
+module_param(clk, int, 0);
+module_param(debug, int, 0);
+module_param(restart_ms, int, 0);
+module_param(echo, int, S_IRUGO);
+
+MODULE_PARM_DESC(base, "CAN controller base address");
+MODULE_PARM_DESC(irq, "CAN controller interrupt");
+MODULE_PARM_DESC(speed, "CAN bus bitrate");
+MODULE_PARM_DESC(btr, "Bit Timing Register value 0x<btr0><btr1>, e.g. 0x4014");
+MODULE_PARM_DESC(rx_probe, "switch to trx mode after correct msg receiption. (default off)");
+
+MODULE_PARM_DESC(clk, "CAN controller chip clock (default: 16MHz)");
+MODULE_PARM_DESC(debug, "set debug mask (default: 0)");
+MODULE_PARM_DESC(restart_ms, "restart chip on heavy bus errors / bus off after x ms (default 100ms)");
+MODULE_PARM_DESC(echo, "Echo sent frames. default: 1 (On)");
+
+/*
+ * CAN network devices *should* support a local echo functionality
+ * (see Documentation/networking/can.txt). To test the handling of CAN
+ * interfaces that do not support the local echo both driver types are
+ * implemented inside this sja1000 driver. In the case that the driver does
+ * not support the echo the IFF_ECHO remains clear in dev->flags.
+ * This causes the PF_CAN core to perform the echo as a fallback solution.
+ */
+
+/* function declarations */
+
+static void can_restart_dev(unsigned long data);
+static void chipset_init(struct net_device *dev, int wake);
+static void chipset_init_rx(struct net_device *dev);
+static void chipset_init_trx(struct net_device *dev);
+static void can_netdev_setup(struct net_device *dev);
+static struct net_device* can_create_netdev(int dev_num, int hw_regs);
+static int  can_set_drv_name(void);
+int set_reset_mode(struct net_device *dev);
+
+static int sja1000_probe_chip(unsigned long base)
+{
+	if (base && (hw_readreg(base, 0) == 0xFF)) {
+		printk(KERN_INFO "%s: probing @0x%lX failed\n",
+		       drv_name, base);
+		return 0;
+	}
+	return 1;
+}
+
+/*
+ * set baud rate divisor values
+ */
+static void set_btr(struct net_device *dev, int btr0, int btr1)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	/* no bla bla when restarting the device */
+	if (priv->state == STATE_UNINITIALIZED)
+		printk(KERN_INFO "%s: setting BTR0=%02X BTR1=%02X\n",
+		       dev->name, btr0, btr1);
+
+	hw_writereg(dev->base_addr, REG_BTR0, btr0);
+	hw_writereg(dev->base_addr, REG_BTR1, btr1);
+}
+
+/*
+ * calculate baud rate divisor values
+ */
+static void set_baud(struct net_device *dev, int baud, int clock)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	int error;
+	int brp;
+	int tseg;
+	int tseg1 = 0;
+	int tseg2 = 0;
+
+	int best_error = 1000000000;
+	int best_tseg = 0;
+	int best_brp = 0;
+	int best_baud = 0;
+
+	int SAM = (baud > 100000 ? 0 : 1);
+
+	clock >>= 1;
+
+	for (tseg = (0 + 0 + 2) * 2;
+	     tseg <= (MAX_TSEG2 + MAX_TSEG1 + 2) * 2 + 1;
+	     tseg++) {
+		brp = clock / ((1 + tseg / 2) * baud) + tseg % 2;
+		if ((brp > 0) && (brp <= 64)) {
+			error = baud - clock / (brp * (1 + tseg / 2));
+			if (error < 0) {
+				error = -error;
+			}
+			if (error <= best_error) {
+				best_error = error;
+				best_tseg = tseg / 2;
+				best_brp = brp - 1;
+				best_baud = clock / (brp * (1 + tseg / 2));
+			}
+		}
+	}
+	if (best_error && (baud / best_error < 10)) {
+		printk("%s: unable to set baud rate %d (ext clock %dHz)\n",
+		       dev->name, baud, clock * 2);
+		return;
+//		return -EINVAL;
+	}
+	tseg2 = best_tseg - (SAMPLE_POINT * (best_tseg + 1)) / 100;
+	if (tseg2 < 0) {
+		tseg2 = 0;
+	} else if (tseg2 > MAX_TSEG2) {
+		tseg2 = MAX_TSEG2;
+	}
+	tseg1 = best_tseg - tseg2 - 2;
+	if (tseg1 > MAX_TSEG1) {
+		tseg1 = MAX_TSEG1;
+		tseg2 = best_tseg - tseg1 - 2;
+	}
+
+	priv->btr = ((best_brp | JUMPWIDTH)<<8) + 
+		((SAM << 7) | (tseg2 << 4) | tseg1);
+
+	printk(KERN_INFO "%s: calculated best baudrate: %d / btr is 0x%04X\n",
+	       dev->name, best_baud, priv->btr);
+
+	set_btr(dev, (priv->btr>>8) & 0xFF, priv->btr & 0xFF);
+//	set_btr(dev, best_brp | JUMPWIDTH, (SAM << 7) | (tseg2 << 4) | tseg1);
+}
+
+int set_reset_mode(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	unsigned char status = hw_readreg(dev->base_addr, REG_MOD);
+	int i;
+
+	priv->can_stats.bus_error_at_init = priv->can_stats.bus_error;
+
+	/* disable interrupts */
+	hw_writereg(dev->base_addr, REG_IER, IRQ_OFF);
+
+	for (i = 0; i < 10; i++) {
+		/* check reset bit */
+		if (status & MOD_RM) {
+			if (i > 1) {
+				iDBG(KERN_INFO "%s: %s looped %d times\n",
+				     dev->name, __FUNCTION__, i);
+			}
+			priv->state = STATE_RESET_MODE;
+			return 0;
+		}
+
+		hw_writereg(dev->base_addr, REG_MOD, MOD_RM); /* reset chip */
+		status = hw_readreg(dev->base_addr, REG_MOD);
+
+	}
+
+	printk(KERN_ERR "%s: setting sja1000 into reset mode failed!\n",
+	       dev->name);
+	return 1;
+
+}
+
+static int set_normal_mode(struct net_device *dev)
+{
+	unsigned char status = hw_readreg(dev->base_addr, REG_MOD);
+	int i;
+
+	for (i = 0; i < 10; i++) {
+		/* check reset bit */
+		if ((status & MOD_RM) == 0) {
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+			if (i > 1) {
+				struct can_priv *priv = netdev_priv(dev);
+				iDBG(KERN_INFO "%s: %s looped %d times\n",
+				     dev->name, __FUNCTION__, i);
+			}
+#endif
+			return 0;
+		}
+
+		/* set chip to normal mode */
+		hw_writereg(dev->base_addr, REG_MOD, 0x00);
+		status = hw_readreg(dev->base_addr, REG_MOD);
+	}
+
+	printk(KERN_ERR "%s: setting sja1000 into normal mode failed!\n",
+	       dev->name);
+	return 1;
+
+}
+
+static int set_listen_mode(struct net_device *dev)
+{
+	unsigned char status = hw_readreg(dev->base_addr, REG_MOD);
+	int i;
+
+	for (i = 0; i < 10; i++) {
+		/* check reset mode bit */
+		if ((status & MOD_RM) == 0) {
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+			if (i > 1) {
+				struct can_priv *priv = netdev_priv(dev);
+				iDBG(KERN_INFO "%s: %s looped %d times\n",
+				     dev->name, __FUNCTION__, i);
+			}
+#endif
+			return 0;
+		}
+
+		/* set listen only mode, clear reset */
+		hw_writereg(dev->base_addr, REG_MOD, MOD_LOM);
+		status = hw_readreg(dev->base_addr, REG_MOD);
+	}
+
+	printk(KERN_ERR "%s: setting sja1000 into listen mode failed!\n",
+	       dev->name);
+	return 1;
+
+}
+
+/*
+ * initialize SJA1000 chip:
+ *   - reset chip
+ *   - set output mode
+ *   - set baudrate
+ *   - enable interrupts
+ *   - start operating mode
+ */
+static void chipset_init_regs(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	unsigned long base = dev->base_addr;
+
+	/* go into Pelican mode, disable clkout, disable comparator */
+	hw_writereg(base, REG_CDR, 0xCF);
+
+	/* output control */
+	/* connected to external transceiver */
+	hw_writereg(base, REG_OCR, 0x1A);
+
+	/* set acceptance filter (accept all) */
+	hw_writereg(base, REG_ACCC0, 0x00);
+	hw_writereg(base, REG_ACCC1, 0x00);
+	hw_writereg(base, REG_ACCC2, 0x00);
+	hw_writereg(base, REG_ACCC3, 0x00);
+
+	hw_writereg(base, REG_ACCM0, 0xFF);
+	hw_writereg(base, REG_ACCM1, 0xFF);
+	hw_writereg(base, REG_ACCM2, 0xFF);
+	hw_writereg(base, REG_ACCM3, 0xFF);
+
+	/* set baudrate */
+	if (priv->btr) { /* no calculation when btr is provided */
+		set_btr(dev, (priv->btr>>8) & 0xFF, priv->btr & 0xFF);
+	} else {
+		if (priv->speed == 0) {
+			priv->speed = DEFAULT_SPEED;
+		}
+		set_baud(dev, priv->speed * 1000, priv->clock);
+	}
+
+	/* output control */
+	/* connected to external transceiver */
+	hw_writereg(base, REG_OCR, 0x1A);
+}
+
+static void chipset_init(struct net_device *dev, int wake)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	if (priv->rx_probe)
+		chipset_init_rx(dev); /* wait for valid reception first */
+	else
+		chipset_init_trx(dev);
+
+	if ((wake) && netif_queue_stopped(dev)) {
+		if (priv->echo_skb) { /* pending echo? */
+			kfree_skb(priv->echo_skb);
+			priv->echo_skb = NULL;
+		}
+		netif_wake_queue(dev);
+	}
+}
+
+static void chipset_init_rx(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	iDBG(KERN_INFO "%s: %s()\n", dev->name, __FUNCTION__);
+
+	/* set chip into reset mode */
+	set_reset_mode(dev);
+
+	/* set registers */
+	chipset_init_regs(dev);
+
+	/* automatic bit rate detection */
+	set_listen_mode(dev);
+
+	priv->state = STATE_PROBE;
+
+	/* enable receive and error interrupts */
+	hw_writereg(dev->base_addr, REG_IER, IRQ_RI | IRQ_EI);
+}
+
+static void chipset_init_trx(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	iDBG(KERN_INFO "%s: %s()\n", dev->name, __FUNCTION__);
+
+	/* set chip into reset mode */
+	set_reset_mode(dev);
+
+	/* set registers */
+	chipset_init_regs(dev);
+
+	/* leave reset mode */
+	set_normal_mode(dev);
+
+	priv->state = STATE_ACTIVE;
+
+	/* enable all interrupts */
+	hw_writereg(dev->base_addr, REG_IER, IRQ_ALL);
+}
+
+/*
+ * transmit a CAN message
+ * message layout in the sk_buff should be like this:
+ * xx xx xx xx	 ff	 ll   00 11 22 33 44 55 66 77
+ * [  can-id ] [flags] [len] [can data (up to 8 bytes]
+ */
+static int can_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct can_priv  *priv	= netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct can_frame *cf	= (struct can_frame*)skb->data;
+	unsigned long base	= dev->base_addr;
+	uint8_t	fi;
+	uint8_t	dlc;
+	canid_t	id;
+	uint8_t	dreg;
+	int	loop;
+	int	i;
+
+	netif_stop_queue(dev);
+
+	fi = dlc = cf->can_dlc;
+	id = cf->can_id;
+
+	if (id & CAN_RTR_FLAG)
+		fi |= FI_RTR;
+
+	if (id & CAN_EFF_FLAG) {
+		fi |= FI_FF;
+		dreg = EFF_BUF;
+		hw_writereg(base, REG_FI, fi);
+		hw_writereg(base, REG_ID1, (id & 0x1fe00000) >> (5 + 16));
+		hw_writereg(base, REG_ID2, (id & 0x001fe000) >> (5 + 8));
+		hw_writereg(base, REG_ID3, (id & 0x00001fe0) >> 5);
+		hw_writereg(base, REG_ID4, (id & 0x0000001f) << 3);
+	} else {
+		dreg = SFF_BUF;
+		hw_writereg(base, REG_FI, fi);
+		hw_writereg(base, REG_ID1, (id & 0x000007f8) >> 3);
+		hw_writereg(base, REG_ID2, (id & 0x00000007) << 5);
+	}
+
+	for (i = 0; i < dlc; i++) {
+		hw_writereg(base, dreg++, cf->data[i]);
+	}
+
+	hw_writereg(base, REG_CMR, CMD_TR);
+
+	stats->tx_bytes += dlc;
+
+	dev->trans_start = jiffies;
+
+	/* set flag whether this packet has to be looped back */
+	loop = skb->pkt_type == PACKET_LOOPBACK;
+
+	if (!echo || !loop) {
+		kfree_skb(skb);
+		return 0;
+	}
+
+	if (!priv->echo_skb) {
+		struct sock *srcsk = skb->sk;
+
+		if (atomic_read(&skb->users) != 1) {
+			struct sk_buff *old_skb = skb;
+
+			skb = skb_clone(old_skb, GFP_ATOMIC);
+			DBG(KERN_INFO "%s: %s: freeing old skbuff %p, "
+			    "using new skbuff %p\n",
+			    dev->name, __FUNCTION__, old_skb, skb);
+			kfree_skb(old_skb);
+			if (!skb) {
+				return 0;
+			}
+		} else
+			skb_orphan(skb);
+
+		skb->sk = srcsk;
+
+		/* make settings for echo to reduce code in irq context */
+		skb->protocol	= htons(ETH_P_CAN);
+		skb->pkt_type	= PACKET_BROADCAST;
+		skb->ip_summed	= CHECKSUM_UNNECESSARY;
+		skb->dev	= dev;
+
+		/* save this skb for tx interrupt echo handling */
+		priv->echo_skb = skb;
+
+	} else {
+		/* locking problem with netif_stop_queue() ?? */
+		printk(KERN_ERR "%s: %s: occupied echo_skb!\n",
+		       dev->name, __FUNCTION__ );
+		kfree_skb(skb);
+	}
+
+	return 0;
+}
+
+static void can_tx_timeout(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+
+	stats->tx_errors++;
+
+	/* do not conflict with e.g. bus error handling */
+	if (!(priv->timer.expires)){ /* no restart on the run */
+		chipset_init_trx(dev); /* no tx queue wakeup */
+		if (priv->echo_skb) { /* pending echo? */
+			kfree_skb(priv->echo_skb);
+			priv->echo_skb = NULL;
+		}
+		netif_wake_queue(dev); /* wakeup here */
+	}
+	else
+		DBG(KERN_INFO "%s: %s: can_restart_dev already active.\n",
+		    dev->name, __FUNCTION__ );
+
+}
+
+static void can_restart_on(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	if (!(priv->timer.expires)){ /* no restart on the run */
+
+		set_reset_mode(dev);
+
+		priv->timer.function = can_restart_dev;
+		priv->timer.data = (unsigned long) dev;
+
+		/* restart chip on persistent error in <xxx> ms */
+		priv->timer.expires = jiffies + (priv->restart_ms * HZ) / 1000;
+		add_timer(&priv->timer);
+
+		iDBG(KERN_INFO "%s: %s start (%ld)\n",
+		     dev->name, __FUNCTION__ , jiffies);
+	} else
+		iDBG(KERN_INFO "%s: %s already (%ld)\n",
+		     dev->name, __FUNCTION__ , jiffies);
+}
+
+static void can_restart_dev(unsigned long data)
+{
+	struct net_device *dev = (struct net_device*) data;
+	struct can_priv *priv = netdev_priv(dev);
+
+	DBG(KERN_INFO "%s: can_restart_dev (%ld)\n",
+	    dev->name, jiffies);
+
+	/* mark inactive timer */
+	priv->timer.expires = 0;
+
+	if (priv->state != STATE_UNINITIALIZED) {
+
+		/* count number of restarts */
+		priv->can_stats.restarts++;
+
+		chipset_init(dev, 1);
+	}
+}
+
+#if 0
+/* the timerless version */
+
+static void can_restart_now(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	if (priv->state != STATE_UNINITIALIZED) {
+
+		/* count number of restarts */
+		priv->can_stats.restarts++;
+
+		chipset_init(dev, 1);
+	}
+}
+#endif
+
+static void can_rx(struct net_device *dev)
+{
+	struct net_device_stats *stats = &dev->stats;
+	unsigned long base = dev->base_addr;
+	struct can_frame *cf;
+	struct sk_buff	*skb;
+	uint8_t	fi;
+	uint8_t	dreg;
+	canid_t	id;
+	uint8_t	dlc;
+	int	i;
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (skb == NULL) {
+		return;
+	}
+	skb->dev = dev;
+	skb->protocol = htons(ETH_P_CAN);
+
+	fi = hw_readreg(base, REG_FI);
+	dlc = fi & 0x0F;
+
+	if (fi & FI_FF) {
+		/* extended frame format (EFF) */
+		dreg = EFF_BUF;
+		id = (hw_readreg(base, REG_ID1) << (5+16))
+			| (hw_readreg(base, REG_ID2) << (5+8))
+			| (hw_readreg(base, REG_ID3) << 5)
+			| (hw_readreg(base, REG_ID4) >> 3);
+		id |= CAN_EFF_FLAG;
+	} else {
+		/* standard frame format (SFF) */
+		dreg = SFF_BUF;
+		id = (hw_readreg(base, REG_ID1) << 3)
+			| (hw_readreg(base, REG_ID2) >> 5);
+	}
+
+	if (fi & FI_RTR)
+		id |= CAN_RTR_FLAG;
+
+	cf = (struct can_frame*)skb_put(skb, sizeof(struct can_frame));
+	memset(cf, 0, sizeof(struct can_frame));
+	cf->can_id    = id;
+	cf->can_dlc   = dlc;
+	for (i = 0; i < dlc; i++) {
+		cf->data[i] = hw_readreg(base, dreg++);
+	}
+	while (i < 8)
+		cf->data[i++] = 0;
+
+	/* release receive buffer */
+	hw_writereg(base, REG_CMR, CMD_RRB);
+
+	netif_rx(skb);
+
+	dev->last_rx = jiffies;
+	stats->rx_packets++;
+	stats->rx_bytes += dlc;
+}
+
+/*
+ * SJA1000 interrupt handler
+ */
+static irqreturn_t can_interrupt(int irq, void *dev_id)
+{
+	struct net_device *dev	= (struct net_device*)dev_id;
+	struct can_priv *priv	= netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	unsigned long base	= dev->base_addr;
+	uint8_t isrc, status, ecc, alc;
+	int n = 0;
+
+	hw_preirq(dev);
+
+	iiDBG(KERN_INFO "%s: interrupt\n", dev->name);
+
+	if (priv->state == STATE_UNINITIALIZED) {
+		printk(KERN_ERR "%s: %s: uninitialized controller!\n",
+		       dev->name, __FUNCTION__);
+		chipset_init(dev, 1); /* should be possible at this stage */
+		return IRQ_NONE;
+	}
+
+	if (priv->state == STATE_RESET_MODE) {
+		iiDBG(KERN_ERR "%s: %s: controller is in reset mode! "
+		      "MOD=0x%02X IER=0x%02X IR=0x%02X SR=0x%02X!\n",
+		      dev->name, __FUNCTION__, hw_readreg(base, REG_MOD),
+		      hw_readreg(base, REG_IER), hw_readreg(base, REG_IR),
+		      hw_readreg(base, REG_SR));
+		return IRQ_NONE;
+	}
+
+	while ((isrc = hw_readreg(base, REG_IR)) && (n < 20)) {
+		n++;
+		status = hw_readreg(base, REG_SR);
+
+		if (isrc & IRQ_WUI) {
+			/* wake-up interrupt */
+			priv->can_stats.wakeup++;
+		}
+		if (isrc & IRQ_TI) {
+			/* transmission complete interrupt */
+			stats->tx_packets++;
+
+			if (echo && priv->echo_skb) {
+				netif_rx(priv->echo_skb);
+				priv->echo_skb = NULL;
+			}
+
+			netif_wake_queue(dev);
+		}
+		if (isrc & IRQ_RI) {
+			/* receive interrupt */
+
+			while (status & SR_RBS) {
+				can_rx(dev);
+				status = hw_readreg(base, REG_SR);
+			}
+			if (priv->state == STATE_PROBE) {
+				/* valid RX -> switch to trx-mode */
+				iDBG(KERN_INFO "%s: RI #%d#\n", dev->name, n);
+				chipset_init_trx(dev); /* no tx queue wakeup */
+				break; /* check again after init controller */
+			}
+		}
+		if (isrc & IRQ_DOI) {
+			/* data overrun interrupt */
+			iiDBG(KERN_INFO "%s: data overrun isrc=0x%02X "
+			      "status=0x%02X\n",
+			      dev->name, isrc, status);
+			iDBG(KERN_INFO "%s: DOI #%d#\n", dev->name, n);
+			priv->can_stats.data_overrun++;
+			hw_writereg(base, REG_CMR, CMD_CDO); /* clear bit */
+		}
+		if (isrc & IRQ_EI) {
+			/* error warning interrupt */
+			iiDBG(KERN_INFO "%s: error warning isrc=0x%02X "
+			      "status=0x%02X\n",
+			      dev->name, isrc, status);
+			iDBG(KERN_INFO "%s: EI #%d#\n", dev->name, n);
+			priv->can_stats.error_warning++;
+			if (status & SR_BS) {
+				printk(KERN_INFO "%s: BUS OFF, "
+				       "restarting device\n", dev->name);
+				can_restart_on(dev);
+				/* controller has been restarted: leave here */
+				goto out;
+			} else if (status & SR_ES) {
+				iDBG(KERN_INFO "%s: error\n", dev->name);
+			}
+		}
+		if (isrc & IRQ_BEI) {
+			/* bus error interrupt */
+			iiDBG(KERN_INFO "%s: bus error isrc=0x%02X "
+			      "status=0x%02X\n",
+			      dev->name, isrc, status);
+			iDBG(KERN_INFO "%s: BEI #%d# [%d]\n", dev->name, n,
+			     priv->can_stats.bus_error - 
+			     priv->can_stats.bus_error_at_init);
+			priv->can_stats.bus_error++;
+			ecc = hw_readreg(base, REG_ECC);
+			iDBG(KERN_INFO "%s: ECC = 0x%02X (%s, %s, %s)\n",
+			     dev->name, ecc,
+			     (ecc & ECC_DIR) ? "RX" : "TX",
+			     ecc_types[ecc >> ECC_ERR],
+			     ecc_errors[ecc & ECC_SEG]);
+
+			/* when the bus errors flood the system, */
+			/* restart the controller                */
+			if (priv->can_stats.bus_error_at_init +
+			    MAX_BUS_ERRORS < priv->can_stats.bus_error) {
+				iDBG(KERN_INFO "%s: heavy bus errors,"
+				     " restarting device\n", dev->name);
+				can_restart_on(dev);
+				/* controller has been restarted: leave here */
+				goto out;
+			}
+#if 1
+			/* don't know, if this is a good idea, */
+			/* but it works fine ...               */
+			if (hw_readreg(base, REG_RXERR) > 128) {
+				iDBG(KERN_INFO "%s: RX_ERR > 128,"
+				     " restarting device\n", dev->name);
+				can_restart_on(dev);
+				/* controller has been restarted: leave here */
+				goto out;
+			}
+#endif
+		}
+		if (isrc & IRQ_EPI) {
+			/* error passive interrupt */
+			iiDBG(KERN_INFO "%s: error passive isrc=0x%02X"
+			      " status=0x%02X\n",
+			      dev->name, isrc, status);
+			iDBG(KERN_INFO "%s: EPI #%d#\n", dev->name, n);
+			priv->can_stats.error_passive++;
+			if (status & SR_ES) {
+				iDBG(KERN_INFO "%s: -> ERROR PASSIVE, "
+				     "restarting device\n", dev->name);
+				can_restart_on(dev);
+				/* controller has been restarted: leave here */
+				goto out;
+			} else {
+				iDBG(KERN_INFO "%s: -> ERROR ACTIVE\n",
+				     dev->name);
+			}
+		}
+		if (isrc & IRQ_ALI) {
+			/* arbitration lost interrupt */
+			iiDBG(KERN_INFO "%s: error arbitration lost "
+			      "isrc=0x%02X status=0x%02X\n",
+			      dev->name, isrc, status);
+			iDBG(KERN_INFO "%s: ALI #%d#\n", dev->name, n);
+			priv->can_stats.arbitration_lost++;
+			alc = hw_readreg(base, REG_ALC);
+			iDBG(KERN_INFO "%s: ALC = 0x%02X\n", dev->name, alc);
+		}
+	}
+	if (n > 1) {
+		iDBG(KERN_INFO "%s: handled %d IRQs\n", dev->name, n);
+	}
+out:
+	hw_postirq(dev);
+
+	return n == 0 ? IRQ_NONE : IRQ_HANDLED;
+}
+
+/*
+ * initialize CAN bus driver
+ */
+static int can_open(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	/* set chip into reset mode */
+	set_reset_mode(dev);
+
+	priv->state = STATE_UNINITIALIZED;
+
+	/* register interrupt handler */
+	if (request_irq(dev->irq, &can_interrupt, IRQF_SHARED,
+			dev->name, (void*)dev)) {
+		return -EAGAIN;
+	}
+
+	/* init chip */
+	chipset_init(dev, 0);
+	priv->open_time = jiffies;
+
+	netif_start_queue(dev);
+
+	return 0;
+}
+
+/*
+ * stop CAN bus activity
+ */
+static int can_close(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	/* set chip into reset mode */
+	set_reset_mode(dev);
+
+	priv->open_time = 0;
+
+	if (priv->timer.expires) {
+		del_timer(&priv->timer);
+		priv->timer.expires = 0;
+	}
+
+	free_irq(dev->irq, (void*)dev);
+	priv->state = STATE_UNINITIALIZED;
+
+	netif_stop_queue(dev);
+
+	return 0;
+}
+
+#if 0
+static void test_if(struct net_device *dev)
+{
+	int i;
+	int j;
+	int x;
+
+	hw_writereg(base, REG_CDR, 0xCF);
+	for (i = 0; i < 10000; i++) {
+		for (j = 0; j < 256; j++) {
+			hw_writereg(base, REG_EWL, j);
+			x = hw_readreg(base, REG_EWL);
+			if (x != j) {
+				printk(KERN_INFO "%s: is: %02X expected: "
+				       "%02X (%d)\n", dev->name, x, j, i);
+			}
+		}
+	}
+}
+#endif
+
+void can_netdev_setup(struct net_device *dev)
+{
+	/* Fill in the the fields of the device structure
+	   with CAN netdev generic values */
+
+	dev->change_mtu			= NULL;
+	dev->set_mac_address		= NULL;
+	dev->header_ops			= NULL;
+
+	dev->type			= ARPHRD_CAN;
+	dev->hard_header_len		= 0;
+	dev->mtu			= sizeof(struct can_frame);
+	dev->addr_len			= 0;
+	dev->tx_queue_len		= 10;
+
+	dev->flags			= IFF_NOARP;
+
+	/* set flags according to driver capabilities */
+	if (echo)
+		dev->flags |= IFF_ECHO;
+
+	dev->features			= NETIF_F_NO_CSUM;
+
+	dev->open			= can_open;
+	dev->stop			= can_close;
+	dev->hard_start_xmit		= can_start_xmit;
+
+	dev->tx_timeout			= can_tx_timeout;
+	dev->watchdog_timeo		= TX_TIMEOUT;
+}
+
+static struct net_device* can_create_netdev(int dev_num, int hw_regs)
+{
+	struct net_device	*dev;
+	struct can_priv		*priv;
+
+	if (!(dev = alloc_netdev(sizeof(struct can_priv), CAN_NETDEV_NAME,
+				 can_netdev_setup))) {
+		printk(KERN_ERR "%s: out of memory\n", CHIP_NAME);
+		return NULL;
+	}
+
+	printk(KERN_INFO "%s: base 0x%lX / irq %d / speed %d / "
+	       "btr 0x%X / rx_probe %d\n",
+	       drv_name, rbase[dev_num], irq[dev_num],
+	       speed[dev_num], btr[dev_num], rx_probe[dev_num]);
+
+	/* fill net_device structure */
+
+	priv             = netdev_priv(dev);
+
+	dev->irq         = irq[dev_num];
+	dev->base_addr   = rbase[dev_num];
+
+	priv->speed      = speed[dev_num];
+	priv->btr        = btr[dev_num];
+	priv->rx_probe   = rx_probe[dev_num];
+	priv->clock      = clk;
+	priv->hw_regs    = hw_regs;
+	priv->restart_ms = restart_ms;
+	priv->debug      = debug;
+
+	init_timer(&priv->timer);
+	priv->timer.expires = 0;
+
+	if (register_netdev(dev)) {
+		printk(KERN_INFO "%s: register netdev failed\n", CHIP_NAME);
+		free_netdev(dev);
+		return NULL;
+	}
+
+	return dev;
+}
+
+int can_set_drv_name(void)
+{
+	char *hname = hal_name();
+
+	if (strlen(CHIP_NAME) + strlen(hname) >= DRV_NAME_LEN-1) {
+		printk(KERN_ERR "%s: driver name too long!\n", CHIP_NAME);
+		return -EINVAL;
+	}
+	sprintf(drv_name, "%s-%s", CHIP_NAME, hname);
+	return 0;
+}
+
+static void sja1000_exit_module(void)
+{
+	int i, ret;
+
+	for (i = 0; i < MAXDEV; i++) {
+		if (can_dev[i] != NULL) {
+			struct can_priv *priv = netdev_priv(can_dev[i]);
+			unregister_netdev(can_dev[i]);
+			del_timer(&priv->timer);
+			hw_detach(i);
+			hal_release_region(i, SJA1000_IO_SIZE_BASIC);
+			free_netdev(can_dev[i]);
+		}
+	}
+	can_proc_remove(drv_name);
+
+	if ((ret = hal_exit()))
+		printk(KERN_INFO "%s: hal_exit error %d.\n", drv_name, ret);
+}
+
+static __init int sja1000_init_module(void)
+{
+	int i, ret;
+	struct net_device *dev;
+
+	if ((ret = hal_init()))
+		return ret;
+
+	if ((ret = can_set_drv_name()))
+		return ret;
+
+	if (clk < 1000 ) /* MHz command line value */
+		clk *= 1000000;
+
+	if (clk < 1000000 ) /* kHz command line value */
+		clk *= 1000;
+
+	printk(KERN_INFO "%s driver v%s (%s)\n",
+	       drv_name, drv_version, drv_reldate);
+	printk(KERN_INFO "%s - options [clk %d.%06d MHz] [restart_ms %dms]"
+	       " [debug %d]\n",
+	       drv_name, clk/1000000, clk%1000000, restart_ms, debug);
+
+	if (!base[0]) {
+		printk(KERN_INFO "%s: loading defaults.\n", drv_name);
+		hal_use_defaults();
+	}
+		
+	for (i = 0; base[i]; i++) {
+		printk(KERN_DEBUG "%s: checking for %s on address 0x%lX ...\n",
+		       drv_name, CHIP_NAME, base[i]);
+
+		if (!hal_request_region(i, SJA1000_IO_SIZE_BASIC, drv_name)) {
+			printk(KERN_ERR "%s: memory already in use\n",
+			       drv_name);
+			sja1000_exit_module();
+			return -EBUSY;
+		}
+
+		hw_attach(i);
+		hw_reset_dev(i);
+
+		if (!sja1000_probe_chip(rbase[i])) {
+			printk(KERN_ERR "%s: probably missing controller"
+			       " hardware\n", drv_name);
+			hw_detach(i);
+			hal_release_region(i, SJA1000_IO_SIZE_BASIC);
+			sja1000_exit_module();
+			return -ENODEV;
+		}
+
+		dev = can_create_netdev(i, SJA1000_IO_SIZE_BASIC);
+
+		if (dev != NULL) {
+			can_dev[i] = dev;
+			set_reset_mode(dev);
+			can_proc_create(drv_name);
+		} else {
+			can_dev[i] = NULL;
+			hw_detach(i);
+			hal_release_region(i, SJA1000_IO_SIZE_BASIC);
+		}
+	}
+	return 0;
+}
+
+module_init(sja1000_init_module);
+module_exit(sja1000_exit_module);
+
diff --git a/drivers/net/can/old/sja1000/sja1000.h b/drivers/net/can/old/sja1000/sja1000.h
new file mode 100644
index 000000000000..a47b2f629136
--- /dev/null
+++ b/drivers/net/can/old/sja1000/sja1000.h
@@ -0,0 +1,187 @@
+/*
+ * sja1000.h -  Philips SJA1000 network device driver
+ *
+ * Copyright (c) 2003 Matthias Brukner, Trajet Gmbh, Rebenring 33,
+ * 38106 Braunschweig, GERMANY
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#ifndef SJA1000_H
+#define SJA1000_H
+
+#define SJA1000_IO_SIZE_BASIC   0x20
+#define SJA1000_IO_SIZE_PELICAN 0x80 /* unused */
+
+#define CHIP_NAME	"sja1000"
+
+#define DRV_NAME_LEN	30 /* for "<chip_name>-<hal_name>" */
+
+#define PROCBASE          "driver" /* /proc/ ... */
+
+#define DEFAULT_HW_CLK	16000000
+#define DEFAULT_SPEED	500 /* kBit/s */
+
+#define CAN_NETDEV_NAME	"can%d"
+
+#define TX_TIMEOUT      (50*HZ/1000) /* 50ms */ 
+#define RESTART_MS      100  /* restart chip on persistent errors in 100ms */
+#define MAX_BUS_ERRORS  200  /* prevent from flooding bus error interrupts */
+
+/* SJA1000 registers - manual section 6.4 (Pelican Mode) */
+#define REG_MOD		0x00
+#define REG_CMR		0x01
+#define REG_SR		0x02
+#define REG_IR		0x03
+#define REG_IER		0x04
+#define REG_ALC		0x0B
+#define REG_ECC		0x0C
+#define REG_EWL		0x0D
+#define REG_RXERR	0x0E
+#define REG_TXERR	0x0F
+#define REG_ACCC0	0x10
+#define REG_ACCC1	0x11
+#define REG_ACCC2	0x12
+#define REG_ACCC3	0x13
+#define REG_ACCM0	0x14
+#define REG_ACCM1	0x15
+#define REG_ACCM2	0x16
+#define REG_ACCM3	0x17
+#define REG_RMC		0x1D
+#define REG_RBSA	0x1E
+
+/* Common registers - manual section 6.5 */
+#define REG_BTR0	0x06
+#define REG_BTR1	0x07
+#define REG_OCR		0x08
+#define REG_CDR		0x1F
+
+#define REG_FI		0x10
+#define SFF_BUF		0x13
+#define EFF_BUF		0x15
+
+#define FI_FF		0x80
+#define FI_RTR		0x40
+
+#define REG_ID1		0x11
+#define REG_ID2		0x12
+#define REG_ID3		0x13
+#define REG_ID4		0x14
+
+#define CAN_RAM		0x20
+
+/* mode register */
+#define MOD_RM		0x01
+#define MOD_LOM		0x02
+#define MOD_STM		0x04
+#define MOD_AFM		0x08
+#define MOD_SM		0x10
+
+/* commands */
+#define CMD_SRR		0x10
+#define CMD_CDO		0x08
+#define CMD_RRB		0x04
+#define CMD_AT		0x02
+#define CMD_TR		0x01
+
+/* interrupt sources */
+#define IRQ_BEI		0x80
+#define IRQ_ALI		0x40
+#define IRQ_EPI		0x20
+#define IRQ_WUI		0x10
+#define IRQ_DOI		0x08
+#define IRQ_EI		0x04
+#define IRQ_TI		0x02
+#define IRQ_RI		0x01
+#define IRQ_ALL		0xFF
+#define IRQ_OFF		0x00
+
+/* status register content */
+#define SR_BS		0x80
+#define SR_ES		0x40
+#define SR_TS		0x20
+#define SR_RS		0x10
+#define SR_TCS		0x08
+#define SR_TBS		0x04
+#define SR_DOS		0x02
+#define SR_RBS		0x01
+
+#define SR_CRIT (SR_BS|SR_ES)
+
+/* ECC register */
+#define ECC_DIR		0x20
+#define ECC_SEG		0x1F
+#define ECC_ERR		6
+
+/* bus timing */
+#define MAX_TSEG1	15
+#define MAX_TSEG2	 7
+#define SAMPLE_POINT	75
+#define JUMPWIDTH     0x40
+
+/* CAN private data structure */
+
+struct can_priv {
+	struct can_device_stats	can_stats;
+	long			open_time;
+	int			clock;
+	int			hw_regs;
+	int			restart_ms;
+	int			debug;
+	int			speed;
+	int			btr;
+	int			rx_probe;
+	struct timer_list       timer;
+	int			state;
+	struct sk_buff		*echo_skb;
+};
+
+#define STATE_UNINITIALIZED	0
+#define STATE_PROBE		1
+#define STATE_ACTIVE		2
+#define STATE_ERROR_ACTIVE	3
+#define STATE_ERROR_PASSIVE	4
+#define STATE_BUS_OFF		5
+#define STATE_RESET_MODE	6
+
+void can_proc_create(const char *drv_name);
+void can_proc_remove(const char *drv_name);
+
+#endif /* SJA1000_H */
diff --git a/drivers/net/can/sja1000/Makefile b/drivers/net/can/sja1000/Makefile
new file mode 100644
index 000000000000..176393a0001d
--- /dev/null
+++ b/drivers/net/can/sja1000/Makefile
@@ -0,0 +1,30 @@
+#
+#
+
+ifeq ($(KERNELRELEASE),)
+
+KERNELDIR := /lib/modules/$(shell uname -r)/build
+PWD       := $(shell pwd)
+TOPDIR    := $(PWD)/../../../..
+
+modules modules_install clean:
+	$(MAKE) -C $(KERNELDIR) M=$(PWD) $@ TOPDIR=$(TOPDIR)
+
+else
+
+-include $(TOPDIR)/Makefile.common
+#EXTRA_CFLAGS += -I$(TOPDIR)/drivers/net/can/hal
+
+obj-$(CONFIG_CAN_SJA1000) += sja1000.o
+obj-$(CONFIG_CAN_SJA1000_PLATFORM) += sja1000_platform.o
+obj-$(CONFIG_CAN_SJA1000_OF_PLATFORM) += sja1000_of_platform.o
+obj-$(CONFIG_CAN_EMS_PCI) += ems_pci.o
+obj-$(CONFIG_CAN_EMS_PCMCIA) += ems_pcmcia.o
+obj-$(CONFIG_CAN_IXXAT_PCI) += ixxat_pci.o
+obj-$(CONFIG_CAN_PEAK_PCI) += peak_pci.o
+obj-$(CONFIG_CAN_PIPCAN) += pipcan.o
+obj-$(CONFIG_CAN_KVASER_PCI) += kvaser_pci.o
+
+ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
+
+endif
diff --git a/drivers/net/can/sja1000/ems_pci.c b/drivers/net/can/sja1000/ems_pci.c
new file mode 100644
index 000000000000..6076375e2a96
--- /dev/null
+++ b/drivers/net/can/sja1000/ems_pci.c
@@ -0,0 +1,328 @@
+/*
+ * Copyright (C) 2007 Wolfgang Grandegger <wg@grandegger.com>
+ * Copyright (C) 2008 Markus Plessing <plessing@ems-wuensche.com>
+ * Copyright (C) 2008 Sebastian Haas <haas@ems-wuensche.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/delay.h>
+#include <linux/pci.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/io.h>
+
+#include "sja1000.h"
+
+#define DRV_NAME  "ems_pci"
+
+MODULE_AUTHOR("Sebastian Haas <haas@ems-wuenche.com>");
+MODULE_DESCRIPTION("Socket-CAN driver for EMS CPC-PCI/PCIe CAN cards");
+MODULE_SUPPORTED_DEVICE("EMS CPC-PCI/PCIe CAN card");
+MODULE_LICENSE("GPL v2");
+
+#define EMS_PCI_MAX_CHAN 2
+
+struct ems_pci_card {
+	int channels;
+
+	struct pci_dev *pci_dev;
+	struct net_device *net_dev[EMS_PCI_MAX_CHAN];
+
+	void __iomem *conf_addr;
+	void __iomem *base_addr;
+};
+
+#define EMS_PCI_CAN_CLOCK (16000000 / 2)
+
+/*
+ * Register definitions and descriptions are from LinCAN 0.3.3.
+ *
+ * PSB4610 PITA-2 bridge control registers
+ */
+#define PITA2_ICR           0x00	/* Interrupt Control Register */
+#define PITA2_ICR_INT0      0x00000002	/* [RC] INT0 Active/Clear */
+#define PITA2_ICR_INT0_EN   0x00020000	/* [RW] Enable INT0 */
+
+#define PITA2_MISC          0x1c	/* Miscellaneous Register */
+#define PITA2_MISC_CONFIG   0x04000000	/* Multiplexed parallel interface */
+
+/*
+ * The board configuration is probably following:
+ * RX1 is connected to ground.
+ * TX1 is not connected.
+ * CLKO is not connected.
+ * Setting the OCR register to 0xDA is a good idea.
+ * This means  normal output mode , push-pull and the correct polarity.
+ */
+#define EMS_PCI_OCR         (OCR_TX0_PUSHPULL | OCR_TX1_PUSHPULL)
+
+/*
+ * In the CDR register, you should set CBP to 1.
+ * You will probably also want to set the clock divider value to 7
+ * (meaning direct oscillator output) because the second SJA1000 chip
+ * is driven by the first one CLKOUT output.
+ */
+#define EMS_PCI_CDR             (CDR_CBP | CDR_CLKOUT_MASK)
+#define EMS_PCI_MEM_SIZE        4096  /* Size of the remapped io-memory */
+#define EMS_PCI_CAN_BASE_OFFSET 0x400 /* offset where the controllers starts */
+#define EMS_PCI_CAN_CTRL_SIZE   0x200 /* memory size for each controller */
+
+#define EMS_PCI_PORT_BYTES  0x4     /* Each register occupies 4 bytes */
+
+#define EMS_PCI_VENDOR_ID   0x110a  /* PCI device and vendor ID */
+#define EMS_PCI_DEVICE_ID   0x2104
+
+static struct pci_device_id ems_pci_tbl[] = {
+	{EMS_PCI_VENDOR_ID, EMS_PCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
+	{0,}
+};
+MODULE_DEVICE_TABLE(pci, ems_pci_tbl);
+
+/*
+ * Helper to read internal registers from card logic (not CAN)
+ */
+static u8 ems_pci_readb(struct ems_pci_card *card, unsigned int port)
+{
+	return readb((void __iomem *)card->base_addr
+			+ (port * EMS_PCI_PORT_BYTES));
+}
+
+static u8 ems_pci_read_reg(struct net_device *dev, int port)
+{
+	return readb((void __iomem *)dev->base_addr
+			+ (port * EMS_PCI_PORT_BYTES));
+}
+
+static void ems_pci_write_reg(struct net_device *dev, int port, u8 val)
+{
+	writeb(val, (void __iomem *)dev->base_addr
+		+ (port * EMS_PCI_PORT_BYTES));
+}
+
+static void ems_pci_post_irq(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct ems_pci_card *card = (struct ems_pci_card *)priv->priv;
+
+	/* reset int flag of pita */
+	writel(PITA2_ICR_INT0_EN | PITA2_ICR_INT0, card->conf_addr
+		+ PITA2_ICR);
+}
+
+/*
+ * Check if a CAN controller is present at the specified location
+ * by trying to set 'em into the PeliCAN mode
+ */
+static inline int ems_pci_check_chan(struct net_device *dev)
+{
+	unsigned char res;
+
+	/* Make sure SJA1000 is in reset mode */
+	ems_pci_write_reg(dev, REG_MOD, 1);
+
+	ems_pci_write_reg(dev, REG_CDR, CDR_PELICAN);
+
+	/* read reset-values */
+	res = ems_pci_read_reg(dev, REG_CDR);
+
+	if (res == CDR_PELICAN)
+		return 1;
+
+	return 0;
+}
+
+static void ems_pci_del_card(struct pci_dev *pdev)
+{
+	struct ems_pci_card *card = pci_get_drvdata(pdev);
+	struct net_device *dev;
+	int i = 0;
+
+	for (i = 0; i < card->channels; i++) {
+		dev = card->net_dev[i];
+
+		if (!dev)
+			continue;
+
+		dev_info(&pdev->dev, "Removing %s.\n", dev->name);
+		unregister_sja1000dev(dev);
+		free_sja1000dev(dev);
+	}
+
+	if (card->base_addr != NULL)
+		pci_iounmap(card->pci_dev, card->base_addr);
+
+	if (card->conf_addr != NULL)
+		pci_iounmap(card->pci_dev, card->conf_addr);
+
+	kfree(card);
+
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+}
+
+static void ems_pci_card_reset(struct ems_pci_card *card)
+{
+	/* Request board reset */
+	writeb(0, card->base_addr);
+}
+
+/*
+ * Probe PCI device for EMS CAN signature and register each available
+ * CAN channel to SJA1000 Socket-CAN subsystem.
+ */
+static int __devinit ems_pci_add_card(struct pci_dev *pdev,
+					const struct pci_device_id *ent)
+{
+	struct sja1000_priv *priv;
+	struct net_device *dev;
+	struct ems_pci_card *card;
+	int err, i;
+
+	/* Enabling PCI device */
+	if (pci_enable_device(pdev) < 0) {
+		dev_err(&pdev->dev, "Enabling PCI device failed\n");
+		return -ENODEV;
+	}
+
+	/* Allocating card structures to hold addresses, ... */
+	card = kzalloc(sizeof(struct ems_pci_card), GFP_KERNEL);
+	if (card == NULL) {
+		dev_err(&pdev->dev, "Unable to allocate memory\n");
+		pci_disable_device(pdev);
+		return -ENOMEM;
+	}
+
+	pci_set_drvdata(pdev, card);
+
+	card->pci_dev = pdev;
+
+	card->channels = 0;
+
+	/* Remap PITA configuration space, and controller memory area */
+	card->conf_addr = pci_iomap(pdev, 0, EMS_PCI_MEM_SIZE);
+	if (card->conf_addr == NULL) {
+		err = -ENOMEM;
+
+		goto failure_cleanup;
+	}
+
+	card->base_addr = pci_iomap(pdev, 1, EMS_PCI_MEM_SIZE);
+	if (card->base_addr == NULL) {
+		err = -ENOMEM;
+
+		goto failure_cleanup;
+	}
+
+	/* Configure PITA-2 parallel interface (enable MUX) */
+	writel(PITA2_MISC_CONFIG, card->conf_addr + PITA2_MISC);
+
+	/* Check for unique EMS CAN signature */
+	if (ems_pci_readb(card, 0) != 0x55 ||
+	    ems_pci_readb(card, 1) != 0xAA ||
+	    ems_pci_readb(card, 2) != 0x01 ||
+	    ems_pci_readb(card, 3) != 0xCB ||
+	    ems_pci_readb(card, 4) != 0x11) {
+		dev_err(&pdev->dev, "Not EMS Dr. Thomas Wuensche interface\n");
+
+		err = -ENODEV;
+		goto failure_cleanup;
+	}
+
+	ems_pci_card_reset(card);
+
+	/* Detect available channels */
+	for (i = 0; i < EMS_PCI_MAX_CHAN; i++) {
+		dev = alloc_sja1000dev(0);
+		if (dev == NULL) {
+			err = -ENOMEM;
+			goto failure_cleanup;
+		}
+
+		card->net_dev[i] = dev;
+		priv = netdev_priv(dev);
+		priv->priv = card;
+
+		dev->irq = pdev->irq;
+		dev->base_addr = (unsigned long)(card->base_addr
+						+ EMS_PCI_CAN_BASE_OFFSET
+						+ (i * EMS_PCI_CAN_CTRL_SIZE));
+
+		/* Check if channel is present */
+		if (ems_pci_check_chan(dev)) {
+			priv->read_reg  = ems_pci_read_reg;
+			priv->write_reg = ems_pci_write_reg;
+			priv->post_irq  = ems_pci_post_irq;
+			priv->can.bittiming.clock = EMS_PCI_CAN_CLOCK;
+			priv->ocr = EMS_PCI_OCR;
+			priv->cdr = EMS_PCI_CDR;
+
+			SET_NETDEV_DEV(dev, &pdev->dev);
+
+			/* Enable interrupts from card */
+			writel(PITA2_ICR_INT0_EN, card->conf_addr + PITA2_ICR);
+
+			/* Register SJA1000 device */
+			err = register_sja1000dev(dev);
+			if (err) {
+				dev_err(&pdev->dev, "Registering device failed "
+							"(err=%d)\n", err);
+				free_sja1000dev(dev);
+				goto failure_cleanup;
+			}
+
+			card->channels++;
+
+			dev_info(&pdev->dev, "Channel #%d at %#lX, irq %d\n",
+						i + 1, dev->base_addr,
+						dev->irq);
+		} else {
+			free_sja1000dev(dev);
+		}
+	}
+
+	return 0;
+
+failure_cleanup:
+	dev_err(&pdev->dev, "Error: %d. Cleaning Up.\n", err);
+
+	ems_pci_del_card(pdev);
+
+	return err;
+}
+
+static struct pci_driver ems_pci_driver = {
+	.name = DRV_NAME,
+	.id_table = ems_pci_tbl,
+	.probe = ems_pci_add_card,
+	.remove = ems_pci_del_card,
+};
+
+static int __init ems_pci_init(void)
+{
+	return pci_register_driver(&ems_pci_driver);
+}
+
+static void __exit ems_pci_exit(void)
+{
+	pci_unregister_driver(&ems_pci_driver);
+}
+
+module_init(ems_pci_init);
+module_exit(ems_pci_exit);
+
diff --git a/drivers/net/can/sja1000/ems_pcmcia.c b/drivers/net/can/sja1000/ems_pcmcia.c
new file mode 100644
index 000000000000..7c9b88b30301
--- /dev/null
+++ b/drivers/net/can/sja1000/ems_pcmcia.c
@@ -0,0 +1,418 @@
+/*
+ * Copyright (C) 2008 Sebastian Haas <haas@ems-wuensche.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/delay.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <asm/io.h>
+
+#include <pcmcia/cs_types.h>
+#include <pcmcia/cs.h>
+#include <pcmcia/cistpl.h>
+#include <pcmcia/ciscode.h>
+#include <pcmcia/ds.h>
+#include <pcmcia/cisreg.h>
+
+#include "sja1000.h"
+
+#define DRV_NAME  "ems_pcmcia"
+
+MODULE_AUTHOR("Sebastian Haas <haas@ems-wuenche.com>");
+MODULE_DESCRIPTION("Socket-CAN driver for EMS CPC-CARD cards");
+MODULE_SUPPORTED_DEVICE("EMS CPC-CARD CAN card");
+MODULE_LICENSE("GPL v2");
+
+static int debug;
+
+module_param(debug, int, S_IRUGO | S_IWUSR);
+
+MODULE_PARM_DESC(debug, "Set debug level (default: 0)");
+
+#define EMS_PCMCIA_MAX_CHAN 2
+
+struct ems_pcmcia_card {
+	int channels;
+
+	struct pcmcia_device *pcmcia_dev;
+	struct net_device *net_dev[EMS_PCMCIA_MAX_CHAN];
+
+	void __iomem *base_addr;
+};
+
+#define EMS_PCMCIA_CAN_CLOCK (16000000 / 2)
+
+/*
+ * The board configuration is probably following:
+ * RX1 is connected to ground.
+ * TX1 is not connected.
+ * CLKO is not connected.
+ * Setting the OCR register to 0xDA is a good idea.
+ * This means  normal output mode , push-pull and the correct polarity.
+ */
+#define EMS_PCMCIA_OCR         (OCR_TX0_PUSHPULL | OCR_TX1_PUSHPULL)
+
+/*
+ * In the CDR register, you should set CBP to 1.
+ * You will probably also want to set the clock divider value to 7
+ * (meaning direct oscillator output) because the second SJA1000 chip
+ * is driven by the first one CLKOUT output.
+ */
+#define EMS_PCMCIA_CDR             (CDR_CBP | CDR_CLKOUT_MASK)
+#define EMS_PCMCIA_MEM_SIZE        4096  /* Size of the remapped io-memory */
+#define EMS_PCMCIA_CAN_BASE_OFFSET 0x100 /* Offset where controllers starts */
+#define EMS_PCMCIA_CAN_CTRL_SIZE   0x80  /* Memory size for each controller */
+
+#define EMS_CMD_RESET 0x00  /* Perform a reset of the card */
+#define EMS_CMD_MAP   0x03  /* Map CAN controllers into card' memory */
+#define EMS_CMD_UMAP  0x02  /* Unmap CAN controllers from card' memory */
+
+static struct pcmcia_device_id ems_pcmcia_tbl[] = {
+	PCMCIA_DEVICE_PROD_ID123("EMS_T_W", "CPC-Card", "V2.0", 0xeab1ea23,
+				 0xa338573f, 0xe4575800),
+	PCMCIA_DEVICE_NULL,
+};
+
+MODULE_DEVICE_TABLE (pcmcia, ems_pcmcia_tbl);
+
+static void ems_pcmcia_config(struct pcmcia_device *dev);
+
+static u8 ems_pcmcia_read_reg(struct net_device *dev, int port)
+{
+	return readb((void __iomem *)dev->base_addr + port);
+}
+
+static void ems_pcmcia_write_reg(struct net_device *dev, int port, u8 val)
+{
+	writeb(val, (void __iomem *)dev->base_addr + port);
+}
+
+static irqreturn_t ems_pcmcia_interrupt(int irq, void *dev_id)
+{
+	struct ems_pcmcia_card *card = dev_id;
+	struct net_device *dev;
+	irqreturn_t retval = IRQ_NONE;
+	int i, again;
+
+	do {
+		again = 0;
+
+		/* Check interrupt for each channel */
+		for (i = 0; i < EMS_PCMCIA_MAX_CHAN; i++) {
+			dev = card->net_dev[i];
+			if (!dev)
+				continue;
+
+			if (sja1000_interrupt(irq, dev) == IRQ_HANDLED)
+			    again = 1;
+		}
+		/* At least one channel handled the interrupt */
+		if (again)
+			retval = IRQ_HANDLED;
+
+	} while (again);
+
+	return retval;
+}
+
+/*
+ * Check if a CAN controller is present at the specified location
+ * by trying to set 'em into the PeliCAN mode
+ */
+static inline int ems_pcmcia_check_chan(struct net_device *dev)
+{
+	unsigned char res;
+
+	/* Make sure SJA1000 is in reset mode */
+	ems_pcmcia_write_reg(dev, REG_MOD, 1);
+
+	ems_pcmcia_write_reg(dev, REG_CDR, CDR_PELICAN);
+
+	/* read reset-values */
+	res = ems_pcmcia_read_reg(dev, REG_CDR);
+
+	if (res == CDR_PELICAN)
+		return 1;
+
+	return 0;
+}
+
+static void ems_pcmcia_del_card(struct pcmcia_device *pdev)
+{
+	struct ems_pcmcia_card *card = pdev->priv;
+	struct net_device *dev;
+	int i = 0;
+
+	if (!card)
+		return;
+
+	free_irq(pdev->irq.AssignedIRQ, card);
+
+	for (i = 0; i < card->channels; i++) {
+		dev = card->net_dev[i];
+
+		if (!dev)
+			continue;
+
+		printk(KERN_INFO "%s: removing %s on channel #%d\n",
+		       DRV_NAME, dev->name, i);
+		unregister_sja1000dev(dev);
+		free_sja1000dev(dev);
+	}
+
+	writeb(EMS_CMD_UMAP, card->base_addr);
+
+	if (card->base_addr != NULL )
+		iounmap(card->base_addr);
+
+	kfree(card);
+
+	pdev->priv = NULL;
+}
+
+static void ems_pcmcia_card_reset(struct ems_pcmcia_card *card)
+{
+	/* Request board reset */
+	writeb(EMS_CMD_RESET, card->base_addr);
+}
+
+/*
+ * Probe PCI device for EMS CAN signature and register each available
+ * CAN channel to SJA1000 Socket-CAN subsystem.
+ */
+static int __devinit ems_pcmcia_add_card(struct pcmcia_device *pdev,
+					 unsigned long base)
+{
+	struct sja1000_priv *priv;
+	struct net_device *dev;
+	struct ems_pcmcia_card *card;
+	int err, i;
+
+	/* Allocating card structures to hold addresses, ... */
+	card = kzalloc(sizeof(struct ems_pcmcia_card), GFP_KERNEL);
+	if (card == NULL) {
+		printk(KERN_ERR "%s: unable to allocate memory\n", DRV_NAME);
+		return -ENOMEM;
+	}
+
+	pdev->priv = card;
+
+	card->channels = 0;
+
+	card->base_addr = ioremap(base, EMS_PCMCIA_MEM_SIZE);
+	if (card->base_addr == NULL) {
+		err = -ENOMEM;
+		goto failure_cleanup;
+	}
+
+	/* Check for unique EMS CAN signature */
+	if (readw(card->base_addr) != 0xAA55) {
+		printk(KERN_ERR "%s: No EMS CPC Card hardware found.\n",
+		       DRV_NAME);
+
+		err = -ENODEV;
+		goto failure_cleanup;
+	}
+
+	ems_pcmcia_card_reset(card);
+
+	/* Make sure CAN controllers are mapped into card's memory space */
+	writeb(EMS_CMD_MAP, card->base_addr);
+
+	/* Detect available channels */
+	for (i = 0; i < EMS_PCMCIA_MAX_CHAN; i++) {
+		dev = alloc_sja1000dev(0);
+		if (dev == NULL) {
+			err = -ENOMEM;
+			goto failure_cleanup;
+		}
+
+		card->net_dev[i] = dev;
+		priv = netdev_priv(dev);
+		priv->priv = card;
+		SET_NETDEV_DEV(dev, &pdev->dev);
+
+		dev->irq = pdev->irq.AssignedIRQ;
+		dev->base_addr = (unsigned long)(card->base_addr
+					+ EMS_PCMCIA_CAN_BASE_OFFSET
+					+ (i * EMS_PCMCIA_CAN_CTRL_SIZE));
+
+		/* Check if channel is present */
+		if (ems_pcmcia_check_chan(dev)) {
+			priv->read_reg  = ems_pcmcia_read_reg;
+			priv->write_reg = ems_pcmcia_write_reg;
+			priv->can.bittiming.clock = EMS_PCMCIA_CAN_CLOCK;
+			priv->ocr = EMS_PCMCIA_OCR;
+			priv->cdr = EMS_PCMCIA_CDR;
+			priv->flags |= SJA1000_CUSTOM_IRQ_HANDLER;
+
+			/* Register SJA1000 device */
+			err = register_sja1000dev(dev);
+			if (err) {
+				printk(KERN_INFO "%s: registering device "
+				       "failed (err=%d)\n", DRV_NAME, err);
+				free_sja1000dev(dev);
+				goto failure_cleanup;
+			}
+
+			card->channels++;
+
+			printk(KERN_INFO "%s: registered %s on channel "
+			       "#%d at %lX, irq %d\n", DRV_NAME, dev->name,
+			       i, dev->base_addr, dev->irq);
+		} else {
+			free_sja1000dev(dev);
+		}
+	}
+
+	err = request_irq(dev->irq, &ems_pcmcia_interrupt, IRQF_SHARED,
+		DRV_NAME, (void *)card);
+	if (err) {
+		printk(KERN_INFO "Registering device failed (err=%d)\n", err);
+
+		goto failure_cleanup;
+	}
+
+	return 0;
+
+failure_cleanup:
+	printk(KERN_ERR "Error: %d. Cleaning Up.\n", err);
+
+	ems_pcmcia_del_card(pdev);
+
+	return err;
+}
+
+/*
+ * Setup PCMCIA socket and probe for EMS CPC-CARD
+ */
+static int __devinit ems_pcmcia_probe(struct pcmcia_device *dev)
+{
+	/* The io structure describes IO port mapping */
+	dev->io.NumPorts1 = 16;
+	dev->io.Attributes1 = IO_DATA_PATH_WIDTH_8;
+	dev->io.NumPorts2 = 16;
+	dev->io.Attributes2 = IO_DATA_PATH_WIDTH_16;
+	dev->io.IOAddrLines = 5;
+
+	/* Interrupt setup */
+	dev->irq.Attributes = IRQ_TYPE_DYNAMIC_SHARING;
+	dev->irq.IRQInfo1 = IRQ_LEVEL_ID;
+
+	/* General socket configuration */
+	dev->conf.Attributes = CONF_ENABLE_IRQ;
+	dev->conf.IntType = INT_MEMORY_AND_IO;
+	dev->conf.ConfigIndex = 1;
+	dev->conf.Present = PRESENT_OPTION;
+
+	dev->win = NULL;
+
+	ems_pcmcia_config(dev);
+
+	return 0;
+}
+
+/*
+ * Configure PCMCIA socket
+ */
+static void ems_pcmcia_config(struct pcmcia_device *dev)
+{
+	win_req_t req;
+	memreq_t mem;
+
+	int csval;
+
+	/* Allocate a memory window */
+	req.Attributes = WIN_DATA_WIDTH_8|WIN_MEMORY_TYPE_CM|WIN_ENABLE;
+	req.Base = req.Size = 0;
+	req.AccessSpeed = 0;
+
+	csval = pcmcia_request_window(&dev, &req, &dev->win);
+	if (csval) {
+		cs_error(dev, RequestWindow, csval);
+		return;
+	}
+
+	mem.CardOffset = mem.Page = 0;
+	mem.CardOffset = dev->conf.ConfigBase;
+
+	csval = pcmcia_map_mem_page(dev->win, &mem);
+	if (csval) {
+		cs_error(dev, MapMemPage, csval);
+		return;
+	}
+
+	csval = pcmcia_request_irq(dev, &dev->irq);
+	if (csval) {
+		cs_error(dev, RequestIRQ, csval);
+		return;
+	}
+
+	/* This actually configures the PCMCIA socket */
+	csval = pcmcia_request_configuration(dev, &dev->conf);
+	if (csval) {
+		cs_error(dev, RequestConfiguration, csval);
+		return;
+	}
+
+	ems_pcmcia_add_card(dev, req.Base);
+}
+
+/*
+ * Release claimed resources
+ */
+static void ems_pcmcia_remove(struct pcmcia_device *dev)
+{
+	ems_pcmcia_del_card(dev);
+
+	pcmcia_disable_device(dev);
+}
+
+/*
+ * The dev_info variable is the "key" that is used to match up this
+ * device driver with appropriate cards, through the card configuration
+ * database.
+ */
+static dev_info_t dev_info = "can-ems-pcmcia";
+
+static struct pcmcia_driver ems_pcmcia_driver = {
+	.drv = {
+		.name = dev_info,
+		},
+
+	.probe = ems_pcmcia_probe,
+	.remove = ems_pcmcia_remove,
+
+	.id_table = ems_pcmcia_tbl,
+};
+
+static int __init ems_pcmcia_init(void)
+{
+	return pcmcia_register_driver(&ems_pcmcia_driver);
+}
+
+static void __exit ems_pcmcia_exit(void)
+{
+	pcmcia_unregister_driver(&ems_pcmcia_driver);
+}
+
+module_init(ems_pcmcia_init);
+module_exit(ems_pcmcia_exit);
+
diff --git a/drivers/net/can/sja1000/ixxat_pci.c b/drivers/net/can/sja1000/ixxat_pci.c
new file mode 100644
index 000000000000..ab98a9d160ec
--- /dev/null
+++ b/drivers/net/can/sja1000/ixxat_pci.c
@@ -0,0 +1,272 @@
+/*
+ * Copyright (C) 2007 Wolfgang Grandegger <wg@grandegger.com>
+ * Copyright (C) 2008 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/delay.h>
+#include <linux/pci.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/io.h>
+
+#include "sja1000.h"
+
+#define DRV_NAME  "ixxat_pci"
+
+MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de");
+MODULE_DESCRIPTION("Socket-CAN driver for IXXAT PC-I 04/PCI PCI cards");
+MODULE_SUPPORTED_DEVICE("IXXAT PC-I 04/PCI card");
+MODULE_LICENSE("GPL v2");
+
+/* Maximum number of interfaces supported on one card. Currently
+ * we only support a maximum of two interfaces, which is the maximum
+ * of what Ixxat sells anyway.
+ */
+#define IXXAT_PCI_MAX_CAN 2
+
+struct ixxat_pci {
+	struct pci_dev *pci_dev;
+	struct net_device *dev[IXXAT_PCI_MAX_CAN];
+	int conf_addr;
+	void __iomem *base_addr;
+};
+
+#define IXXAT_PCI_CAN_CLOCK  (16000000 / 2)
+
+#define IXXAT_PCI_OCR	     (OCR_TX0_PUSHPULL | OCR_TX0_INVERT | \
+			      OCR_TX1_PUSHPULL)
+#define IXXAT_PCI_CDR	     0
+
+#define CHANNEL_RESET_OFFSET 0x110
+#define CHANNEL_OFFSET      0x200
+
+#define INTCSR_OFFSET        0x4c /* Offset in PLX9050 conf registers */
+#define INTCSR_LINTI1        (1 << 0)
+#define INTCSR_LINTI2        (1 << 3)
+#define INTCSR_PCI           (1 << 6)
+
+/* PCI vender, device and sub-device ID */
+#define IXXAT_PCI_VENDOR_ID  0x10b5
+#define IXXAT_PCI_DEVICE_ID  0x9050
+#define IXXAT_PCI_SUB_SYS_ID 0x2540
+
+#define IXXAT_PCI_BASE_SIZE  0x400
+
+static struct pci_device_id ixxat_pci_tbl[] = {
+	{IXXAT_PCI_VENDOR_ID, IXXAT_PCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, ixxat_pci_tbl);
+
+static u8 ixxat_pci_read_reg(struct net_device *ndev, int port)
+{
+	u8 val;
+	val = readb((void __iomem *)(ndev->base_addr + port));
+	return val;
+}
+
+static void ixxat_pci_write_reg(struct net_device *ndev, int port, u8 val)
+{
+	writeb(val, (void __iomem *)(ndev->base_addr + port));
+}
+
+static void ixxat_pci_del_chan(struct pci_dev *pdev, struct net_device *ndev)
+{
+	dev_info(&pdev->dev, "Removing device %s\n", ndev->name);
+
+	unregister_sja1000dev(ndev);
+
+	free_sja1000dev(ndev);
+}
+
+static struct net_device *ixxat_pci_add_chan(struct pci_dev *pdev,
+		void __iomem *base_addr)
+{
+	struct net_device *ndev;
+	struct sja1000_priv *priv;
+	int err;
+
+	ndev = alloc_sja1000dev(0);
+	if (ndev == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	priv = netdev_priv(ndev);
+
+	ndev->base_addr = (unsigned long)base_addr;
+
+	priv->read_reg = ixxat_pci_read_reg;
+	priv->write_reg = ixxat_pci_write_reg;
+
+	priv->can.bittiming.clock = IXXAT_PCI_CAN_CLOCK;
+
+	priv->ocr = IXXAT_PCI_OCR;
+	priv->cdr = IXXAT_PCI_CDR;
+
+	/* Set and enable PCI interrupts */
+	ndev->irq = pdev->irq;
+
+	dev_dbg(&pdev->dev, "base_addr=%#lx irq=%d\n",
+			ndev->base_addr, ndev->irq);
+
+	SET_NETDEV_DEV(ndev, &pdev->dev);
+
+	err = register_sja1000dev(ndev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to register (err=%d)\n", err);
+		goto failure;
+	}
+
+	return ndev;
+
+failure:
+	free_sja1000dev(ndev);
+	return ERR_PTR(err);
+}
+
+static int __devinit ixxat_pci_init_one(struct pci_dev *pdev,
+				       const struct pci_device_id *ent)
+{
+	struct ixxat_pci *board;
+	int err, intcsr = INTCSR_LINTI1 | INTCSR_PCI;
+	u16 sub_sys_id;
+	void __iomem *base_addr;
+
+	dev_info(&pdev->dev, "Initializing device %04x:%04x\n",
+	       pdev->vendor, pdev->device);
+
+	board = kzalloc(sizeof(*board), GFP_KERNEL);
+	if (!board)
+		return -ENOMEM;
+
+	err = pci_enable_device(pdev);
+	if (err)
+		goto failure;
+
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err)
+		goto failure;
+
+	err = pci_read_config_word(pdev, 0x2e, &sub_sys_id);
+	if (err)
+		goto failure_release_pci;
+
+	if (sub_sys_id != IXXAT_PCI_SUB_SYS_ID)
+		return -ENODEV;
+
+	/* Enable memory and I/O space */
+	err = pci_write_config_word(pdev, 0x04, 0x3);
+	if (err)
+		goto failure_release_pci;
+
+	board->conf_addr = pci_resource_start(pdev, 1);
+
+	base_addr = pci_iomap(pdev, 2, IXXAT_PCI_BASE_SIZE);
+	if (base_addr == NULL) {
+		err = -ENODEV;
+		goto failure_release_pci;
+	}
+
+	board->base_addr = base_addr;
+
+	writeb(0x1, base_addr + CHANNEL_RESET_OFFSET);
+	writeb(0x1, base_addr + CHANNEL_OFFSET + CHANNEL_RESET_OFFSET);
+	udelay(100);
+
+	board->dev[0] = ixxat_pci_add_chan(pdev, base_addr);
+	if (IS_ERR(board->dev[0]))
+		goto failure_iounmap;
+
+	/* Check if second channel is available */
+	if ((readb(base_addr + CHANNEL_OFFSET + REG_MOD) & 0xa1) == 0x21 &&
+	    readb(base_addr + CHANNEL_OFFSET + REG_SR) == 0x0c &&
+	    readb(base_addr + CHANNEL_OFFSET + REG_IR) == 0xe0) {
+		board->dev[1] = ixxat_pci_add_chan(pdev,
+				base_addr + CHANNEL_OFFSET);
+		if (IS_ERR(board->dev[1]))
+			goto failure_unreg_dev0;
+
+		intcsr |= INTCSR_LINTI2;
+	}
+
+	/* enable interrupt(s) in PLX9050 */
+	outb(intcsr, board->conf_addr + INTCSR_OFFSET);
+
+	pci_set_drvdata(pdev, board);
+
+	return 0;
+
+failure_unreg_dev0:
+	ixxat_pci_del_chan(pdev, board->dev[0]);
+
+failure_iounmap:
+	pci_iounmap(pdev, board->base_addr);
+
+failure_release_pci:
+	pci_release_regions(pdev);
+
+failure:
+	kfree(board);
+
+	return err;
+}
+
+static void __devexit ixxat_pci_remove_one(struct pci_dev *pdev)
+{
+	struct ixxat_pci *board = pci_get_drvdata(pdev);
+	int i;
+
+	/* Disable interrupts in PLX9050*/
+	outb(0, board->conf_addr + INTCSR_OFFSET);
+
+	for (i = 0; i < IXXAT_PCI_MAX_CAN; i++) {
+		if (!board->dev[i])
+			break;
+		ixxat_pci_del_chan(pdev, board->dev[i]);
+	}
+
+	pci_iounmap(pdev, board->base_addr);
+
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+
+	kfree(board);
+}
+
+static struct pci_driver ixxat_pci_driver = {
+	.name = DRV_NAME,
+	.id_table = ixxat_pci_tbl,
+	.probe = ixxat_pci_init_one,
+	.remove = __devexit_p(ixxat_pci_remove_one),
+};
+
+static int __init ixxat_pci_init(void)
+{
+	return pci_register_driver(&ixxat_pci_driver);
+}
+
+static void __exit ixxat_pci_exit(void)
+{
+	pci_unregister_driver(&ixxat_pci_driver);
+}
+
+module_init(ixxat_pci_init);
+module_exit(ixxat_pci_exit);
diff --git a/drivers/net/can/sja1000/kvaser_pci.c b/drivers/net/can/sja1000/kvaser_pci.c
new file mode 100644
index 000000000000..0b01ea3d5560
--- /dev/null
+++ b/drivers/net/can/sja1000/kvaser_pci.c
@@ -0,0 +1,415 @@
+/*
+ * Copyright (C) 2008 Per Dalen <per.dalen@cnw.se>
+ *
+ * Parts of this software are based on (derived) the following:
+ *
+ * - Kvaser linux driver, version 4.72 BETA
+ *   Copyright (C) 2002-2007 KVASER AB
+ *
+ * - Lincan driver, version 0.3.3, OCERA project
+ *   Copyright (C) 2004 Pavel Pisa
+ *   Copyright (C) 2001 Arnaud Westenberg
+ *
+ * - Socketcan SJA1000 drivers
+ *   Copyright (C) 2007 Wolfgang Grandegger <wg@grandegger.com>
+ *   Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ *   Copyright (c) 2003 Matthias Brukner, Trajet Gmbh, Rebenring 33,
+ *   38106 Braunschweig, GERMANY
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/delay.h>
+#include <linux/pci.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/io.h>
+
+#include "sja1000.h"
+
+#define DRV_NAME  "kvaser_pci"
+
+MODULE_AUTHOR("Per Dalen <per.dalen@cnw.se>");
+MODULE_DESCRIPTION("Socket-CAN driver for KVASER PCAN PCI cards");
+MODULE_SUPPORTED_DEVICE("KVASER PCAN PCI CAN card");
+MODULE_LICENSE("GPL v2");
+
+#define MAX_NO_OF_CHANNELS        4 /* max no of channels on
+				       a single card */
+
+struct kvaser_pci {
+	int channel;
+	struct pci_dev *pci_dev;
+	struct net_device *slave_dev[MAX_NO_OF_CHANNELS-1];
+	void __iomem *conf_addr;
+	void __iomem *res_addr;
+	int no_channels;
+	u8 xilinx_ver;
+};
+
+#define KVASER_PCI_CAN_CLOCK      (16000000 / 2)
+
+/*
+ * The board configuration is probably following:
+ * RX1 is connected to ground.
+ * TX1 is not connected.
+ * CLKO is not connected.
+ * Setting the OCR register to 0xDA is a good idea.
+ * This means  normal output mode , push-pull and the correct polarity.
+ */
+#define KVASER_PCI_OCR            (OCR_TX0_PUSHPULL | OCR_TX1_PUSHPULL)
+
+/*
+ * In the CDR register, you should set CBP to 1.
+ * You will probably also want to set the clock divider value to 0
+ * (meaning divide-by-2), the Pelican bit, and the clock-off bit
+ * (you will have no need for CLKOUT anyway).
+ */
+#define KVASER_PCI_CDR            (CDR_CBP | CDR_CLKOUT_MASK)
+
+/*
+ * These register values are valid for revision 14 of the Xilinx logic.
+ */
+#define XILINX_VERINT             7   /* Lower nibble simulate interrupts,
+					 high nibble version number. */
+
+#define XILINX_PRESUMED_VERSION   14
+
+/*
+ * Important S5920 registers
+ */
+#define S5920_INTCSR              0x38
+#define S5920_PTCR                0x60
+#define INTCSR_ADDON_INTENABLE_M  0x2000
+
+
+#define KVASER_PCI_PORT_BYTES     0x20
+
+#define PCI_CONFIG_PORT_SIZE      0x80      /* size of the config io-memory */
+#define PCI_PORT_SIZE             0x80      /* size of a channel io-memory */
+#define PCI_PORT_XILINX_SIZE      0x08      /* size of a xilinx io-memory */
+
+#define KVASER_PCI_VENDOR_ID1     0x10e8    /* the PCI device and vendor IDs */
+#define KVASER_PCI_DEVICE_ID1     0x8406
+
+#define KVASER_PCI_VENDOR_ID2     0x1a07    /* the PCI device and vendor IDs */
+#define KVASER_PCI_DEVICE_ID2     0x0008
+
+static struct pci_device_id kvaser_pci_tbl[] = {
+	{KVASER_PCI_VENDOR_ID1, KVASER_PCI_DEVICE_ID1, PCI_ANY_ID, PCI_ANY_ID,},
+	{KVASER_PCI_VENDOR_ID2, KVASER_PCI_DEVICE_ID2, PCI_ANY_ID, PCI_ANY_ID,},
+	{ 0,}
+};
+
+MODULE_DEVICE_TABLE(pci, kvaser_pci_tbl);
+
+static u8 kvaser_pci_read_reg(struct net_device *dev, int port)
+{
+	return ioread8((void __iomem *)(dev->base_addr + port));
+}
+
+static void kvaser_pci_write_reg(struct net_device *dev, int port, u8 val)
+{
+	iowrite8(val, (void __iomem *)(dev->base_addr + port));
+}
+
+static void kvaser_pci_disable_irq(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct kvaser_pci *board = priv->priv;
+	u32 tmp;
+
+	/* Disable interrupts from card */
+	tmp = ioread32(board->conf_addr + S5920_INTCSR);
+	tmp &= ~INTCSR_ADDON_INTENABLE_M;
+	iowrite32(tmp, board->conf_addr + S5920_INTCSR);
+}
+
+static void kvaser_pci_enable_irq(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct kvaser_pci *board = priv->priv;
+	u32 tmp;
+
+	/* Enable interrupts from card */
+	tmp = ioread32(board->conf_addr + S5920_INTCSR);
+	tmp |= INTCSR_ADDON_INTENABLE_M;
+	iowrite32(tmp, board->conf_addr + S5920_INTCSR);
+}
+
+static int number_of_sja1000_chip(void __iomem *base_addr)
+{
+	u8 status;
+	int i;
+
+	for (i = 0; i < MAX_NO_OF_CHANNELS; i++) {
+		/* reset chip */
+		iowrite8(MOD_RM, base_addr +
+			 (i * KVASER_PCI_PORT_BYTES) + REG_MOD);
+		status = ioread8(base_addr +
+				 (i * KVASER_PCI_PORT_BYTES) + REG_MOD);
+		udelay(10);
+		/* check reset bit */
+		if (!(status & MOD_RM))
+			break;
+	}
+
+	return i;
+}
+
+static void kvaser_pci_del_chan(struct net_device *dev)
+{
+	struct sja1000_priv *priv;
+	struct kvaser_pci *board;
+	int i;
+
+	if (!dev)
+		return;
+	priv = netdev_priv(dev);
+	if (!priv)
+		return;
+	board = priv->priv;
+	if (!board)
+		return;
+
+	dev_info(&board->pci_dev->dev, "Removing device %s\n",
+		 dev->name);
+
+	for (i = 0; i < board->no_channels - 1; i++) {
+		if (board->slave_dev[i]) {
+			dev_info(&board->pci_dev->dev, "Removing device %s\n",
+				 board->slave_dev[i]->name);
+			unregister_sja1000dev(board->slave_dev[i]);
+			free_sja1000dev(board->slave_dev[i]);
+		}
+	}
+	unregister_sja1000dev(dev);
+
+	/* Disable PCI interrupts */
+	kvaser_pci_disable_irq(dev);
+
+	pci_iounmap(board->pci_dev, (void __iomem *)dev->base_addr);
+	pci_iounmap(board->pci_dev, board->conf_addr);
+	pci_iounmap(board->pci_dev, board->res_addr);
+
+	free_sja1000dev(dev);
+}
+
+static int kvaser_pci_add_chan(struct pci_dev *pdev, int channel,
+			       struct net_device **master_dev,
+			       void __iomem *conf_addr,
+			       void __iomem *res_addr,
+			       unsigned long base_addr)
+{
+	struct net_device *dev;
+	struct sja1000_priv *priv;
+	struct kvaser_pci *board;
+	int err, init_step;
+
+	dev = alloc_sja1000dev(sizeof(struct kvaser_pci));
+	if (dev == NULL)
+		return -ENOMEM;
+
+	priv = netdev_priv(dev);
+	board = priv->priv;
+
+	board->pci_dev = pdev;
+	board->channel = channel;
+
+	/*S5920*/
+	board->conf_addr = conf_addr;
+
+	/*XILINX board wide address*/
+	board->res_addr = res_addr;
+
+	if (channel == 0) {
+		board->xilinx_ver =
+			ioread8(board->res_addr + XILINX_VERINT) >> 4;
+		init_step = 2;
+
+		/* Assert PTADR# - we're in passive mode so the other bits are
+		   not important */
+		iowrite32(0x80808080UL, board->conf_addr + S5920_PTCR);
+
+		/* Disable interrupts from card */
+		kvaser_pci_disable_irq(dev);
+		/* Enable interrupts from card */
+		kvaser_pci_enable_irq(dev);
+	} else {
+		struct sja1000_priv *master_priv = netdev_priv(*master_dev);
+		struct kvaser_pci *master_board = master_priv->priv;
+		master_board->slave_dev[channel - 1] = dev;
+		master_board->no_channels = channel + 1;
+		board->xilinx_ver = master_board->xilinx_ver;
+	}
+
+	dev->base_addr = base_addr + channel * KVASER_PCI_PORT_BYTES;
+
+	priv->read_reg = kvaser_pci_read_reg;
+	priv->write_reg = kvaser_pci_write_reg;
+
+	priv->can.bittiming.clock = KVASER_PCI_CAN_CLOCK;
+
+	priv->ocr = KVASER_PCI_OCR;
+	priv->cdr = KVASER_PCI_CDR;
+
+	/* Register and setup interrupt handling */
+	dev->irq = pdev->irq;
+	init_step = 4;
+
+	dev_info(&pdev->dev, "base_addr=%#lx conf_addr=%p irq=%d\n",
+		 dev->base_addr, board->conf_addr, dev->irq);
+
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	/* Register SJA1000 device */
+	err = register_sja1000dev(dev);
+	if (err) {
+		dev_err(&pdev->dev, "Registering device failed (err=%d)\n",
+			err);
+		goto failure;
+	}
+
+	if (channel == 0)
+		*master_dev = dev;
+
+	return 0;
+
+failure:
+	kvaser_pci_del_chan(dev);
+	return err;
+}
+
+static int __devinit kvaser_pci_init_one(struct pci_dev *pdev,
+					 const struct pci_device_id *ent)
+{
+	int err;
+	struct net_device *master_dev = NULL;
+	struct sja1000_priv *priv;
+	struct kvaser_pci *board;
+	int no_channels;
+	void __iomem *base_addr = NULL;
+	void __iomem *conf_addr = NULL;
+	void __iomem *res_addr = NULL;
+	int i;
+
+	dev_info(&pdev->dev, "initializing device %04x:%04x\n",
+		 pdev->vendor, pdev->device);
+
+	err = pci_enable_device(pdev);
+	if (err)
+		goto failure;
+
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err)
+		goto failure_release_pci;
+
+	/*S5920*/
+	conf_addr = pci_iomap(pdev, 0, PCI_CONFIG_PORT_SIZE);
+	if (conf_addr == NULL) {
+		err = -ENODEV;
+		goto failure_iounmap;
+	}
+
+	/*XILINX board wide address*/
+	res_addr = pci_iomap(pdev, 2, PCI_PORT_XILINX_SIZE);
+	if (res_addr == NULL) {
+		err = -ENOMEM;
+		goto failure_iounmap;
+	}
+
+	base_addr = pci_iomap(pdev, 1, PCI_PORT_SIZE);
+	if (base_addr == NULL) {
+		err = -ENOMEM;
+		goto failure_iounmap;
+	}
+
+	no_channels = number_of_sja1000_chip(base_addr);
+	if (no_channels == 0) {
+		err = -ENOMEM;
+		goto failure_iounmap;
+	}
+
+	for (i = 0; i < no_channels; i++) {
+		err = kvaser_pci_add_chan(pdev, i, &master_dev,
+					  conf_addr, res_addr,
+					  (unsigned long)base_addr);
+		if (err)
+			goto failure_cleanup;
+	}
+
+	priv = netdev_priv(master_dev);
+	board = priv->priv;
+
+	dev_info(&pdev->dev, "xilinx version=%d number of channels=%d\n",
+		 board->xilinx_ver, board->no_channels);
+
+	pci_set_drvdata(pdev, master_dev);
+	return 0;
+
+failure_cleanup:
+	kvaser_pci_del_chan(master_dev);
+
+failure_iounmap:
+	if (conf_addr == NULL)
+		pci_iounmap(pdev, conf_addr);
+	if (res_addr == NULL)
+		pci_iounmap(pdev, res_addr);
+	if (base_addr == NULL)
+		pci_iounmap(pdev, base_addr);
+
+	pci_release_regions(pdev);
+
+failure_release_pci:
+	pci_disable_device(pdev);
+
+failure:
+	return err;
+
+}
+
+static void __devexit kvaser_pci_remove_one(struct pci_dev *pdev)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+
+	kvaser_pci_del_chan(dev);
+
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+}
+
+static struct pci_driver kvaser_pci_driver = {
+	.name = DRV_NAME,
+	.id_table = kvaser_pci_tbl,
+	.probe = kvaser_pci_init_one,
+	.remove = __devexit_p(kvaser_pci_remove_one),
+};
+
+static int __init kvaser_pci_init(void)
+{
+	return pci_register_driver(&kvaser_pci_driver);
+}
+
+static void __exit kvaser_pci_exit(void)
+{
+	pci_unregister_driver(&kvaser_pci_driver);
+}
+
+module_init(kvaser_pci_init);
+module_exit(kvaser_pci_exit);
diff --git a/drivers/net/can/sja1000/peak_pci.c b/drivers/net/can/sja1000/peak_pci.c
new file mode 100644
index 000000000000..d1089a3d62e0
--- /dev/null
+++ b/drivers/net/can/sja1000/peak_pci.c
@@ -0,0 +1,348 @@
+/*
+ * Copyright (C) 2007 Wolfgang Grandegger <wg@grandegger.com>
+ *
+ * Derived from the PCAN project file driver/src/pcan_pci.c:
+ *
+ * Copyright (C) 2001-2006  PEAK System-Technik GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/delay.h>
+#include <linux/pci.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/io.h>
+
+#include "sja1000.h"
+
+#define DRV_NAME  "peak_pci"
+
+MODULE_AUTHOR("Wolfgang Grandegger <wg@grandegger.com>");
+MODULE_DESCRIPTION("Socket-CAN driver for PEAK PCAN PCI cards");
+MODULE_SUPPORTED_DEVICE("PEAK PCAN PCI CAN card");
+MODULE_LICENSE("GPL v2");
+
+struct peak_pci {
+	int channel;
+	struct pci_dev *pci_dev;
+	struct net_device *slave_dev;
+	volatile void __iomem *conf_addr;
+};
+
+#define PEAK_PCI_SINGLE	     0	/* single channel device */
+#define PEAK_PCI_MASTER	     1	/* multi channel master device */
+#define PEAK_PCI_SLAVE	     2	/* multi channel slave device */
+
+#define PEAK_PCI_CAN_CLOCK   (16000000 / 2)
+
+#define PEAK_PCI_CDR_SINGLE  (CDR_CBP | CDR_CLKOUT_MASK | CDR_CLK_OFF)
+#define PEAK_PCI_CDR_MASTER  (CDR_CBP | CDR_CLKOUT_MASK)
+
+#define PEAK_PCI_OCR 	     OCR_TX0_PUSHPULL
+
+/*
+ * Important PITA registers
+ */
+#define PITA_ICR	     0x00	/* interrupt control register */
+#define PITA_GPIOICR	     0x18	/* general purpose I/O interface
+					   control register */
+#define PITA_MISC	     0x1C	/* miscellanoes register */
+
+#define PCI_CONFIG_PORT_SIZE 0x1000	/* size of the config io-memory */
+#define PCI_PORT_SIZE        0x0400	/* size of a channel io-memory */
+
+#define PEAK_PCI_VENDOR_ID   0x001C	/* the PCI device and vendor IDs */
+#define PEAK_PCI_DEVICE_ID   0x0001
+
+static struct pci_device_id peak_pci_tbl[] = {
+	{PEAK_PCI_VENDOR_ID, PEAK_PCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, peak_pci_tbl);
+
+static u8 peak_pci_read_reg(struct net_device *dev, int port)
+{
+	u8 val;
+	val = readb((const volatile void __iomem *)
+		    (dev->base_addr + (port << 2)));
+	return val;
+}
+
+static void peak_pci_write_reg(struct net_device *dev, int port, u8 val)
+{
+	writeb(val, (volatile void __iomem *)
+	       (dev->base_addr + (port << 2)));
+}
+
+static void peak_pci_post_irq(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct peak_pci *board = priv->priv;
+	u16 icr_low;
+
+	/* Select and clear in Pita stored interrupt */
+	icr_low = readw(board->conf_addr + PITA_ICR);
+	if (board->channel == PEAK_PCI_SLAVE) {
+		if (icr_low & 0x0001)
+			writew(0x0001, board->conf_addr + PITA_ICR);
+	} else {
+		if (icr_low & 0x0002)
+			writew(0x0002, board->conf_addr + PITA_ICR);
+	}
+}
+
+static void peak_pci_del_chan(struct net_device *dev, int init_step)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct peak_pci *board;
+	u16 icr_high;
+
+	if (!dev)
+		return;
+	priv = netdev_priv(dev);
+	if (!priv)
+		return;
+	board = priv->priv;
+	if (!board)
+		return;
+
+	switch (init_step) {
+	case 0:		/* Full cleanup */
+		printk(KERN_INFO "Removing %s device %s\n",
+		       DRV_NAME, dev->name);
+		unregister_sja1000dev(dev);
+	case 4:
+		icr_high = readw(board->conf_addr + PITA_ICR + 2);
+		if (board->channel == PEAK_PCI_SLAVE)
+			icr_high &= ~0x0001;
+		else
+			icr_high &= ~0x0002;
+		writew(icr_high, board->conf_addr + PITA_ICR + 2);
+	case 3:
+		iounmap((void *)dev->base_addr);
+	case 2:
+		if (board->channel != PEAK_PCI_SLAVE)
+			iounmap((void *)board->conf_addr);
+	case 1:
+		free_sja1000dev(dev);
+		break;
+	}
+
+}
+
+static int peak_pci_add_chan(struct pci_dev *pdev, int channel,
+			     struct net_device **master_dev)
+{
+	struct net_device *dev;
+	struct sja1000_priv *priv;
+	struct peak_pci *board;
+	u16 icr_high;
+	unsigned long addr;
+	int err, init_step;
+
+	dev = alloc_sja1000dev(sizeof(struct peak_pci));
+	if (dev == NULL)
+		return -ENOMEM;
+	init_step = 1;
+
+	priv = netdev_priv(dev);
+	board = priv->priv;
+
+	board->pci_dev = pdev;
+	board->channel = channel;
+
+	if (channel != PEAK_PCI_SLAVE) {
+
+		addr = pci_resource_start(pdev, 0);
+		board->conf_addr = ioremap(addr, PCI_CONFIG_PORT_SIZE);
+		if (board->conf_addr == 0) {
+			err = -ENODEV;
+			goto failure;
+		}
+		init_step = 2;
+
+		/* Set GPIO control register */
+		writew(0x0005, board->conf_addr + PITA_GPIOICR + 2);
+
+		/* Enable single or dual channel */
+		if (channel == PEAK_PCI_MASTER)
+			writeb(0x00, board->conf_addr + PITA_GPIOICR);
+		else
+			writeb(0x04, board->conf_addr + PITA_GPIOICR);
+		/* Toggle reset */
+		writeb(0x05, board->conf_addr + PITA_MISC + 3);
+		mdelay(5);
+		/* Leave parport mux mode */
+		writeb(0x04, board->conf_addr + PITA_MISC + 3);
+	} else {
+		struct sja1000_priv *master_priv = netdev_priv(*master_dev);
+		struct peak_pci *master_board = master_priv->priv;
+		master_board->slave_dev = dev;
+		board->conf_addr = master_board->conf_addr;
+	}
+
+	addr = pci_resource_start(pdev, 1);
+	if (channel == PEAK_PCI_SLAVE)
+		addr += PCI_PORT_SIZE;
+
+	dev->base_addr = (unsigned long)ioremap(addr, PCI_PORT_SIZE);
+	if (dev->base_addr == 0) {
+		err = -ENOMEM;
+		goto failure;
+	}
+	init_step = 3;
+
+	priv->read_reg = peak_pci_read_reg;
+	priv->write_reg = peak_pci_write_reg;
+	priv->post_irq = peak_pci_post_irq;
+
+	priv->can.bittiming.clock = PEAK_PCI_CAN_CLOCK;
+
+	priv->ocr = PEAK_PCI_OCR;
+
+	if (channel == PEAK_PCI_MASTER)
+		priv->cdr = PEAK_PCI_CDR_MASTER;
+	else
+		priv->cdr = PEAK_PCI_CDR_SINGLE;
+
+	/* Register and setup interrupt handling */
+	dev->irq = pdev->irq;
+	icr_high = readw(board->conf_addr + PITA_ICR + 2);
+	if (channel == PEAK_PCI_SLAVE)
+		icr_high |= 0x0001;
+	else
+		icr_high |= 0x0002;
+	writew(icr_high, board->conf_addr + PITA_ICR + 2);
+	init_step = 4;
+
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	/* Register SJA1000 device */
+	err = register_sja1000dev(dev);
+	if (err) {
+		printk(KERN_ERR "Registering %s device failed (err=%d)\n",
+		       DRV_NAME, err);
+		goto failure;
+	}
+
+	if (channel != PEAK_PCI_SLAVE)
+		*master_dev = dev;
+
+	printk(KERN_INFO "%s: %s at base_addr=%#lx conf_addr=%p irq=%d\n",
+	       DRV_NAME, dev->name, dev->base_addr, board->conf_addr, dev->irq);
+
+	return 0;
+
+failure:
+	peak_pci_del_chan(dev, init_step);
+	return err;
+}
+
+static int __devinit peak_pci_init_one(struct pci_dev *pdev,
+				       const struct pci_device_id *ent)
+{
+	int err;
+	u16 sub_sys_id;
+	struct net_device *master_dev = NULL;
+
+	printk(KERN_INFO "%s: initializing device %04x:%04x\n",
+	       DRV_NAME, pdev->vendor, pdev->device);
+
+	err = pci_enable_device(pdev);
+	if (err)
+		goto failure;
+
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err)
+		goto failure;
+
+	err = pci_read_config_word(pdev, 0x2e, &sub_sys_id);
+	if (err)
+		goto failure_cleanup;
+
+	err = pci_write_config_word(pdev, 0x44, 0);
+	if (err)
+		goto failure_cleanup;
+
+	if (sub_sys_id > 3) {
+		err = peak_pci_add_chan(pdev,
+					PEAK_PCI_MASTER, &master_dev);
+		if (err)
+			goto failure_cleanup;
+
+		err = peak_pci_add_chan(pdev,
+					PEAK_PCI_SLAVE, &master_dev);
+		if (err)
+			goto failure_cleanup;
+	} else {
+		err = peak_pci_add_chan(pdev, PEAK_PCI_SINGLE,
+					     &master_dev);
+		if (err)
+			goto failure_cleanup;
+	}
+
+	pci_set_drvdata(pdev, master_dev);
+	return 0;
+
+failure_cleanup:
+	if (master_dev)
+		peak_pci_del_chan(master_dev, 0);
+
+	pci_release_regions(pdev);
+
+failure:
+	return err;
+
+}
+
+static void __devexit peak_pci_remove_one(struct pci_dev *pdev)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct peak_pci *board = priv->priv;
+
+	if (board->slave_dev)
+		peak_pci_del_chan(board->slave_dev, 0);
+	peak_pci_del_chan(dev, 0);
+
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+}
+
+static struct pci_driver peak_pci_driver = {
+	.name = DRV_NAME,
+	.id_table = peak_pci_tbl,
+	.probe = peak_pci_init_one,
+	.remove = __devexit_p(peak_pci_remove_one),
+};
+
+static int __init peak_pci_init(void)
+{
+	return pci_register_driver(&peak_pci_driver);
+}
+
+static void __exit peak_pci_exit(void)
+{
+	pci_unregister_driver(&peak_pci_driver);
+}
+
+module_init(peak_pci_init);
+module_exit(peak_pci_exit);
diff --git a/drivers/net/can/sja1000/pipcan.c b/drivers/net/can/sja1000/pipcan.c
new file mode 100644
index 000000000000..b7414b218697
--- /dev/null
+++ b/drivers/net/can/sja1000/pipcan.c
@@ -0,0 +1,201 @@
+/*
+ * Copyright (C) 2008 David Müller, <d.mueller@elsoft.ch>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/io.h>
+
+#include "sja1000.h"
+
+#define DRV_NAME "pipcan"
+
+MODULE_AUTHOR("David Müller <d.mueller@elsoft.ch>");
+MODULE_DESCRIPTION("Socket-CAN driver for MPL PIPCAN module");
+MODULE_SUPPORTED_DEVICE("MPL PIPCAN module");
+MODULE_LICENSE("GPL v2");
+
+#define PIPCAN_CAN_CLOCK  (16000000 / 2)
+
+#define PIPCAN_OCR        (OCR_TX1_PUSHPULL)
+#define PIPCAN_CDR        (CDR_CBP | CDR_CLK_OFF)
+
+#define PIPCAN_IOSIZE     (0x100)
+
+#define PIPCAN_RES        (0x804)
+#define PIPCAN_RST        (0x805)
+
+static u8 pc_read_reg(struct net_device *dev, int reg)
+{
+	return inb(dev->base_addr + reg);
+}
+
+static void pc_write_reg(struct net_device *dev, int reg, u8 val)
+{
+	outb(val, dev->base_addr + reg);
+}
+
+static int __init pc_probe(struct platform_device *pdev)
+{
+	struct net_device *dev;
+	struct sja1000_priv *priv;
+	struct resource *res;
+	int rc, irq;
+
+	rc = -ENODEV;
+	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (!res || !irq)
+		goto exit;
+
+	rc = -EBUSY;
+	if (!request_region(res->start, res->end - res->start + 1, DRV_NAME))
+		goto exit;
+
+	rc = -ENOMEM;
+	dev = alloc_sja1000dev(0);
+	if (!dev)
+		goto exit_release;
+
+	priv = netdev_priv(dev);
+
+	priv->read_reg = pc_read_reg;
+	priv->write_reg = pc_write_reg;
+	priv->can.bittiming.clock = PIPCAN_CAN_CLOCK;
+	priv->ocr = PIPCAN_OCR;
+	priv->cdr = PIPCAN_CDR;
+
+	dev->irq = irq;
+	dev->base_addr = res->start;
+
+	dev_set_drvdata(&pdev->dev, dev);
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	/* deactivate RST */
+	outb(inb(PIPCAN_RST) & ~0x01, PIPCAN_RST);
+
+	rc = register_sja1000dev(dev);
+	if (rc) {
+		dev_err(&pdev->dev, "registering %s failed (err=%d)\n",
+			DRV_NAME, rc);
+		goto exit_free;
+	}
+
+	dev_info(&pdev->dev, "device registered (base_addr=%#lx, irq=%d)\n",
+		 dev->base_addr, dev->irq);
+	return 0;
+
+exit_free:
+	free_sja1000dev(dev);
+
+exit_release:
+	release_region(res->start, res->end - res->start + 1);
+
+exit:
+	return rc;
+}
+
+static int __exit pc_remove(struct platform_device *pdev)
+{
+	struct net_device *dev = dev_get_drvdata(&pdev->dev);
+	struct resource *res;
+
+	dev_set_drvdata(&pdev->dev, NULL);
+	unregister_sja1000dev(dev);
+	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
+
+	free_sja1000dev(dev);
+
+	release_region(res->start, res->end - res->start + 1);
+
+	/* activate RST */
+	outb(inb(PIPCAN_RST) | 0x01, PIPCAN_RST);
+
+	return 0;
+}
+
+static struct platform_driver pc_driver = {
+	.remove = __exit_p(pc_remove),
+	.driver = {
+		   .name = DRV_NAME,
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static struct platform_device *pc_pdev;
+static const u16 pipcan_ioport[] = {0x1000, 0x8000, 0xE000};
+
+static int __init pc_init(void)
+{
+	struct resource r[2];
+	int rc, addr, irq, idx;
+	u8 pc_res;
+
+	/* get PIPCAN resources from EPLD */
+	pc_res = inb(PIPCAN_RES);
+
+	idx = (pc_res & 0x0F);
+	if ((idx <= 0) || (idx > ARRAY_SIZE(pipcan_ioport))) {
+		printk(KERN_ERR DRV_NAME " invalid base address\n");
+		return -EINVAL;
+	}
+	addr = pipcan_ioport[idx-1];
+
+	irq = (pc_res >> 4) & 0x0F;
+	if ((irq < 3) || (irq == 8) || (irq == 13)) {
+		printk(KERN_ERR DRV_NAME " invalid IRQ\n");
+		return -EINVAL;
+	}
+
+	/* fill in resources */
+	memset(&r, 0, sizeof(r));
+	r[0].start = addr;
+	r[0].end = addr + PIPCAN_IOSIZE - 1;
+	r[0].name = DRV_NAME;
+	r[0].flags = IORESOURCE_IO;
+	r[1].start = r[1].end = irq;
+	r[1].name = DRV_NAME;
+	r[1].flags = IORESOURCE_IRQ;
+
+	pc_pdev = platform_device_register_simple(DRV_NAME, 0, r,
+						  ARRAY_SIZE(r));
+	if (IS_ERR(pc_pdev))
+		return PTR_ERR(pc_pdev);
+
+	rc = platform_driver_probe(&pc_driver, pc_probe);
+	if (rc) {
+		platform_device_unregister(pc_pdev);
+		printk(KERN_ERR DRV_NAME
+		       " platform_driver_probe() failed (%d)\n", rc);
+	}
+
+	return rc;
+}
+
+static void __exit pc_exit(void)
+{
+	platform_driver_unregister(&pc_driver);
+	platform_device_unregister(pc_pdev);
+}
+
+module_init(pc_init);
+module_exit(pc_exit);
diff --git a/drivers/net/can/sja1000/sja1000.c b/drivers/net/can/sja1000/sja1000.c
new file mode 100644
index 000000000000..02f49fa703ce
--- /dev/null
+++ b/drivers/net/can/sja1000/sja1000.c
@@ -0,0 +1,677 @@
+/*
+ * sja1000.c -  Philips SJA1000 network device driver
+ *
+ * Copyright (c) 2003 Matthias Brukner, Trajet Gmbh, Rebenring 33,
+ * 38106 Braunschweig, GERMANY
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/fcntl.h>
+#include <linux/interrupt.h>
+#include <linux/ptrace.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/skbuff.h>
+#include <linux/delay.h>
+
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/can/error.h>
+#include <linux/can/dev.h>
+
+#include "sja1000.h"
+
+#define DRV_NAME "sja1000"
+
+MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION(DRV_NAME " CAN netdevice driver");
+
+static struct can_bittiming_const sja1000_bittiming_const = {
+	.tseg1_min = 1,
+	.tseg1_max = 16,
+	.tseg2_min = 1,
+	.tseg2_max = 8,
+	.sjw_max = 4,
+	.brp_min = 1,
+	.brp_max = 64,
+	.brp_inc = 1,
+};
+
+static int sja1000_probe_chip(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+
+	if (dev->base_addr && (priv->read_reg(dev, 0) == 0xFF)) {
+		printk(KERN_INFO "%s: probing @0x%lX failed\n",
+		       DRV_NAME, dev->base_addr);
+		return 0;
+	}
+	return 1;
+}
+
+static int set_reset_mode(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	unsigned char status = priv->read_reg(dev, REG_MOD);
+	int i;
+
+	/* disable interrupts */
+	priv->write_reg(dev, REG_IER, IRQ_OFF);
+
+	for (i = 0; i < 100; i++) {
+		/* check reset bit */
+		if (status & MOD_RM) {
+			priv->can.state = CAN_STATE_STOPPED;
+			return 0;
+		}
+
+		priv->write_reg(dev, REG_MOD, MOD_RM);	/* reset chip */
+		status = priv->read_reg(dev, REG_MOD);
+		udelay(10);
+	}
+
+	dev_err(ND2D(dev), "setting SJA1000 into reset mode failed!\n");
+	return 1;
+
+}
+
+static int set_normal_mode(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	unsigned char status = priv->read_reg(dev, REG_MOD);
+	int i;
+
+	for (i = 0; i < 100; i++) {
+		/* check reset bit */
+		if ((status & MOD_RM) == 0) {
+			priv->can.state = CAN_STATE_ACTIVE;
+			/* enable all interrupts */
+			priv->write_reg(dev, REG_IER, IRQ_ALL);
+
+			return 0;
+		}
+
+		/* set chip to normal mode */
+		priv->write_reg(dev, REG_MOD, 0x00);
+		status = priv->read_reg(dev, REG_MOD);
+		udelay(10);
+	}
+
+	dev_err(ND2D(dev), "setting SJA1000 into normal mode failed!\n");
+	return 1;
+
+}
+
+static void sja1000_start(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+
+	/* leave reset mode */
+	if (priv->can.state != CAN_STATE_STOPPED)
+		set_reset_mode(dev);
+
+	/* Clear error counters and error code capture */
+	priv->write_reg(dev, REG_TXERR, 0x0);
+	priv->write_reg(dev, REG_RXERR, 0x0);
+	priv->read_reg(dev, REG_ECC);
+
+	/* leave reset mode */
+	set_normal_mode(dev);
+}
+
+static int sja1000_set_mode(struct net_device *dev, enum can_mode mode)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+
+	switch (mode) {
+	case CAN_MODE_START:
+		if (!priv->open_time)
+			return -EINVAL;
+
+		sja1000_start(dev);
+		if (netif_queue_stopped(dev))
+			netif_wake_queue(dev);
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+static int sja1000_get_state(struct net_device *dev, enum can_state *state)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	u8 status;
+
+	/* FIXME: inspecting the status register to get the current state
+	 * is not really necessary, because state changes are handled by
+	 * in the ISR and the variable priv->can.state gets updated. The
+	 * CAN devicde interface needs fixing!
+	 */
+
+	spin_lock_irq(&priv->can.irq_lock);
+
+	if (priv->can.state == CAN_STATE_STOPPED) {
+		*state =  CAN_STATE_STOPPED;
+	} else {
+		status = priv->read_reg(dev, REG_SR);
+		if (status & SR_BS)
+			*state = CAN_STATE_BUS_OFF;
+		else if (status & SR_ES) {
+			if (priv->read_reg(dev, REG_TXERR) > 127 ||
+			    priv->read_reg(dev, REG_RXERR) > 127)
+				*state = CAN_STATE_BUS_PASSIVE;
+			else
+				*state = CAN_STATE_BUS_WARNING;
+		} else
+			*state = CAN_STATE_ACTIVE;
+	}
+	/* Check state */
+	if (*state != priv->can.state)
+		dev_err(ND2D(dev),
+			"Oops, state mismatch: hard %d != soft %d\n",
+			*state, priv->can.state);
+	spin_unlock_irq(&priv->can.irq_lock);
+	return 0;
+}
+
+static int sja1000_set_bittiming(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct can_bittiming *bt = &priv->can.bittiming;
+	u8 btr0, btr1;
+
+	btr0 = ((bt->brp - 1) & 0x3f) | (((bt->sjw - 1) & 0x3) << 6);
+	btr1 = ((bt->prop_seg + bt->phase_seg1 - 1) & 0xf) |
+		(((bt->phase_seg2 - 1) & 0x7) << 4) |
+	  ((priv->can.ctrlmode & CAN_CTRLMODE_3_SAMPLES) << 7);
+
+	dev_info(ND2D(dev), "setting BTR0=0x%02x BTR1=0x%02x\n", btr0, btr1);
+
+	priv->write_reg(dev, REG_BTR0, btr0);
+	priv->write_reg(dev, REG_BTR1, btr1);
+
+	return 0;
+}
+
+/*
+ * initialize SJA1000 chip:
+ *   - reset chip
+ *   - set output mode
+ *   - set baudrate
+ *   - enable interrupts
+ *   - start operating mode
+ */
+static void chipset_init(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+
+	/* set clock divider and output control register */
+	priv->write_reg(dev, REG_CDR, priv->cdr | CDR_PELICAN);
+
+	/* set acceptance filter (accept all) */
+	priv->write_reg(dev, REG_ACCC0, 0x00);
+	priv->write_reg(dev, REG_ACCC1, 0x00);
+	priv->write_reg(dev, REG_ACCC2, 0x00);
+	priv->write_reg(dev, REG_ACCC3, 0x00);
+
+	priv->write_reg(dev, REG_ACCM0, 0xFF);
+	priv->write_reg(dev, REG_ACCM1, 0xFF);
+	priv->write_reg(dev, REG_ACCM2, 0xFF);
+	priv->write_reg(dev, REG_ACCM3, 0xFF);
+
+	priv->write_reg(dev, REG_OCR, priv->ocr | OCR_MODE_NORMAL);
+}
+
+/*
+ * transmit a CAN message
+ * message layout in the sk_buff should be like this:
+ * xx xx xx xx	 ff	 ll   00 11 22 33 44 55 66 77
+ * [  can-id ] [flags] [len] [can data (up to 8 bytes]
+ */
+static int sja1000_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct can_frame *cf = (struct can_frame *)skb->data;
+	uint8_t fi;
+	uint8_t dlc;
+	canid_t id;
+	uint8_t dreg;
+	int i;
+
+	netif_stop_queue(dev);
+
+	fi = dlc = cf->can_dlc;
+	id = cf->can_id;
+
+	if (id & CAN_RTR_FLAG)
+		fi |= FI_RTR;
+
+	if (id & CAN_EFF_FLAG) {
+		fi |= FI_FF;
+		dreg = EFF_BUF;
+		priv->write_reg(dev, REG_FI, fi);
+		priv->write_reg(dev, REG_ID1, (id & 0x1fe00000) >> (5 + 16));
+		priv->write_reg(dev, REG_ID2, (id & 0x001fe000) >> (5 + 8));
+		priv->write_reg(dev, REG_ID3, (id & 0x00001fe0) >> 5);
+		priv->write_reg(dev, REG_ID4, (id & 0x0000001f) << 3);
+	} else {
+		dreg = SFF_BUF;
+		priv->write_reg(dev, REG_FI, fi);
+		priv->write_reg(dev, REG_ID1, (id & 0x000007f8) >> 3);
+		priv->write_reg(dev, REG_ID2, (id & 0x00000007) << 5);
+	}
+
+	for (i = 0; i < dlc; i++)
+		priv->write_reg(dev, dreg++, cf->data[i]);
+
+	stats->tx_bytes += dlc;
+	dev->trans_start = jiffies;
+
+	can_put_echo_skb(skb, dev, 0);
+
+	priv->write_reg(dev, REG_CMR, CMD_TR);
+
+	return 0;
+}
+
+static void sja1000_rx(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct can_frame *cf;
+	struct sk_buff *skb;
+	uint8_t fi;
+	uint8_t dreg;
+	canid_t id;
+	uint8_t dlc;
+	int i;
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (skb == NULL)
+		return;
+	skb->dev = dev;
+	skb->protocol = htons(ETH_P_CAN);
+
+	fi = priv->read_reg(dev, REG_FI);
+	dlc = fi & 0x0F;
+
+	if (fi & FI_FF) {
+		/* extended frame format (EFF) */
+		dreg = EFF_BUF;
+		id = (priv->read_reg(dev, REG_ID1) << (5 + 16))
+		    | (priv->read_reg(dev, REG_ID2) << (5 + 8))
+		    | (priv->read_reg(dev, REG_ID3) << 5)
+		    | (priv->read_reg(dev, REG_ID4) >> 3);
+		id |= CAN_EFF_FLAG;
+	} else {
+		/* standard frame format (SFF) */
+		dreg = SFF_BUF;
+		id = (priv->read_reg(dev, REG_ID1) << 3)
+		    | (priv->read_reg(dev, REG_ID2) >> 5);
+	}
+
+	if (fi & FI_RTR)
+		id |= CAN_RTR_FLAG;
+
+	cf = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
+	memset(cf, 0, sizeof(struct can_frame));
+	cf->can_id = id;
+	cf->can_dlc = dlc;
+	for (i = 0; i < dlc; i++)
+		cf->data[i] = priv->read_reg(dev, dreg++);
+
+	while (i < 8)
+		cf->data[i++] = 0;
+
+	/* release receive buffer */
+	priv->write_reg(dev, REG_CMR, CMD_RRB);
+
+	netif_rx(skb);
+
+	dev->last_rx = jiffies;
+	stats->rx_packets++;
+	stats->rx_bytes += dlc;
+}
+
+static int sja1000_err(struct net_device *dev, uint8_t isrc, uint8_t status)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct can_frame *cf;
+	struct sk_buff *skb;
+	enum can_state state = priv->can.state;
+	uint8_t ecc, alc;
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (skb == NULL)
+		return -ENOMEM;
+	skb->dev = dev;
+	skb->protocol = htons(ETH_P_CAN);
+	cf = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
+	memset(cf, 0, sizeof(struct can_frame));
+	cf->can_id = CAN_ERR_FLAG;
+	cf->can_dlc = CAN_ERR_DLC;
+
+	if (isrc & IRQ_DOI) {
+		/* data overrun interrupt */
+		dev_dbg(ND2D(dev), "data overrun interrupt\n");
+		cf->can_id |= CAN_ERR_CRTL;
+		cf->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+		priv->can.can_stats.data_overrun++;
+		priv->write_reg(dev, REG_CMR, CMD_CDO);	/* clear bit */
+	}
+
+	if (isrc & IRQ_EI) {
+		/* error warning interrupt */
+		priv->can.can_stats.error_warning++;
+		dev_dbg(ND2D(dev), "error warning interrupt\n");
+
+		if (status & SR_BS) {
+			state = CAN_STATE_BUS_OFF;
+			cf->can_id |= CAN_ERR_BUSOFF;
+			can_bus_off(dev);
+		} else if (status & SR_ES) {
+			state = CAN_STATE_BUS_WARNING;
+		} else
+			state = CAN_STATE_ACTIVE;
+	}
+	if (isrc & IRQ_BEI) {
+		/* bus error interrupt */
+		priv->can.can_stats.bus_error++;
+		ecc = priv->read_reg(dev, REG_ECC);
+
+		cf->can_id |= CAN_ERR_PROT | CAN_ERR_BUSERROR;
+
+		switch (ecc & ECC_MASK) {
+		case ECC_BIT:
+			cf->data[2] |= CAN_ERR_PROT_BIT;
+			break;
+		case ECC_FORM:
+			cf->data[2] |= CAN_ERR_PROT_FORM;
+			break;
+		case ECC_STUFF:
+			cf->data[2] |= CAN_ERR_PROT_STUFF;
+			break;
+		default:
+			cf->data[2] |= CAN_ERR_PROT_UNSPEC;
+			cf->data[3] = ecc & ECC_SEG;
+			break;
+		}
+		/* Error occured during transmission? */
+		if ((ecc & ECC_DIR) == 0)
+			cf->data[2] |= CAN_ERR_PROT_TX;
+	}
+	if (isrc & IRQ_EPI) {
+		/* error passive interrupt */
+		dev_dbg(ND2D(dev), "error passive interrupt\n");
+		priv->can.can_stats.error_passive++;
+		if (status & SR_ES)
+			state = CAN_STATE_BUS_PASSIVE;
+		else
+			state = CAN_STATE_ACTIVE;
+	}
+	if (isrc & IRQ_ALI) {
+		/* arbitration lost interrupt */
+		dev_dbg(ND2D(dev), "arbitration lost interrupt\n");
+		alc = priv->read_reg(dev, REG_ALC);
+		priv->can.can_stats.arbitration_lost++;
+		cf->can_id |= CAN_ERR_LOSTARB;
+		cf->data[0] = alc & 0x1f;
+	}
+
+	if (state != priv->can.state && (state == CAN_STATE_BUS_WARNING ||
+					 state == CAN_STATE_BUS_PASSIVE)) {
+		uint8_t rxerr = priv->read_reg(dev, REG_RXERR);
+		uint8_t txerr = priv->read_reg(dev, REG_TXERR);
+		cf->can_id |= CAN_ERR_CRTL;
+		if (state == CAN_STATE_BUS_WARNING)
+			cf->data[1] = (txerr > rxerr) ?
+				CAN_ERR_CRTL_TX_WARNING :
+				CAN_ERR_CRTL_RX_WARNING;
+		else
+			cf->data[1] = (txerr > rxerr) ?
+				CAN_ERR_CRTL_TX_PASSIVE :
+				CAN_ERR_CRTL_RX_PASSIVE;
+	}
+
+	priv->can.state = state;
+
+	netif_rx(skb);
+
+	dev->last_rx = jiffies;
+	stats->rx_packets++;
+	stats->rx_bytes += cf->can_dlc;
+
+	return 0;
+}
+
+irqreturn_t sja1000_interrupt(int irq, void *dev_id)
+{
+	struct net_device *dev = (struct net_device *)dev_id;
+	struct sja1000_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	uint8_t isrc, status;
+	int n = 0;
+
+	/* Shared interrupts and IRQ off? */
+	if (priv->read_reg(dev, REG_IER) == IRQ_OFF)
+		return IRQ_NONE;
+
+	if (priv->pre_irq)
+		priv->pre_irq(dev);
+
+	while ((isrc = priv->read_reg(dev, REG_IR)) && (n < SJA1000_MAX_IRQ)) {
+		n++;
+		status = priv->read_reg(dev, REG_SR);
+
+		if (isrc & IRQ_WUI) {
+			/* wake-up interrupt */
+			priv->can.can_stats.wakeup++;
+		}
+		if (isrc & IRQ_TI) {
+			/* transmission complete interrupt */
+			stats->tx_packets++;
+			can_get_echo_skb(dev, 0);
+			netif_wake_queue(dev);
+		}
+		if (isrc & IRQ_RI) {
+			/* receive interrupt */
+			while (status & SR_RBS) {
+				sja1000_rx(dev);
+				status = priv->read_reg(dev, REG_SR);
+			}
+		}
+		if (isrc & (IRQ_DOI | IRQ_EI | IRQ_BEI | IRQ_EPI | IRQ_ALI)) {
+			/* error interrupt */
+			if (sja1000_err(dev, isrc, status))
+				break;
+		}
+	}
+
+	if (priv->post_irq)
+		priv->post_irq(dev);
+
+	if (n >= SJA1000_MAX_IRQ)
+		dev_dbg(ND2D(dev), "%d messages handled in ISR", n);
+
+	return (n) ? IRQ_HANDLED : IRQ_NONE;
+}
+EXPORT_SYMBOL_GPL(sja1000_interrupt);
+
+static int sja1000_open(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	int err;
+
+	/* set chip into reset mode */
+	set_reset_mode(dev);
+
+	/* determine and set bittime */
+	err = can_set_bittiming(dev);
+	if (err)
+		return err;
+
+	/* register interrupt handler, if not done by the device driver */
+	if (!(priv->flags & SJA1000_CUSTOM_IRQ_HANDLER)) {
+		err = request_irq(dev->irq, &sja1000_interrupt, IRQF_SHARED,
+				  dev->name, (void *)dev);
+		if (err)
+			return -EAGAIN;
+	}
+
+	/* init and start chi */
+	sja1000_start(dev);
+	priv->open_time = jiffies;
+
+	netif_start_queue(dev);
+
+	return 0;
+}
+
+static int sja1000_close(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+
+	set_reset_mode(dev);
+	netif_stop_queue(dev);
+	priv->open_time = 0;
+	can_close_cleanup(dev);
+
+	if (!(priv->flags & SJA1000_CUSTOM_IRQ_HANDLER))
+		free_irq(dev->irq, (void *)dev);
+
+	return 0;
+}
+
+struct net_device *alloc_sja1000dev(int sizeof_priv)
+{
+	struct net_device *dev;
+	struct sja1000_priv *priv;
+
+	dev = alloc_candev(sizeof(struct sja1000_priv) + sizeof_priv);
+	if (!dev)
+		return NULL;
+
+	priv = netdev_priv(dev);
+	priv->dev = dev;
+
+	if (sizeof_priv)
+		priv->priv = (void *)priv + sizeof(struct sja1000_priv);
+
+	return dev;
+}
+EXPORT_SYMBOL_GPL(alloc_sja1000dev);
+
+void free_sja1000dev(struct net_device *dev)
+{
+	free_candev(dev);
+}
+EXPORT_SYMBOL_GPL(free_sja1000dev);
+
+int register_sja1000dev(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+	int err;
+
+	if (!sja1000_probe_chip(dev))
+		return -ENODEV;
+
+	dev->flags |= IFF_ECHO;	/* we support local echo */
+
+	dev->open = sja1000_open;
+	dev->stop = sja1000_close;
+	dev->hard_start_xmit = sja1000_start_xmit;
+
+	priv->can.bittiming_const = &sja1000_bittiming_const;
+	priv->can.do_set_bittiming = sja1000_set_bittiming;
+	priv->can.do_get_state = sja1000_get_state;
+	priv->can.do_set_mode = sja1000_set_mode;
+	priv->dev = dev;
+
+	err = register_candev(dev);
+	if (err) {
+		printk(KERN_INFO
+		       "%s: registering netdev failed\n", DRV_NAME);
+		free_netdev(dev);
+		return err;
+	}
+
+	set_reset_mode(dev);
+	chipset_init(dev);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(register_sja1000dev);
+
+void unregister_sja1000dev(struct net_device *dev)
+{
+	set_reset_mode(dev);
+	unregister_candev(dev);
+}
+EXPORT_SYMBOL_GPL(unregister_sja1000dev);
+
+static __init int sja1000_init(void)
+{
+	printk(KERN_INFO "%s CAN netdevice driver\n", DRV_NAME);
+
+	return 0;
+}
+
+module_init(sja1000_init);
+
+static __exit void sja1000_exit(void)
+{
+	printk(KERN_INFO "%s: driver removed\n", DRV_NAME);
+}
+
+module_exit(sja1000_exit);
diff --git a/drivers/net/can/sja1000/sja1000.h b/drivers/net/can/sja1000/sja1000.h
new file mode 100644
index 000000000000..60d4cd690a24
--- /dev/null
+++ b/drivers/net/can/sja1000/sja1000.h
@@ -0,0 +1,177 @@
+/*
+ * sja1000.h -  Philips SJA1000 network device driver
+ *
+ * Copyright (c) 2003 Matthias Brukner, Trajet Gmbh, Rebenring 33,
+ * 38106 Braunschweig, GERMANY
+ *
+ * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+#ifndef SJA1000_DEV_H
+#define SJA1000_DEV_H
+
+#include <linux/can/dev.h>
+#include <linux/can/platform/sja1000.h>
+
+#define SJA1000_MAX_IRQ 20	/* max. number of interrupts handled in ISR */
+
+/* SJA1000 registers - manual section 6.4 (Pelican Mode) */
+#define REG_MOD		0x00
+#define REG_CMR		0x01
+#define REG_SR		0x02
+#define REG_IR		0x03
+#define REG_IER		0x04
+#define REG_ALC		0x0B
+#define REG_ECC		0x0C
+#define REG_EWL		0x0D
+#define REG_RXERR	0x0E
+#define REG_TXERR	0x0F
+#define REG_ACCC0	0x10
+#define REG_ACCC1	0x11
+#define REG_ACCC2	0x12
+#define REG_ACCC3	0x13
+#define REG_ACCM0	0x14
+#define REG_ACCM1	0x15
+#define REG_ACCM2	0x16
+#define REG_ACCM3	0x17
+#define REG_RMC		0x1D
+#define REG_RBSA	0x1E
+
+/* Common registers - manual section 6.5 */
+#define REG_BTR0	0x06
+#define REG_BTR1	0x07
+#define REG_OCR		0x08
+#define REG_CDR		0x1F
+
+#define REG_FI		0x10
+#define SFF_BUF		0x13
+#define EFF_BUF		0x15
+
+#define FI_FF		0x80
+#define FI_RTR		0x40
+
+#define REG_ID1		0x11
+#define REG_ID2		0x12
+#define REG_ID3		0x13
+#define REG_ID4		0x14
+
+#define CAN_RAM		0x20
+
+/* mode register */
+#define MOD_RM		0x01
+#define MOD_LOM		0x02
+#define MOD_STM		0x04
+#define MOD_AFM		0x08
+#define MOD_SM		0x10
+
+/* commands */
+#define CMD_SRR		0x10
+#define CMD_CDO		0x08
+#define CMD_RRB		0x04
+#define CMD_AT		0x02
+#define CMD_TR		0x01
+
+/* interrupt sources */
+#define IRQ_BEI		0x80
+#define IRQ_ALI		0x40
+#define IRQ_EPI		0x20
+#define IRQ_WUI		0x10
+#define IRQ_DOI		0x08
+#define IRQ_EI		0x04
+#define IRQ_TI		0x02
+#define IRQ_RI		0x01
+#define IRQ_ALL		0xFF
+#define IRQ_OFF		0x00
+
+/* status register content */
+#define SR_BS		0x80
+#define SR_ES		0x40
+#define SR_TS		0x20
+#define SR_RS		0x10
+#define SR_TCS		0x08
+#define SR_TBS		0x04
+#define SR_DOS		0x02
+#define SR_RBS		0x01
+
+#define SR_CRIT (SR_BS|SR_ES)
+
+/* ECC register */
+#define ECC_SEG		0x1F
+#define ECC_DIR		0x20
+#define ECC_ERR		6
+#define ECC_BIT		0x00
+#define ECC_FORM	0x40
+#define ECC_STUFF	0x80
+#define ECC_MASK	0xc0
+
+/*
+ * Flags for sja1000priv.flags
+ */
+#define SJA1000_CUSTOM_IRQ_HANDLER 0x1
+
+/*
+ * SJA1000 private data structure
+ */
+struct sja1000_priv {
+	struct can_priv can;	/* must be the first member! */
+	long open_time;
+	struct sk_buff *echo_skb;
+
+	u8 (*read_reg) (struct net_device *dev, int reg);
+	void (*write_reg) (struct net_device *dev, int reg, u8 val);
+	void (*pre_irq) (struct net_device *dev);
+	void (*post_irq) (struct net_device *dev);
+
+	void *priv;		/* for board-specific data */
+	struct net_device *dev;
+
+	u8 ocr;
+	u8 cdr;
+	u32 flags;
+};
+
+struct net_device *alloc_sja1000dev(int sizeof_priv);
+void free_sja1000dev(struct net_device *dev);
+int register_sja1000dev(struct net_device *dev);
+void unregister_sja1000dev(struct net_device *dev);
+
+irqreturn_t sja1000_interrupt(int irq, void *dev_id);
+
+#endif /* SJA1000_DEV_H */
diff --git a/drivers/net/can/sja1000/sja1000_platform.c b/drivers/net/can/sja1000/sja1000_platform.c
new file mode 100644
index 000000000000..a6c5d760895f
--- /dev/null
+++ b/drivers/net/can/sja1000/sja1000_platform.c
@@ -0,0 +1,169 @@
+/*
+ * Copyright (C) 2005 Sascha Hauer, Pengutronix
+ * Copyright (C) 2007 Wolfgang Grandegger <wg@grandegger.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/delay.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/irq.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/can/platform/sja1000.h>
+#include <linux/io.h>
+
+#include "sja1000.h"
+
+#define DRV_NAME "sja1000_platform"
+
+MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
+MODULE_DESCRIPTION("Socket-CAN driver for SJA1000 on the platform bus");
+MODULE_LICENSE("GPL v2");
+
+static u8 sp_read_reg(struct net_device *dev, int reg)
+{
+	return ioread8((void __iomem *)(dev->base_addr + reg));
+}
+
+static void sp_write_reg(struct net_device *dev, int reg, u8 val)
+{
+	iowrite8(val, (void __iomem *)(dev->base_addr + reg));
+}
+
+static int sp_probe(struct platform_device *pdev)
+{
+	int err, irq;
+	void __iomem *addr;
+	struct net_device *dev;
+	struct sja1000_priv *priv;
+	struct resource *res_mem, *res_irq;
+	struct sja1000_platform_data *pdata;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platform data provided!\n");
+		err = -ENODEV;
+		goto exit;
+	}
+
+	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_mem || !res_irq) {
+		err = -ENODEV;
+		goto exit;
+	}
+
+	if (!request_mem_region(res_mem->start,
+				res_mem->end - res_mem->start + 1,
+				DRV_NAME)) {
+		err = -EBUSY;
+		goto exit;
+	}
+
+	addr = ioremap_nocache(res_mem->start,
+			       res_mem->end - res_mem->start + 1);
+	if (!addr) {
+		err = -ENOMEM;
+		goto exit_release;
+	}
+
+	irq = res_irq->start;
+	if (res_irq->flags & IRQF_TRIGGER_MASK)
+		set_irq_type(irq, res_irq->flags & IRQF_TRIGGER_MASK);
+
+	dev = alloc_sja1000dev(0);
+	if (!dev) {
+		err = -ENOMEM;
+		goto exit_iounmap;
+	}
+	priv = netdev_priv(dev);
+
+	priv->read_reg = sp_read_reg;
+	priv->write_reg = sp_write_reg;
+	priv->can.bittiming.clock = pdata->clock;
+	priv->ocr = pdata->ocr;
+	priv->cdr = pdata->cdr;
+
+	dev->irq = irq;
+	dev->base_addr = (unsigned long)addr;
+
+	dev_set_drvdata(&pdev->dev, dev);
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	err = register_sja1000dev(dev);
+	if (err) {
+		dev_err(&pdev->dev, "registering %s failed (err=%d)\n",
+			DRV_NAME, err);
+		goto exit_free;
+	}
+
+	dev_info(&pdev->dev, "%s device registered (base_addr=%#lx, irq=%d)\n",
+		 DRV_NAME, dev->base_addr, dev->irq);
+	return 0;
+
+ exit_free:
+	free_sja1000dev(dev);
+ exit_iounmap:
+	iounmap(addr);
+ exit_release:
+	release_mem_region(res_mem->start, res_mem->end - res_mem->start + 1);
+ exit:
+	return err;
+}
+
+static int sp_remove(struct platform_device *pdev)
+{
+	struct net_device *dev = dev_get_drvdata(&pdev->dev);
+	struct resource *res;
+
+	unregister_sja1000dev(dev);
+	dev_set_drvdata(&pdev->dev, NULL);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(res->start, res->end - res->start + 1);
+
+	if (dev->base_addr)
+		iounmap((void __iomem *)dev->base_addr);
+
+	free_sja1000dev(dev);
+
+	return 0;
+}
+
+static struct platform_driver sp_driver = {
+	.probe = sp_probe,
+	.remove = sp_remove,
+	.driver = {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init sp_init(void)
+{
+	return platform_driver_register(&sp_driver);
+}
+
+static void __exit sp_exit(void)
+{
+	platform_driver_unregister(&sp_driver);
+}
+
+module_init(sp_init);
+module_exit(sp_exit);
diff --git a/drivers/net/can/slcan.c b/drivers/net/can/slcan.c
new file mode 100644
index 000000000000..80fcff3e69d8
--- /dev/null
+++ b/drivers/net/can/slcan.c
@@ -0,0 +1,904 @@
+/*
+ * slcan.c - serial line CAN interface driver (using tty line discipline)
+ *
+ * Copyright (c) 2007 Volkswagen Group Electronic Research
+ * All rights reserved.
+ *
+ * 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.
+ * 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. Neither the name of Volkswagen nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General
+ * Public License ("GPL") version 2, in which case the provisions of the
+ * GPL apply INSTEAD OF those given above.
+ *
+ * The provided data structures and external interfaces from this code
+ * are not restricted to be used by modules with a GPL compatible license.
+ *
+ * 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.
+ *
+ * Send feedback to <socketcan-users@lists.berlios.de>
+ *
+ */
+
+/*
+ * This file is derived from linux/drivers/net/slip.c
+ *
+ * Therefore it has the same (strange?) behaviour not to unregister the
+ * netdevice when detaching the tty. Is there any better solution?
+ *
+ * Do not try to attach, detach and re-attach a tty for this reason ...
+ *
+ * slip.c Authors: Laurence Culhane, <loz@holmes.demon.co.uk>
+ *                 Fred N. van Kempen, <waltje@uwalt.nl.mugnet.org>
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+
+#include <asm/system.h>
+#include <linux/uaccess.h>
+#include <linux/bitops.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/interrupt.h>
+#include <linux/in.h>
+#include <linux/tty.h>
+#include <linux/errno.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/rtnetlink.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/if_slip.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+
+#include <linux/can.h>
+
+static __initdata const char banner[] =
+	KERN_INFO "slcan: serial line CAN interface driver\n";
+
+MODULE_ALIAS_LDISC(N_SLCAN);
+MODULE_DESCRIPTION("serial line CAN interface");
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
+
+#ifdef CONFIG_CAN_DEBUG_DEVICES
+static int debug;
+module_param(debug, int, S_IRUGO);
+#define DBG(args...)       (debug & 1 ? \
+			       (printk(KERN_DEBUG "slcan %s: ", __func__), \
+				printk(args)) : 0)
+#else
+#define DBG(args...)
+#endif
+
+#ifndef N_SLCAN
+#error Your kernel does not support tty line discipline N_SLCAN
+#endif
+/*
+ * As there is currently no line discipline N_SLCAN in the mainstream kernel
+ * you will have to modify two kernel includes & recompile the kernel.
+ *
+ * Add this in include/asm/termios.h after the definition of N_HCI:
+ *        #define N_SLCAN         16
+ *
+ * Increment NR_LDICS in include/linux/tty.h from 16 to 17
+ *
+ * NEW: Since Kernel 2.6.21 you only have to change include/linux/tty.h
+ *
+ */
+
+#define SLC_CHECK_TRANSMIT
+#define SLCAN_MAGIC 0x53CA
+
+static int maxdev = 10;		/* MAX number of SLCAN channels;
+				   This can be overridden with
+				   insmod slcan.ko maxdev=nnn	*/
+module_param(maxdev, int, 0);
+MODULE_PARM_DESC(maxdev, "Maximum number of slcan interfaces");
+
+/* maximum rx buffer len: extended CAN frame with timestamp */
+#define SLC_MTU (sizeof("T1111222281122334455667788EA5F\r")+1)
+
+struct slcan {
+	int			magic;
+
+	/* Various fields. */
+	struct tty_struct	*tty;		/* ptr to TTY structure	     */
+	struct net_device	*dev;		/* easy for intr handling    */
+	spinlock_t		lock;
+
+	/* These are pointers to the malloc()ed frame buffers. */
+	unsigned char		rbuff[SLC_MTU];	/* receiver buffer	     */
+	int			rcount;         /* received chars counter    */
+	unsigned char		xbuff[SLC_MTU];	/* transmitter buffer	     */
+	unsigned char		*xhead;         /* pointer to next XMIT byte */
+	int			xleft;          /* bytes left in XMIT queue  */
+
+	unsigned long		flags;		/* Flag values/ mode etc     */
+#define SLF_INUSE		0		/* Channel in use            */
+#define SLF_ERROR		1               /* Parity, etc. error        */
+
+	unsigned char		leased;
+	dev_t			line;
+	pid_t			pid;
+};
+
+static struct net_device **slcan_devs;
+
+
+ /************************************************************************
+  *			SLCAN ENCAPSULATION FORMAT		  	 *
+  ************************************************************************/
+
+/*
+ * A CAN frame has a can_id (11 bit standard frame format OR 29 bit extended
+ * frame format) a data length code (can_dlc) which can be from 0 to 8
+ * and up to <can_dlc> data bytes as payload.
+ * Additionally a CAN frame may become a remote transmission frame if the
+ * RTR-bit is set. This causes another ECU to send a CAN frame with the
+ * given can_id.
+ *
+ * The SLCAN ASCII representation of these different frame types is:
+ * <type> <id> <dlc> <data>*
+ *
+ * Extended frames (29 bit) are defined by capital characters in the type.
+ * RTR frames are defined as 'r' types - normal frames have 't' type:
+ * t => 11 bit data frame
+ * r => 11 bit RTR frame
+ * T => 29 bit data frame
+ * R => 29 bit RTR frame
+ *
+ * The <id> is 3 (standard) or 8 (extended) bytes in ASCII Hex (base64).
+ * The <dlc> is a one byte ASCII number ('0' - '8')
+ * The <data> section has at much ASCII Hex bytes as defined by the <dlc>
+ *
+ * Examples:
+ *
+ * t1230 : can_id 0x123, can_dlc 0, no data
+ * t4563112233 : can_id 0x456, can_dlc 3, data 0x11 0x22 0x33
+ * T12ABCDEF2AA55 : extended can_id 0x12ABCDEF, can_dlc 2, data 0xAA 0x55
+ * r1230 : can_id 0x123, can_dlc 0, no data, remote transmission request
+ *
+ */
+
+ /************************************************************************
+  *			STANDARD SLCAN DECAPSULATION		  	 *
+  ************************************************************************/
+
+static int asc2nibble(char c)
+{
+
+	if ((c >= '0') && (c <= '9'))
+		return c - '0';
+
+	if ((c >= 'A') && (c <= 'F'))
+		return c - 'A' + 10;
+
+	if ((c >= 'a') && (c <= 'f'))
+		return c - 'a' + 10;
+
+	return 16; /* error */
+}
+
+/* Send one completely decapsulated can_frame to the network layer */
+static void slc_bump(struct slcan *sl)
+{
+	struct net_device_stats *stats = &sl->dev->stats;
+	struct sk_buff *skb;
+	struct can_frame cf;
+	int i, dlc_pos, tmp;
+	unsigned long ultmp;
+	char cmd = sl->rbuff[0];
+
+	if ((cmd != 't') && (cmd != 'T') && (cmd != 'r') && (cmd != 'R'))
+		return;
+
+	if (cmd & 0x20) /* tiny chars 'r' 't' => standard frame format */
+		dlc_pos = 4; /* dlc position tiiid */
+	else
+		dlc_pos = 9; /* dlc position Tiiiiiiiid */
+
+	if (!((sl->rbuff[dlc_pos] >= '0') && (sl->rbuff[dlc_pos] < '9')))
+		return;
+
+	cf.can_dlc = sl->rbuff[dlc_pos] - '0'; /* get can_dlc from ASCII val */
+
+	sl->rbuff[dlc_pos] = 0; /* terminate can_id string */
+
+	if (strict_strtoul(sl->rbuff+1, 16, &ultmp))
+		return;
+	cf.can_id = ultmp;
+
+	if (!(cmd & 0x20)) /* NO tiny chars => extended frame format */
+		cf.can_id |= CAN_EFF_FLAG;
+
+	if ((cmd | 0x20) == 'r') /* RTR frame */
+		cf.can_id |= CAN_RTR_FLAG;
+
+	*(u64 *) (&cf.data) = 0; /* clear payload */
+
+	for (i = 0, dlc_pos++; i < cf.can_dlc; i++) {
+
+		tmp = asc2nibble(sl->rbuff[dlc_pos++]);
+		if (tmp > 0x0F)
+			return;
+		cf.data[i] = (tmp << 4);
+		tmp = asc2nibble(sl->rbuff[dlc_pos++]);
+		if (tmp > 0x0F)
+			return;
+		cf.data[i] |= tmp;
+	}
+
+
+	skb = dev_alloc_skb(sizeof(struct can_frame));
+	if (!skb)
+		return;
+
+	skb->dev = sl->dev;
+	skb->protocol = htons(ETH_P_CAN);
+	skb->pkt_type = PACKET_BROADCAST;
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	memcpy(skb_put(skb, sizeof(struct can_frame)),
+	       &cf, sizeof(struct can_frame));
+	netif_rx(skb);
+
+	sl->dev->last_rx = jiffies;
+	stats->rx_packets++;
+	stats->rx_bytes += cf.can_dlc;
+}
+
+/* parse tty input stream */
+static void slcan_unesc(struct slcan *sl, unsigned char s)
+{
+	struct net_device_stats *stats = &sl->dev->stats;
+
+	if ((s == '\r') || (s == '\a')) { /* CR or BEL ends the pdu */
+		if (!test_and_clear_bit(SLF_ERROR, &sl->flags) &&
+		    (sl->rcount > 4))  {
+			slc_bump(sl);
+		}
+		sl->rcount = 0;
+	} else {
+		if (!test_bit(SLF_ERROR, &sl->flags))  {
+			if (sl->rcount < SLC_MTU)  {
+				sl->rbuff[sl->rcount++] = s;
+				return;
+			} else {
+				stats->rx_over_errors++;
+				set_bit(SLF_ERROR, &sl->flags);
+			}
+		}
+	}
+}
+
+ /************************************************************************
+  *			STANDARD SLCAN ENCAPSULATION		  	 *
+  ************************************************************************/
+
+/* Encapsulate one can_frame and stuff into a TTY queue. */
+static void slc_encaps(struct slcan *sl, struct can_frame *cf)
+{
+	struct net_device_stats *stats = &sl->dev->stats;
+	int actual, idx, i;
+	char cmd;
+
+	if (cf->can_id & CAN_RTR_FLAG)
+		cmd = 'R'; /* becomes 'r' in standard frame format */
+	else
+		cmd = 'T'; /* becomes 't' in standard frame format */
+
+	if (cf->can_id & CAN_EFF_FLAG)
+		sprintf(sl->xbuff, "%c%08X%d", cmd,
+			cf->can_id & CAN_EFF_MASK, cf->can_dlc);
+	else
+		sprintf(sl->xbuff, "%c%03X%d", cmd | 0x20,
+			cf->can_id & CAN_SFF_MASK, cf->can_dlc);
+
+	idx = strlen(sl->xbuff);
+
+	for (i = 0; i < cf->can_dlc; i++)
+		sprintf(&sl->xbuff[idx + 2*i], "%02X", cf->data[i]);
+
+	DBG("ASCII frame = '%s'\n", sl->xbuff);
+
+	strcat(sl->xbuff, "\r"); /* add terminating character */
+
+	/* Order of next two lines is *very* important.
+	 * When we are sending a little amount of data,
+	 * the transfer may be completed inside driver.write()
+	 * routine, because it's running with interrupts enabled.
+	 * In this case we *never* got WRITE_WAKEUP event,
+	 * if we did not request it before write operation.
+	 *       14 Oct 1994  Dmitry Gorodchanin.
+	 */
+	sl->tty->flags |= (1 << TTY_DO_WRITE_WAKEUP);
+	actual = sl->tty->ops->write(sl->tty, sl->xbuff, strlen(sl->xbuff));
+#ifdef SLC_CHECK_TRANSMIT
+	sl->dev->trans_start = jiffies;
+#endif
+	sl->xleft = strlen(sl->xbuff) - actual;
+	sl->xhead = sl->xbuff + actual;
+	stats->tx_bytes += cf->can_dlc;
+}
+
+/*
+ * Called by the driver when there's room for more data.  If we have
+ * more packets to send, we send them here.
+ */
+static void slcan_write_wakeup(struct tty_struct *tty)
+{
+	int actual;
+	struct slcan *sl = (struct slcan *) tty->disc_data;
+	struct net_device_stats *stats = &sl->dev->stats;
+
+	/* First make sure we're connected. */
+	if (!sl || sl->magic != SLCAN_MAGIC || !netif_running(sl->dev))
+		return;
+
+	if (sl->xleft <= 0)  {
+		/* Now serial buffer is almost free & we can start
+		 * transmission of another packet */
+		stats->tx_packets++;
+		tty->flags &= ~(1 << TTY_DO_WRITE_WAKEUP);
+		netif_wake_queue(sl->dev);
+		return;
+	}
+
+	actual = tty->ops->write(tty, sl->xhead, sl->xleft);
+	sl->xleft -= actual;
+	sl->xhead += actual;
+}
+
+static void slc_tx_timeout(struct net_device *dev)
+{
+	struct slcan *sl = netdev_priv(dev);
+
+	spin_lock(&sl->lock);
+
+	if (netif_queue_stopped(dev)) {
+		if (!netif_running(dev))
+			goto out;
+
+		/* May be we must check transmitter timeout here ?
+		 *      14 Oct 1994 Dmitry Gorodchanin.
+		 */
+#ifdef SLC_CHECK_TRANSMIT
+		if (time_before(jiffies, dev->trans_start + 20 * HZ))  {
+			/* 20 sec timeout not reached */
+			goto out;
+		}
+		printk(KERN_WARNING "%s: transmit timed out, %s?\n", dev->name,
+		       (tty_chars_in_buffer(sl->tty) || sl->xleft)
+		       ? "bad line quality" : "driver error");
+		sl->xleft = 0;
+		sl->tty->flags &= ~(1 << TTY_DO_WRITE_WAKEUP);
+		netif_wake_queue(sl->dev);
+#endif
+	}
+out:
+	spin_unlock(&sl->lock);
+}
+
+
+/******************************************
+ *   Routines looking at netdevice side.
+ ******************************************/
+
+/* Send a can_frame to a TTY queue. */
+static int slc_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct slcan *sl = netdev_priv(dev);
+
+	if (skb->len != sizeof(struct can_frame))
+		goto out;
+
+	spin_lock(&sl->lock);
+	if (!netif_running(dev))  {
+		spin_unlock(&sl->lock);
+		printk(KERN_WARNING "%s: xmit: iface is down\n", dev->name);
+		goto out;
+	}
+
+	if (sl->tty == NULL) {
+		spin_unlock(&sl->lock);
+		goto out;
+	}
+
+	netif_stop_queue(sl->dev);
+	slc_encaps(sl, (struct can_frame *) skb->data); /* encaps & send */
+	spin_unlock(&sl->lock);
+
+out:
+	kfree_skb(skb);
+	return 0;
+}
+
+
+/* Netdevice UP -> DOWN routine */
+static int slc_close(struct net_device *dev)
+{
+	struct slcan *sl = netdev_priv(dev);
+
+	spin_lock_bh(&sl->lock);
+	if (sl->tty) {
+		/* TTY discipline is running. */
+		sl->tty->flags &= ~(1 << TTY_DO_WRITE_WAKEUP);
+	}
+	netif_stop_queue(dev);
+	sl->rcount   = 0;
+	sl->xleft    = 0;
+	spin_unlock_bh(&sl->lock);
+
+	return 0;
+}
+
+/* Netdevice DOWN -> UP routine */
+static int slc_open(struct net_device *dev)
+{
+	struct slcan *sl = netdev_priv(dev);
+
+	if (sl->tty == NULL)
+		return -ENODEV;
+
+	sl->flags &= (1 << SLF_INUSE);
+	netif_start_queue(dev);
+	return 0;
+}
+
+/* Netdevice register callback */
+static void slc_setup(struct net_device *dev)
+{
+	dev->open		= slc_open;
+	dev->destructor		= free_netdev;
+	dev->stop		= slc_close;
+	dev->hard_start_xmit	= slc_xmit;
+
+	dev->hard_header_len	= 0;
+	dev->addr_len		= 0;
+	dev->tx_queue_len	= 10;
+
+	dev->mtu		= sizeof(struct can_frame);
+	dev->type		= ARPHRD_CAN;
+#ifdef SLC_CHECK_TRANSMIT
+	dev->tx_timeout		= slc_tx_timeout;
+	dev->watchdog_timeo	= 20*HZ;
+#endif
+
+	/* New-style flags. */
+	dev->flags		= IFF_NOARP;
+	dev->features           = NETIF_F_NO_CSUM;
+}
+
+/******************************************
+ * Routines looking at TTY side.
+ ******************************************/
+
+/*
+ * Handle the 'receiver data ready' interrupt.
+ * This function is called by the 'tty_io' module in the kernel when
+ * a block of SLCAN data has been received, which can now be decapsulated
+ * and sent on to some IP layer for further processing. This will not
+ * be re-entered while running but other ldisc functions may be called
+ * in parallel
+ */
+
+static void slcan_receive_buf(struct tty_struct *tty,
+			      const unsigned char *cp, char *fp, int count)
+{
+	struct slcan *sl = (struct slcan *) tty->disc_data;
+	struct net_device_stats *stats = &sl->dev->stats;
+
+	if (!sl || sl->magic != SLCAN_MAGIC ||
+	    !netif_running(sl->dev))
+		return;
+
+	/* Read the characters out of the buffer */
+	while (count--) {
+		if (fp && *fp++) {
+			if (!test_and_set_bit(SLF_ERROR, &sl->flags))
+				stats->rx_errors++;
+			cp++;
+			continue;
+		}
+		slcan_unesc(sl, *cp++);
+	}
+}
+
+/************************************
+ *  slcan_open helper routines.
+ ************************************/
+
+/* Collect hanged up channels */
+
+static void slc_sync(void)
+{
+	int i;
+	struct net_device *dev;
+	struct slcan	  *sl;
+
+	for (i = 0; i < maxdev; i++) {
+		dev = slcan_devs[i];
+		if (dev == NULL)
+			break;
+
+		sl = netdev_priv(dev);
+		if (sl->tty || sl->leased)
+			continue;
+		if (dev->flags&IFF_UP)
+			dev_close(dev);
+	}
+}
+
+
+/* Find a free SLCAN channel, and link in this `tty' line. */
+static struct slcan *slc_alloc(dev_t line)
+{
+	int i;
+	int sel = -1;
+	int score = -1;
+	struct net_device *dev = NULL;
+	struct slcan       *sl;
+
+	if (slcan_devs == NULL)
+		return NULL;	/* Master array missing ! */
+
+	for (i = 0; i < maxdev; i++) {
+		dev = slcan_devs[i];
+		if (dev == NULL)
+			break;
+
+		sl = netdev_priv(dev);
+		if (sl->leased) {
+			if (sl->line != line)
+				continue;
+			if (sl->tty)
+				return NULL;
+
+			/* Clear ESCAPE & ERROR flags */
+			sl->flags &= (1 << SLF_INUSE);
+			return sl;
+		}
+
+		if (sl->tty)
+			continue;
+
+		if (current->pid == sl->pid) {
+			if (sl->line == line && score < 3) {
+				sel = i;
+				score = 3;
+				continue;
+			}
+			if (score < 2) {
+				sel = i;
+				score = 2;
+			}
+			continue;
+		}
+		if (sl->line == line && score < 1) {
+			sel = i;
+			score = 1;
+			continue;
+		}
+		if (score < 0) {
+			sel = i;
+			score = 0;
+		}
+	}
+
+	if (sel >= 0) {
+		i = sel;
+		dev = slcan_devs[i];
+		if (score > 1) {
+			sl = netdev_priv(dev);
+			sl->flags &= (1 << SLF_INUSE);
+			return sl;
+		}
+	}
+
+	/* Sorry, too many, all slots in use */
+	if (i >= maxdev)
+		return NULL;
+
+	if (dev) {
+		sl = netdev_priv(dev);
+		if (test_bit(SLF_INUSE, &sl->flags)) {
+			unregister_netdevice(dev);
+			dev = NULL;
+			slcan_devs[i] = NULL;
+		}
+	}
+
+	if (!dev) {
+		char name[IFNAMSIZ];
+		sprintf(name, "slc%d", i);
+
+		dev = alloc_netdev(sizeof(*sl), name, slc_setup);
+		if (!dev)
+			return NULL;
+		dev->base_addr  = i;
+	}
+
+	sl = netdev_priv(dev);
+
+	/* Initialize channel control data */
+	sl->magic       = SLCAN_MAGIC;
+	sl->dev	      	= dev;
+	spin_lock_init(&sl->lock);
+	slcan_devs[i] = dev;
+
+	return sl;
+}
+
+/*
+ * Open the high-level part of the SLCAN channel.
+ * This function is called by the TTY module when the
+ * SLCAN line discipline is called for.  Because we are
+ * sure the tty line exists, we only have to link it to
+ * a free SLCAN channel...
+ *
+ * Called in process context serialized from other ldisc calls.
+ */
+
+static int slcan_open(struct tty_struct *tty)
+{
+	struct slcan *sl;
+	int err;
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	if (tty->ops->write == NULL)
+		return -EOPNOTSUPP;
+
+	/* RTnetlink lock is misused here to serialize concurrent
+	   opens of slcan channels. There are better ways, but it is
+	   the simplest one.
+	 */
+	rtnl_lock();
+
+	/* Collect hanged up channels. */
+	slc_sync();
+
+	sl = (struct slcan *) tty->disc_data;
+
+	err = -EEXIST;
+	/* First make sure we're not already connected. */
+	if (sl && sl->magic == SLCAN_MAGIC)
+		goto err_exit;
+
+	/* OK.  Find a free SLCAN channel to use. */
+	err = -ENFILE;
+	sl = slc_alloc(tty_devnum(tty));
+	if (sl == NULL)
+		goto err_exit;
+
+	sl->tty = tty;
+	tty->disc_data = sl;
+	sl->line = tty_devnum(tty);
+	sl->pid = current->pid;
+
+	if (!test_bit(SLF_INUSE, &sl->flags)) {
+		/* Perform the low-level SLCAN initialization. */
+		sl->rcount   = 0;
+		sl->xleft    = 0;
+
+		set_bit(SLF_INUSE, &sl->flags);
+
+		err = register_netdevice(sl->dev);
+		if (err)
+			goto err_free_chan;
+	}
+
+	/* Done.  We have linked the TTY line to a channel. */
+	rtnl_unlock();
+
+	tty->receive_room = 65536;	/* We don't flow control */
+
+	return sl->dev->base_addr;
+
+err_free_chan:
+	sl->tty = NULL;
+	tty->disc_data = NULL;
+	clear_bit(SLF_INUSE, &sl->flags);
+
+err_exit:
+	rtnl_unlock();
+
+	/* Count references from TTY module */
+	return err;
+}
+
+/*
+
+  FIXME: 1,2 are fixed 3 was never true anyway.
+
+   Let me to blame a bit.
+   1. TTY module calls this funstion on soft interrupt.
+   2. TTY module calls this function WITH MASKED INTERRUPTS!
+   3. TTY module does not notify us about line discipline
+      shutdown,
+
+   Seems, now it is clean. The solution is to consider netdevice and
+   line discipline sides as two independent threads.
+
+   By-product (not desired): slc? does not feel hangups and remains open.
+   It is supposed, that user level program (dip, diald, slattach...)
+   will catch SIGHUP and make the rest of work.
+
+   I see no way to make more with current tty code. --ANK
+ */
+
+/*
+ * Close down a SLCAN channel.
+ * This means flushing out any pending queues, and then returning. This
+ * call is serialized against other ldisc functions.
+ */
+static void slcan_close(struct tty_struct *tty)
+{
+	struct slcan *sl = (struct slcan *) tty->disc_data;
+
+	/* First make sure we're connected. */
+	if (!sl || sl->magic != SLCAN_MAGIC || sl->tty != tty)
+		return;
+
+	tty->disc_data = NULL;
+	sl->tty = NULL;
+	if (!sl->leased)
+		sl->line = 0;
+
+	/* Count references from TTY module */
+}
+
+/* Perform I/O control on an active SLCAN channel. */
+static int slcan_ioctl(struct tty_struct *tty, struct file *file,
+		       unsigned int cmd, unsigned long arg)
+{
+	struct slcan *sl = (struct slcan *) tty->disc_data;
+	unsigned int tmp;
+
+	/* First make sure we're connected. */
+	if (!sl || sl->magic != SLCAN_MAGIC)
+		return -EINVAL;
+
+	switch (cmd) {
+	case SIOCGIFNAME:
+		tmp = strlen(sl->dev->name) + 1;
+		if (copy_to_user((void __user *)arg, sl->dev->name, tmp))
+			return -EFAULT;
+		return 0;
+
+	case SIOCSIFHWADDR:
+		return -EINVAL;
+
+	default:
+		return tty_mode_ioctl(tty, file, cmd, arg);
+	}
+}
+
+static struct tty_ldisc_ops slc_ldisc = {
+	.owner 		= THIS_MODULE,
+	.magic 		= TTY_LDISC_MAGIC,
+	.name 		= "slcan",
+	.open 		= slcan_open,
+	.close	 	= slcan_close,
+	.ioctl		= slcan_ioctl,
+	.receive_buf	= slcan_receive_buf,
+	.write_wakeup	= slcan_write_wakeup,
+};
+
+/************************************
+ * general slcan module init/exit
+ ************************************/
+
+static int __init slcan_init(void)
+{
+	int status;
+
+	if (maxdev < 4)
+		maxdev = 4; /* Sanity */
+
+	printk(banner);
+	printk(KERN_INFO "slcan: %d dynamic interface channels.\n", maxdev);
+
+	slcan_devs = kmalloc(sizeof(struct net_device *)*maxdev, GFP_KERNEL);
+	if (!slcan_devs) {
+		printk(KERN_ERR "slcan: can't allocate slcan device array!\n");
+		return -ENOMEM;
+	}
+
+	/* Clear the pointer array, we allocate devices when we need them */
+	memset(slcan_devs, 0, sizeof(struct net_device *)*maxdev);
+
+	/* Fill in our line protocol discipline, and register it */
+	status = tty_register_ldisc(N_SLCAN, &slc_ldisc);
+	if (status != 0)  {
+		printk(KERN_ERR "slcan: can't register line discipline\n");
+		kfree(slcan_devs);
+	}
+	return status;
+}
+
+static void __exit slcan_exit(void)
+{
+	int i;
+	struct net_device *dev;
+	struct slcan *sl;
+	unsigned long timeout = jiffies + HZ;
+	int busy = 0;
+
+	if (slcan_devs == NULL)
+		return;
+
+	/* First of all: check for active disciplines and hangup them.
+	 */
+	do {
+		if (busy)
+			msleep_interruptible(100);
+
+		busy = 0;
+		for (i = 0; i < maxdev; i++) {
+			dev = slcan_devs[i];
+			if (!dev)
+				continue;
+			sl = netdev_priv(dev);
+			spin_lock_bh(&sl->lock);
+			if (sl->tty) {
+				busy++;
+				tty_hangup(sl->tty);
+			}
+			spin_unlock_bh(&sl->lock);
+		}
+	} while (busy && time_before(jiffies, timeout));
+
+
+	for (i = 0; i < maxdev; i++) {
+		dev = slcan_devs[i];
+		if (!dev)
+			continue;
+		slcan_devs[i] = NULL;
+
+		sl = netdev_priv(dev);
+		if (sl->tty) {
+			printk(KERN_ERR "%s: tty discipline still running\n",
+			       dev->name);
+			/* Intentionally leak the control block. */
+			dev->destructor = NULL;
+		}
+
+		unregister_netdev(dev);
+	}
+
+	kfree(slcan_devs);
+	slcan_devs = NULL;
+
+	i = tty_unregister_ldisc(N_SLCAN);
+	if (i)
+		printk(KERN_ERR "slcan: can't unregister ldisc (err %d)\n", i);
+}
+
+module_init(slcan_init);
+module_exit(slcan_exit);
diff --git a/drivers/net/can/softing/Makefile b/drivers/net/can/softing/Makefile
new file mode 100644
index 000000000000..df3fceca7bd0
--- /dev/null
+++ b/drivers/net/can/softing/Makefile
@@ -0,0 +1,20 @@
+# Makefile for softing CAN driver
+
+ifeq ($(KERNELRELEASE),)
+# necessary when used outside kernel
+KERNELDIR := /lib/modules/$(shell uname -r)/build
+PWD       := $(shell pwd)
+TOPDIR    := $(PWD)/../../../..
+
+modules modules_install clean:
+	$(MAKE) -C $(KERNELDIR) M=$(PWD) $@ TOPDIR=$(TOPDIR)
+
+else
+
+-include $(TOPDIR)/Makefile.common
+
+softing-y := softing_main.o softing_fw.o
+obj-$(CONFIG_CAN_SOFTING)        += softing.o
+obj-$(CONFIG_CAN_SOFTING_CS)     += softing_cs.o
+
+endif
diff --git a/drivers/net/can/softing/softing.h b/drivers/net/can/softing/softing.h
new file mode 100644
index 000000000000..e43c7f116799
--- /dev/null
+++ b/drivers/net/can/softing/softing.h
@@ -0,0 +1,268 @@
+/*
+ * softing common interfaces
+ *
+ * by Kurt Van Dijck, 06-2008
+ */
+
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+
+struct softing;
+struct sofing_desc;
+
+/* special attribute, so we should not rely on the ->priv pointers
+ * before knowing how to interpret these
+ */
+struct softing_attribute;
+
+struct softing_priv {
+	struct can_priv can;	/* must be the first member! */
+	struct net_device *netdev;
+	struct softing *card;
+	struct {
+		int pending;
+		/* variables wich hold the circular buffer */
+		int echo_put;
+		int echo_get;
+	} tx;
+	struct can_bittiming_const btr_const;
+	int index;
+	u8 output;
+	u16 chip;
+	struct attribute_group sysfs;
+};
+#define netdev2softing(netdev)	((struct softing_priv *)netdev_priv(netdev))
+
+/* the 'all cards have the same' fields definition */
+extern const struct can_bittiming_const softing_btr_const;
+
+struct softing_desc {
+	unsigned int manf;
+	unsigned int prod;
+	/* generation
+	 * 1st with NEC or SJA1000
+	 * 8bit, exclusive interrupt, ...
+	 * 2nd only SJA11000
+	 * 16bit, shared interrupt
+	 */
+	int generation;
+	unsigned int freq;	/*crystal in MHz */
+	unsigned int max_brp;
+	unsigned int max_sjw;
+	unsigned long dpram_size;
+	char name[32];
+	struct {
+		unsigned long offs;
+		unsigned long addr;
+		char fw[32];
+	} boot, load, app;
+};
+
+struct softing {
+	int nbus;
+	struct softing_priv *bus[2];
+	spinlock_t	 spin; /* protect this structure & DPRAM access */
+
+	struct {
+		/* indication of firmware status */
+		int up;
+		/* protection of the 'up' variable */
+		struct mutex lock;
+	} fw;
+	struct {
+		int nr;
+		int requested;
+		struct tasklet_struct bh;
+		int svc_count;
+	} irq;
+	struct {
+		int pending;
+		int last_bus;
+		/* keep the bus that last tx'd a message,
+		 * in order to let every netdev queue resume
+		 */
+	} tx;
+	struct {
+		unsigned long phys;
+		unsigned long size;
+		unsigned char *virt;
+		unsigned char *end;
+		struct softing_fct  *fct;
+		struct softing_info *info;
+		struct softing_rx  *rx;
+		struct softing_tx  *tx;
+		struct softing_irq *irq;
+		unsigned short *command;
+		unsigned short *receipt;
+	} dpram;
+	struct {
+		unsigned short manf;
+		unsigned short prod;
+		u32  serial, fw, hw, lic;
+		u16  chip [2];
+		u32  freq;
+		const char *name;
+	} id;
+	const struct softing_desc		*desc;
+	struct {
+		int (*reset)	 (struct softing *, int);
+		int (*enable_irq)(struct softing *, int);
+	} fn;
+	struct device *dev;
+	/* sysfs */
+	struct attribute_group sysfs;
+	struct softing_attribute *attr;
+	struct attribute **grp;
+};
+
+extern int	mk_softing(struct softing *);
+/* fields that must be set already are :
+ * ncan
+ * id.manf
+ * id.prod
+ * fn.reset
+ * fn.enable_irq
+ */
+extern void rm_softing(struct softing *);
+/* usefull functions during operation */
+
+extern const struct softing_desc *
+	softing_lookup_desc(unsigned int manf, unsigned int prod);
+
+extern int softing_default_output(struct softing *card
+			, struct softing_priv *priv);
+extern u32 softing_time2usec(struct softing *card, u32 raw);
+
+extern int softing_fct_cmd(struct softing *card
+			, int cmd, int vector, const char *msg);
+
+extern int softing_bootloader_command(struct softing *card
+			, int command, const char *msg);
+
+/* Load firmware after reset */
+extern int softing_load_fw(const char *file, struct softing *card,
+			unsigned char *virt, unsigned int size, int offset);
+
+/* Load final application firmware after bootloader */
+extern int softing_load_app_fw(const char *file, struct softing *card);
+
+extern int softing_reset_chip(struct softing *card);
+
+/* enable or disable irq
+ * only called with fw.lock locked
+ */
+extern int softing_card_irq(struct softing *card, int enable);
+
+/* called when tx queue is flushed */
+extern void softing_flush_echo_skb(struct softing_priv *priv);
+
+/* reinitaliase the card, apply -1 for bus[01] for 'no change' */
+extern int softing_reinit(struct softing *card, int bus0, int bus1);
+
+/* SOFTING DPRAM mappings */
+struct softing_rx {
+	u8  fifo[16][32];
+	u8  dummy1;
+	u16 rd;
+	u16 dummy2;
+	u16 wr;
+	u16  dummy3;
+	u16 lost_msg;
+} __attribute__((packed));
+
+#define TXMAX	31
+struct softing_tx {
+	u8  fifo[32][16];
+	u8  dummy1;
+	u16 rd;
+	u16 dummy2;
+	u16 wr;
+	u8  dummy3;
+} __attribute__((packed));
+
+struct softing_irq {
+	u8 to_host;
+	u8 to_card;
+} __attribute__((packed));
+
+struct softing_fct {
+	s16 param[20]; /* 0 is index */
+	s16 returned;
+	u8  dummy;
+	u16 host_access;
+} __attribute__((packed));
+
+struct softing_info {
+	u8  dummy1;
+	u16 bus_state;
+	u16 dummy2;
+	u16 bus_state2;
+	u16 dummy3;
+	u16 error_state;
+	u16 dummy4;
+	u16 error_state2;
+	u16 dummy5;
+	u16 reset;
+	u16 dummy6;
+	u16 clear_rcv_fifo;
+	u16 dummy7;
+	u16 dummyxx;
+	u16 dummy8;
+	u16 time_reset;
+	u8  dummy9;
+	u32 time;
+	u32 time_wrap;
+	u8  wr_start;
+	u8  wr_end;
+	u8  dummy10;
+	u16 dummy12;
+	u16 dummy12x;
+	u16 dummy13;
+	u16 reset_rcv_fifo;
+	u8  dummy14;
+	u8  reset_xmt_fifo;
+	u8  read_fifo_levels;
+	u16 rcv_fifo_level;
+	u16 xmt_fifo_level;
+} __attribute__((packed));
+
+/* DPRAM return codes */
+#define RES_NONE 0
+#define RES_OK	 1
+#define RES_NOK  2
+#define RES_UNKNOWN 3
+/* DPRAM flags */
+#define CMD_TX		0x01
+#define CMD_ACK 0x02
+#define CMD_XTD 0x04
+#define CMD_RTR 0x08
+#define CMD_ERR 0x10
+#define CMD_BUS2	0x80
+
+/* debug */
+extern int softing_debug;
+
+#define mod_alert(fmt,arg...) { \
+	if (softing_debug >= 0) \
+		printk(KERN_ALERT "[%s] %s:" fmt "\n" \
+			, THIS_MODULE->name \
+			, __func__ \
+			, ##arg); \
+	}
+#define mod_info(fmt,arg...) { \
+	if (softing_debug >= 1) \
+		printk(KERN_INFO	"[%s] %s:" fmt "\n"\
+			, THIS_MODULE->name \
+			, __func__ \
+			, ##arg); \
+	}
+#define mod_trace(fmt,arg...) { \
+	if (softing_debug >= 2) \
+		printk(KERN_DEBUG "[%s] %s:" fmt "\n" \
+			, THIS_MODULE->name \
+			, __func__ \
+			, ##arg); \
+	}
+
diff --git a/drivers/net/can/softing/softing_cs.c b/drivers/net/can/softing/softing_cs.c
new file mode 100644
index 000000000000..e33b614de28f
--- /dev/null
+++ b/drivers/net/can/softing/softing_cs.c
@@ -0,0 +1,427 @@
+/*
+* drivers/net/can/softing/softing_cs.c
+*
+* Copyright (C) 2008
+*
+* - Kurt Van Dijck, EIA Electronics
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/ptrace.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/timer.h>
+#include <linux/major.h>
+#include <linux/io.h>
+
+#include <pcmcia/cs_types.h>
+#include <pcmcia/cs.h>
+#include <pcmcia/cistpl.h>
+#include <pcmcia/ciscode.h>
+#include <pcmcia/ds.h>
+#include <pcmcia/cisreg.h>
+
+#include <asm/system.h>
+
+#include "softing.h"
+
+struct softing_cs {
+	struct softing	 softing;
+	win_req_t win;
+};
+#define softing2cs(x) container_of((x), struct softing_cs, softing)
+
+struct lookup {
+	int i;
+	const char *a;
+};
+
+static const char __devinit *lookup_mask(const struct lookup *lp, int *i)
+{
+	for (; lp->a; ++lp) {
+		if (lp->i & *i) {
+			*i &= ~lp->i;
+			return lp->a;
+		}
+	}
+	return 0;
+}
+
+static int card_reset_via_pcmcia(struct softing *sdev, int v)
+{
+	struct pcmcia_device *pcmcia = to_pcmcia_dev(sdev->dev);
+	conf_reg_t reg;
+	reg.Function = 0; /* socket */
+	reg.Action	 = CS_WRITE;
+	reg.Offset	 = 2;
+	reg.Value	 = v ? 0 : 0x20;
+	return pcmcia_access_configuration_register(pcmcia, &reg);
+}
+
+static int card_reset_via_dpram(struct softing *sdev, int v)
+{
+	if (v) {
+		spin_lock_bh(&sdev->spin);
+		sdev->dpram.virt[0xe00] &= ~1;
+		spin_unlock_bh(&sdev->spin);
+		card_reset_via_pcmcia(sdev, v);
+	} else {
+		card_reset_via_pcmcia(sdev, v);
+		spin_lock_bh(&sdev->spin);
+		sdev->dpram.virt[0xe00] |=  1;
+		spin_unlock_bh(&sdev->spin);
+	}
+	return 0;
+}
+
+static int card_enable_irq_via_pcmcia(struct softing *sdev, int v)
+{
+	int ret;
+	struct pcmcia_device *pcmcia = to_pcmcia_dev(sdev->dev);
+	conf_reg_t reg;
+	memset(&reg, 0, sizeof(reg));
+	reg.Function = 0; /* socket */
+	reg.Action	 = CS_WRITE;
+	reg.Offset	 = 0;
+	reg.Value	 = v ? 0x60 : 0;
+	ret = pcmcia_access_configuration_register(pcmcia, &reg);
+	if (ret)
+		mod_alert("failed %u", ret);
+	return ret;
+}
+
+/* TODO: in 2.6.26, __devinitconst works*/
+static const __devinitdata struct lookup pcmcia_io_attr[] = {
+	{ IO_DATA_PATH_WIDTH_AUTO	, "[auto]"	, },
+	{ IO_DATA_PATH_WIDTH_8		, "8bit"	, },
+	{ IO_DATA_PATH_WIDTH_16		, "16bit"	, },
+	{ 0, 0, },
+};
+
+static const __devinitdata struct lookup pcmcia_mem_attr[] = {
+	{ WIN_ADDR_SPACE_IO	, "IO"		, },
+	{ WIN_MEMORY_TYPE_AM	, "typeAM"	, },
+	{ WIN_ENABLE		, "enable"	, },
+	{ WIN_DATA_WIDTH_8	, "8bit"	, },
+	{ WIN_DATA_WIDTH_16	, "16bit"	, },
+	{ WIN_DATA_WIDTH_32	, "32bit"	, },
+	{ WIN_PAGED		, "paged"	, },
+	{ WIN_SHARED		, "shared"	, },
+	{ WIN_FIRST_SHARED	, "first_shared", },
+	{ WIN_USE_WAIT		, "wait"	, },
+	{ WIN_STRICT_ALIGN	, "strict_align", },
+	{ WIN_MAP_BELOW_1MB	, "below_1MB"	, },
+	{ WIN_PREFETCH		, "prefetch"	, },
+	{ WIN_CACHEABLE		, "cacheable"	, },
+	{ 0, 0, },
+};
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 28)
+/* backported */
+struct pcmcia_cfg_mem {
+	tuple_t tuple;
+	cisparse_t parse;
+	u8 buf[256];
+	cistpl_cftable_entry_t dflt;
+};
+static int pcmcia_loop_config(struct pcmcia_device *p_dev,
+		       int	(*conf_check)	(struct pcmcia_device *p_dev,
+						 cistpl_cftable_entry_t *cfg,
+						 cistpl_cftable_entry_t *dflt,
+						 unsigned int vcc,
+						 void *priv_data),
+		       void *priv_data)
+{
+	struct pcmcia_cfg_mem *cfg_mem;
+
+	tuple_t *tuple;
+	int ret = -ENODEV;
+	unsigned int vcc;
+
+	cfg_mem = kzalloc(sizeof(*cfg_mem), GFP_KERNEL);
+	if (cfg_mem == NULL)
+		return -ENOMEM;
+
+	/* get the current Vcc setting */
+	vcc = p_dev->socket->socket.Vcc;
+
+	tuple = &cfg_mem->tuple;
+	tuple->TupleData = cfg_mem->buf;
+	tuple->TupleDataMax = sizeof(cfg_mem->buf)-1;
+	tuple->TupleOffset = 0;
+	tuple->DesiredTuple = CISTPL_CFTABLE_ENTRY;
+	tuple->Attributes = 0;
+
+	ret = pcmcia_get_first_tuple(p_dev, tuple);
+	while (!ret) {
+		cistpl_cftable_entry_t *cfg = &cfg_mem->parse.cftable_entry;
+
+		if (pcmcia_get_tuple_data(p_dev, tuple))
+			goto next_entry;
+
+		if (pcmcia_parse_tuple(p_dev, tuple, &cfg_mem->parse))
+			goto next_entry;
+
+		/* default values */
+		p_dev->conf.ConfigIndex = cfg->index;
+		if (cfg->flags & CISTPL_CFTABLE_DEFAULT)
+			cfg_mem->dflt = *cfg;
+
+		ret = conf_check(p_dev, cfg, &cfg_mem->dflt, vcc, priv_data);
+		if (!ret)
+			break;
+
+next_entry:
+		ret = pcmcia_get_next_tuple(p_dev, tuple);
+	}
+	kfree(cfg_mem);
+	return ret;
+}
+#endif
+
+static int dev_conf_check(struct pcmcia_device *pdev,
+	cistpl_cftable_entry_t *cf, cistpl_cftable_entry_t *def_cf,
+	unsigned int vcc, void *priv_data)
+{
+	struct softing_cs *csdev = priv_data;
+	struct softing *sdev = &csdev->softing;
+	int ret;
+
+	if (!cf->index)
+		goto do_next;
+	/* power settings (Vcc & Vpp) */
+	if (cf->vcc.present & (1 << CISTPL_POWER_VNOM)) {
+		if (vcc != cf->vcc.param[CISTPL_POWER_VNOM]/10000) {
+			mod_alert("%s: cf->Vcc mismatch\n", __FILE__);
+			goto do_next;
+		}
+	} else if (def_cf->vcc.present & (1 << CISTPL_POWER_VNOM)) {
+		if (vcc != def_cf->vcc.param[CISTPL_POWER_VNOM]/10000) {
+			mod_alert("%s: cf->Vcc mismatch\n", __FILE__);
+			goto do_next;
+		}
+	}
+	if (cf->vpp1.present & (1 << CISTPL_POWER_VNOM))
+		pdev->conf.Vpp
+			= cf->vpp1.param[CISTPL_POWER_VNOM] / 10000;
+
+	else if (def_cf->vpp1.present & (1 << CISTPL_POWER_VNOM))
+		pdev->conf.Vpp
+			= def_cf->vpp1.param[CISTPL_POWER_VNOM] / 10000;
+
+	/* interrupt ? */
+	if (cf->irq.IRQInfo1 || def_cf->irq.IRQInfo1)
+		pdev->conf.Attributes |= CONF_ENABLE_IRQ;
+
+	/* IO window */
+	pdev->io.NumPorts1
+		= pdev->io.NumPorts2
+		= 0;
+	/* Memory window */
+	if ((cf->mem.nwin > 0) || (def_cf->mem.nwin > 0)) {
+		memreq_t map;
+		cistpl_mem_t *mem
+			= (cf->mem.nwin) ? &cf->mem : &def_cf->mem;
+		/* softing specific: choose 8 or 16bit access */
+		csdev->win.Attributes = ((sdev->desc->generation >= 2)
+				? WIN_DATA_WIDTH_16 : WIN_DATA_WIDTH_8)
+			| WIN_MEMORY_TYPE_CM
+			| WIN_ENABLE;
+		csdev->win.Base = mem->win[0].host_addr;
+		csdev->win.Size = mem->win[0].len;
+		csdev->win.AccessSpeed = 0;
+		ret = pcmcia_request_window(&pdev, &csdev->win, &pdev->win);
+		if (ret) {
+			mod_alert("pcmcia_request_window() mismatch\n");
+			goto do_next;
+		}
+		/* softing specific: choose slower access for old cards */
+		if (sdev->desc->generation < 2) {
+			pdev->win->ctl.flags
+				= MAP_ACTIVE | MAP_USE_WAIT;
+			pdev->win->ctl.speed = 3;
+		}
+		map.Page = 0;
+		map.CardOffset = mem->win[0].card_addr;
+		if (pcmcia_map_mem_page(pdev->win, &map)) {
+			mod_alert("pcmcia_map_mem_page() mismatch\n");
+			goto do_next_win;
+		}
+	} else {
+		mod_info("no memory window in tuple %u", cf->index);
+		goto do_next;
+	}
+	return 0;
+do_next_win:
+do_next:
+	pcmcia_disable_device(pdev);
+	return -ENODEV;
+}
+
+static void driver_remove(struct pcmcia_device *pcmcia)
+{
+	struct softing *card = (struct softing *)pcmcia->priv;
+	struct softing_cs *cs = softing2cs(card);
+	mod_trace("%s,device'%s'", card->id.name, pcmcia->devname);
+	rm_softing(card);
+	/* release pcmcia stuff */
+	pcmcia_disable_device(pcmcia);
+	/* free bits */
+	kfree(cs);
+}
+
+static int __devinit driver_probe(struct pcmcia_device *pcmcia)
+{
+	struct softing_cs *cs;
+	struct softing		*card;
+
+	mod_trace("on %s", pcmcia->devname);
+
+	/* Create new softing device */
+	cs = kzalloc(sizeof(*cs), GFP_KERNEL);
+	if (!cs)
+		goto no_mem;
+	/* setup links */
+	card = &cs->softing;
+	pcmcia->priv = card;
+	card->dev = &pcmcia->dev;
+	/* properties */
+	card->id.manf = pcmcia->manf_id;
+	card->id.prod = pcmcia->card_id;
+	card->desc = softing_lookup_desc(card->id.manf, card->id.prod);
+	if (card->desc->generation >= 2) {
+		card->fn.reset = card_reset_via_dpram;
+	} else {
+		card->fn.reset = card_reset_via_pcmcia;
+		card->fn.enable_irq = card_enable_irq_via_pcmcia;
+	}
+
+	card->nbus = 2;
+	/* pcmcia presets */
+	pcmcia->irq.Attributes = IRQ_TYPE_DYNAMIC_SHARING;
+	pcmcia->irq.IRQInfo1 = IRQ_LEVEL_ID;
+	pcmcia->irq.Handler	= 0;
+	pcmcia->conf.Attributes = 0;
+	pcmcia->conf.IntType = INT_MEMORY_AND_IO;
+
+	if (pcmcia_loop_config(pcmcia, dev_conf_check, cs))
+		goto config_failed;
+
+	if (pcmcia_request_irq(pcmcia, &pcmcia->irq))
+		goto config_failed;
+
+	if (pcmcia_request_configuration(pcmcia, &pcmcia->conf))
+		goto config_failed;
+
+	card->dpram.phys = cs->win.Base;
+	card->dpram.size = cs->win.Size;
+
+	if (card->dpram.size != 0x1000) {
+		mod_alert("dpram size 0x%lx mismatch\n", card->dpram.size);
+		goto wrong_dpram;
+	}
+
+	/* Finally, report what we've done */
+	printk(KERN_INFO "[%s] %s: index 0x%02x",
+			THIS_MODULE->name,
+			pcmcia->devname,
+			pcmcia->conf.ConfigIndex);
+	if (pcmcia->conf.Vpp)
+		printk(", Vpp %d.%d", pcmcia->conf.Vpp/10, pcmcia->conf.Vpp%10);
+	if (pcmcia->conf.Attributes & CONF_ENABLE_IRQ) {
+		printk(", irq %d", pcmcia->irq.AssignedIRQ);
+		card->irq.nr = pcmcia->irq.AssignedIRQ;
+	}
+	if (pcmcia->win) {
+		int tmp;
+		const char *p;
+		printk(", mem 0x%08lx-0x%08lx"
+			, card->dpram.phys
+			, card->dpram.phys + card->dpram.size-1);
+		tmp = cs->win.Attributes;
+		while (tmp) {
+			p = lookup_mask(pcmcia_mem_attr, &tmp);
+			if (p)
+				printk(" %s", p);
+		}
+	}
+	printk("\n");
+
+	if (mk_softing(card))
+		goto softing_failed;
+	return 0;
+
+softing_failed:
+wrong_dpram:
+config_failed:
+	kfree(cs);
+no_mem:
+	pcmcia_disable_device(pcmcia);
+	return -ENODEV;
+}
+
+static struct pcmcia_device_id driver_ids[] = {
+	/* softing */
+	PCMCIA_DEVICE_MANF_CARD(0x0168, 0x0001),
+	PCMCIA_DEVICE_MANF_CARD(0x0168, 0x0002),
+	PCMCIA_DEVICE_MANF_CARD(0x0168, 0x0004),
+	PCMCIA_DEVICE_MANF_CARD(0x0168, 0x0005),
+	/* vector , manufacturer? */
+	PCMCIA_DEVICE_MANF_CARD(0x0168, 0x0081),
+	PCMCIA_DEVICE_MANF_CARD(0x0168, 0x0084),
+	PCMCIA_DEVICE_MANF_CARD(0x0168, 0x0085),
+	/* EDIC */
+	PCMCIA_DEVICE_MANF_CARD(0x0168, 0x0102),
+	PCMCIA_DEVICE_MANF_CARD(0x0168, 0x0105),
+	PCMCIA_DEVICE_NULL,
+};
+
+MODULE_DEVICE_TABLE(pcmcia, driver_ids);
+
+static struct pcmcia_driver softing_cs_driver = {
+	.owner		= THIS_MODULE,
+	.drv			= {
+	.name		= "softing_cs",
+	},
+	.probe		= driver_probe,
+	.remove		= driver_remove,
+	.id_table	= driver_ids,
+};
+
+static int __init mod_start(void)
+{
+	mod_trace("");
+	return pcmcia_register_driver(&softing_cs_driver);
+}
+
+static void __exit mod_stop(void)
+{
+	mod_trace("");
+	pcmcia_unregister_driver(&softing_cs_driver);
+}
+
+module_init(mod_start);
+module_exit(mod_stop);
+
+MODULE_DESCRIPTION("softing CANcard driver"
+		", links PCMCIA card to softing driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("softing CANcard2");
+
diff --git a/drivers/net/can/softing/softing_fw.c b/drivers/net/can/softing/softing_fw.c
new file mode 100644
index 000000000000..4d20774c9cb9
--- /dev/null
+++ b/drivers/net/can/softing/softing_fw.c
@@ -0,0 +1,685 @@
+/*
+* drivers/net/can/softing/softing_fw.c
+*
+* Copyright (C) 2008
+*
+* - Kurt Van Dijck, EIA Electronics
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/io.h>
+
+#include "softing.h"
+
+#define fw_dir "softing-4.6/"
+
+const struct can_bittiming_const softing_btr_const = {
+	.tseg1_min = 1,
+	.tseg1_max = 16,
+	.tseg2_min = 1,
+	.tseg2_max = 8,
+	.sjw_max = 4, /* overruled */
+	.brp_min = 1,
+	.brp_max = 32, /* overruled */
+	.brp_inc = 1,
+};
+
+static const struct softing_desc carddescs[] = {
+{
+	.name = "CANcard",
+	.manf = 0x0168, .prod = 0x001,
+	.generation = 1,
+	.freq = 16, .max_brp = 32, .max_sjw = 4,
+	.dpram_size = 0x0800,
+	.boot = {0x0000, 0x000000, fw_dir "bcard.bin",},
+	.load = {0x0120, 0x00f600, fw_dir "ldcard.bin",},
+	.app = {0x0010, 0x0d0000, fw_dir "cancard.bin",},
+}, {
+	.name = "CANcard-NEC",
+	.manf = 0x0168, .prod = 0x002,
+	.generation = 1,
+	.freq = 16, .max_brp = 32, .max_sjw = 4,
+	.dpram_size = 0x0800,
+	.boot = {0x0000, 0x000000, fw_dir "bcard.bin",},
+	.load = {0x0120, 0x00f600, fw_dir "ldcard.bin",},
+	.app = {0x0010, 0x0d0000, fw_dir "cancard.bin",},
+}, {
+	.name = "CANcard-SJA",
+	.manf = 0x0168, .prod = 0x004,
+	.generation = 1,
+	.freq = 20, .max_brp = 32, .max_sjw = 4,
+	.dpram_size = 0x0800,
+	.boot = {0x0000, 0x000000, fw_dir "bcard.bin",},
+	.load = {0x0120, 0x00f600, fw_dir "ldcard.bin",},
+	.app = {0x0010, 0x0d0000, fw_dir "cansja.bin",},
+}, {
+	.name = "CANcard-2",
+	.manf = 0x0168, .prod = 0x005,
+	.generation = 2,
+	.freq = 24, .max_brp = 64, .max_sjw = 4,
+	.dpram_size = 0x0800,
+	.boot = {0x0000, 0x000000, fw_dir "bcard2.bin",},
+	.load = {0x0120, 0x00f600, fw_dir "ldcard2.bin",},
+	.app = {0x0010, 0x0d0000, fw_dir "cancrd2.bin",},
+}, {
+	.name = "Vector-CANcard",
+	.manf = 0x0168, .prod = 0x081,
+	.generation = 1,
+	.freq = 16, .max_brp = 64, .max_sjw = 4,
+	.dpram_size = 0x0800,
+	.boot = {0x0000, 0x000000, fw_dir "bcard.bin",},
+	.load = {0x0120, 0x00f600, fw_dir "ldcard.bin",},
+	.app = {0x0010, 0x0d0000, fw_dir "cancard.bin",},
+}, {
+	.name = "Vector-CANcard-SJA",
+	.manf = 0x0168, .prod = 0x084,
+	.generation = 1,
+	.freq = 20, .max_brp = 32, .max_sjw = 4,
+	.dpram_size = 0x0800,
+	.boot = {0x0000, 0x000000, fw_dir "bcard.bin",},
+	.load = {0x0120, 0x00f600, fw_dir "ldcard.bin",},
+	.app = {0x0010, 0x0d0000, fw_dir "cansja.bin",},
+}, {
+	.name = "Vector-CANcard-2",
+	.manf = 0x0168, .prod = 0x085,
+	.generation = 2,
+	.freq = 24, .max_brp = 64, .max_sjw = 4,
+	.dpram_size = 0x0800,
+	.boot = {0x0000, 0x000000, fw_dir "bcard2.bin",},
+	.load = {0x0120, 0x00f600, fw_dir "ldcard2.bin",},
+	.app = {0x0010, 0x0d0000, fw_dir "cancrd2.bin",},
+}, {
+	.name = "EDICcard-NEC",
+	.manf = 0x0168, .prod = 0x102,
+	.generation = 1,
+	.freq = 16, .max_brp = 64, .max_sjw = 4,
+	.dpram_size = 0x0800,
+	.boot = {0x0000, 0x000000, fw_dir "bcard.bin",},
+	.load = {0x0120, 0x00f600, fw_dir "ldcard.bin",},
+	.app = {0x0010, 0x0d0000, fw_dir "cancard.bin",},
+}, {
+	.name = "EDICcard-2",
+	.manf = 0x0168, .prod = 0x105,
+	.generation = 2,
+	.freq = 24, .max_brp = 64, .max_sjw = 4,
+	.dpram_size = 0x0800,
+	.boot = {0x0000, 0x000000, fw_dir "bcard2.bin",},
+	.load = {0x0120, 0x00f600, fw_dir "ldcard2.bin",},
+	.app = {0x0010, 0x0d0000, fw_dir "cancrd2.bin",},
+	},
+
+/* never tested, but taken from original softing */
+{	.name = "CAN-AC2-104",
+	.manf = 0x0000, .prod = 0x009,
+	.generation = 1,
+	.freq = 25, .max_brp = 64, .max_sjw = 4,
+	.dpram_size = 0x1000,
+	.boot = {0x0000, 0x000000, fw_dir "boot104.bin",},
+	.load = {0x0800, 0x035000, fw_dir "ld104.bin",},
+	.app = {0x0010, 0x120000, fw_dir "canpc104.bin",},
+	},
+{0, 0,},
+};
+
+const struct softing_desc *softing_lookup_desc
+					(unsigned int manf, unsigned int prod)
+{
+	const struct softing_desc *lp = carddescs;
+	for (; lp->name; ++lp) {
+		if ((lp->manf == manf) && (lp->prod == prod))
+			return lp;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(softing_lookup_desc);
+
+int softing_fct_cmd(struct softing *card, int cmd, int vector, const char *msg)
+{
+	int ret;
+	unsigned long stamp;
+	if (vector == RES_OK)
+		vector = RES_NONE;
+	card->dpram.fct->param[0] = cmd;
+	card->dpram.fct->host_access = vector;
+	/* be sure to flush this to the card */
+	wmb();
+	stamp = jiffies;
+	/*wait for card */
+	do {
+		ret = card->dpram.fct->host_access;
+		/* don't have any cached variables */
+		rmb();
+		if (ret == RES_OK) {
+			/*don't read return-value now */
+			ret = card->dpram.fct->returned;
+			if (ret)
+				mod_alert("%s returned %u", msg, ret);
+			return 0;
+		}
+		if ((jiffies - stamp) >= 1 * HZ)
+			break;
+		if (in_interrupt())
+			/* go as fast as possible */
+			continue;
+		/* process context => relax */
+		schedule();
+	} while (!signal_pending(current));
+
+	if (ret == RES_NONE) {
+		mod_alert("%s, no response from card on %u/0x%02x"
+			, msg, cmd, vector);
+		return 1;
+	} else {
+		mod_alert("%s, bad response from card on %u/0x%02x, 0x%04x"
+			, msg, cmd, vector, ret);
+		/*make sure to return something not 0 */
+		return ret ? ret : 1;
+	}
+}
+
+int softing_bootloader_command(struct softing *card
+		, int command, const char *msg)
+{
+	int ret;
+	unsigned long stamp;
+	card->dpram.receipt[0] = RES_NONE;
+	card->dpram.command[0] = command;
+	/* be sure to flush this to the card */
+	wmb();
+	stamp = jiffies;
+	/*wait for card */
+	do {
+		ret = card->dpram.receipt[0];
+		/* don't have any cached variables */
+		rmb();
+		if (ret == RES_OK)
+			return 0;
+		if ((jiffies - stamp) >= (3 * HZ))
+			break;
+		schedule();
+	} while (!signal_pending(current));
+
+	switch (ret) {
+	case RES_NONE:
+		mod_alert("%s: no response from card", msg);
+		break;
+	case RES_NOK:
+		mod_alert("%s: response from card nok", msg);
+		break;
+	case RES_UNKNOWN:
+		mod_alert("%s: command 0x%04x unknown", msg, command);
+		break;
+	default:
+		mod_alert("%s: bad response from card (%u)]", msg, ret);
+		break;
+	}
+	return ret ? ret : 1;
+}
+
+struct fw_hdr {
+	u16 type;
+	u32 addr;
+	u16 len;
+	u16 checksum;
+	const unsigned char *base;
+} __attribute__ ((packed));
+
+static int fw_parse(const unsigned char **pmem, struct fw_hdr *hdr)
+{
+	u16 tmp;
+	const unsigned char *mem;
+	const unsigned char *end;
+	mem = *pmem;
+	hdr->type = (mem[0] << 0) | (mem[1] << 8);
+	hdr->addr = (mem[2] << 0) | (mem[3] << 8)
+		 | (mem[4] << 16) | (mem[5] << 24);
+	hdr->len = (mem[6] << 0) | (mem[7] << 8);
+	hdr->base = &mem[8];
+	hdr->checksum =
+		 (hdr->base[hdr->len] << 0) | (hdr->base[hdr->len + 1] << 8);
+	for (tmp = 0, mem = *pmem, end = &hdr->base[hdr->len]; mem < end; ++mem)
+		tmp += *mem;
+	if (tmp != hdr->checksum)
+		return EINVAL;
+	*pmem += 10 + hdr->len;
+	return 0;
+}
+
+int softing_load_fw(const char *file, struct softing *card,
+			unsigned char *virt, unsigned int size, int offset)
+{
+	const struct firmware *fw;
+	const unsigned char *mem;
+	const unsigned char *end;
+	int ret;
+	u32 start_addr;
+	struct fw_hdr rec;
+	int ok = 0;
+	unsigned char buf[256];
+
+	ret = request_firmware(&fw, file, card->dev);
+	if (ret) {
+		mod_alert("request_firmware(%s) got %i", file, ret);
+		return ret;
+	}
+	mod_trace("%s, firmware(%s) got %u bytes, offset %c0x%04x"
+			, card->id.name, file, (unsigned int)fw->size,
+		  (offset >= 0) ? '+' : '-', abs(offset));
+	/* parse the firmware */
+	mem = fw->data;
+	end = &mem[fw->size];
+	/* look for header record */
+	if (fw_parse(&mem, &rec))
+		goto fw_end;
+	if (rec.type != 0xffff) {
+		mod_alert("firware starts with type 0x%04x", rec.type);
+		goto fw_end;
+	}
+	if (strncmp("Structured Binary Format, Softing GmbH"
+			, rec.base, rec.len)) {
+		mod_info("firware string '%.*s'", rec.len, rec.base);
+		goto fw_end;
+	}
+	ok |= 1;
+	/* ok, we had a header */
+	while (mem < end) {
+		if (fw_parse(&mem, &rec))
+			break;
+		if (rec.type == 3) {
+			/*start address */
+			start_addr = rec.addr;
+			ok |= 2;
+			continue;
+		} else if (rec.type == 1) {
+			/*eof */
+			ok |= 4;
+			goto fw_end;
+		} else if (rec.type != 0) {
+			mod_alert("unknown record type 0x%04x", rec.type);
+			break;
+		}
+
+		if ((rec.addr + rec.len + offset) > size) {
+			mod_alert("firmware out of range (0x%08x / 0x%08x)"
+			, (rec.addr + rec.len + offset), size);
+			goto fw_end;
+		}
+		memcpy_toio(&virt[rec.addr + offset],
+				 rec.base, rec.len);
+		/* be sure to flush caches from IO space */
+		mb();
+		if (rec.len > sizeof(buf)) {
+			mod_info("record is big (%u bytes), not verifying"
+				, rec.len);
+			continue;
+		}
+		/* verify record data */
+		memcpy_fromio(buf, &virt[rec.addr + offset], rec.len);
+		if (!memcmp(buf, rec.base, rec.len))
+			/* is ok */
+			continue;
+		mod_alert("0x%08x:0x%03x at 0x%p failed", rec.addr, rec.len
+			, &virt[rec.addr + offset]);
+		goto fw_end;
+	}
+fw_end:
+	release_firmware(fw);
+	if (0x5 == (ok & 0x5)) {
+		/*got eof & start */
+		return 0;
+	}
+	mod_alert("failed");
+	return EINVAL;
+}
+
+int softing_load_app_fw(const char *file, struct softing *card)
+{
+	const struct firmware *fw;
+	const unsigned char *mem;
+	const unsigned char *end;
+	int ret;
+	struct fw_hdr rec;
+	int ok = 0;
+	u32 start_addr = 0;
+	u16 rx_sum;
+	unsigned int sum;
+	const unsigned char *mem_lp;
+	const unsigned char *mem_end;
+	struct cpy {
+		u32 src;
+		u32 dst;
+		u16 len;
+		u8 do_cs;
+	} __attribute__((packed)) *pcpy =
+		 (struct cpy *)&card->dpram.command[1];
+
+	ret = request_firmware(&fw, file, card->dev);
+	if (ret) {
+		mod_alert("request_firmware(%s) got %i", file, ret);
+		return ret;
+	}
+	mod_trace("%s, firmware(%s) got %lu bytes", card->id.name, file,
+		  (unsigned long)fw->size);
+	/* parse the firmware */
+	mem = fw->data;
+	end = &mem[fw->size];
+	/* look for header record */
+	if (fw_parse(&mem, &rec))
+		goto fw_end;
+	if (rec.type != 0xffff) {
+		mod_alert("firware starts with type 0x%04x", rec.type);
+		goto fw_end;
+	}
+	if (strncmp("Structured Binary Format, Softing GmbH"
+		, rec.base, rec.len)) {
+		mod_info("firware string '%.*s'", rec.len, rec.base);
+		goto fw_end;
+	}
+	ok |= 1;
+	/* ok, we had a header */
+	while (mem < end) {
+		if (fw_parse(&mem, &rec))
+			break;
+
+		if (rec.type == 3) {
+			/*start address */
+			start_addr = rec.addr;
+			ok |= 2;
+			continue;
+		} else if (rec.type == 1) {
+			/*eof */
+			ok |= 4;
+			goto fw_end;
+		} else if (rec.type != 0) {
+			mod_alert("unknown record type 0x%04x", rec.type);
+			break;
+		}
+		/* regualar data */
+		for (sum = 0, mem_lp = rec.base, mem_end = &mem_lp[rec.len];
+			mem_lp < mem_end; ++mem_lp)
+			sum += *mem_lp;
+
+		memcpy_toio(&card->dpram. virt[card->desc->app.offs],
+				 rec.base, rec.len);
+		pcpy->src = card->desc->app.offs + card->desc->app.addr;
+		pcpy->dst = rec.addr;
+		pcpy->len = rec.len;
+		pcpy->do_cs = 1;
+		if (softing_bootloader_command(card, 1, "loading app."))
+			goto fw_end;
+		/*verify checksum */
+		rx_sum = card->dpram.receipt[1];
+		if (rx_sum != (sum & 0xffff)) {
+			mod_alert("SRAM seems to be damaged"
+				", wanted 0x%04x, got 0x%04x", sum, rx_sum);
+			goto fw_end;
+		}
+	}
+fw_end:
+	release_firmware(fw);
+	if (ok == 7) {
+		/*got start, start_addr, & eof */
+		struct cmd {
+			u32 start;
+			u8 autorestart;
+		} *pcmd = (struct cmd *)&card->dpram.command[1];
+		pcmd->start = start_addr;
+		pcmd->autorestart = 1;
+		if (!softing_bootloader_command(card, 3, "start app.")) {
+			mod_trace("%s: card app. run at 0x%06x"
+				, card->id.name, start_addr);
+			return 0;
+		}
+	}
+	mod_alert("failed");
+	return EINVAL;
+}
+
+int softing_reset_chip(struct softing *card)
+{
+	mod_trace("%s", card->id.name);
+	do {
+		/*reset chip */
+		card->dpram.info->reset_rcv_fifo = 0;
+		card->dpram.info->reset = 1;
+		if (!softing_fct_cmd(card, 0, 0, "reset_chip"))
+			break;
+		if (signal_pending(current))
+			goto failed;
+		/*sync */
+		if (softing_fct_cmd(card, 99, 0x55, "sync-a"))
+			goto failed;
+		if (softing_fct_cmd(card, 99, 0xaa, "sync-a"))
+			goto failed;
+	} while (1);
+	card->tx.pending = 0;
+	return 0;
+failed:
+	return -EIO;
+}
+
+int softing_reinit(struct softing *card, int bus0, int bus1)
+{
+	int ret;
+	int restarted_bus = -1;
+	mod_trace("%s", card->id.name);
+	if (!card->fw.up)
+		return -EIO;
+	if (bus0 < 0) {
+		bus0 = (card->bus[0]->netdev->flags & IFF_UP) ? 1 : 0;
+		if (bus0)
+			restarted_bus = 0;
+	} else if (bus1 < 0) {
+		bus1 = (card->bus[1]->netdev->flags & IFF_UP) ? 1 : 0;
+		if (bus1)
+			restarted_bus = 1;
+	}
+	/* collect info */
+	if (card->bus[0]) {
+		card->bus[0]->can.state = CAN_STATE_STOPPED;
+		softing_flush_echo_skb(card->bus[0]);
+	}
+	if (card->bus[1]) {
+		card->bus[1]->can.state = CAN_STATE_STOPPED;
+		softing_flush_echo_skb(card->bus[1]);
+	}
+
+	/* start acting */
+	if (!bus0 && !bus1) {
+		softing_card_irq(card, 0);
+		softing_reset_chip(card);
+		if (card->bus[0])
+			netif_carrier_off(card->bus[0]->netdev);
+		if (card->bus[1])
+			netif_carrier_off(card->bus[1]->netdev);
+		return 0;
+	}
+	ret = softing_reset_chip(card);
+	if (ret) {
+		softing_card_irq(card, 0);
+		return ret;
+	}
+	if (bus0) {
+		/*init chip */
+		card->dpram.fct->param[1] = card->bus[0]->can.bittiming.brp;
+		card->dpram.fct->param[2] = card->bus[0]->can.bittiming.sjw;
+		card->dpram.fct->param[3] =
+			 card->bus[0]->can.bittiming.phase_seg1 +
+			 card->bus[0]->can.bittiming.prop_seg;
+		card->dpram.fct->param[4] =
+			 card->bus[0]->can.bittiming.phase_seg2;
+		card->dpram.fct->param[5] = (card->bus[0]->can.ctrlmode &
+					     CAN_CTRLMODE_3_SAMPLES)?1:0;
+		if (softing_fct_cmd(card, 1, 0, "initialize_chip[0]"))
+			goto failed;
+		/*set mode */
+		card->dpram.fct->param[1] = 0;
+		card->dpram.fct->param[2] = 0;
+		if (softing_fct_cmd(card, 3, 0, "set_mode[0]"))
+			goto failed;
+		/*set filter */
+		card->dpram.fct->param[1] = 0x0000;/*card->bus[0].s.msg; */
+		card->dpram.fct->param[2] = 0x07ff;/*card->bus[0].s.msk; */
+		card->dpram.fct->param[3] = 0x0000;/*card->bus[0].l.msg; */
+		card->dpram.fct->param[4] = 0xffff;/*card->bus[0].l.msk; */
+		card->dpram.fct->param[5] = 0x0000;/*card->bus[0].l.msg >> 16;*/
+		card->dpram.fct->param[6] = 0x1fff;/*card->bus[0].l.msk >> 16;*/
+		if (softing_fct_cmd(card, 7, 0, "set_filter[0]"))
+			goto failed;
+		/*set output control */
+		card->dpram.fct->param[1] = card->bus[0]->output;
+		if (softing_fct_cmd(card, 5, 0, "set_output[0]"))
+			goto failed;
+	}
+	if (bus1) {
+		/*init chip2 */
+		card->dpram.fct->param[1] = card->bus[1]->can.bittiming.brp;
+		card->dpram.fct->param[2] = card->bus[1]->can.bittiming.sjw;
+		card->dpram.fct->param[3] =
+			 card->bus[1]->can.bittiming.phase_seg1 +
+			 card->bus[1]->can.bittiming.prop_seg;
+		card->dpram.fct->param[4] =
+			 card->bus[1]->can.bittiming.phase_seg2;
+		card->dpram.fct->param[5] = (card->bus[1]->can.ctrlmode &
+					     CAN_CTRLMODE_3_SAMPLES)?1:0;
+		if (softing_fct_cmd(card, 2, 0, "initialize_chip[1]"))
+			goto failed;
+		/*set mode2 */
+		card->dpram.fct->param[1] = 0;
+		card->dpram.fct->param[2] = 0;
+		if (softing_fct_cmd(card, 4, 0, "set_mode[1]"))
+			goto failed;
+		/*set filter2 */
+		card->dpram.fct->param[1] = 0x0000;/*card->bus[1].s.msg; */
+		card->dpram.fct->param[2] = 0x07ff;/*card->bus[1].s.msk; */
+		card->dpram.fct->param[3] = 0x0000;/*card->bus[1].l.msg; */
+		card->dpram.fct->param[4] = 0xffff;/*card->bus[1].l.msk; */
+		card->dpram.fct->param[5] = 0x0000;/*card->bus[1].l.msg >> 16;*/
+		card->dpram.fct->param[6] = 0x1fff;/*card->bus[1].l.msk >> 16;*/
+		if (softing_fct_cmd(card, 8, 0, "set_filter[1]"))
+			goto failed;
+		/*set output control2 */
+		card->dpram.fct->param[1] = card->bus[1]->output;
+		if (softing_fct_cmd(card, 6, 0, "set_output[1]"))
+			goto failed;
+	}
+	/*set interrupt */
+	/*enable_error_frame */
+	if (softing_fct_cmd(card, 51, 0, "enable_error_frame"))
+		goto failed;
+	/*initialize interface */
+	card->dpram.fct->param[1] = 1;
+	card->dpram.fct->param[2] = 1;
+	card->dpram.fct->param[3] = 1;
+	card->dpram.fct->param[4] = 1;
+	card->dpram.fct->param[5] = 1;
+	card->dpram.fct->param[6] = 1;
+	card->dpram.fct->param[7] = 1;
+	card->dpram.fct->param[8] = 1;
+	card->dpram.fct->param[9] = 1;
+	card->dpram.fct->param[10] = 1;
+	if (softing_fct_cmd(card, 17, 0, "initialize_interface"))
+		goto failed;
+	/*enable_fifo */
+	if (softing_fct_cmd(card, 36, 0, "enable_fifo"))
+		goto failed;
+	/*enable fifo tx ack */
+	if (softing_fct_cmd(card, 13, 0, "fifo_tx_ack[0]"))
+		goto failed;
+	/*enable fifo tx ack2 */
+	if (softing_fct_cmd(card, 14, 0, "fifo_tx_ack[1]"))
+		goto failed;
+	/*enable timestamps */
+	/*is default, no code found */
+	/*start_chip */
+	if (softing_fct_cmd(card, 11, 0, "start_chip"))
+		goto failed;
+	card->dpram.info->bus_state = 0;
+	card->dpram.info->bus_state2 = 0;
+	mod_info("ok for %s, %s/%s\n", card->bus[0]->netdev->name,
+		 card->bus[1]->netdev->name, card->id.name);
+	if (card->desc->generation < 2) {
+		card->dpram.irq->to_host = 0;
+		/* flush the DPRAM caches */
+		wmb();
+	}
+	/*run once */
+	/*the bottom halve will start flushing the tx-queue too */
+	tasklet_schedule(&card->irq.bh);
+
+	ret = softing_card_irq(card, 1);
+	if (ret)
+		goto failed;
+
+	/*TODO: generate RESTARTED messages */
+
+	if (card->bus[0] && bus0) {
+		card->bus[0]->can.state = CAN_STATE_ACTIVE;
+		netif_carrier_on(card->bus[0]->netdev);
+	}
+	if (card->bus[1] && bus1) {
+		card->bus[1]->can.state = CAN_STATE_ACTIVE;
+		netif_carrier_on(card->bus[1]->netdev);
+	}
+	return 0;
+failed:
+	softing_card_irq(card, 0);
+	softing_reset_chip(card);
+	if (card->bus[0])
+		netif_carrier_off(card->bus[0]->netdev);
+	if (card->bus[1])
+		netif_carrier_off(card->bus[1]->netdev);
+	return -EIO;
+}
+
+
+int softing_default_output(struct softing *card, struct softing_priv *priv)
+{
+	switch (priv->chip) {
+	case 1000:
+		if (card->desc->generation < 2)
+			return 0xfb;
+		return 0xfa;
+	case 5:
+		return 0x60;
+	default:
+		return 0x40;
+	}
+}
+
+u32 softing_time2usec(struct softing *card, u32 raw)
+{
+	/*TODO : don't loose higher order bits in computation */
+	switch (card->desc->freq) {
+	case 20:
+		return raw * 4 / 5;
+	case 24:
+		return raw * 2 / 3;
+	case 25:
+		return raw * 16 / 25;
+	case 0:
+	case 16:
+	default:
+		return raw;
+	}
+}
+
+
diff --git a/drivers/net/can/softing/softing_main.c b/drivers/net/can/softing/softing_main.c
new file mode 100644
index 000000000000..955f9c85a3a4
--- /dev/null
+++ b/drivers/net/can/softing/softing_main.c
@@ -0,0 +1,1058 @@
+/*
+* drivers/net/can/softing/softing_main.c
+*
+* Copyright (C) 2008
+*
+* - Kurt Van Dijck, EIA Electronics
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/io.h>
+
+#include "softing.h"
+
+/* this is the worst thing on the softing API
+ * 2 busses are driven together, I don't know how
+ * to recover a single of them.
+ * Therefore, when one bus is modified, the other
+ * is flushed too
+ */
+void softing_flush_echo_skb(struct softing_priv *priv)
+{
+	can_close_cleanup(priv->netdev);
+	priv->tx.pending = 0;
+	priv->tx.echo_put = 0;
+	priv->tx.echo_get = 0;
+}
+
+/*softing_unlocked_tx_run:*/
+/*trigger the tx queue-ing*/
+/*no locks are grabbed, so be sure to have the spin spinlock*/
+static int netdev_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct softing_priv *priv = (struct softing_priv *)netdev_priv(dev);
+	struct softing *card = priv->card;
+	int ret;
+	int bhlock;
+	u8 *ptr;
+	u8 cmd;
+	unsigned int fifo_wr;
+	struct can_frame msg;
+
+	ret = -ENOTTY;
+	if (in_interrupt()) {
+		bhlock = 0;
+		spin_lock(&card->spin);
+	} else {
+		bhlock = 1;
+		spin_lock_bh(&card->spin);
+	}
+	if (!card->fw.up) {
+		ret = -EIO;
+		goto xmit_done;
+	}
+	if (netif_carrier_ok(priv->netdev) <= 0) {
+		ret = -EBADF;
+		goto xmit_done;
+	}
+	if (card->tx.pending >= TXMAX) {
+		ret = -EBUSY;
+		goto xmit_done;
+	}
+	if (priv->tx.pending >= CAN_ECHO_SKB_MAX) {
+		ret = -EBUSY;
+		goto xmit_done;
+	}
+	fifo_wr = card->dpram.tx->wr;
+	if (fifo_wr == card->dpram.tx->rd) {
+		/*fifo full */
+		ret = -EAGAIN;
+		goto xmit_done;
+	}
+	memcpy(&msg, skb->data, sizeof(msg));
+	ptr = &card->dpram.tx->fifo[fifo_wr][0];
+	cmd = CMD_TX;
+	if (msg.can_id & CAN_RTR_FLAG)
+		cmd |= CMD_RTR;
+	if (msg.can_id & CAN_EFF_FLAG)
+		cmd |= CMD_XTD;
+	if (priv->index)
+		cmd |= CMD_BUS2;
+	*ptr++ = cmd;
+	*ptr++ = msg.can_dlc;
+	*ptr++ = (msg.can_id >> 0);
+	*ptr++ = (msg.can_id >> 8);
+	if (msg.can_id & CAN_EFF_FLAG) {
+		*ptr++ = (msg.can_id >> 16);
+		*ptr++ = (msg.can_id >> 24);
+	} else {
+		/*increment 1, not 2 as you might think */
+		ptr += 1;
+	}
+	if (!(msg.can_id & CAN_RTR_FLAG))
+		memcpy_toio(ptr, &msg.data[0], msg.can_dlc);
+	if (++fifo_wr >=
+		 sizeof(card->dpram.tx->fifo) /
+		 sizeof(card->dpram.tx->fifo[0]))
+		fifo_wr = 0;
+	card->dpram.tx->wr = fifo_wr;
+	ret = 0;
+	++card->tx.pending;
+	++priv->tx.pending;
+	can_put_echo_skb(skb, dev, priv->tx.echo_put);
+	++priv->tx.echo_put;
+	if (priv->tx.echo_put >= CAN_ECHO_SKB_MAX)
+		priv->tx.echo_put = 0;
+	/* clear pointer, so don't erase later */
+	skb = 0;
+xmit_done:
+	if (bhlock)
+		spin_unlock_bh(&card->spin);
+	else
+		spin_unlock(&card->spin);
+	if (card->tx.pending >= TXMAX) {
+		struct softing_priv *bus;
+		int j;
+		for (j = 0; j < card->nbus; ++j) {
+			bus = card->bus[j];
+			if (!bus)
+				continue;
+			netif_stop_queue(bus->netdev);
+		}
+	}
+
+	/* free skb, if not handled by the driver */
+	if (skb)
+		dev_kfree_skb(skb);
+	return ret;
+}
+
+static int softing_dev_svc_once(struct softing *card)
+{
+	int j;
+	struct softing_priv *bus;
+	struct sk_buff *skb;
+	struct can_frame msg;
+
+	unsigned int fifo_rd;
+	unsigned int cnt = 0;
+	struct net_device_stats *stats;
+
+	memset(&msg, 0, sizeof(msg));
+	if (card->dpram.rx->lost_msg) {
+		/*reset condition */
+		card->dpram.rx->lost_msg = 0;
+		/* prepare msg */
+		msg.can_id = CAN_ERR_FLAG | CAN_ERR_CRTL;
+		msg.can_dlc = CAN_ERR_DLC;
+		msg.data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+		/*service to both busses, we don't know which one generated */
+		for (j = 0; j < card->nbus; ++j) {
+			bus = card->bus[j];
+			if (!bus)
+				continue;
+			if (!netif_carrier_ok(bus->netdev))
+				continue;
+			++bus->can.can_stats.data_overrun;
+			skb = dev_alloc_skb(sizeof(msg));
+			if (!skb)
+				return -ENOMEM;
+			skb->dev = bus->netdev;
+			skb->protocol = htons(ETH_P_CAN);
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
+			memcpy(skb_put(skb, sizeof(msg)), &msg, sizeof(msg));
+			if (netif_rx(skb))
+				dev_kfree_skb_irq(skb);
+		}
+		memset(&msg, 0, sizeof(msg));
+		++cnt;
+	}
+
+	fifo_rd = card->dpram.rx->rd;
+	if (++fifo_rd >=
+		 sizeof(card->dpram.rx->fifo) / sizeof(card->dpram.rx->fifo[0]))
+		fifo_rd = 0;
+	if (card->dpram.rx->wr != fifo_rd) {
+		u8 *ptr;
+		u32 tmp;
+		u8 cmd;
+		int do_skb;
+
+		ptr = &card->dpram.rx->fifo[fifo_rd][0];
+
+		cmd = *ptr++;
+		if (cmd == 0xff) {
+			/*not quite usefull, probably the card has got out */
+			mod_alert("got cmd 0x%02x, I suspect the card is lost"
+				, cmd);
+		}
+		/*mod_trace("0x%02x", cmd);*/
+		bus = card->bus[0];
+		if (cmd & CMD_BUS2)
+			bus = card->bus[1];
+
+		stats = &bus->netdev->stats;
+		if (cmd & CMD_ERR) {
+			u8 can_state;
+			u8 state;
+			state = *ptr++;
+
+			msg.can_id = CAN_ERR_FLAG;
+			msg.can_dlc = CAN_ERR_DLC;
+
+			if (state & 0x80) {
+				can_state = CAN_STATE_BUS_OFF;
+				msg.can_id |= CAN_ERR_BUSOFF;
+				state = 2;
+			} else if (state & 0x60) {
+				can_state = CAN_STATE_BUS_PASSIVE;
+				msg.can_id |= CAN_ERR_BUSERROR;
+				state = 1;
+			} else {
+				can_state = CAN_STATE_ACTIVE;
+				state = 0;
+				do_skb = 0;
+			}
+			/*update DPRAM */
+			if (!bus->index)
+				card->dpram.info->bus_state = state;
+			else
+				card->dpram.info->bus_state2 = state;
+			/*timestamp */
+			tmp =	 (ptr[0] <<  0)
+				|(ptr[1] <<  8)
+				|(ptr[2] << 16)
+				|(ptr[3] << 24);
+			ptr += 4;
+			/*msg.time = */ softing_time2usec(card, tmp);
+			/*trigger dual port RAM */
+			mb();
+			card->dpram.rx->rd = fifo_rd;
+			/*update internal status */
+			if (can_state != bus->can.state) {
+				bus->can.state = can_state;
+				if (can_state == 1)
+					bus->can.can_stats.error_passive += 1;
+			}
+			bus->can.can_stats.bus_error += 1;
+
+			/*trigger socketcan */
+			if (state == 2) {
+				/* this calls can_close_cleanup() */
+				softing_flush_echo_skb(bus);
+				can_bus_off(bus->netdev);
+				netif_stop_queue(bus->netdev);
+			}
+			if ((state == CAN_STATE_BUS_OFF)
+				 || (state == CAN_STATE_BUS_PASSIVE)) {
+				skb = dev_alloc_skb(sizeof(msg));
+				if (!skb)
+					return -ENOMEM;
+				skb->dev = bus->netdev;
+				skb->protocol = htons(ETH_P_CAN);
+				skb->ip_summed = CHECKSUM_UNNECESSARY;
+				memcpy(skb_put(skb, sizeof(msg)), &msg,
+						 sizeof(msg));
+				if (netif_rx(skb))
+					dev_kfree_skb_irq(skb);
+			}
+		} else {
+			if (cmd & CMD_RTR)
+				msg.can_id |= CAN_RTR_FLAG;
+			/* acknowledge, was tx msg
+			 * no real tx flag to set
+			if (cmd & CMD_ACK) {
+			}
+			 */
+			msg.can_dlc = *ptr++;
+			if (msg.can_dlc > 8)
+				msg.can_dlc = 8;
+			if (cmd & CMD_XTD) {
+				msg.can_id |= CAN_EFF_FLAG;
+				msg.can_id |=
+						(ptr[0] << 0)
+					 | (ptr[1] << 8)
+					 | (ptr[2] << 16)
+					 | (ptr[3] << 24);
+				ptr += 4;
+			} else {
+				msg.can_id |= (ptr[0] << 0) | (ptr[1] << 8);
+				ptr += 2;
+			}
+			tmp = (ptr[0] << 0)
+				 | (ptr[1] << 8)
+				 | (ptr[2] << 16)
+				 | (ptr[3] << 24);
+			ptr += 4;
+			/*msg.time = */ softing_time2usec(card, tmp);
+			memcpy_fromio(&msg.data[0], ptr, 8);
+			ptr += 8;
+			/*trigger dual port RAM */
+			mb();
+			card->dpram.rx->rd = fifo_rd;
+			/*update socket */
+			if (cmd & CMD_ACK) {
+				can_get_echo_skb(bus->netdev, bus->tx.echo_get);
+				++bus->tx.echo_get;
+				if (bus->tx.echo_get >= CAN_ECHO_SKB_MAX)
+					bus->tx.echo_get = 0;
+				if (bus->tx.pending)
+					--bus->tx.pending;
+				if (card->tx.pending)
+					--card->tx.pending;
+				stats->tx_packets += 1;
+				stats->tx_bytes += msg.can_dlc;
+			} else {
+				stats->rx_packets += 1;
+				stats->rx_bytes += msg.can_dlc;
+				bus->netdev->last_rx = jiffies;
+				skb = dev_alloc_skb(sizeof(msg));
+				if (skb) {
+					skb->dev = bus->netdev;
+					skb->protocol = htons(ETH_P_CAN);
+					skb->ip_summed = CHECKSUM_UNNECESSARY;
+					memcpy(skb_put(skb, sizeof(msg)), &msg,
+							 sizeof(msg));
+					if (netif_rx(skb))
+						dev_kfree_skb_irq(skb);
+				}
+			}
+		}
+		++cnt;
+	}
+	return cnt;
+}
+
+static void softing_dev_svc(unsigned long param)
+{
+	struct softing *card = (struct softing *)param;
+	struct softing_priv *bus;
+	int j;
+	int offset;
+
+	spin_lock(&card->spin);
+	while (softing_dev_svc_once(card) > 0)
+		++card->irq.svc_count;
+	/*resume tx queue's */
+	offset = card->tx.last_bus;
+	for (j = 0; j < card->nbus; ++j) {
+		if (card->tx.pending >= TXMAX)
+			break;
+		bus = card->bus[(j + offset) % card->nbus];
+		if (netif_carrier_ok(bus->netdev))
+			netif_wake_queue(bus->netdev);
+	}
+	spin_unlock(&card->spin);
+}
+
+static void card_seems_down(struct softing *card)
+{
+	/* free interrupt, but probably
+	 * in wrong (interrupt) context
+	if (card->irq.requested) {
+		free_irq(card->irq.nr, card);
+		card->irq.requested = 0;
+		card->fw.up = 0;
+	}
+	*/
+	mod_alert("I think the card is vanished");
+}
+
+static
+irqreturn_t dev_interrupt_shared(int irq, void *dev_id)
+{
+	struct softing *card = (struct softing *)dev_id;
+	unsigned char ir;
+	ir = card->dpram.virt[0xe02];
+	card->dpram.virt[0xe02] = 0;
+	if (card->dpram.rx->rd == 0xffff) {
+		card_seems_down(card);
+		return IRQ_NONE;
+	}
+	if (ir == 1) {
+		tasklet_schedule(&card->irq.bh);
+		return IRQ_HANDLED;
+	} else if (ir == 0x10) {
+		return IRQ_NONE;
+	} else {
+		return IRQ_NONE;
+	}
+}
+
+static
+irqreturn_t dev_interrupt_nshared(int irq, void *dev_id)
+{
+	struct softing *card = (struct softing *)dev_id;
+	unsigned char irq_host;
+	irq_host = card->dpram.irq->to_host;
+	/* make sure we have a copy, before clearing the variable in DPRAM */
+	rmb();
+	card->dpram.irq->to_host = 0;
+	/* make sure we cleared it */
+	wmb();
+	mod_trace("0x%02x", irq_host);
+	if (card->dpram.rx->rd == 0xffff) {
+		card_seems_down(card);
+		return IRQ_NONE;
+	}
+	tasklet_schedule(&card->irq.bh);
+	return IRQ_HANDLED;
+}
+
+static int netdev_open(struct net_device *ndev)
+{
+	struct softing_priv *priv = netdev_priv(ndev);
+	struct softing *card = priv->card;
+	int fw;
+	int ret;
+
+	mod_trace("%s", ndev->name);
+	/* determine and set bittime */
+	ret = can_set_bittiming(ndev);
+	if (ret)
+		return ret;
+	if (mutex_lock_interruptible(&card->fw.lock))
+		return -ERESTARTSYS;
+	fw = card->fw.up;
+	if (fw)
+		softing_reinit(card
+			, (card->bus[0] == priv) ? 1 : -1
+			, (card->bus[1] == priv) ? 1 : -1);
+	mutex_unlock(&card->fw.lock);
+	if (!fw)
+		return -EIO;
+	netif_start_queue(ndev);
+	return 0;
+}
+
+static int netdev_stop(struct net_device *ndev)
+{
+	struct softing_priv *priv = netdev_priv(ndev);
+	struct softing *card = priv->card;
+	int fw;
+
+	mod_trace("%s", ndev->name);
+	netif_stop_queue(ndev);
+	netif_carrier_off(ndev);
+	softing_flush_echo_skb(priv);
+	can_close_cleanup(ndev);
+	if (mutex_lock_interruptible(&card->fw.lock))
+		return -ERESTARTSYS;
+	fw = card->fw.up;
+	if (fw)
+		softing_reinit(card
+			, (card->bus[0] == priv) ? 0 : -1
+			, (card->bus[1] == priv) ? 0 : -1);
+	mutex_unlock(&card->fw.lock);
+	if (!fw)
+		return -EIO;
+	return 0;
+}
+
+static int candev_get_state(struct net_device *ndev, enum can_state *state)
+{
+	struct softing_priv *priv = netdev_priv(ndev);
+	mod_trace("%s", ndev->name);
+	if (priv->netdev->flags & IFF_UP)
+		*state = CAN_STATE_STOPPED;
+	else if (priv->can.state == CAN_STATE_STOPPED)
+		*state = CAN_STATE_STOPPED;
+	else
+		*state = CAN_STATE_ACTIVE;
+	return 0;
+}
+
+static int candev_set_mode(struct net_device *ndev, enum can_mode mode)
+{
+	struct softing_priv *priv = netdev_priv(ndev);
+	struct softing *card = priv->card;
+	mod_trace("%s %u", ndev->name, mode);
+	switch (mode) {
+	case CAN_MODE_START:
+		/*recovery from busoff? */
+		if (mutex_lock_interruptible(&card->fw.lock))
+			return -ERESTARTSYS;
+		softing_reinit(card, -1, -1);
+		mutex_unlock(&card->fw.lock);
+		break;
+	case CAN_MODE_STOP:
+	case CAN_MODE_SLEEP:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+/*assume the card->lock is held*/
+
+int softing_card_irq(struct softing *card, int enable)
+{
+	int ret;
+	if (!enable) {
+		if (card->irq.requested && card->irq.nr) {
+			free_irq(card->irq.nr, card);
+			card->irq.requested = 0;
+		}
+		return 0;
+	}
+	if (!card->irq.requested && (card->irq.nr)) {
+		irqreturn_t(*fn) (int, void *);
+		unsigned int flags;
+		flags = IRQF_DISABLED | IRQF_SHARED;/*| IRQF_TRIGGER_LOW; */
+		fn = dev_interrupt_nshared;
+		if (card->desc->generation >= 2)
+			fn = dev_interrupt_shared;
+		ret = request_irq(card->irq.nr, fn, flags, card->id.name, card);
+		if (ret) {
+			mod_alert("%s, request_irq(%u) failed"
+				, card->id.name, card->irq.nr
+				);
+			return ret;
+		}
+		card->irq.requested = 1;
+	}
+	return 0;
+}
+
+static void shutdown_card(struct softing *card)
+{
+	int fw_up = 0;
+	mod_trace("%s", card->id.name);
+	if (mutex_lock_interruptible(&card->fw.lock))
+		/* return -ERESTARTSYS*/;
+	fw_up = card->fw.up;
+	card->fw.up = 0;
+
+	if (card->irq.requested && card->irq.nr) {
+		free_irq(card->irq.nr, card);
+		card->irq.requested = 0;
+	}
+	if (fw_up) {
+		if (card->fn.enable_irq)
+			card->fn.enable_irq(card, 0);
+		if (card->fn.reset)
+			card->fn.reset(card, 1);
+	}
+	mutex_unlock(&card->fw.lock);
+	tasklet_kill(&card->irq.bh);
+}
+
+static int boot_card(struct softing *card)
+{
+	mod_trace("%s", card->id.name);
+	if (mutex_lock_interruptible(&card->fw.lock))
+		return -ERESTARTSYS;
+	if (card->fw.up) {
+		mutex_unlock(&card->fw.lock);
+		return 0;
+	}
+	/*reset board */
+
+	if (card->fn.enable_irq)
+		card->fn.enable_irq(card, 1);
+	/*boot card */
+	if (card->fn.reset)
+		card->fn.reset(card, 1);
+	/*test dp ram */
+	if (card->dpram.virt) {
+		unsigned char *lp;
+		static const unsigned char stream[]
+		= { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, };
+		unsigned char back[sizeof(stream)];
+		for (lp = card->dpram.virt;
+			  &lp[sizeof(stream)] <= card->dpram.end;
+			  lp += sizeof(stream)) {
+			memcpy_toio(lp, stream, sizeof(stream));
+			/* flush IO cache */
+			mb();
+			memcpy_fromio(back, lp, sizeof(stream));
+
+			if (memcmp(back, stream, sizeof(stream))) {
+				char line[3 * sizeof(stream)
+					/ sizeof(stream[0]) + 1];
+				char *pline = line;
+				unsigned char *addr = lp;
+				for (lp = back; lp < &back[sizeof(stream)
+						/ sizeof(stream[0])]; ++lp)
+					pline += sprintf(pline, " %02x", *lp);
+
+				mod_alert("write to dpram failed at 0x%p, %s"
+					, addr, line);
+				goto open_failed;
+			}
+		}
+		/*fill dpram with 0x55 */
+		/*for (lp = card->dpram.virt; lp <= card->dpram.end; ++lp) {
+		 *lp = 0x55;
+		 }*/
+		wmb();
+	} else {
+		goto open_failed;
+	}
+	/*load boot firmware */
+	if (softing_load_fw(card->desc->boot.fw, card, card->dpram.virt,
+				 card->dpram.size,
+				 card->desc->boot.offs -
+				 card->desc->boot.addr))
+		goto open_failed;
+	/*load load firmware */
+	if (softing_load_fw(card->desc->load.fw, card, card->dpram.virt,
+				 card->dpram.size,
+				 card->desc->load.offs -
+				 card->desc->load.addr))
+		goto open_failed;
+
+	if (card->fn.reset)
+		card->fn.reset(card, 0);
+	if (softing_bootloader_command(card, 0, "card boot"))
+		goto open_failed;
+	if (softing_load_app_fw(card->desc->app.fw, card))
+		goto open_failed;
+	/*reset chip */
+	card->dpram.info->reset_rcv_fifo = 0;
+	card->dpram.info->reset = 1;
+	/*sync */
+	if (softing_fct_cmd(card, 99, 0x55, "sync-a"))
+		goto open_failed;
+	if (softing_fct_cmd(card, 99, 0xaa, "sync-a"))
+		goto open_failed;
+	/*reset chip */
+	if (softing_fct_cmd(card, 0, 0, "reset_chip"))
+		goto open_failed;
+	/*get_serial */
+	if (softing_fct_cmd(card, 43, 0, "get_serial_number"))
+		goto open_failed;
+	card->id.serial =
+		 (u16) card->dpram.fct->param[1] +
+		 (((u16) card->dpram.fct->param[2]) << 16);
+	/*get_version */
+	if (softing_fct_cmd(card, 12, 0, "get_version"))
+		goto open_failed;
+	card->id.fw = (u16) card->dpram.fct->param[1];
+	card->id.hw = (u16) card->dpram.fct->param[2];
+	card->id.lic = (u16) card->dpram.fct->param[3];
+	card->id.chip[0] = (u16) card->dpram.fct->param[4];
+	card->id.chip[1] = (u16) card->dpram.fct->param[5];
+
+	mod_info("%s, card booted, "
+			"serial %u, fw %u, hw %u, lic %u, chip (%u,%u)",
+		  card->id.name, card->id.serial, card->id.fw, card->id.hw,
+		  card->id.lic, card->id.chip[0], card->id.chip[1]);
+
+	card->fw.up = 1;
+	mutex_unlock(&card->fw.lock);
+	return 0;
+open_failed:
+	card->fw.up = 0;
+	if (card->fn.enable_irq)
+		card->fn.enable_irq(card, 0);
+	if (card->fn.reset)
+		card->fn.reset(card, 1);
+	mutex_unlock(&card->fw.lock);
+	return EINVAL;
+}
+
+/*sysfs stuff*/
+
+/* Because the struct softing may be used by pcmcia devices
+ * as well as pci devices, * we have no clue how to get
+ * from a struct device * towards the struct softing *.
+ * It may go over a pci_device->priv or over a pcmcia_device->priv.
+ * Therefore, provide the struct softing pointer within the attribute.
+ * Then we don't need driver/bus specific things in these attributes
+ */
+struct softing_attribute {
+	struct device_attribute dev;
+	ssize_t (*show) (struct softing *card, char *buf);
+	ssize_t (*store)(struct softing *card, const char *buf, size_t count);
+	struct softing *card;
+};
+
+static ssize_t rd_card_attr(struct device *dev, struct device_attribute *attr
+		, char *buf) {
+	struct softing_attribute *cattr
+		= container_of(attr, struct softing_attribute, dev);
+	return cattr->show ? cattr->show(cattr->card, buf) : 0;
+}
+static ssize_t wr_card_attr(struct device *dev, struct device_attribute *attr
+		, const char *buf, size_t count) {
+	struct softing_attribute *cattr
+		= container_of(attr, struct softing_attribute, dev);
+	return cattr->store ? cattr->store(cattr->card, buf, count) : 0;
+}
+
+#define declare_attr(_name, _mode, _show, _store) { \
+	.dev = { \
+		.attr = { \
+			.name = __stringify(_name), \
+			.mode = _mode, \
+		}, \
+		.show = rd_card_attr, \
+		.store = wr_card_attr, \
+	}, \
+	.show =	_show, \
+	.store = _store, \
+}
+
+#define CARD_SHOW(name, member) \
+static ssize_t show_##name(struct softing *card, char *buf) { \
+	return sprintf(buf, "%u\n", card->member); \
+}
+CARD_SHOW(serial	, id.serial);
+CARD_SHOW(firmware	, id.fw);
+CARD_SHOW(hardware	, id.hw);
+CARD_SHOW(license	, id.lic);
+CARD_SHOW(freq		, id.freq);
+CARD_SHOW(txpending	, tx.pending);
+
+static const struct softing_attribute card_attr_proto [] = {
+	declare_attr(serial	, 0444, show_serial	, 0),
+	declare_attr(firmware	, 0444, show_firmware	, 0),
+	declare_attr(hardware	, 0444, show_hardware	, 0),
+	declare_attr(license	, 0444, show_license	, 0),
+	declare_attr(freq	, 0444, show_freq	, 0),
+	declare_attr(txpending	, 0644, show_txpending	, 0),
+};
+
+static int mk_card_sysfs(struct softing *card)
+{
+	int size;
+	int j;
+
+	size = sizeof(card_attr_proto)/sizeof(card_attr_proto[0]);
+	card->attr = kmalloc((size+1)*sizeof(card->attr[0]), GFP_KERNEL);
+	if (!card->attr)
+		goto attr_mem_failed;
+	memcpy(card->attr, card_attr_proto, size * sizeof(card->attr[0]));
+	memset(&card->attr[size], 0, sizeof(card->attr[0]));
+
+	card->grp  = kmalloc((size+1)*sizeof(card->grp [0]), GFP_KERNEL);
+	if (!card->grp)
+		goto grp_mem_failed;
+
+	for (j = 0; j < size; ++j) {
+		card->attr[j].card = card;
+		card->grp[j] = &card->attr[j].dev.attr;
+		if (!card->attr[j].show)
+			card->attr[j].dev.attr.mode &= ~(S_IRUGO);
+		if (!card->attr[j].store)
+			card->attr[j].dev.attr.mode &= ~(S_IWUGO);
+	}
+	card->grp[size] = 0;
+	card->sysfs.name	= "softing";
+	card->sysfs.attrs = card->grp;
+	if (sysfs_create_group(&card->dev->kobj, &card->sysfs) < 0)
+		goto sysfs_failed;
+
+	return 0;
+
+sysfs_failed:
+	kfree(card->grp);
+grp_mem_failed:
+	kfree(card->attr);
+attr_mem_failed:
+	return -1;
+}
+static void rm_card_sysfs(struct softing *card)
+{
+	sysfs_remove_group(&card->dev->kobj, &card->sysfs);
+	kfree(card->grp);
+	kfree(card->attr);
+}
+
+static ssize_t show_chip(struct device *dev
+		, struct device_attribute *attr, char *buf)
+{
+	struct net_device *ndev = to_net_dev(dev);
+	struct softing_priv *priv = netdev2softing(ndev);
+	return sprintf(buf, "%i\n", priv->chip);
+}
+
+static ssize_t show_output(struct device *dev
+		, struct device_attribute *attr, char *buf)
+{
+	struct net_device *ndev = to_net_dev(dev);
+	struct softing_priv *priv = netdev2softing(ndev);
+	return sprintf(buf, "0x%02x\n", priv->output);
+}
+
+static ssize_t store_output(struct device *dev
+		, struct device_attribute *attr
+		, const char *buf, size_t count)
+{
+	struct net_device *ndev = to_net_dev(dev);
+	struct softing_priv *priv = netdev2softing(ndev);
+	struct softing *card = priv->card;
+
+	u8 v = simple_strtoul(buf, NULL, 10) & 0xFFU;
+
+	if (mutex_lock_interruptible(&card->fw.lock))
+		return -ERESTARTSYS;
+	if (ndev->flags & IFF_UP) {
+		int j;
+		/* we will need a restart */
+		for (j = 0; j < card->nbus; ++j) {
+			if (j == priv->index)
+				/* me, myself & I */
+				continue;
+			if (card->bus[j]->netdev->flags & IFF_UP) {
+				mutex_unlock(&card->fw.lock);
+				return -EBUSY;
+			}
+		}
+		priv->output = v;
+		softing_reinit(card, -1, -1);
+	} else {
+		priv->output = v;
+	}
+	mutex_unlock(&card->fw.lock);
+	return count;
+}
+/* TODO
+ * the latest softing cards support sleep mode too
+ */
+
+static const DEVICE_ATTR(chip, S_IRUGO, show_chip, 0);
+static const DEVICE_ATTR(output, S_IRUGO | S_IWUSR, show_output, store_output);
+
+static const struct attribute *const netdev_sysfs_entries [] = {
+	&dev_attr_chip		.attr,
+	&dev_attr_output	.attr,
+	0,
+};
+static const struct attribute_group netdev_sysfs = {
+	.name  = 0,
+	.attrs = (struct attribute **)netdev_sysfs_entries,
+};
+
+static int mk_netdev_sysfs(struct softing_priv *priv)
+{
+	if (!priv->netdev->dev.kobj.sd) {
+		mod_alert("sysfs_create_group would fail");
+		return ENODEV;
+	}
+	return sysfs_create_group(&priv->netdev->dev.kobj, &netdev_sysfs);
+}
+static void rm_netdev_sysfs(struct softing_priv *priv)
+{
+	sysfs_remove_group(&priv->netdev->dev.kobj, &netdev_sysfs);
+}
+
+static struct softing_priv *mk_netdev(struct softing *card, u16 chip_id)
+{
+	struct net_device *ndev;
+	struct softing_priv *priv;
+
+	ndev = alloc_candev(sizeof(*priv));
+	if (!ndev) {
+		mod_alert("alloc_candev failed");
+		return 0;
+	}
+	priv = netdev_priv(ndev);
+	priv->netdev		= ndev;
+	priv->card		= card;
+	memcpy(&priv->btr_const, &softing_btr_const, sizeof(priv->btr_const));
+	priv->btr_const.brp_max = card->desc->max_brp;
+	priv->btr_const.sjw_max = card->desc->max_sjw;
+	priv->can.bittiming_const = &priv->btr_const;
+	priv->can.bittiming.clock = 8000000;
+	priv->chip		= chip_id;
+	priv->output = softing_default_output(card, priv);
+	SET_NETDEV_DEV(ndev, card->dev);
+
+	ndev->flags |= IFF_ECHO;
+	ndev->open		= netdev_open;
+	ndev->stop		= netdev_stop;
+	ndev->hard_start_xmit	= netdev_start_xmit;
+	priv->can.do_get_state	= candev_get_state;
+	priv->can.do_set_mode	= candev_set_mode;
+
+	return priv;
+}
+
+static void rm_netdev(struct softing_priv *priv)
+{
+	free_candev(priv->netdev);
+}
+
+static int reg_netdev(struct softing_priv *priv)
+{
+	int ret;
+	netif_carrier_off(priv->netdev);
+	ret = register_candev(priv->netdev);
+	if (ret) {
+		mod_alert("%s, register failed", priv->card->id.name);
+		goto reg_failed;
+	}
+	ret = mk_netdev_sysfs(priv);
+	if (ret) {
+		mod_alert("%s, sysfs failed", priv->card->id.name);
+		goto sysfs_failed;
+	}
+	return 0;
+sysfs_failed:
+	unregister_candev(priv->netdev);
+reg_failed:
+	return EINVAL;
+}
+
+static void unreg_netdev(struct softing_priv *priv)
+{
+	rm_netdev_sysfs(priv);
+	unregister_candev(priv->netdev);
+}
+
+void rm_softing(struct softing *card)
+{
+	int j;
+
+	/*first, disable card*/
+	shutdown_card(card);
+
+	for (j = 0; j < card->nbus; ++j) {
+		unreg_netdev(card->bus[j]);
+		rm_netdev(card->bus[j]);
+	}
+
+	rm_card_sysfs(card);
+
+	iounmap(card->dpram.virt);
+}
+EXPORT_SYMBOL(rm_softing);
+
+int mk_softing(struct softing *card)
+{
+	int j;
+
+	/* try_module_get(THIS_MODULE); */
+	mutex_init(&card->fw.lock);
+	spin_lock_init(&card->spin);
+	tasklet_init(&card->irq.bh, softing_dev_svc, (unsigned long)card);
+
+	card->desc = softing_lookup_desc(card->id.manf, card->id.prod);
+	if (!card->desc) {
+		mod_alert("0x%04x:0x%04x not supported\n", card->id.manf,
+			  card->id.prod);
+		goto lookup_failed;
+	}
+	card->id.name = card->desc->name;
+	mod_trace("can (%s)", card->id.name);
+
+	card->dpram.virt = ioremap(card->dpram.phys, card->dpram.size);
+	if (!card->dpram.virt) {
+		mod_alert("dpram ioremap failed\n");
+		goto ioremap_failed;
+	}
+
+	card->dpram.size = card->desc->dpram_size;
+	card->dpram.end = &card->dpram.virt[card->dpram.size];
+	/*initialize_board */
+	card->dpram.rx = (struct softing_rx *)&card->dpram.virt[0x0000];
+	card->dpram.tx = (struct softing_tx *)&card->dpram.virt[0x0400];
+	card->dpram.fct = (struct softing_fct *)&card->dpram.virt[0x0300];
+	card->dpram.info = (struct softing_info *)&card->dpram.virt[0x0330];
+	card->dpram.command = (unsigned short *)&card->dpram.virt[0x07e0];
+	card->dpram.receipt = (unsigned short *)&card->dpram.virt[0x07f0];
+	card->dpram.irq = (struct softing_irq *)&card->dpram.virt[0x07fe];
+
+	/*reset card */
+	if (card->fn.reset)
+		card->fn.reset(card, 1);
+	if (boot_card(card)) {
+		mod_alert("%s, failed to boot", card->id.name);
+		goto boot_failed;
+	}
+
+	/*only now, the chip's are known */
+	card->id.freq = card->desc->freq * 1000000UL;
+
+	if (mk_card_sysfs(card)) {
+		mod_alert("%s, sysfs failed", card->id.name);
+		goto sysfs_failed;
+	}
+
+	if (card->nbus > (sizeof(card->bus) / sizeof(card->bus[0]))) {
+		card->nbus = sizeof(card->bus) / sizeof(card->bus[0]);
+		mod_alert("%s, going for %u busses", card->id.name, card->nbus);
+	}
+
+	for (j = 0; j < card->nbus; ++j) {
+		card->bus[j] = mk_netdev(card, card->id.chip[j]);
+		if (!card->bus[j]) {
+			mod_alert("%s: failed to make can[%i]", card->id.name,
+				  j);
+			goto netdev_failed;
+		}
+		card->bus[j]->index = j;
+	}
+	for (j = 0; j < card->nbus; ++j) {
+		if (reg_netdev(card->bus[j])) {
+			mod_alert("%s: failed to register can[%i]",
+				  card->id.name, j);
+			goto reg_failed;
+		}
+	}
+	mod_trace("card initialised");
+	return 0;
+
+reg_failed:
+	for (j = 0; j < card->nbus; ++j)
+		unreg_netdev(card->bus[j]);
+netdev_failed:
+	for (j = 0; j < card->nbus; ++j) {
+		if (card->bus[j])
+			rm_netdev(card->bus[j]);
+	}
+	rm_card_sysfs(card);
+sysfs_failed:
+	shutdown_card(card);
+boot_failed:
+	iounmap(card->dpram.virt);
+	card->dpram.virt = 0;
+	card->dpram.end = 0;
+ioremap_failed:
+lookup_failed:
+	tasklet_kill(&card->irq.bh);
+	return EINVAL;
+}
+EXPORT_SYMBOL(mk_softing);
+
+static int __init mod_start(void)
+{
+	mod_trace("");
+	return 0;
+}
+
+static void __exit mod_stop(void)
+{
+	mod_trace("");
+}
+
+module_init(mod_start);
+module_exit(mod_stop);
+
+MODULE_DESCRIPTION("socketcan softing driver");
+MODULE_AUTHOR("Kurt Van Dijck <kurt.van.dijck@eia.be>");
+MODULE_LICENSE("GPL");
+
+int softing_debug = 1;
+EXPORT_SYMBOL(softing_debug);
+module_param(softing_debug, int , S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(softing_debug, "trace softing functions");
diff --git a/drivers/net/can/sysfs.c b/drivers/net/can/sysfs.c
new file mode 100644
index 000000000000..746f6410cbeb
--- /dev/null
+++ b/drivers/net/can/sysfs.c
@@ -0,0 +1,509 @@
+/*
+ * Copyright (C) 2007-2008 Wolfgang Grandegger <wg@grandegger.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/capability.h>
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <net/sock.h>
+#include <linux/rtnetlink.h>
+
+#include <linux/can.h>
+#include <linux/can/dev.h>
+
+#include "sysfs.h"
+
+#ifdef CONFIG_SYSFS
+
+/*
+ * SYSFS access functions and attributes. Use same locking as
+ * net/core/net-sysfs.c does.
+ */
+static inline int dev_isalive(const struct net_device *dev)
+{
+	return dev->reg_state <= NETREG_REGISTERED;
+}
+
+/* use same locking rules as GIF* ioctl's */
+static ssize_t can_dev_show(struct device *d,
+			    struct device_attribute *attr, char *buf,
+			    ssize_t (*fmt)(struct net_device *, char *))
+{
+	struct net_device *dev = to_net_dev(d);
+	ssize_t ret = -EINVAL;
+
+	read_lock(&dev_base_lock);
+	if (dev_isalive(dev))
+		ret = (*fmt)(dev, buf);
+	read_unlock(&dev_base_lock);
+
+	return ret;
+}
+
+/* generate a show function for simple field */
+#define CAN_DEV_SHOW(field, fmt_string)					\
+static ssize_t fmt_can_##field(struct net_device *dev, char *buf)	\
+{									\
+	struct can_priv *priv = netdev_priv(dev);			\
+	return sprintf(buf, fmt_string, priv->field);			\
+}									\
+static ssize_t show_can_##field(struct device *d,			\
+				struct device_attribute *attr,		\
+				char *buf)				\
+{									\
+	return can_dev_show(d, attr, buf, fmt_can_##field);		\
+}
+
+/* use same locking and permission rules as SIF* ioctl's */
+static ssize_t can_dev_store(struct device *d, struct device_attribute *attr,
+			     const char *buf, size_t len,
+			     int (*set)(struct net_device *, unsigned long))
+{
+	struct net_device *dev = to_net_dev(d);
+	unsigned long new;
+	int ret = -EINVAL;
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	ret = strict_strtoul(buf, 0, &new);
+	if (ret)
+		goto out;
+
+	rtnl_lock();
+	if (dev_isalive(dev)) {
+		ret = (*set)(dev, new);
+		if (!ret)
+			ret = len;
+	}
+	rtnl_unlock();
+out:
+	return ret;
+}
+
+#define CAN_CREATE_FILE(_dev, _name)					\
+	if (device_create_file(&_dev->dev, &dev_attr_##_name))		\
+		dev_err(ND2D(_dev),					\
+			"Couldn't create device file for ##_name\n")
+
+#define CAN_REMOVE_FILE(_dev, _name)					\
+	device_remove_file(&_dev->dev, &dev_attr_##_name)		\
+
+CAN_DEV_SHOW(ctrlmode, "0x%x\n");
+
+static int change_can_ctrlmode(struct net_device *dev, unsigned long ctrlmode)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	int err = 0;
+
+	if (priv->state != CAN_STATE_STOPPED)
+		return -EBUSY;
+
+	if (priv->do_set_ctrlmode)
+		err = priv->do_set_ctrlmode(dev, ctrlmode);
+
+	if (!err)
+		priv->ctrlmode = ctrlmode;
+
+	return err;
+}
+
+static ssize_t store_can_ctrlmode(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t len)
+{
+	return can_dev_store(dev, attr, buf, len, change_can_ctrlmode);
+}
+
+static DEVICE_ATTR(can_ctrlmode, S_IRUGO | S_IWUSR,
+		   show_can_ctrlmode, store_can_ctrlmode);
+
+static const char *can_state_names[] = {
+	"active", "bus-warn", "bus-pass" , "bus-off",
+	"stopped", "sleeping", "unkown"
+};
+
+static ssize_t printf_can_state(struct net_device *dev, char *buf)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	enum can_state state;
+	int err = 0;
+
+	if (priv->do_get_state) {
+		err = priv->do_get_state(dev, &state);
+		if (err)
+			goto out;
+		priv->state = state;
+	} else
+		state = priv->state;
+
+	if (state >= ARRAY_SIZE(can_state_names))
+		state = ARRAY_SIZE(can_state_names) - 1;
+	err = sprintf(buf, "%s\n", can_state_names[state]);
+out:
+	return err;
+}
+
+static ssize_t show_can_state(struct device *d,
+			      struct device_attribute *attr, char *buf)
+{
+	return can_dev_show(d, attr, buf, printf_can_state);
+}
+
+static DEVICE_ATTR(can_state, S_IRUGO, show_can_state, NULL);
+
+CAN_DEV_SHOW(restart_ms, "%d\n");
+
+static int change_can_restart_ms(struct net_device *dev, unsigned long ms)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	if (priv->restart_ms < 0)
+		return -EOPNOTSUPP;
+	priv->restart_ms = ms;
+	return 0;
+}
+
+static ssize_t store_can_restart_ms(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t len)
+{
+	return can_dev_store(dev, attr, buf, len, change_can_restart_ms);
+}
+
+static DEVICE_ATTR(can_restart_ms, S_IRUGO | S_IWUSR,
+		   show_can_restart_ms, store_can_restart_ms);
+
+static ssize_t printf_can_echo(struct net_device *dev, char *buf)
+{
+	return sprintf(buf, "%d\n", dev->flags & IFF_ECHO ? 1 : 0);
+}
+
+static ssize_t show_can_echo(struct device *d,
+			  struct device_attribute *attr, char *buf)
+{
+	return can_dev_show(d, attr, buf, printf_can_echo);
+}
+
+static int change_can_echo(struct net_device *dev, unsigned long on)
+{
+	if (on)
+		dev->flags |= IFF_ECHO;
+	else
+		dev->flags &= ~IFF_ECHO;
+	return 0;
+}
+
+static ssize_t store_can_echo(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t len)
+{
+	return can_dev_store(dev, attr, buf, len, change_can_echo);
+}
+
+static DEVICE_ATTR(can_echo, S_IRUGO | S_IWUSR, show_can_echo, store_can_echo);
+
+static int change_can_restart(struct net_device *dev, unsigned long on)
+{
+	return can_restart_now(dev);
+}
+
+static ssize_t store_can_restart(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t len)
+{
+	return can_dev_store(dev, attr, buf, len, change_can_restart);
+}
+
+static DEVICE_ATTR(can_restart, S_IWUSR, NULL, store_can_restart);
+
+/* Show a given attribute if the CAN bittiming group */
+static ssize_t can_btc_show(const struct device *d,
+			    struct device_attribute *attr, char *buf,
+			    unsigned long offset)
+{
+	struct net_device *dev = to_net_dev(d);
+	struct can_priv *priv = netdev_priv(dev);
+	struct can_bittiming_const *btc = priv->bittiming_const;
+	ssize_t ret = -EINVAL;
+
+	WARN_ON(offset >= sizeof(struct can_bittiming_const) ||
+		offset % sizeof(u32) != 0);
+
+	read_lock(&dev_base_lock);
+	if (dev_isalive(dev) && btc)
+		ret = sprintf(buf, "%d\n",
+			      *(u32 *)(((u8 *)btc) + offset));
+
+	read_unlock(&dev_base_lock);
+	return ret;
+}
+
+/* Generate a read-only bittiming const attribute */
+#define CAN_BT_CONST_ENTRY(name)					\
+static ssize_t show_##name(struct device *d,				\
+			   struct device_attribute *attr, char *buf) 	\
+{									\
+	return can_btc_show(d, attr, buf,				\
+			    offsetof(struct can_bittiming_const, name));\
+}									\
+static DEVICE_ATTR(hw_##name, S_IRUGO, show_##name, NULL)
+
+CAN_BT_CONST_ENTRY(tseg1_min);
+CAN_BT_CONST_ENTRY(tseg1_max);
+CAN_BT_CONST_ENTRY(tseg2_min);
+CAN_BT_CONST_ENTRY(tseg2_max);
+CAN_BT_CONST_ENTRY(sjw_max);
+CAN_BT_CONST_ENTRY(brp_min);
+CAN_BT_CONST_ENTRY(brp_max);
+CAN_BT_CONST_ENTRY(brp_inc);
+
+static ssize_t can_bt_show(const struct device *d,
+			   struct device_attribute *attr, char *buf,
+			   unsigned long offset)
+{
+	struct net_device *dev = to_net_dev(d);
+	struct can_priv *priv = netdev_priv(dev);
+	struct can_bittiming *bt = &priv->bittiming;
+	ssize_t ret = -EINVAL;
+	u32 *ptr, val;
+
+	WARN_ON(offset >= sizeof(struct can_bittiming) ||
+		offset % sizeof(u32) != 0);
+
+	read_lock(&dev_base_lock);
+	if (dev_isalive(dev)) {
+		ptr = (u32 *)(((u8 *)bt) + offset);
+		if (ptr == &bt->sample_point &&
+		    priv->state != CAN_STATE_STOPPED)
+			val = can_sample_point(bt);
+		else
+			val = *ptr;
+		ret = sprintf(buf, "%d\n", val);
+	}
+	read_unlock(&dev_base_lock);
+	return ret;
+}
+
+static ssize_t can_bt_store(const struct device *d,
+			    struct device_attribute *attr,
+			    const char *buf, size_t count,
+			    unsigned long offset)
+{
+	struct net_device *dev = to_net_dev(d);
+	struct can_priv *priv = netdev_priv(dev);
+	struct can_bittiming *bt = &priv->bittiming;
+	unsigned long new;
+	ssize_t ret = -EINVAL;
+	u32 *ptr;
+
+	if (priv->state != CAN_STATE_STOPPED)
+		return -EBUSY;
+
+	WARN_ON(offset >= sizeof(struct can_bittiming) ||
+		offset % sizeof(u32) != 0);
+
+	ret = strict_strtoul(buf, 0, &new);
+	if (ret)
+		goto out;
+
+	ptr = (u32 *)(((u8 *)bt) + offset);
+	rtnl_lock();
+	if (dev_isalive(dev)) {
+		*ptr = (u32)new;
+
+		if ((ptr == &bt->bitrate) || (ptr == &bt->sample_point)) {
+			bt->tq = 0;
+			bt->brp = 0;
+			bt->sjw = 0;
+			bt->prop_seg = 0;
+			bt->phase_seg1 = 0;
+			bt->phase_seg2 = 0;
+		} else {
+			bt->bitrate = 0;
+			bt->sample_point = 0;
+		}
+		ret = count;
+	}
+	rtnl_unlock();
+out:
+	return ret;
+}
+
+#define CAN_BT_ENTRY_RO(name)						\
+static ssize_t show_##name(struct device *d,				\
+			   struct device_attribute *attr, char *buf) 	\
+{									\
+	return can_bt_show(d, attr, buf,				\
+			   offsetof(struct can_bittiming, name));	\
+}									\
+static DEVICE_ATTR(hw_##name, S_IRUGO, show_##name, NULL)
+
+CAN_BT_ENTRY_RO(clock);
+
+#define CAN_BT_ENTRY(name)						\
+static ssize_t show_##name(struct device *d,				\
+			   struct device_attribute *attr, char *buf) 	\
+{									\
+	return can_bt_show(d, attr, buf,				\
+			   offsetof(struct can_bittiming, name));	\
+}									\
+static ssize_t store_##name(struct device *d,				\
+			    struct device_attribute *attr,		\
+			    const char *buf, size_t count)		\
+{									\
+	return can_bt_store(d, attr, buf, count,			\
+			    offsetof(struct can_bittiming, name));	\
+}									\
+static DEVICE_ATTR(name, S_IRUGO | S_IWUSR, show_##name, store_##name)
+
+CAN_BT_ENTRY(bitrate);
+CAN_BT_ENTRY(sample_point);
+CAN_BT_ENTRY(tq);
+CAN_BT_ENTRY(prop_seg);
+CAN_BT_ENTRY(phase_seg1);
+CAN_BT_ENTRY(phase_seg2);
+CAN_BT_ENTRY(sjw);
+
+static struct attribute *can_bittiming_attrs[] = {
+	&dev_attr_hw_tseg1_min.attr,
+	&dev_attr_hw_tseg1_max.attr,
+	&dev_attr_hw_tseg2_max.attr,
+	&dev_attr_hw_tseg2_min.attr,
+	&dev_attr_hw_sjw_max.attr,
+	&dev_attr_hw_brp_min.attr,
+	&dev_attr_hw_brp_max.attr,
+	&dev_attr_hw_brp_inc.attr,
+	&dev_attr_hw_clock.attr,
+	&dev_attr_bitrate.attr,
+	&dev_attr_sample_point.attr,
+	&dev_attr_tq.attr,
+	&dev_attr_prop_seg.attr,
+	&dev_attr_phase_seg1.attr,
+	&dev_attr_phase_seg2.attr,
+	&dev_attr_sjw.attr,
+	NULL
+};
+
+static struct attribute_group can_bittiming_group = {
+	.name = "can_bittiming",
+	.attrs = can_bittiming_attrs,
+};
+
+/* Show a given attribute in the CAN statistics group */
+static ssize_t can_stat_show(const struct device *d,
+			     struct device_attribute *attr, char *buf,
+			     unsigned long offset)
+{
+	struct net_device *dev = to_net_dev(d);
+	struct can_priv *priv = netdev_priv(dev);
+	struct can_device_stats *stats = &priv->can_stats;
+	ssize_t ret = -EINVAL;
+
+	WARN_ON(offset >= sizeof(struct can_device_stats) ||
+		offset % sizeof(unsigned long) != 0);
+
+	read_lock(&dev_base_lock);
+	if (dev_isalive(dev))
+		ret = sprintf(buf, "%ld\n",
+			      *(unsigned long *)(((u8 *)stats) + offset));
+
+	read_unlock(&dev_base_lock);
+	return ret;
+}
+
+/* Generate a read-only CAN statistics attribute */
+#define CAN_STAT_ENTRY(name)						\
+static ssize_t show_##name(struct device *d,				\
+			   struct device_attribute *attr, char *buf) 	\
+{									\
+	return can_stat_show(d, attr, buf,				\
+			     offsetof(struct can_device_stats, name));	\
+}									\
+static DEVICE_ATTR(name, S_IRUGO, show_##name, NULL)
+
+CAN_STAT_ENTRY(error_warning);
+CAN_STAT_ENTRY(error_passive);
+CAN_STAT_ENTRY(bus_error);
+CAN_STAT_ENTRY(arbitration_lost);
+CAN_STAT_ENTRY(data_overrun);
+CAN_STAT_ENTRY(wakeup);
+CAN_STAT_ENTRY(restarts);
+
+static struct attribute *can_statistics_attrs[] = {
+	&dev_attr_error_warning.attr,
+	&dev_attr_error_passive.attr,
+	&dev_attr_bus_error.attr,
+	&dev_attr_arbitration_lost.attr,
+	&dev_attr_data_overrun.attr,
+	&dev_attr_wakeup.attr,
+	&dev_attr_restarts.attr,
+	NULL
+};
+
+static struct attribute_group can_statistics_group = {
+	.name = "can_statistics",
+	.attrs = can_statistics_attrs,
+};
+
+void can_create_sysfs(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+	int err;
+
+	CAN_CREATE_FILE(dev, can_ctrlmode);
+	CAN_CREATE_FILE(dev, can_echo);
+	CAN_CREATE_FILE(dev, can_restart);
+	CAN_CREATE_FILE(dev, can_state);
+	CAN_CREATE_FILE(dev, can_restart_ms);
+
+	err = sysfs_create_group(&(dev->dev.kobj),
+				 &can_statistics_group);
+	if (err) {
+		printk(KERN_EMERG
+		       "couldn't create sysfs group for CAN statistics\n");
+	}
+
+	if (priv->bittiming_const) {
+		err = sysfs_create_group(&(dev->dev.kobj),
+					 &can_bittiming_group);
+		if (err) {
+			printk(KERN_EMERG "couldn't create sysfs "
+			       "group for CAN bittiming\n");
+		}
+	}
+}
+
+void can_remove_sysfs(struct net_device *dev)
+{
+	struct can_priv *priv = netdev_priv(dev);
+
+	CAN_REMOVE_FILE(dev, can_ctrlmode);
+	CAN_REMOVE_FILE(dev, can_echo);
+	CAN_REMOVE_FILE(dev, can_state);
+	CAN_REMOVE_FILE(dev, can_restart);
+	CAN_REMOVE_FILE(dev, can_restart_ms);
+
+	sysfs_remove_group(&(dev->dev.kobj), &can_statistics_group);
+	if (priv->bittiming_const)
+		sysfs_remove_group(&(dev->dev.kobj), &can_bittiming_group);
+}
+
+#endif /* CONFIG_SYSFS */
+
+
+
diff --git a/drivers/net/can/sysfs.h b/drivers/net/can/sysfs.h
new file mode 100644
index 000000000000..e21f2fa4b158
--- /dev/null
+++ b/drivers/net/can/sysfs.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) 2007 Wolfgang Grandegger <wg@grandegger.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef CAN_SYSFS_H
+#define CAN_SYSFS_H
+
+void can_create_sysfs(struct net_device *dev);
+void can_remove_sysfs(struct net_device *dev);
+
+#endif /* CAN_SYSFS_H */
diff --git a/drivers/net/can/vcan.c b/drivers/net/can/vcan.c
index 103f0f1df280..69e6b804c477 100644
--- a/drivers/net/can/vcan.c
+++ b/drivers/net/can/vcan.c
@@ -128,26 +128,27 @@ static int vcan_tx(struct sk_buff *skb, struct net_device *dev)
 	return NETDEV_TX_OK;
 }
 
+
 static void vcan_setup(struct net_device *dev)
 {
-	dev->type              = ARPHRD_CAN;
-	dev->mtu               = sizeof(struct can_frame);
-	dev->hard_header_len   = 0;
-	dev->addr_len          = 0;
-	dev->tx_queue_len      = 0;
-	dev->flags             = IFF_NOARP;
+	dev->type		= ARPHRD_CAN;
+	dev->mtu		= sizeof(struct can_frame);
+	dev->hard_header_len	= 0;
+	dev->addr_len		= 0;
+	dev->tx_queue_len	= 0;
+	dev->flags		= IFF_NOARP;
 
 	/* set flags according to driver capabilities */
 	if (echo)
 		dev->flags |= IFF_ECHO;
 
-	dev->hard_start_xmit   = vcan_tx;
-	dev->destructor        = free_netdev;
+	dev->hard_start_xmit	= vcan_tx;
+	dev->destructor		= free_netdev;
 }
 
 static struct rtnl_link_ops vcan_link_ops __read_mostly = {
-       .kind           = "vcan",
-       .setup          = vcan_setup,
+	.kind	= "vcan",
+	.setup	= vcan_setup,
 };
 
 static __init int vcan_init_module(void)
diff --git a/drivers/net/cs89x0.c b/drivers/net/cs89x0.c
index 7107620f615d..d56846d8d5b4 100644
--- a/drivers/net/cs89x0.c
+++ b/drivers/net/cs89x0.c
@@ -193,12 +193,17 @@ static unsigned int cs8900_irq_map[] = {IRQ_IXDP2X01_CS8900, 0, 0, 0};
 #define CIRRUS_DEFAULT_IRQ	VH_INTC_INT_NUM_CASCADED_INTERRUPT_1 /* Event inputs bank 1 - ID 35/bit 3 */
 static unsigned int netcard_portlist[] __used __initdata = {CIRRUS_DEFAULT_BASE, 0};
 static unsigned int cs8900_irq_map[] = {CIRRUS_DEFAULT_IRQ, 0, 0, 0};
-#elif defined(CONFIG_MACH_MX31ADS)
-#include <mach/board-mx31ads.h>
-static unsigned int netcard_portlist[] __used __initdata = {
-	PBC_BASE_ADDRESS + PBC_CS8900A_IOBASE + 0x300, 0
-};
-static unsigned cs8900_irq_map[] = {EXPIO_INT_ENET_INT, 0, 0, 0};
+#elif defined(CONFIG_ARCH_MXC)
+/*! Null terminated portlist used to probe for the CS8900A device on ISA Bus
+ * Add 3 to reset the page window before probing (fixes eth probe when deployed
+ * using nand_boot)
+ */
+extern unsigned int netcard_portlist[2];
+/*!
+ * The CS8900A has 4 IRQ pins, which is software selectable, CS8900A interrupt
+ * pin 0 is used for interrupt generation.
+ */
+extern unsigned int cs8900_irq_map[4];
 #else
 static unsigned int netcard_portlist[] __used __initdata =
    { 0x300, 0x320, 0x340, 0x360, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2a0, 0x2c0, 0x2e0, 0};
@@ -1034,7 +1039,7 @@ skip_this_frame:
 
 void  __init reset_chip(struct net_device *dev)
 {
-#if !defined(CONFIG_MACH_MX31ADS)
+#if !defined(CONFIG_ARCH_MXC)
 #if !defined(CONFIG_MACH_IXDP2351) && !defined(CONFIG_ARCH_IXDP2X01)
 	struct net_local *lp = netdev_priv(dev);
 	int ioaddr = dev->base_addr;
@@ -1063,7 +1068,7 @@ void  __init reset_chip(struct net_device *dev)
 	reset_start_time = jiffies;
 	while( (readreg(dev, PP_SelfST) & INIT_DONE) == 0 && jiffies - reset_start_time < 2)
 		;
-#endif /* !CONFIG_MACH_MX31ADS */
+#endif	/* !CONFIG_ARCH_MXC */
 }
 
 
diff --git a/drivers/net/enc28j60.c b/drivers/net/enc28j60.c
index 36cb6e95b465..98c07af8a2f9 100644
--- a/drivers/net/enc28j60.c
+++ b/drivers/net/enc28j60.c
@@ -36,6 +36,7 @@
 #define DRV_VERSION	"1.01"
 
 #define SPI_OPLEN	1
+#define MAX_ENC_CARDS 	1
 
 #define ENC28J60_MSG_DEFAULT	\
 	(NETIF_MSG_PROBE | NETIF_MSG_IFUP | NETIF_MSG_IFDOWN | NETIF_MSG_LINK)
@@ -49,6 +50,10 @@
 /* Max TX retries in case of collision as suggested by errata datasheet */
 #define MAX_TX_RETRYCOUNT	16
 
+#ifdef CONFIG_ARCH_STMP3XXX
+#include <mach/stmp3xxx.h>
+#include <mach/regs-ocotp.h>
+#endif
 enum {
 	RXFILTER_NORMAL,
 	RXFILTER_MULTI,
@@ -81,6 +86,69 @@ static struct {
 	u32 msg_enable;
 } debug = { -1 };
 
+static int random_mac;	/* = 0 */
+static char *mac[MAX_ENC_CARDS];
+
+static int enc28j60_get_mac(unsigned char *dev_addr, int idx)
+{
+	int i, r;
+	char *p, *item;
+	unsigned long v;
+	unsigned char sv[10];
+
+	if (idx < 0)
+		idx = 0;
+	if (idx > MAX_ENC_CARDS)
+		return false;
+
+	if (!mac[idx]) {
+#ifdef CONFIG_ARCH_STMP3XXX
+		if (get_evk_board_version() >= 1) {
+			int mac1 , mac2 , retry = 0;
+
+			HW_OCOTP_CTRL_SET(BM_OCOTP_CTRL_RD_BANK_OPEN);
+			while (HW_OCOTP_CTRL_RD() & BM_OCOTP_CTRL_BUSY) {
+				msleep(10);
+				retry++;
+				if (retry > 10)
+					return false;
+			}
+
+			mac1 = HW_OCOTP_CUSTn_RD(0);
+			mac2 = HW_OCOTP_CUSTn_RD(1);
+			if (MAX_ADDR_LEN < 6)
+				return false;
+
+			dev_addr[0] = (mac1 >> 24) & 0xFF;
+			dev_addr[1] = (mac1 >> 16) & 0xFF;
+			dev_addr[2] = (mac1 >> 8) & 0xFF;
+			dev_addr[3] = (mac1 >> 0) & 0xFF;
+			dev_addr[4] = (mac2 >> 8) & 0xFF;
+			dev_addr[5] = (mac2 >> 0) & 0xFF;
+			return true;
+		}
+#endif
+		return false;
+	}
+
+	item = mac[idx];
+	for (i = 0; i < MAX_ADDR_LEN; i++) {
+		p = strchr(item, ':');
+		if (!p)
+			sprintf(sv, "0x%s", item);
+		else
+			sprintf(sv, "0x%*.*s", p - item, p-item, item);
+		r = strict_strtoul(sv, 0, &v);
+		dev_addr[i] = v;
+		if (p)
+			item = p + 1;
+		else
+			break;
+		if (r < 0)
+			return false;
+	}
+	return true;
+}
 /*
  * SPI read buffer
  * wait for the SPI transfer and copy received data to destination
@@ -90,10 +158,13 @@ spi_read_buf(struct enc28j60_net *priv, int len, u8 *data)
 {
 	u8 *rx_buf = priv->spi_transfer_buf + 4;
 	u8 *tx_buf = priv->spi_transfer_buf;
-	struct spi_transfer t = {
-		.tx_buf = tx_buf,
-		.rx_buf = rx_buf,
-		.len = SPI_OPLEN + len,
+	struct spi_transfer tt = {
+		.tx_buf	= tx_buf,
+		.len	= SPI_OPLEN,
+	};
+	struct spi_transfer tr = {
+		.rx_buf	= rx_buf,
+		.len	= len,
 	};
 	struct spi_message msg;
 	int ret;
@@ -102,10 +173,11 @@ spi_read_buf(struct enc28j60_net *priv, int len, u8 *data)
 	tx_buf[1] = tx_buf[2] = tx_buf[3] = 0;	/* don't care */
 
 	spi_message_init(&msg);
-	spi_message_add_tail(&t, &msg);
+	spi_message_add_tail(&tt, &msg);
+	spi_message_add_tail(&tr, &msg);
 	ret = spi_sync(priv->spi, &msg);
 	if (ret == 0) {
-		memcpy(data, &rx_buf[SPI_OPLEN], len);
+		memcpy(data, rx_buf, len);
 		ret = msg.status;
 	}
 	if (ret && netif_msg_drv(priv))
@@ -196,16 +268,32 @@ static void enc28j60_soft_reset(struct enc28j60_net *priv)
  */
 static void enc28j60_set_bank(struct enc28j60_net *priv, u8 addr)
 {
-	if ((addr & BANK_MASK) != priv->bank) {
-		u8 b = (addr & BANK_MASK) >> 5;
+	u8 b = (addr & BANK_MASK) >> 5;
+
+	/* These registers (EIE, EIR, ESTAT, ECON2, ECON1)
+	 * are present in all banks, no need to switch bank
+	 */
+	if (addr >= EIE && addr <= ECON1)
+		return;
 
-		if (b != (ECON1_BSEL1 | ECON1_BSEL0))
+	/* Clear or set each bank selection bit as needed */
+	if ((b & ECON1_BSEL0) != (priv->bank & ECON1_BSEL0)) {
+		if (b & ECON1_BSEL0)
+			spi_write_op(priv, ENC28J60_BIT_FIELD_SET, ECON1,
+				     ECON1_BSEL0);
+		else
+			spi_write_op(priv, ENC28J60_BIT_FIELD_CLR, ECON1,
+				     ECON1_BSEL0);
+	}
+	if ((b & ECON1_BSEL1) != (priv->bank & ECON1_BSEL1)) {
+		if (b & ECON1_BSEL1)
+			spi_write_op(priv, ENC28J60_BIT_FIELD_SET, ECON1,
+				     ECON1_BSEL1);
+		else
 			spi_write_op(priv, ENC28J60_BIT_FIELD_CLR, ECON1,
-				     ECON1_BSEL1 | ECON1_BSEL0);
-		if (b != 0)
-			spi_write_op(priv, ENC28J60_BIT_FIELD_SET, ECON1, b);
-		priv->bank = (addr & BANK_MASK);
+				     ECON1_BSEL1);
 	}
+	priv->bank = b;
 }
 
 /*
@@ -930,7 +1018,7 @@ static void enc28j60_hw_rx(struct net_device *ndev)
 	if (netif_msg_rx_status(priv))
 		enc28j60_dump_rsv(priv, __func__, next_packet, len, rxstat);
 
-	if (!RSV_GETBIT(rxstat, RSV_RXOK)) {
+	if (!RSV_GETBIT(rxstat, RSV_RXOK) || len > MAX_FRAMELEN) {
 		if (netif_msg_rx_err(priv))
 			dev_err(&ndev->dev, "Rx Error (%04x)\n", rxstat);
 		ndev->stats.rx_errors++;
@@ -938,6 +1026,8 @@ static void enc28j60_hw_rx(struct net_device *ndev)
 			ndev->stats.rx_crc_errors++;
 		if (RSV_GETBIT(rxstat, RSV_LENCHECKERR))
 			ndev->stats.rx_frame_errors++;
+		if (len > MAX_FRAMELEN)
+			ndev->stats.rx_over_errors++;
 	} else {
 		skb = dev_alloc_skb(len + NET_IP_ALIGN);
 		if (!skb) {
@@ -1093,8 +1183,24 @@ static int enc28j60_rx_interrupt(struct net_device *ndev)
 				priv->max_pk_counter);
 	}
 	ret = pk_counter;
-	while (pk_counter-- > 0)
+	while (pk_counter-- > 0) {
+		if (!priv->full_duplex) {
+			/*
+			 * This works only in HALF DUPLEX mode:
+			 * when more than 2 packets are available, start
+			 * transmission of 11111.. frame by setting
+			 * FCON0 (0x01) in EFLOCON
+			 *
+			 * This bit can be cleared either explicitly, or by
+			 * trasmitting the packet in enc28j60_hw_tx.
+			 */
+			if (pk_counter > 2)
+				locked_reg_bfset(priv, EFLOCON, 0x01);
+			if (pk_counter == 1)
+				locked_reg_bfclr(priv, EFLOCON, 0x01);
+		}
 		enc28j60_hw_rx(ndev);
+	}
 
 	return ret;
 }
@@ -1220,6 +1326,11 @@ static void enc28j60_irq_work_handler(struct work_struct *work)
  */
 static void enc28j60_hw_tx(struct enc28j60_net *priv)
 {
+	if (!priv->tx_skb) {
+		enc28j60_tx_clear(priv->netdev, false);
+		return;
+	}
+
 	if (netif_msg_tx_queued(priv))
 		printk(KERN_DEBUG DRV_NAME
 			": Tx Packet Len:%d\n", priv->tx_skb->len);
@@ -1523,6 +1634,7 @@ static int __devinit enc28j60_probe(struct spi_device *spi)
 	struct net_device *dev;
 	struct enc28j60_net *priv;
 	int ret = 0;
+	int set;
 
 	if (netif_msg_drv(&debug))
 		dev_info(&spi->dev, DRV_NAME " Ethernet driver %s loaded\n",
@@ -1556,7 +1668,11 @@ static int __devinit enc28j60_probe(struct spi_device *spi)
 		ret = -EIO;
 		goto error_irq;
 	}
-	random_ether_addr(dev->dev_addr);
+
+	/* need a counter here, to count instances of enc28j60 devices */
+	set = enc28j60_get_mac(dev->dev_addr, -1);
+	if (!set || random_mac)
+		random_ether_addr(dev->dev_addr);
 	enc28j60_set_hw_macaddr(dev);
 
 	/* Board setup must set the relevant edge trigger type;
@@ -1616,6 +1732,40 @@ static int __devexit enc28j60_remove(struct spi_device *spi)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int
+enc28j60_suspend(struct spi_device *spi, pm_message_t state)
+{
+	struct enc28j60_net *priv = dev_get_drvdata(&spi->dev);
+	struct net_device *net_dev = priv ? priv->netdev : NULL;
+
+	if (net_dev && netif_running(net_dev)) {
+		netif_stop_queue(net_dev);
+		netif_device_detach(net_dev);
+		disable_irq(spi->irq);
+	}
+	return 0;
+}
+
+static int
+enc28j60_resume(struct spi_device *spi)
+{
+	struct enc28j60_net *priv = dev_get_drvdata(&spi->dev);
+	struct net_device *net_dev = priv ? priv->netdev : NULL;
+
+	if (net_dev && netif_running(net_dev)) {
+		enable_irq(spi->irq);
+		netif_device_attach(net_dev);
+		netif_start_queue(net_dev);
+		schedule_work(&priv->restart_work);
+	}
+	return 0;
+}
+#else
+#define enc28j60_resume 	NULL
+#define enc28j60_suspend 	NULL
+#endif
+
 static struct spi_driver enc28j60_driver = {
 	.driver = {
 		   .name = DRV_NAME,
@@ -1623,6 +1773,8 @@ static struct spi_driver enc28j60_driver = {
 	 },
 	.probe = enc28j60_probe,
 	.remove = __devexit_p(enc28j60_remove),
+	.suspend = enc28j60_suspend,
+	.resume = enc28j60_resume,
 };
 
 static int __init enc28j60_init(void)
@@ -1645,4 +1797,6 @@ MODULE_DESCRIPTION(DRV_NAME " ethernet driver");
 MODULE_AUTHOR("Claudio Lanconelli <lanconelli.claudio@eptar.com>");
 MODULE_LICENSE("GPL");
 module_param_named(debug, debug.msg_enable, int, 0);
+module_param(random_mac, int, 0444);
+module_param_array(mac, charp, NULL, 0);
 MODULE_PARM_DESC(debug, "Debug verbosity level (0=none, ..., ffff=all)");
diff --git a/drivers/net/fec.c b/drivers/net/fec.c
index ecd5c71a7a8a..94d55a17d11c 100644
--- a/drivers/net/fec.c
+++ b/drivers/net/fec.c
@@ -18,6 +18,9 @@
  * Bug fixes and cleanup by Philippe De Muyter (phdm@macqel.be)
  * Copyright (c) 2004-2006 Macq Electronique SA.
  */
+/*
+ * Copyright 2006-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
 
 #include <linux/module.h>
 #include <linux/kernel.h>
@@ -36,16 +39,29 @@
 #include <linux/spinlock.h>
 #include <linux/workqueue.h>
 #include <linux/bitops.h>
+#include <linux/clk.h>
 
 #include <asm/irq.h>
 #include <asm/uaccess.h>
 #include <asm/io.h>
 #include <asm/pgtable.h>
 #include <asm/cacheflush.h>
+#include <asm/mach-types.h>
 
+#if defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
+    defined(CONFIG_M520x) || defined(CONFIG_M532x)
 #include <asm/coldfire.h>
 #include <asm/mcfsim.h>
 #include "fec.h"
+#define FEC_ALIGNMENT  (0x03)          /*FEC needs 4bytes alignment*/
+#elif defined(CONFIG_ARCH_MXC)
+#include <mach/hardware.h>
+#include <mach/iim.h>
+#include "fec.h"
+#define FEC_ALIGNMENT  (0x0F)          /*FEC needs 128bits(32bytes) alignment*/
+#endif
+
+#define FEC_ADDR_ALIGNMENT(x) ((unsigned char *)(((unsigned long )(x) + (FEC_ALIGNMENT)) & (~FEC_ALIGNMENT)))
 
 #if defined(CONFIG_FEC2)
 #define	FEC_MAX_PORTS	2
@@ -53,7 +69,7 @@
 #define	FEC_MAX_PORTS	1
 #endif
 
-#if defined(CONFIG_M5272)
+#if defined(CONFIG_M5272) || defined(CONFIG_ARCH_MXC)
 #define HAVE_mii_link_interrupt
 #endif
 
@@ -72,6 +88,8 @@ static unsigned int fec_hw[] = {
 	(MCF_MBAR+0x30000),
 #elif defined(CONFIG_M532x)
 	(MCF_MBAR+0xfc030000),
+#elif defined(CONFIG_ARCH_MXC)
+	(IO_ADDRESS(FEC_BASE_ADDR)),
 #else
 	&(((immap_t *)IMAP_ADDR)->im_cpm.cp_fec),
 #endif
@@ -149,6 +167,12 @@ typedef struct {
 #define FEC_ENET_MII	((uint)0x00800000)	/* MII interrupt */
 #define FEC_ENET_EBERR	((uint)0x00400000)	/* SDMA bus error */
 
+#ifndef CONFIG_ARCH_MXC
+#define FEC_ENET_MASK   ((uint)0xffc00000)
+#else
+#define FEC_ENET_MASK   ((uint)0xfff80000)
+#endif
+
 /* The FEC stores dest/src/type, data, and checksum for receive packets.
  */
 #define PKT_MAXBUF_SIZE		1518
@@ -162,7 +186,7 @@ typedef struct {
  * account when setting it.
  */
 #if defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
-    defined(CONFIG_M520x) || defined(CONFIG_M532x)
+    defined(CONFIG_M520x) || defined(CONFIG_M532x) || defined(CONFIG_ARCH_MXC)
 #define	OPT_FRAME_SIZE	(PKT_MAXBUF_SIZE << 16)
 #else
 #define	OPT_FRAME_SIZE	0
@@ -185,11 +209,13 @@ struct fec_enet_private {
 	/* The saved address of a sent-in-place packet/buffer, for skfree(). */
 	unsigned char *tx_bounce[TX_RING_SIZE];
 	struct	sk_buff* tx_skbuff[TX_RING_SIZE];
+	struct  sk_buff* rx_skbuff[RX_RING_SIZE];
 	ushort	skb_cur;
 	ushort	skb_dirty;
 
 	/* CPM dual port RAM relative addresses.
 	*/
+	void *  cbd_mem_base;           /* save the virtual base address of rx&tx buffer descripter */
 	cbd_t	*rx_bd_base;		/* Address of Rx and Tx buffers. */
 	cbd_t	*tx_bd_base;
 	cbd_t	*cur_rx, *cur_tx;		/* The next free ring entry */
@@ -206,6 +232,7 @@ struct fec_enet_private {
 	uint	phy_speed;
 	phy_info_t const	*phy;
 	struct work_struct phy_task;
+	struct net_device *net;
 
 	uint	sequence_done;
 	uint	mii_phy_task_queued;
@@ -217,6 +244,8 @@ struct fec_enet_private {
 	int	link;
 	int	old_link;
 	int	full_duplex;
+
+	struct clk *clk;
 };
 
 static int fec_enet_open(struct net_device *dev);
@@ -231,6 +260,17 @@ static void fec_restart(struct net_device *dev, int duplex);
 static void fec_stop(struct net_device *dev);
 static void fec_set_mac_address(struct net_device *dev);
 
+static void __inline__ fec_dcache_inv_range(void * start, void * end);
+static void __inline__ fec_dcache_flush_range(void * start, void * end);
+
+/*
+ *  fec_copy_threshold controls the copy when recieving ethernet frame.
+ *     If ethernet header aligns 4bytes, the ip header and upper header will not aligns 4bytes.
+ *     The resean is ethernet header is 14bytes.
+ *     And the max size of tcp & ip header is 128bytes. Normally it is 40bytes.
+ *     So I set the default value between 128 to 256.
+ */
+static int fec_copy_threshold = -1;
 
 /* MII processing.  We keep this as simple as possible.  Requests are
  * placed on the list (if there is room).  When the request is finished
@@ -342,10 +382,10 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	 *	4-byte boundaries. Use bounce buffers to copy data
 	 *	and get it aligned. Ugh.
 	 */
-	if (bdp->cbd_bufaddr & 0x3) {
+	if ((bdp->cbd_bufaddr) & FEC_ALIGNMENT) {
 		unsigned int index;
 		index = bdp - fep->tx_bd_base;
-		memcpy(fep->tx_bounce[index], (void *) bdp->cbd_bufaddr, bdp->cbd_datlen);
+		memcpy(fep->tx_bounce[index], (void *) skb->data, skb->len);
 		bdp->cbd_bufaddr = __pa(fep->tx_bounce[index]);
 	}
 
@@ -359,8 +399,8 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	/* Push the data cache so the CPM does not get stale memory
 	 * data.
 	 */
-	flush_dcache_range((unsigned long)skb->data,
-			   (unsigned long)skb->data + skb->len);
+	fec_dcache_flush_range(__va(bdp->cbd_bufaddr), __va(bdp->cbd_bufaddr) +
+		 bdp->cbd_datlen);
 
 	/* Send it on its way.  Tell FEC it's ready, interrupt when done,
 	 * it's the last BD of the frame, and to put the CRC on the end.
@@ -373,7 +413,7 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	dev->trans_start = jiffies;
 
 	/* Trigger transmission start */
-	fecp->fec_x_des_active = 0;
+	fecp->fec_x_des_active = 0x01000000;
 
 	/* If this was the last BD in the ring, start at the beginning again.
 	*/
@@ -460,7 +500,7 @@ fec_enet_interrupt(int irq, void * dev_id)
 
 		/* Handle receive event in its own function.
 		 */
-		if (int_events & FEC_ENET_RXF) {
+		if (int_events & (FEC_ENET_RXF | FEC_ENET_RXB)) {
 			ret = IRQ_HANDLED;
 			fec_enet_rx(dev);
 		}
@@ -469,7 +509,7 @@ fec_enet_interrupt(int irq, void * dev_id)
 		   descriptors. FEC handles all errors, we just discover
 		   them as part of the transmit process.
 		*/
-		if (int_events & FEC_ENET_TXF) {
+		if (int_events & (FEC_ENET_TXF | FEC_ENET_TXB)) {
 			ret = IRQ_HANDLED;
 			fec_enet_tx(dev);
 		}
@@ -572,6 +612,7 @@ fec_enet_rx(struct net_device *dev)
 	struct	sk_buff	*skb;
 	ushort	pkt_len;
 	__u8 *data;
+	int     rx_index ;
 
 #ifdef CONFIG_M532x
 	flush_cache_all();
@@ -588,7 +629,7 @@ fec_enet_rx(struct net_device *dev)
 	bdp = fep->cur_rx;
 
 while (!((status = bdp->cbd_sc) & BD_ENET_RX_EMPTY)) {
-
+	rx_index = bdp - fep->rx_bd_base;
 #ifndef final_version
 	/* Since we have allocated space to hold a complete frame,
 	 * the last indicator should be set.
@@ -638,14 +679,31 @@ while (!((status = bdp->cbd_sc) & BD_ENET_RX_EMPTY)) {
 	 * include that when passing upstream as it messes up
 	 * bridging applications.
 	 */
-	skb = dev_alloc_skb(pkt_len-4);
+	if ((pkt_len - 4) < fec_copy_threshold) {
+		skb = dev_alloc_skb(pkt_len);
+	} else {
+		skb = dev_alloc_skb(FEC_ENET_RX_FRSIZE);
+	}
 
 	if (skb == NULL) {
 		printk("%s: Memory squeeze, dropping packet.\n", dev->name);
 		dev->stats.rx_dropped++;
 	} else {
-		skb_put(skb,pkt_len-4);	/* Make room */
-		skb_copy_to_linear_data(skb, data, pkt_len-4);
+		if ((pkt_len - 4) < fec_copy_threshold) {
+			skb_reserve(skb, 2);    /*skip 2bytes, so ipheader is align 4bytes*/
+			skb_put(skb,pkt_len-4); /* Make room */
+			skb_copy_to_linear_data(skb, data, pkt_len-4);
+		} else {
+			struct sk_buff * pskb = fep->rx_skbuff[rx_index];
+
+			fec_dcache_inv_range(skb->data, skb->data +
+					     FEC_ENET_RX_FRSIZE);
+			fep->rx_skbuff[rx_index] = skb;
+			skb->data = FEC_ADDR_ALIGNMENT(skb->data);
+			bdp->cbd_bufaddr = __pa(skb->data);
+                        skb_put(pskb,pkt_len-4);        /* Make room */
+                        skb = pskb;
+                }
 		skb->protocol=eth_type_trans(skb,dev);
 		netif_rx(skb);
 	}
@@ -672,7 +730,7 @@ while (!((status = bdp->cbd_sc) & BD_ENET_RX_EMPTY)) {
 	 * incoming frames.  On a heavily loaded network, we should be
 	 * able to keep up at the expense of system resources.
 	 */
-	fecp->fec_r_des_active = 0;
+	fecp->fec_r_des_active = 0x01000000;
 #endif
    } /* while (!((status = bdp->cbd_sc) & BD_ENET_RX_EMPTY)) */
 	fep->cur_rx = (cbd_t *)bdp;
@@ -1207,6 +1265,48 @@ static phy_info_t phy_info_dp83848= {
 	},
 };
 
+static phy_info_t phy_info_lan8700 = {
+	0x0007C0C,
+	"LAN8700",
+	(const phy_cmd_t []) { /* config */
+		{ mk_mii_read(MII_REG_CR), mii_parse_cr },
+		{ mk_mii_read(MII_REG_ANAR), mii_parse_anar },
+		{ mk_mii_end, }
+	},
+	(const phy_cmd_t []) { /* startup */
+		{ mk_mii_write(MII_REG_CR, 0x1200), NULL }, /* autonegotiate */
+		{ mk_mii_read(MII_REG_SR), mii_parse_sr },
+		{ mk_mii_end, }
+	},
+	(const phy_cmd_t []) { /* act_int */
+		{ mk_mii_end, }
+	},
+	(const phy_cmd_t []) { /* shutdown */
+		{ mk_mii_end, }
+	},
+};
+
+static phy_info_t phy_info_lan8710 = {
+	0x0007C0F,
+	"LAN8710",
+	(const phy_cmd_t []) { /* config */
+		{ mk_mii_read(MII_REG_CR), mii_parse_cr },
+		{ mk_mii_read(MII_REG_ANAR), mii_parse_anar },
+		{ mk_mii_end, }
+	},
+	(const phy_cmd_t []) { /* startup */
+		{ mk_mii_write(MII_REG_CR, 0x1200), NULL }, /* autonegotiate */
+		{ mk_mii_read(MII_REG_SR), mii_parse_sr },
+		{ mk_mii_end, }
+	},
+	(const phy_cmd_t []) { /* act_int */
+		{ mk_mii_end, }
+	},
+	(const phy_cmd_t []) { /* shutdown */
+		{ mk_mii_end, }
+	},
+};
+
 /* ------------------------------------------------------------------------- */
 
 static phy_info_t const * const phy_info[] = {
@@ -1216,6 +1316,8 @@ static phy_info_t const * const phy_info[] = {
 	&phy_info_am79c874,
 	&phy_info_ks8721bl,
 	&phy_info_dp83848,
+	&phy_info_lan8700,
+	&phy_info_lan8710,
 	NULL
 };
 
@@ -1227,6 +1329,21 @@ mii_link_interrupt(int irq, void * dev_id);
 
 #if defined(CONFIG_M5272)
 /*
+ *  * do some initializtion based architecture of this chip
+ *   */
+static void __inline__ fec_arch_init(void)
+{
+	        return;
+}
+/*
+ *  * do some cleanup based architecture of this chip
+ *   */
+static void __inline__ fec_arch_exit(void)
+{
+	        return;
+}
+
+/*
  *	Code specific to Coldfire 5272 setup.
  */
 static void __inline__ fec_request_intrs(struct net_device *dev)
@@ -1327,15 +1444,40 @@ static void __inline__ fec_phy_ack_intr(void)
 	*icrp = 0x0d000000;
 }
 
-static void __inline__ fec_localhw_setup(void)
+static void __inline__ fec_localhw_setup(struct net_device *dev)
+{
+}
+
+/*
+ * invalidate dcache related with the virtual memory range(start, end)
+ */
+static void __inline__ fec_dcache_inv_range(void * start, void * end)
+{
+	return ;
+}
+
+/*
+ * flush dcache related with the virtual memory range(start, end)
+ */
+static void __inline__ fec_dcache_flush_range(void * start, void * end)
+{
+	return ;
+}
+
+/*
+ * map memory space (addr, addr+size) to uncachable erea.
+ */
+static unsigned long __inline__ fec_map_uncache(unsigned long addr, int size)
 {
+	return addr;
 }
 
 /*
- *	Do not need to make region uncached on 5272.
+ * unmap memory erea started with addr from uncachable erea.
  */
-static void __inline__ fec_uncache(unsigned long addr)
+static void __inline__ fec_unmap_uncache(void * addr)
 {
+	return ;
 }
 
 /* ------------------------------------------------------------------------- */
@@ -1343,6 +1485,22 @@ static void __inline__ fec_uncache(unsigned long addr)
 #elif defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x)
 
 /*
+ * do some initializtion based architecture of this chip
+ */
+static void __inline__ fec_arch_init(void)
+{
+	return;
+}
+
+/*
+ * do some cleanup based architecture of this chip
+ */
+static void __inline__ fec_arch_exit(void)
+{
+	return;
+}
+
+/*
  *	Code specific to Coldfire 5230/5231/5232/5234/5235,
  *	the 5270/5271/5274/5275 and 5280/5282 setups.
  */
@@ -1489,20 +1647,58 @@ static void __inline__ fec_phy_ack_intr(void)
 {
 }
 
-static void __inline__ fec_localhw_setup(void)
+static void __inline__ fec_localhw_setup(struct net_device *dev)
 {
 }
+/*
+ * invalidate dcache related with the virtual memory range(start, end)
+ */
+static void __inline__ fec_dcache_inv_range(void * start, void * end)
+{
+	        return ;
+}
 
 /*
- *	Do not need to make region uncached on 5272.
+ * flush dcache related with the virtual memory range(start, end)
  */
-static void __inline__ fec_uncache(unsigned long addr)
+static void __inline__ fec_dcache_flush_range(void * start, void * end)
 {
+	return ;
+}
+
+/*
+ * map memory space (addr, addr+size) to uncachable erea.
+ */
+static unsigned long __inline__ fec_map_uncache(unsigned long addr, int size)
+{
+	return addr;
+}
+
+/*
+ * unmap memory erea started with addr from uncachable erea.
+ */
+static void __inline__ fec_unmap_uncache(void * addr)
+{
+	return ;
 }
 
 /* ------------------------------------------------------------------------- */
 
 #elif defined(CONFIG_M520x)
+/*
+ * do some initializtion based architecture of this chip
+ */
+static void __inline__ fec_arch_init(void)
+{
+	return;
+}
+/*
+ * do some cleanup based architecture of this chip
+ */
+static void __inline__ fec_arch_exit(void)
+{
+	return;
+}
 
 /*
  *	Code specific to Coldfire 520x
@@ -1610,17 +1806,63 @@ static void __inline__ fec_phy_ack_intr(void)
 {
 }
 
-static void __inline__ fec_localhw_setup(void)
+static void __inline__ fec_localhw_setup(struct net_device *dev)
+{
+}
+
+/*
+ * invalidate dcache related with the virtual memory range(start, end)
+ */
+static void __inline__ fec_dcache_inv_range(void * start, void * end)
+{
+	return ;
+}
+
+/*
+ * flush dcache related with the virtual memory range(start, end)
+ */
+static void __inline__ fec_dcache_flush_range(void * start, void * end)
 {
+	return ;
 }
 
-static void __inline__ fec_uncache(unsigned long addr)
+/*
+ * map memory space (addr, addr+size) to uncachable erea.
+ */
+static unsigned long __inline__ fec_map_uncache(unsigned long addr, int size)
 {
+	return addr;
 }
 
+/*
+ * unmap memory erea started with addr from uncachable erea.
+ */
+static void __inline__ fec_unmap_uncache(void * addr)
+{
+	return ;
+}
+
+
 /* ------------------------------------------------------------------------- */
 
 #elif defined(CONFIG_M532x)
+
+/*
+ * do some initializtion based architecture of this chip
+ */
+static void __inline__ fec_arch_init(void)
+{
+	return;
+}
+
+/*
+ * do some cleanup based architecture of this chip
+ */
+static void __inline__ fec_arch_exit(void)
+{
+	return;
+}
+
 /*
  * Code specific for M532x
  */
@@ -1749,21 +1991,297 @@ static void __inline__ fec_phy_ack_intr(void)
 {
 }
 
-static void __inline__ fec_localhw_setup(void)
+static void __inline__ fec_localhw_setup(struct net_device *dev)
 {
 }
 
 /*
- *	Do not need to make region uncached on 532x.
+ * invalidate dcache related with the virtual memory range(start, end)
  */
-static void __inline__ fec_uncache(unsigned long addr)
+static void __inline__ fec_dcache_inv_range(void * start, void * end)
 {
+	return ;
+}
+
+/*
+ * flush dcache related with the virtual memory range(start, end)
+ */
+static void __inline__ fec_dcache_flush_range(void * start, void * end)
+{
+	return ;
+}
+
+/*
+ * map memory space (addr, addr+size) to uncachable erea.
+ */
+static unsigned long __inline__ fec_map_uncache(unsigned long addr, int size)
+{
+	return addr;
+}
+
+/*
+ * unmap memory erea started with addr from uncachable erea.
+ */
+static void __inline__ fec_unmap_uncache(void *  addr)
+{
+	return ;
 }
 
 /* ------------------------------------------------------------------------- */
 
+#elif defined(CONFIG_ARCH_MXC)
+
+extern void gpio_fec_active(void);
+extern void gpio_fec_inactive(void);
+extern unsigned int expio_intr_fec;
+
+/*
+ * do some initializtion based architecture of this chip
+ */
+static void __inline__ fec_arch_init(void)
+{
+	struct clk *clk;
+	gpio_fec_active();
+	clk = clk_get(NULL, "fec_clk");
+	clk_enable(clk);
+	clk_put(clk);
+	return;
+}
+/*
+ * do some cleanup based architecture of this chip
+ */
+static void __inline__ fec_arch_exit(void)
+{
+	struct clk *clk;
+	clk = clk_get(NULL, "fec_clk");
+	clk_disable(clk);
+	clk_put(clk);
+	gpio_fec_inactive();
+	return;
+}
+
+/*
+ * Code specific to Freescale i.MXC
+ */
+static void __inline__ fec_request_intrs(struct net_device *dev)
+{
+	/* Setup interrupt handlers. */
+	if (request_irq(MXC_INT_FEC, fec_enet_interrupt, 0, "fec", dev) != 0)
+		panic("FEC: Could not allocate FEC IRQ(%d)!\n", MXC_INT_FEC);
+	/* TODO: disable now due to CPLD issue */
+	if ((expio_intr_fec > 0) &&
+	(request_irq(expio_intr_fec, mii_link_interrupt, 0, "fec(MII)", dev) != 0))
+		panic("FEC: Could not allocate FEC(MII) IRQ(%d)!\n", expio_intr_fec);
+	disable_irq(expio_intr_fec);
+}
+
+static void __inline__ fec_set_mii(struct net_device *dev, struct fec_enet_private *fep)
+{
+	u32 rate;
+	struct clk *clk;
+	volatile fec_t *fecp;
+	fecp = fep->hwp;
+	fecp->fec_r_cntrl = OPT_FRAME_SIZE | 0x04;
+	fecp->fec_x_cntrl = 0x00;
+
+	/*
+	 * Set MII speed to 2.5 MHz
+	 */
+	clk = clk_get(NULL, "fec_clk");
+	rate = clk_get_rate(clk);
+	clk_put(clk);
+
+	fep->phy_speed =
+		((((rate / 2 + 4999999) / 2500000) / 2) & 0x3F) << 1;
+	fecp->fec_mii_speed = fep->phy_speed;
+	fec_restart(dev, 0);
+}
+
+#define FEC_IIM_BASE    IO_ADDRESS(IIM_BASE_ADDR)
+static void __inline__ fec_get_mac(struct net_device *dev)
+{
+	struct fec_enet_private *fep = netdev_priv(dev);
+	volatile fec_t *fecp;
+	unsigned char *iap, tmpaddr[ETH_ALEN];
+	int i;
+	unsigned long fec_mac_base = FEC_IIM_BASE + MXC_IIMKEY0;
+	fecp = fep->hwp;
+
+	if (fecp->fec_addr_low || fecp->fec_addr_high) {
+		*((unsigned long *) &tmpaddr[0]) =
+			be32_to_cpu(fecp->fec_addr_low);
+		*((unsigned short *) &tmpaddr[4]) =
+			be32_to_cpu(fecp->fec_addr_high);
+		iap = &tmpaddr[0];
+	} else {
+		if (cpu_is_mx27_rev(CHIP_REV_2_0) > 0)
+			fec_mac_base = FEC_IIM_BASE + MXC_IIMMAC;
+
+		memset(tmpaddr, 0, ETH_ALEN);
+		if (!(machine_is_mx35_3ds() || cpu_is_mx51())) {
+			/*
+			 * Get MAC address from IIM.
+			 * If it is all 1's or 0's, use the default.
+			 */
+			for (i = 0; i < ETH_ALEN; i++)
+				tmpaddr[ETH_ALEN-1-i] =
+				__raw_readb(fec_mac_base + i * 4);
+		}
+		iap = &tmpaddr[0];
+
+		if ((iap[0] == 0) && (iap[1] == 0) && (iap[2] == 0) &&
+			(iap[3] == 0) && (iap[4] == 0) && (iap[5] == 0))
+			iap = fec_mac_default;
+		if ((iap[0] == 0xff) && (iap[1] == 0xff) && (iap[2] == 0xff) &&
+		    (iap[3] == 0xff) && (iap[4] == 0xff) && (iap[5] == 0xff))
+			iap = fec_mac_default;
+	}
+
+        memcpy(dev->dev_addr, iap, ETH_ALEN);
+
+        /* Adjust MAC if using default MAC address */
+        if (iap == fec_mac_default)
+		dev->dev_addr[ETH_ALEN-1] = fec_mac_default[ETH_ALEN-1] + fep->index;
+}
+
+#ifndef MODULE
+static int fec_mac_setup(char *new_mac)
+{
+	char *ptr, *p = new_mac;
+	int i = 0;
+
+	while (p && (*p) && i < 6) {
+		ptr = strchr(p, ':');
+		if (ptr)
+			*ptr++ = '\0';
+
+		if (strlen(p)) {
+			unsigned long tmp = simple_strtoul(p, NULL, 16);
+			if (tmp > 0xff)
+				break;
+			fec_mac_default[i++] = tmp;
+		}
+		p = ptr;
+	}
+
+	return 0;
+}
+
+__setup("fec_mac=", fec_mac_setup);
+#endif
+
+static void __inline__ fec_enable_phy_intr(void)
+{
+	if (expio_intr_fec > 0)
+		enable_irq(expio_intr_fec);
+}
+
+static void __inline__ fec_disable_phy_intr(void)
+{
+	if (expio_intr_fec > 0)
+		disable_irq(expio_intr_fec);
+}
+
+static void __inline__ fec_phy_ack_intr(void)
+{
+	if (expio_intr_fec > 0)
+		disable_irq(expio_intr_fec);
+}
+
+#ifdef CONFIG_ARCH_MX25
+/*
+ * i.MX25 allows RMII mode to be configured via a gasket
+ */
+#define FEC_MIIGSK_CFGR_FRCONT (1 << 6)
+#define FEC_MIIGSK_CFGR_LBMODE (1 << 4)
+#define FEC_MIIGSK_CFGR_EMODE (1 << 3)
+#define FEC_MIIGSK_CFGR_IF_MODE_MASK (3 << 0)
+#define FEC_MIIGSK_CFGR_IF_MODE_MII (0 << 0)
+#define FEC_MIIGSK_CFGR_IF_MODE_RMII (1 << 0)
+
+#define FEC_MIIGSK_ENR_READY (1 << 2)
+#define FEC_MIIGSK_ENR_EN (1 << 1)
+
+static void __inline__ fec_localhw_setup(struct net_device *dev)
+{
+	struct fec_enet_private *fep = netdev_priv(dev);
+	volatile fec_t *fecp = fep->hwp;
+	/*
+	 * Set up the MII gasket for RMII mode
+	 */
+	printk("%s: enable RMII gasket\n", dev->name);
 
+	/* disable the gasket and wait */
+	fecp->fec_miigsk_enr = 0;
+	while (fecp->fec_miigsk_enr & FEC_MIIGSK_ENR_READY)
+		udelay(1);
+
+	/* configure the gasket for RMII, 50 MHz, no loopback, no echo */
+	fecp->fec_miigsk_cfgr = FEC_MIIGSK_CFGR_IF_MODE_RMII;
+
+	/* re-enable the gasket */
+	fecp->fec_miigsk_enr = FEC_MIIGSK_ENR_EN;
+}
 #else
+static void __inline__ fec_localhw_setup(struct net_device *dev)
+{
+}
+#endif
+
+/*
+ * invalidate dcache related with the virtual memory range(start, end)
+ */
+static void __inline__ fec_dcache_inv_range(void * start, void * end)
+{
+	dma_sync_single_for_device(NULL, (unsigned long)__pa(start),
+				   (unsigned long)(end - start),
+				   DMA_FROM_DEVICE);
+	return ;
+}
+
+/*
+ * flush dcache related with the virtual memory range(start, end)
+ */
+static void __inline__ fec_dcache_flush_range(void * start, void * end)
+{
+	dma_sync_single_for_device(NULL, (unsigned long)__pa(start),
+				   (unsigned long)(end - start), DMA_TO_DEVICE);
+	return ;
+}
+
+/*
+ * map memory space (addr, addr+size) to uncachable erea.
+ */
+static unsigned long __inline__  fec_map_uncache(unsigned long addr, int size)
+{
+	return (unsigned long)ioremap(__pa(addr), size);
+}
+
+/*
+ * unmap memory erea started with addr from uncachable erea.
+ */
+static void __inline__ fec_unmap_uncache(void * addr)
+{
+	return iounmap(addr);
+}
+
+/* ------------------------------------------------------------------------- */
+
+#else
+/*
+ * do some initializtion based architecture of this chip
+ */
+static void __inline__ fec_arch_init(void)
+{
+	return;
+}
+/*
+ * do some cleanup based architecture of this chip
+ */
+static void __inline__ fec_arch_exit(void)
+{
+	return;
+}
 
 /*
  *	Code specific to the MPC860T setup.
@@ -1831,7 +2349,7 @@ static void __inline__ fec_phy_ack_intr(void)
 {
 }
 
-static void __inline__ fec_localhw_setup(void)
+static void __inline__ fec_localhw_setup(struct net_device *dev)
 {
 	volatile fec_t *fecp;
 
@@ -1842,12 +2360,40 @@ static void __inline__ fec_localhw_setup(void)
 	fecp->fec_fun_code = 0x78000000;
 }
 
-static void __inline__ fec_uncache(unsigned long addr)
+/*
+ * invalidate dcache related with the virtual memory range(start, end)
+ */
+static void __inline__ fec_dcache_inv_range(void * start, void * end)
+{
+	return ;
+}
+
+/*
+ * flush dcache related with the virtual memory range(start, end)
+ */
+static void __inline__ fec_dcache_flush_range(void * start, void * end)
+{
+	return ;
+}
+
+/*
+ * map memory space (addr, addr+size) to uncachable erea.
+ */
+static unsigned long __inline__ fec_map_uncache(unsigned long addr, int size)
 {
 	pte_t *pte;
 	pte = va_to_pte(mem_addr);
 	pte_val(*pte) |= _PAGE_NO_CACHE;
 	flush_tlb_page(init_mm.mmap, mem_addr);
+	return addr;
+}
+
+/*
+ *  * unmap memory erea started with addr from uncachable erea.
+ *   */
+static void __inline__ fec_unmap_uncache(void *  addr)
+{
+	return ;
 }
 
 #endif
@@ -2073,10 +2619,14 @@ mii_link_interrupt(int irq, void * dev_id)
 #if 0
 	disable_irq(fep->mii_irq);  /* disable now, enable later */
 #endif
-
-	mii_do_cmd(dev, fep->phy->ack_int);
-	mii_do_cmd(dev, phy_cmd_relink);  /* restart and display status */
-
+        /*
+	 * Some board will trigger phy interrupt before phy enable.
+	 * And at that moment , fep->phy is not initialized.
+	 */
+	if (fep->phy) {
+		mii_do_cmd(dev, fep->phy->ack_int);
+		mii_do_cmd(dev, phy_cmd_relink);  /* restart and display status */
+	}
 	return IRQ_HANDLED;
 }
 #endif
@@ -2086,6 +2636,7 @@ fec_enet_open(struct net_device *dev)
 {
 	struct fec_enet_private *fep = netdev_priv(dev);
 
+	fec_arch_init();
 	/* I should reset the ring buffers here, but I don't yet know
 	 * a simple way to do that.
 	 */
@@ -2094,6 +2645,8 @@ fec_enet_open(struct net_device *dev)
 	fep->sequence_done = 0;
 	fep->link = 0;
 
+	fec_restart(dev, 1);
+
 	if (fep->phy) {
 		mii_do_cmd(dev, fep->phy->ack_int);
 		mii_do_cmd(dev, fep->phy->config);
@@ -2110,18 +2663,14 @@ fec_enet_open(struct net_device *dev)
 			schedule();
 
 		mii_do_cmd(dev, fep->phy->startup);
-
-		/* Set the initial link state to true. A lot of hardware
-		 * based on this device does not implement a PHY interrupt,
-		 * so we are never notified of link change.
-		 */
-		fep->link = 1;
-	} else {
-		fep->link = 1; /* lets just try it and see */
-		/* no phy,  go full duplex,  it's most likely a hub chip */
-		fec_restart(dev, 1);
 	}
 
+	/* Set the initial link state to true. A lot of hardware
+	 * based on this device does not implement a PHY interrupt,
+	 * so we are never notified of link change.
+	 */
+	fep->link = 1;
+
 	netif_start_queue(dev);
 	fep->opened = 1;
 	return 0;		/* Success */
@@ -2135,9 +2684,10 @@ fec_enet_close(struct net_device *dev)
 	/* Don't know what to do yet.
 	*/
 	fep->opened = 0;
-	netif_stop_queue(dev);
-	fec_stop(dev);
-
+	if (fep->link) {
+		fec_stop(dev);
+	}
+	fec_arch_exit();
 	return 0;
 }
 
@@ -2248,6 +2798,7 @@ int __init fec_enet_init(struct net_device *dev)
 	unsigned long	mem_addr;
 	volatile cbd_t	*bdp;
 	cbd_t		*cbd_base;
+	struct  sk_buff* pskb;
 	volatile fec_t	*fecp;
 	int 		i, j;
 	static int	index = 0;
@@ -2256,6 +2807,8 @@ int __init fec_enet_init(struct net_device *dev)
 	if (index >= FEC_MAX_PORTS)
 		return -ENXIO;
 
+	fep->net = dev;
+
 	/* Allocate memory for buffer descriptors.
 	*/
 	mem_addr = __get_free_page(GFP_KERNEL);
@@ -2264,6 +2817,7 @@ int __init fec_enet_init(struct net_device *dev)
 		return -ENOMEM;
 	}
 
+	fep->cbd_mem_base = (void *)mem_addr;
 	spin_lock_init(&fep->hw_lock);
 	spin_lock_init(&fep->mii_lock);
 
@@ -2288,10 +2842,14 @@ int __init fec_enet_init(struct net_device *dev)
 	 */
 	fec_get_mac(dev);
 
-	cbd_base = (cbd_t *)mem_addr;
-	/* XXX: missing check for allocation failure */
+	cbd_base = (cbd_t *)fec_map_uncache(mem_addr, PAGE_SIZE);
+	if (cbd_base == NULL) {
+		free_page(mem_addr);
+		printk("FEC: map descriptor memory to uncacheable failed?\n");
+		return -ENOMEM;
+	}
 
-	fec_uncache(mem_addr);
+	/* XXX: missing check for allocation failure */
 
 	/* Set receive and transmit descriptor base.
 	*/
@@ -2306,25 +2864,26 @@ int __init fec_enet_init(struct net_device *dev)
 	/* Initialize the receive buffer descriptors.
 	*/
 	bdp = fep->rx_bd_base;
-	for (i=0; i<FEC_ENET_RX_PAGES; i++) {
-
-		/* Allocate a page.
-		*/
-		mem_addr = __get_free_page(GFP_KERNEL);
-		/* XXX: missing check for allocation failure */
-
-		fec_uncache(mem_addr);
-
-		/* Initialize the BD for every fragment in the page.
-		*/
-		for (j=0; j<FEC_ENET_RX_FRPPG; j++) {
-			bdp->cbd_sc = BD_ENET_RX_EMPTY;
-			bdp->cbd_bufaddr = __pa(mem_addr);
-			mem_addr += FEC_ENET_RX_FRSIZE;
-			bdp++;
+	for (i=0; i<RX_RING_SIZE; i++,  bdp++) {
+		pskb = dev_alloc_skb(FEC_ENET_RX_FRSIZE);
+		if(pskb == NULL) {
+			for(; i>0; i--) {
+				if( fep->rx_skbuff[i-1] ) {
+					kfree_skb(fep->rx_skbuff[i-1]);
+					fep->rx_skbuff[i-1] = NULL;
+				}
+			}
+			printk("FEC: allocate skb fail when initializing rx buffer \n");
+			free_page(mem_addr);
+			return -ENOMEM;
 		}
+		fep->rx_skbuff[i] = pskb;
+		fec_dcache_inv_range(pskb->data, pskb->data +
+				     FEC_ENET_RX_FRSIZE);
+		pskb->data = FEC_ADDR_ALIGNMENT(pskb->data);
+		bdp->cbd_sc = BD_ENET_RX_EMPTY;
+		bdp->cbd_bufaddr = __pa(pskb->data);
 	}
-
 	/* Set the last buffer to wrap.
 	*/
 	bdp--;
@@ -2357,19 +2916,23 @@ int __init fec_enet_init(struct net_device *dev)
 
 	/* Set receive and transmit descriptor base.
 	*/
-	fecp->fec_r_des_start = __pa((uint)(fep->rx_bd_base));
-	fecp->fec_x_des_start = __pa((uint)(fep->tx_bd_base));
+	fecp->fec_r_des_start = __pa((uint)(fep->cbd_mem_base));
+	fecp->fec_x_des_start = __pa((uint)(fep->cbd_mem_base + RX_RING_SIZE*sizeof(cbd_t)));
 
 	/* Install our interrupt handlers. This varies depending on
 	 * the architecture.
 	*/
 	fec_request_intrs(dev);
 
+	/* Clear and enable interrupts */
+	fecp->fec_ievent = FEC_ENET_MASK;
+	fecp->fec_imask = FEC_ENET_TXF | FEC_ENET_TXB | FEC_ENET_RXF | FEC_ENET_RXB | FEC_ENET_MII;
+
 	fecp->fec_grp_hash_table_high = 0;
 	fecp->fec_grp_hash_table_low = 0;
 	fecp->fec_r_buff_size = PKT_MAXBLR_SIZE;
 	fecp->fec_ecntrl = 2;
-	fecp->fec_r_des_active = 0;
+	fecp->fec_r_des_active = 0x01000000;
 #ifndef CONFIG_M5272
 	fecp->fec_hash_table_high = 0;
 	fecp->fec_hash_table_low = 0;
@@ -2392,10 +2955,6 @@ int __init fec_enet_init(struct net_device *dev)
 	/* setup MII interface */
 	fec_set_mii(dev, fep);
 
-	/* Clear and enable interrupts */
-	fecp->fec_ievent = 0xffc00000;
-	fecp->fec_imask = (FEC_ENET_TXF | FEC_ENET_RXF | FEC_ENET_MII);
-
 	/* Queue up command to detect the PHY and initialize the
 	 * remainder of the interface.
 	 */
@@ -2427,9 +2986,15 @@ fec_restart(struct net_device *dev, int duplex)
 	fecp->fec_ecntrl = 1;
 	udelay(10);
 
+	/* Enable interrupts we wish to service.
+	 */
+	fecp->fec_imask = FEC_ENET_TXF | FEC_ENET_TXB | FEC_ENET_RXF | FEC_ENET_RXB | FEC_ENET_MII;
+
 	/* Clear any outstanding interrupt.
-	*/
-	fecp->fec_ievent = 0xffc00000;
+	 *
+	 */
+	fecp->fec_ievent = FEC_ENET_MASK;
+
 	fec_enable_phy_intr();
 
 	/* Set station address.
@@ -2445,12 +3010,12 @@ fec_restart(struct net_device *dev, int duplex)
 	*/
 	fecp->fec_r_buff_size = PKT_MAXBLR_SIZE;
 
-	fec_localhw_setup();
+	fec_localhw_setup(dev);
 
 	/* Set receive and transmit descriptor base.
 	*/
-	fecp->fec_r_des_start = __pa((uint)(fep->rx_bd_base));
-	fecp->fec_x_des_start = __pa((uint)(fep->tx_bd_base));
+	fecp->fec_r_des_start = __pa((uint)(fep->cbd_mem_base));
+	fecp->fec_x_des_start = __pa((uint)(fep->cbd_mem_base + RX_RING_SIZE*sizeof(cbd_t)));
 
 	fep->dirty_tx = fep->cur_tx = fep->tx_bd_base;
 	fep->cur_rx = fep->rx_bd_base;
@@ -2517,11 +3082,7 @@ fec_restart(struct net_device *dev, int duplex)
 	/* And last, enable the transmit and receive processing.
 	*/
 	fecp->fec_ecntrl = 2;
-	fecp->fec_r_des_active = 0;
-
-	/* Enable interrupts we wish to service.
-	*/
-	fecp->fec_imask = (FEC_ENET_TXF | FEC_ENET_RXF | FEC_ENET_MII);
+	fecp->fec_r_des_active = 0x01000000;
 }
 
 static void
@@ -2530,6 +3091,8 @@ fec_stop(struct net_device *dev)
 	volatile fec_t *fecp;
 	struct fec_enet_private *fep;
 
+	netif_stop_queue(dev);
+
 	fep = netdev_priv(dev);
 	fecp = fep->hwp;
 
@@ -2561,15 +3124,18 @@ fec_stop(struct net_device *dev)
 static int __init fec_enet_module_init(void)
 {
 	struct net_device *dev;
-	int i, err;
+	int i, err, ret = 0;
 	DECLARE_MAC_BUF(mac);
 
 	printk("FEC ENET Version 0.2\n");
+	fec_arch_init();
 
 	for (i = 0; (i < FEC_MAX_PORTS); i++) {
 		dev = alloc_etherdev(sizeof(struct fec_enet_private));
-		if (!dev)
-			return -ENOMEM;
+		if (!dev) {
+			ret = -ENOMEM;
+			goto exit;
+		}
 		err = fec_enet_init(dev);
 		if (err) {
 			free_netdev(dev);
@@ -2578,13 +3144,19 @@ static int __init fec_enet_module_init(void)
 		if (register_netdev(dev) != 0) {
 			/* XXX: missing cleanup here */
 			free_netdev(dev);
-			return -EIO;
+			ret = -EIO;
+			goto exit;
 		}
 
 		printk("%s: ethernet %s\n",
 		       dev->name, print_mac(mac, dev->dev_addr));
 	}
-	return 0;
+
+exit:
+	fec_arch_exit();
+	return ret;
+
+
 }
 
 module_init(fec_enet_module_init);
diff --git a/drivers/net/fec.h b/drivers/net/fec.h
index 292719daceff..cb21ef1a8052 100644
--- a/drivers/net/fec.h
+++ b/drivers/net/fec.h
@@ -14,7 +14,7 @@
 /****************************************************************************/
 
 #if defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
-    defined(CONFIG_M520x) || defined(CONFIG_M532x)
+    defined(CONFIG_M520x) || defined(CONFIG_M532x) || defined(CONFIG_ARCH_MXC)
 /*
  *	Just figures, Motorola would have to change the offsets for
  *	registers in the same peripheral device on different models
@@ -56,6 +56,10 @@ typedef struct fec {
 	unsigned long	fec_r_des_start;	/* Receive descriptor ring */
 	unsigned long	fec_x_des_start;	/* Transmit descriptor ring */
 	unsigned long	fec_r_buff_size;	/* Maximum receive buff size */
+	unsigned long	fec_reserved12[93];
+	unsigned long	fec_miigsk_cfgr;	/* MIIGSK config register */
+	unsigned long	fec_reserved13;
+	unsigned long	fec_miigsk_enr;		/* MIIGSK enable register */
 } fec_t;
 
 #else
@@ -103,12 +107,22 @@ typedef struct fec {
 /*
  *	Define the buffer descriptor structure.
  */
+/* Please see "Receive Buffer Descriptor Field Definitions" in Specification.
+ * It's LE.
+ */
+#ifdef CONFIG_ARCH_MXC
+typedef struct bufdesc {
+	unsigned short  cbd_datlen;             /* Data length */
+	unsigned short  cbd_sc;                 /* Control and status info */
+	unsigned long   cbd_bufaddr;            /* Buffer address */
+} cbd_t;
+#else
 typedef struct bufdesc {
 	unsigned short	cbd_sc;			/* Control and status info */
 	unsigned short	cbd_datlen;		/* Data length */
 	unsigned long	cbd_bufaddr;		/* Buffer address */
 } cbd_t;
-
+#endif
 
 /*
  *	The following definitions courtesy of commproc.h, which where
diff --git a/drivers/net/irda/Kconfig b/drivers/net/irda/Kconfig
index e6317557a531..e372f35ae038 100644
--- a/drivers/net/irda/Kconfig
+++ b/drivers/net/irda/Kconfig
@@ -342,5 +342,9 @@ config MCS_FIR
 	  To compile it as a module, choose M here: the module will be called
 	  mcs7780.
 
+config MXC_FIR
+	tristate "Freescale MXC FIR driver"
+	depends on ARCH_MXC && IRDA
+
 endmenu
 
diff --git a/drivers/net/irda/Makefile b/drivers/net/irda/Makefile
index 5d20fde32a24..d32839cf9e73 100644
--- a/drivers/net/irda/Makefile
+++ b/drivers/net/irda/Makefile
@@ -18,6 +18,7 @@ obj-$(CONFIG_VLSI_FIR)		+= vlsi_ir.o
 obj-$(CONFIG_VIA_FIR)		+= via-ircc.o
 obj-$(CONFIG_PXA_FICP)	        += pxaficp_ir.o
 obj-$(CONFIG_MCS_FIR)	        += mcs7780.o
+obj-$(CONFIG_MXC_FIR)	        += mxc_ir.o
 obj-$(CONFIG_AU1000_FIR)	+= au1k_ir.o
 # SIR drivers
 obj-$(CONFIG_IRTTY_SIR)		+= irtty-sir.o	sir-dev.o
diff --git a/drivers/net/irda/mxc_ir.c b/drivers/net/irda/mxc_ir.c
new file mode 100644
index 000000000000..7adcf5363078
--- /dev/null
+++ b/drivers/net/irda/mxc_ir.c
@@ -0,0 +1,1777 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * Based on sa1100_ir.c - Copyright 2000-2001 Russell King
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxc_ir.c
+ *
+ * @brief Driver for the Freescale Semiconductor MXC FIRI.
+ *
+ * This driver is based on drivers/net/irda/sa1100_ir.c, by Russell King.
+ *
+ * @ingroup FIRI
+ */
+
+/*
+ * Include Files
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/netdevice.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/clk.h>
+
+#include <net/irda/irda.h>
+#include <net/irda/wrapper.h>
+#include <net/irda/irda_device.h>
+
+#include <asm/irq.h>
+#include <asm/dma.h>
+#include <mach/hardware.h>
+#include <mach/mxc_uart.h>
+#include "mxc_ir.h"
+
+#define IS_SIR(mi)		( (mi)->speed <= 115200 )
+#define IS_MIR(mi)		( (mi)->speed < 4000000 && (mi)->speed >= 576000 )
+#define IS_FIR(mi)		( (mi)->speed >= 4000000 )
+
+#define SDMA_START_DELAY()	{  \
+				volatile int j,k;\
+				int i;\
+				for(i=0;i<10000;i++)\
+				k=j;\
+				}
+
+#define IRDA_FRAME_SIZE_LIMIT   2047
+#define UART_BUFF_SIZE 		14384
+
+#define UART4_UFCR_TXTL  	16
+#define UART4_UFCR_RXTL 	1
+
+#define FIRI_SDMA_TX
+#define FIRI_SDMA_RX
+
+/*!
+ * This structure is a way for the low level driver to define their own
+ * \b mxc_irda structure. This structure includes SK buffers, DMA buffers.
+ * and has other elements that are specifically required by this driver.
+ */
+struct mxc_irda {
+	/*!
+	 * This keeps track of device is running or not
+	 */
+	unsigned char open;
+
+	/*!
+	 * This holds current FIRI communication speed
+	 */
+	int speed;
+
+	/*!
+	 * This holds FIRI communication speed for next packet
+	 */
+	int newspeed;
+
+	/*!
+	 * SK buffer for transmitter
+	 */
+	struct sk_buff *txskb;
+
+	/*!
+	 * SK buffer for receiver
+	 */
+	struct sk_buff *rxskb;
+
+#ifdef FIRI_SDMA_RX
+	/*!
+	 * SK buffer for tasklet
+	 */
+	struct sk_buff *tskb;
+#endif
+
+	/*!
+	 * DMA address for transmitter
+	 */
+	dma_addr_t dma_rx_buff_phy;
+
+	/*!
+	 * DMA address for receiver
+	 */
+	dma_addr_t dma_tx_buff_phy;
+
+	/*!
+	 * DMA Transmit buffer length
+	 */
+	unsigned int dma_tx_buff_len;
+
+	/*!
+	 * DMA channel for transmitter
+	 */
+	int txdma_ch;
+
+	/*!
+	 * DMA channel for receiver
+	 */
+	int rxdma_ch;
+
+	/*!
+	 * IrDA network device statistics
+	 */
+	struct net_device_stats stats;
+
+	/*!
+	 * The device structure used to get FIRI information
+	 */
+	struct device *dev;
+
+	/*!
+	 * Resource structure for UART, which will maintain base addresses and IRQs.
+	 */
+	struct resource *uart_res;
+
+	/*!
+	 * Base address of UART, used in readl and writel.
+	 */
+	void *uart_base;
+
+	/*!
+	 * Resource structure for FIRI, which will maintain base addresses and IRQs.
+	 */
+	struct resource *firi_res;
+
+	/*!
+	 * Base address of FIRI, used in readl and writel.
+	 */
+	void *firi_base;
+
+	/*!
+	 * UART IRQ number.
+	 */
+	int uart_irq;
+
+	/*!
+	 * Second UART IRQ number in case the interrupt lines are not muxed.
+	 */
+	int uart_irq1;
+
+	/*!
+	 * UART clock needed for baud rate calculations
+	 */
+	struct clk *uart_clk;
+
+	/*!
+	 * UART clock needed for baud rate calculations
+	 */
+	unsigned long uart_clk_rate;
+
+	/*!
+	 * FIRI clock needed for baud rate calculations
+	 */
+	struct clk *firi_clk;
+
+	/*!
+	 * FIRI IRQ number.
+	 */
+	int firi_irq;
+
+	/*!
+	 * IrLAP layer instance
+	 */
+	struct irlap_cb *irlap;
+
+	/*!
+	 * Driver supported baudrate capabilities
+	 */
+	struct qos_info qos;
+
+	/*!
+	 * Temporary transmit buffer used by the driver
+	 */
+	iobuff_t tx_buff;
+
+	/*!
+	 * Temporary receive buffer used by the driver
+	 */
+	iobuff_t rx_buff;
+
+	/*!
+	 * Pointer to platform specific data structure.
+	 */
+	struct mxc_ir_platform_data *mxc_ir_plat;
+
+	/*!
+	 * This holds the power management status of this module.
+	 */
+	int suspend;
+
+};
+
+extern void gpio_firi_active(void *, unsigned int);
+extern void gpio_firi_inactive(void);
+extern void gpio_firi_init(void);
+
+void mxc_irda_firi_init(struct mxc_irda *si);
+#ifdef FIRI_SDMA_RX
+static void mxc_irda_fir_dma_rx_irq(void *id, int error_status,
+				    unsigned int count);
+#endif
+#ifdef FIRI_SDMA_TX
+static void mxc_irda_fir_dma_tx_irq(void *id, int error_status,
+				    unsigned int count);
+#endif
+
+/*!
+ * This function allocates and maps the receive buffer,
+ * unless it is already allocated.
+ *
+ * @param   si   FIRI device specific structure.
+ * @return  The function returns 0 on success and a non-zero value on
+ *          failure.
+ */
+static int mxc_irda_rx_alloc(struct mxc_irda *si)
+{
+#ifdef FIRI_SDMA_RX
+	mxc_dma_requestbuf_t dma_request;
+#endif
+	if (si->rxskb) {
+		return 0;
+	}
+
+	si->rxskb = alloc_skb(IRDA_FRAME_SIZE_LIMIT + 1, GFP_ATOMIC);
+
+	if (!si->rxskb) {
+		dev_err(si->dev, "mxc_ir: out of memory for RX SKB\n");
+		return -ENOMEM;
+	}
+
+	/*
+	 * Align any IP headers that may be contained
+	 * within the frame.
+	 */
+	skb_reserve(si->rxskb, 1);
+
+#ifdef FIRI_SDMA_RX
+	si->dma_rx_buff_phy =
+	    dma_map_single(si->dev, si->rxskb->data, IRDA_FRAME_SIZE_LIMIT,
+			   DMA_FROM_DEVICE);
+
+	dma_request.num_of_bytes = IRDA_FRAME_SIZE_LIMIT;
+	dma_request.dst_addr = si->dma_rx_buff_phy;
+	dma_request.src_addr = si->firi_res->start;
+
+	mxc_dma_config(si->rxdma_ch, &dma_request, 1, MXC_DMA_MODE_READ);
+#endif
+	return 0;
+}
+
+/*!
+ * This function is called to disable the FIRI dma
+ *
+ * @param   si  FIRI port specific structure.
+ */
+static void mxc_irda_disabledma(struct mxc_irda *si)
+{
+	/* Stop all DMA activity. */
+#ifdef FIRI_SDMA_TX
+	mxc_dma_disable(si->txdma_ch);
+#endif
+#ifdef FIRI_SDMA_RX
+	mxc_dma_disable(si->rxdma_ch);
+#endif
+}
+
+/*!
+ * This function is called to set the IrDA communications speed.
+ *
+ * @param   si     FIRI specific structure.
+ * @param   speed  new Speed to be configured for.
+ *
+ * @return  The function returns 0 on success and a non-zero value on
+ *          failure.
+ */
+static int mxc_irda_set_speed(struct mxc_irda *si, int speed)
+{
+	unsigned long flags;
+	int ret = 0;
+	unsigned int num, denom, baud;
+	unsigned int cr;
+
+	dev_dbg(si->dev, "speed:%d\n", speed);
+	switch (speed) {
+	case 9600:
+	case 19200:
+	case 38400:
+	case 57600:
+	case 115200:
+		dev_dbg(si->dev, "starting SIR\n");
+		baud = speed;
+		if (IS_FIR(si)) {
+#ifdef FIRI_SDMA_RX
+			mxc_dma_disable(si->rxdma_ch);
+#endif
+			cr = readl(si->firi_base + FIRITCR);
+			cr &= ~FIRITCR_TE;
+			writel(cr, si->firi_base + FIRITCR);
+
+			cr = readl(si->firi_base + FIRIRCR);
+			cr &= ~FIRIRCR_RE;
+			writel(cr, si->firi_base + FIRIRCR);
+
+		}
+		local_irq_save(flags);
+
+		/* Disable Tx and Rx */
+		cr = readl(si->uart_base + MXC_UARTUCR2);
+		cr &= ~(MXC_UARTUCR2_RXEN | MXC_UARTUCR2_TXEN);
+		writel(cr, si->uart_base + MXC_UARTUCR2);
+
+		gpio_firi_inactive();
+
+		num = baud / 100 - 1;
+		denom = si->uart_clk_rate / 1600 - 1;
+		if ((denom < 65536) && (si->uart_clk_rate > 1600)) {
+			writel(num, si->uart_base + MXC_UARTUBIR);
+			writel(denom, si->uart_base + MXC_UARTUBMR);
+		}
+
+		si->speed = speed;
+
+		writel(0xFFFF, si->uart_base + MXC_UARTUSR1);
+		writel(0xFFFF, si->uart_base + MXC_UARTUSR2);
+
+		/* Enable Receive Overrun and Data Ready interrupts. */
+		cr = readl(si->uart_base + MXC_UARTUCR4);
+		cr |= (MXC_UARTUCR4_OREN | MXC_UARTUCR4_DREN);
+		writel(cr, si->uart_base + MXC_UARTUCR4);
+
+		cr = readl(si->uart_base + MXC_UARTUCR2);
+		cr |= (MXC_UARTUCR2_RXEN | MXC_UARTUCR2_TXEN);
+		writel(cr, si->uart_base + MXC_UARTUCR2);
+
+		local_irq_restore(flags);
+		break;
+	case 4000000:
+		local_irq_save(flags);
+
+		/* Disable Receive Overrun and Data Ready interrupts. */
+		cr = readl(si->uart_base + MXC_UARTUCR4);
+		cr &= ~(MXC_UARTUCR4_OREN | MXC_UARTUCR4_DREN);
+		writel(cr, si->uart_base + MXC_UARTUCR4);
+
+		/* Disable Tx and Rx */
+		cr = readl(si->uart_base + MXC_UARTUCR2);
+		cr &= ~(MXC_UARTUCR2_RXEN | MXC_UARTUCR2_TXEN);
+		writel(cr, si->uart_base + MXC_UARTUCR2);
+
+		/*
+		 * FIR configuration
+		 */
+		mxc_irda_disabledma(si);
+
+		cr = readl(si->firi_base + FIRITCR);
+		cr &= ~FIRITCR_TE;
+		writel(cr, si->firi_base + FIRITCR);
+
+		gpio_firi_active(si->firi_base + FIRITCR, FIRITCR_TPP);
+
+		si->speed = speed;
+
+		cr = readl(si->firi_base + FIRIRCR);
+		cr |= FIRIRCR_RE;
+		writel(cr, si->firi_base + FIRIRCR);
+
+		dev_dbg(si->dev, "Going for fast IRDA ...\n");
+		ret = mxc_irda_rx_alloc(si);
+
+		/* clear RX status register */
+		writel(0xFFFF, si->firi_base + FIRIRSR);
+#ifdef FIRI_SDMA_RX
+		if (si->rxskb) {
+			mxc_dma_enable(si->rxdma_ch);
+		}
+#endif
+		local_irq_restore(flags);
+
+		break;
+	default:
+		dev_err(si->dev, "speed not supported by FIRI\n");
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * This function is called to set the IrDA communications speed.
+ *
+ * @param   si     FIRI specific structure.
+ *
+ * @return  The function returns 0 on success and a non-zero value on
+ *          failure.
+ */
+static inline int mxc_irda_fir_error(struct mxc_irda *si)
+{
+	struct sk_buff *skb = si->rxskb;
+	unsigned int dd_error, crc_error, overrun_error;
+	unsigned int sr;
+
+	if (!skb) {
+		dev_err(si->dev, "no skb!\n");
+		return -1;
+	}
+
+	sr = readl(si->firi_base + FIRIRSR);
+	dd_error = sr & FIRIRSR_DDE;
+	crc_error = sr & FIRIRSR_CRCE;
+	overrun_error = sr & FIRIRSR_RFO;
+
+	if (!(dd_error | crc_error | overrun_error)) {
+		return 0;
+	}
+	dev_err(si->dev, "dde,crce,rfo=%d,%d,%d.\n", dd_error, crc_error,
+		overrun_error);
+	si->stats.rx_errors++;
+	if (crc_error) {
+		si->stats.rx_crc_errors++;
+	}
+	if (dd_error) {
+		si->stats.rx_frame_errors++;
+	}
+	if (overrun_error) {
+		si->stats.rx_frame_errors++;
+	}
+	writel(sr, si->firi_base + FIRIRSR);
+
+	return -1;
+}
+
+#ifndef FIRI_SDMA_RX
+/*!
+ * FIR interrupt service routine to handle receive.
+ *
+ * @param   dev     pointer to the net_device structure
+ */
+void mxc_irda_fir_irq_rx(struct net_device *dev)
+{
+	struct mxc_irda *si = dev->priv;
+	struct sk_buff *skb = si->rxskb;
+	unsigned int sr, len;
+	int i;
+	unsigned char *p = skb->data;
+
+	/*
+	 * Deal with any receive errors.
+	 */
+	if (mxc_irda_fir_error(si) != 0) {
+		return;
+	}
+
+	sr = readl(si->firi_base + FIRIRSR);
+
+	if (!(sr & FIRIRSR_RPE)) {
+		return;
+	}
+
+	/*
+	 * Coming here indicates that fir rx packet has been successfully recieved.
+	 * And No error happened so far.
+	 */
+	writel(sr | FIRIRSR_RPE, si->firi_base + FIRIRSR);
+
+	len = (sr & FIRIRSR_RFP) >> 8;
+
+	/* 4 bytes of CRC */
+	len -= 4;
+
+	skb_put(skb, len);
+
+	for (i = 0; i < len; i++) {
+		*p++ = readb(si->firi_base + FIRIRXFIFO);
+	}
+
+	/* Discard the four CRC bytes */
+	for (i = 0; i < 4; i++) {
+		readb(si->firi_base + FIRIRXFIFO);
+	}
+
+	/*
+	 * Deal with the case of packet complete.
+	 */
+	skb->dev = dev;
+	skb->mac.raw = skb->data;
+	skb->protocol = htons(ETH_P_IRDA);
+	si->stats.rx_packets++;
+	si->stats.rx_bytes += len;
+	netif_rx(skb);
+
+	si->rxskb = NULL;
+	mxc_irda_rx_alloc(si);
+
+	writel(0xFFFF, si->firi_base + FIRIRSR);
+
+}
+#endif
+
+/*!
+ * FIR interrupt service routine to handle transmit.
+ *
+ * @param   dev     pointer to the net_device structure
+ */
+void mxc_irda_fir_irq_tx(struct net_device *dev)
+{
+	struct mxc_irda *si = dev->priv;
+	struct sk_buff *skb = si->txskb;
+	unsigned int cr, sr;
+
+	sr = readl(si->firi_base + FIRITSR);
+	writel(sr, si->firi_base + FIRITSR);
+
+	if (sr & FIRITSR_TC) {
+
+#ifdef FIRI_SDMA_TX
+		mxc_dma_disable(si->txdma_ch);
+#endif
+		cr = readl(si->firi_base + FIRITCR);
+		cr &= ~(FIRITCR_TCIE | FIRITCR_TE);
+		writel(cr, si->firi_base + FIRITCR);
+
+		if (si->newspeed) {
+			mxc_irda_set_speed(si, si->newspeed);
+			si->newspeed = 0;
+		}
+		si->txskb = NULL;
+
+		cr = readl(si->firi_base + FIRIRCR);
+		cr |= FIRIRCR_RE;
+		writel(cr, si->firi_base + FIRIRCR);
+
+		writel(0xFFFF, si->firi_base + FIRIRSR);
+		/*
+		 * Account and free the packet.
+		 */
+		if (skb) {
+#ifdef FIRI_SDMA_TX
+			dma_unmap_single(si->dev, si->dma_tx_buff_phy, skb->len,
+					 DMA_TO_DEVICE);
+#endif
+			si->stats.tx_packets++;
+			si->stats.tx_bytes += skb->len;
+			dev_kfree_skb_irq(skb);
+		}
+		/*
+		 * Make sure that the TX queue is available for sending
+		 * (for retries).  TX has priority over RX at all times.
+		 */
+		netif_wake_queue(dev);
+	}
+}
+
+/*!
+ * This is FIRI interrupt handler.
+ *
+ * @param   dev     pointer to the net_device structure
+ */
+void mxc_irda_fir_irq(struct net_device *dev)
+{
+	struct mxc_irda *si = dev->priv;
+	unsigned int sr1, sr2;
+
+	sr1 = readl(si->firi_base + FIRIRSR);
+	sr2 = readl(si->firi_base + FIRITSR);
+
+	if (sr2 & FIRITSR_TC)
+		mxc_irda_fir_irq_tx(dev);
+#ifndef FIRI_SDMA_RX
+	if (sr1 & (FIRIRSR_RPE | FIRIRSR_RFO))
+		mxc_irda_fir_irq_rx(dev);
+#endif
+
+}
+
+/*!
+ * This is the SIR transmit routine.
+ *
+ * @param   si     FIRI specific structure.
+ *
+ * @param   dev     pointer to the net_device structure
+ *
+ * @return  The function returns 0 on success and a non-zero value on
+ *          failure.
+ */
+static int mxc_irda_sir_txirq(struct mxc_irda *si, struct net_device *dev)
+{
+	unsigned int sr1, sr2, cr;
+	unsigned int status;
+
+	sr1 = readl(si->uart_base + MXC_UARTUSR1);
+	sr2 = readl(si->uart_base + MXC_UARTUSR2);
+	cr = readl(si->uart_base + MXC_UARTUCR2);
+
+	/*
+	 * Echo cancellation for IRDA Transmit chars
+	 * Disable the receiver and enable Transmit complete.
+	 */
+	cr &= ~MXC_UARTUCR2_RXEN;
+	writel(cr, si->uart_base + MXC_UARTUCR2);
+	cr = readl(si->uart_base + MXC_UARTUCR4);
+	cr |= MXC_UARTUCR4_TCEN;
+	writel(cr, si->uart_base + MXC_UARTUCR4);
+
+	while ((sr1 & MXC_UARTUSR1_TRDY) && si->tx_buff.len) {
+
+		writel(*si->tx_buff.data++, si->uart_base + MXC_UARTUTXD);
+		si->tx_buff.len -= 1;
+		sr1 = readl(si->uart_base + MXC_UARTUSR1);
+	}
+
+	if (si->tx_buff.len == 0) {
+		si->stats.tx_packets++;
+		si->stats.tx_bytes += si->tx_buff.data - si->tx_buff.head;
+
+		/*Yoohoo...we are done...Lets stop Tx */
+		cr = readl(si->uart_base + MXC_UARTUCR1);
+		cr &= ~MXC_UARTUCR1_TRDYEN;
+		writel(cr, si->uart_base + MXC_UARTUCR1);
+
+		do {
+			status = readl(si->uart_base + MXC_UARTUSR2);
+		} while (!(status & MXC_UARTUSR2_TXDC));
+
+		if (si->newspeed) {
+			mxc_irda_set_speed(si, si->newspeed);
+			si->newspeed = 0;
+		}
+		/* I'm hungry! */
+		netif_wake_queue(dev);
+
+		/* Is the transmit complete to reenable the receiver? */
+		if (status & MXC_UARTUSR2_TXDC) {
+
+			cr = readl(si->uart_base + MXC_UARTUCR2);
+			cr |= MXC_UARTUCR2_RXEN;
+			writel(cr, si->uart_base + MXC_UARTUCR2);
+			/* Disable the Transmit complete interrupt bit */
+			cr = readl(si->uart_base + MXC_UARTUCR4);
+			cr &= ~MXC_UARTUCR4_TCEN;
+			writel(cr, si->uart_base + MXC_UARTUCR4);
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * This is the SIR receive routine.
+ *
+ * @param   si     FIRI specific structure.
+ *
+ * @param   dev     pointer to the net_device structure
+ *
+ * @return  The function returns 0 on success and a non-zero value on
+ *          failure.
+ */
+static int mxc_irda_sir_rxirq(struct mxc_irda *si, struct net_device *dev)
+{
+	unsigned int data, status;
+	volatile unsigned int sr2;
+
+	sr2 = readl(si->uart_base + MXC_UARTUSR2);
+	while ((sr2 & MXC_UARTUSR2_RDR) == 1) {
+		data = readl(si->uart_base + MXC_UARTURXD);
+		status = data & 0xf400;
+		if (status & MXC_UARTURXD_ERR) {
+			dev_err(si->dev, "Receive an incorrect data =0x%x.\n",
+				data);
+			si->stats.rx_errors++;
+			if (status & MXC_UARTURXD_OVRRUN) {
+				si->stats.rx_fifo_errors++;
+				dev_err(si->dev, "Rx overrun.\n");
+			}
+			if (status & MXC_UARTURXD_FRMERR) {
+				si->stats.rx_frame_errors++;
+				dev_err(si->dev, "Rx frame error.\n");
+			}
+			if (status & MXC_UARTURXD_PRERR) {
+				dev_err(si->dev, "Rx parity error.\n");
+			}
+			/* Other: it is the Break char.
+			 * Do nothing for it. throw out the data.
+			 */
+			async_unwrap_char(dev, &si->stats, &si->rx_buff,
+					  (data & 0xFF));
+		} else {
+			/* It is correct data. */
+			data &= 0xFF;
+			async_unwrap_char(dev, &si->stats, &si->rx_buff, data);
+
+			dev->last_rx = jiffies;
+		}
+		sr2 = readl(si->uart_base + MXC_UARTUSR2);
+
+		writel(0xFFFF, si->uart_base + MXC_UARTUSR1);
+		writel(0xFFFF, si->uart_base + MXC_UARTUSR2);
+	}			/*while */
+	return 0;
+
+}
+
+static irqreturn_t mxc_irda_irq(int irq, void *dev_id)
+{
+	struct net_device *dev = dev_id;
+	struct mxc_irda *si = dev->priv;
+
+	if (IS_FIR(si)) {
+		mxc_irda_fir_irq(dev);
+		return IRQ_HANDLED;
+	}
+
+	if (readl(si->uart_base + MXC_UARTUCR2) & MXC_UARTUCR2_RXEN) {
+		mxc_irda_sir_rxirq(si, dev);
+	}
+	if ((readl(si->uart_base + MXC_UARTUCR1) & MXC_UARTUCR1_TRDYEN) &&
+	    (readl(si->uart_base + MXC_UARTUSR1) & MXC_UARTUSR1_TRDY)) {
+		mxc_irda_sir_txirq(si, dev);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mxc_irda_tx_irq(int irq, void *dev_id)
+{
+
+	struct net_device *dev = dev_id;
+	struct mxc_irda *si = dev->priv;
+
+	mxc_irda_sir_txirq(si, dev);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mxc_irda_rx_irq(int irq, void *dev_id)
+{
+
+	struct net_device *dev = dev_id;
+	struct mxc_irda *si = dev->priv;
+
+	/* Clear the aging timer bit */
+	writel(MXC_UARTUSR1_AGTIM, si->uart_base + MXC_UARTUSR1);
+
+	mxc_irda_sir_rxirq(si, dev);
+
+	return IRQ_HANDLED;
+}
+
+#ifdef FIRI_SDMA_RX
+struct tasklet_struct dma_rx_tasklet;
+
+static void mxc_irda_rx_task(unsigned long tparam)
+{
+	struct mxc_irda *si = (struct mxc_irda *)tparam;
+	struct sk_buff *lskb = si->tskb;
+
+	si->tskb = NULL;
+	if (lskb) {
+		lskb->mac_header = lskb->data;
+		lskb->protocol = htons(ETH_P_IRDA);
+		netif_rx(lskb);
+	}
+}
+
+/*!
+ * Receiver DMA callback routine.
+ *
+ * @param   id   pointer to network device structure
+ * @param   error_status   used to pass error status to this callback function
+ * @param   count   number of bytes received
+ */
+static void mxc_irda_fir_dma_rx_irq(void *id, int error_status,
+				    unsigned int count)
+{
+	struct net_device *dev = id;
+	struct mxc_irda *si = dev->priv;
+	struct sk_buff *skb = si->rxskb;
+	unsigned int cr;
+	unsigned int len;
+
+	cr = readl(si->firi_base + FIRIRCR);
+	cr &= ~FIRIRCR_RE;
+	writel(cr, si->firi_base + FIRIRCR);
+	cr = readl(si->firi_base + FIRIRCR);
+	cr |= FIRIRCR_RE;
+	writel(cr, si->firi_base + FIRIRCR);
+	len = count - 4;	/* remove 4 bytes for CRC */
+	skb_put(skb, len);
+	skb->dev = dev;
+	si->tskb = skb;
+	tasklet_schedule(&dma_rx_tasklet);
+
+	if (si->dma_rx_buff_phy != 0)
+		dma_unmap_single(si->dev, si->dma_rx_buff_phy,
+				 IRDA_FRAME_SIZE_LIMIT, DMA_FROM_DEVICE);
+
+	si->rxskb = NULL;
+	mxc_irda_rx_alloc(si);
+
+	SDMA_START_DELAY();
+	writel(0xFFFF, si->firi_base + FIRIRSR);
+
+	if (si->rxskb) {
+		mxc_dma_enable(si->rxdma_ch);
+	}
+}
+#endif
+
+#ifdef FIRI_SDMA_TX
+/*!
+ * This function is called by SDMA Interrupt Service Routine to indicate
+ * requested DMA transfer is completed.
+ *
+ * @param   id   pointer to network device structure
+ * @param   error_status   used to pass error status to this callback function
+ * @param   count   number of bytes sent
+ */
+static void mxc_irda_fir_dma_tx_irq(void *id, int error_status,
+				    unsigned int count)
+{
+	struct net_device *dev = id;
+	struct mxc_irda *si = dev->priv;
+
+	mxc_dma_disable(si->txdma_ch);
+}
+#endif
+
+/*!
+ * This function is called by Linux IrDA network subsystem to
+ * transmit the Infrared data packet. The TX DMA channel is configured
+ * to transfer SK buffer data to FIRI TX FIFO along with DMA transfer
+ * completion routine.
+ *
+ * @param   skb   The packet that is queued to be sent
+ * @param   dev   net_device structure.
+ *
+ * @return  The function returns 0 on success and a negative value on
+ *          failure.
+ */
+static int mxc_irda_hard_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct mxc_irda *si = dev->priv;
+	int speed = irda_get_next_speed(skb);
+	unsigned int cr;
+
+	/*
+	 * Does this packet contain a request to change the interface
+	 * speed?  If so, remember it until we complete the transmission
+	 * of this frame.
+	 */
+	if (speed != si->speed && speed != -1) {
+		si->newspeed = speed;
+	}
+
+	/* If this is an empty frame, we can bypass a lot. */
+	if (skb->len == 0) {
+		if (si->newspeed) {
+			si->newspeed = 0;
+			mxc_irda_set_speed(si, speed);
+		}
+		dev_kfree_skb(skb);
+		return 0;
+	}
+
+	/* We must not be transmitting... */
+	netif_stop_queue(dev);
+	if (IS_SIR(si)) {
+
+		si->tx_buff.data = si->tx_buff.head;
+		si->tx_buff.len = async_wrap_skb(skb, si->tx_buff.data,
+						 si->tx_buff.truesize);
+		cr = readl(si->uart_base + MXC_UARTUCR1);
+		cr |= MXC_UARTUCR1_TRDYEN;
+		writel(cr, si->uart_base + MXC_UARTUCR1);
+		dev_kfree_skb(skb);
+	} else {
+		unsigned int mtt = irda_get_mtt(skb);
+		unsigned char *p = skb->data;
+		unsigned int skb_len = skb->len;
+#ifdef FIRI_SDMA_TX
+		mxc_dma_requestbuf_t dma_request;
+#else
+		unsigned int i, sr;
+#endif
+
+		skb_len = skb_len + ((4 - (skb_len % 4)) % 4);
+
+		if (si->txskb) {
+			BUG();
+		}
+		si->txskb = skb;
+
+		/*
+		 * If we have a mean turn-around time, impose the specified
+		 * specified delay.  We could shorten this by timing from
+		 * the point we received the packet.
+		 */
+		if (mtt) {
+			udelay(mtt);
+		}
+
+		cr = readl(si->firi_base + FIRIRCR);
+		cr &= ~FIRIRCR_RE;
+		writel(cr, si->firi_base + FIRIRCR);
+
+		writel(skb->len - 1, si->firi_base + FIRITCTR);
+
+#ifdef FIRI_SDMA_TX
+		/*
+		 * Configure DMA Tx Channel for source and destination addresses,
+		 * Number of bytes in SK buffer to transfer and Transfer complete
+		 * callback function.
+		 */
+		si->dma_tx_buff_len = skb_len;
+		si->dma_tx_buff_phy =
+		    dma_map_single(si->dev, p, skb_len, DMA_TO_DEVICE);
+
+		dma_request.num_of_bytes = skb_len;
+		dma_request.dst_addr = si->firi_res->start + FIRITXFIFO;
+		dma_request.src_addr = si->dma_tx_buff_phy;
+
+		mxc_dma_config(si->txdma_ch, &dma_request, 1,
+			       MXC_DMA_MODE_WRITE);
+
+		mxc_dma_enable(si->txdma_ch);
+#endif
+		cr = readl(si->firi_base + FIRITCR);
+		cr |= FIRITCR_TCIE;
+		writel(cr, si->firi_base + FIRITCR);
+
+		cr |= FIRITCR_TE;
+		writel(cr, si->firi_base + FIRITCR);
+
+#ifndef FIRI_SDMA_TX
+		for (i = 0; i < skb->len;) {
+			sr = readl(si->firi_base + FIRITSR);
+			/* TFP = number of bytes in the TX FIFO for the
+			 * Transmitter
+			 * */
+			if ((sr >> 8) < 128) {
+				writeb(*p, si->firi_base + FIRITXFIFO);
+				p++;
+				i++;
+			}
+		}
+#endif
+	}
+
+	dev->trans_start = jiffies;
+	return 0;
+}
+
+/*!
+ * This function handles network interface ioctls passed to this driver..
+ *
+ * @param   dev   net device structure
+ * @param   ifreq user request data
+ * @param   cmd   command issued
+ *
+ * @return  The function returns 0 on success and a non-zero value on
+ *           failure.
+ */
+static int mxc_irda_ioctl(struct net_device *dev, struct ifreq *ifreq, int cmd)
+{
+	struct if_irda_req *rq = (struct if_irda_req *)ifreq;
+	struct mxc_irda *si = dev->priv;
+	int ret = -EOPNOTSUPP;
+
+	switch (cmd) {
+		/* This function will be used by IrLAP to change the speed */
+	case SIOCSBANDWIDTH:
+		dev_dbg(si->dev, "%s:with cmd SIOCSBANDWIDTH\n", __FUNCTION__);
+		if (capable(CAP_NET_ADMIN)) {
+			/*
+			 * We are unable to set the speed if the
+			 * device is not running.
+			 */
+			if (si->open) {
+				ret = mxc_irda_set_speed(si, rq->ifr_baudrate);
+			} else {
+				dev_err(si->dev, "mxc_ir_ioctl: SIOCSBANDWIDTH:\
+                                         !netif_running\n");
+				ret = 0;
+			}
+		}
+		break;
+	case SIOCSMEDIABUSY:
+		dev_dbg(si->dev, "%s:with cmd SIOCSMEDIABUSY\n", __FUNCTION__);
+		ret = -EPERM;
+		if (capable(CAP_NET_ADMIN)) {
+			irda_device_set_media_busy(dev, TRUE);
+			ret = 0;
+		}
+		break;
+	case SIOCGRECEIVING:
+		rq->ifr_receiving =
+		    IS_SIR(si) ? si->rx_buff.state != OUTSIDE_FRAME : 0;
+		ret = 0;
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+/*!
+ * Kernel interface routine to get current statistics of the device
+ * which includes the number bytes/packets transmitted/received,
+ * receive errors, CRC errors, framing errors etc.
+ *
+ * @param  dev  the net_device structure
+ *
+ * @return This function returns IrDA network statistics
+ */
+static struct net_device_stats *mxc_irda_stats(struct net_device *dev)
+{
+	struct mxc_irda *si = dev->priv;
+	return &si->stats;
+}
+
+/*!
+ * FIRI init function
+ *
+ * @param si  FIRI device specific structure.
+ */
+void mxc_irda_firi_init(struct mxc_irda *si)
+{
+	unsigned int firi_baud, osf = 6;
+	unsigned int tcr, rcr, cr;
+
+	si->firi_clk = clk_get(si->dev, "firi_clk");
+	firi_baud = clk_round_rate(si->firi_clk, 48004500);
+	if ((firi_baud < 47995500) ||
+	    (clk_set_rate(si->firi_clk, firi_baud) < 0)) {
+		dev_err(si->dev, "Unable to set FIR clock to 48MHz.\n");
+		return;
+	}
+	clk_enable(si->firi_clk);
+
+	writel(0xFFFF, si->firi_base + FIRITSR);
+	writel(0xFFFF, si->firi_base + FIRIRSR);
+	writel(0x00, si->firi_base + FIRITCR);
+	writel(0x00, si->firi_base + FIRIRCR);
+
+	/* set _BL & _OSF */
+	cr = (osf - 1) | (16 << 5);
+	writel(cr, si->firi_base + FIRICR);
+
+#ifdef FIRI_SDMA_TX
+	tcr =
+	    FIRITCR_TDT_FIR | FIRITCR_TM_FIR | FIRITCR_TCIE |
+	    FIRITCR_PCF | FIRITCR_PC;
+#else
+	tcr = FIRITCR_TM_FIR | FIRITCR_TCIE | FIRITCR_PCF | FIRITCR_PC;
+#endif
+
+#ifdef FIRI_SDMA_RX
+	rcr =
+	    FIRIRCR_RPEDE | FIRIRCR_RM_FIR | FIRIRCR_RDT_FIR |
+	    FIRIRCR_RPA | FIRIRCR_RPP;
+#else
+	rcr =
+	    FIRIRCR_RPEDE | FIRIRCR_RM_FIR | FIRIRCR_RDT_FIR | FIRIRCR_RPEIE |
+	    FIRIRCR_RPA | FIRIRCR_PAIE | FIRIRCR_RFOIE | FIRIRCR_RPP;
+#endif
+
+	writel(tcr, si->firi_base + FIRITCR);
+	writel(rcr, si->firi_base + FIRIRCR);
+	cr = 0;
+	writel(cr, si->firi_base + FIRITCTR);
+}
+
+/*!
+ * This function initialises the UART.
+ *
+ * @param   si  FIRI port specific structure.
+ *
+ * @return  The function returns 0 on success.
+ */
+static int mxc_irda_uart_init(struct mxc_irda *si)
+{
+	unsigned int per_clk;
+	unsigned int num, denom, baud, ufcr = 0;
+	unsigned int cr;
+	int d = 1;
+	int uart_ir_mux = 0;
+
+	if (si->mxc_ir_plat)
+		uart_ir_mux = si->mxc_ir_plat->uart_ir_mux;
+	/*
+	 * Clear Status Registers 1 and 2
+	 **/
+	writel(0xFFFF, si->uart_base + MXC_UARTUSR1);
+	writel(0xFFFF, si->uart_base + MXC_UARTUSR2);
+
+	/* Configure the IOMUX for the UART */
+	gpio_firi_init();
+
+	per_clk = clk_get_rate(si->uart_clk);
+	baud = per_clk / 16;
+	if (baud > 1500000) {
+		baud = 1500000;
+		d = per_clk / ((baud * 16) + 1000);
+		if (d > 6) {
+			d = 6;
+		}
+	}
+	clk_enable(si->uart_clk);
+
+	si->uart_clk_rate = per_clk / d;
+	writel(si->uart_clk_rate / 1000, si->uart_base + MXC_UARTONEMS);
+
+	writel(si->mxc_ir_plat->ir_rx_invert | MXC_UARTUCR4_IRSC,
+	       si->uart_base + MXC_UARTUCR4);
+
+	if (uart_ir_mux) {
+		writel(MXC_UARTUCR3_RXDMUXSEL | si->mxc_ir_plat->ir_tx_invert |
+		       MXC_UARTUCR3_DSR, si->uart_base + MXC_UARTUCR3);
+	} else {
+		writel(si->mxc_ir_plat->ir_tx_invert | MXC_UARTUCR3_DSR,
+		       si->uart_base + MXC_UARTUCR3);
+	}
+
+	writel(MXC_UARTUCR2_IRTS | MXC_UARTUCR2_CTS | MXC_UARTUCR2_WS |
+	       MXC_UARTUCR2_ATEN | MXC_UARTUCR2_TXEN | MXC_UARTUCR2_RXEN,
+	       si->uart_base + MXC_UARTUCR2);
+	/* Wait till we are out of software reset */
+	do {
+		cr = readl(si->uart_base + MXC_UARTUCR2);
+	} while (!(cr & MXC_UARTUCR2_SRST));
+
+	ufcr |= (UART4_UFCR_TXTL << MXC_UARTUFCR_TXTL_OFFSET) |
+	    ((6 - d) << MXC_UARTUFCR_RFDIV_OFFSET) | UART4_UFCR_RXTL;
+	writel(ufcr, si->uart_base + MXC_UARTUFCR);
+
+	writel(MXC_UARTUCR1_UARTEN | MXC_UARTUCR1_IREN,
+	       si->uart_base + MXC_UARTUCR1);
+
+	baud = 9600;
+	num = baud / 100 - 1;
+	denom = si->uart_clk_rate / 1600 - 1;
+
+	if ((denom < 65536) && (si->uart_clk_rate > 1600)) {
+		writel(num, si->uart_base + MXC_UARTUBIR);
+		writel(denom, si->uart_base + MXC_UARTUBMR);
+	}
+
+	writel(0x0000, si->uart_base + MXC_UARTUTS);
+	return 0;
+
+}
+
+/*!
+ * This function enables FIRI port.
+ *
+ * @param   si  FIRI port specific structure.
+ *
+ * @return  The function returns 0 on success and a non-zero value on
+ *          failure.
+ */
+static int mxc_irda_startup(struct mxc_irda *si)
+{
+	int ret = 0;
+
+	mxc_irda_uart_init(si);
+	mxc_irda_firi_init(si);
+
+	/* configure FIRI device for speed */
+	ret = mxc_irda_set_speed(si, si->speed = 9600);
+
+	return ret;
+}
+
+/*!
+ * When an ifconfig is issued which changes the device flag to include
+ * IFF_UP this function is called. It is only called when the change
+ * occurs, not when the interface remains up. The function grabs the interrupt
+ * resources and registers FIRI interrupt service routines, requests for DMA
+ * channels, configures the DMA channel. It then initializes  the IOMUX
+ * registers to configure the pins for FIRI signals and finally initializes the
+ * various FIRI registers and enables the port for reception.
+ *
+ * @param   dev   net device structure that is being opened
+ *
+ * @return  The function returns 0 for a successful open and non-zero value
+ *          on failure.
+ */
+static int mxc_irda_start(struct net_device *dev)
+{
+	struct mxc_irda *si = dev->priv;
+	int err;
+	int ints_muxed = 0;
+	mxc_dma_device_t dev_id = 0;
+
+	if (si->uart_irq == si->uart_irq1)
+		ints_muxed = 1;
+
+	si->speed = 9600;
+
+	if (si->uart_irq == si->firi_irq) {
+		err =
+		    request_irq(si->uart_irq, mxc_irda_irq, 0, dev->name, dev);
+		if (err) {
+			dev_err(si->dev, "%s:Failed to request the IRQ\n",
+				__FUNCTION__);
+			return err;
+		}
+		/*
+		 * The interrupt must remain disabled for now.
+		 */
+		disable_irq(si->uart_irq);
+	} else {
+		err =
+		    request_irq(si->firi_irq, mxc_irda_irq, 0, dev->name, dev);
+		if (err) {
+			dev_err(si->dev, "%s:Failed to request FIRI IRQ\n",
+				__FUNCTION__);
+			return err;
+		}
+		/*
+		 * The interrupt must remain disabled for now.
+		 */
+		disable_irq(si->firi_irq);
+		if (ints_muxed) {
+
+			err = request_irq(si->uart_irq, mxc_irda_irq, 0,
+					  dev->name, dev);
+			if (err) {
+				dev_err(si->dev,
+					"%s:Failed to request UART IRQ\n",
+					__FUNCTION__);
+				goto err_irq1;
+			}
+			/*
+			 * The interrupt must remain disabled for now.
+			 */
+			disable_irq(si->uart_irq);
+		} else {
+			err = request_irq(si->uart_irq, mxc_irda_tx_irq, 0,
+					  dev->name, dev);
+			if (err) {
+				dev_err(si->dev,
+					"%s:Failed to request UART IRQ\n",
+					__FUNCTION__);
+				goto err_irq1;
+			}
+			err = request_irq(si->uart_irq1, mxc_irda_rx_irq, 0,
+					  dev->name, dev);
+			if (err) {
+				dev_err(si->dev,
+					"%s:Failed to request UART1 IRQ\n",
+					__FUNCTION__);
+				goto err_irq2;
+			}
+			/*
+			 * The interrupts must remain disabled for now.
+			 */
+			disable_irq(si->uart_irq);
+			disable_irq(si->uart_irq1);
+		}
+	}
+#ifdef FIRI_SDMA_RX
+	dev_id = MXC_DMA_FIR_RX;
+	si->rxdma_ch = mxc_dma_request(dev_id, "MXC FIRI RX");
+	if (si->rxdma_ch < 0) {
+		dev_err(si->dev, "Cannot allocate FIR DMA channel\n");
+		goto err_rx_dma;
+	}
+	mxc_dma_callback_set(si->rxdma_ch, mxc_irda_fir_dma_rx_irq,
+			     (void *)dev_get_drvdata(si->dev));
+#endif
+#ifdef FIRI_SDMA_TX
+
+	dev_id = MXC_DMA_FIR_TX;
+	si->txdma_ch = mxc_dma_request(dev_id, "MXC FIRI TX");
+	if (si->txdma_ch < 0) {
+		dev_err(si->dev, "Cannot allocate FIR DMA channel\n");
+		goto err_tx_dma;
+	}
+	mxc_dma_callback_set(si->txdma_ch, mxc_irda_fir_dma_tx_irq,
+			     (void *)dev_get_drvdata(si->dev));
+#endif
+	/* Setup the serial port port for the initial speed. */
+	err = mxc_irda_startup(si);
+	if (err) {
+		goto err_startup;
+	}
+
+	/* Open a new IrLAP layer instance. */
+	si->irlap = irlap_open(dev, &si->qos, "mxc");
+	err = -ENOMEM;
+	if (!si->irlap) {
+		goto err_irlap;
+	}
+
+	/* Now enable the interrupt and start the queue */
+	si->open = 1;
+	si->suspend = 0;
+
+	if (si->uart_irq == si->firi_irq) {
+		enable_irq(si->uart_irq);
+	} else {
+		enable_irq(si->firi_irq);
+		if (ints_muxed == 1) {
+			enable_irq(si->uart_irq);
+		} else {
+			enable_irq(si->uart_irq);
+			enable_irq(si->uart_irq1);
+		}
+	}
+
+	netif_start_queue(dev);
+	return 0;
+
+      err_irlap:
+	si->open = 0;
+	mxc_irda_disabledma(si);
+      err_startup:
+#ifdef FIRI_SDMA_TX
+	mxc_dma_free(si->txdma_ch);
+      err_tx_dma:
+#endif
+#ifdef FIRI_SDMA_RX
+	mxc_dma_free(si->rxdma_ch);
+      err_rx_dma:
+#endif
+	if (si->uart_irq1 && !ints_muxed)
+		free_irq(si->uart_irq1, dev);
+      err_irq2:
+	if (si->uart_irq != si->firi_irq)
+		free_irq(si->uart_irq, dev);
+      err_irq1:
+	if (si->firi_irq)
+		free_irq(si->firi_irq, dev);
+	return err;
+}
+
+/*!
+ * This function is called when IFF_UP flag has been cleared by the user via
+ * the ifconfig irda0 down command. This function stops any further
+ * transmissions being queued, and then disables the interrupts.
+ * Finally it resets the device.
+ * @param   dev   the net_device structure
+ *
+ * @return  int   the function always returns 0 indicating a success.
+ */
+static int mxc_irda_stop(struct net_device *dev)
+{
+	struct mxc_irda *si = netdev_priv(dev);
+	unsigned long flags;
+
+	/* Stop IrLAP */
+	if (si->irlap) {
+		irlap_close(si->irlap);
+		si->irlap = NULL;
+	}
+
+	netif_stop_queue(dev);
+
+	/*Save flags and disable the FIRI interrupts.. */
+	if (si->open) {
+		local_irq_save(flags);
+		disable_irq(si->uart_irq);
+		free_irq(si->uart_irq, dev);
+		if (si->uart_irq != si->firi_irq) {
+			disable_irq(si->firi_irq);
+			free_irq(si->firi_irq, dev);
+			if (si->uart_irq1 != si->uart_irq) {
+				disable_irq(si->uart_irq1);
+				free_irq(si->uart_irq1, dev);
+			}
+		}
+		local_irq_restore(flags);
+		si->open = 0;
+	}
+#ifdef FIRI_SDMA_RX
+	if (si->rxdma_ch) {
+		mxc_dma_disable(si->rxdma_ch);
+		mxc_dma_free(si->rxdma_ch);
+		if (si->dma_rx_buff_phy) {
+			dma_unmap_single(si->dev, si->dma_rx_buff_phy,
+					 IRDA_FRAME_SIZE_LIMIT,
+					 DMA_FROM_DEVICE);
+			si->dma_rx_buff_phy = 0;
+		}
+		si->rxdma_ch = 0;
+	}
+	tasklet_kill(&dma_rx_tasklet);
+#endif
+#ifdef FIRI_SDMA_TX
+	if (si->txdma_ch) {
+		mxc_dma_disable(si->txdma_ch);
+		mxc_dma_free(si->txdma_ch);
+		if (si->dma_tx_buff_phy) {
+			dma_unmap_single(si->dev, si->dma_tx_buff_phy,
+					 si->dma_tx_buff_len, DMA_TO_DEVICE);
+			si->dma_tx_buff_phy = 0;
+		}
+		si->txdma_ch = 0;
+	}
+#endif
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/*!
+ * This function is called to put the FIRI in a low power state. Refer to the
+ * document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure used to give information on which FIRI
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_irda_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct mxc_irda *si = netdev_priv(ndev);
+	unsigned int cr;
+	unsigned long flags;
+
+	if (!si) {
+		return 0;
+	}
+	if (si->suspend == 1) {
+		dev_err(si->dev,
+			" suspend - Device is already suspended ... \n");
+		return 0;
+	}
+	if (si->open) {
+
+		netif_device_detach(ndev);
+		mxc_irda_disabledma(si);
+
+		/*Save flags and disable the FIRI interrupts.. */
+		local_irq_save(flags);
+		disable_irq(si->uart_irq);
+		if (si->uart_irq != si->firi_irq) {
+			disable_irq(si->firi_irq);
+			if (si->uart_irq != si->uart_irq1) {
+				disable_irq(si->uart_irq1);
+			}
+		}
+		local_irq_restore(flags);
+
+		/* Disable Tx and Rx and then disable the UART clock */
+		cr = readl(si->uart_base + MXC_UARTUCR2);
+		cr &= ~(MXC_UARTUCR2_TXEN | MXC_UARTUCR2_RXEN);
+		writel(cr, si->uart_base + MXC_UARTUCR2);
+		cr = readl(si->uart_base + MXC_UARTUCR1);
+		cr &= ~MXC_UARTUCR1_UARTEN;
+		writel(cr, si->uart_base + MXC_UARTUCR1);
+		clk_disable(si->uart_clk);
+
+		/*Disable Tx and Rx for FIRI and then disable the FIRI clock.. */
+		cr = readl(si->firi_base + FIRITCR);
+		cr &= ~FIRITCR_TE;
+		writel(cr, si->firi_base + FIRITCR);
+		cr = readl(si->firi_base + FIRIRCR);
+		cr &= ~FIRIRCR_RE;
+		writel(cr, si->firi_base + FIRIRCR);
+		clk_disable(si->firi_clk);
+
+		gpio_firi_inactive();
+
+		si->suspend = 1;
+		si->open = 0;
+	}
+	return 0;
+}
+
+/*!
+ * This function is called to bring the FIRI back from a low power state. Refer
+ * to the document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure used to give information on which FIRI
+ *                to resume
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_irda_resume(struct platform_device *pdev)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct mxc_irda *si = netdev_priv(ndev);
+	unsigned long flags;
+
+	if (!si) {
+		return 0;
+	}
+
+	if (si->suspend == 1 && !si->open) {
+
+		/*Initialise the UART first */
+		clk_enable(si->uart_clk);
+
+		/*Now init FIRI */
+		gpio_firi_active(si->firi_base + FIRITCR, FIRITCR_TPP);
+		mxc_irda_startup(si);
+
+		/* Enable the UART and FIRI interrupts.. */
+		local_irq_save(flags);
+		enable_irq(si->uart_irq);
+		if (si->uart_irq != si->firi_irq) {
+			enable_irq(si->firi_irq);
+			if (si->uart_irq != si->uart_irq1) {
+				enable_irq(si->uart_irq1);
+			}
+		}
+		local_irq_restore(flags);
+
+		/* Let the kernel know that we are alive and kicking.. */
+		netif_device_attach(ndev);
+
+		si->suspend = 0;
+		si->open = 1;
+	}
+	return 0;
+}
+#else
+#define mxc_irda_suspend NULL
+#define mxc_irda_resume  NULL
+#endif
+
+static int mxc_irda_init_iobuf(iobuff_t * io, int size)
+{
+	io->head = kmalloc(size, GFP_KERNEL | GFP_DMA);
+	if (io->head != NULL) {
+		io->truesize = size;
+		io->in_frame = FALSE;
+		io->state = OUTSIDE_FRAME;
+		io->data = io->head;
+	}
+	return io->head ? 0 : -ENOMEM;
+
+}
+
+/*!
+ * This function is called during the driver binding process.
+ * This function requests for memory, initializes net_device structure and
+ * registers with kernel.
+ *
+ * @param   pdev  the device structure used to store device specific
+ *                information that is used by the suspend, resume and remove
+ *                functions
+ *
+ * @return  The function returns 0 on success and a non-zero value on failure
+ */
+static int mxc_irda_probe(struct platform_device *pdev)
+{
+	struct net_device *dev;
+	struct mxc_irda *si;
+	struct resource *uart_res, *firi_res;
+	int uart_irq, firi_irq, uart_irq1;
+	unsigned int baudrate_mask = 0;
+	int err;
+
+	uart_res = &pdev->resource[0];
+	uart_irq = pdev->resource[1].start;
+
+	firi_res = &pdev->resource[2];
+	firi_irq = pdev->resource[3].start;
+
+	uart_irq1 = pdev->resource[4].start;
+
+	if (!uart_res || uart_irq == NO_IRQ || !firi_res || firi_irq == NO_IRQ) {
+		dev_err(&pdev->dev, "Unable to find resources\n");
+		return -ENXIO;
+	}
+
+	err =
+	    request_mem_region(uart_res->start, SZ_16K,
+			       "MXC_IRDA") ? 0 : -EBUSY;
+	if (err) {
+		dev_err(&pdev->dev, "Failed to request UART memory region\n");
+		return -ENOMEM;
+	}
+
+	err =
+	    request_mem_region(firi_res->start, SZ_16K,
+			       "MXC_IRDA") ? 0 : -EBUSY;
+	if (err) {
+		dev_err(&pdev->dev, "Failed to request FIRI  memory region\n");
+		goto err_mem_1;
+	}
+
+	dev = alloc_irdadev(sizeof(struct mxc_irda));
+	if (!dev) {
+		goto err_mem_2;
+	}
+
+	si = netdev_priv(dev);
+	si->dev = &pdev->dev;
+
+	si->mxc_ir_plat = pdev->dev.platform_data;
+	si->uart_clk = si->mxc_ir_plat->uart_clk;
+
+	si->uart_res = uart_res;
+	si->firi_res = firi_res;
+	si->uart_irq = uart_irq;
+	si->firi_irq = firi_irq;
+	si->uart_irq1 = uart_irq1;
+
+	si->uart_base = ioremap(uart_res->start, SZ_16K);
+	si->firi_base = ioremap(firi_res->start, SZ_16K);
+
+	if (!(si->uart_base || si->firi_base)) {
+		err = -ENOMEM;
+		goto err_mem_3;
+	}
+
+	/*
+	 * Initialise the SIR buffers
+	 */
+	err = mxc_irda_init_iobuf(&si->rx_buff, UART_BUFF_SIZE);
+	if (err) {
+		goto err_mem_4;
+	}
+
+	err = mxc_irda_init_iobuf(&si->tx_buff, UART_BUFF_SIZE);
+	if (err) {
+		goto err_mem_5;
+	}
+
+	dev->hard_start_xmit = mxc_irda_hard_xmit;
+	dev->open = mxc_irda_start;
+	dev->stop = mxc_irda_stop;
+	dev->do_ioctl = mxc_irda_ioctl;
+	dev->get_stats = mxc_irda_stats;
+
+	irda_init_max_qos_capabilies(&si->qos);
+
+	/*
+	 * We support
+	 * SIR(9600, 19200,38400, 57600 and 115200 bps)
+	 * FIR(4 Mbps)
+	 * Min Turn Time set to 1ms or greater.
+	 */
+	baudrate_mask |= IR_9600 | IR_19200 | IR_38400 | IR_57600 | IR_115200;
+	baudrate_mask |= IR_4000000 << 8;
+
+	si->qos.baud_rate.bits &= baudrate_mask;
+	si->qos.min_turn_time.bits = 0x7;
+
+	irda_qos_bits_to_value(&si->qos);
+
+#ifdef FIRI_SDMA_RX
+	si->tskb = NULL;
+	tasklet_init(&dma_rx_tasklet, mxc_irda_rx_task, (unsigned long)si);
+#endif
+	err = register_netdev(dev);
+	if (err == 0) {
+		platform_set_drvdata(pdev, dev);
+	} else {
+		kfree(si->tx_buff.head);
+	      err_mem_5:
+		kfree(si->rx_buff.head);
+	      err_mem_4:
+		iounmap(si->uart_base);
+		iounmap(si->firi_base);
+	      err_mem_3:
+		free_netdev(dev);
+	      err_mem_2:
+		release_mem_region(firi_res->start, SZ_16K);
+	      err_mem_1:
+		release_mem_region(uart_res->start, SZ_16K);
+	}
+	return err;
+}
+
+/*!
+ * Dissociates the driver from the FIRI device. Removes the appropriate FIRI
+ * port structure from the kernel.
+ *
+ * @param   pdev  the device structure used to give information on which FIRI
+ *                to remove
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_irda_remove(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct mxc_irda *si = netdev_priv(dev);
+
+	if (si->uart_base)
+		iounmap(si->uart_base);
+	if (si->firi_base)
+		iounmap(si->firi_base);
+	if (si->firi_res->start)
+		release_mem_region(si->firi_res->start, SZ_16K);
+	if (si->uart_res->start)
+		release_mem_region(si->uart_res->start, SZ_16K);
+	if (si->tx_buff.head)
+		kfree(si->tx_buff.head);
+	if (si->rx_buff.head)
+		kfree(si->rx_buff.head);
+
+	platform_set_drvdata(pdev, NULL);
+	unregister_netdev(dev);
+	free_netdev(dev);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcir_driver = {
+	.driver = {
+		   .name = "mxcir",
+		   },
+	.probe = mxc_irda_probe,
+	.remove = mxc_irda_remove,
+	.suspend = mxc_irda_suspend,
+	.resume = mxc_irda_resume,
+};
+
+/*!
+ * This function is used to initialize the FIRI driver module. The function
+ * registers the power management callback functions with the kernel and also
+ * registers the FIRI callback functions.
+ *
+ * @return  The function returns 0 on success and a non-zero value on failure.
+ */
+static int __init mxc_irda_init(void)
+{
+	return platform_driver_register(&mxcir_driver);
+}
+
+/*!
+ * This function is used to cleanup all resources before the driver exits.
+ */
+static void __exit mxc_irda_exit(void)
+{
+	platform_driver_unregister(&mxcir_driver);
+}
+
+module_init(mxc_irda_init);
+module_exit(mxc_irda_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor");
+MODULE_DESCRIPTION("MXC IrDA(SIR/FIR) driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/irda/mxc_ir.h b/drivers/net/irda/mxc_ir.h
new file mode 100644
index 000000000000..6b22ca129f27
--- /dev/null
+++ b/drivers/net/irda/mxc_ir.h
@@ -0,0 +1,133 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef __MXC_FIRI_REG_H__
+#define __MXC_FIRI_REG_H__
+
+#include <mach/hardware.h>
+
+/*!
+ * @defgroup FIRI Fast IR Driver
+ */
+
+/*!
+ * @file mxc_ir.h
+ *
+ * @brief  MXC FIRI header file
+ *
+ * This file defines base address and bits of FIRI registers
+ *
+ * @ingroup FIRI
+ */
+
+/*!
+ * FIRI maximum packet length
+ */
+#define FIR_MAX_RXLEN           2047
+
+/*
+ * FIRI Transmitter Control Register
+ */
+#define FIRITCR 0x00
+/*
+ * FIRI Transmitter Count Register
+ */
+#define FIRITCTR 0x04
+/*
+ * FIRI Receiver Control Register
+ */
+#define FIRIRCR 0x08
+/*
+ * FIRI Transmitter Status Register
+ */
+#define FIRITSR 0x0C
+/*
+ * FIRI Receiver Status Register
+ */
+#define FIRIRSR 0x10
+/*
+ * FIRI Transmitter FIFO
+ */
+#define FIRITXFIFO 0x14
+/*
+ * FIRI Receiver FIFO
+ */
+#define FIRIRXFIFO 0x18
+/*
+ * FIRI Control Register
+ */
+#define FIRICR 0x1C
+
+/*
+ * Bit definitions of Transmitter Controller Register
+ */
+#define FIRITCR_HAG     (1<<24)	/* H/W address generator */
+#define FIRITCR_SRF_FIR (0<<13)	/* Start field repeat factor */
+#define FIRITCR_SRF_MIR (1<<13)	/* Start field Repeat Factor */
+#define FIRITCR_TDT_MIR (2<<10)	/* TX trigger for MIR is set to 32 bytes) */
+#define FIRITCR_TDT_FIR (1<<10)	/* TX trigger for FIR is set to 16 bytes) */
+#define FIRITCR_TCIE    (1<<9)	/* TX Complete Interrupt Enable */
+#define FIRITCR_TPEIE   (1<<8)	/* TX Packet End Interrupt Enable */
+#define FIRITCR_TFUIE   (1<<7)	/* TX FIFO Under-run Interrupt Enable */
+#define FIRITCR_PCF     (1<<6)	/* Packet Complete by FIFO */
+#define FIRITCR_PC      (1<<5)	/* Packet Complete */
+#define FIRITCR_SIP     (1<<4)	/* TX Enable of SIP */
+#define FIRITCR_TPP     (1<<3)	/* TX Pulse Polarity bit */
+#define FIRITCR_TM_FIR  (0<<1)	/* TX Mode 4 Mbps */
+#define FIRITCR_TM_MIR1 (1<<1)	/* TX Mode 0.576 Mbps */
+#define FIRITCR_TM_MIR2 (1<<2)	/* TX Mode 1.152 Mbps */
+#define FIRITCR_TE      (1<<0)	/* TX Enable */
+
+/*
+ * Bit definitions of Transmitter Count Register
+ */
+#define FIRITCTR_TPL     511	/* TX Packet Length set to 512 bytes */
+
+/*
+ * Bit definitions of Receiver Control Register
+ */
+#define FIRIRCR_RAM     (1<<24)	/* RX Address Match */
+#define FIRIRCR_RPEDE   (1<<11)	/* Packet End DMA request Enable */
+#define FIRIRCR_RDT_MIR (2<<8)	/* DMA Trigger level(64 bytes in RXFIFO) */
+#define FIRIRCR_RDT_FIR (1<<8)	/* DMA Trigger level(16 bytes in RXFIFO) */
+#define FIRIRCR_RPA     (1<<7)	/* RX Packet Abort */
+#define FIRIRCR_RPEIE   (1<<6)	/* RX Packet End Interrupt Enable */
+#define FIRIRCR_PAIE    (1<<5)	/* Packet Abort Interrupt Enable */
+#define FIRIRCR_RFOIE   (1<<4)	/* RX FIFO Overrun Interrupt Enable */
+#define FIRIRCR_RPP     (1<<3)	/* RX Pulse Polarity bit */
+#define FIRIRCR_RM_FIR  (0<<1)	/* 4 Mbps */
+#define FIRIRCR_RM_MIR1 (1<<1)	/* 0.576 Mbps */
+#define FIRIRCR_RM_MIR2 (1<<2)	/* 1.152 Mbps */
+#define FIRIRCR_RE      (1<<0)	/* RX Enable */
+
+/* Transmitter Status Register */
+#define FIRITSR_TFP     0xFF00	/* Mask for available bytes in TX FIFO */
+#define FIRITSR_TC      (1<<3)	/* Transmit Complete bit */
+#define FIRITSR_SIPE    (1<<2)	/* SIP End bit */
+#define FIRITSR_TPE     (1<<1)	/* Transmit Packet End */
+#define FIRITSR_TFU     (1<<0)	/* TX FIFO Under-run */
+
+/* Receiver Status Register */
+#define FIRIRSR_RFP     0xFF00	/* mask for available bytes RX FIFO */
+#define FIRIRSR_PAS     (1<<5)	/* preamble search */
+#define FIRIRSR_RPE     (1<<4)	/* RX Packet End */
+#define FIRIRSR_RFO     (1<<3)	/* RX FIFO Overrun */
+#define FIRIRSR_BAM     (1<<2)	/* Broadcast Address Match */
+#define FIRIRSR_CRCE    (1<<1)	/* CRC error */
+#define FIRIRSR_DDE     (1<<0)	/* Address, control or data field error */
+
+/* FIRI Control Register */
+#define FIRICR_BL       (32<<5)	/* Burst Length is set to 32 */
+#define FIRICR_OSF      (0<<1)	/* Over Sampling Factor */
+
+#endif				/* __MXC_FIRI_REG_H__ */
diff --git a/drivers/net/ns9xxx-eth.c b/drivers/net/ns9xxx-eth.c
new file mode 100755
index 000000000000..4fc84c7a6d41
--- /dev/null
+++ b/drivers/net/ns9xxx-eth.c
@@ -0,0 +1,1499 @@
+/*
+ * drivers/net/ns9xxx-eth.c
+ *
+ * Copyright (C) 2007,2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/* open issues
+ *  - use phy irq
+ *  - VLAN
+ *  - clk_enable only at open time?
+ *  - PM for hibernate
+ */
+#include <linux/clk.h>
+#include <linux/crc32.h>
+#include <linux/etherdevice.h>
+#include <linux/mii.h>
+#include <linux/ns9xxx-eth.h>
+#include <linux/phy.h>
+#include <linux/platform_device.h>
+#include <linux/timer.h>
+#include <asm/gpio.h>
+
+#define DRIVER_NAME "ns9xxx-eth"
+
+#define ETH_EGCR1	0x0000
+#define ETH_EGCR1_ERX		(1 << 31)
+#define ETH_EGCR1_ERXDMA	(1 << 30)
+#define ETH_EGCR1_ERXSHT	(1 << 28)
+#define ETH_EGCR1_ETX		(1 << 23)
+#define ETH_EGCR1_ETXDMA	(1 << 22)
+#define ETH_EGCR1_ERXINIT	(1 << 19)
+#define ETH_EGCR1_PM		(3 << 14)
+#define ETH_EGCR1_PM_MII		0
+#define ETH_EGCR1_MACHRST	(1 <<  9)
+#define ETH_EGCR1_ITXA		(1 <<  8)
+#define ETH_EGCR1_MB1		(1 << 21)
+
+#define ETH_EGCR2	0x0004
+/* TCLER is different comparing the ns9360 vs ns921x */
+/* FIXME TCLER should be used differently per processor */
+#define ETH_EGCR2_TCLER_NS921X	(1 <<  7)
+#define ETH_EGCR2_TCLER_NS9360	(1 <<  3)
+#define ETH_EGCR2_TCLER		(1 <<  7)
+#define ETH_EGCR2_TKICK		(1 <<  3)
+#define ETH_EGCR2_AUTOZ		(1 <<  2)
+#define ETH_EGCR2_CLRCNT	(1 <<  1)
+#define ETH_EGCR2_STEN		(1 <<  0)
+
+#define ETH_EGSR	0x0008
+#define ETH_EGSR_RXINIT		(1 << 20)
+
+#define ETH_TS		0x0018
+#define ETH_TS_OK		( 1 << 15)
+#define ETH_TS_BR		( 1 << 14)
+#define ETH_TS_MC		( 1 << 13)
+#define ETH_TS_AL		( 1 << 12)
+#define ETH_TS_AED		( 1 << 11)
+#define ETH_TS_AEC		( 1 << 10)
+#define ETH_TS_AUR		( 1 <<  9)
+#define ETH_TS_AJ		( 1 <<  8)
+#define ETH_TS_DEF		( 1 <<  6)
+#define ETH_TS_CRC		( 1 <<  5)
+#define ETH_TS_COLC		(15 <<  0)
+
+#define ETH_MAC1	0x0400
+#define ETH_MAC1_SRST		(1 << 15)
+#define ETH_MAC1_RXEN		(1 <<  0)
+
+#define ETH_MAC2	0x0404
+#define ETH_MAC2_PADEN		(1 <<  5)
+#define ETH_MAC2_CRCEN		(1 <<  4)
+#define ETH_MAC2_FULLD		(1 <<  0)
+
+#define ETH_B2BIPG	0x0408
+
+#define ETH_EPSR	0x0418
+#define ETH_EPSR_SPEED_MASK	(1 << 8)
+#define ETH_EPSR_SPEED_100	(1 << 8)
+#define ETH_EPSR_SPEED_10	(0 << 8)
+
+
+#define ETH_MIIMCFG	0x0420
+#define ETH_MIIMCFG_RMIIM	(1 << 15)
+#define ETH_MIIMCFG_CLKS	(7 <<  2)
+#define ETH_MIIMCFG_CLKS_DIV40	(7 <<  2)
+
+#define ETH_MIIMCMD	0x0424
+#define ETH_MIIMCMD_READ	(1 <<  0)
+#define ETH_MIIMADDR	0x0428
+#define ETH_MIIMWD	0x042c
+#define ETH_MIIMRD	0x0430
+#define ETH_MIIMIR	0x0434
+#define ETH_MIIMIR_LF		(1 <<  3)
+#define ETH_MIIMIR_BUSY		(1 <<  0)
+
+#define ETH_SA1		0x0440
+#define ETH_SA2		0x0444
+#define ETH_SA3		0x0448
+
+#define ETH_SAF		0x0500
+#define ETH_SAF_PRO		(1 <<  3)
+#define ETH_SAF_PRM		(1 <<  2)
+#define ETH_SAF_PRA		(1 <<  1)
+#define ETH_SAF_BROAD		(1 <<  0)
+
+#define ETH_HT1		0x0504
+#define ETH_HT2		0x0508
+
+#define ETH_STAT_TR64	0x0680
+#define ETH_STAT_TR127	0x0684
+#define ETH_STAT_TR255	0x0688
+#define ETH_STAT_TR511	0x068C
+#define ETH_STAT_TR1K	0x0690
+#define ETH_STAT_TRMAX	0x0694
+#define ETH_STAT_TRMGV	0x0698
+#define ETH_STAT_RBYT	0x069C
+#define ETH_STAT_RPKT	0x06A0
+#define ETH_STAT_RFCS	0x06A4
+#define ETH_STAT_RMCA	0x06A8
+#define ETH_STAT_RBCA	0x06AC
+#define ETH_STAT_RXCF	0x06B0
+#define ETH_STAT_RXPF	0x06B4
+#define ETH_STAT_RXUO	0x06B8
+#define ETH_STAT_RALN	0x06BC
+#define ETH_STAT_RFLR	0x06C0
+#define ETH_STAT_RCDE	0x06C4
+#define ETH_STAT_RCSE	0x06C8
+#define ETH_STAT_RUND	0x06CC
+#define ETH_STAT_ROVR	0x06D0
+#define ETH_STAT_RFRG	0x06D4
+#define ETH_STAT_RJBR	0x06D8
+#define ETH_STAT_TBYT	0x06E0
+#define ETH_STAT_TPKT	0x06E4
+#define ETH_STAT_TMCA	0x06E8
+#define ETH_STAT_TBCA	0x06EC
+#define ETH_STAT_TDFR	0x06F4
+#define ETH_STAT_TEDF	0x06F8
+#define ETH_STAT_TSCL	0x06FC
+#define ETH_STAT_TMCL	0x0700
+#define ETH_STAT_TLCL	0x0704
+#define ETH_STAT_TXCL	0x0708
+#define ETH_STAT_TNCL	0x070C
+#define ETH_STAT_TJBR	0x0718
+#define ETH_STAT_TFCS	0x071C
+#define ETH_STAT_TOVR	0x0724
+#define ETH_STAT_TUND	0x0728
+#define ETH_STAT_TFRG	0x072C
+
+#define ETH_RXABDP	0x0a00
+#define ETH_RXBBDP	0x0a04
+#define ETH_RXCBDP	0x0a08
+#define ETH_RXDBDP	0x0a0c
+
+#define ETH_IS		0x0a10
+#define ETH_IS_RXOVFLDATA	(1 << 25)
+#define ETH_IS_RXOVFLSTAT	(1 << 24)
+#define ETH_IS_RXDONEA		(1 << 22)
+#define ETH_IS_RXDONEB		(1 << 21)
+#define ETH_IS_RXDONEC		(1 << 20)
+#define ETH_IS_RXDONED		(1 << 19)
+#define ETH_IS_RXNOBUF		(1 << 18)
+#define ETH_IS_RXBUFFUL		(1 << 17)
+#define ETH_IS_RXBR		(1 << 16)
+#define ETH_IS_TXDONE		(1 <<  2)
+#define ETH_IS_TXERR		(1 <<  1)
+#define ETH_IS_TXIDLE		(1 <<  0)
+
+#define ETH_IE		0x0a14
+#define ETH_IE_RXOVFLDATA	(1 << 25)
+#define ETH_IE_RXOVFLSTAT	(1 << 24)
+#define ETH_IE_RXDONEA		(1 << 22)
+#define ETH_IE_RXDONEB		(1 << 21)
+#define ETH_IE_RXDONEC		(1 << 20)
+#define ETH_IE_RXDONED		(1 << 19)
+#define ETH_IE_RXNOBUF		(1 << 18)
+#define ETH_IE_RXBUFFUL		(1 << 17)
+#define ETH_IE_RXBR		(1 << 16)
+#define ETH_IE_TXDONE		(1 <<  2)
+#define ETH_IE_TXERR		(1 <<  1)
+#define ETH_IE_TXIDLE		(1 <<  0)
+
+#define	ETH_RX_IRQS		(ETH_IE_RXOVFLDATA | \
+				 ETH_IE_RXDONEA | \
+				 ETH_IE_RXDONEB | \
+				 ETH_IE_RXDONEC | \
+				 ETH_IE_RXDONED | \
+				 ETH_IE_RXNOBUF | \
+				 ETH_IE_RXBUFFUL)
+
+#define	ETH_TX_IRQS		(ETH_IE_TXDONE | ETH_IE_TXERR)
+
+#define ETH_TXBDP	0x0a18
+
+#define ETH_TRBDP	0x0a1c
+
+#define ETH_RXFREE	0x0a3c
+#define ETH_RXFREE_A		(1 <<  0)
+
+#define ETH_TXBDR	0x1000
+
+/* hardware limits sets from ethernet controller */
+#define	HW_RX_RINGS		(4)
+#define	MAX_ETH_FRAME_LEN	(1522)
+
+/* software limits sets by driver */
+#define TOTAL_NR_TXDESC		(64)
+#define NR_RXDESC_PER_RING	(45)
+#define TOTAL_NR_RXDESC		(HW_RX_RINGS*NR_RXDESC_PER_RING) /* total number of descriptors */
+
+/* masks for DMA descriptors handling */
+#define DMADESC_WRAP		(1 << 15)
+#define DMADESC_INTR		(1 << 14)
+#define DMATXDESC_LAST		(1 << 13)
+#define DMARXDESC_EN		(1 << 13)
+#define DMADESC_FULL		(1 << 12)
+
+#ifdef CONFIG_GPIO_ETH_ACTIVITY_LED
+ #define ACTIVITYLED_TOGGLE_TIMEOUT	2	/* in jiffies */
+ #define ACTIVITYLED_OFF_TIMEOUT	20	/* in jiffies */
+ unsigned int rxtx_activity = 0;
+ struct timer_list activityled_timer;
+#endif
+
+union ns9xxx_dma_desc {
+	struct {
+		u32 source;
+		u16 len;
+		u16 reserved;
+		u32 dest;
+		u16 status;
+		u16 flags;
+	};
+	u32 data[4];
+};
+
+struct ns9xxx_eth_priv {
+	unsigned char __iomem *membase;
+	resource_size_t mapbase;
+	spinlock_t lock;
+	struct net_device_stats stats;
+	struct clk *clk;
+	unsigned int irqtx;
+	unsigned int irqrx;
+#ifdef CONFIG_GPIO_ETH_ACTIVITY_LED
+	unsigned int activityled;
+#endif
+
+	/* phy management */
+	int lastlink;
+	int lastduplex;
+	int lastspeed;
+	struct mii_bus *mdiobus;
+	struct phy_device *phy;
+
+	/* rx stuff */
+	struct sk_buff **rxskb;
+	union ns9xxx_dma_desc *rxdesc;
+	dma_addr_t rxdeschandle;
+
+	/* tx stuff */
+	struct sk_buff **txskb;
+	u16 txbusy;
+	u16 txfree;
+
+	/* work to recover from rx stall condition that can happen at 100 Mbps HD */
+	struct delayed_work recover_from_rx_stall;
+};
+
+static inline u32 ethread32(struct net_device *dev, unsigned int offset)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	u32 ret = ioread32(priv->membase + offset);
+
+	dev_vdbg(&dev->dev, "read  0x%p -> 0x%08x\n",
+			priv->membase + offset, ret);
+
+	return ret;
+}
+
+static inline void ethwrite32(struct net_device *dev,
+		u32 value, unsigned int offset)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+
+	dev_vdbg(&dev->dev, "write 0x%p <- 0x%08x\n",
+			priv->membase + offset, value);
+	iowrite32(value, priv->membase + offset);
+}
+
+static inline void ethupdate32(struct net_device *dev,
+			       u32 and, u32 or, unsigned int offset)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	u32 reg;
+
+	reg = ioread32(priv->membase + offset) & and;
+	iowrite32(reg | or, priv->membase + offset);
+	dev_vdbg(&dev->dev, "update 0x%p <- 0x%08x\n",
+		 priv->membase + offset, reg | or);
+}
+
+#ifdef CONFIG_GPIO_ETH_ACTIVITY_LED
+static void toggle_activityled(void)
+{
+	/* set activity flag */
+	rxtx_activity = 1;
+	/* run timer if not already running */
+	if(!timer_pending(&activityled_timer))
+		mod_timer(&activityled_timer, jiffies + ACTIVITYLED_TOGGLE_TIMEOUT);
+}
+
+static void activityled_timer_fn(unsigned long gpio)
+{
+	static int cnt = 0;
+
+	if(rxtx_activity) {
+		/* toggle RX/TX Ethernet activity LED */
+		gpio_set_value(gpio, !gpio_get_value(gpio));
+		mod_timer(&activityled_timer, jiffies + ACTIVITYLED_TOGGLE_TIMEOUT);
+		cnt = 0;
+		rxtx_activity = 0;
+	} else {
+		if(cnt++ < ACTIVITYLED_OFF_TIMEOUT / ACTIVITYLED_TOGGLE_TIMEOUT)
+			mod_timer(&activityled_timer, jiffies + ACTIVITYLED_TOGGLE_TIMEOUT);
+		else {
+			gpio_set_value(gpio, 1); /* switch LED off  */
+			cnt = 0;
+		}
+	}
+
+}
+#endif
+
+static int ns9xxx_eth_miibus_pollbusy(struct mii_bus *bus)
+{
+	unsigned int timeout = 0x3000;
+	struct net_device *dev = bus->priv;
+
+	while (timeout--) {
+		u32 miimir;
+
+		miimir = ethread32(dev, ETH_MIIMIR);
+
+		if (!(miimir & ETH_MIIMIR_BUSY))
+			break;
+
+		cpu_relax();
+	}
+
+	return timeout ? 0 : -EBUSY;
+}
+
+static int ns9xxx_eth_mdiobus_read(struct mii_bus *bus, int phyid, int regnum)
+{
+	struct net_device *dev = bus->priv;
+	int ret;
+
+	if ((phyid & ~0x1f) || (regnum & ~0x1f))
+		return -EINVAL;
+
+	ret = ns9xxx_eth_miibus_pollbusy(bus);
+	if (ret)
+		goto out;
+
+	ethwrite32(dev, phyid << 8 | regnum, ETH_MIIMADDR);
+	ethwrite32(dev, 0, ETH_MIIMCMD);
+	ethwrite32(dev, ETH_MIIMCMD_READ, ETH_MIIMCMD);
+
+	ret = ns9xxx_eth_miibus_pollbusy(bus);
+	if (ret)
+		goto out;
+
+	ret = ethread32(dev, ETH_MIIMRD) & 0xffff;
+
+out:
+	dev_vdbg(bus->parent, "%s,  phyid = %d, regnum = %d -> %04x\n",
+			__func__, phyid, regnum, ret);
+
+	return ret;
+}
+
+static int ns9xxx_eth_mdiobus_write(struct mii_bus *bus,
+		int phyid, int regnum, u16 val)
+{
+	struct net_device *dev = bus->priv;
+	int ret;
+
+	if ((phyid & ~0x1f) || (regnum & ~0x1f))
+		return -EINVAL;
+
+	ret = ns9xxx_eth_miibus_pollbusy(bus);
+	if (ret)
+		goto out;
+
+	ethwrite32(dev, phyid << 8 | regnum, ETH_MIIMADDR);
+	ethwrite32(dev, val, ETH_MIIMWD);
+
+	ret = ns9xxx_eth_miibus_pollbusy(bus);
+
+out:
+	dev_vdbg(bus->parent, "%s: phyid = %d, regnum = %d, val = %04hx -> %04x\n",
+			__func__, phyid, regnum, val, ret);
+
+	return ret;
+}
+
+static int ns9xxx_eth_mdiobus_reset(struct mii_bus *bus)
+{
+	struct net_device *dev = bus->priv;
+
+	dev_dbg(bus->parent, "%s\n", __func__);
+
+	ethwrite32(dev, ETH_MIIMCFG_RMIIM, ETH_MIIMCFG);
+
+	/* TODO: currently the biggest divider (40) is used.  This could be
+	 * tuned depending on the PHY.  phylib doesn't provide the needed
+	 * information, though :-( */
+	ethwrite32(dev, ETH_MIIMCFG_CLKS_DIV40, ETH_MIIMCFG);
+
+	return 0;
+}
+
+static inline int ns9xxx_eth_create_skbuff(struct net_device* dev, const int descr)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	struct sk_buff *skb;
+	int ret;
+
+	skb = dev_alloc_skb(MAX_ETH_FRAME_LEN);
+	if (likely(skb)) {
+		priv->rxskb[descr] = skb;
+		skb->dev = dev;
+
+		priv->rxdesc[descr].source = dma_map_single(&dev->dev, skb->data,
+				skb->len, DMA_FROM_DEVICE);
+
+		priv->rxdesc[descr].len = MAX_ETH_FRAME_LEN;
+		priv->rxdesc[descr].flags = DMADESC_INTR | DMARXDESC_EN |
+			(descr == (NR_RXDESC_PER_RING - 1) ? DMADESC_WRAP : 0);
+		ret = 0;
+	} else {
+		printk(KERN_ERR "%s: out of memory\n", __func__);
+		priv->rxdesc[descr].flags = 0;
+		ret = -ENOMEM;
+	}
+
+	return ret;
+}
+
+static inline void ns9xxx_eth_rx_process_ring(struct net_device* dev, unsigned int ring)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	union ns9xxx_dma_desc *desc;
+	unsigned int i, endring;
+
+	endring = (ring + 1) * NR_RXDESC_PER_RING;
+	desc = &priv->rxdesc[ring * NR_RXDESC_PER_RING];
+
+	for (i = ring * NR_RXDESC_PER_RING; i < endring; i++, desc++) {
+		if (desc->flags & DMADESC_FULL) {
+			struct sk_buff *skb;
+			skb = dev_alloc_skb(MAX_ETH_FRAME_LEN);
+			if (likely(skb)) {
+				skb_reserve(skb, 2);	/* 16 byte IP header align */
+				memcpy(skb_put(skb, desc->len), (unsigned char *)priv->rxskb[i]->data,
+				desc->len);
+				skb->protocol = eth_type_trans(skb, dev);
+				priv->stats.rx_packets++;
+				priv->stats.rx_bytes += desc->len;
+				dev->last_rx = jiffies;
+				netif_rx(skb);
+			} else {
+				printk(KERN_ERR "%s: out of memory, dropping packet\n", __func__);
+				dev->stats.rx_dropped++;
+			}
+			desc->len = MAX_ETH_FRAME_LEN;
+			desc->flags = DMADESC_INTR | DMARXDESC_EN | (i == (NR_RXDESC_PER_RING - 1) ? DMADESC_WRAP : 0);
+		}
+	}
+}
+
+static irqreturn_t ns9xxx_eth_rx_int(int irq, void *dev_id)
+{
+	struct net_device *dev = dev_id;
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	unsigned long flags;
+	u32 is, rxdonemask, ring;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	is = ethread32(dev, ETH_IS);
+	/* Acknowledge interrupts */
+	ethwrite32(dev, is & ETH_RX_IRQS, ETH_IS);
+	dev_vdbg(&dev->dev, "%s: ETH_IS=%08x\n", __func__, is);
+
+	if (is & ETH_IS_RXOVFLDATA) {
+		if (!(is & (ETH_IS_RXDONEA | ETH_IS_RXDONEB |
+		    ETH_IS_RXDONEC | ETH_IS_RXDONEA))) {
+			/* The ETH_IS_RXOVFLDATA bit is set, then the receiver front
+			 * end has apparently locked up. We schedule a work that resets
+			 * the interface. We check the DONE bits to try to empty the
+			 * receive rings of packets before the receiver reset. Note
+			 * that once we get into this lockup state, the ETH_IS_RXOVFLDATA
+			 * interrupt will happen continously until we reset the receiver.
+			 */
+			ethupdate32(dev, ~(ETH_EGCR1_ERX | ETH_EGCR1_ERXDMA), 0, ETH_EGCR1);
+			ethupdate32(dev, ~ETH_MAC1_RXEN, 0, ETH_MAC1);
+			ethupdate32(dev, ~ETH_IE_RXOVFLDATA, 0, ETH_IE);
+
+			schedule_delayed_work(&priv->recover_from_rx_stall, 0);
+		}
+	}
+
+	if (is & (ETH_IS_RXNOBUF | ETH_IS_RXBUFFUL)) {
+		priv->stats.rx_dropped++;
+	}
+
+	for (ring = 0, rxdonemask = ETH_IS_RXDONEA; ring < HW_RX_RINGS; ring++) {
+		if (is & rxdonemask) {
+			ns9xxx_eth_rx_process_ring(dev, ring);
+			ethwrite32(dev, 1 << ring, ETH_RXFREE);
+		}
+		rxdonemask >>= 1;
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+#ifdef CONFIG_GPIO_ETH_ACTIVITY_LED
+	toggle_activityled();
+#endif
+	return IRQ_HANDLED;
+}
+
+static void ns9xxx_eth_recover_from_rx_stall(struct work_struct *work)
+{
+	struct ns9xxx_eth_priv *priv =
+	    container_of(work, struct ns9xxx_eth_priv,
+			 recover_from_rx_stall.work);
+
+	struct net_device *dev =
+	    container_of((void *)priv, struct net_device, priv);
+	unsigned long flags;
+	int i, timeout = 20;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	for (i = 0; i < HW_RX_RINGS; i++) {
+		ethwrite32(dev, priv->rxdeschandle + (i * NR_RXDESC_PER_RING),
+			   ETH_RXABDP + (i * 4));
+	}
+
+	ethupdate32(dev, 0xffffffff, ETH_EGSR_RXINIT, ETH_EGSR);
+	ethupdate32(dev, 0xffffffff, ETH_EGCR1_ERX, ETH_EGCR1);
+	ethupdate32(dev, 0xffffffff, ETH_EGCR1_ERXINIT, ETH_EGCR1);
+
+	while (!(ethread32(dev, ETH_EGSR) & ETH_EGSR_RXINIT) && timeout--)
+		udelay(1);
+
+	ethupdate32(dev, ~ETH_EGCR1_ERXINIT, 0, ETH_EGCR1);
+
+	/* Re-enable the overflow interrupt */
+	ethupdate32(dev, 0xffffffff, ETH_IE_RXOVFLDATA, ETH_IE);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static inline int ns9xxx_eth_num_txbusy(struct net_device *dev)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+
+	return (TOTAL_NR_TXDESC + priv->txfree - priv->txbusy) %
+		TOTAL_NR_TXDESC;
+}
+
+static inline int ns9xxx_eth_num_txfree(struct net_device *dev)
+{
+	return TOTAL_NR_TXDESC - ns9xxx_eth_num_txbusy(dev);
+}
+
+static void ns9xxx_eth_read_txdesc(struct net_device *dev,
+		union ns9xxx_dma_desc *txbuffer)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	int i;
+
+	for (i = 0; i < 4; ++i)
+		txbuffer->data[i] = ethread32(dev,
+				ETH_TXBDR + 16 * priv->txbusy + 4 * i);
+}
+
+static void ns9xxx_eth_start_tx_dma(struct net_device *dev)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	u32 egcr2, ie;
+	u32 start;
+
+	dev_vdbg(&dev->dev, "%s\n", __func__);
+
+	dev->trans_start = jiffies;
+
+	/* XXX: really kick TCLER? */
+
+	egcr2 = ethread32(dev, ETH_EGCR2);
+	if (egcr2 & (ETH_EGCR2_TCLER | ETH_EGCR2_TKICK)) {
+		egcr2 &= ~(ETH_EGCR2_TCLER | ETH_EGCR2_TKICK);
+		ethwrite32(dev, egcr2, ETH_EGCR2);
+	}
+
+	start = 4 * priv->txbusy;
+
+	ie = ethread32(dev, ETH_IE);
+	if ((ie & ETH_TX_IRQS) == 0) {
+		u32 egcr1 = ethread32(dev, ETH_EGCR1);
+		ethwrite32(dev, start, ETH_TXBDP);
+
+		ethwrite32(dev, ie | ETH_TX_IRQS, ETH_IE);
+		ethwrite32(dev, egcr1 | ETH_EGCR1_ETXDMA, ETH_EGCR1);
+	} else
+		ethwrite32(dev, start, ETH_TRBDP);
+
+	egcr2 |= ETH_EGCR2_TCLER | ETH_EGCR2_TKICK;
+	ethwrite32(dev, egcr2, ETH_EGCR2);
+}
+
+static irqreturn_t ns9xxx_eth_tx_int(int irq, void *dev_id)
+{
+	struct net_device *dev = dev_id;
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	u32 is;
+	struct sk_buff *skb;
+	union ns9xxx_dma_desc txbuffer;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	is = ethread32(dev, ETH_IS);
+	dev_vdbg(&dev->dev, "%s: ETH_IS=%08x\n", __func__, is);
+
+	/* ack */
+	ethwrite32(dev, is & (ETH_IS_TXDONE | ETH_IS_TXERR), ETH_IS);
+
+	while (1) {
+		if (!ns9xxx_eth_num_txbusy(dev))
+			break;
+
+		skb = priv->txskb[priv->txbusy];
+
+		ns9xxx_eth_read_txdesc(dev, &txbuffer);
+
+		if (txbuffer.flags & DMADESC_FULL)
+			break;
+
+		dma_unmap_single(&dev->dev, txbuffer.source,
+				skb->len, DMA_TO_DEVICE);
+
+		priv->txbusy = (priv->txbusy + 1) % TOTAL_NR_TXDESC;
+
+		if (txbuffer.status & ETH_TS_OK) {
+			priv->stats.tx_packets++;
+			priv->stats.tx_bytes += skb->len;
+		} else {
+			priv->stats.tx_errors++;
+
+			/* XXX: fill in tx_aborted_errors etc. */
+			/* kick TCLER? */
+		}
+
+		dev_kfree_skb_irq(skb);
+	}
+
+	if (ns9xxx_eth_num_txbusy(dev) &&
+			(is & ETH_IS_TXIDLE))
+		ns9xxx_eth_start_tx_dma(dev);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+#ifdef CONFIG_GPIO_ETH_ACTIVITY_LED
+	toggle_activityled();
+#endif
+
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static void ns9xxx_eth_netpoll(struct net_device *dev)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	unsigned long flags;
+
+	local_irq_save(flags);
+	ns9xxx_eth_rx_int(priv->irqrx, dev);
+	ns9xxx_eth_tx_int(priv->irqtx, dev);
+	local_irq_restore(flags);
+}
+#endif
+
+static void ns9xxx_eth_adjust_link(struct net_device *dev)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	struct phy_device *phydev = priv->phy;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if ((phydev->link != priv->lastlink) ||
+	    (priv->lastspeed != phydev->speed) ||
+	    (priv->lastduplex != phydev->duplex)){
+
+		if (phydev->link) {
+			u32 mac2 = ethread32(dev, ETH_MAC2);
+			u32 epsr = ethread32(dev, ETH_EPSR);
+
+			/* Adjsut speed */
+			epsr &= ~ETH_EPSR_SPEED_MASK;
+			epsr |= (phydev->speed == SPEED_100) ?
+				ETH_EPSR_SPEED_100 : 0;
+			ethwrite32(dev, epsr, ETH_EPSR);
+			priv->lastspeed = phydev->speed;
+
+			/* Adjsut duplex */
+			mac2 &= ~ETH_MAC2_FULLD;
+			mac2 |= phydev->duplex ? ETH_MAC2_FULLD : 0;
+			ethwrite32(dev, mac2, ETH_MAC2);
+			ethwrite32(dev, phydev->duplex ? 0x15 : 0x12,
+				   ETH_B2BIPG);
+			priv->lastduplex = phydev->duplex;
+
+			dev_info(&dev->dev, "link up (%d/%s)\n", phydev->speed,
+				 (phydev->duplex == DUPLEX_FULL) ?
+				 "full" : "half");
+		} else {
+			/* link down */
+			priv->lastspeed = 0;
+			priv->lastduplex = -1;
+			dev_info(&dev->dev, "link down\n");
+		}
+		priv->lastlink = phydev->link;
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static int ns9xxx_eth_hwinit(struct net_device *dev)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	u32 egcr1, ie;
+	int timeout, i;
+
+	dev_dbg(&dev->dev, "%s\n", __func__);
+
+	/* disable everything */
+	ethwrite32(dev, ETH_EGCR1_PM_MII, ETH_EGCR1);
+	ethwrite32(dev, 0, ETH_IE);
+
+	/* ack any pending irq */
+	ethwrite32(dev, ethread32(dev, ETH_IS), ETH_IS);
+
+	/* program station address */
+	ethwrite32(dev, dev->dev_addr[0] | dev->dev_addr[1] << 8, ETH_SA3);
+	ethwrite32(dev, dev->dev_addr[2] | dev->dev_addr[3] << 8, ETH_SA2);
+	ethwrite32(dev, dev->dev_addr[4] | dev->dev_addr[5] << 8, ETH_SA1);
+
+	for (i = 0; i <  HW_RX_RINGS; i++) {
+		ethwrite32(dev, priv->rxdeschandle + (i * NR_RXDESC_PER_RING),
+			   ETH_RXABDP + (i * 4));
+	}
+
+	ethwrite32(dev, ETH_SAF_BROAD, ETH_SAF);
+
+	egcr1 = ETH_EGCR1_ERX | ETH_EGCR1_ERXDMA | ETH_EGCR1_ETX |
+		ETH_EGCR1_PM_MII | ETH_EGCR1_ITXA | ETH_EGCR1_MB1;
+	ethwrite32(dev, egcr1 | ETH_EGCR1_ERXINIT, ETH_EGCR1);
+
+	timeout = 6;
+	while (!(ethread32(dev, ETH_EGSR) & ETH_EGSR_RXINIT) && timeout--)
+		udelay(1);
+
+	if (!timeout)
+		return -EBUSY;
+
+	ethwrite32(dev, ETH_EGSR_RXINIT, ETH_EGSR);
+	ethwrite32(dev, egcr1, ETH_EGCR1);
+
+	ethwrite32(dev, ETH_MAC1_RXEN, ETH_MAC1);
+	ethwrite32(dev, ETH_MAC2_CRCEN | ETH_MAC2_PADEN, ETH_MAC2);
+	ethwrite32(dev, 0x12, ETH_B2BIPG);
+
+	/* clear and enable statistics */
+	ethupdate32(dev, 0xffffffff, ETH_EGCR2_CLRCNT, ETH_EGCR2);
+	ethupdate32(dev, ~ETH_EGCR2_CLRCNT, 0, ETH_EGCR2);
+	ethupdate32(dev, 0xffffffff, ETH_EGCR2_AUTOZ | ETH_EGCR2_STEN, ETH_EGCR2);
+
+	ie = ethread32(dev, ETH_IE);
+	ethwrite32(dev, ie | ETH_RX_IRQS, ETH_IE);
+
+	ethwrite32(dev, 0xf, ETH_RXFREE);
+	return 0;
+}
+
+static void ns9xxx_eth_hwdisable(struct net_device *dev)
+{
+	dev_dbg(&dev->dev, "%s\n", __func__);
+
+	ethwrite32(dev, 0, ETH_EGCR1);
+	ethwrite32(dev, 0, ETH_IE);
+	ethwrite32(dev, 0, ETH_MAC1);
+	ethwrite32(dev, 0, ETH_MAC2);
+}
+
+static int ns9xxx_eth_open(struct net_device *dev)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	struct resource *res;
+	int ret = -ENOMEM;
+	int i;
+
+	dev_dbg(&dev->dev, "%s\n", __func__);
+
+	res = request_mem_region(priv->mapbase, 0x2800, DRIVER_NAME);
+	if (!res) {
+		dev_dbg(&dev->dev, "%s: err_request_mem\n", __func__);
+		goto err_request_mem;
+	}
+
+	phy_start(priv->phy);
+
+	priv->txfree = 1;
+	priv->txbusy = 1;
+
+	priv->rxdesc = dma_alloc_coherent(&dev->dev,
+			sizeof(*priv->rxdesc) * TOTAL_NR_RXDESC,
+			&priv->rxdeschandle, GFP_KERNEL);
+	if (!priv->rxdesc) {
+		dev_dbg(&dev->dev, "%s: err_alloc_rxdesc\n", __func__);
+		ret = -ENOMEM;
+		goto err_alloc_rxdesc;
+	}
+
+	priv->rxskb = kmalloc(sizeof(*priv->rxskb) * TOTAL_NR_RXDESC,
+			GFP_KERNEL);
+	if (!priv->rxskb) {
+		dev_dbg(&dev->dev, "%s: err_alloc_rxskb\n", __func__);
+		goto err_alloc_rxskb;
+	}
+
+	for (i = 0; i < TOTAL_NR_RXDESC; ++i) {
+		ret = ns9xxx_eth_create_skbuff(dev, i);
+
+		if (ret) {
+			dev_dbg(&dev->dev, "%s: err_setup_rxskb (i = %d)\n",
+					__func__, i);
+			goto err_setup_rxskb;
+		}
+	}
+
+	priv->txskb = kmalloc(sizeof(*priv->txskb) * TOTAL_NR_TXDESC,
+			GFP_KERNEL);
+	if (!priv->txskb) {
+		dev_dbg(&dev->dev, "%s: err_alloc_txskb\n", __func__);
+		goto err_alloc_txskb;
+	}
+
+	ret = ns9xxx_eth_hwinit(dev);
+	if (ret) {
+		dev_dbg(&dev->dev, "%s: err_hwinit -> %d\n", __func__, ret);
+		goto err_hwinit;
+	}
+
+	ret = request_irq(priv->irqtx, ns9xxx_eth_tx_int, 0, DRIVER_NAME, dev);
+	if (ret) {
+		dev_dbg(&dev->dev, "%s: err_request_irq_tx -> %d\n",
+				__func__, ret);
+		goto err_request_irq_tx;
+	}
+
+	ret = request_irq(priv->irqrx, ns9xxx_eth_rx_int, 0, DRIVER_NAME, dev);
+	if (ret) {
+		dev_dbg(&dev->dev, "%s: err_request_irq_rx -> %d\n",
+				__func__, ret);
+
+		free_irq(priv->irqtx, dev);
+err_request_irq_tx:
+
+		ns9xxx_eth_hwdisable(dev);
+err_hwinit:
+
+err_setup_rxskb:
+		for (i = 0; priv->rxskb[i]; ++i) {
+			dma_unmap_single(&dev->dev, priv->rxdesc[i].source,
+					priv->rxskb[i]->len, DMA_FROM_DEVICE);
+			kfree_skb(priv->rxskb[i]);
+		}
+
+		kfree(priv->txskb);
+err_alloc_txskb:
+
+		kfree(priv->rxskb);
+err_alloc_rxskb:
+
+		dma_free_coherent(&dev->dev,
+				sizeof(*priv->rxdesc) * TOTAL_NR_RXDESC,
+				priv->rxdesc, priv->rxdeschandle);
+err_alloc_rxdesc:
+
+		release_mem_region(priv->mapbase, 0x2800);
+err_request_mem:
+
+		return ret;
+	}
+
+	netif_start_queue(dev);
+
+	return 0;
+}
+
+static int ns9xxx_eth_stop(struct net_device *dev)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+
+	dev_dbg(&dev->dev, "%s\n", __func__);
+
+	free_irq(priv->irqrx, dev);
+	free_irq(priv->irqtx, dev);
+
+	cancel_delayed_work(&priv->recover_from_rx_stall);
+
+	ns9xxx_eth_hwdisable(dev);
+	kfree(priv->txskb);
+	kfree(priv->rxskb);
+	dma_free_coherent(&dev->dev,
+			sizeof(*priv->rxdesc) * TOTAL_NR_RXDESC,
+			priv->rxdesc, priv->rxdeschandle);
+
+	release_mem_region(priv->mapbase, 0x2800);
+
+	return 0;
+}
+
+static int ns9xxx_eth_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	unsigned long flags;
+	int ret;
+
+	if (!netif_running(dev))
+		return -EINVAL;
+
+	if (!priv->phy)
+		return -EINVAL;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	ret = phy_mii_ioctl(priv->phy, if_mii(ifr), cmd);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return ret;
+}
+
+static void ns9xxx_eth_add_txdesc(struct net_device *dev,
+		union ns9xxx_dma_desc *txbuffer)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	int i;
+
+	dev_vdbg(&dev->dev, "%s: txfree=%hu, txbusy=%hu\n",
+			__func__, priv->txfree, priv->txbusy);
+
+	for (i = 0; i < 4; ++i)
+		ethwrite32(dev, txbuffer->data[i],
+				ETH_TXBDR + 16 * priv->txfree + 4 * i);
+
+	priv->txfree = (priv->txfree + 1) % TOTAL_NR_TXDESC;
+}
+
+static int ns9xxx_eth_hard_start_xmit(struct sk_buff *skb,
+		struct net_device *dev)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	union ns9xxx_dma_desc txbuffer = {};
+	unsigned long flags;
+	int ret;
+
+	/* do I need to lock already here? */
+	spin_lock_irqsave(&priv->lock, flags);
+
+	dev_vdbg(&dev->dev, "%s(skb=%p): skb->data=%p\n",
+			__func__, skb, skb->data);
+
+	if (likely(ns9xxx_eth_num_txfree(dev) > 0)) {
+		txbuffer.source = dma_map_single(&dev->dev, skb->data,
+				skb->len, DMA_TO_DEVICE);
+		txbuffer.len = skb->len;
+		txbuffer.flags = DMATXDESC_LAST | DMADESC_FULL;
+
+		if (unlikely(priv->txfree == TOTAL_NR_TXDESC - 1))
+			txbuffer.flags |= DMADESC_WRAP;
+
+		priv->txskb[priv->txfree] = skb;
+		ns9xxx_eth_add_txdesc(dev, &txbuffer);
+
+		if (ns9xxx_eth_num_txbusy(dev) == 1)
+			ns9xxx_eth_start_tx_dma(dev);
+
+		ret = NETDEV_TX_OK;
+	} else {
+		netif_stop_queue(dev);
+		ret = NETDEV_TX_BUSY;
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return ret;
+}
+
+static void ns9xxx_eth_tx_timeout(struct net_device *dev)
+{
+	/* XXX */
+	dev_warn(&dev->dev, "%s unimplemented\n", __func__);
+}
+
+static struct net_device_stats *ns9xxx_eth_get_stats(struct net_device *dev)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+
+	priv->stats.rx_length_errors += ethread32(dev, ETH_STAT_RFLR);
+	priv->stats.rx_over_errors += ethread32(dev, ETH_STAT_ROVR);
+	priv->stats.rx_crc_errors += ethread32(dev, ETH_STAT_RFCS);
+	priv->stats.rx_frame_errors += ethread32(dev, ETH_STAT_RALN);
+	priv->stats.rx_errors += ethread32(dev, ETH_STAT_RCDE) +
+				 ethread32(dev, ETH_STAT_RCSE) +
+				 ethread32(dev, ETH_STAT_RUND) +
+				 ethread32(dev, ETH_STAT_RFRG) +
+				 ethread32(dev, ETH_STAT_RJBR);
+	priv->stats.multicast += ethread32(dev, ETH_STAT_RMCA);
+	priv->stats.tx_aborted_errors += ethread32(dev, ETH_STAT_TXCL);
+	priv->stats.collisions += ethread32(dev, ETH_STAT_TNCL);
+
+	return &priv->stats;
+}
+
+static void ns9xxx_set_multicast_list(struct net_device *dev)
+{
+	u32 saf = ETH_SAF_BROAD;
+
+	dev_dbg(&dev->dev, "%s\n", __func__);
+
+	/* TODO: use the hash tables to improve multicast reception */
+	if (dev->flags & IFF_PROMISC)
+		/* get everything */
+		saf |= ETH_SAF_PRO;
+
+	else if (dev->flags & IFF_ALLMULTI)
+		/* get all multicast traffic */
+		saf |= ETH_SAF_PRM;
+
+	else if (dev->mc_count > 0) {
+		struct dev_addr_list *m;
+		u32 ht[2] = {0, 0};
+
+		for (m = dev->mc_list; m; m = m->next) {
+			/*
+			 * The HRM of ns9360 and ns9215 state that the upper 6
+			 * bits are used to calculate the bit in the hash table,
+			 * but the sample code (and the NET+OS driver) uses bits
+			 * 28:23 ...
+			 */
+			u32 crc = ether_crc(ETH_ALEN, m->dmi_addr) >> 23;
+			crc &= 0x3f;
+
+			ht[crc & 0x20 ? 1 : 0] |= 1 << (crc & 0x1f);
+		}
+
+		saf |= ETH_SAF_PRA;
+
+		ethwrite32(dev, ht[0], ETH_HT1);
+		ethwrite32(dev, ht[1], ETH_HT2);
+	}
+
+	ethwrite32(dev, saf, ETH_SAF);
+}
+
+static int ns9xxx_eth_mdiobus_init(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	struct plat_ns9xxx_eth *pdata = pdev->dev.platform_data;
+	int i;
+	char phyid[BUS_ID_SIZE];
+	int ret = -ENOMEM;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	priv->mdiobus = mdiobus_alloc();
+	if (priv->mdiobus == NULL)
+		goto err_out;
+
+	priv->mdiobus->name = DRIVER_NAME "-mii";
+	snprintf(priv->mdiobus->id, MII_BUS_ID_SIZE, "0");
+	priv->mdiobus->priv = dev;
+	priv->mdiobus->read = ns9xxx_eth_mdiobus_read;
+	priv->mdiobus->write = ns9xxx_eth_mdiobus_write;
+	priv->mdiobus->reset = ns9xxx_eth_mdiobus_reset;
+	priv->mdiobus->phy_mask = pdata->phy_mask;
+	priv->mdiobus->parent = &pdev->dev;
+	priv->mdiobus->irq = kmalloc(sizeof(*priv->mdiobus->irq) * PHY_MAX_ADDR,
+			GFP_KERNEL);
+
+	if (!priv->mdiobus->irq) {
+		dev_dbg(&pdev->dev, "%s: err_alloc_irq\n", __func__);
+		goto err_alloc_irq;
+	}
+
+	for (i = 0; i < PHY_MAX_ADDR; ++i)
+		priv->mdiobus->irq[i] = PHY_POLL;
+
+	ret = mdiobus_register(priv->mdiobus);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_mdiobus_register -> %d\n",
+				__func__, ret);
+		goto err_mdiobus_register;
+	}
+
+	for (i = 0; i < PHY_MAX_ADDR; ++i)
+		if (priv->mdiobus->phy_map[i])
+			break;
+
+	if (i >= PHY_MAX_ADDR) {
+		dev_dbg(&pdev->dev, "%s: no phy found\n", __func__);
+		ret = -ENODEV;
+		goto err_find_phy;
+	}
+
+	dev_dbg(&pdev->dev, "%s: using phy at address %d\n", __func__, i);
+
+	snprintf(phyid, sizeof(phyid), PHY_ID_FMT,
+			priv->mdiobus->id, i);
+
+	priv->phy = phy_connect(dev, phyid, &ns9xxx_eth_adjust_link,
+			0, PHY_INTERFACE_MODE_MII);
+
+	if (IS_ERR(priv->phy)) {
+		ret = PTR_ERR(priv->phy);
+		dev_dbg(&pdev->dev, "%s: err_phy_connect -> %d\n",
+				__func__, ret);
+err_find_phy:
+
+		mdiobus_unregister(priv->mdiobus);
+err_mdiobus_register:
+
+		kfree(priv->mdiobus->irq);
+err_alloc_irq:
+		mdiobus_free(priv->mdiobus);
+err_out:
+		return ret;
+	}
+
+	priv->phy->supported &= SUPPORTED_10baseT_Half |
+		SUPPORTED_10baseT_Full |
+		SUPPORTED_100baseT_Half |
+		SUPPORTED_100baseT_Full |
+		SUPPORTED_Autoneg |
+		SUPPORTED_MII;
+
+	priv->phy->advertising = priv->phy->supported;
+
+	return 0;
+}
+
+static int ns9xxx_eth_mdiobus_disable(struct net_device *dev)
+{
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+
+	dev_dbg(&dev->dev, "%s\n", __func__);
+
+	phy_disconnect(priv->phy);
+	mdiobus_unregister(priv->mdiobus);
+	kfree(priv->mdiobus->irq);
+	mdiobus_free(priv->mdiobus);
+
+	return 0;
+}
+
+static __devinit int ns9xxx_eth_pdrv_probe(struct platform_device *pdev)
+{
+	struct net_device *dev;
+	struct ns9xxx_eth_priv *priv;
+
+	struct resource *mem;
+	struct plat_ns9xxx_eth *pdata = pdev->dev.platform_data;
+	unsigned char __iomem *membase;
+	u32 sa;
+	int ret = -ENODEV;
+	int i;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	if (!pdata)
+		goto err_pdata;
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem) {
+		dev_dbg(&pdev->dev, "%s: err_get_mem\n", __func__);
+		goto err_get_mem;
+	}
+
+	ret = -ENOMEM;
+
+	membase = ioremap(mem->start, 0x2800);
+	if (!membase) {
+		dev_dbg(&pdev->dev, "%s: err_ioremap\n", __func__);
+		goto err_ioremap;
+	}
+
+	dev = alloc_etherdev(sizeof(*priv));
+	if (!dev) {
+		dev_dbg(&pdev->dev, "%s: err_alloc_etherdev\n", __func__);
+		goto err_alloc_etherdev;
+	}
+
+	dev->dev.coherent_dma_mask = (u32)-1;
+
+	SET_NETDEV_DEV(dev, &pdev->dev);
+	platform_set_drvdata(pdev, dev);
+
+	dev->open = ns9xxx_eth_open;
+	dev->stop = ns9xxx_eth_stop;
+	dev->hard_start_xmit = ns9xxx_eth_hard_start_xmit;
+	dev->tx_timeout = ns9xxx_eth_tx_timeout;
+	dev->get_stats = ns9xxx_eth_get_stats;
+	dev->set_multicast_list = ns9xxx_set_multicast_list;
+	dev->do_ioctl = ns9xxx_eth_ioctl;
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	dev->poll_controller = ns9xxx_eth_netpoll;
+#endif
+
+	/* TODO: implement VLAN */
+	dev->features = 0;
+
+	priv = netdev_priv(dev);
+
+	spin_lock_init(&priv->lock);
+	INIT_DELAYED_WORK(&priv->recover_from_rx_stall, ns9xxx_eth_recover_from_rx_stall);
+
+	priv->membase = membase;
+	priv->mapbase = mem->start;
+	priv->irqrx = pdata->irqrx;
+	priv->irqtx = pdata->irqtx;
+#ifdef CONFIG_GPIO_ETH_ACTIVITY_LED
+	priv->activityled = pdata->activityled;
+	/* init kernel timer for toggling
+	 * the activity LED */
+	activityled_timer.data = (unsigned long)priv->activityled;
+	activityled_timer.function = activityled_timer_fn;
+	activityled_timer.expires = jiffies + ACTIVITYLED_TOGGLE_TIMEOUT;
+	init_timer(&activityled_timer);
+	add_timer(&activityled_timer);
+#endif
+
+	priv->clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(priv->clk)) {
+		ret = PTR_ERR(priv->clk);
+		dev_dbg(&pdev->dev, "%s: err_clk_get -> %d\n", __func__, ret);
+		goto err_clk_get;
+	}
+
+	ret = clk_enable(priv->clk);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_clk_enable -> %d\n",
+				__func__, ret);
+		goto err_clk_enable;
+	}
+
+	sa = ethread32(dev, ETH_SA1);
+	if (sa == 0) {
+		dev_warn(&pdev->dev, "Warning: Using default mac address "
+				"00:04:f3:ff:ff:fa\n");
+		memcpy(dev->dev_addr, "\x00\x04\xf3\xff\xff\xfa", ETH_ALEN);
+	} else {
+		DECLARE_MAC_BUF(mac);
+
+		/* assume the bootloader has provided a valid mac address */
+		dev->dev_addr[4] = sa;
+		dev->dev_addr[5] = sa >> 8;
+		sa = ethread32(dev, ETH_SA2);
+		dev->dev_addr[2] = sa;
+		dev->dev_addr[3] = sa >> 8;
+		sa = ethread32(dev, ETH_SA3);
+		dev->dev_addr[0] = sa;
+		dev->dev_addr[1] = sa >> 8;
+		dev_dbg(&pdev->dev, "mac address: %s\n",
+				print_mac(mac, dev->dev_addr));
+	}
+
+	ethwrite32(dev, ETH_EGCR1_PM_MII | ETH_EGCR1_MACHRST, ETH_EGCR1);
+	udelay(5);
+	ethwrite32(dev, ETH_EGCR1_PM_MII | ETH_EGCR1_ETX, ETH_EGCR1);
+
+	ethwrite32(dev, 0, ETH_MAC1);
+
+	dev_dbg(&pdev->dev, "%s: clear tx DMA descs\n", __func__);
+	for (i = 0; i < 4 * TOTAL_NR_TXDESC; ++i)
+		ethwrite32(dev, 0, ETH_TXBDR + 4 * i);
+
+	ret = ns9xxx_eth_mdiobus_init(pdev);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_mdiobus_init -> %d\n",
+				__func__, ret);
+		goto err_mdiobus_init;
+	}
+
+	/* Make the device wakeup capable, but disabled by default */
+	device_init_wakeup(&pdev->dev, 1);
+	device_set_wakeup_enable(&pdev->dev, 0);
+
+	ret = register_netdev(dev);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_register_netdev -> %d\n",
+				__func__, ret);
+
+		ns9xxx_eth_mdiobus_disable(dev);
+err_mdiobus_init:
+
+		clk_disable(priv->clk);
+err_clk_enable:
+
+		clk_put(priv->clk);
+err_clk_get:
+
+		platform_set_drvdata(pdev, NULL);
+		free_netdev(dev);
+err_alloc_etherdev:
+
+		iounmap(membase);
+err_ioremap:
+err_get_mem:
+err_pdata:
+		return ret;
+	}
+
+	/* When using nfsroot, there are RPC requests which can be sent
+	 * before the link is up (long autonegotiation...). Then the
+	 * RPC is not completed until the autonegotiation timeouts (which
+	 * adds long time to the boot process)
+	 * Following line tries to workaround that situation by starting
+	 * the phy here, so we have some worked completed in advance to
+	 * the driver open call */
+	phy_start(priv->phy);
+
+	dev_info(&pdev->dev, "%s at MMIO %p\n", dev->name, membase);
+	dev_dbg(&pdev->dev, "&priv = %p\n", &priv);
+	dev_dbg(&pdev->dev, "&priv->txfree = %p\n", &priv->txfree);
+	dev_dbg(&pdev->dev, "&priv->txbusy = %p\n", &priv->txbusy);
+
+	return 0;
+}
+
+static __devexit int ns9xxx_eth_pdrv_remove(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	unregister_netdev(dev);
+
+	ns9xxx_eth_mdiobus_disable(dev);
+
+	clk_disable(priv->clk);
+
+	clk_put(priv->clk);
+
+	platform_set_drvdata(pdev, NULL);
+
+	iounmap(priv->membase);
+
+	free_netdev(dev);
+
+#ifdef CONFIG_GPIO_ETH_ACTIVITY_LED
+	del_timer(&activityled_timer);
+#endif
+
+	return 0;
+}
+
+#if defined(CONFIG_PM)
+static int ns9xxx_eth_pdrv_suspend(struct platform_device *pdev,
+		pm_message_t state)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	if (state.event != PM_EVENT_SUSPEND)
+		/* XXX: implement full state saving */
+		return -EBUSY;
+
+	netif_device_detach(dev);
+
+	if (device_may_wakeup(&pdev->dev)) {
+
+		ret = enable_irq_wake(priv->irqrx);
+		if (ret) {
+			dev_dbg(&pdev->dev, "%s: err_enable_irq_wake -> %d\n",
+					__func__, ret);
+			goto err_enable_irq_wake;
+		}
+
+	} else {
+		dev_dbg(&pdev->dev, "%s: !device_may_wakeup\n", __func__);
+		clk_disable(priv->clk);
+	}
+
+	return 0;
+
+err_enable_irq_wake:
+	netif_device_attach(dev);
+
+	return ret;
+}
+
+static int ns9xxx_eth_pdrv_resume(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct ns9xxx_eth_priv *priv = netdev_priv(dev);
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	if (device_may_wakeup(&pdev->dev))
+		disable_irq_wake(priv->irqrx);
+	else {
+		ret = clk_enable(priv->clk);
+		if (ret) {
+			dev_dbg(&pdev->dev, "%s: err_clk_enable -> %d",
+					__func__, ret);
+			return ret;
+		}
+	}
+
+	netif_device_attach(dev);
+
+	return 0;
+}
+#endif
+
+static struct platform_driver ns9xxx_eth_pdriver = {
+	.probe = ns9xxx_eth_pdrv_probe,
+	.remove = __devexit_p(ns9xxx_eth_pdrv_remove),
+#if defined(CONFIG_PM)
+	.suspend = ns9xxx_eth_pdrv_suspend,
+	.resume = ns9xxx_eth_pdrv_resume,
+#endif
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ns9xxx_eth_init(void)
+{
+	int ret;
+
+	/* catch compiler bugs, endian issues etc pp */
+	BUG_ON(offsetof(union ns9xxx_dma_desc, data) != 0x0);
+	BUG_ON(offsetof(union ns9xxx_dma_desc, source) != 0x0);
+	BUG_ON(offsetof(union ns9xxx_dma_desc, len) != 0x4);
+	BUG_ON(offsetof(union ns9xxx_dma_desc, dest) != 0x8);
+	BUG_ON(offsetof(union ns9xxx_dma_desc, flags) != 0xe);
+	BUG_ON(offsetof(union ns9xxx_dma_desc, status) != 0xc);
+
+	ret = platform_driver_register(&ns9xxx_eth_pdriver);
+	if (ret) {
+		pr_debug("%s: err_pdrv_register\n", __func__);
+
+		return ret;
+	}
+	pr_info("Digi NS9XXX Ethernet driver\n");
+
+	return 0;
+}
+
+static void __exit ns9xxx_eth_exit(void)
+{
+	platform_driver_unregister(&ns9xxx_eth_pdriver);
+}
+
+module_init(ns9xxx_eth_init);
+module_exit(ns9xxx_eth_exit);
+
+MODULE_AUTHOR("Uwe Kleine-Koenig");
+MODULE_DESCRIPTION("Digi NS9XXX ethernet driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/net/smc911x.h b/drivers/net/smc911x.h
index cc7d85bdfb3e..2ea8f4aa1d91 100644
--- a/drivers/net/smc911x.h
+++ b/drivers/net/smc911x.h
@@ -52,6 +52,14 @@
 
 #ifdef SMC_USE_PXA_DMA
 #define SMC_USE_DMA
+  #define SMC_USE_32BIT		1
+  #define SMC_IRQ_SENSE		IRQF_TRIGGER_LOW
+#endif
+
+#ifdef CONFIG_ARCH_MXC
+  #define SMC_USE_16BIT		0
+  #define SMC_USE_32BIT		1
+  #define SMC_IRQ_SENSE		IRQF_TRIGGER_LOW
 #endif
 
 /* store this information for the driver.. */
diff --git a/drivers/net/smsc9118/Makefile b/drivers/net/smsc9118/Makefile
new file mode 100644
index 000000000000..d33fe1dc8e38
--- /dev/null
+++ b/drivers/net/smsc9118/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_SMSC9118)	+= smsc911x.o
diff --git a/drivers/net/smsc9118/smsc911x.c b/drivers/net/smsc9118/smsc911x.c
new file mode 100644
index 000000000000..3a55f78cee8a
--- /dev/null
+++ b/drivers/net/smsc9118/smsc911x.c
@@ -0,0 +1,2711 @@
+/***************************************************************************
+ *
+ * Copyright (C) 2004-2007  SMSC
+ * Copyright (C) 2005 ARM
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ ***************************************************************************
+ * Rewritten, heavily based on smsc911x simple driver by SMSC.
+ * Partly uses io macros from smc91x.c by Nicolas Pitre
+ *
+ * Supported devices:
+ *   LAN9115, LAN9116, LAN9117, LAN9118
+ *   LAN9215, LAN9216, LAN9217, LAN9218
+ *
+ * History:
+ *   05/05/2005 bahadir.balban@arm.com
+ *     - Transition to linux coding style
+ *     - Platform driver and module interface
+ *
+ *   17/07/2006 steve.glendinning@smsc.com
+ *     - Added support for LAN921x family
+ *     - Added workaround for multicast filters
+ *
+ *   31/07/2006 steve.glendinning@smsc.com
+ *     - Removed tasklet, using NAPI poll instead
+ *     - Multiple device support
+ *     - Large tidy-up following feedback from netdev list
+ *
+ *   03/08/2006 steve.glendinning@smsc.com
+ *     - Added ethtool support
+ *     - Convert to use generic MII interface
+ *
+ *   04/08/2006 bahadir.balban@arm.com
+ *     - Added ethtool eeprom r/w support
+ *
+ *   17/06/2007 steve.glendinning@smsc.com
+ *     - Incorporate changes from Bill Gatliff and Russell King
+ *
+ *   04/07/2007 steve.glendinning@smsc.com
+ *     - move irq configuration to platform_device
+ *     - fix link poller after interface is stopped and restarted
+ *
+ *   13/07/2007 bahadir.balban@arm.com
+ *     - set irq polarity before requesting irq
+ *
+ *   26/06/2007 hennerich@blackfin.uclinux.org
+ *     - Fixed minor style issue to pass checkpatch.pl
+ */
+
+#include <linux/crc32.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/etherdevice.h>
+#include <linux/ethtool.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/kernel.h>
+#include <linux/mii.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/timer.h>
+#include <linux/version.h>
+#include <linux/bug.h>
+#include <linux/bitops.h>
+#include <linux/irq.h>
+#include <asm/io.h>
+
+/* For having the same platform-data as in the Vanilla kernel */
+#include <linux/smc911x.h>
+
+#include "smsc911x.h"
+
+#define SMSC_CHIPNAME		"smsc911x"
+#define SMSC_DRV_VERSION	"2007-07-13"
+
+MODULE_LICENSE("GPL");
+
+
+/* Base address of the connected controller: S3C2410_CS5 = 0x28000000 */
+#define printk_err(fmt, args...)                printk(KERN_ERR "[ ERROR ] smsc911x: " fmt, ## args)
+#define printk_info(fmt, args...)               printk(KERN_INFO "smsc911x: " fmt, ## args)
+
+#if 0
+#define SMSC911X_DEBUG
+#endif
+
+#ifdef SMSC911X_DEBUG
+#  define printk_debug(fmt, args...)            printk(KERN_DEBUG "smsc911x: " fmt, ## args)
+#else
+#  define printk_debug(fmt, args...)
+#endif
+
+/* Enables the debug messages for the PM-operations (WOL, suspend, etc.) */
+#if 0
+#define SMSC911X_PM_DEBUG
+#endif
+
+#ifdef SMSC911X_PM_DEBUG
+#  define printk_pmdbg(fmt, args...)            printk(KERN_DEBUG "smsc911x: " fmt, ## args)
+#else
+#  define printk_pmdbg(fmt, args...)
+#endif
+
+struct smsc911x_data {
+	void __iomem *ioaddr;
+
+	unsigned int idrev;
+	unsigned int generation;	/* used to decide which workarounds apply */
+
+	/* device configuration */
+	unsigned int irq_polarity;
+	unsigned int irq_type;
+	unsigned int irq_flags;
+
+	/* This needs to be acquired before calling any of below:
+	 * smsc911x_mac_read(), smsc911x_mac_write()
+	 * smsc911x_phy_read(), smsc911x_phy_write()
+	 */
+	spinlock_t phy_lock;
+
+	struct mii_if_info mii;
+	unsigned int using_extphy;
+	u32 msg_enable;
+#ifdef USE_LED1_WORK_AROUND
+	unsigned int gpio_setting;
+	unsigned int gpio_orig_setting;
+#endif
+	struct net_device *netdev;
+	struct napi_struct napi;
+	struct timer_list link_poll_timer;
+	unsigned int stop_link_poll;
+
+	unsigned int software_irq_signal;
+
+#ifdef USE_PHY_WORK_AROUND
+#define MIN_PACKET_SIZE (64)
+	char loopback_tx_pkt[MIN_PACKET_SIZE];
+	char loopback_rx_pkt[MIN_PACKET_SIZE];
+	unsigned int resetcount;
+#endif
+
+	/* Members for Multicast filter workaround */
+	unsigned int multicast_update_pending;
+	unsigned int set_bits_mask;
+	unsigned int clear_bits_mask;
+	unsigned int hashhi;
+	unsigned int hashlo;
+	unsigned int last_rxstat;
+
+	/* Registers for the internal PM */
+	unsigned long mac_wucsr;
+	unsigned long pmt_ctrl;
+	unsigned long phy_intmsk;
+};
+
+
+static int smsc911x_set_mac(struct net_device *dev, void *addr);
+
+#if SMSC_CAN_USE_32BIT
+
+static inline u32 smsc911x_reg_read(struct smsc911x_data *pdata, u32 reg)
+{
+	return readl(pdata->ioaddr + reg);
+}
+
+static inline void smsc911x_reg_write(u32 val, struct smsc911x_data *pdata,
+				      u32 reg)
+{
+	writel(val, pdata->ioaddr + reg);
+}
+
+#else				/* SMSC_CAN_USE_32BIT */
+
+static inline u32 smsc911x_reg_read(struct smsc911x_data *pdata, u32 reg)
+{
+	u32 reg_val;
+	unsigned long flags;
+
+	/* these two 16-bit reads must be performed consecutively, so must
+	 * not be interrupted by our own ISR (which would start another
+	 * read operation) */
+	local_irq_save(flags);
+	reg_val = ((readw(pdata->ioaddr + reg) & 0xFFFF) |
+		   ((readw(pdata->ioaddr + reg + 2) & 0xFFFF) << 16));
+	local_irq_restore(flags);
+
+	return reg_val;
+}
+
+static inline void smsc911x_reg_write(u32 val, struct smsc911x_data *pdata,
+				      u32 reg)
+{
+	unsigned long flags;
+
+	/* these two 16-bit writes must be performed consecutively, so must
+	 * not be interrupted by our own ISR (which would start another
+	 * read operation) */
+	local_irq_save(flags);
+	writew(val & 0xFFFF, pdata->ioaddr + reg);
+	writew((val >> 16) & 0xFFFF, pdata->ioaddr + reg + 2);
+	local_irq_restore(flags);
+}
+
+#endif				/* SMSC_CAN_USE_32BIT */
+
+#ifndef CONFIG_BLACKFIN
+/* Writes a packet to the TX_DATA_FIFO */
+static inline void
+smsc911x_tx_writefifo(struct smsc911x_data *pdata, unsigned int *buf,
+		      unsigned int wordcount)
+{
+	while (wordcount--)
+		smsc911x_reg_write(*buf++, pdata, TX_DATA_FIFO);
+}
+
+/* Reads a packet out of the RX_DATA_FIFO */
+static inline void
+smsc911x_rx_readfifo(struct smsc911x_data *pdata, unsigned int *buf,
+		     unsigned int wordcount)
+{
+	while (wordcount--)
+		*buf++ = smsc911x_reg_read(pdata, RX_DATA_FIFO);
+}
+#else
+/* Writes a packet to the TX_DATA_FIFO */
+static inline void
+smsc911x_tx_writefifo(struct smsc911x_data *pdata, unsigned int *buf,
+		      unsigned int wordcount)
+{
+	if (wordcount > 24)
+		dma_outsl((u_long)pdata->ioaddr + TX_DATA_FIFO, buf, wordcount);
+	else
+		outsl((u_long)pdata->ioaddr + TX_DATA_FIFO, buf, wordcount);
+}
+
+/* Reads a packet out of the RX_DATA_FIFO */
+static inline void
+smsc911x_rx_readfifo(struct smsc911x_data *pdata, unsigned int *buf,
+		     unsigned int wordcount)
+{
+	if (wordcount > 24)
+		dma_insl((u_long)pdata->ioaddr + RX_DATA_FIFO, buf, wordcount);
+	else
+		insl((u_long)pdata->ioaddr + RX_DATA_FIFO, buf, wordcount);
+}
+#endif
+
+/* waits for MAC not busy, with timeout.  Only called by smsc911x_mac_read
+ * and smsc911x_mac_write, so assumes phy_lock is held */
+static int smsc911x_mac_notbusy(struct smsc911x_data *pdata)
+{
+	int i;
+	u32 val;
+
+	for (i = 0; i < 40; i++) {
+		val = smsc911x_reg_read(pdata, MAC_CSR_CMD);
+		if (!(val & MAC_CSR_CMD_CSR_BUSY_))
+			return 1;
+	}
+	SMSC_WARNING("Timed out waiting for MAC not BUSY. "
+		     "MAC_CSR_CMD: 0x%08X", val);
+	return 0;
+}
+
+/* Fetches a MAC register value. Assumes phy_lock is acquired */
+static u32 smsc911x_mac_read(struct smsc911x_data *pdata, unsigned int offset)
+{
+	unsigned int temp;
+
+#ifdef CONFIG_DEBUG_SPINLOCK
+	if (!spin_is_locked(&pdata->phy_lock))
+		SMSC_WARNING("phy_lock not held");
+#endif				/* CONFIG_DEBUG_SPINLOCK */
+
+	temp = smsc911x_reg_read(pdata, MAC_CSR_CMD);
+	if (unlikely(temp & MAC_CSR_CMD_CSR_BUSY_)) {
+		SMSC_WARNING("smsc911x_mac_read failed, MAC busy at entry");
+		return 0xFFFFFFFF;
+	}
+
+	/* Send the MAC cmd */
+	smsc911x_reg_write(((offset & 0xFF) | MAC_CSR_CMD_CSR_BUSY_
+			    | MAC_CSR_CMD_R_NOT_W_), pdata, MAC_CSR_CMD);
+
+	/* Workaround for hardware read-after-write restriction */
+	temp = smsc911x_reg_read(pdata, BYTE_TEST);
+
+	/* Wait for the read to happen */
+	if (likely(smsc911x_mac_notbusy(pdata)))
+		return smsc911x_reg_read(pdata, MAC_CSR_DATA);
+
+	SMSC_WARNING("smsc911x_mac_read failed, MAC busy after read");
+	return 0xFFFFFFFF;
+}
+
+/* Set a mac register, phy_lock must be acquired before calling */
+static void smsc911x_mac_write(struct smsc911x_data *pdata,
+			       unsigned int offset, u32 val)
+{
+	unsigned int temp;
+
+#ifdef CONFIG_DEBUG_SPINLOCK
+	if (!spin_is_locked(&pdata->phy_lock))
+		SMSC_WARNING("phy_lock not held");
+#endif				/* CONFIG_DEBUG_SPINLOCK */
+
+	temp = smsc911x_reg_read(pdata, MAC_CSR_CMD);
+	if (unlikely(temp & MAC_CSR_CMD_CSR_BUSY_)) {
+		SMSC_WARNING("smsc911x_mac_write failed, MAC busy at entry");
+		return;
+	}
+
+	/* Send data to write */
+	smsc911x_reg_write(val, pdata, MAC_CSR_DATA);
+
+	/* Write the actual data */
+	smsc911x_reg_write(((offset & 0xFF) | MAC_CSR_CMD_CSR_BUSY_), pdata,
+			   MAC_CSR_CMD);
+
+	/* Workaround for hardware read-after-write restriction */
+	temp = smsc911x_reg_read(pdata, BYTE_TEST);
+
+	/* Wait for the write to complete */
+	if (likely(smsc911x_mac_notbusy(pdata)))
+		return;
+
+	SMSC_WARNING("smsc911x_mac_write failed, MAC busy after write");
+}
+
+/* Gets a phy register, phy_lock must be acquired before calling */
+static u16 smsc911x_phy_read(struct smsc911x_data *pdata, unsigned int index)
+{
+	unsigned int addr;
+	int i;
+
+#ifdef CONFIG_DEBUG_SPINLOCK
+	if (!spin_is_locked(&pdata->phy_lock))
+		SMSC_WARNING("phy_lock not held");
+#endif				/* CONFIG_DEBUG_SPINLOCK */
+
+	/* Confirm MII not busy */
+	if (unlikely(smsc911x_mac_read(pdata, MII_ACC) & MII_ACC_MII_BUSY_)) {
+		SMSC_WARNING("MII is busy in smsc911x_phy_read???");
+		return 0;
+	}
+
+	/* Set the address, index & direction (read from PHY) */
+	addr = (((pdata->mii.phy_id) & 0x1F) << 11)
+	    | ((index & 0x1F) << 6);
+	smsc911x_mac_write(pdata, MII_ACC, addr);
+
+	/* Wait for read to complete w/ timeout */
+	for (i = 0; i < 100; i++) {
+		/* See if MII is finished yet */
+		if (!(smsc911x_mac_read(pdata, MII_ACC) & MII_ACC_MII_BUSY_)) {
+			return smsc911x_mac_read(pdata, MII_DATA);
+		}
+	}
+	SMSC_WARNING("Timed out waiting for MII write to finish");
+	return 0xFFFF;
+}
+
+/* Sets a phy register, phy_lock must be acquired before calling */
+static void smsc911x_phy_write(struct smsc911x_data *pdata,
+			       unsigned int index, u16 val)
+{
+	unsigned int addr;
+	int i;
+
+#ifdef CONFIG_DEBUG_SPINLOCK
+	if (!spin_is_locked(&pdata->phy_lock))
+		SMSC_WARNING("phy_lock not held");
+#endif				/* CONFIG_DEBUG_SPINLOCK */
+
+	/* Confirm MII not busy */
+	if (unlikely(smsc911x_mac_read(pdata, MII_ACC) & MII_ACC_MII_BUSY_)) {
+		SMSC_WARNING("MII is busy in smsc911x_write_phy???");
+		return;
+	}
+
+	/* Put the data to write in the MAC */
+	smsc911x_mac_write(pdata, MII_DATA, val);
+
+	/* Set the address, index & direction (write to PHY) */
+	addr = (((pdata->mii.phy_id) & 0x1F) << 11) |
+	    ((index & 0x1F) << 6) | MII_ACC_MII_WRITE_;
+	smsc911x_mac_write(pdata, MII_ACC, addr);
+
+	/* Wait for write to complete w/ timeout */
+	for (i = 0; i < 100; i++) {
+		/* See if MII is finished yet */
+		if (!(smsc911x_mac_read(pdata, MII_ACC) & MII_ACC_MII_BUSY_))
+			return;
+	}
+	SMSC_WARNING("Timed out waiting for MII write to finish");
+}
+
+static int smsc911x_mdio_read(struct net_device *dev, int phy_id, int location)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned long flags;
+	int reg;
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	reg = smsc911x_phy_read(pdata, location);
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+	return reg;
+}
+
+static void smsc911x_mdio_write(struct net_device *dev, int phy_id,
+				int location, int val)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	smsc911x_phy_write(pdata, location, val);
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+}
+
+/* Autodetects and initialises external phy for SMSC9115 and SMSC9117 flavors.
+ * If something goes wrong, returns -ENODEV to revert back to internal phy.
+ * Performed at initialisation only, so interrupts are enabled */
+static int smsc911x_phy_initialise_external(struct smsc911x_data *pdata)
+{
+	unsigned int address;
+	unsigned int hwcfg;
+	unsigned int phyid1;
+	unsigned int phyid2;
+
+	hwcfg = smsc911x_reg_read(pdata, HW_CFG);
+
+	/* External phy is requested, supported, and detected */
+	if (hwcfg & HW_CFG_EXT_PHY_DET_) {
+
+		/* Attempt to switch to external phy for auto-detecting
+		 * its address. Assuming tx and rx are stopped because
+		 * smsc911x_phy_initialise is called before
+		 * smsc911x_rx_initialise and tx_initialise.
+		 */
+
+		/* Disable phy clocks to the MAC */
+		hwcfg &= (~HW_CFG_PHY_CLK_SEL_);
+		hwcfg |= HW_CFG_PHY_CLK_SEL_CLK_DIS_;
+		smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+		udelay(10);	/* Enough time for clocks to stop */
+
+		/* Switch to external phy */
+		hwcfg |= HW_CFG_EXT_PHY_EN_;
+		smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+
+		/* Enable phy clocks to the MAC */
+		hwcfg &= (~HW_CFG_PHY_CLK_SEL_);
+		hwcfg |= HW_CFG_PHY_CLK_SEL_EXT_PHY_;
+		smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+		udelay(10);	/* Enough time for clocks to restart */
+
+		hwcfg |= HW_CFG_SMI_SEL_;
+		smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+
+		/* Auto-detect PHY */
+		spin_lock_irq(&pdata->phy_lock);
+		for (address = 0; address <= 31; address++) {
+			pdata->mii.phy_id = address;
+			phyid1 = smsc911x_phy_read(pdata, MII_PHYSID1);
+			phyid2 = smsc911x_phy_read(pdata, MII_PHYSID2);
+			if ((phyid1 != 0xFFFFU) || (phyid2 != 0xFFFFU)) {
+				SMSC_TRACE("Detected PHY at address = "
+					   "0x%02X = %d", address, address);
+				break;
+			}
+		}
+		spin_unlock_irq(&pdata->phy_lock);
+
+		if ((phyid1 == 0xFFFFU) && (phyid2 == 0xFFFFU)) {
+			SMSC_WARNING("External PHY is not accessable, "
+				     "using internal PHY instead");
+			/* Revert back to internal phy settings. */
+
+			/* Disable phy clocks to the MAC */
+			hwcfg &= (~HW_CFG_PHY_CLK_SEL_);
+			hwcfg |= HW_CFG_PHY_CLK_SEL_CLK_DIS_;
+			smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+			udelay(10);	/* Enough time for clocks to stop */
+
+			/* Switch to internal phy */
+			hwcfg &= (~HW_CFG_EXT_PHY_EN_);
+			smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+
+			/* Enable phy clocks to the MAC */
+			hwcfg &= (~HW_CFG_PHY_CLK_SEL_);
+			hwcfg |= HW_CFG_PHY_CLK_SEL_INT_PHY_;
+			smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+			udelay(10);	/* Enough time for clocks to restart */
+
+			hwcfg &= (~HW_CFG_SMI_SEL_);
+			smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+			/* Use internal phy */
+			return -ENODEV;
+		} else {
+			SMSC_TRACE("Successfully switched to external PHY");
+			pdata->using_extphy = 1;
+		}
+	} else {
+		SMSC_WARNING("No external PHY detected.");
+		SMSC_WARNING("Using internal PHY instead.");
+		/* Use internal phy */
+		return -ENODEV;
+	}
+	return 0;
+}
+
+/* called by phy_initialise and loopback test */
+static int smsc911x_phy_reset(struct smsc911x_data *pdata)
+{
+	unsigned int temp;
+	unsigned int i = 100000;
+	unsigned long flags;
+
+	SMSC_TRACE("Performing PHY BCR Reset");
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	smsc911x_phy_write(pdata, MII_BMCR, BMCR_RESET);
+	do {
+		udelay(10);
+		temp = smsc911x_phy_read(pdata, MII_BMCR);
+	} while ((i--) && (temp & BMCR_RESET));
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+	if (temp & BMCR_RESET) {
+		SMSC_WARNING("PHY reset failed to complete.");
+		return 0;
+	}
+	/* Extra delay required because the phy may not be completed with
+	 * its reset when BMCR_RESET is cleared. Specs say 256 uS is
+	 * enough delay but using 1ms here to be safe
+	 */
+	msleep(1);
+
+	return 1;
+}
+
+/* Fetches a tx status out of the status fifo */
+static unsigned int smsc911x_tx_get_txstatus(struct smsc911x_data *pdata)
+{
+	unsigned int result =
+	    smsc911x_reg_read(pdata, TX_FIFO_INF) & TX_FIFO_INF_TSUSED_;
+
+	if (result != 0)
+		result = smsc911x_reg_read(pdata, TX_STATUS_FIFO);
+
+	return result;
+}
+
+/* Fetches the next rx status */
+static unsigned int smsc911x_rx_get_rxstatus(struct smsc911x_data *pdata)
+{
+	unsigned int result =
+	    smsc911x_reg_read(pdata, RX_FIFO_INF) & RX_FIFO_INF_RXSUSED_;
+
+	if (result != 0)
+		result = smsc911x_reg_read(pdata, RX_STATUS_FIFO);
+
+	return result;
+}
+
+#ifdef USE_PHY_WORK_AROUND
+static int smsc911x_phy_check_loopbackpkt(struct smsc911x_data *pdata)
+{
+	unsigned int tries;
+	u32 wrsz;
+	u32 rdsz;
+	u32 bufp;
+
+	for (tries = 0; tries < 10; tries++) {
+		unsigned int txcmd_a;
+		unsigned int txcmd_b;
+		unsigned int status;
+		unsigned int pktlength;
+		unsigned int i;
+
+		/* Zero-out rx packet memory */
+		memset(pdata->loopback_rx_pkt, 0, MIN_PACKET_SIZE);
+
+		/* Write tx packet to 118 */
+		txcmd_a = (((unsigned int)pdata->loopback_tx_pkt)
+			   & 0x03) << 16;
+		txcmd_a |= TX_CMD_A_FIRST_SEG_ | TX_CMD_A_LAST_SEG_;
+		txcmd_a |= MIN_PACKET_SIZE;
+
+		txcmd_b = MIN_PACKET_SIZE << 16 | MIN_PACKET_SIZE;
+
+		smsc911x_reg_write(txcmd_a, pdata, TX_DATA_FIFO);
+		smsc911x_reg_write(txcmd_b, pdata, TX_DATA_FIFO);
+
+		bufp = ((u32) pdata->loopback_tx_pkt) & 0xFFFFFFFC;
+		wrsz = MIN_PACKET_SIZE + 3;
+		wrsz += (((u32) pdata->loopback_tx_pkt) & 0x3);
+		wrsz >>= 2;
+
+		smsc911x_tx_writefifo(pdata, (unsigned int *)bufp, wrsz);
+
+		/* Wait till transmit is done */
+		i = 60;
+		do {
+			udelay(5);
+			status = smsc911x_tx_get_txstatus(pdata);
+		} while ((i--) && (!status));
+
+		if (!status) {
+			SMSC_WARNING("Failed to transmit during loopback test");
+			continue;
+		}
+		if (status & TX_STS_ES_) {
+			SMSC_WARNING("Transmit encountered errors during "
+				     "loopback test");
+			continue;
+		}
+
+		/* Wait till receive is done */
+		i = 60;
+		do {
+			udelay(5);
+			status = smsc911x_rx_get_rxstatus(pdata);
+		} while ((i--) && (!status));
+
+		if (!status) {
+			SMSC_WARNING("Failed to receive during loopback test");
+			continue;
+		}
+		if (status & RX_STS_ES_) {
+			SMSC_WARNING("Receive encountered errors during "
+				     "loopback test");
+			continue;
+		}
+
+		pktlength = ((status & 0x3FFF0000UL) >> 16);
+		bufp = (u32)pdata->loopback_rx_pkt;
+		rdsz = pktlength + 3;
+		rdsz += ((u32)pdata->loopback_rx_pkt) & 0x3;
+		rdsz >>= 2;
+
+		smsc911x_rx_readfifo(pdata, (unsigned int *)bufp, rdsz);
+
+		if (pktlength != (MIN_PACKET_SIZE + 4)) {
+			SMSC_WARNING("Unexpected packet size during "
+				     "loop back test, size=%d, "
+				     "will retry", pktlength);
+		} else {
+			unsigned int j;
+			int mismatch = 0;
+			for (j = 0; j < MIN_PACKET_SIZE; j++) {
+				if (pdata->loopback_tx_pkt[j]
+				    != pdata->loopback_rx_pkt[j]) {
+					mismatch = 1;
+					break;
+				}
+			}
+			if (!mismatch) {
+				SMSC_TRACE("Successfully verified "
+					   "loopback packet");
+				return 1;
+			} else {
+				SMSC_WARNING("Data miss match during "
+					     "loop back test, will retry.");
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int smsc911x_phy_loopbacktest(struct smsc911x_data *pdata)
+{
+	int result = 0;
+	unsigned int i;
+	unsigned int val;
+	unsigned long flags;
+
+	/* Initialise tx packet */
+	for (i = 0; i < 6; i++) {
+		/* Use broadcast destination address */
+		pdata->loopback_tx_pkt[i] = (char)0xFF;
+	}
+
+	for (i = 6; i < 12; i++) {
+		/* Use incrementing source address */
+		pdata->loopback_tx_pkt[i] = (char)i;
+	}
+
+	/* Set length type field */
+	pdata->loopback_tx_pkt[12] = 0x00;
+	pdata->loopback_tx_pkt[13] = 0x00;
+	for (i = 14; i < MIN_PACKET_SIZE; i++) {
+		pdata->loopback_tx_pkt[i] = (char)i;
+	}
+
+	val = smsc911x_reg_read(pdata, HW_CFG);
+	val &= HW_CFG_TX_FIF_SZ_;
+	val |= HW_CFG_SF_;
+	smsc911x_reg_write(val, pdata, HW_CFG);
+
+	smsc911x_reg_write(TX_CFG_TX_ON_, pdata, TX_CFG);
+	smsc911x_reg_write((((unsigned int)pdata->loopback_rx_pkt)
+			    & 0x03) << 8, pdata, RX_CFG);
+
+	for (i = 0; i < 10; i++) {
+		/* Set PHY to 10/FD, no ANEG, and loopback mode */
+		spin_lock_irqsave(&pdata->phy_lock, flags);
+		smsc911x_phy_write(pdata, MII_BMCR, 0x4100);
+
+		/* Enable MAC tx/rx, FD */
+		smsc911x_mac_write(pdata, MAC_CR, MAC_CR_FDPX_
+				   | MAC_CR_TXEN_ | MAC_CR_RXEN_);
+		spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+		if (smsc911x_phy_check_loopbackpkt(pdata)) {
+			result = 1;
+			break;
+		}
+		pdata->resetcount++;
+
+		/* Disable MAC rx */
+		spin_lock_irqsave(&pdata->phy_lock, flags);
+		smsc911x_mac_write(pdata, MAC_CR, 0);
+		spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+		smsc911x_phy_reset(pdata);
+	}
+
+	/* Disable MAC */
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	smsc911x_mac_write(pdata, MAC_CR, 0);
+
+	/* Cancel PHY loopback mode */
+	smsc911x_phy_write(pdata, MII_BMCR, 0);
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+	smsc911x_reg_write(0, pdata, TX_CFG);
+	smsc911x_reg_write(0, pdata, RX_CFG);
+
+	return result;
+}
+#endif				/* USE_PHY_WORK_AROUND */
+
+
+inline static void smsc911x_phy_dump_regs(struct smsc911x_data *pdata)
+{
+	printk("BCR   = 0x%04x\n", smsc911x_phy_read(pdata, MII_BMCR));
+	printk("BSR   = 0x%04x\n", smsc911x_phy_read(pdata, MII_BMSR));
+	printk("ID1   = 0x%04x\n", smsc911x_phy_read(pdata, MII_PHYSID1));
+	printk("ID2   = 0x%04x\n", smsc911x_phy_read(pdata, MII_PHYSID2));
+	printk("ADVER = 0x%04x\n", smsc911x_phy_read(pdata, MII_ADVERTISE));
+	printk("LPA   = 0x%04x\n", smsc911x_phy_read(pdata, MII_LPA));
+	printk("EXP   = 0x%04x\n", smsc911x_phy_read(pdata, MII_EXPANSION));
+}
+
+/* assumes phy_lock is held */
+static void smsc911x_phy_update_flowcontrol(struct smsc911x_data *pdata)
+{
+	unsigned int temp;
+
+	if (pdata->mii.full_duplex) {
+		unsigned int phy_adv;
+		unsigned int phy_lpa;
+		phy_adv = smsc911x_phy_read(pdata, MII_ADVERTISE);
+		phy_lpa = smsc911x_phy_read(pdata, MII_LPA);
+		if (phy_adv & phy_lpa & LPA_PAUSE_CAP) {
+			/* Both ends support symmetric pause, enable
+			 * PAUSE receive and transmit */
+			smsc911x_mac_write(pdata, FLOW, 0xFFFF0002);
+			temp = smsc911x_reg_read(pdata, AFC_CFG);
+			temp |= 0xF;
+			smsc911x_reg_write(temp, pdata, AFC_CFG);
+		} else if (((phy_adv & ADVERTISE_PAUSE_ALL) ==
+			    ADVERTISE_PAUSE_ALL) &&
+			   ((phy_lpa & LPA_PAUSE_ALL) == LPA_PAUSE_ASYM)) {
+			/* We support symmetric and asym pause, the
+			 * other end only supports asym, Enable PAUSE
+			 * receive, disable PAUSE transmit */
+			smsc911x_mac_write(pdata, FLOW, 0xFFFF0002);
+			temp = smsc911x_reg_read(pdata, AFC_CFG);
+			temp &= ~0xF;
+			smsc911x_reg_write(temp, pdata, AFC_CFG);
+		} else {
+			/* Disable PAUSE receive and transmit */
+			smsc911x_mac_write(pdata, FLOW, 0);
+			temp = smsc911x_reg_read(pdata, AFC_CFG);
+			temp &= ~0xF;
+			smsc911x_reg_write(temp, pdata, AFC_CFG);
+		}
+	} else {
+		smsc911x_mac_write(pdata, FLOW, 0);
+		temp = smsc911x_reg_read(pdata, AFC_CFG);
+		temp |= 0xF;
+		smsc911x_reg_write(temp, pdata, AFC_CFG);
+	}
+}
+
+static void smsc911x_phy_update_duplex(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned long flags;
+	unsigned int mac_cr;
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+
+	mac_cr = smsc911x_mac_read(pdata, MAC_CR);
+	if (pdata->mii.full_duplex) {
+		SMSC_TRACE("configuring for full duplex mode");
+		mac_cr |= MAC_CR_FDPX_;
+	} else {
+		SMSC_TRACE("configuring for half duplex mode");
+		mac_cr &= ~MAC_CR_FDPX_;
+	}
+	smsc911x_mac_write(pdata, MAC_CR, mac_cr);
+	smsc911x_phy_update_flowcontrol(pdata);
+
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+}
+
+
+/* Update link mode if any thing has changed */
+static void smsc911x_phy_update_linkmode(struct net_device *dev, int init)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	if (mii_check_media(&pdata->mii, netif_msg_link(pdata), init))
+		smsc911x_phy_update_duplex(dev);
+	/* mii_check_media() exists if the media is forced... */
+	if (pdata->mii.force_media) {
+		int cur_link = mii_link_ok(&pdata->mii);
+		int prev_link = netif_carrier_ok(dev);
+
+		if (!prev_link && cur_link) {
+			printk(KERN_INFO "%s: link up\n", dev->name);
+			netif_carrier_on(dev);
+		} else if (prev_link && !cur_link) {
+			printk(KERN_INFO "%s: link down\n", dev->name);
+			netif_carrier_off(dev);
+		}
+	}
+
+#ifdef USE_LED1_WORK_AROUND
+	if (netif_carrier_ok(dev)) {
+		if ((pdata->gpio_orig_setting & GPIO_CFG_LED1_EN_) &&
+		    (!pdata->using_extphy)) {
+			/* Restore orginal GPIO configuration */
+			pdata->gpio_setting = pdata->gpio_orig_setting;
+			smsc911x_reg_write(pdata->gpio_setting, pdata,
+					   GPIO_CFG);
+		}
+	} else {
+		/* Check global setting that LED1
+		 * usage is 10/100 indicator */
+		pdata->gpio_setting = smsc911x_reg_read(pdata, GPIO_CFG);
+		if ((pdata->gpio_setting & GPIO_CFG_LED1_EN_)
+		    && (!pdata->using_extphy)) {
+			/* Force 10/100 LED off, after saving
+			 * orginal GPIO configuration */
+			pdata->gpio_orig_setting = pdata->gpio_setting;
+
+			pdata->gpio_setting &= ~GPIO_CFG_LED1_EN_;
+			pdata->gpio_setting |= (GPIO_CFG_GPIOBUF0_
+						| GPIO_CFG_GPIODIR0_
+						| GPIO_CFG_GPIOD0_);
+			smsc911x_reg_write(pdata->gpio_setting, pdata,
+					   GPIO_CFG);
+		}
+	}
+#endif				/* USE_LED1_WORK_AROUND */
+}
+
+/* Entry point for the link poller */
+static void smsc911x_phy_checklink(unsigned long ptr)
+{
+	struct net_device *dev = (struct net_device *)ptr;
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	smsc911x_phy_update_linkmode(dev, 0);
+
+	if (!(pdata->stop_link_poll)) {
+		pdata->link_poll_timer.expires = jiffies + 2 * HZ;
+		add_timer(&pdata->link_poll_timer);
+	} else {
+		pdata->stop_link_poll = 0;
+	}
+}
+
+static void smsc911x_phy_set_automdx(struct smsc911x_data *pdata)
+{
+	u16 ctrlstatus;
+
+	ctrlstatus = smsc911x_phy_read(pdata, 27);
+	ctrlstatus &= 0x1fff;
+	ctrlstatus |= (0x6 << 13);
+	smsc911x_phy_write(pdata, 27, ctrlstatus);
+}
+
+/* Initialises the PHY layer.  Called at initialisation by open() so
+ * interrupts are enabled */
+static int smsc911x_phy_initialise(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned int phyid1 = 0;
+	unsigned int phyid2 = 0;
+	unsigned int temp;
+
+	printk_debug("Calling phy_initialise()\n");
+
+	pdata->using_extphy = 0;
+
+	switch (pdata->idrev & 0xFFFF0000) {
+	case 0x01170000:
+	case 0x01150000:
+		/* External PHY supported, try to autodetect */
+		if (smsc911x_phy_initialise_external(pdata) < 0) {
+			SMSC_TRACE("External PHY is not detected, using "
+				   "internal PHY instead");
+			pdata->mii.phy_id = 1;
+		}
+		break;
+	default:
+		SMSC_TRACE("External PHY is not supported, using internal PHY "
+			   "instead");
+		pdata->mii.phy_id = 1;
+		break;
+	}
+
+	spin_lock_irq(&pdata->phy_lock);
+	phyid1 = smsc911x_phy_read(pdata, MII_PHYSID1);
+	phyid2 = smsc911x_phy_read(pdata, MII_PHYSID2);
+	spin_unlock_irq(&pdata->phy_lock);
+
+	if ((phyid1 == 0xFFFF) && (phyid2 == 0xFFFF)) {
+		SMSC_WARNING("Internal PHY not detected!");
+		return 0;
+	}
+
+	/* Reset the phy */
+	if (!smsc911x_phy_reset(pdata)) {
+		SMSC_WARNING("PHY reset failed to complete.");
+		return 0;
+	}
+#ifdef USE_PHY_WORK_AROUND
+	if (!smsc911x_phy_loopbacktest(pdata)) {
+		SMSC_WARNING("Failed Loop Back Test");
+		return 0;
+	} else {
+		SMSC_TRACE("Passed Loop Back Test");
+	}
+#endif				/* USE_PHY_WORK_AROUND */
+
+
+	smsc911x_phy_set_automdx(pdata);
+
+	/* Advertise all speeds and pause capabilities */
+	spin_lock_irq(&pdata->phy_lock);
+	temp = smsc911x_phy_read(pdata, MII_ADVERTISE);
+	temp |= (ADVERTISE_ALL | ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM);
+	smsc911x_phy_write(pdata, MII_ADVERTISE, temp);
+	pdata->mii.advertising = temp;
+
+	if (!pdata->using_extphy) {
+		/* using internal phy, enable PHY interrupts */
+		smsc911x_phy_read(pdata, MII_INTSTS);
+		smsc911x_phy_write(pdata, MII_INTMSK, PHY_INTMSK_DEFAULT_);
+	}
+
+	/* begin to establish link */
+	smsc911x_phy_write(pdata, MII_BMCR, BMCR_ANENABLE | BMCR_ANRESTART);
+	spin_unlock_irq(&pdata->phy_lock);
+
+	smsc911x_phy_update_linkmode(dev, 1);
+
+	pdata->stop_link_poll = 0;
+	setup_timer(&pdata->link_poll_timer, smsc911x_phy_checklink,
+		(unsigned long)dev);
+	pdata->link_poll_timer.expires = jiffies + 2 * HZ;
+	add_timer(&pdata->link_poll_timer);
+
+	printk_debug("PHY initialised succesfully\n");
+	return 1;
+}
+
+/* Gets the number of tx statuses in the fifo */
+static unsigned int smsc911x_tx_get_txstatcount(struct smsc911x_data *pdata)
+{
+	unsigned int result = (smsc911x_reg_read(pdata, TX_FIFO_INF)
+			       & TX_FIFO_INF_TSUSED_) >> 16;
+	return result;
+}
+
+/* Reads tx statuses and increments counters where necessary */
+static void smsc911x_tx_update_txcounters(struct smsc911x_data *pdata)
+{
+	struct net_device *netdev = pdata->netdev;
+	unsigned int tx_stat;
+
+	while ((tx_stat = smsc911x_tx_get_txstatus(pdata)) != 0) {
+		if (unlikely(tx_stat & 0x80000000)) {
+			/* In this driver the packet tag is used as the packet
+			 * length. Since a packet length can never reach the
+			 * size of 0x8000, this bit is reserved. It is worth
+			 * noting that the "reserved bit" in the warning above
+			 * does not reference a hardware defined reserved bit
+			 * but rather a driver defined one.
+			 */
+			SMSC_WARNING("Packet tag reserved bit is high");
+		} else {
+			if (unlikely(tx_stat & 0x00008000)) {
+				printk_debug("TX status error: 0x%08x (MAC 0x%08x)\n",
+					     tx_stat, smsc911x_mac_read(pdata, MAC_CR));
+				netdev->stats.tx_errors++;
+			} else {
+				netdev->stats.tx_packets++;
+				netdev->stats.tx_bytes += (tx_stat >> 16);
+			}
+			if (unlikely(tx_stat & 0x00000100)) {
+				netdev->stats.collisions += 16;
+				netdev->stats.tx_aborted_errors += 1;
+			} else {
+				netdev->stats.collisions +=
+				    ((tx_stat >> 3) & 0xF);
+			}
+			if (unlikely(tx_stat & 0x00000800)) {
+				netdev->stats.tx_carrier_errors += 1;
+			}
+			if (unlikely(tx_stat & 0x00000200)) {
+				netdev->stats.collisions++;
+				netdev->stats.tx_aborted_errors++;
+			}
+		}
+	}
+}
+
+/* Increments the Rx error counters */
+static void
+smsc911x_rx_counterrors(struct smsc911x_data *pdata, unsigned int rxstat)
+{
+	struct net_device *netdev = pdata->netdev;
+	int crc_err = 0;
+
+	if (unlikely(rxstat & 0x00008000)) {
+		netdev->stats.rx_errors++;
+		if (unlikely(rxstat & 0x00000002)) {
+			netdev->stats.rx_crc_errors++;
+			crc_err = 1;
+		}
+	}
+	if (likely(!crc_err)) {
+		if (unlikely((rxstat & 0x00001020) == 0x00001020)) {
+			/* Frame type indicates length,
+			 * and length error is set */
+			netdev->stats.rx_length_errors++;
+		}
+		if (rxstat & RX_STS_MCAST_)
+			netdev->stats.multicast++;
+	}
+}
+
+/* Quickly dumps bad packets */
+static void
+smsc911x_rx_fastforward(struct smsc911x_data *pdata, unsigned int pktbytes)
+{
+	unsigned int pktwords = (pktbytes + NET_IP_ALIGN + 3) >> 2;
+
+	if (likely(pktwords >= 4)) {
+		unsigned int timeout = 500;
+		unsigned int val;
+		smsc911x_reg_write(RX_DP_CTRL_RX_FFWD_, pdata, RX_DP_CTRL);
+		do {
+			udelay(1);
+			val = smsc911x_reg_read(pdata, RX_DP_CTRL);
+		} while (timeout-- && (val & RX_DP_CTRL_RX_FFWD_));
+
+		if (unlikely(timeout == 0))
+			SMSC_WARNING("Timed out waiting for RX FFWD "
+				     "to finish, RX_DP_CTRL: 0x%08X", val);
+	} else {
+		unsigned int temp;
+		while (pktwords--)
+			temp = smsc911x_reg_read(pdata, RX_DATA_FIFO);
+	}
+}
+
+/* NAPI poll function */
+static int smsc911x_poll(struct napi_struct *napi, int budget)
+{
+	struct smsc911x_data *pdata = container_of(napi, struct smsc911x_data, napi);
+	struct net_device *dev = pdata->netdev;
+	int npackets = 0;
+
+	while (npackets < budget) {
+		unsigned int pktlength;
+		unsigned int pktwords;
+		unsigned int rxstat = smsc911x_rx_get_rxstatus(pdata);
+
+		/* break out of while loop if there are no more packets waiting */
+		if (!rxstat) {
+			printk_debug("Stopping the RX poll\n");
+			break;
+		}
+
+		pktlength = ((rxstat & 0x3FFF0000) >> 16);
+		pktwords = (pktlength + NET_IP_ALIGN + 3) >> 2;
+		printk_debug("Going to read %i words (pktlen %i)\n",
+			     pktwords, pktlength);
+
+		smsc911x_rx_counterrors(pdata, rxstat);
+
+		if (likely((rxstat & RX_STS_ES_) == 0)) {
+			struct sk_buff *skb;
+			skb = dev_alloc_skb(pktlength + NET_IP_ALIGN);
+			if (likely(skb)) {
+				skb->data = skb->head;
+				skb->tail = skb->head;
+				/* Align IP on 16B boundary */
+				skb_reserve(skb, NET_IP_ALIGN);
+				skb_put(skb, pktlength - 4);
+				smsc911x_rx_readfifo(pdata,
+						     (unsigned int *)skb->head,
+						     pktwords);
+				skb->dev = dev;
+				skb->protocol = eth_type_trans(skb, dev);
+				skb->ip_summed = CHECKSUM_NONE;
+				netif_receive_skb(skb);
+
+				/* Update counters */
+				dev->stats.rx_packets++;
+				dev->stats.rx_bytes += (pktlength - 4);
+				dev->last_rx = jiffies;
+				npackets++;
+				continue;
+			} else {
+				SMSC_WARNING("Unable to allocate sk_buff "
+					     "for rx packet, in PIO path");
+				dev->stats.rx_dropped++;
+			}
+		}
+		/* At this point, the packet is to be read out
+		 * of the fifo and discarded */
+		smsc911x_rx_fastforward(pdata, pktlength);
+	}
+
+	dev->stats.rx_dropped += smsc911x_reg_read(pdata, RX_DROP);
+	smsc911x_reg_write(INT_STS_RSFL_, pdata, INT_STS);
+
+	if (npackets < budget) {
+		unsigned int temp;
+		/* We processed all packets available.  Tell NAPI it can
+		 * stop polling then re-enable rx interrupts */
+		netif_rx_complete(dev, napi);
+		temp = smsc911x_reg_read(pdata, INT_EN);
+		temp |= INT_EN_RSFL_EN_;
+		smsc911x_reg_write(temp, pdata, INT_EN);
+	}
+
+	/* Return total received packets */
+	return npackets;
+}
+
+/* Returns hash bit number for given MAC address
+ * Example:
+ * 01 00 5E 00 00 01 -> returns bit number 31 */
+static unsigned int smsc911x_hash(char addr[ETH_ALEN])
+{
+	unsigned int crc;
+	unsigned int result;
+
+	crc = ether_crc(ETH_ALEN, addr);
+	result = (crc >> 26) & 0x3f;
+
+	return result;
+}
+
+static void smsc911x_rx_multicast_update(struct smsc911x_data *pdata)
+{
+	/* Performs the multicast & mac_cr update.  This is called when
+	 * safe on the current hardware, and with the phy_lock held */
+	unsigned int mac_cr = smsc911x_mac_read(pdata, MAC_CR);
+	mac_cr |= pdata->set_bits_mask;
+	mac_cr &= ~(pdata->clear_bits_mask);
+	smsc911x_mac_write(pdata, MAC_CR, mac_cr);
+	smsc911x_mac_write(pdata, HASHH, pdata->hashhi);
+	smsc911x_mac_write(pdata, HASHL, pdata->hashlo);
+}
+
+static void smsc911x_rx_multicast_update_workaround(struct smsc911x_data *pdata)
+{
+	unsigned int mac_cr;
+
+	/* This function is only called for older LAN911x devices
+	 * (revA or revB), where MAC_CR, HASHH and HASHL should not
+	 * be modified during Rx - newer devices immediately update the
+	 * registers.
+	 *
+	 * This is called from interrupt context */
+
+	spin_lock(&pdata->phy_lock);
+
+	/* Check Rx has stopped */
+	if (smsc911x_mac_read(pdata, MAC_CR) & MAC_CR_RXEN_)
+		SMSC_WARNING("Rx not stopped\n");
+
+	/* Perform the update - safe to do now Rx has stopped */
+	smsc911x_rx_multicast_update(pdata);
+
+	/* Re-enable Rx */
+	mac_cr = smsc911x_mac_read(pdata, MAC_CR);
+	mac_cr |= MAC_CR_RXEN_;
+	smsc911x_mac_write(pdata, MAC_CR, mac_cr);
+
+	pdata->multicast_update_pending = 0;
+
+	spin_unlock(&pdata->phy_lock);
+}
+
+/* Sets the device MAC address to dev_addr, called with phy_lock held */
+static void
+smsc911x_set_mac_address(struct smsc911x_data *pdata, u8 dev_addr[6])
+{
+	u32 mac_high16 = (dev_addr[5] << 8) | dev_addr[4];
+	u32 mac_low32 = (dev_addr[3] << 24) | (dev_addr[2] << 16) |
+	    (dev_addr[1] << 8) | dev_addr[0];
+
+	smsc911x_mac_write(pdata, ADDRH, mac_high16);
+	smsc911x_mac_write(pdata, ADDRL, mac_low32);
+}
+
+static int smsc911x_open(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned int timeout;
+	unsigned int temp;
+	unsigned int intcfg = 0;
+	struct sockaddr addr;
+
+	/* Reset the LAN911x */
+	smsc911x_reg_write(HW_CFG_SRST_, pdata, HW_CFG);
+	timeout = 10;
+	do {
+		udelay(10);
+		temp = smsc911x_reg_read(pdata, HW_CFG);
+	} while ((--timeout) && (temp & HW_CFG_SRST_));
+
+	if (unlikely(temp & HW_CFG_SRST_)) {
+		printk_err("Failed to complete reset");
+		return -ENODEV;
+	}
+
+	smsc911x_reg_write(0x00050000, pdata, HW_CFG);
+	smsc911x_reg_write(0x006E3740, pdata, AFC_CFG);
+
+	/* Make sure EEPROM has finished loading before setting GPIO_CFG */
+	timeout = 50;
+	while ((timeout--) &&
+	       (smsc911x_reg_read(pdata, E2P_CMD) & E2P_CMD_EPC_BUSY_)) {
+		udelay(10);
+	}
+
+	if (unlikely(timeout == 0)) {
+		SMSC_WARNING("Timed out waiting for EEPROM "
+			     "busy bit to clear");
+	}
+#if USE_DEBUG >= 1
+	smsc911x_reg_write(0x00670700, pdata, GPIO_CFG);
+#else
+	smsc911x_reg_write(0x70070000, pdata, GPIO_CFG);
+#endif
+
+	/* Initialise irqs, but leave all sources disabled */
+	smsc911x_reg_write(0, pdata, INT_EN);
+	smsc911x_reg_write(0xFFFFFFFF, pdata, INT_STS);
+
+	/* Set interrupt deassertion to 100uS */
+	//intcfg = ((0x38 << 24) | INT_CFG_IRQ_EN_);
+	intcfg = ((0x00 << 24) | INT_CFG_IRQ_EN_);
+	// PPH modified intcfg = ((10 << 24) | INT_CFG_IRQ_EN_);
+
+	if (pdata->irq_polarity) {
+		SMSC_TRACE("irq polarity: active high");
+		intcfg |= INT_CFG_IRQ_POL_;
+	} else {
+		SMSC_TRACE("irq polarity: active low");
+	}
+
+	if (pdata->irq_type) {
+		SMSC_TRACE("irq type: push-pull");
+		intcfg |= INT_CFG_IRQ_TYPE_;
+	} else {
+		SMSC_TRACE("irq type: open drain");
+	}
+
+	smsc911x_reg_write(intcfg, pdata, INT_CFG);
+
+	printk_debug("Testing irq handler using IRQ %d\n", dev->irq);
+	pdata->software_irq_signal = 0;
+	smp_wmb();
+
+	temp = smsc911x_reg_read(pdata, INT_EN);
+	temp |= INT_EN_SW_INT_EN_;
+	smsc911x_reg_write(temp, pdata, INT_EN);
+
+	timeout = 1000;
+	while (timeout--) {
+		smp_rmb();
+		if (pdata->software_irq_signal)
+			break;
+		msleep(1);
+	}
+
+	if (!pdata->software_irq_signal) {
+		printk(KERN_WARNING "%s: ISR failed signaling test (IRQ %d)\n",
+		       dev->name, dev->irq);
+		return -ENODEV;
+	}
+
+	printk_debug("IRQ handler passed test using IRQ %d\n", dev->irq);
+	netif_carrier_off(dev);
+
+	if (!smsc911x_phy_initialise(dev)) {
+		printk_err("Failed to initialize the PHY");
+		return -ENODEV;
+	}
+
+	temp = smsc911x_reg_read(pdata, HW_CFG);
+	temp &= HW_CFG_TX_FIF_SZ_;
+	temp |= HW_CFG_SF_;
+	smsc911x_reg_write(temp, pdata, HW_CFG);
+
+	temp = smsc911x_reg_read(pdata, FIFO_INT);
+	temp |= FIFO_INT_TX_AVAIL_LEVEL_;
+	temp &= ~(FIFO_INT_RX_STS_LEVEL_);
+	smsc911x_reg_write(temp, pdata, FIFO_INT);
+
+	/* set RX Data offset to 2 bytes for alignment */
+	smsc911x_reg_write((2 << 8), pdata, RX_CFG);
+
+	/* enable the polling before enabling the interrupts */
+	napi_enable(&pdata->napi);
+
+	temp = smsc911x_reg_read(pdata, INT_EN);
+	temp |= (INT_EN_TDFA_EN_ | INT_EN_RSFL_EN_ | INT_EN_RDFL_EN_);
+	smsc911x_reg_write(temp, pdata, INT_EN);
+
+	spin_lock_irq(&pdata->phy_lock);
+
+	/*
+	 * Reenable the full duplex mode, otherwise the TX engine will generate
+	 * status errors (Luis Galdos)
+	 */
+	temp = smsc911x_mac_read(pdata, MAC_CR);
+	temp |= (MAC_CR_TXEN_ | MAC_CR_RXEN_ | MAC_CR_HBDIS_ | MAC_CR_FDPX_);
+	smsc911x_mac_write(pdata, MAC_CR, temp);
+	spin_unlock_irq(&pdata->phy_lock);
+
+	smsc911x_reg_write(TX_CFG_TX_ON_, pdata, TX_CFG);
+
+	/* Set the MAC once again */
+	memcpy(addr.sa_data, dev->dev_addr, dev->addr_len);
+	if(smsc911x_set_mac(dev, &addr))
+		printk_err("Couldn't set the MAC address.\n");
+
+	netif_start_queue(dev);
+	return 0;
+}
+
+/* Entry point for stopping the interface */
+static int smsc911x_stop(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	printk_info("Stopping the interface\n");
+
+	napi_disable(&pdata->napi);
+
+	/* disable interrupts */
+       	smsc911x_reg_write(0, pdata, INT_EN);
+
+	pdata->stop_link_poll = 1;
+	del_timer_sync(&pdata->link_poll_timer);
+
+	netif_stop_queue(dev);
+
+	/* At this point all Rx and Tx activity is stopped */
+	dev->stats.rx_dropped += smsc911x_reg_read(pdata, RX_DROP);
+	smsc911x_tx_update_txcounters(pdata);
+
+	/* Stop sending data after the last transmission */
+	smsc911x_reg_write(TX_CFG_STOP_TX_, pdata, TX_CFG);
+
+	SMSC_TRACE("Interface stopped");
+	return 0;
+}
+
+/* Entry point for transmitting a packet */
+static int smsc911x_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned int freespace;
+	unsigned int tx_cmd_a;
+	unsigned int tx_cmd_b;
+	unsigned int temp;
+	u32 wrsz;
+	u32 bufp;
+
+	freespace = smsc911x_reg_read(pdata, TX_FIFO_INF) & TX_FIFO_INF_TDFREE_;
+
+	if (unlikely(freespace < TX_FIFO_LOW_THRESHOLD))
+		SMSC_WARNING("Tx data fifo low, space available: %d",
+			     freespace);
+
+	/* Word alignment adjustment */
+	tx_cmd_a = ((((unsigned int)(skb->data)) & 0x03) << 16);
+	tx_cmd_a |= TX_CMD_A_FIRST_SEG_ | TX_CMD_A_LAST_SEG_;
+	tx_cmd_a |= (unsigned int)skb->len;
+
+	tx_cmd_b = ((unsigned int)skb->len) << 16;
+	tx_cmd_b |= (unsigned int)skb->len;
+
+	smsc911x_reg_write(tx_cmd_a, pdata, TX_DATA_FIFO);
+	smsc911x_reg_write(tx_cmd_b, pdata, TX_DATA_FIFO);
+
+	bufp = ((u32)skb->data) & 0xFFFFFFFC;
+	wrsz = (u32)skb->len + 3;
+	wrsz += ((u32)skb->data) & 0x3;
+	wrsz >>= 2;
+
+	smsc911x_tx_writefifo(pdata, (unsigned int *)bufp, wrsz);
+	freespace -= (skb->len + 32);
+	dev_kfree_skb(skb);
+	dev->trans_start = jiffies;
+
+	if (unlikely(smsc911x_tx_get_txstatcount(pdata) >= 30))
+		smsc911x_tx_update_txcounters(pdata);
+
+	if (freespace < TX_FIFO_LOW_THRESHOLD) {
+		netif_stop_queue(dev);
+		temp = smsc911x_reg_read(pdata, FIFO_INT);
+		temp &= 0x00FFFFFF;
+		temp |= 0x32000000;
+		smsc911x_reg_write(temp, pdata, FIFO_INT);
+	}
+
+	return NETDEV_TX_OK;
+}
+
+/* Entry point for getting status counters */
+static struct net_device_stats *smsc911x_get_stats(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	smsc911x_tx_update_txcounters(pdata);
+	dev->stats.rx_dropped += smsc911x_reg_read(pdata, RX_DROP);
+	return &dev->stats;
+}
+
+/* Entry point for setting addressing modes */
+static void smsc911x_set_multicast_list(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned long flags;
+
+	if (dev->flags & IFF_PROMISC) {
+		/* Enabling promiscuous mode */
+		pdata->set_bits_mask = MAC_CR_PRMS_;
+		pdata->clear_bits_mask = (MAC_CR_MCPAS_ | MAC_CR_HPFILT_);
+		pdata->hashhi = 0;
+		pdata->hashlo = 0;
+	} else if (dev->flags & IFF_ALLMULTI) {
+		/* Enabling all multicast mode */
+		pdata->set_bits_mask = MAC_CR_MCPAS_;
+		pdata->clear_bits_mask = (MAC_CR_PRMS_ | MAC_CR_HPFILT_);
+		pdata->hashhi = 0;
+		pdata->hashlo = 0;
+	} else if (dev->mc_count > 0) {
+		/* Enabling specific multicast addresses */
+		unsigned int hash_high = 0;
+		unsigned int hash_low = 0;
+		unsigned int count = 0;
+		struct dev_mc_list *mc_list = dev->mc_list;
+
+		pdata->set_bits_mask = MAC_CR_HPFILT_;
+		pdata->clear_bits_mask = (MAC_CR_PRMS_ | MAC_CR_MCPAS_);
+
+		while (mc_list) {
+			count++;
+			if ((mc_list->dmi_addrlen) == ETH_ALEN) {
+				unsigned int bitnum =
+				    smsc911x_hash(mc_list->dmi_addr);
+				unsigned int mask = 0x01 << (bitnum & 0x1F);
+				if (bitnum & 0x20)
+					hash_high |= mask;
+				else
+					hash_low |= mask;
+			} else {
+				SMSC_WARNING("dmi_addrlen != 6");
+			}
+			mc_list = mc_list->next;
+		}
+		if (count != (unsigned int)dev->mc_count)
+			SMSC_WARNING("mc_count != dev->mc_count");
+
+		pdata->hashhi = hash_high;
+		pdata->hashlo = hash_low;
+	} else {
+		/* Enabling local MAC address only */
+		pdata->set_bits_mask = 0;
+		pdata->clear_bits_mask =
+		    (MAC_CR_PRMS_ | MAC_CR_MCPAS_ | MAC_CR_HPFILT_);
+		pdata->hashhi = 0;
+		pdata->hashlo = 0;
+	}
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+
+	if (pdata->generation <= 1) {
+		/* Older hardware revision - cannot change these flags while
+		 * receiving data */
+		if (!pdata->multicast_update_pending) {
+			unsigned int temp;
+			SMSC_TRACE("scheduling mcast update");
+			pdata->multicast_update_pending = 1;
+
+			/* Request the hardware to stop, then perform the
+			 * update when we get an RX_STOP interrupt */
+			smsc911x_reg_write(INT_STS_RXSTOP_INT_, pdata, INT_STS);
+			temp = smsc911x_reg_read(pdata, INT_EN);
+			temp |= INT_EN_RXSTOP_INT_EN_;
+			smsc911x_reg_write(temp, pdata, INT_EN);
+
+			temp = smsc911x_mac_read(pdata, MAC_CR);
+			temp &= ~(MAC_CR_RXEN_);
+			smsc911x_mac_write(pdata, MAC_CR, temp);
+		} else {
+			/* There is another update pending, this should now
+			 * use the newer values */
+		}
+	} else {
+		/* Newer hardware revision - can write immediately */
+		smsc911x_rx_multicast_update(pdata);
+	}
+
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+}
+
+static irqreturn_t smsc911x_irqhandler(int irq, void *dev_id)
+{
+	struct net_device *dev = dev_id;
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned int intsts;
+	unsigned int inten;
+	unsigned int temp;
+	unsigned int intcfg;
+	int serviced = IRQ_NONE;
+
+	intcfg = smsc911x_reg_read(pdata, INT_CFG);
+	intsts = smsc911x_reg_read(pdata, INT_STS);
+	inten = smsc911x_reg_read(pdata, INT_EN);
+
+	printk_debug("New IRQ: intsts 0x%08x\n", intsts);
+
+	if ((intcfg & (INT_CFG_IRQ_INT_ | INT_CFG_IRQ_EN_)) != (INT_CFG_IRQ_INT_ |
+								INT_CFG_IRQ_EN_))
+		return serviced;
+
+
+	if (unlikely(intsts & inten & INT_STS_SW_INT_)) {
+		temp = smsc911x_reg_read(pdata, INT_EN);
+		temp &= (~INT_EN_SW_INT_EN_);
+		smsc911x_reg_write(temp, pdata, INT_EN);
+		smsc911x_reg_write(INT_STS_SW_INT_, pdata, INT_STS);
+		pdata->software_irq_signal = 1;
+		smp_wmb();
+		serviced = IRQ_HANDLED;
+	}
+
+	if (unlikely(intsts & inten & INT_STS_RXSTOP_INT_)) {
+		/* Called when there is a multicast update scheduled and
+		 * it is now safe to complete the update */
+		SMSC_TRACE("RX Stop interrupt");
+		temp = smsc911x_reg_read(pdata, INT_EN);
+		temp &= (~INT_EN_RXSTOP_INT_EN_);
+		smsc911x_reg_write(temp, pdata, INT_EN);
+		smsc911x_reg_write(INT_STS_RXSTOP_INT_, pdata, INT_STS);
+		smsc911x_rx_multicast_update_workaround(pdata);
+		serviced = IRQ_HANDLED;
+	}
+
+	if (intsts & inten & INT_STS_TDFA_) {
+		temp = smsc911x_reg_read(pdata, FIFO_INT);
+		temp |= FIFO_INT_TX_AVAIL_LEVEL_;
+		smsc911x_reg_write(temp, pdata, FIFO_INT);
+		smsc911x_reg_write(INT_STS_TDFA_, pdata, INT_STS);
+		netif_wake_queue(dev);
+		serviced = IRQ_HANDLED;
+	}
+
+	if (unlikely(intsts & inten & INT_STS_RXE_)) {
+		smsc911x_reg_write(INT_STS_RXE_, pdata, INT_STS);
+		serviced = IRQ_HANDLED;
+	}
+
+	if (likely(intsts & inten & INT_STS_RSFL_)) {
+		if (likely(netif_rx_schedule_prep(dev, &pdata->napi))) {
+			/* Disable Rx interrupts and schedule NAPI poll */
+			temp = smsc911x_reg_read(pdata, INT_EN);
+			temp &= (~INT_EN_RSFL_EN_);
+			smsc911x_reg_write(temp, pdata, INT_EN);
+			__netif_rx_schedule(dev, &pdata->napi);
+		}
+
+		serviced = IRQ_HANDLED;
+	}
+
+	if (unlikely(intsts & inten & INT_STS_PHY_INT_)) {
+		smsc911x_reg_write(INT_STS_PHY_INT_, pdata, INT_STS);
+		spin_lock(&pdata->phy_lock);
+		temp = smsc911x_phy_read(pdata, MII_INTSTS);
+		spin_unlock(&pdata->phy_lock);
+		SMSC_TRACE("PHY interrupt, sts 0x%04X", (u16)temp);
+		smsc911x_phy_update_linkmode(dev, 0);
+		serviced = IRQ_HANDLED;
+	}
+	return serviced;
+}
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+void smsc911x_poll_controller(struct net_device *dev)
+{
+	disable_irq(dev->irq);
+	smsc911x_irqhandler(0, dev);
+	enable_irq(dev->irq);
+}
+#endif				/* CONFIG_NET_POLL_CONTROLLER */
+
+/* Standard ioctls for mii-tool */
+static int smsc911x_do_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned int chg_in_duplex;
+	int ret;
+
+	if (!netif_running(dev))
+		return -EINVAL;
+	ret = generic_mii_ioctl(&pdata->mii, if_mii(ifr), cmd, &chg_in_duplex);
+	if ((ret == 0) && (chg_in_duplex != 0))
+		smsc911x_phy_update_duplex(dev);
+
+	return ret;
+}
+
+static int
+smsc911x_ethtool_getsettings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	cmd->maxtxpkt = 1;
+	cmd->maxrxpkt = 1;
+	return mii_ethtool_gset(&pdata->mii, cmd);
+}
+
+static int
+smsc911x_ethtool_setsettings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	return mii_ethtool_sset(&pdata->mii, cmd);
+}
+
+static void smsc911x_ethtool_getdrvinfo(struct net_device *dev,
+					struct ethtool_drvinfo *info)
+{
+	strncpy(info->driver, SMSC_CHIPNAME, sizeof(info->driver));
+	strncpy(info->version, SMSC_DRV_VERSION, sizeof(info->version));
+	strncpy(info->bus_info, dev->dev.bus_id, sizeof(info->bus_info));
+}
+
+static int smsc911x_ethtool_nwayreset(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	return mii_nway_restart(&pdata->mii);
+}
+
+static u32 smsc911x_ethtool_getmsglevel(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	return pdata->msg_enable;
+}
+
+static void smsc911x_ethtool_setmsglevel(struct net_device *dev, u32 level)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	pdata->msg_enable = level;
+}
+
+static int smsc911x_ethtool_getregslen(struct net_device *dev)
+{
+	return (((E2P_CMD - ID_REV) / 4 + 1) + (WUCSR - MAC_CR) + 1 + 32) *
+	    sizeof(u32);
+}
+
+static void
+smsc911x_ethtool_getregs(struct net_device *dev, struct ethtool_regs *regs,
+			 void *buf)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned long flags;
+	unsigned int i;
+	unsigned int j = 0;
+	u32 *data = buf;
+
+	regs->version = pdata->idrev;
+	for (i = ID_REV; i <= E2P_CMD; i += (sizeof(u32)))
+		data[j++] = smsc911x_reg_read(pdata, i);
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	for (i = MAC_CR; i <= WUCSR; i++)
+		data[j++] = smsc911x_mac_read(pdata, i);
+	for (i = 0; i <= 31; i++)
+		data[j++] = smsc911x_phy_read(pdata, i);
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+}
+
+static void smsc911x_eeprom_enable_access(struct smsc911x_data *pdata)
+{
+	unsigned int temp = smsc911x_reg_read(pdata, GPIO_CFG);
+	temp &= ~GPIO_CFG_EEPR_EN_;
+	smsc911x_reg_write(temp, pdata, GPIO_CFG);
+	msleep(1);
+}
+
+static int smsc911x_eeprom_send_cmd(struct smsc911x_data *pdata, u32 op)
+{
+	int timeout = 100;
+	u32 e2cmd;
+
+	SMSC_TRACE("op 0x%08x", op);
+	if (smsc911x_reg_read(pdata, E2P_CMD) & E2P_CMD_EPC_BUSY_) {
+		SMSC_WARNING("Busy at start");
+		return -EBUSY;
+	}
+
+	e2cmd = op | E2P_CMD_EPC_BUSY_;
+	smsc911x_reg_write(e2cmd, pdata, E2P_CMD);
+
+	do {
+		msleep(1);
+		e2cmd = smsc911x_reg_read(pdata, E2P_CMD);
+	} while ((e2cmd & E2P_CMD_EPC_BUSY_) && (timeout--));
+
+	if (!timeout) {
+		SMSC_TRACE("TIMED OUT");
+		return -EAGAIN;
+	}
+
+	if (e2cmd & E2P_CMD_EPC_TIMEOUT_) {
+		SMSC_TRACE("Error occured during eeprom operation");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int smsc911x_eeprom_read_location(struct smsc911x_data *pdata,
+					 u8 address, u8 *data)
+{
+	u32 op = E2P_CMD_EPC_CMD_READ_ | address;
+	int ret;
+
+	SMSC_TRACE("address 0x%x", address);
+	ret = smsc911x_eeprom_send_cmd(pdata, op);
+
+	if (!ret)
+		data[address] = smsc911x_reg_read(pdata, E2P_DATA);
+
+	return ret;
+}
+
+static int smsc911x_eeprom_write_location(struct smsc911x_data *pdata,
+					  u8 address, u8 data)
+{
+	u32 op = E2P_CMD_EPC_CMD_ERASE_ | address;
+	int ret;
+
+	SMSC_TRACE("address 0x%x, data 0x%x", address, data);
+	ret = smsc911x_eeprom_send_cmd(pdata, op);
+
+	if (!ret) {
+		op = E2P_CMD_EPC_CMD_WRITE_ | address;
+		smsc911x_reg_write((u32)data, pdata, E2P_DATA);
+		ret = smsc911x_eeprom_send_cmd(pdata, op);
+	}
+
+	return ret;
+}
+
+static int smsc911x_ethtool_get_eeprom_len(struct net_device *dev)
+{
+	return SMSC911X_EEPROM_SIZE;
+}
+
+static int smsc911x_ethtool_get_eeprom(struct net_device *dev,
+				       struct ethtool_eeprom *eeprom, u8 *data)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	u8 eeprom_data[SMSC911X_EEPROM_SIZE];
+	int len;
+	int i;
+
+	smsc911x_eeprom_enable_access(pdata);
+
+	len = min(eeprom->len, SMSC911X_EEPROM_SIZE);
+	for (i = 0; i < len; i++) {
+		int ret = smsc911x_eeprom_read_location(pdata, i, eeprom_data);
+		if (ret < 0) {
+			eeprom->len = 0;
+			return ret;
+		}
+	}
+
+	memcpy(data, &eeprom_data[eeprom->offset], len);
+	eeprom->len = len;
+	return 0;
+}
+
+static int smsc911x_ethtool_set_eeprom(struct net_device *dev,
+				       struct ethtool_eeprom *eeprom, u8 *data)
+{
+	int ret;
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	smsc911x_eeprom_enable_access(pdata);
+	smsc911x_eeprom_send_cmd(pdata, E2P_CMD_EPC_CMD_EWEN_);
+	ret = smsc911x_eeprom_write_location(pdata, eeprom->offset, *data);
+	smsc911x_eeprom_send_cmd(pdata, E2P_CMD_EPC_CMD_EWDS_);
+
+	/* Single byte write, according to man page */
+	eeprom->len = 1;
+
+	return ret;
+}
+
+static int smsc911x_ethtool_set_wol(struct net_device *dev,
+				    struct ethtool_wolinfo *wol)
+{
+	struct smsc911x_data *pdata;
+
+	/* Check for unsupported options */
+        if (wol->wolopts & (WAKE_MAGICSECURE | WAKE_UCAST | WAKE_MCAST
+			    | WAKE_BCAST | WAKE_ARP))
+                return -EINVAL;
+
+	pdata = netdev_priv(dev);
+
+	/* When disable the WOL options need to disable the PHY-interrupts too */
+	if (!wol->wolopts) {
+		printk_pmdbg("[ WOL  ] Disabling all sources\n");
+		pdata->pmt_ctrl &= ~(PMT_CTRL_WOL_EN_ | PMT_CTRL_ED_EN_);
+		pdata->phy_intmsk &= ~PHY_INTMSK_ENERGYON_;
+		pdata->mac_wucsr = 0;
+		goto exit_set_wol;
+	}
+
+	/*
+	 * For the magic packet we MUST configure the MAC too, but we can't do it
+	 * at this point, cause the controller stops working.
+	 */
+	if (wol->wolopts & WAKE_MAGIC) {
+		printk_pmdbg("WOL: Enabling magic frame\n");
+		pdata->mac_wucsr |= WUCSR_MPEN_;
+		pdata->pmt_ctrl |= PMT_CTRL_WOL_EN_;
+	}
+
+	/* For the PHY-wakeup we must use the energy detection */
+	if (wol->wolopts & WAKE_PHY) {
+		printk_pmdbg("[ WOL  ] Enabling PHY energy\n");
+		pdata->phy_intmsk |= PHY_INTMSK_ENERGYON_;
+		pdata->pmt_ctrl |= PMT_CTRL_ED_EN_;
+	}
+
+ exit_set_wol:
+	return 0;
+}
+
+/* Function for getting the infos about the WOL */
+static void smsc911x_ethtool_get_wol(struct net_device *net_dev,
+				     struct ethtool_wolinfo *wol)
+{
+	/* Only for magic and PHY power detection available up now */
+        wol->supported = WAKE_MAGIC | WAKE_PHY;
+}
+
+static struct ethtool_ops smsc911x_ethtool_ops = {
+	.get_settings	= smsc911x_ethtool_getsettings,
+	.set_settings	= smsc911x_ethtool_setsettings,
+	.get_link	= ethtool_op_get_link,
+	.get_drvinfo	= smsc911x_ethtool_getdrvinfo,
+	.nway_reset	= smsc911x_ethtool_nwayreset,
+	.get_msglevel	= smsc911x_ethtool_getmsglevel,
+	.set_msglevel	= smsc911x_ethtool_setmsglevel,
+	.get_regs_len	= smsc911x_ethtool_getregslen,
+	.get_regs	= smsc911x_ethtool_getregs,
+	.get_eeprom_len = smsc911x_ethtool_get_eeprom_len,
+	.get_eeprom	= smsc911x_ethtool_get_eeprom,
+	.set_eeprom	= smsc911x_ethtool_set_eeprom,
+	.get_wol        = smsc911x_ethtool_get_wol,
+	.set_wol        = smsc911x_ethtool_set_wol,
+};
+
+
+static int smsc911x_set_mac(struct net_device *dev, void *addr)
+{
+	unsigned int reg;
+	int retval;
+	unsigned long flags;
+	struct smsc911x_data *pdata;
+	unsigned int low, high;
+	struct sockaddr *paddr = addr;
+
+	printk_debug("Set mac called\n");
+
+	pdata = netdev_priv(dev);
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+
+	/* First check that the MAC is not busy */
+	reg = smsc911x_reg_read(pdata, MAC_CSR_CMD);
+	if (unlikely(reg & MAC_CSR_CMD_CSR_BUSY_)) {
+		printk_err("smsc911x_mac_read failed, MAC busy at entry");
+		retval = -EBUSY;
+		goto exit_unlock;
+	}
+
+	/* Get the MAC address */
+	high = 0;
+	memcpy(&low, &(paddr->sa_data[0]), 4);
+	memcpy(&high, &(paddr->sa_data[4]), 2);
+	printk_debug("Going to set the MAC %04X%08X\n", high, low);
+
+	/* Now set the high address */
+	smsc911x_reg_write(high, pdata, MAC_CSR_DATA);
+	smsc911x_reg_write(ADDRH | MAC_CSR_CMD_CSR_BUSY_, pdata, MAC_CSR_CMD);
+	reg = smsc911x_reg_read(pdata, BYTE_TEST);
+	if (!smsc911x_mac_notbusy(pdata)) {
+		retval = -EBUSY;
+		goto exit_unlock;
+	}
+
+	/* First set the low address */
+	smsc911x_reg_write(low, pdata, MAC_CSR_DATA);
+	smsc911x_reg_write(ADDRL | MAC_CSR_CMD_CSR_BUSY_, pdata, MAC_CSR_CMD);
+	reg = smsc911x_reg_read(pdata, BYTE_TEST);
+	if (!smsc911x_mac_notbusy(pdata)) {
+		retval = -EBUSY;
+		goto exit_unlock;
+	}
+
+	/* And save the IP inside the driver structure */
+	memcpy(dev->dev_addr, paddr->sa_data, dev->addr_len);
+
+	printk_debug("MAC successful changed to %02X%08X\n",
+		     smsc911x_mac_read(pdata, ADDRH),
+		     smsc911x_mac_read(pdata, ADDRL));
+
+	retval = 0;
+
+ exit_unlock:
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+	return retval;
+}
+
+/* Initializing private device structures */
+static int smsc911x_init(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	SMSC_TRACE("Driver Parameters:");
+	SMSC_TRACE("LAN base: 0x%08lX", (unsigned long)pdata->ioaddr);
+	SMSC_TRACE("IRQ: %d", dev->irq);
+	SMSC_TRACE("PHY will be autodetected.");
+
+	if (pdata->ioaddr == 0) {
+		SMSC_WARNING("pdata->ioaddr: 0x00000000");
+		return -ENODEV;
+	}
+
+	/* Default generation to zero (all workarounds apply) */
+	pdata->generation = 0;
+
+	pdata->idrev = smsc911x_reg_read(pdata, ID_REV);
+	if (((pdata->idrev >> 16) & 0xFFFF) == (pdata->idrev & 0xFFFF)) {
+		SMSC_WARNING("idrev top 16 bits equal to bottom 16 bits, "
+			     "idrev: 0x%08X", pdata->idrev);
+		SMSC_TRACE("This may mean the chip is set for 32 bit while "
+			   "the bus is reading as 16 bit");
+		return -ENODEV;
+	}
+	switch (pdata->idrev & 0xFFFF0000) {
+	case 0x01180000:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE("LAN9118 Beacon identified, idrev: 0x%08X",
+				   pdata->idrev);
+			pdata->generation = 0;
+			break;
+		case 1UL:
+			SMSC_TRACE
+			    ("LAN9118 Concord A0 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 1;
+			break;
+		case 2UL:
+			SMSC_TRACE
+			    ("LAN9118 Concord A1 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9118 Concord A1 identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		}
+		break;
+
+	case 0x01170000:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE("LAN9117 Beacon identified, idrev: 0x%08X",
+				   pdata->idrev);
+			pdata->generation = 0;
+			break;
+		case 1UL:
+			SMSC_TRACE
+			    ("LAN9117 Concord A0 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 1;
+			break;
+		case 2UL:
+			SMSC_TRACE
+			    ("LAN9117 Concord A1 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9117 Concord A1 identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		}
+		break;
+
+	case 0x01160000:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_WARNING("LAN911x not identified, idrev: 0x%08X",
+				     pdata->idrev);
+			return -ENODEV;
+		case 1UL:
+			SMSC_TRACE
+			    ("LAN9116 Concord A0 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 1;
+			break;
+		case 2UL:
+			SMSC_TRACE
+			    ("LAN9116 Concord A1 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9116 Concord A1 identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		}
+		break;
+
+	case 0x01150000:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_WARNING("LAN911x not identified, idrev: 0x%08X",
+				     pdata->idrev);
+			return -ENODEV;
+		case 1UL:
+			SMSC_TRACE
+			    ("LAN9115 Concord A0 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 1;
+			break;
+		case 2UL:
+			SMSC_TRACE
+			    ("LAN9115 Concord A1 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9115 Concord A1 identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		}
+		break;
+
+	case 0x118A0000UL:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE
+			    ("LAN9218 Boylston identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9218 Boylston identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		}
+		break;
+
+	case 0x117A0000UL:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE
+			    ("LAN9217 Boylston identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9217 Boylston identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		}
+		break;
+
+	case 0x116A0000UL:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE
+			    ("LAN9216 Boylston identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9216 Boylston identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		}
+		break;
+
+	case 0x115A0000UL:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE
+			    ("LAN9215 Boylston identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9215 Boylston identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		}
+		break;
+
+	case 0x92100000UL:
+	case 0x92110000UL:
+	case 0x92200000UL:
+	case 0x92210000UL:
+		/* LAN9210/LAN9211/LAN9220/LAN9221 */
+		pdata->generation = 4;
+		break;
+
+	default:
+		SMSC_WARNING("LAN911x not identified, idrev: 0x%08X",
+			     pdata->idrev);
+		return -ENODEV;
+	}
+
+	if (pdata->generation == 0)
+		SMSC_WARNING("This driver is not intended "
+			     "for this chip revision");
+
+	ether_setup(dev);
+	dev->open = smsc911x_open;
+	dev->stop = smsc911x_stop;
+	dev->hard_start_xmit = smsc911x_hard_start_xmit;
+	dev->get_stats = smsc911x_get_stats;
+	dev->set_multicast_list = smsc911x_set_multicast_list;
+	dev->flags |= IFF_MULTICAST;
+	dev->do_ioctl = smsc911x_do_ioctl;
+	dev->set_mac_address = smsc911x_set_mac;
+	netif_napi_add(dev, &pdata->napi, smsc911x_poll, 64);
+	dev->ethtool_ops = &smsc911x_ethtool_ops;
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	dev->poll_controller = smsc911x_poll_controller;
+#endif				/* CONFIG_NET_POLL_CONTROLLER */
+
+	pdata->mii.phy_id_mask = 0x1f;
+	pdata->mii.reg_num_mask = 0x1f;
+	pdata->mii.force_media = 0;
+	pdata->mii.full_duplex = 0;
+	pdata->mii.dev = dev;
+	pdata->mii.mdio_read = smsc911x_mdio_read;
+	pdata->mii.mdio_write = smsc911x_mdio_write;
+
+	pdata->msg_enable = NETIF_MSG_LINK;
+
+	return 0;
+}
+
+static int smsc911x_drv_remove(struct platform_device *pdev)
+{
+	struct net_device *dev;
+	struct smsc911x_data *pdata;
+	struct resource *res;
+
+	dev = platform_get_drvdata(pdev);
+	BUG_ON(!dev);
+	pdata = netdev_priv(dev);
+	BUG_ON(!pdata);
+	BUG_ON(!pdata->ioaddr);
+
+	SMSC_TRACE("Stopping driver.");
+	platform_set_drvdata(pdev, NULL);
+	unregister_netdev(dev);
+	free_irq(dev->irq, dev);
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   "smsc911x-memory");
+	if (!res)
+		platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	release_mem_region(res->start, res->end - res->start);
+
+	iounmap(pdata->ioaddr);
+
+	free_netdev(dev);
+
+	return 0;
+}
+
+static int smsc911x_drv_probe(struct platform_device *pdev)
+{
+	struct net_device *dev;
+	struct smsc911x_data *pdata;
+	struct resource *res;
+	unsigned int intcfg = 0;
+	int res_size;
+	int retval;
+
+	printk(KERN_INFO "%s: Driver version %s.\n", SMSC_CHIPNAME,
+		SMSC_DRV_VERSION);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   "smsc911x-memory");
+	if (!res)
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		printk(KERN_WARNING "%s: Could not allocate resource.\n",
+		       SMSC_CHIPNAME);
+		retval = -ENODEV;
+		goto out_0;
+	}
+	res_size = res->end - res->start;
+
+	if (!request_mem_region(res->start, res_size, SMSC_CHIPNAME)) {
+		retval = -EBUSY;
+		goto out_0;
+	}
+
+	dev = alloc_etherdev(sizeof(struct smsc911x_data));
+	if (!dev) {
+		printk(KERN_WARNING "%s: Could not allocate device.\n",
+		       SMSC_CHIPNAME);
+		retval = -ENOMEM;
+		goto out_release_io_1;
+	}
+
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	pdata = netdev_priv(dev);
+	pdata->netdev = dev;
+
+	dev->irq = platform_get_irq(pdev, 0);
+	pdata->ioaddr = ioremap_nocache(res->start, res_size);
+
+	/* copy config parameters across if present, otherwise pdata
+	 * defaults to zeros */
+	if (pdev->dev.platform_data) {
+		struct smc911x_platdata *config = pdev->dev.platform_data;
+		pdata->irq_polarity = config->irq_polarity;
+		pdata->irq_flags  = config->irq_flags;
+	}
+
+	if (pdata->ioaddr == NULL) {
+		SMSC_WARNING("Error smsc911x base address invalid");
+		retval = -ENOMEM;
+		goto out_free_netdev_2;
+	}
+
+	retval = smsc911x_init(dev);
+	if (retval < 0)
+		goto out_unmap_io_3;
+
+	/* configure irq polarity and type before connecting isr */
+	if (pdata->irq_polarity)
+		intcfg |= INT_CFG_IRQ_POL_;
+
+	/*
+	 * @XXX: The "irq_type" is not used at this moment, because we are using
+	 * the same platform-data as the driver from the Vanilla-kernel.
+	 * (Luis Galdos)
+	 */
+	if (pdata->irq_type)
+		intcfg |= INT_CFG_IRQ_TYPE_;
+
+	smsc911x_reg_write(intcfg, pdata, INT_CFG);
+
+	retval = request_irq(dev->irq, smsc911x_irqhandler,
+			     pdata->irq_flags,
+			     SMSC_CHIPNAME, dev);
+	if (retval) {
+		SMSC_WARNING("Unable to claim requested irq: %d", dev->irq);
+		goto out_unmap_io_3;
+	}
+
+	platform_set_drvdata(pdev, dev);
+
+	retval = register_netdev(dev);
+	if (retval) {
+		SMSC_WARNING("Error %i registering device", retval);
+		goto out_unset_drvdata_4;
+	} else {
+		SMSC_TRACE("Network interface: \"%s\"", dev->name);
+	}
+
+	spin_lock_init(&pdata->phy_lock);
+
+	spin_lock_irq(&pdata->phy_lock);
+
+	/* Check if mac address has been specified when bringing interface up */
+	if (is_valid_ether_addr(dev->dev_addr)) {
+		smsc911x_set_mac(dev, dev->dev_addr);
+		SMSC_TRACE("MAC Address is specified by configuration");
+	} else {
+		/* Try reading mac address from device. if EEPROM is present
+		 * it will already have been set */
+		u32 mac_high16 = smsc911x_mac_read(pdata, ADDRH);
+		u32 mac_low32 = smsc911x_mac_read(pdata, ADDRL);
+		dev->dev_addr[0] = (u8)(mac_low32);
+		dev->dev_addr[1] = (u8)(mac_low32 >> 8);
+		dev->dev_addr[2] = (u8)(mac_low32 >> 16);
+		dev->dev_addr[3] = (u8)(mac_low32 >> 24);
+		dev->dev_addr[4] = (u8)(mac_high16);
+		dev->dev_addr[5] = (u8)(mac_high16 >> 8);
+
+		if (is_valid_ether_addr(dev->dev_addr)) {
+			/* eeprom values are valid  so use them */
+			SMSC_TRACE("Mac Address is read from LAN911x EEPROM");
+		} else {
+			/* eeprom values are invalid, generate random MAC */
+			random_ether_addr(dev->dev_addr);
+			smsc911x_set_mac_address(pdata, dev->dev_addr);
+			SMSC_TRACE("MAC Address is set to random_ether_addr");
+		}
+	}
+
+	spin_unlock_irq(&pdata->phy_lock);
+
+	printk_info("%s: MAC Address: %02x:%02x:%02x:%02x:%02x:%02x\n",
+		    dev->name, dev->dev_addr[0], dev->dev_addr[1], dev->dev_addr[2],
+		    dev->dev_addr[3], dev->dev_addr[4], dev->dev_addr[5]);
+
+        /* Enable the wakeup over this device (Luis Galdos) */
+        device_init_wakeup(&pdev->dev, 1);
+        device_set_wakeup_enable(&pdev->dev, 0);
+	return 0;
+
+out_unset_drvdata_4:
+	platform_set_drvdata(pdev, NULL);
+	free_irq(dev->irq, dev);
+out_unmap_io_3:
+	iounmap(pdata->ioaddr);
+out_free_netdev_2:
+	free_netdev(dev);
+out_release_io_1:
+	release_mem_region(res->start, res->end - res->start);
+out_0:
+	return retval;
+}
+
+/* Enter in the suspend mode */
+#if defined(CONFIG_PM)
+
+/*
+ * For the mode D1 we MUST left the interrupts enabled
+ */
+static int smsc911x_drv_state_wakeup(struct smsc911x_data *pdata, int mode)
+{
+	int retval;
+	unsigned long regval;
+
+	retval = 0;
+
+	if (mode != 1 && mode != 2)
+		return -EINVAL;
+
+	/* Clear already received WUs */
+	regval = smsc911x_mac_read(pdata, WUCSR);
+	regval &= ~(WUCSR_MPR_ | WUCSR_WUFR_);
+	regval |= pdata->mac_wucsr; /* Magic packet enable 'WUCSR_MPEN_' */
+	printk_pmdbg("[ SUSP ] WUCSR    0x%08lx\n", regval);
+	smsc911x_mac_write(pdata, WUCSR, regval);
+
+	/* For the D2 we must enable the PHY interrupt for the energy detection */
+	regval =  smsc911x_reg_read(pdata, INT_EN);
+	regval |= (INT_EN_PME_INT_EN_ | INT_EN_PHY_INT_EN_);
+	printk_pmdbg("[ SUSP ] INT_EN   0x%08lx\n", regval);
+	smsc911x_reg_write(regval, pdata, INT_EN);
+
+	if (mode /* @FIXME: Enabled only for D2 */) {
+		u16 phy_mode;
+
+		phy_mode = smsc911x_phy_read(pdata, MII_INTMSK);
+                phy_mode |= PHY_INTMSK_ENERGYON_;
+                smsc911x_phy_write(pdata, MII_INTMSK, phy_mode);
+	}
+
+        /* Clear the PM mode and clear the current wakeup status */
+	regval = smsc911x_reg_read(pdata, PMT_CTRL);
+	regval &= ~PMT_CTRL_PM_MODE_;
+	regval |= PMT_CTRL_WUPS_;
+	printk_pmdbg("[ SUSP ] PMT_CTRL 0x%08lx\n", regval);
+	smsc911x_reg_write(regval, pdata, PMT_CTRL);
+
+	/* Enable the PME at prior and the wake on LAN */
+	regval = smsc911x_reg_read(pdata, PMT_CTRL);
+	regval |= pdata->pmt_ctrl; /* Enable the ENERGY detect or WOL interrupt */
+	regval |= PMT_CTRL_PME_EN_;
+
+	if (mode == 1)
+		regval |= PMT_CTRL_PM_MODE_D1_;
+	else
+		regval |= PMT_CTRL_PM_MODE_D2_;
+
+	printk_pmdbg("[ SUSP ] PMT_CTRL 0x%08lx\n", regval);
+	smsc911x_reg_write(regval, pdata, PMT_CTRL);
+
+	return retval;
+}
+
+/* For the state D2 we must disable the host-interrupts */
+static int smsc911x_drv_state_d2(struct smsc911x_data *pdata)
+{
+	unsigned long regval;
+
+	/* Disable the interrupts of the controller */
+	regval = smsc911x_reg_read(pdata, INT_CFG);
+	regval &= ~INT_CFG_IRQ_EN_;
+	smsc911x_reg_write(regval, pdata, INT_CFG);
+
+	/* Set the phy to the power down mode */
+	regval = smsc911x_phy_read(pdata, MII_BMCR);
+	regval |= BMCR_PDOWN;
+	smsc911x_phy_write(pdata, MII_BMCR, regval);
+
+	/*
+	 * Enter into the power mode D2 (the controller doesn't
+	 * support the mode D3)
+	 */
+	regval = smsc911x_reg_read(pdata, PMT_CTRL);
+	regval &= ~PMT_CTRL_PM_MODE_;
+	regval |= PMT_CTRL_PM_MODE_D2_;
+	smsc911x_reg_write(regval, pdata, PMT_CTRL);
+
+	return 0;
+}
+
+static int smsc911x_drv_suspend(struct platform_device *pdev, pm_message_t state)
+{
+       struct net_device *ndev;
+       struct smsc911x_data *pdata;
+       int retval;
+
+       ndev = platform_get_drvdata(pdev);
+       pdata = netdev_priv(ndev);
+
+       if (!ndev)
+               return -ENODEV;
+
+       /* @FIXME: Implement the other supported power modes of the smsc911x */
+        if (state.event != PM_EVENT_SUSPEND)
+                return -ENOTSUPP;
+
+       if (netif_running(ndev)) {
+
+               /* The below code is coming from the WinCE guys */
+               netif_device_detach(ndev);
+
+	       /*
+		* If configured as wakeup-source enter the mode D1 for packet
+		* detection using the standard IRQ-line
+		*/
+	       if (device_may_wakeup(&pdev->dev)) {
+
+		       /*
+			* Sanity check for verifying that a wakeup-source was
+			* configured from the user space. If the energy-detect
+			* wakeup was enabled, then use the D2 for entering into the
+			* power mode
+			*/
+		       if (!(pdata->pmt_ctrl & (PMT_CTRL_WOL_EN_ | PMT_CTRL_ED_EN_))) {
+			       printk_err("[ SUSP ] No WOL source defined.\n");
+			       retval = -EINVAL;
+			       goto err_attach;
+		       }
+
+		       /*
+			* By the WOL (magic packet, etc.) we can ONLY use the D1, but
+			* for the energy detect over the PHY we can change into D2
+			*/
+		       if (pdata->pmt_ctrl & PMT_CTRL_WOL_EN_) {
+			       printk_pmdbg("[ SUSP ] Preparing D1 with wakeup\n");
+			       smsc911x_drv_state_wakeup(pdata, 1);
+		       } else {
+			       /* @TEST: Use first only D1 for the wakups */
+			       printk_pmdbg("[ SUSP ] Preparing D2 with wakeup\n");
+			       smsc911x_drv_state_wakeup(pdata, 2);
+		       }
+
+		       enable_irq_wake(ndev->irq);
+
+	       } else {
+		       /*
+			* Enter into the power mode D2 (the controller doesn't
+			* support the mode D3)
+			*/
+		       smsc911x_drv_state_d2(pdata);
+	       }
+       }
+
+       return 0;
+
+err_attach:
+       netif_device_attach(ndev);
+       return retval;
+}
+
+static int smsc911x_drv_resume(struct platform_device *pdev)
+{
+	int retval;
+        struct net_device *ndev;
+	unsigned long pmt_ctrl;
+
+	pmt_ctrl = 0;
+	ndev = platform_get_drvdata(pdev);
+	retval = 0;
+        if (ndev) {
+                struct smsc911x_data *pdata = netdev_priv(ndev);
+
+                if (netif_running(ndev)) {
+                       unsigned long timeout;
+                       unsigned long regval, pmt_ctrl;
+
+                       /* Assert the byte test register for waking up */
+                       smsc911x_reg_write(0x0, pdata, BYTE_TEST);
+
+                       timeout = 100000;
+                       do {
+                               timeout--;
+                               regval = smsc911x_reg_read(pdata, PMT_CTRL);
+                               udelay(1);
+                       } while (timeout && !(regval & PMT_CTRL_READY_));
+
+                       if (!timeout) {
+                               printk_err("Wakeup timeout by the controller\n");
+                               retval = -EBUSY;
+			       goto exit_resume;
+                       }
+
+		       /*
+			* Check if we received a PM interrupt
+			* Please take note that we are supporting ONLY the Wake On LAN
+			* interrupts, and not the energy-on
+			* (Luis Galdos)
+			*/
+		       pmt_ctrl = smsc911x_reg_read(pdata, PMT_CTRL);
+		       regval = smsc911x_reg_read(pdata, INT_STS);
+		       printk_pmdbg("[ WAKE ] PMT_CTRL   0x%08lx\n", pmt_ctrl);
+		       printk_pmdbg("[ WAKE ] INT_STS    0x%08lx\n", regval);
+		       if (regval & (INT_STS_PME_INT_ | INT_STS_PHY_INT_)) {
+
+			       /* Disable the power management interrupts */
+			       regval = smsc911x_reg_read(pdata, PMT_CTRL);
+			       pmt_ctrl = regval & (PMT_CTRL_WOL_EN_ | PMT_CTRL_ED_EN_);
+			       regval &= ~(PMT_CTRL_WOL_EN_ | PMT_CTRL_PME_EN_ |
+					   PMT_CTRL_ED_EN_);
+			       smsc911x_reg_write(regval, pdata, PMT_CTRL);
+
+			       /* Disable the PM interrupts */
+			       regval =  smsc911x_reg_read(pdata, INT_EN);
+			       regval &= ~(INT_EN_PME_INT_EN_ | INT_EN_PHY_INT_EN_);
+			       smsc911x_reg_write(regval, pdata, INT_EN);
+
+			       /* Disable the wakeup-events on the MAC */
+			       regval = smsc911x_mac_read(pdata, WUCSR);
+			       regval &= ~(WUCSR_MPEN_);
+			       smsc911x_mac_write(pdata, WUCSR, regval);
+		       }
+
+		       /* @XXX: Clear only the interrupts that were generated */
+		       regval = (INT_STS_PME_INT_ | INT_STS_PHY_INT_);
+		       smsc911x_reg_write(regval, pdata, INT_STS);
+
+		       /* Set the controller into the state D0 */
+                       regval = smsc911x_reg_read(pdata, PMT_CTRL);
+                       regval &= ~PMT_CTRL_PM_MODE_;
+		       regval |= PMT_CTRL_PM_MODE_D0_;
+                       smsc911x_reg_write(regval, pdata, PMT_CTRL);
+
+                       /* Paranoic sanity checks */
+                       regval = smsc911x_reg_read(pdata, PMT_CTRL);
+                       if (regval & PMT_CTRL_PM_MODE_)
+                               printk_err("PM mode isn't disabled (0x%04lx)\n", regval);
+
+                       if (!(regval & PMT_CTRL_READY_)) {
+			       retval = -EBUSY;
+                               printk_err("Device is still NOT ready.\n");
+			       goto exit_resume;
+		       }
+
+                       regval = smsc911x_phy_read(pdata, MII_BMCR);
+                       regval &= ~BMCR_PDOWN;
+                       smsc911x_phy_write(pdata, MII_BMCR, regval);
+
+                       /* Reenable the interrupts now */
+                       regval = smsc911x_reg_read(pdata, INT_CFG);
+                       regval |= INT_CFG_IRQ_EN_;
+                       smsc911x_reg_write(regval, pdata, INT_CFG);
+
+		       /* Reset the wakeup control and status register */
+		       smsc911x_mac_write(pdata, WUCSR, 0x00);
+
+		       netif_device_attach(ndev);
+                }
+        }
+
+exit_resume:
+	return retval;
+}
+#else
+#define smsc911x_drv_suspend                  NULL
+#define smsc911x_drv_resume                   NULL
+#endif /* defined(CONFIG_PM) */
+
+static struct platform_driver smsc911x_driver = {
+	.probe = smsc911x_drv_probe,
+	.remove = smsc911x_drv_remove,
+	.suspend = smsc911x_drv_suspend,
+	.resume = smsc911x_drv_resume,
+	.driver = {
+		.name = SMSC_CHIPNAME,
+	},
+};
+
+/* Entry point for loading the module */
+static int __init smsc911x_init_module(void)
+{
+	printk(KERN_INFO "SMSC 911X: Starting the platform driver.\n");
+	return platform_driver_register(&smsc911x_driver);
+}
+
+/* entry point for unloading the module */
+static void __exit smsc911x_cleanup_module(void)
+{
+	platform_driver_unregister(&smsc911x_driver);
+}
+
+module_init(smsc911x_init_module);
+module_exit(smsc911x_cleanup_module);
diff --git a/drivers/net/smsc9118/smsc911x.h b/drivers/net/smsc9118/smsc911x.h
new file mode 100644
index 000000000000..a2ee96aeb989
--- /dev/null
+++ b/drivers/net/smsc9118/smsc911x.h
@@ -0,0 +1,393 @@
+/***************************************************************************
+ *
+ * Copyright (C) 2004-2007  SMSC
+ * Copyright (C) 2005 ARM
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ ***************************************************************************/
+#ifndef __SMSC911X_H__
+#define __SMSC911X_H__
+
+#if defined(CONFIG_MACH_CC9M2443JS) || defined(CONFIG_MACH_CCW9M2443JS)
+# define SMSC_CAN_USE_32BIT	0
+#else
+# define SMSC_CAN_USE_32BIT	1
+#endif
+
+
+//#define SMSC_CAN_USE_32BIT	1
+#define TX_FIFO_LOW_THRESHOLD	(u32)1600
+#define SMSC911X_EEPROM_SIZE	(u32)7
+#define USE_DEBUG 		0
+//#define USE_DEBUG 		2
+
+/* implements a PHY loopback test at initialisation time, to ensure a packet
+ * can be succesfully looped back */
+#define USE_PHY_WORK_AROUND
+
+/* 10/100 LED link-state inversion when media is disconnected */
+/* #define USE_LED1_WORK_AROUND */
+
+/* platform_device configuration data, should be assigned to
+ * the platform_device's dev.platform_data */
+struct smsc911x_platform_config {
+	unsigned int irq_polarity;
+	unsigned int irq_type;
+};
+
+#if USE_DEBUG >= 1
+#define SMSC_WARNING(fmt, args...) \
+		printk(KERN_EMERG "SMSC_WARNING: %s: " fmt "\n", \
+			__FUNCTION__ , ## args)
+#else
+#define SMSC_WARNING(msg, args...)
+#endif				/* USE_DEBUG >= 1 */
+
+#if USE_DEBUG >= 2
+#define SMSC_TRACE(fmt,args...) \
+		printk(KERN_EMERG "SMSC_TRACE: %s: " fmt "\n", \
+			__FUNCTION__ , ## args)
+#else
+#define SMSC_TRACE(msg, args...)
+#endif				/* USE_DEBUG >= 2 */
+
+/* SMSC911x registers and bitfields */
+#define RX_DATA_FIFO			0x00
+
+#define TX_DATA_FIFO			0x20
+#define TX_CMD_A_ON_COMP_		0x80000000
+#define TX_CMD_A_BUF_END_ALGN_		0x03000000
+#define TX_CMD_A_4_BYTE_ALGN_		0x00000000
+#define TX_CMD_A_16_BYTE_ALGN_		0x01000000
+#define TX_CMD_A_32_BYTE_ALGN_		0x02000000
+#define TX_CMD_A_DATA_OFFSET_		0x001F0000
+#define TX_CMD_A_FIRST_SEG_		0x00002000
+#define TX_CMD_A_LAST_SEG_		0x00001000
+#define TX_CMD_A_BUF_SIZE_		0x000007FF
+#define TX_CMD_B_PKT_TAG_		0xFFFF0000
+#define TX_CMD_B_ADD_CRC_DISABLE_	0x00002000
+#define TX_CMD_B_DISABLE_PADDING_	0x00001000
+#define TX_CMD_B_PKT_BYTE_LENGTH_	0x000007FF
+
+#define RX_STATUS_FIFO			0x40
+#define RX_STS_ES_			0x00008000
+#define RX_STS_MCAST_			0x00000400
+
+#define RX_STATUS_FIFO_PEEK		0x44
+
+#define TX_STATUS_FIFO			0x48
+#define TX_STS_ES_			0x00008000
+
+#define TX_STATUS_FIFO_PEEK		0x4C
+
+#define ID_REV				0x50
+#define ID_REV_CHIP_ID_			0xFFFF0000
+#define ID_REV_REV_ID_			0x0000FFFF
+
+#define INT_CFG				0x54
+#define INT_CFG_INT_DEAS_		0xFF000000
+#define INT_CFG_INT_DEAS_CLR_		0x00004000
+#define INT_CFG_INT_DEAS_STS_		0x00002000
+#define INT_CFG_IRQ_INT_		0x00001000
+#define INT_CFG_IRQ_EN_			0x00000100
+#define INT_CFG_IRQ_POL_		0x00000010
+#define INT_CFG_IRQ_TYPE_		0x00000001
+
+#define INT_STS				0x58
+#define INT_STS_SW_INT_			0x80000000
+#define INT_STS_TXSTOP_INT_		0x02000000
+#define INT_STS_RXSTOP_INT_		0x01000000
+#define INT_STS_RXDFH_INT_		0x00800000
+#define INT_STS_RXDF_INT_		0x00400000
+#define INT_STS_TX_IOC_			0x00200000
+#define INT_STS_RXD_INT_		0x00100000
+#define INT_STS_GPT_INT_		0x00080000
+#define INT_STS_PHY_INT_		0x00040000
+#define INT_STS_PME_INT_		0x00020000
+#define INT_STS_TXSO_			0x00010000
+#define INT_STS_RWT_			0x00008000
+#define INT_STS_RXE_			0x00004000
+#define INT_STS_TXE_			0x00002000
+#define INT_STS_TDFU_			0x00000800
+#define INT_STS_TDFO_			0x00000400
+#define INT_STS_TDFA_			0x00000200
+#define INT_STS_TSFF_			0x00000100
+#define INT_STS_TSFL_			0x00000080
+#define INT_STS_RXDF_			0x00000040
+#define INT_STS_RDFL_			0x00000020
+#define INT_STS_RSFF_			0x00000010
+#define INT_STS_RSFL_			0x00000008
+#define INT_STS_GPIO2_INT_		0x00000004
+#define INT_STS_GPIO1_INT_		0x00000002
+#define INT_STS_GPIO0_INT_		0x00000001
+
+#define INT_EN				0x5C
+#define INT_EN_SW_INT_EN_		0x80000000
+#define INT_EN_TXSTOP_INT_EN_		0x02000000
+#define INT_EN_RXSTOP_INT_EN_		0x01000000
+#define INT_EN_RXDFH_INT_EN_		0x00800000
+#define INT_EN_TIOC_INT_EN_		0x00200000
+#define INT_EN_RXD_INT_EN_		0x00100000
+#define INT_EN_GPT_INT_EN_		0x00080000
+#define INT_EN_PHY_INT_EN_		0x00040000
+#define INT_EN_PME_INT_EN_		0x00020000
+#define INT_EN_TXSO_EN_			0x00010000
+#define INT_EN_RWT_EN_			0x00008000
+#define INT_EN_RXE_EN_			0x00004000
+#define INT_EN_TXE_EN_			0x00002000
+#define INT_EN_TDFU_EN_			0x00000800
+#define INT_EN_TDFO_EN_			0x00000400
+#define INT_EN_TDFA_EN_			0x00000200
+#define INT_EN_TSFF_EN_			0x00000100
+#define INT_EN_TSFL_EN_			0x00000080
+#define INT_EN_RXDF_EN_			0x00000040
+#define INT_EN_RDFL_EN_			0x00000020
+#define INT_EN_RSFF_EN_			0x00000010
+#define INT_EN_RSFL_EN_			0x00000008
+#define INT_EN_GPIO2_INT_		0x00000004
+#define INT_EN_GPIO1_INT_		0x00000002
+#define INT_EN_GPIO0_INT_		0x00000001
+
+#define BYTE_TEST			0x64
+
+#define FIFO_INT			0x68
+#define FIFO_INT_TX_AVAIL_LEVEL_	0xFF000000
+#define FIFO_INT_TX_STS_LEVEL_		0x00FF0000
+#define FIFO_INT_RX_AVAIL_LEVEL_	0x0000FF00
+#define FIFO_INT_RX_STS_LEVEL_		0x000000FF
+
+#define RX_CFG				0x6C
+#define RX_CFG_RX_END_ALGN_		0xC0000000
+#define RX_CFG_RX_END_ALGN4_		0x00000000
+#define RX_CFG_RX_END_ALGN16_		0x40000000
+#define RX_CFG_RX_END_ALGN32_		0x80000000
+#define RX_CFG_RX_DMA_CNT_		0x0FFF0000
+#define RX_CFG_RX_DUMP_			0x00008000
+#define RX_CFG_RXDOFF_			0x00001F00
+
+#define TX_CFG				0x70
+#define TX_CFG_TXS_DUMP_		0x00008000
+#define TX_CFG_TXD_DUMP_		0x00004000
+#define TX_CFG_TXSAO_			0x00000004
+#define TX_CFG_TX_ON_			0x00000002
+#define TX_CFG_STOP_TX_			0x00000001
+
+#define HW_CFG				0x74
+#define HW_CFG_TTM_			0x00200000
+#define HW_CFG_SF_			0x00100000
+#define HW_CFG_TX_FIF_SZ_		0x000F0000
+#define HW_CFG_TR_			0x00003000
+#define HW_CFG_SRST_			0x00000001
+
+/* only available on 115/117 */
+#define HW_CFG_PHY_CLK_SEL_		0x00000060
+#define HW_CFG_PHY_CLK_SEL_INT_PHY_	0x00000000
+#define HW_CFG_PHY_CLK_SEL_EXT_PHY_	0x00000020
+#define HW_CFG_PHY_CLK_SEL_CLK_DIS_	0x00000040
+#define HW_CFG_SMI_SEL_		 	0x00000010
+#define HW_CFG_EXT_PHY_DET_		0x00000008
+#define HW_CFG_EXT_PHY_EN_		0x00000004
+#define HW_CFG_SRST_TO_			0x00000002
+
+/* only available  on 116/118 */
+#define HW_CFG_32_16_BIT_MODE_		0x00000004
+
+#define RX_DP_CTRL			0x78
+#define RX_DP_CTRL_RX_FFWD_		0x80000000
+
+#define RX_FIFO_INF			0x7C
+#define RX_FIFO_INF_RXSUSED_		0x00FF0000
+#define RX_FIFO_INF_RXDUSED_		0x0000FFFF
+
+#define TX_FIFO_INF			0x80
+#define TX_FIFO_INF_TSUSED_		0x00FF0000
+#define TX_FIFO_INF_TDFREE_		0x0000FFFF
+
+#define PMT_CTRL			0x84
+#define PMT_CTRL_PM_MODE_		0x00003000
+#define PMT_CTRL_PM_MODE_D0_		0x00000000
+#define PMT_CTRL_PM_MODE_D1_		0x00001000
+#define PMT_CTRL_PM_MODE_D2_		0x00002000
+#define PMT_CTRL_PM_MODE_D3_		0x00003000
+#define PMT_CTRL_PHY_RST_		0x00000400
+#define PMT_CTRL_WOL_EN_		0x00000200
+#define PMT_CTRL_ED_EN_			0x00000100
+#define PMT_CTRL_PME_TYPE_		0x00000040
+#define PMT_CTRL_WUPS_			0x00000030
+#define PMT_CTRL_WUPS_NOWAKE_		0x00000000
+#define PMT_CTRL_WUPS_ED_		0x00000010
+#define PMT_CTRL_WUPS_WOL_		0x00000020
+#define PMT_CTRL_WUPS_MULTI_		0x00000030
+#define PMT_CTRL_PME_IND_		0x00000008
+#define PMT_CTRL_PME_POL_		0x00000004
+#define PMT_CTRL_PME_EN_		0x00000002
+#define PMT_CTRL_READY_			0x00000001
+
+#define GPIO_CFG			0x88
+#define GPIO_CFG_LED3_EN_		0x40000000
+#define GPIO_CFG_LED2_EN_		0x20000000
+#define GPIO_CFG_LED1_EN_		0x10000000
+#define GPIO_CFG_GPIO2_INT_POL_		0x04000000
+#define GPIO_CFG_GPIO1_INT_POL_		0x02000000
+#define GPIO_CFG_GPIO0_INT_POL_		0x01000000
+#define GPIO_CFG_EEPR_EN_		0x00700000
+#define GPIO_CFG_GPIOBUF2_		0x00040000
+#define GPIO_CFG_GPIOBUF1_		0x00020000
+#define GPIO_CFG_GPIOBUF0_		0x00010000
+#define GPIO_CFG_GPIODIR2_		0x00000400
+#define GPIO_CFG_GPIODIR1_		0x00000200
+#define GPIO_CFG_GPIODIR0_		0x00000100
+#define GPIO_CFG_GPIOD4_		0x00000020
+#define GPIO_CFG_GPIOD3_		0x00000010
+#define GPIO_CFG_GPIOD2_		0x00000004
+#define GPIO_CFG_GPIOD1_		0x00000002
+#define GPIO_CFG_GPIOD0_		0x00000001
+
+#define GPT_CFG				0x8C
+#define GPT_CFG_TIMER_EN_		0x20000000
+#define GPT_CFG_GPT_LOAD_		0x0000FFFF
+
+#define GPT_CNT				0x90
+#define GPT_CNT_GPT_CNT_		0x0000FFFF
+
+#define ENDIAN				0x98
+
+#define FREE_RUN			0x9C
+
+#define RX_DROP				0xA0
+
+#define MAC_CSR_CMD			0xA4
+#define MAC_CSR_CMD_CSR_BUSY_		0x80000000
+#define MAC_CSR_CMD_R_NOT_W_		0x40000000
+#define MAC_CSR_CMD_CSR_ADDR_		0x000000FF
+
+#define MAC_CSR_DATA			0xA8
+
+#define AFC_CFG				0xAC
+#define AFC_CFG_AFC_HI_			0x00FF0000
+#define AFC_CFG_AFC_LO_			0x0000FF00
+#define AFC_CFG_BACK_DUR_		0x000000F0
+#define AFC_CFG_FCMULT_			0x00000008
+#define AFC_CFG_FCBRD_			0x00000004
+#define AFC_CFG_FCADD_			0x00000002
+#define AFC_CFG_FCANY_			0x00000001
+
+#define E2P_CMD				0xB0
+#define E2P_CMD_EPC_BUSY_		0x80000000
+#define E2P_CMD_EPC_CMD_		0x70000000
+#define E2P_CMD_EPC_CMD_READ_		0x00000000
+#define E2P_CMD_EPC_CMD_EWDS_		0x10000000
+#define E2P_CMD_EPC_CMD_EWEN_		0x20000000
+#define E2P_CMD_EPC_CMD_WRITE_		0x30000000
+#define E2P_CMD_EPC_CMD_WRAL_		0x40000000
+#define E2P_CMD_EPC_CMD_ERASE_		0x50000000
+#define E2P_CMD_EPC_CMD_ERAL_		0x60000000
+#define E2P_CMD_EPC_CMD_RELOAD_		0x70000000
+#define E2P_CMD_EPC_TIMEOUT_		0x00000200
+#define E2P_CMD_MAC_ADDR_LOADED_	0x00000100
+#define E2P_CMD_EPC_ADDR_		0x000000FF
+
+#define E2P_DATA			0xB4
+#define E2P_DATA_EEPROM_DATA_		0x000000FF
+#define LAN_REGISTER_EXTENT		0x00000100
+
+/*
+ * MAC Control and Status Register (Indirect Address)
+ * Offset (through the MAC_CSR CMD and DATA port)
+ */
+#define MAC_CR				0x01
+#define MAC_CR_RXALL_			0x80000000
+#define MAC_CR_HBDIS_			0x10000000
+#define MAC_CR_RCVOWN_			0x00800000
+#define MAC_CR_LOOPBK_			0x00200000
+#define MAC_CR_FDPX_			0x00100000
+#define MAC_CR_MCPAS_			0x00080000
+#define MAC_CR_PRMS_			0x00040000
+#define MAC_CR_INVFILT_			0x00020000
+#define MAC_CR_PASSBAD_			0x00010000
+#define MAC_CR_HFILT_			0x00008000
+#define MAC_CR_HPFILT_			0x00002000
+#define MAC_CR_LCOLL_			0x00001000
+#define MAC_CR_BCAST_			0x00000800
+#define MAC_CR_DISRTY_			0x00000400
+#define MAC_CR_PADSTR_			0x00000100
+#define MAC_CR_BOLMT_MASK_		0x000000C0
+#define MAC_CR_DFCHK_			0x00000020
+#define MAC_CR_TXEN_			0x00000008
+#define MAC_CR_RXEN_			0x00000004
+
+#define ADDRH				0x02
+
+#define ADDRL				0x03
+
+#define HASHH				0x04
+
+#define HASHL				0x05
+
+#define MII_ACC				0x06
+#define MII_ACC_PHY_ADDR_		0x0000F800
+#define MII_ACC_MIIRINDA_		0x000007C0
+#define MII_ACC_MII_WRITE_		0x00000002
+#define MII_ACC_MII_BUSY_		0x00000001
+
+#define MII_DATA			0x07
+
+#define FLOW				0x08
+#define FLOW_FCPT_			0xFFFF0000
+#define FLOW_FCPASS_			0x00000004
+#define FLOW_FCEN_			0x00000002
+#define FLOW_FCBSY_			0x00000001
+
+#define VLAN1				0x09
+
+#define VLAN2				0x0A
+
+#define WUFF				0x0B
+
+#define WUCSR				0x0C
+#define WUCSR_GUE_			0x00000200
+#define WUCSR_WUFR_			0x00000040
+#define WUCSR_MPR_			0x00000020
+#define WUCSR_WAKE_EN_			0x00000004
+#define WUCSR_MPEN_			0x00000002
+
+/*
+ * Phy definitions (vendor-specific)
+ */
+#define LAN9118_PHY_ID			0x00C0001C
+
+#define MII_INTSTS			0x1D
+
+#define MII_INTMSK			0x1E
+#define PHY_INTMSK_AN_RCV_		(1 << 1)
+#define PHY_INTMSK_PDFAULT_		(1 << 2)
+#define PHY_INTMSK_AN_ACK_		(1 << 3)
+#define PHY_INTMSK_LNKDOWN_		(1 << 4)
+#define PHY_INTMSK_RFAULT_		(1 << 5)
+#define PHY_INTMSK_AN_COMP_		(1 << 6)
+#define PHY_INTMSK_ENERGYON_		(1 << 7)
+#define PHY_INTMSK_DEFAULT_		(PHY_INTMSK_ENERGYON_ | \
+					 PHY_INTMSK_AN_COMP_ | \
+					 PHY_INTMSK_RFAULT_ | \
+					 PHY_INTMSK_LNKDOWN_)
+
+#define ADVERTISE_PAUSE_ALL		(ADVERTISE_PAUSE_CAP | \
+					 ADVERTISE_PAUSE_ASYM)
+
+#define LPA_PAUSE_ALL			(LPA_PAUSE_CAP | \
+					 LPA_PAUSE_ASYM)
+
+#endif				/* __SMSC911X_H__ */
diff --git a/drivers/net/smsc911x.c b/drivers/net/smsc911x.c
new file mode 100644
index 000000000000..6af63187bd3a
--- /dev/null
+++ b/drivers/net/smsc911x.c
@@ -0,0 +1,2253 @@
+/***************************************************************************
+ *
+ * Copyright (C) 2004-2007  SMSC
+ * Copyright (C) 2005 ARM
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ ***************************************************************************
+ * Rewritten, heavily based on smsc911x simple driver by SMSC.
+ * Partly uses io macros from smc91x.c by Nicolas Pitre
+ *
+ * Supported devices:
+ *   LAN9115, LAN9116, LAN9117, LAN9118
+ *   LAN9215, LAN9216, LAN9217, LAN9218
+ *
+ * History:
+ *   05/05/2005 bahadir.balban@arm.com
+ *     - Transition to linux coding style
+ *     - Platform driver and module interface
+ *
+ *   17/07/2006 steve.glendinning@smsc.com
+ *     - Added support for LAN921x family
+ *     - Added workaround for multicast filters
+ *
+ *   31/07/2006 steve.glendinning@smsc.com
+ *     - Removed tasklet, using NAPI poll instead
+ *     - Multiple device support
+ *     - Large tidy-up following feedback from netdev list
+ *
+ *   03/08/2006 steve.glendinning@smsc.com
+ *     - Added ethtool support
+ *     - Convert to use generic MII interface
+ *
+ *   04/08/2006 bahadir.balban@arm.com
+ *     - Added ethtool eeprom r/w support
+ *
+ *   17/06/2007 steve.glendinning@smsc.com
+ *     - Incorporate changes from Bill Gatliff and Russell King
+ *
+ *   04/07/2007 steve.glendinning@smsc.com
+ *     - move irq configuration to platform_device
+ *     - fix link poller after interface is stopped and restarted
+ *
+ *   13/07/2007 bahadir.balban@arm.com
+ *     - set irq polarity before requesting irq
+ */
+
+#include <linux/crc32.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/etherdevice.h>
+#include <linux/ethtool.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/kernel.h>
+#include <linux/mii.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/timer.h>
+#include <linux/version.h>
+#include <linux/bug.h>
+#include <linux/bitops.h>
+#include <linux/irq.h>
+#include <asm/io.h>
+#include "smsc911x.h"
+
+#define SMSC_CHIPNAME		"smsc911x"
+#define SMSC_DRV_VERSION	"2007-07-13"
+
+MODULE_LICENSE("GPL");
+
+struct smsc911x_data {
+	void __iomem *ioaddr;
+
+	unsigned int idrev;
+	unsigned int generation;	/* used to decide which workarounds apply */
+
+	/* device configuration */
+	unsigned int irq_polarity;
+	unsigned int irq_type;
+
+	/* This needs to be acquired before calling any of below:
+	 * smsc911x_mac_read(), smsc911x_mac_write()
+	 * smsc911x_phy_read(), smsc911x_phy_write()
+	 */
+	spinlock_t phy_lock;
+
+	struct net_device_stats stats;
+	struct mii_if_info mii;
+	unsigned int using_extphy;
+	u32 msg_enable;
+#ifdef USE_LED1_WORK_AROUND
+	unsigned int gpio_setting;
+	unsigned int gpio_orig_setting;
+#endif
+	struct net_device *netdev;
+	struct napi_struct napi;
+	struct timer_list link_poll_timer;
+	unsigned int stop_link_poll;
+
+	unsigned int software_irq_signal;
+
+#ifdef USE_PHY_WORK_AROUND
+#define MIN_PACKET_SIZE (64)
+	char loopback_tx_pkt[MIN_PACKET_SIZE];
+	char loopback_rx_pkt[MIN_PACKET_SIZE];
+	unsigned int resetcount;
+#endif
+
+	/* Members for Multicast filter workaround */
+	unsigned int multicast_update_pending;
+	unsigned int set_bits_mask;
+	unsigned int clear_bits_mask;
+	unsigned int hashhi;
+	unsigned int hashlo;
+};
+
+#if SMSC_CAN_USE_32BIT
+
+static inline u32 smsc911x_reg_read(struct smsc911x_data *pdata, u32 reg)
+{
+	return readl(pdata->ioaddr + reg);
+}
+
+static inline void smsc911x_reg_write(u32 val, struct smsc911x_data *pdata,
+				      u32 reg)
+{
+	writel(val, pdata->ioaddr + reg);
+}
+
+#elif SMSC_CAN_USE_SPI
+
+static u32 smsc911x_reg_read(struct smsc911x_data *pdata, u32 reg)
+{
+	u32 reg_val;
+	unsigned long flags;
+
+	local_irq_save(flags);
+	reg_val = spi_cpld_read(reg);
+	local_irq_restore(flags);
+
+	return reg_val;
+}
+
+static void smsc911x_reg_write(u32 val, struct smsc911x_data *pdata,
+				      u32 reg)
+{
+	unsigned long flags;
+
+	local_irq_save(flags);
+	spi_cpld_write(reg, val);
+	local_irq_restore(flags);
+}
+
+#else				/* SMSC_CAN_USE_32BIT */
+
+static inline u32 smsc911x_reg_read(struct smsc911x_data *pdata, u32 reg)
+{
+	u32 reg_val;
+	unsigned long flags;
+
+	/* these two 16-bit reads must be performed consecutively, so must
+	 * not be interrupted by our own ISR (which would start another
+	 * read operation) */
+	local_irq_save(flags);
+	reg_val = ((readw(pdata->ioaddr + reg) & 0xFFFF) |
+		   ((readw(pdata->ioaddr + reg + 2) & 0xFFFF) << 16));
+	local_irq_restore(flags);
+
+	return reg_val;
+}
+
+static inline void smsc911x_reg_write(u32 val, struct smsc911x_data *pdata,
+				      u32 reg)
+{
+	unsigned long flags;
+
+	/* these two 16-bit writes must be performed consecutively, so must
+	 * not be interrupted by our own ISR (which would start another
+	 * read operation) */
+	local_irq_save(flags);
+	writew(val & 0xFFFF, pdata->ioaddr + reg);
+	writew((val >> 16) & 0xFFFF, pdata->ioaddr + reg + 2);
+	local_irq_restore(flags);
+}
+
+#endif				/* SMSC_CAN_USE_32BIT */
+
+/* Writes a packet to the TX_DATA_FIFO */
+static inline void
+smsc911x_tx_writefifo(struct smsc911x_data *pdata, unsigned int *buf,
+		      unsigned int wordcount)
+{
+	while (wordcount--)
+		smsc911x_reg_write(*buf++, pdata, TX_DATA_FIFO);
+}
+
+/* Reads a packet out of the RX_DATA_FIFO */
+static inline void
+smsc911x_rx_readfifo(struct smsc911x_data *pdata, unsigned int *buf,
+		     unsigned int wordcount)
+{
+	while (wordcount--)
+		*buf++ = smsc911x_reg_read(pdata, RX_DATA_FIFO);
+}
+
+/* waits for MAC not busy, with timeout.  Only called by smsc911x_mac_read
+ * and smsc911x_mac_write, so assumes phy_lock is held */
+static int smsc911x_mac_notbusy(struct smsc911x_data *pdata)
+{
+	int i;
+	u32 val;
+
+	for (i = 0; i < 40; i++) {
+		val = smsc911x_reg_read(pdata, MAC_CSR_CMD);
+		if (!(val & MAC_CSR_CMD_CSR_BUSY_))
+			return 1;
+	}
+	SMSC_WARNING("Timed out waiting for MAC not BUSY. "
+		     "MAC_CSR_CMD: 0x%08X", val);
+	return 0;
+}
+
+/* Fetches a MAC register value. Assumes phy_lock is acquired */
+static u32 smsc911x_mac_read(struct smsc911x_data *pdata, unsigned int offset)
+{
+	unsigned int temp;
+
+#ifdef CONFIG_DEBUG_SPINLOCK
+	if (!spin_is_locked(&pdata->phy_lock))
+		SMSC_WARNING("phy_lock not held");
+#endif				/* CONFIG_DEBUG_SPINLOCK */
+
+	temp = smsc911x_reg_read(pdata, MAC_CSR_CMD);
+	if (unlikely(temp & MAC_CSR_CMD_CSR_BUSY_)) {
+		SMSC_WARNING("smsc911x_mac_read failed, MAC busy at entry");
+		return 0xFFFFFFFF;
+	}
+
+	/* Send the MAC cmd */
+	smsc911x_reg_write(((offset & 0xFF) | MAC_CSR_CMD_CSR_BUSY_
+			    | MAC_CSR_CMD_R_NOT_W_), pdata, MAC_CSR_CMD);
+
+	/* Workaround for hardware read-after-write restriction */
+	temp = smsc911x_reg_read(pdata, BYTE_TEST);
+
+	/* Wait for the read to happen */
+	if (likely(smsc911x_mac_notbusy(pdata)))
+		return smsc911x_reg_read(pdata, MAC_CSR_DATA);
+
+	SMSC_WARNING("smsc911x_mac_read failed, MAC busy after read");
+	return 0xFFFFFFFF;
+}
+
+/* Set a mac register, phy_lock must be acquired before calling */
+static void smsc911x_mac_write(struct smsc911x_data *pdata,
+			       unsigned int offset, u32 val)
+{
+	unsigned int temp;
+
+#ifdef CONFIG_DEBUG_SPINLOCK
+	if (!spin_is_locked(&pdata->phy_lock))
+		SMSC_WARNING("phy_lock not held");
+#endif				/* CONFIG_DEBUG_SPINLOCK */
+
+	temp = smsc911x_reg_read(pdata, MAC_CSR_CMD);
+	if (unlikely(temp & MAC_CSR_CMD_CSR_BUSY_)) {
+		SMSC_WARNING("smsc911x_mac_write failed, MAC busy at entry");
+		return;
+	}
+
+	/* Send data to write */
+	smsc911x_reg_write(val, pdata, MAC_CSR_DATA);
+
+	/* Write the actual data */
+	smsc911x_reg_write(((offset & 0xFF) | MAC_CSR_CMD_CSR_BUSY_), pdata,
+			   MAC_CSR_CMD);
+
+	/* Workaround for hardware read-after-write restriction */
+	temp = smsc911x_reg_read(pdata, BYTE_TEST);
+
+	/* Wait for the write to complete */
+	if (likely(smsc911x_mac_notbusy(pdata)))
+		return;
+
+	SMSC_WARNING("smsc911x_mac_write failed, MAC busy after write");
+}
+
+/* Gets a phy register, phy_lock must be acquired before calling */
+static u16 smsc911x_phy_read(struct smsc911x_data *pdata, unsigned int index)
+{
+	unsigned int addr;
+	int i;
+
+#ifdef CONFIG_DEBUG_SPINLOCK
+	if (!spin_is_locked(&pdata->phy_lock))
+		SMSC_WARNING("phy_lock not held");
+#endif				/* CONFIG_DEBUG_SPINLOCK */
+
+	/* Confirm MII not busy */
+	if (unlikely(smsc911x_mac_read(pdata, MII_ACC) & MII_ACC_MII_BUSY_)) {
+		SMSC_WARNING("MII is busy in smsc911x_phy_read???");
+		return 0;
+	}
+
+	/* Set the address, index & direction (read from PHY) */
+	addr = (((pdata->mii.phy_id) & 0x1F) << 11)
+	    | ((index & 0x1F) << 6);
+	smsc911x_mac_write(pdata, MII_ACC, addr);
+
+	/* Wait for read to complete w/ timeout */
+	for (i = 0; i < 100; i++) {
+		/* See if MII is finished yet */
+		if (!(smsc911x_mac_read(pdata, MII_ACC) & MII_ACC_MII_BUSY_)) {
+			return smsc911x_mac_read(pdata, MII_DATA);
+		}
+	}
+	SMSC_WARNING("Timed out waiting for MII write to finish");
+	return 0xFFFF;
+}
+
+/* Sets a phy register, phy_lock must be acquired before calling */
+static void smsc911x_phy_write(struct smsc911x_data *pdata,
+			       unsigned int index, u16 val)
+{
+	unsigned int addr;
+	int i;
+
+#ifdef CONFIG_DEBUG_SPINLOCK
+	if (!spin_is_locked(&pdata->phy_lock))
+		SMSC_WARNING("phy_lock not held");
+#endif				/* CONFIG_DEBUG_SPINLOCK */
+
+	/* Confirm MII not busy */
+	if (unlikely(smsc911x_mac_read(pdata, MII_ACC) & MII_ACC_MII_BUSY_)) {
+		SMSC_WARNING("MII is busy in smsc911x_write_phy???");
+		return;
+	}
+
+	/* Put the data to write in the MAC */
+	smsc911x_mac_write(pdata, MII_DATA, val);
+
+	/* Set the address, index & direction (write to PHY) */
+	addr = (((pdata->mii.phy_id) & 0x1F) << 11) |
+	    ((index & 0x1F) << 6) | MII_ACC_MII_WRITE_;
+	smsc911x_mac_write(pdata, MII_ACC, addr);
+
+	/* Wait for write to complete w/ timeout */
+	for (i = 0; i < 100; i++) {
+		/* See if MII is finished yet */
+		if (!(smsc911x_mac_read(pdata, MII_ACC) & MII_ACC_MII_BUSY_))
+			return;
+	}
+	SMSC_WARNING("Timed out waiting for MII write to finish");
+}
+
+static int smsc911x_mdio_read(struct net_device *dev, int phy_id, int location)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned long flags;
+	int reg;
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	reg = smsc911x_phy_read(pdata, location);
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+	return reg;
+}
+
+static void smsc911x_mdio_write(struct net_device *dev, int phy_id,
+				int location, int val)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	smsc911x_phy_write(pdata, location, val);
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+}
+
+/* Autodetects and initialises external phy for SMSC9115 and SMSC9117 flavors.
+ * If something goes wrong, returns -ENODEV to revert back to internal phy.
+ * Performed at initialisation only, so interrupts are enabled */
+static int smsc911x_phy_initialise_external(struct smsc911x_data *pdata)
+{
+	unsigned int address;
+	unsigned int hwcfg;
+	unsigned int phyid1;
+	unsigned int phyid2;
+
+	hwcfg = smsc911x_reg_read(pdata, HW_CFG);
+
+	/* External phy is requested, supported, and detected */
+	if (hwcfg & HW_CFG_EXT_PHY_DET_) {
+
+		/* Attempt to switch to external phy for auto-detecting
+		 * its address. Assuming tx and rx are stopped because
+		 * smsc911x_phy_initialise is called before
+		 * smsc911x_rx_initialise and tx_initialise.
+		 */
+
+		/* Disable phy clocks to the MAC */
+		hwcfg &= (~HW_CFG_PHY_CLK_SEL_);
+		hwcfg |= HW_CFG_PHY_CLK_SEL_CLK_DIS_;
+		smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+		udelay(10);	/* Enough time for clocks to stop */
+
+		/* Switch to external phy */
+		hwcfg |= HW_CFG_EXT_PHY_EN_;
+		smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+
+		/* Enable phy clocks to the MAC */
+		hwcfg &= (~HW_CFG_PHY_CLK_SEL_);
+		hwcfg |= HW_CFG_PHY_CLK_SEL_EXT_PHY_;
+		smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+		udelay(10);	/* Enough time for clocks to restart */
+
+		hwcfg |= HW_CFG_SMI_SEL_;
+		smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+
+		/* Auto-detect PHY */
+		spin_lock_irq(&pdata->phy_lock);
+		for (address = 0; address <= 31; address++) {
+			pdata->mii.phy_id = address;
+			phyid1 = smsc911x_phy_read(pdata, MII_PHYSID1);
+			phyid2 = smsc911x_phy_read(pdata, MII_PHYSID2);
+			if ((phyid1 != 0xFFFFU) || (phyid2 != 0xFFFFU)) {
+				SMSC_TRACE("Detected PHY at address = "
+					   "0x%02X = %d", address, address);
+				break;
+			}
+		}
+		spin_unlock_irq(&pdata->phy_lock);
+
+		if ((phyid1 == 0xFFFFU) && (phyid2 == 0xFFFFU)) {
+			SMSC_WARNING("External PHY is not accessable, "
+				     "using internal PHY instead");
+			/* Revert back to internal phy settings. */
+
+			/* Disable phy clocks to the MAC */
+			hwcfg &= (~HW_CFG_PHY_CLK_SEL_);
+			hwcfg |= HW_CFG_PHY_CLK_SEL_CLK_DIS_;
+			smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+			udelay(10);	/* Enough time for clocks to stop */
+
+			/* Switch to internal phy */
+			hwcfg &= (~HW_CFG_EXT_PHY_EN_);
+			smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+
+			/* Enable phy clocks to the MAC */
+			hwcfg &= (~HW_CFG_PHY_CLK_SEL_);
+			hwcfg |= HW_CFG_PHY_CLK_SEL_INT_PHY_;
+			smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+			udelay(10);	/* Enough time for clocks to restart */
+
+			hwcfg &= (~HW_CFG_SMI_SEL_);
+			smsc911x_reg_write(hwcfg, pdata, HW_CFG);
+			/* Use internal phy */
+			return -ENODEV;
+		} else {
+			SMSC_TRACE("Successfully switched to external PHY");
+			pdata->using_extphy = 1;
+		}
+	} else {
+		SMSC_WARNING("No external PHY detected.");
+		SMSC_WARNING("Using internal PHY instead.");
+		/* Use internal phy */
+		return -ENODEV;
+	}
+	return 0;
+}
+
+/* called by phy_initialise and loopback test */
+static int smsc911x_phy_reset(struct smsc911x_data *pdata)
+{
+	unsigned int temp;
+	unsigned int i = 100000;
+	unsigned long flags;
+
+	SMSC_TRACE("Performing PHY BCR Reset");
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	smsc911x_phy_write(pdata, MII_BMCR, BMCR_RESET);
+	do {
+		udelay(10);
+		temp = smsc911x_phy_read(pdata, MII_BMCR);
+	} while ((i--) && (temp & BMCR_RESET));
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+	if (temp & BMCR_RESET) {
+		SMSC_WARNING("PHY reset failed to complete.");
+		return 0;
+	}
+	/* Extra delay required because the phy may not be completed with
+	 * its reset when BMCR_RESET is cleared. Specs say 256 uS is
+	 * enough delay but using 1ms here to be safe
+	 */
+	msleep(1);
+
+	return 1;
+}
+
+/* Fetches a tx status out of the status fifo */
+static unsigned int smsc911x_tx_get_txstatus(struct smsc911x_data *pdata)
+{
+	unsigned int result =
+	    smsc911x_reg_read(pdata, TX_FIFO_INF) & TX_FIFO_INF_TSUSED_;
+
+	if (result != 0)
+		result = smsc911x_reg_read(pdata, TX_STATUS_FIFO);
+
+	return result;
+}
+
+/* Fetches the next rx status */
+static unsigned int smsc911x_rx_get_rxstatus(struct smsc911x_data *pdata)
+{
+	unsigned int result =
+	    smsc911x_reg_read(pdata, RX_FIFO_INF) & RX_FIFO_INF_RXSUSED_;
+
+	if (result != 0)
+		result = smsc911x_reg_read(pdata, RX_STATUS_FIFO);
+
+	return result;
+}
+
+#ifdef USE_PHY_WORK_AROUND
+static int smsc911x_phy_check_loopbackpkt(struct smsc911x_data *pdata)
+{
+	unsigned int tries;
+	u32 wrsz;
+	u32 rdsz;
+	u32 bufp;
+
+	for (tries = 0; tries < 10; tries++) {
+		unsigned int txcmd_a;
+		unsigned int txcmd_b;
+		unsigned int status;
+		unsigned int pktlength;
+		unsigned int i;
+
+		/* Zero-out rx packet memory */
+		memset(pdata->loopback_rx_pkt, 0, MIN_PACKET_SIZE);
+
+		/* Write tx packet to 118 */
+		txcmd_a = (((unsigned int)pdata->loopback_tx_pkt)
+			   & 0x03) << 16;
+		txcmd_a |= TX_CMD_A_FIRST_SEG_ | TX_CMD_A_LAST_SEG_;
+		txcmd_a |= MIN_PACKET_SIZE;
+
+		txcmd_b = MIN_PACKET_SIZE << 16 | MIN_PACKET_SIZE;
+
+		smsc911x_reg_write(txcmd_a, pdata, TX_DATA_FIFO);
+		smsc911x_reg_write(txcmd_b, pdata, TX_DATA_FIFO);
+
+		bufp = ((u32) pdata->loopback_tx_pkt) & 0xFFFFFFFC;
+		wrsz = MIN_PACKET_SIZE + 3;
+		wrsz += (((u32) pdata->loopback_tx_pkt) & 0x3);
+		wrsz >>= 2;
+
+		smsc911x_tx_writefifo(pdata, (unsigned int *)bufp, wrsz);
+
+		/* Wait till transmit is done */
+		i = 60;
+		do {
+			udelay(5);
+			status = smsc911x_tx_get_txstatus(pdata);
+		} while ((i--) && (!status));
+
+		if (!status) {
+			SMSC_WARNING("Failed to transmit during loopback test");
+			continue;
+		}
+		if (status & TX_STS_ES_) {
+			SMSC_WARNING("Transmit encountered errors during "
+				     "loopback test");
+			continue;
+		}
+
+		/* Wait till receive is done */
+		i = 60;
+		do {
+			udelay(5);
+			status = smsc911x_rx_get_rxstatus(pdata);
+		} while ((i--) && (!status));
+
+		if (!status) {
+			SMSC_WARNING("Failed to receive during loopback test");
+			continue;
+		}
+		if (status & RX_STS_ES_) {
+			SMSC_WARNING("Receive encountered errors during "
+				     "loopback test");
+			continue;
+		}
+
+		pktlength = ((status & 0x3FFF0000UL) >> 16);
+		bufp = (u32)pdata->loopback_rx_pkt;
+		rdsz = pktlength + 3;
+		rdsz += ((u32)pdata->loopback_rx_pkt) & 0x3;
+		rdsz >>= 2;
+
+		smsc911x_rx_readfifo(pdata, (unsigned int *)bufp, rdsz);
+
+		if (pktlength != (MIN_PACKET_SIZE + 4)) {
+			SMSC_WARNING("Unexpected packet size during "
+				     "loop back test, size=%d, "
+				     "will retry", pktlength);
+		} else {
+			unsigned int j;
+			int mismatch = 0;
+			for (j = 0; j < MIN_PACKET_SIZE; j++) {
+				if (pdata->loopback_tx_pkt[j]
+				    != pdata->loopback_rx_pkt[j]) {
+					mismatch = 1;
+					break;
+				}
+			}
+			if (!mismatch) {
+				SMSC_TRACE("Successfully verified "
+					   "loopback packet");
+				return 1;
+			} else {
+				SMSC_WARNING("Data miss match during "
+					     "loop back test, will retry.");
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int smsc911x_phy_loopbacktest(struct smsc911x_data *pdata)
+{
+	int result = 0;
+	unsigned int i;
+	unsigned int val;
+	unsigned long flags;
+
+	/* Initialise tx packet */
+	for (i = 0; i < 6; i++) {
+		/* Use broadcast destination address */
+		pdata->loopback_tx_pkt[i] = (char)0xFF;
+	}
+
+	for (i = 6; i < 12; i++) {
+		/* Use incrementing source address */
+		pdata->loopback_tx_pkt[i] = (char)i;
+	}
+
+	/* Set length type field */
+	pdata->loopback_tx_pkt[12] = 0x00;
+	pdata->loopback_tx_pkt[13] = 0x00;
+	for (i = 14; i < MIN_PACKET_SIZE; i++) {
+		pdata->loopback_tx_pkt[i] = (char)i;
+	}
+
+	val = smsc911x_reg_read(pdata, HW_CFG);
+	val &= HW_CFG_TX_FIF_SZ_;
+	val |= HW_CFG_SF_;
+	smsc911x_reg_write(val, pdata, HW_CFG);
+
+	smsc911x_reg_write(TX_CFG_TX_ON_, pdata, TX_CFG);
+	smsc911x_reg_write((((unsigned int)pdata->loopback_rx_pkt)
+			    & 0x03) << 8, pdata, RX_CFG);
+
+	for (i = 0; i < 10; i++) {
+		/* Set PHY to 10/FD, no ANEG, and loopback mode */
+		spin_lock_irqsave(&pdata->phy_lock, flags);
+		smsc911x_phy_write(pdata, MII_BMCR, 0x4100);
+
+		/* Enable MAC tx/rx, FD */
+		smsc911x_mac_write(pdata, MAC_CR, MAC_CR_FDPX_
+				   | MAC_CR_TXEN_ | MAC_CR_RXEN_);
+		spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+		if (smsc911x_phy_check_loopbackpkt(pdata)) {
+			result = 1;
+			break;
+		}
+		pdata->resetcount++;
+
+		/* Disable MAC rx */
+		spin_lock_irqsave(&pdata->phy_lock, flags);
+		smsc911x_mac_write(pdata, MAC_CR, 0);
+		spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+		smsc911x_phy_reset(pdata);
+	}
+
+	/* Disable MAC */
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	smsc911x_mac_write(pdata, MAC_CR, 0);
+
+	/* Cancel PHY loopback mode */
+	smsc911x_phy_write(pdata, MII_BMCR, 0);
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+	smsc911x_reg_write(0, pdata, TX_CFG);
+	smsc911x_reg_write(0, pdata, RX_CFG);
+
+	return result;
+}
+#endif				/* USE_PHY_WORK_AROUND */
+
+/* assumes phy_lock is held */
+static void smsc911x_phy_update_flowcontrol(struct smsc911x_data *pdata)
+{
+	unsigned int temp;
+
+	if (pdata->mii.full_duplex) {
+		unsigned int phy_adv;
+		unsigned int phy_lpa;
+		phy_adv = smsc911x_phy_read(pdata, MII_ADVERTISE);
+		phy_lpa = smsc911x_phy_read(pdata, MII_LPA);
+		if (phy_adv & phy_lpa & LPA_PAUSE_CAP) {
+			/* Both ends support symmetric pause, enable
+			 * PAUSE receive and transmit */
+			smsc911x_mac_write(pdata, FLOW, 0xFFFF0002);
+			temp = smsc911x_reg_read(pdata, AFC_CFG);
+			temp |= 0xF;
+			smsc911x_reg_write(temp, pdata, AFC_CFG);
+		} else if (((phy_adv & ADVERTISE_PAUSE_ALL) ==
+			    ADVERTISE_PAUSE_ALL) &&
+			   ((phy_lpa & LPA_PAUSE_ALL) == LPA_PAUSE_ASYM)) {
+			/* We support symmetric and asym pause, the
+			 * other end only supports asym, Enable PAUSE
+			 * receive, disable PAUSE transmit */
+			smsc911x_mac_write(pdata, FLOW, 0xFFFF0002);
+			temp = smsc911x_reg_read(pdata, AFC_CFG);
+			temp &= ~0xF;
+			smsc911x_reg_write(temp, pdata, AFC_CFG);
+		} else {
+			/* Disable PAUSE receive and transmit */
+			smsc911x_mac_write(pdata, FLOW, 0);
+			temp = smsc911x_reg_read(pdata, AFC_CFG);
+			temp &= ~0xF;
+			smsc911x_reg_write(temp, pdata, AFC_CFG);
+		}
+	} else {
+		smsc911x_mac_write(pdata, FLOW, 0);
+		temp = smsc911x_reg_read(pdata, AFC_CFG);
+		temp |= 0xF;
+		smsc911x_reg_write(temp, pdata, AFC_CFG);
+	}
+}
+
+/* Update link mode if any thing has changed */
+static void smsc911x_phy_update_linkmode(struct net_device *dev, int init)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned long flags;
+
+	if (mii_check_media(&pdata->mii, netif_msg_link(pdata), init)) {
+		/* duplex state has changed */
+		unsigned int mac_cr;
+
+		spin_lock_irqsave(&pdata->phy_lock, flags);
+		mac_cr = smsc911x_mac_read(pdata, MAC_CR);
+		if (pdata->mii.full_duplex) {
+			SMSC_TRACE("configuring for full duplex mode");
+			mac_cr |= MAC_CR_FDPX_;
+		} else {
+			SMSC_TRACE("configuring for half duplex mode");
+			mac_cr &= ~MAC_CR_FDPX_;
+		}
+		smsc911x_mac_write(pdata, MAC_CR, mac_cr);
+
+		smsc911x_phy_update_flowcontrol(pdata);
+
+		spin_unlock_irqrestore(&pdata->phy_lock, flags);
+	}
+#ifdef USE_LED1_WORK_AROUND
+	if (netif_carrier_ok(dev)) {
+		if ((pdata->gpio_orig_setting & GPIO_CFG_LED1_EN_) &&
+		    (!pdata->using_extphy)) {
+			/* Restore orginal GPIO configuration */
+			pdata->gpio_setting = pdata->gpio_orig_setting;
+			smsc911x_reg_write(pdata->gpio_setting, pdata,
+					   GPIO_CFG);
+		}
+	} else {
+		/* Check global setting that LED1
+		 * usage is 10/100 indicator */
+		pdata->gpio_setting = smsc911x_reg_read(pdata, GPIO_CFG);
+		if ((pdata->gpio_setting & GPIO_CFG_LED1_EN_)
+		    && (!pdata->using_extphy)) {
+			/* Force 10/100 LED off, after saving
+			 * orginal GPIO configuration */
+			pdata->gpio_orig_setting = pdata->gpio_setting;
+
+			pdata->gpio_setting &= ~GPIO_CFG_LED1_EN_;
+			pdata->gpio_setting |= (GPIO_CFG_GPIOBUF0_
+						| GPIO_CFG_GPIODIR0_
+						| GPIO_CFG_GPIOD0_);
+			smsc911x_reg_write(pdata->gpio_setting, pdata,
+					   GPIO_CFG);
+		}
+	}
+#endif				/* USE_LED1_WORK_AROUND */
+}
+
+/* Entry point for the link poller */
+static void smsc911x_phy_checklink(unsigned long ptr)
+{
+	struct net_device *dev = (struct net_device *)ptr;
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	smsc911x_phy_update_linkmode(dev, 0);
+
+	if (!(pdata->stop_link_poll)) {
+		pdata->link_poll_timer.expires = jiffies + 2 * HZ;
+		add_timer(&pdata->link_poll_timer);
+	} else {
+		pdata->stop_link_poll = 0;
+	}
+}
+
+/* Initialises the PHY layer.  Called at initialisation by open() so
+ * interrupts are enabled */
+static int smsc911x_phy_initialise(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned int phyid1 = 0;
+	unsigned int phyid2 = 0;
+	unsigned int temp;
+
+	pdata->using_extphy = 0;
+
+	switch (pdata->idrev & 0xFFFF0000) {
+	case 0x01170000:
+	case 0x01150000:
+		/* External PHY supported, try to autodetect */
+		if (smsc911x_phy_initialise_external(pdata) < 0) {
+			SMSC_TRACE("External PHY is not detected, using "
+				   "internal PHY instead");
+			pdata->mii.phy_id = 1;
+		}
+		break;
+	default:
+		SMSC_TRACE("External PHY is not supported, using internal PHY "
+			   "instead");
+		pdata->mii.phy_id = 1;
+		break;
+	}
+
+	spin_lock_irq(&pdata->phy_lock);
+	phyid1 = smsc911x_phy_read(pdata, MII_PHYSID1);
+	phyid2 = smsc911x_phy_read(pdata, MII_PHYSID2);
+	spin_unlock_irq(&pdata->phy_lock);
+
+	if ((phyid1 == 0xFFFF) && (phyid2 == 0xFFFF)) {
+		SMSC_WARNING("Internal PHY not detected!");
+		return 0;
+	}
+
+	/* Reset the phy */
+	if (!smsc911x_phy_reset(pdata)) {
+		SMSC_WARNING("PHY reset failed to complete.");
+		return 0;
+	}
+#ifdef USE_PHY_WORK_AROUND
+	if (!smsc911x_phy_loopbacktest(pdata)) {
+		SMSC_WARNING("Failed Loop Back Test");
+		return 0;
+	} else {
+		SMSC_TRACE("Passed Loop Back Test");
+	}
+#endif				/* USE_PHY_WORK_AROUND */
+
+	/* Advertise all speeds and pause capabilities */
+	spin_lock_irq(&pdata->phy_lock);
+	temp = smsc911x_phy_read(pdata, MII_ADVERTISE);
+	temp |= (ADVERTISE_ALL | ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM);
+	smsc911x_phy_write(pdata, MII_ADVERTISE, temp);
+	pdata->mii.advertising = temp;
+
+	if (!pdata->using_extphy) {
+		/* using internal phy, enable PHY interrupts */
+		smsc911x_phy_read(pdata, MII_INTSTS);
+		smsc911x_phy_write(pdata, MII_INTMSK, PHY_INTMSK_DEFAULT_);
+	}
+
+	/* begin to establish link */
+	smsc911x_phy_write(pdata, MII_BMCR, BMCR_ANENABLE | BMCR_ANRESTART);
+	spin_unlock_irq(&pdata->phy_lock);
+
+	smsc911x_phy_update_linkmode(dev, 1);
+
+	setup_timer(&pdata->link_poll_timer, smsc911x_phy_checklink,
+		(unsigned long)dev);
+	pdata->link_poll_timer.expires = jiffies + 2 * HZ;
+	add_timer(&pdata->link_poll_timer);
+
+	SMSC_TRACE("phy initialised succesfully");
+	return 1;
+}
+
+/* Gets the number of tx statuses in the fifo */
+static unsigned int smsc911x_tx_get_txstatcount(struct smsc911x_data *pdata)
+{
+	unsigned int result = (smsc911x_reg_read(pdata, TX_FIFO_INF)
+			       & TX_FIFO_INF_TSUSED_) >> 16;
+	return result;
+}
+
+/* Reads tx statuses and increments counters where necessary */
+static void smsc911x_tx_update_txcounters(struct smsc911x_data *pdata)
+{
+	unsigned int tx_stat;
+
+	while ((tx_stat = smsc911x_tx_get_txstatus(pdata)) != 0) {
+		if (unlikely(tx_stat & 0x80000000)) {
+			/* In this driver the packet tag is used as the packet
+			 * length. Since a packet length can never reach the
+			 * size of 0x8000, this bit is reserved. It is worth
+			 * noting that the "reserved bit" in the warning above
+			 * does not reference a hardware defined reserved bit
+			 * but rather a driver defined one.
+			 */
+			SMSC_WARNING("Packet tag reserved bit is high");
+		} else {
+			if (unlikely(tx_stat & 0x00008000)) {
+				pdata->stats.tx_errors++;
+			} else {
+				pdata->stats.tx_packets++;
+				pdata->stats.tx_bytes += (tx_stat >> 16);
+			}
+			if (unlikely(tx_stat & 0x00000100)) {
+				pdata->stats.collisions += 16;
+				pdata->stats.tx_aborted_errors += 1;
+			} else {
+				pdata->stats.collisions +=
+				    ((tx_stat >> 3) & 0xF);
+			}
+			if (unlikely(tx_stat & 0x00000800)) {
+				pdata->stats.tx_carrier_errors += 1;
+			}
+			if (unlikely(tx_stat & 0x00000200)) {
+				pdata->stats.collisions++;
+				pdata->stats.tx_aborted_errors++;
+			}
+		}
+	}
+}
+
+/* Increments the Rx error counters */
+static void
+smsc911x_rx_counterrors(struct smsc911x_data *pdata, unsigned int rxstat)
+{
+	int crc_err = 0;
+
+	if (unlikely(rxstat & 0x00008000)) {
+		pdata->stats.rx_errors++;
+		if (unlikely(rxstat & 0x00000002)) {
+			pdata->stats.rx_crc_errors++;
+			crc_err = 1;
+		}
+	}
+	if (likely(!crc_err)) {
+		if (unlikely((rxstat & 0x00001020) == 0x00001020)) {
+			/* Frame type indicates length,
+			 * and length error is set */
+			pdata->stats.rx_length_errors++;
+		}
+		if (rxstat & RX_STS_MCAST_)
+			pdata->stats.multicast++;
+	}
+}
+
+/* Quickly dumps bad packets */
+static void
+smsc911x_rx_fastforward(struct smsc911x_data *pdata, unsigned int pktbytes)
+{
+	unsigned int pktwords = (pktbytes + NET_IP_ALIGN + 3) >> 2;
+
+	if (likely(pktwords >= 4)) {
+		unsigned int timeout = 500;
+		unsigned int val;
+		smsc911x_reg_write(RX_DP_CTRL_RX_FFWD_, pdata, RX_DP_CTRL);
+		do {
+			udelay(1);
+			val = smsc911x_reg_read(pdata, RX_DP_CTRL);
+		} while (timeout-- && (val & RX_DP_CTRL_RX_FFWD_));
+
+		if (unlikely(timeout == 0))
+			SMSC_WARNING("Timed out waiting for RX FFWD "
+				     "to finish, RX_DP_CTRL: 0x%08X", val);
+	} else {
+		unsigned int temp;
+		while (pktwords--)
+			temp = smsc911x_reg_read(pdata, RX_DATA_FIFO);
+	}
+}
+
+/* NAPI poll function */
+static int smsc911x_poll(struct napi_struct *napi, int budget)
+{
+	struct smsc911x_data *pdata = container_of(napi, struct smsc911x_data, napi);
+	struct net_device *dev = pdata->netdev;
+	int npackets = 0;
+
+	while (npackets < budget) {
+		unsigned int pktlength;
+		unsigned int pktwords;
+		unsigned int rxstat = smsc911x_rx_get_rxstatus(pdata);
+
+		/* break out of while loop if there are no more packets waiting */
+		if (!rxstat)
+			break;
+
+		pktlength = ((rxstat & 0x3FFF0000) >> 16);
+		pktwords = (pktlength + NET_IP_ALIGN + 3) >> 2;
+		smsc911x_rx_counterrors(pdata, rxstat);
+
+		if (likely((rxstat & RX_STS_ES_) == 0)) {
+			struct sk_buff *skb;
+			skb = dev_alloc_skb(pktlength + NET_IP_ALIGN);
+			if (likely(skb)) {
+				skb->data = skb->head;
+				skb->tail = skb->head;
+				/* Align IP on 16B boundary */
+				skb_reserve(skb, NET_IP_ALIGN);
+				skb_put(skb, pktlength - 4);
+				smsc911x_rx_readfifo(pdata,
+						     (unsigned int *)skb->head,
+						     pktwords);
+				skb->dev = dev;
+				skb->protocol = eth_type_trans(skb, dev);
+				skb->ip_summed = CHECKSUM_NONE;
+				netif_receive_skb(skb);
+
+				/* Update counters */
+				pdata->stats.rx_packets++;
+				pdata->stats.rx_bytes += (pktlength - 4);
+				dev->last_rx = jiffies;
+				npackets++;
+				continue;
+			} else {
+				SMSC_WARNING("Unable to allocate sk_buff "
+					     "for rx packet, in PIO path");
+				pdata->stats.rx_dropped++;
+			}
+		}
+		/* At this point, the packet is to be read out
+		 * of the fifo and discarded */
+		smsc911x_rx_fastforward(pdata, pktlength);
+	}
+
+	pdata->stats.rx_dropped += smsc911x_reg_read(pdata, RX_DROP);
+	smsc911x_reg_write(INT_STS_RSFL_, pdata, INT_STS);
+
+	if (npackets < budget) {
+		unsigned int temp;
+		/* We processed all packets available.  Tell NAPI it can
+		 * stop polling then re-enable rx interrupts */
+		netif_rx_complete(dev, napi);
+		temp = smsc911x_reg_read(pdata, INT_EN);
+		temp |= INT_EN_RSFL_EN_;
+		smsc911x_reg_write(temp, pdata, INT_EN);
+	}
+
+	/* There are still packets waiting */
+	return npackets;
+}
+
+/* Returns hash bit number for given MAC address
+ * Example:
+ * 01 00 5E 00 00 01 -> returns bit number 31 */
+static unsigned int smsc911x_hash(char addr[ETH_ALEN])
+{
+	unsigned int crc;
+	unsigned int result;
+
+	crc = ether_crc(ETH_ALEN, addr);
+	result = (crc >> 26) & 0x3f;
+
+	return result;
+}
+
+static void smsc911x_rx_multicast_update(struct smsc911x_data *pdata)
+{
+	/* Performs the multicast & mac_cr update.  This is called when
+	 * safe on the current hardware, and with the phy_lock held */
+	unsigned int mac_cr = smsc911x_mac_read(pdata, MAC_CR);
+	mac_cr |= pdata->set_bits_mask;
+	mac_cr &= ~(pdata->clear_bits_mask);
+	smsc911x_mac_write(pdata, MAC_CR, mac_cr);
+	smsc911x_mac_write(pdata, HASHH, pdata->hashhi);
+	smsc911x_mac_write(pdata, HASHL, pdata->hashlo);
+	SMSC_TRACE("maccr 0x%08X, HASHH 0x%08X, HASHL 0x%08X", mac_cr,
+		   pdata->hashhi, pdata->hashlo);
+}
+
+static void smsc911x_rx_multicast_update_workaround(struct smsc911x_data *pdata)
+{
+	unsigned int mac_cr;
+
+	/* This function is only called for older LAN911x devices
+	 * (revA or revB), where MAC_CR, HASHH and HASHL should not
+	 * be modified during Rx - newer devices immediately update the
+	 * registers.
+	 *
+	 * This is called from interrupt context */
+
+	spin_lock(&pdata->phy_lock);
+
+	/* Check Rx has stopped */
+	if (smsc911x_mac_read(pdata, MAC_CR) & MAC_CR_RXEN_)
+		SMSC_WARNING("Rx not stopped\n");
+
+	/* Perform the update - safe to do now Rx has stopped */
+	smsc911x_rx_multicast_update(pdata);
+
+	/* Re-enable Rx */
+	mac_cr = smsc911x_mac_read(pdata, MAC_CR);
+	mac_cr |= MAC_CR_RXEN_;
+	smsc911x_mac_write(pdata, MAC_CR, mac_cr);
+
+	pdata->multicast_update_pending = 0;
+
+	spin_unlock(&pdata->phy_lock);
+}
+
+/* Sets the device MAC address to dev_addr, called with phy_lock held */
+static void
+smsc911x_set_mac_address(struct smsc911x_data *pdata, u8 dev_addr[6])
+{
+	u32 mac_high16 = (dev_addr[5] << 8) | dev_addr[4];
+	u32 mac_low32 = (dev_addr[3] << 24) | (dev_addr[2] << 16) |
+	    (dev_addr[1] << 8) | dev_addr[0];
+
+	smsc911x_mac_write(pdata, ADDRH, mac_high16);
+	smsc911x_mac_write(pdata, ADDRL, mac_low32);
+}
+
+static int smsc911x_open(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned int timeout;
+	unsigned int temp;
+	unsigned int intcfg;
+
+	spin_lock_init(&pdata->phy_lock);
+
+	/* Reset the LAN911x */
+	smsc911x_reg_write(HW_CFG_SRST_, pdata, HW_CFG);
+	timeout = 10;
+	do {
+		udelay(10);
+		temp = smsc911x_reg_read(pdata, HW_CFG);
+	} while ((--timeout) && (temp & HW_CFG_SRST_));
+
+	if (unlikely(temp & HW_CFG_SRST_)) {
+		SMSC_WARNING("Failed to complete reset");
+		return -ENODEV;
+	}
+
+	smsc911x_reg_write(0x00050000, pdata, HW_CFG);
+	smsc911x_reg_write(0x006E3740, pdata, AFC_CFG);
+
+	/* Make sure EEPROM has finished loading before setting GPIO_CFG */
+	timeout = 50;
+	while ((timeout--) &&
+	       (smsc911x_reg_read(pdata, E2P_CMD) & E2P_CMD_EPC_BUSY_)) {
+		udelay(10);
+	}
+
+	if (unlikely(timeout == 0)) {
+		SMSC_WARNING("Timed out waiting for EEPROM "
+			     "busy bit to clear");
+	}
+#if USE_DEBUG >= 1
+	smsc911x_reg_write(0x00670700, pdata, GPIO_CFG);
+#else
+	smsc911x_reg_write(0x70070000, pdata, GPIO_CFG);
+#endif
+
+	/* Initialise irqs, but leave all sources disabled */
+	smsc911x_reg_write(0, pdata, INT_EN);
+	smsc911x_reg_write(0xFFFFFFFF, pdata, INT_STS);
+
+	/* Set interrupt deassertion to 100uS */
+	intcfg = ((10 << 24) | INT_CFG_IRQ_EN_);
+
+	if (pdata->irq_polarity) {
+		SMSC_TRACE("irq polarity: active high");
+		intcfg |= INT_CFG_IRQ_POL_;
+	} else {
+		SMSC_TRACE("irq polarity: active low");
+	}
+
+	if (pdata->irq_type) {
+		SMSC_TRACE("irq type: push-pull");
+		intcfg |= INT_CFG_IRQ_TYPE_;
+	} else {
+		SMSC_TRACE("irq type: open drain");
+	}
+
+	smsc911x_reg_write(intcfg, pdata, INT_CFG);
+
+	SMSC_TRACE("Testing irq handler using IRQ %d", dev->irq);
+	pdata->software_irq_signal = 0;
+	smp_wmb();
+
+	temp = smsc911x_reg_read(pdata, INT_EN);
+	temp |= INT_EN_SW_INT_EN_;
+	smsc911x_reg_write(temp, pdata, INT_EN);
+
+	timeout = 1000;
+	while (timeout--) {
+		smp_rmb();
+		if (pdata->software_irq_signal)
+			break;
+		msleep(1);
+	}
+
+	if (!pdata->software_irq_signal) {
+		printk(KERN_WARNING "%s: ISR failed signaling test (IRQ %d)\n",
+		       dev->name, dev->irq);
+		return -ENODEV;
+	}
+	SMSC_TRACE("IRQ handler passed test using IRQ %d", dev->irq);
+
+	printk(KERN_INFO "%s: SMSC911x/921x identified at %#08lx, IRQ: %d\n",
+	       dev->name, (unsigned long)pdata->ioaddr, dev->irq);
+
+	netif_carrier_off(dev);
+
+	if (!smsc911x_phy_initialise(dev)) {
+		SMSC_WARNING("Failed to initialize PHY");
+		return -ENODEV;
+	}
+
+	smsc911x_set_mac_address(pdata, dev->dev_addr);
+
+	temp = smsc911x_reg_read(pdata, HW_CFG);
+	temp &= HW_CFG_TX_FIF_SZ_;
+	temp |= HW_CFG_SF_;
+	smsc911x_reg_write(temp, pdata, HW_CFG);
+
+	temp = smsc911x_reg_read(pdata, FIFO_INT);
+	temp |= FIFO_INT_TX_AVAIL_LEVEL_;
+	temp &= ~(FIFO_INT_RX_STS_LEVEL_);
+	smsc911x_reg_write(temp, pdata, FIFO_INT);
+
+	/* set RX Data offset to 2 bytes for alignment */
+	smsc911x_reg_write((2 << 8), pdata, RX_CFG);
+
+	napi_enable(&pdata->napi);
+
+	temp = smsc911x_reg_read(pdata, INT_EN);
+	temp |= (INT_EN_TDFA_EN_ | INT_EN_RSFL_EN_ | INT_EN_PHY_INT_EN_);
+	smsc911x_reg_write(temp, pdata, INT_EN);
+
+	spin_lock_irq(&pdata->phy_lock);
+	temp = smsc911x_mac_read(pdata, MAC_CR);
+	temp |= (MAC_CR_TXEN_ | MAC_CR_RXEN_ | MAC_CR_HBDIS_);
+	smsc911x_mac_write(pdata, MAC_CR, temp);
+	spin_unlock_irq(&pdata->phy_lock);
+
+	smsc911x_reg_write(TX_CFG_TX_ON_, pdata, TX_CFG);
+
+	netif_start_queue(dev);
+	return 0;
+}
+
+/* Entry point for stopping the interface */
+static int smsc911x_stop(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	napi_disable(&pdata->napi);
+
+	pdata->stop_link_poll = 1;
+	del_timer_sync(&pdata->link_poll_timer);
+
+	smsc911x_reg_write((smsc911x_reg_read(pdata, INT_CFG) &
+			    (~INT_CFG_IRQ_EN_)), pdata, INT_CFG);
+	netif_stop_queue(dev);
+
+	/* At this point all Rx and Tx activity is stopped */
+	pdata->stats.rx_dropped += smsc911x_reg_read(pdata, RX_DROP);
+	smsc911x_tx_update_txcounters(pdata);
+
+	SMSC_TRACE("Interface stopped");
+	return 0;
+}
+
+/* Entry point for transmitting a packet */
+static int smsc911x_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned int freespace;
+	unsigned int tx_cmd_a;
+	unsigned int tx_cmd_b;
+	unsigned int temp;
+	u32 wrsz;
+	u32 bufp;
+
+	freespace = smsc911x_reg_read(pdata, TX_FIFO_INF) & TX_FIFO_INF_TDFREE_;
+
+	if (unlikely(freespace < TX_FIFO_LOW_THRESHOLD))
+		SMSC_WARNING("Tx data fifo low, space available: %d",
+			     freespace);
+
+	/* Word alignment adjustment */
+	tx_cmd_a = ((((unsigned int)(skb->data)) & 0x03) << 16);
+	tx_cmd_a |= TX_CMD_A_FIRST_SEG_ | TX_CMD_A_LAST_SEG_;
+	tx_cmd_a |= (unsigned int)skb->len;
+
+	tx_cmd_b = ((unsigned int)skb->len) << 16;
+	tx_cmd_b |= (unsigned int)skb->len;
+
+	smsc911x_reg_write(tx_cmd_a, pdata, TX_DATA_FIFO);
+	smsc911x_reg_write(tx_cmd_b, pdata, TX_DATA_FIFO);
+
+	bufp = ((u32)skb->data) & 0xFFFFFFFC;
+	wrsz = (u32)skb->len + 3;
+	wrsz += ((u32)skb->data) & 0x3;
+	wrsz >>= 2;
+
+	smsc911x_tx_writefifo(pdata, (unsigned int *)bufp, wrsz);
+	freespace -= (skb->len + 32);
+	dev_kfree_skb(skb);
+	dev->trans_start = jiffies;
+
+	if (unlikely(smsc911x_tx_get_txstatcount(pdata) >= 30))
+		smsc911x_tx_update_txcounters(pdata);
+
+	if (freespace < TX_FIFO_LOW_THRESHOLD) {
+		netif_stop_queue(dev);
+		temp = smsc911x_reg_read(pdata, FIFO_INT);
+		temp &= 0x00FFFFFF;
+		temp |= 0x32000000;
+		smsc911x_reg_write(temp, pdata, FIFO_INT);
+	}
+
+	return NETDEV_TX_OK;
+}
+
+/* Entry point for getting status counters */
+static struct net_device_stats *smsc911x_get_stats(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	smsc911x_tx_update_txcounters(pdata);
+	pdata->stats.rx_dropped += smsc911x_reg_read(pdata, RX_DROP);
+	return &pdata->stats;
+}
+
+/* Entry point for setting addressing modes */
+static void smsc911x_set_multicast_list(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned long flags;
+
+	if (dev->flags & IFF_PROMISC) {
+		/* Enabling promiscuous mode */
+		pdata->set_bits_mask = MAC_CR_PRMS_;
+		pdata->clear_bits_mask = (MAC_CR_MCPAS_ | MAC_CR_HPFILT_);
+		pdata->hashhi = 0;
+		pdata->hashlo = 0;
+	} else if (dev->flags & IFF_ALLMULTI) {
+		/* Enabling all multicast mode */
+		pdata->set_bits_mask = MAC_CR_MCPAS_;
+		pdata->clear_bits_mask = (MAC_CR_PRMS_ | MAC_CR_HPFILT_);
+		pdata->hashhi = 0;
+		pdata->hashlo = 0;
+	} else if (dev->mc_count > 0) {
+		/* Enabling specific multicast addresses */
+		unsigned int hash_high = 0;
+		unsigned int hash_low = 0;
+		unsigned int count = 0;
+		struct dev_mc_list *mc_list = dev->mc_list;
+
+		pdata->set_bits_mask = MAC_CR_HPFILT_;
+		pdata->clear_bits_mask = (MAC_CR_PRMS_ | MAC_CR_MCPAS_);
+
+		while (mc_list) {
+			count++;
+			if ((mc_list->dmi_addrlen) == ETH_ALEN) {
+				unsigned int bitnum =
+				    smsc911x_hash(mc_list->dmi_addr);
+				unsigned int mask = 0x01 << (bitnum & 0x1F);
+				if (bitnum & 0x20)
+					hash_high |= mask;
+				else
+					hash_low |= mask;
+			} else {
+				SMSC_WARNING("dmi_addrlen != 6");
+			}
+			mc_list = mc_list->next;
+		}
+		if (count != (unsigned int)dev->mc_count)
+			SMSC_WARNING("mc_count != dev->mc_count");
+
+		pdata->hashhi = hash_high;
+		pdata->hashlo = hash_low;
+	} else {
+		/* Enabling local MAC address only */
+		pdata->set_bits_mask = 0;
+		pdata->clear_bits_mask =
+		    (MAC_CR_PRMS_ | MAC_CR_MCPAS_ | MAC_CR_HPFILT_);
+		pdata->hashhi = 0;
+		pdata->hashlo = 0;
+	}
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+
+	if (pdata->generation <= 1) {
+		/* Older hardware revision - cannot change these flags while
+		 * receiving data */
+		if (!pdata->multicast_update_pending) {
+			unsigned int temp;
+			SMSC_TRACE("scheduling mcast update");
+			pdata->multicast_update_pending = 1;
+
+			/* Request the hardware to stop, then perform the
+			 * update when we get an RX_STOP interrupt */
+			smsc911x_reg_write(INT_STS_RXSTOP_INT_, pdata, INT_STS);
+			temp = smsc911x_reg_read(pdata, INT_EN);
+			temp |= INT_EN_RXSTOP_INT_EN_;
+			smsc911x_reg_write(temp, pdata, INT_EN);
+
+			temp = smsc911x_mac_read(pdata, MAC_CR);
+			temp &= ~(MAC_CR_RXEN_);
+			smsc911x_mac_write(pdata, MAC_CR, temp);
+		} else {
+			/* There is another update pending, this should now
+			 * use the newer values */
+		}
+	} else {
+		/* Newer hardware revision - can write immediately */
+		smsc911x_rx_multicast_update(pdata);
+	}
+
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+}
+
+static irqreturn_t smsc911x_irqhandler(int irq, void *dev_id)
+{
+	struct net_device *dev = dev_id;
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned int intsts;
+	unsigned int inten;
+	unsigned int temp;
+	int serviced = IRQ_NONE;
+
+	intsts = smsc911x_reg_read(pdata, INT_STS);
+	inten = smsc911x_reg_read(pdata, INT_EN);
+
+	if (unlikely(intsts & inten & INT_STS_SW_INT_)) {
+		temp = smsc911x_reg_read(pdata, INT_EN);
+		temp &= (~INT_EN_SW_INT_EN_);
+		smsc911x_reg_write(temp, pdata, INT_EN);
+		smsc911x_reg_write(INT_STS_SW_INT_, pdata, INT_STS);
+		pdata->software_irq_signal = 1;
+		smp_wmb();
+		serviced = IRQ_HANDLED;
+	}
+
+	if (unlikely(intsts & inten & INT_STS_RXSTOP_INT_)) {
+		/* Called when there is a multicast update scheduled and
+		 * it is now safe to complete the update */
+		SMSC_TRACE("RX Stop interrupt");
+		temp = smsc911x_reg_read(pdata, INT_EN);
+		temp &= (~INT_EN_RXSTOP_INT_EN_);
+		smsc911x_reg_write(temp, pdata, INT_EN);
+		smsc911x_reg_write(INT_STS_RXSTOP_INT_, pdata, INT_STS);
+		smsc911x_rx_multicast_update_workaround(pdata);
+		serviced = IRQ_HANDLED;
+	}
+
+	if (intsts & inten & INT_STS_TDFA_) {
+		temp = smsc911x_reg_read(pdata, FIFO_INT);
+		temp |= FIFO_INT_TX_AVAIL_LEVEL_;
+		smsc911x_reg_write(temp, pdata, FIFO_INT);
+		smsc911x_reg_write(INT_STS_TDFA_, pdata, INT_STS);
+		netif_wake_queue(dev);
+		serviced = IRQ_HANDLED;
+	}
+
+	if (unlikely(intsts & inten & INT_STS_RXE_)) {
+		smsc911x_reg_write(INT_STS_RXE_, pdata, INT_STS);
+		serviced = IRQ_HANDLED;
+	}
+
+	if (likely(intsts & inten & INT_STS_RSFL_)) {
+		/* Disable Rx interrupts and schedule NAPI poll */
+		temp = smsc911x_reg_read(pdata, INT_EN);
+		temp &= (~INT_EN_RSFL_EN_);
+		smsc911x_reg_write(temp, pdata, INT_EN);
+		netif_rx_schedule(dev, &pdata->napi);
+		serviced = IRQ_HANDLED;
+	}
+
+	if (unlikely(intsts & inten & INT_STS_PHY_INT_)) {
+		smsc911x_reg_write(INT_STS_PHY_INT_, pdata, INT_STS);
+		spin_lock(&pdata->phy_lock);
+		temp = smsc911x_phy_read(pdata, MII_INTSTS);
+		spin_unlock(&pdata->phy_lock);
+		SMSC_TRACE("PHY interrupt, sts 0x%04X", (u16)temp);
+		smsc911x_phy_update_linkmode(dev, 0);
+		serviced = IRQ_HANDLED;
+	}
+	return serviced;
+}
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+void smsc911x_poll_controller(struct net_device *dev)
+{
+	disable_irq(dev->irq);
+	smsc911x_irqhandler(0, dev);
+	enable_irq(dev->irq);
+}
+#endif				/* CONFIG_NET_POLL_CONTROLLER */
+
+/* Standard ioctls for mii-tool */
+static int smsc911x_do_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	struct mii_ioctl_data *data = if_mii(ifr);
+	unsigned long flags;
+
+	SMSC_TRACE("ioctl cmd 0x%x", cmd);
+	switch (cmd) {
+	case SIOCGMIIPHY:
+		data->phy_id = pdata->mii.phy_id;
+		return 0;
+	case SIOCGMIIREG:
+		spin_lock_irqsave(&pdata->phy_lock, flags);
+		data->val_out = smsc911x_phy_read(pdata, data->reg_num);
+		spin_unlock_irqrestore(&pdata->phy_lock, flags);
+		return 0;
+	case SIOCSMIIREG:
+		spin_lock_irqsave(&pdata->phy_lock, flags);
+		smsc911x_phy_write(pdata, data->reg_num, data->val_in);
+		spin_unlock_irqrestore(&pdata->phy_lock, flags);
+		return 0;
+	}
+
+	SMSC_TRACE("unsupported ioctl cmd");
+	return -1;
+}
+
+static int
+smsc911x_ethtool_getsettings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	cmd->maxtxpkt = 1;
+	cmd->maxrxpkt = 1;
+	return mii_ethtool_gset(&pdata->mii, cmd);
+}
+
+static int
+smsc911x_ethtool_setsettings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	return mii_ethtool_sset(&pdata->mii, cmd);
+}
+
+static void smsc911x_ethtool_getdrvinfo(struct net_device *dev,
+					struct ethtool_drvinfo *info)
+{
+	strncpy(info->driver, SMSC_CHIPNAME, sizeof(info->driver));
+	strncpy(info->version, SMSC_DRV_VERSION, sizeof(info->version));
+	strncpy(info->bus_info, dev->dev.bus_id, sizeof(info->bus_info));
+}
+
+static int smsc911x_ethtool_nwayreset(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	return mii_nway_restart(&pdata->mii);
+}
+
+static u32 smsc911x_ethtool_getmsglevel(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	return pdata->msg_enable;
+}
+
+static void smsc911x_ethtool_setmsglevel(struct net_device *dev, u32 level)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	pdata->msg_enable = level;
+}
+
+static int smsc911x_ethtool_getregslen(struct net_device *dev)
+{
+	return (((E2P_CMD - ID_REV) / 4 + 1) + (WUCSR - MAC_CR) + 1 + 32) *
+	    sizeof(u32);
+}
+
+static void
+smsc911x_ethtool_getregs(struct net_device *dev, struct ethtool_regs *regs,
+			 void *buf)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	unsigned long flags;
+	unsigned int i;
+	unsigned int j = 0;
+	u32 *data = buf;
+
+	regs->version = pdata->idrev;
+	for (i = ID_REV; i <= E2P_CMD; i += (sizeof(u32)))
+		data[j++] = smsc911x_reg_read(pdata, i);
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	for (i = MAC_CR; i <= WUCSR; i++)
+		data[j++] = smsc911x_mac_read(pdata, i);
+	for (i = 0; i <= 31; i++)
+		data[j++] = smsc911x_phy_read(pdata, i);
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+}
+
+static void smsc911x_eeprom_enable_access(struct smsc911x_data *pdata)
+{
+	unsigned int temp = smsc911x_reg_read(pdata, GPIO_CFG);
+	temp &= ~GPIO_CFG_EEPR_EN_;
+	smsc911x_reg_write(temp, pdata, GPIO_CFG);
+	msleep(1);
+}
+
+static int smsc911x_eeprom_send_cmd(struct smsc911x_data *pdata, u32 op)
+{
+	int timeout = 100;
+	u32 e2cmd;
+
+	SMSC_TRACE("op 0x%08x", op);
+	if (smsc911x_reg_read(pdata, E2P_CMD) & E2P_CMD_EPC_BUSY_) {
+		SMSC_WARNING("Busy at start");
+		return -EBUSY;
+	}
+
+	e2cmd = op | E2P_CMD_EPC_BUSY_;
+	smsc911x_reg_write(e2cmd, pdata, E2P_CMD);
+
+	do {
+		msleep(1);
+		e2cmd = smsc911x_reg_read(pdata, E2P_CMD);
+	} while ((e2cmd & E2P_CMD_EPC_BUSY_) && (timeout--));
+
+	if (!timeout) {
+		SMSC_TRACE("TIMED OUT");
+		return -EAGAIN;
+	}
+
+	if (e2cmd & E2P_CMD_EPC_TIMEOUT_) {
+		SMSC_TRACE("Error occured during eeprom operation");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int smsc911x_eeprom_read_location(struct smsc911x_data *pdata,
+					 u8 address, u8 *data)
+{
+	u32 op = E2P_CMD_EPC_CMD_READ_ | address;
+	int ret;
+
+	SMSC_TRACE("address 0x%x", address);
+	ret = smsc911x_eeprom_send_cmd(pdata, op);
+
+	if (!ret)
+		data[address] = smsc911x_reg_read(pdata, E2P_DATA);
+
+	return ret;
+}
+
+static int smsc911x_eeprom_write_location(struct smsc911x_data *pdata,
+					  u8 address, u8 data)
+{
+	u32 op = E2P_CMD_EPC_CMD_ERASE_ | address;
+	int ret;
+
+	SMSC_TRACE("address 0x%x, data 0x%x", address, data);
+	ret = smsc911x_eeprom_send_cmd(pdata, op);
+
+	if (!ret) {
+		op = E2P_CMD_EPC_CMD_WRITE_ | address;
+		smsc911x_reg_write((u32)data, pdata, E2P_DATA);
+		ret = smsc911x_eeprom_send_cmd(pdata, op);
+	}
+
+	return ret;
+}
+
+static int smsc911x_ethtool_get_eeprom_len(struct net_device *dev)
+{
+	return SMSC911X_EEPROM_SIZE;
+}
+
+static int smsc911x_ethtool_get_eeprom(struct net_device *dev,
+				       struct ethtool_eeprom *eeprom, u8 *data)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+	u8 eeprom_data[SMSC911X_EEPROM_SIZE];
+	int len;
+	int i;
+
+	smsc911x_eeprom_enable_access(pdata);
+
+	len = min(eeprom->len, SMSC911X_EEPROM_SIZE);
+	for (i = 0; i < len; i++) {
+		int ret = smsc911x_eeprom_read_location(pdata, i, eeprom_data);
+		if (ret < 0) {
+			eeprom->len = 0;
+			return ret;
+		}
+	}
+
+	memcpy(data, &eeprom_data[eeprom->offset], len);
+	eeprom->len = len;
+	return 0;
+}
+
+static int smsc911x_ethtool_set_eeprom(struct net_device *dev,
+				       struct ethtool_eeprom *eeprom, u8 *data)
+{
+	int ret;
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	smsc911x_eeprom_enable_access(pdata);
+	smsc911x_eeprom_send_cmd(pdata, E2P_CMD_EPC_CMD_EWEN_);
+	ret = smsc911x_eeprom_write_location(pdata, eeprom->offset, *data);
+	smsc911x_eeprom_send_cmd(pdata, E2P_CMD_EPC_CMD_EWDS_);
+
+	/* Single byte write, according to man page */
+	eeprom->len = 1;
+
+	return ret;
+}
+
+static struct ethtool_ops smsc911x_ethtool_ops = {
+	.get_settings = smsc911x_ethtool_getsettings,
+	.set_settings = smsc911x_ethtool_setsettings,
+	.get_link = ethtool_op_get_link,
+	.get_drvinfo = smsc911x_ethtool_getdrvinfo,
+	.nway_reset = smsc911x_ethtool_nwayreset,
+	.get_msglevel = smsc911x_ethtool_getmsglevel,
+	.set_msglevel = smsc911x_ethtool_setmsglevel,
+	.get_regs_len = smsc911x_ethtool_getregslen,
+	.get_regs = smsc911x_ethtool_getregs,
+	.get_eeprom_len = smsc911x_ethtool_get_eeprom_len,
+	.get_eeprom = smsc911x_ethtool_get_eeprom,
+	.set_eeprom = smsc911x_ethtool_set_eeprom,
+};
+
+/* Initializing private device structures */
+static int smsc911x_init(struct net_device *dev)
+{
+	u32 mac_high16;
+	u32 mac_low32;
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	SMSC_TRACE("Driver Parameters:");
+	SMSC_TRACE("LAN base: 0x%08lX", (unsigned long)pdata->ioaddr);
+	SMSC_TRACE("IRQ: %d", dev->irq);
+	SMSC_TRACE("PHY will be autodetected.");
+
+	if (pdata->ioaddr == 0) {
+		SMSC_WARNING("pdata->ioaddr: 0x00000000");
+		return -ENODEV;
+	}
+
+	/* Default generation to zero (all workarounds apply) */
+	pdata->generation = 0;
+
+	pdata->idrev = smsc911x_reg_read(pdata, ID_REV);
+	if (((pdata->idrev >> 16) & 0xFFFF) == (pdata->idrev & 0xFFFF)) {
+		SMSC_WARNING("idrev top 16 bits equal to bottom 16 bits, "
+			     "idrev: 0x%08X", pdata->idrev);
+		SMSC_TRACE("This may mean the chip is set for 32 bit while "
+			   "the bus is reading as 16 bit");
+		return -ENODEV;
+	}
+	switch (pdata->idrev & 0xFFFF0000) {
+	case 0x01180000:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE("LAN9118 Beacon identified, idrev: 0x%08X",
+				   pdata->idrev);
+			pdata->generation = 0;
+			break;
+		case 1UL:
+			SMSC_TRACE
+			    ("LAN9118 Concord A0 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 1;
+			break;
+		case 2UL:
+			SMSC_TRACE
+			    ("LAN9118 Concord A1 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9118 Concord A1 identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		}
+		break;
+
+	case 0x01170000:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE("LAN9117 Beacon identified, idrev: 0x%08X",
+				   pdata->idrev);
+			pdata->generation = 0;
+			break;
+		case 1UL:
+			SMSC_TRACE
+			    ("LAN9117 Concord A0 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 1;
+			break;
+		case 2UL:
+			SMSC_TRACE
+			    ("LAN9117 Concord A1 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9117 Concord A1 identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		}
+		break;
+
+	case 0x01160000:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_WARNING("LAN911x not identified, idrev: 0x%08X",
+				     pdata->idrev);
+			return -ENODEV;
+		case 1UL:
+			SMSC_TRACE
+			    ("LAN9116 Concord A0 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 1;
+			break;
+		case 2UL:
+			SMSC_TRACE
+			    ("LAN9116 Concord A1 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9116 Concord A1 identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		}
+		break;
+
+	case 0x01150000:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_WARNING("LAN911x not identified, idrev: 0x%08X",
+				     pdata->idrev);
+			return -ENODEV;
+		case 1UL:
+			SMSC_TRACE
+			    ("LAN9115 Concord A0 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 1;
+			break;
+		case 2UL:
+			SMSC_TRACE
+			    ("LAN9115 Concord A1 identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9115 Concord A1 identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 2;
+			break;
+		}
+		break;
+
+	case 0x118A0000UL:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE
+			    ("LAN9218 Boylston identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9218 Boylston identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		}
+		break;
+
+	case 0x117A0000UL:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE
+			    ("LAN9217 Boylston identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9217 Boylston identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		}
+		break;
+
+	case 0x116A0000UL:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE
+			    ("LAN9216 Boylston identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9216 Boylston identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		}
+		break;
+
+	case 0x115A0000UL:
+		switch (pdata->idrev & 0x0000FFFFUL) {
+		case 0UL:
+			SMSC_TRACE
+			    ("LAN9215 Boylston identified, idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		default:
+			SMSC_TRACE
+			    ("LAN9215 Boylston identified (NEW), idrev: 0x%08X",
+			     pdata->idrev);
+			pdata->generation = 3;
+			break;
+		}
+		break;
+
+	case 0x92100000UL:
+	case 0x92110000UL:
+	case 0x92200000UL:
+	case 0x92210000UL:
+		/* LAN9210/LAN9211/LAN9220/LAN9221 */
+		pdata->generation = 4;
+		break;
+
+	default:
+		SMSC_WARNING("LAN911x not identified, idrev: 0x%08X",
+			     pdata->idrev);
+		return -ENODEV;
+	}
+
+	if (pdata->generation == 0)
+		SMSC_WARNING("This driver is not intended "
+			     "for this chip revision");
+
+	ether_setup(dev);
+	dev->open = smsc911x_open;
+	dev->stop = smsc911x_stop;
+	dev->hard_start_xmit = smsc911x_hard_start_xmit;
+	dev->get_stats = smsc911x_get_stats;
+	dev->set_multicast_list = smsc911x_set_multicast_list;
+	dev->flags |= IFF_MULTICAST;
+	dev->do_ioctl = smsc911x_do_ioctl;
+	netif_napi_add(dev, &pdata->napi, smsc911x_poll, 64);
+	dev->ethtool_ops = &smsc911x_ethtool_ops;
+
+	/* Check if mac address has been specified when bringing interface up */
+	if (is_valid_ether_addr(dev->dev_addr)) {
+		smsc911x_set_mac_address(pdata, dev->dev_addr);
+		SMSC_TRACE("MAC Address is specified by configuration");
+	} else {
+		/* Try reading mac address from device. if EEPROM is present
+		 * it will already have been set */
+		u32 mac_high16 = smsc911x_mac_read(pdata, ADDRH);
+		u32 mac_low32 = smsc911x_mac_read(pdata, ADDRL);
+		dev->dev_addr[0] = (u8)(mac_low32);
+		dev->dev_addr[1] = (u8)(mac_low32 >> 8);
+		dev->dev_addr[2] = (u8)(mac_low32 >> 16);
+		dev->dev_addr[3] = (u8)(mac_low32 >> 24);
+		dev->dev_addr[4] = (u8)(mac_high16);
+		dev->dev_addr[5] = (u8)(mac_high16 >> 8);
+
+		if (is_valid_ether_addr(dev->dev_addr)) {
+			/* eeprom values are valid  so use them */
+			SMSC_TRACE("Mac Address is read from LAN911x EEPROM");
+		} else {
+			/* eeprom values are invalid, generate random MAC */
+			random_ether_addr(dev->dev_addr);
+			smsc911x_set_mac_address(pdata, dev->dev_addr);
+			SMSC_TRACE("MAC Address is set to random_ether_addr");
+		}
+	}
+
+	printk(KERN_INFO
+	       "%s: SMSC911x MAC Address: %02x:%02x:%02x:%02x:%02x:%02x\n",
+	       dev->name, dev->dev_addr[0], dev->dev_addr[1], dev->dev_addr[2],
+	       dev->dev_addr[3], dev->dev_addr[4], dev->dev_addr[5]);
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	dev->poll_controller = smsc911x_poll_controller;
+#endif				/* CONFIG_NET_POLL_CONTROLLER */
+
+	pdata->mii.phy_id_mask = 0x1f;
+	pdata->mii.reg_num_mask = 0x1f;
+	pdata->mii.force_media = 0;
+	pdata->mii.full_duplex = 0;
+	pdata->mii.dev = dev;
+	pdata->mii.mdio_read = smsc911x_mdio_read;
+	pdata->mii.mdio_write = smsc911x_mdio_write;
+
+	pdata->msg_enable = NETIF_MSG_LINK;
+
+	return 0;
+}
+
+static int smsc911x_drv_remove(struct platform_device *pdev)
+{
+	struct net_device *dev;
+	struct smsc911x_data *pdata;
+	struct resource *res;
+
+	dev = platform_get_drvdata(pdev);
+	BUG_ON(!dev);
+	pdata = netdev_priv(dev);
+	BUG_ON(!pdata);
+	BUG_ON(!pdata->ioaddr);
+
+	SMSC_TRACE("Stopping driver.");
+	platform_set_drvdata(pdev, NULL);
+	unregister_netdev(dev);
+	free_irq(dev->irq, dev);
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   "smsc911x-memory");
+	if (!res)
+		platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	release_mem_region(res->start, res->end - res->start);
+
+	iounmap(pdata->ioaddr);
+
+	free_netdev(dev);
+
+	return 0;
+}
+
+static int smsc911x_drv_probe(struct platform_device *pdev)
+{
+	struct net_device *dev;
+	struct smsc911x_data *pdata;
+	struct resource *res;
+	unsigned int intcfg = 0;
+	int res_size;
+	int retval;
+
+	printk(KERN_INFO "%s: Driver version %s.\n", SMSC_CHIPNAME,
+		SMSC_DRV_VERSION);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   "smsc911x-memory");
+	if (!res)
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		printk(KERN_WARNING "%s: Could not allocate resource.\n",
+		       SMSC_CHIPNAME);
+		retval = -ENODEV;
+		goto out_0;
+	}
+	res_size = res->end - res->start;
+
+	if (!request_mem_region(res->start, res_size, SMSC_CHIPNAME)) {
+		retval = -EBUSY;
+		goto out_0;
+	}
+
+	dev = alloc_etherdev(sizeof(struct smsc911x_data));
+	if (!dev) {
+		printk(KERN_WARNING "%s: Could not allocate device.\n",
+		       SMSC_CHIPNAME);
+		retval = -ENOMEM;
+		goto out_release_io_1;
+	}
+
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	pdata = netdev_priv(dev);
+	pdata->netdev = dev;
+
+	dev->irq = platform_get_irq(pdev, 0);
+	pdata->ioaddr = ioremap_nocache(res->start, res_size);
+
+	/* copy config parameters across if present, otherwise pdata
+	 * defaults to zeros */
+	if (pdev->dev.platform_data) {
+		struct smsc911x_platform_config *config = pdev->dev.platform_data;
+		pdata->irq_polarity = config->irq_polarity;
+		pdata->irq_type  = config->irq_type;
+	}
+
+	if (pdata->ioaddr == NULL) {
+		SMSC_WARNING("Error smsc911x base address invalid");
+		retval = -ENOMEM;
+		goto out_free_netdev_2;
+	}
+
+	if ((retval = smsc911x_init(dev)) < 0)
+		goto out_unmap_io_3;
+
+	/* configure irq polarity and type before connecting isr */
+	if (pdata->irq_polarity)
+		intcfg |= INT_CFG_IRQ_POL_;
+
+	if (pdata->irq_type)
+		intcfg |= INT_CFG_IRQ_TYPE_;
+
+	smsc911x_reg_write(intcfg, pdata, INT_CFG);
+
+	retval = request_irq(dev->irq, smsc911x_irqhandler, IRQF_DISABLED,
+			     SMSC_CHIPNAME, dev);
+	if (retval) {
+		SMSC_WARNING("Unable to claim requested irq: %d", dev->irq);
+		goto out_unmap_io_3;
+	}
+
+	platform_set_drvdata(pdev, dev);
+
+	retval = register_netdev(dev);
+	if (retval) {
+		SMSC_WARNING("Error %i registering device", retval);
+		goto out_unset_drvdata_4;
+	} else {
+		SMSC_TRACE("Network interface: \"%s\"", dev->name);
+	}
+
+	return 0;
+
+out_unset_drvdata_4:
+	platform_set_drvdata(pdev, NULL);
+	free_irq(dev->irq, dev);
+out_unmap_io_3:
+	iounmap(pdata->ioaddr);
+out_free_netdev_2:
+	free_netdev(dev);
+out_release_io_1:
+	release_mem_region(res->start, res->end - res->start);
+out_0:
+	return retval;
+}
+
+static int smsc911x_drv_suspend(struct platform_device *pdev,
+				       pm_message_t state)
+{
+	unsigned long flags;
+	struct net_device *dev;
+	struct smsc911x_data *pdata;
+
+	dev = platform_get_drvdata(pdev);
+	BUG_ON(!dev);
+	pdata = netdev_priv(dev);
+	BUG_ON(!pdata);
+	BUG_ON(!pdata->ioaddr);
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	smsc911x_phy_write(pdata, MII_BMCR, BMCR_PDOWN);
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+
+	smsc911x_reg_write(PMT_CTRL_PM_MODE_D2_, pdata, PMT_CTRL);
+
+	return 0;
+}
+
+static int smsc911x_drv_resume(struct platform_device *pdev)
+{
+	unsigned long flags;
+	struct net_device *dev;
+	struct smsc911x_data *pdata;
+	unsigned int temp;
+
+	dev = platform_get_drvdata(pdev);
+	BUG_ON(!dev);
+	pdata = netdev_priv(dev);
+	BUG_ON(!pdata);
+	BUG_ON(!pdata->ioaddr);
+
+	smsc911x_reg_write(0xFFFFFFFF, pdata, BYTE_TEST);
+	while (!(smsc911x_reg_read(pdata, PMT_CTRL) & PMT_CTRL_READY_))
+		;
+
+	spin_lock_irqsave(&pdata->phy_lock, flags);
+	smsc911x_phy_write(pdata, MII_BMCR, BMCR_ANENABLE | BMCR_ANRESTART);
+	spin_unlock_irqrestore(&pdata->phy_lock, flags);
+	return 0;
+}
+
+static struct platform_driver smsc911x_driver = {
+	.probe = smsc911x_drv_probe,
+	.remove = smsc911x_drv_remove,
+	.suspend = smsc911x_drv_suspend,
+	.resume = smsc911x_drv_resume,
+	.driver = {
+		.name = SMSC_CHIPNAME,
+	},
+};
+
+/* Entry point for loading the module */
+static int __init smsc911x_init_module(void)
+{
+	return platform_driver_register(&smsc911x_driver);
+}
+
+/* entry point for unloading the module */
+static void __exit smsc911x_cleanup_module(void)
+{
+	platform_driver_unregister(&smsc911x_driver);
+}
+
+module_init(smsc911x_init_module);
+module_exit(smsc911x_cleanup_module);
diff --git a/drivers/net/smsc911x.h b/drivers/net/smsc911x.h
new file mode 100644
index 000000000000..d67c9971b136
--- /dev/null
+++ b/drivers/net/smsc911x.h
@@ -0,0 +1,395 @@
+/***************************************************************************
+ *
+ * Copyright (C) 2004-2007  SMSC
+ * Copyright (C) 2005 ARM
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ ***************************************************************************/
+#ifndef __SMSC911X_H__
+#define __SMSC911X_H__
+
+#if defined(CONFIG_MACH_MX37_3DS) || defined(CONFIG_MACH_MX25_3DS)
+#define SMSC_CAN_USE_SPI	1
+#define SMSC_CAN_USE_32BIT	0
+#elif defined(CONFIG_MACH_MX31_3DS) || defined(CONFIG_MACH_MX35_3DS)
+#define SMSC_CAN_USE_32BIT      0
+#define SMSC_CAN_USE_SPI        0
+#else
+#define SMSC_CAN_USE_32BIT	1
+#define SMSC_CAN_USE_SPI	0
+#endif
+
+#define TX_FIFO_LOW_THRESHOLD	(u32)1600
+#define SMSC911X_EEPROM_SIZE	(u32)7
+#define USE_DEBUG 		0
+
+/* implements a PHY loopback test at initialisation time, to ensure a packet
+ * can be succesfully looped back */
+#define USE_PHY_WORK_AROUND
+
+/* 10/100 LED link-state inversion when media is disconnected */
+#define USE_LED1_WORK_AROUND
+
+/* platform_device configuration data, should be assigned to
+ * the platform_device's dev.platform_data */
+struct smsc911x_platform_config {
+	unsigned int irq_polarity;
+	unsigned int irq_type;
+};
+
+#if USE_DEBUG >= 1
+#define SMSC_WARNING(fmt, args...) \
+		printk(KERN_EMERG "SMSC_WARNING: %s: " fmt "\n", \
+			__FUNCTION__ , ## args)
+#else
+#define SMSC_WARNING(msg, args...)
+#endif				/* USE_DEBUG >= 1 */
+
+#if USE_DEBUG >= 2
+#define SMSC_TRACE(fmt,args...) \
+		printk(KERN_EMERG "SMSC_TRACE: %s: " fmt "\n", \
+			__FUNCTION__ , ## args)
+#else
+#define SMSC_TRACE(msg,args...)
+#endif				/* USE_DEBUG >= 2 */
+
+/* SMSC911x registers and bitfields */
+#define RX_DATA_FIFO			0x00
+
+#define TX_DATA_FIFO			0x20
+#define TX_CMD_A_ON_COMP_		0x80000000
+#define TX_CMD_A_BUF_END_ALGN_		0x03000000
+#define TX_CMD_A_4_BYTE_ALGN_		0x00000000
+#define TX_CMD_A_16_BYTE_ALGN_		0x01000000
+#define TX_CMD_A_32_BYTE_ALGN_		0x02000000
+#define TX_CMD_A_DATA_OFFSET_		0x001F0000
+#define TX_CMD_A_FIRST_SEG_		0x00002000
+#define TX_CMD_A_LAST_SEG_		0x00001000
+#define TX_CMD_A_BUF_SIZE_		0x000007FF
+#define TX_CMD_B_PKT_TAG_		0xFFFF0000
+#define TX_CMD_B_ADD_CRC_DISABLE_	0x00002000
+#define TX_CMD_B_DISABLE_PADDING_	0x00001000
+#define TX_CMD_B_PKT_BYTE_LENGTH_	0x000007FF
+
+#define RX_STATUS_FIFO			0x40
+#define RX_STS_ES_			0x00008000
+#define RX_STS_MCAST_			0x00000400
+
+#define RX_STATUS_FIFO_PEEK		0x44
+
+#define TX_STATUS_FIFO			0x48
+#define TX_STS_ES_			0x00008000
+
+#define TX_STATUS_FIFO_PEEK		0x4C
+
+#define ID_REV				0x50
+#define ID_REV_CHIP_ID_			0xFFFF0000
+#define ID_REV_REV_ID_			0x0000FFFF
+
+#define INT_CFG				0x54
+#define INT_CFG_INT_DEAS_		0xFF000000
+#define INT_CFG_INT_DEAS_CLR_		0x00004000
+#define INT_CFG_INT_DEAS_STS_		0x00002000
+#define INT_CFG_IRQ_INT_		0x00001000
+#define INT_CFG_IRQ_EN_			0x00000100
+#define INT_CFG_IRQ_POL_		0x00000010
+#define INT_CFG_IRQ_TYPE_		0x00000001
+
+#define INT_STS				0x58
+#define INT_STS_SW_INT_			0x80000000
+#define INT_STS_TXSTOP_INT_		0x02000000
+#define INT_STS_RXSTOP_INT_		0x01000000
+#define INT_STS_RXDFH_INT_		0x00800000
+#define INT_STS_RXDF_INT_		0x00400000
+#define INT_STS_TX_IOC_			0x00200000
+#define INT_STS_RXD_INT_		0x00100000
+#define INT_STS_GPT_INT_		0x00080000
+#define INT_STS_PHY_INT_		0x00040000
+#define INT_STS_PME_INT_		0x00020000
+#define INT_STS_TXSO_			0x00010000
+#define INT_STS_RWT_			0x00008000
+#define INT_STS_RXE_			0x00004000
+#define INT_STS_TXE_			0x00002000
+#define INT_STS_TDFU_			0x00000800
+#define INT_STS_TDFO_			0x00000400
+#define INT_STS_TDFA_			0x00000200
+#define INT_STS_TSFF_			0x00000100
+#define INT_STS_TSFL_			0x00000080
+#define INT_STS_RXDF_			0x00000040
+#define INT_STS_RDFL_			0x00000020
+#define INT_STS_RSFF_			0x00000010
+#define INT_STS_RSFL_			0x00000008
+#define INT_STS_GPIO2_INT_		0x00000004
+#define INT_STS_GPIO1_INT_		0x00000002
+#define INT_STS_GPIO0_INT_		0x00000001
+
+#define INT_EN				0x5C
+#define INT_EN_SW_INT_EN_		0x80000000
+#define INT_EN_TXSTOP_INT_EN_		0x02000000
+#define INT_EN_RXSTOP_INT_EN_		0x01000000
+#define INT_EN_RXDFH_INT_EN_		0x00800000
+#define INT_EN_TIOC_INT_EN_		0x00200000
+#define INT_EN_RXD_INT_EN_		0x00100000
+#define INT_EN_GPT_INT_EN_		0x00080000
+#define INT_EN_PHY_INT_EN_		0x00040000
+#define INT_EN_PME_INT_EN_		0x00020000
+#define INT_EN_TXSO_EN_			0x00010000
+#define INT_EN_RWT_EN_			0x00008000
+#define INT_EN_RXE_EN_			0x00004000
+#define INT_EN_TXE_EN_			0x00002000
+#define INT_EN_TDFU_EN_			0x00000800
+#define INT_EN_TDFO_EN_			0x00000400
+#define INT_EN_TDFA_EN_			0x00000200
+#define INT_EN_TSFF_EN_			0x00000100
+#define INT_EN_TSFL_EN_			0x00000080
+#define INT_EN_RXDF_EN_			0x00000040
+#define INT_EN_RDFL_EN_			0x00000020
+#define INT_EN_RSFF_EN_			0x00000010
+#define INT_EN_RSFL_EN_			0x00000008
+#define INT_EN_GPIO2_INT_		0x00000004
+#define INT_EN_GPIO1_INT_		0x00000002
+#define INT_EN_GPIO0_INT_		0x00000001
+
+#define BYTE_TEST			0x64
+
+#define FIFO_INT			0x68
+#define FIFO_INT_TX_AVAIL_LEVEL_	0xFF000000
+#define FIFO_INT_TX_STS_LEVEL_		0x00FF0000
+#define FIFO_INT_RX_AVAIL_LEVEL_	0x0000FF00
+#define FIFO_INT_RX_STS_LEVEL_		0x000000FF
+
+#define RX_CFG				0x6C
+#define RX_CFG_RX_END_ALGN_		0xC0000000
+#define RX_CFG_RX_END_ALGN4_		0x00000000
+#define RX_CFG_RX_END_ALGN16_		0x40000000
+#define RX_CFG_RX_END_ALGN32_		0x80000000
+#define RX_CFG_RX_DMA_CNT_		0x0FFF0000
+#define RX_CFG_RX_DUMP_			0x00008000
+#define RX_CFG_RXDOFF_			0x00001F00
+
+#define TX_CFG				0x70
+#define TX_CFG_TXS_DUMP_		0x00008000
+#define TX_CFG_TXD_DUMP_		0x00004000
+#define TX_CFG_TXSAO_			0x00000004
+#define TX_CFG_TX_ON_			0x00000002
+#define TX_CFG_STOP_TX_			0x00000001
+
+#define HW_CFG				0x74
+#define HW_CFG_TTM_			0x00200000
+#define HW_CFG_SF_			0x00100000
+#define HW_CFG_TX_FIF_SZ_		0x000F0000
+#define HW_CFG_TR_			0x00003000
+#define HW_CFG_SRST_			0x00000001
+
+/* only available on 115/117 */
+#define HW_CFG_PHY_CLK_SEL_		0x00000060
+#define HW_CFG_PHY_CLK_SEL_INT_PHY_	0x00000000
+#define HW_CFG_PHY_CLK_SEL_EXT_PHY_	0x00000020
+#define HW_CFG_PHY_CLK_SEL_CLK_DIS_	0x00000040
+#define HW_CFG_SMI_SEL_		 	0x00000010
+#define HW_CFG_EXT_PHY_DET_		0x00000008
+#define HW_CFG_EXT_PHY_EN_		0x00000004
+#define HW_CFG_SRST_TO_			0x00000002
+
+/* only available  on 116/118 */
+#define HW_CFG_32_16_BIT_MODE_		0x00000004
+
+#define RX_DP_CTRL			0x78
+#define RX_DP_CTRL_RX_FFWD_		0x80000000
+
+#define RX_FIFO_INF			0x7C
+#define RX_FIFO_INF_RXSUSED_		0x00FF0000
+#define RX_FIFO_INF_RXDUSED_		0x0000FFFF
+
+#define TX_FIFO_INF			0x80
+#define TX_FIFO_INF_TSUSED_		0x00FF0000
+#define TX_FIFO_INF_TDFREE_		0x0000FFFF
+
+#define PMT_CTRL			0x84
+#define PMT_CTRL_PM_MODE_		0x00003000
+#define PMT_CTRL_PM_MODE_D0_		0x00000000
+#define PMT_CTRL_PM_MODE_D1_		0x00001000
+#define PMT_CTRL_PM_MODE_D2_		0x00002000
+#define PMT_CTRL_PM_MODE_D3_		0x00003000
+#define PMT_CTRL_PHY_RST_		0x00000400
+#define PMT_CTRL_WOL_EN_		0x00000200
+#define PMT_CTRL_ED_EN_			0x00000100
+#define PMT_CTRL_PME_TYPE_		0x00000040
+#define PMT_CTRL_WUPS_			0x00000030
+#define PMT_CTRL_WUPS_NOWAKE_		0x00000000
+#define PMT_CTRL_WUPS_ED_		0x00000010
+#define PMT_CTRL_WUPS_WOL_		0x00000020
+#define PMT_CTRL_WUPS_MULTI_		0x00000030
+#define PMT_CTRL_PME_IND_		0x00000008
+#define PMT_CTRL_PME_POL_		0x00000004
+#define PMT_CTRL_PME_EN_		0x00000002
+#define PMT_CTRL_READY_			0x00000001
+
+#define GPIO_CFG			0x88
+#define GPIO_CFG_LED3_EN_		0x40000000
+#define GPIO_CFG_LED2_EN_		0x20000000
+#define GPIO_CFG_LED1_EN_		0x10000000
+#define GPIO_CFG_GPIO2_INT_POL_		0x04000000
+#define GPIO_CFG_GPIO1_INT_POL_		0x02000000
+#define GPIO_CFG_GPIO0_INT_POL_		0x01000000
+#define GPIO_CFG_EEPR_EN_		0x00700000
+#define GPIO_CFG_GPIOBUF2_		0x00040000
+#define GPIO_CFG_GPIOBUF1_		0x00020000
+#define GPIO_CFG_GPIOBUF0_		0x00010000
+#define GPIO_CFG_GPIODIR2_		0x00000400
+#define GPIO_CFG_GPIODIR1_		0x00000200
+#define GPIO_CFG_GPIODIR0_		0x00000100
+#define GPIO_CFG_GPIOD4_		0x00000020
+#define GPIO_CFG_GPIOD3_		0x00000010
+#define GPIO_CFG_GPIOD2_		0x00000004
+#define GPIO_CFG_GPIOD1_		0x00000002
+#define GPIO_CFG_GPIOD0_		0x00000001
+
+#define GPT_CFG				0x8C
+#define GPT_CFG_TIMER_EN_		0x20000000
+#define GPT_CFG_GPT_LOAD_		0x0000FFFF
+
+#define GPT_CNT				0x90
+#define GPT_CNT_GPT_CNT_		0x0000FFFF
+
+#define ENDIAN				0x98
+
+#define FREE_RUN			0x9C
+
+#define RX_DROP				0xA0
+
+#define MAC_CSR_CMD			0xA4
+#define MAC_CSR_CMD_CSR_BUSY_		0x80000000
+#define MAC_CSR_CMD_R_NOT_W_		0x40000000
+#define MAC_CSR_CMD_CSR_ADDR_		0x000000FF
+
+#define MAC_CSR_DATA			0xA8
+
+#define AFC_CFG				0xAC
+#define AFC_CFG_AFC_HI_			0x00FF0000
+#define AFC_CFG_AFC_LO_			0x0000FF00
+#define AFC_CFG_BACK_DUR_		0x000000F0
+#define AFC_CFG_FCMULT_			0x00000008
+#define AFC_CFG_FCBRD_			0x00000004
+#define AFC_CFG_FCADD_			0x00000002
+#define AFC_CFG_FCANY_			0x00000001
+
+#define E2P_CMD				0xB0
+#define E2P_CMD_EPC_BUSY_		0x80000000
+#define E2P_CMD_EPC_CMD_		0x70000000
+#define E2P_CMD_EPC_CMD_READ_		0x00000000
+#define E2P_CMD_EPC_CMD_EWDS_		0x10000000
+#define E2P_CMD_EPC_CMD_EWEN_		0x20000000
+#define E2P_CMD_EPC_CMD_WRITE_		0x30000000
+#define E2P_CMD_EPC_CMD_WRAL_		0x40000000
+#define E2P_CMD_EPC_CMD_ERASE_		0x50000000
+#define E2P_CMD_EPC_CMD_ERAL_		0x60000000
+#define E2P_CMD_EPC_CMD_RELOAD_		0x70000000
+#define E2P_CMD_EPC_TIMEOUT_		0x00000200
+#define E2P_CMD_MAC_ADDR_LOADED_	0x00000100
+#define E2P_CMD_EPC_ADDR_		0x000000FF
+
+#define E2P_DATA			0xB4
+#define E2P_DATA_EEPROM_DATA_		0x000000FF
+#define LAN_REGISTER_EXTENT		0x00000100
+
+/*
+ * MAC Control and Status Register (Indirect Address)
+ * Offset (through the MAC_CSR CMD and DATA port)
+ */
+#define MAC_CR				0x01
+#define MAC_CR_RXALL_			0x80000000
+#define MAC_CR_HBDIS_			0x10000000
+#define MAC_CR_RCVOWN_			0x00800000
+#define MAC_CR_LOOPBK_			0x00200000
+#define MAC_CR_FDPX_			0x00100000
+#define MAC_CR_MCPAS_			0x00080000
+#define MAC_CR_PRMS_			0x00040000
+#define MAC_CR_INVFILT_			0x00020000
+#define MAC_CR_PASSBAD_			0x00010000
+#define MAC_CR_HFILT_			0x00008000
+#define MAC_CR_HPFILT_			0x00002000
+#define MAC_CR_LCOLL_			0x00001000
+#define MAC_CR_BCAST_			0x00000800
+#define MAC_CR_DISRTY_			0x00000400
+#define MAC_CR_PADSTR_			0x00000100
+#define MAC_CR_BOLMT_MASK_		0x000000C0
+#define MAC_CR_DFCHK_			0x00000020
+#define MAC_CR_TXEN_			0x00000008
+#define MAC_CR_RXEN_			0x00000004
+
+#define ADDRH				0x02
+
+#define ADDRL				0x03
+
+#define HASHH				0x04
+
+#define HASHL				0x05
+
+#define MII_ACC				0x06
+#define MII_ACC_PHY_ADDR_		0x0000F800
+#define MII_ACC_MIIRINDA_		0x000007C0
+#define MII_ACC_MII_WRITE_		0x00000002
+#define MII_ACC_MII_BUSY_		0x00000001
+
+#define MII_DATA			0x07
+
+#define FLOW				0x08
+#define FLOW_FCPT_			0xFFFF0000
+#define FLOW_FCPASS_			0x00000004
+#define FLOW_FCEN_			0x00000002
+#define FLOW_FCBSY_			0x00000001
+
+#define VLAN1				0x09
+
+#define VLAN2				0x0A
+
+#define WUFF				0x0B
+
+#define WUCSR				0x0C
+#define WUCSR_GUE_			0x00000200
+#define WUCSR_WUFR_			0x00000040
+#define WUCSR_MPR_			0x00000020
+#define WUCSR_WAKE_EN_			0x00000004
+#define WUCSR_MPEN_			0x00000002
+
+/*
+ * Phy definitions (vendor-specific)
+ */
+#define LAN9118_PHY_ID			0x00C0001C
+
+#define MII_INTSTS			0x1D
+
+#define MII_INTMSK			0x1E
+#define PHY_INTMSK_AN_RCV_		(1 << 1)
+#define PHY_INTMSK_PDFAULT_		(1 << 2)
+#define PHY_INTMSK_AN_ACK_		(1 << 3)
+#define PHY_INTMSK_LNKDOWN_		(1 << 4)
+#define PHY_INTMSK_RFAULT_		(1 << 5)
+#define PHY_INTMSK_AN_COMP_		(1 << 6)
+#define PHY_INTMSK_ENERGYON_		(1 << 7)
+#define PHY_INTMSK_DEFAULT_		(PHY_INTMSK_ENERGYON_ | \
+					 PHY_INTMSK_AN_COMP_ | \
+					 PHY_INTMSK_RFAULT_ | \
+					 PHY_INTMSK_LNKDOWN_)
+
+#define ADVERTISE_PAUSE_ALL		(ADVERTISE_PAUSE_CAP | \
+					 ADVERTISE_PAUSE_ASYM)
+
+#define LPA_PAUSE_ALL			(LPA_PAUSE_CAP | \
+					 LPA_PAUSE_ASYM)
+
+#endif				/* __SMSC911X_H__ */
diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index 45bdf0b339bb..46bcaca38b46 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -717,5 +717,44 @@ source "drivers/net/wireless/b43/Kconfig"
 source "drivers/net/wireless/b43legacy/Kconfig"
 source "drivers/net/wireless/zd1211rw/Kconfig"
 source "drivers/net/wireless/rt2x00/Kconfig"
+source "drivers/net/wireless/digiPiper/Kconfig"
+
+config DIGI_WI_G
+	tristate "Digi Wireless Module 802.11ab/g"
+	depends on WLAN_80211 && MACH_CCW9C
+	select IEEE80211
+	select IEEE80211_SOFTMAC
+	---help---
+	  A 802.11ab/g wireless driver for Digi ConnectCore Wi-9C modules.
+
+config DIGI_WI_G_HW_ENCRYPTION
+	bool "Use HW encryption"
+	depends on DIGI_WI_G
+	default y
+	help
+	  AES/CCMP decoding is performed by using hardware logic instead of
+	  software emulation. This can also be adjusted at run time with the
+	  kernel command line option dw_sw_aes=0
+
+config DIGI_WI_G_DEBUG
+	bool "Debugging support"
+	depends on DIGI_WI_G
+	default n
+	help
+	  The digi_wi_g driver is compiled with debugging support.
+	  Only the driver maintainer may need to enable it.
+
+choice
+	prompt "UBEC Transceiver Revision"
+	depends on DIGI_WI_G
+	default DIGI_WI_G_UBEC_JD
+
+config DIGI_WI_G_UBEC_JD
+	bool "UBEC Revision JD"
+
+config DIGI_WI_G_UBEC_HC
+	bool "UBEC Revision HC"
+
+endchoice
 
 endmenu
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index 59d2d805f60b..5fcf43bbf8c5 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -38,6 +38,7 @@ obj-$(CONFIG_HOSTAP)		+= hostap/
 obj-$(CONFIG_B43)		+= b43/
 obj-$(CONFIG_B43LEGACY)		+= b43legacy/
 obj-$(CONFIG_ZD1211RW)		+= zd1211rw/
+obj-$(CONFIG_DIGI_PIPER_WIFI)   += digiPiper/
 
 # 16-bit wireless PCMCIA client drivers
 obj-$(CONFIG_PCMCIA_RAYCS)	+= ray_cs.o
@@ -67,3 +68,4 @@ obj-$(CONFIG_ATH5K)	+= ath5k/
 obj-$(CONFIG_ATH9K)	+= ath9k/
 
 obj-$(CONFIG_MAC80211_HWSIM)	+= mac80211_hwsim.o
+obj-$(CONFIG_DIGI_WI_G)	+= digi_wi_g.o
diff --git a/drivers/net/wireless/digiPiper/Kconfig b/drivers/net/wireless/digiPiper/Kconfig
new file mode 100644
index 000000000000..aa0659f0e499
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/Kconfig
@@ -0,0 +1,27 @@
+config DIGI_PIPER_WIFI
+	bool "Digi Piper Wifi support"
+	depends on MAC80211 && WLAN_80211 && I2C && (MACH_CCW9P9215JS || MACH_CCW9M2443JS)
+	---help---
+	  This driver is for the Piper 802.11 MAC by Digi.  This MAC is
+	  supported on the Digi ConnectCore Wi-9P 9215 and Digi ConnectCore Wi-9M 2443
+	  embedded modules.
+
+config PIPER_STATUS_LED
+	bool "Enable GPIO for Wifi status LED"
+	depends on DIGI_PIPER_WIFI && MACH_CCW9M2443JS
+	default y
+	help
+	  ConnectCore Wi-9M 2443 does not provide any dedicated LED 
+	  in the module for showing the Wifi status.
+	  This option lets you define one available CPU GPIO for this purpose.
+	  Default value is set to USER LED 1 on JumpStart board.
+
+config PIPER_STATUS_LED_GPIO
+	int "GPIO for Wifi status LED (0-144)"
+	range 0 144
+	depends on PIPER_STATUS_LED
+	default "141"
+	help
+	  Set CPU GPIO for Wifi status LED.
+
+	  Default: User LED 1 on JumpStart board (LE5)
diff --git a/drivers/net/wireless/digiPiper/Makefile b/drivers/net/wireless/digiPiper/Makefile
new file mode 100644
index 000000000000..a3686b942088
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/Makefile
@@ -0,0 +1,16 @@
+
+mpiper-y := piper.o
+mpiper-y += airoha.o
+mpiper-y += digiDebug.o
+mpiper-y += digiIsr.o
+mpiper-y += digiTx.o
+mpiper-y += digiRx.o
+mpiper-y += digiMac80211.o
+mpiper-y += piperDsp.o
+mpiper-y += piperMacAssist.o
+mpiper-y += phy.o
+mpiper-y += adc121c027.o
+mpiper-y += airohaCalibration.o
+mpiper-y += digiPs.o
+obj-$(CONFIG_DIGI_PIPER_WIFI) := mpiper.o
+
diff --git a/drivers/net/wireless/digiPiper/adc121c027.c b/drivers/net/wireless/digiPiper/adc121c027.c
new file mode 100644
index 000000000000..17564bcd7a66
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/adc121c027.c
@@ -0,0 +1,161 @@
+/*
+    adc121C027.c - Analog to Digital converter integrated into Piper.
+
+    Copyright (C) 2009 Digi International <sales2@digi.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.
+*/
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/hwmon-sysfs.h>
+
+#include "airohaCalibration.h"
+#include "pipermain.h"
+
+/* Addresses to scan: none, device is not autodetected */
+/* static const unsigned short normal_i2c[] = { I2C_CLIENT_END }; */
+
+#define ADC_I2C_ADDR		(0x51)
+#define ADC_CYCLE_TIME		(0x20)
+
+static const unsigned short normal_i2c[] = { ADC_I2C_ADDR, I2C_CLIENT_END };
+static const unsigned short dummy_i2c_addrlist[] = { I2C_CLIENT_END };
+
+static struct i2c_client_address_data addr = {
+	.normal_i2c = normal_i2c,
+	.probe = dummy_i2c_addrlist,
+	.ignore = dummy_i2c_addrlist,
+};
+
+enum adc121C027_cmd {
+        ADC_RESULT              = 0,
+        ADC_ALERT_STATUS        = 1,
+        ADC_CONFIGURATION       = 2,
+        ADC_LOW_LIMIT           = 3,
+        ADC_HIGH_LIMIT          = 4,
+        ADC_HYSTERESIS          = 5,
+        ADC_LOWEST_VALUE        = 6,
+        ADC_HIGHEST_VALUE       = 7,
+};
+
+static u16 adc121C027_read_peak(struct airohaCalibrationData *cal)
+{
+	struct i2c_client *i2cclient = (struct i2c_client *)cal->priv;
+
+	return be16_to_cpu(i2c_smbus_read_word_data(i2cclient, ADC_HIGHEST_VALUE));
+}
+
+static void adc121C027_clear_peak(struct airohaCalibrationData *cal)
+{
+	struct i2c_client *i2cclient = (struct i2c_client *)cal->priv;
+
+	i2c_smbus_write_word_data(i2cclient, ADC_HIGHEST_VALUE, 0);
+}
+
+static u16 adc121C027_read_last_sample(struct airohaCalibrationData *cal)
+{
+	struct i2c_client *i2cclient = (struct i2c_client *)cal->priv;
+
+	return be16_to_cpu(i2c_smbus_read_word_data(i2cclient, ADC_RESULT));
+}
+
+static int adc121C027_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	return (client->addr != ADC_I2C_ADDR) ? -EINVAL : 0;
+}
+
+static int adc121C027_remove(struct i2c_client *client)
+{
+	/* Real shut down will be done by adc121C027_shutdown() */
+	return 0;
+}
+
+static const struct i2c_device_id adc121C027_id[] = {
+	{ "adc121C027", 0 },
+	{}
+};
+
+static struct i2c_driver adc121C027_driver = {
+	.driver = {
+		.name   = "adc121C027",
+	},
+	.probe          = adc121C027_probe,
+	.remove         =  __devexit_p(adc121C027_remove),
+	.id_table       = adc121C027_id,
+	.address_data   = &addr,
+};
+
+/* Turn on automatic A/D process by setting a non zero cycle time */
+static void adc121C027_hw_init(struct airohaCalibrationData *cal)
+{
+	struct i2c_client *i2cclient = (struct i2c_client *)cal->priv;
+
+	i2c_smbus_write_word_data(i2cclient, ADC_CONFIGURATION, ADC_CYCLE_TIME);
+}
+
+void adc121C027_shutdown(struct airohaCalibrationData *cal)
+{
+	struct i2c_client *i2cclient = (struct i2c_client *)cal->priv;
+
+	if (i2cclient) {
+		i2c_unregister_device(i2cclient);
+		cal->priv = NULL;
+	}
+	i2c_del_driver(&adc121C027_driver);
+}
+
+int adc121C027_init(struct airohaCalibrationData *cal, int i2cadapter)
+{
+	struct i2c_board_info board_info = {
+		.type = "adc121C027",
+		.addr = ADC_I2C_ADDR,
+	};
+	struct i2c_adapter *adapter;
+	struct i2c_client *adc_i2c_client;
+	int ret;
+
+	ret = i2c_add_driver(&adc121C027_driver);
+	if (ret) {
+		printk(KERN_WARNING PIPER_DRIVER_NAME
+			": error adding driver adc121C027_driver (%d)\n", ret);
+		return ret;
+	}
+
+	adapter = i2c_get_adapter(i2cadapter);
+	if (!adapter) {
+		printk(KERN_WARNING PIPER_DRIVER_NAME
+			": error getting i2c adapter\n");
+		return -EINVAL;
+	}
+    
+	adc_i2c_client = i2c_new_device(adapter, &board_info);
+	if (!adc_i2c_client) {
+		printk(KERN_WARNING PIPER_DRIVER_NAME
+			": error creating new i2c client\n");
+		return -EINVAL;
+	}
+
+	cal->priv = (void *)adc_i2c_client;
+	adc121C027_hw_init(cal);
+
+	cal->cops = kmalloc(sizeof(struct calibration_ops), GFP_KERNEL);
+	if (!cal->cops) {
+		printk(KERN_WARNING PIPER_DRIVER_NAME
+			": unable to allocate memory for cal->cops\n");
+		return -ENOMEM;
+	}
+	cal->cops->adc_read_peak = adc121C027_read_peak;
+	cal->cops->adc_clear_peak = adc121C027_clear_peak;
+	cal->cops->adc_read_last_val = adc121C027_read_last_sample;
+	cal->cops->adc_shutdown = adc121C027_shutdown;
+    
+	return 0;
+}    
+EXPORT_SYMBOL_GPL(adc121C027_init);
+
+
diff --git a/drivers/net/wireless/digiPiper/adc121c027.h b/drivers/net/wireless/digiPiper/adc121c027.h
new file mode 100644
index 000000000000..1453d8a66963
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/adc121c027.h
@@ -0,0 +1,17 @@
+/*
+    adc121C027.h - Analog to Digital converter integrated into Piper.
+
+    Copyright (C) 2009 Digi International <sales2@digi.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.
+*/
+
+#ifndef ADC121C027_H
+#define ADC121C027_H
+
+int adc121C027_init(struct airohaCalibrationData *cal, int i2cadapter);
+
+#endif /* ADC121C027_H */
+
diff --git a/drivers/net/wireless/digiPiper/airoha.c b/drivers/net/wireless/digiPiper/airoha.c
new file mode 100644
index 000000000000..17ef8a812f98
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/airoha.c
@@ -0,0 +1,986 @@
+/*
+ * Ubec AH7230 radio support.
+ *
+ * Copyright © 2009  Digi International, Inc
+ *
+ * Author: Contact support@digi.com for information about this software.
+ *
+ * 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/delay.h>
+#include <net/mac80211.h>
+#include <net/wireless.h>
+
+#include "pipermain.h"
+#include "mac.h"
+#include "airohaCalibration.h"
+#include "airoha.h"
+
+/*
+ * Number of us to change channels.  I counted the number of udelays once
+ * and it was about 2030, plus the 1 us delays for each register write.
+ * So probably about 2200 in reality, I'm saying 2500 to be safe.
+ */
+#define CHANNEL_CHANGE_TIME	(2500)
+
+/*
+ * Maximum possible receive signal strength in dbm.  Most of the
+ * values will be negative.
+ */
+#define MAX_SIGNAL_IN_DBM	(5)
+
+#define read_reg(reg)		priv->ac->rd_reg(priv,reg)
+#define write_reg(reg,val,op)	priv->ac->wr_reg(priv,reg,val,op)
+#define mac_set_tx_power(x)	al7230_set_txpwr(hw,x)
+
+static unsigned int hw_revision = WCD_HW_REV_A;
+static unsigned int hw_platform = WCD_CCW9P_PLATFORM;
+
+static void InitializeRF(struct ieee80211_hw *hw, int band_selection);
+static int al7230_set_txpwr(struct ieee80211_hw *hw, uint8_t val);
+
+static const struct {
+	unsigned int integer;
+	unsigned int fraction;
+	unsigned int address4;
+	unsigned int tracking;
+} freqTableAiroha_7230[] = {
+	{ 0, 0, 0, 0 },					// 0
+
+	// 2.4 GHz band (802.11b/g)
+	{ 0x00379, 0x13333, 0x7FD78, TRACK_BG_BAND },	// B-1   (2412 MHz)   1
+	{ 0x00379, 0x1B333, 0x7FD78, TRACK_BG_BAND },	// B-2   (2417 MHz)   2
+	{ 0x00379, 0x03333, 0x7FD78, TRACK_BG_BAND },	// B-3   (2422 MHz)   3
+	{ 0x00379, 0x0B333, 0x7FD78, TRACK_BG_BAND },	// B-4   (2427 MHz)   4
+	{ 0x0037A, 0x13333, 0x7FD78, TRACK_BG_BAND },	// B-5   (2432 MHz)   5
+	{ 0x0037A, 0x1B333, 0x7FD78, TRACK_BG_BAND },	// B-6   (2437 MHz)   6
+	{ 0x0037A, 0x03333, 0x7FD78, TRACK_BG_BAND },	// B-7   (2442 MHz)   7
+	{ 0x0037A, 0x0B333, 0x7FD78, TRACK_BG_BAND },	// B-8   (2447 MHz)   8
+	{ 0x0037B, 0x13333, 0x7FD78, TRACK_BG_BAND },	// B-9   (2452 MHz)   9
+	{ 0x0037B, 0x1B333, 0x7FD78, TRACK_BG_BAND },	// B-10  (2457 MHz)  10
+	{ 0x0037B, 0x03333, 0x7FD78, TRACK_BG_BAND },	// B-11  (2462 MHz)  11
+	{ 0x0037B, 0x0B333, 0x7FD78, TRACK_BG_BAND },	// B-12  (2467 MHz)  12
+	{ 0x0037C, 0x13333, 0x7FD78, TRACK_BG_BAND },	// B-13  (2472 MHz)  13
+	{ 0x0037C, 0x06666, 0x7FD78, TRACK_BG_BAND },	// B-14  (2484 MHz)  14
+
+	{ 0, 0, 0, 0 },					// reserved for future b/g expansion 15
+	{ 0, 0, 0, 0 },					// reserved for future b/g expansion 16
+
+	// Extended 4 GHz bands (802.11a) - Lower Band
+	{ 0x0FF52, 0x00000, 0x67F78, TRACK_4920_4980_A_BAND }, // L-184 (4920 MHz)  17
+	{ 0x0FF52, 0x0AAAA, 0x77F78, TRACK_4920_4980_A_BAND }, // L-188 (4940 MHz)  18
+	{ 0x0FF53, 0x15555, 0x77F78, TRACK_4920_4980_A_BAND }, // L-192 (4960 MHz)  19
+	{ 0x0FF53, 0x00000, 0x67F78, TRACK_4920_4980_A_BAND }, // L-196 (4980 MHz)  20
+
+	// Extended 5 GHz bands (802.11a)
+	{ 0x0FF54, 0x00000, 0x67F78, TRACK_5150_5350_A_BAND }, // A-8   (5040 MHz)  21 tracking?
+	{ 0x0FF54, 0x0AAAA, 0x77F78, TRACK_5150_5350_A_BAND }, // A-12  (5060 MHz)  22 tracking?
+	{ 0x0FF55, 0x15555, 0x77F78, TRACK_5150_5350_A_BAND }, // A-16  (5080 MHz)  23 tracking?
+	{ 0x0FF56, 0x05555, 0x77F78, TRACK_5150_5350_A_BAND }, // A-34  (5170 MHz)  24
+	{ 0x0FF56, 0x0AAAA, 0x77F78, TRACK_5150_5350_A_BAND }, // A-36  (5180 MHz)  25
+	{ 0x0FF57, 0x10000, 0x77F78, TRACK_5150_5350_A_BAND }, // A-38  (5190 MHz)  26
+	{ 0x0FF57, 0x15555, 0x77F78, TRACK_5150_5350_A_BAND }, // A-40  (5200 MHz)  27
+	{ 0x0FF57, 0x1AAAA, 0x77F78, TRACK_5150_5350_A_BAND }, // A-42  (5210 MHz)  28
+	{ 0x0FF57, 0x00000, 0x67F78, TRACK_5150_5350_A_BAND }, // A-44  (5220 MHz)  29
+	{ 0x0FF57, 0x05555, 0x77F78, TRACK_5150_5350_A_BAND }, // A-46  (5230 MHz)  30
+	{ 0x0FF57, 0x0AAAA, 0x77F78, TRACK_5150_5350_A_BAND }, // A-48  (5240 MHz)  31
+
+	{ 0x0FF58, 0x15555, 0x77F78, TRACK_5150_5350_A_BAND }, // A-52  (5260 MHz)  32
+	{ 0x0FF58, 0x00000, 0x67F78, TRACK_5150_5350_A_BAND }, // A-56  (5280 MHz)  33
+	{ 0x0FF58, 0x0AAAA, 0x77F78, TRACK_5150_5350_A_BAND }, // A-60  (5300 MHz)  34
+	{ 0x0FF59, 0x15555, 0x77F78, TRACK_5150_5350_A_BAND }, // A-64  (5320 MHz)  35
+
+	{ 0x0FF5C, 0x15555, 0x77F78, TRACK_5470_5725_A_BAND }, // A-100 (5500 MHz)  36
+	{ 0x0FF5C, 0x00000, 0x67F78, TRACK_5470_5725_A_BAND }, // A-104 (5520 MHz)  37
+	{ 0x0FF5C, 0x0AAAA, 0x77F78, TRACK_5470_5725_A_BAND }, // A-108 (5540 MHz)  38
+	{ 0x0FF5D, 0x15555, 0x77F78, TRACK_5470_5725_A_BAND }, // A-112 (5560 MHz)  39
+	{ 0x0FF5D, 0x00000, 0x67F78, TRACK_5470_5725_A_BAND }, // A-116 (5580 MHz)  40
+	{ 0x0FF5D, 0x0AAAA, 0x77F78, TRACK_5470_5725_A_BAND }, // A-120 (5600 MHz)  41
+	{ 0x0FF5E, 0x15555, 0x77F78, TRACK_5470_5725_A_BAND }, // A-124 (5620 MHz)  42
+	{ 0x0FF5E, 0x00000, 0x67F78, TRACK_5470_5725_A_BAND }, // A-128 (5640 MHz)  43
+	{ 0x0FF5E, 0x0AAAA, 0x77F78, TRACK_5470_5725_A_BAND }, // A-132 (5660 MHz)  44
+	{ 0x0FF5F, 0x15555, 0x77F78, TRACK_5470_5725_A_BAND }, // A-136 (5680 MHz)  45
+	{ 0x0FF5F, 0x00000, 0x67F78, TRACK_5470_5725_A_BAND }, // A-140 (5700 MHz)  46
+
+	{ 0x0FF60, 0x18000, 0x77F78, TRACK_5725_5825_A_BAND }, // A-149 (5745 MHz)  47
+	{ 0x0FF60, 0x02AAA, 0x77F78, TRACK_5725_5825_A_BAND }, // A-153 (5765 MHz)  48
+	{ 0x0FF60, 0x0D555, 0x77F78, TRACK_5725_5825_A_BAND }, // A-157 (5785 MHz)  49
+	{ 0x0FF61, 0x18000, 0x77F78, TRACK_5725_5825_A_BAND }, // A-161 (5805 MHz)  50
+	{ 0x0FF61, 0x02AAA, 0x77F78, TRACK_5725_5825_A_BAND }, // A-165 (5825 MHz)  51
+};
+
+#define CHAN4G(idx, _freq)					\
+	.band			= IEEE80211_BAND_2GHZ,		\
+	.center_freq		= (_freq),			\
+	.hw_value		= idx,				\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 12
+
+static struct ieee80211_channel al7230_bg_channels[] = {
+	{ CHAN4G(1, 2412) },
+	{ CHAN4G(2, 2417) },
+	{ CHAN4G(3, 2422) },
+	{ CHAN4G(4, 2427) },
+	{ CHAN4G(5, 2432) },
+	{ CHAN4G(6, 2437) },
+	{ CHAN4G(7, 2442) },
+	{ CHAN4G(8, 2447) },
+	{ CHAN4G(9, 2452) },
+	{ CHAN4G(10, 2457) },
+	{ CHAN4G(11, 2462) },
+	{ CHAN4G(12, 2467) },
+	{ CHAN4G(13, 2472) },
+	{ CHAN4G(14, 2484) },
+};
+
+static const struct ieee80211_rate al7230_bg_rates[] = {
+	/* psk/cck rates */
+	{
+		.bitrate = 10,
+		.flags = IEEE80211_RATE_SHORT_PREAMBLE,
+	},
+	{
+		.bitrate = 20,
+		.flags = IEEE80211_RATE_SHORT_PREAMBLE,
+	},
+	{
+		.bitrate = 55,
+		.flags = IEEE80211_RATE_SHORT_PREAMBLE,
+	},
+	{
+		.bitrate = 110,
+		.flags = IEEE80211_RATE_SHORT_PREAMBLE,
+	},
+	/* ofdm rates */
+	{
+		.bitrate = 60,
+		.hw_value = 0xb,
+	},
+	{
+		.bitrate = 90,
+		.hw_value = 0xf,
+	},
+	{
+		.bitrate = 120,
+		.hw_value = 0xa,
+	},
+	{
+		.bitrate = 180,
+		.hw_value = 0xe,
+	},
+	{
+		.bitrate = 240,
+		.hw_value = 0x9,
+	},
+	{
+		.bitrate = 360,
+		.hw_value = 0xd,
+	},
+	{
+		.bitrate = 480,
+		.hw_value = 0x8,
+	},
+	{
+		.bitrate = 540,
+		.hw_value = 0xc,
+	},
+};
+
+#define CHAN5G(idx, frequency)					\
+	.band			= IEEE80211_BAND_5GHZ,		\
+	.center_freq		= frequency,			\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 8,				\
+	.hw_value		= idx
+
+static struct ieee80211_channel al7230_a_channels[] = {
+	{ CHAN5G(17, 4920) },
+	{ CHAN5G(18, 4940) },
+	{ CHAN5G(19, 4960) },
+	{ CHAN5G(20, 4980) },
+
+	{ CHAN5G(21, 5040) },
+	{ CHAN5G(22, 5060) },
+	{ CHAN5G(23, 5080) },
+	{ CHAN5G(24, 5170) },
+	{ CHAN5G(25, 5180) },
+	{ CHAN5G(26, 5190) },
+	{ CHAN5G(27, 5200) },
+	{ CHAN5G(28, 5210) },
+	{ CHAN5G(29, 5220) },
+	{ CHAN5G(30, 5230) },
+	{ CHAN5G(31, 5240) },
+
+	{ CHAN5G(32, 5260) },
+	{ CHAN5G(33, 5280) },
+	{ CHAN5G(34, 5300) },
+	{ CHAN5G(35, 5320) },
+
+	{ CHAN5G(36, 5500) },
+	{ CHAN5G(37, 5520) },
+	{ CHAN5G(38, 5540) },
+	{ CHAN5G(39, 5560) },
+	{ CHAN5G(40, 5580) },
+	{ CHAN5G(41, 5600) },
+	{ CHAN5G(42, 5620) },
+	{ CHAN5G(43, 5640) },
+	{ CHAN5G(44, 5660) },
+	{ CHAN5G(45, 5680) },
+	{ CHAN5G(46, 5700) },
+
+	{ CHAN5G(47, 5745) },
+	{ CHAN5G(48, 5765) },
+	{ CHAN5G(49, 5785) },
+	{ CHAN5G(50, 5805) },
+	{ CHAN5G(51, 5825) }
+};
+
+static const struct ieee80211_rate al7230_a_rates[] = {
+	/* ofdm rates */
+	{
+		.bitrate = 60,
+		.hw_value = 0xb,
+	},
+	{
+		.bitrate = 90,
+		.hw_value = 0xf,
+	},
+	{
+		.bitrate = 120,
+		.hw_value = 0xa,
+	},
+	{
+		.bitrate = 180,
+		.hw_value = 0xe,
+	},
+	{
+		.bitrate = 240,
+		.hw_value = 0x9,
+	},
+	{
+		.bitrate = 360,
+		.hw_value = 0xd,
+	},
+	{
+		.bitrate = 480,
+		.hw_value = 0x8,
+	},
+	{
+		.bitrate = 540,
+		.hw_value = 0xc,
+	},
+};
+
+static enum ieee80211_band getBand(int channelIndex)
+{
+	enum ieee80211_band result;
+
+	if (channelIndex >= BAND_A_OFFSET) {
+		result = IEEE80211_BAND_5GHZ;
+	} else {
+		result = IEEE80211_BAND_2GHZ;
+	}
+
+	return result;
+}
+
+static int getFrequency(int channelIndex)
+{
+	int result;
+
+	if (getBand(channelIndex) == IEEE80211_BAND_5GHZ) {
+		result = al7230_a_channels[channelIndex - BAND_A_OFFSET].center_freq;
+	} else {
+		result = al7230_bg_channels[channelIndex - 1].center_freq;
+	}
+
+	return result;
+}
+
+static int write_rf(struct ieee80211_hw *hw, unsigned char reg, unsigned int val)
+{
+	struct piper_priv *priv = hw->priv;
+	int err;
+
+	err = write_reg(BB_SPI_DATA, val << 4 | reg, op_write);
+	udelay(3);		/* Mike Schaffner says to allow 2 us or more between all writes */
+	return err;
+}
+
+
+/*
+ * This function is called to set the value of Airoha register
+ * 0xc.  This register must be set to different values depending
+ * on the H/W revision of the board due to changes in the board
+ * design.
+ */
+static void set_hw_specific_parameters(struct ieee80211_hw *hw,
+											unsigned int band,
+										    unsigned int hw_revision,
+										    unsigned int hw_platform)
+{
+	switch (hw_platform) {
+		case WCD_CCW9P_PLATFORM:
+			switch (hw_revision) {
+				case WCD_HW_REV_PROTOTYPE:
+				case WCD_HW_REV_PILOT:
+				case WCD_HW_REV_A:
+				default:
+					if (band == IEEE80211_BAND_2GHZ) {
+						write_rf(hw, 0xc, 0x2b);
+					} else {
+						write_rf(hw, 0xc, 0x00143 );
+					}
+					break;
+			}
+			break;
+		case WCD_CCW9M_PLATFORM:
+			switch (hw_revision) {
+				case WCD_HW_REV_PROTOTYPE:
+				case WCD_HW_REV_PILOT:
+					if (band == IEEE80211_BAND_2GHZ) {
+						write_rf(hw, 0xc, 0xa3);
+					} else {
+						write_rf(hw, 0xc, 0x00143 );
+					}
+				break;
+
+				case WCD_HW_REV_A:
+				default:
+					if (band == IEEE80211_BAND_2GHZ) {
+						write_rf(hw, 0xc, 0x70);
+					} else {
+						write_rf(hw, 0xc, 0x00143 );
+					}
+					break;
+			}
+			break;
+		default:
+			break;
+	}
+}
+
+static int al7230_rf_set_chan_private(struct ieee80211_hw *hw, int channelIndex, bool enable_rx)
+{
+	struct piper_priv *priv = hw->priv;
+	static int rf_band;
+#ifdef WANT_DEBUG
+	const char *channelLookup[] = {
+		"invalid 0",
+		"B-1",
+		"B-2",
+		"B-3",
+		"B-4",
+		"B-5",
+		"B-6",
+		"B-7",
+		"B-8",
+		"B-9",
+		"B-10",
+		"B-11",
+		"B-12",
+		"B-13",
+		"B-14",
+		"invalid 15",
+		"invalid 16",
+		"L-184",
+		"L-188",
+		"L-192",
+		"L-196",
+		"A-8",
+		"A-12",
+		"A-16",
+		"A-34",
+		"A-36",
+		"A-38",
+		"A-40",
+		"A-42",
+		"A-44",
+		"A-46",
+		"A-48",
+		"A-52",
+		"A-56",
+		"A-60",
+		"A-64",
+		"A-100",
+		"A-104",
+		"A-108",
+		"A-112",
+		"A-116",
+		"A-120",
+		"A-124",
+		"A-128",
+		"A-132",
+		"A-136",
+		"A-140",
+		"A-149",
+		"A-153",
+		"A-157",
+		"A-161",
+		"A-165"
+	};
+printk(KERN_ERR "Setting channel %s\n", channelLookup[channelIndex]);
+#endif
+	if (channelIndex >= BAND_A_OFFSET)
+		rf_band = IEEE80211_BAND_5GHZ;
+	else
+		rf_band = IEEE80211_BAND_2GHZ;
+	/* Disable the rx processing path */
+	write_reg(BB_GENERAL_CTL, ~BB_GENERAL_CTL_RX_EN, op_and);
+
+    write_reg(BB_OUTPUT_CONTROL, 0xfffff33f, op_and);
+    write_reg(BB_OUTPUT_CONTROL, 0x00000880, op_or);
+
+	if (priv->pdata->rf_transceiver == RF_AIROHA_2236) {
+/* TODO, when using this transceiver, resolve this commented code */
+#ifdef BUILD_THIS_CODE_SECTION
+		write_reg(BB_GENERAL_STAT, BB_GENERAL_STAT_B_EN, op_or);
+
+		if (macParams.band == WLN_BAND_B) {
+			/* turn off OFDM */
+			write_reg(BB_GENERAL_STAT, ~BB_GENERAL_STAT_A_EN, op_and);
+		} else {
+			/* turn on OFDM */
+			write_reg(BB_GENERAL_STAT, BB_GENERAL_STAT_A_EN, op_or);
+		}
+		/* set the 802.11b/g frequency band specific tracking constant */
+		write_reg(BB_TRACK_CONTROL, 0xff00ffff, op_and);
+
+		write_reg(BB_TRACK_CONTROL, TRACK_BG_BAND, op_or);
+
+		/* perform chip and frequency-band specific RF initialization */
+		InitializeRF(hw, rf_band);
+
+		mac_set_tx_power(priv->tx_power);
+
+		write_rf(hw, 0, freqTableAiroha_2236[channelIndex].integer);
+		write_rf(hw, 1, freqTableAiroha_2236[channelIndex].fraction);
+
+		/* critical delay for correct calibration */
+		udelay(150);
+
+		/*
+		 * TXON, PAON and RXON should all be low before Calibration
+		 * TXON and PAON will be low as long as no frames are written to the TX
+                 * DATA fifo.
+		 * RXON will be low as long as the receive path is not enabled (bit 0 of
+                 * GEN CTL register is 0).
+		 */
+
+		/* calibrate RF transceiver */
+
+		/* TXDCOC->active; RCK->disable */
+		write_rf(hw, 15, 0x00D87);
+		udelay(50);
+		/* TXDCOC->disable; RCK->enable */
+		write_rf(hw, 15, 0x00787);
+		udelay(50);
+		/* TXDCOC->disable; RCK->disable */
+		write_rf(hw, 15, 0x00587);
+		udelay(50);
+
+		/* configure the baseband processing engine */
+		write_reg(BB_GENERAL_CTL, ~BB_GENERAL_CTL_GEN_5GEN, op_and);
+
+		/*Re-enable the rx processing path */
+		write_reg(BB_GENERAL_CTL, BB_GENERAL_CTL_RX_EN, op_or);
+#endif
+	} else if (priv->pdata->rf_transceiver == RF_AIROHA_7230) {
+		/* enable the frequency-band specific PA */
+		if (rf_band == IEEE80211_BAND_2GHZ) {
+			//HW_GEN_CONTROL &= ~GEN_PA_ON;
+			write_reg(BB_GENERAL_STAT, BB_GENERAL_STAT_A_EN | BB_GENERAL_STAT_B_EN,
+				  op_or);
+
+			/* set the 802.11b/g frequency band specific tracking constant */
+			write_reg(BB_TRACK_CONTROL, 0xff00ffff, op_and);
+
+			write_reg(BB_TRACK_CONTROL, TRACK_BG_BAND, op_or);
+
+		} else {
+			//HW_GEN_CONTROL |= GEN_PA_ON;
+
+			// turn off PSK/CCK
+			write_reg(BB_GENERAL_STAT, ~BB_GENERAL_STAT_B_EN, op_and);
+
+			// turn on OFDM
+			write_reg(BB_GENERAL_STAT, BB_GENERAL_STAT_A_EN, op_or);
+
+			/* Set the 802.11a frequency sub-band specific tracking constant */
+			/* All 8 supported 802.11a channels are in this 802.11a frequency sub-band */
+			write_reg(BB_TRACK_CONTROL, 0xff00ffff, op_and);
+
+			write_reg(BB_TRACK_CONTROL, freqTableAiroha_7230[channelIndex].tracking,
+				  op_or);
+		}
+
+		/* perform chip and frequency-band specific RF initialization */
+		InitializeRF(hw, rf_band);
+
+		mac_set_tx_power(priv->tx_power);
+
+		/* Set the channel frequency */
+		write_rf(hw, 0, freqTableAiroha_7230[channelIndex].integer);
+		udelay(150);				/* Mike Schaffner says this is needed here */
+		write_rf(hw, 1, freqTableAiroha_7230[channelIndex].fraction);
+		udelay(150);				/* Mike Schaffner says this is needed here */
+		write_rf(hw, 4, freqTableAiroha_7230[channelIndex].address4);
+		udelay(150);				/* Mike Schaffner says this is needed here */
+
+
+		// Select the frequency band: 5Ghz or 2.4Ghz
+		if (rf_band == IEEE80211_BAND_5GHZ) {
+			/* calibrate RF transceiver */
+
+			/* TXDCOC->active; RCK->disable */
+			write_rf(hw, 15, 0x9ABA8);
+			udelay(50);
+
+			/* TXDCOC->disable; RCK->enable */
+			write_rf(hw, 15, 0x3ABA8);
+			udelay(50);
+
+			/* TXDCOC->disable; RCK->disable */
+			write_rf(hw, 15, 0x12BAC);
+			udelay(50);
+
+			/* configure the baseband processing engine */
+			/*
+			 * This bit always as to be turned off when we are using
+			 * the Airoha chip, even though it's named the 5G EN bit.
+			 * It has to do with how they hooked up the Airoha.
+			 */
+			write_reg(BB_GENERAL_CTL, ~BB_GENERAL_CTL_GEN_5GEN, op_and);
+		} else {
+			/* calibrate RF transceiver */
+
+			/* TXDCOC->active; RCK->disable */
+			write_rf(hw, 15, 0x9ABA8);
+			udelay(50);
+
+			/* TXDCOC->disable; RCK->enable */
+			write_rf(hw, 15, 0x3ABA8);
+			udelay(50);
+
+			/* TXDCOC->disable; RCK->disable */
+			write_rf(hw, 15, 0x1ABA8);
+			udelay(50);
+
+			/* configure the baseband processing engine */
+			write_reg(BB_GENERAL_CTL, ~BB_GENERAL_CTL_GEN_5GEN, op_and);
+			/*
+			 * No short preambles allowed for ODFM.
+			 */
+			write_reg(BB_GENERAL_CTL, ~BB_GENERAL_CTL_SH_PRE, op_and);
+		}
+
+		/*Re-enable the rx processing path */
+		if (enable_rx)
+			write_reg(BB_GENERAL_CTL, BB_GENERAL_CTL_RX_EN, op_or);
+
+		/* re-enable transmitter */
+        write_reg(BB_OUTPUT_CONTROL, 0xfffff33f, op_and);
+	} else {
+		printk(KERN_WARNING PIPER_DRIVER_NAME ": undefined rf transceiver!\n");
+		return -EINVAL;
+	}
+
+    /*
+     * This is a patch for a problem which should be corrected in
+     * hardware on new units.  We are rewriting the MAC address
+     * because on units without the H/W patch the address can
+     * be corrupted when we change channels.
+     */
+    piper_set_macaddr(priv);
+digiWifiDumpRegisters(priv, MAIN_REGS);
+
+	return 0;
+}
+
+static int al7230_rf_set_chan(struct ieee80211_hw *hw, int channelIndex)
+{
+	return al7230_rf_set_chan_private(hw, channelIndex, true);
+}
+
+static int al7230_rf_set_chan_no_rx(struct ieee80211_hw *hw, int channelIndex)
+{
+	return al7230_rf_set_chan_private(hw, channelIndex, false);
+}
+
+
+
+static int al7230_set_txpwr(struct ieee80211_hw *hw, uint8_t value)
+{
+	struct piper_priv *priv = hw->priv;
+
+	if (priv->pdata->rf_transceiver == RF_AIROHA_2236) {
+		const unsigned char powerTable_2236[] = {
+			4, 10, 10, 18, 22, 22, 28, 28,
+			33, 33, 36, 38, 40, 43, 45, 47
+		};
+		write_rf(hw, 9, 0x05440 | powerTable_2236[value & 0xf]);
+	} else if (priv->pdata->rf_transceiver == RF_AIROHA_7230) {
+		const unsigned char powerTable_7230[] = {
+			0x14, 0x14, 0x14, 0x18, 0x18, 0x1c, 0x1c, 0x20,
+			0x20, 0x24, 0x24, 0x29, 0x29, 0x2c, 0x2c, 0x30
+		};
+		int correctedPowerIndex = digiWifiCalibrationPowerIndex(priv);
+
+		if (correctedPowerIndex != -1) {
+		    write_rf(hw, 11, 0x08040 | correctedPowerIndex);
+		} else {
+		    write_rf(hw, 11, 0x08040 | powerTable_7230[value & 0xf]);
+		}
+	} else {
+		printk(KERN_WARNING PIPER_DRIVER_NAME
+		       ": undefined rf transceiver!\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void al7230_set_power_index(struct ieee80211_hw *hw, unsigned int value)
+{
+	write_rf(hw, 11, 0x08040 | value);
+}
+
+static void InitializeRF(struct ieee80211_hw *hw, int band_selection)
+{
+	struct piper_priv *priv = hw->priv;
+
+	if (priv->pdata->rf_transceiver == RF_AIROHA_2236) {
+		digi_dbg("**** transceiver == RF_AIROHA_2236\n");
+		/* Initial settings for 20 MHz reference frequency, 802.11b/g */
+
+		/* CH_integer: Frequency register 0 */
+		write_rf(hw, 0, 0x01f79 );
+
+		/* CH_fraction: Frequency register 1 */
+		write_rf(hw, 1, 0x03333 );
+
+		/*Config 1 = default value */
+		write_rf(hw, 2, 0x00B80 );
+
+		/*Config 2 = default value */
+		write_rf(hw, 3, 0x00E7F );
+
+		/*Config 3 = default value */
+		write_rf(hw, 4, 0x0905A );
+
+		/*Config 4 = default value */
+		write_rf(hw, 5, 0x0F4DC );
+
+		/*Config 5 = default value */
+		write_rf(hw, 6, 0x0805B );
+
+		/*Config 6 = Crystal frequency /2 to pll reference divider */
+		write_rf(hw, 7, 0x0116C );
+
+		/*Config 7 = RSSI = default value */
+		write_rf(hw, 8, 0x05B68 );
+
+		/* TX gain control for LA2236 */
+		write_rf(hw, 9, 0x05460 );   // sit at the middle
+
+		/* RX Gain = digi specific value: AGC adjustment is done over the GC1-GC7
+		IC pins interface. AGC MAX GAIN value is configured in the FPGA BB register
+		instead of the RF register here below */
+		write_rf(hw, 10, 0x001BB );
+
+		/* TX Gain = digi specific vaue: TX GAIN set using the register */
+		write_rf(hw, 11, 0x000f9 );
+
+		/* PA current = default value */
+		write_rf(hw, 12, 0x039D8 );
+
+		/* Config 8 = default value  */
+		write_rf(hw, 13, 0x08000 );
+
+		/* Config 9 = default value */
+		write_rf(hw, 14, 0x00000 );
+
+		/* Config 10 = default value  */
+		write_rf(hw, 15, 0x00587 );
+
+		//mac_set_tx_power (macParams.tx_power);
+
+		/* Calibration procedure */
+		write_reg(BB_OUTPUT_CONTROL, 0x00000300, op_or);
+		udelay(150);
+
+		/* TXDCOC->active; RCK->disable */
+		write_rf(hw, 15, 0x00D87 );
+		udelay(50);
+
+		/* TXDCOC->disable; RCK->enable */
+		write_rf(hw, 15, 0x00787 );
+		udelay(50);
+
+		/* TXDCOC->disable; RCK->disable */
+		write_rf(hw, 15, 0x00587 );
+		udelay(50);
+	} else if (priv->pdata->rf_transceiver == RF_AIROHA_7230) {
+		switch (band_selection) {
+		case IEEE80211_BAND_2GHZ:
+			/* Initial settings for 20 MHz reference frequency, 802.11b/g */
+			write_reg(BB_OUTPUT_CONTROL, 0xfffffcff, op_and);
+			write_reg(BB_OUTPUT_CONTROL, 0x00000200, op_or);
+			udelay(150);
+
+			/* Frequency register 0 */
+			write_rf(hw, 0, 0x00379 );
+
+			/* Frequency register 1 */
+			write_rf(hw, 1, 0x13333 );
+			udelay(10);
+
+			/*Config 1 = default value */
+			write_rf(hw, 2, 0x841FF );
+
+			/*Config 2 = default value */
+			write_rf(hw, 3, 0x3FDFA );
+
+			/*Config 3 = default value */
+			write_rf(hw, 4, 0x7FD78 );
+
+			/*Config 4 = default value */
+			write_rf(hw, 5, 0x802BF );
+
+			/*Config 5 = default value */
+			write_rf(hw, 6, 0x56AF3 );
+
+			/*Config 6 = Crystal frequency /2 to pll reference divider */
+			write_rf(hw, 7, 0xCE000 );
+
+			/*Config 7 = RSSI = default value */
+			write_rf(hw, 8, 0x6EBC0 );
+
+			/* Filter BW  = default value */
+			write_rf(hw, 9, 0x221BB );
+
+			/* RX Gain = digi specific value: AGC adjustment is done over the GC1-GC7
+			IC pins interface. AGC MAX GAIN value is configured in the FPGA BB register
+			instead of the RF register here below */
+			write_rf(hw, 10, 0xE0040 );
+
+			/* TX Gain = digi specific vaue: TX GAIN set using the register */
+			// write_rf(hw, 11, 0x08070);
+			mac_set_tx_power (priv->tx_power);  //Digi value
+
+			/* PA current = default value */
+			set_hw_specific_parameters(hw, IEEE80211_BAND_2GHZ, hw_revision, hw_platform);
+
+			/* Config 8 = default value  */
+			write_rf(hw, 13, 0xFFFFF );
+
+			/* Config 9 = default value */
+			write_rf(hw, 14, 0x00000 );
+
+			/* Config 10 = default value  */
+			write_rf(hw, 15, 0x1ABA8 );
+
+			/* Calibration procedure */
+			write_reg(BB_OUTPUT_CONTROL, 0x00000300, op_or);
+
+			udelay(150);
+
+			/* Calibration procedure */
+
+			/* TXDCOC->active; RCK->disable */
+			write_rf(hw, 15, 0x9ABA8 );
+			udelay(50);
+
+			/* TXDCOC->disable; RCK->enable */
+			write_rf(hw, 15, 0x3ABA8 );
+			udelay(50);
+
+			/* TXDCOC->disable; RCK->disable */
+			write_rf(hw, 15, 0x1ABA8 );
+			udelay(50);
+
+			write_reg(BB_GENERAL_CTL, ~BB_GENERAL_CTL_MAX_GAIN_MASK, op_and);
+			write_reg(BB_GENERAL_CTL, BB_GENERAL_CTL_DEFAULT_MAX_GAIN_BG, op_or);
+			break;
+
+		case IEEE80211_BAND_5GHZ:
+			/* Initial settings for 20 MHz reference frequency, 802.11a */
+			write_reg(BB_OUTPUT_CONTROL, 0xfffffcff, op_and);
+			write_reg(BB_OUTPUT_CONTROL, 0x00000200, op_or);
+			udelay(150);
+
+			/* Frequency register 0 */
+			write_rf(hw, 0, 0x0FF56 );
+
+			/* Frequency register 1 */
+			write_rf(hw, 1, 0x0AAAA );
+
+			udelay(10);
+
+			/*Config 1 = default value */
+			write_rf(hw, 2, 0x451FE );
+
+			/*Config 2 = default value */
+			write_rf(hw, 3, 0x5FDFA );
+
+			/*Config 3 = default value */
+			write_rf(hw, 4, 0x67f78 );
+
+			/*Config 4 = default value */
+			write_rf(hw, 5, 0x853FF );
+
+			/*Config 5 = default value */
+			write_rf(hw, 6, 0x56AF3 );
+
+			/*Config 6 = Crystal frequency /2 to pll reference divider */
+			write_rf(hw, 7, 0xCE000 );
+
+			/*Config 7 = RSSI = default value */
+			write_rf(hw, 8, 0x6EBC0 );
+
+			/* Filter BW  = default value */
+			write_rf(hw, 9, 0x221BB );
+
+			/* RX Gain = digi value */
+			write_rf(hw, 10, 0xE0600 );
+
+			/* TX Gain = digi specific vaue: TX GAIN set using the register */
+			// write_rf(hw, 11, 0x08070 );
+			mac_set_tx_power (priv->tx_power);  //Digi value
+
+			/* PA current = default value */
+			set_hw_specific_parameters(hw, IEEE80211_BAND_5GHZ, hw_revision, hw_platform);
+
+			/* Config 8 = default value  */
+			write_rf(hw, 13, 0xFFFFF );
+
+			    /* Config 9 = default value */
+			write_rf(hw, 14, 0x00000 );
+
+			/* Config 10 = default value  */
+			write_rf(hw, 15, 0x12BAC );
+
+			/* Calibration procedure */
+			write_reg(BB_OUTPUT_CONTROL, 0x00000300, op_or);
+
+			udelay(150);
+
+			/* Calibration procedure */
+
+			/* TXDCOC->active; RCK->disable */
+			write_rf(hw, 15, 0x9ABA8 );
+			udelay(50);
+
+			/* TXDCOC->disable; RCK->enable */
+			write_rf(hw, 15, 0x3ABA8 );
+			udelay(50);
+
+			/* TXDCOC->disable; RCK->disable */
+			write_rf(hw, 15, 0x12BAC );
+			udelay(50);
+			write_reg(BB_GENERAL_CTL, ~BB_GENERAL_CTL_MAX_GAIN_MASK, op_and);
+			write_reg(BB_GENERAL_CTL, BB_GENERAL_CTL_DEFAULT_MAX_GAIN_A, op_or);
+			break;
+		}
+	} else {
+		printk(KERN_WARNING PIPER_DRIVER_NAME
+		       ": undefined rf transceiver!\n");
+	}
+}
+
+static int al7230_rf_stop(struct ieee80211_hw *hw)
+{
+	return 0;
+}
+
+static void getOfdmBrs(int channelIndex, u64 brsBitMask, unsigned int *ofdm, unsigned int *psk)
+{
+	/*
+	 * brsBitMask is a bit mask into the al7230_bg_rates array.  Bit 0 refers
+	 * to the first entry in the array, bit 1 the second, and so on.  The first
+	 * 4 bits/array entries refer to the PSK bit rates we support, the next 8
+	 * bits/array entries refer to the OFDM rates we support.  So the PSK BRS
+	 * mask is bits 0-3, the OFDM bit mask is bits 4-11.
+	 */
+
+	if (getBand(channelIndex) == IEEE80211_BAND_2GHZ)
+	{
+		*psk = brsBitMask & 0xf;
+		*ofdm = (brsBitMask & 0xff0) >> 4;
+	}
+	else
+	{
+		*psk = 0;
+		*ofdm = (brsBitMask & 0xff);
+	}
+}
+
+static struct ieee80211_supported_band al7230_bands[] = {
+	{
+        	.band = IEEE80211_BAND_2GHZ,
+        	.n_channels = ARRAY_SIZE(al7230_bg_channels),
+        	.n_bitrates = ARRAY_SIZE(al7230_bg_rates),
+        	.channels = (struct ieee80211_channel *) al7230_bg_channels,
+        	.bitrates = (struct ieee80211_rate *) al7230_bg_rates,
+	},
+	{
+        	.band = IEEE80211_BAND_5GHZ,
+        	.n_channels = ARRAY_SIZE(al7230_a_channels),
+        	.n_bitrates = ARRAY_SIZE(al7230_a_rates),
+        	.channels = (struct ieee80211_channel *) al7230_a_channels,
+        	.bitrates = (struct ieee80211_rate *) al7230_a_rates,
+	},
+};
+
+static const struct ieee80211_rate *getRate(unsigned int rateIndex)
+{
+    return &al7230_bg_rates[rateIndex];
+}
+
+
+/*
+ * This routine can power up or power down the airoha transceiver.
+ * When the transceiver is powered back up, you must delay 1 ms and
+ * then call the set channel routine to make it operational again.
+ */
+static void power_on(struct ieee80211_hw *hw, bool want_power_on)
+{
+    if (want_power_on) {
+		write_rf(hw, 15, 0x1ABA8 ); /* this is actually for 2 Ghz */
+    } else {
+        write_rf(hw, 15, 0x1ABAE );
+    }
+}
+
+static void al7230_set_hw_info(struct ieee80211_hw *hw, int channel,
+							   u16 hw_platform_code)
+{
+	hw_revision = hw_platform_code & WCD_HW_REV_MASK;
+	hw_platform = (hw_platform_code & WCD_PLATFORM_MASK);
+
+	set_hw_specific_parameters(hw, getBand(channel), hw_revision, hw_platform);
+}
+
+struct digi_rf_ops al7230_rf_ops = {
+	.name			= "Airoha 7230",
+	.init			= InitializeRF,
+	.stop			= al7230_rf_stop,
+	.set_chan		= al7230_rf_set_chan,
+	.set_chan_no_rx	= al7230_rf_set_chan_no_rx,
+	.set_pwr		= al7230_set_txpwr,
+	.set_pwr_index		= al7230_set_power_index,
+	.set_hw_info    = al7230_set_hw_info,
+	.channelChangeTime	= CHANNEL_CHANGE_TIME,
+	.maxSignal		= MAX_SIGNAL_IN_DBM,
+	.getOfdmBrs		= getOfdmBrs,
+	.getBand		= getBand,
+	.getFrequency		= getFrequency,
+	.getRate		= getRate,
+	.bands			= al7230_bands,
+	.power_on       = power_on,
+	.n_bands		= ARRAY_SIZE(al7230_bands),
+};
+EXPORT_SYMBOL_GPL(al7230_rf_ops);
diff --git a/drivers/net/wireless/digiPiper/airoha.h b/drivers/net/wireless/digiPiper/airoha.h
new file mode 100644
index 000000000000..aa3b1d10fbe3
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/airoha.h
@@ -0,0 +1,30 @@
+#ifndef DIGI_RF_AH7230_H_
+#define DIGI_RF_AH7230_H_
+
+/* 0 is reserved for unknown transceiver */
+#define RF_AIROHA_7230		(1)
+#define RF_AIROHA_2236		(2)
+
+#define SPI_INIT_AIROHA		(0x00000018)	/* AIROHA-specific SPI length */
+#define	SPI_INIT_AIROHA2236	(0x00000014)	/* AIROHA 2236-specific SPI length */
+#define	GEN_INIT_AIROHA_24GHZ	(0x31720005)	/*  Initial state; 2.4GHZ_PA_ON= active low; bit 25 */
+#define	GEN_INIT_AIROHA_50GHZ	(0x33760008)	/* Initial state; 5.0GHZ_PA_ON= active high; bit 25 */
+
+#define AIROHA_LOWEST_PSK_RATE_INDEX	(0)
+#define AIROHA_LOWEST_OFDM_RATE_INDEX	(4)
+#define AIROHA_55_MBPS_RATE_INDEX		(2)
+
+/*
+ * Subtract this number from the channel index to index into
+ * the 802.11a channel array.
+ */
+#define BAND_A_OFFSET		(17)
+
+struct digi_rf_freq {
+	uint16_t integer;
+	uint16_t fract;
+};
+
+extern struct digi_rf_ops al7230_rf_ops;
+
+#endif
diff --git a/drivers/net/wireless/digiPiper/airohaCalibration.c b/drivers/net/wireless/digiPiper/airohaCalibration.c
new file mode 100644
index 000000000000..9da912f320ba
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/airohaCalibration.c
@@ -0,0 +1,974 @@
+/*
+ * This file contains the code which performs automatic recalibration of the
+ * Airoha transceiver.
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * All rights reserved.
+ *
+ * This program is free softbware; 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/kthread.h>
+#include <linux/timer.h>
+#include <linux/crc32.h>
+
+#include "pipermain.h"
+#include "mac.h"
+#include "airoha.h"
+#include "airohaCalibration.h"
+#include "adc121c027.h"
+
+#define POWER_INDEX_STEP                (1)	/* TODO: come up with a rational value for this */
+
+#define SAMPLE_TIMEOUT              (HZ * 10)	/* TODO: What is a good sample timeout?  Do we need one? */
+#define RECALIBRATION_PERIOD        (HZ * 15)	/* amount of time to wait between recalibrations */
+
+/*
+ * This defines the amount of time to wait for the power levels
+ * to settle down after making a large correction (user has
+ * changed power level).
+ */
+#define DEBOUNCE_DELAY				(HZ * 2)
+
+#define MAX_TOLERATED_ERROR_HIGH		(300)	/* maximum over target we will allow */
+#define MAX_TOLERATED_ERROR_LOW			(500)	/* maximum under the target we will allow */
+#define CONVERT_TO_MDBM(x)              (1000 * (x))	/* power levels are dBm externally, but dBm/1000 internally */
+
+#define NVRAM_WCAL_SIGNATURE            "WCALDATA"
+
+#define MINIMUM_POWER_INDEX             (10)
+
+/*
+ * Set piperp->calibrationTxRate to this value to make the
+ * transmit routine use the data rate specified by the
+ * mac80211 library.
+ */
+#define USE_MAC80211_DATA_RATE			(NULL)
+
+/*
+ * Events we will wait for, also return values for waitForEvent().
+ */
+#define TIMED_OUT_EVENT                 (1 << 0)
+#define TRANSMIT_DONE_EVENT             (1 << 1)
+#define SHUTDOWN_AUTOCALIBRATION_EVENT  (1 << 2)
+#define RESTART_AUTOCALIBRATION_EVENT   (1 << 3)
+
+/*
+ * Set this constant to (1) if you want to force calibration status to
+ * be printed.
+ */
+#define WANT_CALIBRATION_STATUS		(0)
+
+
+static struct airohaCalibrationData calibration;
+
+static DECLARE_WAIT_QUEUE_HEAD(waitQueue);
+
+
+
+/*
+ * This routine is called to shut down the transmit ADC sampler.
+ */
+static void stopSampler(struct piper_priv *digi)
+{
+	digi->tx_calib_cb = NULL;
+}
+
+/*
+ * This routine is called to update the state of the calibration state machine
+ * and wake up its thread.
+ */
+static void kickCalibrationThread(struct piper_priv *digi, unsigned int event)
+{
+	unsigned long spinlockFlags;
+
+	spin_lock_irqsave(&calibration.lock, spinlockFlags);
+	calibration.events |= event;
+	spin_unlock_irqrestore(&calibration.lock, spinlockFlags);
+	wake_up_interruptible(&waitQueue);
+}
+
+
+/*
+ * This routine is called each time we complete a transmit while we are in
+ * the sampling state.  We record the peak ADC reading.  We kick the state
+ * machine if we now have all the samples we need.
+ */
+static void processSample(struct piper_priv *digi)
+{
+#define MINIMUM_ADC_VALUE       (10)	/* if ADC is below this value, it's probably bad */
+	if (calibration.sampleCount < MAX_SAMPLES) {
+		/*
+		 * Read the ADC value.  It is a 12-bit value.  We shift it 4 bits to
+		 * create an 8-bit value.
+		 */
+		calibration.sample[calibration.sampleCount].sample =
+		    (calibration.cops->adc_read_peak(&calibration) >> 4);
+		if (calibration.sample[calibration.sampleCount].sample >
+		    MINIMUM_ADC_VALUE) {
+			calibration.sampleCount++;
+		}
+	}
+}
+
+static void transmitHasCompleted(struct piper_priv *digi)
+{
+	stopSampler(digi);
+	kickCalibrationThread(digi, TRANSMIT_DONE_EVENT);
+}
+
+
+
+/*
+ * Determine the appropriate transmit rate to use during calibration.
+ */
+static struct ieee80211_rate *determineCalibrationTxRate(struct piper_priv
+							 *digi)
+{
+	unsigned int rates = digi->ac->rd_reg(digi, MAC_SSID_LEN);
+	struct ieee80211_rate *calibrationTxRate;
+
+	rates &= (MAC_PSK_BRS_MASK | MAC_OFDM_BRS_MASK);
+
+	if ((digi->rf->getBand(digi->channel) == IEEE80211_BAND_2GHZ)
+	    && (rates & MAC_PSK_BRS_MASK)) {
+		calibrationTxRate =
+		    (struct ieee80211_rate *) digi->rf->
+		    getRate(AIROHA_LOWEST_PSK_RATE_INDEX);
+	} else {
+		calibrationTxRate =
+		    (struct ieee80211_rate *) digi->rf->
+		    getRate(AIROHA_LOWEST_OFDM_RATE_INDEX);
+	}
+
+	return calibrationTxRate;
+}
+
+
+
+
+/*
+ * Start collecting sample ADC peak measurements for calibration.  Start
+ * the process by installing the callbacks which the transmit code will
+ * use to notify us when transmit frames go out.
+ */
+static void startSampleCollection(struct piper_priv *digi)
+{
+	calibration.cops->adc_clear_peak(&calibration);
+	digi->tx_calib_cb = transmitHasCompleted;
+}
+
+
+static unsigned int waitForEvent(unsigned int timeout, unsigned int eventToWaitFor)
+{
+#define ALL_EVENTS_TO_WAIT_FOR(x)   (eventToWaitFor \
+                                     | SHUTDOWN_AUTOCALIBRATION_EVENT \
+	                                 | RESTART_AUTOCALIBRATION_EVENT)
+
+	unsigned long spinlockFlags;
+	int ccode;
+	unsigned int event;
+	int result = TIMED_OUT_EVENT;
+
+	if (timeout != 0) {
+		ccode = wait_event_interruptible_timeout(waitQueue,
+							 ((calibration.
+							   events &
+							   ALL_EVENTS_TO_WAIT_FOR
+							   (eventToWaitFor))
+							  != 0), timeout);
+		__set_current_state(TASK_RUNNING);
+
+	} else {
+		ccode = 0;
+	}
+	spin_lock_irqsave(&calibration.lock, spinlockFlags);
+	event = calibration.events;
+	calibration.events = 0;
+	spin_unlock_irqrestore(&calibration.lock, spinlockFlags);
+
+	if ((ccode < 0) || (event & SHUTDOWN_AUTOCALIBRATION_EVENT)) {
+		result = SHUTDOWN_AUTOCALIBRATION_EVENT;
+	} else if (event & RESTART_AUTOCALIBRATION_EVENT) {
+		result = RESTART_AUTOCALIBRATION_EVENT;
+	} else if (event & eventToWaitFor) {
+		result = (event & eventToWaitFor);
+	} else {
+		result = TIMED_OUT_EVENT;
+	}
+
+	return result;
+}
+
+
+#ifdef WANT_CAL_DEBUG
+static void printPoint(wcd_point_t * p)
+{
+	printk("(%d, %d, %d)", p->out_power, p->adc_val, p->power_index);
+}
+#endif
+
+
+/*
+ * This routine finds the closest pair of points in a calibration curve.
+ *
+ *      curve       calibration curve
+ *      value       look for the pr of points closest to this value
+ *      p1          storage for one point
+ *      p2          storage for another point
+ *      field       tells us which field in the point struct to compare
+ */
+static void findClosestPoints(wcd_curve_t * curve, int value,
+			      wcd_point_t ** p1, wcd_point_t ** p2, int field)
+{
+	if (value <= curve->points[0].out_power) {
+		*p1 = &curve->points[0];
+		*p2 = &curve->points[1];
+	} else if (value >=
+		   curve->points[calibration.nvram->header.numcalpoints - 1].out_power) {
+		*p1 = &curve->points[calibration.nvram->header.numcalpoints - 2];
+		*p2 = &curve->points[calibration.nvram->header.numcalpoints - 1];
+	} else {
+		unsigned int idx;
+
+		for (idx = 1; idx < calibration.nvram->header.numcalpoints; idx++) {
+			if ((value < curve->points[idx].out_power)
+			    || (idx == (calibration.nvram->header.numcalpoints - 1))) {
+				*p1 = &curve->points[idx - 1];
+				*p2 = &curve->points[idx];
+				break;
+			} else if (value == curve->points[idx].out_power) {
+				/*
+				 * Note that the if statement befpre the for loop already tested for the
+				 * value being equal to the first or last point in the curve, so we don't
+				 * have to worry about that condition in the code below.
+				 */
+				if ((value -
+				     curve->points[idx - 1].out_power) >=
+				    (curve->points[idx + 1].out_power - value)) {
+					/*
+					 * If the two points are equal distant, then favor the larger pair
+					 * because I think the values on the low end are screwy.
+					 */
+					*p1 = &curve->points[idx];
+					*p2 = &curve->points[idx + 1];
+				} else {
+					*p1 = &curve->points[idx - 1];
+					*p2 = &curve->points[idx];
+				}
+				break;
+			}
+		}
+	}
+}
+
+
+/*
+ * Compute the slope of a curve between 2 points.  The slope is the rise over the run,
+ * or (Y2 - Y1)/(X2 - X1).  This function handles more than one type of slope.
+ */
+static int computeSlopeTimes1000(wcd_point_t * p1, wcd_point_t * p2, int slopeType)
+{
+	int slope = 0;
+	int divisor;
+
+	switch (slopeType) {
+	default:
+		digi_dbg("Unexpected slope type %d.\n", slopeType);
+		break;
+	case POWER_INDEX_OVER_OUT_POWER:
+		divisor = (p2->out_power - p1->out_power);
+		if (divisor != 0) {
+			slope =
+			    (((p2->power_index - p1->power_index) * 1000) +
+			     (divisor / 2)) / divisor;
+		} else {
+			digi_dbg("divisor is zero\n");
+		}
+		break;
+	case ADC_OVER_OUT_POWER:
+		divisor = (p2->out_power - p1->out_power);
+		if (divisor != 0) {
+			slope =
+			    (((p2->adc_val - p1->adc_val) * 1000) +
+			     (divisor / 2)) / divisor;
+		} else {
+			digi_dbg("divisor is zero\n");
+		}
+		break;
+	case OUT_POWER_OVER_ADC:
+		divisor = (p2->adc_val - p1->adc_val);
+		if (divisor != 0) {
+			slope =
+			    (((p2->out_power - p1->out_power) * 1000) +
+			     (divisor / 2)) / divisor;
+		} else {
+			digi_dbg("divisor is zero\n");
+		}
+		break;
+	case POWER_INDEX_OVER_ADC:
+		divisor = (p2->adc_val - p1->adc_val);
+		if (divisor != 0) {
+			slope =
+			    (((p2->power_index - p1->power_index) * 1000) +
+			     (divisor / 2)) / divisor;
+		} else {
+			digi_dbg("divisor is zero\n");
+		}
+		break;
+	}
+
+	return slope;
+}
+
+
+/*
+ * If (x,y) is a point on a curve, then compute y given x, the slope of the curve,
+ * and a known point on the curve.
+ *
+ * If (Xd, Yd) is the desired point, p1 is the known point, and m the slope, then
+ *
+ *      Yd - p1->y = m(Xd - p1->x)
+ *      Yd         = m(Xd - p1->x) + p1->y
+ *      Yd         = m(Xd) - m(p1->x) + p1->y
+ */
+static int computeY(wcd_point_t * p1, int slopeTimes1000, int x, int slopeType)
+{
+	int y = 0;
+
+	switch (slopeType) {
+	default:
+		digi_dbg("Unexpected slope type %d.\n", slopeType);
+		break;
+	case POWER_INDEX_OVER_OUT_POWER:
+		y = (((slopeTimes1000 * x) -
+		      (slopeTimes1000 * p1->out_power) + 500) / 1000) + p1->power_index;
+		break;
+	case ADC_OVER_OUT_POWER:
+		y = (((slopeTimes1000 * x) -
+		      (slopeTimes1000 * p1->out_power) + 500) / 1000) + p1->adc_val;
+		break;
+	case OUT_POWER_OVER_ADC:
+		y = (((slopeTimes1000 * x) -
+		      (slopeTimes1000 * p1->adc_val) + 500) / 1000) + p1->out_power;
+		break;
+	case POWER_INDEX_OVER_ADC:
+		y = (((slopeTimes1000 * x) -
+		      (slopeTimes1000 * p1->adc_val) + 500) / 1000) + p1->power_index;
+		break;
+	}
+
+	return y;
+}
+
+
+/*
+ * Return a pointer to the curve we should use for the currently selected
+ * channel.
+ */
+static wcd_curve_t *determineCurve(struct piper_priv *digi)
+{
+	unsigned int rates = digi->ac->rd_reg(digi, MAC_SSID_LEN);
+	wcd_curve_t *curve = NULL;
+
+	if (digi->rf->getBand(digi->channel) == IEEE80211_BAND_2GHZ) {
+		if (rates & MAC_PSK_BRS_MASK) {
+			/*
+			 * This is the normal case for 802.11b and 802.11bg.  We select
+			 * the PSK curve.
+			 */
+			digi_dbg("Using bg curve [%d][%d]\n",
+				 digi->channel, WCD_B_CURVE_INDEX);
+			curve = &calibration.nvram->cal_curves_bg[digi->channel]
+			    [WCD_B_CURVE_INDEX];
+		} else {	/* if associated with AP that only supports G rates */
+
+			/*
+			 * This is a very unusual, but theoretically possible case.  We
+			 * are associated with an AP that only supports OFDM modulation
+			 * (G only, no B).  We determine this by looking at the BRS
+			 * setting.  If no PSK rates are set for BRS, then we assume that
+			 * this must be a G only AP.  Obviously, we must select the OFDM curve.
+			 */
+			digi_dbg("Using bg curve [%d][%d]\n",
+				 digi->channel, WCD_G_CURVE_INDEX);
+			curve = &calibration.nvram->cal_curves_bg[digi->channel]
+			    [WCD_G_CURVE_INDEX];
+		}
+	} else {
+		/*
+		 * An 802.11a channel is selected.
+		 */
+		curve = &calibration.nvram->cal_curves_a[digi->channel - BAND_A_OFFSET];
+		digi_dbg("Using A curve [%d]\n", digi->channel - BAND_A_OFFSET);
+	}
+
+	return curve;
+}
+
+/*
+ * Determine the maximum mdBm allowed for the current channel.  Return the
+ * lesser of the max and the mdBm value passed to us.
+ */
+static int getFilteredPower(struct piper_priv *digi, int mdBm)
+{
+	int max = 0;
+
+	if (digi->channel < BAND_A_OFFSET) {
+		max = 16000;	/* all BG channels can go to 16 dBm */
+	} else if (digi->channel < (BAND_A_OFFSET + 4)) {
+		max = 3000;	/* first 4 A channels max out at 3 dBm */
+	} else {
+		max = 8000;	/* all other A channels can handle 8 dBm */
+	}
+
+	if (mdBm > max) {
+		mdBm = max;
+	}
+
+	return mdBm;
+}
+
+
+/*
+ * This routine performs open loop calibration for Airoha.  It takes a value in mdbm
+ * and uses the factory calibration routines to determine the appropriate register
+ * value to write to airoha.
+ */
+static void setInitialPowerLevel(struct piper_priv *digi, int mdBm)
+{
+	wcd_point_t *p1, *p2;
+
+	mdBm = getFilteredPower(digi, mdBm);
+	digi_dbg("Setting initial powerlevel %d milli dBm.\n", mdBm);
+	calibration.curve = determineCurve(digi);
+	findClosestPoints(calibration.curve, mdBm, &p1, &p2, OUT_POWER);
+	calibration.slopeTimes1000 =
+	    computeSlopeTimes1000(p1, p2, POWER_INDEX_OVER_OUT_POWER);
+	if (abs(p1->out_power - mdBm) < abs(p2->out_power - mdBm)) {
+		calibration.p1 = p1;
+	} else {
+		calibration.p1 = p2;
+	}
+	calibration.powerIndex =
+	    computeY(calibration.p1, calibration.slopeTimes1000, mdBm,
+		     POWER_INDEX_OVER_OUT_POWER);
+	calibration.correctedPowerIndex = calibration.powerIndex;
+
+	digi_dbg("Computed power index = %d.\n", calibration.powerIndex);
+	digi->rf->set_pwr_index(digi->hw, calibration.powerIndex);
+
+	/*
+	 * Let's compute and save the expected ADC value while we have all the necessary
+	 * information handy.
+	 */
+#ifdef WANT_CAL_DEBUG
+	digi_dbg("Using points ");
+	printPoint(p1);
+	printPoint(p2);
+	printk("\n");
+	digi_dbg("Max ADC = %d.\n", calibration.curve->max_adc_value);
+#endif
+	calibration.adcSlopeTimes1000 = computeSlopeTimes1000(p1, p2, ADC_OVER_OUT_POWER);
+	calibration.expectedAdc =
+	    computeY(calibration.p1, calibration.adcSlopeTimes1000, mdBm,
+		     ADC_OVER_OUT_POWER);
+	if (calibration.expectedAdc > calibration.curve->max_adc_value) {
+		calibration.expectedAdc = calibration.curve->max_adc_value;
+	}
+	digi_dbg("adcSlopeTimes1000 = %d, expectedAdc = %d\n",
+		 calibration.adcSlopeTimes1000, calibration.expectedAdc);
+	calibration.outPowerSlopeTimes1000 =
+	    computeSlopeTimes1000(p1, p2, OUT_POWER_OVER_ADC);
+	calibration.powerIndexSlopeTimes1000 =
+	    computeSlopeTimes1000(p1, p2, POWER_INDEX_OVER_ADC);
+}
+
+
+
+/*
+ * This routine performs closed loop recalibration.  It is called periodically
+ * to adjust the transmit power level.  It will be called after the ADC levels
+ * for several transmit frames have been sampled.  It does the following:
+ *
+ *      1.  Average the samples together.
+ *      2.  If the measured ADC level is too low, then increase the power
+ *          level one step.
+ *      3.  If the measured ADC level is too high, then decrease the power
+ *          level one step.
+ */
+static void recalibrate(struct piper_priv *digi)
+{
+	unsigned int idx;
+	int actualAdc = 0;
+	int errorInAdc, errorInMdbm;
+	wcd_point_t p = {
+		.out_power = 0,
+		.adc_val = 0
+	};
+	int needCorrection = 0;
+
+#ifdef WANT_CAL_DEBUG_1
+	digi_dbg("Samples: ");
+#endif
+	for (idx = 0; idx < calibration.sampleCount; idx++) {
+#ifdef WANT_CAL_DEBUG_1
+		printk("%d, ", calibration.sample[idx].sample);
+#endif
+		actualAdc += calibration.sample[idx].sample;
+	}
+#ifdef WANT_CAL_DEBUG_1
+	printk("\n");
+#endif
+	actualAdc = actualAdc / calibration.sampleCount;
+
+#ifdef WANT_TO_FORCE_26
+
+	digi->rf->set_pwr_index(digi->hw, 26);
+#else
+
+	errorInAdc = actualAdc - calibration.expectedAdc;
+	errorInMdbm =
+	    computeY(&p, calibration.outPowerSlopeTimes1000, errorInAdc,
+		     OUT_POWER_OVER_ADC);
+	needCorrection = (errorInMdbm > MAX_TOLERATED_ERROR_HIGH);
+	if (errorInMdbm < 0) {
+		needCorrection = ((MAX_TOLERATED_ERROR_LOW + errorInMdbm) < 0);
+	}
+	if (needCorrection) {
+		int correction = computeY(calibration.p1,
+					  calibration.powerIndexSlopeTimes1000,
+					  actualAdc, POWER_INDEX_OVER_ADC);
+#if defined(WANT_CAL_DEBUG)
+		int oldIndex = calibration.correctedPowerIndex;
+#endif
+		correction = (3 * (calibration.powerIndex - correction)) / 4;
+		if (correction == 0) {
+			if (errorInAdc > 0) {
+				/*
+				 * If power level is too high but the minimum correction would
+				 * overshoot the target, do it anyway because we would rather
+				 * be below the target rather than over the target.
+				 */
+				correction = -1;
+			} else {
+				/*
+				 * But if the power level is too low, but the minimum correction
+				 * would overshoot, then leave it be.  Better too low than too high.
+				 */
+			}
+		}
+		calibration.correctedPowerIndex += correction;
+		if (calibration.correctedPowerIndex < MINIMUM_POWER_INDEX) {
+			calibration.correctedPowerIndex = MINIMUM_POWER_INDEX;
+		}
+		digi->rf->set_pwr_index(digi->hw, calibration.correctedPowerIndex);
+#ifdef WANT_CAL_DEBUG
+		digi_dbg
+		    ("actualAdc = %d, expectedAdc = %d, error mdbm = %d\n",
+		     actualAdc, calibration.expectedAdc, errorInMdbm);
+		digi_dbg
+		    ("Power index corrected by %d: was %d, set to %d.\n",
+		     correction, oldIndex, calibration.correctedPowerIndex);
+#endif
+#if WANT_CALIBRATION_STATUS
+		if (correction != 0)
+			printk(KERN_ERR
+			       "error mdBm = %d, correcting airoha index by %d\n",
+			       errorInMdbm, correction);
+#endif
+	}
+#endif
+}
+
+
+/*
+ * This routine is called by the 80211mac library to set a new power level.  We
+ * update the value in context memory and then kick the autocalibration thread.
+ */
+static int setNewPowerLevel(struct ieee80211_hw *hw, uint8_t newPowerLevel)
+{
+	struct piper_priv *digi = hw->priv;
+
+	(void) newPowerLevel;	/* has already been written into piper->tx_power */
+	/*
+	 * Kick the calibration thread.
+	 */
+	stopSampler(digi);
+	kickCalibrationThread(digi, RESTART_AUTOCALIBRATION_EVENT);
+
+	return 0;
+}
+
+
+
+
+/*
+ * Compute the maximum ADC value for a curve given the maximum
+ * allowed power in dBm.
+ */
+static u8 getMaxAdcValue(wcd_curve_t * curve, int dBm)
+{
+	wcd_point_t *p1, *p2;
+	int slopeTimes1000;
+	u8 result = 0;
+	int mdBm = 1000 * dBm;
+
+	findClosestPoints(curve, mdBm, &p1, &p2, OUT_POWER);
+	slopeTimes1000 = computeSlopeTimes1000(p1, p2, ADC_OVER_OUT_POWER);
+
+	if (abs(p1->out_power - mdBm) < abs(p2->out_power - mdBm)) {
+		result = computeY(p1, slopeTimes1000, mdBm, ADC_OVER_OUT_POWER);
+	} else {
+		result = computeY(p2, slopeTimes1000, mdBm, ADC_OVER_OUT_POWER);
+	}
+
+	return result;
+}
+
+
+/*
+ * The version 1.0 calibration data provided maximum Airoha index
+ * values, but later versions provided maximum ADC values.  This
+ * routine replaces the max Airoha indexes with max ADC values.
+ */
+static void determineMaxAdcValues(wcd_data_t * cdata)
+{
+	int i;
+
+	for (i = 0; i < WCD_CHANNELS_BG; i++) {
+		/*
+		 * All BG channels are limited to 16 dBm.
+		 */
+		cdata->cal_curves_bg[i][0].max_adc_value =
+		    getMaxAdcValue(&cdata->cal_curves_bg[i][0], 16);
+		cdata->cal_curves_bg[i][1].max_adc_value =
+		    getMaxAdcValue(&cdata->cal_curves_bg[i][1], 16);
+	}
+	for (i = 0; i < 4; i++) {
+		/*
+		 * First 4 802.11a channels are limited to 3 dBm.
+		 */
+		cdata->cal_curves_a[i].max_adc_value =
+		    getMaxAdcValue(&cdata->cal_curves_a[i], 3);
+	}
+	for (i = 4; i < WCD_CHANNELS_A; i++) {
+		/*
+		 * All other 802.11a channels are limited to 8 dBm.
+		 */
+		cdata->cal_curves_a[i].max_adc_value =
+		    getMaxAdcValue(&cdata->cal_curves_a[i], 8);
+	}
+}
+
+
+
+/*
+ * This routine writes a default set of calibration values into the
+ * calibration data structure.  Maximum power output is severely limited.
+ */
+static void setDefaultCalibrationValues(struct piper_priv *piperp)
+{
+#define MIN_MDBM			(-2905)
+#define MAX_BG_MDBM			(6000)
+#define DEFAULT_NUM_POINTS	(DEFAULT_NUM_POINTS)
+#define BAND_A_1_START		(0)
+#define BAND_A_2_START		(4)
+#define BAND_A_3_START		(7)
+#define BAND_A_4_START		(15)
+#define BAND_A_5_START		(19)
+#define BAND_A_6_START		(30)
+
+	int i;
+
+	calibration.nvram->header.numcalpoints = 2;
+
+	for (i = 0; i < WCD_CHANNELS_BG; i++) {
+		calibration.nvram->cal_curves_bg[i][0].max_adc_value = 52;
+		calibration.nvram->cal_curves_bg[i][0].points[0].out_power = MIN_MDBM;
+		calibration.nvram->cal_curves_bg[i][0].points[0].adc_val = 19;
+		calibration.nvram->cal_curves_bg[i][0].points[0].power_index = 0;
+		calibration.nvram->cal_curves_bg[i][0].points[1].out_power = 6000;
+		calibration.nvram->cal_curves_bg[i][0].points[1].adc_val = 52;
+		calibration.nvram->cal_curves_bg[i][0].points[1].power_index = 16;
+
+		calibration.nvram->cal_curves_bg[i][0].max_adc_value = 48;
+		calibration.nvram->cal_curves_bg[i][1].points[0].out_power = MIN_MDBM;
+		calibration.nvram->cal_curves_bg[i][1].points[0].adc_val = 12;
+		calibration.nvram->cal_curves_bg[i][1].points[0].power_index = 0;
+		calibration.nvram->cal_curves_bg[i][1].points[1].out_power = 6000;
+		calibration.nvram->cal_curves_bg[i][1].points[1].adc_val = 48;
+		calibration.nvram->cal_curves_bg[i][1].points[1].power_index = 24;
+	}
+
+	for (i = BAND_A_1_START; i < BAND_A_2_START; i++) {
+		calibration.nvram->cal_curves_a[i].max_adc_value = 22;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = MIN_MDBM;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 11;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 0;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = 0;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 22;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 19;
+	}
+	for (i = BAND_A_2_START; i < BAND_A_3_START; i++) {
+		calibration.nvram->cal_curves_a[i].max_adc_value = 29;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = MIN_MDBM;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 13;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 0;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = 2000;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 29;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 20;
+	}
+	for (i = BAND_A_3_START; i < BAND_A_4_START; i++) {
+		calibration.nvram->cal_curves_a[i].max_adc_value = 42;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = MIN_MDBM;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 15;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 0;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = 4000;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 42;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 22;
+	}
+	for (i = BAND_A_4_START; i < BAND_A_5_START; i++) {
+		calibration.nvram->cal_curves_a[i].max_adc_value = 54;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = MIN_MDBM;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 21;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 0;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = 2000;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 54;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 18;
+	}
+	for (i = BAND_A_5_START; i < BAND_A_6_START; i++) {
+		calibration.nvram->cal_curves_a[i].max_adc_value = 39;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = MIN_MDBM;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 13;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 0;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = 2000;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 39;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 26;
+	}
+	for (i = BAND_A_6_START; i < WCD_CHANNELS_A; i++) {
+		calibration.nvram->cal_curves_a[i].max_adc_value = 31;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = MIN_MDBM;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 11;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 0;
+		calibration.nvram->cal_curves_a[i].points[0].out_power = 2000;
+		calibration.nvram->cal_curves_a[i].points[0].adc_val = 31;
+		calibration.nvram->cal_curves_a[i].points[0].power_index = 30;
+	}
+}
+
+
+/*
+ * The calibration data is passed to the kernel from U-Boot.  The kernel
+ * start up routines will have copied the data into digi->pdata->wcd.
+ * We do a few sanity checks on the data and set up our own pointers to
+ * it.
+ */
+static int getCalibrationData(struct piper_priv *digi)
+{
+	int result = 0;
+
+	calibration.nvram = &digi->pdata->wcd;
+
+	if (strncmp(calibration.nvram->header.magic_string,
+		    NVRAM_WCAL_SIGNATURE, strlen(NVRAM_WCAL_SIGNATURE)) == 0) {
+		unsigned int crc = ~crc32_le(~0,
+					     (unsigned char const *) calibration.nvram->
+					     cal_curves_bg,
+					     calibration.nvram->header.wcd_len);
+
+		if (crc == calibration.nvram->header.wcd_crc) {
+			digi_dbg("CRC and signature for calibration data is okay\n");
+			result = 0;
+			if ((calibration.nvram->header.ver_major == '1')
+			    && (calibration.nvram->header.ver_minor == '0')) {
+				digi_dbg("Converting version 1.0 calibration data\n");
+				determineMaxAdcValues(calibration.nvram);
+				/*
+				 * Now that we have updated the format of the data, we need
+				 * to recompute the check sum and set the new version.
+				 */
+				calibration.nvram->header.ver_minor = '1';
+				calibration.nvram->header.wcd_crc =
+				    ~crc32_le(~0, (unsigned char const *)
+					      calibration.nvram->
+					      cal_curves_bg,
+					      calibration.nvram->header.wcd_len);
+			}
+			digi->rf->set_hw_info(digi->hw, digi->channel,
+								  calibration.nvram->header.hw_platform);
+		} else {
+			digi_dbg("Calibration data has invalid CRC.\n");
+			setDefaultCalibrationValues(digi);
+		}
+	} else {
+		digi_dbg("Calibration data has invalid signature.\n");
+		setDefaultCalibrationValues(digi);
+	}
+
+	return result;
+}
+
+
+
+/*
+ * This routine:
+ *
+ *      1. Loads the ADC driver.
+ *      2. Loads the calibration data.
+ *      3. Implements the automatic calibration state machine.
+ *
+ */
+static int digiWifiCalibrationThreadEntry(void *data)
+{
+	struct piper_priv *digi = data;
+	int state;
+
+	__set_current_state(TASK_RUNNING);
+
+	while (1) {
+		/*
+		 * We, the wireless driver, may be loaded before the I2C core has
+		 * loaded.  Therefore we may not be able to load our ADC driver,
+		 * which is an I2C client driver, when we load.  This loop tries
+		 * and retries to load the ADC driver until it succeeds.
+		 */
+
+		/* TODO, FIXME make following code dependent on platform information
+		 * allowign to initialize different adc */
+		if (adc121C027_init(&calibration, digi->pdata->i2c_adapter_num) == 0) {
+			digi_dbg("ADC driver loaded...\n");
+			break;
+		}
+		digi_dbg("Will try to load ADC driver again...\n");
+		ssleep(10);
+	}
+
+	if (getCalibrationData(digi) == 0) {
+		digi->rf->set_pwr = setNewPowerLevel;
+
+		state = RESTART_STATE;
+
+		digi_dbg("Starting autocalibration state machine.\n");
+		do {
+			int event;
+			int timeout = 0;
+			int expectedEvent = TIMED_OUT_EVENT;
+			int nextState = RESTART_STATE;
+
+			switch (state) {
+			case RESTART_STATE:
+				setInitialPowerLevel(digi,
+						     CONVERT_TO_MDBM(digi->tx_power));
+	            calibration.initialized = true;
+				calibration.sampleCount = 0;
+				nextState = COLLECT_SAMPLES_STATE;
+				timeout = DEBOUNCE_DELAY;
+				expectedEvent = TIMED_OUT_EVENT;
+				break;
+			case COLLECT_SAMPLES_STATE:
+				digi->calibrationTxRate =
+				    determineCalibrationTxRate(digi);
+				startSampleCollection(digi);
+				nextState = GOT_SAMPLE_STATE;
+				timeout = SAMPLE_TIMEOUT;
+				expectedEvent = TRANSMIT_DONE_EVENT;
+				break;
+			case GOT_SAMPLE_STATE:
+				processSample(digi);
+				if (calibration.sampleCount < MAX_SAMPLES) {
+					nextState = COLLECT_SAMPLES_STATE;
+					timeout = 0;
+					break;
+				}
+				/* fall through is intended operation */
+			case RECALIBRATE_STATE:
+				recalibrate(digi);
+				calibration.sampleCount = 0;
+				digi->calibrationTxRate = USE_MAC80211_DATA_RATE;
+				nextState = COLLECT_SAMPLES_STATE;
+				timeout = RECALIBRATION_PERIOD;
+				expectedEvent = TIMED_OUT_EVENT;
+				break;
+			default:
+				digi_dbg("Unknown state %d\n", state);
+				nextState = COLLECT_SAMPLES_STATE;
+				timeout = RECALIBRATION_PERIOD;
+				expectedEvent = TIMED_OUT_EVENT;
+				break;
+			}
+
+			state = nextState;
+			event = waitForEvent(timeout, expectedEvent);
+
+			switch (event) {
+			case SHUTDOWN_AUTOCALIBRATION_EVENT:
+				digi_dbg("Received SHUTDOWN_AUTOCALIBRATION_EVENT\n");
+				break;
+			case RESTART_AUTOCALIBRATION_EVENT:
+				digi_dbg("Received RESTART_AUTOCALIBRATION_EVENT\n");
+				state = RESTART_STATE;
+				break;
+			case TRANSMIT_DONE_EVENT:
+				break;
+			case TIMED_OUT_EVENT:
+				if (state == GOT_SAMPLE_STATE) {
+					state = COLLECT_SAMPLES_STATE;
+				}
+			}
+		} while (!kthread_should_stop());
+	} else {
+		printk(KERN_ERR
+		       "\nWarning: Wireless interface calibration data is corrupted.\n");
+		printk(KERN_ERR
+		       "         The wireless interface will operate at a low power level.\n");
+		while (!kthread_should_stop()) {
+			ssleep(1);
+		}
+	}
+	calibration.cops->adc_shutdown(&calibration);
+
+	return 0;
+}
+
+
+
+/*
+ * This routine is called at initialization to set up the Airoha calibration routines.
+ */
+void digiWifiInitCalibration(struct piper_priv *digi)
+{
+
+	calibration.events = 0;
+	calibration.sampleCount = 0;
+	calibration.initialized = false;
+
+	spin_lock_init(&calibration.lock);
+
+	calibration.threadCB =
+	    kthread_run(digiWifiCalibrationThreadEntry, digi,
+			PIPER_DRIVER_NAME " - calibration");
+}
+
+int digiWifiCalibrationPowerIndex(struct piper_priv *piperp)
+{
+    if (calibration.initialized)
+        return calibration.correctedPowerIndex;
+    else
+        return -1;
+}
+
+
+void digiWifiDeInitCalibration(struct piper_priv *digi)
+{
+	calibration.events = SHUTDOWN_AUTOCALIBRATION_EVENT;
+	wake_up_interruptible(&waitQueue);
+	kthread_stop(calibration.threadCB);
+	calibration.initialized = false;
+}
+
+EXPORT_SYMBOL_GPL(digiWifiDeInitCalibration);
diff --git a/drivers/net/wireless/digiPiper/airohaCalibration.h b/drivers/net/wireless/digiPiper/airohaCalibration.h
new file mode 100644
index 000000000000..36543c87fc51
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/airohaCalibration.h
@@ -0,0 +1,108 @@
+/*
+ * This file contains the code which performs automatic recalibration of the
+ * Airoha transceiver.
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#ifndef AIROHA_CALIBRATION_H
+#define AIROHA_CALIBRATION_H
+
+#include "pipermain.h"
+
+
+#define MAX_SAMPLES                     (3)
+
+/*
+ * Field values used for computing ABS values.
+ */
+enum {
+	OUT_POWER,
+	ADC_VAL,
+	POWER_INDEX
+};
+
+#ifdef WANT_CAL_DEBUG
+static const char *fieldDescription[] = {
+	"OUT_POWER",
+	"ADC_VAL",
+	"POWER_INDEX"
+};
+#endif
+
+/*
+ * States for the auto calibration thread.
+ */
+enum {
+	RESTART_STATE,
+	COLLECT_SAMPLES_STATE,
+	GOT_SAMPLE_STATE,
+	RECALIBRATE_STATE
+};
+
+#ifdef WANT_CAL_DEBUG
+static const char *stateText[] = {
+	"RESTART_STATE",
+	"COLLECT_SAMPLES_STATE",
+	"GOT_SAMPLE_STATE",
+	"RECALIBRATE_STATE"
+};
+#endif
+
+
+/*
+ * Slope types accepted by computeSlope().
+ */
+enum {
+	POWER_INDEX_OVER_OUT_POWER,
+	ADC_OVER_OUT_POWER,
+	OUT_POWER_OVER_ADC,
+	POWER_INDEX_OVER_ADC
+};
+
+
+typedef struct {
+	unsigned rate;		/* rate packet transmitted at */
+	unsigned int sample;	/* measured sample */
+} sampleInfo_t;
+
+struct airohaCalibrationData {
+	struct task_struct *threadCB;
+	spinlock_t lock;
+	volatile unsigned int events;
+	unsigned int sampleCount;
+	sampleInfo_t sample[MAX_SAMPLES];
+	wcd_data_t *nvram;
+	wcd_curve_t *curve;
+	int slopeTimes1000;
+	int adcSlopeTimes1000;
+	int outPowerSlopeTimes1000;
+	int powerIndexSlopeTimes1000;
+	int expectedAdc;
+	int powerIndex, correctedPowerIndex;
+	wcd_point_t *p1;
+
+
+	void *priv;
+	struct calibration_ops *cops;
+	bool initialized;
+};
+
+struct calibration_ops {
+	u16(*adc_read_peak) (struct airohaCalibrationData *);
+	void (*adc_clear_peak) (struct airohaCalibrationData *);
+	 u16(*adc_read_last_val) (struct airohaCalibrationData *);
+	void (*adc_shutdown) (struct airohaCalibrationData *);
+};
+
+
+void digiWifiInitCalibration(struct piper_priv *digi);
+void digiWifiDeInitCalibration(struct piper_priv *digi);
+int digiWifiCalibrationPowerIndex(struct piper_priv *piperp);
+
+#endif				/* AIROHA_CALIBRATION_H */
diff --git a/drivers/net/wireless/digiPiper/digiDebug.c b/drivers/net/wireless/digiPiper/digiDebug.c
new file mode 100644
index 000000000000..627dd8c698ec
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/digiDebug.c
@@ -0,0 +1,205 @@
+/*
+ * digiDebug.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/*
+ * This file contains some debugging routines.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+
+#include "pipermain.h"
+#include "mac.h"
+
+#define DUMP_WORDS_MAX      (700)
+static unsigned int dumpWordsWord[DUMP_WORDS_MAX];
+static unsigned int dumpWordsCount = 0;
+
+void digiWifiDumpWordsAdd(unsigned int word)
+{
+    if (dumpWordsCount < DUMP_WORDS_MAX)
+    {
+        dumpWordsWord[dumpWordsCount++] = word;
+    }
+}
+void digiWifiDumpWordsDump(void)
+{
+    unsigned int *p = dumpWordsWord;
+    unsigned int wordsToGo = dumpWordsCount;
+
+    dumpWordsCount = 0;
+
+    while (wordsToGo >= 4)
+    {
+        digi_dbg("%8.8X %8.8X - %8.8X %8.8X\n", p[0], p[1], p[2], p[3]);
+        p += 4;
+        wordsToGo -= 4;
+    }
+    if (wordsToGo == 3)
+    {
+        digi_dbg("%8.8X %8.8X - %8.8X\n", p[0], p[1], p[2]);
+    }
+    else if (wordsToGo == 2)
+    {
+        digi_dbg("%8.8X %8.8X \n", p[0], p[1]);
+    }
+    else if (wordsToGo == 1)
+    {
+        digi_dbg("%8.8X \n", p[0]);
+    }
+    digi_dbg("--------------\n");
+}
+
+void digiWifiDumpWordsReset(void)
+{
+    dumpWordsCount = 0;
+}
+
+
+void digiWifiDumpBuffer(u8 *buffer, unsigned int length)
+{
+    unsigned int i, word;
+
+    digiWifiDumpWordsReset();
+
+    for (i = 0; i < length / sizeof(unsigned int); i++)
+    {
+        memcpy(&word, &buffer[i*sizeof(unsigned int)], sizeof(word));
+        digiWifiDumpWordsAdd(cpu_to_be32(word));
+    }
+
+    digiWifiDumpWordsDump();
+}
+
+
+void digiWifiDumpSkb(struct sk_buff *skb)
+{
+    unsigned int bytesLeft = skb->len;
+    unsigned char *p = skb->data;
+
+    digi_dbg("skb has %d bytes\n", skb->len);
+    while (bytesLeft >= 16)
+    {
+        digi_dbg("%2.2X %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X - %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
+                 p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9], p[10], p[11], p[12], p[13], p[14], p[15]);
+        p += 16;
+        bytesLeft -= 16;
+    }
+    if (bytesLeft >= 8)
+    {
+        digi_dbg("%2.2X %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
+                 p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7]);
+        p += 8;
+        bytesLeft -= 8;
+    }
+    if (bytesLeft >= 4)
+    {
+        digi_dbg("%2.2X %2.2X %2.2X %2.2X \n",
+                 p[0], p[1], p[2], p[3]);
+        p += 4;
+        bytesLeft -= 4;
+    }
+    if (bytesLeft >= 2)
+    {
+        digi_dbg("%2.2X %2.2X \n",
+                 p[0], p[1]);
+        p += 2;
+        bytesLeft -= 2;
+    }
+    if (bytesLeft >= 1)
+    {
+        digi_dbg("%2.2X \n",
+                 p[0]);
+        p += 1;
+        bytesLeft -= 1;
+    }
+}
+
+EXPORT_SYMBOL_GPL(digiWifiDumpSkb);
+
+
+void digiWifiDumpRegisters(struct piper_priv *digi, unsigned int regs)
+{
+#ifdef WANT_DEBUG
+    unsigned int i;
+
+    if (regs & CTRL_STATUS_REGS)
+    {
+                printk(KERN_ERR "  %10.10s = 0x%8.8X  %10.10s = 0x%8.8X\n", "Gen Ctrl", digi->ac->rd_reg(digi, BB_GENERAL_CTL),
+                         "Gen Status", digi->ac->rd_reg(digi, BB_GENERAL_STAT));
+    }
+    else if (regs & IRQ_REGS)
+    {
+                printk(KERN_ERR "  %10.10s = 0x%8.8X  %10.10s = 0x%8.8X\n", "IRQ Mask", digi->ac->rd_reg(digi, BB_IRQ_MASK),
+                         "IRQ Status", digi->ac->rd_reg(digi, BB_IRQ_STAT));
+    }
+    else if (regs & MAIN_REGS)
+    {
+        const char *regNames[] = {"Version", "Gen Ctrl", "Gen Status", "RSSI/AES",
+                                  "Int Mask", "Int Status", "SPI Data", "SPI Ctrl",
+                                  "Data FIFO", "not used", "conf-1", "conf-2", "AES FIFO",
+                                "not used", "AES Ctrl", "IO Ctrl"};
+        printk(KERN_ERR "Main Registers:\n");
+        for (i = BB_VERSION; i <= BB_OUTPUT_CONTROL; i = i+8)
+        {
+            if ((i != BB_DATA_FIFO) && (i != BB_AES_FIFO))
+            {
+                printk(KERN_ERR "  %10.10s = 0x%8.8X  %10.10s = 0x%8.8X\n", regNames[i>>2], digi->ac->rd_reg(digi, i), regNames[(i>>2) + 1], digi->ac->rd_reg(digi, i+4));
+            }
+        }
+    }
+    if (regs & MAC_REGS)
+    {
+        const char *regNames[] = {"STA ID0", "STA ID1", "BSS ID0", "BSS ID1",
+                                  "OFDM/PSK", "Backoff", "DTIM/List", "B Int",
+                                "Rev/M Stat", "C C/M CTL", "Measure", "Beac Fltr"};
+
+        printk(KERN_ERR "Secondary Registers:\n");
+        for (i = MAC_STA_ID0; i <= MAC_BEACON_FILT; i = i+8)
+        {
+            printk(KERN_ERR "  %10.10s = 0x%8.8X  %10.10s = 0x%8.8X\n", regNames[((i - MAC_STA_ID0) >>2)], digi->ac->rd_reg(digi, i), regNames[((i - MAC_STA_ID0)>>2) + 1], digi->ac->rd_reg(digi, i+4));
+        }
+    }
+    if (regs & FRAME_BUFFER_REGS)
+    {
+        unsigned int word[4];
+        printk(KERN_ERR "Real time frame buffer\n");
+
+        word[0] = be32_to_cpu(digi->ac->rd_reg(digi, 0xc0));
+        word[1] = be32_to_cpu(digi->ac->rd_reg(digi, 0xc4));
+        word[2] = be32_to_cpu(digi->ac->rd_reg(digi, 0xc8));
+        word[3] = be32_to_cpu(digi->ac->rd_reg(digi, 0xcc));
+        printk(KERN_ERR " %8.8X %8.8X - %8.8X %8.8X\n", word[0], word[1], word[2], word[3]);
+        word[0] = be32_to_cpu(digi->ac->rd_reg(digi, 0xd0));
+        word[1] = be32_to_cpu(digi->ac->rd_reg(digi, 0xd4));
+        word[2] = be32_to_cpu(digi->ac->rd_reg(digi, 0xd8));
+        word[3] = be32_to_cpu(digi->ac->rd_reg(digi, 0xdc));
+        printk(KERN_ERR " %8.8X %8.8X - %8.8X %8.8X\n", word[0], word[1], word[2], word[3]);
+    }
+    if (regs & FIFO_REGS)
+    {
+        unsigned int word[4];
+        printk(KERN_ERR "FIFO contents\n");
+
+        word[0] = digi->ac->rd_reg(digi, BB_DATA_FIFO);
+        word[1] = digi->ac->rd_reg(digi, BB_DATA_FIFO);
+        word[2] = digi->ac->rd_reg(digi, BB_DATA_FIFO);
+        word[3] = digi->ac->rd_reg(digi, BB_DATA_FIFO);
+        printk(KERN_ERR " %8.8X %8.8X - %8.8X %8.8X\n", word[0], word[1], word[2], word[3]);
+        word[0] = digi->ac->rd_reg(digi, BB_DATA_FIFO);
+        word[1] = digi->ac->rd_reg(digi, BB_DATA_FIFO);
+        word[2] = digi->ac->rd_reg(digi, BB_DATA_FIFO);
+        word[3] = digi->ac->rd_reg(digi, BB_DATA_FIFO);
+        printk(KERN_ERR " %8.8X %8.8X - %8.8X %8.8X\n", word[0], word[1], word[2], word[3]);
+    }
+#endif
+}
+EXPORT_SYMBOL_GPL(digiWifiDumpRegisters);
diff --git a/drivers/net/wireless/digiPiper/digiIsr.c b/drivers/net/wireless/digiPiper/digiIsr.c
new file mode 100644
index 000000000000..e2128e254055
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/digiIsr.c
@@ -0,0 +1,159 @@
+/*
+ * digiIsr.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/*
+ * This file contains the routines that are related to processing interrupts
+ * from the MAC.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+
+#include "pipermain.h"
+#include "mac.h"
+#include "phy.h"
+
+#define IRQ_DEBUG	(0)
+#if IRQ_DEBUG
+static int dlevel = DWARNING;
+#define dprintk(level, fmt, arg...)	if (level >= dlevel)			\
+					printk(KERN_ERR PIPER_DRIVER_NAME	\
+					    ": %s - " fmt, __func__, ##arg)
+#else
+#define dprintk(level, fmt, arg...)	do {} while (0)
+#endif
+
+/*
+ * This routine handles interrupts from the MAC.
+ */
+irqreturn_t piper_irq_handler(int irq, void *dev_id)
+{
+	struct piper_priv *piperp = dev_id;
+	u32 status;
+
+	/* Acknowledge pending interrupts */
+	status = piperp->ac->rd_reg(piperp, BB_IRQ_STAT);
+	status &= piperp->ac->rd_reg(piperp, BB_IRQ_MASK);
+	piperp->ac->wr_reg(piperp, BB_IRQ_STAT, status, op_write);
+
+	if (status & BB_IRQ_MASK_RX_FIFO) {
+		/*
+		 * This interrupt indicates we have a frame in the FIFO.
+		 * Set up to receive the packet.  Disable further interrupts
+		 * until the receive is complete.
+		 */
+		piperp->clear_irq_mask_bit(piperp, BB_IRQ_MASK_RX_FIFO);
+		/*
+		 * Call the receive routine directly inside the irq handler
+		 * or in the tasklet, depending on configuration.
+		 */
+#if WANT_TO_RECEIVE_FRAMES_IN_ISR
+		piper_rx_tasklet((unsigned long) piperp);
+#else
+		tasklet_hi_schedule(&piperp->rx_tasklet);
+#endif
+	}
+
+	if (status & BB_IRQ_MASK_TX_FIFO_EMPTY) {
+		/*
+		 * Transmit complete interrupt.  This IRQ is only unmasked if we are
+		 * not expecting the packet to be ACKed.  This will be the case for
+		 * broadcasts.  In this case, tell mac80211 the transmit occurred and
+		 * restart the tx queue.
+		 */
+		if (piper_tx_getqueue(piperp) != NULL) {
+			piperp->tx_signal_strength = 0;
+			piperp->tx_result = TX_COMPLETE;
+			tasklet_hi_schedule(&piperp->tx_tasklet);
+		} else {
+			dprintk(DWARNING, "BB_IRQ_MASK_TX_FIFO_EMPTY and null packet?\n");
+		}
+		piperp->clear_irq_mask_bit(piperp, BB_IRQ_MASK_TX_FIFO_EMPTY |
+					BB_IRQ_MASK_TIMEOUT | BB_IRQ_MASK_TX_ABORT);
+	}
+
+	if (status & BB_IRQ_MASK_TIMEOUT) {
+		/* AP did not ACK our TX packet */
+		if (piper_tx_getqueue(piperp) != NULL) {
+			/* Update retry counter */
+			tasklet_hi_schedule(&piperp->tx_tasklet);
+		} else {
+			dprintk(DWARNING, "BB_IRQ_MASK_TIMEOUT and null packet?\n");
+		}
+		piperp->clear_irq_mask_bit(piperp, BB_IRQ_MASK_TX_FIFO_EMPTY |
+					BB_IRQ_MASK_TIMEOUT | BB_IRQ_MASK_TX_ABORT);
+	}
+
+	if (unlikely(status & BB_IRQ_MASK_TX_ABORT)) {
+		dprintk(DWARNING, "TX abort\n");
+
+		/* Could not transmit a packet because the media was busy */
+		if (piper_tx_getqueue(piperp) != NULL) {
+			tasklet_hi_schedule(&piperp->tx_tasklet);
+		} else {
+			dprintk(DWARNING, "BB_IRQ_MASK_TX_ABORT and null packet?\n");
+		}
+		piperp->clear_irq_mask_bit(piperp, BB_IRQ_MASK_TX_FIFO_EMPTY |
+					BB_IRQ_MASK_TIMEOUT | BB_IRQ_MASK_TX_ABORT);
+	}
+
+	if (status & BB_IRQ_MASK_TBTT) {
+		/*
+		 * This interrupt occurs at the start of a beacon period.  The only thing
+		 * we need to do is to write a new beacon backoff value.
+		 */
+		u32 reg = piperp->ac->rd_reg(piperp, MAC_BEACON_FILT) & ~MAC_BEACON_BACKOFF_MASK;
+		piperp->ac->wr_reg(piperp, MAC_BEACON_FILT,
+				reg | piperp->get_next_beacon_backoff(), op_write);
+		/*
+		 * TODO:
+		 * Improve the way we keep track of whether or not we sent the last
+		 * beacon.  What we are doing now is to assume that we did until and
+		 * unless we receive a beacon.  What we should do is look for either
+		 * a beacon or a TX end interrupt.  However, since mac80211 doesn't
+		 * tell us what the ATIM window is, we have to assume it is zero,
+		 * which means we could be transmitting a frame at the same
+		 * time we are sending the beacon, so there isn't really any easy
+		 * way for us to do this.  In fact, even if there was an ATIM
+		 * window, we could have started a transmit just before we get this
+		 * interrupt, so I'm not sure how we are really suppose to keep
+		 * track of this.
+		 */
+		/* assume we sent last beacon unless we receive one */
+		piperp->beacon.weSentLastOne = true;
+	}
+
+	if (status & BB_IRQ_MASK_ATIM) {
+		/*
+		 * This interrupt should not occur since we are not using it.  When in
+		 * IBSS mode, the beacon period starts at the TBTT interrupt and ends
+		 * at this interrupt.  We are not suppose to send packets between the
+		 * two interrupts.  However, mac80211 does not seem to provide a way
+		 * for us to find out how long the ATIM period is, so we have to assume
+		 * that there isn't one.
+		 *
+		 * If we were supporting this interrupt we would have to synchronize
+		 * with the transmit routine so that transmit is paused during this
+		 * time.
+		 */
+		dprintk(DWARNING, "BB_IRQ_MASK_ATIM irq (0x%08x)\n", status);
+		piperp->clear_irq_mask_bit(piperp, BB_IRQ_MASK_ATIM);
+	}
+
+	if (unlikely(status & BB_IRQ_MASK_RX_OVERRUN))
+		piperp->pstats.rx_overruns++;
+
+	return IRQ_HANDLED;
+}
+EXPORT_SYMBOL_GPL(piper_irq_handler);
+
+
+
diff --git a/drivers/net/wireless/digiPiper/digiMac80211.c b/drivers/net/wireless/digiPiper/digiMac80211.c
new file mode 100644
index 000000000000..b9ca3ef10230
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/digiMac80211.c
@@ -0,0 +1,1000 @@
+/*
+ * digiMac80211.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/*
+ * This file contains the routines that interface with the mac80211
+ * library.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/etherdevice.h>
+#include <net/mac80211.h>
+#include <crypto/aes.h>
+#include "pipermain.h"
+#include "mac.h"
+#include "phy.h"
+#include "digiPs.h"
+
+#define MAC_DEBUG	(1)
+
+#if MAC_DEBUG
+static int dlevel = DWARNING;
+#define dprintk(level, fmt, arg...)	if (level >= dlevel)			\
+					printk(KERN_ERR PIPER_DRIVER_NAME	\
+					    ": %s - " fmt, __func__, ##arg)
+#else
+#define dprintk(level, fmt, arg...)	do {} while (0)
+#endif
+
+/*
+ * This constant determines how many times per second the led_timer_fn
+ * function will be called.  (HZ >> 3) means 8 times a second.
+ */
+#define LED_TIMER_RATE		(HZ >> 3)
+#define LED_MAX_COUNT			(15)
+
+/*
+ * This function is called from a timer to blink an LED in order to
+ * indicate what are current state is.
+ */
+static void led_timer_fn(unsigned long context)
+{
+	struct piper_priv *piperp = (struct piper_priv *) context;
+	static unsigned int count = 0;
+
+	if(!piperp->pdata->set_led)
+		return;
+
+	switch (piperp->led_state) {
+		case led_shutdown:
+			/* Turn LED off if we are shut down */
+			piperp->pdata->set_led(piperp, STATUS_LED, 0);
+			break;
+		case led_adhoc:
+			/* Blink LED slowly in ad-hoc mode */
+			piperp->pdata->set_led(piperp, STATUS_LED, (count & 8) ? 0 : 1);
+			break;
+		case led_not_associated:
+			/* Blink LED rapidly when not associated with an AP */
+			piperp->pdata->set_led(piperp, STATUS_LED, (count & 1) ? 0 : 1);
+			break;
+		case led_associated:
+			/* LED steadily on when associated */
+			piperp->pdata->set_led(piperp, STATUS_LED, 1);
+			break;
+		default:
+			/* Oops.  How did we get here? */
+			break;
+	}
+	count++;
+	if (count > LED_MAX_COUNT) {
+		count = 0;
+	}
+
+	piperp->led_timer.expires += LED_TIMER_RATE;
+	add_timer(&piperp->led_timer);
+}
+
+/*
+ * This function sets the current LED state.
+ */
+static int piper_set_status_led(struct ieee80211_hw *hw, enum led_states state)
+{
+	struct piper_priv *piperp = (struct piper_priv *)hw->priv;
+
+	piperp->led_state = state;
+
+	if(!piperp->pdata->set_led)
+		return -ENOSYS;
+
+	if (state == led_shutdown)
+		piperp->pdata->set_led(piperp, STATUS_LED, 0);
+
+	return 0;
+}
+
+/*
+ * This routine is called to enable IBSS support whenever we receive
+ * configuration commands from mac80211 related to IBSS support.  The
+ * routine examines the configuration settings to determine if IBSS
+ * support should be enabled and, if so, turns on automatic beacon
+ * generation.
+ *
+ * TODO: This code may need to be moved into piper.c since other
+ *       H/W does not implement automatic generate of beacons.
+ */
+static void piper_enable_ibss(struct piper_priv *piperp, enum nl80211_iftype iftype)
+{
+	dprintk(DVVERBOSE, "\n");
+
+	if (((iftype == NL80211_IFTYPE_ADHOC) || (iftype == NL80211_IFTYPE_MESH_POINT))
+	    && (piperp->beacon.loaded) && (piperp->beacon.enabled)
+	    && ((piperp->ac->rd_reg(piperp, MAC_CFP_ATIM) & MAC_BEACON_INTERVAL_MASK) != 0)) {
+		/*
+		 * If we come here, then we are running in IBSS mode, beacons are enabled,
+		 * and we have the information we need, so start sending beacons.
+		 */
+		/* TODO: Handle non-zero ATIM period.  mac80211 currently has no way to
+		 * tell us what the ATIM period is, but someday they might fix that.*/
+
+		u32 reg = piperp->ac->rd_reg(piperp, MAC_BEACON_FILT) & ~MAC_BEACON_BACKOFF_MASK;
+		piperp->ac->wr_reg(piperp, MAC_BEACON_FILT,
+				  reg | piperp->get_next_beacon_backoff(), op_write);
+		/* enable beacon interrupts*/
+		piperp->set_irq_mask_bit(piperp, BB_IRQ_MASK_TBTT);
+		piperp->ac->wr_reg(piperp,
+				  MAC_CTL, MAC_CTL_BEACON_TX | MAC_CTL_IBSS, op_or);
+		dprintk(DVERBOSE, "IBSS turned ON\n");
+		piper_set_status_led(piperp->hw, led_adhoc);
+	} else {
+		/*
+		 * If we come here, then either we are not suppose to transmit beacons,
+		 * or we do not yet have all the information we need to transmit
+		 * beacons.  Make sure the automatic beacon function is disabled.
+		 */
+                 /* shut down beacons */
+		piperp->ac->wr_reg(piperp, MAC_CTL,
+				  ~(MAC_CTL_BEACON_TX | MAC_CTL_IBSS), op_and);
+		piperp->set_irq_mask_bit(piperp, BB_IRQ_MASK_TBTT);
+		dprintk(DVERBOSE, "IBSS turned OFF\n");
+	}
+}
+
+/*
+ * Set the transmit power level.  The real work is done in the
+ * transceiver code.
+ */
+static int piper_set_tx_power(struct ieee80211_hw *hw, int power)
+{
+	struct piper_priv *digi = hw->priv;
+	int err;
+	int oldTxPower = digi->tx_power;
+
+	dprintk(DVVERBOSE, "\n");
+
+	if (power == digi->tx_power)
+		return 0;
+
+	digi->tx_power = power;
+	err = digi->rf->set_pwr(hw, power);
+	if (err)
+		digi->tx_power = oldTxPower;
+
+	return err;
+}
+
+/*
+ * Utility routine that sets a sequence number for a data packet.
+ */
+static void assign_seq_number(struct sk_buff *skb, bool increment)
+{
+#define SEQUENCE_NUMBER_MASK        (0xfff0)
+	static u16 seq_number = 0;
+	_80211HeaderType *header = (_80211HeaderType *)skb->data;
+
+	dprintk(DVVERBOSE, "\n");
+
+	if (skb->len >= sizeof(*header)) {
+		u16 seq_field;
+
+		/*
+		 * TODO:  memcpy's are here because I am concerned we may get
+		 *        an unaligned frame.  Is this a real possibility?  Or
+		 *        am I just wasting CPU time?
+		 */
+		memcpy(&seq_field, &header->squ.sq16, sizeof(header->squ.sq16));
+		seq_field &= ~SEQUENCE_NUMBER_MASK;
+		seq_field |= (SEQUENCE_NUMBER_MASK & (seq_number << 4));
+		memcpy(&header->squ.sq16, &seq_field, sizeof(header->squ.sq16));
+		if (increment)
+			seq_number++;
+	}
+}
+
+#define GETU32(pt) (((u32)(pt)[0] << 24) ^ ((u32)(pt)[1] << 16) ^ ((u32)(pt)[2] <<  8) ^ ((u32)(pt)[3]))
+/* Get 16 bits at byte pointer */
+#define	GET16(bp)		((bp)[0] | ((bp)[1] << 8))
+/* Get 32 bits at byte pointer */
+#define	GET32(bp)		((bp)[0] | ((bp)[1] << 8) | ((bp)[2] << 16) | ((bp)[3] << 24))
+/* Store 16 bits at byte pointer */
+#define	SET16(bp, data)		{ (bp)[0] = (data); \
+				(bp)[1] = (data) >> 8; }
+/* Store 32 bits at byte pointer */
+#define	SET32(bp, data)		{ (bp)[0] = (data); \
+				(bp)[1] = (data) >> 8;  \
+				(bp)[2] = (data) >> 16; \
+				(bp)[3] = (data) >> 24; }
+
+static inline void dw_inc_48(u48* n)
+{
+	(*n)++;
+	*n &= ((u64) 1 << 48) - 1;
+}
+
+/*
+ * This function prepares a blob of data we will have to send to the AES
+ * H/W encryption engine.  The data consists of the AES initialization
+ * vector and 2 16 byte headers.
+ *
+ * Returns true if successful, or false if something goes wrong
+ */
+bool piper_prepare_aes_datablob(struct piper_priv *piperp, unsigned int keyIndex,
+				u8 *aesBlob, u8 *frame, u32 length, bool isTransmit)
+{
+	_80211HeaderType *header = (_80211HeaderType *)frame;
+	u8 *body = &frame[sizeof(*header)];
+	int dlen = length - (_80211_HEADER_LENGTH + PIPER_EXTIV_SIZE);
+
+	dprintk(DVVERBOSE, "\n");
+
+	if (keyIndex >= PIPER_MAX_KEYS) {
+		dprintk(DWARNING, "encryption key index %d is out of range.\n",
+			keyIndex);
+		return false;
+	}
+
+	if (piperp->key[keyIndex].valid == false)
+		return false;
+
+	/* Set up CCM initial block for MIC IV */
+	memset(aesBlob, 0, AES_BLOB_LENGTH);
+	aesBlob[0] = 0x59;
+	aesBlob[1] = 0;
+	memcpy (&aesBlob[2], header->addr2, ETH_ALEN);
+	aesBlob[8]  = body[7];
+	aesBlob[9]  = body[6];
+	aesBlob[10] = body[5];
+	aesBlob[11] = body[4];
+	aesBlob[12] = body[1];
+	aesBlob[13] = body[0];
+	aesBlob[14] = dlen >> 8;
+	aesBlob[15] = dlen;
+
+	/* Set up MIC header blocks */
+#define AES_HEADER_0_OFFSET (16)
+#define AES_HEADER_1_OFFSET (32)
+	aesBlob[AES_HEADER_0_OFFSET+0] = 0;
+	aesBlob[AES_HEADER_0_OFFSET+1] = 22;
+	aesBlob[AES_HEADER_0_OFFSET+2] = frame[0] & 0xcf;
+	aesBlob[AES_HEADER_0_OFFSET+3] = frame[1] & 0xd7;
+	/*
+	 * This memcpy writes data into the last 12 bytes of the first header
+	 * and the first 6 bytes of the 2nd header.  I did it as one memcpy
+	 * call for efficiency.
+	 */
+	memcpy(&aesBlob[AES_HEADER_0_OFFSET+4], header->addr1, 3*ETH_ALEN);
+	aesBlob[AES_HEADER_1_OFFSET+6] = header->squ.sq.frag;
+	aesBlob[AES_HEADER_1_OFFSET+7] = 0;
+	memset (&aesBlob[AES_HEADER_1_OFFSET+8], 0, 8);	/* clear vector location in data */
+
+	return true;
+}
+
+/*
+ * mac80211 calls this routine to transmit a frame.  We set up
+ * up the information the trasmit tasklet will need, and then
+ * schedule the tasklet.
+ */
+int piper_hw_tx_private(struct ieee80211_hw *hw, struct sk_buff *skb, tx_skb_return_cb_t skb_return_cb)
+{
+	struct piper_priv *piperp = hw->priv;
+	struct ieee80211_tx_info *txInfo = IEEE80211_SKB_CB(skb);
+	unsigned long flags;
+
+	dprintk(DVVERBOSE, "\n");
+
+	if (piperp->is_radio_on == false) {
+		dprintk(DERROR, "called with radio off\n");
+		return -EBUSY;
+	}
+
+	/*
+	 * Our H/W can only transmit a single packet at a time.  mac80211
+	 * already maintains a queue of packets, so there is no reason
+	 * for us to set up another one.  We stop the mac80211 queue everytime
+	 * we get a transmit request and restart it when the transmit
+	 * operation completes.
+	 */
+	ieee80211_stop_queues(hw);
+
+	if (txInfo->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
+		assign_seq_number(skb,
+				  !!(txInfo->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT));
+	}
+
+	piperp->use_hw_aes = false;
+	if (txInfo->control.hw_key != NULL) {
+		/*
+		 * We've been passed an encryption key, so mac80211 must want us
+		 * to encrypt the packet with our fancy H/W encryptor.  Let's get
+		 * set up for that now.
+		 */
+		piperp->tx_aes_key = txInfo->control.hw_key->hw_key_idx;
+		piperp->use_hw_aes =
+			  piper_prepare_aes_datablob(piperp,
+						     txInfo->control.hw_key->hw_key_idx,
+						     (u8 *)piperp->tx_aes_blob, skb->data,
+						     skb->len, true);
+	}
+	piper_ps_set_header_flag(piperp, ((_80211HeaderType *) skb->data));		/* set power management bit as appropriate*/
+
+	/*
+	 * Add space at the start of the frame for the H/W level transmit header.
+	 * We can't generate the header now. It must be generated everytime we
+	 * transmit because the transmit rate changes when we do retries.
+	 */
+	skb_push(skb, TX_HEADER_LENGTH);
+
+	piperp->pstats.tx_retry_index = 0;
+	piperp->pstats.tx_total_tetries = 0;
+	memset(piperp->pstats.tx_retry_count, 0, sizeof(piperp->pstats.tx_retry_count));
+	txInfo->flags &= ~(IEEE80211_TX_STAT_TX_FILTERED |
+			   IEEE80211_TX_STAT_ACK |
+			   IEEE80211_TX_STAT_AMPDU |
+			   IEEE80211_TX_STAT_AMPDU_NO_BACK);
+	piperp->pstats.tx_queue.len++;
+	piperp->pstats.tx_queue.count++;
+
+	if (piper_tx_enqueue(piperp, skb, skb_return_cb) == -1) {
+		skb_pull(skb, TX_HEADER_LENGTH);		/* undo the skb_push above */
+		return -EBUSY;
+	}
+
+	spin_lock_irqsave(&piperp->tx_tasklet_lock, flags);
+	if (!piperp->tx_tasklet_running) {
+		piperp->tx_tasklet_running = true;
+		tasklet_hi_schedule(&piperp->tx_tasklet);
+	}
+
+	spin_unlock_irqrestore(&piperp->tx_tasklet_lock, flags);
+
+	piperp->pstats.tx_start_count++;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(piper_hw_tx_private);
+
+
+int piper_hw_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	return piper_hw_tx_private(hw, skb, ieee80211_tx_status_irqsafe);
+}
+
+
+/*
+ * mac80211 calls this routine to initialize the H/W.
+ */
+static int piper_hw_start(struct ieee80211_hw *hw)
+{
+	struct piper_priv *piperp = hw->priv;
+	int ret = 0;
+
+	dprintk(DVVERBOSE, "\n");
+	piperp->if_type = __NL80211_IFTYPE_AFTER_LAST;
+
+	/* Initialize the MAC H/W */
+	if ((ret = piperp->init_hw(piperp, IEEE80211_BAND_2GHZ)) != 0) {
+		dprintk(DERROR, "unable to initialize piper HW (%d)\n", ret);
+		return ret;
+	}
+
+	piperp->is_radio_on = true;
+
+	/*
+	 * Initialize the antenna with the defualt setting defined in the
+	 * probe function. This can be changed, currently, through a sysfs
+	 * entry in the device directory
+	 */
+	if ((ret = piperp->set_antenna(piperp, piperp->antenna)) != 0) {
+		dprintk(DERROR, "piper_set_antenna_div() failed (%d)\n", ret);
+		return ret;
+	}
+
+	/* set status led to link off */
+	piper_set_status_led(hw, led_shutdown);
+
+	/* Get the tasklets ready to roll */
+	piperp->tx_result = TX_NOT_DONE;
+	tasklet_enable(&piperp->rx_tasklet);
+	tasklet_enable(&piperp->tx_tasklet);
+
+	/*
+	 * Enable receive interrupts, but leave the transmit interrupts
+	 * and beacon interrupts off for now.
+	 */
+	piperp->clear_irq_mask_bit(piperp, 0xffffffff);
+	piperp->set_irq_mask_bit(piperp,
+				 BB_IRQ_MASK_RX_OVERRUN | BB_IRQ_MASK_RX_FIFO);
+	enable_irq(piperp->irq);
+
+	memset(piperp->bssid, 0, ETH_ALEN);
+
+	return 0;
+}
+
+/*
+ * mac80211 calls this routine to shut us down.
+ */
+static void piper_hw_stop(struct ieee80211_hw *hw)
+{
+	struct piper_priv *piperp = hw->priv;
+
+	dprintk(DVVERBOSE, "\n");
+
+	/* Initialize the MAC H/W */
+	if (piperp->deinit_hw)
+		piperp->deinit_hw(piperp);
+
+	/* set status led to link off */
+	if (piper_set_status_led(hw, led_shutdown))
+		return;		/* hardware's probably gone, give up */
+
+	/* turn off phy */
+	piperp->rf->stop(hw);
+
+	/* Disable interrupts before turning off */
+	disable_irq(piperp->irq);
+
+	/* turn off MAX_GAIN, ADC clocks, and so on */
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, ~BB_GENERAL_CTL_RESET, op_and);
+
+	/* turn off MAC control/mac filt/aes key */
+	piperp->ac->wr_reg(piperp, MAC_CTL, 0, op_write);
+
+	/* turn off interrupts */
+	tasklet_disable(&piperp->rx_tasklet);
+	tasklet_disable(&piperp->tx_tasklet);
+	piperp->clear_irq_mask_bit(piperp, 0xffffffff);
+}
+
+/*
+ * mac80211 calls this routine to really start the H/W.
+ * The device type is also set here.
+ */
+static int piper_hw_add_intf(struct ieee80211_hw *hw,
+		struct ieee80211_if_init_conf *conf)
+{
+	struct piper_priv *piperp = hw->priv;
+
+	dprintk(DVVERBOSE, "if type: %x\n", conf->type);
+
+	/* __NL80211_IFTYPE_AFTER_LAST means no mode selected */
+	if (piperp->if_type != __NL80211_IFTYPE_AFTER_LAST) {
+		dprintk(DERROR, "unsupported interface type %x, expected %x\n",
+			conf->type, __NL80211_IFTYPE_AFTER_LAST);
+		return -EOPNOTSUPP;
+	}
+
+	switch (conf->type) {
+	case NL80211_IFTYPE_ADHOC:
+		piper_set_status_led(piperp->hw, led_adhoc);
+		piperp->if_type = conf->type;
+		break;
+
+	case NL80211_IFTYPE_STATION:
+		piper_set_status_led(hw, led_not_associated);
+		piperp->if_type = conf->type;
+		break;
+
+	case NL80211_IFTYPE_MESH_POINT:
+		piperp->if_type = conf->type;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+/*
+ * mac80211 calls this function to shut down us down.
+ */
+static void piper_hw_rm_intf(struct ieee80211_hw *hw,
+		struct ieee80211_if_init_conf *conf)
+{
+	struct piper_priv *digi = hw->priv;
+
+	dprintk(DVVERBOSE, "\n");
+	digi->if_type = __NL80211_IFTYPE_AFTER_LAST;
+}
+
+/*
+ * mac80211 calls this function to pass us configuration information.
+ */
+static int piper_config(struct ieee80211_hw *hw, u32 changed)
+{
+	struct piper_priv *piperp = hw->priv;
+	struct ieee80211_conf *conf = &hw->conf;
+	u32 tempval;
+	int err;
+
+	dprintk(DVVERBOSE, "\n");
+
+	if (changed & IEEE80211_CONF_CHANGE_PS) {
+		/*
+		 * Enable power save mode if bit set in flags, and if we are in station
+		 * mode.  Power save is not supported in ad-hoc/mesh mode.
+		 */
+		piper_ps_scan_event(piperp);
+		piper_ps_set(piperp, (	(conf->flags & IEEE80211_CONF_PS)
+							  && (piperp->if_type == NL80211_IFTYPE_STATION)
+							  && (piperp->areWeAssociated)));
+	}
+	/* Should we turn the radio off? */
+	if ((piperp->is_radio_on = (conf->radio_enabled != 0)) != 0) {
+		piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+				  BB_GENERAL_CTL_RX_EN, op_or);
+	} else {
+		dprintk(DNORMAL, "Turning radio off\n");
+		return piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+					~BB_GENERAL_CTL_RX_EN, op_and);
+	}
+
+	/* Adjust the beacon interval and listen interval */
+	tempval = piperp->ac->rd_reg(piperp, MAC_CFP_ATIM) & ~MAC_BEACON_INTERVAL_MASK;
+	tempval |= conf->beacon_int << MAC_BEACON_INTERVAL_SHIFT;
+	piperp->ac->wr_reg(piperp, MAC_CFP_ATIM, tempval, op_write);
+
+	tempval = piperp->ac->rd_reg(piperp, MAC_DTIM_PERIOD) & ~MAC_LISTEN_INTERVAL_MASK;
+	tempval |= conf->listen_interval;
+	piperp->ac->wr_reg(piperp, MAC_DTIM_PERIOD, tempval, op_write);
+
+	/* Adjust the power level */
+	if ((err = piper_set_tx_power(hw, conf->power_level)) != 0) {
+		dprintk(DERROR, "unable to set tx power to %d\n",
+		      conf->power_level);
+		return err;
+	}
+
+	/* Set channel */
+	if (conf->channel->hw_value != piperp->channel) {
+		piper_ps_scan_event(piperp);
+		if ((err = piperp->rf->set_chan(hw, conf->channel->hw_value)) !=0) {
+			dprintk(DERROR, "unable to set ch to %d\n",
+				conf->channel->hw_value);
+			return err;
+		}
+		piperp->channel = conf->channel->hw_value;
+	}
+
+	return 0;
+}
+
+/*
+ * mac80211 calls this routine to set BSS related configuration settings.
+ */
+static int piper_hw_config_intf(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		struct ieee80211_if_conf *conf)
+{
+	struct piper_priv *piperp = hw->priv;
+	u32 bssid[2];
+
+	dprintk(DVVERBOSE, "\n");
+
+	if (conf->changed & IEEE80211_IFCC_BSSID &&
+	    !is_zero_ether_addr(conf->bssid) &&
+	    !is_multicast_ether_addr(conf->bssid)) {
+
+		piper_ps_scan_event(piperp);
+		switch (vif->type) {
+		case NL80211_IFTYPE_STATION:
+		case NL80211_IFTYPE_ADHOC:
+			dprintk(DVERBOSE, "BSSID: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
+				conf->bssid[0], conf->bssid[1], conf->bssid[2],
+				conf->bssid[3],	conf->bssid[4], conf->bssid[5]);
+
+			bssid[0] = conf->bssid[3] | conf->bssid[2] << 8 |
+				  conf->bssid[1] << 16 | conf->bssid[0] << 24;
+			bssid[1] = conf->bssid[5] << 16 | conf->bssid[4] << 24;
+
+			if ((bssid[0] == 0) && (bssid[1] == 0)) {
+				/*
+				* If we come here, then the MAC layer is telling us to set a 0
+				* SSID.  In this case, we really want to set the SSID to the
+				* broadcast address so that we receive broadcasts.
+				*/
+				bssid[0] = 0xffffffff;
+				bssid[1] = 0xffffffff;
+			}
+			piperp->ac->wr_reg(piperp, MAC_BSS_ID0, bssid[0], op_write);
+			piperp->ac->wr_reg(piperp, MAC_BSS_ID1, bssid[1], op_write);
+			memcpy(piperp->bssid, conf->bssid, ETH_ALEN);
+			break;
+		default:
+			break;
+		}
+	}
+
+	if ((conf->changed & IEEE80211_IFCC_BEACON) &&
+	    (piperp->if_type == NL80211_IFTYPE_ADHOC)) {
+		struct sk_buff *beacon = ieee80211_beacon_get(hw, vif);
+		struct ieee80211_rate rate;
+
+		if (!beacon)
+			return -ENOMEM;
+
+		rate.bitrate = 10;	/* beacons always sent at 1 Megabit*/
+		skb_push(beacon, TX_HEADER_LENGTH);
+		phy_set_plcp(beacon->data, beacon->len - TX_HEADER_LENGTH, &rate, 0);
+		piperp->ac->wr_reg(piperp, MAC_CTL, ~MAC_CTL_BEACON_TX, op_and);
+		piperp->load_beacon(piperp, beacon->data, beacon->len);
+
+		/* TODO: digi->beacon.enabled should be set by IEEE80211_IFCC_BEACON_ENABLED
+                 when we update to latest mac80211 */
+		piperp->beacon.enabled = true;
+		piper_enable_ibss(piperp, vif->type);
+		dev_kfree_skb(beacon);          /* we are responsible for freeing this buffer*/
+	}
+
+	return 0;
+}
+
+/*
+ * mac80211 wants to change our frame filtering settings.  We don't
+ * actually support this.
+ */
+static void piper_hw_config_filter(struct ieee80211_hw *hw,
+		unsigned int changed_flags, unsigned int *total_flags,
+		int mc_count, struct dev_addr_list *mclist)
+{
+	dprintk(DVVERBOSE, "\n");
+
+	/* we don't support filtering so clear all flags; however, we also
+	 * can't pass failed FCS/PLCP frames, so don't clear those. */
+	*total_flags &= (FIF_FCSFAIL | FIF_PLCPFAIL);
+}
+
+/*
+ * There are 1024 TU's (time units) to a second, and HZ jiffies to a
+ * second.  This macro converts TUs to jiffies.
+ */
+#define TU_TO_JIFFIES(x)		(((x*HZ) + 512) / 1024)
+
+
+/*
+ * mac80211 calls this routine when something about our BSS has changed.
+ * Usually, this routine only gets called when we associate, or disassociate.
+ */
+static void piper_hw_bss_changed(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			   struct ieee80211_bss_conf *conf, u32 changed)
+{
+	struct piper_priv *piperp = hw->priv;
+	u32 reg;
+
+	dprintk(DVVERBOSE, " changed = 0x%08x\n", changed);
+
+	if (changed & BSS_CHANGED_ASSOC) {
+		piper_ps_scan_event(piperp);
+		/* Our association status has changed */
+		if (piperp->if_type == NL80211_IFTYPE_STATION) {
+			piper_set_status_led(hw, conf->assoc ? led_associated : led_not_associated);
+		}
+		piperp->areWeAssociated = conf->assoc;
+		piperp->ps.aid = conf->aid;
+
+		digi_dbg(" AP %s\n", conf->assoc ?
+			"associated" : "disassociated");
+	}
+	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
+		piperp->tx_cts = conf->use_cts_prot;
+	}
+	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
+#define WANT_SHORT_PREAMBLE_SUPPORT     (1)
+/* TODO: Determine if short preambles really hurt us, or if I'm just seeing things. */
+#if WANT_SHORT_PREAMBLE_SUPPORT
+		if (conf->use_short_preamble) {
+			piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+					   BB_GENERAL_CTL_SH_PRE, op_or);
+		} else {
+			piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+					   ~BB_GENERAL_CTL_SH_PRE, op_and);
+		}
+		piperp->use_short_preamble = conf->use_short_preamble;
+#else
+		piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, BB_GENERAL_CTL_SH_PRE, op_or);
+#endif
+	}
+
+	if (changed & BSS_CHANGED_BASIC_RATES) {
+		/*
+		 * The list of transmit rates has changed.  Update the
+		 * rates we will receive at to match those the AP will
+		 * transmit at.  This should improve our receive performance
+		 * since we won't listen to junk at the wrong rate.
+		 */
+		unsigned int ofdm = 0, psk = 0;
+
+		reg = piperp->ac->rd_reg(piperp, MAC_SSID_LEN) &
+				         ~(MAC_OFDM_BRS_MASK | MAC_PSK_BRS_MASK);
+
+		piperp->rf->getOfdmBrs(piperp->channel, conf->basic_rates, &ofdm, &psk);
+		reg |= ofdm << MAC_OFDM_BRS_SHIFT;
+		reg |= psk << MAC_PSK_BRS_SHIFT;
+		piperp->ac->wr_reg(piperp, MAC_SSID_LEN, reg, op_write);
+
+		dprintk(DVERBOSE, "BRS mask set to 0x%8.8X\n", reg);
+
+		if (ofdm == 0) {
+			/* Disable ofdm receiver if no ofdm rates supported */
+			piperp->ac->wr_reg(piperp, BB_GENERAL_STAT,
+					   ~BB_GENERAL_STAT_A_EN, op_and);
+		} else {
+			/* Enable ofdm receiver if any ofdm rates supported */
+			piperp->ac->wr_reg(piperp, BB_GENERAL_STAT,
+					   BB_GENERAL_STAT_A_EN, op_or);
+		}
+	}
+
+	/* Save new DTIM period */
+	reg = piperp->ac->rd_reg(piperp, MAC_DTIM_PERIOD) & ~MAC_DTIM_PERIOD_MASK;
+	reg |= conf->dtim_period << MAC_DTIM_PERIOD_SHIFT;
+	piperp->ac->wr_reg(piperp, MAC_DTIM_PERIOD, reg, op_write);
+	reg = piperp->ac->rd_reg(piperp, MAC_CFP_ATIM) & ~MAC_DTIM_CFP_MASK;
+	piperp->ps.beacon_int = conf->beacon_int;
+	reg |= conf->beacon_int << 16;
+	piperp->ac->wr_reg(piperp, MAC_CFP_ATIM, reg, op_write);
+}
+
+/*
+ * Use the SSL library routines to expand the AES key.
+ */
+static int piper_expand_aes_key(struct ieee80211_key_conf *key,
+				u32 *expandedKey)
+{
+	struct crypto_aes_ctx aes;
+	int ret;
+
+	dprintk(DVVERBOSE, "\n");
+
+	if (key->keylen != AES_KEYSIZE_128)
+		return -EOPNOTSUPP;
+
+	ret = crypto_aes_expand_key(&aes, key->key, key->keylen);
+	if (ret)
+		return -EOPNOTSUPP;
+
+	memcpy(expandedKey, aes.key_enc, EXPANDED_KEY_LENGTH);
+
+	return 0;
+}
+
+/*
+ * mac80211 calls this routine to set a new encryption key, or to
+ * retire an old one.  We support H/W AES encryption/decryption, so
+ * save the AES related keys.
+ */
+static int piper_hw_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			    const u8 *local_address, const u8 *address,
+			    struct ieee80211_key_conf *key)
+{
+	struct piper_priv *piperp = hw->priv;
+	int ret = 0;
+
+	dprintk(DVVERBOSE, "\n");
+
+	if ((key->alg != ALG_CCMP) || (key->keyidx >= PIPER_MAX_KEYS)) {
+		/*
+		 * If we come here, then mac80211 was trying to set a key for some
+		 * algorithm other than AES, or trying to set a key index greater
+		 * than 3.  We only support AES, and only 4 keys.
+		 */
+		ret = -EOPNOTSUPP;
+		goto set_key_error;
+	}
+	key->hw_key_idx = key->keyidx;
+
+	if (cmd == SET_KEY) {
+		ret = piper_expand_aes_key(key, piperp->key[key->keyidx].expandedKey);
+		if (ret)
+			goto set_key_error;
+
+		if (!piperp->key[key->keyidx].valid)
+			piperp->aes_key_count++;
+		piperp->key[key->keyidx].txPn = 0;
+		piperp->key[key->keyidx].rxPn = 0;
+		piperp->key[key->keyidx].valid = (ret == 0);
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+	} else {
+		/* disable key */
+		if (piperp->key[key->keyidx].valid)
+			piperp->aes_key_count--;
+		piperp->key[key->keyidx].valid = false;
+	}
+
+	if (piperp->aes_key_count > 0)
+		piperp->ac->wr_reg(piperp, MAC_CTL, ~MAC_CTL_AES_DISABLE, op_and);
+	else
+		piperp->ac->wr_reg(piperp, MAC_CTL, MAC_CTL_AES_DISABLE, op_or);
+
+set_key_error:
+	if (ret)
+		dprintk(DVERBOSE, "unable to set AES key\n");
+
+	return ret;
+}
+
+/*
+ * mac80211 calls this routine to determine if we transmitted the
+ * last beacon.  Unfortunately, we can't tell for sure.  We give
+ * mac80211 our best guess.
+ */
+static int piper_hw_tx_last_beacon(struct ieee80211_hw *hw)
+{
+	struct piper_priv *piperp = hw->priv;
+
+	dprintk(DVVERBOSE, "\n");
+	return piperp->beacon.weSentLastOne ? 1 : 0;
+}
+
+static int piper_get_tx_stats(struct ieee80211_hw *hw,
+			      struct ieee80211_tx_queue_stats *stats)
+{
+	struct piper_priv *piperp = hw->priv;
+
+	dprintk(DVVERBOSE, "\n");
+	if (stats)
+		memcpy(stats, &piperp->pstats.tx_queue, sizeof(piperp->pstats.tx_queue));
+
+	return 0;
+}
+
+static int piper_get_stats(struct ieee80211_hw *hw,
+			   struct ieee80211_low_level_stats *stats)
+{
+	struct piper_priv *piperp = hw->priv;
+
+	dprintk(DVVERBOSE, "\n");
+	if (stats)
+		memcpy(stats, &piperp->pstats.ll_stats, sizeof(piperp->pstats.ll_stats));
+
+	return 0;
+}
+
+const struct ieee80211_ops hw_ops = {
+	.tx			= piper_hw_tx,
+	.start 			= piper_hw_start,
+	.stop			= piper_hw_stop,
+	.add_interface		= piper_hw_add_intf,
+	.remove_interface	= piper_hw_rm_intf,
+	.config			= piper_config,
+	.config_interface	= piper_hw_config_intf,
+	.configure_filter 	= piper_hw_config_filter,
+	.bss_info_changed 	= piper_hw_bss_changed,
+	.tx_last_beacon		= piper_hw_tx_last_beacon,
+	.set_key		= piper_hw_set_key,
+	.get_tx_stats 		= piper_get_tx_stats,
+	.get_stats 		= piper_get_stats,
+};
+
+/*
+ * This routine is called by the probe routine to allocate the
+ * data structure we need to communicate with mac80211.
+ */
+int piper_alloc_hw(struct piper_priv **priv, size_t priv_sz)
+{
+	struct piper_priv *piperp;
+	struct ieee80211_hw *hw;
+
+	hw = ieee80211_alloc_hw(priv_sz, &hw_ops);
+	if (!hw)
+		return -ENOMEM;
+
+	hw->flags |= IEEE80211_HW_RX_INCLUDES_FCS
+		  | IEEE80211_HW_SIGNAL_DBM
+		  | IEEE80211_HW_NOISE_DBM
+		  | IEEE80211_HW_SPECTRUM_MGMT
+		  | IEEE80211_HW_NO_STACK_DYNAMIC_PS
+#if !WANT_SHORT_PREAMBLE_SUPPORT
+		  | IEEE80211_HW_2GHZ_SHORT_SLOT_INCAPABLE
+#endif
+	             /* | IEEE80211_HW_SPECTRUM_MGMT TODO:  Turn this on when we are ready*/;
+
+	hw->queues = 1;
+	hw->ampdu_queues = 0;
+	hw->extra_tx_headroom = 4 + sizeof(struct ofdm_hdr);
+	piperp = hw->priv;
+	*priv = piperp;
+	piperp->pstats.tx_queue.len = 0;
+	piperp->pstats.tx_queue.limit = 1;
+	piperp->pstats.tx_queue.count = 0;
+	piperp->areWeAssociated = false;
+	memset(&piperp->pstats.ll_stats, 0, sizeof(piperp->pstats.ll_stats));
+	piperp->hw = hw;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(piper_alloc_hw);
+
+/*
+ * This routine is called by the remove function to free the memory
+ * allocated by piper_alloc_hw.
+ */
+void piper_free_hw(struct piper_priv *piperp)
+{
+	ieee80211_free_hw(piperp->hw);
+}
+EXPORT_SYMBOL_GPL(piper_free_hw);
+
+/*
+ * This routine is called by the probe routine to register
+ * with mac80211.
+ */
+int piper_register_hw(struct piper_priv *priv, struct device *dev,
+		      struct digi_rf_ops *rf)
+{
+	struct ieee80211_hw *hw = priv->hw;
+	u8 macaddr[8];
+	u32 temp;
+	int i, ret;
+
+	dprintk(DVVERBOSE, "\n");
+
+	priv->rf = rf;
+	for (i = 0; i < rf->n_bands; i++) {
+		enum ieee80211_band b = rf->bands[i].band;
+		hw->wiphy->bands[b] = &rf->bands[i];
+	}
+	hw->wiphy->interface_modes =   BIT(NL80211_IFTYPE_ADHOC)
+	                             | BIT(NL80211_IFTYPE_STATION)
+/* TODO: Enable this             | BIT(NL80211_IFTYPE_MESH_POINT) */
+	                             ;
+	hw->channel_change_time = rf->channelChangeTime;
+	hw->vif_data_size = 0;
+	hw->sta_data_size = 0;
+	hw->max_rates = IEEE80211_TX_MAX_RATES;
+	hw->max_rate_tries = 5;             /* completely arbitrary, and apparently ignored by the rate algorithm */
+	hw->max_signal = rf->maxSignal;
+	hw->max_listen_interval = 10;       /* I don't think APs will work with values larger than 4 actually */
+
+	SET_IEEE80211_DEV(hw, dev);
+
+	temp = cpu_to_be32(priv->ac->rd_reg(priv, MAC_STA_ID0));
+	memcpy(macaddr, &temp, sizeof(temp));
+	temp = cpu_to_be32(priv->ac->rd_reg(priv, MAC_STA_ID1));
+	memcpy(&macaddr[4], &temp, sizeof(temp));
+	SET_IEEE80211_PERM_ADDR(hw, macaddr);
+
+	if ((ret = ieee80211_register_hw(hw)) != 0) {
+		dprintk(DERROR, "unable to register ieee80211 hw\n");
+		goto error;
+	}
+
+	printk(KERN_INFO PIPER_DRIVER_NAME ": registered as %s\n",
+	      	wiphy_name(hw->wiphy));
+
+	init_timer(&priv->led_timer);
+	priv->led_state = led_shutdown;
+	priv->led_timer.function = led_timer_fn;
+	priv->led_timer.data = (unsigned long) priv;
+	priv->led_timer.expires = jiffies + LED_TIMER_RATE;
+	add_timer(&priv->led_timer);
+
+error:
+	return ret;
+}
+EXPORT_SYMBOL_GPL(piper_register_hw);
+
+
+void piper_unregister_hw(struct piper_priv *piperp)
+{
+	dprintk(DVVERBOSE, "\n");
+	del_timer_sync(&piperp->led_timer);
+	piper_set_status_led(piperp->hw, led_shutdown);
+	ieee80211_unregister_hw(piperp->hw);
+}
+EXPORT_SYMBOL_GPL(piper_unregister_hw);
+
+
+MODULE_DESCRIPTION("Digi Piper WLAN core");
+MODULE_AUTHOR("contact support@digi.com for information about this code");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/digiPiper/digiPs.c b/drivers/net/wireless/digiPiper/digiPs.c
new file mode 100644
index 000000000000..c6ae6b0b656a
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/digiPs.c
@@ -0,0 +1,1164 @@
+/*
+ * digiPs.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/*
+ * This file contains the routines that are related to transmitting
+ * frames.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+
+#include "pipermain.h"
+#include "mac.h"
+#include "phy.h"
+#include "airoha.h"
+#include "digiPs.h"
+
+/*
+ * Macro converts milliseconds to HZ.
+ *
+ * TODO:  Look for standard Linux version of this.
+ */
+#define MILLS_TO_JIFFIES(x)		((((x)*HZ) + 500) / 1000)
+
+
+
+
+#define MINIMUM_SLEEP_PERIOD	(20)
+
+/*
+ * Amount of time before shutdown deadline to start the shutdown process.  This
+ * should allow enough time to get one last frame out, which will be the
+ * null-data frame notifying the AP that we are shutting down.
+ */
+#define PS_TRANSMITTER_SHUTDOWN_MS 	(10)
+
+/*
+ * Amount of time we will wake up before the next beacon.  We try to wake up before
+ * the next beacon so that we don't miss it.
+ */
+#define PS_WAKE_BEFORE_BEACON_MS	(20)
+
+/*
+ * Minimum amount of time we we keep awake for.
+ */
+#define PS_MINUMUM_POWER_UP_PERIOD_MS	(10)
+
+/*
+ * Minimum amount of time we will sleep.  If we will end up sleeping
+ * for less than this, then don't go to sleep.
+ */
+#define PS_MINIMUM_SLEEP_TIME (10)
+
+/*
+ * Length of one tick of the transmit clean up timer in ms.  This is
+ * the minimum amount of time we will sleep for.
+ */
+#define PS_TRANSMIT_TIMER_TICK_MS	((1000/HZ) ? (1000/HZ) : 1)
+
+/*
+ * Length of time we will wait past the expected arrival of a beacon before assuming
+ * that we missed it.
+ */
+#define PS_BEACON_TIMEOUT_MS		(40)
+
+
+/*
+ * Minimum beacon interval we will support for duty cycling.  There is so much overhead
+ * in duty cycling that it doesn't make sense to do it for short beacon intervals.
+ */
+#define PS_MINIMUM_BEACON_INT		(100)
+
+/*
+ * Amount of time we will pause duty cycling for after receiving an event that suggests
+ * wpa_supplicant is attempting to reassociate with an AP.
+ */
+#define PS_SCAN_DELAY				(5000)
+
+
+// Powersave register index
+#define	INDX_GEN_CONTROL    0	// General control
+#define	INDX_GEN_STATUS	    1	// General status
+#define	INDX_RSSI_AES		2	// RSSI and AES status
+#define	INDX_INTR_MASK	    3	// Interrupt mask
+#define INDX_SPI_CTRL       4	// RF SPI control
+#define INDX_CONF1          5	// Configuration 1
+#define INDX_CONF2          6	// Configuration 2
+#define	INDX_AES_MODE		7	// ARS mode
+#define INDX_OUT_CTRL       8	// Output control
+#define INDX_MAC_CONTROL    9	// MAC control
+#define INDX_STAID_1		10  // first part of stations ID
+#define INDX_STAID_2		11  // 2nd part of station ID
+#define INDX_BSSID_1		12  // 1st part of BSS ID
+#define INDX_BSSID_2		13  // 2nd part of BSS ID
+#define INDX_BRS_SSID		14  // BRS mask and SSID length
+#define INDX_BACKOFF		15	// backoff
+#define INDX_DTIM_LISTEN	16  // DTIM perido and listen interval
+#define INDX_BEACON_INT		17	// beacon interval
+#define INDX_MAC_CTL		18	// MAC control register
+#define INDX_BEACON_MASK	19	// beacon mask and backoff
+#define INDX_TOTAL          20
+
+static u32 savedRegs[INDX_TOTAL];	// Used to save registers for sleep mode
+
+
+/*
+ * TODO: Delete this.
+ */
+struct ps_stats {
+	unsigned int modeStart;
+	unsigned int cycleStart;
+	unsigned int receivedBeacons;
+	unsigned int missedBeacons;
+	unsigned int jiffiesOn;
+	unsigned int jiffiesOff;
+} stats;
+
+
+
+static void ps_free_frame(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct piper_priv *piperp = hw->priv;
+
+	if (skb) {
+		dev_kfree_skb(skb);
+		piperp->ps.frames_pending--;
+	}
+}
+
+
+
+
+
+#define ACK_SIZE		(14)	/* length of ACK in bytes */
+
+// Length (in usecs) of a MAC frame of bytes at rate (in 500kbps units)
+// not including SIFS and PLCP preamble/header
+#define	CCK_DURATION(bytes, rate)		((16*(bytes)+(rate)-1)/(rate))
+
+#define USE_SHORTPREAMBLE(is_1_Mbit, use_short_preamble)	((!is_1_Mbit) & use_short_preamble)
+
+// Length (in usecs) of SIFS and PLCP preamble/header.
+#define	PRE_LEN(is_1_Mbit, use_short_preamble)			(USE_SHORTPREAMBLE(is_1_Mbit, use_short_preamble) ? 106 : 202)
+
+// Duration (in usecs) of an OFDM frame at rate (in 500kbps units)
+// including SIFS and PLCP preamble/header
+#define	OFDM_DURATION(bytes, rate)	(36 + 4*((4*(bytes)+(rate)+10)/(rate)))
+
+static unsigned int getRateIndex(struct piper_priv *piperp)
+{
+	unsigned int rates = piperp->ac->rd_reg(piperp, MAC_SSID_LEN);
+	unsigned int result = AIROHA_LOWEST_OFDM_RATE_INDEX;
+
+	if (piperp->rf->getBand(piperp->channel) == IEEE80211_BAND_2GHZ) {
+		if ((rates & MAC_PSK_BRS_MASK) != 0) {
+			result = AIROHA_LOWEST_PSK_RATE_INDEX;
+		}
+	}
+
+	return result;
+}
+
+static int getAckDuration(struct piper_priv *piperp)
+{
+	bool is_1_Mbit = (getRateIndex(piperp) == AIROHA_LOWEST_PSK_RATE_INDEX);
+	int duration = 0;
+
+	if (is_1_Mbit) {
+		duration = CCK_DURATION(ACK_SIZE, 1);
+	} else {
+		duration = OFDM_DURATION(ACK_SIZE, 6);
+	}
+
+	duration += PRE_LEN(is_1_Mbit, piperp->use_short_preamble);
+
+	return duration;
+}
+
+
+/*
+ * This function is used to notify the AP about the current state of
+ * power save.  One of the bits in the 802.11 header field is set to
+ * indicate the status of power save.  This bit is actually set appropriately
+ * for all frames sent, we just send a null data frame just to make
+ * sure something is sent to the AP in a reasonable amount of time.
+ */
+/*
+ * Possible values for is_power_management_on argument
+ */
+#define POWERING_DOWN		(true)
+#define POWERED_UP			(false)
+void piper_sendNullDataFrame(struct piper_priv *piperp, bool is_power_management_on)
+{
+	struct sk_buff *skb = NULL;
+	_80211HeaderType *header;
+	struct ieee80211_tx_info *tx_info;
+
+	skb =
+	    __dev_alloc_skb(sizeof(_80211HeaderType) +
+			    piperp->hw->extra_tx_headroom, GFP_ATOMIC);
+	if (skb == NULL)
+		goto piper_sendNullDataFrame_Exit;
+
+	tx_info = (struct ieee80211_tx_info *) skb->cb;
+
+	skb_reserve(skb, piperp->hw->extra_tx_headroom);
+	header = (_80211HeaderType *) skb_put(skb, sizeof(_80211HeaderType));
+	memset(header, 0, sizeof(*header));
+	header->fc.type = TYPE_NULL_DATA;
+	header->fc.pwrMgt = is_power_management_on;
+	header->duration = getAckDuration(piperp);
+	memcpy(header->addr1, piperp->bssid, sizeof(header->addr1));
+	memcpy(header->addr2, piperp->pdata->macaddr, sizeof(header->addr2));
+	memcpy(header->addr3, piperp->bssid, sizeof(header->addr3));
+
+	tx_info->flags = IEEE80211_TX_CTL_ASSIGN_SEQ | IEEE80211_TX_CTL_FIRST_FRAGMENT;
+	tx_info->band = piperp->rf->getBand(piperp->channel);
+	tx_info->antenna_sel_tx = 1;	/* actually this is ignored for now */
+	tx_info->control.rates[0].idx = 0;
+	tx_info->control.rates[0].count = 2;	/* no retries.  Don't tie us up waiting for an ACK */
+	tx_info->control.rates[0].flags = 0;
+	tx_info->control.rates[1].idx = -1;
+	tx_info->control.rts_cts_rate_idx = -1;
+	piperp->ps.frames_pending++;
+
+	if (piper_hw_tx_private(piperp->hw, skb, ps_free_frame) != 0) {
+		/* printk(KERN_ERR
+		       "piper_hw_tx() failed unexpectedly when sending null data frame.\n"); */
+		ps_free_frame(piperp->hw, skb);
+	}
+
+piper_sendNullDataFrame_Exit:
+	return;
+}
+
+
+
+
+#define RESET_PIPER		(1)				/* must be set to 1 (0 case is only for debugging)*/
+#define PS_DONT_FORCE		(false)		/* set force to this value if we want to be safe */
+#define PS_FORCE_POWER_DOWN	(true)		/* set force to this value to shut down the H/W reguardless*/
+/*
+ * This routine shuts down Piper and the Airoha transceiver.  First we check to
+ * make sure the driver and H/W are idle.  Then we save the state of the H/W.
+ * Then we shut down the Airoha and place piper into reset.
+ */
+int piper_MacEnterSleepMode(struct piper_priv *piperp, bool force)
+{
+	/*
+	 * Interrupts are already disabled when we get here.
+	 */
+
+	if (piperp->ps.poweredDown)
+		return 0;
+
+	savedRegs[INDX_INTR_MASK] = piperp->ac->rd_reg(piperp, BB_IRQ_MASK);
+	piperp->ac->wr_reg(piperp, BB_IRQ_MASK, 0, op_write);
+
+    if (!force) {
+    	if (   (piperp->ps.rxTaskletRunning)
+    	    || ((piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_BUSY))
+    		|| (  (piperp->ac->rd_reg(piperp, BB_GENERAL_CTL)
+    	     	& BB_GENERAL_CTL_TX_FIFO_EMPTY) == 0)
+			|| (piperp->tx_tasklet_running)
+    		|| (  (piperp->ac->rd_reg(piperp, BB_GENERAL_STAT)
+    			& BB_GENERAL_STAT_RX_FIFO_EMPTY) == 0)
+    		|| (piperp->ac->rd_reg(piperp, BB_IRQ_STAT) & savedRegs[INDX_INTR_MASK])) {
+
+			piperp->ac->wr_reg(piperp, BB_IRQ_MASK, savedRegs[INDX_INTR_MASK], op_write);
+    		return -1;
+		}
+    }
+
+	disable_irq(piperp->irq);
+
+	savedRegs[INDX_GEN_CONTROL] = piperp->ac->rd_reg(piperp, BB_GENERAL_CTL);
+	savedRegs[INDX_GEN_STATUS] = piperp->ac->rd_reg(piperp, BB_GENERAL_STAT);
+	savedRegs[INDX_RSSI_AES] = piperp->ac->rd_reg(piperp, BB_RSSI) & ~BB_RSSI_EAS_BUSY;
+	savedRegs[INDX_SPI_CTRL] = piperp->ac->rd_reg(piperp, BB_SPI_CTRL);
+	savedRegs[INDX_CONF1] = piperp->ac->rd_reg(piperp, BB_TRACK_CONTROL);
+	savedRegs[INDX_CONF2] = piperp->ac->rd_reg(piperp, BB_CONF_2);
+	savedRegs[INDX_OUT_CTRL] = piperp->ac->rd_reg(piperp, BB_OUTPUT_CONTROL);
+	savedRegs[INDX_MAC_CONTROL] = piperp->ac->rd_reg(piperp, MAC_CTL);
+
+	savedRegs[INDX_STAID_1]		= piperp->ac->rd_reg(piperp, MAC_STA_ID0);
+	savedRegs[INDX_STAID_2]		= piperp->ac->rd_reg(piperp, MAC_STA_ID1);
+	savedRegs[INDX_BSSID_1]		= piperp->ac->rd_reg(piperp, MAC_BSS_ID0);
+	savedRegs[INDX_BSSID_2]		= piperp->ac->rd_reg(piperp, MAC_BSS_ID1);
+	savedRegs[INDX_BRS_SSID]	= piperp->ac->rd_reg(piperp, MAC_SSID_LEN);
+	savedRegs[INDX_BACKOFF]		= piperp->ac->rd_reg(piperp, MAC_BACKOFF);
+	savedRegs[INDX_DTIM_LISTEN]	= piperp->ac->rd_reg(piperp, MAC_DTIM_PERIOD);
+	savedRegs[INDX_BEACON_INT]	= piperp->ac->rd_reg(piperp, MAC_CFP_ATIM);
+	savedRegs[INDX_MAC_CTL]		= piperp->ac->rd_reg(piperp, MAC_CTL);
+	savedRegs[INDX_BEACON_MASK]	= piperp->ac->rd_reg(piperp, MAC_BEACON_FILT);
+
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, ~BB_GENERAL_CTL_RX_EN, op_and);	//disable receiving
+	piperp->ac->wr_reg(piperp, MAC_CTL, 0, op_write);
+	piperp->ac->wr_reg(piperp, BB_IRQ_MASK, 0, op_write);
+
+#if RESET_PIPER
+    // Power down the airoha transceiver
+	piperp->rf->power_on(piperp->hw, false);
+	udelay(10);
+	// hold the transceiver in reset mode
+	if (piperp->pdata->reset)
+		piperp->pdata->reset(piperp, 1);
+#endif
+	stats.jiffiesOn += jiffies - stats.cycleStart;
+	stats.cycleStart = jiffies;
+	piperp->ps.poweredDown = true;
+
+	return 0;
+}
+
+
+#define PS_NO_SPIKE_SUPPRESSION		(false)		/* want_spike_suppression = don't want spike suppression*/
+#define PS_WANT_SPIKE_SUPPRESSION	(true)		/* want_spike_suppression = do spike suppression*/
+/*
+ * Turn the H/W back on after it was shutdown with piper_MacEnterSleepMode.
+ *
+ *	1. Power up the hardware.
+ *  2. Perform the spike suppression routine if desired.  The spike suppression
+ *     routine resyncs the clocks in Piper in order to prevent us from generating
+ *     noise spikes at 1 and 2 MHz.  Unfortunately it takes an indeterminate amount
+ *     of time, so we normally don't do it and just make sure we do not send at
+ *	   at those data rates while duty cycling.
+ * 3.  Set the channel.  The Airoha was shut off so we have to set the channel
+ *     again.
+ * 4.  Restore the state of piper registers.
+ * 5.  Zero out and reset the transmitter FIFO.  Mike Schaffner claims this should
+ *     not be necessary, but we seem to run into trouble when we don't do it.
+ * 6.  Restore the interrupts.
+ */
+void piper_MacEnterActiveMode(struct piper_priv *piperp, bool want_spike_suppression)
+{
+	int i;
+// #define WANT_DEBUG
+#ifdef WANT_DEBUG
+	static unsigned int run = 0;
+#endif
+
+#if RESET_PIPER
+
+	if (piperp->pdata->reset) {
+#ifdef WANT_DEBUG
+		if (piperp->ac->rd_reg(piperp, BB_GENERAL_CTL) & BB_GENERAL_CTL_TX_FIFO_FULL) {
+			printk(KERN_ERR "**** While in reset, run = %d\n", run);
+			digiWifiDumpRegisters(piperp, MAIN_REGS);
+			while(1);
+		}
+		if (piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_BUSY) {
+			printk(KERN_ERR "**** While in reset AES busy set, run = %d\n", run);
+			digiWifiDumpRegisters(piperp, MAIN_REGS);
+			while(1);
+		}
+#endif
+	    piperp->pdata->reset(piperp, 0);
+	    udelay(10);
+
+#ifdef WANT_DEBUG
+		if (piperp->ac->rd_reg(piperp, BB_GENERAL_CTL) & BB_GENERAL_CTL_TX_FIFO_FULL) {
+			printk(KERN_ERR "**** After reset, run = %d\n", run);
+			digiWifiDumpRegisters(piperp, MAIN_REGS);
+			while(1);
+		}
+		if (piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_BUSY) {
+			printk(KERN_ERR "**** After reset AES busy set, run = %d\n", run);
+			digiWifiDumpRegisters(piperp, MAIN_REGS);
+			while(1);
+		}
+		run++;
+#endif
+	    piperp->rf->power_on(piperp->hw, true);
+	    mdelay(1);
+	    piper_spike_suppression(piperp, want_spike_suppression);
+	}
+#endif
+
+	piperp->ac->wr_reg(piperp, BB_IRQ_STAT, 0xff, op_write);
+
+
+#if RESET_PIPER
+	piperp->rf->set_chan_no_rx(piperp->hw, piperp->channel);
+#endif
+
+      // store the registers back
+
+	piperp->ac->wr_reg(piperp, BB_GENERAL_STAT, 0x30000000, op_write);
+	piperp->ac->wr_reg(piperp, BB_RSSI, savedRegs[INDX_RSSI_AES] & ~BB_RSSI_EAS_BUSY, op_write);
+
+//	piperp->ac->wr_reg(piperp, BB_IRQ_MASK, savedRegs[INDX_INTR_MASK], op_write);
+	piperp->ac->wr_reg(piperp, BB_SPI_CTRL, savedRegs[INDX_SPI_CTRL], op_write);
+	piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, savedRegs[INDX_CONF1], op_write);
+	piperp->ac->wr_reg(piperp, BB_CONF_2, savedRegs[INDX_CONF2], op_write);
+	piperp->ac->wr_reg(piperp, BB_AES_CTL, 0, op_write);
+	piperp->ac->wr_reg(piperp, BB_OUTPUT_CONTROL, savedRegs[INDX_OUT_CTRL], op_write);
+	piperp->ac->wr_reg(piperp, MAC_CTL, savedRegs[INDX_MAC_CONTROL], op_write);
+
+	piperp->ac->wr_reg(piperp, MAC_STA_ID0,     savedRegs[INDX_STAID_1],          op_write);
+	piperp->ac->wr_reg(piperp, MAC_STA_ID1,     savedRegs[INDX_STAID_2],          op_write);
+	piperp->ac->wr_reg(piperp, MAC_BSS_ID0,     savedRegs[INDX_BSSID_1],          op_write);
+	piperp->ac->wr_reg(piperp, MAC_BSS_ID1,     savedRegs[INDX_BSSID_2],          op_write);
+	piperp->ac->wr_reg(piperp, MAC_SSID_LEN,    savedRegs[INDX_BRS_SSID],       op_write);
+	piperp->ac->wr_reg(piperp, MAC_BACKOFF,     savedRegs[INDX_BACKOFF],          op_write);
+	piperp->ac->wr_reg(piperp, MAC_DTIM_PERIOD, savedRegs[INDX_DTIM_LISTEN],  op_write);
+	piperp->ac->wr_reg(piperp, MAC_CFP_ATIM,    savedRegs[INDX_BEACON_INT], op_write);
+	piperp->ac->wr_reg(piperp, MAC_CTL,         savedRegs[INDX_MAC_CTL],          op_write);
+	piperp->ac->wr_reg(piperp, MAC_BEACON_FILT, savedRegs[INDX_BEACON_MASK],  op_write);
+
+//****
+	// set bit-11 in the general control register to a 1 to start the processors
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+			   BB_GENERAL_CTL_MAC_ASSIST_ENABLE, op_or);
+
+	// set the TX-hold bit
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, 0x37720080, op_write);
+
+	// clear the TX-FIFO memory
+	for (i = 0; i < 448; i++)
+		piperp->ac->wr_reg(piperp, BB_DATA_FIFO, 0, op_write);
+
+	// clear RX-FIFO memory
+	for (i = 0; i < 512; i++)
+		piperp->ac->rd_reg(piperp, BB_DATA_FIFO);
+
+	// reset the TX-FIFO and RX-FIFO
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, 0x377200E0, op_write);
+
+
+	// release the TX-hold and reset
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, 0x37720000, op_write);
+
+
+//***
+	/*
+	 * Reset the interrupt mask.  We could have been receiving a frame when we
+	 * powered down.  This could cause us to store the wrong mask, so we want
+	 * to make sure we enabe the RX interrupts.  However, we should not have the
+	 * TX interrupts enabled when we come out of power save mode.
+	 */
+	udelay(50);
+	piperp->ac->wr_reg(piperp, BB_IRQ_STAT, 0xff, op_write);
+	piperp->clear_irq_mask_bit(piperp, 0xffffffff);
+	piperp->set_irq_mask_bit(piperp, BB_IRQ_MASK_RX_OVERRUN | BB_IRQ_MASK_RX_FIFO);
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+	             (savedRegs[INDX_GEN_CONTROL] | 0x37000000 |
+	              BB_GENERAL_CTL_RX_EN), op_write);
+
+	stats.jiffiesOff += jiffies - stats.cycleStart;
+	stats.cycleStart = jiffies;
+
+	/* TODO, this is a temporary workaround and should be better analyzed in future.
+	 * The problem is that the general power save code is not synchronized with the
+	 * dynamic PW and this is causing that, the following line, unbalances the
+	 * piper wireless irq */
+	if (piperp->ps.poweredDown != false)
+		enable_irq(piperp->irq);
+
+	piperp->ps.poweredDown = false;
+}
+
+
+
+
+/*
+ * So what crazy thing are we doing here?  Well, Piper has a bug where it
+ * can send noise spikes at 1 Mbps and 2 Mbps if it is powered up without
+ * running a special spike suppression routine.  The spike suppression code
+ * takes an average of 30 ms, and I have timed it taking as long as 300 ms.
+ * This is not something you want to use for duty cycling.  The solution is
+ * to avoid sending at those two rates.  After the transmit routine determines
+ * the rate mac80211 specified, it will call us and we will decide whether
+ * we like that rate.  If it is one of our bad rates, then we will bump it
+ * up to a good rate.
+ */
+struct ieee80211_rate *piper_ps_check_rate(struct piper_priv *piperp,
+					   struct ieee80211_rate *rate)
+{
+#define BAD_RATE_1MBPS		(10)
+#define BAD_RATE_2MBPS		(20)
+	if ((piperp->ps.mode == PS_MODE_LOW_POWER) && (rate != NULL)) {
+		if ((rate->bitrate == BAD_RATE_1MBPS)
+		    || (rate->bitrate == BAD_RATE_2MBPS)) {
+			rate =
+			    (struct ieee80211_rate *) piperp->rf->
+			    getRate(AIROHA_55_MBPS_RATE_INDEX);
+		}
+	}
+
+	return rate;
+}
+
+EXPORT_SYMBOL_GPL(piper_ps_check_rate);
+
+
+
+/*
+ * This routine restarts the transmitter after powering back
+ * up, or failing to power down.
+ *
+ * 	1) Clear the power management bit so that frames will be
+ *     sent indicating that we are poweredup.
+ *  2) Set the flag to allow transmits again.
+ *  3) If we stopped the mac80211 transmitter queue, then start
+ *     it back up again.
+ *  4) Notify the AP that we are awake by sending a null-data frame
+ *     with the power management bit clear.
+ */
+static void ps_resume_transmits(struct piper_priv *piperp)
+{
+	piperp->ps.power_management = POWERED_UP;
+ 	piperp->ps.allowTransmits = true;
+	piperp->ps.stopped_tx_queues = false;
+ 	piper_sendNullDataFrame(piperp, POWERED_UP);
+	ieee80211_wake_queues(piperp->hw);
+}
+
+
+
+
+
+
+/*
+ * This routine sets an event timer.  The ps_state_machine routine
+ * will be executed when the event timer expires.
+ */
+static void ps_set_timer_event(struct piper_priv *piperp,
+								enum piper_ps_event next_event,
+								unsigned int timeout_ms)
+{
+	unsigned int delay_in_jiffies = MILLS_TO_JIFFIES(timeout_ms);
+
+	if (delay_in_jiffies == 0)
+		delay_in_jiffies++;
+
+	del_timer_sync(&piperp->ps.timer);
+	piperp->ps.this_event = next_event;
+	piperp->ps.timer.expires = delay_in_jiffies + jiffies;
+	add_timer(&piperp->ps.timer);
+}
+
+
+/*
+ * This routine cancels an event timer set by ps_set_timer_event.
+ */
+static void ps_cancel_timer_event(struct piper_priv *piperp)
+{
+	del_timer_sync(&piperp->ps.timer);
+}
+
+
+#define WANT_STATE_MACHINE_DEBUG		(0)
+#if WANT_STATE_MACHINE_DEBUG
+
+struct event_record {
+	enum piper_ps_event event;
+	enum piper_ps_state state;
+};
+
+#define MAX_EVENTS_RECORDED		(15)
+static unsigned int debug_events_index = 0;
+static struct event_record debug_events[MAX_EVENTS_RECORDED];
+
+void debug_track_event(enum piper_ps_event event, enum piper_ps_state state)
+{
+	debug_events[debug_events_index].event = event;
+	debug_events[debug_events_index].state = state;
+	if (debug_events_index == MAX_EVENTS_RECORDED) {
+		unsigned int i;
+
+		printk(KERN_ERR);
+		for (i = 0; i < MAX_EVENTS_RECORDED; i++) {
+			printk("(%d,%d)", debug_events[i].event,
+					debug_events[i].state);
+		}
+		printk("\n");
+		debug_events_index = 0;
+	} else {
+		debug_events_index++;
+	}
+}
+
+
+#else
+	#define debug_track_event(event, state)
+#endif
+
+/*
+ * This is the entry point into the duty cycle state machine.  It may be called
+ * by:
+ *
+ *		1) piper_ps_handle_beacon when a beacon frame is received.
+ *		2) the receiver task when we receive the ACK for the last pending frame
+ *		   when we are waiting to power down.
+ *      3) by ps_timer for many timer events.
+ */
+static void ps_state_machine(struct piper_priv *piperp, enum piper_ps_event event)
+{
+	unsigned long flags;
+
+	debug_track_event(event, piperp->ps.state);
+
+	spin_lock_irqsave(&piperp->ps.lock, flags);
+
+
+	switch (event) {
+		case PS_EVENT_BEACON_RECEIVED:
+			/*
+			 * We just received a beacon.  This is really the driving event in this state
+			 * machine.  Everything else is synchronized from it.
+			 *
+			 * We know we are powered up because we just received the beacon.  The normal
+			 * control path is to set a timer which will expire when we need to start
+			 * preparations for powering down again.
+			 */
+			ps_cancel_timer_event(piperp);				/* cancel missed beacon timer*/
+			stats.receivedBeacons++;
+			if (   (!piperp->areWeAssociated)
+			    || (piperp->ps.beacon_int <  PS_MINIMUM_BEACON_INT)
+			    || (piperp->ps.scan_timer != 0)) {
+				/*
+				 * Don't duty cyle while we are trying to associate.
+				 */
+				piperp->ps.state = PS_STATE_WAIT_FOR_BEACON;
+				if (piperp->ps.scan_timer) {
+					piperp->ps.scan_timer--;
+				}
+				break;
+			}
+			if (piperp->ps.state == PS_STATE_WAIT_FOR_BEACON) {
+				int timeout;
+				/*
+				 * Calculate amount of time to sleep.
+				 */
+				piperp->ps.sleep_time = (piperp->ps.beacon_int * (100 - piperp->power_duty)) / 100;
+
+				/*
+				 * Now figure out how long we have before it's time to go to sleep.  We
+				 * have to wake up at least PS_WAKE_BEFORE_BEACON_MS before we expect to
+				 * receive the next beacon, and we need to start powering down at least
+				 * PS_TRANSMITTER_SHUTDOWN_MS ahead of time.
+				 */
+				timeout = piperp->ps.beacon_int - (piperp->ps.sleep_time + PS_TRANSMITTER_SHUTDOWN_MS + PS_WAKE_BEFORE_BEACON_MS);
+				if ((timeout > 0) && (piperp->ps.sleep_time > PS_MINIMUM_SLEEP_TIME)) {
+					/*
+					 * If there is enough time left that it makes sense to duty
+					 * cycle, then program the timer and advance to the next state.
+					 */
+					piperp->ps.state = PS_STATE_WAIT_FOR_STOP_TRANSMIT_EVENT;
+					ps_set_timer_event(piperp, PS_EVENT_STOP_TRANSMIT_TIMER_EXPIRED, timeout);
+
+					break;
+				}
+			} else {
+				 if (   (piperp->ps.state == PS_STATE_WAIT_FOR_TRANSMITTER_DONE)
+				 	 || (piperp->ps.state == PS_STATE_WAIT_FOR_TRANSMITTER_DONE_EVENT)) {
+					ps_resume_transmits(piperp);
+				}
+				digi_dbg("*** Beacon event in state %d.\n", piperp->ps.state);
+			}
+			/*
+			 * We will come here if we were in the wrong state when we received the
+			 * beacon, or if the duty cycle is too short.
+			 */
+			piperp->ps.state = PS_STATE_WAIT_FOR_BEACON;
+			ps_set_timer_event(piperp, PS_EVENT_MISSED_BEACON,
+						  piperp->ps.beacon_int + PS_BEACON_TIMEOUT_MS);
+			break;
+		case PS_EVENT_STOP_TRANSMIT_TIMER_EXPIRED:
+			/*
+			 * This event is hit when it's time to start powering down.  Unfortunately, this is
+			 * not a simple thing to do.  The first things to do are:
+			 *
+			 *  1. Set the power save on flag.  This will cause any transmit frame to be
+			 * 	   sent with the power management bit set.
+			 *  2. Stop the mac80211 layer from sending us anymore frames.
+			 *  3. Signal the AP that we are powering down by sending a null-data frame with the
+			 * 	   power management bit set.
+			 */
+			 if (piperp->ps.state == PS_STATE_WAIT_FOR_STOP_TRANSMIT_EVENT) {
+			 	if (piperp->ps.scan_timer) {
+			 		/*
+			 		 * Don't shut down if we are scanning.
+			 		 */
+					piperp->ps.state = PS_STATE_WAIT_FOR_BEACON;
+					break;
+				}
+			 	piperp->ps.power_management = POWERING_DOWN;
+			 	piperp->ps.allowTransmits = false;
+			 	ieee80211_stop_queues(piperp->hw);
+			 	piperp->ps.stopped_tx_queues = true;
+			 	piper_sendNullDataFrame(piperp, POWERING_DOWN);
+			 	piperp->ps.state = PS_STATE_WAIT_FOR_TRANSMITTER_DONE;
+				ps_set_timer_event(piperp, PS_EVENT_TRANSMITTER_DONE_TIMER_TICK,
+							PS_TRANSMIT_TIMER_TICK_MS);
+			} else {
+				/*
+				 * This should never happen (famous last words)
+				 */
+				digi_dbg("** stop tx event, state = %d.\n", piperp->ps.state);
+				piperp->ps.state = PS_STATE_WAIT_FOR_BEACON;
+				ps_set_timer_event(piperp, PS_EVENT_MISSED_BEACON,
+							piperp->ps.beacon_int + PS_BEACON_TIMEOUT_MS);
+			}
+			break;
+		case PS_EVENT_TRANSMITTER_DONE:
+			/*
+			 * This event is triggered when the receive task finishes processing the ACK
+			 * from the null-data frame sent by the PS_EVENT_STOP_TRANSMIT_TIMER_EXPIRED event.
+			 * We try to power down now.
+			 */
+			 if (piperp->ps.scan_timer == 0) {
+				 if (   (piperp->ps.state == PS_STATE_WAIT_FOR_TRANSMITTER_DONE)
+				 	 || (piperp->ps.state == PS_STATE_WAIT_FOR_TRANSMITTER_DONE_EVENT)) {
+					ps_cancel_timer_event(piperp);				/* cancel transmitter done timeout timer*/
+				 	if (piper_MacEnterSleepMode(piperp, PS_DONT_FORCE) == 0) {
+				 		piperp->ps.state = PS_STATE_WAIT_FOR_WAKEUP_ALARM;
+				 		/*
+				 		 * Note that the value PS_EVENT_TRANSMITTER_DONE_TIMER_TICK is
+				 		 * updated as necessary by the PS_EVENT_TRANSMITTER_DONE_TIMER_TICK
+				 		 * event to take into account the amount of time it took for the
+				 		 * transmitter to finish sending the last frame.
+				 		 */
+				 		ps_set_timer_event(piperp, PS_EVENT_WAKEUP, piperp->ps.sleep_time);
+				 		break;
+					}
+				 } else {
+#ifdef WANT_DEBUG
+		printk(KERN_ERR "couldn't sleep, rxt=%d, AES busy = %d, txfifo=%d, txt=%d, rxfifo=%d\n",
+				(piperp->ps.rxTaskletRunning),((piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_BUSY) != 0),
+				(  (piperp->ac->rd_reg(piperp, BB_GENERAL_CTL)& BB_GENERAL_CTL_TX_FIFO_EMPTY) == 0),
+				(piperp->tx_tasklet_running),
+    	     	(  (piperp->ac->rd_reg(piperp, BB_GENERAL_STAT)
+    			& BB_GENERAL_STAT_RX_FIFO_EMPTY) == 0));
+#endif
+				 	digi_dbg("** PS_EVENT_TRANSMITTER_DONE event, but state == %d.\n", piperp->ps.state);
+				}
+			}
+			 /*
+			  * If we fall through to here, then either we are in the wrong state, or we were
+			  * not able to power down the H/W.
+			  */
+			ps_resume_transmits(piperp);
+			piperp->ps.state = PS_STATE_WAIT_FOR_BEACON;
+			ps_set_timer_event(piperp, PS_EVENT_MISSED_BEACON,
+						 piperp->ps.beacon_int + PS_BEACON_TIMEOUT_MS);
+			break;
+		case PS_EVENT_TRANSMITTER_DONE_TIMER_TICK:
+			/*
+			 * This event is triggered periodically while we are waiting for the
+			 * transmitter to finish sending that last packet.  We decrement
+			 * piperp->ps.sleep_time (which is used by the PS_EVENT_TRANSMITTER_DONE
+			 * event).  If piperp->ps.sleep_time is still larger than our minimum
+			 * required sleep time, then we just restart the timer.
+			 */
+			if (piperp->ps.state == PS_STATE_WAIT_FOR_TRANSMITTER_DONE) {
+				piperp->ps.sleep_time -= PS_TRANSMIT_TIMER_TICK_MS;
+				if (piperp->ps.sleep_time >= PS_MINIMUM_SLEEP_TIME) {
+				 	piperp->ps.state = PS_STATE_WAIT_FOR_TRANSMITTER_DONE;
+					ps_set_timer_event(piperp, PS_EVENT_TRANSMITTER_DONE_TIMER_TICK,
+								PS_TRANSMIT_TIMER_TICK_MS);
+				} else {
+					/*
+					 * Transmitter did not shut down in time.  Resume normal operations
+					 * and stay awake until the next beacon.
+					 */
+					ps_resume_transmits(piperp);
+				 	piperp->ps.state = PS_STATE_WAIT_FOR_BEACON;
+					ps_set_timer_event(piperp, PS_EVENT_MISSED_BEACON,
+								 piperp->ps.sleep_time + PS_WAKE_BEFORE_BEACON_MS
+								 + PS_BEACON_TIMEOUT_MS);
+				}
+			} else if (piperp->ps.state == PS_STATE_WAIT_FOR_TRANSMITTER_DONE_EVENT) {
+				piperp->ps.sleep_time -= PS_TRANSMIT_TIMER_TICK_MS;
+				/*
+				 * The piper_ps_rx_task_exiting routine sets this state just before it
+				 * releases the lock on ps.state and calls this event.  If we ever
+				 * come here, then the timer tick occurred just between the time
+				 * piper_ps_rx_task_exiting released the lock and ps_state_machine
+				 * reset it.  Since the tx done event is in progress, we should ignore
+				 * this tick.
+				 */
+				break;
+			} else {
+				digi_dbg("** done tick in state %d.\n", piperp->ps.state);
+			}
+			break;
+		case PS_EVENT_WAKEUP:
+			/*
+			 * This event is called when we have powered down and it is time
+			 * to power back up again.
+			 *
+			 *	1) Power up the H/W.
+			 *	2) Resume normal operations
+			 *  3) Update our state.
+			 *  4) Set a timeout for receiving the next beacom frame.
+			 */
+			if (piperp->ps.state == PS_STATE_WAIT_FOR_WAKEUP_ALARM) {
+				piper_MacEnterActiveMode(piperp, PS_NO_SPIKE_SUPPRESSION);
+				ps_resume_transmits(piperp);
+			 	piperp->ps.state = PS_STATE_WAIT_FOR_BEACON;
+				ps_set_timer_event(piperp, PS_EVENT_MISSED_BEACON,
+							 PS_BEACON_TIMEOUT_MS + PS_WAKE_BEFORE_BEACON_MS);
+			} else {
+				digi_dbg("** wake event in state %d.\n", piperp->ps.state);
+			}
+			break;
+		case PS_EVENT_MISSED_BEACON:
+			/*
+			 * This event is called when we miss a beacon.  For now just update
+			 * our statistics.
+			 */
+		 	piperp->ps.state = PS_STATE_WAIT_FOR_BEACON;
+		 	if ((piperp->areWeAssociated)  && (piperp->ps.scan_timer == 0)) {
+				stats.missedBeacons++;
+				ps_set_timer_event(piperp, PS_EVENT_MISSED_BEACON, piperp->ps.beacon_int);
+			}
+			break;
+		default:
+			digi_dbg("**** ps_state_machine received unknown event %d.\n", event);
+			break;
+	}
+	spin_unlock_irqrestore(&piperp->ps.lock, flags);
+
+}
+
+/*
+ * This routine is called by the receiver task when it exits.  We use it to generate
+ * the PS_EVENT_TRANSMITTER_DONE event.  The event signifies that the transmitter has
+ * sent our null-data frame and is idle.  We determine this by checking frames_pending,
+ * while will be nonzero if a null-data frame is waiting to be sent, and the machine's
+ * state.
+ */
+void piper_ps_rx_task_exiting(struct piper_priv *piperp)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&piperp->ps.lock, flags);
+
+	if (   (piperp->ps.frames_pending == 0)
+		&& (piperp->ps.state == PS_STATE_WAIT_FOR_TRANSMITTER_DONE)) {
+		/*
+		 * We have a race condition between the transmitter done tick and
+		 * this routine.  So this routine changes the state to
+		 * PS_STATE_WAIT_FOR_TRANSMITTER_DONE_EVENT before it releases the
+		 * lock so that we don't get confused.
+		 */
+		piperp->ps.state = PS_STATE_WAIT_FOR_TRANSMITTER_DONE_EVENT;
+		spin_unlock_irqrestore(&piperp->ps.lock, flags);
+		ps_state_machine(piperp, PS_EVENT_TRANSMITTER_DONE);
+	} else {
+		spin_unlock_irqrestore(&piperp->ps.lock, flags);
+	}
+}
+
+/*
+ * Called when the event timer expires.  Call the state machine to handle
+ * the event.
+ */
+static void ps_timer(unsigned long context)
+{
+	struct piper_priv *piperp = (struct piper_priv *) context;
+
+	ps_state_machine(piperp, piperp->ps.this_event);
+}
+
+
+
+/*
+ * This routine is called when we receive a beacon.  We extract the beacon interval
+ * in case it has changed and then call the state machine.
+ */
+static void piper_ps_handle_beacon(struct piper_priv *piperp, struct sk_buff *skb)
+{
+#define BEACON_INT_LSB					(8)
+#define BEACON_INT_MSB					(9)
+	u32 beacon_int;
+	_80211HeaderType *header = (_80211HeaderType *) skb->data;
+	bool fromOurAp = piperp->areWeAssociated
+					&& (memcmp(piperp->bssid, header->addr3, sizeof (header->addr3)) == 0);
+
+	/*
+	 * mac80211 does not inform us when the beacon interval changes, so we have
+	 * to read this information from the beacon ourselves.
+	 */
+
+	if (fromOurAp) {
+		beacon_int = skb->data[sizeof(_80211HeaderType) + BEACON_INT_LSB];
+		beacon_int |= (skb->data[sizeof(_80211HeaderType) + BEACON_INT_MSB] << 8);
+		piperp->ps.beacon_int = beacon_int;
+
+		if (piperp->ps.mode == PS_MODE_LOW_POWER) {
+			ps_state_machine(piperp, PS_EVENT_BEACON_RECEIVED);
+		}
+	}
+}
+
+
+
+/*
+ * This routine is called when mac80211 starts doing things that might indicate it
+ * is attempting to scan or reassociate.  Things like changing the channel or
+ * disassociating.  When we receive an event like that, we stop duty cycling for
+ * a while since it may interfere with attempts to reassociate with an access point.
+ */
+void piper_ps_scan_event(struct piper_priv *piperp)
+{
+	(void) piperp;
+#if 0
+	/*
+	 * It appears that pausing duty cycling during association events may actually
+	 * worsen performance I suspect that either the AP or mac80211 is measuring
+	 * our throughput and adjusting the load accordingly, and that momentary changes
+	 * in performance caused by pausing duty cyling interfere with this.
+	 *
+	 * TODO:  Consider removing this code.  I left it in for now in case we decide
+	 *        to try it again, but if we're not going to use it, it just makes the
+	 *        driver more confusing and should be removed.
+	 */
+	if (piperp->ps.beacon_int != 0) {
+		piperp->ps.scan_timer = PS_SCAN_DELAY / piperp->ps.beacon_int;
+	} else {
+		piperp->ps.scan_timer = PS_SCAN_DELAY / 100;
+	}
+#endif
+}
+
+
+
+/*
+ * This routine is called so we can process incoming frames.  We do the
+ * handshaking to receive buffered frames in PS mode here.
+ */
+void piper_ps_process_receive_frame(struct piper_priv *piperp, struct sk_buff *skb)
+{
+	_80211HeaderType *header = (_80211HeaderType *) skb->data;
+
+	if (header->fc.type == TYPE_BEACON) {
+		piper_ps_handle_beacon(piperp, skb);
+	} else if (   (header->fc.type == TYPE_ASSOC_RESP)
+			   || (header->fc.type == TYPE_REASSOC_RESP)
+			   || (header->fc.type == TYPE_PROBE_RESP)
+			   || (header->fc.type == TYPE_DISASSOC)
+			   || (header->fc.type == TYPE_DEAUTH)
+			   || (header->fc.type == TYPE_ACTION)) {
+		piper_ps_scan_event(piperp);
+	}
+}
+
+EXPORT_SYMBOL_GPL(piper_ps_process_receive_frame);
+
+
+
+/*
+ * This function turns power save mode on or off.
+ */
+void piper_ps_set(struct piper_priv *piperp, bool powerSaveOn)
+{
+#define MAX_SHUTDOWN_TIMEOUT		(100)
+	unsigned long flags;
+
+	spin_lock_irqsave(&piperp->ps.lock, flags);
+
+	piper_ps_scan_event(piperp);
+	if (powerSaveOn) {
+		if (piperp->ps.beacon_int >= PS_MINIMUM_BEACON_INT) {
+			if (piperp->ps.mode != PS_MODE_LOW_POWER) {
+			    piperp->ps.aid = 0;
+			    piperp->ps.mode = PS_MODE_LOW_POWER;
+			    piperp->ps.state= PS_STATE_WAIT_FOR_BEACON;
+			    piperp->ps.power_management = POWERED_UP;
+			    piperp->ps.poweredDown = false;
+			    piperp->ps.allowTransmits = true;
+			    piperp->ps.stopped_tx_queues = false;
+			    stats.receivedBeacons = 0;
+			    stats.missedBeacons = 0;
+				stats.modeStart = jiffies;
+				stats.cycleStart = jiffies;
+				stats.jiffiesOff = 0;
+				stats.jiffiesOn = 0;
+				piper_sendNullDataFrame(piperp, POWERED_UP);
+				/*
+				 * Will start it the next time we receive a beacon.
+				 */
+			}
+		} else {
+			printk(KERN_ERR
+			       "\nUnable to set power save mode because the beacon \n"
+			       "interval set on this access point less than 100ms.\n");
+		}
+	} else {
+		ps_cancel_timer_event(piperp);
+		if (piperp->ps.mode == PS_MODE_LOW_POWER) {
+			piperp->ps.mode = PS_MODE_FULL_POWER;		/* stop duty cycle timer */
+			if (piperp->ps.poweredDown) {
+				/*
+				 * If we were powered down, then power up and do the spike suppression.
+				 */
+				piper_MacEnterActiveMode(piperp, PS_WANT_SPIKE_SUPPRESSION);
+			} else {
+				unsigned int timeout = 50;
+				int result;
+				/*
+				 * If we branch here, then we were already powered up.  You would think
+				 * that we would be all set, but it's not that easy.  Piper has a bug in
+				 * it where we have to run a special spike suppression routine when we
+				 * power it up.  However, this routine takes an average of 30 ms to run,
+				 * and I've see it take as long as 300 ms.  This is not acceptable when
+				 * we are duty cycling every 100 ms.  To get around this, we do NOT do
+				 * the spike suppression while duty cycling.  Instead, we simply avoid
+				 * transmitting at those rates which would cause spikes.  Now, however,
+				 * we are ending duty cycling and returning to normal operations so we
+				 * have to do the spike suppression.  Since we are powered up, the first
+				 * thing to do is to power down.
+				 */
+				if (piperp->ps.state != PS_STATE_WAIT_FOR_STOP_TRANSMIT_EVENT) {
+					/*
+					 * If we come here, then we did not happen to be trying to power down
+					 * just as we got the command from mac80211, so we have to start
+					 * the procedure.  This is the normal case.
+					 *
+					 * 1. Set the power save on flag.  This will cause frames to be
+					 *    transmitted with the power management bit on.  The reason
+					 *    for doing that is to tell the AP to stop sending us frames.
+					 * 2. Stop the mac80211 layer from sending us more frames by stopping
+					 *    the transmit queues.
+					 * 3. Send a null-data frame to the AP with the power management bit
+					 *    set.  This should cause it to stop sending us frames.
+					 */
+				 	piperp->ps.power_management = POWERING_DOWN;
+				 	piperp->ps.allowTransmits = false;
+				 	ieee80211_stop_queues(piperp->hw);
+				 	piperp->ps.stopped_tx_queues = true;
+				 	piper_sendNullDataFrame(piperp, POWERING_DOWN);
+				}
+				/*
+				 * Now wait for that last frame to go and and then shut down.
+				 */
+				 result = -1;
+				 for (timeout = 0; (timeout < MAX_SHUTDOWN_TIMEOUT) && (result != 0); timeout++) {
+					spin_unlock_irqrestore(&piperp->ps.lock, flags);
+					mdelay(10);
+					spin_lock_irqsave(&piperp->ps.lock, flags);
+					result = piper_MacEnterSleepMode(piperp, PS_DONT_FORCE);
+				}
+				if (result != 0) {
+					/*
+					 * This is bad.  For some reason we are not able to power down.  We
+					 * will try to force it now, but this may end up putting the driver
+					 * or H/W into a bad state.  However, we can't sit in the loop above
+					 * forever either.
+					 */
+#ifdef WANT_DEBUG
+					printk(KERN_ERR "Forcing Piper to power down\n");
+					printk(KERN_ERR "BB_RSSI_EAS_BUSY = %d\n", piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_BUSY);
+					printk(KERN_ERR "BB_GENERAL_CTL_TX_FIFO_EMPTY = %d\n",
+					    piperp->ac->rd_reg(piperp, BB_GENERAL_CTL) & BB_GENERAL_CTL_TX_FIFO_EMPTY);
+					printk(KERN_ERR "BB_GENERAL_STAT_RX_FIFO_EMPTY = %d\n",
+						piperp->ac->rd_reg(piperp, BB_GENERAL_STAT) & BB_GENERAL_STAT_RX_FIFO_EMPTY);
+					digiWifiDumpRegisters(piperp, MAIN_REGS | MAC_REGS);
+#endif
+					piper_MacEnterSleepMode(piperp, PS_FORCE_POWER_DOWN);
+				}
+				/*
+				 * Wait a moment and then power the H/W back up and execute the spike suppression
+				 * routine.
+				 */
+				spin_unlock_irqrestore(&piperp->ps.lock, flags);
+				mdelay(30);
+				spin_lock_irqsave(&piperp->ps.lock, flags);
+				piper_MacEnterActiveMode(piperp, PS_WANT_SPIKE_SUPPRESSION);
+				ps_resume_transmits(piperp);
+			}
+			stats.jiffiesOn += jiffies - stats.cycleStart;
+#define WANT_STATS		(0)
+#if WANT_STATS
+			if ((piperp->ps.beacon_int != 0)
+			    && ((jiffies - stats.modeStart) != 0)) {
+				printk(KERN_ERR
+				       "jiffiesOff = %u, jiffiesOn = %u, total time = %lu\n",
+				       stats.jiffiesOff, stats.jiffiesOn,
+				       (jiffies - stats.modeStart));
+				printk(KERN_ERR
+				       "Powered down %ld percent of the time.\n",
+				       (stats.jiffiesOff * 100) / (jiffies - stats.modeStart));
+				printk(KERN_ERR
+				       "Received %u of %lu beacons while in powersave mode.\n",
+				       stats.receivedBeacons,
+				       (jiffies -
+					stats.modeStart) /
+				       MILLS_TO_JIFFIES(piperp->ps.beacon_int));
+				printk(KERN_ERR "received %d beacons, missed %d\n",
+						stats.receivedBeacons, stats.missedBeacons);
+				printk(KERN_ERR "allowTransmits = %d, stopped_tx_queues = %d, q_count = %d\n",
+ 						 	piperp->ps.allowTransmits, piperp->ps.stopped_tx_queues,
+ 						 	piperp->tx_queue_count);
+				if ((stats.receivedBeacons + stats.missedBeacons) != 0)
+					printk(KERN_ERR "%d%% beacons were missed\n",
+						(100 * stats.missedBeacons) / (stats.receivedBeacons + stats.missedBeacons));
+			}
+#endif
+		}
+	    piperp->ps.aid = 0;
+	    piperp->ps.state= PS_STATE_WAIT_FOR_BEACON;
+	    piperp->ps.power_management = POWERED_UP;
+	    piperp->ps.poweredDown = false;
+	    piperp->ps.allowTransmits = true;
+	    piperp->ps.stopped_tx_queues = false;
+		ps_resume_transmits(piperp);
+		piper_sendNullDataFrame(piperp, POWERED_UP);
+	}
+
+	spin_unlock_irqrestore(&piperp->ps.lock, flags);
+}
+
+EXPORT_SYMBOL_GPL(piper_ps_set);
+
+
+
+/*
+ * Called when driver is loaded.  Initialize our context.
+ */
+void piper_ps_init(struct piper_priv *piperp)
+{
+	memset(&piperp->ps, 0, sizeof(piperp->ps));
+    piperp->ps.beacon_int = 100;
+    piperp->ps.aid = 0;
+	init_timer(&piperp->ps.timer);
+	piperp->ps.timer.function = ps_timer;
+	piperp->ps.timer.data = (unsigned long) piperp;
+    piperp->ps.mode = PS_MODE_FULL_POWER;
+    piperp->ps.state= PS_STATE_WAIT_FOR_BEACON;
+	spin_lock_init(&piperp->ps.lock);
+    piperp->ps.power_management = POWERED_UP;
+    piperp->ps.poweredDown = false;
+    piperp->ps.rxTaskletRunning;
+    piperp->ps.allowTransmits = true;
+    piperp->ps.stopped_tx_queues = false;
+    piperp->ps.frames_pending = 0;
+}
+
+EXPORT_SYMBOL_GPL(piper_ps_init);
+
+
+/*
+ * Called when driver is unloaded.  Make sure the PS
+ * timer is shut down.
+ */
+void piper_ps_deinit(struct piper_priv *piperp)
+{
+	piper_ps_set(piperp, true);
+	piperp->ps.mode = PS_MODE_FULL_POWER;
+	del_timer_sync(&piperp->ps.timer);
+}
+
+EXPORT_SYMBOL_GPL(piper_ps_deinit);
diff --git a/drivers/net/wireless/digiPiper/digiPs.h b/drivers/net/wireless/digiPiper/digiPs.h
new file mode 100644
index 000000000000..1feedd87e228
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/digiPs.h
@@ -0,0 +1,50 @@
+/*
+ * Header file for digiPs.c.  Declares functions used for power save mode.
+ */
+
+#ifndef digiPs_h
+
+#define digiPS_h
+
+#include <net/mac80211.h>
+#include "pipermain.h"
+
+enum piper_ps_event {
+	PS_EVENT_BEACON_RECEIVED,
+	PS_EVENT_STOP_TRANSMIT_TIMER_EXPIRED,
+	PS_EVENT_TRANSMITTER_DONE,
+    PS_EVENT_TRANSMITTER_DONE_TIMER_TICK,
+	PS_EVENT_WAKEUP,
+	PS_EVENT_MISSED_BEACON
+};
+
+enum piper_ps_tx_completion_result {
+	PS_RETURN_SKB_TO_MAC80211,
+	PS_DONT_RETURN_SKB_TO_MAC80211
+};
+
+enum piper_ps_active_result {
+	PS_CONTINUE_TRANSMIT,
+	PS_STOP_TRANSMIT
+};
+
+
+/*
+ * Current version of mac80211 doesn't set power management bit in frame headers,
+ * so I guess we have to for now.
+ *
+ * TODO:  See if we still have to do this in the next drop.
+ */
+#define piper_ps_set_header_flag(piperp, header) 	\
+			header->fc.pwrMgt = (piperp->ps.power_management)
+
+int piper_ps_active(struct piper_priv *piperp);
+void piper_ps_process_receive_frame(struct piper_priv *piperp,
+				    struct sk_buff *skb);
+void piper_ps_init(struct piper_priv *piperp);
+void piper_ps_deinit(struct piper_priv *piperp);
+void piper_ps_set(struct piper_priv *piperp, bool powerSaveOn);
+struct ieee80211_rate *piper_ps_check_rate(struct piper_priv *piperp,
+					   struct ieee80211_rate *rate);
+
+#endif
diff --git a/drivers/net/wireless/digiPiper/digiRx.c b/drivers/net/wireless/digiPiper/digiRx.c
new file mode 100644
index 000000000000..01ff486311b1
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/digiRx.c
@@ -0,0 +1,412 @@
+/*
+ * digiRx.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/*
+ * This file contains the routines that are related to transmitting
+ * frames.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+
+#include "pipermain.h"
+#include "mac.h"
+#include "phy.h"
+#include "digiPs.h"
+
+#define WANT_RECEIVE_COUNT_SCROLL	(0)
+#define AES_TIMEOUT			(200)
+#define RX_DEBUG			(1)
+
+#if RX_DEBUG
+static int dlevel = DWARNING;
+#define dprintk(level, fmt, arg...)	if (level >= dlevel)			\
+					printk(KERN_ERR PIPER_DRIVER_NAME	\
+					    ": %s - " fmt, __func__, ##arg)
+#else
+#define dprintk(level, fmt, arg...)	do {} while (0)
+#endif
+
+
+/*
+ * This routine is called to flush the receive and transmit FIFOs.  It is used
+ * for error recovery when we detect corrupted data in the FIFO's.  It should be
+ * called with the AES lock set.
+ */
+static void reset_fifo(struct piper_priv *piperp)
+{
+	unsigned int i;
+
+	piperp->ac->rd_reg(piperp, BB_AES_CTL);
+	piperp->ac->wr_reg(piperp, BB_AES_CTL, 0, op_write);
+	// clear the TX-FIFO memory
+	for (i = 0; i < 448; i++)
+		piperp->ac->wr_reg(piperp, BB_DATA_FIFO, 0, op_write);
+
+	// clear RX-FIFO memory
+	for (i = 0; i < 512; i++)
+		piperp->ac->rd_reg(piperp, BB_DATA_FIFO);
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, BB_GENERAL_CTL_RXFIFORST, op_or);
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, ~BB_GENERAL_CTL_RXFIFORST, op_and);
+}
+
+/*
+ * This routine is called to receive a frame.  The hardware header has
+ * already been read from the FIFO.  We need to read out the frame.  If
+ * the frame was encrypted with AES and we have the correct key, then
+ * we use the AES H/W encryption engine to decrypt the frame.  We also
+ * set up a ieee80211_rx_status structure with the appropriate info.
+ *
+ * Arguments
+ *      digi                context information
+ *      skb                 empty buffer to receive packet into
+ *      length              number of bytes in FIFO
+ *      fr_ctrl_field   buffer to copy frame control header into
+ *      status              status structure we must write status into
+ *
+ * Returns
+ *      true            frame was received
+ *      false           an encryption error was detected
+ */
+static bool receive_packet(struct piper_priv *piperp, struct sk_buff *skb, int length,
+			  frameControlFieldType_t * fr_ctrl_field,
+			  struct ieee80211_rx_status *status)
+{
+	_80211HeaderType *header;
+	bool result = true;
+	int originalLength = length;
+	int headerlen;
+#if WANT_RECEIVE_COUNT_SCROLL
+	static int packetCount = 0;
+#endif
+
+	headerlen = _80211_HEADER_LENGTH;
+	if (length <  _80211_HEADER_LENGTH) {
+		/*
+		 * If we branch here, then there is not enough data to make a
+		 * complete header.  This is possible if this is a control frame.
+		 * Adjust our length so that we do not read too much data from
+		 * the FIFO.
+		 */
+		headerlen = length;
+	}
+
+	/*
+	 * Read the frame header.  This includes the frame control fields
+	 * as well as the 802.11 header.
+	 */
+	header = (_80211HeaderType *) skb_put(skb, headerlen);
+	length -= headerlen;
+	piperp->ac->rd_fifo(piperp, BB_DATA_FIFO, (uint8_t *) header, headerlen);
+	memcpy(fr_ctrl_field, &header->fc, sizeof(fr_ctrl_field));
+
+ 	if (((u32)(skb->tail)) & 0x3) {
+		/* align data */
+		skb_reserve(skb, 4 - ((u32)(skb->tail) & 0x3));
+ 	}
+
+	if (header->fc.protected) {
+		/*
+		 * If we branch here, then the frame is encrypted.  We need
+		 * to figure out if we should try to decrypt it.
+		 */
+		unsigned char *rsnHeader;
+		unsigned int aesDataBlob[AES_BLOB_LENGTH / sizeof(unsigned int)];
+		unsigned int keyIndex;
+
+		rsnHeader = skb_put(skb, PIPER_EXTIV_SIZE);
+
+		/*
+		 * Step 1: Read the rest of the unencrypted data, which should
+		 *         consist of the extiv fields.
+		 */
+		piperp->ac->rd_fifo(piperp, BB_DATA_FIFO, rsnHeader, PIPER_EXTIV_SIZE);
+		length -= PIPER_EXTIV_SIZE;
+		keyIndex = rsnHeader[3] >> 6;
+
+		if (piper_prepare_aes_datablob(piperp, keyIndex, (u8 *) aesDataBlob,
+					       (unsigned char *)header, originalLength - 12,
+					       false)) {
+			/*
+			 * If we come here, then we have the correct encryption key for
+			 * the frame and will now try to decrypt it.
+			 */
+			unsigned int timeout = AES_TIMEOUT;
+			unsigned long flags;
+
+			spin_lock_irqsave(&piperp->aesLock, flags);
+
+			/*
+			 * Step 2: Wait for AES to become ready.
+			 */
+			while (piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_BUSY) {
+				timeout--;
+				if (timeout == 0) {
+					/*
+					 * If we come here, then AES busy appears to be stuck high.  It should only be
+					 * high for a maximum of about 80 us when it is encrypting a transmit frame.
+					 * Our timeout value is high enough to guarantee that the engine has had enough
+					 * time to complete the transmit.  Apparently there is data stuck in the FIFO
+					 * from either a previous transmit or receive.
+					 */
+					dprintk(DWARNING, "1st AES busy never became ready\n");
+					digiWifiDumpRegisters(piperp, MAIN_REGS | MAC_REGS);
+					/*
+					 * Recover by flushing the FIFO and returning in error.
+					 */
+					reset_fifo(piperp);
+#if 0
+					/*
+					 * TODO: Figure out why this code snippet doesn't work.  I would think
+					 * that if we reset the fifo, we should just return in error since we will
+					 * have discarded the frame.  However, when we do that the system hangs
+					 * (after a while).  This doesn't make sense.
+					 */
+					spin_unlock_irqrestore(&piperp->aesLock, flags);
+					result = false;
+					goto receive_packet_exit;
+#else
+					break;
+#endif
+				}
+				udelay(1);
+			}
+
+			/*
+			 * Step 3: Set the AES mode, and then read from the AES control
+			 *         register to put the AES engine into receive mode.
+			 */
+			piperp->ac->rd_reg(piperp, BB_AES_CTL);
+
+			/*
+			 * Step 4: Write the expanded AES key into the AES FIFO.
+			 */
+			piperp->ac->wr_fifo(piperp, BB_AES_FIFO,
+				      (unsigned char *)piperp->key[keyIndex].expandedKey,
+				      EXPANDED_KEY_LENGTH);
+
+			/*
+			 * Step 5: Write the AES IV and headers into the AES FIFO.
+			 */
+			piperp->ac->wr_fifo(piperp, BB_AES_FIFO, (unsigned char *)aesDataBlob,
+				      AES_BLOB_LENGTH);
+
+			/*
+			 * Step 6: Now, finally, read the unencrypted frame from the
+			 *         AES FIFO.  Adjust the length so that we don't try
+			 *         to read the MIC or the ICV which the AES engine will
+			 *         process for us.
+			 */
+			length -= MIC_SIZE + ICV_SIZE;
+			piperp->ac->rd_fifo(piperp, BB_AES_FIFO, skb_put(skb, length), length);
+			/*
+			 * mac80211 seems to expect there to be a MIC even if the packet
+			 * has already been decrypted.  It will drop off what it thinks
+			 * are the extra MIC bytes, so add some extra bytes that it
+			 * can drop off without losing any data.
+			 */
+			skb_put(skb, MIC_SIZE);	/* add fake MIC */
+
+			/*
+			 * Step 7: Wait for AES to become ready.
+			 */
+			while (piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_BUSY) {
+				timeout--;
+				if (timeout == 0) {
+					dprintk(DWARNING, "2nd AES busy never became ready\n");
+					digiWifiDumpRegisters(piperp, MAIN_REGS | MAC_REGS);
+				}
+				udelay(1);
+			}
+			result = ((piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_MIC) != 0);
+			timeout = 500;
+			while ((piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_FIFO_EMPTY) == 0) {
+				timeout--;
+				piperp->ac->rd_reg(piperp, BB_AES_FIFO);
+				udelay(1);
+			}
+#ifdef WANT_DEBUG
+			if (piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_BUSY) {
+				digi_dbg("AES busy set at end of rx\n");
+			}
+#endif
+			spin_unlock_irqrestore(&piperp->aesLock, flags);
+
+			/*  pad an extra 8 bytes for the MIC which the H/W strips */
+			skb_put(skb, 8);
+			if (result) {
+				status->flag |= RX_FLAG_DECRYPTED;
+			} else {
+				digi_dbg("Error decrypting packet\n");
+			}
+		} else {
+			/*
+			 * If we branch here, then we are not able to decrypt the
+			 * packet possibly because we don't have the key, or because
+			 * the packet was encrypted using TKIP.  Read the rest of the
+			 * encrypted data.  mac80211 will have to decrypt it in software.
+			 */
+			piperp->ac->rd_fifo(piperp, BB_DATA_FIFO, skb_put(skb, length),
+					    length);
+		}
+	} else {
+		/*
+		 * Frame is not encrypted, so just read it.
+		 */
+		piperp->ac->rd_fifo(piperp, BB_DATA_FIFO, skb_put(skb, length), length);
+	}
+
+#if WANT_RECEIVE_COUNT_SCROLL
+	if (((++packetCount) & 1023) == 0) {
+		printk(KERN_ERR "\n%d recd, tx_start_count = %d, tx_complete_count = %d.\n",
+			packetCount, piperp->pstats.tx_start_count,
+			piperp->pstats.tx_complete_count);
+	}
+#endif
+#if 0
+receive_packet_exit:
+#endif
+	return result;
+}
+
+/*
+ * This routine is called when we receive an ACK.  This should be after
+ * we have transmitted a packet.  We need to tell the upper layer we
+ * have a packet by calling ieee80211_tx_status_irqsafe with status
+ * information.  The transmit routine also disables queuing whenever we
+ * transmit since we can only transmit one packet at a time, so we need
+ * to reenable to transmit queue too.
+ */
+static inline void handle_ack(struct piper_priv *piperp, int signal_strength)
+{
+	if (piper_tx_getqueue(piperp) && piperp->expectingAck) {
+		struct ieee80211_tx_info *txInfo = IEEE80211_SKB_CB(piper_tx_getqueue(piperp));
+		if ((txInfo->flags & IEEE80211_TX_CTL_NO_ACK) == 0) {
+			piperp->clear_irq_mask_bit(piperp,
+						   BB_IRQ_MASK_TX_FIFO_EMPTY |
+						   BB_IRQ_MASK_TIMEOUT |
+						   BB_IRQ_MASK_TX_ABORT);
+			piperp->tx_signal_strength = signal_strength;
+			piperp->tx_result = RECEIVED_ACK;
+			tasklet_hi_schedule(&piperp->tx_tasklet);
+		}
+	}
+}
+
+
+/*
+ * This is the entry point for the receive tasklet.  It is executed
+ * to process receive packets.  It allocates an SKB and receives
+ * the packet into it.
+ *
+ * We may be called from the receive ISR if WANT_TO_RECEIVE_FRAMES_IN_ISR
+ * is set.
+ */
+void piper_rx_tasklet(unsigned long context)
+{
+	struct piper_priv *piperp = (struct piper_priv *)context;
+
+	/*
+	 * This while loop will keep executing as long as the H/W indicates there
+	 * are more frames in the FIFO to be received.
+	 */
+
+	piperp->ps.rxTaskletRunning = true;
+
+	while (((piperp->ac->rd_reg(piperp, BB_GENERAL_STAT) &
+			BB_GENERAL_STAT_RX_FIFO_EMPTY) == 0)
+			&& (!piperp->ps.poweredDown)) {
+		struct sk_buff *skb = NULL;
+		struct ieee80211_rx_status status = { 0 };
+		struct rx_frame_hdr header;
+		unsigned int length = 0;
+		frameControlFieldType_t fr_ctrl_field;
+
+		/*
+		 * Read and process the H/W header.  This header is created by
+		 * the hardware is is not part of the frame.
+		 */
+		piperp->ac->rd_fifo(piperp, BB_DATA_FIFO, (u8 *)&header, sizeof(header));
+		phy_process_plcp(piperp, &header, &status, &length);
+		if ((length == 0) || (length > (RX_FIFO_SIZE - 48))) {	/* 48 bytes for padding and related stuff */
+			unsigned long flags;
+
+			dprintk(DERROR, "bogus frame length (%d)\n", length);
+			dprintk(DERROR, "0x%08x 0x%08x\n", *(u32 *)&header, *(((u32 *)&header) + 1));
+			spin_lock_irqsave(&piperp->aesLock, flags);
+			reset_fifo(piperp);
+			spin_unlock_irqrestore(&piperp->aesLock, flags);
+			continue;
+		}
+
+		if (length != 0) {
+
+			skb = __dev_alloc_skb(RX_FIFO_SIZE + 100, GFP_ATOMIC);
+			if (skb == NULL) {
+				/* Oops.  Out of memory.  Exit the tasklet */
+				dprintk(DERROR, "__dev_alloc_skb failed\n");
+				break;
+			}
+
+			if (receive_packet(piperp, skb, length, &fr_ctrl_field, &status)) {
+
+				if (length >= _80211_HEADER_LENGTH)
+				{
+					/*
+					 * If using the Airoha transceiver, then we want to monitor
+					 * the receive signal strength and continuously adjust the
+					 * receive amplifier so that we get the best possible signal
+					 * to noise ratio.
+					 */
+					unsigned int rssi = phy_determine_rssi(&header);
+
+					piperp->adjust_max_agc(piperp, rssi, (_80211HeaderType *) skb->data);
+				}
+
+				if (fr_ctrl_field.type == TYPE_ACK)
+					handle_ack(piperp, status.signal);
+
+				if ((fr_ctrl_field.type == TYPE_ACK)
+				    || (fr_ctrl_field.type == TYPE_RTS)
+				    || (fr_ctrl_field.type == TYPE_CTS)) {
+					/*
+					 * Don't pass up RTS, CTS, or ACK frames.  They just
+					 * confuse the stack
+					 */
+					dev_kfree_skb(skb);
+				} else {
+					if (fr_ctrl_field.type == TYPE_BEACON) {
+						piperp->beacon.weSentLastOne = false;
+					}
+
+				piper_ps_process_receive_frame(piperp, skb);
+#if WANT_TO_RECEIVE_FRAMES_IN_ISR
+					ieee80211_rx_irqsafe(piperp->hw, skb, &status);
+#else
+					ieee80211_rx(piperp->hw, skb, &status);
+#endif
+				}
+			} else {
+				/* Frame failed MIC, so discard it */
+				dprintk(DWARNING, "dropping bad frame\n");
+				dev_kfree_skb(skb);
+			}
+		}
+	}
+
+	piperp->ps.rxTaskletRunning = false;
+	piperp->set_irq_mask_bit(piperp, BB_IRQ_MASK_RX_FIFO);
+	piper_ps_rx_task_exiting(piperp);
+}
+EXPORT_SYMBOL_GPL(piper_rx_tasklet);
+
+
+
diff --git a/drivers/net/wireless/digiPiper/digiTx.c b/drivers/net/wireless/digiPiper/digiTx.c
new file mode 100644
index 000000000000..4bea846a42a4
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/digiTx.c
@@ -0,0 +1,604 @@
+/*
+ * digiTx.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+/*
+ * This file contains the routines that are related to transmitting
+ * frames.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/jiffies.h>
+#include <linux/timer.h>
+
+#include "pipermain.h"
+#include "mac.h"
+#include "phy.h"
+#include "digiPs.h"
+
+#define FRAME_CONTROL_FIELD_OFFSET      (sizeof(struct tx_frame_hdr) + sizeof(struct psk_cck_hdr))
+#define AES_TIMEOUT			(200)
+
+#define TX_DEBUG			(1)
+
+#if TX_DEBUG
+//static int dlevel = DWARNING;
+#define dprintk(level, fmt, arg...)	if (level >= dlevel)			\
+					printk(KERN_ERR PIPER_DRIVER_NAME	\
+					    ": %s - " fmt, __func__, ##arg)
+#else
+#define dprintk(level, fmt, arg...)	do {} while (0)
+#endif
+
+/*
+ * Adds an entry into the tx queue.
+ */
+int piper_tx_enqueue(struct piper_priv *piperp, struct sk_buff *skb, tx_skb_return_cb_t skb_return_cb)
+{
+	unsigned long flags;
+	int result = -1;
+
+	spin_lock_irqsave(&piperp->tx_queue_lock, flags);
+	if (NEXT_TX_QUEUE_INDEX(piperp->tx_queue_head) != piperp->tx_queue_tail) {
+		piperp->tx_queue[piperp->tx_queue_head].skb = skb;
+		piperp->tx_queue[piperp->tx_queue_head].skb_return_cb = skb_return_cb;
+		piperp->tx_queue_head = NEXT_TX_QUEUE_INDEX(piperp->tx_queue_head);
+		piperp->tx_queue_count++;
+		result = 0;
+	}
+	spin_unlock_irqrestore(&piperp->tx_queue_lock, flags);
+
+	return result;
+}
+EXPORT_SYMBOL_GPL(piper_tx_enqueue);
+
+
+/*
+ * Returns the skb for the current element.
+ */
+struct sk_buff *piper_tx_getqueue(struct piper_priv *piperp)
+{
+	if (piperp->tx_queue_head == piperp->tx_queue_tail) {
+		return NULL;
+	} else {
+		return piperp->tx_queue[piperp->tx_queue_tail].skb;
+	}
+}
+EXPORT_SYMBOL_GPL(piper_tx_getqueue);
+
+/*
+ * Returns the skb buffer call back for the current element.
+ */
+static inline tx_skb_return_cb_t piper_tx_getqueue_return_fn(struct piper_priv *piperp)
+{
+	return piperp->tx_queue[piperp->tx_queue_tail].skb_return_cb;
+}
+
+
+/*
+ * Called to advance the queue tail.
+ */
+static inline void piper_tx_queue_next(struct piper_priv *piperp)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&piperp->tx_queue_lock, flags);
+	if (piperp->tx_queue_head != piperp->tx_queue_tail) {
+		piperp->tx_queue[piperp->tx_queue_tail].skb = NULL;
+		piperp->tx_queue[piperp->tx_queue_tail].skb_return_cb = NULL;
+		piperp->tx_queue_tail = NEXT_TX_QUEUE_INDEX(piperp->tx_queue_tail);
+		piperp->tx_queue_count--;
+	}
+	spin_unlock_irqrestore(&piperp->tx_queue_lock, flags);
+}
+
+/*
+ * Called when we unload to clear any remaining queue entries.
+ */
+void piper_empty_tx_queue(struct piper_priv *piperp)
+{
+	while (piper_tx_getqueue(piperp)) {
+		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(piper_tx_getqueue(piperp));
+
+		ieee80211_tx_info_clear_status(info);
+		piper_tx_getqueue_return_fn(piperp)(piperp->hw, piper_tx_getqueue(piperp));
+		piper_tx_queue_next(piperp);
+	}
+}
+EXPORT_SYMBOL_GPL(piper_empty_tx_queue);
+
+
+bool piper_tx_queue_half_full(struct piper_priv *piperp)
+{
+	return (piperp->tx_queue_count >= (PIPER_TX_QUEUE_SIZE >> 1));
+}
+EXPORT_SYMBOL_GPL(piper_tx_queue_half_full);
+
+
+
+/*
+ * This routine writes a frame using H/W AES encryption.
+ *
+ * Arguments
+ *      digi            context
+ *      buffer          pointer to start of frame
+ *      length          number of bytes in frame
+ *
+ * Return Values
+ *      0           success
+ *      !0          transmit failed
+ */
+static int piper_write_aes(struct piper_priv *piperp, unsigned char *buffer,
+			   unsigned int length)
+{
+	int result;
+	int timeout = AES_TIMEOUT;
+	unsigned long spinLockFlags;
+
+	/*
+	 * Step 1: Wait for AES to become ready.
+	 */
+	spin_lock_irqsave(&piperp->aesLock, spinLockFlags);
+	while (piperp->ac->rd_reg(piperp, BB_RSSI) & BB_RSSI_EAS_BUSY) {
+		timeout--;
+		if (timeout == 0) {
+			/*
+			 * If we come here, then AES busy appears to be stuck high.  It should only be
+			 * high for a maximum of about 80 us when it is encrypting a transmit frame.
+			 * Our timeout value is high enough to guarantee that the engine has had enough
+			 * time to complete the transmit.  Apparently there is data stuck in the FIFO
+			 * from either a previous transmit or receive.
+			 */
+			digi_dbg("write AES, AES busy stuck on\n");
+			digiWifiDumpRegisters(piperp, MAIN_REGS | MAC_REGS);
+			/*
+			 * We recover by simply continuing on.  Step 3 writes to the AES control
+			 * register.  This will reset the AES engine and clear the error condition.
+			 */
+			break;
+		}
+		udelay(1);
+	}
+
+	/*
+	 * Step 2: Write the unencrypted part of the frame into the normal
+	 *         data FIFO.
+	 */
+	piperp->ac->wr_fifo(piperp, BB_DATA_FIFO, buffer,
+		      _80211_HEADER_LENGTH + TX_HEADER_LENGTH + PIPER_EXTIV_SIZE);
+
+	/*
+	 * Step 3: Write to the AES control register.  Writing to it puts
+	 *         AES H/W engine into transmit mode.  We also make sure
+	 *         the AES mode is set correctly.
+	 */
+	piperp->ac->wr_reg(piperp, BB_AES_CTL, 0, op_write);
+
+	/*
+	 * Step 4: Write the expanded AES key into the AES FIFO.
+	 */
+	piperp->ac->wr_fifo(piperp, BB_AES_FIFO,
+		      (unsigned char *)piperp->key[piperp->tx_aes_key].expandedKey,
+		      EXPANDED_KEY_LENGTH);
+
+	/*
+	 * Step 5: Write the AES IV and headers into the AES FIFO.
+	 */
+	piperp->ac->wr_fifo(piperp, BB_AES_FIFO, (unsigned char *)piperp->tx_aes_blob,
+			    AES_BLOB_LENGTH);
+
+	/*
+	 * Step 6: Now, finally, write the part of the frame that needs to
+	 *         be encrypted into the AES FIFO.
+	 */
+	result =
+	    piperp->ac->wr_fifo(piperp, BB_AES_FIFO,
+			  &buffer[_80211_HEADER_LENGTH + TX_HEADER_LENGTH + PIPER_EXTIV_SIZE],
+			  length - (_80211_HEADER_LENGTH + TX_HEADER_LENGTH +
+				    PIPER_EXTIV_SIZE));
+
+	spin_unlock_irqrestore(&piperp->aesLock, spinLockFlags);
+
+	return result;
+}
+
+/*
+ * Determine what bit rate the next retry should be sent at.
+ *
+ * The mac80211 library passes us an array of tx bit rates.  Each entry
+ * has a rate index and a limit (max number of retries at that rate).
+ * We use the rate index to build the H/W transmit header.  The limit
+ * is decremented each time we retry.  When it reaches zero, we try the
+ * next rate in the array.
+ */
+static struct ieee80211_rate *get_tx_rate(struct piper_priv *piperp, struct ieee80211_tx_info *info)
+{
+	struct ieee80211_rate *ret = NULL;
+
+	if (piperp->pstats.tx_retry_count[piperp->pstats.tx_retry_index] >=
+	    info->control.rates[piperp->pstats.tx_retry_index].count) {
+		piperp->pstats.tx_retry_index++;
+	}
+
+	if (piperp->pstats.tx_retry_index >= IEEE80211_TX_MAX_RATES) {
+	    return NULL;        /* don't go beyond the end of the rates array */
+	}
+
+	if (piperp->pstats.tx_retry_index == 0) {
+		ret = ieee80211_get_tx_rate(piperp->hw, info);
+	} else {
+		ret = ieee80211_get_alt_retry_rate(piperp->hw, info, piperp->pstats.tx_retry_index - 1);
+	}
+
+	if (ret != NULL) {
+		if (piperp->calibrationTxRate) {
+			ret = piperp->calibrationTxRate;
+		}
+	}
+
+	ret = piper_ps_check_rate(piperp, ret);
+
+	return ret;
+}
+
+/*
+ * This function returns a value for the contention window in microseconds.  We
+ * start with the contention window at CW_MIN and double it everytime we have to
+ * retry.
+ */
+static u16 piper_get_cw(struct piper_priv *piperp, bool isFirstTime)
+{
+	static u16 cw = DEFAULT_CW_MIN;
+
+	if (isFirstTime) {
+		cw = DEFAULT_CW_MIN;
+	} else {
+		cw <<= 1;
+		if (cw > DEFAULT_CW_MAX) {
+			cw = DEFAULT_CW_MAX;
+		}
+	}
+	return (cw + (10 * (piperp->rand() & (cw - 1)))) & 0xffff;
+}
+
+/*
+ * This function will prepend an RTS or CTS to self frame ahead of the current
+ * TX frame.  This is done if the wantRts or wantCts flag is set.  The mac80211
+ * library determines if either of these flags is set.
+ *
+ * The RTS or CTS message is written into the transmit FIFO ahead of the
+ * data frame.  Note that RTS and CTS messages are always sent in the clear
+ * so we do not have to worry about encryption.
+ *
+ * Our caller, the master transmit routine, is responsible for setting the
+ * transmit hold bit before calling us and clearing it after the data frame
+ * has been written into the FIFO.  This ensures that the RTS/CTS frame is
+ * not transmitted until after the data frame is ready to go.
+ *
+ * Also note that if we are unable to send the RTS/CTS frame, then the H/W
+ * is smart enough to also about the data frame.  So we will not send
+ * the data frame without the RTS/CTS frame.
+ */
+static void handle_rts_cts(struct piper_priv *piperp,
+			   struct ieee80211_tx_info *txInfo, unsigned int frameType)
+{
+	piperp->tx_rts = false;
+
+	if (frameType == TYPE_DATA) {
+		unsigned int header[2];
+		struct ieee80211_rate *rate = NULL;
+		bool wantCts = (!!(txInfo->control.rates[piperp->pstats.tx_retry_index].flags
+				   & IEEE80211_TX_RC_USE_CTS_PROTECT)
+				| piperp->tx_cts);
+		bool wantRts = !!(txInfo->control.rates[piperp->pstats.tx_retry_index].flags
+				  & IEEE80211_TX_RC_USE_RTS_CTS);
+
+		if ((wantRts) || (wantCts)) {
+			/*
+			 * If we are sending an RTS or a CTS, then get the rate information.
+			 */
+			if (piperp->calibrationTxRate) {
+				rate = piperp->calibrationTxRate;
+			} else {
+				rate = ieee80211_get_rts_cts_rate(piperp->hw, txInfo);
+			}
+			if (rate == NULL) {
+				digi_dbg
+				    ("ieee80211_get_rts_cts_rate(digi->hw, txInfo) returned NULL!\n");
+			}
+		}
+		if ((wantRts) && (rate)) {
+			/*
+			 * We're sending an RTS, so load it into the FIFO.
+			 */
+			struct ieee80211_rts rtsFrame;
+
+			ieee80211_rts_get(piperp->hw, txInfo->control.vif,
+					  piper_tx_getqueue(piperp)->data + TX_HEADER_LENGTH,
+					  piper_tx_getqueue(piperp)->len - TX_HEADER_LENGTH, txInfo,
+					  &rtsFrame);
+			/*
+			 * If we come here, then we need to send an RTS frame ahead of the
+			 * current data frame.
+			 */
+			phy_set_plcp((unsigned char *)header, sizeof(struct ieee80211_rts),
+				     rate, 0);
+			piperp->ac->wr_fifo(piperp, BB_DATA_FIFO, (unsigned char *)header,
+				      TX_HEADER_LENGTH);
+			piperp->ac->wr_fifo(piperp, BB_DATA_FIFO, (unsigned char *)&rtsFrame,
+				      sizeof(rtsFrame));
+			if (piperp->pstats.tx_total_tetries != 0) {
+				piperp->pstats.ll_stats.dot11RTSFailureCount++;
+			}
+			piperp->tx_rts = true;
+		} else if ((wantCts) && (rate)) {
+			/*
+			 * We're sending a CTS, so load it into the FIFO.
+			 */
+			struct ieee80211_cts ctsFrame;
+
+			ieee80211_ctstoself_get(piperp->hw, txInfo->control.vif,
+						piper_tx_getqueue(piperp)->data + TX_HEADER_LENGTH,
+						piper_tx_getqueue(piperp)->len - TX_HEADER_LENGTH,
+						txInfo, &ctsFrame);
+			/*
+			 * At the time this code was written, the mac80211 library had
+			 * a bug in the ieee80211_ctstoself_get which caused it to copy
+			 * the wrong MAC address into the cts frame.  So we copy the
+			 * right one (ours) in now.
+			 */
+			memcpy(piperp->ctsFrame.ra, piperp->hw->wiphy->perm_addr, ETH_ALEN);
+
+			/*
+			 * If we come here, then we need to send a CTS to self frame ahead of the
+			 * current data frame.
+			 */
+			phy_set_plcp((unsigned char *)header, sizeof(struct ieee80211_cts),
+				     rate, 0);
+			piperp->ac->wr_fifo(piperp, BB_DATA_FIFO, (unsigned char *)header,
+				      TX_HEADER_LENGTH);
+			piperp->ac->wr_fifo(piperp, BB_DATA_FIFO, (unsigned char *)&ctsFrame,
+				      sizeof(ctsFrame));
+		}
+	}
+}
+
+/*
+ * This routine is called to report the result of a transmit operation to
+ * mac80211.  It is used for both successful transmissions and failures.
+ * It sends the result to the stack, removes the current tx frame from the
+ * queue, and then wakes
+ * up the transmit queue.
+ */
+void packet_tx_done(struct piper_priv *piperp, tx_result_t result,
+			   int signal_strength)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(piper_tx_getqueue(piperp));
+	int i;
+	struct sk_buff *skb;
+	unsigned long flags;
+
+#define WANT_TRANSMIT_RESULT 	(0)
+#if WANT_TRANSMIT_RESULT
+	const char *resultText[] =
+	{
+		"RECEIVED_ACK",
+		"TX_COMPLETE",
+		"OUT_OF_RETRIES",
+		"TX_NOT_DONE"
+	};
+#endif
+	del_timer_sync(&piperp->tx_timer);
+    piperp->expectingAck = false;
+
+#if WANT_TRANSMIT_RESULT
+	printk(KERN_ERR "Transmit result %s\n", resultText[result]);
+#endif
+	if (piperp->tx_calib_cb)
+		piperp->tx_calib_cb(piperp);
+
+	if (piper_tx_getqueue(piperp) != NULL) {
+		skb_pull(piper_tx_getqueue(piperp), TX_HEADER_LENGTH);
+
+		ieee80211_tx_info_clear_status(info);
+
+		/* prepare statistics and pass them to the stack */
+		for (i = 0; i < IEEE80211_TX_MAX_RATES; i++) {
+			info->status.rates[i].count = piperp->pstats.tx_retry_count[i];
+			if (info->status.rates[i].count == 0)
+				info->status.rates[i].idx = -1;
+		}
+
+		info->status.ack_signal = signal_strength;
+		info->flags |= (result == RECEIVED_ACK) ? IEEE80211_TX_STAT_ACK : 0;
+		piperp->pstats.tx_complete_count++;
+		piperp->pstats.tx_queue.len--;
+		if (piperp->tx_rts)
+			piperp->pstats.ll_stats.dot11RTSSuccessCount++;
+
+		skb = piper_tx_getqueue(piperp);
+		piper_tx_getqueue_return_fn(piperp)(piperp->hw, skb);
+
+		piper_tx_queue_next(piperp);
+
+		if (piper_tx_getqueue(piperp) == NULL) {
+			spin_lock_irqsave(&piperp->tx_tasklet_lock, flags);
+			piperp->tx_tasklet_running = false;
+			spin_unlock_irqrestore(&piperp->tx_tasklet_lock, flags);
+			if (piperp->ps.allowTransmits) {
+				ieee80211_wake_queues(piperp->hw);
+			} else {
+				/*
+				 * Do not wake up the mac80211 Tx queue if we are trying to power
+				 * down.  Make sure we set the stopped_tx_queues flag so that we
+				 * know to restart the queues.
+				 */
+				piperp->ps.stopped_tx_queues = true;
+			}
+		} else {
+			if (result == OUT_OF_RETRIES) {
+				/*
+				 * If we come here, then we are being called from the middle of the
+				 * transmit routine and we have to reschedule the transmit task to
+				 * start dequeueing the next frame.
+				 */
+				tasklet_hi_schedule(&piperp->tx_tasklet);
+			}
+		}
+	} else {
+		digi_dbg("packet_tx_done called with empty queue\n");
+	}
+
+	piperp->tx_result = TX_NOT_DONE;
+}
+EXPORT_SYMBOL_GPL(packet_tx_done);
+
+/*
+ * This function is the entry point for the transmit tasklet.  It
+ * is called to transmit frames.  It will first be called to transmit
+ * the frame and then to retry if the original transmit fails.  So
+ * it does both the first transmit and the subsequent retries.
+ *
+ * Arguments
+ *      context     context information
+ */
+void piper_tx_tasklet(unsigned long context)
+{
+	struct piper_priv *piperp = (struct piper_priv *)context;
+	frameControlFieldType_t *fc;
+	int err;
+
+	piperp->expectingAck = false;
+	del_timer_sync(&piperp->tx_timer);
+	if ((piperp->tx_result == RECEIVED_ACK) || (piperp->tx_result == TX_COMPLETE)) {
+		/*
+		 * We will come here if the receiver task received an ACK, or if we got
+		 * a tx fifo empty interrupt.  In these cases the receiver thread or ISR
+		 * schedule the tx tasklet to handle the event rather than calling
+		 * packet_tx_done directly.
+		 */
+		packet_tx_done(piperp, piperp->tx_result, piperp->tx_signal_strength);
+	}
+
+
+	/*
+	 * Clear flags here to cover ACK case.  We do not clear the flags in the ACK
+	 * routine since it is possible to receive an ACK after we have started the
+	 * next packet.  The appropriate interrupts will be reenabled if we decide
+	 * to retransmit.
+	 */
+	piperp->clear_irq_mask_bit(piperp,
+				BB_IRQ_MASK_TX_FIFO_EMPTY | BB_IRQ_MASK_TIMEOUT |
+				BB_IRQ_MASK_TX_ABORT);
+
+	if (piper_tx_getqueue(piperp) != NULL) {
+		struct ieee80211_tx_info *txInfo = IEEE80211_SKB_CB(piper_tx_getqueue(piperp));
+		struct ieee80211_rate *txRate = get_tx_rate(piperp, txInfo);
+
+		if (txRate != NULL) {
+			fc = (frameControlFieldType_t *)
+					&piper_tx_getqueue(piperp)->data[FRAME_CONTROL_FIELD_OFFSET];
+
+			/* set the retry bit if this is not the first try */
+			if (piperp->pstats.tx_retry_count[0] != 0)
+				fc->retry = 1;
+
+			piperp->ac->wr_reg(piperp, MAC_BACKOFF,
+					   piper_get_cw(piperp,
+					   (piperp->pstats.tx_retry_count[0] == 0)),
+					   op_write);
+
+			/*
+			 * Build the H/W transmit header.  The transmit header is rebuilt on each
+			 * retry because it has the TX rate information which may change for
+			 * retries.
+			 */
+			phy_set_plcp(piper_tx_getqueue(piperp)->data,
+				     piper_tx_getqueue(piperp)->len - TX_HEADER_LENGTH,
+				     txRate, piperp->use_hw_aes ? 8 : 0);
+
+			/*
+			 * Pause the transmitter so that we don't start transmitting before we
+			 * are ready.
+			 */
+			piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, BB_GENERAL_CTL_TX_HOLD, op_or);
+
+			handle_rts_cts(piperp, txInfo, fc->type);
+
+			if (piperp->use_hw_aes == true && txInfo->control.hw_key != NULL)  {
+				err =
+				    piper_write_aes(piperp, piper_tx_getqueue(piperp)->data,
+						    piper_tx_getqueue(piperp)->len);
+			} else {
+				err =
+				    piperp->ac->wr_fifo(piperp, BB_DATA_FIFO, piper_tx_getqueue(piperp)->data,
+						piper_tx_getqueue(piperp)->len);
+			}
+
+			/* Clear any pending TX interrupts */
+			piperp->ac->wr_reg(piperp, BB_IRQ_STAT,
+					  BB_IRQ_MASK_TX_FIFO_EMPTY | BB_IRQ_MASK_TIMEOUT |
+					  BB_IRQ_MASK_TX_ABORT, op_write);
+
+			/*
+			 * Now start the transmitter.
+			 */
+			piperp->expectingAck = ((txInfo->flags & IEEE80211_TX_CTL_NO_ACK) == 0);
+			piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, ~BB_GENERAL_CTL_TX_HOLD,
+					  op_and);
+
+			/*
+			 * Set interrupt flags.  Use the timeout interrupt if we expect
+			 * an ACK.  Use the FIFO empty interrupt if we do not expect an ACK.
+			 */
+			if (txInfo->flags & IEEE80211_TX_CTL_NO_ACK) {
+				piperp->set_irq_mask_bit(piperp,
+							 BB_IRQ_MASK_TX_FIFO_EMPTY |
+							 BB_IRQ_MASK_TX_ABORT);
+			} else {
+				/*
+				 * We set up a timer to fire in 1/4 second.  We should not need it, but somehow
+				 * we seem to miss a timeout interrupt occasionally.  Perhaps we encounter a receive
+				 * overrun which causes the H/W to discard the ACK packet without generating
+				 * a timeout.
+				 */
+				piperp->tx_timer.expires = jiffies + (HZ >> 2);
+				add_timer(&piperp->tx_timer);
+
+				/*
+				 * Also set the IRQ mask to listen for timeouts and TX aborts.  We will receive
+				 * an ACK (which is handled by the RX routine) if the TX is successful.
+				 */
+				piperp->set_irq_mask_bit(piperp,
+							 BB_IRQ_MASK_TIMEOUT |
+							 BB_IRQ_MASK_TX_ABORT);
+			}
+			if ((piperp->pstats.tx_total_tetries != 0) &&
+			    ((txInfo->flags & IEEE80211_TX_CTL_NO_ACK) == 0)) {
+				piperp->pstats.ll_stats.dot11ACKFailureCount++;
+			}
+			piperp->pstats.tx_retry_count[piperp->pstats.tx_retry_index]++;
+			piperp->pstats.tx_total_tetries++;
+		} else {
+			packet_tx_done(piperp, OUT_OF_RETRIES, 0);
+		}
+	} else {
+		long unsigned int flags;
+
+		spin_lock_irqsave(&piperp->tx_tasklet_lock, flags);
+		piperp->tx_tasklet_running = false;
+		spin_unlock_irqrestore(&piperp->tx_tasklet_lock, flags);
+	}
+}
+EXPORT_SYMBOL_GPL(piper_tx_tasklet);
+
+
+
diff --git a/drivers/net/wireless/digiPiper/mac.h b/drivers/net/wireless/digiPiper/mac.h
new file mode 100644
index 000000000000..892a15a8b67c
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/mac.h
@@ -0,0 +1,375 @@
+#ifndef DIGI_MAC_H_
+#define DIGI_MAC_H_
+
+enum baseband_control_regs {
+	BB_VERSION = 0x00,
+	BB_GENERAL_CTL = 0x04,
+	BB_GENERAL_STAT = 0x08,
+	BB_RSSI = 0x0c,
+	BB_IRQ_MASK = 0x10,
+	BB_IRQ_STAT = 0x14,
+	BB_SPI_DATA = 0x18,
+	BB_SPI_CTRL = 0x1c,
+	BB_DATA_FIFO = 0x20,
+	BB_TRACK_CONTROL = 0x28,
+	BB_CONF_2 = 0x2c,
+	BB_AES_FIFO = 0x30,
+	BB_AES_CTL = 0x38,
+	BB_OUTPUT_CONTROL = 0x3c
+};
+
+#define BB_VERSION_MASK(v)		((v) & 0xffff)
+
+#define BB_GENERAL_CTL_RX_EN		(1<<0)
+#define BB_GENERAL_CTL_ANT_DIV		(1<<1)
+#define BB_GENERAL_CTL_ANT_SEL		(1<<2)
+#define	BB_GENERAL_CTL_GEN_5GEN		(1<<3)		// 5 GHz band enable
+#define BB_GENERAL_CTL_SH_PRE       (1<<4)
+#define BB_GENERAL_CTL_RXFIFORST    (1<<5)
+#define BB_GENERAL_CTL_TXFIFORST    (1<<6)
+#define BB_GENERAL_CTL_TX_HOLD		(1<<7)
+#define BB_GENERAL_CTL_BEACON_EN	(1<<8)
+#define BB_GENERAL_CTL_FW_LOAD_ENABLE   (1 << 9)
+#define BB_GENERAL_CTL_DSP_LOAD_ENABLE  (1 << 10)
+#define BB_GENERAL_CTL_MAC_ASSIST_ENABLE (1 << 11)
+#define BB_GENERAL_CTL_TX_FIFO_FULL	 (1<<15)
+#define BB_GENERAL_CTL_TX_FIFO_EMPTY (1<<14)
+/* TODO: verify max gain value for Piper and Wi9p*/
+#define BB_GENERAL_CTL_MAX_GAIN(g)	(((g) & 0x7f)<<16)
+#define BB_GENERAL_CTL_PWR_UP		(1<<24)
+#define BB_GENERAL_CTL_ADC_CLK_EN	(1<<25)
+#define BB_GENERAL_CTL_BOOT_STAT	(1<<28)
+#define BB_GENERAL_CTL_CLK_EN		(1<<29)
+#define BB_GENERAL_CTL_SPI_RST		(1<<30)
+
+#define BB_GENERAL_CTL_MAX_GAIN_MASK (0x007F0000)
+#define BB_GENERAL_CTL_DEFAULT_MAX_GAIN_A	(0x00790000)
+#define BB_GENERAL_CTL_DEFAULT_MAX_GAIN_BG	(0x007c0000)
+
+#if defined(CONFIG_PIPER_WIFI)
+#if 0
+#define BB_GENERAL_CTL_INIT		(BB_GENERAL_CTL_MAX_GAIN(0x7a) | \
+		BB_GENERAL_CTL_PWR_UP | BB_GENERAL_CTL_ADC_CLK_EN | \
+		BB_GENERAL_CTL_BOOT_STAT | BB_GENERAL_CTL_CLK_EN)
+#define BB_GENERAL_CTL_RESET		(BB_GENERAL_CTL_MAX_GAIN(0x7f) | \
+		BB_GENERAL_CTL_ADC_CLK_EN | BB_GENERAL_CTL_BOOT_STAT | \
+		BB_GENERAL_CTL_SPI_RST)
+#else
+#define BB_GENERAL_CTL_INIT		(BB_GENERAL_CTL_MAX_GAIN(0x7a)
+
+#define BB_GENERAL_CTL_RESET		(BB_GENERAL_CTL_MAX_GAIN(0x7f) | \
+                                BB_GENERAL_CTL_SPI_RST
+#endif
+#else
+#define BB_GENERAL_CTL_INIT		(BB_GENERAL_CTL_MAX_GAIN(0x7a) | \
+		BB_GENERAL_CTL_PWR_UP | BB_GENERAL_CTL_ADC_CLK_EN | \
+		BB_GENERAL_CTL_BOOT_STAT | BB_GENERAL_CTL_CLK_EN)
+
+#define BB_GENERAL_CTL_RESET		(BB_GENERAL_CTL_MAX_GAIN(0x7f) | \
+		BB_GENERAL_CTL_ADC_CLK_EN | BB_GENERAL_CTL_BOOT_STAT | \
+		BB_GENERAL_CTL_SPI_RST)
+#endif
+#define BB_RSSI_LED			    (1<<8)
+#define BB_RSSI_EAS_FIFO_EMPTY  (1 << 16)
+#define BB_RSSI_EAS_FIFO_FULL   (1 << 17)
+#define BB_RSSI_EAS_BUSY        (1 << 18)
+#define BB_RSSI_EAS_MIC         (1 << 19)
+#define BB_RSSI_ANT_MASK		(0xff<<24)
+#define BB_RSSI_ANT_NO_DIV_MAP	(0x96000000)
+#define BB_RSSI_ANT_DIV_MAP     (0x1E000000)
+
+#define BB_GENERAL_STAT_RESET		(1<<30)
+/*
+ * STAT_B_EN is a constant that defines a bit in the Wireless Controller FPGA Baseband Control Register
+ * for General Status, which enables the PSK/CCK receiver baseband circuitry (802.11b receiver).
+ * STAT_A_EN is a constant that defines a bit in the Wireless Controller FPGA Baseband Control Register
+ * for General Status, which enables the OFDM receive baseband circuitry (802.11a receiver).
+ */
+
+#define BB_GENERAL_STAT_B_EN		0x10000000      // B EN (PSK/CCK)
+#define BB_GENERAL_STAT_A_EN		0x20000000      // A EN (OFDM)
+#define BB_GENERAL_STAT_RX_FIFO_EMPTY   (1 << 4)
+#define BB_GENERAL_STAT_DC_DIS      (1 << 24)
+#define BB_GENERAL_STAT_SRC_DIS		(1 << 16)
+#define BB_GENERAL_STAT_SPRD_DIS    (1 << 17)
+#define BB_GENERAL_STAT_DLL_DIS		(1 << 18)
+#define TRACK_TX_B_GAIN_MASK	0xff000000		// Mask word for B_TX_GAIN
+#define TRACK_TX_B_GAIN_NORMAL	0xA0000000		// normal setting for B_TX_GAIN
+#define TRACK_BG_BAND           0x00430000     // Tracking constant for 802.11 b/g frequency band
+#define TRACK_CONSTANT_MASK		0x00ff0000		// mask for tracking constant
+#define TRACK_4920_4980_A_BAND  0x00210000     // Tracking constant for 802.11 a sub-frequency band
+#define TRACK_5150_5350_A_BAND  0x001F0000     // Tracking constant for 802.11 a sub-frequency band
+#define TRACK_5470_5725_A_BAND  0x001D0000     // Tracking constant for 802.11 a sub-frequency band
+#define TRACK_5725_5825_A_BAND  0x001C0000     // Tracking constant for 802.11 a sub-frequency band
+
+
+#define BB_IRQ_MASK_RX_FIFO		(1<<0)
+#define BB_IRQ_MASK_TX_FIFO_EMPTY	(1<<1)
+#define BB_IRQ_MASK_TIMEOUT		(1<<2)
+#define BB_IRQ_MASK_TX_ABORT		(1<<3)
+#define BB_IRQ_MASK_TBTT		(1<<4)
+#define BB_IRQ_MASK_ATIM		(1<<5)
+#define BB_IRQ_MASK_RX_OVERRUN		(1<<6)
+
+#define BB_AES_CTL_KEY_LOAD		(1<<2)
+#define BB_AES_CTL_AES_MODE		(1<<4)
+
+enum mac_control_regs {
+	MAC_STA_ID0 = 0x40,
+	MAC_STA_ID1 = 0x44,
+	MAC_BSS_ID0 = 0x48,
+	MAC_BSS_ID1 = 0x4c,
+	MAC_SSID_LEN = 0x50,	/* OFDM_BRS, PSK_BRS, TX_CTL, SSID_LEN */
+	MAC_BACKOFF = 0x54,	/* actually 0x56; 2 low order bytes are empty */
+	MAC_DTIM_PERIOD = 0x58,
+	/*MAC_CFP_PERIOD = 0x59,*/
+	/*MAC_LISTEN_INTERVAL = 0x5a,*/
+	MAC_CFP_ATIM = 0x5c,	/* beacon interval, CFP/ATIM duration */
+	MAC_STATUS = 0x60,
+	/*MAC_TXP_TIMING = 0x62,*/
+	/*MAC_STATUS = 0x63,*/
+	MAC_CTL = 0x64,		/* MAC_AES_KEY_DIS (8 bits), MAC_CTL (8 bits) */
+	MAC_MEASURE = 0x68,	/* actually 0x69 */
+	/*MAC_REMAIN_BO = 0x6a,*/
+	MAC_BEACON_FILT = 0x6c,	/* actally 0x6d */
+	/*MAC_BEACON_BO = 0x6e,*/
+	MAC_STA2_ID0 = 0xb0,
+	MAC_STA2_ID1 = 0xb4,
+	MAC_STA3_ID0 = 0xb8,
+	MAC_STA3_ID1 = 0xbc,
+
+	MAC_EEPROM_CTL = 0xf0,
+	MAC_EEPROM_DATA = 0xf8,
+
+	MAC_SSID = 0x80,
+
+	BEACON_FIFO = 0x85,     /* dummy value used to select data fifo for beacon load */
+};
+
+
+#define MAC_SSID_LEN_MASK           (0x000000ff)
+#define MAC_REVISION_MASK(v)		(((v) >> 16) & 0xffff)
+
+#define MAC_BEACON_INTERVAL_SHIFT   (16)
+#define MAC_BEACON_INTERVAL_MASK    (0xffff0000)
+
+#define MAC_ATIM_PERIOD_MASK        (0x0000ffff)
+
+#define MAC_LISTEN_INTERVAL_MASK    (0x0000ffff)
+
+#define MAC_DTIM_PERIOD_SHIFT       (24)
+#define MAC_DTIM_PERIOD_MASK        (0xff000000)
+
+#define MAC_DTIM_CFP_SHIFT          (16)
+#define MAC_DTIM_CFP_MASK           (0x00ff0000)
+
+#define MAC_OFDM_BRS_MASK           (0xff000000)
+#define MAC_OFDM_BRS_SHIFT          (24)
+#define MAC_PSK_BRS_MASK            (0x000f0000)
+#define MAC_PSK_BRS_SHIFT           (16)
+
+#define MAC_BEACON_BACKOFF_MASK     (0x0000ffff)
+
+#define MAC_BRS_MASK                (MAC_OFDM_BRS_MASK | MAC_PSK_BRS_MASK)
+
+#define MAC_CTL_TX_REQ          (1)
+#define MAC_CTL_BEACON_TX		(1<<2)
+#define MAC_CTL_IBSS			(1<<4)
+#define MAC_CTL_AES_DISABLE		(1<<5)
+#define MAC_CTL_MAC_FLTR		(1<<6)
+#define MAC_CTL_KEY0_DISABLE		(1<<8)
+#define MAC_CTL_KEY1_DISABLE		(1<<9)
+#define MAC_CTL_KEY2_DISABLE		(1<<10)
+#define MAC_CTL_KEY3_DISABLE		(1<<11)
+
+#define MAC_EEPROM_CTL_WAIT_MS		21
+
+/*
+ * RX packets look something like:
+ * <custom bus protocol header(s)> - protocol-dependent
+ * <rx_frame_hdr> - 4 bytes
+ * <plcp; either psk_cck_hdr or ofdm_hdr> - 4 bytes
+ * <mac header> - dealt with by the mac80211 stack, not the driver
+ * <data>
+ *
+ * TX packets are similar:
+ * <custom bus protocol header(s)>
+ * <tx_frame_hdr> - 4 bytes
+ * <plcp; either psk_cck_hdr or ofdm_hdr> - 4 bytes
+ * <mac header>
+ * <data>
+ */
+
+
+
+// MAC type field values
+#define	TYPE_ASSOC_REQ		0x00	// Association request
+#define	TYPE_ASSOC_RESP		0x10	// Association response
+#define	TYPE_REASSOC_REQ	0x20	// Reassociation request
+#define	TYPE_REASSOC_RESP	0x30	// Reassociation response
+#define	TYPE_PROBE_REQ		0x40	// Probe request
+#define	TYPE_PROBE_RESP		0x50	// Probe response
+
+#define	TYPE_BEACON		0x80	// Beacon
+#define	TYPE_ATIM		0x90	// Annoucement traffice indication
+#define	TYPE_DISASSOC		0xa0	// Disassociation
+#define	TYPE_AUTH		0xb0	// Authentication
+#define	TYPE_DEAUTH		0xc0	// Deauthentication
+#define TYPE_ACTION		0xd0    // Action
+
+#define TYPE_RTS		0xb4	// Request to send
+#define TYPE_CTS		0xc4	// Clear to send
+#define TYPE_ACK		0xd4	// Acknowledgement
+#define TYPE_PSPOLL		0xa4	// Power Save(PS)-Poll
+
+#define TYPE_DATA		0x08	// Data
+#define TYPE_NULL_DATA		0x48    // Null Data
+
+struct tx_frame_hdr {
+#if 1
+	unsigned int modulation_type:8;
+	unsigned int length:9;
+	unsigned int pad:15;
+#else
+	uint8_t modulation_type;
+	__le16 length:9;
+	unsigned int pad:15;
+#endif
+} __attribute__((packed));
+
+
+#define MOD_TYPE_PSKCCK		0x00
+#define MOD_TYPE_OFDM		0xee
+
+struct psk_cck_hdr {
+	uint8_t signal;		/* x100Kbps */
+	uint8_t service;
+	__le16 length;		/* usecs */
+} __attribute__((packed));
+
+/* PSK/CCK PLCP service field bits */
+#define PLCP_SERVICE_LOCKED	0x04	/* locked clocks */
+#define PLCP_SERVICE_MODSEL	0x08	/* modulation selection */
+#define PLCP_SERVICE_LENEXT	0x80	/* length extension */
+
+struct ofdm_hdr {
+	unsigned int rate:4;
+	unsigned int pad_a:1;
+	unsigned int length:12;	/* in bytes */
+	unsigned int parity:1;
+	unsigned int pad_b:14;
+} __attribute__((packed));
+
+
+struct rx_frame_hdr {
+	uint8_t modulation_type;
+	unsigned int rssi_variable_gain_attenuator:5;
+	unsigned int rssi_low_noise_amp:2;
+	unsigned int antenna:1;
+	__be16 freq_offset;
+	union
+	{
+	    struct psk_cck_hdr psk;
+	    struct ofdm_hdr ofdm;
+	} mod;
+} __attribute__((packed));
+
+typedef struct
+{
+	unsigned type		:8;		// Type, subtype, version
+	unsigned toDS		:1;		// To distribution service (AP)
+	unsigned fromDS		:1;		// From distribution service (AP)
+	unsigned moreFrag	:1;		// More fragments
+	unsigned retry		:1;		// Retransmission
+	unsigned pwrMgt		:1;		// Power management state
+	unsigned moreData	:1;		// More data buffered
+	unsigned protectd	:1;		// Encrypted
+	unsigned order		:1;		// Strictly ordered
+} frameControlFieldType_t;
+
+#define WLN_ADDR_SIZE       (6)
+
+typedef unsigned char MacAddr[WLN_ADDR_SIZE];
+
+
+#define	PACKED_H
+#define	PACKED_F	__attribute__ ((packed))
+
+typedef PACKED_H struct {
+	unsigned type		:8;		// Type, subtype, version
+#ifdef BIG_ENDIAN
+	unsigned order		:1;		// Strictly ordered
+	unsigned protected	:1;		// Encrypted
+	unsigned moreData	:1;		// More data buffered
+	unsigned pwrMgt		:1;		// Power management state
+	unsigned retry		:1;		// Retransmission
+	unsigned moreFrag	:1;		// More fragments
+	unsigned fromDS		:1;		// From distribution service (AP)
+	unsigned toDS		:1;		// To distribution service (AP)
+#else
+	unsigned toDS		:1;		// To distribution service (AP)
+	unsigned fromDS		:1;		// From distribution service (AP)
+	unsigned moreFrag	:1;		// More fragments
+	unsigned retry		:1;		// Retransmission
+	unsigned pwrMgt		:1;		// Power management state
+	unsigned moreData	:1;		// More data buffered
+	unsigned protected	:1;		// Encrypted
+	unsigned order		:1;		// Strictly ordered
+#endif
+} PACKED_F FrameControl_t;
+
+
+// Sequence control field
+// Need to swap bytes on BIG_ENDIAN
+typedef PACKED_H struct {
+#ifdef BIG_ENDIAN
+	unsigned seq		:12;	// Sequence number
+	unsigned frag		:4;		// Fragment number
+#else
+	unsigned frag		:4;		// Fragment number
+	unsigned seq		:12;	// Sequence number
+#endif
+} PACKED_F SeqControl;
+
+// Union of sequence control types
+typedef PACKED_H union {
+	SeqControl sq;				// Sequence control fields
+	unsigned short sq16;	    // Sequence control as 16-bit int (needs byte swap)
+} PACKED_F SeqControlU;
+
+
+
+typedef PACKED_H struct {
+	FrameControl_t fc;			// Frame control
+	unsigned short	duration;   // Duration/ID (needs byte swap)
+	MacAddr	addr1;				// Address 1
+	MacAddr	addr2;				// Address 2
+	MacAddr	addr3;				// Address 3
+	SeqControlU squ;			// Sequence control fields
+} PACKED_F _80211HeaderType;
+
+typedef PACKED_H struct {
+	FrameControl_t fc;			// Frame control
+	unsigned short aid;   		// association identifier
+	MacAddr	addr1;				// Address 1
+	MacAddr	addr2;				// Address 2
+} PACKED_F _80211PSPollType;
+
+#define _80211_HEADER_LENGTH	(sizeof(_80211HeaderType))
+#define TX_HEADER_LENGTH	(sizeof(struct ofdm_hdr) + sizeof(struct tx_frame_hdr))
+/* FIFO sizes in bytes */
+#define TX_FIFO_SIZE            1792
+#define RX_FIFO_SIZE            2048
+
+#define RATE_MASK_BASIC		0x0153
+#define RATE_MASK_OFDM		0x0ff0
+#define RATE_MASK_PSK_CCK	0x000f
+
+#define BEACON_INT		100	/* in TU */
+
+
+#define	DEFAULT_CW_MIN		32			// Min contention window size
+#define	DEFAULT_CW_MAX		1024		// Max contention window size
+
+#define ASLOT_TIME		20
+#endif
diff --git a/drivers/net/wireless/digiPiper/phy.c b/drivers/net/wireless/digiPiper/phy.c
new file mode 100644
index 000000000000..5a6c53638e81
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/phy.c
@@ -0,0 +1,269 @@
+/*
+ * Linux device driver for Digi's WiWave WLAN card
+ *
+ * Copyright © 2008  Digi International, Inc
+ *
+ * Author: Andres Salomon <dilinger@debian.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/module.h>
+#include <net/mac80211.h>
+
+#include "pipermain.h"
+#include "mac.h"
+#include "airoha.h"
+#include "phy.h"
+
+#define PHY_DEBUG	(0)
+
+#if PHY_DEBUG
+static int dlevel = DVERBOSE;
+#define dprintk(level, fmt, arg...)	if (level >= dlevel)			\
+					printk(KERN_ERR PIPER_DRIVER_NAME	\
+					    ": %s - " fmt, __func__, ##arg)
+#else
+#define dprintk(level, fmt, arg...)	do {} while (0)
+#endif
+
+#define NUMBER_OF_WORD32(x)     ((x + 3) >> 2)
+
+static int is_ofdm_rate(int rate)
+{
+	return (rate % 3) == 0;
+}
+
+void phy_set_plcp(unsigned char *frame, unsigned length, struct ieee80211_rate *rate, int aes_len)
+{
+	int ofdm = is_ofdm_rate(rate->bitrate);
+	int plcp_len = length + FCS_LEN + aes_len;
+	struct tx_frame_hdr *hdr;
+
+	if (ofdm) {
+		/* OFDM header */
+		struct ofdm_hdr *ofdm;
+
+		ofdm = (struct ofdm_hdr *) &frame[sizeof(struct tx_frame_hdr)];
+		memset(ofdm, 0, sizeof(*ofdm));
+		ofdm->rate = rate->hw_value;
+		ofdm->length = cpu_to_le16(plcp_len);
+	} else {
+		/* PSK/CCK header */
+		struct psk_cck_hdr *pskcck;
+		int us_len;
+
+		pskcck = (struct psk_cck_hdr *) &frame[sizeof(struct tx_frame_hdr)];
+		pskcck->signal = rate->bitrate;
+		pskcck->service = PLCP_SERVICE_LOCKED;
+
+		/* convert length from bytes to usecs */
+		switch (rate->bitrate) {
+		case 10:
+			us_len = plcp_len * 8;
+			break;
+		case 20:
+			us_len = plcp_len * 4;
+			break;
+		case 55:
+			us_len = (16 * plcp_len + 10) / 11;
+			break;
+		case 110:
+			us_len = (8 * plcp_len + 10) / 11;
+
+			/* set length extension bit if needed */
+			dprintk(DALL, "us_len = %d, plcp_len = %d, (11 * us_len) = %d, \
+ 				(11 * us_len) / 8 = %d\n", us_len, plcp_len,
+				(11 * us_len), (11 * us_len) / 8);
+
+			if ((11 * us_len) / 8 > plcp_len) {
+				pskcck->service |= PLCP_SERVICE_LENEXT;
+				dprintk(DALL, "Set PLCP_SERVICE_LENEXT, \
+				    pskcck->service = 0x%4.4X\n", pskcck->service);
+			} else {
+				dprintk(DALL, "Did not set PLCP_SERVICE_LENEXT, \
+				  pskcck->service = 0x%4.4X\n", pskcck->service);
+			}
+			break;
+		default:
+			digi_dbg("rate = %p, rate->bitrate%d\n", rate, rate->bitrate);
+			WARN_ON(1);
+			us_len = 0;
+		}
+
+		pskcck->length = cpu_to_le16(us_len);
+
+		dprintk(DALL, "pskcck .length = %d, signal = %d, service = %d\n",
+			pskcck->length, pskcck->signal, pskcck->service);
+		dprintk(DALL, "rate->bitrate=%x (@%dM), pckcck->length=%d\n",
+			rate->bitrate, rate->bitrate/10, pskcck->length);
+	}
+
+	hdr = (struct tx_frame_hdr *) frame;
+	hdr->pad = 0;
+	hdr->length = NUMBER_OF_WORD32((length + aes_len + TX_HEADER_LENGTH));
+	hdr->modulation_type = ofdm ? MOD_TYPE_OFDM : MOD_TYPE_PSKCCK;
+
+	dprintk(DVVERBOSE, "frame hdr .length = %d, .modulation_type = %d\n",
+		hdr->length, hdr->modulation_type);
+
+	dprintk(DVERBOSE, "TX: %d byte %s packet @ %dmbit\n", length,
+		ofdm ? "OFDM" : "PSK/CCK", rate->bitrate/10);
+}
+EXPORT_SYMBOL_GPL(phy_set_plcp);
+
+
+static int get_signal(struct rx_frame_hdr *hdr, enum ieee80211_band rf_band, int transceiver)
+{
+    int gain;
+    int signal;
+
+    if (transceiver == RF_AIROHA_2236) {
+        const u8 lnaTable_al2236[] =
+        {
+            0, 0, 20, 36
+        };
+
+        // Map high gain values to dbm
+        const signed char gainTable_al2236[] =
+        {
+           -85, -85, -88, -88, -92
+        };
+        // Convert received signal strength to dbm
+        gain = lnaTable_al2236[hdr->rssi_low_noise_amp] + 2*hdr->rssi_variable_gain_attenuator;
+        if (gain > 92)
+            signal = -96;
+        else if (gain > 87)
+            signal = gainTable_al2236[gain - 88];
+        else
+            signal = 4 - gain;
+    } else {
+        static const u8 lnaTable_al7230_24ghz[] =
+        {
+            0, 0, 18, 42
+        };
+        static const u8 lnaTable_al7230_50ghz[] =
+        {
+            0, 0, 17, 37
+        };
+        /* Convert received signal strength to dbm for RF_AIROHA_7230 */
+        if (rf_band == IEEE80211_BAND_2GHZ) {
+            gain = lnaTable_al7230_24ghz[hdr->rssi_low_noise_amp] + 2*hdr->rssi_variable_gain_attenuator;
+            signal = 2 - gain;
+        } else {
+            gain = lnaTable_al7230_50ghz[hdr->rssi_low_noise_amp] + 2*hdr->rssi_variable_gain_attenuator;
+            signal = -5 - gain;
+        }
+    }
+
+    return signal;
+}
+
+
+
+
+/* FIXME, following limtis should depend on the platform */
+#define PIPER_MAX_SIGNAL_DBM		(-30)
+#define PIPER_MIN_SIGNAL_DBM		(-90)
+
+static int calculate_link_quality(int signal)
+{
+	int quality;
+
+	if (signal < PIPER_MIN_SIGNAL_DBM)
+		quality = 0;
+	else if (signal > PIPER_MAX_SIGNAL_DBM)
+		quality = 100;
+	else
+		quality = (signal - PIPER_MIN_SIGNAL_DBM) * 100 /
+			  (PIPER_MAX_SIGNAL_DBM - PIPER_MIN_SIGNAL_DBM);
+
+	dprintk(DVERBOSE, "signal: %d, quality: %d/100\n", signal, quality);
+
+	return quality;
+}
+
+unsigned int phy_determine_rssi(struct rx_frame_hdr *hdr)
+{
+	return (hdr->rssi_low_noise_amp << 5) | hdr->rssi_variable_gain_attenuator;
+}
+EXPORT_SYMBOL_GPL(phy_determine_rssi);
+
+
+
+void phy_process_plcp(struct piper_priv *piper, struct rx_frame_hdr *hdr,
+		struct ieee80211_rx_status *status, unsigned int *length)
+{
+	unsigned rate, i;
+	struct digi_rf_ops *rf = piper->rf;
+
+	memset(status, 0, sizeof(*status));
+	status->band = piper->rf->getBand(piper->channel);
+	status->signal = get_signal(hdr, status->band, piper->pdata->rf_transceiver);
+	status->antenna = hdr->antenna;
+	status->freq = piper->rf->getFrequency(piper->channel);
+	status->qual = calculate_link_quality(status->signal);
+
+	if (hdr->modulation_type == MOD_TYPE_OFDM) {
+		/* OFDM */
+		struct ofdm_hdr *ofdm = &hdr->mod.ofdm;
+		const int ofdm_rate[] = {
+			480, 240, 120, 60, 540, 360, 180, 90
+		};
+
+		rate = ofdm_rate[ofdm->rate & 0x7];
+		*length = le16_to_cpu(ofdm->length);
+		dprintk(DVVERBOSE, "%d byte OFDM packet @ %dmbit\n",
+			*length, rate/10);
+	} else {
+		/* PSK/CCK */
+		struct psk_cck_hdr *pskcck = &hdr->mod.psk;
+
+		rate = pskcck->signal;
+
+		*length = le16_to_cpu(pskcck->length);
+		switch (rate) {
+		case 10:
+			*length /= 8;
+			break;
+		case 20:
+			*length /= 4;
+			break;
+		case 55:
+			*length = (11 * (*length)) / 16;
+			break;
+		case 110:
+			*length = (11 * (*length)) / 8;
+			if (pskcck->service & PLCP_SERVICE_LENEXT)
+				(*length)--;
+			break;
+		default:
+			/* WARN_ON(1); This happens to often for us to generate that long error message */
+			*length = 0;
+		}
+
+		dprintk(DVVERBOSE, "%d byte PSK/CCK packet @ %dmbit\n",
+			*length, rate/10);
+	}
+
+	/* match rate with the list of bitrates that we supplied the stack */
+	for (i = 0; i < rf->bands[status->band].n_bitrates; i++) {
+		if (rf->bands[status->band].bitrates[i].bitrate == rate)
+			break;
+	}
+
+	if (i != rf->bands[status->band].n_bitrates)
+		status->rate_idx = i;
+	else {
+	    *length = 0;
+		digi_dbg(PIPER_DRIVER_NAME
+				": couldn't find bitrate index for %d?\n",
+				rate);
+		status->flag |= RX_FLAG_FAILED_PLCP_CRC;
+		return;
+	}
+}
+EXPORT_SYMBOL_GPL(phy_process_plcp);
diff --git a/drivers/net/wireless/digiPiper/phy.h b/drivers/net/wireless/digiPiper/phy.h
new file mode 100644
index 000000000000..96500ab92e3e
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/phy.h
@@ -0,0 +1,28 @@
+/*
+ * Linux device driver for Digi's WiWave WLAN card
+ *
+ * Copyright © 2008  Digi International, Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef phy_h_
+#define phy_h_
+
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/module.h>
+#include <net/mac80211.h>
+
+
+#include "pipermain.h"
+#include "mac.h"
+
+void phy_set_plcp(unsigned char *frame, unsigned length, struct ieee80211_rate *rate, int aes_len);
+void phy_process_plcp(struct piper_priv *piper, struct rx_frame_hdr *hdr,
+		struct ieee80211_rx_status *status, unsigned int *length);
+unsigned int phy_determine_rssi(struct rx_frame_hdr *hdr);
+
+#endif
diff --git a/drivers/net/wireless/digiPiper/piper.c b/drivers/net/wireless/digiPiper/piper.c
new file mode 100644
index 000000000000..a3c24720dd11
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/piper.c
@@ -0,0 +1,1032 @@
+/*
+ * piper.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <net/mac80211.h>
+#include <linux/usb.h>
+#include <linux/kthread.h>
+#include <linux/platform_device.h>
+#include <asm/gpio.h>
+#include <linux/timer.h>
+
+#include "pipermain.h"
+#include "mac.h"
+#include "phy.h"
+#include "airoha.h"
+#include "airohaCalibration.h"
+#include "piperDsp.h"
+#include "piperMacAssist.h"
+#include "digiPs.h"
+
+#define WANT_AIROHA_CALIBRATION     (1)
+#define WANT_DEBUG_COMMANDS			(1)
+
+
+static void piper_clear_irq_mask(struct piper_priv *piperp, unsigned int bits)
+{
+	piperp->ac->wr_reg(piperp, BB_IRQ_MASK, ~bits, op_and);
+}
+
+static void piper_set_irq_mask(struct piper_priv *piperp, unsigned int bits)
+{
+	piperp->ac->wr_reg(piperp, BB_IRQ_MASK, bits, op_or);
+}
+
+/* Generate a random number */
+static int local_rand(void)
+{
+	static unsigned long next = 1;
+
+	/* RAND_MAX assumed to be 32767 */
+	next = next * 1103515245 + 12345;
+	return((unsigned)(next/65536) % 32768);
+}
+
+/*
+ * Load the MAC Assist firmware into the chip. This is done by setting a bit
+ * in the control register to enable MAC Assist firmware download, and then
+ * writing the firmware into the data FIFO.
+ */
+void piper_load_mac_firmware(struct piper_priv *piperp)
+{
+	unsigned int i;
+
+	printk(KERN_DEBUG PIPER_DRIVER_NAME ": loading MAC Assist firmware\n");
+
+	/* Zero out MAC assist SRAM (put into known state before enabling MAC assist) */
+	for (i = 0; i < 0x100; i += 4)
+		piperp->ac->wr_reg(piperp, i, 0, op_write);
+
+	/* Enable download the MAC Assist program RAM */
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, BB_GENERAL_CTL_FW_LOAD_ENABLE, op_or);
+
+	/* load MAC Assist data */
+	for (i = 0; i < piper_macassist_data_len; i++)
+		piperp->ac->wr_reg(piperp, BB_DATA_FIFO, piper_wifi_macassist_ucode[i],
+				   op_write);
+
+	/* disable MAC Assist download */
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, ~BB_GENERAL_CTL_FW_LOAD_ENABLE, op_and);
+}
+
+/*
+ * Load the DSP firmware into the chip.  This is done by setting a bit
+ * in the control register to enable DSP firmware download, and then
+ * writing the firmware into the data FIFO.
+ */
+void piper_load_dsp_firmware(struct piper_priv *piperp)
+{
+	unsigned int i;
+
+	printk(KERN_DEBUG PIPER_DRIVER_NAME ": loading DSP firmware\n");
+
+	/* Enable load of DSP firmware */
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, BB_GENERAL_CTL_DSP_LOAD_ENABLE, op_or);
+
+	/* load DSP data */
+	for (i = 0; i < piper_dsp_data_len; i++)
+		piperp->ac->wr_reg(piperp, BB_DATA_FIFO, piper_wifi_dsp_ucode[i],
+				   op_write);
+
+	/* Disable load of DSP firmware */
+	udelay(10);
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, ~BB_GENERAL_CTL_DSP_LOAD_ENABLE, op_and);
+
+	/* Let her rip */
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, BB_GENERAL_CTL_MAC_ASSIST_ENABLE, op_or);
+}
+
+
+/*
+ * This routine corrects a bug in the Piper chip where internal clocks would
+ * be out of sync with each other and cause the chip to generate noise spikes.
+ * This problem should be fixed in the next chip (Chopper).
+ *
+ * I'm not sure exactly what this code is doing.  It comes straight from the
+ * guy who designed the chip.
+ */
+int piper_spike_suppression(struct piper_priv *piperp, bool retry)
+{
+	int timeout1 = 300, timeout2 = 300;
+	int ret = 0;
+
+	/*
+	 * Initial timing measurement to avoid spike
+	 * The new "magic" value is 0x63 at address 0xA62.  Bit-0 indicates the
+	 * timing measurement is complete.  Bit-1 indicates that a second timing
+	 * measurment was performed.  The upper nibble is the timing measurement
+	 * value. This code should eliminate the possibility of spikes at the
+	 * beginning of all PSK/CCK frames and eliminate the spikes at the end of
+	 * all PSK (1M, 2M) frames.
+	 */
+
+	/* reset the timing value */
+	piperp->ac->wr_reg(piperp, MAC_STATUS, 0xffff00ff, op_and);
+
+	while ((piperp->ac->rd_reg(piperp, MAC_STATUS) & 0x0000ff00) != 0x00006300) {
+
+		/* reset the timing value */
+		piperp->ac->wr_reg(piperp, MAC_STATUS, 0xffff00ff, op_and);
+
+		/* issue WiFi soft reset */
+		piperp->ac->wr_reg(piperp, BB_GENERAL_STAT, 0x40000000, op_write);
+
+		/* Set TX_ON Low */
+		piperp->ac->wr_reg(piperp, BB_OUTPUT_CONTROL, 0xffffff3f, op_and);
+		piperp->ac->wr_reg(piperp, BB_OUTPUT_CONTROL, 0x00000080, op_or);
+
+		/* Set PA_2G Low */
+		piperp->ac->wr_reg(piperp, BB_OUTPUT_CONTROL, 0xfffff0ff, op_and);
+		piperp->ac->wr_reg(piperp, BB_OUTPUT_CONTROL, 0x00000a00, op_or);
+
+		/* Set RX_ON low  */
+		piperp->ac->wr_reg(piperp, BB_OUTPUT_CONTROL, 0xcfffffff, op_and);
+		piperp->ac->wr_reg(piperp, BB_OUTPUT_CONTROL, 0x20000000, op_or);
+
+		/* start the WiFi mac & dsp */
+		piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, 0x37720820, op_write);
+		timeout1 = 500;
+
+		/* Wait for timing measurement to finish */
+		while ((piperp->ac->rd_reg(piperp, MAC_STATUS) & 0x0000ff00) != 0x00000100) {
+			udelay(2);
+			timeout1--;
+			if (!timeout1)
+				break;
+		}
+
+		timeout2--;
+		if (!timeout2) {
+			ret = -EIO;
+			break;
+		}
+
+		if (!retry) {
+			ret = -EIO;
+			break;
+		}
+	}
+
+	/* Set TX_ON/RXHP_ON and RX to normal wifi, restore the reset value to HW_OUT_CTRL */
+	piperp->ac->wr_reg(piperp, BB_OUTPUT_CONTROL, 0x1, op_write);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(piper_spike_suppression);
+
+void piper_reset_mac(struct piper_priv *piperp)
+{
+	int i;
+
+	/* set the TX-hold bit */
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, 0x37720080, op_write);
+
+	/* clear the TX-FIFO memory */
+	for (i = 0; i < 448; i++)
+		piperp->ac->wr_reg(piperp, BB_DATA_FIFO, 0, op_write);
+
+	/* reset the TX-FIFO */
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, 0x377200C0, op_write);
+
+	/* release the TX-hold and reset */
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, 0x37720000, op_write);
+
+/*	iowrite32(ioread32(piperp->vbase + MAC_STATUS) & ~0x40000000,
+		  piperp->vbase + BB_GENERAL_STAT);*/
+	mdelay(1);
+}
+
+/*
+ * Load the MAC address into the chip. Use the value stored in the
+ * environment, if there is one, otherwise use the default value.
+ */
+void piper_set_macaddr(struct piper_priv *piperp)
+{
+	/* Default MAC Addr used if the nvram parameters are corrupted */
+	u8 mac[6] = {0x00, 0x04, 0xf3, 0x11, 0x43, 0x35};
+	u8 *pmac = piperp->pdata->macaddr;
+	int i;
+    static bool firstTime = true;
+
+	for (i = 0; i < 6; i++) {
+		if (*(pmac + i) != 0xff)
+			break;
+		if (i == 5) {
+			/* There is a problem with the parameters, use default */
+			if (firstTime) {
+    			printk(KERN_INFO PIPER_DRIVER_NAME
+    				": invalid mac address, using default\n");
+    		}
+			memcpy(piperp->pdata->macaddr, mac, sizeof(piperp->pdata->macaddr));
+		}
+	}
+
+	firstTime = false;
+	memcpy(piperp->hw->wiphy->perm_addr, piperp->pdata->macaddr,
+	       sizeof(piperp->hw->wiphy->perm_addr));
+
+	/* configure ethernet address */
+	piperp->ac->wr_reg(piperp, MAC_STA_ID0, *(pmac + 3) | *(pmac + 2) << 8 |
+			   *(pmac + 1) << 16 | *(pmac + 0) << 24, op_write);
+	piperp->ac->wr_reg(piperp, MAC_STA_ID1, *(pmac + 5) << 16 | *(pmac + 4) << 24,
+			   op_write);
+}
+EXPORT_SYMBOL_GPL(piper_set_macaddr);
+
+
+/* Configure the H/W with the antenna settings */
+static int piper_set_antenna(struct piper_priv *piperp, enum antenna_select sel)
+{
+	if (sel == ANTENNA_BOTH) {
+		piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+				   BB_GENERAL_CTL_ANT_DIV, op_or);
+		piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+				   ~BB_GENERAL_CTL_ANT_SEL, op_and);
+	} else {
+		piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+				   ~BB_GENERAL_CTL_ANT_DIV, op_and);
+		/* select the antenna if !diversity */
+		if (sel == ANTENNA_1)
+			piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+					   ~BB_GENERAL_CTL_ANT_SEL, op_and);
+		else
+			piperp->ac->wr_reg(piperp, BB_GENERAL_CTL,
+					   BB_GENERAL_CTL_ANT_SEL, op_or);
+	}
+
+	/* select which antenna to transmit on */
+	piperp->ac->wr_reg(piperp, BB_RSSI, ~BB_RSSI_ANT_MASK, op_and);
+	if (sel == ANTENNA_BOTH)
+		piperp->ac->wr_reg(piperp, BB_RSSI, BB_RSSI_ANT_DIV_MAP, op_or);
+	else
+		piperp->ac->wr_reg(piperp, BB_RSSI, BB_RSSI_ANT_NO_DIV_MAP, op_or);
+
+	return 0;
+}
+
+/*
+ * Compute a beacon backoff time as described in section 11.1.2.2 of 802.11 spec.
+ *
+ */
+static u16 get_next_beacon_backoff(void)
+{
+#define MAX_BEACON_BACKOFF      (2 * ASLOT_TIME * DEFAULT_CW_MIN)
+
+	/*
+	* We shift the result of local_rand() by 4 bits because the notes
+	* for the algorithm say that we shouldn't rely on the last few
+	* bits being random.  Other than that, we just take the random
+	* value and make sure it is less than MAX_BEACON_BACKOFF.
+	*/
+	return (local_rand() >> 4) % MAX_BEACON_BACKOFF;
+}
+
+static int load_beacon(struct piper_priv *digi, unsigned char *buffer,
+                        unsigned int length)
+{
+    return digi->ac->wr_fifo(digi, BEACON_FIFO, buffer, length);
+}
+
+static int piper_init_rx_tx(struct piper_priv *piperp)
+{
+	tasklet_init(&piperp->rx_tasklet, piper_rx_tasklet, (unsigned long)piperp);
+	tasklet_disable(&piperp->rx_tasklet);
+	piperp->expectingAck = false;
+
+	spin_lock_init(&piperp->tx_tasklet_lock);
+	spin_lock_init(&piperp->tx_queue_lock);
+	piperp->tx_tasklet_running = false;
+    memset(&piperp->tx_queue, 0, sizeof(piperp->tx_queue));
+    piperp->tx_queue_head = 0;
+    piperp->tx_queue_tail = 0;
+    piperp->tx_queue_count = 0;
+	tasklet_init(&piperp->tx_tasklet, piper_tx_tasklet, (unsigned long)piperp);
+	tasklet_disable(&piperp->tx_tasklet);
+
+	return 0;
+}
+
+static void piper_free_rx_tx(struct piper_priv *piperp)
+{
+	tasklet_disable(&piperp->rx_tasklet);
+	tasklet_kill(&piperp->rx_tasklet);
+	tasklet_disable(&piperp->tx_tasklet);
+	tasklet_kill(&piperp->tx_tasklet);
+	piper_empty_tx_queue(piperp);
+}
+
+/*
+ * This function sets the tracking control according to a channel's
+ * frequency.
+ */
+static int piper_set_tracking_constant(struct piper_priv *piperp, unsigned megahertz)
+{
+	piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, ~TRACK_CONSTANT_MASK, op_and);
+	if (megahertz < 4920)
+	{
+		piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, TRACK_BG_BAND, op_or);
+	}
+	else if (megahertz <= 4980)
+	{
+		piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, TRACK_4920_4980_A_BAND, op_or);
+	}
+	else if (megahertz <= 5350)
+	{
+		piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, TRACK_5150_5350_A_BAND, op_or);
+	}
+	else if (megahertz <= 5725)
+	{
+		piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, TRACK_5470_5725_A_BAND, op_or);
+	}
+	else
+	{
+		piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, TRACK_5725_5825_A_BAND, op_or);
+	}
+
+	return 0;
+}
+
+/*
+ * This function is called to set the value of the B_TX_GAIN field of the
+ * HW_CONF1 mac register. This register must be set to different values depending
+ * on the H/W revision of the board due to changes in the board design.
+ */
+static unsigned int get_b_tx_gain(struct piper_priv *piperp)
+{
+	u16 platform = piperp->pdata->wcd.header.hw_platform & WCD_PLATFORM_MASK;
+	u16 hw_revision = piperp->pdata->wcd.header.hw_platform & WCD_HW_REV_MASK;
+	unsigned int tx_gain = 0;
+
+	switch (platform) {
+		case WCD_CCW9P_PLATFORM:
+			tx_gain = TRACK_TX_B_GAIN_NORMAL;
+			break;
+		case WCD_CCW9M_PLATFORM:
+			switch (hw_revision) {
+				case WCD_HW_REV_PROTOTYPE:
+				case WCD_HW_REV_PILOT:
+					tx_gain = 0xc0000000;
+					break;
+				case WCD_HW_REV_A:
+				default:
+					tx_gain = 0x90000000;
+					break;
+			}
+			break;
+	}
+	return tx_gain;
+}
+
+
+static int piper_init_hw(struct piper_priv *piperp, enum ieee80211_band band)
+{
+	int ret = 0;
+
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, BB_GENERAL_CTL_INIT, op_write);
+
+	/* Initialize baseband general control register for the specific transceiver */
+	if (piperp->pdata->rf_transceiver == RF_AIROHA_7230) {
+		if (band == IEEE80211_BAND_2GHZ) {
+			piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, GEN_INIT_AIROHA_24GHZ, op_write);
+			piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, 0xff00ffff, op_and);
+			piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, TRACK_BG_BAND, op_or);
+			digi_dbg("piper_init_hw Initialized for band B / BG\n");
+		} else {
+			piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, GEN_INIT_AIROHA_50GHZ, op_write);
+			piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, 0xff00ffff, op_and);
+			piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, TRACK_5150_5350_A_BAND, op_or);
+			digi_dbg("piper_init_hw Initialized for band A\n");
+		}
+		piperp->ac->wr_reg(piperp, BB_CONF_2, 0x09325ad4, op_write);
+		/* Initialize the SPI word length */
+		piperp->ac->wr_reg(piperp, BB_SPI_CTRL, SPI_INIT_AIROHA, op_write);
+	} else if (piperp->pdata->rf_transceiver == RF_AIROHA_2236) {
+		piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, GEN_INIT_AIROHA_24GHZ, op_write);
+		piperp->ac->wr_reg(piperp, BB_CONF_2, 0x09325ad4, op_write);
+		piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, 0xff00ffff, op_and);
+		piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, TRACK_BG_BAND, op_or);
+		/* Initialize the SPI word length */
+		piperp->ac->wr_reg(piperp, BB_SPI_CTRL, SPI_INIT_AIROHA2236, op_write);
+	} else {
+		printk(KERN_WARNING PIPER_DRIVER_NAME ": undefined rf transceiver!\n");
+		return -EINVAL;
+	}
+
+	/*
+	 *Clear the Intretupt Mask Register before enabling external intretupts.
+	 * Also clear out any status bits in the Intretupt Status Register.
+	 */
+	piperp->ac->wr_reg(piperp, BB_IRQ_MASK, 0, op_write);
+	piperp->ac->wr_reg(piperp, BB_IRQ_STAT, 0xff, op_write);
+
+	/*
+	 * If this firmware supports additional MAC addresses.
+	 */
+	if (((piperp->ac->rd_reg(piperp, MAC_STATUS) >> 16) & 0xff) >= 8) {
+		/* Disable additional addresses to start with */
+		piperp->ac->wr_reg(piperp, MAC_CTL, ~MAC_CTL_MAC_FLTR, op_and);
+		piperp->ac->wr_reg(piperp, MAC_STA2_ID0, 0, op_write);
+		piperp->ac->wr_reg(piperp, MAC_STA2_ID1, 0, op_write);
+		piperp->ac->wr_reg(piperp, MAC_STA3_ID0, 0, op_write);
+		piperp->ac->wr_reg(piperp, MAC_STA3_ID1, 0, op_write);
+	}
+	/* TODO:  Set this register programatically */
+	piperp->ac->wr_reg(piperp, MAC_DTIM_PERIOD, 0x0, op_write);
+
+	/*
+	 * Note that antenna diversity will be set by hw_start, which is the
+	 * caller of this function.
+	 */
+
+	/* reset RX and TX FIFOs */
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, BB_GENERAL_CTL_RXFIFORST
+			 | BB_GENERAL_CTL_TXFIFORST, op_or);
+	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, ~(BB_GENERAL_CTL_RXFIFORST
+						 | BB_GENERAL_CTL_TXFIFORST), op_and);
+
+	piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, 0xC043002C, op_write);
+	piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, ~TRACK_TX_B_GAIN_MASK, op_and);
+	piperp->ac->wr_reg(piperp, BB_TRACK_CONTROL, get_b_tx_gain(piperp), op_or);
+
+	/* Initialize RF transceiver */
+	piperp->rf->init(piperp->hw, band);
+	piperp->ac->wr_reg(piperp, BB_OUTPUT_CONTROL, 0x04000001, op_or);
+	piperp->ac->wr_reg(piperp, MAC_CFP_ATIM, 0x0, op_write);
+	piperp->ac->wr_reg(piperp, BB_GENERAL_STAT, ~(BB_GENERAL_STAT_DC_DIS
+						  | BB_GENERAL_STAT_SPRD_DIS), op_and);
+	piperp->ac->wr_reg(piperp, BB_GENERAL_STAT, ~(BB_GENERAL_STAT_SRC_DIS
+						  | BB_GENERAL_STAT_DLL_DIS), op_and);
+
+	piperp->ac->wr_reg(piperp, MAC_SSID_LEN, (MAC_OFDM_BRS_MASK | MAC_PSK_BRS_MASK),
+			   op_write);
+
+	/*
+	 * Set BSSID to the broadcast address so that we receive all packets.  The stack
+	 * will set a real BSSID when it's ready.
+	 */
+	piperp->ac->wr_reg(piperp, MAC_BSS_ID0, 0xffffffff, op_write);
+	piperp->ac->wr_reg(piperp, MAC_BSS_ID1, 0xffffffff, op_write);
+
+	piperp->ps.poweredDown = false;
+
+#if WANT_AIROHA_CALIBRATION
+	digi_dbg("Calling digiWifiInitCalibration()\n");
+	digiWifiInitCalibration(piperp);
+#endif
+
+	return ret;
+}
+
+static int piper_deinit_hw(struct piper_priv *piperp)
+{
+	int ret = 0;
+#if WANT_AIROHA_CALIBRATION
+	digi_dbg("Calling digiWifiDeInitCalibration()\n");
+	digiWifiDeInitCalibration(piperp);
+#endif
+
+	return ret;
+}
+
+
+static void adjust_max_agc(struct piper_priv *piperp, unsigned int rssi, _80211HeaderType *header)
+{
+#define LOWEST_MAXAGC_AL2236        0x76
+#define HIGHEST_MAXAGC_AL2236       0x7B
+#define HIGHEST_MAXAGC_AL7230_24GHZ       0x7c
+#define LOWEST_MAXAGC_AL7230_24GHZ        0x76
+#define HIGHEST_MAXAGC_AL7230_50GHZ       0x79
+#define LOWEST_MAXAGC_AL7230_50GHZ        0x73
+#define RSSI_AVG_COUNT  8
+
+    unsigned char maxgain = 0;
+    static unsigned char lowest = 0, highest = 0;
+    static int k=0, j=0, i =0, tempRssi=0;
+	static unsigned int savedRSSI[RSSI_AVG_COUNT]; /****/
+
+	savedRSSI[k % RSSI_AVG_COUNT] = rssi;
+	if (   (piperp->pdata->rf_transceiver == RF_AIROHA_2236)
+		|| (piperp->pdata->rf_transceiver == RF_AIROHA_7230)) {
+
+	    if (piperp->pdata->rf_transceiver == RF_AIROHA_2236)
+	    {
+	        lowest = LOWEST_MAXAGC_AL2236;
+	        highest = HIGHEST_MAXAGC_AL2236;
+	    }
+	    else
+	    {
+
+	        if (piperp->rf->getBand(piperp->channel) == IEEE80211_BAND_5GHZ) {
+	            highest = HIGHEST_MAXAGC_AL7230_50GHZ;
+	            lowest = LOWEST_MAXAGC_AL7230_50GHZ;
+	        }
+	        else {
+	            highest = HIGHEST_MAXAGC_AL7230_24GHZ;
+	            lowest = LOWEST_MAXAGC_AL7230_24GHZ;
+	        }
+	    }
+
+	    if (piperp->areWeAssociated)
+	    {
+
+	        if (   (piperp->if_type == NL80211_IFTYPE_ADHOC)
+	        	|| (piperp->if_type == NL80211_IFTYPE_MESH_POINT))
+	        {
+	            //Monitor the receive signal strength from Ad-hoc network
+	            if (memcmp (piperp->bssid, header->addr3, sizeof(piperp->bssid)) == 0)
+	            {
+	                /* we don't do avareging on all the signals here because it may come from different
+	                 * unit in that Ad-hoc network. Instead, we do avareging on the signals with higher rssi
+	                 */
+
+	                if ((rssi + 4) > lowest)
+	                {
+	                    k++;
+	                    tempRssi += rssi;
+
+	                    if (k >= RSSI_AVG_COUNT)
+	                    {
+	                        maxgain = (((tempRssi/k) + 4) > highest)? highest : ((tempRssi/k) + 4) ;
+	                        k = 0;
+	                        tempRssi = 0;
+	                        i =0;
+	                    }
+	                }
+	                else
+	                {
+	                    i++;
+	                    if (i >= (RSSI_AVG_COUNT*4))
+	                    {
+	                        maxgain = lowest;
+	                        i = 0;
+	                    }
+
+	                }
+	            }
+	        }
+	        else
+	        {
+	            //Monitor the receive signal strength from the frames we received from the associated AP
+	            if (memcmp (piperp->bssid, header->addr2, sizeof(piperp->bssid)) == 0)
+	            {
+	                //averaging all the signals because they come from the same AP
+	                k++;
+	                tempRssi += rssi;
+
+	                if (k >= RSSI_AVG_COUNT*2)
+	                {
+	                    if (((tempRssi/k) + 4) > lowest)
+	                        maxgain = (((tempRssi/k) + 4) > highest)? highest : ((tempRssi/k) + 4) ;
+	                    else
+	                        maxgain = lowest;
+
+	                    k = 0;
+	                    tempRssi = 0;
+	                }
+	            }
+	        }
+	        j = 0;
+	    }
+	    else
+	    {
+	        j++;
+	        if (j >= (RSSI_AVG_COUNT*4))
+	        {
+	            maxgain = highest;
+	            j = 0;
+	        }
+	        k = 0;
+	        tempRssi = 0;
+	    }
+
+	    if(   (maxgain != 0)
+	       && (maxgain != ((piperp->ac->rd_reg(piperp, BB_GENERAL_CTL) & BB_GENERAL_CTL_MAX_GAIN_MASK) >> 16)))
+	    {
+	    	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, ~BB_GENERAL_CTL_MAX_GAIN_MASK, op_and);
+	    	piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, (maxgain << 16) & BB_GENERAL_CTL_MAX_GAIN_MASK, op_or);
+	    }
+	}
+}
+
+
+/* Make sure all keys are disabled when we start */
+static void piper_init_keys(struct piper_priv *piperp)
+{
+	unsigned int i;
+
+	for (i = 0; i < PIPER_MAX_KEYS; i++)
+		piperp->key[i].valid = false;
+
+	piperp->aes_key_count = 0;
+}
+
+static void tx_timer_timeout(unsigned long arg)
+{
+	struct piper_priv *piperp = (struct piper_priv *) arg;
+
+	tasklet_hi_schedule(&piperp->tx_tasklet);
+}
+
+/* sysfs entries to get/set antenna mode */
+static ssize_t show_antenna_sel(struct device *dev,
+			 struct device_attribute *attr,
+			 char *buf)
+{
+	struct piper_priv *piperp = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%s\n", piperp->antenna == ANTENNA_BOTH ? "diversity" :
+		       piperp->antenna == ANTENNA_1 ? "primary" : "secondary");
+}
+
+static ssize_t store_antenna_sel(struct device *dev, struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct piper_priv *piperp = dev_get_drvdata(dev);
+	enum antenna_select ant;
+	size_t len = count;
+	int ret;
+
+	ant = piperp->antenna;
+
+	if (buf[count - 1] == '\n')
+		len--;
+
+	/* TODO check also string length */
+	if (!strncmp("diversity", buf, len))
+		ant = ANTENNA_BOTH;
+	else if (!strncmp("primary", buf, len))
+		ant = ANTENNA_1;
+	else if (!strncmp("secondary", buf, len))
+		ant = ANTENNA_2;
+
+	if (ant != piperp->antenna) {
+		if ((ret = piperp->set_antenna(piperp, ant)) != 0) {
+			printk(KERN_WARNING PIPER_DRIVER_NAME
+			       ": error setting antenna to %d (err: %d)\n", ant, ret);
+		} else
+			piperp->antenna = ant;
+	}
+
+	return count;
+}
+static DEVICE_ATTR(antenna_sel, S_IWUSR | S_IRUGO, show_antenna_sel, store_antenna_sel);
+
+static ssize_t show_power_duty(struct device *dev, struct device_attribute *attr,
+			       char *buf)
+{
+	struct piper_priv *piperp = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", piperp->power_duty);
+}
+
+static ssize_t store_power_duty(struct device *dev, struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+#define MINIMUM_DUTY_CYCLE	(33)
+#define LIMIT_LINEAL_DUTY_CYCLE	(75)
+	struct piper_priv *piperp = dev_get_drvdata(dev);
+	int pw_duty;
+	ssize_t ret = -EINVAL;
+
+	ret = sscanf(buf, "%d\n", &pw_duty);
+	if (ret > 0) {
+		if (pw_duty < MINIMUM_DUTY_CYCLE) {
+			piperp->power_duty = MINIMUM_DUTY_CYCLE;
+		} else if (pw_duty > LIMIT_LINEAL_DUTY_CYCLE && pw_duty < 100) {
+			piperp->power_duty = LIMIT_LINEAL_DUTY_CYCLE;
+		} else if (pw_duty == 100 ||
+			(pw_duty >= MINIMUM_DUTY_CYCLE && pw_duty <= LIMIT_LINEAL_DUTY_CYCLE)) {
+			piperp->power_duty = pw_duty;
+		}
+	}
+
+	return ret < 0 ? ret : count;
+}
+static DEVICE_ATTR(power_duty, S_IWUSR | S_IRUGO, show_power_duty, store_power_duty);
+
+#if WANT_DEBUG_COMMANDS
+
+static ssize_t show_debug_cmd(struct device *dev, struct device_attribute *attr,
+			       char *buf)
+{
+	struct piper_priv *piperp = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", piperp->debug_cmd);
+}
+
+static ssize_t store_debug_cmd(struct device *dev, struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	struct piper_priv *piperp = dev_get_drvdata(dev);
+	ssize_t ret = -EINVAL;
+
+	if (strlen(buf) < sizeof(piperp->debug_cmd))
+	{
+		if (strstr(buf, "dump") != NULL) {
+			digiWifiDumpRegisters(piperp, MAIN_REGS | MAC_REGS);
+			ret = 1;
+		} else if (strstr(buf, "ps_state") != NULL) {
+			printk(KERN_ERR "rxTaskletRunning = %d, allowTransmits = %d, stopped_tx_queues = %d\n",
+					piperp->ps.rxTaskletRunning, piperp->ps.allowTransmits, piperp->ps.stopped_tx_queues);
+			ret = 1;
+		} else if (strstr(buf, "rssi_dump") != NULL) {
+		    spinlock_t lock;
+		    unsigned long flags;
+            unsigned int rssi;
+
+	        spin_lock_init(&lock);
+	        spin_lock_irqsave(&piperp->ps.lock, flags);
+		    piperp->ac->wr_reg(piperp, BB_GENERAL_CTL, ~BB_GENERAL_CTL_RX_EN, op_and);
+		    udelay(15);
+		    rssi = piperp->ac->rd_reg(piperp, BB_RSSI);
+		    printk(KERN_ERR "\n**rssi = 0x%8.8X\n", rssi);
+			digiWifiDumpRegisters(piperp, MAIN_REGS | MAC_REGS);
+			ret = 1;
+			spin_unlock_irqrestore(&lock, flags);
+		} else {
+			strcpy(piperp->debug_cmd, buf);
+			piperp->debug_cmd[strlen(buf)-1] = 0;		/* truncate the \n */
+		}
+		ret = count;
+	}
+
+	return ret < 0 ? ret : count;
+}
+static DEVICE_ATTR(debug_cmd, S_IWUSR | S_IRUGO, show_debug_cmd, store_debug_cmd);
+
+#endif
+
+#ifdef CONFIG_PM
+
+static int piper_suspend(struct platform_device *dev, pm_message_t state)
+{
+	struct piper_priv *piperp = platform_get_drvdata(dev);
+	unsigned long flags;
+
+	/* TODO, use in future the ps.lock instead of fully disabling interrupts here */
+	piperp->power_save_was_on_when_suspended = (piperp->ps.mode == PS_MODE_LOW_POWER);
+	if (piperp->power_save_was_on_when_suspended)
+		piper_ps_set(piperp, false);
+	mdelay(10);
+	piper_sendNullDataFrame(piperp, true);
+	ssleep(1);
+
+	local_irq_save(flags);
+	/*
+	 * Save power save state and then make sure power save is turned off.
+	 */
+	piper_MacEnterSleepMode(piperp, true);
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static int piper_resume(struct platform_device *dev)
+{
+	struct piper_priv *piperp = platform_get_drvdata(dev);
+	unsigned long flags;
+
+	if (piperp->pdata->early_resume)
+		piperp->pdata->early_resume(piperp);
+
+	/* TODO, use in future the ps.lock instead of fully disabling interrupts here */
+	local_irq_save(flags);
+	piper_MacEnterActiveMode(piperp, true);
+	if (piperp->tx_tasklet_running) {
+	    tasklet_hi_schedule(&piperp->tx_tasklet);
+	} else {
+	    ieee80211_wake_queues(piperp->hw);
+	}
+	local_irq_restore(flags);
+
+	/*
+	 * Restore power save if it was on before
+	 */
+	if (piperp->power_save_was_on_when_suspended) {
+		piper_ps_set(piperp, true);
+	} else {
+		piper_sendNullDataFrame(piperp, false);
+	}
+
+	return 0;
+}
+#else
+#define piper_suspend	NULL
+#define piper_resume	NULL
+#endif
+
+static int __init piper_probe(struct platform_device* pdev)
+{
+	struct piper_pdata *pdata = pdev->dev.platform_data;
+	struct piper_priv *piperp;
+	int ret = 0;
+
+	if (!pdata)
+		return -EINVAL;
+
+	ret = piper_alloc_hw(&piperp, sizeof(*piperp));
+	if (ret) {
+		printk(KERN_ERR PIPER_DRIVER_NAME ": failed to alloc piper_priv\n");
+		return ret;
+	}
+
+	piperp->ac = kzalloc(sizeof(struct access_ops), GFP_KERNEL);
+	if (!piperp->ac){
+		printk(KERN_ERR PIPER_DRIVER_NAME ": failed to alloc memory for ac struct\n");
+		ret = -ENOMEM;
+		goto error_alloc;
+	}
+
+	piperp->drv_name = PIPER_DRIVER_NAME;
+	dev_set_drvdata(&pdev->dev, piperp);
+	piperp->pdata = pdata;
+	pdata->piperp = piperp;
+	spin_lock_init(&piperp->ac->reg_lock);
+	spin_lock_init(&piperp->aesLock);
+
+	piperp->vbase = ioremap(pdev->resource[0].start,
+				pdev[0].resource->end - pdev->resource[0].start);
+
+	if (!piperp->vbase) {
+		printk(KERN_ERR PIPER_DRIVER_NAME ": ioremap base %x, len %x error\n",
+		       pdev->resource[0].start,	pdev[0].resource->end - pdev->resource[0].start);
+		ret = -ENOMEM;
+		goto error_remap;
+	}
+
+	piperp->pstats.tx_start_count = 0;
+	piperp->pstats.tx_complete_count = 0;
+
+	/*
+	 * Platform initialization. This will initialize the hardware, including the load
+	 * of the mac and dsp firmware into the piper chip
+	 */
+	if (pdata->init) {
+		if ((ret = pdata->init(piperp)) != 0) {
+			printk(KERN_ERR PIPER_DRIVER_NAME
+				": platform init() returned error (%d)\n", ret);
+			goto error_init;
+		}
+	}
+
+	piper_ps_init(piperp);
+	init_timer(&piperp->tx_timer);
+	piperp->tx_timer.function = tx_timer_timeout;
+	piperp->tx_timer.data = (unsigned long) piperp;
+	piper_init_rx_tx(piperp);
+	piper_init_keys(piperp);
+
+	piperp->init_hw = piper_init_hw;
+	piperp->deinit_hw = piper_deinit_hw;
+	piperp->set_irq_mask_bit = piper_set_irq_mask;
+	piperp->clear_irq_mask_bit = piper_clear_irq_mask;
+	piperp->load_beacon = load_beacon;
+	piperp->rand = local_rand;
+	piperp->get_next_beacon_backoff = get_next_beacon_backoff;
+	piperp->set_antenna = piper_set_antenna;
+	piperp->set_tracking_constant = piper_set_tracking_constant;
+	piperp->antenna = ANTENNA_1;
+	piperp->adjust_max_agc = adjust_max_agc;
+
+	/*
+	 * Set the default duty cycle value.  Note that duty cycling
+	 * is disabled reguardless of what this variable is set to until
+	 * the user types "iwconfig wlan0 power on".  I just love the
+	 * "power on" syntax to turn *down* the power.
+	 */
+	piperp->power_duty = 100;
+
+	/* TODO this should be read earlier and actions should be taken
+	 * based on different revisions at driver initialization or runtime */
+	piperp->version = piperp->ac->rd_reg(piperp, BB_VERSION);
+
+	piperp->irq = pdev->resource[1].start;
+	piperp->tx_cts = false;
+	piperp->beacon.loaded = false;
+	piperp->beacon.enabled = false;
+	piperp->beacon.weSentLastOne = false;
+
+	ret = request_irq(piperp->irq, piper_irq_handler,
+			  IRQF_TRIGGER_HIGH, PIPER_DRIVER_NAME,	piperp);
+	if (ret) {
+		printk(KERN_ERR PIPER_DRIVER_NAME ": unable to request irq %d (%d)",
+			piperp->irq, ret);
+		goto retor_irq;
+	}
+
+	disable_irq(piperp->irq);
+
+	ret = piper_register_hw(piperp, &pdev->dev, &al7230_rf_ops);
+	if (ret) {
+		printk(KERN_ERR PIPER_DRIVER_NAME ": failed to register priv\n");
+		goto error_reg_hw;
+	}
+
+	if (pdata->late_init)
+		pdata->late_init(piperp);
+
+	ret = device_create_file(&pdev->dev, &dev_attr_antenna_sel);
+	if (ret) {
+		printk(KERN_ERR PIPER_DRIVER_NAME ": failed to create sysfs file\n");
+		goto error_sysfs;
+	}
+
+	ret = device_create_file(&pdev->dev, &dev_attr_power_duty);
+	if (ret) {
+		printk(KERN_ERR PIPER_DRIVER_NAME ": failed to create sysfs file\n");
+		goto error_sysfs;
+	}
+
+	strcpy(piperp->debug_cmd, "off");
+#if WANT_DEBUG_COMMANDS
+	ret = device_create_file(&pdev->dev, &dev_attr_debug_cmd);
+	if (ret) {
+		printk(KERN_ERR PIPER_DRIVER_NAME ": failed to create sysfs file\n");
+		goto error_sysfs;
+	}
+#endif
+
+	printk(KERN_INFO PIPER_DRIVER_NAME ": driver loaded (fw ver = 0x%08x)\n",
+		piperp->version);
+
+	return 0;
+
+error_sysfs:
+	piper_unregister_hw(piperp);
+error_reg_hw:
+	piper_ps_deinit(piperp);
+	piper_free_rx_tx(piperp);
+retor_irq:
+	free_irq(piperp->irq, piperp);
+error_init:
+	iounmap(piperp->vbase);
+	piperp->vbase = NULL;
+error_remap:
+	release_resource(pdev->resource);
+error_alloc:
+	piper_free_hw(piperp);
+	return ret;
+}
+
+static int piper_remove(struct platform_device *pdev)
+{
+	struct piper_priv *piperp = dev_get_drvdata(&pdev->dev);
+
+	printk(KERN_DEBUG PIPER_DRIVER_NAME " %s\n", __func__);
+
+	device_remove_file(&pdev->dev, &dev_attr_antenna_sel);
+	device_remove_file(&pdev->dev, &dev_attr_power_duty);
+#if WANT_DEBUG_COMMANDS
+	device_remove_file(&pdev->dev, &dev_attr_debug_cmd);
+#endif
+
+	piper_ps_deinit(piperp);
+	piper_unregister_hw(piperp);
+	disable_irq(piperp->irq);
+	piper_clear_irq_mask(piperp, 0xffffffff);
+	free_irq(piperp->irq, piperp);
+	piper_free_rx_tx(piperp);
+	release_resource(pdev->resource);
+	piper_free_hw(piperp);
+
+	return 0;
+}
+
+/* describes the driver */
+static struct platform_driver piper_driver = {
+	.probe		= piper_probe,
+	.remove		= piper_remove,
+	.suspend 	= piper_suspend,
+	.resume		= piper_resume,
+	.driver 	= {
+		.name  = PIPER_DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init piper_init_module(void)
+{
+	return platform_driver_register(&piper_driver);
+}
+
+static void __exit piper_exit_module(void)
+{
+	platform_driver_unregister(&piper_driver);
+}
+
+module_init(piper_init_module);
+module_exit(piper_exit_module);
+
+MODULE_DESCRIPTION("Digi Piper WLAN Driver");
+MODULE_AUTHOR("Contact support@digi.com for questions on this code");
+MODULE_VERSION(DRV_VERS);
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/digiPiper/piperDsp.c b/drivers/net/wireless/digiPiper/piperDsp.c
new file mode 100644
index 000000000000..0f3d0e1b9b5e
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/piperDsp.c
@@ -0,0 +1,285 @@
+/*
+   Copyright (c) 2007-2008 Digi International Inc., All Rights Reserved
+
+   This software contains proprietary and confidential information of Digi
+   International Inc.  By accepting transfer of this copy, Recipient agrees
+   to retain this software in confidence, to prevent disclosure to others,
+   and to make no use of this software other than that for which it was
+   delivered.  This is an unpublished copyrighted work of Digi International
+   Inc.  Except as permitted by federal law, 17 USC 117, copying is strictly
+   prohibited.
+
+   Restricted Rights Legend
+
+   Use, duplication, or disclosure by the Government is subject to
+   restrictions set forth in sub-paragraph (c)(1)(ii) of The Rights in
+   Technical Data and Computer Software clause at DFARS 252.227-7031 or
+   subparagraphs (c)(1) and (2) of the Commercial Computer Software -
+   Restricted Rights at 48 CFR 52.227-19, as applicable.
+
+   Digi International Inc. 11001 Bren Road East, Minnetonka, MN 55343
+
+   WiFi DSP Code for Piper
+*/
+
+
+const unsigned long piper_wifi_dsp_ucode[1024] = {
+  0x32000,  0x3200F,  0x320AB,  0x0403F,
+  0x0C000,  0x38000,  0x38060,  0x10004,
+  0x10008,  0x10009,  0x1000A,  0x1000B,
+  0x10001,  0x10010,  0x08000,  0x08000,
+  0x10001,  0x08000,  0x08000,  0x08000,
+  0x08000,  0x08000,  0x08000,  0x08000,
+  0x32030,  0x39010,  0x39010,  0x30802,
+  0x2B030,  0x10001,  0x10800,  0x38000,
+  0x10004,  0x10040,  0x11002,  0x320D1,
+  0x041AD,  0x0C000,  0x38000,  0x38160,
+  0x38400,  0x061AD,  0x38100,  0x39171,
+  0x39571,  0x32072,  0x11002,  0x320E1,
+  0x38100,  0x39171,  0x39571,  0x36002,
+  0x29039,  0x38000,  0x11002,  0x38000,
+  0x38060,  0x10001,  0x083D5,  0x10050,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x10104,  0x10013,  0x321D1,  0x0422D,
+  0x0E000,  0x38000,  0x38260,  0x38400,
+  0x0622D,  0x0C000,  0x38200,  0x39271,
+  0x39671,  0x32002,  0x10003,  0x321E1,
+  0x38200,  0x39271,  0x39671,  0x10013,
+  0x321E1,  0x38200,  0x39271,  0x39671,
+  0x36002,  0x29034,  0x38000,  0x10003,
+  0x10001,  0x38060,  0x083B1,  0x10050,
+  0x10104,  0x10040,  0x10213,  0x320D1,
+  0x0422D,  0x0E000,  0x38000,  0x38260,
+  0x38400,  0x0622D,  0x0C000,  0x38200,
+  0x39271,  0x39671,  0x10003,  0x321E1,
+  0x38200,  0x39271,  0x39671,  0x10013,
+  0x321E1,  0x38200,  0x39271,  0x39671,
+  0x10003,  0x10001,  0x38060,  0x10004,
+  0x10000,  0x08392,  0x10010,  0x08380,
+  0x30802,  0x29003,  0x37012,  0x082BF,
+  0x38000,  0x29002,  0x37012,  0x08078,
+  0x29002,  0x37012,  0x08105,  0x29002,
+  0x37012,  0x081C2,  0x29002,  0x37172,
+  0x0822F,  0x04236,  0x10801,  0x29002,
+  0x370F2,  0x0838A,  0x29002,  0x370F2,
+  0x083A7,  0x29002,  0x370F2,  0x083C4,
+  0x38000,  0x08002,  0x38000,  0x38000,
+  0x30822,  0x10104,  0x10040,  0x10213,
+  0x320D1,  0x0422D,  0x0E000,  0x38000,
+  0x38260,  0x38400,  0x0622D,  0x0C000,
+  0x38200,  0x39271,  0x39671,  0x10003,
+  0x321E1,  0x38200,  0x39271,  0x39671,
+  0x10013,  0x321E1,  0x38200,  0x39271,
+  0x39671,  0x10003,  0x38000,  0x38060,
+  0x36002,  0x2A005,  0x0422D,  0x38400,
+  0x0402D,  0x38060,  0x38400,  0x10004,
+  0x10000,  0x10001,  0x08349,  0x10010,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x3601F,  0x30033,  0x23103,  0x38000,
+  0x08030,  0x38000,  0x08031,  0x38000,
+  0x327F0,  0x300F1,  0x37091,  0x21034,
+  0x30803,  0x300F2,  0x23009,  0x32081,
+  0x10002,  0x04180,  0x0C100,  0x38000,
+  0x38040,  0x04400,  0x320A1,  0x38060,
+  0x04900,  0x0E630,  0x38000,  0x38000,
+  0x38060,  0x38000,  0x2103E,  0x38060,
+  0x04000,  0x0C000,  0x300F1,  0x22004,
+  0x37041,  0x23003,  0x04401,  0x38000,
+  0x38020,  0x3601F,  0x30033,  0x2310C,
+  0x38000,  0x082EA,  0x10010,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x38000,  0x10800,  0x38000,
+  0x300F2,  0x32081,  0x35021,  0x20017,
+  0x300F2,  0x37052,  0x28036,  0x0440A,
+  0x0C000,  0x38000,  0x38060,  0x30011,
+  0x20004,  0x0440A,  0x0E000,  0x2103F,
+  0x38060,  0x05410,  0x0E000,  0x38000,
+  0x38060,  0x08005,  0x38000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x04000,
+  0x10008,  0x05C02,  0x0C000,  0x38000,
+  0x38020,  0x04C12,  0x0E000,  0x10002,
+  0x38020,  0x0C209,  0x04914,  0x38000,
+  0x0C409,  0x38040,  0x38000,  0x38000,
+  0x38020,  0x32011,  0x36001,  0x0E000,
+  0x05E02,  0x38000,  0x38120,  0x0E201,
+  0x04914,  0x38000,  0x0E401,  0x38040,
+  0x38000,  0x38020,  0x21034,  0x38000,
+  0x10202,  0x04924,  0x38000,  0x38400,
+  0x10202,  0x041A4,  0x0C100,  0x38060,
+  0x04424,  0x38060,  0x04924,  0x0E630,
+  0x320A1,  0x38000,  0x38060,  0x38000,
+  0x2103E,  0x38060,  0x04403,  0x0C000,
+  0x300F0,  0x38020,  0x0E000,  0x37030,
+  0x39030,  0x39030,  0x0401B,  0x10401,
+  0x38040,  0x10001,  0x05C1C,  0x38000,
+  0x38040,  0x08282,  0x10010,  0x00000,
+  0x10023,  0x10104,  0x0422D,  0x0C080,
+  0x32180,  0x38260,  0x0E080,  0x38200,
+  0x39270,  0x39270,  0x05C2E,  0x0C000,
+  0x38060,  0x100D3,  0x0422D,  0x0C080,
+  0x32180,  0x38260,  0x0E080,  0x38200,
+  0x39270,  0x39270,  0x05C2E,  0x0E000,
+  0x38060,  0x10004,  0x10003,  0x10402,
+  0x0422D,  0x0C000,  0x323F0,  0x38200,
+  0x39270,  0x39770,  0x04401,  0x0C000,
+  0x1000B,  0x38020,  0x06409,  0x38020,
+  0x38020,  0x38020,  0x1000A,  0x08254,
+  0x10010,  0x38000,  0x38000,  0x38000,
+  0x10462,  0x10014,  0x041AD,  0x0C000,
+  0x32180,  0x38160,  0x0E000,  0x38100,
+  0x39170,  0x39170,  0x05C2E,  0x0C000,
+  0x38060,  0x10512,  0x041AD,  0x0C000,
+  0x32180,  0x38160,  0x0E000,  0x38100,
+  0x39170,  0x39170,  0x05C2E,  0x0E000,
+  0x38060,  0x10004,  0x05036,  0x0E008,
+  0x10212,  0x38060,  0x38400,  0x04427,
+  0x0C000,  0x38060,  0x0403F,  0x38000,
+  0x38020,  0x0493F,  0x0E630,  0x32051,
+  0x38000,  0x38060,  0x38000,  0x2103E,
+  0x38060,  0x300F1,  0x37061,  0x0443E,
+  0x0C000,  0x38020,  0x0643F,  0x0C000,
+  0x38020,  0x38020,  0x38020,  0x0563F,
+  0x0E000,  0x2000A,  0x38000,  0x21004,
+  0x38020,  0x04C37,  0x08014,  0x38020,
+  0x2000C,  0x38000,  0x21006,  0x38000,
+  0x04637,  0x38020,  0x05C37,  0x0800B,
+  0x38020,  0x38020,  0x21005,  0x04437,
+  0x38020,  0x05637,  0x08004,  0x38020,
+  0x05C37,  0x38020,  0x38000,  0x11102,
+  0x041B6,  0x0C000,  0x38000,  0x38020,
+  0x04E37,  0x0E000,  0x38020,  0x05C3F,
+  0x0C000,  0x38020,  0x04438,  0x0E000,
+  0x38020,  0x04000,  0x38000,  0x10008,
+  0x04401,  0x0C000,  0x38020,  0x06439,
+  0x0E000,  0x38020,  0x38020,  0x38020,
+  0x38020,  0x38020,  0x38020,  0x04439,
+  0x38000,  0x38020,  0x04409,  0x0C000,
+  0x1000B,  0x06409,  0x38020,  0x38020,
+  0x38020,  0x1000A,  0x10212,  0x04136,
+  0x0C800,  0x38000,  0x38060,  0x11112,
+  0x041B6,  0x0E000,  0x38000,  0x38060,
+  0x25004,  0x38000,  0x10800,  0x38000,
+  0x38000,  0x10400,  0x081CD,  0x38000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x10422,  0x10023,  0x10114,  0x04BAD,
+  0x0C080,  0x32190,  0x38300,  0x39370,
+  0x39F70,  0x104D2,  0x100D3,  0x32190,
+  0x38300,  0x39370,  0x39F70,  0x38000,
+  0x10433,  0x0422D,  0x0C000,  0x32030,
+  0x39270,  0x39A10,  0x10063,  0x10334,
+  0x32030,  0x39270,  0x39A10,  0x10512,
+  0x041AD,  0x32020,  0x39170,  0x39510,
+  0x10782,  0x10114,  0x32020,  0x39170,
+  0x39510,  0x104D3,  0x0422D,  0x0C070,
+  0x10334,  0x38260,  0x0E070,  0x32060,
+  0x39210,  0x39270,  0x107B2,  0x104D3,
+  0x10020,  0x05BAD,  0x0E060,  0x32190,
+  0x38300,  0x39370,  0x39B70,  0x13FC3,
+  0x0422D,  0x0C070,  0x10114,  0x38260,
+  0x0E070,  0x32060,  0x39210,  0x39270,
+  0x104E2,  0x13FC3,  0x05BAD,  0x0E060,
+  0x32190,  0x38300,  0x39370,  0x39B70,
+  0x10004,  0x08326,  0x10000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x10114,  0x10173,  0x10902,  0x0616D,
+  0x0C800,  0x321B0,  0x38260,  0x38260,
+  0x38260,  0x39270,  0x39770,  0x0416D,
+  0x321B0,  0x10173,  0x10912,  0x38260,
+  0x38260,  0x38260,  0x39270,  0x39770,
+  0x10003,  0x10C42,  0x0616D,  0x0C800,
+  0x321B0,  0x38260,  0x38260,  0x38260,
+  0x39270,  0x39770,  0x0416D,  0x321B0,
+  0x10003,  0x10C52,  0x38260,  0x38260,
+  0x38260,  0x39270,  0x39770,  0x32028,
+  0x0C070,  0x10973,  0x0423F,  0x38060,
+  0x10B33,  0x0E070,  0x108E3,  0x38060,
+  0x38060,  0x0E060,  0x10AA3,  0x38000,
+  0x38060,  0x0C000,  0x0503F,  0x10222,
+  0x38400,  0x080C6,  0x38000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x108D3,  0x10104,  0x10140,  0x040F6,
+  0x0C202,  0x32181,  0x38200,  0x38080,
+  0x382C0,  0x380A0,  0x216FF,  0x380A0,
+  0x10823,  0x0C202,  0x32181,  0x38200,
+  0x38080,  0x382C0,  0x380A0,  0x216FF,
+  0x380A0,  0x080FA,  0x10010,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x10902,  0x108D3,  0x10114,  0x10140,
+  0x047F6,  0x0D202,  0x32181,  0x100E1,
+  0x38280,  0x38040,  0x381A0,  0x216FE,
+  0x38440,  0x10C42,  0x10823,  0x047F6,
+  0x0D202,  0x32171,  0x38280,  0x38040,
+  0x381A0,  0x216FE,  0x38440,  0x381A0,
+  0x386E0,  0x321A1,  0x2103F,  0x10001,
+  0x38400,  0x080D2,  0x10010,  0x00000,
+  0x38000,  0x38000,  0x38000,  0x0423F,
+  0x10973,  0x0C000,  0x38060,  0x10B33,
+  0x0E000,  0x108E3,  0x38060,  0x38060,
+  0x0E030,  0x10AA3,  0x38000,  0x38060,
+  0x18083,  0x0C0C0,  0x0443F,  0x36018,
+  0x38060,  0x21002,  0x0C000,  0x32018,
+  0x0503F,  0x10222,  0x38400,  0x041A4,
+  0x0C100,  0x38060,  0x04424,  0x38060,
+  0x048A4,  0x0E630,  0x32061,  0x38000,
+  0x38060,  0x38000,  0x2103E,  0x38060,
+  0x10080,  0x04401,  0x0C000,  0x38020,
+  0x06639,  0x0E000,  0x38020,  0x04409,
+  0x0C000,  0x1000B,  0x06409,  0x38020,
+  0x38020,  0x38020,  0x1000A,  0x30811,
+  0x22007,  0x37021,  0x20003,  0x38000,
+  0x08383,  0x38000,  0x083A1,  0x38000,
+  0x10902,  0x108D3,  0x10104,  0x10140,
+  0x047F6,  0x0D202,  0x32181,  0x100E1,
+  0x38080,  0x38240,  0x381A0,  0x38460,
+  0x214FD,  0x38740,  0x10C42,  0x10823,
+  0x047F6,  0x0D202,  0x32171,  0x38080,
+  0x38240,  0x381A0,  0x38460,  0x214FD,
+  0x38740,  0x381A0,  0x38460,  0x384E0,
+  0x38000,  0x38000,  0x38000,  0x38000,
+  0x38000,  0x38000,  0x38000,  0x38000,
+  0x38000,  0x38400,  0x10001,  0x08068,
+  0x10010,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x041A4,  0x0C100,  0x38000,  0x38060,
+  0x04424,  0x32061,  0x38060,  0x04924,
+  0x0E630,  0x38000,  0x38000,  0x38060,
+  0x38000,  0x2103E,  0x38060,  0x10080,
+  0x108D3,  0x10104,  0x10040,  0x040F6,
+  0x0C202,  0x32181,  0x38200,  0x38200,
+  0x38200,  0x21240,  0x21640,  0x10823,
+  0x32181,  0x38200,  0x38200,  0x38200,
+  0x21240,  0x21640,  0x04401,  0x0C000,
+  0x38020,  0x06639,  0x0E000,  0x38020,
+  0x04409,  0x0C000,  0x1000B,  0x06409,
+  0x38020,  0x38020,  0x38020,  0x1000A,
+  0x0801F,  0x10010,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000
+};
+
+const int piper_dsp_data_len = (sizeof piper_wifi_dsp_ucode)/4;
diff --git a/drivers/net/wireless/digiPiper/piperDsp.h b/drivers/net/wireless/digiPiper/piperDsp.h
new file mode 100644
index 000000000000..836ffcb96341
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/piperDsp.h
@@ -0,0 +1,8 @@
+#ifndef PIPER_DSP_H_
+#define PIPER_DSP_H_
+
+
+extern const unsigned long piper_wifi_dsp_ucode[];
+extern const int piper_dsp_data_len;
+
+#endif
diff --git a/drivers/net/wireless/digiPiper/piperMacAssist.c b/drivers/net/wireless/digiPiper/piperMacAssist.c
new file mode 100644
index 000000000000..1a8ada81b84b
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/piperMacAssist.c
@@ -0,0 +1,285 @@
+/*
+   Copyright (c) 2007-2008 Digi International Inc., All Rights Reserved
+
+   This software contains proprietary and confidential information of Digi
+   International Inc.  By accepting transfer of this copy, Recipient agrees
+   to retain this software in confidence, to prevent disclosure to others,
+   and to make no use of this software other than that for which it was
+   delivered.  This is an unpublished copyrighted work of Digi International
+   Inc.  Except as permitted by federal law, 17 USC 117, copying is strictly
+   prohibited.
+
+   Restricted Rights Legend
+
+   Use, duplication, or disclosure by the Government is subject to
+   restrictions set forth in sub-paragraph (c)(1)(ii) of The Rights in
+   Technical Data and Computer Software clause at DFARS 252.227-7031 or
+   subparagraphs (c)(1) and (2) of the Commercial Computer Software -
+   Restricted Rights at 48 CFR 52.227-19, as applicable.
+
+   Digi International Inc. 11001 Bren Road East, Minnetonka, MN 55343
+
+   WiFi MAC assist Code for Piper
+*/
+
+const unsigned long piper_wifi_macassist_ucode[2048] = {
+  0x00F00,  0x2CF22,  0x3033B,  0x00E20,  0x2CED0,  0x2CFD2,  0x2CFD8,  0x2CFD9,
+  0x2CF31,  0x2CFDB,  0x00E20,  0x2CEDA,  0x00E64,  0x2CE1D,  0x00EFF,  0x2CEEA,
+  0x2CEEB,  0x00E01,  0x2CE20,  0x00E10,  0x2CE21,  0x04522,  0x00603,  0x2C6CE,
+  0x00001,  0x2C040,  0x00000,  0x2C041,  0x2C042,  0x2C043,  0x0016E,  0x2C144,
+  0x00000,  0x0010A,  0x00200,  0x0A5FF,  0x35027,  0x0010B,  0x00280,  0x2C245,
+  0x2C146,  0x2C047,  0x00180,  0x2C148,  0x2C049,  0x2C0CE,  0x00000,  0x00100,
+  0x00223,  0x040C0,  0x302E2,  0x3031A,  0x00030,  0x0011C,  0x00801,  0x042C0,
+  0x0A204,  0x35037,  0x042C0,  0x0A5FF,  0x35040,  0x00031,  0x00117,  0x00806,
+  0x30338,  0x1C001,  0x35C40,  0x1C101,  0x35C43,  0x00000,  0x042C0,  0x043C0,
+  0x044C0,  0x045C0,  0x046C0,  0x047C0,  0x00000,  0x1C801,  0x35C4D,  0x048C1,
+  0x049C1,  0x04AC1,  0x04BC1,  0x04CC1,  0x04DC1,  0x0D320,  0x0D430,  0x0D540,
+  0x0D650,  0x0D760,  0x19230,  0x19240,  0x19250,  0x19260,  0x19270,  0x0A880,
+  0x35062,  0x18208,  0x0A980,  0x35065,  0x18208,  0x0AA80,  0x35068,  0x18208,
+  0x0AB80,  0x3506B,  0x18208,  0x0AC80,  0x3506E,  0x18208,  0x0AD80,  0x35071,
+  0x18208,  0x20206,  0x04122,  0x0A1FF,  0x3547B,  0x2C222,  0x0E260,  0x35015,
+  0x0E260,  0x00151,  0x3407C,  0x00103,  0x19120,  0x2C122,  0x00E00,  0x2CECE,
+  0x3C001,  0x04EC3,  0x0AE20,  0x04FC2,  0x0AF80,  0x0DEF0,  0x04F30,  0x2CE30,
+  0x0FFE0,  0x35095,  0x0EE20,  0x35495,  0x3C000,  0x04EC1,  0x0AE80,  0x3548D,
+  0x04E17,  0x2CED4,  0x04E16,  0x2CED6,  0x3C001,  0x04E27,  0x0AE10,  0x04F33,
+  0x2CE33,  0x0EF10,  0x0BFE0,  0x350AC,  0x00E00,  0x00FE0,  0x2DEF0,  0x18F01,
+  0x0EFE8,  0x350A4,  0x0EFE8,  0x3409E,  0x04F1D,  0x00E00,  0x1DEF0,  0x2CEEA,
+  0x04F1C,  0x00E00,  0x1FEF0,  0x2CEEB,  0x04E27,  0x0AE80,  0x350C8,  0x3C000,
+  0x00E01,  0x2CECE,  0x04EC1,  0x0AE80,  0x354B2,  0x04EE0,  0x2CE27,  0x04EE1,
+  0x2CE26,  0x04EE2,  0x2CE25,  0x04EE3,  0x2CE24,  0x04EE4,  0x2CE23,  0x04EE5,
+  0x2CE22,  0x04EE6,  0x04FE7,  0x2CE21,  0x2CF20,  0x00E00,  0x2CECE,  0x3C001,
+  0x04E35,  0x04F36,  0x0DEF0,  0x3507E,  0x04E3A,  0x04FEB,  0x15EF0,  0x3547E,
+  0x04FEA,  0x04E37,  0x15EF0,  0x3547E,  0x3C000,  0x04EC1,  0x0AE80,  0x354D5,
+  0x04E36,  0x12EFF,  0x354E7,  0x04F35,  0x14F40,  0x35CE7,  0x00E00,  0x2CED8,
+  0x20F06,  0x20F06,  0x2CFD9,  0x3C001,  0x2CE36,  0x2CE35,  0x3407E,  0x04F35,
+  0x1CF01,  0x1EE00,  0x2CF35,  0x2CE36,  0x00E0F,  0x2CED8,  0x2CED9,  0x3C001,
+  0x04E37,  0x04F3A,  0x18E01,  0x1AF00,  0x2CE37,  0x2CF3A,  0x3407E,  0x2CF32,
+  0x04FC0,  0x20F0E,  0x3190B,  0x20F0E,  0x31BFC,  0x20F0E,  0x3195B,  0x20F0E,
+  0x31A72,  0x20F0E,  0x31ACA,  0x20F0E,  0x319DC,  0x20F0E,  0x319E3,  0x20F0E,
+  0x31A63,  0x04F32,  0x38001,  0x04031,  0x0C001,  0x2C031,  0x040C2,  0x0A008,
+  0x35120,  0x04031,  0x0A004,  0x35517,  0x04031,  0x0C080,  0x2C031,  0x302E2,
+  0x040C3,  0x0A010,  0x3511C,  0x00016,  0x18011,  0x00100,  0x00200,  0x3031A,
+  0x302FF,  0x0E3FF,  0x35127,  0x04031,  0x0A0BF,  0x2C031,  0x3412A,  0x04031,
+  0x0C040,  0x2C031,  0x04244,  0x040C3,  0x0A010,  0x35547,  0x302E8,  0x00400,
+  0x04510,  0x0A5FF,  0x35534,  0x0C515,  0x13350,  0x3513B,  0x01430,  0x1232F,
+  0x35544,  0x2030E,  0x34134,  0x1232F,  0x3553F,  0x2030E,  0x34134,  0x124FF,
+  0x35544,  0x2030E,  0x35D34,  0x00401,  0x302F4,  0x2C33B,  0x2A000,  0x04511,
+  0x1426E,  0x3514F,  0x14237,  0x35152,  0x14214,  0x35155,  0x34158,  0x0026E,
+  0x12508,  0x35559,  0x00237,  0x12504,  0x35559,  0x00214,  0x12502,  0x35559,
+  0x0020A,  0x2C23B,  0x2A000,  0x04127,  0x00001,  0x2C0CE,  0x040C2,  0x0A040,
+  0x35163,  0x00005,  0x2C0CE,  0x0A110,  0x35192,  0x04048,  0x0E080,  0x3516C,
+  0x04048,  0x0E050,  0x3516C,  0x34192,  0x00012,  0x04140,  0x0E1EE,  0x35175,
+  0x041C1,  0x0A120,  0x35574,  0x18020,  0x18007,  0x302FF,  0x00100,  0x19030,
+  0x1A100,  0x30338,  0x042E0,  0x043E1,  0x044E2,  0x045E3,  0x046E4,  0x047E5,
+  0x048E6,  0x049E7,  0x19200,  0x1B310,  0x1A400,  0x1A500,  0x1A600,  0x1A700,
+  0x1A800,  0x1A900,  0x2C260,  0x2C361,  0x2C462,  0x2C563,  0x2C664,  0x2C765,
+  0x2C866,  0x2C967,  0x0002C,  0x00100,  0x042C2,  0x044C1,  0x0A480,  0x35595,
+  0x2C0D0,  0x2C1D2,  0x043C2,  0x0A3E0,  0x2C3C2,  0x04140,  0x04348,  0x00000,
+  0x2C0CE,  0x04031,  0x0A0FB,  0x2C031,  0x0A204,  0x355B6,  0x0A30F,  0x0E304,
+  0x351B6,  0x04012,  0x0A001,  0x351B6,  0x0E1EE,  0x355AF,  0x0C004,  0x0C00A,
+  0x2C012,  0x00064,  0x1C001,  0x355B2,  0x00040,  0x2C0C4,  0x040C2,  0x0A040,
+  0x2B400,  0x0403E,  0x0A0FF,  0x2B400,  0x00001,  0x2C0CE,  0x04040,  0x0A0FF,
+  0x351C5,  0x0E001,  0x351C5,  0x0E0EF,  0x355D4,  0x04042,  0x0A0FF,  0x351CB,
+  0x0E001,  0x355D4,  0x341CE,  0x04041,  0x0A0FF,  0x351D4,  0x04043,  0x0A0FF,
+  0x355D4,  0x00000,  0x2C0CE,  0x2A000,  0x00080,  0x2C0C4,  0x0403F,  0x18001,
+  0x2C03F,  0x00000,  0x2C0CE,  0x2A000,  0x00008,  0x2C0C4,  0x00100,  0x2C13E,
+  0x00012,  0x00200,  0x3431A,  0x00101,  0x2C1CE,  0x0402B,  0x12001,  0x35203,
+  0x04128,  0x121FF,  0x351EE,  0x1C101,  0x2C128,  0x35603,  0x04129,  0x0422D,
+  0x0432C,  0x0442F,  0x0452E,  0x12002,  0x351F7,  0x2C128,  0x341F9,  0x0A0FE,
+  0x2C02B,  0x00000,  0x2C0CE,  0x0401D,  0x1D200,  0x0401C,  0x1F300,  0x2C237,
+  0x2C33A,  0x2C435,  0x2C536,  0x00000,  0x2C0CE,  0x04027,  0x0A010,  0x35236,
+  0x3033B,  0x040D5,  0x2C02B,  0x040D7,  0x2C02A,  0x00100,  0x0401F,  0x1D100,
+  0x2C1E8,  0x00100,  0x0401E,  0x1F100,  0x2C1E9,  0x040C2,  0x0A004,  0x3561D,
+  0x3033B,  0x00004,  0x2C0D0,  0x00000,  0x2C0D2,  0x04027,  0x0A004,  0x3522C,
+  0x30338,  0x30338,  0x040C2,  0x0C080,  0x2C0C2,  0x040C2,  0x0A040,  0x3522C,
+  0x0402F,  0x2C0D4,  0x0402E,  0x2C0D6,  0x00110,  0x040C1,  0x0A010,  0x35234,
+  0x04031,  0x0D010,  0x2C031,  0x34246,  0x2C1C4,  0x34246,  0x00010,  0x2C0C4,
+  0x04034,  0x0C000,  0x35246,  0x040ED,  0x0C000,  0x35646,  0x3033B,  0x0421F,
+  0x0431E,  0x2C2D8,  0x2C3D9,  0x04119,  0x1D010,  0x2C0ED,  0x0421D,  0x0431C,
+  0x00000,  0x00100,  0x1D020,  0x1F130,  0x3033B,  0x2C0EA,  0x2C1EB,  0x040EC,
+  0x0C000,  0x35655,  0x04118,  0x1D010,  0x2C0EC,  0x3033B,  0x040EE,  0x0C000,
+  0x2B400,  0x041EF,  0x0C100,  0x2B400,  0x0421B,  0x0431A,  0x1D020,  0x1F130,
+  0x2C0EE,  0x2C1EF,  0x2A000,  0x3033B,  0x040D5,  0x2C02B,  0x040D7,  0x2C02A,
+  0x040C1,  0x0A010,  0x3566E,  0x00020,  0x2C0C4,  0x2A000,  0x04031,  0x0C020,
+  0x2C031,  0x2A000,  0x04012,  0x0A0F7,  0x2C012,  0x00500,  0x00601,  0x00100,
+  0x00700,  0x04031,  0x0A0F7,  0x2C031,  0x2C53E,  0x040C3,  0x0A0C0,  0x35287,
+  0x00603,  0x0A080,  0x35684,  0x00605,  0x2C6CE,  0x04440,  0x342A4,  0x00702,
+  0x2C6CE,  0x04440,  0x0424C,  0x0A201,  0x356A4,  0x04248,  0x0E2B4,  0x35293,
+  0x04249,  0x0A204,  0x35298,  0x2C5CE,  0x04231,  0x0C208,  0x2C231,  0x2C6CE,
+  0x00203,  0x04048,  0x2C5CE,  0x0E0C4,  0x352B9,  0x00030,  0x0A4EE,  0x356A2,
+  0x00080,  0x1805E,  0x00209,  0x342B9,  0x2C5CE,  0x00000,  0x00100,  0x04231,
+  0x0A240,  0x356AD,  0x042C1,  0x0A201,  0x356B2,  0x0A4EE,  0x356B0,  0x000FE,
+  0x1803C,  0x1A100,  0x00200,  0x0A4EE,  0x356B7,  0x18016,  0x1A100,  0x18012,
+  0x1A100,  0x3031A,  0x12702,  0x352C2,  0x0A202,  0x356C2,  0x04131,  0x12108,
+  0x356C2,  0x0C780,  0x04131,  0x0A130,  0x0D710,  0x2C7C4,  0x04731,  0x0A7CF,
+  0x2C731,  0x2A000,  0x04531,  0x12504,  0x352D4,  0x3033B,  0x0A5FB,  0x2C531,
+  0x00500,  0x2C5D8,  0x2C5D9,  0x342DE,  0x04031,  0x0A080,  0x2B400,  0x04131,
+  0x0A1F7,  0x2C131,  0x00184,  0x2C1C4,  0x00101,  0x2C1CE,  0x04440,  0x302E2,
+  0x00500,  0x342A4,  0x040C1,  0x0A080,  0x356E2,  0x00020,  0x2C0DB,  0x2A000,
+  0x00310,  0x12202,  0x352EC,  0x00301,  0x12201,  0x356F0,  0x20306,  0x20306,
+  0x12204,  0x352F3,  0x20306,  0x2A000,  0x00308,  0x12455,  0x356F8,  0x0030C,
+  0x12433,  0x352FB,  0x0C301,  0x1240F,  0x352FE,  0x0C302,  0x2A000,  0x04240,
+  0x0E2EE,  0x35708,  0x04244,  0x003FF,  0x0A208,  0x2B000,  0x00300,  0x2A000,
+  0x003FF,  0x04244,  0x0E26E,  0x3570D,  0x00311,  0x04244,  0x0E237,  0x35711,
+  0x00323,  0x04244,  0x0E214,  0x35715,  0x00360,  0x04244,  0x0E20A,  0x2B400,
+  0x003C0,  0x2A000,  0x0C000,  0x35322,  0x04325,  0x01430,  0x20306,  0x19430,
+  0x19040,  0x1A100,  0x1C01A,  0x1E100,  0x04AC1,  0x0AA80,  0x35724,  0x043DB,
+  0x19030,  0x1A100,  0x12180,  0x3532F,  0x00001,  0x00100,  0x34331,  0x18001,
+  0x1A100,  0x2C0D0,  0x2C1D2,  0x043C2,  0x0A3E0,  0x0D230,  0x2C2C2,  0x2A000,
+  0x04AC1,  0x0AA80,  0x35338,  0x04AC1,  0x0AA80,  0x3573B,  0x2A000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00001,  0x2C0CF,  0x2A000,  0x340F7,
+  0x040C3,  0x0A002,  0x35008,  0x300EA,  0x04031,  0x0C002,  0x2C031,  0x343FC,
+  0x04031,  0x12002,  0x3540E,  0x12001,  0x353FC,  0x300EA,  0x04031,  0x0A0F8,
+  0x2C031,  0x30058,  0x04031,  0x0A0F7,  0x2C031,  0x04123,  0x01010,  0x0A00C,
+  0x0E004,  0x35020,  0x04031,  0x0A080,  0x35042,  0x00084,  0x2C0C4,  0x34042,
+  0x04023,  0x0C010,  0x2C023,  0x04027,  0x0A008,  0x35429,  0x01010,  0x0A003,
+  0x3501A,  0x04031,  0x0A080,  0x35036,  0x04048,  0x0A0EC,  0x0E0C4,  0x3503A,
+  0x00084,  0x2C0C4,  0x04031,  0x0A07F,  0x2C031,  0x3403A,  0x04048,  0x0A0EC,
+  0x0E0C4,  0x35042,  0x040C1,  0x0A002,  0x35040,  0x00040,  0x2C0C4,  0x3401A,
+  0x00001,  0x2C0C4,  0x04031,  0x0A07F,  0x2C031,  0x04023,  0x0A00C,  0x0E004,
+  0x357FC,  0x0404C,  0x0A001,  0x3504E,  0x300B0,  0x343FC,  0x04023,  0x0A001,
+  0x35453,  0x300BF,  0x343FC,  0x04048,  0x0E050,  0x357FC,  0x300DB,  0x343FC,
+  0x04031,  0x0A040,  0x3505F,  0x04023,  0x0C020,  0x2C023,  0x3406B,  0x040C1,
+  0x0A001,  0x3506B,  0x04023,  0x0C004,  0x2C023,  0x04048,  0x0A003,  0x35073,
+  0x04023,  0x0C008,  0x2C023,  0x040C3,  0x0A010,  0x3506F,  0x00054,  0x1804E,
+  0x00100,  0x00200,  0x34306,  0x040C2,  0x0A004,  0x2B400,  0x040C3,  0x0A010,
+  0x3507A,  0x00016,  0x18012,  0x00100,  0x00200,  0x30306,  0x0004C,  0x05100,
+  0x0A101,  0x2B000,  0x04008,  0x0A001,  0x35088,  0x04048,  0x0E080,  0x3509A,
+  0x04049,  0x0A002,  0x3548C,  0x00008,  0x18050,  0x05100,  0x0A101,  0x35494,
+  0x00208,  0x3032E,  0x0C400,  0x2B400,  0x04048,  0x0E040,  0x3549A,  0x04027,
+  0x0A010,  0x2B000,  0x0402D,  0x0A0FF,  0x350AC,  0x04048,  0x0E080,  0x354AC,
+  0x04063,  0x04139,  0x2C039,  0x0F010,  0x354AC,  0x04062,  0x04138,  0x0422D,
+  0x0B020,  0x2C038,  0x0F010,  0x2B000,  0x04023,  0x0C002,  0x2C023,  0x2A000,
+  0x04048,  0x0A0EC,  0x0E0E4,  0x354BC,  0x04036,  0x04135,  0x0D010,  0x2B400,
+  0x3033C,  0x2C0D8,  0x2C0D9,  0x2A000,  0x04048,  0x0E080,  0x310DB,  0x0414B,
+  0x0A180,  0x2B400,  0x0414A,  0x0424B,  0x1C10A,  0x1E200,  0x35CC9,  0x00100,
+  0x00200,  0x30340,  0x2BC00,  0x04048,  0x0E0B4,  0x2B400,  0x2A000,  0x04031,
+  0x0C004,  0x2C031,  0x00078,  0x00100,  0x042C3,  0x0A210,  0x350D9,  0x0006A,
+  0x00101,  0x00209,  0x34306,  0x0466D,  0x1866F,  0x05160,  0x19610,  0x0416A,
+  0x0A10C,  0x2C134,  0x350E9,  0x1860A,  0x05160,  0x18601,  0x05260,  0x30340,
+  0x1C60B,  0x34213,  0x00000,  0x2C023,  0x04031,  0x0A040,  0x2B400,  0x040C3,
+  0x0A010,  0x354FB,  0x04046,  0x0A001,  0x354FB,  0x04045,  0x14005,  0x35CFB,
+  0x040C3,  0x0A002,  0x354F8,  0x04048,  0x0A003,  0x2B400,  0x0004C,  0x05100,
+  0x0A101,  0x2B400,  0x0424C,  0x0434D,  0x0444E,  0x0454F,  0x04750,  0x04851,
+  0x04100,  0x0F120,  0x04001,  0x0F030,  0x0D100,  0x04002,  0x0F040,  0x0D100,
+  0x04003,  0x0F050,  0x0D100,  0x04004,  0x0F070,  0x0D100,  0x04005,  0x0F080,
+  0x0D100,  0x35146,  0x04027,  0x0A040,  0x35145,  0x00001,  0x2C0CE,  0x04130,
+  0x0F120,  0x04038,  0x0F200,  0x04031,  0x0F030,  0x0D100,  0x04039,  0x0F030,
+  0x0D200,  0x04032,  0x0F040,  0x0D100,  0x0403A,  0x0F040,  0x0D200,  0x04033,
+  0x0F050,  0x0D100,  0x0403B,  0x0F050,  0x0D200,  0x04034,  0x0F070,  0x0D100,
+  0x0403C,  0x0F070,  0x0D200,  0x04035,  0x0433D,  0x00400,  0x2C4CE,  0x0F080,
+  0x0D100,  0x35146,  0x0F380,  0x0D230,  0x35146,  0x2A000,  0x04023,  0x0C001,
+  0x2C023,  0x040C1,  0x0A002,  0x2B400,  0x04048,  0x0A00C,  0x0E004,  0x35564,
+  0x04048,  0x0A0E0,  0x0E0A0,  0x35164,  0x0E060,  0x2B400,  0x04131,  0x0A108,
+  0x2B000,  0x3020A,  0x040C3,  0x0A002,  0x3555A,  0x040C1,  0x0A002,  0x35577,
+  0x040C1,  0x0A001,  0x35177,  0x2A000,  0x3020A,  0x0413B,  0x040C3,  0x0A010,
+  0x355C5,  0x0C1C0,  0x002EE,  0x00300,  0x00701,  0x00603,  0x040C3,  0x0A002,
+  0x3556E,  0x040C1,  0x0A002,  0x35577,  0x040C1,  0x0A001,  0x3557C,  0x042C2,
+  0x0A2E0,  0x0C203,  0x2C2C2,  0x2A000,  0x2C6CE,  0x2C240,  0x2C341,  0x2C342,
+  0x2C343,  0x2C144,  0x2C745,  0x2C346,  0x2C347,  0x2C3CE,  0x04248,  0x0434A,
+  0x0444B,  0x01540,  0x0E580,  0x35193,  0x01520,  0x0E5B4,  0x35195,  0x04549,
+  0x0A504,  0x35595,  0x0A400,  0x0A300,  0x341AB,  0x00528,  0x01010,  0x0A007,
+  0x351A9,  0x0E004,  0x351A9,  0x0E001,  0x351A9,  0x18504,  0x0E004,  0x351A9,
+  0x0E007,  0x351A9,  0x18508,  0x0E004,  0x351A9,  0x18504,  0x0E005,  0x351A9,
+  0x1850C,  0x1D350,  0x1E400,  0x00000,  0x2C6CE,  0x0E2B4,  0x351B1,  0x005D4,
+  0x341B2,  0x005C4,  0x2C548,  0x2C049,  0x2C34A,  0x2C44B,  0x00306,  0x00152,
+  0x0024C,  0x2C0CE,  0x05810,  0x2C6CE,  0x2D820,  0x18101,  0x18201,  0x1C301,
+  0x355B9,  0x2C0CE,  0x00001,  0x2C03E,  0x2A000,  0x045C1,  0x0A540,  0x351CC,
+  0x01510,  0x0E50A,  0x355CC,  0x00114,  0x01510,  0x00784,  0x00800,  0x0E56E,
+  0x351D9,  0x00704,  0x0E559,  0x351D8,  0x0E523,  0x351D7,  0x18838,  0x18823,
+  0x1880A,  0x1880B,  0x00603,  0x00300,  0x040C3,  0x0A002,  0x355DC,  0x040C1,
+  0x0A002,  0x35577,  0x040C1,  0x0A001,  0x35177,  0x2C6CE,  0x2C340,  0x2C341,
+  0x2C342,  0x2C343,  0x2C144,  0x2C745,  0x2C846,  0x2C347,  0x2C3CE,  0x04248,
+  0x0434A,  0x0444B,  0x01540,  0x0E580,  0x351FC,  0x01520,  0x0E5B4,  0x351FE,
+  0x04549,  0x0A504,  0x355FE,  0x0A400,  0x0A300,  0x341AB,  0x01580,  0x1850A,
+  0x1D350,  0x1E400,  0x045C1,  0x0A540,  0x35606,  0x185A0,  0x18520,  0x1D350,
+  0x1E400,  0x341AB,  0x00000,  0x00100,  0x00217,  0x04348,  0x0E3B4,  0x35211,
+  0x00207,  0x30306,  0x2A000,  0x18603,  0x04027,  0x0A010,  0x0416A,  0x0A102,
+  0x20106,  0x20106,  0x20106,  0x0F010,  0x2B400,  0x0C100,  0x35237,  0x0426D,
+  0x04013,  0x0F020,  0x2B400,  0x0006E,  0x00100,  0x0A2FF,  0x2B000,  0x00500,
+  0x05300,  0x00701,  0x2C7CE,  0x05410,  0x00700,  0x2C7CE,  0x0F340,  0x0D530,
+  0x18001,  0x18101,  0x1C201,  0x35628,  0x0C500,  0x2B400,  0x34257,  0x00058,
+  0x00108,  0x00206,  0x00500,  0x05300,  0x05410,  0x0F340,  0x0D530,  0x18001,
+  0x18101,  0x1C201,  0x3563B,  0x0C500,  0x2B400,  0x04034,  0x0C000,  0x35257,
+  0x18603,  0x05060,  0x00100,  0x1D100,  0x2C1ED,  0x18601,  0x05060,  0x2C019,
+  0x18601,  0x05060,  0x2C01F,  0x18601,  0x05060,  0x2C01E,  0x18602,  0x04048,
+  0x0A0EC,  0x0E080,  0x35676,  0x04023,  0x0C002,  0x2C023,  0x040C2,  0x0A080,
+  0x35265,  0x040C2,  0x0A05F,  0x2C0C2,  0x2C0C2,  0x18603,  0x04027,  0x0A010,
+  0x3526F,  0x05060,  0x2C01F,  0x18601,  0x05060,  0x2C01E,  0x34276,  0x05060,
+  0x00100,  0x1D100,  0x2C1EC,  0x18601,  0x05060,  0x2C018,  0x040C3,  0x0A010,
+  0x35685,  0x04044,  0x04145,  0x04246,  0x00304,  0x2020E,  0x20108,  0x20008,
+  0x2010E,  0x20008,  0x1C301,  0x35680,  0x34294,  0x04046,  0x04147,  0x04244,
+  0x003C0,  0x0E26E,  0x3568C,  0x00311,  0x04244,  0x0E237,  0x35690,  0x00323,
+  0x04244,  0x0E214,  0x35694,  0x00360,  0x1D030,  0x1E100,  0x30339,  0x1801A,
+  0x1A100,  0x04260,  0x19200,  0x04361,  0x1B310,  0x04462,  0x1A400,  0x04563,
+  0x1A500,  0x04664,  0x1A600,  0x04765,  0x1A700,  0x04866,  0x1A800,  0x04967,
+  0x1A900,  0x0406A,  0x0A002,  0x352BF,  0x040E0,  0x1D020,  0x040E1,  0x1F030,
+  0x040E2,  0x1F040,  0x040E3,  0x1F050,  0x040E4,  0x1F060,  0x040E5,  0x1F070,
+  0x040E6,  0x1F080,  0x040E7,  0x1F090,  0x2BC00,  0x00000,  0x342C0,  0x30339,
+  0x2C2E0,  0x2C3E1,  0x2C4E2,  0x2C5E3,  0x2C6E4,  0x2C7E5,  0x2C8E6,  0x2C9E7,
+  0x01230,  0x0401D,  0x0411C,  0x00D27,  0x00C01,  0x20006,  0x20100,  0x35AD5,
+  0x18C01,  0x0EC10,  0x352F4,  0x0EC10,  0x342CD,  0x20108,  0x342D8,  0x2010E,
+  0x20008,  0x1CC01,  0x352EC,  0x01A80,  0x01B90,  0x1DA00,  0x1FB10,  0x35AD7,
+  0x018A0,  0x019B0,  0x342D7,  0x1CD01,  0x352F8,  0x20306,  0x20400,  0x20500,
+  0x20600,  0x20700,  0x20800,  0x20900,  0x01A80,  0x01B90,  0x1DA00,  0x1FB10,
+  0x35AE3,  0x018A0,  0x019B0,  0x342E3,  0x00100,  0x008FF,  0x009FF,  0x34301,
+  0x30339,  0x0401D,  0x0411C,  0x04AE1,  0x0FA20,  0x0AAFC,  0x35301,  0x18801,
+  0x1A900,  0x1D800,  0x1F910,  0x2C8EA,  0x2C9EB,  0x2A000,  0x0442C,  0x0C000,
+  0x3530D,  0x04325,  0x01430,  0x20306,  0x19430,  0x19040,  0x1A100,  0x043C3,
+  0x0A310,  0x35717,  0x04344,  0x0A302,  0x35317,  0x0031A,  0x34318,  0x0031E,
+  0x1D030,  0x1E100,  0x04AC1,  0x0AA80,  0x3571A,  0x04ADB,  0x190A0,  0x1A100,
+  0x12180,  0x35325,  0x00002,  0x00100,  0x34327,  0x18002,  0x1A100,  0x2C0D0,
+  0x2C1D2,  0x043C2,  0x0A3E0,  0x0D230,  0x2C2C2,  0x2A000,  0x00506,  0x00400,
+  0x05100,  0x05320,  0x0F130,  0x0D410,  0x18201,  0x18001,  0x1C501,  0x35730,
+  0x2A000,  0x04AC1,  0x0AA80,  0x35339,  0x04AC1,  0x0AA80,  0x3573C,  0x2A000,
+  0x30339,  0x043D8,  0x044D9,  0x18301,  0x1A400,  0x1D310,  0x1F420,  0x2BC00,
+  0x2C1D8,  0x2C2D9,  0x2A000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,
+  0x00000,  0x00000,  0x00000,  0x00000,  0x00000,  0x2C0CF,  0x34000,  0x00000
+};
+
+
+const int piper_macassist_data_len = (sizeof piper_wifi_macassist_ucode)/4;
diff --git a/drivers/net/wireless/digiPiper/piperMacAssist.h b/drivers/net/wireless/digiPiper/piperMacAssist.h
new file mode 100644
index 000000000000..27b2b77fb493
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/piperMacAssist.h
@@ -0,0 +1,9 @@
+
+#ifndef PIPER_MACASSIST_H_
+#define PIPER_MACASSIST_H_
+
+extern const unsigned long piper_wifi_macassist_ucode[];
+extern const int piper_macassist_data_len;
+
+
+#endif
diff --git a/drivers/net/wireless/digiPiper/pipermain.h b/drivers/net/wireless/digiPiper/pipermain.h
new file mode 100644
index 000000000000..dba31309e081
--- /dev/null
+++ b/drivers/net/wireless/digiPiper/pipermain.h
@@ -0,0 +1,381 @@
+#ifndef __PIPER_H_
+#define __PIPER_H_
+
+#include <linux/completion.h>
+#include <linux/if_ether.h>
+#include <linux/spinlock.h>
+#include <net/mac80211.h>
+#include <linux/i2c.h>
+#include "mac.h"
+
+
+// #define WANT_DEBUG
+#ifdef WANT_DEBUG
+#define digi_dbg(fmt, arg...) \
+    printk(KERN_ERR PIPER_DRIVER_NAME ": %s - " fmt, __func__, ##arg)
+#else
+#define digi_dbg(fmt, arg...) \
+	do { } while (0)
+#endif
+
+#define ERROR(x)		printk(KERN_ALERT x)
+
+/* Debug levels */
+#define DSILENT			0xffff
+#define DERROR			0xfff0
+#define DWARNING		0xffe0
+#define DNORMAL			0xffd0
+#define DVERBOSE		0xffc0
+#define DVVERBOSE		0xffb0
+#define DALL			0xffa0
+
+
+#define PIPER_DRIVER_NAME	"piper"
+#define DRV_VERS		"0.1"
+
+/* Useful defines for AES */
+#define	PIPER_EXTIV_SIZE	8	/* IV and extended IV size */
+#define	MIC_SIZE		8	/* Message integrity check size */
+#define ICV_SIZE		4
+#define	DATA_SIZE		28	/* Data frame header+FCS size */
+#define	CCMP_SIZE		(PIPER_EXTIV_SIZE + MIC_SIZE)	/* Total CCMP size */
+#define EXPANDED_KEY_LENGTH	(176)	/* length of expanded AES key */
+#define PIPER_MAX_KEYS		(4)
+#define AES_BLOB_LENGTH		(48)	/* length of AES IV and headers */
+
+/* Calibration constants */
+#define WCD_MAGIC		"WCALDATA"
+#define WCD_MAX_CAL_POINTS	(8)
+#define WCD_CHANNELS_BG		(14)
+#define WCD_CHANNELS_A		(35)
+#define WCD_B_CURVE_INDEX       (0)
+#define WCD_G_CURVE_INDEX       (1)
+
+/*
+ * Set this #define to receive frames in the ISR.  This may improve
+ * performance under heavy load at the expense of interrupt latency.
+ */
+#define WANT_TO_RECEIVE_FRAMES_IN_ISR	(0)
+
+
+typedef u64 u48;
+
+/*
+ * This enum lists the possible LED states.
+ */
+enum led_states {
+    led_shutdown,
+    led_adhoc,
+    led_not_associated,
+    led_associated
+};
+
+/* Available leds */
+enum wireless_led {
+    STATUS_LED,
+    ACTIVITY_LED,
+};
+
+#define WCD_HW_REV_MASK			0xf000
+#define WCD_HW_REV_PROTOTYPE	0x0000
+#define WCD_HW_REV_PILOT		0x1000
+#define WCD_HW_REV_A			0x2000
+#define WCD_PLATFORM_MASK		0x0ff0
+#define WCD_CCW9P_PLATFORM		0x0010
+#define WCD_CCW9M_PLATFORM		0x0020
+
+
+typedef struct nv_wcd_header {
+    char magic_string[8];	/* WCALDATA */
+    char ver_major;		/* Major version in ascii */
+    char ver_minor;		/* Minor version in ascii */
+    u16 hw_platform;		/* Hardware Platform used for calibration */
+    u8 numcalpoints;		/* Number of points per curve */
+    u8 padding[107];		/* Reserved for future use */
+    u32 wcd_len;		/* Total length of the data section */
+    u32 wcd_crc;		/* Data section crc32 */
+} nv_wcd_header_t;
+
+typedef struct wcd_point {
+    s16 out_power;		/* Output Power */
+    u16 adc_val;		/* Measured ADC val */
+    u8 power_index;		/* Airoha Power Index */
+    u8 reserved[3];		/* For future use */
+} wcd_point_t;
+
+typedef struct wcd_curve {
+    u8 max_adc_value;		/* maximum allowed ADC value for this curve */
+    u8 reserved[3];		/* Resered for future use */
+    /* Calibration curve points */
+    wcd_point_t points[WCD_MAX_CAL_POINTS];
+} wcd_curve_t;
+
+typedef struct wcd_data {
+    nv_wcd_header_t header;
+    wcd_curve_t cal_curves_bg[WCD_CHANNELS_BG][2];
+    wcd_curve_t cal_curves_a[WCD_CHANNELS_A];
+} wcd_data_t;
+
+typedef enum {
+    op_write,
+    op_or,
+    op_and
+} reg_op_t;
+
+enum antenna_select {
+    ANTENNA_BOTH = 0,
+    ANTENNA_1,
+    ANTENNA_2,
+};
+
+typedef struct {
+    bool loaded;
+    bool enabled;
+    bool weSentLastOne;
+} piperBeaconInfo_t;
+
+typedef enum {
+    RECEIVED_ACK,
+    TX_COMPLETE,
+    OUT_OF_RETRIES,
+    TX_NOT_DONE
+} tx_result_t;
+
+
+/* Structure that holds the information we need to support H/W AES encryption */
+struct piperKeyInfo {
+    bool valid;			/* indicates if this record is valid */
+    u8 addr[ETH_ALEN];		/* MAC address associated with key */
+    u32 expandedKey[EXPANDED_KEY_LENGTH / sizeof(u32)];
+    u48 txPn;			/* packet number for transmit */
+    u48 rxPn;			/* expected receive packet number */
+};
+
+/* rf */
+struct digi_rf_ops {
+    const char *name;
+    void (*init) (struct ieee80211_hw *, int);
+    int (*stop) (struct ieee80211_hw *);
+    int (*set_chan) (struct ieee80211_hw *, int chan);
+    int (*set_chan_no_rx) (struct ieee80211_hw *, int chan);
+    int (*set_pwr) (struct ieee80211_hw *, uint8_t val);
+    void (*set_pwr_index) (struct ieee80211_hw *, unsigned int val);
+    void (*power_on) (struct ieee80211_hw *, bool want_power_on);
+    void (*getOfdmBrs) (int channel, u64 brsBitMask, unsigned int *ofdm,
+			unsigned int *psk);
+    enum ieee80211_band (*getBand) (int);
+    int (*getFrequency) (int);
+    void (*set_hw_info)(struct ieee80211_hw *, int channel, u16 hw_platform);
+    const struct ieee80211_rate *(*getRate) (unsigned int);
+    int channelChangeTime;
+    s8 maxSignal;
+    struct ieee80211_supported_band *bands;
+    u8 n_bands;
+};
+
+struct piper_stats {
+    u32 rx_overruns;
+    u32 tx_complete_count;
+    u32 tx_start_count;
+    u32 tx_total_tetries;
+    u32 tx_retry_count[IEEE80211_TX_MAX_RATES];
+    u32 tx_retry_index;
+    struct ieee80211_tx_queue_stats tx_queue;
+    struct ieee80211_low_level_stats ll_stats;
+    spinlock_t lock;
+};
+
+enum piper_ps_mode {
+    PS_MODE_LOW_POWER,
+    PS_MODE_FULL_POWER
+};
+
+enum piper_ps_state {
+    PS_STATE_WAIT_FOR_BEACON,
+    PS_STATE_WAIT_FOR_STOP_TRANSMIT_EVENT,
+    PS_STATE_WAIT_FOR_TRANSMITTER_DONE,
+    PS_STATE_WAIT_FOR_WAKEUP_ALARM,
+    PS_STATE_WAIT_FOR_TRANSMITTER_DONE_EVENT
+};
+
+struct piper_priv;
+
+struct piper_ps {
+    u32 beacon_int;
+    u16 aid;
+	volatile unsigned int scan_timer;
+    unsigned int sleep_time;
+    struct timer_list timer;
+    enum piper_ps_mode mode;
+    enum piper_ps_state state;
+    unsigned int this_event;
+    spinlock_t lock;
+    volatile bool power_management;
+    volatile bool poweredDown;
+    volatile bool rxTaskletRunning;
+    volatile bool allowTransmits;
+    volatile bool stopped_tx_queues;
+    volatile unsigned int frames_pending;
+};
+
+typedef void (*tx_skb_return_cb_t)(struct ieee80211_hw *hw,
+				 struct sk_buff *skb);
+
+struct piper_queue {
+	struct sk_buff *skb;
+	tx_skb_return_cb_t skb_return_cb;
+};
+
+#define PIPER_TX_QUEUE_SIZE			(16)
+#define NEXT_TX_QUEUE_INDEX(x)		((x+1) & 0xf)
+
+struct piper_priv {
+    const char *drv_name;
+    char debug_cmd[32];
+    u32 version;
+    struct piper_pdata *pdata;
+    struct ieee80211_hw *hw;
+    struct ieee80211_key_conf txKeyInfo;
+    struct ieee80211_cts ctsFrame;
+    struct ieee80211_rts rtsFrame;
+    struct ieee80211_rate *calibrationTxRate;
+    struct piper_stats pstats;
+    struct tasklet_struct rx_tasklet;
+    struct tasklet_struct tx_tasklet;
+    spinlock_t tx_tasklet_lock;
+    bool tx_tasklet_running;
+    spinlock_t tx_queue_lock;
+    struct piper_queue tx_queue[PIPER_TX_QUEUE_SIZE];
+    unsigned int tx_queue_head;
+    unsigned int tx_queue_tail;
+    unsigned int tx_queue_count;
+    bool expectingAck;
+    struct timer_list tx_timer;
+    struct timer_list led_timer;
+    enum led_states led_state;
+    struct piper_ps ps;
+    bool power_save_was_on_when_suspended;
+    struct access_ops *ac;
+    spinlock_t aesLock;
+    struct digi_rf_ops *rf;
+    void *__iomem vbase;
+    int irq;
+    tx_result_t tx_result;
+    int tx_signal_strength;
+
+    /* Function callbacks */
+    int (*init_hw) (struct piper_priv *, enum ieee80211_band);
+    int (*deinit_hw) (struct piper_priv *);
+    void (*set_irq_mask_bit) (struct piper_priv *, u32);
+    void (*clear_irq_mask_bit) (struct piper_priv *, u32);
+     u16(*get_next_beacon_backoff) (void);
+    int (*load_beacon) (struct piper_priv *, u8 *, u32);
+    int (*rand) (void);
+    void (*tx_calib_cb) (struct piper_priv *);
+    int (*set_antenna) (struct piper_priv *, enum antenna_select);
+    int (*set_tracking_constant) (struct piper_priv * piperp,
+				  unsigned megahertz);
+    void (*adjust_max_agc) (struct piper_priv * piperp, unsigned int rssi,
+			    _80211HeaderType * header);
+
+    /* General settings */
+    enum nl80211_iftype if_type;
+    bool areWeAssociated;
+    bool is_radio_on;
+    bool use_short_preamble;
+    int channel;
+    int tx_power;
+    u8 bssid[ETH_ALEN];
+    bool tx_cts;
+    bool tx_rts;
+    enum antenna_select antenna;
+    int power_duty;
+
+    /* AES stuff */
+    bool use_hw_aes;
+    u32 aes_key_count;
+    struct piperKeyInfo key[PIPER_MAX_KEYS];
+    u32 tx_aes_key;
+    u32 tx_aes_blob[AES_BLOB_LENGTH / sizeof(u32)];
+
+    /* IBSS */
+    piperBeaconInfo_t beacon;
+};
+
+struct access_ops {
+    spinlock_t reg_lock;
+    int (*wr_reg) (struct piper_priv *, u8 reg, u32 val, reg_op_t op);
+     u32(*rd_reg) (struct piper_priv *, u8 reg);
+    int (*wr_fifo) (struct piper_priv *, u8 addr, u8 * buf, int len);
+    int (*rd_fifo) (struct piper_priv *, u8 addr, u8 * buf, int len);
+};
+
+struct piper_pdata {
+    u8 macaddr[6];
+    int rst_gpio;
+    int irq_gpio;
+    int status_led_gpio;
+    int rf_transceiver;
+    wcd_data_t wcd;
+    int i2c_adapter_num;
+    struct piper_priv *piperp;
+
+    /* Platform callbacks */
+    void (*reset) (struct piper_priv *, int);
+    int (*init) (struct piper_priv *);
+    int (*late_init) (struct piper_priv *);
+    void (*set_led) (struct piper_priv *, enum wireless_led, int);
+    void (*early_resume) (struct piper_priv *);
+};
+
+/* main */
+int piper_alloc_hw(struct piper_priv **priv, size_t priv_sz);
+void piper_free_hw(struct piper_priv *priv);
+int piper_register_hw(struct piper_priv *priv, struct device *dev,
+		      struct digi_rf_ops *rf);
+void piper_unregister_hw(struct piper_priv *priv);
+irqreturn_t piper_irq_handler(int irq, void *dev_id);
+void packet_tx_done(struct piper_priv *piperp,
+		    tx_result_t result, int singalstrength);
+void piper_rx_tasklet(unsigned long context);
+void piper_tx_tasklet(unsigned long context);
+bool piper_prepare_aes_datablob(struct piper_priv *digi,
+				unsigned int keyIndex, u8 * aesBlob,
+				u8 * frame, u32 length, bool isTransmit);
+void piper_load_mac_firmware(struct piper_priv *piperp);
+void piper_load_dsp_firmware(struct piper_priv *piperp);
+int piper_spike_suppression(struct piper_priv *piperp, bool retry);
+void piper_reset_mac(struct piper_priv *piperp);
+void piper_set_macaddr(struct piper_priv *piperp);
+int piper_hw_tx_private(struct ieee80211_hw *hw, struct sk_buff *skb, tx_skb_return_cb_t fn);
+void piper_empty_tx_queue(struct piper_priv *piperp);
+int piper_tx_enqueue(struct piper_priv *piperp, struct sk_buff *skb, tx_skb_return_cb_t skb_return_cb);
+struct sk_buff *piper_tx_getqueue(struct piper_priv *piperp);
+bool piper_tx_queue_half_full(struct piper_priv *piperp);
+void piper_set_macaddr(struct piper_priv *piperp);
+void piper_MacEnterActiveMode(struct piper_priv *piperp, bool want_spike_suppression);
+int piper_MacEnterSleepMode(struct piper_priv *piperp, bool force);
+void piper_sendNullDataFrame(struct piper_priv *piperp, bool isPowerSaveOn);
+void piper_ps_rx_task_exiting(struct piper_priv *piperp);
+void piper_ps_scan_event(struct piper_priv *piperp);
+
+/*
+ * Defines for debugging function dumpRegisters
+ */
+#define MAIN_REGS           (1)
+#define MAC_REGS            (2)
+#define RF_REGS             (4)
+#define FRAME_BUFFER_REGS   (8)
+#define CTRL_STATUS_REGS    (0x10)
+#define FIFO_REGS           (0x20)
+#define IRQ_REGS            (0x40)
+#define ALL_REGS            (0xf)
+
+void digiWifiDumpRegisters(struct piper_priv *digi, unsigned int regs);
+void digiWifiDumpSkb(struct sk_buff *skb);
+
+extern void digiWifiDumpWordsAdd(unsigned int word);
+extern void digiWifiDumpWordsDump(void);
+extern void digiWifiDumpWordsReset(void);
+
+#endif				/* __PIPER_H_ */
diff --git a/drivers/net/wireless/digi_wi_g.c b/drivers/net/wireless/digi_wi_g.c
new file mode 100644
index 000000000000..af9dd8eb4694
--- /dev/null
+++ b/drivers/net/wireless/digi_wi_g.c
@@ -0,0 +1,5021 @@
+/*
+ *  wifi/digi_wi_g.c
+ *
+ *  Copyright (C) 2007 by Digi International Inc.
+ *  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 version2  as published by
+ *  the Free Software Foundation.
+*/
+/*
+ *  !Revision:   $Revision: 1.147 $
+ *  !Author:     Bernd Westermann/Markus Pietrek/Miriam Ruiz
+ *  !Descr:
+ *  !References: [1] 802.11G_Programming_Guide_1.0
+ *               [2] /net/m/ISO_STANDARDS/802.11g-2003.pdf
+ *               [3] Wireless LANs: 802.11 WLAN Technologie und praktische
+ *                   Umsetzung im Detail
+*/
+
+#include <linux/module.h>
+#include <linux/platform_device.h> /* platform_get */
+#include <linux/init.h>		/* __init */
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ioport.h>	/* request_region */
+#include <linux/delay.h>	/* udelay */
+#include <linux/crc32.h>
+#include <linux/vmalloc.h>      /* vmalloc */
+#include <linux/random.h>       /* get_random_bytes */
+#include <linux/workqueue.h>
+
+#include <asm/uaccess.h>
+#include <asm/gpio.h>
+#include <asm/io.h>
+#include <asm/irq.h>		/* NO_IRQ */
+#include <mach/regs-mem.h>
+#include <mach/regs-sys-common.h>
+
+/* for supported_rates */
+#include <linux/../../net/ieee80211/softmac/ieee80211softmac_priv.h>
+#undef assert
+
+#include "digi_wi_g.h"
+
+#define REMOVE_ME
+
+#ifdef CONFIG_DIGI_WI_G_DEBUG
+# define DBG_INIT          0x0001
+# define DBG_MINOR         0x0002
+# define DBG_RX            0x0004
+# define DBG_TX            0x0008
+# define DBG_INT           0x0010
+# define DBG_INTERFACE     0x0020
+# define DBG_LINK          0x0040
+# define DBG_SECURITY      0x0080
+# define DBG_UPDATE_RATE   0x0100
+# define DBG_TIMEOUT       0x0200
+# define DBG_TIME          0x0800
+# define DBG_MAJOR         0x1000
+# define DBG_HW_IO         0x2000
+# define DBG_ERROR         0x4000
+# define DBG_ERROR_CRIT    0x8000
+
+static unsigned int dw_dbg_level = DBG_ERROR_CRIT;
+
+# define DBG(flag, format, ...)          \
+	do { \
+		if ((dw_dbg_level & (flag)) == (flag))           \
+			printk(KERN_INFO format "\n", ##__VA_ARGS__); \
+	} while (0)
+# define DBG_FN(flag)                       \
+	do {      \
+		if ((dw_dbg_level & (flag)) == (flag))       \
+			printk(KERN_INFO DRIVER_NAME "@%s:line %d\n", __func__, __LINE__); \
+	} while (0)
+# define DBG_EXEC(expr) \
+	do { \
+		expr; \
+	} while (0)
+
+# define ASSERT(expr) \
+	do {                          \
+		if (!(expr)) {                               \
+			ERROR("Assertion failed! line %d %s",     \
+				__LINE__,#expr);           \
+		}                                                       \
+	} while (0)
+# define REQUIRE_LOCKED_ANY(lock) \
+	do { \
+		if (unlikely(!spin_is_locked(&lock))) {      \
+			ERROR(#lock " should be locked\n"); \
+			dump_stack();                             \
+		}                                                 \
+	} while (0)
+
+# define REQUIRE_UNLOCKED_ANY(lock) \
+	do { \
+		if (unlikely(spin_is_locked(&lock))) {      \
+			ERROR(#lock " should be unlocked\n"); \
+			dump_stack();                             \
+		}                                                 \
+	} while (0)
+
+# define REQUIRE_LOCKED(priv)   REQUIRE_LOCKED_ANY(priv->lock)
+# define REQUIRE_UNLOCKED(priv) REQUIRE_UNLOCKED_ANY(priv->lock)
+
+/* for timing collection */
+enum {
+	INT = 0,
+	INT_DONE,
+	INT_TASKLET,
+	INT_TASKLET_DONE,
+	RX_FIFO,
+	RX_AES_FIFO,
+	TX_FIFO,
+	TX_AES_FIFO,
+	RX_TASKLET,
+	RX_TASKLET_DONE,
+	RX_FRAME_TO_STACK,
+	RX_FRAME_TO_STACK_DONE,
+	RX_PROCESS_FRAME,
+	RX_PROCESS_FRAME_RX,
+	RX_PROCESS_FRAME_DONE,
+	RX_DECRYPT,
+	RX_DECRYPT_DONE,
+	RX_OVERRUN,
+	RX_WORK,
+	RX_WORK_DONE,
+	RX_PAUSE,
+	START_XMIT,
+	START_XMIT_DONE,
+	TX_TASKLET,
+	TX_TASKLET_DONE,
+	TX_SEND,
+	INTERMEDIATE,
+	INTERMEDIATE2,
+	SET_CHANNEL,
+};
+
+# define NAME(x) [x] = #x
+
+static const char* ns_hperf_type_names[] = {
+	NAME(INT),
+	NAME(INT_DONE),
+	NAME(INT_TASKLET),
+	NAME(INT_TASKLET_DONE),
+	NAME(RX_FIFO),
+	NAME(RX_AES_FIFO),
+	NAME(TX_FIFO),
+	NAME(TX_AES_FIFO),
+	NAME(RX_TASKLET),
+	NAME(RX_TASKLET_DONE),
+	NAME(RX_FRAME_TO_STACK),
+	NAME(RX_FRAME_TO_STACK_DONE),
+	NAME(RX_PROCESS_FRAME),
+	NAME(RX_PROCESS_FRAME_RX),
+	NAME(RX_PROCESS_FRAME_DONE),
+	NAME(RX_DECRYPT),
+	NAME(RX_DECRYPT_DONE),
+	NAME(RX_OVERRUN),
+	NAME(RX_WORK),
+	NAME(RX_WORK_DONE),
+	NAME(RX_PAUSE),
+	NAME(START_XMIT),
+	NAME(START_XMIT_DONE),
+	NAME(TX_TASKLET),
+	NAME(TX_TASKLET_DONE),
+	NAME(TX_SEND),
+	NAME(INTERMEDIATE),
+	NAME(INTERMEDIATE2),
+	NAME(SET_CHANNEL),
+};
+
+#define NS_HPERF_SIZE 2048
+
+/* ns_hperf_type_names needs to be define first before including */
+//# include <asm/arch-ns9xxx/ns9xxx_hperf.h>
+
+/* only in debug case we are interested in revision */
+# define REVISION " $Revision: 1.147 $"
+#else /* CONFIG_DIGI_WI_G_DEBUG */
+# define DBG(flag, format, ...)		do {} while (0)
+# define DBG_FN(flag)			do {} while (0)
+# define DBG_EXEC(flag)			do {} while (0)
+# define ASSERT(expr)			do {} while (0)
+# define REQUIRE_LOCKED(lock)		do {} while (0)
+# define REQUIRE_LOCKED_ANY(lock)	do {} while (0)
+# define REQUIRE_UNLOCKED(lock)		do {} while (0)
+# define REQUIRE_UNLOCKED_ANY(lock)	do {} while (0)
+# define REVISION ""
+#endif /* CONFIG_DIGI_WI_G_DEBUG */
+
+#define CLEAR(x)			memset(&(x), 0, sizeof((x)))
+#define dw_iosetbits32(offs, mask)	dw_iowrite32(dw_ioread32(offs)|(mask),(offs))
+#define dw_iocleanbits32(offs, mask)	dw_iowrite32(dw_ioread32(offs)&~(mask),(offs))
+
+#define BASIC_RATE_MASK(x) ((x) & ((unsigned char) ~IEEE80211_BASIC_RATE_MASK))
+#define BASIC_RATE(x) ((x) | IEEE80211_BASIC_RATE_MASK)
+
+#define ERROR(format, ...)	printk(KERN_ERR "*** ERROR " DRIVER_NAME\
+					" @ %s: " format "\n",	\
+					__func__,		\
+					##__VA_ARGS__)
+#define ERRORL(format, ...)	printkl(KERN_ERR "*** ERROR " DRIVER_NAME\
+					" @ %s: " format "\n",	\
+					__func__,		\
+					##__VA_ARGS__)
+#define WARNL(format, ...)	printkl(KERN_INFO DRIVER_NAME\
+					" @ %s: " format "\n",	\
+					__func__,		\
+					##__VA_ARGS__)
+#define to_dev(pdev)		platform_get_drvdata(pdev)
+
+#define rate_is_enough(priv)	((priv)->rate.tx_data_any >= 4)
+/* at least 3/4 are acknowledged */
+#define rate_is_success(priv)	\
+	(4 * (priv)->rate.tx_data_ack > 3 * (priv)->rate.tx_data_any)
+/* less than 1/4 are not acknowledged */
+#define rate_is_failure(priv)	\
+	(4 * (priv)->rate.tx_data_ack < (priv)->rate.tx_data_any)
+
+#define MAC_GROUP		0x01  /* broadcast or multicast address */
+#define IS_GROUP_ADDR(addr)	(addr[ 0 ] & MAC_GROUP)
+
+#define IS_FRAME(fc, ftype, stype) \
+	(((fc) & (IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) == (ftype | stype))
+#define IS_DATA(fc)		IS_FRAME(fc, IEEE80211_FTYPE_DATA, IEEE80211_STYPE_DATA)
+#define IS_MGMT(fc)		IS_FRAME(fc, IEEE80211_FTYPE_MGMT, 0 /* any */)
+
+
+#define ACK_SIZE	14 /* ACK frame size */
+
+#define USE_SHORTPRE(fi, rate)	\
+	(((rate) != IEEE80211_CCK_RATE_1MB) && fi->use_short_preamble)
+/* from Net+OS: mac_rate.c */
+/* not including SIFS and PLCP preamble/header */
+#define	LENGTH(bytes, rate)	((16 * (bytes) + (rate) - 1) / (rate))
+/* Length (in usecs) of SIFS and PLCP preamble/header. */
+#define	PRE_LEN( fi, rate)	(USE_SHORTPRE(fi, rate) ? 106 : 202)
+/* Duration (in usecs) of an OFDM frame at rate (in 500kbps units)
+ * including SIFS and PLCP preamble/header */
+#define	OFDM_DUR(bytes, rate)	(36 + 4 * ((4 * (bytes) + (rate) + 10) / (rate)))
+
+typedef struct {
+	u8  bps;	/* bit rate in 500kbps units */
+	u8  ofdm_code;	/* ofdm rate code, 0 if not ofdm */
+	u16 ack_len;	/* duration of ack or cts in usecs */
+} rate_info_t;
+
+typedef int dw_rate_index_t;
+
+/* separate maintained static because of polling performance */
+static void* __iomem vbase = NULL;
+
+static void dw_cw_set(const dw_priv_t* priv, u16 fc);
+static void dw_rx_fifo_error(dw_priv_t* priv);
+static int  dw_rx_frame_get_length(const dw_frame_rx_t* frame);
+static int  dw_rx_frame_is_duplicate(dw_priv_t* priv,
+		const struct ieee80211_hdr_3addr* hdr);
+static void dw_rx_frame_give_to_stack(dw_priv_t* priv, dw_frame_rx_t* frame);
+static void dw_rx_frame_fetch(dw_priv_t* priv);
+static void dw_rx_tasklet_handler(unsigned long data);
+static void dw_tx_set_plcp(dw_frame_tx_info_t* fi, int fragment, int rate);
+static int dw_tx_frame_prepare(dw_priv_t* priv,
+		dw_frame_tx_info_t* fi, struct ieee80211_txb* txb);
+static void dw_tx_fragment_send(dw_priv_t* priv, dw_frame_tx_t* frame);
+static void dw_tx_tasklet_handler(unsigned long data);
+static irqreturn_t dw_int(int irq, void *dev_id);
+static void dw_tx_reset(dw_priv_t* priv);
+static void dw_tx_wait_for_idle_and_pause(dw_priv_t* priv);
+static void dw_tx_continue_queue(dw_priv_t* priv);
+static int dw_ieee80211_hard_start_xmit(struct ieee80211_txb* txb,
+		struct net_device* dev, int priority);
+static void dw_update_status_led(dw_priv_t* priv);
+static void dw_rate_reset(dw_priv_t* priv);
+static void dw_rate_update(dw_priv_t* priv);
+static void dw_management_timer(u_long a);
+
+/* initialization stuff */
+static int  __init dw_hw_init_card(struct net_device* dev);
+static int  __init dw_start_dev(struct platform_device* pdev);
+static int  __init dw_probe( struct platform_device* pdev);
+static void dw_stop_dev(struct platform_device* pdev);
+static int  dw_remove(struct platform_device* pdev);
+static void dw_release_device(struct device* dev);
+
+static void dw_set_channel(dw_priv_t*priv, u8 channel);
+
+/* softmac/ieee interface stuff */
+static void dw_softmac_txrates_change(struct net_device* dev, u32 changes);
+static int dw_wx_set_encode(struct net_device* dev,
+		struct iw_request_info* info,
+		union iwreq_data* data, char* extra);
+static int dw_wx_set_encodeext(struct net_device* dev,
+		struct iw_request_info* info, union iwreq_data* data,
+		char* extra);
+static void dw_softmac_notify_authenticated(
+	struct net_device* dev, int event_type, void* context);
+static void dw_softmac_set_chan(struct net_device* dev, u8 channel);
+static void dw_softmac_set_bssid_filter(
+	struct net_device *dev, const u8* bssid);
+static void dw_ieee80211_set_security(struct net_device* dev,
+		struct ieee80211_security* sec);
+static int dw_geo_init(dw_priv_t* priv);
+static int dw_open(struct net_device* dev);
+static int dw_close(struct net_device* dev);
+static void dw_set_multicast_list(struct net_device* dev);
+
+/* ********** Local Variables ********** */
+
+static const char* dw_version =
+	"WiFi: " DRIVER_NAME " driver " COMPILE_TIME REVISION;
+
+/* define the resources the driver will use */
+static struct resource dw_mem = {
+	.name  = DRIVER_NAME,
+	.start = MAC_BASE_PHYS,
+	.end   = MAC_BASE_PHYS + MAC_BASE_SIZE,
+	.flags = IORESOURCE_MEM,
+};
+
+/* describes the device */
+static struct platform_device dw_device = {
+	.id     = -1,
+	.name   = DRIVER_NAME,/* must be equal to platform-driver.driver.name*/
+	.resource = &dw_mem,
+	.dev = {
+		.release = dw_release_device,
+	},
+};
+
+/* describes the driver */
+static struct platform_driver dw_driver = {
+	.probe  = dw_probe,
+	.remove = dw_remove,
+	.driver = {
+		.name  = DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+};
+
+/* RF transceiver frequency divider for each channel */
+#if defined(CONFIG_DIGI_WI_G_UBEC_JD)
+static const struct {
+	u16 integer;
+	u16 fraction;
+} freq_table[] = {
+	{ 0, 0 },
+	{ 0x6847, 0x0999 }, /* 1 (2412 MHz) */
+	{ 0x6847, 0x099b }, /* 2 (2417 MHz) */
+	{ 0x6867, 0x0998 }, /* 3 */
+	{ 0x6867, 0x099a }, /* 4 */
+	{ 0x6867, 0x0999 }, /* 5 */
+	{ 0x6867, 0x099b }, /* 6 */
+	{ 0x6857, 0x0998 }, /* 7 */
+	{ 0x6857, 0x099a }, /* 8 */
+	{ 0x6857, 0x0999 }, /* 9 */
+	{ 0x6857, 0x099b }, /* 10 */
+	{ 0x6877, 0x0998 }, /* 11 */
+	{ 0x6877, 0x099a }, /* 12 */
+	{ 0x6877, 0x0999 }, /* 13 (2472 MHz) */
+	{ 0x684f, 0x0ccc }, /* 14 (2484 MHz) */
+};
+#elif defined(CONFIG_DIGI_WI_G_UBEC_HC)
+static const struct {
+	u16 integer;
+	u16 fraction;
+} freq_table[] = {
+	{ 0, 0 },
+	{ 0x04c7, 0x0999 }, /* 1 (2412 MHz) */
+	{ 0x04c7, 0x099b }, /* 2 (2417 MHz) */
+	{ 0x04e7, 0x0998 }, /* 3 */
+	{ 0x04e7, 0x099a }, /* 4 */
+	{ 0x04e7, 0x0999 }, /* 5 */
+	{ 0x04e7, 0x099b }, /* 6 */
+	{ 0x04d7, 0x0998 }, /* 7 */
+	{ 0x04d7, 0x099a }, /* 8 */
+	{ 0x04d7, 0x0999 }, /* 9 */
+	{ 0x04d7, 0x099b }, /* 10 */
+	{ 0x04f7, 0x0998 }, /* 11 */
+	{ 0x04f7, 0x099a }, /* 12 */
+	{ 0x04f7, 0x0999 }, /* 13 (2472 MHz) */
+	{ 0x04cf, 0x0ccc }, /* 14 (2484 MHz) */
+};
+#endif
+/* see [2] 10.4.42. */
+static const u8 dw_rates[ RATES_SUPPORTED ] = {
+	BASIC_RATE(IEEE80211_CCK_RATE_1MB),
+	BASIC_RATE(IEEE80211_CCK_RATE_2MB),
+	BASIC_RATE(IEEE80211_CCK_RATE_5MB),
+	/* we need to give 11MB also to AP, as otherwise we are not
+	 * authenticated or softmac ignores it because rates don't match */
+	BASIC_RATE(IEEE80211_CCK_RATE_11MB),
+	IEEE80211_OFDM_RATE_6MB,
+	IEEE80211_OFDM_RATE_9MB,
+	IEEE80211_OFDM_RATE_12MB,
+	IEEE80211_OFDM_RATE_18MB,
+	IEEE80211_OFDM_RATE_24MB,
+	IEEE80211_OFDM_RATE_36MB,
+	IEEE80211_OFDM_RATE_48MB,
+	IEEE80211_OFDM_RATE_54MB,
+};
+
+/* basic rate will be calculated */
+#define MK_CCK(rate, ofdm) \
+	{ .bps = rate, .ofdm_code = ofdm, .ack_len = LENGTH(ACK_SIZE, rate) }
+#define MK_OFDM(rate, ofdm) \
+	{ .bps = rate, .ofdm_code = ofdm, .ack_len = OFDM_DUR(ACK_SIZE, rate) }
+/* they need to be ordered in their bitrate, because softmac returns us
+ * ap_ri.rate this way, and rates_info uses the indexes. */
+static const rate_info_t rates_info[ RATES_SUPPORTED ] = {
+	MK_CCK( IEEE80211_CCK_RATE_1MB,    0  ),
+	MK_CCK( IEEE80211_CCK_RATE_2MB,    0  ),
+	MK_CCK( IEEE80211_CCK_RATE_5MB,    0  ),
+	MK_OFDM(IEEE80211_OFDM_RATE_6MB,   0xb),
+	MK_OFDM(IEEE80211_OFDM_RATE_9MB,   0xf),
+	MK_CCK( IEEE80211_CCK_RATE_11MB,   0  ),
+	MK_OFDM(IEEE80211_OFDM_RATE_12MB,  0xa),
+	MK_OFDM(IEEE80211_OFDM_RATE_18MB,  0xe),
+	MK_OFDM(IEEE80211_OFDM_RATE_24MB,  0x9),
+	MK_OFDM(IEEE80211_OFDM_RATE_36MB,  0xd),
+	MK_OFDM(IEEE80211_OFDM_RATE_48MB,  0x8),
+	MK_OFDM(IEEE80211_OFDM_RATE_54MB,  0xc),
+};
+#undef MK_OFDM
+#undef MK_CCK
+
+#ifdef CONFIG_DIGI_WI_G_HW_ENCRYPTION
+# define DW_SW_AES_DEFAULT 0
+#else
+# define DW_SW_AES_DEFAULT 1
+#endif  /* CONFIG_DIGI_WI_G_HW_ENCRYPTION */
+
+static unsigned int dw_sw_aes  = DW_SW_AES_DEFAULT;
+static unsigned int dw_cfg_vco = 0;  /* use built-in */
+
+/* Tables for the conversion of the signal strenght to dBm */
+/* Map LNA value to gain value */
+static const unsigned char lnatable[] =	{0, 0, 23, 39};
+/* Map high gain values to dBm */
+static const char gaintable[] = {-82, -84, -85, -86, -87, -89, -90, -92, -94, -98};
+
+/* ********** Local Functions ********** */
+
+/* ********** inline stuff ********** */
+
+/**
+ * dw_to_48 - converts to numbers to an 48bit number
+ */
+static inline u48 dw_to_48(u32 n1, u16 n0)
+{
+	return (((u64) n1) << 16) | n0;
+}
+
+/**
+ * dw_48_inc - increments a 48bit number, wrapping aroung on 1<<48
+ */
+static inline void dw_inc_48(u48* n)
+{
+	(*n)++;
+	*n &= ((u64) 1 << 48) - 1;
+}
+
+/**
+ * dw_ioread32 - reads from memory mapped FPGA
+ */
+static inline u32 dw_ioread32(u32 offs)
+{
+	u32 val = ioread32(vbase + offs);
+
+	DBG(DBG_HW_IO, "R %04x = %x", offs, val);
+
+	return val;
+}
+
+/**
+ * dw_write32 - writes to memory mapped FPGA
+ */
+static inline void dw_iowrite32(u32 val, u32 offs)
+{
+	DBG(DBG_HW_IO, "W %04x = %x", offs, val);
+	iowrite32(val, vbase + offs);
+}
+
+/**
+ * dw_hw_get_status - returns the FPGA's status
+ */
+static inline u32 dw_hw_get_status(const dw_priv_t* priv)
+{
+	REQUIRE_LOCKED(priv);
+
+	return dw_ioread32(HW_GEN_STATUS);
+}
+
+/**
+ * dw_hw_memcpy_to - copies memory to FPGA @ offs
+ */
+static inline void dw_hw_memcpy_to(u32 offs, const void* _src, int len)
+{
+	u32* src = (u32 *) _src;
+
+	for (; len > 0; len -= 4, offs += 4)
+		dw_iowrite32(cpu_to_be32(*src++), offs);
+}
+
+/**
+ * dw_hw_memcpy_from - copies memory from FPGA @ offs
+ */
+static inline void dw_hw_memcpy_from(void* _dst, u32 offs, int len)
+{
+	u32* dst = (u32 *) _dst;
+
+	for (; len > 0; len -= 4, offs += 4)
+		*dst++ = be32_to_cpu(dw_ioread32(offs));
+}
+
+static inline void dw_hw_write_rf(u8 addr, u32 data)
+{
+	dw_iowrite32((((u32) addr) << 20) | data, HW_SPI_DATA);
+
+	udelay(10);
+}
+
+/**
+ * dw_hw_read_fifo - read's the FIFO's and swaps data
+ *
+ * no check for underrun is performed
+ */
+static inline void dw_hw_read_fifo(void * _dst, int _len)
+{
+	u32* dst = (u32 *) _dst;
+	int len = _len;
+
+	if (dw_ioread32(HW_GEN_STATUS) & STAT_RXFE)
+		printk("Reading from an EMPTY RX FIFO\n");
+
+	for (; len >= 32; len -= 32) {
+		*dst++ = be32_to_cpu(dw_ioread32(HW_DATA_FIFO));
+		*dst++ = be32_to_cpu(dw_ioread32(HW_DATA_FIFO));
+		*dst++ = be32_to_cpu(dw_ioread32(HW_DATA_FIFO));
+		*dst++ = be32_to_cpu(dw_ioread32(HW_DATA_FIFO));
+		*dst++ = be32_to_cpu(dw_ioread32(HW_DATA_FIFO));
+		*dst++ = be32_to_cpu(dw_ioread32(HW_DATA_FIFO));
+		*dst++ = be32_to_cpu(dw_ioread32(HW_DATA_FIFO));
+		*dst++ = be32_to_cpu(dw_ioread32(HW_DATA_FIFO));
+	}
+
+	for (; len > 0; len -= 4)
+		*dst++ = be32_to_cpu(dw_ioread32(HW_DATA_FIFO));
+}
+
+/**
+ * dw_hw_write_fifo - writes swapped data to FIFO
+ *
+ * no check for overrun is performed
+ */
+static inline void dw_hw_write_fifo(const void* _src, int _len)
+{
+	const u32* src = (const u32 *) _src;
+	int len = _len;
+
+	for (; len >= 32; len -= 32) {
+		dw_iowrite32(cpu_to_be32(*src++), HW_DATA_FIFO);
+		dw_iowrite32(cpu_to_be32(*src++), HW_DATA_FIFO);
+		dw_iowrite32(cpu_to_be32(*src++), HW_DATA_FIFO);
+		dw_iowrite32(cpu_to_be32(*src++), HW_DATA_FIFO);
+		dw_iowrite32(cpu_to_be32(*src++), HW_DATA_FIFO);
+		dw_iowrite32(cpu_to_be32(*src++), HW_DATA_FIFO);
+		dw_iowrite32(cpu_to_be32(*src++), HW_DATA_FIFO);
+		dw_iowrite32(cpu_to_be32(*src++), HW_DATA_FIFO);
+	}
+
+	for (; len > 0; len -= 4)
+		dw_iowrite32(cpu_to_be32(*src++), HW_DATA_FIFO);
+}
+
+/**
+ * dw_hw_aes_read_fifo - read's the AES FIFO's and swaps data
+ *
+ */
+static inline void dw_hw_aes_read_fifo(void* _dst, int len)
+{
+	int timeout = AES_BUSY_TIMEOUT;
+	u32* dst = (u32 *) _dst;
+
+	while (len > 0) {
+		if (!(dw_ioread32(HW_RSSI_AES) & AES_EMPTY)) {
+			*dst++ = be32_to_cpu(dw_ioread32(HW_AES_FIFO));
+			len -= 4;
+		} else {
+			/* !TODO. No calibration. When interrupts are enabled, use jiffies */
+			if (!timeout) {
+				ERROR("Timeout on read AES FIFO @ %i", len);
+				break;
+			}
+			timeout--;
+		}
+	} /* while (len > 0) */
+}
+
+/**
+ * dw_hw_aes_read_fifo_noswap - read's the AES FIFO's
+ *
+ */
+static inline void dw_hw_aes_read_fifo_noswap(void* _dst, int len)
+{
+	int timeout = AES_BUSY_TIMEOUT;
+	u32* dst = (u32 *) _dst;
+
+	while (len > 0) {
+		if (!(dw_ioread32(HW_RSSI_AES) & AES_EMPTY)) {
+			*dst++ = dw_ioread32(HW_AES_FIFO);
+			len -= 4;
+		} else {
+			/* !TODO: No calibration. When interrupts are enabled, use jiffies */
+			if (!timeout) {
+				ERROR("Timeout on read AES FIFO @ %i", len);
+				break;
+			}
+			timeout--;
+		}
+	} /* while (len > 0) */
+}
+
+/**
+ * dw_hw_aes_write_fifo - writes swapped data to AES FIFO
+ *
+ * no check for overrun is performed
+ */
+static inline void dw_hw_aes_write_fifo(const void* _src, int len)
+{
+	const u32* src = (const u32 *) _src;
+	int timeout = AES_BUSY_TIMEOUT;
+
+	while (len > 0) {
+		if (!(dw_ioread32(HW_RSSI_AES) & AES_FULL)) {
+			dw_iowrite32(cpu_to_be32(*src++), HW_AES_FIFO);
+			len -= 4;
+		} else {
+			/* !TODO: No calibration. When interrupts are enabled, use jiffies */
+			if (!timeout) {
+				ERROR("Timeout on write AES FIFO");
+				break;
+			}
+			timeout--;
+		}
+	} /* while (len > 0) */
+}
+
+/**
+ * dw_hw_aes_write_fifo_noswap - writes to AES FIFO
+ *
+ * no check for overrun is performed
+ */
+static inline void dw_hw_aes_write_fifo_noswap(const void* _src, int len)
+{
+	const u32* src = (const u32 *) _src;
+	int timeout = AES_BUSY_TIMEOUT;
+
+	while (len > 0) {
+		if (!(dw_ioread32(HW_RSSI_AES) & AES_FULL)) {
+			dw_iowrite32(*src++, HW_AES_FIFO);
+			len -= 4;
+		} else {
+			/* !TODO: No calibration. When interrupts are enabled, use jiffies */
+			if (!timeout) {
+				ERROR("Timeout on write AES FIFO");
+				break;
+			}
+			timeout--;
+		}
+	} /* while (len > 0) */
+}
+
+/**
+ * dw_hw_aes_wait - waits until AES is finished
+ *
+ * @return: 0 on timeout, otherwise > 1
+ */
+static inline int dw_hw_aes_wait(void)
+{
+	int timeout = AES_BUSY_TIMEOUT;
+
+	/* !TODO: redesign it to run with interrupts enabled, then use jiffies */
+	DBG_FN(DBG_TIMEOUT);
+
+	while (timeout && (dw_ioread32(HW_RSSI_AES) & AES_BUSY)) {
+		timeout--;
+		ndelay(1);
+	}
+
+	if (!timeout)
+		ERROR("Timedout on AES");
+
+	return timeout;
+}
+
+/**
+ * dw_channel_to_freq_a - calculates frequency out of channel (for 802.11a)
+ */
+static inline int dw_channel_to_freq_a(u8 channel)
+{
+	return (5000 + (5 * channel));
+}
+
+/**
+ * dw_channel_to_freq_bg - calculates frequency out of channel (for 802.11b/g)
+ */
+static inline int dw_channel_to_freq_bg(u8 channel)
+{
+	int freq;
+
+	if (14 == channel)
+		freq = 2484;
+	else
+		freq = 2407 + (5 * channel);
+
+	return freq;
+}
+
+static inline void dw_set_led_on(int led, u8 on)
+{
+	gpio_set_value(led, !on);
+}
+
+/**
+ * dw_list_move - moves a list from src and appends it to dst
+ */
+static inline void dw_list_move(struct list_head* dst, struct list_head* src)
+{
+	struct list_head* cursor;
+	struct list_head* next;
+
+	list_for_each_safe(cursor, next, src)
+		list_move_tail(cursor, dst);
+}
+
+static inline void dw_hw_set_vco(int channel)
+{
+	u32 vco = dw_cfg_vco;
+
+	if (!vco)
+#if defined(CONFIG_DIGI_WI_G_UBEC_JD)
+		vco = 0x46662;
+#elif defined(CONFIG_DIGI_WI_G_UBEC_HC)
+		vco = 0x3020;
+#else
+		BUG();
+#endif
+	dw_hw_write_rf(3, vco);
+}
+
+/**
+ * dw_rate_info - returns the index to rates_basic/rates_info
+ *
+ * bitrate is in 500kbps
+ */
+static inline dw_rate_index_t dw_rate_info_index(int bitrate)
+{
+	dw_rate_index_t i = 0;
+
+	while (i < ARRAY_SIZE(rates_info)) {
+		if (rates_info[ i ].bps == bitrate)
+			return i;
+		i++;
+	}
+
+	ERROR("Unsupported rate %i\n", bitrate);
+
+	return 0;
+}
+
+/**
+ * dw_rx_pause - pauses the receiver
+ *
+ * This will lead probably to FIFO overruns. In this case, the FPGA will not
+ * send the acknowledgment, so the sender will try again.
+ * And this gives the stack time to clear the queue.
+ */
+static inline void dw_rx_pause(dw_priv_t* priv, char pause)
+{
+	DBG_FN(DBG_RX | DBG_MINOR);
+	REQUIRE_LOCKED(priv);
+
+	priv->rx.pause = pause;
+
+	if (pause)
+		dw_iocleanbits32(HW_INTR_MASK, INTR_RXFIFO);
+	else
+		dw_iosetbits32(HW_INTR_MASK, INTR_RXFIFO);
+}
+
+/* ********** now the not inline stuff ********** */
+
+/***********************************************************************
+ * @Function: dw_ccmp_get_data_tx
+ * @Return:
+ * @Descr: Get AES encryption data for a frame
+ ***********************************************************************/
+static void dw_ccmp_get_data_tx(dw_priv_t *priv, struct ieee80211_hdr_3addr * hdr,
+		const dw_fragment_tx_t* frag, ccmp_data_t* data,
+		u8* extiv, int dlen)
+{
+	ccmp_key_t *key   = frag->crypt.key;
+	u8 *bp;
+
+	DBG_FN(DBG_TX);
+
+	/* Increment packet number */
+	dw_inc_48(&key->tx_pn);
+
+	CLEAR(*data);
+
+	memset(extiv, 0, EXTIV_SIZE);
+
+	SET16(extiv, key->tx_pn & 0xffff);
+	extiv[3] = priv->ieee->tx_keyidx << 6 | EXT_IV;
+
+	SET32(&extiv[4], key->tx_pn >> 16);
+
+	/* Set up CCM initial block for MIC IV */
+	data->init[0] = 0x59;
+	data->init[1] = 0;
+	memcpy (&data->init[2], hdr->addr2, ETH_ALEN);
+	data->init[8]  = extiv[7];
+	data->init[9]  = extiv[6];
+	data->init[10] = extiv[5];
+	data->init[11] = extiv[4];
+	data->init[12] = extiv[1];
+	data->init[13] = extiv[0];
+	data->init[14] = dlen >> 8;
+	data->init[15] = dlen;
+
+	/* Set up MIC header blocks */
+	bp = (u8 *) &hdr->frame_ctl;
+
+	data->header[0] = 0;
+	data->header[1] = 22;
+	data->header[2] = bp[0] & 0xcf;
+	data->header[3] = bp[1] & 0xd7;
+	memcpy(&data->header[4], hdr->addr1, 3*ETH_ALEN);
+	data->header[22] = WLAN_GET_SEQ_FRAG(le16_to_cpu(hdr->seq_ctl));
+	data->header[23] = 0;
+	memset (&data->header[24], 0, 8);
+}
+
+/***********************************************************************
+ * @Function: dw_ccmp_get_data_rx
+ * @Return:
+ * @Descr: Get AES encryption data for a frame
+ ***********************************************************************/
+static int dw_ccmp_get_data_rx(dw_priv_t *priv,
+		const struct ieee80211_hdr_3addr * hdr,
+		int dlen, ccmp_data_t *data)
+{
+	ccmp_key_t *key;
+	u8 *bp;
+
+	DBG_FN(DBG_RX);
+
+	/* Not encrypted */
+	if (dlen < 0 || !(hdr->payload[3] & EXT_IV)) {
+		return 0;
+	}
+
+	/* Key not set */
+	key = &priv->aeskeys[hdr->payload[3] >> 6];
+	if (!key->valid) {
+		return 0;
+	}
+
+	CLEAR(*data);
+
+	/* Set up CCM initial block for MIC IV */
+	data->init[0] = 0x59;
+	data->init[1] = 0;
+	memcpy (data->init+2, hdr->addr2, ETH_ALEN);
+
+	/* extiv */
+	data->init[8]  = hdr->payload[7];
+	data->init[9]  = hdr->payload[6];
+	data->init[10] = hdr->payload[5];
+	data->init[11] = hdr->payload[4];
+	data->init[12] = hdr->payload[1];
+	data->init[13] = hdr->payload[0];
+	data->init[14] = dlen >> 8;
+	data->init[15] = dlen;
+
+	/* Set up MIC header blocks */
+	bp = (u8 *) &hdr->frame_ctl;
+
+	data->header[0] = 0;
+	data->header[1] = 22;
+	data->header[2] = bp[0] & 0xcf;
+	data->header[3] = bp[1] & 0xd7;
+	memcpy (data->header+4, hdr->addr1, 3*ETH_ALEN);
+	data->header[22] = WLAN_GET_SEQ_FRAG(le16_to_cpu(hdr->seq_ctl));
+	data->header[23] = 0;
+	memset (data->header+24, 0, 8);
+
+	return 1;
+}
+
+/***********************************************************************
+ * Function: dump_hex_buffer
+ * Return: nothing
+ * Descr: prints a buffer hexadecimal and with character if printable
+ ***********************************************************************/
+static void dump_hex_buffer(const void* buffer, const int len)
+{
+	const unsigned char* hexbuffer = (const unsigned char*)buffer;
+	int i;
+	const int colcount = 16;
+	const int colnum = 4;
+	const int colcut = colcount / colnum;
+
+	for (i = 0; i < len; i += colcount) {
+		/* print one row*/
+		int j, rowlen;
+
+		if (i+colcount <= len)
+			rowlen = colcount;
+		else
+			rowlen = len - i;
+
+		printk("%08X  ", (int) hexbuffer);
+		printk("    ");
+
+		/* print hexadecimal representation */
+		for (j = 0; j < rowlen; j++) {
+			printk("%02X ", *(hexbuffer+j));
+			if ((j + 1) % colcut == 0)
+				/* additional separator*/
+				printk(" ");
+		}
+
+		for (j = rowlen; j < colcount; j++)
+			printk("   ");
+
+		if (rowlen != colcount)
+			for (j = 0; j <= (colcount - rowlen - 1) / colcut; j++)
+				printk(" ");
+
+		printk("  ");
+
+		/* print character representation row */
+		for (j=0; j < rowlen; j++) {
+			unsigned char c = *(hexbuffer+j);
+			if (c < 32 || c > 127)
+				c = '.';
+
+			printk("%c", c);
+		}
+		printk("\n");
+		hexbuffer += colcount;
+	}
+}
+
+/**
+ * dw_plcp_get_bitrate_cck - returns the bitrate of HW's internal rate code
+ *
+ * retval is rate_code/5. But division is 1us slower than switch (5us to 4us)
+ */
+static int dw_plcp_get_bitrate_cck(int rate_code)
+{
+	volatile int ret;
+
+	DBG_FN(DBG_RX | DBG_MINOR);
+
+	switch(rate_code) {
+		case 0x0A: ret = IEEE80211_CCK_RATE_1MB;  break;
+		case 0x14: ret = IEEE80211_CCK_RATE_2MB;  break;
+		case 0x37: ret = IEEE80211_CCK_RATE_5MB;  break;
+		case 0x6E: ret = IEEE80211_CCK_RATE_11MB; break;
+		default:
+//			ERROR("Unknown rate_code %i cck, using 1MB\n", rate_code);
+			ret = IEEE80211_CCK_RATE_1MB;  break;
+	}
+
+	return ret;
+}
+
+/**
+ * dw_plcp_get_ratecode_cck - returns the HW's internal rate code from bitrate
+ *
+ * retval is 5*bitrate. But switch is 1us slower than multiplication (2us to 3us)
+ */
+static int dw_plcp_get_ratecode_cck(int bitrate)
+{
+	DBG_FN(DBG_TX | DBG_MINOR);
+
+	/* no check for unsupported bitrates, but upper layer should have handled it */
+	return 5 * bitrate;
+}
+
+/**
+ * dw_plcp_get_bitrate_ofdm - returns the bitrate of HW's internal rate code.
+ *
+ * see [1], 2.5.2
+ */
+static int dw_plcp_get_bitrate_ofdm(int rate_code)
+{
+	int ret;
+
+	DBG_FN(DBG_RX | DBG_MINOR);
+
+	switch(rate_code) {
+		case 0xB: ret = IEEE80211_OFDM_RATE_6MB;  break;
+		case 0xF: ret = IEEE80211_OFDM_RATE_9MB;  break;
+		case 0xA: ret = IEEE80211_OFDM_RATE_12MB; break;
+		case 0xE: ret = IEEE80211_OFDM_RATE_18MB; break;
+		case 0x9: ret = IEEE80211_OFDM_RATE_24MB; break;
+		case 0xD: ret = IEEE80211_OFDM_RATE_36MB; break;
+		case 0x8: ret = IEEE80211_OFDM_RATE_48MB; break;
+		case 0xC: ret = IEEE80211_OFDM_RATE_54MB; break;
+		default:
+			ERROR("Unknown rate_code %i ofdm, using 6MB\n", rate_code);
+			ret = IEEE80211_OFDM_RATE_6MB;  break;
+	}
+
+	return ret;
+}
+
+/**
+ * dw_plcp_get_ratecode_ofdm - returns the HW's internal rate code from bitrate
+ *
+ * see [1], 2.5.2
+ */
+static int dw_plcp_get_ratecode_ofdm(int bitrate)
+{
+	DBG_FN(DBG_TX | DBG_MINOR);
+
+	return rates_info[ dw_rate_info_index(bitrate) ].ofdm_code;
+}
+
+/**
+ * dw_cw_set - updates the contention window depending on the frame type
+ */
+static void dw_cw_set(const dw_priv_t* priv, u16 fc)
+{
+	int cw;
+
+	DBG_FN(DBG_INIT);
+	REQUIRE_LOCKED(priv);
+
+	if (fc & IEEE80211_STYPE_BEACON)
+		cw = 2 * CW_MIN + 1;
+	else if (fc & IEEE80211_STYPE_PROBE_RESP)
+		cw = CW_MIN;
+	else
+		cw = priv->cw;
+
+	/* Set backoff timer */
+	if (cw) {
+		u16 rand;
+		get_random_bytes(&rand, 2);
+
+		/* Pick a random time up to cw, convert to usecs */
+		cw = 10 * (rand & cw);
+		if (fc & IEEE80211_STYPE_BEACON)
+			dw_iowrite32(cw, HW_BEACON_BO);
+		else
+			dw_iowrite32(cw, HW_BACKOFF);
+	}
+}
+
+/**
+ * RF transceiver transmitter gain for each power level.
+ * This is the 0-15 power level, bit reversed.
+ */
+static unsigned char powerTable[] = {
+	0x0, 0x8, 0x4, 0xc, 0x2, 0xa, 0x6, 0xe,
+	0x1, 0x9, 0x5, 0xd, 0x3, 0xb, 0x7, 0xf
+};
+
+/**
+ * This is used to set the transmit power.  Values can range
+ * from 0-15, where 8 is the default and I am told provides about
+ * 12dBm (16mW) output power.
+ */
+static int dw_set_tx_power(struct dw_priv *priv, unsigned char value)
+{
+#if defined(CONFIG_DIGI_WI_G_UBEC_JD)
+	if (value > DW_TX_POWER_MAX_DBM)
+		value = DW_TX_POWER_MAX_DBM;
+
+	dw_hw_write_rf(5, 0x19e40 | powerTable[value]);
+	priv->tx_power = value;
+	return 1;
+#elif defined(CONFIG_DIGI_WI_G_UBEC_HC)
+	if (value < 0)
+		value = 0;
+	// Map max value (15) to 14 to avoid hardware problems
+	else if (value > 14)
+		value = 14;
+#ifdef _UNDEFINED_
+	dw_hw_write_rf(5, 0x09e40 | powerTable[value]);
+	priv->tx_power = value;
+	return 1;
+#else
+	dw_hw_write_rf(5, 0x09ee0);
+	priv->tx_power = 0;
+	return -EIO;
+#endif
+
+#endif // defined(CONFIG_DIGI_WI_G_UBEC_JD)
+}
+
+/**
+ * dw_rx_fifo_error - reports a fifo error and resets it
+ */
+static void dw_rx_fifo_error(dw_priv_t* priv)
+{
+	DBG_FN(DBG_RX);
+	REQUIRE_LOCKED(priv);
+	REQUIRE_LOCKED_ANY(priv->ieee->lock);
+
+	/* the reason of the error should have been reported already */
+
+	/* give a pulse */
+	dw_iosetbits32(HW_GEN_CONTROL, GEN_RXFIFORST);
+	wmb();
+	dw_iocleanbits32(HW_GEN_CONTROL, GEN_RXFIFORST);
+
+	priv->wstats.discard.misc++;
+}
+
+/**
+ * dw_rx_frame_get_length - determines the frame length from hardware values
+ *
+ * @return frame length in bytes or -1 on failure
+ */
+static int dw_rx_frame_get_length(const dw_frame_rx_t* frame)
+{
+	const dw_hw_hdr_rx_t* hdr = &frame->hdr;
+	int len = 0;
+
+	DBG_FN(DBG_RX);
+
+	if (MOD_OFDM == hdr->mod.mod_type) {
+		const dw_hw_ofdm_t* ofdm = &hdr->plcp.ofdm;
+		/* switch and no table because check for bad data from FPGA */
+		switch(ofdm->rate) {
+		case 0xB:
+		case 0xF:
+		case 0xA:
+		case 0xE:
+		case 0x9:
+		case 0xD:
+		case 0x8:
+		case 0xC:
+			break;
+		default:
+			ERROR("Wrong rate %i", ofdm->rate);
+			goto error;
+		}
+		len = ofdm->length;
+	} else {
+		const dw_hw_pskcck_t* pskcck = &hdr->plcp.pskcck;
+		int service;
+
+		/* service check */
+		service  = pskcck->service;
+		service &= ~SERVICE_LOCKED;
+		service &= ~SERVICE_MODSEL;
+		service &= ~SERVICE_LENEXT;
+
+		/* Use a switch to avoid doing a divide operation.*/
+		len = pskcck->length;
+		switch(pskcck->signal) {
+		case 10:
+			len /= 8;
+			break;
+		case 20:
+			len /= 4;
+			break;
+		case 55:
+			len = (11 * len) / 16;
+			break;
+		case 110:
+			len = (11 * len) / 8;
+			if (pskcck->service & SERVICE_LENEXT)
+			len--;
+			break;
+		default:
+			ERRORL("Signal not defined %i %i", pskcck->signal,
+			pskcck->length);
+
+			/* !TODO: Remove me when solved  */
+			dump_hex_buffer(hdr, sizeof(*hdr));
+			goto error;
+		}
+	}
+
+	/* check length for integrity?  */
+	if (unlikely(len > HW_RX_FIFO_SIZE)) {
+		ERRORL("Wrong size %i", len);
+		goto error;
+	}
+
+	return len;
+
+error:
+	return -1;
+}
+
+/**
+ * dw_rx_frame_is_duplicate - checks whether the frame has been already
+ * received. This is not done by hardware.
+ *
+ * @return 1 if duplicate otherwise 1
+ */
+static int dw_rx_frame_is_duplicate(dw_priv_t* priv,
+		const struct ieee80211_hdr_3addr* hdr)
+{
+	struct list_head* it;
+	dw_duplicate_t*   sender = NULL;
+	int               is_duplicate = 0;
+
+	REQUIRE_LOCKED(priv);
+
+	/* do we had anything from that sender already? */
+	list_for_each(it, &priv->rx.dups.known.list) {
+                /* addr2 is sender */
+                dw_duplicate_t* entry = list_entry(it, dw_duplicate_t, list);
+                if (!memcmp(entry->src, hdr->addr2, ARRAY_SIZE(entry->src))) {
+                        /* found sender */
+                        sender = entry;
+                        break;
+                }
+        } /* list_for_each */
+
+        if (NULL == sender) {
+                /* create an entry for the new sender */
+                struct list_head* new;
+
+                if (unlikely(list_empty(&priv->rx.dups.free.list)))
+                        /* the last one in known list was the first
+                         * added and so may possible be the oldest.
+                         * Using jiffies is probably not necessary */
+                        new = priv->rx.dups.known.list.prev;
+                else
+                        new = priv->rx.dups.free.list.next;
+
+                /* move it to head of known entries */
+                list_del(new);
+                list_add(new, &priv->rx.dups.known.list);
+
+                sender = list_entry(new, dw_duplicate_t, list);
+
+                memcpy(sender->src, hdr->addr2, ARRAY_SIZE(sender->src));
+        } else {
+                /* did we receive the frame already? */
+                u16 fc;
+
+                fc = le16_to_cpu(hdr->frame_ctl);
+                if ((fc & IEEE80211_FCTL_RETRY) &&
+                    (sender->seq_ctl == hdr->seq_ctl))
+                        /* we did see already the sequence number */
+                        is_duplicate = 1;
+        } /* if (NULL == sender) */
+
+        if (!is_duplicate)
+                /* update sequence control field */
+                sender->seq_ctl = hdr->seq_ctl;
+
+        return is_duplicate;
+}
+
+/**
+ * dw_rx_frame_decrypt - decrypts a frame when necessary.
+ *
+ * skb->data contains undecrypted data on return
+ */
+#ifndef REMOVE_ME
+static void dw_rx_frame_decrypt(dw_priv_t* priv, struct sk_buff* skb)
+{
+        DBG_FN(DBG_RX | DBG_MINOR);
+}
+#endif
+
+/**
+ * dw_rx_frame_give_to_stack - give the kernel/user the data
+ */
+static void dw_rx_frame_give_to_stack(dw_priv_t* priv, dw_frame_rx_t* frame)
+{
+        static struct ieee80211_rx_stats stats;
+	int gain; /* For received signal strength to dBm conversion */
+
+        DBG_FN(DBG_RX | DBG_MAJOR);
+
+        /* processing needs time, but we want to be able to stil copy frames
+         * from Rx FIFO. Therefore we are unlocked. */
+        REQUIRE_UNLOCKED(priv);
+
+#ifndef REMOVE_ME
+        dw_rx_frame_decrypt(priv, frame->skb);
+#endif
+
+        /* update stats */
+        CLEAR(stats);
+        stats.mask =
+                IEEE80211_STATMASK_RSSI   |
+                IEEE80211_STATMASK_SIGNAL |
+                IEEE80211_STATMASK_RATE;
+        stats.mac_time = jiffies;
+        stats.rate = ((MOD_OFDM == frame->hdr.mod.mod_type) ?
+                       dw_plcp_get_bitrate_ofdm(frame->hdr.plcp.ofdm.rate) :
+                       dw_plcp_get_bitrate_cck(frame->hdr.plcp.pskcck.signal));
+        stats.freq = IEEE80211_24GHZ_BAND;
+        stats.len  = frame->skb->len;
+
+
+	/* Convert received signal strength to dBm */
+	gain = lnatable[frame->hdr.mod.rssi_lna] + 2 * frame->hdr.mod.rssi_vga;
+	if (gain > 96)
+		stats.signal = -98;
+	else if (gain > 86)
+		stats.signal = gaintable[gain-87];
+	else
+		stats.signal = 5 - gain;
+
+	/* RSSI is used only internally to determine best network and not reported back */
+
+        /* RSSI (Received Signal Strength Indication) is a measurement of the
+         * power present in a received radio signal. In an IEEE 802.11 system
+         * RSSI is the received signal strength in a wireless environment, in
+         * arbitrary units. RSSI measurements will vary from 0 to 255 depending
+         * on the vendor. It consists of a one byte integer value. A value of 1
+         * will indicate the minimum signal strength detectable by the wireless
+         * card, while 0 indicates no signal. The value has a maximum of
+         * RSSI_Max. - http://en.wikipedia.org/wiki/RSSI
+         *
+         * See also: http://www.ces.clemson.edu/linux/dbm-rssi.shtml
+         */
+	stats.rssi   = MAC_RSSI_MAX
+		       - 15 * (frame->hdr.mod.rssi_lna - 1)
+		       - 2 * (frame->hdr.mod.rssi_vga);
+
+        /* we don't know it here, but ieee80211 stack will look into it with
+           our patch in ieee80211_rx.c before
+           + if (255 == stats->received_channel)
+           + stats->received_channel = network->channel;
+           memcpy(&network->stats, stats, sizeof(network->stats));
+        */
+        stats.received_channel = 255;
+
+        /* put it on stack */
+        ieee80211_rx_any(priv->ieee, frame->skb, &stats);
+
+        /* allocate next buffer */
+        frame->skb = dev_alloc_skb(DW_MTU);
+        if (unlikely(NULL == frame->skb)) {
+                dw_iocleanbits32(HW_GEN_CONTROL, GEN_RXEN);
+                panic(DRIVER_NAME ": Out of memory\n");
+        }
+}
+
+/**
+ * dw_rx_frame_fetch - Copies a frame from FIFO
+ */
+static void dw_rx_frame_fetch(dw_priv_t* priv)
+{
+        struct list_head* element;
+        dw_frame_rx_t*    frame;
+        int len;
+        int ignore_frame = 0;
+        u16 fc;
+	char spy_buf[2312];
+	unsigned int spy_len = 0;
+
+        DBG_FN(DBG_RX | DBG_INT);
+        REQUIRE_LOCKED(priv);
+
+        if (unlikely(list_empty(&priv->rx.queue.free.list))) {
+                ERROR("Frame received, but queue empty. Receiver should be paused");
+                len = 0xffff;   /* for DBG_MARK */
+
+                /* nowhere to store. Reset FIFO */
+                spin_lock(&priv->ieee->lock);
+                priv->ieee->stats.rx_over_errors++;
+                dw_rx_fifo_error(priv);
+                spin_unlock(&priv->ieee->lock);
+
+		return;
+        }
+
+        /* use a free skb and fill it with the frame */
+        element = priv->rx.queue.free.list.next;
+        frame   = list_entry(element, dw_frame_rx_t, list);
+
+	memset(spy_buf, 0, sizeof(spy_buf)); spy_len = 0;
+
+        /* copy frame header, swapped, we need it's header for length */
+        dw_hw_read_fifo(&frame->hdr, sizeof(frame->hdr));
+	memcpy(spy_buf + spy_len, &frame->hdr, sizeof(frame->hdr)); spy_len += sizeof(frame->hdr);
+
+        len = dw_rx_frame_get_length(frame);
+
+        if ((len >= sizeof(struct ieee80211_hdr)) && (len <= DW_MTU)) {
+                /* read data and if necessary decrypt it */
+                size_t remaining_len = len;
+                /*  */
+                size_t delta_decrypt_header = sizeof(struct ieee80211_hdr_3addr) + EXTIV_SIZE - sizeof(struct ieee80211_hdr);
+
+                struct ieee80211_hdr* hdr = (struct ieee80211_hdr*) frame->skb->data;
+                struct ieee80211_hdr_3addr* hdr3 = (struct ieee80211_hdr_3addr*) hdr;
+                char* data = frame->skb->data;
+
+
+                /* reads data and keeps track of remaining_len */
+#define READ_FIFO(to_read) do { \
+		dw_hw_read_fifo(data, to_read); \
+		memcpy(spy_buf + spy_len, data, to_read); spy_len += to_read; \
+		data += to_read; \
+		remaining_len -= to_read; \
+		ASSERT(remaining_len >= 0); } while (0)
+
+                /* read header */
+                READ_FIFO(sizeof(struct ieee80211_hdr));
+
+                fc = le16_to_cpu(hdr->frame_ctl);
+
+                if (!dw_sw_aes &&
+                    (fc & IEEE80211_FCTL_PROTECTED) &&
+                    (remaining_len > delta_decrypt_header)) {
+                        /* there is a encrypted frame present */
+                        ccmp_data_t cdata;
+                        ccmp_key_t* key;
+                        size_t data_len;
+                        int index;
+                        u48 pn;
+
+                        /* get frame headers and init vector from rx FIFO */
+                        READ_FIFO(delta_decrypt_header);
+
+                        /* get key index from message */
+                        index = (hdr3->payload[ 3 ] >> 6) & (WEP_KEYS - 1);
+                        key   = &priv->aeskeys[ index ];
+
+                        /* get packet number from IV and check for replay.
+                           packet number must be greated or equal than
+                           expected one. Takes care of wrap around. */
+                        pn = dw_to_48(GET32(&hdr3->payload[ 4 ]),
+                                       GET16(&hdr3->payload[ 0 ]));
+
+                        data_len = remaining_len - IEEE80211_FCS_LEN - MIC_SIZE;
+
+                        if (key->valid &&  /* we know the key */
+                            (pn - key->rx_pn >= 0) &&
+                            dw_ccmp_get_data_rx(priv, hdr3, data_len, &cdata)) {
+                                /* payload doesn't include MIC or CCMP */
+                                len -= CCMP_SIZE;
+
+                                /* !TODO. Convert AES_wait into a non-busy
+                                   polling function */
+                                /* retreive and decrypt encoded payload data */
+                                dw_hw_aes_wait();
+
+                                /* configure mode and key */
+                                dw_iowrite32(HW_AES_MODE_1 | (index & 0xf),
+                                              HW_AES_MODE);
+
+                                /* this read puts AES into decrypt mode */
+                                dw_ioread32(HW_AES_MODE);
+
+                                /* write key and init vector to AES engine */
+                                dw_hw_aes_write_fifo(&cdata, sizeof(cdata));
+
+                                /* get decrypted payload data.
+                                 * We will overvwrite the CCMP header
+                                 * previously read. But the stack doesn't want
+                                 * to see it anyway because it expects unencrypted data */
+                                dw_hw_aes_read_fifo(hdr3->payload, data_len);
+
+                                /* wait for MIC calculation to finish.
+                                   !TODO: convert to non-busy */
+                                dw_hw_aes_wait();
+
+                                if (dw_ioread32(HW_RSSI_AES) & AES_MIC) {
+                                        /* frame was ok */
+                                        dw_inc_48(&key->rx_pn);
+                                        /* the stack should not convert it */
+                                        hdr->frame_ctl = cpu_to_le16(fc & ~IEEE80211_FCTL_PROTECTED);
+                                } else {
+                                        ignore_frame = 1;
+                                        spin_lock(&priv->ieee->lock);
+                                        priv->ieee->ieee_stats.rx_discards_undecryptable++;
+                                        spin_unlock(&priv->ieee->lock);
+                                        ERROR("Wrong MIC");
+                                }
+                        } else {
+                                /* TKIP decrypted? Le'ts handle it by SW */
+                                READ_FIFO(remaining_len);
+                        }
+#undef READ_FIFO
+                } else {
+                        /* retrieve remaining unencrypted data.
+                           We read FCS, but stack will ignore it. */
+                        dw_hw_read_fifo(data, remaining_len);
+			memcpy(spy_buf + spy_len, data, remaining_len); spy_len += remaining_len;
+                }
+        } else {
+                if (len > DW_MTU)
+		{
+                        ERROR("Oversized frame with %i bytes, ignoring it\n", len);
+
+		}
+
+                spin_lock(&priv->ieee->lock);
+                priv->ieee->stats.rx_length_errors++;
+                dw_rx_fifo_error(priv);
+                spin_unlock(&priv->ieee->lock);
+
+                ignore_frame = 1;
+        }
+
+        if (!ignore_frame) {
+                /* process frame */
+
+                int skblen  = len - IEEE80211_FCS_LEN;/* need no FCS further */
+                int ignore  = 0;
+                int control = ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL);
+
+                switch(fc & IEEE80211_FCTL_FTYPE) {
+                    case IEEE80211_FTYPE_MGMT:  /* no break */
+                    case IEEE80211_FTYPE_DATA:
+                        break;
+
+                    case IEEE80211_FTYPE_CTL:   /* no break */
+                    default:
+                        ignore = 1;
+                        break;
+                }
+
+		if (!ignore &&
+                    (len >= sizeof(struct ieee80211_hdr_3addr)) &&
+                    dw_rx_frame_is_duplicate(priv,
+                                              (const struct ieee80211_hdr_3addr*) frame->skb->data))
+                        ignore = 1;
+
+                if (!ignore) {
+                        /* the layer ignores the above frames, so we don't
+                         * need to alloc/free skb's for them. CTL are
+                         * ignored anyway and not freed by layer (bug)?
+                         * -> out of memory
+                         *
+                         * We have read the whole frame because a CTL frame is
+                         * typically 10 Bytes large, the header 4 Bytes.
+                         * Not much gain for reading only header, but a loose
+                         * for all other frames and still some overhead of
+                         * calculation  */
+                        skb_put(frame->skb, skblen);
+
+                        if (IS_DATA(fc))
+                                priv->rate.have_activity = 1;
+
+                        list_move_tail(element, &priv->rx.queue.filled.list);
+
+                        if (unlikely(list_empty(&priv->rx.queue.free.list)))
+                                /* no room to store any longer. So, FIFO may
+                                 * overrun, but in this case, no 802.11
+                                 * acknowledgements are transmitted and the
+                                 * frame is repeatedly sent => less errors */
+
+                                dw_rx_pause(priv, 1);
+                } else {
+                        if (priv->tx.last_was_data && control &&
+                            ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ACK))
+                        {
+                                priv->tx.data_pending_ack = NULL;
+                                priv->rate.tx_data_ack++;
+                        }
+
+                } /* fc */
+        } else {
+                spin_lock(&priv->ieee->lock);
+                priv->ieee->stats.rx_dropped++;
+                spin_unlock(&priv->ieee->lock);
+        }
+}
+
+/**
+ * dw_rx_tasklet_handler - Read's all frames in fifo
+ */
+static void dw_rx_tasklet_handler(unsigned long data)
+{
+        dw_priv_t*        priv = (dw_priv_t*) data;
+        struct list_head  frames;
+        struct list_head* cursor;
+        unsigned long     flags;
+
+        DBG_FN(DBG_RX | DBG_INT);
+
+        /* move the filled list to our context, so the tasklet is able to
+         * continue mostly unlocked. */
+        INIT_LIST_HEAD(&frames);
+
+        dw_lock(priv, flags);
+        dw_list_move(&frames, &priv->rx.queue.filled.list);
+        dw_unlock(priv, flags);
+
+        list_for_each(cursor, &frames) {
+                dw_frame_rx_t* frame = list_entry(cursor, dw_frame_rx_t, list);
+                dw_rx_frame_give_to_stack(priv, frame);
+        }
+
+        /* now move the processed frame list that is now empty again back to
+           free  */
+        dw_lock(priv, flags);
+        dw_list_move(&priv->rx.queue.free.list, &frames);
+        dw_rx_pause(priv, 0);
+        dw_unlock(priv, flags);
+}
+
+/**
+ * dw_tx_reset - reset's the Tx FIFO when a timeout is detected
+ */
+static void dw_tx_reset(dw_priv_t* priv)
+{
+        DBG_FN(DBG_TX | DBG_ERROR);
+        REQUIRE_LOCKED(priv);
+
+        ERRORL("TX Reset and retrying transmission");
+
+        /* reset FIFO */
+
+        dw_iosetbits32(HW_GEN_CONTROL, GEN_TXFIFORST);
+        wmb();
+        dw_iocleanbits32(HW_GEN_CONTROL, GEN_TXFIFORST);
+
+        priv->tx.data_pending_ack = NULL;
+
+        /* retransmit it */
+
+        if (likely(!list_empty(&priv->tx.queued))) {
+                dw_frame_tx_t* frame = list_entry(priv->tx.queued.prev, dw_frame_tx_t, list);
+                dw_tx_fragment_send(priv, frame);
+        } else {
+                ERROR("Tx Queue is empty, but we have a tx timeout????");
+        }
+}
+
+/**
+ * dw_tx_wait_for_idle_and_pause - on return, no frame is processed any more by
+ * Tx
+ *
+ * After the return it is safe to configure the transmitter
+ */
+static void dw_tx_wait_for_idle_and_pause(dw_priv_t* priv)
+{
+        unsigned long flags;
+        int sleep;
+
+        DBG_FN(DBG_TX);
+        REQUIRE_UNLOCKED(priv);
+
+        /* no more new transmits until dw_tx_continue_queue */
+        dw_lock(priv, flags);
+        priv->tx.pause = 1;
+        sleep          = priv->tx.pending;
+        netif_stop_queue(priv->dev);
+        dw_unlock(priv, flags);
+
+        if (sleep)
+                /* wait for queue to be emptied.
+                 * we mustn't be locked, because we can sleep. But that's not a
+                 * problem. If the interrupt handler is faster, it will
+                 * already have freed semaphore, so we run through */
+                down(&priv->tx.pause_sem);
+}
+
+static void dw_tx_continue_queue(dw_priv_t* priv)
+{
+        unsigned long flags;
+
+        DBG_FN(DBG_TX);
+        REQUIRE_UNLOCKED(priv);
+
+        dw_lock(priv, flags);
+
+        priv->tx.pause = 0;
+        netif_wake_queue(priv->dev);
+        if (!list_empty(&priv->tx.queued)) {
+                /* transmitter is paused, so nothing is pending. Send next
+                   queued entry */
+                dw_frame_tx_t* frame = list_entry(priv->tx.queued.prev, dw_frame_tx_t, list);
+                dw_tx_fragment_send(priv, frame);
+        }
+
+        dw_unlock(priv, flags);
+}
+
+
+/**
+ * dw_tx_set_plcp - set's PLCP Header of the frame.
+ */
+static void dw_tx_set_plcp(dw_frame_tx_info_t* fi, int fragment, int rate)
+{
+        dw_fragment_tx_t* frag = &fi->frags[ fragment ];
+        dw_hw_hdr_tx_t* hdr    = &frag->hdr;
+        size_t          length = frag->phys_len;
+
+        DBG_FN(DBG_TX);
+
+        CLEAR(*hdr);
+        /* FIFO length in words of complete frame with header,
+           rounded up. FCS is added automatically */
+        hdr->mod.length = (sizeof(*hdr) + length + 3) / 4;
+
+        /* FCS length is required for signal */
+        length += IEEE80211_FCS_LEN;
+        if (!ieee80211_is_cck_rate(rate)) {
+                hdr->mod.mod_type     = MOD_OFDM;
+                hdr->plcp.ofdm.rate   = dw_plcp_get_ratecode_ofdm(rate);
+                hdr->plcp.ofdm.length = length;
+                hdr->plcp.ofdm.raw32  = cpu_to_le32(hdr->plcp.ofdm.raw32);
+        } else {
+                int signal = dw_plcp_get_ratecode_cck(rate);
+                hdr->mod.mod_type        = MOD_PSKCCK;
+                hdr->plcp.pskcck.signal  = signal;
+                hdr->plcp.pskcck.service = SERVICE_LOCKED;
+
+                /* convert length from bytes to microseconds */
+                switch(signal) {
+                    case 10:  length *= 8; break;
+                    case 20:  length *= 4; break;
+                    case 55:  length = (16 * length + 10) / 11; break;
+                    case 110:
+                        length = (8 * length + 10) / 11;
+                        /* set length extension bit if needed */
+                        if ((11 * length) / 8 > ( frag->phys_len + IEEE80211_FCS_LEN))
+                                hdr->plcp.pskcck.service |= SERVICE_LENEXT;
+                        break;
+                    default:
+                        ERRORL("Unsupported signal/rate %i/%i", signal,rate);
+                        break;
+                }
+                hdr->plcp.pskcck.length = cpu_to_le16(length);
+                hdr->plcp.pskcck.raw32  = cpu_to_le32(hdr->plcp.pskcck.raw32);
+        }
+        hdr->mod.raw32 = cpu_to_le32(hdr->mod.raw32);
+}
+
+/**
+ * dw_tx_ack_duration -
+ *
+ * @return the duration for the acknowledgement of our data package in us
+ */
+static int dw_tx_ack_duration(
+        dw_priv_t* priv,
+        const dw_frame_tx_info_t* fi,
+        dw_rate_index_t index)
+{
+        const rate_info_t* rate_info = &rates_info[ priv->tx.basics[ index ] ];
+        /* ack/crts is sent at equal or lower basic rate */
+        int dur = rate_info->ack_len;
+
+        REQUIRE_LOCKED(priv);
+
+        /* add psk/cck preamble time */
+        if (!rate_info->ofdm_code)
+                dur += PRE_LEN(fi, rate_info->bps);
+
+        return dur;
+}
+
+/**
+ * dw_tx_duration -
+ *
+ * PLCP must be set.
+ * @return the duration for the frame in us
+ */
+static int dw_tx_duration(
+        dw_priv_t* priv,
+        const dw_frame_tx_info_t* fi,
+        int fragment,
+        int rate)
+{
+        int index = dw_rate_info_index(rate);
+        int dur;
+
+        /* frame duration */
+        if (rates_info[ index ].ofdm_code)
+                dur = OFDM_DUR(fi->frags[ fragment ].phys_len, rate);
+        else
+                dur = PRE_LEN(fi, rates_info[ index ].bps) + le16_to_cpu(fi->frags[ fragment ].hdr.plcp.pskcck.length);
+
+        return dur + dw_tx_ack_duration(priv, fi, index);
+}
+
+
+/**
+ * dw_tx_frame_prepare - prepare everything that is needed to send linux frame
+ *
+ * Determine PLCP, rate, sequence/fragment number
+ *
+ * @return 0 on failure. Frame needs to be dropped
+ */
+static int dw_tx_frame_prepare(
+        dw_priv_t* priv,
+        dw_frame_tx_info_t* fi,
+        struct ieee80211_txb* txb)
+{
+        struct ieee80211_hdr_3addr* hdr3        = NULL;
+        const struct ieee80211_hdr_1addr* hdr1  = NULL;
+        struct ieee80211_hdr* hdr  = (struct ieee80211_hdr *)txb->fragments[ 0 ]->data;
+        int fc = le16_to_cpu(hdr->frame_ctl);
+        int i;
+        int rate;
+        int rate_last_fragment;
+        int rate_index;
+        int duration;
+
+        REQUIRE_LOCKED(priv);
+
+        if (txb->fragments[ 0 ]->len >= sizeof(struct ieee80211_hdr_3addr))
+                /* we need it for sequence number */
+                 hdr3 = (struct ieee80211_hdr_3addr*) hdr;
+        if (txb->fragments[ 0 ]->len >= sizeof(struct ieee80211_hdr_1addr))
+                /* we need it for group addressing */
+                 hdr1 = (struct ieee80211_hdr_1addr*) hdr;
+
+        CLEAR(*fi);
+        fi->txb     = txb;
+        fi->is_data = IS_DATA(fc);
+
+        /* the stack sets encrypted whenenver the frame needs to be encrypted,
+         * but set's FCTL_PROTECTED only when it has encrypted it itself. This
+         * leaves CCMP for us.
+         * encrypted can also be set to 1 if we don't provide hardware. */
+        fi->use_hw_encryption = fi->txb->encrypted && !(fc & IEEE80211_FCTL_PROTECTED);
+        if (fi->use_hw_encryption) {
+                fc |= IEEE80211_FCTL_PROTECTED;
+                hdr->frame_ctl = cpu_to_le16(fc);
+        }
+
+        /* determine bit rate for fragment */
+        spin_lock(&priv->softmac->lock);
+        rate = rate_last_fragment = priv->softmac->txrates.default_rate;
+
+        if (IS_MGMT(fc))
+                rate_last_fragment = dw_rates[ 0 ];
+        else if ((NULL != hdr3) && is_multicast_ether_addr(hdr3->addr1))
+                rate = rate_last_fragment = priv->softmac->txrates.mcast_rate;
+        else if (fc & IEEE80211_FCTL_PROTECTED)
+                /* send all but the last at broadcast_rate */
+                rate_last_fragment = priv->softmac->txrates.mcast_rate;
+        spin_unlock(&priv->softmac->lock);
+
+        /* generate sequence number, encryption and plcp */
+        for (i = 0; i < fi->txb->nr_frags; i++) {
+                dw_fragment_tx_t* frag = &fi->frags[ i ];
+                struct sk_buff*   skb  = txb->fragments[ i ];
+                int last_frag = (i == fi->txb->nr_frags - 1);
+                int rate_frag = BASIC_RATE_MASK(last_frag ? rate_last_fragment : rate);
+
+                if (txb->fragments[ i ]->len >= sizeof(struct ieee80211_hdr_3addr)) {
+                        int fragment = (fi->txb->rts_included ? (i - 1) : i);
+                        /* it holds a sequence/fragment number */
+                        struct ieee80211_hdr_3addr* frag_hdr3 = (struct ieee80211_hdr_3addr*) txb->fragments[ i ]->data;
+                        /* see ieee80211.h: WLAN_GET_SEQ_FRAG(seq) */
+                        frag_hdr3->seq_ctl = cpu_to_le16((atomic_read(&priv->tx.seq_nr) << 4) |
+                                                         (fragment & 0xf));
+                }
+
+                fi->use_short_preamble =
+                        (rate_frag != IEEE80211_CCK_RATE_1MB) && priv->softmac->bssinfo.short_preamble;
+
+                frag->phys_len = skb->len;
+
+                /* provide encryption. Requires sequence number for MIC */
+                if (fi->use_hw_encryption) {
+                        frag->crypt.key_index = priv->ieee->tx_keyidx;
+                        frag->crypt.key = &priv->aeskeys[ frag->crypt.key_index ];
+
+                        /* Key not set */
+                        if (!frag->crypt.key->valid)
+                                goto error;
+
+                        /* EXTIV and MIC are automatically appended */
+                        frag->phys_len += CCMP_SIZE;
+                }
+
+                /* requires fragment length with encryption */
+                dw_tx_set_plcp(fi, i, rate_frag);
+        } /* plcp */
+
+        /* add duration, for initial fragments */
+        rate_index = dw_rate_info_index(BASIC_RATE_MASK(rate));
+        for (i = 0; i < fi->txb->nr_frags - 1; i++) {
+                struct ieee80211_hdr* frag_hdr  = (struct ieee80211_hdr *)txb->fragments[ i ]->data;
+
+                /* ack of this frame + data of next frame */
+                duration = dw_tx_ack_duration(priv, fi, rate_index) +
+                        dw_tx_duration(priv, fi, i + 1, rate);
+
+                frag_hdr->duration_id = cpu_to_le16(duration);
+        }
+
+        /* set duration for final or last fragment */
+        if ((NULL != hdr1) && IS_GROUP_ADDR(hdr1->addr1))
+                duration = 0;
+        else
+                duration = dw_tx_ack_duration(priv, fi,
+                                               dw_rate_info_index(BASIC_RATE_MASK(rate_last_fragment)));
+
+        hdr->duration_id = cpu_to_le16(duration);
+
+        if (NULL != hdr3)
+                /* no need for cutting it at 0xffff, will be done on
+                   assignment to seq_ctl */
+                atomic_inc(&priv->tx.seq_nr);
+
+        return 1;
+
+error:
+        return 0;
+}
+
+/**
+ * dw_tx_fragment_send - copies a fragment to FIFO and sets all registers.
+ */
+static void dw_tx_fragment_send(
+        dw_priv_t* priv,
+        dw_frame_tx_t* frame)
+{
+        const dw_fragment_tx_t* frag  = &frame->s.frags[ priv->tx.fragment ];
+        const struct sk_buff* skb     = frame->s.txb->fragments[ priv->tx.fragment ];
+        struct ieee80211_hdr* hdr  = (struct ieee80211_hdr *)skb->data;
+        struct ieee80211_hdr_3addr* hdr3 = (struct ieee80211_hdr_3addr*) hdr;
+        int hw_gen_ctrl = dw_ioread32(HW_GEN_CONTROL);
+        u16 fc          = le16_to_cpu(hdr->frame_ctl);
+
+        DBG_FN(DBG_TX);
+        REQUIRE_LOCKED(priv);
+
+        //if (priv->tx.retries) {
+        if (priv->tx.times_sent) {
+                /* Add RETRY flag, sequence number is already set */
+                fc |= IEEE80211_FCTL_RETRY;
+                hdr->frame_ctl = cpu_to_le16(fc);
+                if (priv->cw < CW_MAX)
+                        /* code starts always with 2^n-1. Therefore, we still
+                         * always have 2^n-1 and never overrun CW_MAX. */
+                        priv->cw = priv->cw * 2 + 1;
+
+                priv->wstats.discard.retries++;
+        } else {
+                priv->cw = CW_MIN;
+        }
+
+        dw_cw_set(priv, fc);
+
+        if (frame->s.use_short_preamble)
+                hw_gen_ctrl |= GEN_SHPRE;
+        else
+                /* may be set from a previous frame */
+                hw_gen_ctrl &= ~GEN_SHPRE;
+
+        priv->tx.last_was_data = frame->s.is_data;
+        if (frame->s.is_data) {
+                priv->rate.have_activity = 1;
+                priv->rate.tx_data_any++;
+        }
+
+        /* sent frame, either bye AES or unencrypted engine */
+        if (!frame->s.use_hw_encryption) {
+        /* Prevent transmitting until all data have been written into the tx fifo */
+                dw_iowrite32(hw_gen_ctrl | GEN_TXHOLD, HW_GEN_CONTROL);
+                dw_hw_write_fifo(&frag->hdr, sizeof(frag->hdr));
+                dw_hw_write_fifo(skb->data,  skb->len);
+                dw_iowrite32(hw_gen_ctrl, HW_GEN_CONTROL);
+        } else {
+                ccmp_data_t cdata;
+                u8 extiv[ EXTIV_SIZE ];
+
+                dw_ccmp_get_data_tx(priv, hdr3, frag, &cdata, extiv, skb->len - sizeof(*hdr3));
+
+                /* frame ctl is part of data used for MIC. */
+                dw_hw_aes_wait();
+
+                /* prevent transmitting until encrypted data is ready */
+                dw_iosetbits32(HW_GEN_CONTROL, GEN_TXHOLD);
+
+                /* write MAC, Frame Header and IV FIFO */
+                dw_hw_write_fifo(&frag->hdr, sizeof(frag->hdr) );
+                dw_hw_write_fifo(hdr3, sizeof(*hdr3) );
+                dw_hw_write_fifo(&extiv, sizeof(extiv));
+
+                /* configure mode and key */
+                dw_iowrite32(HW_AES_MODE_1 | (frag->crypt.key_index & 0xf),
+                              HW_AES_MODE);
+
+                /* write init block to AES FIFO */
+                dw_hw_aes_write_fifo(&cdata, sizeof(cdata));
+
+                /* start transmit */
+                dw_iocleanbits32(HW_GEN_CONTROL, GEN_TXHOLD);
+
+                /* write plaintext data to AES FIFO */
+                dw_hw_aes_write_fifo(hdr3->payload, skb->len - sizeof(*hdr3));
+        }
+        priv->dev->trans_start = jiffies;
+        priv->tx.pending       = 1;
+        priv->tx.times_sent++;
+
+	if (priv->ieee->iw_mode != IW_MODE_ADHOC) {
+		if (((*(char *)(skb->data)) & 0x0C) == 0x08) {	/* If it is a data frame */
+			priv->tx.data_pending_ack = (void *)frag;
+			priv->tx.jiffies_pending_ack = jiffies;
+		}
+	}
+}
+
+/**
+ * dw_tx_tasklet_handler - One fragment has been completed.
+ *
+ * is only called when entries are in tx.queued list
+ */
+static void dw_tx_tasklet_handler(unsigned long data)
+{
+        dw_priv_t*     priv        = (dw_priv_t*) data;
+        dw_frame_tx_t* frame       = NULL;
+        unsigned int   retry_frame = 0;
+        unsigned long  flags;
+
+        DBG_FN(DBG_TX | DBG_INT);
+
+        dw_lock(priv, flags);
+
+        /* check if there are further fragments left */
+        if (list_empty(&priv->tx.queued) && !priv->tx.timeout)
+                /* Spurious Interrupt. May happen if we slow everything down
+                 * with debug messages */
+                goto out;
+
+        spin_lock(&priv->ieee->lock);  /* interrupts already disabled */
+
+        frame = list_entry(priv->tx.queued.next, dw_frame_tx_t, list);
+        if (priv->tx.timeout) {
+                int max_retries =
+                        ((frame->s.txb->fragments[ priv->tx.fragment ]->len >= priv->ieee->rts) ?
+                          priv->long_retry_limit :
+                          priv->short_retry_limit);
+
+                priv->tx.retries++;
+                priv->tx.timeout = 0;
+
+                if (priv->tx.retries <= max_retries) {
+                        retry_frame = 1;
+                } else {
+                        priv->tx.data_pending_ack = NULL;
+                        priv->ieee->ieee_stats.tx_retry_limit_exceeded++;
+                }
+        } /* if (priv->tx.timeout) */
+
+        if (!retry_frame && priv->tx.data_pending_ack == NULL) {
+                /* send next fragment */
+                priv->tx.fragment++;
+                if (priv->tx.fragment == frame->s.txb->nr_frags) {
+                        /* frame is complete. Free resources */
+                        ieee80211_txb_free(frame->s.txb);
+                        frame->s.txb = NULL;
+
+                        /* queue entry can be reused */
+                        list_move_tail(priv->tx.queued.next, &priv->tx.free);
+                        if (!list_empty(&priv->tx.queued))
+                                /* take next frame's first fragment */
+                                frame = list_entry(priv->tx.queued.next, dw_frame_tx_t, list);
+                        else
+                                frame = NULL;
+
+                        priv->tx.fragment = 0;
+                        priv->tx.retries = 0;
+                        priv->tx.times_sent = 0;
+
+                        if (!priv->tx.pause &&
+                            netif_queue_stopped(priv->dev))
+                                /* we have space again */
+                                netif_wake_queue(priv->dev);
+
+                }
+        }
+
+        if (priv->tx.data_pending_ack && time_after(jiffies, priv->tx.jiffies_pending_ack + ACK_TIMEOUT)) {
+                priv->tx.data_pending_ack = NULL;
+        }
+
+        spin_unlock(&priv->ieee->lock);
+
+        if (NULL != frame)
+        {
+                dw_tx_fragment_send(priv, frame);
+        } else {
+                /* no more frames */
+                priv->tx.pending = 0;
+        }
+
+        if (priv->tx.pause && list_empty(&priv->tx.queued))
+                /* nothing is being processed any longer.
+                   Awake listener */
+                up(&priv->tx.pause_sem);
+
+out:
+        dw_unlock(priv, flags);
+}
+
+/* Supported rates info elements */
+static const u8 ratesA[]  = { DW_ELEM_SUPRATES, 8, DW_RATE_BASIC+12, 18, DW_RATE_BASIC+24, 36, DW_RATE_BASIC+48, 72, 96, 108 };
+static const u8 ratesB[]  = { DW_ELEM_SUPRATES, 4, DW_RATE_BASIC+2, DW_RATE_BASIC+4, 11, 22 };
+static const u8 ratesG[]  = { DW_ELEM_SUPRATES, 8, DW_RATE_BASIC+2, DW_RATE_BASIC+4, 11, 22, 12, 18, 24, 36 };
+static const u8 ratesGx[] = { DW_ELEM_EXTSUPRATES, 4, 48, 72, 96, 108 };
+
+static void dw_beacon_set_plcp(dw_hw_hdr_tx_t * hdr, int rate, size_t phys_len)
+{
+        size_t length = phys_len;
+        memset(hdr, 0, sizeof(*hdr));
+
+	/**
+	 * Length in words, including Frame Header and PLCP Header, and excluding FCS, rounded up
+	 * FCS is added automatically
+	 */
+        hdr->mod.length = (phys_len - FCS_SIZE + 3) / 4;
+
+        /* FCS length is required for signal */
+        length += IEEE80211_FCS_LEN;
+        if (!ieee80211_is_cck_rate(rate)) {
+                hdr->mod.mod_type     = MOD_OFDM;
+                hdr->plcp.ofdm.rate   = dw_plcp_get_ratecode_ofdm(rate);
+                hdr->plcp.ofdm.length = length;
+                hdr->plcp.ofdm.raw32  = cpu_to_le32(hdr->plcp.ofdm.raw32);
+        } else {
+                int signal = dw_plcp_get_ratecode_cck(rate);
+                hdr->mod.mod_type        = MOD_PSKCCK;
+                hdr->plcp.pskcck.signal  = signal;
+                hdr->plcp.pskcck.service = SERVICE_LOCKED;
+
+                /* convert length from bytes to microseconds */
+                switch(signal) {
+                    case 10:  length *= 8; break;
+                    case 20:  length *= 4; break;
+                    case 55:  length = (16 * length + 10) / 11; break;
+                    case 110:
+                        length = (8 * length + 10) / 11;
+                        /* set length extension bit if needed */
+                        if ((11 * length) / 8 > ( phys_len + IEEE80211_FCS_LEN))
+                                hdr->plcp.pskcck.service |= SERVICE_LENEXT;
+                        break;
+                    default:
+                        ERRORL("Unsupported signal/rate %i/%i", signal, rate);
+                        break;
+                }
+                hdr->plcp.pskcck.length = cpu_to_le16(length);
+                hdr->plcp.pskcck.raw32  = cpu_to_le32(hdr->plcp.pskcck.raw32);
+        }
+        hdr->mod.raw32 = cpu_to_le32(hdr->mod.raw32);
+}
+
+/**
+ * Store supported rates elements into a buffer
+ * @param bp Pointer into buffer
+ * @param elem Element to store: DW_ELEM_SUPRATES, DW_ELEM_EXTSUPRATES, or 0 for both
+ * @param channel Channel number
+ * @return Updated buffer pointer
+ */
+static u8 *dw_beacon_set_rates(u8 *bp, int elem, int channel)
+{
+	const u8 *sr;
+
+	if (DW_CHAN_5G (channel))
+		sr = ratesA;
+	//else if (OPT_BONLY)
+	//	sr = ratesB;
+	else
+		sr = ratesG;
+
+	/* Store up to 8 supported rates */
+	if (elem != DW_ELEM_EXTSUPRATES) {
+		memcpy (bp, sr, sr[1]+2);
+		bp += bp[1] + 2;
+	}
+
+	/* Store remaining extended supported rates */
+	if (elem != DW_ELEM_SUPRATES && sr == ratesG) {
+		memcpy (bp, ratesGx, ratesGx[1]+2);
+		bp += bp[1] + 2;
+	}
+
+	return bp;
+}
+
+/**
+ * Create beacon and probe response frames to send in an IBSS
+ * @param interval Beacon interval in TU
+ * @return 1 if success, 0 if error
+ */
+static void dw_beacon_make_beacon(dw_priv_t * priv, int interval)
+{
+    dw_beacon_frame * bcnFrame = &priv->beacon_frame;
+    u16 atimWindow = 0; /* ATIM window size, 0 if none */
+
+    u8 *bp = bcnFrame->body;
+    u16 bss_caps = 0, caps;
+
+    u8 bss_addr[ ETH_ALEN ];
+
+    priv->beacon_body_length = 0;
+
+    bss_caps = DW_CAP_IBSS;
+    //if (!(macParams.encrypt & WLN_ENCR_OPEN))
+    //    bss_caps |= CAP_PRIVACY;
+    //if (OPT_SHORTPRE)
+    //    bss_caps |= CAP_SHORTPRE;
+
+    memcpy(bss_addr, priv->adhoc.bssid, ETH_ALEN);
+
+    /* Init beacon MAC header */
+    memset (bcnFrame, 0, sizeof (dw_beacon_frame));
+    bcnFrame->fc = IEEE80211_STYPE_BEACON;
+    memset (bcnFrame->addr1, 0xff, ETH_ALEN);
+    DW_SET_ADDR (bcnFrame->addr2, priv->dev->dev_addr); /* station MAC address */
+    DW_SET_ADDR (bcnFrame->addr3, bss_addr); /* BSS to associate with */
+
+    /** Set fixed params
+     * Timestamp is set by hardware */
+    SET16 (&bp[8], interval);
+
+    /* Set capabilities */
+    caps = bss_caps & (DW_CAP_ESS|DW_CAP_IBSS|DW_CAP_PRIVACY);
+    /* Use short preamble if allowed in BSS and params and rate > 1 mbps. */
+    /* caps |= DW_CAP_SHORTPRE; */
+    SET16 (&bp[10], caps);
+    bp += 12;
+
+    /* Set SSID */
+    bp[0] = DW_ELEM_SSID;
+    bp[1] = priv->softmac->associnfo.req_essid.len;
+    memcpy (&bp[2], priv->softmac->associnfo.req_essid.data, priv->softmac->associnfo.req_essid.len);
+    bp += bp[1] + 2;
+
+    /* Set supported rates */
+    bp = dw_beacon_set_rates(bp, DW_ELEM_SUPRATES, priv->adhoc.channel);
+
+    /* Set channel number */
+    if (!DW_CHAN_5G (priv->adhoc.channel)) {
+        bp[0] = DW_ELEM_DSPARAM;
+        bp[1] = 1;
+        bp[2] = priv->adhoc.channel;
+        bp += bp[1] + 2;
+    }
+
+    /* Set IBSS ATIM window */
+    bp[0] = DW_ELEM_IBSSPARAM;
+    bp[1] = 2;
+    SET16 (&bp[2], atimWindow);
+    bp += bp[1] + 2;
+
+    /* Set ERP info. */
+    //if (!DW_CHAN_5G (priv->adhoc.channel) && !(OPT_BONLY)))
+    {
+        bp[0] = DW_ELEM_ERPINFO;
+        bp[1] = 1;
+        bp[2] = 0;
+        bp += bp[1] + 2;
+    }
+
+    /* Set extended supported rates */
+    bp = dw_beacon_set_rates(bp, DW_ELEM_EXTSUPRATES, priv->adhoc.channel);
+    priv->beacon_body_length = ((u8 *)bp - (u8 *)bcnFrame->body);
+    dw_beacon_set_plcp(&bcnFrame->hwHdr, IEEE80211_CCK_RATE_1MB , priv->beacon_body_length + sizeof(dw_beacon_frame) - BEACON_BODY_SIZE + FCS_SIZE);
+    bcnFrame->hwHdr.plcp.pskcck.raw32 = cpu_to_be32(0x0a046802);
+    priv->beacon_ready = 1;
+}
+
+static void dw_beacon_start_IBSS(struct dw_priv *priv)
+{
+	int atim = 0;
+
+	dw_beacon_make_beacon(priv, DW_BEACON_INT);
+	/* If starting IBSS, set beacon and ATIM intervals */
+	dw_iowrite32(atim | (DW_BEACON_INT << 16), HW_CFP_ATIM);
+	dw_cw_set(priv, IEEE80211_STYPE_BEACON);
+	/* Write beacon frame to beacon buffer */
+	dw_iosetbits32(HW_GEN_CONTROL, GEN_BEACEN);
+	dw_hw_write_fifo((void *)&priv->beacon_frame, priv->beacon_body_length + sizeof(dw_beacon_frame) - BEACON_BODY_SIZE);
+	dw_iocleanbits32(HW_GEN_CONTROL, GEN_BEACEN);
+	/* Set interrupt mask to enable TBTT and ATIM interrupts */
+	dw_iosetbits32(HW_INTR_MASK, INTR_TBTT|INTR_ATIM);
+	/* Enable IBSS mode */
+	dw_iosetbits32(HW_MAC_CONTROL, CTRL_IBSS|CTRL_BEACONTX);
+}
+
+/**
+ * dw_beacon_associate - does whatever is necessary for association
+ */
+static void dw_beacon_associate(struct ieee80211softmac_device *mac,
+	struct ieee80211_assoc_response *resp,
+	struct ieee80211softmac_network *net)
+{
+	u16 cap = 0;
+	u8 erp_value = net->erp_value;
+
+	if (resp != NULL)
+		cap = le16_to_cpu(resp->capability);
+	mac->associnfo.associating = 0;
+	mac->bssinfo.supported_rates = net->supported_rates;
+	ieee80211softmac_recalc_txrates(mac);
+
+	mac->associnfo.associated = 1;
+
+	if (resp != NULL)
+		mac->associnfo.short_preamble_available =
+		(cap & WLAN_CAPABILITY_SHORT_PREAMBLE) != 0;
+	ieee80211softmac_process_erp(mac, erp_value);
+
+	if (mac->set_bssid_filter)
+		mac->set_bssid_filter(mac->dev, net->bssid);
+	memcpy(mac->ieee->bssid, net->bssid, ETH_ALEN);
+	netif_carrier_on(mac->dev);
+
+	if (resp != NULL)
+		mac->association_id = le16_to_cpup(&resp->aid);
+}
+
+static void dw_beacon_start(struct work_struct *work)
+{
+	dw_priv_t *priv = container_of((struct delayed_work *)work, struct dw_priv, beacon_work);
+	unsigned long flags;
+	int channel;
+	struct ieee80211softmac_network *net;
+	struct ieee80211softmac_device *mac = priv->softmac;
+
+	if (IW_MODE_ADHOC != priv->ieee->iw_mode
+		|| mac->associnfo.associating
+		|| mac->scanning
+		|| priv->beacon_ready
+	) return;
+
+	if (!mac->associnfo.req_essid.len
+		|| *mac->associnfo.req_essid.data == '\0'
+	) return;
+
+	channel = priv->adhoc.channel;
+	if (!channel) channel = priv->channel;
+	if (!channel) channel = DW_IBSS_DEFAULT_CHANNEL;
+
+	memset(&priv->adhoc, 0, sizeof(priv->adhoc));
+	get_random_bytes(priv->adhoc.bssid, ETH_ALEN); // Select a random BSSID
+	priv->adhoc.bssid[0] &= ~DW_MAC_GROUP;  // clear group bit
+	priv->adhoc.bssid[0] |=  DW_MAC_LOCAL;  // set local bit
+	priv->adhoc.channel = channel;
+	priv->adhoc.mode=IW_MODE_ADHOC;
+	priv->adhoc.essid.len = mac->associnfo.req_essid.len;
+	memcpy(priv->adhoc.essid.data, mac->associnfo.req_essid.data, IW_ESSID_MAX_SIZE + 1);
+
+	net = ieee80211softmac_get_network_by_essid_locked(mac, &mac->associnfo.associate_essid);
+	if (!net) {
+		net = &priv->adhoc;
+
+		mac->set_channel(mac->dev, net->channel);
+		if (mac->set_bssid_filter)
+			mac->set_bssid_filter(mac->dev, net->bssid);
+
+		spin_lock_irqsave(&mac->lock, flags);
+		dw_beacon_associate(mac, NULL, net);
+		ieee80211softmac_call_events_locked(mac, IEEE80211SOFTMAC_EVENT_ASSOCIATED, net);
+		spin_unlock_irqrestore(&mac->lock, flags);
+
+		mac->associnfo.scan_retry = IEEE80211SOFTMAC_ASSOC_SCAN_RETRY_LIMIT;
+		mac->associnfo.bssvalid = 1;
+		mac->associnfo.channel = net->channel;
+		memcpy(mac->associnfo.bssid, net->bssid, ETH_ALEN);
+		mac->associnfo.associate_essid.len = net->essid.len;
+		memcpy(mac->associnfo.associate_essid.data, net->essid.data, IW_ESSID_MAX_SIZE + 1);
+	} else {
+		memcpy(priv->adhoc.bssid, net->bssid, ETH_ALEN);
+		priv->adhoc.essid.len = net->essid.len;
+		memcpy(priv->adhoc.essid.data, net->essid.data, IW_ESSID_MAX_SIZE + 1);
+	}
+
+	dw_beacon_start_IBSS(priv);
+}
+
+/**
+ * dw_int - interrupt handler for WiFi FPGA
+ */
+static irqreturn_t dw_int(int irq, void *dev_id)
+{
+	dw_priv_t* priv = dev_id;
+	u32 status;
+
+	DBG_FN(DBG_INT);
+
+	/* acknowledge interrupt */
+	spin_lock(&priv->lock);
+	status = dw_ioread32(HW_INTR_STATUS);
+	dw_iowrite32(status, HW_INTR_STATUS);
+
+	if (status & INTR_RXFIFO) {
+		int frame_received = 0;
+		/* process all frames */
+		while (!priv->rx.pause &&
+			(!(dw_hw_get_status(priv) & STAT_RXFE))) {
+			frame_received = 1;
+			dw_rx_frame_fetch(priv);
+		}
+
+		if (frame_received)
+			/* the FIFO is prone to run full, do it with high
+			priority  */
+			tasklet_hi_schedule(&priv->rx.tasklet);
+
+		DBG_EXEC(status &= ~INTR_RXFIFO);
+	} /* if (status & INTR_RXFIFO) */
+
+	if (status & INTR_TIMEOUT) {
+		priv->tx.timeout = 1;
+		DBG_EXEC(status &= ~INTR_TIMEOUT);
+	}
+
+	if (unlikely(status & INTR_ABORT)) {
+		priv->tx.timeout = 1;
+		/* retransmit it */
+		DBG_EXEC(status &= ~INTR_ABORT);
+	}
+
+        if (status & INTR_TXEND) {
+                tasklet_schedule(&priv->tx.tasklet);
+                DBG_EXEC(status &= ~INTR_TXEND);
+        }
+
+	if (status & (INTR_TBTT | INTR_ATIM)) {
+		/* is emitted by hardware even if masked out :-(. */
+
+		/** Beacon frame is already in beacon buffer.
+		 * Only set backoff timer here. */
+		dw_cw_set(priv, IEEE80211_STYPE_BEACON);
+		DBG_EXEC(status &= ~(INTR_TBTT | INTR_ATIM));
+	}
+
+	if (unlikely(status & INTR_RXOVERRUN)) {
+		DBG(DBG_RX, "Receiver overrun");
+		spin_lock(&priv->ieee->lock);
+		priv->ieee->stats.rx_over_errors++;
+		spin_unlock(&priv->ieee->lock);
+		DBG_EXEC(status &= ~INTR_RXOVERRUN);
+	}
+
+#ifdef CONFIG_DIGI_WI_G_DEBUG
+	/* when not developing/debugging, we don't run extra tests. */
+	if (unlikely(status))
+		/* check that we had everything handled*/
+		ERRORL("Unhandled interrupt 0x%08x, mask is 0x%08x", status,
+				dw_ioread32(HW_INTR_MASK));
+#endif  /* CONFIG_DIGI_WI_G_DEBUG */
+
+	spin_unlock(&priv->lock);
+
+	return IRQ_HANDLED;
+}
+
+static int dw_ieee80211_handle_beacon (
+	struct net_device * dev,
+	struct ieee80211_beacon * beacon,
+	struct ieee80211_network * network)
+{	
+	dw_priv_t* priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+
+	if ( !priv->softmac->associnfo.associated && 
+	     !priv->softmac->associnfo.associating &&
+	     IW_MODE_INFRA == priv->ieee->iw_mode &&
+	     priv->jiffies_last_beacon + RESCAN_TIMEOUT < jiffies &&
+	     (strncmp(priv->softmac->associnfo.req_essid.data, 
+	     beacon->info_element->data, beacon->info_element->len) == 0) &&
+	     priv->reconnection_attempts &&
+	     priv->reconnection_attempts < MAX_RECONNECTION_ATTEMPTS ) {
+		dw_lock(priv, flags);
+		dw_set_channel(priv, network->channel);
+		ieee80211softmac_try_reassoc(priv->softmac);
+		priv->jiffies_last_beacon = jiffies;
+		priv->reconnection_attempts++;
+		dw_unlock(priv, flags);
+	}
+
+	if ( priv->softmac->associnfo.associated &&
+	     IW_MODE_INFRA == priv->ieee->iw_mode &&
+	     memcmp(network->bssid, beacon->header.addr3, ETH_ALEN) == 0 ) {
+		priv->jiffies_last_beacon = jiffies;
+		priv->reconnection_attempts = 0;
+	}
+	return ieee80211softmac_handle_beacon(dev, beacon, network);
+}
+
+/* Allocate a management frame */
+static u8 * ieee80211softmac_alloc_mgt(u32 size)
+{
+	u8 * data;
+
+	/* Add the header and FCS to the size */
+	size = size + IEEE80211_3ADDR_LEN;
+	if (size > IEEE80211_DATA_LEN)
+		return NULL;
+	/* Allocate the frame */
+	data = kzalloc(size, GFP_ATOMIC);
+	return data;
+}
+
+static void
+ieee80211softmac_hdr_2addr(struct ieee80211softmac_device *mac,
+	struct ieee80211_hdr_2addr *header, u32 type, u8 *dest)
+{
+	/* Fill in the frame control flags */
+	header->frame_ctl = cpu_to_le16(type);
+	/* Control packets always have WEP turned off */
+	if (type > IEEE80211_STYPE_CFENDACK && type < IEEE80211_STYPE_PSPOLL)
+		header->frame_ctl |= mac->ieee->sec.level ? cpu_to_le16(IEEE80211_FCTL_PROTECTED) : 0;
+
+	/* Fill in the duration */
+	header->duration_id = 0;
+	/* FIXME: How do I find this?
+	 * calculate. But most drivers just fill in 0 (except if it's a station id of course) */
+
+	/* Fill in the Destination Address */
+	if (dest == NULL)
+		memset(header->addr1, 0xFF, ETH_ALEN);
+	else
+		memcpy(header->addr1, dest, ETH_ALEN);
+	/* Fill in the Source Address */
+	memcpy(header->addr2, mac->ieee->dev->dev_addr, ETH_ALEN);
+}
+
+static void
+ieee80211softmac_hdr_3addr(struct ieee80211softmac_device *mac,
+	struct ieee80211_hdr_3addr *header, u32 type, u8 *dest, u8 *bssid)
+{
+	/* This is common with 2addr, so use that instead */
+	ieee80211softmac_hdr_2addr(mac, (struct ieee80211_hdr_2addr *)header, type, dest);
+
+	/* Fill in the BSS ID */
+	if (bssid == NULL)
+		memset(header->addr3, 0xFF, ETH_ALEN);
+	else
+		memcpy(header->addr3, bssid, ETH_ALEN);
+
+	/* Fill in the sequence # */
+	/* FIXME: I need to add this to the softmac struct
+	 * shouldn't the sequence number be in ieee80211? */
+}
+
+static int dw_ieee80211_handle_probe_request (
+	struct net_device * dev,
+	struct ieee80211_probe_request * req,
+	struct ieee80211_rx_stats * stats)
+{
+	dw_priv_t* priv = ieee80211softmac_priv(dev);
+
+	if (priv->softmac->associnfo.associate_essid.len == req->info_element->len &&
+		memcmp(priv->softmac->associnfo.associate_essid.data, req->info_element->data, priv->softmac->associnfo.associate_essid.len) == 0) {
+		struct ieee80211_probe_response *pkt = NULL;
+		struct ieee80211softmac_device *mac = priv->softmac;
+		struct ieee80211softmac_network *net = &priv->adhoc;
+		u8 *data;
+		u32 pkt_size = 0;
+		int encrypt_mpdu = 0;
+
+		pkt = (struct ieee80211_probe_response *)ieee80211softmac_alloc_mgt(priv->beacon_body_length);
+
+		if (unlikely(pkt == NULL)) {
+			printk("Error, packet is nonexistant or 0 length\n");
+			return -ENOMEM;
+		}
+
+		ieee80211softmac_hdr_3addr(mac, &(pkt->header), IEEE80211_STYPE_PROBE_RESP, net->bssid, priv->adhoc.bssid);
+		data = (u8 *)pkt->info_element;
+		memcpy(pkt->info_element, priv->beacon_frame.body, priv->beacon_body_length);
+		data += priv->beacon_body_length;
+		pkt_size = (data - (u8 *)pkt);
+		ieee80211_tx_frame(priv->ieee, (struct ieee80211_hdr *)pkt, IEEE80211_3ADDR_LEN, pkt_size, encrypt_mpdu);
+		kfree(pkt);
+	}
+
+	return 0;
+}
+
+/**
+ * dw_ieee80211_hard_start_xmit - prepares and sends a frame or put it in tx
+ * queue for transmission when current frames are completed.
+ */
+static int dw_ieee80211_hard_start_xmit(
+	struct ieee80211_txb* txb,
+	struct net_device*    dev,
+	int priority)
+{
+	dw_priv_t*      priv  = ieee80211softmac_priv(dev);
+	dw_frame_tx_t*  frame = NULL;
+	int             err   = -ENOMEM;
+	unsigned long   flags;
+	unsigned long   ieeeflags;
+	int             i;
+	dw_frame_tx_info_t info;
+
+	DBG_FN(DBG_TX);
+
+	/* sanity checks of txb */
+	BUG_ON(!txb->nr_frags);  /* nothing to do */
+	if (txb->nr_frags >= TX_MAX_FRAGS) {
+		/* we are lazy and use a fixed-size array */
+		ERROR("Too many fragments, dropping it: %i", txb->nr_frags);
+		goto error;
+	}
+
+	/* Check if the device queue is big enough for every fragment. If not,
+	 * drop the whole packet. */
+	for (i = 0; i < txb->nr_frags; i++) {
+		if (unlikely(txb->fragments[ i ]->len >
+				(HW_TX_FIFO_SIZE + sizeof(dw_hw_hdr_tx_t)))) {
+			ERROR("PIO Tx Device queue too small, dropping it");
+
+			goto error;
+		}
+	}
+
+	if (netif_queue_stopped(dev)) {
+		/* apperently, softmac doesn't honor netif_queue_stopped,
+		   at least for control messages. Block it ourself. */
+		err = -EBUSY;
+		goto error;
+	}
+
+	/* take the next free queue entry and provide it's data */
+	dw_lock(priv, flags);
+
+	/* prepare everything. */
+	if (!dw_tx_frame_prepare(priv, &info, txb))
+		goto error_unlock;
+
+	frame    = list_entry(priv->tx.free.next, dw_frame_tx_t, list);
+	frame->s = info;
+
+	/* put it to tx queue*/
+	if (list_empty(&priv->tx.queued) && !priv->tx.pending) {
+		/* no transmission is running yet, so the queue won't be
+		   processed by int handler. Write it directly to FIFO
+		   start with first fragment. */
+		priv->tx.retries = 0;
+		priv->tx.times_sent = 0;
+		dw_tx_fragment_send(priv, frame);
+	}
+	/* int handler is locked, so we can manage queue list after sending */
+	list_move_tail(priv->tx.free.next, &priv->tx.queued);
+
+	if (list_empty(&priv->tx.free)) {
+		/* Driver starts always with free entries.
+		   But now they are all in use. Wait until we have some free
+		   entries */
+		netif_stop_queue(dev);
+	}
+
+	dw_unlock(priv, flags);
+
+	return 0;
+
+error_unlock:
+	dw_unlock(priv, flags);
+
+error:
+	/* txb is free'd on !0 return */
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+	priv->ieee->stats.tx_dropped++;
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+
+	return err;
+}
+
+/**
+ * dw_update_status_led - updates the LED depending on association info
+ */
+static void dw_update_status_led(dw_priv_t* priv)
+{
+	static int reduce_rate = 0;
+	reduce_rate++;          /* slow it down to 500ms, don't divide */
+
+	if (reduce_rate == 9) {
+		static int count = 0;
+		reduce_rate = 0;
+
+		count++;
+		spin_lock(&priv->ieee->lock);
+		spin_lock(&priv->softmac->lock);
+		if (priv->softmac->associnfo.associated) {
+			if (IW_MODE_ADHOC == priv->ieee->iw_mode)
+				/* slow blink, 7/8 on */
+				dw_set_led_on(PIN_LED, count & 7);
+			else
+				dw_set_led_on(PIN_LED, 1);  /* solid on */
+		} else
+			dw_set_led_on(PIN_LED, count & 1);  /* faster blink */
+		spin_unlock(&priv->softmac->lock);
+		spin_unlock(&priv->ieee->lock);
+	}
+}
+
+/**
+ * dw_rate_reset - resets rate statistics
+ */
+static void dw_rate_reset(dw_priv_t* priv)
+{
+	int i;
+
+	DBG_FN(DBG_INTERFACE | DBG_UPDATE_RATE);
+	REQUIRE_LOCKED(priv);
+
+	CLEAR(priv->rate);
+
+	priv->rate.success_threshold = THRESH_MIN;
+
+	/* find index in ap_ri that matches default_rate */
+	for (i = 0; i < priv->ap_ri.count; i++) {
+		if (priv->softmac->txrates.default_rate == BASIC_RATE_MASK(priv->ap_ri.rates[ i ])) {
+			priv->rate.index = i;
+			break;
+		}
+	}
+}
+
+/**
+ * dw_rate_update - selects a better rate depending on frame error count
+ *
+ * taken from wifi_mac:mac_rate.c:UpdateRate
+ */
+static void dw_rate_update(dw_priv_t* priv)
+{
+	int changed = 0;
+
+	DBG_FN(DBG_UPDATE_RATE);
+	REQUIRE_LOCKED(priv);
+
+	if (atomic_read(&priv->fix_rate) || !rate_is_enough(priv))
+		/* user requested fix rate or not enough data*/
+		return;
+
+	spin_lock(&priv->softmac->lock);
+	if (!priv->softmac->associnfo.associated)
+		goto out;
+
+	if (rate_is_success(priv)) {
+		/* try to increase rate */
+		priv->rate.success++;
+		if ((priv->rate.success >= priv->rate.success_threshold) &&
+		    (priv->rate.index < (priv->ap_ri.count - 1))) {
+			/* we are successfull long enough */
+			priv->rate.recovery = 1;
+			priv->rate.success  = 0;
+			priv->rate.index++;
+			changed = 1;
+		} else
+			priv->rate.recovery = 0;
+	} else if (rate_is_failure(priv)) {
+		/* decrease rate */
+		if (priv->rate.index > 0) {
+			if (priv->rate.recovery) {
+				/* errors resulted from a rate increase.
+				   double successfull intervals needed to
+				   incrase next time */
+				priv->rate.success_threshold *= 2;
+				if (priv->rate.success_threshold > THRESH_MAX)
+					priv->rate.success_threshold = THRESH_MAX;
+			} else
+				priv->rate.success_threshold = THRESH_MIN;
+
+			priv->rate.index--;
+			changed = 1;
+		}
+
+		priv->rate.success  = 0;
+		priv->rate.recovery = 0;
+	}
+
+	if (changed) {
+		priv->softmac->txrates.default_rate = BASIC_RATE_MASK(priv->ap_ri.rates[ priv->rate.index ]);
+		DBG(DBG_UPDATE_RATE, "Rate changed to %i",
+		     priv->softmac->txrates.default_rate);
+	}
+
+	/* reset counter so we work on the next time slice */
+	priv->rate.tx_data_any = priv->rate.tx_data_ack = 0;
+
+out:
+	spin_unlock(&priv->softmac->lock);
+}
+
+/**
+ * dw_management_timer - performs all periodic background non tx/rx work
+ */
+static void dw_management_timer(unsigned long a)
+{
+	dw_priv_t*    priv = (dw_priv_t*) a;
+	unsigned long flags;
+
+	DBG_FN(DBG_UPDATE_RATE);
+
+	dw_lock(priv, flags);
+
+	switch (priv->ieee->iw_mode) {
+		case IW_MODE_ADHOC:
+			if (!priv->softmac->scanning && !priv->beacon_ready) {
+				schedule_delayed_work(&priv->beacon_work, 0);
+			}
+			break;
+		case IW_MODE_INFRA:
+			if (priv->softmac->associnfo.associated && priv->jiffies_last_beacon + BEACON_TIMEOUT < jiffies)
+			{
+				priv->softmac->associnfo.associated = 0;
+				ieee80211softmac_start_scan(priv->softmac);
+				priv->jiffies_last_beacon = jiffies;
+				ieee80211softmac_try_reassoc(priv->softmac);
+				priv->reconnection_attempts = 1;
+			} else if (
+				!priv->softmac->associnfo.associated
+				&& !priv->softmac->associnfo.associating
+				&& priv->reconnection_attempts
+				&& priv->softmac->associnfo.req_essid.len
+				&& priv->softmac->associnfo.req_essid.data
+				&& *priv->softmac->associnfo.req_essid.data
+				&& priv->jiffies_last_beacon + RESCAN_TIMEOUT < jiffies)
+			{
+				if (priv->reconnection_attempts < MAX_RECONNECTION_ATTEMPTS) {
+					priv->softmac->associnfo.associated = 0;
+					ieee80211softmac_start_scan(priv->softmac);
+					priv->jiffies_last_beacon = jiffies;
+					ieee80211softmac_try_reassoc(priv->softmac);
+					priv->reconnection_attempts++;
+				} else {
+					ieee80211softmac_disassoc(priv->softmac);
+					memset(priv->softmac->associnfo.bssid, 0, ETH_ALEN);
+					priv->reconnection_attempts = 0;
+				}
+			}
+			break;
+		default:
+			break;
+	}
+
+	dw_update_status_led(priv);
+
+	if (priv->rate.have_activity) {
+		priv->rate.have_activity = 0;
+		/* blink when data arrives */
+		dw_set_led_on(PIN_ACTIVITY_LED,
+			       priv->activity.counter & 1);
+
+		priv->activity.counter++;
+	} else
+		/* turn it off */
+		dw_set_led_on(PIN_ACTIVITY_LED, 0);
+
+	priv->rate.counter++;
+	if (!(priv->rate.counter % MANAGEMENT_TICKS_FOR_UPDATE))
+		/* we don't need to update rate each LED timer event */
+		dw_rate_update(priv);
+
+	if (priv->tx.pending && time_after(jiffies, priv->dev->trans_start + TX_TIMEOUT)) {
+		/* we can't use tx_timeout. It only works when
+		 * netif_carrier_ok, but that is set only when associated.
+		 * But we could loose association and run into a tx
+		 * hanger. Therefore we;d never get associated, and never
+		 * reset.So do it homemade. */
+		dw_tx_reset(priv);
+	}
+
+	mod_timer(&priv->management_timer, jiffies + MANAGEMENT_JIFFIES);
+	dw_unlock(priv, flags);
+}
+
+/**
+ * dw_hw_init_card - initializes card and memory settings
+ */
+static int __init dw_hw_init_card(struct net_device* dev)
+{
+	dw_priv_t*    priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	u32 firmware_id;
+	u32 ctrl;
+	u32 mac_status;
+
+	DBG_FN(DBG_INIT);
+
+	dw_lock(priv, flags);
+
+	/* Configure PIO pins */
+	gpio_direction_output(PIN_LED, 0);
+	gpio_direction_output(PIN_ACTIVITY_LED, 0);
+	dw_set_led_on(PIN_LED, 0);
+	dw_set_led_on(PIN_ACTIVITY_LED, 0);
+	gpio_configure_ns9360(PIN_INTR, 0, 1, 0);
+
+	/* CS2 is used for wireless baseband registers.
+	   Map to MAC_BASE address, 32 bit width, 8 wait states.*/
+	iowrite32(0x82, MEM_DMCONF(2));
+	iowrite32(0, MEM_SMWED(2));
+	iowrite32(2, MEM_SMOED(2));
+	iowrite32(8, MEM_SMRD(2));
+	iowrite32(0, MEM_SMPMRD(2));
+	iowrite32(4, MEM_SMWD(2));
+	iowrite32(2, MEM_SWT(2));
+
+	iowrite32(MAC_BASE_PHYS, SYS_SMCSSMB(2));
+	iowrite32(MAC_MASK, SYS_SMCSSMM(2));
+
+	/* Init baseband processor and MAC assist */
+
+	ctrl = GEN_INIT | GEN_ANTDIV;
+
+	/* we can't do a softreset here */
+
+	/* go back to normal state */
+
+	dw_iowrite32(ctrl,       HW_GEN_CONTROL);
+	dw_iowrite32(0,          HW_MAC_CONTROL);
+	dw_iowrite32(0,          HW_INTR_MASK);
+	/* Mike's recommendation for antenna diversity and antenna map value.
+	   Transmit only on primary antenna, receive on diversity */
+	dw_iowrite32(0x88000000, HW_RSSI_AES);
+
+	/* get MAC from fpga */
+	dw_hw_memcpy_from(dev->dev_addr, HW_STAID0, ETH_ALEN);
+
+	/* Initialize RF tranceiver */
+#if defined(CONFIG_DIGI_WI_G_UBEC_JD)
+	dw_hw_write_rf(7, 0x27ffe); /* TEST register */
+	dw_hw_write_rf(6, 0xf81ac); /* Filter Register */
+	dw_set_tx_power(priv, DW_TX_POWER_DEFAULT); /* Transmitter Gain */
+	dw_hw_write_rf(4, 0x0002b); /* Receiver Gain */
+
+	dw_hw_set_vco(0);     /* no channel selected yet */
+
+	dw_hw_write_rf(0, 0x25f9c); /* Mode Control - Calibrate Filter */
+	udelay(10);
+#elif defined(CONFIG_DIGI_WI_G_UBEC_HC)
+	dw_hw_set_vco(0);     /* no channel selected yet */
+	dw_hw_write_rf(4, 0x0007b); /* Receiver Gain */
+	dw_set_tx_power(priv, DW_TX_POWER_DEFAULT); /* Transmitter Gain */
+	dw_hw_write_rf(6, 0xf81ac); /* Filter Register */
+	dw_hw_write_rf(7, 0x3fffe); /* TEST register */
+
+	dw_hw_write_rf(0, 0x27fdc); /* Mode Control - Calibrate Filter */
+	udelay(10);
+	dw_hw_write_rf(0, 0x27fd4); /* Mode Control - RX/TX mode */
+#endif
+
+	/* Firmware version */
+	firmware_id = dw_ioread32(HW_VERSION);
+	mac_status = dw_ioread32(HW_MAC_STATUS);
+	printk(KERN_DEBUG DRIVER_NAME ": FPGA  HW version: %i.%02i  FW Version: %i.%02i\n",
+		(firmware_id >> 8) & 0xff, firmware_id & 0xff,
+		(mac_status >> 24) & 0xff, (mac_status >> 16) & 0xff);
+
+	dw_unlock(priv, flags);
+
+	return 1;
+}
+
+/**
+ * dw_reset_rx_dups - resets the duplicate list and forgets the senders
+ */
+static void dw_reset_rx_dups(dw_priv_t* priv)
+{
+	unsigned long flags;
+	int i;
+
+	REQUIRE_UNLOCKED(priv);
+
+	dw_lock(priv, flags);
+
+	CLEAR(priv->rx.dups);
+
+	/* for detecting duplicates */
+	INIT_LIST_HEAD(&priv->rx.dups.known.list);
+	INIT_LIST_HEAD(&priv->rx.dups.free.list);
+	for (i = 0; i < ARRAY_SIZE(priv->rx.dups.entries); i++) {
+		struct list_head* entry = &priv->rx.dups.entries[ i ].list;
+		INIT_LIST_HEAD(entry);
+		list_add_tail(entry, &priv->rx.dups.free.list);
+	} /* for (i = 0) */
+
+	dw_unlock(priv, flags);
+}
+
+/**
+ * dw_setup_rx_queue - creates the list structure
+ *
+ * @return: -ENOMEM on no failures otherwise 0
+ */
+static int __init dw_setup_rx_queue(dw_priv_t* priv)
+{
+        dw_frame_rx_t* tmp = NULL;
+        size_t aligned_size;
+        size_t buffer_size;
+        int    i;
+
+        INIT_LIST_HEAD(&priv->rx.queue.free.list);
+        INIT_LIST_HEAD(&priv->rx.queue.filled.list);
+
+        aligned_size = ((char*) &tmp[ 1 ]) - ((char*) &tmp[ 0 ]);
+        buffer_size  = aligned_size * RX_QUEUE_SIZE;
+        priv->rx.queue.buffer =
+                (dw_frame_rx_t*) vmalloc(buffer_size);
+        if (NULL == priv->rx.queue.buffer)
+                goto error;
+
+        memset(priv->rx.queue.buffer, 0, buffer_size);
+
+        /* create frame skb and list for moving unswapped frames */
+        for (i = 0; i < RX_QUEUE_SIZE; i++) {
+                dw_frame_rx_t*    frame = &priv->rx.queue.buffer[ i ];
+                struct list_head* entry = &frame->list;
+
+                frame->skb = dev_alloc_skb(DW_MTU);
+                if (NULL == frame->skb) {
+                        for (; i > 0; i--) {
+                                dev_kfree_skb(frame->skb);
+                                frame->skb = NULL;
+                        }
+
+                        goto error_skb;
+                }
+
+                INIT_LIST_HEAD(entry);
+                list_add(entry, &priv->rx.queue.free.list);
+        }
+
+        return 0;
+
+error_skb:
+        vfree(priv->rx.queue.buffer);
+        priv->rx.queue.buffer = NULL;
+error:
+        ERROR("No Memory for Rx Queue");
+        return -ENOMEM;
+}
+
+/**
+ * dw_start_dev - starts resource
+ */
+static int dw_start_dev(struct platform_device* pdev)
+{
+	struct net_device* dev  = to_dev(pdev);
+	struct dw_priv*    priv = ieee80211softmac_priv(dev);
+	struct list_head*  cursor;
+	int err = -ENODEV;
+	int i;
+
+	DBG_FN(DBG_INIT);
+
+	CLEAR(*priv);
+
+	spin_lock_init(&priv->lock);
+	priv->dev         = dev;
+	priv->short_retry_limit = DW_DEFAULT_SHORT_RETRY_LIMIT;
+	priv->long_retry_limit  = DW_DEFAULT_LONG_RETRY_LIMIT;
+	snprintf(priv->nick, IW_ESSID_MAX_SIZE, "Digi Wireless b/g");
+
+	atomic_set(&priv->fix_rate, 0);
+
+	/* the stack may later reduce the number of supported rates if the
+	 * access point can't provide them */
+	ASSERT(ARRAY_SIZE(dw_rates) <= ARRAY_SIZE(priv->ap_ri.rates));
+	for (i = 0; i < ARRAY_SIZE(dw_rates); i++)
+		priv->ap_ri.rates[ i ] = dw_rates[ i ];
+	priv->ap_ri.count = i;
+
+	/* rx */
+	err = dw_setup_rx_queue(priv);
+	if (err < 0)
+		goto error_rx_queue;
+
+	tasklet_init(&priv->rx.tasklet, dw_rx_tasklet_handler,
+		      (unsigned long) priv);
+	tasklet_disable(&priv->rx.tasklet);  /* will be enabled in open */
+
+	/* tx */
+	atomic_set(&priv->tx.seq_nr, 0);
+	/* first down should sleep */
+	init_MUTEX_LOCKED(&priv->tx.pause_sem);
+	INIT_LIST_HEAD(&priv->tx.queued);
+	INIT_LIST_HEAD(&priv->tx.free);
+	for (i = 0; i < ARRAY_SIZE(priv->tx.frames); i++) {
+		struct list_head* entry = &priv->tx.frames[ i ].list;
+		INIT_LIST_HEAD(entry);
+		list_add_tail(entry, &priv->tx.free);
+	}
+
+	tasklet_init(&priv->tx.tasklet, dw_tx_tasklet_handler,
+			(unsigned long) priv);
+	tasklet_disable(&priv->tx.tasklet);
+
+	init_timer(&priv->management_timer);
+	priv->management_timer.function = dw_management_timer;
+	priv->management_timer.data     = (unsigned long) priv;
+
+	if (!dw_hw_init_card(dev))
+		goto error_init;
+
+	/* card is setup, now we can register the irq */
+	err = request_irq(dev->irq, dw_int, 0, dev->name,
+			priv);
+	if (err) {
+		ERROR("register interrupt %d failed, err %d", dev->irq, err);
+		goto error_irq;
+	}
+	disable_irq(dev->irq);
+
+	priv->softmac = ieee80211_priv(dev);
+	priv->softmac->set_channel      = dw_softmac_set_chan;
+	priv->softmac->set_bssid_filter = dw_softmac_set_bssid_filter;
+	priv->softmac->txrates_change   = dw_softmac_txrates_change;
+	priv->ieee = netdev_priv(dev);
+	priv->ieee->modulation = IEEE80211_OFDM_MODULATION | IEEE80211_CCK_MODULATION;
+	priv->ieee->hard_start_xmit = dw_ieee80211_hard_start_xmit;
+	priv->ieee->set_security    = dw_ieee80211_set_security;
+	priv->ieee->handle_beacon   = dw_ieee80211_handle_beacon;
+	priv->ieee->handle_probe_request = dw_ieee80211_handle_probe_request;
+	priv->ieee->iw_mode         = IW_MODE_INFRA;
+	priv->ieee->freq_band       = IEEE80211_24GHZ_BAND;
+	priv->ieee->rts             = DW_MAX_RTS_THRESHOLD;
+	priv->ieee->perfect_rssi    = MAC_RSSI_MAX;
+	priv->ieee->worst_rssi      = 0;
+	priv->ieee->config         |= CFG_IEEE80211_RTS;
+
+	if (dw_geo_init(priv) < 0)
+		goto error_init2;
+
+	if (register_netdev(dev))
+		goto error_init2;
+
+	ieee80211softmac_notify(dev, IEEE80211SOFTMAC_EVENT_AUTHENTICATED,
+			dw_softmac_notify_authenticated, NULL);
+
+	return 0;
+
+error_init2:
+	free_irq(dev->irq, priv);
+error_irq:
+error_init:
+	list_for_each(cursor, &priv->rx.queue.free.list) {
+		dw_frame_rx_t* frame = list_entry(cursor, dw_frame_rx_t, list);
+		dev_kfree_skb(frame->skb);
+		frame->skb = NULL;
+	}
+	vfree(priv->rx.queue.buffer);
+	priv->rx.queue.buffer = NULL;
+
+error_rx_queue:
+	return err;
+}
+
+/**
+ * dw_stop_dev - stops device
+ */
+static void dw_stop_dev(struct platform_device* pdev)
+{
+	struct net_device* dev  = to_dev(pdev);
+	struct dw_priv*    priv = ieee80211softmac_priv(dev);
+	unsigned long      flags;
+	struct list_head*  it;
+
+	DBG_FN(DBG_INIT);
+
+	dw_lock(priv, flags);
+	dw_iowrite32(0, HW_MAC_CONTROL);
+	dw_unlock(priv, flags);
+
+	list_for_each(it, &priv->rx.queue.free.list) {
+		dw_frame_rx_t* frame = list_entry(it, dw_frame_rx_t, list);
+		dev_kfree_skb(frame->skb);
+		frame->skb = NULL;
+	}
+	vfree(priv->rx.queue.buffer);
+
+	unregister_netdev(dev);
+
+	dw_set_led_on(PIN_LED, 0);
+	dw_set_led_on(PIN_ACTIVITY_LED, 0);
+
+	gpio_free(PIN_LED);
+	gpio_free(PIN_ACTIVITY_LED);
+	gpio_free(PIN_INTR);
+}
+
+/**
+ * dw_remove - stops device and removes all resources
+ */
+static int dw_remove(struct platform_device* pdev)
+{
+	struct net_device* dev = to_dev(pdev);
+
+	DBG_FN(DBG_INIT);
+
+	dw_stop_dev(pdev);
+
+	/* unmap resources */
+	if (0 != dev->base_addr) {
+		iounmap(vbase);
+		vbase = NULL;
+	}
+	free_irq(dev->irq, ieee80211softmac_priv(dev));
+
+	free_ieee80211softmac(dev);
+	dev = NULL;
+
+	platform_set_drvdata(pdev, NULL);
+
+	release_resource(pdev->resource);
+
+	return 0;
+}
+
+/**
+ * dw_release_device - dummy function
+ */
+static void dw_release_device(struct device* dev)
+{
+	DBG_FN(DBG_INIT);
+
+	/* nothing to do. But we need a !NULL function */
+}
+
+/**
+ * dw_softmac_txrates_change - adjusts rate to allowed values.
+ *
+ * Select a better on.
+ */
+static void dw_softmac_txrates_change(struct net_device* dev, u32 changes)
+{
+	dw_priv_t* priv       = ieee80211softmac_priv(dev);
+	int        was_locked = spin_is_locked(&priv->lock);
+	unsigned long flags   = 0;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	if (!was_locked)
+		/* we are called either directly from open/user interface,
+		   or from our tasklet context when processing a frame.*/
+		dw_lock(priv, flags);
+
+	/* new rate */
+	CLEAR(priv->rate);
+
+	dw_rate_reset(priv);
+
+	if (!was_locked)
+		dw_unlock(priv, flags);
+}
+
+/**
+ * dw_wx_set_encode - changes the encoding
+ *
+ * Used by wireless tools <= 28 for WEP-1. Not AES
+ */
+static int dw_wx_set_encode(struct net_device* dev,
+		struct iw_request_info* info, union iwreq_data* data,
+		char* extra)
+{
+	dw_priv_t*    priv = ieee80211softmac_priv(dev);
+	unsigned long ieeeflags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+
+	priv->ieee->host_encrypt      = 1;
+	priv->ieee->host_encrypt_msdu = 1;
+	priv->ieee->host_decrypt      = 1;
+	priv->ieee->host_mc_decrypt   = 1;
+
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+
+	return ieee80211_wx_set_encode(priv->ieee, info, data, extra);
+}
+
+/**
+ * dw_wx_set_encodeext - changes the encoding
+ *
+ * Only used by wpa_supplicant or wireless_tool >= 29
+ */
+static int dw_wx_set_encodeext(struct net_device* dev,
+	struct iw_request_info* info, union iwreq_data* data,
+	char* extra)
+{
+	dw_priv_t*            priv = ieee80211softmac_priv(dev);
+	struct iw_encode_ext* ext = (struct iw_encode_ext*) extra;
+	unsigned long ieeeflags;
+	int           host = (dw_sw_aes || (IW_ENCODE_ALG_CCMP != ext->alg));
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+
+	priv->ieee->host_encrypt      = host;
+	priv->ieee->host_encrypt_msdu = host;
+	priv->ieee->host_decrypt      = host;
+	priv->ieee->host_mc_decrypt   = host;
+
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+
+	/* wx_set_encodeext will initialize itself depending on host_encrypt.
+	 * After wx_set_encodeext, it will be of no further use. */
+	return ieee80211_wx_set_encodeext(priv->ieee, info, data, extra);
+}
+
+/**
+ * dw_softmac_notify_authenticated - called when authenticated
+ *
+ * Resets statistics
+ */
+static void dw_softmac_notify_authenticated(struct net_device* dev,
+		int event_type,  void* context)
+{
+	dw_priv_t*    priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+
+	DBG_FN(DBG_INTERFACE);
+
+	dw_lock(priv, flags);
+	dw_rate_reset(priv);
+	dw_unlock(priv, flags);
+}
+
+/***********************************************************************
+ * @Function: dw_ieee80211_get_network_by_bssid
+ * @Return:
+ * @Descr: Get a network from the list by BSSID with locking
+ ***********************************************************************/
+struct ieee80211_network *
+dw_ieee80211_get_network_by_bssid(struct dw_priv *priv, u8 *bssid)
+{
+	unsigned long ieeeflags;
+	struct ieee80211_network *ieee_net = NULL, *tmp_net;
+	struct list_head *list_ptr;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+	list_for_each(list_ptr, &priv->ieee->network_list) {
+		tmp_net = list_entry(list_ptr, struct ieee80211_network, list);
+		if (!memcmp(tmp_net->bssid, bssid, ETH_ALEN)) {
+			if (ieee_net==NULL) {
+				ieee_net = tmp_net;
+			} else {
+				if (tmp_net->last_scanned > ieee_net->last_scanned)
+					ieee_net = tmp_net;
+			}
+		}
+	}
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+	return ieee_net;
+}
+
+/* CCMP encryption data */
+static const u32 te4[256] = {
+	0x63636363U, 0x7c7c7c7cU, 0x77777777U, 0x7b7b7b7bU,
+	0xf2f2f2f2U, 0x6b6b6b6bU, 0x6f6f6f6fU, 0xc5c5c5c5U,
+	0x30303030U, 0x01010101U, 0x67676767U, 0x2b2b2b2bU,
+	0xfefefefeU, 0xd7d7d7d7U, 0xababababU, 0x76767676U,
+	0xcacacacaU, 0x82828282U, 0xc9c9c9c9U, 0x7d7d7d7dU,
+	0xfafafafaU, 0x59595959U, 0x47474747U, 0xf0f0f0f0U,
+	0xadadadadU, 0xd4d4d4d4U, 0xa2a2a2a2U, 0xafafafafU,
+	0x9c9c9c9cU, 0xa4a4a4a4U, 0x72727272U, 0xc0c0c0c0U,
+	0xb7b7b7b7U, 0xfdfdfdfdU, 0x93939393U, 0x26262626U,
+	0x36363636U, 0x3f3f3f3fU, 0xf7f7f7f7U, 0xccccccccU,
+	0x34343434U, 0xa5a5a5a5U, 0xe5e5e5e5U, 0xf1f1f1f1U,
+	0x71717171U, 0xd8d8d8d8U, 0x31313131U, 0x15151515U,
+	0x04040404U, 0xc7c7c7c7U, 0x23232323U, 0xc3c3c3c3U,
+	0x18181818U, 0x96969696U, 0x05050505U, 0x9a9a9a9aU,
+	0x07070707U, 0x12121212U, 0x80808080U, 0xe2e2e2e2U,
+	0xebebebebU, 0x27272727U, 0xb2b2b2b2U, 0x75757575U,
+	0x09090909U, 0x83838383U, 0x2c2c2c2cU, 0x1a1a1a1aU,
+	0x1b1b1b1bU, 0x6e6e6e6eU, 0x5a5a5a5aU, 0xa0a0a0a0U,
+	0x52525252U, 0x3b3b3b3bU, 0xd6d6d6d6U, 0xb3b3b3b3U,
+	0x29292929U, 0xe3e3e3e3U, 0x2f2f2f2fU, 0x84848484U,
+	0x53535353U, 0xd1d1d1d1U, 0x00000000U, 0xededededU,
+	0x20202020U, 0xfcfcfcfcU, 0xb1b1b1b1U, 0x5b5b5b5bU,
+	0x6a6a6a6aU, 0xcbcbcbcbU, 0xbebebebeU, 0x39393939U,
+	0x4a4a4a4aU, 0x4c4c4c4cU, 0x58585858U, 0xcfcfcfcfU,
+	0xd0d0d0d0U, 0xefefefefU, 0xaaaaaaaaU, 0xfbfbfbfbU,
+	0x43434343U, 0x4d4d4d4dU, 0x33333333U, 0x85858585U,
+	0x45454545U, 0xf9f9f9f9U, 0x02020202U, 0x7f7f7f7fU,
+	0x50505050U, 0x3c3c3c3cU, 0x9f9f9f9fU, 0xa8a8a8a8U,
+	0x51515151U, 0xa3a3a3a3U, 0x40404040U, 0x8f8f8f8fU,
+	0x92929292U, 0x9d9d9d9dU, 0x38383838U, 0xf5f5f5f5U,
+	0xbcbcbcbcU, 0xb6b6b6b6U, 0xdadadadaU, 0x21212121U,
+	0x10101010U, 0xffffffffU, 0xf3f3f3f3U, 0xd2d2d2d2U,
+	0xcdcdcdcdU, 0x0c0c0c0cU, 0x13131313U, 0xececececU,
+	0x5f5f5f5fU, 0x97979797U, 0x44444444U, 0x17171717U,
+	0xc4c4c4c4U, 0xa7a7a7a7U, 0x7e7e7e7eU, 0x3d3d3d3dU,
+	0x64646464U, 0x5d5d5d5dU, 0x19191919U, 0x73737373U,
+	0x60606060U, 0x81818181U, 0x4f4f4f4fU, 0xdcdcdcdcU,
+	0x22222222U, 0x2a2a2a2aU, 0x90909090U, 0x88888888U,
+	0x46464646U, 0xeeeeeeeeU, 0xb8b8b8b8U, 0x14141414U,
+	0xdedededeU, 0x5e5e5e5eU, 0x0b0b0b0bU, 0xdbdbdbdbU,
+	0xe0e0e0e0U, 0x32323232U, 0x3a3a3a3aU, 0x0a0a0a0aU,
+	0x49494949U, 0x06060606U, 0x24242424U, 0x5c5c5c5cU,
+	0xc2c2c2c2U, 0xd3d3d3d3U, 0xacacacacU, 0x62626262U,
+	0x91919191U, 0x95959595U, 0xe4e4e4e4U, 0x79797979U,
+	0xe7e7e7e7U, 0xc8c8c8c8U, 0x37373737U, 0x6d6d6d6dU,
+	0x8d8d8d8dU, 0xd5d5d5d5U, 0x4e4e4e4eU, 0xa9a9a9a9U,
+	0x6c6c6c6cU, 0x56565656U, 0xf4f4f4f4U, 0xeaeaeaeaU,
+	0x65656565U, 0x7a7a7a7aU, 0xaeaeaeaeU, 0x08080808U,
+	0xbabababaU, 0x78787878U, 0x25252525U, 0x2e2e2e2eU,
+	0x1c1c1c1cU, 0xa6a6a6a6U, 0xb4b4b4b4U, 0xc6c6c6c6U,
+	0xe8e8e8e8U, 0xddddddddU, 0x74747474U, 0x1f1f1f1fU,
+	0x4b4b4b4bU, 0xbdbdbdbdU, 0x8b8b8b8bU, 0x8a8a8a8aU,
+	0x70707070U, 0x3e3e3e3eU, 0xb5b5b5b5U, 0x66666666U,
+	0x48484848U, 0x03030303U, 0xf6f6f6f6U, 0x0e0e0e0eU,
+	0x61616161U, 0x35353535U, 0x57575757U, 0xb9b9b9b9U,
+	0x86868686U, 0xc1c1c1c1U, 0x1d1d1d1dU, 0x9e9e9e9eU,
+	0xe1e1e1e1U, 0xf8f8f8f8U, 0x98989898U, 0x11111111U,
+	0x69696969U, 0xd9d9d9d9U, 0x8e8e8e8eU, 0x94949494U,
+	0x9b9b9b9bU, 0x1e1e1e1eU, 0x87878787U, 0xe9e9e9e9U,
+	0xcecececeU, 0x55555555U, 0x28282828U, 0xdfdfdfdfU,
+	0x8c8c8c8cU, 0xa1a1a1a1U, 0x89898989U, 0x0d0d0d0dU,
+	0xbfbfbfbfU, 0xe6e6e6e6U, 0x42424242U, 0x68686868U,
+	0x41414141U, 0x99999999U, 0x2d2d2d2dU, 0x0f0f0f0fU,
+	0xb0b0b0b0U, 0x54545454U, 0xbbbbbbbbU, 0x16161616U,
+};
+
+static const u32 rcon[] = {
+	/* for 128-bit blocks, Rijndael never uses more than 10 rcon values */
+	0x01000000, 0x02000000,
+	0x04000000, 0x08000000,
+	0x10000000, 0x20000000,
+	0x40000000, 0x80000000,
+	0x1B000000, 0x36000000,
+};
+
+/***********************************************************************
+ * @Function: dw_aes_set_encrypt_key
+ * @Return:
+ * @Descr: Expands the cipher key into the encryption key schedule
+ ***********************************************************************/
+static int dw_aes_set_encrypt_key(const unsigned char * user_key,
+		const int bits, struct aes_key_st * key)
+{
+	u32 *rk;
+	int i = 0;
+	u32 temp;
+
+        DBG_FN(DBG_SECURITY);
+
+	if (!user_key || !key)
+		return -1;
+	if (bits != 128 && bits != 192 && bits != 256)
+		return -2;
+
+	rk = (u32*)key->rd_key;
+
+	rk[0] = GETU32(user_key    );
+	rk[1] = GETU32(user_key +  4);
+	rk[2] = GETU32(user_key +  8);
+	rk[3] = GETU32(user_key + 12);
+	if (bits == 128) {
+		while (1) {
+			temp  = rk[3];
+			rk[4] = rk[0] ^
+				(te4[(temp >> 16) & 0xff] & 0xff000000) ^
+				(te4[(temp >>  8) & 0xff] & 0x00ff0000) ^
+				(te4[(temp     ) & 0xff] & 0x0000ff00) ^
+				(te4[(temp >> 24)       ] & 0x000000ff) ^
+				rcon[i];
+			rk[5] = rk[1] ^ rk[4];
+			rk[6] = rk[2] ^ rk[5];
+			rk[7] = rk[3] ^ rk[6];
+			if (++i == 10) {
+				return 0;
+			}
+			rk += 4;
+		}
+	}
+	rk[4] = GETU32(user_key + 16);
+	rk[5] = GETU32(user_key + 20);
+	if (bits == 192) {
+		while (1) {
+			temp = rk[ 5];
+			rk[ 6] = rk[ 0] ^
+				(te4[(temp >> 16) & 0xff] & 0xff000000) ^
+				(te4[(temp >>  8) & 0xff] & 0x00ff0000) ^
+				(te4[(temp     ) & 0xff] & 0x0000ff00) ^
+				(te4[(temp >> 24)       ] & 0x000000ff) ^
+				rcon[i];
+			rk[ 7] = rk[ 1] ^ rk[ 6];
+			rk[ 8] = rk[ 2] ^ rk[ 7];
+			rk[ 9] = rk[ 3] ^ rk[ 8];
+			if (++i == 8) {
+				return 0;
+			}
+			rk[10] = rk[ 4] ^ rk[ 9];
+			rk[11] = rk[ 5] ^ rk[10];
+			rk += 6;
+		}
+	}
+	rk[6] = GETU32(user_key + 24);
+	rk[7] = GETU32(user_key + 28);
+	if (bits == 256) {
+		while (1) {
+			temp = rk[ 7];
+			rk[ 8] = rk[ 0] ^
+				(te4[(temp >> 16) & 0xff] & 0xff000000) ^
+				(te4[(temp >>  8) & 0xff] & 0x00ff0000) ^
+				(te4[(temp     ) & 0xff] & 0x0000ff00) ^
+				(te4[(temp >> 24)       ] & 0x000000ff) ^
+				rcon[i];
+			rk[ 9] = rk[ 1] ^ rk[ 8];
+			rk[10] = rk[ 2] ^ rk[ 9];
+			rk[11] = rk[ 3] ^ rk[10];
+			if (++i == 7) {
+				return 0;
+			}
+			temp = rk[11];
+			rk[12] = rk[ 4] ^
+				(te4[(temp >> 24)       ] & 0xff000000) ^
+				(te4[(temp >> 16) & 0xff] & 0x00ff0000) ^
+				(te4[(temp >>  8) & 0xff] & 0x0000ff00) ^
+				(te4[(temp     ) & 0xff] & 0x000000ff);
+			rk[13] = rk[ 5] ^ rk[12];
+			rk[14] = rk[ 6] ^ rk[13];
+			rk[15] = rk[ 7] ^ rk[14];
+
+			rk += 8;
+		}
+	}
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: dw_ccmp_set_key
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static u8 dw_ccmp_set_key(struct dw_priv *priv, int id, u8 *data, int len, u8 *seq)
+{
+	ccmp_key_t *key;
+
+	DBG_FN(DBG_SECURITY);
+
+	key = &priv->aeskeys[id];
+
+	if (len == CCMP_KEY_SIZE)
+	{
+		CLEAR(*key);
+		key->valid = 1;
+		/* Set sequence counter if given */
+		if (seq)
+			key->rx_pn  = dw_to_48(GET32(seq+2), GET16(seq));
+
+		/* Store only AES key schedule for this key */
+		dw_aes_set_encrypt_key(data, AES_BITS, &key->rk);
+	}
+	else if (len == 0) {
+		key->valid = 0;
+	}
+	else
+		return 0;
+
+	return 1;
+}
+
+
+/***********************************************************************
+ * @Function: dw_send_key
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_send_key(int index, unsigned long *key)
+{
+	DBG_FN(DBG_SECURITY);
+
+	if (!dw_hw_aes_wait())
+		goto error;
+
+	/* Set key load mode */
+	dw_iowrite32(0x14 | index, HW_AES_MODE);
+
+	/* Write key to hw */
+	dw_hw_aes_write_fifo_noswap(key, 4*44);
+
+	return 0;
+
+error:
+	return -1;
+}
+
+
+/***********************************************************************
+ * @Function: dw_send_tx_key
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_send_tx_key(struct dw_priv *priv, int index)
+{
+	ccmp_key_t *key;
+
+	DBG_FN(DBG_SECURITY);
+
+	if (!(priv->ieee->sec.flags & (1 << index)))
+		return 0;
+
+	/* expand key */
+	if (0 == dw_ccmp_set_key(priv, index, priv->ieee->sec.keys[index], priv->ieee->sec.key_sizes[index], 0)) {
+		printk(KERN_ERR "Error while expanding tx Key!!!\n");
+		return -1;
+	}
+	key = &priv->aeskeys[index];
+
+	return dw_send_key(index, key->rk.rd_key);
+}
+
+
+/***********************************************************************
+ * @Function: dw_send_keys
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_send_keys(struct dw_priv *priv)
+{
+	ccmp_key_t *key;
+	int i, len, err = 0;
+
+	DBG_FN(DBG_SECURITY);
+	REQUIRE_LOCKED(priv);
+
+	/* Note: AES keys cannot be set for multiple times.
+	 * Only set it at the first time. */
+	for (i = 0; i < 4; i++) {
+		key = &priv->aeskeys[i];
+		len = priv->ieee->sec.key_sizes[i];
+		if (!(priv->ieee->sec.flags & (1 << i))) {
+			continue;
+		}
+
+		/* expand key */
+		if (0 == dw_ccmp_set_key(priv, i, priv->ieee->sec.keys[i], len, 0)) {
+			printk(KERN_ERR "Error while expanding Key!!!\n");
+			continue;
+		}
+
+		if (dw_send_key(i, key->rk.rd_key)<0)
+			err = -1;
+	}
+	return err;
+}
+
+
+/***********************************************************************
+ * @Function: dw_set_hwcrypto_keys
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static void dw_set_hwcrypto_keys(struct dw_priv *priv)
+{
+	int err = 0;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	switch (priv->ieee->sec.level) {
+	case SEC_LEVEL_3:
+		if (priv->ieee->sec.flags & SEC_ACTIVE_KEY)
+			if (dw_send_tx_key(priv, priv->ieee->sec.active_key) < 0)
+				err = -1;
+
+		if (!dw_sw_aes)
+			if (dw_send_keys(priv) < 0)
+				err = -1;
+		break;
+	default:
+		break;
+	}
+	if (err < 0)
+		printk(KERN_ERR "Error while sending keys to hardware!!!\n");
+}
+
+
+/***********************************************************************
+ * @Function: dw_ieee80211_set_security
+ * @Return:
+ * @Descr: set_security() callback in struct ieee80211_device
+ ***********************************************************************/
+static void dw_ieee80211_set_security(struct net_device *dev,
+					   struct ieee80211_security *sec)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	int i, flag;
+	unsigned long flags;
+	unsigned long ieeeflags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+	for (i = 0; i < 4; i++) {
+		flag = 1 << i;
+		if (sec->flags & (flag)) {
+			priv->ieee->sec.encode_alg[i] = sec->encode_alg[i];
+			priv->ieee->sec.key_sizes[i] = sec->key_sizes[i];
+			if (sec->key_sizes[i] == 0)
+				priv->ieee->sec.flags &= ~(flag);
+			else {
+				memcpy(priv->ieee->sec.keys[i], sec->keys[i],
+				       sec->key_sizes[i]);
+				priv->ieee->sec.flags |= (flag);
+			}
+		} else if (sec->level != SEC_LEVEL_1)
+			priv->ieee->sec.flags &= ~(flag);
+	}
+
+	if (sec->flags & SEC_ACTIVE_KEY) {
+		if (sec->active_key <= 3) {
+			priv->ieee->sec.active_key = sec->active_key;
+			priv->ieee->sec.flags |= SEC_ACTIVE_KEY;
+		} else
+			priv->ieee->sec.flags &= ~SEC_ACTIVE_KEY;
+	} else
+		priv->ieee->sec.flags &= ~SEC_ACTIVE_KEY;
+
+	if ((sec->flags & SEC_AUTH_MODE) &&
+	    (priv->ieee->sec.auth_mode != sec->auth_mode)) {
+		priv->ieee->sec.auth_mode = sec->auth_mode;
+		priv->ieee->sec.flags |= SEC_AUTH_MODE;
+	}
+
+	if (sec->flags & SEC_ENABLED && priv->ieee->sec.enabled != sec->enabled) {
+		priv->ieee->sec.flags |= SEC_ENABLED;
+		priv->ieee->sec.enabled = sec->enabled;
+	}
+
+	if (sec->flags & SEC_ENCRYPT)
+		priv->ieee->sec.encrypt = sec->encrypt;
+
+	if (sec->flags & SEC_LEVEL && priv->ieee->sec.level != sec->level) {
+		priv->ieee->sec.level = sec->level;
+		priv->ieee->sec.flags |= SEC_LEVEL;
+	}
+
+	if (!priv->ieee->host_encrypt && (sec->flags & SEC_ENCRYPT))
+		dw_set_hwcrypto_keys(priv);
+
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+	dw_unlock(priv, flags);
+}
+
+
+/***********************************************************************
+ * @Function: dw_softmac_set_bssid_filter
+ * @Return:
+ * @Descr: Set BSS mode and IDs (irq)
+ ***********************************************************************/
+static void dw_softmac_set_bssid_filter(struct net_device *dev, const u8 *bssid)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	struct ieee80211softmac_network *net;
+	int i;
+	int was_locked = spin_is_locked(&priv->lock);
+	unsigned long flags = 0;
+	int last_basic = 0;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	if (!was_locked)
+		dw_lock(priv, flags);
+
+	priv->beacon_ready = 0;
+
+	/* Set BSSID in hardware */
+	dw_hw_memcpy_to(HW_BSSID0, bssid, ETH_ALEN);
+
+	if (priv->softmac->associnfo.associated) {
+		/* Set SSID in hardware */
+		dw_iowrite32(priv->softmac->associnfo.associate_essid.len |
+				0x000e0000, HW_SSID_LEN);
+		/*TODO set rates*/
+
+		/* get ratesinfo from assocnetwork */
+		net = ieee80211softmac_get_network_by_essid_locked(priv->softmac, &priv->softmac->associnfo.associate_essid);
+
+		if (net) {
+			/* provide only the subset of the rates we support */
+			priv->ap_ri.count = 0;
+
+			for (i=0; i < net->supported_rates.count; i++) {
+				int j = 0;
+				for (j = 0; j < ARRAY_SIZE(dw_rates); j++) {
+					if ((net->supported_rates.rates[ i ] & 0x7F) == (dw_rates[ j ] & 0x7F)) {
+						priv->ap_ri.rates[ priv->ap_ri.count++ ] = net->supported_rates.rates[ i ];
+
+						break;
+					}
+				}
+			}
+		}
+
+
+		dw_hw_memcpy_to(HW_SSID, priv->softmac->associnfo.associate_essid.data, priv->softmac->associnfo.associate_essid.len);
+		/* Disable IBSS mode */
+		//dw_iocleanbits32(HW_MAC_CONTROL, CTRL_IBSS | CTRL_BEACONTX);
+	} else {
+		/* reset it */
+		for (i = 0; i < ARRAY_SIZE(dw_rates); i++)
+			priv->ap_ri.rates[ i ] = dw_rates[ i ];
+		priv->ap_ri.count = i;
+	}
+
+	/* determine equal or lower basic rate for the rate being used.
+	   The acknowledge of AP is send with
+	   api_ri.rates[ basics[ data_rate_index ] ]*/
+	for (i = 0; i < ARRAY_SIZE(priv->tx.basics); i++) {
+		if ((priv->ap_ri.rates[ i ] & IEEE80211_BASIC_RATE_MASK) == IEEE80211_BASIC_RATE_MASK)
+			last_basic = i;
+		priv->tx.basics[ i ] = last_basic;
+	}
+
+	priv->softmac->txrates.default_rate = BASIC_RATE_MASK(priv->ap_ri.rates[ priv->rate.index ]);
+	DBG(DBG_UPDATE_RATE, "Rate set to %i", priv->softmac->txrates.default_rate);
+	priv->rate.tx_data_any = priv->rate.tx_data_ack = 0; // reset counter so we work on the next time slice
+
+	if (!was_locked)
+		dw_unlock(priv, flags);
+
+}
+
+/***********************************************************************
+ * @Function: dw_set_channel
+ * @Return:
+ * @Descr: Select a channel
+ ***********************************************************************/
+static void dw_set_channel(dw_priv_t* priv, u8 channel)
+{
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+	REQUIRE_LOCKED(priv);
+
+	dw_iocleanbits32(HW_GEN_CONTROL, GEN_RXEN);
+	priv->beacon_ready = 0;
+
+	/* Set frequency divider for channel */
+	dw_hw_write_rf (1, freq_table[channel].integer);
+	dw_hw_write_rf (2, freq_table[channel].fraction);
+
+#if defined(CONFIG_DIGI_WI_G_UBEC_JD)
+	/* Filter  calibration */
+	dw_hw_write_rf (0, 0x25f9c);
+	udelay (10);
+
+	/* VCO calibration */
+	dw_hw_write_rf (0, 0x25f9a);
+	udelay (80);
+
+	/* Mode Control - RX/TX mode */
+	dw_hw_write_rf (0, 0x25f94);
+
+	/* Allow the trannsceiver to settle in the new mode of operation */
+	udelay (10);
+#elif defined(CONFIG_DIGI_WI_G_UBEC_HC)
+	/* VCO Calibration */
+	dw_hw_write_rf (0, 0x27fda);
+	udelay(40);
+
+	/* Set frequency divider for channel again */
+	dw_hw_write_rf (1, freq_table[channel].integer);
+	dw_hw_write_rf (2, freq_table[channel].fraction);
+
+	/* Mode Control - RX/TX mode */
+	dw_hw_write_rf (0, 0x27fd4);
+
+	dw_hw_set_vco(channel);
+#endif
+	priv->channel = channel;
+	dw_iosetbits32(HW_GEN_CONTROL, GEN_RXEN);
+}
+
+/***********************************************************************
+ * @Function: dw_softmac_set_chan
+ * @Return:
+ * @Descr: Select a channel)
+ ***********************************************************************/
+static void dw_softmac_set_chan(struct net_device* dev, u8 channel)
+{
+	dw_priv_t*    priv = ieee80211softmac_priv(dev);
+	//struct ieee80211softmac_device *mac = ieee80211_priv(dev);
+	unsigned long flags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	//if (IW_MODE_ADHOC == priv->ieee->iw_mode && !mac->scanning)
+	//    printk("Selected channel = %d\n", channel);
+
+	/* we really don't want to change the channel while there is something
+	 * in the queue for a different channel */
+	dw_tx_wait_for_idle_and_pause(priv);
+
+	dw_lock(priv, flags);
+	dw_set_channel( priv, channel);
+	dw_unlock(priv, flags);
+
+	dw_tx_continue_queue(priv);
+}
+
+
+/***********************************************************************
+ * @Function: dw_geo_init
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_geo_init(dw_priv_t *priv)
+{
+	struct ieee80211_geo *geo;
+	struct ieee80211_channel *chan;
+	int i;
+	u8 channel;
+	const char *iso_country;
+
+	DBG_FN(DBG_INIT);
+
+	geo = kzalloc(sizeof(*geo), GFP_KERNEL);
+	if (!geo)
+		return -ENOMEM;
+
+	iso_country = "XX";
+
+	for (i = 0, channel = IEEE80211_52GHZ_MIN_CHANNEL;
+	channel <= IEEE80211_52GHZ_MAX_CHANNEL; channel++) {
+		chan = &geo->a[i++];
+		chan->freq = dw_channel_to_freq_a(channel);
+		chan->channel = channel;
+	}
+	geo->a_channels = i;
+	for (i = 0, channel = IEEE80211_24GHZ_MIN_CHANNEL;
+		channel <= IEEE80211_24GHZ_MAX_CHANNEL; channel++) {
+		chan = &geo->bg[i++];
+		chan->freq = dw_channel_to_freq_bg(channel);
+		chan->channel = channel;
+	}
+	geo->bg_channels = i;
+	memcpy(geo->name, iso_country, 2);
+	if (0 /*TODO: Outdoor use only */)
+		geo->name[2] = 'O';
+	else if (0 /*TODO: Indoor use only */)
+		geo->name[2] = 'I';
+	else
+		geo->name[2] = ' ';
+	geo->name[3] = '\0';
+
+	ieee80211_set_geo(priv->ieee, geo);
+	kfree(geo);
+
+	return 1;
+}
+
+
+/***********************************************************************
+ * @Function: dw_get_name
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_get_name(struct net_device *dev, struct iw_request_info *info,
+		union iwreq_data *data, char *extra)
+{
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	strcpy(data->name, "IEEE 802.11b/g");
+	return 0;
+}
+
+/***********************************************************************
+ * @Function: dw_wx_get_encode
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_get_encode(struct net_device *dev,
+				   struct iw_request_info *info,
+				   union iwreq_data *data,
+				   char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	int err;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	err = ieee80211_wx_get_encode(priv->ieee, info, data, extra);
+
+	return err;
+}
+
+/***********************************************************************
+ * @Function: dw_wx_get_encodeext
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_get_encodeext(struct net_device *dev,
+		struct iw_request_info *info, union iwreq_data *data,
+		char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	int err;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	err = ieee80211_wx_get_encodeext(priv->ieee, info, data, extra);
+
+	return err;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_set_channelfreq
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_set_channelfreq(struct net_device *dev,
+				      struct iw_request_info *info,
+				      union iwreq_data *data,
+				      char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	u8 channel;
+	int err = -EINVAL;
+	unsigned long flags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_tx_wait_for_idle_and_pause(priv);
+
+	dw_lock(priv, flags);
+
+	if ((data->freq.m >= 0) && (data->freq.m <= 1000)) {
+		channel = data->freq.m;
+	} else {
+		channel = ieee80211_freq_to_channel(priv->ieee, data->freq.m);
+	}
+	if (ieee80211_is_valid_channel(priv->ieee, channel)) {
+		dw_set_channel(priv, channel);
+		err = 0;
+	}
+
+	dw_unlock(priv, flags);
+
+	dw_tx_continue_queue(priv);
+
+	return err;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_get_channelfreq
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_get_channelfreq(struct net_device *dev,
+		struct iw_request_info *info, union iwreq_data *data,
+		char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+	data->freq.e = 1;
+	data->freq.m = ieee80211_channel_to_freq(priv->ieee, priv->channel) * 100000;
+	data->freq.flags = IW_FREQ_FIXED;
+	dw_unlock(priv, flags);
+	return 0;
+}
+
+/***********************************************************************
+ * @Function: dw_wx_set_mode
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_set_mode(struct net_device *dev, struct iw_request_info *info,
+		union iwreq_data *data, char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	unsigned long ieeeflags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+
+	priv->ieee->iw_mode = data->mode;
+
+	if (IW_MODE_ADHOC == priv->ieee->iw_mode) {
+		if (priv->channel)
+			priv->adhoc.channel = priv->channel;
+	} else {
+		dw_iocleanbits32(HW_MAC_CONTROL, CTRL_IBSS|CTRL_BEACONTX); /* Disable IBSS mode */
+	}
+
+	priv->beacon_ready = 0;
+	priv->beacon_body_length=0;
+
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_get_mode
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_get_mode(struct net_device *dev, struct iw_request_info *info,
+		union iwreq_data *data, char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	unsigned long ieeeflags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+	data->mode = priv->ieee->iw_mode;
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_get_rangeparams
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_get_rangeparams(struct net_device *dev,
+		struct iw_request_info *info, union iwreq_data *data,
+		char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	struct iw_range *range = (struct iw_range *)extra;
+	const struct ieee80211_geo *geo;
+	int i, j, level;
+	unsigned long ieeeflags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+	data->data.length = sizeof(*range);
+	memset(range, 0, sizeof(*range));
+
+	/*TODO: What about 802.11b? Have to meassure!!! */
+	/* 54Mb/s == ~27Mb/s payload throughput (802.11g) */
+	range->throughput = 27 * 1000 * 1000;
+	range->min_nwid = 0x0000;
+	range->max_nwid = 0x0020;
+
+	range->max_qual.qual = 100;
+	range->max_qual.level = MAC_RSSI_MAX;
+	range->max_qual.noise = 0;
+	range->max_qual.updated =
+		IW_QUAL_QUAL_UPDATED | IW_QUAL_LEVEL_UPDATED;
+
+	range->avg_qual.qual    = 0;
+	range->avg_qual.level   = 0;
+	range->avg_qual.noise   = 0;
+	range->avg_qual.updated = IW_QUAL_NOISE_INVALID;
+
+	range->min_rts = DW_MIN_RTS_THRESHOLD;
+	range->max_rts = DW_MAX_RTS_THRESHOLD;
+	range->min_frag = MIN_FRAG_THRESHOLD;
+	range->max_frag = MAX_FRAG_THRESHOLD;
+
+	range->encoding_size[0] = 5;
+	range->encoding_size[1] = 13;
+	range->num_encoding_sizes = 2;
+	range->max_encoding_tokens = WEP_KEYS;
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 19;
+
+	range->enc_capa = IW_ENC_CAPA_WPA |
+			  IW_ENC_CAPA_WPA2 |
+			  IW_ENC_CAPA_CIPHER_TKIP |
+			  IW_ENC_CAPA_CIPHER_CCMP;
+
+	for (i = 0; i < ARRAY_SIZE(dw_rates); i++)
+		range->bitrate[ i ] = RATE_IN_BS(BASIC_RATE_MASK(dw_rates[ i ]));
+	range->num_bitrates = i;
+
+	geo = ieee80211_get_geo(priv->ieee);
+	range->num_channels = geo->bg_channels;
+	j = 0;
+	for (i = 0; i < geo->bg_channels; i++) {
+		if (j == IW_MAX_FREQUENCIES)
+			break;
+		range->freq[j].i = j + 1;
+		range->freq[j].m = geo->bg[i].freq * 100000;
+		range->freq[j].e = 1;
+		j++;
+	}
+	range->num_frequency = j;
+
+	/* retry limit capabilities */
+	range->retry_capa = IW_RETRY_LIMIT | IW_RETRY_LIFETIME;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->r_time_flags = IW_RETRY_LIFETIME;
+
+	/* I don't know the range.*/
+	range->min_retry = 1;
+	range->max_retry = 65535;
+	range->min_r_time = 1024;
+	range->max_r_time = 65535 * 1024;
+
+	/* txpower is supported in dBm's */
+//	if (priv->ieee->iw_mode == IW_MODE_ADHOC) {
+		range->txpower_capa = IW_TXPOW_DBM;
+		range->num_txpower = IW_MAX_TXPOWER;
+		for (i = 0, level = (DW_TX_POWER_MAX_DBM * 16);
+				i < IW_MAX_TXPOWER;
+				i++, level -=
+				((DW_TX_POWER_MAX_DBM -
+				DW_TX_POWER_MIN_DBM) * 16) / (IW_MAX_TXPOWER - 1))
+			range->txpower[i] = level / 16;
+//	} else {
+//		range->txpower_capa = 0;
+//		range->num_txpower = 0;
+//	}
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: dw_get_wireless_stats
+ * @Return:
+ * @Descr: Get wireless statistics.  Called by /proc/net/wireless and by SIOCGIWSTATS
+ ***********************************************************************/
+static struct iw_statistics *dw_get_wireless_stats(struct net_device *dev)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	struct ieee80211softmac_device *mac = ieee80211_priv(dev);
+	struct iw_statistics *wstats;
+	unsigned long ieeeflags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	wstats = &priv->wstats;
+	if (!mac->associnfo.associated) {
+		wstats->miss.beacon = 0;
+		wstats->discard.retries = 0;
+		wstats->discard.nwid = 0;
+		wstats->discard.code = 0;
+		wstats->discard.fragment = 0;
+		wstats->discard.misc = 0;
+		wstats->qual.qual = 0;
+		wstats->qual.level = 0;
+		wstats->qual.noise = 0;
+		wstats->qual.updated = IW_QUAL_NOISE_INVALID |
+			IW_QUAL_QUAL_INVALID | IW_QUAL_LEVEL_INVALID;
+	} else {
+		spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+		/* fill in the real statistics when iface associated */
+		wstats->qual.qual =
+					(100 *
+					(priv->ieee->perfect_rssi - priv->ieee->worst_rssi) *
+					(priv->ieee->perfect_rssi - priv->ieee->worst_rssi) -
+					(priv->ieee->perfect_rssi - priv->ieee->networks->stats.rssi) *
+					(15 * (priv->ieee->perfect_rssi - priv->ieee->worst_rssi) +
+					62 * (priv->ieee->perfect_rssi -
+					priv->ieee->networks->stats.rssi))) /
+					((priv->ieee->perfect_rssi -
+					priv->ieee->worst_rssi) * (priv->ieee->perfect_rssi -
+					priv->ieee->worst_rssi));
+		/*
+		 * !TODO. Seems they are not calculated. Take
+		 * one from the last frame */
+		wstats->qual.level = priv->ieee->networks->stats.signal;
+		wstats->qual.noise = priv->ieee->networks->stats.noise;
+		wstats->qual.updated = IW_QUAL_QUAL_UPDATED | IW_QUAL_LEVEL_UPDATED;
+		wstats->discard.code = priv->ieee->ieee_stats.rx_discards_undecryptable;
+		wstats->discard.retries = priv->ieee->ieee_stats.tx_retry_limit_exceeded;
+		wstats->discard.nwid = priv->ieee->ieee_stats.tx_discards_wrong_sa;
+		wstats->discard.fragment = priv->ieee->ieee_stats.rx_fragments;
+		wstats->discard.misc = priv->wstats.discard.misc;
+		wstats->miss.beacon = 0;	/* FIXME */
+		spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+	}
+	return wstats;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_set_nick
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_set_nick(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *data,
+			       char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	size_t len;
+	unsigned long flags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+	len =  min((size_t)data->data.length, (size_t)IW_ESSID_MAX_SIZE);
+	memcpy(priv->nick, extra, len);
+	priv->nick[len] = '\0';
+	dw_unlock(priv, flags);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_get_nick
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_get_nick(struct net_device *dev, struct iw_request_info *info,
+		union iwreq_data *data, char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	size_t len;
+	unsigned long flags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+	len = strlen(priv->nick) + 1;
+	memcpy(extra, priv->nick, len);
+	data->data.length = (__u16)len;
+	dw_unlock(priv, flags);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_set_rts
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_set_rts(struct net_device *dev, struct iw_request_info *info,
+		union iwreq_data *data, char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	int err = -EINVAL;
+	unsigned long flags;
+	unsigned long ieeeflags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+	if (data->rts.disabled) {
+		priv->ieee->rts = DW_MAX_RTS_THRESHOLD;
+		err = 0;
+	} else {
+		if (data->rts.value >= DW_MIN_RTS_THRESHOLD &&
+		    data->rts.value <= DW_MAX_RTS_THRESHOLD) {
+			priv->ieee->rts = data->rts.value;
+			err = 0;
+		}
+	}
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+	dw_unlock(priv, flags);
+
+	return err;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_get_rts
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_get_rts(struct net_device *dev, struct iw_request_info *info,
+		union iwreq_data *data, char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	unsigned long ieeeflags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+	data->rts.value = priv->ieee->rts;
+	data->rts.fixed = 0;
+	data->rts.disabled = (priv->ieee->rts == DW_MAX_RTS_THRESHOLD);
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+	dw_unlock(priv, flags);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_set_rate
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_set_rate(struct net_device *dev,
+			      struct iw_request_info *info,
+			      union iwreq_data *data,
+			      char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	s32 in_rate = data->bitrate.value;
+	int err = 0;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+
+	if (in_rate == -1)
+		/* auto */
+		atomic_set(&priv->fix_rate, 0);
+	else
+		atomic_set(&priv->fix_rate, 1);
+
+	err = ieee80211softmac_wx_set_rate(dev, info, data, extra);
+
+	dw_unlock(priv, flags);
+
+	return err;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_set_frag
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_set_frag(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *data,
+			       char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	int err = -EINVAL;
+	unsigned long flags;
+	unsigned long ieeeflags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+	if (data->frag.disabled) {
+		priv->ieee->fts = MAX_FRAG_THRESHOLD;
+		err = 0;
+	} else {
+		if (data->frag.value >= MIN_FRAG_THRESHOLD &&
+		    data->frag.value <= MAX_FRAG_THRESHOLD) {
+			priv->ieee->fts = data->frag.value & ~0x1;
+			err = 0;
+		}
+	}
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+	dw_unlock(priv, flags);
+
+	return err;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_get_frag
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_get_frag(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *data,
+			       char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	unsigned long ieeeflags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+	spin_lock_irqsave(&priv->ieee->lock, ieeeflags);
+	data->frag.value = priv->ieee->fts;
+	data->frag.fixed = 0;
+	data->frag.disabled = (priv->ieee->fts == MAX_FRAG_THRESHOLD);
+	spin_unlock_irqrestore(&priv->ieee->lock, ieeeflags);
+	dw_unlock(priv, flags);
+
+	return 0;
+}
+
+/***********************************************************************
+ * @Function: dw_wx_set_xmitpower
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_set_xmitpower(struct net_device *dev,
+				    struct iw_request_info *info,
+				    union iwreq_data *data,
+				    char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	int err = 0, value;
+	unsigned long flags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	if ((data->txpower.flags & IW_TXPOW_TYPE) != IW_TXPOW_DBM) return -EINVAL;
+	if (data->txpower.fixed == 0) return -EINVAL;
+	if (data->txpower.value < DW_TX_POWER_MIN_DBM || data->txpower.value > DW_TX_POWER_MAX_DBM) return -EINVAL;
+
+	value = data->txpower.value;
+	dw_lock(priv, flags);
+	err = dw_set_tx_power(priv, value);
+	dw_unlock(priv, flags);
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_get_xmitpower
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_get_xmitpower(struct net_device *dev,
+		struct iw_request_info *info, union iwreq_data *data,
+		char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	data->txpower.fixed = 1;
+	data->txpower.value = priv->tx_power;
+	data->txpower.flags = IW_TXPOW_DBM;
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_set_retry
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_set_retry(struct net_device *dev, struct iw_request_info *info,
+		union iwreq_data *wrqu, char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	if (wrqu->retry.flags & IW_RETRY_LIFETIME || wrqu->retry.disabled)
+		return -EINVAL;
+
+	if (!(wrqu->retry.flags & IW_RETRY_LIMIT))
+		return 0;
+
+	if (wrqu->retry.value < 0 || wrqu->retry.value > 255)
+		return -EINVAL;
+
+	dw_lock(priv, flags);
+	if (wrqu->retry.flags & IW_RETRY_MIN)
+		priv->short_retry_limit = (u8) wrqu->retry.value;
+	else if (wrqu->retry.flags & IW_RETRY_MAX)
+		priv->long_retry_limit = (u8) wrqu->retry.value;
+	else {
+		priv->short_retry_limit = (u8) wrqu->retry.value;
+		priv->long_retry_limit = (u8) wrqu->retry.value;
+	}
+
+	dw_unlock(priv, flags);
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: dw_wx_get_retry
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_wx_get_retry(struct net_device *dev, struct iw_request_info *info,
+		union iwreq_data *wrqu, char *extra)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+	wrqu->retry.disabled = 0;
+
+	if ((wrqu->retry.flags & IW_RETRY_TYPE) == IW_RETRY_LIFETIME) {
+		dw_unlock(priv, flags);
+		return -EINVAL;
+	}
+
+	if (wrqu->retry.flags & IW_RETRY_MAX) {
+		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
+		wrqu->retry.value = priv->long_retry_limit;
+	} else if (wrqu->retry.flags & IW_RETRY_MIN) {
+		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MIN;
+		wrqu->retry.value = priv->short_retry_limit;
+	} else {
+		wrqu->retry.flags = IW_RETRY_LIMIT;
+		wrqu->retry.value = priv->short_retry_limit;
+	}
+	dw_unlock(priv, flags);
+
+	return 0;
+}
+
+#ifdef WX
+# undef WX
+#endif
+#define WX(ioctl)  [(ioctl) - SIOCSIWCOMMIT]
+static const iw_handler dw_wx_handlers[] = {
+	/* Wireless Identification */
+	WX(SIOCGIWNAME)		= dw_get_name,
+	/* Basic operations */
+	WX(SIOCSIWFREQ)		= dw_wx_set_channelfreq,
+	WX(SIOCGIWFREQ)		= dw_wx_get_channelfreq,
+	WX(SIOCSIWMODE)		= dw_wx_set_mode,
+	WX(SIOCGIWMODE)		= dw_wx_get_mode,
+	/* Informative stuff */
+	WX(SIOCGIWRANGE)	= dw_wx_get_rangeparams,
+	/* Spy support (statistics per MAC address - used for Mobile IP support) */
+	WX(SIOCSIWSPY)		= iw_handler_set_spy,
+	WX(SIOCGIWSPY)		= iw_handler_get_spy,
+	WX(SIOCSIWTHRSPY)	= iw_handler_set_thrspy,
+	WX(SIOCGIWTHRSPY)	= iw_handler_get_thrspy,
+	/* Access Point manipulation */
+	WX(SIOCSIWAP)		= ieee80211softmac_wx_set_wap,
+	WX(SIOCGIWAP)		= ieee80211softmac_wx_get_wap,
+	WX(SIOCSIWSCAN)		= ieee80211softmac_wx_trigger_scan,
+	WX(SIOCGIWSCAN)		= ieee80211softmac_wx_get_scan_results,
+	/* 802.11 specific support */
+	WX(SIOCSIWESSID)	= ieee80211softmac_wx_set_essid,
+	WX(SIOCGIWESSID)	= ieee80211softmac_wx_get_essid,
+	WX(SIOCSIWNICKN)	= dw_wx_set_nick,
+	WX(SIOCGIWNICKN)	= dw_wx_get_nick,
+	/* Other parameters */
+	WX(SIOCSIWRATE)		= dw_wx_set_rate,
+	WX(SIOCGIWRATE)		= ieee80211softmac_wx_get_rate,
+	WX(SIOCSIWRTS)		= dw_wx_set_rts,
+	WX(SIOCGIWRTS)		= dw_wx_get_rts,
+	WX(SIOCSIWFRAG)		= dw_wx_set_frag,
+	WX(SIOCGIWFRAG)		= dw_wx_get_frag,
+	WX(SIOCSIWTXPOW)	= dw_wx_set_xmitpower,
+	WX(SIOCGIWTXPOW)	= dw_wx_get_xmitpower,
+	WX(SIOCSIWRETRY)	= dw_wx_set_retry,
+	WX(SIOCGIWRETRY)	= dw_wx_get_retry,
+	/* Encoding */
+	WX(SIOCSIWENCODE)	= dw_wx_set_encode,
+	WX(SIOCGIWENCODE)	= dw_wx_get_encode,
+	WX(SIOCSIWENCODEEXT)	= dw_wx_set_encodeext,
+	WX(SIOCGIWENCODEEXT)	= dw_wx_get_encodeext,
+	/* Power saving */
+/*TODO	WX(SIOCSIWPOWER)	= dw_wx_set_power, */
+/*TODO	WX(SIOCGIWPOWER)	= dw_wx_get_power, */
+	WX(SIOCSIWGENIE)	= ieee80211softmac_wx_set_genie,
+	WX(SIOCGIWGENIE)	= ieee80211softmac_wx_get_genie,
+	WX(SIOCSIWMLME)		= ieee80211softmac_wx_set_mlme,
+	WX(SIOCSIWAUTH)		= ieee80211_wx_set_auth,
+	WX(SIOCGIWAUTH)		= ieee80211_wx_get_auth,
+};
+#undef WX
+
+#ifdef NEED_PRIVATE_HANDLER
+# include "dw_priv_handler.c"
+#else
+static const iw_handler dw_priv_wx_handlers[] = {};
+static const struct iw_priv_args dw_priv_wx_args[] = {};
+#endif
+
+const struct iw_handler_def dw_wx_handlers_def = {
+	.standard		= dw_wx_handlers,
+	.num_standard		= ARRAY_SIZE(dw_wx_handlers),
+	.num_private		= ARRAY_SIZE(dw_priv_wx_handlers),
+	.num_private_args	= ARRAY_SIZE(dw_priv_wx_args),
+	.private		= dw_priv_wx_handlers,
+	.private_args		= dw_priv_wx_args,
+	.get_wireless_stats	= dw_get_wireless_stats,
+};
+
+/***********************************************************************
+ * @Function: dw_close
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int dw_close(struct net_device* dev)
+{
+	struct dw_priv* priv = ieee80211softmac_priv(dev);
+	unsigned long   flags;
+	struct list_head*  it;
+
+	DBG_FN(DBG_INIT);
+
+	ieee80211softmac_stop(dev);
+
+	/* don't disable anything before softmac_stop. Maybe SoftMAC will run a
+	 * disassoc cycle somewhat later. */
+	del_timer_sync(&priv->management_timer);
+
+	cancel_delayed_work(&priv->beacon_work);
+
+	disable_irq(dev->irq);
+
+	tasklet_disable(&priv->rx.tasklet);
+	/* from this point, rx queue.filled is empty, and it is not paused */
+	tasklet_disable(&priv->tx.tasklet);
+
+	dw_lock(priv, flags);
+
+	priv->tx.data_pending_ack = NULL;
+
+	/* delete unsend frames */
+	list_for_each(it, &priv->tx.queued) {
+		dw_frame_tx_t* frame = list_entry(it, dw_frame_tx_t, list);
+		ieee80211_txb_free(frame->s.txb);
+		frame->s.txb = NULL;
+	}
+	dw_list_move(&priv->tx.free, &priv->tx.queued);
+
+	dw_iocleanbits32(HW_GEN_CONTROL, GEN_RXEN);
+	dw_iowrite32(0, HW_INTR_MASK);
+	dw_iowrite32(dw_ioread32(HW_INTR_STATUS), HW_INTR_STATUS);
+
+	dw_set_led_on(PIN_LED, 0);
+	dw_set_led_on(PIN_ACTIVITY_LED, 0);
+
+	dw_unlock(priv, flags);
+
+	return 0;
+}
+
+/***********************************************************************
+ * @Function: dw_open
+ * @Return:
+ * @Descr: init/reinit
+ ***********************************************************************/
+static int dw_open(struct net_device* dev)
+{
+	struct dw_priv* priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+
+	DBG_FN(DBG_INIT);
+
+	dw_reset_rx_dups(priv);
+
+	priv->tx.data_pending_ack = NULL;
+
+	priv->jiffies_last_beacon = jiffies;
+	priv->reconnection_attempts = 0;
+
+	tasklet_enable(&priv->rx.tasklet);
+	tasklet_enable(&priv->tx.tasklet);
+
+	ieee80211softmac_start(dev);
+
+	ieee80211softmac_set_rates(dev, ARRAY_SIZE(dw_rates), (u8*) dw_rates); /* cast away const */
+
+	/* softmac sets user_rate to 24MB, but we start much slower to work
+	   always */
+	priv->softmac->txrates.default_rate = priv->softmac->txrates.user_rate = IEEE80211_CCK_RATE_1MB;
+
+	dw_lock(priv, flags);
+
+	priv->beacon_ready = 0;
+	INIT_DELAYED_WORK(&priv->beacon_work, dw_beacon_start);
+	priv->beacon_body_length=0;
+	priv->adhoc.channel = DW_IBSS_DEFAULT_CHANNEL;
+
+	/* acknowledge pending interrupts */
+	dw_iowrite32(dw_ioread32(HW_INTR_STATUS), HW_INTR_STATUS);
+	dw_iowrite32(INTR_ALL,       HW_INTR_MASK);
+	dw_iowrite32(HW_AES_MODE_1,  HW_AES_MODE );
+	mod_timer(&priv->management_timer, jiffies + MANAGEMENT_JIFFIES);
+
+	enable_irq(dev->irq);
+
+	dw_iosetbits32(HW_GEN_CONTROL, GEN_RXEN);
+
+	dw_unlock(priv, flags);
+
+	return 0;
+}
+
+/***********************************************************************
+ * @Function: dw_set_multicast_list
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static void dw_set_multicast_list(struct net_device* dev)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+
+	DBG_FN(DBG_INTERFACE | DBG_MINOR);
+
+	dw_lock(priv, flags);
+	if (dev->flags & IFF_PROMISC) {
+		dw_iosetbits32(HW_MAC_CONTROL, CTRL_PROMISC);
+	} else {
+		dw_iocleanbits32(HW_MAC_CONTROL, CTRL_PROMISC);
+	}
+	dw_unlock(priv, flags);
+}
+
+/***********************************************************************
+ * @Function: dw_net_get_stats
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static struct net_device_stats * dw_net_get_stats(struct net_device *dev)
+{
+	struct dw_priv *priv = ieee80211softmac_priv(dev);
+
+	DBG_FN(DBG_INTERFACE);
+
+	return &priv->ieee->stats;
+}
+
+/**
+ * dw_probe - probe for resources and for chip being present
+ *
+ * @return <0 on failure
+ */
+static int __init dw_probe(struct platform_device* pdev)
+{
+	struct net_device* dev = NULL;
+	int err = -ENODEV;
+
+	DBG_FN(DBG_INIT);
+
+	/* Create the network device object. */
+	dev = alloc_ieee80211softmac(sizeof(dw_priv_t));
+	if (NULL == dev) {
+		ERROR(":  Couldn't alloc_ieee80211softmac ");
+		err = -ENOMEM;
+		goto error_alloc;
+	}
+	platform_set_drvdata(pdev, dev);
+	dev->irq  = INTR_ID;
+
+	if (dev_alloc_name(dev, "wlan%d") < 0) {
+		ERROR("Couldn't get name");
+		goto error_name;
+	}
+
+	/* allocate resources and map memory */
+	if (gpio_request(PIN_LED, DRIVER_NAME) != 0) {
+		ERROR("GPIO %i already in use", PIN_LED);
+		goto error_pin_led;
+	}
+	if (gpio_request(PIN_ACTIVITY_LED, DRIVER_NAME) != 0) {
+		ERROR("GPIO %i already in use", PIN_ACTIVITY_LED);
+		goto error_pin_activity_led;
+	}
+	if (gpio_request(PIN_INTR, DRIVER_NAME) != 0) {
+		ERROR("GPIO %i already in use", PIN_INTR);
+		goto error_pin_intr;
+	}
+
+	err = request_resource(&iomem_resource, pdev->resource);
+	if (err) {
+		ERROR("Memory already in used: 0x%08x...0x%08x",
+				pdev->resource->start, pdev->resource->end);
+		goto error_res;
+	}
+
+	vbase = ioremap(pdev->resource->start,
+			pdev->resource->end - pdev->resource->start);
+	if (NULL == vbase) {
+                ERROR("ioremap failed");
+                err = -ENOMEM;
+                goto error_remap;
+        }
+        dev->base_addr = (int) vbase;
+
+        /* test if fpga is programmed */
+        if (dw_ioread32(HW_VERSION) == 0xffffffff) {
+                ERROR("FPGA not present");
+                goto error_fpga;
+        }
+
+        /* all resources available, initializw remaining stuff */
+        dev->open               = dw_open;
+        dev->stop               = dw_close;
+        dev->set_multicast_list = dw_set_multicast_list;
+        dev->wireless_handlers  = &dw_wx_handlers_def;
+        dev->get_stats          = dw_net_get_stats;
+
+        /* initialize hardware/private stuff */
+        err = dw_start_dev(pdev);
+        if (err)
+                goto error;
+
+        return 0;
+
+error_fpga:
+        iounmap(vbase);
+        vbase = NULL;
+error_remap:
+        release_resource(pdev->resource);
+error_res:
+        gpio_free(PIN_INTR);
+error_pin_intr:
+        gpio_free(PIN_ACTIVITY_LED);
+error_pin_activity_led:
+        gpio_free(PIN_LED);
+error_pin_led:
+error_name:
+        /* the IRQ hasn't been setup, so unwind a part and don't go to
+         * dw_remove */
+        free_ieee80211softmac(dev);
+error_alloc:
+        return err;
+
+error:
+        /* dev memory is setup, so we can use the remove function */
+        dw_remove(pdev);
+
+        return err;
+}
+
+/***********************************************************************
+ * @Function: dw_init_module
+ * @Return: 0 if successfull; -ENODEV if interrupt or device is not available
+ * @Descr: initialize module
+ ***********************************************************************/
+static int __init dw_init_module(void)
+{
+	int err;
+
+        DBG_FN(DBG_INIT);
+
+	printk(KERN_INFO "%s\n", dw_version);
+
+        if (dw_sw_aes)
+                printk(KERN_NOTICE "AES encoding/decoding is done in software\n");
+
+        err = platform_device_register(&dw_device);
+        if (err) {
+                ERROR("Device Register Failed");
+                goto error;
+        }
+
+        err = platform_driver_register(&dw_driver);
+        if (err) {
+                ERROR("Driver Register Failed");
+                goto error;
+        }
+
+        return 0;
+
+error:
+        return err;
+}
+
+
+/***********************************************************************
+ * @Function: dw_cleanup_module
+ * @Return: nothing
+ * @Descr: deinitializes module. Actually will never be called due to
+ * MOD_INC_USE_COUNT > 1. Will be fixed if hardware watchdog can be disabled.
+ ***********************************************************************/
+static void __exit dw_cleanup_module(void)
+{
+        DBG_FN(DBG_INIT);
+
+	platform_driver_unregister(&dw_driver);
+	platform_device_unregister(&dw_device);
+}
+
+#ifndef MODULE
+/**
+ * dw_sw_cmdline - parses kernel commandline
+ *
+ * @return always 1
+ */
+static int __init dw_sw_cmdline(char* arg)
+{
+        if ((NULL != arg) && *arg)
+                dw_sw_aes = simple_strtoul(arg, NULL, 10);
+
+        return 1;
+}
+#endif  /* MODULE */
+
+module_init(dw_init_module);
+module_exit(dw_cleanup_module);
+
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Bernd Westermann");
+MODULE_AUTHOR("Markus Pietrek");
+MODULE_AUTHOR("Miriam Ruiz");
+MODULE_DESCRIPTION("WiFi driver for ConnectCore Wi-9C");
+
+module_param(dw_sw_aes, int, 0x00);
+MODULE_PARM_DESC(dw_sw_aes, "If set, AES encoding/decoding is done in software.");
+
+module_param(dw_cfg_vco, int, 0x00);
+MODULE_PARM_DESC(dw_cfg_vco, "Config Value of VCO (0 use built-in)");
+
+#ifdef CONFIG_DIGI_WI_G_DEBUG
+module_param(dw_dbg_level, int, 0x00);
+MODULE_PARM_DESC(dw_dbg_level, "debug output level");
+#endif /* CONFIG_DIGI_WI_G_DEBUG */
+
+#ifndef MODULE
+__setup("dw_sw_aes=", dw_sw_cmdline);
+#endif /* MODULE */
diff --git a/drivers/net/wireless/digi_wi_g.h b/drivers/net/wireless/digi_wi_g.h
new file mode 100644
index 000000000000..95bc6f06b1a5
--- /dev/null
+++ b/drivers/net/wireless/digi_wi_g.h
@@ -0,0 +1,660 @@
+/*******************************************************************************
+ * digi_wi_g.h
+ *
+ * Support for DIGI Wireless Module.
+ *
+ * $Id: digi_wi_g.h,v 1.64 2008-02-13 11:02:34 mruiz Exp $
+ * @Author: Bernd Westermann
+ * @References: [1] NET+OS code mac_hw_wi9c.c
+ *
+ * Copyright (C) 2007 by FS Forth-Systeme GmbH
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ *
+ *******************************************************************************
+ *  History:
+ *     14/03/06	Initial Version
+ *
+ *******************************************************************************/
+
+#ifndef FS_DIGI_WI_G_DRIVER_H
+#define FS_DIGI_WI_G_DRIVER_H
+
+#include <net/ieee80211.h>
+#include <net/ieee80211softmac.h>
+#include <net/ieee80211softmac_wx.h>
+
+#include <net/iw_handler.h>     /* New driver API */
+#include <mach/regs-bbu.h>
+#include <mach/hardware.h>	/* BBUS_CLK_FREQ */
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+
+#ifdef dprintkl
+#undef dprintkl
+#endif
+#ifdef dprintk
+#undef dprintk
+#endif
+
+
+#define DRIVER_MAJOR 	0
+#define DRIVER_MINOR 	60
+#define DRIVER_NAME  	"digi_wi_g"
+
+/* for LEDs, update rate etc. */
+/* LED time is 50ms */
+#define MANAGEMENT_TIME_MS       50
+#define	MANAGEMENT_JIFFIES	 ( ( MANAGEMENT_TIME_MS * HZ ) / 1000 )
+/* update rate only every 500ms */
+#define MANAGEMENT_TICKS_FOR_UPDATE ( 500 / MANAGEMENT_TIME_MS )
+#define TX_TIMEOUT		(4*HZ/2)
+#define ACK_TIMEOUT		(HZ/2)
+#define BEACON_TIMEOUT	(HZ)
+#define RESCAN_TIMEOUT	(15*HZ)
+
+#define MAX_RECONNECTION_ATTEMPTS 5
+
+/* Min/max successful intervals to increase rate */
+#define	THRESH_MIN		1
+#define	THRESH_MAX		10
+
+#define TX_QUEUE_SIZE            8  /* not tuned yet */
+#define TX_MAX_FRAGS            16
+#define RX_QUEUE_SIZE           44  /* one ICMP frame */
+
+#define RX_DUP_SENDER_SIZE      16  /* somewhat lower than MAX_NETWORK_COUNT
+                                     * because in worst case we need to scan
+                                     * RX_DUP_SENDER_SIZE until we found the
+                                     * active one */
+
+#define DW_PIO_MAXTXPACKETS      4
+
+#define DW_DEFAULT_SHORT_RETRY_LIMIT	7
+#define DW_DEFAULT_LONG_RETRY_LIMIT	4
+#define AES_BUSY_TIMEOUT	1000000 /* in ns */
+
+#define printkl(f, x...)  do { if (printk_ratelimit()) printk(f ,##x); } while (0)
+
+#ifdef CONFIG_DIGI_WI_G_DEBUG
+# define assert(expr) \
+	do {	\
+		if (unlikely(!(expr))) {	\
+		printk(KERN_ERR PFX "ASSERTION FAILED (%s) \nat: %s:%d:%s()\n",	\
+			#expr, __FILE__, __LINE__, __FUNCTION__);	\
+		}						\
+	} while (0)
+# define dprintkl		printkl
+# define dprintk(f, x...)  do { printk(f ,##x); } while (0)
+# define COMPILE_TIME " from ("__TIME__ ") "
+#else
+# define assert(expr)	do { /* nothing */ } while (0)
+# define dprintkl(f, x...)	do { /* nothing */ } while (0)
+# define dprintk(f, x...)  do { /* nothing */ } while (0)
+# define COMPILE_TIME ""
+#endif  /* CONFIG_DIGI_WI_G_DEBUG */
+
+#define	DIGI_WIRELESS_G_REG_PHYS	NS9XXX_CSxSTAT_PHYS(2)
+
+/* Hardware register defines */
+#define	MAC_BASE_PHYS	DIGI_WIRELESS_G_REG_PHYS
+#define MAC_BASE_SIZE	0xe0000/*100U*/			/* Register segment size */
+#define MAC_MASK	0xffffc001			/* Size mask and enable bit */
+
+/* Baseband control registers */
+#define	HW_VERSION		(0x00)	/* Version */
+#define	HW_GEN_CONTROL		(0x04)	/* General control */
+#define	HW_GEN_STATUS		(0x08)	/* General status */
+#define	HW_RSSI_AES		(0x0c)	/* RSSI and AES status */
+#define	HW_INTR_MASK		(0x10)	/* Interrupt mask */
+#define	HW_INTR_STATUS		(0x14)	/* Interrupt status */
+#define	HW_SPI_DATA		(0x18)	/* RF SPI data register */
+#define	HW_SPI_CONTROL		(0x1c)	/* RF SPI control register */
+#define	HW_DATA_FIFO		(0x20)	/* Data FIFO */
+#define	HW_AES_FIFO		(0x30)	/* AES FIFO */
+#define	HW_AES_MODE		(0x38)	/* AES mode */
+
+/* MAC control registers */
+#define	HW_STAID0		(0x40)	/* Station ID (6 bytes) */
+#define	HW_STAID1		(0x44)
+#define	HW_BSSID0		(0x48)	/* BSS ID (6 bytes) */
+#define	HW_BSSID1		(0x4c)
+#define	HW_SSID_LEN		(0x50)	/* SSID length (8 bits) */
+#define	HW_BACKOFF		(0x54)	/* Backoff period (16 bits) */
+#define	HW_LISTEN		(0x58)	/* Listen interval (16 bits), CFP (8 bits), DTIM (8 bits) */
+#define	HW_CFP_ATIM		(0x5c)	/* CFP max duration/ATIM period (16 bits), beacon interval (16 bits) */
+#define	HW_MAC_STATUS		(0x60)	/* MAC status (8 bits) */
+#define	HW_MAC_CONTROL		(0x64)	/* MAC control (8 bits) */
+#define	HW_REMAIN_BO		(0x68)	/* Remaining backoff (16 bits) */
+#define	HW_BEACON_BO		(0x6c)	/* Beacon backoff (16 bits), beacon mask (8 bits) */
+#define	HW_SSID			(0x80)	/* Service set ID (32 bytes) */
+
+/* FIFO sizes in bytes */
+#define	HW_TX_FIFO_SIZE	1792
+#define	HW_RX_FIFO_SIZE	2048
+
+
+/* General control register bits */
+#define	GEN_RXEN	0x00000001	/* Receive enable */
+#define	GEN_ANTDIV	0x00000002	/* Antenna diversity */
+#define	GEN_ANTSEL	0x00000004	/* Antenna select */
+#define	GEN_5GEN	0x00000008	/* 5 GHz band enable */
+#define	GEN_SHPRE	0x00000010	/* Transmit short preamble */
+#define	GEN_RXFIFORST	0x00000020	/* Receive FIFO reset */
+#define	GEN_TXFIFORST	0x00000040	/* Transmit FIFO reset */
+#define	GEN_TXHOLD	0x00000080	/* Transmit FIFO hold */
+#define	GEN_BEACEN	0x00000100	/* Beacon enable */
+#define	GEN_BST		0x00001000	/* Boot status */
+#define	GEN_CLKEN	0x00002000	/* Clock enable */
+#define	GEN_TXFIFOEMPTY	0x00004000	/* Transmit FIFO empty */
+#define	GEN_TXFIFOFULL	0x00008000	/* Transmit FIFO full */
+
+#if defined(CONFIG_DIGI_WI_G_UBEC_JD)
+# define GEN_INIT	0x377a0000	/* Initial state */
+#elif defined(CONFIG_DIGI_WI_G_UBEC_HC)
+# define GEN_INIT	0x37700000	/* Initial state */
+#else
+# error "You need to choose an UBEC Transceiver Revision"
+#endif
+
+/* General status register bits */
+#define	STAT_RXFE	0x00000010	/* Receive FIFO empty */
+#define SFT_RESET       0x04000000      /* WiFi Baseband soft reset */
+/* AES status register bits */
+#define	AES_EMPTY	0x00010000	/* AES receive FIFO empty */
+#define	AES_FULL	0x00020000	/* AES transmit FIFO full */
+#define	AES_BUSY	0x00040000	/* AES engine busy */
+#define	AES_MIC		0x00080000	/* AES MIC correct */
+
+/* Interrupt mask and status register bits */
+#define INTR_RXFIFO	0x00000001	/* Receive FIFO not empty */
+#define INTR_TXEND	0x00000002	/* Transmit complete */
+#define INTR_TIMEOUT	0x00000004	/* CTS/ACK receive timeout */
+#define INTR_ABORT	0x00000008	/* CTS transmit abort */
+#define INTR_TBTT	0x00000010	/* Beacon transmission time */
+#define INTR_ATIM	0x00000020	/* ATIM interval end */
+#define INTR_RXOVERRUN	0x00000040	/* Receive FIFO overrun */
+#define INTR_ALL	( INTR_RXFIFO    | \
+                          INTR_TIMEOUT   | \
+                          INTR_RXOVERRUN | \
+                          INTR_TXEND )
+
+/* MAC control register bits */
+#define	CTRL_TXREQ	0x00000001	/* Transmit request */
+#define	CTRL_AUTOTXDIS	0x00000002	/* Auto-transmit disable */
+#define	CTRL_BEACONTX	0x00000004	/* Beacon transmit enable */
+#define	CTRL_PROMISC	0x00000008	/* Promiscuous mode */
+#define	CTRL_IBSS	0x00000010	/* IBBS mode */
+
+#define HW_AES_MODE_0   0x00000000
+#define HW_AES_MODE_1   0x00000010
+
+/* BOOTMUX bit */
+#define	BOOTMUX_LOW	0x00000001	/* Bootmux,bit 0, must go low after load */
+
+/* PIO pins */
+#define PIN_INIT		58		/* FPGA program init */
+#define PIN_INTR		65		/* Interrupt (same as done) */
+#define PIN_LED			67		/* Link status LED */
+#define PIN_ACTIVITY_LED	66		/* Link activity LED */
+#define PIN_BOOTMUX		66		/* Enables serial ports */
+
+#define INTR_ID		IRQ_NS9XXX_ETHPHY	/* Interrupt ID for PIN_INTR */
+
+/* Get/set/clear a PIO pin */
+#define PIO_SET(pin)	ns9xxx_gpio_setpin(pin, 1)
+#define PIO_CLR(pin)	ns9xxx_gpio_setpin(pin, 0)
+
+#define	CW_MIN		31	/* Min contention window size */
+#define	CW_MAX		1023	/* Max contention window size */
+
+/* Maximum rxsignal strength we expect to see (this is in dB) */
+#define MAC_RSSI_MAX 0x4f
+
+/* the CCK/OFDM base is 2 * Mb/s */
+#define RATE_IN_BS(x) ((x) * 500000)
+
+/* see email from Mike Schaffner from 16.03.07 (RE: Verification of
+ * initialization to mpietrek and hbujanda */
+#define VCO_DEFAULT_CHANNEL_12 	0x7160
+#define VCO_DEFAULT		0x7020
+
+#define RATES_SUPPORTED		12
+
+#define AES_MAXNR 14
+
+#define CCMP_KEY_SIZE	16			/* CCMP key size */
+#define AES_BLOCK_SIZE	16
+#define	EXTIV_SIZE	8			/* IV and extended IV size */
+#define	MIC_SIZE	8			/* Message integrity check size */
+#define	CCMP_SIZE	(EXTIV_SIZE+MIC_SIZE)	/* Total CCMP size */
+#define FCS_SIZE 4 // FCS (CRC-32) size
+#define	DATA_SIZE	28			/* Data frame header+FCS size */
+/* Key ID byte in data frame body */
+#define	EXT_IV		0x20	/* Extended IV is present */
+
+#define DW_TX_POWER_MIN_DBM 0
+#define DW_TX_POWER_MAX_DBM 15
+#define DW_TX_POWER_DEFAULT 10
+
+typedef u64 u48;
+
+struct aes_key_st {
+    unsigned long rd_key[4 *(AES_MAXNR + 1)];
+};
+
+typedef union {
+        struct {
+                u8	signal;		/* (rate in 100 kbps) */
+                u8	service;	/* Service: OR of SERVICE_xxx */
+                u16	length;		/* Length in usecs (needs byte swap) */
+        };
+        u8  raw[4];
+        u32 raw32;
+}  __attribute__((__packed__)) dw_hw_pskcck_t;
+
+typedef union {
+        struct {
+                unsigned rate		:4;	/* Data rate */
+                unsigned reserved	:1;
+                unsigned length		:12;	/* Length in bytes */
+                unsigned parity		:1;	/* Even parity bit */
+                unsigned _pad   	:14;	/* Service field */
+        };
+        u8  raw[4];
+        u32 raw32;
+} __attribute__((__packed__)) dw_hw_ofdm_t;
+
+/* modulation header (rx ) */
+typedef union {
+        struct {
+                unsigned mod_type : 8;
+                unsigned rssi_vga : 5;
+                unsigned rssi_lna : 2;
+                unsigned ant	  : 1;
+                unsigned freq_off : 16;	/* Frequency offset (needs byte swap) */
+        };
+        u32 raw32;
+} __attribute__((__packed__)) dw_hw_mod_rx_t;
+
+/* modulation header ( tx ) */
+typedef union {
+        struct {
+                unsigned int mod_type :8;
+                unsigned int length   :9;
+                unsigned int pad      :15;
+        };
+        u32 raw32;
+} __attribute__((__packed__)) dw_hw_mod_tx_t;
+
+typedef union {
+        dw_hw_pskcck_t pskcck;
+        dw_hw_ofdm_t   ofdm;
+} __attribute__((__packed__)) dw_hw_plcp_t;
+
+typedef struct {
+        dw_hw_mod_tx_t mod;
+        dw_hw_plcp_t   plcp;
+} __attribute__((__packed__)) dw_hw_hdr_tx_t;
+
+typedef struct {
+        dw_hw_mod_rx_t mod;
+        dw_hw_plcp_t   plcp;
+} __attribute__((__packed__)) dw_hw_hdr_rx_t;
+
+/* CCMP key data */
+
+typedef struct {
+	u8	init[AES_BLOCK_SIZE];
+	u8	header[2*AES_BLOCK_SIZE];
+} __attribute__ ((__packed__)) ccmp_data_t;
+
+typedef struct {
+	u8	valid;		/* TRUE if key is valid */
+        u48     tx_pn;          /* transmit packet number */
+        u48     rx_pn;          /* next receive packet number */
+	struct aes_key_st	rk;	/* AES key schedule */
+} ccmp_key_t;
+
+/* stores the necessary data to detect duplicate frames */
+typedef struct {
+        struct list_head list;
+        u8  		src[ ETH_ALEN ];  /* addr2/sender  */
+        u16 		seq_ctl;  /* seq_ctl of last received frame */
+} dw_duplicate_t;
+
+typedef struct {
+        struct list_head list;
+        struct sk_buff*  skb;  /* message data */
+        dw_hw_hdr_rx_t   hdr;
+} dw_frame_rx_t;
+
+typedef struct {
+        dw_frame_rx_t  free;
+        dw_frame_rx_t  filled;
+        dw_frame_rx_t* buffer;
+} dw_frame_rx_queue_t;
+
+typedef struct {
+        dw_hw_hdr_tx_t hdr;
+
+        /* physical length of data, including CCMP. Is not included skb->len if
+         * hardware encryption is performed. */
+        size_t phys_len;
+
+        struct {
+                ccmp_key_t* key;
+                int         key_index;
+        } crypt;
+} dw_fragment_tx_t;
+
+typedef struct {
+        struct ieee80211_txb* txb;
+        dw_fragment_tx_t      frags[ TX_MAX_FRAGS ];
+
+        u8 is_data            : 1;
+        u8 use_hw_encryption  : 1;
+        u8 use_short_preamble : 1;
+} dw_frame_tx_info_t;
+
+typedef struct {
+        struct list_head   list;
+        dw_frame_tx_info_t s;
+} dw_frame_tx_t;
+
+// IBSS
+
+#define	DW_BEACON_INT		100		// IBSS Beacon interval (in TU)
+
+//
+// 802.11 MIB constants
+//
+#define	DW_SHORT_RETRY_LIMIT	7		// Small frame transmit retry limit
+#define	DW_LONG_RETRY_LIMIT	4		// Large frame transmit retry limit
+
+#define	DW_TU				1024L/1000	// Time unit (in msecs)
+#define	DW_MAX_TX_LIFETIME		(512*TU)	// Transmit lifetime limit (in msecs)
+#define	DW_MAX_RX_LIFETIME		(512*TU)	// Receive lifetime limit (in msecs)
+
+// Max number of fragments
+#define	DW_MAX_FRAGS			16
+
+// Frame header modulation type field
+#define	DW_MOD_PSKCCK		0x00	// PSK/CCK modulation
+#define	DW_MOD_OFDM			0xee	// OFDM modulation
+
+// PSK/CCK PLCP service field bits
+#define	DW_SERVICE_LOCKED		0x04	// Locked clocks
+#define	DW_SERVICE_MODSEL		0x08	// Modulation selection
+#define	DW_SERVICE_LENEXT		0x80	// Length extension
+
+// MAC type field values
+#define	DW_TYPE_ASSOC_REQ	0x00	// Association request
+#define	DW_TYPE_ASSOC_RESP	0x10	// Association response
+#define	DW_TYPE_REASSOC_REQ	0x20	// Reassociation request
+#define	DW_TYPE_REASSOC_RESP	0x30	// Reassociation response
+#define	DW_TYPE_PROBE_REQ	0x40	// Probe request
+#define	DW_TYPE_PROBE_RESP	0x50	// Probe response
+
+#define	DW_TYPE_BEACON		0x80	// Beacon
+#define	DW_TYPE_ATIM			0x90	// Annoucement traffice indication
+#define	DW_TYPE_DISASSOC		0xa0	// Disassociation
+#define	DW_TYPE_AUTH			0xb0	// Authentication
+#define	DW_TYPE_DEAUTH		0xc0	// Deauthentication
+
+#define	DW_TYPE_RTS			0xb4	// Request to send
+#define	DW_TYPE_CTS			0xc4	// Clear to send
+#define	DW_TYPE_ACK			0xd4	// Acknowledgement
+#define	DW_TYPE_PSPOLL		0xa4	// Power Save(PS)-Poll
+
+#define	DW_TYPE_DATA			0x08	// Data
+
+// TRUE if buf is data or management frame
+#define	DW_IS_DATA(buf)			(((buf)->macHdr.fc.type & 0xcf) == TYPE_DATA)
+#define	DW_IS_MGMT(buf)		(((buf)->macHdr.fc.type & 0x0f) == 0)
+
+// MAC address macros
+#define	DW_MAC_GROUP			0x01	// Broadcast or multicast address
+#define	DW_MAC_LOCAL			0x02	// Locally administered address
+
+#define	DW_EQUAL_ADDR(a1, a2)	(memcmp (a1, a2, ETH_ALEN) == 0)
+#define	DW_SET_ADDR(a1, a2)		(memcpy (a1, a2, ETH_ALEN))
+
+// Authentication algorithm number field values
+#define	DW_AUTH_OPEN			0x00	// Open system
+#define	DW_AUTH_SHAREDKEY		0x01	// Shared key
+#define	DW_AUTH_LEAP			0x80	// LEAP
+
+// Capability information field bits
+#define	DW_CAP_ESS			0x0001	// Extended service set (infrastructure)
+#define	DW_CAP_IBSS			0x0002	// Independent BSS (ad hoc)
+#define	DW_CAP_POLLABLE		0x0004	// Contention free pollable
+#define	DW_CAP_POLLREQ		0x0008	// Contention free poll request
+#define	DW_CAP_PRIVACY		0x0010	// Privacy (WEP) required
+#define	DW_CAP_SHORTPRE		0x0020	// Short preambles allowed
+#define	DW_CAP_PBCC			0x0040	// PBCC modulation allowed
+#define	DW_CAP_AGILITY			0x0080	// Channel agility in use
+#define	DW_CAP_SHORTSLOT		0x0400	// Short slot time in use
+#define	DW_CAP_DSSSOFDM		0x2000	// DSSS-OFDM in use
+
+// Status code field values
+#define	DW_STAT_SUCCESS		0
+
+// Reason code field values
+#define	DW_REAS_NOLONGERVALID	2
+#define	DW_REAS_DEAUTH_LEAVING	3
+#define	DW_REAS_INACTIVITY		4
+#define	DW_REAS_INCORRECT_FRAME_UNAUTH 6
+#define	DW_REAS_INCORRECT_FRAME_UNASSO 7
+
+// Information element IDs
+#define	DW_ELEM_SSID			0		// Service set ID
+#define	DW_ELEM_SUPRATES		1		// Supported rates
+#define	DW_ELEM_DSPARAM		3		// DS parameter set
+#define	DW_ELEM_IBSSPARAM		6		// IBSS parameter set
+#define	DW_ELEM_COUNTRY      	7		// Country information
+#define	DW_ELEM_CHALLENGE		16		// Challenge text
+#define	DW_ELEM_ERPINFO		42		// Extended rate PHY info
+#define	DW_ELEM_RSN			48		// Robust security network (WPA2)
+#define	DW_ELEM_EXTSUPRATES	50		// Extended supported rates
+#define	DW_ELEM_VENDOR		221		// Vendor extension (WPA)
+
+// 802.11d related defines
+// minimum length field value in country information elelment
+#define	DW_COUNTRY_INFO_MIN_LEN   6
+
+// Supported rates bits
+#define	DW_RATE_BASIC			0x80	// Bit set if basic rate
+
+// TRUE if channel number in 5 GHz band
+#define	DW_CHAN_5G(chan)		((chan) > 14)
+
+// ERP info bits
+#define	DW_ERP_NONERP			0x01	// Non-ERP present
+#define	DW_ERP_USEPROTECT		0x02	// Use protection
+#define	DW_ERP_BARKER			0x04	// Barker (long) preamble mode
+
+// Key ID byte in data frame body
+#define	DW_EXT_IV				0x20	// Extended IV is present
+
+// Correct CRC-32 check value
+#define	DW_GOOD_CRC32			0x2144df1c
+
+#pragma pack()
+
+#define BEACON_BODY_SIZE 64
+
+// MAC buffer, including complete MAC frame
+typedef struct {
+	dw_hw_hdr_tx_t hwHdr; // Frame and PLCP headers
+	u16 fc; // Frame control
+	u16 duration; // Duration/ID (needs byte swap)
+	u8 addr1[ ETH_ALEN ]; // Address 1
+	u8 addr2[ ETH_ALEN ]; // Address 2
+	u8 addr3[ ETH_ALEN ]; // Address 3
+	u16 seq_ctl; // Sequence control fields
+	u8 body[BEACON_BODY_SIZE];
+} __attribute__ ((packed)) dw_beacon_frame;
+
+// Length (in usecs) of a MAC frame of bytes at rate (in 500kbps units)
+// not including SIFS and PLCP preamble/header
+#define	DW_LENGTH_uS(bytes, rate)		((16*(bytes)+(rate)-1)/(rate))
+
+// Length (in usecs) of SIFS and PLCP preamble/header.
+#define	DW_PRE_LEN_uS(rate)			(USE_SHORTPRE(rate) ? 106 : 202)
+
+// Duration (in usecs) of an OFDM frame at rate (in 500kbps units)
+// including SIFS and PLCP preamble/header
+#define	DW_OFDM_DUR(bytes, rate)	(36 + 4*((4*(bytes)+(rate)+10)/(rate)))
+
+// Information on each supported rate
+typedef struct {
+	u8	bps; // Bit rate in 500kbps units
+	u8	ofdmCode; // OFDM rate code, 0 if not OFDM
+	u16	ackLen; // Duration of ACK or CTS in usecs
+} RateInfo;
+
+#define DW_IBSS_DEFAULT_CHANNEL 6
+
+// End of IBSS
+
+typedef struct dw_priv {
+	struct net_device*              dev;
+	struct ieee80211_device*        ieee;
+	struct ieee80211softmac_device* softmac;
+
+	struct iw_statistics wstats;
+
+	spinlock_t lock;
+
+	struct timer_list management_timer;
+
+	/* Additional information, specific to the 80211 cores. */
+	/* Driver status flags. */
+	u32 recovery:1;	/* TRUE if interval follows rate increase */
+	/* Interrupt Service Routine tasklet (bottom-half) */
+	u8 channel;
+	/* Informational stuff. */
+	char nick[IW_ESSID_MAX_SIZE + 1];
+	int short_retry_limit;
+	int long_retry_limit;
+	ccmp_key_t aeskeys[WEP_KEYS];
+	int cw;		/* Contention window size */
+	int success;		/* Successful intervals */
+	int success_threshold;	/* Successful intervals needed to increase rate */
+        atomic_t fix_rate;
+
+        struct ieee80211softmac_ratesinfo ap_ri;
+
+        struct {
+                int counter;
+        } activity;
+
+        struct {
+                int index;
+                int counter;
+                int recovery;
+                int tx_data_any;
+                int tx_data_ack;
+                int have_activity;
+                int success;
+                int success_threshold;
+        } rate;
+
+        struct {
+                struct tasklet_struct tasklet;
+                dw_frame_rx_queue_t   queue;
+                char                  pause;
+
+                /* maintains the list of received frames */
+                struct {
+                        dw_duplicate_t known;
+                        dw_duplicate_t free;
+                        dw_duplicate_t entries[ RX_DUP_SENDER_SIZE ];
+                } dups;
+        } rx;
+
+        struct {
+                struct tasklet_struct tasklet;
+                dw_frame_tx_t         frames[ TX_QUEUE_SIZE ];
+                struct list_head      queued;
+                struct list_head      free;
+                atomic_t              seq_nr;
+                int                   timeout;
+                int                   times_sent;
+                int                   retries;
+                int                   fragment;
+                char                  pending;
+                char                  pause;
+                struct semaphore      pause_sem;
+                char                  last_was_data;
+                int                   basics[ RATES_SUPPORTED ];
+                void *                data_pending_ack;
+                unsigned long         jiffies_pending_ack;
+        } tx;
+
+	unsigned char tx_power;
+
+	int beacon_ready;
+	struct delayed_work beacon_work;
+	int beacon_body_length;
+	dw_beacon_frame beacon_frame;
+	struct ieee80211softmac_network adhoc;
+
+	unsigned long jiffies_last_beacon;
+	int reconnection_attempts;
+}  dw_priv_t;
+
+/* dw_(un)lock() protect struct dw_private.
+ */
+#define dw_lock(priv, flags)	spin_lock_irqsave(&(priv)->lock, flags)
+#define dw_unlock(priv, flags)	spin_unlock_irqrestore(&(priv)->lock, flags)
+
+/* Frame header modulation type field */
+#define	MOD_PSKCCK	0x00	/* PSK/CCK modulation */
+#define	MOD_OFDM	0xee	/* OFDM modulation */
+
+/* PLCP service field bits */
+#define SERVICE_LOCKED	0x04    /* Locked clocks */
+#define SERVICE_MODSEL	0x08    /* Modulation selection */
+#define SERVICE_LENEXT	0x80    /* Length extension */
+
+/* Threshold values. */
+#define DW_MIN_RTS_THRESHOLD		1U
+#define DW_MAX_RTS_THRESHOLD		2304U
+/* 1536 is default for most routers, but our FIFO is larger, so we could accept
+ * more data, e.g. for improvements when doing AES etc. This has been reported
+ * being done with Apple iTunes */
+#define DW_MTU				2048
+
+#define	AES_BITS	128	/* 128 bit keys, 10 rounds */
+
+# define GETU32(pt) (((u32)(pt)[0] << 24) ^ ((u32)(pt)[1] << 16) ^ ((u32)(pt)[2] <<  8) ^ ((u32)(pt)[3]))
+/* Get 16 bits at byte pointer */
+#define	GET16(bp)		((bp)[0] | ((bp)[1] << 8))
+/* Get 32 bits at byte pointer */
+#define	GET32(bp)		((bp)[0] | ((bp)[1] << 8) | ((bp)[2] << 16) | ((bp)[3] << 24))
+/* Store 16 bits at byte pointer */
+#define	SET16(bp, data)		{ (bp)[0] = (data); \
+				(bp)[1] = (data) >> 8; }
+/* Store 32 bits at byte pointer */
+#define	SET32(bp, data)		{ (bp)[0] = (data); \
+				(bp)[1] = (data) >> 8;  \
+				(bp)[2] = (data) >> 16; \
+				(bp)[3] = (data) >> 24; }
+
+#endif /* FS_DIGI_WI_G_DRIVER_H */
diff --git a/drivers/net/wireless/digi_wi_g_priv_handler.c b/drivers/net/wireless/digi_wi_g_priv_handler.c
new file mode 100644
index 000000000000..c7ab82a2e335
--- /dev/null
+++ b/drivers/net/wireless/digi_wi_g_priv_handler.c
@@ -0,0 +1,345 @@
+/*
+ *  net/wireless/digi_wi_g_priv_handler.c
+ *
+ *  Copyright (C) 2007 by Digi International Inc.
+ *  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 version2  as published by
+ *  the Free Software Foundation.
+*/
+/*
+ *  !Revision:   $Revision: 1.1 $
+ *  !Author:     Markus Pietrek
+ *  !Descr:      Contains private user handler stuff.
+*/
+
+#warning fix the getter functions, they compare 0 instead of '0'
+
+/***********************************************************************
+ * @Function: digi_wi_g_wx_set_swencryption
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int digi_wi_g_wx_set_swencryption(struct net_device *dev,
+				       struct iw_request_info *info,
+				       union iwreq_data *data,
+				       char *extra)
+{
+	struct digi_wi_g_private *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	char on;
+	
+        DBG_FN( DBG_INTERFACE | DBG_MINOR );
+
+	on = *extra;
+
+	digi_wi_g_lock(priv, flags);
+	if (on)
+		priv->hw_aes = 0;
+	else 
+		priv->hw_aes = 1;
+	digi_wi_g_unlock(priv, flags);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: digi_wi_g_wx_get_swencryption
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int digi_wi_g_wx_get_swencryption(struct net_device *dev,
+				       struct iw_request_info *info,
+				       union iwreq_data *data,
+				       char *extra)
+{
+	struct digi_wi_g_private *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	int on;
+
+        DBG_FN( DBG_INTERFACE | DBG_MINOR );
+
+	digi_wi_g_lock(priv, flags);
+	if (priv->hw_aes)
+		on = 0;
+	else
+		on = 1;
+	digi_wi_g_unlock(priv, flags);
+
+	if (on)
+		strncpy(extra, "1 (SW encryption enabled) ", MAX_WX_STRING);
+	else
+		strncpy(extra, "0 (SW encryption disabled) ", MAX_WX_STRING);
+	data->data.length = strlen(extra + 1);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: digi_wi_g_wx_set_ant_div
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int digi_wi_g_wx_set_ant_div(struct net_device *dev,
+				       struct iw_request_info *info,
+				       union iwreq_data *data,
+				       char *extra)
+{
+	struct digi_wi_g_private *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	char on;
+	
+        DBG_FN( DBG_INTERFACE | DBG_MINOR );
+
+	on = *extra;
+
+	digi_wi_g_lock(priv, flags);
+	if (on) {
+		priv->ant_div = 1;
+		HW_GEN_CONTROL |= GEN_ANTDIV;
+	} else {
+		priv->ant_div = 0;
+		HW_GEN_CONTROL &= ~GEN_ANTDIV;
+	}
+	digi_wi_g_unlock(priv, flags);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: digi_wi_g_wx_get_ant_div
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int digi_wi_g_wx_get_ant_div(struct net_device *dev,
+				       struct iw_request_info *info,
+				       union iwreq_data *data,
+				       char *extra)
+{
+	struct digi_wi_g_private *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	int on;
+
+        DBG_FN( DBG_INTERFACE | DBG_MINOR );
+
+	digi_wi_g_lock(priv, flags);
+	if (priv->ant_div)
+		on = 1;
+	else
+		on = 0;
+	digi_wi_g_unlock(priv, flags);
+
+	if (on)
+		strncpy(extra, "1 (Antenna Diversity is enabled) ", MAX_WX_STRING);
+	else
+		strncpy(extra, "0 (Antenna Diversity is disabled) ", MAX_WX_STRING);
+	data->data.length = strlen(extra + 1);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: digi_wi_g_wx_set_antenna
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int digi_wi_g_wx_set_antenna(struct net_device *dev,
+				       struct iw_request_info *info,
+				       union iwreq_data *data,
+				       char *extra)
+{
+	struct digi_wi_g_private *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	char ant;
+	
+        DBG_FN( DBG_INTERFACE | DBG_MINOR );
+
+	ant = *extra;
+
+	if ((ant == 0) || (ant == 1)) {
+		digi_wi_g_lock(priv, flags);
+		if (ant == 0) {
+			priv->antenna = 0;
+			HW_GEN_CONTROL &= ~GEN_ANTSEL;
+		} else {
+			priv->antenna = 1;
+			HW_GEN_CONTROL |= GEN_ANTSEL;
+		}
+		digi_wi_g_unlock(priv, flags);
+	} else {
+		printk(PFX KERN_ERR "Value should be 0 or 1.\n");
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: digi_wi_g_wx_get_antenna
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int digi_wi_g_wx_get_antenna(struct net_device *dev,
+				       struct iw_request_info *info,
+				       union iwreq_data *data,
+				       char *extra)
+{
+	struct digi_wi_g_private *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	int ant;
+
+        DBG_FN( DBG_INTERFACE | DBG_MINOR );
+
+	digi_wi_g_lock(priv, flags);
+	ant = priv->antenna;
+	digi_wi_g_unlock(priv, flags);
+
+	if (ant == 0)
+		strncpy(extra, "0 (Antenna 1 selected) ", MAX_WX_STRING);
+	else
+		strncpy(extra, "1 (Antenna 2 selected) ", MAX_WX_STRING);
+	data->data.length = strlen(extra + 1);
+
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: digi_wi_g_wx_set_rx_off
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int digi_wi_g_wx_set_rx_off(struct net_device *dev,
+				       struct iw_request_info *info,
+				       union iwreq_data *data,
+				       char *extra)
+{
+	struct digi_wi_g_private *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	char rx_off;
+	
+        DBG_FN( DBG_INTERFACE | DBG_MINOR );
+
+	rx_off = *extra;
+
+	if ((rx_off == 0) || (rx_off == 1)) {
+		digi_wi_g_lock(priv, flags);
+		if (rx_off == 0) {
+			priv->rx_off = 0;
+		} else {
+			priv->rx_off = 1;
+		}
+		digi_wi_g_unlock(priv, flags);
+	} else {
+		printk(PFX KERN_ERR "Value should be 0 or 1.\n");
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+
+/***********************************************************************
+ * @Function: digi_wi_g_wx_get_rx_off
+ * @Return:
+ * @Descr:
+ ***********************************************************************/
+static int digi_wi_g_wx_get_rx_off(struct net_device *dev,
+				       struct iw_request_info *info,
+				       union iwreq_data *data,
+				       char *extra)
+{
+	struct digi_wi_g_private *priv = ieee80211softmac_priv(dev);
+	unsigned long flags;
+	int rx_off;
+
+        DBG_FN( DBG_INTERFACE | DBG_MINOR );
+
+	digi_wi_g_lock(priv, flags);
+	rx_off = priv->rx_off;
+	digi_wi_g_unlock(priv, flags);
+
+	if (rx_off == 0)
+		strncpy(extra, "0 (rx is on) ", MAX_WX_STRING);
+	else
+		strncpy(extra, "1 (rx is off) ", MAX_WX_STRING);
+	data->data.length = strlen(extra + 1);
+
+	return 0;
+}
+
+static const iw_handler digi_wi_g_priv_wx_handlers[] = {
+	/* Enable/Disable Software Encryption mode */
+	digi_wi_g_wx_set_swencryption,
+	/* Get Software Encryption mode */
+	digi_wi_g_wx_get_swencryption,
+	/* Enable/Disable Antenna Diversity mode */
+	digi_wi_g_wx_set_ant_div,
+	/* Get Antenna Diversity mode */
+	digi_wi_g_wx_get_ant_div,
+	/* Select Antenna */
+	digi_wi_g_wx_set_antenna,
+	/* Get Antenna */
+	digi_wi_g_wx_get_antenna,
+	/* Set rx off */
+	digi_wi_g_wx_set_rx_off,
+	/* Get rx off */
+	digi_wi_g_wx_get_rx_off,
+};
+
+#define PRIV_WX_SET_SWENCRYPTION	(SIOCIWFIRSTPRIV + 0)
+#define PRIV_WX_GET_SWENCRYPTION	(SIOCIWFIRSTPRIV + 1)
+#define PRIV_WX_SET_ANTDIV		(SIOCIWFIRSTPRIV + 2)
+#define PRIV_WX_GET_ANTDIV		(SIOCIWFIRSTPRIV + 3)
+#define PRIV_WX_SET_ANTSEL		(SIOCIWFIRSTPRIV + 4)
+#define PRIV_WX_GET_ANTSEL		(SIOCIWFIRSTPRIV + 5)
+#define PRIV_WX_SET_RX_OFF		(SIOCIWFIRSTPRIV + 6)
+#define PRIV_WX_GET_RX_OFF		(SIOCIWFIRSTPRIV + 7)
+
+static const struct iw_priv_args digi_wi_g_priv_wx_args[] = {
+	{
+		.cmd		= PRIV_WX_SET_SWENCRYPTION,
+		.set_args	= IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+		.name		= "set_swencrypt",
+	},
+	{
+		.cmd		= PRIV_WX_GET_SWENCRYPTION,
+		.get_args	= IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		.name		= "get_swencrypt",
+	},
+	{
+		.cmd		= PRIV_WX_SET_ANTDIV,
+		.set_args	= IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+		.name		= "set_ant_div",
+	},
+	{
+		.cmd		= PRIV_WX_GET_ANTDIV,
+		.get_args	= IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		.name		= "get_ant_div",
+	},
+	{
+		.cmd		= PRIV_WX_SET_ANTSEL,
+		.set_args	= IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+		.name		= "set_ant_sel",
+	},
+	{
+		.cmd		= PRIV_WX_GET_ANTSEL,
+		.get_args	= IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		.name		= "get_ant_sel",
+	},
+	{
+		.cmd		= PRIV_WX_SET_RX_OFF,
+		.set_args	= IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+		.name		= "set_rx_off",
+	},
+	{
+		.cmd		= PRIV_WX_GET_RX_OFF,
+		.get_args	= IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		.name		= "get_rx_off",
+	},
+};
+
diff --git a/drivers/pcmcia/Kconfig b/drivers/pcmcia/Kconfig
index 222904411a13..835082cb1637 100644
--- a/drivers/pcmcia/Kconfig
+++ b/drivers/pcmcia/Kconfig
@@ -275,6 +275,21 @@ config AT91_CF
 	  Say Y here to support the CompactFlash controller on AT91 chips.
 	  Or choose M to compile the driver as a module named "at91_cf".
 
+config S3C2443_PCMCIA
+	tristate "S3C2443 PCMCIA driver"
+	depends on PCMCIA && CPU_S3C2443
+	help
+	  Say Y here to support the CompactFlash/PCMCIA interface with the
+	  S3C2443 SoC
+
+config PCMCIA_MX31ADS
+	tristate "MX31ADS PCMCIA support"
+	depends on ARM && MACH_MX31ADS && PCMCIA
+	help
+	  Say Y here to include support for the Freescale i.MX31 PCMCIA controller.
+
+	  This driver is also available as a module called mx31ads_pcmcia.
+
 config ELECTRA_CF
 	tristate "Electra CompactFlash Controller"
 	depends on PCMCIA && PPC_PASEMI
diff --git a/drivers/pcmcia/Makefile b/drivers/pcmcia/Makefile
index 238629ad7f7c..44a60c9bc0e5 100644
--- a/drivers/pcmcia/Makefile
+++ b/drivers/pcmcia/Makefile
@@ -32,7 +32,9 @@ obj-$(CONFIG_PCMCIA_VRC4173)			+= vrc4173_cardu.o
 obj-$(CONFIG_OMAP_CF)				+= omap_cf.o
 obj-$(CONFIG_BFIN_CFPCMCIA)			+= bfin_cf_pcmcia.o
 obj-$(CONFIG_AT91_CF)				+= at91_cf.o
+obj-$(CONFIG_S3C2443_PCMCIA)			+= s3c2443_pcmcia.o
 obj-$(CONFIG_ELECTRA_CF)			+= electra_cf.o
+obj-$(CONFIG_PCMCIA_MX31ADS)			+= mx31ads-pcmcia.o
 
 sa11xx_core-y					+= soc_common.o sa11xx_base.o
 pxa2xx_core-y					+= soc_common.o pxa2xx_base.o
diff --git a/drivers/pcmcia/mx31ads-pcmcia.c b/drivers/pcmcia/mx31ads-pcmcia.c
new file mode 100644
index 000000000000..8031da1af6c7
--- /dev/null
+++ b/drivers/pcmcia/mx31ads-pcmcia.c
@@ -0,0 +1,1291 @@
+/*======================================================================
+    drivers/pcmcia/mx31ads-pcmica.c
+
+    Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+
+    Device driver for the PCMCIA control functionality of i.Mx31
+    microprocessors.
+
+    The contents of this file are subject to the Mozilla Public
+    License Version 1.1 (the "License"); you may not use this file
+    except in compliance with the License. You may obtain a copy of
+    the License at http://www.mozilla.org/MPL/
+
+    Software distributed under the License is distributed on an "AS
+    IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
+    implied. See the License for the specific language governing
+    rights and limitations under the License.
+
+    The initial developer of the original code is John G. Dorsey
+    <john+@cs.cmu.edu>.  Portions created by John G. Dorsey are
+    Copyright (C) 1999 John G. Dorsey.  All Rights Reserved.
+
+    Alternatively, the contents of this file may be used under the
+    terms of the GNU Public License version 2 (the "GPL"), in which
+    case the provisions of the GPL are applicable instead of the
+    above.  If you wish to allow the use of your version of this file
+    only under the terms of the GPL and not to allow others to use
+    your version of this file under the MPL, indicate your decision
+    by deleting the provisions above and replace them with the notice
+    and other provisions required by the GPL.  If you do not delete
+    the provisions above, a recipient may use your version of this
+    file under either the MPL or the GPL.
+
+======================================================================*/
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+
+#include <pcmcia/cs_types.h>
+#include <pcmcia/cs.h>
+#include <pcmcia/ss.h>
+#include <asm/mach-types.h>
+#include <mach/pcmcia.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/timer.h>
+#include <linux/mm.h>
+#include <linux/spinlock.h>
+
+#include <mach/hardware.h>
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+
+#include "mx31ads-pcmcia.h"
+#include <linux/irq.h>
+
+#define	MX31ADS_PCMCIA_IRQ	MXC_INT_PCMCIA
+
+/*
+ * The mapping of window size to bank size value
+ */
+static bsize_map_t bsize_map[] = {
+	/* Window size  Bank size       */
+	{POR_1, POR_BSIZE_1},
+	{POR_2, POR_BSIZE_2},
+	{POR_4, POR_BSIZE_4},
+	{POR_8, POR_BSIZE_8},
+	{POR_16, POR_BSIZE_16},
+	{POR_32, POR_BSIZE_32},
+	{POR_64, POR_BSIZE_64},
+	{POR_128, POR_BSIZE_128},
+	{POR_256, POR_BSIZE_256},
+	{POR_512, POR_BSIZE_512},
+
+	{POR_1K, POR_BSIZE_1K},
+	{POR_2K, POR_BSIZE_2K},
+	{POR_4K, POR_BSIZE_4K},
+	{POR_8K, POR_BSIZE_8K},
+	{POR_16K, POR_BSIZE_16K},
+	{POR_32K, POR_BSIZE_32K},
+	{POR_64K, POR_BSIZE_64K},
+	{POR_128K, POR_BSIZE_128K},
+	{POR_256K, POR_BSIZE_256K},
+	{POR_512K, POR_BSIZE_512K},
+
+	{POR_1M, POR_BSIZE_1M},
+	{POR_2M, POR_BSIZE_2M},
+	{POR_4M, POR_BSIZE_4M},
+	{POR_8M, POR_BSIZE_8M},
+	{POR_16M, POR_BSIZE_16M},
+	{POR_32M, POR_BSIZE_32M},
+	{POR_64M, POR_BSIZE_64M}
+};
+
+#define to_mx31ads_pcmcia_socket(x)	container_of(x, struct mx31ads_pcmcia_socket, socket)
+
+/* mx31ads_pcmcia_find_bsize()
+ * ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+ *
+ * Find the bsize according to the window size passed in
+ *
+ * Return:
+ */
+static int mx31ads_pcmcia_find_bsize(unsigned long win_size)
+{
+	int i, nr = sizeof(bsize_map) / sizeof(bsize_map_t);
+	int bsize = -1;
+
+	for (i = 0; i < nr; i++) {
+		if (bsize_map[i].win_size == win_size) {
+			bsize = bsize_map[i].bsize;
+			break;
+		}
+	}
+
+	pr_debug(KERN_INFO "nr = %d bsize = 0x%0x\n", nr, bsize);
+	if (bsize < 0 || i > nr) {
+		pr_debug(KERN_INFO "No such bsize\n");
+		return -ENODEV;
+	}
+
+	return bsize;
+}
+
+/* mx31ads_common_pcmcia_sock_init()
+ * ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+ *
+ * (Re-)Initialise the socket, turning on status interrupts
+ * and PCMCIA bus.  This must wait for power to stabilise
+ * so that the card status signals report correctly.
+ *
+ * Returns: 0
+ */
+static int mx31ads_common_pcmcia_sock_init(struct pcmcia_socket *sock)
+{
+	struct mx31ads_pcmcia_socket *skt = to_mx31ads_pcmcia_socket(sock);
+
+	pr_debug(KERN_INFO "initializing socket\n");
+
+	skt->ops->socket_init(skt);
+	return 0;
+}
+
+/*
+ * mx31ads_common_pcmcia_config_skt
+ * ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+ *
+ * Convert PCMCIA socket state to our socket configure structure.
+ */
+static int
+mx31ads_common_pcmcia_config_skt(struct mx31ads_pcmcia_socket *skt,
+				 socket_state_t * state)
+{
+	int ret;
+
+	ret = skt->ops->configure_socket(skt, state);
+	if (ret == 0) {
+		/*
+		 * This really needs a better solution.  The IRQ
+		 * may or may not be claimed by the driver.
+		 */
+		if (skt->irq_state != 1 && state->io_irq) {
+			skt->irq_state = 1;
+			set_irq_type(skt->irq, IRQF_TRIGGER_FALLING);
+		} else if (skt->irq_state == 1 && state->io_irq == 0) {
+			skt->irq_state = 0;
+			set_irq_type(skt->irq, IRQF_TRIGGER_RISING);
+		}
+
+		skt->cs_state = *state;
+	}
+
+	if (ret < 0)
+		pr_debug(KERN_ERR "mx31ads_common_pcmcia: unable to configure"
+			 " socket\n");
+
+	return ret;
+}
+
+/*
+ * mx31ads_common_pcmcia_suspend()
+ * ^^^^^^^^^^^^^^^^^^^^^^^^^^^
+ *
+ * Remove power on the socket, disable IRQs from the card.
+ * Turn off status interrupts, and disable the PCMCIA bus.
+ *
+ * Returns: 0
+ */
+static int mx31ads_common_pcmcia_suspend(struct pcmcia_socket *sock)
+{
+	struct mx31ads_pcmcia_socket *skt = to_mx31ads_pcmcia_socket(sock);
+	int ret;
+
+	pr_debug(KERN_INFO "suspending socket\n");
+
+	ret = mx31ads_common_pcmcia_config_skt(skt, &dead_socket);
+	if (ret == 0)
+		skt->ops->socket_suspend(skt);
+
+	return ret;
+}
+
+static unsigned int mx31ads_common_pcmcia_skt_state(struct mx31ads_pcmcia_socket
+						    *skt)
+{
+	struct pcmcia_state state;
+	unsigned int stat;
+
+	memset(&state, 0, sizeof(struct pcmcia_state));
+
+	skt->ops->socket_state(skt, &state);
+
+	stat = state.detect ? SS_DETECT : 0;
+	stat |= state.ready ? SS_READY : 0;
+	stat |= state.wrprot ? SS_WRPROT : 0;
+	stat |= state.vs_3v ? SS_3VCARD : 0;
+	stat |= state.vs_Xv ? SS_XVCARD : 0;
+
+	/* The power status of individual sockets is not available
+	 * explicitly from the hardware, so we just remember the state
+	 * and regurgitate it upon request:
+	 */
+	stat |= skt->cs_state.Vcc ? SS_POWERON : 0;
+
+	if (skt->cs_state.flags & SS_IOCARD)
+		stat |= state.bvd1 ? SS_STSCHG : 0;
+	else {
+		if (state.bvd1 == 0)
+			stat |= SS_BATDEAD;
+		else if (state.bvd2 == 0)
+			stat |= SS_BATWARN;
+	}
+
+	pr_debug(KERN_INFO "stat = 0x%08x\n", stat);
+
+	return stat;
+}
+
+/*
+ *  Implements the get_status() operation for the in-kernel PCMCIA
+ * service (formerly SS_GetStatus in Card Services). Essentially just
+ * fills in bits in `status' according to internal driver state or
+ * the value of the voltage detect chipselect register.
+ *
+ * As a debugging note, during card startup, the PCMCIA core issues
+ * three set_socket() commands in a row the first with RESET deasserted,
+ * the second with RESET asserted, and the last with RESET deasserted
+ * again. Following the third set_socket(), a get_status() command will
+ * be issued. The kernel is looking for the SS_READY flag (see
+ * setup_socket(), reset_socket(), and unreset_socket() in cs.c).
+ *
+ * Returns: 0
+ */
+static int mx31ads_common_pcmcia_get_status(struct pcmcia_socket *sock,
+					    unsigned int *status)
+{
+	struct mx31ads_pcmcia_socket *skt = to_mx31ads_pcmcia_socket(sock);
+
+	skt->status = mx31ads_common_pcmcia_skt_state(skt);
+	*status = skt->status;
+
+	return 0;
+}
+
+/*
+ * Implements the set_socket() operation for the in-kernel PCMCIA
+ * service (formerly SS_SetSocket in Card Services). We more or
+ * less punt all of this work and let the kernel handle the details
+ * of power configuration, reset, &c. We also record the value of
+ * `state' in order to regurgitate it to the PCMCIA core later.
+ *
+ * Returns: 0
+ */
+static int mx31ads_common_pcmcia_set_socket(struct pcmcia_socket *sock,
+					    socket_state_t * state)
+{
+	struct mx31ads_pcmcia_socket *skt = to_mx31ads_pcmcia_socket(sock);
+
+	pr_debug(KERN_INFO
+		 "mask: %s%s%s%s%s%sflags: %s%s%s%s%s%sVcc %d Vpp %d irq %d\n",
+		 (state->csc_mask == 0) ? "<NONE> " : "",
+		 (state->csc_mask & SS_DETECT) ? "DETECT " : "",
+		 (state->csc_mask & SS_READY) ? "READY " : "",
+		 (state->csc_mask & SS_BATDEAD) ? "BATDEAD " : "",
+		 (state->csc_mask & SS_BATWARN) ? "BATWARN " : "",
+		 (state->csc_mask & SS_STSCHG) ? "STSCHG " : "",
+		 (state->flags == 0) ? "<NONE> " : "",
+		 (state->flags & SS_PWR_AUTO) ? "PWR_AUTO " : "",
+		 (state->flags & SS_IOCARD) ? "IOCARD " : "",
+		 (state->flags & SS_RESET) ? "RESET " : "",
+		 (state->flags & SS_SPKR_ENA) ? "SPKR_ENA " : "",
+		 (state->flags & SS_OUTPUT_ENA) ? "OUTPUT_ENA " : "",
+		 state->Vcc, state->Vpp, state->io_irq);
+
+	pr_debug(KERN_INFO
+		 "csc_mask: %08x flags: %08x Vcc: %d Vpp: %d io_irq: %d\n",
+		 state->csc_mask, state->flags, state->Vcc, state->Vpp,
+		 state->io_irq);
+
+	return mx31ads_common_pcmcia_config_skt(skt, state);
+}
+
+/*
+ * Set address and profile to window registers PBR, POR, POFR
+ */
+static int mx31ads_pcmcia_set_window_reg(ulong start, ulong end, u_int window)
+{
+	int bsize;
+	ulong size = end - start + 1;
+
+	bsize = mx31ads_pcmcia_find_bsize(size);
+	if (bsize < 0) {
+		pr_debug("Cannot set the window register\n");
+		return -1;
+	}
+	/* Disable the window */
+	_reg_PCMCIA_POR(window) &= ~PCMCIA_POR_PV;
+
+	/* Set PBR, POR, POFR */
+	_reg_PCMCIA_PBR(window) = start;
+	_reg_PCMCIA_POR(window) &= ~(PCMCIA_POR_PRS_MASK
+				     | PCMCIA_POR_WPEN
+				     | PCMCIA_POR_WP
+				     | PCMCIA_POR_BSIZE_MASK
+				     | PCMCIA_POR_PPS_8);
+	_reg_PCMCIA_POR(window) |= bsize | PCMCIA_POR_PPS_16;
+
+	switch (window) {
+	case IO_WINDOW:
+		_reg_PCMCIA_POR(window) |= PCMCIA_POR_PRS(PCMCIA_POR_PRS_IO);
+		break;
+
+	case ATTRIBUTE_MEMORY_WINDOW:
+		_reg_PCMCIA_POR(window) |=
+		    PCMCIA_POR_PRS(PCMCIA_POR_PRS_ATTRIBUTE);
+		break;
+
+	case COMMON_MEMORY_WINDOW:
+		_reg_PCMCIA_POR(window) |=
+		    PCMCIA_POR_PRS(PCMCIA_POR_PRS_COMMON);
+		break;
+
+	default:
+		pr_debug("Window %d is not support\n", window);
+		return -1;
+	}
+	_reg_PCMCIA_POFR(window) = 0;
+
+	/* Enable the window */
+	_reg_PCMCIA_POR(window) |= PCMCIA_POR_PV;
+
+	return 0;
+}
+
+/*
+ * Implements the set_io_map() operation for the in-kernel PCMCIA
+ * service (formerly SS_SetIOMap in Card Services). We configure
+ * the map speed as requested, but override the address ranges
+ * supplied by Card Services.
+ *
+ * Returns: 0 on success, -1 on error
+ */
+static int
+mx31ads_common_pcmcia_set_io_map(struct pcmcia_socket *sock,
+				 struct pccard_io_map *map)
+{
+	struct mx31ads_pcmcia_socket *skt = to_mx31ads_pcmcia_socket(sock);
+	unsigned short speed = map->speed;
+
+	pr_debug("map %u  speed %u start 0x%08lx stop 0x%08lx\n",
+		 map->map, map->speed, map->start, map->stop);
+	pr_debug("flags: %s%s%s%s%s%s%s%s\n",
+		 (map->flags == 0) ? "<NONE>" : "",
+		 (map->flags & MAP_ACTIVE) ? "ACTIVE " : "",
+		 (map->flags & MAP_16BIT) ? "16BIT " : "",
+		 (map->flags & MAP_AUTOSZ) ? "AUTOSZ " : "",
+		 (map->flags & MAP_0WS) ? "0WS " : "",
+		 (map->flags & MAP_WRPROT) ? "WRPROT " : "",
+		 (map->flags & MAP_USE_WAIT) ? "USE_WAIT " : "",
+		 (map->flags & MAP_PREFETCH) ? "PREFETCH " : "");
+
+	if (map->map >= MAX_IO_WIN) {
+		pr_debug(KERN_ERR "%s(): map (%d) out of range\n", __FUNCTION__,
+			 map->map);
+		return -1;
+	}
+
+	if (map->flags & MAP_ACTIVE) {
+		if (speed == 0)
+			speed = PCMCIA_IO_ACCESS;
+	} else {
+		speed = 0;
+	}
+
+	skt->spd_io[map->map] = speed;
+	skt->ops->set_timing(skt);
+
+	if (map->stop == 1)
+		map->stop = PAGE_SIZE - 1;
+
+	skt->socket.io_offset = (unsigned long)skt->virt_io;
+	map->stop -= map->start;
+	map->stop += (unsigned long)skt->virt_io;
+	map->start = (unsigned long)skt->virt_io;
+
+	mx31ads_pcmcia_set_window_reg(skt->res_io.start, skt->res_io.end,
+				      IO_WINDOW);
+
+	pr_debug(KERN_ERR "IO window: _reg_PCMCIA_PBR(%d) = %08x\n",
+		 IO_WINDOW, _reg_PCMCIA_PBR(IO_WINDOW));
+	pr_debug(KERN_ERR "IO window: _reg_PCMCIA_POR(%d) = %08x\n",
+		 IO_WINDOW, _reg_PCMCIA_POR(IO_WINDOW));
+
+	return 0;
+}
+
+/*
+ * Implements the set_mem_map() operation for the in-kernel PCMCIA
+ * service (formerly SS_SetMemMap in Card Services). We configure
+ * the map speed as requested, but override the address ranges
+ * supplied by Card Services.
+ *
+ * Returns: 0 on success, -1 on error
+ */
+static int
+mx31ads_common_pcmcia_set_mem_map(struct pcmcia_socket *sock,
+				  struct pccard_mem_map *map)
+{
+	struct mx31ads_pcmcia_socket *skt = to_mx31ads_pcmcia_socket(sock);
+	struct resource *res;
+	unsigned short speed = map->speed;
+
+	pr_debug
+	    (KERN_INFO
+	     "map %u speed %u card_start %08x flags%08x static_start %08lx\n",
+	     map->map, map->speed, map->card_start, map->flags,
+	     map->static_start);
+	pr_debug(KERN_INFO "flags: %s%s%s%s%s%s%s%s\n",
+		 (map->flags == 0) ? "<NONE>" : "",
+		 (map->flags & MAP_ACTIVE) ? "ACTIVE " : "",
+		 (map->flags & MAP_16BIT) ? "16BIT " : "",
+		 (map->flags & MAP_AUTOSZ) ? "AUTOSZ " : "",
+		 (map->flags & MAP_0WS) ? "0WS " : "",
+		 (map->flags & MAP_WRPROT) ? "WRPROT " : "",
+		 (map->flags & MAP_ATTRIB) ? "ATTRIB " : "",
+		 (map->flags & MAP_USE_WAIT) ? "USE_WAIT " : "");
+
+	if (map->map >= MAX_WIN)
+		return -EINVAL;
+
+	if (map->flags & MAP_ACTIVE) {
+		if (speed == 0)
+			speed = 300;
+	} else {
+		speed = 0;
+	}
+
+	if (map->flags & MAP_ATTRIB) {
+		res = &skt->res_attr;
+		skt->spd_attr[map->map] = speed;
+		skt->spd_mem[map->map] = 0;
+		mx31ads_pcmcia_set_window_reg(res->start, res->end,
+					      ATTRIBUTE_MEMORY_WINDOW);
+
+		pr_debug(KERN_INFO "Attr window: _reg_PCMCIA_PBR(%d) = %08x\n",
+			 ATTRIBUTE_MEMORY_WINDOW,
+			 _reg_PCMCIA_PBR(ATTRIBUTE_MEMORY_WINDOW));
+		pr_debug(KERN_INFO "_reg_PCMCIA_POR(%d) = %08x\n",
+			 ATTRIBUTE_MEMORY_WINDOW,
+			 _reg_PCMCIA_POR(ATTRIBUTE_MEMORY_WINDOW));
+
+	} else {
+		res = &skt->res_mem;
+		skt->spd_attr[map->map] = 0;
+		skt->spd_mem[map->map] = speed;
+		mx31ads_pcmcia_set_window_reg(res->start, res->end,
+					      COMMON_MEMORY_WINDOW);
+
+		pr_debug(KERN_INFO "Com window: _reg_PCMCIA_PBR(%d) = %08x\n",
+			 COMMON_MEMORY_WINDOW,
+			 _reg_PCMCIA_PBR(COMMON_MEMORY_WINDOW));
+		pr_debug(KERN_INFO "Com window: _reg_PCMCIA_POR(%d) = %08x\n",
+			 COMMON_MEMORY_WINDOW,
+			 _reg_PCMCIA_POR(COMMON_MEMORY_WINDOW));
+	}
+
+	skt->ops->set_timing(skt);
+
+	map->static_start = res->start + map->card_start;
+
+	return 0;
+}
+
+static struct pccard_operations mx31ads_common_pcmcia_operations = {
+	.init = mx31ads_common_pcmcia_sock_init,
+	.suspend = mx31ads_common_pcmcia_suspend,
+	.get_status = mx31ads_common_pcmcia_get_status,
+	.set_socket = mx31ads_common_pcmcia_set_socket,
+	.set_io_map = mx31ads_common_pcmcia_set_io_map,
+	.set_mem_map = mx31ads_common_pcmcia_set_mem_map,
+};
+
+/* ============================================================================== */
+
+static inline void mx31ads_pcmcia_irq_config(void)
+{
+	/* Setup irq */
+	_reg_PCMCIA_PER =
+	    (PCMCIA_PER_RDYLE | PCMCIA_PER_CDE1 | PCMCIA_PER_CDE2);
+}
+
+static inline void mx31ads_pcmcia_invalidate_windows(void)
+{
+	int i;
+
+	for (i = 0; i < PCMCIA_WINDOWS; i++) {
+		_reg_PCMCIA_PBR(i) = 0;
+		_reg_PCMCIA_POR(i) = 0;
+		_reg_PCMCIA_POFR(i) = 0;
+	}
+}
+
+extern void gpio_pcmcia_active(void);
+extern void gpio_pcmcia_inactive(void);
+
+static int mx31ads_pcmcia_hw_init(struct mx31ads_pcmcia_socket *skt)
+{
+	/* Configure the pins for PCMCIA */
+	gpio_pcmcia_active();
+
+	/*
+	 * enabling interrupts at this time causes a flood of interrupts
+	 * if a card is present, so wait for configure_socket
+	 * to enable them when requested.
+	 *
+	 * mx31ads_pcmcia_irq_config();
+	 */
+	mx31ads_pcmcia_invalidate_windows();
+
+	/* Register interrupt. */
+	skt->irq = MX31ADS_PCMCIA_IRQ;
+
+	return 0;
+}
+
+static void mx31ads_pcmcia_free_irq(struct mx31ads_pcmcia_socket *skt,
+				    unsigned int irq)
+{
+	free_irq(irq, skt);
+}
+
+static void mx31ads_pcmcia_hw_shutdown(struct mx31ads_pcmcia_socket *skt)
+{
+	mx31ads_pcmcia_invalidate_windows();
+	mx31ads_pcmcia_free_irq(skt, MX31ADS_PCMCIA_IRQ);
+
+	/* Disable the pins */
+	gpio_pcmcia_inactive();
+}
+
+/*
+ * Get the socket state
+ */
+static void
+mx31ads_pcmcia_socket_state(struct mx31ads_pcmcia_socket *skt,
+			    struct pcmcia_state *state)
+{
+	unsigned long pins;
+
+	pins = _reg_PCMCIA_PIPR;
+	pr_debug(KERN_INFO "_reg_PCMCIA_PIPR = 0x%08lx\n", pins);
+
+	state->ready = (pins & PCMCIA_PIPR_RDY) ? 1 : 0;
+	state->bvd2 = (pins & PCMCIA_PIPR_BVD2) ? 1 : 0;
+	state->bvd1 = (pins & PCMCIA_PIPR_BVD1) ? 1 : 0;
+
+	if ((pins & PCMCIA_PIPR_CD) == PCMCIA_PIPR_CD) {
+		state->detect = 0;
+		skt->cs_state.csc_mask |= SS_INSERTION;
+	} else {
+		state->detect = 1;
+	}
+	state->detect = (pins & PCMCIA_PIPR_CD) ? 0 : 1;
+	state->wrprot = (pins & PCMCIA_PIPR_WP) ? 1 : 0;
+	state->poweron = (pins & PCMCIA_PIPR_POWERON) ? 1 : 0;
+#if 0
+	if ((pins & PCMCIA_PIPR_CD) == PCMCIA_PIPR_CD) {
+		state->detect = 0;
+		skt->cs_state.csc_mask |= SS_INSERTION;
+	} else {
+		state->detect = 1;
+	}
+	if (pins & PCMCIA_PIPR_VS_5V) {
+		state->vs_3v = 0;
+		skt->cs_state.Vcc = 33;
+	} else {
+		state->vs_3v = 1;
+		skt->cs_state.Vcc = 50;
+	}
+#endif
+	state->vs_3v = (pins & PCMCIA_PIPR_VS_5V) ? 0 : 1;
+	state->vs_Xv = 0;
+}
+
+static __inline__ void mx31ads_pcmcia_low_power(bool enable)
+{
+	if (enable)
+		_reg_PCMCIA_PGCR |= PCMCIA_PGCR_LPMEN;
+	else
+		_reg_PCMCIA_PGCR &= ~PCMCIA_PGCR_LPMEN;
+}
+
+static __inline__ void mx31ads_pcmcia_soft_reset(void)
+{
+	_reg_PCMCIA_PGCR |= PCMCIA_PGCR_RESET;
+	msleep(2);
+
+	_reg_PCMCIA_PGCR &= ~(PCMCIA_PGCR_RESET | PCMCIA_PGCR_LPMEN);
+	_reg_PCMCIA_PGCR |= PCMCIA_PGCR_POE;
+	msleep(2);
+	pr_debug(KERN_INFO "_reg_PCMCIA_PGCR = %08x\n", _reg_PCMCIA_PGCR);
+}
+
+static int
+mx31ads_pcmcia_configure_socket(struct mx31ads_pcmcia_socket *skt,
+				const socket_state_t * state)
+{
+	int ret = 0;
+
+	if (state->Vcc != 0 && state->Vcc != 33 && state->Vcc != 50) {
+		pr_debug(KERN_ERR "mx31ads-pcmcia: unrecognized Vcc %d\n",
+			 state->Vcc);
+		return -1;
+	}
+
+	pr_debug(KERN_INFO "PIPR = %x, desired Vcc = %d.%dV\n",
+		 _reg_PCMCIA_PIPR, state->Vcc / 10, state->Vcc % 10);
+
+	if (!(skt->socket.state & SOCKET_PRESENT) && (skt->pre_stat == 1)) {
+		pr_debug(KERN_INFO "Socket enter low power mode\n");
+		skt->pre_stat = 0;
+		mx31ads_pcmcia_low_power(1);
+	}
+
+	if (state->flags & SS_RESET) {
+		mx31ads_pcmcia_soft_reset();
+
+		/* clean out previous tenant's trash */
+		_reg_PCMCIA_PGSR = (PCMCIA_PGSR_NWINE
+				    | PCMCIA_PGSR_LPE
+				    | PCMCIA_PGSR_SE
+				    | PCMCIA_PGSR_CDE | PCMCIA_PGSR_WPE);
+	}
+	/* enable interrupts if requested, else turn 'em off */
+	if (skt->irq)
+		mx31ads_pcmcia_irq_config();
+	else
+		_reg_PCMCIA_PER = 0;
+
+	if (skt->socket.state & SOCKET_PRESENT) {
+		skt->pre_stat = 1;
+	}
+	return ret;
+}
+
+static void mx31ads_pcmcia_enable_irq(struct mx31ads_pcmcia_socket *skt,
+				      unsigned int irq)
+{
+	set_irq_type(irq, IRQF_TRIGGER_RISING);
+	set_irq_type(irq, IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING);
+}
+
+static void mx31ads_pcmcia_disable_irq(struct mx31ads_pcmcia_socket *skt,
+				       unsigned int irq)
+{
+	set_irq_type(irq, IRQF_TRIGGER_NONE);
+}
+
+/*
+ * Enable card status IRQs on (re-)initialisation.  This can
+ * be called at initialisation, power management event, or
+ * pcmcia event.
+ */
+static void mx31ads_pcmcia_socket_init(struct mx31ads_pcmcia_socket *skt)
+{
+	mx31ads_pcmcia_soft_reset();
+
+	mx31ads_pcmcia_enable_irq(skt, MX31ADS_PCMCIA_IRQ);
+}
+
+/*
+ * Disable card status IRQ on suspend.
+ */
+static void mx31ads_pcmcia_socket_suspend(struct mx31ads_pcmcia_socket *skt)
+{
+	mx31ads_pcmcia_disable_irq(skt, MX31ADS_PCMCIA_IRQ);
+	mx31ads_pcmcia_low_power(1);
+}
+
+/* ==================================================================================== */
+
+/*
+ * PCMCIA strobe hold time
+ */
+static inline u_int mx31ads_pcmcia_por_psht(u_int pcmcia_cycle_ns,
+					    u_int hclk_cycle_ns)
+{
+	u_int psht;
+
+	return psht = pcmcia_cycle_ns / hclk_cycle_ns;
+}
+
+/*
+ * PCMCIA strobe set up time
+ */
+static inline u_int mx31ads_pcmcia_por_psst(u_int pcmcia_cycle_ns,
+					    u_int hclk_cycle_ns)
+{
+	u_int psst;
+
+	return psst = pcmcia_cycle_ns / hclk_cycle_ns;
+}
+
+/*
+ * PCMCIA strobe length time
+ */
+static inline u_int mx31ads_pcmcia_por_pslt(u_int pcmcia_cycle_ns,
+					    u_int hclk_cycle_ns)
+{
+	u_int pslt;
+
+	return pslt = pcmcia_cycle_ns / hclk_cycle_ns + 2;
+}
+
+/*
+ * mx31ads_pcmcia_default_mecr_timing
+ * ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+ *
+ * Calculate MECR clock wait states for given CPU clock
+ * speed and command wait state. This function can be over-
+ * written by a board specific version.
+ *
+ * The default is to simply calculate the BS values as specified in
+ * the INTEL SA1100 development manual
+ * "Expansion Memory (PCMCIA) Configuration Register (MECR)"
+ * that's section 10.2.5 in _my_ version of the manual ;)
+ */
+static unsigned int mx31ads_pcmcia_default_mecr_timing(struct
+						       mx31ads_pcmcia_socket
+						       *skt,
+						       unsigned int cpu_speed,
+						       unsigned int cmd_time)
+{
+	return 0;
+}
+
+/*
+ * Calculate the timing code
+ */
+static u_int mx31ads_pcmcia_cal_code(u_int speed_ns, u_int clk_ns)
+{
+	u_int code;
+
+	code = PCMCIA_POR_PSHT(mx31ads_pcmcia_por_psht(speed_ns, clk_ns))
+	    | PCMCIA_POR_PSST(mx31ads_pcmcia_por_psst(speed_ns, clk_ns))
+	    | PCMCIA_POR_PSL(mx31ads_pcmcia_por_pslt(speed_ns, clk_ns));
+
+	return code;
+}
+
+/*
+ * set MECR value for socket <sock> based on this sockets
+ * io, mem and attribute space access speed.
+ * Call board specific BS value calculation to allow boards
+ * to tweak the BS values.
+ */
+static int mx31ads_pcmcia_set_window_timing(u_int speed_ns, u_int window,
+					    u_int clk_ns)
+{
+	u_int code = 0;
+
+	switch (window) {
+	case IO_WINDOW:
+		code = mx31ads_pcmcia_cal_code(speed_ns, clk_ns);
+		break;
+	case COMMON_MEMORY_WINDOW:
+		code = mx31ads_pcmcia_cal_code(speed_ns, clk_ns);
+		break;
+	case ATTRIBUTE_MEMORY_WINDOW:
+		code = mx31ads_pcmcia_cal_code(speed_ns, clk_ns);
+		break;
+	default:
+		break;
+	}
+
+	/* Disable the window */
+	_reg_PCMCIA_POR(window) &= ~PCMCIA_POR_PV;
+
+	/* Clear the register fisrt */
+	_reg_PCMCIA_POR(window) &= ~(PCMCIA_POR_PSST_MASK
+				     | PCMCIA_POR_PSL_MASK
+				     | PCMCIA_POR_PSHT_MASK);
+	/* And then set the register */
+	_reg_PCMCIA_POR(window) |= code;
+
+	/* Enable the window */
+	_reg_PCMCIA_POR(window) |= PCMCIA_POR_PV;
+
+	return 0;
+}
+
+static unsigned short calc_speed(unsigned short *spds, int num,
+				 unsigned short dflt)
+{
+	unsigned short speed = 0;
+	int i;
+
+	for (i = 0; i < num; i++)
+		if (speed < spds[i])
+			speed = spds[i];
+	if (speed == 0)
+		speed = dflt;
+
+	return speed;
+}
+
+static void
+mx31ads_common_pcmcia_get_timing(struct mx31ads_pcmcia_socket *skt,
+				 struct mx31ads_pcmcia_timing *timing)
+{
+	timing->io = calc_speed(skt->spd_io, MAX_IO_WIN, PCMCIA_IO_ACCESS);
+	timing->mem = calc_speed(skt->spd_mem, MAX_WIN, PCMCIA_3V_MEM_ACCESS);
+	timing->attr =
+	    calc_speed(skt->spd_attr, MAX_WIN, PCMCIA_ATTR_MEM_ACCESS);
+}
+
+static int mx31ads_pcmcia_set_timing(struct mx31ads_pcmcia_socket *skt)
+{
+	u_int clk_ns;
+	struct mx31ads_pcmcia_timing timing;
+
+	/* How many nanoseconds */
+	clk_ns = (1000 * 1000 * 1000) / clk_get_rate(skt->clk);
+	pr_debug(KERN_INFO "clk_ns = %d\n", clk_ns);
+
+	mx31ads_common_pcmcia_get_timing(skt, &timing);
+	pr_debug(KERN_INFO "timing: io %d, mem %d, attr %d\n", timing.io,
+		 timing.mem, timing.attr);
+
+	mx31ads_pcmcia_set_window_timing(timing.io, IO_WINDOW, clk_ns);
+	mx31ads_pcmcia_set_window_timing(timing.mem, COMMON_MEMORY_WINDOW,
+					 clk_ns);
+	mx31ads_pcmcia_set_window_timing(timing.attr, ATTRIBUTE_MEMORY_WINDOW,
+					 clk_ns);
+
+	return 0;
+}
+
+static int mx31ads_pcmcia_show_timing(struct mx31ads_pcmcia_socket *skt,
+				      char *buf)
+{
+	return 0;
+}
+
+static struct pcmcia_low_level mx31ads_pcmcia_ops = {
+	.owner = THIS_MODULE,
+	.hw_init = mx31ads_pcmcia_hw_init,
+	.hw_shutdown = mx31ads_pcmcia_hw_shutdown,
+	.socket_state = mx31ads_pcmcia_socket_state,
+	.configure_socket = mx31ads_pcmcia_configure_socket,
+
+	.socket_init = mx31ads_pcmcia_socket_init,
+	.socket_suspend = mx31ads_pcmcia_socket_suspend,
+
+	.get_timing = mx31ads_pcmcia_default_mecr_timing,
+	.set_timing = mx31ads_pcmcia_set_timing,
+	.show_timing = mx31ads_pcmcia_show_timing,
+};
+
+/* =================================================================================== */
+
+LIST_HEAD(mx31ads_pcmcia_sockets);
+DECLARE_MUTEX(mx31ads_pcmcia_sockets_lock);
+
+static DEFINE_SPINLOCK(status_lock);
+
+struct bittbl {
+	unsigned int mask;
+	const char *name;
+};
+
+static struct bittbl status_bits[] = {
+	{SS_WRPROT, "SS_WRPROT"},
+	{SS_BATDEAD, "SS_BATDEAD"},
+	{SS_BATWARN, "SS_BATWARN"},
+	{SS_READY, "SS_READY"},
+	{SS_DETECT, "SS_DETECT"},
+	{SS_POWERON, "SS_POWERON"},
+	{SS_STSCHG, "SS_STSCHG"},
+	{SS_3VCARD, "SS_3VCARD"},
+	{SS_XVCARD, "SS_XVCARD"},
+};
+
+static struct bittbl conf_bits[] = {
+	{SS_PWR_AUTO, "SS_PWR_AUTO"},
+	{SS_IOCARD, "SS_IOCARD"},
+	{SS_RESET, "SS_RESET"},
+	{SS_DMA_MODE, "SS_DMA_MODE"},
+	{SS_SPKR_ENA, "SS_SPKR_ENA"},
+	{SS_OUTPUT_ENA, "SS_OUTPUT_ENA"},
+};
+
+static void
+dump_bits(char **p, const char *prefix, unsigned int val, struct bittbl *bits,
+	  int sz)
+{
+	char *b = *p;
+	int i;
+
+	b += sprintf(b, "%-9s:", prefix);
+	for (i = 0; i < sz; i++)
+		if (val & bits[i].mask)
+			b += sprintf(b, " %s", bits[i].name);
+	*b++ = '\n';
+	*p = b;
+}
+
+/*
+ * Implements the /sys/class/pcmcia_socket/??/status file.
+ *
+ * Returns: the number of characters added to the buffer
+ */
+static ssize_t show_status(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct mx31ads_pcmcia_socket *skt =
+	    container_of(dev, struct mx31ads_pcmcia_socket, socket.dev);
+	char *p = buf;
+
+	p += sprintf(p, "slot     : %d\n", skt->nr);
+
+	dump_bits(&p, "status", skt->status,
+		  status_bits, ARRAY_SIZE(status_bits));
+	dump_bits(&p, "csc_mask", skt->cs_state.csc_mask,
+		  status_bits, ARRAY_SIZE(status_bits));
+	dump_bits(&p, "cs_flags", skt->cs_state.flags,
+		  conf_bits, ARRAY_SIZE(conf_bits));
+
+	p += sprintf(p, "Vcc      : %d\n", skt->cs_state.Vcc);
+	p += sprintf(p, "Vpp      : %d\n", skt->cs_state.Vpp);
+	p += sprintf(p, "IRQ      : %d (%d)\n", skt->cs_state.io_irq, skt->irq);
+	if (skt->ops->show_timing)
+		p += skt->ops->show_timing(skt, p);
+
+	return p - buf;
+}
+
+static DEVICE_ATTR(status, S_IRUGO, show_status, NULL);
+
+static void mx31ads_common_check_status(struct mx31ads_pcmcia_socket *skt)
+{
+	unsigned int events;
+
+	pr_debug(KERN_INFO "entering PCMCIA monitoring thread\n");
+
+	do {
+		unsigned int status;
+		unsigned long flags;
+
+		status = mx31ads_common_pcmcia_skt_state(skt);
+
+		spin_lock_irqsave(&status_lock, flags);
+		events = (status ^ skt->status) & skt->cs_state.csc_mask;
+		skt->status = status;
+		spin_unlock_irqrestore(&status_lock, flags);
+
+		pr_debug(KERN_INFO "events: %s%s%s%s%s%s\n",
+			 events == 0 ? "<NONE>" : "",
+			 events & SS_DETECT ? "DETECT " : "",
+			 events & SS_READY ? "READY " : "",
+			 events & SS_BATDEAD ? "BATDEAD " : "",
+			 events & SS_BATWARN ? "BATWARN " : "",
+			 events & SS_STSCHG ? "STSCHG " : "");
+
+		if (events)
+			pcmcia_parse_events(&skt->socket, events);
+	} while (events);
+}
+
+/*
+ * Service routine for socket driver interrupts (requested by the
+ * low-level PCMCIA init() operation via mx31ads_common_pcmcia_thread()).
+ * The actual interrupt-servicing work is performed by
+ * mx31ads_common_pcmcia_thread(), largely because the Card Services event-
+ * handling code performs scheduling operations which cannot be
+ * executed from within an interrupt context.
+ */
+static irqreturn_t mx31ads_common_pcmcia_interrupt(int irq, void *dev)
+{
+	struct mx31ads_pcmcia_socket *skt = dev;
+	volatile u32 pscr, pgsr;
+
+	dev_dbg(dev, "servicing IRQ %d\n", irq);
+
+	/* clear interrupt states */
+	pscr = _reg_PCMCIA_PSCR;
+	_reg_PCMCIA_PSCR = pscr;
+
+	pgsr = _reg_PCMCIA_PGSR;
+	_reg_PCMCIA_PGSR = pgsr;
+
+	mx31ads_common_check_status(skt);
+
+	return IRQ_HANDLED;
+}
+
+/* Let's poll for events in addition to IRQs since IRQ only is unreliable... */
+static void mx31ads_common_pcmcia_poll_event(unsigned long dummy)
+{
+	struct mx31ads_pcmcia_socket *skt =
+	    (struct mx31ads_pcmcia_socket *)dummy;
+	pr_debug(KERN_INFO "polling for events\n");
+
+	mod_timer(&skt->poll_timer, jiffies + PCMCIA_POLL_PERIOD);
+
+	mx31ads_common_check_status(skt);
+}
+
+#define mx31ads_pcmcia_cpufreq_register()
+#define mx31ads_pcmcia_cpufreq_unregister()
+
+static int mx31ads_common_drv_pcmcia_probe(struct platform_device *pdev,
+					   struct pcmcia_low_level *ops)
+{
+	struct mx31ads_pcmcia_socket *skt;
+	int vs, value, ret;
+	struct pccard_io_map map;
+
+	down(&mx31ads_pcmcia_sockets_lock);
+
+	skt = kzalloc(sizeof(struct mx31ads_pcmcia_socket), GFP_KERNEL);
+	if (!skt) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/*
+	 * Initialise the socket structure.
+	 */
+	skt->socket.ops = &mx31ads_common_pcmcia_operations;
+	skt->socket.owner = ops->owner;
+	skt->socket.driver_data = skt;
+
+	init_timer(&skt->poll_timer);
+	skt->poll_timer.function = mx31ads_common_pcmcia_poll_event;
+	skt->poll_timer.data = (unsigned long)skt;
+	skt->poll_timer.expires = jiffies + PCMCIA_POLL_PERIOD;
+
+	skt->irq = MX31ADS_PCMCIA_IRQ;
+	skt->socket.dev.parent = &pdev->dev;
+	skt->ops = ops;
+
+	skt->clk = clk_get(NULL, "ahb_clk");
+
+	skt->res_skt.start = _PCMCIA(0);
+	skt->res_skt.end = _PCMCIA(0) + PCMCIASp - 1;
+	skt->res_skt.name = MX31ADS_PCMCIA;
+	skt->res_skt.flags = IORESOURCE_MEM;
+
+	ret = request_resource(&iomem_resource, &skt->res_skt);
+	if (ret)
+		goto out_err_1;
+
+	skt->res_io.start = _PCMCIAIO(0);
+	skt->res_io.end = _PCMCIAIO(0) + PCMCIAIOSp - 1;
+	skt->res_io.name = "io";
+	skt->res_io.flags = IORESOURCE_MEM | IORESOURCE_BUSY;
+
+	ret = request_resource(&skt->res_skt, &skt->res_io);
+	if (ret)
+		goto out_err_2;
+
+	skt->res_mem.start = _PCMCIAMem(0);
+	skt->res_mem.end = _PCMCIAMem(0) + PCMCIAMemSp - 1;
+	skt->res_mem.name = "memory";
+	skt->res_mem.flags = IORESOURCE_MEM;
+
+	ret = request_resource(&skt->res_skt, &skt->res_mem);
+	if (ret)
+		goto out_err_3;
+
+	skt->res_attr.start = _PCMCIAAttr(0);
+	skt->res_attr.end = _PCMCIAAttr(0) + PCMCIAAttrSp - 1;
+	skt->res_attr.name = "attribute";
+	skt->res_attr.flags = IORESOURCE_MEM;
+
+	ret = request_resource(&skt->res_skt, &skt->res_attr);
+	if (ret)
+		goto out_err_4;
+
+	skt->virt_io = ioremap(skt->res_io.start, 0x10000);
+	if (skt->virt_io == NULL) {
+		ret = -ENOMEM;
+		goto out_err_5;
+	}
+
+	if (list_empty(&mx31ads_pcmcia_sockets))
+		mx31ads_pcmcia_cpufreq_register();
+
+	list_add(&skt->node, &mx31ads_pcmcia_sockets);
+
+	/*
+	 * We initialize default socket timing here, because
+	 * we are not guaranteed to see a SetIOMap operation at
+	 * runtime.
+	 */
+	ops->set_timing(skt);
+
+	ret = ops->hw_init(skt);
+	if (ret)
+		goto out_err_6;
+
+	ret = request_irq(skt->irq, mx31ads_common_pcmcia_interrupt,
+			  IRQF_SHARED | IRQF_DISABLED, "PCMCIA IRQ", skt);
+	if (ret)
+		goto out_err_6;
+
+	skt->socket.features = SS_CAP_STATIC_MAP | SS_CAP_PCCARD;
+	skt->socket.resource_ops = &pccard_static_ops;
+	skt->socket.irq_mask = 0;
+	skt->socket.map_size = PCMCIAPrtSp;
+	skt->socket.pci_irq = skt->irq;
+	skt->socket.io_offset = (unsigned long)skt->virt_io;
+
+	skt->status = mx31ads_common_pcmcia_skt_state(skt);
+	skt->pre_stat = 0;
+	ret = pcmcia_register_socket(&skt->socket);
+	if (ret)
+		goto out_err_7;
+	/* FIXED ME  workaround for binding with ide-cs. ide usage io port 0x100~0x107 and 0x10e */
+	map.map = 0;
+	map.flags = MAP_ACTIVE | MAP_16BIT;
+	map.start = 0;
+	map.stop = PCMCIAIOSp - 1;
+	map.speed = 0;
+	mx31ads_common_pcmcia_set_io_map(&skt->socket, &map);
+
+	vs = _reg_PCMCIA_PIPR & PCMCIA_PIPR_VS;
+	value = vs & PCMCIA_PIPR_VS_5V ? 50 : 33;
+	dev_dbg(&pdev->dev, "PCMCIA: Voltage the card supports: %d.%dV\n",
+		value / 10, value % 10);
+
+	add_timer(&skt->poll_timer);
+
+	ret = device_create_file(&skt->socket.dev, &dev_attr_status);
+	if (ret < 0)
+		goto out_err_8;
+
+	platform_set_drvdata(pdev, skt);
+	ret = 0;
+	goto out;
+
+      out_err_8:
+	del_timer_sync(&skt->poll_timer);
+	pcmcia_unregister_socket(&skt->socket);
+
+      out_err_7:
+	flush_scheduled_work();
+	free_irq(skt->irq, skt);
+	ops->hw_shutdown(skt);
+      out_err_6:
+	list_del(&skt->node);
+	iounmap(skt->virt_io);
+      out_err_5:
+	release_resource(&skt->res_attr);
+      out_err_4:
+	release_resource(&skt->res_mem);
+      out_err_3:
+	release_resource(&skt->res_io);
+      out_err_2:
+	release_resource(&skt->res_skt);
+      out_err_1:
+
+	kfree(skt);
+      out:
+	up(&mx31ads_pcmcia_sockets_lock);
+	return ret;
+}
+
+static int mx31ads_drv_pcmcia_remove(struct platform_device *pdev)
+{
+	struct mx31ads_pcmcia_socket *skt = platform_get_drvdata(pdev);
+
+	platform_set_drvdata(pdev, NULL);
+
+	down(&mx31ads_pcmcia_sockets_lock);
+
+	del_timer_sync(&skt->poll_timer);
+
+	pcmcia_unregister_socket(&skt->socket);
+
+	flush_scheduled_work();
+
+	skt->ops->hw_shutdown(skt);
+
+	mx31ads_common_pcmcia_config_skt(skt, &dead_socket);
+
+	list_del(&skt->node);
+	iounmap(skt->virt_io);
+	skt->virt_io = NULL;
+	release_resource(&skt->res_attr);
+	release_resource(&skt->res_mem);
+	release_resource(&skt->res_io);
+	release_resource(&skt->res_skt);
+
+	if (list_empty(&mx31ads_pcmcia_sockets))
+		mx31ads_pcmcia_cpufreq_unregister();
+
+	up(&mx31ads_pcmcia_sockets_lock);
+
+	kfree(skt);
+
+	return 0;
+}
+
+static int mx31ads_drv_pcmcia_probe(struct platform_device *pdev)
+{
+	if (!machine_is_mx31ads())
+		return -ENODEV;
+
+	return mx31ads_common_drv_pcmcia_probe(pdev, &mx31ads_pcmcia_ops);
+}
+
+static int mx31ads_drv_pcmcia_suspend(struct platform_device *pdev,
+				      pm_message_t state)
+{
+	return pcmcia_socket_dev_suspend(&pdev->dev, state);
+}
+
+static int mx31ads_drv_pcmcia_resume(struct platform_device *pdev)
+{
+	return pcmcia_socket_dev_resume(&pdev->dev);
+}
+
+/*
+ * Low level functions
+ */
+static struct platform_driver mx31ads_pcmcia_driver = {
+	.driver = {
+		   .name = MX31ADS_PCMCIA,
+		   },
+	.probe = mx31ads_drv_pcmcia_probe,
+	.remove = mx31ads_drv_pcmcia_remove,
+	.suspend = mx31ads_drv_pcmcia_suspend,
+	.resume = mx31ads_drv_pcmcia_resume,
+};
+
+/* mx31ads_pcmcia_init()
+ *
+ */
+static int __init mx31ads_pcmcia_init(void)
+{
+	int ret;
+
+	if ((ret = platform_driver_register(&mx31ads_pcmcia_driver)))
+		return ret;
+	pr_debug(KERN_INFO "PCMCIA: Initialize i.Mx31 pcmcia socket\n");
+
+	return ret;
+}
+
+/* mx31ads_pcmcia_exit()
+ *
+ */
+static void __exit mx31ads_pcmcia_exit(void)
+{
+	platform_driver_unregister(&mx31ads_pcmcia_driver);
+}
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("i.MX31 PCMCIA Socket Controller");
+MODULE_LICENSE("GPL");
+
+module_init(mx31ads_pcmcia_init);
+module_exit(mx31ads_pcmcia_exit);
diff --git a/drivers/pcmcia/mx31ads-pcmcia.h b/drivers/pcmcia/mx31ads-pcmcia.h
new file mode 100644
index 000000000000..3f7014b3e8bf
--- /dev/null
+++ b/drivers/pcmcia/mx31ads-pcmcia.h
@@ -0,0 +1,155 @@
+/*
+ * linux/drivers/pcmcia/mx31ads-pcmcia.h
+ *
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This file contains definitions for the PCMCIA support code common to
+ * integrated SOCs like the i.Mx31 microprocessors.
+ */
+#ifndef _ASM_ARCH_PCMCIA
+#define _ASM_ARCH_PCMCIA
+
+/* include the world */
+#include <linux/cpufreq.h>
+#include <pcmcia/cs_types.h>
+#include <pcmcia/cs.h>
+#include <pcmcia/ss.h>
+#include <pcmcia/cistpl.h>
+#include "cs_internal.h"
+
+#define	MX31ADS_PCMCIA	"Mx31ads_pcmcia_socket"
+
+struct device;
+struct pcmcia_low_level;
+
+/*
+ * This structure encapsulates per-socket state which we might need to
+ * use when responding to a Card Services query of some kind.
+ */
+struct mx31ads_pcmcia_socket {
+	struct pcmcia_socket socket;
+
+	/*
+	 * Info from low level handler
+	 */
+	struct device *dev;
+	unsigned int nr;
+	unsigned int irq;
+
+	struct clk *clk;
+
+	/*
+	 * Core PCMCIA state
+	 */
+	struct pcmcia_low_level *ops;
+
+	unsigned int status;
+	unsigned int pre_stat;
+	socket_state_t cs_state;
+
+	unsigned short spd_io[MAX_IO_WIN];
+	unsigned short spd_mem[MAX_WIN];
+	unsigned short spd_attr[MAX_WIN];
+
+	struct resource res_skt;
+	struct resource res_io;
+	struct resource res_mem;
+	struct resource res_attr;
+	void *virt_io;
+
+	unsigned int irq_state;
+
+	struct timer_list poll_timer;
+	struct list_head node;
+};
+
+struct pcmcia_state {
+	unsigned detect:1,
+	    ready:1, bvd1:1, bvd2:1, wrprot:1, vs_3v:1, vs_Xv:1, poweron:1;
+};
+
+struct pcmcia_low_level {
+	struct module *owner;
+
+	/* first socket in system */
+	int first;
+	/* nr of sockets */
+	int nr;
+
+	int (*hw_init) (struct mx31ads_pcmcia_socket *);
+	void (*hw_shutdown) (struct mx31ads_pcmcia_socket *);
+
+	void (*socket_state) (struct mx31ads_pcmcia_socket *,
+			      struct pcmcia_state *);
+	int (*configure_socket) (struct mx31ads_pcmcia_socket *,
+				 const socket_state_t *);
+
+	/*
+	 * Enable card status IRQs on (re-)initialisation.  This can
+	 * be called at initialisation, power management event, or
+	 * pcmcia event.
+	 */
+	void (*socket_init) (struct mx31ads_pcmcia_socket *);
+
+	/*
+	 * Disable card status IRQs and PCMCIA bus on suspend.
+	 */
+	void (*socket_suspend) (struct mx31ads_pcmcia_socket *);
+
+	/*
+	 * Hardware specific timing routines.
+	 * If provided, the get_timing routine overrides the SOC default.
+	 */
+	unsigned int (*get_timing) (struct mx31ads_pcmcia_socket *,
+				    unsigned int, unsigned int);
+	int (*set_timing) (struct mx31ads_pcmcia_socket *);
+	int (*show_timing) (struct mx31ads_pcmcia_socket *, char *);
+
+#ifdef CONFIG_CPU_FREQ
+	/*
+	 * CPUFREQ support.
+	 */
+	int (*frequency_change) (struct mx31ads_pcmcia_socket *, unsigned long,
+				 struct cpufreq_freqs *);
+#endif
+};
+
+struct mx31ads_pcmcia_timing {
+	unsigned short io;
+	unsigned short mem;
+	unsigned short attr;
+};
+
+typedef struct {
+	ulong win_size;
+	int bsize;
+} bsize_map_t;
+
+/*
+ * The PC Card Standard, Release 7, section 4.13.4, says that twIORD
+ * has a minimum value of 165ns. Section 4.13.5 says that twIOWR has
+ * a minimum value of 165ns, as well. Section 4.7.2 (describing
+ * common and attribute memory write timing) says that twWE has a
+ * minimum value of 150ns for a 250ns cycle time (for 5V operation;
+ * see section 4.7.4), or 300ns for a 600ns cycle time (for 3.3V
+ * operation, also section 4.7.4). Section 4.7.3 says that taOE
+ * has a maximum value of 150ns for a 300ns cycle time (for 5V
+ * operation), or 300ns for a 600ns cycle time (for 3.3V operation).
+ *
+ * When configuring memory maps, Card Services appears to adopt the policy
+ * that a memory access time of "0" means "use the default." The default
+ * PCMCIA I/O command width time is 165ns. The default PCMCIA 5V attribute
+ * and memory command width time is 150ns; the PCMCIA 3.3V attribute and
+ * memory command width time is 300ns.
+ */
+#define PCMCIA_IO_ACCESS		(165)
+#define PCMCIA_5V_MEM_ACCESS	(150)
+#define PCMCIA_3V_MEM_ACCESS	(300)
+#define PCMCIA_ATTR_MEM_ACCESS	(300)
+
+/*
+ * The socket driver actually works nicely in interrupt-driven form,
+ * so the (relatively infrequent) polling is "just to be sure."
+ */
+#define PCMCIA_POLL_PERIOD    (2*HZ)
+#endif
diff --git a/drivers/pcmcia/s3c2443_pcmcia.c b/drivers/pcmcia/s3c2443_pcmcia.c
new file mode 100755
index 000000000000..4ee321bf0e78
--- /dev/null
+++ b/drivers/pcmcia/s3c2443_pcmcia.c
@@ -0,0 +1,491 @@
+/* linux/drivers/pcmcia/s3c2443_pcmcia.c
+ *
+ * Copyright (c) 2009 Digi Internationa Inc.
+ * http://www.digi.com
+ * 
+ * Based on similar driver for other ARM SoC, like the at91_cf.c, from
+ * David Brownell and other authors.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/cpufreq.h>
+#include <linux/ioport.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+
+#include <pcmcia/cs_types.h>
+#include <pcmcia/ss.h>
+#include <pcmcia/cistpl.h>
+
+#include <plat/pcmcia.h>
+#include <mach/regs-cfata.h>
+#include <mach/regs-bus.h>
+#include <mach/gpio.h>
+
+#define DRV_NAME	"s3c2443-pcmcia"
+#define	SZ_2K		(2 * SZ_1K)
+
+struct s3c2443_pcmcia_socket {
+	struct pcmcia_socket		socket;
+	struct platform_device		*pdev;
+	struct s3c2443_pcmcia_pdata	*pdata;
+	unsigned			present:1;
+	int				irq;
+	int				irq_cd;
+	unsigned long			phys_io;
+	unsigned long			phys_attr;
+	unsigned long			phys_comm;
+	void __iomem			*pcmcia_base;
+};
+
+static inline int s3c2443_pcmcia_present(struct s3c2443_pcmcia_socket *psock)
+{
+	return s3c2410_gpio_getpin(psock->pdata->gpio_detect) ? 0 : 1;
+}
+
+static int s3c2443_pcmcia_socket_init(struct pcmcia_socket *s)
+{
+	return 0;
+}
+
+static irqreturn_t s3c2443_pcmcia_irq_cd(int irq, void *data)
+{
+	struct s3c2443_pcmcia_socket *psock = data;
+	unsigned present;
+
+	present = s3c2443_pcmcia_present(psock);
+	if (present != psock->present) {
+		psock->present = present;
+		pr_debug("%s: card %s\n", DRV_NAME,
+			 present ? "present" : "gone");
+
+		pcmcia_parse_events(&psock->socket, SS_DETECT);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t s3c2443_pcmcia_irq(int irq, void *data)
+{
+	struct s3c2443_pcmcia_socket *psock = data;
+	irqreturn_t ret = IRQ_NONE;
+	u32 irqreg;
+
+	/* read interrupt sources and acknowledge irqs */
+	irqreg = readl(psock->pcmcia_base + S3C2443_PCCARD_INT);
+	writel(irqreg, psock->pcmcia_base + S3C2443_PCCARD_INT);
+
+	/* Mask before checking the interrupt source */
+	irqreg = irqreg & ~(irqreg >> 8) & 0x07;
+
+	if (irqreg) {
+		ret = IRQ_HANDLED;
+
+		if (irqreg & S3C2443_PCC_INTSRC_IREQ)
+			pcmcia_parse_events(&psock->socket, SS_STSCHG);
+		if (irqreg & S3C2443_PCC_INTSRC_ERR_N)
+			printk(KERN_WARNING "%s: %s, error interrupt\n",
+			       DRV_NAME, __func__);
+		if (irqreg & S3C2443_PCC_INTSRC_CD)
+			printk(KERN_WARNING "%s: %s, card detect irq ???\n",
+			       DRV_NAME, __func__);
+	}
+
+	return ret;
+}
+
+static int s3c2443_pcmcia_get_status(struct pcmcia_socket *s, u_int *sp)
+{
+	struct s3c2443_pcmcia_socket *psock;
+	u32 muxreg;
+
+	if (!sp)
+		return -EINVAL;
+
+	psock = container_of(s, struct s3c2443_pcmcia_socket, socket);
+
+	/* NOTE: CF is always 3VCARD */
+	if (s3c2443_pcmcia_present(psock)) {
+		*sp = SS_READY | SS_DETECT | SS_3VCARD;
+
+		muxreg = readl(psock->pcmcia_base + S3C2443_MUX_REG);
+		if (!(muxreg & S3C2443_MUX_PWREN_PWOFF))
+			*sp |= SS_POWERON;
+
+		s->irq.AssignedIRQ = 0;
+		s->pci_irq = psock->irq;
+	} else
+		*sp = 0;
+
+	return 0;
+}
+
+static int
+s3c2443_pcmcia_set_socket(struct pcmcia_socket *sock, struct socket_state_t *s)
+{
+	struct s3c2443_pcmcia_socket *psock = 
+		      container_of(sock, struct s3c2443_pcmcia_socket, socket);
+	u32 muxreg;
+	u32 cfg;
+
+	muxreg = readl(psock->pcmcia_base + S3C2443_MUX_REG);
+	muxreg &= ~S3C2443_MUX_PWREN_PWOFF;
+	cfg = readl(psock->pcmcia_base + S3C2443_PCCARD_CFG);
+	cfg &= ~S3C2443_PCC_CARD_RESET;
+
+	switch (s->Vcc) {
+	case 0:
+		writel(muxreg | S3C2443_MUX_PWREN_PWOFF,
+		       psock->pcmcia_base + S3C2443_MUX_REG);
+		break;
+	case 33:
+		writel(muxreg | S3C2443_MUX_PWREN_PWON,
+		       psock->pcmcia_base + S3C2443_MUX_REG);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (s->flags & SS_RESET)
+		cfg |= S3C2443_PCC_CARD_RESET;
+
+	writel(cfg, psock->pcmcia_base + S3C2443_PCCARD_CFG);
+	pr_debug("%s: Vcc %d, io_irq %d, flags %04x csc %04x\n",
+		DRV_NAME, s->Vcc, s->io_irq, s->flags, s->csc_mask);
+
+	return 0;
+}
+
+static int s3c2443_pcmcia_suspend(struct pcmcia_socket *s)
+{
+	return s3c2443_pcmcia_set_socket(s, &dead_socket);
+}
+
+/* we already mapped the I/O region */
+static int s3c2443_pcmcia_set_io_map(struct pcmcia_socket *s, struct pccard_io_map *io)
+{
+	struct s3c2443_pcmcia_socket *psock;
+	u32 cfg;
+
+	psock = container_of(s, struct s3c2443_pcmcia_socket, socket);
+	io->flags &= MAP_ACTIVE | MAP_16BIT | MAP_AUTOSZ;
+
+	/* Use 16 bit accesses */
+	cfg = readl(psock->pcmcia_base + S3C2443_PCCARD_CFG);
+	cfg |= S3C2443_PCC_DEVICE_IO_16;
+	writel(cfg, psock->pcmcia_base + S3C2443_PCCARD_CFG);
+
+	io->start = psock->socket.io_offset;
+	io->stop = io->start + SZ_2K - 1;
+
+	return 0;
+}
+
+/* pcmcia layer maps/unmaps mem regions */
+static int
+s3c2443_pcmcia_set_mem_map(struct pcmcia_socket *s, struct pccard_mem_map *map)
+{
+	struct s3c2443_pcmcia_socket *psock;
+
+	if (map->card_start)
+		return -EINVAL;
+
+	psock = container_of(s, struct s3c2443_pcmcia_socket, socket);
+
+	map->flags &= (MAP_ACTIVE | MAP_ATTRIB | MAP_16BIT);
+	map->static_start = (map->flags & MAP_ATTRIB) ? 
+			    psock->phys_attr : psock->phys_comm;
+
+	return 0;
+}
+
+static struct pccard_operations s3c2443_pcmcia_ops = {
+	.init			= s3c2443_pcmcia_socket_init,
+	.suspend		= s3c2443_pcmcia_suspend,
+	.get_status		= s3c2443_pcmcia_get_status,
+	.set_socket		= s3c2443_pcmcia_set_socket,
+	.set_io_map		= s3c2443_pcmcia_set_io_map,
+	.set_mem_map		= s3c2443_pcmcia_set_mem_map,
+};
+
+
+static int s3c2443_pcmcia_hw_config(struct s3c2443_pcmcia_socket *psock)
+{
+	void __iomem *ebicon;
+	int ret = 0;
+	u32 reg;
+
+	ebicon = ioremap(S3C2443_PA_EBI + 0x8, 0x4);
+	if (ebicon == NULL) {
+		ret =-EBUSY;
+		goto err_unmap;
+	}
+
+	/* Configure EBI bank 2 and 3 for the CF interface */
+	reg = readl(ebicon);
+	reg |= S3C2443_EBICON_BANK3_CFG | S3C2443_EBICON_BANK2_CFG;
+	writel(reg, ebicon);
+
+	/* Configure the IO lines*/
+	s3c2443_gpio_cfgpin(S3C2410_GPG15, S3C2443_GPG15_CF_PWR);
+	s3c2443_gpio_cfgpin(S3C2410_GPG14, S3C2443_GPG14_CF_RESET);
+	s3c2443_gpio_cfgpin(S3C2410_GPG13, S3C2443_GPG13_CF_nREG);
+	s3c2443_gpio_cfgpin(S3C2410_GPG12, S3C2443_GPG12_nINPACK);
+	s3c2443_gpio_cfgpin(S3C2410_GPG11, S3C2443_GPG11_CF_nIREQ);
+
+	s3c2443_gpio_cfgpin(S3C2410_GPA10, 0);
+	s3c2443_gpio_cfgpin(S3C2410_GPA11, 1);
+	s3c2443_gpio_cfgpin(S3C2410_GPA12, 1);
+	s3c2443_gpio_cfgpin(S3C2410_GPA15, 1);
+
+	/* TODO use the proper api after adding s3c2443_gpio_pullup */  
+	/* Enable the pullup for interrupt line  */
+	reg = readl(S3C2410_GPGUP);
+	writel(reg & ~(3 << 22), S3C2410_GPGUP);
+
+	/* Clear the card detect condition */
+	reg = readl(S3C24XX_MISCCR) & ~S3C2443_MISCCR_nCD_CF;
+	writel(reg, S3C24XX_MISCCR);
+ 
+	/* Output Port enabled, Card power off, PCCARD mode */
+	writel(S3C2443_MUX_OUTPUT_ENABLE | S3C2443_MUX_PWREN_PWOFF | 
+	       S3C2443_MUX_MODE_PCCARD, psock->pcmcia_base + S3C2443_MUX_REG);
+
+	/* Acknowledge any pending interrupt and mask all irqs to get started */
+	writel(S3C2443_PCC_INTSRC_ALL, psock->pcmcia_base + S3C2443_PCCARD_INT);
+	writel(S3C2443_PCC_INTMSK_ALL, psock->pcmcia_base + S3C2443_PCCARD_INT);
+
+ 	writel(S3C2443_PCC_INTMSK_CD | S3C2443_PCC_INTSRC_ALL,
+ 	       psock->pcmcia_base + S3C2443_PCCARD_INT);
+
+	/* Configure timmings for IO, ATTR and COMM areas */
+	writel(0x061D04, psock->pcmcia_base + S3C2443_PCCARD_ATTR);
+	writel(0x031109, psock->pcmcia_base + S3C2443_PCCARD_IO);
+	writel(0x061D04, psock->pcmcia_base + S3C2443_PCCARD_COMM);
+
+err_unmap:
+	iounmap(ebicon);
+	return ret;
+}
+
+static int __init s3c2443_pcmcia_probe(struct platform_device *pdev)
+{
+	struct s3c2443_pcmcia_socket *psock;
+	struct s3c2443_pcmcia_pdata *pdata;
+	struct resource *res;
+	int ret;
+
+	pdata = pdev->dev.platform_data;
+
+	if (!pdata || !pdata->gpio_detect) {
+		pr_debug("%s: %s, platform data not available\n", DRV_NAME, __func__);
+		ret = -ENODEV;
+		goto error;
+	}
+
+	psock = kzalloc(sizeof(struct s3c2443_pcmcia_socket), GFP_KERNEL);
+	if (psock == NULL) {
+		pr_debug("%s: %s, can not allocate memory\n", DRV_NAME, __func__);
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	psock->irq = platform_get_irq(pdev, 0);
+	if (psock->irq < 0) {
+		pr_debug("%s: %s, can not get IORESOURCE_IRQ\n", DRV_NAME, __func__);
+		ret =-ENOENT;
+		goto error_get_irq;
+	}
+	psock->socket.pci_irq = psock->irq;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		pr_debug("%s: %s, can not get IORESOURCE_MEM\n", DRV_NAME, __func__);
+		ret = -ENOENT;
+		goto error_get_irq;
+	}
+
+	psock->pdata = pdata;
+	psock->pdev = pdev;
+	platform_set_drvdata(pdev, psock);
+	psock->phys_io = res->start + S3C2443_IO_BASE;
+	psock->phys_attr = res->start + S3C2443_ATTR_BASE;
+
+	if (!request_mem_region(res->start, res->end - res->start + 1,
+				pdev->name)) {
+		pr_debug("%s: %s,request_mem_region failed\n", DRV_NAME, __func__);
+		ret = -EBUSY;
+		goto error_get_irq;
+	}
+
+	psock->socket.io_offset = (unsigned long)ioremap(psock->phys_io, SZ_2K);
+	if (!psock->socket.io_offset) {
+		pr_debug("%s: %s, can not map IO space at 0x%08x\n", 
+			 DRV_NAME, __func__, (unsigned int)psock->phys_io);
+		ret = -ENXIO;
+		goto error_map;
+	}
+
+	psock->pcmcia_base = ioremap(res->start, res->end - res->start + 1);
+	if (!psock->pcmcia_base) {
+		pr_debug("%s: %s, can not map IO space at 0x%08x\n", 
+			 DRV_NAME, __func__, (unsigned int)(res->start + S3C2443_MUX_REG));
+		ret = -ENXIO;
+		goto error_map2;
+	}
+
+	ret = s3c2443_pcmcia_hw_config(psock);
+	if (ret) {
+		pr_debug("%s: %s, failed to configure PCMCIA hardware\n", DRV_NAME, __func__);
+		goto error_hwcfg;
+	}
+
+	psock->irq_cd = s3c2410_gpio_getirq(pdata->gpio_detect);
+	if (psock->irq_cd < 0) {
+		pr_debug("%s: %s, not irq available for card detection\n", 
+			 DRV_NAME, __func__);
+		ret = -ENOENT;
+		goto error_irq_cd;
+	}
+
+	ret = request_irq(psock->irq_cd, s3c2443_pcmcia_irq_cd, 
+		IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING, DRV_NAME, psock);
+	if (ret < 0) {
+		pr_debug("%s: %s, can not request irq %d (ret %d)\n", 
+			DRV_NAME, __func__, psock->irq_cd, ret);
+		ret = -EBUSY;
+		goto error_irq_cd;
+	}
+
+	ret = request_irq(psock->irq, s3c2443_pcmcia_irq,
+				IRQF_SHARED, DRV_NAME, psock);
+	if (ret < 0) {
+		pr_debug("%s: %s, can not request irq %d (ret %d)\n", 
+			DRV_NAME, __func__, psock->irq, ret);
+		ret = -EBUSY;
+		goto error_req_irq;
+	}
+
+	psock->socket.owner = THIS_MODULE;
+	psock->socket.dev.parent = &pdev->dev;
+	psock->socket.ops = &s3c2443_pcmcia_ops;
+	psock->socket.resource_ops = &pccard_static_ops;
+	psock->socket.features = SS_CAP_PCCARD | SS_CAP_STATIC_MAP
+				| SS_CAP_MEM_ALIGN;
+	psock->socket.map_size = SZ_2K;
+	psock->socket.io[0].res = res;
+
+
+	ret = pcmcia_register_socket(&psock->socket);
+	if (ret < 0) {
+		pr_debug("%s: %s, can not register pcmcia socket\n", 
+			DRV_NAME, __func__);
+		goto error_reqister;
+	}
+
+	pr_info("%s: PCMCIA driver\n", DRV_NAME);
+	return 0;
+
+error_reqister:
+	free_irq(psock->irq, psock);
+error_req_irq:
+	free_irq(psock->irq_cd, psock);
+error_irq_cd:
+error_hwcfg:
+	iounmap(psock->pcmcia_base);
+error_map2:
+	iounmap((void __iomem *)psock->socket.io_offset);
+error_map:
+	release_mem_region(res->start, res->end - res->start + 1);
+error_get_irq:
+	kfree(psock);
+error:
+	return ret;
+}
+
+static int __exit s3c2443_pcmcia_remove(struct platform_device *pdev)
+{
+	struct s3c2443_pcmcia_socket *psock = platform_get_drvdata(pdev);
+	struct s3c2443_pcmcia_pdata *pdata = psock->pdata;
+	struct resource *res = psock->socket.io[0].res;
+
+	pcmcia_unregister_socket(&psock->socket);
+	iounmap((void __iomem *)psock->socket.io_offset);
+	iounmap(psock->pcmcia_base);
+	release_mem_region(res->start, res->end - res->start + 1);
+	free_irq(psock->irq, psock);
+	device_init_wakeup(&pdev->dev, 0);
+	
+	if (pdata->gpio_detect) {
+		free_irq(psock->irq_cd, psock);
+		gpio_free(pdata->gpio_detect);
+	}
+	kfree(psock);
+
+	return 0;
+}
+
+#ifdef	CONFIG_PM
+static int 
+s3c2443_pcmcia_drv_suspend(struct platform_device *pdev, pm_message_t mesg)
+{
+	return pcmcia_socket_dev_suspend(&pdev->dev, mesg);
+}
+
+static int
+s3c2443_pcmcia_drv_resume(struct platform_device *pdev)
+{
+ 	struct s3c2443_pcmcia_socket *psock = platform_get_drvdata(pdev);
+ 	int ret;
+
+ 	ret = s3c2443_pcmcia_hw_config(psock);
+ 	if (ret)
+ 		return ret;
+
+	return pcmcia_socket_dev_resume(&pdev->dev);
+}
+#else
+#define	s3c2443_pcmcia_drv_suspend	NULL
+#define	s3c2443_pcmcia_drv_resume	NULL
+#endif
+
+static struct platform_driver s3c2443_pcmcia_driver = {
+	.probe		= s3c2443_pcmcia_probe,
+	.remove		= __exit_p(s3c2443_pcmcia_remove),
+	.suspend 	= s3c2443_pcmcia_drv_suspend,
+	.resume 	= s3c2443_pcmcia_drv_resume,
+	.driver		= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init s3c2443_pcmcia_init(void)
+{
+	return platform_driver_register(&s3c2443_pcmcia_driver);
+}
+
+static void __exit s3c2443_pcmcia_exit(void)
+{
+	platform_driver_unregister(&s3c2443_pcmcia_driver);
+}
+
+module_init(s3c2443_pcmcia_init);
+module_exit(s3c2443_pcmcia_exit);
+
+MODULE_AUTHOR("Pedro Perez de Heredia");
+MODULE_DESCRIPTION("S3C2443 PCMCIA core socket driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:s3c2443-pcmcia");
+
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 8e0c2b47803c..f1eb6b3f9cab 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -68,4 +68,11 @@ config BATTERY_BQ27x00
 	help
 	  Say Y here to enable support for batteries with BQ27200(I2C) chip.
 
+config BATTERY_STMP3XXX
+	tristate "Sigmatel STMP3xxx SoC battery charger driver"
+	depends on ARCH_STMP3XXX
+	help
+	  Say Y to enable support for the battery charger state machine
+	  for the Sigmatel STMP3xxx based SoC's.
+
 endif # POWER_SUPPLY
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index e8f1ecec5d8f..588e7d70f20f 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -23,3 +23,4 @@ obj-$(CONFIG_BATTERY_OLPC)	+= olpc_battery.o
 obj-$(CONFIG_BATTERY_TOSA)	+= tosa_battery.o
 obj-$(CONFIG_BATTERY_WM97XX)	+= wm97xx_battery.o
 obj-$(CONFIG_BATTERY_BQ27x00)	+= bq27x00_battery.o
+obj-$(CONFIG_BATTERY_STMP3XXX)	+= stmp37xx/
diff --git a/drivers/power/stmp37xx/Makefile b/drivers/power/stmp37xx/Makefile
new file mode 100644
index 000000000000..0d4f16e78ddf
--- /dev/null
+++ b/drivers/power/stmp37xx/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for the STMP3xxx battery charger driver
+#
+
+obj-$(CONFIG_BATTERY_STMP3XXX) += stmp3xxx-battery.o
+
+stmp3xxx-battery-objs := ddi_bc_api.o ddi_bc_hw.o ddi_bc_init.o \
+		         ddi_bc_ramp.o ddi_bc_sm.o ddi_power_battery.o linux.o
+
diff --git a/drivers/power/stmp37xx/ddi_bc_api.c b/drivers/power/stmp37xx/ddi_bc_api.c
new file mode 100644
index 000000000000..5fd062ec4eb9
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_bc_api.c
@@ -0,0 +1,566 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+//! \addtogroup ddi_bc
+//! @{
+//
+// Copyright (c) 2004-2005 SigmaTel, Inc.
+//
+//! \file       ddi_bc_api.c
+//! \brief      Contains the Battery Charger API.
+//! \date       06/2005
+//!
+//! This file contains Battery Charger API.
+//!
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+// Includes
+////////////////////////////////////////////////////////////////////////////////
+
+#include "ddi_bc_internal.h"
+
+////////////////////////////////////////////////////////////////////////////////
+// Variables
+////////////////////////////////////////////////////////////////////////////////
+
+//! This structure holds the current Battery Charger configuration.
+
+ddi_bc_Cfg_t g_ddi_bc_Configuration;
+
+extern uint32_t g_ddi_bc_u32StateTimer;
+extern ddi_bc_BrokenReason_t ddi_bc_gBrokenReason;
+extern bool bRestartChargeCycle;
+
+////////////////////////////////////////////////////////////////////////////////
+// Code
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the Battery Charger configuration.
+//!
+//! \fntype Function
+//!
+//! This function reports the Battery Charger configuration.
+//!
+//! Note that, if the Battery Charger has not yet been initialized, the data
+//! returned by this function is unknown.
+//!
+//! \param[in,out]  pCfg  A pointer to a structure that will receive the data.
+//!
+////////////////////////////////////////////////////////////////////////////////
+void ddi_bc_QueryCfg(ddi_bc_Cfg_t * pCfg)
+{
+
+	//--------------------------------------------------------------------------
+	// Return the current configuration.
+	//--------------------------------------------------------------------------
+
+	*pCfg = g_ddi_bc_Configuration;
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Shut down the Battery Charger.
+//!
+//! \fntype Function
+//!
+//! This function immediately shuts down the Battery Charger hardware and
+//! returns the state machine to the Uninitialized state. Use this function to
+//! safely “mummify” the battery charger before retiring it from memory.
+//!
+////////////////////////////////////////////////////////////////////////////////
+void ddi_bc_ShutDown()
+{
+
+	//--------------------------------------------------------------------------
+	// Reset the current ramp.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampReset();
+
+	//--------------------------------------------------------------------------
+	// Move to the Uninitialized state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_UNINITIALIZED;
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Advances the state machine.
+//!
+//! \fntype Function
+//!
+//! This function advances the state machine.
+//!
+//! \retval DDI_BC_STATUS_SUCCESS          If all goes well
+//! \retval DDI_BC_STATUS_NOT_INITIALIZED  If the Battery Charger is not yet
+//!                                        initialized.
+//! \retval DDI_BC_STATUS_BROKEN           If the battery violated a time-out
+//!                                        and has been declared broken.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_StateMachine()
+{
+
+	//--------------------------------------------------------------------------
+	// Check if we've been initialized yet.
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_State == DDI_BC_STATE_UNINITIALIZED) {
+		return (DDI_BC_STATUS_NOT_INITIALIZED);
+	}
+	//--------------------------------------------------------------------------
+	// Execute the function for the current state.
+	//--------------------------------------------------------------------------
+
+	return (stateFunctionTable[g_ddi_bc_State] ());
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Get the Battery Charger's current state.
+//!
+//! \fntype Function
+//!
+//! This function returns the current state.
+//!
+//! \retval The current state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_State_t ddi_bc_GetState()
+{
+	//--------------------------------------------------------------------------
+	// Return the current state.
+	//--------------------------------------------------------------------------
+
+	return (g_ddi_bc_State);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Disable the Battery Charger.
+//!
+//! \fntype Function
+//!
+//! This function forces the Battery Charger into the Disabled state.
+//!
+//! \retval DDI_BC_STATUS_SUCCESS          If all goes well
+//! \retval DDI_BC_STATUS_NOT_INITIALIZED  If the Battery Charger is not yet
+//!                                        initialized.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_SetDisable()
+{
+
+	//--------------------------------------------------------------------------
+	// Check if we've been initialized yet.
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_State == DDI_BC_STATE_UNINITIALIZED) {
+		return (DDI_BC_STATUS_NOT_INITIALIZED);
+	}
+	//--------------------------------------------------------------------------
+	// Check if we've been initialized yet.
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_State == DDI_BC_STATE_BROKEN) {
+		return (DDI_BC_STATUS_BROKEN);
+	}
+	//--------------------------------------------------------------------------
+	// Reset the current ramp. This will jam the current to zero and power off
+	// the charging hardware.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampReset();
+
+	//--------------------------------------------------------------------------
+	// Reset the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer = 0;
+
+	//--------------------------------------------------------------------------
+	// Move to the Disabled state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_DISABLED;
+
+	//--------------------------------------------------------------------------
+	// Return success.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Enable the Battery Charger.
+//!
+//! \fntype Function
+//!
+//! If the Battery Charger is in the Disabled state, this function moves it to
+//! the Waiting to Charge state.
+//!
+//! \retval DDI_BC_STATUS_SUCCESS          If all goes well
+//! \retval DDI_BC_STATUS_NOT_INITIALIZED  If the Battery Charger is not yet
+//!                                        initialized.
+//! \retval DDI_BC_STATUS_NOT_DISABLED     If the Battery Charger is not
+//!                                        disabled.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_SetEnable()
+{
+
+	//--------------------------------------------------------------------------
+	// Check if we've been initialized yet.
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_State == DDI_BC_STATE_UNINITIALIZED) {
+		return (DDI_BC_STATUS_NOT_INITIALIZED);
+	}
+	//--------------------------------------------------------------------------
+	// If we're not in the Disabled state, this is pointless.
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_State != DDI_BC_STATE_DISABLED) {
+		return (DDI_BC_STATUS_NOT_DISABLED);
+	}
+	//--------------------------------------------------------------------------
+	// Reset the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer = 0;
+	//--------------------------------------------------------------------------
+	// Move to the Waiting to Charge state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_WAITING_TO_CHARGE;
+
+	//--------------------------------------------------------------------------
+	// Return success.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Declare the battery to be broken.
+//!
+//! \fntype Function
+//!
+//! This function forces the Battery Charger into the Broken state.
+//!
+//! \retval DDI_BC_STATUS_SUCCESS          If all goes well
+//! \retval DDI_BC_STATUS_NOT_INITIALIZED  If the Battery Charger is not yet
+//!                                        initialized.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_SetBroken()
+{
+
+	//--------------------------------------------------------------------------
+	// Check if we've been initialized yet.
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_State == DDI_BC_STATE_UNINITIALIZED) {
+		return (DDI_BC_STATUS_NOT_INITIALIZED);
+	}
+	//--------------------------------------------------------------------------
+	// Reset the current ramp. This will jam the current to zero and power off
+	// the charging hardware.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampReset();
+
+	//--------------------------------------------------------------------------
+	// Reset the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer = 0;
+
+	//--------------------------------------------------------------------------
+	// Move to the Broken state.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_gBrokenReason = DDI_BC_BROKEN_CHARGING_TIMEOUT;
+
+	g_ddi_bc_State = DDI_BC_STATE_BROKEN;
+
+	//--------------------------------------------------------------------------
+	// Return success.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Declare the battery to be fixed.
+//!
+//! \fntype Function
+//!
+//! If the Battery Charger is in the Broken state, this function moves it to
+//! the Disabled state.
+//!
+//! \retval DDI_BC_STATUS_SUCCESS          If all goes well
+//! \retval DDI_BC_STATUS_NOT_INITIALIZED  If the Battery Charger is not yet
+//!                                        initialized.
+//! \retval DDI_BC_STATUS_NOT_BROKEN       If the Battery Charger is not broken.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_SetFixed()
+{
+
+	//--------------------------------------------------------------------------
+	// Check if we've been initialized yet.
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_State == DDI_BC_STATE_UNINITIALIZED) {
+		return (DDI_BC_STATUS_NOT_INITIALIZED);
+	}
+	//--------------------------------------------------------------------------
+	// If we're not in the Broken state, this is pointless.
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_State != DDI_BC_STATE_BROKEN) {
+		return (DDI_BC_STATUS_NOT_BROKEN);
+	}
+	//--------------------------------------------------------------------------
+	// Reset the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer = 0;
+
+	//--------------------------------------------------------------------------
+	// Unitialize the Broken Reason
+	//--------------------------------------------------------------------------
+
+	ddi_bc_gBrokenReason = DDI_BC_BROKEN_UNINITIALIZED;
+
+	//--------------------------------------------------------------------------
+	// Move to the Disabled state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_DISABLED;
+
+	//--------------------------------------------------------------------------
+	// Return success.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Set the current limit.
+//!
+//! \fntype Function
+//!
+//! This function applies a limit to the current that the Battery Charger can
+//! draw.
+//!
+//! \param[in]  u16Limit  The maximum current the Battery Charger can draw
+//!                       (in mA).
+//!
+//! \retval  The expressible version of the limit.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_SetCurrentLimit(uint16_t u16Limit)
+{
+
+	//--------------------------------------------------------------------------
+	// Set the limit and return what is actually expressible.
+	//--------------------------------------------------------------------------
+
+	return (ddi_bc_RampSetLimit(u16Limit));
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the current limit.
+//!
+//! \fntype Function
+//!
+//! This function reports the limit to the current that the Battery Charger can
+//! draw.
+//!
+//! \retval  The current limit.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_GetCurrentLimit(void)
+{
+
+	//--------------------------------------------------------------------------
+	// Set the limit and return what is actually expressible.
+	//--------------------------------------------------------------------------
+
+	return (ddi_bc_RampGetLimit());
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Set the battery charger state machine period.
+//!
+//! \fntype Function
+//!
+//! This function sets a new state machine period.  The Period and Slope should
+//! be coordinated to achieve the minimal ramp step current which will minimize
+//! transients on the system.
+//!
+//! \param[in]  u32StateMachinePeriod  (in milliseconds)
+//! \param[in]  u16CurrentRampSlope (in mA/s)
+//!
+//! \retval SUCCESS                        If all goes well
+//! \retval ERROR_DDI_BCM_NOT_INITIALIZED  If the Battery Charger is not yet
+//!                                        initialized.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_SetNewPeriodAndSlope(uint32_t u32StateMachinePeriod,
+					    uint16_t u16CurrentRampSlope)
+{
+	//--------------------------------------------------------------------------
+	// Check if we've been initialized yet.
+	//--------------------------------------------------------------------------
+	bool bDisableRequired;
+
+	if (g_ddi_bc_State == DDI_BC_STATE_UNINITIALIZED) {
+		return (DDI_BC_STATUS_NOT_INITIALIZED);
+	}
+
+	if (g_ddi_bc_State == DDI_BC_STATE_DISABLED)
+		bDisableRequired = false;
+	else {
+		bDisableRequired = true;
+		ddi_bc_SetDisable();
+	}
+
+	// Looking at the code, changing the period while the battery charger is running
+	// doesn't seem to have a negative affect.  One could wrap this in the mutex
+	// or implement further coordination if it did.
+	g_ddi_bc_Configuration.u32StateMachinePeriod = u32StateMachinePeriod;
+	g_ddi_bc_Configuration.u16CurrentRampSlope = u16CurrentRampSlope;
+
+	if (bDisableRequired)
+		ddi_bc_SetEnable();
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the state machine period.
+//!
+//! \fntype Function
+//!
+//! This function reports the battery charger period.
+//!
+//! \retval  The battery charger period (in milliseconds).
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint32_t ddi_bc_GetStateMachinePeriod()
+{
+	return (g_ddi_bc_Configuration.u32StateMachinePeriod);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the current ramp slope.
+//!
+//! \fntype Function
+//!
+//! This function reports the current ramp slope.
+//!
+//! \retval  The current ramp slope (in mA/s).
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint32_t ddi_bc_GetCurrentRampSlope()
+{
+	return (g_ddi_bc_Configuration.u16CurrentRampSlope);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the time spent in the present state (milliseconds)
+//!
+//! \fntype Function
+//!
+//! This function reports the time spent in the present charging state.  Note that
+//! for the states that actually charge the battery, this time does not include the
+//! time spent under alarm conditions such as die termperature alarm or battery
+//! temperature alarm.
+//!
+//! \retval  The time spent in the current state in milliseconds.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint32_t ddi_bc_GetStateTime(void)
+{
+	return g_ddi_bc_u32StateTimer;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the reason for being in the broken state
+//!
+//! \fntype Function
+//!
+//!
+//! \retval  ddi_bc_BrokenReason_t enumeration
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_BrokenReason_t ddi_bc_GetBrokenReason(void)
+{
+	return ddi_bc_gBrokenReason;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Restart the charge cycle
+//!
+//! \fntype Function
+//!
+//!
+//! \retval  SUCCESS
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_ForceChargingToStart(void)
+{
+	static int16_t restarts = 0;
+
+	if (restarts < DDI_BC_MAX_RESTART_CYCLES) {
+		restarts++;
+		bRestartChargeCycle = true;
+	}
+
+	return (DDI_BC_STATUS_SUCCESS);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// End of file
+////////////////////////////////////////////////////////////////////////////////
+//! @}
diff --git a/drivers/power/stmp37xx/ddi_bc_hw.c b/drivers/power/stmp37xx/ddi_bc_hw.c
new file mode 100644
index 000000000000..fab82a738b01
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_bc_hw.c
@@ -0,0 +1,474 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include "ddi_bc_internal.h"
+
+////////////////////////////////////////////////////////////////////////////////
+//! \addtogroup ddi_bc
+//! @{
+//
+// Copyright (c) 2004-2005 SigmaTel, Inc.
+//
+//! \file       ddi_bc_hw.c
+//! \brief      Contains the Battery Charger hardware operations.
+//! \date       06/2005
+//!
+//! This file contains Battery Charger hardware operations.
+//!
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+// Includes and external references
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+// Variables
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+// Code
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report if the battery charging hardware is available.
+//!
+//! \fntype Function
+//!
+//! This function reports if the battery charging hardware is available by
+//! reading the corresponding laser fuse bit.
+//!
+//! \retval  Zero if the battery charging hardware is not available. Non-zero
+//!          otherwise.
+//!
+////////////////////////////////////////////////////////////////////////////////
+int ddi_bc_hwBatteryChargerIsEnabled(void)
+{
+	//TODO: replace ddi_bc_hwBatteryChargerIsEnabled with the function below in the code
+	return (int)ddi_power_GetBatteryChargerEnabled();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the battery configuration.
+//!
+//! \fntype Function
+//!
+//! This function reports the hardware battery configuration.
+//!
+//! \retval  A value that indicates the battery configuration.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_BatteryMode_t ddi_bc_hwGetBatteryMode(void)
+{
+	//TODO: replace ddi_bc_hwGetBatteryMode() with the function below.
+	return (ddi_bc_BatteryMode_t) ddi_power_GetBatteryMode();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the bias current source.
+//!
+//! \fntype Function
+//!
+//! This function  Battery Charger.
+//!
+//! \retval  A value that indicates the current bias current source.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_BiasCurrentSource_t ddi_bc_hwGetBiasCurrentSource(void)
+{
+	// TODO: replace ddi_bc_BiasCurrentSource_t() with the function below.
+	return (ddi_bc_BiasCurrentSource_t) ddi_power_GetBiasCurrentSource();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Sets the bias current source.
+//!
+//! \fntype Function
+//!
+//! This function sets the bias current source used by the battery charger
+//! hardware. The battery charger is usually configured to use an externally
+//! generated bias current, which is expected to be quite precise. To reduce
+//! component count, for example, it can be configured to generate a lesser-
+//! quality bias current internally.
+//!
+//! \param[in]  source  Indicates the source of the bias current.
+//!
+//! \retval  DDI_BC_STATUS_SUCCESS       If the operation succeeded.
+//! \retval  DDI_BC_STATUS_BAD_ARGUMENT  If the given source argument isn't one
+//!                                      of the expected values.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_hwSetBiasCurrentSource(ddi_bc_BiasCurrentSource_t source)
+{
+	ddi_power_BiasCurrentSource_t eSource =
+	    (ddi_power_BiasCurrentSource_t) source;
+
+	//--------------------------------------------------------------------------
+	// In the following code, there are two points that can be confusing. Both
+	// of these problems derive from silliness in the data sheet.
+	//
+	// The field of interest here is HW_POWER_BATTCHRG.USE_EXTERN_R. This is
+	// poorly named for two reasons:
+	//
+	// 1) What we're really doing is selecting the source of the bias current.
+	//    If the application chooses to use an external bias current, then
+	//    there will, of course, be a resistor involved (apparently, a good-
+	//    quality one at about 620 Ohms).
+	//
+	// 2) If the bit is SET, that tells the hardware to use the INTERNAL bias
+	//    current. If this bit is CLEAR, that tells the hardware to use the
+	//    EXTERNAL bias current. Thus, the actual logic for this bit is
+	//    reversed from the sense indicated by its name.
+	//--------------------------------------------------------------------------
+
+	// TODO: replace all ddi_bc_hwSetBiasCurrentSource() with function  below.
+	return (ddi_bc_Status_t) ddi_power_SetBiasCurrentSource(eSource);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the voltage across the battery.
+//!
+//! \fntype Function
+//!
+//! This function reports the voltage across the battery.
+//!
+//! \retval The voltage across the battery, in mV.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_hwGetBatteryVoltage(void)
+{
+	//TODO: replace ddi_bc_hwGetBattery with function below
+	return ddi_power_GetBattery();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report on the presence of the power supply.
+//!
+//! \fntype Function
+//!
+//! This function repots on whether or not the 5V power supply is present.
+//!
+//! \retval  Zero if the power supply is not present. Non-zero otherwise.
+//!
+////////////////////////////////////////////////////////////////////////////////
+int ddi_bc_hwPowerSupplyIsPresent(void)
+{
+	// TODO: replace ddi_bc_hwPowerSupplyIsPresent with the functino below.
+	return (int)ddi_power_Get5vPresentFlag();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the maximum charging current.
+//!
+//! \fntype Function
+//!
+//! This function reports the maximum charging current that will be offered to
+//! the battery, as currently set in the hardware.
+//!
+//! \retval  The maximum current setting in the hardware.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_hwGetMaxCurrent(void)
+{
+	// TODO: replace ddi_bc_hwGetMaxCurrent() with the below function
+	return (uint16_t) ddi_power_GetMaxBatteryChargeCurrent();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Set the maximum charging current.
+//!
+//! \fntype Function
+//!
+//! This function sets the maximum charging current that will be offered to the
+//! battery.
+//!
+//! Note that the hardware has a minimum resolution of 10mA and a maximum
+//! expressible value of 780mA (see the data sheet for details). If the given
+//! current cannot be expressed exactly, then the largest expressible smaller
+//! value will be used. The return reports the actual value that was effected.
+//!
+//! \param[in]  u16Limit  The maximum charging current, in mA.
+//!
+//! \retval  The actual value that was effected.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_hwSetMaxCurrent(uint16_t u16Limit)
+{
+	//TODO: replace ddi_bc_hwSetMaxChargeCurrent
+	return ddi_power_SetMaxBatteryChargeCurrent(u16Limit);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Set the charging current threshold.
+//!
+//! \fntype Function
+//!
+//! This function sets the charging current threshold. When the actual current
+//! flow to the battery is less than this threshold, the HW_POWER_STS.CHRGSTS
+//! flag is clear.
+//!
+//! Note that the hardware has a minimum resolution of 10mA and a maximum
+//! expressible value of 180mA (see the data sheet for details). If the given
+//! current cannot be expressed exactly, then the largest expressible smaller
+//! value will be used. The return reports the actual value that was effected.
+//!
+//! \param[in]  u16Threshold  The charging current threshold, in mA.
+//!
+//! \retval  The actual value that was effected.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_hwSetCurrentThreshold(uint16_t u16Threshold)
+{
+	//TODO: replace calls to ddi_bc_hwSetCurrentThreshold with the one below
+	return ddi_power_SetBatteryChargeCurrentThreshold(u16Threshold);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the charging current threshold.
+//!
+//! \fntype Function
+//!
+//! This function reports the charging current threshold. When the actual
+//! current flow to the battery is less than this threshold, the
+//! HW_POWER_STS.CHRGSTS flag is clear.
+//!
+//! Note that the hardware has a minimum resolution of 10mA and a maximum
+//! expressible value of 180mA (see the data sheet for details).
+//!
+//! \retval  The charging current threshold, in mA.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_hwGetCurrentThreshold(void)
+{
+	//TODO: replace calls to ddi_bc_hwGetCurrentThreshold with function below
+	return ddi_power_GetBatteryChargeCurrentThreshold();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report if the charger hardware power is on.
+//!
+//! \fntype Function
+//!
+//! This function reports if the charger hardware power is on.
+//!
+//! \retval  Zero if the charger hardware is not powered. Non-zero otherwise.
+//!
+////////////////////////////////////////////////////////////////////////////////
+int ddi_bc_hwChargerPowerIsOn(void)
+{
+
+	//--------------------------------------------------------------------------
+	// Note that the bit we're looking at is named PWD_BATTCHRG. The "PWD"
+	// stands for "power down". Thus, when the bit is set, the battery charger
+	// hardware is POWERED DOWN.
+	//--------------------------------------------------------------------------
+
+	//--------------------------------------------------------------------------
+	// Read the register and return the result.
+	//--------------------------------------------------------------------------
+
+	//TODO: replace ddi_bc_hwChargerPowerIsOn with function below
+	return ddi_power_GetChargerPowered();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Turn the charging hardware on or off.
+//!
+//! \fntype Function
+//!
+//! This function turns the charging hardware on or off.
+//!
+//! \param[in]  on  Indicates whether the charging hardware should be on or off.
+//!
+////////////////////////////////////////////////////////////////////////////////
+void ddi_bc_hwSetChargerPower(int on)
+{
+
+	//--------------------------------------------------------------------------
+	// Note that the bit we're looking at is named PWD_BATTCHRG. The "PWD"
+	// stands for "power down". Thus, when the bit is set, the battery charger
+	// hardware is POWERED DOWN.
+	//--------------------------------------------------------------------------
+
+	//--------------------------------------------------------------------------
+	// Hit the power switch.
+	//--------------------------------------------------------------------------
+
+	//TODO: replace ddi_bc_hwSetChargerPower with functino below
+	ddi_power_SetChargerPowered(on);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Reports if the charging current has fallen below the threshold.
+//!
+//! \fntype Function
+//!
+//! This function reports if the charging current that the battery is accepting
+//! has fallen below the threshold.
+//!
+//! Note that this bit is regarded by the hardware guys as very slightly
+//! unreliable. They recommend that you don't believe a value of zero until
+//! you've sampled it twice.
+//!
+//! \retval  Zero if the battery is accepting less current than indicated by the
+//!          charging threshold. Non-zero otherwise.
+//!
+////////////////////////////////////////////////////////////////////////////////
+int ddi_bc_hwGetChargeStatus(void)
+{
+	return ddi_power_GetChargeStatus();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report on the die temperature.
+//!
+//! \fntype Function
+//!
+//! This function reports on the die temperature.
+//!
+//! \param[out]  pLow   The low  end of the temperature range.
+//! \param[out]  pHigh  The high end of the temperature range.
+//!
+////////////////////////////////////////////////////////////////////////////////
+void ddi_bc_hwGetDieTemp(int16_t * pLow, int16_t * pHigh)
+{
+	// TODO: replace ddi_bc_hwGetDieTemp with function below
+	ddi_power_GetDieTemp(pLow, pHigh);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the battery temperature reading.
+//!
+//! \fntype Function
+//!
+//! This function examines the configured LRADC channel and reports the battery
+//! temperature reading.
+//!
+//! \param[out]  pReading  A pointer to a variable that will receive the
+//!                        temperature reading.
+//!
+//! \retval  DDI_BC_STATUS_SUCCESS          If the operation succeeded.
+//! \retval  DDI_BC_STATUS_NOT_INITIALIZED  If the Battery Charger is not yet
+//!                                         initialized.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_hwGetBatteryTemp(uint16_t * pReading)
+{
+	return (ddi_bc_Status_t)DDI_BC_STATUS_HARDWARE_DISABLED;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Convert a current in mA to a hardware setting.
+//!
+//! \fntype Function
+//!
+//! This function converts a current measurement in mA to a hardware setting
+//! used by HW_POWER_BATTCHRG.STOP_ILIMIT or HW_POWER_BATTCHRG.BATTCHRG_I.
+//!
+//! Note that the hardware has a minimum resolution of 10mA and a maximum
+//! expressible value of 780mA (see the data sheet for details). If the given
+//! current cannot be expressed exactly, then the largest expressible smaller
+//! value will be used.
+//!
+//! \param[in]  u16Current  The current of interest.
+//!
+//! \retval  The corresponding setting.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint8_t ddi_bc_hwCurrentToSetting(uint16_t u16Current)
+{
+	return ddi_power_convert_current_to_setting(u16Current);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Convert a hardware current setting to a value in mA.
+//!
+//! \fntype Function
+//!
+//! This function converts a setting used by HW_POWER_BATTCHRG.STOP_ILIMIT or
+//! HW_POWER_BATTCHRG.BATTCHRG_I into an actual current measurement in mA.
+//!
+//! Note that the hardware current fields are 6 bits wide. The higher bits in
+//! the 8-bit input parameter are ignored.
+//!
+//! \param[in]  u8Setting  A hardware current setting.
+//!
+//! \retval  The corresponding current in mA.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_hwSettingToCurrent(uint8_t u8Setting)
+{
+	return ddi_power_convert_setting_to_current(u8Setting);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Compute the actual current expressible in the hardware.
+//!
+//! \fntype Function
+//!
+//! Given a desired current, this function computes the actual current
+//! expressible in the hardware.
+//!
+//! Note that the hardware has a minimum resolution of 10mA and a maximum
+//! expressible value of 780mA (see the data sheet for details). If the given
+//! current cannot be expressed exactly, then the largest expressible smaller
+//! value will be used.
+//!
+//! \param[in]  u16Current  The current of interest.
+//!
+//! \retval  The corresponding current in mA.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_hwExpressibleCurrent(uint16_t u16Current)
+{
+	//TODO: replace the bc function with this one
+	return ddi_power_ExpressibleCurrent(u16Current);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Checks to see if the DCDC has been manually enabled
+//!
+//! \fntype Function
+//!
+//! \retval  true if DCDC is ON, false if DCDC is OFF.
+//!
+////////////////////////////////////////////////////////////////////////////////
+bool ddi_bc_hwIsDcdcOn(void)
+{
+	return ddi_power_IsDcdcOn();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// End of file
+////////////////////////////////////////////////////////////////////////////////
+//! @}
diff --git a/drivers/power/stmp37xx/ddi_bc_hw.h b/drivers/power/stmp37xx/ddi_bc_hw.h
new file mode 100644
index 000000000000..68ef98aa4303
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_bc_hw.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+//! \addtogroup ddi_bc
+//! @{
+//
+// Copyright (c) 2004-2005 SigmaTel, Inc.
+//
+//! \file ddi_bc_hw.h
+//! \brief Internal header file for Battery Charger hardware operations.
+//! \date 06/2005
+//!
+//! This file contains internal declarations for Battery Charger hardware
+//! operations.
+//!
+//! \see ddi_bc.c and related files.
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef _DDI_BC_HW_H
+#define _DDI_BC_HW_H
+
+////////////////////////////////////////////////////////////////////////////////
+// Definitions
+////////////////////////////////////////////////////////////////////////////////
+
+//! The enumeration of battery modes.
+
+typedef enum _ddi_bc_BatteryMode {
+	DDI_BC_BATTERY_MODE_LI_ION_2_CELLS = 0,
+	DDI_BC_BATTERY_MODE_LI_ION_1_CELL = 1,
+	DDI_BC_BATTERY_MODE_2_CELLS = 2,
+	DDI_BC_BATTERY_MODE_1_CELL = 3
+} ddi_bc_BatteryMode_t;
+
+//! The enumeration of bias current sources.
+
+typedef enum _ddi_bc_BiasCurrentSource {
+	DDI_BC_EXTERNAL_BIAS_CURRENT = 0,
+	DDI_BC_INTERNAL_BIAS_CURRENT = 1,
+} ddi_bc_BiasCurrentSource_t;
+
+////////////////////////////////////////////////////////////////////////////////
+// Prototypes
+////////////////////////////////////////////////////////////////////////////////
+
+extern int ddi_bc_hwBatteryChargerIsEnabled(void);
+extern ddi_bc_BatteryMode_t ddi_bc_hwGetBatteryMode(void);
+extern ddi_bc_BiasCurrentSource_t ddi_bc_hwGetBiasCurrentSource(void);
+extern ddi_bc_Status_t
+ddi_bc_hwSetBiasCurrentSource(ddi_bc_BiasCurrentSource_t);
+extern ddi_bc_Status_t ddi_bc_hwSetChargingVoltage(uint16_t);
+extern uint16_t ddi_bc_hwGetBatteryVoltage(void);
+extern int ddi_bc_hwPowerSupplyIsPresent(void);
+extern uint16_t ddi_bc_hwSetMaxCurrent(uint16_t);
+extern uint16_t ddi_bc_hwGetMaxCurrent(void);
+extern uint16_t ddi_bc_hwSetCurrentThreshold(uint16_t);
+extern uint16_t ddi_bc_hwGetCurrentThreshold(void);
+extern int ddi_bc_hwChargerPowerIsOn(void);
+extern void ddi_bc_hwSetChargerPower(int);
+extern int ddi_bc_hwGetChargeStatus(void);
+extern void ddi_bc_hwGetDieTemp(int16_t *, int16_t *);
+extern ddi_bc_Status_t ddi_bc_hwGetBatteryTemp(uint16_t *);
+uint8_t ddi_bc_hwCurrentToSetting(uint16_t);
+uint16_t ddi_bc_hwSettingToCurrent(uint8_t);
+uint16_t ddi_bc_hwExpressibleCurrent(uint16_t);
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Checks to see if the DCDC has been manually enabled
+//!
+//! \fntype Function
+//!
+//! \retval  true if DCDC is ON, false if DCDC is OFF.
+//!
+////////////////////////////////////////////////////////////////////////////////
+bool ddi_bc_hwIsDcdcOn(void);
+
+////////////////////////////////////////////////////////////////////////////////
+// End of file
+////////////////////////////////////////////////////////////////////////////////
+#endif				// _DDI_BC_H
+//! @}
diff --git a/drivers/power/stmp37xx/ddi_bc_init.c b/drivers/power/stmp37xx/ddi_bc_init.c
new file mode 100644
index 000000000000..ef323b33b74f
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_bc_init.c
@@ -0,0 +1,204 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include "ddi_bc_internal.h"
+
+////////////////////////////////////////////////////////////////////////////////
+//! \addtogroup ddi_bc
+//! @{
+//
+// Copyright (c) 2004-2005 SigmaTel, Inc.
+//
+//! \file       ddi_bc_init.c
+//! \brief      Contains the Battery Charger initialization function.
+//! \date       06/2005
+//!
+//! This file contains Battery Charger initialization function.
+//!
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+// Includes and external references
+////////////////////////////////////////////////////////////////////////////////
+#include <mach/ddi_bc.h>
+#include "ddi_bc_internal.h"
+
+////////////////////////////////////////////////////////////////////////////////
+// Code
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+//! \brief Initialize the Battery Charger.
+//!
+//! \fntype Function
+//!
+//! This function initializes the Battery Charger.
+//!
+//! \param[in]  pCfg  A pointer to the new configuration.
+//!
+//! \retval  DDI_BC_STATUS_SUCCESS
+//!             If the operation succeeded.
+//! \retval  DDI_BC_STATUS_ALREADY_INITIALIZED
+//!             If the Battery Charger is already initialized.
+//! \retval  DDI_BC_STATUS_HARDWARE_DISABLED
+//!             If the Battery Charger hardware is disabled by a laser fuse.
+//! \retval  DDI_BC_STATUS_BAD_BATTERY_MODE
+//!             If the power supply is set up for a non-rechargeable battery.
+//! \retval  DDI_BC_STATUS_CLOCK_GATE_CLOSED
+//!             If the clock gate for the power supply registers is closed.
+//! \retval  DDI_BC_STATUS_CFG_BAD_CHARGING_VOLTAGE
+//!             If the charging voltage is not either 4100 or 4200.
+//! \retval  DDI_BC_STATUS_CFG_BAD_BATTERY_TEMP_CHANNEL
+//!             If the LRADC channel number for monitoring battery temperature
+//!             is bad.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_Init(ddi_bc_Cfg_t * pCfg)
+{
+
+	//--------------------------------------------------------------------------
+	// We can only be initialized if we're in the Uninitialized state.
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_State != DDI_BC_STATE_UNINITIALIZED) {
+		return (DDI_BC_STATUS_ALREADY_INITIALIZED);
+	}
+	//--------------------------------------------------------------------------
+	// Check if the battery charger hardware has been disabled by laser fuse.
+	//--------------------------------------------------------------------------
+
+	if (!ddi_power_GetBatteryChargerEnabled())
+		return (DDI_BC_STATUS_HARDWARE_DISABLED);
+
+	//--------------------------------------------------------------------------
+	// Check if the power supply has been set up for a non-rechargeable battery.
+	//--------------------------------------------------------------------------
+
+	switch (ddi_power_GetBatteryMode()) {
+
+	case DDI_POWER_BATT_MODE_LIION:
+		break;
+
+		// TODO: we'll need to do NiMH also
+	default:
+		return (DDI_BC_STATUS_BAD_BATTERY_MODE);
+		//break;
+
+	}
+
+	//--------------------------------------------------------------------------
+	// Make sure that the clock gate has been opened for the power supply
+	// registers. If not, then none of our writes to those registers will
+	// succeed, which will kind of slow us down...
+	//--------------------------------------------------------------------------
+
+	if (ddi_power_GetPowerClkGate()) {
+		return (DDI_BC_STATUS_CLOCK_GATE_CLOSED);
+	}
+	//--------------------------------------------------------------------------
+	// Check the incoming configuration for nonsense.
+	//--------------------------------------------------------------------------
+
+	//
+	// Only permitted charging voltage: 4200mV.
+	//
+
+	if (pCfg->u16ChargingVoltage != DDI_BC_LIION_CHARGING_VOLTAGE) {
+		return (DDI_BC_STATUS_CFG_BAD_CHARGING_VOLTAGE);
+	}
+	//
+	// There are 8 LRADC channels.
+	//
+
+	if (pCfg->u8BatteryTempChannel > 7) {
+		return (DDI_BC_STATUS_CFG_BAD_BATTERY_TEMP_CHANNEL);
+	}
+	//--------------------------------------------------------------------------
+	// Accept the configuration.
+	//--------------------------------------------------------------------------
+
+	//--------------------------------------------------------------------------
+	//  ddi_bc_Cfg_t.u16ChargingThresholdCurrent is destined for the
+	//  register field HW_POWER_BATTCHRG.STOP_ILIMIT.  This 4-bit field
+	//  is unevenly quantized to provide a useful range of currents.  A
+	//  side effect of the quantization is that the field can only be
+	//  set to certain unevenly-spaced values.
+	//
+	//  Here, we use the two functions that manipulate the register field
+	//  to adjust u16ChargingThresholdCurrent to match the quantized value.
+	//--------------------------------------------------------------------------
+	pCfg->u16ChargingThresholdCurrent =
+	    ddi_power_ExpressibleCurrent(pCfg->u16ChargingThresholdCurrent);
+
+	//--------------------------------------------------------------------------
+	//  ...similar situation with ddi_bc_Cfg_t.u16BatteryTempSafeCurrent and
+	//  u16DieTempSafeCurrent.
+	//--------------------------------------------------------------------------
+	pCfg->u16BatteryTempSafeCurrent =
+	    ddi_power_ExpressibleCurrent(pCfg->u16BatteryTempSafeCurrent);
+	pCfg->u16DieTempSafeCurrent =
+	    ddi_power_ExpressibleCurrent(pCfg->u16DieTempSafeCurrent);
+
+	g_ddi_bc_Configuration = *pCfg;
+
+	//--------------------------------------------------------------------------
+	// Set the bias current source.
+	//--------------------------------------------------------------------------
+
+	{
+		ddi_power_BiasCurrentSource_t Flag;
+
+		Flag =
+		    g_ddi_bc_Configuration.useInternalBias ?
+		    DDI_POWER_INTERNAL_BIAS_CURRENT :
+		    DDI_POWER_EXTERNAL_BIAS_CURRENT;
+
+		ddi_power_SetBiasCurrentSource(Flag);
+
+	}
+
+	//--------------------------------------------------------------------------
+	// Turn the charger hardware off. This is a very important initial condition
+	// because we only flip the power switch on the hardware when we make
+	// transitions. Baseline, it needs to be off.
+	//--------------------------------------------------------------------------
+
+	ddi_power_SetChargerPowered(0);
+
+	//--------------------------------------------------------------------------
+	// Reset the current ramp. This will jam the current to zero and power off
+	// the charging hardware.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampReset();
+
+	//--------------------------------------------------------------------------
+	// Move to the Disabled state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_DISABLED;
+
+	//--------------------------------------------------------------------------
+	// Return success.
+	//--------------------------------------------------------------------------
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+	printk("%s: success\n", __func__);
+#endif
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// End of file
+////////////////////////////////////////////////////////////////////////////////
+//! @}
diff --git a/drivers/power/stmp37xx/ddi_bc_internal.h b/drivers/power/stmp37xx/ddi_bc_internal.h
new file mode 100644
index 000000000000..c1fb1b20f271
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_bc_internal.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+//! \addtogroup ddi_bc
+//! @{
+//
+// Copyright (c) 2004-2005 SigmaTel, Inc.
+//
+//! \file ddi_bc_internal.h
+//! \brief Internal header file for the Battery Charger device driver.
+//! \date 06/2005
+//!
+//! This file contains internal declarations for the Battery Charger device
+//! driver.
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef _DDI_BC_INTERNAL_H
+#define _DDI_BC_INTERNAL_H
+
+////////////////////////////////////////////////////////////////////////////////
+// Includes
+////////////////////////////////////////////////////////////////////////////////
+
+#include <mach/ddi_bc.h>
+#include "ddi_bc_hw.h"
+#include "ddi_bc_ramp.h"
+#include "ddi_bc_sm.h"
+#include "ddi_power_battery.h"
+
+////////////////////////////////////////////////////////////////////////////////
+// Externs
+////////////////////////////////////////////////////////////////////////////////
+
+extern bool g_ddi_bc_Configured;
+extern ddi_bc_Cfg_t g_ddi_bc_Configuration;
+
+////////////////////////////////////////////////////////////////////////////////
+// End of file
+////////////////////////////////////////////////////////////////////////////////
+#endif				// _DDI_BC_H
+//! @}
diff --git a/drivers/power/stmp37xx/ddi_bc_ramp.c b/drivers/power/stmp37xx/ddi_bc_ramp.c
new file mode 100644
index 000000000000..1de3a53259f8
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_bc_ramp.c
@@ -0,0 +1,724 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+//! \addtogroup ddi_bc
+//! @{
+//
+// Copyright (c) 2004-2005 SigmaTel, Inc.
+//
+//! \file       ddi_bc_ramp.c
+//! \brief      Contains the Battery Charger current ramp controller.
+//! \date       06/2005
+//!
+//! This file contains Battery Charger current ramp controller.
+//!
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+// Includes and external references
+////////////////////////////////////////////////////////////////////////////////
+
+#include <mach/ddi_bc.h>
+#include "ddi_bc_internal.h"
+
+////////////////////////////////////////////////////////////////////////////////
+// Definitions
+////////////////////////////////////////////////////////////////////////////////
+
+//! This is the control structure for the current ramp.
+
+typedef struct _ddi_bc_RampControl {
+
+	uint32_t u32AccumulatedTime;
+
+	//!< The accumulated time since we last changed the actual
+	//!< current setting in the hardware. If the time between
+	//!< steps is quite short, we may have to wait for several steps
+	//!< before we can actually change the hardware setting.
+
+	uint16_t u16Target;
+
+	//!< The target current, regardless of expressibility.
+
+	uint16_t u16Limit;
+
+	//!< The current limit, regardless of expressibility.
+
+	uint8_t dieTempAlarm:1;
+
+	//!< Indicates if we are operating under a die temperature
+	//!< alarm.
+
+	uint8_t batteryTempAlarm:1;
+
+	//!< Indicates if we are operating under a battery temperature
+	//!< alarm.
+
+	uint8_t ambientTempAlarm:1;
+
+	//!< Indicates if we are operating under an ambient temperature
+	//!< alarm.
+
+} ddi_bc_RampControl_t;
+
+////////////////////////////////////////////////////////////////////////////////
+// Variables
+////////////////////////////////////////////////////////////////////////////////
+
+//! This structure contains control information for the current ramp.
+
+static ddi_bc_RampControl_t g_RampControl;
+
+////////////////////////////////////////////////////////////////////////////////
+// Code
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Reset the current ramp.
+//!
+//! \fntype Function
+//!
+//! This function resets the current ramp.
+//!
+//! Note that this function does NOT reset the temperature alarms or the current
+//! limit. Those can only be changed explicitly.
+//!
+////////////////////////////////////////////////////////////////////////////////
+void ddi_bc_RampReset()
+{
+
+	//--------------------------------------------------------------------------
+	// Reset the control structure.
+	//--------------------------------------------------------------------------
+
+	g_RampControl.u32AccumulatedTime = 0;
+	g_RampControl.u16Target = 0;
+
+	//--------------------------------------------------------------------------
+	// Step the ramp. Note that we don't care if this function returns an error.
+	// We're stepping the ramp to make sure it takes immediate effect, if
+	// possible. But, for example, if the Battery Charger is not yet
+	// initialized, it doesn't matter.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampStep(0);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Set the target current.
+//!
+//! \fntype Function
+//!
+//! This function sets the target current and implements it immediately.
+//!
+//! Note that this function does NOT reset the temperature alarms. Those can
+//! only be reset explicitly.
+//!
+//! \param[in]  u16Target  The target current.
+//!
+//! \retval  The expressible version of the target.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_RampSetTarget(uint16_t u16Target)
+{
+
+	//--------------------------------------------------------------------------
+	// Set the target.
+	//--------------------------------------------------------------------------
+
+	g_RampControl.u16Target = u16Target;
+
+	//--------------------------------------------------------------------------
+	// Step the ramp. Note that we don't care if this function returns an error.
+	// We're stepping the ramp to make sure it takes immediate effect, if
+	// possible. But, for example, if the Battery Charger is not yet
+	// initialized, it doesn't matter.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampStep(0);
+
+	//--------------------------------------------------------------------------
+	// Compute and return the expressible target.
+	//--------------------------------------------------------------------------
+
+	return (ddi_bc_hwExpressibleCurrent(u16Target));
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the target.
+//!
+//! \fntype Function
+//!
+//! This function reports the target.
+//!
+//! \retval  The target.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_RampGetTarget(void)
+{
+
+	//--------------------------------------------------------------------------
+	// Return the target.
+	//--------------------------------------------------------------------------
+
+	return (g_RampControl.u16Target);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Set the current limit.
+//!
+//! \fntype Function
+//!
+//! This function sets the current limit and implements it immediately.
+//!
+//! \param[in]  u16Limit  The current limit.
+//!
+//! \retval  The expressible version of the limit.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_RampSetLimit(uint16_t u16Limit)
+{
+
+	//--------------------------------------------------------------------------
+	// Set the limit.
+	//--------------------------------------------------------------------------
+
+	g_RampControl.u16Limit = u16Limit;
+
+	//--------------------------------------------------------------------------
+	// Step the ramp. Note that we don't care if this function returns an error.
+	// We're stepping the ramp to make sure it takes immediate effect, if
+	// possible. But, for example, if the Battery Charger is not yet
+	// initialized, it doesn't matter.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampStep(0);
+
+	//--------------------------------------------------------------------------
+	// Compute and return the expressible limit.
+	//--------------------------------------------------------------------------
+
+	return (ddi_bc_hwExpressibleCurrent(u16Limit));
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the current limit.
+//!
+//! \fntype Function
+//!
+//! This function reports the current limit.
+//!
+//! \retval  The current limit.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_bc_RampGetLimit(void)
+{
+
+	//--------------------------------------------------------------------------
+	// Return the current limit.
+	//--------------------------------------------------------------------------
+
+	return (g_RampControl.u16Limit);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Update alarms.
+//!
+//! \fntype Function
+//!
+//! This function checks for all alarms and updates the current ramp
+//! accordingly.
+//!
+////////////////////////////////////////////////////////////////////////////////
+void ddi_bc_RampUpdateAlarms()
+{
+
+	// Set to true if something changed and we need to step the ramp right away.
+
+	int iStepTheRamp = 0;
+
+	//--------------------------------------------------------------------------
+	// Are we monitoring die temperature?
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_Configuration.monitorDieTemp) {
+
+		//----------------------------------------------------------------------
+		// Get the die temperature range.
+		//----------------------------------------------------------------------
+
+		int16_t i16Low;
+		int16_t i16High;
+
+		ddi_bc_hwGetDieTemp(&i16Low, &i16High);
+
+		//----------------------------------------------------------------------
+		// Now we need to decide if it's time to raise or lower the alarm. The
+		// first question to ask is: Were we already under an alarm?
+		//----------------------------------------------------------------------
+
+		if (g_RampControl.dieTempAlarm) {
+
+			//------------------------------------------------------------------
+			// If control arrives here, we were already under an alarm. We'll
+			// change that if the high end of the temperature range drops below
+			// the low temperature mark.
+			//------------------------------------------------------------------
+
+			if (i16High < g_ddi_bc_Configuration.u8DieTempLow) {
+
+				//--------------------------------------------------------------
+				// If control arrives here, we're safe now. Drop the alarm.
+				//--------------------------------------------------------------
+
+				g_RampControl.dieTempAlarm = 0;
+
+				iStepTheRamp = !0;
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+				printk("Battery charger: releasing "
+				       "die temp alarm: [%d, %d] < %d\r\n",
+				       (int32_t) i16Low, (int32_t) i16High,
+				       (int32_t) g_ddi_bc_Configuration.
+				       u8DieTempLow);
+#endif
+
+			}
+
+		} else {
+
+			//------------------------------------------------------------------
+			// If control arrives here, we were not under an alarm. We'll change
+			// that if the high end of the temperature range rises above the
+			// high temperature mark.
+			//------------------------------------------------------------------
+
+			if (i16High >= g_ddi_bc_Configuration.u8DieTempHigh) {
+
+				//--------------------------------------------------------------
+				// If control arrives here, we're running too hot. Raise the
+				// alarm.
+				//--------------------------------------------------------------
+
+				g_RampControl.dieTempAlarm = 1;
+
+				iStepTheRamp = !0;
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+				printk("Battery charger: declaring "
+				       "die temp alarm: [%d, %d] >= %d\r\n",
+				       (int32_t) i16Low, (int32_t) i16High,
+				       (int32_t) g_ddi_bc_Configuration.
+				       u8DieTempLow);
+#endif
+			}
+
+		}
+
+	}
+	//--------------------------------------------------------------------------
+	// Are we monitoring battery temperature?
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_Configuration.monitorBatteryTemp) {
+
+		ddi_bc_Status_t status;
+
+		//----------------------------------------------------------------------
+		// Get the battery temperature reading.
+		//----------------------------------------------------------------------
+
+		uint16_t u16Reading;
+
+		status = ddi_bc_hwGetBatteryTemp(&u16Reading);
+
+		//----------------------------------------------------------------------
+		// If there was a problem, then we ignore the reading. Otherwise, let's
+		// have a look.
+		//----------------------------------------------------------------------
+
+		if (status == DDI_BC_STATUS_SUCCESS) {
+
+			//------------------------------------------------------------------
+			// Now we need to decide if it's time to raise or lower the alarm.
+			// The first question to ask is: Were we already under an alarm?
+			//------------------------------------------------------------------
+
+			if (g_RampControl.batteryTempAlarm) {
+
+				//--------------------------------------------------------------
+				// If control arrives here, we were already under an alarm.
+				// We'll change that if the reading drops below the low mark.
+				//--------------------------------------------------------------
+
+				if (u16Reading <
+				    g_ddi_bc_Configuration.u16BatteryTempLow) {
+
+					//----------------------------------------------------------
+					// If control arrives here, we're safe now. Drop the alarm.
+					//----------------------------------------------------------
+
+					g_RampControl.batteryTempAlarm = 0;
+
+					iStepTheRamp = !0;
+
+				}
+
+			} else {
+
+				//--------------------------------------------------------------
+				// If control arrives here, we were not under an alarm. We'll
+				// change that if the reading rises above the high mark.
+				//--------------------------------------------------------------
+
+				if (u16Reading >=
+				    g_ddi_bc_Configuration.u16BatteryTempHigh) {
+
+					//----------------------------------------------------------
+					// If control arrives here, we're running too hot. Raise the
+					// alarm.
+					//----------------------------------------------------------
+
+					g_RampControl.batteryTempAlarm = 1;
+
+					iStepTheRamp = !0;
+
+				}
+
+			}
+
+		}
+
+	}
+	//--------------------------------------------------------------------------
+	// Do we need to step the ramp?
+	//--------------------------------------------------------------------------
+
+	if (iStepTheRamp)
+		ddi_bc_RampStep(0);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Reports the state of the die temperature alarm.
+//!
+//! \fntype Function
+//!
+//! This function reports the state of the die temperature alarm.
+//!
+//! \retval  The state of the die temperature alarm.
+//!
+////////////////////////////////////////////////////////////////////////////////
+int ddi_bc_RampGetDieTempAlarm(void)
+{
+	return (g_RampControl.dieTempAlarm);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Reports the state of the battery temperature alarm.
+//!
+//! \fntype Function
+//!
+//! This function reports the state of the battery temperature alarm.
+//!
+//! \retval  The state of the battery temperature alarm.
+//!
+////////////////////////////////////////////////////////////////////////////////
+int ddi_bc_RampGetBatteryTempAlarm(void)
+{
+	return (g_RampControl.batteryTempAlarm);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Reports the state of the ambient temperature alarm.
+//!
+//! \fntype Function
+//!
+//! This function reports the state of the ambient temperature alarm.
+//!
+//! \retval  The state of the ambient temperature alarm.
+//!
+////////////////////////////////////////////////////////////////////////////////
+int ddi_bc_RampGetAmbientTempAlarm(void)
+{
+	return (g_RampControl.ambientTempAlarm);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Step the current ramp.
+//!
+//! \fntype Function
+//!
+//! This function steps the current ramp forward through the given amount of time.
+//!
+//! \param[in] u32Time  The time increment to add.
+//!
+//! \retval  DDI_BC_STATUS_SUCCESS          If the operation succeeded.
+//! \retval  DDI_BC_STATUS_NOT_INITIALIZED  If the Battery Charger is not yet
+//!                                         initialized.
+//!
+////////////////////////////////////////////////////////////////////////////////
+ddi_bc_Status_t ddi_bc_RampStep(uint32_t u32Time)
+{
+
+	uint16_t u16MaxNow;
+	uint16_t u16Target;
+	uint16_t u16Cart;
+	int32_t i32Delta;
+
+	//--------------------------------------------------------------------------
+	// Make sure the Battery Charger is initialized.
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_State == DDI_BC_STATE_UNINITIALIZED) {
+		return (DDI_BC_STATUS_NOT_INITIALIZED);
+	}
+	//--------------------------------------------------------------------------
+	// Figure out how much current the hardware is set to draw right now.
+	//--------------------------------------------------------------------------
+
+	u16MaxNow = ddi_bc_hwGetMaxCurrent();
+
+	//--------------------------------------------------------------------------
+	// Start with the target.
+	//--------------------------------------------------------------------------
+
+	u16Target = g_RampControl.u16Target;
+
+	//--------------------------------------------------------------------------
+	// Check the target against the hard limit.
+	//--------------------------------------------------------------------------
+
+	if (u16Target > g_RampControl.u16Limit)
+		u16Target = g_RampControl.u16Limit;
+
+	//--------------------------------------------------------------------------
+	// Check if the die temperature alarm is active.
+	//--------------------------------------------------------------------------
+
+	if (g_RampControl.dieTempAlarm) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, we are under a die temperature alarm. Clamp
+		// the target current.
+		//----------------------------------------------------------------------
+
+		if (u16Target > g_ddi_bc_Configuration.u16DieTempSafeCurrent) {
+			u16Target =
+			    g_ddi_bc_Configuration.u16DieTempSafeCurrent;
+		}
+
+	}
+	//--------------------------------------------------------------------------
+	// Check if the battery temperature alarm is active.
+	//--------------------------------------------------------------------------
+
+	if (g_RampControl.batteryTempAlarm) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, we are under a battery temperature alarm.
+		// Clamp the target current.
+		//----------------------------------------------------------------------
+
+		if (u16Target >
+		    g_ddi_bc_Configuration.u16BatteryTempSafeCurrent) {
+			u16Target =
+			    g_ddi_bc_Configuration.u16BatteryTempSafeCurrent;
+		}
+
+	}
+	//--------------------------------------------------------------------------
+	// Now we know the target current. Figure out what is actually expressible
+	// in the hardware.
+	//--------------------------------------------------------------------------
+
+	u16Target = ddi_bc_hwExpressibleCurrent(u16Target);
+
+	//--------------------------------------------------------------------------
+	// Compute the difference between the expressible target and what's actually
+	// set in the hardware right now.
+	//--------------------------------------------------------------------------
+
+	i32Delta = ((int32_t) u16Target) - ((int32_t) u16MaxNow);
+
+	//--------------------------------------------------------------------------
+	// Check if the delta is zero.
+	//--------------------------------------------------------------------------
+
+	if (i32Delta == 0) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, there is no difference between what we want
+		// and what's set in the hardware.
+		//
+		// Before we leave, though, we don't want to leave any accumulated time
+		// laying around for the next ramp up. Zero it out.
+		//----------------------------------------------------------------------
+
+		g_RampControl.u32AccumulatedTime = 0;
+
+		//----------------------------------------------------------------------
+		// Return success.
+		//----------------------------------------------------------------------
+
+		return (DDI_BC_STATUS_SUCCESS);
+
+	}
+	//--------------------------------------------------------------------------
+	// Check if the delta is negative.
+	//--------------------------------------------------------------------------
+
+	if (i32Delta < 0) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, the new target is lower than what's
+		// currently set in the hardware. Since that means we're *reducing* the
+		// current draw, we can do it right now. Just gimme a sec here...
+		//----------------------------------------------------------------------
+
+		ddi_bc_hwSetMaxCurrent(u16Target);
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+		printk("Battery charger: setting max charge "
+		       "current to: %hdmA\r\n", u16Target);
+#endif
+
+		//----------------------------------------------------------------------
+		// Flip the power switch on the charging hardware according to the new
+		// current setting.
+		//----------------------------------------------------------------------
+
+		ddi_bc_hwSetChargerPower(u16Target != 0);
+
+		//----------------------------------------------------------------------
+		// We don't want to leave any accumulated time laying around for the
+		// next ramp up. Zero it out.
+		//----------------------------------------------------------------------
+
+		g_RampControl.u32AccumulatedTime = 0;
+
+		//----------------------------------------------------------------------
+		// Return success.
+		//----------------------------------------------------------------------
+
+		return (DDI_BC_STATUS_SUCCESS);
+
+	}
+	//--------------------------------------------------------------------------
+	// If control arrives here, the target current is higher than what's set in
+	// the hardware right now. That means we're going to ramp it up. To do that,
+	// we're going to "buy" more milliamps by "spending" milliseconds of time.
+	// Add the time we've "banked" to the time we've been credited in this call.
+	//--------------------------------------------------------------------------
+
+	u32Time += g_RampControl.u32AccumulatedTime;
+
+	//--------------------------------------------------------------------------
+	// Now we know how much we can spend. How much current will it buy?
+	//--------------------------------------------------------------------------
+
+	u16Cart = (g_ddi_bc_Configuration.u16CurrentRampSlope * u32Time) / 1000;
+
+	//--------------------------------------------------------------------------
+	// Check how the current we can afford stacks up against the target we want.
+	//--------------------------------------------------------------------------
+
+	if ((u16MaxNow + u16Cart) < u16Target) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, we can't afford to buy all the current we
+		// want. Compute the maximum we can afford, and then figure out what we
+		// can actually express in the hardware.
+		//----------------------------------------------------------------------
+
+		u16Target = ddi_bc_hwExpressibleCurrent(u16MaxNow + u16Cart);
+
+		//----------------------------------------------------------------------
+		// Check if the result isn't actually different from what's set in the
+		// the hardware right now.
+		//----------------------------------------------------------------------
+
+		if (u16Target == u16MaxNow) {
+
+			//------------------------------------------------------------------
+			// If control arrives here, we are so poor that we can't yet afford
+			// to buy enough current to make a change in the expressible
+			// hardware setting. Since we didn't spend any of our time, put the
+			// new balance back in the bank.
+			//------------------------------------------------------------------
+
+			g_RampControl.u32AccumulatedTime = u32Time;
+
+			//------------------------------------------------------------------
+			// Leave dispiritedly.
+			//------------------------------------------------------------------
+
+			return (DDI_BC_STATUS_SUCCESS);
+
+		}
+
+	}
+	//--------------------------------------------------------------------------
+	// If control arrives here, we can afford to buy enough current to get us
+	// all the way to the target. Set it.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_hwSetMaxCurrent(u16Target);
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+	printk("Battery charger: setting max charge"
+	       "current to: %hdmA\r\n", u16Target);
+#endif
+
+	//--------------------------------------------------------------------------
+	// Flip the power switch on the charging hardware according to the new
+	// current setting.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_hwSetChargerPower(u16Target != 0);
+
+	//--------------------------------------------------------------------------
+	// We're at the target, so we're finished buying current. Zero out the
+	// account.
+	//--------------------------------------------------------------------------
+
+	g_RampControl.u32AccumulatedTime = 0;
+
+	//--------------------------------------------------------------------------
+	// Return success.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// End of file
+////////////////////////////////////////////////////////////////////////////////
+//! @}
diff --git a/drivers/power/stmp37xx/ddi_bc_ramp.h b/drivers/power/stmp37xx/ddi_bc_ramp.h
new file mode 100644
index 000000000000..5111a5496fcf
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_bc_ramp.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+//! \addtogroup ddi_bc
+//! @{
+//
+// Copyright (c) 2004-2005 SigmaTel, Inc.
+//
+//! \file ddi_bc_ramp.h
+//! \brief Internal header file for Battery Charger current ramp controller.
+//! \date 06/2005
+//!
+//! This file contains internal declarations for Battery current ramp
+//! controller.
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef _DDI_BC_RAMP_H
+#define _DDI_BC_RAMP_H
+
+////////////////////////////////////////////////////////////////////////////////
+// Prototypes
+////////////////////////////////////////////////////////////////////////////////
+
+extern void ddi_bc_RampReset(void);
+extern uint16_t ddi_bc_RampSetTarget(uint16_t);
+extern uint16_t ddi_bc_RampGetTarget(void);
+extern uint16_t ddi_bc_RampSetLimit(uint16_t);
+extern uint16_t ddi_bc_RampGetLimit(void);
+extern void ddi_bc_RampUpdateAlarms(void);
+extern int ddi_bc_RampGetDieTempAlarm(void);
+extern int ddi_bc_RampGetBatteryTempAlarm(void);
+extern int ddi_bc_RampGetAmbientTempAlarm(void);
+extern ddi_bc_Status_t ddi_bc_RampStep(uint32_t);
+
+////////////////////////////////////////////////////////////////////////////////
+// End of file
+////////////////////////////////////////////////////////////////////////////////
+#endif				// _DDI_BC_H
+//! @}
diff --git a/drivers/power/stmp37xx/ddi_bc_sm.c b/drivers/power/stmp37xx/ddi_bc_sm.c
new file mode 100644
index 000000000000..8bd8d2cbb58b
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_bc_sm.c
@@ -0,0 +1,1122 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+//! \addtogroup ddi_bc
+//! @{
+//
+// Copyright (c) 2004-2005 SigmaTel, Inc.
+//
+//! \file       ddi_bc_sm.c
+//! \brief      Contains the Battery Charger state machine.
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+// Includes
+////////////////////////////////////////////////////////////////////////////////
+
+#include <mach/ddi_bc.h>
+#include "ddi_bc_internal.h"
+
+#include <linux/delay.h>
+
+////////////////////////////////////////////////////////////////////////////////
+// Definitions
+////////////////////////////////////////////////////////////////////////////////
+
+// This is the minimum time we must charge before we transition from
+// the charging state to the topping off. If we reach the
+// u16ChargingThresholdCurrent charge curent before then, the battery was
+// already full so we can avoid the risk of charging it past .1C for
+// too long.
+
+#define TRANSITION_TO_TOPOFF_MINIMUM_CHARGE_TIME_mS 1 * 60 * 1000	// 1 minute
+
+// If DCDCs are active, we can't complete the charging cycle.  Once
+// they are stay off for this amount of time, we will restart the
+// charge cycle.
+#define DCDC_INACTIVITY_TIMER_THRESHOLD 1 * 60 * 1000	// 1 minute
+
+////////////////////////////////////////////////////////////////////////////////
+// Variables
+////////////////////////////////////////////////////////////////////////////////
+
+// The current state.
+
+ddi_bc_State_t g_ddi_bc_State = DDI_BC_STATE_UNINITIALIZED;
+
+// This table contains pointers to the functions that implement states. The
+// table is indexed by state. Note that it's critically important for this
+// table to agree with the state enumeration in ddi_bc.h.
+
+static ddi_bc_Status_t ddi_bc_Uninitialized(void);
+static ddi_bc_Status_t ddi_bc_Broken(void);
+static ddi_bc_Status_t ddi_bc_Disabled(void);
+static ddi_bc_Status_t ddi_bc_WaitingToCharge(void);
+static ddi_bc_Status_t ddi_bc_Conditioning(void);
+static ddi_bc_Status_t ddi_bc_Charging(void);
+static ddi_bc_Status_t ddi_bc_ToppingOff(void);
+static ddi_bc_Status_t ddi_bc_DcdcModeWaitingToCharge(void);
+
+ddi_bc_Status_t(*const (stateFunctionTable[])) (void) = {
+ddi_bc_Uninitialized,
+	    ddi_bc_Broken,
+	    ddi_bc_Disabled,
+	    ddi_bc_WaitingToCharge,
+	    ddi_bc_Conditioning,
+	    ddi_bc_Charging, ddi_bc_ToppingOff, ddi_bc_DcdcModeWaitingToCharge};
+
+// Used by states that need to watch the time.
+uint32_t g_ddi_bc_u32StateTimer = 0;
+
+// If DCDCs are active, we can't complete the charging cycle.  Once
+// they are stay off for this amount of time, we will restart the
+// charge cycle.
+static uint32_t DcdcInactityTimer = DCDC_INACTIVITY_TIMER_THRESHOLD;
+
+// Always attempt to charge on first 5V connection even if DCDCs are ON.
+bool bRestartChargeCycle = true;
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+static uint16_t u16ExternalBatteryPowerVoltageCheck = 0;
+#endif
+
+ddi_bc_BrokenReason_t ddi_bc_gBrokenReason = DDI_BC_BROKEN_UNINITIALIZED;
+
+////////////////////////////////////////////////////////////////////////////////
+// Code
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Transition to the DCDC mode Waiting to Charge state.
+//!
+//! \fntype Function
+//!
+//! This function implements the transition to the DCDC Mode Waiting
+//!  to Charge state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Transition to the DCDC mode Waiting to Charge state.
+//!
+//! \fntype Function
+//!
+//! This function implements the transition to the DCDC Mode Waiting
+//!  to Charge state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static void TransitionToDcdcModeWaitingToCharge(void)
+{
+
+	//--------------------------------------------------------------------------
+	// Reset the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer = 0;
+
+	//--------------------------------------------------------------------------
+	// Reset the current ramp.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampReset();
+
+	//--------------------------------------------------------------------------
+	// Move to the Waiting to Charge state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_DCDC_MODE_WAITING_TO_CHARGE;
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+	printk("Battery charger: now waiting to charge (DCDC Mode)\n");
+#endif
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Transition to the Waiting to Charge state.
+//!
+//! \fntype Function
+//!
+//! This function implements the transition to the Waiting to Charge state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static void TransitionToWaitingToCharge(void)
+{
+
+	//--------------------------------------------------------------------------
+	// Reset the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer = 0;
+
+	//--------------------------------------------------------------------------
+	// Reset the current ramp.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampReset();
+
+	//--------------------------------------------------------------------------
+	// Move to the Waiting to Charge state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_WAITING_TO_CHARGE;
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+	printk("Battery charger: now waiting to charge\n");
+#endif
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Transition to the Conditioning state.
+//!
+//! \fntype Function
+//!
+//! This function implements the transition to the Conditioning state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static void TransitionToConditioning(void)
+{
+
+	//--------------------------------------------------------------------------
+	// Reset the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer = 0;
+
+	//--------------------------------------------------------------------------
+	// Set up the current ramp for conditioning.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampSetTarget(g_ddi_bc_Configuration.u16ConditioningCurrent);
+
+	//--------------------------------------------------------------------------
+	// Move to the Conditioning state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_CONDITIONING;
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+	printk("Battery charger: now conditioning\n");
+#endif
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Transition to the Charging state.
+//!
+//! \fntype Function
+//!
+//! This function implements the transition to the Charging state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static void TransitionToCharging(void)
+{
+
+	//--------------------------------------------------------------------------
+	// Reset the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer = 0;
+
+	//--------------------------------------------------------------------------
+	// Set up the current ramp for charging.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampSetTarget(g_ddi_bc_Configuration.u16ChargingCurrent);
+
+	//--------------------------------------------------------------------------
+	// We'll be finished charging when the current flow drops below this level.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_hwSetCurrentThreshold(g_ddi_bc_Configuration.
+				     u16ChargingThresholdCurrent);
+
+	//--------------------------------------------------------------------------
+	// Move to the Charging state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_CHARGING;
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+	printk("Battery charger: now charging\n");
+#endif
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Transition to the Topping Off state.
+//!
+//! \fntype Function
+//!
+//! This function implements the transition to the Topping Off state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static void TransitionToToppingOff(void)
+{
+
+	//--------------------------------------------------------------------------
+	// Reset the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer = 0;
+
+	//--------------------------------------------------------------------------
+	// Set up the current ramp for topping off.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampSetTarget(g_ddi_bc_Configuration.u16ChargingCurrent);
+
+	//--------------------------------------------------------------------------
+	// Move to the Topping Off state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_TOPPING_OFF;
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+	printk("Battery charger: now topping off\n");
+#endif
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Transition to the Broken state.
+//!
+//! \fntype Function
+//!
+//! This function implements the transition to the Broken state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static void TransitionToBroken(void)
+{
+
+	//--------------------------------------------------------------------------
+	// Reset the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer = 0;
+
+	//--------------------------------------------------------------------------
+	// Reset the current ramp.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampReset();
+
+	//--------------------------------------------------------------------------
+	// Move to the Broken state.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_State = DDI_BC_STATE_BROKEN;
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+	printk("Battery charger: declaring a broken battery\n");
+#endif
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Uninitialized state function.
+//!
+//! \fntype Function
+//!
+//! This function implements the Uninitialized state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static ddi_bc_Status_t ddi_bc_Uninitialized(void)
+{
+
+	//--------------------------------------------------------------------------
+	// The first order of business is to update alarms.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampUpdateAlarms();
+
+	//--------------------------------------------------------------------------
+	// Increment the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer += g_ddi_bc_Configuration.u32StateMachinePeriod;
+
+	//--------------------------------------------------------------------------
+	// The only way to leave this state is with a call to ddi_bc_Initialize. So,
+	// calling this state function does nothing.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Broken state function.
+//!
+//! \fntype Function
+//!
+//! This function implements the Broken state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static ddi_bc_Status_t ddi_bc_Broken(void)
+{
+
+	//--------------------------------------------------------------------------
+	// The first order of business is to update alarms.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampUpdateAlarms();
+
+	//--------------------------------------------------------------------------
+	// Increment the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer += g_ddi_bc_Configuration.u32StateMachinePeriod;
+
+	//--------------------------------------------------------------------------
+	// The only way to leave this state is with a call to ddi_bc_SetFixed. So,
+	// calling this state function does nothing.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Disabled state function.
+//!
+//! \fntype Function
+//!
+//! This function implements the Disabled state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static ddi_bc_Status_t ddi_bc_Disabled(void)
+{
+
+	//--------------------------------------------------------------------------
+	// The first order of business is to update alarms.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampUpdateAlarms();
+
+	//--------------------------------------------------------------------------
+	// Increment the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer += g_ddi_bc_Configuration.u32StateMachinePeriod;
+
+	//--------------------------------------------------------------------------
+	// The only way to leave this state is with a call to ddi_bc_SetEnable. So,
+	// calling this state function does nothing.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Waitin to Charge state function.
+//!
+//! \fntype Function
+//!
+//! This function implements the Waiting to Charge state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static ddi_bc_Status_t ddi_bc_DcdcModeWaitingToCharge(void)
+{
+
+	uint16_t u16BatteryVoltage;
+
+	//--------------------------------------------------------------------------
+	// The first order of business is to update alarms.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampUpdateAlarms();
+
+	//--------------------------------------------------------------------------
+	// Increment the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer += g_ddi_bc_Configuration.u32StateMachinePeriod;
+
+	//--------------------------------------------------------------------------
+	// Check if the power supply is present. If not, we're not going anywhere.
+	//--------------------------------------------------------------------------
+
+	if (!ddi_bc_hwPowerSupplyIsPresent()) {
+		TransitionToCharging();
+		return (DDI_BC_STATUS_SUCCESS);
+	}
+	//--------------------------------------------------------------------------
+	// If control arrives here, we're connected to a power supply. Have a look
+	// at the battery voltage.
+	//--------------------------------------------------------------------------
+
+	u16BatteryVoltage = ddi_bc_hwGetBatteryVoltage();
+
+	//--------------------------------------------------------------------------
+	// If topping off has not yet occurred, we do not care if DCDCs are on or not.
+	// If we have already topped off at least once, then we want to delay so that
+	// we give the battery a chance to discharge some instead of constantly topping
+	// it off.
+	//--------------------------------------------------------------------------
+
+	if (ddi_bc_hwIsDcdcOn()) {
+
+		//--------------------------------------------------------------------------
+		// If DCDCs have turned on, restert the DCDCInactivityTimer;
+		//--------------------------------------------------------------------------
+		DcdcInactityTimer = 0;
+
+		//--------------------------------------------------------------------------
+		// If the battery voltage measurement is at or below the LowDcdcBatteryVoltage
+		// level, we should definetly start charging the battery.  The
+		// LowDcdcBatteryVoltage value should be low enough to account for IR voltage
+		// drop from the battery under heavy DCDC load.
+		//--------------------------------------------------------------------------
+
+		if (u16BatteryVoltage <
+		    g_ddi_bc_Configuration.u16LowDcdcBatteryVoltage_mv) {
+			bRestartChargeCycle = true;
+
+		} else {
+			return (DDI_BC_STATUS_SUCCESS);
+		}
+	} else if (DcdcInactityTimer < DCDC_INACTIVITY_TIMER_THRESHOLD) {
+		DcdcInactityTimer +=
+		    g_ddi_bc_Configuration.u32StateMachinePeriod;
+		if (DcdcInactityTimer >= DCDC_INACTIVITY_TIMER_THRESHOLD) {
+			bRestartChargeCycle = true;
+		}
+	}
+
+	if (bRestartChargeCycle) {
+		bRestartChargeCycle = false;
+		// start charging
+		if (u16BatteryVoltage <
+		    g_ddi_bc_Configuration.u16ConditioningThresholdVoltage) {
+
+			//----------------------------------------------------------------------
+			// If control arrives here, the battery is very low and it needs to be
+			// conditioned.
+			//----------------------------------------------------------------------
+
+			TransitionToConditioning();
+		} else {
+
+			//----------------------------------------------------------------------
+			// If control arrives here, the battery isn't too terribly low.
+			//----------------------------------------------------------------------
+
+			TransitionToCharging();
+		}
+
+	}
+
+	return (DDI_BC_STATUS_SUCCESS);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Waitin to Charge state function.
+//!
+//! \fntype Function
+//!
+//! This function implements the Waiting to Charge state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static ddi_bc_Status_t ddi_bc_WaitingToCharge(void)
+{
+	uint16_t u16BatteryVoltage;
+	//--------------------------------------------------------------------------
+	// The first order of business is to update alarms.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampUpdateAlarms();
+
+	//--------------------------------------------------------------------------
+	// Increment the state timer.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer += g_ddi_bc_Configuration.u32StateMachinePeriod;
+
+	//--------------------------------------------------------------------------
+	// Check if the power supply is present. If not, we're not going anywhere.
+	//--------------------------------------------------------------------------
+
+	if (!ddi_bc_hwPowerSupplyIsPresent()) {
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+		u16ExternalBatteryPowerVoltageCheck = 0;
+#endif
+		return (DDI_BC_STATUS_SUCCESS);
+	}
+	//--------------------------------------------------------------------------
+	// If control arrives here, we're connected to a power supply. Have a look
+	// at the battery voltage.
+	//--------------------------------------------------------------------------
+
+	u16BatteryVoltage = ddi_bc_hwGetBatteryVoltage();
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+	if (u16ExternalBatteryPowerVoltageCheck) {
+		if ((u16ExternalBatteryPowerVoltageCheck - u16BatteryVoltage) >
+		    300) {
+			//----------------------------------------------------------------------
+			// If control arrives here, battery voltage has dropped too quickly after
+			// the first charge cycle.  We think an external voltage regulator is
+			// connected.  If the DCDCs are on and this voltage drops too quickly,
+			// it will cause large droops and possible brownouts so we disable
+			// charging.
+			//----------------------------------------------------------------------
+
+			ddi_bc_gBrokenReason =
+			    DDI_BC_BROKEN_EXTERNAL_BATTERY_VOLTAGE_DETECTED;
+
+			TransitionToBroken();
+
+			//----------------------------------------------------------------------
+			// Tell our caller the battery appears to be broken.
+			//----------------------------------------------------------------------
+
+			return (DDI_BC_STATUS_BROKEN);
+		} else {
+			// reset this check
+			u16ExternalBatteryPowerVoltageCheck = 0;
+		}
+
+	}
+#endif
+
+	//--------------------------------------------------------------------------
+	// In addition to 5V, is DCDC on also?  If so, swith to DCDC Mode Waiting
+	// to Charge state.
+	//--------------------------------------------------------------------------
+
+	if (ddi_bc_hwIsDcdcOn()) {
+		TransitionToDcdcModeWaitingToCharge();
+		return (DDI_BC_STATUS_SUCCESS);
+	}
+
+	//--------------------------------------------------------------------------
+	// If the battery voltage isn't low, we don't need to be charging it. We
+	// use a 5% margin to decide.
+	//--------------------------------------------------------------------------
+
+	if (!bRestartChargeCycle) {
+		uint16_t x;
+
+		x = u16BatteryVoltage + (u16BatteryVoltage / 20);
+
+		if (x >= g_ddi_bc_Configuration.u16ChargingVoltage)
+			return (DDI_BC_STATUS_SUCCESS);
+
+	}
+
+	bRestartChargeCycle = false;
+	//--------------------------------------------------------------------------
+	// If control arrives here, the battery is low. How low?
+	//--------------------------------------------------------------------------
+
+	if (u16BatteryVoltage <
+	    g_ddi_bc_Configuration.u16ConditioningThresholdVoltage) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, the battery is very low and it needs to be
+		// conditioned.
+		//----------------------------------------------------------------------
+
+		TransitionToConditioning();
+
+	} else {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, the battery isn't too terribly low.
+		//----------------------------------------------------------------------
+
+		TransitionToCharging();
+
+	}
+
+	//--------------------------------------------------------------------------
+	// Return success.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Conditioning state function.
+//!
+//! \fntype Function
+//!
+//! This function implements the Conditioning state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static ddi_bc_Status_t ddi_bc_Conditioning(void)
+{
+
+	//--------------------------------------------------------------------------
+	// The first order of business is to update alarms.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampUpdateAlarms();
+
+	//--------------------------------------------------------------------------
+	// If we're not under an alarm, increment the state timer.
+	//--------------------------------------------------------------------------
+
+	if (!ddi_bc_RampGetDieTempAlarm() && !ddi_bc_RampGetBatteryTempAlarm()) {
+		g_ddi_bc_u32StateTimer +=
+		    g_ddi_bc_Configuration.u32StateMachinePeriod;
+	}
+	//--------------------------------------------------------------------------
+	// Check if the power supply is still around.
+	//--------------------------------------------------------------------------
+
+	if (!ddi_bc_hwPowerSupplyIsPresent()) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, the power supply has been removed. Go back
+		// and wait.
+		//----------------------------------------------------------------------
+
+		TransitionToWaitingToCharge();
+
+		//----------------------------------------------------------------------
+		// Return success.
+		//----------------------------------------------------------------------
+
+		return (DDI_BC_STATUS_SUCCESS);
+
+	}
+
+	//--------------------------------------------------------------------------
+	// If control arrives here, we're still connected to a power supply.
+	// Check if a battery is connected.  If the voltage rises to high with only
+	// conditioning charge current, we determine that a battery is not connected.
+	// If that is not the case and a battery is connected, check
+	// if the battery voltage indicates it still needs conditioning.
+	//--------------------------------------------------------------------------
+
+//	if (ddi_bc_hwGetBatteryVoltage() >= 3900) {
+	if ((ddi_bc_hwGetBatteryVoltage() >
+	     g_ddi_bc_Configuration.u16ConditioningMaxVoltage) &&
+	    (ddi_power_GetMaxBatteryChargeCurrent() <
+	     g_ddi_bc_Configuration.u16ConditioningCurrent)) {
+		//----------------------------------------------------------------------
+		// If control arrives here, voltage has risen too quickly for so
+		// little charge being applied so their must be no battery connected.
+		//----------------------------------------------------------------------
+
+		ddi_bc_gBrokenReason = DDI_BC_BROKEN_NO_BATTERY_DETECTED;
+
+		TransitionToBroken();
+
+		//----------------------------------------------------------------------
+		// Tell our caller the battery appears to be broken.
+		//----------------------------------------------------------------------
+
+		return (DDI_BC_STATUS_BROKEN);
+
+	}
+
+	if (ddi_bc_hwGetBatteryVoltage() >=
+	    g_ddi_bc_Configuration.u16ConditioningMaxVoltage) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, this battery no longer needs conditioning.
+		//----------------------------------------------------------------------
+
+		TransitionToCharging();
+
+		//----------------------------------------------------------------------
+		// Return success.
+		//----------------------------------------------------------------------
+
+		return (DDI_BC_STATUS_SUCCESS);
+
+	}
+	//--------------------------------------------------------------------------
+	// Have we been in this state too long?
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_u32StateTimer >=
+	    g_ddi_bc_Configuration.u32ConditioningTimeout) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, we've been here too long.
+		//----------------------------------------------------------------------
+
+		ddi_bc_gBrokenReason = DDI_BC_BROKEN_CHARGING_TIMEOUT;
+
+		TransitionToBroken();
+
+		//----------------------------------------------------------------------
+		// Tell our caller the battery appears to be broken.
+		//----------------------------------------------------------------------
+
+		return (DDI_BC_STATUS_BROKEN);
+
+	}
+	//--------------------------------------------------------------------------
+	// If control arrives here, we're staying in this state. Step the current
+	// ramp.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampStep(g_ddi_bc_Configuration.u32StateMachinePeriod);
+
+	//--------------------------------------------------------------------------
+	// Return success.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Charging state function.
+//!
+//! \fntype Function
+//!
+//! This function implements the Charging state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static ddi_bc_Status_t ddi_bc_Charging(void)
+{
+
+	//--------------------------------------------------------------------------
+	// This variable counts the number of times we've seen the charging status
+	// bit cleared.
+	//--------------------------------------------------------------------------
+
+	static int iStatusCount = 0;
+	bool bIsDcdcOn;
+
+	//--------------------------------------------------------------------------
+	// The first order of business is to update alarms.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampUpdateAlarms();
+
+	//--------------------------------------------------------------------------
+	// If we're not under an alarm, increment the state timer.
+	//--------------------------------------------------------------------------
+
+	if (!ddi_bc_RampGetDieTempAlarm() && !ddi_bc_RampGetBatteryTempAlarm()) {
+		g_ddi_bc_u32StateTimer +=
+		    g_ddi_bc_Configuration.u32StateMachinePeriod;
+	}
+	//--------------------------------------------------------------------------
+	// Check if the power supply is still around.
+	//--------------------------------------------------------------------------
+
+	if (!ddi_bc_hwPowerSupplyIsPresent()) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, the power supply has been removed. Go back
+		// and wait.
+		//----------------------------------------------------------------------
+
+		TransitionToWaitingToCharge();
+
+		//----------------------------------------------------------------------
+		// Return success.
+		//----------------------------------------------------------------------
+
+		return (DDI_BC_STATUS_SUCCESS);
+
+	}
+	//--------------------------------------------------------------------------
+	// If control arrives here, we're still connected to a power supply. We need
+	// to decide now if the battery is still charging, or if it's nearly full.
+	// If it's still charging, we'll stay in this state. Otherwise, we'll move
+	// to the Topping Off state.
+	//
+	// Most of the time, we decide that the battery is still charging simply by
+	// checking if the the actual current flow is above the charging threshold
+	// current (as indicated by the charge status bit). However, if we're
+	// still ramping up to full charging current, the hardware may still be set
+	// to deliver an amount that's less than the threshold. In that case, the
+	// charging status bit would *definitely* show a low charging current, but
+	// that doesn't mean the battery is ready for topping off.
+	//
+	// So, in summary, we will move to the Topping Off state if both of the
+	// following are true:
+	//
+	//  1) The maximum current set in the hardware is greater than the charging
+	//     threshold.
+	//  -AND-
+	//  2) The actual current flow is also higher than the threshold (as
+	//     indicated by the charge status bit).
+	//
+	//--------------------------------------------------------------------------
+
+	bIsDcdcOn = ddi_bc_hwIsDcdcOn();
+	if (bIsDcdcOn) {
+		ddi_bc_hwSetCurrentThreshold(g_ddi_bc_Configuration.
+					     u16DdcdModeChargingThresholdCurrent);
+	} else {
+		ddi_bc_hwSetCurrentThreshold(g_ddi_bc_Configuration.
+					     u16ChargingThresholdCurrent);
+	}
+
+	{
+		uint16_t u16ActualProgrammedCurrent = ddi_bc_hwGetMaxCurrent();
+
+		//----------------------------------------------------------------------
+		// Get the Maximum current that we will ramp to.
+		//----------------------------------------------------------------------
+
+		//----------------------------------------------------------------------
+		// Not all possible values are expressible by the BATTCHRG_I bitfield.
+		// The following coverts the max current value into the the closest hardware
+		// expressible bitmask equivalent.  Then, it converts this back to the actual
+		// decimal current value that this bitmask represents.
+		//----------------------------------------------------------------------
+
+		uint16_t u16CurrentRampTarget = ddi_bc_RampGetTarget();
+
+		if (u16CurrentRampTarget > ddi_bc_RampGetLimit())
+			u16CurrentRampTarget = ddi_bc_RampGetLimit();
+
+		//----------------------------------------------------------------------
+		// Not all possible values are expressible by the BATTCHRG_I bitfield.
+		// The following coverts the max current value into the the closest hardware
+		// expressible bitmask equivalent.  Then, it converts this back to the actual
+		// decimal current value that this bitmask represents.
+		//----------------------------------------------------------------------
+
+		u16CurrentRampTarget =
+		    ddi_bc_hwExpressibleCurrent(u16CurrentRampTarget);
+
+		//----------------------------------------------------------------------
+		// We want to wait before we check the charge status bit until the ramping
+		// up is complete.  Because the charge status bit is noisy, we want to
+		// disregard it until the programmed charge currint in BATTCHRG_I is well
+		// beyond the STOP_ILIMIT value.
+		//----------------------------------------------------------------------
+		if ((u16ActualProgrammedCurrent >= u16CurrentRampTarget) &&
+		    !ddi_bc_hwGetChargeStatus()) {
+			uint8_t u8IlimitThresholdLimit;
+			//----------------------------------------------------------------------
+			// If control arrives here, the hardware flag is telling us that the
+			// charging current has fallen below the threshold. We need to see this
+			// happen twice consecutively before we believe it. Increment the count.
+			//----------------------------------------------------------------------
+
+			iStatusCount++;
+
+			//----------------------------------------------------------------------
+			// If we are in DCDC operting mode, we only need this criteria to be true
+			// once before we advance to topping off.  In 5V only mode, we want 10
+			// consecutive times before advancing to topping off.
+			//----------------------------------------------------------------------
+
+			if (bIsDcdcOn)
+				u8IlimitThresholdLimit = 1;
+			else
+				u8IlimitThresholdLimit = 10;
+
+			//----------------------------------------------------------------------
+			// How many times in a row have we seen this status bit low?
+			//----------------------------------------------------------------------
+
+			if (iStatusCount >= u8IlimitThresholdLimit) {
+
+				//------------------------------------------------------------------
+				// If control arrives here, we've seen the CHRGSTS bit low too many
+				// times. This means it's time to move back to the waiting to charge
+				// state if DCDCs are present or move to the Topping Off state if
+				// no DCDCs are present.  Because we can't measure only the current
+				// going to the battery when the DCDCs are active, we don't know when
+				// to start topping off or how long to top off.
+				//
+				// First, reset the status count for the next time we're in this
+				// state.
+				//------------------------------------------------------------------
+
+				iStatusCount = 0;
+
+#ifdef CONFIG_POWER_SUPPLY_DEBUG
+				u16ExternalBatteryPowerVoltageCheck =
+				    ddi_bc_hwGetBatteryVoltage();
+#endif
+
+				if (bIsDcdcOn) {
+					// We will restart the charge cycle once the DCDCs are no
+					// longer present.
+					DcdcInactityTimer = 0;
+
+					TransitionToWaitingToCharge();
+				} else if (g_ddi_bc_u32StateTimer <=
+					   TRANSITION_TO_TOPOFF_MINIMUM_CHARGE_TIME_mS)
+				{
+					//------------------------------------------------------------------
+					// If we haven't actually didn't have to charge very long
+					// then the battery was already full.  In this case, we do
+					// not top off so that we do not needlessly overcharge the
+					//  battery.
+					//------------------------------------------------------------------
+					TransitionToWaitingToCharge();
+				} else {
+
+					//------------------------------------------------------------------
+					// Move to the Topping Off state.
+					//------------------------------------------------------------------
+
+					TransitionToToppingOff();
+				}
+				//------------------------------------------------------------------
+				// Return success.
+				//------------------------------------------------------------------
+
+				return (DDI_BC_STATUS_SUCCESS);
+
+			}
+
+		} else {
+
+			//----------------------------------------------------------------------
+			// If control arrives here, the battery is still charging. Clear the
+			// status count.
+			//----------------------------------------------------------------------
+
+			iStatusCount = 0;
+
+		}
+
+	}
+
+	//--------------------------------------------------------------------------
+	// Have we been in this state too long?
+	//--------------------------------------------------------------------------
+
+	if (g_ddi_bc_u32StateTimer >= g_ddi_bc_Configuration.u32ChargingTimeout) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, we've been here too long.
+		//----------------------------------------------------------------------
+
+		ddi_bc_gBrokenReason = DDI_BC_BROKEN_CHARGING_TIMEOUT;
+
+		TransitionToBroken();
+
+		//----------------------------------------------------------------------
+		// Tell our caller the battery appears to be broken.
+		//----------------------------------------------------------------------
+
+		return (DDI_BC_STATUS_BROKEN);
+
+	}
+	//--------------------------------------------------------------------------
+	// If control arrives here, we're staying in this state. Step the current
+	// ramp.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampStep(g_ddi_bc_Configuration.u32StateMachinePeriod);
+
+	//--------------------------------------------------------------------------
+	// Return success.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Topping Off state function.
+//!
+//! \fntype Function
+//!
+//! This function implements the Topping Off state.
+//!
+////////////////////////////////////////////////////////////////////////////////
+static ddi_bc_Status_t ddi_bc_ToppingOff(void)
+{
+
+	//--------------------------------------------------------------------------
+	// The first order of business is to update alarms.
+
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampUpdateAlarms();
+
+	//--------------------------------------------------------------------------
+	// Increment the state timer. Notice that, unlike other states, we increment
+	// the state timer whether or not we're under an alarm.
+	//--------------------------------------------------------------------------
+
+	g_ddi_bc_u32StateTimer += g_ddi_bc_Configuration.u32StateMachinePeriod;
+
+	//--------------------------------------------------------------------------
+	// Check if the power supply is still around.
+	//--------------------------------------------------------------------------
+
+	if (!ddi_bc_hwPowerSupplyIsPresent()) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, the power supply has been removed. Go back
+		// and wait.
+		//---------------------------------------------------------------------
+
+		TransitionToWaitingToCharge();
+
+		//----------------------------------------------------------------------
+		// Return success.
+		//----------------------------------------------------------------------
+
+		return (DDI_BC_STATUS_SUCCESS);
+
+	}
+
+	//--------------------------------------------------------------------------
+	// Are we done topping off?
+	//--------------------------------------------------------------------------
+	if (g_ddi_bc_u32StateTimer >= g_ddi_bc_Configuration.u32TopOffPeriod) {
+
+		//----------------------------------------------------------------------
+		// If control arrives here, we're done topping off.
+		//----------------------------------------------------------------------
+
+		TransitionToWaitingToCharge();
+
+	}
+	//--------------------------------------------------------------------------
+	// If control arrives here, we're staying in this state. Step the current
+	// ramp.
+	//--------------------------------------------------------------------------
+
+	ddi_bc_RampStep(g_ddi_bc_Configuration.u32StateMachinePeriod);
+
+	//--------------------------------------------------------------------------
+	// Return success.
+	//--------------------------------------------------------------------------
+
+	return (DDI_BC_STATUS_SUCCESS);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// End of file
+////////////////////////////////////////////////////////////////////////////////
+//! @}
diff --git a/drivers/power/stmp37xx/ddi_bc_sm.h b/drivers/power/stmp37xx/ddi_bc_sm.h
new file mode 100644
index 000000000000..0e5fd8a1f144
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_bc_sm.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+//! \addtogroup ddi_bc
+//! @{
+//
+// Copyright (c) 2004-2005 SigmaTel, Inc.
+//
+//! \file ddi_bc_sm.h
+//! \brief Header file for the Battery Charger state machine.
+//! \date 06/2005
+//!
+//! This file contains declarations for the Battery Charger state machine.
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef _DDI_BC_SM_H
+#define _DDI_BC_SM_H
+
+////////////////////////////////////////////////////////////////////////////////
+// Externs
+////////////////////////////////////////////////////////////////////////////////
+
+//! The current state.
+
+extern ddi_bc_State_t g_ddi_bc_State;
+
+//! The state function table.
+
+extern ddi_bc_Status_t(*const (stateFunctionTable[])) (void);
+
+////////////////////////////////////////////////////////////////////////////////
+// End of file
+////////////////////////////////////////////////////////////////////////////////
+#endif				// _DDI_BC_H
+//! @}
diff --git a/drivers/power/stmp37xx/ddi_power_battery.c b/drivers/power/stmp37xx/ddi_power_battery.c
new file mode 100644
index 000000000000..b474e53ac9fa
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_power_battery.c
@@ -0,0 +1,1035 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+//! \addtogroup ddi_power
+//! @{
+//
+// Copyright(C) 2005 SigmaTel, Inc.
+//
+//! \file ddi_power_battery.c
+//! \brief Implementation file for the power driver battery charger.
+//!
+////////////////////////////////////////////////////////////////////////////////
+//   Includes and external references
+////////////////////////////////////////////////////////////////////////////////
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <asm/processor.h> /* cpu_relax */
+#include <mach/hardware.h>
+#include <mach/ddi_bc.h>
+#include <mach/lradc.h>
+#include <mach/regs-power.h>
+#include <mach/regs-lradc.h>
+#include "ddi_bc_internal.h"
+
+//! \brief Base voltage to start battery calculations for LiIon
+#define BATT_BRWNOUT_LIION_BASE_MV 2800
+//! \brief Constant to help with determining whether to round up or
+//! not during calculation
+#define BATT_BRWNOUT_LIION_CEILING_OFFSET_MV 39
+//! \brief Number of mV to add if rounding up in LiIon mode
+#define BATT_BRWNOUT_LIION_LEVEL_STEP_MV 40
+//! \brief Constant value to be calculated by preprocessing
+#define BATT_BRWNOUT_LIION_EQN_CONST \
+	(BATT_BRWNOUT_LIION_BASE_MV - BATT_BRWNOUT_LIION_CEILING_OFFSET_MV)
+//! \brief Base voltage to start battery calculations for Alkaline/NiMH
+#define BATT_BRWNOUT_ALKAL_BASE_MV 800
+//! \brief Constant to help with determining whether to round up or
+//! not during calculation
+#define BATT_BRWNOUT_ALKAL_CEILING_OFFSET_MV 19
+//! \brief Number of mV to add if rounding up in Alkaline/NiMH mode
+#define BATT_BRWNOUT_ALKAL_LEVEL_STEP_MV 20
+//! \brief Constant value to be calculated by preprocessing
+#define BATT_BRWNOUT_ALKAL_EQN_CONST \
+	(BATT_BRWNOUT_ALKAL_BASE_MV - BATT_BRWNOUT_ALKAL_CEILING_OFFSET_MV)
+
+#define GAIN_CORRECTION 1012    // 1.012
+
+#define VBUSVALID_THRESH_2_90V		0x0
+#define VBUSVALID_THRESH_4_00V		0x1
+#define VBUSVALID_THRESH_4_10V		0x2
+#define VBUSVALID_THRESH_4_20V		0x3
+#define VBUSVALID_THRESH_4_30V		0x4
+#define VBUSVALID_THRESH_4_40V		0x5
+#define VBUSVALID_THRESH_4_50V		0x6
+#define VBUSVALID_THRESH_4_60V		0x7
+
+#define LINREG_OFFSET_STEP_BELOW	0x2
+#define BP_POWER_BATTMONITOR_BATT_VAL	16
+#define BP_POWER_CHARGE_BATTCHRG_I	0
+#define BP_POWER_CHARGE_STOP_ILIMIT	8
+
+#define VDD4P2_ENABLED
+
+////////////////////////////////////////////////////////////////////////////////
+// Globals & Variables
+////////////////////////////////////////////////////////////////////////////////
+
+
+/* Select your 5V Detection method */
+ static ddi_power_5vDetection_t DetectionMethod =
+			DDI_POWER_5V_VDD5V_GT_VDDIO;
+/* static ddi_power_5vDetection_t DetectionMethod = DDI_POWER_5V_VBUSVALID; */
+
+////////////////////////////////////////////////////////////////////////////////
+// Code
+////////////////////////////////////////////////////////////////////////////////
+
+#if 0
+static void dump_regs(void)
+{
+	printk("HW_POWER_CHARGE      0x%08x\n", HW_POWER_CHARGE_RD());
+	printk("HW_POWER_STS         0x%08x\n", HW_POWER_STS_RD());
+	printk("HW_POWER_BATTMONITOR 0x%08x\n", HW_POWER_BATTMONITOR_RD());
+}
+#endif
+
+//! This array maps bit numbers to current increments, as used in the register
+//! fields HW_POWER_CHARGE.STOP_ILIMIT and HW_POWER_CHARGE.BATTCHRG_I.
+static const uint16_t currentPerBit[] = {  10,  20,  50, 100, 200, 400 };
+
+uint16_t ddi_power_convert_current_to_setting(uint16_t u16Current)
+{
+	int       i;
+	uint16_t  u16Mask;
+	uint16_t  u16Setting = 0;
+
+	// Scan across the bit field, adding in current increments.
+	u16Mask = (0x1 << 5);
+
+	for (i = 5; (i >= 0) && (u16Current > 0); i--, u16Mask >>= 1) {
+		if (u16Current >= currentPerBit[i]) {
+			u16Current -= currentPerBit[i];
+			u16Setting |= u16Mask;
+		}
+	}
+
+	// Return the result.
+	return(u16Setting);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//! See hw_power.h for details.
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_power_convert_setting_to_current(uint16_t u16Setting)
+{
+	int       i;
+	uint16_t  u16Mask;
+	uint16_t  u16Current = 0;
+
+	// Scan across the bit field, adding in current increments.
+	u16Mask = (0x1 << 5);
+
+	for (i = 5; i >= 0; i--, u16Mask >>= 1) {
+		if (u16Setting & u16Mask) u16Current += currentPerBit[i];
+	}
+
+	// Return the result.
+	return(u16Current);
+}
+
+void ddi_power_Enable5vDetection(void)
+{
+	// Disable hardware power down when 5V is inserted or removed
+	HW_POWER_5VCTRL_CLR(BM_POWER_5VCTRL_PWDN_5VBRNOUT);
+
+	/* Enabling VBUSVALID hardware detection even if VDD5V_GT_VDDIO
+	 * is the detection method being used for 5V status (hardware
+	 * or software).  This is in case any other drivers (such as
+	 * USB) are specifically monitoring VBUSVALID status
+	 */
+	HW_POWER_5VCTRL_SET(BM_POWER_5VCTRL_VBUSVALID_5VDETECT);
+
+	// Set 5V detection threshold to 4.3V for VBUSVALID.
+	HW_POWER_5VCTRL_SET(
+		BF_POWER_5VCTRL_VBUSVALID_TRSH(VBUSVALID_THRESH_4_30V));
+
+	// gotta set LINREG_OFFSET to STEP_BELOW according to manual
+	HW_POWER_VDDIOCTRL_SET(
+		BF_POWER_VDDIOCTRL_LINREG_OFFSET(LINREG_OFFSET_STEP_BELOW));
+	HW_POWER_VDDACTRL_SET(
+		BF_POWER_VDDACTRL_LINREG_OFFSET(LINREG_OFFSET_STEP_BELOW));
+	HW_POWER_VDDDCTRL_SET(
+		BF_POWER_VDDDCTRL_LINREG_OFFSET(LINREG_OFFSET_STEP_BELOW));
+
+	/* Clear vbusvalid interrupt flag */
+	HW_POWER_CTRL_CLR(BM_POWER_CTRL_VBUSVALID_IRQ);
+	HW_POWER_CTRL_CLR(BM_POWER_CTRL_VDD5V_GT_VDDIO_IRQ);
+
+
+	/* enable 5V Detection interrupt vbusvalid irq */
+	switch (DetectionMethod) {
+	case DDI_POWER_5V_VBUSVALID:
+		/* Check VBUSVALID for 5V present */
+		HW_POWER_CTRL_SET(BM_POWER_CTRL_ENIRQ_VBUS_VALID);
+		break;
+	case DDI_POWER_5V_VDD5V_GT_VDDIO:
+		/* Check VDD5V_GT_VDDIO for 5V present */
+		HW_POWER_CTRL_SET(BM_POWER_CTRL_ENIRQ_VDD5V_GT_VDDIO);
+		break;
+	}
+}
+
+/*
+ * This function prepares the hardware for a 5V-to-battery handoff. It assumes
+ * the current configuration is using 5V as the power source.  The 5V
+ * interrupt will be set up for a 5V removal.
+ */
+void ddi_power_enable_5v_to_battery_handoff(void)
+{
+	/* Clear vbusvalid interrupt flag */
+	HW_POWER_CTRL_CLR(BM_POWER_CTRL_VBUSVALID_IRQ);
+	HW_POWER_CTRL_CLR(BM_POWER_CTRL_VDD5V_GT_VDDIO_IRQ);
+
+	/* detect 5v unplug */
+	HW_POWER_CTRL_CLR(BM_POWER_CTRL_POLARITY_VBUSVALID);
+	HW_POWER_CTRL_CLR(BM_POWER_CTRL_POLARITY_VDD5V_GT_VDDIO);
+
+#ifndef VDD4P2_ENABLED
+	// Enable automatic transition to DCDC
+	HW_POWER_5VCTRL_SET(BM_POWER_5VCTRL_DCDC_XFER);
+#endif
+}
+
+/*
+ * This function will handle all the power rail transitions necesarry to power
+ * the chip from the battery when it was previously powered from the 5V power
+ * source.
+ */
+void ddi_power_execute_5v_to_battery_handoff(void)
+{
+
+
+#ifdef VDD4P2_ENABLED
+
+	HW_POWER_DCDC4P2_CLR(BM_POWER_DCDC4P2_ENABLE_DCDC |
+			BM_POWER_DCDC4P2_ENABLE_4P2);
+	HW_POWER_5VCTRL_SET(BM_POWER_5VCTRL_PWD_CHARGE_4P2);
+#else
+	// VDDD has different configurations depending on the battery type
+	// and battery level.
+
+	// For LiIon battery, we will use the DCDC to power VDDD.
+	// Use LinReg offset for DCDC mode.
+	HW_POWER_VDDDCTRL_SET(
+		BF_POWER_VDDDCTRL_LINREG_OFFSET(LINREG_OFFSET_STEP_BELOW));
+	// Turn on the VDDD DCDC output and turn off the VDDD LinReg output.
+	HW_POWER_VDDDCTRL_CLR(BM_POWER_VDDDCTRL_DISABLE_FET);
+	HW_POWER_VDDDCTRL_CLR(BM_POWER_VDDDCTRL_ENABLE_LINREG);
+	// Make sure stepping is enabled when using DCDC.
+	HW_POWER_VDDDCTRL_CLR(BM_POWER_VDDDCTRL_DISABLE_STEPPING);
+
+	// Power VDDA and VDDIO from the DCDC.
+
+	// Use LinReg offset for DCDC mode.
+	HW_POWER_VDDACTRL_SET(
+		BF_POWER_VDDACTRL_LINREG_OFFSET(LINREG_OFFSET_STEP_BELOW));
+	// Turn on the VDDA DCDC converter output and turn off LinReg output.
+	HW_POWER_VDDACTRL_CLR(BM_POWER_VDDACTRL_DISABLE_FET);
+	HW_POWER_VDDACTRL_CLR(BM_POWER_VDDACTRL_ENABLE_LINREG);
+	// Make sure stepping is enabled when using DCDC.
+	HW_POWER_VDDACTRL_CLR(BM_POWER_VDDACTRL_DISABLE_STEPPING);
+
+	// Use LinReg offset for DCDC mode.
+	HW_POWER_VDDIOCTRL_SET(
+		BF_POWER_VDDIOCTRL_LINREG_OFFSET(LINREG_OFFSET_STEP_BELOW));
+	// Turn on the VDDIO DCDC output and turn on the LinReg output.
+	HW_POWER_VDDIOCTRL_CLR(BM_POWER_VDDIOCTRL_DISABLE_FET);
+	HW_POWER_5VCTRL_CLR(BM_POWER_5VCTRL_ILIMIT_EQ_ZERO);
+	// Make sure stepping is enabled when using DCDC.
+	HW_POWER_VDDIOCTRL_CLR(BM_POWER_VDDIOCTRL_DISABLE_STEPPING);
+#endif
+}
+
+/*
+ * This function sets up battery-to-5V handoff. The power switch from
+ * battery to 5V is automatic. This funtion enables the 5V present detection
+ * such that the 5V interrupt can be generated if it is enabled. (The interrupt
+ * handler can inform software the 5V present event.) To deal with noise or
+ * a high current, this function enables DCDC1/2 based on the battery mode.
+ */
+void ddi_power_enable_battery_to_5v_handoff(void)
+{
+	/* Clear vbusvalid interrupt flag */
+	HW_POWER_CTRL_CLR(BM_POWER_CTRL_VBUSVALID_IRQ);
+	HW_POWER_CTRL_CLR(BM_POWER_CTRL_VDD5V_GT_VDDIO_IRQ);
+
+	/* detect 5v plug-in */
+	HW_POWER_CTRL_SET(BM_POWER_CTRL_POLARITY_VBUSVALID);
+	HW_POWER_CTRL_SET(BM_POWER_CTRL_POLARITY_VDD5V_GT_VDDIO);
+
+#ifndef VDD4P2_ENABLED
+	// Force current from 5V to be zero by disabling its entry source.
+	HW_POWER_5VCTRL_SET(BM_POWER_5VCTRL_ILIMIT_EQ_ZERO);
+#endif
+	// Allow DCDC be to active when 5V is present.
+	HW_POWER_5VCTRL_SET(BM_POWER_5VCTRL_ENABLE_DCDC);
+}
+
+/* This function handles the transitions on each of theVDD5V_GT_VDDIO power
+ * rails necessary to power the chip from the 5V power supply when it was
+ * previously powered from the battery power supply.
+ */
+void ddi_power_execute_battery_to_5v_handoff(void)
+{
+
+#ifdef VDD4P2_ENABLED
+
+	bool orig_vbusvalid_5vdetect = false;
+	bool orig_pwd_bo = false;
+	uint8_t orig_vbusvalid_threshold;
+
+
+	/* recording orignal values that will be modified
+	* temporarlily to handle a chip bug.  See chip errata
+	* for CQ ENGR00115837
+	*/
+	orig_vbusvalid_threshold =
+		(__raw_readl(HW_POWER_5VCTRL_ADDR)
+		& BM_POWER_5VCTRL_VBUSVALID_TRSH)
+		>> BP_POWER_5VCTRL_VBUSVALID_TRSH;
+
+	if (__raw_readl(HW_POWER_5VCTRL_ADDR) &
+		BM_POWER_5VCTRL_VBUSVALID_5VDETECT)
+			orig_vbusvalid_5vdetect = true;
+
+	if (__raw_readl(HW_POWER_MINPWR_ADDR) &
+		BM_POWER_MINPWR_PWD_BO)
+			orig_pwd_bo = true;
+
+	/* disable mechanisms that get erroneously tripped by
+	* when setting the DCDC4P2 EN_DCDC
+	*/
+	HW_POWER_5VCTRL_CLR(BM_POWER_5VCTRL_VBUSVALID_5VDETECT);
+	HW_POWER_5VCTRL_CLR(BF_POWER_5VCTRL_VBUSVALID_TRSH(0x7));
+	HW_POWER_5VCTRL_SET(BM_POWER_MINPWR_PWD_BO);
+
+	HW_POWER_DCDC4P2_SET(BM_POWER_DCDC4P2_ENABLE_4P2);
+
+
+	/* as a todo, we'll want to ramp up the charge current first
+	 * to minimize disturbances on the VDD5V rail
+	 */
+	ddi_power_SetChargerPowered(1);
+
+	/* Until the previous todo is completed, we'll want to give a delay
+	 * to allow the charging up of the 4p2 capacitor.
+	 */
+	udelay(10);
+
+
+	HW_POWER_DCDC4P2_SET(BM_POWER_DCDC4P2_ENABLE_DCDC);
+
+	udelay(20);
+	/* coming from a known value of 0 so this is ok */
+	HW_POWER_5VCTRL_SET(BF_POWER_5VCTRL_VBUSVALID_TRSH(
+			orig_vbusvalid_threshold));
+
+	if (orig_vbusvalid_5vdetect)
+		HW_POWER_5VCTRL_SET(BM_POWER_5VCTRL_VBUSVALID_5VDETECT);
+
+
+	if (!orig_pwd_bo)
+		HW_POWER_5VCTRL_CLR(BM_POWER_MINPWR_PWD_BO);
+
+
+
+
+
+#else
+
+	// Disable the DCDC during 5V connections.
+	HW_POWER_5VCTRL_CLR(BM_POWER_5VCTRL_ENABLE_DCDC);
+
+	// Power the VDDD/VDDA/VDDIO rail from the linear regulator.  The DCDC
+	// is ready to automatically power the chip when 5V is removed.
+	// Use this configuration when powering from 5V
+
+	// Use LinReg offset for LinReg mode
+	HW_POWER_VDDDCTRL_SET(
+		BF_POWER_VDDDCTRL_LINREG_OFFSET(LINREG_OFFSET_STEP_BELOW));
+	// Turn on the VDDD LinReg and turn on the VDDD DCDC output.  The
+	// ENABLE_DCDC must be cleared to avoid LinReg and DCDC conflict.
+	HW_POWER_VDDDCTRL_SET(BM_POWER_VDDDCTRL_ENABLE_LINREG);
+	HW_POWER_VDDDCTRL_CLR(BM_POWER_VDDDCTRL_DISABLE_FET);
+	// Make sure stepping is disabled when using linear regulators
+	HW_POWER_VDDDCTRL_SET(BM_POWER_VDDDCTRL_DISABLE_STEPPING);
+
+	// Use LinReg offset for LinReg mode
+	HW_POWER_VDDACTRL_SET(
+		BF_POWER_VDDACTRL_LINREG_OFFSET(LINREG_OFFSET_STEP_BELOW));
+	// Turn on the VDDA LinReg output and prepare the DCDC for transfer.
+	// ENABLE_DCDC must be clear to avoid DCDC and LinReg conflict.
+	HW_POWER_VDDACTRL_SET(BM_POWER_VDDACTRL_ENABLE_LINREG);
+	HW_POWER_VDDACTRL_CLR(BM_POWER_VDDACTRL_DISABLE_FET);
+	// Make sure stepping is disabled when using linear regulators
+	HW_POWER_VDDACTRL_SET(BM_POWER_VDDACTRL_DISABLE_STEPPING);
+
+	// Use LinReg offset for LinReg mode.
+	HW_POWER_VDDIOCTRL_SET(
+		BF_POWER_VDDIOCTRL_LINREG_OFFSET(LINREG_OFFSET_STEP_BELOW));
+	// Turn on the VDDIO LinReg output and prepare the VDDIO DCDC output.
+        // ENABLE_DCDC must be cleared to prevent DCDC and LinReg conflict.
+	HW_POWER_VDDIOCTRL_CLR(BM_POWER_VDDIOCTRL_DISABLE_FET);
+	HW_POWER_5VCTRL_CLR(BM_POWER_5VCTRL_ILIMIT_EQ_ZERO);
+	// Make sure stepping is disabled when using DCDC.
+	HW_POWER_VDDIOCTRL_SET(BM_POWER_VDDIOCTRL_DISABLE_STEPPING);
+#endif
+}
+
+void ddi_power_init_handoff(void)
+{
+	/* The following settings give optimal power supply capability */
+
+
+
+	// enable 5v presence detection
+	ddi_power_Enable5vDetection();
+
+	if (ddi_power_Get5vPresentFlag())
+		/* It's 5V mode, enable 5V-to-battery handoff */
+		ddi_power_enable_5v_to_battery_handoff();
+	else
+		/* It's battery mode, enable battery-to-5V handoff */
+		ddi_power_enable_battery_to_5v_handoff();
+
+	// Finally enable the battery adjust
+	HW_POWER_BATTMONITOR_SET(BM_POWER_BATTMONITOR_EN_BATADJ);
+}
+
+int ddi_power_init_battery(void)
+{
+	int ret;
+
+
+	/* the following code to enable automatic battery measurement
+	 * should have already been enabled in the boot prep files.  Not
+	 * sure if this is necessary or possibly suceptible to mis-coordination
+	 */
+	// Init LRADC channel 7
+	ret = hw_lradc_init_ladder(BATTERY_VOLTAGE_CH,
+				   LRADC_DELAY_TRIGGER_BATTERY,
+				   200);
+	if (ret) {
+		printk(KERN_ERR "%s: hw_lradc_init_ladder failed\n", __func__);
+		return ret;
+	}
+
+	HW_LRADC_CONVERSION_SET(BM_LRADC_CONVERSION_AUTOMATIC);
+
+	// Set li-ion mode
+	HW_LRADC_CONVERSION_SET(BF_LRADC_CONVERSION_SCALE_FACTOR(2));
+
+	// Turn off divide-by-two - we already have a divide-by-four
+	// as part of the hardware
+	HW_LRADC_CTRL2_CLR(
+		BF_LRADC_CTRL2_DIVIDE_BY_TWO(1 << BATTERY_VOLTAGE_CH));
+
+	HW_POWER_CHARGE_SET(BM_POWER_CHARGE_ENABLE_FAULT_DETECT);
+
+	// kick off the trigger
+	hw_lradc_set_delay_trigger_kick(LRADC_DELAY_TRIGGER_BATTERY, 1);
+
+	HW_POWER_LOOPCTRL_SET(BM_POWER_LOOPCTRL_RCSCALE_THRESH);
+	HW_POWER_LOOPCTRL_SET(BF_POWER_LOOPCTRL_EN_RCSCALE(3));
+
+	HW_POWER_MINPWR_CLR(BM_POWER_MINPWR_HALF_FETS);
+	HW_POWER_MINPWR_SET(BM_POWER_MINPWR_DOUBLE_FETS);
+
+	/* prepare handoff */
+	ddi_power_init_handoff();
+
+	return 0;
+}
+
+/*
+ * Use the the lradc7 channel dedicated for battery voltage measurement to
+ * get the die temperature from on-chip sensor.
+ */
+uint16_t MeasureInternalDieTemperature(void)
+{
+	uint32_t  ch8Value, ch9Value;
+
+	/* power up internal tep sensor block */
+	HW_LRADC_CTRL2_CLR(BM_LRADC_CTRL2_TEMPSENSE_PWD);
+
+	/* mux to the lradc 8th temp channel */
+	HW_LRADC_CTRL4_CLR(BF_LRADC_CTRL4_LRADC7SELECT(0xF));
+	HW_LRADC_CTRL4_SET(BF_LRADC_CTRL4_LRADC7SELECT(8));
+
+	/* Clear the interrupt flag */
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC7_IRQ);
+	HW_LRADC_CTRL0_SET(BF_LRADC_CTRL0_SCHEDULE(1 << BATTERY_VOLTAGE_CH));
+	// Wait for conversion complete
+        while (!(HW_LRADC_CTRL1_RD() & BM_LRADC_CTRL1_LRADC7_IRQ))
+                cpu_relax();
+	/* Clear the interrupt flag again */
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC7_IRQ);
+	// read temperature value and clr lradc
+	ch8Value = HW_LRADC_CHn_RD(BATTERY_VOLTAGE_CH) & BM_LRADC_CHn_VALUE;
+	HW_LRADC_CHn_CLR(BATTERY_VOLTAGE_CH, BM_LRADC_CHn_VALUE);
+
+	/* mux to the lradc 9th temp channel */
+	HW_LRADC_CTRL4_CLR(BF_LRADC_CTRL4_LRADC7SELECT(0xF));
+	HW_LRADC_CTRL4_SET(BF_LRADC_CTRL4_LRADC7SELECT(9));
+
+	/* Clear the interrupt flag */
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC7_IRQ);
+	HW_LRADC_CTRL0_SET(BF_LRADC_CTRL0_SCHEDULE(1 << BATTERY_VOLTAGE_CH));
+	// Wait for conversion complete
+        while (!(HW_LRADC_CTRL1_RD() & BM_LRADC_CTRL1_LRADC7_IRQ))
+                cpu_relax();
+	/* Clear the interrupt flag */
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC7_IRQ);
+	// read temperature value
+	ch9Value = HW_LRADC_CHn_RD(BATTERY_VOLTAGE_CH) & BM_LRADC_CHn_VALUE;
+	HW_LRADC_CHn_CLR(BATTERY_VOLTAGE_CH, BM_LRADC_CHn_VALUE);
+
+	/* power down temp sensor block */
+	HW_LRADC_CTRL2_SET(BM_LRADC_CTRL2_TEMPSENSE_PWD);
+
+	/* mux back to the lradc 7th battery voltage channel */
+	HW_LRADC_CTRL4_CLR(BF_LRADC_CTRL4_LRADC7SELECT(0xF));
+	HW_LRADC_CTRL4_SET(BF_LRADC_CTRL4_LRADC7SELECT(7));
+
+	/* Clear the interrupt flag */
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC7_IRQ);
+	HW_LRADC_CTRL0_SET(BF_LRADC_CTRL0_SCHEDULE(1 << BATTERY_VOLTAGE_CH));
+	// Wait for conversion complete
+        while (!(HW_LRADC_CTRL1_RD() & BM_LRADC_CTRL1_LRADC7_IRQ))
+                cpu_relax();
+	/* Clear the interrupt flag */
+	HW_LRADC_CTRL1_CLR(BM_LRADC_CTRL1_LRADC7_IRQ);
+
+	return (uint16_t)((ch9Value-ch8Value)*GAIN_CORRECTION/4000);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+//! Name: ddi_power_GetBatteryMode
+//!
+//! \brief
+////////////////////////////////////////////////////////////////////////////////
+ddi_power_BatteryMode_t ddi_power_GetBatteryMode(void)
+{
+#if 0
+	return (HW_POWER_STS_RD() & BM_POWER_STS_MODE) ?
+		DDI_POWER_BATT_MODE_ALKALINE_NIMH :
+		DDI_POWER_BATT_MODE_LIION;
+#endif
+	return DDI_POWER_BATT_MODE_LIION;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//! Name: ddi_power_GetBatteryChargerEnabled
+//!
+//! \brief
+////////////////////////////////////////////////////////////////////////////////
+bool ddi_power_GetBatteryChargerEnabled(void)
+{
+#if 0
+	return (HW_POWER_STS_RD() & BM_POWER_STS_BATT_CHRG_PRESENT) ? 1 : 0;
+#endif
+	return 1;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report if the charger hardware power is on.
+//!
+//! \fntype Function
+//!
+//! This function reports if the charger hardware power is on.
+//!
+//! \retval  Zero if the charger hardware is not powered. Non-zero otherwise.
+//!
+//! Note that the bit we're looking at is named PWD_BATTCHRG. The "PWD"
+//! stands for "power down". Thus, when the bit is set, the battery charger
+//! hardware is POWERED DOWN.
+////////////////////////////////////////////////////////////////////////////////
+bool ddi_power_GetChargerPowered(void)
+{
+	return (HW_POWER_CHARGE_RD() & BM_POWER_CHARGE_PWD_BATTCHRG) ? 0 : 1;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Turn the charging hardware on or off.
+//!
+//! \fntype Function
+//!
+//! This function turns the charging hardware on or off.
+//!
+//! \param[in]  on  Indicates whether the charging hardware should be on or off.
+//!
+//! Note that the bit we're looking at is named PWD_BATTCHRG. The "PWD"
+//! stands for "power down". Thus, when the bit is set, the battery charger
+//! hardware is POWERED DOWN.
+////////////////////////////////////////////////////////////////////////////////
+void ddi_power_SetChargerPowered(bool bPowerOn)
+{
+	// Hit the battery charge power switch.
+	if (bPowerOn) {
+		HW_POWER_CHARGE_CLR(BM_POWER_CHARGE_PWD_BATTCHRG);
+		HW_POWER_5VCTRL_CLR(BM_POWER_5VCTRL_PWD_CHARGE_4P2);
+	} else {
+		HW_POWER_CHARGE_SET(BM_POWER_CHARGE_PWD_BATTCHRG);
+#ifndef VDD4P2_ENABLED
+		HW_POWER_5VCTRL_SET(BM_POWER_5VCTRL_PWD_CHARGE_4P2);
+#endif
+	}
+
+//#ifdef CONFIG_POWER_SUPPLY_DEBUG
+#if 0
+	printk("Battery charger: charger %s\n", bPowerOn ? "ON!" : "OFF");
+	dump_regs();
+#endif
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Reports if the charging current has fallen below the threshold.
+//!
+//! \fntype Function
+//!
+//! This function reports if the charging current that the battery is accepting
+//! has fallen below the threshold.
+//!
+//! Note that this bit is regarded by the hardware guys as very slightly
+//! unreliable. They recommend that you don't believe a value of zero until
+//! you've sampled it twice.
+//!
+//! \retval  Zero if the battery is accepting less current than indicated by the
+//!          charging threshold. Non-zero otherwise.
+//!
+////////////////////////////////////////////////////////////////////////////////
+int ddi_power_GetChargeStatus(void)
+{
+	return (HW_POWER_STS_RD() & BM_POWER_STS_CHRGSTS) ? 1 : 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Battery Voltage
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the voltage across the battery.
+//!
+//! \fntype Function
+//!
+//! This function reports the voltage across the battery. Should return a
+//! value in range ~3000 - 4200 mV.
+//!
+//! \retval The voltage across the battery, in mV.
+//!
+////////////////////////////////////////////////////////////////////////////////
+
+//! \brief Constant value for 8mV steps used in battery translation
+#define BATT_VOLTAGE_8_MV 8
+
+uint16_t ddi_power_GetBattery(void)
+{
+	uint32_t    u16BattVolt;
+
+	// Get the raw result of battery measurement
+	u16BattVolt = HW_POWER_BATTMONITOR_RD();
+	u16BattVolt &= BM_POWER_BATTMONITOR_BATT_VAL;
+	u16BattVolt >>= BP_POWER_BATTMONITOR_BATT_VAL;
+
+	// Adjust for 8-mV LSB resolution and return
+	u16BattVolt *= BATT_VOLTAGE_8_MV;
+
+//#ifdef CONFIG_POWER_SUPPLY_DEBUG
+#if 0
+	printk("Battery charger: %u mV\n", u16BattVolt);
+#endif
+
+	return u16BattVolt;
+}
+
+#if 0
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report the voltage across the battery.
+//!
+//! \fntype Function
+//!
+//! This function reports the voltage across the battery.
+//!
+//! \retval The voltage across the battery, in mV.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_power_GetBatteryBrownout(void)
+{
+	uint32_t    u16BatteryBrownoutLevel;
+
+	// Get battery brownout level
+	u16BatteryBrownoutLevel = HW_POWER_BATTMONITOR_RD();
+	u16BatteryBrownoutLevel &= BM_POWER_BATTMONITOR_BRWNOUT_LVL;
+	u16BatteryBrownoutLevel >>= BP_POWER_BATTMONITOR_BRWNOUT_LVL;
+
+	// Calculate battery brownout level
+	switch (ddi_power_GetBatteryMode()) {
+        case DDI_POWER_BATT_MODE_LIION:
+		u16BatteryBrownoutLevel *= BATT_BRWNOUT_LIION_LEVEL_STEP_MV;
+		u16BatteryBrownoutLevel += BATT_BRWNOUT_LIION_BASE_MV;
+		break;
+        case DDI_POWER_BATT_MODE_ALKALINE_NIMH:
+		u16BatteryBrownoutLevel *= BATT_BRWNOUT_ALKAL_LEVEL_STEP_MV;
+		u16BatteryBrownoutLevel += BATT_BRWNOUT_ALKAL_BASE_MV;
+		break;
+        default:
+		u16BatteryBrownoutLevel = 0;
+		break;
+	}
+	return u16BatteryBrownoutLevel;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Set battery brownout level
+//!
+//! \fntype     Reentrant Function
+//!
+//! This function sets the battery brownout level in millivolt. It transforms the
+//! input brownout value from millivolts to the hardware register bit field value
+//! taking the ceiling value in the calculation.
+//!
+//! \param[in]  u16BattBrownout_mV      Battery battery brownout level in mV
+//!
+//! \return     SUCCESS
+//!
+////////////////////////////////////////////////////////////////////////////////
+int ddi_power_SetBatteryBrownout(uint16_t u16BattBrownout_mV)
+{
+	int16_t i16BrownoutLevel;
+	int ret = 0;
+
+	// Calculate battery brownout level
+	switch (ddi_power_GetBatteryMode()) {
+        case DDI_POWER_BATT_MODE_LIION:
+		i16BrownoutLevel  = u16BattBrownout_mV -
+			BATT_BRWNOUT_LIION_EQN_CONST;
+		i16BrownoutLevel /= BATT_BRWNOUT_LIION_LEVEL_STEP_MV;
+		break;
+        case DDI_POWER_BATT_MODE_ALKALINE_NIMH:
+		i16BrownoutLevel  = u16BattBrownout_mV -
+			BATT_BRWNOUT_ALKAL_EQN_CONST;
+		i16BrownoutLevel /= BATT_BRWNOUT_ALKAL_LEVEL_STEP_MV;
+		break;
+        default:
+		return -EINVAL;
+	}
+
+	// Do a check to make sure nothing went wrong.
+	if (i16BrownoutLevel <= 0x0f) {
+		//Write the battery brownout level
+		HW_POWER_BATTMONITOR_SET(
+			BF_POWER_BATTMONITOR_BRWNOUT_LVL(i16BrownoutLevel));
+	} else
+		ret = -EINVAL;
+
+	return ret;
+}
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// Currents
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+//! Name: ddi_power_SetBiasCurrentSource
+//!
+//! \brief
+////////////////////////////////////////////////////////////////////////////////
+int ddi_power_SetBiasCurrentSource(ddi_power_BiasCurrentSource_t eSource)
+{
+	switch (eSource) {
+	case DDI_POWER_INTERNAL_BIAS_CURRENT:
+		HW_POWER_CHARGE_SET(BM_POWER_CHARGE_USE_EXTERN_R);
+		break;
+	case DDI_POWER_EXTERNAL_BIAS_CURRENT:
+		HW_POWER_CHARGE_CLR(BM_POWER_CHARGE_USE_EXTERN_R);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//! Name: ddi_power_GetBiasCurrentSource
+//!
+//! \brief
+////////////////////////////////////////////////////////////////////////////////
+ddi_power_BiasCurrentSource_t ddi_power_GetBiasCurrentSource(void)
+{
+	return (HW_POWER_CHARGE_RD() & BM_POWER_CHARGE_USE_EXTERN_R) ?
+		DDI_POWER_INTERNAL_BIAS_CURRENT :
+		DDI_POWER_EXTERNAL_BIAS_CURRENT;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//! Name: ddi_power_SetMaxBatteryChargeCurrent
+//!
+//! \brief
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_power_SetMaxBatteryChargeCurrent(uint16_t u16MaxCur)
+{
+	uint32_t   u16OldSetting;
+	uint32_t   u16NewSetting;
+	uint32_t   u16ToggleMask;
+
+	// Get the old setting.
+	u16OldSetting = (HW_POWER_CHARGE_RD() & BM_POWER_CHARGE_BATTCHRG_I) >>
+		BP_POWER_CHARGE_BATTCHRG_I;
+
+	// Convert the new threshold into a setting.
+	u16NewSetting = ddi_power_convert_current_to_setting(u16MaxCur);
+
+	// Compute the toggle mask.
+	u16ToggleMask = u16OldSetting ^ u16NewSetting;
+
+	// Write to the toggle register.
+	HW_POWER_CHARGE_TOG(u16ToggleMask << BP_POWER_CHARGE_BATTCHRG_I);
+
+	// Tell the caller what current we're set at now.
+	return ddi_power_convert_setting_to_current(u16NewSetting);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//! Name: ddi_power_GetMaxBatteryChargeCurrent
+//!
+//! \brief
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_power_GetMaxBatteryChargeCurrent(void)
+{
+	uint32_t u8Bits;
+
+	// Get the raw data from register
+	u8Bits = (HW_POWER_CHARGE_RD() & BM_POWER_CHARGE_BATTCHRG_I) >>
+		BP_POWER_CHARGE_BATTCHRG_I;
+
+	// Translate raw data to current (in mA) and return it
+	return ddi_power_convert_setting_to_current(u8Bits);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//! Name: ddi_power_GetMaxChargeCurrent
+//!
+//! \brief
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_power_SetBatteryChargeCurrentThreshold(uint16_t u16Thresh)
+{
+	uint32_t   u16OldSetting;
+	uint32_t   u16NewSetting;
+	uint32_t   u16ToggleMask;
+
+	//-------------------------------------------------------------------
+	// See ddi_power_SetMaxBatteryChargeCurrent for an explanation of
+	// why we're using the toggle register here.
+	//
+	// Since this function doesn't have any major hardware effect,
+	// we could use the usual macros for writing to this bit field. But,
+	// for the sake of parallel construction and any potentially odd
+	// effects on the status bit, we use the toggle register in the same
+	// way as ddi_bc_hwSetMaxCurrent.
+	//-------------------------------------------------------------------
+
+	//-------------------------------------------------------------------
+	// The threshold hardware can't express as large a range as the max
+	// current setting, but we can use the same functions as long as we
+	// add an extra check here.
+	//
+	// Thresholds larger than 180mA can't be expressed.
+	//-------------------------------------------------------------------
+
+	if (u16Thresh > 180)
+		u16Thresh = 180;
+
+	////////////////////////////////////////
+	// Create the mask
+	////////////////////////////////////////
+
+	// Get the old setting.
+	u16OldSetting = (HW_POWER_CHARGE_RD() & BM_POWER_CHARGE_STOP_ILIMIT) >>
+		BP_POWER_CHARGE_STOP_ILIMIT;
+
+	// Convert the new threshold into a setting.
+	u16NewSetting = ddi_power_convert_current_to_setting(u16Thresh);
+
+	// Compute the toggle mask.
+	u16ToggleMask = u16OldSetting ^ u16NewSetting;
+
+	/////////////////////////////////////////
+	// Write to the register
+	/////////////////////////////////////////
+
+	// Write to the toggle register.
+	HW_POWER_CHARGE_TOG(BF_POWER_CHARGE_STOP_ILIMIT(u16ToggleMask));
+
+	// Tell the caller what current we're set at now.
+	return ddi_power_convert_setting_to_current(u16NewSetting);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//! Name: ddi_power_GetBatteryChargeCurrentThreshold
+//!
+//! \brief
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_power_GetBatteryChargeCurrentThreshold(void)
+{
+	uint32_t u16Threshold;
+
+	u16Threshold = (HW_POWER_CHARGE_RD() & BM_POWER_CHARGE_STOP_ILIMIT) >>
+		BP_POWER_CHARGE_STOP_ILIMIT;
+
+	return ddi_power_convert_setting_to_current(u16Threshold);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Conversion
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Compute the actual current expressible in the hardware.
+//!
+//! \fntype Function
+//!
+//! Given a desired current, this function computes the actual current
+//! expressible in the hardware.
+//!
+//! Note that the hardware has a minimum resolution of 10mA and a maximum
+//! expressible value of 780mA (see the data sheet for details). If the given
+//! current cannot be expressed exactly, then the largest expressible smaller
+//! value will be used.
+//!
+//! \param[in]  u16Current  The current of interest.
+//!
+//! \retval  The corresponding current in mA.
+//!
+////////////////////////////////////////////////////////////////////////////////
+uint16_t ddi_power_ExpressibleCurrent(uint16_t u16Current)
+{
+	return ddi_power_convert_setting_to_current(
+		ddi_power_convert_current_to_setting(u16Current));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//! Name: ddi_power_Get5VPresent
+//!
+//! \brief
+////////////////////////////////////////////////////////////////////////////////
+bool ddi_power_Get5vPresentFlag(void)
+{
+	switch (DetectionMethod) {
+	case DDI_POWER_5V_VBUSVALID:
+		// Check VBUSVALID for 5V present
+		return ((HW_POWER_STS_RD() & BM_POWER_STS_VBUSVALID) != 0);
+	case DDI_POWER_5V_VDD5V_GT_VDDIO:
+		// Check VDD5V_GT_VDDIO for 5V present
+		return ((HW_POWER_STS_RD() & BM_POWER_STS_VDD5V_GT_VDDIO) != 0);
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Report on the die temperature.
+//!
+//! \fntype Function
+//!
+//! This function reports on the die temperature.
+//!
+//! \param[out]  pLow   The low  end of the temperature range.
+//! \param[out]  pHigh  The high end of the temperature range.
+//!
+////////////////////////////////////////////////////////////////////////////////
+// Temperature constant
+#define TEMP_READING_ERROR_MARGIN 5
+#define KELVIN_TO_CELSIUS_CONST 273
+
+void ddi_power_GetDieTemp(int16_t * pLow, int16_t * pHigh)
+{
+	int16_t i16High, i16Low;
+	uint16_t u16Reading;
+
+	// Get the reading in Kelvins
+	u16Reading = MeasureInternalDieTemperature();
+
+	// Adjust for error margin
+	i16High = u16Reading + TEMP_READING_ERROR_MARGIN;
+	i16Low  = u16Reading - TEMP_READING_ERROR_MARGIN;
+
+	// Convert to Celsius
+	i16High -= KELVIN_TO_CELSIUS_CONST;
+	i16Low  -= KELVIN_TO_CELSIUS_CONST;
+
+//#ifdef CONFIG_POWER_SUPPLY_DEBUG
+#if 0
+	printk("Battery charger: Die temp %d to %d C\n", i16Low, i16High);
+#endif
+	// Return the results
+	*pHigh = i16High;
+	*pLow  = i16Low;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+//!
+//! \brief Checks to see if the DCDC has been manually enabled
+//!
+//! \fntype Function
+//!
+//! \retval  true if DCDC is ON, false if DCDC is OFF.
+//!
+////////////////////////////////////////////////////////////////////////////////
+bool ddi_power_IsDcdcOn(void)
+{
+	return (HW_POWER_5VCTRL_RD() & BM_POWER_5VCTRL_ENABLE_DCDC) ? 1 : 0;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+//! See hw_power.h for details.
+////////////////////////////////////////////////////////////////////////////////
+void ddi_power_SetPowerClkGate(bool bGate)
+{
+	// Gate/Ungate the clock to the power block
+	if (bGate) {
+		HW_POWER_CTRL_SET(BM_POWER_CTRL_CLKGATE);
+	} else {
+		HW_POWER_CTRL_CLR(BM_POWER_CTRL_CLKGATE);
+	}
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//! See hw_power.h for details.
+////////////////////////////////////////////////////////////////////////////////
+bool ddi_power_GetPowerClkGate(void)
+{
+	return (HW_POWER_CTRL_RD() & BM_POWER_CTRL_CLKGATE) ? 1 : 0;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// End of file
+////////////////////////////////////////////////////////////////////////////////
+//! @}
diff --git a/drivers/power/stmp37xx/ddi_power_battery.h b/drivers/power/stmp37xx/ddi_power_battery.h
new file mode 100644
index 000000000000..2d7c1a5411ad
--- /dev/null
+++ b/drivers/power/stmp37xx/ddi_power_battery.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+//! \brief Battery modes
+typedef enum {
+	// 37xx battery modes
+        //! \brief LiIon battery powers the player
+	DDI_POWER_BATT_MODE_LIION           = 0,
+        //! \brief Alkaline/NiMH battery powers the player
+	DDI_POWER_BATT_MODE_ALKALINE_NIMH   = 1,
+} ddi_power_BatteryMode_t;
+
+
+//! \brief Available sources for bias currents
+typedef enum {
+        //! \brief Use external resistor to generate bias current
+	DDI_POWER_EXTERNAL_BIAS_CURRENT = 0x0,
+        //! \brief Use internal resistor to generate bias current
+	DDI_POWER_INTERNAL_BIAS_CURRENT = 0x1
+} ddi_power_BiasCurrentSource_t;
+
+//! \brief Possible 5V detection methods
+typedef enum {
+	//! \brief Use VBUSVALID comparator for detection
+	DDI_POWER_5V_VBUSVALID,
+	//! \brief Use VDD5V_GT_VDDIO comparison for detection
+	DDI_POWER_5V_VDD5V_GT_VDDIO
+} ddi_power_5vDetection_t;
+
+
+uint16_t ddi_power_convert_current_to_setting(uint16_t u16Current);
+uint16_t ddi_power_convert_setting_to_current(uint16_t u16Setting);
+void ddi_power_enable_5v_to_battery_handoff(void);
+void ddi_power_execute_5v_to_battery_handoff(void);
+void ddi_power_enable_battery_to_5v_handoff(void);
+void ddi_power_execute_battery_to_5v_handoff(void);
+int ddi_power_init_battery(void);
+ddi_power_BatteryMode_t ddi_power_GetBatteryMode(void);
+bool ddi_power_GetBatteryChargerEnabled(void);
+bool ddi_power_GetChargerPowered(void);
+void ddi_power_SetChargerPowered(bool bPowerOn);
+int ddi_power_GetChargeStatus(void);
+uint16_t ddi_power_GetBattery(void);
+uint16_t ddi_power_GetBatteryBrownout(void);
+int ddi_power_SetBatteryBrownout(uint16_t u16BattBrownout_mV);
+int ddi_power_SetBiasCurrentSource(ddi_power_BiasCurrentSource_t eSource);
+ddi_power_BiasCurrentSource_t ddi_power_GetBiasCurrentSource(void);
+uint16_t ddi_power_SetMaxBatteryChargeCurrent(uint16_t u16MaxCur);
+uint16_t ddi_power_GetMaxBatteryChargeCurrent(void);
+uint16_t ddi_power_SetBatteryChargeCurrentThreshold(uint16_t u16Thresh);
+uint16_t ddi_power_GetBatteryChargeCurrentThreshold(void);
+uint16_t ddi_power_ExpressibleCurrent(uint16_t u16Current);
+bool ddi_power_Get5vPresentFlag(void);
+void ddi_power_GetDieTemp(int16_t * pLow, int16_t * pHigh);
+bool ddi_power_IsDcdcOn(void);
+void ddi_power_SetPowerClkGate(bool bGate);
+bool ddi_power_GetPowerClkGate(void);
diff --git a/drivers/power/stmp37xx/linux.c b/drivers/power/stmp37xx/linux.c
new file mode 100644
index 000000000000..664896004dc7
--- /dev/null
+++ b/drivers/power/stmp37xx/linux.c
@@ -0,0 +1,622 @@
+/*
+ * Linux glue to STMP3xxx battery state machine.
+ *
+ * Author: Steve Longerbeam <stevel@embeddedalley.com>
+ *
+ * Copyright (C) 2008 EmbeddedAlley Solutions Inc.
+ * Copyright 2008-2009 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/jiffies.h>
+#include <linux/sched.h>
+#include <mach/ddi_bc.h>
+#include "ddi_bc_internal.h"
+#include <linux/regulator/consumer.h>
+#include <linux/regulator/driver.h>
+#include <mach/regulator.h>
+#include <mach/regs-power.h>
+#include <mach/regs-usbphy.h>
+#include <linux/delay.h>
+
+#include <linux/interrupt.h>
+
+
+struct stmp3xxx_info {
+	struct device *dev;
+	struct regulator *regulator;
+
+	struct power_supply bat;
+	struct power_supply ac;
+	struct power_supply usb;
+
+	ddi_bc_Cfg_t *sm_cfg;
+	struct mutex sm_lock;
+	struct timer_list sm_timer;
+	struct work_struct sm_work;
+	struct resource *vdd5v_irq;
+	int is_ac_online;
+#define USB_ONLINE      0x01
+#define USB_REG_SET     0x02
+#define USB_SM_RESTART  0x04
+#define USB_SHUTDOWN    0x08
+#define USB_N_SEND      0x10
+	int is_usb_online;
+};
+
+#define to_stmp3xxx_info(x) container_of((x), struct stmp3xxx_info, bat)
+
+/* There is no direct way to detect wall power presence, so assume the AC
+ * power source is valid if 5V presents and USB device is disconnected.
+ * If USB device is connected then assume that AC is offline and USB power
+ * is online.
+ */
+#define is_usb_plugged()(HW_USBPHY_STATUS_RD() & \
+		BM_USBPHY_STATUS_DEVPLUGIN_STATUS)
+#define is_ac_online()	\
+		(ddi_power_Get5vPresentFlag() ? (!is_usb_plugged()) : 0)
+#define is_usb_online()	\
+		(ddi_power_Get5vPresentFlag() ? (!!is_usb_plugged()) : 0)
+
+/*
+ * Power properties
+ */
+static enum power_supply_property stmp3xxx_power_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+};
+
+static int stmp3xxx_power_get_property(struct power_supply *psy,
+				     enum power_supply_property psp,
+				     union power_supply_propval *val)
+{
+	switch (psp) {
+	case POWER_SUPPLY_PROP_ONLINE:
+		if (psy->type == POWER_SUPPLY_TYPE_MAINS)
+			/* ac online */
+			val->intval = is_ac_online();
+		else
+			/* usb online */
+			val->intval = is_usb_online();
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+/*
+ * Battery properties
+ */
+static enum power_supply_property stmp3xxx_bat_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_TEMP,
+};
+
+static int stmp3xxx_bat_get_property(struct power_supply *psy,
+				     enum power_supply_property psp,
+				     union power_supply_propval *val)
+{
+	struct stmp3xxx_info *info = to_stmp3xxx_info(psy);
+	ddi_bc_State_t state;
+	ddi_bc_BrokenReason_t reason;
+	int temp_alarm;
+	int16_t temp_lo, temp_hi;
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		state = ddi_bc_GetState();
+		switch (state) {
+		case DDI_BC_STATE_CONDITIONING:
+		case DDI_BC_STATE_CHARGING:
+		case DDI_BC_STATE_TOPPING_OFF:
+			val->intval = POWER_SUPPLY_STATUS_CHARGING;
+			break;
+		case DDI_BC_STATE_DISABLED:
+			val->intval = ddi_power_Get5vPresentFlag() ?
+				POWER_SUPPLY_STATUS_NOT_CHARGING :
+			POWER_SUPPLY_STATUS_DISCHARGING;
+			break;
+		default:
+			/* TODO: detect full */
+			val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
+			break;
+		}
+		break;
+	case POWER_SUPPLY_PROP_PRESENT:
+		/* is battery present */
+		state = ddi_bc_GetState();
+		switch (state) {
+		case DDI_BC_STATE_WAITING_TO_CHARGE:
+		case DDI_BC_STATE_DCDC_MODE_WAITING_TO_CHARGE:
+		case DDI_BC_STATE_CONDITIONING:
+		case DDI_BC_STATE_CHARGING:
+		case DDI_BC_STATE_TOPPING_OFF:
+		case DDI_BC_STATE_DISABLED:
+			val->intval = 1;
+			break;
+		case DDI_BC_STATE_BROKEN:
+			val->intval = !(ddi_bc_GetBrokenReason() ==
+					DDI_BC_BROKEN_NO_BATTERY_DETECTED);
+			break;
+		default:
+			val->intval = 0;
+			break;
+		}
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		temp_alarm = ddi_bc_RampGetDieTempAlarm();
+		if (temp_alarm) {
+			val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		} else {
+			state = ddi_bc_GetState();
+			switch (state) {
+			case DDI_BC_STATE_BROKEN:
+				reason = ddi_bc_GetBrokenReason();
+				val->intval =
+				   (reason == DDI_BC_BROKEN_CHARGING_TIMEOUT) ?
+					POWER_SUPPLY_HEALTH_DEAD :
+					POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+				break;
+			case DDI_BC_STATE_UNINITIALIZED:
+				val->intval = POWER_SUPPLY_HEALTH_UNKNOWN;
+				break;
+			default:
+				val->intval = POWER_SUPPLY_HEALTH_GOOD;
+				break;
+			}
+		}
+		break;
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		val->intval = POWER_SUPPLY_TECHNOLOGY_LION;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		/* uV */
+		val->intval = ddi_power_GetBattery() * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		/* uA */
+		val->intval = ddi_power_GetMaxBatteryChargeCurrent() * 1000;
+		break;
+	case POWER_SUPPLY_PROP_TEMP:
+		mutex_lock(&info->sm_lock);
+		ddi_power_GetDieTemp(&temp_lo, &temp_hi);
+		mutex_unlock(&info->sm_lock);
+		val->intval = temp_lo + (temp_hi - temp_lo) / 2;
+
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void state_machine_timer(unsigned long data)
+{
+	struct stmp3xxx_info *info = (struct stmp3xxx_info *)data;
+	ddi_bc_Cfg_t *cfg = info->sm_cfg;
+	int ret;
+
+	/* schedule next call to state machine */
+	mod_timer(&info->sm_timer,
+		  jiffies + msecs_to_jiffies(cfg->u32StateMachinePeriod));
+
+	ret = schedule_work(&info->sm_work);
+	if (!ret)
+		dev_dbg(info->dev, "state machine failed to schedule\n");
+
+}
+/*
+ * Assumtion:
+ * AC power can't be switched to USB w/o system reboot
+ * and vice-versa
+ */
+static void state_machine_work(struct work_struct *work)
+{
+	struct stmp3xxx_info *info =
+		container_of(work, struct stmp3xxx_info, sm_work);
+
+	mutex_lock(&info->sm_lock);
+
+	if (info->is_usb_online & USB_SHUTDOWN) {
+		info->is_usb_online = 0;
+		if (!info->regulator)
+			goto out;
+		regulator_set_current_limit(info->regulator, 0, 0);
+		goto out;
+	}
+
+	if (is_ac_online()) {
+		if (info->is_ac_online)
+			goto done;
+
+		/* ac supply connected */
+		dev_info(info->dev, "changed power connection to ac/5v \n");
+
+		info->is_ac_online = 1;
+		info->is_usb_online = 0;
+		ddi_bc_SetCurrentLimit(600 /*mA*/);
+		ddi_power_execute_battery_to_5v_handoff();
+		ddi_power_enable_5v_to_battery_handoff();
+		goto done;
+	}
+
+	if (!is_usb_online())
+		goto out;
+
+	if (info->is_usb_online & USB_REG_SET)
+		goto done;
+
+	info->is_ac_online = 0;
+	info->is_usb_online |= USB_ONLINE;
+
+	if (!info->regulator) {
+		info->regulator = regulator_get(NULL, "charger-1");
+		if (!info->regulator || IS_ERR(info->regulator)) {
+			dev_err(info->dev,
+				"%s: failed to get regulator\n", __func__);
+			info->regulator = NULL;
+			ddi_bc_SetCurrentLimit(350 /*mA*/);
+			ddi_power_execute_battery_to_5v_handoff();
+			ddi_power_enable_5v_to_battery_handoff();
+			goto done;
+		} else
+			regulator_set_mode(info->regulator,
+					   REGULATOR_MODE_FAST);
+	}
+
+	if (!(info->is_usb_online & USB_N_SEND)) {
+		info->is_usb_online |= USB_N_SEND;
+	}
+
+	if (regulator_set_current_limit(info->regulator, 150000, 150000)) {
+		dev_err(info->dev, "reg_set_current(150000) failed\n");
+
+		ddi_bc_SetCurrentLimit(0 /*mA*/);
+		dev_dbg(info->dev, "charge current set to 0\n");
+		mod_timer(&info->sm_timer, jiffies + msecs_to_jiffies(1000));
+		goto done;
+	}
+
+	dev_dbg(info->dev, "%s: charge current set to 100mA\n", __func__);
+	ddi_bc_SetCurrentLimit(100 /*mA*/);
+	regulator_set_current_limit(info->regulator, 100000, 100000);
+	if (info->is_usb_online & USB_SM_RESTART) {
+		info->is_usb_online &= ~USB_SM_RESTART;
+		ddi_bc_SetEnable();
+	}
+
+	info->is_usb_online |= USB_REG_SET;
+
+	dev_info(info->dev, "changed power connection to usb/5v present\n");
+	ddi_power_execute_battery_to_5v_handoff();
+	ddi_power_enable_5v_to_battery_handoff();
+	ddi_bc_SetEnable();
+
+done:
+	ddi_bc_StateMachine();
+out:
+	mutex_unlock(&info->sm_lock);
+}
+
+static int bc_sm_restart(struct stmp3xxx_info *info)
+{
+	ddi_bc_Status_t bcret;
+	int ret = 0;
+
+	mutex_lock(&info->sm_lock);
+
+	/* ungate power clk */
+	ddi_power_SetPowerClkGate(0);
+
+	/*
+	 * config battery charger state machine and move it to the Disabled
+	 * state. This must be done before starting the state machine.
+	 */
+	bcret = ddi_bc_Init(info->sm_cfg);
+	if (bcret != DDI_BC_STATUS_SUCCESS) {
+		dev_err(info->dev, "state machine init failed: %d\n", bcret);
+		ret = -EIO;
+		goto out;
+	}
+
+	/*
+	 * Check what power supply options we have right now. If
+	 * we're able to do any battery charging, then set the
+	 * appropriate current limit and enable. Otherwise, leave
+	 * the battery charger disabled.
+	 */
+	if (is_ac_online()) {
+		/* ac supply connected */
+		dev_info(info->dev, "ac/5v present, enabling state machine\n");
+
+		info->is_ac_online = 1;
+		info->is_usb_online = 0;
+		ddi_bc_SetCurrentLimit(600 /*mA*/);
+		ddi_bc_SetEnable();
+	} else if (is_usb_online()) {
+		/* usb supply connected */
+		dev_info(info->dev, "usb/5v present, enabling state machine\n");
+
+		info->is_ac_online = 0;
+		info->is_usb_online = USB_ONLINE | USB_SM_RESTART;
+	} else {
+		/* not powered */
+		dev_info(info->dev, "%s: 5v not present\n", __func__);
+
+		info->is_ac_online = 0;
+		info->is_usb_online = 0;
+		ddi_bc_SetDisable();
+	}
+
+	/* schedule first call to state machine */
+	mod_timer(&info->sm_timer, jiffies + 1);
+out:
+	mutex_unlock(&info->sm_lock);
+	return ret;
+}
+
+static irqreturn_t stmp3xxx_vdd5v_irq(int irq, void *cookie)
+{
+	struct stmp3xxx_info *info = (struct stmp3xxx_info *)cookie;
+
+	if (ddi_power_Get5vPresentFlag()) {
+		dev_info(info->dev, "5v present, reenable state machine\n");
+
+		ddi_bc_SetEnable();
+
+		/*
+		 * We only ack/negate the interrupt here,
+		 * as we can't decide yet if we really can
+		 * switch to 5V (USB bits not ready)
+		 */
+		ddi_power_enable_5v_to_battery_handoff();
+	} else {
+		dev_info(info->dev, "5v went away, disabling state machine\n");
+
+		ddi_bc_SetDisable();
+
+		info->is_ac_online = 0;
+		if (info->is_usb_online)
+			info->is_usb_online = USB_SHUTDOWN;
+
+		ddi_power_execute_5v_to_battery_handoff();
+		ddi_power_enable_battery_to_5v_handoff();
+		mod_timer(&info->sm_timer, jiffies + 1);
+
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int stmp3xxx_bat_probe(struct platform_device *pdev)
+{
+	struct stmp3xxx_info *info;
+	int ret = 0;
+
+	if (!pdev->dev.platform_data) {
+		printk(KERN_ERR "%s: missing platform data\n", __func__);
+		return -ENODEV;
+	}
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->vdd5v_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (info->vdd5v_irq == NULL) {
+		printk(KERN_ERR "%s: failed to get irq resouce\n", __func__);
+		goto free_info;
+	}
+
+	platform_set_drvdata(pdev, info);
+
+	info->dev    = &pdev->dev;
+	info->sm_cfg = pdev->dev.platform_data;
+
+	/* initialize bat power_supply struct */
+	info->bat.name           = "battery";
+	info->bat.type           = POWER_SUPPLY_TYPE_BATTERY;
+	info->bat.properties     = stmp3xxx_bat_props;
+	info->bat.num_properties = ARRAY_SIZE(stmp3xxx_bat_props);
+	info->bat.get_property   = stmp3xxx_bat_get_property;
+
+	/* initialize ac power_supply struct */
+	info->ac.name           = "ac";
+	info->ac.type           = POWER_SUPPLY_TYPE_MAINS;
+	info->ac.properties     = stmp3xxx_power_props;
+	info->ac.num_properties = ARRAY_SIZE(stmp3xxx_power_props);
+	info->ac.get_property   = stmp3xxx_power_get_property;
+
+	/* initialize usb power_supply struct */
+	info->usb.name           = "usb";
+	info->usb.type           = POWER_SUPPLY_TYPE_USB;
+	info->usb.properties     = stmp3xxx_power_props;
+	info->usb.num_properties = ARRAY_SIZE(stmp3xxx_power_props);
+	info->usb.get_property   = stmp3xxx_power_get_property;
+
+	init_timer(&info->sm_timer);
+	info->sm_timer.data = (unsigned long)info;
+	info->sm_timer.function = state_machine_timer;
+
+	mutex_init(&info->sm_lock);
+	INIT_WORK(&info->sm_work, state_machine_work);
+
+	/* init LRADC channels to measure battery voltage and die temp */
+	ddi_power_init_battery();
+	HW_POWER_5VCTRL_CLR(BM_POWER_5VCTRL_ENABLE_LINREG_ILIMIT);
+
+	ret = bc_sm_restart(info);
+	if (ret)
+		goto free_info;
+
+	ret = request_irq(info->vdd5v_irq->start,
+			stmp3xxx_vdd5v_irq, IRQF_DISABLED | IRQF_SHARED,
+			pdev->name, info);
+	if (ret) {
+		dev_err(info->dev, "failed to request irq\n");
+		goto stop_sm;
+	}
+
+	ret = power_supply_register(&pdev->dev, &info->bat);
+	if (ret) {
+		dev_err(info->dev, "failed to register battery\n");
+		goto free_irq;
+	}
+
+	ret = power_supply_register(&pdev->dev, &info->ac);
+	if (ret) {
+		dev_err(info->dev, "failed to register ac power supply\n");
+		goto unregister_bat;
+	}
+
+	ret = power_supply_register(&pdev->dev, &info->usb);
+	if (ret) {
+		dev_err(info->dev, "failed to register usb power supply\n");
+		goto unregister_ac;
+	}
+
+	/* enable usb device presence detection */
+	HW_USBPHY_CTRL_SET(BM_USBPHY_CTRL_ENDEVPLUGINDETECT);
+
+	return 0;
+
+unregister_ac:
+	power_supply_unregister(&info->ac);
+unregister_bat:
+	power_supply_unregister(&info->bat);
+free_irq:
+	free_irq(info->vdd5v_irq->start, pdev);
+stop_sm:
+	ddi_bc_ShutDown();
+free_info:
+	kfree(info);
+	return ret;
+}
+
+static int stmp3xxx_bat_remove(struct platform_device *pdev)
+{
+	struct stmp3xxx_info *info = platform_get_drvdata(pdev);
+
+	if (info->regulator)
+		regulator_put(info->regulator);
+	free_irq(info->vdd5v_irq->start, pdev);
+	ddi_bc_ShutDown();
+	power_supply_unregister(&info->usb);
+	power_supply_unregister(&info->ac);
+	power_supply_unregister(&info->bat);
+	return 0;
+}
+
+static void stmp3xxx_bat_shutdown(struct platform_device *pdev)
+{
+	ddi_bc_ShutDown();
+}
+
+
+#ifdef CONFIG_PM
+
+static int stmp3xxx_bat_suspend(struct platform_device *pdev, pm_message_t msg)
+{
+	struct stmp3xxx_info *info = platform_get_drvdata(pdev);
+
+	mutex_lock(&info->sm_lock);
+
+	/* disable 5v irq */
+	HW_POWER_CTRL_CLR(BM_POWER_CTRL_ENIRQ_VDD5V_GT_VDDIO);
+
+	ddi_bc_SetDisable();
+	/* cancel state machine timer */
+	del_timer_sync(&info->sm_timer);
+
+	mutex_unlock(&info->sm_lock);
+	return 0;
+}
+
+static int stmp3xxx_bat_resume(struct platform_device *pdev)
+{
+	struct stmp3xxx_info *info = platform_get_drvdata(pdev);
+	ddi_bc_Cfg_t *cfg = info->sm_cfg;
+
+	mutex_lock(&info->sm_lock);
+
+	if (is_ac_online()) {
+		/* ac supply connected */
+		dev_info(info->dev, "ac/5v present, enabling state machine\n");
+
+		info->is_ac_online = 1;
+		info->is_usb_online = 0;
+		ddi_bc_SetCurrentLimit(600 /*mA*/);
+		ddi_bc_SetEnable();
+	} else if (is_usb_online()) {
+		/* usb supply connected */
+		dev_info(info->dev, "usb/5v present, enabling state machine\n");
+
+		info->is_ac_online = 0;
+		info->is_usb_online = 1;
+		ddi_bc_SetCurrentLimit(350 /*mA*/);
+		ddi_bc_SetEnable();
+	} else {
+		/* not powered */
+		dev_info(info->dev, "%s: 5v not present\n", __func__);
+
+		info->is_ac_online = 0;
+		info->is_usb_online = 0;
+	}
+
+	/* enable 5v irq */
+	HW_POWER_CTRL_SET(BM_POWER_CTRL_ENIRQ_VDD5V_GT_VDDIO);
+
+	/* reschedule calls to state machine */
+	mod_timer(&info->sm_timer,
+		  jiffies + msecs_to_jiffies(cfg->u32StateMachinePeriod));
+
+	mutex_unlock(&info->sm_lock);
+	return 0;
+}
+
+#else
+#define stmp3xxx_bat_suspend NULL
+#define stmp3xxx_bat_resume  NULL
+#endif
+
+static struct platform_driver stmp3xxx_batdrv = {
+	.probe		= stmp3xxx_bat_probe,
+	.remove		= stmp3xxx_bat_remove,
+	.shutdown       = stmp3xxx_bat_shutdown,
+	.suspend	= stmp3xxx_bat_suspend,
+	.resume		= stmp3xxx_bat_resume,
+	.driver		= {
+		.name	= "stmp3xxx-battery",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init stmp3xxx_bat_init(void)
+{
+	return platform_driver_register(&stmp3xxx_batdrv);
+}
+
+static void __exit stmp3xxx_bat_exit(void)
+{
+	platform_driver_unregister(&stmp3xxx_batdrv);
+}
+
+module_init(stmp3xxx_bat_init);
+module_exit(stmp3xxx_bat_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Steve Longerbeam <stevel@embeddedalley.com>");
+MODULE_DESCRIPTION("Linux glue to STMP3xxx battery state machine");
diff --git a/drivers/pwm/Kconfig b/drivers/pwm/Kconfig
new file mode 100644
index 000000000000..5aeb7b7453b1
--- /dev/null
+++ b/drivers/pwm/Kconfig
@@ -0,0 +1,35 @@
+#
+# PWM infrastructure and devices
+#
+
+menuconfig GENERIC_PWM
+	tristate "PWM Support"
+	help
+	  This enables PWM support through the generic PWM library.
+	  If unsure, say N.
+
+if GENERIC_PWM
+
+config ATMEL_PWM
+	tristate "Atmel AT32/AT91 PWM support"
+	depends on AVR32 || ARCH_AT91
+	help
+	  This option enables device driver support for the PWMC
+	  peripheral channels found on certain Atmel processors.
+	  Pulse Width Modulation is used many for purposes, including
+	  software controlled power-efficent backlights on LCD
+	  displays, motor control, and waveform generation.  If
+	  unsure, say N.
+
+config NS921X_PWM
+	tristate "Digi NS921X PWM support"
+	depends on PROCESSOR_NS9215 && !CC9P9215JS_SERIAL_PORTA && !CC9P9215JS_SPI && !CC9P9215JS_SERIAL_PORTC_CTSRTSRXTX && !CC9P9215JS_SERIAL_PORTC_FULL
+	help
+	  This option enables device driver support for the PWMC
+	  peripheral channels found on certain NetSilicon processors.
+	  Pulse Width Modulation is used many for purposes, including
+	  software controlled power-efficent backlights on LCD
+	  displays, motor control, and waveform generation.  If
+	  unsure, say N.
+
+endif
diff --git a/drivers/pwm/Makefile b/drivers/pwm/Makefile
new file mode 100644
index 000000000000..f04d64f39f9f
--- /dev/null
+++ b/drivers/pwm/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for pwm devices
+#
+obj-$(CONFIG_GENERIC_PWM) 	:= pwm.o
+
+obj-$(CONFIG_ATMEL_PWM)		+= atmel-pwm.o
+obj-$(CONFIG_NS921X_PWM)	+= ns9xxx-pwm.o
diff --git a/drivers/pwm/atmel-pwm.c b/drivers/pwm/atmel-pwm.c
new file mode 100644
index 000000000000..e522b16e3cd6
--- /dev/null
+++ b/drivers/pwm/atmel-pwm.c
@@ -0,0 +1,633 @@
+/*
+ * drivers/pwm/atmel-pwm.c
+ *
+ * Copyright (C) 2008 Bill Gatliff
+ * Copyright (C) 2007 David Brownell
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+
+enum {
+	/* registers common to the PWMC peripheral */
+	PWMC_MR = 0,
+	PWMC_ENA = 4,
+	PWMC_DIS = 8,
+	PWMC_SR = 0xc,
+	PWMC_IER = 0x10,
+	PWMC_IDR = 0x14,
+	PWMC_IMR = 0x18,
+	PWMC_ISR = 0x1c,
+
+	/* registers per each PWMC channel */
+	PWMC_CMR = 0,
+	PWMC_CDTY = 4,
+	PWMC_CPRD = 8,
+	PWMC_CCNT = 0xc,
+	PWMC_CUPD = 0x10,
+
+	/* how to find each channel */
+	PWMC_CHAN_BASE = 0x200,
+	PWMC_CHAN_STRIDE = 0x20,
+
+	/* CMR bits of interest */
+	PWMC_CMR_CPD = 10,
+	PWMC_CMR_CPOL = 9,
+	PWMC_CMR_CALG = 8,
+	PWMC_CMR_CPRE_MASK = 0xf,
+};
+
+struct atmel_pwm {
+	struct pwm_device pwm;
+	spinlock_t lock;
+	void __iomem *iobase;
+	struct clk *clk;
+	u32 *sync_mask;
+	int irq;
+	u32 ccnt_mask;
+};
+
+
+static inline void
+pwmc_writel(const struct atmel_pwm *p,
+	    unsigned offset, u32 val)
+{
+	__raw_writel(val, p->iobase + offset);
+}
+
+
+static inline u32
+pwmc_readl(const struct atmel_pwm *p,
+	   unsigned offset)
+{
+	return __raw_readl(p->iobase + offset);
+}
+
+
+static inline void
+pwmc_chan_writel(const struct pwm_channel *p,
+		 u32 offset, u32 val)
+{
+	const struct atmel_pwm *ap
+		= container_of(p->pwm, struct atmel_pwm, pwm);
+
+	if (PWMC_CMR == offset)
+		val &= ((1 << PWMC_CMR_CPD)
+			| (1 << PWMC_CMR_CPOL)
+			| (1 << PWMC_CMR_CALG)
+			| (PWMC_CMR_CPRE_MASK));
+	else
+		val &= ap->ccnt_mask;
+
+	pwmc_writel(ap, offset + PWMC_CHAN_BASE
+		    + (p->chan * PWMC_CHAN_STRIDE), val);
+}
+
+
+static inline u32
+pwmc_chan_readl(const struct pwm_channel *p,
+		u32 offset)
+{
+	const struct atmel_pwm *ap
+		= container_of(p->pwm, struct atmel_pwm, pwm);
+
+	return pwmc_readl(ap, offset + PWMC_CHAN_BASE
+			  + (p->chan * PWMC_CHAN_STRIDE));
+}
+
+
+static inline int
+__atmel_pwm_is_on(struct pwm_channel *p)
+{
+	struct atmel_pwm *ap = container_of(p->pwm, struct atmel_pwm, pwm);
+	return (pwmc_readl(ap, PWMC_SR) & (1 << p->chan)) ? 1 : 0;
+}
+
+
+static inline void
+__atmel_pwm_unsynchronize(struct pwm_channel *p,
+			  struct pwm_channel *to_p)
+{
+	const struct atmel_pwm *ap
+		= container_of(p->pwm, struct atmel_pwm, pwm);
+	int wchan;
+
+	if (to_p) {
+		ap->sync_mask[p->chan] &= ~(1 << to_p->chan);
+		ap->sync_mask[to_p->chan] &= ~(1 << p->chan);
+		goto done;
+	}
+
+	ap->sync_mask[p->chan] = 0;
+	for (wchan = 0; wchan < ap->pwm.nchan; wchan++)
+		ap->sync_mask[wchan] &= ~(1 << p->chan);
+done:
+	pr_debug("%s:%d sync_mask %x\n",
+		 p->pwm->bus_id, p->chan, ap->sync_mask[p->chan]);
+}
+
+
+static inline void
+__atmel_pwm_synchronize(struct pwm_channel *p,
+			struct pwm_channel *to_p)
+{
+	const struct atmel_pwm *ap
+		= container_of(p->pwm, struct atmel_pwm, pwm);
+
+	if (!to_p)
+		return;
+
+	ap->sync_mask[p->chan] |= (1 << to_p->chan);
+	ap->sync_mask[to_p->chan] |= (1 << p->chan);
+
+	pr_debug("%s:%d sync_mask %x\n",
+		 p->pwm->bus_id, p->chan, ap->sync_mask[p->chan]);
+}
+
+
+static inline void
+__atmel_pwm_stop(struct pwm_channel *p)
+{
+	struct atmel_pwm *ap = container_of(p->pwm, struct atmel_pwm, pwm);
+	u32 chid = 1 << p->chan;
+
+	pwmc_writel(ap, PWMC_DIS, ap->sync_mask[p->chan] | chid);
+}
+
+
+static inline void
+__atmel_pwm_start(struct pwm_channel *p)
+{
+	struct atmel_pwm *ap = container_of(p->pwm, struct atmel_pwm, pwm);
+	u32 chid = 1 << p->chan;
+
+	pwmc_writel(ap, PWMC_ENA, ap->sync_mask[p->chan] | chid);
+}
+
+
+static int
+atmel_pwm_synchronize(struct pwm_channel *p,
+		      struct pwm_channel *to_p)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&p->lock, flags);
+	__atmel_pwm_synchronize(p, to_p);
+	spin_unlock_irqrestore(&p->lock, flags);
+	return 0;
+}
+
+
+static int
+atmel_pwm_unsynchronize(struct pwm_channel *p,
+			struct pwm_channel *from_p)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&p->lock, flags);
+	__atmel_pwm_unsynchronize(p, from_p);
+	spin_unlock_irqrestore(&p->lock, flags);
+	return 0;
+}
+
+
+static inline int
+__atmel_pwm_config_polarity(struct pwm_channel *p,
+			    struct pwm_channel_config *c)
+{
+	u32 cmr = pwmc_chan_readl(p, PWMC_CMR);
+
+	if (c->polarity)
+		cmr &= ~BIT(PWMC_CMR_CPOL);
+	else
+		cmr |= BIT(PWMC_CMR_CPOL);
+	pwmc_chan_writel(p, PWMC_CMR, cmr);
+
+	pr_debug("%s:%d polarity %d\n", p->pwm->bus_id,
+		 p->chan, c->polarity);
+	return 0;
+}
+
+
+static inline int
+__atmel_pwm_config_duty_ticks(struct pwm_channel *p,
+			      struct pwm_channel_config *c)
+{
+	u32 cmr, cprd, cpre, cdty;
+
+	cmr = pwmc_chan_readl(p, PWMC_CMR);
+	cprd = pwmc_chan_readl(p, PWMC_CPRD);
+
+	cpre = cmr & PWMC_CMR_CPRE_MASK;
+	cmr &= ~BIT(PWMC_CMR_CPD);
+
+	cdty = cprd - (c->duty_ticks >> cpre);
+
+	p->duty_ticks = c->duty_ticks;
+
+	if (__atmel_pwm_is_on(p)) {
+		pwmc_chan_writel(p, PWMC_CMR, cmr);
+		pwmc_chan_writel(p, PWMC_CUPD, cdty);
+	} else
+		pwmc_chan_writel(p, PWMC_CDTY, cdty);
+
+	pr_debug("%s:%d duty_ticks = %lu cprd = %x"
+		 " cdty = %x cpre = %x\n",
+		 p->pwm->bus_id, p->chan, p->duty_ticks,
+		 cprd, cdty, cpre);
+
+	return 0;
+}
+
+
+static inline int
+__atmel_pwm_config_period_ticks(struct pwm_channel *p,
+				struct pwm_channel_config *c)
+{
+	u32 cmr, cprd, cpre;
+
+	cpre = fls(c->period_ticks);
+	if (cpre < 16)
+		cpre = 0;
+	else {
+		cpre -= 15;
+		if (cpre > 10)
+			return -EINVAL;
+	}
+
+	cmr = pwmc_chan_readl(p, PWMC_CMR);
+	cmr &= ~PWMC_CMR_CPRE_MASK;
+	cmr |= cpre;
+
+	cprd = c->period_ticks >> cpre;
+
+	pwmc_chan_writel(p, PWMC_CMR, cmr);
+	pwmc_chan_writel(p, PWMC_CPRD, cprd);
+	p->period_ticks = c->period_ticks;
+
+	pr_debug("%s:%d period_ticks = %lu cprd = %x cpre = %x\n",
+		 p->pwm->bus_id, p->chan, p->period_ticks, cprd, cpre);
+
+	return 0;
+}
+
+
+
+static int
+atmel_pwm_config_nosleep(struct pwm_channel *p,
+			 struct pwm_channel_config *c)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&p->lock, flags);
+
+	switch (c->config_mask) {
+
+	case PWM_CONFIG_DUTY_TICKS:
+		__atmel_pwm_config_duty_ticks(p, c);
+		break;
+
+	case PWM_CONFIG_STOP:
+		__atmel_pwm_stop(p);
+		pr_debug("%s:%d stop\n", p->pwm->bus_id, p->chan);
+		break;
+
+	case PWM_CONFIG_START:
+		__atmel_pwm_start(p);
+		pr_debug("%s:%d start\n", p->pwm->bus_id, p->chan);
+		break;
+
+	case PWM_CONFIG_POLARITY:
+		__atmel_pwm_config_polarity(p, c);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	spin_unlock_irqrestore(&p->lock, flags);
+	return ret;
+}
+
+
+static int
+atmel_pwm_stop_sync(struct pwm_channel *p)
+{
+	struct atmel_pwm *ap = container_of(p->pwm, struct atmel_pwm, pwm);
+	int ret;
+	int was_on = __atmel_pwm_is_on(p);
+
+	if (was_on) {
+		do {
+			init_completion(&p->complete);
+			set_bit(FLAG_STOP, &p->flags);
+			pwmc_writel(ap, PWMC_IER, 1 << p->chan);
+
+			pr_debug("%s:%d waiting on stop_sync completion...\n",
+				 p->pwm->bus_id, p->chan);
+
+			ret = wait_for_completion_interruptible(&p->complete);
+
+			pr_debug("%s:%d stop_sync complete (%d)\n",
+				 p->pwm->bus_id, p->chan, ret);
+
+			if (ret)
+				return ret;
+		} while (p->flags & BIT(FLAG_STOP));
+	}
+
+	pr_debug("%s:%d stop_sync returning %d\n",
+		 p->pwm->bus_id, p->chan, was_on);
+
+	return was_on;
+}
+
+
+static int
+atmel_pwm_config(struct pwm_channel *p,
+		 struct pwm_channel_config *c)
+{
+	int was_on = 0;
+
+	if (p->pwm->config_nosleep) {
+		if (!p->pwm->config_nosleep(p, c))
+			return 0;
+	}
+
+	might_sleep();
+
+	pr_debug("%s:%d config_mask %x\n",
+		 p->pwm->bus_id, p->chan, c->config_mask);
+
+	was_on = atmel_pwm_stop_sync(p);
+	if (was_on < 0)
+		return was_on;
+
+	if (c->config_mask & PWM_CONFIG_PERIOD_TICKS) {
+		__atmel_pwm_config_period_ticks(p, c);
+		if (!(c->config_mask & PWM_CONFIG_DUTY_TICKS)) {
+			struct pwm_channel_config d = {
+				.config_mask = PWM_CONFIG_DUTY_TICKS,
+				.duty_ticks = p->duty_ticks,
+			};
+			__atmel_pwm_config_duty_ticks(p, &d);
+		}
+	}
+
+	if (c->config_mask & PWM_CONFIG_DUTY_TICKS)
+		__atmel_pwm_config_duty_ticks(p, c);
+
+	if (c->config_mask & PWM_CONFIG_POLARITY)
+		__atmel_pwm_config_polarity(p, c);
+
+	if ((c->config_mask & PWM_CONFIG_START)
+	    || (was_on && !(c->config_mask & PWM_CONFIG_STOP)))
+		__atmel_pwm_start(p);
+
+	return 0;
+}
+
+
+static void
+__atmel_pwm_set_callback(struct pwm_channel *p,
+			 pwm_callback_t callback)
+{
+	struct atmel_pwm *ap = container_of(p->pwm, struct atmel_pwm, pwm);
+
+	p->callback = callback;
+	pwmc_writel(ap, p->callback ? PWMC_IER : PWMC_IDR, 1 << p->chan);
+	pr_debug("%s:%d set_callback %p\n", p->pwm->bus_id, p->chan, callback);
+}
+
+
+static int
+atmel_pwm_set_callback(struct pwm_channel *p,
+		       pwm_callback_t callback)
+{
+	struct atmel_pwm *ap = container_of(p->pwm, struct atmel_pwm, pwm);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ap->lock, flags);
+	__atmel_pwm_set_callback(p, callback);
+	spin_unlock_irqrestore(&ap->lock, flags);
+
+	return 0;
+}
+
+
+static int
+atmel_pwm_request(struct pwm_channel *p)
+{
+	struct atmel_pwm *ap = container_of(p->pwm, struct atmel_pwm, pwm);
+	unsigned long flags;
+
+	spin_lock_irqsave(&p->lock, flags);
+	clk_enable(ap->clk);
+	p->tick_hz = clk_get_rate(ap->clk);
+	__atmel_pwm_unsynchronize(p, NULL);
+	__atmel_pwm_stop(p);
+	spin_unlock_irqrestore(&p->lock, flags);
+
+	return 0;
+}
+
+
+static void
+atmel_pwm_free(struct pwm_channel *p)
+{
+	struct atmel_pwm *ap = container_of(p->pwm, struct atmel_pwm, pwm);
+	clk_disable(ap->clk);
+}
+
+
+static irqreturn_t
+atmel_pwmc_irq(int irq, void *data)
+{
+	struct atmel_pwm *ap = data;
+	struct pwm_channel *p;
+	u32 isr;
+	int chid;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ap->lock, flags);
+
+	isr = pwmc_readl(ap, PWMC_ISR);
+	for (chid = 0; isr; chid++, isr >>= 1) {
+		p = &ap->pwm.channels[chid];
+		if (isr & 1) {
+			if (p->callback)
+				p->callback(p);
+			if (p->flags & BIT(FLAG_STOP)) {
+				__atmel_pwm_stop(p);
+				clear_bit(FLAG_STOP, &p->flags);
+			}
+			complete_all(&p->complete);
+		}
+	}
+
+	spin_unlock_irqrestore(&ap->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+
+static int __init
+atmel_pwmc_probe(struct platform_device *pdev)
+{
+	struct atmel_pwm *ap;
+	struct resource *r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	int ret = 0;
+
+	ap = kzalloc(sizeof(*ap), GFP_KERNEL);
+	if (!ap) {
+		ret = -ENOMEM;
+		goto err_atmel_pwm_alloc;
+	}
+
+	spin_lock_init(&ap->lock);
+	platform_set_drvdata(pdev, ap);
+
+	ap->pwm.bus_id = pdev->dev.bus_id;
+
+	ap->pwm.nchan = 4; /* TODO: true only for SAM9263 and AP7000 */
+	ap->ccnt_mask = 0xffffUL; /* TODO: true only for SAM9263 */
+
+	ap->sync_mask = kzalloc(ap->pwm.nchan * sizeof(u32), GFP_KERNEL);
+	if (!ap->sync_mask) {
+		ret = -ENOMEM;
+		goto err_alloc_sync_masks;
+	}
+
+	ap->pwm.owner = THIS_MODULE;
+	ap->pwm.request = atmel_pwm_request;
+	ap->pwm.free = atmel_pwm_free;
+	ap->pwm.config_nosleep = atmel_pwm_config_nosleep;
+	ap->pwm.config = atmel_pwm_config;
+	ap->pwm.synchronize = atmel_pwm_synchronize;
+	ap->pwm.unsynchronize = atmel_pwm_unsynchronize;
+	ap->pwm.set_callback = atmel_pwm_set_callback;
+
+	ap->clk = clk_get(&pdev->dev, "pwm_clk");
+	if (IS_ERR(ap->clk)) {
+		pr_info("%s: clk_get error %ld\n",
+			ap->pwm.bus_id, PTR_ERR(ap->clk));
+		ret = -ENODEV;
+		goto err_clk_get;
+	}
+
+	ap->iobase = ioremap_nocache(r->start, r->end - r->start + 1);
+	if (!ap->iobase) {
+		ret = -ENODEV;
+		goto err_ioremap;
+	}
+
+	clk_enable(ap->clk);
+	pwmc_writel(ap, PWMC_DIS, -1);
+	pwmc_writel(ap, PWMC_IDR, -1);
+	clk_disable(ap->clk);
+
+	ap->irq = platform_get_irq(pdev, 0);
+	if (ap->irq != -ENXIO) {
+		ret = request_irq(ap->irq, atmel_pwmc_irq, 0,
+				  ap->pwm.bus_id, ap);
+		if (ret)
+			goto err_request_irq;
+	}
+
+	ret = pwm_register(&ap->pwm);
+	if (ret)
+		goto err_pwm_register;
+
+	return 0;
+
+err_pwm_register:
+	if (ap->irq != -ENXIO)
+		free_irq(ap->irq, ap);
+err_request_irq:
+	iounmap(ap->iobase);
+err_ioremap:
+	clk_put(ap->clk);
+err_clk_get:
+	platform_set_drvdata(pdev, NULL);
+err_alloc_sync_masks:
+	kfree(ap);
+err_atmel_pwm_alloc:
+	return ret;
+}
+
+
+static int __devexit
+atmel_pwmc_remove(struct platform_device *pdev)
+{
+	struct atmel_pwm *ap = platform_get_drvdata(pdev);
+	int ret;
+
+	/* TODO: what can we do if this fails? */
+	ret = pwm_unregister(&ap->pwm);
+
+	clk_enable(ap->clk);
+	pwmc_writel(ap, PWMC_IDR, -1);
+	pwmc_writel(ap, PWMC_DIS, -1);
+	clk_disable(ap->clk);
+
+	if (ap->irq != -ENXIO)
+		free_irq(ap->irq, ap);
+
+	clk_put(ap->clk);
+	iounmap(ap->iobase);
+
+	kfree(ap);
+
+	return 0;
+}
+
+
+static struct platform_driver atmel_pwm_driver = {
+	.driver = {
+		.name = "atmel_pwmc",
+		.owner = THIS_MODULE,
+	},
+	.probe = atmel_pwmc_probe,
+	.remove = __devexit_p(atmel_pwmc_remove),
+};
+
+
+static int __init atmel_pwm_init(void)
+{
+	return platform_driver_register(&atmel_pwm_driver);
+}
+module_init(atmel_pwm_init);
+
+
+static void atmel_pwm_exit(void)
+{
+	platform_driver_unregister(&atmel_pwm_driver);
+}
+module_exit(atmel_pwm_exit);
+
+
+MODULE_AUTHOR("Bill Gatliff <bgat <at> billgatliff.com>");
+MODULE_DESCRIPTION("Driver for Atmel PWMC peripheral");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:atmel_pwmc");
+
diff --git a/drivers/pwm/ns9xxx-pwm.c b/drivers/pwm/ns9xxx-pwm.c
new file mode 100644
index 000000000000..b128c971da33
--- /dev/null
+++ b/drivers/pwm/ns9xxx-pwm.c
@@ -0,0 +1,554 @@
+/*
+ * drivers/pwm/ns9xxx-pwm.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+# define LAST_COUNT	0xFFFFFFFF	/* 32 bit counter */
+// # define DEBUG
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+#include <mach/gpio.h>
+#include <mach/regs-sys-ns921x.h>
+#include <mach/regs-io-ns921x.h>
+#include <mach/ns9xxx-pwm.h>
+
+struct ns9xxx_pwm {
+	struct pwm_device pwm;
+	spinlock_t lock;
+	struct clk *clk;
+
+	struct ns9xxx_pwm_pdata *pdata;
+};
+
+/* @XXX: Return NULL by failures */
+static inline struct ns9xxx_pwm_channel *ns9xxx_pwm_to_channel(struct pwm_channel *p)
+{
+	struct ns9xxx_pwm *np;
+	struct ns9xxx_pwm_channel *retval;
+
+
+	if (!p) {
+		pr_err("%s: NULL pointer passed!\n", __func__);
+		return NULL;
+	}
+
+	np = container_of(p->pwm, struct ns9xxx_pwm, pwm);
+	if (!np) {
+		pr_err("%s: NO np found!\n", __func__);
+		return NULL;
+	}
+
+	if (!np->pdata) {
+		pr_err("%s: NO platform data found!\n", __func__);
+		return NULL;
+	}
+	
+	if (!np->pdata->channels)
+		pr_err("NO channels passed?\n");
+
+	retval = np->pdata->channels + p->chan;
+	if (!retval)
+		pr_err("%s: Invalid channel number passed?\n", __func__);
+
+	return retval;
+}
+
+/* Checks if pwm is being used */
+static inline int __ns9xxx_pwm_is_on(struct pwm_channel *p)
+{
+	struct ns9xxx_pwm *np = container_of(p->pwm, struct ns9xxx_pwm, pwm);
+	u32 timer_control;
+//	struct ns9xxx_pwm_pdata *pdata;
+	struct ns9xxx_pwm_channel *pch;
+
+	if (!np) {
+		pr_err("Unable to fetch structure ns9xxx_pwm.\n");
+		return -EINVAL;
+	}
+
+	//pdata = np->pdata;
+	//pch = pdata->channels + p->chan;
+	pch = ns9xxx_pwm_to_channel(p);
+	timer_control = readl(SYS_TC(pch->timer));
+
+	return (timer_control & NS921X_TCR_TE) ? 1 : 0;
+}
+
+/* Starts PWM output */
+static inline void __ns9xxx_pwm_start(struct pwm_channel *p)
+{
+	//struct ns9xxx_pwm *np = container_of(p->pwm, struct ns9xxx_pwm, pwm);
+	struct ns9xxx_pwm_channel *pch;
+
+	pch = ns9xxx_pwm_to_channel(p);
+
+	writel(NS921X_TCR_RELOADEN | NS921X_TCR_BITTIMER |
+	       NS921X_TCR_TCLKSEL_AHB | NS921X_TCR_TE |
+	       NS921X_TCR_TMODE2_PWM, SYS_TC(pch->timer));
+}
+
+/* Stops PWM output */
+static inline void __ns9xxx_pwm_stop(struct pwm_channel *p)
+{
+	struct ns9xxx_pwm_channel *ch;
+
+	ch = ns9xxx_pwm_to_channel(p);
+	writel(0x0, SYS_TC(ch->timer));
+}
+
+/* Configures GPIO */
+static inline int __ns9xxx_pwm_config_gpio(struct pwm_channel *p, int invert)
+{
+	int gpionr;
+	struct ns9xxx_pwm_channel *ch;
+
+	if (!p) {
+		pr_err("Unable to fetch structure ns9xxx_pwm.\n");
+		return -EINVAL;
+	}
+
+	ch = ns9xxx_pwm_to_channel(p);
+	gpionr = ch->gpio;
+	gpio_configure_ns921x_unlocked(gpionr, 0,
+				       invert ? NS921X_GPIO_INVERT : NS921X_GPIO_DONT_INVERT,
+				       NS921X_GPIO_FUNC_2, 0);
+
+	return 0;
+}
+
+/* Toggles PWM output polarity */
+static inline int
+__ns9xxx_pwm_config_polarity(struct pwm_channel *p, struct pwm_channel_config *c)
+{
+	return __ns9xxx_pwm_config_gpio(p, c->polarity);
+}
+
+/* Configures duty_ticks */
+static inline int
+__ns9xxx_pwm_config_duty_ticks(struct pwm_channel *p, struct pwm_channel_config *c)
+{
+	struct ns9xxx_pwm *np = container_of(p->pwm, struct ns9xxx_pwm, pwm);
+	struct ns9xxx_pwm_channel *ch;
+	u32 reload, low;
+
+	ch = ns9xxx_pwm_to_channel(p);
+
+	if ((np == NULL) || (ch == NULL)) {
+		pr_err("Incorrect parameters in __ns9xxx_pwm_config_duty_ticks()\n");
+		return -EINVAL;
+	}
+
+	/* While trying to configure duty without period, assume 50% duty cycle */
+	if (p->period_ticks == 0)
+		p->period_ticks = c->duty_ticks * 2;
+
+	reload = LAST_COUNT - p->period_ticks;
+	low = reload + c->duty_ticks;
+
+	/* 
+	 * There are [0-9] Timers, but PWM extended runs on [6-9] SYS_THR
+	 * and SYS_TLR is just available for [6-9], so the mapping
+	 * would be: 0-> Timer 6, .., 3-> Timer 9
+	 */
+	writel(reload, SYS_TRELCCR(ch->timer));
+	writel(reload, SYS_THR(ch->timer - 6));	/* high equals to reload */
+	writel(low, SYS_TLR(ch->timer - 6));
+
+	p->duty_ticks = c->duty_ticks;
+
+	pr_debug
+	    ("\np->period_ticks: %lu chan: %i p->duty_ticks: %lu cduty_ticks: %lu cperiod_ticks: %lu \n",
+	     p->period_ticks, p->chan, p->duty_ticks, c->duty_ticks, c->period_ticks);
+	pr_debug("period_ns: %lu chan: %i duty_ns: %lu config_mask: %i gpio: %i\n",
+		 c->period_ns, p->chan, c->duty_ns, c->config_mask, ch->gpio);
+	pr_debug("LAST_COUNT: %X reload: %X low: %X\n", LAST_COUNT, reload, low);
+
+	return 0;
+}
+
+
+/* 
+ * Figure to understand PWM concepts
+ *		-----------------
+ *		|		|
+ *		|		|
+ * --------------		-----------------
+ * ^		^		^		^
+ * RELOAD	HIGH		LOW		LAST_COUNT
+ *
+ * Following driver assumes RELOAD = HIGH
+ */
+
+/* Configures period_ticks */
+static inline int
+__ns9xxx_pwm_config_period_ticks(struct pwm_channel *p, struct pwm_channel_config *c)
+{
+	struct ns9xxx_pwm *np = container_of(p->pwm, struct ns9xxx_pwm, pwm);
+	struct ns9xxx_pwm_channel *ch;
+	u32 reload, low;
+
+	ch = ns9xxx_pwm_to_channel(p);
+
+	if ((np == NULL) || (ch == NULL)) {
+		pr_err("Incorrect parameters in __ns9xxx_pwm_config_duty_ticks()\n");
+		return -EINVAL;
+	}
+
+	reload = LAST_COUNT - c->period_ticks;
+
+	/* While trying to configure period without duty, assume 50% duty cycle */
+	if (p->duty_ticks == 0)
+		p->duty_ticks = c->period_ticks / 2;
+
+	low = reload + p->duty_ticks;
+
+	/* 
+	 * There are [0-9] Timers, but PWM extended runs on [6-9]
+	 * SYS_THR and SYS_TLR is just available for [6-9], so the mnpping
+	 * would be: 0-> Timer 6, .., 3-> Timer 9
+	 */
+	writel(reload, SYS_TRELCCR(ch->timer));
+	writel(reload, SYS_THR(ch->timer - 6));
+	writel(low, SYS_TLR(ch->timer - 6));
+
+	p->period_ticks = c->period_ticks;
+
+	pr_debug
+	    ("\np->period_ticks: %lu chan: %i p->duty_ticks: %lu cduty_ticks: %lu cperiod_ticks: %lu\n",
+	     p->period_ticks, p->chan, p->duty_ticks, c->duty_ticks, c->period_ticks);
+	pr_debug("period_ns: %lu chan: %i duty_ns: %lu config_mask: %i gpio: %i\n",
+		 c->period_ns, p->chan, c->duty_ns, c->config_mask, ch->gpio);
+	pr_debug("LAST_COUNT: %X reload: %X low: %X \n", LAST_COUNT, reload, low);
+
+	return 0;
+}
+
+/* No sleeping configuration */
+static int ns9xxx_pwm_config_nosleep(struct pwm_channel *p, struct pwm_channel_config *c)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&p->lock, flags);
+
+	switch (c->config_mask) {
+
+	case PWM_CONFIG_DUTY_TICKS:
+		__ns9xxx_pwm_config_duty_ticks(p, c);
+		pr_debug("duty_ns: %lu period_ns: %lu\n", c->duty_ns, c->period_ns);
+		break;
+
+	case PWM_CONFIG_PERIOD_TICKS:
+		__ns9xxx_pwm_config_period_ticks(p, c);
+		pr_debug("duty_ns: %lu period_ns: %lu\n", c->duty_ns, c->period_ns);
+		break;
+
+	case PWM_CONFIG_STOP:
+		__ns9xxx_pwm_stop(p);
+		pr_debug("%s:%d stop\n", p->pwm->bus_id, p->chan);
+		break;
+
+	case PWM_CONFIG_START:
+		__ns9xxx_pwm_start(p);
+		pr_debug("%s:%d start\n", p->pwm->bus_id, p->chan);
+		break;
+
+	case PWM_CONFIG_POLARITY:
+		__ns9xxx_pwm_config_polarity(p, c);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	spin_unlock_irqrestore(&p->lock, flags);
+	return ret;
+}
+
+/* PWM configuration */
+static int ns9xxx_pwm_config(struct pwm_channel *p, struct pwm_channel_config *c)
+{
+	int was_on = 0;
+
+	if (p->pwm->config_nosleep) {
+		if (!p->pwm->config_nosleep(p, c))
+			return 0;
+	}
+
+	might_sleep();
+
+	was_on = __ns9xxx_pwm_is_on(p);
+	if (was_on < 0)
+		return was_on;
+
+	if (c->config_mask & PWM_CONFIG_PERIOD_TICKS) {
+		__ns9xxx_pwm_config_period_ticks(p, c);
+		if (!(c->config_mask & PWM_CONFIG_DUTY_TICKS)) {
+			struct pwm_channel_config d = {
+				.config_mask = PWM_CONFIG_DUTY_TICKS,
+				.duty_ticks = p->duty_ticks,
+			};
+			__ns9xxx_pwm_config_duty_ticks(p, &d);
+		}
+	}
+
+	if (c->config_mask & PWM_CONFIG_DUTY_TICKS)
+		__ns9xxx_pwm_config_duty_ticks(p, c);
+
+	if (c->config_mask & PWM_CONFIG_POLARITY)
+		__ns9xxx_pwm_config_polarity(p, c);
+
+	if ((c->config_mask & PWM_CONFIG_START)
+	    || (was_on && !(c->config_mask & PWM_CONFIG_STOP)))
+		__ns9xxx_pwm_start(p);
+
+	pr_debug("%s:%d config_mask %x\n", p->pwm->bus_id, p->chan, c->config_mask);
+
+	return 0;
+}
+
+/* PWM request function */
+static int ns9xxx_pwm_request(struct pwm_channel *p)
+{
+	struct ns9xxx_pwm *np = container_of(p->pwm, struct ns9xxx_pwm, pwm);
+	struct ns9xxx_pwm_channel *chi;
+	unsigned long flags;
+	char pwmgpio[20];
+	int ret;
+
+	chi = ns9xxx_pwm_to_channel(p);
+
+	spin_lock_irqsave(&p->lock, flags);
+
+	sprintf(pwmgpio, "%s.%d", p->pwm->bus_id, p->chan);
+	ret = gpio_request(chi->gpio, pwmgpio);
+	if (ret) {
+		pr_err("Request of GPIO %i failure\n", chi->gpio);
+		goto gpio_err;
+	}
+
+	ret = __ns9xxx_pwm_config_gpio(p, 0);
+	if (ret) {
+		pr_err("Configuring GPIO %i failure\n", chi->gpio);
+		goto gpio_cfg;
+	}
+
+	/* TODO, request also the timer? use the enable bit of the timer to 
+	 * determine if the timer is alredy enabled == requested 
+	 *
+	 * This can not be done, ATM, as Uboot sets TE active by default 
+	 */
+
+	clk_enable(np->clk);
+	p->tick_hz = clk_get_rate(np->clk);
+	__ns9xxx_pwm_stop(p);
+	spin_unlock_irqrestore(&p->lock, flags);
+
+	pr_debug("%s: tick_hz = %lu\n", __func__, p->tick_hz);
+
+	return 0;
+
+gpio_cfg:
+	gpio_configure_ns921x_unlocked(chi->gpio, 1, NS921X_GPIO_DONT_INVERT,
+				       NS921X_GPIO_FUNC_3, 0);
+	gpio_free(chi->gpio);
+gpio_err:
+	return ret;
+}
+
+/* Frees resources taken by PWM */
+static void ns9xxx_pwm_free(struct pwm_channel *p)
+{
+	struct ns9xxx_pwm *np = container_of(p->pwm, struct ns9xxx_pwm, pwm);
+	struct ns9xxx_pwm_channel *chi;
+	chi = ns9xxx_pwm_to_channel(p);
+
+
+	pr_debug("%s\n", __func__);
+
+	clk_disable(np->clk);
+	
+	gpio_configure_ns921x_unlocked(chi->gpio, 1, NS921X_GPIO_DONT_INVERT,
+				       NS921X_GPIO_FUNC_3, 0);
+	gpio_free(chi->gpio);
+}
+
+static int __init ns9xxx_pwmc_probe(struct platform_device *pdev)
+{
+	struct ns9xxx_pwm *np;
+	struct ns9xxx_pwm_pdata *pdata;
+	int ret;
+
+	pr_debug("Probing a new device (ID %i)\n", pdev->id);
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		pr_err("No platform data passed? Aborting.\n");
+		return -EINVAL;
+	}
+
+	/* Sanity check */
+	if (pdata->number_channels > NS9XXX_PWM_CHANNEL_MAX) {
+		pr_err("Invalid number '%i' of channels\n", pdata->number_channels);
+		return -EINVAL;
+	}
+
+	pr_debug("%i PWM channels registered.\n", pdata->number_channels);
+
+	np = kzalloc(sizeof(*np), GFP_KERNEL);
+	if (!np) {
+		ret = -ENOMEM;
+		goto err_ns9xxx_pwm_alloc;
+	}
+
+	spin_lock_init(&np->lock);
+	platform_set_drvdata(pdev, np);
+
+	/* @XXX: Probably not the best place for this assignment */
+	np->pdata = pdata;
+
+	np->pwm.bus_id = pdev->dev.bus_id;
+	np->pwm.nchan = pdata->number_channels;
+
+	/* Copy the external platform data to our internal structure */
+	//memcpy(&np->pdata, pdata, sizeof(*pdata));
+
+	np->pwm.owner = THIS_MODULE;
+	np->pwm.request = ns9xxx_pwm_request;
+	np->pwm.free = ns9xxx_pwm_free;
+	np->pwm.config_nosleep = ns9xxx_pwm_config_nosleep;
+	np->pwm.config = ns9xxx_pwm_config;
+
+	ret = pwm_register(&np->pwm);
+	if (ret) {
+		pr_debug("Failure registering pwm\n");
+		goto err_pwm_register;
+	}
+
+	np->clk = clk_get(&pdev->dev, "ahbclock");
+	if (IS_ERR(np->clk)) {
+		pr_info("%s: clk_get error %ld\n", np->pwm.bus_id, PTR_ERR(np->clk));
+		ret = -ENODEV;
+		goto err_clk_get;
+	}
+
+	return 0;
+
+err_pwm_register:
+	clk_put(np->clk);
+err_clk_get:
+	platform_set_drvdata(pdev, NULL);
+	kfree(np);
+err_ns9xxx_pwm_alloc:
+	return ret;
+}
+
+static int __devexit ns9xxx_pwmc_remove(struct platform_device *pdev)
+{
+	struct ns9xxx_pwm *np;
+	struct ns9xxx_pwm_pdata *pdata;
+	struct device *d = &pdev->dev;
+	struct ns9xxx_pwm_channel *chi;
+	struct pwm_channel *p;
+	int ret;
+	u32 i;
+
+	pr_debug("Removing device (ID %i)\n", pdev->id);
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		pr_err("No platform data passed? Aborting.\n");
+		return -EINVAL;
+	}
+
+	np = platform_get_drvdata(pdev);
+	p = np->pwm.channels;
+
+	ret = pwm_unregister(&np->pwm);
+	if (ret) {
+		pr_debug("Failure unregistering pwm\n");
+	}
+
+	/*
+	 * @TODO: We might want to reset Read and Capture Register, but we can not write to it. 
+	 */
+	for (i=0; i < pdata->number_channels; i++) {
+
+		chi = ns9xxx_pwm_to_channel(p+i);
+
+		/* Resets Timer Control Register */
+		writel(0x00, SYS_TC(chi->timer));
+
+		/* Resets High, Low, Reload Registers*/
+		writel(0x00, SYS_TRELCCR(chi->timer));
+		writel(0x00, SYS_THR(chi->timer - 6));	/* high equals to reload */
+		writel(0x00, SYS_TLR(chi->timer - 6));
+
+		/* Sets GPIO as inputs */
+		gpio_configure_ns921x_unlocked(chi->gpio, 1, NS921X_GPIO_DONT_INVERT,
+					       NS921X_GPIO_FUNC_3, 0);
+
+		/* Frees GPIO */
+		gpio_free(chi->gpio);
+	}
+
+	clk_put(np->clk);
+	platform_set_drvdata(pdev, NULL);
+
+	kfree(np);
+	module_put(d->driver->owner);
+
+	return 0;
+}
+
+static struct platform_driver ns9xxx_pwm_driver = {
+	.driver = {
+		   .name = "ns9xxx_pwmc",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = ns9xxx_pwmc_probe,
+	.remove = __devexit_p(ns9xxx_pwmc_remove),
+};
+
+static int __init ns9xxx_pwm_init(void)
+{
+	return platform_driver_register(&ns9xxx_pwm_driver);
+}
+
+module_init(ns9xxx_pwm_init);
+
+static void ns9xxx_pwm_exit(void)
+{
+	platform_driver_unregister(&ns9xxx_pwm_driver);
+}
+
+module_exit(ns9xxx_pwm_exit);
+
+MODULE_AUTHOR("Hector Oron <Hector.Oron <at> digi.com>");
+MODULE_DESCRIPTION("Driver for ns9xxx PWMC peripheral");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:ns9xxx_pwmc");
diff --git a/drivers/pwm/pwm.c b/drivers/pwm/pwm.c
new file mode 100644
index 000000000000..32e84f86bf62
--- /dev/null
+++ b/drivers/pwm/pwm.c
@@ -0,0 +1,626 @@
+/*
+ * drivers/pwm/pwm.c
+ *
+ * Copyright (C) 2008 Bill Gatliff
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#if 0
+#define DEBUG
+#endif
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/spinlock.h>
+#include <linux/fs.h>
+#include <linux/completion.h>
+#include <linux/workqueue.h>
+#include <linux/pwm.h>
+
+static int __pwm_create_sysfs(struct pwm_device *pwm);
+
+static LIST_HEAD(pwm_device_list);
+static DEFINE_MUTEX(device_list_mutex);
+static struct class pwm_class;
+static struct workqueue_struct *pwm_handler_workqueue;
+
+int pwm_register(struct pwm_device *pwm)
+{
+	struct pwm_channel *p;
+	int wchan;
+	int ret = 0;
+
+	spin_lock_init(&pwm->list_lock);
+
+	p = kcalloc(pwm->nchan, sizeof(struct pwm_channel), GFP_KERNEL);
+	if (!p)
+		return -ENOMEM;
+
+	for (wchan = 0; wchan < pwm->nchan; wchan++) {
+		spin_lock_init(&p[wchan].lock);
+		init_completion(&p[wchan].complete);
+		p[wchan].chan = wchan;
+		p[wchan].pwm = pwm;
+	}
+
+	pwm->channels = p;
+
+	mutex_lock(&device_list_mutex);
+
+	list_add_tail(&pwm->list, &pwm_device_list);
+	ret = __pwm_create_sysfs(pwm);
+	if (ret) {
+		mutex_unlock(&device_list_mutex);
+		goto err_create_sysfs;
+	}
+
+	mutex_unlock(&device_list_mutex);
+
+	pr_info("%s: %d channels\n", pwm->bus_id, pwm->nchan);
+	return 0;
+
+err_create_sysfs:
+	kfree(p);
+
+	return ret;
+}
+
+EXPORT_SYMBOL(pwm_register);
+
+static int __match_device(struct device *dev, void *data)
+{
+	return dev_get_drvdata(dev) == data;
+}
+
+int pwm_unregister(struct pwm_device *pwm)
+{
+	int wchan;
+	struct device *dev;
+
+	mutex_lock(&device_list_mutex);
+
+	for (wchan = 0; wchan < pwm->nchan; wchan++) {
+		if (pwm->channels[wchan].flags & FLAG_REQUESTED) {
+			mutex_unlock(&device_list_mutex);
+			return -EBUSY;
+		}
+	}
+
+	for (wchan = 0; wchan < pwm->nchan; wchan++) {
+		dev = class_find_device(&pwm_class, NULL,
+					&pwm->channels[wchan], __match_device);
+		if (dev) {
+			put_device(dev);
+			device_unregister(dev);
+		}
+	}
+
+	kfree(pwm->channels);
+	list_del(&pwm->list);
+	mutex_unlock(&device_list_mutex);
+
+	return 0;
+}
+
+EXPORT_SYMBOL(pwm_unregister);
+
+static struct pwm_device *__pwm_find_device(const char *bus_id)
+{
+	struct pwm_device *p;
+
+	list_for_each_entry(p, &pwm_device_list, list) {
+		if (!strcmp(bus_id, p->bus_id))
+			return p;
+	}
+	return NULL;
+}
+
+static int __pwm_request_channel(struct pwm_channel *p, const char *requester)
+{
+	int ret;
+
+	if (test_and_set_bit(FLAG_REQUESTED, &p->flags))
+		return -EBUSY;
+
+	if (p->pwm->request) {
+		ret = p->pwm->request(p);
+		if (ret) {
+			clear_bit(FLAG_REQUESTED, &p->flags);
+			return ret;
+		}
+	}
+
+	p->requester = requester;
+	return 0;
+}
+
+struct pwm_channel *pwm_request(const char *bus_id, int chan, const char *requester)
+{
+	struct pwm_device *p;
+	int ret;
+
+	mutex_lock(&device_list_mutex);
+
+	p = __pwm_find_device(bus_id);
+	if (!p || chan >= p->nchan)
+		goto err_no_device;
+
+	if (!try_module_get(p->owner))
+		goto err_module_get_failed;
+
+	ret = __pwm_request_channel(&p->channels[chan], requester);
+	if (ret)
+		goto err_request_failed;
+
+	mutex_unlock(&device_list_mutex);
+
+	pr_debug("%s: %s:%d returns %p\n", __func__, bus_id, chan, &p->channels[chan]);
+
+	return &p->channels[chan];
+
+err_request_failed:
+	module_put(p->owner);
+err_module_get_failed:
+err_no_device:
+
+	mutex_unlock(&device_list_mutex);
+
+	pr_debug("%s: %s:%d returns NULL\n", __func__, bus_id, chan);
+
+	return NULL;
+}
+
+EXPORT_SYMBOL(pwm_request);
+
+void pwm_free(struct pwm_channel *p)
+{
+	mutex_lock(&device_list_mutex);
+
+	if (!test_and_clear_bit(FLAG_REQUESTED, &p->flags))
+		goto done;
+
+	pwm_stop(p);
+	pwm_unsynchronize(p, NULL);
+	pwm_set_handler(p, NULL, NULL);
+
+	if (p->pwm->free)
+		p->pwm->free(p);
+	module_put(p->pwm->owner);
+
+	pr_debug("%s: %s:%d free\n", __func__, p->pwm->bus_id, p->chan);
+
+done:
+	mutex_unlock(&device_list_mutex);
+}
+
+EXPORT_SYMBOL(pwm_free);
+
+unsigned long pwm_ns_to_ticks(struct pwm_channel *p, unsigned long nsecs)
+{
+	unsigned long long ticks;
+
+	ticks = nsecs;
+	ticks *= p->tick_hz;
+	do_div(ticks, 1000000000);
+
+	return ticks;
+}
+
+EXPORT_SYMBOL(pwm_ns_to_ticks);
+
+unsigned long pwm_ticks_to_ns(struct pwm_channel *p, unsigned long ticks)
+{
+	unsigned long long ns;
+
+	if (!p->tick_hz)
+		return 0;
+
+	ns = ticks;
+	ns *= 1000000000UL;
+	do_div(ns, p->tick_hz);
+	return ns;
+}
+
+EXPORT_SYMBOL(pwm_ticks_to_ns);
+
+static void pwm_config_ns_to_ticks(struct pwm_channel *p, struct pwm_channel_config *c)
+{
+	if (c->config_mask & PWM_CONFIG_PERIOD_NS) {
+		c->period_ticks = pwm_ns_to_ticks(p, c->period_ns);
+		c->config_mask &= ~PWM_CONFIG_PERIOD_NS;
+		c->config_mask |= PWM_CONFIG_PERIOD_TICKS;
+	}
+
+	if (c->config_mask & PWM_CONFIG_DUTY_NS) {
+		c->duty_ticks = pwm_ns_to_ticks(p, c->duty_ns);
+		c->config_mask &= ~PWM_CONFIG_DUTY_NS;
+		c->config_mask |= PWM_CONFIG_DUTY_TICKS;
+	}
+}
+
+static void pwm_config_percent_to_ticks(struct pwm_channel *p, struct pwm_channel_config *c)
+{
+	if (c->config_mask & PWM_CONFIG_DUTY_PERCENT) {
+		if (c->config_mask & PWM_CONFIG_PERIOD_TICKS)
+			c->duty_ticks = c->period_ticks;
+		else
+			c->duty_ticks = p->period_ticks;
+
+		c->duty_ticks *= c->duty_percent;
+		c->duty_ticks /= 100;
+		c->config_mask &= ~PWM_CONFIG_DUTY_PERCENT;
+		c->config_mask |= PWM_CONFIG_DUTY_TICKS;
+	}
+}
+
+int pwm_config_nosleep(struct pwm_channel *p, struct pwm_channel_config *c)
+{
+	if (!p->pwm->config_nosleep)
+		return -EINVAL;
+
+	pwm_config_ns_to_ticks(p, c);
+	pwm_config_percent_to_ticks(p, c);
+
+	return p->pwm->config_nosleep(p, c);
+}
+
+EXPORT_SYMBOL(pwm_config_nosleep);
+
+int pwm_config(struct pwm_channel *p, struct pwm_channel_config *c)
+{
+	int ret = 0;
+
+	if (unlikely(!p->pwm->config)) {
+		pr_debug("%s: %s:%d has no config handler (-EINVAL)\n",
+			 __func__, p->pwm->bus_id, p->chan);
+		return -EINVAL;
+	}
+
+	pwm_config_ns_to_ticks(p, c);
+	pwm_config_percent_to_ticks(p, c);
+
+	switch (c->config_mask & (PWM_CONFIG_PERIOD_TICKS | PWM_CONFIG_DUTY_TICKS)) {
+	case PWM_CONFIG_PERIOD_TICKS:
+		if (p->duty_ticks > c->period_ticks) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	case PWM_CONFIG_DUTY_TICKS:
+		if (p->period_ticks < c->duty_ticks) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	case PWM_CONFIG_DUTY_TICKS | PWM_CONFIG_PERIOD_TICKS:
+		if (c->duty_ticks > c->period_ticks) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	default:
+		break;
+	}
+
+err:
+	pr_debug("%s: config_mask %d period_ticks %lu duty_ticks %lu"
+		 " polarity %d duty_ns %lu period_ns %lu duty_percent %d\n",
+		 __func__, c->config_mask, c->period_ticks, c->duty_ticks,
+		 c->polarity, c->duty_ns, c->period_ns, c->duty_percent);
+
+	if (ret)
+		return ret;
+	return p->pwm->config(p, c);
+}
+
+EXPORT_SYMBOL(pwm_config);
+
+int pwm_set_period_ns(struct pwm_channel *p, unsigned long period_ns)
+{
+	struct pwm_channel_config c = {
+		.config_mask = PWM_CONFIG_PERIOD_TICKS,
+		.period_ticks = pwm_ns_to_ticks(p, period_ns),
+	};
+
+	return pwm_config(p, &c);
+}
+
+EXPORT_SYMBOL(pwm_set_period_ns);
+
+unsigned long pwm_get_period_ns(struct pwm_channel *p)
+{
+	return pwm_ticks_to_ns(p, p->period_ticks);
+}
+
+EXPORT_SYMBOL(pwm_get_period_ns);
+
+int pwm_set_duty_ns(struct pwm_channel *p, unsigned long duty_ns)
+{
+	struct pwm_channel_config c = {
+		.config_mask = PWM_CONFIG_DUTY_TICKS,
+		.duty_ticks = pwm_ns_to_ticks(p, duty_ns),
+	};
+	return pwm_config(p, &c);
+}
+
+EXPORT_SYMBOL(pwm_set_duty_ns);
+
+unsigned long pwm_get_duty_ns(struct pwm_channel *p)
+{
+	return pwm_ticks_to_ns(p, p->duty_ticks);
+}
+
+EXPORT_SYMBOL(pwm_get_duty_ns);
+
+int pwm_set_duty_percent(struct pwm_channel *p, int percent)
+{
+	struct pwm_channel_config c = {
+		.config_mask = PWM_CONFIG_DUTY_PERCENT,
+		.duty_percent = percent,
+	};
+	return pwm_config(p, &c);
+}
+
+EXPORT_SYMBOL(pwm_set_duty_percent);
+
+int pwm_set_polarity(struct pwm_channel *p, int active_high)
+{
+	struct pwm_channel_config c = {
+		.config_mask = PWM_CONFIG_POLARITY,
+		.polarity = active_high,
+	};
+	return pwm_config(p, &c);
+}
+
+EXPORT_SYMBOL(pwm_set_polarity);
+
+int pwm_start(struct pwm_channel *p)
+{
+	struct pwm_channel_config c = {
+		.config_mask = PWM_CONFIG_START,
+	};
+	return pwm_config(p, &c);
+}
+
+EXPORT_SYMBOL(pwm_start);
+
+int pwm_stop(struct pwm_channel *p)
+{
+	struct pwm_channel_config c = {
+		.config_mask = PWM_CONFIG_STOP,
+	};
+	return pwm_config(p, &c);
+}
+
+EXPORT_SYMBOL(pwm_stop);
+
+int pwm_synchronize(struct pwm_channel *p, struct pwm_channel *to_p)
+{
+	if (p->pwm != to_p->pwm) {
+		/* TODO: support cross-device synchronization */
+		return -EINVAL;
+	}
+
+	if (!p->pwm->synchronize)
+		return -EINVAL;
+
+	return p->pwm->synchronize(p, to_p);
+}
+
+EXPORT_SYMBOL(pwm_synchronize);
+
+int pwm_unsynchronize(struct pwm_channel *p, struct pwm_channel *from_p)
+{
+	if (from_p && (p->pwm != from_p->pwm)) {
+		/* TODO: support cross-device synchronization */
+		return -EINVAL;
+	}
+
+	if (!p->pwm->unsynchronize)
+		return -EINVAL;
+
+	return p->pwm->unsynchronize(p, from_p);
+}
+
+EXPORT_SYMBOL(pwm_unsynchronize);
+
+static void pwm_handler(struct work_struct *w)
+{
+	struct pwm_channel *p = container_of(w, struct pwm_channel,
+					     handler_work);
+	if (p->handler && p->handler(p, p->handler_data))
+		pwm_stop(p);
+}
+
+static void __pwm_callback(struct pwm_channel *p)
+{
+	queue_work(pwm_handler_workqueue, &p->handler_work);
+	pr_debug("%s:%d handler %p scheduled with data %p\n",
+		 p->pwm->bus_id, p->chan, p->handler, p->handler_data);
+}
+
+int pwm_set_handler(struct pwm_channel *p, pwm_handler_t handler, void *data)
+{
+	if (p->pwm->set_callback) {
+		p->handler_data = data;
+		p->handler = handler;
+		INIT_WORK(&p->handler_work, pwm_handler);
+		return p->pwm->set_callback(p, handler ? __pwm_callback : NULL);
+	}
+	return -EINVAL;
+}
+
+EXPORT_SYMBOL(pwm_set_handler);
+
+static ssize_t pwm_run_store(struct device *dev,
+			     struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct pwm_channel *p = dev_get_drvdata(dev);
+	if (sysfs_streq(buf, "1"))
+		pwm_start(p);
+	else if (sysfs_streq(buf, "0"))
+		pwm_stop(p);
+	return len;
+}
+
+static DEVICE_ATTR(run, 0200, NULL, pwm_run_store);
+
+static ssize_t pwm_duty_ns_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct pwm_channel *p = dev_get_drvdata(dev);
+	return sprintf(buf, "%lu\n", pwm_get_duty_ns(p));
+}
+
+static ssize_t pwm_duty_ns_store(struct device *dev,
+				 struct device_attribute *attr, const char *buf, size_t len)
+{
+	unsigned long duty_ns;
+	struct pwm_channel *p = dev_get_drvdata(dev);
+
+	if (1 == sscanf(buf, "%lu", &duty_ns))
+		pwm_set_duty_ns(p, duty_ns);
+	return len;
+}
+
+static DEVICE_ATTR(duty_ns, 0644, pwm_duty_ns_show, pwm_duty_ns_store);
+
+static ssize_t pwm_period_ns_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct pwm_channel *p = dev_get_drvdata(dev);
+	return sprintf(buf, "%lu\n", pwm_get_period_ns(p));
+}
+
+static ssize_t pwm_period_ns_store(struct device *dev,
+				   struct device_attribute *attr, const char *buf, size_t len)
+{
+	unsigned long period_ns;
+	struct pwm_channel *p = dev_get_drvdata(dev);
+
+	if (1 == sscanf(buf, "%lu", &period_ns))
+		pwm_set_period_ns(p, period_ns);
+
+	return len;
+}
+
+static DEVICE_ATTR(period_ns, 0644, pwm_period_ns_show, pwm_period_ns_store);
+
+static ssize_t pwm_polarity_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct pwm_channel *p = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", p->active_low ? 0 : 1);
+}
+
+static ssize_t pwm_polarity_store(struct device *dev,
+				  struct device_attribute *attr, const char *buf, size_t len)
+{
+	int polarity;
+	struct pwm_channel *p = dev_get_drvdata(dev);
+
+	if (1 == sscanf(buf, "%d", &polarity))
+		pwm_set_polarity(p, polarity);
+	return len;
+}
+
+static DEVICE_ATTR(polarity, 0644, pwm_polarity_show, pwm_polarity_store);
+
+static ssize_t pwm_request_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct pwm_channel *p = dev_get_drvdata(dev);
+	mutex_lock(&device_list_mutex);
+	__pwm_request_channel(p, "sysfs");
+	mutex_unlock(&device_list_mutex);
+
+	return sprintf(buf, "%s\n", p->requester);
+}
+
+static ssize_t pwm_request_store(struct device *dev,
+				 struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct pwm_channel *p = dev_get_drvdata(dev);
+	pwm_free(p);
+	return len;
+}
+
+static DEVICE_ATTR(request, 0644, pwm_request_show, pwm_request_store);
+
+static const struct attribute *pwm_attrs[] = {
+	&dev_attr_run.attr,
+	&dev_attr_polarity.attr,
+	&dev_attr_duty_ns.attr,
+	&dev_attr_period_ns.attr,
+	&dev_attr_request.attr,
+	NULL,
+};
+
+static const struct attribute_group pwm_device_attr_group = {
+	.attrs = (struct attribute **)pwm_attrs,
+};
+
+static int __pwm_create_sysfs(struct pwm_device *pwm)
+{
+	int ret = 0;
+	struct device *dev;
+	int wchan;
+
+	for (wchan = 0; wchan < pwm->nchan; wchan++) {
+		dev = device_create(&pwm_class, pwm->dev, MKDEV(0, 0),
+				    pwm->channels + wchan, "%s:%d", pwm->bus_id, wchan);
+		if (!dev)
+			goto err_dev_create;
+		ret = sysfs_create_group(&dev->kobj, &pwm_device_attr_group);
+		if (ret)
+			goto err_dev_group_create;
+	}
+
+	return ret;
+
+err_dev_group_create:
+err_dev_create:
+	/* TODO: undo all the successful device_create calls */
+	return -ENODEV;
+}
+
+static struct class_attribute pwm_class_attrs[] = {
+	__ATTR_NULL,
+};
+
+static struct class pwm_class = {
+	.name = "pwm",
+	.owner = THIS_MODULE,
+
+	.class_attrs = pwm_class_attrs,
+};
+
+static int __init pwm_init(void)
+{
+	int ret;
+
+	/* TODO: how to deal with devices that register very early? */
+
+	ret = class_register(&pwm_class);
+	if (ret < 0)
+		return ret;
+
+	pwm_handler_workqueue = create_workqueue("pwmd");
+
+	return 0;
+}
+
+postcore_initcall(pwm_init);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index 39360e2a4540..b6c600dac022 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -73,4 +73,34 @@ config REGULATOR_DA903X
 	  Say y here to support the BUCKs and LDOs regulators found on
 	  Dialog Semiconductor DA9030/DA9034 PMIC.
 
+config REGULATOR_MC13892
+	tristate "MC13892 Regulator Support"
+	depends on REGULATOR
+	depends on MXC_PMIC_MC13892
+	default y
+
+config REGULATOR_MC13783
+	tristate "MC13783 Regulator Support"
+	depends on REGULATOR
+	depends on MXC_PMIC_MC13783
+	default y
+
+config REGULATOR_MC34704
+	tristate "MC34704 Regulator Support"
+	depends on REGULATOR
+	depends on MXC_PMIC_MC34704
+	default y
+
+config REGULATOR_STMP3XXX
+	tristate "STMP3xxx Regulator Support"
+	depends on REGULATOR
+	depends on ARCH_STMP3XXX
+	default y
+
+config REGULATOR_MC9S08DZ60
+	tristate "mc9s08dz60 Regulator Support"
+	depends on REGULATOR
+	depends on MXC_PMIC_MC9S08DZ60
+	default y
+
 endif
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 254d40c02ee8..bc47cb77d7c5 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -2,7 +2,6 @@
 # Makefile for regulator drivers.
 #
 
-
 obj-$(CONFIG_REGULATOR) += core.o
 obj-$(CONFIG_REGULATOR_FIXED_VOLTAGE) += fixed.o
 obj-$(CONFIG_REGULATOR_VIRTUAL_CONSUMER) += virtual.o
@@ -12,4 +11,11 @@ obj-$(CONFIG_REGULATOR_WM8350) += wm8350-regulator.o
 obj-$(CONFIG_REGULATOR_WM8400) += wm8400-regulator.o
 obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
 
+obj-$(CONFIG_REGULATOR_MC13892) += reg-mc13892.o
+obj-$(CONFIG_REGULATOR_MC13783) += reg-mc13783.o
+obj-$(CONFIG_REGULATOR_MC34704) += reg-mc34704.o
+obj-$(CONFIG_REGULATOR_STMP3XXX) += stmp3xxx.o
+
+obj-$(CONFIG_REGULATOR_MC9S08DZ60) += reg-mc9s08dz60.o
+
 ccflags-$(CONFIG_REGULATOR_DEBUG) += -DDEBUG
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 02a774424e8d..d2a23066e3fb 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -927,6 +927,11 @@ struct regulator *regulator_get(struct device *dev, const char *id)
 			goto found;
 		}
 	}
+	list_for_each_entry(rdev, &regulator_list, list) {
+		if (strcmp(rdev->desc->name, id) == 0) {
+			goto found;
+		}
+	}
 	printk(KERN_ERR "regulator: Unable to get requested regulator: %s\n",
 	       id);
 	mutex_unlock(&regulator_list_mutex);
diff --git a/drivers/regulator/reg-mc13783.c b/drivers/regulator/reg-mc13783.c
new file mode 100644
index 000000000000..c761c7b99243
--- /dev/null
+++ b/drivers/regulator/reg-mc13783.c
@@ -0,0 +1,2661 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/driver.h>
+#include <linux/mfd/mc13783/core.h>
+#include <linux/platform_device.h>
+#include <linux/pmic_status.h>
+#include <linux/pmic_external.h>
+
+/*
+ * Convenience conversion.
+ * Here atm, maybe there is somewhere better for this.
+ */
+#define mV_to_uV(mV) (mV * 1000)
+#define uV_to_mV(uV) (uV / 1000)
+#define V_to_uV(V) (mV_to_uV(V * 1000))
+#define uV_to_V(uV) (uV_to_mV(uV) / 1000)
+
+/*!
+ * @enum regulator_voltage_sw
+ * @brief PMIC regulator SW output voltage.
+ */
+enum {
+	SW_0_9V = 0,		/*!< 0.900 V */
+	SW_0_925V,		/*!< 0.925 V */
+	SW_0_95V,		/*!< 0.950 V */
+	SW_0_975V,		/*!< 0.975 V */
+	SW_1V,			/*!< 1.000 V */
+	SW_1_025V,		/*!< 1.025 V */
+	SW_1_05V,		/*!< 1.050 V */
+	SW_1_075V,		/*!< 1.075 V */
+	SW_1_1V,		/*!< 1.100 V */
+	SW_1_125V,		/*!< 1.125 V */
+	SW_1_15V,		/*!< 1.150 V */
+	SW_1_175V,		/*!< 1.175 V */
+	SW_1_2V,		/*!< 1.200 V */
+	SW_1_225V,		/*!< 1.225 V */
+	SW_1_25V,		/*!< 1.250 V */
+	SW_1_275V,		/*!< 1.275 V */
+	SW_1_3V,		/*!< 1.300 V */
+	SW_1_325V,		/*!< 1.325 V */
+	SW_1_35V,		/*!< 1.350 V */
+	SW_1_375V,		/*!< 1.375 V */
+	SW_1_4V,		/*!< 1.400 V */
+	SW_1_425V,		/*!< 1.425 V */
+	SW_1_45V,		/*!< 1.450 V */
+	SW_1_475V,		/*!< 1.475 V */
+	SW_1_5V,		/*!< 1.500 V */
+	SW_1_525V,		/*!< 1.525 V */
+	SW_1_55V,		/*!< 1.550 V */
+	SW_1_575V,		/*!< 1.575 V */
+	SW_1_6V,		/*!< 1.600 V */
+	SW_1_625V,		/*!< 1.625 V */
+	SW_1_65V,		/*!< 1.650 V */
+	SW_1_675V,		/*!< 1.675 V */
+	SW_1_7V,		/*!< 1.700 V */
+	SW_1_8V = 36,		/*!< 1.800 V */
+	SW_1_85V = 40,		/*!< 1.850 V */
+	SW_2V = 44,		/*!< 2_000 V */
+	SW_2_1V = 48,		/*!< 2_100 V */
+	SW_2_2V = 52,		/*!< 2_200 V */
+} regulator_voltage_sw;
+
+/*!
+ * @enum regulator_voltage_violo
+ * @brief PMIC regulator VIOLO output voltage.
+ */
+enum {
+	VIOLO_1_2V = 0,		/*!< 1.2 V */
+	VIOLO_1_3V,		/*!< 1.3 V */
+	VIOLO_1_5V,		/*!< 1.5 V */
+	VIOLO_1_8V,		/*!< 1.8 V */
+} regulator_voltage_violo;
+
+/*!
+ * @enum regulator_voltage_vdig
+ * @brief PMIC regulator VDIG output voltage.
+ */
+enum {
+	VDIG_1_2V = 0,		/*!< 1.2 V */
+	VDIG_1_3V,		/*!< 1.3 V */
+	VDIG_1_5V,		/*!< 1.5 V */
+	VDIG_1_8V,		/*!< 1.8 V */
+} regulator_voltage_vdig;
+
+/*!
+ * @enum regulator_voltage_vgen
+ * @brief PMIC regulator VGEN output voltage.
+ */
+enum {
+	VGEN_1_2V = 0,		/*!< 1.2 V */
+	VGEN_1_3V,		/*!< 1.3 V */
+	VGEN_1_5V,		/*!< 1.5 V */
+	VGEN_1_8V,		/*!< 1.8 V */
+	VGEN_1_1V,		/*!< 1.1 V */
+	VGEN_2V,		/*!< 2 V */
+	VGEN_2_775V,		/*!< 2.775 V */
+	VGEN_2_4V,		/*!< 2.4 V */
+} regulator_voltage_vgen;
+
+/*!
+ * @enum regulator_voltage_vrfdig
+ * @brief PMIC regulator VRFDIG output voltage.
+ */
+enum {
+	VRFDIG_1_2V = 0,	/*!< 1.2 V */
+	VRFDIG_1_5V,		/*!< 1.5 V */
+	VRFDIG_1_8V,		/*!< 1.8 V */
+	VRFDIG_1_875V,		/*!< 1.875 V */
+} regulator_voltage_vrfdig;
+
+/*!
+ * @enum regulator_voltage_vrfref
+ * @brief PMIC regulator VRFREF output voltage.
+ */
+enum {
+	VRFREF_2_475V = 0,	/*!< 2.475 V */
+	VRFREF_2_6V,		/*!< 2.600 V */
+	VRFREF_2_7V,		/*!< 2.700 V */
+	VRFREF_2_775V,		/*!< 2.775 V */
+} regulator_voltage_vrfref;
+
+/*!
+ * @enum regulator_voltage_vrfcp
+ * @brief PMIC regulator VRFCP output voltage.
+ */
+enum {
+	VRFCP_2_7V = 0,		/*!< 2.700 V */
+	VRFCP_2_775V,		/*!< 2.775 V */
+} regulator_voltage_vrfcp;
+
+/*!
+ * @enum regulator_voltage_vsim
+ * @brief PMIC linear regulator VSIM output voltage.
+ */
+enum {
+	VSIM_1_8V = 0,		/*!< 1.8 V */
+	VSIM_2_9V,		/*!< 2.90 V */
+	VSIM_3V = 1,		/*!< 3 V */
+} regulator_voltage_vsim;
+
+/*!
+ * @enum regulator_voltage_vesim
+ * @brief PMIC regulator VESIM output voltage.
+ */
+enum {
+	VESIM_1_8V = 0,		/*!< 1.80 V */
+	VESIM_2_9V,		/*!< 2.90 V */
+} regulator_voltage_vesim;
+
+/*!
+ * @enum regulator_voltage_vcam
+ * @brief PMIC regulator VCAM output voltage.
+ */
+enum {
+	VCAM_1_5V = 0,		/*!< 1.50 V */
+	VCAM_1_8V,		/*!< 1.80 V */
+	VCAM_2_5V,		/*!< 2.50 V */
+	VCAM_2_55V,		/*!< 2.55 V */
+	VCAM_2_6V,		/*!< 2.60 V */
+	VCAM_2_75V,		/*!< 2.75 V */
+	VCAM_2_8V,		/*!< 2.80 V */
+	VCAM_3V,		/*!< 3.00 V */
+} regulator_voltage_vcam;
+
+/*!
+ * @enum regulator_voltage_vvib
+ * @brief PMIC linear regulator V_VIB output voltage.
+ */
+enum {
+	VVIB_1_3V = 0,		/*!< 1.30 V */
+	VVIB_1_8V,		/*!< 1.80 V */
+	VVIB_2V,		/*!< 2 V */
+	VVIB_3V,		/*!< 3 V */
+} regulator_voltage_vvib;
+
+/*!
+ * @enum regulator_voltage_vmmc
+ * @brief MC13783 PMIC regulator VMMC output voltage.
+ */
+enum {
+	VMMC_1_6V = 0,		/*!< 1.60 V */
+	VMMC_1_8V,		/*!< 1.80 V */
+	VMMC_2V,		/*!< 2.00 V */
+	VMMC_2_6V,		/*!< 2.60 V */
+	VMMC_2_7V,		/*!< 2.70 V */
+	VMMC_2_8V,		/*!< 2.80 V */
+	VMMC_2_9V,		/*!< 2.90 V */
+	VMMC_3V,		/*!< 3.00 V */
+} regulator_voltage_vmmc;
+
+/*!
+ * @enum regulator_voltage_vrf
+ * @brief PMIC regulator VRF output voltage.
+ */
+enum {
+	VRF_1_5V = 0,		/*!< 1.500 V */
+	VRF_1_875V,		/*!< 1.875 V */
+	VRF_2_7V,		/*!< 2.700 V */
+	VRF_2_775V,		/*!< 2.775 V */
+} regulator_voltage_vrf;
+
+/*!
+ * @enum regulator_voltage_sw3
+ * @brief PMIC Switch mode regulator SW3 output voltages.
+ */
+enum {
+	SW3_5V = 0,		/*!< 5.0 V */
+	SW3_5_5V = 3,		/*!< 5.5 V */
+} regulator_voltage_sw3;
+
+/*!
+     * The \b TPmicDVSTransitionSpeed enum defines the rate with which the
+     * voltage transition occurs.
+     */
+enum {
+	ESysDependent,
+	E25mVEach4us,
+	E25mVEach8us,
+	E25mvEach16us
+} DVS_transition_speed;
+
+/*
+ * Reg Regulator Mode 0
+ */
+#define VAUDIO_EN_LSH	0
+#define VAUDIO_EN_WID	1
+#define VAUDIO_EN_ENABLE	1
+#define VAUDIO_EN_DISABLE	0
+#define VIOHI_EN_LSH	3
+#define VIOHI_EN_WID	1
+#define VIOHI_EN_ENABLE	1
+#define VIOHI_EN_DISABLE	0
+#define VIOLO_EN_LSH	6
+#define VIOLO_EN_WID	1
+#define VIOLO_EN_ENABLE	1
+#define VIOLO_EN_DISABLE	0
+#define VDIG_EN_LSH	9
+#define VDIG_EN_WID	1
+#define VDIG_EN_ENABLE	1
+#define VDIG_EN_DISABLE	0
+#define VGEN_EN_LSH	12
+#define VGEN_EN_WID	1
+#define VGEN_EN_ENABLE	1
+#define VGEN_EN_DISABLE	0
+#define VRFDIG_EN_LSH	15
+#define VRFDIG_EN_WID	1
+#define VRFDIG_EN_ENABLE	1
+#define VRFDIG_EN_DISABLE	0
+#define VRFREF_EN_LSH	18
+#define VRFREF_EN_WID	1
+#define VRFREF_EN_ENABLE	1
+#define VRFREF_EN_DISABLE	0
+#define VRFCP_EN_LSH	21
+#define VRFCP_EN_WID	1
+#define VRFCP_EN_ENABLE	1
+#define VRFCP_EN_DISABLE	0
+
+/*
+ * Reg Regulator Mode 1
+ */
+#define VSIM_EN_LSH	0
+#define VSIM_EN_WID	1
+#define VSIM_EN_ENABLE	1
+#define VSIM_EN_DISABLE	0
+#define VESIM_EN_LSH	3
+#define VESIM_EN_WID	1
+#define VESIM_EN_ENABLE	1
+#define VESIM_EN_DISABLE	0
+#define VCAM_EN_LSH	6
+#define VCAM_EN_WID	1
+#define VCAM_EN_ENABLE	1
+#define VCAM_EN_DISABLE	0
+#define VRFBG_EN_LSH	9
+#define VRFBG_EN_WID	1
+#define VRFBG_EN_ENABLE	1
+#define VRFBG_EN_DISABLE	0
+#define VVIB_EN_LSH	11
+#define VVIB_EN_WID	1
+#define VVIB_EN_ENABLE	1
+#define VVIB_EN_DISABLE	0
+#define VRF1_EN_LSH	12
+#define VRF1_EN_WID	1
+#define VRF1_EN_ENABLE	1
+#define VRF1_EN_DISABLE	0
+#define VRF2_EN_LSH	15
+#define VRF2_EN_WID	1
+#define VRF2_EN_ENABLE	1
+#define VRF2_EN_DISABLE	0
+#define VMMC1_EN_LSH	18
+#define VMMC1_EN_WID	1
+#define VMMC1_EN_ENABLE	1
+#define VMMC1_EN_DISABLE	0
+#define VMMC2_EN_LSH	21
+#define VMMC2_EN_WID	1
+#define VMMC2_EN_ENABLE	1
+#define VMMC2_EN_DISABLE	0
+
+/*
+ * Reg Regulator Setting 0
+ */
+#define VIOLO_LSH		2
+#define VIOLO_WID		2
+#define VDIG_LSH		4
+#define VDIG_WID		2
+#define VGEN_LSH		6
+#define VGEN_WID		3
+#define VRFDIG_LSH		9
+#define VRFDIG_WID		2
+#define VRFREF_LSH		11
+#define VRFREF_WID		2
+#define VRFCP_LSH		13
+#define VRFCP_WID		1
+#define VSIM_LSH		14
+#define VSIM_WID		1
+#define VESIM_LSH		15
+#define VESIM_WID		1
+#define VCAM_LSH		16
+#define VCAM_WID		3
+
+/*
+ * Reg Regulator Setting 1
+ */
+#define VVIB_LSH		0
+#define VVIB_WID		2
+#define VRF1_LSH		2
+#define VRF1_WID		2
+#define VRF2_LSH		4
+#define VRF2_WID		2
+#define VMMC1_LSH		6
+#define VMMC1_WID		3
+#define VMMC2_LSH		9
+#define VMMC2_WID		3
+
+/*
+ * Reg Switcher 0
+ */
+#define SW1A_LSH		0
+#define SW1A_WID		6
+#define SW1A_DVS_LSH	6
+#define SW1A_DVS_WID	6
+#define SW1A_STDBY_LSH	12
+#define SW1A_STDBY_WID	6
+
+/*
+ * Reg Switcher 1
+ */
+#define SW1B_LSH		0
+#define SW1B_WID		6
+#define SW1B_DVS_LSH	6
+#define SW1B_DVS_WID	6
+#define SW1B_STDBY_LSH	12
+#define SW1B_STDBY_WID	6
+
+/*
+ * Reg Switcher 2
+ */
+#define SW2A_LSH		0
+#define SW2A_WID		6
+#define SW2A_DVS_LSH	6
+#define SW2A_DVS_WID	6
+#define SW2A_STDBY_LSH	12
+#define SW2A_STDBY_WID	6
+
+/*
+ * Reg Switcher 3
+ */
+#define SW2B_LSH		0
+#define SW2B_WID		6
+#define SW2B_DVS_LSH	6
+#define SW2B_DVS_WID	6
+#define SW2B_STDBY_LSH	12
+#define SW2B_STDBY_WID	6
+
+/*
+ * Reg Switcher 4
+ */
+#define SW1A_MODE_LSH		0
+#define SW1A_MODE_WID		2
+#define SW1A_STBY_MODE_LSH		2
+#define SW1A_STBY_MODE_WID		2
+#define SW1A_DVS_SPEED_LSH		6
+#define SW1A_DVS_SPEED_WID		2
+#define SW1B_MODE_LSH		10
+#define SW1B_MODE_WID		2
+#define SW1B_STBY_MODE_LSH		12
+#define SW1B_STBY_MODE_WID		2
+#define SW1B_DVS_SPEED_LSH		14
+#define SW1B_DVS_SPEED_WID		2
+
+/*
+ * Reg Switcher 5
+ */
+#define SW2A_MODE_LSH		0
+#define SW2A_MODE_WID		2
+#define SW2A_STBY_MODE_LSH		2
+#define SW2A_STBY_MODE_WID		2
+#define SW2A_DVS_SPEED_LSH		6
+#define SW2A_DVS_SPEED_WID		2
+#define SW2B_MODE_LSH		10
+#define SW2B_MODE_WID		2
+#define SW2B_STBY_MODE_LSH		12
+#define SW2B_STBY_MODE_WID		2
+#define SW2B_DVS_SPEED_LSH		14
+#define SW2B_DVS_SPEED_WID		2
+#define SW3_LSH			18
+#define SW3_WID			2
+#define SW3_EN_LSH			20
+#define SW3_EN_WID			2
+#define SW3_EN_ENABLE		1
+#define SW3_EN_DISABLE		0
+
+/*
+ * Reg Regulator Misc.
+ */
+#define GPO1_EN_LSH	6
+#define GPO1_EN_WID	1
+#define GPO1_EN_ENABLE	1
+#define GPO1_EN_DISABLE	0
+#define GPO2_EN_LSH	8
+#define GPO2_EN_WID	1
+#define GPO2_EN_ENABLE	1
+#define GPO2_EN_DISABLE	0
+#define GPO3_EN_LSH	10
+#define GPO3_EN_WID	1
+#define GPO3_EN_ENABLE	1
+#define GPO3_EN_DISABLE	0
+#define GPO4_EN_LSH	12
+#define GPO4_EN_WID	1
+#define GPO4_EN_ENABLE	1
+#define GPO4_EN_DISABLE	0
+
+/*
+ * Switcher mode configuration
+ */
+#define SW_MODE_SYNC_RECT_EN	0
+#define SW_MODE_PULSE_NO_SKIP_EN	1
+#define SW_MODE_PULSE_SKIP_EN	2
+#define SW_MODE_LOW_POWER_EN	3
+
+#define dvs_speed E25mvEach16us
+
+static int mc13783_vaudio_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VAUDIO_EN, VAUDIO_EN_ENABLE);
+	register_mask = BITFMASK(VAUDIO_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vaudio_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VAUDIO_EN, VAUDIO_EN_DISABLE);
+	register_mask = BITFMASK(VAUDIO_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_viohi_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VIOHI_EN, VIOHI_EN_ENABLE);
+	register_mask = BITFMASK(VIOHI_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_viohi_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VIOHI_EN, VIOHI_EN_DISABLE);
+	register_mask = BITFMASK(VIOHI_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_violo_set_voltage(struct regulator_dev *reg,
+				     int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0, register1 = 0;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1200) && (mV < 1300))
+		voltage = VIOLO_1_2V;
+	else if ((mV >= 1300) && (mV < 1500))
+		voltage = VIOLO_1_3V;
+	else if ((mV >= 1500) && (mV < 1800))
+		voltage = VIOLO_1_5V;
+	else
+		voltage = VIOLO_1_8V;
+
+	register_val = BITFVAL(VIOLO, voltage);
+	register_mask = BITFMASK(VIOLO);
+	register1 = REG_REGULATOR_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_violo_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_0,
+				 &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VIOLO);
+
+	switch (voltage) {
+	case VIOLO_1_2V:
+		mV = 1200;
+		break;
+	case VIOLO_1_3V:
+		mV = 1300;
+		break;
+	case VIOLO_1_5V:
+		mV = 1500;
+		break;
+	case VIOLO_1_8V:
+		mV = 1800;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_violo_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VIOLO_EN, VIOLO_EN_ENABLE);
+	register_mask = BITFMASK(VIOLO_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_violo_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VIOLO_EN, VIOLO_EN_DISABLE);
+	register_mask = BITFMASK(VIOLO_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vdig_set_voltage(struct regulator_dev *reg,
+				    int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1200) && (mV < 1300))
+		voltage = VDIG_1_2V;
+	else if ((mV >= 1300) && (mV < 1500))
+		voltage = VDIG_1_3V;
+	else if ((mV >= 1500) && (mV < 1800))
+		voltage = VDIG_1_5V;
+	else
+		voltage = VDIG_1_8V;
+
+	register_val = BITFVAL(VDIG, voltage);
+	register_mask = BITFMASK(VDIG);
+	register1 = REG_REGULATOR_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vdig_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_0,
+				  &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VDIG);
+
+	switch (voltage) {
+	case VDIG_1_2V:
+		mV = 1200;
+		break;
+	case VDIG_1_3V:
+		mV = 1300;
+		break;
+	case VDIG_1_5V:
+		mV = 1500;
+		break;
+	case VDIG_1_8V:
+		mV = 1800;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vdig_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VDIG_EN, VDIG_EN_ENABLE);
+	register_mask = BITFMASK(VDIG_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vdig_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VDIG_EN, VDIG_EN_DISABLE);
+	register_mask = BITFMASK(VDIG_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vgen_set_voltage(struct regulator_dev *reg,
+				    int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+	int vgenid = rdev_get_id(reg);
+
+	printk(KERN_INFO "VGEN ID is %d\n", vgenid);
+
+	if ((mV >= 1100) && (mV < 1200))
+		voltage = VGEN_1_1V;
+	else if ((mV >= 1200) && (mV < 1300))
+		voltage = VGEN_1_2V;
+	else if ((mV >= 1300) && (mV < 1500))
+		voltage = VGEN_1_3V;
+	else if ((mV >= 1500) && (mV < 1800))
+		voltage = VGEN_1_5V;
+	else if ((mV >= 1800) && (mV < 2000))
+		voltage = VGEN_1_8V;
+	else if ((mV >= 2000) && (mV < 2400))
+		voltage = VGEN_2V;
+	else if ((mV >= 2400) && (mV < 2775))
+		voltage = VGEN_2_4V;
+	else
+		voltage = VGEN_2_775V;
+
+	register_val = BITFVAL(VGEN, voltage);
+	register_mask = BITFMASK(VGEN);
+	register1 = REG_REGULATOR_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vgen_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_0,
+				  &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VGEN);
+
+	switch (voltage) {
+	case VGEN_1_2V:
+		mV = 1200;
+		break;
+	case VGEN_1_3V:
+		mV = 1300;
+		break;
+	case VGEN_1_5V:
+		mV = 1500;
+		break;
+	case VGEN_1_8V:
+		mV = 1800;
+		break;
+	case VGEN_1_1V:
+		mV = 1100;
+		break;
+	case VGEN_2V:
+		mV = 2000;
+		break;
+	case VGEN_2_775V:
+		mV = 2775;
+		break;
+	case VGEN_2_4V:
+		mV = 2400;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vgen_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VGEN_EN, VGEN_EN_ENABLE);
+	register_mask = BITFMASK(VGEN_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vgen_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VGEN_EN, VGEN_EN_DISABLE);
+	register_mask = BITFMASK(VGEN_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrfdig_set_voltage(struct regulator_dev *reg,
+				      int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1200) && (mV < 1500))
+		voltage = VRFDIG_1_2V;
+	else if ((mV >= 1500) && (mV < 1300))
+		voltage = VRFDIG_1_5V;
+	else if ((mV >= 1800) && (mV < 1875))
+		voltage = VRFDIG_1_8V;
+	else
+		voltage = VRFDIG_1_875V;
+
+	register_val = BITFVAL(VRFDIG, voltage);
+	register_mask = BITFMASK(VRFDIG);
+	register1 = REG_REGULATOR_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrfdig_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_0,
+				  &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VRFDIG);
+
+	switch (voltage) {
+	case VRFDIG_1_2V:
+		mV = 1200;
+		break;
+	case VRFDIG_1_5V:
+		mV = 1500;
+		break;
+	case VRFDIG_1_8V:
+		mV = 1800;
+		break;
+	case VRFDIG_1_875V:
+		mV = 1875;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vrfdig_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VRFDIG_EN, VRFDIG_EN_ENABLE);
+	register_mask = BITFMASK(VRFDIG_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrfdig_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VRFDIG_EN, VRFDIG_EN_DISABLE);
+	register_mask = BITFMASK(VRFDIG_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrfref_set_voltage(struct regulator_dev *reg,
+				      int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 2475) && (mV < 2600))
+		voltage = VRFREF_2_475V;
+	else if ((mV >= 2600) && (mV < 2700))
+		voltage = VRFREF_2_6V;
+	else if ((mV >= 2700) && (mV < 2775))
+		voltage = VRFREF_2_7V;
+	else
+		voltage = VRFREF_2_775V;
+
+	register_val = BITFVAL(VRFREF, voltage);
+	register_mask = BITFMASK(VRFREF);
+	register1 = REG_REGULATOR_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrfref_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_0,
+				  &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VRFREF);
+
+	switch (voltage) {
+	case VRFREF_2_475V:
+		mV = 2475;
+		break;
+	case VRFREF_2_6V:
+		mV = 2600;
+		break;
+	case VRFREF_2_7V:
+		mV = 2700;
+		break;
+	case VRFREF_2_775V:
+		mV = 2775;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vrfref_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VRFREF_EN, VRFREF_EN_ENABLE);
+	register_mask = BITFMASK(VRFREF_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrfref_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VRFREF_EN, VRFREF_EN_DISABLE);
+	register_mask = BITFMASK(VRFREF_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrfcp_set_voltage(struct regulator_dev *reg,
+				     int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 2700) && (mV < 2775))
+		voltage = VRFCP_2_7V;
+	else
+		voltage = VRFCP_2_775V;
+
+	register_val = BITFVAL(VRFCP, voltage);
+	register_mask = BITFMASK(VRFCP);
+	register1 = REG_REGULATOR_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrfcp_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_0,
+				  &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VRFCP);
+
+	switch (voltage) {
+	case VRFCP_2_7V:
+		mV = 2700;
+		break;
+	case VRFCP_2_775V:
+		mV = 2775;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vrfcp_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VRFCP_EN, VRFCP_EN_ENABLE);
+	register_mask = BITFMASK(VRFCP_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrfcp_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VRFCP_EN, VRFCP_EN_DISABLE);
+	register_mask = BITFMASK(VRFCP_EN);
+	register1 = REG_REGULATOR_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vsim_set_voltage(struct regulator_dev *reg,
+				    int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1800) && (mV < 2900))
+		voltage = VSIM_1_8V;
+	else
+		voltage = VSIM_2_9V;
+
+	register_val = BITFVAL(VSIM, voltage);
+	register_mask = BITFMASK(VSIM);
+	register1 = REG_REGULATOR_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vsim_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_0,
+				  &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VSIM);
+
+	switch (voltage) {
+	case VSIM_1_8V:
+		mV = 1800;
+		break;
+	case VSIM_2_9V:
+		mV = 1900;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vsim_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VSIM_EN, VSIM_EN_ENABLE);
+	register_mask = BITFMASK(VSIM_EN);
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vsim_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VSIM_EN, VSIM_EN_DISABLE);
+	register_mask = BITFMASK(VSIM_EN);
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vesim_set_voltage(struct regulator_dev *reg,
+				     int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1800) && (mV < 2900))
+		voltage = VESIM_1_8V;
+	else
+		voltage = VESIM_2_9V;
+
+	register_val = BITFVAL(VESIM, voltage);
+	register_mask = BITFMASK(VESIM);
+	register1 = REG_REGULATOR_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vesim_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_0,
+				  &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VESIM);
+
+	switch (voltage) {
+	case VESIM_1_8V:
+		mV = 1800;
+		break;
+	case VESIM_2_9V:
+		mV = 1900;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vesim_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VESIM_EN, VESIM_EN_ENABLE);
+	register_mask = BITFMASK(VESIM_EN);
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vesim_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VESIM_EN, VESIM_EN_DISABLE);
+	register_mask = BITFMASK(VESIM_EN);
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vcam_set_voltage(struct regulator_dev *reg,
+				    int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1500) && (mV < 1800))
+		voltage = VCAM_1_5V;
+	else if ((mV >= 1800) && (mV < 2500))
+		voltage = VCAM_1_8V;
+	else if ((mV >= 2500) && (mV < 2550))
+		voltage = VCAM_2_5V;
+	else if ((mV >= 2550) && (mV < 2600))
+		voltage = VCAM_2_55V;
+	if ((mV >= 2600) && (mV < 2750))
+		voltage = VCAM_2_6V;
+	else if ((mV >= 2750) && (mV < 2800))
+		voltage = VCAM_2_75V;
+	else if ((mV >= 2800) && (mV < 3000))
+		voltage = VCAM_2_8V;
+	else
+		voltage = VCAM_3V;
+
+	register_val = BITFVAL(VCAM, voltage);
+	register_mask = BITFMASK(VCAM);
+	register1 = REG_REGULATOR_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vcam_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_0,
+				  &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VCAM);
+
+	switch (voltage) {
+	case VCAM_1_5V:
+		mV = 1500;
+		break;
+	case VCAM_1_8V:
+		mV = 1800;
+		break;
+	case VCAM_2_5V:
+		mV = 2500;
+		break;
+	case VCAM_2_55V:
+		mV = 2550;
+		break;
+	case VCAM_2_6V:
+		mV = 2600;
+		break;
+	case VCAM_2_75V:
+		mV = 2750;
+		break;
+	case VCAM_2_8V:
+		mV = 2800;
+		break;
+	case VCAM_3V:
+		mV = 3000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vcam_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VCAM_EN, VCAM_EN_ENABLE);
+	register_mask = BITFMASK(VCAM_EN);
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vcam_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VCAM_EN, VCAM_EN_DISABLE);
+	register_mask = BITFMASK(VCAM_EN);
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vvib_set_voltage(struct regulator_dev *reg,
+				    int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1300) && (mV < 1800))
+		voltage = VVIB_1_3V;
+	else if ((mV >= 1800) && (mV < 2000))
+		voltage = VVIB_1_8V;
+	else if ((mV >= 2000) && (mV < 3000))
+		voltage = VVIB_2V;
+	else
+		voltage = VVIB_3V;
+
+	register_val = BITFVAL(VVIB, voltage);
+	register_mask = BITFMASK(VVIB);
+	register1 = REG_REGULATOR_SETTING_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vvib_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_1,
+				  &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VVIB);
+
+	switch (voltage) {
+	case VVIB_1_3V:
+		mV = 1300;
+		break;
+	case VVIB_1_8V:
+		mV = 1800;
+		break;
+	case VVIB_2V:
+		mV = 2000;
+		break;
+	case VVIB_3V:
+		mV = 3000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vvib_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VVIB_EN, VVIB_EN_ENABLE);
+	register_mask = BITFMASK(VVIB_EN);
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vvib_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VVIB_EN, VVIB_EN_DISABLE);
+	register_mask = BITFMASK(VVIB_EN);
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrf_set_voltage(struct regulator_dev *reg, int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, rf = rdev_get_id(reg), mV = uV / 1000;
+
+	if ((mV >= 1500) && (mV < 1875))
+		voltage = VRF_1_5V;
+	else if ((mV >= 1875) && (mV < 2700))
+		voltage = VRF_1_875V;
+	else if ((mV >= 2700) && (mV < 2775))
+		voltage = VRF_2_7V;
+	else
+		voltage = VRF_2_775V;
+
+	switch (rf) {
+	case MC13783_VRF1:
+		register_val = BITFVAL(VRF1, voltage);
+		register_mask = BITFMASK(VRF1);
+		break;
+	case MC13783_VRF2:
+		register_val = BITFVAL(VRF2, voltage);
+		register_mask = BITFMASK(VRF2);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	register1 = REG_REGULATOR_SETTING_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrf_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, rf = rdev_get_id(reg), mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_1,
+				  &register_val, PMIC_ALL_BITS));
+
+	switch (rf) {
+	case MC13783_VRF1:
+		voltage = BITFEXT(register_val, VRF1);
+		break;
+	case MC13783_VRF2:
+		voltage = BITFEXT(register_val, VRF2);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	switch (voltage) {
+	case VRF_1_5V:
+		mV = 1500;
+		break;
+	case VRF_1_875V:
+		mV = 1875;
+		break;
+	case VRF_2_7V:
+		mV = 2700;
+		break;
+	case VRF_2_775V:
+		mV = 2775;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vrf_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int vrf = rdev_get_id(reg);
+
+	switch (vrf) {
+	case MC13783_VRF1:
+		register_val = BITFVAL(VRF1_EN, VRF1_EN_ENABLE);
+		register_mask = BITFMASK(VRF1_EN);
+		break;
+	case MC13783_VRF2:
+		register_val = BITFVAL(VRF2_EN, VRF2_EN_ENABLE);
+		register_mask = BITFMASK(VRF2_EN);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vrf_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int vrf = rdev_get_id(reg);
+
+	switch (vrf) {
+	case MC13783_VRF1:
+		register_val = BITFVAL(VRF1_EN, VRF1_EN_DISABLE);
+		register_mask = BITFMASK(VRF1_EN);
+		break;
+	case MC13783_VRF2:
+		register_val = BITFVAL(VRF2_EN, VRF2_EN_DISABLE);
+		register_mask = BITFMASK(VRF2_EN);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vmmc_set_voltage(struct regulator_dev *reg,
+				    int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mmc = rdev_get_id(reg), mV = uV / 1000;
+
+	printk(KERN_INFO "VMMC ID is %d\n", mmc);
+
+	if ((mV >= 1600) && (mV < 1800))
+		voltage = VMMC_1_6V;
+	else if ((mV >= 1800) && (mV < 2000))
+		voltage = VMMC_1_8V;
+	else if ((mV >= 2000) && (mV < 2600))
+		voltage = VMMC_2V;
+	else if ((mV >= 2600) && (mV < 2700))
+		voltage = VMMC_2_6V;
+	else if ((mV >= 2700) && (mV < 2800))
+		voltage = VMMC_2_7V;
+	else if ((mV >= 2800) && (mV < 2900))
+		voltage = VMMC_2_8V;
+	else if ((mV >= 2900) && (mV < 3000))
+		voltage = VMMC_2_9V;
+	else
+		voltage = VMMC_3V;
+
+	switch (mmc) {
+	case MC13783_VMMC1:
+		register_val = BITFVAL(VMMC1, voltage);
+		register_mask = BITFMASK(VMMC1);
+		break;
+	case MC13783_VMMC2:
+		register_val = BITFVAL(VMMC2, voltage);
+		register_mask = BITFMASK(VMMC2);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	register1 = REG_REGULATOR_SETTING_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vmmc_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mmc = rdev_get_id(reg), mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_REGULATOR_SETTING_1,
+				  &register_val, PMIC_ALL_BITS));
+
+	switch (mmc) {
+	case MC13783_VMMC1:
+		voltage = BITFEXT(register_val, VMMC1);
+		break;
+	case MC13783_VMMC2:
+		voltage = BITFEXT(register_val, VMMC2);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (voltage) {
+	case VMMC_1_6V:
+		mV = 1600;
+		break;
+	case VMMC_1_8V:
+		mV = 1800;
+		break;
+	case VMMC_2V:
+		mV = 2000;
+		break;
+	case VMMC_2_6V:
+		mV = 2600;
+		break;
+	case VMMC_2_7V:
+		mV = 2700;
+		break;
+	case VMMC_2_8V:
+		mV = 2800;
+		break;
+	case VMMC_2_9V:
+		mV = 2900;
+		break;
+	case VMMC_3V:
+		mV = 3000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_vmmc_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int vmmc = rdev_get_id(reg);
+
+	switch (vmmc) {
+	case MC13783_VMMC1:
+		register_val = BITFVAL(VMMC1_EN, VMMC1_EN_ENABLE);
+		register_mask = BITFMASK(VMMC1_EN);
+		break;
+	case MC13783_VMMC2:
+		register_val = BITFVAL(VMMC2_EN, VMMC2_EN_ENABLE);
+		register_mask = BITFMASK(VMMC2_EN);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_vmmc_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int vmmc = rdev_get_id(reg);
+
+	switch (vmmc) {
+	case MC13783_VMMC1:
+		register_val = BITFVAL(VMMC1_EN, VMMC1_EN_DISABLE);
+		register_mask = BITFMASK(VMMC1_EN);
+		break;
+	case MC13783_VMMC2:
+		register_val = BITFVAL(VMMC2_EN, VMMC2_EN_DISABLE);
+		register_mask = BITFMASK(VMMC2_EN);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	register1 = REG_REGULATOR_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_gpo_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int gpo = rdev_get_id(reg);
+
+	switch (gpo) {
+	case MC13783_GPO1:
+		register_val = BITFVAL(GPO1_EN, GPO1_EN_ENABLE);
+		register_mask = BITFMASK(GPO1_EN);
+		break;
+	case MC13783_GPO2:
+		register_val = BITFVAL(GPO2_EN, GPO2_EN_ENABLE);
+		register_mask = BITFMASK(GPO2_EN);
+		break;
+	case MC13783_GPO3:
+		register_val = BITFVAL(GPO3_EN, GPO3_EN_ENABLE);
+		register_mask = BITFMASK(GPO3_EN);
+		break;
+	case MC13783_GPO4:
+		register_val = BITFVAL(GPO4_EN, GPO4_EN_ENABLE);
+		register_mask = BITFMASK(GPO4_EN);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	register1 = REG_POWER_MISCELLANEOUS;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_gpo_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int gpo = rdev_get_id(reg);
+
+	switch (gpo) {
+	case MC13783_GPO1:
+		register_val = BITFVAL(GPO1_EN, GPO1_EN_DISABLE);
+		register_mask = BITFMASK(GPO1_EN);
+		break;
+	case MC13783_GPO2:
+		register_val = BITFVAL(GPO2_EN, GPO2_EN_DISABLE);
+		register_mask = BITFMASK(GPO2_EN);
+		break;
+	case MC13783_GPO3:
+		register_val = BITFVAL(GPO3_EN, GPO3_EN_DISABLE);
+		register_mask = BITFMASK(GPO3_EN);
+		break;
+	case MC13783_GPO4:
+		register_val = BITFVAL(GPO4_EN, GPO4_EN_DISABLE);
+		register_mask = BITFMASK(GPO4_EN);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	register1 = REG_POWER_MISCELLANEOUS;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_sw3_set_voltage(struct regulator_dev *reg, int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0, register1 = 0;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 5000) && (mV < 5500))
+		voltage = SW3_5V;
+	else
+		voltage = SW3_5_5V;
+
+	register_val = BITFVAL(SW3, voltage);
+	register_mask = BITFMASK(SW3);
+	register1 = REG_SWITCHERS_5;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_sw3_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_5,
+				 &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, SW3);
+
+	if (voltage == SW3_5_5V)
+		mV = 5500;
+	else
+		mV = 5000;
+
+	return mV * 1000;
+}
+
+static int mc13783_sw3_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(SW3_EN, SW3_EN_ENABLE);
+	register_mask = BITFMASK(SW3_EN);
+	register1 = REG_SWITCHERS_5;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_sw3_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(SW3_EN, SW3_EN_DISABLE);
+	register_mask = BITFMASK(SW3_EN);
+	register1 = REG_SWITCHERS_5;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_sw_set_normal_voltage(struct regulator_dev *reg,
+					 int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1 = 0;
+	int voltage, sw = rdev_get_id(reg), mV = uV / 1000;
+
+	if ((mV >= 900) && (mV < 925))
+		voltage = SW_0_9V;
+	else if ((mV >= 925) && (mV < 950))
+		voltage = SW_0_925V;
+	else if ((mV >= 950) && (mV < 975))
+		voltage = SW_0_95V;
+	else if ((mV >= 975) && (mV < 1000))
+		voltage = SW_0_975V;
+	else if ((mV >= 1000) && (mV < 1025))
+		voltage = SW_1V;
+	else if ((mV >= 1025) && (mV < 1050))
+		voltage = SW_1_025V;
+	else if ((mV >= 1050) && (mV < 1075))
+		voltage = SW_1_05V;
+	else if ((mV >= 1075) && (mV < 1100))
+		voltage = SW_1_075V;
+	else if ((mV >= 1100) && (mV < 1125))
+		voltage = SW_1_1V;
+	else if ((mV >= 1125) && (mV < 1150))
+		voltage = SW_1_125V;
+	else if ((mV >= 1150) && (mV < 1175))
+		voltage = SW_1_15V;
+	else if ((mV >= 1175) && (mV < 1200))
+		voltage = SW_1_175V;
+	else if ((mV >= 1200) && (mV < 1225))
+		voltage = SW_1_2V;
+	else if ((mV >= 1225) && (mV < 1250))
+		voltage = SW_1_225V;
+	else if ((mV >= 1250) && (mV < 1275))
+		voltage = SW_1_25V;
+	else if ((mV >= 1275) && (mV < 1300))
+		voltage = SW_1_275V;
+	else if ((mV >= 1300) && (mV < 1325))
+		voltage = SW_1_3V;
+	else if ((mV >= 1325) && (mV < 1350))
+		voltage = SW_1_325V;
+	else if ((mV >= 1350) && (mV < 1375))
+		voltage = SW_1_35V;
+	else if ((mV >= 1375) && (mV < 1400))
+		voltage = SW_1_375V;
+	else if ((mV >= 1400) && (mV < 1425))
+		voltage = SW_1_4V;
+	else if ((mV >= 1425) && (mV < 1450))
+		voltage = SW_1_425V;
+	else if ((mV >= 1450) && (mV < 1475))
+		voltage = SW_1_45V;
+	else if ((mV >= 1475) && (mV < 1500))
+		voltage = SW_1_475V;
+	else if ((mV >= 1500) && (mV < 1525))
+		voltage = SW_1_5V;
+	else if ((mV >= 1525) && (mV < 1550))
+		voltage = SW_1_525V;
+	else if ((mV >= 1550) && (mV < 1575))
+		voltage = SW_1_55V;
+	else if ((mV >= 1575) && (mV < 1600))
+		voltage = SW_1_575V;
+	else if ((mV >= 1600) && (mV < 1625))
+		voltage = SW_1_6V;
+	else if ((mV >= 1625) && (mV < 1650))
+		voltage = SW_1_625V;
+	else if ((mV >= 1650) && (mV < 1675))
+		voltage = SW_1_65V;
+	else if ((mV >= 1675) && (mV < 1700))
+		voltage = SW_1_675V;
+	else if ((mV >= 1700) && (mV < 1800))
+		voltage = SW_1_7V;
+	else if ((mV >= 1800) && (mV < 1850))
+		voltage = SW_1_8V;
+	else if ((mV >= 1850) && (mV < 2000))
+		voltage = SW_1_85V;
+	else if ((mV >= 2000) && (mV < 2100))
+		voltage = SW_2V;
+	else if ((mV >= 2100) && (mV < 2200))
+		voltage = SW_2_1V;
+	else
+		voltage = SW_2_2V;
+
+	switch (sw) {
+	case MC13783_SW1A:
+		register1 = REG_SWITCHERS_0;
+		register_val = BITFVAL(SW1A, voltage);
+		register_mask = BITFMASK(SW1A);
+		break;
+	case MC13783_SW1B:
+		register1 = REG_SWITCHERS_1;
+		register_val = BITFVAL(SW1B, voltage);
+		register_mask = BITFMASK(SW1B);
+		break;
+	case MC13783_SW2A:
+		register1 = REG_SWITCHERS_2;
+		register_val = BITFVAL(SW2A, voltage);
+		register_mask = BITFMASK(SW2A);
+		break;
+	case MC13783_SW2B:
+		register1 = REG_SWITCHERS_3;
+		register_val = BITFVAL(SW2B, voltage);
+		register_mask = BITFMASK(SW2B);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_sw_get_normal_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0, sw = rdev_get_id(reg);
+
+	switch (sw) {
+	case MC13783_SW1A:
+		CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_0,
+					  &register_val, PMIC_ALL_BITS));
+		voltage = BITFEXT(register_val, SW1A);
+		break;
+	case MC13783_SW1B:
+		CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_1,
+					  &register_val, PMIC_ALL_BITS));
+		voltage = BITFEXT(register_val, SW1B);
+		break;
+	case MC13783_SW2A:
+		CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_2,
+					  &register_val, PMIC_ALL_BITS));
+		voltage = BITFEXT(register_val, SW2A);
+		break;
+	case MC13783_SW2B:
+		CHECK_ERROR(pmic_read_reg(REG_SWITCHERS_3,
+					  &register_val, PMIC_ALL_BITS));
+		voltage = BITFEXT(register_val, SW2B);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (voltage) {
+	case SW_0_9V:
+		mV = 900;
+		break;
+	case SW_0_925V:
+		mV = 925;
+		break;
+	case SW_0_95V:
+		mV = 950;
+		break;
+	case SW_0_975V:
+		mV = 975;
+		break;
+	case SW_1V:
+		mV = 1000;
+		break;
+	case SW_1_025V:
+		mV = 1025;
+		break;
+	case SW_1_05V:
+		mV = 1050;
+		break;
+	case SW_1_075V:
+		mV = 1075;
+		break;
+	case SW_1_1V:
+		mV = 1100;
+		break;
+	case SW_1_125V:
+		mV = 1125;
+		break;
+	case SW_1_15V:
+		mV = 1150;
+		break;
+	case SW_1_175V:
+		mV = 1175;
+		break;
+	case SW_1_2V:
+		mV = 1200;
+		break;
+	case SW_1_225V:
+		mV = 1225;
+		break;
+	case SW_1_25V:
+		mV = 1250;
+		break;
+	case SW_1_275V:
+		mV = 1275;
+		break;
+	case SW_1_3V:
+		mV = 1300;
+		break;
+	case SW_1_325V:
+		mV = 1325;
+		break;
+	case SW_1_35V:
+		mV = 1350;
+		break;
+	case SW_1_375V:
+		mV = 1375;
+		break;
+	case SW_1_4V:
+		mV = 1400;
+		break;
+	case SW_1_425V:
+		mV = 1425;
+		break;
+	case SW_1_45V:
+		mV = 1450;
+		break;
+	case SW_1_475V:
+		mV = 1475;
+		break;
+	case SW_1_5V:
+		mV = 1500;
+		break;
+	case SW_1_525V:
+		mV = 1525;
+		break;
+	case SW_1_55V:
+		mV = 1550;
+		break;
+	case SW_1_575V:
+		mV = 1575;
+		break;
+	case SW_1_6V:
+		mV = 1600;
+		break;
+	case SW_1_625V:
+		mV = 1625;
+		break;
+	case SW_1_65V:
+		mV = 1650;
+		break;
+	case SW_1_675V:
+		mV = 1675;
+		break;
+	case SW_1_7V:
+		mV = 1700;
+		break;
+	case SW_1_8V:
+		mV = 1800;
+		break;
+	case SW_1_85V:
+		mV = 1850;
+		break;
+	case SW_2V:
+		mV = 2000;
+		break;
+	case SW_2_1V:
+		mV = 2100;
+		break;
+	case SW_2_2V:
+		mV = 2200;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13783_sw_normal_enable(struct regulator_dev *reg)
+{
+	return 0;
+}
+
+static int mc13783_sw_normal_disable(struct regulator_dev *reg)
+{
+	return 0;
+}
+
+static int mc13783_sw_stby_enable(struct regulator_dev *reg)
+{
+	return 0;
+}
+
+static int mc13783_sw_stby_disable(struct regulator_dev *reg)
+{
+	return 0;
+}
+
+static int mc13783_sw_set_stby_voltage(struct regulator_dev *reg, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1 = 0;
+	int voltage, sw = rdev_get_id(reg), mV = uV / 1000;
+
+	if ((mV >= 900) && (mV < 925))
+		voltage = SW_0_9V;
+	else if ((mV >= 925) && (mV < 950))
+		voltage = SW_0_925V;
+	else if ((mV >= 950) && (mV < 975))
+		voltage = SW_0_95V;
+	else if ((mV >= 975) && (mV < 1000))
+		voltage = SW_0_975V;
+	else if ((mV >= 1000) && (mV < 1025))
+		voltage = SW_1V;
+	else if ((mV >= 1025) && (mV < 1050))
+		voltage = SW_1_025V;
+	else if ((mV >= 1050) && (mV < 1075))
+		voltage = SW_1_05V;
+	else if ((mV >= 1075) && (mV < 1100))
+		voltage = SW_1_075V;
+	else if ((mV >= 1100) && (mV < 1125))
+		voltage = SW_1_1V;
+	else if ((mV >= 1125) && (mV < 1150))
+		voltage = SW_1_125V;
+	else if ((mV >= 1150) && (mV < 1175))
+		voltage = SW_1_15V;
+	else if ((mV >= 1175) && (mV < 1200))
+		voltage = SW_1_175V;
+	else if ((mV >= 1200) && (mV < 1225))
+		voltage = SW_1_2V;
+	else if ((mV >= 1225) && (mV < 1250))
+		voltage = SW_1_225V;
+	else if ((mV >= 1250) && (mV < 1275))
+		voltage = SW_1_25V;
+	else if ((mV >= 1275) && (mV < 1300))
+		voltage = SW_1_275V;
+	else if ((mV >= 1300) && (mV < 1325))
+		voltage = SW_1_3V;
+	else if ((mV >= 1325) && (mV < 1350))
+		voltage = SW_1_325V;
+	else if ((mV >= 1350) && (mV < 1375))
+		voltage = SW_1_35V;
+	else if ((mV >= 1375) && (mV < 1400))
+		voltage = SW_1_375V;
+	else if ((mV >= 1400) && (mV < 1425))
+		voltage = SW_1_4V;
+	else if ((mV >= 1425) && (mV < 1450))
+		voltage = SW_1_425V;
+	else if ((mV >= 1450) && (mV < 1475))
+		voltage = SW_1_45V;
+	else if ((mV >= 1475) && (mV < 1500))
+		voltage = SW_1_475V;
+	else if ((mV >= 1500) && (mV < 1525))
+		voltage = SW_1_5V;
+	else if ((mV >= 1525) && (mV < 1550))
+		voltage = SW_1_525V;
+	else if ((mV >= 1550) && (mV < 1575))
+		voltage = SW_1_55V;
+	else if ((mV >= 1575) && (mV < 1600))
+		voltage = SW_1_575V;
+	else if ((mV >= 1600) && (mV < 1625))
+		voltage = SW_1_6V;
+	else if ((mV >= 1625) && (mV < 1650))
+		voltage = SW_1_625V;
+	else if ((mV >= 1650) && (mV < 1675))
+		voltage = SW_1_65V;
+	else if ((mV >= 1675) && (mV < 1700))
+		voltage = SW_1_675V;
+	else if ((mV >= 1700) && (mV < 1800))
+		voltage = SW_1_7V;
+	else if ((mV >= 1800) && (mV < 1850))
+		voltage = SW_1_8V;
+	else if ((mV >= 1850) && (mV < 2000))
+		voltage = SW_1_85V;
+	else if ((mV >= 2000) && (mV < 2100))
+		voltage = SW_2V;
+	else if ((mV >= 2100) && (mV < 2200))
+		voltage = SW_2_1V;
+	else
+		voltage = SW_2_2V;
+
+	switch (sw) {
+	case MC13783_SW1A:
+		register1 = REG_SWITCHERS_0;
+		register_val = BITFVAL(SW1A_STDBY, voltage);
+		register_mask = BITFMASK(SW1A_STDBY);
+		break;
+	case MC13783_SW1B:
+		register1 = REG_SWITCHERS_1;
+		register_val = BITFVAL(SW1B_STDBY, voltage);
+		register_mask = BITFMASK(SW1B_STDBY);
+		break;
+	case MC13783_SW2A:
+		register1 = REG_SWITCHERS_2;
+		register_val = BITFVAL(SW2A_STDBY, voltage);
+		register_mask = BITFMASK(SW2A_STDBY);
+		break;
+	case MC13783_SW2B:
+		register1 = REG_SWITCHERS_3;
+		register_val = BITFVAL(SW2B_STDBY, voltage);
+		register_mask = BITFMASK(SW2B_STDBY);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13783_sw_set_normal_mode(struct regulator_dev *reg,
+				      unsigned int mode)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int register1 = 0;
+	unsigned int l_mode;
+	int sw = rdev_get_id(reg);
+
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		/* SYNC RECT mode */
+		l_mode = SW_MODE_SYNC_RECT_EN;
+		break;
+	case REGULATOR_MODE_NORMAL:
+		/* PULSE SKIP mode */
+		l_mode = SW_MODE_PULSE_SKIP_EN;
+		break;
+	case REGULATOR_MODE_IDLE:
+		/* LOW POWER mode */
+		l_mode = SW_MODE_LOW_POWER_EN;
+		break;
+	case REGULATOR_MODE_STANDBY:
+		/* NO PULSE SKIP mode */
+		l_mode = SW_MODE_PULSE_NO_SKIP_EN;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (sw) {
+	case MC13783_SW1A:
+		reg_val = BITFVAL(SW1A_MODE, l_mode);
+		reg_mask = BITFMASK(SW1A_MODE);
+		register1 = REG_SWITCHERS_4;
+		break;
+	case MC13783_SW1B:
+		reg_val = BITFVAL(SW1B_MODE, l_mode);
+		reg_mask = BITFMASK(SW1B_MODE);
+		register1 = REG_SWITCHERS_4;
+		break;
+	case MC13783_SW2A:
+		reg_val = BITFVAL(SW2A_MODE, l_mode);
+		reg_mask = BITFMASK(SW2A_MODE);
+		register1 = REG_SWITCHERS_5;
+		break;
+	case MC13783_SW2B:
+		reg_val = BITFVAL(SW2B_MODE, l_mode);
+		reg_mask = BITFMASK(SW2B_MODE);
+		register1 = REG_SWITCHERS_5;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return pmic_write_reg(register1, reg_val, reg_mask);
+}
+
+static unsigned int mc13783_sw_get_normal_mode(struct regulator_dev *reg)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int register1 = 0;
+	unsigned int l_mode = 0;
+	int sw = rdev_get_id(reg);
+	int ret = 0;
+
+	switch (sw) {
+	case MC13783_SW1A:
+		reg_mask = BITFMASK(SW1A_MODE);
+		register1 = REG_SWITCHERS_4;
+		break;
+	case MC13783_SW1B:
+		reg_mask = BITFMASK(SW1B_MODE);
+		register1 = REG_SWITCHERS_4;
+		break;
+	case MC13783_SW2A:
+		reg_mask = BITFMASK(SW2A_MODE);
+		register1 = REG_SWITCHERS_5;
+		break;
+	case MC13783_SW2B:
+		reg_mask = BITFMASK(SW2B_MODE);
+		register1 = REG_SWITCHERS_5;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = pmic_read_reg(register1, &reg_val, reg_mask);
+	if (ret != 0)
+		return ret;
+
+	switch (sw) {
+	case MC13783_SW1A:
+		l_mode = BITFEXT(reg_val, SW1A_MODE);
+		break;
+	case MC13783_SW1B:
+		l_mode = BITFEXT(reg_val, SW1B_MODE);
+		break;
+	case MC13783_SW2A:
+		l_mode = BITFEXT(reg_val, SW2A_MODE);
+		break;
+	case MC13783_SW2B:
+		l_mode = BITFEXT(reg_val, SW2B_MODE);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (l_mode == SW_MODE_SYNC_RECT_EN) {
+		return REGULATOR_MODE_FAST;
+	} else if (l_mode == SW_MODE_PULSE_NO_SKIP_EN) {
+		return REGULATOR_MODE_STANDBY;
+	} else if (l_mode == SW_MODE_PULSE_SKIP_EN) {
+		return REGULATOR_MODE_NORMAL;
+	} else if (l_mode == SW_MODE_LOW_POWER_EN) {
+		return REGULATOR_MODE_IDLE;
+	} else {
+		return -EINVAL;
+	}
+}
+
+static int mc13783_sw_set_stby_mode(struct regulator_dev *reg,
+				    unsigned int mode)
+{
+	unsigned int reg_val = 0, reg_mask = 0;
+	unsigned int register1 = 0;
+	unsigned int l_mode;
+	int sw = rdev_get_id(reg);
+
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		/* SYNC RECT mode */
+		l_mode = SW_MODE_SYNC_RECT_EN;
+		break;
+	case REGULATOR_MODE_NORMAL:
+		/* PULSE SKIP mode */
+		l_mode = SW_MODE_PULSE_SKIP_EN;
+		break;
+	case REGULATOR_MODE_IDLE:
+		/* LOW POWER mode */
+		l_mode = SW_MODE_LOW_POWER_EN;
+		break;
+	case REGULATOR_MODE_STANDBY:
+		/* NO PULSE SKIP mode */
+		l_mode = SW_MODE_PULSE_NO_SKIP_EN;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (sw) {
+	case MC13783_SW1A:
+		reg_val = BITFVAL(SW1A_STBY_MODE, l_mode);
+		reg_mask = BITFMASK(SW1A_STBY_MODE);
+		register1 = REG_SWITCHERS_4;
+		break;
+	case MC13783_SW1B:
+		reg_val = BITFVAL(SW1B_STBY_MODE, l_mode);
+		reg_mask = BITFMASK(SW1B_STBY_MODE);
+		register1 = REG_SWITCHERS_4;
+		break;
+	case MC13783_SW2A:
+		reg_val = BITFVAL(SW2A_STBY_MODE, l_mode);
+		reg_mask = BITFMASK(SW2A_STBY_MODE);
+		register1 = REG_SWITCHERS_5;
+		break;
+	case MC13783_SW2B:
+		reg_val = BITFVAL(SW2B_STBY_MODE, l_mode);
+		reg_mask = BITFMASK(SW2B_STBY_MODE);
+		register1 = REG_SWITCHERS_5;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return pmic_write_reg(register1, reg_val, reg_mask);
+}
+
+static struct regulator_ops mc13783_vaudio_ops = {
+	.enable = mc13783_vaudio_enable,
+	.disable = mc13783_vaudio_disable,
+};
+
+static struct regulator_ops mc13783_viohi_ops = {
+	.enable = mc13783_viohi_enable,
+	.disable = mc13783_viohi_disable,
+};
+
+static struct regulator_ops mc13783_violo_ops = {
+	.set_voltage = mc13783_violo_set_voltage,
+	.get_voltage = mc13783_violo_get_voltage,
+	.enable = mc13783_violo_enable,
+	.disable = mc13783_violo_disable,
+};
+
+static struct regulator_ops mc13783_vdig_ops = {
+	.set_voltage = mc13783_vdig_set_voltage,
+	.get_voltage = mc13783_vdig_get_voltage,
+	.enable = mc13783_vdig_enable,
+	.disable = mc13783_vdig_disable,
+};
+
+static struct regulator_ops mc13783_vgen_ops = {
+	.set_voltage = mc13783_vgen_set_voltage,
+	.get_voltage = mc13783_vgen_get_voltage,
+	.enable = mc13783_vgen_enable,
+	.disable = mc13783_vgen_disable,
+};
+
+static struct regulator_ops mc13783_vrfdig_ops = {
+	.set_voltage = mc13783_vrfdig_set_voltage,
+	.get_voltage = mc13783_vrfdig_get_voltage,
+	.enable = mc13783_vrfdig_enable,
+	.disable = mc13783_vrfdig_disable,
+};
+
+static struct regulator_ops mc13783_vrfref_ops = {
+	.set_voltage = mc13783_vrfref_set_voltage,
+	.get_voltage = mc13783_vrfref_get_voltage,
+	.enable = mc13783_vrfref_enable,
+	.disable = mc13783_vrfref_disable,
+};
+
+static struct regulator_ops mc13783_vrfcp_ops = {
+	.set_voltage = mc13783_vrfcp_set_voltage,
+	.get_voltage = mc13783_vrfcp_get_voltage,
+	.enable = mc13783_vrfcp_enable,
+	.disable = mc13783_vrfcp_disable,
+};
+
+static struct regulator_ops mc13783_vsim_ops = {
+	.set_voltage = mc13783_vsim_set_voltage,
+	.get_voltage = mc13783_vsim_get_voltage,
+	.enable = mc13783_vsim_enable,
+	.disable = mc13783_vsim_disable,
+};
+
+static struct regulator_ops mc13783_vesim_ops = {
+	.set_voltage = mc13783_vesim_set_voltage,
+	.get_voltage = mc13783_vesim_get_voltage,
+	.enable = mc13783_vesim_enable,
+	.disable = mc13783_vesim_disable,
+};
+
+static struct regulator_ops mc13783_vcam_ops = {
+	.set_voltage = mc13783_vcam_set_voltage,
+	.get_voltage = mc13783_vcam_get_voltage,
+	.enable = mc13783_vcam_enable,
+	.disable = mc13783_vcam_disable,
+};
+
+static struct regulator_ops mc13783_vvib_ops = {
+	.set_voltage = mc13783_vvib_set_voltage,
+	.get_voltage = mc13783_vvib_get_voltage,
+	.enable = mc13783_vvib_enable,
+	.disable = mc13783_vvib_disable,
+};
+
+static struct regulator_ops mc13783_vrf_ops = {
+	.set_voltage = mc13783_vrf_set_voltage,
+	.get_voltage = mc13783_vrf_get_voltage,
+	.enable = mc13783_vrf_enable,
+	.disable = mc13783_vrf_disable,
+};
+
+static struct regulator_ops mc13783_vmmc_ops = {
+	.set_voltage = mc13783_vmmc_set_voltage,
+	.get_voltage = mc13783_vmmc_get_voltage,
+	.enable = mc13783_vmmc_enable,
+	.disable = mc13783_vmmc_disable,
+};
+
+static struct regulator_ops mc13783_gpo_ops = {
+	.enable = mc13783_gpo_enable,
+	.disable = mc13783_gpo_disable,
+};
+
+static struct regulator_ops mc13783_sw3_ops = {
+	.set_voltage = mc13783_sw3_set_voltage,
+	.get_voltage = mc13783_sw3_get_voltage,
+	.enable = mc13783_sw3_enable,
+	.disable = mc13783_sw3_disable,
+};
+
+static struct regulator_ops mc13783_sw1_ops = {
+	.set_voltage = mc13783_sw_set_normal_voltage,
+	.get_voltage = mc13783_sw_get_normal_voltage,
+	.get_mode = mc13783_sw_get_normal_mode,
+	.set_mode = mc13783_sw_set_normal_mode,
+	.set_suspend_voltage = mc13783_sw_set_stby_voltage,
+	.set_suspend_enable = mc13783_sw_stby_enable,
+	.set_suspend_disable = mc13783_sw_stby_disable,
+	.set_suspend_mode = mc13783_sw_set_stby_mode,
+};
+
+static struct regulator_ops mc13783_sw_normal_ops = {
+	.set_voltage = mc13783_sw_set_normal_voltage,
+	.get_voltage = mc13783_sw_get_normal_voltage,
+	.get_mode = mc13783_sw_get_normal_mode,
+	.set_mode = mc13783_sw_set_normal_mode,
+	.enable = mc13783_sw_normal_enable,
+	.disable = mc13783_sw_normal_disable,
+};
+
+static struct regulator_desc reg_mc13783[] = {
+	{
+		.name = "SW1A",
+		.id = MC13783_SW1A,
+		.ops = &mc13783_sw1_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "SW1B",
+		.id = MC13783_SW1B,
+		.ops = &mc13783_sw_normal_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "SW2A",
+		.id = MC13783_SW2A,
+		.ops = &mc13783_sw_normal_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "SW2B",
+		.id = MC13783_SW2B,
+		.ops = &mc13783_sw_normal_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "SW3",
+		.id = MC13783_SW3,
+		.ops = &mc13783_sw3_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VAUDIO",
+		.id = MC13783_VAUDIO,
+		.ops = &mc13783_vaudio_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VIOHI",
+		.id = MC13783_VIOHI,
+		.ops = &mc13783_viohi_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VIOLO",
+		.id = MC13783_VIOLO,
+		.ops = &mc13783_violo_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VDIG",
+		.id = MC13783_VDIG,
+		.ops = &mc13783_vdig_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VGEN",
+		.id = MC13783_VGEN,
+		.ops = &mc13783_vgen_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VRFDIG",
+		.id = MC13783_VRFDIG,
+		.ops = &mc13783_vrfdig_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VRFREF",
+		.id = MC13783_VRFREF,
+		.ops = &mc13783_vrfref_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VRFCP",
+		.id = MC13783_VRFCP,
+		.ops = &mc13783_vrfcp_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VSIM",
+		.id = MC13783_VSIM,
+		.ops = &mc13783_vsim_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VESIM",
+		.id = MC13783_VESIM,
+		.ops = &mc13783_vesim_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VCAM",
+		.id = MC13783_VCAM,
+		.ops = &mc13783_vcam_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VRFBG",
+		.id = MC13783_VRFBG,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VVIB",
+		.id = MC13783_VVIB,
+		.ops = &mc13783_vvib_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VRF1",
+		.id = MC13783_VRF1,
+		.ops = &mc13783_vrf_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VRF2",
+		.id = MC13783_VRF2,
+		.ops = &mc13783_vrf_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VMMC1",
+		.id = MC13783_VMMC1,
+		.ops = &mc13783_vmmc_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VMMC2",
+		.id = MC13783_VMMC2,
+		.ops = &mc13783_vmmc_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "GPO1",
+		.id = MC13783_GPO1,
+		.ops = &mc13783_gpo_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "GPO2",
+		.id = MC13783_GPO2,
+		.ops = &mc13783_gpo_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "GPO3",
+		.id = MC13783_GPO3,
+		.ops = &mc13783_gpo_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "GPO4",
+		.id = MC13783_GPO4,
+		.ops = &mc13783_gpo_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+};
+
+/*
+ * Init and Exit
+ */
+
+static int reg_mc13783_probe(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev;
+
+	/* register regulator */
+	rdev = regulator_register(&reg_mc13783[pdev->id], &pdev->dev,
+				  dev_get_drvdata(&pdev->dev));
+	if (IS_ERR(rdev)) {
+		dev_err(&pdev->dev, "failed to register %s\n",
+			reg_mc13783[pdev->id].name);
+		return PTR_ERR(rdev);
+	}
+
+	return 0;
+}
+
+static int mc13783_regulator_remove(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+
+	regulator_unregister(rdev);
+
+	return 0;
+}
+
+int mc13783_register_regulator(struct mc13783 *mc13783, int reg,
+			       struct regulator_init_data *initdata)
+{
+	struct platform_device *pdev;
+	int ret;
+
+	if (mc13783->pmic.pdev[reg])
+		return -EBUSY;
+
+	pdev = platform_device_alloc("mc13783-regulatr", reg);
+	if (!pdev)
+		return -ENOMEM;
+
+	mc13783->pmic.pdev[reg] = pdev;
+
+	initdata->driver_data = mc13783;
+
+	pdev->dev.platform_data = initdata;
+	pdev->dev.parent = mc13783->dev;
+	platform_set_drvdata(pdev, mc13783);
+	ret = platform_device_add(pdev);
+
+	if (ret != 0) {
+		dev_err(mc13783->dev, "Failed to register regulator %d: %d\n",
+			reg, ret);
+		platform_device_del(pdev);
+		mc13783->pmic.pdev[reg] = NULL;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mc13783_register_regulator);
+
+static struct platform_driver mc13783_regulator_driver = {
+	.probe = reg_mc13783_probe,
+	.remove = mc13783_regulator_remove,
+	.driver		= {
+		.name	= "mc13783-regulatr",
+			/* o left out due to string length */
+	},
+};
+
+static int __init mc13783_regulator_subsys_init(void)
+{
+	return platform_driver_register(&mc13783_regulator_driver);
+}
+subsys_initcall(mc13783_regulator_subsys_init);
+
+static void __exit mc13783_regulator_exit(void)
+{
+	platform_driver_unregister(&mc13783_regulator_driver);
+}
+module_exit(mc13783_regulator_exit);
+
+
+/* Module information */
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MC13783 Regulator driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/reg-mc13892.c b/drivers/regulator/reg-mc13892.c
new file mode 100644
index 000000000000..70b0d529a94d
--- /dev/null
+++ b/drivers/regulator/reg-mc13892.c
@@ -0,0 +1,1849 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/driver.h>
+#include <linux/mfd/mc13892/core.h>
+#include <linux/platform_device.h>
+#include <linux/pmic_status.h>
+#include <linux/pmic_external.h>
+
+/*
+ * Convenience conversion.
+ * Here atm, maybe there is somewhere better for this.
+ */
+#define mV_to_uV(mV) (mV * 1000)
+#define uV_to_mV(uV) (uV / 1000)
+#define V_to_uV(V) (mV_to_uV(V * 1000))
+#define uV_to_V(uV) (uV_to_mV(uV) / 1000)
+
+enum {
+	VDIG_1_05V = 0,
+	VDIG_1_25V,
+	VDIG_1_65V,
+	VDIG_1_80V,
+} regulator_voltage_vdig;
+
+enum {
+	VPLL_1_05V = 0,
+	VPLL_1_25V,
+	VPLL_1_65V,
+	VPLL_1_80V,
+} regulator_voltage_vpll;
+
+enum {
+	VGEN1_1_2V = 0,
+	VGEN1_1_5V,
+	VGEN1_2_775V,
+	VGEN1_3_15V,
+} regulator_voltage_vgen1;
+
+enum {
+	VGEN2_1_2V = 0,
+	VGEN2_1_5V,
+	VGEN2_1_6V,
+	VGEN2_1_8V,
+	VGEN2_2_7V,
+	VGEN2_2_8V,
+	VGEN2_3_0V,
+	VGEN2_3_15V,
+} regulator_voltage_vgen2;
+
+enum {
+	VGEN3_1_8V = 0,
+	VGEN3_2_9V,
+} regulator_voltage_vgen3;
+
+enum {
+	VSD_1_8V = 0,
+	VSD_2_0V,
+	VSD_2_6V,
+	VSD_2_7V,
+	VSD_2_8V,
+	VSD_2_9V,
+	VSD_3_0V,
+	VSD_3_15V,
+} regulator_voltage_vsd;
+
+enum {
+	VCAM_2_5V,
+	VCAM_2_6V,
+	VCAM_2_75V,
+	VCAM_3_0V,
+} regulator_voltage_vcam;
+
+enum {
+	VAUDIO_2_3V,
+	VAUDIO_2_5V,
+	VAUDIO_2_775V,
+	VAUDIO_3V,
+} regulator_voltage_vaudio;
+
+enum {
+	VUSB2_2_4V,
+	VUSB2_2_6V,
+	VUSB2_2_7V,
+	VUSB2_2_775V,
+} regulator_voltage_vusb2;
+
+enum {
+	VVIDEO_2_7V,
+	VVIDEO_2_775V,
+	VVIDEO_2_5V,
+	VVIDEO_2_6V,
+} regulator_voltage_vvideo;
+
+#define VAUDIO_LSH	4
+#define VAUDIO_WID	2
+#define VAUDIO_EN_LSH	15
+#define VAUDIO_EN_WID	1
+#define VAUDIO_EN_ENABLE	1
+#define VAUDIO_EN_DISABLE	0
+
+#define VUSB2_LSH	11
+#define VUSB2_WID	2
+#define VUSB2_EN_LSH	18
+#define VUSB2_EN_WID	1
+#define VUSB2_EN_ENABLE	1
+#define VUSB2_EN_DISABLE	0
+
+#define VVIDEO_LSH	2
+#define VVIDEO_WID	2
+#define VVIDEO_EN_LSH	12
+#define VVIDEO_EN_WID	1
+#define VVIDEO_EN_ENABLE	1
+#define VVIDEO_EN_DISABLE	0
+
+#define SWBST_EN_LSH	20
+#define SWBST_EN_WID	1
+#define SWBST_EN_ENABLE	1
+#define SWBST_EN_DISABLE	0
+
+#define VIOHI_EN_LSH	3
+#define VIOHI_EN_WID	1
+#define VIOHI_EN_ENABLE	1
+#define VIOHI_EN_DISABLE	0
+
+#define VDIG_LSH	4
+#define VDIG_WID	2
+#define VDIG_EN_LSH	9
+#define VDIG_EN_WID	1
+#define VDIG_EN_ENABLE	1
+#define VDIG_EN_DISABLE	0
+
+#define VPLL_LSH	9
+#define VPLL_WID	2
+#define VPLL_EN_LSH	15
+#define VPLL_EN_WID	1
+#define VPLL_EN_ENABLE	1
+#define VPLL_EN_DISABLE	0
+
+#define VGEN1_LSH	0
+#define VGEN1_WID	2
+#define VGEN1_EN_LSH	0
+#define VGEN1_EN_WID	1
+#define VGEN1_EN_ENABLE	1
+#define VGEN1_EN_DISABLE	0
+
+#define VGEN2_LSH	6
+#define VGEN2_WID	3
+#define VGEN2_EN_LSH	12
+#define VGEN2_EN_WID	1
+#define VGEN2_EN_ENABLE	1
+#define VGEN2_EN_DISABLE	0
+
+#define VGEN3_LSH	14
+#define VGEN3_WID	1
+#define VGEN3_EN_LSH	0
+#define VGEN3_EN_WID	1
+#define VGEN3_EN_ENABLE	1
+#define VGEN3_EN_DISABLE	0
+
+#define VSD_LSH	6
+#define VSD_WID	3
+#define VSD_EN_LSH	18
+#define VSD_EN_WID	1
+#define VSD_EN_ENABLE	1
+#define VSD_EN_DISABLE	0
+
+#define VCAM_LSH	16
+#define VCAM_WID	2
+#define VCAM_EN_LSH	6
+#define VCAM_EN_WID	1
+#define VCAM_EN_ENABLE	1
+#define VCAM_EN_DISABLE	0
+#define VCAM_CONFIG_LSH	9
+#define VCAM_CONFIG_WID	1
+#define VCAM_CONFIG_EXT	1
+#define VCAM_CONFIG_INT	0
+
+#define SW1_LSH		0
+#define SW1_WID		5
+#define SW1_DVS_LSH	5
+#define SW1_DVS_WID	5
+#define SW1_STDBY_LSH	10
+#define SW1_STDBY_WID	5
+
+#define SW2_LSH		0
+#define SW2_WID		5
+#define SW2_DVS_LSH	5
+#define SW2_DVS_WID	5
+#define SW2_STDBY_LSH	10
+#define SW2_STDBY_WID	5
+
+#define SW3_LSH		0
+#define SW3_WID		5
+#define SW3_STDBY_LSH	10
+#define SW3_STDBY_WID	5
+
+#define SW4_LSH		0
+#define SW4_WID		5
+#define SW4_STDBY_LSH	10
+#define SW4_STDBY_WID	5
+
+#define VUSB_EN_LSH	3
+#define VUSB_EN_WID	1
+#define VUSB_EN_ENABLE	1
+#define VUSB_EN_DISABLE	0
+
+#define GPO1_EN_LSH	6
+#define GPO1_EN_WID	1
+#define GPO1_EN_ENABLE	1
+#define GPO1_EN_DISABLE	0
+
+#define GPO2_EN_LSH	8
+#define GPO2_EN_WID	1
+#define GPO2_EN_ENABLE	1
+#define GPO2_EN_DISABLE	0
+
+#define GPO3_EN_LSH	10
+#define GPO3_EN_WID	1
+#define GPO3_EN_ENABLE	1
+#define GPO3_EN_DISABLE	0
+
+#define GPO4_EN_LSH	12
+#define GPO4_EN_WID	1
+#define GPO4_EN_ENABLE	1
+#define GPO4_EN_DISABLE	0
+
+#define GPO4_ADIN_LSH	21
+#define GPO4_ADIN_WID	1
+#define GPO4_ADIN_ENABLE	1
+#define GPO4_ADIN_DISABLE	0
+
+#define PWGT1SPI_EN_LSH	15
+#define PWGT1SPI_EN_WID	1
+#define PWGT1SPI_EN_ENABLE	0
+#define PWGT1SPI_EN_DISABLE	1
+
+#define PWGT2SPI_EN_LSH	16
+#define PWGT2SPI_EN_WID	1
+#define PWGT2SPI_EN_ENABLE	0
+#define PWGT2SPI_EN_DISABLE	1
+
+#define SWXHI_LSH	23
+#define SWXHI_WID	1
+#define SWXHI_ON	1
+#define SWXHI_OFF	0
+
+static int mc13892_get_sw_hi_bit(int sw)
+{
+	unsigned int register_val = 0;
+	unsigned int reg = 0;
+
+	switch (sw) {
+	case MC13892_SW1:
+		reg = REG_SW_0;
+		break;
+	case MC13892_SW2:
+		reg = REG_SW_1;
+		break;
+	case MC13892_SW3:
+		reg = REG_SW_2;
+		break;
+	case MC13892_SW4:
+		reg = REG_SW_3;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	CHECK_ERROR(pmic_read_reg(reg, &register_val, PMIC_ALL_BITS));
+	return (register_val & 0x800000) >> SWXHI_LSH;
+}
+
+static int mc13892_get_voltage_value(int *hi, int mV)
+{
+	int voltage;
+
+	if (mV < 600)
+		mV = 600;
+	if (mV > 1850)
+		mV = 1850;
+
+	if (mV > 1375)
+		*hi = 1;
+	if (mV < 1100)
+		*hi = 0;
+
+	if (*hi == 0)
+		voltage = (mV - 600) / 25;
+	else
+		voltage = (mV - 1100) / 25;
+
+	return voltage;
+}
+
+static int mc13892_get_voltage_mV(int hi, int voltage)
+{
+	int mV;
+
+	if (hi == 0)
+		mV = voltage * 25 + 600;
+	else
+		mV = voltage * 25 + 1100;
+
+	return mV;
+}
+
+static int mc13892_sw_set_voltage(struct regulator_dev *reg, int MiniV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1 = 0;
+	int voltage;
+	int sw = rdev_get_id(reg);
+	int mV = uV / 1000;
+	int hi;
+
+	hi = mc13892_get_sw_hi_bit(sw);
+	voltage = mc13892_get_voltage_value(&hi, mV);
+
+	switch (sw) {
+	case MC13892_SW1:
+		register1 = REG_SW_0;
+		register_val = BITFVAL(SW1, voltage);
+		register_mask = BITFMASK(SW1);
+		break;
+	case MC13892_SW2:
+		register1 = REG_SW_1;
+		register_val = BITFVAL(SW2, voltage);
+		register_mask = BITFMASK(SW2);
+		break;
+	case MC13892_SW3:
+		register1 = REG_SW_2;
+		register_val = BITFVAL(SW3, voltage);
+		register_mask = BITFMASK(SW3);
+		break;
+	case MC13892_SW4:
+		register1 = REG_SW_3;
+		register_val = BITFVAL(SW4, voltage);
+		register_mask = BITFMASK(SW4);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	register_val |= (hi << SWXHI_LSH);
+	register_mask |= (1 << SWXHI_LSH);
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_sw_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0;
+	int mV = 0;
+	int sw = rdev_get_id(reg);
+	int hi;
+
+	switch (sw) {
+	case MC13892_SW1:
+		CHECK_ERROR(pmic_read_reg(REG_SW_0,
+					  &register_val, PMIC_ALL_BITS));
+		voltage = BITFEXT(register_val, SW1);
+		break;
+	case MC13892_SW2:
+		CHECK_ERROR(pmic_read_reg(REG_SW_1,
+					  &register_val, PMIC_ALL_BITS));
+		voltage = BITFEXT(register_val, SW2);
+		break;
+	case MC13892_SW3:
+		CHECK_ERROR(pmic_read_reg(REG_SW_2,
+					  &register_val, PMIC_ALL_BITS));
+		voltage = BITFEXT(register_val, SW3);
+		break;
+	case MC13892_SW4:
+		CHECK_ERROR(pmic_read_reg(REG_SW_3,
+					  &register_val, PMIC_ALL_BITS));
+		voltage = BITFEXT(register_val, SW4);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	hi = mc13892_get_sw_hi_bit(sw);
+	mV = mc13892_get_voltage_mV(hi, voltage);
+
+	return mV * 1000;
+}
+
+static int mc13892_sw_stby_enable(struct regulator_dev *reg)
+{
+	return 0;
+}
+
+static int mc13892_sw_stby_disable(struct regulator_dev *reg)
+{
+	return 0;
+}
+
+static int mc13892_sw_stby_set_mode(struct regulator_dev *reg, unsigned int mode)
+{
+	return 0;
+}
+
+static int mc13892_sw_stby_set_voltage(struct regulator_dev *reg, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1 = 0;
+	int voltage, mV = uV / 1000, hi;
+	int sw = rdev_get_id(reg);
+
+	hi = mc13892_get_sw_hi_bit(sw);
+	voltage = mc13892_get_voltage_value(&hi, mV);
+
+	switch (sw) {
+	case MC13892_SW1:
+		register1 = REG_SW_0;
+		register_val = BITFVAL(SW1_STDBY, voltage);
+		register_mask = BITFMASK(SW1_STDBY);
+		break;
+	case MC13892_SW2:
+		register1 = REG_SW_1;
+		register_val = BITFVAL(SW2_STDBY, voltage);
+		register_mask = BITFMASK(SW2_STDBY);
+		break;
+	case MC13892_SW3:
+		register1 = REG_SW_2;
+		register_val = BITFVAL(SW3_STDBY, voltage);
+		register_mask = BITFMASK(SW3_STDBY);
+		break;
+	case MC13892_SW4:
+		register1 = REG_SW_3;
+		register_val = BITFVAL(SW4_STDBY, voltage);
+		register_mask = BITFMASK(SW4_STDBY);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	register_val |= (hi << SWXHI_LSH);
+	register_mask |= (1 << SWXHI_LSH);
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_swbst_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(SWBST_EN, SWBST_EN_ENABLE);
+	register_mask = BITFMASK(SWBST_EN);
+	register1 = REG_SW_5;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_swbst_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(SWBST_EN, SWBST_EN_DISABLE);
+	register_mask = BITFMASK(SWBST_EN);
+	register1 = REG_SW_5;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_viohi_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VIOHI_EN, VIOHI_EN_ENABLE);
+	register_mask = BITFMASK(VIOHI_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_viohi_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VIOHI_EN, VIOHI_EN_DISABLE);
+	register_mask = BITFMASK(VIOHI_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vusb_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VUSB_EN, VUSB_EN_ENABLE);
+	register_mask = BITFMASK(VUSB_EN);
+	register1 = REG_USB1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vusb_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VUSB_EN, VUSB_EN_DISABLE);
+	register_mask = BITFMASK(VUSB_EN);
+	register1 = REG_USB1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vdig_set_voltage(struct regulator_dev *reg,
+				    int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1050) && (mV < 1250))
+		voltage = VDIG_1_05V;
+	else if ((mV >= 1250) && (mV < 1650))
+		voltage = VDIG_1_25V;
+	else if ((mV >= 1650) && (mV < 1800))
+		voltage = VDIG_1_65V;
+	else
+		voltage = VDIG_1_80V;
+
+	register_val = BITFVAL(VDIG, voltage);
+	register_mask = BITFMASK(VDIG);
+	register1 = REG_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vdig_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SETTING_0, &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VDIG);
+
+	switch (voltage) {
+	case VDIG_1_05V:
+		mV = 1050;
+		break;
+	case VDIG_1_25V:
+		mV = 1250;
+		break;
+	case VDIG_1_65V:
+		mV = 1650;
+		break;
+	case VDIG_1_80V:
+		mV = 1800;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13892_vdig_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VDIG_EN, VDIG_EN_ENABLE);
+	register_mask = BITFMASK(VDIG_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vdig_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VDIG_EN, VDIG_EN_DISABLE);
+	register_mask = BITFMASK(VDIG_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vpll_set_voltage(struct regulator_dev *reg,
+				    int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1050) && (mV < 1250))
+		voltage = VPLL_1_05V;
+	else if ((mV >= 1250) && (mV < 1650))
+		voltage = VPLL_1_25V;
+	else if ((mV >= 1650) && (mV < 1800))
+		voltage = VPLL_1_65V;
+	else
+		voltage = VPLL_1_80V;
+
+	register_val = BITFVAL(VPLL, voltage);
+	register_mask = BITFMASK(VPLL);
+	register1 = REG_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vpll_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SETTING_0, &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VPLL);
+
+	switch (voltage) {
+	case VPLL_1_05V:
+		mV = 1050;
+		break;
+	case VPLL_1_25V:
+		mV = 1250;
+		break;
+	case VPLL_1_65V:
+		mV = 1650;
+		break;
+	case VPLL_1_80V:
+		mV = 1800;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13892_vpll_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VPLL_EN, VPLL_EN_ENABLE);
+	register_mask = BITFMASK(VPLL_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vpll_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VPLL_EN, VPLL_EN_DISABLE);
+	register_mask = BITFMASK(VPLL_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vaudio_set_voltage(struct regulator_dev *reg,
+				      int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 2300) && (mV < 2500))
+		voltage = VAUDIO_2_3V;
+	else if ((mV >= 2500) && (mV < 2775))
+		voltage = VAUDIO_2_5V;
+	else if ((mV >= 2775) && (mV < 3000))
+		voltage = VAUDIO_2_775V;
+	else
+		voltage = VAUDIO_3V;
+
+	register_val = BITFVAL(VAUDIO, voltage);
+	register_mask = BITFMASK(VAUDIO);
+	register1 = REG_SETTING_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vaudio_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SETTING_1, &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VAUDIO);
+
+	switch (voltage) {
+	case VAUDIO_2_3V:
+		mV = 2300;
+		break;
+	case VAUDIO_2_5V:
+		mV = 2500;
+		break;
+	case VAUDIO_2_775V:
+		mV = 2775;
+		break;
+	case VAUDIO_3V:
+		mV = 3000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13892_vaudio_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VAUDIO_EN, VAUDIO_EN_ENABLE);
+	register_mask = BITFMASK(VAUDIO_EN);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vaudio_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VAUDIO_EN, VAUDIO_EN_DISABLE);
+	register_mask = BITFMASK(VAUDIO_EN);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vusb2_set_voltage(struct regulator_dev *reg,
+				     int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 2400) && (mV < 2600))
+		voltage = VUSB2_2_4V;
+	else if ((mV >= 2600) && (mV < 2700))
+		voltage = VUSB2_2_6V;
+	else if ((mV >= 2700) && (mV < 2775))
+		voltage = VUSB2_2_7V;
+	else
+		voltage = VUSB2_2_775V;
+
+	register_val = BITFVAL(VUSB2, voltage);
+	register_mask = BITFMASK(VUSB2);
+	register1 = REG_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vusb2_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SETTING_0, &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VUSB2);
+
+	switch (voltage) {
+	case VUSB2_2_4V:
+		mV = 2400;
+		break;
+	case VUSB2_2_6V:
+		mV = 2600;
+		break;
+	case VUSB2_2_7V:
+		mV = 2700;
+		break;
+	case VUSB2_2_775V:
+		mV = 2775;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13892_vusb2_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VUSB2_EN, VUSB2_EN_ENABLE);
+	register_mask = BITFMASK(VUSB2_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vusb2_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VUSB2_EN, VUSB2_EN_DISABLE);
+	register_mask = BITFMASK(VUSB2_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vvideo_set_voltage(struct regulator_dev *reg,
+				      int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 2500) && (mV < 2600))
+		voltage = VVIDEO_2_5V;
+	else if ((mV >= 2600) && (mV < 2700))
+		voltage = VVIDEO_2_6V;
+	else if ((mV >= 2700) && (mV < 2775))
+		voltage = VVIDEO_2_7V;
+	else
+		voltage = VVIDEO_2_775V;
+
+	register_val = BITFVAL(VVIDEO, voltage);
+	register_mask = BITFMASK(VVIDEO);
+	register1 = REG_SETTING_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vvideo_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SETTING_1, &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VVIDEO);
+
+	switch (voltage) {
+	case VVIDEO_2_5V:
+		mV = 2500;
+		break;
+	case VVIDEO_2_6V:
+		mV = 2600;
+		break;
+	case VVIDEO_2_7V:
+		mV = 2700;
+		break;
+	case VVIDEO_2_775V:
+		mV = 2775;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13892_vvideo_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VVIDEO_EN, VVIDEO_EN_ENABLE);
+	register_mask = BITFMASK(VVIDEO_EN);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vvideo_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VVIDEO_EN, VVIDEO_EN_DISABLE);
+	register_mask = BITFMASK(VVIDEO_EN);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vsd_set_voltage(struct regulator_dev *reg,
+				   int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1800) && (mV < 2000))
+		voltage = VSD_1_8V;
+	else if ((mV >= 2000) && (mV < 2600))
+		voltage = VSD_2_0V;
+	else if ((mV >= 2600) && (mV < 2700))
+		voltage = VSD_2_6V;
+	else if ((mV >= 2700) && (mV < 2800))
+		voltage = VSD_2_7V;
+	else if ((mV >= 2800) && (mV < 2900))
+		voltage = VSD_2_8V;
+	else if ((mV >= 2900) && (mV < 3000))
+		voltage = VSD_2_9V;
+	else if ((mV >= 3000) && (mV < 3150))
+		voltage = VSD_3_0V;
+	else
+		voltage = VSD_3_15V;
+
+	register_val = BITFVAL(VSD, voltage);
+	register_mask = BITFMASK(VSD);
+	register1 = REG_SETTING_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vsd_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SETTING_1, &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VSD);
+
+	switch (voltage) {
+	case VSD_1_8V:
+		mV = 1800;
+		break;
+	case VSD_2_0V:
+		mV = 2000;
+		break;
+	case VSD_2_6V:
+		mV = 2600;
+		break;
+	case VSD_2_7V:
+		mV = 2700;
+		break;
+	case VSD_2_8V:
+		mV = 2800;
+		break;
+	case VSD_2_9V:
+		mV = 2900;
+		break;
+	case VSD_3_0V:
+		mV = 3000;
+		break;
+	case VSD_3_15V:
+		mV = 3150;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13892_vsd_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VSD_EN, VSD_EN_ENABLE);
+	register_mask = BITFMASK(VSD_EN);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vsd_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VSD_EN, VSD_EN_DISABLE);
+	register_mask = BITFMASK(VSD_EN);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vcam_set_voltage(struct regulator_dev *reg,
+				    int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 2500) && (mV < 2600))
+		voltage = VCAM_2_5V;
+	else if ((mV >= 2600) && (mV < 2750))
+		voltage = VCAM_2_6V;
+	else if ((mV >= 2750) && (mV < 3000))
+		voltage = VCAM_2_75V;
+	else
+		voltage = VCAM_3_0V;
+
+	register_val = BITFVAL(VCAM, voltage);
+	register_mask = BITFMASK(VCAM);
+	register1 = REG_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vcam_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SETTING_0, &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VCAM);
+
+	switch (voltage) {
+	case VCAM_2_5V:
+		mV = 2500;
+		break;
+	case VCAM_2_6V:
+		mV = 2600;
+		break;
+	case VCAM_2_75V:
+		mV = 2750;
+		break;
+	case VCAM_3_0V:
+		mV = 3000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13892_vcam_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VCAM_EN, VCAM_EN_ENABLE);
+	register_mask = BITFMASK(VCAM_EN);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vcam_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VCAM_EN, VCAM_EN_DISABLE);
+	register_mask = BITFMASK(VCAM_EN);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vcam_set_mode(struct regulator_dev *reg, unsigned int mode)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		register_val = BITFVAL(VCAM_CONFIG, VCAM_CONFIG_EXT);
+		break;
+	case REGULATOR_MODE_NORMAL:
+		register_val = BITFVAL(VCAM_CONFIG, VCAM_CONFIG_INT);
+		break;
+	default:
+		return -EINVAL;
+	}
+	register_mask = BITFMASK(VCAM_CONFIG);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+unsigned int mc13892_vcam_get_mode(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int config = 0, mode = VCAM_CONFIG_INT;
+
+	CHECK_ERROR(pmic_read_reg(REG_MODE_1, &register_val, PMIC_ALL_BITS));
+	config = BITFEXT(register_val, VCAM_CONFIG);
+
+	switch (config) {
+	case VCAM_CONFIG_EXT:
+		mode = REGULATOR_MODE_FAST;
+		break;
+	case VCAM_CONFIG_INT:
+		mode = REGULATOR_MODE_NORMAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return mode;
+}
+
+static int mc13892_vgen1_set_voltage(struct regulator_dev *reg,
+				     int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1200) && (mV < 1500))
+		voltage = VGEN1_1_2V;
+	else if ((mV >= 1500) && (mV < 2775))
+		voltage = VGEN1_1_5V;
+	else if ((mV >= 2775) && (mV < 3150))
+		voltage = VGEN1_2_775V;
+	else
+		voltage = VGEN1_3_15V;
+
+	register_val = BITFVAL(VGEN1, voltage);
+	register_mask = BITFMASK(VGEN1);
+	register1 = REG_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vgen1_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SETTING_0, &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VGEN1);
+
+	switch (voltage) {
+	case VGEN1_1_2V:
+		mV = 1200;
+		break;
+	case VGEN1_1_5V:
+		mV = 1500;
+		break;
+	case VGEN1_2_775V:
+		mV = 2775;
+		break;
+	case VGEN1_3_15V:
+		mV = 3150;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13892_vgen1_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VGEN1_EN, VGEN1_EN_ENABLE);
+	register_mask = BITFMASK(VGEN1_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vgen1_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VGEN1_EN, VGEN1_EN_DISABLE);
+	register_mask = BITFMASK(VGEN1_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vgen2_set_voltage(struct regulator_dev *reg,
+				     int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1200) && (mV < 1500))
+		voltage = VGEN2_1_2V;
+	else if ((mV >= 1500) && (mV < 1600))
+		voltage = VGEN2_1_5V;
+	else if ((mV >= 1600) && (mV < 1800))
+		voltage = VGEN2_1_6V;
+	else if ((mV >= 1800) && (mV < 2700))
+		voltage = VGEN2_1_8V;
+	else if ((mV >= 2700) && (mV < 2800))
+		voltage = VGEN2_2_7V;
+	else if ((mV >= 2800) && (mV < 3000))
+		voltage = VGEN2_2_8V;
+	else if ((mV >= 3000) && (mV < 3150))
+		voltage = VGEN2_3_0V;
+	else
+		voltage = VGEN2_3_15V;
+
+	register_val = BITFVAL(VGEN2, voltage);
+	register_mask = BITFMASK(VGEN2);
+	register1 = REG_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vgen2_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SETTING_0, &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VGEN2);
+
+	switch (voltage) {
+	case VGEN2_1_2V:
+		mV = 1200;
+		break;
+	case VGEN2_1_5V:
+		mV = 1500;
+		break;
+	case VGEN2_1_6V:
+		mV = 1600;
+		break;
+	case VGEN2_1_8V:
+		mV = 1800;
+		break;
+	case VGEN2_2_7V:
+		mV = 2700;
+		break;
+	case VGEN2_2_8V:
+		mV = 2800;
+		break;
+	case VGEN2_3_0V:
+		mV = 3000;
+		break;
+	case VGEN2_3_15V:
+		mV = 3150;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13892_vgen2_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VGEN2_EN, VGEN2_EN_ENABLE);
+	register_mask = BITFMASK(VGEN2_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vgen2_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VGEN2_EN, VGEN2_EN_DISABLE);
+	register_mask = BITFMASK(VGEN2_EN);
+	register1 = REG_MODE_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vgen3_set_voltage(struct regulator_dev *reg,
+				     int minuV, int uV)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int voltage, mV = uV / 1000;
+
+	if ((mV >= 1800) && (mV < 2900))
+		voltage = VGEN3_1_8V;
+	else
+		voltage = VGEN3_2_9V;
+
+	register_val = BITFVAL(VGEN3, voltage);
+	register_mask = BITFMASK(VGEN3);
+	register1 = REG_SETTING_0;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vgen3_get_voltage(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0;
+	int voltage = 0, mV = 0;
+
+	CHECK_ERROR(pmic_read_reg(REG_SETTING_0, &register_val, PMIC_ALL_BITS));
+	voltage = BITFEXT(register_val, VGEN3);
+
+	switch (voltage) {
+	case VGEN3_1_8V:
+		mV = 1800;
+		break;
+	case VGEN3_2_9V:
+		mV = 2900;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mV * 1000;
+}
+
+static int mc13892_vgen3_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VGEN3_EN, VGEN3_EN_ENABLE);
+	register_mask = BITFMASK(VGEN3_EN);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_vgen3_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+
+	register_val = BITFVAL(VGEN3_EN, VGEN3_EN_DISABLE);
+	register_mask = BITFMASK(VGEN3_EN);
+	register1 = REG_MODE_1;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_gpo_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int gpo = rdev_get_id(reg);
+
+	switch (gpo) {
+	case MC13892_GPO1:
+		register_val = BITFVAL(GPO1_EN, GPO1_EN_ENABLE);
+		register_mask = BITFMASK(GPO1_EN);
+		break;
+	case MC13892_GPO2:
+		register_val = BITFVAL(GPO2_EN, GPO2_EN_ENABLE);
+		register_mask = BITFMASK(GPO2_EN);
+		break;
+	case MC13892_GPO3:
+		register_val = BITFVAL(GPO3_EN, GPO3_EN_ENABLE);
+		register_mask = BITFMASK(GPO3_EN);
+		break;
+	case MC13892_GPO4:
+		register_val = BITFVAL(GPO4_EN, GPO4_EN_ENABLE) +
+		    BITFVAL(GPO4_ADIN, GPO4_ADIN_DISABLE);
+		register_mask = BITFMASK(GPO4_EN) + BITFMASK(GPO4_ADIN);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	register1 = REG_POWER_MISC;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_gpo_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int gpo = rdev_get_id(reg);
+
+	switch (gpo) {
+	case MC13892_GPO1:
+		register_val = BITFVAL(GPO1_EN, GPO1_EN_DISABLE);
+		register_mask = BITFMASK(GPO1_EN);
+		break;
+	case MC13892_GPO2:
+		register_val = BITFVAL(GPO2_EN, GPO2_EN_DISABLE);
+		register_mask = BITFMASK(GPO2_EN);
+		break;
+	case MC13892_GPO3:
+		register_val = BITFVAL(GPO3_EN, GPO3_EN_DISABLE);
+		register_mask = BITFMASK(GPO3_EN);
+		break;
+	case MC13892_GPO4:
+		register_val = BITFVAL(GPO4_EN, GPO4_EN_DISABLE);
+		register_mask = BITFMASK(GPO4_EN);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	register1 = REG_POWER_MISC;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_power_gating_enable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int gpo = rdev_get_id(reg);
+
+	switch (gpo) {
+	case MC13892_PWGT1:
+		register_val = BITFVAL(PWGT1SPI_EN, PWGT1SPI_EN_ENABLE);
+		register_mask = BITFMASK(PWGT1SPI_EN);
+		break;
+	case MC13892_PWGT2:
+		register_val = BITFVAL(PWGT2SPI_EN, PWGT2SPI_EN_ENABLE);
+		register_mask = BITFMASK(PWGT2SPI_EN);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	register1 = REG_POWER_MISC;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static int mc13892_power_gating_disable(struct regulator_dev *reg)
+{
+	unsigned int register_val = 0, register_mask = 0;
+	unsigned int register1;
+	int gpo = rdev_get_id(reg);
+
+	switch (gpo) {
+	case MC13892_PWGT1:
+		register_val = BITFVAL(PWGT1SPI_EN, PWGT1SPI_EN_DISABLE);
+		register_mask = BITFMASK(PWGT1SPI_EN);
+		break;
+	case MC13892_PWGT2:
+		register_val = BITFVAL(PWGT2SPI_EN, PWGT2SPI_EN_DISABLE);
+		register_mask = BITFMASK(PWGT2SPI_EN);
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	register1 = REG_POWER_MISC;
+
+	return pmic_write_reg(register1, register_val, register_mask);
+}
+
+static struct regulator_ops mc13892_sw_ops = {
+	.enable = mc13892_sw_stby_enable,
+	.disable = mc13892_sw_stby_disable,
+	.set_voltage = mc13892_sw_set_voltage,
+	.get_voltage = mc13892_sw_get_voltage,
+	.set_suspend_voltage = mc13892_sw_stby_set_voltage,
+	.set_suspend_enable = mc13892_sw_stby_enable,
+	.set_suspend_disable = mc13892_sw_stby_disable,
+	.set_suspend_mode = mc13892_sw_stby_set_mode,
+};
+
+static struct regulator_ops mc13892_swbst_ops = {
+	.enable = mc13892_swbst_enable,
+	.disable = mc13892_swbst_disable,
+};
+
+static struct regulator_ops mc13892_viohi_ops = {
+	.enable = mc13892_viohi_enable,
+	.disable = mc13892_viohi_disable,
+};
+
+static struct regulator_ops mc13892_vusb_ops = {
+	.enable = mc13892_vusb_enable,
+	.disable = mc13892_vusb_disable,
+};
+
+static struct regulator_ops mc13892_vdig_ops = {
+	.set_voltage = mc13892_vdig_set_voltage,
+	.get_voltage = mc13892_vdig_get_voltage,
+	.enable = mc13892_vdig_enable,
+	.disable = mc13892_vdig_disable,
+};
+
+static struct regulator_ops mc13892_vpll_ops = {
+	.set_voltage = mc13892_vpll_set_voltage,
+	.get_voltage = mc13892_vpll_get_voltage,
+	.enable = mc13892_vpll_enable,
+	.disable = mc13892_vpll_disable,
+};
+
+static struct regulator_ops mc13892_vusb2_ops = {
+	.set_voltage = mc13892_vusb2_set_voltage,
+	.get_voltage = mc13892_vusb2_get_voltage,
+	.enable = mc13892_vusb2_enable,
+	.disable = mc13892_vusb2_disable,
+};
+
+static struct regulator_ops mc13892_vvideo_ops = {
+	.set_voltage = mc13892_vvideo_set_voltage,
+	.get_voltage = mc13892_vvideo_get_voltage,
+	.enable = mc13892_vvideo_enable,
+	.disable = mc13892_vvideo_disable,
+};
+
+static struct regulator_ops mc13892_vaudio_ops = {
+	.set_voltage = mc13892_vaudio_set_voltage,
+	.get_voltage = mc13892_vaudio_get_voltage,
+	.enable = mc13892_vaudio_enable,
+	.disable = mc13892_vaudio_disable,
+};
+
+static struct regulator_ops mc13892_vsd_ops = {
+	.set_voltage = mc13892_vsd_set_voltage,
+	.get_voltage = mc13892_vsd_get_voltage,
+	.enable = mc13892_vsd_enable,
+	.disable = mc13892_vsd_disable,
+};
+
+static struct regulator_ops mc13892_vcam_ops = {
+	.set_voltage = mc13892_vcam_set_voltage,
+	.get_voltage = mc13892_vcam_get_voltage,
+	.enable = mc13892_vcam_enable,
+	.disable = mc13892_vcam_disable,
+	.set_mode = mc13892_vcam_set_mode,
+	.get_mode = mc13892_vcam_get_mode,
+};
+
+static struct regulator_ops mc13892_vgen1_ops = {
+	.set_voltage = mc13892_vgen1_set_voltage,
+	.get_voltage = mc13892_vgen1_get_voltage,
+	.enable = mc13892_vgen1_enable,
+	.disable = mc13892_vgen1_disable,
+};
+
+static struct regulator_ops mc13892_vgen2_ops = {
+	.set_voltage = mc13892_vgen2_set_voltage,
+	.get_voltage = mc13892_vgen2_get_voltage,
+	.enable = mc13892_vgen2_enable,
+	.disable = mc13892_vgen2_disable,
+};
+
+static struct regulator_ops mc13892_vgen3_ops = {
+	.set_voltage = mc13892_vgen3_set_voltage,
+	.get_voltage = mc13892_vgen3_get_voltage,
+	.enable = mc13892_vgen3_enable,
+	.disable = mc13892_vgen3_disable,
+};
+
+static struct regulator_ops mc13892_gpo_ops = {
+	.enable = mc13892_gpo_enable,
+	.disable = mc13892_gpo_disable,
+};
+
+static struct regulator_ops mc13892_power_gating_ops = {
+	.enable = mc13892_power_gating_enable,
+	.disable = mc13892_power_gating_disable,
+
+};
+
+static struct regulator_desc mc13892_reg[] = {
+	{
+		.name = "SW1",
+		.id = MC13892_SW1,
+		.ops = &mc13892_sw_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "SW2",
+		.id = MC13892_SW2,
+		.ops = &mc13892_sw_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "SW3",
+		.id = MC13892_SW3,
+		.ops = &mc13892_sw_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "SW4",
+		.id = MC13892_SW4,
+		.ops = &mc13892_sw_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "SWBST",
+		.id = MC13892_SWBST,
+		.ops = &mc13892_swbst_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VIOHI",
+		.id = MC13892_VIOHI,
+		.ops = &mc13892_viohi_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VPLL",
+		.id = MC13892_VPLL,
+		.ops = &mc13892_vpll_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VDIG",
+		.id = MC13892_VDIG,
+		.ops = &mc13892_vdig_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VSD",
+		.id = MC13892_VSD,
+		.ops = &mc13892_vsd_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VUSB2",
+		.id = MC13892_VUSB2,
+		.ops = &mc13892_vusb2_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VVIDEO",
+		.id = MC13892_VVIDEO,
+		.ops = &mc13892_vvideo_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VAUDIO",
+		.id = MC13892_VAUDIO,
+		.ops = &mc13892_vaudio_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VCAM",
+		.id = MC13892_VCAM,
+		.ops = &mc13892_vcam_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VGEN1",
+		.id = MC13892_VGEN1,
+		.ops = &mc13892_vgen1_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VGEN2",
+		.id = MC13892_VGEN2,
+		.ops = &mc13892_vgen2_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VGEN3",
+		.id = MC13892_VGEN3,
+		.ops = &mc13892_vgen3_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "VUSB",
+		.id = MC13892_VUSB,
+		.ops = &mc13892_vusb_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "GPO1",
+		.id = MC13892_GPO1,
+		.ops = &mc13892_gpo_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "GPO2",
+		.id = MC13892_GPO2,
+		.ops = &mc13892_gpo_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "GPO3",
+		.id = MC13892_GPO3,
+		.ops = &mc13892_gpo_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "GPO4",
+		.id = MC13892_GPO4,
+		.ops = &mc13892_gpo_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "PWGT1",
+		.id = MC13892_PWGT1,
+		.ops = &mc13892_power_gating_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "PWGT2",
+		.id = MC13892_PWGT2,
+		.ops = &mc13892_power_gating_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+};
+
+static int mc13892_regulator_probe(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev;
+
+	/* register regulator */
+	rdev = regulator_register(&mc13892_reg[pdev->id], &pdev->dev,
+				  dev_get_drvdata(&pdev->dev));
+	if (IS_ERR(rdev)) {
+		dev_err(&pdev->dev, "failed to register %s\n",
+			mc13892_reg[pdev->id].name);
+		return PTR_ERR(rdev);
+	}
+
+	return 0;
+}
+
+
+static int mc13892_regulator_remove(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+
+	regulator_unregister(rdev);
+
+	return 0;
+}
+
+int mc13892_register_regulator(struct mc13892 *mc13892, int reg,
+			      struct regulator_init_data *initdata)
+{
+	struct platform_device *pdev;
+	int ret;
+
+	if (mc13892->pmic.pdev[reg])
+		return -EBUSY;
+
+	pdev = platform_device_alloc("mc13892-regulatr", reg);
+	if (!pdev)
+		return -ENOMEM;
+
+	mc13892->pmic.pdev[reg] = pdev;
+
+	initdata->driver_data = mc13892;
+
+	pdev->dev.platform_data = initdata;
+	pdev->dev.parent = mc13892->dev;
+	platform_set_drvdata(pdev, mc13892);
+	ret = platform_device_add(pdev);
+
+	if (ret != 0) {
+		dev_err(mc13892->dev, "Failed to register regulator %d: %d\n",
+			reg, ret);
+		platform_device_del(pdev);
+		mc13892->pmic.pdev[reg] = NULL;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mc13892_register_regulator);
+
+static struct platform_driver mc13892_regulator_driver = {
+	.probe = mc13892_regulator_probe,
+	.remove = mc13892_regulator_remove,
+	.driver		= {
+		.name	= "mc13892-regulatr",
+	},
+};
+
+static int __init mc13892_regulator_init(void)
+{
+	return platform_driver_register(&mc13892_regulator_driver);
+}
+subsys_initcall(mc13892_regulator_init);
+
+static void __exit mc13892_regulator_exit(void)
+{
+	platform_driver_unregister(&mc13892_regulator_driver);
+}
+module_exit(mc13892_regulator_exit);
+
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MC13892 Regulator driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/reg-mc34704.c b/drivers/regulator/reg-mc34704.c
new file mode 100644
index 000000000000..a832a639b8ec
--- /dev/null
+++ b/drivers/regulator/reg-mc34704.c
@@ -0,0 +1,288 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/ioctl.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/driver.h>
+#include <linux/mfd/mc34704/core.h>
+#include <linux/platform_device.h>
+#include <linux/pmic_status.h>
+#include <linux/pmic_external.h>
+
+#define MC34704_ONOFFA	0x8
+#define MC34704_ONOFFC	0x4
+#define MC34704_ONOFFD	0x2
+#define MC34704_ONOFFE	0x1
+
+/* Private data for MC34704 regulators */
+
+struct reg_mc34704_priv {
+	short enable;		/* enable bit, if available */
+	short v_default;	/* default regulator voltage in mV */
+	int dvs_min;		/* minimum voltage change in units of 2.5% */
+	int dvs_max;		/* maximum voltage change in units of 2.5% */
+	char i2c_dvs;		/* i2c DVS register number */
+	char i2c_stat;		/* i2c status register number */
+};
+struct reg_mc34704_priv mc34704_reg_priv[] = {
+	{
+	 .v_default = REG1_V_MV,
+	 .dvs_min = REG1_DVS_MIN_PCT / 2.5,
+	 .dvs_max = REG1_DVS_MAX_PCT / 2.5,
+	 .i2c_dvs = 0x4,
+	 .i2c_stat = 0x5,
+	 .enable = MC34704_ONOFFA,
+	 },
+	{
+	 .v_default = REG2_V_MV,
+	 .dvs_min = REG2_DVS_MIN_PCT / 2.5,
+	 .dvs_max = REG2_DVS_MAX_PCT / 2.5,
+	 .i2c_dvs = 0x6,
+	 .i2c_stat = 0x7,
+	 },
+	{
+	 .v_default = REG3_V_MV,
+	 .dvs_min = REG3_DVS_MIN_PCT / 2.5,
+	 .dvs_max = REG3_DVS_MAX_PCT / 2.5,
+	 .i2c_dvs = 0x8,
+	 .i2c_stat = 0x9,
+	 },
+	{
+	 .v_default = REG4_V_MV,
+	 .dvs_min = REG4_DVS_MIN_PCT / 2.5,
+	 .dvs_max = REG4_DVS_MAX_PCT / 2.5,
+	 .i2c_dvs = 0xA,
+	 .i2c_stat = 0xB,
+	 },
+	{
+	 .v_default = REG5_V_MV,
+	 .dvs_min = REG5_DVS_MIN_PCT / 2.5,
+	 .dvs_max = REG5_DVS_MAX_PCT / 2.5,
+	 .i2c_dvs = 0xC,
+	 .i2c_stat = 0xE,
+	 .enable = MC34704_ONOFFE,
+	 },
+};
+
+static int mc34704_set_voltage(struct regulator_dev *reg, int MiniV, int uV)
+{
+	struct reg_mc34704_priv *priv = rdev_get_drvdata(reg);
+	int mV = uV / 1000;
+	int dV = mV - priv->v_default;
+
+	/* compute dynamic voltage scaling value */
+	int dvs = 1000 * dV / priv->v_default / 25;
+
+	/* clip to regulator limits */
+	if (dvs > priv->dvs_max)
+		dvs = priv->dvs_max;
+	if (dvs < priv->dvs_min)
+		dvs = priv->dvs_min;
+
+	return pmic_write_reg(priv->i2c_dvs, dvs << 1, 0x1E);
+}
+
+static int mc34704_get_voltage(struct regulator_dev *reg)
+{
+	int mV;
+	struct reg_mc34704_priv *priv = rdev_get_drvdata(reg);
+	int val, dvs;
+
+	CHECK_ERROR(pmic_read_reg(priv->i2c_dvs, &val, 0xF));
+
+	dvs = (val >> 1) & 0xF;
+
+	/* dvs is 4-bit 2's complement; sign-extend it */
+	if (dvs & 8)
+		dvs |= -1 & ~0xF;
+
+	/* Regulator voltage is adjusted by (dvs * 2.5%) */
+	mV = priv->v_default * (1000 + 25 * dvs) / 1000;
+
+	return 1000 * mV;
+}
+
+static int mc34704_enable_reg(struct regulator_dev *reg)
+{
+	struct reg_mc34704_priv *priv = rdev_get_drvdata(reg);
+
+	if (priv->enable)
+		return pmic_write_reg(REG_MC34704_GENERAL2, -1, priv->enable);
+
+	return PMIC_ERROR;
+}
+
+static int mc34704_disable_reg(struct regulator_dev *reg)
+{
+	struct reg_mc34704_priv *priv = rdev_get_drvdata(reg);
+
+	if (priv->enable)
+		return pmic_write_reg(REG_MC34704_GENERAL2, 0, priv->enable);
+
+	return PMIC_ERROR;
+}
+
+static int mc34704_is_reg_enabled(struct regulator_dev *reg)
+{
+	struct reg_mc34704_priv *priv = rdev_get_drvdata(reg);
+	int val;
+
+	if (priv->enable) {
+		CHECK_ERROR(pmic_read_reg(REG_MC34704_GENERAL2, &val,
+					  priv->enable));
+		return val ? 1 : 0;
+	} else {
+		return PMIC_ERROR;
+	}
+}
+
+static struct regulator_ops mc34704_full_ops = {
+	.set_voltage = mc34704_set_voltage,
+	.get_voltage = mc34704_get_voltage,
+	.enable = mc34704_enable_reg,
+	.disable = mc34704_disable_reg,
+	.is_enabled = mc34704_is_reg_enabled,
+};
+
+static struct regulator_ops mc34704_partial_ops = {
+	.set_voltage = mc34704_set_voltage,
+	.get_voltage = mc34704_get_voltage,
+};
+
+static struct regulator_desc reg_mc34704[] = {
+	{
+	 .name = "REG1_BKLT",
+	 .id = MC34704_BKLT,
+	 .ops = &mc34704_full_ops,
+	 .irq = 0,
+	 .type = REGULATOR_VOLTAGE,
+	 .owner = THIS_MODULE},
+	{
+	 .name = "REG2_CPU",
+	 .id = MC34704_CPU,
+	 .ops = &mc34704_partial_ops,
+	 .irq = 0,
+	 .type = REGULATOR_VOLTAGE,
+	 .owner = THIS_MODULE},
+	{
+	 .name = "REG3_CORE",
+	 .id = MC34704_CORE,
+	 .ops = &mc34704_partial_ops,
+	 .irq = 0,
+	 .type = REGULATOR_VOLTAGE,
+	 .owner = THIS_MODULE},
+	{
+	 .name = "REG4_DDR",
+	 .id = MC34704_DDR,
+	 .ops = &mc34704_partial_ops,
+	 .irq = 0,
+	 .type = REGULATOR_VOLTAGE,
+	 .owner = THIS_MODULE},
+	{
+	 .name = "REG5_PERS",
+	 .id = MC34704_PERS,
+	 .ops = &mc34704_full_ops,
+	 .irq = 0,
+	 .type = REGULATOR_VOLTAGE,
+	 .owner = THIS_MODULE},
+};
+
+static int mc34704_regulator_probe(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev;
+
+	/* register regulator */
+	rdev = regulator_register(&reg_mc34704[pdev->id], &pdev->dev,
+				  (void *)&mc34704_reg_priv[pdev->id]);
+	if (IS_ERR(rdev)) {
+		dev_err(&pdev->dev, "failed to register %s\n",
+			reg_mc34704[pdev->id].name);
+		return PTR_ERR(rdev);
+	}
+
+	return 0;
+}
+
+static int mc34704_regulator_remove(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+
+	regulator_unregister(rdev);
+
+	return 0;
+}
+
+int mc34704_register_regulator(struct mc34704 *mc34704, int reg,
+			       struct regulator_init_data *initdata)
+{
+	struct platform_device *pdev;
+	int ret;
+
+	if (mc34704->pmic.pdev[reg])
+		return -EBUSY;
+
+	pdev = platform_device_alloc("mc34704-regulatr", reg);
+	if (!pdev)
+		return -ENOMEM;
+
+	mc34704->pmic.pdev[reg] = pdev;
+
+	initdata->driver_data = mc34704;
+
+	pdev->dev.platform_data = initdata;
+	pdev->dev.driver_data = &mc34704_reg_priv[reg];
+	pdev->dev.parent = mc34704->dev;
+	platform_set_drvdata(pdev, mc34704);
+	ret = platform_device_add(pdev);
+
+	if (ret != 0) {
+		dev_err(mc34704->dev, "Failed to register regulator %d: %d\n",
+			reg, ret);
+		platform_device_del(pdev);
+		mc34704->pmic.pdev[reg] = NULL;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mc34704_register_regulator);
+
+static struct platform_driver mc34704_regulator_driver = {
+	.probe = mc34704_regulator_probe,
+	.remove = mc34704_regulator_remove,
+	.driver = {
+		   .name = "mc34704-regulatr",
+		   },
+};
+
+static int __init mc34704_regulator_init(void)
+{
+	return platform_driver_register(&mc34704_regulator_driver);
+}
+subsys_initcall(mc34704_regulator_init);
+
+static void __exit mc34704_regulator_exit(void)
+{
+	platform_driver_unregister(&mc34704_regulator_driver);
+}
+module_exit(mc34704_regulator_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MC34704 Regulator Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/reg-mc9s08dz60.c b/drivers/regulator/reg-mc9s08dz60.c
new file mode 100644
index 000000000000..c55100ded635
--- /dev/null
+++ b/drivers/regulator/reg-mc9s08dz60.c
@@ -0,0 +1,235 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/driver.h>
+#include <linux/mfd/mc9s08dz60/core.h>
+#include <linux/mfd/mc9s08dz60/pmic.h>
+#include <linux/platform_device.h>
+#include <linux/pmic_status.h>
+
+/* lcd */
+static int mc9s08dz60_lcd_enable(struct regulator_dev *reg)
+{
+	return pmic_gpio_set_bit_val(MCU_GPIO_REG_GPIO_CONTROL_1, 6, 1);
+}
+
+static int mc9s08dz60_lcd_disable(struct regulator_dev *reg)
+{
+	return pmic_gpio_set_bit_val(MCU_GPIO_REG_GPIO_CONTROL_1, 6, 0);
+}
+
+static struct regulator_ops mc9s08dz60_lcd_ops = {
+	.enable = mc9s08dz60_lcd_enable,
+	.disable = mc9s08dz60_lcd_disable,
+};
+
+/* wifi */
+static int mc9s08dz60_wifi_enable(struct regulator_dev *reg)
+{
+	return pmic_gpio_set_bit_val(MCU_GPIO_REG_GPIO_CONTROL_1, 5, 1);
+}
+
+static int mc9s08dz60_wifi_disable(struct regulator_dev *reg)
+{
+	return pmic_gpio_set_bit_val(MCU_GPIO_REG_GPIO_CONTROL_1, 5, 0);
+}
+
+static struct regulator_ops mc9s08dz60_wifi_ops = {
+	.enable = mc9s08dz60_wifi_enable,
+	.disable = mc9s08dz60_wifi_disable,
+};
+
+/* hdd */
+static int mc9s08dz60_hdd_enable(struct regulator_dev *reg)
+{
+	return pmic_gpio_set_bit_val(MCU_GPIO_REG_GPIO_CONTROL_1, 4, 1);
+}
+
+static int mc9s08dz60_hdd_disable(struct regulator_dev *reg)
+{
+	return pmic_gpio_set_bit_val(MCU_GPIO_REG_GPIO_CONTROL_1, 4, 0);
+}
+
+static struct regulator_ops mc9s08dz60_hdd_ops = {
+	.enable = mc9s08dz60_hdd_enable,
+	.disable = mc9s08dz60_hdd_disable,
+};
+
+/* gps */
+static int mc9s08dz60_gps_enable(struct regulator_dev *reg)
+{
+	return pmic_gpio_set_bit_val(MCU_GPIO_REG_GPIO_CONTROL_2, 0, 1);
+}
+
+static int mc9s08dz60_gps_disable(struct regulator_dev *reg)
+{
+	return pmic_gpio_set_bit_val(MCU_GPIO_REG_GPIO_CONTROL_2, 0, 0);
+}
+
+static struct regulator_ops mc9s08dz60_gps_ops = {
+	.enable = mc9s08dz60_gps_enable,
+	.disable = mc9s08dz60_gps_disable,
+};
+
+/* speaker */
+static int mc9s08dz60_speaker_enable(struct regulator_dev *reg)
+{
+	return pmic_gpio_set_bit_val(MCU_GPIO_REG_GPIO_CONTROL_1, 0, 1);
+}
+
+static int mc9s08dz60_speaker_disable(struct regulator_dev *reg)
+{
+	return pmic_gpio_set_bit_val(MCU_GPIO_REG_GPIO_CONTROL_1, 0, 0);
+}
+
+static struct regulator_ops mc9s08dz60_speaker_ops = {
+	.enable = mc9s08dz60_speaker_enable,
+	.disable = mc9s08dz60_speaker_disable,
+};
+
+static struct regulator_desc mc9s08dz60_reg[] = {
+	{
+		.name = "LCD",
+		.id = MC9S08DZ60_LCD,
+		.ops = &mc9s08dz60_lcd_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	 },
+	{
+		.name = "WIFI",
+		.id = MC9S08DZ60_WIFI,
+		.ops = &mc9s08dz60_wifi_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	 },
+	{
+		.name = "HDD",
+		.id = MC9S08DZ60_HDD,
+		.ops = &mc9s08dz60_hdd_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	 },
+	{
+		.name = "GPS",
+		.id = MC9S08DZ60_GPS,
+		.ops = &mc9s08dz60_gps_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	 },
+	{
+		.name = "SPKR",
+		.id = MC9S08DZ60_SPKR,
+		.ops = &mc9s08dz60_speaker_ops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	 },
+
+};
+
+static int mc9s08dz60_regulator_probe(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev;
+
+	/* register regulator */
+	rdev = regulator_register(&mc9s08dz60_reg[pdev->id], &pdev->dev,
+				  dev_get_drvdata(&pdev->dev));
+	if (IS_ERR(rdev)) {
+		dev_err(&pdev->dev, "failed to register %s\n",
+			mc9s08dz60_reg[pdev->id].name);
+		return PTR_ERR(rdev);
+	}
+
+	return 0;
+}
+
+
+static int mc9s08dz60_regulator_remove(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+
+	regulator_unregister(rdev);
+
+	return 0;
+}
+
+int mc9s08dz60_register_regulator(struct mc9s08dz60 *mc9s08dz60, int reg,
+			      struct regulator_init_data *initdata)
+{
+	struct platform_device *pdev;
+	int ret;
+
+	if (mc9s08dz60->pmic.pdev[reg])
+		return -EBUSY;
+
+	pdev = platform_device_alloc("mc9s08dz60-regu", reg);
+	if (!pdev)
+		return -ENOMEM;
+
+	mc9s08dz60->pmic.pdev[reg] = pdev;
+
+	initdata->driver_data = mc9s08dz60;
+
+	pdev->dev.platform_data = initdata;
+	pdev->dev.parent = mc9s08dz60->dev;
+	platform_set_drvdata(pdev, mc9s08dz60);
+	ret = platform_device_add(pdev);
+
+	if (ret != 0) {
+		dev_err(mc9s08dz60->dev,
+			"Failed to register regulator %d: %d\n",
+			reg, ret);
+		platform_device_del(pdev);
+		mc9s08dz60->pmic.pdev[reg] = NULL;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mc9s08dz60_register_regulator);
+
+static struct platform_driver mc9s08dz60_regulator_driver = {
+	.probe = mc9s08dz60_regulator_probe,
+	.remove = mc9s08dz60_regulator_remove,
+	.driver		= {
+		.name	= "mc9s08dz60-regu",
+	},
+};
+
+static int __init mc9s08dz60_regulator_init(void)
+{
+	return platform_driver_register(&mc9s08dz60_regulator_driver);
+}
+subsys_initcall(mc9s08dz60_regulator_init);
+
+static void __exit mc9s08dz60_regulator_exit(void)
+{
+	platform_driver_unregister(&mc9s08dz60_regulator_driver);
+}
+module_exit(mc9s08dz60_regulator_exit);
+
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MC9S08DZ60 Regulator driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/stmp3xxx.c b/drivers/regulator/stmp3xxx.c
new file mode 100644
index 000000000000..762a18496347
--- /dev/null
+++ b/drivers/regulator/stmp3xxx.c
@@ -0,0 +1,300 @@
+/*
+ * Freescale STMP378X voltage regulators
+ *
+ * Embedded Alley Solutions, Inc <source@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/driver.h>
+#include <mach/power.h>
+#include <mach/regulator.h>
+
+static int stmp3xxx_set_voltage(struct regulator_dev *reg, int MiniV, int uv)
+{
+	struct stmp3xxx_regulator *stmp_reg = rdev_get_drvdata(reg);
+
+	if (stmp_reg->rdata->set_voltage)
+		return stmp_reg->rdata->set_voltage(stmp_reg, uv);
+	else
+		return -ENOTSUPP;
+}
+
+
+static int stmp3xxx_get_voltage(struct regulator_dev *reg)
+{
+	struct stmp3xxx_regulator *stmp_reg = rdev_get_drvdata(reg);
+
+	if (stmp_reg->rdata->get_voltage)
+		return stmp_reg->rdata->get_voltage(stmp_reg);
+	else
+		return -ENOTSUPP;
+}
+
+static int stmp3xxx_set_current(struct regulator_dev *reg, int min_uA, int uA)
+{
+	struct stmp3xxx_regulator *stmp_reg = rdev_get_drvdata(reg);
+
+	if (stmp_reg->rdata->set_current)
+		return stmp_reg->rdata->set_current(stmp_reg, uA);
+	else
+		return -ENOTSUPP;
+}
+
+static int stmp3xxx_get_current(struct regulator_dev *reg)
+{
+	struct stmp3xxx_regulator *stmp_reg = rdev_get_drvdata(reg);
+
+	if (stmp_reg->rdata->get_current)
+		return stmp_reg->rdata->get_current(stmp_reg);
+	else
+		return -ENOTSUPP;
+}
+
+static int stmp3xxx_enable(struct regulator_dev *reg)
+{
+	struct stmp3xxx_regulator *stmp_reg = rdev_get_drvdata(reg);
+
+	return stmp_reg->rdata->enable(stmp_reg);
+}
+
+static int stmp3xxx_disable(struct regulator_dev *reg)
+{
+	struct stmp3xxx_regulator *stmp_reg = rdev_get_drvdata(reg);
+
+	return stmp_reg->rdata->disable(stmp_reg);
+}
+
+static int stmp3xxx_is_enabled(struct regulator_dev *reg)
+{
+	struct stmp3xxx_regulator *stmp_reg = rdev_get_drvdata(reg);
+
+	return stmp_reg->rdata->is_enabled(stmp_reg);
+}
+
+static int stmp3xxx_set_mode(struct regulator_dev *reg, unsigned int mode)
+{
+	struct stmp3xxx_regulator *stmp_reg = rdev_get_drvdata(reg);
+
+	return stmp_reg->rdata->set_mode(stmp_reg, mode);
+}
+
+static unsigned int stmp3xxx_get_mode(struct regulator_dev *reg)
+{
+	struct stmp3xxx_regulator *stmp_reg = rdev_get_drvdata(reg);
+
+	return stmp_reg->rdata->get_mode(stmp_reg);
+}
+
+static unsigned int stmp3xxx_get_optimum_mode(struct regulator_dev *reg,
+				int input_uV, int output_uV, int load_uA)
+{
+	struct stmp3xxx_regulator *stmp_reg = rdev_get_drvdata(reg);
+
+	if (stmp_reg->rdata->get_optimum_mode)
+		return stmp_reg->rdata->get_optimum_mode(stmp_reg, input_uV,
+							 output_uV, load_uA);
+	else
+		return -ENOTSUPP;
+}
+
+static struct regulator_ops stmp3xxx_rops = {
+	.set_voltage	= stmp3xxx_set_voltage,
+	.get_voltage	= stmp3xxx_get_voltage,
+	.set_current_limit	= stmp3xxx_set_current,
+	.get_current_limit	= stmp3xxx_get_current,
+	.enable		= stmp3xxx_enable,
+	.disable	= stmp3xxx_disable,
+	.is_enabled	= stmp3xxx_is_enabled,
+	.set_mode	= stmp3xxx_set_mode,
+	.get_mode	= stmp3xxx_get_mode,
+	.get_optimum_mode = stmp3xxx_get_optimum_mode,
+};
+
+static struct regulator_desc stmp3xxx_reg_desc[] = {
+	{
+		.name = "vddd",
+		.id = STMP3XXX_VDDD,
+		.ops = &stmp3xxx_rops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "vdda",
+		.id = STMP3XXX_VDDA,
+		.ops = &stmp3xxx_rops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "vddio",
+		.id = STMP3XXX_VDDIO,
+		.ops = &stmp3xxx_rops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "vddd_bo",
+		.id = STMP3XXX_VDDDBO,
+		.ops = &stmp3xxx_rops,
+		.irq = 0,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE
+	},
+	{
+		.name = "overall_current",
+		.id = STMP3XXX_OVERALL_CUR,
+		.ops = &stmp3xxx_rops,
+		.irq = 0,
+		.type = REGULATOR_CURRENT,
+		.owner = THIS_MODULE
+	},
+};
+
+static int reg_callback(struct notifier_block *self,
+			unsigned long event, void *data)
+{
+	unsigned long flags;
+	struct stmp3xxx_regulator *sreg =
+		container_of(self, struct stmp3xxx_regulator , nb);
+
+	switch (event) {
+	case STMP3XXX_REG5V_IS_USB:
+		spin_lock_irqsave(&sreg->lock, flags);
+		sreg->rdata->max_current = 500000;
+		spin_unlock_irqrestore(&sreg->lock, flags);
+		break;
+	case STMP3XXX_REG5V_NOT_USB:
+		spin_lock_irqsave(&sreg->lock, flags);
+		sreg->rdata->max_current = 0x7fffffff;
+		spin_unlock_irqrestore(&sreg->lock, flags);
+		break;
+	}
+
+	return 0;
+}
+
+int stmp3xxx_regulator_probe(struct platform_device *pdev)
+{
+	struct regulator_desc *rdesc;
+	struct regulator_dev *rdev;
+	struct stmp3xxx_regulator *sreg;
+
+	sreg = platform_get_drvdata(pdev);
+	sreg->cur_current = 0;
+	sreg->next_current = 0;
+	sreg->cur_voltage = 0;
+
+	init_waitqueue_head(&sreg->wait_q);
+	spin_lock_init(&sreg->lock);
+
+	if (pdev->id > STMP3XXX_OVERALL_CUR) {
+		rdesc = kzalloc(sizeof(struct regulator_desc), GFP_KERNEL);
+		memcpy(rdesc, &stmp3xxx_reg_desc[STMP3XXX_OVERALL_CUR],
+			sizeof(struct regulator_desc));
+		rdesc->name = kstrdup(sreg->rdata->name, GFP_KERNEL);
+	} else
+		rdesc = &stmp3xxx_reg_desc[pdev->id];
+
+	pr_debug("probing regulator %s %s %d\n",
+			sreg->rdata->name,
+			rdesc->name,
+			pdev->id);
+
+	/* register regulator */
+	rdev = regulator_register(rdesc, &pdev->dev,
+				  sreg);
+
+	if (IS_ERR(rdev)) {
+		dev_err(&pdev->dev, "failed to register %s\n",
+			rdesc->name);
+		return PTR_ERR(rdev);
+	}
+
+	if (sreg->rdata->max_current) {
+		struct regulator *regu;
+		regu = regulator_get(NULL, sreg->rdata->name);
+		sreg->nb.notifier_call = reg_callback;
+		regulator_register_notifier(regu, &sreg->nb);
+	}
+
+	return 0;
+}
+
+
+int stmp3xxx_regulator_remove(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+
+	regulator_unregister(rdev);
+
+	return 0;
+
+}
+
+int stmp3xxx_register_regulator(
+		struct stmp3xxx_regulator *reg_data, int reg,
+			      struct regulator_init_data *initdata)
+{
+	struct platform_device *pdev;
+	int ret;
+
+	pdev = platform_device_alloc("stmp3xxx_reg", reg);
+	if (!pdev)
+		return -ENOMEM;
+
+	pdev->dev.platform_data = initdata;
+
+	platform_set_drvdata(pdev, reg_data);
+	ret = platform_device_add(pdev);
+
+	if (ret != 0) {
+		pr_debug("Failed to register regulator %d: %d\n",
+			reg, ret);
+		platform_device_del(pdev);
+	}
+	pr_debug("register regulator %s, %d: %d\n",
+			reg_data->rdata->name, reg, ret);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(stmp3xxx_register_regulator);
+
+struct platform_driver stmp3xxx_reg = {
+	.driver = {
+		.name	= "stmp3xxx_reg",
+	},
+	.probe	= stmp3xxx_regulator_probe,
+	.remove	= stmp3xxx_regulator_remove,
+};
+
+int stmp3xxx_regulator_init(void)
+{
+	return platform_driver_register(&stmp3xxx_reg);
+}
+
+void stmp3xxx_regulator_exit(void)
+{
+	platform_driver_unregister(&stmp3xxx_reg);
+}
+
+postcore_initcall(stmp3xxx_regulator_init);
+module_exit(stmp3xxx_regulator_exit);
+
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 123092d8a984..541908343d97 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -522,6 +522,17 @@ config RTC_DRV_S3C
 	  This driver can also be build as a module. If so, the module
 	  will be called rtc-s3c.
 
+config RTC_DRV_STMP3XXX
+	tristate "Sigmatel STMP3xxx series SoC RTC"
+	depends on ARCH_STMP3XXX && RTC_CLASS
+	help
+	  Say Y here to get support for the real-time clock peripheral
+	  on Sigmatel STMP3xxx series SoCs (tested on STMP3700).
+
+	  This driver can also be build as a module. If so, the module
+	  will be called rtc-stmp3xxx.
+
+
 config RTC_DRV_EP93XX
 	tristate "Cirrus Logic EP93XX"
 	depends on ARCH_EP93XX
@@ -657,9 +668,38 @@ config RTC_DRV_PARISC
 	  firmware calls. If you do not know what you are doing, you should
 	  just say Y.
 
+config RTC_MXC
+	tristate "Freescale MXC Real Time Clock"
+	depends on ARCH_MXC
+	depends on RTC_CLASS
+	help
+	  Support for Freescale RTC MXC
+
+config RTC_DRV_MXC_V2
+	tristate "Freescale MXC Secure Real Time Clock"
+	depends on ARCH_MXC
+	depends on RTC_CLASS
+	help
+	  Support for Freescale SRTC MXC
+
+config RTC_DRV_IMXDI
+	tristate "Freescale IMX DryIce Real Time Clock"
+	depends on ARCH_MXC
+	depends on RTC_CLASS
+	help
+	  Support for Freescale IMX DryIce RTC
+
+config RTC_MC13892
+	tristate "Freescale MC13892 Real Time Clock"
+	depends on ARCH_MXC && MXC_PMIC_MC13892
+	depends on RTC_CLASS
+	help
+	  Support for Freescale MC13892 RTC
+
 config RTC_DRV_PPC
        tristate "PowerPC machine dependent RTC support"
        depends on PPC
+       depends on RTC_CLASS
        help
 	 The PowerPC kernel has machine-specific functions for accessing
 	 the RTC. This exposes that functionality through the generic RTC
@@ -679,4 +719,14 @@ config RTC_DRV_STARFIRE
 	  If you say Y here you will get support for the RTC found on
 	  Starfire systems.
 
+config RTC_DRV_NS9XXX
+	tristate "Digi ns9xxx On-Chip RTC"
+	depends on PROCESSOR_NS9215 || PROCESSOR_NS9360
+	help
+	  Driver for the internal RTC (Realtime Clock) module found on
+	  Digi ns9xxx CPUs.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-ns9xxx.
+
 endif # RTC_CLASS
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 6e79c912bf9e..990f49c743bc 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -47,6 +47,7 @@ obj-$(CONFIG_RTC_DRV_SUN4V)	+= rtc-sun4v.o
 obj-$(CONFIG_RTC_DRV_STARFIRE)	+= rtc-starfire.o
 obj-$(CONFIG_RTC_DRV_MAX6900)	+= rtc-max6900.o
 obj-$(CONFIG_RTC_DRV_MAX6902)	+= rtc-max6902.o
+obj-$(CONFIG_RTC_DRV_NS9XXX)	+= rtc-ns9xxx.o
 obj-$(CONFIG_RTC_DRV_OMAP)	+= rtc-omap.o
 obj-$(CONFIG_RTC_DRV_PCF8563)	+= rtc-pcf8563.o
 obj-$(CONFIG_RTC_DRV_PCF8583)	+= rtc-pcf8583.o
@@ -64,9 +65,14 @@ obj-$(CONFIG_RTC_DRV_S3C)	+= rtc-s3c.o
 obj-$(CONFIG_RTC_DRV_SA1100)	+= rtc-sa1100.o
 obj-$(CONFIG_RTC_DRV_SH)	+= rtc-sh.o
 obj-$(CONFIG_RTC_DRV_STK17TA8)	+= rtc-stk17ta8.o
+obj-$(CONFIG_RTC_DRV_STMP3XXX)	+= rtc-stmp3xxx.o
 obj-$(CONFIG_RTC_DRV_TEST)	+= rtc-test.o
 obj-$(CONFIG_RTC_DRV_TWL4030)	+= rtc-twl4030.o
 obj-$(CONFIG_RTC_DRV_V3020)	+= rtc-v3020.o
 obj-$(CONFIG_RTC_DRV_VR41XX)	+= rtc-vr41xx.o
 obj-$(CONFIG_RTC_DRV_WM8350)	+= rtc-wm8350.o
 obj-$(CONFIG_RTC_DRV_X1205)	+= rtc-x1205.o
+obj-$(CONFIG_RTC_MXC)		+= rtc-mxc.o
+obj-$(CONFIG_RTC_DRV_MXC_V2)	+= rtc-mxc_v2.o
+obj-$(CONFIG_RTC_DRV_IMXDI)	+= rtc-imxdi.o
+obj-$(CONFIG_RTC_MC13892)		+= rtc-mc13892.o
diff --git a/drivers/rtc/rtc-ds1307.c b/drivers/rtc/rtc-ds1307.c
index 162330b9d1dc..1efa70d20968 100644
--- a/drivers/rtc/rtc-ds1307.c
+++ b/drivers/rtc/rtc-ds1307.c
@@ -16,7 +16,7 @@
 #include <linux/string.h>
 #include <linux/rtc.h>
 #include <linux/bcd.h>
-
+#include <linux/platform_device.h>
 
 
 /* We can't determine type by probing, but if we expect pre-Linux code
@@ -748,6 +748,17 @@ read_rtc:
 				bin2bcd(tmp));
 	}
 
+	/*
+	 * Assume that with an IRQ-line, we can wakeup the system by using it.
+	 * But for this purpose it's required to enable the wakeup-support before
+	 * registering the RTC (otherwise it will fail)
+	 * (Luis Galdos)
+	 */
+	if (want_irq) {
+		device_init_wakeup(&client->dev, 1);
+		device_set_wakeup_enable(&client->dev, 0);
+	}
+	
 	ds1307->rtc = rtc_device_register(client->name, &client->dev,
 				&ds13xx_rtc_ops, THIS_MODULE);
 	if (IS_ERR(ds1307->rtc)) {
@@ -811,6 +822,37 @@ static int __devexit ds1307_remove(struct i2c_client *client)
 	return 0;
 }
 
+#if defined(CONFIG_PM)
+static int ds1307_suspend(struct i2c_client *client, pm_message_t state)
+{
+	if (device_may_wakeup(&client->dev)) {
+		enable_irq_wake(client->irq);
+	}
+	
+	return 0;
+}
+
+static int ds1307_resume(struct i2c_client *client)
+{
+	if (device_may_wakeup(&client->dev)) {
+		struct ds1307 *ds1307;
+
+		ds1307 = i2c_get_clientdata(client);
+		
+		disable_irq_wake(client->irq);
+
+		/* Disable the IRQ, then the work function will enable it */
+		disable_irq_nosync(client->irq);
+		schedule_work(&ds1307->work);
+	}
+	
+	return 0;
+}
+#else
+#define ds1307_suspend				NULL
+#define ds1307_resume				NULL
+#endif /* defined(CONFIG_PM) */
+
 static struct i2c_driver ds1307_driver = {
 	.driver = {
 		.name	= "rtc-ds1307",
@@ -818,6 +860,8 @@ static struct i2c_driver ds1307_driver = {
 	},
 	.probe		= ds1307_probe,
 	.remove		= __devexit_p(ds1307_remove),
+	.resume		= ds1307_resume,
+	.suspend        = ds1307_suspend,
 	.id_table	= ds1307_id,
 };
 
diff --git a/drivers/rtc/rtc-imxdi.c b/drivers/rtc/rtc-imxdi.c
new file mode 100644
index 000000000000..b54fb638c840
--- /dev/null
+++ b/drivers/rtc/rtc-imxdi.c
@@ -0,0 +1,580 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/* based on rtc-mc13892.c */
+
+/*
+ * This driver uses the 47-bit 32 kHz counter in the Freescale DryIce block
+ * to implement a Linux RTC. Times and alarms are truncated to seconds.
+ * Since the RTC framework performs API locking via rtc->ops_lock the
+ * only simultaneous accesses we need to deal with is updating DryIce
+ * registers while servicing an alarm.
+ *
+ * Note that reading the DSR (DryIce Status Register) automatically clears
+ * the WCF (Write Complete Flag). All DryIce writes are synchronized to the
+ * LP (Low Power) domain and set the WCF upon completion. Writes to the
+ * DIER (DryIce Interrupt Enable Register) are the only exception. These
+ * occur at normal bus speeds and do not set WCF.  Periodic interrupts are
+ * not supported by the hardware.
+ */
+
+/* #define DEBUG */
+/* #define DI_DEBUG_REGIO */
+
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/workqueue.h>
+
+/* DryIce Register Definitions */
+
+#define DTCMR     0x00           /* Time Counter MSB Reg */
+#define DTCLR     0x04           /* Time Counter LSB Reg */
+
+#define DCAMR     0x08           /* Clock Alarm MSB Reg */
+#define DCALR     0x0c           /* Clock Alarm LSB Reg */
+#define DCAMR_UNSET  0xFFFFFFFF  /* doomsday - 1 sec */
+
+#define DCR       0x10           /* Control Reg */
+#define DCR_TCE   (1 << 3)       /* Time Counter Enable */
+
+#define DSR       0x14           /* Status Reg */
+#define DSR_WBF   (1 << 10)      /* Write Busy Flag */
+#define DSR_WNF   (1 << 9)       /* Write Next Flag */
+#define DSR_WCF   (1 << 8)       /* Write Complete Flag */
+#define DSR_WEF   (1 << 7)       /* Write Error Flag */
+#define DSR_CAF   (1 << 4)       /* Clock Alarm Flag */
+#define DSR_NVF   (1 << 1)       /* Non-Valid Flag */
+#define DSR_SVF   (1 << 0)       /* Security Violation Flag */
+
+#define DIER      0x18           /* Interrupt Enable Reg */
+#define DIER_WNIE (1 << 9)       /* Write Next Interrupt Enable */
+#define DIER_WCIE (1 << 8)       /* Write Complete Interrupt Enable */
+#define DIER_WEIE (1 << 7)       /* Write Error Interrupt Enable */
+#define DIER_CAIE (1 << 4)       /* Clock Alarm Interrupt Enable */
+
+#ifndef DI_DEBUG_REGIO
+/* dryice read register */
+#define di_read(pdata, reg)  __raw_readl((pdata)->ioaddr + (reg))
+
+/* dryice write register */
+#define di_write(pdata, val, reg)  __raw_writel((val), (pdata)->ioaddr + (reg))
+#else
+/* dryice read register - debug version */
+static inline u32 di_read(struct rtc_drv_data *pdata, int reg)
+{
+	u32 val = __raw_readl(pdata->ioaddr + reg);
+	pr_info("di_read(0x%02x) = 0x%08x\n", reg, val);
+	return val;
+}
+
+/* dryice write register - debug version */
+static inline void di_write(struct rtc_drv_data *pdata, u32 val, int reg)
+{
+	printk(KERN_INFO "di_write(0x%08x, 0x%02x)\n", val, reg);
+	__raw_writel(val, pdata->ioaddr + reg);
+}
+#endif
+
+/*
+ * dryice write register with wait and error handling.
+ * all registers, except for DIER, should use this method.
+ */
+#define di_write_wait_err(pdata, val, reg, rc, label) \
+		do { \
+			if (di_write_wait((pdata), (val), (reg))) { \
+				rc = -EIO; \
+				goto label; \
+			} \
+		} while (0)
+
+struct rtc_drv_data {
+	struct platform_device *pdev;  /* pointer to platform dev */
+	struct rtc_device *rtc;        /* pointer to rtc struct */
+	unsigned long baseaddr;        /* physical bass address */
+	void __iomem *ioaddr;          /* virtual base address */
+	int size;                      /* size of register region */
+	int irq;                       /* dryice normal irq */
+	struct clk *clk;               /* dryice clock control */
+	u32 dsr;                       /* copy of dsr reg from isr */
+	spinlock_t irq_lock;           /* irq resource lock */
+	wait_queue_head_t write_wait;  /* write-complete queue */
+	struct mutex write_mutex;      /* force reg writes to be sequential */
+	struct work_struct work;       /* schedule alarm work */
+};
+
+/*
+ * enable a dryice interrupt
+ */
+static inline void di_int_enable(struct rtc_drv_data *pdata, u32 intr)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&pdata->irq_lock, flags);
+	di_write(pdata, di_read(pdata, DIER) | intr, DIER);
+	spin_unlock_irqrestore(&pdata->irq_lock, flags);
+}
+
+/*
+ * disable a dryice interrupt
+ */
+static inline void di_int_disable(struct rtc_drv_data *pdata, u32 intr)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&pdata->irq_lock, flags);
+	di_write(pdata, di_read(pdata, DIER) & ~intr, DIER);
+	spin_unlock_irqrestore(&pdata->irq_lock, flags);
+}
+
+/*
+ * This function attempts to clear the dryice write-error flag.
+ *
+ * A dryice write error is similar to a bus fault and should not occur in
+ * normal operation.  Clearing the flag requires another write, so the root
+ * cause of the problem may need to be fixed before the flag can be cleared.
+ */
+static void clear_write_error(struct rtc_drv_data *pdata)
+{
+	int cnt;
+
+	dev_warn(&pdata->pdev->dev, "WARNING: Register write error!\n");
+
+	for (;;) {
+		/* clear the write error flag */
+		di_write(pdata, DSR_WEF, DSR);
+
+		/* wait for it to take effect */
+		for (cnt = 0; cnt < 100; cnt++) {
+			if ((di_read(pdata, DSR) & DSR_WEF) == 0)
+				return;
+			udelay(10);
+		}
+		dev_err(&pdata->pdev->dev,
+			"ERROR: Cannot clear write-error flag!\n");
+	}
+}
+
+/*
+ * Write a dryice register and wait until it completes.
+ *
+ * This function uses interrupts to determine when the
+ * write has completed.
+ */
+static int di_write_wait(struct rtc_drv_data *pdata, u32 val, int reg)
+{
+	int ret;
+	int rc = 0;
+
+	/* serialize register writes */
+	mutex_lock(&pdata->write_mutex);
+
+	/* enable the write-complete interrupt */
+	di_int_enable(pdata, DIER_WCIE);
+
+	pdata->dsr = 0;
+
+	/* do the register write */
+	di_write(pdata, val, reg);
+
+	/* wait for the write to finish */
+	ret = wait_event_interruptible_timeout(pdata->write_wait,
+					       pdata->dsr & (DSR_WCF | DSR_WEF),
+					       1 * HZ);
+	if (ret == 0)
+		dev_warn(&pdata->pdev->dev, "Write-wait timeout\n");
+
+	/* check for write error */
+	if (pdata->dsr & DSR_WEF) {
+		clear_write_error(pdata);
+		rc = -EIO;
+	}
+	mutex_unlock(&pdata->write_mutex);
+	return rc;
+}
+
+/*
+ * rtc device ioctl
+ *
+ * The rtc framework handles the basic rtc ioctls on behalf
+ * of the driver by calling the functions registered in the
+ * rtc_ops structure.
+ */
+static int dryice_rtc_ioctl(struct device *dev, unsigned int cmd,
+			    unsigned long arg)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+
+	dev_dbg(dev, "%s(0x%x)\n", __func__, cmd);
+	switch (cmd) {
+	case RTC_AIE_OFF:  /* alarm disable */
+		di_int_disable(pdata, DIER_CAIE);
+		return 0;
+
+	case RTC_AIE_ON:  /* alarm enable */
+		di_int_enable(pdata, DIER_CAIE);
+		return 0;
+	}
+	return -ENOIOCTLCMD;
+}
+
+/*
+ * read the seconds portion of the current time from the dryice time counter
+ */
+static int dryice_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	unsigned long now;
+
+	dev_dbg(dev, "%s\n", __func__);
+	now = di_read(pdata, DTCMR);
+	rtc_time_to_tm(now, tm);
+
+	return 0;
+}
+
+/*
+ * set the seconds portion of dryice time counter and clear the
+ * fractional part.
+ */
+static int dryice_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	unsigned long now;
+	int rc;
+
+	dev_dbg(dev, "%s\n", __func__);
+	rc = rtc_tm_to_time(tm, &now);
+	if (rc == 0) {
+		/* zero the fractional part first */
+		di_write_wait_err(pdata, 0, DTCLR, rc, err);
+		di_write_wait_err(pdata, now, DTCMR, rc, err);
+	}
+err:
+	return rc;
+}
+
+/*
+ * read the seconds portion of the alarm register.
+ * the fractional part of the alarm register is always zero.
+ */
+static int dryice_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	u32 dcamr;
+
+	dev_dbg(dev, "%s\n", __func__);
+	dcamr = di_read(pdata, DCAMR);
+	rtc_time_to_tm(dcamr, &alarm->time);
+
+	/* alarm is enabled if the interrupt is enabled */
+	alarm->enabled = (di_read(pdata, DIER) & DIER_CAIE) != 0;
+
+	/* don't allow the DSR read to mess up DSR_WCF */
+	mutex_lock(&pdata->write_mutex);
+
+	/* alarm is pending if the alarm flag is set */
+	alarm->pending = (di_read(pdata, DSR) & DSR_CAF) != 0;
+
+	mutex_unlock(&pdata->write_mutex);
+
+	return 0;
+}
+
+/*
+ * set the seconds portion of dryice alarm register
+ */
+static int dryice_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	unsigned long now;
+	unsigned long alarm_time;
+	int rc;
+
+	dev_dbg(dev, "%s\n", __func__);
+	rc = rtc_tm_to_time(&alarm->time, &alarm_time);
+	if (rc)
+		return rc;
+
+	/* don't allow setting alarm in the past */
+	now = di_read(pdata, DTCMR);
+	if (alarm_time < now)
+		return -EINVAL;
+
+	/* write the new alarm time */
+	di_write_wait_err(pdata, (u32)alarm_time, DCAMR, rc, err);
+
+	if (alarm->enabled)
+		di_int_enable(pdata, DIER_CAIE);  /* enable alarm intr */
+	else
+		di_int_disable(pdata, DIER_CAIE); /* disable alarm intr */
+err:
+	return rc;
+}
+
+static struct rtc_class_ops dryice_rtc_ops = {
+	.ioctl = dryice_rtc_ioctl,
+	.read_time = dryice_rtc_read_time,
+	.set_time = dryice_rtc_set_time,
+	.read_alarm = dryice_rtc_read_alarm,
+	.set_alarm = dryice_rtc_set_alarm,
+};
+
+/*
+ * dryice "normal" interrupt handler
+ */
+static irqreturn_t dryice_norm_irq(int irq, void *dev_id)
+{
+	struct rtc_drv_data *pdata = dev_id;
+	u32 dsr, dier;
+	irqreturn_t rc = IRQ_NONE;
+
+	dier = di_read(pdata, DIER);
+
+	/* handle write complete and write error cases */
+	if ((dier & DIER_WCIE)) {
+		/*If the write wait queue is empty then there is no pending
+		   operations. It means the interrupt is for DryIce -Security.
+		   IRQ must be returned as none.*/
+		if (list_empty_careful(&pdata->write_wait.task_list))
+			return rc;
+
+		/* DSR_WCF clears itself on DSR read */
+	    dsr = di_read(pdata, DSR);
+		if ((dsr & (DSR_WCF | DSR_WEF))) {
+			/* mask the interrupt */
+			di_int_disable(pdata, DIER_WCIE);
+
+			/* save the dsr value for the wait queue */
+			pdata->dsr |= dsr;
+
+			wake_up_interruptible(&pdata->write_wait);
+			rc = IRQ_HANDLED;
+		}
+	}
+
+	/* handle the alarm case */
+	if ((dier & DIER_CAIE)) {
+		/* DSR_WCF clears itself on DSR read */
+	    dsr = di_read(pdata, DSR);
+		if (dsr & DSR_CAF) {
+			/* mask the interrupt */
+			di_int_disable(pdata, DIER_CAIE);
+
+			/* finish alarm in user context */
+			schedule_work(&pdata->work);
+			rc = IRQ_HANDLED;
+		}
+	}
+	return rc;
+}
+
+/*
+ * post the alarm event from user context so it can sleep
+ * on the write completion.
+ */
+static void dryice_work(struct work_struct *work)
+{
+	struct rtc_drv_data *pdata = container_of(work, struct rtc_drv_data,
+						  work);
+	int rc;
+
+	/* dismiss the interrupt (ignore error) */
+	di_write_wait_err(pdata, DSR_CAF, DSR, rc, err);
+err:
+	/*
+	 * pass the alarm event to the rtc framework. note that
+	 * rtc_update_irq expects to be called with interrupts off.
+	 */
+	local_irq_disable();
+	rtc_update_irq(pdata->rtc, 1, RTC_AF | RTC_IRQF);
+	local_irq_enable();
+}
+
+/*
+ * probe for dryice rtc device
+ */
+static int dryice_rtc_probe(struct platform_device *pdev)
+{
+	struct rtc_device *rtc;
+	struct resource *res;
+	struct rtc_drv_data *pdata = NULL;
+	void __iomem *ioaddr = NULL;
+	int rc = 0;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	pdata->pdev = pdev;
+	pdata->irq = -1;
+	pdata->size = res->end - res->start + 1;
+
+	if (!request_mem_region(res->start, pdata->size, pdev->name)) {
+		rc = -EBUSY;
+		goto err;
+	}
+	pdata->baseaddr = res->start;
+	ioaddr = ioremap(pdata->baseaddr, pdata->size);
+	if (!ioaddr) {
+		rc = -ENOMEM;
+		goto err;
+	}
+	pdata->ioaddr = ioaddr;
+	pdata->irq = platform_get_irq(pdev, 0);
+
+	init_waitqueue_head(&pdata->write_wait);
+
+	INIT_WORK(&pdata->work, dryice_work);
+
+	mutex_init(&pdata->write_mutex);
+
+	pdata->clk = clk_get(NULL, "dryice_clk");
+	clk_enable(pdata->clk);
+
+	if (pdata->irq >= 0) {
+		if (request_irq(pdata->irq, dryice_norm_irq, IRQF_SHARED,
+				pdev->name, pdata) < 0) {
+			dev_warn(&pdev->dev, "interrupt not available.\n");
+			pdata->irq = -1;
+			goto err;
+		}
+	}
+
+	/*
+	 * Initialize dryice hardware
+	 */
+
+	/* put dryice into valid state */
+	if (di_read(pdata, DSR) & DSR_NVF)
+		di_write_wait_err(pdata, DSR_NVF | DSR_SVF, DSR, rc, err);
+
+	/* mask alarm interrupt */
+	di_int_disable(pdata, DIER_CAIE);
+
+	/* initialize alarm */
+	di_write_wait_err(pdata, DCAMR_UNSET, DCAMR, rc, err);
+	di_write_wait_err(pdata, 0, DCALR, rc, err);
+
+	/* clear alarm flag */
+	if (di_read(pdata, DSR) & DSR_CAF)
+		di_write_wait_err(pdata, DSR_CAF, DSR, rc, err);
+
+	/* the timer won't count if it has never been written to */
+	if (!di_read(pdata, DTCMR))
+		di_write_wait_err(pdata, 0, DTCMR, rc, err);
+
+	/* start keeping time */
+	if (!(di_read(pdata, DCR) & DCR_TCE))
+		di_write_wait_err(pdata, di_read(pdata, DCR) | DCR_TCE, DCR,
+				  rc, err);
+
+	rtc = rtc_device_register(pdev->name, &pdev->dev,
+				  &dryice_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc)) {
+		rc = PTR_ERR(rtc);
+		goto err;
+	}
+	pdata->rtc = rtc;
+	platform_set_drvdata(pdev, pdata);
+
+	return 0;
+err:
+	if (pdata->rtc)
+		rtc_device_unregister(pdata->rtc);
+
+	if (pdata->irq >= 0)
+		free_irq(pdata->irq, pdata);
+
+	if (pdata->clk) {
+		clk_disable(pdata->clk);
+		clk_put(pdata->clk);
+	}
+
+	if (pdata->ioaddr)
+		iounmap(pdata->ioaddr);
+
+	if (pdata->baseaddr)
+		release_mem_region(pdata->baseaddr, pdata->size);
+
+	kfree(pdata);
+
+	return rc;
+}
+
+static int __exit dryice_rtc_remove(struct platform_device *pdev)
+{
+	struct rtc_drv_data *pdata = platform_get_drvdata(pdev);
+
+	flush_scheduled_work();
+
+	if (pdata->rtc)
+		rtc_device_unregister(pdata->rtc);
+
+	/* mask alarm interrupt */
+	di_int_disable(pdata, DIER_CAIE);
+
+	if (pdata->irq >= 0)
+		free_irq(pdata->irq, pdata);
+
+	if (pdata->clk) {
+		clk_disable(pdata->clk);
+		clk_put(pdata->clk);
+	}
+
+	if (pdata->ioaddr)
+		iounmap(pdata->ioaddr);
+
+	if (pdata->baseaddr)
+		release_mem_region(pdata->baseaddr, pdata->size);
+
+	kfree(pdata);
+
+	return 0;
+}
+
+static struct platform_driver dryice_rtc_driver = {
+	.driver = {
+		   .name = "imxdi_rtc",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = dryice_rtc_probe,
+	.remove = __exit_p(dryice_rtc_remove),
+};
+
+static int __init dryice_rtc_init(void)
+{
+	pr_info("IMXDI Realtime Clock Driver (RTC)\n");
+	return platform_driver_register(&dryice_rtc_driver);
+}
+
+static void __exit dryice_rtc_exit(void)
+{
+	platform_driver_unregister(&dryice_rtc_driver);
+}
+
+module_init(dryice_rtc_init);
+module_exit(dryice_rtc_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IMXDI Realtime Clock Driver (RTC)");
+MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-mc13892.c b/drivers/rtc/rtc-mc13892.c
new file mode 100644
index 000000000000..e579c1853a26
--- /dev/null
+++ b/drivers/rtc/rtc-mc13892.c
@@ -0,0 +1,256 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/rtc.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include <linux/pmic_status.h>
+#include <linux/pmic_external.h>
+
+#define RTC_TIME_LSH		0
+#define RTC_DAY_LSH		0
+#define RTCALARM_TIME_LSH	0
+#define RTCALARM_DAY_LSH	0
+
+#define RTC_TIME_WID		17
+#define RTC_DAY_WID		15
+#define RTCALARM_TIME_WID	17
+#define RTCALARM_DAY_WID	15
+
+static unsigned long rtc_status;
+
+static int mxc_rtc_open(struct device *dev)
+{
+	if (test_and_set_bit(1, &rtc_status))
+		return -EBUSY;
+	return 0;
+}
+
+static void mxc_rtc_release(struct device *dev)
+{
+	clear_bit(1, &rtc_status);
+}
+
+static int mxc_rtc_ioctl(struct device *dev, unsigned int cmd,
+			 unsigned long arg)
+{
+	switch (cmd) {
+	case RTC_AIE_OFF:
+		pr_debug("alarm off\n");
+		CHECK_ERROR(pmic_write_reg(REG_RTC_ALARM, 0x100000, 0x100000));
+		return 0;
+	case RTC_AIE_ON:
+		pr_debug("alarm on\n");
+		CHECK_ERROR(pmic_write_reg(REG_RTC_ALARM, 0, 0x100000));
+		return 0;
+	}
+
+	return -ENOIOCTLCMD;
+}
+
+static int mxc_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	unsigned int tod_reg_val = 0;
+	unsigned int day_reg_val = 0, day_reg_val2;
+	unsigned int mask, value;
+	unsigned long time;
+
+	do {
+		mask = BITFMASK(RTC_DAY);
+		CHECK_ERROR(pmic_read_reg(REG_RTC_DAY, &value, mask));
+		day_reg_val = BITFEXT(value, RTC_DAY);
+
+		mask = BITFMASK(RTC_TIME);
+		CHECK_ERROR(pmic_read_reg(REG_RTC_TIME, &value, mask));
+		tod_reg_val = BITFEXT(value, RTC_TIME);
+
+		mask = BITFMASK(RTC_DAY);
+		CHECK_ERROR(pmic_read_reg(REG_RTC_DAY, &value, mask));
+		day_reg_val2 = BITFEXT(value, RTC_DAY);
+	} while (day_reg_val != day_reg_val2);
+
+	time = (unsigned long)((unsigned long)(tod_reg_val &
+					       0x0001FFFF) +
+			       (unsigned long)(day_reg_val * 86400));
+
+	rtc_time_to_tm(time, tm);
+
+	return 0;
+}
+
+static int mxc_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	unsigned int tod_reg_val = 0;
+	unsigned int day_reg_val, day_reg_val2 = 0;
+	unsigned int mask, value;
+	unsigned long time;
+
+	if (rtc_valid_tm(tm))
+		return -1;
+
+	rtc_tm_to_time(tm, &time);
+
+	tod_reg_val = time % 86400;
+	day_reg_val = time / 86400;
+
+	do {
+		mask = BITFMASK(RTC_DAY);
+		value = BITFVAL(RTC_DAY, day_reg_val);
+		CHECK_ERROR(pmic_write_reg(REG_RTC_DAY, value, mask));
+
+		mask = BITFMASK(RTC_TIME);
+		value = BITFVAL(RTC_TIME, tod_reg_val);
+		CHECK_ERROR(pmic_write_reg(REG_RTC_TIME, value, mask));
+
+		mask = BITFMASK(RTC_DAY);
+		CHECK_ERROR(pmic_read_reg(REG_RTC_DAY, &value, mask));
+		day_reg_val2 = BITFEXT(value, RTC_DAY);
+	} while (day_reg_val != day_reg_val2);
+
+	return 0;
+}
+
+static int mxc_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	unsigned int tod_reg_val = 0;
+	unsigned int day_reg_val = 0;
+	unsigned int mask, value;
+	unsigned long time;
+
+	mask = BITFMASK(RTCALARM_TIME);
+	CHECK_ERROR(pmic_read_reg(REG_RTC_ALARM, &value, mask));
+	tod_reg_val = BITFEXT(value, RTCALARM_TIME);
+
+	mask = BITFMASK(RTCALARM_DAY);
+	CHECK_ERROR(pmic_read_reg(REG_RTC_DAY_ALARM, &value, mask));
+	day_reg_val = BITFEXT(value, RTCALARM_DAY);
+
+	time = (unsigned long)((unsigned long)(tod_reg_val &
+					       0x0001FFFF) +
+			       (unsigned long)(day_reg_val * 86400));
+	rtc_time_to_tm(time, &(alrm->time));
+
+	return 0;
+}
+
+static int mxc_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	unsigned int tod_reg_val = 0;
+	unsigned int day_reg_val = 0;
+	unsigned int mask, value;
+	unsigned long time;
+
+	if (rtc_valid_tm(&alrm->time))
+		return -1;
+
+	rtc_tm_to_time(&alrm->time, &time);
+
+	tod_reg_val = time % 86400;
+	day_reg_val = time / 86400;
+
+	mask = BITFMASK(RTCALARM_TIME);
+	value = BITFVAL(RTCALARM_TIME, tod_reg_val);
+	CHECK_ERROR(pmic_write_reg(REG_RTC_ALARM, value, mask));
+
+	mask = BITFMASK(RTCALARM_DAY);
+	value = BITFVAL(RTCALARM_DAY, day_reg_val);
+	CHECK_ERROR(pmic_write_reg(REG_RTC_DAY_ALARM, value, mask));
+
+	return 0;
+}
+
+struct rtc_drv_data {
+	struct rtc_device *rtc;
+	pmic_event_callback_t event;
+};
+
+static struct rtc_class_ops mxc_rtc_ops = {
+	.open = mxc_rtc_open,
+	.release = mxc_rtc_release,
+	.ioctl = mxc_rtc_ioctl,
+	.read_time = mxc_rtc_read_time,
+	.set_time = mxc_rtc_set_time,
+	.read_alarm = mxc_rtc_read_alarm,
+	.set_alarm = mxc_rtc_set_alarm,
+};
+
+static void mxc_rtc_alarm_int(void *data)
+{
+	struct rtc_drv_data *pdata = data;
+
+	rtc_update_irq(pdata->rtc, 1, RTC_AF | RTC_IRQF);
+}
+
+static int mxc_rtc_probe(struct platform_device *pdev)
+{
+	struct rtc_drv_data *pdata = NULL;
+
+	printk(KERN_INFO "mc13892 rtc probe start\n");
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+
+	if (!pdata)
+		return -ENOMEM;
+
+	pdata->event.func = mxc_rtc_alarm_int;
+	pdata->event.param = pdata;
+	CHECK_ERROR(pmic_event_subscribe(EVENT_TODAI, pdata->event));
+
+	device_init_wakeup(&pdev->dev, 1);
+	pdata->rtc = rtc_device_register(pdev->name, &pdev->dev,
+					 &mxc_rtc_ops, THIS_MODULE);
+
+	platform_set_drvdata(pdev, pdata);
+	if (IS_ERR(pdata->rtc))
+		return -1;
+
+	printk(KERN_INFO "mc13892 rtc probe succeed\n");
+	return 0;
+}
+
+static int __exit mxc_rtc_remove(struct platform_device *pdev)
+{
+	struct rtc_drv_data *pdata = platform_get_drvdata(pdev);
+
+	rtc_device_unregister(pdata->rtc);
+	CHECK_ERROR(pmic_event_unsubscribe(EVENT_TODAI, pdata->event));
+
+	return 0;
+}
+
+static struct platform_driver mxc_rtc_driver = {
+	.driver = {
+		   .name = "pmic_rtc",
+		   },
+	.probe = mxc_rtc_probe,
+	.remove = __exit_p(mxc_rtc_remove),
+};
+
+static int __init mxc_rtc_init(void)
+{
+	return platform_driver_register(&mxc_rtc_driver);
+}
+
+static void __exit mxc_rtc_exit(void)
+{
+	platform_driver_unregister(&mxc_rtc_driver);
+
+}
+
+module_init(mxc_rtc_init);
+module_exit(mxc_rtc_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MC13892 Realtime Clock Driver (RTC)");
+MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-mxc.c b/drivers/rtc/rtc-mxc.c
new file mode 100644
index 000000000000..b6b168d8ac6b
--- /dev/null
+++ b/drivers/rtc/rtc-mxc.c
@@ -0,0 +1,806 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/*
+ * Implementation based on rtc-ds1553.c
+ */
+
+/*!
+ * @defgroup RTC Real Time Clock (RTC) Driver
+ */
+/*!
+ * @file rtc-mxc.c
+ * @brief Real Time Clock interface
+ *
+ * This file contains Real Time Clock interface for Linux.
+ *
+ * @ingroup RTC
+ */
+
+#include <linux/rtc.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/uaccess.h>
+
+#include <mach/hardware.h>
+#define RTC_INPUT_CLK_32768HZ	(0x00 << 5)
+#define RTC_INPUT_CLK_32000HZ	(0x01 << 5)
+#define RTC_INPUT_CLK_38400HZ	(0x02 << 5)
+
+#define RTC_SW_BIT      (1 << 0)
+#define RTC_ALM_BIT     (1 << 2)
+#define RTC_1HZ_BIT     (1 << 4)
+#define RTC_2HZ_BIT     (1 << 7)
+#define RTC_SAM0_BIT    (1 << 8)
+#define RTC_SAM1_BIT    (1 << 9)
+#define RTC_SAM2_BIT    (1 << 10)
+#define RTC_SAM3_BIT    (1 << 11)
+#define RTC_SAM4_BIT    (1 << 12)
+#define RTC_SAM5_BIT    (1 << 13)
+#define RTC_SAM6_BIT    (1 << 14)
+#define RTC_SAM7_BIT    (1 << 15)
+#define PIT_ALL_ON      (RTC_2HZ_BIT | RTC_SAM0_BIT | RTC_SAM1_BIT | \
+                         RTC_SAM2_BIT | RTC_SAM3_BIT | RTC_SAM4_BIT | \
+                         RTC_SAM5_BIT | RTC_SAM6_BIT | RTC_SAM7_BIT)
+
+#define RTC_ENABLE_BIT  (1 << 7)
+
+#define MAX_PIE_NUM     9
+#define MAX_PIE_FREQ    512
+const u32 PIE_BIT_DEF[MAX_PIE_NUM][2] = {
+	{2, RTC_2HZ_BIT},
+	{4, RTC_SAM0_BIT},
+	{8, RTC_SAM1_BIT},
+	{16, RTC_SAM2_BIT},
+	{32, RTC_SAM3_BIT},
+	{64, RTC_SAM4_BIT},
+	{128, RTC_SAM5_BIT},
+	{256, RTC_SAM6_BIT},
+	{MAX_PIE_FREQ, RTC_SAM7_BIT},
+};
+
+/* Those are the bits from a classic RTC we want to mimic */
+#define RTC_IRQF                0x80	/* any of the following 3 is active */
+#define RTC_PF                  0x40	/* Periodic interrupt */
+#define RTC_AF                  0x20	/* Alarm interrupt */
+#define RTC_UF                  0x10	/* Update interrupt for 1Hz RTC */
+
+#define MXC_RTC_TIME            0
+#define MXC_RTC_ALARM           1
+
+#define RTC_HOURMIN    0x00	/*  32bit rtc hour/min counter reg */
+#define RTC_SECOND     0x04	/*  32bit rtc seconds counter reg */
+#define RTC_ALRM_HM    0x08	/*  32bit rtc alarm hour/min reg */
+#define RTC_ALRM_SEC   0x0C	/*  32bit rtc alarm seconds reg */
+#define RTC_RTCCTL     0x10	/*  32bit rtc control reg */
+#define RTC_RTCISR     0x14	/*  32bit rtc interrupt status reg */
+#define RTC_RTCIENR    0x18	/*  32bit rtc interrupt enable reg */
+#define RTC_STPWCH     0x1C	/*  32bit rtc stopwatch min reg */
+#define RTC_DAYR       0x20	/*  32bit rtc days counter reg */
+#define RTC_DAYALARM   0x24	/*  32bit rtc day alarm reg */
+#define RTC_TEST1      0x28	/*  32bit rtc test reg 1 */
+#define RTC_TEST2      0x2C	/*  32bit rtc test reg 2 */
+#define RTC_TEST3      0x30	/*  32bit rtc test reg 3 */
+
+struct rtc_plat_data {
+	struct rtc_device *rtc;
+	void __iomem *ioaddr;
+	unsigned long baseaddr;
+	int irq;
+	struct clk *clk;
+	unsigned int irqen;
+	int alrm_sec;
+	int alrm_min;
+	int alrm_hour;
+	int alrm_mday;
+};
+
+/*!
+ * @defgroup RTC Real Time Clock (RTC) Driver
+ */
+/*!
+ * @file rtc-mxc.c
+ * @brief Real Time Clock interface
+ *
+ * This file contains Real Time Clock interface for Linux.
+ *
+ * @ingroup RTC
+ */
+
+#if defined(CONFIG_MXC_PMIC_SC55112_RTC) || defined(CONFIG_MXC_MC13783_RTC) ||\
+    defined(CONFIG_MXC_MC9SDZ60_RTC)
+#include <linux/pmic_rtc.h>
+#else
+#define pmic_rtc_get_time(args)	MXC_EXTERNAL_RTC_NONE
+#define pmic_rtc_set_time(args)	MXC_EXTERNAL_RTC_NONE
+#define pmic_rtc_loaded()		0
+#endif
+
+#define RTC_VERSION		"1.0"
+#define MXC_EXTERNAL_RTC_OK	0
+#define MXC_EXTERNAL_RTC_ERR	-1
+#define MXC_EXTERNAL_RTC_NONE	-2
+
+/*!
+ * This function reads the RTC value from some external source.
+ *
+ * @param  second       pointer to the returned value in second
+ *
+ * @return 0 if successful; non-zero otherwise
+ */
+int get_ext_rtc_time(u32 * second)
+{
+	int ret = 0;
+	struct timeval tmp;
+	if (!pmic_rtc_loaded()) {
+		return MXC_EXTERNAL_RTC_NONE;
+	}
+
+	ret = pmic_rtc_get_time(&tmp);
+
+	if (0 == ret)
+		*second = tmp.tv_sec;
+	else
+		ret = MXC_EXTERNAL_RTC_ERR;
+
+	return ret;
+}
+
+/*!
+ * This function sets external RTC
+ *
+ * @param  second       value in second to be set to external RTC
+ *
+ * @return 0 if successful; non-zero otherwise
+ */
+int set_ext_rtc_time(u32 second)
+{
+	int ret = 0;
+	struct timeval tmp;
+
+	if (!pmic_rtc_loaded()) {
+		return MXC_EXTERNAL_RTC_NONE;
+	}
+
+	tmp.tv_sec = second;
+
+	ret = pmic_rtc_set_time(&tmp);
+
+	if (0 != ret)
+		ret = MXC_EXTERNAL_RTC_ERR;
+
+	return ret;
+}
+
+static u32 rtc_freq = 2;	/* minimun value for PIE */
+static unsigned long rtc_status;
+
+static struct rtc_time g_rtc_alarm = {
+	.tm_year = 0,
+	.tm_mon = 0,
+	.tm_mday = 0,
+	.tm_hour = 0,
+	.tm_mon = 0,
+	.tm_sec = 0,
+};
+
+static DEFINE_SPINLOCK(rtc_lock);
+
+/*!
+ * This function is used to obtain the RTC time or the alarm value in
+ * second.
+ *
+ * @param  time_alarm   use MXC_RTC_TIME for RTC time value; MXC_RTC_ALARM for alarm value
+ *
+ * @return The RTC time or alarm time in second.
+ */
+static u32 get_alarm_or_time(struct device *dev, int time_alarm)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	u32 day, hr, min, sec, hr_min;
+	if (time_alarm == MXC_RTC_TIME) {
+		day = readw(ioaddr + RTC_DAYR);
+		hr_min = readw(ioaddr + RTC_HOURMIN);
+		sec = readw(ioaddr + RTC_SECOND);
+	} else if (time_alarm == MXC_RTC_ALARM) {
+		day = readw(ioaddr + RTC_DAYALARM);
+		hr_min = (0x0000FFFF) & readw(ioaddr + RTC_ALRM_HM);
+		sec = readw(ioaddr + RTC_ALRM_SEC);
+	} else {
+		panic("wrong value for time_alarm=%d\n", time_alarm);
+	}
+
+	hr = hr_min >> 8;
+	min = hr_min & 0x00FF;
+
+	return ((((day * 24 + hr) * 60) + min) * 60 + sec);
+}
+
+/*!
+ * This function sets the RTC alarm value or the time value.
+ *
+ * @param  time_alarm   the new alarm value to be updated in the RTC
+ * @param  time         use MXC_RTC_TIME for RTC time value; MXC_RTC_ALARM for alarm value
+ */
+static void set_alarm_or_time(struct device *dev, int time_alarm, u32 time)
+{
+	u32 day, hr, min, sec, temp;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	day = time / 86400;
+	time -= day * 86400;
+	/* time is within a day now */
+	hr = time / 3600;
+	time -= hr * 3600;
+	/* time is within an hour now */
+	min = time / 60;
+	sec = time - min * 60;
+
+	temp = (hr << 8) + min;
+
+	if (time_alarm == MXC_RTC_TIME) {
+		writew(day, ioaddr + RTC_DAYR);
+		writew(sec, ioaddr + RTC_SECOND);
+		writew(temp, ioaddr + RTC_HOURMIN);
+	} else if (time_alarm == MXC_RTC_ALARM) {
+		writew(day, ioaddr + RTC_DAYALARM);
+		writew(sec, ioaddr + RTC_ALRM_SEC);
+		writew(temp, ioaddr + RTC_ALRM_HM);
+	} else {
+		panic("wrong value for time_alarm=%d\n", time_alarm);
+	}
+}
+
+/*!
+ * This function updates the RTC alarm registers and then clears all the
+ * interrupt status bits.
+ *
+ * @param  alrm         the new alarm value to be updated in the RTC
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+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;
+
+	now = get_alarm_or_time(dev, MXC_RTC_TIME);
+	rtc_time_to_tm(now, &now_tm);
+	alarm_tm.tm_year = now_tm.tm_year;
+	alarm_tm.tm_mon = now_tm.tm_mon;
+	alarm_tm.tm_mday = now_tm.tm_mday;
+	alarm_tm.tm_hour = alrm->tm_hour;
+	alarm_tm.tm_min = alrm->tm_min;
+	alarm_tm.tm_sec = alrm->tm_sec;
+	rtc_tm_to_time(&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;
+}
+
+/*!
+ * This function is the RTC interrupt service routine.
+ *
+ * @param  irq          RTC IRQ number
+ * @param  dev_id       device ID which is not used
+ *
+ * @return IRQ_HANDLED as defined in the include/linux/interrupt.h file.
+ */
+static irqreturn_t mxc_rtc_interrupt(int irq, void *dev_id)
+{
+	struct platform_device *pdev = dev_id;
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	u32 status;
+	u32 events = 0;
+	spin_lock(&rtc_lock);
+	status = readw(ioaddr + RTC_RTCISR) & readw(ioaddr + RTC_RTCIENR);
+	/* 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) {
+		events |= (RTC_AF | RTC_IRQF);
+	}
+	if (status & RTC_1HZ_BIT) {
+		events |= (RTC_UF | RTC_IRQF);
+	}
+	if (status & PIT_ALL_ON) {
+		events |= (RTC_PF | RTC_IRQF);
+	}
+
+	if ((status & RTC_ALM_BIT) && rtc_valid_tm(&g_rtc_alarm)) {
+		rtc_update_alarm(&pdev->dev, &g_rtc_alarm);
+	}
+
+	spin_unlock(&rtc_lock);
+	rtc_update_irq(pdata->rtc, 1, events);
+	return IRQ_HANDLED;
+}
+
+/*!
+ * This function is used to open the RTC driver by registering the RTC
+ * interrupt service routine.
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int mxc_rtc_open(struct device *dev)
+{
+	if (test_and_set_bit(1, &rtc_status))
+		return -EBUSY;
+	return 0;
+}
+
+/*!
+ * clear all interrupts and release the IRQ
+ */
+static void mxc_rtc_release(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	void __iomem *ioaddr = pdata->ioaddr;
+
+	spin_lock_irq(&rtc_lock);
+	writew(0, ioaddr + RTC_RTCIENR);	/* Disable all rtc interrupts */
+	writew(0xFFFFFFFF, ioaddr + RTC_RTCISR);	/* Clear all interrupt status */
+	spin_unlock_irq(&rtc_lock);
+	rtc_status = 0;
+}
+
+/*!
+ * This function is used to support some ioctl calls directly.
+ * Other ioctl calls are supported indirectly through the
+ * arm/common/rtctime.c file.
+ *
+ * @param  cmd          ioctl command as defined in include/linux/rtc.h
+ * @param  arg          value for the ioctl command
+ *
+ * @return  0 if successful or negative value otherwise.
+ */
+static int mxc_rtc_ioctl(struct device *dev, unsigned int cmd,
+			 unsigned long arg)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	int i;
+	switch (cmd) {
+	case RTC_PIE_OFF:
+		writew((readw(ioaddr + RTC_RTCIENR) & ~PIT_ALL_ON),
+		       ioaddr + RTC_RTCIENR);
+		return 0;
+	case RTC_IRQP_SET:
+		if (arg < 2 || arg > MAX_PIE_FREQ || (arg % 2) != 0)
+			return -EINVAL;	/* Also make sure a power of 2Hz */
+		if ((arg > 64) && (!capable(CAP_SYS_RESOURCE)))
+			return -EACCES;
+		rtc_freq = arg;
+		return 0;
+	case RTC_IRQP_READ:
+		return put_user(rtc_freq, (u32 *) arg);
+	case RTC_PIE_ON:
+		for (i = 0; i < MAX_PIE_NUM; i++) {
+			if (PIE_BIT_DEF[i][0] == rtc_freq) {
+				break;
+			}
+		}
+		if (i == MAX_PIE_NUM) {
+			return -EACCES;
+		}
+		spin_lock_irq(&rtc_lock);
+		writew((readw(ioaddr + RTC_RTCIENR) | PIE_BIT_DEF[i][1]),
+		       ioaddr + RTC_RTCIENR);
+		spin_unlock_irq(&rtc_lock);
+		return 0;
+	case RTC_AIE_OFF:
+		spin_lock_irq(&rtc_lock);
+		writew((readw(ioaddr + RTC_RTCIENR) & ~RTC_ALM_BIT),
+		       ioaddr + RTC_RTCIENR);
+		spin_unlock_irq(&rtc_lock);
+		return 0;
+
+	case RTC_AIE_ON:
+		spin_lock_irq(&rtc_lock);
+		writew((readw(ioaddr + RTC_RTCIENR) | RTC_ALM_BIT),
+		       ioaddr + RTC_RTCIENR);
+		spin_unlock_irq(&rtc_lock);
+		return 0;
+
+	case RTC_UIE_OFF:	/* UIE is for the 1Hz interrupt */
+		spin_lock_irq(&rtc_lock);
+		writew((readw(ioaddr + RTC_RTCIENR) & ~RTC_1HZ_BIT),
+		       ioaddr + RTC_RTCIENR);
+		spin_unlock_irq(&rtc_lock);
+		return 0;
+
+	case RTC_UIE_ON:
+		spin_lock_irq(&rtc_lock);
+		writew((readw(ioaddr + RTC_RTCIENR) | RTC_1HZ_BIT),
+		       ioaddr + RTC_RTCIENR);
+		spin_unlock_irq(&rtc_lock);
+		return 0;
+	}
+	return -ENOIOCTLCMD;
+}
+
+/*!
+ * This function reads the current RTC time into tm in Gregorian date.
+ *
+ * @param  tm           contains the RTC time value upon return
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int mxc_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	u32 val;
+
+	/* Avoid roll-over from reading the different registers */
+	do {
+		val = get_alarm_or_time(dev, MXC_RTC_TIME);
+	} while (val != get_alarm_or_time(dev, MXC_RTC_TIME));
+
+	rtc_time_to_tm(val, tm);
+	return 0;
+}
+
+/*!
+ * This function sets the internal RTC time based on tm in Gregorian date.
+ *
+ * @param  tm           the time value to be set in the RTC
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int mxc_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	unsigned long time;
+	int ret;
+	ret = rtc_tm_to_time(tm, &time);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Avoid roll-over from reading the different registers */
+	do {
+		set_alarm_or_time(dev, MXC_RTC_TIME, time);
+	} while (time != get_alarm_or_time(dev, MXC_RTC_TIME));
+
+	ret = set_ext_rtc_time(time);
+
+	if (ret != MXC_EXTERNAL_RTC_OK) {
+		if (ret == MXC_EXTERNAL_RTC_NONE) {
+			pr_info("No external RTC\n");
+			ret = 0;
+		} else
+			pr_info("Failed to set external RTC\n");
+	}
+
+	return ret;
+}
+
+/*!
+ * This function reads the current alarm value into the passed in \b alrm
+ * argument. It updates the \b alrm's pending field value based on the whether
+ * an alarm interrupt occurs or not.
+ *
+ * @param  alrm         contains the RTC alarm value upon return
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int mxc_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	void __iomem *ioaddr = pdata->ioaddr;
+
+	rtc_time_to_tm(get_alarm_or_time(dev, MXC_RTC_ALARM), &alrm->time);
+	alrm->pending =
+	    ((readw(ioaddr + RTC_RTCISR) & RTC_ALM_BIT) != 0) ? 1 : 0;
+
+	return 0;
+}
+
+/*!
+ * This function sets the RTC alarm based on passed in alrm.
+ *
+ * @param  alrm         the alarm value to be set in the RTC
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int mxc_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	int ret;
+
+	spin_lock_irq(&rtc_lock);
+	if (rtc_valid_tm(&alrm->time)) {
+		if (alrm->time.tm_sec > 59 ||
+		    alrm->time.tm_hour > 23 || alrm->time.tm_min > 59) {
+			ret = -EINVAL;
+			goto out;
+		}
+		ret = rtc_update_alarm(dev, &alrm->time);
+	} else {
+		if ((ret = rtc_valid_tm(&alrm->time)))
+			goto out;
+		ret = rtc_update_alarm(dev, &alrm->time);
+	}
+
+	if (ret == 0) {
+		memcpy(&g_rtc_alarm, &alrm->time, sizeof(struct rtc_time));
+
+		if (alrm->enabled) {
+			writew((readw(ioaddr + RTC_RTCIENR) | RTC_ALM_BIT),
+			       ioaddr + RTC_RTCIENR);
+		} else {
+			writew((readw(ioaddr + RTC_RTCIENR) & ~RTC_ALM_BIT),
+			       ioaddr + RTC_RTCIENR);
+		}
+	}
+      out:
+	spin_unlock_irq(&rtc_lock);
+
+	return ret;
+}
+
+/*!
+ * This function is used to provide the content for the /proc/driver/rtc
+ * file.
+ *
+ * @param  buf          the buffer to hold the information that the driver wants to write
+ *
+ * @return  The number of bytes written into the rtc file.
+ */
+static int mxc_rtc_proc(struct device *dev, struct seq_file *sq)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	char *p = sq->buf;
+
+	p += sprintf(p, "alarm_IRQ\t: %s\n",
+		     (((readw(ioaddr + RTC_RTCIENR)) & RTC_ALM_BIT) !=
+		      0) ? "yes" : "no");
+	p += sprintf(p, "update_IRQ\t: %s\n",
+		     (((readw(ioaddr + RTC_RTCIENR)) & RTC_1HZ_BIT) !=
+		      0) ? "yes" : "no");
+	p += sprintf(p, "periodic_IRQ\t: %s\n",
+		     (((readw(ioaddr + RTC_RTCIENR)) & PIT_ALL_ON) !=
+		      0) ? "yes" : "no");
+	p += sprintf(p, "periodic_freq\t: %d\n", rtc_freq);
+
+	return p - (sq->buf);
+}
+
+/*!
+ * The RTC driver structure
+ */
+static struct rtc_class_ops mxc_rtc_ops = {
+	.open = mxc_rtc_open,
+	.release = mxc_rtc_release,
+	.ioctl = mxc_rtc_ioctl,
+	.read_time = mxc_rtc_read_time,
+	.set_time = mxc_rtc_set_time,
+	.read_alarm = mxc_rtc_read_alarm,
+	.set_alarm = mxc_rtc_set_alarm,
+	.proc = mxc_rtc_proc,
+};
+
+/*! MXC RTC Power management control */
+
+static struct timespec mxc_rtc_delta;
+
+static int mxc_rtc_probe(struct platform_device *pdev)
+{
+	struct clk *clk;
+	struct timespec tv;
+	struct resource *res;
+	struct rtc_device *rtc;
+	struct rtc_plat_data *pdata = NULL;
+	u32 reg;
+	int ret = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	pdata->clk = clk_get(&pdev->dev, "rtc_clk");
+	clk_enable(pdata->clk);
+
+	pdata->baseaddr = res->start;
+	pdata->ioaddr = ((void *)(IO_ADDRESS(pdata->baseaddr)));
+	/* Configure and enable the RTC */
+	pdata->irq = platform_get_irq(pdev, 0);
+	if (pdata->irq >= 0) {
+		if (request_irq(pdata->irq, mxc_rtc_interrupt, IRQF_SHARED,
+				pdev->name, pdev) < 0) {
+			dev_warn(&pdev->dev, "interrupt not available.\n");
+			pdata->irq = -1;
+		}
+	}
+	rtc =
+	    rtc_device_register(pdev->name, &pdev->dev, &mxc_rtc_ops,
+				THIS_MODULE);
+	if (IS_ERR(rtc)) {
+		ret = PTR_ERR(rtc);
+		if (pdata->irq >= 0)
+			free_irq(pdata->irq, pdev);
+		kfree(pdata);
+		return ret;
+	}
+	pdata->rtc = rtc;
+	platform_set_drvdata(pdev, pdata);
+	tv.tv_nsec = 0;
+	tv.tv_sec = get_alarm_or_time(&pdev->dev, MXC_RTC_TIME);
+	clk = clk_get(NULL, "ckil");
+	if (clk_get_rate(clk) == 32768)
+		reg = RTC_INPUT_CLK_32768HZ;
+	else if (clk_get_rate(clk) == 32000)
+		reg = RTC_INPUT_CLK_32000HZ;
+	else if (clk_get_rate(clk) == 38400)
+		reg = RTC_INPUT_CLK_38400HZ;
+	else {
+		printk(KERN_ALERT "rtc clock is not valid");
+		return -EINVAL;
+	}
+	clk_put(clk);
+	reg |= RTC_ENABLE_BIT;
+	writew(reg, (pdata->ioaddr + RTC_RTCCTL));
+	if (((readw(pdata->ioaddr + RTC_RTCCTL)) & RTC_ENABLE_BIT) == 0) {
+		printk(KERN_ALERT "rtc : hardware module can't be enabled!\n");
+		return -EPERM;
+	}
+	printk("Real TIme clock Driver v%s \n", RTC_VERSION);
+	return ret;
+}
+
+static int __exit mxc_rtc_remove(struct platform_device *pdev)
+{
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	rtc_device_unregister(pdata->rtc);
+	if (pdata->irq >= 0) {
+		free_irq(pdata->irq, pdev);
+	}
+	clk_disable(pdata->clk);
+	clk_put(pdata->clk);
+	kfree(pdata);
+	mxc_rtc_release(NULL);
+	return 0;
+}
+
+/*!
+ * This function is called to save the system time delta relative to
+ * the MXC RTC when enterring a low power state. This time delta is
+ * then used on resume to adjust the system time to account for time
+ * loss while suspended.
+ *
+ * @param   pdev  not used
+ * @param   state Power state to enter.
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct timespec tv;
+
+	/* calculate time delta for suspend */
+	/* RTC precision is 1 second; adjust delta for avg 1/2 sec err */
+	tv.tv_nsec = NSEC_PER_SEC >> 1;
+	tv.tv_sec = get_alarm_or_time(&pdev->dev, MXC_RTC_TIME);
+	set_normalized_timespec(&mxc_rtc_delta,
+				xtime.tv_sec - tv.tv_sec,
+				xtime.tv_nsec - tv.tv_nsec);
+
+	return 0;
+}
+
+/*!
+ * This function is called to correct the system time based on the
+ * current MXC RTC time relative to the time delta saved during
+ * suspend.
+ *
+ * @param   pdev  not used
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_rtc_resume(struct platform_device *pdev)
+{
+	struct timespec tv;
+	struct timespec ts;
+
+	tv.tv_nsec = 0;
+	tv.tv_sec = get_alarm_or_time(&pdev->dev, MXC_RTC_TIME);
+
+	/* restore wall clock using delta against this RTC;
+	 * adjust again for avg 1/2 second RTC sampling error
+	 */
+	set_normalized_timespec(&ts,
+				tv.tv_sec + mxc_rtc_delta.tv_sec,
+				(NSEC_PER_SEC >> 1) + mxc_rtc_delta.tv_nsec);
+	do_settimeofday(&ts);
+
+	return 0;
+}
+
+/*!
+ * Contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_rtc_driver = {
+	.driver = {
+		   .name = "mxc_rtc",
+		   },
+	.probe = mxc_rtc_probe,
+	.remove = __exit_p(mxc_rtc_remove),
+	.suspend = mxc_rtc_suspend,
+	.resume = mxc_rtc_resume,
+};
+
+/*!
+ * This function creates the /proc/driver/rtc file and registers the device RTC
+ * in the /dev/misc directory. It also reads the RTC value from external source
+ * and setup the internal RTC properly.
+ *
+ * @return  -1 if RTC is failed to initialize; 0 is successful.
+ */
+static int __init mxc_rtc_init(void)
+{
+	return platform_driver_register(&mxc_rtc_driver);
+}
+
+/*!
+ * This function removes the /proc/driver/rtc file and un-registers the
+ * device RTC from the /dev/misc directory.
+ */
+static void __exit mxc_rtc_exit(void)
+{
+	platform_driver_unregister(&mxc_rtc_driver);
+
+}
+
+device_initcall_sync(mxc_rtc_init);
+module_exit(mxc_rtc_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Realtime Clock Driver (RTC)");
+MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-mxc_v2.c b/drivers/rtc/rtc-mxc_v2.c
new file mode 100644
index 000000000000..2e886cf2715c
--- /dev/null
+++ b/drivers/rtc/rtc-mxc_v2.c
@@ -0,0 +1,719 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/*
+ * Implementation based on rtc-ds1553.c
+ */
+
+/*!
+ * @defgroup RTC Real Time Clock (RTC) Driver
+ */
+/*!
+ * @file rtc-mxc_v2.c
+ * @brief Real Time Clock interface
+ *
+ * This file contains Real Time Clock interface for Linux.
+ *
+ * @ingroup RTC
+ */
+
+#include <linux/delay.h>
+#include <linux/rtc.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/uaccess.h>
+#include <mach/hardware.h>
+#include <asm/io.h>
+
+#define SRTC_LPPDR_INIT       0x41736166	/* init for glitch detect */
+
+#define SRTC_LPCR_SWR_LP      (1 << 0)	/* lp software reset */
+#define SRTC_LPCR_EN_LP       (1 << 3)	/* lp enable */
+#define SRTC_LPCR_WAE         (1 << 4)	/* lp wakeup alarm enable */
+#define SRTC_LPCR_SAE         (1 << 5)	/* lp security alarm enable */
+#define SRTC_LPCR_SI          (1 << 6)	/* lp security interrupt enable */
+#define SRTC_LPCR_ALP         (1 << 7)	/* lp alarm flag */
+#define SRTC_LPCR_LTC         (1 << 8)	/* lp lock time counter */
+#define SRTC_LPCR_LMC         (1 << 9)	/* lp lock monotonic counter */
+#define SRTC_LPCR_SV          (1 << 10)	/* lp security violation */
+#define SRTC_LPCR_NSA         (1 << 11)	/* lp non secure access */
+#define SRTC_LPCR_NVEIE       (1 << 12)	/* lp non valid state exit int en */
+#define SRTC_LPCR_IEIE        (1 << 13)	/* lp init state exit int enable */
+#define SRTC_LPCR_NVE         (1 << 14)	/* lp non valid state exit bit */
+#define SRTC_LPCR_IE          (1 << 15)	/* lp init state exit bit */
+
+#define SRTC_LPCR_ALL_INT_EN (SRTC_LPCR_WAE | SRTC_LPCR_SAE | \
+			      SRTC_LPCR_SI | SRTC_LPCR_ALP | \
+			      SRTC_LPCR_NVEIE | SRTC_LPCR_IEIE)
+
+#define SRTC_LPSR_TRI         (1 << 0)	/* lp time read invalidate */
+#define SRTC_LPSR_PGD         (1 << 1)	/* lp power supply glitc detected */
+#define SRTC_LPSR_CTD         (1 << 2)	/* lp clock tampering detected */
+#define SRTC_LPSR_ALP         (1 << 3)	/* lp alarm flag */
+#define SRTC_LPSR_MR          (1 << 4)	/* lp monotonic counter rollover */
+#define SRTC_LPSR_TR          (1 << 5)	/* lp time rollover */
+#define SRTC_LPSR_EAD         (1 << 6)	/* lp external alarm detected */
+#define SRTC_LPSR_IT0         (1 << 7)	/* lp IIM throttle */
+#define SRTC_LPSR_IT1         (1 << 8)
+#define SRTC_LPSR_IT2         (1 << 9)
+#define SRTC_LPSR_SM0         (1 << 10)	/* lp security mode */
+#define SRTC_LPSR_SM1         (1 << 11)
+#define SRTC_LPSR_STATE_LP0   (1 << 12)	/* lp state */
+#define SRTC_LPSR_STATE_LP1   (1 << 13)
+#define SRTC_LPSR_NVES        (1 << 14)	/* lp non-valid state exit status */
+#define SRTC_LPSR_IES         (1 << 15)	/* lp init state exit status */
+
+#define MAX_PIE_NUM     15
+#define MAX_PIE_FREQ    32768
+#define MIN_PIE_FREQ	1
+
+#define SRTC_PI0         (1 << 0)
+#define SRTC_PI1         (1 << 1)
+#define SRTC_PI2         (1 << 2)
+#define SRTC_PI3         (1 << 3)
+#define SRTC_PI4         (1 << 4)
+#define SRTC_PI5         (1 << 5)
+#define SRTC_PI6         (1 << 6)
+#define SRTC_PI7         (1 << 7)
+#define SRTC_PI8         (1 << 8)
+#define SRTC_PI9         (1 << 9)
+#define SRTC_PI10        (1 << 10)
+#define SRTC_PI11        (1 << 11)
+#define SRTC_PI12        (1 << 12)
+#define SRTC_PI13        (1 << 13)
+#define SRTC_PI14        (1 << 14)
+#define SRTC_PI15        (1 << 15)
+
+#define PIT_ALL_ON      (SRTC_PI1 | SRTC_PI2 | SRTC_PI3 | \
+			SRTC_PI4 | SRTC_PI5 | SRTC_PI6 | SRTC_PI7 | \
+			SRTC_PI8 | SRTC_PI9 | SRTC_PI10 | SRTC_PI11 | \
+			SRTC_PI12 | SRTC_PI13 | SRTC_PI14 | SRTC_PI15)
+
+#define SRTC_SWR_HP      (1 << 0)	/* hp software reset */
+#define SRTC_EN_HP       (1 << 3)	/* hp enable */
+#define SRTC_TS          (1 << 4)	/* time syncronize hp with lp */
+
+#define SRTC_IE_AHP      (1 << 16)	/* Alarm HP Interrupt Enable bit */
+#define SRTC_IE_WDHP     (1 << 18)	/* Write Done HP Interrupt Enable bit */
+#define SRTC_IE_WDLP     (1 << 19)	/* Write Done LP Interrupt Enable bit */
+
+#define SRTC_ISR_AHP     (1 << 16)	/* interrupt status: alarm hp */
+#define SRTC_ISR_WDHP    (1 << 18)	/* interrupt status: write done hp */
+#define SRTC_ISR_WDLP    (1 << 19)	/* interrupt status: write done lp */
+#define SRTC_ISR_WPHP    (1 << 20)	/* interrupt status: write pending hp */
+#define SRTC_ISR_WPLP    (1 << 21)	/* interrupt status: write pending lp */
+
+#define SRTC_LPSCMR	0x00	/* LP Secure Counter MSB Reg */
+#define SRTC_LPSCLR	0x04	/* LP Secure Counter LSB Reg */
+#define SRTC_LPSAR	0x08	/* LP Secure Alarm Reg */
+#define SRTC_LPSMCR	0x0C	/* LP Secure Monotonic Counter Reg */
+#define SRTC_LPCR	0x10	/* LP Control Reg */
+#define SRTC_LPSR	0x14	/* LP Status Reg */
+#define SRTC_LPPDR	0x18	/* LP Power Supply Glitch Detector Reg */
+#define SRTC_LPGR	0x1C	/* LP General Purpose Reg */
+#define SRTC_HPCMR	0x20	/* HP Counter MSB Reg */
+#define SRTC_HPCLR	0x24	/* HP Counter LSB Reg */
+#define SRTC_HPAMR	0x28	/* HP Alarm MSB Reg */
+#define SRTC_HPALR	0x2C	/* HP Alarm LSB Reg */
+#define SRTC_HPCR	0x30	/* HP Control Reg */
+#define SRTC_HPISR	0x34	/* HP Interrupt Status Reg */
+#define SRTC_HPIENR	0x38	/* HP Interrupt Enable Reg */
+
+#define SRTC_SECMODE_MASK	0x3	/* the mask of SRTC security mode */
+#define SRTC_SECMODE_LOW	0x0	/* Low Security */
+#define SRTC_SECMODE_MED	0x1	/* Medium Security */
+#define SRTC_SECMODE_HIGH	0x2	/* High Security */
+#define SRTC_SECMODE_RESERVED	0x3	/* Reserved */
+
+struct rtc_drv_data {
+	struct rtc_device *rtc;
+	void __iomem *ioaddr;
+	unsigned long baseaddr;
+	int irq;
+	struct clk *clk;
+	bool irq_enable;
+};
+
+/*!
+ * @defgroup RTC Real Time Clock (RTC) Driver
+ */
+/*!
+ * @file rtc-mxc.c
+ * @brief Real Time Clock interface
+ *
+ * This file contains Real Time Clock interface for Linux.
+ *
+ * @ingroup RTC
+ */
+
+static unsigned long rtc_status;
+
+static DEFINE_SPINLOCK(rtc_lock);
+
+/*!
+ * This function does write synchronization for writes to the lp srtc block.
+ * To take care of the asynchronous CKIL clock, all writes from the IP domain
+ * will be synchronized to the CKIL domain.
+ */
+static inline void rtc_write_sync_lp(void __iomem *ioaddr)
+{
+	unsigned int i, count;
+	/* Wait for 3 CKIL cycles */
+	for (i = 0; i < 3; i++) {
+		count = __raw_readl(ioaddr + SRTC_LPSCLR);
+		while
+			((__raw_readl(ioaddr + SRTC_LPSCLR)) == count);
+	}
+}
+
+/*!
+ * This function updates the RTC alarm registers and then clears all the
+ * interrupt status bits.
+ *
+ * @param  alrm         the new alarm value to be updated in the RTC
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int rtc_update_alarm(struct device *dev, struct rtc_time *alrm)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	struct rtc_time alarm_tm, now_tm;
+	unsigned long now, time;
+	int ret;
+
+	now = __raw_readl(ioaddr + SRTC_LPSCMR);
+	rtc_time_to_tm(now, &now_tm);
+
+	alarm_tm.tm_year = now_tm.tm_year;
+	alarm_tm.tm_mon = now_tm.tm_mon;
+	alarm_tm.tm_mday = now_tm.tm_mday;
+
+	alarm_tm.tm_hour = alrm->tm_hour;
+	alarm_tm.tm_min = alrm->tm_min;
+	alarm_tm.tm_sec = alrm->tm_sec;
+
+	rtc_tm_to_time(&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);
+
+	__raw_writel(time, ioaddr + SRTC_LPSAR);
+
+	/* clear alarm interrupt status bit */
+	__raw_writel(SRTC_LPSR_ALP, ioaddr + SRTC_LPSR);
+
+	return ret;
+}
+
+/*!
+ * This function is the RTC interrupt service routine.
+ *
+ * @param  irq          RTC IRQ number
+ * @param  dev_id       device ID which is not used
+ *
+ * @return IRQ_HANDLED as defined in the include/linux/interrupt.h file.
+ */
+static irqreturn_t mxc_rtc_interrupt(int irq, void *dev_id)
+{
+	struct platform_device *pdev = dev_id;
+	struct rtc_drv_data *pdata = platform_get_drvdata(pdev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	u32 lp_status, lp_cr;
+	u32 events = 0;
+
+	clk_enable(pdata->clk);
+	lp_status = __raw_readl(ioaddr + SRTC_LPSR);
+	lp_cr = __raw_readl(ioaddr + SRTC_LPCR);
+
+	/* update irq data & counter */
+	if (lp_status & SRTC_LPSR_ALP) {
+		if (lp_cr & SRTC_LPCR_ALP)
+			events |= (RTC_AF | RTC_IRQF);
+
+		/* disable further lp alarm interrupts */
+		lp_cr &= ~(SRTC_LPCR_ALP | SRTC_LPCR_WAE);
+	}
+
+	/* Update interrupt enables */
+	__raw_writel(lp_cr, ioaddr + SRTC_LPCR);
+
+	/* If no interrupts are enabled, turn off interrupts in kernel */
+	if (((lp_cr & SRTC_LPCR_ALL_INT_EN) == 0) && (pdata->irq_enable)) {
+		disable_irq(pdata->irq);
+		pdata->irq_enable = false;
+	}
+
+	/* clear interrupt status */
+	__raw_writel(lp_status, ioaddr + SRTC_LPSR);
+	clk_disable(pdata->clk);
+
+	rtc_update_irq(pdata->rtc, 1, events);
+	return IRQ_HANDLED;
+}
+
+/*!
+ * This function is used to open the RTC driver.
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int mxc_rtc_open(struct device *dev)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	clk_enable(pdata->clk);
+
+	if (test_and_set_bit(1, &rtc_status))
+		return -EBUSY;
+	return 0;
+}
+
+/*!
+ * clear all interrupts and release the IRQ
+ */
+static void mxc_rtc_release(struct device *dev)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+
+	clk_disable(pdata->clk);
+
+	rtc_status = 0;
+}
+
+/*!
+ * This function is used to support some ioctl calls directly.
+ * Other ioctl calls are supported indirectly through the
+ * arm/common/rtctime.c file.
+ *
+ * @param  cmd          ioctl command as defined in include/linux/rtc.h
+ * @param  arg          value for the ioctl command
+ *
+ * @return  0 if successful or negative value otherwise.
+ */
+static int mxc_rtc_ioctl(struct device *dev, unsigned int cmd,
+			 unsigned long arg)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	unsigned long lock_flags = 0;
+	u32 lp_cr;
+
+	switch (cmd) {
+	case RTC_AIE_OFF:
+		spin_lock_irqsave(&rtc_lock, lock_flags);
+		lp_cr = __raw_readl(ioaddr + SRTC_LPCR);
+		lp_cr &= ~(SRTC_LPCR_ALP | SRTC_LPCR_WAE);
+		if (((lp_cr & SRTC_LPCR_ALL_INT_EN) == 0)
+		    && (pdata->irq_enable)) {
+			disable_irq(pdata->irq);
+			pdata->irq_enable = false;
+		}
+		__raw_writel(lp_cr, ioaddr + SRTC_LPCR);
+		spin_unlock_irqrestore(&rtc_lock, lock_flags);
+		return 0;
+
+	case RTC_AIE_ON:
+		spin_lock_irqsave(&rtc_lock, lock_flags);
+		if (!pdata->irq_enable) {
+			enable_irq(pdata->irq);
+			pdata->irq_enable = true;
+		}
+		lp_cr = __raw_readl(ioaddr + SRTC_LPCR);
+		lp_cr |= SRTC_LPCR_ALP | SRTC_LPCR_WAE;
+		__raw_writel(lp_cr, ioaddr + SRTC_LPCR);
+		spin_unlock_irqrestore(&rtc_lock, lock_flags);
+		return 0;
+	}
+
+	return -ENOIOCTLCMD;
+}
+
+/*!
+ * This function reads the current RTC time into tm in Gregorian date.
+ *
+ * @param  tm           contains the RTC time value upon return
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int mxc_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+
+	rtc_time_to_tm(__raw_readl(ioaddr + SRTC_LPSCMR), tm);
+	return 0;
+}
+
+/*!
+ * This function sets the internal RTC time based on tm in Gregorian date.
+ *
+ * @param  tm           the time value to be set in the RTC
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int mxc_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	unsigned long time;
+	int ret;
+	ret = rtc_tm_to_time(tm, &time);
+	if (ret != 0)
+		return ret;
+
+	__raw_writel(time, ioaddr + SRTC_LPSCMR);
+	rtc_write_sync_lp(ioaddr);
+
+	return 0;
+}
+
+/*!
+ * This function reads the current alarm value into the passed in \b alrm
+ * argument. It updates the \b alrm's pending field value based on the whether
+ * an alarm interrupt occurs or not.
+ *
+ * @param  alrm         contains the RTC alarm value upon return
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int mxc_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+
+	rtc_time_to_tm(__raw_readl(ioaddr + SRTC_LPSAR), &alrm->time);
+	alrm->pending =
+	    ((__raw_readl(ioaddr + SRTC_LPSR) & SRTC_LPSR_ALP) != 0) ? 1 : 0;
+
+	return 0;
+}
+
+/*!
+ * This function sets the RTC alarm based on passed in alrm.
+ *
+ * @param  alrm         the alarm value to be set in the RTC
+ *
+ * @return  0 if successful; non-zero otherwise.
+ */
+static int mxc_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	unsigned long lock_flags = 0;
+	u32 lp_cr;
+	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;
+		}
+	}
+
+	spin_lock_irqsave(&rtc_lock, lock_flags);
+	lp_cr = __raw_readl(ioaddr + SRTC_LPCR);
+
+	ret = rtc_update_alarm(dev, &alrm->time);
+	if (ret)
+		goto out;
+
+	if (alrm->enabled)
+		lp_cr |= (SRTC_LPCR_ALP | SRTC_LPCR_WAE);
+	else
+		lp_cr &= ~(SRTC_LPCR_ALP | SRTC_LPCR_WAE);
+
+	if (lp_cr & SRTC_LPCR_ALL_INT_EN) {
+		if (!pdata->irq_enable) {
+			enable_irq(pdata->irq);
+			pdata->irq_enable = true;
+		}
+	} else {
+		if (pdata->irq_enable) {
+			disable_irq(pdata->irq);
+			pdata->irq_enable = false;
+		}
+	}
+
+	__raw_writel(lp_cr, ioaddr + SRTC_LPCR);
+
+out:
+	spin_unlock_irqrestore(&rtc_lock, lock_flags);
+	rtc_write_sync_lp(ioaddr);
+	return ret;
+}
+
+/*!
+ * This function is used to provide the content for the /proc/driver/rtc
+ * file.
+ *
+ * @param  seq  buffer to hold the information that the driver wants to write
+ *
+ * @return  The number of bytes written into the rtc file.
+ */
+static int mxc_rtc_proc(struct device *dev, struct seq_file *seq)
+{
+	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+
+	clk_enable(pdata->clk);
+	seq_printf(seq, "alarm_IRQ\t: %s\n",
+		   (((__raw_readl(ioaddr + SRTC_LPCR)) & SRTC_LPCR_ALP) !=
+		    0) ? "yes" : "no");
+	clk_disable(pdata->clk);
+
+	return 0;
+}
+
+/*!
+ * The RTC driver structure
+ */
+static struct rtc_class_ops mxc_rtc_ops = {
+	.open = mxc_rtc_open,
+	.release = mxc_rtc_release,
+	.ioctl = mxc_rtc_ioctl,
+	.read_time = mxc_rtc_read_time,
+	.set_time = mxc_rtc_set_time,
+	.read_alarm = mxc_rtc_read_alarm,
+	.set_alarm = mxc_rtc_set_alarm,
+	.proc = mxc_rtc_proc,
+};
+
+/*! MXC RTC Power management control */
+static int mxc_rtc_probe(struct platform_device *pdev)
+{
+	struct clk *clk;
+	struct timespec tv;
+	struct resource *res;
+	struct rtc_device *rtc;
+	struct rtc_drv_data *pdata = NULL;
+	struct mxc_srtc_platform_data *plat_data = NULL;
+	void __iomem *ioaddr;
+	void __iomem *srtc_secmode_addr;
+	int ret = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	pdata->clk = clk_get(&pdev->dev, "rtc_clk");
+	clk_enable(pdata->clk);
+	pdata->baseaddr = res->start;
+	pdata->ioaddr = ioremap(pdata->baseaddr, 0x40);
+	ioaddr = pdata->ioaddr;
+
+	/* Configure and enable the RTC */
+	pdata->irq = platform_get_irq(pdev, 0);
+	if (pdata->irq >= 0) {
+		if (request_irq(pdata->irq, mxc_rtc_interrupt, IRQF_SHARED,
+				pdev->name, pdev) < 0) {
+			dev_warn(&pdev->dev, "interrupt not available.\n");
+			pdata->irq = -1;
+		} else {
+			disable_irq(pdata->irq);
+			pdata->irq_enable = false;
+		}
+	}
+
+	clk = clk_get(NULL, "rtc_clk");
+	if (clk_get_rate(clk) != 32768) {
+		printk(KERN_ALERT "rtc clock is not valid");
+		ret = -EINVAL;
+		clk_put(clk);
+		goto err_out;
+	}
+	clk_put(clk);
+
+	/* initialize glitch detect */
+	__raw_writel(SRTC_LPPDR_INIT, ioaddr + SRTC_LPPDR);
+	udelay(100);
+
+	/* clear lp interrupt status */
+	__raw_writel(0xFFFFFFFF, ioaddr + SRTC_LPSR);
+	udelay(100);;
+
+	plat_data = (struct mxc_srtc_platform_data *)pdev->dev.platform_data;
+	clk = clk_get(NULL, "iim_clk");
+	clk_enable(clk);
+	srtc_secmode_addr = ioremap(plat_data->srtc_sec_mode_addr, 1);
+
+	/* Check SRTC security mode */
+	if (((__raw_readl(srtc_secmode_addr) & SRTC_SECMODE_MASK) ==
+	    SRTC_SECMODE_LOW) && (cpu_is_mx51_rev(CHIP_REV_1_0) == 1)) {
+		/* Workaround for MX51 TO1 due to inaccurate CKIL clock */
+		__raw_writel(SRTC_LPCR_EN_LP, ioaddr + SRTC_LPCR);
+		udelay(100);
+	} else {
+		/* move out of init state */
+		__raw_writel((SRTC_LPCR_IE | SRTC_LPCR_NSA),
+			     ioaddr + SRTC_LPCR);
+
+		udelay(100);
+
+		while ((__raw_readl(ioaddr + SRTC_LPSR) & SRTC_LPSR_IES) == 0);
+
+		/* move out of non-valid state */
+		__raw_writel((SRTC_LPCR_IE | SRTC_LPCR_NVE | SRTC_LPCR_NSA |
+			      SRTC_LPCR_EN_LP), ioaddr + SRTC_LPCR);
+
+		udelay(100);
+
+		while ((__raw_readl(ioaddr + SRTC_LPSR) & SRTC_LPSR_NVES) == 0);
+
+		__raw_writel(0xFFFFFFFF, ioaddr + SRTC_LPSR);
+		udelay(100);
+	}
+	clk_disable(clk);
+	clk_put(clk);
+
+	rtc = rtc_device_register(pdev->name, &pdev->dev,
+				  &mxc_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc)) {
+		ret = PTR_ERR(rtc);
+		goto err_out;
+	}
+
+	pdata->rtc = rtc;
+	platform_set_drvdata(pdev, pdata);
+
+	tv.tv_nsec = 0;
+	tv.tv_sec = __raw_readl(ioaddr + SRTC_LPSCMR);
+
+	/* By default, devices should wakeup if they can */
+	/* So srtc is set as "should wakeup" as it can */
+	device_init_wakeup(&pdev->dev, 1);
+
+	clk_disable(pdata->clk);
+
+	return ret;
+
+err_out:
+	clk_disable(pdata->clk);
+	iounmap(ioaddr);
+	if (pdata->irq >= 0)
+		free_irq(pdata->irq, pdev);
+	kfree(pdata);
+	return ret;
+}
+
+static int __exit mxc_rtc_remove(struct platform_device *pdev)
+{
+	struct rtc_drv_data *pdata = platform_get_drvdata(pdev);
+	rtc_device_unregister(pdata->rtc);
+	if (pdata->irq >= 0)
+		free_irq(pdata->irq, pdev);
+
+	clk_disable(pdata->clk);
+	clk_put(pdata->clk);
+	kfree(pdata);
+	return 0;
+}
+
+/*!
+ * This function is called to save the system time delta relative to
+ * the MXC RTC when enterring a low power state. This time delta is
+ * then used on resume to adjust the system time to account for time
+ * loss while suspended.
+ *
+ * @param   pdev  not used
+ * @param   state Power state to enter.
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct rtc_drv_data *pdata = platform_get_drvdata(pdev);
+
+	if (device_may_wakeup(&pdev->dev)) {
+		enable_irq_wake(pdata->irq);
+	} else {
+		if (pdata->irq_enable)
+			disable_irq(pdata->irq);
+	}
+
+	return 0;
+}
+
+/*!
+ * This function is called to correct the system time based on the
+ * current MXC RTC time relative to the time delta saved during
+ * suspend.
+ *
+ * @param   pdev  not used
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_rtc_resume(struct platform_device *pdev)
+{
+	struct rtc_drv_data *pdata = platform_get_drvdata(pdev);
+
+	if (device_may_wakeup(&pdev->dev)) {
+		disable_irq_wake(pdata->irq);
+	} else {
+		if (pdata->irq_enable)
+			enable_irq(pdata->irq);
+	}
+
+	return 0;
+}
+
+/*!
+ * Contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_rtc_driver = {
+	.driver = {
+		   .name = "mxc_rtc",
+		   },
+	.probe = mxc_rtc_probe,
+	.remove = __exit_p(mxc_rtc_remove),
+	.suspend = mxc_rtc_suspend,
+	.resume = mxc_rtc_resume,
+};
+
+/*!
+ * This function creates the /proc/driver/rtc file and registers the device RTC
+ * in the /dev/misc directory. It also reads the RTC value from external source
+ * and setup the internal RTC properly.
+ *
+ * @return  -1 if RTC is failed to initialize; 0 is successful.
+ */
+static int __init mxc_rtc_init(void)
+{
+	return platform_driver_register(&mxc_rtc_driver);
+}
+
+/*!
+ * This function removes the /proc/driver/rtc file and un-registers the
+ * device RTC from the /dev/misc directory.
+ */
+static void __exit mxc_rtc_exit(void)
+{
+	platform_driver_unregister(&mxc_rtc_driver);
+
+}
+
+module_init(mxc_rtc_init);
+module_exit(mxc_rtc_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Realtime Clock Driver (RTC)");
+MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-ns9xxx.c b/drivers/rtc/rtc-ns9xxx.c
new file mode 100644
index 000000000000..a34689bff9ff
--- /dev/null
+++ b/drivers/rtc/rtc-ns9xxx.c
@@ -0,0 +1,946 @@
+/*
+ * linux/drivers/rtc/rtc-ns9xxx.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/rtc.h>
+#include <linux/clk.h>
+#include <linux/bcd.h>
+#include <linux/platform_device.h>
+
+#include <asm/io.h>
+#include <asm/mach/time.h>
+
+/* registers */
+#define RTC_CONFIG			(0x00)
+#define RTC_CONFIG_TIMEON			(0)
+#define RTC_CONFIG_CALON			(0)
+#define RTC_CONFIG_TIMEOFF			(1 << 0)
+#define RTC_CONFIG_CALOFF			(1 << 1)
+
+#define RTC_1224H			(0x04)
+#define RTC_1224H_MODE				(1 << 0)
+#define RTC_1224H_MODE_12H			(1 << 0)
+#define RTC_1224H_MODE_24H			(0 << 0)
+
+#define RTC_TIME			(0x08)
+#define RTC_TIME_PM			(1 << 30)
+#define RTC_TIME_HOUR_MASK		(0x3f000000)
+#define RTC_TIME_MINUTE_MASK		(0x007f0000)
+#define RTC_TIME_SECOND_MASK		(0x00007f00)
+#define RTC_TIME_MONTH_MASK		(0x000000f8)
+#define RTC_TIME_DATE_MASK		(0x00003f00)
+#define RTC_TIME_DAY_MASK		(0x00000007)
+#define RTC_TIME_CENTURY_MASK		(0x3f000000)
+#define RTC_TIME_YEAR_MASK		(0x00ff0000)
+
+#define RTC_CAL				(0x0c)
+#define RTC_ALARMTIME			(0x10)
+#define RTC_ALARMCAL			(0x14)
+
+#define RTC_ALARMENABLE			(0x18)
+#define RTC_ALARMENABLE_HSEC			(1 << 0)
+#define RTC_ALARMENABLE_SEC			(1 << 1)
+#define RTC_ALARMENABLE_MIN			(1 << 2)
+#define RTC_ALARMENABLE_HOUR			(1 << 3)
+#define RTC_ALARMENABLE_DATE			(1 << 4)
+#define RTC_ALARMENABLE_MNTH			(1 << 5)
+#define RTC_ALARMENABLE_ALL			(0x3f)
+
+#define RTC_EVENT			(0x1c)
+
+#define RTC_IRQENABLE			(0x20)
+#define RTC_IRQDISABLE			(0x24)
+#define RTC_IRQSTATUS			(0x28)
+#define RTC_IRQ_HUNDREDTH		(1 << 0)
+#define RTC_IRQ_SECOND			(1 << 1)
+#define RTC_IRQ_MINUTE			(1 << 2)
+#define RTC_IRQ_HOUR			(1 << 3)
+#define RTC_IRQ_DATE			(1 << 4)
+#define RTC_IRQ_MONTH			(1 << 5)
+#define RTC_IRQ_ALARM			(1 << 6)
+
+#define RTC_STATUS			(0x2c)
+#define RTC_STATUS_TIMEVALID			(1 << 0)
+#define RTC_STATUS_CALVALID			(1 << 1)
+#define RTC_STATUS_ALARMTIMEVALID		(1 << 2)
+#define RTC_STATUS_ALARMCALVALID		(1 << 3)
+
+/* configuration/status shifts */
+#define RTC_TIME_HOUR_SHIFT		(24)
+#define RTC_TIME_MINUTE_SHIFT		(16)
+#define RTC_TIME_SECOND_SHIFT		(8)
+#define RTC_CAL_MONTH_SHIFT		(3)
+#define RTC_CAL_DATE_SHIFT		(8)
+#define RTC_CAL_DAY_SHIFT		(0)
+#define RTC_CAL_CENTURY_SHIFT		(24)
+#define RTC_CAL_YEAR_SHIFT		(16)
+
+#define RTC_IRQ_PERIOD			RTC_IRQ_SECOND
+#define RTC_IRQ_ALL							\
+		(RTC_IRQ_HUNDREDTH | RTC_IRQ_SECOND |			\
+		RTC_IRQ_MINUTE | RTC_IRQ_HOUR |				\
+		RTC_IRQ_DATE | RTC_IRQ_MONTH |				\
+		RTC_IRQ_ALARM)
+
+#define RTC_RAM				(0xc0)
+#define RTC_RAM_SIZE			(0x100 - RTC_RAM)
+
+#define DRIVER_NAME "rtc-ns9xxx"
+
+struct ns9xxx_rtc_pdata {
+	struct rtc_device *rtc;
+	struct clk *clk;
+	struct clk *pmclk;
+	void __iomem *ioaddr;
+	struct resource *mem;
+	int irq;
+	u8 irq_status;
+	u8 event;
+};
+
+static int __ns9xxx_rtc_timeon(struct ns9xxx_rtc_pdata *pdata, u32 config)
+{
+	if (config & (RTC_CONFIG_TIMEOFF | RTC_CONFIG_CALOFF)) {
+		dev_dbg(&pdata->rtc->dev,
+				"time or cal is stopped, RTC_CONFIG=0x%08x\n",
+				config);
+
+		return 0;
+	}
+	return 1;
+}
+
+static int ns9xxx_rtc_timeon(struct ns9xxx_rtc_pdata *pdata)
+{
+	u32 config = ioread32(pdata->ioaddr + RTC_CONFIG);
+	return __ns9xxx_rtc_timeon(pdata, config);
+}
+
+static int __ns9xxx_rtc_timevalid(struct ns9xxx_rtc_pdata *pdata, u32 status)
+{
+	if (~status & (RTC_STATUS_TIMEVALID | RTC_STATUS_CALVALID)) {
+		dev_dbg(&pdata->rtc->dev,
+				"time is invalid, RTC_STATUS=0x%08x\n",
+				status);
+		return 0;
+	}
+
+	return 1;
+}
+static int ns9xxx_rtc_timevalid(struct ns9xxx_rtc_pdata *pdata)
+{
+	u32 status = ioread32(pdata->ioaddr + RTC_STATUS);
+	return __ns9xxx_rtc_timevalid(pdata, status);
+}
+
+static int ns9xxx_rtc_timeonandvalid(struct ns9xxx_rtc_pdata *pdata)
+{
+	return ns9xxx_rtc_timeon(pdata) && ns9xxx_rtc_timevalid(pdata);
+}
+
+static int ns9xxx_rtc_alarmvalid(struct ns9xxx_rtc_pdata *pdata)
+{
+	int ret;
+	u32 status;
+
+	ret = ns9xxx_rtc_timeon(pdata);
+	if (!ret)
+		return ret;
+
+	status = ioread32(pdata->ioaddr + RTC_STATUS);
+	ret = __ns9xxx_rtc_timevalid(pdata, status);
+	if (!ret)
+		return ret;
+
+	if (~status & (RTC_STATUS_ALARMTIMEVALID |
+				(RTC_STATUS_ALARMCALVALID))) {
+		dev_dbg(&pdata->rtc->dev,
+				"alarm is invalid, RTC_STATUS=0x%08x\n",
+				status);
+		return 0;
+	}
+
+	return 1;
+}
+
+#ifdef CONFIG_RTC_INTF_DEV
+
+static int ns9xxx_rtc_ioctl(struct device *dev, unsigned int cmd,
+		unsigned long arg)
+{
+	struct ns9xxx_rtc_pdata *pdata = dev_get_drvdata(dev);
+	int ret;
+
+	dev_dbg(dev, "%s(cmd=%u, arg=%lu)\n", __func__, cmd, arg);
+	ret = clk_enable(pdata->clk);
+	if (ret)
+		return ret;
+
+	pdata->event |= ioread32(pdata->ioaddr + RTC_EVENT);
+
+	switch (cmd) {
+	case RTC_AIE_OFF:
+		/* disable alarm-irq */
+		pdata->irq_status &= ~RTC_IRQ_ALARM;
+		iowrite32(RTC_IRQ_ALARM, pdata->ioaddr + RTC_IRQDISABLE);
+		break;
+
+	case RTC_AIE_ON:
+		if (ns9xxx_rtc_alarmvalid(pdata)) {
+			/* enable alarm-irq */
+			pdata->irq_status |= RTC_IRQ_ALARM;
+			iowrite32(RTC_IRQ_ALARM, pdata->ioaddr + RTC_IRQENABLE);
+			pdata->event &= ~RTC_IRQ_ALARM;
+		} else {
+			ret = -EINVAL;
+			dev_dbg(dev, "%s: alarm not valid\n", __func__);
+		}
+
+		break;
+
+	case RTC_UIE_OFF:
+		/* disable periodic irq */
+		pdata->irq_status &= ~RTC_IRQ_PERIOD;
+		iowrite32(RTC_IRQ_PERIOD, pdata->ioaddr + RTC_IRQDISABLE);
+		break;
+
+	case RTC_UIE_ON:
+		if (ns9xxx_rtc_timeonandvalid(pdata)) {
+			/* enable periodic irq */
+			pdata->irq_status |= RTC_IRQ_PERIOD;
+			iowrite32(RTC_IRQ_PERIOD,
+					pdata->ioaddr + RTC_IRQENABLE);
+			pdata->event &= ~RTC_IRQ_PERIOD;
+		} else {
+			ret = -EINVAL;
+			dev_dbg(dev, "%s: time not valid\n", __func__);
+		}
+
+		break;
+
+	default:
+		ret = -ENOIOCTLCMD;
+	}
+
+	clk_disable(pdata->clk);
+
+	dev_dbg(dev, "%s(cmd=%u, arg=%lu) -> %d\n", __func__, cmd, arg, ret);
+	return ret;
+}
+
+#else
+# define ns9xxx_rtc_ioctl NULL
+#endif /* CONFIG_RTC_INTF_DEV */
+
+static int ns9xxx_rtc_get_time_sync(struct ns9xxx_rtc_pdata *pdata,
+		unsigned int *time, unsigned int *cal)
+{
+	u32 cal1, time1, cal2;
+	int tries = 5;
+
+	dev_dbg(&pdata->rtc->dev, "%s\n", __func__);
+
+	if (!ns9xxx_rtc_timeonandvalid(pdata)) {
+		dev_dbg(&pdata->rtc->dev, "%s -> %d\n", __func__, -EINVAL);
+		return -EINVAL;
+	}
+
+	/* necessary to avoid time and date beeing out of sync */
+	do {
+		cal1 = ioread32(pdata->ioaddr + RTC_CAL);
+		time1 = ioread32(pdata->ioaddr + RTC_TIME);
+		cal2 = ioread32(pdata->ioaddr + RTC_CAL);
+	} while ((cal1 != cal2) && --tries);
+
+	if (!tries) {
+		dev_dbg(&pdata->rtc->dev, "%s -> %d\n", __func__, -EBUSY);
+		return -EBUSY;
+	}
+
+	*time = time1;
+	*cal = cal1;
+
+	dev_dbg(&pdata->rtc->dev, "%s: cal/time = %08x/%08x -> 0\n",
+			__func__, cal1, time1);
+	return 0;
+}
+
+static void ns9xxx_rtc_decode_time(struct rtc_time *tm,
+		u32 time, u32 cal, u32 mode1224)
+{
+	tm->tm_hour = bcd2bin((time & RTC_TIME_HOUR_MASK)
+			>> RTC_TIME_HOUR_SHIFT);
+	if (unlikely((mode1224 & RTC_1224H_MODE) == RTC_1224H_MODE_12H)) {
+		if (time & RTC_TIME_PM)
+			tm->tm_hour += 12;
+	}
+	tm->tm_min = bcd2bin((time & RTC_TIME_MINUTE_MASK)
+			>> RTC_TIME_MINUTE_SHIFT);
+	tm->tm_sec = bcd2bin((time & RTC_TIME_SECOND_MASK)
+			>> RTC_TIME_SECOND_SHIFT);
+
+	tm->tm_mon = (bcd2bin((cal & RTC_TIME_MONTH_MASK)
+			>> RTC_CAL_MONTH_SHIFT) - 1);
+	tm->tm_mday = bcd2bin((cal & RTC_TIME_DATE_MASK)
+			>> RTC_CAL_DATE_SHIFT);
+	tm->tm_wday = (bcd2bin(cal & RTC_TIME_DAY_MASK)
+			>> RTC_CAL_DAY_SHIFT) - 1;
+	tm->tm_year = bcd2bin((cal & RTC_TIME_CENTURY_MASK)
+			>> RTC_CAL_CENTURY_SHIFT) * 100;
+	tm->tm_year += bcd2bin((cal & RTC_TIME_YEAR_MASK)
+			>> RTC_CAL_YEAR_SHIFT);
+	tm->tm_yday = rtc_year_days(tm->tm_mday, tm->tm_mon, tm->tm_year);
+	tm->tm_year -= 1900;
+
+	pr_debug("%s: %08x/%08x -> %d-%d-%d %d:%d:%d\n",
+			__func__, cal, time, tm->tm_year,
+			tm->tm_mon, tm->tm_mday,
+			tm->tm_hour, tm->tm_min, tm->tm_sec);
+}
+
+static void ns9xxx_rtc_encode_time(struct rtc_time *tm,
+		unsigned int *time, unsigned int *cal)
+{
+	*time  = bin2bcd(tm->tm_hour) << RTC_TIME_HOUR_SHIFT;
+	*time |= bin2bcd(tm->tm_min) << RTC_TIME_MINUTE_SHIFT;
+	*time |= bin2bcd(tm->tm_sec) << RTC_TIME_SECOND_SHIFT;
+
+	*cal  = bin2bcd(tm->tm_mon + 1) << RTC_CAL_MONTH_SHIFT;
+	*cal |= bin2bcd(tm->tm_mday) << RTC_CAL_DATE_SHIFT;
+	/* NET+OS uses 1 to encode Sunday, so we follow that practise */
+	*cal |= bin2bcd(tm->tm_wday + 1) << RTC_CAL_DAY_SHIFT;
+	*cal |= bin2bcd((tm->tm_year + 1900) % 100)
+			<< RTC_CAL_YEAR_SHIFT;
+	*cal |= bin2bcd((tm->tm_year + 1900) / 100)
+			<< RTC_CAL_CENTURY_SHIFT;
+	pr_debug("%s: cal/time = %08x/%08x\n", __func__, *cal, *time);
+}
+
+static int ns9xxx_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	u32 uninitialized_var(time), uninitialized_var(cal);
+	u32 mode1224;
+	int ret;
+	struct ns9xxx_rtc_pdata *pdata = dev_get_drvdata(dev);
+
+	ret = clk_enable(pdata->clk);
+	if (ret)
+		return ret;
+
+	ret = ns9xxx_rtc_get_time_sync(pdata, &time, &cal);
+
+	mode1224 = ioread32(pdata->ioaddr + RTC_1224H);
+
+	clk_disable(pdata->clk);
+
+	if (!ret)
+		ns9xxx_rtc_decode_time(tm, time, cal, mode1224);
+
+	return ret;
+}
+
+static int ns9xxx_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	unsigned int time, cal, stat;
+	int ret;
+	struct ns9xxx_rtc_pdata *pdata = dev_get_drvdata(dev);
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (rtc_valid_tm(tm) != 0)
+		return -EINVAL;
+
+	ns9xxx_rtc_encode_time(tm, &time, &cal);
+
+	ret = clk_enable(pdata->clk);
+	if (ret)
+		return ret;
+
+	/* disable operation */
+	iowrite32(RTC_CONFIG_TIMEOFF | RTC_CONFIG_CALOFF,
+			pdata->ioaddr + RTC_CONFIG);
+
+	iowrite32(time, pdata->ioaddr + RTC_TIME);
+	iowrite32(cal, pdata->ioaddr + RTC_CAL);
+
+	iowrite32(RTC_1224H_MODE_24H, pdata->ioaddr + RTC_1224H);
+
+	/* reenable operation */
+	iowrite32(RTC_CONFIG_TIMEON | RTC_CONFIG_CALON,
+			pdata->ioaddr + RTC_CONFIG);
+
+	/* make sure configs are valid */
+	stat = ioread32(pdata->ioaddr + RTC_STATUS);
+
+	clk_disable(pdata->clk);
+
+	if (~stat & (RTC_STATUS_TIMEVALID | RTC_STATUS_CALVALID)) {
+		dev_dbg(dev, "%s: invalid date/time: RTC_STATUS = 0x%08x\n",
+				__func__, stat);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int ns9xxx_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alm)
+{
+	unsigned int time, cal;
+	int ret;
+	struct ns9xxx_rtc_pdata *pdata = dev_get_drvdata(dev);
+	u32 mode1224, status;
+	u32 almenable;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	ret = clk_enable(pdata->clk);
+	if (ret)
+		return ret;
+
+	status = ioread32(pdata->ioaddr + RTC_STATUS);
+	if (~status & (RTC_STATUS_ALARMTIMEVALID | RTC_STATUS_ALARMCALVALID)) {
+		dev_dbg(dev, "time or cal is invalid, RTC_STATUS=0x%08x\n",
+				status);
+		return -EINVAL;
+	}
+
+	mode1224 = ioread32(pdata->ioaddr + RTC_1224H);
+	time = ioread32(pdata->ioaddr + RTC_ALARMTIME);
+	cal  = ioread32(pdata->ioaddr + RTC_ALARMCAL);
+
+	dev_dbg(dev, "%s: cal/time = %08x/%08x\n", __func__, cal, time);
+	ns9xxx_rtc_decode_time(&(alm->time), time, cal, mode1224);
+
+	almenable = ioread32(pdata->ioaddr + RTC_ALARMENABLE);
+	dev_dbg(dev, "%s: ALARMENABLE = %08x\n", __func__, almenable);
+
+	if (!(almenable & RTC_ALARMENABLE_SEC))
+		alm->time.tm_sec = -1;
+
+	if (!(almenable & RTC_ALARMENABLE_MIN))
+		alm->time.tm_min = -1;
+
+	if (!(almenable & RTC_ALARMENABLE_HOUR))
+		alm->time.tm_hour = -1;
+
+	if (!(almenable & RTC_ALARMENABLE_DATE))
+		alm->time.tm_mday = -1;
+
+	if (!(almenable & RTC_ALARMENABLE_MNTH))
+		alm->time.tm_mon = -1;
+
+	alm->time.tm_year = -1;
+
+	alm->enabled = !!(pdata->irq_status & RTC_IRQ_ALARM);
+
+	clk_disable(pdata->clk);
+
+	return 0;
+}
+
+static int ns9xxx_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alm)
+{
+	unsigned int time, cal, stat;
+	int ret = 0;
+	struct ns9xxx_rtc_pdata *pdata = dev_get_drvdata(dev);
+
+	if (rtc_valid_tm(&(alm->time)) != 0)
+		return -EINVAL;
+
+	ns9xxx_rtc_encode_time(&(alm->time), &time, &cal);
+
+	dev_dbg(dev, "%s: %08x/%08x\n", __func__, cal, time);
+
+	ret = clk_enable(pdata->clk);
+	if (ret)
+		return ret;
+
+	if (!alm->enabled) {
+		iowrite32(RTC_IRQ_ALARM, pdata->ioaddr + RTC_IRQDISABLE);
+		pdata->irq_status &= ~RTC_IRQ_ALARM;
+	}
+
+	iowrite32(time, pdata->ioaddr + RTC_ALARMTIME);
+	iowrite32(cal, pdata->ioaddr + RTC_ALARMCAL);
+
+	if (alm->enabled) {
+		iowrite32(RTC_ALARMENABLE_ALL, pdata->ioaddr + RTC_ALARMENABLE);
+
+		/* make sure configs are valid */
+		stat = ioread32(pdata->ioaddr + RTC_STATUS);
+		if (!(stat & (RTC_STATUS_ALARMTIMEVALID |
+						RTC_STATUS_ALARMCALVALID))) {
+			dev_dbg(dev, "%s: invalid date/time: "
+					"RTC_STATUS = 0x%08x\n",
+					__func__, stat);
+			ret = -EINVAL;
+		}
+
+		iowrite32(RTC_IRQ_ALARM, pdata->ioaddr + RTC_IRQENABLE);
+		pdata->irq_status |= RTC_IRQ_ALARM;
+	}
+
+	clk_disable(pdata->clk);
+
+	return ret;
+}
+
+static int ns9xxx_rtc_proc(struct device *dev, struct seq_file *seq)
+{
+	unsigned int stat;
+	int ret;
+	struct ns9xxx_rtc_pdata *pdata = dev_get_drvdata(dev);
+
+	ret = clk_enable(pdata->clk);
+	if (ret)
+		return ret;
+
+	stat = ioread32(pdata->ioaddr + RTC_IRQSTATUS);
+
+	clk_disable(pdata->clk);
+
+	return seq_printf(seq,
+		"IRQ on:%s%s%s%s%s%s\n",
+		(stat & RTC_IRQ_ALARM)  ? "" : " alarm",
+		(stat & RTC_IRQ_MONTH)  ? "" : " month_rollover",
+		(stat & RTC_IRQ_DATE)   ? "" : " day_rollover",
+		(stat & RTC_IRQ_HOUR)   ? "" : " hour_rollover",
+		(stat & RTC_IRQ_MINUTE) ? "" : " minute_rollover",
+		(stat & RTC_IRQ_SECOND) ? "" : " second_rollover");
+}
+
+static struct rtc_class_ops ns9xxx_rtc_ops = {
+	.ioctl		= ns9xxx_rtc_ioctl,
+	.read_time	= ns9xxx_rtc_read_time,
+	.set_time	= ns9xxx_rtc_set_time,
+	.read_alarm	= ns9xxx_rtc_read_alarm,
+	.set_alarm	= ns9xxx_rtc_set_alarm,
+	.proc		= ns9xxx_rtc_proc,
+};
+
+static irqreturn_t ns9xxx_rtc_irq_handler(int irq, void *irq_data)
+{
+	u8 event;
+	struct ns9xxx_rtc_pdata *pdata = irq_data;
+	int handled = 0, ret;
+
+	ret = clk_enable(pdata->clk);
+	if (unlikely(ret)) {
+		dev_warn(&pdata->rtc->dev, "%s: Could not enable clock, "
+				"error is %d\n", __func__, ret);
+		return IRQ_NONE;
+	}
+
+	pdata->event |= ioread32(pdata->ioaddr + RTC_EVENT);
+	event = pdata->event & pdata->irq_status;
+	dev_dbg(&pdata->rtc->dev, "%s: RTC_IRQSTATUS = %x, RTC_EVENT = %x\n",
+			__func__, pdata->irq_status, pdata->event);
+	pdata->event = 0;
+
+	if (event & (RTC_IRQ_ALARM | RTC_IRQ_PERIOD)) {
+		unsigned long rtcevents = RTC_IRQF;
+
+		if (event & RTC_IRQ_ALARM) {
+			rtcevents |= RTC_AF;
+
+			/* according to the comment in rtc_sysfs_show_wakealarm
+			 * further alarm irqs must not occur.
+			 */
+			pdata->irq_status &= ~RTC_IRQ_ALARM;
+			iowrite32(RTC_IRQ_ALARM,
+					pdata->ioaddr + RTC_IRQDISABLE);
+		}
+
+		if (event & RTC_IRQ_PERIOD)
+			rtcevents |= RTC_UF;
+
+		rtc_update_irq(pdata->rtc, 1, rtcevents);
+
+		handled = 1;
+	}
+
+	clk_disable(pdata->clk);
+
+	return IRQ_RETVAL(handled);
+}
+
+static ssize_t ns9xxx_rtc_nvram_read(struct kobject *kobj,
+		struct bin_attribute *bin_attr, char *buf,
+		loff_t pos, size_t size)
+{
+	loff_t npos = pos;
+	struct ns9xxx_rtc_pdata *pdata = bin_attr->private;
+	u32 data;
+	int ret;
+
+	if (pos >= RTC_RAM_SIZE)
+		return 0;
+
+	if (pos + size > RTC_RAM_SIZE)
+		size = RTC_RAM_SIZE - pos;
+
+	ret = clk_enable(pdata->clk);
+	if (ret)
+		return ret;
+
+	if (npos & 3) {
+		data = ioread32(pdata->ioaddr + RTC_RAM + (npos & ~3));
+		pr_debug("%s: nvram[0x%04llx]  -> 0x%08x\n", __func__, npos & ~3, data);
+	} else
+		data = 0; /* silence gcc */
+
+	for (; size > 0; npos++, size--) {
+		if (!(npos & 3)) {
+			data = ioread32(pdata->ioaddr + RTC_RAM + npos);
+			pr_debug("%s: nvram[0x%04llx]  -> 0x%08x\n", __func__, npos, data);
+		}
+
+		*buf++ = data >> ((npos & 3) << 3);
+	}
+
+	clk_disable(pdata->clk);
+
+	return npos - pos;
+}
+
+static ssize_t ns9xxx_rtc_nvram_write(struct kobject *kobj,
+		struct bin_attribute *bin_attr, char *buf,
+		loff_t pos, size_t size)
+{
+	loff_t npos = pos;
+	struct ns9xxx_rtc_pdata *pdata = bin_attr->private;
+	int ret;
+	u32 data;
+
+	if (pos >= RTC_RAM_SIZE)
+		return 0;
+
+	if (pos + size > RTC_RAM_SIZE)
+		size = RTC_RAM_SIZE - pos;
+
+	pr_debug("%s: pos = 0x%04llx, size = 0x%x\n", __func__, pos, size);
+	ret = clk_enable(pdata->clk);
+	if (ret)
+		return ret;
+
+	if (npos & 3) {
+		data = ioread32(pdata->ioaddr + RTC_RAM + (npos & ~3));
+		pr_debug("%s a: nvram[0x%04llx]  -> 0x%08x\n", __func__, npos & ~3, data);
+		data &= (1 << ((npos & 3) << 3)) - 1;
+	} else
+		data = 0;
+
+	for (; size > 0; npos++, size--) {
+		data |= *buf++ << ((npos & 3) << 3);
+
+		if ((npos & 3) == 3) {
+			pr_debug("%s b: nvram[0x%04llx] <-  0x%08x\n", __func__, npos & ~3, data);
+			iowrite32(data, pdata->ioaddr + RTC_RAM + (npos & ~3));
+
+			/* the next write doesn't fill a complete u32.  Note
+			 * that size isn't decremented yet for the last byte */
+			if (size < 5 && size > 1) {
+				data = ioread32(pdata->ioaddr + RTC_RAM + npos + 1);
+				pr_debug("%s c: nvram[0x%04llx]  -> 0x%08x\n", __func__, npos + 1, data);
+				data &= ~((1 << ((size - 1) << 3)) - 1);
+			} else
+				data = 0;
+		}
+	}
+	if (npos & 3) {
+		pr_debug("%s d: nvram[0x%04llx] <-  0x%08x\n", __func__, npos & ~3, data);
+		iowrite32(data, pdata->ioaddr + RTC_RAM + (npos & ~3));
+	}
+
+	clk_disable(pdata->clk);
+
+	return npos - pos;
+}
+
+static struct bin_attribute ns9xxx_rtc_nvram_attr = {
+	.attr = {
+		.name = "nvram",
+		.mode = S_IRUGO | S_IWUGO,
+		.owner = THIS_MODULE,
+	},
+	.size = RTC_RAM_SIZE,
+	.read = ns9xxx_rtc_nvram_read,
+	.write = ns9xxx_rtc_nvram_write,
+};
+
+static int __init ns9xxx_rtc_probe(struct platform_device *pdev)
+{
+	struct ns9xxx_rtc_pdata *pdata;
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, pdata);
+
+	pdata->irq = platform_get_irq(pdev, 0);
+	if (pdata->irq < 0) {
+		ret = -ENOENT;
+		dev_dbg(&pdev->dev, "err_get_irq\n");
+		goto err_get_irq;
+	}
+
+	pdata->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!pdata->mem) {
+		ret = -ENOENT;
+		dev_dbg(&pdev->dev, "err_get_resource\n");
+		goto err_get_resource;
+	}
+
+	if (!request_mem_region(pdata->mem->start,
+			pdata->mem->end - pdata->mem->start + 1,
+			DRIVER_NAME)) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "err_request_mem_region\n");
+		goto err_request_mem_region;
+	}
+
+	pdata->ioaddr = ioremap(pdata->mem->start,
+			pdata->mem->end - pdata->mem->start + 1);
+	if (!pdata->ioaddr) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "err_ioremap\n");
+		goto err_ioremap;
+	} else
+		dev_dbg(&pdev->dev, "ioaddr = 0x%p\n", pdata->ioaddr);
+
+	pdata->clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(pdata->clk)) {
+		ret = PTR_ERR(pdata->clk);
+		dev_dbg(&pdev->dev, "err_clk_get -> %d\n", ret);
+		goto err_clk_get;
+	}
+
+	ret = clk_enable(pdata->clk);
+	if (ret) {
+		dev_dbg(&pdev->dev, "err_clk_enable -> %d\n", ret);
+		goto err_clk_enable;
+	}
+
+#if defined(CONFIG_PM)
+	pdata->pmclk = clk_get(&pdev->dev, DRIVER_NAME "-pm");
+	if (IS_ERR(pdata->pmclk)) {
+		ret = PTR_ERR(pdata->pmclk);
+		dev_dbg(&pdev->dev, "err_clk_get_pm -> %d\n", ret);
+		goto err_clk_get_pm;
+	}
+
+	/* Make the device wakeup capable, but disabled by default */
+	device_init_wakeup(&pdev->dev, 1);
+	device_set_wakeup_enable(&pdev->dev, 0);
+#endif
+
+	/* disable interrupts and clear event flags */
+	iowrite32(RTC_IRQ_ALL, pdata->ioaddr + RTC_IRQDISABLE);
+	ioread32(pdata->ioaddr + RTC_EVENT);
+
+	pdata->rtc = rtc_device_register(DRIVER_NAME, &pdev->dev,
+			&ns9xxx_rtc_ops, THIS_MODULE);
+	if (IS_ERR(pdata->rtc))	{
+		ret = PTR_ERR(pdata->rtc);
+		dev_dbg(&pdev->dev, "err_rtc_device_register -> %d\n", ret);
+		goto err_rtc_device_register;
+	}
+
+	ns9xxx_rtc_nvram_attr.private = pdata;
+	ret = sysfs_create_bin_file(&pdev->dev.kobj, &ns9xxx_rtc_nvram_attr);
+	if (ret)
+		goto err_sysfs_bin;
+
+	ret = request_irq(pdata->irq, ns9xxx_rtc_irq_handler, IRQF_SHARED,
+			pdata->rtc->dev.bus_id, pdata);
+	if (ret) {
+		dev_dbg(&pdev->dev, "err_request_irq -> %d\n", ret);
+
+		rtc_device_unregister(pdata->rtc);
+err_rtc_device_register:
+		sysfs_remove_bin_file(&pdev->dev.kobj, &ns9xxx_rtc_nvram_attr);
+err_sysfs_bin:
+#if defined(CONFIG_PM)
+		clk_put(pdata->pmclk);
+err_clk_get_pm:
+
+#endif
+		clk_disable(pdata->clk);
+err_clk_enable:
+
+		clk_put(pdata->clk);
+err_clk_get:
+
+		iounmap(pdata->ioaddr);
+err_ioremap:
+
+		release_mem_region(pdata->mem->start,
+				pdata->mem->end - pdata->mem->start + 1);
+err_request_mem_region:
+err_get_resource:
+err_get_irq:
+
+		kfree(pdata);
+		return ret;
+	}
+
+	clk_disable(pdata->clk);
+
+	return 0;
+}
+
+static int __devexit ns9xxx_rtc_remove(struct platform_device *pdev)
+{
+	struct ns9xxx_rtc_pdata *pdata = platform_get_drvdata(pdev);
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+	ret = clk_enable(pdata->clk);
+	if (ret)
+		return ret;
+
+	/* disable interrupts */
+	iowrite32(RTC_IRQ_ALL, pdata->ioaddr + RTC_IRQDISABLE);
+
+	clk_disable(pdata->clk);
+
+	free_irq(pdata->irq, pdata);
+	sysfs_remove_bin_file(&pdev->dev.kobj, &ns9xxx_rtc_nvram_attr);
+	rtc_device_unregister(pdata->rtc);
+	clk_put(pdata->clk);
+	iounmap(pdata->ioaddr);
+	release_mem_region(pdata->mem->start,
+			pdata->mem->end - pdata->mem->start + 1);
+	kfree(pdata);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+static int ns9xxx_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct ns9xxx_rtc_pdata *pdata = platform_get_drvdata(pdev);
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	ret = clk_enable(pdata->clk);
+	if (ret)
+		return ret;
+
+	if (device_may_wakeup(&pdev->dev)) {
+
+		ret = clk_enable(pdata->pmclk);
+		if (ret) {
+			dev_dbg(&pdev->dev, "%s: err_enable_pmclk -> %d\n",
+					__func__, ret);
+			goto err;
+		}
+
+		enable_irq_wake(pdata->irq);
+		dev_dbg(&pdev->dev, "%s: irq_status = %08x\n",
+				__func__, pdata->irq_status);
+
+	} else
+		/* disable irqs */
+		iowrite32(RTC_IRQ_ALL, pdata->ioaddr + RTC_IRQDISABLE);
+
+err:
+	clk_disable(pdata->clk);
+
+	return ret;
+}
+
+static int ns9xxx_rtc_resume(struct platform_device *pdev)
+{
+	struct ns9xxx_rtc_pdata *pdata = platform_get_drvdata(pdev);
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	ret = clk_enable(pdata->clk);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_clk_enable -> %d\n",
+				__func__, ret);
+		return ret;
+	}
+
+	if (device_may_wakeup(&pdev->dev)) {
+		disable_irq_wake(pdata->irq);
+		clk_disable(pdata->pmclk);
+	} else
+		iowrite32(pdata->irq_status, pdata->ioaddr + RTC_IRQENABLE);
+
+	clk_disable(pdata->clk);
+
+	return 0;
+}
+
+#else
+# define ns9xxx_rtc_suspend NULL
+# define ns9xxx_rtc_resume NULL
+#endif /* CONFIG_PM */
+
+static void ns9xxx_rtc_shutdown(struct platform_device *pdev)
+{
+	struct ns9xxx_rtc_pdata *pdata = platform_get_drvdata(pdev);
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	ret = clk_enable(pdata->clk);
+	if (ret) {
+		dev_warn(&pdev->dev, "%s: cannot enable rtc clock to disable "
+				"interrupts\n", __func__);
+		return;
+	}
+
+	/* just disable interrupts */
+	iowrite32(RTC_IRQ_ALL,
+			pdata->ioaddr + RTC_IRQDISABLE);
+
+	clk_disable(pdata->clk);
+}
+
+static struct platform_driver ns9xxx_rtc_driver = {
+	.probe		= ns9xxx_rtc_probe,
+	.remove		= __devexit_p(ns9xxx_rtc_remove),
+	.suspend	= ns9xxx_rtc_suspend,
+	.resume		= ns9xxx_rtc_resume,
+	.shutdown	= ns9xxx_rtc_shutdown,
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init ns9xxx_rtc_init(void)
+{
+	return platform_driver_register(&ns9xxx_rtc_driver);
+}
+
+static void __exit ns9xxx_rtc_exit(void)
+{
+	platform_driver_unregister(&ns9xxx_rtc_driver);
+}
+
+module_init(ns9xxx_rtc_init);
+module_exit(ns9xxx_rtc_exit);
+
+MODULE_AUTHOR("Uwe Kleine-Koenig <Uwe.Kleine-Koenig@digi.com>");
+MODULE_DESCRIPTION("RTC driver for Digi ns9xxx");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/rtc/rtc-stmp3xxx.c b/drivers/rtc/rtc-stmp3xxx.c
new file mode 100644
index 000000000000..51499cd0ba81
--- /dev/null
+++ b/drivers/rtc/rtc-stmp3xxx.c
@@ -0,0 +1,277 @@
+/*
+ * Freescale STMP37XX/STMP378X Real Time Clock driver
+ *
+ * Copyright (c) 2007 Sigmatel, Inc.
+ * Peter Hartley, <peter.hartley@sigmatel.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/string.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/rtc.h>
+#include <linux/bcd.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/uaccess.h>
+
+#include <mach/stmp3xxx.h>
+#include <mach/hardware.h>
+#include <mach/irqs.h>
+#include <mach/regs-rtc.h>
+
+struct stmp3xxx_rtc_data {
+	struct rtc_device *rtc;
+	unsigned irq_count;
+};
+
+/* Time read/write */
+static int stmp3xxx_rtc_gettime(struct device *dev, struct rtc_time *rtc_tm)
+{
+	while (HW_RTC_STAT_RD() & BF_RTC_STAT_STALE_REGS(0x80))
+		cpu_relax();
+
+	rtc_time_to_tm(HW_RTC_SECONDS_RD(), rtc_tm);
+	return 0;
+}
+
+static int stmp3xxx_rtc_settime(struct device *dev, struct rtc_time *rtc_tm)
+{
+	unsigned long t;
+	int rc = rtc_tm_to_time(rtc_tm, &t);
+
+	if (rc == 0) {
+		HW_RTC_SECONDS_WR(t);
+
+		/* The datasheet doesn't say which way round the
+		 * NEW_REGS/STALE_REGS bitfields go. In fact it's 0x1=P0,
+		 * 0x2=P1, .., 0x20=P5, 0x40=ALARM, 0x80=SECONDS,
+		 */
+		while (HW_RTC_STAT_RD() & BF_RTC_STAT_NEW_REGS(0x80))
+			cpu_relax();
+	}
+	return rc;
+}
+
+static irqreturn_t stmp3xxx_rtc_interrupt(int irq, void *dev_id)
+{
+	struct platform_device *pdev = to_platform_device(dev_id);
+	struct stmp3xxx_rtc_data *data = platform_get_drvdata(pdev);
+	u32 status;
+	u32 events = 0;
+
+	status = HW_RTC_CTRL_RD() &
+			(BM_RTC_CTRL_ALARM_IRQ | BM_RTC_CTRL_ONEMSEC_IRQ);
+	if (status & BM_RTC_CTRL_ALARM_IRQ) {
+		HW_RTC_CTRL_CLR(BM_RTC_CTRL_ALARM_IRQ);
+		events |= RTC_AF | RTC_IRQF;
+	}
+	if (status & BM_RTC_CTRL_ONEMSEC_IRQ) {
+		HW_RTC_CTRL_CLR(BM_RTC_CTRL_ONEMSEC_IRQ);
+		if (++data->irq_count % 1000 == 0) {
+			events |= RTC_UF | RTC_IRQF;
+			data->irq_count = 0;
+		}
+	}
+
+	if (events)
+		rtc_update_irq(data->rtc, 1, events);
+
+	return IRQ_HANDLED;
+}
+
+static int stmp3xxx_rtc_open(struct device *dev)
+{
+	int r;
+
+	r = request_irq(IRQ_RTC_ALARM, stmp3xxx_rtc_interrupt,
+			IRQF_DISABLED, "RTC alarm", dev);
+	if (r) {
+		dev_err(dev, "Cannot claim IRQ%d\n", IRQ_RTC_ALARM);
+		goto fail_1;
+	}
+	r = request_irq(IRQ_RTC_1MSEC, stmp3xxx_rtc_interrupt,
+			IRQF_DISABLED, "RTC tick", dev);
+	if (r) {
+		dev_err(dev, "Cannot claim IRQ%d\n", IRQ_RTC_1MSEC);
+		goto fail_2;
+	}
+
+	return 0;
+fail_2:
+	free_irq(IRQ_RTC_ALARM, dev);
+fail_1:
+	return r;
+}
+
+static void stmp3xxx_rtc_release(struct device *dev)
+{
+	HW_RTC_CTRL_CLR(BM_RTC_CTRL_ALARM_IRQ_EN | BM_RTC_CTRL_ONEMSEC_IRQ_EN);
+	free_irq(IRQ_RTC_ALARM, dev);
+	free_irq(IRQ_RTC_1MSEC, dev);
+}
+
+static int stmp3xxx_rtc_ioctl(struct device *dev, unsigned int cmd,
+			      unsigned long arg)
+{
+	struct stmp3xxx_rtc_data *data = dev_get_drvdata(dev);
+
+	switch (cmd) {
+	case RTC_AIE_OFF:
+		HW_RTC_PERSISTENT0_CLR(BM_RTC_PERSISTENT0_ALARM_EN |
+					BM_RTC_PERSISTENT0_ALARM_WAKE_EN);
+		HW_RTC_CTRL_CLR(BM_RTC_CTRL_ALARM_IRQ_EN);
+		break;
+	case RTC_AIE_ON:
+		HW_RTC_PERSISTENT0_SET(BM_RTC_PERSISTENT0_ALARM_EN |
+					BM_RTC_PERSISTENT0_ALARM_WAKE_EN);
+		HW_RTC_CTRL_SET(BM_RTC_CTRL_ALARM_IRQ_EN);
+		break;
+	case RTC_UIE_ON:
+		data->irq_count = 0;
+		HW_RTC_CTRL_SET(BM_RTC_CTRL_ONEMSEC_IRQ_EN);
+		break;
+	case RTC_UIE_OFF:
+		HW_RTC_CTRL_CLR(BM_RTC_CTRL_ONEMSEC_IRQ_EN);
+		break;
+	default:
+		return -ENOIOCTLCMD;
+	}
+
+	return 0;
+}
+static int stmp3xxx_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alm)
+{
+	u32 t = HW_RTC_ALARM_RD();
+
+	rtc_time_to_tm(t, &alm->time);
+
+	return 0;
+}
+
+static int stmp3xxx_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alm)
+{
+	unsigned long t;
+
+	rtc_tm_to_time(&alm->time, &t);
+	HW_RTC_ALARM_WR(t);
+	return 0;
+}
+
+static struct rtc_class_ops stmp3xxx_rtc_ops = {
+	.open		= stmp3xxx_rtc_open,
+	.release	= stmp3xxx_rtc_release,
+	.ioctl          = stmp3xxx_rtc_ioctl,
+	.read_time	= stmp3xxx_rtc_gettime,
+	.set_time	= stmp3xxx_rtc_settime,
+	.read_alarm	= stmp3xxx_rtc_read_alarm,
+	.set_alarm	= stmp3xxx_rtc_set_alarm,
+};
+
+static int stmp3xxx_rtc_remove(struct platform_device *dev)
+{
+	struct stmp3xxx_rtc_data *rtc_data = platform_get_drvdata(dev);
+
+	if (rtc_data) {
+		rtc_device_unregister(rtc_data->rtc);
+		kfree(rtc_data);
+	}
+
+	return 0;
+}
+
+static int stmp3xxx_rtc_probe(struct platform_device *pdev)
+{
+	u32 hwversion = HW_RTC_VERSION_RD();
+	u32 rtc_stat = HW_RTC_STAT_RD();
+	struct stmp3xxx_rtc_data *rtc_data = kzalloc(sizeof *rtc_data,
+						     GFP_KERNEL);
+
+	if ((rtc_stat & BM_RTC_STAT_RTC_PRESENT) == 0)
+		return -ENODEV;
+	if (!rtc_data)
+		return -ENOMEM;
+
+	stmp3xxx_reset_block(REGS_RTC_BASE, 1);
+
+	HW_RTC_PERSISTENT0_CLR(BM_RTC_PERSISTENT0_ALARM_EN |
+				BM_RTC_PERSISTENT0_ALARM_WAKE_EN |
+				BM_RTC_PERSISTENT0_ALARM_WAKE);
+
+	printk(KERN_INFO "STMP3xxx RTC driver v1.0 hardware v%u.%u.%u\n",
+	       (hwversion >> 24),
+	       (hwversion >> 16) & 0xFF,
+	       hwversion & 0xFFFF);
+
+	rtc_data->rtc = rtc_device_register(pdev->name, &pdev->dev,
+				&stmp3xxx_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc_data->rtc)) {
+		kfree(rtc_data);
+		return PTR_ERR(rtc_data->rtc);
+	}
+
+	platform_set_drvdata(pdev, rtc_data);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int stmp3xxx_rtc_suspend(struct platform_device *dev, pm_message_t state)
+{
+	return 0;
+}
+
+static int stmp3xxx_rtc_resume(struct platform_device *dev)
+{
+	stmp3xxx_reset_block(REGS_RTC_BASE, 1);
+	HW_RTC_PERSISTENT0_CLR(BM_RTC_PERSISTENT0_ALARM_EN |
+				BM_RTC_PERSISTENT0_ALARM_WAKE_EN |
+				BM_RTC_PERSISTENT0_ALARM_WAKE);
+	return 0;
+}
+#else
+#define stmp3xxx_rtc_suspend	NULL
+#define stmp3xxx_rtc_resume	NULL
+#endif
+
+static struct platform_driver stmp3xxx_rtcdrv = {
+	.probe		= stmp3xxx_rtc_probe,
+	.remove		= stmp3xxx_rtc_remove,
+	.suspend	= stmp3xxx_rtc_suspend,
+	.resume		= stmp3xxx_rtc_resume,
+	.driver		= {
+		.name	= "stmp3xxx-rtc",
+		.owner	= THIS_MODULE,
+	},
+};
+
+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_DESCRIPTION("STMP3xxx RTC Driver");
+MODULE_AUTHOR("dmitry pervushin <dimka@embeddedalley.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/scsi/Kconfig b/drivers/scsi/Kconfig
index 403ecad48d4b..be8272b22341 100644
--- a/drivers/scsi/Kconfig
+++ b/drivers/scsi/Kconfig
@@ -257,10 +257,19 @@ config SCSI_SCAN_ASYNC
 	  or async on the kernel's command line.
 
 config SCSI_WAIT_SCAN
-	tristate
+	tristate "create module which waits for SCSI scanning to finish"
 	default m
 	depends on SCSI
 	depends on MODULES
+	help
+	  When this module is loaded, it will do nothing else than wait for
+	  SCSI low-level drivers to finish asynchronous scanning for devices.
+	  This module will be called scsi_wait_scan.
+
+	  It's default and safe to create this module even if it's not used
+	  in your distro's init scripts. You can override this if you are
+	  really sure you don't need or want it. It does not make sense to
+	  compile this into the kernel with Y.
 
 menu "SCSI Transports"
 	depends on SCSI
diff --git a/drivers/serial/8250.c b/drivers/serial/8250.c
index 303272af386e..45412dff2c72 100644
--- a/drivers/serial/8250.c
+++ b/drivers/serial/8250.c
@@ -862,6 +862,10 @@ static void autoconfig_16550a(struct uart_8250_port *up)
 	 * Check for presence of the EFR when DLAB is set.
 	 * Only ST16C650V1 UARTs pass this test.
 	 */
+#ifndef CONFIG_ARCH_MXC
+	/* This test fails as EFR reads 0, but our uart requires LCR=0xBF
+	 *  to access EFR.
+	 */
 	serial_outp(up, UART_LCR, UART_LCR_DLAB);
 	if (serial_in(up, UART_EFR) == 0) {
 		serial_outp(up, UART_EFR, 0xA8);
@@ -875,6 +879,7 @@ static void autoconfig_16550a(struct uart_8250_port *up)
 		serial_outp(up, UART_EFR, 0);
 		return;
 	}
+#endif
 
 	/*
 	 * Maybe it requires 0xbf to be written to the LCR.
@@ -1397,7 +1402,12 @@ static void transmit_chars(struct uart_8250_port *up)
 
 	count = up->tx_loadsz;
 	do {
+#ifdef CONFIG_ARCH_MXC
+		/* Seems like back-to-back accesses are a problem */
+		serial_out_sync(up, UART_TX, xmit->buf[xmit->tail]);
+#else
 		serial_out(up, UART_TX, xmit->buf[xmit->tail]);
+#endif
 		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
 		up->port.icount.tx++;
 		if (uart_circ_empty(xmit))
diff --git a/drivers/serial/Kconfig b/drivers/serial/Kconfig
index 579d63a81aa2..4ef33d8623a8 100644
--- a/drivers/serial/Kconfig
+++ b/drivers/serial/Kconfig
@@ -304,6 +304,31 @@ config SERIAL_AMBA_PL010_CONSOLE
 	  your boot loader (lilo or loadlin) about how to pass options to the
 	  kernel at boot time.)
 
+config SERIAL_MXC
+    tristate "MXC Internal serial port support"
+    depends on ARCH_MXC
+    select SERIAL_CORE
+    help
+      This selects the Freescale Semiconductor MXC Internal UART driver.
+      If unsure, say N.
+
+config SERIAL_MXC_CONSOLE
+    bool "Support for console on a MXC/MX27/MX21 Internal serial port"
+    depends on SERIAL_MXC=y
+    select SERIAL_CORE_CONSOLE
+    help
+	  Say Y here if you wish to use an MXC Internal 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).
+
+	  Even if you say Y here, the currently visible framebuffer 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=ttymxc". (Try "man bootparam" or see the documentation of
+	  your boot loader (lilo or loadlin) about how to pass options to the
+	  kernel at boot time.)
+
+
 config SERIAL_AMBA_PL011
 	tristate "ARM AMBA PL011 serial port support"
 	depends on ARM_AMBA
@@ -508,6 +533,62 @@ config SERIAL_S3C2440
 	help
 	  Serial port support for the Samsung S3C2440 and S3C2442 SoC
 
+config SERIAL_S3C2410_ENABLE_PORTA
+	bool "Enable the port A support on the S3C24XX"
+	depends on SERIAL_S3C2410 || SERIAL_S3C2440
+        default y
+	help
+	  Enable the serial port A of the S3C24XX
+
+config SERIAL_S3C2410_ENABLE_PORTB
+       	bool "Enable the port B support on the S3C24XX"
+	depends on SERIAL_S3C2410 || SERIAL_S3C2440
+        default y
+	help
+	  Enable the serial port B of the S3C24XX
+
+config SERIAL_S3C2410_ENABLE_PORTC
+	bool "Enable the port C support on the S3C24XX"
+	depends on SERIAL_S3C2410 || SERIAL_S3C2440
+        default y
+	help
+	  Enable the serial port C of the S3C24XX
+
+config SERIAL_S3C2410_ENABLE_PORTD
+	bool "Enable the port D support on the S3C24XX"
+	depends on SERIAL_S3C2410 || SERIAL_S3C2440
+        default y
+	help
+          Enable the serial port D of the S3C24XX. Enabling this option will
+	  disable the HW flow control of the port C.
+	
+config SERIAL_S3C2410_HWCTRL
+        bool "Enable the HW flow control on the S3C24XX ports"
+        depends on SERIAL_S3C2410 || SERIAL_S3C2440
+        help
+          Enables the support for the hardware handshake on the S3C24XX
+
+config SERIAL_S3C2410_PORTA_HWCTRL
+	bool "Enable HW flow control of the port A"
+	depends on (SERIAL_S3C2410 || SERIAL_S3C2440) && SERIAL_S3C2410_HWCTRL && SERIAL_S3C2410_ENABLE_PORTA
+        default n
+	help
+          Enable the HW flow control on the port A of the S3C24XX
+
+config SERIAL_S3C2410_PORTB_HWCTRL
+	bool "Enable HW flow control of the port B"
+	depends on (SERIAL_S3C2410 || SERIAL_S3C2440) && SERIAL_S3C2410_HWCTRL && SERIAL_S3C2410_ENABLE_PORTB
+        default n
+	help
+          Enable the HW flow control on the port B of the S3C24XX
+
+config SERIAL_S3C2410_PORTC_HWCTRL
+	bool "Enable HW flow control of the port C"
+	depends on (SERIAL_S3C2410 || SERIAL_S3C2440) && SERIAL_S3C2410_HWCTRL && \
+	SERIAL_S3C2410_ENABLE_PORTC && !SERIAL_S3C2410_ENABLE_PORTD
+        default n
+	help
+          Enable the HW flow control on the port C of the S3C24XX
 
 
 config SERIAL_DZ
@@ -1372,4 +1453,77 @@ config SPORT_BAUD_RATE
 	default 19200 if (SERIAL_SPORT_BAUD_RATE_19200)
 	default 9600 if (SERIAL_SPORT_BAUD_RATE_9600)
 
+config SERIAL_NS921X
+	tristate "Digi NS921x serial port support"
+	depends on ARM && ARCH_NS9XXX && PROCESSOR_NS921X
+	select SERIAL_CORE
+	help
+	  If you have a Digi NS921x based system and wish to use its serial
+	  ports, say Y or M here
+
+config SERIAL_NS921X_CONSOLE
+	bool "Console on Digi NS921x serial"
+	depends on SERIAL_NS921X=y
+	select SERIAL_CORE_CONSOLE
+	help
+	  Console support for serial ports of Digi NS921x based systems
+
+config SERIAL_NS9360
+	tristate "Digi NS9360 serial port support"
+	depends on ARM && ARCH_NS9XXX && PROCESSOR_NS9360
+	select SERIAL_CORE
+	help
+	  If you have a Digi NS9360 based system and wish to use its serial
+	  ports, say Y or M here
+
+config SERIAL_NS9360_CONSOLE
+	bool "Console on Digi NS9360 serial"
+	depends on SERIAL_NS9360=y
+	select SERIAL_CORE_CONSOLE
+	help
+	  Console support for serial ports of Digi NS9360 based systems
+
+config SERIAL_NS9360_COMPAT
+	bool "Keep backward compatility"
+	depends on SERIAL_NS9360
+	default y
+	help
+	  The serial driver for ns9360 used ttyS device node in the past
+	  which leads to problems, if there are also 8215 compatible serial
+	  ports in the system. In newer versions this changed to ttyNS.
+	  Keep this config enabled to still get the old device names.
+	  If you are sure your bootloader can handle this feel free
+	  to dectivate this.
+
+config SERIAL_STMP_DBG
+        tristate "STMP debug serial port support"
+        depends on ARCH_STMP3XXX
+        select SERIAL_CORE
+        help
+          Driver for Sigmatel 36XX/37XX internal debug serial port
+
+config SERIAL_STMP_DBG_CONSOLE
+        bool "Support for console on STMP37XX DBG serial port"
+        depends on SERIAL_STMP_DBG=y
+        select SERIAL_CORE_CONSOLE
+        ---help---
+          Say Y here if you wish to use the STMP36XX/37XX debug serial port 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).
+
+          Even if you say Y here, the currently visible framebuffer 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=ttyAM0". (Try "man bootparam" or see the documentation of
+          your boot loader (lilo or loadlin) about how to pass options to the
+          kernel at boot time.)
+
+config SERIAL_STMP_APP
+        tristate "STMP app serial port support"
+        depends on ARCH_STMP3XXX
+        select SERIAL_CORE
+        help
+          Driver for Sigmatel 36XX/37XX internal application serial port
+
 endmenu
diff --git a/drivers/serial/Makefile b/drivers/serial/Makefile
index 0c17c8ddb19d..e5a112b79d79 100644
--- a/drivers/serial/Makefile
+++ b/drivers/serial/Makefile
@@ -69,7 +69,13 @@ obj-$(CONFIG_SERIAL_SGI_IOC3) += ioc3_serial.o
 obj-$(CONFIG_SERIAL_ATMEL) += atmel_serial.o
 obj-$(CONFIG_SERIAL_UARTLITE) += uartlite.o
 obj-$(CONFIG_SERIAL_NETX) += netx-serial.o
+obj-$(CONFIG_SERIAL_MXC) += mxc_uart.o
+obj-$(CONFIG_SERIAL_MXC_CONSOLE) += mxc_uart_early.o
 obj-$(CONFIG_SERIAL_OF_PLATFORM) += of_serial.o
 obj-$(CONFIG_SERIAL_KS8695) += serial_ks8695.o
 obj-$(CONFIG_KGDB_SERIAL_CONSOLE) += kgdboc.o
 obj-$(CONFIG_SERIAL_QE) += ucc_uart.o
+obj-$(CONFIG_SERIAL_NS921X) += ns921x-serial.o
+obj-$(CONFIG_SERIAL_NS9360) += ns9360-serial.o
+obj-$(CONFIG_SERIAL_STMP_DBG) += stmp-dbg.o
+obj-$(CONFIG_SERIAL_STMP_APP) += stmp-app.o
diff --git a/drivers/serial/mxc_uart.c b/drivers/serial/mxc_uart.c
new file mode 100644
index 000000000000..6827590a605d
--- /dev/null
+++ b/drivers/serial/mxc_uart.c
@@ -0,0 +1,1948 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/serial/mxc_uart.c
+ *
+ * @brief Driver for the Freescale Semiconductor MXC serial ports based on
+ * drivers/char/serial.c, by Linus Torvalds, Theodore Ts'o.
+ *
+ * @ingroup UART
+ */
+
+/*
+ * Include Files
+ */
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/tty.h>
+#include <linux/string.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/serial.h>
+#include <linux/console.h>
+#include <linux/platform_device.h>
+#include <linux/sysrq.h>
+#include <linux/dma-mapping.h>
+#include <linux/clk.h>
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <asm/dma.h>
+#include <asm/div64.h>
+#include <mach/hardware.h>
+#include <mach/mxc_uart.h>
+
+#if defined(CONFIG_SERIAL_MXC_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
+#define SUPPORT_SYSRQ
+#endif
+#define SERIAL_MXC_MAJOR        207
+#define SERIAL_MXC_MINOR        16
+#define MXC_ISR_PASS_LIMIT      256
+#define UART_CREAD_BIT          256
+
+/* IRDA minimum pulse duration in micro seconds */
+#define MIN_PULSE_DUR           2
+/*
+ * Transmit DMA buffer size is set to 1024 bytes, this is limited
+ * by UART_XMIT_SIZE.
+ */
+#define TXDMA_BUFF_SIZE         UART_XMIT_SIZE
+/*
+ * Receive DMA sub-buffer size
+ */
+#define RXDMA_BUFF_SIZE         128
+
+/*!
+ * This structure is used to store the information for DMA data transfer.
+ */
+typedef struct {
+	/*!
+	 * Holds the read channel number.
+	 */
+	int rd_channel;
+	/*!
+	 * Holds the write channel number.
+	 */
+	int wr_channel;
+	/*!
+	 * UART Transmit Event ID
+	 */
+	int tx_event_id;
+	/*!
+	 * UART Receive Event ID
+	 */
+	int rx_event_id;
+	/*!
+	 * DMA Transmit tasklet
+	 */
+	struct tasklet_struct dma_tx_tasklet;
+	/*!
+	 * Flag indicates if the channel is in use
+	 */
+	int dma_txchnl_inuse;
+} dma_info;
+
+/*!
+ * This is used to indicate if we want echo cancellation in the Irda mode.
+ */
+static int echo_cancel;
+extern void gpio_uart_active(int port, int no_irda);
+extern void gpio_uart_inactive(int port, int no_irda);
+extern void config_uartdma_event(int port);
+
+static uart_mxc_port *mxc_ports[MXC_UART_NR];
+
+/*!
+ * This array holds the DMA channel information for each MXC UART
+ */
+static dma_info dma_list[MXC_UART_NR];
+
+/*!
+ * This function is called by the core driver to stop UART transmission.
+ * This might be due to the TTY layer indicating that the user wants to stop
+ * transmission.
+ *
+ * @param   port       the port structure for the UART passed in by the core
+ *                     driver
+ */
+static void mxcuart_stop_tx(struct uart_port *port)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) port;
+	volatile unsigned int cr1;
+
+	cr1 = readl(port->membase + MXC_UARTUCR1);
+	/* Disable Transmitter rdy interrupt */
+	if (umxc->dma_enabled == 1) {
+		cr1 &= ~MXC_UARTUCR1_TXDMAEN;
+	} else {
+		cr1 &= ~MXC_UARTUCR1_TRDYEN;
+	}
+	writel(cr1, port->membase + MXC_UARTUCR1);
+}
+
+/*!
+ * DMA Transmit tasklet method is scheduled on completion of a DMA transmit
+ * to send out any more data that is available in the UART xmit buffer.
+ *
+ * @param   arg  driver private data
+ */
+static void dma_tx_do_tasklet(unsigned long arg)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) arg;
+	struct circ_buf *xmit = &umxc->port.info->xmit;
+	mxc_dma_requestbuf_t writechnl_request;
+	int tx_num;
+	unsigned long flags;
+
+	spin_lock_irqsave(&umxc->port.lock, flags);
+	tx_num = uart_circ_chars_pending(xmit);
+	if (tx_num > 0) {
+		if (xmit->tail > xmit->head) {
+			memcpy(umxc->tx_buf, xmit->buf + xmit->tail,
+			       UART_XMIT_SIZE - xmit->tail);
+			memcpy(umxc->tx_buf + (UART_XMIT_SIZE - xmit->tail),
+			       xmit->buf, xmit->head);
+		} else {
+			memcpy(umxc->tx_buf, xmit->buf + xmit->tail, tx_num);
+		}
+		umxc->tx_handle = dma_map_single(umxc->port.dev, umxc->tx_buf,
+						 TXDMA_BUFF_SIZE,
+						 DMA_TO_DEVICE);
+
+		writechnl_request.dst_addr = umxc->port.mapbase + MXC_UARTUTXD;
+		writechnl_request.src_addr = umxc->tx_handle;
+		writechnl_request.num_of_bytes = tx_num;
+
+		if ((mxc_dma_config(dma_list[umxc->port.line].wr_channel,
+				    &writechnl_request, 1,
+				    MXC_DMA_MODE_WRITE)) == 0) {
+			mxc_dma_enable(dma_list[umxc->port.line].wr_channel);
+		}
+	} else {
+		/* No more data available in the xmit queue, clear the flag */
+		dma_list[umxc->port.line].dma_txchnl_inuse = 0;
+	}
+	spin_unlock_irqrestore(&umxc->port.lock, flags);
+}
+
+/*!
+ * DMA Write callback is called by the SDMA controller after it has sent out all
+ * the data from the user buffer. This function updates the xmit buffer pointers.
+ *
+ * @param   arg   driver private data
+ * @param   error any DMA error
+ * @param   count amount of data that was transferred
+ */
+static void mxcuart_dma_writecallback(void *arg, int error, unsigned int count)
+{
+	uart_mxc_port *umxc = arg;
+	struct circ_buf *xmit = &umxc->port.info->xmit;
+	int tx_num;
+
+	if (error != MXC_DMA_TRANSFER_ERROR) {
+		tx_num = count;
+		umxc->port.icount.tx += tx_num;
+		xmit->tail = (xmit->tail + tx_num) & (UART_XMIT_SIZE - 1);
+	}
+
+	dma_unmap_single(umxc->port.dev, umxc->tx_handle, TXDMA_BUFF_SIZE,
+			 DMA_TO_DEVICE);
+	tx_num = uart_circ_chars_pending(xmit);
+	/* Schedule a tasklet to send out the pending characters */
+	if (tx_num > 0) {
+		tasklet_schedule(&dma_list[umxc->port.line].dma_tx_tasklet);
+	} else {
+		dma_list[umxc->port.line].dma_txchnl_inuse = 0;
+	}
+	if (tx_num < WAKEUP_CHARS) {
+		uart_write_wakeup(&umxc->port);
+	}
+}
+
+/*!
+ * This function is called by the core driver to start transmitting characters.
+ * This function enables the transmit interrupts.
+ *
+ * @param   port       the port structure for the UART passed in by the core
+ *                     driver
+ */
+static void mxcuart_start_tx(struct uart_port *port)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) port;
+	struct circ_buf *xmit = &umxc->port.info->xmit;
+	volatile unsigned int cr1;
+	mxc_dma_requestbuf_t writechnl_request;
+	int tx_num;
+
+	cr1 = readl(port->membase + MXC_UARTUCR1);
+	/* Enable Transmitter rdy interrupt */
+	if (umxc->dma_enabled == 1) {
+		/*
+		 * If the channel is in use then return immediately and use
+		 * the dma_tx tasklet to transfer queued data when current DMA
+		 * transfer is complete
+		 */
+		if (dma_list[umxc->port.line].dma_txchnl_inuse == 1) {
+			return;
+		}
+		tx_num = uart_circ_chars_pending(xmit);
+		if (tx_num > 0) {
+			dma_list[umxc->port.line].dma_txchnl_inuse = 1;
+			if (xmit->tail > xmit->head) {
+				memcpy(umxc->tx_buf, xmit->buf + xmit->tail,
+				       UART_XMIT_SIZE - xmit->tail);
+				memcpy(umxc->tx_buf +
+				       (UART_XMIT_SIZE - xmit->tail), xmit->buf,
+				       xmit->head);
+			} else {
+				memcpy(umxc->tx_buf, xmit->buf + xmit->tail,
+				       tx_num);
+			}
+			umxc->tx_handle =
+			    dma_map_single(umxc->port.dev, umxc->tx_buf,
+					   TXDMA_BUFF_SIZE, DMA_TO_DEVICE);
+
+			writechnl_request.dst_addr =
+			    umxc->port.mapbase + MXC_UARTUTXD;
+			writechnl_request.src_addr = umxc->tx_handle;
+			writechnl_request.num_of_bytes = tx_num;
+			if ((mxc_dma_config
+			     (dma_list[umxc->port.line].wr_channel,
+			      &writechnl_request, 1,
+			      MXC_DMA_MODE_WRITE)) == 0) {
+				mxc_dma_enable(dma_list[umxc->port.line].
+					       wr_channel);
+			}
+			cr1 |= MXC_UARTUCR1_TXDMAEN;
+		}
+	} else {
+		cr1 |= MXC_UARTUCR1_TRDYEN;
+	}
+	writel(cr1, port->membase + MXC_UARTUCR1);
+}
+
+/*!
+ * This function is called by the core driver to stop receiving characters; the
+ * port is in the process of being closed.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ */
+static void mxcuart_stop_rx(struct uart_port *port)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) port;
+	volatile unsigned int cr1;
+
+	cr1 = readl(port->membase + MXC_UARTUCR1);
+	if (umxc->dma_enabled == 1) {
+		cr1 &= ~MXC_UARTUCR1_RXDMAEN;
+	} else {
+		cr1 &= ~MXC_UARTUCR1_RRDYEN;
+	}
+	writel(cr1, port->membase + MXC_UARTUCR1);
+}
+
+/*!
+ * This function is called by the core driver to enable the modem status
+ * interrupts. If the port is configured to be in DTE mode then it enables the
+ * DCDDELT and RIDELT interrupts in addition to the DTRDEN interrupt. The RTSDEN
+ * interrupt is enabled only for interrupt-driven hardware flow control.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ */
+static void mxcuart_enable_ms(struct uart_port *port)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) port;
+	volatile unsigned int cr1, cr3;
+
+	/*
+	 * RTS interrupt is enabled only if we are using interrupt-driven
+	 * software controlled hardware flow control
+	 */
+	if (umxc->hardware_flow == 0) {
+		cr1 = readl(umxc->port.membase + MXC_UARTUCR1);
+		cr1 |= MXC_UARTUCR1_RTSDEN;
+		writel(cr1, umxc->port.membase + MXC_UARTUCR1);
+	}
+	cr3 = readl(umxc->port.membase + MXC_UARTUCR3);
+	cr3 |= MXC_UARTUCR3_DTRDEN;
+	if (umxc->mode == MODE_DTE) {
+		cr3 |= MXC_UARTUCR3_DCD | MXC_UARTUCR3_RI;
+	}
+	writel(cr3, umxc->port.membase + MXC_UARTUCR3);
+}
+
+/*!
+ * This function is called from the interrupt service routine if the status bit
+ * indicates that the receive fifo data level is above the set threshold. The
+ * function reads the character and queues them into the TTY layers read
+ * buffer. The function also looks for break characters, parity and framing
+ * errors in the received character and sets the appropriate flag in the TTY
+ * receive buffer.
+ *
+ * @param   umxc   the MXC UART port structure, this includes the \b uart_port
+ *                 structure and other members that are specific to MXC UARTs
+ */
+static void mxcuart_rx_chars(uart_mxc_port * umxc)
+{
+	volatile unsigned int ch, sr2;
+	unsigned int status, flag, max_count = 256;
+
+	sr2 = readl(umxc->port.membase + MXC_UARTUSR2);
+	while (((sr2 & MXC_UARTUSR2_RDR) == 1) && (max_count-- > 0)) {
+		ch = readl(umxc->port.membase + MXC_UARTURXD);
+
+		flag = TTY_NORMAL;
+		status = ch | UART_CREAD_BIT;
+		ch &= 0xFF;	/* Clear the upper bits */
+		umxc->port.icount.rx++;
+
+		/*
+		 * Check to see if there is an  error in the received
+		 * character. Perform the appropriate actions based on the
+		 * error bit that was set.
+		 */
+		if (status & MXC_UARTURXD_ERR) {
+			if (status & MXC_UARTURXD_BRK) {
+				/*
+				 * Clear the frame and parity error bits
+				 * as these always get set on receiving a
+				 * break character
+				 */
+				status &= ~(MXC_UARTURXD_FRMERR |
+					    MXC_UARTURXD_PRERR);
+				umxc->port.icount.brk++;
+				if (uart_handle_break(&umxc->port)) {
+					goto ignore_char;
+				}
+			} else if (status & MXC_UARTURXD_FRMERR) {
+				umxc->port.icount.frame++;
+			} else if (status & MXC_UARTURXD_PRERR) {
+				umxc->port.icount.parity++;
+			}
+			if (status & MXC_UARTURXD_OVRRUN) {
+				umxc->port.icount.overrun++;
+			}
+
+			status &= umxc->port.read_status_mask;
+
+			if (status & MXC_UARTURXD_BRK) {
+				flag = TTY_BREAK;
+			} else if (status & MXC_UARTURXD_FRMERR) {
+				flag = TTY_FRAME;
+			} else if (status & MXC_UARTURXD_PRERR) {
+				flag = TTY_PARITY;
+			}
+		}
+
+		if (uart_handle_sysrq_char(&umxc->port, ch)) {
+			goto ignore_char;
+		}
+
+		uart_insert_char(&umxc->port, status, MXC_UARTURXD_OVRRUN, ch,
+				 flag);
+	      ignore_char:
+		sr2 = readl(umxc->port.membase + MXC_UARTUSR2);
+	}
+	tty_flip_buffer_push(umxc->port.info->port.tty);
+}
+
+/*!
+ * This function is called from the interrupt service routine if the status bit
+ * indicates that the transmit fifo is emptied below its set threshold and
+ * requires data. The function pulls characters from the TTY layers write
+ * buffer and writes it out to the UART transmit fifo.
+ *
+ * @param   umxc   the MXC UART port structure, this includes the \b uart_port
+ *                 structure and other members that are specific to MXC UARTs
+ */
+static void mxcuart_tx_chars(uart_mxc_port * umxc)
+{
+	struct circ_buf *xmit = &umxc->port.info->xmit;
+	int count;
+
+	/*
+	 * Transmit the XON/XOFF character if required
+	 */
+	if (umxc->port.x_char) {
+		writel(umxc->port.x_char, umxc->port.membase + MXC_UARTUTXD);
+		umxc->port.icount.tx++;
+		umxc->port.x_char = 0;
+		return;
+	}
+
+	/*
+	 * Check to see if there is any data to be sent and that the
+	 * port has not been currently stopped by anything.
+	 */
+	if (uart_circ_empty(xmit) || uart_tx_stopped(&umxc->port)) {
+		mxcuart_stop_tx(&umxc->port);
+		return;
+	}
+
+	count = umxc->port.fifosize - umxc->tx_threshold;
+	do {
+		writel(xmit->buf[xmit->tail],
+		       umxc->port.membase + MXC_UARTUTXD);
+		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		umxc->port.icount.tx++;
+		if (uart_circ_empty(xmit)) {
+			break;
+		}
+	} while (--count > 0);
+
+	/*
+	 * Check to see if we have flushed enough characters to ask for more
+	 * to be sent to us, if so, we notify the user space that we can
+	 * accept more data
+	 */
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS) {
+		uart_write_wakeup(&umxc->port);
+	}
+
+	if (uart_circ_empty(xmit)) {
+		mxcuart_stop_tx(&umxc->port);
+	}
+}
+
+/*!
+ * This function is called from the interrupt service routine if there is a
+ * change in the modem signals. This function handles these signal changes and
+ * also clears the appropriate status register bits.
+ *
+ * @param   umxc   the MXC UART port structure, this includes the \b uart_port
+ *                 structure and other members that are specific to MXC UARTs
+ * @param   sr1    contents of status register 1
+ * @param   sr2    contents of status register 2
+ */
+static void mxcuart_modem_status(uart_mxc_port * umxc, unsigned int sr1,
+				 unsigned int sr2)
+{
+	if (umxc->mode == MODE_DTE) {
+		if (sr2 & MXC_UARTUSR2_DCDDELT) {
+			uart_handle_dcd_change(&umxc->port,
+					       !(sr2 & MXC_UARTUSR2_DCDIN));
+		}
+		if (sr2 & MXC_UARTUSR2_RIDELT) {
+			umxc->port.icount.rng++;
+		}
+	}
+	if (sr1 & MXC_UARTUSR1_DTRD) {
+		umxc->port.icount.dsr++;
+	}
+	if ((umxc->hardware_flow == 0) && (sr1 & MXC_UARTUSR1_RTSD)) {
+		uart_handle_cts_change(&umxc->port, sr1 & MXC_UARTUSR1_RTSS);
+	}
+
+	wake_up_interruptible(&umxc->port.info->delta_msr_wait);
+}
+
+/*!
+ * Interrupt service routine registered to handle the muxed ANDed interrupts.
+ * This routine is registered only in the case where the UART interrupts are
+ * muxed.
+ *
+ * @param   irq    the interrupt number
+ * @param   dev_id driver private data
+ *
+ * @return  The function returns \b IRQ_RETVAL(1) if interrupt was handled,
+ *          returns \b IRQ_RETVAL(0) if the interrupt was not handled.
+ *          \b IRQ_RETVAL is defined in \b include/linux/interrupt.h.
+ */
+static irqreturn_t mxcuart_int(int irq, void *dev_id)
+{
+	uart_mxc_port *umxc = dev_id;
+	volatile unsigned int sr1, sr2, cr1, cr;
+	unsigned int pass_counter = MXC_ISR_PASS_LIMIT;
+	unsigned int term_cond = 0;
+	int handled = 0;
+
+	sr1 = readl(umxc->port.membase + MXC_UARTUSR1);
+	sr2 = readl(umxc->port.membase + MXC_UARTUSR2);
+	cr1 = readl(umxc->port.membase + MXC_UARTUCR1);
+
+	do {
+		/* Clear the bits that triggered the interrupt */
+		writel(sr1, umxc->port.membase + MXC_UARTUSR1);
+		writel(sr2, umxc->port.membase + MXC_UARTUSR2);
+		/*
+		 * Read if there is data available
+		 */
+		if (sr2 & MXC_UARTUSR2_RDR) {
+			mxcuart_rx_chars(umxc);
+		}
+
+		if ((sr1 & (MXC_UARTUSR1_RTSD | MXC_UARTUSR1_DTRD)) ||
+		    (sr2 & (MXC_UARTUSR2_DCDDELT | MXC_UARTUSR2_RIDELT))) {
+			mxcuart_modem_status(umxc, sr1, sr2);
+		}
+
+		/*
+		 * Send data if there is data to be sent
+		 */
+		if ((cr1 & MXC_UARTUCR1_TRDYEN) && (sr1 & MXC_UARTUSR1_TRDY)) {
+			/* Echo cancellation for IRDA Transmit chars */
+			if (umxc->ir_mode == IRDA && echo_cancel) {
+				/* Disable the receiver */
+				cr = readl(umxc->port.membase + MXC_UARTUCR2);
+				cr &= ~MXC_UARTUCR2_RXEN;
+				writel(cr, umxc->port.membase + MXC_UARTUCR2);
+				/* Enable Transmit complete intr to reenable RX */
+				cr = readl(umxc->port.membase + MXC_UARTUCR4);
+				cr |= MXC_UARTUCR4_TCEN;
+				writel(cr, umxc->port.membase + MXC_UARTUCR4);
+			}
+			mxcuart_tx_chars(umxc);
+		}
+
+		if (pass_counter-- == 0) {
+			break;
+		}
+
+		sr1 = readl(umxc->port.membase + MXC_UARTUSR1);
+		sr2 = readl(umxc->port.membase + MXC_UARTUSR2);
+
+		/* Is the transmit complete to reenable the receiver? */
+		if (umxc->ir_mode == IRDA && echo_cancel) {
+			if (sr2 & MXC_UARTUSR2_TXDC) {
+				cr = readl(umxc->port.membase + MXC_UARTUCR2);
+				cr |= MXC_UARTUCR2_RXEN;
+				writel(cr, umxc->port.membase + MXC_UARTUCR2);
+				/* Disable the Transmit complete interrupt bit */
+				cr = readl(umxc->port.membase + MXC_UARTUCR4);
+				cr &= ~MXC_UARTUCR4_TCEN;
+				writel(cr, umxc->port.membase + MXC_UARTUCR4);
+			}
+		}
+
+		/*
+		 * If there is no data to send or receive and if there is no
+		 * change in the modem status signals then quit the routine
+		 */
+		term_cond = sr1 & (MXC_UARTUSR1_RTSD | MXC_UARTUSR1_DTRD);
+		term_cond |= sr2 & (MXC_UARTUSR2_RDR | MXC_UARTUSR2_DCDDELT);
+		term_cond |= !(sr2 & MXC_UARTUSR2_TXFE);
+	} while (term_cond > 0);
+
+	handled = 1;
+	return IRQ_RETVAL(handled);
+}
+
+/*!
+ * Interrupt service routine registered to handle the transmit interrupts. This
+ * routine is registered only in the case where the UART interrupts are not
+ * muxed.
+ *
+ * @param   irq    the interrupt number
+ * @param   dev_id driver private data
+ *
+ * @return  The function returns \b IRQ_RETVAL(1) if interrupt was handled,
+ *          returns \b IRQ_RETVAL(0) if the interrupt was not handled.
+ *          \b IRQ_RETVAL is defined in include/linux/interrupt.h.
+ */
+static irqreturn_t mxcuart_tx_int(int irq, void *dev_id)
+{
+	uart_mxc_port *umxc = dev_id;
+	int handled = 0;
+	volatile unsigned int sr2, cr;
+
+	/* Echo cancellation for IRDA Transmit chars */
+	if (umxc->ir_mode == IRDA && echo_cancel) {
+		/* Disable the receiver */
+		cr = readl(umxc->port.membase + MXC_UARTUCR2);
+		cr &= ~MXC_UARTUCR2_RXEN;
+		writel(cr, umxc->port.membase + MXC_UARTUCR2);
+		/* Enable Transmit complete to reenable receiver */
+		cr = readl(umxc->port.membase + MXC_UARTUCR4);
+		cr |= MXC_UARTUCR4_TCEN;
+		writel(cr, umxc->port.membase + MXC_UARTUCR4);
+	}
+
+	mxcuart_tx_chars(umxc);
+
+	/* Is the transmit complete to reenable the receiver? */
+	if (umxc->ir_mode == IRDA && echo_cancel) {
+		sr2 = readl(umxc->port.membase + MXC_UARTUSR2);
+		if (sr2 & MXC_UARTUSR2_TXDC) {
+			cr = readl(umxc->port.membase + MXC_UARTUCR2);
+			cr |= MXC_UARTUCR2_RXEN;
+			writel(cr, umxc->port.membase + MXC_UARTUCR2);
+			/* Disable the Transmit complete interrupt bit */
+			cr = readl(umxc->port.membase + MXC_UARTUCR4);
+			cr &= ~MXC_UARTUCR4_TCEN;
+			writel(cr, umxc->port.membase + MXC_UARTUCR4);
+		}
+	}
+
+	handled = 1;
+
+	return IRQ_RETVAL(handled);
+}
+
+/*!
+ * Interrupt service routine registered to handle the receive interrupts. This
+ * routine is registered only in the case where the UART interrupts are not
+ * muxed.
+ *
+ * @param   irq    the interrupt number
+ * @param   dev_id driver private data
+ *
+ * @return  The function returns \b IRQ_RETVAL(1) if interrupt was handled,
+ *          returns \b IRQ_RETVAL(0) if the interrupt was not handled.
+ *          \b IRQ_RETVAL is defined in include/linux/interrupt.h.
+ */
+static irqreturn_t mxcuart_rx_int(int irq, void *dev_id)
+{
+	uart_mxc_port *umxc = dev_id;
+	int handled = 0;
+
+	/* Clear the aging timer bit */
+	writel(MXC_UARTUSR1_AGTIM, umxc->port.membase + MXC_UARTUSR1);
+	mxcuart_rx_chars(umxc);
+	handled = 1;
+
+	return IRQ_RETVAL(handled);
+}
+
+/*!
+ * Interrupt service routine registered to handle the master interrupts. This
+ * routine is registered only in the case where the UART interrupts are not
+ * muxed.
+ *
+ * @param   irq    the interrupt number
+ * @param   dev_id driver private data
+ *
+ * @return  The function returns \b IRQ_RETVAL(1) if interrupt was handled,
+ *          returns \b IRQ_RETVAL(0) if the interrupt was not handled.
+ *          \b IRQ_RETVAL is defined in include/linux/interrupt.h.
+ */
+static irqreturn_t mxcuart_mint_int(int irq, void *dev_id)
+{
+	uart_mxc_port *umxc = dev_id;
+	int handled = 0;
+	volatile unsigned int sr1, sr2;
+
+	sr1 = readl(umxc->port.membase + MXC_UARTUSR1);
+	sr2 = readl(umxc->port.membase + MXC_UARTUSR2);
+	/* Clear the modem status interrupt bits */
+	writel(MXC_UARTUSR1_RTSD | MXC_UARTUSR1_DTRD,
+	       umxc->port.membase + MXC_UARTUSR1);
+	writel(MXC_UARTUSR2_DCDDELT | MXC_UARTUSR2_RIDELT,
+	       umxc->port.membase + MXC_UARTUSR2);
+	mxcuart_modem_status(umxc, sr1, sr2);
+	handled = 1;
+
+	return IRQ_RETVAL(handled);
+}
+
+/*!
+ * This function is called by the core driver to test whether the transmitter
+ * fifo and shift register for the UART port are empty.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ *
+ * @return  The function returns TIOCSER_TEMT if it is empty, else returns 0.
+ */
+static unsigned int mxcuart_tx_empty(struct uart_port *port)
+{
+	volatile unsigned int sr2;
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->lock, flags);
+	sr2 = readl(port->membase + MXC_UARTUSR2);
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	return sr2 & MXC_UARTUSR2_TXDC ? TIOCSER_TEMT : 0;
+}
+
+/*!
+ * This function is called by the core driver to get the current status of the
+ * modem input signals. The state of the output signals is not collected.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ *
+ * @return  The function returns an integer that contains the ORed value of the
+ *          status of all the modem input signals or error.
+ */
+static unsigned int mxcuart_get_mctrl(struct uart_port *port)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) port;
+	unsigned int result = 0;
+	volatile unsigned int sr1, sr2;
+
+	sr1 = readl(umxc->port.membase + MXC_UARTUSR1);
+	sr2 = readl(umxc->port.membase + MXC_UARTUSR2);
+
+	if (sr1 & MXC_UARTUSR1_RTSS) {
+		result |= TIOCM_CTS;
+	}
+	if (umxc->mode == MODE_DTE) {
+		if (!(sr2 & MXC_UARTUSR2_DCDIN)) {
+			result |= TIOCM_CAR;
+		}
+		if (!(sr2 & MXC_UARTUSR2_RIIN)) {
+			result |= TIOCM_RI;
+		}
+	}
+	return result;
+}
+
+/*!
+ * This function is called by the core driver to set the state of the modem
+ * control lines.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ * @param   mctrl  the state that the modem control lines should be changed to
+ */
+static void mxcuart_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) port;
+	volatile unsigned int cr2 = 0, cr3 = 0, uts = 0;
+
+	cr2 = readl(port->membase + MXC_UARTUCR2);
+	cr3 = readl(port->membase + MXC_UARTUCR3);
+	uts = readl(port->membase + MXC_UARTUTS);
+
+	if (mctrl & TIOCM_RTS) {
+		/*
+		 * Return to hardware-driven hardware flow control if the
+		 * option is enabled
+		 */
+		if (umxc->hardware_flow == 1) {
+			cr2 |= MXC_UARTUCR2_CTSC;
+		} else {
+			cr2 |= MXC_UARTUCR2_CTS;
+			cr2 &= ~MXC_UARTUCR2_CTSC;
+		}
+	} else {
+		cr2 &= ~(MXC_UARTUCR2_CTS | MXC_UARTUCR2_CTSC);
+	}
+	writel(cr2, port->membase + MXC_UARTUCR2);
+
+	if (mctrl & TIOCM_DTR) {
+		cr3 |= MXC_UARTUCR3_DSR;
+	} else {
+		cr3 &= ~MXC_UARTUCR3_DSR;
+	}
+	writel(cr3, port->membase + MXC_UARTUCR3);
+
+	if (mctrl & TIOCM_LOOP) {
+		if (umxc->ir_mode == IRDA) {
+			echo_cancel = 0;
+		} else {
+			uts |= MXC_UARTUTS_LOOP;
+		}
+	} else {
+		if (umxc->ir_mode == IRDA) {
+			echo_cancel = 1;
+		} else {
+			uts &= ~MXC_UARTUTS_LOOP;
+		}
+	}
+	writel(uts, port->membase + MXC_UARTUTS);
+}
+
+/*!
+ * This function is called by the core driver to control the transmission of
+ * the break signal. If break_state is non-zero, the break signal is
+ * transmitted, the signal is terminated when another call is made with
+ * break_state set to 0.
+ *
+ * @param   port          the port structure for the UART passed in by the core
+ *                        driver
+ * @param   break_state   the requested state of the break signal
+ */
+static void mxcuart_break_ctl(struct uart_port *port, int break_state)
+{
+	volatile unsigned int cr1;
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->lock, flags);
+	cr1 = readl(port->membase + MXC_UARTUCR1);
+	if (break_state == -1) {
+		cr1 |= MXC_UARTUCR1_SNDBRK;
+	} else {
+		cr1 &= ~MXC_UARTUCR1_SNDBRK;
+	}
+	writel(cr1, port->membase + MXC_UARTUCR1);
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+/*!
+ * The read DMA callback, this method is called when the DMA buffer has received its
+ * data. This functions copies the data to the tty buffer and updates the tty buffer
+ * pointers. It also queues the DMA buffer back to the DMA system.
+ *
+ * @param   arg   driver private data
+ * @param   error any DMA error
+ * @param   cnt   amount of data that was transferred
+ */
+static void mxcuart_dmaread_callback(void *arg, int error, unsigned int cnt)
+{
+	uart_mxc_port *umxc = arg;
+	struct tty_struct *tty = umxc->port.info->port.tty;
+	int buff_id, flip_cnt, num_bufs;
+	mxc_dma_requestbuf_t readchnl_request;
+	mxc_uart_rxdmamap *rx_buf_elem = NULL;
+	unsigned int sr1, sr2;
+	char flag;
+
+	num_bufs = umxc->dma_rxbuf_size / RXDMA_BUFF_SIZE;
+	/* Clear the aging timer bit */
+	writel(MXC_UARTUSR1_AGTIM, umxc->port.membase + MXC_UARTUSR1);
+
+	buff_id = umxc->dma_rxbuf_id;
+	flag = TTY_NORMAL;
+
+	if ((umxc->dma_rxbuf_id += 1) >= num_bufs) {
+		umxc->dma_rxbuf_id = 0;
+	}
+
+	rx_buf_elem = (mxc_uart_rxdmamap *) (umxc->rx_dmamap + buff_id);
+
+	if (error == MXC_DMA_TRANSFER_ERROR) {
+
+		sr1 = __raw_readl(umxc->port.membase + MXC_UARTUSR1);
+		sr2 = __raw_readl(umxc->port.membase + MXC_UARTUSR2);
+
+		if (sr2 & MXC_UARTUSR2_BRCD) {
+			umxc->port.icount.brk++;
+			if (uart_handle_break(&umxc->port)) {
+				goto drop_data;
+			}
+		} else if (sr1 & MXC_UARTUSR1_PARITYERR) {
+			umxc->port.icount.parity++;
+		} else if (sr1 & MXC_UARTUSR1_FRAMERR) {
+			umxc->port.icount.frame++;
+		} else if (sr2 & MXC_UARTUSR2_ORE) {
+			umxc->port.icount.overrun++;
+
+		}
+
+		if (umxc->port.read_status_mask & MXC_UARTURXD_BRK) {
+			if (sr2 & MXC_UARTUSR2_BRCD)
+				flag = TTY_BREAK;
+		} else if (umxc->port.read_status_mask & MXC_UARTURXD_PRERR) {
+			if (sr1 & MXC_UARTUSR1_PARITYERR)
+				flag = TTY_PARITY;
+		} else if (umxc->port.read_status_mask & MXC_UARTURXD_FRMERR) {
+			if (sr1 & MXC_UARTUSR1_FRAMERR)
+				flag = TTY_FRAME;
+		} else if (umxc->port.read_status_mask & MXC_UARTURXD_OVRRUN) {
+			if (sr2 & MXC_UARTUSR2_ORE)
+				flag = TTY_OVERRUN;
+		}
+/* By default clearing all error bits in status reg */
+		__raw_writel((MXC_UARTUSR2_BRCD | MXC_UARTUSR2_ORE),
+			     umxc->port.membase + MXC_UARTUSR2);
+		__raw_writel((MXC_UARTUSR1_PARITYERR | MXC_UARTUSR1_FRAMERR),
+			     umxc->port.membase + MXC_UARTUSR1);
+	}
+
+	flip_cnt = tty_buffer_request_room(tty, cnt);
+
+	/* Check for space availability in the TTY Flip buffer */
+	if (flip_cnt <= 0) {
+		goto drop_data;
+	}
+	umxc->port.icount.rx += flip_cnt;
+
+	tty_insert_flip_string(tty, rx_buf_elem->rx_buf, flip_cnt);
+
+	if (flag != TTY_NORMAL) {
+		tty_insert_flip_char(tty, 0, flag);
+	}
+
+	tty_flip_buffer_push(tty);
+	umxc->port.info->port.tty->real_raw = 1;
+
+      drop_data:
+	readchnl_request.src_addr = umxc->port.mapbase;
+	readchnl_request.dst_addr = rx_buf_elem->rx_handle;
+	readchnl_request.num_of_bytes = RXDMA_BUFF_SIZE;
+	mxc_dma_config(dma_list[umxc->port.line].rd_channel, &readchnl_request,
+		       1, MXC_DMA_MODE_READ);
+	mxc_dma_enable(dma_list[umxc->port.line].rd_channel);
+}
+
+/*!
+ * Allocates DMA read and write channels, creates DMA read and write buffers and
+ * sets the channel specific parameters.
+ *
+ * @param   d_info the structure that holds all the DMA information for a
+ *                 particular MXC UART
+ * @param   umxc   the MXC UART port structure, this includes the \b uart_port
+ *                 structure and other members that are specific to MXC UARTs
+ *
+ * @return  The function returns 0 on success and a non-zero value on failure.
+ */
+static int mxcuart_initdma(dma_info * d_info, uart_mxc_port * umxc)
+{
+	int ret = 0, rxbufs, i, j;
+	mxc_dma_requestbuf_t *readchnl_reqelem;
+	mxc_uart_rxdmamap *rx_buf_elem;
+
+	/* Request for the read and write channels */
+	d_info->rd_channel = mxc_dma_request(umxc->dma_rx_id, "MXC UART Read");
+	if (d_info->rd_channel < 0) {
+		printk(KERN_ERR "MXC UART: Cannot allocate DMA read channel\n");
+		return -1;
+	} else {
+		d_info->wr_channel =
+		    mxc_dma_request(umxc->dma_tx_id, "MXC UART Write");
+		if (d_info->wr_channel < 0) {
+			mxc_dma_free(d_info->rd_channel);
+			printk(KERN_ERR
+			       "MXC UART: Cannot allocate DMA write channel\n");
+			return -1;
+		}
+	}
+
+	/* Allocate the DMA Transmit Buffer */
+	if ((umxc->tx_buf = kmalloc(TXDMA_BUFF_SIZE, GFP_KERNEL)) == NULL) {
+		ret = -1;
+		goto err_dma_tx_buff;
+	}
+	rxbufs = umxc->dma_rxbuf_size / RXDMA_BUFF_SIZE;
+	/* Allocate the DMA Virtual Receive Buffer */
+	if ((umxc->rx_dmamap = kmalloc(rxbufs * sizeof(mxc_uart_rxdmamap),
+				       GFP_KERNEL)) == NULL) {
+		ret = -1;
+		goto err_dma_rx_buff;
+	}
+
+	/* Allocate the DMA Receive Request structures */
+	if ((readchnl_reqelem =
+	     kmalloc(rxbufs * sizeof(mxc_dma_requestbuf_t),
+		     GFP_KERNEL)) == NULL) {
+		ret = -1;
+		goto err_request;
+	}
+
+	for (i = 0; i < rxbufs; i++) {
+		rx_buf_elem = (mxc_uart_rxdmamap *) (umxc->rx_dmamap + i);
+		rx_buf_elem->rx_buf =
+		    dma_alloc_coherent(NULL, RXDMA_BUFF_SIZE,
+				       &rx_buf_elem->rx_handle, GFP_DMA);
+		if (rx_buf_elem->rx_buf == NULL) {
+			for (j = 0; j < i; j++) {
+				rx_buf_elem =
+				    (mxc_uart_rxdmamap *) (umxc->rx_dmamap + j);
+				dma_free_coherent(NULL, RXDMA_BUFF_SIZE,
+						  rx_buf_elem->rx_buf,
+						  rx_buf_elem->rx_handle);
+			}
+			ret = -1;
+			goto cleanup;
+		}
+	}
+
+	umxc->dma_rxbuf_id = 0;
+	/* Setup the DMA read request structures */
+	for (i = 0; i < rxbufs; i++) {
+		rx_buf_elem = (mxc_uart_rxdmamap *) (umxc->rx_dmamap + i);
+		(readchnl_reqelem + i)->src_addr = umxc->port.mapbase;
+		(readchnl_reqelem + i)->dst_addr = rx_buf_elem->rx_handle;
+		(readchnl_reqelem + i)->num_of_bytes = RXDMA_BUFF_SIZE;
+	}
+	mxc_dma_config(d_info->rd_channel, readchnl_reqelem, rxbufs,
+		       MXC_DMA_MODE_READ);
+	mxc_dma_callback_set(d_info->rd_channel, mxcuart_dmaread_callback,
+			     umxc);
+	mxc_dma_callback_set(d_info->wr_channel, mxcuart_dma_writecallback,
+			     umxc);
+
+	/* Start the read channel */
+	mxc_dma_enable(d_info->rd_channel);
+	kfree(readchnl_reqelem);
+	tasklet_init(&d_info->dma_tx_tasklet, dma_tx_do_tasklet,
+		     (unsigned long)umxc);
+	d_info->dma_txchnl_inuse = 0;
+	return ret;
+      cleanup:
+	kfree(readchnl_reqelem);
+      err_request:
+	kfree(umxc->rx_dmamap);
+      err_dma_rx_buff:
+	kfree(umxc->tx_buf);
+      err_dma_tx_buff:
+	mxc_dma_free(d_info->rd_channel);
+	mxc_dma_free(d_info->wr_channel);
+
+	return ret;
+}
+
+/*!
+ * Stops DMA and frees the DMA resources
+ *
+ * @param   d_info the structure that holds all the DMA information for a
+ *                 particular MXC UART
+ * @param   umxc   the MXC UART port structure, this includes the \b uart_port
+ *                 structure and other members that are specific to MXC UARTs
+ */
+static void mxcuart_freedma(dma_info * d_info, uart_mxc_port * umxc)
+{
+	int i, rxbufs;
+	mxc_uart_rxdmamap *rx_buf_elem;
+
+	rxbufs = umxc->dma_rxbuf_size / RXDMA_BUFF_SIZE;
+
+	for (i = 0; i < rxbufs; i++) {
+		rx_buf_elem = (mxc_uart_rxdmamap *) (umxc->rx_dmamap + i);
+		dma_free_coherent(NULL, RXDMA_BUFF_SIZE,
+				  rx_buf_elem->rx_buf, rx_buf_elem->rx_handle);
+	}
+	kfree(umxc->rx_dmamap);
+	kfree(umxc->tx_buf);
+	mxc_dma_free(d_info->rd_channel);
+	mxc_dma_free(d_info->wr_channel);
+}
+
+/*!
+ * This function is called to free the interrupts.
+ *
+ * @param   umxc   the MXC UART port structure, this includes the \b uart_port
+ *                 structure and other members that are specific to MXC UARTs
+ */
+static void mxcuart_free_interrupts(uart_mxc_port * umxc)
+{
+	free_irq(umxc->port.irq, umxc);
+	if (umxc->ints_muxed == 0) {
+		free_irq(umxc->irqs[0], umxc);
+		free_irq(umxc->irqs[1], umxc);
+	}
+}
+
+/*!
+ * Calculate and set the UART port clock value
+ *
+ * @param   umxc     the MXC UART port structure, this includes the \b uart_port
+ *                   structure and other members that are specific to MXC UARTs
+ * @param   per_clk  peripheral clock coming into the MXC UART module
+ * @param   req_baud current baudrate requested
+ * @param   div      returns the reference frequency divider value
+ */
+static void mxcuart_set_ref_freq(uart_mxc_port * umxc, unsigned long per_clk,
+				 unsigned int req_baud, int *div)
+{
+	unsigned int d = 1;
+
+	/*
+	 * Choose the smallest possible prescaler to maximize
+	 * the chance of using integer scaling.  Ensure that
+	 * the calculation won't overflow.  Limit the denom
+	 * to 15 bits since a 16-bit denom doesn't work.
+	 */
+	if (req_baud < (1 << (31 - (4 + 15))))
+		d = per_clk / (req_baud << (4 + 15)) + 1;
+
+	umxc->port.uartclk = per_clk / d;
+
+	/*
+	 * Set the ONEMS register that is used by IR special case bit and
+	 * the Escape character detect logic
+	 */
+	writel(umxc->port.uartclk / 1000, umxc->port.membase + MXC_UARTONEMS);
+	*div = d;
+}
+
+/*!
+ * This function is called by the core driver to initialize the low-level
+ * driver. The function grabs the interrupt resources and registers its
+ * interrupt service routines. It then initializes the IOMUX registers to
+ * configure the pins for UART signals and finally initializes the various
+ * UART registers and enables the port for reception.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ *
+ * @return  The function returns 0 on success and a non-zero value on failure.
+ */
+static int mxcuart_startup(struct uart_port *port)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) port;
+	int retval;
+	volatile unsigned int cr, cr1 = 0, cr2 = 0, ufcr = 0;
+
+	/*
+	 * Some UARTs need separate registrations for the interrupts as
+	 * they do not take the muxed interrupt output to the ARM core
+	 */
+	if (umxc->ints_muxed == 1) {
+		retval = request_irq(umxc->port.irq, mxcuart_int, 0,
+				     "mxcintuart", umxc);
+		if (retval != 0) {
+			return retval;
+		}
+	} else {
+		retval = request_irq(umxc->port.irq, mxcuart_tx_int,
+				     0, "mxcintuart", umxc);
+		if (retval != 0) {
+			return retval;
+		} else {
+			retval = request_irq(umxc->irqs[0], mxcuart_rx_int,
+					     0, "mxcintuart", umxc);
+			if (retval != 0) {
+				free_irq(umxc->port.irq, umxc);
+				return retval;
+			} else {
+				retval =
+				    request_irq(umxc->irqs[1], mxcuart_mint_int,
+						0, "mxcintuart", umxc);
+				if (retval != 0) {
+					free_irq(umxc->port.irq, umxc);
+					free_irq(umxc->irqs[0], umxc);
+					return retval;
+				}
+			}
+		}
+	}
+
+	/* Initialize the DMA if we need SDMA data transfer */
+	if (umxc->dma_enabled == 1) {
+		retval = mxcuart_initdma(dma_list + umxc->port.line, umxc);
+		if (retval != 0) {
+			printk
+			    (KERN_ERR
+			     "MXC UART: Failed to initialize DMA for UART %d\n",
+			     umxc->port.line);
+			mxcuart_free_interrupts(umxc);
+			return retval;
+		}
+		/* Configure the GPR register to receive SDMA events */
+		config_uartdma_event(umxc->port.line);
+	}
+
+	/*
+	 * Clear Status Registers 1 and 2
+	 */
+	writel(0xFFFF, umxc->port.membase + MXC_UARTUSR1);
+	writel(0xFFFF, umxc->port.membase + MXC_UARTUSR2);
+
+	/* Configure the IOMUX for the UART */
+	gpio_uart_active(umxc->port.line, umxc->ir_mode);
+
+	/*
+	 * Set the transceiver invert bits if required
+	 */
+	if (umxc->ir_mode == IRDA) {
+		echo_cancel = 1;
+		writel(umxc->ir_rx_inv | MXC_UARTUCR4_IRSC, umxc->port.membase
+		       + MXC_UARTUCR4);
+		writel(umxc->rxd_mux | umxc->ir_tx_inv,
+		       umxc->port.membase + MXC_UARTUCR3);
+	} else {
+		writel(umxc->rxd_mux, umxc->port.membase + MXC_UARTUCR3);
+	}
+
+	/*
+	 * Initialize UCR1,2 and UFCR registers
+	 */
+	if (umxc->dma_enabled == 1) {
+		cr2 = (MXC_UARTUCR2_TXEN | MXC_UARTUCR2_RXEN);
+	} else {
+		cr2 =
+		    (MXC_UARTUCR2_ATEN | MXC_UARTUCR2_TXEN | MXC_UARTUCR2_RXEN);
+	}
+
+	writel(cr2, umxc->port.membase + MXC_UARTUCR2);
+	/* Wait till we are out of software reset */
+	do {
+		cr = readl(umxc->port.membase + MXC_UARTUCR2);
+	} while (!(cr & MXC_UARTUCR2_SRST));
+
+	if (umxc->mode == MODE_DTE) {
+		ufcr |= ((umxc->tx_threshold << MXC_UARTUFCR_TXTL_OFFSET) |
+			 MXC_UARTUFCR_DCEDTE | MXC_UARTUFCR_RFDIV | umxc->
+			 rx_threshold);
+	} else {
+		ufcr |= ((umxc->tx_threshold << MXC_UARTUFCR_TXTL_OFFSET) |
+			 MXC_UARTUFCR_RFDIV | umxc->rx_threshold);
+	}
+	writel(ufcr, umxc->port.membase + MXC_UARTUFCR);
+
+	/*
+	 * Finally enable the UART and the Receive interrupts
+	 */
+	if (umxc->ir_mode == IRDA) {
+		cr1 |= MXC_UARTUCR1_IREN;
+	}
+	if (umxc->dma_enabled == 1) {
+		cr1 |= (MXC_UARTUCR1_RXDMAEN | MXC_UARTUCR1_ATDMAEN |
+			MXC_UARTUCR1_UARTEN);
+	} else {
+		cr1 |= (MXC_UARTUCR1_RRDYEN | MXC_UARTUCR1_UARTEN);
+	}
+	writel(cr1, umxc->port.membase + MXC_UARTUCR1);
+
+	return 0;
+}
+
+/*!
+ * This function is called by the core driver for the low-level driver to free
+ * its resources. The function frees all its interrupts and disables the UART.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ */
+static void mxcuart_shutdown(struct uart_port *port)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) port;
+
+	/* Disable the IOMUX for the UART */
+	gpio_uart_inactive(umxc->port.line, umxc->ir_mode);
+	mxcuart_free_interrupts(umxc);
+	/* Disable all interrupts, port and break condition */
+	writel(0, umxc->port.membase + MXC_UARTUCR1);
+	writel(0, umxc->port.membase + MXC_UARTUCR3);
+	if (umxc->dma_enabled == 1) {
+		mxcuart_freedma(dma_list + umxc->port.line, umxc);
+	}
+}
+
+/*!
+ * This function is called while changing the UART parameters. It is called to
+ * check if the Infrared special case bit (IRSC) in control register 4 should
+ * be set.
+ *
+ * @param   baudrate   the desired baudrate
+ *
+ * @return  The functions returns 0 if the IRSC bit does not have to be set,
+ *          else it returns a 1.
+ */
+/*
+static int mxcuart_setir_special(u_int baudrate)
+{
+        u_int thresh_val;
+
+        thresh_val = 1000000 / (8 * MIN_PULSE_DUR);
+        if (baudrate > thresh_val) {
+                return 0;
+        }
+
+        return 1;
+}
+*/
+
+/*!
+ * This function is called by the core driver to change the UART parameters,
+ * including baudrate, word length, parity, stop bits. The function also updates
+ * the port structures mask registers to indicate the types of events the user is
+ * interested in receiving.
+ *
+ * @param   port    the port structure for the UART passed in by the core driver
+ * @param   termios the desired termios settings
+ * @param   old     old termios
+ */
+static void mxcuart_set_termios(struct uart_port *port,
+				struct ktermios *termios, struct ktermios *old)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) port;
+	volatile unsigned int cr4 = 0, cr2 = 0, ufcr;
+	u_int num, denom, baud;
+	u_int cr2_mask;		/* Used to add the changes to CR2 */
+	unsigned long flags, per_clk;
+	int div;
+
+	cr2_mask = ~(MXC_UARTUCR2_IRTS | MXC_UARTUCR2_CTSC | MXC_UARTUCR2_PREN |
+		     MXC_UARTUCR2_PROE | MXC_UARTUCR2_STPB | MXC_UARTUCR2_WS);
+
+	per_clk = clk_get_rate(umxc->clk);
+
+	/*
+	 * Ask the core to get the baudrate, if requested baudrate is not
+	 * between max and min, then either use the baudrate in old termios
+	 * setting. If it's still invalid, we try 9600 baud.
+	 */
+	baud = uart_get_baud_rate(&umxc->port, termios, old, 0, per_clk / 16);
+	/* Set the Reference frequency divider */
+	mxcuart_set_ref_freq(umxc, per_clk, baud, &div);
+
+	/* Byte size, default is 8-bit mode */
+	switch (termios->c_cflag & CSIZE) {
+	case CS7:
+		cr2 = 0;
+		break;
+	default:
+		cr2 = MXC_UARTUCR2_WS;
+		break;
+	}
+	/* Check to see if we need 2 Stop bits */
+	if (termios->c_cflag & CSTOPB) {
+		cr2 |= MXC_UARTUCR2_STPB;
+	}
+
+	/* Check to see if we need Parity checking */
+	if (termios->c_cflag & PARENB) {
+		cr2 |= MXC_UARTUCR2_PREN;
+		if (termios->c_cflag & PARODD) {
+			cr2 |= MXC_UARTUCR2_PROE;
+		}
+	}
+	spin_lock_irqsave(&umxc->port.lock, flags);
+
+	ufcr = readl(umxc->port.membase + MXC_UARTUFCR);
+	ufcr = (ufcr & (~MXC_UARTUFCR_RFDIV_MASK)) |
+	    ((6 - div) << MXC_UARTUFCR_RFDIV_OFFSET);
+	writel(ufcr, umxc->port.membase + MXC_UARTUFCR);
+
+	/*
+	 * Update the per-port timeout
+	 */
+	uart_update_timeout(&umxc->port, termios->c_cflag, baud);
+
+	umxc->port.read_status_mask = MXC_UARTURXD_OVRRUN;
+	/*
+	 * Enable appropriate events to be passed to the TTY layer
+	 */
+	if (termios->c_iflag & INPCK) {
+		umxc->port.read_status_mask |= MXC_UARTURXD_FRMERR |
+		    MXC_UARTURXD_PRERR;
+	}
+	if (termios->c_iflag & (BRKINT | PARMRK)) {
+		umxc->port.read_status_mask |= MXC_UARTURXD_BRK;
+	}
+
+	/*
+	 * Characters to ignore
+	 */
+	umxc->port.ignore_status_mask = 0;
+	if (termios->c_iflag & IGNPAR) {
+		umxc->port.ignore_status_mask |= MXC_UARTURXD_FRMERR |
+		    MXC_UARTURXD_PRERR;
+	}
+	if (termios->c_iflag & IGNBRK) {
+		umxc->port.ignore_status_mask |= MXC_UARTURXD_BRK;
+		/*
+		 * If we are ignoring parity and break indicators,
+		 * ignore overruns too (for real raw support)
+		 */
+		if (termios->c_iflag & IGNPAR) {
+			umxc->port.ignore_status_mask |= MXC_UARTURXD_OVRRUN;
+		}
+	}
+
+	/*
+	 * Ignore all characters if CREAD is not set, still receive characters
+	 * from the port, but throw them away.
+	 */
+	if ((termios->c_cflag & CREAD) == 0) {
+		umxc->port.ignore_status_mask |= UART_CREAD_BIT;
+	}
+
+	cr4 = readl(umxc->port.membase + MXC_UARTUCR4);
+	if (UART_ENABLE_MS(port, termios->c_cflag)) {
+		mxcuart_enable_ms(port);
+		if (umxc->hardware_flow == 1) {
+			cr4 = (cr4 & (~MXC_UARTUCR4_CTSTL_MASK)) |
+			    (umxc->cts_threshold << MXC_UARTUCR4_CTSTL_OFFSET);
+			cr2 |= MXC_UARTUCR2_CTSC;
+			umxc->port.info->port.tty->hw_stopped = 0;
+		} else {
+			cr2 |= MXC_UARTUCR2_IRTS;
+		}
+	} else {
+		cr2 |= MXC_UARTUCR2_IRTS;
+	}
+
+	/* Add Parity, character length and stop bits information */
+	cr2 |= (readl(umxc->port.membase + MXC_UARTUCR2) & cr2_mask);
+	writel(cr2, umxc->port.membase + MXC_UARTUCR2);
+	/*
+	   if (umxc->ir_mode == IRDA) {
+	   ret = mxcuart_setir_special(baud);
+	   if (ret == 0) {
+	   cr4 &= ~MXC_UARTUCR4_IRSC;
+	   } else {
+	   cr4 |= MXC_UARTUCR4_IRSC;
+	   }
+	   } */
+	writel(cr4, umxc->port.membase + MXC_UARTUCR4);
+
+	/*
+	 * Set baud rate
+	 */
+
+	/* Use integer scaling, if possible. Limit the denom to 15 bits. */
+	num = 0;
+	denom = (umxc->port.uartclk + 8 * baud) / (16 * baud) - 1;
+
+	/* Use fractional scaling if needed to limit the max error to 0.5% */
+	if (denom < 100) {
+		u64 n64 = (u64) 16 * 0x8000 * baud + (umxc->port.uartclk / 2);
+		do_div(n64, umxc->port.uartclk);
+		num = (u_int) n64 - 1;
+		denom = 0x7fff;
+	}
+	writel(num, umxc->port.membase + MXC_UARTUBIR);
+	writel(denom, umxc->port.membase + MXC_UARTUBMR);
+
+	spin_unlock_irqrestore(&umxc->port.lock, flags);
+}
+
+/*!
+ * This function is called by the core driver to know the UART type.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ *
+ * @return  The function returns a pointer to a string describing the UART port.
+ */
+static const char *mxcuart_type(struct uart_port *port)
+{
+	return port->type == PORT_MXC ? "Freescale MXC" : NULL;
+}
+
+/*!
+ * This function is called by the core driver to release the memory resources
+ * currently in use by the UART port.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ */
+static void mxcuart_release_port(struct uart_port *port)
+{
+	release_mem_region(port->mapbase, SZ_4K);
+}
+
+/*!
+ * This function is called by the core driver to request memory resources for
+ * the UART port.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ *
+ * @return  The function returns \b -EBUSY on failure, else it returns 0.
+ */
+static int mxcuart_request_port(struct uart_port *port)
+{
+	return request_mem_region(port->mapbase, SZ_4K, "serial_mxc")
+	    != NULL ? 0 : -EBUSY;
+}
+
+/*!
+ * This function is called by the core driver to perform any autoconfiguration
+ * steps required for the UART port. This function sets the port->type field.
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ * @param   flags  bit mask of the required configuration
+ */
+static void mxcuart_config_port(struct uart_port *port, int flags)
+{
+	if ((flags & UART_CONFIG_TYPE) && (mxcuart_request_port(port) == 0)) {
+		port->type = PORT_MXC;
+	}
+}
+
+/*!
+ * This function is called by the core driver to verify that the new serial
+ * port information contained within \a ser is suitable for this UART port type.
+ * The function checks to see if the UART port type specified by the user
+ * application while setting the UART port information matches what is stored
+ * in the define \b PORT_MXC found in the header file include/linux/serial_core.h
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ * @param   ser    the new serial port information
+ *
+ * @return  The function returns 0 on success or \b -EINVAL if the port type
+ *          specified is not equal to \b PORT_MXC.
+ */
+static int mxcuart_verify_port(struct uart_port *port,
+			       struct serial_struct *ser)
+{
+	int ret = 0;
+	if (ser->type != PORT_UNKNOWN && ser->type != PORT_MXC) {
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+/*!
+ * This function is used to send a high priority XON/XOFF character
+ *
+ * @param   port   the port structure for the UART passed in by the core driver
+ * @param   ch     the character to send
+ */
+static void mxcuart_send_xchar(struct uart_port *port, char ch)
+{
+	unsigned long flags;
+
+	port->x_char = ch;
+	if (port->info->port.tty->hw_stopped) {
+		return;
+	}
+
+	if (ch) {
+		spin_lock_irqsave(&port->lock, flags);
+		port->ops->start_tx(port);
+		spin_unlock_irqrestore(&port->lock, flags);
+	}
+}
+
+/*!
+ * This function is used enable/disable the MXC UART clocks
+ *
+ * @param   port      the port structure for the UART passed in by the core driver
+ * @param   state     New PM state
+ * @param   oldstate  Current PM state
+ */
+static void
+mxcuart_pm(struct uart_port *port, unsigned int state, unsigned int oldstate)
+{
+	uart_mxc_port *umxc = (uart_mxc_port *) port;
+
+	if (state)
+		clk_disable(umxc->clk);
+	else
+		clk_enable(umxc->clk);
+}
+
+/*!
+ * This structure contains the pointers to the control functions that are
+ * invoked by the core serial driver to access the UART hardware. The
+ * structure is passed to serial_core.c file during registration.
+ */
+static struct uart_ops mxc_ops = {
+	.tx_empty = mxcuart_tx_empty,
+	.set_mctrl = mxcuart_set_mctrl,
+	.get_mctrl = mxcuart_get_mctrl,
+	.stop_tx = mxcuart_stop_tx,
+	.start_tx = mxcuart_start_tx,
+	.stop_rx = mxcuart_stop_rx,
+	.enable_ms = mxcuart_enable_ms,
+	.break_ctl = mxcuart_break_ctl,
+	.startup = mxcuart_startup,
+	.shutdown = mxcuart_shutdown,
+	.set_termios = mxcuart_set_termios,
+	.type = mxcuart_type,
+	.pm = mxcuart_pm,
+	.release_port = mxcuart_release_port,
+	.request_port = mxcuart_request_port,
+	.config_port = mxcuart_config_port,
+	.verify_port = mxcuart_verify_port,
+	.send_xchar = mxcuart_send_xchar,
+};
+
+#ifdef CONFIG_SERIAL_MXC_CONSOLE
+
+/*
+ * Write out a character once the UART is ready
+ */
+static inline void mxcuart_console_write_char(struct uart_port *port, char ch)
+{
+	volatile unsigned int status;
+
+	do {
+		status = readl(port->membase + MXC_UARTUSR1);
+	} while ((status & MXC_UARTUSR1_TRDY) == 0);
+	writel(ch, port->membase + MXC_UARTUTXD);
+}
+
+/*!
+ * This function is called to write the console messages through the UART port.
+ *
+ * @param   co    the console structure
+ * @param   s     the log message to be written to the UART
+ * @param   count length of the message
+ */
+static void mxcuart_console_write(struct console *co, const char *s,
+				  u_int count)
+{
+	struct uart_port *port = &mxc_ports[co->index]->port;
+	volatile unsigned int status, oldcr1, oldcr2, oldcr3, cr2, cr3;
+	int i;
+
+	/*
+	 * First save the control registers and then disable the interrupts
+	 */
+	oldcr1 = readl(port->membase + MXC_UARTUCR1);
+	oldcr2 = readl(port->membase + MXC_UARTUCR2);
+	oldcr3 = readl(port->membase + MXC_UARTUCR3);
+	cr2 =
+	    oldcr2 & ~(MXC_UARTUCR2_ATEN | MXC_UARTUCR2_RTSEN |
+		       MXC_UARTUCR2_ESCI);
+	cr3 =
+	    oldcr3 & ~(MXC_UARTUCR3_DCD | MXC_UARTUCR3_RI |
+		       MXC_UARTUCR3_DTRDEN);
+	writel(MXC_UARTUCR1_UARTEN, port->membase + MXC_UARTUCR1);
+	writel(cr2, port->membase + MXC_UARTUCR2);
+	writel(cr3, port->membase + MXC_UARTUCR3);
+	/*
+	 * Do each character
+	 */
+	for (i = 0; i < count; i++) {
+		mxcuart_console_write_char(port, s[i]);
+		if (s[i] == '\n') {
+			mxcuart_console_write_char(port, '\r');
+		}
+	}
+	/*
+	 * Finally, wait for the transmitter to become empty
+	 */
+	do {
+		status = readl(port->membase + MXC_UARTUSR2);
+	} while (!(status & MXC_UARTUSR2_TXDC));
+
+	/*
+	 * Restore the control registers
+	 */
+	writel(oldcr1, port->membase + MXC_UARTUCR1);
+	writel(oldcr2, port->membase + MXC_UARTUCR2);
+	writel(oldcr3, port->membase + MXC_UARTUCR3);
+}
+
+/*!
+ * Initializes the UART port to be used to print console message with the
+ * options specified. If no options are specified, then the function
+ * initializes the UART with the default options of baudrate=115200, 8 bit
+ * word size, no parity, no flow control.
+ *
+ * @param   co      The console structure
+ * @param   options Any console options passed in from the command line
+ *
+ * @return  The function returns 0 on success or error.
+ */
+static int __init mxcuart_console_setup(struct console *co, char *options)
+{
+	uart_mxc_port *umxc;
+	int baud = 115200;
+	int bits = 8;
+	int parity = 'n';
+	int flow = 'n';
+	volatile unsigned int cr = 0;
+
+	/*
+	 * Check whether an invalid uart number had been specified, and if
+	 * so, search for the first available port that does have console
+	 * support
+	 */
+	if (co->index >= MXC_UART_NR) {
+		co->index = 0;
+	}
+	umxc = mxc_ports[co->index];
+
+	if (umxc == NULL) {
+		return -ENODEV;
+	}
+
+	clk_enable(umxc->clk);
+
+	/* initialize port.lock else oops */
+	spin_lock_init(&umxc->port.lock);
+
+	/*
+	 * Initialize the UART registers
+	 */
+	writel(MXC_UARTUCR1_UARTEN, umxc->port.membase + MXC_UARTUCR1);
+	/* Enable the transmitter and do a software reset */
+	writel(MXC_UARTUCR2_TXEN, umxc->port.membase + MXC_UARTUCR2);
+	/* Wait till we are out of software reset */
+	do {
+		cr = readl(umxc->port.membase + MXC_UARTUCR2);
+	} while (!(cr & MXC_UARTUCR2_SRST));
+
+	writel(0x0, umxc->port.membase + MXC_UARTUCR3);
+	writel(0x0, umxc->port.membase + MXC_UARTUCR4);
+	/* Set TXTL to 2, RXTL to 1 and RFDIV to 2 */
+	cr = 0x0800 | MXC_UARTUFCR_RFDIV | 0x1;
+	if (umxc->mode == MODE_DTE) {
+		cr |= MXC_UARTUFCR_DCEDTE;
+	}
+	writel(cr, umxc->port.membase + MXC_UARTUFCR);
+	writel(0xFFFF, umxc->port.membase + MXC_UARTUSR1);
+	writel(0xFFFF, umxc->port.membase + MXC_UARTUSR2);
+
+	if (options != NULL) {
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+	}
+	gpio_uart_active(umxc->port.line, umxc->ir_mode);
+	return uart_set_options(&umxc->port, co, baud, parity, bits, flow);
+}
+
+static struct uart_driver mxc_reg;
+
+/*!
+ * This structure contains the pointers to the UART console functions. It is
+ * passed as an argument when registering the console.
+ */
+static struct console mxc_console = {
+	.name = "ttymxc",
+	.write = mxcuart_console_write,
+	.device = uart_console_device,
+	.setup = mxcuart_console_setup,
+	.flags = CON_PRINTBUFFER,
+	.index = -1,
+	.data = &mxc_reg,
+};
+
+/*!
+ * This function registers the console callback functions with the kernel.
+ */
+static int __init mxcuart_console_init(void)
+{
+	register_console(&mxc_console);
+	return 0;
+}
+
+console_initcall(mxcuart_console_init);
+
+static int __init find_port(struct uart_port *p)
+{
+	int line;
+	struct uart_port *port;
+	for (line = 0; line < MXC_UART_NR; line++) {
+		if (!mxc_ports[line])
+			continue;
+		port = &mxc_ports[line]->port;
+		if (uart_match_port(p, port))
+			return line;
+	}
+	return -ENODEV;
+}
+
+int __init mxc_uart_start_console(struct uart_port *port, char *options)
+{
+	int line;
+	line = find_port(port);
+	if (line < 0)
+		return -ENODEV;
+
+	add_preferred_console("ttymxc", line, options);
+	printk("Switching Console to ttymxc%d at %s 0x%lx (options '%s')\n",
+	       line, port->iotype == UPIO_MEM ? "MMIO" : "I/O port",
+	       port->iotype ==
+	       UPIO_MEM ? (unsigned long)port->mapbase : (unsigned long)port->
+	       iobase, options);
+
+	if (!(mxc_console.flags & CON_ENABLED)) {
+		mxc_console.flags &= ~CON_PRINTBUFFER;
+		register_console(&mxc_console);
+	}
+	return 0;
+}
+
+#define MXC_CONSOLE     &mxc_console
+#else
+#define MXC_CONSOLE     NULL
+#endif				/* CONFIG_SERIAL_MXC_CONSOLE */
+
+/*!
+ * This structure contains the information such as the name of the UART driver
+ * that appears in the /dev folder, major and minor numbers etc. This structure
+ * is passed to the serial_core.c file.
+ */
+static struct uart_driver mxc_reg = {
+	.owner = THIS_MODULE,
+	.driver_name = "ttymxc",
+	.dev_name = "ttymxc",
+	.major = SERIAL_MXC_MAJOR,
+	.minor = SERIAL_MXC_MINOR,
+	.nr = MXC_UART_NR,
+	.cons = MXC_CONSOLE,
+};
+
+/*!
+ * This function is called to put the UART in a low power state. Refer to the
+ * document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure used to give information on which UART
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int mxcuart_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	uart_mxc_port *umxc = platform_get_drvdata(pdev);
+
+	if (umxc == NULL)
+		return 0;	/* skip disabled ports */
+
+	if (umxc->port.info && umxc->port.info->flags & UIF_INITIALIZED)
+		uart_suspend_port(&mxc_reg, &umxc->port);
+
+	if (umxc->port.info && umxc->port.info->flags & UIF_SUSPENDED)
+		umxc->port.info->port.tty->hw_stopped = 1;
+
+	return 0;
+}
+
+/*!
+ * This function is called to bring the UART back from a low power state. Refer
+ * to the document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure used to give information on which UART
+ *                to resume
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int mxcuart_resume(struct platform_device *pdev)
+{
+	uart_mxc_port *umxc = platform_get_drvdata(pdev);
+
+	if (umxc == NULL)
+		return 0;	/* skip disabled ports */
+
+	if (umxc->port.info && umxc->port.info->flags & UIF_SUSPENDED) {
+		umxc->port.info->port.tty->hw_stopped = 0;
+		uart_resume_port(&mxc_reg, &umxc->port);
+	}
+
+	return 0;
+}
+
+/*!
+ * This function is called during the driver binding process. Based on the UART
+ * that is being probed this function adds the appropriate UART port structure
+ * in the core driver.
+ *
+ * @param   pdev  the device structure used to store device specific
+ *                information that is used by the suspend, resume and remove
+ *                functions
+ *
+ * @return  The function returns 0 if successful; -1 otherwise.
+ */
+static int mxcuart_probe(struct platform_device *pdev)
+{
+	int id = pdev->id;
+
+	mxc_ports[id] = pdev->dev.platform_data;
+	mxc_ports[id]->port.ops = &mxc_ops;
+
+	/* Do not use UARTs that are disabled during integration */
+	if (mxc_ports[id]->enabled == 1) {
+		mxc_ports[id]->port.dev = &pdev->dev;
+		spin_lock_init(&mxc_ports[id]->port.lock);
+		/* Enable the low latency flag for DMA UART ports */
+		if (mxc_ports[id]->dma_enabled == 1) {
+			mxc_ports[id]->port.flags |= UPF_LOW_LATENCY;
+		}
+
+		mxc_ports[id]->clk = clk_get(&pdev->dev, "uart_clk");
+		if (mxc_ports[id]->clk == NULL)
+			return -1;
+
+		uart_add_one_port(&mxc_reg, &mxc_ports[id]->port);
+		platform_set_drvdata(pdev, mxc_ports[id]);
+	}
+	return 0;
+}
+
+/*!
+ * Dissociates the driver from the UART device. Removes the appropriate UART
+ * port structure from the core driver.
+ *
+ * @param   pdev  the device structure used to give information on which UART
+ *                to remove
+ *
+ * @return  The function always returns 0.
+ */
+static int mxcuart_remove(struct platform_device *pdev)
+{
+	uart_mxc_port *umxc = platform_get_drvdata(pdev);
+
+	platform_set_drvdata(pdev, NULL);
+
+	if (umxc) {
+		uart_remove_one_port(&mxc_reg, &umxc->port);
+	}
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcuart_driver = {
+	.driver = {
+		   .name = "mxcintuart",
+		   },
+	.probe = mxcuart_probe,
+	.remove = mxcuart_remove,
+	.suspend = mxcuart_suspend,
+	.resume = mxcuart_resume,
+};
+
+/*!
+ * This function is used to initialize the UART driver module. The function
+ * registers the power management callback functions with the kernel and also
+ * registers the UART callback functions with the core serial driver.
+ *
+ * @return  The function returns 0 on success and a non-zero value on failure.
+ */
+static int __init mxcuart_init(void)
+{
+	int ret = 0;
+
+	printk(KERN_INFO "Serial: MXC Internal UART driver\n");
+	ret = uart_register_driver(&mxc_reg);
+	if (ret == 0) {
+		/* Register the device driver structure. */
+		ret = platform_driver_register(&mxcuart_driver);
+		if (ret != 0) {
+			uart_unregister_driver(&mxc_reg);
+		}
+	}
+	return ret;
+}
+
+/*!
+ * This function is used to cleanup all resources before the driver exits.
+ */
+static void __exit mxcuart_exit(void)
+{
+	platform_driver_unregister(&mxcuart_driver);
+	uart_unregister_driver(&mxc_reg);
+}
+
+module_init(mxcuart_init);
+module_exit(mxcuart_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC serial port driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/serial/mxc_uart_early.c b/drivers/serial/mxc_uart_early.c
new file mode 100644
index 000000000000..0b5ceaef3ec0
--- /dev/null
+++ b/drivers/serial/mxc_uart_early.c
@@ -0,0 +1,253 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/serial/mxc_uart_early.c
+ *
+ * @brief Driver for the Freescale Semiconductor MXC serial ports based on
+ * drivers/char/8250_early.c, Copyright 2004 Hewlett-Packard Development Company,
+ * L.P.	by Bjorn Helgaasby.
+ *
+ * Early serial console for MXC UARTS.
+ *
+ * This is for use before the serial driver has initialized, in
+ * particular, before the UARTs have been discovered and named.
+ * Instead of specifying the console device as, e.g., "ttymxc0",
+ * we locate the device directly by its MMIO or I/O port address.
+ *
+ * The user can specify the device directly, e.g.,
+ *	console=mxcuart,0x43f90000,115200n8
+ * or platform code can call early_uart_console_init() to set
+ * the early UART device.
+ *
+ * After the normal serial driver starts, we try to locate the
+ * matching ttymxc device and start a console there.
+ */
+
+/*
+ * Include Files
+ */
+
+#include <linux/tty.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/console.h>
+#include <linux/serial_core.h>
+#include <linux/serial_reg.h>
+#include <linux/clk.h>
+#include <mach/mxc_uart.h>
+
+struct mxc_early_uart_device {
+	struct uart_port port;
+	char options[16];	/* e.g., 115200n8 */
+	unsigned int baud;
+};
+
+int __init mxc_uart_start_console(struct uart_port *, char *);
+static struct mxc_early_uart_device mxc_early_device __initdata;
+static int mxc_early_uart_registered __initdata;
+static struct clk *clk;
+
+/*
+ * Write out a character once the UART is ready
+ */
+static void __init mxcuart_console_write_char(struct uart_port *port, int ch)
+{
+	unsigned int status;
+
+	do {
+		status = readl(port->membase + MXC_UARTUSR1);
+	} while ((status & MXC_UARTUSR1_TRDY) == 0);
+	writel(ch, port->membase + MXC_UARTUTXD);
+}
+
+/*!
+ * This function is called to write the console messages through the UART port.
+ *
+ * @param   co    the console structure
+ * @param   s     the log message to be written to the UART
+ * @param   count length of the message
+ */
+void __init early_mxcuart_console_write(struct console *co, const char *s,
+					u_int count)
+{
+	struct uart_port *port = &mxc_early_device.port;
+	volatile unsigned int status, oldcr1, oldcr2, oldcr3, cr2, cr3;
+
+	/*
+	 * First save the control registers and then disable the interrupts
+	 */
+	oldcr1 = readl(port->membase + MXC_UARTUCR1);
+	oldcr2 = readl(port->membase + MXC_UARTUCR2);
+	oldcr3 = readl(port->membase + MXC_UARTUCR3);
+	cr2 =
+	    oldcr2 & ~(MXC_UARTUCR2_ATEN | MXC_UARTUCR2_RTSEN |
+		       MXC_UARTUCR2_ESCI);
+	cr3 =
+	    oldcr3 & ~(MXC_UARTUCR3_DCD | MXC_UARTUCR3_RI |
+		       MXC_UARTUCR3_DTRDEN);
+	writel(MXC_UARTUCR1_UARTEN, port->membase + MXC_UARTUCR1);
+	writel(cr2, port->membase + MXC_UARTUCR2);
+	writel(cr3, port->membase + MXC_UARTUCR3);
+
+	/* Transmit string */
+	uart_console_write(port, s, count, mxcuart_console_write_char);
+
+	/*
+	 * Finally, wait for the transmitter to become empty
+	 */
+	do {
+		status = readl(port->membase + MXC_UARTUSR2);
+	} while (!(status & MXC_UARTUSR2_TXDC));
+
+	/*
+	 * Restore the control registers
+	 */
+	writel(oldcr1, port->membase + MXC_UARTUCR1);
+	writel(oldcr2, port->membase + MXC_UARTUCR2);
+	writel(oldcr3, port->membase + MXC_UARTUCR3);
+}
+
+static unsigned int __init probe_baud(struct uart_port *port)
+{
+	/* FIXME Return Default Baud Rate */
+	return 115200;
+}
+
+static int __init parse_options(struct mxc_early_uart_device *device,
+				char *options)
+{
+	struct uart_port *port = &device->port;
+	int mapsize = 64;
+	int length;
+
+	if (!options)
+		return -ENODEV;
+
+	port->uartclk = 5600000;
+	port->iotype = UPIO_MEM;
+	port->mapbase = simple_strtoul(options, &options, 0);
+	port->membase = ioremap(port->mapbase, mapsize);
+
+	if ((options = strchr(options, ','))) {
+		options++;
+		device->baud = simple_strtoul(options, NULL, 0);
+		length = min(strcspn(options, " "), sizeof(device->options));
+		strncpy(device->options, options, length);
+	} else {
+		device->baud = probe_baud(port);
+		snprintf(device->options, sizeof(device->options), "%u",
+			 device->baud);
+	}
+	printk(KERN_INFO
+	       "MXC_Early serial console at MMIO 0x%x (options '%s')\n",
+	       port->mapbase, device->options);
+	return 0;
+}
+
+static int __init mxc_early_uart_setup(struct console *console, char *options)
+{
+	struct mxc_early_uart_device *device = &mxc_early_device;
+	int err;
+	if (device->port.membase || device->port.iobase)
+		return 0;
+	if ((err = parse_options(device, options)) < 0)
+		return err;
+	return 0;
+}
+
+static struct console mxc_early_uart_console __initdata = {
+	.name = "mxcuart",
+	.write = early_mxcuart_console_write,
+	.setup = mxc_early_uart_setup,
+	.flags = CON_PRINTBUFFER,
+	.index = -1,
+};
+
+static int __init mxc_early_uart_console_init(void)
+{
+
+	if (!mxc_early_uart_registered) {
+		register_console(&mxc_early_uart_console);
+		mxc_early_uart_registered = 1;
+	}
+
+	return 0;
+}
+
+int __init mxc_early_serial_console_init(char *cmdline)
+{
+	char *options;
+	int err;
+	int uart_paddr;
+
+	options = strstr(cmdline, "console=mxcuart");
+	if (!options)
+		return -ENODEV;
+
+	/* Extracting MXC UART Uart Port Address from cmdline */
+	options = strchr(cmdline, ',') + 1;
+	uart_paddr = simple_strtoul(options, NULL, 16);
+
+#ifdef UART1_BASE_ADDR
+	if (uart_paddr == UART1_BASE_ADDR)
+		clk = clk_get(NULL, "uart_clk.0");
+#endif
+#ifdef UART2_BASE_ADDR
+	if (uart_paddr == UART2_BASE_ADDR)
+		clk = clk_get(NULL, "uart_clk.1");
+#endif
+#ifdef UART3_BASE_ADDR
+	if (uart_paddr == UART3_BASE_ADDR)
+		clk = clk_get(NULL, "uart_clk.2");
+#endif
+	if (clk == NULL)
+		return -1;
+
+	/* Enable Early MXC UART Clock */
+	clk_enable(clk);
+
+	options = strchr(cmdline, ',') + 1;
+	if ((err = mxc_early_uart_setup(NULL, options)) < 0)
+		return err;
+	return mxc_early_uart_console_init();
+}
+
+int __init mxc_early_uart_console_switch(void)
+{
+	struct mxc_early_uart_device *device = &mxc_early_device;
+	struct uart_port *port = &device->port;
+	int mmio, line;
+
+	if (!(mxc_early_uart_console.flags & CON_ENABLED))
+		return 0;
+	/* Try to start the normal driver on a matching line.  */
+	mmio = (port->iotype == UPIO_MEM);
+	line = mxc_uart_start_console(port, device->options);
+
+	if (line < 0)
+		printk("No ttymxc device at %s 0x%lx for console\n",
+		       mmio ? "MMIO" : "I/O port",
+		       mmio ? port->mapbase : (unsigned long)port->iobase);
+
+	unregister_console(&mxc_early_uart_console);
+	if (mmio)
+		iounmap(port->membase);
+
+	clk_disable(clk);
+	clk_put(clk);
+
+	return 0;
+}
+
+late_initcall(mxc_early_uart_console_switch);
diff --git a/drivers/serial/mxc_uart_reg.h b/drivers/serial/mxc_uart_reg.h
new file mode 100644
index 000000000000..c0d1e812fe6a
--- /dev/null
+++ b/drivers/serial/mxc_uart_reg.h
@@ -0,0 +1,128 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __MXC_UART_REG_H__
+#define __MXC_UART_REG_H__
+
+/* Address offsets of the UART registers */
+#define MXC_UARTURXD            0x000	/* Receive reg */
+#define MXC_UARTUTXD            0x040	/* Transmitter reg */
+#define	MXC_UARTUCR1            0x080	/* Control reg 1 */
+#define MXC_UARTUCR2            0x084	/* Control reg 2 */
+#define MXC_UARTUCR3            0x088	/* Control reg 3 */
+#define MXC_UARTUCR4            0x08C	/* Control reg 4 */
+#define MXC_UARTUFCR            0x090	/* FIFO control reg */
+#define MXC_UARTUSR1            0x094	/* Status reg 1 */
+#define MXC_UARTUSR2            0x098	/* Status reg 2 */
+#define MXC_UARTUESC            0x09C	/* Escape character reg */
+#define MXC_UARTUTIM            0x0A0	/* Escape timer reg */
+#define MXC_UARTUBIR            0x0A4	/* BRM incremental reg */
+#define MXC_UARTUBMR            0x0A8	/* BRM modulator reg */
+#define MXC_UARTUBRC            0x0AC	/* Baud rate count reg */
+#define MXC_UARTONEMS           0x0B0	/* One millisecond reg */
+#define MXC_UARTUTS             0x0B4	/* Test reg */
+
+/* Bit definations of UCR1 */
+#define MXC_UARTUCR1_ADEN       0x8000
+#define MXC_UARTUCR1_ADBR       0x4000
+#define MXC_UARTUCR1_TRDYEN     0x2000
+#define MXC_UARTUCR1_IDEN       0x1000
+#define MXC_UARTUCR1_RRDYEN     0x0200
+#define MXC_UARTUCR1_RXDMAEN    0x0100
+#define MXC_UARTUCR1_IREN       0x0080
+#define MXC_UARTUCR1_TXMPTYEN   0x0040
+#define MXC_UARTUCR1_RTSDEN     0x0020
+#define MXC_UARTUCR1_SNDBRK     0x0010
+#define MXC_UARTUCR1_TXDMAEN    0x0008
+#define MXC_UARTUCR1_ATDMAEN    0x0004
+#define MXC_UARTUCR1_DOZE       0x0002
+#define MXC_UARTUCR1_UARTEN     0x0001
+
+/* Bit definations of UCR2 */
+#define MXC_UARTUCR2_ESCI       0x8000
+#define MXC_UARTUCR2_IRTS       0x4000
+#define MXC_UARTUCR2_CTSC       0x2000
+#define MXC_UARTUCR2_CTS        0x1000
+#define MXC_UARTUCR2_PREN       0x0100
+#define MXC_UARTUCR2_PROE       0x0080
+#define MXC_UARTUCR2_STPB       0x0040
+#define MXC_UARTUCR2_WS         0x0020
+#define MXC_UARTUCR2_RTSEN      0x0010
+#define MXC_UARTUCR2_ATEN       0x0008
+#define MXC_UARTUCR2_TXEN       0x0004
+#define MXC_UARTUCR2_RXEN       0x0002
+#define MXC_UARTUCR2_SRST       0x0001
+
+/* Bit definations of UCR3 */
+#define MXC_UARTUCR3_DTREN      0x2000
+#define MXC_UARTUCR3_PARERREN   0x1000
+#define MXC_UARTUCR3_FRAERREN   0x0800
+#define MXC_UARTUCR3_DSR        0x0400
+#define MXC_UARTUCR3_DCD        0x0200
+#define MXC_UARTUCR3_RI         0x0100
+#define MXC_UARTUCR3_RXDSEN     0x0040
+#define MXC_UARTUCR3_AWAKEN     0x0010
+#define MXC_UARTUCR3_DTRDEN     0x0008
+#define MXC_UARTUCR3_RXDMUXSEL  0x0004
+#define MXC_UARTUCR3_INVT       0x0002
+
+/* Bit definations of UCR4 */
+#define MXC_UARTUCR4_CTSTL_OFFSET       10
+#define MXC_UARTUCR4_CTSTL_MASK         (0x3F << 10)
+#define MXC_UARTUCR4_INVR               0x0200
+#define MXC_UARTUCR4_ENIRI              0x0100
+#define MXC_UARTUCR4_REF16              0x0040
+#define MXC_UARTUCR4_IRSC               0x0020
+#define MXC_UARTUCR4_TCEN               0x0008
+#define MXC_UARTUCR4_OREN               0x0002
+#define MXC_UARTUCR4_DREN               0x0001
+
+/* Bit definations of UFCR */
+#define MXC_UARTUFCR_RFDIV              0x0200	/* Ref freq div is set to 2 */
+#define MXC_UARTUFCR_RFDIV_OFFSET       7
+#define MXC_UARTUFCR_RFDIV_MASK         (0x7 << 7)
+#define MXC_UARTUFCR_TXTL_OFFSET        10
+#define MXC_UARTUFCR_DCEDTE             0x0040
+
+/* Bit definations of URXD */
+#define MXC_UARTURXD_ERR        0x4000
+#define MXC_UARTURXD_OVRRUN     0x2000
+#define MXC_UARTURXD_FRMERR     0x1000
+#define MXC_UARTURXD_BRK        0x0800
+#define MXC_UARTURXD_PRERR      0x0400
+
+/* Bit definations of USR1 */
+#define MXC_UARTUSR1_PARITYERR  0x8000
+#define MXC_UARTUSR1_RTSS       0x4000
+#define MXC_UARTUSR1_TRDY       0x2000
+#define MXC_UARTUSR1_RTSD       0x1000
+#define MXC_UARTUSR1_FRAMERR    0x0400
+#define MXC_UARTUSR1_RRDY       0x0200
+#define MXC_UARTUSR1_AGTIM      0x0100
+#define MXC_UARTUSR1_DTRD       0x0080
+#define MXC_UARTUSR1_AWAKE      0x0010
+
+/* Bit definations of USR2 */
+#define MXC_UARTUSR2_TXFE       0x4000
+#define MXC_UARTUSR2_IDLE       0x1000
+#define MXC_UARTUSR2_RIDELT     0x0400
+#define MXC_UARTUSR2_RIIN       0x0200
+#define MXC_UARTUSR2_DCDDELT    0x0040
+#define MXC_UARTUSR2_DCDIN      0x0020
+#define MXC_UARTUSR2_TXDC       0x0008
+#define MXC_UARTUSR2_ORE        0x0002
+#define MXC_UARTUSR2_RDR        0x0001
+
+/* Bit definations of UTS */
+#define MXC_UARTUTS_LOOP        0x1000
+
+#endif				/* __MXC_UART_REG_H__ */
diff --git a/drivers/serial/ns921x-serial.c b/drivers/serial/ns921x-serial.c
new file mode 100644
index 000000000000..7aa8f7f2b103
--- /dev/null
+++ b/drivers/serial/ns921x-serial.c
@@ -0,0 +1,1401 @@
+/*
+ * drivers/serial/ns921x-serial.c
+ *
+ * Copyright (C) 2007,2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#if defined(CONFIG_SERIAL_NS921X_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
+#define SUPPORT_SYSRQ
+#endif
+
+#include <linux/clk.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/serial_core.h>
+#include <linux/serial.h>
+
+#include <mach/ns921x-serial.h>
+#include <mach/gpio.h>
+
+#define HUB_IFS		0x0000
+#define HUB_IFS_RXFSRIP		(1 << 25)
+#define HUB_IFS_TXFSRIP		(1 << 19)
+#define HUB_IFS_MODIP		(1 << 18)
+#define HUB_IFS_RXFE		(1 << 13)
+#define HUB_IFS_TXFF		(1 << 11)
+#define HUB_IFS_TXFE		(1 << 10)
+
+#define HUB_DMARXCTRL	0x0004
+#define HUB_DMARXCTRL_DIRECT	(1 << 28)
+
+#define HUB_RXIC	0x000c
+#define HUB_RXIC_RXTHRS		(15 << 28)
+#define HUB_RXIC_RXFSRIE	(1 << 25)
+
+#define HUB_DMRXSF	0x0010
+#define HUB_DMRXSF_BYTE		(7 << 9)
+#define HUB_DMRXSF_FULL		(1 << 7)
+
+#define HUB_DMRXDF	0x0014
+
+#define HUB_DMATXCTRL	0x0018
+#define HUB_DMATXCTRL_DIRECT	(1 << 28)
+
+#define HUB_TXIC	0x0020
+#define HUB_TXIC_TXTHRS		(15 << 28)
+#define HUB_TXIC_TXFUFIE	(1 << 26)
+#define HUB_TXIC_TXFSRIE	(1 << 25)
+
+#define HUB_DMTXDF	0x0028
+
+#define UART_WC		0x1000
+#define UART_WC_RXEN		(1 << 30)
+#define UART_WC_TXEN		(1 << 29)
+#define UART_WC_RTSEN		(1 << 19)
+#define UART_WC_RXFLUSH		(1 << 17)
+#define UART_WC_TXFLUSH		(1 << 16)
+#define UART_WC_TXFLOW_CHAR	(1 << 11)
+#define UART_WC_TXFLOW_SOFT	(1 << 10)
+#define UART_WC_TXFLOW_RI	(1 << 9)
+#define UART_WC_TXFLOW_DSR	(1 << 8)
+#define UART_WC_TXFLOW_DCD	(1 << 7)
+#define UART_WC_TXFLOW_CTS	(1 << 6)
+#define UART_WC_TXFLOW	(UART_WC_TXFLOW_RI | UART_WC_TXFLOW_DSR | \
+			 UART_WC_TXFLOW_DCD | UART_WC_TXFLOW_CTS)
+
+#define UART_IE		0x1004
+#define UART_IE_OFLOW		(1 << 19)
+#define UART_IE_PARITY		(1 << 18)
+#define UART_IE_FRAME		(1 << 17)
+#define UART_IE_BREAK		(1 << 16)
+#define UART_IE_DSR		(1 <<  7)
+#define UART_IE_DCD		(1 <<  6)
+#define UART_IE_CTS		(1 <<  5)
+#define UART_IE_RI		(1 <<  4)
+#define UART_IE_TBC		(1 <<  3)
+#define UART_IE_RBC		(1 <<  2)
+#define UART_IE_TXIDLE		(1 <<  1)
+
+#define UART_IS		0x1008
+#define UART_IS_OFLOW		(1 << 19)
+#define UART_IS_PARITY		(1 << 18)
+#define UART_IS_FRAME		(1 << 17)
+#define UART_IS_BREAK		(1 << 16)
+#define UART_IS_DSR		(1 <<  7)
+#define UART_IS_DCD		(1 <<  6)
+#define UART_IS_CTS		(1 <<  5)
+#define UART_IS_RI		(1 <<  4)
+#define UART_IS_TBC		(1 <<  3)
+#define UART_IS_RBC		(1 <<  2)
+#define UART_IS_TXIDLE		(1 <<  1)
+
+#define UIE_RX (UART_IE_OFLOW | UART_IE_PARITY | UART_IE_FRAME | \
+		UART_IE_BREAK | UART_IE_RBC)
+#define UIS_RX (UART_IS_OFLOW | UART_IS_PARITY | UART_IS_FRAME | \
+		UART_IS_BREAK | UART_IS_RBC)
+#define UIE_MS (UART_IE_DSR | UART_IE_DCD | UART_IE_CTS | UART_IE_RI)
+#define UIS_MS (UART_IS_DSR | UART_IS_DCD | UART_IS_CTS | UART_IS_RI)
+
+#define UART_CGAPCTRL	0x100c
+#define UART_CGAPCTRL_EN	(1 << 31)
+
+#define UART_BGAPCTRL	0x1010
+#define UART_BGAPCTRL_EN	(1 << 31)
+
+#define UART_RCMC0	0x1014
+#define UART_RCMC0_ENABLE	(1 << 31)
+#define UART_RCMC0_MASK		(0xff << 16)
+#define UART_RCMC0_DATA		(0xff << 0)
+
+#define UART_AWC	0x1030
+#define UART_AWC_ENABLE		(1 << 0)
+
+#define UART_BRDL       0x1100	/* DLAB = 1 */
+
+#define UART_UIE	0x1104	/* DLAB = 0 */
+#define UART_UIE_ETBEI		(1 << 1)
+
+#define UART_BRDM	0x1104	/* DLAB = 1 */
+
+#define UART_FCR	0x1108
+#define UART_FCR_TXCLR		(1 << 2)
+#define UART_FCR_RXCLR		(1 << 1)
+#define UART_FCR_FIFOEN		(1 << 0)
+
+#define UART_LCR	0x110c
+#define UART_LCR_DLAB		(1 << 7)
+#define UART_LCR_SBC		(1 << 6)
+#define UART_LCR_SPAR		(1 << 5)
+#define UART_LCR_EPAR		(1 << 4)
+#define UART_LCR_PARITY		(1 << 3)
+#define UART_LCR_STOP		(1 << 2)
+#define UART_LCR_WLEN		0x0003
+#define UART_LCR_WLEN_5		0x0000
+#define UART_LCR_WLEN_6		0x0001
+#define UART_LCR_WLEN_7		0x0002
+#define UART_LCR_WLEN_8		0x0003
+
+#define UART_MCR	0x1110
+#define UART_MCR_AFE		(1 << 5)
+#define UART_MCR_LOOP		(1 << 4)
+#define UART_MCR_RTS		(1 << 1)
+#define UART_MCR_DTR		(1 << 0)
+
+#define UART_LSR	0x1114
+#define UART_LSR_TEMT		(1 << 6)
+
+#define UART_MSR	0x1118
+#define UART_MSR_DCD		(1 << 7)
+#define UART_MSR_RI		(1 << 6)
+#define UART_MSR_DSR		(1 << 5)
+#define UART_MSR_CTS		(1 << 4)
+#define UART_MSR_DDCD		(1 << 3)
+#define UART_MSR_TERI		(1 << 2)
+#define UART_MSR_DDSR		(1 << 1)
+#define UART_MSR_DCTS		(1 << 0)
+
+#define UMSR_ANYDELTA		(UART_MSR_DDCD | UART_MSR_TERI | \
+		UART_MSR_DDSR | UART_MSR_DCTS)
+
+#define DRIVER_NAME "ns921x-serial"
+#define NS921X_TTY_NAME "ttyNS"
+#define NS921X_TTY_MAJOR 204 /* XXX */
+#define NS921X_TTY_MINOR_START 196 /* XXX */
+
+#define NS921X_UART_NR 4
+
+#define FIFOSIZE 16
+#define TXTHRESHOLD 1
+#define RXTHRESHOLD 5
+
+#define up2unp(up) container_of(up, struct uart_ns921x_port, port)
+struct uart_ns921x_port {
+	struct uart_port port;
+	struct clk *clk;
+#define NSUPF_TXIRQPENDING 1
+#define NSUPF_SCHEDSTOPTX 2
+	unsigned int flags;
+	u32 ifs2ack;
+	u32 is2ack;
+	struct ns921x_uart_data *data;
+};
+
+#if defined(CONFIG_DEBUG_LL) && 0
+void printch(char);
+void printhex2(unsigned);
+void printhex4(unsigned);
+void printhex8(unsigned);
+void printascii(const char *);
+#else
+#define printch(c) ((void)0)
+#define printhex2(u) ((void)0)
+#define printhex4(u) ((void)0)
+#define printhex8(u) ((void)0)
+#define printascii(s) ((void)0)
+#endif
+
+static inline u32 uartread32(struct uart_port *port, unsigned int offset)
+{
+	u32 ret = ioread32(port->membase + offset);
+
+#if defined(DEBUG_UARTRW)
+	dev_dbg(port->dev, "read  0x%p -> 0x%08x\n",
+			port->membase + offset, ret);
+#endif
+
+	return ret;
+}
+
+static inline void uartwrite32(struct uart_port *port,
+		u32 value, unsigned int offset)
+{
+#if defined(DEBUG_UARTRW)
+	dev_dbg(port->dev, "write 0x%p <- 0x%08x\n",
+			port->membase + offset, value);
+#endif
+	iowrite32(value, port->membase + offset);
+}
+
+static inline void uartwrite8(struct uart_port *port, u8 value,
+		unsigned int offset)
+{
+#if defined(DEBUG_UARTRW)
+	dev_dbg(port->dev, "write 0x%p <- 0x%02x\n",
+			port->membase + offset, value);
+#endif
+	iowrite8(value, port->membase + offset);
+}
+
+/*
+ * bits 19 to 31 of HUB_IFS need to be cleard by writing a 1 to it.
+ * ns921x_uart_read_ifs and ns921x_uart_clear_ifs help debugging missed clears
+ * by tracking the bits set
+ */
+#define IFS_BITSTOACK 0xfff80000
+static inline u32 ns921x_uart_read_ifs(struct uart_ns921x_port *unp)
+{
+	u32 ret = uartread32(&unp->port, HUB_IFS);
+	if (unp->ifs2ack)
+		dev_dbg(unp->port.dev, "%s: %s still unacked: %x "
+				"(called from %p)\n", __func__, "IFS",
+				unp->ifs2ack, __builtin_return_address(0));
+	unp->ifs2ack |= ret & IFS_BITSTOACK;
+
+	if (unp->ifs2ack & ~ret)
+		dev_dbg(unp->port.dev, "%s: rereading %s doesn't "
+				"yield unacked bits (called from %p): "
+				"%08x %08x\n", __func__, "IFS",
+				__builtin_return_address(0), unp->ifs2ack, ret);
+
+	return ret;
+}
+
+static inline void ns921x_uart_clear_ifs(struct uart_ns921x_port *unp,
+		unsigned int mask)
+{
+	if (mask & ~unp->ifs2ack) {
+		dev_dbg(unp->port.dev, "%s: unp->%s = %08x, mask = %08x\n",
+				__func__, "ifs2ack", unp->ifs2ack, mask);
+		BUG();
+	}
+	BUG_ON(mask & ~IFS_BITSTOACK);
+	unp->ifs2ack &= ~mask;
+	uartwrite32(&unp->port, mask, HUB_IFS);
+}
+
+/*
+ * all bits of UART_IS need to be cleard by writing a 1 to it.
+ * ns921x_uart_read_is and ns921x_uart_clear_is help debugging missed clears by
+ * tracking the bits set
+ */
+static inline u32 ns921x_uart_read_is(struct uart_ns921x_port *unp)
+{
+	u32 ret = uartread32(&unp->port, UART_IS);
+	if (unp->is2ack)
+		dev_dbg(unp->port.dev, "%s: %s still unacked: %x "
+				"(called from %p)\n", __func__, "IS",
+				unp->is2ack, __builtin_return_address(0));
+	unp->is2ack |= ret;
+
+	if (unp->is2ack & ~ret)
+		dev_dbg(unp->port.dev, "%s: rereading %s doesn't "
+				"yield unacked bits (called from %p): "
+				"%08x %08x\n", __func__, "IS",
+				__builtin_return_address(0), unp->is2ack, ret);
+
+	return ret;
+}
+
+static inline void ns921x_uart_clear_is(struct uart_ns921x_port *unp,
+		u32 mask)
+{
+	if (mask & ~unp->is2ack) {
+		dev_dbg(unp->port.dev, "%s: unp->%s = %08x, mask = %08x\n",
+				__func__, "is2ack", unp->is2ack, mask);
+		BUG();
+	}
+	unp->is2ack &= ~mask;
+	uartwrite32(&unp->port, mask, UART_IS);
+}
+
+static inline void ns921x_uart_rmw_uartie(struct uart_port *port,
+		u32 mask, u32 value)
+{
+	u32 ie = uartread32(port, UART_IE);
+
+	BUG_ON((value & mask) != value);
+
+	ie = (ie & ~mask) | value;
+
+	uartwrite32(port, ie, UART_IE);
+}
+
+static unsigned int ns921x_uart_tx_empty(struct uart_port *port)
+{
+	struct uart_ns921x_port *unp = up2unp(port);
+	/* Should I use UART_IS_TXIDLE? HUB_IFS_TXFE? */
+
+	u32 lsr;
+
+#if defined(NSUPF_TXIRQPENDING)
+	if (unp->flags & NSUPF_TXIRQPENDING)
+		return 0;
+#endif
+
+	lsr = uartread32(&unp->port, UART_LSR);
+
+	if (!(lsr & UART_LSR_TEMT))
+		return 0;
+
+	return TIOCSER_TEMT;
+}
+
+static void ns921x_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	u32 mcr = 0;
+
+	/* RTS signal should be controlled as gpio to allow using AFE */
+	if (mctrl & TIOCM_RTS)
+		mcr |= UART_MCR_RTS;
+
+	if (mctrl & TIOCM_DTR)
+		mcr |= UART_MCR_DTR;
+
+	if (mctrl & TIOCM_LOOP)
+		mcr |= UART_MCR_LOOP;
+
+	uartwrite32(port, mcr, UART_MCR);
+}
+
+
+/*
+ * Depending on the configuration of the HW flow control, the CTS line is
+ * not be passed to the uart, making impossible to read the status of some
+ * modem lines through the modem status register.
+ * Following function workarounds that by reading the CTS line as gpio.
+ */
+static unsigned int ns921x_uart_check_msr(struct uart_port *port, u32 irqstat)
+{
+	struct uart_ns921x_port *unp = up2unp(port);
+	u32 status, cts_gpio;
+
+	/* We can read the status of the modem lines through the
+	 * modem status register, but we need to add the value of 
+	 * CTS line from the gpio value */
+	status = uartread32(port, UART_MSR);
+	cts_gpio = unp->data->gpios[2];
+	status |= (gpio_get_value(cts_gpio) != 0) ? 0 : UART_MSR_CTS;
+
+	/* For event detection on the modem lines, we use the interrupt status
+	 * register instead of the delta section of the modem status register.
+	 * The interrupt flags are valid in all the cases */
+	if ((irqstat & (UART_IS_DSR|UART_IS_DCD|UART_IS_CTS|UART_IS_RI)) && 
+	     port->info != NULL) {
+		if (irqstat & UART_IS_RI)
+			port->icount.rng++;
+		if (irqstat & UART_IS_DSR)
+			port->icount.dsr++;
+		if (irqstat & UART_IS_DCD)
+			uart_handle_dcd_change(port, status & UART_MSR_DCD);
+		if (irqstat & UART_IS_CTS)
+			uart_handle_cts_change(port, status & UART_MSR_CTS);
+
+		wake_up_interruptible(&port->info->delta_msr_wait);
+	}
+
+	return status;
+}
+
+static unsigned int ns921x_uart_get_mctrl(struct uart_port *port)
+{
+	unsigned int status;
+	unsigned int ret = 0;
+
+	status = ns921x_uart_check_msr(port, 0);
+
+	if (status & UART_MSR_DCD)
+		ret |= TIOCM_CAR;
+	if (status & UART_MSR_RI)
+		ret |= TIOCM_RNG;
+	if (status & UART_MSR_DSR)
+		ret |= TIOCM_DSR;
+	if (status & UART_MSR_CTS) 
+		ret |= TIOCM_CTS;
+
+	return ret;
+}
+
+static void ns921x_uart_stop_tx_real(struct uart_port *port)
+{
+	struct uart_ns921x_port *unp = up2unp(port);
+
+	u32 txic = uartread32(&unp->port, HUB_TXIC);
+
+	printch('.');
+
+	ns921x_uart_rmw_uartie(&unp->port, UART_IE_TXIDLE, 0);
+
+	uartwrite32(port, txic & ~HUB_TXIC_TXFSRIE, HUB_TXIC);
+
+#if defined(NSUPF_TXIRQPENDING)
+	unp->flags &= ~NSUPF_TXIRQPENDING;
+#endif
+}
+
+/* called with port->lock taken */
+static void ns921x_uart_stop_tx(struct uart_port *port)
+{
+#if defined(NSUPF_TXIRQPENDING) && defined(NSUPF_SCHEDSTOPTX)
+	struct uart_ns921x_port *unp = up2unp(port);
+
+	/* don't stop the port if there is a tx irq pending */
+	if (unp->flags & NSUPF_TXIRQPENDING) {
+		unp->flags |= NSUPF_SCHEDSTOPTX;
+	} else {
+		ns921x_uart_stop_tx_real(port);
+
+		unp->flags &= ~NSUPF_SCHEDSTOPTX;
+	}
+#else
+	ns921x_uart_stop_tx_real(port);
+#endif
+}
+
+/* send out chars in xmit buffer.  This is called with port->lock taken */
+static void ns921x_uart_tx_chars(struct uart_ns921x_port *unp,
+		unsigned int freebuffers)
+{
+	struct circ_buf *xmit = &unp->port.info->xmit;
+	unsigned long ifs;
+
+	BUG_ON(!freebuffers);
+
+	assert_spin_locked(&unp->port.lock);
+
+	/*
+	 * If the FIFO is not empty return at this point, then the
+	 * interrupt-handler will recall this function
+	 */
+	ifs = ns921x_uart_read_ifs(unp);
+	if (!(ifs & HUB_IFS_TXFE))
+		return;
+	
+	if (unp->port.x_char) {
+		uartwrite8(&unp->port, unp->port.x_char, HUB_DMTXDF);
+		unp->port.icount.tx++;
+		unp->port.x_char = 0;
+		freebuffers--;
+#if defined(NSUPF_TXIRQPENDING)
+		unp->flags |= NSUPF_TXIRQPENDING;
+#endif
+	}
+	if (uart_circ_empty(xmit) || uart_tx_stopped(&unp->port)) {
+		ns921x_uart_stop_tx(&unp->port);
+		return;
+	}
+
+	printch('T');
+	printhex4(uart_circ_chars_pending(xmit));
+	while (freebuffers && uart_circ_chars_pending(xmit)) {
+		if (uart_circ_chars_pending(xmit) >= 4) {
+			u32 fourchars = xmit->buf[xmit->tail];
+			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+			fourchars |= xmit->buf[xmit->tail] << 8;
+			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+			fourchars |= xmit->buf[xmit->tail] << 16;
+			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+			fourchars |= xmit->buf[xmit->tail] << 24;
+			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+
+			uartwrite32(&unp->port, fourchars, HUB_DMTXDF);
+			printhex8(cpu_to_be32(fourchars));
+			unp->port.icount.tx += 4;
+		} else {
+			uartwrite8(&unp->port,
+					xmit->buf[xmit->tail], HUB_DMTXDF);
+			printhex2(xmit->buf[xmit->tail]);
+			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+			unp->port.icount.tx++;
+		}
+#if defined(NSUPF_TXIRQPENDING)
+		unp->flags |= NSUPF_TXIRQPENDING;
+#endif
+		--freebuffers;
+	}
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(&unp->port);
+
+	if (uart_circ_empty(xmit))
+		ns921x_uart_stop_tx(&unp->port);
+}
+
+#define maskshiftfac(mask)		((mask) & (-(mask)))
+#define im2mask(val, mask)						\
+	(((val) * maskshiftfac(mask)) & (mask))
+
+/* called with port->lock taken */
+static void ns921x_uart_start_tx(struct uart_port *port)
+{
+	struct uart_ns921x_port *unp = up2unp(port);
+
+	u32 txic = uartread32(port, HUB_TXIC);
+
+	uartwrite32(port, txic | im2mask(TXTHRESHOLD, HUB_TXIC_TXTHRS) |
+			HUB_TXIC_TXFSRIE, HUB_TXIC);
+	ns921x_uart_rmw_uartie(&unp->port, UART_IE_TXIDLE, UART_IE_TXIDLE);
+
+#if defined(NSUPF_SCHEDSTOPTX)
+	unp->flags &= ~NSUPF_SCHEDSTOPTX;
+#endif
+
+	if (
+#if defined(NSUPF_TXIRQPENDING)
+			!(unp->flags & NSUPF_TXIRQPENDING) &&
+#endif
+			!(ns921x_uart_read_ifs(unp) & HUB_IFS_TXFF)) {
+		unsigned int freebuffers = 1;
+		ns921x_uart_tx_chars(up2unp(port), freebuffers);
+	}
+}
+
+static void ns921x_uart_stop_rx(struct uart_port *port)
+{
+	dev_vdbg(port->dev, "%s\n", __func__);
+
+	ns921x_uart_rmw_uartie(port, UIE_RX, 0);
+}
+
+static void ns921x_uart_enable_ms(struct uart_port *port)
+{
+	dev_vdbg(port->dev, "%s\n", __func__);
+
+	ns921x_uart_rmw_uartie(port, UIE_MS, UIE_MS);
+}
+
+static void ns921x_uart_break_ctl(struct uart_port *port, int break_state)
+{
+	unsigned long flags;
+	u32 lcr;
+
+	dev_vdbg(port->dev, "%s\n", __func__);
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	lcr = uartread32(port, UART_LCR);
+
+	/* XXX: amba_pl011_break_ctl tests for break_state being -1,
+	 * Documentation/serial/driver tells to tests for != 0 */
+	if (break_state)
+		lcr |= UART_LCR_SBC;
+	else
+		lcr &= ~UART_LCR_SBC;
+
+	uartwrite32(port, lcr, UART_LCR);
+
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static void ns921x_uart_rx_char(struct uart_ns921x_port *unp,
+		unsigned int ch, u32 is)
+{
+	unsigned int flag = TTY_NORMAL;
+
+	unp->port.icount.rx++;
+#define ISERR	(UART_IS_BREAK | UART_IS_PARITY | UART_IS_FRAME | UART_IS_OFLOW)
+
+	dev_vdbg(unp->port.dev, "%s: IS=%x\n", __func__, is);
+	if (unlikely(is & ISERR)) {
+		if (is & UART_IS_BREAK) {
+			dev_vdbg(unp->port.dev, "break, IS=%x\n", is);
+			unp->port.icount.brk++;
+			if (uart_handle_break(&unp->port))
+				return;
+		} else if (is & UART_IS_PARITY)
+			unp->port.icount.parity++;
+		else if (is & UART_IS_FRAME)
+			unp->port.icount.frame++;
+
+		if (is & UART_IS_OFLOW)
+			unp->port.icount.overrun++;
+
+		is &= unp->port.read_status_mask;
+
+		if (is & UART_IS_BREAK)
+			flag = TTY_BREAK;
+		else if (is & UART_IS_PARITY)
+			flag = TTY_PARITY;
+		else if (is & UART_IS_FRAME)
+			flag = TTY_FRAME;
+	}
+
+	if (uart_handle_sysrq_char(&unp->port, ch))
+		return;
+
+	dev_vdbg(unp->port.dev, "%s: insert %x (IS=%x, ism=%x, flag=%x)\n",
+			__func__, ch, is, unp->port.ignore_status_mask, flag);
+	uart_insert_char(&unp->port, is, UART_IS_OFLOW, ch, flag);
+}
+
+/* This is called with port->lock taken */
+static void ns921x_uart_rx_chars(struct uart_ns921x_port *unp,
+		u32 ifs, u32 is)
+{
+	struct tty_struct *tty = unp->port.info->port.tty;
+
+	if (is & (UART_IS_RBC | UIS_RX))
+		ns921x_uart_clear_is(unp, is & (UART_IS_RBC | UIS_RX));
+
+	if (ifs & HUB_IFS_RXFE) {
+		if (is & (UART_IS_BREAK | UART_IS_OFLOW))
+			ns921x_uart_rx_char(unp, 0, is);
+	}
+
+	while (!(ifs & HUB_IFS_RXFE)) {
+		u32 dmrxsf = uartread32(&unp->port, HUB_DMRXSF);
+		u32 dmrxdf = uartread32(&unp->port, HUB_DMRXDF);
+		/* XXX: better put this into a macro */
+		unsigned int bytes = (dmrxsf & HUB_DMRXSF_BYTE) >> 9;
+		int i;
+
+		dev_vdbg(unp->port.dev, "%s: DMRXSF = %x, DMRXDF = %x\n",
+				__func__, dmrxsf, dmrxdf);
+
+		BUG_ON(bytes > 4);
+
+		for (i = 0; i < bytes; ++i) {
+			unsigned int ch = (dmrxdf >> (8 * i)) & 0xff;
+
+			/*
+			 * assume break and errors only apply to the last
+			 * character.
+			 */
+			ns921x_uart_rx_char(unp, ch, i == bytes - 1 ? is : 0);
+		}
+		ifs = ns921x_uart_read_ifs(unp);
+	}
+	spin_unlock(&unp->port.lock);
+	tty_flip_buffer_push(tty);
+	spin_lock(&unp->port.lock);
+}
+
+static irqreturn_t ns921x_uart_int(int irq, void *dev_id)
+{
+	struct uart_ns921x_port *unp = dev_id;
+	u32 ifs, uninitialized_var(is);
+
+	spin_lock(&unp->port.lock);
+
+	ifs = ns921x_uart_read_ifs(unp);
+
+	if (ifs & HUB_IFS_TXFSRIP) {
+		ns921x_uart_clear_ifs(unp, HUB_IFS_TXFSRIP);
+
+		/* don't clear NSUPF_TXIRQPENDING, we still wait for TXdone */
+
+		ns921x_uart_tx_chars(unp, FIFOSIZE - TXTHRESHOLD - 1);
+	}
+
+	if ((ifs & HUB_IFS_RXFSRIP) || (ifs & HUB_IFS_MODIP))
+		is = ns921x_uart_read_is(unp);
+
+	if (ifs & HUB_IFS_RXFSRIP) {
+		ns921x_uart_clear_ifs(unp, HUB_IFS_RXFSRIP);
+
+		ns921x_uart_rx_chars(unp, ifs, is);
+	}
+
+	if (ifs & HUB_IFS_MODIP) {
+		dev_vdbg(unp->port.dev, "%s: IS=%x\n", __func__, is);
+
+		/* test for HUB_IFS_RXFSRIP not being set to only fetch
+		 * characters once.
+		 */
+		if (is & UIS_RX && !(ifs & HUB_IFS_RXFSRIP))
+			ns921x_uart_rx_chars(unp, ifs, is);
+
+		if (is & UIS_MS)
+			ns921x_uart_clear_is(unp, is & UIS_MS);
+
+		/* XXX: call this only after an MS irq? */
+		ns921x_uart_check_msr(&unp->port, is);
+
+		if (is & UART_IS_TXIDLE) {
+			unsigned int freebuffers = TXTHRESHOLD;
+#if defined(NSUPF_TXIRQPENDING)
+			unp->flags &= ~NSUPF_TXIRQPENDING;
+#endif
+
+			ns921x_uart_clear_is(unp, UART_IS_TXIDLE);
+
+			/* The fifo might not be empty.  The following can
+			 * happen:
+			 * - irq for HUB_IFS_TXFSRIP serviced without
+			 *   UART_IS_TXIDLE
+			 * - before new data is written the fifo becomes empty
+			 *   and UART_IS_TXIDLE is pending
+			 * - UART_IS_TXIDLE is serviced with fifo still having
+			 *   some data from servicing UART_IS_TXIDLE.
+			 *
+			 * So we only write TXTHRESHOLD buffers.  Only if the
+			 * fifo is reported to be empty and we didn't wrote
+			 * something above, the full fifo is used.
+			 */
+			if ((ifs & (HUB_IFS_TXFE | HUB_IFS_TXFSRIP)) ==
+					HUB_IFS_TXFE)
+				freebuffers = FIFOSIZE;
+
+			ns921x_uart_tx_chars(unp, freebuffers);
+		}
+
+		if (unp->is2ack)
+			dev_dbg(unp->port.dev,
+					"%s: unacked flags in UART_IS: %x %x\n",
+					__func__, unp->is2ack, is);
+	}
+
+	if (unp->ifs2ack)
+		dev_dbg(unp->port.dev, "%s: unacked flags in HUB_IFS: %x\n",
+				__func__, unp->ifs2ack);
+
+#if defined(NSUPF_SCHEDSTOPTX) && defined(NSUPF_TXIRQPENDING)
+	if ((unp->flags & (NSUPF_TXIRQPENDING | NSUPF_SCHEDSTOPTX)) ==
+			NSUPF_SCHEDSTOPTX) {
+		ns921x_uart_stop_tx_real(&unp->port);
+		unp->flags &= ~NSUPF_SCHEDSTOPTX;
+	}
+#endif
+
+	spin_unlock(&unp->port.lock);
+
+	return IRQ_HANDLED;
+}
+
+static int ns921x_uart_startup(struct uart_port *port)
+{
+	struct uart_ns921x_port *unp = up2unp(port);
+	int ret;
+	u32 rxic;
+	u32 wc;
+
+	unp->flags = 0;
+
+	ret = clk_enable(unp->clk);
+	if (ret) {
+		dev_dbg(port->dev, "%s: err_clkenable", __func__);
+		goto err_clkenable;
+	}
+
+	unp->port.uartclk = clk_get_rate(unp->clk);
+
+	ret = request_irq(unp->port.irq, ns921x_uart_int, 0, DRIVER_NAME, unp);
+	if (ret) {
+		dev_dbg(port->dev, "%s: err_request_irq", __func__);
+
+		clk_disable(unp->clk);
+err_clkenable:
+
+		return ret;
+	}
+
+	ns921x_uart_rmw_uartie(port, UIE_RX, UIE_RX);
+
+	rxic = uartread32(port, HUB_RXIC);
+	uartwrite32(port, rxic | im2mask(RXTHRESHOLD, HUB_RXIC_RXTHRS)
+			| HUB_RXIC_RXFSRIE, HUB_RXIC);
+
+	wc = UART_WC_RXEN | UART_WC_TXEN;
+	if (unp->data->rtsen)
+		wc |= UART_WC_RTSEN;
+	uartwrite32(port, wc, UART_WC);
+	uartwrite32(&unp->port, UART_FCR_FIFOEN, UART_FCR);
+
+	return 0;
+}
+
+static void ns921x_uart_shutdown(struct uart_port *port)
+{
+	struct uart_ns921x_port *unp = up2unp(port);
+
+	/* wait up to 10ms for the fifo to empty */
+	unsigned long expire = jiffies + msecs_to_jiffies(10);
+
+	printch('\\');
+
+	while (!ns921x_uart_tx_empty(port)) {
+		msleep(1);
+		if (time_after(jiffies, expire))
+			break;
+	}
+
+	ns921x_uart_rmw_uartie(port, -1, 0);
+	uartwrite32(port, 0, UART_WC);
+
+	ns921x_uart_break_ctl(port, 0);
+
+	free_irq(port->irq, unp);
+
+	clk_disable(unp->clk);
+}
+
+static void ns921x_uart_set_termios(struct uart_port *port,
+		struct ktermios *termios, struct ktermios *old)
+{
+	unsigned long flags;
+	u32 cval = 0;
+
+	unsigned int baud, quot;
+	u32 saved_wc;
+
+	switch (termios->c_cflag & CSIZE) {
+	case CS5:
+		cval = UART_LCR_WLEN_5;
+		break;
+	case CS6:
+		cval = UART_LCR_WLEN_6;
+		break;
+	case CS7:
+		cval = UART_LCR_WLEN_7;
+		break;
+	default:
+	case CS8:
+		cval = UART_LCR_WLEN_8;
+		break;
+	}
+
+	if (termios->c_cflag & CSTOPB)
+		cval |= UART_LCR_STOP;
+
+	if (termios->c_cflag & PARENB) {
+		cval |= UART_LCR_PARITY;
+
+		if (!(termios->c_cflag & PARODD))
+			cval |= UART_LCR_EPAR;
+#ifdef CMSPAR
+		/* stick parity:
+		 *   - MARK parity requires flags PARENB | CMSPAR | PARODD
+		 *     and in the controller the EPS and PEN bitfields
+		 *     must be set to 0
+		 *   - SPACE parity requires flags PARENB | CMSPAR | ~PARODD
+		 *     and in the controller the EPS and PEN bitfields
+		 *     must be set to 1 (these are already set by the
+		 *     code above this, so there is nothing to do here)
+		 *   In any case SP bitfield must be set to 1
+		 */
+		if(termios->c_cflag & CMSPAR) {
+			cval |= UART_LCR_SPAR;	/* set stick parity */
+			if (termios->c_cflag & PARODD) {
+				cval &= ~UART_LCR_EPAR;
+				cval &= ~UART_LCR_PARITY;
+			}
+		}
+#endif
+	}
+
+	baud = uart_get_baud_rate(port, termios, old, 0, port->uartclk / 16);
+	quot = uart_get_divisor(port, baud);
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	uart_update_timeout(port, termios->c_cflag, baud);
+
+	port->read_status_mask = UART_IS_OFLOW;
+	if (termios->c_iflag & INPCK)
+		port->read_status_mask |= UART_IS_FRAME | UART_IS_PARITY;
+	if (termios->c_iflag & (BRKINT | PARMRK))
+		port->read_status_mask |= UART_IS_BREAK;
+
+	port->ignore_status_mask = 0;
+	if (termios->c_iflag & IGNPAR)
+		port->ignore_status_mask |= UART_IS_FRAME | UART_IS_PARITY;
+	if (termios->c_iflag & IGNBRK) {
+		port->ignore_status_mask |= UART_IS_BREAK;
+		/*
+		 * If we're ignoring parity and break indicators,
+		 * ignore overruns too (for real raw support).
+		 */
+		if (termios->c_iflag & IGNPAR)
+			port->ignore_status_mask |= UART_IS_OFLOW;
+	}
+
+	/* disable everything, prepare fifo flushing */
+	saved_wc = uartread32(port, UART_WC);
+	uartwrite32(port, UART_WC_RXFLUSH | UART_WC_TXFLUSH, UART_WC);
+
+	if (UART_ENABLE_MS(port, termios->c_cflag))
+		ns921x_uart_enable_ms(port);
+
+	/* set character gap period */
+	uartwrite32(port, UART_CGAPCTRL_EN | (10 * (port->uartclk / baud) - 1),
+			UART_CGAPCTRL);
+
+	/* set buffer gap period */
+	uartwrite32(port, UART_BGAPCTRL_EN | (640 * (port->uartclk / baud) - 1),
+			UART_BGAPCTRL);
+
+	/* prepare to access baud rate registers */
+	uartwrite32(port, UART_LCR_DLAB, UART_LCR);
+
+	/* set baud rate */
+	uartwrite32(port, quot & 0xff, UART_BRDL);
+	uartwrite32(port, (quot >> 8) & 0xff, UART_BRDM);
+
+	udelay(1);
+	uartwrite32(port, cval, UART_LCR);
+
+	/* flush fifos and restore state */
+	uartwrite32(port, saved_wc, UART_WC);
+	uartwrite32(port, UART_UIE_ETBEI, UART_UIE);
+
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static const char *ns921x_uart_type(struct uart_port *port)
+{
+	return port->type == PORT_NS921X ? "NS921X" : NULL;
+}
+
+static void ns921x_uart_release_port(struct uart_port *port)
+{
+	/* XXX: release_mem_region is marked as Compatibility cruft ??? */
+	release_mem_region(port->mapbase, 0x8000);
+}
+
+static int ns921x_uart_request_port(struct uart_port *port)
+{
+	return request_mem_region(port->mapbase,
+			0x8000, DRIVER_NAME) ? 0 : -EBUSY;
+}
+
+static void ns921x_uart_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE) {
+		port->type = PORT_NS921X;
+		ns921x_uart_request_port(port);
+	}
+}
+
+static int ns921x_uart_verify_port(struct uart_port *port,
+		struct serial_struct *ser)
+{
+	int ret = 0;
+	if (ser->type != PORT_UNKNOWN && ser->type != PORT_NS921X)
+		ret = -EINVAL;
+
+	if (ser->irq < 0 || ser->irq >= NR_IRQS)
+		ret = -EINVAL;
+
+	if (ser->baud_base < 9600)
+		ret = -EINVAL;
+
+	return ret;
+}
+
+static struct uart_ops ns921x_uart_pops = {
+	.tx_empty = ns921x_uart_tx_empty,
+	.set_mctrl = ns921x_uart_set_mctrl,
+	.get_mctrl = ns921x_uart_get_mctrl,
+	.stop_tx = ns921x_uart_stop_tx,
+	.start_tx = ns921x_uart_start_tx,
+	.stop_rx = ns921x_uart_stop_rx,
+	.enable_ms = ns921x_uart_enable_ms,
+	.break_ctl = ns921x_uart_break_ctl,
+	.startup = ns921x_uart_startup,
+	.shutdown = ns921x_uart_shutdown,
+	.set_termios = ns921x_uart_set_termios,
+	/* .pm = ns921x_uart_pm, */
+	/* .set_wake = ns921x_uart_set_wake, */
+	.type = ns921x_uart_type,
+	.release_port = ns921x_uart_release_port,
+	.request_port = ns921x_uart_request_port,
+	.config_port = ns921x_uart_config_port,
+	.verify_port = ns921x_uart_verify_port,
+	/* .ioctl */
+};
+
+static struct uart_ns921x_port *ns921x_uart_ports[NS921X_UART_NR];
+
+#if defined(CONFIG_SERIAL_NS921X_CONSOLE)
+
+static void ns921x_uart_console_putchar(struct uart_port *port, int ch)
+{
+	struct uart_ns921x_port *unp = up2unp(port);
+
+	while (ns921x_uart_read_ifs(unp) & HUB_IFS_TXFF)
+		barrier();
+
+	uartwrite8(&unp->port, ch, HUB_DMTXDF);
+}
+
+/* called with console_sem hold.  irqs locally disabled */
+static void ns921x_uart_console_write(struct console *co,
+		const char *s, unsigned int count)
+{
+	struct uart_ns921x_port *unp = ns921x_uart_ports[co->index];
+	u32 saved_txic, saved_wc, saved_uie;
+	u32 new_wc;
+
+	BUG_ON(!irqs_disabled());
+
+	saved_txic = uartread32(&unp->port, HUB_TXIC);
+	saved_wc = uartread32(&unp->port, UART_WC);
+	saved_uie = uartread32(&unp->port, UART_UIE);
+
+	new_wc = saved_wc | UART_WC_RXEN | UART_WC_TXEN;
+	new_wc &= ~(UART_WC_RXFLUSH | UART_WC_TXFLUSH);
+
+	/* XXX: assert baud, bits, parity etc. to be correct. */
+
+	uartwrite32(&unp->port, 0, HUB_TXIC);
+	uartwrite32(&unp->port, UART_FCR_FIFOEN, UART_FCR);
+	uartwrite32(&unp->port, UART_UIE_ETBEI, UART_UIE);
+	uartwrite32(&unp->port, new_wc, UART_WC);
+
+	uart_console_write(&unp->port, s, count, ns921x_uart_console_putchar);
+
+	/* wait for HUB fifo to become empty */
+	while (!(ns921x_uart_read_ifs(unp) & HUB_IFS_TXFE))
+		barrier();
+
+	while (!(uartread32(&unp->port, UART_LSR) & UART_LSR_TEMT))
+		barrier();
+
+	uartwrite32(&unp->port, saved_wc, UART_WC);
+	uartwrite32(&unp->port, saved_uie, UART_UIE);
+	uartwrite32(&unp->port, saved_txic, HUB_TXIC);
+}
+
+static void __init ns921x_uart_console_get_options(struct uart_ns921x_port *unp,
+		int *baud, int *parity, int *bits, int *flow)
+{
+	if (uartread32(&unp->port, UART_WC) & UART_WC_TXEN) {
+		u32 cval = uartread32(&unp->port, UART_LCR);
+		unsigned int quot = 0;
+
+		uartwrite32(&unp->port, UART_LCR_DLAB, UART_LCR);
+
+		/* XXX: write barrier */
+
+		quot = (uartread32(&unp->port, UART_BRDM) & 0xff) << 8;
+		quot |= uartread32(&unp->port, UART_BRDL) & 0xff;
+
+		uartwrite32(&unp->port, cval, UART_LCR);
+
+		*baud = unp->port.uartclk / (16 * quot);
+
+		*parity = 'n';
+		if (cval & UART_LCR_PARITY) {
+			if (cval & UART_LCR_EPAR)
+				*parity = 'e';
+			else
+				*parity = 'o';
+		}
+		if (cval & UART_LCR_SPAR) {
+			if( (cval & UART_LCR_PARITY) &&
+			    (cval & UART_LCR_EPAR) )
+				*parity = 'm';
+			else
+				*parity = 's';
+		}
+
+		switch (cval & UART_LCR_WLEN) {
+		case UART_LCR_WLEN_5:
+			*bits = 5;
+			break;
+		case UART_LCR_WLEN_6:
+			*bits = 6;
+			break;
+		case UART_LCR_WLEN_7:
+			*bits = 7;
+			break;
+		case UART_LCR_WLEN_8:
+			*bits = 8;
+			break;
+		}
+
+		/* XXX: *flow = ... */
+	}
+}
+
+static int __init ns921x_uart_console_setup(struct console *co, char *options)
+{
+	struct uart_ns921x_port *unp;
+	int baud = 38400;
+	int bits = 8;
+	int parity = 'n';
+	int flow = 'n';
+	int ret;
+
+	if (co->index >= NS921X_UART_NR)
+		co->index = 0;
+	unp = ns921x_uart_ports[co->index];
+	if (!unp)
+		return -ENODEV;
+
+	ret = clk_enable(unp->clk);
+	if (ret)
+		return ret;
+
+	unp->port.uartclk = clk_get_rate(unp->clk);
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+	else
+		ns921x_uart_console_get_options(unp,
+				&baud, &parity, &bits, &flow);
+
+	ret = uart_set_options(&unp->port, co, baud, parity, bits, flow);
+
+	/* disable the clock only in the error path, because
+	 * otherwise it would need to be enabled and disabled in console_write
+	 * where it must not sleep.  But as clk_disable does a gpio_free it
+	 * might.
+	 */
+	if (ret)
+		clk_disable(unp->clk);
+
+	return ret;
+}
+
+static struct uart_driver ns921x_uart_reg;
+static struct console ns921x_uart_console = {
+	.name = NS921X_TTY_NAME,
+	.write = ns921x_uart_console_write,
+	/* .read */
+	.device = uart_console_device,
+	/* .unblank */
+	.setup = ns921x_uart_console_setup,
+	/* .early_setup */
+	.flags = CON_PRINTBUFFER,
+	.index = -1,
+	/* .cflag */
+	.data = &ns921x_uart_reg,
+	/* .next */
+};
+
+#define NS921X_UART_CONSOLE (&ns921x_uart_console)
+#else
+#define NS921X_UART_CONSOLE NULL
+#endif
+
+static struct uart_driver ns921x_uart_reg = {
+	.owner = THIS_MODULE,
+	.driver_name = DRIVER_NAME,
+	.dev_name = NS921X_TTY_NAME,
+	.major = NS921X_TTY_MAJOR,
+	.minor = NS921X_TTY_MINOR_START,
+	.nr = NS921X_UART_NR,
+	.cons = NS921X_UART_CONSOLE,
+};
+
+static __devinit int ns921x_uart_pdrv_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct uart_ns921x_port *unp;
+	struct resource *mem;
+	void __iomem *base;
+	int line, irq;
+
+	line = pdev->id;
+	if (line < 0 || line >= ARRAY_SIZE(ns921x_uart_ports)) {
+		ret = -ENODEV;
+		dev_dbg(&pdev->dev, "%s: err_line\n", __func__);
+		goto err_line;
+	}
+
+	if (ns921x_uart_ports[line] != NULL) {
+		ret = -EBUSY;
+		dev_dbg(&pdev->dev, "%s: err_line\n", __func__);
+		goto err_line;
+	}
+
+	unp = kzalloc(sizeof(struct uart_ns921x_port), GFP_KERNEL);
+	ns921x_uart_ports[line] = unp;
+
+	if (unp == NULL) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc\n", __func__);
+		goto err_alloc;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		ret = -ENOENT;
+		dev_info(&pdev->dev, "%s: err_get_irq\n", __func__);
+		goto err_get_irq;
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem) {
+		ret = -ENODEV;
+		dev_dbg(&pdev->dev, "%s: err_get_mem\n", __func__);
+		goto err_get_mem;
+	}
+
+	base = ioremap(mem->start, 0x8000);
+	if (!base) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_ioremap\n", __func__);
+		goto err_ioremap;
+	}
+	dev_dbg(&pdev->dev, "%s: membase = %p, unp = %p\n",
+			__func__, base, unp);
+
+	unp->clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(unp->clk)) {
+		ret = PTR_ERR(unp->clk);
+		dev_dbg(&pdev->dev, "%s: err_clk_get -> %d\n", __func__, ret);
+		goto err_clk_get;
+	}
+
+	unp->data = pdev->dev.platform_data;
+	unp->port.dev = &pdev->dev;
+	unp->port.mapbase = mem->start;
+	unp->port.membase = base;
+	unp->port.iotype = UPIO_MEM;
+	unp->port.irq = irq;
+	unp->port.fifosize = 64; /* XXX */
+	unp->port.ops = &ns921x_uart_pops;
+	unp->port.flags = UPF_BOOT_AUTOCONF;
+	unp->port.line = line;
+
+	/* no DMA */
+	uartwrite32(&unp->port, HUB_DMARXCTRL_DIRECT, HUB_DMARXCTRL);
+	uartwrite32(&unp->port, HUB_DMATXCTRL_DIRECT, HUB_DMATXCTRL);
+
+	ret = uart_add_one_port(&ns921x_uart_reg, &unp->port);
+	if (ret) {
+
+		dev_dbg(&pdev->dev, "%s: err_uart_add1port -> %d\n",
+				__func__, ret);
+
+		clk_put(unp->clk);
+err_clk_get:
+
+		iounmap(base);
+err_ioremap:
+err_get_mem:
+err_get_irq:
+
+		kfree(unp);
+		ns921x_uart_ports[line] = NULL;
+err_alloc:
+
+err_line:
+
+		return ret;
+	}
+
+	/* Make the device wakeup capable, but disabled by default */
+	device_init_wakeup(&pdev->dev, 1);
+	device_set_wakeup_enable(&pdev->dev, 0);
+
+	platform_set_drvdata(pdev, unp);
+
+	return 0;
+}
+
+static __devexit int ns921x_uart_pdrv_remove(struct platform_device *pdev)
+{
+	int line;
+	struct uart_ns921x_port *unp = platform_get_drvdata(pdev);
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	line = unp->port.line;
+
+	uart_remove_one_port(&ns921x_uart_reg, &unp->port);
+	clk_put(unp->clk);
+	iounmap(unp->port.membase);
+	kfree(unp);
+	ns921x_uart_ports[line] = NULL;
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int ns921x_uart_pdrv_suspend(struct platform_device *pdev,
+		pm_message_t state)
+{
+	struct uart_ns921x_port *unp = platform_get_drvdata(pdev);
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	if (device_may_wakeup(&pdev->dev)) {
+		ret = enable_irq_wake(unp->port.irq);
+		if (ret)
+			dev_dbg(&pdev->dev, "%s: err_enable_irq_wake -> %d\n",
+					__func__, ret);
+
+		else {
+			uartwrite32(&unp->port, UART_RCMC0_ENABLE |
+					UART_RCMC0_MASK, UART_RCMC0);
+			/* Write a 0 and then enable to clear previous interrupts */
+			uartwrite32(&unp->port, 0x0, UART_AWC);
+			uartwrite32(&unp->port, UART_AWC_ENABLE, UART_AWC);
+		}
+	} else
+		ret = uart_suspend_port(&ns921x_uart_reg, &unp->port);
+
+	return ret;
+}
+
+static int ns921x_uart_pdrv_resume(struct platform_device *pdev)
+{
+	struct uart_ns921x_port *unp = platform_get_drvdata(pdev);
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	if (device_may_wakeup(&pdev->dev)) {
+		uartwrite32(&unp->port, 0x0, UART_RCMC0);
+		ret = disable_irq_wake(unp->port.irq);
+	}
+	else
+		ret = uart_resume_port(&ns921x_uart_reg, &unp->port);
+
+	return ret;
+}
+#else
+#define ns921x_uart_pdrv_suspend	NULL
+#define ns921x_uart_pdrv_resume		NULL
+#endif
+
+static struct platform_driver ns921x_uart_pdriver = {
+	.probe = ns921x_uart_pdrv_probe,
+	.remove = __devexit_p(ns921x_uart_pdrv_remove),
+	.suspend = ns921x_uart_pdrv_suspend,
+	.resume = ns921x_uart_pdrv_resume,
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ns921x_uart_init(void)
+{
+	int ret;
+
+	ret = uart_register_driver(&ns921x_uart_reg);
+	if (ret) {
+		pr_debug("%s: err_uart_register_driver\n", __func__);
+		goto err_uart_register_driver;
+	}
+
+	ret = platform_driver_register(&ns921x_uart_pdriver);
+
+	if (ret) {
+		pr_debug("%s: err_pdrv_register\n", __func__);
+
+		uart_unregister_driver(&ns921x_uart_reg);
+err_uart_register_driver:
+
+		return ret;
+	}
+	pr_info("Digi NS921x UART driver\n");
+
+	return 0;
+}
+
+static void __exit ns921x_uart_exit(void)
+{
+	platform_driver_unregister(&ns921x_uart_pdriver);
+	uart_unregister_driver(&ns921x_uart_reg);
+}
+
+module_init(ns921x_uart_init);
+module_exit(ns921x_uart_exit);
+
+MODULE_AUTHOR("Uwe Kleine-Koenig");
+MODULE_DESCRIPTION("Digi NS921x UART driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/serial/ns9360-serial.c b/drivers/serial/ns9360-serial.c
new file mode 100644
index 000000000000..ac37f6f5fcab
--- /dev/null
+++ b/drivers/serial/ns9360-serial.c
@@ -0,0 +1,878 @@
+/*
+ * drivers/serial/ns9360-serial.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ * Based on drivers/serial/ns9xxx_serial.c by Markus Pietrek
+ */
+
+#include <linux/clk.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/serial_core.h>
+#include <linux/serial.h>
+
+/* register offsets */
+
+#define UART_CTRLA	0x00
+#define UART_CTRLA_CE			(1 << 31)
+#define UART_CTRLA_BRK			(1 << 30)
+#define UART_CTRLA_EPS			(1 << 28)
+#define UART_CTRLA_PE			(1 << 27)
+#define UART_CTRLA_STOP			(1 << 26)
+#define UART_CTRLA_WLS_5		(0)
+#define UART_CTRLA_WLS_6		(1 << 24)
+#define UART_CTRLA_WLS_7		(2 << 24)
+#define UART_CTRLA_WLS_8		(3 << 24)
+#define UART_CTRLA_DTR			(1 << 17)
+#define UART_CTRLA_RTS			(1 << 16)
+#define UART_CTRLA_RIE_RDY		(1 << 11)
+#define UART_CTRLA_RIE_CLOSED		(1 << 9)
+#define UART_CTRLA_RIE_MASK		(0xe00)
+#define UART_CTRLA_RIC_MASK		(0xe0)
+#define UART_CTRLA_TIC_HALF		(1 << 2)
+#define UART_CTRLA_TIC_MASK		(0x1e)
+
+#define UART_CTRLB	0x04
+#define UART_CTRLB_RCGT			(1 << 26)
+
+#define UART_STATUSA		0x08
+#define UART_STATUSA_RXFDB_MASK		(3 << 20)
+#define UART_STATUSA_DCD		(1 << 19)
+#define UART_STATUSA_RI			(1 << 18)
+#define UART_STATUSA_DSR		(1 << 17)
+#define UART_STATUSA_CTS		(1 << 16)
+#define UART_STATUSA_RBRK		(1 << 15)
+#define UART_STATUSA_RFE		(1 << 14)
+#define UART_STATUSA_RPE		(1 << 13)
+#define UART_STATUSA_ROVER		(1 << 12)
+#define UART_STATUSA_RRDY		(1 << 11)
+#define UART_STATUSA_RBC		(1 << 9)
+#define UART_STATUSA_TRDY		(1 << 3)
+#define UART_STATUSA_TEMPTY		(1 << 0)
+
+#define UART_FIFO		0x10
+
+#define UART_BITRATE		0x0c
+#define UART_BITRATE_EBIT		(1 << 31)
+#define UART_BITRATE_TMODE		(1 << 30)
+#define UART_BITRATE_CLKMUX_BCLK	(1 << 24)
+#define UART_BITRATE_TCDR_16		(1 << 20)
+#define UART_BITRATE_RCDR_16		(1 << 18)
+#define UART_BITRATE_N_MASK		(0x7fff)
+
+#define UART_RXCHARTIMER	0x18
+#define UART_RXCHARTIMER_TRUN		(1 << 31)
+
+#if defined(CONFIG_SERIAL_NS9360_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
+#define SUPPORT_SYSRQ
+#endif
+
+#define DRIVER_NAME "ns9360-serial"
+
+#ifdef CONFIG_SERIAL_NS9360_COMPAT
+# define NS9360_TTY_NAME	"ttyS"
+# define NS9360_TTY_MAJOR	TTY_MAJOR
+# define NS9360_TTY_MINOR_START	64
+#else
+# define NS9360_TTY_NAME	"ttyNS"
+# define NS9360_TTY_MAJOR	204
+# define NS9360_TTY_MINOR_START	200
+#endif
+
+#define NS9360_UART_NR 4
+
+#define up2unp(up) container_of(up, struct uart_ns9360_port, port)
+struct uart_ns9360_port {
+	struct uart_port port;
+	struct clk *clk;
+};
+
+#ifdef CONFIG_SERIAL_NS9750_CONSOLE
+
+#endif
+
+static inline u32 uartread32(struct uart_port *port, unsigned int offset)
+{
+	u32 ret = ioread32(port->membase + offset);
+
+#if defined(DEBUG_UARTRW)
+	dev_info(port->dev, "read  0x%p -> 0x%08x\n",
+			port->membase + offset, ret);
+#endif
+
+	return ret;
+}
+
+static inline void uartwrite32(struct uart_port *port,
+		u32 value, unsigned int offset)
+{
+#if defined(DEBUG_UARTRW)
+	dev_info(port->dev, "write 0x%p <- 0x%08x\n",
+			port->membase + offset, value);
+#endif
+	iowrite32(value, port->membase + offset);
+}
+
+static inline void uartwrite8(struct uart_port *port, u8 value,
+		unsigned int offset)
+{
+#if defined(DEBUG_UARTRW)
+	dev_info(port->dev, "write 0x%p <- 0x%02x\n",
+			port->membase + offset, value);
+#endif
+	iowrite8(value, port->membase + offset);
+}
+
+static inline void ns9360_uart_wait_fifo_empty(struct uart_port *port)
+{
+	while (!(uartread32(port, UART_STATUSA) & UART_STATUSA_TEMPTY))
+		udelay(1);
+
+	/* TODO: until we have buffer closed again, do just a delay, enough
+	 * for 38400 baud */
+	mdelay(1);
+}
+
+static inline void ns9360_uart_wait_xmitr(struct uart_port *port)
+{
+	while (!(uartread32(port, UART_STATUSA) & UART_STATUSA_TRDY))
+		udelay(1);
+}
+
+/* called with port->lock taken */
+static void ns9360_uart_stop_tx(struct uart_port *port)
+{
+	u32 ctrl;
+	struct uart_ns9360_port *unp = up2unp(port);
+
+	assert_spin_locked(&unp->port.lock);
+
+	/* disable TIC_HALF */
+	ctrl = uartread32(port, UART_CTRLA);
+	ctrl &= ~UART_CTRLA_TIC_HALF;
+	uartwrite32(port, ctrl, UART_CTRLA);
+}
+
+/* receive chars from serial fifo and store them in tty
+ * This is called with port->lock taken */
+static void ns9360_uart_rx_chars(struct uart_ns9360_port *unp)
+{
+	struct tty_struct *tty = unp->port.info->port.tty;
+	u32 status, available, characters, flag;
+
+	/* acknowledge rbc if set */
+	status = uartread32(&unp->port, UART_STATUSA);
+	if (status & UART_STATUSA_RBC) {
+		uartwrite32(&unp->port, UART_STATUSA_RBC, UART_STATUSA);
+		status = uartread32(&unp->port, UART_STATUSA);
+	}
+
+	while (status & UART_STATUSA_RRDY) {
+		available = (status & UART_STATUSA_RXFDB_MASK) >> 20;
+		available = available ? : 4;
+
+		flag = TTY_NORMAL;
+
+		if (status & UART_STATUSA_ROVER) {
+			unp->port.icount.overrun++;
+			flag |= TTY_OVERRUN;
+		}
+		if (status & UART_STATUSA_RFE) {
+			unp->port.icount.frame++;
+			flag |= TTY_FRAME;
+		}
+		if (status & UART_STATUSA_RPE) {
+			unp->port.icount.parity++;
+			flag |= TTY_PARITY;
+		}
+		if (status & UART_STATUSA_RBRK) {
+			unp->port.icount.brk++;
+			flag |= TTY_BREAK;
+		}
+
+		/* read characters from fifo */
+		characters = uartread32(&unp->port, UART_FIFO);
+		do {
+			unp->port.icount.rx++;
+			/* TODO: switch to uart_insert_char */
+			tty_insert_flip_char(tty, characters & 0xff, flag);
+			characters >>= 8;
+		} while (--available);
+
+		status = uartread32(&unp->port, UART_STATUSA);
+	}
+
+	if (!tty->low_latency)
+		tty_flip_buffer_push(tty);
+
+#ifdef SUPPORT_SYSRQ
+	sport->port.sysrq = 0;
+#endif
+}
+
+/* send out chars in xmit buffer.  This is called with port->lock taken */
+static void ns9360_uart_tx_chars(struct uart_ns9360_port *unp)
+{
+	struct circ_buf *xmit = &unp->port.info->xmit;
+
+	assert_spin_locked(&unp->port.lock);
+
+	if (unp->port.x_char) {
+		uartwrite8(&unp->port, unp->port.x_char, UART_FIFO);
+		unp->port.icount.tx++;
+		unp->port.x_char = 0;
+	} else
+		if (!uart_circ_empty(xmit) && !uart_tx_stopped(&unp->port)) {
+			while (uartread32(&unp->port, UART_STATUSA) &
+					UART_STATUSA_TRDY) {
+				uartwrite8(&unp->port, xmit->buf[xmit->tail],
+						UART_FIFO);
+				xmit->tail = (xmit->tail + 1) &
+					(UART_XMIT_SIZE - 1);
+				unp->port.icount.tx++;
+				if (uart_circ_empty(xmit))
+					break;
+			}
+
+			/* wakup ourself if number of pending chars do not reach
+			 * the minimum wakeup level */
+			if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+				uart_write_wakeup(&unp->port);
+		}
+
+	/* if uart is empty now ? */
+	if (uart_circ_empty(xmit) || uart_tx_stopped(&unp->port))
+		ns9360_uart_stop_tx(&unp->port);
+}
+
+static irqreturn_t ns9360_uart_rxint(int irq, void *dev_id)
+{
+	struct uart_ns9360_port *unp = dev_id;
+
+	spin_lock(&unp->port.lock);
+	ns9360_uart_rx_chars(unp);
+	spin_unlock(&unp->port.lock);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t ns9360_uart_txint(int irq, void *dev_id)
+{
+	struct uart_ns9360_port *unp = dev_id;
+
+	spin_lock(&unp->port.lock);
+	ns9360_uart_tx_chars(unp);
+	spin_unlock(&unp->port.lock);
+
+	return IRQ_HANDLED;
+}
+
+static void ns9360_uart_release_port(struct uart_port *port)
+{
+	/* XXX: release_mem_region is marked as Compatibility cruft ??? */
+	release_mem_region(port->mapbase, 0x3f);
+}
+
+static int ns9360_uart_request_port(struct uart_port *port)
+{
+	return request_mem_region(port->mapbase, 0x3f,
+			DRIVER_NAME) ? 0 : -EBUSY;
+}
+
+static void ns9360_uart_enable_ms(struct uart_port *port)
+{
+	u32 status;
+
+	status = uartread32(port, UART_STATUSA);
+	/* status |= NS_SER_CTRL_A_RIC_MA; */
+	/* FIXME: sense? */
+	uartwrite32(port, status, UART_STATUSA);
+}
+
+static void ns9360_uart_shutdown(struct uart_port *port)
+{
+	struct uart_ns9360_port *unp = up2unp(port);
+	unsigned long flags;
+	u32 ctrl;
+
+	ns9360_uart_wait_fifo_empty(port);
+
+	spin_lock_irqsave(&port->lock, flags);
+	ctrl = uartread32(port, UART_CTRLA);
+	ctrl &= ~UART_CTRLA_RIE_RDY;
+	uartwrite32(port, ctrl, UART_CTRLA);
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	free_irq(port->irq, unp);
+	free_irq(port->irq + 1, unp);
+
+	clk_disable(unp->clk);
+}
+
+static int ns9360_uart_startup(struct uart_port *port)
+{
+	struct uart_ns9360_port *unp = up2unp(port);
+	int ret;
+	u32 ctrl;
+	unsigned long flags;
+
+	ret = clk_enable(unp->clk);
+	if (ret) {
+		dev_info(port->dev, "%s: err_clk_enable", __func__);
+		goto err_clk_enable;
+	}
+
+	unp->port.uartclk = clk_get_rate(unp->clk);
+
+	ret = request_irq(unp->port.irq, ns9360_uart_rxint, 0,
+			DRIVER_NAME, unp);
+	if (ret) {
+		dev_info(port->dev, "%s: err_request_irq_rx", __func__);
+		goto err_request_irq_rx;
+	}
+
+	ret = request_irq(unp->port.irq + 1, ns9360_uart_txint, 0,
+			DRIVER_NAME, unp);
+	if (ret) {
+		dev_info(port->dev, "%s: err_request_irq_tx", __func__);
+		goto err_request_irq_tx;
+	}
+
+	/* enable receive interrupts */
+	spin_lock_irqsave (&unp->port.lock, flags);
+	ctrl = uartread32(&unp->port, UART_CTRLA);
+	ctrl |= UART_CTRLA_CE | UART_CTRLA_RIE_RDY | UART_CTRLA_RIE_CLOSED;
+	uartwrite32(&unp->port, ctrl, UART_CTRLA);
+
+	/* enable modem status interrupts */
+	ns9360_uart_enable_ms(&unp->port);
+	spin_unlock_irqrestore (&unp->port.lock, flags);
+
+	/* enable character gap timer */
+	uartwrite32(&unp->port, UART_CTRLB_RCGT, UART_CTRLB);
+
+	return 0;
+
+err_request_irq_tx:
+	free_irq(unp->port.irq, unp);
+err_request_irq_rx:
+	clk_disable(unp->clk);
+err_clk_enable:
+	return ret;
+}
+
+static void ns9360_uart_set_termios(struct uart_port *port,
+		struct ktermios *termios, struct ktermios *old)
+{
+	struct uart_ns9360_port *unp = up2unp(port);
+	unsigned long flags;
+	unsigned int baud, quot, nr_bits;
+	u32 ctrl, bitrate, gap_timer;
+
+	baud = uart_get_baud_rate(port, termios, old, 0, port->uartclk / 16);
+	quot = uart_get_divisor(port, baud);
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	uart_update_timeout(port, termios->c_cflag, baud);
+
+	/* read out configuration and mask out bits going to be updated */
+	ctrl = uartread32(port, UART_CTRLA);
+	ctrl &= ~(UART_CTRLA_WLS_8 | UART_CTRLA_STOP | UART_CTRLA_PE |
+			UART_CTRLA_EPS);
+
+	ctrl |= UART_CTRLA_CE;
+
+	switch (termios->c_cflag & CSIZE) {
+	case CS5:
+		ctrl |= UART_CTRLA_WLS_5;
+		nr_bits = 5;
+		break;
+	case CS6:
+		ctrl |= UART_CTRLA_WLS_6;
+		nr_bits = 6;
+		break;
+	case CS7:
+		ctrl |= UART_CTRLA_WLS_7;
+		nr_bits = 7;
+		break;
+	default:
+	case CS8:
+		ctrl |= UART_CTRLA_WLS_8;
+		nr_bits = 8;
+		break;
+	}
+
+	if (termios->c_cflag & CSTOPB) {
+		ctrl |= UART_CTRLA_STOP;
+		nr_bits++;
+	}
+
+	if (termios->c_cflag & PARENB) {
+		ctrl |= UART_CTRLA_PE;
+		nr_bits++;
+	}
+
+	if (!(termios->c_cflag & PARODD)) {
+		ctrl |= UART_CTRLA_EPS;
+		nr_bits++;
+	}
+
+	/* set configuration */
+	ns9360_uart_wait_fifo_empty(port);
+	uartwrite32(port, ctrl, UART_CTRLA);
+
+	/* set baudrate */
+	bitrate = UART_BITRATE_EBIT | UART_BITRATE_CLKMUX_BCLK |
+		UART_BITRATE_TMODE | UART_BITRATE_TCDR_16 |
+		UART_BITRATE_RCDR_16 | ((quot - 1) & UART_BITRATE_N_MASK);
+	uartwrite32(port, bitrate, UART_BITRATE);
+
+	/* set character gap timer */
+	gap_timer = UART_RXCHARTIMER_TRUN;
+
+	/* calculate gap timer */
+	gap_timer |= (clk_get_rate(unp->clk) * nr_bits / baud / 8) - 1;
+	pr_debug(DRIVER_NAME " %s: gap-timer = %08x (baud=%i, clk=%li)\n",
+			__func__, gap_timer, baud, clk_get_rate(unp->clk));
+
+	uartwrite32(port, gap_timer, UART_RXCHARTIMER);
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static unsigned int ns9360_uart_tx_empty(struct uart_port *port)
+{
+	u32 status;
+
+	status = uartread32(port, UART_STATUSA);
+	return (status & UART_STATUSA_TRDY) ? TIOCSER_TEMT : 0;
+}
+
+static void ns9360_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	u32 mcr;
+
+	mcr = uartread32(port, UART_CTRLA);
+
+	if (mctrl & TIOCM_RTS)
+		mcr &= ~UART_CTRLA_RTS;
+	else
+		mcr |= UART_CTRLA_RTS;
+
+	if (mctrl & TIOCM_DTR)
+		mcr &= ~UART_CTRLA_DTR;
+	else
+		mcr |= UART_CTRLA_DTR;
+
+	uartwrite32(port, mcr, UART_CTRLA);
+}
+
+static unsigned int ns9360_uart_get_mctrl(struct uart_port *port)
+{
+	unsigned int status;
+	unsigned int ret = 0;
+
+	status = uartread32(port, UART_STATUSA);
+
+	if (!(status & UART_STATUSA_DCD))
+		ret |= TIOCM_CD;
+	if (!(status & UART_STATUSA_CTS))
+		ret |= TIOCM_CTS;
+	if (!(status & UART_STATUSA_DSR))
+		ret |= TIOCM_DSR;
+	if (!(status & UART_STATUSA_RI))
+		ret |= TIOCM_RI;
+
+	return ret;
+}
+
+static int ns9360_uart_verify_port(struct uart_port *port,
+		struct serial_struct *ser)
+{
+	struct uart_ns9360_port *unp = up2unp(port);
+	int ret = 0;
+
+	if (ser->type != PORT_UNKNOWN && ser->type != PORT_NS9360)
+		ret = -EINVAL;
+
+	if (ser->irq != unp->port.irq)
+		ret = -EINVAL;
+
+	if (ser->io_type != UPIO_MEM)
+		ret = -EINVAL;
+
+	return ret;
+}
+
+static void ns9360_uart_start_tx(struct uart_port *port)
+{
+	u32 ctrl;
+
+	ctrl = uartread32(port, UART_CTRLA);
+	ctrl |= UART_CTRLA_TIC_HALF;
+	uartwrite32(port, ctrl, UART_CTRLA);
+}
+
+static void ns9360_uart_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE)
+		if (!ns9360_uart_request_port(port))
+			port->type = PORT_NS9360;
+}
+
+static const char *ns9360_uart_type(struct uart_port *port)
+{
+	return port->type == PORT_NS9360 ? "NS9360" : NULL;
+}
+
+static void ns9360_uart_break_ctl(struct uart_port *port, int break_state)
+{
+	u32 ctrl;
+
+	ctrl = uartread32(port, UART_CTRLA);
+
+	if (break_state == -1)
+		ctrl |= UART_CTRLA_BRK;
+	else
+		ctrl &= ~UART_CTRLA_BRK;
+
+	uartwrite32(port, ctrl, UART_CTRLA);
+}
+
+static void ns9360_uart_stop_rx(struct uart_port *port)
+{
+	u32 ctrl;
+
+	ctrl = uartread32(port, UART_CTRLA);
+	ctrl &= ~(UART_CTRLA_RIE_RDY | UART_CTRLA_RIE_CLOSED);
+	uartwrite32(port, ctrl, UART_CTRLA);
+}
+
+static struct uart_ops ns9360_uart_pops = {
+	.stop_rx = ns9360_uart_stop_rx,
+	.enable_ms = ns9360_uart_enable_ms,
+	.break_ctl = ns9360_uart_break_ctl,
+	.type = ns9360_uart_type,
+	.config_port = ns9360_uart_config_port,
+	.start_tx = ns9360_uart_start_tx,
+	.verify_port = ns9360_uart_verify_port,
+	.set_mctrl = ns9360_uart_set_mctrl,
+	.get_mctrl = ns9360_uart_get_mctrl,
+	.tx_empty = ns9360_uart_tx_empty,
+	.set_termios = ns9360_uart_set_termios,
+	.shutdown = ns9360_uart_shutdown,
+	.startup = ns9360_uart_startup,
+	.stop_tx = ns9360_uart_stop_tx,
+	.release_port = ns9360_uart_release_port,
+	.request_port = ns9360_uart_request_port,
+};
+
+static struct uart_ns9360_port *ns9360_uart_ports[NS9360_UART_NR];
+
+#ifdef CONFIG_SERIAL_NS9360_CONSOLE
+
+static void ns9360_uart_console_write(struct console *co,
+		const char *s, unsigned int count)
+{
+	struct uart_ns9360_port *unp = ns9360_uart_ports[co->index];
+	u32 ctrl, saved_ctrl;
+
+	/* save current state */
+	saved_ctrl = uartread32(&unp->port, UART_CTRLA);
+
+	/* keep configuration, disable interrupts and enable uart */
+	ctrl = (saved_ctrl | UART_CTRLA_CE) & ~(UART_CTRLA_TIC_MASK |
+			UART_CTRLA_RIC_MASK | UART_CTRLA_RIE_MASK);
+	uartwrite32(&unp->port, ctrl, UART_CTRLA);
+
+	while (count) {
+		ns9360_uart_wait_xmitr(&unp->port);
+
+		if (*s == '\n') {
+			uartwrite8(&unp->port, '\r', UART_FIFO);
+			ns9360_uart_wait_xmitr(&unp->port);
+		}
+
+		uartwrite8(&unp->port, *s, UART_FIFO);
+
+		count--;
+		s++;
+	}
+
+	/* wait for fifo empty if uart was (and will be) disabled */
+	if (!(saved_ctrl & UART_CTRLA_CE))
+		ns9360_uart_wait_fifo_empty(&unp->port);
+
+	/* recover state */
+	uartwrite32(&unp->port, saved_ctrl, UART_CTRLA);
+}
+
+/* If the port was already initialised (eg, by a boot loader),
+ * try to determine the current setup. */
+static void __init ns9360_uart_console_get_options(struct uart_ns9360_port *unp,
+		int *baud, int *parity, int *bits, int *flow)
+{
+	u32 ctrl, bitrate;
+
+	ctrl = uartread32(&unp->port, UART_CTRLA);
+	bitrate = uartread32(&unp->port, UART_BITRATE);
+
+	/* not enabled, don't modify defaults */
+	if (!(ctrl & UART_CTRLA_CE))
+		return;
+
+	switch (ctrl & UART_CTRLA_WLS_8) {
+	case UART_CTRLA_WLS_5:
+		*bits = 5;
+		break;
+	case UART_CTRLA_WLS_6:
+		*bits = 6;
+		break;
+	case UART_CTRLA_WLS_7:
+		*bits = 7;
+		break;
+	default:
+	case UART_CTRLA_WLS_8:
+		*bits = 8;
+		break;
+	}
+
+	*parity = 'n';
+	if (ctrl & UART_CTRLA_PE) {
+		/* parity enabled */
+		if (ctrl & UART_CTRLA_EPS)
+			*parity = 'o';
+		else
+			*parity = 'e';
+	}
+
+	*baud = unp->port.uartclk /
+		(((bitrate & UART_BITRATE_N_MASK) + 1) * 16);
+}
+
+static int __init ns9360_uart_console_setup(struct console *co, char *options)
+{
+	struct uart_ns9360_port *unp;
+	int baud = 38400;
+	int bits = 8;
+	int parity = 'n';
+	int flow = 'n';
+
+	if (co->index >= NS9360_UART_NR)
+		co->index = 0;
+
+	unp = ns9360_uart_ports[co->index];
+	if (!unp)
+		return -ENODEV;
+
+	/* XXX: assert unp->clk is enabled */
+	unp->port.uartclk = clk_get_rate(unp->clk);
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+	else
+		ns9360_uart_console_get_options(unp,
+				&baud, &parity, &bits, &flow);
+
+	/* Enable UART. For some strange reason, in NS9360 the UART
+	 * must be enabled in the setup, rather than in the write
+	 * function, otherwise nothing is printed and the system
+	 * doesn't boot */
+	uartwrite32(&unp->port,
+		   uartread32(&unp->port, UART_CTRLA) | UART_CTRLA_CE,
+		   UART_CTRLA);
+
+	return uart_set_options(&unp->port, co, baud, parity, bits, flow);
+}
+
+static struct uart_driver ns9360_uart_reg;
+static struct console ns9360_uart_console = {
+	.name = NS9360_TTY_NAME,
+	.write = ns9360_uart_console_write,
+	.device = uart_console_device,
+	.setup = ns9360_uart_console_setup,
+	.flags = CON_PRINTBUFFER,
+	.index = -1,
+	.data = &ns9360_uart_reg,
+};
+
+#define NS9360_UART_CONSOLE (&ns9360_uart_console)
+#else
+#define NS9360_UART_CONSOLE NULL
+#endif
+
+static struct uart_driver ns9360_uart_reg = {
+	.owner = THIS_MODULE,
+	.driver_name = DRIVER_NAME,
+	.dev_name = NS9360_TTY_NAME,
+	.major = NS9360_TTY_MAJOR,
+	.minor = NS9360_TTY_MINOR_START,
+	.nr = NS9360_UART_NR,
+	.cons = NS9360_UART_CONSOLE,
+};
+
+static __devinit int ns9360_uart_pdrv_probe(struct platform_device *pdev)
+{
+	int ret, line, irq;
+	struct uart_ns9360_port *unp;
+	struct resource *mem;
+	void __iomem *base;
+
+	line = pdev->id;
+	if (line < 0 || line >= ARRAY_SIZE(ns9360_uart_ports)) {
+		ret = -ENODEV;
+		dev_info(&pdev->dev, "%s: err_line\n", __func__);
+		goto err_line;
+	}
+
+	if (ns9360_uart_ports[line] != NULL) {
+		ret = -EBUSY;
+		dev_info(&pdev->dev, "%s: err_line\n", __func__);
+		goto err_line;
+	}
+
+	unp = kzalloc(sizeof(struct uart_ns9360_port), GFP_KERNEL);
+	ns9360_uart_ports[line] = unp;
+
+	if (unp == NULL) {
+		ret = -ENOMEM;
+		dev_info(&pdev->dev, "%s: err_alloc\n", __func__);
+		goto err_alloc;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		ret = -ENOENT;
+		dev_info(&pdev->dev, "%s: err_get_irq\n", __func__);
+		goto err_get_irq;
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem) {
+		ret = -ENODEV;
+		dev_info(&pdev->dev, "%s: err_get_mem\n", __func__);
+		goto err_get_mem;
+	}
+
+	base = ioremap(mem->start, 0x3f);
+	if (!base) {
+		ret = -ENOMEM;
+		dev_info(&pdev->dev, "%s: err_ioremap\n", __func__);
+		goto err_ioremap;
+	}
+	dev_info(&pdev->dev, "%s: membase = %p, unp = %p\n",
+			__func__, base, unp);
+
+	unp->clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(unp->clk)) {
+		ret = PTR_ERR(unp->clk);
+		dev_info(&pdev->dev, "%s: err_clk_get -> %d\n", __func__, ret);
+		goto err_clk_get;
+	}
+
+	unp->port.dev = &pdev->dev;
+	unp->port.mapbase = mem->start;
+	unp->port.membase = base;
+	unp->port.iotype = UPIO_MEM;
+	unp->port.irq = irq;
+	unp->port.fifosize = 64; /* XXX */
+	unp->port.ops = &ns9360_uart_pops;
+	unp->port.flags = UPF_BOOT_AUTOCONF;
+	unp->port.line = line;
+
+	ret = uart_add_one_port(&ns9360_uart_reg, &unp->port);
+	if (ret) {
+		dev_info(&pdev->dev, "%s: err_uart_add1port -> %d\n",
+				__func__, ret);
+		goto err_uart_add1port;
+	}
+
+	return 0;
+
+err_uart_add1port:
+	clk_put(unp->clk);
+err_clk_get:
+	iounmap(base);
+err_ioremap:
+	release_resource(mem);
+err_get_mem:
+err_get_irq:
+	kfree(unp);
+	ns9360_uart_ports[line] = NULL;
+err_alloc:
+err_line:
+	return ret;
+}
+
+static __devexit int ns9360_uart_pdrv_remove(struct platform_device *pdev)
+{
+	int line;
+	struct uart_ns9360_port *unp = platform_get_drvdata(pdev);
+
+	line = unp->port.line;
+
+	uart_remove_one_port(&ns9360_uart_reg, &unp->port);
+	clk_put(unp->clk);
+	iounmap(unp->port.membase);
+	kfree(unp);
+	ns9360_uart_ports[line] = NULL;
+
+	return 0;
+}
+
+static struct platform_driver ns9360_uart_pdriver = {
+	.probe = ns9360_uart_pdrv_probe,
+	.remove = __devexit_p(ns9360_uart_pdrv_remove),
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ns9360_uart_init(void)
+{
+	int ret;
+
+	ret = uart_register_driver(&ns9360_uart_reg);
+	if (ret) {
+		pr_debug("%s: unable to register uart driver\n", __func__);
+		goto err_uart_register_driver;
+	}
+
+	ret = platform_driver_register(&ns9360_uart_pdriver);
+	if (ret) {
+		pr_debug("%s: unable to register platform driver\n", __func__);
+		goto err_platform_driver_register;
+	}
+
+	pr_info("Digi NS9360 UART driver\n");
+
+	return 0;
+
+err_platform_driver_register:
+	uart_unregister_driver(&ns9360_uart_reg);
+err_uart_register_driver:
+	return ret;
+}
+
+static void __exit ns9360_uart_exit(void)
+{
+	platform_driver_unregister(&ns9360_uart_pdriver);
+	uart_unregister_driver(&ns9360_uart_reg);
+}
+
+module_init(ns9360_uart_init);
+module_exit(ns9360_uart_exit);
+
+MODULE_DESCRIPTION("Digi NS9360 UART driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/serial/samsung.c b/drivers/serial/samsung.c
index 1e219d3d0352..ea30bc6d84e3 100644
--- a/drivers/serial/samsung.c
+++ b/drivers/serial/samsung.c
@@ -63,8 +63,12 @@
 #ifdef CONFIG_CPU_S3C2400
 #define NR_PORTS (2)
 #else
+#if defined(CONFIG_MACH_CC9M2443JS) || defined(CONFIG_MACH_CCW9M2443JS)
+#define NR_PORTS (4)
+#else
 #define NR_PORTS (3)
 #endif
+#endif
 
 /* port irq numbers */
 
@@ -827,7 +831,7 @@ static struct uart_ops s3c24xx_serial_ops = {
 static struct uart_driver s3c24xx_uart_drv = {
 	.owner		= THIS_MODULE,
 	.dev_name	= "s3c2410_serial",
-	.nr		= 3,
+	.nr		= NR_PORTS,
 	.cons		= S3C24XX_SERIAL_CONSOLE,
 	.driver_name	= S3C24XX_SERIAL_NAME,
 	.major		= S3C24XX_SERIAL_MAJOR,
@@ -872,7 +876,20 @@ static struct s3c24xx_uart_port s3c24xx_serial_ports[NR_PORTS] = {
 			.flags		= UPF_BOOT_AUTOCONF,
 			.line		= 2,
 		}
-	}
+	},
+	[3] = {
+                .port = {
+                        .lock           = __SPIN_LOCK_UNLOCKED(s3c24xx_serial_ports[3].port.lock),
+                        .iotype         = UPIO_MEM,
+                        .irq            = IRQ_S3C2443_RX3,
+                        .uartclk        = 0,
+                        .fifosize       = 16,
+                        .ops            = &s3c24xx_serial_ops,
+                        .flags          = UPF_BOOT_AUTOCONF,
+                        .line           = 3,
+                }
+        }
+
 #endif
 };
 
@@ -945,6 +962,10 @@ static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
 
 	port->mapbase	= res->start;
 	port->membase	= S3C24XX_VA_UART + (res->start - S3C24XX_PA_UART);
+
+	/* Use the number of the HW port as line number (Luis Galdos) */
+	port->line      = cfg->hwport;
+	
 	ret = platform_get_irq(platdev, 0);
 	if (ret < 0)
 		port->irq = 0;
diff --git a/drivers/serial/stmp-app.c b/drivers/serial/stmp-app.c
new file mode 100644
index 000000000000..6a56c444cf20
--- /dev/null
+++ b/drivers/serial/stmp-app.c
@@ -0,0 +1,1064 @@
+/*
+ * Freescale STMP37XX/STMP378X Application UART driver
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/wait.h>
+#include <linux/tty.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+#include <linux/serial.h>
+#include <linux/serial_core.h>
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <linux/clk.h>
+#include <linux/cpufreq.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+
+#include <asm/cacheflush.h>
+#include <mach/hardware.h>
+#include <mach/regs-apbx.h>
+#include <mach/regs-uartapp.h>
+#include <mach/regs-pinctrl.h>
+#include <mach/stmp3xxx.h>
+
+#include <asm/mach-types.h>
+
+#include "stmp-app.h"
+
+static int pio_mode /* = 0 */; 	/* PIO mode = 1, DMA mode = 0	*/
+
+static struct platform_driver stmp_appuart_driver = {
+	.probe = stmp_appuart_probe,
+	.remove = __devexit_p(stmp_appuart_remove),
+	.suspend = stmp_appuart_suspend,
+	.resume = stmp_appuart_resume,
+	.driver = {
+		.name = "stmp37xx-appuart",
+		.owner = THIS_MODULE,
+	},
+};
+
+static struct uart_driver stmp_appuart_uart = {
+	.owner = THIS_MODULE,
+	.driver_name = "appuart",
+	.dev_name = "ttySP",
+	.major = 242,
+	.minor = 0,
+	.nr = 1,
+};
+
+static inline struct stmp_appuart_port *to_appuart(struct uart_port *u)
+{
+	return container_of(u, struct stmp_appuart_port, port);
+}
+
+static struct uart_ops stmp_appuart_ops = {
+	.tx_empty       = stmp_appuart_tx_empty,
+	.start_tx       = stmp_appuart_start_tx,
+	.stop_tx	= stmp_appuart_stop_tx,
+	.stop_rx	= stmp_appuart_stop_rx,
+	.enable_ms      = stmp_appuart_enable_ms,
+	.break_ctl      = stmp_appuart_break_ctl,
+	.set_mctrl	= stmp_appuart_set_mctrl,
+	.get_mctrl      = stmp_appuart_get_mctrl,
+	.startup	= stmp_appuart_startup,
+	.shutdown       = stmp_appuart_shutdown,
+	.set_termios    = stmp_appuart_settermios,
+	.type	   	= stmp_appuart_type,
+	.release_port   = stmp_appuart_release_port,
+	.request_port   = stmp_appuart_request_port,
+	.config_port    = stmp_appuart_config_port,
+	.verify_port    = stmp_appuart_verify_port,
+};
+
+static inline int chr(int c)
+{
+	if (c < 0x20 || c > 0x7F)
+		return '#';
+	return c;
+}
+
+/* Allocate and initialize rx and tx DMA chains */
+static inline int stmp_appuart_dma_init(struct stmp_appuart_port *s)
+{
+	int err = 0;
+	struct stmp3xxx_dma_descriptor *t = &s->tx_desc;
+#ifndef RX_CHAIN
+	struct stmp3xxx_dma_descriptor *r = &s->rx_desc;
+#else
+	int i;
+#endif
+
+	err = stmp3xxx_dma_request(s->dma_rx, s->dev, s->dev->bus_id);
+	if (err)
+		goto out;
+	err = stmp3xxx_dma_request(s->dma_tx, s->dev, s->dev->bus_id);
+	if (err)
+		goto out1;
+
+#ifndef RX_CHAIN
+	err = stmp3xxx_dma_allocate_command(s->dma_rx, r);
+	if (err)
+		goto out2;
+#endif
+	err = stmp3xxx_dma_allocate_command(s->dma_tx, t);
+	if (err)
+		goto out3;
+	t->virtual_buf_ptr = dma_alloc_coherent(s->dev,
+					   TX_BUFFER_SIZE,
+					   &t->command->buf_ptr,
+					   GFP_DMA);
+	if (!t->virtual_buf_ptr)
+		goto out4;
+#ifdef DEBUG
+	memset(t->virtual_buf_ptr, 0x4B, TX_BUFFER_SIZE);
+#endif
+
+#ifndef RX_CHAIN
+	r->virtual_buf_ptr = dma_alloc_coherent(s->dev,
+					   RX_BUFFER_SIZE,
+					   &r->command->buf_ptr,
+					   GFP_DMA);
+	if (!r->virtual_buf_ptr)
+		goto out5;
+#ifdef DEBUG
+	memset(r->virtual_buf_ptr, 0x4C, RX_BUFFER_SIZE);
+#endif
+#else
+	stmp3xxx_dma_make_chain(s->dma_rx, &s->rx_chain, s->rxd, RX_CHAIN);
+	for (i = 0; i < RX_CHAIN; i++) {
+		struct stmp3xxx_dma_descriptor *r = s->rxd + i;
+
+		r->command->cmd =
+			BF_APBX_CHn_CMD_XFER_COUNT(RX_BUFFER_SIZE) |
+			BF_APBX_CHn_CMD_CMDWORDS(1) |
+			BM_APBX_CHn_CMD_WAIT4ENDCMD |
+			BM_APBX_CHn_CMD_SEMAPHORE |
+			BM_APBX_CHn_CMD_IRQONCMPLT |
+			BM_APBX_CHn_CMD_CHAIN |
+			BF_APBX_CHn_CMD_COMMAND(
+				BV_APBX_CHn_CMD_COMMAND__DMA_WRITE);
+		r->virtual_buf_ptr = dma_alloc_coherent(s->dev,
+					   RX_BUFFER_SIZE,
+					   &r->command->buf_ptr,
+					   GFP_DMA);
+		r->command->pio_words[0] = /* BM_UARTAPP_CTRL0_RUN | */
+			BF_UARTAPP_CTRL0_XFER_COUNT(RX_BUFFER_SIZE)|
+			BM_UARTAPP_CTRL0_RXTO_ENABLE |
+			BF_UARTAPP_CTRL0_RXTIMEOUT(3);
+	}
+#endif
+	return 0;
+
+	/*
+	 * would be necessary on other error paths
+
+	dma_free_coherent( s->dev, RX_BUFFER_SIZE, r->virtual_buf_ptr,
+			   r->command->buf_ptr);
+	*/
+out5:
+	dma_free_coherent(s->dev, TX_BUFFER_SIZE, t->virtual_buf_ptr,
+			   t->command->buf_ptr);
+out4:
+	stmp3xxx_dma_free_command(s->dma_tx, t);
+out3:
+#ifndef RX_CHAIN
+	stmp3xxx_dma_free_command(s->dma_rx, r);
+#endif
+out2:
+	stmp3xxx_dma_release(s->dma_tx);
+out1:
+	stmp3xxx_dma_release(s->dma_rx);
+out:
+	WARN_ON(err);
+	return err;
+}
+
+
+static void stmp_appuart_on(struct platform_device *dev)
+{
+	struct stmp_appuart_port *s = platform_get_drvdata(dev);
+
+	if (!pio_mode) {
+		/*
+		   Tell DMA to select UART.
+		   Both DMA channels are shared between app UART and IrDA.
+		   Target id of 0 means UART, 1 means IrDA
+		 */
+		stmp3xxx_dma_set_alt_target(s->dma_rx, 0);
+		stmp3xxx_dma_set_alt_target(s->dma_tx, 0);
+		/*
+		  Reset DMA channels
+		 */
+		stmp3xxx_dma_reset_channel(s->dma_rx);
+		stmp3xxx_dma_reset_channel(s->dma_tx);
+		stmp3xxx_dma_enable_interrupt(s->dma_rx);
+		stmp3xxx_dma_enable_interrupt(s->dma_tx);
+	}
+}
+
+#ifdef CONFIG_CPU_FREQ
+static int stmp_appuart_updateclk(struct device *dev, void *clkdata)
+{
+	struct stmp_appuart_port *s = dev_get_drvdata(dev);
+
+	if (s) {
+		s->port.uartclk = clk_get_rate(s->clk) * 1000;
+		/* FIXME: perform actual update */
+	}
+	return 0;
+}
+
+static int stmp_appuart_notifier(struct notifier_block *self,
+			unsigned long phase, void *p)
+{
+	int r = 0;
+
+	if ((phase == CPUFREQ_POSTCHANGE) ||
+	    (phase == CPUFREQ_RESUMECHANGE)) {
+			/* get new uartclock and setspeed */
+			r = driver_for_each_device(
+					&stmp_appuart_driver.driver,
+					NULL, p,
+					stmp_appuart_updateclk);
+	}
+	return (r == 0) ? NOTIFY_OK : NOTIFY_DONE;
+}
+
+static struct notifier_block stmp_appuart_nb = {
+	.notifier_call = &stmp_appuart_notifier,
+};
+#endif /* CONFIG_CPU_FREQ */
+
+static int __devinit stmp_appuart_probe(struct platform_device *device)
+{
+	struct stmp_appuart_port *s;
+	int err = 0;
+	struct resource *r;
+	int i;
+	u32 version;
+	int (*pinctl)(int req, int id);
+
+	s = kzalloc(sizeof(struct stmp_appuart_port), GFP_KERNEL);
+	if (!s) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	spin_lock_init(&s->lock);
+
+	s->clk = clk_get(NULL, "uart");
+	if (IS_ERR(s->clk)) {
+		err = PTR_ERR(s->clk);
+		goto out_free;
+	}
+	clk_enable(s->clk);
+	r = platform_get_resource(device, IORESOURCE_MEM, 0);
+	if (!r) {
+		err = -ENXIO;
+		goto out_free_clk;
+	}
+	s->port.mapbase = r->start;
+	s->port.irq = platform_get_irq(device, 0);
+	s->port.ops = &stmp_appuart_ops;
+	s->port.iotype = UPIO_MEM;
+	s->port.line = device->id < 0 ? 0 : device->id;
+	s->port.fifosize = 16;
+	s->port.timeout = HZ/10;
+	s->port.uartclk = clk_get_rate(s->clk) * 1000;
+	s->port.type = PORT_STMP37xx;
+	s->port.dev = s->dev = get_device(&device->dev);
+	s->ctrl = 0;
+	s->keep_irq = 0;
+
+	r = platform_get_resource(device, IORESOURCE_MEM, 0);
+	if (!r) {
+		err = -ENXIO;
+		goto out_free_clk;
+	}
+
+	dev_dbg(s->dev, "%s\n", __func__);
+	for (i = 0; i < ARRAY_SIZE(s->irq); i++)  {
+		s->irq[i] = platform_get_irq(device, i);
+		dev_dbg(s->dev, "Resources: irq[%d] = %d\n", i, s->irq[i]);
+		if (s->irq[i] < 0) {
+			err = s->irq[i];
+			goto out_free_clk;
+		}
+	}
+
+	r = platform_get_resource(device, IORESOURCE_DMA, 0);
+	if (!r) {
+		err = -ENXIO;
+		goto out_free;
+	}
+	s->dma_rx = r->start;
+
+	r = platform_get_resource(device, IORESOURCE_DMA, 1);
+	if (!r) {
+		err = -ENXIO;
+		goto out_free;
+	}
+	s->dma_tx = r->start;
+
+	r = platform_get_resource(device, IORESOURCE_MEM, 0);
+	if (!r) {
+		err = -ENXIO;
+		goto out_free;
+	}
+	s->mem = r->start;
+	s->memsize = r->end - r->start;
+
+
+#ifdef CONFIG_CPU_FREQ
+	cpufreq_register_notifier(&stmp_appuart_nb,
+			CPUFREQ_TRANSITION_NOTIFIER);
+#endif
+	platform_set_drvdata(device, s);
+
+	device_init_wakeup(&device->dev, 1);
+
+	stmp_appuart_dma_init(s);
+	stmp_appuart_on(device);
+
+	pinctl = device->dev.platform_data;
+	if (pinctl) {
+		err = pinctl(1, device->id);
+		if (err)
+			goto out_free_clk;
+	}
+
+	err = uart_add_one_port(&stmp_appuart_uart, &s->port);
+	if (err)
+		goto out_free_pins;
+
+	version = HW_UARTAPP_VERSION_RD();
+	printk(KERN_INFO"Found APPUART %d.%d.%d\n",
+		(version >> 24) & 0xFF,
+		(version >> 16) & 0xFF,
+		version & 0xFFFF);
+	return 0;
+
+out_free_pins:
+	if (pinctl)
+		pinctl(0, device->id);
+out_free_clk:
+	clk_put(s->clk);
+out_free:
+	platform_set_drvdata(device, NULL);
+	kfree(s);
+out:
+	return err;
+}
+
+static int __devexit stmp_appuart_remove(struct platform_device *device)
+{
+	struct stmp_appuart_port *s;
+	void (*pinctl)(int req, int id);
+
+	s = platform_get_drvdata(device);
+	if (s) {
+		pinctl = device->dev.platform_data;
+		put_device(s->dev);
+		clk_disable(s->clk);
+		clk_put(s->clk);
+		uart_remove_one_port(&stmp_appuart_uart, &s->port);
+		if (pinctl)
+			pinctl(0, device->id);
+		kfree(s);
+		platform_set_drvdata(device, NULL);
+	}
+
+	return 0;
+}
+
+static int stmp_appuart_suspend(struct platform_device *device,
+		pm_message_t state)
+{
+#ifdef CONFIG_PM
+	struct stmp_appuart_port *s = platform_get_drvdata(device);
+
+	if (!s)
+		return 0;
+	s->keep_irq = device_may_wakeup(&device->dev);
+	uart_suspend_port(&stmp_appuart_uart, &s->port);
+	if (!s->keep_irq)
+		clk_disable(s->clk);
+#endif
+	return 0;
+}
+
+static int stmp_appuart_resume(struct platform_device *device)
+{
+#ifdef CONFIG_PM
+	struct stmp_appuart_port *s = platform_get_drvdata(device);
+
+	if (!s)
+		return 0;
+
+	if (!s->keep_irq)
+		clk_enable(s->clk);
+	stmp_appuart_on(device);
+	uart_resume_port(&stmp_appuart_uart, &s->port);
+	s->keep_irq = 0;
+#endif
+	return 0;
+}
+
+static int __init stmp_appuart_init()
+{
+	int r;
+
+	r = uart_register_driver(&stmp_appuart_uart);
+	if (r)
+		goto out;
+	r = platform_driver_register(&stmp_appuart_driver);
+	if (r)
+		goto out_err;
+	return 0;
+out_err:
+	uart_unregister_driver(&stmp_appuart_uart);
+out:
+	return r;
+}
+
+static void __exit stmp_appuart_exit()
+{
+	platform_driver_unregister(&stmp_appuart_driver);
+	uart_unregister_driver(&stmp_appuart_uart);
+}
+
+module_init(stmp_appuart_init)
+module_exit(stmp_appuart_exit)
+
+static void stmp_appuart_stop_rx(struct uart_port *u)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+
+	dev_dbg(s->dev, "%s\n", __func__);
+	HW_UARTAPP_CTRL2_CLR_NB(s->mem, BM_UARTAPP_CTRL2_RXE);
+}
+
+static void stmp_appuart_break_ctl(struct uart_port *u, int ctl)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+
+	dev_dbg(s->dev, "%s: break = %s\n", __func__, ctl ? "on" : "off");
+	if (ctl)
+		HW_UARTAPP_LINECTRL_SET_NB(s->mem, BM_UARTAPP_LINECTRL_BRK);
+	else
+		HW_UARTAPP_LINECTRL_CLR_NB(s->mem, BM_UARTAPP_LINECTRL_BRK);
+}
+
+static void stmp_appuart_enable_ms(struct uart_port *port)
+{
+	/* just empty */
+}
+
+static void stmp_appuart_set_mctrl(struct uart_port *u, unsigned mctrl)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+
+	u32 ctrl = HW_UARTAPP_CTRL2_RD_NB(s->mem);
+
+	dev_dbg(s->dev, "%s (%x)\n", __func__, mctrl);
+	ctrl &= ~BM_UARTAPP_CTRL2_RTS;
+	if (mctrl & TIOCM_RTS) {
+		dev_dbg(s->dev, "...RTS\n");
+		ctrl |= BM_UARTAPP_CTRL2_RTS;
+	}
+	s->ctrl = mctrl;
+	dev_dbg(s->dev, "...%x; ctrl = %x\n", s->ctrl, ctrl);
+	HW_UARTAPP_CTRL2_WR_NB(s->mem, ctrl);
+}
+
+static u32 stmp_appuart_get_mctrl(struct uart_port *u)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+	u32 stat = HW_UARTAPP_STAT_RD_NB(s->mem);
+	int ctrl2 = HW_UARTAPP_CTRL2_RD_NB(s->mem);
+	u32 mctrl = s->ctrl;
+
+	dev_dbg(s->dev, "%s:\n", __func__);
+	mctrl &= ~TIOCM_CTS;
+	if (stat & BM_UARTAPP_STAT_CTS) {
+		dev_dbg(s->dev, "CTS");
+		mctrl |= TIOCM_CTS;
+	}
+	if (ctrl2 & BM_UARTAPP_CTRL2_RTS) {
+		dev_dbg(s->dev, "RTS");
+		mctrl |= TIOCM_RTS;
+	}
+	dev_dbg(s->dev, "...%x\n", mctrl);
+	return mctrl;
+}
+
+static int stmp_appuart_request_port(struct uart_port *u)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+	int err = 0;
+
+	if (!request_mem_region(s->mem, s->memsize, s->dev->bus_id))
+		err = -ENXIO;
+	return err;
+
+}
+
+static void stmp_appuart_release_port(struct uart_port *u)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+
+	release_mem_region(s->mem, s->memsize);
+}
+
+static int stmp_appuart_verify_port(struct uart_port *u,
+		struct serial_struct *ser)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+
+	dev_dbg(s->dev, "%s\n", __func__);
+	return 0;
+}
+
+static void stmp_appuart_config_port(struct uart_port *u, int flags)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+
+	dev_dbg(s->dev, "%s\n", __func__);
+}
+
+
+static const char *stmp_appuart_type(struct uart_port *u)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+
+	dev_dbg(s->dev, "%s\n", __func__);
+	return s->dev->bus_id;
+}
+
+static void stmp_appuart_settermios(struct uart_port *u,
+		struct ktermios *nw, struct ktermios *old)
+{
+	static struct ktermios saved;
+	struct stmp_appuart_port *s = to_appuart(u);
+	unsigned int cflag;
+	u32 bm, ctrl, ctrl2, div;
+	int err = 0;
+	unsigned baud;
+
+	dev_dbg(s->dev, "%s\n", __func__);
+
+	if (nw)
+		memcpy(&saved, nw, sizeof *nw);
+	else
+		nw = old = &saved;
+
+	cflag = nw->c_cflag;
+
+	ctrl = BM_UARTAPP_LINECTRL_FEN;
+	ctrl2 = HW_UARTAPP_CTRL2_RD_NB(s->mem);
+
+	/* byte size */
+	switch (cflag & CSIZE) {
+	case CS5: bm = 0; break;
+	case CS6: bm = 1; break;
+	case CS7: bm = 2; break;
+	case CS8: bm = 3; break;
+	default:  err = -EINVAL; break;
+	}
+	if (err)
+		goto out;
+
+	dev_dbg(s->dev, "Byte size %d bytes, mask %x\n",
+			bm + 5, BF_UARTAPP_LINECTRL_WLEN(bm));
+	ctrl |= BF_UARTAPP_LINECTRL_WLEN(bm);
+
+	/* parity */
+	if (cflag & PARENB) {
+		dev_dbg(s->dev, "Parity check enabled\n");
+		ctrl |= BM_UARTAPP_LINECTRL_PEN | BM_UARTAPP_LINECTRL_SPS;
+		if ((cflag & PARODD) == 0) {
+			dev_dbg(s->dev, "(Even) mask = %x\n",
+				BM_UARTAPP_LINECTRL_PEN |
+				BM_UARTAPP_LINECTRL_SPS |
+				BM_UARTAPP_LINECTRL_EPS);
+			ctrl |= BM_UARTAPP_LINECTRL_EPS;
+		} else
+			dev_dbg(s->dev, "(Odd) mask = %x\n",
+				BM_UARTAPP_LINECTRL_PEN |
+				BM_UARTAPP_LINECTRL_SPS);
+	} else
+		dev_dbg(s->dev, "Parity check disabled.\n");
+
+	/* figure out the stop bits requested */
+	if (cflag & CSTOPB) {
+		dev_dbg(s->dev, "Stop bits, mask = %x\n",
+				BM_UARTAPP_LINECTRL_STP2);
+		ctrl |= BM_UARTAPP_LINECTRL_STP2;
+	} else
+		dev_dbg(s->dev, "No stop bits\n");
+
+	/* figure out the hardware flow control settings */
+	if (cflag & CRTSCTS) {
+		dev_dbg(s->dev, "RTS/CTS flow control\n");
+		ctrl2 |= BM_UARTAPP_CTRL2_CTSEN /* | BM_UARTAPP_CTRL2_RTSEN */;
+	} else {
+		dev_dbg(s->dev, "RTS/CTS disabled\n");
+		ctrl2 &= ~BM_UARTAPP_CTRL2_CTSEN;
+	}
+
+	/* set baud rate */
+	baud = uart_get_baud_rate(u, nw, old, 0, u->uartclk);
+	dev_dbg(s->dev, "Baud rate requested: %d (clk = %d)\n",
+			baud, u->uartclk);
+	div = u->uartclk * 32 / baud;
+	ctrl |= BF_UARTAPP_LINECTRL_BAUD_DIVFRAC(div & 0x3F);
+	ctrl |= BF_UARTAPP_LINECTRL_BAUD_DIVINT(div >> 6);
+
+	if ((cflag & CREAD) != 0) {
+		dev_dbg(s->dev, "RX started\n");
+		ctrl2 |= BM_UARTAPP_CTRL2_RXE | BM_UARTAPP_CTRL2_RXDMAE;
+	}
+
+	if (!err) {
+		dev_dbg(s->dev, "CTRLS = %x + %x\n", ctrl, ctrl2);
+		HW_UARTAPP_LINECTRL_WR_NB(s->mem, ctrl);
+		HW_UARTAPP_CTRL2_WR_NB(s->mem, ctrl2);
+	}
+out:
+	return /* err */;
+}
+
+static int stmp_appuart_free_irqs(struct stmp_appuart_port *s)
+{
+	int irqn = 0;
+
+	if (s->keep_irq) {
+		dev_dbg(s->dev, "keep_irq != 0, ignoring\n");
+		return 0;
+	}
+	for (irqn = 0; irqn < ARRAY_SIZE(s->irq); irqn++)
+		free_irq(s->irq[irqn], s);
+	return 0;
+}
+
+void stmp_appuart_rx(struct stmp_appuart_port *s, u8 *rx_buffer, int count)
+{
+	u8 c;
+	int flag;
+	struct tty_struct *tty = s->port.info->port.tty;
+	u32 stat;
+
+	spin_lock(&s->lock);
+	stat = HW_UARTAPP_STAT_RD_NB(s->mem);
+
+	if (count < 0) {
+		count = HW_UARTAPP_STAT_RD_NB(s->mem) & BM_UARTAPP_STAT_RXCOUNT;
+		dev_dbg(s->dev, "count = %d\n", count);
+	}
+
+	for (;;) {
+		if (!rx_buffer) {
+			if (stat & BM_UARTAPP_STAT_RXFE)
+				break;
+			c = HW_UARTAPP_DATA_RD_NB(s->mem) & 0xFF;
+		} else {
+			if (count-- <= 0)
+				break;
+			c = *rx_buffer++;
+			dev_dbg(s->dev, "Received: %x(%c)\n", c, chr(c));
+		}
+
+		flag = TTY_NORMAL;
+		if (stat & BM_UARTAPP_STAT_BERR) {
+			stat &= ~BM_UARTAPP_STAT_BERR;
+			s->port.icount.brk++;
+			if (uart_handle_break(&s->port))
+				goto ignore;
+			flag = TTY_BREAK;
+		} else if (stat & BM_UARTAPP_STAT_PERR) {
+			stat &= ~BM_UARTAPP_STAT_PERR;
+				s->port.icount.parity++;
+				flag = TTY_PARITY;
+		} else if (stat & BM_UARTAPP_STAT_FERR) {
+			stat &= ~BM_UARTAPP_STAT_FERR;
+			s->port.icount.frame++;
+			flag = TTY_FRAME;
+		}
+
+		if (stat & BM_UARTAPP_STAT_OERR)
+			s->port.icount.overrun++;
+
+		if (uart_handle_sysrq_char(&s->port, c))
+			goto ignore;
+
+		uart_insert_char(&s->port, stat,
+			BM_UARTAPP_STAT_OERR, c, flag);
+ignore:
+		if (pio_mode) {
+			HW_UARTAPP_STAT_WR_NB(s->mem, stat);
+			stat = HW_UARTAPP_STAT_RD_NB(s->mem);
+		}
+	}
+
+	HW_UARTAPP_STAT_WR_NB(s->mem, stat);
+	tty_flip_buffer_push(tty);
+	spin_unlock(&s->lock);
+}
+
+static inline void stmp_appuart_submit_rx(struct stmp_appuart_port *s)
+{
+#ifndef RX_CHAIN
+	struct stmp3xxx_dma_descriptor *r = &s->rx_desc;
+
+	dev_dbg(s->dev, "Submitting RX DMA request\n");
+	r->command->cmd =
+			BM_APBX_CHn_CMD_HALTONTERMINATE |
+			BF_APBX_CHn_CMD_XFER_COUNT(RX_BUFFER_SIZE) |
+			BF_APBX_CHn_CMD_CMDWORDS(1) |
+			BM_APBX_CHn_CMD_WAIT4ENDCMD |
+			BM_APBX_CHn_CMD_SEMAPHORE |
+			BM_APBX_CHn_CMD_IRQONCMPLT |
+			BF_APBX_CHn_CMD_COMMAND(
+				BV_APBX_CHn_CMD_COMMAND__DMA_WRITE);
+	r->command->pio_words[0] = HW_UARTAPP_CTRL0_RD() |
+			BF_UARTAPP_CTRL0_XFER_COUNT(RX_BUFFER_SIZE)|
+			BM_UARTAPP_CTRL0_RXTO_ENABLE |
+			BF_UARTAPP_CTRL0_RXTIMEOUT(3);
+	r->command->pio_words[0] &= ~BM_UARTAPP_CTRL0_RUN;
+
+	stmp3xxx_dma_reset_channel(s->dma_rx);
+	stmp3xxx_dma_go(s->dma_rx, r, 1);
+#endif
+}
+
+static irqreturn_t stmp_appuart_irq_int(int irq, void *context)
+{
+	u32 istatus;
+	struct stmp_appuart_port *s = context;
+	u32 stat = HW_UARTAPP_STAT_RD_NB(s->mem);
+
+	istatus = HW_UARTAPP_INTR_RD_NB(s->mem);
+	dev_dbg(s->dev, "IRQ: int(%d), status = %08X\n", irq, istatus);
+
+	if (istatus & BM_UARTAPP_INTR_CTSMIS) {
+		uart_handle_cts_change(&s->port, stat & BM_UARTAPP_STAT_CTS);
+		dev_dbg(s->dev, "CTS change: %x\n", stat & BM_UARTAPP_STAT_CTS);
+		HW_UARTAPP_INTR_CLR_NB(s->mem, BM_UARTAPP_INTR_CTSMIS);
+	}
+
+	else if (istatus & BM_UARTAPP_INTR_RTIS) {
+		dev_dbg(s->dev, "RX timeout, draining out\n");
+		stmp_appuart_submit_rx(s);
+	}
+
+	else
+		dev_info(s->dev, "Unhandled status %x\n", istatus);
+
+	HW_UARTAPP_INTR_CLR_NB(s->mem, istatus & 0xFFFF);
+
+	return IRQ_HANDLED;
+}
+
+
+static irqreturn_t stmp_appuart_irq_rx(int irq, void *context)
+{
+	struct stmp_appuart_port *s = context;
+	int count = -1;
+
+	stmp3xxx_dma_clear_interrupt(s->dma_rx);
+	dev_dbg(s->dev, "%s(%d), count = %d\n", __func__, irq, count);
+
+#ifndef RX_CHAIN
+	stmp_appuart_rx(s, s->rx_desc.virtual_buf_ptr, count);
+	stmp_appuart_submit_rx(s);
+#else
+	if (circ_advance_cooked(&s->rx_chain) == 0) {
+		BUG();
+		return IRQ_HANDLED;
+	}
+
+	circ_advance_active(&s->rx_chain, 1);
+	while (s->rx_chain.cooked_count) {
+		stmp_appuart_rx(s,
+			stmp3xxx_dma_circ_get_cooked_head(&s->rx_chain)->
+				virtual_buf_ptr, -1);
+		circ_advance_free(&s->rx_chain, 1);
+	}
+#endif
+	return IRQ_HANDLED;
+}
+
+static void stmp_appuart_submit_tx(struct stmp_appuart_port *s, int size)
+{
+	struct stmp3xxx_dma_descriptor *d = &s->tx_desc;
+
+	dev_dbg(s->dev, "Submitting TX DMA request, %d bytes\n", size);
+	d->command->pio_words[0] =
+		/* BM_UARTAPP_CTRL1_RUN | */ BF_UARTAPP_CTRL1_XFER_COUNT(size);
+	d->command->cmd = BF_APBX_CHn_CMD_XFER_COUNT(size) |
+		BF_APBX_CHn_CMD_CMDWORDS(1) |
+		BM_APBX_CHn_CMD_WAIT4ENDCMD |
+		BM_APBX_CHn_CMD_SEMAPHORE |
+		BM_APBX_CHn_CMD_IRQONCMPLT |
+		BF_APBX_CHn_CMD_COMMAND(BV_APBX_CHn_CMD_COMMAND__DMA_READ);
+	stmp3xxx_dma_go(s->dma_tx, d, 1);
+}
+
+static irqreturn_t stmp_appuart_irq_tx(int irq, void *context)
+{
+	struct stmp_appuart_port *s = context;
+	struct uart_port *u = &s->port;
+	int bytes;
+
+	stmp3xxx_dma_clear_interrupt(s->dma_tx);
+	dev_dbg(s->dev, "%s(%d)\n", __func__, irq);
+
+	bytes = stmp_appuart_copy_tx(u, s->tx_desc.virtual_buf_ptr,
+			TX_BUFFER_SIZE);
+	if (bytes > 0) {
+		dev_dbg(s->dev, "Sending %d bytes\n", bytes);
+		stmp_appuart_submit_tx(s, bytes);
+	}
+	return IRQ_HANDLED;
+}
+
+static int stmp_appuart_request_irqs(struct stmp_appuart_port *s)
+{
+	int err = 0;
+
+	/*
+	 * order counts. resources should be listed in the same order
+	 */
+	irq_handler_t handlers[] = {
+		stmp_appuart_irq_int,
+		stmp_appuart_irq_rx,
+		stmp_appuart_irq_tx,
+	};
+	char *handlers_names[] = {
+		"appuart internal",
+		"appuart rx",
+		"appuart tx",
+	};
+	int irqn;
+
+	if (s->keep_irq) {
+		dev_dbg(s->dev, "keep_irq is set, skipping request_irq");
+		return 0;
+	}
+	for (irqn = 0; irqn < ARRAY_SIZE(handlers); irqn++) {
+		err = request_irq(s->irq[irqn], handlers[irqn],
+				0,  handlers_names[irqn], s);
+		dev_dbg(s->dev, "Requested IRQ %d with status %d\n",
+				s->irq[irqn], err);
+		if (err)
+			goto out;
+	}
+	return 0;
+out:
+	stmp_appuart_free_irqs(s);
+	return err;
+}
+
+static struct timer_list timer_task;
+
+static void stmp_appuart_check_rx(unsigned long data)
+{
+	stmp_appuart_rx((struct stmp_appuart_port *)data, NULL, -1);
+	mod_timer(&timer_task, jiffies + 2 * HZ);
+}
+
+static int stmp_appuart_startup(struct uart_port *u)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+	int err;
+
+	dev_dbg(s->dev, "%s\n", __func__);
+
+	s->tx_buffer_index = 0;
+
+	err = stmp_appuart_request_irqs(s);
+	if (err)
+		goto out;
+
+	if (!s->keep_irq)
+	/* Release the block from reset and start the clocks. */
+	stmp3xxx_reset_block(s->mem, 0);
+
+	HW_UARTAPP_CTRL2_SET_NB(s->mem, BM_UARTAPP_CTRL2_UARTEN);
+	/* Enable the Application UART DMA bits. */
+	if (!pio_mode) {
+		HW_UARTAPP_CTRL2_SET_NB(s->mem,
+			 BM_UARTAPP_CTRL2_TXDMAE | BM_UARTAPP_CTRL2_RXDMAE |
+			 BM_UARTAPP_CTRL2_DMAONERR);
+		/* clear any pending interrupts */
+		HW_UARTAPP_INTR_WR_NB(s->mem, 0);
+
+		/* reset all dma channels */
+		stmp3xxx_dma_reset_channel(s->dma_tx);
+		stmp3xxx_dma_reset_channel(s->dma_rx);
+	} else {
+		HW_UARTAPP_INTR_WR_NB(s->mem, BM_UARTAPP_INTR_RXIEN |
+				BM_UARTAPP_INTR_RTIEN);
+	}
+	HW_UARTAPP_INTR_SET_NB(s->mem, BM_UARTAPP_INTR_CTSMIEN);
+
+	/*
+	 * Enable fifo so all four bytes of a DMA word are written to
+	 * output (otherwise, only the LSB is written, ie. 1 in 4 bytes)
+	 */
+	HW_UARTAPP_LINECTRL_SET_NB(s->mem, BM_UARTAPP_LINECTRL_FEN);
+
+	if (!pio_mode) {
+#ifndef RX_CHAIN
+		stmp_appuart_submit_rx(s);
+#else
+		circ_clear_chain(&s->rx_chain);
+		stmp3xxx_dma_go(s->dma_rx, &s->rxd[0], 0);
+		circ_advance_active(&s->rx_chain, 1);
+#endif
+	} else {
+		init_timer(&timer_task);
+		timer_task.function = stmp_appuart_check_rx;
+		timer_task.expires = jiffies + HZ;
+		timer_task.data = (unsigned long)s;
+		add_timer(&timer_task);
+	}
+
+out:
+	return err;
+}
+
+static void stmp_appuart_shutdown(struct uart_port *u)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+
+	dev_dbg(s->dev, "%s\n", __func__);
+
+	if (!s->keep_irq)
+		/* set the IP block to RESET; this should disable clock too. */
+		HW_UARTAPP_CTRL0_SET_NB(s->mem, BM_UARTAPP_CTRL0_SFTRST);
+
+	if (!pio_mode) {
+		/* reset all dma channels */
+		stmp3xxx_dma_reset_channel(s->dma_tx);
+		stmp3xxx_dma_reset_channel(s->dma_rx);
+	} else {
+		del_timer(&timer_task);
+	}
+	stmp_appuart_free_irqs(s);
+}
+
+static unsigned int stmp_appuart_tx_empty(struct uart_port *u)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+
+	if (pio_mode)
+		if (HW_UARTAPP_STAT_RD_NB(s->mem) & BM_UARTAPP_STAT_TXFE)
+			return TIOCSER_TEMT;
+		else
+			return 0;
+	else
+		return stmp3xxx_dma_running(s->dma_tx) ? 0: TIOCSER_TEMT;
+}
+
+static void stmp_appuart_start_tx(struct uart_port *u)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+	int bytes;
+
+	dev_dbg(s->dev, "%s\n", __func__);
+
+	/* enable transmitter */
+	HW_UARTAPP_CTRL2_SET_NB(s->mem, BM_UARTAPP_CTRL2_TXE);
+
+	if (!pio_mode) {
+		if (stmp3xxx_dma_running(s->dma_tx))
+			return;
+		bytes = stmp_appuart_copy_tx(u, s->tx_desc.virtual_buf_ptr,
+				TX_BUFFER_SIZE);
+		if (bytes <= 0)
+			return;
+
+		dev_dbg(s->dev, "Started DMA transfer with descriptor %p, "
+			"command %p, %d bytes long\n",
+			&s->tx_desc, s->tx_desc.command, bytes);
+		stmp_appuart_submit_tx(s, bytes);
+	} else {
+		int count = 0;
+		u8 c;
+
+		while (!(HW_UARTAPP_STAT_RD_NB(s->mem) &
+					BM_UARTAPP_STAT_TXFF)) {
+			if (stmp_appuart_copy_tx(u, &c, 1) <= 0)
+				break;
+			dev_dbg(s->dev, "%d: '%c'/%x\n",
+				++count, chr(c), c);
+			HW_UARTAPP_DATA_WR_NB(s->mem, c);
+		}
+	}
+}
+
+static void stmp_appuart_stop_tx(struct uart_port *u)
+{
+	struct stmp_appuart_port *s = to_appuart(u);
+
+	dev_dbg(s->dev, "%s\n", __func__);
+	HW_UARTAPP_CTRL2_CLR_NB(s->mem, BM_UARTAPP_CTRL2_TXE);
+}
+
+static int stmp_appuart_copy_tx(struct uart_port *u, u8 *target,
+		int tx_buffer_size)
+{
+	int last = 0, portion;
+	struct circ_buf *xmit = &u->info->xmit;
+
+	while (last < tx_buffer_size) {	/* let's fill the only descriptor */
+		if (u->x_char) {
+			target[last++] = u->x_char;
+			u->x_char = 0;
+		} else if (!uart_circ_empty(xmit) && !uart_tx_stopped(u)) {
+			portion = min((u32)tx_buffer_size,
+				(u32)uart_circ_chars_pending(xmit));
+			portion = min((u32)portion,
+				(u32)CIRC_CNT_TO_END(xmit->head, xmit->tail,
+					UART_XMIT_SIZE));
+			memcpy(target + last, &xmit->buf[xmit->tail], portion);
+			xmit->tail = (xmit->tail + portion) &
+				(UART_XMIT_SIZE - 1);
+			if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+				uart_write_wakeup(u);
+			last += portion;
+		} else { /* All tx data copied into buffer */
+			return last;
+		}
+	}
+	return last;
+}
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("stmp3xxx app uart driver");
+MODULE_AUTHOR("dmitry pervushin <dimka@embeddedalley.com>");
+module_param(pio_mode, int, 0);
diff --git a/drivers/serial/stmp-app.h b/drivers/serial/stmp-app.h
new file mode 100644
index 000000000000..a4fee4217bba
--- /dev/null
+++ b/drivers/serial/stmp-app.h
@@ -0,0 +1,81 @@
+/*
+ * Freescale STMP37XX/STMP378X Application UART driver
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __STMP_APPUART_H
+#define __STMP_APPUART_H
+
+#define RX_BUFFER_SIZE	4
+#define TX_BUFFER_SIZE	0xFFF0
+
+#include <mach/dma.h>
+
+/* #define RX_CHAIN 2 */
+
+struct stmp_appuart_port {
+	int	keep_irq;
+	int	irq[3];
+	u32	mem, memsize;
+	int	dma_rx, dma_tx;
+	struct clk *clk;
+	struct device *dev;
+	struct uart_port port;
+	unsigned tx_buffer_index;
+	struct stmp3xxx_dma_descriptor tx_desc;
+#ifndef RX_CHAIN
+	struct stmp3xxx_dma_descriptor rx_desc;
+#else
+	struct stmp3xxx_dma_descriptor rxd[RX_CHAIN];
+	struct stmp37xx_circ_dma_chain rx_chain;
+#endif
+
+	u32 ctrl;
+	u8 running;
+	spinlock_t lock; /* protects irq handler */
+};
+
+#ifdef CONFIG_CPU_FREQ
+static int stmp_appuart_updateclk(struct device *dev, void *clkdata);
+static int stmp_appuart_notifier(struct notifier_block *self,
+		unsigned long phase, void *p);
+#endif /* CONFIG_CPU_FREQ */
+static int __init stmp_appuart_probe(struct platform_device *device);
+static int stmp_appuart_remove(struct platform_device *device);
+static int stmp_appuart_suspend(struct platform_device *device,
+		pm_message_t state);
+static int stmp_appuart_resume(struct platform_device *device);
+static int __init stmp_appuart_init(void);
+static void __exit stmp_appuart_exit(void);
+static int stmp_appuart_request_port(struct uart_port *u);
+static void stmp_appuart_release_port(struct uart_port *u);
+static int stmp_appuart_verify_port(struct uart_port *u,
+		struct serial_struct *);
+static void stmp_appuart_config_port(struct uart_port *u, int flags);
+static const char *stmp_appuart_type(struct uart_port *u);
+static void stmp_appuart_settermios(struct uart_port *u,
+		struct ktermios *nw, struct ktermios *old);
+static void stmp_appuart_shutdown(struct uart_port *u);
+static int stmp_appuart_startup(struct uart_port *u);
+static u32 stmp_appuart_get_mctrl(struct uart_port *u);
+static void stmp_appuart_set_mctrl(struct uart_port *u, unsigned mctrl);
+static void stmp_appuart_enable_ms(struct uart_port *port);
+static void stmp_appuart_break_ctl(struct uart_port *port, int ctl);
+static unsigned int stmp_appuart_tx_empty(struct uart_port *u);
+static void stmp_appuart_stop_rx(struct uart_port *u);
+static void stmp_appuart_start_tx(struct uart_port *u);
+static void stmp_appuart_stop_tx(struct uart_port *u);
+static int stmp_appuart_copy_tx(struct uart_port *u, u8 *target, int size);
+#endif
diff --git a/drivers/serial/stmp-dbg.c b/drivers/serial/stmp-dbg.c
new file mode 100644
index 000000000000..2848bb88ea7c
--- /dev/null
+++ b/drivers/serial/stmp-dbg.c
@@ -0,0 +1,839 @@
+/*
+ *  Freescale STMP37XX/STMP378X Debug UART driver
+ *
+ *  Based on drivers/char/serial.c, by Linus Torvalds, Theodore Ts'o.
+ *
+ *  Copyright 1999 ARM Limited
+ *  Copyright (C) 2000 Deep Blue Solutions Ltd.
+ *  Modifications for STMP36XX Debug Serial (c) 2005 Sigmatel Inc
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/autoconf.h>
+
+#if defined(CONFIG_SERIAL_STMP_DBG_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
+#define SUPPORT_SYSRQ
+#endif
+
+#include <linux/module.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/console.h>
+#include <linux/sysrq.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/serial_core.h>
+#include <linux/serial.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+
+#include <mach/regs-uartdbg.h>
+
+#include <mach/stmp3xxx.h>
+
+#include "stmp-dbg.h"
+
+/* treated as variable unless submitted to open-source */
+#define PORT_STMPDBG		100
+#define UART_NR			1
+#define SERIAL_STMPDBG_MAJOR	204
+#define SERIAL_STMPDBG_MINOR	16
+
+#define ISR_PASS_LIMIT		256
+
+#define STMPDBG_DEVID		"Debug UART"
+
+
+static int force_cd = 1;
+
+static struct uart_driver stmpdbg_reg;
+
+/*
+ * We wrap our port structure around the generic uart_port.
+ */
+struct uart_stmpdbg_port {
+	struct uart_port	port;
+	struct clk		*clk;
+	unsigned int		im;	/* interrupt mask */
+	unsigned int		old_status;
+	int			suspended;
+};
+
+
+static void stmpdbg_stop_tx(struct uart_port *port)
+{
+	struct uart_stmpdbg_port *uap = (struct uart_stmpdbg_port *)port;
+
+	uap->im &= ~UART011_TXIM;
+	__raw_writel(uap->im, uap->port.membase + UART011_IMSC);
+}
+
+static void stmpdbg_start_tx(struct uart_port *port)
+{
+	struct uart_stmpdbg_port *uap = (struct uart_stmpdbg_port *)port;
+
+	uap->im |= UART011_TXIM;
+	__raw_writel(uap->im, uap->port.membase + UART011_IMSC);
+}
+
+static void stmpdbg_stop_rx(struct uart_port *port)
+{
+	struct uart_stmpdbg_port *uap = (struct uart_stmpdbg_port *)port;
+
+	uap->im &= ~(UART011_RXIM|UART011_RTIM|UART011_FEIM|
+		     UART011_PEIM|UART011_BEIM|UART011_OEIM);
+	__raw_writel(uap->im, uap->port.membase + UART011_IMSC);
+}
+
+static void stmpdbg_enable_ms(struct uart_port *port)
+{
+	struct uart_stmpdbg_port *uap = (struct uart_stmpdbg_port *)port;
+
+	uap->im |= UART011_RIMIM|UART011_CTSMIM|UART011_DCDMIM|UART011_DSRMIM;
+	__raw_writel(uap->im, uap->port.membase + UART011_IMSC);
+}
+
+static void stmpdbg_rx_chars(struct uart_stmpdbg_port *uap)
+{
+	struct tty_struct *tty = uap->port.info->port.tty;
+	unsigned int status, ch, flag, rsr, max_count = 256;
+
+	status = __raw_readl(uap->port.membase + UART01x_FR);
+	while ((status & UART01x_FR_RXFE) == 0 && max_count--) {
+#if 0
+		if (tty->flip.count >= TTY_FLIPBUF_SIZE) {
+			if (tty->low_latency)
+				tty_flip_buffer_push(tty);
+			/*
+			 * If this failed then we will throw away the
+			 * bytes but must do so to clear interrupts
+			 */
+		}
+#endif
+
+		ch = __raw_readl(uap->port.membase + UART01x_DR);
+		flag = TTY_NORMAL;
+		uap->port.icount.rx++;
+
+		/*
+		 * Note that the error handling code is
+		 * out of the main execution path
+		 */
+		rsr = __raw_readl(uap->port.membase + UART01x_RSR)
+			| UART_DUMMY_RSR_RX;
+		if (unlikely(rsr & UART01x_RSR_ANY)) {
+			if (rsr & UART01x_RSR_BE) {
+				rsr &= ~(UART01x_RSR_FE | UART01x_RSR_PE);
+				uap->port.icount.brk++;
+				if (uart_handle_break(&uap->port))
+					goto ignore_char;
+			} else if (rsr & UART01x_RSR_PE)
+				uap->port.icount.parity++;
+			else if (rsr & UART01x_RSR_FE)
+				uap->port.icount.frame++;
+			if (rsr & UART01x_RSR_OE)
+				uap->port.icount.overrun++;
+
+			rsr &= uap->port.read_status_mask;
+
+			if (rsr & UART01x_RSR_BE)
+				flag = TTY_BREAK;
+			else if (rsr & UART01x_RSR_PE)
+				flag = TTY_PARITY;
+			else if (rsr & UART01x_RSR_FE)
+				flag = TTY_FRAME;
+		}
+
+		if (uart_handle_sysrq_char(&uap->port, ch))
+			goto ignore_char;
+
+		uart_insert_char(&uap->port, rsr, UART01x_RSR_OE, ch, flag);
+
+ignore_char:
+		status = __raw_readl(uap->port.membase + UART01x_FR);
+	}
+	tty_flip_buffer_push(tty);
+	return;
+}
+
+static void stmpdbg_tx_chars(struct uart_stmpdbg_port *uap)
+{
+	struct circ_buf *xmit = &uap->port.info->xmit;
+	int count;
+
+	if (uap->port.x_char) {
+		__raw_writel(uap->port.x_char, uap->port.membase + UART01x_DR);
+		uap->port.icount.tx++;
+		uap->port.x_char = 0;
+		return;
+	}
+	if (uart_circ_empty(xmit) || uart_tx_stopped(&uap->port)) {
+		stmpdbg_stop_tx(&uap->port);
+		return;
+	}
+
+	count = uap->port.fifosize >> 1;
+	do {
+		__raw_writel(xmit->buf[xmit->tail],
+				uap->port.membase + UART01x_DR);
+		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		uap->port.icount.tx++;
+		if (uart_circ_empty(xmit))
+			break;
+	} while (--count > 0);
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(&uap->port);
+
+	if (uart_circ_empty(xmit))
+		stmpdbg_stop_tx(&uap->port);
+}
+
+static void stmpdbg_modem_status(struct uart_stmpdbg_port *uap)
+{
+	unsigned int status, delta;
+
+	status = __raw_readl(uap->port.membase + UART01x_FR) &
+		UART01x_FR_MODEM_ANY;
+
+	delta = status ^ uap->old_status;
+	uap->old_status = status;
+
+	if (!delta)
+		return;
+
+	if (delta & UART01x_FR_DCD)
+		uart_handle_dcd_change(&uap->port, status & UART01x_FR_DCD);
+
+	if (delta & UART01x_FR_DSR)
+		uap->port.icount.dsr++;
+
+	if (delta & UART01x_FR_CTS)
+		uart_handle_cts_change(&uap->port, status & UART01x_FR_CTS);
+
+	wake_up_interruptible(&uap->port.info->delta_msr_wait);
+}
+
+static irqreturn_t stmpdbg_int(int irq, void *dev_id)
+{
+	struct uart_stmpdbg_port *uap = dev_id;
+	unsigned int status, pass_counter = ISR_PASS_LIMIT;
+	int handled = 0;
+
+	spin_lock(&uap->port.lock);
+
+	status = __raw_readl(uap->port.membase + UART011_MIS);
+	if (status) {
+		do {
+			__raw_writel(status & ~(UART011_TXIS|UART011_RTIS|
+					  UART011_RXIS),
+			       uap->port.membase + UART011_ICR);
+
+			if (status & (UART011_RTIS|UART011_RXIS))
+				stmpdbg_rx_chars(uap);
+			if (status & (UART011_DSRMIS|UART011_DCDMIS|
+				      UART011_CTSMIS|UART011_RIMIS))
+				stmpdbg_modem_status(uap);
+			if (status & UART011_TXIS)
+				stmpdbg_tx_chars(uap);
+
+			if (pass_counter-- == 0)
+				break;
+
+			status = __raw_readl(uap->port.membase + UART011_MIS);
+		} while (status != 0);
+		handled = 1;
+	}
+
+	spin_unlock(&uap->port.lock);
+
+	return IRQ_RETVAL(handled);
+}
+
+static unsigned int stmpdbg_tx_empty(struct uart_port *port)
+{
+	struct uart_stmpdbg_port *uap = (struct uart_stmpdbg_port *)port;
+	unsigned int status = __raw_readl(uap->port.membase + UART01x_FR);
+	return status & (UART01x_FR_BUSY|UART01x_FR_TXFF) ? 0 : TIOCSER_TEMT;
+}
+
+static unsigned int stmpdbg_get_mctrl(struct uart_port *port)
+{
+	struct uart_stmpdbg_port *uap = (struct uart_stmpdbg_port *)port;
+	unsigned int result = 0;
+	unsigned int status = __raw_readl(uap->port.membase + UART01x_FR);
+
+#define TEST_AND_SET_BIT(uartbit, tiocmbit)	do { \
+		if (status & uartbit)		\
+			result |= tiocmbit;	\
+	} while (0)
+
+	TEST_AND_SET_BIT(UART01x_FR_DCD, TIOCM_CAR);
+	TEST_AND_SET_BIT(UART01x_FR_DSR, TIOCM_DSR);
+	TEST_AND_SET_BIT(UART01x_FR_CTS, TIOCM_CTS);
+	TEST_AND_SET_BIT(UART011_FR_RI, TIOCM_RNG);
+#undef TEST_AND_SET_BIT
+	if (force_cd)
+		result |= TIOCM_CAR;
+	return result;
+}
+
+static void stmpdbg_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	struct uart_stmpdbg_port *uap = (struct uart_stmpdbg_port *)port;
+	unsigned int cr;
+
+	cr = __raw_readl(uap->port.membase + UART011_CR);
+
+#define	TEST_AND_SET_BIT(tiocmbit, uartbit)	do { \
+		if (mctrl & tiocmbit)		\
+			cr |= uartbit;		\
+		else				\
+			cr &= ~uartbit;		\
+	} while (0)
+
+	TEST_AND_SET_BIT(TIOCM_RTS, UART011_CR_RTS);
+	TEST_AND_SET_BIT(TIOCM_DTR, UART011_CR_DTR);
+	TEST_AND_SET_BIT(TIOCM_OUT1, UART011_CR_OUT1);
+	TEST_AND_SET_BIT(TIOCM_OUT2, UART011_CR_OUT2);
+	TEST_AND_SET_BIT(TIOCM_LOOP, UART011_CR_LBE);
+#undef TEST_AND_SET_BIT
+
+	__raw_writel(cr, uap->port.membase + UART011_CR);
+}
+
+static void stmpdbg_break_ctl(struct uart_port *port, int break_state)
+{
+	struct uart_stmpdbg_port *uap = (struct uart_stmpdbg_port *)port;
+	unsigned long flags;
+	unsigned int lcr_h;
+
+	spin_lock_irqsave(&uap->port.lock, flags);
+	lcr_h = __raw_readl(uap->port.membase + UART011_LCRH);
+	if (break_state == -1)
+		lcr_h |= UART01x_LCRH_BRK;
+	else
+		lcr_h &= ~UART01x_LCRH_BRK;
+	__raw_writel(lcr_h, uap->port.membase + UART011_LCRH);
+	spin_unlock_irqrestore(&uap->port.lock, flags);
+}
+
+static int stmpdbg_startup(struct uart_port *port)
+{
+	struct uart_stmpdbg_port *uap = (struct uart_stmpdbg_port *)port;
+	u32 cr, lcr;
+	int retval;
+
+	/*
+	 * Allocate the IRQ
+	 */
+	retval = request_irq(uap->port.irq, stmpdbg_int, 0, STMPDBG_DEVID, uap);
+	if (retval)
+		return retval;
+
+	__raw_writel(0, uap->port.membase + UART01x_DR); /* wake up the UART */
+
+	__raw_writel(UART011_IFLS_RX4_8|UART011_IFLS_TX4_8,
+	       uap->port.membase + UART011_IFLS);
+
+	/*
+	 * Provoke TX FIFO interrupt into asserting.
+	 */
+	cr = UART01x_CR_UARTEN | UART011_CR_RXE | UART011_CR_TXE;
+	__raw_writel(cr, uap->port.membase + UART011_CR);
+
+	lcr = __raw_readl(uap->port.membase + UART011_LCRH);
+	lcr |= UART01x_LCRH_FEN;
+	__raw_writel(lcr, uap->port.membase + UART011_LCRH);
+
+	/*
+	 * initialise the old status of the modem signals
+	 */
+	uap->old_status = __raw_readl(uap->port.membase + UART01x_FR) &
+		UART01x_FR_MODEM_ANY;
+
+	/*
+	 * Finally, enable interrupts
+	 */
+	spin_lock_irq(&uap->port.lock);
+	uap->im = UART011_RXIM | UART011_RTIM;
+	__raw_writel(uap->im, uap->port.membase + UART011_IMSC);
+	spin_unlock_irq(&uap->port.lock);
+
+	return 0;
+}
+
+static void stmpdbg_shutdown(struct uart_port *port)
+{
+	struct uart_stmpdbg_port *uap = (struct uart_stmpdbg_port *)port;
+	unsigned long val;
+
+	/*
+	 * disable all interrupts
+	 */
+	spin_lock_irq(&uap->port.lock);
+	uap->im = 0;
+	__raw_writel(uap->im, uap->port.membase + UART011_IMSC);
+	__raw_writel(0xffff, uap->port.membase + UART011_ICR);
+	spin_unlock_irq(&uap->port.lock);
+
+	free_irq(uap->port.irq, uap);
+
+	/*
+	 * disable the port
+	 */
+	__raw_writel(UART01x_CR_UARTEN | UART011_CR_TXE,
+			uap->port.membase + UART011_CR);
+
+	/*
+	 * disable break condition and fifos
+	 */
+	val = __raw_readl(uap->port.membase + UART011_LCRH);
+	val &= ~(UART01x_LCRH_BRK | UART01x_LCRH_FEN);
+	__raw_writel(val, uap->port.membase + UART011_LCRH);
+}
+
+static void
+stmpdbg_set_termios(struct uart_port *port, struct ktermios *termios,
+		     struct ktermios *old)
+{
+	unsigned int lcr_h, old_cr;
+	unsigned long flags;
+	unsigned int baud, quot;
+
+	/*
+	 * Ask the core to calculate the divisor for us.
+	 */
+	baud = uart_get_baud_rate(port, termios, old, 0, port->uartclk/16);
+	quot = port->uartclk * 4 / baud;
+
+	switch (termios->c_cflag & CSIZE) {
+	case CS5:
+		lcr_h = UART01x_LCRH_WLEN_5;
+		break;
+	case CS6:
+		lcr_h = UART01x_LCRH_WLEN_6;
+		break;
+	case CS7:
+		lcr_h = UART01x_LCRH_WLEN_7;
+		break;
+	default: /* CS8 */
+		lcr_h = UART01x_LCRH_WLEN_8;
+		break;
+	}
+	if (termios->c_cflag & CSTOPB)
+		lcr_h |= UART01x_LCRH_STP2;
+	if (termios->c_cflag & PARENB) {
+		lcr_h |= UART01x_LCRH_PEN;
+		if (!(termios->c_cflag & PARODD))
+			lcr_h |= UART01x_LCRH_EPS;
+	}
+	lcr_h |= UART01x_LCRH_FEN;
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	/*
+	 * Update the per-port timeout.
+	 */
+	uart_update_timeout(port, termios->c_cflag, baud);
+
+	port->read_status_mask = UART01x_RSR_OE;
+	if (termios->c_iflag & INPCK)
+		port->read_status_mask |= UART01x_RSR_FE | UART01x_RSR_PE;
+	if (termios->c_iflag & (BRKINT | PARMRK))
+		port->read_status_mask |= UART01x_RSR_BE;
+
+	/*
+	 * Characters to ignore
+	 */
+	port->ignore_status_mask = 0;
+	if (termios->c_iflag & IGNPAR)
+		port->ignore_status_mask |= UART01x_RSR_FE | UART01x_RSR_PE;
+	if (termios->c_iflag & IGNBRK) {
+		port->ignore_status_mask |= UART01x_RSR_BE;
+		/*
+		 * If we're ignoring parity and break indicators,
+		 * ignore overruns too (for real raw support).
+		 */
+		if (termios->c_iflag & IGNPAR)
+			port->ignore_status_mask |= UART01x_RSR_OE;
+	}
+
+	/*
+	 * Ignore all characters if CREAD is not set.
+	 */
+	if ((termios->c_cflag & CREAD) == 0)
+		port->ignore_status_mask |= UART_DUMMY_RSR_RX;
+
+	if (UART_ENABLE_MS(port, termios->c_cflag))
+		stmpdbg_enable_ms(port);
+
+	/* first, disable everything */
+	old_cr = __raw_readl(port->membase + UART011_CR);
+	__raw_writel(0, port->membase + UART011_CR);
+
+	/* Set baud rate */
+	__raw_writel(quot & 0x3f, port->membase + UART011_FBRD);
+	__raw_writel(quot >> 6, port->membase + UART011_IBRD);
+
+	/*
+	 * ----------v----------v----------v----------v-----
+	 * NOTE: MUST BE WRITTEN AFTER UARTLCR_M & UARTLCR_L
+	 * ----------^----------^----------^----------^-----
+	 */
+	__raw_writel(lcr_h, port->membase + UART011_LCRH);
+	__raw_writel(old_cr, port->membase + UART011_CR);
+
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static const char *stmpdbg_type(struct uart_port *port)
+{
+	return port->type == PORT_STMPDBG ? STMPDBG_DEVID : NULL;
+}
+
+
+/*
+ * Release the memory region(s) being used by 'port'
+ */
+static void stmpdbg_release_port(struct uart_port *port)
+{
+	release_mem_region(port->mapbase, UART_PORT_SIZE);
+}
+
+/*
+ * Request the memory region(s) being used by 'port'
+ */
+static int stmpdbg_request_port(struct uart_port *port)
+{
+	return request_mem_region(port->mapbase, UART_PORT_SIZE, STMPDBG_DEVID)
+			!= NULL ? 0 : -EBUSY;
+}
+
+/*
+ * Configure/autoconfigure the port.
+ */
+static void stmpdbg_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE) {
+		port->type = PORT_STMPDBG;
+		stmpdbg_request_port(port);
+	}
+}
+
+/*
+ * verify the new serial_struct (for TIOCSSERIAL).
+ */
+static int stmpdbg_verify_port(struct uart_port *port,
+		struct serial_struct *ser)
+{
+	int ret = 0;
+	if (ser->type != PORT_UNKNOWN && ser->type != PORT_STMPDBG)
+		ret = -EINVAL;
+	if (ser->irq < 0 || ser->irq >= NR_IRQS)
+		ret = -EINVAL;
+	if (ser->baud_base < 9600)
+		ret = -EINVAL;
+	return ret;
+}
+
+static struct uart_ops stmpdbg_pops = {
+	.tx_empty	= stmpdbg_tx_empty,
+	.set_mctrl	= stmpdbg_set_mctrl,
+	.get_mctrl	= stmpdbg_get_mctrl,
+	.stop_tx	= stmpdbg_stop_tx,
+	.start_tx	= stmpdbg_start_tx,
+	.stop_rx	= stmpdbg_stop_rx,
+	.enable_ms	= stmpdbg_enable_ms,
+	.break_ctl	= stmpdbg_break_ctl,
+	.startup	= stmpdbg_startup,
+	.shutdown	= stmpdbg_shutdown,
+	.set_termios	= stmpdbg_set_termios,
+	.type		= stmpdbg_type,
+	.release_port	= stmpdbg_release_port,
+	.request_port	= stmpdbg_request_port,
+	.config_port	= stmpdbg_config_port,
+	.verify_port	= stmpdbg_verify_port,
+};
+
+static struct uart_stmpdbg_port stmpdbg_ports[UART_NR] = {
+	{
+		.port	= {
+			/* This *is* the virtual address */
+			.membase	= (void *)HW_UARTDBGDR_ADDR,
+			.mapbase	= HW_UARTDBGDR_ADDR,
+			.iotype		= SERIAL_IO_MEM,
+			.irq		= IRQ_DEBUG_UART,
+			.fifosize	= 16,
+			.ops		= &stmpdbg_pops,
+			.flags		= ASYNC_BOOT_AUTOCONF,
+			.line		= 0,
+			.uartclk	= 24000000,
+		},
+	}
+};
+
+#ifdef CONFIG_SERIAL_STMP_DBG_CONSOLE
+
+static void
+stmpdbg_console_write(struct console *co, const char *s, unsigned int count)
+{
+	struct uart_port *port = &stmpdbg_ports[co->index].port;
+	unsigned int status, old_cr;
+	int i;
+
+	/*
+	 *	First save the CR then disable the interrupts
+	 */
+	old_cr = UART_GET_CR(port);
+	UART_PUT_CR(port, UART01x_CR_UARTEN);
+
+	/*
+	 *	Now, do each character
+	 */
+	for (i = 0; i < count; i++) {
+		do {
+			status = UART_GET_FR(port);
+		} while (!UART_TX_READY(status));
+		UART_PUT_CHAR(port, s[i]);
+		if (s[i] == '\n') {
+			do {
+				status = UART_GET_FR(port);
+			} while (!UART_TX_READY(status));
+			UART_PUT_CHAR(port, '\r');
+		}
+	}
+
+	/*
+	 *	Finally, wait for transmitter to become empty
+	 *	and restore the TCR
+	 */
+	do {
+		status = UART_GET_FR(port);
+	} while (status & UART01x_FR_BUSY);
+	UART_PUT_CR(port, old_cr);
+}
+
+static void __init
+stmpdbg_console_get_options(struct uart_port *port, int *baud,
+			     int *parity, int *bits)
+{
+	if (UART_GET_CR(port) & UART01x_CR_UARTEN) {
+		unsigned int lcr_h, quot;
+		lcr_h = UART_GET_LCRH(port);
+
+		*parity = 'n';
+		if (lcr_h & UART01x_LCRH_PEN) {
+			if (lcr_h & UART01x_LCRH_EPS)
+				*parity = 'e';
+			else
+				*parity = 'o';
+		}
+
+		if ((lcr_h & 0x60) == UART01x_LCRH_WLEN_7)
+			*bits = 7;
+		else
+			*bits = 8;
+
+		quot = UART_GET_LCRL(port) | UART_GET_LCRM(port) << 8;
+		*baud = port->uartclk / (16 * (quot + 1));
+	}
+}
+
+static int __init stmpdbg_console_setup(struct console *co, char *options)
+{
+	struct uart_port *port;
+	int baud = 115200;
+	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 >= UART_NR)
+		co->index = 0;
+	port = &stmpdbg_ports[co->index].port;
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+	else
+		stmpdbg_console_get_options(port, &baud, &parity, &bits);
+
+	return uart_set_options(port, co, baud, parity, bits, flow);
+}
+
+
+static struct console stmpdbg_console = {
+	.name		= "ttyAM",
+	.write		= stmpdbg_console_write,
+	.device		= uart_console_device,
+	.setup		= stmpdbg_console_setup,
+	.flags		= CON_PRINTBUFFER,
+	.index		= -1,
+	.data		= &stmpdbg_reg,
+};
+
+static int __init stmpdbg_console_init(void)
+{
+	/*
+	 * All port initializations are done statically
+	 */
+	register_console(&stmpdbg_console);
+	return 0;
+}
+console_initcall(stmpdbg_console_init);
+
+static int __init stmpdbg_late_console_init(void)
+{
+	if (!(stmpdbg_console.flags & CON_ENABLED))
+		register_console(&stmpdbg_console);
+	return 0;
+}
+late_initcall(stmpdbg_late_console_init);
+
+#endif
+
+static struct uart_driver stmpdbg_reg = {
+	.owner			= THIS_MODULE,
+	.driver_name		= "ttyAM",
+	.dev_name		= "ttyAM",
+	.major			= SERIAL_STMPDBG_MAJOR,
+	.minor			= SERIAL_STMPDBG_MINOR,
+	.nr			= UART_NR,
+#ifdef CONFIG_SERIAL_STMP_DBG_CONSOLE
+	.cons			= &stmpdbg_console,
+#endif
+};
+
+static int __devinit stmpdbguart_probe(struct platform_device *device)
+{
+	int ret = 0;
+	int i;
+	void (*cfg)(int request, int port) = NULL;
+
+	if (device->dev.platform_data)
+		cfg = device->dev.platform_data;
+
+	device_init_wakeup(&device->dev, 1);
+
+	for (i = 0; i < UART_NR; i++) {
+		stmpdbg_ports[i].clk = clk_get(NULL, "uart");
+		if (IS_ERR(stmpdbg_ports[i].clk))
+			continue;
+		stmpdbg_ports[i].suspended = 0;
+		stmpdbg_ports[i].port.dev = &device->dev;
+		stmpdbg_ports[i].port.uartclk =
+			clk_get_rate(stmpdbg_ports[i].clk) * 1000;
+		if (cfg)
+			(*cfg)(1, i);
+		uart_add_one_port(&stmpdbg_reg, &stmpdbg_ports[i].port);
+	}
+	return ret;
+}
+
+static int __devexit stmpdbguart_remove(struct platform_device *device)
+{
+	int i;
+	void (*cfg)(int request, int port) = NULL;
+
+	if (device->dev.platform_data)
+		cfg = device->dev.platform_data;
+	for (i = 0; i < UART_NR; i++) {
+		clk_put(stmpdbg_ports[i].clk);
+		uart_remove_one_port(&stmpdbg_reg, &stmpdbg_ports[0].port);
+		if (cfg)
+			(*cfg)(0, i);
+	}
+	return 0;
+}
+
+static int stmpdbguart_suspend(struct platform_device *device,
+					pm_message_t state)
+{
+#ifdef CONFIG_PM
+	int i;
+	int deep_sleep = (stmp37xx_pm_get_target() != PM_SUSPEND_STANDBY);
+
+	for (i = 0; i < UART_NR; i++) {
+		if (deep_sleep) {
+			uart_suspend_port(&stmpdbg_reg,
+				     &stmpdbg_ports[i].port);
+			clk_disable(stmpdbg_ports[i].clk);
+			stmpdbg_ports[i].suspended = 1;
+		}
+	}
+#endif
+	return 0;
+}
+
+static int stmpdbguart_resume(struct platform_device *device)
+{
+	int ret = 0;
+#ifdef CONFIG_PM
+	int i;
+
+	for (i = 0; i < UART_NR; i++) {
+		if (stmpdbg_ports[i].suspended) {
+			clk_enable(stmpdbg_ports[i].clk);
+			uart_resume_port(&stmpdbg_reg, &stmpdbg_ports[i].port);
+		}
+		stmpdbg_ports[i].suspended = 0;
+	}
+#endif
+	return ret;
+}
+
+static struct platform_driver stmpdbguart_driver = {
+	.probe = stmpdbguart_probe,
+	.remove = __devexit_p(stmpdbguart_remove),
+	.suspend = stmpdbguart_suspend,
+	.resume = stmpdbguart_resume,
+	.driver = {
+		.name = "stmp37xx-dbguart",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init stmpdbg_init(void)
+{
+	int ret;
+
+	ret = uart_register_driver(&stmpdbg_reg);
+	if (ret)
+		goto out;
+
+	ret = platform_driver_register(&stmpdbguart_driver);
+	if (ret)
+		uart_unregister_driver(&stmpdbg_reg);
+out:
+	return ret;
+}
+
+static void __exit stmpdbg_exit(void)
+{
+	platform_driver_unregister(&stmpdbguart_driver);
+	uart_unregister_driver(&stmpdbg_reg);
+}
+
+module_init(stmpdbg_init);
+module_exit(stmpdbg_exit);
+module_param(force_cd, int, 0644);
+MODULE_AUTHOR("ARM Ltd/Deep Blue Solutions Ltd/Sigmatel Inc");
+MODULE_DESCRIPTION("STMP37xx debug uart");
+MODULE_LICENSE("GPL");
diff --git a/drivers/serial/stmp-dbg.h b/drivers/serial/stmp-dbg.h
new file mode 100644
index 000000000000..38f3e6747fb3
--- /dev/null
+++ b/drivers/serial/stmp-dbg.h
@@ -0,0 +1,180 @@
+/*
+ *  Freescale STMP37XX/STMP378X Debug UART driver
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __STMP_DBG_H
+#define __STMP_DBG_H
+/*
+ * UART register offsets
+ */
+#define UART01x_DR		0x00	/* Data read or written 	*/
+#define UART01x_RSR		0x04	/* Receive status register 	*/
+#define UART01x_ECR		0x04	/* Error clear register (write) */
+#define UART010_LCRH		0x08	/* Line control register, MSB 	*/
+#define UART010_LCRM		0x0C	/* Line control register, 	*/
+#define UART010_LCRL		0x10	/* Line control register, LSB	*/
+#define UART010_CR		0x14	/* Control register. 		*/
+#define UART01x_FR		0x18	/* Flag register (Read only). 	*/
+#define UART010_IIR		0x1C	/* Interrupt identification 	*/
+#define UART010_ICR		0x1C	/* Interrupt clear register	*/
+#define UART01x_ILPR		0x20	/* IrDA low power counter 	*/
+#define UART011_IBRD		0x24	/* Integer baud rate divisor 	*/
+#define UART011_FBRD		0x28	/* Fractional baud rate divisor	*/
+#define UART011_LCRH		0x2c	/* Line control			*/
+#define UART011_CR		0x30	/* Control 			*/
+#define UART011_IFLS		0x34	/* Interrupt fifo level select 	*/
+#define UART011_IMSC		0x38	/* Interrupt mask		*/
+#define UART011_RIS		0x3c	/* Raw  	*/
+#define UART011_MIS		0x40	/* Masked 	*/
+#define UART011_ICR		0x44	/* Interrupt clear register	*/
+#define UART011_DMACR		0x48	/* DMA control register		*/
+
+#define UART011_DR_OE		(1 << 11)
+#define UART011_DR_BE		(1 << 10)
+#define UART011_DR_PE		(1 << 9)
+#define UART011_DR_FE		(1 << 8)
+
+#define UART01x_RSR_OE		0x08
+#define UART01x_RSR_BE		0x04
+#define UART01x_RSR_PE		0x02
+#define UART01x_RSR_FE		0x01
+
+#define UART011_FR_RI		0x100
+#define UART011_FR_TXFE		0x080
+#define UART011_FR_RXFF		0x040
+#define UART01x_FR_TXFF		0x020
+#define UART01x_FR_RXFE		0x010
+#define UART01x_FR_BUSY		0x008
+#define UART01x_FR_DCD		0x004
+#define UART01x_FR_DSR		0x002
+#define UART01x_FR_CTS		0x001
+#define UART01x_FR_TMSK		(UART01x_FR_TXFF + UART01x_FR_BUSY)
+
+#define UART011_CR_CTSEN	0x8000	/* CTS hardware flow control */
+#define UART011_CR_RTSEN	0x4000	/* RTS hardware flow control */
+#define UART011_CR_OUT2		0x2000	/* OUT2 */
+#define UART011_CR_OUT1		0x1000	/* OUT1 */
+#define UART011_CR_RTS		0x0800	/* RTS */
+#define UART011_CR_DTR		0x0400	/* DTR */
+#define UART011_CR_RXE		0x0200	/* receive enable */
+#define UART011_CR_TXE		0x0100	/* transmit enable */
+#define UART011_CR_LBE		0x0080	/* loopback enable */
+#define UART010_CR_RTIE		0x0040
+#define UART010_CR_TIE		0x0020
+#define UART010_CR_RIE		0x0010
+#define UART010_CR_MSIE		0x0008
+#define UART01x_CR_IIRLP	0x0004	/* SIR low power mode */
+#define UART01x_CR_SIREN	0x0002	/* SIR enable */
+#define UART01x_CR_UARTEN	0x0001	/* UART enable */
+
+#define UART011_LCRH_SPS	0x80
+#define UART01x_LCRH_WLEN_8	0x60
+#define UART01x_LCRH_WLEN_7	0x40
+#define UART01x_LCRH_WLEN_6	0x20
+#define UART01x_LCRH_WLEN_5	0x00
+#define UART01x_LCRH_FEN	0x10
+#define UART01x_LCRH_STP2	0x08
+#define UART01x_LCRH_EPS	0x04
+#define UART01x_LCRH_PEN	0x02
+#define UART01x_LCRH_BRK	0x01
+
+#define UART010_IIR_RTIS	0x08
+#define UART010_IIR_TIS		0x04
+#define UART010_IIR_RIS		0x02
+#define UART010_IIR_MIS		0x01
+
+#define UART011_IFLS_RX1_8	(0 << 3)
+#define UART011_IFLS_RX2_8	(1 << 3)
+#define UART011_IFLS_RX4_8	(2 << 3)
+#define UART011_IFLS_RX6_8	(3 << 3)
+#define UART011_IFLS_RX7_8	(4 << 3)
+#define UART011_IFLS_TX1_8	(0 << 0)
+#define UART011_IFLS_TX2_8	(1 << 0)
+#define UART011_IFLS_TX4_8	(2 << 0)
+#define UART011_IFLS_TX6_8	(3 << 0)
+#define UART011_IFLS_TX7_8	(4 << 0)
+
+/* Interrupt masks */
+#define UART011_OEIM		(1 << 10)	/* overrun error 	*/
+#define UART011_BEIM		(1 << 9)	/* break error 		*/
+#define UART011_PEIM		(1 << 8)	/* parity error 	*/
+#define UART011_FEIM		(1 << 7)	/* framing error 	*/
+#define UART011_RTIM		(1 << 6)	/* receive timeout 	*/
+#define UART011_TXIM		(1 << 5)	/* transmit 		*/
+#define UART011_RXIM		(1 << 4)	/* receive 		*/
+#define UART011_DSRMIM		(1 << 3)	/* DSR interrupt mask 	*/
+#define UART011_DCDMIM		(1 << 2)	/* DCD interrupt mask 	*/
+#define UART011_CTSMIM		(1 << 1)	/* CTS interrupt mask 	*/
+#define UART011_RIMIM		(1 << 0)	/* RI interrupt mask 	*/
+
+/* Interrupt statuses */
+#define UART011_OEIS		(1 << 10)	/* overrun error 	*/
+#define UART011_BEIS		(1 << 9)	/* break error 		*/
+#define UART011_PEIS		(1 << 8)	/* parity error 	*/
+#define UART011_FEIS		(1 << 7)	/* framing error  	*/
+#define UART011_RTIS		(1 << 6)	/* receive timeout  	*/
+#define UART011_TXIS		(1 << 5)	/* transmit  		*/
+#define UART011_RXIS		(1 << 4)	/* receive  		*/
+#define UART011_DSRMIS		(1 << 3)	/* DSR  		*/
+#define UART011_DCDMIS		(1 << 2)	/* DCD  		*/
+#define UART011_CTSMIS		(1 << 1)	/* CTS  		*/
+#define UART011_RIMIS		(1 << 0)	/* RI  			*/
+
+/* Interrupt clear masks */
+#define UART011_OEIC		(1 << 10)	/* overrun error 	*/
+#define UART011_BEIC		(1 << 9)	/* break error  	*/
+#define UART011_PEIC		(1 << 8)	/* parity error  	*/
+#define UART011_FEIC		(1 << 7)	/* framing error  	*/
+#define UART011_RTIC		(1 << 6)	/* receive timeout  	*/
+#define UART011_TXIC		(1 << 5)	/* transmit  		*/
+#define UART011_RXIC		(1 << 4)	/* receive  		*/
+#define UART011_DSRMIC		(1 << 3)	/* DSR  		*/
+#define UART011_DCDMIC		(1 << 2)	/* DCD  		*/
+#define UART011_CTSMIC		(1 << 1)	/* CTS  		*/
+#define UART011_RIMIC		(1 << 0)	/* RI  			*/
+
+#define UART011_DMAONERR	(1 << 2)	/* disable dma on error */
+#define UART011_TXDMAE		(1 << 1)	/* enable transmit dma 	*/
+#define UART011_RXDMAE		(1 << 0)	/* enable receive dma 	*/
+
+#define UART01x_RSR_ANY		(UART01x_RSR_OE | UART01x_RSR_BE | \
+				 UART01x_RSR_PE | UART01x_RSR_FE)
+#define UART01x_FR_MODEM_ANY	(UART01x_FR_DCD | UART01x_FR_DSR | \
+				 UART01x_FR_CTS)
+
+/*
+ * Access macros for the AMBA UARTs
+ */
+#define UART_GET_INT_STATUS(p)	readb((p)->membase + UART010_IIR)
+#define UART_PUT_ICR(p, c)	writel((c), (p)->membase + UART010_ICR)
+#define UART_GET_FR(p)		readb((p)->membase + UART01x_FR)
+#define UART_GET_CHAR(p)	readb((p)->membase + UART01x_DR)
+#define UART_PUT_CHAR(p, c)	writel((c), (p)->membase + UART01x_DR)
+#define UART_GET_RSR(p)		readb((p)->membase + UART01x_RSR)
+#define UART_GET_CR(p)		readb((p)->membase + UART010_CR)
+#define UART_PUT_CR(p, c)	writel((c), (p)->membase + UART010_CR)
+#define UART_GET_LCRL(p)	readb((p)->membase + UART010_LCRL)
+#define UART_PUT_LCRL(p, c)	writel((c), (p)->membase + UART010_LCRL)
+#define UART_GET_LCRM(p)	readb((p)->membase + UART010_LCRM)
+#define UART_PUT_LCRM(p, c)	writel((c), (p)->membase + UART010_LCRM)
+#define UART_GET_LCRH(p)	readb((p)->membase + UART010_LCRH)
+#define UART_PUT_LCRH(p, c)	writel((c), (p)->membase + UART010_LCRH)
+#define UART_RX_DATA(s)		(((s) & UART01x_FR_RXFE) == 0)
+#define UART_TX_READY(s)	(((s) & UART01x_FR_TXFF) == 0)
+#define UART_TX_EMPTY(p)	((UART_GET_FR(p) & UART01x_FR_TMSK) == 0)
+
+#define UART_DUMMY_RSR_RX	/*256*/0
+#define UART_PORT_SIZE		64
+
+#endif /* STMP_DBG_H */
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index b9d0efb6803f..189adbfee1c0 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -181,6 +181,12 @@ config SPI_S3C24XX_GPIO
 	  the inbuilt hardware cannot provide the transfer mode, or
 	  where the board is using non hardware connected pins.
 
+config SPI_S3C2443
+        tristate "Samsung S3C2443 High Speed SPI"
+        depends on SPI_MASTER && CPU_S3C2443
+        help
+          High Speed SPI driver for Samsung S3C2443
+	  
 config SPI_SH_SCI
 	tristate "SuperH SCI SPI controller"
 	depends on SUPERH
@@ -204,6 +210,58 @@ config SPI_XILINX
 	  See the "OPB Serial Peripheral Interface (SPI) (v1.00e)"
 	  Product Specification document (DS464) for hardware details.
 
+config SPI_NS921X
+	tristate "Digi ns921x SPI controller"
+	depends on PROCESSOR_NS921X
+	help
+	  Driver for SPI controller in Digi ns921x CPUs
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called spi_ns921x.
+
+config SPI_NS9360
+	tristate "Digi ns9360 SPI controller"
+	depends on PROCESSOR_NS9360
+	help
+	  Driver for SPI controller in Digi ns9360 CPUs
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called spi_ns9360.
+
+config SPI_MXC
+	tristate "MXC CSPI controller as SPI Master"
+	depends on ARCH_MXC && SPI_MASTER
+	select SPI_BITBANG
+	help
+	  This implements the SPI master mode using MXC CSPI.
+
+config SPI_MXC_TEST_LOOPBACK
+	bool "LOOPBACK Testing of CSPIs"
+	depends on SPI_MXC
+	select SPI_SPIDEV
+	default n
+
+config SPI_MXC_SELECT1
+	bool "CSPI1"
+	depends on SPI_MXC
+	default y
+
+config SPI_MXC_SELECT2
+	bool "CSPI2"
+	depends on SPI_MXC && (!ARCH_MXC91221)
+	default n
+
+config SPI_MXC_SELECT3
+	bool "CSPI3"
+	depends on SPI_MXC && (ARCH_MX3 || ARCH_MX27 || ARCH_MX25 || ARCH_MX37 || ARCH_MX51)
+	default n
+
+config SPI_STMP3XXX
+	tristate "Freescale STMP37xx/378x SPI/SSP controller"
+	depends on ARCH_STMP3XXX && SPI_MASTER
+	help
+	  SPI driver for Freescale STMP37xx/378x SoC SSP interface
+
 #
 # Add new SPI master controllers in alphabetical order above this line
 #
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index ccf18de34e1e..d1190a82a22f 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -27,8 +27,13 @@ obj-$(CONFIG_SPI_MPC83xx)		+= spi_mpc83xx.o
 obj-$(CONFIG_SPI_S3C24XX_GPIO)		+= spi_s3c24xx_gpio.o
 obj-$(CONFIG_SPI_S3C24XX)		+= spi_s3c24xx.o
 obj-$(CONFIG_SPI_TXX9)			+= spi_txx9.o
+obj-$(CONFIG_SPI_MXC)			+= mxc_spi.o
 obj-$(CONFIG_SPI_XILINX)		+= xilinx_spi.o
 obj-$(CONFIG_SPI_SH_SCI)		+= spi_sh_sci.o
+obj-$(CONFIG_SPI_NS921X)		+= spi_ns921x.o
+obj-$(CONFIG_SPI_NS9360)		+= spi_ns9360.o
+obj-$(CONFIG_SPI_S3C2443)               += spi_s3c2443.o
+obj-$(CONFIG_SPI_STMP3XXX)		+= spi_stmp.o
 # 	... add above this line ...
 
 # SPI protocol drivers (device/link on bus)
diff --git a/drivers/spi/mxc_spi.c b/drivers/spi/mxc_spi.c
new file mode 100644
index 000000000000..32e764059a68
--- /dev/null
+++ b/drivers/spi/mxc_spi.c
@@ -0,0 +1,1302 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-licensisr_locke.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup SPI Configurable Serial Peripheral Interface (CSPI) Driver
+ */
+
+/*!
+ * @file mxc_spi.c
+ * @brief This file contains the implementation of the SPI master controller services
+ *
+ *
+ * @ingroup SPI
+ */
+
+#include <linux/completion.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/types.h>
+#include <linux/clk.h>
+#include <linux/spi/spi.h>
+#include <linux/spi/spi_bitbang.h>
+#include <mach/hardware.h>
+
+#define MXC_CSPIRXDATA		0x00
+#define MXC_CSPITXDATA		0x04
+#define MXC_CSPICTRL		0x08
+#define MXC_CSPICONFIG		0x08
+#define MXC_CSPIINT			0x0C
+
+#define MXC_CSPICTRL_DISABLE	0x0
+#define MXC_CSPICTRL_SLAVE	0x0
+#define MXC_CSPICTRL_CSMASK	0x3
+#define MXC_CSPICTRL_SMC	(1 << 3)
+
+#define MXC_CSPIINT_TEEN_SHIFT		0
+#define MXC_CSPIINT_THEN_SHIFT	1
+#define MXC_CSPIINT_TFEN_SHIFT		2
+#define MXC_CSPIINT_RREN_SHIFT		3
+#define MXC_CSPIINT_RHEN_SHIFT       4
+#define MXC_CSPIINT_RFEN_SHIFT        5
+#define MXC_CSPIINT_ROEN_SHIFT        6
+
+#define MXC_HIGHPOL	0x0
+#define MXC_NOPHA	0x0
+#define MXC_LOWSSPOL		0x0
+
+#define MXC_CSPISTAT_TE		0
+#define MXC_CSPISTAT_TH		1
+#define MXC_CSPISTAT_TF		2
+#define MXC_CSPISTAT_RR		3
+#define MXC_CSPISTAT_RH		4
+#define MXC_CSPISTAT_RF		5
+#define MXC_CSPISTAT_RO		6
+
+#define MXC_CSPIPERIOD_32KHZ	(1 << 15)
+
+/*!
+ * @struct mxc_spi_unique_def
+ * @brief This structure contains information that differs with
+ * SPI master controller hardware version
+ */
+struct mxc_spi_unique_def {
+	/* Width of valid bits in MXC_CSPIINT */
+	unsigned int intr_bit_shift;
+	/* Chip Select shift */
+	unsigned int cs_shift;
+	/* Bit count shift */
+	unsigned int bc_shift;
+	/* Bit count mask */
+	unsigned int bc_mask;
+	/* Data Control shift */
+	unsigned int drctrl_shift;
+	/* Transfer Complete shift */
+	unsigned int xfer_complete;
+	/* Bit counnter overflow shift */
+	unsigned int bc_overflow;
+	/* FIFO Size */
+	unsigned int fifo_size;
+	/* Control reg address */
+	unsigned int ctrl_reg_addr;
+	/* Status reg address */
+	unsigned int stat_reg_addr;
+	/* Period reg address */
+	unsigned int period_reg_addr;
+	/* Test reg address */
+	unsigned int test_reg_addr;
+	/* Reset reg address */
+	unsigned int reset_reg_addr;
+	/* SPI mode mask */
+	unsigned int mode_mask;
+	/* SPI enable */
+	unsigned int spi_enable;
+	/* XCH bit */
+	unsigned int xch;
+	/* Spi mode shift */
+	unsigned int mode_shift;
+	/* Spi master mode enable */
+	unsigned int master_enable;
+	/* TX interrupt enable diff */
+	unsigned int tx_inten_dif;
+	/* RX interrupt enable bit diff */
+	unsigned int rx_inten_dif;
+	/* Interrupt status diff */
+	unsigned int int_status_dif;
+	/* Low pol shift */
+	unsigned int low_pol_shift;
+	/* Phase shift */
+	unsigned int pha_shift;
+	/* SS control shift */
+	unsigned int ss_ctrl_shift;
+	/* SS pol shift */
+	unsigned int ss_pol_shift;
+	/* Maximum data rate */
+	unsigned int max_data_rate;
+	/* Data mask */
+	unsigned int data_mask;
+	/* Data shift */
+	unsigned int data_shift;
+	/* Loopback control */
+	unsigned int lbc;
+	/* RX count off */
+	unsigned int rx_cnt_off;
+	/* RX count mask */
+	unsigned int rx_cnt_mask;
+	/* Reset start */
+	unsigned int reset_start;
+	/* SCLK control inactive state shift */
+	unsigned int sclk_ctl_shift;
+};
+
+struct mxc_spi;
+
+/*!
+ * Structure to group together all the data buffers and functions
+ * used in data transfers.
+ */
+struct mxc_spi_xfer {
+	/* Transmit buffer */
+	const void *tx_buf;
+	/* Receive buffer */
+	void *rx_buf;
+	/* Data transfered count */
+	unsigned int count;
+	/* Data received count, descending sequence, zero means no more data to
+	   be received */
+	unsigned int rx_count;
+	/* Function to read the FIFO data to rx_buf */
+	void (*rx_get) (struct mxc_spi *, u32 val);
+	/* Function to get the data to be written to FIFO */
+	 u32(*tx_get) (struct mxc_spi *);
+};
+
+/*!
+ * This structure is a way for the low level driver to define their own
+ * \b spi_master structure. This structure includes the core \b spi_master
+ * structure that is provided by Linux SPI Framework/driver as an
+ * element and has other elements that are specifically required by this
+ * low-level driver.
+ */
+struct mxc_spi {
+	/* SPI Master and a simple I/O queue runner */
+	struct spi_bitbang mxc_bitbang;
+	/* Completion flags used in data transfers */
+	struct completion xfer_done;
+	/* Data transfer structure */
+	struct mxc_spi_xfer transfer;
+	/* Resource structure, which will maintain base addresses and IRQs */
+	struct resource *res;
+	/* Base address of CSPI, used in readl and writel */
+	void *base;
+	/* CSPI IRQ number */
+	int irq;
+	/* CSPI Clock id */
+	struct clk *clk;
+	/* CSPI input clock SCLK */
+	unsigned long spi_ipg_clk;
+	/* CSPI registers' bit pattern */
+	struct mxc_spi_unique_def *spi_ver_def;
+	/* Control reg address */
+	void *ctrl_addr;
+	/* Status reg address */
+	void *stat_addr;
+	/* Period reg address */
+	void *period_addr;
+	/* Test reg address */
+	void *test_addr;
+	/* Reset reg address */
+	void *reset_addr;
+	/* Chipselect active function */
+	void (*chipselect_active) (int cspi_mode, int status, int chipselect);
+	/* Chipselect inactive function */
+	void (*chipselect_inactive) (int cspi_mode, int status, int chipselect);
+};
+
+#ifdef CONFIG_SPI_MXC_TEST_LOOPBACK
+struct spi_chip_info {
+	int lb_enable;
+};
+
+static struct spi_chip_info lb_chip_info = {
+	.lb_enable = 1,
+};
+
+static struct spi_board_info loopback_info[] = {
+#ifdef CONFIG_SPI_MXC_SELECT1
+	{
+	 .modalias = "spidev",
+	 .controller_data = &lb_chip_info,
+	 .irq = 0,
+	 .max_speed_hz = 4000000,
+	 .bus_num = 1,
+	 .chip_select = 4,
+	 },
+#endif
+#ifdef CONFIG_SPI_MXC_SELECT2
+	{
+	 .modalias = "spidev",
+	 .controller_data = &lb_chip_info,
+	 .irq = 0,
+	 .max_speed_hz = 4000000,
+	 .bus_num = 2,
+	 .chip_select = 4,
+	 },
+#endif
+#ifdef CONFIG_SPI_MXC_SELECT3
+	{
+	 .modalias = "spidev",
+	 .controller_data = &lb_chip_info,
+	 .irq = 0,
+	 .max_speed_hz = 4000000,
+	 .bus_num = 3,
+	 .chip_select = 4,
+	 },
+#endif
+};
+#endif
+
+static struct mxc_spi_unique_def spi_ver_2_3 = {
+	.intr_bit_shift = 8,
+	.cs_shift = 18,
+	.bc_shift = 20,
+	.bc_mask = 0xFFF,
+	.drctrl_shift = 16,
+	.xfer_complete = (1 << 7),
+	.bc_overflow = 0,
+	.fifo_size = 64,
+	.ctrl_reg_addr = 4,
+	.stat_reg_addr = 0x18,
+	.period_reg_addr = 0x1C,
+	.test_reg_addr = 0x20,
+	.reset_reg_addr = 0x8,
+	.mode_mask = 0xF,
+	.spi_enable = 0x1,
+	.xch = (1 << 2),
+	.mode_shift = 4,
+	.master_enable = 0,
+	.tx_inten_dif = 0,
+	.rx_inten_dif = 0,
+	.int_status_dif = 0,
+	.low_pol_shift = 4,
+	.pha_shift = 0,
+	.ss_ctrl_shift = 8,
+	.ss_pol_shift = 12,
+	.max_data_rate = 0xF,
+	.data_mask = 0xFF,
+	.data_shift = 8,
+	.lbc = (1 << 31),
+	.rx_cnt_off = 8,
+	.rx_cnt_mask = (0x7F << 8),
+	.reset_start = 0,
+	.sclk_ctl_shift = 20,
+};
+
+static struct mxc_spi_unique_def spi_ver_0_7 = {
+	.intr_bit_shift = 8,
+	.cs_shift = 12,
+	.bc_shift = 20,
+	.bc_mask = 0xFFF,
+	.drctrl_shift = 8,
+	.xfer_complete = (1 << 7),
+	.bc_overflow = 0,
+	.fifo_size = 8,
+	.ctrl_reg_addr = 0,
+	.stat_reg_addr = 0x14,
+	.period_reg_addr = 0x18,
+	.test_reg_addr = 0x1C,
+	.reset_reg_addr = 0x0,
+	.mode_mask = 0x1,
+	.spi_enable = 0x1,
+	.xch = (1 << 2),
+	.mode_shift = 1,
+	.master_enable = 1 << 1,
+	.tx_inten_dif = 0,
+	.rx_inten_dif = 0,
+	.int_status_dif = 0,
+	.low_pol_shift = 4,
+	.pha_shift = 5,
+	.ss_ctrl_shift = 6,
+	.ss_pol_shift = 7,
+	.max_data_rate = 0x7,
+	.data_mask = 0x7,
+	.data_shift = 16,
+	.lbc = (1 << 14),
+	.rx_cnt_off = 4,
+	.rx_cnt_mask = (0xF << 4),
+	.reset_start = 1,
+};
+
+static struct mxc_spi_unique_def spi_ver_0_5 = {
+	.intr_bit_shift = 9,
+	.cs_shift = 12,
+	.bc_shift = 20,
+	.bc_mask = 0xFFF,
+	.drctrl_shift = 8,
+	.xfer_complete = (1 << 8),
+	.bc_overflow = (1 << 7),
+	.fifo_size = 8,
+	.ctrl_reg_addr = 0,
+	.stat_reg_addr = 0x14,
+	.period_reg_addr = 0x18,
+	.test_reg_addr = 0x1C,
+	.reset_reg_addr = 0x0,
+	.mode_mask = 0x1,
+	.spi_enable = 0x1,
+	.xch = (1 << 2),
+	.mode_shift = 1,
+	.master_enable = 1 << 1,
+	.tx_inten_dif = 0,
+	.rx_inten_dif = 0,
+	.int_status_dif = 0,
+	.low_pol_shift = 4,
+	.pha_shift = 5,
+	.ss_ctrl_shift = 6,
+	.ss_pol_shift = 7,
+	.max_data_rate = 0x7,
+	.data_mask = 0x7,
+	.data_shift = 16,
+	.lbc = (1 << 14),
+	.rx_cnt_off = 4,
+	.rx_cnt_mask = (0xF << 4),
+	.reset_start = 1,
+};
+
+static struct mxc_spi_unique_def spi_ver_0_4 = {
+	.intr_bit_shift = 9,
+	.cs_shift = 24,
+	.bc_shift = 8,
+	.bc_mask = 0x1F,
+	.drctrl_shift = 20,
+	.xfer_complete = (1 << 8),
+	.bc_overflow = (1 << 7),
+	.fifo_size = 8,
+	.ctrl_reg_addr = 0,
+	.stat_reg_addr = 0x14,
+	.period_reg_addr = 0x18,
+	.test_reg_addr = 0x1C,
+	.reset_reg_addr = 0x0,
+	.mode_mask = 0x1,
+	.spi_enable = 0x1,
+	.xch = (1 << 2),
+	.mode_shift = 1,
+	.master_enable = 1 << 1,
+	.tx_inten_dif = 0,
+	.rx_inten_dif = 0,
+	.int_status_dif = 0,
+	.low_pol_shift = 4,
+	.pha_shift = 5,
+	.ss_ctrl_shift = 6,
+	.ss_pol_shift = 7,
+	.max_data_rate = 0x7,
+	.data_mask = 0x7,
+	.data_shift = 16,
+	.lbc = (1 << 14),
+	.rx_cnt_off = 4,
+	.rx_cnt_mask = (0xF << 4),
+	.reset_start = 1,
+};
+
+static struct mxc_spi_unique_def spi_ver_0_0 = {
+	.intr_bit_shift = 18,
+	.cs_shift = 19,
+	.bc_shift = 0,
+	.bc_mask = 0x1F,
+	.drctrl_shift = 12,
+	.xfer_complete = (1 << 3),
+	.bc_overflow = (1 << 8),
+	.fifo_size = 8,
+	.ctrl_reg_addr = 0,
+	.stat_reg_addr = 0x0C,
+	.period_reg_addr = 0x14,
+	.test_reg_addr = 0x10,
+	.reset_reg_addr = 0x1C,
+	.mode_mask = 0x1,
+	.spi_enable = (1 << 10),
+	.xch = (1 << 9),
+	.mode_shift = 11,
+	.master_enable = 1 << 11,
+	.tx_inten_dif = 9,
+	.rx_inten_dif = 10,
+	.int_status_dif = 1,
+	.low_pol_shift = 5,
+	.pha_shift = 6,
+	.ss_ctrl_shift = 7,
+	.ss_pol_shift = 8,
+	.max_data_rate = 0x10,
+	.data_mask = 0x1F,
+	.data_shift = 14,
+	.lbc = (1 << 14),
+	.rx_cnt_off = 4,
+	.rx_cnt_mask = (0xF << 4),
+	.reset_start = 1,
+};
+
+extern void gpio_spi_active(int cspi_mod);
+extern void gpio_spi_inactive(int cspi_mod);
+
+#define MXC_SPI_BUF_RX(type)	\
+void mxc_spi_buf_rx_##type(struct mxc_spi *master_drv_data, u32 val)\
+{\
+	type *rx = master_drv_data->transfer.rx_buf;\
+	*rx++ = (type)val;\
+	master_drv_data->transfer.rx_buf = rx;\
+}
+
+#define MXC_SPI_BUF_TX(type)    \
+u32 mxc_spi_buf_tx_##type(struct mxc_spi *master_drv_data)\
+{\
+	u32 val;\
+	const type *tx = master_drv_data->transfer.tx_buf;\
+	val = *tx++;\
+	master_drv_data->transfer.tx_buf = tx;\
+	return val;\
+}
+
+MXC_SPI_BUF_RX(u8)
+    MXC_SPI_BUF_TX(u8)
+    MXC_SPI_BUF_RX(u16)
+    MXC_SPI_BUF_TX(u16)
+    MXC_SPI_BUF_RX(u32)
+    MXC_SPI_BUF_TX(u32)
+
+/*!
+ * This function enables CSPI interrupt(s)
+ *
+ * @param        master_data the pointer to mxc_spi structure
+ * @param        irqs        the irq(s) to set (can be a combination)
+ *
+ * @return       This function returns 0 if successful, -1 otherwise.
+ */
+static int spi_enable_interrupt(struct mxc_spi *master_data, unsigned int irqs)
+{
+	if (irqs & ~((1 << master_data->spi_ver_def->intr_bit_shift) - 1)) {
+		return -1;
+	}
+
+	__raw_writel((irqs | __raw_readl(MXC_CSPIINT + master_data->ctrl_addr)),
+		     MXC_CSPIINT + master_data->ctrl_addr);
+	return 0;
+}
+
+/*!
+ * This function disables CSPI interrupt(s)
+ *
+ * @param        master_data the pointer to mxc_spi structure
+ * @param        irqs        the irq(s) to reset (can be a combination)
+ *
+ * @return       This function returns 0 if successful, -1 otherwise.
+ */
+static int spi_disable_interrupt(struct mxc_spi *master_data, unsigned int irqs)
+{
+	if (irqs & ~((1 << master_data->spi_ver_def->intr_bit_shift) - 1)) {
+		return -1;
+	}
+
+	__raw_writel((~irqs &
+		      __raw_readl(MXC_CSPIINT + master_data->ctrl_addr)),
+		     MXC_CSPIINT + master_data->ctrl_addr);
+	return 0;
+}
+
+/*!
+ * This function sets the baud rate for the SPI module.
+ *
+ * @param        master_data the pointer to mxc_spi structure
+ * @param        baud        the baud rate
+ *
+ * @return       This function returns the baud rate divisor.
+ */
+static unsigned int spi_find_baudrate(struct mxc_spi *master_data,
+				      unsigned int baud)
+{
+	unsigned int divisor;
+	unsigned int shift = 0;
+
+	/* Calculate required divisor (rounded) */
+	divisor = (master_data->spi_ipg_clk + baud / 2) / baud;
+	while (divisor >>= 1)
+		shift++;
+
+	if (master_data->spi_ver_def == &spi_ver_0_0) {
+		shift = (shift - 1) * 2;
+	} else if (master_data->spi_ver_def == &spi_ver_2_3) {
+		shift = shift;
+	} else {
+		shift -= 2;
+	}
+
+	if (shift > master_data->spi_ver_def->max_data_rate)
+		shift = master_data->spi_ver_def->max_data_rate;
+
+	return (shift << master_data->spi_ver_def->data_shift);
+}
+
+/*!
+ * This function loads the transmit fifo.
+ *
+ * @param  base             the CSPI base address
+ * @param  count            number of words to put in the TxFIFO
+ * @param  master_drv_data  spi master structure
+ */
+static void spi_put_tx_data(void *base, unsigned int count,
+			    struct mxc_spi *master_drv_data)
+{
+	unsigned int ctrl_reg;
+	unsigned int data;
+	int i = 0;
+
+	/* Perform Tx transaction */
+	for (i = 0; i < count; i++) {
+		data = master_drv_data->transfer.tx_get(master_drv_data);
+		__raw_writel(data, base + MXC_CSPITXDATA);
+	}
+
+	ctrl_reg = __raw_readl(base + MXC_CSPICTRL);
+
+	ctrl_reg |= master_drv_data->spi_ver_def->xch;
+
+	__raw_writel(ctrl_reg, base + MXC_CSPICTRL);
+
+	return;
+}
+
+/*!
+ * This function configures the hardware CSPI for the current SPI device.
+ * It sets the word size, transfer mode, data rate for this device.
+ *
+ * @param       spi     	the current SPI device
+ * @param	is_active 	indicates whether to active/deactivate the current device
+ */
+void mxc_spi_chipselect(struct spi_device *spi, int is_active)
+{
+	struct mxc_spi *master_drv_data;
+	struct mxc_spi_xfer *ptransfer;
+	struct mxc_spi_unique_def *spi_ver_def;
+	unsigned int ctrl_reg = 0;
+	unsigned int config_reg = 0;
+	unsigned int xfer_len;
+	unsigned int cs_value;
+
+	if (is_active == BITBANG_CS_INACTIVE) {
+		/*Need to deselect the slave */
+		return;
+	}
+
+	/* Get the master controller driver data from spi device's master */
+
+	master_drv_data = spi_master_get_devdata(spi->master);
+	clk_enable(master_drv_data->clk);
+	spi_ver_def = master_drv_data->spi_ver_def;
+
+	xfer_len = spi->bits_per_word;
+
+	if (spi_ver_def == &spi_ver_2_3) {
+		/* Control Register Settings for transfer to this slave */
+		ctrl_reg = master_drv_data->spi_ver_def->spi_enable;
+		ctrl_reg |=
+		    ((spi->chip_select & MXC_CSPICTRL_CSMASK) << spi_ver_def->
+		     cs_shift);
+		ctrl_reg |=
+		    (((1 << (spi->chip_select & MXC_CSPICTRL_CSMASK)) &
+		      spi_ver_def->mode_mask) << spi_ver_def->mode_shift);
+		ctrl_reg |=
+		    spi_find_baudrate(master_drv_data, spi->max_speed_hz);
+		ctrl_reg |=
+		    (((xfer_len -
+		       1) & spi_ver_def->bc_mask) << spi_ver_def->bc_shift);
+
+		if (spi->mode & SPI_CPHA)
+			config_reg |=
+			    (((1 << (spi->chip_select & MXC_CSPICTRL_CSMASK)) &
+			      spi_ver_def->mode_mask) <<
+			     spi_ver_def->pha_shift);
+
+		if ((spi->mode & SPI_CPOL)) {
+			config_reg |=
+			    (((1 << (spi->chip_select & MXC_CSPICTRL_CSMASK)) &
+			      spi_ver_def->mode_mask) <<
+			     spi_ver_def->low_pol_shift);
+			config_reg |=
+			    (((1 << (spi->chip_select & MXC_CSPICTRL_CSMASK)) &
+			      spi_ver_def->mode_mask) <<
+			     spi_ver_def->sclk_ctl_shift);
+		}
+		cs_value = (__raw_readl(MXC_CSPICONFIG +
+					master_drv_data->ctrl_addr) >>
+			    spi_ver_def->ss_pol_shift) & spi_ver_def->mode_mask;
+		if (spi->mode & SPI_CS_HIGH) {
+			config_reg |=
+			    ((((1 << (spi->chip_select & MXC_CSPICTRL_CSMASK)) &
+			       spi_ver_def->mode_mask) | cs_value) <<
+			     spi_ver_def->ss_pol_shift);
+		} else
+			config_reg |=
+			    ((~((1 << (spi->chip_select &
+				       MXC_CSPICTRL_CSMASK)) &
+				spi_ver_def->mode_mask) & cs_value) <<
+			     spi_ver_def->ss_pol_shift);
+		config_reg |=
+		    (((1 << (spi->chip_select & MXC_CSPICTRL_CSMASK)) &
+		      spi_ver_def->mode_mask) << spi_ver_def->ss_ctrl_shift);
+		__raw_writel(0, master_drv_data->base + MXC_CSPICTRL);
+		__raw_writel(ctrl_reg, master_drv_data->base + MXC_CSPICTRL);
+		__raw_writel(config_reg,
+			     MXC_CSPICONFIG + master_drv_data->ctrl_addr);
+	} else {
+		/* Control Register Settings for transfer to this slave */
+		ctrl_reg = master_drv_data->spi_ver_def->spi_enable;
+		ctrl_reg |=
+		    (((spi->chip_select & MXC_CSPICTRL_CSMASK) << spi_ver_def->
+		      cs_shift) | spi_ver_def->mode_mask <<
+		     spi_ver_def->mode_shift);
+		ctrl_reg |=
+		    spi_find_baudrate(master_drv_data, spi->max_speed_hz);
+		ctrl_reg |=
+		    (((xfer_len -
+		       1) & spi_ver_def->bc_mask) << spi_ver_def->bc_shift);
+		if (spi->mode & SPI_CPHA)
+			ctrl_reg |=
+			    spi_ver_def->mode_mask << spi_ver_def->pha_shift;
+		if (!(spi->mode & SPI_CPOL))
+			ctrl_reg |=
+			    spi_ver_def->mode_mask << spi_ver_def->
+			    low_pol_shift;
+		if (spi->mode & SPI_CS_HIGH)
+			ctrl_reg |=
+			    spi_ver_def->mode_mask << spi_ver_def->ss_pol_shift;
+		if (spi_ver_def == &spi_ver_0_7)
+			ctrl_reg |=
+			    spi_ver_def->mode_mask << spi_ver_def->
+			    ss_ctrl_shift;
+
+		__raw_writel(ctrl_reg, master_drv_data->base + MXC_CSPICTRL);
+	}
+
+	/* Initialize the functions for transfer */
+	ptransfer = &master_drv_data->transfer;
+	if (xfer_len <= 8) {
+		ptransfer->rx_get = mxc_spi_buf_rx_u8;
+		ptransfer->tx_get = mxc_spi_buf_tx_u8;
+	} else if (xfer_len <= 16) {
+		ptransfer->rx_get = mxc_spi_buf_rx_u16;
+		ptransfer->tx_get = mxc_spi_buf_tx_u16;
+	} else {
+		ptransfer->rx_get = mxc_spi_buf_rx_u32;
+		ptransfer->tx_get = mxc_spi_buf_tx_u32;
+	}
+#ifdef CONFIG_SPI_MXC_TEST_LOOPBACK
+	{
+		struct spi_chip_info *lb_chip =
+		    (struct spi_chip_info *)spi->controller_data;
+		if (!lb_chip)
+			__raw_writel(0, master_drv_data->test_addr);
+		else if (lb_chip->lb_enable)
+			__raw_writel(spi_ver_def->lbc,
+				     master_drv_data->test_addr);
+	}
+#endif
+	clk_disable(master_drv_data->clk);
+	return;
+}
+
+/*!
+ * This function is called when an interrupt occurs on the SPI modules.
+ * It is the interrupt handler for the SPI modules.
+ *
+ * @param        irq        the irq number
+ * @param        dev_id     the pointer on the device
+ *
+ * @return       The function returns IRQ_HANDLED when handled.
+ */
+static irqreturn_t mxc_spi_isr(int irq, void *dev_id)
+{
+	struct mxc_spi *master_drv_data = dev_id;
+	irqreturn_t ret = IRQ_NONE;
+	unsigned int status;
+	int fifo_size;
+	unsigned int pass_counter;
+
+	fifo_size = master_drv_data->spi_ver_def->fifo_size;
+	pass_counter = fifo_size;
+
+	/* Read the interrupt status register to determine the source */
+	status = __raw_readl(master_drv_data->stat_addr);
+	do {
+		u32 rx_tmp =
+		    __raw_readl(master_drv_data->base + MXC_CSPIRXDATA);
+
+		if (master_drv_data->transfer.rx_buf)
+			master_drv_data->transfer.rx_get(master_drv_data,
+							 rx_tmp);
+		(master_drv_data->transfer.count)--;
+		(master_drv_data->transfer.rx_count)--;
+		ret = IRQ_HANDLED;
+		if (pass_counter-- == 0) {
+			break;
+		}
+		status = __raw_readl(master_drv_data->stat_addr);
+	} while (status &
+		 (1 <<
+		  (MXC_CSPISTAT_RR +
+		   master_drv_data->spi_ver_def->int_status_dif)));
+
+	if (master_drv_data->transfer.rx_count)
+		return ret;
+
+	if (master_drv_data->transfer.count) {
+		if (master_drv_data->transfer.tx_buf) {
+			u32 count = (master_drv_data->transfer.count >
+				     fifo_size) ? fifo_size :
+			    master_drv_data->transfer.count;
+			master_drv_data->transfer.rx_count = count;
+			spi_put_tx_data(master_drv_data->base, count,
+					master_drv_data);
+		}
+	} else {
+		complete(&master_drv_data->xfer_done);
+	}
+
+	return ret;
+}
+
+/*!
+ * This function initialize the current SPI device.
+ *
+ * @param        spi     the current SPI device.
+ *
+ */
+int mxc_spi_setup(struct spi_device *spi)
+{
+	if (spi->max_speed_hz < 0) {
+		return -EINVAL;
+	}
+
+	if (!spi->bits_per_word)
+		spi->bits_per_word = 8;
+
+	pr_debug("%s: mode %d, %u bpw, %d hz\n", __FUNCTION__,
+		 spi->mode, spi->bits_per_word, spi->max_speed_hz);
+
+	return 0;
+}
+
+/*!
+ * This function is called when the data has to transfer from/to the
+ * current SPI device in poll mode
+ *
+ * @param        spi        the current spi device
+ * @param        t          the transfer request - read/write buffer pairs
+ *
+ * @return       Returns 0 on success.
+ */
+int mxc_spi_poll_transfer(struct spi_device *spi, struct spi_transfer *t)
+{
+	struct mxc_spi *master_drv_data = NULL;
+	int count, i;
+	volatile unsigned int status;
+	u32 rx_tmp;
+	u32 fifo_size;
+	int chipselect_status;
+
+	mxc_spi_chipselect(spi, BITBANG_CS_ACTIVE);
+
+	/* Get the master controller driver data from spi device's master */
+	master_drv_data = spi_master_get_devdata(spi->master);
+
+	chipselect_status = __raw_readl(MXC_CSPICONFIG +
+					master_drv_data->ctrl_addr);
+	chipselect_status >>= master_drv_data->spi_ver_def->ss_pol_shift &
+	    master_drv_data->spi_ver_def->mode_mask;
+	if (master_drv_data->chipselect_active)
+		master_drv_data->chipselect_active(spi->master->bus_num,
+						   chipselect_status,
+						   (spi->chip_select &
+						    MXC_CSPICTRL_CSMASK) + 1);
+
+	clk_enable(master_drv_data->clk);
+
+	/* Modify the Tx, Rx, Count */
+	master_drv_data->transfer.tx_buf = t->tx_buf;
+	master_drv_data->transfer.rx_buf = t->rx_buf;
+	master_drv_data->transfer.count = t->len;
+	fifo_size = master_drv_data->spi_ver_def->fifo_size;
+
+	count = (t->len > fifo_size) ? fifo_size : t->len;
+	spi_put_tx_data(master_drv_data->base, count, master_drv_data);
+
+	while ((((status = __raw_readl(master_drv_data->test_addr)) &
+		 master_drv_data->spi_ver_def->rx_cnt_mask) >> master_drv_data->
+		spi_ver_def->rx_cnt_off) != count) ;
+
+	for (i = 0; i < count; i++) {
+		rx_tmp = __raw_readl(master_drv_data->base + MXC_CSPIRXDATA);
+		master_drv_data->transfer.rx_get(master_drv_data, rx_tmp);
+	}
+
+	clk_disable(master_drv_data->clk);
+	if (master_drv_data->chipselect_inactive)
+		master_drv_data->chipselect_inactive(spi->master->bus_num,
+						     chipselect_status,
+						     (spi->chip_select &
+						      MXC_CSPICTRL_CSMASK) + 1);
+	return 0;
+}
+
+/*!
+ * This function is called when the data has to transfer from/to the
+ * current SPI device. It enables the Rx interrupt, initiates the transfer.
+ * When Rx interrupt occurs, the completion flag is set. It then disables
+ * the Rx interrupt.
+ *
+ * @param        spi        the current spi device
+ * @param        t          the transfer request - read/write buffer pairs
+ *
+ * @return       Returns 0 on success -1 on failure.
+ */
+int mxc_spi_transfer(struct spi_device *spi, struct spi_transfer *t)
+{
+	struct mxc_spi *master_drv_data = NULL;
+	int count;
+	int chipselect_status;
+	u32 fifo_size;
+
+	/* Get the master controller driver data from spi device's master */
+
+	master_drv_data = spi_master_get_devdata(spi->master);
+
+	chipselect_status = __raw_readl(MXC_CSPICONFIG +
+					master_drv_data->ctrl_addr);
+	chipselect_status >>= master_drv_data->spi_ver_def->ss_pol_shift &
+	    master_drv_data->spi_ver_def->mode_mask;
+	if (master_drv_data->chipselect_active)
+		master_drv_data->chipselect_active(spi->master->bus_num,
+						   chipselect_status,
+						   (spi->chip_select &
+						    MXC_CSPICTRL_CSMASK) + 1);
+
+	clk_enable(master_drv_data->clk);
+	/* Modify the Tx, Rx, Count */
+	master_drv_data->transfer.tx_buf = t->tx_buf;
+	master_drv_data->transfer.rx_buf = t->rx_buf;
+	master_drv_data->transfer.count = t->len;
+	fifo_size = master_drv_data->spi_ver_def->fifo_size;
+	INIT_COMPLETION(master_drv_data->xfer_done);
+
+	/* Enable the Rx Interrupts */
+
+	spi_enable_interrupt(master_drv_data,
+			     1 << (MXC_CSPIINT_RREN_SHIFT +
+				   master_drv_data->spi_ver_def->rx_inten_dif));
+	count = (t->len > fifo_size) ? fifo_size : t->len;
+
+	/* Perform Tx transaction */
+	master_drv_data->transfer.rx_count = count;
+	spi_put_tx_data(master_drv_data->base, count, master_drv_data);
+
+	/* Wait for transfer completion */
+	wait_for_completion(&master_drv_data->xfer_done);
+
+	/* Disable the Rx Interrupts */
+
+	spi_disable_interrupt(master_drv_data,
+			      1 << (MXC_CSPIINT_RREN_SHIFT +
+				    master_drv_data->spi_ver_def->
+				    rx_inten_dif));
+
+	clk_disable(master_drv_data->clk);
+	if (master_drv_data->chipselect_inactive)
+		master_drv_data->chipselect_inactive(spi->master->bus_num,
+						     chipselect_status,
+						     (spi->chip_select &
+						      MXC_CSPICTRL_CSMASK) + 1);
+	return (t->len - master_drv_data->transfer.count);
+}
+
+/*!
+ * This function releases the current SPI device's resources.
+ *
+ * @param        spi     the current SPI device.
+ *
+ */
+void mxc_spi_cleanup(struct spi_device *spi)
+{
+}
+
+/*!
+ * This function is called during the driver binding process. Based on the CSPI
+ * hardware module that is being probed this function adds the appropriate SPI module
+ * structure in the SPI core driver.
+ *
+ * @param   pdev  the device structure used to store device specific
+ *                information that is used by the suspend, resume and remove
+ *                functions.
+ *
+ * @return  The function returns 0 on successful registration and initialization
+ *          of CSPI module. Otherwise returns specific error code.
+ */
+static int mxc_spi_probe(struct platform_device *pdev)
+{
+	struct mxc_spi_master *mxc_platform_info;
+	struct spi_master *master;
+	struct mxc_spi *master_drv_data = NULL;
+	unsigned int spi_ver;
+	int ret = -ENODEV;
+
+	/* Get the platform specific data for this master device */
+
+	mxc_platform_info = (struct mxc_spi_master *)pdev->dev.platform_data;
+	if (!mxc_platform_info) {
+		dev_err(&pdev->dev, "can't get the platform data for CSPI\n");
+		return -EINVAL;
+	}
+
+	/* Allocate SPI master controller */
+
+	master = spi_alloc_master(&pdev->dev, sizeof(struct mxc_spi));
+	if (!master) {
+		dev_err(&pdev->dev, "can't alloc for spi_master\n");
+		return -ENOMEM;
+	}
+
+	/* Set this device's driver data to master */
+
+	platform_set_drvdata(pdev, master);
+
+	/* Set this master's data from platform_info */
+
+	master->bus_num = pdev->id + 1;
+	master->num_chipselect = mxc_platform_info->maxchipselect;
+#ifdef CONFIG_SPI_MXC_TEST_LOOPBACK
+	master->num_chipselect += 1;
+#endif
+	/* Set the master controller driver data for this master */
+
+	master_drv_data = spi_master_get_devdata(master);
+	master_drv_data->mxc_bitbang.master = spi_master_get(master);
+	if (mxc_platform_info->chipselect_active)
+		master_drv_data->chipselect_active =
+		    mxc_platform_info->chipselect_active;
+	if (mxc_platform_info->chipselect_inactive)
+		master_drv_data->chipselect_inactive =
+		    mxc_platform_info->chipselect_inactive;
+
+	/* Identify SPI version */
+
+	spi_ver = mxc_platform_info->spi_version;
+	if (spi_ver == 7) {
+		master_drv_data->spi_ver_def = &spi_ver_0_7;
+	} else if (spi_ver == 5) {
+		master_drv_data->spi_ver_def = &spi_ver_0_5;
+	} else if (spi_ver == 4) {
+		master_drv_data->spi_ver_def = &spi_ver_0_4;
+	} else if (spi_ver == 0) {
+		master_drv_data->spi_ver_def = &spi_ver_0_0;
+	} else if (spi_ver == 23) {
+		master_drv_data->spi_ver_def = &spi_ver_2_3;
+	}
+
+	dev_dbg(&pdev->dev, "SPI_REV 0.%d\n", spi_ver);
+
+	/* Set the master bitbang data */
+
+	master_drv_data->mxc_bitbang.chipselect = mxc_spi_chipselect;
+	master_drv_data->mxc_bitbang.txrx_bufs = mxc_spi_transfer;
+	master_drv_data->mxc_bitbang.master->setup = mxc_spi_setup;
+	master_drv_data->mxc_bitbang.master->cleanup = mxc_spi_cleanup;
+
+	/* Initialize the completion object */
+
+	init_completion(&master_drv_data->xfer_done);
+
+	/* Set the master controller register addresses and irqs */
+
+	master_drv_data->res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!master_drv_data->res) {
+		dev_err(&pdev->dev, "can't get platform resource for CSPI%d\n",
+			master->bus_num);
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	if (!request_mem_region(master_drv_data->res->start,
+				master_drv_data->res->end -
+				master_drv_data->res->start + 1, pdev->name)) {
+		dev_err(&pdev->dev, "request_mem_region failed for CSPI%d\n",
+			master->bus_num);
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	master_drv_data->base = (void *)IO_ADDRESS(master_drv_data->res->start);
+	if (!master_drv_data->base) {
+		dev_err(&pdev->dev, "invalid base address for CSPI%d\n",
+			master->bus_num);
+		ret = -EINVAL;
+		goto err1;
+	}
+
+	master_drv_data->irq = platform_get_irq(pdev, 0);
+	if (master_drv_data->irq < 0) {
+		dev_err(&pdev->dev, "can't get IRQ for CSPI%d\n",
+			master->bus_num);
+		ret = -EINVAL;
+		goto err1;
+	}
+
+	/* Register for SPI Interrupt */
+
+	ret = request_irq(master_drv_data->irq, mxc_spi_isr,
+			  0, "CSPI_IRQ", master_drv_data);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "request_irq failed for CSPI%d\n",
+			master->bus_num);
+		goto err1;
+	}
+
+	/* Setup any GPIO active */
+
+	gpio_spi_active(master->bus_num - 1);
+
+	/* Enable the CSPI Clock, CSPI Module, set as a master */
+
+	master_drv_data->ctrl_addr =
+	    master_drv_data->base + master_drv_data->spi_ver_def->ctrl_reg_addr;
+	master_drv_data->stat_addr =
+	    master_drv_data->base + master_drv_data->spi_ver_def->stat_reg_addr;
+	master_drv_data->period_addr =
+	    master_drv_data->base +
+	    master_drv_data->spi_ver_def->period_reg_addr;
+	master_drv_data->test_addr =
+	    master_drv_data->base + master_drv_data->spi_ver_def->test_reg_addr;
+	master_drv_data->reset_addr =
+	    master_drv_data->base +
+	    master_drv_data->spi_ver_def->reset_reg_addr;
+
+	master_drv_data->clk = clk_get(&pdev->dev, "cspi_clk");
+	clk_enable(master_drv_data->clk);
+	master_drv_data->spi_ipg_clk = clk_get_rate(master_drv_data->clk);
+
+	__raw_writel(master_drv_data->spi_ver_def->reset_start,
+		     master_drv_data->reset_addr);
+	udelay(1);
+	__raw_writel((master_drv_data->spi_ver_def->spi_enable +
+		      master_drv_data->spi_ver_def->master_enable),
+		     master_drv_data->base + MXC_CSPICTRL);
+	__raw_writel(MXC_CSPIPERIOD_32KHZ, master_drv_data->period_addr);
+	__raw_writel(0, MXC_CSPIINT + master_drv_data->ctrl_addr);
+
+	/* Start the SPI Master Controller driver */
+
+	ret = spi_bitbang_start(&master_drv_data->mxc_bitbang);
+
+	if (ret != 0)
+		goto err2;
+
+	printk(KERN_INFO "CSPI: %s-%d probed\n", pdev->name, pdev->id);
+
+#ifdef CONFIG_SPI_MXC_TEST_LOOPBACK
+	{
+		int i;
+		struct spi_board_info *bi = &loopback_info[0];
+		for (i = 0; i < ARRAY_SIZE(loopback_info); i++, bi++) {
+			if (bi->bus_num != master->bus_num)
+				continue;
+
+			dev_info(&pdev->dev,
+				 "registering loopback device '%s'\n",
+				 bi->modalias);
+
+			spi_new_device(master, bi);
+		}
+	}
+#endif
+	clk_disable(master_drv_data->clk);
+	return ret;
+
+      err2:
+	gpio_spi_inactive(master->bus_num - 1);
+	clk_disable(master_drv_data->clk);
+	clk_put(master_drv_data->clk);
+	free_irq(master_drv_data->irq, master_drv_data);
+      err1:
+	release_mem_region(pdev->resource[0].start,
+			   pdev->resource[0].end - pdev->resource[0].start + 1);
+      err:
+	spi_master_put(master);
+	kfree(master);
+	platform_set_drvdata(pdev, NULL);
+	return ret;
+}
+
+/*!
+ * Dissociates the driver from the SPI master controller. Disables the CSPI module.
+ * It handles the release of SPI resources like IRQ, memory,..etc.
+ *
+ * @param   pdev  the device structure used to give information on which SPI
+ *                to remove
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_spi_remove(struct platform_device *pdev)
+{
+	struct spi_master *master = platform_get_drvdata(pdev);
+
+	if (master) {
+		struct mxc_spi *master_drv_data =
+		    spi_master_get_devdata(master);
+
+		gpio_spi_inactive(master->bus_num - 1);
+
+		/* Disable the CSPI module */
+		clk_enable(master_drv_data->clk);
+		__raw_writel(MXC_CSPICTRL_DISABLE,
+			     master_drv_data->base + MXC_CSPICTRL);
+		clk_disable(master_drv_data->clk);
+		/* Unregister for SPI Interrupt */
+
+		free_irq(master_drv_data->irq, master_drv_data);
+
+		release_mem_region(master_drv_data->res->start,
+				   master_drv_data->res->end -
+				   master_drv_data->res->start + 1);
+
+		/* Stop the SPI Master Controller driver */
+
+		spi_bitbang_stop(&master_drv_data->mxc_bitbang);
+
+		spi_master_put(master);
+	}
+
+	printk(KERN_INFO "CSPI: %s-%d removed\n", pdev->name, pdev->id);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int spi_bitbang_suspend(struct spi_bitbang *bitbang)
+{
+	unsigned long flags;
+	unsigned limit = 500;
+
+	spin_lock_irqsave(&bitbang->lock, flags);
+	while (!list_empty(&bitbang->queue) && limit--) {
+		spin_unlock_irqrestore(&bitbang->lock, flags);
+
+		dev_dbg(&bitbang->master->dev, "wait for queue\n");
+		msleep(10);
+
+		spin_lock_irqsave(&bitbang->lock, flags);
+	}
+	if (!list_empty(&bitbang->queue)) {
+		dev_err(&bitbang->master->dev, "queue didn't empty\n");
+		return -EBUSY;
+	}
+	spin_unlock_irqrestore(&bitbang->lock, flags);
+
+	return 0;
+}
+
+static void spi_bitbang_resume(struct spi_bitbang *bitbang)
+{
+	spin_lock_init(&bitbang->lock);
+	INIT_LIST_HEAD(&bitbang->queue);
+
+	bitbang->busy = 0;
+}
+
+/*!
+ * This function puts the SPI master controller in low-power mode/state.
+ *
+ * @param   pdev  the device structure used to give information on which SDHC
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_spi_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct mxc_spi *master_drv_data = spi_master_get_devdata(master);
+	int ret = 0;
+
+	spi_bitbang_suspend(&master_drv_data->mxc_bitbang);
+	clk_enable(master_drv_data->clk);
+	__raw_writel(MXC_CSPICTRL_DISABLE,
+		     master_drv_data->base + MXC_CSPICTRL);
+	clk_disable(master_drv_data->clk);
+	gpio_spi_inactive(master->bus_num - 1);
+
+	return ret;
+}
+
+/*!
+ * This function brings the SPI master controller back from low-power state.
+ *
+ * @param   pdev  the device structure used to give information on which SDHC
+ *                to resume
+ *
+ * @return  The function always returns 0.
+ */
+static int mxc_spi_resume(struct platform_device *pdev)
+{
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct mxc_spi *master_drv_data = spi_master_get_devdata(master);
+
+	gpio_spi_active(master->bus_num - 1);
+
+	spi_bitbang_resume(&master_drv_data->mxc_bitbang);
+	clk_enable(master_drv_data->clk);
+	__raw_writel(master_drv_data->spi_ver_def->spi_enable,
+		     master_drv_data->base + MXC_CSPICTRL);
+	clk_disable(master_drv_data->clk);
+	return 0;
+}
+#else
+#define mxc_spi_suspend  NULL
+#define mxc_spi_resume   NULL
+#endif				/* CONFIG_PM */
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_spi_driver = {
+	.driver = {
+		   .name = "mxc_spi",
+		   .bus = &platform_bus_type,
+		   .owner = THIS_MODULE,
+		   },
+	.probe = mxc_spi_probe,
+	.remove = mxc_spi_remove,
+	.suspend_late = mxc_spi_suspend,
+	.resume_early = mxc_spi_resume,
+};
+
+/*!
+ * This function implements the init function of the SPI device.
+ * It is called when the module is loaded. It enables the required
+ * clocks to CSPI module(if any) and activates necessary GPIO pins.
+ *
+ * @return       This function returns 0.
+ */
+static int __init mxc_spi_init(void)
+{
+	pr_debug("Registering the SPI Controller Driver\n");
+	return platform_driver_register(&mxc_spi_driver);
+}
+
+/*!
+ * This function implements the exit function of the SPI device.
+ * It is called when the module is unloaded. It deactivates the
+ * the GPIO pin associated with CSPI hardware modules.
+ *
+ */
+static void __exit mxc_spi_exit(void)
+{
+	pr_debug("Unregistering the SPI Controller Driver\n");
+	platform_driver_unregister(&mxc_spi_driver);
+}
+
+subsys_initcall(mxc_spi_init);
+module_exit(mxc_spi_exit);
+
+MODULE_DESCRIPTION("SPI Master Controller driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index 3734dc9708e1..de9f5cb36ea4 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -660,7 +660,7 @@ int spi_write_then_read(struct spi_device *spi,
 
 	int			status;
 	struct spi_message	message;
-	struct spi_transfer	x;
+	struct spi_transfer	x[2];
 	u8			*local_buf;
 
 	/* Use preallocated DMA-safe buffer.  We can't avoid copying here,
@@ -671,9 +671,15 @@ int spi_write_then_read(struct spi_device *spi,
 		return -EINVAL;
 
 	spi_message_init(&message);
-	memset(&x, 0, sizeof x);
-	x.len = n_tx + n_rx;
-	spi_message_add_tail(&x, &message);
+	memset(x, 0, sizeof x);
+	if (n_tx) {
+		x[0].len = n_tx;
+		spi_message_add_tail(&x[0], &message);
+	}
+	if (n_rx) {
+		x[1].len = n_rx;
+		spi_message_add_tail(&x[1], &message);
+	}
 
 	/* ... unless someone else is using the pre-allocated buffer */
 	if (!mutex_trylock(&lock)) {
@@ -684,15 +690,15 @@ int spi_write_then_read(struct spi_device *spi,
 		local_buf = buf;
 
 	memcpy(local_buf, txbuf, n_tx);
-	x.tx_buf = local_buf;
-	x.rx_buf = local_buf;
+	x[0].tx_buf = local_buf;
+	x[1].rx_buf = local_buf + n_tx;
 
 	/* do the i/o */
 	status = spi_sync(spi, &message);
 	if (status == 0)
-		memcpy(rxbuf, x.rx_buf + n_tx, n_rx);
+		memcpy(rxbuf, x[1].rx_buf, n_rx);
 
-	if (x.tx_buf == buf)
+	if (x[0].tx_buf == buf)
 		mutex_unlock(&lock);
 	else
 		kfree(local_buf);
diff --git a/drivers/spi/spi_ns921x.c b/drivers/spi/spi_ns921x.c
new file mode 100644
index 000000000000..dedff9b8c369
--- /dev/null
+++ b/drivers/spi/spi_ns921x.c
@@ -0,0 +1,852 @@
+/*
+ * linux/drivers/spi/spi_ns921x.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * All rights reserved.
+ * (C) Copyright 2009, Emerald Electronics Design, LLC,
+ *		Mark Litwack <mlitwack@employees.org>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <linux/spinlock.h>
+//#include <linux/uaccess.h>
+//#include <linux/workqueue.h>
+
+#include <mach/dma.h>
+#include <mach/gpio.h>
+#include <mach/spi.h>
+
+/* registers */
+#define	DMA_IRQSTATUS			(0x0000)
+#define	DMA_RXCTL			(0x0004)
+#define	DMA_RXBUF			(0x0008)
+#define	DMA_RXIRQCFG			(0x000c)
+#define	DMA_TXCTL			(0x0018)
+#define	DMA_TXBUF			(0x001c)
+#define	DMA_TXIRQCFG			(0x0020)
+
+#define SPI_CONFIG			(0x1000)
+#define SPI_CLOCK			(0x1010)
+#define SPI_IRQENABLE			(0x1020)
+#define SPI_IRQSTATUS			(0x1024)
+
+/* DMA config bit fields */
+#define DMA_ENABLECHANNEL		(1 << 31)
+/* DMA interrupt/FIFO status flags */
+#define DMA_IRQSTAT_RXNCIP	(1 << 31)
+#define DMA_IRQSTAT_RXECIP	(1 << 30)
+#define DMA_IRQSTAT_RXNRIP	(1 << 29)
+#define DMA_IRQSTAT_RXCAIP	(1 << 28)
+#define DMA_IRQSTAT_RXPCIP	(1 << 27)
+#define DMA_IRQSTAT_TXNCIP	(1 << 24)
+#define DMA_IRQSTAT_TXECIP	(1 << 23)
+#define DMA_IRQSTAT_TXNRIP	(1 << 22)
+#define DMA_IRQSTAT_TXCAIP	(1 << 21)
+/* DMA interrupt configuration flags */
+#define DMA_IRQCFG_NCIE		(1 << 24)
+#define DMA_IRQCFG_ECIE		(1 << 23)
+#define DMA_IRQCFG_NRIE		(1 << 22)
+#define DMA_IRQCFG_CAIE		(1 << 21)
+#define DMA_IRQCFG_PCIE		(1 << 20)
+#define DMA_IRQCFG_BLENSTAT	(0xFFFF)
+/* SPI config bit fields */
+#define SPI_CONFIG_MASTER		(1 << 0)
+#define SPI_CONFIG_SLAVE		(1 << 1)
+#define SPI_CONFIG_BITORDRMSB		(1 << 2)
+#define SPI_CONFIG_RXBYTE		(1 << 3)
+#define SPI_CONFIG_MODE0		(0x00)
+#define SPI_CONFIG_MODE1		(0x10)
+#define SPI_CONFIG_MODE2		(0x20)
+#define SPI_CONFIG_MODE3		(0x30)
+#define SPI_CLOCK_ENABLE		(1 << 16)
+#define SPI_IRQENABLE_RXIDLE		(1 << 0)
+#define SPI_IRQENABLE_TXIDLE		(1 << 1)
+#define SPI_IRQSTATUS_RXNRIP		(1 << 29)
+#define SPI_IRQSTATUS_TXNRIP		(1 << 22)
+
+/* config masks */
+#define SPI_CONFIG_MODE_MASK		(0x30)
+#define SPI_CONFIG_DISCARD_MASK		(0x700)
+#define SPI_CLOCK_DIVISOR_MASK		(0x3FF)
+
+/* config shifts */
+#define SPI_CONFIG_MODE_SHIFT		(4)
+#define SPI_CONFIG_DISCARD_SHIFT	(8)
+#define SPI_CLOCK_DIVISOR_SHIFT		(0)
+
+#define SPI_IDLE_LEVEL			0xff
+#define MAX_RATE			33330000
+#define	MAX_BUFFER_SIZE			65532
+
+#define	SPI_CS_ACTIVE		1
+#define	SPI_CS_INACTIVE		0
+
+#define DRIVER_NAME			"spi_ns921x"
+
+union ns9xxx_dma_desc {
+	struct {
+		u32 source;
+		u16 len;
+		u16 reserved1;
+		u32 reserved2;
+
+		u16 status;
+#define DMADESC_WRAP	(1 << 15)
+#define DMADESC_INTR	(1 << 14)
+#define DMATXDESC_LAST	(1 << 13)
+#define DMADESC_FULL	(1 << 12)
+		u16 flags;
+	};
+	u32 data[4];
+};
+
+struct spi_ns921x {
+	void __iomem		*ioaddr;	/* SPI DMA base address */
+	struct resource		*mem;
+	struct clk		*clk;
+
+//	struct workqueue_struct	*workqueue;
+	struct work_struct	work;
+	spinlock_t		lock;
+	struct list_head	queue;
+	struct spi_transfer	*current_transfer;
+	unsigned long		remaining_bytes;
+	unsigned		dma_xfer_len;
+	u32			flags;
+	wait_queue_head_t	waitq;
+
+	int			irq;
+	unsigned int		speed;
+
+/*	union ns9xxx_dma_desc	*rxdesc;
+	union ns9xxx_dma_desc	*txdesc;
+	dma_addr_t		rxdesc_real;
+	dma_addr_t		txdesc_real;
+*/
+	void			*buf_tx;
+	void			*buf_rx;
+	dma_addr_t		buf_tx_dma;
+	dma_addr_t		buf_rx_dma;
+	union ns9xxx_dma_desc	*desc_tx;
+	union ns9xxx_dma_desc	*desc_rx;
+	dma_addr_t		desc_tx_dma;
+	dma_addr_t		desc_rx_dma;
+
+	struct spi_ns9xxx_data	*pdata;
+	//struct tasklet_struct   xmit_tasklet;
+};
+
+#define BUFFER_SIZE		PAGE_SIZE
+#define INVALID_DMA_ADDRESS	0xffffffff
+#define SPI_RX_IDONE		0x00000001
+#define SPI_TX_IDONE		0x00000002
+
+static void spi_ns921x_chipsel(struct spi_device *spi, int active)
+{
+unsigned int cspol = spi->mode & SPI_CS_HIGH ? 1 : 0;
+void (* func)(struct spi_device *spi, int active) = spi->controller_data;
+struct spi_ns921x *info = spi_master_get_devdata(spi->master);
+
+	/* use special function to toggle CS? */
+	if( func != NULL ) {
+		/* Reconfigure CS line as GPIO input */
+		if( SPI_CS_ACTIVE == active )
+			gpio_configure_ns921x(info->pdata->gpios[SPI_EN_GPIO_OFFSET],
+						NS921X_GPIO_INPUT,
+						NS921X_GPIO_DONT_INVERT,
+						NS921X_GPIO_FUNC_3,
+						NS921X_GPIO_ENABLE_PULLUP );
+
+		/* Call the special function */
+		func( spi, cspol == active);
+
+		/* Reconfigure CS line as SPI_EN */
+		if( SPI_CS_INACTIVE == active )
+			gpio_configure_ns921x(info->pdata->gpios[SPI_EN_GPIO_OFFSET],
+						0,
+						0,
+						info->pdata->gpio_funcs[SPI_EN_GPIO_OFFSET],
+						0);
+	}
+	else {
+		/* Use a GPIO to toggle CS? */
+		if( info->pdata->gpio_funcs[SPI_EN_GPIO_OFFSET] == NS921X_GPIO_FUNC_GPIO )
+			gpio_set_value( info->pdata->gpios[SPI_EN_GPIO_OFFSET], cspol == active );
+		/* otherwise CS will be automatically controlled by the spi engine */
+	}
+}
+
+/*
+ * Configure the controller for the passed SPI-device. Additionally it will
+ * use the configuration values of the SPI-transfer if it's different than NULL
+ */
+static int spi_ns921x_setupxfer(struct spi_device *spi,
+				 struct spi_transfer *t)
+{
+	struct spi_ns921x *info = spi_master_get_devdata(spi->master);
+	unsigned int div,reg;
+	unsigned int bpw;
+	unsigned int hz;
+
+	pr_debug("Setup for next xfer called\n");
+
+	/* If the bits per word is zero, then use the default value of the SPI-device */
+	bpw = t ? t->bits_per_word : spi->bits_per_word;
+	if (!bpw)
+		bpw = spi->bits_per_word;
+
+	/* We only support eight bits per word */
+	if (bpw != 8) {
+		pr_err("Invalid bits-per-word (%d)\n", bpw);
+		return -EINVAL;
+	}
+
+	/*
+	 * If the clock rate of the xfer is zero, then use the already configured
+	 * rate of this SPI-device
+	 */
+	hz  = t ? t->speed_hz : spi->max_speed_hz;
+	if (!hz)
+		hz = spi->max_speed_hz;
+
+	if( hz > MAX_RATE ) {
+		pr_err("Invalid speed (%d). Max is %d\n", hz, spi->max_speed_hz );
+		return -EINVAL;
+	}
+
+	/* calculate divisor */
+	div = (clk_get_rate(info->clk) / hz + 1);
+	if(div > SPI_CLOCK_DIVISOR_MASK) {
+		pr_err("Invalid speed (%d). Min is %ld\n",
+			hz,
+			clk_get_rate(info->clk)/(SPI_CLOCK_DIVISOR_MASK-1));
+		return -EINVAL;
+	}
+
+	/* configure clock divisor (3 steps, see hardware reference) */
+	/* step1: unset SPI_CLOCK_ENABLE, don't touch other bits */
+	reg = ioread32(info->ioaddr + SPI_CLOCK);
+	iowrite32(reg & ~SPI_CLOCK_ENABLE, info->ioaddr + SPI_CLOCK);
+
+	reg = div & (SPI_CLOCK_DIVISOR_MASK << SPI_CLOCK_DIVISOR_SHIFT);
+	iowrite32(reg, info->ioaddr + SPI_CLOCK);
+
+	/* step3: set SPI_CLOCK_ENABLE, don't touch other bits */
+	iowrite32(reg | SPI_CLOCK_ENABLE, info->ioaddr + SPI_CLOCK);
+
+	return 0;
+}
+
+static int spi_ns921x_setup(struct spi_device *spi)
+{
+	struct spi_ns921x *info = spi_master_get_devdata(spi->master);
+	unsigned int reg;
+	int ret;
+
+	if (spi->chip_select > spi->master->num_chipselect) {
+		dev_dbg(&spi->dev,
+				"setup: invalid chipselect %u (%u defined)\n",
+				spi->chip_select, spi->master->num_chipselect);
+		return -EINVAL;
+	}
+
+	if (!spi->bits_per_word)
+		spi->bits_per_word = 8;
+
+	/* configure SPI controller */
+	reg = SPI_CONFIG_MASTER;
+
+	if (!(spi->mode & SPI_LSB_FIRST))
+		reg |= SPI_CONFIG_BITORDRMSB;
+
+	if (spi->mode & SPI_CPOL) {
+		if (spi->mode & SPI_CPHA)
+			reg |= SPI_CONFIG_MODE3;
+		else
+			reg |= SPI_CONFIG_MODE2;
+	} else if (spi->mode & SPI_CPHA)
+		reg |= SPI_CONFIG_MODE1;
+	else
+		reg |= SPI_CONFIG_MODE0;
+
+	iowrite32(reg, info->ioaddr + SPI_CONFIG);
+
+	ret = spi_ns921x_setupxfer(spi, NULL);
+	if (!ret)
+		pr_debug("Mode %d, %u bpw, %d HZ\n",
+			     spi->mode, spi->bits_per_word,
+			     spi->max_speed_hz);
+
+	return ret;
+}
+
+static void spi_ns921x_map_xfer(struct spi_device *spi,
+		struct spi_transfer *xfer)
+{
+	xfer->tx_dma = xfer->rx_dma = INVALID_DMA_ADDRESS;
+	/* TODO: Check this DMA mapping
+	if (!(xfer->len & (L1_CACHE_BYTES - 1))) {
+		if (xfer->tx_buf && !((unsigned long)xfer->tx_buf &
+					(L1_CACHE_BYTES - 1)))
+			xfer->tx_dma = dma_map_single(&spi->dev, (void *)xfer->tx_buf,
+						xfer->len, DMA_TO_DEVICE);
+		if (xfer->rx_buf && !((unsigned long)xfer->rx_buf &
+					(L1_CACHE_BYTES - 1)))
+			xfer->rx_dma = dma_map_single(&spi->dev, xfer->rx_buf,
+						xfer->len, DMA_FROM_DEVICE);
+	}
+	*/
+}
+
+static void spi_ns921x_next_xfer(struct spi_master *master,
+		struct spi_message *msg)
+{
+	struct spi_ns921x *info = spi_master_get_devdata(master);
+	struct spi_transfer *xfer;
+	dma_addr_t tx_dma, rx_dma;
+	u32 len;
+	int err;
+
+	xfer = info->current_transfer;
+	if (!xfer || info->remaining_bytes == 0) {
+		if (xfer)
+			xfer = list_entry(xfer->transfer_list.next,
+					struct spi_transfer, transfer_list);
+		else
+			xfer = list_entry(msg->transfers.next,
+					struct spi_transfer, transfer_list);
+		info->remaining_bytes = xfer->len;
+		info->current_transfer = xfer;
+	}
+
+	len = info->remaining_bytes;
+
+	/* Set the requested configuration for this new SPI-transfer */
+	err = spi_ns921x_setupxfer(msg->spi, xfer);
+	if (err) {
+		pr_err("Setting up the next SPI-transfer\n");
+		list_del(&msg->queue);
+		msg->status = err;
+
+		spin_unlock(&info->lock);
+		msg->complete(msg->context);
+		spin_lock(&info->lock);
+
+		info->current_transfer = NULL;
+		return;
+	}
+
+	tx_dma = xfer->tx_dma;
+	rx_dma = xfer->rx_dma;
+
+	/* Some drivers (like mmc_spi) set rx_dma=0 if they
+	 * don't need any return data.  If they do this, we
+	 * put the unwanted rx data in rx_dma and hope they
+	 * didn't ask to send more than BUFFER_SIZE (4096)
+	 * bytes.  This is at least better than trashing
+	 * page 0.
+	 *
+	 * - mwl
+	 */
+	if (!rx_dma || rx_dma == INVALID_DMA_ADDRESS) {
+		rx_dma = info->buf_rx_dma;
+		if (len > BUFFER_SIZE)
+			len = BUFFER_SIZE;
+	}
+	if (tx_dma == INVALID_DMA_ADDRESS) {
+		if (xfer->tx_buf) {
+			tx_dma = info->buf_tx_dma;
+			if (len > BUFFER_SIZE)
+				len = BUFFER_SIZE;
+			memcpy(info->buf_tx, xfer->tx_buf +
+					xfer->len - info->remaining_bytes, len);
+			dma_sync_single_for_device(&master->dev,
+					info->buf_tx_dma, len, DMA_TO_DEVICE);
+		} else {
+			/* Send undefined data; rx_dma is handy */
+			tx_dma = rx_dma;
+		}
+	}
+
+	/* The IO hub DMA controller doesn't cope well with
+	 * non-aligned rx buffers (and possibly tx too).  On
+	 * rx, at least, it causes the last byte read by the
+	 * peripheral on the *previous* transfer to be
+	 * delivered again if the current transfer is
+	 * unaligned.
+	 *
+	 * For now, just warn about it so someone can fix
+	 * the offending caller when it occurs.  It might
+	 * not be a problem on tx.
+	 *
+	 * - mwl
+	 */
+	if (rx_dma & 0x3) {
+		dev_err(&master->dev, "unaligned rx_dma (=0x%08x, len=%u)\n",
+			rx_dma, len);
+	}
+	if (tx_dma & 0x3) {
+		dev_err(&master->dev, "unaligned tx_dma (=0x%08x, len=%u)\n",
+			tx_dma, len);
+	}
+
+	/* Setup the DMA channels */
+	/* reset dma controller */
+	iowrite32(0, info->ioaddr + DMA_RXCTL);
+	iowrite32(0, info->ioaddr + DMA_TXCTL);
+	/* set DMA descriptor */
+	memset(info->desc_tx, 0, sizeof(*info->desc_tx));
+	info->desc_tx->source = (u32)(tx_dma);
+	info->desc_tx->len = len;
+	info->desc_tx->flags =
+		DMADESC_WRAP | DMADESC_INTR | DMATXDESC_LAST | DMADESC_FULL;
+
+	memset(info->desc_rx, 0, sizeof(*info->desc_rx));
+	info->desc_rx->source = (u32)(rx_dma);
+	info->desc_rx->len = len;
+	info->desc_rx->flags = DMADESC_WRAP;
+
+	info->dma_xfer_len = len;
+
+	/* configure dma channels */
+	iowrite32( DMA_IRQCFG_NCIE | DMA_IRQCFG_ECIE |
+		DMA_IRQCFG_NRIE | DMA_IRQCFG_CAIE | DMA_IRQCFG_PCIE,
+		info->ioaddr + DMA_RXIRQCFG );
+	iowrite32((u32)info->desc_tx_dma, info->ioaddr + DMA_TXBUF);
+	iowrite32( DMA_IRQCFG_NCIE | DMA_IRQCFG_ECIE |
+		DMA_IRQCFG_NRIE | DMA_IRQCFG_CAIE,
+		info->ioaddr + DMA_TXIRQCFG );
+	iowrite32((u32)info->desc_rx_dma, info->ioaddr + DMA_RXBUF);
+
+	/* fire up dma channels */
+	iowrite32(DMA_ENABLECHANNEL, info->ioaddr + DMA_RXCTL);
+	iowrite32(DMA_ENABLECHANNEL, info->ioaddr + DMA_TXCTL);
+}
+
+static void spi_ns921x_next_message(struct spi_master *master)
+{
+	struct spi_ns921x *info = spi_master_get_devdata(master);
+	struct spi_message *msg;
+
+	BUG_ON(info->current_transfer);
+
+	msg = list_entry(info->queue.next, struct spi_message, queue);
+
+	spi_ns921x_chipsel(msg->spi, SPI_CS_ACTIVE);
+	spi_ns921x_next_xfer(master, msg);
+}
+
+static int spi_ns921x_transfer(struct spi_device *spi, struct spi_message *msg)
+{
+	struct spi_ns921x *info = spi_master_get_devdata(spi->master);
+	struct spi_transfer *xfer;
+	unsigned long flags;
+	if (unlikely(list_empty(&msg->transfers)
+			|| !spi->max_speed_hz))
+		return -EINVAL;
+
+	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+		if (!(xfer->tx_buf || xfer->rx_buf)) {
+			dev_dbg(&spi->dev, "missing rx or tx buf\n");
+			return -EINVAL;
+		}
+	}
+
+	/* scrub dcache "early" */
+	if (!msg->is_dma_mapped) {
+		list_for_each_entry(xfer, &msg->transfers, transfer_list)
+			spi_ns921x_map_xfer(spi, xfer);
+	}
+
+	msg->status = -EINPROGRESS;
+	msg->actual_length = 0;
+
+	spin_lock_irqsave(&info->lock, flags);
+	list_add_tail(&msg->queue, &info->queue);
+	if (!info->current_transfer)
+		spi_ns921x_next_message(spi->master);
+	spin_unlock_irqrestore(&info->lock, flags);
+
+	return 0;
+}
+
+static void spi_ns921x_irq_trigger_next(struct spi_master *master)
+{
+	struct spi_ns921x *info = spi_master_get_devdata(master);
+	struct spi_message *msg;
+	struct spi_transfer *xfer;
+	u32 len, ctrl;
+
+	BUG_ON((info->flags & (SPI_TX_IDONE|SPI_RX_IDONE)) != (SPI_TX_IDONE|SPI_RX_IDONE));
+
+	info->flags &= ~(SPI_TX_IDONE|SPI_RX_IDONE);
+	xfer = info->current_transfer;
+	msg = list_entry(info->queue.next, struct spi_message, queue);
+
+	len = info->dma_xfer_len;
+	if (len > BUFFER_SIZE)
+		len = BUFFER_SIZE;
+	/*
+	 * If the rx buffer wasn't aligned, we used a bounce
+	 * buffer for the transfer. Copy the data back and
+	 * make the bounce buffer ready for re-use.
+	 */
+	if (xfer->rx_buf && xfer->rx_dma == INVALID_DMA_ADDRESS) {
+		dma_sync_single_for_cpu(&master->dev, info->buf_rx_dma,
+					len, DMA_FROM_DEVICE);
+		memcpy(xfer->rx_buf, info->buf_rx + xfer->len -
+				info->remaining_bytes, len);
+	}
+
+	info->remaining_bytes -= len;
+
+	if (info->remaining_bytes == 0) {
+		msg->actual_length += xfer->len;
+
+		if (!msg->is_dma_mapped) {
+			if (xfer->tx_dma != INVALID_DMA_ADDRESS)
+				dma_unmap_single(&master->dev,
+						 xfer->tx_dma,
+						 xfer->len,
+						 DMA_TO_DEVICE);
+			if (xfer->rx_dma != INVALID_DMA_ADDRESS)
+				dma_unmap_single(&master->dev,
+						 xfer->rx_dma,
+						 xfer->len,
+						 DMA_FROM_DEVICE);
+		}
+
+		/* REVISIT: udelay in irq is unfriendly */
+		if (xfer->delay_usecs)
+			udelay(xfer->delay_usecs);
+
+		if (msg->transfers.prev == &xfer->transfer_list) {
+
+			/* Message complete */
+			spi_ns921x_chipsel(msg->spi, SPI_CS_INACTIVE);
+
+			list_del(&msg->queue);
+			msg->status = 0;
+
+			spin_unlock(&info->lock);
+			msg->complete(msg->context);
+			spin_lock(&info->lock);
+
+			info->current_transfer = NULL;
+
+			/* continue; complete() may have queued requests */
+			if (list_empty(&info->queue)) {
+				ctrl = ioread32(info->ioaddr +
+						DMA_RXCTL);
+				ctrl &= ~DMA_ENABLECHANNEL;
+				iowrite32(ctrl, info->ioaddr +
+						DMA_RXCTL);
+				ctrl = ioread32(info->ioaddr +
+						DMA_TXCTL);
+				ctrl &= ~DMA_ENABLECHANNEL;
+				iowrite32(ctrl, info->ioaddr +
+						DMA_TXCTL);
+			} else
+				spi_ns921x_next_message(master);
+		} else {
+			if (xfer->cs_change) {
+				spi_ns921x_chipsel(msg->spi, SPI_CS_INACTIVE);
+				udelay(1);
+				spi_ns921x_chipsel(msg->spi, SPI_CS_ACTIVE);
+			}
+
+			spi_ns921x_next_xfer(master, msg);
+		}
+	} else {
+		/* There are still pending data in the current transfer */
+		spi_ns921x_next_xfer(master, msg);
+	}
+}
+
+static irqreturn_t spi_ns921x_irq(int irq, void *dev_id)
+{
+	struct spi_master *master = dev_id;
+	struct spi_ns921x *info = spi_master_get_devdata(master);
+	u32 status, dlen, rx_status;
+
+	spin_lock(&info->lock);
+
+	/* get status and mask all pending interrupts */
+	status = ioread32(info->ioaddr + DMA_IRQSTATUS);
+	iowrite32(status, info->ioaddr + DMA_IRQSTATUS);
+
+	/* TX interrupt error conditions */
+	if (status & (DMA_IRQSTAT_TXECIP | DMA_IRQSTAT_TXCAIP )) {
+		dev_err(&master->dev, "tx dma failure! status = 0x%08x\n",
+				status);
+		/* retrigger the xfer */
+		spi_ns921x_next_xfer(master, list_entry(info->queue.next,
+					struct spi_message, queue));
+	}
+	/* RX interrupt error conditions */
+	rx_status = ioread32(info->ioaddr + DMA_RXIRQCFG);
+	dlen = rx_status & DMA_IRQCFG_BLENSTAT;
+
+	if (info->dma_xfer_len != dlen)
+		/* ?? what to do... ?? */
+		dev_err(&master->dev, "incomplete dma xfer"
+				"(%d/%d bytes transfered) \n",
+				dlen, info->dma_xfer_len);
+
+	if (status & (DMA_IRQSTAT_RXECIP | DMA_IRQSTAT_RXCAIP |
+				DMA_IRQSTAT_RXPCIP)) {
+		dev_err(&master->dev, "rx dma failure! status = 0x%08x\n",
+				status);
+
+		/* retrigger the xfer */
+		spi_ns921x_next_xfer(master, list_entry(info->queue.next,
+					struct spi_message, queue));
+	}
+	/* TX normal completion */
+	if (status & DMA_IRQSTAT_TXNCIP) {
+		info->flags |= SPI_TX_IDONE;
+		if (info->flags & SPI_RX_IDONE)
+			spi_ns921x_irq_trigger_next(master);
+	}
+	/* RX normal completion */
+	if (status & DMA_IRQSTAT_RXNCIP) {
+		info->flags |= SPI_RX_IDONE;
+		if (info->flags & SPI_TX_IDONE)
+			spi_ns921x_irq_trigger_next(master);
+	}
+	spin_unlock(&info->lock);
+
+	return IRQ_HANDLED;
+}
+
+static __devinit int spi_ns921x_probe(struct platform_device *pdev)
+{
+	struct spi_master *master;
+	struct spi_ns921x *info;
+	int ret;
+	int gpio_cs, gpio_cs_func;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*info));
+	if (!master) {
+		pr_debug(DRIVER_NAME ": cannot allocate spi master\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, master);
+
+	info = spi_master_get_devdata(master);
+	info->pdata = pdev->dev.platform_data;
+	info->irq = platform_get_irq(pdev, 0);
+	if (info->irq < 0) {
+		pr_debug(DRIVER_NAME ": no interrupt available\n");
+		ret = -ENOENT;
+		goto err_res;
+	}
+
+	info->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!info->mem) {
+		pr_debug(DRIVER_NAME ": memory not available\n");
+		ret = -ENOENT;
+		goto err_res;
+	}
+
+	if (!request_mem_region(info->mem->start,
+			info->mem->end - info->mem->start + 1,
+			DRIVER_NAME)) {
+		pr_debug(DRIVER_NAME ": memory already mapped\n");
+		ret = -EIO;
+		goto err_res;
+	}
+
+	info->ioaddr = ioremap(info->mem->start,
+			info->mem->end - info->mem->start + 1);
+	if (!info->ioaddr) {
+		pr_debug(DRIVER_NAME ":  unable to remap IO memory\n");
+		ret = -EIO;
+		goto err_remap;
+	}
+
+	master->bus_num = pdev->id;
+	/* hardware controlled cs */
+	master->num_chipselect = 1;
+
+	/* If CS is GPIO controlled, configure it as output */
+	gpio_cs = info->pdata->gpios[SPI_EN_GPIO_OFFSET];
+	gpio_cs_func = info->pdata->gpio_funcs[SPI_EN_GPIO_OFFSET];
+	if( NS921X_GPIO_FUNC_GPIO == gpio_cs_func )
+		gpio_direction_output( gpio_cs,
+				gpio_get_value( gpio_cs ) );
+
+	master->setup = spi_ns921x_setup;
+	master->transfer = spi_ns921x_transfer;
+
+	/* prepare waitq and spinlock */
+	spin_lock_init(&info->lock);
+	INIT_LIST_HEAD(&info->queue);
+	init_waitqueue_head(&info->waitq);
+
+	/* register and map memory for dma-descriptors */
+	info->buf_tx = dma_alloc_coherent(&pdev->dev, BUFFER_SIZE,
+					&info->buf_tx_dma, GFP_KERNEL);
+	if (!info->buf_tx) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc_buf_tx\n", __func__);
+		goto err_alloc_buf_tx;
+	}
+
+	info->buf_rx = dma_alloc_coherent(&pdev->dev, BUFFER_SIZE,
+					&info->buf_rx_dma, GFP_KERNEL);
+	if (!info->buf_rx) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc_buf_rx\n", __func__);
+		goto err_alloc_buf_rx;
+	}
+
+	info->desc_tx = dma_alloc_coherent(&pdev->dev,
+			sizeof(info->desc_tx), &info->desc_tx_dma, GFP_DMA);
+	if (!info->desc_tx) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc_desc_tx\n", __func__);
+		goto err_alloc_desc_tx;
+	}
+
+	info->desc_rx = dma_alloc_coherent(&pdev->dev,
+			sizeof(info->desc_rx), &info->desc_rx_dma, GFP_DMA);
+	if (!info->desc_rx) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc_desc_rx\n", __func__);
+		goto err_alloc_desc_rx;
+	}
+
+	info->clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(info->clk)) {
+		pr_debug(DRIVER_NAME ": no clock available\n");
+		ret = PTR_ERR(info->clk);
+		goto err_clk;
+	}
+
+	ret = clk_enable(info->clk);
+	if (ret) {
+		pr_debug(DRIVER_NAME ": cannot enable clock\n");
+		goto err_clk;
+	}
+
+	/* reset dma controller */
+	iowrite32(0, info->ioaddr + DMA_RXCTL);
+	iowrite32(0, info->ioaddr + DMA_TXCTL);
+
+	/* register interrupt service routine */
+	ret = request_irq(info->irq, spi_ns921x_irq, 0,
+			pdev->name, master);
+	if (ret) {
+		pr_debug(DRIVER_NAME ": cannot register SPI interrupt\n");
+		goto err_irq;
+	}
+
+	dev_info(&pdev->dev, "NS921x SPI controller at 0x%p (irq: %d)\n",
+			info->ioaddr, info->irq );
+
+	ret = spi_register_master(master);
+	if (ret) {
+		pr_debug(DRIVER_NAME ": cannot register spi master\n");
+		goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+	//destroy_workqueue(info->workqueue);
+//err_workqueue:
+	free_irq(info->irq, info);
+err_irq:
+	clk_disable(info->clk);
+err_clk:
+	dma_free_coherent(&pdev->dev, sizeof(info->desc_rx),
+			info->desc_rx, info->desc_rx_dma);
+err_alloc_desc_rx:
+	dma_free_coherent(&pdev->dev, sizeof(info->desc_tx),
+			info->desc_tx, info->desc_tx_dma);
+err_alloc_desc_tx:
+	dma_free_coherent(&pdev->dev, BUFFER_SIZE,
+			info->buf_rx, info->buf_rx_dma);
+err_alloc_buf_rx:
+	dma_free_coherent(&pdev->dev, BUFFER_SIZE,
+			info->buf_tx, info->buf_tx_dma);
+err_alloc_buf_tx:
+	iounmap(info->ioaddr);
+
+err_remap:
+	release_mem_region(info->mem->start,
+			info->mem->end - info->mem->start + 1);
+err_res:
+	spi_master_put(master);
+
+	return ret;
+}
+
+static __devexit int spi_ns921x_remove(struct platform_device *pdev)
+{
+	struct spi_master *master = spi_master_get(platform_get_drvdata(pdev));
+	struct spi_ns921x *info = spi_master_get_devdata(master);
+
+	spi_unregister_master(master);
+//	destroy_workqueue(info->workqueue);
+	free_irq(info->irq, info);
+	clk_disable(info->clk);
+	dma_free_coherent(&pdev->dev, sizeof(info->desc_rx),
+			info->desc_rx, info->desc_rx_dma);
+	dma_free_coherent(&pdev->dev, sizeof(info->desc_tx),
+			info->desc_tx, info->desc_tx_dma);
+	dma_free_coherent(&pdev->dev, BUFFER_SIZE,
+			info->buf_rx, info->buf_rx_dma);
+	dma_free_coherent(&pdev->dev, BUFFER_SIZE,
+			info->buf_tx, info->buf_tx_dma);
+	iounmap(info->ioaddr);
+	release_mem_region(info->mem->start,
+			info->mem->end - info->mem->start + 1);
+	spi_master_put(master);
+
+	return 0;
+}
+
+static struct platform_driver spi_ns9xxx_driver = {
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= spi_ns921x_probe,
+	.remove		= __devexit_p(spi_ns921x_remove),
+};
+
+static __init int spi_ns921x_init(void)
+{
+	return platform_driver_register(&spi_ns9xxx_driver);
+}
+
+static __exit void spi_ns921x_exit(void)
+{
+	platform_driver_unregister(&spi_ns9xxx_driver);
+}
+
+module_init(spi_ns921x_init);
+module_exit(spi_ns921x_exit);
+
+MODULE_AUTHOR("Digi International Inc.");
+MODULE_DESCRIPTION("Digi ns921x SPI Controller driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/spi/spi_ns9360.c b/drivers/spi/spi_ns9360.c
new file mode 100644
index 000000000000..8dde47409f3a
--- /dev/null
+++ b/drivers/spi/spi_ns9360.c
@@ -0,0 +1,890 @@
+/*
+ * linux/drivers/spi/spi_ns9360.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <linux/spinlock.h>
+
+#include <asm/dma.h>
+#include <mach/gpio.h>
+#include <mach/spi.h>
+
+#define DRIVER_NAME	"spi_ns9360"
+
+union ns9xxx_dma_desc {
+	struct {
+		u32 source;
+		u16 len;
+		u16 reserved1;
+		u32 reserved2;
+
+		u16 status;
+#define DMADESC_WRAP	(1 << 15)
+#define DMADESC_INTR	(1 << 14)
+#define DMATXDESC_LAST	(1 << 13)
+#define DMADESC_FULL	(1 << 12)
+		u16 flags;
+	};
+	u32 data[4];
+};
+
+struct spi_ns9360 {
+	void __iomem		*ioaddr;
+	void __iomem		*ioaddr_bbus;
+	struct resource		*mem;
+	struct resource		*mem_bbus;
+	struct clk		*clk;
+	struct spi_ns9xxx_data	*pdata;
+	unsigned		irq_tx;
+	unsigned		irq_rx;
+
+	spinlock_t		lock;
+	struct list_head	queue;
+	struct spi_transfer	*current_transfer;
+	unsigned long		remaining_bytes;
+	unsigned		dma_xfer_len;
+	u32			flags;
+
+	void			*buf_tx;
+	void			*buf_rx;
+	dma_addr_t		buf_tx_dma;
+	dma_addr_t		buf_rx_dma;
+	union ns9xxx_dma_desc	*desc_tx;
+	union ns9xxx_dma_desc	*desc_rx;
+	dma_addr_t		desc_tx_dma;
+	dma_addr_t		desc_rx_dma;
+};
+
+#define BUFFER_SIZE		PAGE_SIZE
+#define INVALID_DMA_ADDRESS	0xffffffff
+#define SPI_RX_IDONE		0x00000001
+#define SPI_TX_IDONE		0x00000002
+#define	SPI_DEFAULT_BPS		1000000
+#define	SPI_CS_ACTIVE		1
+#define	SPI_CS_INACTIVE		0
+
+#define BBUS_DMA_RXBUFP	0x00
+#define BBUS_DMA_TXBUFP	0x20
+#define BBUS_DMA_RXCTRL	0x10
+#define BBUS_DMA_TXCTRL	0x30
+#define BBUS_DMA_CTRL_CE	(1 << 31)
+#define BBUS_DMA_CTRL_MODE_FBR	(0x01 << 26)
+#define BBUS_DMA_CTRL_MODE_FBW	(0x00 << 26)
+#define BBUS_DMA_CTRL_RESET	(1 << 18)
+#define BBUS_DMA_CTRL_BTE1	0
+
+#define BBUS_DMA_RXSTAT	0x14
+#define BBUS_DMA_TXSTAT	0x34
+#define BBUS_DMA_STAT_NCIP	(1 << 31)
+#define BBUS_DMA_STAT_ECIP	(1 << 30)
+#define BBUS_DMA_STAT_CAIP	(1 << 28)
+#define BBUS_DMA_STAT_PCIP	(1 << 27)
+#define BBUS_DMA_STAT_NCIE	(1 << 24)
+#define BBUS_DMA_STAT_ECIE	(1 << 23)
+#define BBUS_DMA_STAT_NRIE	(1 << 22)
+#define BBUS_DMA_STAT_CAIE	(1 << 21)
+#define BBUS_DMA_STAT_PCIE	(1 << 20)
+#define BBUS_DMA_STAT_BLEN_MA	(0x0000FFFF)
+
+#define SPI_CTRLA	0x00
+#define SPI_CTRLA_CE		(1 << 31)
+#define SPI_CTRLA_WLS8		(3 << 24)
+#define SPI_CTRLA_ERXDMA	(1 << 8)
+#define SPI_CTRLA_ETXDMA	(1 << 0)
+
+#define SPI_CTRLB	0x04
+#define SPI_CTRLB_MODE_SPIM	(1 << 21)
+#define SPI_CTRLB_BITORDR	(1 << 19)
+#define SPI_BITRATE	0x0C
+#define SPI_BITRATE_EBIT	(1 << 31)
+#define SPI_BITRATE_TMODE	(1 << 30)
+#define SPI_BITRATE_CLKMUX	(0x01 << 24)
+#define SPI_BITRATE_TXCINV	(1 << 23)
+#define SPI_BITRATE_RXCINV	(1 << 22)
+#define SPI_BITRATE_SPCPOL_LOW	(1 << 21)
+#define SER_BITRATE_N_MA	(0x00007FFF)
+
+#define	SPI_DMA_TX_STAT_OPT \
+	(BBUS_DMA_STAT_NCIE | BBUS_DMA_STAT_ECIE | BBUS_DMA_STAT_NRIE | \
+	 BBUS_DMA_STAT_CAIE | BBUS_DMA_STAT_PCIE)
+#define	SPI_DMA_RX_STAT_OPT \
+	(BBUS_DMA_STAT_NCIE | BBUS_DMA_STAT_ECIE | BBUS_DMA_STAT_NRIE | \
+	 BBUS_DMA_STAT_CAIE | BBUS_DMA_STAT_PCIE)
+#define	SPI_DMA_TX_CTRL_OPT \
+	(BBUS_DMA_CTRL_MODE_FBR | BBUS_DMA_CTRL_BTE1 | BBUS_DMA_CTRL_CE)
+#define	SPI_DMA_RX_CTRL_OPT \
+	(BBUS_DMA_CTRL_MODE_FBW | BBUS_DMA_CTRL_BTE1 | BBUS_DMA_CTRL_CE)
+
+static void spi_ns9360_chipsel(struct spi_device *spi, int active)
+{
+unsigned int cspol = spi->mode & SPI_CS_HIGH ? 1 : 0;
+void (* func)(struct spi_device *spi, int active) = spi->controller_data;
+struct spi_ns9360 *info = spi_master_get_devdata(spi->master);
+
+	/* use special function to toggle CS? */
+	if( func )
+	{
+		/* Reconfigure CS line as GPIO input */
+		if( SPI_CS_ACTIVE == active )
+			gpio_configure_ns9360(info->pdata->gpios[SPI_EN_GPIO_OFFSET],
+						NS9360_GPIO_INPUT,
+						NS9360_GPIO_DONT_INVERT,
+						NS9360_GPIO_FUNC_3);
+
+		/* Call the special function */
+		func( spi, cspol == active);
+
+		/* Reconfigure CS line as SPI_EN */
+		if( SPI_CS_INACTIVE == active )
+			gpio_configure_ns9360(info->pdata->gpios[SPI_EN_GPIO_OFFSET],
+						0,
+						0,
+						info->pdata->gpio_funcs[SPI_EN_GPIO_OFFSET]);
+	}
+	else {
+		/* Use a GPIO to toggle CS? */
+		if( info->pdata->gpio_funcs[SPI_EN_GPIO_OFFSET] == NS9360_GPIO_FUNC_GPIO )
+			gpio_set_value( info->pdata->gpios[SPI_EN_GPIO_OFFSET], cspol == active );
+		/* otherwise CS will be automatically controlled by the spi engine */
+	}
+}
+
+/*
+ * Configure the controller for the passed SPI-device. Additionally it will
+ * use the configuration values of the SPI-transfer if it's different than NULL
+ */
+static int spi_ns9360_setupxfer(struct spi_device *spi,
+				 struct spi_transfer *t)
+{
+	struct spi_ns9360 *info = spi_master_get_devdata(spi->master);
+	unsigned int bpw;
+	unsigned int hz;
+	u32 brate;
+	u32 ctrl;
+
+	pr_debug("Setup for next xfer called\n");
+
+	/* If the bits per word is zero, then use the default value of the SPI-device */
+	bpw = t ? t->bits_per_word : spi->bits_per_word;
+	if (!bpw)
+		bpw = spi->bits_per_word;
+
+	/* We only support eight bits per word */
+	if (bpw != 8) {
+		pr_err("Invalid bits-per-word (%d)\n", bpw);
+		return -EINVAL;
+	}
+
+	/*
+	 * If the clock rate of the xfer is zero, then use the already configured
+	 * rate of this SPI-device
+	 */
+	if(!spi->max_speed_hz)
+		spi->max_speed_hz = SPI_DEFAULT_BPS;
+	hz  = t ? t->speed_hz : spi->max_speed_hz;
+	if (!hz)
+		hz = spi->max_speed_hz;
+
+	if( hz > (clk_get_rate(info->clk) / 4) ) {
+		pr_err("Invalid speed (%d)\n", hz );
+		return -EINVAL;
+	}
+	/* reset previous configuration */
+	iowrite32(0x00, info->ioaddr + SPI_CTRLA);
+
+	/* configure bitrate register */
+	brate = clk_get_rate(info->clk) / hz / 2 - 1;
+	if ((brate & SER_BITRATE_N_MA) != brate) {
+		dev_err(&spi->dev, "invalid bitrate %d\n", hz);
+		return -EINVAL;
+	}
+
+	brate |= SPI_BITRATE_EBIT | SPI_BITRATE_CLKMUX | SPI_BITRATE_TMODE;
+
+	if ((spi->mode & SPI_MODE_3) == SPI_MODE_0)
+		brate |= SPI_BITRATE_SPCPOL_LOW;
+	else if ((spi->mode & SPI_MODE_3) == SPI_MODE_1)
+		brate |= SPI_BITRATE_TXCINV | SPI_BITRATE_RXCINV;
+	else if ((spi->mode & SPI_MODE_3) == SPI_MODE_2)
+		brate |= SPI_BITRATE_SPCPOL_LOW | SPI_BITRATE_TXCINV |
+			SPI_BITRATE_RXCINV;
+
+	iowrite32(brate, info->ioaddr + SPI_BITRATE);
+
+	/* configure bitorder and master operation */
+	ctrl = SPI_CTRLB_MODE_SPIM;
+
+	if (!(spi->mode & SPI_LSB_FIRST))
+		ctrl |= SPI_CTRLB_BITORDR;
+
+	iowrite32(ctrl, info->ioaddr + SPI_CTRLB);
+
+	/* enable this channel */
+	ctrl = SPI_CTRLA_CE | SPI_CTRLA_WLS8;
+	iowrite32(ctrl, info->ioaddr + SPI_CTRLA);
+
+	return 0;
+}
+
+static int spi_ns9360_setup(struct spi_device *spi)
+{
+	struct spi_ns9360 *info = spi_master_get_devdata(spi->master);
+	int ret;
+
+	if (spi->chip_select > spi->master->num_chipselect) {
+		dev_dbg(&spi->dev,
+				"setup: invalid chipselect %u (%u defined)\n",
+				spi->chip_select, spi->master->num_chipselect);
+		return -EINVAL;
+	}
+
+	if (!spi->bits_per_word)
+		spi->bits_per_word = 8;
+
+	if ((spi->mode & SPI_CS_HIGH) && !info->pdata) {
+		/* FIXME: switch inverter on CS-pin */
+	}
+
+	/* Configure per-transfer settings (speed/bpw/mode) */
+	ret = spi_ns9360_setupxfer(spi, NULL);
+
+	return ret;
+}
+
+static void spi_ns9360_map_xfer(struct spi_device *spi,
+		struct spi_transfer *xfer)
+{
+	xfer->tx_dma = xfer->rx_dma = INVALID_DMA_ADDRESS;
+	/* TODO: Check this DMA mapping
+	if (!(xfer->len & (L1_CACHE_BYTES - 1))) {
+		if (xfer->tx_buf && !((unsigned long)xfer->tx_buf &
+					(L1_CACHE_BYTES - 1)))
+			xfer->tx_dma = dma_map_single(&spi->dev, (void *)xfer->tx_buf,
+						xfer->len, DMA_TO_DEVICE);
+		if (xfer->rx_buf && !((unsigned long)xfer->rx_buf &
+					(L1_CACHE_BYTES - 1)))
+			xfer->rx_dma = dma_map_single(&spi->dev, xfer->rx_buf,
+						xfer->len, DMA_FROM_DEVICE);
+	}
+	*/
+}
+
+static void spi_ns9360_next_xfer(struct spi_master *master,
+		struct spi_message *msg)
+{
+	struct spi_ns9360 *info = spi_master_get_devdata(master);
+	struct spi_transfer *xfer;
+	dma_addr_t tx_dma, rx_dma;
+	u32 len, ctrl;
+	int err;
+
+	xfer = info->current_transfer;
+	if (!xfer || info->remaining_bytes == 0) {
+		if (xfer)
+			xfer = list_entry(xfer->transfer_list.next,
+					struct spi_transfer, transfer_list);
+		else
+			xfer = list_entry(msg->transfers.next,
+					struct spi_transfer, transfer_list);
+		info->remaining_bytes = xfer->len;
+		info->current_transfer = xfer;
+	}
+
+	len = info->remaining_bytes;
+
+	/* Set the requested configuration for this new SPI-transfer */
+	err = spi_ns9360_setupxfer(msg->spi, xfer);
+	if (err) {
+		pr_err("Setting up the next SPI-transfer\n");
+		list_del(&msg->queue);
+		msg->status = err;
+
+		spin_unlock(&info->lock);
+		msg->complete(msg->context);
+		spin_lock(&info->lock);
+
+		info->current_transfer = NULL;
+		return;
+	}
+
+	tx_dma = xfer->tx_dma;
+	rx_dma = xfer->rx_dma;
+
+	/* Some drivers (like mmc_spi) set rx_dma=0 if they
+	 * don't need any return data.  If they do this, we
+	 * put the unwanted rx data in rx_dma and hope they
+	 * didn't ask to send more than BUFFER_SIZE (4096)
+	 * bytes.  This is at least better than trashing
+	 * page 0.
+	 *
+	 * - mwl
+	 */
+	if (!rx_dma || rx_dma == INVALID_DMA_ADDRESS) {
+		rx_dma = info->buf_rx_dma;
+		if (len > BUFFER_SIZE)
+			len = BUFFER_SIZE;
+	}
+	if (tx_dma == INVALID_DMA_ADDRESS) {
+		if (xfer->tx_buf) {
+			tx_dma = info->buf_tx_dma;
+			if (len > BUFFER_SIZE)
+				len = BUFFER_SIZE;
+			memcpy(info->buf_tx, xfer->tx_buf +
+					xfer->len - info->remaining_bytes, len);
+			dma_sync_single_for_device(&master->dev,
+					info->buf_tx_dma, len, DMA_TO_DEVICE);
+		} else {
+			/* Send undefined data; rx_dma is handy */
+			tx_dma = rx_dma;
+		}
+	}
+
+	/* The IO hub DMA controller doesn't cope well with
+	 * non-aligned rx buffers (and possibly tx too).  On
+	 * rx, at least, it causes the last byte read by the
+	 * peripheral on the *previous* transfer to be
+	 * delivered again if the current transfer is
+	 * unaligned.
+	 *
+	 * For now, just warn about it so someone can fix
+	 * the offending caller when it occurs.  It might
+	 * not be a problem on tx.
+	 *
+	 * - mwl
+	 */
+	if (rx_dma & 0x3) {
+		dev_err(&master->dev, "unaligned rx_dma (=0x%08x, len=%u)\n",
+			rx_dma, len);
+	}
+	if (tx_dma & 0x3) {
+		dev_err(&master->dev, "unaligned tx_dma (=0x%08x, len=%u)\n",
+			tx_dma, len);
+	}
+
+	/* setup the DMA channels */
+	iowrite32(BBUS_DMA_CTRL_RESET, info->ioaddr_bbus + BBUS_DMA_RXCTRL);
+	iowrite32(BBUS_DMA_CTRL_RESET, info->ioaddr_bbus + BBUS_DMA_TXCTRL);
+
+	memset(info->desc_tx, 0, sizeof(*info->desc_tx));
+	info->desc_tx->source = (u32)(tx_dma);
+	info->desc_tx->len = len;
+	info->desc_tx->flags =
+		DMADESC_WRAP | DMADESC_INTR | DMATXDESC_LAST | DMADESC_FULL;
+
+	memset(info->desc_rx, 0, sizeof(*info->desc_rx));
+	info->desc_rx->source = (u32)(rx_dma);
+	info->desc_rx->len = len;
+	info->desc_rx->flags = DMADESC_WRAP;
+
+	info->dma_xfer_len = len;
+
+	/* configure dma channels */
+	iowrite32(SPI_DMA_TX_STAT_OPT, info->ioaddr_bbus + BBUS_DMA_TXSTAT);
+	iowrite32(SPI_DMA_RX_STAT_OPT, info->ioaddr_bbus + BBUS_DMA_RXSTAT);
+	iowrite32((u32)info->desc_tx_dma, info->ioaddr_bbus + BBUS_DMA_TXBUFP);
+	iowrite32((u32)info->desc_rx_dma, info->ioaddr_bbus + BBUS_DMA_RXBUFP);
+	iowrite32(SPI_DMA_TX_CTRL_OPT, info->ioaddr_bbus + BBUS_DMA_TXCTRL);
+	iowrite32(SPI_DMA_RX_CTRL_OPT, info->ioaddr_bbus + BBUS_DMA_RXCTRL);
+
+	/* fire up dma channels */
+	ctrl = ioread32(info->ioaddr + SPI_CTRLA);
+	ctrl |= SPI_CTRLA_ETXDMA | SPI_CTRLA_ERXDMA;
+	iowrite32(ctrl, info->ioaddr + SPI_CTRLA);
+}
+
+static void spi_ns9360_next_message(struct spi_master *master)
+{
+	struct spi_ns9360 *info = spi_master_get_devdata(master);
+	struct spi_message *msg;
+
+	BUG_ON(info->current_transfer);
+
+	msg = list_entry(info->queue.next, struct spi_message, queue);
+
+	spi_ns9360_chipsel(msg->spi, SPI_CS_ACTIVE);
+	spi_ns9360_next_xfer(master, msg);
+}
+
+static int spi_ns9360_transfer(struct spi_device *spi, struct spi_message *msg)
+{
+	struct spi_ns9360 *info = spi_master_get_devdata(spi->master);
+	struct spi_transfer *xfer;
+	unsigned long flags;
+
+	if (unlikely(list_empty(&msg->transfers)
+			|| !spi->max_speed_hz))
+		return -EINVAL;
+
+	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+		if (!(xfer->tx_buf || xfer->rx_buf)) {
+			dev_dbg(&spi->dev, "missing rx or tx buf\n");
+			return -EINVAL;
+		}
+	}
+
+	/* scrub dcache "early" */
+	if (!msg->is_dma_mapped) {
+		list_for_each_entry(xfer, &msg->transfers, transfer_list)
+			spi_ns9360_map_xfer(spi, xfer);
+	}
+
+	msg->status = -EINPROGRESS;
+	msg->actual_length = 0;
+
+	spin_lock_irqsave(&info->lock, flags);
+	list_add_tail(&msg->queue, &info->queue);
+	if (!info->current_transfer)
+		spi_ns9360_next_message(spi->master);
+	spin_unlock_irqrestore(&info->lock, flags);
+
+	return 0;
+}
+
+static void spi_ns9360_irq_trigger_next(struct spi_master *master)
+{
+	struct spi_ns9360 *info = spi_master_get_devdata(master);
+	struct spi_message *msg;
+	struct spi_transfer *xfer;
+	u32 len, ctrl;
+
+	BUG_ON((info->flags & (SPI_TX_IDONE|SPI_RX_IDONE)) != (SPI_TX_IDONE|SPI_RX_IDONE));
+
+	info->flags &= ~(SPI_TX_IDONE|SPI_RX_IDONE);
+	xfer = info->current_transfer;
+	msg = list_entry(info->queue.next, struct spi_message, queue);
+
+	len = info->dma_xfer_len;
+	if (len > BUFFER_SIZE)
+		len = BUFFER_SIZE;
+
+	/*
+	 * If the rx buffer wasn't aligned, we used a bounce
+	 * buffer for the transfer. Copy the data back and
+	 * make the bounce buffer ready for re-use.
+	 */
+	if (xfer->rx_buf && xfer->rx_dma == INVALID_DMA_ADDRESS) {
+		dma_sync_single_for_cpu(&master->dev, info->buf_rx_dma,
+					len, DMA_FROM_DEVICE);
+		memcpy(xfer->rx_buf, info->buf_rx + xfer->len -
+				info->remaining_bytes, len);
+	}
+
+	info->remaining_bytes -= len;
+
+	if (info->remaining_bytes == 0) {
+		msg->actual_length += xfer->len;
+
+		if (!msg->is_dma_mapped) {
+			if (xfer->tx_dma != INVALID_DMA_ADDRESS)
+				dma_unmap_single(&master->dev,
+						 xfer->tx_dma,
+						 xfer->len,
+						 DMA_TO_DEVICE);
+			if (xfer->rx_dma != INVALID_DMA_ADDRESS)
+				dma_unmap_single(&master->dev,
+						 xfer->rx_dma,
+						 xfer->len,
+						 DMA_FROM_DEVICE);
+		}
+
+		/* REVISIT: udelay in irq is unfriendly */
+		if (xfer->delay_usecs)
+			udelay(xfer->delay_usecs);
+
+		if (msg->transfers.prev == &xfer->transfer_list) {
+
+			/* Message complete */
+			spi_ns9360_chipsel(msg->spi, SPI_CS_INACTIVE);
+
+			list_del(&msg->queue);
+			msg->status = 0;
+
+			spin_unlock(&info->lock);
+			msg->complete(msg->context);
+			spin_lock(&info->lock);
+
+			info->current_transfer = NULL;
+
+			/* continue; complete() may have queued requests */
+			if (list_empty(&info->queue)) {
+				ctrl = ioread32(info->ioaddr_bbus +
+						BBUS_DMA_RXCTRL);
+				ctrl &= ~BBUS_DMA_CTRL_CE;
+				iowrite32(ctrl, info->ioaddr_bbus +
+						BBUS_DMA_RXCTRL);
+				ctrl = ioread32(info->ioaddr_bbus +
+						BBUS_DMA_TXCTRL);
+				ctrl &= ~BBUS_DMA_CTRL_CE;
+				iowrite32(ctrl, info->ioaddr_bbus +
+						BBUS_DMA_TXCTRL);
+			} else
+				spi_ns9360_next_message(master);
+		} else {
+			if (xfer->cs_change) {
+				spi_ns9360_chipsel(msg->spi, SPI_CS_INACTIVE);
+				udelay(1);
+				spi_ns9360_chipsel(msg->spi, SPI_CS_ACTIVE);
+			}
+
+			spi_ns9360_next_xfer(master, msg);
+		}
+	} else {
+		/* There are still pending data in the current transfer */
+		spi_ns9360_next_xfer(master, msg);
+	}
+}
+
+static irqreturn_t spi_ns9360_irq_tx(int irq, void *dev_id)
+{
+	struct spi_master *master = dev_id;
+	struct spi_ns9360 *info = spi_master_get_devdata(master);
+	u32 status;
+
+	spin_lock(&info->lock);
+
+	/* get status and mask all pending interrupts */
+	status = ioread32(info->ioaddr_bbus + BBUS_DMA_TXSTAT);
+	iowrite32(status, info->ioaddr_bbus + BBUS_DMA_TXSTAT);
+
+	if (status & (BBUS_DMA_STAT_ECIP | BBUS_DMA_STAT_CAIP |
+				BBUS_DMA_STAT_PCIP)) {
+		dev_err(&master->dev, "tx dma failure! status = 0x%08x\n",
+				status);
+
+		/* retrigger the xfer */
+		spi_ns9360_next_xfer(master, list_entry(info->queue.next,
+					struct spi_message, queue));
+	}
+
+	if (status & BBUS_DMA_STAT_NCIP) {
+		info->flags |= SPI_TX_IDONE;
+		if (info->flags & SPI_RX_IDONE)
+			spi_ns9360_irq_trigger_next(master);
+	}
+
+	spin_unlock(&info->lock);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t spi_ns9360_irq_rx(int irq, void *dev_id)
+{
+	struct spi_master *master = dev_id;
+	struct spi_ns9360 *info = spi_master_get_devdata(master);
+	u32 status, dlen;
+
+	spin_lock(&info->lock);
+
+	/* get status and mask all pending interrupts */
+	status = ioread32(info->ioaddr_bbus + BBUS_DMA_RXSTAT);
+	iowrite32(status, info->ioaddr_bbus + BBUS_DMA_RXSTAT);
+
+	dlen = status & BBUS_DMA_STAT_BLEN_MA;
+	if (info->dma_xfer_len != dlen)
+		/* ?? what to do... ?? */
+		dev_err(&master->dev, "incomplete dma xfer"
+				"(%d/%d bytes transfered) \n",
+				dlen, info->dma_xfer_len);
+
+	if (status & (BBUS_DMA_STAT_ECIP | BBUS_DMA_STAT_CAIP |
+				BBUS_DMA_STAT_PCIP)) {
+		dev_err(&master->dev, "rx dma failure! status = 0x%08x\n",
+				status);
+
+		/* retrigger the xfer */
+		spi_ns9360_next_xfer(master, list_entry(info->queue.next,
+					struct spi_message, queue));
+	}
+
+	if (status & BBUS_DMA_STAT_NCIP) {
+		info->flags |= SPI_RX_IDONE;
+		if (info->flags & SPI_TX_IDONE)
+			spi_ns9360_irq_trigger_next(master);
+	}
+
+	spin_unlock(&info->lock);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit spi_ns9360_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct spi_master *master;
+	struct spi_ns9360 *info;
+	int gpio_cs, gpio_cs_func;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*info));
+	if (!master) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc_master\n", __func__);
+		goto err_alloc_master;
+	}
+
+	platform_set_drvdata(pdev, master);
+
+	info = spi_master_get_devdata(master);
+	info->pdata = pdev->dev.platform_data;
+
+	info->irq_rx = platform_get_irq(pdev, 0);
+	if (info->irq_rx < 0) {
+		ret = -ENOENT;
+		dev_dbg(&pdev->dev, "%s: err_get_irq_rx\n", __func__);
+		goto err_get_irq_rx;
+	}
+
+	info->irq_tx = platform_get_irq(pdev, 1);
+	if (info->irq_tx < 0) {
+		ret = -ENOENT;
+		dev_dbg(&pdev->dev, "%s: err_get_irq_tx\n", __func__);
+		goto err_get_irq_tx;
+	}
+
+	info->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!info->mem) {
+		ret = -ENOENT;
+		dev_dbg(&pdev->dev, "%s: err_get_mem\n", __func__);
+		goto err_get_mem;
+	}
+
+	if (!request_mem_region(info->mem->start,
+			info->mem->end - info->mem->start + 1,
+			pdev->name)) {
+		ret = -ENXIO;
+		dev_dbg(&pdev->dev, "%s: err_req_mem\n", __func__);
+		goto err_req_mem;
+	}
+
+	info->ioaddr = ioremap(info->mem->start,
+			info->mem->end - info->mem->start + 1);
+	if (!info->ioaddr) {
+		ret = -ENXIO;
+		dev_dbg(&pdev->dev, "%s: err_map_mem\n", __func__);
+		goto err_map_mem;
+	}
+
+	info->mem_bbus = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!info->mem_bbus) {
+		ret = -ENOENT;
+		dev_dbg(&pdev->dev, "%s: err_get_mem_bbus\n", __func__);
+		goto err_get_mem_bbus;
+	}
+
+	if (!request_mem_region(info->mem_bbus->start,
+			info->mem_bbus->end - info->mem_bbus->start + 1,
+			pdev->name)) {
+		ret = -ENXIO;
+		dev_dbg(&pdev->dev, "%s: err_req_mem_bbus\n", __func__);
+		goto err_req_mem_bbus;
+	}
+
+	info->ioaddr_bbus = ioremap(info->mem_bbus->start,
+			info->mem_bbus->end - info->mem_bbus->start + 1);
+	if (!info->ioaddr_bbus) {
+		ret = -ENXIO;
+		dev_dbg(&pdev->dev, "%s: err_map_mem_bbus\n", __func__);
+		goto err_map_mem_bbus;
+	}
+
+	master->bus_num = pdev->id;
+	/* hardware controlled cs */
+	master->num_chipselect = 1;
+
+	/* If CS is GPIO controlled, configure it as output */
+	gpio_cs = info->pdata->gpios[SPI_EN_GPIO_OFFSET];
+		gpio_cs_func = info->pdata->gpio_funcs[SPI_EN_GPIO_OFFSET];
+		if( NS9360_GPIO_FUNC_GPIO == gpio_cs_func )
+			gpio_direction_output( gpio_cs,
+					gpio_get_value( gpio_cs ) );
+
+	master->setup = spi_ns9360_setup;
+	master->transfer = spi_ns9360_transfer;
+
+	spin_lock_init(&info->lock);
+	INIT_LIST_HEAD(&info->queue);
+
+	info->buf_tx = dma_alloc_coherent(&pdev->dev, BUFFER_SIZE,
+					&info->buf_tx_dma, GFP_KERNEL);
+	if (!info->buf_tx) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc_buf_tx\n", __func__);
+		goto err_alloc_buf_tx;
+	}
+
+	info->buf_rx = dma_alloc_coherent(&pdev->dev, BUFFER_SIZE,
+					&info->buf_rx_dma, GFP_KERNEL);
+	if (!info->buf_rx) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc_buf_rx\n", __func__);
+		goto err_alloc_buf_rx;
+	}
+
+	info->desc_tx = dma_alloc_coherent(&pdev->dev,
+			sizeof(info->desc_tx), &info->desc_tx_dma, GFP_DMA);
+	if (!info->desc_tx) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc_desc_tx\n", __func__);
+		goto err_alloc_desc_tx;
+	}
+
+	info->desc_rx = dma_alloc_coherent(&pdev->dev,
+			sizeof(info->desc_rx), &info->desc_rx_dma, GFP_DMA);
+	if (!info->desc_rx) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_alloc_desc_rx\n", __func__);
+		goto err_alloc_desc_rx;
+	}
+
+	info->clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(info->clk)) {
+		ret = PTR_ERR(info->clk);
+		dev_dbg(&pdev->dev, "%s: err_clk_get\n", __func__);
+		goto err_clk_get;
+	}
+
+	ret = clk_enable(info->clk);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_clk_enable\n", __func__);
+		goto err_clk_enable;
+	}
+
+	ret = request_irq(info->irq_rx, spi_ns9360_irq_rx, 0,
+			pdev->name, master);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_req_irq_rx\n", __func__);
+		goto err_req_irq_rx;
+	}
+
+	ret = request_irq(info->irq_tx, spi_ns9360_irq_tx, 0,
+			pdev->name, master);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_req_irq_tx\n", __func__);
+		goto err_req_irq_tx;
+	}
+
+	ret = spi_register_master(master);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_reg_reg_master\n", __func__);
+		goto err_reg_master;
+	}
+
+	dev_info(&pdev->dev, "NS9360 SPI controller at 0x%p (irq: %d/%d)\n",
+			info->ioaddr, info->irq_rx, info->irq_tx);
+
+	return 0;
+
+err_reg_master:
+	free_irq(info->irq_tx, master);
+err_req_irq_tx:
+	free_irq(info->irq_rx, master);
+err_req_irq_rx:
+	clk_disable(info->clk);
+err_clk_enable:
+	clk_put(info->clk);
+err_clk_get:
+	dma_free_coherent(&pdev->dev, sizeof(info->desc_rx),
+			info->desc_rx, info->desc_rx_dma);
+err_alloc_desc_rx:
+	dma_free_coherent(&pdev->dev, sizeof(info->desc_tx),
+			info->desc_tx, info->desc_tx_dma);
+err_alloc_desc_tx:
+	dma_free_coherent(&pdev->dev, BUFFER_SIZE,
+			info->buf_rx, info->buf_rx_dma);
+err_alloc_buf_rx:
+	dma_free_coherent(&pdev->dev, BUFFER_SIZE,
+			info->buf_tx, info->buf_tx_dma);
+err_alloc_buf_tx:
+	iounmap(info->ioaddr_bbus);
+err_map_mem_bbus:
+	release_mem_region(info->mem_bbus->start,
+			info->mem_bbus->end - info->mem_bbus->start + 1);
+err_req_mem_bbus:
+err_get_mem_bbus:
+	iounmap(info->ioaddr);
+err_map_mem:
+	release_mem_region(info->mem->start,
+			info->mem->end - info->mem->start + 1);
+err_req_mem:
+err_get_mem:
+err_get_irq_tx:
+err_get_irq_rx:
+	spi_master_put(master);
+err_alloc_master:
+
+	return ret;
+}
+
+static int __devexit spi_ns9360_remove(struct platform_device *pdev)
+{
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct spi_ns9360 *info = spi_master_get_devdata(master);
+
+	free_irq(info->irq_tx, master);
+	free_irq(info->irq_rx, master);
+
+	clk_disable(info->clk);
+	clk_put(info->clk);
+
+	dma_free_coherent(&pdev->dev, sizeof(info->desc_rx),
+			info->desc_rx, info->desc_rx_dma);
+	dma_free_coherent(&pdev->dev, sizeof(info->desc_tx),
+			info->desc_tx, info->desc_tx_dma);
+	dma_free_coherent(&pdev->dev, BUFFER_SIZE,
+			info->buf_rx, info->buf_rx_dma);
+	dma_free_coherent(&pdev->dev, BUFFER_SIZE,
+			info->buf_tx, info->buf_tx_dma);
+
+	iounmap(info->ioaddr_bbus);
+	release_mem_region(info->mem_bbus->start,
+			info->mem_bbus->end - info->mem_bbus->start + 1);
+
+	iounmap(info->ioaddr);
+	release_mem_region(info->mem->start,
+			info->mem->end - info->mem->start + 1);
+
+	spi_master_put(master);
+
+	return 0;
+}
+
+static struct platform_driver spi_ns9360_driver = {
+	.probe		= spi_ns9360_probe,
+	.remove		= __devexit_p(spi_ns9360_remove),
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init spi_ns9360_init(void)
+{
+	return platform_driver_register(&spi_ns9360_driver);
+}
+
+static void __exit spi_ns9360_exit(void)
+{
+	platform_driver_unregister(&spi_ns9360_driver);
+}
+
+module_init(spi_ns9360_init);
+module_exit(spi_ns9360_exit);
+
+MODULE_DESCRIPTION("NS9360 SPI Driver");
+MODULE_AUTHOR("Digi International Inc.");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi_s3c2443.c b/drivers/spi/spi_s3c2443.c
new file mode 100644
index 000000000000..7b0d087776dc
--- /dev/null
+++ b/drivers/spi/spi_s3c2443.c
@@ -0,0 +1,1229 @@
+/* -*- linux-c -*-
+ *
+ * linux/drivers/spi/spi_s3c2443.c
+ *
+ * Copyright (c) 2008 Digi International
+ *
+ * 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.
+ *
+ *  !Revision:   $Revision: 1.1.1.1 $
+ *  !Author:     Luis Galdos
+ *  !Descr:      High Speed SPI driver for the S3C2443
+ *  !References: Based on the driver of the Samsung SMDK for the S3C2443
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+
+#include <linux/spi/spi.h>
+#include <linux/spi/spi_bitbang.h>
+
+#include <asm/io.h>
+#include <asm/dma.h>
+#include <mach/hardware.h>
+#include <asm/delay.h>
+
+#include <mach/gpio.h>
+#include <mach/regs-gpio.h>
+#include <mach/regs-gpioj.h>
+#include <asm/plat-s3c24xx/regs-spi.h>
+#include <mach/dma.h>
+#include <asm/plat-s3c24xx/spi.h>
+#include <mach/dma.h>
+#include <mach/regs-s3c2443-clock.h>
+
+
+/* This macro selects the virtual DMA-channel to use */
+#define SPI_TX_CHANNEL				DMACH_SPI0
+#define SPI_RX_CHANNEL				DMACH_SPI0_RX
+
+#define S3C2443_SPI_DMA_BUFFER			(PAGE_SIZE)
+
+/* Supported modes */
+#define S3C2443_SPI_SUPPORTED_MODES		(SPI_CPOL | SPI_CPHA)
+
+/* DMA transfer unit (byte). */
+#define S3C24XX_DMA_XFER_BYTE			1
+#define S3C24XX_DMA_XFER_HWORD			2
+#define S3C24XX_DMA_XFER_WORD			4
+
+/* Used for setting the CS on the different SPI-operations */
+#define SPI_CS_ACTIVE				1
+#define SPI_CS_INACTIVE				0
+
+#define printk_err(fmt, args...)                printk(KERN_ERR "[ ERROR ] spi-s3c2443: " fmt, ## args)
+#define printk_info(fmt, args...)               printk(KERN_INFO "spi-s3c2443: " fmt, ## args)
+
+#if 0
+#define S3C2443_SPI_DEBUG
+#endif
+
+#ifdef S3C2443_SPI_DEBUG
+#  define printk_debug(fmt, args...)		printk(KERN_DEBUG "spi-s3c2443: %s() " fmt, __FUNCTION__ , ## args)
+#else
+#  define printk_debug(fmt, args...)
+#endif
+
+
+enum s3c2443_xfer_t {
+	S3C2443_DMA_TX = 0,
+	S3C2443_DMA_RX = 1,
+};
+
+
+struct s3c2443_spi {
+	void __iomem		*regs;
+	int			 irq;
+	struct clk		*clk;
+	struct resource		*ioarea;
+	struct spi_master	*master;
+	struct device		*dev;
+	struct s3c2443_spi_info *pdata;
+
+	struct list_head xmit_queue;
+	spinlock_t xmit_lock;
+	struct spi_transfer *current_xfer;
+	long remaining_bytes;
+	unsigned long dma_xfer_len;
+	struct tasklet_struct xmit_tasklet;
+	void *tx_buf;
+	void *rx_buf;
+	dma_addr_t tx_dma;
+	dma_addr_t rx_dma;
+
+	dmach_t dma_ch;
+	struct s3c2410_dma_client dmach;
+	dmach_t dma_ch_rx;
+	struct s3c2410_dma_client dmach_rx;
+};
+
+
+/* Internal functions */
+static void s3c2443_spi_next_message(struct spi_master *master);
+static void s3c2443_spi_next_xfer(struct spi_master *master, struct spi_message *msg);
+
+
+static inline struct s3c2443_spi *spi_to_hw(struct spi_device *sdev)
+{
+	return spi_master_get_devdata(sdev->master);
+}
+
+static inline struct s3c2443_spi *master_to_hw(struct spi_master *master)
+{
+	return spi_master_get_devdata(master);
+}
+
+
+static void s3c2443_spi_chipsel(struct spi_device *spi, int active)
+{
+	/* unsigned int cspol; */
+	struct s3c2443_spi *hw;
+	void (* cs_callback)(struct spi_device *_spi, int _act);
+	unsigned long slavecfg;
+	struct s3c2443_spi_info *info;
+	struct s3c24xx_spi_gpio *cs;
+	
+	hw = spi_to_hw(spi);
+	if (!hw) {
+		printk_err("HW null pointer found!\n");
+		return;
+	}
+	
+	/*
+	 * If the SPI-device has a pointer function for controlling the
+	 * chip select, then we will call its passed function. Otherwise the
+	 * CS will be controlled by the SPI-hardware
+	 */
+	if (spi->controller_data) {
+		info = hw->pdata;
+		cs = info->cs;
+		
+		printk_debug("Calling the controller data\n");
+		
+		/*
+		 * Since the SPI-device will control its own CS disable ours by
+		 * configuring it as input, then we have enabled the pull-up, right?
+		 */
+		if (active == SPI_CS_ACTIVE)
+			s3c2410_gpio_cfgpin(cs->nr, S3C2410_GPIO_INPUT);
+
+		cs_callback = spi->controller_data;
+		cs_callback(spi, !active);
+		
+		/* Now reenable our CS, but before disable the last CS-activation! */
+		if (active == SPI_CS_INACTIVE) {
+			writel(S3C2443_SPI0_SLAVE_SIG_INACT,
+			       hw->regs + S3C2443_SPI0_SLAVE_SEL);
+			s3c2410_gpio_cfgpin(cs->nr, cs->cfg);
+		}
+	}
+
+	/*
+	 * Even an external interrupt is being used, we MUST enable the slave
+	 * register for being able to shift the data to the bus. Otherwise the
+	 * FIFO will not send the data to the bus, and an error from the DMA-engine
+	 * will be generated: "... timeout ... load buffer ..."	 
+	 */
+	printk_debug("Using the HW-CS (%i)\n", active);
+	slavecfg = S3C2443_SPI0_SLAVE_SIG_INACT;
+	if (active == SPI_CS_ACTIVE)
+		slavecfg = S3C2443_SPI0_SLAVE_SIG_ACT;
+	
+	writel(slavecfg, hw->regs + S3C2443_SPI0_SLAVE_SEL);
+}
+
+/*
+ * Please reset the SPI-controller before starting a transfer. Otherwise the
+ * configuration of the last transfer will interfere with the new transfer
+ */
+static void s3c2443_spi_sw_reset(struct s3c2443_spi *spi)
+{
+	unsigned long cfg;
+	
+	cfg = readl(spi->regs + S3C2443_SPI0_CH_CFG);
+        writel(cfg | S3C2443_SPI0_CH_SW_RST, spi->regs + S3C2443_SPI0_CH_CFG);
+	writel(cfg, spi->regs + S3C2443_SPI0_CH_CFG);
+}
+
+
+static int s3c2443_spi_hw_init(struct s3c2443_spi *hw)
+{
+	unsigned long misccr, clkcfg;
+	struct s3c2443_spi_info *info;
+	int cnt;
+	struct s3c24xx_spi_gpio *cs;
+
+	info = hw->pdata;
+	
+        writel(readl(S3C2443_SCLKCON) | S3C2443_SCLKCON_HSSPICLK, S3C2443_SCLKCON);
+        writel(readl(S3C2443_PCLKCON) | S3C2443_PCLKCON_HSSPI, S3C2443_PCLKCON);
+	
+	if (info->input_clk == S3C2443_HSSPI_INCLK_PCLK)
+		clkcfg = S3C2443_SPI0_CLKSEL_PCLK;
+	else
+		clkcfg = S3C2443_SPI0_CLKSEL_EPLL;
+	writel(clkcfg, hw->regs + S3C2443_SPI0_CLK_CFG);
+
+	misccr = __raw_readl(S3C24XX_MISCCR);
+	__raw_writel(misccr | (1 << 31), S3C24XX_MISCCR);
+		
+        /* HSSPI software reset */
+	s3c2443_spi_sw_reset(hw);
+	
+	/* Configure the chip selects depending on the passed platform data */	
+        s3c2410_gpio_cfgpin(info->miso.nr, info->miso.cfg);
+	s3c2410_gpio_cfgpin(info->mosi.nr, info->mosi.cfg);
+	s3c2410_gpio_cfgpin(info->clk.nr, info->clk.cfg);
+	for (cnt = 0; cnt < info->num_chipselect; cnt++) {
+
+		/*
+		 * We need to activate the pullup for being able to reconfigure the
+		 * CS as an input IO
+		 */
+		cs = info->cs + cnt;
+		s3c2410_gpio_cfgpin(cs->nr, cs->cfg);
+		s3c2410_gpio_pullup(cs->nr, 0);
+	}
+	        
+	return 0;
+}
+
+/*
+ * Configure the controller for the passed SPI-device. Additionally it will
+ * use the configuration values of the SPI-transfer if it's different than NULL
+ */
+static int s3c2443_spi_setupxfer(struct spi_device *spi,
+				 struct spi_transfer *t)
+{
+	struct s3c2443_spi *hw = spi_to_hw(spi);
+	unsigned int bpw;
+	unsigned int hz, max_hz, min_hz;
+	unsigned int div;
+	unsigned long clkreg, chcfg, mode;
+	
+	printk_debug("Setup for next xfer called\n");
+
+	/* If the bits per word is zero, then use the default value of the SPI-device */
+	bpw = t ? t->bits_per_word : spi->bits_per_word;
+	if (!bpw)
+		bpw = spi->bits_per_word;
+
+	/* We only support eigth bits per word */
+	if (bpw != 8) {
+		printk_err("Invalid bits-per-word (%d)\n", bpw);
+		return -EINVAL;
+	}
+
+	/*
+	 * If the clock rate of the xfer is zero, then use the already configured
+	 * rate of this SPI-device
+	 */
+	hz  = t ? t->speed_hz : spi->max_speed_hz;
+	if (!hz)
+		hz = spi->max_speed_hz;
+	
+	/* The below calculation is coming from the SMDK */
+	max_hz = clk_get_rate(hw->clk) / 2;
+	min_hz = max_hz / 256;
+	if (hz > max_hz || hz < min_hz) {
+		printk_err("Speed %d Hz out of range (Min %i | Max %i)\n",
+			   hz, min_hz, max_hz);
+		return -EINVAL;
+	}
+	
+	div = clk_get_rate(hw->clk) / hz;
+	div /= 2;
+	div = (div) ? (div - 1) : (0);
+	if (div > S3C2443_SPI0_CLK_PRE_MASK) {
+		printk_err("Invalid clock divider %d (%i Hz)\n", div, hz);
+		return -EINVAL;
+	}
+
+	/* Only modify the prescaler */
+	printk_debug("Setting pre-scaler to %d (%d Hz)\n", div, hz);
+	clkreg = readl(hw->regs + S3C2443_SPI0_CLK_CFG) & ~(S3C2443_SPI0_CLK_PRE_MASK);
+	writel(clkreg | div, hw->regs + S3C2443_SPI0_CLK_CFG);
+	
+	/*
+	 * For some reasons the clock configuration isn't working correctly by
+	 * the FIRST execution. For this reason let's make a sanity check and
+	 * if required reset the hardware and reconfigure the clock again. This seems
+	 * to solve the problem.
+	 * Luis Galdos
+	 */
+	clkreg = readl(hw->regs + S3C2443_SPI0_CLK_CFG);
+	if ((clkreg & S3C2443_SPI0_CLK_PRE_MASK) != div) {
+		printk_debug("Need to reconfigure the clock\n");
+		s3c2443_spi_hw_init(hw);
+		s3c2443_spi_sw_reset(hw);
+		writel(clkreg | div, hw->regs + S3C2443_SPI0_CLK_CFG);
+	}
+
+	/* Set the correct mode for the passed SPI-transfer */
+	mode = spi->mode & 0x03;
+	switch (mode) {
+	case SPI_MODE_2:
+		/* Sampled by falling edges with a high inactive clock (MODE_2) */
+		chcfg = S3C2443_SPI0_CH_FALLING | S3C2443_SPI0_CH_FORMAT_A;
+		break;
+
+	case SPI_MODE_1:
+		/* Sampled by falling edges with a low inactive clock (MODE_1) */
+		chcfg = S3C2443_SPI0_CH_RISING | S3C2443_SPI0_CH_FORMAT_B;
+		break;
+
+	case SPI_MODE_3:		
+		/* Sampled by rising edges with a high inactive clock (MODE_3) */
+		chcfg = S3C2443_SPI0_CH_FALLING | S3C2443_SPI0_CH_FORMAT_B;
+		break;
+
+	case SPI_MODE_0:
+	default:
+		/* Sampled by rising edges with a low inactive clock (MODE_0) */
+		chcfg = S3C2443_SPI0_CH_RISING | S3C2443_SPI0_CH_FORMAT_A;
+		break;
+	}
+	
+	printk_debug("Configuring the mode %lu\n", mode);
+	writel(chcfg , hw->regs + S3C2443_SPI0_CH_CFG);	
+	return 0;
+}
+
+/*
+ * Configure the master-controller. This function is only called when the higher
+ * layer wants to MODIFY the current configuration of the controller.
+ */
+static int s3c2443_spi_setup(struct spi_device *spi)
+{
+	int ret;
+
+	if (!spi->bits_per_word)
+		spi->bits_per_word = 8;
+
+	if (spi->mode & ~S3C2443_SPI_SUPPORTED_MODES) {
+		printk_debug("Unsupported mode bits %x\n",
+			     spi->mode & ~S3C2443_SPI_SUPPORTED_MODES);
+		return -EINVAL;
+	}
+
+	ret = s3c2443_spi_setupxfer(spi, NULL);
+	if (!ret)
+		printk_debug("Mode %d, %u bpw, %d HZ\n",
+			     spi->mode, spi->bits_per_word,
+			     spi->max_speed_hz);
+
+	return ret;
+}
+
+/*
+ * Tasklet function for checking the message queue. If there is another transfer
+ * pending, then this function will start the transfer, otherwise it calls the
+ * complete function of the SPI-message and reset the internal data for the
+ * next available message of the internal message queue and start it too
+ */
+static void s3c2443_spi_next_tasklet(unsigned long data)
+{
+	struct s3c2443_spi *hw;
+	struct spi_transfer *xfer;
+	struct spi_message *msg;
+	unsigned long stat;
+	int timeout, err;
+
+	hw = (struct s3c2443_spi *)data;
+	if (!hw)
+		return;
+	
+	xfer = hw->current_xfer;
+	msg = list_entry(hw->xmit_queue.next, struct spi_message, queue);
+	if (!xfer || !msg)
+		return;
+
+	/*
+	 * If we used the own internal DMA-buffer for the DMA-transfer then copy
+	 * the data to the higher SPI-message. If not, then cause we have
+	 * passed the transfer-buffer to the DMA-controller
+	 */
+	if (!msg->is_dma_mapped && xfer->rx_buf && hw->dma_xfer_len) {
+		printk_debug("Copying %lu bytes to SPI-buffer %p\n",
+			     hw->dma_xfer_len, xfer->rx_buf);
+		memcpy(xfer->rx_buf, hw->rx_buf, hw->dma_xfer_len);
+	}
+
+	/*
+	 * We have a problem at this place, then we don't know if
+	 * the shift-register is really empty. For this reason we
+	 * use the below loop. Please note that by high configured
+	 * clocks the shift register will be already empty at this point
+	 * and by low clocks the usleep is not really relevant
+	 * compared with the complete transmission time. The problem is
+	 * that udelay will cause a higher CPU-load, but we don't
+	 * want to sleep before calling the tasklet, and at this place
+	 * is no more possible to take a siesta.
+	 * Luis Galdos
+	 */
+	err = (hw->remaining_bytes < 0) ? (hw->remaining_bytes) : 0;
+	if (!err && xfer->tx_buf) {
+		stat = readl(hw->regs + S3C2443_SPI0_STATUS);
+		timeout = (err) ? 0 : 0xffff;
+		while (!(stat & S3C2443_SPI0_STUS_TX_DONE) && timeout) {
+			udelay(2);
+			stat = readl(hw->regs + S3C2443_SPI0_STATUS);
+			timeout--;
+		}
+
+		if (!timeout)
+			err = -ETIME;
+	}
+	
+	/*
+	 * If NO more data is remaining then update the internal variables of the
+	 * SPI-message and call the complete-function. The errors are passed to us
+	 * with an negative value of remaining_bytes
+	 */
+	if (hw->remaining_bytes == 0 || err) {
+
+		/* Disable the IRQs and FIFOs, then we are done */
+		writel(0x00, hw->regs + S3C2443_SPI0_CH_CFG);
+
+		msg->actual_length += hw->dma_xfer_len;
+
+		if (xfer->delay_usecs)
+			udelay(xfer->delay_usecs);
+
+		/* Check if the current SPI-transfer was the last of the message */
+		if (msg->transfers.prev == &xfer->transfer_list || err) {
+			msg->status = err;
+			
+			/* Disable the slave selection (chip select inactive) */
+			s3c2443_spi_chipsel(msg->spi, SPI_CS_INACTIVE);
+
+			stat = readl(hw->regs + S3C2443_SPI0_STATUS);
+
+			printk_debug("Msg complete %i | stat 0x08%x)\n",
+				     err, (unsigned int)stat);
+			
+			list_del(&msg->queue);
+			msg->complete(msg->context);
+
+			/*
+			 * Now, check for the next message of the internal xmit-queue.
+			 * But first lock the code segment, then probably somebody is
+			 * trying to start a new transfer at this moment
+			 */
+			spin_lock(&hw->xmit_lock);
+			hw->current_xfer = NULL;
+			if (!list_empty(&hw->xmit_queue))
+				s3c2443_spi_next_message(hw->master);
+			spin_unlock(&hw->xmit_lock);
+			
+		} else {
+			printk_debug("Calling the next XFER of the msg %p\n", msg);
+
+			/* Toggle the CS if requested */
+			if (xfer->cs_change) {
+                                s3c2443_spi_chipsel(msg->spi, SPI_CS_INACTIVE);
+                                udelay(1);
+                                s3c2443_spi_chipsel(msg->spi, SPI_CS_ACTIVE);
+                        }
+			
+			s3c2443_spi_next_xfer(hw->master, msg);			
+		}
+		
+	} else {
+		/*
+		 * Start the next write operation if there is remaining data of the
+		 * last SPI-transfer
+		 */
+		printk_debug("Need to send more data (%lu)\n", hw->remaining_bytes);
+		s3c2443_spi_next_xfer(hw->master, msg);
+	}
+
+}
+
+/*
+ * The DMA-subsytem calls this function from an interrupt context. For this
+ * reason we only update the remaining-bytes counter and check if can
+ * schedule the tasklet for the SPI-complete. BUT the called will be scheduled
+ * only if the FIFO has no more data to send, otherwise we will modify the interrupt
+ * registers for receiving an interrupt from the TX-FIFO later.
+ */
+static void s3c2443_spi_dma_callback(struct s3c2410_dma_chan *ch,
+				       void *_hw, int size,
+				       enum s3c2410_dma_buffresult result)
+{
+	struct s3c2443_spi *hw;
+	unsigned long mode, inten, stat;
+	int is_tx;
+	
+	hw = (struct s3c2443_spi *)_hw;
+	if (!hw) {
+		printk_err("Got a NULL pointer from the DMA-callback!\n");
+		return;
+	}
+
+	/* Check if the callback corresponds to the RX or TX-channel */
+	is_tx = (ch->client == &hw->dmach) ? (1) : (0);
+	printk_debug("%s DMA callback | Ch %i\n", (is_tx) ? "TX" : "RX", ch->number);
+		
+	/* If the transfer has a RX-channel running then need to wait for it */
+	if (is_tx && hw->current_xfer->rx_buf && result == S3C2410_RES_OK) {
+		printk_debug("Need to wait for the RX-DMA\n");
+		return;
+	}
+
+	/*
+	 * If an error is detected pass the info to the tasklet too (place the error
+	 * message into the status of the message).
+	 * By aborts only set the number of remaining bytes to zero, so that the
+	 * tasklet will complete the message
+	 */
+	switch (result) {
+	case S3C2410_RES_OK:
+		printk_debug("Result OK\n");
+		hw->remaining_bytes -= size;
+		hw->dma_xfer_len += size;
+		break;
+	case S3C2410_RES_ERR:
+		printk_err("Result ERROR\n");
+		hw->dma_xfer_len = 0;
+		hw->remaining_bytes = -ERESTART;
+		break;
+	case S3C2410_RES_ABORT:
+		printk_debug("Result ABORT\n");
+		hw->dma_xfer_len += size;
+		hw->remaining_bytes = -ECONNABORTED;
+		break;
+	}
+
+	/*
+	 * At this point, normally the TX-FIFO has still data to transfer to the
+	 * bus. That means for us, we can enable the interrupt for the TX-FIFO
+	 * which informs us about the TX-done. So, only enable
+	 * the level IRQ, so that the IRQ-handler will call the tasklet later.
+	 * BTW, we only enable the interrupt when we are transferring data using
+	 * the DMA-controller, otherwise the DMA-subsystem calls this function when
+	 * all the bytes were already read
+	 */
+	if (likely(result == S3C2410_RES_OK && hw->current_xfer->tx_buf)) {
+		stat = readl(hw->regs + S3C2443_SPI0_STATUS);
+		
+		printk_debug("Need to wait for the TX-FIFO | stat 0x%x\n",
+		       (unsigned int)stat);		
+
+		/* Disable the DMA-mode for the TX-FIFO */
+		mode = readl(hw->regs + S3C2443_SPI0_MODE_CFG);
+		mode &= ~S3C2443_SPI0_MODE_TXDMA_ON;
+		mode |= (1 << 5);
+		writel(mode, hw->regs + S3C2443_SPI0_MODE_CFG);
+
+		/* Enable the ready interrupt */
+		inten = readl(hw->regs + S3C2443_SPI0_INT_EN);
+		inten |= S3C2443_SPI0_INT_TX_UNDERRUN_EN |
+			S3C2443_SPI0_INT_TX_FIFORDY_EN;	
+		writel(inten , hw->regs + S3C2443_SPI0_INT_EN);
+	} else {
+		printk_debug("Calling the tasklet from the DMA-callback\n");
+		tasklet_schedule(&hw->xmit_tasklet);
+	}
+}
+
+/*
+ * IMPORTANT: If the data transfer is smaller than four bytes, then the SPI-controller
+ * must be configured without the burst-mode! Otherwise the channel will run
+ * amok!
+ * Luis Galdos
+ */
+static int s3c2443_spi_dma_init(dmach_t dma_ch, enum s3c2443_xfer_t mode,
+				int length)
+{
+	int xmode;
+	
+	printk_debug("DMA init for next %s\n", (mode == S3C2443_DMA_TX) ? "TX" : "RX");
+
+	if (mode == S3C2443_DMA_TX) {
+		/*
+		 * For TX-transfers configure the destination with fixed address and
+		 * inside the APB-bus. The source of transfers will be the
+		 * system memory
+		 */
+                s3c2410_dma_devconfig(dma_ch, S3C2410_DMASRC_MEM,
+				      S3C2410_DISRCC_INC | S3C2410_DISRCC_APB,
+				      S3C2443_SPI0_TX_DATA_PA);
+        } else if (mode == S3C2443_DMA_RX) {
+		/*
+		 * By RX-transfer the source is the RX-FIFO of the SPI-controller.
+		 * The controller is part of the APB and should not increment
+		 * the address
+		 */
+                s3c2410_dma_devconfig(dma_ch, S3C2410_DMASRC_HW,
+				      S3C2410_DISRCC_INC | S3C2410_DISRCC_APB,
+				      S3C2443_SPI0_RX_DATA_PA);
+	} else {
+		printk_err("Invalid DMA transfer mode (%i)\n", mode);
+		return -EINVAL;
+	}
+
+	/*
+	 * Select the correct transfer mode depending on the number of bytes
+	 * to transfer. Unfortunately the DMA-controller can transfer only data
+	 * quantums depending on the number of the data size to be transferred
+	 * (see register: DCON, field DSZ)
+	 * Luis Galdos
+	 */
+	if (!(length & 0x3))
+		xmode = S3C24XX_DMA_XFER_WORD;
+	else
+		xmode = S3C24XX_DMA_XFER_BYTE;
+
+	/* @XXX: Don't use the burst mode, then it's not working with this driver */
+	s3c2410_dma_config(dma_ch,
+			   xmode,
+			   S3C2410_DCON_HANDSHAKE |
+			   S3C2410_DCON_SYNC_PCLK |
+			   S3C2410_DCON_HWTRIG);
+	
+	s3c2410_dma_setflags(dma_ch, S3C2410_DMAF_AUTOSTART);
+        return 0;
+}
+
+/*
+ * This function is used to handle the next transfer of a SPI-message.
+ * If the configuration of the transfer failed, then this function will
+ * set the error code and will schedule the tasklet for completing the
+ * SPI-message correctly.
+ */
+static void s3c2443_spi_next_xfer(struct spi_master *master,
+				  struct spi_message *msg)
+{
+	struct spi_transfer *xfer;
+	struct s3c2443_spi *hw;
+	unsigned long len;
+	dma_addr_t rx_dma, tx_dma;
+	int err;
+	unsigned long clkcfg, chcfg, modecfg, spi_packet;
+	
+	hw = master_to_hw(master);
+
+	/* Always reset the controller first */
+	s3c2443_spi_sw_reset(hw);
+
+	/*
+	 * Check if we are going to start sending a complete new message, or if
+	 * the passed message was already used for sending a SPI-transfer
+	 */
+	xfer = hw->current_xfer;
+	if (!xfer || hw->remaining_bytes == 0) {
+		if (xfer)
+			xfer = list_entry(xfer->transfer_list.next,
+					  struct spi_transfer, transfer_list);
+		else
+			xfer = list_entry(msg->transfers.next,
+					  struct spi_transfer, transfer_list);
+
+		hw->current_xfer = xfer;
+		hw->remaining_bytes = xfer->len;
+		hw->dma_xfer_len = 0;
+	}
+
+	/* Set the requested configuration for this new SPI-transfer */
+	err = s3c2443_spi_setupxfer(msg->spi, xfer);
+	if (err) {
+		printk_err("Setting up the next SPI-transfer\n");
+		hw->remaining_bytes = err;
+		goto exit_err;
+	}
+	
+	len = hw->remaining_bytes;
+
+        /* 2. Enable the output clock (but don't modify the clock prescaler) */
+	clkcfg = readl(hw->regs + S3C2443_SPI0_CLK_CFG);
+	clkcfg |= S3C2443_SPI0_ENCLK_ENABLE;
+	writel(clkcfg, hw->regs + S3C2443_SPI0_CLK_CFG);
+	
+	/*
+	 * Configure the transfer mode depending on the number of bytes to send
+	 * @XXX: For some unknown reasons we can't enable the burst mode. Please be
+	 * aware then the burst mode for the FIFOs will work ONLY if the
+	 * DMA-channel is configured for the burst mode too (so far so good).
+	 * Luis Galdos
+	 */
+	modecfg = S3C2443_SPI0_MODE_CH_TSZ_BYTE;
+	if (!(len & 0x3))
+		modecfg = S3C2443_SPI0_MODE_CH_TSZ_WORD;
+
+	/*
+	 * Enable the DMA-modes depending on the transfer type
+	 */
+	if (xfer->tx_buf)
+		modecfg |= S3C2443_SPI0_MODE_TXDMA_ON;
+	if (xfer->rx_buf)
+		modecfg	|= S3C2443_SPI0_MODE_RXDMA_ON;
+	
+        writel(modecfg, hw->regs + S3C2443_SPI0_MODE_CFG);
+
+        /*
+	 * 4. Set SPI INT_EN register
+	 * Disable all the interrupts then we only use the DMA-controller for
+	 * transferring the data from the RAM to the FIFO
+	 */
+        writel(0x00, hw->regs + S3C2443_SPI0_INT_EN);
+
+	/* Clear the pending register */
+        writel(0x1f, hw->regs + S3C2443_SPI0_PENDING_CLR);
+
+        /*
+         * If the passed SPI-buffer isn't DMA-mapped, then use the internal
+         * DMA-buffers for sending the data to the DMA-layer. If we are using
+	 * the internal buffer, then check that we can only send the maximal
+	 * number of bytes. In the tasklet the transfer of the rest data can
+	 * be triggered.
+         */
+        tx_dma = xfer->tx_dma;
+        rx_dma = xfer->rx_dma;
+        if (!msg->is_dma_mapped) {
+
+		printk_debug("SPI-message not DMA mapped\n");
+		
+                rx_dma = hw->rx_dma;
+                if (len > S3C2443_SPI_DMA_BUFFER)
+                        len = S3C2443_SPI_DMA_BUFFER;
+
+                if (xfer->tx_buf) {
+                        tx_dma = hw->tx_dma;
+
+                        if (len > S3C2443_SPI_DMA_BUFFER)
+                                len = S3C2443_SPI_DMA_BUFFER;
+
+			/* IMPORTANT: Copy the TX-data from the correct place! */
+                        memcpy(hw->tx_buf,
+                               xfer->tx_buf + hw->dma_xfer_len, len);
+                }
+        }
+
+        /*
+	 * 5. Set Packet Count configuration register
+	 * For some unknown reasons the transfer of only ONE byte isn't
+	 * working correctly. The data byte transferred from the DMA-controller
+	 * isn't flushed from the TX-FIFO to the bus. But disabling the trailling
+	 * count and reconfiguring the TX-FIFO helps to fix this problem.
+	 * (Luis Galdos)
+	 */
+	spi_packet = len | S3C2443_SPI0_PACKET_CNT_EN;
+	if (unlikely(len == 1)) {
+		spi_packet = 0;
+		s3c2443_spi_sw_reset(hw);
+		s3c2443_spi_setupxfer(msg->spi, xfer);
+	}
+        writel(spi_packet, hw->regs + S3C2443_SPI0_PACKET_CNT);
+	
+	printk_debug("%s : %lu bytes | Rx %p | Tx %p\n", (xfer->tx_buf) ? "TX" : "RX",
+		     len, xfer->rx_buf, xfer->tx_buf);
+
+	s3c2443_spi_chipsel(msg->spi, SPI_CS_ACTIVE);
+	
+	/*
+	 * Prepare the DMA-channel with the correct direction, and callback-function
+	 * before sending the data to the DMA-subsystem
+	 * If the TX-channel is being used, then only enable the TX-callback, otherwise
+	 * use the RX-channel for the callback
+	 */
+	s3c2410_dma_set_buffdone_fn(SPI_TX_CHANNEL, NULL);
+	s3c2410_dma_set_buffdone_fn(SPI_RX_CHANNEL, NULL);
+	if (xfer->tx_buf)
+		s3c2410_dma_set_buffdone_fn(SPI_TX_CHANNEL, s3c2443_spi_dma_callback);
+	if (xfer->rx_buf)
+		s3c2410_dma_set_buffdone_fn(SPI_RX_CHANNEL, s3c2443_spi_dma_callback);
+
+	
+	if (xfer->rx_buf) {
+		s3c2443_spi_dma_init(hw->dma_ch_rx, S3C2443_DMA_RX, len);
+		err = s3c2410_dma_enqueue(hw->dma_ch_rx, hw, rx_dma, len);
+		if (err) {
+			printk_err("Starting the RX DMA-channel, %i\n", err);
+			s3c2410_dma_ctrl(hw->dma_ch_rx, S3C2410_DMAOP_FLUSH);
+			hw->remaining_bytes = -EBUSY;
+			goto exit_err;
+		}
+	}
+
+	if (xfer->tx_buf) {
+		s3c2443_spi_dma_init(hw->dma_ch, S3C2443_DMA_TX, len);
+		err = s3c2410_dma_enqueue(hw->dma_ch, hw, tx_dma, len);
+		if (err) {
+			printk_err("Starting the TX DMA-channel, %i\n", err);
+			hw->remaining_bytes = -EBUSY;
+			goto exit_err;
+		}
+	}
+
+	/* Init the FIFOs */
+        chcfg = readl(hw->regs + S3C2443_SPI0_CH_CFG);
+        chcfg &= ~(S3C2443_SPI0_CH_TXCH_ON | S3C2443_SPI0_CH_RXCH_ON);
+        if (xfer->tx_buf)
+                chcfg |= S3C2443_SPI0_CH_TXCH_ON;
+        if (xfer->rx_buf)
+                chcfg |= S3C2443_SPI0_CH_RXCH_ON;
+
+        writel(chcfg, hw->regs + S3C2443_SPI0_CH_CFG);
+
+	return;
+	
+	/* By errors only schedule the tasklet */
+ exit_err:
+	tasklet_schedule(&hw->xmit_tasklet);
+}
+
+/* Get the head of the message queue and start the xmit */
+static void s3c2443_spi_next_message(struct spi_master *master)
+{
+	struct s3c2443_spi *hw;
+	struct spi_message *msg;
+	
+	hw = master_to_hw(master);
+
+	msg = list_entry(hw->xmit_queue.next, struct spi_message, queue);
+	
+	printk_debug("Starting to handle a new message %p\n", msg);
+
+	/* And start to send the new message now */
+	/* s3c2443_spi_chipsel(msg->spi, SPI_CS_ACTIVE); */
+	s3c2443_spi_next_xfer(master, msg);
+}
+
+/*
+ * This function will be called from the external SPI-layer for sending data
+ * to the bus. It will return inmediately, if another message is being
+ * transmitted, otherwise it will start sending the data to the bus
+ */
+static int s3c2443_spi_transfer(struct spi_device *spi, struct spi_message *msg)
+{
+	struct s3c2443_spi *hw = spi_to_hw(spi);
+	struct spi_transfer *xfer;
+
+	if (unlikely(list_empty(&msg->transfers)))
+		return -EINVAL;
+
+	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+		if (!xfer->tx_buf && !xfer->rx_buf) {
+			printk_err("Missing rx/tx buffer.\n");
+			return -EINVAL;
+		}
+	}
+
+	msg->status = -EINPROGRESS;
+	msg->actual_length = 0;
+
+	/*
+	 * Add the new arrived message to the internal queue and start the
+	 * transfer if the xmit-queue is IDLE
+	 */
+	spin_lock(&hw->xmit_lock);
+	list_add_tail(&msg->queue, &hw->xmit_queue);
+	if (!hw->current_xfer)
+		s3c2443_spi_next_message(spi->master);
+	spin_unlock(&hw->xmit_lock);
+	
+	return 0;
+}
+
+/*
+ * Normally the IRQs are not used for transferring data to the FIFOs, we use only
+ * the DMA-controller for this purpose.
+ *
+ * @FIXME: For some reasons we are not clearing all the interrupts correctly. With
+ * the activated debug prints you will see what I'm talking about.
+ * (Luis Galdos)
+ */
+static irqreturn_t s3c2443_spi_irq(int irq, void *_hw)
+{
+	struct s3c2443_spi *hw = _hw;
+	unsigned int spsta = readl(hw->regs + S3C2443_SPI0_STATUS);
+
+	spin_lock(&hw->xmit_lock);
+
+	/* Disable all the interrupts and clear the pending errors */
+	writel(0x00, hw->regs + S3C2443_SPI0_INT_EN);
+	writel(spsta, hw->regs + S3C2443_SPI0_STATUS);
+	writel(0x1f, hw->regs + S3C2443_SPI0_PENDING_CLR);
+	
+	printk_debug("IRQ stat 0x%08x\n", spsta);
+
+	/*
+	 * If we detect an error than force to the complete of the SPI-message by
+	 * setting the number of remaining bytes to zero
+	 */
+	if (spsta &
+	    (S3C2443_SPI0_STUS_RX_OVERRUN_ERR | S3C2443_SPI0_STUS_RX_UNDERRUN_ERR)) {
+		hw->remaining_bytes = 0;
+		printk_err("RX error (0x%08x)\n", spsta);
+	}
+
+	if (spsta &
+	    (S3C2443_SPI0_STUS_TX_OVERRUN_ERR | S3C2443_SPI0_STUS_TX_UNDERRUN_ERR)) {
+		hw->remaining_bytes = 0;
+		printk_err("TX error (0x%08x)\n", spsta);
+	}
+
+	/* Force the complete of the current transfer */
+	tasklet_schedule(&hw->xmit_tasklet);
+	
+	spin_unlock(&hw->xmit_lock);
+	return IRQ_HANDLED;
+}
+
+static int __devinit s3c2443_spi_probe(struct platform_device *pdev)
+{
+	struct s3c2443_spi_info *pdata;
+	struct s3c2443_spi *hw;
+	struct spi_master *master;
+	struct resource *res;
+	int err = 0;
+
+	printk_info("Probing device with the ID %i\n", pdev->id);
+	
+	master = spi_alloc_master(&pdev->dev, sizeof(struct s3c2443_spi));
+	if (master == NULL) {
+		dev_err(&pdev->dev, "No memory for spi_master\n");
+		err = -ENOMEM;
+		goto err_exit;
+	}
+
+	hw = spi_master_get_devdata(master);
+	memset(hw, 0, sizeof(struct s3c2443_spi));
+
+	hw->master = spi_master_get(master);
+	hw->pdata = pdev->dev.platform_data;
+	hw->dev = &pdev->dev;
+
+	pdata = pdev->dev.platform_data;
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "No platform data supplied\n");
+		err = -ENOENT;
+		goto err_put_spi;
+	}
+
+	/* Sanity checks for the passed platform data */
+	hw->pdata = pdata;
+	if (hw->pdata->num_chipselect > 1) {
+		printk_err("Only supports one chip select (%i passed)\n",
+			   hw->pdata->num_chipselect);
+		goto err_put_spi;
+	}
+
+	platform_set_drvdata(pdev, hw);
+
+	/* find and map our resources */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		printk_err("Cannot get IORESOURCE_MEM\n");
+		err = -ENOENT;
+		goto err_put_spi;
+	}
+
+	hw->ioarea = request_mem_region(res->start, (res->end - res->start)+1,
+					pdev->name);
+	if (hw->ioarea == NULL) {
+		printk_err("Cannot reserve region\n");
+		err = -ENXIO;
+		goto err_put_spi;
+	}
+
+	hw->regs = ioremap(res->start, (res->end - res->start) + 1);
+	if (hw->regs == NULL) {
+		printk_err("Cannot map IO\n");
+		err = -ENXIO;
+		goto err_release_iomem;
+	}
+
+	hw->irq = platform_get_irq(pdev, 0);
+	if (hw->irq <= 0) {
+		printk_err("No IRQ specified? Aborting.\n");
+		err = -ENOENT;
+		goto err_unmap_iomem;
+	}
+
+	err = request_irq(hw->irq, s3c2443_spi_irq, 0, pdev->name, hw);
+	if (err) {
+		printk_err("Cannot claim IRQ\n");
+		goto err_unmap_iomem;
+	}
+
+	/* Use the passed input clock */
+	if (pdata->input_clk == S3C2443_HSSPI_INCLK_PCLK)
+		hw->clk = clk_get(&pdev->dev, "pclk");
+	else if (pdata->input_clk == S3C2443_HSSPI_INCLK_EPLL)
+		hw->clk = clk_get(&pdev->dev, "epll");
+	else {
+		printk_err("Invalid input clock passed (%i)\n", pdata->input_clk);
+		goto err_free_irq;
+	}
+	
+	if (IS_ERR(hw->clk)) {
+		printk_err("No clock for device\n");
+		err = PTR_ERR(hw->clk);
+		goto err_free_irq;
+	}
+
+	/* @FIXME: For the moment, permanently enable the clock */
+	clk_enable(hw->clk);
+
+	printk_info("Input clock frequency: %lu Hz\n", clk_get_rate(hw->clk));
+	
+	/* Init the internal data */
+	spin_lock_init(&hw->xmit_lock);
+	INIT_LIST_HEAD(&hw->xmit_queue);
+	
+	/* Create the DMA-pool for DMA-unmapped message */
+	hw->tx_buf = dma_alloc_coherent(&pdev->dev, S3C2443_SPI_DMA_BUFFER,
+					&hw->tx_dma, GFP_DMA);
+	if (!hw->tx_buf) {
+		printk_err("Couldn't get the TX DMA-memory\n");
+		err = -ENOMEM;
+		goto err_put_clk;
+	}
+
+	hw->rx_buf = dma_alloc_coherent(&pdev->dev, S3C2443_SPI_DMA_BUFFER,
+					&hw->rx_dma, GFP_DMA);
+	if (!hw->rx_buf) {
+		printk_err("Couldn't get the RX DMA-memory\n");
+		err = -ENOMEM;
+		goto err_free_txdma;
+	}
+	
+	/* We are safe, init the internal data for receiving the transfer requests */
+	tasklet_init(&hw->xmit_tasklet, s3c2443_spi_next_tasklet,
+		     (unsigned long)hw);
+
+	/* Request the channel for the TX-transfers */
+	hw->dmach.name = (char *)pdev->name;
+	err = s3c2410_dma_request(SPI_TX_CHANNEL, &hw->dmach, hw);
+	if (err < 0) {
+		printk_err("TX DMA channel %i request failed\n.", SPI_TX_CHANNEL);
+		goto err_free_rxdma;
+	}
+	hw->dma_ch = SPI_TX_CHANNEL;
+	printk_debug("Got the TX DMA-channel %i (%i)\n", hw->dma_ch, SPI_TX_CHANNEL);
+
+	/* Request the channel for the RX-transfers */
+	hw->dmach_rx.name = (char *)pdev->name;
+	err = s3c2410_dma_request(SPI_RX_CHANNEL, &hw->dmach_rx, hw);
+	if (err < 0) {
+		printk_err("RX DMA channel %i request failed\n.", SPI_RX_CHANNEL);
+		goto err_free_chtx;
+	}
+	hw->dma_ch_rx = SPI_RX_CHANNEL;
+	printk_debug("Got the RX DMA-channel %i (%i)\n", hw->dma_ch_rx, SPI_RX_CHANNEL);
+		
+	/* Set the callback function for the DMA-channel */
+	s3c2410_dma_set_buffdone_fn(SPI_TX_CHANNEL, NULL);
+	s3c2410_dma_set_opfn(SPI_TX_CHANNEL, NULL);
+	s3c2410_dma_set_buffdone_fn(SPI_RX_CHANNEL, NULL);
+	s3c2410_dma_set_opfn(SPI_RX_CHANNEL, NULL);
+	
+	/* Setup and register the SPI master */
+	master->num_chipselect = pdata->num_chipselect;
+	master->setup          = s3c2443_spi_setup;
+	master->transfer       = s3c2443_spi_transfer;
+	master->bus_num	       = pdata->bus_num;
+	err = spi_register_master(hw->master);
+	if (err) {
+		printk_err("Failed to register the SPI master\n");
+		goto err_free_chrx;
+	}
+
+	/* Init the hardware (reset, enable clock, etc.) */
+	s3c2443_spi_hw_init(hw);
+	
+	return 0;
+
+ err_free_chrx:
+	s3c2410_dma_free(SPI_RX_CHANNEL, &hw->dmach);
+
+ err_free_chtx:
+	s3c2410_dma_free(SPI_TX_CHANNEL, &hw->dmach);
+
+ err_free_rxdma:
+	dma_free_coherent(&pdev->dev, S3C2443_SPI_DMA_BUFFER, hw->rx_buf, hw->rx_dma);
+
+ err_free_txdma:
+	dma_free_coherent(&pdev->dev, S3C2443_SPI_DMA_BUFFER, hw->tx_buf, hw->tx_dma);
+	
+ err_put_clk:
+	clk_disable(hw->clk);
+	clk_put(hw->clk);
+
+ err_free_irq:
+	free_irq(hw->irq, hw);
+
+ err_unmap_iomem:
+	iounmap(hw->regs);
+
+ err_release_iomem:
+	release_resource(hw->ioarea);
+	kfree(hw->ioarea);
+
+ err_put_spi:
+	spi_master_put(hw->master);;
+
+ err_exit:
+	platform_set_drvdata(pdev, NULL);
+	return err;
+}
+
+static int __devexit s3c2443_spi_remove(struct platform_device *pdev)
+{
+	struct s3c2443_spi *hw = platform_get_drvdata(pdev);
+
+	spi_unregister_master(hw->master);
+
+
+	tasklet_kill(&hw->xmit_tasklet);
+	
+	clk_disable(hw->clk);
+	clk_put(hw->clk);
+
+	free_irq(hw->irq, hw);
+	iounmap(hw->regs);
+
+	release_resource(hw->ioarea);
+	kfree(hw->ioarea);
+
+	/* Free the allocated DMA-buffers */
+	dma_free_coherent(&pdev->dev, S3C2443_SPI_DMA_BUFFER, hw->tx_buf, hw->tx_dma);
+	dma_free_coherent(&pdev->dev, S3C2443_SPI_DMA_BUFFER, hw->rx_buf, hw->rx_dma);
+	
+	/* Free the DMA-channels */
+	s3c2410_dma_free(SPI_TX_CHANNEL, &hw->dmach);
+	s3c2410_dma_free(SPI_RX_CHANNEL, &hw->dmach_rx);
+
+	/* Free the SPI-master */
+	spi_master_put(hw->master);
+	
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+
+#ifdef CONFIG_PM
+static int s3c2443_spi_suspend(struct platform_device *pdev, pm_message_t msg)
+{
+	struct s3c2443_spi *hw = platform_get_drvdata(pdev);
+
+	clk_disable(hw->clk);
+	return 0;
+}
+
+static int s3c2443_spi_resume(struct platform_device *pdev)
+{
+	struct s3c2443_spi *hw;
+	int retval;
+
+	hw = platform_get_drvdata(pdev);
+
+        /* @FIXME: Why do we need to free the DMA-channels? */
+	s3c2410_dma_free(SPI_TX_CHANNEL, &hw->dmach);
+	s3c2410_dma_free(SPI_RX_CHANNEL, &hw->dmach_rx);
+
+	retval = s3c2410_dma_request(SPI_TX_CHANNEL, &hw->dmach, hw);
+	if (retval < 0)
+		goto exit_resume;
+	
+	retval = s3c2410_dma_request(SPI_RX_CHANNEL, &hw->dmach_rx, hw);
+	if (retval < 0)
+		goto exit_free_tx;
+	
+	clk_enable(hw->clk);
+	return 0;
+
+exit_free_tx:
+	s3c2410_dma_free(SPI_TX_CHANNEL, &hw->dmach);
+	
+exit_resume:
+	return retval;
+}
+#else
+#define s3c2443_spi_suspend NULL
+#define s3c2443_spi_resume  NULL
+#endif
+
+MODULE_ALIAS("platform:s3c2443-spi");
+
+static struct platform_driver s3c2443_spi_driver = {
+	.probe		= s3c2443_spi_probe,
+	.remove		= __devexit_p(s3c2443_spi_remove),
+	.suspend	= s3c2443_spi_suspend,
+	.resume		= s3c2443_spi_resume,
+	.driver		= {
+		.name	= "spi-s3c2443",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init s3c2443_spi_init(void)
+{
+        return platform_driver_register(&s3c2443_spi_driver);
+}
+
+static void __exit s3c2443_spi_exit(void)
+{
+        platform_driver_unregister(&s3c2443_spi_driver);
+}
+
+module_init(s3c2443_spi_init);
+module_exit(s3c2443_spi_exit);
+
+MODULE_DESCRIPTION("S3C2443 SPI Driver");
+MODULE_AUTHOR("Luis Galdos, <luis.galdos@digi.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi_stmp.c b/drivers/spi/spi_stmp.c
new file mode 100644
index 000000000000..a0ae2087fdbb
--- /dev/null
+++ b/drivers/spi/spi_stmp.c
@@ -0,0 +1,693 @@
+/*
+ * Freescale STMP378X SPI master driver
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/dma-mapping.h>
+#include <linux/errno.h>
+#include <asm/dma.h>
+
+#include <mach/stmp3xxx.h>
+#include <mach/regs-ssp.h>
+#include <mach/regs-apbh.h>
+#include "spi_stmp.h"
+
+/* 0 means DMA modei(recommended, default), !0 - PIO mode */
+static int pio /* = 0 */;
+static int debug;
+
+/**
+ * stmp_spi_init_hw
+ *
+ * Initialize the SSP port
+ */
+static int stmp_spi_init_hw(struct stmp_spi *ss)
+{
+	int err;
+
+	err = stmp37xx_spi_pins_request(ss->master_dev->bus_id, ss->id);
+	if (err)
+		goto out;
+
+	ss->clk = clk_get(NULL, "ssp");
+	if (IS_ERR(ss->clk)) {
+		err = PTR_ERR(ss->clk);
+		goto out_free_pins;
+	}
+	clk_enable(ss->clk);
+
+	stmp3xxx_reset_block(ss->regs, 0);
+	stmp3xxx_dma_reset_channel(ss->dma);
+
+	return 0;
+
+out_free_pins:
+	stmp37xx_spi_pins_release(ss->master_dev->bus_id, ss->id);
+out:
+	return err;
+}
+
+static void stmp_spi_release_hw(struct stmp_spi *ss)
+{
+	if (ss->clk && !IS_ERR(ss->clk)) {
+		clk_disable(ss->clk);
+		clk_put(ss->clk);
+	}
+	stmp37xx_spi_pins_release(ss->master_dev->bus_id, ss->id);
+}
+
+static int stmp_spi_setup_transfer(struct spi_device *spi,
+		struct spi_transfer *t)
+{
+	u8 bits_per_word;
+	u32 hz;
+	struct stmp_spi *ss /* = spi_master_get_devdata(spi->master) */;
+	u16 rate;
+
+	ss = spi_master_get_devdata(spi->master);
+
+	bits_per_word = spi->bits_per_word;
+	if (t && t->bits_per_word)
+		bits_per_word = t->bits_per_word;
+
+	/*
+	  Calculate speed:
+		- by default, use maximum speed from ssp clk
+		- if device overrides it, use it
+		- if transfer specifies other speed, use transfer's one
+	 */
+	hz = 1000 * ss->speed_khz / ss->divider;
+	if (spi->max_speed_hz)
+		hz = min(hz, spi->max_speed_hz);
+	if (t && t->speed_hz)
+		hz = min(hz, t->speed_hz);
+
+	if (hz == 0) {
+		dev_err(&spi->dev, "Cannot continue with zero clock\n");
+		return -EINVAL;
+	}
+
+	if (bits_per_word != 8) {
+		dev_err(&spi->dev, "%s, unsupported bits_per_word=%d\n",
+			__func__, bits_per_word);
+		return -EINVAL;
+	}
+
+	dev_dbg(&spi->dev, "Requested clk rate = %uHz, max = %uHz/%d = %uHz\n",
+		hz, ss->speed_khz, ss->divider,
+		ss->speed_khz * 1000 / ss->divider);
+
+	if (ss->speed_khz * 1000 / ss->divider < hz) {
+		dev_err(&spi->dev, "%s, unsupported clock rate %uHz\n",
+			__func__, hz);
+		return -EINVAL;
+	}
+
+	rate = 1000 * ss->speed_khz/ss->divider/hz;
+
+	HW_SSP_TIMING_WR_NB(ss->regs,
+		BF_SSP_TIMING_CLOCK_DIVIDE(ss->divider) |
+		BF_SSP_TIMING_CLOCK_RATE(rate - 1));
+
+	HW_SSP_CTRL1_WR_NB(ss->regs,
+		BF_SSP_CTRL1_SSP_MODE(BV_SSP_CTRL1_SSP_MODE__SPI) |
+		BF_SSP_CTRL1_WORD_LENGTH(BV_SSP_CTRL1_WORD_LENGTH__EIGHT_BITS) |
+		((spi->mode & SPI_CPOL) ? BM_SSP_CTRL1_POLARITY : 0) |
+		((spi->mode & SPI_CPHA) ? BM_SSP_CTRL1_PHASE : 0) |
+		(pio ? 0 : BM_SSP_CTRL1_DMA_ENABLE));
+
+	HW_SSP_CMD0_SET(0x00);
+
+	return 0;
+}
+
+
+static void stmp_spi_cleanup(struct spi_device *spi)
+{
+	struct stmp37xx_spi_platform_data *pdata = spi->dev.platform_data;
+
+	if (pdata && pdata->hw_release)
+		pdata->hw_release(spi);
+}
+
+/* the spi->mode bits understood by this driver: */
+#define MODEBITS (SPI_CPOL | SPI_CPHA)
+static int stmp_spi_setup(struct spi_device *spi)
+{
+	struct stmp37xx_spi_platform_data *pdata;
+	struct stmp_spi *ss;
+	int err = 0;
+
+	ss = spi_master_get_devdata(spi->master);
+
+	if (!spi->bits_per_word)
+		spi->bits_per_word = 8;
+
+	if (spi->mode & ~MODEBITS) {
+		dev_err(&spi->dev, "%s: unsupported mode bits %x\n",
+			__func__, spi->mode & ~MODEBITS);
+		err = -EINVAL;
+		goto out;
+	}
+
+	dev_dbg(&spi->dev, "%s, mode %d, %u bits/w\n",
+		__func__, spi->mode & MODEBITS, spi->bits_per_word);
+
+	pdata = spi->dev.platform_data;
+
+	if (pdata && pdata->hw_init) {
+		err = pdata->hw_init(spi);
+		if (err)
+			goto out;
+	}
+
+	err = stmp_spi_setup_transfer(spi, NULL);
+	if (err)
+		goto out2;
+	return 0;
+
+out2:
+	if (pdata)
+		pdata->hw_release(spi);
+out:
+	dev_err(&spi->dev, "Failed to setup transfer, error = %d\n", err);
+	return err;
+}
+
+static inline u32 stmp_spi_cs(unsigned cs)
+{
+	return  ((cs & 1) ? BM_SSP_CTRL0_WAIT_FOR_CMD : 0) |
+		((cs & 2) ? BM_SSP_CTRL0_WAIT_FOR_IRQ : 0);
+}
+
+static int stmp_spi_txrx_dma(struct stmp_spi *ss, int cs,
+		unsigned char *buf, dma_addr_t dma_buf, int len,
+		int *first, int *last, int write)
+{
+	u32 c0 = 0;
+	dma_addr_t spi_buf_dma = dma_buf;
+	int count, status = 0;
+	enum dma_data_direction dir = write ? DMA_TO_DEVICE : DMA_FROM_DEVICE;
+
+	c0 |= (*first ? BM_SSP_CTRL0_LOCK_CS : 0);
+	c0 |= (*last ? BM_SSP_CTRL0_IGNORE_CRC : 0);
+	c0 |= (write ? 0 : BM_SSP_CTRL0_READ);
+	c0 |= BM_SSP_CTRL0_DATA_XFER;
+
+	c0 |= stmp_spi_cs(cs);
+
+	c0 |= BF_SSP_CTRL0_XFER_COUNT(len);
+
+	if (!dma_buf)
+		spi_buf_dma = dma_map_single(ss->master_dev, buf, len, dir);
+
+	ss->d.command->cmd =
+		BF_APBH_CHn_CMD_XFER_COUNT(len) |
+		BF_APBH_CHn_CMD_CMDWORDS(1) |
+		BM_APBH_CHn_CMD_WAIT4ENDCMD |
+		BM_APBH_CHn_CMD_IRQONCMPLT  |
+		BF_APBH_CHn_CMD_COMMAND(
+			write ? BV_APBH_CHn_CMD_COMMAND__DMA_READ :
+				BV_APBH_CHn_CMD_COMMAND__DMA_WRITE);
+	ss->d.command->pio_words[0] = c0;
+	ss->d.command->buf_ptr = spi_buf_dma;
+
+	stmp3xxx_dma_reset_channel(ss->dma);
+	stmp3xxx_dma_clear_interrupt(ss->dma);
+	stmp3xxx_dma_enable_interrupt(ss->dma);
+	init_completion(&ss->done);
+	stmp3xxx_dma_go(ss->dma, &ss->d, 1);
+	wait_for_completion(&ss->done);
+	count = 10000;
+	while ((HW_SSP_CTRL0_RD() & BM_SSP_CTRL0_RUN) && count--)
+		continue;
+	if (count <= 0) {
+		printk(KERN_ERR"%c: timeout on line %s:%d\n",
+				write ? 'W':'C', __func__, __LINE__);
+		status = -ETIMEDOUT;
+	}
+
+	if (!dma_buf)
+		dma_unmap_single(ss->master_dev, spi_buf_dma, len, dir);
+
+	return status;
+}
+
+static inline void stmp_spi_enable(struct stmp_spi *ss)
+{
+	HW_SSP_CTRL0_SET_NB(ss->regs, BM_SSP_CTRL0_LOCK_CS);
+	HW_SSP_CTRL0_CLR_NB(ss->regs, BM_SSP_CTRL0_IGNORE_CRC);
+}
+
+static inline void stmp_spi_disable(struct stmp_spi *ss)
+{
+	HW_SSP_CTRL0_CLR_NB(ss->regs, BM_SSP_CTRL0_LOCK_CS);
+	HW_SSP_CTRL0_SET_NB(ss->regs, BM_SSP_CTRL0_IGNORE_CRC);
+}
+
+static int stmp_spi_txrx_pio(struct stmp_spi *ss, int cs,
+		unsigned char *buf, int len,
+		int *first, int *last, int write)
+{
+	int count;
+
+	if (*first) {
+		stmp_spi_enable(ss);
+		*first = 0;
+	}
+
+	HW_SSP_CTRL0_SET(stmp_spi_cs(cs));
+
+	while (len--) {
+		if (*last && len == 0) {
+			stmp_spi_disable(ss);
+			*last = 0;
+		}
+		HW_SSP_CTRL0_CLR_NB(ss->regs, BM_SSP_CTRL0_XFER_COUNT);
+		HW_SSP_CTRL0_SET_NB(ss->regs, 1); /* byte-by-byte */
+
+		if (write)
+			HW_SSP_CTRL0_CLR_NB(ss->regs, BM_SSP_CTRL0_READ);
+		else
+			HW_SSP_CTRL0_SET_NB(ss->regs, BM_SSP_CTRL0_READ);
+
+		/* Run! */
+		HW_SSP_CTRL0_SET_NB(ss->regs, BM_SSP_CTRL0_RUN);
+		count = 10000;
+		while (((HW_SSP_CTRL0_RD() & BM_SSP_CTRL0_RUN) == 0) && count--)
+			continue;
+		if (count <= 0) {
+			printk(KERN_ERR"%c: timeout on line %s:%d\n",
+					write ? 'W':'C', __func__, __LINE__);
+			break;
+		}
+
+		if (write)
+			HW_SSP_DATA_WR_NB(ss->regs, *buf);
+
+		/* Set TRANSFER */
+		HW_SSP_CTRL0_SET_NB(ss->regs, BM_SSP_CTRL0_DATA_XFER);
+
+		if (!write) {
+			count = 10000;
+			while (count-- &&
+				(HW_SSP_STATUS_RD_NB(ss->regs) &
+					BM_SSP_STATUS_FIFO_EMPTY))
+				continue;
+			if (count <= 0) {
+				printk(KERN_ERR"%c: timeout on line %s:%d\n",
+					write ? 'W':'C', __func__, __LINE__);
+				break;
+			}
+			*buf = (HW_SSP_DATA_RD_NB(ss->regs) & 0xFF);
+		}
+
+		count = 10000;
+		while ((HW_SSP_CTRL0_RD() & BM_SSP_CTRL0_RUN) && count--)
+			continue;
+		if (count <= 0) {
+			printk(KERN_ERR"%c: timeout on line %s:%d\n",
+				write ? 'W':'C', __func__, __LINE__);
+			break;
+		}
+
+		/* advance to the next byte */
+		buf++;
+	}
+	return len < 0 ? 0 : -ETIMEDOUT;
+}
+
+static int stmp_spi_handle_message(struct stmp_spi *ss, struct spi_message *m)
+{
+	int first, last;
+	struct spi_transfer *t, *tmp_t;
+	int status = 0;
+	int cs;
+
+	first = last = 0;
+
+	cs = m->spi->chip_select;
+
+	list_for_each_entry_safe(t, tmp_t, &m->transfers, transfer_list) {
+
+		stmp_spi_setup_transfer(m->spi, t);
+
+		if (&t->transfer_list == m->transfers.next)
+			first = !0;
+		if (&t->transfer_list == m->transfers.prev)
+			last = !0;
+		if (t->rx_buf && t->tx_buf) {
+			pr_debug("%s: cannot send and receive simultaneously\n",
+				__func__);
+			return -EINVAL;
+		}
+
+		/*
+		  REVISIT:
+		  here driver completely ignores setting of t->cs_change
+		 */
+		if (t->tx_buf) {
+			status = pio ?
+			   stmp_spi_txrx_pio(ss, cs, (void *)t->tx_buf,
+				   t->len, &first, &last, 1) :
+			   stmp_spi_txrx_dma(ss, cs, (void *)t->tx_buf,
+				   t->tx_dma, t->len, &first, &last, 1);
+			if (debug) {
+				if (t->len < 0x10)
+					print_hex_dump_bytes("Tx ",
+						DUMP_PREFIX_OFFSET,
+						t->tx_buf, t->len);
+				else
+					pr_debug("Tx: %d bytes\n", t->len);
+			}
+		}
+		if (t->rx_buf) {
+			status = pio ?
+			   stmp_spi_txrx_pio(ss, cs, t->rx_buf,
+				   t->len, &first, &last, 0):
+			   stmp_spi_txrx_dma(ss, cs, t->rx_buf,
+				   t->rx_dma, t->len, &first, &last, 0);
+			if (debug) {
+				if (t->len < 0x10)
+					print_hex_dump_bytes("Rx ",
+						DUMP_PREFIX_OFFSET,
+						t->rx_buf, t->len);
+				else
+					pr_debug("Rx: %d bytes\n", t->len);
+			}
+		}
+
+		if (status)
+			break;
+
+		first = last = 0;
+
+	}
+	return status;
+}
+
+/**
+ * stmp_spi_handle
+ *
+ * The workhorse of the driver - it handles messages from the list
+ *
+ **/
+static void stmp_spi_handle(struct work_struct *w)
+{
+	struct stmp_spi *ss = container_of(w, struct stmp_spi, work);
+	unsigned long flags;
+	struct spi_message *m;
+
+	BUG_ON(w == NULL);
+
+	spin_lock_irqsave(&ss->lock, flags);
+	while (!list_empty(&ss->queue)) {
+		m = list_entry(ss->queue.next, struct spi_message, queue);
+		list_del_init(&m->queue);
+		spin_unlock_irqrestore(&ss->lock, flags);
+
+		m->status = stmp_spi_handle_message(ss, m);
+		if (m->complete)
+			m->complete(m->context);
+
+		spin_lock_irqsave(&ss->lock, flags);
+	}
+	spin_unlock_irqrestore(&ss->lock, flags);
+
+	return;
+}
+
+/**
+ * stmp_spi_transfer
+ *
+ * Called indirectly from spi_async, queues all the messages to
+ * spi_handle_message
+ *
+ * @spi: spi device
+ * @m: message to be queued
+**/
+static int stmp_spi_transfer(struct spi_device *spi, struct spi_message *m)
+{
+	struct stmp_spi *ss = spi_master_get_devdata(spi->master);
+	unsigned long flags;
+
+	m->status = -EINPROGRESS;
+	spin_lock_irqsave(&ss->lock, flags);
+	list_add_tail(&m->queue, &ss->queue);
+	queue_work(ss->workqueue, &ss->work);
+	spin_unlock_irqrestore(&ss->lock, flags);
+	return 0;
+}
+
+static irqreturn_t stmp_spi_irq(int irq, void *dev_id)
+{
+	struct stmp_spi *ss = dev_id;
+
+	stmp3xxx_dma_clear_interrupt(ss->dma);
+	complete(&ss->done);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t stmp_spi_irq_err(int irq, void *dev_id)
+{
+	struct stmp_spi *ss = dev_id;
+	u32 c1, st;
+
+	c1 = HW_SSP_CTRL1_RD_NB(ss->regs);
+	st = HW_SSP_STATUS_RD_NB(ss->regs);
+	printk(KERN_ERR"IRQ - ERROR!, status = 0x%08X, c1 = 0x%08X\n", st, c1);
+	HW_SSP_CTRL1_CLR_NB(ss->regs, c1 & 0xCCCC0000);
+
+	return IRQ_HANDLED;
+}
+
+static int __init stmp_spi_probe(struct platform_device *dev)
+{
+	int err = 0;
+	struct spi_master *master;
+	struct stmp_spi *ss;
+	struct resource *r;
+	u32 mem;
+
+	/* Get resources(memory, IRQ) associated with the device */
+	master = spi_alloc_master(&dev->dev, sizeof(struct stmp_spi));
+
+	if (master == NULL) {
+		err = -ENOMEM;
+		goto out0;
+	}
+
+	platform_set_drvdata(dev, master);
+
+	r = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	if (r == NULL) {
+		err = -ENODEV;
+		goto out_put_master;
+	}
+
+	ss = spi_master_get_devdata(master);
+	ss->master_dev = &dev->dev;
+	ss->id = dev->id;
+
+	INIT_WORK(&ss->work, stmp_spi_handle);
+	INIT_LIST_HEAD(&ss->queue);
+	spin_lock_init(&ss->lock);
+	ss->workqueue = create_singlethread_workqueue(dev->dev.bus_id);
+	master->transfer = stmp_spi_transfer;
+	master->setup = stmp_spi_setup;
+	master->cleanup = stmp_spi_cleanup;
+
+	if (!request_mem_region(r->start,
+			r->end - r->start + 1, dev->dev.bus_id)) {
+		err = -ENXIO;
+		goto out_put_master;
+	}
+	mem = r->start;
+
+	ss->regs = r->start;
+
+	ss->irq = platform_get_irq(dev, 0);
+	if (ss->irq < 0) {
+		err = -ENXIO;
+		goto out_put_master;
+	}
+
+	r = platform_get_resource(dev, IORESOURCE_DMA, 0);
+	if (r == NULL) {
+		err = -ENODEV;
+		goto out_put_master;
+	}
+
+	ss->dma = r->start;
+	err = stmp3xxx_dma_request(ss->dma, &dev->dev, dev->dev.bus_id);
+	if (err)
+		goto out_put_master;
+
+	err = stmp3xxx_dma_allocate_command(ss->dma, &ss->d);
+	if (err)
+		goto out_free_dma;
+
+	master->bus_num = dev->id;
+	master->num_chipselect = 1;
+
+	/* SPI controller initializations */
+	err = stmp_spi_init_hw(ss);
+	if (err) {
+		dev_dbg(&dev->dev, "cannot initialize hardware\n");
+		goto out_free_dma_desc;
+	}
+
+	clk_set_rate(ss->clk, 120000);
+	ss->speed_khz = clk_get_rate(ss->clk);
+	ss->divider = 2;
+	dev_info(&dev->dev, "Max possible speed %d = %ld/%d kHz\n",
+		ss->speed_khz, clk_get_rate(ss->clk), ss->divider);
+
+	/* Register for SPI Interrupt */
+	err = request_irq(ss->irq, stmp_spi_irq, 0,
+			dev->dev.bus_id, ss);
+	if (err) {
+		dev_dbg(&dev->dev, "request_irq failed, %d\n", err);
+		goto out_release_hw;
+	}
+	err = request_irq(IRQ_SSP_ERROR, stmp_spi_irq_err, IRQF_SHARED,
+			dev->dev.bus_id, ss);
+	if (err) {
+		dev_dbg(&dev->dev, "request_irq(error) failed, %d\n", err);
+		goto out_free_irq;
+	}
+
+	err = spi_register_master(master);
+	if (err) {
+		dev_dbg(&dev->dev, "cannot register spi master, %d\n", err);
+		goto out_free_irq_2;
+	}
+	dev_info(&dev->dev, "at 0x%08X mapped to 0x%08X, irq=%d, bus %d, %s\n",
+			mem, (u32)ss->regs, ss->irq,
+			master->bus_num, pio ? "PIO" : "DMA");
+	return 0;
+
+out_free_irq_2:
+	free_irq(IRQ_SSP_ERROR, ss);
+out_free_irq:
+	free_irq(ss->irq, ss);
+out_free_dma_desc:
+	stmp3xxx_dma_free_command(ss->dma, &ss->d);
+out_free_dma:
+	stmp3xxx_dma_release(ss->dma);
+out_release_hw:
+	stmp_spi_release_hw(ss);
+out_put_master:
+	spi_master_put(master);
+out0:
+	return err;
+}
+
+static int __devexit stmp_spi_remove(struct platform_device *dev)
+{
+	struct stmp_spi *ss;
+	struct spi_master *master;
+
+	master = platform_get_drvdata(dev);
+	if (master == NULL)
+		goto out0;
+	ss = spi_master_get_devdata(master);
+	if (ss == NULL)
+		goto out1;
+	free_irq(ss->irq, ss);
+	if (ss->workqueue)
+		destroy_workqueue(ss->workqueue);
+	stmp3xxx_dma_free_command(ss->dma, &ss->d);
+	stmp3xxx_dma_release(ss->dma);
+	stmp_spi_release_hw(ss);
+	platform_set_drvdata(dev, 0);
+out1:
+	spi_master_put(master);
+out0:
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int stmp_spi_suspend(struct platform_device *pdev, pm_message_t pmsg)
+{
+	struct stmp_spi *ss;
+	struct spi_master *master;
+
+	master = platform_get_drvdata(pdev);
+	ss = spi_master_get_devdata(master);
+
+	ss->saved_timings = HW_SSP_TIMING_RD_NB(ss->regs);
+	clk_disable(ss->clk);
+
+	return 0;
+}
+
+static int stmp_spi_resume(struct platform_device *pdev)
+{
+	struct stmp_spi *ss;
+	struct spi_master *master;
+
+	master = platform_get_drvdata(pdev);
+	ss = spi_master_get_devdata(master);
+
+	clk_enable(ss->clk);
+	HW_SSP_CTRL0_CLR_NB(ss->regs,
+		BM_SSP_CTRL0_SFTRST | BM_SSP_CTRL0_CLKGATE);
+	HW_SSP_TIMING_SET_NB(ss->regs, ss->saved_timings);
+
+	return 0;
+}
+
+#else
+#define stmp_spi_suspend NULL
+#define stmp_spi_resume  NULL
+#endif
+
+static struct platform_driver stmp_spi_driver = {
+	.probe	= stmp_spi_probe,
+	.remove	= __devexit_p(stmp_spi_remove),
+	.driver = {
+		.name = "stmp37xx_ssp",
+		.owner = THIS_MODULE,
+	},
+	.suspend = stmp_spi_suspend,
+	.resume  = stmp_spi_resume,
+};
+
+static int __init stmp_spi_init(void)
+{
+	return platform_driver_register(&stmp_spi_driver);
+}
+
+static void __exit stmp_spi_exit(void)
+{
+	platform_driver_unregister(&stmp_spi_driver);
+}
+
+module_init(stmp_spi_init);
+module_exit(stmp_spi_exit);
+module_param(pio, int, S_IRUGO);
+module_param(debug, int, S_IRUGO);
+MODULE_AUTHOR("dmitry pervushin <dimka@embeddedalley.com>");
+MODULE_DESCRIPTION("STMP37xx SPI/SSP");
+MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi_stmp.h b/drivers/spi/spi_stmp.h
new file mode 100644
index 000000000000..aef6fac2747e
--- /dev/null
+++ b/drivers/spi/spi_stmp.h
@@ -0,0 +1,49 @@
+/*
+ * Freescale STMP378X SPI master driver
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef __SPI_STMP_H
+#define __SPI_STMP_H
+
+/* These two come from arch/arm/mach-xxxxx/spi.c */
+int stmp37xx_spi_pins_request(char *id, int ssp);
+void stmp37xx_spi_pins_release(char *id, int ssp);
+
+struct stmp_spi {
+	int		id;
+
+	u32		regs;	/* vaddr of the control registers */
+
+	u32		irq;
+	u32		dma;
+	struct stmp3xxx_dma_descriptor d;
+
+	u32		speed_khz;
+	u32		saved_timings;
+	u32		divider;
+
+	struct clk	*clk;
+	struct device	*master_dev;
+
+	struct work_struct work;
+	struct workqueue_struct *workqueue;
+	spinlock_t lock;
+	struct list_head queue;
+
+	struct completion done;
+};
+
+#endif /* __SPI_STMP_H */
diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
index 289d81adfb9c..83babb0a1df7 100644
--- a/drivers/usb/Kconfig
+++ b/drivers/usb/Kconfig
@@ -150,4 +150,6 @@ source "drivers/usb/atm/Kconfig"
 
 source "drivers/usb/gadget/Kconfig"
 
+source "drivers/usb/otg/Kconfig"
+
 endif # USB_SUPPORT
diff --git a/drivers/usb/Makefile b/drivers/usb/Makefile
index 8b7c419b876e..ee6b18a80c03 100644
--- a/drivers/usb/Makefile
+++ b/drivers/usb/Makefile
@@ -37,3 +37,6 @@ obj-$(CONFIG_USB)		+= misc/
 
 obj-$(CONFIG_USB_ATM)		+= atm/
 obj-$(CONFIG_USB_SPEEDTOUCH)	+= atm/
+
+obj-$(CONFIG_USB_OTG)		+= otg/
+
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index e1b42626d04d..3208360664c8 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -117,6 +117,10 @@ static inline int is_root_hub(struct usb_device *udev)
 	return (udev->parent == NULL);
 }
 
+#if CONFIG_PM
+extern int usb_host_wakeup_irq(struct device *wkup_dev);
+extern void usb_host_set_wakeup(struct device *wkup_dev, bool para);
+#endif
 /*-------------------------------------------------------------------------*/
 
 /*
@@ -1725,8 +1729,17 @@ irqreturn_t usb_hcd_irq (int irq, void *__hcd)
 	 */
 	local_irq_save(flags);
 
-	if (unlikely(hcd->state == HC_STATE_HALT ||
-		     !test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags))) {
+	if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
+		/* if receive a remote wakeup interrrupt after suspend */
+		if (usb_host_wakeup_irq(hcd->self.controller)) {
+			/* disable remote wake up irq */
+			usb_host_set_wakeup(hcd->self.controller, false);
+			set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+			hcd->driver->irq(hcd);
+			rc = IRQ_HANDLED;
+		} else
+			rc = IRQ_NONE;
+	} else if (unlikely(hcd->state == HC_STATE_HALT)) {
 		rc = IRQ_NONE;
 	} else if (hcd->driver->irq(hcd) == IRQ_NONE) {
 		rc = IRQ_NONE;
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index b19cbfcd51da..d89645f8f680 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -37,6 +37,15 @@
 #endif
 #endif
 
+#ifdef CONFIG_ARCH_STMP3XXX
+#define STMP3XXX_USB_HOST_HACK
+#endif
+
+#ifdef STMP3XXX_USB_HOST_HACK
+#include <linux/fsl_devices.h>
+#include <mach/regs-usbphy.h>
+#endif
+
 struct usb_hub {
 	struct device		*intfdev;	/* the "interface" device */
 	struct usb_device	*hdev;
@@ -884,6 +893,17 @@ static int hub_configure(struct usb_hub *hub,
 		goto fail;
 	}
 
+	/*
+	 * The second USB-port of the S3C2443 is generating interrupts, although
+	 * it's configured for the USB-device controller, and not for the USB-host.
+	 * This seems to be a hardware BUG.
+	 * (Luis Galdos)
+	 */
+#if (defined(CONFIG_MACH_CC9M2443JS) || defined(CONFIG_MACH_CCW9M2443JS)) && defined(CONFIG_USB_GADGET_S3C2443)
+	/* Only use one port by the internal Root-Hub (devnum = 1) */
+	if (hdev->devnum == 1)
+		hub->descriptor->bNbrPorts = 1;
+#endif
 	hdev->maxchild = hub->descriptor->bNbrPorts;
 	dev_info (hub_dev, "%d port%s detected\n", hdev->maxchild,
 		(hdev->maxchild == 1) ? "" : "s");
@@ -1140,6 +1160,14 @@ static int hub_probe(struct usb_interface *intf, const struct usb_device_id *id)
 				"hub nested too deep\n");
 		return -E2BIG;
 	}
+	
+	/* With OTG enabled, suspending root hub results in gadget not
+	 * working because gadget uses the same root hub. We disable
+	 * this feature when OTG is selected.
+	 */
+#if defined(CONFIG_PM) && defined(CONFIG_USB_EHCI_ARC_OTG)
+	hdev->autosuspend_disabled = 1;
+#endif
 
 #ifdef	CONFIG_USB_OTG_BLACKLIST_HUB
 	if (hdev->parent) {
@@ -2631,6 +2659,19 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 			break;
 		}
 	}
+
+#ifdef STMP3XXX_USB_HOST_HACK
+	{	/*Must enable HOSTDISCONDETECT after second reset*/
+		if (port1 == 1) {
+			if (udev->speed == USB_SPEED_HIGH) {
+				HW_USBPHY_CTRL_SET(
+					BM_USBPHY_CTRL_ENHOSTDISCONDETECT
+				);
+			}
+		}
+	}
+#endif
+
 	if (retval)
 		goto fail;
 
@@ -2755,6 +2796,34 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		"port %d, status %04x, change %04x, %s\n",
 		port1, portstatus, portchange, portspeed (portstatus));
 
+#ifdef STMP3XXX_USB_HOST_HACK
+	{
+	/*
+	 * FIXME: the USBPHY of STMP3xxx SoC has bug. The usb port power
+	 * is never enabled during standard ehci reset procedure if the
+	 * external device once passed plug/unplug procedure. This work-
+	 * around resets and reinitiates USBPHY before the ehci port reset
+	 * sequence started.
+	 */
+		struct device *dev = hcd->self.controller;
+		struct fsl_usb2_platform_data *pdata;
+
+		pdata = (struct fsl_usb2_platform_data *)dev->platform_data;
+		if (dev->parent && dev->type) {
+			if (port1 == 1 && pdata->platform_init)
+				pdata->platform_init(NULL);
+		}
+		if (port1 == 1) {
+			if (!(portstatus&USB_PORT_STAT_CONNECTION)) {
+				/* Must clear HOSTDISCONDETECT when disconnect*/
+				HW_USBPHY_CTRL_CLR(
+					BM_USBPHY_CTRL_ENHOSTDISCONDETECT);
+			}
+		}
+	}
+#endif
+
+
 	if (hub->has_indicators) {
 		set_port_led(hub, port1, HUB_LED_AUTO);
 		hub->indicator[port1-1] = INDICATOR_AUTO;
@@ -2833,6 +2902,7 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		return;
 	}
 
+
 	for (i = 0; i < SET_CONFIG_TRIES; i++) {
 
 		/* reallocate for each attempt, since references
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index dd4cd5a51370..86a74f1c43f1 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -208,17 +208,6 @@ config USB_OMAP
 	default USB_GADGET
 	select USB_GADGET_SELECTED
 
-config USB_OTG
-	boolean "OTG Support"
-	depends on USB_GADGET_OMAP && ARCH_OMAP_OTG && USB_OHCI_HCD
-	help
-	   The most notable feature of USB OTG is support for a
-	   "Dual-Role" device, which can act as either a device
-	   or a host.  The initial role choice can be changed
-	   later, when two dual-role devices talk to each other.
-
-	   Select this only if your OMAP board has a Mini-AB connector.
-
 config USB_GADGET_PXA25X
 	boolean "PXA 25x or IXP 4xx"
 	depends on (ARCH_PXA && PXA25x) || ARCH_IXP4XX
@@ -270,6 +259,21 @@ config USB_PXA27X
 	default USB_GADGET
 	select USB_GADGET_SELECTED
 
+config USB_GADGET_S3C2443
+        boolean "S3C2443 USB Device Controller"
+        depends on S3C2443_USB_PHY_UDC
+        select USB_GADGET_SELECTED
+        help
+          Samsung's S3C2443 with an integrated full speed USB 2.0 device
+          controller.
+
+config USB_S3C2443
+        tristate
+        depends on USB_GADGET_S3C2443
+        default USB_GADGET
+        select USB_GADGET_SELECTED
+        select USB_GADGET_DUALSPEED
+
 config USB_GADGET_S3C2410
 	boolean "S3C2410 USB Device Controller"
 	depends on ARCH_S3C2410
@@ -290,7 +294,7 @@ config USB_S3C2410
 config USB_S3C2410_DEBUG
 	boolean "S3C2410 udc debug messages"
 	depends on USB_GADGET_S3C2410
-
+	
 #
 # Controllers available in both integrated and discrete versions
 #
@@ -419,6 +423,31 @@ config USB_GOKU
 	default USB_GADGET
 	select USB_GADGET_SELECTED
 
+config USB_GADGET_ARC
+	boolean "Freescale USB Device Controller"
+	depends on ARCH_MXC || ARCH_STMP3XXX
+	select USB_GADGET_DUALSPEED if USB_GADGET_FSL_1504 || USB_GADGET_FSL_UTMI
+	help
+	   Some Freescale processors have a USBOTG controller,
+	   which supports device mode.
+
+	   Say "y" to link the driver statically, or "m" to build a
+	   dynamically linked module called "arc_udc" and force all
+	   gadget drivers to also be dynamically linked.
+
+config USB_STATIC_IRAM_PPH
+	bool "Apply static IRAM patch"
+	depends on USB_GADGET_ARC && (ARCH_MX37 || ARCH_MX3 || ARCH_MX25)
+	help
+	   Apply static IRAM patch to peripheral driver.  
+
+config USB_ARC
+	tristate
+	depends on USB_GADGET_ARC
+	default USB_GADGET
+	select USB_GADGET_SELECTED
+
+
 
 #
 # LAST -- dummy/emulated controller
@@ -466,6 +495,69 @@ config USB_GADGET_DUALSPEED
 	  Means that gadget drivers should include extra descriptors
 	  and code to handle dual-speed controllers.
 
+config USB_GADGET_ARC_OTG
+	bool "Support for DR peripheral port on Freescale controller"
+	depends on USB_GADGET_ARC
+	default y
+	help
+	  Enable support for the Freescale Dual Role port in peripheral mode.
+
+choice
+	prompt "Select transceiver for DR port"
+	depends on USB_GADGET_ARC_OTG
+	help
+	  Choose the transceiver to use with the Freescale DR port.
+
+config USB_GADGET_FSL_MC13783
+	bool "Freescale MC13783"
+	depends on !USB_EHCI_FSL_1301 && !USB_EHCI_FSL_1504 && !USB_EHCI_FSL_UTMI && !MACH_MX25_3DS
+	---help---
+	  Enable support for the Full Speed Freescale MC13783 transceiver.
+
+	  The mx27ads, mx31ads and mx32ads boards require modifications
+	  to support this transceiver.
+
+config USB_GADGET_FSL_1301
+	bool "Philips ISP1301"
+	depends on !USB_EHCI_FSL_MC13783 && !USB_EHCI_FSL_1504 && !USB_EHCI_FSL_UTMI && !MACH_MX25_3DS
+	---help---
+	  Enable support for the Full Speed Philips ISP1301 transceiver.
+
+	  This is the factory default for the mx27ads board.
+	  The mx31ads and mx32ads boards require modifications
+	  to support this transceiver.
+
+config USB_GADGET_FSL_1504
+	bool "Philips ISP1504"
+	depends on !USB_EHCI_FSL_MC13783 && !USB_EHCI_FSL_1301 && !USB_EHCI_FSL_UTMI && !MACH_MX25_3DS
+	---help---
+	  Enable support for the High Speed Philips ISP1504 transceiver.
+
+	  This is the factory default for the mx31ads and mx32ads boards.
+	  The mx27ads board requires modifications to support this transceiver.
+
+config USB_GADGET_FSL_UTMI
+	bool "On-chip UTMI"
+	depends on !USB_EHCI_FSL_MC13783 && !USB_EHCI_FSL_1301 && !USB_EHCI_FSL_1504
+	---help---
+	  Enable support for the High Speed Philips ISP1504 transceiver.
+
+	  This is the factory default for the mx35 board.
+
+endchoice
+
+config USB_OTG
+	boolean "OTG Support"
+	depends on (USB_GADGET_OMAP && ARCH_OMAP_OTG && USB_OHCI_HCD) || \
+		   (USB_GADGET_ARC && (ARCH_MXC || ARCH_STMP3XXX) && USB_EHCI_HCD)
+	help
+	   The most notable feature of USB OTG is support for a
+	   "Dual-Role" device, which can act as either a device
+	   or a host.  The initial role choice can be changed
+	   later, when two dual-role devices talk to each other.
+
+	   Select this only if your OMAP board has a Mini-AB connector.
+
 #
 # USB Gadget Drivers
 #
@@ -601,6 +693,12 @@ config USB_FILE_STORAGE
 	  Say "y" to link the driver statically, or "m" to build a
 	  dynamically linked module called "g_file_storage".
 
+config STMP_UTP
+	bool "UTP over Storage Gadget"
+	depends on USB_FILE_STORAGE && ARCH_STMP3XXX
+	help
+	  Freescale's extension to MSC protocol
+
 config USB_FILE_STORAGE_TEST
 	bool "File-backed Storage Gadget testing version"
 	depends on USB_FILE_STORAGE
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index bd4041b47dce..1c6e3c29c0d4 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -14,11 +14,13 @@ obj-$(CONFIG_USB_GOKU)		+= goku_udc.o
 obj-$(CONFIG_USB_OMAP)		+= omap_udc.o
 obj-$(CONFIG_USB_LH7A40X)	+= lh7a40x_udc.o
 obj-$(CONFIG_USB_S3C2410)	+= s3c2410_udc.o
+obj-$(CONFIG_USB_S3C2443)	+= s3c2443_udc.o
 obj-$(CONFIG_USB_AT91)		+= at91_udc.o
 obj-$(CONFIG_USB_ATMEL_USBA)	+= atmel_usba_udc.o
 obj-$(CONFIG_USB_FSL_USB2)	+= fsl_usb2_udc.o
 obj-$(CONFIG_USB_M66592)	+= m66592-udc.o
 obj-$(CONFIG_USB_FSL_QE)	+= fsl_qe_udc.o
+obj-$(CONFIG_USB_ARC)		+= arcotg_udc.o
 
 #
 # USB gadget drivers
diff --git a/drivers/usb/gadget/arcotg_udc.c b/drivers/usb/gadget/arcotg_udc.c
new file mode 100644
index 000000000000..367a9b059196
--- /dev/null
+++ b/drivers/usb/gadget/arcotg_udc.c
@@ -0,0 +1,3045 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#undef DEBUG
+#undef VERBOSE
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/ioport.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/timer.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/proc_fs.h>
+#include <linux/mm.h>
+#include <linux/moduleparam.h>
+#include <linux/device.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/otg.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/fsl_devices.h>
+#include <linux/dmapool.h>
+
+#include <asm/byteorder.h>
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+#include <asm/unaligned.h>
+#include <asm/dma.h>
+#include <asm/cacheflush.h>
+
+#include "arcotg_udc.h"
+#include <mach/arc_otg.h>
+
+#define	DRIVER_DESC	"ARC USBOTG Device Controller driver"
+#define	DRIVER_AUTHOR	"Freescale Semiconductor"
+#define	DRIVER_VERSION	"1 August 2005"
+
+#ifdef CONFIG_PPC_MPC512x
+#define BIG_ENDIAN_DESC
+#endif
+
+#ifdef BIG_ENDIAN_DESC
+#define cpu_to_hc32(x)	(x)
+#define hc32_to_cpu(x)	(x)
+#else
+#define cpu_to_hc32(x)	cpu_to_le32((x))
+#define hc32_to_cpu(x)	le32_to_cpu((x))
+#endif
+
+#define	DMA_ADDR_INVALID	(~(dma_addr_t)0)
+
+static const char driver_name[] = "fsl-usb2-udc";
+static const char driver_desc[] = DRIVER_DESC;
+
+volatile static struct usb_dr_device *dr_regs;
+volatile static struct usb_sys_interface *usb_sys_regs;
+
+/* it is initialized in probe()  */
+static struct fsl_udc *udc_controller;
+
+#ifdef POSTPONE_FREE_LAST_DTD
+static struct ep_td_struct *last_free_td;
+#endif
+static const struct usb_endpoint_descriptor
+fsl_ep0_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	0,
+	.bmAttributes =		USB_ENDPOINT_XFER_CONTROL,
+	.wMaxPacketSize =	USB_MAX_CTRL_PAYLOAD,
+};
+static const size_t g_iram_size = IRAM_TD_PPH_SIZE;
+
+typedef int (*dev_sus)(struct device *dev, pm_message_t state);
+typedef int (*dev_res) (struct device *dev);
+static int udc_suspend(struct fsl_udc *udc);
+static int fsl_udc_suspend(struct platform_device *pdev, pm_message_t state);
+static int fsl_udc_resume(struct platform_device *pdev);
+static void fsl_ep_fifo_flush(struct usb_ep *_ep);
+
+#ifdef CONFIG_USB_OTG
+/* Get platform resource from OTG driver */
+extern struct resource *otg_get_resources(void);
+#endif
+
+extern void fsl_platform_set_test_mode(struct fsl_usb2_platform_data *pdata, enum usb_test_mode mode);
+
+static inline void
+dr_wake_up_enable(struct fsl_udc *udc, bool enable)
+{
+	struct fsl_usb2_platform_data *pdata;
+	pdata = udc->pdata;
+
+	if (device_can_wakeup(udc_controller->gadget.dev.parent)) {
+		if (pdata->wake_up_enable)
+			pdata->wake_up_enable(pdata, enable);
+	}
+}
+
+#ifdef CONFIG_PPC32
+#define fsl_readl(addr)		in_le32((addr))
+#define fsl_writel(addr, val32) out_le32((val32), (addr))
+#else
+#define fsl_readl(addr)		readl((addr))
+#define fsl_writel(addr, val32) writel((addr), (val32))
+#endif
+
+/********************************************************************
+ *	Internal Used Function
+********************************************************************/
+
+#ifdef DUMP_QUEUES
+static void dump_ep_queue(struct fsl_ep *ep)
+{
+	int ep_index;
+	struct fsl_req *req;
+	struct ep_td_struct *dtd;
+
+	if (list_empty(&ep->queue)) {
+		pr_debug("udc: empty\n");
+		return;
+	}
+
+	ep_index = ep_index(ep) * 2 + ep_is_in(ep);
+	pr_debug("udc: ep=0x%p  index=%d\n", ep, ep_index);
+
+	list_for_each_entry(req, &ep->queue, queue) {
+		pr_debug("udc: req=0x%p  dTD count=%d\n", req, req->dtd_count);
+		pr_debug("udc: dTD head=0x%p  tail=0x%p\n", req->head,
+			 req->tail);
+
+		dtd = req->head;
+
+		while (dtd) {
+			if (le32_to_cpu(dtd->next_td_ptr) & DTD_NEXT_TERMINATE)
+				break;	/* end of dTD list */
+
+			dtd = dtd->next_td_virt;
+		}
+	}
+}
+#else
+static inline void dump_ep_queue(struct fsl_ep *ep)
+{
+}
+#endif
+
+/*-----------------------------------------------------------------
+ * done() - retire a request; caller blocked irqs
+ * @status : request status to be set, only works when
+ *	request is still in progress.
+ *--------------------------------------------------------------*/
+static void done(struct fsl_ep *ep, struct fsl_req *req, int status)
+{
+	struct fsl_udc *udc = NULL;
+	unsigned char stopped = ep->stopped;
+	struct ep_td_struct *curr_td, *next_td;
+	int j;
+
+	udc = (struct fsl_udc *)ep->udc;
+	/* Removed the req from fsl_ep->queue */
+	list_del_init(&req->queue);
+
+	/* req.status should be set as -EINPROGRESS in ep_queue() */
+	if (req->req.status == -EINPROGRESS)
+		req->req.status = status;
+	else
+		status = req->req.status;
+
+	/* Free dtd for the request */
+	next_td = req->head;
+	for (j = 0; j < req->dtd_count; j++) {
+		curr_td = next_td;
+		if (j != req->dtd_count - 1) {
+			next_td = curr_td->next_td_virt;
+#ifdef POSTPONE_FREE_LAST_DTD
+			dma_pool_free(udc->td_pool, curr_td, curr_td->td_dma);
+		} else {
+			if (last_free_td != NULL)
+				dma_pool_free(udc->td_pool, last_free_td,
+						last_free_td->td_dma);
+			last_free_td = curr_td;
+		}
+#else
+		}
+
+		dma_pool_free(udc->td_pool, curr_td, curr_td->td_dma);
+#endif
+	}
+
+	if (USE_MSC_WR(req->req.length)) {
+		req->req.dma -= 1;
+		memmove(req->req.buf, req->req.buf + 1, MSC_BULK_CB_WRAP_LEN);
+	}
+
+	if (req->mapped) {
+		dma_unmap_single(ep->udc->gadget.dev.parent,
+			req->req.dma, req->req.length,
+			ep_is_in(ep)
+				? DMA_TO_DEVICE
+				: DMA_FROM_DEVICE);
+		req->req.dma = DMA_ADDR_INVALID;
+		req->mapped = 0;
+	} else
+		dma_sync_single_for_cpu(ep->udc->gadget.dev.parent,
+			req->req.dma, req->req.length,
+			ep_is_in(ep)
+				? DMA_TO_DEVICE
+				: DMA_FROM_DEVICE);
+
+	if (status && (status != -ESHUTDOWN))
+		VDBG("complete %s req %p stat %d len %u/%u",
+			ep->ep.name, &req->req, status,
+			req->req.actual, req->req.length);
+
+	ep->stopped = 1;
+
+	spin_unlock(&ep->udc->lock);
+	/* complete() is from gadget layer,
+	 * eg fsg->bulk_in_complete() */
+	if (req->req.complete)
+		req->req.complete(&ep->ep, &req->req);
+
+	spin_lock(&ep->udc->lock);
+	ep->stopped = stopped;
+}
+
+/*-----------------------------------------------------------------
+ * nuke(): delete all requests related to this ep
+ * called with spinlock held
+ *--------------------------------------------------------------*/
+static void nuke(struct fsl_ep *ep, int status)
+{
+	ep->stopped = 1;
+
+	/* Flush fifo */
+	fsl_ep_fifo_flush(&ep->ep);
+
+	/* Whether this eq has request linked */
+	while (!list_empty(&ep->queue)) {
+		struct fsl_req *req = NULL;
+
+		req = list_entry(ep->queue.next, struct fsl_req, queue);
+		done(ep, req, status);
+	}
+	dump_ep_queue(ep);
+}
+
+/*------------------------------------------------------------------
+	Internal Hardware related function
+ ------------------------------------------------------------------*/
+
+static void dr_phy_low_power_mode(struct fsl_udc *udc, bool enable)
+{
+	u32 temp;
+
+	if (!device_can_wakeup(udc_controller->gadget.dev.parent))
+		return;
+	temp = fsl_readl(&dr_regs->portsc1);
+	if ((enable) && !(temp & PORTSCX_PHY_LOW_POWER_SPD)) {
+		temp |= PORTSCX_PHY_LOW_POWER_SPD;
+		fsl_writel(temp, &dr_regs->portsc1);
+
+		if (udc_controller->pdata->usb_clock_for_pm)
+			udc_controller->pdata->usb_clock_for_pm(false);
+	} else if ((!enable) && (temp & PORTSCX_PHY_LOW_POWER_SPD)) {
+		if (udc_controller->pdata->usb_clock_for_pm)
+			udc_controller->pdata->usb_clock_for_pm(true);
+
+		temp &= ~PORTSCX_PHY_LOW_POWER_SPD;
+		fsl_writel(temp, &dr_regs->portsc1);
+	}
+}
+
+
+
+static int dr_controller_setup(struct fsl_udc *udc)
+{
+	unsigned int tmp = 0, portctrl = 0;
+	unsigned int __attribute((unused)) ctrl = 0;
+	unsigned long timeout;
+	struct fsl_usb2_platform_data *pdata;
+
+#define FSL_UDC_RESET_TIMEOUT 1000
+
+	/* before here, make sure dr_regs has been initialized */
+	if (!udc)
+		return -EINVAL;
+	pdata = udc->pdata;
+
+	/* Stop and reset the usb controller */
+	tmp = fsl_readl(&dr_regs->usbcmd);
+	tmp &= ~USB_CMD_RUN_STOP;
+	fsl_writel(tmp, &dr_regs->usbcmd);
+
+	tmp = fsl_readl(&dr_regs->usbcmd);
+	tmp |= USB_CMD_CTRL_RESET;
+	fsl_writel(tmp, &dr_regs->usbcmd);
+
+	/* Wait for reset to complete */
+	timeout = jiffies + FSL_UDC_RESET_TIMEOUT;
+	while (fsl_readl(&dr_regs->usbcmd) & USB_CMD_CTRL_RESET) {
+		if (time_after(jiffies, timeout)) {
+			ERR("udc reset timeout! \n");
+			return -ETIMEDOUT;
+		}
+		cpu_relax();
+	}
+
+	/* Set the controller as device mode */
+	tmp = fsl_readl(&dr_regs->usbmode);
+	tmp &= ~USB_MODE_CTRL_MODE_MASK;	/* clear mode bits */
+	tmp |= USB_MODE_CTRL_MODE_DEVICE;
+	/* Disable Setup Lockout */
+	tmp |= USB_MODE_SETUP_LOCK_OFF;
+	if (pdata->es)
+		tmp |= USB_MODE_ES;
+	fsl_writel(tmp, &dr_regs->usbmode);
+
+	fsl_platform_set_device_mode(pdata);
+
+	/* Clear the setup status */
+	fsl_writel(0, &dr_regs->usbsts);
+
+	tmp = udc->ep_qh_dma;
+	tmp &= USB_EP_LIST_ADDRESS_MASK;
+	fsl_writel(tmp, &dr_regs->endpointlistaddr);
+
+	VDBG("vir[qh_base] is %p phy[qh_base] is 0x%8x reg is 0x%8x",
+		(int)udc->ep_qh, (int)tmp,
+		fsl_readl(&dr_regs->endpointlistaddr));
+
+	/* Config PHY interface */
+	portctrl = fsl_readl(&dr_regs->portsc1);
+	portctrl &= ~(PORTSCX_PHY_TYPE_SEL | PORTSCX_PORT_WIDTH);
+	switch (udc->phy_mode) {
+	case FSL_USB2_PHY_ULPI:
+		portctrl |= PORTSCX_PTS_ULPI;
+		break;
+	case FSL_USB2_PHY_UTMI_WIDE:
+		portctrl |= PORTSCX_PTW_16BIT;
+		/* fall through */
+	case FSL_USB2_PHY_UTMI:
+		portctrl |= PORTSCX_PTS_UTMI;
+		break;
+	case FSL_USB2_PHY_SERIAL:
+		portctrl |= PORTSCX_PTS_FSLS;
+		break;
+	default:
+		return -EINVAL;
+	}
+	fsl_writel(portctrl, &dr_regs->portsc1);
+
+	if (pdata->change_ahb_burst) {
+		/* if usb should not work in default INCRx mode */
+		tmp = fsl_readl(&dr_regs->sbuscfg);
+		tmp = (tmp & ~0x07) | pdata->ahb_burst_mode;
+		fsl_writel(tmp, &dr_regs->sbuscfg);
+	}
+
+	if (pdata->have_sysif_regs) {
+		/* Config control enable i/o output, cpu endian register */
+		ctrl = __raw_readl(&usb_sys_regs->control);
+		ctrl |= USB_CTRL_IOENB;
+		__raw_writel(ctrl, &usb_sys_regs->control);
+	}
+
+#if defined(CONFIG_PPC32) && !defined(CONFIG_NOT_COHERENT_CACHE)
+	/* Turn on cache snooping hardware, since some PowerPC platforms
+	 * wholly rely on hardware to deal with cache coherent. */
+
+	if (pdata->have_sysif_regs) {
+		/* Setup Snooping for all the 4GB space */
+		tmp = SNOOP_SIZE_2GB;	/* starts from 0x0, size 2G */
+		__raw_writel(tmp, &usb_sys_regs->snoop1);
+		tmp |= 0x80000000;	/* starts from 0x8000000, size 2G */
+		__raw_writel(tmp, &usb_sys_regs->snoop2);
+	}
+#endif
+
+	return 0;
+}
+
+/* Enable DR irq and set controller to run state */
+static void dr_controller_run(struct fsl_udc *udc)
+{
+	u32 temp;
+
+	fsl_platform_pullup_enable(udc->pdata);
+
+	/* Enable DR irq reg */
+	temp = USB_INTR_INT_EN | USB_INTR_ERR_INT_EN
+		| USB_INTR_PTC_DETECT_EN | USB_INTR_RESET_EN
+		| USB_INTR_DEVICE_SUSPEND | USB_INTR_SYS_ERR_EN;
+
+	fsl_writel(temp, &dr_regs->usbintr);
+
+	/* If PHY clock is disabled, enable it */
+	if (udc_controller->pdata->usb_clock_for_pm)
+		udc_controller->pdata->usb_clock_for_pm(1);
+
+	if (device_can_wakeup(udc_controller->gadget.dev.parent)) {
+		/* enable BSV irq */
+		temp = fsl_readl(&dr_regs->otgsc);
+		temp |= OTGSC_B_SESSION_VALID_IRQ_EN;
+		fsl_writel(temp, &dr_regs->otgsc);
+	}
+
+	/* If vbus not on and used low power mode */
+	if (!(fsl_readl(&dr_regs->otgsc) & OTGSC_B_SESSION_VALID)
+	    && device_can_wakeup(udc_controller->gadget.dev.parent)) {
+		/* enable wake up */
+		dr_wake_up_enable(udc, true);
+		/* Set stopped before low power mode */
+		udc->stopped = 1;
+		/* close PHY clock */
+		dr_phy_low_power_mode(udc, true);
+		printk(KERN_INFO "udc enter low power mode \n");
+	} else {
+		/*
+		 add some delay for USB timing issue. USB may be
+		 recognize as FS device
+		 during USB gadget remote wake up function
+		*/
+		mdelay(100);
+		/* Clear stopped bit */
+		udc->stopped = 0;
+		/* Set controller to Run */
+		temp = fsl_readl(&dr_regs->usbcmd);
+		temp |= USB_CMD_RUN_STOP;
+		fsl_writel(temp, &dr_regs->usbcmd);
+		printk(KERN_INFO "udc run \n");
+	}
+
+	return;
+}
+
+static void dr_controller_stop(struct fsl_udc *udc)
+{
+	unsigned int tmp;
+
+	pr_debug("%s\n", __func__);
+
+	/* if we're in OTG mode, and the Host is currently using the port,
+	 * stop now and don't rip the controller out from under the
+	 * ehci driver
+	 */
+	if (udc->gadget.is_otg) {
+		if (!(fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID)) {
+			pr_debug("udc: Leaving early\n");
+			return;
+		}
+	}
+
+	/* disable all INTR */
+	fsl_writel(0, &dr_regs->usbintr);
+
+	/* disable wake up */
+	dr_wake_up_enable(udc, false);
+	/* disable BSV irq */
+	tmp = fsl_readl(&dr_regs->otgsc);
+	tmp &= ~OTGSC_B_SESSION_VALID_IRQ_EN;
+	fsl_writel(tmp, &dr_regs->otgsc);
+
+	/* Set stopped bit for isr */
+	udc->stopped = 1;
+
+	/* disable IO output */
+/*	usb_sys_regs->control = 0; */
+
+	fsl_platform_pullup_disable(udc->pdata);
+
+	/* set controller to Stop */
+	tmp = fsl_readl(&dr_regs->usbcmd);
+	tmp &= ~USB_CMD_RUN_STOP;
+	fsl_writel(tmp, &dr_regs->usbcmd);
+
+	return;
+}
+
+void dr_ep_setup(unsigned char ep_num, unsigned char dir, unsigned char ep_type)
+{
+	unsigned int tmp_epctrl = 0;
+
+	tmp_epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
+	if (dir) {
+		if (ep_num)
+			tmp_epctrl |= EPCTRL_TX_DATA_TOGGLE_RST;
+		tmp_epctrl |= EPCTRL_TX_ENABLE;
+		tmp_epctrl |= ((unsigned int)(ep_type)
+				<< EPCTRL_TX_EP_TYPE_SHIFT);
+	} else {
+		if (ep_num)
+			tmp_epctrl |= EPCTRL_RX_DATA_TOGGLE_RST;
+		tmp_epctrl |= EPCTRL_RX_ENABLE;
+		tmp_epctrl |= ((unsigned int)(ep_type)
+				<< EPCTRL_RX_EP_TYPE_SHIFT);
+	}
+
+	fsl_writel(tmp_epctrl, &dr_regs->endptctrl[ep_num]);
+}
+
+static void
+dr_ep_change_stall(unsigned char ep_num, unsigned char dir, int value)
+{
+	u32 tmp_epctrl = 0;
+
+	tmp_epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
+
+	if (value) {
+		/* set the stall bit */
+		if (dir)
+			tmp_epctrl |= EPCTRL_TX_EP_STALL;
+		else
+			tmp_epctrl |= EPCTRL_RX_EP_STALL;
+	} else {
+		/* clear the stall bit and reset data toggle */
+		if (dir) {
+			tmp_epctrl &= ~EPCTRL_TX_EP_STALL;
+			tmp_epctrl |= EPCTRL_TX_DATA_TOGGLE_RST;
+		} else {
+			tmp_epctrl &= ~EPCTRL_RX_EP_STALL;
+			tmp_epctrl |= EPCTRL_RX_DATA_TOGGLE_RST;
+		}
+	}
+	fsl_writel(tmp_epctrl, &dr_regs->endptctrl[ep_num]);
+}
+
+/* Get stall status of a specific ep
+   Return: 0: not stalled; 1:stalled */
+static int dr_ep_get_stall(unsigned char ep_num, unsigned char dir)
+{
+	u32 epctrl;
+
+	epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
+	if (dir)
+		return (epctrl & EPCTRL_TX_EP_STALL) ? 1 : 0;
+	else
+		return (epctrl & EPCTRL_RX_EP_STALL) ? 1 : 0;
+}
+
+/********************************************************************
+	Internal Structure Build up functions
+********************************************************************/
+
+/*------------------------------------------------------------------
+* struct_ep_qh_setup(): set the Endpoint Capabilites field of QH
+ * @zlt: Zero Length Termination Select (1: disable; 0: enable)
+ * @mult: Mult field
+ ------------------------------------------------------------------*/
+static void struct_ep_qh_setup(struct fsl_udc *udc, unsigned char ep_num,
+		unsigned char dir, unsigned char ep_type,
+		unsigned int max_pkt_len,
+		unsigned int zlt, unsigned char mult)
+{
+	struct ep_queue_head *p_QH = &udc->ep_qh[2 * ep_num + dir];
+	unsigned int tmp = 0;
+
+	/* set the Endpoint Capabilites in QH */
+	switch (ep_type) {
+	case USB_ENDPOINT_XFER_CONTROL:
+		/* Interrupt On Setup (IOS). for control ep  */
+		tmp = (max_pkt_len << EP_QUEUE_HEAD_MAX_PKT_LEN_POS)
+			| EP_QUEUE_HEAD_IOS;
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		tmp = (max_pkt_len << EP_QUEUE_HEAD_MAX_PKT_LEN_POS)
+			| (mult << EP_QUEUE_HEAD_MULT_POS);
+		break;
+	case USB_ENDPOINT_XFER_BULK:
+	case USB_ENDPOINT_XFER_INT:
+		tmp = max_pkt_len << EP_QUEUE_HEAD_MAX_PKT_LEN_POS;
+		break;
+	default:
+		VDBG("error ep type is %d", ep_type);
+		return;
+	}
+	if (zlt)
+		tmp |= EP_QUEUE_HEAD_ZLT_SEL;
+	p_QH->max_pkt_length = cpu_to_hc32(tmp);
+
+	return;
+}
+
+/* Setup qh structure and ep register for ep0. */
+static void ep0_setup(struct fsl_udc *udc)
+{
+	/* the intialization of an ep includes: fields in QH, Regs,
+	 * fsl_ep struct */
+	struct_ep_qh_setup(udc, 0, USB_RECV, USB_ENDPOINT_XFER_CONTROL,
+			USB_MAX_CTRL_PAYLOAD, 0, 0);
+	struct_ep_qh_setup(udc, 0, USB_SEND, USB_ENDPOINT_XFER_CONTROL,
+			USB_MAX_CTRL_PAYLOAD, 0, 0);
+	dr_ep_setup(0, USB_RECV, USB_ENDPOINT_XFER_CONTROL);
+	dr_ep_setup(0, USB_SEND, USB_ENDPOINT_XFER_CONTROL);
+
+	return;
+
+}
+
+/***********************************************************************
+		Endpoint Management Functions
+***********************************************************************/
+
+/*-------------------------------------------------------------------------
+ * when configurations are set, or when interface settings change
+ * for example the do_set_interface() in gadget layer,
+ * the driver will enable or disable the relevant endpoints
+ * ep0 doesn't use this routine. It is always enabled.
+-------------------------------------------------------------------------*/
+static int fsl_ep_enable(struct usb_ep *_ep,
+		const struct usb_endpoint_descriptor *desc)
+{
+	struct fsl_udc *udc = NULL;
+	struct fsl_ep *ep = NULL;
+	unsigned short max = 0;
+	unsigned char mult = 0, zlt;
+	int retval = -EINVAL;
+	unsigned long flags = 0;
+
+	ep = container_of(_ep, struct fsl_ep, ep);
+
+	pr_debug("udc: %s ep.name=%s\n", __func__, ep->ep.name);
+	/* catch various bogus parameters */
+	if (!_ep || !desc || ep->desc
+			|| (desc->bDescriptorType != USB_DT_ENDPOINT))
+		return -EINVAL;
+
+	udc = ep->udc;
+
+	if (!udc->driver || (udc->gadget.speed == USB_SPEED_UNKNOWN))
+		return -ESHUTDOWN;
+
+	max = le16_to_cpu(desc->wMaxPacketSize);
+
+	/* Disable automatic zlp generation.  Driver is reponsible to indicate
+	 * explicitly through req->req.zero.  This is needed to enable multi-td
+	 * request. */
+	zlt = 1;
+
+	/* Assume the max packet size from gadget is always correct */
+	switch (desc->bmAttributes & 0x03) {
+	case USB_ENDPOINT_XFER_CONTROL:
+	case USB_ENDPOINT_XFER_BULK:
+	case USB_ENDPOINT_XFER_INT:
+		/* mult = 0.  Execute N Transactions as demonstrated by
+		 * the USB variable length packet protocol where N is
+		 * computed using the Maximum Packet Length (dQH) and
+		 * the Total Bytes field (dTD) */
+		mult = 0;
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		/* Calculate transactions needed for high bandwidth iso */
+		mult = (unsigned char)(1 + ((max >> 11) & 0x03));
+		max = max & 0x8ff;	/* bit 0~10 */
+		/* 3 transactions at most */
+		if (mult > 3)
+			goto en_done;
+		break;
+	default:
+		goto en_done;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	ep->ep.maxpacket = max;
+	ep->desc = desc;
+	ep->stopped = 0;
+
+	/* Controller related setup */
+	/* Init EPx Queue Head (Ep Capabilites field in QH
+	 * according to max, zlt, mult) */
+	struct_ep_qh_setup(udc, (unsigned char) ep_index(ep),
+			(unsigned char) ((desc->bEndpointAddress & USB_DIR_IN)
+					?  USB_SEND : USB_RECV),
+			(unsigned char) (desc->bmAttributes
+					& USB_ENDPOINT_XFERTYPE_MASK),
+			max, zlt, mult);
+
+	/* Init endpoint ctrl register */
+	dr_ep_setup((unsigned char) ep_index(ep),
+			(unsigned char) ((desc->bEndpointAddress & USB_DIR_IN)
+					? USB_SEND : USB_RECV),
+			(unsigned char) (desc->bmAttributes
+					& USB_ENDPOINT_XFERTYPE_MASK));
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+	retval = 0;
+
+	VDBG("enabled %s (ep%d%s) maxpacket %d", ep->ep.name,
+			ep->desc->bEndpointAddress & 0x0f,
+			(desc->bEndpointAddress & USB_DIR_IN)
+				? "in" : "out", max);
+en_done:
+	return retval;
+}
+
+/*---------------------------------------------------------------------
+ * @ep : the ep being unconfigured. May not be ep0
+ * Any pending and uncomplete req will complete with status (-ESHUTDOWN)
+*---------------------------------------------------------------------*/
+static int fsl_ep_disable(struct usb_ep *_ep)
+{
+	struct fsl_udc *udc = NULL;
+	struct fsl_ep *ep = NULL;
+	unsigned long flags = 0;
+	u32 epctrl;
+	int ep_num;
+
+	ep = container_of(_ep, struct fsl_ep, ep);
+	if (!_ep || !ep->desc) {
+		VDBG("%s not enabled", _ep ? ep->ep.name : NULL);
+		return -EINVAL;
+	}
+
+	/* disable ep on controller */
+	ep_num = ep_index(ep);
+	epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
+	if (ep_is_in(ep))
+		epctrl &= ~EPCTRL_TX_ENABLE;
+	else
+		epctrl &= ~EPCTRL_RX_ENABLE;
+	fsl_writel(epctrl, &dr_regs->endptctrl[ep_num]);
+
+	udc = (struct fsl_udc *)ep->udc;
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* nuke all pending requests (does flush) */
+	nuke(ep, -ESHUTDOWN);
+
+	ep->desc = 0;
+	ep->stopped = 1;
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	VDBG("disabled %s OK", _ep->name);
+	return 0;
+}
+
+/*---------------------------------------------------------------------
+ * allocate a request object used by this endpoint
+ * the main operation is to insert the req->queue to the eq->queue
+ * Returns the request, or null if one could not be allocated
+*---------------------------------------------------------------------*/
+static struct usb_request *
+fsl_alloc_request(struct usb_ep *_ep, gfp_t gfp_flags)
+{
+	struct fsl_req *req = NULL;
+
+	req = kzalloc(sizeof *req, gfp_flags);
+	if (!req)
+		return NULL;
+
+	req->req.dma = DMA_ADDR_INVALID;
+	pr_debug("udc: req=0x%p   set req.dma=0x%x\n", req, req->req.dma);
+	INIT_LIST_HEAD(&req->queue);
+
+	return &req->req;
+}
+
+static void fsl_free_request(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct fsl_req *req = NULL;
+
+	req = container_of(_req, struct fsl_req, req);
+
+	if (_req)
+		kfree(req);
+}
+
+static void update_qh(struct fsl_req *req)
+{
+	struct fsl_ep *ep = req->ep;
+	int i = ep_index(ep) * 2 + ep_is_in(ep);
+	u32 temp;
+	struct ep_queue_head *dQH = &ep->udc->ep_qh[i];
+
+	/* Write dQH next pointer and terminate bit to 0 */
+	temp = req->head->td_dma & EP_QUEUE_HEAD_NEXT_POINTER_MASK;
+	if (NEED_IRAM(req->ep)) {
+		/* set next dtd stop bit,ensure only one dtd in this list */
+		req->cur->next_td_ptr |= cpu_to_hc32(DTD_NEXT_TERMINATE);
+		temp = req->cur->td_dma & EP_QUEUE_HEAD_NEXT_POINTER_MASK;
+	}
+	dQH->next_dtd_ptr = cpu_to_hc32(temp);
+	/* Clear active and halt bit */
+	temp = cpu_to_hc32(~(EP_QUEUE_HEAD_STATUS_ACTIVE
+			     | EP_QUEUE_HEAD_STATUS_HALT));
+	dQH->size_ioc_int_sts &= temp;
+
+	/* Prime endpoint by writing 1 to ENDPTPRIME */
+	temp = ep_is_in(ep)
+	    ? (1 << (ep_index(ep) + 16))
+	    : (1 << (ep_index(ep)));
+	fsl_writel(temp, &dr_regs->endpointprime);
+}
+
+/*-------------------------------------------------------------------------*/
+static int fsl_queue_td(struct fsl_ep *ep, struct fsl_req *req)
+{
+	u32 temp, bitmask, tmp_stat;
+
+	/* VDBG("QH addr Register 0x%8x", dr_regs->endpointlistaddr);
+	VDBG("ep_qh[%d] addr is 0x%8x", i, (u32)&(ep->udc->ep_qh[i])); */
+
+	bitmask = ep_is_in(ep)
+		? (1 << (ep_index(ep) + 16))
+		: (1 << (ep_index(ep)));
+
+	/* check if the pipe is empty */
+	if (!(list_empty(&ep->queue))) {
+		/* Add td to the end */
+		struct fsl_req *lastreq;
+		lastreq = list_entry(ep->queue.prev, struct fsl_req, queue);
+		if (NEED_IRAM(ep)) {
+			/* only one dtd in dqh */
+			lastreq->tail->next_td_ptr =
+			    cpu_to_hc32(req->head->td_dma | DTD_NEXT_TERMINATE);
+			goto out;
+		} else {
+			lastreq->tail->next_td_ptr =
+			    cpu_to_hc32(req->head->td_dma & DTD_ADDR_MASK);
+		}
+		/* Read prime bit, if 1 goto done */
+		if (fsl_readl(&dr_regs->endpointprime) & bitmask)
+			goto out;
+		do {
+			/* Set ATDTW bit in USBCMD */
+			temp = fsl_readl(&dr_regs->usbcmd);
+			fsl_writel(temp | USB_CMD_ATDTW, &dr_regs->usbcmd);
+
+			/* Read correct status bit */
+			tmp_stat = fsl_readl(&dr_regs->endptstatus) & bitmask;
+
+		} while (!(fsl_readl(&dr_regs->usbcmd) & USB_CMD_ATDTW));
+
+		/* Write ATDTW bit to 0 */
+		temp = fsl_readl(&dr_regs->usbcmd);
+		fsl_writel(temp & ~USB_CMD_ATDTW, &dr_regs->usbcmd);
+
+		if (tmp_stat)
+			goto out;
+	}
+	update_qh(req);
+out:
+	return 0;
+}
+
+/* Fill in the dTD structure
+ * @req: request that the transfer belongs to
+ * @length: return actually data length of the dTD
+ * @dma: return dma address of the dTD
+ * @is_last: return flag if it is the last dTD of the request
+ * return: pointer to the built dTD */
+static struct ep_td_struct *fsl_build_dtd(struct fsl_req *req, unsigned *length,
+		dma_addr_t *dma, int *is_last)
+{
+	u32 swap_temp;
+	struct ep_td_struct *dtd;
+
+	/* how big will this transfer be? */
+	*length = min(req->req.length - req->req.actual,
+			(unsigned)EP_MAX_LENGTH_TRANSFER);
+	if (NEED_IRAM(req->ep))
+		*length = min(*length, g_iram_size);
+	dtd = dma_pool_alloc(udc_controller->td_pool, GFP_KERNEL, dma);
+	if (dtd == NULL)
+		return dtd;
+
+	dtd->td_dma = *dma;
+	/* Clear reserved field */
+	swap_temp = hc32_to_cpu(dtd->size_ioc_sts);
+	swap_temp &= ~DTD_RESERVED_FIELDS;
+	dtd->size_ioc_sts = cpu_to_hc32(swap_temp);
+
+	/* Init all of buffer page pointers */
+	swap_temp = (u32) (req->req.dma + req->req.actual);
+	if (NEED_IRAM(req->ep))
+		swap_temp = (u32) (req->req.dma);
+	dtd->buff_ptr0 = cpu_to_hc32(swap_temp);
+	dtd->buff_ptr1 = cpu_to_hc32(swap_temp + 0x1000);
+	dtd->buff_ptr2 = cpu_to_hc32(swap_temp + 0x2000);
+	dtd->buff_ptr3 = cpu_to_hc32(swap_temp + 0x3000);
+	dtd->buff_ptr4 = cpu_to_hc32(swap_temp + 0x4000);
+
+	req->req.actual += *length;
+
+	/* zlp is needed if req->req.zero is set */
+	if (req->req.zero) {
+		if (*length == 0 || (*length % req->ep->ep.maxpacket) != 0)
+			*is_last = 1;
+		else
+			*is_last = 0;
+	} else if (req->req.length == req->req.actual)
+		*is_last = 1;
+	else
+		*is_last = 0;
+
+	if ((*is_last) == 0)
+		VDBG("multi-dtd request!\n");
+	/* Fill in the transfer size; set active bit */
+	swap_temp = ((*length << DTD_LENGTH_BIT_POS) | DTD_STATUS_ACTIVE);
+
+	/* Enable interrupt for the last dtd of a request */
+	if (*is_last && !req->req.no_interrupt)
+		swap_temp |= DTD_IOC;
+	if (NEED_IRAM(req->ep))
+		swap_temp |= DTD_IOC;
+
+	dtd->size_ioc_sts = cpu_to_hc32(swap_temp);
+
+	mb();
+
+	VDBG("length = %d address= 0x%x", *length, (int)*dma);
+
+	return dtd;
+}
+
+/* Generate dtd chain for a request */
+static int fsl_req_to_dtd(struct fsl_req *req)
+{
+	unsigned	count;
+	int		is_last;
+	int		is_first = 1;
+	struct ep_td_struct	*last_dtd = NULL, *dtd;
+	dma_addr_t dma;
+
+	if (NEED_IRAM(req->ep)) {
+		req->oridma = req->req.dma;
+		/* here, replace user buffer to iram buffer */
+		if (ep_is_in(req->ep)) {
+			req->req.dma = req->ep->udc->iram_buffer[1];
+			if ((list_empty(&req->ep->queue))) {
+				/* copy data only when no bulk in transfer is
+				   running */
+				memcpy((char *)req->ep->udc->iram_buffer_v[1],
+				       req->req.buf, min(req->req.length,
+							 g_iram_size));
+			}
+		} else {
+			req->req.dma = req->ep->udc->iram_buffer[0];
+		}
+	}
+
+	if (USE_MSC_WR(req->req.length))
+		req->req.dma += 1;
+
+	do {
+		dtd = fsl_build_dtd(req, &count, &dma, &is_last);
+		if (dtd == NULL)
+			return -ENOMEM;
+
+		if (is_first) {
+			is_first = 0;
+			req->head = dtd;
+		} else {
+			last_dtd->next_td_ptr = cpu_to_hc32(dma);
+			last_dtd->next_td_virt = dtd;
+		}
+		last_dtd = dtd;
+
+		req->dtd_count++;
+	} while (!is_last);
+
+	dtd->next_td_ptr = cpu_to_hc32(DTD_NEXT_TERMINATE);
+	req->cur = req->head;
+	req->tail = dtd;
+
+	return 0;
+}
+
+/* queues (submits) an I/O request to an endpoint */
+static int
+fsl_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
+{
+	struct fsl_ep *ep = container_of(_ep, struct fsl_ep, ep);
+	struct fsl_req *req = container_of(_req, struct fsl_req, req);
+	struct fsl_udc *udc;
+	unsigned long flags;
+	int is_iso = 0;
+
+	/* catch various bogus parameters */
+	if (!_req || !req->req.buf || (ep_index(ep)
+				      && !list_empty(&req->queue))) {
+		VDBG("%s, bad params\n", __func__);
+		return -EINVAL;
+	}
+	if (!_ep || (!ep->desc && ep_index(ep))) {
+		VDBG("%s, bad ep\n", __func__);
+		return -EINVAL;
+	}
+	if (ep->desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
+		if (req->req.length > ep->ep.maxpacket)
+			return -EMSGSIZE;
+		is_iso = 1;
+	}
+
+	udc = ep->udc;
+	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN)
+		return -ESHUTDOWN;
+
+	req->ep = ep;
+
+	/* map virtual address to hardware */
+	if (req->req.dma == DMA_ADDR_INVALID) {
+		req->req.dma = dma_map_single(ep->udc->gadget.dev.parent,
+					req->req.buf,
+					req->req.length, ep_is_in(ep)
+						? DMA_TO_DEVICE
+						: DMA_FROM_DEVICE);
+		req->mapped = 1;
+	} else {
+		dma_sync_single_for_device(ep->udc->gadget.dev.parent,
+					req->req.dma, req->req.length,
+					ep_is_in(ep)
+						? DMA_TO_DEVICE
+						: DMA_FROM_DEVICE);
+		req->mapped = 0;
+	}
+
+	req->req.status = -EINPROGRESS;
+	req->req.actual = 0;
+	req->dtd_count = 0;
+	if (NEED_IRAM(ep)) {
+		req->last_one = 0;
+		req->buffer_offset = 0;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* build dtds and push them to device queue */
+	if (!fsl_req_to_dtd(req)) {
+		fsl_queue_td(ep, req);
+	} else {
+		spin_unlock_irqrestore(&udc->lock, flags);
+		return -ENOMEM;
+	}
+
+	/* irq handler advances the queue */
+	if (req != NULL)
+		list_add_tail(&req->queue, &ep->queue);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/* dequeues (cancels, unlinks) an I/O request from an endpoint */
+static int fsl_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct fsl_ep *ep = container_of(_ep, struct fsl_ep, ep);
+	struct fsl_req *req;
+	unsigned long flags;
+	int ep_num, stopped, ret = 0;
+	u32 epctrl;
+
+	if (!_ep || !_req)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ep->udc->lock, flags);
+	stopped = ep->stopped;
+
+	/* Stop the ep before we deal with the queue */
+	ep->stopped = 1;
+	ep_num = ep_index(ep);
+	epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
+	if (ep_is_in(ep))
+		epctrl &= ~EPCTRL_TX_ENABLE;
+	else
+		epctrl &= ~EPCTRL_RX_ENABLE;
+	fsl_writel(epctrl, &dr_regs->endptctrl[ep_num]);
+
+	/* make sure it's actually queued on this endpoint */
+	list_for_each_entry(req, &ep->queue, queue) {
+		if (&req->req == _req)
+			break;
+	}
+	if (&req->req != _req) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* The request is in progress, or completed but not dequeued */
+	if (ep->queue.next == &req->queue) {
+		_req->status = -ECONNRESET;
+		fsl_ep_fifo_flush(_ep);	/* flush current transfer */
+
+		/* The request isn't the last request in this ep queue */
+		if (req->queue.next != &ep->queue) {
+			struct ep_queue_head *qh;
+			struct fsl_req *next_req;
+
+			qh = ep->qh;
+			next_req = list_entry(req->queue.next, struct fsl_req,
+					queue);
+
+			/* Point the QH to the first TD of next request */
+			fsl_writel((u32) next_req->head, &qh->curr_dtd_ptr);
+		}
+
+		/* The request hasn't been processed, patch up the TD chain */
+	} else {
+		struct fsl_req *prev_req;
+
+		prev_req = list_entry(req->queue.prev, struct fsl_req, queue);
+		fsl_writel(fsl_readl(&req->tail->next_td_ptr),
+				&prev_req->tail->next_td_ptr);
+
+	}
+
+	done(ep, req, -ECONNRESET);
+
+	/* Enable EP */
+out:	epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
+	if (ep_is_in(ep))
+		epctrl |= EPCTRL_TX_ENABLE;
+	else
+		epctrl |= EPCTRL_RX_ENABLE;
+	fsl_writel(epctrl, &dr_regs->endptctrl[ep_num]);
+	ep->stopped = stopped;
+
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+	return ret;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*-----------------------------------------------------------------
+ * modify the endpoint halt feature
+ * @ep: the non-isochronous endpoint being stalled
+ * @value: 1--set halt  0--clear halt
+ * Returns zero, or a negative error code.
+*----------------------------------------------------------------*/
+static int fsl_ep_set_halt(struct usb_ep *_ep, int value)
+{
+	struct fsl_ep *ep = NULL;
+	unsigned long flags = 0;
+	int status = -EOPNOTSUPP;	/* operation not supported */
+	unsigned char ep_dir = 0, ep_num = 0;
+	struct fsl_udc *udc = NULL;
+
+	ep = container_of(_ep, struct fsl_ep, ep);
+	udc = ep->udc;
+	if (!_ep || !ep->desc) {
+		status = -EINVAL;
+		goto out;
+	}
+
+	if (ep->desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
+		status = -EOPNOTSUPP;
+		goto out;
+	}
+
+	/* Attempt to halt IN ep will fail if any transfer requests
+	 * are still queue */
+	if (value && ep_is_in(ep) && !list_empty(&ep->queue)) {
+		status = -EAGAIN;
+		goto out;
+	}
+
+	status = 0;
+	ep_dir = ep_is_in(ep) ? USB_SEND : USB_RECV;
+	ep_num = (unsigned char)(ep_index(ep));
+	spin_lock_irqsave(&ep->udc->lock, flags);
+	dr_ep_change_stall(ep_num, ep_dir, value);
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+
+	if (ep_index(ep) == 0) {
+		udc->ep0_dir = 0;
+	}
+out:
+	VDBG(" %s %s halt stat %d", ep->ep.name,
+			value ?  "set" : "clear", status);
+
+	return status;
+}
+
+static int arcotg_fifo_status(struct usb_ep *_ep)
+{
+	struct fsl_ep *ep;
+	struct fsl_udc *udc;
+	int size = 0;
+	u32 bitmask;
+	struct ep_queue_head *d_qh;
+
+	ep = container_of(_ep, struct fsl_ep, ep);
+	if (!_ep || (!ep->desc && ep_index(ep) != 0))
+		return -ENODEV;
+
+	udc = (struct fsl_udc *)ep->udc;
+
+	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN)
+		return -ESHUTDOWN;
+
+	d_qh = &ep->udc->ep_qh[ep_index(ep) * 2 + ep_is_in(ep)];
+
+	bitmask = (ep_is_in(ep)) ? (1 << (ep_index(ep) + 16)) :
+	    (1 << (ep_index(ep)));
+
+	if (fsl_readl(&dr_regs->endptstatus) & bitmask)
+		size = (d_qh->size_ioc_int_sts & DTD_PACKET_SIZE)
+		    >> DTD_LENGTH_BIT_POS;
+
+	pr_debug("%s %u\n", __func__, size);
+	return size;
+}
+
+static void fsl_ep_fifo_flush(struct usb_ep *_ep)
+{
+	struct fsl_ep *ep;
+	int ep_num, ep_dir;
+	u32 bits;
+	unsigned long timeout;
+#define FSL_UDC_FLUSH_TIMEOUT 1000
+
+	if (!_ep) {
+		return;
+	} else {
+		ep = container_of(_ep, struct fsl_ep, ep);
+		if (!ep->desc)
+			return;
+	}
+	ep_num = ep_index(ep);
+	ep_dir = ep_is_in(ep) ? USB_SEND : USB_RECV;
+
+	if (ep_num == 0)
+		bits = (1 << 16) | 1;
+	else if (ep_dir == USB_SEND)
+		bits = 1 << (16 + ep_num);
+	else
+		bits = 1 << ep_num;
+
+	timeout = jiffies + FSL_UDC_FLUSH_TIMEOUT;
+	do {
+		fsl_writel(bits, &dr_regs->endptflush);
+
+		/* Wait until flush complete */
+		while (fsl_readl(&dr_regs->endptflush)) {
+			if (time_after(jiffies, timeout)) {
+				ERR("ep flush timeout\n");
+				return;
+			}
+			cpu_relax();
+		}
+		/* See if we need to flush again */
+	} while (fsl_readl(&dr_regs->endptstatus) & bits);
+}
+
+static struct usb_ep_ops fsl_ep_ops = {
+	.enable = fsl_ep_enable,
+	.disable = fsl_ep_disable,
+
+	.alloc_request = fsl_alloc_request,
+	.free_request = fsl_free_request,
+
+	.queue = fsl_ep_queue,
+	.dequeue = fsl_ep_dequeue,
+
+	.set_halt = fsl_ep_set_halt,
+	.fifo_status = arcotg_fifo_status,
+	.fifo_flush = fsl_ep_fifo_flush,	/* flush fifo */
+};
+
+/*-------------------------------------------------------------------------
+		Gadget Driver Layer Operations
+-------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------
+ * Get the current frame number (from DR frame_index Reg )
+ *----------------------------------------------------------------------*/
+static int fsl_get_frame(struct usb_gadget *gadget)
+{
+	return (int)(fsl_readl(&dr_regs->frindex) & USB_FRINDEX_MASKS);
+}
+
+/*-----------------------------------------------------------------------
+ * Tries to wake up the host connected to this gadget
+ -----------------------------------------------------------------------*/
+static int fsl_wakeup(struct usb_gadget *gadget)
+{
+	struct fsl_udc *udc = container_of(gadget, struct fsl_udc, gadget);
+	u32 portsc;
+
+	/* Remote wakeup feature not enabled by host */
+	if (!udc->remote_wakeup)
+		return -ENOTSUPP;
+
+	portsc = fsl_readl(&dr_regs->portsc1);
+	/* not suspended? */
+	if (!(portsc & PORTSCX_PORT_SUSPEND))
+		return 0;
+	/* trigger force resume */
+	portsc |= PORTSCX_PORT_FORCE_RESUME;
+	fsl_writel(portsc, &dr_regs->portsc1);
+	return 0;
+}
+
+static int can_pullup(struct fsl_udc *udc)
+{
+	return udc->driver && udc->softconnect && udc->vbus_active;
+}
+
+/* Notify controller that VBUS is powered, Called by whatever
+   detects VBUS sessions */
+static int fsl_vbus_session(struct usb_gadget *gadget, int is_active)
+{
+	struct fsl_udc	*udc;
+	unsigned long	flags;
+
+	udc = container_of(gadget, struct fsl_udc, gadget);
+	spin_lock_irqsave(&udc->lock, flags);
+	VDBG("VBUS %s\n", is_active ? "on" : "off");
+	udc->vbus_active = (is_active != 0);
+	if (can_pullup(udc))
+		fsl_writel((fsl_readl(&dr_regs->usbcmd) | USB_CMD_RUN_STOP),
+				&dr_regs->usbcmd);
+	else
+		fsl_writel((fsl_readl(&dr_regs->usbcmd) & ~USB_CMD_RUN_STOP),
+				&dr_regs->usbcmd);
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return 0;
+}
+
+/* constrain controller's VBUS power usage
+ * This call is used by gadget drivers during SET_CONFIGURATION calls,
+ * reporting how much power the device may consume.  For example, this
+ * could affect how quickly batteries are recharged.
+ *
+ * Returns zero on success, else negative errno.
+ */
+static int fsl_vbus_draw(struct usb_gadget *gadget, unsigned mA)
+{
+	struct fsl_udc *udc;
+
+	udc = container_of(gadget, struct fsl_udc, gadget);
+	if (udc->transceiver)
+		return otg_set_power(udc->transceiver, mA);
+	return -ENOTSUPP;
+}
+
+/* Change Data+ pullup status
+ * this func is used by usb_gadget_connect/disconnet
+ */
+static int fsl_pullup(struct usb_gadget *gadget, int is_on)
+{
+	struct fsl_udc *udc;
+
+	udc = container_of(gadget, struct fsl_udc, gadget);
+	udc->softconnect = (is_on != 0);
+	if (can_pullup(udc))
+		fsl_writel((fsl_readl(&dr_regs->usbcmd) | USB_CMD_RUN_STOP),
+				&dr_regs->usbcmd);
+	else
+		fsl_writel((fsl_readl(&dr_regs->usbcmd) & ~USB_CMD_RUN_STOP),
+				&dr_regs->usbcmd);
+
+	return 0;
+}
+
+/* defined in gadget.h */
+static struct usb_gadget_ops fsl_gadget_ops = {
+	.get_frame = fsl_get_frame,
+	.wakeup = fsl_wakeup,
+/*	.set_selfpowered = fsl_set_selfpowered,	*/ /* Always selfpowered */
+	.vbus_session = fsl_vbus_session,
+	.vbus_draw = fsl_vbus_draw,
+	.pullup = fsl_pullup,
+};
+
+/* Set protocol stall on ep0, protocol stall will automatically be cleared
+   on new transaction */
+static void ep0stall(struct fsl_udc *udc)
+{
+	u32 tmp;
+
+	/* must set tx and rx to stall at the same time */
+	tmp = fsl_readl(&dr_regs->endptctrl[0]);
+	tmp |= EPCTRL_TX_EP_STALL | EPCTRL_RX_EP_STALL;
+	fsl_writel(tmp, &dr_regs->endptctrl[0]);
+	udc->ep0_dir = 0;
+}
+
+/* Prime a status phase for ep0 */
+static int ep0_prime_status(struct fsl_udc *udc, int direction)
+{
+	struct fsl_req *req = udc->status_req;
+	struct fsl_ep *ep;
+	int status = 0;
+
+	if (direction == EP_DIR_IN)
+		udc->ep0_dir = USB_DIR_IN;
+	else
+		udc->ep0_dir = USB_DIR_OUT;
+
+	ep = &udc->eps[0];
+
+	req->ep = ep;
+	req->req.length = 0;
+	req->req.status = -EINPROGRESS;
+
+	status = fsl_ep_queue(&ep->ep, &req->req, GFP_ATOMIC);
+	return status;
+}
+
+static inline int udc_reset_ep_queue(struct fsl_udc *udc, u8 pipe)
+{
+	struct fsl_ep *ep = get_ep_by_pipe(udc, pipe);
+
+	if (!ep->name)
+		return 0;
+
+	nuke(ep, -ESHUTDOWN);
+
+	return 0;
+}
+
+/*
+ * ch9 Set address
+ */
+static void ch9setaddress(struct fsl_udc *udc, u16 value, u16 index, u16 length)
+{
+	/* Save the new address to device struct */
+	udc->device_address = (u8) value;
+	/* Update usb state */
+	udc->usb_state = USB_STATE_ADDRESS;
+	/* Status phase */
+	if (ep0_prime_status(udc, EP_DIR_IN))
+		ep0stall(udc);
+}
+
+/*
+ * ch9 Get status
+ */
+static void ch9getstatus(struct fsl_udc *udc, u8 request_type, u16 value,
+		u16 index, u16 length)
+{
+	u16 tmp = 0;		/* Status, cpu endian */
+
+	struct fsl_req *req;
+	struct fsl_ep *ep;
+	int status = 0;
+
+	ep = &udc->eps[0];
+
+	if ((request_type & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
+		/* Get device status */
+		tmp = 1 << USB_DEVICE_SELF_POWERED;
+		tmp |= udc->remote_wakeup << USB_DEVICE_REMOTE_WAKEUP;
+	} else if ((request_type & USB_RECIP_MASK) == USB_RECIP_INTERFACE) {
+		/* Get interface status */
+		/* We don't have interface information in udc driver */
+		tmp = 0;
+	} else if ((request_type & USB_RECIP_MASK) == USB_RECIP_ENDPOINT) {
+		/* Get endpoint status */
+		struct fsl_ep *target_ep;
+
+		target_ep = get_ep_by_pipe(udc, get_pipe_by_windex(index));
+
+		/* stall if endpoint doesn't exist */
+		if (!target_ep->desc)
+			goto stall;
+		tmp = dr_ep_get_stall(ep_index(target_ep), ep_is_in(target_ep))
+				<< USB_ENDPOINT_HALT;
+	}
+
+	udc->ep0_dir = USB_DIR_IN;
+	/* Borrow the per device data_req */
+	/* status_req had been used to prime status */
+	req = udc->data_req;
+	/* Fill in the reqest structure */
+	*((u16 *) req->req.buf) = cpu_to_le16(tmp);
+	req->ep = ep;
+	req->req.length = 2;
+
+	status = fsl_ep_queue(&ep->ep, &req->req, GFP_ATOMIC);
+	if (status) {
+		udc_reset_ep_queue(udc, 0);
+		ERR("Can't respond to getstatus request \n");
+		goto stall;
+	}
+	return;
+stall:
+	ep0stall(udc);
+
+}
+
+static void setup_received_irq(struct fsl_udc *udc,
+		struct usb_ctrlrequest *setup)
+{
+	u16 wValue = le16_to_cpu(setup->wValue);
+	u16 wIndex = le16_to_cpu(setup->wIndex);
+	u16 wLength = le16_to_cpu(setup->wLength);
+
+	udc_reset_ep_queue(udc, 0);
+
+	if (wLength) {
+		int dir;
+		dir = EP_DIR_IN;
+		if (setup->bRequestType & USB_DIR_IN) {
+			dir = EP_DIR_OUT;
+		}
+		if (ep0_prime_status(udc, dir))
+			ep0stall(udc);
+	}
+	/* We process some stardard setup requests here */
+	switch (setup->bRequest) {
+	case USB_REQ_GET_STATUS:
+		/* Data+Status phase from udc */
+		if ((setup->bRequestType & (USB_DIR_IN | USB_TYPE_MASK))
+					!= (USB_DIR_IN | USB_TYPE_STANDARD))
+			break;
+		ch9getstatus(udc, setup->bRequestType, wValue, wIndex, wLength);
+		return;
+
+	case USB_REQ_SET_ADDRESS:
+		/* Status phase from udc */
+		if (setup->bRequestType != (USB_DIR_OUT | USB_TYPE_STANDARD
+						| USB_RECIP_DEVICE))
+			break;
+		ch9setaddress(udc, wValue, wIndex, wLength);
+		return;
+
+	case USB_REQ_CLEAR_FEATURE:
+	case USB_REQ_SET_FEATURE:
+		/* Status phase from udc */
+	{
+		int rc = -EOPNOTSUPP;
+		u16 ptc = 0;
+
+		if ((setup->bRequestType & (USB_RECIP_MASK | USB_TYPE_MASK))
+				== (USB_RECIP_ENDPOINT | USB_TYPE_STANDARD)) {
+			int pipe = get_pipe_by_windex(wIndex);
+			struct fsl_ep *ep;
+
+			if (wValue != 0 || wLength != 0 || pipe > udc->max_ep)
+				break;
+			ep = get_ep_by_pipe(udc, pipe);
+
+			spin_unlock(&udc->lock);
+			rc = fsl_ep_set_halt(&ep->ep,
+					(setup->bRequest == USB_REQ_SET_FEATURE)
+						? 1 : 0);
+			spin_lock(&udc->lock);
+
+		} else if ((setup->bRequestType & (USB_RECIP_MASK
+				| USB_TYPE_MASK)) == (USB_RECIP_DEVICE
+				| USB_TYPE_STANDARD)) {
+			/* Note: The driver has not include OTG support yet.
+			 * This will be set when OTG support is added */
+			if (setup->wValue == USB_DEVICE_TEST_MODE)
+				ptc = setup->wIndex >> 8;
+			else if (gadget_is_otg(&udc->gadget)) {
+				if (setup->bRequest ==
+				    USB_DEVICE_B_HNP_ENABLE)
+					udc->gadget.b_hnp_enable = 1;
+				else if (setup->bRequest ==
+					 USB_DEVICE_A_HNP_SUPPORT)
+					udc->gadget.a_hnp_support = 1;
+				else if (setup->bRequest ==
+					 USB_DEVICE_A_ALT_HNP_SUPPORT)
+					udc->gadget.a_alt_hnp_support = 1;
+			}
+			rc = 0;
+		} else
+			break;
+
+		if (rc == 0) {
+			if (ep0_prime_status(udc, EP_DIR_IN))
+				ep0stall(udc);
+		}
+		if (ptc) {
+			u32 tmp;
+
+			mdelay(10);
+			fsl_platform_set_test_mode(udc->pdata, ptc);
+			tmp = fsl_readl(&dr_regs->portsc1) | (ptc << 16);
+			fsl_writel(tmp, &dr_regs->portsc1);
+			printk(KERN_INFO "udc: switch to test mode 0x%x.\n", ptc);
+		}
+
+		return;
+	}
+
+	default:
+		break;
+	}
+
+	/* Requests handled by gadget */
+	if (wLength) {
+		/* Data phase from gadget, status phase from udc */
+		udc->ep0_dir = (setup->bRequestType & USB_DIR_IN)
+				?  USB_DIR_IN : USB_DIR_OUT;
+		spin_unlock(&udc->lock);
+		if (udc->driver->setup(&udc->gadget,
+				&udc->local_setup_buff) < 0) {
+			/* cancel status phase */
+			udc_reset_ep_queue(udc, 0);
+			ep0stall(udc);
+		}
+	} else {
+		/* No data phase, IN status from gadget */
+		udc->ep0_dir = USB_DIR_IN;
+		spin_unlock(&udc->lock);
+		if (udc->driver->setup(&udc->gadget,
+				&udc->local_setup_buff) < 0)
+			ep0stall(udc);
+	}
+	spin_lock(&udc->lock);
+}
+
+/* Process request for Data or Status phase of ep0
+ * prime status phase if needed */
+static void ep0_req_complete(struct fsl_udc *udc, struct fsl_ep *ep0,
+		struct fsl_req *req)
+{
+	if (udc->usb_state == USB_STATE_ADDRESS) {
+		/* Set the new address */
+		u32 new_address = (u32) udc->device_address;
+		fsl_writel(new_address << USB_DEVICE_ADDRESS_BIT_POS,
+				&dr_regs->deviceaddr);
+	}
+
+	done(ep0, req, 0);
+}
+
+/* Tripwire mechanism to ensure a setup packet payload is extracted without
+ * being corrupted by another incoming setup packet */
+static void tripwire_handler(struct fsl_udc *udc, u8 ep_num, u8 *buffer_ptr)
+{
+	u32 temp;
+	struct ep_queue_head *qh;
+	struct fsl_usb2_platform_data *pdata = udc->pdata;
+
+	qh = &udc->ep_qh[ep_num * 2 + EP_DIR_OUT];
+
+	/* Clear bit in ENDPTSETUPSTAT */
+	temp = fsl_readl(&dr_regs->endptsetupstat);
+	fsl_writel(temp | (1 << ep_num), &dr_regs->endptsetupstat);
+
+	/* while a hazard exists when setup package arrives */
+	do {
+		/* Set Setup Tripwire */
+		temp = fsl_readl(&dr_regs->usbcmd);
+		fsl_writel(temp | USB_CMD_SUTW, &dr_regs->usbcmd);
+
+		/* Copy the setup packet to local buffer */
+		if (pdata->le_setup_buf) {
+			u32 *p = (u32 *)buffer_ptr;
+			u32 *s = (u32 *)qh->setup_buffer;
+
+			/* Convert little endian setup buffer to CPU endian */
+			*p++ = le32_to_cpu(*s++);
+			*p = le32_to_cpu(*s);
+		} else {
+			memcpy(buffer_ptr, (u8 *) qh->setup_buffer, 8);
+		}
+	} while (!(fsl_readl(&dr_regs->usbcmd) & USB_CMD_SUTW));
+
+	/* Clear Setup Tripwire */
+	temp = fsl_readl(&dr_regs->usbcmd);
+	fsl_writel(temp & ~USB_CMD_SUTW, &dr_regs->usbcmd);
+}
+
+static void iram_process_ep_complete(struct fsl_req *curr_req,
+				     int cur_transfer)
+{
+	char *buf;
+	u32 len;
+	int in = ep_is_in(curr_req->ep);
+
+	if (in)
+		buf = (char *)udc_controller->iram_buffer_v[1];
+	else
+		buf = (char *)udc_controller->iram_buffer_v[0];
+
+	if (curr_req->cur->next_td_ptr == cpu_to_hc32(DTD_NEXT_TERMINATE)
+	    || (cur_transfer < g_iram_size)
+	    || (curr_req->req.length == curr_req->req.actual))
+		curr_req->last_one = 1;
+
+	if (curr_req->last_one) {
+		/* the last transfer */
+		if (!in) {
+			memcpy(curr_req->req.buf + curr_req->buffer_offset, buf,
+			       cur_transfer);
+		}
+		if (curr_req->tail->next_td_ptr !=
+				    cpu_to_hc32(DTD_NEXT_TERMINATE)) {
+			/* have next request,queue it */
+			struct fsl_req *next_req;
+			next_req =
+			    list_entry(curr_req->queue.next,
+				       struct fsl_req, queue);
+			if (in)
+				memcpy(buf, next_req->req.buf,
+				       min(g_iram_size, next_req->req.length));
+			update_qh(next_req);
+		}
+		curr_req->req.dma = curr_req->oridma;
+	} else {
+		/* queue next dtd */
+		/* because had next dtd, so should finish */
+		/* tranferring g_iram_size data */
+		curr_req->buffer_offset += g_iram_size;
+		/* pervious set stop bit,now clear it */
+		curr_req->cur->next_td_ptr &= ~cpu_to_hc32(DTD_NEXT_TERMINATE);
+		curr_req->cur = curr_req->cur->next_td_virt;
+		if (in) {
+			len =
+			    min(curr_req->req.length - curr_req->buffer_offset,
+				g_iram_size);
+			memcpy(buf, curr_req->req.buf + curr_req->buffer_offset,
+			       len);
+		} else {
+			memcpy(curr_req->req.buf + curr_req->buffer_offset -
+			       g_iram_size, buf, g_iram_size);
+		}
+		update_qh(curr_req);
+	}
+}
+
+/* process-ep_req(): free the completed Tds for this req */
+static int process_ep_req(struct fsl_udc *udc, int pipe,
+		struct fsl_req *curr_req)
+{
+	struct ep_td_struct *curr_td;
+	int	td_complete, actual, remaining_length, j, tmp;
+	int	status = 0;
+	int	errors = 0;
+	struct  ep_queue_head *curr_qh = &udc->ep_qh[pipe];
+	int direction = pipe % 2;
+	int total = 0, real_len;
+
+	curr_td = curr_req->head;
+	td_complete = 0;
+	actual = curr_req->req.length;
+	real_len = curr_req->req.length;
+
+	for (j = 0; j < curr_req->dtd_count; j++) {
+		remaining_length = (hc32_to_cpu(curr_td->size_ioc_sts)
+					& DTD_PACKET_SIZE)
+				>> DTD_LENGTH_BIT_POS;
+		if (NEED_IRAM(curr_req->ep)) {
+			if (real_len >= g_iram_size) {
+				actual = g_iram_size;
+				real_len -= g_iram_size;
+			} else {	/* the last packet */
+				actual = real_len;
+				curr_req->last_one = 1;
+			}
+		}
+		actual -= remaining_length;
+		total += actual;
+
+		errors = hc32_to_cpu(curr_td->size_ioc_sts) & DTD_ERROR_MASK;
+		if (errors) {
+			if (errors & DTD_STATUS_HALTED) {
+				ERR("dTD error %08x QH=%d\n", errors, pipe);
+				/* Clear the errors and Halt condition */
+				tmp = hc32_to_cpu(curr_qh->size_ioc_int_sts);
+				tmp &= ~errors;
+				curr_qh->size_ioc_int_sts = cpu_to_hc32(tmp);
+				status = -EPIPE;
+				/* FIXME: continue with next queued TD? */
+
+				break;
+			}
+			if (errors & DTD_STATUS_DATA_BUFF_ERR) {
+				VDBG("Transfer overflow");
+				status = -EPROTO;
+				break;
+			} else if (errors & DTD_STATUS_TRANSACTION_ERR) {
+				VDBG("ISO error");
+				status = -EILSEQ;
+				break;
+			} else
+				ERR("Unknown error has occured (0x%x)!\r\n",
+					errors);
+
+		} else if (hc32_to_cpu(curr_td->size_ioc_sts)
+				& DTD_STATUS_ACTIVE) {
+			VDBG("Request not complete");
+			status = REQ_UNCOMPLETE;
+			return status;
+		} else if (remaining_length) {
+			if (direction) {
+				VDBG("Transmit dTD remaining length not zero");
+				status = -EPROTO;
+				break;
+			} else {
+				td_complete++;
+				break;
+			}
+		} else {
+			td_complete++;
+			VDBG("dTD transmitted successful ");
+		}
+		if (NEED_IRAM(curr_req->ep))
+			if (curr_td->
+			    next_td_ptr & cpu_to_hc32(DTD_NEXT_TERMINATE))
+				break;
+		if (j != curr_req->dtd_count - 1)
+			curr_td = (struct ep_td_struct *)curr_td->next_td_virt;
+	}
+
+	if (status)
+		return status;
+	curr_req->req.actual = total;
+	if (NEED_IRAM(curr_req->ep))
+		iram_process_ep_complete(curr_req, actual);
+	return 0;
+}
+
+/* Process a DTD completion interrupt */
+static void dtd_complete_irq(struct fsl_udc *udc)
+{
+	u32 bit_pos;
+	int i, ep_num, direction, bit_mask, status;
+	struct fsl_ep *curr_ep;
+	struct fsl_req *curr_req, *temp_req;
+
+	/* Clear the bits in the register */
+	bit_pos = fsl_readl(&dr_regs->endptcomplete);
+	fsl_writel(bit_pos, &dr_regs->endptcomplete);
+
+	if (!bit_pos)
+		return;
+
+	for (i = 0; i < udc->max_ep * 2; i++) {
+		ep_num = i >> 1;
+		direction = i % 2;
+
+		bit_mask = 1 << (ep_num + 16 * direction);
+
+		if (!(bit_pos & bit_mask))
+			continue;
+
+		curr_ep = get_ep_by_pipe(udc, i);
+
+		/* If the ep is configured */
+		if (curr_ep->name == NULL) {
+			INFO("Invalid EP?");
+			continue;
+		}
+
+		/* process the req queue until an uncomplete request */
+		list_for_each_entry_safe(curr_req, temp_req, &curr_ep->queue,
+				queue) {
+			status = process_ep_req(udc, i, curr_req);
+
+			VDBG("status of process_ep_req= %d, ep = %d",
+					status, ep_num);
+			if (status == REQ_UNCOMPLETE)
+				break;
+			/* write back status to req */
+			curr_req->req.status = status;
+
+			if (ep_num == 0) {
+				ep0_req_complete(udc, curr_ep, curr_req);
+				break;
+			} else {
+				if (NEED_IRAM(curr_ep)) {
+					if (curr_req->last_one)
+						done(curr_ep, curr_req, status);
+					/* only check the 1th req */
+					break;
+				} else
+					done(curr_ep, curr_req, status);
+			}
+		}
+		dump_ep_queue(curr_ep);
+	}
+}
+
+/* Process a port change interrupt */
+static void port_change_irq(struct fsl_udc *udc)
+{
+	u32 speed;
+
+	if (udc->bus_reset)
+		udc->bus_reset = 0;
+
+	/* Bus resetting is finished */
+	if (!(fsl_readl(&dr_regs->portsc1) & PORTSCX_PORT_RESET)) {
+		/* Get the speed */
+		speed = (fsl_readl(&dr_regs->portsc1)
+				& PORTSCX_PORT_SPEED_MASK);
+		switch (speed) {
+		case PORTSCX_PORT_SPEED_HIGH:
+			udc->gadget.speed = USB_SPEED_HIGH;
+			break;
+		case PORTSCX_PORT_SPEED_FULL:
+			udc->gadget.speed = USB_SPEED_FULL;
+			break;
+		case PORTSCX_PORT_SPEED_LOW:
+			udc->gadget.speed = USB_SPEED_LOW;
+			break;
+		default:
+			udc->gadget.speed = USB_SPEED_UNKNOWN;
+			break;
+		}
+	}
+
+	/* Update USB state */
+	if (!udc->resume_state)
+		udc->usb_state = USB_STATE_DEFAULT;
+}
+
+/* Process suspend interrupt */
+static void suspend_irq(struct fsl_udc *udc)
+{
+	pr_debug("%s\n", __func__);
+
+	udc->resume_state = udc->usb_state;
+	udc->usb_state = USB_STATE_SUSPENDED;
+
+	/* report suspend to the driver, serial.c does not support this */
+	if (udc->driver->suspend)
+		udc->driver->suspend(&udc->gadget);
+}
+
+/* Process Wake up interrupt */
+static void wake_up_irq(struct fsl_udc *udc)
+{
+	pr_debug("%s\n", __func__);
+
+	/* disable wake up irq */
+	dr_wake_up_enable(udc_controller, false);
+	if (udc_controller->pdata->usb_clock_for_pm)
+		udc_controller->pdata->usb_clock_for_pm(true);
+	udc->stopped = 0;
+}
+
+static void bus_resume(struct fsl_udc *udc)
+{
+	udc->usb_state = udc->resume_state;
+	udc->resume_state = 0;
+
+	/* report resume to the driver, serial.c does not support this */
+	if (udc->driver->resume)
+		udc->driver->resume(&udc->gadget);
+}
+
+/* Clear up all ep queues */
+static int reset_queues(struct fsl_udc *udc)
+{
+	u8 pipe;
+
+	for (pipe = 0; pipe < udc->max_pipes; pipe++)
+		udc_reset_ep_queue(udc, pipe);
+
+	/* report disconnect; the driver is already quiesced */
+	udc->driver->disconnect(&udc->gadget);
+
+	return 0;
+}
+
+/* Process reset interrupt */
+static void reset_irq(struct fsl_udc *udc)
+{
+	u32 temp;
+
+	/* Clear the device address */
+	temp = fsl_readl(&dr_regs->deviceaddr);
+	fsl_writel(temp & ~USB_DEVICE_ADDRESS_MASK, &dr_regs->deviceaddr);
+
+	udc->device_address = 0;
+
+	/* Clear usb state */
+	udc->resume_state = 0;
+	udc->ep0_dir = 0;
+	udc->remote_wakeup = 0;	/* default to 0 on reset */
+	udc->gadget.b_hnp_enable = 0;
+	udc->gadget.a_hnp_support = 0;
+	udc->gadget.a_alt_hnp_support = 0;
+
+	/* Clear all the setup token semaphores */
+	temp = fsl_readl(&dr_regs->endptsetupstat);
+	fsl_writel(temp, &dr_regs->endptsetupstat);
+
+	/* Clear all the endpoint complete status bits */
+	temp = fsl_readl(&dr_regs->endptcomplete);
+	fsl_writel(temp, &dr_regs->endptcomplete);
+
+	/* Write 1s to the flush register */
+	fsl_writel(0xffffffff, &dr_regs->endptflush);
+
+	if (fsl_readl(&dr_regs->portsc1) & PORTSCX_PORT_RESET) {
+		VDBG("Bus reset");
+		/* Bus is reseting */
+		udc->bus_reset = 1;
+		/* Reset all the queues, include XD, dTD, EP queue
+		 * head and TR Queue */
+		reset_queues(udc);
+		udc->usb_state = USB_STATE_DEFAULT;
+	} else {
+		VDBG("Controller reset");
+		/* initialize usb hw reg except for regs for EP, not
+		 * touch usbintr reg */
+		dr_controller_setup(udc);
+
+		/* Reset all internal used Queues */
+		reset_queues(udc);
+
+		ep0_setup(udc);
+
+		/* Enable DR IRQ reg, Set Run bit, change udc state */
+		dr_controller_run(udc);
+		udc->usb_state = USB_STATE_ATTACHED;
+	}
+}
+
+/* if wakup udc, return true; else return false*/
+bool try_wake_up_udc(struct fsl_udc *udc)
+{
+	u32 irq_src;
+
+	/* when udc is stopped, only handle wake up irq */
+	if (udc->stopped) {
+		if (!device_can_wakeup(&(udc->pdata->pdev->dev)))
+			return false;
+		/* check to see if wake up irq */
+		irq_src =  fsl_readl(&dr_regs->usbctrl);
+		if (irq_src & USB_CTRL_OTG_WUIR) {
+			wake_up_irq(udc);
+		}
+	}
+
+	if (!device_can_wakeup(udc_controller->gadget.dev.parent))
+		return true;
+
+	/* check if Vbus change irq */
+	irq_src = fsl_readl(&dr_regs->otgsc);
+	if (irq_src & OTGSC_B_SESSION_VALID_IRQ_STS) {
+		u32 tmp;
+		fsl_writel(irq_src, &dr_regs->otgsc);
+		tmp = fsl_readl(&dr_regs->usbcmd);
+		/* check BSV bit to see if fall or rise */
+		if (irq_src & OTGSC_B_SESSION_VALID) {
+			udc->stopped = 0;
+			fsl_writel(tmp | USB_CMD_RUN_STOP, &dr_regs->usbcmd);
+			printk(KERN_INFO "udc out low power mode\n");
+		} else {
+			printk(KERN_INFO "udc enter low power mode \n");
+			fsl_writel(tmp & ~USB_CMD_RUN_STOP, &dr_regs->usbcmd);
+			/* enable wake up */
+			dr_wake_up_enable(udc, true);
+			udc->stopped = 1;
+			/* close USB PHY clock */
+			dr_phy_low_power_mode(udc, true);
+			return false;
+		}
+	}
+
+	return true;
+}
+
+/*
+ * USB device controller interrupt handler
+ */
+static irqreturn_t fsl_udc_irq(int irq, void *_udc)
+{
+	struct fsl_udc *udc = _udc;
+	u32 irq_src;
+	irqreturn_t status = IRQ_NONE;
+	unsigned long flags;
+
+	if (try_wake_up_udc(udc) == false)
+		return IRQ_NONE;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	irq_src = fsl_readl(&dr_regs->usbsts) & fsl_readl(&dr_regs->usbintr);
+	/* Clear notification bits */
+	fsl_writel(irq_src, &dr_regs->usbsts);
+
+	/* VDBG("irq_src [0x%8x]", irq_src); */
+
+	/* Need to resume? */
+	if (udc->usb_state == USB_STATE_SUSPENDED)
+		if ((fsl_readl(&dr_regs->portsc1) & PORTSCX_PORT_SUSPEND) == 0)
+			bus_resume(udc);
+
+	/* USB Interrupt */
+	if (irq_src & USB_STS_INT) {
+		VDBG("Packet int");
+		/* Setup package, we only support ep0 as control ep */
+		if (fsl_readl(&dr_regs->endptsetupstat) & EP_SETUP_STATUS_EP0) {
+			tripwire_handler(udc, 0,
+					(u8 *) (&udc->local_setup_buff));
+			setup_received_irq(udc, &udc->local_setup_buff);
+			status = IRQ_HANDLED;
+		}
+
+		/* completion of dtd */
+		if (fsl_readl(&dr_regs->endptcomplete)) {
+			dtd_complete_irq(udc);
+			status = IRQ_HANDLED;
+		}
+	}
+
+	/* SOF (for ISO transfer) */
+	if (irq_src & USB_STS_SOF) {
+		status = IRQ_HANDLED;
+	}
+
+	/* Port Change */
+	if (irq_src & USB_STS_PORT_CHANGE) {
+		port_change_irq(udc);
+		status = IRQ_HANDLED;
+	}
+
+	/* Reset Received */
+	if (irq_src & USB_STS_RESET) {
+		VDBG("reset int");
+		reset_irq(udc);
+		status = IRQ_HANDLED;
+	}
+
+	/* Sleep Enable (Suspend) */
+	if (irq_src & USB_STS_SUSPEND) {
+		suspend_irq(udc);
+		status = IRQ_HANDLED;
+	}
+
+	if (irq_src & (USB_STS_ERR | USB_STS_SYS_ERR)) {
+		VDBG("Error IRQ %x ", irq_src);
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return status;
+}
+
+/*----------------------------------------------------------------*
+ * Hook to gadget drivers
+ * Called by initialization code of gadget drivers
+*----------------------------------------------------------------*/
+int usb_gadget_register_driver(struct usb_gadget_driver *driver)
+{
+	int retval = -ENODEV;
+	unsigned long flags = 0;
+
+	if (!udc_controller)
+		return -ENODEV;
+
+	if (!driver || (driver->speed != USB_SPEED_FULL
+				&& driver->speed != USB_SPEED_HIGH)
+			|| !driver->bind || !driver->disconnect
+			|| !driver->setup)
+		return -EINVAL;
+
+	if (udc_controller->driver)
+		return -EBUSY;
+
+	/* lock is needed but whether should use this lock or another */
+	spin_lock_irqsave(&udc_controller->lock, flags);
+
+	driver->driver.bus = 0;
+	/* hook up the driver */
+	udc_controller->driver = driver;
+	udc_controller->gadget.dev.driver = &driver->driver;
+	spin_unlock_irqrestore(&udc_controller->lock, flags);
+
+	dr_phy_low_power_mode(udc_controller, false);
+	/* bind udc driver to gadget driver */
+	retval = driver->bind(&udc_controller->gadget);
+	if (retval) {
+		VDBG("bind to %s --> %d", driver->driver.name, retval);
+		udc_controller->gadget.dev.driver = 0;
+		udc_controller->driver = 0;
+		goto out;
+	}
+
+	if (udc_controller->transceiver) {
+		/* Suspend the controller until OTG enable it */
+		udc_controller->stopped = 1;
+		printk(KERN_INFO "Suspend udc for OTG auto detect\n");
+		dr_wake_up_enable(udc_controller, true);
+		dr_phy_low_power_mode(udc_controller, true);
+
+		/* export udc suspend/resume call to OTG */
+		udc_controller->gadget.dev.driver->suspend = (dev_sus)fsl_udc_suspend;
+		udc_controller->gadget.dev.driver->resume = (dev_res)fsl_udc_resume;
+
+		/* connect to bus through transceiver */
+		if (udc_controller->transceiver) {
+			retval = otg_set_peripheral(udc_controller->transceiver,
+						    &udc_controller->gadget);
+			if (retval < 0) {
+				ERR("can't bind to transceiver\n");
+				driver->unbind(&udc_controller->gadget);
+				udc_controller->gadget.dev.driver = 0;
+				udc_controller->driver = 0;
+				return retval;
+			}
+		}
+	} else {
+		/* Enable DR IRQ reg and Set usbcmd reg  Run bit */
+		dr_controller_run(udc_controller);
+		udc_controller->usb_state = USB_STATE_ATTACHED;
+		udc_controller->ep0_dir = 0;
+	}
+	printk(KERN_INFO "%s: bind to driver %s \n",
+			udc_controller->gadget.name, driver->driver.name);
+
+out:
+	if (retval)
+		printk(KERN_DEBUG "retval %d \n", retval);
+	return retval;
+}
+EXPORT_SYMBOL(usb_gadget_register_driver);
+
+/* Disconnect from gadget driver */
+int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
+{
+	struct fsl_ep *loop_ep;
+	unsigned long flags;
+
+	if (!udc_controller)
+		return -ENODEV;
+
+	if (!driver || driver != udc_controller->driver || !driver->unbind)
+		return -EINVAL;
+
+	if (udc_controller->transceiver)
+		(void)otg_set_peripheral(udc_controller->transceiver, 0);
+
+	/* stop DR, disable intr */
+	dr_controller_stop(udc_controller);
+	/* open phy clock for following operation */
+	dr_phy_low_power_mode(udc_controller, false);
+
+	/* in fact, no needed */
+	udc_controller->usb_state = USB_STATE_ATTACHED;
+	udc_controller->ep0_dir = 0;
+
+	/* stand operation */
+	spin_lock_irqsave(&udc_controller->lock, flags);
+	udc_controller->gadget.speed = USB_SPEED_UNKNOWN;
+	nuke(&udc_controller->eps[0], -ESHUTDOWN);
+	list_for_each_entry(loop_ep, &udc_controller->gadget.ep_list,
+			ep.ep_list)
+		nuke(loop_ep, -ESHUTDOWN);
+	spin_unlock_irqrestore(&udc_controller->lock, flags);
+
+	/* disconnect gadget before unbinding */
+	driver->disconnect(&udc_controller->gadget);
+
+	/* unbind gadget and unhook driver. */
+	driver->unbind(&udc_controller->gadget);
+	udc_controller->gadget.dev.driver = 0;
+	udc_controller->driver = 0;
+
+	dr_wake_up_enable(udc_controller, false);
+	dr_phy_low_power_mode(udc_controller, true);
+
+	printk(KERN_INFO "unregistered gadget driver '%s'\r\n",
+	       driver->driver.name);
+	return 0;
+}
+EXPORT_SYMBOL(usb_gadget_unregister_driver);
+
+/*-------------------------------------------------------------------------
+		PROC File System Support
+-------------------------------------------------------------------------*/
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+
+#include <linux/seq_file.h>
+
+static const char proc_filename[] = "driver/fsl_usb2_udc";
+
+static int fsl_proc_read(char *page, char **start, off_t off, int count,
+		int *eof, void *_dev)
+{
+	char *buf = page;
+	char *next = buf;
+	unsigned size = count;
+	unsigned long flags;
+	int t, i;
+	u32 tmp_reg;
+	struct fsl_ep *ep = NULL;
+	struct fsl_req *req;
+	struct fsl_usb2_platform_data *pdata;
+
+	struct fsl_udc *udc = udc_controller;
+	pdata = udc->pdata;
+	if (off != 0)
+		return 0;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* ------basic driver infomation ---- */
+	t = scnprintf(next, size,
+			DRIVER_DESC "\n"
+			"%s version: %s\n"
+			"Gadget driver: %s\n\n",
+			driver_name, DRIVER_VERSION,
+			udc->driver ? udc->driver->driver.name : "(none)");
+	size -= t;
+	next += t;
+
+	/* ------ DR Registers ----- */
+	tmp_reg = fsl_readl(&dr_regs->usbcmd);
+	t = scnprintf(next, size,
+			"USBCMD reg:\n"
+			"SetupTW: %d\n"
+			"Run/Stop: %s\n\n",
+			(tmp_reg & USB_CMD_SUTW) ? 1 : 0,
+			(tmp_reg & USB_CMD_RUN_STOP) ? "Run" : "Stop");
+	size -= t;
+	next += t;
+
+	tmp_reg = fsl_readl(&dr_regs->usbsts);
+	t = scnprintf(next, size,
+			"USB Status Reg:\n"
+			"Dr Suspend: %d" "Reset Received: %d" "System Error: %s"
+			"USB Error Interrupt: %s\n\n",
+			(tmp_reg & USB_STS_SUSPEND) ? 1 : 0,
+			(tmp_reg & USB_STS_RESET) ? 1 : 0,
+			(tmp_reg & USB_STS_SYS_ERR) ? "Err" : "Normal",
+			(tmp_reg & USB_STS_ERR) ? "Err detected" : "No err");
+	size -= t;
+	next += t;
+
+	tmp_reg = fsl_readl(&dr_regs->usbintr);
+	t = scnprintf(next, size,
+			"USB Intrrupt Enable Reg:\n"
+			"Sleep Enable: %d" "SOF Received Enable: %d"
+			"Reset Enable: %d\n"
+			"System Error Enable: %d"
+			"Port Change Dectected Enable: %d\n"
+			"USB Error Intr Enable: %d" "USB Intr Enable: %d\n\n",
+			(tmp_reg & USB_INTR_DEVICE_SUSPEND) ? 1 : 0,
+			(tmp_reg & USB_INTR_SOF_EN) ? 1 : 0,
+			(tmp_reg & USB_INTR_RESET_EN) ? 1 : 0,
+			(tmp_reg & USB_INTR_SYS_ERR_EN) ? 1 : 0,
+			(tmp_reg & USB_INTR_PTC_DETECT_EN) ? 1 : 0,
+			(tmp_reg & USB_INTR_ERR_INT_EN) ? 1 : 0,
+			(tmp_reg & USB_INTR_INT_EN) ? 1 : 0);
+	size -= t;
+	next += t;
+
+	tmp_reg = fsl_readl(&dr_regs->frindex);
+	t = scnprintf(next, size,
+			"USB Frame Index Reg:" "Frame Number is 0x%x\n\n",
+			(tmp_reg & USB_FRINDEX_MASKS));
+	size -= t;
+	next += t;
+
+	tmp_reg = fsl_readl(&dr_regs->deviceaddr);
+	t = scnprintf(next, size,
+			"USB Device Address Reg:" "Device Addr is 0x%x\n\n",
+			(tmp_reg & USB_DEVICE_ADDRESS_MASK));
+	size -= t;
+	next += t;
+
+	tmp_reg = fsl_readl(&dr_regs->endpointlistaddr);
+	t = scnprintf(next, size,
+			"USB Endpoint List Address Reg:"
+			"Device Addr is 0x%x\n\n",
+			(tmp_reg & USB_EP_LIST_ADDRESS_MASK));
+	size -= t;
+	next += t;
+
+	tmp_reg = fsl_readl(&dr_regs->portsc1);
+	t = scnprintf(next, size,
+		"USB Port Status&Control Reg:\n"
+		"Port Transceiver Type : %s" "Port Speed: %s \n"
+		"PHY Low Power Suspend: %s" "Port Reset: %s"
+		"Port Suspend Mode: %s \n" "Over-current Change: %s"
+		"Port Enable/Disable Change: %s\n"
+		"Port Enabled/Disabled: %s"
+		"Current Connect Status: %s\n\n", ({
+			char *s;
+			switch (tmp_reg & PORTSCX_PTS_FSLS) {
+			case PORTSCX_PTS_UTMI:
+				s = "UTMI"; break;
+			case PORTSCX_PTS_ULPI:
+				s = "ULPI "; break;
+			case PORTSCX_PTS_FSLS:
+				s = "FS/LS Serial"; break;
+			default:
+				s = "None"; break;
+			}
+			s; }), ({
+			char *s;
+			switch (tmp_reg & PORTSCX_PORT_SPEED_UNDEF) {
+			case PORTSCX_PORT_SPEED_FULL:
+				s = "Full Speed"; break;
+			case PORTSCX_PORT_SPEED_LOW:
+				s = "Low Speed"; break;
+			case PORTSCX_PORT_SPEED_HIGH:
+				s = "High Speed"; break;
+			default:
+				s = "Undefined"; break;
+			}
+			s;
+		}),
+		(tmp_reg & PORTSCX_PHY_LOW_POWER_SPD) ?
+		"Normal PHY mode" : "Low power mode",
+		(tmp_reg & PORTSCX_PORT_RESET) ? "In Reset" :
+		"Not in Reset",
+		(tmp_reg & PORTSCX_PORT_SUSPEND) ? "In " : "Not in",
+		(tmp_reg & PORTSCX_OVER_CURRENT_CHG) ? "Dected" :
+		"No",
+		(tmp_reg & PORTSCX_PORT_EN_DIS_CHANGE) ? "Disable" :
+		"Not change",
+		(tmp_reg & PORTSCX_PORT_ENABLE) ? "Enable" :
+		"Not correct",
+		(tmp_reg & PORTSCX_CURRENT_CONNECT_STATUS) ?
+		"Attached" : "Not-Att");
+	size -= t;
+	next += t;
+
+	tmp_reg = fsl_readl(&dr_regs->usbmode);
+	t = scnprintf(next, size,
+			"USB Mode Reg:" "Controller Mode is : %s\n\n", ({
+				char *s;
+				switch (tmp_reg & USB_MODE_CTRL_MODE_HOST) {
+				case USB_MODE_CTRL_MODE_IDLE:
+					s = "Idle"; break;
+				case USB_MODE_CTRL_MODE_DEVICE:
+					s = "Device Controller"; break;
+				case USB_MODE_CTRL_MODE_HOST:
+					s = "Host Controller"; break;
+				default:
+					s = "None"; break;
+				}
+				s;
+			}));
+	size -= t;
+	next += t;
+
+	tmp_reg = fsl_readl(&dr_regs->endptsetupstat);
+	t = scnprintf(next, size,
+			"Endpoint Setup Status Reg:" "SETUP on ep 0x%x\n\n",
+			(tmp_reg & EP_SETUP_STATUS_MASK));
+	size -= t;
+	next += t;
+
+	for (i = 0; i < udc->max_ep / 2; i++) {
+		tmp_reg = fsl_readl(&dr_regs->endptctrl[i]);
+		t = scnprintf(next, size, "EP Ctrl Reg [0x%x]: = [0x%x]\n",
+				i, tmp_reg);
+		size -= t;
+		next += t;
+	}
+	tmp_reg = fsl_readl(&dr_regs->endpointprime);
+	t = scnprintf(next, size, "EP Prime Reg = [0x%x]\n", tmp_reg);
+	size -= t;
+	next += t;
+
+	if (pdata->have_sysif_regs) {
+		tmp_reg = usb_sys_regs->snoop1;
+		t = scnprintf(next, size, "\nSnoop1 Reg = [0x%x]\n\n", tmp_reg);
+		size -= t;
+		next += t;
+
+		tmp_reg = usb_sys_regs->control;
+		t = scnprintf(next, size, "General Control Reg = [0x%x]\n\n",
+				tmp_reg);
+		size -= t;
+		next += t;
+	}
+
+	/* ------fsl_udc, fsl_ep, fsl_request structure information ----- */
+	ep = &udc->eps[0];
+	t = scnprintf(next, size, "For %s Maxpkt is 0x%x index is 0x%x\n",
+			ep->ep.name, ep_maxpacket(ep), ep_index(ep));
+	size -= t;
+	next += t;
+
+	if (list_empty(&ep->queue)) {
+		t = scnprintf(next, size, "its req queue is empty\n\n");
+		size -= t;
+		next += t;
+	} else {
+		list_for_each_entry(req, &ep->queue, queue) {
+			t = scnprintf(next, size,
+				"req %p actual 0x%x length 0x%x  buf %p\n",
+				&req->req, req->req.actual,
+				req->req.length, req->req.buf);
+			size -= t;
+			next += t;
+		}
+	}
+	/* other gadget->eplist ep */
+	list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list) {
+		if (ep->desc) {
+			t = scnprintf(next, size,
+					"\nFor %s Maxpkt is 0x%x "
+					"index is 0x%x\n",
+					ep->ep.name, ep_maxpacket(ep),
+					ep_index(ep));
+			size -= t;
+			next += t;
+
+			if (list_empty(&ep->queue)) {
+				t = scnprintf(next, size,
+						"its req queue is empty\n\n");
+				size -= t;
+				next += t;
+			} else {
+				list_for_each_entry(req, &ep->queue, queue) {
+					t = scnprintf(next, size,
+						"req %p actual 0x%x length"
+						"0x%x  buf %p\n",
+						&req->req, req->req.actual,
+						req->req.length, req->req.buf);
+					size -= t;
+					next += t;
+					} /* end for each_entry of ep req */
+				}	/* end for else */
+			}	/* end for if(ep->queue) */
+		}		/* end (ep->desc) */
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	*eof = 1;
+	return count - size;
+}
+
+#define create_proc_file()	create_proc_read_entry(proc_filename, \
+				0, NULL, fsl_proc_read, NULL)
+
+#define remove_proc_file()	remove_proc_entry(proc_filename, NULL)
+
+#else				/* !CONFIG_USB_GADGET_DEBUG_FILES */
+
+#define create_proc_file()	do {} while (0)
+#define remove_proc_file()	do {} while (0)
+
+#endif				/* CONFIG_USB_GADGET_DEBUG_FILES */
+
+/*-------------------------------------------------------------------------*/
+
+/* Release udc structures */
+static void fsl_udc_release(struct device *dev)
+{
+	complete(udc_controller->done);
+	dma_free_coherent(dev, udc_controller->ep_qh_size,
+			udc_controller->ep_qh, udc_controller->ep_qh_dma);
+	kfree(udc_controller);
+}
+
+/******************************************************************
+	Internal structure setup functions
+*******************************************************************/
+/*------------------------------------------------------------------
+ * init resource for globle controller
+ * Return the udc handle on success or NULL on failure
+ ------------------------------------------------------------------*/
+static int __init struct_udc_setup(struct fsl_udc *udc,
+		struct platform_device *pdev)
+{
+	struct fsl_usb2_platform_data *pdata;
+	size_t size;
+
+	pdata = pdev->dev.platform_data;
+	udc->phy_mode = pdata->phy_mode;
+
+	udc->eps = kzalloc(sizeof(struct fsl_ep) * udc->max_ep, GFP_KERNEL);
+	if (!udc->eps) {
+		ERR("malloc fsl_ep failed\n");
+		return -1;
+	}
+
+	/* initialized QHs, take care of alignment */
+	size = udc->max_ep * sizeof(struct ep_queue_head);
+	if (size < QH_ALIGNMENT)
+		size = QH_ALIGNMENT;
+	else if ((size % QH_ALIGNMENT) != 0) {
+		size += QH_ALIGNMENT + 1;
+		size &= ~(QH_ALIGNMENT - 1);
+	}
+	udc->ep_qh = dma_alloc_coherent(&pdev->dev, size,
+					&udc->ep_qh_dma, GFP_KERNEL);
+	if (!udc->ep_qh) {
+		ERR("malloc QHs for udc failed\n");
+		kfree(udc->eps);
+		return -1;
+	}
+
+	udc->ep_qh_size = size;
+
+	/* Initialize ep0 status request structure */
+	/* FIXME: fsl_alloc_request() ignores ep argument */
+	udc->status_req = container_of(fsl_alloc_request(NULL, GFP_KERNEL),
+			struct fsl_req, req);
+	/* allocate a small amount of memory to get valid address */
+	udc->status_req->req.buf = kmalloc(8, GFP_KERNEL);
+	udc->status_req->req.dma = virt_to_phys(udc->status_req->req.buf);
+	/* Initialize ep0 data request structure */
+	udc->data_req = container_of(fsl_alloc_request(NULL, GFP_KERNEL),
+			struct fsl_req, req);
+	udc->data_req->req.buf = kmalloc(8, GFP_KERNEL);
+	udc->data_req->req.dma = virt_to_phys(udc->data_req->req.buf);
+
+	udc->resume_state = USB_STATE_NOTATTACHED;
+	udc->usb_state = USB_STATE_POWERED;
+	udc->ep0_dir = 0;
+	udc->remote_wakeup = 0;	/* default to 0 on reset */
+	spin_lock_init(&udc->lock);
+
+	return 0;
+}
+
+/*----------------------------------------------------------------
+ * Setup the fsl_ep struct for eps
+ * Link fsl_ep->ep to gadget->ep_list
+ * ep0out is not used so do nothing here
+ * ep0in should be taken care
+ *--------------------------------------------------------------*/
+static int __init struct_ep_setup(struct fsl_udc *udc, unsigned char index,
+		char *name, int link)
+{
+	struct fsl_ep *ep = &udc->eps[index];
+
+	ep->udc = udc;
+	strcpy(ep->name, name);
+	ep->ep.name = ep->name;
+
+	ep->ep.ops = &fsl_ep_ops;
+	ep->stopped = 0;
+
+	/* for ep0: maxP defined in desc
+	 * for other eps, maxP is set by epautoconfig() called by gadget layer
+	 */
+	ep->ep.maxpacket = (unsigned short) ~0;
+
+	/* the queue lists any req for this ep */
+	INIT_LIST_HEAD(&ep->queue);
+
+	/* gagdet.ep_list used for ep_autoconfig so no ep0 */
+	if (link)
+		list_add_tail(&ep->ep.ep_list, &udc->gadget.ep_list);
+	ep->gadget = &udc->gadget;
+	ep->qh = &udc->ep_qh[index];
+
+	return 0;
+}
+
+/* Driver probe function
+ * all intialization operations implemented here except enabling usb_intr reg
+ * board setup should have been done in the platform code
+ */
+static int __init fsl_udc_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
+	int ret = -ENODEV;
+	unsigned int i;
+	u32 dccparams;
+
+	if (strcmp(pdev->name, driver_name)) {
+		VDBG("Wrong device\n");
+		return -ENODEV;
+	}
+
+	udc_controller = kzalloc(sizeof(struct fsl_udc), GFP_KERNEL);
+	if (udc_controller == NULL) {
+		ERR("malloc udc failed\n");
+		return -ENOMEM;
+	}
+	udc_controller->pdata = pdata;
+
+#ifdef CONFIG_USB_OTG
+	/* Memory and interrupt resources will be passed from OTG */
+	udc_controller->transceiver = otg_get_transceiver();
+	if (!udc_controller->transceiver) {
+		printk(KERN_ERR "Can't find OTG driver!\n");
+		ret = -ENODEV;
+		goto err1a;
+	}
+
+	res = otg_get_resources();
+	if (!res) {
+		DBG("resource not registered!\n");
+		return -ENODEV;
+	}
+#else
+	if ((pdev->dev.parent) &&
+		(to_platform_device(pdev->dev.parent)->resource)) {
+		pdev->resource =
+			to_platform_device(pdev->dev.parent)->resource;
+		pdev->num_resources =
+			to_platform_device(pdev->dev.parent)->num_resources;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		ret = -ENXIO;
+		goto err1a;
+	}
+
+	if (!request_mem_region(res->start, resource_size(res),
+				driver_name)) {
+		ERR("request mem region for %s failed \n", pdev->name);
+		ret = -EBUSY;
+		goto err1a;
+	}
+#endif
+
+	dr_regs = ioremap(res->start, resource_size(res));
+	if (!dr_regs) {
+		ret = -ENOMEM;
+		goto err1;
+	}
+	pdata->regs = (void *)dr_regs;
+	/*
+	 * do platform specific init: check the clock, grab/config pins, etc.
+	 */
+	if (pdata->platform_init && pdata->platform_init(pdev)) {
+		ret = -ENODEV;
+		goto err2a;
+	}
+
+	if (pdata->have_sysif_regs)
+		usb_sys_regs = (struct usb_sys_interface *)
+				((u32)dr_regs + USB_DR_SYS_OFFSET);
+
+	/* Read Device Controller Capability Parameters register */
+	dccparams = fsl_readl(&dr_regs->dccparams);
+	if (!(dccparams & DCCPARAMS_DC)) {
+		ERR("This SOC doesn't support device role\n");
+		ret = -ENODEV;
+		goto err2;
+	}
+	/* Get max device endpoints */
+	/* DEN is bidirectional ep number, max_ep doubles the number */
+	udc_controller->max_ep = (dccparams & DCCPARAMS_DEN_MASK) * 2;
+
+#ifdef CONFIG_USB_OTG
+	res++;
+	udc_controller->irq = res->start;
+#else
+	udc_controller->irq = platform_get_irq(pdev, 0);
+#endif
+	if (!udc_controller->irq) {
+		ret = -ENODEV;
+		goto err2;
+	}
+
+	ret = request_irq(udc_controller->irq, fsl_udc_irq, IRQF_SHARED,
+			driver_name, udc_controller);
+	if (ret != 0) {
+		ERR("cannot request irq %d err %d \n",
+				udc_controller->irq, ret);
+		goto err2;
+	}
+
+	/* Initialize the udc structure including QH member and other member */
+	if (struct_udc_setup(udc_controller, pdev)) {
+		ERR("Can't initialize udc data structure\n");
+		ret = -ENOMEM;
+		goto err3;
+	}
+
+	if (!udc_controller->transceiver) {
+		/* initialize usb hw reg except for regs for EP,
+		 * leave usbintr reg untouched */
+		dr_controller_setup(udc_controller);
+	}
+
+	/* Setup gadget structure */
+	udc_controller->gadget.ops = &fsl_gadget_ops;
+	udc_controller->gadget.is_dualspeed = 1;
+	udc_controller->gadget.ep0 = &udc_controller->eps[0].ep;
+	INIT_LIST_HEAD(&udc_controller->gadget.ep_list);
+	udc_controller->gadget.speed = USB_SPEED_UNKNOWN;
+	udc_controller->gadget.name = driver_name;
+
+	/* Setup gadget.dev and register with kernel */
+	strcpy(udc_controller->gadget.dev.bus_id, "gadget");
+	udc_controller->gadget.dev.release = fsl_udc_release;
+	udc_controller->gadget.dev.parent = &pdev->dev;
+	ret = device_register(&udc_controller->gadget.dev);
+	if (ret < 0)
+		goto err3;
+
+	if (udc_controller->transceiver) {
+		udc_controller->gadget.is_otg = 1;
+		/* now didn't support lpm in OTG mode*/
+		device_set_wakeup_capable(&pdev->dev, 0);
+	}
+
+	/* setup QH and epctrl for ep0 */
+	ep0_setup(udc_controller);
+
+	/* setup udc->eps[] for ep0 */
+	struct_ep_setup(udc_controller, 0, "ep0", 0);
+	/* for ep0: the desc defined here;
+	 * for other eps, gadget layer called ep_enable with defined desc
+	 */
+	udc_controller->eps[0].desc = &fsl_ep0_desc;
+	udc_controller->eps[0].ep.maxpacket = USB_MAX_CTRL_PAYLOAD;
+
+	/* setup the udc->eps[] for non-control endpoints and link
+	 * to gadget.ep_list */
+	for (i = 1; i < (int)(udc_controller->max_ep / 2); i++) {
+		char name[14];
+
+		sprintf(name, "ep%dout", i);
+		struct_ep_setup(udc_controller, i * 2, name, 1);
+		sprintf(name, "ep%din", i);
+		struct_ep_setup(udc_controller, i * 2 + 1, name, 1);
+	}
+
+	/* use dma_pool for TD management */
+	udc_controller->td_pool = dma_pool_create("udc_td", &pdev->dev,
+			sizeof(struct ep_td_struct),
+			DTD_ALIGNMENT, UDC_DMA_BOUNDARY);
+	if (udc_controller->td_pool == NULL) {
+		ret = -ENOMEM;
+		goto err4;
+	}
+	if (g_iram_size) {
+		for (i = 0; i < IRAM_PPH_NTD; i++) {
+			udc_controller->iram_buffer[i] =
+			    USB_IRAM_BASE_ADDR + i * g_iram_size;
+			udc_controller->iram_buffer_v[i] =
+			    IO_ADDRESS(udc_controller->iram_buffer[i]);
+		}
+	}
+#ifdef POSTPONE_FREE_LAST_DTD
+	last_free_td = NULL;
+#endif
+
+	dr_wake_up_enable(udc_controller, false);
+	udc_controller->stopped = 1;
+	dr_phy_low_power_mode(udc_controller, true);
+
+	create_proc_file();
+	return 0;
+
+err4:
+	device_unregister(&udc_controller->gadget.dev);
+err3:
+	free_irq(udc_controller->irq, udc_controller);
+err2:
+	if (pdata->platform_uninit)
+		pdata->platform_uninit(pdata);
+err2a:
+	iounmap((u8 __iomem *)dr_regs);
+err1:
+	if (!udc_controller->transceiver)
+		release_mem_region(res->start, resource_size(res));
+err1a:
+	kfree(udc_controller);
+	udc_controller = NULL;
+	return ret;
+}
+
+/* Driver removal function
+ * Free resources and finish pending transactions
+ */
+static int __exit fsl_udc_remove(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
+
+	DECLARE_COMPLETION(done);
+
+	if (!udc_controller)
+		return -ENODEV;
+	udc_controller->done = &done;
+	/* open USB PHY clock */
+	dr_phy_low_power_mode(udc_controller, false);
+
+	/* DR has been stopped in usb_gadget_unregister_driver() */
+	remove_proc_file();
+
+	/* Free allocated memory */
+	kfree(udc_controller->status_req->req.buf);
+	kfree(udc_controller->status_req);
+	kfree(udc_controller->data_req->req.buf);
+	kfree(udc_controller->data_req);
+	kfree(udc_controller->eps);
+#ifdef POSTPONE_FREE_LAST_DTD
+	if (last_free_td != NULL)
+		dma_pool_free(udc_controller->td_pool, last_free_td,
+				last_free_td->td_dma);
+#endif
+	dma_pool_destroy(udc_controller->td_pool);
+	free_irq(udc_controller->irq, udc_controller);
+	iounmap((u8 __iomem *)dr_regs);
+
+#ifndef CONFIG_USB_OTG
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(res->start, resource_size(res));
+#endif
+
+	device_unregister(&udc_controller->gadget.dev);
+	/* free udc --wait for the release() finished */
+	wait_for_completion(&done);
+
+	/*
+	 * do platform specific un-initialization:
+	 * release iomux pins, etc.
+	 */
+	if (pdata->platform_uninit)
+		pdata->platform_uninit(pdata);
+
+	return 0;
+}
+
+static int udc_suspend(struct fsl_udc *udc)
+{
+	u32 mode, usbcmd;
+
+	mode = fsl_readl(&dr_regs->usbmode) & USB_MODE_CTRL_MODE_MASK;
+	usbcmd = fsl_readl(&dr_regs->usbcmd);
+
+	pr_debug("%s(): mode 0x%x stopped %d\n", __func__, mode, udc->stopped);
+
+	/*
+	 * If the controller is already stopped, then this must be a
+	 * PM suspend.  Remember this fact, so that we will leave the
+	 * controller stopped at PM resume time.
+	 */
+	if (udc->stopped) {
+		pr_debug("gadget already stopped, leaving early\n");
+		udc->already_stopped = 1;
+		return 0;
+	}
+
+	if (mode != USB_MODE_CTRL_MODE_DEVICE) {
+		pr_debug("gadget not in device mode, leaving early\n");
+		return 0;
+	}
+
+	udc->stopped = 1;
+	/* if the suspend is not for switch to host in otg mode */
+	if ((!(udc->gadget.is_otg)) ||
+			(fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID)) {
+		dr_wake_up_enable(udc, true);
+		dr_phy_low_power_mode(udc, true);
+	}
+
+	/* stop the controller */
+	usbcmd = fsl_readl(&dr_regs->usbcmd) & ~USB_CMD_RUN_STOP;
+	fsl_writel(usbcmd, &dr_regs->usbcmd);
+
+	printk(KERN_INFO "USB Gadget suspended\n");
+
+	return 0;
+}
+
+/*-----------------------------------------------------------------
+ * Modify Power management attributes
+ * Used by OTG statemachine to disable gadget temporarily
+ -----------------------------------------------------------------*/
+static int fsl_udc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	if ((udc_controller->usb_state > USB_STATE_POWERED) &&
+			(udc_controller->usb_state < USB_STATE_SUSPENDED))
+		return -EBUSY;
+
+	return udc_suspend(udc_controller);
+}
+
+/*-----------------------------------------------------------------
+ * Invoked on USB resume. May be called in_interrupt.
+ * Here we start the DR controller and enable the irq
+ *-----------------------------------------------------------------*/
+static int fsl_udc_resume(struct platform_device *pdev)
+{
+	pr_debug("%s(): stopped %d  already_stopped %d\n", __func__,
+		 udc_controller->stopped, udc_controller->already_stopped);
+
+	/*
+	 * If the controller was stopped at suspend time, then
+	 * don't resume it now.
+	 */
+	if (udc_controller->already_stopped) {
+		udc_controller->already_stopped = 0;
+		pr_debug("gadget was already stopped, leaving early\n");
+		return 0;
+	}
+
+	/* Enable DR irq reg and set controller Run */
+	if (udc_controller->stopped) {
+		dr_wake_up_enable(udc_controller, false);
+		dr_phy_low_power_mode(udc_controller, false);
+		mdelay(1);
+
+		dr_controller_setup(udc_controller);
+		dr_controller_run(udc_controller);
+	}
+	udc_controller->usb_state = USB_STATE_ATTACHED;
+	udc_controller->ep0_dir = 0;
+
+	printk(KERN_INFO "USB Gadget resumed\n");
+	return 0;
+}
+
+/*-------------------------------------------------------------------------
+	Register entry point for the peripheral controller driver
+--------------------------------------------------------------------------*/
+
+static struct platform_driver udc_driver = {
+	.remove  = __exit_p(fsl_udc_remove),
+	/* these suspend and resume are not usb suspend and resume */
+	.suspend = fsl_udc_suspend,
+	.resume  = fsl_udc_resume,
+	.probe = fsl_udc_probe,
+	.driver  = {
+		.name = driver_name,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init udc_init(void)
+{
+	printk(KERN_INFO "%s (%s)\n", driver_desc, DRIVER_VERSION);
+	return platform_driver_register(&udc_driver);
+}
+
+module_init(udc_init);
+
+static void __exit udc_exit(void)
+{
+	platform_driver_unregister(&udc_driver);
+	printk(KERN_INFO "%s unregistered \n", driver_desc);
+}
+
+module_exit(udc_exit);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/gadget/arcotg_udc.h b/drivers/usb/gadget/arcotg_udc.h
new file mode 100644
index 000000000000..f7950c25b45b
--- /dev/null
+++ b/drivers/usb/gadget/arcotg_udc.h
@@ -0,0 +1,703 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file arcotg_udc.h
+ * @brief Freescale USB device/endpoint management registers
+ * @ingroup USB
+ */
+
+#ifndef __ARCOTG_UDC_H
+#define __ARCOTG_UDC_H
+
+#define TRUE 1
+#define FALSE 0
+
+#define MSC_BULK_CB_WRAP_LEN 31
+#if CONFIG_ARCH_MXC
+#define USE_MSC_WR(len) (((cpu_is_mx37_rev(CHIP_REV_1_0) == 1) ||\
+	(cpu_is_mx51_rev(CHIP_REV_2_0) < 0)) && ((len) == MSC_BULK_CB_WRAP_LEN))
+#else
+#define USE_MSC_WR(len) false
+#endif
+
+/* Iram patch */
+#ifdef CONFIG_USB_STATIC_IRAM_PPH
+/* size of 1 qTD's buffer,one is for BULK IN and other is BULK OUT */
+#define IRAM_TD_PPH_SIZE	(USB_IRAM_SIZE / 2)
+#define IRAM_PPH_NTD	2	/* number of TDs in IRAM  */
+#else
+#define IRAM_TD_PPH_SIZE	0
+#define IRAM_PPH_NTD	0
+#endif
+
+#ifndef USB_IRAM_BASE_ADDR
+#define USB_IRAM_BASE_ADDR 0
+#endif
+
+#define NEED_IRAM(ep) ((g_iram_size) && \
+	((ep)->desc->bmAttributes == USB_ENDPOINT_XFER_BULK))
+
+#ifdef CONFIG_ARCH_MX51
+#define POSTPONE_FREE_LAST_DTD
+#else
+#undef POSTPONE_FREE_LAST_DTD
+#endif
+
+/* ### define USB registers here
+ */
+#define USB_MAX_ENDPOINTS		8
+#define USB_MAX_PIPES			(USB_MAX_ENDPOINTS*2)
+#define USB_MAX_CTRL_PAYLOAD		64
+#define	USB_DR_SYS_OFFSET		0x400
+
+#define	USB_DR_OFFSET	0x3100
+
+struct usb_dr_device {
+	/* Capability register */
+	u32 id;
+	u32 res1[35];
+	u32 sbuscfg;		/* sbuscfg ahb burst */
+	u32 res11[27];
+	u16 caplength;		/* Capability Register Length */
+	u16 hciversion;		/* Host Controller Interface Version */
+	u32 hcsparams;		/* Host Controller Structual Parameters */
+	u32 hccparams;		/* Host Controller Capability Parameters */
+	u32 res2[5];
+	u32 dciversion;		/* Device Controller Interface Version */
+	u32 dccparams;		/* Device Controller Capability Parameters */
+	u32 res3[6];
+	/* Operation register */
+	u32 usbcmd;		/* USB Command Register */
+	u32 usbsts;		/* USB Status Register */
+	u32 usbintr;		/* USB Interrupt Enable Register */
+	u32 frindex;		/* Frame Index Register */
+	u32 res4;
+	u32 deviceaddr;		/* Device Address */
+	u32 endpointlistaddr;	/* Endpoint List Address Register */
+	u32 res5;
+	u32 burstsize;		/* Master Interface Data Burst Size Register */
+	u32 txttfilltuning;	/* Transmit FIFO Tuning Controls Register */
+	u32 res6[6];
+	u32 configflag;		/* Configure Flag Register */
+	u32 portsc1;		/* Port 1 Status and Control Register */
+	u32 res7[7];
+	u32 otgsc;		/* On-The-Go Status and Control */
+	u32 usbmode;		/* USB Mode Register */
+	u32 endptsetupstat;	/* Endpoint Setup Status Register */
+	u32 endpointprime;	/* Endpoint Initialization Register */
+	u32 endptflush;		/* Endpoint Flush Register */
+	u32 endptstatus;	/* Endpoint Status Register */
+	u32 endptcomplete;	/* Endpoint Complete Register */
+	u32 endptctrl[8 * 2];	/* Endpoint Control Registers */
+	u32 res8[256];
+#ifdef CONFIG_ARCH_MX51
+	u32 res9[128];		/* i.MX51 start from 0x800 */
+#endif
+	u32 usbctrl;
+	u32 otgmirror;
+	u32 phyctrl0;
+	u32 phyctrl1;
+	u32 ctrl1;
+	u32 uh2ctrl;
+};
+
+ /* non-EHCI USB system interface registers (Big Endian) */
+struct usb_sys_interface {
+	u32 snoop1;
+	u32 snoop2;
+	u32 age_cnt_thresh;	/* Age Count Threshold Register */
+	u32 pri_ctrl;		/* Priority Control Register */
+	u32 si_ctrl;		/* System Interface Control Register */
+	u8 res[236];
+	u32 control;		/* General Purpose Control Register */
+};
+
+/* ep0 transfer state */
+#define WAIT_FOR_SETUP          0
+#define DATA_STATE_XMIT         1
+#define DATA_STATE_NEED_ZLP     2
+#define WAIT_FOR_OUT_STATUS     3
+#define DATA_STATE_RECV         4
+
+/* Device Controller Capability Parameter register */
+#define DCCPARAMS_DC				0x00000080
+#define DCCPARAMS_DEN_MASK			0x0000001f
+
+/* Frame Index Register Bit Masks */
+#define	USB_FRINDEX_MASKS			(0x3fff)
+/* USB CMD  Register Bit Masks */
+#define  USB_CMD_RUN_STOP                     (0x00000001)
+#define  USB_CMD_CTRL_RESET                   (0x00000002)
+#define  USB_CMD_PERIODIC_SCHEDULE_EN         (0x00000010)
+#define  USB_CMD_ASYNC_SCHEDULE_EN            (0x00000020)
+#define  USB_CMD_INT_AA_DOORBELL              (0x00000040)
+#define  USB_CMD_ASP                          (0x00000300)
+#define  USB_CMD_ASYNC_SCH_PARK_EN            (0x00000800)
+#define  USB_CMD_SUTW                         (0x00002000)
+#define  USB_CMD_ATDTW                        (0x00004000)
+#define  USB_CMD_ITC                          (0x00FF0000)
+
+/* bit 15,3,2 are frame list size */
+#define  USB_CMD_FRAME_SIZE_1024              (0x00000000)
+#define  USB_CMD_FRAME_SIZE_512               (0x00000004)
+#define  USB_CMD_FRAME_SIZE_256               (0x00000008)
+#define  USB_CMD_FRAME_SIZE_128               (0x0000000C)
+#define  USB_CMD_FRAME_SIZE_64                (0x00008000)
+#define  USB_CMD_FRAME_SIZE_32                (0x00008004)
+#define  USB_CMD_FRAME_SIZE_16                (0x00008008)
+#define  USB_CMD_FRAME_SIZE_8                 (0x0000800C)
+
+/* bit 9-8 are async schedule park mode count */
+#define  USB_CMD_ASP_00                       (0x00000000)
+#define  USB_CMD_ASP_01                       (0x00000100)
+#define  USB_CMD_ASP_10                       (0x00000200)
+#define  USB_CMD_ASP_11                       (0x00000300)
+#define  USB_CMD_ASP_BIT_POS                  (8)
+
+/* bit 23-16 are interrupt threshold control */
+#define  USB_CMD_ITC_NO_THRESHOLD             (0x00000000)
+#define  USB_CMD_ITC_1_MICRO_FRM              (0x00010000)
+#define  USB_CMD_ITC_2_MICRO_FRM              (0x00020000)
+#define  USB_CMD_ITC_4_MICRO_FRM              (0x00040000)
+#define  USB_CMD_ITC_8_MICRO_FRM              (0x00080000)
+#define  USB_CMD_ITC_16_MICRO_FRM             (0x00100000)
+#define  USB_CMD_ITC_32_MICRO_FRM             (0x00200000)
+#define  USB_CMD_ITC_64_MICRO_FRM             (0x00400000)
+#define  USB_CMD_ITC_BIT_POS                  (16)
+
+/* USB STS Register Bit Masks */
+#define  USB_STS_INT                          (0x00000001)
+#define  USB_STS_ERR                          (0x00000002)
+#define  USB_STS_PORT_CHANGE                  (0x00000004)
+#define  USB_STS_FRM_LST_ROLL                 (0x00000008)
+#define  USB_STS_SYS_ERR                      (0x00000010)
+#define  USB_STS_IAA                          (0x00000020)
+#define  USB_STS_RESET                        (0x00000040)
+#define  USB_STS_SOF                          (0x00000080)
+#define  USB_STS_SUSPEND                      (0x00000100)
+#define  USB_STS_HC_HALTED                    (0x00001000)
+#define  USB_STS_RCL                          (0x00002000)
+#define  USB_STS_PERIODIC_SCHEDULE            (0x00004000)
+#define  USB_STS_ASYNC_SCHEDULE               (0x00008000)
+
+/* USB INTR Register Bit Masks */
+#define  USB_INTR_INT_EN                      (0x00000001)
+#define  USB_INTR_ERR_INT_EN                  (0x00000002)
+#define  USB_INTR_PTC_DETECT_EN               (0x00000004)
+#define  USB_INTR_FRM_LST_ROLL_EN             (0x00000008)
+#define  USB_INTR_SYS_ERR_EN                  (0x00000010)
+#define  USB_INTR_ASYN_ADV_EN                 (0x00000020)
+#define  USB_INTR_RESET_EN                    (0x00000040)
+#define  USB_INTR_SOF_EN                      (0x00000080)
+#define  USB_INTR_DEVICE_SUSPEND              (0x00000100)
+
+/* Device Address bit masks */
+#define  USB_DEVICE_ADDRESS_MASK              (0xFE000000)
+#define  USB_DEVICE_ADDRESS_BIT_POS           (25)
+
+/* endpoint list address bit masks */
+#define USB_EP_LIST_ADDRESS_MASK              (0xfffff800)
+
+/* PORTSCX  Register Bit Masks */
+#define  PORTSCX_CURRENT_CONNECT_STATUS       (0x00000001)
+#define  PORTSCX_CONNECT_STATUS_CHANGE        (0x00000002)
+#define  PORTSCX_PORT_ENABLE                  (0x00000004)
+#define  PORTSCX_PORT_EN_DIS_CHANGE           (0x00000008)
+#define  PORTSCX_OVER_CURRENT_ACT             (0x00000010)
+#define  PORTSCX_OVER_CURRENT_CHG             (0x00000020)
+#define  PORTSCX_PORT_FORCE_RESUME            (0x00000040)
+#define  PORTSCX_PORT_SUSPEND                 (0x00000080)
+#define  PORTSCX_PORT_RESET                   (0x00000100)
+#define  PORTSCX_LINE_STATUS_BITS             (0x00000C00)
+#define  PORTSCX_PORT_POWER                   (0x00001000)
+#define  PORTSCX_PORT_INDICTOR_CTRL           (0x0000C000)
+#define  PORTSCX_PORT_TEST_CTRL               (0x000F0000)
+#define  PORTSCX_WAKE_ON_CONNECT_EN           (0x00100000)
+#define  PORTSCX_WAKE_ON_CONNECT_DIS          (0x00200000)
+#define  PORTSCX_WAKE_ON_OVER_CURRENT         (0x00400000)
+#define  PORTSCX_PHY_LOW_POWER_SPD            (0x00800000)
+#define  PORTSCX_PORT_FORCE_FULL_SPEED        (0x01000000)
+#define  PORTSCX_PORT_SPEED_MASK              (0x0C000000)
+#define  PORTSCX_PORT_WIDTH                   (0x10000000)
+#define  PORTSCX_PHY_TYPE_SEL                 (0xC0000000)
+
+/* bit 11-10 are line status */
+#define  PORTSCX_LINE_STATUS_SE0              (0x00000000)
+#define  PORTSCX_LINE_STATUS_JSTATE           (0x00000400)
+#define  PORTSCX_LINE_STATUS_KSTATE           (0x00000800)
+#define  PORTSCX_LINE_STATUS_UNDEF            (0x00000C00)
+#define  PORTSCX_LINE_STATUS_BIT_POS          (10)
+
+/* bit 15-14 are port indicator control */
+#define  PORTSCX_PIC_OFF                      (0x00000000)
+#define  PORTSCX_PIC_AMBER                    (0x00004000)
+#define  PORTSCX_PIC_GREEN                    (0x00008000)
+#define  PORTSCX_PIC_UNDEF                    (0x0000C000)
+#define  PORTSCX_PIC_BIT_POS                  (14)
+
+/* bit 19-16 are port test control */
+#define  PORTSCX_PTC_DISABLE                  (0x00000000)
+#define  PORTSCX_PTC_JSTATE                   (0x00010000)
+#define  PORTSCX_PTC_KSTATE                   (0x00020000)
+#define  PORTSCX_PTC_SEQNAK                   (0x00030000)
+#define  PORTSCX_PTC_PACKET                   (0x00040000)
+#define  PORTSCX_PTC_FORCE_EN                 (0x00050000)
+#define  PORTSCX_PTC_BIT_POS                  (16)
+
+/* bit 27-26 are port speed */
+#define  PORTSCX_PORT_SPEED_FULL              (0x00000000)
+#define  PORTSCX_PORT_SPEED_LOW               (0x04000000)
+#define  PORTSCX_PORT_SPEED_HIGH              (0x08000000)
+#define  PORTSCX_PORT_SPEED_UNDEF             (0x0C000000)
+#define  PORTSCX_SPEED_BIT_POS                (26)
+
+/* OTGSC Register Bit Masks */
+#define  OTGSC_B_SESSION_VALID_IRQ_EN           (1 << 27)
+#define  OTGSC_B_SESSION_VALID_IRQ_STS          (1 << 19)
+#define  OTGSC_B_SESSION_VALID                  (1 << 11)
+
+/* bit 28 is parallel transceiver width for UTMI interface */
+#define  PORTSCX_PTW                          (0x10000000)
+#define  PORTSCX_PTW_8BIT                     (0x00000000)
+#define  PORTSCX_PTW_16BIT                    (0x10000000)
+
+/* bit 31-30 are port transceiver select */
+#define  PORTSCX_PTS_UTMI                     (0x00000000)
+#define  PORTSCX_PTS_ULPI                     (0x80000000)
+#define  PORTSCX_PTS_FSLS                     (0xC0000000)
+#define  PORTSCX_PTS_BIT_POS                  (30)
+
+/* USB MODE Register Bit Masks */
+#define  USB_MODE_CTRL_MODE_IDLE              (0x00000000)
+#define  USB_MODE_CTRL_MODE_DEVICE            (0x00000002)
+#define  USB_MODE_CTRL_MODE_HOST              (0x00000003)
+#define  USB_MODE_CTRL_MODE_MASK              0x00000003
+#define  USB_MODE_CTRL_MODE_RSV               (0x00000001)
+#define  USB_MODE_ES                          0x00000004 /* (big) Endian Sel */
+#define  USB_MODE_SETUP_LOCK_OFF              (0x00000008)
+#define  USB_MODE_STREAM_DISABLE              (0x00000010)
+/* Endpoint Flush Register */
+#define EPFLUSH_TX_OFFSET		      (0x00010000)
+#define EPFLUSH_RX_OFFSET		      (0x00000000)
+
+/* Endpoint Setup Status bit masks */
+#define  EP_SETUP_STATUS_MASK                 (0x0000003F)
+#define  EP_SETUP_STATUS_EP0		      (0x00000001)
+
+/* ENDPOINTCTRLx  Register Bit Masks */
+#define  EPCTRL_TX_ENABLE                     (0x00800000)
+#define  EPCTRL_TX_DATA_TOGGLE_RST            (0x00400000)	/* Not EP0 */
+#define  EPCTRL_TX_DATA_TOGGLE_INH            (0x00200000)	/* Not EP0 */
+#define  EPCTRL_TX_TYPE                       (0x000C0000)
+#define  EPCTRL_TX_DATA_SOURCE                (0x00020000)	/* Not EP0 */
+#define  EPCTRL_TX_EP_STALL                   (0x00010000)
+#define  EPCTRL_RX_ENABLE                     (0x00000080)
+#define  EPCTRL_RX_DATA_TOGGLE_RST            (0x00000040)	/* Not EP0 */
+#define  EPCTRL_RX_DATA_TOGGLE_INH            (0x00000020)	/* Not EP0 */
+#define  EPCTRL_RX_TYPE                       (0x0000000C)
+#define  EPCTRL_RX_DATA_SINK                  (0x00000002)	/* Not EP0 */
+#define  EPCTRL_RX_EP_STALL                   (0x00000001)
+
+/* bit 19-18 and 3-2 are endpoint type */
+#define  EPCTRL_EP_TYPE_CONTROL               (0)
+#define  EPCTRL_EP_TYPE_ISO                   (1)
+#define  EPCTRL_EP_TYPE_BULK                  (2)
+#define  EPCTRL_EP_TYPE_INTERRUPT             (3)
+#define  EPCTRL_TX_EP_TYPE_SHIFT              (18)
+#define  EPCTRL_RX_EP_TYPE_SHIFT              (2)
+
+/* SNOOPn Register Bit Masks */
+#define  SNOOP_ADDRESS_MASK                   (0xFFFFF000)
+#define  SNOOP_SIZE_ZERO                      (0x00)	/* snooping disable */
+#define  SNOOP_SIZE_4KB                       (0x0B)	/* 4KB snoop size */
+#define  SNOOP_SIZE_8KB                       (0x0C)
+#define  SNOOP_SIZE_16KB                      (0x0D)
+#define  SNOOP_SIZE_32KB                      (0x0E)
+#define  SNOOP_SIZE_64KB                      (0x0F)
+#define  SNOOP_SIZE_128KB                     (0x10)
+#define  SNOOP_SIZE_256KB                     (0x11)
+#define  SNOOP_SIZE_512KB                     (0x12)
+#define  SNOOP_SIZE_1MB                       (0x13)
+#define  SNOOP_SIZE_2MB                       (0x14)
+#define  SNOOP_SIZE_4MB                       (0x15)
+#define  SNOOP_SIZE_8MB                       (0x16)
+#define  SNOOP_SIZE_16MB                      (0x17)
+#define  SNOOP_SIZE_32MB                      (0x18)
+#define  SNOOP_SIZE_64MB                      (0x19)
+#define  SNOOP_SIZE_128MB                     (0x1A)
+#define  SNOOP_SIZE_256MB                     (0x1B)
+#define  SNOOP_SIZE_512MB                     (0x1C)
+#define  SNOOP_SIZE_1GB                       (0x1D)
+#define  SNOOP_SIZE_2GB                       (0x1E)	/* 2GB snoop size */
+
+/* pri_ctrl Register Bit Masks */
+#define  PRI_CTRL_PRI_LVL1                    (0x0000000C)
+#define  PRI_CTRL_PRI_LVL0                    (0x00000003)
+
+/* si_ctrl Register Bit Masks */
+#define  SI_CTRL_ERR_DISABLE                  (0x00000010)
+#define  SI_CTRL_IDRC_DISABLE                 (0x00000008)
+#define  SI_CTRL_RD_SAFE_EN                   (0x00000004)
+#define  SI_CTRL_RD_PREFETCH_DISABLE          (0x00000002)
+#define  SI_CTRL_RD_PREFEFETCH_VAL            (0x00000001)
+
+/* control Register Bit Masks */
+#define  USB_CTRL_IOENB                       (0x00000004)
+#define  USB_CTRL_ULPI_INT0EN                 (0x00000001)
+#define  USB_CTRL_OTG_WUIR                   (0x80000000)
+#define  USB_CTRL_OTG_WUIE                   (0x08000000)
+#define  USB_CTRL_OTG_VWUE			(0x00001000)
+#define  USB_CTRL_OTG_IWUE			(0x00100000)
+
+/* PHY control0 Register Bit Masks */
+#define	PHY_CTRL0_CONF2			(1 << 26)
+
+/* USB UH2 CTRL Register Bits */
+#define USB_UH2_OVBWK_EN		(1 << 6) /* OTG VBUS Wakeup Enable */
+#define USB_UH2_OIDWK_EN		(1 << 5) /* OTG ID Wakeup Enable */
+/*!
+ * Endpoint Queue Head data struct
+ * Rem: all the variables of qh are LittleEndian Mode
+ * and NEXT_POINTER_MASK should operate on a LittleEndian, Phy Addr
+ */
+struct ep_queue_head {
+	/*!
+	 * Mult(31-30) , Zlt(29) , Max Pkt len  and IOS(15)
+	 */
+	u32 max_pkt_length;
+
+	/*!
+	 *  Current dTD Pointer(31-5)
+	 */
+	u32 curr_dtd_ptr;
+
+	/*!
+	 *  Next dTD Pointer(31-5), T(0)
+	 */
+	u32 next_dtd_ptr;
+
+	/*!
+	 *  Total bytes (30-16), IOC (15), MultO(11-10), STS (7-0)
+	 */
+	u32 size_ioc_int_sts;
+
+	/*!
+	 * Buffer pointer Page 0 (31-12)
+	 */
+	u32 buff_ptr0;
+
+	/*!
+	 * Buffer pointer Page 1 (31-12)
+	 */
+	u32 buff_ptr1;
+
+	/*!
+	 * Buffer pointer Page 2 (31-12)
+	 */
+	u32 buff_ptr2;
+
+	/*!
+	 * Buffer pointer Page 3 (31-12)
+	 */
+	u32 buff_ptr3;
+
+	/*!
+	 * Buffer pointer Page 4 (31-12)
+	 */
+	u32 buff_ptr4;
+
+	/*!
+	 * reserved field 1
+	 */
+	u32 res1;
+	/*!
+	 * Setup data 8 bytes
+	 */
+	u8 setup_buffer[8];	/* Setup data 8 bytes */
+
+	/*!
+	 * reserved field 2,pad out to 64 bytes
+	 */
+	u32 res2[4];
+};
+
+/* Endpoint Queue Head Bit Masks */
+#define  EP_QUEUE_HEAD_MULT_POS               (30)
+#define  EP_QUEUE_HEAD_ZLT_SEL                (0x20000000)
+#define  EP_QUEUE_HEAD_MAX_PKT_LEN_POS        (16)
+#define  EP_QUEUE_HEAD_MAX_PKT_LEN(ep_info)   (((ep_info)>>16)&0x07ff)
+#define  EP_QUEUE_HEAD_IOS                    (0x00008000)
+#define  EP_QUEUE_HEAD_NEXT_TERMINATE         (0x00000001)
+#define  EP_QUEUE_HEAD_IOC                    (0x00008000)
+#define  EP_QUEUE_HEAD_MULTO                  (0x00000C00)
+#define  EP_QUEUE_HEAD_STATUS_HALT	      (0x00000040)
+#define  EP_QUEUE_HEAD_STATUS_ACTIVE          (0x00000080)
+#define  EP_QUEUE_CURRENT_OFFSET_MASK         (0x00000FFF)
+#define  EP_QUEUE_HEAD_NEXT_POINTER_MASK      0xFFFFFFE0
+#define  EP_QUEUE_FRINDEX_MASK                (0x000007FF)
+#define  EP_MAX_LENGTH_TRANSFER               (0x4000)
+
+/*!
+ * Endpoint Transfer Descriptor data struct
+ *  Rem: all the variables of td are LittleEndian Mode
+ *       must be 32-byte aligned
+ */
+struct ep_td_struct {
+	/*!
+	 *  Next TD pointer(31-5), T(0) set indicate invalid
+	 */
+	u32 next_td_ptr;
+
+	/*!
+	 *  Total bytes (30-16), IOC (15),MultO(11-10), STS (7-0)
+	 */
+	u32 size_ioc_sts;
+
+	/*!
+	 * Buffer pointer Page 0
+	 */
+	u32 buff_ptr0;
+
+	/*!
+	 * Buffer pointer Page 1
+	 */
+	u32 buff_ptr1;
+
+	/*!
+	 * Buffer pointer Page 2
+	 */
+	u32 buff_ptr2;
+
+	/*!
+	 * Buffer pointer Page 3
+	 */
+	u32 buff_ptr3;
+
+	/*!
+	 * Buffer pointer Page 4
+	 */
+	u32 buff_ptr4;
+
+	/*!
+	 * dma address of this td
+	 * */
+	dma_addr_t td_dma;
+
+	/*!
+	 * virtual address of next td
+	 * */
+	struct ep_td_struct *next_td_virt;
+
+	/*!
+	 * make it an even 16 words
+	 * */
+	u32 res[7];
+};
+
+/*!
+ * Endpoint Transfer Descriptor bit Masks
+ */
+#define  DTD_NEXT_TERMINATE                   (0x00000001)
+#define  DTD_IOC                              (0x00008000)
+#define  DTD_STATUS_ACTIVE                    (0x00000080)
+#define  DTD_STATUS_HALTED                    (0x00000040)
+#define  DTD_STATUS_DATA_BUFF_ERR             (0x00000020)
+#define  DTD_STATUS_TRANSACTION_ERR           (0x00000008)
+#define  DTD_RESERVED_FIELDS                  (0x80007300)
+#define  DTD_ADDR_MASK                        0xFFFFFFE0
+#define  DTD_PACKET_SIZE                      (0x7FFF0000)
+#define  DTD_LENGTH_BIT_POS                   (16)
+#define  DTD_ERROR_MASK                       (DTD_STATUS_HALTED | \
+                                               DTD_STATUS_DATA_BUFF_ERR | \
+                                               DTD_STATUS_TRANSACTION_ERR)
+/* Alignment requirements; must be a power of two */
+#define DTD_ALIGNMENT				0x20
+#define QH_ALIGNMENT				2048
+
+/* Controller dma boundary */
+#define UDC_DMA_BOUNDARY			0x1000
+
+/* -----------------------------------------------------------------------*/
+/* ##### enum data
+*/
+typedef enum {
+	e_ULPI,
+	e_UTMI_8BIT,
+	e_UTMI_16BIT,
+	e_SERIAL
+} e_PhyInterface;
+
+/*-------------------------------------------------------------------------*/
+
+struct fsl_req {
+	struct usb_request req;
+	struct list_head queue;
+	/* ep_queue() func will add
+	   a request->queue into a udc_ep->queue 'd tail */
+	struct fsl_ep *ep;
+	unsigned mapped;
+
+	struct ep_td_struct *head, *tail;	/* For dTD List
+						   this is a BigEndian Virtual addr */
+	unsigned int dtd_count;
+	/* just for IRAM patch */
+	dma_addr_t oridma;	/* original dma */
+	size_t buffer_offset;	/* offset of user buffer */
+	int last_one;		/* mark if reach to last packet */
+	struct ep_td_struct *cur;	/* current tranfer dtd */
+};
+
+#define REQ_UNCOMPLETE		(1)
+
+struct fsl_ep {
+	struct usb_ep ep;
+	struct list_head queue;
+	struct fsl_udc *udc;
+	struct ep_queue_head *qh;
+	const struct usb_endpoint_descriptor *desc;
+	struct usb_gadget *gadget;
+
+	char name[14];
+	unsigned stopped:1;
+};
+
+#define EP_DIR_IN	1
+#define EP_DIR_OUT	0
+
+struct fsl_udc {
+	struct usb_gadget gadget;
+	struct usb_gadget_driver *driver;
+	struct fsl_usb2_platform_data *pdata;
+	struct fsl_ep *eps;
+	unsigned int max_ep;
+	unsigned int irq;
+
+	struct usb_ctrlrequest local_setup_buff;
+	spinlock_t lock;
+	u32 xcvr_type;
+	struct otg_transceiver *transceiver;
+	unsigned softconnect:1;
+	unsigned vbus_active:1;
+	unsigned stopped:1;
+	unsigned remote_wakeup:1;
+	unsigned already_stopped:1;
+
+	struct ep_queue_head *ep_qh;	/* Endpoints Queue-Head */
+	struct fsl_req *status_req;	/* ep0 status request */
+	struct fsl_req *data_req;	/* ep0 data request */
+	struct dma_pool *td_pool;	/* dma pool for DTD */
+	enum fsl_usb2_phy_modes phy_mode;
+
+	size_t ep_qh_size;		/* size after alignment adjustment*/
+	dma_addr_t ep_qh_dma;		/* dma address of QH */
+
+	u32 max_pipes;		/* Device max pipes */
+	u32 max_use_endpts;	/* Max endpointes to be used */
+	u32 bus_reset;		/* Device is bus reseting */
+	u32 resume_state;	/* USB state to resume */
+	u32 usb_state;		/* USB current state */
+	u32 usb_next_state;	/* USB next state */
+	u32 ep0_dir;		/* Endpoint zero direction: can be
+				   USB_DIR_IN or USB_DIR_OUT */
+	u32 usb_sof_count;	/* SOF count */
+	u32 errors;		/* USB ERRORs count */
+	u8 device_address;	/* Device USB address */
+
+	struct completion *done;	/* to make sure release() is done */
+	u32 iram_buffer[IRAM_PPH_NTD];
+	u32 iram_buffer_v[IRAM_PPH_NTD];
+};
+
+/*-------------------------------------------------------------------------*/
+
+#ifdef DEBUG
+#define DBG(fmt, args...) 	printk(KERN_DEBUG "[%s]  " fmt "\n", \
+				__func__, ## args)
+#else
+#define DBG(fmt, args...)	do {} while (0)
+#endif
+
+#if 0
+static void dump_msg(const char *label, const u8 * buf, unsigned int length)
+{
+	unsigned int start, num, i;
+	char line[52], *p;
+
+	if (length >= 512)
+		return;
+	pr_debug("udc: %s, length %u:\n", label, length);
+	start = 0;
+	while (length > 0) {
+		num = min(length, 16u);
+		p = line;
+		for (i = 0; i < num; ++i) {
+			if (i == 8)
+				*p++ = ' ';
+			sprintf(p, " %02x", buf[i]);
+			p += 3;
+		}
+		*p = 0;
+		printk(KERN_DEBUG "%6x: %s\n", start, line);
+		buf += num;
+		start += num;
+		length -= num;
+	}
+}
+#endif
+
+#ifdef VERBOSE
+#define VDBG		DBG
+#else
+#define VDBG(stuff...)	do {} while (0)
+#endif
+
+#define ERR(stuff...)		printk(KERN_ERR "udc: " stuff)
+#define INFO(stuff...)		printk(KERN_INFO "udc: " stuff)
+
+/*-------------------------------------------------------------------------*/
+
+/* ### Add board specific defines here
+ */
+
+/*
+ * ### pipe direction macro from device view
+ */
+#define USB_RECV	(0)	/* OUT EP */
+#define USB_SEND	(1)	/* IN EP */
+
+/*
+ * ### internal used help routines.
+ */
+#define ep_index(EP)         ((EP)->desc->bEndpointAddress&0xF)
+#define ep_maxpacket(EP)     ((EP)->ep.maxpacket)
+
+#define ep_is_in(EP)	( (ep_index(EP) == 0) ? (EP->udc->ep0_dir == \
+				USB_DIR_IN ):((EP)->desc->bEndpointAddress \
+				& USB_DIR_IN)==USB_DIR_IN)
+
+#define get_ep_by_pipe(udc, pipe)	((pipe == 1)? &udc->eps[0]: \
+					&udc->eps[pipe])
+#define get_pipe_by_windex(windex)	((windex & USB_ENDPOINT_NUMBER_MASK) \
+					* 2 + ((windex & USB_DIR_IN) ? 1 : 0))
+
+/* Bulk only class request */
+#define USB_BULK_RESET_REQUEST          0xff
+
+#if defined(CONFIG_ARCH_MXC) || defined(CONFIG_ARCH_STMP3XXX)
+#include <mach/fsl_usb_gadget.h>
+#elif CONFIG_PPC32
+#include <asm/fsl_usb_gadget.h>
+#endif
+
+#endif				/* __ARCOTG_UDC_H */
diff --git a/drivers/usb/gadget/epautoconf.c b/drivers/usb/gadget/epautoconf.c
index 9462e30192d8..8e20d46e3b83 100644
--- a/drivers/usb/gadget/epautoconf.c
+++ b/drivers/usb/gadget/epautoconf.c
@@ -162,7 +162,11 @@ ep_matches (
 	desc->bEndpointAddress &= USB_DIR_IN;
 	if (isdigit (ep->name [2])) {
 		u8	num = simple_strtol (&ep->name [2], NULL, 10);
-		desc->bEndpointAddress |= num;
+		if (desc->bEndpointAddress & 0x7) {
+			if (num != (desc->bEndpointAddress & 0x7))
+				return 0;
+		} else
+			desc->bEndpointAddress |= num;
 #ifdef	MANY_ENDPOINTS
 	} else if (desc->bEndpointAddress & USB_DIR_IN) {
 		if (++in_epnum > 15)
diff --git a/drivers/usb/gadget/ether.c b/drivers/usb/gadget/ether.c
index 37252d0012a7..8e34d9dd0fe7 100644
--- a/drivers/usb/gadget/ether.c
+++ b/drivers/usb/gadget/ether.c
@@ -385,7 +385,7 @@ static int __init init(void)
 {
 	return usb_composite_register(&eth_driver);
 }
-module_init(init);
+late_initcall(init);
 
 static void __exit cleanup(void)
 {
diff --git a/drivers/usb/gadget/file_storage.c b/drivers/usb/gadget/file_storage.c
index c4e62a6297d7..90199d4ec3fc 100644
--- a/drivers/usb/gadget/file_storage.c
+++ b/drivers/usb/gadget/file_storage.c
@@ -692,8 +692,16 @@ struct fsg_dev {
 	unsigned int		nluns;
 	struct lun		*luns;
 	struct lun		*curlun;
+
+#ifdef CONFIG_STMP_UTP
+	void			*utp;
+#endif
 };
 
+#ifdef CONFIG_STMP_UTP
+#include "stmp_updater.h"
+#endif
+
 typedef void (*fsg_routine_t)(struct fsg_dev *);
 
 static int exception_in_progress(struct fsg_dev *fsg)
@@ -712,6 +720,10 @@ static void set_bulk_out_req_length(struct fsg_dev *fsg,
 	if (rem > 0)
 		length += fsg->bulk_out_maxpacket - rem;
 	bh->outreq->length = length;
+#if 0
+	if (bh->bulk_out_intended_length == 31)
+		bh->outreq->length = 31;
+#endif
 }
 
 static struct fsg_dev			*the_fsg;
@@ -731,7 +743,7 @@ static void dump_msg(struct fsg_dev *fsg, const char *label,
 	if (length < 512) {
 		DBG(fsg, "%s, length %u:\n", label, length);
 		print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET,
-				16, 1, buf, length, 0);
+				16, 1, buf, length, !0);
 	}
 }
 
@@ -791,6 +803,11 @@ static u32 get_be32(u8 *buf)
 			((u32) buf[2] << 8) | ((u32) buf[3]);
 }
 
+static u64 get_be64(u8 *buf)
+{
+	return ((u64)get_be32(buf) << 32) | get_be32(buf + 4);
+}
+
 static void put_be16(u8 *buf, u16 val)
 {
 	buf[0] = val >> 8;
@@ -837,7 +854,11 @@ device_desc = {
 
 	.iManufacturer =	STRING_MANUFACTURER,
 	.iProduct =		STRING_PRODUCT,
+#ifdef CONFIG_STMP_UTP
+	.iSerialNumber = 0,
+#else
 	.iSerialNumber =	STRING_SERIAL,
+#endif
 	.bNumConfigurations =	1,
 };
 
@@ -2073,6 +2094,13 @@ static int do_request_sense(struct fsg_dev *fsg, struct fsg_buffhd *bh)
 	}
 #endif
 
+#ifdef CONFIG_STMP_UTP
+	if (utp_get_sense(fsg) == 0) {	/* got the sense from the UTP */
+		sd = UTP_CTX(fsg)->sd;
+		sdinfo = UTP_CTX(fsg)->sdinfo;
+		valid = 0;
+	} else
+#endif
 	if (!curlun) {		// Unsupported LUNs are okay
 		fsg->bad_lun_okay = 1;
 		sd = SS_LOGICAL_UNIT_NOT_SUPPORTED;
@@ -2094,6 +2122,9 @@ static int do_request_sense(struct fsg_dev *fsg, struct fsg_buffhd *bh)
 	buf[7] = 18 - 8;			// Additional sense length
 	buf[12] = ASC(sd);
 	buf[13] = ASCQ(sd);
+#ifdef CONFIG_STMP_UTP
+	put_be32(&buf[8], UTP_CTX(fsg)->sdinfo_h);
+#endif
 	return 18;
 }
 
@@ -2780,6 +2811,13 @@ static int do_scsi_command(struct fsg_dev *fsg)
 	fsg->phase_error = 0;
 	fsg->short_packet_received = 0;
 
+#ifdef CONFIG_STMP_UTP
+	reply = utp_handle_message(fsg, fsg->cmnd, reply);
+
+	if (reply != -EINVAL)
+		return reply;
+#endif
+
 	down_read(&fsg->filesem);	// We're using the backing file
 	switch (fsg->cmnd[0]) {
 
@@ -3443,10 +3481,12 @@ static int fsg_main_thread(void *fsg_)
 	/* Allow the thread to be frozen */
 	set_freezable();
 
+#ifndef CONFIG_STMP_UTP
 	/* Arrange for userspace references to be interpreted as kernel
 	 * pointers.  That way we can pass a kernel pointer to a routine
 	 * that expects a __user pointer and it will work okay. */
 	set_fs(get_ds());
+#endif
 
 	/* The main loop */
 	while (fsg->state != FSG_STATE_TERMINATED) {
@@ -3754,6 +3794,9 @@ static void /* __init_or_exit */ fsg_unbind(struct usb_gadget *gadget)
 	}
 
 	set_gadget_data(gadget, NULL);
+#ifdef CONFIG_STMP_UTP
+	utp_exit(fsg);
+#endif
 }
 
 
@@ -3788,6 +3831,17 @@ static int __init check_parameters(struct fsg_dev *fsg)
 
 	prot = simple_strtol(mod_data.protocol_parm, NULL, 0);
 
+#ifdef CONFIG_STMP_UTP
+	mod_data.can_stall = 0;
+	mod_data.removable = 1;
+	mod_data.nluns = 1;
+	mod_data.file[0] = NULL;
+	mod_data.vendor = 0x066F;
+	mod_data.product = 0x37FF;
+	pr_info("%s:UTP settings are in place now, overriding defaults\n",
+		__func__);
+#endif
+
 #ifdef CONFIG_USB_FILE_STORAGE_TEST
 	if (strnicmp(mod_data.transport_parm, "BBB", 10) == 0) {
 		;		// Use default setting
@@ -3840,8 +3894,9 @@ static int __init check_parameters(struct fsg_dev *fsg)
 
 	return 0;
 }
-
-
+#ifdef CONFIG_STMP_UTP
+#include "stmp_updater.c"
+#endif
 static int __init fsg_bind(struct usb_gadget *gadget)
 {
 	struct fsg_dev		*fsg = the_fsg;
@@ -3866,6 +3921,10 @@ static int __init fsg_bind(struct usb_gadget *gadget)
 		dev_attr_file.store = store_file;
 	}
 
+#ifdef CONFIG_STMP_UTP
+	utp_init(fsg);
+#endif
+
 	/* Find out how many LUNs there should be */
 	i = mod_data.nluns;
 	if (i == 0)
diff --git a/drivers/usb/gadget/gadget_chips.h b/drivers/usb/gadget/gadget_chips.h
index 4e3107dd2f34..79b90416e8de 100644
--- a/drivers/usb/gadget/gadget_chips.h
+++ b/drivers/usb/gadget/gadget_chips.h
@@ -92,6 +92,12 @@
 #define	gadget_is_pxa27x(g)	0
 #endif
 
+#ifdef CONFIG_USB_GADGET_ARC
+#define	gadget_is_arcotg(g)	!strcmp("fsl-usb2-udc", (g)->name)
+#else
+#define	gadget_is_arcotg(g)	0
+#endif
+
 #ifdef CONFIG_USB_GADGET_ATMEL_USBA
 #define gadget_is_atmel_usba(g)	!strcmp("atmel_usba_udc", (g)->name)
 #else
@@ -158,6 +164,12 @@
 #define gadget_is_fsl_qe(g)	0
 #endif
 
+/* Samsung S3C2443 UDC */
+#ifdef CONFIG_USB_GADGET_S3C2443
+#define gadget_is_s3c2443(g)    !strcmp("s3c2443_udc", (g)->name)
+#else
+#define gadget_is_s3c2443(g)    0
+#endif
 
 // CONFIG_USB_GADGET_SX2
 // CONFIG_USB_GADGET_AU1X00
@@ -225,6 +237,10 @@ static inline int usb_gadget_controller_number(struct usb_gadget *gadget)
 		return 0x21;
 	else if (gadget_is_fsl_qe(gadget))
 		return 0x22;
+        else if (gadget_is_s3c2443(gadget))
+                return 0x23;
+	else if (gadget_is_arcotg(gadget))
+		return 0x24;
 	return -ENOENT;
 }
 
diff --git a/drivers/usb/gadget/inode.c b/drivers/usb/gadget/inode.c
index eeb26c0f88e5..9c9c612ec691 100644
--- a/drivers/usb/gadget/inode.c
+++ b/drivers/usb/gadget/inode.c
@@ -40,6 +40,8 @@
 #include <linux/usb/gadgetfs.h>
 #include <linux/usb/gadget.h>
 
+#include <linux/delay.h>
+#include <linux/time.h>
 
 /*
  * The gadgetfs API maps each endpoint to a file descriptor so that you
@@ -81,6 +83,9 @@ MODULE_DESCRIPTION (DRIVER_DESC);
 MODULE_AUTHOR ("David Brownell");
 MODULE_LICENSE ("GPL");
 
+/* Cancel IO, To store the bulkin and bulkout ep data. */
+static struct ep_data *gp_ep_bulkin_data;
+static struct ep_data *gp_ep_bulkout_data;
 
 /*----------------------------------------------------------------------*/
 
@@ -265,6 +270,10 @@ static const char *CHIP;
 #define INFO(dev,fmt,args...) \
 	xprintk(dev , KERN_INFO , fmt , ## args)
 
+/* Cancel IO */
+static int mtp_ctrl_cmd;
+static int gbCancelFlag;
+static unsigned long mtptimestamp;
 
 /*----------------------------------------------------------------------*/
 
@@ -275,6 +284,17 @@ static const char *CHIP;
  * precise FIFO status when recovering from cancellation.
  */
 
+/* Cancel IO */
+static void cancel_io_process(struct work_struct *work)
+{
+	if (gp_ep_bulkout_data->req->status == -EINPROGRESS)
+		usb_ep_dequeue(gp_ep_bulkout_data->ep, gp_ep_bulkout_data->req);
+
+	if (gp_ep_bulkin_data->req->status == -EINPROGRESS)
+		usb_ep_dequeue(gp_ep_bulkin_data->ep, gp_ep_bulkin_data->req);
+}
+static DECLARE_DELAYED_WORK(cancel_work, cancel_io_process);
+
 static void epio_complete (struct usb_ep *ep, struct usb_request *req)
 {
 	struct ep_data	*epdata = ep->driver_data;
@@ -870,10 +890,13 @@ ep_open (struct inode *inode, struct file *fd)
 {
 	struct ep_data		*data = inode->i_private;
 	int			value = -EBUSY;
+	char *epin = "ep1in";
+	char *epout = "ep1out";
 
 	if (down_interruptible (&data->lock) != 0)
 		return -EINTR;
 	spin_lock_irq (&data->dev->lock);
+
 	if (data->dev->state == STATE_DEV_UNBOUND)
 		value = -ENOENT;
 	else if (data->state == STATE_EP_DISABLED) {
@@ -882,9 +905,16 @@ ep_open (struct inode *inode, struct file *fd)
 		get_ep (data);
 		fd->private_data = data;
 		VDEBUG (data->dev, "%s ready\n", data->name);
+		/* Cancel IO */
+		if (0 == strcmp(data->name, epin))
+			gp_ep_bulkin_data = fd->private_data;
+
+		if (0 == strcmp(data->name, epout))
+			gp_ep_bulkout_data = fd->private_data;
 	} else
 		DBG (data->dev, "%s state %d\n",
 			data->name, data->state);
+
 	spin_unlock_irq (&data->dev->lock);
 	up (&data->lock);
 	return value;
@@ -996,7 +1026,7 @@ ep0_read (struct file *fd, char __user *buf, size_t len, loff_t *ptr)
 			retval = -EL2HLT;
 			dev->state = STATE_DEV_CONNECTED;
 
-		} else if (len == 0) {		/* ack SET_CONFIGURATION etc */
+		} else if (len == 0) { /* ack SET_CONFIGURATION etc */
 			struct usb_ep		*ep = dev->gadget->ep0;
 			struct usb_request	*req = dev->req;
 
@@ -1017,7 +1047,7 @@ ep0_read (struct file *fd, char __user *buf, size_t len, loff_t *ptr)
 				usb_gadget_vbus_draw(dev->gadget, 2 * power);
 			}
 
-		} else {			/* collect OUT data */
+		} else { /* collect OUT data */
 			if ((fd->f_flags & O_NONBLOCK) != 0
 					&& !dev->setup_out_ready) {
 				retval = -EAGAIN;
@@ -1042,11 +1072,15 @@ ep0_read (struct file *fd, char __user *buf, size_t len, loff_t *ptr)
 				retval = -EIO;
 			else {
 				len = min (len, (size_t)dev->req->actual);
-// FIXME don't call this with the spinlock held ...
+/* FIXME don't call this with the spinlock held ... */
 				if (copy_to_user (buf, dev->req->buf, len))
 					retval = -EFAULT;
+				else
+					/* Bug of Cancel IO 6 bytes read. */
+					retval = len;
 				clean_req (dev->gadget->ep0, dev->req);
 				/* NOTE userspace can't yet choose to stall */
+				dev->state = STATE_DEV_CONNECTED;	/* Cancel IO */
 			}
 		}
 		goto done;
@@ -1060,6 +1094,12 @@ ep0_read (struct file *fd, char __user *buf, size_t len, loff_t *ptr)
 	len -= len % sizeof (struct usb_gadgetfs_event);
 	dev->usermode_setup = 1;
 
+    /* Cancel IO, signal abort blocked IO. */
+	if (mtp_ctrl_cmd == 1) {
+		mtp_ctrl_cmd = 0;
+		schedule_delayed_work(&cancel_work, HZ / 100);
+	}
+
 scan:
 	/* return queued events right away */
 	if (dev->ev_next != 0) {
@@ -1386,6 +1426,16 @@ gadgetfs_setup (struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 	struct usb_gadgetfs_event	*event;
 	u16				w_value = le16_to_cpu(ctrl->wValue);
 	u16				w_length = le16_to_cpu(ctrl->wLength);
+	struct timeval tv;
+
+    /* Cancel IO */
+	if (0x67 == ctrl->bRequest && 1 == gbCancelFlag
+	    && dev->state == STATE_DEV_SETUP)
+		dev->state = STATE_DEV_CONNECTED;
+
+	if (0x67 == ctrl->bRequest && 2 == mtp_ctrl_cmd
+	    && dev->state == STATE_DEV_SETUP)
+		dev->state = STATE_DEV_CONNECTED;
 
 	spin_lock (&dev->lock);
 	dev->setup_abort = 0;
@@ -1413,6 +1463,11 @@ gadgetfs_setup (struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 	 */
 	} else if (dev->state == STATE_DEV_SETUP)
 		dev->setup_abort = 1;
+	/*Cancel IO */
+	if (mtp_ctrl_cmd == 1 && gbCancelFlag == 1 && dev->setup_abort == 1) {
+		INFO(dev, "0x64->setup\n");
+		dev->setup_abort = 0;
+	}
 
 	req->buf = dev->rbuf;
 	req->dma = DMA_ADDR_INVALID;
@@ -1438,7 +1493,7 @@ gadgetfs_setup (struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 			make_qualifier (dev);
 			break;
 		case USB_DT_OTHER_SPEED_CONFIG:
-			// FALLTHROUGH
+			/* FALLTHROUGH */
 #endif
 		case USB_DT_CONFIG:
 			value = config_buf (dev,
@@ -1545,11 +1600,63 @@ delegate:
 				/* we can't currently stall these */
 				dev->setup_can_stall = 0;
 			}
+			/* Cancel IO */
+			if (0x67 == ctrl->bRequest && 1 == gbCancelFlag) {
+				gbCancelFlag = 0;
+
+				setup_req(gadget->ep0, dev->req, 4);
+				*(unsigned long *)dev->req->buf = 0x20190004;
+				usb_ep_queue(gadget->ep0, dev->req, GFP_ATOMIC);
+
+				spin_unlock(&dev->lock);
+				return 0;
+			}
+			if (ctrl->bRequest == 0x67 && mtp_ctrl_cmd == 2) {
+				/* get status */
+				mtp_ctrl_cmd = 0;
+			}
 
 			/* state changes when reader collects event */
 			event = next_event (dev, GADGETFS_SETUP);
 			event->u.setup = *ctrl;
+			/*  Cancel IO */
+			if (0x64 == ctrl->bRequest) {
+				mtp_ctrl_cmd = 1;
+				gbCancelFlag = 1;
+
+				/* get the timestamp */
+				do_gettimeofday(&tv);
+				mtptimestamp = tv.tv_usec;
+				event->u.setup.wValue =
+				    (unsigned short)tv.tv_usec;
+			}
+			if (0x66 == ctrl->bRequest) {
+				/* get the timestamp */
+				do_gettimeofday(&tv);
+				mtptimestamp = tv.tv_usec;
+				event->u.setup.wValue =
+				    (unsigned short)tv.tv_usec;
+			}
+
 			ep0_readable (dev);
+			/* Reset request. */
+			if (ctrl->bRequest == 0x66) {	/* reset ,send ZLP */
+				mtp_ctrl_cmd = 2;
+
+				if (gp_ep_bulkout_data->req->status ==
+				    -EINPROGRESS) {
+					usb_ep_dequeue(gp_ep_bulkout_data->ep,
+						       gp_ep_bulkout_data->req);
+				}
+				if (gp_ep_bulkin_data->req->status ==
+				    -EINPROGRESS) {
+					usb_ep_dequeue(gp_ep_bulkin_data->ep,
+						       gp_ep_bulkin_data->req);
+				}
+			}
+			if (ctrl->bRequest == 0x65)
+				pr_debug("i:0x65,not supported\n");
+
 			spin_unlock (&dev->lock);
 			return 0;
 		}
diff --git a/drivers/usb/gadget/s3c2443_udc.c b/drivers/usb/gadget/s3c2443_udc.c
new file mode 100644
index 000000000000..3eaf7d14f84a
--- /dev/null
+++ b/drivers/usb/gadget/s3c2443_udc.c
@@ -0,0 +1,2810 @@
+/* -*- linux-c -*-
+ *
+ * drivers/usb/gadget/s3c24xx_udc.c
+ *
+ * Samsung S3C on-chip full/high speed USB device controllers
+ *
+ * $Id: s3c-udc-hs.c,v 1.26 2007/02/22 09:45:04 ihlee215 Exp $*
+ *
+ * Copyright (C) 2006 for Samsung Electronics
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include "s3c2443_udc.h"
+#include <linux/platform_device.h>
+#include <linux/moduleparam.h>
+#include <linux/bug.h>
+
+/* @TODO: USB Device DMA support */
+#define RX_DMA_MODE 0
+#define TX_DMA_MODE 0
+
+#if 0
+#define DEBUG_S3C2443_UDC
+#endif
+
+#define pk_err(fmt, args...)			printk(KERN_ERR "[ ERROR ] s3c2443-udc: " fmt, ## args)
+#define pk_info(fmt, args...)			printk(KERN_DEBUG "s3c2443-udc: " fmt, ## args)
+
+#ifdef DEBUG_S3C2443_UDC
+#define pk_dbg(fmt, args...)			printk(KERN_DEBUG "s3c2443-udc: %s() " fmt, __func__ , \
+							## args)
+#else
+#define pk_dbg(fmt, args...)			do { } while(0)
+#endif
+
+#if 0
+#define S3C2443_UDC_DBG_OUT
+#endif
+
+#if defined(S3C2443_UDC_DBG_OUT)
+#define pk_dbg_out(fmt, args...)		printk(KERN_DEBUG "[OUT] " fmt, ## args)
+#else
+#define pk_dbg_out(fmt, args...)		do { } while(0)
+#endif /* S3C2443_UDC_DBG_OUT */
+
+/*
+ * This macro enables the debug messages when the driver is going to access to the
+ * internal queue of the IN-endpoints
+ */
+#if 0
+#define DEBUG_S3C2443_UDC_QUEUE
+#endif
+
+/* Some driver infos */
+#define	DRIVER_DESC		                "S3C2443 Dual-speed USB Device"
+#define DRIVER_NAME                             "s3c2443_udc"
+#define	DRIVER_BUILD_TIME	                 __TIME__
+#define	DRIVER_BUILD_DATE	                 __DATE__
+
+#define IOMEMSIZE(s)		                (s->end - s->start + 1)
+
+/* Internal variables */
+struct s3c24xx_udc *the_controller;
+static const char driver_desc[] = DRIVER_DESC;
+static const char ep0name[] = "ep0-control";
+
+/* Max packet sizes */
+static u32 ep0_fifo_size = 64;
+static u32 ep_fifo_size =  512;
+static u32 ep_fifo_size2 = 1024;
+
+/* Internal functions */
+static int s3c24xx_udc_ep_enable(struct usb_ep *ep,
+				 const struct usb_endpoint_descriptor *);
+static int s3c24xx_udc_ep_disable(struct usb_ep *ep);
+static struct usb_request *s3c24xx_udc_alloc_request(struct usb_ep *ep, gfp_t gfp_flags);
+static void s3c24xx_udc_free_request(struct usb_ep *ep, struct usb_request *);
+static int s3c24xx_udc_queue(struct usb_ep *ep, struct usb_request *, gfp_t gfp_flags);
+static int s3c24xx_udc_dequeue(struct usb_ep *ep, struct usb_request *);
+static int s3c24xx_udc_set_halt(struct usb_ep *ep, int);
+static int s3c24xx_udc_fifo_status(struct usb_ep *ep);
+static void s3c24xx_udc_fifo_flush(struct usb_ep *ep);
+static void s3c24xx_udc_ep0_kick(struct s3c24xx_udc *udc, struct s3c_ep *ep);
+static void s3c24xx_handle_ep0(struct s3c24xx_udc *udc);
+static void done(struct s3c_ep *ep, struct s3c_request *req, int status);
+static void stop_activity(struct s3c24xx_udc *dev, struct usb_gadget_driver *driver);
+static int s3c24xx_udc_enable(struct s3c24xx_udc *udc);
+static void s3c24xx_udc_set_address(struct s3c24xx_udc *dev, unsigned char address);
+static void reconfig_usbd(struct s3c24xx_udc *udc);
+static void s3c24xx_ep0_setup(struct s3c24xx_udc *udc);
+static int s3c24xx_udc_write_fifo(struct s3c_ep *ep, struct s3c_request *req);
+
+
+static inline struct s3c24xx_udc *gadget_to_udc(struct usb_gadget *gadget)
+{
+        return container_of(gadget, struct s3c24xx_udc, gadget);
+}
+
+static spinlock_t regs_lock = SPIN_LOCK_UNLOCKED;
+
+static inline void s3c2443_print_err_packet_setup(int errcode,
+						  struct usb_ctrlrequest *pctrl)
+{
+	printk(KERN_DEBUG "[ ERROR ] s3c2443-udc: Err %i | bRequestType 0x%02x | "
+	       "bRequest 0x%02x | wValue 0x%04x | wIndex 0x%04x | wLength %u\n",
+	       errcode, pctrl->bRequestType, pctrl->bRequest,
+	       pctrl->wValue, pctrl->wIndex, pctrl->wLength);
+}
+
+/* Read access to one of the indexed registers */
+static inline ulong usb_read(struct s3c24xx_udc *udc, ulong port, u8 ind)
+{
+	ulong retval;
+
+	spin_lock(&regs_lock);
+	writel(ind, udc->base + S3C24XX_UDC_IR_REG);
+	retval = readl(udc->base + port);
+	spin_unlock(&regs_lock);
+	return retval;
+}
+
+/* Write access to one of the indexed registers */
+static inline void usb_write(struct s3c24xx_udc *udc, ulong val, ulong port, u8 ind)
+{
+	spin_lock(&regs_lock);
+	writel(ind, udc->base + S3C24XX_UDC_IR_REG);
+	writel(val, udc->base + port);
+	spin_unlock(&regs_lock);
+}
+
+static inline void usb_set(struct s3c24xx_udc *udc, ulong val, ulong port, u8 ind)
+{
+	spin_lock(&regs_lock);
+	writel(ind, udc->base + S3C24XX_UDC_IR_REG);
+	writel(readl(udc->base + port) | val, udc->base + port);
+	spin_unlock(&regs_lock);
+}
+
+static inline void usb_clear(struct s3c24xx_udc *udc, ulong val, ulong port, u8 ind)
+{
+	spin_lock(&regs_lock);
+	writel(ind, udc->base + S3C24XX_UDC_IR_REG);
+	writel(readl(udc->base + port) & ~val, udc->base + port);
+	spin_unlock(&regs_lock);
+}
+
+/* Return a value different than zero if the EP is enabled */
+static inline int s3c24xx_ep_enabled(struct s3c24xx_udc *udc, int epnr)
+{
+	ulong regval;
+
+	regval = readl(udc->base + S3C24XX_UDC_EIER_REG);
+	return (regval & (1 << epnr));
+}
+
+/* Enable/disable the interrupt of the passed EP-number */
+static inline void s3c24xx_ep_irq_enable(struct s3c24xx_udc *udc, int epnr, int enable)
+{
+	ulong eier;
+
+	eier = readl(udc->base + S3C24XX_UDC_EIER_REG);
+	if (enable)
+		eier |= (1 << epnr);
+	else
+		eier &= ~(1 << epnr);
+	writel(eier, udc->base + S3C24XX_UDC_EIER_REG);
+}
+
+static inline void s3c2443_udc_print_regs(char *marke, struct s3c24xx_udc *udc, int epnr)
+{
+	struct regs_t {
+		char *name;
+		ulong addr;
+	};
+
+	int pos, old_epnr;
+	ulong regval;
+	static const struct regs_t regs[] = {
+		{ "EIR   ", S3C24XX_UDC_EIR_REG },
+		{ "EIER  ", S3C24XX_UDC_EIER_REG },
+		{ "EDR   ", S3C24XX_UDC_EDR_REG },
+		{ "TR    ", S3C24XX_UDC_TR_REG },
+		{ "SSR   ", S3C24XX_UDC_SSR_REG },
+		{ "SCR   ", S3C24XX_UDC_SCR_REG },
+		{ "EP0SR ", S3C24XX_UDC_EP0SR_REG },
+		{ "FCON  ", S3C24XX_UDC_FIFO_CON_REG },
+		{ "FSTAT ", S3C24XX_UDC_FIFO_STATUS_REG },
+		{ "ESR   ", S3C24XX_UDC_ESR_REG },
+		{ "ECR   ", S3C24XX_UDC_ECR_REG },
+		{ "BRCR  ", S3C24XX_UDC_BRCR_REG },
+		{ "BWCR  ", S3C24XX_UDC_BWCR_REG },
+	};
+
+	/* First get a backup of the current EP number */
+	old_epnr = readl(udc->base + S3C24XX_UDC_IR_REG);
+	writel(epnr, udc->base + S3C24XX_UDC_IR_REG);
+
+	/* Now print all the registers */
+	printk(KERN_DEBUG "%s\n", marke);
+	for (pos = 0; pos < ARRAY_SIZE(regs); pos++) {
+		regval = usb_read(udc, regs[pos].addr, epnr);
+		printk(KERN_DEBUG "%s: 0x%08lx\n", regs[pos].name, regval);
+	}
+
+	writel(old_epnr, udc->base + S3C24XX_UDC_IR_REG);
+}
+
+static struct usb_ep_ops s3c24xx_ep_ops = {
+	.enable = s3c24xx_udc_ep_enable,
+	.disable = s3c24xx_udc_ep_disable,
+
+	.alloc_request = s3c24xx_udc_alloc_request,
+	.free_request = s3c24xx_udc_free_request,
+
+	.queue = s3c24xx_udc_queue,
+	.dequeue = s3c24xx_udc_dequeue,
+
+	.set_halt = s3c24xx_udc_set_halt,
+	.fifo_status = s3c24xx_udc_fifo_status,
+	.fifo_flush = s3c24xx_udc_fifo_flush,
+};
+
+/*
+ * Function for writing from the request buffer into the EP-FIFO
+ * The function updates the internal actual length of the USB-request for a possible
+ * next transfer of the same request.
+ * The return value is the number of remaining bytes in the request. If the return
+ * value is equal zero, then there is no more data to process in the request
+ * (Luis Galdos)
+ */
+static inline int s3c24xx_udc_write_packet(struct s3c_ep *ep, struct s3c_request *req)
+{
+	u16 *buf;
+	int length, count;
+	u32 fifo = ep->fifo;
+	struct s3c24xx_udc *udc;
+	int max, remaining, epnr;
+	u8 *ptr;
+
+	/* @XXX: Need some sanity checks (Luis Galdos) */
+	udc = ep->dev;
+	max = ep->ep.maxpacket;
+	epnr = ep_index(ep);
+
+	/* Get the number of remaining bytes */
+	remaining = req->req.length - req->req.actual;
+	if (!remaining) {
+		pk_dbg("EP%i: Sending ZLP (actual: %i)\n",
+			     epnr, req->req.actual);
+
+		/* Send a frame with zero length */
+		/* usb_set(udc, S3C24XX_UDC_ECR_TZLS, S3C24XX_UDC_ECR_REG, epnr); */
+		usb_write(udc, 0, S3C24XX_UDC_BWCR_REG, epnr);
+
+		length = remaining;
+		goto exit_write_packet;
+	}
+
+	/* Use first a u8 pointer for obtaining the correct buffer address */
+	ptr = req->req.buf + req->req.actual;
+	buf = (u16 *)ptr;
+	prefetch(buf);
+
+	/* Only send the maximal allowed number of bytes */
+	length = min(remaining, max);
+	req->req.actual += length;
+
+	/* First write the number of bytes to transfer, and then fill the FIFO */
+	usb_write(udc, length, S3C24XX_UDC_BWCR_REG, epnr);
+	for (count = 0; count < length; count += 2)
+		writel(*buf++, udc->base + fifo);
+
+	/* Return the number of remaining bytes of the passed request */
+exit_write_packet:
+	return (remaining - length);
+}
+
+/*
+ * Test function which returns the number of bytes written into the FIFO.
+ * This new function was implemented due an unknown issue registered with the
+ * Ethernet-gadget (the UDC send packets with size of 514 bytes!)
+ * (Luis Galdos)
+ */
+static inline int s3c24xx_udc_write_packet2(struct s3c_ep *ep, struct s3c_request *req)
+{
+	u16 *buf;
+	int length, count;
+	u32 fifo = ep->fifo;
+	struct s3c24xx_udc *udc;
+	int max, remaining, epnr;
+	u8 *ptr;
+
+	udc = ep->dev;
+	max = ep->ep.maxpacket;
+	epnr = ep_index(ep);
+
+	/* Get the number of remaining bytes */
+	remaining = req->req.length - req->req.actual;
+	if (!remaining) {
+
+		pk_dbg("EP%i: Sending ZLP (actual: %i)\n", epnr,
+			     req->req.actual);
+
+		/*
+		 * Send a frame with zero length.
+		 * DONT use the TZLS control bit of the EP control register ECR.
+		 */
+		usb_write(udc, 0, S3C24XX_UDC_BWCR_REG, epnr);
+		length = 0;
+		goto exit_write_packet;
+	}
+
+	/* Use first an u8 pointer for obtaining the correct buffer address */
+	ptr = req->req.buf + req->req.actual;
+	buf = (u16 *)ptr;
+	prefetch(buf);
+
+	/* Only send the maximal allowed number of bytes */
+	length = min(remaining, max);
+	req->req.actual += length;
+
+	/* First write the number of bytes to transfer, and then fill the FIFO */
+	usb_write(udc, length, S3C24XX_UDC_BWCR_REG, epnr);
+	for (count = 0; count < length; count += 2)
+		writel(*buf++, udc->base + fifo);
+
+	/* Sanity check before writting into the FIFO */
+#if defined(DEBUG_S3C2443_UDC_QUEUE)
+	{
+		ulong esr;
+
+		esr = usb_read(udc, S3C24XX_UDC_ESR_REG, ep_index(ep));
+		printk(KERN_DEBUG
+		       "%p: len=%i, act=%i, ep=%02x, bwcr=0x%04x, esr=0x%04lx\n",
+		       req, req->req.length, req->req.actual, epnr, length, esr);
+	}
+#endif
+
+	/* Return the number of remaining bytes of the passed request */
+ exit_write_packet:
+	return length;
+}
+
+/*
+ * Check the current state of the VBUS pin. If no VBUS pin was passed through the
+ * platform data, then assume the bus is always ON.
+ * (Luis Galdos)
+ */
+static inline int s3c2443_udc_vbus_state(struct s3c24xx_udc *udc)
+{
+	int retval;
+	struct s3c2410_udc_mach_info *info;
+
+	info = udc->mach_info;
+	retval = 1;
+	if (info && info->vbus_pin) {
+		unsigned long iocfg;
+
+		/* @XXX: Do we really need to change to INPUT first? */
+		iocfg = s3c2410_gpio_getcfg(info->vbus_pin);
+		s3c2410_gpio_cfgpin(info->vbus_pin, S3C2410_GPIO_INPUT);
+                retval = s3c2410_gpio_getpin(info->vbus_pin);
+                s3c2410_gpio_cfgpin(info->vbus_pin, iocfg);
+
+                if (info->vbus_pin_inverted)
+                        retval = !retval;
+	}
+
+	return retval;
+}
+
+/*
+ * Disable the controller by resetting the PHY for informing the USB-host
+ * that the device was disconnected
+ * (Luis Galdos)
+ */
+static void s3c24xx_udc_disable(struct s3c24xx_udc *udc)
+{
+	ulong regval;
+
+	pk_dbg("UDC disable called\n");
+
+	/* Disable the EP interrupts */
+	writel(0, udc->base + S3C24XX_UDC_EIER_REG);
+	writel(0xff, udc->base + S3C24XX_UDC_EIR_REG);
+
+	/* Clear all the status bits of the EP0 and flush it */
+	writel(S3C24XX_UDC_EP0SR_RSR | S3C24XX_UDC_EP0SR_TST |
+	       S3C24XX_UDC_EP0SR_SHT | S3C24XX_UDC_EP0SR_LWO,
+	       udc->base + S3C24XX_UDC_EP0SR_REG);
+	writel(0, udc->base + S3C24XX_UDC_EP0CR_REG);
+
+	/* Unset the function address */
+	s3c24xx_udc_set_address(udc, 0);
+
+	udc->ep0state = WAIT_FOR_SETUP;
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	udc->usb_address = 0;
+
+	/* Clear all the status bits from the system status register */
+	regval = S3C24XX_UDC_INT_RESET | S3C24XX_UDC_INT_SUSPEND |
+		S3C24XX_UDC_INT_RESUME | S3C24XX_UDC_INT_SDE |
+		S3C24XX_UDC_SSR_TBM | S3C24XX_UDC_INT_VBUSON |
+		S3C24XX_UDC_SSR_VBUSOFF;
+	writel(regval, udc->base + S3C24XX_UDC_SSR_REG);
+
+	/* Reset the USB-function and the PHY */
+	writel(S3C2443_URSTCON_PHY | S3C2443_URSTCON_FUNC, S3C2443_URSTCON);
+
+	/* PHY power disable */
+	regval = readl(S3C2443_PWRCFG);
+	regval &= ~S3C2443_PWRCFG_USBPHY_ON;
+	writel(regval, S3C2443_PWRCFG);
+}
+
+/*
+ * Function for sending request data to the FIFO
+ * This function uses the EP-lock for avoiding the wrong queue order of the packets
+ * that are incoming from the Gadget-driver
+ * (Luis Galdos)
+ */
+static void s3c24xx_udc_epin_tasklet_func(unsigned long data)
+{
+	struct s3c_ep *ep;
+	struct s3c_request *req;
+	int retval;
+	ulong esr;
+	struct s3c24xx_udc *udc;
+
+	ep = (struct s3c_ep *)data;
+	if (!ep) {
+		pk_err("Invalid EP pointer. Aborting %s\n", __func__);
+		return;
+	}
+
+	spin_lock(&ep->lock);
+
+	udc = ep->dev;
+	esr = usb_read(udc, S3C24XX_UDC_ESR_REG, ep_index(ep));
+
+	/*
+	 * Paranoic sanity check: If the FIFO has still a packet, then abort this
+	 * tasklet and wait for the call from the interrupt handler (TPS)
+	 */
+	if (S3C24XX_UDC_ESR_PSIFNR(esr) == 2) {
+		pk_dbg("The FIFO seems to have still a packet\n");
+		goto exit_unlock;
+	}
+
+	/* Check if there is a pending request for us */
+	if (list_empty(&ep->queue))
+		goto exit_unlock;
+
+	/* Get the next request from the queue of the endpoint */
+	req = list_entry(ep->queue.next, struct s3c_request, queue);
+	if (!req) {
+		pk_err("EP%i: NULL request pointer.\n", ep_index(ep));
+		goto exit_unlock;
+	}
+
+#if defined(DEBUG_S3C2443_UDC_QUEUE)
+	{
+		u8 ch1, ch2;
+		int len, act;
+		u8 *ptr = (u8 *)req->req.buf;
+		len = req->req.length;
+		act = req->req.actual;
+		ch1 = *ptr;
+		ch2 = *(ptr + len - 1);
+		printk(KERN_DEBUG "%p: act=%i, ep=%02x, 0x%02x ... 0x%02x\n",
+		       req, act, ep_index(ep), ch1, ch2);
+	}
+#endif
+	retval = s3c24xx_udc_write_fifo(ep, req);
+
+ exit_unlock:
+	spin_unlock(&ep->lock);
+}
+
+/*
+ * Restart the UDC and the corresponding resources (tasklet, queues, etc.)
+ * (Luis Galdos)
+ */
+static void s3c24xx_udc_reinit(struct s3c24xx_udc *udc)
+{
+	u32 i;
+
+	/* device/ep0 records init */
+	INIT_LIST_HEAD(&udc->gadget.ep_list);
+	INIT_LIST_HEAD(&udc->gadget.ep0->ep_list);
+	udc->ep0state = WAIT_FOR_SETUP;
+
+	/* basic endpoint records init */
+	for (i = 0; i < S3C_MAX_ENDPOINTS; i++) {
+		struct s3c_ep *ep = &udc->ep[i];
+
+		if (i != 0)
+			list_add_tail(&ep->ep.ep_list, &udc->gadget.ep_list);
+
+		ep->desc = 0;
+		ep->stopped = 0;
+		INIT_LIST_HEAD(&ep->queue);
+		ep->pio_irqs = 0;
+	}
+
+	/* the rest was statically initialized, and is read-only */
+}
+
+#define BYTES2MAXP(x)	(x / 8)
+#define MAXP2BYTES(x)	(x * 8)
+
+/*
+ * Until it's enabled, this UDC should be completely invisible
+ * to any USB host.
+ */
+static int s3c24xx_udc_enable(struct s3c24xx_udc *udc)
+{
+	unsigned long regval;
+
+	pk_dbg("UDC enable called\n");
+
+	/* First disable the HOST functionality! */
+	regval = __raw_readl(S3C2443_UCLKCON);
+	regval &= ~S3C2443_UCLKCON_HOST_ENABLE;
+	regval |= S3C2443_UCLKCON_THOST_DISABLE;
+	__raw_writel(regval, S3C2443_UCLKCON);
+
+	/* if reset by sleep wakeup, control the retention I/O cell */
+	if (__raw_readl(S3C2443_RSTSTAT) & 0x8)
+		__raw_writel(__raw_readl(S3C2443_RSTCON)|(1<<16), S3C2443_RSTCON);
+
+	/* PHY power enable */
+	regval = __raw_readl(S3C2443_PWRCFG);
+	regval |= S3C2443_PWRCFG_USBPHY_ON;
+	__raw_writel(regval, S3C2443_PWRCFG);
+
+	/*
+	 * USB device 2.0 must reset like bellow,
+	 * 1st phy reset and after at least 10us, func_reset & host reset
+	 * phy reset can reset bellow registers.
+	 */
+	/* PHY 2.0 S/W reset */
+	regval = S3C2443_URSTCON_PHY;
+	__raw_writel(regval, S3C2443_URSTCON);
+	udelay(20);
+	__raw_writel(0x00, S3C2443_URSTCON);
+
+	/* Function reset, but DONT TOUCH THE HOST! */
+	regval = S3C2443_URSTCON_FUNC;
+	__raw_writel(regval, S3C2443_URSTCON);
+	__raw_writel(0x00, S3C2443_URSTCON);
+
+	/* 48Mhz, Oscillator, External X-tal, device */
+	regval = S3C2443_PHYCTRL_EXTCLK_OSCI;
+	__raw_writel(regval, S3C2443_PHYCTRL);
+
+	/*
+	 * D+ pull up disable(VBUS detect), USB2.0 Function clock Enable,
+	 * USB1.1 HOST disable, USB2.0 PHY test enable
+	 */
+	regval = __raw_readl(S3C2443_UCLKCON);
+	regval |= S3C2443_UCLKCON_FUNC_ENABLE;
+	__raw_writel(regval, S3C2443_UCLKCON);
+
+	reconfig_usbd(udc);
+
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+
+	/*
+	 * So, now enable the pull up, USB2.0 Function clock Enable and
+	 * USB2.0 PHY test enable
+	 */
+	regval = __raw_readl(S3C2443_UCLKCON);
+	regval |= S3C2443_UCLKCON_VBUS_PULLUP | S3C2443_UCLKCON_FUNC_ENABLE |
+		S3C2443_UCLKCON_TFUNC_ENABLE | S3C2443_UCLKCON_THOST_DISABLE;
+	__raw_writel(regval, S3C2443_UCLKCON);
+	return 0;
+}
+
+/*
+ * Function called from the Gadget-drivers for registering a new profile.
+ */
+int usb_gadget_register_driver(struct usb_gadget_driver *driver)
+{
+	struct s3c24xx_udc *udc = the_controller;
+	int retval;
+
+	if (!driver)
+		return -EINVAL;
+
+	pk_dbg("Starting to register '%s'\n", driver->driver.name);
+
+	if (driver->speed != USB_SPEED_FULL && driver->speed != USB_SPEED_HIGH) {
+		pk_err("Only Full and High speed supported.\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * The 'unbind' function is not required when the Gadget driver is compiled
+	 * as built-in (Luis Galdos)
+	 */
+	if (!driver->bind || !driver->disconnect || !driver->setup) {
+		pk_err("Missing function: Bind %p | Disconnect %p | Setup %p\n",
+			   driver->bind, driver->disconnect, driver->setup);
+		return -EINVAL;
+	}
+
+	if (!udc) {
+		pk_err("No UDC-controller probed? Aborting.\n");
+		return -ENODEV;
+	}
+
+	if (udc->driver) {
+		pk_err("UDC already in use by '%s'\n", udc->driver->driver.name);
+		return -EBUSY;
+	}
+
+	/* first hook up the driver ... */
+	udc->driver = driver;
+	udc->gadget.dev.driver = &driver->driver;
+
+	retval = device_add(&udc->gadget.dev);
+	if (retval) {
+		pk_err("Couldn't add the new Gadget device (%i)\n", retval);
+		goto err_exit;
+	}
+
+	retval = driver->bind(&udc->gadget);
+	if (retval) {
+		pk_err("%s: bind to driver %s --> error %d\n", udc->gadget.name,
+		       driver->driver.name, retval);
+		goto err_del_device;
+	}
+
+	enable_irq(IRQ_USBD);
+
+	/*
+	 * If a host was already detected, then only call the UDC enable function,
+	 * otherwise check over the configured GPIO if a host is connected.
+	 */
+	if (udc->vbus)
+		s3c24xx_udc_enable(udc);
+	else {
+		struct s3c2410_udc_mach_info *info;
+		unsigned long state, iocfg;
+
+		info = udc->mach_info;
+
+		iocfg = s3c2410_gpio_getcfg(info->vbus_pin);
+		s3c2410_gpio_cfgpin(info->vbus_pin, S3C2410_GPIO_INPUT);
+		state = s3c2410_gpio_getpin(info->vbus_pin);
+		s3c2410_gpio_cfgpin(info->vbus_pin, iocfg);
+
+		if (info->vbus_pin_inverted)
+			state = !state;
+
+		if (state)
+			s3c24xx_udc_enable(udc);
+	}
+
+	pk_dbg("Gadget '%s' registered\n", driver->driver.name);
+	return 0;
+
+ err_del_device:
+	device_del(&udc->gadget.dev);
+
+ err_exit:
+	udc->driver = NULL;
+	udc->gadget.dev.driver = NULL;
+	return retval;
+}
+
+EXPORT_SYMBOL(usb_gadget_register_driver);
+
+/*
+ * Unregister entry point for the peripheral controller driver.
+ */
+int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
+{
+	struct s3c24xx_udc *udc = the_controller;
+	unsigned long flags;
+
+	if (!udc)
+		return -ENODEV;
+
+	if (!driver || driver != udc->driver || !driver->unbind)
+		return -EINVAL;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	udc->driver = NULL;
+	stop_activity(udc, driver);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	driver->unbind(&udc->gadget);
+
+	device_del(&udc->gadget.dev);
+
+	disable_irq(IRQ_USBD);
+
+	pk_dbg("Unregistered gadget driver '%s'\n", driver->driver.name);
+
+	/* Disable the pull-up for informing the host about the removed driver */
+	s3c24xx_udc_disable(udc);
+
+	return 0;
+}
+
+EXPORT_SYMBOL(usb_gadget_unregister_driver);
+
+/*
+ * Write request to FIFO (max write == maxp size)
+ * Return:  0 = still running, 1 = completed, negative = errno
+ */
+static int s3c24xx_udc_write_fifo(struct s3c_ep *ep, struct s3c_request *req)
+{
+	int max, count;
+	int is_last, is_short;
+
+	count = s3c24xx_udc_write_packet2(ep, req);
+	max = le16_to_cpu(ep->desc->wMaxPacketSize);
+
+	is_short = (count !=  max) ? (1) : (0);
+
+	/* If the packet is short, we dont need to send an additional ZLP */
+	if (is_short) {
+		pk_dbg("EP%i: Short packet\n", ep_index(ep));
+		is_last = 1;
+	} else {
+		if (req->req.length != req->req.actual || req->req.zero)
+			is_last = 0;
+		else {
+			pk_dbg("EP%i: Clear ZLP\n", ep_index(ep));
+			is_last = 1;
+		}
+	}
+
+	pk_dbg("TX EP%i: C %i | L %i - A %i | %c %c %c\n",
+		     ep_index(ep), count, req->req.length, req->req.actual,
+		     is_last ? 'L':' ', is_short ? 'S':' ', req->req.zero ? 'Z':' ');
+
+	/* If this was the last packet, then call the done callback */
+	if (is_last) {
+
+#if defined(DEBUG_S3C2443_UDC_QUEUE)
+		int len, act;
+		len = req->req.length;
+		act = req->req.actual;
+		printk(KERN_DEBUG "%p: len=%i, act=%i, ep=%02x [D]\n",
+		       req, len, act, ep_index(ep));
+#endif
+
+		done(ep, req, 0);
+	}
+
+	return 0;
+}
+
+/*
+ * Read to request from FIFO (max read == bytes in fifo)
+ * Return: 0 = still running, 1 = completed, negative = errno
+ */
+static int s3c24xx_udc_read_fifo(struct s3c_ep *ep, struct s3c_request *req)
+{
+	u32 csr;
+	u16 *buf;
+	unsigned bufferspace, count, count_bytes, is_short = 0, is_done = 0;
+	u32 fifo = ep->fifo;
+	struct s3c24xx_udc *udc;
+
+	udc = ep->dev;
+	csr = usb_read(udc, S3C24XX_UDC_ESR_REG, ep_index(ep));
+
+	/*
+	 * If the FIFO is empty then return zero, so that a caller, like the queue-
+	 * function, doesn't fail. Returning zero means that the request is not done
+	 * and it can be added to the internal EP-request queue
+	 * (Luis Galdos)
+	 */
+	if (!(csr & S3C24XX_UDC_ESR_RPS)) {
+		pk_dbg("EP%i: No packet to read.\n", ep_index(ep));
+		return 0;
+	}
+
+	buf = req->req.buf + req->req.actual;
+	prefetchw(buf);
+
+	/* Calculate the current buffer space */
+	bufferspace = req->req.length - req->req.actual;
+
+	/* Read all bytes from this packet */
+	count = usb_read(udc, S3C24XX_UDC_BRCR_REG, ep_index(ep));
+	if (csr & S3C24XX_UDC_ESR_LWO)
+		count_bytes = count * 2 - 1;
+	else
+		count_bytes = count * 2;
+
+	/* Update the actual variable of the request */
+	req->req.actual += min(count_bytes, bufferspace);
+
+	is_short = (count_bytes < ep->ep.maxpacket);
+	is_done = (req->req.actual == req->req.length) ? 1 : 0;
+
+	/*
+	 * G : Got
+	 * A : Actual
+	 * T : Total to get
+	 */
+	if (is_short || is_done) {
+		pk_dbg_out("EP%u: G %d | A %d | T %d [%c%c]\n",
+			   ep_index(ep), count_bytes, req->req.actual, req->req.length,
+			   is_short ? 'S' : ' ', is_done ? 'D' : ' ');
+	}
+
+	while (likely(count-- != 0)) {
+		u16 byte = (u16)readl(udc->base + fifo);
+
+		/*
+		 * If there is no more space in the request-buffer, then continue
+		 * reading from the FIFO and return with the done value
+		 * (Luis Galdos)
+		 */
+		if (unlikely(bufferspace == 0)) {
+			req->req.status = -EOVERFLOW;
+			is_short = 1;
+		} else {
+			*buf++ = byte;
+			bufferspace--;
+		}
+	}
+
+	/*
+	 * If the complete FIFO-data passed into the request-buffer, then
+	 * return one, otherwise skip the return
+	 * (Luis Galdos)
+	 */
+	if (is_short || is_done) {
+		done(ep, req, 0);
+		return 1;
+	}
+
+	/* finished that packet.  the next one may be waiting... */
+	return 0;
+}
+
+/*
+ * Retire a request from the internal EP-queue and call the complete
+ * function of the Gadget-request
+ * (Luis Galdos)
+ */
+static void done(struct s3c_ep *ep, struct s3c_request *req, int status)
+{
+	unsigned int stopped = ep->stopped;
+
+	list_del_init(&req->queue);
+
+	/*
+	 * If the queue is empty and the EP has the OUT direction, then disable
+	 * the receive operation, otherwise we will lost some packets from
+	 * the host.
+	 */
+	if (!ep_is_in(ep) && list_empty(&ep->queue)) {
+		ulong ecr;
+		struct s3c24xx_udc *udc;
+
+		udc = ep->dev;
+		ecr = usb_read(udc, S3C24XX_UDC_ECR_REG, ep_index(ep));
+		ecr |= S3C24XX_UDC_ECR_OUTPKTHLD;
+		usb_write(udc, ecr, S3C24XX_UDC_ECR_REG, ep_index(ep));
+	}
+
+	if (likely(req->req.status == -EINPROGRESS))
+		req->req.status = status;
+	else
+		status = req->req.status;
+
+	if (status && status != -ESHUTDOWN) {
+		pk_dbg("EP%i: done req %p | stat %d | actual %u | length %u\n",
+			     ep_index(ep),
+			     &req->req, status, req->req.actual, req->req.length);
+	}
+
+	/* don't modify queue heads during completion callback */
+	ep->stopped = 1;
+
+	/*
+	 * We must unlock the queue of the EP at this place, then the Gadget-driver
+	 * probably will try to enqueue a new request by calling our queue-function.
+	 * (Luis Galdos)
+	 */
+/* 	spin_unlock(&ep->lock); */
+/* 	spin_unlock(&ep->dev->lock); */
+	req->req.complete(&ep->ep, &req->req);
+/* 	spin_lock(&ep->dev->lock); */
+/* 	spin_lock(&ep->lock); */
+
+	ep->stopped = stopped;
+}
+
+/* Nuke/dequeue all the requested transfers */
+void nuke(struct s3c_ep *ep, int status)
+{
+	struct s3c_request *req;
+
+	pk_dbg("EP%i: Nuke function called\n", ep_index(ep));
+
+	/* called with irqs blocked */
+	while (!list_empty(&ep->queue)) {
+		req = list_entry(ep->queue.next, struct s3c_request, queue);
+		done(ep, req, status);
+	}
+}
+
+/*
+ * This function handles the IN-operations of the endpoints different than zero
+ */
+static void s3c24xx_udc_in_epn(struct s3c24xx_udc *udc, u32 epnr)
+{
+	ulong esr, handled;
+	struct s3c_ep *ep = &udc->ep[epnr];
+
+	handled = 0;
+
+	spin_lock(&ep->lock);
+
+	esr = usb_read(udc, S3C24XX_UDC_ESR_REG, epnr);
+
+	/* ACK the function stall condition */
+	if (esr & S3C24XX_UDC_ESR_FSC) {
+		pk_dbg("EP%i: Function stall\n", epnr);
+		usb_set(udc, S3C24XX_UDC_ESR_FSC, S3C24XX_UDC_ESR_REG, epnr);
+		handled = 1;
+	}
+
+	/* The flush operation generates an interrupt too */
+	if (esr & S3C24XX_UDC_ESR_FFS) {
+		pk_dbg("EP%i: FIFO flush detected\n", epnr);
+		usb_set(udc, S3C24XX_UDC_ESR_FFS, S3C24XX_UDC_ESR_REG, epnr);
+		handled = 1;
+	}
+
+	/* Underflow check */
+	if (esr & S3C24XX_UDC_ESR_FUDR) {
+		pk_dbg("EP%i: Underflow detected\n", epnr);
+		usb_set(udc, S3C24XX_UDC_ESR_FUDR, S3C24XX_UDC_ESR_REG, epnr);
+		handled = 1;
+	}
+
+	/* Overflow check */
+	if (esr & S3C24XX_UDC_ESR_FOVF) {
+		pk_dbg("EP%i: Overflow detected\n", epnr);
+		usb_set(udc, S3C24XX_UDC_ESR_FOVF, S3C24XX_UDC_ESR_REG, epnr);
+		handled = 1;
+	}
+
+	/* By successed transfer of a IN-packet then only schedule the tasklet */
+	if (esr & S3C24XX_UDC_ESR_TPS) {
+		usb_set(udc, S3C24XX_UDC_ESR_TPS, S3C24XX_UDC_ESR_REG, epnr);
+		tasklet_hi_schedule(&ep->in_tasklet);
+		handled = 1;
+	}
+
+	spin_unlock(&ep->lock);
+
+	if (!handled)
+		pk_info("EP%i: Unhandled IRQ (ESR 0x%04lx)\n", epnr, esr);
+}
+
+/*
+ * This function is used for reading OUT-frames from the EP0. We can't use the same
+ * function for the SETUP-requests, then here we must pass the data to the
+ * higher Gadget-driver.
+ */
+static void s3c2443_udc_ep0_read(struct s3c24xx_udc *udc)
+{
+	ulong ep0sr;
+	int bytes, count, bufferspace;
+	struct s3c_ep *ep;
+	struct s3c_request *req;
+	u16 *buf;
+
+	ep = &udc->ep[0];
+
+	spin_lock(&ep->lock);
+
+	/*
+	 * @FIXME: Remove this delay. At this moment we need it for having a
+	 * working RNDIS-support when connected to a WinXP host machine.
+	 * (Luis Galdos)
+	 */
+	if (udc->ep0state == DATA_STATE_RECV)
+		udelay(100);
+
+	/* If there is nothing to read only return at this point */
+	ep0sr = readl(udc->base + S3C24XX_UDC_EP0SR_REG);
+        if (!(ep0sr & S3C24XX_UDC_EP0SR_RSR))
+		goto exit_unlock;
+
+	/* Check if we are waiting for a setup frame */
+	if (udc->ep0state == WAIT_FOR_SETUP) {
+		s3c24xx_ep0_setup(udc);
+		goto exit_unlock;
+	}
+
+	pk_dbg("Current state of EP0 is %i\n", udc->ep0state);
+
+	/* Now get the number of bytes to read from the FIFO */
+        count = usb_read(udc, S3C24XX_UDC_BRCR_REG, ep_index(ep));
+        if (ep0sr & S3C24XX_UDC_EP0SR_LWO)
+                bytes = count * 2 - 1;
+        else
+                bytes = count * 2;
+
+	/* Check if we have a request for this data */
+	req = list_entry(ep->queue.next, struct s3c_request, queue);
+
+	if (!req) {
+		pk_err("Going to flush a EP0 frame\n");
+		goto exit_ack;
+	}
+
+        buf = req->req.buf + req->req.actual;
+        prefetchw(buf);
+	bufferspace = req->req.length - req->req.actual;
+	req->req.actual += min(bytes, bufferspace);
+
+	pk_dbg("EP0 READ: %i bytes | space %i | req.len %i | reg.act %i\n",
+		     bytes, bufferspace, req->req.length, req->req.actual);
+	while (likely(count-- != 0)) {
+                u16 byte = (u16)readl(udc->base + ep->fifo);
+		*buf++ = byte;
+	}
+
+	/* If we are done with this request then call the corresponding function */
+	if (req->req.length == req->req.actual) {
+		udc->ep0state = WAIT_FOR_SETUP;
+		done(ep, req, 0);
+	}
+
+ exit_ack:
+	writel(S3C24XX_UDC_EP0_RX_SUCCESS, udc->base + S3C24XX_UDC_EP0SR_REG);
+
+ exit_unlock:
+	spin_unlock(&ep->lock);
+}
+
+
+/*
+ * The below function is called when data was received with an OUT-transaction
+ */
+static void s3c24xx_udc_out_epn(struct s3c24xx_udc *udc, u32 ep_idx)
+{
+	struct s3c_ep *ep;
+	struct s3c_request *req;
+	ulong esr, epnr, handled;
+
+	ep = &udc->ep[ep_idx];
+	epnr = ep_index(ep);
+	if (epnr != ep_idx) {
+		pk_err("Invalid EP structure (%lu) or index (%u) passed\n",
+			   epnr, ep_idx);
+		return;
+	}
+
+	/* Read the status register of the EP */
+	handled = 0;
+	esr = usb_read(udc, S3C24XX_UDC_ESR_REG, epnr);
+	pk_dbg("EP%lu: Status reg 0x%08lx\n", epnr, esr);
+
+	if (unlikely(!(ep->desc))) {
+		pk_err("No descriptor for EP%lu\n", epnr);
+		return;
+	}
+
+	if (esr & S3C24XX_UDC_ESR_FSC) {
+		pk_dbg("EP%lu stall sent\n", epnr);
+		usb_set(udc, S3C24XX_UDC_ESR_FSC, S3C24XX_UDC_ESR_REG, epnr);
+		handled = 1;
+	}
+
+	if (esr & S3C24XX_UDC_ESR_FFS) {
+		pk_dbg("EP%lu FIFO flush\n", epnr);
+		usb_set(udc, S3C24XX_UDC_ESR_FFS, S3C24XX_UDC_ESR_REG, epnr);
+		handled = 1;
+	}
+
+	/* RX means we have received some data over an OUT-transaction */
+	if (esr & S3C24XX_UDC_ESR_RPS) {
+		int retval;
+		ulong packets;
+
+		/*
+		 * The read-fifo function returns zero if there is
+		 * additional data in the FIFO. In that case read once
+		 * again from the FIFO
+		 */
+		packets = (esr & S3C24XX_UDC_ESR_PSIF_TWO) ? 2 : 1;
+		while (packets--) {
+
+			req = (list_empty(&ep->queue)) ? NULL :
+				list_entry(ep->queue.next, struct s3c_request, queue);
+
+			/*
+			 * If we dont have a request for the received data, then only
+			 * break the loop and return without flushing the FIFO.
+			 */
+			if (unlikely(!req)) {
+				ulong count_bytes, count;
+				ulong csr;
+
+				/* Read all bytes from this packet */
+				csr = usb_read(udc, S3C24XX_UDC_ESR_REG, ep_index(ep));
+				count = usb_read(udc, S3C24XX_UDC_BRCR_REG, ep_idx);
+				if (csr & S3C24XX_UDC_ESR_LWO)
+					count_bytes = count * 2 - 1;
+				else
+					count_bytes = count * 2;
+
+				pk_dbg_out("EP%lu: No OUT req. queued (len %lu)\n",
+					   epnr, count_bytes);
+				break;
+			}
+
+			retval = s3c24xx_udc_read_fifo(ep, req);
+			if (retval < 0) {
+				pk_err("EP%lu: FIFO read (%i)\n", epnr, retval);
+				break;
+			}
+		}
+
+		handled = 1;
+	}
+
+	/* Handlings for the overflow and underrun */
+	if (esr & S3C24XX_UDC_ESR_FOVF) {
+		pk_err("EP%lu FIFO overflow\n", epnr);
+		usb_set(udc, S3C24XX_UDC_ESR_FOVF, S3C24XX_UDC_ESR_REG, epnr);
+	}
+
+	if (esr & S3C24XX_UDC_ESR_FUDR) {
+		pk_err("EP%lu FIFO underrun\n", epnr);
+		usb_set(udc, S3C24XX_UDC_ESR_FUDR, S3C24XX_UDC_ESR_REG, epnr);
+	}
+
+	/*
+	 * Check if the function was handled, otherwise only uses a debug message
+	 * for informing about the error
+	 */
+	if (!handled) {
+		pk_dbg("EP%lu: Unhandled OUT | ESR 0x%08lx.\n", epnr, esr);
+	}
+}
+
+static void stop_activity(struct s3c24xx_udc *udc,
+			  struct usb_gadget_driver *driver)
+{
+	int i;
+
+	/* don't disconnect drivers more than once */
+	if (udc->gadget.speed == USB_SPEED_UNKNOWN)
+		driver = NULL;
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+
+	/* prevent new request submissions, kill any outstanding requests  */
+	for (i = 0; i < S3C_MAX_ENDPOINTS; i++) {
+		struct s3c_ep *ep = &udc->ep[i];
+		ep->stopped = 1;
+		nuke(ep, -ESHUTDOWN);
+	}
+
+	/* report disconnect; the driver is already quiesced */
+	if (driver) {
+		spin_unlock(&udc->lock);
+		driver->disconnect(&udc->gadget);
+		spin_lock(&udc->lock);
+	}
+
+	/* re-init driver-visible data structures */
+	s3c24xx_udc_reinit(udc);
+}
+
+static void reconfig_usbd(struct s3c24xx_udc *udc)
+{
+	struct s3c_ep *ep;
+	int cnt;
+	unsigned long edr;
+
+	/*
+	 * Configure the endpoints depending on the defined structure which
+	 * will be used by the gadget-drivers (see below)
+	 */
+	edr = 0;
+	for (cnt = 1; cnt < S3C_MAX_ENDPOINTS; cnt++) {
+		ep = &the_controller->ep[cnt];
+		if (ep->bEndpointAddress & USB_DIR_IN)
+			edr |= (1 << cnt);
+	}
+	writel(edr, udc->base + S3C24XX_UDC_EDR_REG);
+
+	/* Reset the endpoint configuration registers */
+	for (cnt = 1; cnt < S3C_MAX_ENDPOINTS; cnt++) {
+		ep = &the_controller->ep[cnt];
+		usb_write(udc, 0, S3C24XX_UDC_ECR_REG, ep_index(ep));
+	}
+
+	/* Only enable the EP0 */
+	writel(0x1, udc->base + S3C24XX_UDC_EIER_REG);
+
+ 	writel(0x0, udc->base + S3C24XX_UDC_TR_REG);
+
+	/* error interrupt enable, 16bit bus, Little format,
+	 * suspend&reset enable
+	 */
+	writel(S3C24XX_UDC_EIE_EN |
+	       S3C24XX_UDC_RRD_EN |
+	       S3C24XX_UDC_SUS_EN |
+	       S3C24XX_UDC_RST_EN,
+	       udc->base + S3C24XX_UDC_SCR_REG);
+
+	writel(0x0000, udc->base + S3C24XX_UDC_EP0CR_REG);
+
+	writel(0, udc->base + S3C24XX_UDC_IR_REG);
+}
+
+static void s3c24xx_set_max_pktsize(struct s3c24xx_udc *udc, enum usb_device_speed speed)
+{
+	if (speed == USB_SPEED_HIGH) {
+		ep0_fifo_size = 64;
+		ep_fifo_size = 512;
+		ep_fifo_size2 = 1024;
+		udc->gadget.speed = USB_SPEED_HIGH;
+	} else {
+		ep0_fifo_size = 64;
+		ep_fifo_size = 64;
+		ep_fifo_size2 = 64;
+		udc->gadget.speed = USB_SPEED_FULL;
+	}
+
+	udc->ep[0].ep.maxpacket = ep0_fifo_size;
+	udc->ep[1].ep.maxpacket = ep_fifo_size;
+	udc->ep[2].ep.maxpacket = ep_fifo_size;
+	udc->ep[3].ep.maxpacket = ep_fifo_size;
+	udc->ep[4].ep.maxpacket = ep_fifo_size;
+	udc->ep[5].ep.maxpacket = ep_fifo_size;
+	udc->ep[6].ep.maxpacket = ep_fifo_size;
+	udc->ep[7].ep.maxpacket = ep_fifo_size;
+	udc->ep[8].ep.maxpacket = ep_fifo_size;
+
+	usb_write(udc, ep0_fifo_size, S3C24XX_UDC_MAXP_REG, 0);
+	usb_write(udc, ep_fifo_size, S3C24XX_UDC_MAXP_REG, 1);
+	usb_write(udc, ep_fifo_size, S3C24XX_UDC_MAXP_REG, 2);
+	usb_write(udc, ep_fifo_size, S3C24XX_UDC_MAXP_REG, 3);
+	usb_write(udc, ep_fifo_size, S3C24XX_UDC_MAXP_REG, 4);
+	usb_write(udc, ep_fifo_size, S3C24XX_UDC_MAXP_REG, 5);
+	usb_write(udc, ep_fifo_size, S3C24XX_UDC_MAXP_REG, 6);
+	usb_write(udc, ep_fifo_size, S3C24XX_UDC_MAXP_REG, 7);
+	usb_write(udc, ep_fifo_size, S3C24XX_UDC_MAXP_REG, 8);
+}
+
+static int s3c24xx_udc_set_pullup(struct s3c24xx_udc *udc, int is_on)
+{
+	struct s3c2410_udc_mach_info *info;
+	enum s3c2410_udc_cmd_e cmd;
+
+	info = udc->mach_info;
+
+	if (is_on)
+		cmd = S3C2410_UDC_P_ENABLE;
+	else
+		cmd = S3C2410_UDC_P_DISABLE;
+
+	/* Call the platform dependet function if it's available */
+	if (info && info->udc_command) {
+		info->udc_command(cmd);
+	} else {
+                if (is_on) {
+			/*
+			 * Only enable the UDC if a Gadget-driver was already registered,
+			 * otherwise by the registration the UDC-enable function should
+			 * be called.
+			 * (Luis Galdos)
+			 */
+			if (udc->driver)
+				s3c24xx_udc_enable(udc);
+		} else {
+                        if (udc->gadget.speed != USB_SPEED_UNKNOWN) {
+                                if (udc->driver && udc->driver->disconnect)
+                                        udc->driver->disconnect(&udc->gadget);
+
+                        }
+                        s3c24xx_udc_disable(udc);
+                }
+        }
+
+        return 0;
+}
+
+static int s3c24xx_udc_vbus_session(struct usb_gadget *gadget, int is_active)
+{
+        struct s3c24xx_udc *udc = gadget_to_udc(gadget);
+
+        udc->vbus = (is_active != 0);
+        s3c24xx_udc_set_pullup(udc, is_active);
+        return 0;
+}
+
+/*
+ * This function is called by detection of the bus-power over the requested IRQ
+ */
+static irqreturn_t s3c24xx_udc_vbus_irq(int irq, void *_udc)
+{
+	struct s3c24xx_udc	*udc = _udc;
+	unsigned int		value;
+	struct s3c2410_udc_mach_info *info;
+	ulong cfg;
+
+	info = udc->mach_info;
+
+	/* Some cpus cannot read from an line configured to IRQ! */
+	cfg = s3c2410_gpio_getcfg(info->vbus_pin);
+	s3c2410_gpio_cfgpin(info->vbus_pin, S3C2410_GPIO_INPUT);
+	value = s3c2410_gpio_getpin(info->vbus_pin);
+	s3c2410_gpio_cfgpin(info->vbus_pin, cfg);
+
+	if (info->vbus_pin_inverted)
+		value = !value;
+
+	pk_dbg("Bus detect %s: vbus %i | value %i\n",
+		     info->vbus_pin_inverted ? "inverted" : "", udc->vbus, value);
+
+	if (value != udc->vbus)
+		s3c24xx_udc_vbus_session(&udc->gadget, value);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Interrupt handler of the USB-function. The interrupts to detect are coming
+ * from the SMDK, but it doesn't consider the speed detection, which can lead
+ * to some failures.
+ *
+ * (Luis Galdos)
+ */
+#define S3C2443_UDC_INT_CHECK			(0xff8f | S3C24XX_UDC_INT_HSP)
+static irqreturn_t s3c24xx_udc_irq(int irq, void *_udc)
+{
+	struct s3c24xx_udc *udc = _udc;
+	u32 intr_out, intr_in, intr_all;
+	u32 sys_stat, sys_stat_chk;
+	u32 stat, cnt;
+	unsigned long flags;
+	struct s3c_ep *ep;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	sys_stat = readl(udc->base + S3C24XX_UDC_SSR_REG);
+	stat = sys_stat;
+	intr_all = readl(udc->base + S3C24XX_UDC_EIR_REG);
+
+	/* We have only 3 usable eps now */
+	sys_stat_chk = sys_stat & S3C2443_UDC_INT_CHECK;
+
+	/* Only check for the correct endpoints (Luis Galdos) */
+	for (cnt = 0, intr_in = 0; cnt < S3C_MAX_ENDPOINTS; cnt++) {
+		ep = &udc->ep[cnt];
+
+		/* Skip the OUT-endpoints different than zero */
+		if (!(ep->bEndpointAddress & USB_DIR_IN) && cnt != 0)
+			continue;
+
+		if (s3c24xx_ep_enabled(udc, cnt))
+			intr_in |= (1 << cnt);
+	}
+	intr_in &= intr_all;
+
+	/* Check for the OUT-EPs that have generated an interrupt (Luis Galdos) */
+	for (cnt = 0, intr_out = 0; cnt < S3C_MAX_ENDPOINTS; cnt++) {
+		ep = &udc->ep[cnt];
+
+		/* Skip the IN-endpoints */
+		if (ep->bEndpointAddress & USB_DIR_IN)
+			continue;
+
+		if (s3c24xx_ep_enabled(udc, cnt))
+			intr_out |= (1 << cnt);
+	}
+	intr_out &= intr_all;
+
+	pk_dbg("UDC IRQ: stat 0x%08x (0x%08x) | in 0x%08x | out 0x%08x\n",
+		     stat, sys_stat_chk, intr_in, intr_out);
+
+	if (!intr_out && !intr_in && !sys_stat_chk)
+		goto exit_ack;
+
+	if (sys_stat) {
+		if (sys_stat & S3C24XX_UDC_INT_VBUSON) {
+			pk_dbg("Vbus ON interrupt\n");
+			writel(S3C24XX_UDC_INT_VBUSON, udc->base + S3C24XX_UDC_SSR_REG);
+			udc->vbus = 1;
+		}
+
+		if (sys_stat & S3C24XX_UDC_INT_ERR) {
+			pk_dbg("ERROR interrupt\n");
+			writel(S3C24XX_UDC_INT_ERR,
+			       udc->base + S3C24XX_UDC_SSR_REG);
+		}
+
+		if (sys_stat & S3C24XX_UDC_INT_SDE) {
+
+			writel(S3C24XX_UDC_INT_SDE,
+			       udc->base + S3C24XX_UDC_SSR_REG);
+
+			if (sys_stat & S3C24XX_UDC_INT_HSP) {
+				pk_dbg("HIGH SPEED detection\n");
+				s3c24xx_set_max_pktsize(udc, USB_SPEED_HIGH);
+			} else {
+				pk_dbg("FULL SPEED detection\n");
+				s3c24xx_set_max_pktsize(udc, USB_SPEED_FULL);
+			}
+		}
+
+		if (sys_stat & S3C24XX_UDC_INT_HSP) {
+
+			writel(S3C24XX_UDC_INT_HSP, udc->base + S3C24XX_UDC_SSR_REG);
+			pk_dbg("High Speed interrupt\n");
+		}
+
+		/*
+		 * @HW-BUG: If we get a suspend interrupt BUT are still connected to
+		 * the bus, then the host-port number 2 registers a status change
+		 * and tries to enable the port. This leads to a failure (see [1])
+		 * since we are using the USB-PHY as device, and NOT as host-port.
+		 *
+		 * [1] hub 1-0:1.0: Cannot enable port 2.  Maybe the USB cable is bad?
+		 *
+		 * The only option to avoid this error is disabling the USB-PHY so that
+		 * a RESUME condition is generated and the host will ONLY try to
+		 * enumerate an apparently connected USB-device
+		 */
+		if (sys_stat & S3C24XX_UDC_INT_SUSPEND) {
+
+			pk_dbg("SUSPEND interrupt\n");
+
+			/* First ACK the interrupt after the bug fix! */
+			writel(S3C24XX_UDC_INT_SUSPEND,
+			       udc->base + S3C24XX_UDC_SSR_REG);
+			if (udc->gadget.speed != USB_SPEED_UNKNOWN
+			    && udc->driver
+			    && udc->driver->suspend) {
+				udc->driver->suspend(&udc->gadget);
+			}
+		}
+
+		/*
+		 * By the resume interrupts the following error message is printed:
+		 * hub 1-0:1.0: unable to enumerate USB device on port 2
+		 */
+		if (sys_stat & S3C24XX_UDC_INT_RESUME) {
+			pk_dbg("RESUME interrupt\n");
+			writel(S3C24XX_UDC_INT_RESUME,
+			       udc->base + S3C24XX_UDC_SSR_REG);
+			if (udc->gadget.speed != USB_SPEED_UNKNOWN
+			    && udc->driver
+			    && udc->driver->resume) {
+				udc->driver->resume(&udc->gadget);
+			}
+		}
+
+		if (sys_stat & S3C24XX_UDC_INT_RESET) {
+			pk_dbg("RESET interrupt\n");
+			writel(S3C24XX_UDC_INT_RESET,
+			       udc->base + S3C24XX_UDC_SSR_REG);
+			reconfig_usbd(udc);
+			udc->ep0state = WAIT_FOR_SETUP;
+		}
+
+		if (sys_stat & (S3C24XX_UDC_SSR_TBM | S3C24XX_UDC_SSR_EOERR |
+			    S3C24XX_UDC_SSR_DCERR))
+			pk_err("Unexpected sys failure: 0x%08x\n", sys_stat);
+
+	}
+
+	if (intr_in) {
+		unsigned long cnt, epm;
+
+		/* FIXME: Workaround for preventing FIFO under/overrun */
+		udelay(300);
+
+		if (intr_in & S3C24XX_UDC_INT_EP0) {
+			ulong ep0sr;
+
+			/* First handle the arrived data, and then clear the IRQ */
+			s3c24xx_handle_ep0(udc);
+			writel(S3C24XX_UDC_INT_EP0, udc->base + S3C24XX_UDC_EIR_REG);
+
+			/*
+			 * By long setup-handlings it's possible to have a TST at
+			 * this point.
+			 */
+			ep0sr = readl(udc->base + S3C24XX_UDC_EP0SR_REG);
+			if (ep0sr & S3C24XX_UDC_EP0SR_TST)
+				writel(S3C24XX_UDC_EP0SR_TST,
+				       udc->base + S3C24XX_UDC_EP0SR_REG);
+		}
+
+
+		/* First get the EP-number that generated the interrupt */
+		for (cnt = 1; cnt < S3C_MAX_ENDPOINTS; cnt++) {
+			epm = (1 << cnt);
+			if (intr_in & epm) {
+				writel(epm, udc->base + S3C24XX_UDC_EIR_REG);
+				s3c24xx_udc_in_epn(udc, cnt);
+				/* writel(epm, udc->base + S3C24XX_UDC_EIR_REG); */
+			}
+		}
+	}
+
+	/* Check for OUT-frames by the endpoints */
+	if (intr_out) {
+		unsigned long cnt, epm;
+
+		/* FIXME: Workaround for preventing FIFO under/overrun */
+		udelay(100);
+
+		/* And the EP0 can receive OUT-transfers too! */
+		if (intr_out & 0x1)
+			s3c2443_udc_ep0_read(udc);
+
+		for (cnt = 1; cnt < S3C_MAX_ENDPOINTS; cnt++) {
+			epm = (1 << cnt);
+			if (intr_out & epm) {
+				writel(epm, udc->base + S3C24XX_UDC_EIR_REG);
+				s3c24xx_udc_out_epn(udc, cnt);
+			}
+		}
+	}
+
+ exit_ack:
+	/* writel(stat, udc->base + S3C24XX_UDC_SSR_REG); */
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return IRQ_HANDLED;
+}
+
+/*
+ * Enable one EP, but only if it's different than the EP zero
+ * This function is called from the gadget-drivers over the UDC-operation functions
+ */
+static int s3c24xx_udc_ep_enable(struct usb_ep *_ep,
+				 const struct usb_endpoint_descriptor *desc)
+{
+	struct s3c_ep *ep;
+	struct s3c24xx_udc *udc;
+	unsigned long flags, epnr, regval;
+
+	ep = container_of(_ep, struct s3c_ep, ep);
+	if (!_ep || !desc || ep->desc) {
+		pk_err("NULL pointer or bad EP or descriptor found.\n");
+		return -EINVAL;
+	} else if (_ep->name == ep0name) {
+		pk_err("Invalid EP name (%s) to enable.\n", _ep->name);
+		return -EINVAL;
+	} else if (desc->bDescriptorType != USB_DT_ENDPOINT) {
+		pk_err("Invalid descriptor type (USB_DT_ENDPOINT)\n");
+		return -EINVAL;
+	} else if (ep->bEndpointAddress != desc->bEndpointAddress) {
+		pk_err("Invalid EP address found (valid %x | invalid %x)\n",
+			   ep->bEndpointAddress, desc->bEndpointAddress);
+		return -EINVAL;
+	} else if (ep_maxpacket(ep) < le16_to_cpu(desc->wMaxPacketSize)) {
+		pk_err("Invalid EP size %u (max. %u)\n",
+			   le16_to_cpu(desc->wMaxPacketSize), ep_maxpacket(ep));
+		return -EINVAL;
+	}
+
+	/* xfer types must match, except that interrupt ~= bulk */
+	if (ep->bmAttributes != desc->bmAttributes
+	    && ep->bmAttributes != USB_ENDPOINT_XFER_BULK
+	    && desc->bmAttributes != USB_ENDPOINT_XFER_INT) {
+		pk_err("Type mismatch by EP %s\n", _ep->name);
+		return -EINVAL;
+	}
+
+	/* hardware _could_ do smaller, but driver doesn't */
+	if ((desc->bmAttributes == USB_ENDPOINT_XFER_BULK
+	     && le16_to_cpu(desc->wMaxPacketSize) > ep_maxpacket(ep))
+	    || !desc->wMaxPacketSize) {
+		pk_err("Bad %s maxpacket (desc %u | max %u)\n", _ep->name,
+			   le16_to_cpu(desc->wMaxPacketSize), ep_maxpacket(ep));
+		return -ERANGE;
+	}
+
+	udc = ep->dev;
+	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN) {
+		pk_err("Bogus device state\n");
+		return -ESHUTDOWN;
+	}
+
+	spin_lock_irqsave(&ep->dev->lock, flags);
+
+	ep->stopped = 0;
+	ep->desc = desc;
+	ep->pio_irqs = 0;
+	ep->ep.maxpacket = le16_to_cpu(desc->wMaxPacketSize);
+
+	/* Enable the interrupt for this EP */
+	epnr = ep_index(ep);
+	regval = readl(udc->base + S3C24XX_UDC_EIER_REG);
+	regval |= (1 << epnr);
+	writel(regval, udc->base + S3C24XX_UDC_EIER_REG);
+
+	/* Enable the dual FIFO mode */
+	regval = usb_read(udc, S3C24XX_UDC_ECR_REG, epnr);
+	regval |= S3C24XX_UDC_ECR_DUEN;
+        usb_write(udc, regval, S3C24XX_UDC_ECR_REG, epnr);
+
+	/* Reset halt state */
+	s3c24xx_udc_set_halt(_ep, 0);
+
+	spin_unlock_irqrestore(&ep->dev->lock, flags);
+
+	pk_dbg("Enabled %s | Addr. 0x%02x\n", _ep->name, ep->bEndpointAddress);
+	return 0;
+}
+
+static int s3c24xx_udc_ep_disable(struct usb_ep *_ep)
+{
+	struct s3c_ep *ep;
+	unsigned long flags;
+	int epnr;
+	struct s3c24xx_udc *udc;
+	ulong regval;
+
+	if (!_ep) {
+		pk_err("Null pointer passed! Aborting.\n");
+		return -EINVAL;
+	}
+
+	ep = container_of(_ep, struct s3c_ep, ep);
+	if (!ep->desc) {
+		pk_dbg("%s has an empty descriptor\n", ep->ep.name);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&ep->dev->lock, flags);
+
+	/* Disable the corresponding IRQ */
+	udc = ep->dev;
+	epnr = ep_index(ep);
+	regval = readl(udc->base + S3C24XX_UDC_EIER_REG);
+	regval &= ~(1 << epnr);
+	writel(regval, udc->base + S3C24XX_UDC_EIER_REG);
+
+	/* Nuke all pending requests */
+	nuke(ep, -ESHUTDOWN);
+
+	ep->desc = 0;
+	ep->stopped = 1;
+
+	spin_unlock_irqrestore(&ep->dev->lock, flags);
+	return 0;
+}
+
+static struct usb_request *s3c24xx_udc_alloc_request(struct usb_ep *ep, gfp_t gfp_flags)
+{
+	struct s3c_request *req;
+
+	req = kmalloc(sizeof *req, gfp_flags);
+	if (!req)
+		return NULL;
+
+	memset(req, 0, sizeof *req);
+	INIT_LIST_HEAD(&req->queue);
+
+	return &req->req;
+}
+
+static void s3c24xx_udc_free_request(struct usb_ep *ep, struct usb_request *_req)
+{
+	struct s3c_request *req;
+
+	req = container_of(_req, struct s3c_request, req);
+	WARN_ON(!list_empty(&req->queue));
+	kfree(req);
+}
+
+/*
+ * This function is called by the Gadget-drivers when they have a request for
+ * the UDC (for us).
+ */
+static int s3c24xx_udc_queue(struct usb_ep *_ep, struct usb_request *_req,
+			     gfp_t gfp_flags)
+{
+	struct s3c_request *req;
+	struct s3c_ep *ep;
+	struct s3c24xx_udc *udc;
+	unsigned long flags;
+	int retval;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	req = container_of(_req, struct s3c_request, req);
+	if (unlikely(!_req || !_req->complete || !_req->buf ||
+		     !list_empty(&req->queue))) {
+		pk_err("Bad params for a new EP queue\n");
+		retval = -EINVAL;
+		goto exit_queue_unlock;
+	}
+
+	ep = container_of(_ep, struct s3c_ep, ep);
+	if (unlikely(!_ep || (!ep->desc && ep->ep.name != ep0name))) {
+		pk_err("Bad EP or invalid descriptor\n");
+		retval = -EINVAL;
+		goto exit_queue_unlock;
+	}
+
+	udc = ep->dev;
+	if (unlikely(!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN)) {
+		pk_err("Bogus device state %p\n", udc->driver);
+		retval = -ESHUTDOWN;
+		goto exit_queue_unlock;
+	}
+
+	_req->status = -EINPROGRESS;
+	_req->actual = 0;
+
+	/* Is this possible? */
+	if (!_req->length) {
+		pk_dbg("EP%i: Empty request | Zero %i\n",
+			     ep_index(ep), req->req.zero);
+		done(ep, req, 0);
+		retval = 0;
+		goto exit_queue_unlock;
+	}
+
+	/*
+	 * By the IN-endpoints only add the new request to the
+	 * internal EP-queue and schedule the tasklet. The tasklet
+	 * function will check if the request data can be sent or
+	 * not.
+	 * (Luis Galdos)
+	 */
+	if (ep_is_in(ep) && ep_index(ep) != 0) {
+		/* unsigned long flags; */
+
+		/* spin_lock_irqsave(&ep->lock, flags); */
+#if defined(DEBUG_S3C2443_UDC_QUEUE)
+		{
+			u8 ch1, ch2;
+			int len;
+			u8 *ptr;
+
+			ptr = (u8 *)_req->buf;
+			len = _req->length;
+			ch1 = *ptr;
+			ch2 = *(ptr + len - 1);
+			printk(KERN_DEBUG "%p: len=%i, ep=%02x, 0x%02x ... 0x%02x [N]\n",
+			       req, len, ep_index(ep), ch1, ch2);
+		}
+#endif
+		list_add_tail(&req->queue, &ep->queue);
+		/* spin_unlock_irqrestore(&ep->lock, flags); */
+		tasklet_hi_schedule(&ep->in_tasklet);
+	} else {
+		int handled;
+
+		/*
+		 * If the request couldn't be handled, then tail it into the
+		 * queue of the endpoint
+		 */
+		handled = 0;
+		if (list_empty(&ep->queue) && likely(!ep->stopped)) {
+
+			if (unlikely(ep_index(ep) == 0)) {
+				list_add_tail(&req->queue, &ep->queue);
+				s3c24xx_udc_ep0_kick(udc, ep);
+				handled = 1;
+
+			} else {
+				/*
+				 * The read-function returns zero if the request is not
+				 * done (there is free available buffer space). In this
+				 * case we must add the request to the internal queue.
+				 * (Luis Galdos)
+				 */
+				retval = s3c24xx_udc_read_fifo(ep, req);
+				handled = (retval == 1) ?  (1) : (0);
+
+				/* Error handling */
+				if (retval < 0) {
+					pk_err("EP%i: Read FIFO error\n",
+						   ep_index(ep));
+					goto exit_queue_unlock;
+				}
+			}
+		}
+
+		/* Advances the queue with the non handled request */
+		if (!handled) {
+
+			if (!ep_is_in(ep)) {
+				/* Free the hold configuration bit for receiving further packets */
+				ulong ecr;
+
+				ecr = usb_read(udc, S3C24XX_UDC_ECR_REG, ep_index(ep));
+				ecr &= ~S3C24XX_UDC_ECR_OUTPKTHLD;
+				usb_write(udc, ecr, S3C24XX_UDC_ECR_REG, ep_index(ep));
+
+				pk_dbg_out("EP%i: Queued len %u\n", ep_index(ep), _req->length);
+			}
+
+			list_add_tail(&req->queue, &ep->queue);
+		}
+	}
+
+	retval = 0;
+
+ exit_queue_unlock:
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return retval;
+}
+
+/* Dequeue one USB-request */
+static int s3c24xx_udc_dequeue(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct s3c_ep *ep;
+	struct s3c_request *req;
+	unsigned long flags;
+
+	ep = container_of(_ep, struct s3c_ep, ep);
+	if (!_ep || ep->ep.name == ep0name)
+		return -EINVAL;
+
+	pk_dbg("EP%i: Dequeue called\n", ep_index(ep));
+
+	spin_lock_irqsave(&ep->dev->lock, flags);
+
+	/* Make sure it's actually queued on this endpoint */
+	list_for_each_entry(req, &ep->queue, queue) {
+		if (&req->req == _req)
+			break;
+	}
+	if (&req->req != _req) {
+		spin_unlock_irqrestore(&ep->dev->lock, flags);
+		return -EINVAL;
+	}
+
+	done(ep, req, -ECONNRESET);
+
+	spin_unlock_irqrestore(&ep->dev->lock, flags);
+	return 0;
+}
+
+/*
+ * Halt specific EP. If the value is equal one, then halt the EP, by zero
+ * enable the EP once again
+ */
+static int s3c24xx_udc_set_halt(struct usb_ep *_ep, int halt)
+{
+	struct s3c_ep *ep;
+	unsigned long flags;
+	int retval, epnr;
+	struct s3c24xx_udc *udc;
+
+	ep = container_of(_ep, struct s3c_ep, ep);
+	udc = ep->dev;
+	epnr = ep_index(ep);
+
+	pk_dbg("%s the EP%i\n", halt ? "Halting" : "Enabling", epnr);
+
+	if (!ep->desc) {
+                pk_err("Attempted to halt uninitialized ep %s\n", ep->ep.name);
+                return -ENODEV;
+        }
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+        /*
+	 * Don't halt the EP if it's an IN and not empty
+	 */
+	retval = 0;
+        if (ep_is_in(ep) && !list_empty(&ep->queue)) {
+                retval = -EAGAIN;
+        } else {
+                if (halt)
+			usb_set(udc, S3C24XX_UDC_ECR_ESS, S3C24XX_UDC_ECR_REG, epnr);
+                else
+			usb_clear(udc, S3C24XX_UDC_ECR_ESS, S3C24XX_UDC_ECR_REG, epnr);
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return retval;
+}
+
+/*
+ * Return the available data bytes of the EP-FIFO
+ */
+static int s3c24xx_udc_fifo_status(struct usb_ep *_ep)
+{
+	u32 csr;
+	int count = 0;
+	struct s3c_ep *ep;
+
+	ep = container_of(_ep, struct s3c_ep, ep);
+	if (!_ep) {
+		pk_dbg("%s: bad ep\n", __FUNCTION__);
+		return -ENODEV;
+	}
+
+	/* LPD can't report unclaimed bytes from IN fifos */
+	if (ep_is_in(ep))
+		return -EOPNOTSUPP;
+
+	csr = usb_read(ep->dev, S3C24XX_UDC_EP_STATUS_REG, ep_index(ep));
+	if (ep->dev->gadget.speed != USB_SPEED_UNKNOWN ||
+	    csr & S3C24XX_UDC_EP_RX_SUCCESS) {
+
+		count = usb_read(ep->dev, S3C24XX_UDC_BYTE_READ_CNT_REG, ep_index(ep));
+
+		if (usb_read(ep->dev, S3C24XX_UDC_EP_STATUS_REG, ep_index(ep))
+		    & S3C24XX_UDC_EP_LWO)
+			count = count * 2 -1;
+		else
+			count = count * 2;
+	}
+
+	return count;
+}
+
+/*
+ * Flush the FIFO of the endpoint
+ */
+static void s3c24xx_udc_fifo_flush(struct usb_ep *_ep)
+{
+	struct s3c_ep *ep;
+	struct s3c24xx_udc *udc;
+	int epnr;
+
+	if (!_ep) {
+		pk_err("Can't flush an EP, NULL pointer passed\n");
+		return;
+	}
+
+	ep = container_of(_ep, struct s3c_ep, ep);
+	epnr = ep_index(ep);
+	udc = ep->dev;
+
+	if (unlikely(epnr == 0)) {
+		pk_err("EP0 can't be flushed. Aborting.\n");
+		return;
+	}
+
+	/*
+	 * Flush the EP by using the control register
+	 * IMPORTANT: Dont enable the below code, otherwise is not possible to
+	 * recover the EP from it, we need to restart the UDC for this purpose (we dont
+	 * really want to do it!).
+	 */
+#if 0
+	{
+		ulong ecr;
+
+		ecr = usb_read(udc, S3C24XX_UDC_ECR_REG, epnr);
+		pk_err("EP%i: Flushing now [ecr 0x%08lx]\n", epnr, ecr);
+		usb_write(udc, ecr | S3C24XX_UDC_ECR_FLUSH, S3C24XX_UDC_ECR_REG, epnr);
+	}
+#endif
+}
+
+/*
+ * Function used for reading the data from the FIFO to the passed buffer
+ * This function is only used for the setup-handling
+ */
+static inline int s3c24xx_udc_ep0_setup_read(struct s3c_ep *ep, u16 *cp, int max)
+{
+        int bytes;
+        int count, pending;
+        struct s3c24xx_udc *udc;
+        ulong ep0sr;
+
+        udc = ep->dev;
+
+        ep0sr = readl(udc->base + S3C24XX_UDC_EP0SR_REG);
+        if (!(ep0sr & S3C24XX_UDC_EP0SR_RSR)) {
+                pk_dbg("RSR-bit unset. Aborting setup read.\n");
+                return 0;
+        }
+
+        /* Now get the number of bytes to read from the FIFO */
+        count = usb_read(udc, S3C24XX_UDC_BRCR_REG, ep_index(ep));
+        if (ep0sr & S3C24XX_UDC_EP0SR_LWO)
+                bytes = count * 2 - 1;
+        else
+                bytes = count * 2;
+
+	/*
+	 * If we not enough space, then only process maximal number of bytes
+	 */
+	pending = 0;
+        if (bytes > max) {
+                pk_info("Setup packet length %i exceeds max. %i\n", bytes, max);
+                count = max / 2;
+                bytes = max;
+		pending = 1;
+        }
+
+        while (count--)
+                *cp++ = (u16)readl(udc->base + S3C24XX_UDC_EP0BR_REG);
+
+        /*
+         * IMPORTANT: Dont delete the below line, then otherwise the controller will
+         * not work (but why not?)
+         * (Luis Galdos)
+         */
+	if (!pending)
+		writel(S3C24XX_UDC_EP0_RX_SUCCESS, udc->base + S3C24XX_UDC_EP0SR_REG);
+
+	return bytes;
+}
+
+/*
+ * udc_set_address - set the USB address for this device
+ * @address:
+ *
+ * Called from control endpoint function
+ * after it decodes a set address setup packet.
+ */
+static void s3c24xx_udc_set_address(struct s3c24xx_udc *udc, unsigned char address)
+{
+	udc->usb_address = address;
+}
+
+/* Write data into the FIFO of the EP0 */
+static void s3c24xx_udc_ep0_write(struct s3c24xx_udc *udc)
+{
+	struct s3c_request *req;
+	struct s3c_ep *ep = &udc->ep[0];
+	int ret, completed, need_zlp = 0;
+
+	if (list_empty(&ep->queue))
+		req = 0;
+	else
+		req = list_entry(ep->queue.next, struct s3c_request, queue);
+
+	if (!req) {
+		pk_dbg("EP0: NULL write request?\n");
+		return;
+	}
+
+	if (req->req.length == 0) {
+		udc->ep0state = WAIT_FOR_SETUP;
+	   	done(ep, req, 0);
+		return;
+	}
+
+	if (req->req.length - req->req.actual == ep0_fifo_size) {
+		/* Next write will end with the packet size, */
+		/* so we need Zero-length-packet */
+		need_zlp = 1;
+	}
+
+	/* The write function returns the number of remaining bytes of this request */
+	ret = s3c24xx_udc_write_packet(ep, req);
+	completed = (ret == 0) ? 1 : 0;
+
+	if (completed && !need_zlp) {
+		pk_dbg("EP0: Finished, waiting for status\n");
+		udc->ep0state = WAIT_FOR_SETUP;
+		done(ep, req, 0);
+	} else if (need_zlp) {
+		/* The next TX-interrupt will send the ZLP */
+		udc->ep0state = DATA_STATE_NEED_ZLP;
+	} else
+		/* We need to send more data to the host in the next transfer */
+		pk_dbg("EP0: not finished | %p\n", req);
+}
+
+/* Return zero if NO additional request is available */
+static inline int s3c2443_ep0_fix_set_setup(struct s3c24xx_udc *udc,
+					    struct usb_ctrlrequest *ctrl)
+{
+	int timeout_us, cnt;
+	ulong ep0sr;
+	int retval;
+	struct s3c_ep *ep;
+
+	ep = &udc->ep[0];
+	timeout_us = 1000;
+	do {
+		udelay(1);
+		ep0sr = readl(udc->base + S3C24XX_UDC_EP0SR_REG);
+		timeout_us--;
+	} while (timeout_us && !(ep0sr & S3C24XX_UDC_EP0SR_RSR));
+
+	/* If a timeout happens then returns zero */
+	retval = 0;
+	if (timeout_us) {
+		cnt = s3c24xx_udc_ep0_setup_read(ep, (u16 *)ctrl,
+						 sizeof(struct usb_ctrlrequest));
+		if (cnt > 0)
+			retval = 1;
+	}
+
+	return retval;
+}
+
+/*
+ * Wait for a setup packet and read if from the FIFO before passint it to the
+ * gadget driver
+ */
+#define S3C2443_SETUP_IS_SET_REQ(ctrl) \
+			(ctrl->bRequest == USB_REQ_SET_CONFIGURATION || \
+			ctrl->bRequest == USB_REQ_SET_INTERFACE || \
+			ctrl->bRequest == USB_REQ_SET_DESCRIPTOR || \
+			ctrl->bRequest == USB_REQ_SET_FEATURE || \
+			ctrl->bRequest == USB_REQ_SET_ADDRESS)
+
+#define S3C2443_MAX_NUMBER_SETUPS			(10)
+static void s3c24xx_ep0_setup(struct s3c24xx_udc *udc)
+{
+	struct s3c_ep *ep;
+	int retval, bytes, cnt;
+	struct usb_ctrlrequest *pctrl;
+	struct usb_ctrlrequest ctrls[S3C2443_MAX_NUMBER_SETUPS];
+	int recv_ctrls[S3C2443_MAX_NUMBER_SETUPS];
+
+	memset(recv_ctrls, 0x0, sizeof(recv_ctrls));
+	memset(ctrls, 0x0, sizeof(ctrls));
+
+	/* Nuke all previous transfers of this control EP */
+	ep = &udc->ep[0];
+	nuke(ep, -EPROTO);
+
+	/*
+	 * @FIXME: This is a really bad code, but at this moment there is no better
+	 * proposal for a workaround, since I dont know why the HELL the controller
+	 * stops working by some SETUP-frames which require a status stage.
+	 * Probably the problem is that the controller sends automatically the
+	 * IN-Data of the status stage (with length zero) and it doesn't NAK the
+	 * proceeded IN-request. Since, we have still data from the first IN-frame in
+	 * the FIFO and the second arrived very fast, the FIFO gets stuck.
+	 * Unfortunately we can't reset the FIFO (the control bit for flushing the
+	 * FIFO seems to be only for the EPs different than zero).
+	 *
+	 * For avoiding the described problem probably a delayed-work function can help,
+	 * otherwise the below code can't be removed.
+	 * (Luis Galdos)
+	 */
+
+	/* Read the first SETUP frame as a normal frame */
+	pctrl = &ctrls[0];
+        recv_ctrls[0] = s3c24xx_udc_ep0_setup_read(ep, (u16 *)pctrl, sizeof(* pctrl));
+	if (recv_ctrls[0] <= 0) {
+		pk_dbg("Nothing to process? Aborting.\n");
+		return;
+	}
+
+	/* Here we need to read the other packets */
+	for (cnt = 1; S3C2443_SETUP_IS_SET_REQ(pctrl) && cnt < S3C2443_MAX_NUMBER_SETUPS;
+		     cnt++) {
+		pctrl = &ctrls[cnt];
+		bytes = s3c2443_ep0_fix_set_setup(udc, pctrl);
+
+		if (bytes <= 0)
+			break;
+
+		recv_ctrls[cnt] = bytes;
+	}
+
+	if (cnt == S3C2443_MAX_NUMBER_SETUPS)
+		pk_err("@FIXME: Handling by SETUP overflows\n");
+
+	/* Now start with the processing of the SETUP-frames */
+	retval = 0;
+	for (cnt = 0; recv_ctrls[cnt] > 0 && cnt < S3C2443_MAX_NUMBER_SETUPS; cnt++) {
+
+		pctrl = &ctrls[cnt];
+		bytes = recv_ctrls[cnt];
+
+		/* Set the correct direction for this SETUP-frame */
+		if (likely(pctrl->bRequestType & USB_DIR_IN)) {
+			pk_dbg("EP0: Preparing new IN frame (%i bytes)\n", bytes);
+			ep->bEndpointAddress |= USB_DIR_IN;
+		} else {
+			pk_dbg("EP0: Preparing new OUT frame (%i bytes)\n", bytes);
+			ep->bEndpointAddress &= ~USB_DIR_IN;
+		}
+
+		/* Handle some SETUP packets ourselves */
+		retval = 0;
+		switch (pctrl->bRequest) {
+		case USB_REQ_SET_ADDRESS:
+			if (pctrl->bRequestType != (USB_TYPE_STANDARD |
+						    USB_RECIP_DEVICE))
+				break;
+
+			/*
+			 * If the setup frame was for us, then continue with the
+			 * next available packet
+			 */
+			pk_dbg("Set address request (%d)\n", pctrl->wValue);
+			s3c24xx_udc_set_address(udc, pctrl->wValue);
+			continue;
+
+		default:
+			pk_dbg("bRequestType 0x%02x | bRequest 0x%02x | "
+				     "wValue 0x%04x | wIndex 0x%04x | wLength %u\n",
+				     pctrl->bRequestType, pctrl->bRequest,
+				     pctrl->wValue, pctrl->wIndex, pctrl->wLength);
+			break;
+		}
+
+		/* Check if need to call the Gadget-setup handler */
+		if (likely(udc->driver)) {
+			spin_unlock(&udc->lock);
+			retval = udc->driver->setup(&udc->gadget, pctrl);
+			spin_lock(&udc->lock);
+
+			/* Error values are littler than zero */
+			if (retval < 0)
+				s3c2443_print_err_packet_setup(retval, pctrl);
+		}
+	}
+
+	/* By error-free setups return at this place */
+	if (!retval)
+		return;
+
+	/* @XXX: Test if the STALL is really send to the host */
+	udc->ep0state = WAIT_FOR_SETUP;
+	writel(S3C24XX_UDC_EP0CR_ESS, udc->base + S3C24XX_UDC_EP0CR_REG);
+}
+
+/*
+ * handle ep0 interrupt
+ */
+static void s3c24xx_handle_ep0(struct s3c24xx_udc *udc)
+{
+	struct s3c_ep *ep = &udc->ep[0];
+	u32 csr;
+	unsigned long handled;
+
+	handled = 0;
+ 	csr = readl(udc->base + S3C24XX_UDC_EP0SR_REG);
+
+	/* Clear the STALL bit */
+	if (csr & S3C24XX_UDC_EP0_STALL) {
+		pk_dbg("EP0: Stall success\n");
+		writel(S3C24XX_UDC_EP0_STALL, udc->base + S3C24XX_UDC_EP0SR_REG);
+		nuke(ep, -ECONNABORTED);
+		udc->ep0state = WAIT_FOR_SETUP;
+		handled = 1;
+	}
+
+	/*
+	 * We must check if there is additional data to send. We send at this
+	 * place the ZLP too (DONT try to do it inside the EP0-write function!)
+	 */
+	if (csr & S3C24XX_UDC_EP0_TX_SUCCESS) {
+		struct s3c_ep *ep0 = &udc->ep[0];
+		struct s3c_request *req;
+		int left;
+
+		req = list_entry(ep0->queue.next, struct s3c_request, queue);
+		if (req) {
+			left = req->req.length - req->req.actual;
+			pk_dbg("EP0: TX success | %p: left %i\n", req, left);
+
+			/* Send the pending ZLP of the last request */
+			if (left || udc->ep0state == DATA_STATE_NEED_ZLP)
+				s3c24xx_udc_ep0_write(udc);
+		}
+
+		/*
+		 * Clear the status bit ONLY if we are not going to send a ZLP in the
+		 * next IN-transfer
+		 */
+		if (udc->ep0state != DATA_STATE_NEED_ZLP)
+			writel(S3C24XX_UDC_EP0_TX_SUCCESS,
+			       udc->base + S3C24XX_UDC_EP0SR_REG);
+
+		handled = 1;
+	}
+
+	/* Check if we have received data from the host (SETUP-frames) */
+	if (csr & S3C24XX_UDC_EP0_RX_SUCCESS) {
+		if (udc->ep0state == WAIT_FOR_SETUP) {
+			pk_dbg("EP0: RX success | Wait for setup\n");
+			s3c24xx_ep0_setup(udc);
+		} else if (udc->ep0state == DATA_STATE_RECV) {
+			s3c2443_udc_ep0_read(udc);
+		} else {
+			pk_err("EP0: RX success | Strange state %i\n",
+				   udc->ep0state);
+			udc->ep0state = WAIT_FOR_SETUP;
+			writel(S3C24XX_UDC_EP0SR_RSR, udc->base + S3C24XX_UDC_EP0SR_REG);
+		}
+
+		handled = 1;
+	}
+
+	/*
+	 * Under unknown conditions the EP0 is generating some interrupts which we can't
+	 * identify. Since these IRQs seem to have none side effects, we only print
+	 * a debug message and return inmediately. Please note that the IRQs are
+	 * being generated only by the enumeration of the device, just when the EP0 is
+	 * in use.
+	 * @XXX: Add some additional register outprints (EP0SR, EP0CR, SSR, etc.)
+	 * (Luis Galdos)
+	 */
+	if (!handled) {
+		pk_dbg("[ ERROR ] s3c24xx-udc: Unhandled EP0 IRQ.\n");
+	}
+}
+
+/*
+ * This function is called for the gadget-drivers which uses the EP0 for transferring
+ * data to/from the host. This is the case of the RNDIS driver.
+ * (Luis Galdos)
+ */
+static void s3c24xx_udc_ep0_kick(struct s3c24xx_udc *udc, struct s3c_ep *ep)
+{
+	if (ep_is_in(ep)) {
+		udc->ep0state = DATA_STATE_XMIT;
+		s3c24xx_udc_ep0_write(udc);
+	} else {
+		/* Always set the state of the endpoint first */
+		udc->ep0state = DATA_STATE_RECV;
+		s3c2443_udc_ep0_read(udc);
+	}
+}
+
+static int s3c_udc_get_frame(struct usb_gadget *_gadget)
+{
+	u32 frame;
+	struct s3c24xx_udc *udc;
+
+	udc = the_controller;
+	if (!udc)
+		return -ENODEV;
+
+	frame = readl(udc->base + S3C24XX_UDC_FNR_REG);
+	return (frame & 0x7ff);
+}
+
+static int s3c_udc_wakeup(struct usb_gadget *_gadget)
+{
+	return -ENOTSUPP;
+}
+
+static const struct usb_gadget_ops s3c_udc_ops = {
+	.get_frame = s3c_udc_get_frame,
+	.wakeup = s3c_udc_wakeup,
+	/* current versions must always be self-powered */
+};
+
+static void nop_release(struct device *dev)
+{
+	pk_dbg("%s %s\n", __FUNCTION__, dev->bus_id);
+}
+
+/*
+ * At this moment provide only non-configurable endpoints (address, direction and
+ * type fixed). The syntax definition is coming from the file[1].
+ *
+ * [1] drivers/usb/gadget/autoconf.c:ep_matches()
+ */
+static struct s3c24xx_udc memory = {
+	.usb_address = 0,
+
+	.gadget = {
+		.ops = &s3c_udc_ops,
+		.ep0 = &memory.ep[0].ep,
+		.name = DRIVER_NAME,
+		.dev = {
+			.bus_id = "gadget",
+			.release = nop_release,
+		},
+	},
+
+	.ep[0] = {
+		.ep = {
+			.name = ep0name,
+			.ops = &s3c24xx_ep_ops,
+			.maxpacket = EP0_FIFO_SIZE,
+		},
+		.dev = &memory,
+		.bEndpointAddress = 0,
+		.bmAttributes = 0,
+		.ep_type = ep_control,
+		.fifo = S3C24XX_UDC_EP0BR_REG,
+	},
+
+	.ep[1] = {
+		.ep = {
+			.name = "ep1in-bulk",
+			.ops = &s3c24xx_ep_ops,
+			.maxpacket = EP_FIFO_SIZE2,
+		},
+		.dev = &memory,
+		.bEndpointAddress = USB_DIR_IN | 1,
+		.bmAttributes = USB_ENDPOINT_XFER_BULK,
+		.ep_type = ep_bulk_in,
+		.fifo = S3C24XX_UDC_EP1BR_REG,
+	},
+
+	.ep[2] = {
+		.ep = {
+			.name = "ep2out-bulk",
+			.ops = &s3c24xx_ep_ops,
+			.maxpacket = EP_FIFO_SIZE,
+		},
+		.dev = &memory,
+		.bEndpointAddress = 2,
+		.bmAttributes = USB_ENDPOINT_XFER_BULK,
+		.ep_type = ep_bulk_out,
+		.fifo = S3C24XX_UDC_EP2BR_REG,
+	},
+
+	.ep[3] = {
+		.ep = {
+			.name = "ep3in-int",
+			.ops = &s3c24xx_ep_ops,
+			.maxpacket = EP_FIFO_SIZE,
+		},
+		.dev = &memory,
+		.bEndpointAddress = USB_DIR_IN | 3,
+		.bmAttributes = USB_ENDPOINT_XFER_INT,
+		.ep_type = ep_interrupt,
+		.fifo = S3C24XX_UDC_EP3BR_REG,
+	},
+
+	.ep[4] = {
+		.ep = {
+			.name = "ep4out-int",
+			.ops = &s3c24xx_ep_ops,
+			.maxpacket = EP_FIFO_SIZE,
+		},
+		.dev = &memory,
+		.bEndpointAddress = 4,
+		.bmAttributes = USB_ENDPOINT_XFER_INT,
+		.ep_type = ep_interrupt,
+		.fifo = S3C24XX_UDC_EP4BR_REG,
+	},
+	.ep[5] = {
+		.ep = {
+			.name = "ep5in-int",
+			.ops = &s3c24xx_ep_ops,
+			.maxpacket = EP_FIFO_SIZE,
+		},
+		.dev = &memory,
+		.bEndpointAddress = USB_DIR_IN | 5,
+		.bmAttributes = USB_ENDPOINT_XFER_INT,
+		.ep_type = ep_interrupt,
+		.fifo = S3C24XX_UDC_EP5BR_REG,
+	},
+	.ep[6] = {
+		.ep = {
+			.name = "ep6out-int",
+			.ops = &s3c24xx_ep_ops,
+			.maxpacket = EP_FIFO_SIZE,
+		},
+		.dev = &memory,
+		.bEndpointAddress = 6,
+		.bmAttributes = USB_ENDPOINT_XFER_INT,
+		.ep_type = ep_interrupt,
+		.fifo = S3C24XX_UDC_EP6BR_REG,
+	},
+	.ep[7] = {
+		.ep = {
+			.name = "ep7in-int",
+			.ops = &s3c24xx_ep_ops,
+			.maxpacket = EP_FIFO_SIZE,
+		},
+		.dev = &memory,
+		.bEndpointAddress = USB_DIR_IN | 7,
+		.bmAttributes = USB_ENDPOINT_XFER_INT,
+		.ep_type = ep_interrupt,
+		.fifo = S3C24XX_UDC_EP7BR_REG,
+	},
+	.ep[8] = {
+		.ep = {
+			.name = "ep8out-int",
+			.ops = &s3c24xx_ep_ops,
+			.maxpacket = EP_FIFO_SIZE,
+		},
+		.dev = &memory,
+		.bEndpointAddress = 8,
+		.bmAttributes = USB_ENDPOINT_XFER_INT,
+		.ep_type = ep_interrupt,
+		.fifo = S3C24XX_UDC_EP8BR_REG,
+	},
+};
+
+
+/*
+ * probe - binds to the platform device
+ */
+static int s3c24xx_udc_probe(struct platform_device *pdev)
+{
+	struct s3c24xx_udc *udc = &memory;
+	int retval;
+	struct s3c2410_udc_mach_info *mach_info;
+	int cnt;
+
+	pk_dbg("Probing a new device ID %i\n", pdev->id);
+
+	/* Get the platform data */
+	mach_info = pdev->dev.platform_data;
+	if (!mach_info) {
+		pk_err("No platform data? Aborting.\n");
+		retval = -EINVAL;
+		goto err_exit;
+	}
+
+	udc->mach_info = mach_info;
+	udc->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!udc->mem) {
+		pk_err("Couldn't get the IO memory resource\n");
+		retval = -EINVAL;
+		goto err_exit;
+	}
+
+        udc->mem = request_mem_region(udc->mem->start, IOMEMSIZE(udc->mem), pdev->name);
+        if (!udc->mem) {
+                pk_err("Failed to request IO memory region.\n");
+                retval = -ENOENT;
+                goto err_exit;
+        }
+
+	udc->base = ioremap(udc->mem->start, IOMEMSIZE(udc->mem));
+	if (!udc->base) {
+		pk_err("Couldn't ioremap the IO memory region\n");
+		retval = -EINVAL;
+		goto err_free_mem;
+	}
+
+	/* Init the internal gadget device */
+	device_initialize(&udc->gadget.dev);
+	udc->gadget.dev.parent = &pdev->dev;
+	udc->gadget.dev.dma_mask = pdev->dev.dma_mask;
+
+	/* @XXX: We can use only one device, right? */
+	the_controller = udc;
+	platform_set_drvdata(pdev, udc);
+
+	/* Init the EPs */
+	s3c24xx_udc_reinit(udc);
+
+	/* Disable the platform dependent UDC-hardware */
+	s3c24xx_udc_disable(udc);
+
+	spin_lock_init(&udc->lock);
+	udc->dev = pdev;
+
+	udc->gadget.is_dualspeed = 1;
+	udc->gadget.is_otg = 0;
+	udc->gadget.is_a_peripheral = 0;
+	udc->gadget.b_hnp_enable = 0;
+	udc->gadget.a_hnp_support = 0;
+	udc->gadget.a_alt_hnp_support = 0;
+
+	/* Get the IRQ for the internal handling of the EPs */
+	retval = request_irq(IRQ_USBD, s3c24xx_udc_irq,
+			     IRQF_DISABLED, pdev->name, udc);
+	if (retval) {
+		pk_err("Cannot get irq %i, err %d\n", IRQ_USBD, retval);
+		retval = -EBUSY;
+		goto err_iounmap;
+	}
+
+	/* Activate the driver first when is going to be used */
+	disable_irq(IRQ_USBD);
+	pk_dbg("IRQ %i for the UDC\n", IRQ_USBD);
+
+	if (mach_info && mach_info->vbus_pin > 0) {
+		udc->irq_vbus = s3c2410_gpio_getirq(mach_info->vbus_pin);
+		retval = request_irq(udc->irq_vbus,
+				     s3c24xx_udc_vbus_irq,
+				     IRQF_DISABLED | IRQF_SHARED |
+				     IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+				     pdev->name, udc);
+		if (retval) {
+			pk_err("Can't get vbus IRQ (%i) for IO %i (err %d)\n",
+				   udc->irq_vbus, mach_info->vbus_pin, retval);
+			goto err_free_udc_irq;
+		}
+		pk_dbg("IRQ %i for vbus detection\n", udc->irq_vbus);
+	} else
+		udc->vbus = 1;
+
+	/*
+	 * Init the tasklet for the IN-endpoints at this place, so that we can kill
+	 * the tasklets when the module is going to be removed
+	 * (Luis Galdos)
+	 */
+	for (cnt = 0; cnt < S3C_MAX_ENDPOINTS; cnt++) {
+		struct s3c_ep *ep = &udc->ep[cnt];
+
+		if (ep_is_in(ep)) {
+			tasklet_init(&ep->in_tasklet,
+				     s3c24xx_udc_epin_tasklet_func,
+				     (unsigned long)ep);
+		}
+
+		spin_lock_init(&ep->lock);
+	}
+
+	/*
+	 * Enable the wakeup support only if a interrupt IO for the bus detection
+	 * was passed with the platform data
+	 */
+	if (udc->irq_vbus) {
+		pk_dbg("Enabling the wakeup IRQ %i\n", udc->irq_vbus);
+		device_init_wakeup(&pdev->dev, 1);
+		device_set_wakeup_enable(&pdev->dev, 0);
+	}
+
+	return 0;
+
+ err_free_udc_irq:
+	free_irq(IRQ_USBD, udc);
+
+ err_iounmap:
+	iounmap(udc->base);
+
+ err_free_mem:
+	release_mem_region(udc->mem->start, IOMEMSIZE(udc->mem));
+
+ err_exit:
+	platform_set_drvdata(pdev, NULL);
+
+	/*
+	 * Unset the controller pointer otherwise the function for registering
+	 * a new gadget can crash the system (it uses this pointer)
+	 * (Luis Galdos)
+	 */
+	the_controller = NULL;
+	return retval;
+}
+
+
+static int s3c24xx_udc_remove(struct platform_device *pdev)
+{
+	struct s3c2410_udc_mach_info *imach;
+	struct s3c24xx_udc *udc;
+	int cnt;
+	struct s3c_ep *ep;
+
+	pk_dbg("Removing the UDC driver (ID %i)\n", pdev->id);
+
+	udc = platform_get_drvdata(pdev);
+	imach = pdev->dev.platform_data;
+
+	/* Kill the tasklet of all the IN-endpoints */
+	for (cnt = 0; cnt < S3C_MAX_ENDPOINTS; cnt++) {
+		ep = &udc->ep[cnt];
+		if (ep_is_in(ep))
+			tasklet_kill(&ep->in_tasklet);
+	}
+
+	s3c24xx_udc_disable(udc);
+	usb_gadget_unregister_driver(udc->driver);
+
+	free_irq(IRQ_USBD, udc);
+
+	if (udc->irq_vbus) {
+		pk_dbg("Disabling the wakeup IRQ %i\n", udc->irq_vbus);
+		device_init_wakeup(&pdev->dev, 0);
+	}
+
+	/*
+	 * If an IRQ for the vbus was passed, then disable it too
+	 * (Luis Galdos)
+	 */
+	if (imach && udc->irq_vbus) {
+		pk_dbg("Freeing the vbus IRQ %i\n", udc->irq_vbus);
+		free_irq(udc->irq_vbus, udc);
+		udc->irq_vbus = 0;
+	}
+
+	release_mem_region(udc->mem->start, IOMEMSIZE(udc->mem));
+
+	platform_set_drvdata(pdev, NULL);
+
+	the_controller = NULL;
+
+	return 0;
+}
+
+/*
+ * From another UDC-drivers seems to be, that is required to disconnect the
+ * USB-device if it's connected to a host.
+ */
+#ifdef CONFIG_PM
+static int s3c2443_udc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct s3c24xx_udc *udc;
+	int retval;
+
+	udc = platform_get_drvdata(pdev);
+
+	/* Need to disconnect first */
+	if (udc->vbus)
+		s3c24xx_udc_disable(udc);
+
+	/* Enable the wakeup if requested */
+	retval = 0;
+	if (device_may_wakeup(&pdev->dev)) {
+
+		/* Paranoic sanity check! */
+		if (!udc->irq_vbus) {
+			pk_err("No wakeup IRQ defined?\n");
+			retval = -EINVAL;
+			goto exit_suspend;
+		}
+
+		retval = enable_irq_wake(udc->irq_vbus);
+		if (retval)
+			goto exit_suspend;
+	}
+
+exit_suspend:
+	return retval;
+}
+
+static int s3c2443_udc_resume(struct platform_device *pdev)
+{
+	struct s3c24xx_udc *udc;
+	int retval;
+
+	udc = platform_get_drvdata(pdev);
+
+	retval = 0;
+	if (device_may_wakeup(&pdev->dev)) {
+
+		/* Paranoic sanity check */
+		if (!udc->irq_vbus) {
+			pk_err("No wakeup IRQ defined?\n");
+			retval = -EINVAL;
+			goto exit_resume;
+		}
+
+		disable_irq_wake(udc->irq_vbus);
+	}
+
+	/*
+	 * Check the current state of the VBUS, then probably a host was connected
+	 * during the suspend-time and this will not generate an interrupt
+	 */
+	udc->vbus = s3c2443_udc_vbus_state(udc);
+
+	/* Enable the UDC for starting the enumeration */
+	if (udc->vbus && udc->driver)
+		retval = s3c24xx_udc_enable(udc);
+
+exit_resume:
+	return retval;
+}
+#else
+#define s3c2443_udc_suspend				NULL
+#define s3c2443_udc_resume				NULL
+#endif
+
+static struct platform_driver s3c24xx_udc_driver = {
+	.probe		= s3c24xx_udc_probe,
+	.remove		= s3c24xx_udc_remove,
+	.suspend        = s3c2443_udc_suspend,
+	.resume         = s3c2443_udc_resume,
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= DRIVER_NAME,
+	},
+};
+
+static int __init udc_init(void)
+{
+	int ret;
+
+	pk_info("Loading (%s - %s)\n", DRIVER_BUILD_DATE, DRIVER_BUILD_TIME);
+	ret = platform_driver_register(&s3c24xx_udc_driver);
+	return ret;
+}
+
+static void __exit udc_exit(void)
+{
+	pk_info("Unloading (%s - %s)\n", DRIVER_BUILD_DATE, DRIVER_BUILD_TIME);
+	platform_driver_unregister(&s3c24xx_udc_driver);
+}
+
+module_init(udc_init);
+module_exit(udc_exit);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("Luis Galdos, luis.galdos[at]digi.com");
+MODULE_AUTHOR("Samsung Electronics");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/gadget/s3c2443_udc.h b/drivers/usb/gadget/s3c2443_udc.h
new file mode 100644
index 000000000000..f22985128cff
--- /dev/null
+++ b/drivers/usb/gadget/s3c2443_udc.h
@@ -0,0 +1,159 @@
+/*
+ * drivers/usb/gadget/s3c-udc.h
+ * Samsung S3C on-chip full/high speed USB device controllers
+ * $Id: s3c-udc.h,v 1.6 2006/11/30 22:55:18 dasan Exp $*
+ * Copyright (C) 2005 for Samsung Electronics 
+ * 		- by Jaswinder Singh Brar <jassi@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+ 
+#ifndef __S3C_USB_GADGET
+#define __S3C_USB_GADGET
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/ioport.h>
+#include <linux/types.h>
+#include <linux/version.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/timer.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/proc_fs.h>
+#include <linux/mm.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/byteorder.h>
+#include <asm/dma.h>
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+#include <asm/unaligned.h>
+#include <mach/hardware.h>
+
+#include <linux/usb.h>
+#include <linux/usb/gadget.h>
+
+
+#include <asm/plat-s3c24xx/regs-udc.h>
+#include <asm/plat-s3c24xx/udc.h>
+#include <linux/io.h>
+#include <mach/regs-s3c2443-clock.h>
+#include <mach/regs-gpio.h>
+#include <mach/regs-irq.h>
+
+
+// Max packet size
+#ifdef CONFIG_USB_GADGET_S3C_FS
+#define EP0_FIFO_SIZE		8
+#define EP_FIFO_SIZE		64
+#define S3C_MAX_ENDPOINTS	5
+#else
+#define EP0_FIFO_SIZE		64
+#define EP_FIFO_SIZE		512
+#define EP_FIFO_SIZE2		1024
+#define S3C_MAX_ENDPOINTS	9
+#endif
+
+#define WAIT_FOR_SETUP          0
+#define DATA_STATE_XMIT         1
+#define DATA_STATE_NEED_ZLP     2
+#define WAIT_FOR_OUT_STATUS     3
+#define DATA_STATE_RECV         4
+
+
+
+
+
+typedef enum ep_type {
+	ep_control,
+	ep_bulk_in,
+	ep_bulk_out,
+	ep_interrupt
+} ep_type_t;
+
+
+
+struct s3c_ep {
+	struct usb_ep ep;
+	struct s3c24xx_udc *dev;
+
+	const struct usb_endpoint_descriptor *desc;
+	struct list_head queue;
+	unsigned long pio_irqs;
+
+	u8 stopped;
+	u8 bEndpointAddress;
+	u8 bmAttributes;
+
+	ep_type_t ep_type;
+	u32 fifo;
+#ifdef CONFIG_USB_GADGET_S3C_FS
+	u32 csr1;
+	u32 csr2;
+#endif
+
+	/* Tasklet for the data handling of the IN-endpoints (Luis Galdos) */
+	struct tasklet_struct in_tasklet;
+	spinlock_t lock;
+};
+
+
+
+struct s3c_request {
+	struct usb_request req;
+	struct list_head queue;
+};
+
+
+
+struct s3c24xx_udc {
+	struct usb_gadget gadget;
+	struct usb_gadget_driver *driver;
+	struct platform_device *dev;
+	spinlock_t lock;
+
+	int ep0state;
+	struct s3c_ep ep[S3C_MAX_ENDPOINTS];
+
+	unsigned char usb_address;
+
+	unsigned req_pending:1, req_std:1, req_config:1;
+
+	
+	struct s3c2410_udc_mach_info *mach_info;
+	struct resource *mem;
+	void __iomem *base;
+	u8 vbus;
+	
+	/* Interrupts for bus detection and UDC internal */
+	int irq_vbus;
+	int irq_udc;
+};
+
+extern struct s3c24xx_udc *the_controller;
+
+#define ep_is_in(ep) 		(((ep)->bEndpointAddress&USB_DIR_IN) == USB_DIR_IN)
+#define ep_index(ep) 		((ep)->bEndpointAddress&0xF)
+#define ep_maxpacket(ep) 	((ep)->ep.maxpacket)
+
+#endif
diff --git a/drivers/usb/gadget/serial.c b/drivers/usb/gadget/serial.c
index 37879af1c433..e7bd43d15bed 100644
--- a/drivers/usb/gadget/serial.c
+++ b/drivers/usb/gadget/serial.c
@@ -273,7 +273,7 @@ static int __init init(void)
 
 	return usb_composite_register(&gserial_driver);
 }
-module_init(init);
+late_initcall(init);
 
 static void __exit cleanup(void)
 {
diff --git a/drivers/usb/gadget/stmp_updater.c b/drivers/usb/gadget/stmp_updater.c
new file mode 100644
index 000000000000..6642bb5bed76
--- /dev/null
+++ b/drivers/usb/gadget/stmp_updater.c
@@ -0,0 +1,473 @@
+/*
+ * Freescale STMP378X UUT driver
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2009 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+static int utp_init(struct fsg_dev *fsg)
+{
+	init_waitqueue_head(&utp_context.wq);
+	INIT_LIST_HEAD(&utp_context.read);
+	INIT_LIST_HEAD(&utp_context.write);
+	mutex_init(&utp_context.lock);
+	utp_context.buffer = vmalloc(0x10000);
+	if (!utp_context.buffer)
+		return -EIO;
+	utp_context.utp_version = 0x1ull;
+	fsg->utp = &utp_context;
+	return misc_register(&utp_dev);
+}
+
+static void utp_exit(struct fsg_dev *fsg)
+{
+	vfree(utp_context.buffer);
+	misc_deregister(&utp_dev);
+}
+
+static struct utp_user_data *utp_user_data_alloc(size_t size)
+{
+	struct utp_user_data *uud;
+
+	uud = kzalloc(size + sizeof(*uud), GFP_KERNEL);
+	if (!uud)
+		return uud;
+	uud->data.size = size + sizeof(uud->data);
+	INIT_LIST_HEAD(&uud->link);
+	return uud;
+}
+
+static void utp_user_data_free(struct utp_user_data *uud)
+{
+	list_del(&uud->link);
+	kfree(uud);
+}
+
+#define WAIT_ACTIVITY(queue) \
+ wait_event_interruptible(utp_context.wq, !list_empty(&utp_context.queue))
+
+static ssize_t utp_file_read(struct file *file,
+			     char __user *buf,
+			     size_t size,
+			     loff_t *off)
+{
+	struct utp_user_data *uud;
+	size_t size_to_put;
+	int free = 0;
+
+	WAIT_ACTIVITY(read);
+
+	mutex_lock(&utp_context.lock);
+	uud = list_first_entry(&utp_context.read, struct utp_user_data, link);
+	mutex_unlock(&utp_context.lock);
+	size_to_put = uud->data.size;
+	if (size >= size_to_put)
+		free = !0;
+	if (copy_to_user(buf, &uud->data, size_to_put))
+		return -EACCES;
+	if (free)
+		utp_user_data_free(uud);
+	else {
+		pr_info("sizeof = %d, size = %d\n",
+			sizeof(uud->data),
+			uud->data.size);
+
+		pr_err("Will not free utp_user_data, because buffer size = %d,"
+			"need to put %d\n", size, size_to_put);
+	}
+
+	return size_to_put;
+}
+
+
+static ssize_t utp_file_write(struct file *file, const char __user *buf,
+				size_t size, loff_t *off)
+{
+	struct utp_user_data *uud;
+
+	if (size < sizeof(uud->data))
+		return -EINVAL;
+	uud = utp_user_data_alloc(size);
+	if (copy_from_user(&uud->data, buf, size))
+		return -EACCES;
+	mutex_lock(&utp_context.lock);
+	list_add_tail(&uud->link, &utp_context.write);
+	wake_up(&utp_context.wq);
+	mutex_unlock(&utp_context.lock);
+	return size;
+}
+
+static int utp_get_sense(struct fsg_dev *fsg)
+{
+	if (UTP_CTX(fsg)->processed == 0)
+		return -1;
+
+	UTP_CTX(fsg)->processed = 0;
+	return 0;
+}
+
+static int utp_do_read(struct fsg_dev *fsg, void *data, size_t size)
+{
+	struct fsg_buffhd	*bh;
+	int			rc;
+	u32			amount_left;
+	unsigned int		amount;
+
+	/* Get the starting Logical Block Address and check that it's
+	 * not too big */
+
+	amount_left = size;
+	if (unlikely(amount_left == 0))
+		return -EIO;		/* No default reply*/
+
+	pr_debug("%s: sending %d\n", __func__, size);
+	for (;;) {
+		/* Figure out how much we need to read:
+		 * Try to read the remaining amount.
+		 * But don't read more than the buffer size.
+		 * And don't try to read past the end of the file.
+		 * Finally, if we're not at a page boundary, don't read past
+		 *	the next page.
+		 * If this means reading 0 then we were asked to read past
+		 *	the end of file. */
+		amount = min((unsigned int) amount_left, mod_data.buflen);
+
+		/* Wait for the next buffer to become available */
+		bh = fsg->next_buffhd_to_fill;
+		while (bh->state != BUF_STATE_EMPTY) {
+			rc = sleep_thread(fsg);
+			if (rc)
+				return rc;
+		}
+
+		/* If we were asked to read past the end of file,
+		 * end with an empty buffer. */
+		if (amount == 0) {
+			bh->inreq->length = 0;
+			bh->state = BUF_STATE_FULL;
+			break;
+		}
+
+		/* Perform the read */
+		pr_info("Copied to %p, %d bytes started from %d\n",
+				bh->buf, amount, size - amount_left);
+		memcpy(bh->buf, data + size - amount_left, amount);
+		amount_left  -= amount;
+		fsg->residue -= amount;
+
+		bh->inreq->length = amount;
+		bh->state = BUF_STATE_FULL;
+
+		/* Send this buffer and go read some more */
+		bh->inreq->zero = 0;
+
+		start_transfer(fsg, fsg->bulk_in, bh->inreq,
+				&bh->inreq_busy, &bh->state);
+
+		fsg->next_buffhd_to_fill = bh->next;
+
+		if (amount_left <= 0)
+			break;
+	}
+
+	return size - amount_left;
+}
+
+static int utp_do_write(struct fsg_dev *fsg, void *data, size_t size)
+{
+	struct fsg_buffhd	*bh;
+	int			get_some_more;
+	u32			amount_left_to_req, amount_left_to_write;
+	unsigned int		amount;
+	int			rc;
+	loff_t			offset;
+
+	/* Carry out the file writes */
+	get_some_more = 1;
+	amount_left_to_req = amount_left_to_write = size;
+
+	if (unlikely(amount_left_to_write == 0))
+		return -EIO;
+
+	offset = 0;
+	while (amount_left_to_write > 0) {
+
+		/* Queue a request for more data from the host */
+		bh = fsg->next_buffhd_to_fill;
+		if (bh->state == BUF_STATE_EMPTY && get_some_more) {
+
+			/* Figure out how much we want to get:
+			 * Try to get the remaining amount.
+			 * But don't get more than the buffer size.
+			 * And don't try to go past the end of the file.
+			 * If we're not at a page boundary,
+			 *	don't go past the next page.
+			 * If this means getting 0, then we were asked
+			 *	to write past the end of file.
+			 * Finally, round down to a block boundary. */
+			amount = min(amount_left_to_req, mod_data.buflen);
+
+			if (amount == 0) {
+				get_some_more = 0;
+				/* cry now */
+				continue;
+			}
+
+			/* Get the next buffer */
+			amount_left_to_req -= amount;
+			if (amount_left_to_req == 0)
+				get_some_more = 0;
+
+			/* amount is always divisible by 512, hence by
+			 * the bulk-out maxpacket size */
+			bh->outreq->length = bh->bulk_out_intended_length =
+					amount;
+			bh->outreq->short_not_ok = 1;
+			start_transfer(fsg, fsg->bulk_out, bh->outreq,
+					&bh->outreq_busy, &bh->state);
+			fsg->next_buffhd_to_fill = bh->next;
+			continue;
+		}
+
+		/* Write the received data to the backing file */
+		bh = fsg->next_buffhd_to_drain;
+		if (bh->state == BUF_STATE_EMPTY && !get_some_more)
+			break;			/* We stopped early */
+		if (bh->state == BUF_STATE_FULL) {
+			smp_rmb();
+			fsg->next_buffhd_to_drain = bh->next;
+			bh->state = BUF_STATE_EMPTY;
+
+			/* Did something go wrong with the transfer? */
+			if (bh->outreq->status != 0)
+				/* cry again, COMMUNICATION_FAILURE */
+				break;
+
+			amount = bh->outreq->actual;
+
+			/* Perform the write */
+			memcpy(data + offset, bh->buf, amount);
+
+			offset += amount;
+			if (signal_pending(current))
+				return -EINTR;		/* Interrupted!*/
+			amount_left_to_write -= amount;
+			fsg->residue -= amount;
+
+			/* Did the host decide to stop early? */
+			if (bh->outreq->actual != bh->outreq->length) {
+				fsg->short_packet_received = 1;
+				break;
+			}
+			continue;
+		}
+
+		/* Wait for something to happen */
+		rc = sleep_thread(fsg);
+		if (rc)
+			return rc;
+	}
+
+	return -EIO;
+}
+
+static inline void utp_set_sense(struct fsg_dev *fsg, u16 code, u64 reply)
+{
+	UTP_CTX(fsg)->processed = true;
+	UTP_CTX(fsg)->sdinfo = reply & 0xFFFFFFFF;
+	UTP_CTX(fsg)->sdinfo_h = (reply >> 32) & 0xFFFFFFFF;
+	UTP_CTX(fsg)->sd = (UTP_SENSE_KEY << 16) | code;
+}
+
+static void utp_poll(struct fsg_dev *fsg)
+{
+	struct utp_context *ctx = UTP_CTX(fsg);
+	struct utp_user_data *uud = NULL;
+
+	mutex_lock(&ctx->lock);
+	if (!list_empty(&ctx->write))
+		uud = list_first_entry(&ctx->write, struct utp_user_data, link);
+	mutex_unlock(&ctx->lock);
+
+	if (uud) {
+		if (uud->data.flags & UTP_FLAG_STATUS) {
+			pr_debug("%s: exit with status %d\n", __func__,
+				uud->data.status);
+			UTP_SS_EXIT(fsg, uud->data.status);
+		} else {
+			pr_debug("%s: pass\n", __func__);
+			UTP_SS_PASS(fsg);
+		}
+		utp_user_data_free(uud);
+	} else {
+		pr_debug("%s: still busy...\n", __func__);
+		UTP_SS_BUSY(fsg, --ctx->counter);
+	}
+}
+
+static int utp_exec(struct fsg_dev *fsg,
+		    char *command,
+		    int cmdsize,
+		    unsigned long long payload)
+{
+	struct utp_user_data *uud = NULL, *uud2r;
+	struct utp_context *ctx = UTP_CTX(fsg);
+
+	uud2r = utp_user_data_alloc(cmdsize + 1);
+	uud2r->data.flags = UTP_FLAG_COMMAND;
+	uud2r->data.payload = payload;
+	strncpy(uud2r->data.command, command, cmdsize);
+
+	mutex_lock(&ctx->lock);
+	list_add_tail(&uud2r->link, &ctx->read);
+	mutex_unlock(&ctx->lock);
+	wake_up(&ctx->wq);
+
+	if (command[0] == '!')	/* there will be no response */
+		return 0;
+
+	WAIT_ACTIVITY(write);
+
+	mutex_lock(&ctx->lock);
+	if (!list_empty(&ctx->write)) {
+		uud = list_first_entry(&ctx->write, struct utp_user_data, link);
+#ifdef DEBUG
+		pr_info("UUD:\n\tFlags = %02X\n", uud->data.flags);
+		if (uud->data.flags & UTP_FLAG_DATA) {
+			pr_info("\tbufsize = %d\n", uud->data.bufsize);
+			print_hex_dump(KERN_DEBUG, "\t", DUMP_PREFIX_NONE,
+				16, 2, uud->data.data, uud->data.bufsize, true);
+		}
+		if (uud->data.flags & UTP_FLAG_REPORT_BUSY)
+			pr_info("\tBUSY\n");
+#endif
+	}
+	mutex_unlock(&ctx->lock);
+
+	if (uud->data.flags & UTP_FLAG_DATA) {
+		memcpy(ctx->buffer, uud->data.data, uud->data.bufsize);
+		UTP_SS_SIZE(fsg, uud->data.bufsize);
+		utp_user_data_free(uud);
+		return 0;
+	}
+
+	if (uud->data.flags & UTP_FLAG_REPORT_BUSY) {
+		utp_user_data_free(uud);
+		ctx->counter = 0xFFFF;
+		UTP_SS_BUSY(fsg, ctx->counter);
+		return 0;
+	}
+
+	utp_user_data_free(uud);
+	UTP_SS_PASS(fsg);
+
+	return -1;
+}
+
+static int utp_send_status(struct fsg_dev *fsg)
+{
+	struct fsg_buffhd	*bh;
+	u8			status = USB_STATUS_PASS;
+	struct bulk_cs_wrap	*csw;
+	int 			rc;
+
+	/* Wait for the next buffer to become available */
+	bh = fsg->next_buffhd_to_fill;
+	while (bh->state != BUF_STATE_EMPTY) {
+		rc = sleep_thread(fsg);
+		if (rc)
+			return rc;
+	}
+
+	if (fsg->phase_error) {
+		DBG(fsg, "sending phase-error status\n");
+		status = USB_STATUS_PHASE_ERROR;
+
+	} else if ((UTP_CTX(fsg)->sd & 0xFFFF) != UTP_REPLY_PASS) {
+		status = USB_STATUS_FAIL;
+	}
+
+	csw = bh->buf;
+
+	/* Store and send the Bulk-only CSW */
+	csw->Signature = __constant_cpu_to_le32(USB_BULK_CS_SIG);
+	csw->Tag = fsg->tag;
+	csw->Residue = cpu_to_le32(fsg->residue);
+	csw->Status = status;
+
+	bh->inreq->length = USB_BULK_CS_WRAP_LEN;
+	bh->inreq->zero = 0;
+	start_transfer(fsg, fsg->bulk_in, bh->inreq,
+			&bh->inreq_busy, &bh->state);
+	fsg->next_buffhd_to_fill = bh->next;
+	return 0;
+}
+
+static int utp_handle_message(struct fsg_dev *fsg,
+			      char *cdb_data,
+			      int default_reply)
+{
+	struct utp_msg *m = (struct utp_msg *)cdb_data;
+	void *data = NULL;
+	int r;
+	struct utp_user_data *uud2r;
+	unsigned long long param;
+	unsigned long tag;
+
+	if (m->f0 != 0xF0)
+		return default_reply;
+
+	tag = get_be32((void *)&m->utp_msg_tag);
+	param = get_be64((void *)&m->param);
+	pr_debug("Type 0x%x, tag 0x%08lx, param %llx\n",
+			m->utp_msg_type, tag, param);
+
+	switch ((enum utp_msg_type)m->utp_msg_type) {
+
+	case UTP_POLL:
+		if (get_be64((void *)&m->param) == 1) {
+			pr_debug("%s: version request\n", __func__);
+			UTP_SS_EXIT(fsg, UTP_CTX(fsg)->utp_version);
+			break;
+		}
+		utp_poll(fsg);
+		break;
+	case UTP_EXEC:
+		pr_debug("%s: EXEC\n", __func__);
+		data = kzalloc(fsg->data_size, GFP_KERNEL);
+		utp_do_write(fsg, data, fsg->data_size);
+		utp_exec(fsg, data, fsg->data_size, param);
+		kfree(data);
+		break;
+	case UTP_GET:
+		pr_debug("%s: GET, %d bytes\n", __func__, fsg->data_size);
+		r = utp_do_read(fsg, UTP_CTX(fsg)->buffer, fsg->data_size);
+		UTP_SS_PASS(fsg);
+		break;
+	case UTP_PUT:
+		pr_debug("%s: PUT, %d bytes\n", __func__, fsg->data_size);
+		uud2r = utp_user_data_alloc(fsg->data_size);
+		uud2r->data.bufsize = fsg->data_size;
+		uud2r->data.flags = UTP_FLAG_DATA;
+		utp_do_write(fsg, uud2r->data.data, fsg->data_size);
+		/* don't know what will be written */
+		mutex_lock(&UTP_CTX(fsg)->lock);
+		list_add_tail(&uud2r->link, &UTP_CTX(fsg)->read);
+		mutex_unlock(&UTP_CTX(fsg)->lock);
+		wake_up(&UTP_CTX(fsg)->wq);
+		UTP_SS_PASS(fsg);
+		break;
+	}
+
+	utp_send_status(fsg);
+	return -1;
+}
+
diff --git a/drivers/usb/gadget/stmp_updater.h b/drivers/usb/gadget/stmp_updater.h
new file mode 100644
index 000000000000..e1248d1093c8
--- /dev/null
+++ b/drivers/usb/gadget/stmp_updater.h
@@ -0,0 +1,139 @@
+/*
+ * Freescale STMP378X UUT driver
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2009 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef __STMP_UPDATER_H
+#define __STMP_UPDATER_H
+
+#include <linux/miscdevice.h>
+#include <linux/list.h>
+#include <linux/vmalloc.h>
+
+static int utp_init(struct fsg_dev *fsg);
+static void utp_exit(struct fsg_dev *fsg);
+static ssize_t utp_file_read(struct file *file,
+			     char __user *buf,
+			     size_t size,
+			     loff_t *off);
+
+static ssize_t utp_file_write(struct file *file,
+			      const char __user *buf,
+			      size_t size,
+			      loff_t *off);
+
+static struct utp_user_data *utp_user_data_alloc(size_t size);
+static void utp_user_data_free(struct utp_user_data *uud);
+static int utp_get_sense(struct fsg_dev *fsg);
+static int utp_do_read(struct fsg_dev *fsg, void *data, size_t size);
+static int utp_do_write(struct fsg_dev *fsg, void *data, size_t size);
+static inline void utp_set_sense(struct fsg_dev *fsg, u16 code, u64 reply);
+static int utp_handle_message(struct fsg_dev *fsg,
+			      char *cdb_data,
+			      int default_reply);
+
+#define UTP_REPLY_PASS		0
+#define UTP_REPLY_EXIT		0x8001
+#define UTP_REPLY_BUSY		0x8002
+#define UTP_REPLY_SIZE		0x8003
+#define UTP_SENSE_KEY		9
+
+#define UTP_MINOR		222
+/* MISC_DYNAMIC_MINOR would be better, but... */
+
+#define UTP_COMMAND_SIZE	80
+
+#define UTP_SS_EXIT(fsg, r) 	utp_set_sense(fsg, UTP_REPLY_EXIT, (u64)r)
+#define UTP_SS_PASS(fsg)	utp_set_sense(fsg, UTP_REPLY_PASS, 0)
+#define UTP_SS_BUSY(fsg, r)	utp_set_sense(fsg, UTP_REPLY_BUSY, (u64)r)
+#define UTP_SS_SIZE(fsg, r)	utp_set_sense(fsg, UTP_REPLY_SIZE, (u64)r)
+
+#pragma pack(1)
+struct utp_msg {
+	u8  f0;
+	u8  utp_msg_type;
+	u32 utp_msg_tag;
+	union {
+		struct {
+			u32 param_lsb;
+			u32 param_msb;
+		};
+		u64 param;
+	};
+};
+
+enum utp_msg_type {
+	UTP_POLL = 0,
+	UTP_EXEC,
+	UTP_GET,
+	UTP_PUT,
+};
+
+static struct utp_context {
+	wait_queue_head_t wq;
+	struct mutex lock;
+	struct list_head read;
+	struct list_head write;
+	u32 sd, sdinfo, sdinfo_h;			/* sense data */
+	int processed;
+	u8 *buffer;
+	u32 counter;
+	u64 utp_version;
+} utp_context;
+
+static const struct file_operations utp_fops = {
+	.open	= nonseekable_open,
+	.read	= utp_file_read,
+	.write	= utp_file_write,
+};
+
+static struct miscdevice utp_dev = {
+	.minor 	= UTP_MINOR,
+	.name 	= "utp",
+	.fops	= &utp_fops,
+};
+
+#define UTP_FLAG_COMMAND	0x00000001
+#define UTP_FLAG_DATA		0x00000002
+#define UTP_FLAG_STATUS		0x00000004
+#define UTP_FLAG_REPORT_BUSY	0x10000000
+struct utp_message {
+	u32 	flags;
+	size_t 	size;
+	union {
+		struct {
+			u64 payload;
+			char command[1];
+		};
+		struct {
+			size_t bufsize;
+			u8 data[1];
+		};
+		u32 status;
+	};
+};
+
+struct utp_user_data {
+	struct  list_head 	link;
+	struct  utp_message	data;
+};
+#pragma pack()
+
+static inline struct utp_context *UTP_CTX(struct fsg_dev *fsg)
+{
+	return (struct utp_context *)fsg->utp;
+}
+
+#endif /* __STMP_UPDATER_H */
+
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index f3a75a929e0a..4e539e30f1f6 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -42,9 +42,100 @@ config USB_EHCI_HCD
 	  To compile this driver as a module, choose M here: the
 	  module will be called ehci-hcd.
 
+config USB_EHCI_ARC
+	bool "Support for Freescale controller"
+	depends on USB_EHCI_HCD && (ARCH_MXC || ARCH_STMP3XXX)
+	---help---
+	   Some Freescale processors have an integrated High Speed
+	   USBOTG controller, which supports EHCI host mode.
+
+	   Say "y" here to add support for this controller
+	   to the EHCI HCD driver.
+
+config USB_EHCI_ARC_H1
+	bool "Support for Host1 port on Freescale controller"
+	depends on USB_EHCI_ARC	&& (ARCH_MX27 || ARCH_MX3 || ARCH_MX51)
+	---help---
+	  Enable support for the USB Host1 port.
+
+config USB_EHCI_ARC_H2
+	bool "Support for Host2 port on Freescale controller"
+	depends on USB_EHCI_ARC	&& \
+		   (ARCH_MX25 || ARCH_MX27 || ARCH_MX3 || ARCH_MX35 || ARCH_MX51)
+	---help---
+	  Enable support for the USB Host2 port.
+
+config USB_EHCI_ARC_OTG
+	bool "Support for DR host port on Freescale controller"
+	depends on USB_EHCI_ARC
+	default y
+	---help---
+	  Enable support for the USB OTG port in HS/FS Host mode.
+
+config USB_STATIC_IRAM
+	bool "Use IRAM for USB"
+	depends on USB_EHCI_ARC
+	---help---
+	  Enable this option to use IRAM instead of DRAM for USB
+	  structures and buffers.  This option will reduce bus
+	  contention on systems with large (VGA+) framebuffer
+	  devices and heavy USB activity.  There are performance
+	  penalties and usage restrictions when using this option.
+
+	  If in doubt, say N.
+
+choice
+	prompt "Select transceiver for DR port"
+	depends on USB_EHCI_ARC_OTG
+	default USB_EHCI_FSL_1504 if ARCH_MX3
+	default USB_EHCI_FSL_1301 if ARCH_MX27
+	default USB_EHCI_FSL_UTMI if (ARCH_MX25 || ARCH_MX35 || ARCH_MX37 || ARCH_MX51 || ARCH_STMP3XXX)
+	---help---
+	  Choose the transceiver to use with the Freescale DR port.
+
+config USB_EHCI_FSL_MC13783
+	bool "Freescale MC13783"
+	depends on !MACH_MX25_3DS
+	---help---
+	  Enable support for the Full Speed Freescale MC13783 transceiver.
+
+	  The mx27ads, mx31ads and mx32ads boards require modifications
+	  to support this transceiver.
+
+config USB_EHCI_FSL_1301
+	bool "Philips ISP1301"
+	depends on !MACH_MX25_3DS
+	---help---
+	  Enable support for the Full Speed Philips ISP1301 transceiver.
+
+	  This is the factory default for the mx27ads board.
+	  The mx31ads and mx32ads boards require modifications
+	  to support this transceiver.
+
+config USB_EHCI_FSL_1504
+	bool "Philips ISP1504"
+	depends on MACH_MX27ADS || MACH_MX31ADS || MACH_MX32ADS ||MACH_MX31_3DS
+	---help---
+	  Enable support for the High Speed Philips ISP1504 transceiver.
+
+	  This is the factory default for the mx31ads and mx32ads boards.
+	  The mx27ads board requires modifications to support this transceiver.
+
+config USB_EHCI_FSL_UTMI
+	bool "Internal UTMI"
+	depends on (ARCH_MX25 || ARCH_MX35 || ARCH_MX37 || ARCH_MX51 || ARCH_STMP3XXX)
+	---help---
+	  Enable support for the on-chip High Speed UTMI transceiver.
+
+	  This is the factory default for the mx35ads board.
+
+endchoice
+
+
 config USB_EHCI_ROOT_HUB_TT
 	bool "Root Hub Transaction Translators"
 	depends on USB_EHCI_HCD
+	default y if USB_EHCI_ARC
 	---help---
 	  Some EHCI chips have vendor-specific extensions to integrate
 	  transaction translators, so that no OHCI or UHCI companion
diff --git a/drivers/usb/host/ehci-arc.c b/drivers/usb/host/ehci-arc.c
new file mode 100644
index 000000000000..dd2929f472e8
--- /dev/null
+++ b/drivers/usb/host/ehci-arc.c
@@ -0,0 +1,635 @@
+/*
+ * (C) Copyright David Brownell 2000-2002
+ * Copyright (c) 2005 MontaVista Software
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ * Ported to 834x by Randy Vinson <rvinson@mvista.com> using code provided
+ * by Hunter Wu.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/fsl_devices.h>
+#include <linux/usb/otg.h>
+
+#include "ehci-fsl.h"
+
+extern struct resource *otg_get_resources(void);
+
+#undef EHCI_PROC_PTC
+#ifdef EHCI_PROC_PTC		/* /proc PORTSC:PTC support */
+/*
+ * write a PORTSC:PTC value to /proc/driver/ehci-ptc
+ * to put the controller into test mode.
+ */
+#include <linux/proc_fs.h>
+#include <asm/uaccess.h>
+#define EFPSL 3			/* ehci fsl proc string length */
+
+static int ehci_fsl_proc_read(char *page, char **start, off_t off, int count,
+			      int *eof, void *data)
+{
+	return 0;
+}
+
+static int ehci_fsl_proc_write(struct file *file, const char __user *buffer,
+			       unsigned long count, void *data)
+{
+	int ptc;
+	u32 portsc;
+	struct ehci_hcd *ehci = (struct ehci_hcd *) data;
+	char str[EFPSL] = {0};
+
+	if (count > EFPSL-1)
+		return -EINVAL;
+
+	if (copy_from_user(str, buffer, count))
+		return -EFAULT;
+
+	str[count] = '\0';
+
+	ptc = simple_strtoul(str, NULL, 0);
+
+	portsc = ehci_readl(ehci, &ehci->regs->port_status[0]);
+	portsc &= ~(0xf << 16);
+	portsc |= (ptc << 16);
+	printk(KERN_INFO "PTC %x  portsc %08x\n", ptc, portsc);
+
+	ehci_writel(ehci, portsc, &ehci->regs->port_status[0]);
+
+	return count;
+}
+
+static int ehci_testmode_init(struct ehci_hcd *ehci)
+{
+	struct proc_dir_entry *entry;
+
+	entry = create_proc_read_entry("driver/ehci-ptc", 0644, NULL,
+				       ehci_fsl_proc_read, ehci);
+	if (!entry)
+		return -ENODEV;
+
+	entry->write_proc = ehci_fsl_proc_write;
+	return 0;
+}
+#else
+static int ehci_testmode_init(struct ehci_hcd *ehci)
+{
+	return 0;
+}
+#endif	/* /proc PORTSC:PTC support */
+
+
+/* PCI-based HCs are common, but plenty of non-PCI HCs are used too */
+
+/* configure so an HC device and id are always provided */
+/* always called with process context; sleeping is OK */
+
+/**
+ * usb_hcd_fsl_probe - initialize FSL-based HCDs
+ * @drvier: Driver to be used for this HCD
+ * @pdev: USB Host Controller being probed
+ * Context: !in_interrupt()
+ *
+ * Allocates basic resources for this USB host controller.
+ *
+ */
+static int usb_hcd_fsl_probe(const struct hc_driver *driver,
+			     struct platform_device *pdev)
+{
+	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
+	struct usb_hcd *hcd;
+	struct resource *res;
+	int irq;
+	int retval;
+
+	pr_debug("initializing FSL-SOC USB Controller\n");
+
+	/* Need platform data for setup */
+	if (!pdata) {
+		dev_err(&pdev->dev,
+			"No platform data for %s.\n", pdev->dev.bus_id);
+		return -ENODEV;
+	}
+
+	/*
+	 * This is a host mode driver, verify that we're supposed to be
+	 * in host mode.
+	 */
+	if (!((pdata->operating_mode == FSL_USB2_DR_HOST) ||
+	      (pdata->operating_mode == FSL_USB2_MPH_HOST) ||
+	      (pdata->operating_mode == FSL_USB2_DR_OTG))) {
+		dev_err(&pdev->dev,
+			"Non Host Mode configured for %s. "
+			"Wrong driver linked.\n", pdev->dev.bus_id);
+		return -ENODEV;
+	}
+
+	hcd = usb_create_hcd(driver, &pdev->dev, pdev->dev.bus_id);
+	if (!hcd) {
+		retval = -ENOMEM;
+		goto err1;
+	}
+
+#if defined(CONFIG_USB_OTG)
+	if (pdata->operating_mode == FSL_USB2_DR_OTG) {
+		res = otg_get_resources();
+		if (!res) {
+			dev_err(&pdev->dev,
+				"Found HC with no IRQ. Check %s setup!\n",
+				pdev->dev.bus_id);
+			return -ENODEV;
+		}
+		irq = res[1].start;
+		hcd->rsrc_start = res[0].start;
+		hcd->rsrc_len = res[0].end - res[0].start + 1;
+	} else
+#endif
+	{
+		res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+		if (!res) {
+			dev_err(&pdev->dev,
+				"Found HC with no IRQ. Check %s setup!\n",
+				pdev->dev.bus_id);
+			return -ENODEV;
+		}
+		irq = res->start;
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+		hcd->rsrc_start = res->start;
+		hcd->rsrc_len = resource_size(res);
+
+		if (!request_mem_region(hcd->rsrc_start, hcd->rsrc_len,
+					driver->description)) {
+			dev_dbg(&pdev->dev, "controller already in use\n");
+			retval = -EBUSY;
+			goto err2;
+		}
+	}
+
+	hcd->regs = ioremap(hcd->rsrc_start, hcd->rsrc_len);
+
+	if (hcd->regs == NULL) {
+		dev_dbg(&pdev->dev, "error mapping memory\n");
+		retval = -EFAULT;
+		goto err3;
+	}
+	pdata->regs = hcd->regs;
+
+	/*
+	 * do platform specific init: check the clock, grab/config pins, etc.
+	 */
+	if (pdata->platform_init && pdata->platform_init(pdev)) {
+		retval = -ENODEV;
+		goto err3;
+	}
+
+	fsl_platform_set_host_mode(hcd);
+	hcd->power_budget = pdata->power_budget;
+
+	retval = usb_add_hcd(hcd, irq, IRQF_DISABLED | IRQF_SHARED);
+	if (retval != 0)
+		goto err4;
+
+	fsl_platform_set_vbus_power(pdata, 1);
+
+#ifdef CONFIG_USB_OTG
+	if (pdata->operating_mode == FSL_USB2_DR_OTG) {
+		struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+
+		dbg("pdev=0x%p  hcd=0x%p  ehci=0x%p\n", pdev, hcd, ehci);
+
+		ehci->transceiver = otg_get_transceiver();
+		dbg("ehci->transceiver=0x%p\n", ehci->transceiver);
+
+		if (ehci->transceiver) {
+			retval = otg_set_host(ehci->transceiver,
+					      &ehci_to_hcd(ehci)->self);
+			if (retval) {
+				if (ehci->transceiver)
+					put_device(ehci->transceiver->dev);
+				goto err4;
+			}
+		} else {
+			printk(KERN_ERR "can't find transceiver\n");
+			retval = -ENODEV;
+			goto err4;
+		}
+	}
+#endif
+
+	if (pdata->suspended) {
+		pdata->suspended = 0;
+		if (pdata->already_suspended)
+			pdata->already_suspended = 0;
+	}
+
+	fsl_platform_set_ahb_burst(hcd);
+	ehci_testmode_init(hcd_to_ehci(hcd));
+	return retval;
+
+err4:
+	iounmap(hcd->regs);
+err3:
+	if (pdata->operating_mode != FSL_USB2_DR_OTG)
+		release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
+err2:
+	usb_put_hcd(hcd);
+err1:
+	dev_err(&pdev->dev, "init %s fail, %d\n", pdev->dev.bus_id, retval);
+	if (pdata->platform_uninit)
+		pdata->platform_uninit(pdata);
+	return retval;
+}
+
+/* may be called without controller electrically present */
+/* may be called with controller, bus, and devices active */
+
+/**
+ * usb_hcd_fsl_remove - shutdown processing for FSL-based HCDs
+ * @dev: USB Host Controller being removed
+ * Context: !in_interrupt()
+ *
+ * Reverses the effect of usb_hcd_fsl_probe().
+ *
+ */
+static void usb_hcd_fsl_remove(struct usb_hcd *hcd,
+			       struct platform_device *pdev)
+{
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
+
+	/* DDD shouldn't we turn off the power here? */
+	fsl_platform_set_vbus_power(pdata, 0);
+
+	if (ehci->transceiver) {
+		(void)otg_set_host(ehci->transceiver, 0);
+		put_device(ehci->transceiver->dev);
+	} else {
+		release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
+	}
+
+	usb_remove_hcd(hcd);
+	usb_put_hcd(hcd);
+
+	/*
+	 * do platform specific un-initialization:
+	 * release iomux pins, etc.
+	 */
+	if (pdata->platform_uninit)
+		pdata->platform_uninit(pdata);
+
+	iounmap(hcd->regs);
+}
+
+static void fsl_setup_phy(struct ehci_hcd *ehci,
+			  enum fsl_usb2_phy_modes phy_mode, int port_offset)
+{
+	u32 portsc;
+
+	portsc = ehci_readl(ehci, &ehci->regs->port_status[port_offset]);
+	portsc &= ~(PORT_PTS_MSK | PORT_PTS_PTW);
+
+	switch (phy_mode) {
+	case FSL_USB2_PHY_ULPI:
+		portsc |= PORT_PTS_ULPI;
+		break;
+	case FSL_USB2_PHY_SERIAL:
+		portsc |= PORT_PTS_SERIAL;
+		break;
+	case FSL_USB2_PHY_UTMI_WIDE:
+		portsc |= PORT_PTS_PTW;
+		/* fall through */
+	case FSL_USB2_PHY_UTMI:
+		portsc |= PORT_PTS_UTMI;
+		break;
+	case FSL_USB2_PHY_NONE:
+		break;
+	}
+	ehci_writel(ehci, portsc, &ehci->regs->port_status[port_offset]);
+}
+
+/* called after powerup, by probe or system-pm "wakeup" */
+static int ehci_fsl_reinit(struct ehci_hcd *ehci)
+{
+	fsl_platform_usb_setup(ehci);
+	ehci_port_power(ehci, 0);
+	return 0;
+}
+
+/* called during probe() after chip reset completes */
+static int ehci_fsl_setup(struct usb_hcd *hcd)
+{
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	int retval;
+	struct fsl_usb2_platform_data *pdata;
+	pdata = hcd->self.controller->platform_data;
+
+	ehci->big_endian_desc = pdata->big_endian_desc;
+	ehci->big_endian_mmio = pdata->big_endian_mmio;
+
+	/* EHCI registers start at offset 0x100 */
+	ehci->caps = hcd->regs + 0x100;
+	ehci->regs = hcd->regs + 0x100 +
+	    HC_LENGTH(ehci_readl(ehci, &ehci->caps->hc_capbase));
+	dbg_hcs_params(ehci, "reset");
+	dbg_hcc_params(ehci, "reset");
+
+	/* cache this readonly data; minimize chip reads */
+	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
+
+	retval = ehci_halt(ehci);
+
+	/* data structure init */
+	retval = ehci_init(hcd);
+	if (retval)
+		return retval;
+
+	hcd->has_tt = 1;
+
+	ehci->sbrn = 0x20;
+
+	ehci_reset(ehci);
+
+	retval = ehci_fsl_reinit(ehci);
+	return retval;
+}
+
+static const struct hc_driver ehci_fsl_hc_driver = {
+	.description = hcd_name,
+	.product_desc = "Freescale On-Chip EHCI Host Controller",
+	.hcd_priv_size = sizeof(struct ehci_hcd),
+
+	/*
+	 * generic hardware linkage
+	 */
+	.irq = ehci_irq,
+	.flags = FSL_PLATFORM_HC_FLAGS,
+
+	/*
+	 * basic lifecycle operations
+	 */
+	.reset = ehci_fsl_setup,
+	.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,
+
+	/*
+	 * 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,
+	.start_port_reset = ehci_start_port_reset,
+};
+
+static int ehci_fsl_drv_probe(struct platform_device *pdev)
+{
+	if (usb_disabled())
+		return -ENODEV;
+
+	return usb_hcd_fsl_probe(&ehci_fsl_hc_driver, pdev);
+}
+
+static int ehci_fsl_drv_remove(struct platform_device *pdev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+
+	usb_hcd_fsl_remove(hcd, pdev);
+	return 0;
+}
+
+static void ehci_fsl_drv_shutdown(struct platform_device *pdev)
+{
+	usb_hcd_platform_shutdown(pdev);
+}
+
+#ifdef CONFIG_PM
+extern void usb_host_set_wakeup(struct device *wkup_dev, bool para);
+/* suspend/resume, section 4.3 */
+
+/* These routines rely on the bus (pci, platform, etc)
+ * to handle powerdown and wakeup, and currently also on
+ * transceivers that don't need any software attention to set up
+ * the right sort of wakeup.
+ *
+ * They're also used for turning on/off the port when doing OTG.
+ */
+static int ehci_fsl_drv_suspend(struct platform_device *pdev,
+				pm_message_t message)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	u32 tmp, port_status;
+	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
+
+#ifdef DEBUG
+	u32 mode = ehci_readl(ehci, hcd->regs + FSL_SOC_USB_USBMODE);
+	mode &= USBMODE_CM_MASK;
+	tmp = ehci_readl(ehci, hcd->regs + 0x140);	/* usbcmd */
+
+	printk(KERN_DEBUG "%s('%s'): suspend=%d already_suspended=%d "
+	       "mode=%d  usbcmd %08x\n", __func__, pdata->name,
+	       pdata->suspended, pdata->already_suspended, mode, tmp);
+#endif
+
+	/*
+	 * If the controller is already suspended, then this must be a
+	 * PM suspend.  Remember this fact, so that we will leave the
+	 * controller suspended at PM resume time.
+	 */
+	if (pdata->suspended) {
+		pr_debug("%s: already suspended, leaving early\n", __func__);
+		pdata->already_suspended = 1;
+		return 0;
+	}
+
+	pr_debug("%s: suspending...\n", __func__);
+
+	printk(KERN_INFO "USB Host suspended\n");
+
+	port_status = ehci_readl(ehci, &ehci->regs->port_status[0]);
+	hcd->state = HC_STATE_SUSPENDED;
+	pdev->dev.power.power_state = PMSG_SUSPEND;
+
+	/* ignore non-host interrupts */
+	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+
+	/* stop the controller */
+	tmp = ehci_readl(ehci, &ehci->regs->command);
+	tmp &= ~CMD_RUN;
+	ehci_writel(ehci, tmp, &ehci->regs->command);
+
+	/* save EHCI registers */
+	pdata->pm_command = ehci_readl(ehci, &ehci->regs->command);
+	pdata->pm_command &= ~CMD_RUN;
+	pdata->pm_status  = ehci_readl(ehci, &ehci->regs->status);
+	pdata->pm_intr_enable  = ehci_readl(ehci, &ehci->regs->intr_enable);
+	pdata->pm_frame_index  = ehci_readl(ehci, &ehci->regs->frame_index);
+	pdata->pm_segment  = ehci_readl(ehci, &ehci->regs->segment);
+	pdata->pm_frame_list  = ehci_readl(ehci, &ehci->regs->frame_list);
+	pdata->pm_async_next  = ehci_readl(ehci, &ehci->regs->async_next);
+	pdata->pm_configured_flag  =
+		ehci_readl(ehci, &ehci->regs->configured_flag);
+	pdata->pm_portsc = ehci_readl(ehci, &ehci->regs->port_status[0]);
+
+	/* clear the W1C bits */
+	pdata->pm_portsc &= cpu_to_hc32(ehci, ~PORT_RWC_BITS);
+
+	pdata->suspended = 1;
+
+	if (!device_may_wakeup(&(pdev->dev))) {
+		/* clear PP to cut power to the port */
+		tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
+		tmp &= ~PORT_POWER;
+		ehci_writel(ehci, tmp, &ehci->regs->port_status[0]);
+		return 0;
+	}
+
+	/* device_may_wakeup */
+	if (!((ehci->transceiver) &&
+			(readl(hcd->regs + 0x1A4) & (1 << 8)))) {
+		/* enable remote wake up irq */
+		usb_host_set_wakeup(&(pdev->dev), true);
+
+	/* We CAN NOT enable wake up by connetion and disconnection
+	 * concurrently */
+	tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
+	/* if there is no usb device connectted */
+	if (port_status & PORT_CONNECT) {
+		/* enable wake up by usb device disconnection */
+		tmp |= PORT_WKDISC_E;
+		tmp &= ~(PORT_WKOC_E | PORT_WKCONN_E);
+	} else {
+		/* enable wake up by usb device insertion */
+		tmp |= PORT_WKCONN_E;
+		tmp &= ~(PORT_WKOC_E | PORT_WKDISC_E);
+	}
+	ehci_writel(ehci, tmp, &ehci->regs->port_status[0]);
+
+	/* Set the port into suspend */
+	tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
+	tmp |= PORT_SUSPEND;
+	ehci_writel(ehci, tmp, &ehci->regs->port_status[0]);
+
+	/* Disable PHY clock */
+	tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
+	tmp |= (1 << 23);
+	ehci_writel(ehci, tmp, &ehci->regs->port_status[0]);
+	}
+
+	if (pdata->platform_suspend)
+		pdata->platform_suspend(pdata);
+
+	return 0;
+}
+
+static int ehci_fsl_drv_resume(struct platform_device *pdev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	u32 tmp;
+	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
+
+	pr_debug("%s('%s'): suspend=%d already_suspended=%d\n", __func__,
+		pdata->name, pdata->suspended, pdata->already_suspended);
+
+	/*
+	 * If the controller was already suspended at suspend time,
+	 * then don't resume it now.
+	 */
+	if (pdata->already_suspended) {
+		pr_debug("already suspended, leaving early\n");
+		pdata->already_suspended = 0;
+		return 0;
+	}
+
+	if (!pdata->suspended) {
+		pr_debug("not suspended, leaving early\n");
+		return 0;
+	}
+
+	if (device_may_wakeup(&(pdev->dev))) {
+		tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
+		if (tmp & (1 << 23)) {
+			tmp &= ~(1 << 23);
+			ehci_writel(ehci, tmp, &ehci->regs->port_status[0]);
+			msleep(10);
+		}
+	}
+
+	pdata->suspended = 0;
+
+	pr_debug("%s resuming...\n", __func__);
+
+	/* set host mode */
+	fsl_platform_set_host_mode(hcd);
+
+	if (pdata->platform_resume)
+		pdata->platform_resume(pdata);
+
+	/* restore EHCI registers */
+	ehci_writel(ehci, pdata->pm_command, &ehci->regs->command);
+	ehci_writel(ehci, pdata->pm_intr_enable, &ehci->regs->intr_enable);
+	ehci_writel(ehci, pdata->pm_frame_index, &ehci->regs->frame_index);
+	ehci_writel(ehci, pdata->pm_segment, &ehci->regs->segment);
+	ehci_writel(ehci, pdata->pm_frame_list, &ehci->regs->frame_list);
+	ehci_writel(ehci, pdata->pm_async_next, &ehci->regs->async_next);
+	ehci_writel(ehci, pdata->pm_configured_flag,
+		    &ehci->regs->configured_flag);
+	ehci_writel(ehci, pdata->pm_portsc, &ehci->regs->port_status[0]);
+
+	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+	hcd->state = HC_STATE_RUNNING;
+	pdev->dev.power.power_state = PMSG_ON;
+
+	tmp = ehci_readl(ehci, &ehci->regs->command);
+	tmp |= CMD_RUN;
+	ehci_writel(ehci, tmp, &ehci->regs->command);
+
+	usb_hcd_resume_root_hub(hcd);
+
+	printk(KERN_INFO "USB Host resumed\n");
+	return 0;
+}
+#endif				/* CONFIG_USB_OTG */
+
+MODULE_ALIAS("fsl-ehci");
+
+static struct platform_driver ehci_fsl_driver = {
+	.probe = ehci_fsl_drv_probe,
+	.remove = ehci_fsl_drv_remove,
+	.shutdown = ehci_fsl_drv_shutdown,
+#ifdef CONFIG_PM
+	.suspend = ehci_fsl_drv_suspend,
+	.resume = ehci_fsl_drv_resume,
+#endif
+	.driver = {
+		   .name = "fsl-ehci",
+		   },
+};
diff --git a/drivers/usb/host/ehci-fsl.h b/drivers/usb/host/ehci-fsl.h
index b5e59db53347..f3db681f5025 100644
--- a/drivers/usb/host/ehci-fsl.h
+++ b/drivers/usb/host/ehci-fsl.h
@@ -19,6 +19,9 @@
 #define _EHCI_FSL_H
 
 /* offsets for the non-ehci registers in the FSL SOC USB controller */
+#define FSL_SOC_USB_SBUSCFG     0x90
+#define FSL_SOC_USB_BURSTSIZE	0x160
+#define FSL_SOC_USB_TXFILLTUNING	0x164
 #define FSL_SOC_USB_ULPIVP	0x170
 #define FSL_SOC_USB_PORTSC1	0x184
 #define PORT_PTS_MSK		(3<<30)
@@ -26,8 +29,12 @@
 #define PORT_PTS_ULPI		(2<<30)
 #define	PORT_PTS_SERIAL		(3<<30)
 #define PORT_PTS_PTW		(1<<28)
+#define PORT_PTS_PHCD		(1<<23)
 #define FSL_SOC_USB_PORTSC2	0x188
 #define FSL_SOC_USB_USBMODE	0x1a8
+#define USBMODE_CM_HOST		(3 << 0)	/* controller mode: host */
+#define USBMODE_ES		(1 << 2)	/* (Big) Endian Select */
+
 #define FSL_SOC_USB_SNOOP1	0x400	/* NOTE: big-endian */
 #define FSL_SOC_USB_SNOOP2	0x404	/* NOTE: big-endian */
 #define FSL_SOC_USB_AGECNTTHRSH	0x408	/* NOTE: big-endian */
@@ -35,4 +42,11 @@
 #define FSL_SOC_USB_SICTRL	0x410	/* NOTE: big-endian */
 #define FSL_SOC_USB_CTRL	0x500	/* NOTE: big-endian */
 #define SNOOP_SIZE_2GB		0x1e
+
+#if defined(CONFIG_ARCH_MXC) || defined(CONFIG_ARCH_STMP3XXX)
+#include <mach/fsl_usb.h>
+#elif CONFIG_PPC32
+#include <asm/fsl_usb.h>
+#endif
+
 #endif				/* _EHCI_FSL_H */
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 4725d15d096f..50b6fa08e987 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -253,8 +253,13 @@ static void end_unlink_async(struct ehci_hcd *ehci);
 static void ehci_work(struct ehci_hcd *ehci);
 
 #include "ehci-hub.c"
+#ifdef CONFIG_USB_STATIC_IRAM
+#include "ehci-mem-iram.c"
+#include "ehci-q-iram.c"
+#else
 #include "ehci-mem.c"
 #include "ehci-q.c"
+#endif
 #include "ehci-sched.c"
 
 /*-------------------------------------------------------------------------*/
@@ -589,6 +594,14 @@ static int ehci_run (struct usb_hcd *hcd)
 #endif
 	}
 
+	/* For those designs that contain both host & device capability,
+	 * the controller will default to an idle state and will need to
+	 * be initialized to the desired operating mode after reset.
+	 * For combination host/device controllers of FSL, SW need program
+	 * For combination host/device controllers of FSL, SW need program
+	 */
+	temp = readl(hcd->regs + 0x1a8);
+	writel(temp | (3 << 0), hcd->regs + 0x1a8);
 
 	// Philips, Intel, and maybe others need CMD_RUN before the
 	// root hub will detect new devices (why?); NEC doesn't
@@ -1014,6 +1027,11 @@ MODULE_LICENSE ("GPL");
 #define	PLATFORM_DRIVER		ehci_hcd_au1xxx_driver
 #endif
 
+#ifdef CONFIG_USB_EHCI_ARC
+#include "ehci-arc.c"
+#define	PLATFORM_DRIVER		ehci_fsl_driver
+#endif
+
 #ifdef CONFIG_PPC_PS3
 #include "ehci-ps3.c"
 #define	PS3_SYSTEM_BUS_DRIVER	ps3_ehci_driver
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 218f9660d7ee..6a607d240949 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -530,6 +530,37 @@ ehci_hub_descriptor (
 	desc->wHubCharacteristics = cpu_to_le16(temp);
 }
 
+#ifdef CONFIG_USB_OTG
+static int ehci_start_port_reset(struct usb_hcd *hcd, unsigned port)
+{
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	u32 status;
+
+	if (!port)
+		return -EINVAL;
+	port--;
+
+	/* start port reset before HNP protocol time out */
+	status = readl(&ehci->regs->port_status[port]);
+	if (!(status & PORT_CONNECT))
+		return -ENODEV;
+
+	/* khubd will finish the reset later */
+	if (ehci_is_TDI(ehci))
+		writel(PORT_RESET | (status & ~(PORT_CSC | PORT_PEC
+				| PORT_OCC)), &ehci->regs->port_status[port]);
+	else
+		writel(PORT_RESET, &ehci->regs->port_status[port]);
+
+	return 0;
+}
+#else
+static int ehci_start_port_reset(struct usb_hcd *hcd, unsigned port)
+{
+	return 0;
+}
+#endif /* CONFIG_USB_OTG */
+
 /*-------------------------------------------------------------------------*/
 
 static int ehci_hub_control (
diff --git a/drivers/usb/host/ehci-mem-iram.c b/drivers/usb/host/ehci-mem-iram.c
new file mode 100644
index 000000000000..66dd6e484fdd
--- /dev/null
+++ b/drivers/usb/host/ehci-mem-iram.c
@@ -0,0 +1,506 @@
+/*
+ * Copyright (c) 2001 by David Brownell
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+/* this file is part of ehci-hcd.c */
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * There's basically three types of memory:
+ *	- data used only by the HCD ... kmalloc is fine
+ *	- async and periodic schedules, shared by HC and HCD ... these
+ *	  need to use dma_pool or dma_alloc_coherent
+ *	- driver buffers, read/written by HC ... single shot DMA mapped
+ *
+ * There's also "register" data (e.g. PCI or SOC), which is memory mapped.
+ * No memory seen by this driver is pageable.
+ */
+
+/*-------------------------------------------------------------------------*/
+
+/* Allocate the key transfer structures from the previously allocated pool */
+#include <linux/smp_lock.h>
+
+bool use_iram_qtd;
+
+struct memDesc {
+	u32 start;
+	u32 end;
+	struct memDesc *next;
+} ;
+
+static u32 g_usb_pool_start;
+static s32 g_usb_pool_count;
+static u32 g_total_pages;
+static u32 g_alignment = 32;
+struct memDesc *g_allocated_desc;
+static spinlock_t g_usb_sema;
+static u32 g_debug_qtd_allocated;
+static u32 g_debug_qH_allocated;
+static int g_alloc_map;
+
+/*!
+ * usb_pool_initialize
+ *
+ * @param       memPool         start address of the pool
+ * @param       poolSize        memory pool size
+ * @param       alignment       alignment for example page alignmnet will be 4K
+ *
+ * @return      0 for success  -1 for errors
+ */
+static int usb_pool_initialize(u32 memPool, u32 poolSize, u32 alignment)
+{
+	if (g_usb_pool_count) {
+		printk(KERN_INFO "usb_pool_initialize : already initialzed.\n");
+		return 0;
+	}
+
+	g_alignment = alignment;
+	if (g_alignment == 0) {
+		printk(KERN_INFO
+		       "usb_pool_initialize : g_alignment can not be zero.\n");
+		g_alignment = 32;
+	}
+
+	g_total_pages = poolSize / g_alignment;
+	g_usb_pool_start = (u32) memPool;
+
+	g_allocated_desc = kmalloc(sizeof(struct memDesc), GFP_KERNEL);
+	if (!g_allocated_desc) {
+		printk(KERN_ALERT "usb_pool_initialize : kmalloc failed \n");
+		return (-1);
+	}
+
+	g_allocated_desc->start = 0;
+	g_allocated_desc->end = 0;
+	g_allocated_desc->next = NULL;
+
+	spin_lock_init(&g_usb_sema);
+	g_usb_pool_count++;
+	return (0);
+}
+
+static void usb_pool_deinit()
+{
+	if (--g_usb_pool_count < 0)
+		g_usb_pool_count = 0;
+}
+
+/*!
+ * usb_malloc
+ *
+ * @param       size        memory pool size
+ *
+ * @return      physical address, 0 for error
+ */
+static u32 usb_malloc(u32 size, gfp_t mem_flags)
+{
+	unsigned long flags;
+	struct memDesc *prevDesc = NULL;
+	struct memDesc *nextDesc = NULL;
+	struct memDesc *currentDesc = NULL;
+	u32 pages = (size + g_alignment - 1) / g_alignment;
+
+	if ((size == 0) || (pages > g_total_pages))
+		return 0;
+
+	currentDesc = kmalloc(sizeof(struct memDesc), mem_flags);
+	if (!currentDesc) {
+		printk(KERN_ALERT "usb_malloc: kmalloc failed \n");
+		return 0;
+	}
+
+	spin_lock_irqsave(&g_usb_sema, flags);
+
+	/* Create the first Allocated descriptor */
+	if (!g_allocated_desc->next) {
+		g_allocated_desc->next = currentDesc;
+		currentDesc->start = 0;
+		currentDesc->end = pages;
+		currentDesc->next = NULL;
+		spin_unlock_irqrestore(&g_usb_sema, flags);
+		return (g_usb_pool_start + currentDesc->start * g_alignment);
+	}
+
+	/* Find the free spot */
+	prevDesc = g_allocated_desc;
+	while (prevDesc->next) {
+		nextDesc = prevDesc->next;
+		if (pages <= nextDesc->start - prevDesc->end) {
+			currentDesc->start = prevDesc->end;
+			currentDesc->end = currentDesc->start + pages;
+			currentDesc->next = nextDesc;
+			prevDesc->next = currentDesc;
+			break;
+		}
+		prevDesc = nextDesc;
+	}
+
+	/* Do not find the free spot inside the chain, append to the end */
+	if (!prevDesc->next) {
+		if (pages > (g_total_pages - prevDesc->end)) {
+			spin_unlock_irqrestore(&g_usb_sema, flags);
+			kfree(currentDesc);
+			return 0;
+		} else {
+			currentDesc->start = prevDesc->end;
+			currentDesc->end = currentDesc->start + pages;
+			currentDesc->next = NULL;
+			prevDesc->next = currentDesc;
+		}
+	}
+
+	spin_unlock_irqrestore(&g_usb_sema, flags);
+	return (g_usb_pool_start + currentDesc->start * g_alignment);
+}
+
+/*!
+ * usb_free
+ *
+ * @param       physical        physical address try to free
+ *
+ */
+static void usb_free(u32 physical)
+{
+	unsigned long flags;
+	struct memDesc *prevDesc = NULL;
+	struct memDesc *nextDesc = NULL;
+	u32 pages = (physical - g_usb_pool_start) / g_alignment;
+
+	/* Protect the memory pool data structures. */
+	spin_lock_irqsave(&g_usb_sema, flags);
+
+	prevDesc = g_allocated_desc;
+	while (prevDesc->next) {
+		nextDesc = prevDesc->next;
+		if (nextDesc->start == pages) {
+			prevDesc->next = nextDesc->next;
+			kfree(nextDesc);
+			break;
+		}
+		prevDesc = prevDesc->next;
+	}
+	/* All done with memory pool data structures. */
+	spin_unlock_irqrestore(&g_usb_sema, flags);
+}
+
+static int address_to_buffer(struct ehci_hcd *ehci, int address)
+{
+	int i;
+
+	for (i = 0; i < IRAM_NTD; i++) {
+		if (ehci->usb_address[i] == address)
+			return i;
+	}
+	return IRAM_NTD;
+}
+
+static void use_buffer(struct ehci_hcd *ehci, int address)
+{
+	int i;
+
+	for (i = 0; i < IRAM_NTD; i++) {
+		if (ehci->usb_address[i] == address)
+			return;
+	}
+
+	if (ehci->usb_address[0] == 0) {
+		ehci->usb_address[0] = address;
+		printk(KERN_INFO "usb_address[0] %x\n", address);
+		return;
+	} else if (ehci->usb_address[1] == 0) {
+		ehci->usb_address[1] = address;
+		printk(KERN_INFO "usb_address[1] %x\n", address);
+		return;
+	} else
+		printk(KERN_ALERT "qh_make run out of iRAM, already be used");
+}
+
+static u32 alloc_iram_buf(void)
+{
+	int i;
+
+	for (i = 0; i < IRAM_NTD; i++) {
+		if (!(g_alloc_map & (1 << i))) {
+			g_alloc_map |= (1 << i);
+			return USB_IRAM_BASE_ADDR + i * (IRAM_TD_SIZE * 2);
+		}
+	}
+	panic("Out of IRAM buffers\n");
+}
+
+void free_iram_buf(u32 buf)
+{
+	int i = (buf - USB_IRAM_BASE_ADDR) / (IRAM_TD_SIZE * 2);
+
+	g_alloc_map &= ~(1 << i);
+}
+
+static inline void ehci_qtd_init(struct ehci_hcd *ehci, struct ehci_qtd *qtd,
+				 dma_addr_t dma)
+{
+	memset(qtd, 0, sizeof *qtd);
+	qtd->qtd_dma = dma;
+	qtd->hw_token = cpu_to_le32(QTD_STS_HALT);
+	qtd->hw_next = EHCI_LIST_END(ehci);
+	qtd->hw_alt_next = EHCI_LIST_END(ehci);
+	INIT_LIST_HEAD(&qtd->qtd_list);
+}
+
+static struct ehci_qtd *ehci_qtd_alloc(struct ehci_hcd *ehci, gfp_t flags)
+{
+	struct ehci_qtd *qtd;
+	dma_addr_t dma;
+
+	if (use_iram_qtd) {
+		dma = usb_malloc(sizeof(struct ehci_qtd), flags);
+		if (dma != 0)
+			qtd = (struct ehci_qtd *)IO_ADDRESS(dma);
+		else
+			qtd = dma_pool_alloc(ehci->qtd_pool, flags, &dma);
+	}
+	else
+		qtd = dma_pool_alloc(ehci->qtd_pool, flags, &dma);
+
+	if (qtd != NULL) {
+		ehci_qtd_init(ehci, qtd, dma);
+		++g_debug_qtd_allocated;
+	} else {
+		panic
+		    ("out of i-ram for qtd allocation g_debug_qtd_allocated %d \
+			size%d \n", g_debug_qtd_allocated,
+			sizeof(struct ehci_qtd));
+	}
+	return qtd;
+}
+
+static inline void ehci_qtd_free(struct ehci_hcd *ehci, struct ehci_qtd *qtd)
+{
+	if ((qtd->qtd_dma & (USB_IRAM_BASE_ADDR & 0xFFF00000)) ==
+	    (USB_IRAM_BASE_ADDR & 0xFFF00000))
+		usb_free(qtd->qtd_dma);
+	else
+		dma_pool_free(ehci->qtd_pool, qtd, qtd->qtd_dma);
+	--g_debug_qtd_allocated;
+}
+
+static void qh_destroy(struct ehci_qh *qh)
+{
+	struct ehci_hcd *ehci = qh->ehci;
+
+	/* clean qtds first, and know this is not linked */
+	if (!list_empty(&qh->qtd_list) || qh->qh_next.ptr) {
+		ehci_dbg(ehci, "unused qh not empty!\n");
+		BUG();
+	}
+	if (qh->dummy)
+		ehci_qtd_free(ehci, qh->dummy);
+	int i;
+	for (i = 0; i < IRAM_NTD; i++) {
+		if (ehci->usb_address[i] == (qh->hw_info1 & 0x7F))
+			ehci->usb_address[i] = 0;
+	}
+
+	if ((qh->qh_dma & (USB_IRAM_BASE_ADDR & 0xFFF00000)) ==
+	    (USB_IRAM_BASE_ADDR & 0xFFF00000))
+		usb_free(qh->qh_dma);
+	else
+		dma_pool_free(ehci->qh_pool, qh, qh->qh_dma);
+	--g_debug_qH_allocated;
+}
+
+static struct ehci_qh *ehci_qh_alloc(struct ehci_hcd *ehci, gfp_t flags)
+{
+	struct ehci_qh *qh;
+	dma_addr_t dma;
+
+	dma = usb_malloc(sizeof(struct ehci_qh), flags);
+	if (dma != 0)
+		qh = (struct ehci_qh *)IO_ADDRESS(dma);
+	else
+		qh = (struct ehci_qh *)
+		    dma_pool_alloc(ehci->qh_pool, flags, &dma);
+	++g_debug_qH_allocated;
+	if (qh == NULL) {
+		panic("run out of i-ram for qH allocation\n");
+		return qh;
+	}
+
+	memset(qh, 0, sizeof *qh);
+	qh->refcount = 1;
+	qh->ehci = ehci;
+	qh->qh_dma = dma;
+	INIT_LIST_HEAD(&qh->qtd_list);
+
+	/* dummy td enables safe urb queuing */
+	qh->dummy = ehci_qtd_alloc(ehci, flags);
+	if (qh->dummy == NULL) {
+		ehci_dbg(ehci, "no dummy td\n");
+		dma_pool_free(ehci->qh_pool, qh, qh->qh_dma);
+		qh = NULL;
+	}
+	return qh;
+}
+
+/* to share a qh (cpu threads, or hc) */
+static inline struct ehci_qh *qh_get(struct ehci_qh *qh)
+{
+	WARN_ON(!qh->refcount);
+	qh->refcount++;
+	return qh;
+}
+
+static inline void qh_put(struct ehci_qh *qh)
+{
+	if (!--qh->refcount)
+		qh_destroy(qh);
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* The queue heads and transfer descriptors are managed from pools tied
+ * to each of the "per device" structures.
+ * This is the initialisation and cleanup code.
+ */
+
+static void ehci_mem_cleanup(struct ehci_hcd *ehci)
+{
+	if (ehci->async)
+		qh_put(ehci->async);
+	ehci->async = NULL;
+
+	/* DMA consistent memory and pools */
+	if (ehci->qtd_pool)
+		dma_pool_destroy(ehci->qtd_pool);
+	ehci->qtd_pool = NULL;
+
+	if (ehci->qh_pool) {
+		dma_pool_destroy(ehci->qh_pool);
+		ehci->qh_pool = NULL;
+	}
+
+	if (ehci->itd_pool)
+		dma_pool_destroy(ehci->itd_pool);
+	ehci->itd_pool = NULL;
+
+	if (ehci->sitd_pool)
+		dma_pool_destroy(ehci->sitd_pool);
+	ehci->sitd_pool = NULL;
+
+	if (ehci->periodic)
+		dma_free_coherent(ehci_to_hcd(ehci)->self.controller,
+				  ehci->periodic_size * sizeof(u32),
+				  ehci->periodic, ehci->periodic_dma);
+	ehci->periodic = NULL;
+
+	if (ehci->iram_buffer[0])
+		free_iram_buf(ehci->iram_buffer[0]);
+	if (ehci->iram_buffer[1])
+		free_iram_buf(ehci->iram_buffer[1]);
+
+	/* shadow periodic table */
+	kfree(ehci->pshadow);
+	ehci->pshadow = NULL;
+	usb_pool_deinit();
+}
+
+/* remember to add cleanup code (above) if you add anything here */
+static int ehci_mem_init(struct ehci_hcd *ehci, gfp_t flags)
+{
+	int i;
+	g_usb_pool_count = 0;
+	g_debug_qtd_allocated = 0;
+	g_debug_qH_allocated = 0;
+	g_alloc_map = 0;
+
+	if (cpu_is_mx37())
+		use_iram_qtd = 0;
+	else
+		use_iram_qtd = 1;
+
+	usb_pool_initialize(USB_IRAM_BASE_ADDR + IRAM_TD_SIZE * IRAM_NTD * 2,
+			    USB_IRAM_SIZE - IRAM_TD_SIZE * IRAM_NTD * 2, 32);
+
+	if (!ehci->iram_buffer[0]) {
+		ehci->iram_buffer[0] = alloc_iram_buf();
+		ehci->iram_buffer_v[0] = IO_ADDRESS(ehci->iram_buffer[0]);
+		ehci->iram_buffer[1] = alloc_iram_buf();
+		ehci->iram_buffer_v[1] = IO_ADDRESS(ehci->iram_buffer[1]);
+	}
+
+	/* QTDs for control/bulk/intr transfers */
+	ehci->qtd_pool = dma_pool_create("ehci_qtd",
+					ehci_to_hcd(ehci)->self.controller,
+					sizeof(struct ehci_qtd),
+					32/* byte alignment (for hw parts) */
+					 , 4096 /* can't cross 4K */);
+	if (!ehci->qtd_pool)
+		goto fail;
+
+	/* QHs for control/bulk/intr transfers */
+	ehci->qh_pool = dma_pool_create("ehci_qh",
+					ehci_to_hcd(ehci)->self.controller,
+					sizeof(struct ehci_qh),
+					32 /* byte alignment (for hw parts) */ ,
+					4096 /* can't cross 4K */);
+	if (!ehci->qh_pool)
+		goto fail;
+
+	ehci->async = ehci_qh_alloc(ehci, flags);
+	if (!ehci->async)
+		goto fail;
+
+	/* ITD for high speed ISO transfers */
+	ehci->itd_pool = dma_pool_create("ehci_itd",
+					ehci_to_hcd(ehci)->self.controller,
+					sizeof(struct ehci_itd),
+					32/* byte alignment (for hw parts) */
+					 , 4096 /* can't cross 4K */);
+	if (!ehci->itd_pool)
+		goto fail;
+
+	/* SITD for full/low speed split ISO transfers */
+	ehci->sitd_pool = dma_pool_create("ehci_sitd",
+					ehci_to_hcd(ehci)->self.controller,
+					sizeof(struct ehci_sitd),
+					32/* byte alignment (for hw parts) */
+					  , 4096 /* can't cross 4K */);
+	if (!ehci->sitd_pool)
+		goto fail;
+
+	ehci->periodic = (__le32 *)
+	    dma_alloc_coherent(ehci_to_hcd(ehci)->self.controller,
+			       ehci->periodic_size * sizeof(__le32),
+			       &ehci->periodic_dma, 0);
+
+	if (ehci->periodic == NULL)
+		goto fail;
+
+	for (i = 0; i < ehci->periodic_size; i++)
+		ehci->periodic[i] = EHCI_LIST_END(ehci);
+
+	/* software shadow of hardware table */
+	ehci->pshadow = kcalloc(ehci->periodic_size, sizeof(void *), flags);
+	if (ehci->pshadow != NULL)
+		return 0;
+
+fail:
+	ehci_dbg(ehci, "couldn't init memory\n");
+	ehci_mem_cleanup(ehci);
+	return -ENOMEM;
+}
diff --git a/drivers/usb/host/ehci-q-iram.c b/drivers/usb/host/ehci-q-iram.c
new file mode 100644
index 000000000000..318888563380
--- /dev/null
+++ b/drivers/usb/host/ehci-q-iram.c
@@ -0,0 +1,1345 @@
+/*
+ * Copyright (C) 2001-2004 by David Brownell
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#undef EHCI_NO_ERR_COUNT
+static size_t g_iram_size = IRAM_TD_SIZE;
+
+/* this file is part of ehci-hcd.c */
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * EHCI hardware queue manipulation ... the core.  QH/QTD manipulation.
+ *
+ * Control, bulk, and interrupt traffic all use "qh" lists.  They list "qtd"
+ * entries describing USB transactions, max 16-20kB/entry (with 4kB-aligned
+ * buffers needed for the larger number).  We use one QH per endpoint, queue
+ * multiple urbs (all three types) per endpoint.  URBs may need several qtds.
+ *
+ * ISO traffic uses "ISO TD" (itd, and sitd) records, and (along with
+ * interrupts) needs careful scheduling.  Performance improvements can be
+ * an ongoing challenge.  That's in "ehci-sched.c".
+ *
+ * USB 1.1 devices are handled (a) by "companion" OHCI or UHCI root hubs,
+ * or otherwise through transaction translators (TTs) in USB 2.0 hubs using
+ * (b) special fields in qh entries or (c) split iso entries.  TTs will
+ * buffer low/full speed data so the host collects it at high speed.
+ */
+
+/*-------------------------------------------------------------------------*/
+/* fill a qtd, returning how much of the buffer we were able to queue up */
+static int qtd_fill(struct ehci_hcd *ehci, struct ehci_qtd *qtd, dma_addr_t buf,
+		    size_t len, int token, int maxpacket)
+{
+	int i, count;
+	u64 addr = buf;
+	struct urb *urb = qtd->urb;
+
+	if (usb_pipebulk(urb->pipe) &&
+	    (address_to_buffer(ehci, usb_pipedevice(urb->pipe)) != 2)) {
+		urb->use_iram = 1;
+		qtd->buffer_offset = (size_t) (buf - urb->transfer_dma);
+		token |= QTD_IOC;
+		if (usb_pipeout(urb->pipe)) {
+			addr = ehci->iram_buffer[address_to_buffer(ehci,
+						usb_pipedevice(urb->pipe))];
+		} else if (usb_pipein(urb->pipe)) {
+			addr = ehci->iram_buffer[address_to_buffer(ehci,
+						usb_pipedevice(urb->pipe))] +
+						g_iram_size;
+		}
+	} else {
+		urb->use_iram = 0;
+		addr = buf;
+	}
+	len = min(g_iram_size, len);
+
+	/* one buffer entry per 4K ... first might be short or unaligned */
+	qtd->hw_buf[0] = cpu_to_hc32(ehci, (u32) addr);
+	qtd->hw_buf_hi[0] = cpu_to_hc32(ehci, (u32) (addr >> 32));
+	count = 0x1000 - (buf & 0x0fff);	/* rest of that page */
+	if (likely(len < count))	/* ... iff needed */
+		count = len;
+	else {
+		buf += 0x1000;
+		buf &= ~0x0fff;
+
+		/* per-qtd limit: from 16K to 20K (best alignment) */
+		for (i = 1; count < len && i < 5; i++) {
+			addr = buf;
+			qtd->hw_buf[i] = cpu_to_hc32(ehci, (u32) addr);
+			qtd->hw_buf_hi[i] =
+			    cpu_to_hc32(ehci, (u32) (addr >> 32));
+			buf += 0x1000;
+			if ((count + 0x1000) < len)
+				count += 0x1000;
+			else
+				count = len;
+		}
+
+		/* short packets may only terminate transfers */
+		if (count != len)
+			count -= (count % maxpacket);
+	}
+	qtd->hw_token = cpu_to_hc32(ehci, (count << 16) | token);
+	qtd->length = count;
+
+	return count;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static inline void
+qh_update(struct ehci_hcd *ehci, struct ehci_qh *qh, struct ehci_qtd *qtd)
+{
+	/* writes to an active overlay are unsafe */
+	BUG_ON(qh->qh_state != QH_STATE_IDLE);
+
+	qh->hw_qtd_next = QTD_NEXT(ehci, qtd->qtd_dma);
+	qh->hw_alt_next = EHCI_LIST_END(ehci);
+
+	/* Except for control endpoints, we make hardware maintain data
+	 * toggle (like OHCI) ... here (re)initialize the toggle in the QH,
+	 * and set the pseudo-toggle in udev. Only usb_clear_halt() will
+	 * ever clear it.
+	 */
+	if (!(qh->hw_info1 & cpu_to_hc32(ehci, 1 << 14))) {
+		unsigned is_out, epnum;
+
+		is_out = !(qtd->hw_token & cpu_to_hc32(ehci, 1 << 8));
+		epnum = (hc32_to_cpup(ehci, &qh->hw_info1) >> 8) & 0x0f;
+		if (unlikely(!usb_gettoggle(qh->dev, epnum, is_out))) {
+			qh->hw_token &= ~cpu_to_hc32(ehci, QTD_TOGGLE);
+			usb_settoggle(qh->dev, epnum, is_out, 1);
+		}
+	}
+
+	/* HC must see latest qtd and qh data before we clear ACTIVE+HALT */
+	wmb();
+	qh->hw_token &= cpu_to_hc32(ehci, QTD_TOGGLE | QTD_STS_PING);
+}
+
+/* if it weren't for a common silicon quirk (writing the dummy into the qh
+ * overlay, so qh->hw_token wrongly becomes inactive/halted), only fault
+ * recovery (including urb dequeue) would need software changes to a QH...
+ */
+static void qh_refresh(struct ehci_hcd *ehci, struct ehci_qh *qh)
+{
+	struct ehci_qtd *qtd;
+
+	if (list_empty(&qh->qtd_list))
+		qtd = qh->dummy;
+	else {
+		qtd = list_entry(qh->qtd_list.next, struct ehci_qtd, qtd_list);
+		/* first qtd may already be partially processed */
+		if (cpu_to_hc32(ehci, qtd->qtd_dma) == qh->hw_current)
+			qtd = NULL;
+	}
+
+	if (qtd)
+		qh_update(ehci, qh, qtd);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int qtd_copy_status(struct ehci_hcd *ehci,
+			   struct urb *urb, size_t length, u32 token)
+{
+	int status = -EINPROGRESS;
+
+	/* count IN/OUT bytes, not SETUP (even short packets) */
+	if (likely(QTD_PID(token) != 2))
+		urb->actual_length += length - QTD_LENGTH(token);
+
+	/* don't modify error codes */
+	if (unlikely(urb->unlinked))
+		return status;
+
+	/* force cleanup after short read; not always an error */
+	if (unlikely(IS_SHORT_READ(token)))
+		status = -EREMOTEIO;
+
+	/* serious "can't proceed" faults reported by the hardware */
+	if (token & QTD_STS_HALT) {
+		if (token & QTD_STS_BABBLE) {
+			/* FIXME "must" disable babbling device's port too */
+			status = -EOVERFLOW;
+		} else if (token & QTD_STS_MMF) {
+			/* fs/ls interrupt xfer missed the complete-split */
+			status = -EPROTO;
+		} else if (token & QTD_STS_DBE) {
+			status = (QTD_PID(token) == 1)	/* IN ? */
+			    ? -ENOSR	/* hc couldn't read data */
+			    : -ECOMM;	/* hc couldn't write data */
+		} else if (token & QTD_STS_XACT) {
+			/* timeout, bad crc, wrong PID, etc; retried */
+			if (QTD_CERR(token))
+				status = -EPIPE;
+			else {
+				ehci_dbg(ehci, "devpath %s ep%d%s 3strikes\n",
+					 urb->dev->devpath,
+					 usb_pipeendpoint(urb->pipe),
+					 usb_pipein(urb->pipe) ? "in" : "out");
+				status = -EPROTO;
+			}
+			/* CERR nonzero + no errors + halt --> stall */
+		} else if (QTD_CERR(token))
+			status = -EPIPE;
+		else		/* unknown */
+			status = -EPROTO;
+
+		ehci_vdbg(ehci,
+			  "dev%d ep%d%s qtd token %08x --> status %d\n",
+			  usb_pipedevice(urb->pipe),
+			  usb_pipeendpoint(urb->pipe),
+			  usb_pipein(urb->pipe) ? "in" : "out", token, status);
+
+		/* if async CSPLIT failed, try cleaning out the TT buffer */
+		if (status != -EPIPE && urb->dev->tt && !usb_pipeint(urb->pipe)
+		    && ((token & QTD_STS_MMF) != 0 || QTD_CERR(token) == 0)
+		    && (!ehci_is_TDI(ehci)
+			|| urb->dev->tt->hub !=
+			ehci_to_hcd(ehci)->self.root_hub)) {
+#ifdef DEBUG
+			struct usb_device *tt = urb->dev->tt->hub;
+			dev_dbg(&tt->dev,
+				"clear tt buffer port %d, a%d ep%d t%08x\n",
+				urb->dev->ttport, urb->dev->devnum,
+				usb_pipeendpoint(urb->pipe), token);
+#endif /* DEBUG */
+			/* REVISIT ARC-derived cores don't clear the root
+			 * hub TT buffer in this way...
+			 */
+			usb_hub_tt_clear_buffer(urb->dev, urb->pipe);
+		}
+	}
+
+	return status;
+}
+
+static void
+ehci_urb_done(struct ehci_hcd *ehci, struct urb *urb, int status)
+__releases(ehci->lock) __acquires(ehci->lock)
+{
+	if (likely(urb->hcpriv != NULL)) {
+		struct ehci_qh *qh = (struct ehci_qh *)urb->hcpriv;
+
+		/* S-mask in a QH means it's an interrupt urb */
+		if ((qh->hw_info2 & cpu_to_hc32(ehci, QH_SMASK)) != 0) {
+
+			/* ... update hc-wide periodic stats (for usbfs) */
+			ehci_to_hcd(ehci)->self.bandwidth_int_reqs--;
+		}
+		qh_put(qh);
+	}
+
+	if (unlikely(urb->unlinked)) {
+		COUNT(ehci->stats.unlink);
+	} else {
+		/* report non-error and short read status as zero */
+		if (status == -EINPROGRESS || status == -EREMOTEIO)
+			status = 0;
+		COUNT(ehci->stats.complete);
+	}
+
+#ifdef EHCI_URB_TRACE
+	ehci_dbg(ehci,
+		 "%s %s urb %p ep%d%s status %d len %d/%d\n",
+		 __func__, urb->dev->devpath, urb,
+		 usb_pipeendpoint(urb->pipe),
+		 usb_pipein(urb->pipe) ? "in" : "out",
+		 status, urb->actual_length, urb->transfer_buffer_length);
+#endif
+
+	/* complete() can reenter this HCD */
+	usb_hcd_unlink_urb_from_ep(ehci_to_hcd(ehci), urb);
+	spin_unlock(&ehci->lock);
+	usb_hcd_giveback_urb(ehci_to_hcd(ehci), urb, status);
+	spin_lock(&ehci->lock);
+}
+
+static void start_unlink_async(struct ehci_hcd *ehci, struct ehci_qh *qh);
+static void unlink_async(struct ehci_hcd *ehci, struct ehci_qh *qh);
+
+static void intr_deschedule(struct ehci_hcd *ehci, struct ehci_qh *qh);
+static int qh_schedule(struct ehci_hcd *ehci, struct ehci_qh *qh);
+
+/*
+ * Process and free completed qtds for a qh, returning URBs to drivers.
+ * Chases up to qh->hw_current.  Returns number of completions called,
+ * indicating how much "real" work we did.
+ */
+static unsigned qh_completions(struct ehci_hcd *ehci, struct ehci_qh *qh)
+{
+	struct ehci_qtd *last = NULL, *end = qh->dummy;
+	struct list_head *entry, *tmp;
+	int last_status = -EINPROGRESS;
+	int stopped;
+	unsigned count = 0;
+	u8 state;
+	__le32 halt = HALT_BIT(ehci);
+	__hc32 temp_hw_qtd_next = 0;
+
+	if (unlikely(list_empty(&qh->qtd_list)))
+		return count;
+
+	/* completions (or tasks on other cpus) must never clobber HALT
+	 * till we've gone through and cleaned everything up, even when
+	 * they add urbs to this qh's queue or mark them for unlinking.
+	 *
+	 * NOTE:  unlinking expects to be done in queue order.
+	 */
+	state = qh->qh_state;
+	qh->qh_state = QH_STATE_COMPLETING;
+	stopped = (state == QH_STATE_IDLE);
+
+	/* remove de-activated QTDs from front of queue.
+	 * after faults (including short reads), cleanup this urb
+	 * then let the queue advance.
+	 * if queue is stopped, handles unlinks.
+	 */
+	list_for_each_safe(entry, tmp, &qh->qtd_list) {
+		struct ehci_qtd *qtd;
+		struct urb *urb;
+		struct ehci_qtd *qtd2;
+		struct urb *urb2;
+
+		u32 token = 0;
+
+		qtd = list_entry(entry, struct ehci_qtd, qtd_list);
+		urb = qtd->urb;
+
+		/* clean up any state from previous QTD ... */
+		if (last) {
+			if (likely(last->urb != urb)) {
+				ehci_urb_done(ehci, last->urb, last_status);
+				count++;
+				last_status = -EINPROGRESS;
+			}
+			ehci_qtd_free(ehci, last);
+			last = NULL;
+		}
+
+		/* ignore urbs submitted during completions we reported */
+		if (qtd == end)
+			break;
+
+		/* hardware copies qtd out of qh overlay */
+		rmb();
+		token = hc32_to_cpu(ehci, qtd->hw_token);
+
+		/* always clean up qtds the hc de-activated */
+		if ((token & QTD_STS_ACTIVE) == 0) {
+
+			/* on STALL, error, and short reads this urb must
+			 * complete and all its qtds must be recycled.
+			 */
+			if ((token & QTD_STS_HALT) != 0) {
+				stopped = 1;
+
+			/* magic dummy for some short reads; qh won't advance.
+			 * that silicon quirk can kick in with this dummy too.
+			 *
+			 * other short reads won't stop the queue, including
+			 * control transfers (status stage handles that) or
+			 * most other single-qtd reads ... the queue stops if
+			 * URB_SHORT_NOT_OK was set so the driver submitting
+			 * the urbs could clean it up.
+			 */
+			} else if (IS_SHORT_READ(token)
+				&& !(qtd->hw_alt_next & EHCI_LIST_END(ehci))) {
+				if (urb->use_iram && usb_pipein(urb->pipe)) {
+					if (urb->transfer_buffer == NULL) {
+						memcpy(phys_to_virt
+						       (urb->transfer_dma) +
+						       qtd->buffer_offset,
+						       ehci->
+						       iram_buffer_v
+						       [address_to_buffer
+							(ehci,
+							 usb_pipedevice(urb->
+									pipe))]
+						       + g_iram_size,
+						       min(g_iram_size,
+							   qtd->length));
+					} else {
+						memcpy(urb->transfer_buffer +
+						       qtd->buffer_offset,
+						       ehci->
+						       iram_buffer_v
+						       [address_to_buffer
+							(ehci,
+							 usb_pipedevice(urb->
+									pipe))]
+						       + g_iram_size,
+						       min(g_iram_size,
+							   qtd->length));
+					}
+				}
+				stopped = 1;
+				goto halt;
+			} else if (urb->use_iram && (!qtd->last_one)
+				   && usb_pipeout(urb->pipe)) {
+				ehci->
+				    iram_in_use[address_to_buffer
+						(ehci,
+						 usb_pipedevice(urb->pipe))] =
+				    1;
+				qtd2 =
+				    list_entry(tmp, struct ehci_qtd, qtd_list);
+				if (urb->transfer_buffer == NULL) {
+					memcpy(ehci->
+					       iram_buffer_v[address_to_buffer
+							     (ehci,
+							      usb_pipedevice
+							      (urb->pipe))],
+					       phys_to_virt(urb->transfer_dma) +
+					       qtd->buffer_offset + qtd->length,
+					       min(g_iram_size, qtd2->length));
+				} else {
+					memcpy(ehci->
+					       iram_buffer_v[address_to_buffer
+							     (ehci,
+							      usb_pipedevice
+							      (urb->pipe))],
+					       urb->transfer_buffer +
+					       qtd->buffer_offset + qtd->length,
+					       min(g_iram_size, qtd2->length));
+				}
+				temp_hw_qtd_next =
+				    QTD_NEXT(ehci, qtd->hw_next) & 0xFFFFFFFE;
+			} else if (urb->use_iram && (qtd->last_one)
+				   && usb_pipeout(urb->pipe)) {
+				urb->use_iram = 0;
+				qtd2 =
+				    list_entry(tmp, struct ehci_qtd, qtd_list);
+				if (tmp != &qh->qtd_list) {
+					urb2 = qtd2->urb;
+					if (urb2 && urb2->use_iram == 1) {
+						ehci->
+						    iram_in_use
+						    [address_to_buffer
+						     (ehci,
+						      usb_pipedevice(urb->
+								     pipe))] =
+						    1;
+						if (urb2->transfer_buffer ==
+						    NULL) {
+							memcpy(ehci->
+							       iram_buffer_v
+							[address_to_buffer
+								(ehci,
+								 usb_pipedevice
+								 (urb->pipe))],
+							       phys_to_virt
+							       (urb2->
+								transfer_dma),
+							       min(g_iram_size,
+								   qtd2->
+								   length));
+						} else {
+							memcpy(ehci->
+							       iram_buffer_v
+							[address_to_buffer
+								(ehci,
+								 usb_pipedevice
+								 (urb->pipe))],
+							       urb2->
+							       transfer_buffer,
+							       min(g_iram_size,
+								   qtd2->
+								   length));
+						}
+					} else {
+						ehci->
+						    iram_in_use
+						    [address_to_buffer
+						     (ehci,
+						      usb_pipedevice(urb->
+								     pipe))] =
+						    0;
+					}
+				} else {
+					ehci->
+					    iram_in_use[address_to_buffer
+							(ehci,
+							 usb_pipedevice(urb->
+									pipe))]
+					    = 0;
+				}
+				temp_hw_qtd_next =
+				    QTD_NEXT(ehci, qtd->hw_next) & 0xFFFFFFFE;
+			} else if (urb->use_iram && usb_pipein(urb->pipe)) {
+				if (urb->transfer_buffer == NULL) {
+					memcpy(phys_to_virt(urb->transfer_dma) +
+					       qtd->buffer_offset,
+					       ehci->
+					       iram_buffer_v[address_to_buffer
+							     (ehci,
+							      usb_pipedevice
+							      (urb->pipe))] +
+					       g_iram_size, min(g_iram_size,
+								qtd->length));
+				} else {
+					memcpy(urb->transfer_buffer +
+					       qtd->buffer_offset,
+					       ehci->
+					       iram_buffer_v[address_to_buffer
+							     (ehci,
+							      usb_pipedevice
+							      (urb->pipe))] +
+					       g_iram_size, min(g_iram_size,
+								qtd->length));
+				}
+				temp_hw_qtd_next =
+				    QTD_NEXT(ehci, qtd->hw_next) & 0xFFFFFFFE;
+			}
+			/* stop scanning when we reach qtds the hc is using */
+		} else if (likely(!stopped
+				  && HC_IS_RUNNING(ehci_to_hcd(ehci)->state))) {
+			break;
+
+		/* scan the whole queue for unlinks whenever it stops */
+		} else {
+			stopped = 1;
+
+			/* cancel everything if we halt, suspend, etc */
+			if (!HC_IS_RUNNING(ehci_to_hcd(ehci)->state))
+				last_status = -ESHUTDOWN;
+
+			/* this qtd is active; skip it unless a previous qtd
+			 * for its urb faulted, or its urb was canceled.
+			 */
+			else if (last_status == -EINPROGRESS && !urb->unlinked)
+				continue;
+
+			/* qh unlinked; token in overlay may be most current */
+			if (state == QH_STATE_IDLE
+					&& cpu_to_hc32(ehci, qtd->qtd_dma)
+						== qh->hw_current)
+				token = hc32_to_cpu(ehci, qh->hw_token);
+
+			/* qh unlinked; token in overlay may be most current */
+			if (state == QH_STATE_IDLE
+			    && cpu_to_hc32(ehci, qtd->qtd_dma)
+			    == qh->hw_current)
+				token = hc32_to_cpu(ehci, qh->hw_token);
+
+			/* force halt for unlinked or blocked qh, so we'll
+			 * patch the qh later and so that completions can't
+			 * activate it while we "know" it's stopped.
+			 */
+			if ((halt & qh->hw_token) == 0) {
+halt:
+				qh->hw_token |= halt;
+				wmb();
+			}
+		}
+
+		/* unless we already know the urb's status, collect qtd status
+		 * and update count of bytes transferred.  in common short read
+		 * cases with only one data qtd (including control transfers),
+		 * queue processing won't halt.  but with two or more qtds (for
+		 * example, with a 32 KB transfer), when the first qtd gets a
+		 * short read the second must be removed by hand.
+		 */
+		if (last_status == -EINPROGRESS) {
+			last_status = qtd_copy_status(ehci, urb,
+					qtd->length, token);
+			if (last_status == -EREMOTEIO
+					&& (qtd->hw_alt_next
+						& EHCI_LIST_END(ehci)))
+				last_status = -EINPROGRESS;
+		}
+
+		/* if we're removing something not at the queue head,
+		 * patch the hardware queue pointer.
+		 */
+
+		if (stopped && qtd->qtd_list.prev != &qh->qtd_list) {
+			last = list_entry(qtd->qtd_list.prev,
+					  struct ehci_qtd, qtd_list);
+			last->hw_next = qtd->hw_next;
+		}
+
+/* remove qtd; it's recycled after possible urb completion */
+		list_del(&qtd->qtd_list);
+		last = qtd;
+	}
+
+	/* last urb's completion might still need calling */
+	if (likely(last != NULL)) {
+		ehci_urb_done(ehci, last->urb, last_status);
+		count++;
+		ehci_qtd_free(ehci, last);
+	}
+
+	/* restore original state; caller must unlink or relink */
+	qh->qh_state = state;
+
+	/* be sure the hardware's done with the qh before refreshing
+	 * it after fault cleanup, or recovering from silicon wrongly
+	 * overlaying the dummy qtd (which reduces DMA chatter).
+	 */
+	if ((stopped != 0) || (qh->hw_qtd_next == EHCI_LIST_END(ehci))
+	    && (temp_hw_qtd_next == 0)) {
+		switch (state) {
+		case QH_STATE_IDLE:
+			qh_refresh(ehci, qh);
+			break;
+		case QH_STATE_LINKED:
+			/* We won't refresh a QH that's linked (after the HC
+			 * stopped the queue).  That avoids a race:
+			 *  - HC reads first part of QH;
+			 *  - CPU updates that first part and the token;
+			 *  - HC reads rest of that QH, including token
+			 * Result:  HC gets an inconsistent image, and then
+			 * DMAs to/from the wrong memory (corrupting it).
+			 *
+			 * That should be rare for interrupt transfers,
+			 * except maybe high bandwidth ...
+			 */
+			if ((cpu_to_hc32(ehci, QH_SMASK)
+			     & qh->hw_info2) != 0) {
+				intr_deschedule(ehci, qh);
+				(void)qh_schedule(ehci, qh);
+			} else
+				unlink_async(ehci, qh);
+			break;
+			/* otherwise, unlink already started */
+		}
+	}
+	if (temp_hw_qtd_next)
+		qh->hw_qtd_next = temp_hw_qtd_next;
+
+	return count;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* high bandwidth multiplier, as encoded in highspeed endpoint descriptors */
+#define hb_mult(wMaxPacketSize) (1 + (((wMaxPacketSize) >> 11) & 0x03))
+/* ... and packet size, for any kind of endpoint descriptor */
+#define max_packet(wMaxPacketSize) ((wMaxPacketSize) & 0x07ff)
+
+/*
+ * reverse of qh_urb_transaction:  free a list of TDs.
+ * used for cleanup after errors, before HC sees an URB's TDs.
+ */
+static void qtd_list_free(struct ehci_hcd *ehci,
+			  struct urb *urb, struct list_head *qtd_list)
+{
+	struct list_head *entry, *temp;
+
+	list_for_each_safe(entry, temp, qtd_list) {
+		struct ehci_qtd *qtd;
+
+		qtd = list_entry(entry, struct ehci_qtd, qtd_list);
+		list_del(&qtd->qtd_list);
+		ehci_qtd_free(ehci, qtd);
+	}
+}
+
+/*
+ * create a list of filled qtds for this URB; won't link into qh.
+ */
+static struct list_head *qh_urb_transaction(struct ehci_hcd *ehci,
+					    struct urb *urb,
+					    struct list_head *head, gfp_t flags)
+{
+	struct ehci_qtd *qtd, *qtd_prev;
+	dma_addr_t buf;
+	int len, maxpacket;
+	int is_input;
+	u32 token;
+
+	/*
+	 * URBs map to sequences of QTDs:  one logical transaction
+	 */
+	qtd = ehci_qtd_alloc(ehci, flags);
+	if (unlikely(!qtd))
+		return NULL;
+	list_add_tail(&qtd->qtd_list, head);
+	qtd->urb = urb;
+
+	token = QTD_STS_ACTIVE;
+	token |= (EHCI_TUNE_CERR << 10);
+	/* for split transactions, SplitXState initialized to zero */
+
+	len = urb->transfer_buffer_length;
+	is_input = usb_pipein(urb->pipe);
+	if (usb_pipecontrol(urb->pipe)) {
+		/* SETUP pid */
+		qtd_fill(ehci, qtd, urb->setup_dma,
+			 sizeof(struct usb_ctrlrequest),
+			 token | (2 /* "setup" */  << 8), 8);
+
+		/* ... and always at least one more pid */
+		token ^= QTD_TOGGLE;
+		qtd_prev = qtd;
+		qtd = ehci_qtd_alloc(ehci, flags);
+		if (unlikely(!qtd))
+			goto cleanup;
+		qtd->urb = urb;
+		qtd_prev->hw_next = QTD_NEXT(ehci, qtd->qtd_dma);
+		list_add_tail(&qtd->qtd_list, head);
+
+		/* for zero length DATA stages, STATUS is always IN */
+		if (len == 0)
+			token |= (1 /* "in" */  << 8);
+	}
+
+	/*
+	 * data transfer stage:  buffer setup
+	 */
+	buf = urb->transfer_dma;
+
+	if (is_input)
+		token |= (1 /* "in" */  << 8);
+	/* else it's already initted to "out" pid (0 << 8) */
+
+	maxpacket = max_packet(usb_maxpacket(urb->dev, urb->pipe, !is_input));
+
+	/*
+	 * buffer gets wrapped in one or more qtds;
+	 * last one may be "short" (including zero len)
+	 * and may serve as a control status ack
+	 */
+	for (;;) {
+		int this_qtd_len;
+		this_qtd_len = qtd_fill(ehci, qtd, buf, len, token, maxpacket);
+		if (urb->use_iram && (!qtd->buffer_offset)
+		    && usb_pipeout(urb->pipe)
+		    && (ehci->
+			iram_in_use[address_to_buffer
+				    (ehci, usb_pipedevice(urb->pipe))] == 0)) {
+			ehci->
+			    iram_in_use[address_to_buffer
+					(ehci, usb_pipedevice(urb->pipe))] = 1;
+			if (urb->transfer_buffer == NULL) {
+				memcpy(ehci->
+				       iram_buffer_v[address_to_buffer
+						     (ehci,
+						      usb_pipedevice(urb->
+								     pipe))],
+				       phys_to_virt(urb->transfer_dma),
+				       min((int)g_iram_size, len));
+			} else {
+				memcpy(ehci->
+				       iram_buffer_v[address_to_buffer
+						     (ehci,
+						      usb_pipedevice(urb->
+								     pipe))],
+				       urb->transfer_buffer,
+				       min((int)g_iram_size, len));
+			}
+		}
+		len -= this_qtd_len;
+		buf += this_qtd_len;
+
+		/*
+		 * short reads advance to a "magic" dummy instead of the next
+		 * qtd ... that forces the queue to stop, for manual cleanup.
+		 * (this will usually be overridden later.)
+		 */
+		if (is_input)
+			qtd->hw_alt_next = ehci->async->hw_alt_next;
+
+		/* qh makes control packets use qtd toggle; maybe switch it */
+		if ((maxpacket & (this_qtd_len + (maxpacket - 1))) == 0)
+			token ^= QTD_TOGGLE;
+
+		if (likely(len <= 0)) {
+			qtd->last_one = 1;
+			break;
+		}
+		qtd_prev = qtd;
+		qtd = ehci_qtd_alloc(ehci, flags);
+		if (unlikely(!qtd))
+			goto cleanup;
+		qtd->urb = urb;
+		if (urb->use_iram)
+			qtd_prev->hw_next = QTD_NEXT(ehci, qtd->qtd_dma) | 0x1;
+		else
+			qtd_prev->hw_next = QTD_NEXT(ehci, qtd->qtd_dma);
+
+		list_add_tail(&qtd->qtd_list, head);
+	}
+
+	/*
+	 * unless the caller requires manual cleanup after short reads,
+	 * have the alt_next mechanism keep the queue running after the
+	 * last data qtd (the only one, for control and most other cases).
+	 */
+	if (likely((urb->transfer_flags & URB_SHORT_NOT_OK) == 0
+		   || usb_pipecontrol(urb->pipe)))
+		qtd->hw_alt_next = EHCI_LIST_END(ehci);
+
+	/*
+	 * control requests may need a terminating data "status" ack;
+	 * bulk ones may need a terminating short packet (zero length).
+	 */
+	if (likely(urb->transfer_buffer_length != 0)) {
+		int one_more = 0;
+
+		if (usb_pipecontrol(urb->pipe)) {
+			one_more = 1;
+			token ^= 0x0100;	/* "in" <--> "out"  */
+			token |= QTD_TOGGLE;	/* force DATA1 */
+		} else if (usb_pipebulk(urb->pipe)
+			   && (urb->transfer_flags & URB_ZERO_PACKET)
+			   && !(urb->transfer_buffer_length % maxpacket))
+			one_more = 1;
+		if (one_more) {
+			qtd_prev = qtd;
+			qtd = ehci_qtd_alloc(ehci, flags);
+			if (unlikely(!qtd))
+				goto cleanup;
+			qtd->urb = urb;
+			qtd_prev->hw_next = QTD_NEXT(ehci, qtd->qtd_dma);
+			list_add_tail(&qtd->qtd_list, head);
+
+			/* never any data in such packets */
+			qtd_fill(ehci, qtd, 0, 0, token, 0);
+		}
+	}
+
+	/* by default, enable interrupt on urb completion */
+	if (likely(!(urb->transfer_flags & URB_NO_INTERRUPT)))
+		qtd->hw_token |= cpu_to_hc32(ehci, QTD_IOC);
+	return head;
+
+cleanup:
+	qtd_list_free(ehci, urb, head);
+	return NULL;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* Would be best to create all qh's from config descriptors,
+ * when each interface/altsetting is established.  Unlink
+ * any previous qh and cancel its urbs first; endpoints are
+ * implicitly reset then (data toggle too).
+ * That'd mean updating how usbcore talks to HCDs. (2.7?)
+ */
+
+/*
+ * Each QH holds a qtd list; a QH is used for everything except iso.
+ *
+ * For interrupt urbs, the scheduler must set the microframe scheduling
+ * mask(s) each time the QH gets scheduled.  For highspeed, that's
+ * just one microframe in the s-mask.  For split interrupt transactions
+ * there are additional complications: c-mask, maybe FSTNs.
+ */
+static struct ehci_qh *qh_make(struct ehci_hcd *ehci,
+			       struct urb *urb, gfp_t flags)
+{
+	struct ehci_qh *qh = ehci_qh_alloc(ehci, flags);
+	u32 info1 = 0, info2 = 0;
+	int is_input, type;
+	int maxp = 0;
+	struct usb_tt		*tt = urb->dev->tt;
+
+	if (!qh)
+		return qh;
+
+	/*
+	 * init endpoint/device data for this QH
+	 */
+	info1 |= usb_pipeendpoint(urb->pipe) << 8;
+	info1 |= usb_pipedevice(urb->pipe) << 0;
+
+	is_input = usb_pipein(urb->pipe);
+	type = usb_pipetype(urb->pipe);
+	maxp = usb_maxpacket(urb->dev, urb->pipe, !is_input);
+
+	/* 1024 byte maxpacket is a hardware ceiling.  High bandwidth
+	 * acts like up to 3KB, but is built from smaller packets.
+	 */
+	if (max_packet(maxp) > 1024) {
+		ehci_dbg(ehci, "bogus qh maxpacket %d\n", max_packet(maxp));
+		goto done;
+	}
+
+	/* Compute interrupt scheduling parameters just once, and save.
+	 * - allowing for high bandwidth, how many nsec/uframe are used?
+	 * - split transactions need a second CSPLIT uframe; same question
+	 * - splits also need a schedule gap (for full/low speed I/O)
+	 * - qh has a polling interval
+	 *
+	 * For control/bulk requests, the HC or TT handles these.
+	 */
+	if (type == PIPE_INTERRUPT) {
+		qh->usecs =
+		    NS_TO_US(usb_calc_bus_time
+			     (USB_SPEED_HIGH, is_input, 0,
+			      hb_mult(maxp) * max_packet(maxp)));
+		qh->start = NO_FRAME;
+
+		if (urb->dev->speed == USB_SPEED_HIGH) {
+			qh->c_usecs = 0;
+			qh->gap_uf = 0;
+
+			qh->period = urb->interval >> 3;
+			if (qh->period == 0 && urb->interval != 1) {
+				/* NOTE interval 2 or 4 uframes could work.
+				 * But interval 1 scheduling is simpler, and
+				 * includes high bandwidth.
+				 */
+				dbg("intr period %d uframes, NYET!",
+				    urb->interval);
+				goto done;
+			}
+		} else {
+			int think_time;
+
+			/* gap is f(FS/LS transfer times) */
+			qh->gap_uf = 1 + usb_calc_bus_time(urb->dev->speed,
+							   is_input, 0,
+							   maxp) / (125 * 1000);
+
+			/* FIXME this just approximates SPLIT/CSPLIT times */
+			if (is_input) {
+				qh->c_usecs = qh->usecs + HS_USECS(0);
+				qh->usecs = HS_USECS(1);
+			} else {
+				qh->usecs += HS_USECS(1);
+				qh->c_usecs = HS_USECS(0);
+			}
+
+			think_time = tt ? tt->think_time : 0;
+			qh->tt_usecs = NS_TO_US(think_time +
+						usb_calc_bus_time(urb->dev->
+								  speed,
+								  is_input, 0,
+								  max_packet
+								  (maxp)));
+			qh->period = urb->interval;
+		}
+	}
+
+	/* support for tt scheduling, and access to toggles */
+	qh->dev = urb->dev;
+
+	/* using TT? */
+	switch (urb->dev->speed) {
+	case USB_SPEED_LOW:
+		info1 |= (1 << 12);	/* EPS "low" */
+		/* FALL THROUGH */
+
+	case USB_SPEED_FULL:
+		/* EPS 0 means "full" */
+		if (type != PIPE_INTERRUPT)
+			info1 |= (EHCI_TUNE_RL_TT << 28);
+		if (type == PIPE_CONTROL) {
+			info1 |= (1 << 27);	/* for TT */
+			info1 |= 1 << 14;	/* toggle from qtd */
+		}
+		info1 |= maxp << 16;
+
+		info2 |= (EHCI_TUNE_MULT_TT << 30);
+
+		/* Some Freescale processors have an erratum in which the
+		 * port number in the queue head was 0..N-1 instead of 1..N.
+		 */
+		if (ehci_has_fsl_portno_bug(ehci))
+			info2 |= (urb->dev->ttport - 1) << 23;
+		else
+			info2 |= urb->dev->ttport << 23;
+
+		/* set the address of the TT; for TDI's integrated
+		 * root hub tt, leave it zeroed.
+		 */
+		if (tt && tt->hub != ehci_to_hcd(ehci)->self.root_hub)
+			info2 |= tt->hub->devnum << 16;
+
+		/* NOTE:  if (PIPE_INTERRUPT) { scheduler sets c-mask } */
+
+		break;
+
+	case USB_SPEED_HIGH:	/* no TT involved */
+		info1 |= (2 << 12);	/* EPS "high" */
+		if (type == PIPE_CONTROL) {
+			info1 |= (EHCI_TUNE_RL_HS << 28);
+			info1 |= 64 << 16;	/* usb2 fixed maxpacket */
+			info1 |= 1 << 14;	/* toggle from qtd */
+			info2 |= (EHCI_TUNE_MULT_HS << 30);
+		} else if (type == PIPE_BULK) {
+			info1 |= (EHCI_TUNE_RL_HS << 28);
+			/* The USB spec says that high speed bulk endpoints
+			 * always use 512 byte maxpacket.  But some device
+			 * vendors decided to ignore that, and MSFT is happy
+			 * to help them do so.  So now people expect to use
+			 * such nonconformant devices with Linux too; sigh.
+			 */
+			info1 |= max_packet(maxp) << 16;
+			info2 |= (EHCI_TUNE_MULT_HS << 30);
+			use_buffer(ehci, usb_pipedevice(urb->pipe));
+		} else {	/* PIPE_INTERRUPT */
+			info1 |= max_packet(maxp) << 16;
+			info2 |= hb_mult(maxp) << 30;
+		}
+		break;
+	default:
+		dbg("bogus dev %p speed %d", urb->dev, urb->dev->speed);
+done:
+		qh_put(qh);
+		return NULL;
+	}
+
+	/* NOTE:  if (PIPE_INTERRUPT) { scheduler sets s-mask } */
+
+	/* init as live, toggle clear, advance to dummy */
+	qh->qh_state = QH_STATE_IDLE;
+	qh->hw_info1 = cpu_to_hc32(ehci, info1);
+	qh->hw_info2 = cpu_to_hc32(ehci, info2);
+	usb_settoggle(urb->dev, usb_pipeendpoint(urb->pipe), !is_input, 1);
+	qh_refresh(ehci, qh);
+	return qh;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* move qh (and its qtds) onto async queue; maybe enable queue.  */
+
+static void qh_link_async(struct ehci_hcd *ehci, struct ehci_qh *qh)
+{
+	__hc32 dma = QH_NEXT(ehci, qh->qh_dma);
+	struct ehci_qh *head;
+
+	/* (re)start the async schedule? */
+	head = ehci->async;
+	timer_action_done(ehci, TIMER_ASYNC_OFF);
+	if (!head->qh_next.qh) {
+		u32 cmd = ehci_readl(ehci, &ehci->regs->command);
+
+		if (!(cmd & CMD_ASE)) {
+			/* in case a clear of CMD_ASE didn't take yet */
+			(void)handshake(ehci, &ehci->regs->status,
+					STS_ASS, 0, 150);
+			cmd |= CMD_ASE | CMD_RUN;
+			ehci_writel(ehci, cmd, &ehci->regs->command);
+			ehci_to_hcd(ehci)->state = HC_STATE_RUNNING;
+			/* posted write need not be known to HC yet ... */
+		}
+	}
+
+	/* clear halt and/or toggle; and maybe recover from silicon quirk */
+	if (qh->qh_state == QH_STATE_IDLE)
+		qh_refresh(ehci, qh);
+
+	/* splice right after start */
+	qh->qh_next = head->qh_next;
+	qh->hw_next = head->hw_next;
+	wmb();
+
+	head->qh_next.qh = qh;
+	head->hw_next = dma;
+
+	qh->qh_state = QH_STATE_LINKED;
+	/* qtd completions reported later by interrupt */
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * For control/bulk/interrupt, return QH with these TDs appended.
+ * Allocates and initializes the QH if necessary.
+ * Returns null if it can't allocate a QH it needs to.
+ * If the QH has TDs (urbs) already, that's great.
+ */
+static struct ehci_qh *qh_append_tds(struct ehci_hcd *ehci,
+				     struct urb *urb,
+				     struct list_head *qtd_list,
+				     int epnum, void **ptr)
+{
+	struct ehci_qh *qh = NULL;
+	__hc32 qh_addr_mask = cpu_to_hc32(ehci, 0x7f);
+
+	qh = (struct ehci_qh *)*ptr;
+	if (unlikely(qh == NULL)) {
+		/* can't sleep here, we have ehci->lock... */
+		qh = qh_make(ehci, urb, GFP_ATOMIC);
+		*ptr = qh;
+	}
+	if (likely(qh != NULL)) {
+		struct ehci_qtd *qtd;
+
+		if (unlikely(list_empty(qtd_list)))
+			qtd = NULL;
+		else
+			qtd = list_entry(qtd_list->next, struct ehci_qtd,
+					 qtd_list);
+
+		/* control qh may need patching ... */
+		if (unlikely(epnum == 0)) {
+
+			/* usb_reset_device() briefly reverts to address 0 */
+			if (usb_pipedevice(urb->pipe) == 0)
+				qh->hw_info1 &= ~qh_addr_mask;
+		}
+
+		/* just one way to queue requests: swap with the dummy qtd.
+		 * only hc or qh_refresh() ever modify the overlay.
+		 */
+		if (likely(qtd != NULL)) {
+			struct ehci_qtd *dummy;
+			dma_addr_t dma;
+			__hc32 token;
+
+			/* to avoid racing the HC, use the dummy td instead of
+			 * the first td of our list (becomes new dummy).  both
+			 * tds stay deactivated until we're done, when the
+			 * HC is allowed to fetch the old dummy (4.10.2).
+			 */
+			token = qtd->hw_token;
+			qtd->hw_token = HALT_BIT(ehci);
+			wmb();
+			dummy = qh->dummy;
+
+			dma = dummy->qtd_dma;
+			*dummy = *qtd;
+			dummy->qtd_dma = dma;
+
+			list_del(&qtd->qtd_list);
+			list_add(&dummy->qtd_list, qtd_list);
+			__list_splice(qtd_list, qh->qtd_list.prev);
+
+			ehci_qtd_init(ehci, qtd, qtd->qtd_dma);
+			qh->dummy = qtd;
+
+			/* hc must see the new dummy at list end */
+			dma = qtd->qtd_dma;
+			qtd = list_entry(qh->qtd_list.prev,
+					 struct ehci_qtd, qtd_list);
+			if (urb->use_iram)
+				qtd->hw_next = QTD_NEXT(ehci, dma) | 0x1;
+			else
+				qtd->hw_next = QTD_NEXT(ehci, dma);
+
+			/* let the hc process these next qtds */
+			wmb();
+			dummy->hw_token = token;
+
+			urb->hcpriv = qh_get(qh);
+		}
+	}
+	return qh;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int
+submit_async(struct ehci_hcd *ehci,
+	     struct urb *urb, struct list_head *qtd_list, gfp_t mem_flags)
+{
+	struct ehci_qtd *qtd;
+	int epnum;
+	unsigned long flags;
+	struct ehci_qh *qh = NULL;
+	int rc;
+
+	qtd = list_entry(qtd_list->next, struct ehci_qtd, qtd_list);
+	epnum = urb->ep->desc.bEndpointAddress;
+
+#ifdef EHCI_URB_TRACE
+	ehci_dbg(ehci,
+		 "%s %s urb %p ep%d%s len %d, qtd %p [qh %p]\n",
+		 __func__, urb->dev->devpath, urb,
+		 epnum & 0x0f, (epnum & USB_DIR_IN) ? "in" : "out",
+		 urb->transfer_buffer_length, qtd, urb->ep->hcpriv);
+#endif
+
+	spin_lock_irqsave(&ehci->lock, flags);
+	if (unlikely(!test_bit(HCD_FLAG_HW_ACCESSIBLE,
+			       &ehci_to_hcd(ehci)->flags))) {
+		rc = -ESHUTDOWN;
+		goto done;
+	}
+	rc = usb_hcd_link_urb_to_ep(ehci_to_hcd(ehci), urb);
+	if (unlikely(rc))
+		goto done;
+
+	qh = qh_append_tds(ehci, urb, qtd_list, epnum, &urb->ep->hcpriv);
+	if (unlikely(qh == NULL)) {
+		usb_hcd_unlink_urb_from_ep(ehci_to_hcd(ehci), urb);
+		rc = -ENOMEM;
+		goto done;
+	}
+
+	/* Control/bulk operations through TTs don't need scheduling,
+	 * the HC and TT handle it when the TT has a buffer ready.
+	 */
+	if (likely(qh->qh_state == QH_STATE_IDLE))
+		qh_link_async(ehci, qh_get(qh));
+done:
+	spin_unlock_irqrestore(&ehci->lock, flags);
+	if (unlikely(qh == NULL))
+		qtd_list_free(ehci, urb, qtd_list);
+	return rc;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* the async qh for the qtds being reclaimed are now unlinked from the HC */
+
+static void end_unlink_async(struct ehci_hcd *ehci)
+{
+	struct ehci_qh *qh = ehci->reclaim;
+	struct ehci_qh *next;
+
+	iaa_watchdog_done(ehci);
+
+	qh->qh_state = QH_STATE_IDLE;
+	qh->qh_next.qh = NULL;
+	qh_put(qh);		/* refcount from reclaim */
+
+	/* other unlink(s) may be pending (in QH_STATE_UNLINK_WAIT) */
+	next = qh->reclaim;
+	ehci->reclaim = next;
+	qh->reclaim = NULL;
+
+	qh_completions(ehci, qh);
+
+	if (!list_empty(&qh->qtd_list)
+	    && HC_IS_RUNNING(ehci_to_hcd(ehci)->state))
+		qh_link_async(ehci, qh);
+	else {
+		qh_put(qh);	/* refcount from async list */
+
+		/* it's not free to turn the async schedule on/off; leave it
+		 * active but idle for a while once it empties.
+		 */
+		if (HC_IS_RUNNING(ehci_to_hcd(ehci)->state)
+		    && ehci->async->qh_next.qh == NULL)
+			timer_action(ehci, TIMER_ASYNC_OFF);
+	}
+
+	if (next) {
+		ehci->reclaim = NULL;
+		start_unlink_async(ehci, next);
+	}
+}
+
+/* makes sure the async qh will become idle */
+/* caller must own ehci->lock */
+
+static void start_unlink_async(struct ehci_hcd *ehci, struct ehci_qh *qh)
+{
+	int cmd = ehci_readl(ehci, &ehci->regs->command);
+	struct ehci_qh *prev;
+
+#ifdef DEBUG
+	assert_spin_locked(&ehci->lock);
+	if (ehci->reclaim
+	    || (qh->qh_state != QH_STATE_LINKED
+		&& qh->qh_state != QH_STATE_UNLINK_WAIT)
+	    )
+		BUG();
+#endif
+
+	/* stop async schedule right now? */
+	if (unlikely(qh == ehci->async)) {
+		/* can't get here without STS_ASS set */
+		if (ehci_to_hcd(ehci)->state != HC_STATE_HALT &&
+			!ehci->reclaim) {
+			/* ... and CMD_IAAD clear */
+			ehci_writel(ehci, cmd & ~CMD_ASE, &ehci->regs->command);
+			wmb();
+			/* handshake later, if we need to */
+			timer_action_done(ehci, TIMER_ASYNC_OFF);
+		}
+		return;
+	}
+
+	qh->qh_state = QH_STATE_UNLINK;
+	ehci->reclaim = qh = qh_get(qh);
+
+	prev = ehci->async;
+	while (prev->qh_next.qh != qh)
+		prev = prev->qh_next.qh;
+
+	prev->hw_next = qh->hw_next;
+	prev->qh_next = qh->qh_next;
+	wmb();
+
+	if (unlikely(ehci_to_hcd(ehci)->state == HC_STATE_HALT)) {
+		/* if (unlikely (qh->reclaim != 0))
+		 *  this will recurse, probably not much
+		 */
+		end_unlink_async(ehci);
+		return;
+	}
+
+	cmd |= CMD_IAAD;
+	ehci_writel(ehci, cmd, &ehci->regs->command);
+	(void)ehci_readl(ehci, &ehci->regs->command);
+	iaa_watchdog_start(ehci);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static void scan_async(struct ehci_hcd *ehci)
+{
+	struct ehci_qh *qh;
+	enum ehci_timer_action action = TIMER_IO_WATCHDOG;
+
+	if (!++(ehci->stamp))
+		ehci->stamp++;
+	timer_action_done(ehci, TIMER_ASYNC_SHRINK);
+rescan:
+	qh = ehci->async->qh_next.qh;
+	if (likely(qh != NULL)) {
+		do {
+			/* clean any finished work for this qh */
+			if (!list_empty(&qh->qtd_list)
+			    && qh->stamp != ehci->stamp) {
+				int temp;
+
+				/* unlinks could happen here; completion
+				 * reporting drops the lock.  rescan using
+				 * the latest schedule, but don't rescan
+				 * qhs we already finished (no looping).
+				 */
+				qh = qh_get(qh);
+				qh->stamp = ehci->stamp;
+				temp = qh_completions(ehci, qh);
+				qh_put(qh);
+				if (temp != 0)
+					goto rescan;
+			}
+
+			/* unlink idle entries, reducing HC PCI usage as well
+			 * as HCD schedule-scanning costs.  delay for any qh
+			 * we just scanned, there's a not-unusual case that it
+			 * doesn't stay idle for long.
+			 * (plus, avoids some kind of re-activation race.)
+			 */
+			if (list_empty(&qh->qtd_list)) {
+				if (qh->stamp == ehci->stamp)
+					action = TIMER_ASYNC_SHRINK;
+				else if (!ehci->reclaim
+					 && qh->qh_state == QH_STATE_LINKED)
+					start_unlink_async(ehci, qh);
+			}
+
+			qh = qh->qh_next.qh;
+		} while (qh);
+	}
+	if (action == TIMER_ASYNC_SHRINK)
+		timer_action(ehci, TIMER_ASYNC_SHRINK);
+}
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index c7d4b5a06bdb..5336a6ff08c9 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -123,6 +123,18 @@ struct ehci_hcd {			/* one per controller */
 
 	u8			sbrn;		/* packed release number */
 
+	/*
+	 * OTG controllers and transceivers need software interaction;
+	 * other external transceivers should be software-transparent
+	 */
+	struct otg_transceiver   *transceiver;
+#ifdef CONFIG_USB_STATIC_IRAM
+	u32			iram_buffer[2];
+	u32			iram_buffer_v[2];
+	int  			iram_in_use[2];
+	int			usb_address[2];
+#endif
+
 	/* irq statistics */
 #ifdef EHCI_STATS
 	struct ehci_stats	stats;
@@ -257,6 +269,10 @@ struct ehci_qtd {
 	struct list_head	qtd_list;		/* sw qtd list */
 	struct urb		*urb;			/* qtd's urb */
 	size_t			length;			/* length of buffer */
+#ifdef CONFIG_USB_STATIC_IRAM
+	size_t			buffer_offset;
+	int			last_one;
+#endif
 } __attribute__ ((aligned (32)));
 
 /* mask NakCnt+T in qh->hw_alt_next */
@@ -698,6 +714,10 @@ static inline u32 hc32_to_cpup (const struct ehci_hcd *ehci, const __hc32 *x)
 #define STUB_DEBUG_FILES
 #endif	/* DEBUG */
 
+#ifdef CONFIG_USB_STATIC_IRAM
+#define IRAM_TD_SIZE	1024		/* size of 1 qTD's buffer */
+#define IRAM_NTD	2		/* number of TDs in IRAM  */
+#endif
 /*-------------------------------------------------------------------------*/
 
 #endif /* __LINUX_EHCI_HCD_H */
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index 8aa3f4556a32..f2a4a785506e 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -1080,6 +1080,11 @@ MODULE_LICENSE ("GPL");
 #define TMIO_OHCI_DRIVER	ohci_hcd_tmio_driver
 #endif
 
+#ifdef CONFIG_PROCESSOR_NS9360
+#include "ohci-ns9360.c"
+#define PLATFORM_DRIVER		ohci_hcd_ns9360_driver
+#endif
+
 #if	!defined(PCI_DRIVER) &&		\
 	!defined(PLATFORM_DRIVER) &&	\
 	!defined(OF_PLATFORM_DRIVER) &&	\
diff --git a/drivers/usb/host/ohci-ns9360.c b/drivers/usb/host/ohci-ns9360.c
new file mode 100644
index 000000000000..b56e31c8738d
--- /dev/null
+++ b/drivers/usb/host/ohci-ns9360.c
@@ -0,0 +1,263 @@
+/*
+ * drivers/usb/host/ohci-ns9360.c
+ *
+ * based on drivers/usb/host/ohci-sa1111.c which has
+ * (C) Copyright 1999 Roman Weissgaerber <weissg@vienna.at>
+ * (C) Copyright 2000-2002 David Brownell <dbrownell@users.sourceforge.net>
+ * (C) Copyright 2002 Hewlett-Packard Company
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ *
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+
+#define NS9360_UHFE_IEN		0x0c
+#define NS9360_UHFE_IEN_OHCIIRQ		(1 << 1)
+#define NS9360_UHFE_ISTAT	0x10
+#define NS9360_UHFE_ISTAT_OHCIIRQ	(1 << 1)
+
+#define DRIVER_NAME "ns9360-ohci"
+
+struct ns9360_ohci_device {
+	struct ohci_hcd ohci;
+
+	struct resource *uhfe;
+	void __iomem *ioaddr;
+	struct clk *clk;
+};
+
+#define hcd2nod(up) ((struct ns9360_ohci_device *)(up->hcd_priv))
+
+extern int usb_disabled(void);
+
+static irqreturn_t usb_hcd_ns9360_hcim_irq(struct usb_hcd *hcd)
+{
+	u32 stat;
+	struct ns9360_ohci_device *priv = hcd2nod(hcd);
+
+	/* let OHCI driver handles this */
+	irqreturn_t ret = ohci_irq(hcd);
+
+	/* acknowledge interrupt in UHFE */
+	stat = ioread32(priv->ioaddr + NS9360_UHFE_ISTAT);
+	iowrite32(stat, priv->ioaddr + NS9360_UHFE_ISTAT);
+
+	return ret;
+}
+
+static int __devinit ohci_ns9360_start(struct usb_hcd *hcd)
+{
+	struct ohci_hcd	*ohci = hcd_to_ohci(hcd);
+	int ret;
+
+	ret = ohci_init(ohci);
+	if (ret < 0)
+		return ret;
+
+	ret = ohci_run(ohci);
+	if (ret < 0) {
+		ohci_err(ohci, "can't start %s", hcd->self.bus_name);
+		ohci_stop(hcd);
+		return ret;
+	}
+
+	return 0;
+}
+
+int usb_hcd_ns9360_probe(const struct hc_driver *driver,
+		struct platform_device *pdev)
+{
+	int ret, irq;
+	struct usb_hcd *hcd;
+	struct resource *mem;
+	struct ns9360_ohci_device *priv;
+
+	hcd = usb_create_hcd(driver, &pdev->dev, "ns9360");
+	if (!hcd) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_create_hcd\n", __func__);
+		goto err_create_hcd;
+	}
+	priv = hcd2nod(hcd);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		ret = -ENODEV;
+		dev_dbg(&pdev->dev, "%s: err_get_irq\n", __func__);
+		goto err_get_irq;
+	}
+
+	priv->uhfe = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!priv->uhfe) {
+		ret = -ENODEV;
+		dev_dbg(&pdev->dev, "%s: err_get_uhfe\n", __func__);
+		goto err_get_uhfe;
+	}
+
+	if (!request_mem_region(priv->uhfe->start,
+				priv->uhfe->end - priv->uhfe->start + 1,
+				DRIVER_NAME)) {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_request_uhfe\n", __func__);
+		goto err_request_uhfe;
+	}
+
+	priv->ioaddr = ioremap(priv->uhfe->start,
+			priv->uhfe->end - priv->uhfe->start + 1);
+	if (!priv->ioaddr) {
+		ret = -ENODEV;
+		dev_dbg(&pdev->dev, "%s: err_map_uhfe\n", __func__);
+		goto err_map_uhfe;
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem) {
+		ret = -ENODEV;
+		dev_dbg(&pdev->dev, "%s: err_get_mem\n", __func__);
+		goto err_get_mem;
+	}
+
+	hcd->rsrc_start = mem->start;
+	hcd->rsrc_len = mem->end - mem->start + 1;
+
+	if (!request_mem_region(hcd->rsrc_start, hcd->rsrc_len, hcd_name))  {
+		ret = -ENOMEM;
+		dev_dbg(&pdev->dev, "%s: err_request_mem\n", __func__);
+		goto err_request_mem;
+}
+
+	hcd->regs = ioremap(hcd->rsrc_start, hcd->rsrc_len);
+	if (!hcd->regs) {
+		ret = -EIO;
+		dev_dbg(&pdev->dev, "%s: err_map_mem\n", __func__);
+		goto err_map_mem;
+	}
+
+	priv->clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(priv->clk)) {
+		ret = PTR_ERR(priv->clk);
+		dev_dbg(&pdev->dev, "%s: err_clk_get\n", __func__);
+		goto err_clk_get;
+	}
+
+	ret = clk_enable(priv->clk);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_clk_enable\n", __func__);
+		goto err_clk_enable;
+	}
+
+	/* enable irq */
+	iowrite32(NS9360_UHFE_IEN_OHCIIRQ, priv->ioaddr + NS9360_UHFE_IEN);
+
+	ohci_hcd_init(hcd_to_ohci(hcd));
+
+	ret = usb_add_hcd(hcd, irq, IRQF_DISABLED);
+	if (ret != 0) {
+		dev_dbg(&pdev->dev, "%s: err_add_hcd\n", __func__);
+		goto err_add_hcd;
+	}
+
+	return 0;
+
+err_add_hcd:
+	clk_disable(priv->clk);
+err_clk_enable:
+	clk_put(priv->clk);
+err_clk_get:
+	iounmap(hcd->regs);
+err_map_mem:
+	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
+err_request_mem:
+err_get_mem:
+	iounmap(priv->ioaddr);
+err_map_uhfe:
+	release_mem_region(priv->uhfe->start,
+			priv->uhfe->end - priv->uhfe->start + 1);
+err_request_uhfe:
+err_get_uhfe:
+err_get_irq:
+	usb_put_hcd(hcd);
+err_create_hcd:
+	return ret;
+}
+
+static int usb_hcd_ns9360_remove(struct usb_hcd *hcd,
+		struct platform_device *pdev)
+{
+	struct ns9360_ohci_device *priv = hcd2nod(hcd);
+
+	usb_remove_hcd(hcd);
+
+	clk_disable(priv->clk);
+	clk_put(priv->clk);
+
+	iounmap(hcd->regs);
+	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
+
+	iounmap(priv->ioaddr);
+	release_mem_region(priv->uhfe->start,
+			priv->uhfe->end - priv->uhfe->start + 1);
+
+	usb_put_hcd(hcd);
+
+	return 0;
+}
+
+static const struct hc_driver ohci_ns9360_hc_driver = {
+	.description =		hcd_name,
+	.product_desc =		"NS9360 OHCI",
+	.hcd_priv_size =	sizeof(struct ns9360_ohci_device),
+
+	/* generic hardware linkage */
+	.irq =			usb_hcd_ns9360_hcim_irq,
+	.flags =		HCD_USB11 | HCD_MEMORY ,
+
+	/* basic lifecycle operations */
+	.start =		ohci_ns9360_start,
+	.stop =			ohci_stop,
+	.shutdown =		ohci_shutdown,
+
+	/* managing i/o requests and associated device resources */
+	.urb_enqueue =		ohci_urb_enqueue,
+	.urb_dequeue =		ohci_urb_dequeue,
+	.endpoint_disable =	ohci_endpoint_disable,
+
+	/* scheduling support */
+	.get_frame_number =	ohci_get_frame,
+
+	/* root hub support */
+	.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 ohci_hcd_ns9360_drv_probe(struct platform_device *dev)
+{
+	return usb_hcd_ns9360_probe(&ohci_ns9360_hc_driver, dev);
+}
+
+static int ohci_hcd_ns9360_drv_remove(struct platform_device *dev)
+{
+	return usb_hcd_ns9360_remove(platform_get_drvdata(dev), dev);
+}
+
+static struct platform_driver ohci_hcd_ns9360_driver = {
+	.probe		= ohci_hcd_ns9360_drv_probe,
+	.remove		= ohci_hcd_ns9360_drv_remove,
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/usb/host/ohci-s3c2410.c b/drivers/usb/host/ohci-s3c2410.c
index f46af7a718d4..f35ea2184830 100644
--- a/drivers/usb/host/ohci-s3c2410.c
+++ b/drivers/usb/host/ohci-s3c2410.c
@@ -25,6 +25,11 @@
 #include <mach/hardware.h>
 #include <mach/usb-control.h>
 
+/* For enabling the second root hub port of the S3C2443 */
+#if defined(CONFIG_S3C2443_USB_PHY_PORT)
+#include <mach/regs-s3c2443-clock.h>
+#endif
+
 #define valid_port(idx) ((idx) == 1 || (idx) == 2)
 
 /* clock device associated with the hcd */
@@ -32,6 +37,57 @@
 static struct clk *clk;
 static struct clk *usb_clk;
 
+
+/*
+ * If the second port is enabled, then only connect the USB PHY to the
+ * host controller path
+ */
+#if defined(CONFIG_S3C2443_USB_PHY_PORT)
+static void usb_hcd_s3c2443_enable_second_port(void)
+{
+	ulong regval;
+		
+	/* PHY power enable */
+	regval = __raw_readl(S3C2443_PWRCFG);
+	regval |= S3C2443_PWRCFG_USBPHY_ON;
+	__raw_writel(regval, S3C2443_PWRCFG);
+	
+	/*
+	 * USB device 2.0 must reset like bellow,
+	 * 1st phy reset and after at least 10us, func_reset & host reset
+	 * phy reset can reset bellow registers.
+	 */
+	regval = S3C2443_URSTCON_PHY;
+	__raw_writel(regval, S3C2443_URSTCON);
+	udelay(20);
+	__raw_writel(0x00, S3C2443_URSTCON);
+	
+	/* Function reset, but DONT TOUCH THE HOST! */
+	regval = S3C2443_URSTCON_FUNC;
+	__raw_writel(regval, S3C2443_URSTCON);
+	__raw_writel(0x00, S3C2443_URSTCON);
+	
+	/* 48Mhz, Oscillator, External X-tal, device */
+	regval = S3C2443_PHYCTRL_EXTCLK_OSCI | S3C2443_PHYCTRL_DOWN_DEV;
+	__raw_writel(regval, S3C2443_PHYCTRL);
+}
+static inline void usb_hcd_s3c2443_disable_second_port(void)
+{
+	ulong regval;
+	
+	/* Reset the USB-function and the PHY */
+	writel(S3C2443_URSTCON_PHY, S3C2443_URSTCON);
+	
+	/* PHY power disable */
+	regval = readl(S3C2443_PWRCFG);
+	regval &= ~S3C2443_PWRCFG_USBPHY_ON;
+	writel(regval, S3C2443_PWRCFG);
+}
+#else
+static inline void usb_hcd_s3c2443_enable_second_port(void) { }
+static inline void usb_hcd_s3c2443_disable_second_port(void) { }
+#endif
+
 /* forward definitions */
 
 static void s3c2410_hcd_oc(struct s3c2410_hcd_info *info, int port_oc);
@@ -136,7 +192,7 @@ static void s3c2410_usb_set_power(struct s3c2410_hcd_info *info,
 
 	if (info->power_control != NULL) {
 		info->port[port-1].power = to;
-		(info->power_control)(port-1, to);
+		(info->power_control)(info, port-1, to);
 	}
 }
 
@@ -285,6 +341,8 @@ static void s3c2410_hcd_oc(struct s3c2410_hcd_info *info, int port_oc)
 	unsigned long flags;
 	int portno;
 
+	//printk(KERN_ERR "s3c2410-ohci: Over current (%i)\n", port_oc);
+	
 	if (info == NULL)
 		return;
 
@@ -301,8 +359,9 @@ static void s3c2410_hcd_oc(struct s3c2410_hcd_info *info, int port_oc)
 
 			/* ok, once over-current is detected,
 			   the port needs to be powered down */
-			s3c2410_usb_set_power(info, portno+1, 0);
-		}
+			s3c2410_usb_set_power(info, portno + 1, 0);
+		} else
+			s3c2410_usb_set_power(info, portno + 1, 1);
 	}
 
 	local_irq_restore(flags);
@@ -350,6 +409,9 @@ static int usb_hcd_s3c2410_probe (const struct hc_driver *driver,
 	s3c2410_usb_set_power(dev->dev.platform_data, 1, 1);
 	s3c2410_usb_set_power(dev->dev.platform_data, 2, 1);
 
+	/* Enable the second port for the S3C2443 */
+	usb_hcd_s3c2443_enable_second_port();
+	
 	hcd = usb_create_hcd(driver, &dev->dev, "s3c24xx");
 	if (hcd == NULL)
 		return -ENOMEM;
@@ -473,6 +535,37 @@ static const struct hc_driver ohci_s3c2410_hc_driver = {
 	.start_port_reset =	ohci_start_port_reset,
 };
 
+#if defined(CONFIG_PM)
+static int ohci_hcd_s3c2410_drv_suspend(struct platform_device *pdev, pm_message_t mesg)
+{
+	/* Disable the clocks before entering the suspend mode */
+	clk_disable(clk);
+	clk_disable(usb_clk);
+
+	usb_hcd_s3c2443_disable_second_port();
+
+	return 0;
+}
+
+static int ohci_hcd_s3c2410_drv_resume(struct platform_device *pdev)
+{
+	struct usb_hcd  *hcd;
+
+	usb_hcd_s3c2443_enable_second_port();
+	
+	/* Restart the clocks */
+	clk_enable(clk);
+	clk_enable(usb_clk);
+
+	hcd = platform_get_drvdata(pdev);
+	ohci_finish_controller_resume(hcd);
+	return 0;
+}
+#else
+#define ohci_hcd_s3c2410_drv_suspend NULL
+#define ohci_hcd_s3c2410_drv_resume  NULL
+#endif /* defined(CONFIG_PM) */
+
 /* device driver */
 
 static int ohci_hcd_s3c2410_drv_probe(struct platform_device *pdev)
@@ -492,8 +585,8 @@ static struct platform_driver ohci_hcd_s3c2410_driver = {
 	.probe		= ohci_hcd_s3c2410_drv_probe,
 	.remove		= ohci_hcd_s3c2410_drv_remove,
 	.shutdown	= usb_hcd_platform_shutdown,
-	/*.suspend	= ohci_hcd_s3c2410_drv_suspend, */
-	/*.resume	= ohci_hcd_s3c2410_drv_resume, */
+	.suspend	= ohci_hcd_s3c2410_drv_suspend,
+	.resume	        = ohci_hcd_s3c2410_drv_resume,
 	.driver		= {
 		.owner	= THIS_MODULE,
 		.name	= "s3c2410-ohci",
diff --git a/drivers/usb/otg/Kconfig b/drivers/usb/otg/Kconfig
new file mode 100644
index 000000000000..f9f868c15e11
--- /dev/null
+++ b/drivers/usb/otg/Kconfig
@@ -0,0 +1,11 @@
+config TRANSCEIVER_MXC_OTG
+	tristate "USB OTG pin detect support"
+	depends on (MC13783_MXC || ISP1504_MXC) && USB_GADGET && USB_EHCI_HCD && USB_OTG
+	help
+	  Support for USB OTG PIN detect on MXC platforms.
+
+config UTMI_MXC_OTG
+	tristate "USB OTG pin detect support for UTMI PHY"
+	depends on  USB_EHCI_FSL_UTMI && USB_GADGET && USB_EHCI_HCD && USB_OTG
+	help
+	  Support for USB OTG PIN detect for UTMI PHY on MXC platforms.
diff --git a/drivers/usb/otg/Makefile b/drivers/usb/otg/Makefile
new file mode 100644
index 000000000000..85979c00620f
--- /dev/null
+++ b/drivers/usb/otg/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for USB OTG controller  driver
+#
+# USB transceiver
+fsl_otg_arc-objs		:= fsl_otg.o otg_fsm.o
+obj-$(CONFIG_ISP1504_MXC_OTG) += fsl_otg_arc.o
+obj-$(CONFIG_UTMI_MXC_OTG) += fsl_otg_arc.o
diff --git a/drivers/usb/otg/fsl_otg.c b/drivers/usb/otg/fsl_otg.c
new file mode 100644
index 000000000000..d5c8cf0434e3
--- /dev/null
+++ b/drivers/usb/otg/fsl_otg.c
@@ -0,0 +1,1200 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * Author: Li Yang <LeoLi@freescale.com>
+ *         Jerry Huang <Chang-Ming.Huang@freescale.com>
+ *
+ * Initialization based on code from Shlomi Gridish.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the  GNU General Public License along
+ * with this program; if not, write  to the Free Software Foundation, Inc.,
+ * 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/smp_lock.h>
+#include <linux/proc_fs.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/reboot.h>
+#include <linux/timer.h>
+#include <linux/list.h>
+#include <linux/usb.h>
+#include <linux/device.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <linux/workqueue.h>
+#include <linux/time.h>
+#include <linux/fsl_devices.h>
+#include <linux/platform_device.h>
+
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+#include <asm/byteorder.h>
+#include <asm/uaccess.h>
+#include <asm/unaligned.h>
+
+#include "fsl_otg.h"
+
+#define CONFIG_USB_OTG_DEBUG_FILES
+#define DRIVER_VERSION "$Revision: 1.55 $"
+#define DRIVER_AUTHOR "Jerry Huang/Li Yang"
+#define DRIVER_DESC "Freescale USB OTG Driver"
+#define DRIVER_INFO DRIVER_VERSION " " DRIVER_DESC
+
+MODULE_DESCRIPTION("Freescale USB OTG Transceiver Driver");
+
+static const char driver_name[] = "fsl-usb2-otg";
+
+const pm_message_t otg_suspend_state = {
+	.event = 1,
+};
+
+#define HA_DATA_PULSE 1
+
+volatile static struct usb_dr_mmap *usb_dr_regs;
+static struct fsl_otg *fsl_otg_dev;
+static int srp_wait_done;
+
+/* FSM timers */
+struct fsl_otg_timer *a_wait_vrise_tmr, *a_wait_bcon_tmr, *a_aidl_bdis_tmr,
+	*b_ase0_brst_tmr, *b_se0_srp_tmr;
+
+/* Driver specific timers */
+struct fsl_otg_timer *b_data_pulse_tmr, *b_vbus_pulse_tmr, *b_srp_fail_tmr,
+	*b_srp_wait_tmr, *a_wait_enum_tmr;
+
+static struct list_head active_timers;
+
+static struct fsl_otg_config fsl_otg_initdata = {
+	.otg_port = 1,
+};
+
+int write_ulpi(u8 addr, u8 data)
+{
+	u32 temp;
+	temp = 0x60000000 | (addr << 16) | data;
+	temp = cpu_to_le32(temp);
+	usb_dr_regs->ulpiview = temp;
+	return 0;
+}
+
+/* prototype declaration */
+void fsl_otg_add_timer(void *timer);
+void fsl_otg_del_timer(void *timer);
+
+/* -------------------------------------------------------------*/
+/* Operations that will be called from OTG Finite State Machine */
+
+/* Charge vbus for vbus pulsing in SRP */
+void fsl_otg_chrg_vbus(int on)
+{
+	if (on)
+		usb_dr_regs->otgsc =
+		    cpu_to_le32((le32_to_cpu(usb_dr_regs->otgsc) &
+				 ~OTGSC_INTSTS_MASK &
+				 ~OTGSC_CTRL_VBUS_DISCHARGE) |
+				OTGSC_CTRL_VBUS_CHARGE);
+	else
+		usb_dr_regs->otgsc =
+		    cpu_to_le32((le32_to_cpu(usb_dr_regs->otgsc) &
+				 ~OTGSC_INTSTS_MASK & ~OTGSC_CTRL_VBUS_CHARGE));
+}
+
+/* Discharge vbus through a resistor to ground */
+void fsl_otg_dischrg_vbus(int on)
+{
+	if (on)
+		usb_dr_regs->otgsc =
+		    cpu_to_le32((le32_to_cpu(usb_dr_regs->otgsc) &
+				 ~OTGSC_INTSTS_MASK)
+				| OTGSC_CTRL_VBUS_DISCHARGE);
+	else
+		usb_dr_regs->otgsc =
+		    cpu_to_le32((le32_to_cpu(usb_dr_regs->otgsc) &
+				 ~OTGSC_INTSTS_MASK &
+				 ~OTGSC_CTRL_VBUS_DISCHARGE));
+}
+
+/* A-device driver vbus, controlled through PP bit in PORTSC */
+void fsl_otg_drv_vbus(int on)
+{
+/*	if (on)
+		usb_dr_regs->portsc =
+		    cpu_to_le32((le32_to_cpu(usb_dr_regs->portsc) &
+				 ~PORTSC_W1C_BITS) | PORTSC_PORT_POWER);
+	else
+		usb_dr_regs->portsc =
+		    cpu_to_le32(le32_to_cpu(usb_dr_regs->portsc) &
+				~PORTSC_W1C_BITS & ~PORTSC_PORT_POWER);
+*/
+}
+
+/*
+ * Pull-up D+, signalling connect by periperal. Also used in
+ * data-line pulsing in SRP
+ */
+void fsl_otg_loc_conn(int on)
+{
+	if (on)
+		usb_dr_regs->otgsc =
+		    cpu_to_le32((le32_to_cpu(usb_dr_regs->otgsc) &
+				 ~OTGSC_INTSTS_MASK) | OTGSC_CTRL_DATA_PULSING);
+	else
+		usb_dr_regs->otgsc =
+		    cpu_to_le32(le32_to_cpu(usb_dr_regs->otgsc) &
+				~OTGSC_INTSTS_MASK & ~OTGSC_CTRL_DATA_PULSING);
+}
+
+/* Generate SOF by host.  This is controlled through suspend/resume the
+ * port.  In host mode, controller will automatically send SOF.
+ * Suspend will block the data on the port.
+ */
+void fsl_otg_loc_sof(int on)
+{
+	u32 tmpval;
+
+	tmpval = readl(&fsl_otg_dev->dr_mem_map->portsc) & ~PORTSC_W1C_BITS;
+	if (on)
+		tmpval |= PORTSC_PORT_FORCE_RESUME;
+	else
+		tmpval |= PORTSC_PORT_SUSPEND;
+	writel(tmpval, &fsl_otg_dev->dr_mem_map->portsc);
+
+}
+
+/* Start SRP pulsing by data-line pulsing, followed with v-bus pulsing. */
+void fsl_otg_start_pulse(void)
+{
+	srp_wait_done = 0;
+#ifdef HA_DATA_PULSE
+	usb_dr_regs->otgsc =
+	    cpu_to_le32((le32_to_cpu(usb_dr_regs->otgsc) & ~OTGSC_INTSTS_MASK)
+			| OTGSC_HA_DATA_PULSE);
+#else
+	fsl_otg_loc_conn(1);
+#endif
+
+	fsl_otg_add_timer(b_data_pulse_tmr);
+}
+
+void fsl_otg_pulse_vbus(void);
+
+void b_data_pulse_end(unsigned long foo)
+{
+#ifdef HA_DATA_PULSE
+#else
+	fsl_otg_loc_conn(0);
+#endif
+
+	/* Do VBUS pulse after data pulse */
+	fsl_otg_pulse_vbus();
+}
+
+void fsl_otg_pulse_vbus(void)
+{
+	srp_wait_done = 0;
+	fsl_otg_chrg_vbus(1);
+	/* start the timer to end vbus charge */
+	fsl_otg_add_timer(b_vbus_pulse_tmr);
+}
+
+void b_vbus_pulse_end(unsigned long foo)
+{
+	fsl_otg_chrg_vbus(0);
+
+	/* As USB3300 using the same a_sess_vld and b_sess_vld voltage
+	 * we need to discharge the bus for a while to distinguish
+	 * residual voltage of vbus pulsing and A device pull up */
+	fsl_otg_dischrg_vbus(1);
+	fsl_otg_add_timer(b_srp_wait_tmr);
+}
+
+void b_srp_end(unsigned long foo)
+{
+	fsl_otg_dischrg_vbus(0);
+	srp_wait_done = 1;
+
+	if ((fsl_otg_dev->otg.state == OTG_STATE_B_SRP_INIT) &&
+	    fsl_otg_dev->fsm.b_sess_vld)
+		fsl_otg_dev->fsm.b_srp_done = 1;
+}
+
+/* Workaround for a_host suspending too fast.  When a_bus_req=0,
+ * a_host will start by SRP.  It needs to set b_hnp_enable before
+ * actually suspending to start HNP
+ */
+void a_wait_enum(unsigned long foo)
+{
+	VDBG("a_wait_enum timeout\n");
+	if (!fsl_otg_dev->otg.host->b_hnp_enable)
+		fsl_otg_add_timer(a_wait_enum_tmr);
+	else
+		otg_statemachine(&fsl_otg_dev->fsm);
+}
+
+/* ------------------------------------------------------*/
+
+/* The timeout callback function to set time out bit */
+void set_tmout(unsigned long indicator)
+{
+	*(int *)indicator = 1;
+}
+
+/* Initialize timers */
+int fsl_otg_init_timers(struct otg_fsm *fsm)
+{
+	/* FSM used timers */
+	a_wait_vrise_tmr = otg_timer_initializer(&set_tmout, TA_WAIT_VRISE,
+				(unsigned long)&fsm->a_wait_vrise_tmout);
+	if (a_wait_vrise_tmr == NULL)
+		return -ENOMEM;
+
+	a_wait_bcon_tmr = otg_timer_initializer(&set_tmout, TA_WAIT_BCON,
+				(unsigned long)&fsm->a_wait_bcon_tmout);
+	if (a_wait_bcon_tmr == NULL)
+		return -ENOMEM;
+
+	a_aidl_bdis_tmr = otg_timer_initializer(&set_tmout, TA_AIDL_BDIS,
+				(unsigned long)&fsm->a_aidl_bdis_tmout);
+	if (a_aidl_bdis_tmr == NULL)
+		return -ENOMEM;
+
+	b_ase0_brst_tmr = otg_timer_initializer(&set_tmout, TB_ASE0_BRST,
+				(unsigned long)&fsm->b_ase0_brst_tmout);
+	if (b_ase0_brst_tmr == NULL)
+		return -ENOMEM;
+
+	b_se0_srp_tmr = otg_timer_initializer(&set_tmout, TB_SE0_SRP,
+				(unsigned long)&fsm->b_se0_srp);
+	if (b_se0_srp_tmr == NULL)
+		return -ENOMEM;
+
+	b_srp_fail_tmr = otg_timer_initializer(&set_tmout, TB_SRP_FAIL,
+				(unsigned long)&fsm->b_srp_done);
+	if (b_srp_fail_tmr == NULL)
+		return -ENOMEM;
+
+	a_wait_enum_tmr = otg_timer_initializer(&a_wait_enum, 10,
+				(unsigned long)&fsm);
+	if (a_wait_enum_tmr == NULL)
+		return -ENOMEM;
+
+	/* device driver used timers */
+	b_srp_wait_tmr = otg_timer_initializer(&b_srp_end, TB_SRP_WAIT, 0);
+	if (b_srp_wait_tmr == NULL)
+		return -ENOMEM;
+
+	b_data_pulse_tmr = otg_timer_initializer(&b_data_pulse_end,
+				TB_DATA_PLS, 0);
+	if (b_data_pulse_tmr == NULL)
+		return -ENOMEM;
+
+	b_vbus_pulse_tmr = otg_timer_initializer(&b_vbus_pulse_end,
+				TB_VBUS_PLS, 0);
+	if (b_vbus_pulse_tmr == NULL)
+		return -ENOMEM;
+
+	return 0;
+}
+
+/* Uninitialize timers */
+void fsl_otg_uninit_timers(void)
+{
+	/* FSM used timers */
+	if (a_wait_vrise_tmr != NULL)
+		kfree(a_wait_vrise_tmr);
+	if (a_wait_bcon_tmr != NULL)
+		kfree(a_wait_bcon_tmr);
+	if (a_aidl_bdis_tmr != NULL)
+		kfree(a_aidl_bdis_tmr);
+	if (b_ase0_brst_tmr != NULL)
+		kfree(b_ase0_brst_tmr);
+	if (b_se0_srp_tmr != NULL)
+		kfree(b_se0_srp_tmr);
+	if (b_srp_fail_tmr != NULL)
+		kfree(b_srp_fail_tmr);
+	if (a_wait_enum_tmr != NULL)
+		kfree(a_wait_enum_tmr);
+
+	/* device driver used timers */
+	if (b_srp_wait_tmr != NULL)
+		kfree(b_srp_wait_tmr);
+	if (b_data_pulse_tmr != NULL)
+		kfree(b_data_pulse_tmr);
+	if (b_vbus_pulse_tmr != NULL)
+		kfree(b_vbus_pulse_tmr);
+}
+
+/* Add timer to timer list */
+void fsl_otg_add_timer(void *gtimer)
+{
+	struct fsl_otg_timer *timer = (struct fsl_otg_timer *)gtimer;
+	struct fsl_otg_timer *tmp_timer;
+
+	/* Check if the timer is already in the active list,
+	 * if so update timer count
+	 */
+	list_for_each_entry(tmp_timer, &active_timers, list)
+	    if (tmp_timer == timer) {
+		timer->count = timer->expires;
+		return;
+	}
+	timer->count = timer->expires;
+	list_add_tail(&timer->list, &active_timers);
+}
+
+/* Remove timer from the timer list; clear timeout status */
+void fsl_otg_del_timer(void *gtimer)
+{
+	struct fsl_otg_timer *timer = (struct fsl_otg_timer *)gtimer;
+	struct fsl_otg_timer *tmp_timer, *del_tmp;
+
+	list_for_each_entry_safe(tmp_timer, del_tmp, &active_timers, list)
+		if (tmp_timer == timer)
+			list_del(&timer->list);
+}
+
+/* Reduce timer count by 1, and find timeout conditions.
+ * Called by fsl_otg 1ms timer interrupt
+ */
+int fsl_otg_tick_timer(void)
+{
+	struct fsl_otg_timer *tmp_timer, *del_tmp;
+	int expired = 0;
+
+	list_for_each_entry_safe(tmp_timer, del_tmp, &active_timers, list) {
+		tmp_timer->count--;
+		/* check if timer expires */
+		if (!tmp_timer->count) {
+			list_del(&tmp_timer->list);
+			tmp_timer->function(tmp_timer->data);
+			expired = 1;
+		}
+	}
+
+	return expired;
+}
+
+/* Reset controller, not reset the bus */
+void otg_reset_controller(void)
+{
+	u32 command;
+
+	command = readl(&usb_dr_regs->usbcmd);
+	command |= (1 << 1);
+	writel(command, &usb_dr_regs->usbcmd);
+	while (readl(&usb_dr_regs->usbcmd) & (1 << 1)) ;
+}
+
+/* Call suspend/resume routines in host driver */
+int fsl_otg_start_host(struct otg_fsm *fsm, int on)
+{
+	struct otg_transceiver *xceiv = fsm->transceiver;
+	struct device *dev;
+	struct fsl_otg *otg_dev = container_of(xceiv, struct fsl_otg, otg);
+	u32 retval = 0;
+
+	if (!xceiv->host)
+		return -ENODEV;
+	dev = xceiv->host->controller;
+
+	/* Update a_vbus_vld state as a_vbus_vld int is disabled
+	 * in device mode
+	 */
+	fsm->a_vbus_vld =
+	    (le32_to_cpu(usb_dr_regs->otgsc) & OTGSC_STS_A_VBUS_VALID) ? 1 : 0;
+	if (on) {
+		/* start fsl usb host controller */
+		if (otg_dev->host_working)
+			goto end;
+		else {
+			otg_reset_controller();
+			VDBG("host on......\n");
+			if (dev->driver->resume) {
+				retval = dev->driver->resume(dev);
+				if (fsm->id) {
+					/* default-b */
+					fsl_otg_drv_vbus(1);
+					/* Workaround: b_host can't driver
+					 * vbus, but PP in PORTSC needs to
+					 * be 1 for host to work.
+					 * So we set drv_vbus bit in
+					 * transceiver to 0 thru ULPI. */
+#if defined(CONFIG_ISP1504_MXC)
+					write_ulpi(0x0c, 0x20);
+#endif
+				}
+			}
+
+			otg_dev->host_working = 1;
+		}
+	} else {
+		/* stop fsl usb host controller */
+		if (!otg_dev->host_working)
+			goto end;
+		else {
+			VDBG("host off......\n");
+			if (dev && dev->driver) {
+				retval = dev->driver->suspend(dev,
+							otg_suspend_state);
+				if (fsm->id)
+					/* default-b */
+					fsl_otg_drv_vbus(0);
+			}
+			otg_dev->host_working = 0;
+		}
+	}
+end:
+	return retval;
+}
+
+/* Call suspend and resume function in udc driver
+ * to stop and start udc driver.
+ */
+int fsl_otg_start_gadget(struct otg_fsm *fsm, int on)
+{
+	struct otg_transceiver *xceiv = fsm->transceiver;
+	struct device *dev;
+
+	if (!xceiv->gadget || !xceiv->gadget->dev.parent)
+		return -ENODEV;
+
+	VDBG("gadget %s \n", on ? "on" : "off");
+	dev = xceiv->gadget->dev.parent;
+
+	if (on)
+		dev->driver->resume(dev);
+	else
+		dev->driver->suspend(dev, otg_suspend_state);
+
+	return 0;
+}
+
+/* Called by initialization code of host driver.  Register host controller
+ * to the OTG.  Suspend host for OTG role detection.
+ */
+static int fsl_otg_set_host(struct otg_transceiver *otg_p, struct usb_bus *host)
+{
+	struct fsl_otg *otg_dev = container_of(otg_p, struct fsl_otg, otg);
+
+	if (!otg_p || otg_dev != fsl_otg_dev)
+		return -ENODEV;
+
+	otg_p->host = host;
+
+	otg_dev->fsm.a_bus_drop = 0;
+	otg_dev->fsm.a_bus_req = 1;
+
+	if (host) {
+		VDBG("host off......\n");
+
+		otg_p->host->otg_port = fsl_otg_initdata.otg_port;
+		otg_p->host->is_b_host = otg_dev->fsm.id;
+		/* must leave time for khubd to finish its thing
+		 * before yanking the host driver out from under it,
+		 * so suspend the host after a short delay.
+		 */
+		otg_dev->host_working = 1;
+		schedule_delayed_work(&otg_dev->otg_event, 100);
+		return 0;
+	} else {		/* host driver going away */
+
+		if (!(le32_to_cpu(otg_dev->dr_mem_map->otgsc) &
+		      OTGSC_STS_USB_ID)) {
+			/* Mini-A cable connected */
+			struct otg_fsm *fsm = &otg_dev->fsm;
+
+			otg_p->state = OTG_STATE_UNDEFINED;
+			fsm->protocol = PROTO_UNDEF;
+		}
+	}
+
+	otg_dev->host_working = 0;
+
+	otg_statemachine(&otg_dev->fsm);
+
+	return 0;
+}
+
+/* Called by initialization code of udc.  Register udc to OTG.*/
+static int fsl_otg_set_peripheral(struct otg_transceiver *otg_p,
+				  struct usb_gadget *gadget)
+{
+	struct fsl_otg *otg_dev = container_of(otg_p, struct fsl_otg, otg);
+
+	VDBG("otg_dev 0x%x\n", (int)otg_dev);
+	VDBG("fsl_otg_dev 0x%x\n", (int)fsl_otg_dev);
+
+	if (!otg_p || otg_dev != fsl_otg_dev)
+		return -ENODEV;
+
+	if (!gadget) {
+		if (!otg_dev->otg.default_a)
+			otg_p->gadget->ops->vbus_draw(otg_p->gadget, 0);
+		usb_gadget_vbus_disconnect(otg_dev->otg.gadget);
+		otg_dev->otg.gadget = 0;
+		otg_dev->fsm.b_bus_req = 0;
+		otg_statemachine(&otg_dev->fsm);
+		return 0;
+	}
+#ifdef DEBUG
+	/*
+	 * debug the initial state of the ID pin when only
+	 * the gadget driver is loaded and no cable is connected.
+	 * sometimes, we get an ID irq right
+	 * after the udc driver's otg_get_transceiver() call
+	 * that indicates that IDpin=0, which means a Mini-A
+	 * connector is attached.  not good.
+	 */
+	DBG("before: fsm.id ID pin=%d", otg_dev->fsm.id);
+	otg_dev->fsm.id = (otg_dev->dr_mem_map->otgsc & OTGSC_STS_USB_ID) ?
+	    1 : 0;
+	DBG("after:  fsm.id ID pin=%d", otg_dev->fsm.id);
+	/*if (!otg_dev->fsm.id) {
+	   printk("OTG Control = 0x%x\n",
+	   isp1504_read(ISP1504_OTGCTL,
+	   &otg_dev->dr_mem_map->ulpiview));
+	   } */
+#endif
+
+	otg_p->gadget = gadget;
+	otg_p->gadget->is_a_peripheral = !otg_dev->fsm.id;
+
+	otg_dev->fsm.b_bus_req = 1;
+
+	/* start the gadget right away if the ID pin says Mini-B */
+	DBG("ID pin=%d\n", otg_dev->fsm.id);
+	if (otg_dev->fsm.id == 1) {
+		fsl_otg_start_host(&otg_dev->fsm, 0);
+		otg_drv_vbus(&otg_dev->fsm, 0);
+		fsl_otg_start_gadget(&otg_dev->fsm, 1);
+	}
+
+	return 0;
+}
+
+/* Set OTG port power, only for B-device */
+static int fsl_otg_set_power(struct otg_transceiver *otg_p, unsigned mA)
+{
+	if (!fsl_otg_dev)
+		return -ENODEV;
+	if (otg_p->state == OTG_STATE_B_PERIPHERAL)
+		printk(KERN_INFO "FSL OTG:Draw %d mA\n", mA);
+
+	return 0;
+}
+
+/* Delayed pin detect interrupt processing.
+ *
+ * When the Mini-A cable is disconnected from the board,
+ * the pin-detect interrupt happens before the disconnnect
+ * interrupts for the connected device(s).  In order to
+ * process the disconnect interrupt(s) prior to switching
+ * roles, the pin-detect interrupts are delayed, and handled
+ * by this routine.
+ */
+static void fsl_otg_event(struct work_struct *work)
+{
+	struct fsl_otg *og = container_of(work, struct fsl_otg, otg_event.work);
+	struct otg_fsm *fsm = &og->fsm;
+
+	if (fsm->id) {		/* switch to gadget */
+		fsl_otg_start_host(fsm, 0);
+		otg_drv_vbus(fsm, 0);
+		fsl_otg_start_gadget(fsm, 1);
+	}
+}
+
+/* B-device start SRP */
+static int fsl_otg_start_srp(struct otg_transceiver *otg_p)
+{
+	struct fsl_otg *otg_dev = container_of(otg_p, struct fsl_otg, otg);
+
+	if (!otg_p || otg_dev != fsl_otg_dev
+	    || otg_p->state != OTG_STATE_B_IDLE)
+		return -ENODEV;
+
+	otg_dev->fsm.b_bus_req = 1;
+	otg_statemachine(&otg_dev->fsm);
+
+	return 0;
+}
+
+/* A_host suspend will call this function to start hnp */
+static int fsl_otg_start_hnp(struct otg_transceiver *otg_p)
+{
+	struct fsl_otg *otg_dev = container_of(otg_p, struct fsl_otg, otg);
+
+	if (!otg_p || otg_dev != fsl_otg_dev)
+		return -ENODEV;
+
+	/* printk("start_hnp.............\n"); */
+	/* clear a_bus_req to enter a_suspend state */
+	otg_dev->fsm.a_bus_req = 0;
+	otg_statemachine(&otg_dev->fsm);
+
+	return 0;
+}
+
+/* Interrupt handler.  OTG/host/peripheral share the same int line.
+ * OTG driver clears OTGSC interrupts and leaves USB interrupts
+ * intact.  It needs to have knowledge of some USB interrupts
+ * such as port change.
+ */
+irqreturn_t fsl_otg_isr(int irq, void *dev_id)
+{
+	struct otg_fsm *fsm = &((struct fsl_otg *)dev_id)->fsm;
+	struct otg_transceiver *otg = &((struct fsl_otg *)dev_id)->otg;
+	u32 otg_int_src, otg_sc;
+
+	otg_sc = le32_to_cpu(usb_dr_regs->otgsc);
+	otg_int_src = otg_sc & OTGSC_INTSTS_MASK & (otg_sc >> 8);
+
+	/* Only clear otg interrupts */
+	usb_dr_regs->otgsc |= cpu_to_le32(otg_sc & OTGSC_INTSTS_MASK);
+
+	/*FIXME: ID change not generate when init to 0 */
+	fsm->id = (otg_sc & OTGSC_STS_USB_ID) ? 1 : 0;
+	otg->default_a = (fsm->id == 0);
+
+	/* process OTG interrupts */
+	if (otg_int_src) {
+		if (otg_int_src & OTGSC_INTSTS_USB_ID) {
+			fsm->id = (otg_sc & OTGSC_STS_USB_ID) ? 1 : 0;
+			otg->default_a = (fsm->id == 0);
+			/* clear conn information */
+			if (fsm->id)
+				fsm->b_conn = 0;
+			else
+				fsm->a_conn = 0;
+
+			if (otg->host)
+				otg->host->is_b_host = fsm->id;
+			if (otg->gadget)
+				otg->gadget->is_a_peripheral = !fsm->id;
+			VDBG("ID int (ID is %d)\n", fsm->id);
+
+			if (fsm->id) {	/* switch to gadget */
+				schedule_delayed_work(&((struct fsl_otg *)
+							dev_id)->otg_event,
+							100);
+			} else {	/* switch to host */
+				cancel_delayed_work(&
+						    ((struct fsl_otg *)dev_id)->
+						    otg_event);
+				fsl_otg_start_gadget(fsm, 0);
+				otg_drv_vbus(fsm, 1);
+				fsl_otg_start_host(fsm, 1);
+			}
+
+			return IRQ_HANDLED;
+		}
+	}
+
+	return IRQ_NONE;
+}
+
+static struct otg_fsm_ops fsl_otg_ops = {
+	.chrg_vbus = fsl_otg_chrg_vbus,
+	.drv_vbus = fsl_otg_drv_vbus,
+	.loc_conn = fsl_otg_loc_conn,
+	.loc_sof = fsl_otg_loc_sof,
+	.start_pulse = fsl_otg_start_pulse,
+
+	.add_timer = fsl_otg_add_timer,
+	.del_timer = fsl_otg_del_timer,
+
+	.start_host = fsl_otg_start_host,
+	.start_gadget = fsl_otg_start_gadget,
+};
+
+/* Initialize the global variable fsl_otg_dev and request IRQ for OTG */
+static int fsl_otg_conf(struct platform_device *pdev)
+{
+	int status;
+	struct fsl_otg *fsl_otg_tc;
+	struct fsl_usb2_platform_data *pdata;
+
+	pdata = pdev->dev.platform_data;
+
+	DBG();
+
+	if (fsl_otg_dev)
+		return 0;
+
+	/* allocate space to fsl otg device */
+	fsl_otg_tc = kzalloc(sizeof(struct fsl_otg), GFP_KERNEL);
+	if (!fsl_otg_tc)
+		return -ENODEV;
+
+	INIT_DELAYED_WORK(&fsl_otg_tc->otg_event, fsl_otg_event);
+
+	INIT_LIST_HEAD(&active_timers);
+	status = fsl_otg_init_timers(&fsl_otg_tc->fsm);
+	if (status) {
+		printk(KERN_INFO "Couldn't init OTG timers\n");
+		fsl_otg_uninit_timers();
+		kfree(fsl_otg_tc);
+		return status;
+	}
+	spin_lock_init(&fsl_otg_tc->fsm.lock);
+
+	/* Set OTG state machine operations */
+	fsl_otg_tc->fsm.ops = &fsl_otg_ops;
+
+	/* initialize the otg structure */
+	fsl_otg_tc->otg.label = DRIVER_DESC;
+	fsl_otg_tc->otg.set_host = fsl_otg_set_host;
+	fsl_otg_tc->otg.set_peripheral = fsl_otg_set_peripheral;
+	fsl_otg_tc->otg.set_power = fsl_otg_set_power;
+	fsl_otg_tc->otg.start_hnp = fsl_otg_start_hnp;
+	fsl_otg_tc->otg.start_srp = fsl_otg_start_srp;
+
+	fsl_otg_dev = fsl_otg_tc;
+
+	/* Store the otg transceiver */
+	status = otg_set_transceiver(&fsl_otg_tc->otg);
+	if (status) {
+		printk(KERN_WARNING ": unable to register OTG transceiver.\n");
+		return status;
+	}
+
+	return 0;
+}
+
+/* OTG Initialization*/
+int usb_otg_start(struct platform_device *pdev)
+{
+	struct fsl_otg *p_otg;
+	struct otg_transceiver *otg_trans = otg_get_transceiver();
+	struct otg_fsm *fsm;
+	volatile unsigned long *p;
+	int status;
+	struct resource *res;
+	u32 temp;
+	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
+
+	p_otg = container_of(otg_trans, struct fsl_otg, otg);
+	fsm = &p_otg->fsm;
+
+	/* Initialize the state machine structure with default values */
+	SET_OTG_STATE(otg_trans, OTG_STATE_UNDEFINED);
+	fsm->transceiver = &p_otg->otg;
+
+	/* We don't require predefined MEM/IRQ resource index */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENXIO;
+
+	/* We don't request_mem_region here to enable resource sharing
+	 * with host/device */
+
+	usb_dr_regs = ioremap(res->start, sizeof(struct usb_dr_mmap));
+	p_otg->dr_mem_map = (struct usb_dr_mmap *)usb_dr_regs;
+	pdata->regs = (void *)usb_dr_regs;
+
+	/* request irq */
+	p_otg->irq = platform_get_irq(pdev, 0);
+	status = request_irq(p_otg->irq, fsl_otg_isr,
+				IRQF_SHARED, driver_name, p_otg);
+	if (status) {
+		dev_dbg(p_otg->otg.dev, "can't get IRQ %d, error %d\n",
+			p_otg->irq, status);
+		iounmap(p_otg->dr_mem_map);
+		kfree(p_otg);
+		return status;
+	}
+
+	if (pdata->platform_init && pdata->platform_init(pdev) != 0)
+		return -EINVAL;
+
+
+	/* Export DR controller resources */
+	otg_set_resources(pdev->resource);
+
+	/* stop the controller */
+	temp = readl(&p_otg->dr_mem_map->usbcmd);
+	temp &= ~USB_CMD_RUN_STOP;
+	writel(temp, &p_otg->dr_mem_map->usbcmd);
+
+	/* reset the controller */
+	temp = readl(&p_otg->dr_mem_map->usbcmd);
+	temp |= USB_CMD_CTRL_RESET;
+	writel(temp, &p_otg->dr_mem_map->usbcmd);
+
+	/* wait reset completed */
+	while (readl(&p_otg->dr_mem_map->usbcmd) & USB_CMD_CTRL_RESET) ;
+
+	/* configure the VBUSHS as IDLE(both host and device) */
+	temp = USB_MODE_STREAM_DISABLE | (pdata->es ? USB_MODE_ES : 0);
+	writel(temp, &p_otg->dr_mem_map->usbmode);
+
+	/* configure PHY interface */
+	temp = readl(&p_otg->dr_mem_map->portsc);
+	temp &= ~(PORTSC_PHY_TYPE_SEL | PORTSC_PTW);
+	switch (pdata->phy_mode) {
+	case FSL_USB2_PHY_ULPI:
+		temp |= PORTSC_PTS_ULPI;
+		break;
+	case FSL_USB2_PHY_UTMI_WIDE:
+		temp |= PORTSC_PTW_16BIT;
+		/* fall through */
+	case FSL_USB2_PHY_UTMI:
+		temp |= PORTSC_PTS_UTMI;
+		/* fall through */
+	default:
+		break;
+	}
+	writel(temp, &p_otg->dr_mem_map->portsc);
+
+	if (pdata->have_sysif_regs) {
+		/* configure control enable IO output, big endian register */
+		p = (volatile unsigned long *)(&p_otg->dr_mem_map->control);
+		temp = *p;
+		temp |= USB_CTRL_IOENB;
+		*p = temp;
+	}
+
+	/* disable all interrupt and clear all OTGSC status */
+	temp = readl(&p_otg->dr_mem_map->otgsc);
+	temp &= ~OTGSC_INTERRUPT_ENABLE_BITS_MASK;
+	temp |= OTGSC_INTERRUPT_STATUS_BITS_MASK | OTGSC_CTRL_VBUS_DISCHARGE;
+	writel(temp, &p_otg->dr_mem_map->otgsc);
+
+
+	/*
+	 * The identification (id) input is FALSE when a Mini-A plug is inserted
+	 * in the devices Mini-AB receptacle. Otherwise, this input is TRUE.
+	 * Also: record initial state of ID pin
+	 */
+	if (le32_to_cpu(p_otg->dr_mem_map->otgsc) & OTGSC_STS_USB_ID) {
+		p_otg->otg.state = OTG_STATE_UNDEFINED;
+		p_otg->fsm.id = 1;
+	} else {
+		p_otg->otg.state = OTG_STATE_A_IDLE;
+		p_otg->fsm.id = 0;
+	}
+
+	DBG("initial ID pin=%d\n", p_otg->fsm.id);
+
+	/* enable OTG ID pin interrupt */
+	temp = readl(&p_otg->dr_mem_map->otgsc);
+	temp |= OTGSC_INTR_USB_ID_EN;
+	temp &= ~(OTGSC_CTRL_VBUS_DISCHARGE | OTGSC_INTR_1MS_TIMER_EN);
+	writel(temp, &p_otg->dr_mem_map->otgsc);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------
+		PROC File System Support
+-------------------------------------------------------------------------*/
+#ifdef CONFIG_USB_OTG_DEBUG_FILES
+
+#include <linux/seq_file.h>
+
+static const char proc_filename[] = "driver/isp1504_otg";
+
+static int otg_proc_read(char *page, char **start, off_t off, int count,
+			 int *eof, void *_dev)
+{
+	struct otg_fsm *fsm = &fsl_otg_dev->fsm;
+	char *buf = page;
+	char *next = buf;
+	unsigned size = count;
+	unsigned long flags;
+	int t;
+	u32 tmp_reg;
+
+	if (off != 0)
+		return 0;
+
+	spin_lock_irqsave(&fsm->lock, flags);
+
+	/* ------basic driver infomation ---- */
+	t = scnprintf(next, size,
+		      DRIVER_DESC "\n" "fsl_usb2_otg version: %s\n\n",
+		      DRIVER_VERSION);
+	size -= t;
+	next += t;
+
+	/* ------ Registers ----- */
+	tmp_reg = le32_to_cpu(usb_dr_regs->otgsc);
+	t = scnprintf(next, size, "OTGSC reg: %08x\n", tmp_reg);
+	size -= t;
+	next += t;
+
+	tmp_reg = le32_to_cpu(usb_dr_regs->portsc);
+	t = scnprintf(next, size, "PORTSC reg: %08x\n", tmp_reg);
+	size -= t;
+	next += t;
+
+	tmp_reg = le32_to_cpu(usb_dr_regs->usbmode);
+	t = scnprintf(next, size, "USBMODE reg: %08x\n", tmp_reg);
+	size -= t;
+	next += t;
+
+	tmp_reg = le32_to_cpu(usb_dr_regs->usbcmd);
+	t = scnprintf(next, size, "USBCMD reg: %08x\n", tmp_reg);
+	size -= t;
+	next += t;
+
+	tmp_reg = le32_to_cpu(usb_dr_regs->usbsts);
+	t = scnprintf(next, size, "USBSTS reg: %08x\n", tmp_reg);
+	size -= t;
+	next += t;
+
+	/* ------ State ----- */
+	t = scnprintf(next, size,
+		      "OTG state: %s\n\n",
+		      state_string(fsl_otg_dev->otg.state));
+	size -= t;
+	next += t;
+
+#if 1 || defined DEBUG
+	/* ------ State Machine Variables ----- */
+	t = scnprintf(next, size, "a_bus_req: %d\n", fsm->a_bus_req);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "b_bus_req: %d\n", fsm->b_bus_req);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "a_bus_resume: %d\n", fsm->a_bus_resume);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "a_bus_suspend: %d\n", fsm->a_bus_suspend);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "a_conn: %d\n", fsm->a_conn);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "a_sess_vld: %d\n", fsm->a_sess_vld);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "a_srp_det: %d\n", fsm->a_srp_det);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "a_vbus_vld: %d\n", fsm->a_vbus_vld);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "b_bus_resume: %d\n", fsm->b_bus_resume);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "b_bus_suspend: %d\n", fsm->b_bus_suspend);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "b_conn: %d\n", fsm->b_conn);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "b_se0_srp: %d\n", fsm->b_se0_srp);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "b_sess_end: %d\n", fsm->b_sess_end);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "b_sess_vld: %d\n", fsm->b_sess_vld);
+	size -= t;
+	next += t;
+
+	t = scnprintf(next, size, "id: %d\n", fsm->id);
+	size -= t;
+	next += t;
+#endif
+
+	spin_unlock_irqrestore(&fsm->lock, flags);
+
+	*eof = 1;
+	return count - size;
+}
+
+#define create_proc_file()	create_proc_read_entry(proc_filename, \
+				0, NULL, otg_proc_read, NULL)
+
+#define remove_proc_file()	remove_proc_entry(proc_filename, NULL)
+
+#else				/* !CONFIG_USB_OTG_DEBUG_FILES */
+
+#define create_proc_file()	do {} while (0)
+#define remove_proc_file()	do {} while (0)
+
+#endif				/*CONFIG_USB_OTG_DEBUG_FILES */
+
+/*----------------------------------------------------------*/
+/* Char driver interface to control some OTG input */
+
+/* This function handle some ioctl command,such as get otg
+ * status and set host suspend
+ */
+static int fsl_otg_ioctl(struct inode *inode, struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	u32 retval = 0;
+
+	switch (cmd) {
+	case GET_OTG_STATUS:
+		retval = fsl_otg_dev->host_working;
+		break;
+
+	case SET_A_SUSPEND_REQ:
+		fsl_otg_dev->fsm.a_suspend_req = arg;
+		break;
+
+	case SET_A_BUS_DROP:
+		fsl_otg_dev->fsm.a_bus_drop = arg;
+		break;
+
+	case SET_A_BUS_REQ:
+		fsl_otg_dev->fsm.a_bus_req = arg;
+		break;
+
+	case SET_B_BUS_REQ:
+		fsl_otg_dev->fsm.b_bus_req = arg;
+		break;
+
+	default:
+		break;
+	}
+
+	otg_statemachine(&fsl_otg_dev->fsm);
+
+	return retval;
+}
+
+static int fsl_otg_open(struct inode *inode, struct file *file)
+{
+
+	return 0;
+}
+
+static int fsl_otg_release(struct inode *inode, struct file *file)
+{
+
+	return 0;
+}
+
+static struct file_operations otg_fops = {
+	.owner = THIS_MODULE,
+	.llseek = NULL,
+	.read = NULL,
+	.write = NULL,
+	.ioctl = fsl_otg_ioctl,
+	.open = fsl_otg_open,
+	.release = fsl_otg_release,
+};
+
+static int __init fsl_otg_probe(struct platform_device *pdev)
+{
+	int status;
+	struct fsl_usb2_platform_data *pdata;
+
+	DBG("pdev=0x%p\n", pdev);
+
+	if (!pdev)
+		return -ENODEV;
+
+	if (!pdev->dev.platform_data)
+		return -ENOMEM;
+
+	pdata = pdev->dev.platform_data;
+
+	/* configure the OTG */
+	status = fsl_otg_conf(pdev);
+	if (status) {
+		printk(KERN_INFO "Couldn't init OTG module\n");
+		return -status;
+	}
+
+	/* start OTG */
+	status = usb_otg_start(pdev);
+
+	if (register_chrdev(FSL_OTG_MAJOR, FSL_OTG_NAME, &otg_fops)) {
+		printk(KERN_WARNING FSL_OTG_NAME
+		       ": unable to register FSL OTG device\n");
+		return -EIO;
+	}
+
+	create_proc_file();
+	return status;
+}
+
+static int fsl_otg_remove(struct platform_device *pdev)
+{
+	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
+
+	otg_set_transceiver(NULL);
+	free_irq(fsl_otg_dev->irq, fsl_otg_dev);
+
+	iounmap((void *)usb_dr_regs);
+
+	kfree(fsl_otg_dev);
+
+	remove_proc_file();
+
+	unregister_chrdev(FSL_OTG_MAJOR, FSL_OTG_NAME);
+
+	if (pdata->platform_uninit)
+		pdata->platform_uninit(pdata);
+
+	return 0;
+}
+
+struct platform_driver fsl_otg_driver = {
+	.probe = fsl_otg_probe,
+	.remove = fsl_otg_remove,
+	.driver = {
+		.name = driver_name,
+		.owner = THIS_MODULE,
+	},
+};
+
+/*-------------------------------------------------------------------------*/
+
+static int __init fsl_usb_otg_init(void)
+{
+	printk(KERN_INFO DRIVER_DESC " loaded, %s\n", DRIVER_VERSION);
+	return platform_driver_register(&fsl_otg_driver);
+}
+
+static void __exit fsl_usb_otg_exit(void)
+{
+	platform_driver_unregister(&fsl_otg_driver);
+	printk(KERN_INFO DRIVER_DESC " unloaded\n");
+}
+
+module_init(fsl_usb_otg_init);
+module_exit(fsl_usb_otg_exit);
+
+MODULE_DESCRIPTION(DRIVER_INFO);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/otg/fsl_otg.h b/drivers/usb/otg/fsl_otg.h
new file mode 100644
index 000000000000..03e232112b83
--- /dev/null
+++ b/drivers/usb/otg/fsl_otg.h
@@ -0,0 +1,413 @@
+/* Copyright 2005-2009 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the  GNU General Public License along
+ * with this program; if not, write  to the Free Software Foundation, Inc.,
+ * 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "otg_fsm.h"
+#include <linux/usb/otg.h>
+#include <linux/ioctl.h>
+
+ /* USB Command  Register Bit Masks */
+#define USB_CMD_RUN_STOP		(0x1<<0  )
+#define USB_CMD_CTRL_RESET		(0x1<<1  )
+#define USB_CMD_PERIODIC_SCHEDULE_EN	(0x1<<4  )
+#define USB_CMD_ASYNC_SCHEDULE_EN	(0x1<<5  )
+#define USB_CMD_INT_AA_DOORBELL		(0x1<<6  )
+#define USB_CMD_ASP			(0x3<<8  )
+#define USB_CMD_ASYNC_SCH_PARK_EN	(0x1<<11 )
+#define USB_CMD_SUTW			(0x1<<13 )
+#define USB_CMD_ATDTW			(0x1<<14 )
+#define USB_CMD_ITC			(0xFF<<16)
+
+/* bit 15,3,2 are frame list size */
+#define USB_CMD_FRAME_SIZE_1024		(0x0<<15 | 0x0<<2)
+#define USB_CMD_FRAME_SIZE_512		(0x0<<15 | 0x1<<2)
+#define USB_CMD_FRAME_SIZE_256		(0x0<<15 | 0x2<<2)
+#define USB_CMD_FRAME_SIZE_128		(0x0<<15 | 0x3<<2)
+#define USB_CMD_FRAME_SIZE_64		(0x1<<15 | 0x0<<2)
+#define USB_CMD_FRAME_SIZE_32		(0x1<<15 | 0x1<<2)
+#define USB_CMD_FRAME_SIZE_16		(0x1<<15 | 0x2<<2)
+#define USB_CMD_FRAME_SIZE_8		(0x1<<15 | 0x3<<2)
+
+/* bit 9-8 are async schedule park mode count */
+#define USB_CMD_ASP_00			(0x0<<8)
+#define USB_CMD_ASP_01			(0x1<<8)
+#define USB_CMD_ASP_10			(0x2<<8)
+#define USB_CMD_ASP_11			(0x3<<8)
+#define USB_CMD_ASP_BIT_POS		(8)
+
+/* bit 23-16 are interrupt threshold control */
+#define USB_CMD_ITC_NO_THRESHOLD	(0x00<<16)
+#define USB_CMD_ITC_1_MICRO_FRM		(0x01<<16)
+#define USB_CMD_ITC_2_MICRO_FRM		(0x02<<16)
+#define USB_CMD_ITC_4_MICRO_FRM		(0x04<<16)
+#define USB_CMD_ITC_8_MICRO_FRM		(0x08<<16)
+#define USB_CMD_ITC_16_MICRO_FRM	(0x10<<16)
+#define USB_CMD_ITC_32_MICRO_FRM	(0x20<<16)
+#define USB_CMD_ITC_64_MICRO_FRM	(0x40<<16)
+#define USB_CMD_ITC_BIT_POS		(16)
+
+/* USB Status Register Bit Masks */
+#define USB_STS_INT			(0x1<<0 )
+#define USB_STS_ERR			(0x1<<1 )
+#define USB_STS_PORT_CHANGE		(0x1<<2 )
+#define USB_STS_FRM_LST_ROLL		(0x1<<3 )
+#define USB_STS_SYS_ERR			(0x1<<4 )
+#define USB_STS_IAA			(0x1<<5 )
+#define USB_STS_RESET_RECEIVED		(0x1<<6 )
+#define USB_STS_SOF			(0x1<<7 )
+#define USB_STS_DCSUSPEND		(0x1<<8 )
+#define USB_STS_HC_HALTED		(0x1<<12)
+#define USB_STS_RCL			(0x1<<13)
+#define USB_STS_PERIODIC_SCHEDULE	(0x1<<14)
+#define USB_STS_ASYNC_SCHEDULE		(0x1<<15)
+
+/* USB Interrupt Enable Register Bit Masks */
+#define USB_INTR_INT_EN			(0x1<<0 )
+#define USB_INTR_ERR_INT_EN		(0x1<<1 )
+#define USB_INTR_PC_DETECT_EN		(0x1<<2 )
+#define USB_INTR_FRM_LST_ROLL_EN	(0x1<<3 )
+#define USB_INTR_SYS_ERR_EN		(0x1<<4 )
+#define USB_INTR_ASYN_ADV_EN		(0x1<<5 )
+#define USB_INTR_RESET_EN		(0x1<<6 )
+#define USB_INTR_SOF_EN			(0x1<<7 )
+#define USB_INTR_DEVICE_SUSPEND		(0x1<<8 )
+
+/* Device Address bit masks */
+#define USB_DEVICE_ADDRESS_MASK		(0x7F<<25)
+#define USB_DEVICE_ADDRESS_BIT_POS	(25)
+/* PORTSC  Register Bit Masks,Only one PORT in OTG mode*/
+#define PORTSC_CURRENT_CONNECT_STATUS	(0x1<<0)
+#define PORTSC_CONNECT_STATUS_CHANGE	(0x1<<1)
+#define PORTSC_PORT_ENABLE		(0x1<<2)
+#define PORTSC_PORT_EN_DIS_CHANGE	(0x1<<3)
+#define PORTSC_OVER_CURRENT_ACT		(0x1<<4)
+#define PORTSC_OVER_CUURENT_CHG		(0x1<<5)
+#define PORTSC_PORT_FORCE_RESUME	(0x1<<6)
+#define PORTSC_PORT_SUSPEND		(0x1<<7)
+#define PORTSC_PORT_RESET		(0x1<<8)
+#define PORTSC_LINE_STATUS_BITS		(0x3<<10)
+#define PORTSC_PORT_POWER		(0x1<<12)
+#define PORTSC_PORT_INDICTOR_CTRL	(0x3<<14)
+#define PORTSC_PORT_TEST_CTRL		(0xF<<16)
+#define PORTSC_WAKE_ON_CONNECT_EN	(0x1<<20)
+#define PORTSC_WAKE_ON_CONNECT_DIS	(0x1<<21)
+#define PORTSC_WAKE_ON_OVER_CURRENT	(0x1<<22)
+#define PORTSC_PHY_LOW_POWER_SPD	(0x1<<23)
+#define PORTSC_PORT_FORCE_FULL_SPEED	(0x1<<24)
+#define PORTSC_PORT_SPEED_MASK		(0x3<<26)
+#define PORTSC_TRANSCEIVER_WIDTH	(0x1<<28)
+#define PORTSC_PHY_TYPE_SEL		(0x3<<30)
+/* bit 11-10 are line status */
+#define PORTSC_LINE_STATUS_SE0		(0x0<<10)
+#define PORTSC_LINE_STATUS_JSTATE	(0x1<<10)
+#define PORTSC_LINE_STATUS_KSTATE	(0x2<<10)
+#define PORTSC_LINE_STATUS_UNDEF	(0x3<<10)
+#define PORTSC_LINE_STATUS_BIT_POS	(10)
+
+/* bit 15-14 are port indicator control */
+#define PORTSC_PIC_OFF			(0x0<<14)
+#define PORTSC_PIC_AMBER		(0x1<<14)
+#define PORTSC_PIC_GREEN		(0x2<<14)
+#define PORTSC_PIC_UNDEF		(0x3<<14)
+#define PORTSC_PIC_BIT_POS		(14)
+
+/* bit 19-16 are port test control */
+#define PORTSC_PTC_DISABLE		(0x0<<16)
+#define PORTSC_PTC_JSTATE		(0x1<<16)
+#define PORTSC_PTC_KSTATE		(0x2<<16)
+#define PORTSC_PTC_SEQNAK		(0x3<<16)
+#define PORTSC_PTC_PACKET		(0x4<<16)
+#define PORTSC_PTC_FORCE_EN		(0x5<<16)
+#define PORTSC_PTC_BIT_POS		(16)
+
+/* bit 27-26 are port speed */
+#define PORTSC_PORT_SPEED_FULL		(0x0<<26)
+#define PORTSC_PORT_SPEED_LOW		(0x1<<26)
+#define PORTSC_PORT_SPEED_HIGH		(0x2<<26)
+#define PORTSC_PORT_SPEED_UNDEF		(0x3<<26)
+#define PORTSC_SPEED_BIT_POS		(26)
+
+/* bit 28 is parallel transceiver width for UTMI interface */
+#define PORTSC_PTW			(0x1<<28)
+#define PORTSC_PTW_8BIT			(0x0<<28)
+#define PORTSC_PTW_16BIT		(0x1<<28)
+
+/* bit 31-30 are port transceiver select */
+#define PORTSC_PTS_UTMI			(0x0<<30)
+#define PORTSC_PTS_ULPI			(0x2<<30)
+#define PORTSC_PTS_FSLS_SERIAL		(0x3<<30)
+#define PORTSC_PTS_BIT_POS		(30)
+
+#define PORTSC_W1C_BITS                    \
+       (PORTSC_CONNECT_STATUS_CHANGE |     \
+	PORTSC_PORT_EN_DIS_CHANGE    |     \
+	PORTSC_OVER_CUURENT_CHG)
+
+/* OTG Status Control Register Bit Masks */
+#define OTGSC_CTRL_VBUS_DISCHARGE	(0x1<<0)
+#define OTGSC_CTRL_VBUS_CHARGE		(0x1<<1)
+#define OTGSC_CTRL_OTG_TERMINATION	(0x1<<3)
+#define OTGSC_CTRL_DATA_PULSING		(0x1<<4)
+#define OTGSC_CTRL_ID_PULL_EN		(0x1<<5)
+#define OTGSC_HA_DATA_PULSE		(0x1<<6)
+#define OTGSC_HA_BA			(0x1<<7)
+#define OTGSC_STS_USB_ID		(0x1<<8)
+#define OTGSC_STS_A_VBUS_VALID		(0x1<<9)
+#define OTGSC_STS_A_SESSION_VALID	(0x1<<10)
+#define OTGSC_STS_B_SESSION_VALID	(0x1<<11)
+#define OTGSC_STS_B_SESSION_END		(0x1<<12)
+#define OTGSC_STS_1MS_TOGGLE		(0x1<<13)
+#define OTGSC_STS_DATA_PULSING		(0x1<<14)
+#define OTGSC_INTSTS_USB_ID		(0x1<<16)
+#define OTGSC_INTSTS_A_VBUS_VALID	(0x1<<17)
+#define OTGSC_INTSTS_A_SESSION_VALID	(0x1<<18)
+#define OTGSC_INTSTS_B_SESSION_VALID	(0x1<<19)
+#define OTGSC_INTSTS_B_SESSION_END	(0x1<<20)
+#define OTGSC_INTSTS_1MS		(0x1<<21)
+#define OTGSC_INTSTS_DATA_PULSING	(0x1<<22)
+#define OTGSC_INTR_USB_ID_EN		(0x1<<24)
+#define OTGSC_INTR_A_VBUS_VALID_EN	(0x1<<25)
+#define OTGSC_INTR_A_SESSION_VALID_EN	(0x1<<26)
+#define OTGSC_INTR_B_SESSION_VALID_EN	(0x1<<27)
+#define OTGSC_INTR_B_SESSION_END_EN	(0x1<<28)
+#define OTGSC_INTR_1MS_TIMER_EN		(0x1<<29)
+#define OTGSC_INTR_DATA_PULSING_EN	(0x1<<30)
+#define OTGSC_INTSTS_MASK		(0x00ff0000)
+
+/* USB MODE Register Bit Masks */
+#define  USB_MODE_CTRL_MODE_IDLE	(0x0<<0)
+#define  USB_MODE_CTRL_MODE_DEVICE	(0x2<<0)
+#define  USB_MODE_CTRL_MODE_HOST	(0x3<<0)
+#define  USB_MODE_CTRL_MODE_RSV		(0x1<<0)
+#define  USB_MODE_SETUP_LOCK_OFF	(0x1<<3)
+#define  USB_MODE_STREAM_DISABLE	(0x1<<4)
+#define  USB_MODE_ES			(0x1<<2) /* (big) Endian Select */
+
+#define MPC8349_OTG_IRQ			(38)
+#define CFG_IMMR_BASE	        	(0xfe000000)
+#define MPC83xx_USB_DR_BASE     	(CFG_IMMR_BASE + 0x23000)
+
+/* control Register Bit Masks */
+#define  USB_CTRL_IOENB			(0x1<<2)
+#define  USB_CTRL_ULPI_INT0EN		(0x1<<0)
+
+/* BCSR5 */
+#define BCSR5_INT_USB			(0x02)
+
+/* USB module clk cfg */
+#define SCCR_OFFS			(0xA08)
+#define SCCR_USB_CLK_DISABLE		(0x00000000)	/* USB clk disable */
+#define SCCR_USB_MPHCM_11		(0x00c00000)
+#define SCCR_USB_MPHCM_01		(0x00400000)
+#define SCCR_USB_MPHCM_10		(0x00800000)
+#define SCCR_USB_DRCM_11		(0x00300000)
+#define SCCR_USB_DRCM_01		(0x00100000)
+#define SCCR_USB_DRCM_10		(0x00200000)
+
+#define SICRL_OFFS			(0x114)
+#define SICRL_USB0			(0x40000000)
+#define SICRL_USB1			(0x20000000)
+
+#define SICRH_OFFS			(0x118)
+#define SICRH_USB_UTMI			(0x00020000)
+
+/* OTG interrupt enable bit masks */
+#define  OTGSC_INTERRUPT_ENABLE_BITS_MASK  \
+	(OTGSC_INTR_USB_ID_EN            | \
+	OTGSC_INTR_1MS_TIMER_EN          | \
+	OTGSC_INTR_A_VBUS_VALID_EN       | \
+	OTGSC_INTR_A_SESSION_VALID_EN    | \
+	OTGSC_INTR_B_SESSION_VALID_EN    | \
+	OTGSC_INTR_B_SESSION_END_EN      | \
+	OTGSC_INTR_DATA_PULSING_EN)
+
+/* OTG interrupt status bit masks */
+#define  OTGSC_INTERRUPT_STATUS_BITS_MASK  \
+	(OTGSC_INTSTS_USB_ID          |    \
+	OTGSC_INTR_1MS_TIMER_EN       |    \
+	OTGSC_INTSTS_A_VBUS_VALID     |    \
+	OTGSC_INTSTS_A_SESSION_VALID  |    \
+	OTGSC_INTSTS_B_SESSION_VALID  |    \
+	OTGSC_INTSTS_B_SESSION_END    |    \
+	OTGSC_INTSTS_DATA_PULSING)
+
+/*
+ *  A-DEVICE timing  constants
+ */
+
+/* Wait for VBUS Rise  */
+#define TA_WAIT_VRISE	(100)	/* a_wait_vrise 100 ms, section: 6.6.5.1 */
+
+/* Wait for B-Connect */
+#define TA_WAIT_BCON	(10000)  /* a_wait_bcon > 1 sec, section: 6.6.5.2
+				  * This is only used to get out of
+				  * OTG_STATE_A_WAIT_BCON state if there was
+				  * no connection for these many milliseconds
+				  */
+
+/* A-Idle to B-Disconnect */
+/* It is necessary for this timer to be more than 750 ms because of a bug in OPT
+ * test 5.4 in which B OPT disconnects after 750 ms instead of 75ms as stated
+ * in the test description
+ */
+#define TA_AIDL_BDIS	(5000)	/* a_suspend minimum 200 ms, section: 6.6.5.3 */
+
+/* B-Idle to A-Disconnect */
+#define TA_BIDL_ADIS	(12)	/* 3 to 200 ms */
+
+/* B-device timing constants */
+
+
+/* Data-Line Pulse Time*/
+#define TB_DATA_PLS	(10)	/* b_srp_init,continue 5~10ms, section:5.3.3 */
+#define TB_DATA_PLS_MIN	(5)	/* minimum 5 ms */
+#define TB_DATA_PLS_MAX	(10)	/* maximum 10 ms */
+
+/* SRP Initiate Time  */
+#define TB_SRP_INIT	(100)	/* b_srp_init,maximum 100 ms, section:5.3.8 */
+
+/* SRP Fail Time  */
+#define TB_SRP_FAIL	(7000)	/* b_srp_init,Fail time 5~30s, section:6.8.2.2*/
+
+/* SRP result wait time */
+#define TB_SRP_WAIT	(60)
+
+/* VBus time */
+#define TB_VBUS_PLS	(30)	/* time to keep vbus pulsing asserted */
+
+/* Discharge time */
+/* This time should be less than 10ms. It varies from system to system. */
+#define TB_VBUS_DSCHRG	(8)
+
+/* A-SE0 to B-Reset  */
+#define TB_ASE0_BRST	(20)	/* b_wait_acon, mini 3.125 ms,section:6.8.2.4 */
+
+/* A bus suspend timer before we can switch to b_wait_aconn */
+#define TB_A_SUSPEND	(7)
+#define TB_BUS_RESUME	(12)
+
+/* SE0 Time Before SRP */
+#define TB_SE0_SRP	(2)	/* b_idle,minimum 2 ms, section:5.3.2 */
+
+
+#define SET_OTG_STATE(otg_ptr, newstate)	((otg_ptr)->state=newstate)
+
+struct usb_dr_mmap {
+	/* Capability register */
+	u8 res1[256];
+	u16 caplength;		/* Capability Register Length */
+	u16 hciversion;		/* Host Controller Interface Version */
+	u32 hcsparams;		/* Host Controller Structual Parameters */
+	u32 hccparams;		/* Host Controller Capability Parameters */
+	u8 res2[20];
+	u32 dciversion;		/* Device Controller Interface Version */
+	u32 dccparams;		/* Device Controller Capability Parameters */
+	u8 res3[24];
+	/* Operation register */
+	u32 usbcmd;		/* USB Command Register */
+	u32 usbsts;		/* USB Status Register */
+	u32 usbintr;		/* USB Interrupt Enable Register */
+	u32 frindex;		/* Frame Index Register */
+	u8 res4[4];
+	u32 deviceaddr;		/* Device Address */
+	u32 endpointlistaddr;	/* Endpoint List Address Register */
+	u8 res5[4];
+	u32 burstsize;		/* Master Interface Data Burst Size Register */
+	u32 txttfilltuning;	/* Transmit FIFO Tuning Controls Register */
+	u8 res6[8];
+	u32 ulpiview;		/* ULPI register access */
+	u8 res7[12];
+	u32 configflag;		/* Configure Flag Register */
+	u32 portsc;		/* Port 1 Status and Control Register */
+	u8 res8[28];
+	u32 otgsc;		/* On-The-Go Status and Control */
+	u32 usbmode;		/* USB Mode Register */
+	u32 endptsetupstat;	/* Endpoint Setup Status Register */
+	u32 endpointprime;	/* Endpoint Initialization Register */
+	u32 endptflush;		/* Endpoint Flush Register */
+	u32 endptstatus;	/* Endpoint Status Register */
+	u32 endptcomplete;	/* Endpoint Complete Register */
+	u32 endptctrl[6];	/* Endpoint Control Registers */
+	u8 res9[552];
+	u32 snoop1;
+	u32 snoop2;
+	u32 age_cnt_thresh;	/* Age Count Threshold Register */
+	u32 pri_ctrl;		/* Priority Control Register */
+	u32 si_ctrl;		/* System Interface Control Register */
+	u8 res10[236];
+#ifdef CONFIG_ARCH_MX51
+	u32 res11[128];
+#endif
+	u32 control;		/* General Purpose Control Register */
+};
+
+
+struct fsl_otg_timer {
+	unsigned long expires;	/* Number of count increase to timeout */
+	unsigned long count;	/* Tick counter */
+	void (*function)(unsigned long);	/* Timeout function */
+	unsigned long data;	/* Data passed to function */
+	struct list_head list;
+};
+
+struct fsl_otg_timer inline *otg_timer_initializer
+(void (*function)(unsigned long), unsigned long expires, unsigned long data)
+{
+	struct fsl_otg_timer *timer;
+	timer = kmalloc(sizeof(struct fsl_otg_timer), GFP_KERNEL);
+	if (timer == NULL)
+		return NULL;
+	timer->function = function;
+	timer->expires = expires;
+	timer->data = data;
+	return timer;
+}
+
+struct fsl_otg {
+	struct otg_transceiver otg;
+	struct otg_fsm fsm;
+	struct usb_dr_mmap *dr_mem_map;
+	struct delayed_work otg_event;
+
+	/*used for usb host */
+	struct work_struct work_wq;
+	u8	host_working;
+
+	int irq;
+};
+
+struct fsl_otg_config {
+	u8 otg_port;
+};
+
+/*For SRP and HNP handle*/
+#define FSL_OTG_MAJOR	66
+#define FSL_OTG_NAME	"fsl-usb2-otg"
+/*Command to OTG driver(ioctl)*/
+#define OTG_IOCTL_MAGIC		FSL_OTG_MAJOR
+/*if otg work as host,it should return 1,otherwise it return 0*/
+#define GET_OTG_STATUS		_IOR(OTG_IOCTL_MAGIC, 1, int)
+#define SET_A_SUSPEND_REQ	_IOW(OTG_IOCTL_MAGIC, 2, int)
+#define SET_A_BUS_DROP		_IOW(OTG_IOCTL_MAGIC, 3, int)
+#define SET_A_BUS_REQ		_IOW(OTG_IOCTL_MAGIC, 4, int)
+#define SET_B_BUS_REQ		_IOW(OTG_IOCTL_MAGIC, 5, int)
+#define GET_A_SUSPEND_REQ	_IOR(OTG_IOCTL_MAGIC, 6, int)
+#define GET_A_BUS_DROP		_IOR(OTG_IOCTL_MAGIC, 7, int)
+#define GET_A_BUS_REQ		_IOR(OTG_IOCTL_MAGIC, 8, int)
+#define GET_B_BUS_REQ		_IOR(OTG_IOCTL_MAGIC, 9, int)
+
+extern const char *state_string(enum usb_otg_state state);
+extern int otg_set_resources(struct resource *resources);
diff --git a/drivers/usb/otg/otg_fsm.c b/drivers/usb/otg/otg_fsm.c
new file mode 100644
index 000000000000..955b21cdd609
--- /dev/null
+++ b/drivers/usb/otg/otg_fsm.c
@@ -0,0 +1,371 @@
+/* OTG Finite State Machine from OTG spec
+ *
+ * Copyright (C) 2006-2008 Freescale Semiconductor, Inc.
+ *
+ * Author: 	Li Yang <LeoLi@freescale.com>
+ * 		Jerry Huang <Chang-Ming.Huang@freescale.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the  GNU General Public License along
+ * with this program; if not, write  to the Free Software Foundation, Inc.,
+ * 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/usb/otg.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/usb.h>
+#include <linux/usb/gadget.h>
+
+#include <asm/types.h>
+#include "otg_fsm.h"
+
+
+/* Defined by device specific driver, for different timer implementation */
+extern void *a_wait_vrise_tmr, *a_wait_bcon_tmr, *a_aidl_bdis_tmr,
+	*b_ase0_brst_tmr, *b_se0_srp_tmr, *b_srp_fail_tmr, *a_wait_enum_tmr;
+
+const char *state_string(enum usb_otg_state state)
+{
+	switch (state) {
+	case OTG_STATE_A_IDLE:		return "a_idle";
+	case OTG_STATE_A_WAIT_VRISE:	return "a_wait_vrise";
+	case OTG_STATE_A_WAIT_BCON:	return "a_wait_bcon";
+	case OTG_STATE_A_HOST:		return "a_host";
+	case OTG_STATE_A_SUSPEND:	return "a_suspend";
+	case OTG_STATE_A_PERIPHERAL:	return "a_peripheral";
+	case OTG_STATE_A_WAIT_VFALL:	return "a_wait_vfall";
+	case OTG_STATE_A_VBUS_ERR:	return "a_vbus_err";
+	case OTG_STATE_B_IDLE:		return "b_idle";
+	case OTG_STATE_B_SRP_INIT:	return "b_srp_init";
+	case OTG_STATE_B_PERIPHERAL:	return "b_peripheral";
+	case OTG_STATE_B_WAIT_ACON:	return "b_wait_acon";
+	case OTG_STATE_B_HOST:		return "b_host";
+	default:			return "UNDEFINED";
+	}
+}
+
+/* Change USB protocol when there is a protocol change */
+static int otg_set_protocol(struct otg_fsm *fsm, int protocol)
+{
+	int ret = 0;
+
+	if (fsm->protocol != protocol) {
+		VDBG("Changing role fsm->protocol= %d; new protocol= %d\n",
+				fsm->protocol, protocol);
+		/* stop old protocol */
+		if (fsm->protocol == PROTO_HOST)
+			ret = fsm->ops->start_host(fsm, 0);
+		else if (fsm->protocol == PROTO_GADGET)
+			ret = fsm->ops->start_gadget(fsm, 0);
+		if (ret)
+			return ret;
+
+		/* start new protocol */
+		if (protocol == PROTO_HOST)
+			ret = fsm->ops->start_host(fsm, 1);
+		else if (protocol == PROTO_GADGET)
+			ret = fsm->ops->start_gadget(fsm, 1);
+		if (ret)
+			return ret;
+
+		fsm->protocol = protocol;
+		return 0;
+	}
+
+	return 0;
+}
+
+static int state_changed;
+
+/* Called when leaving a state.  Do state clean up jobs here */
+void otg_leave_state(struct otg_fsm *fsm, enum usb_otg_state old_state)
+{
+	switch (old_state) {
+	case OTG_STATE_B_IDLE:
+		otg_del_timer(fsm, b_se0_srp_tmr);
+		fsm->b_se0_srp = 0;
+		break;
+	case OTG_STATE_B_SRP_INIT:
+		fsm->b_srp_done = 0;
+		break;
+	case OTG_STATE_B_PERIPHERAL:
+		break;
+	case OTG_STATE_B_WAIT_ACON:
+		otg_del_timer(fsm, b_ase0_brst_tmr);
+		fsm->b_ase0_brst_tmout = 0;
+		break;
+	case OTG_STATE_B_HOST:
+		break;
+	case OTG_STATE_A_IDLE:
+		break;
+	case OTG_STATE_A_WAIT_VRISE:
+		otg_del_timer(fsm, a_wait_vrise_tmr);
+		fsm->a_wait_vrise_tmout = 0;
+		break;
+	case OTG_STATE_A_WAIT_BCON:
+		otg_del_timer(fsm, a_wait_bcon_tmr);
+		fsm->a_wait_bcon_tmout = 0;
+		break;
+	case OTG_STATE_A_HOST:
+		otg_del_timer(fsm, a_wait_enum_tmr);
+		break;
+	case OTG_STATE_A_SUSPEND:
+		otg_del_timer(fsm, a_aidl_bdis_tmr);
+		fsm->a_aidl_bdis_tmout = 0;
+		fsm->a_suspend_req = 0;
+		break;
+	case OTG_STATE_A_PERIPHERAL:
+		break;
+	case OTG_STATE_A_WAIT_VFALL:
+		otg_del_timer(fsm, a_wait_vrise_tmr);
+		break;
+	case OTG_STATE_A_VBUS_ERR:
+		break;
+	default:
+		break;
+	}
+}
+
+/* Called when entering a state */
+int otg_set_state(struct otg_fsm *fsm, enum usb_otg_state new_state)
+{
+	state_changed = 1;
+	if (fsm->transceiver->state == new_state)
+		return 0;
+	VDBG("Set state: %s \n", state_string(new_state));
+	otg_leave_state(fsm, fsm->transceiver->state);
+	switch (new_state) {
+	case OTG_STATE_B_IDLE:
+		otg_drv_vbus(fsm, 0);
+		otg_chrg_vbus(fsm, 0);
+		otg_loc_conn(fsm, 0);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_UNDEF);
+		otg_add_timer(fsm, b_se0_srp_tmr);
+		break;
+	case OTG_STATE_B_SRP_INIT:
+		otg_start_pulse(fsm);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_UNDEF);
+		otg_add_timer(fsm, b_srp_fail_tmr);
+		break;
+	case OTG_STATE_B_PERIPHERAL:
+		otg_chrg_vbus(fsm, 0);
+		otg_loc_conn(fsm, 1);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_GADGET);
+		break;
+	case OTG_STATE_B_WAIT_ACON:
+		otg_chrg_vbus(fsm, 0);
+		otg_loc_conn(fsm, 0);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_HOST);
+		otg_add_timer(fsm, b_ase0_brst_tmr);
+		fsm->a_bus_suspend = 0;
+		break;
+	case OTG_STATE_B_HOST:
+		otg_chrg_vbus(fsm, 0);
+		otg_loc_conn(fsm, 0);
+		otg_loc_sof(fsm, 1);
+		otg_set_protocol(fsm, PROTO_HOST);
+		usb_bus_start_enum(fsm->transceiver->host,
+				fsm->transceiver->host->otg_port);
+		break;
+	case OTG_STATE_A_IDLE:
+		otg_drv_vbus(fsm, 0);
+		otg_chrg_vbus(fsm, 0);
+		otg_loc_conn(fsm, 0);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_HOST);
+		break;
+	case OTG_STATE_A_WAIT_VRISE:
+		otg_drv_vbus(fsm, 1);
+		otg_loc_conn(fsm, 0);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_HOST);
+		otg_add_timer(fsm, a_wait_vrise_tmr);
+		break;
+	case OTG_STATE_A_WAIT_BCON:
+		otg_drv_vbus(fsm, 1);
+		otg_loc_conn(fsm, 0);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_HOST);
+		otg_add_timer(fsm, a_wait_bcon_tmr);
+		break;
+	case OTG_STATE_A_HOST:
+		otg_drv_vbus(fsm, 1);
+		otg_loc_conn(fsm, 0);
+		otg_loc_sof(fsm, 1);
+		otg_set_protocol(fsm, PROTO_HOST);
+		/* When HNP is triggered while a_bus_req = 0, a_host will
+		 * suspend too fast to complete a_set_b_hnp_en */
+		if (!fsm->a_bus_req || fsm->a_suspend_req)
+			otg_add_timer(fsm, a_wait_enum_tmr);
+		break;
+	case OTG_STATE_A_SUSPEND:
+		otg_drv_vbus(fsm, 1);
+		otg_loc_conn(fsm, 0);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_HOST);
+		otg_add_timer(fsm, a_aidl_bdis_tmr);
+
+		break;
+	case OTG_STATE_A_PERIPHERAL:
+		otg_loc_conn(fsm, 1);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_GADGET);
+		otg_drv_vbus(fsm, 1);
+		break;
+	case OTG_STATE_A_WAIT_VFALL:
+		otg_drv_vbus(fsm, 0);
+		otg_loc_conn(fsm, 0);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_HOST);
+		break;
+	case OTG_STATE_A_VBUS_ERR:
+		otg_drv_vbus(fsm, 0);
+		otg_loc_conn(fsm, 0);
+		otg_loc_sof(fsm, 0);
+		otg_set_protocol(fsm, PROTO_UNDEF);
+		break;
+	default:
+		break;
+	}
+
+	fsm->transceiver->state = new_state;
+	return 0;
+}
+
+/* State change judgement */
+int otg_statemachine(struct otg_fsm *fsm)
+{
+	enum usb_otg_state state;
+	unsigned long flags;
+
+	spin_lock_irqsave(&fsm->lock, flags);
+
+	state = fsm->transceiver->state;
+	state_changed = 0;
+	/* State machine state change judgement */
+
+	switch (state) {
+	case OTG_STATE_UNDEFINED:
+		VDBG("fsm->id = %d \n", fsm->id);
+		if (fsm->id)
+			otg_set_state(fsm, OTG_STATE_B_IDLE);
+		else
+			otg_set_state(fsm, OTG_STATE_A_IDLE);
+		break;
+	case OTG_STATE_B_IDLE:
+		if (!fsm->id)
+			otg_set_state(fsm, OTG_STATE_A_IDLE);
+		else if (fsm->b_sess_vld && fsm->transceiver->gadget)
+			otg_set_state(fsm, OTG_STATE_B_PERIPHERAL);
+		else if (fsm->b_bus_req && fsm->b_sess_end && fsm->b_se0_srp)
+			otg_set_state(fsm, OTG_STATE_B_SRP_INIT);
+		break;
+	case OTG_STATE_B_SRP_INIT:
+		if (!fsm->id || fsm->b_srp_done)
+			otg_set_state(fsm, OTG_STATE_B_IDLE);
+		break;
+	case OTG_STATE_B_PERIPHERAL:
+		if (!fsm->id || !fsm->b_sess_vld)
+			otg_set_state(fsm, OTG_STATE_B_IDLE);
+		else if (fsm->b_bus_req && fsm->transceiver->
+				gadget->b_hnp_enable && fsm->a_bus_suspend)
+			otg_set_state(fsm, OTG_STATE_B_WAIT_ACON);
+		break;
+	case OTG_STATE_B_WAIT_ACON:
+		if (fsm->a_conn)
+			otg_set_state(fsm, OTG_STATE_B_HOST);
+		else if (!fsm->id || !fsm->b_sess_vld)
+			otg_set_state(fsm, OTG_STATE_B_IDLE);
+		else if (fsm->a_bus_resume || fsm->b_ase0_brst_tmout) {
+			fsm->b_ase0_brst_tmout = 0;
+			otg_set_state(fsm, OTG_STATE_B_PERIPHERAL);
+		}
+		break;
+	case OTG_STATE_B_HOST:
+		if (!fsm->id || !fsm->b_sess_vld)
+			otg_set_state(fsm, OTG_STATE_B_IDLE);
+		else if (!fsm->b_bus_req || !fsm->a_conn)
+			otg_set_state(fsm, OTG_STATE_B_PERIPHERAL);
+		break;
+	case OTG_STATE_A_IDLE:
+		if (fsm->id)
+			otg_set_state(fsm, OTG_STATE_B_IDLE);
+		else if (!fsm->a_bus_drop && (fsm->a_bus_req || fsm->a_srp_det))
+			otg_set_state(fsm, OTG_STATE_A_WAIT_VRISE);
+		break;
+	case OTG_STATE_A_WAIT_VRISE:
+		if (fsm->id || fsm->a_bus_drop || fsm->a_vbus_vld ||
+				fsm->a_wait_vrise_tmout) {
+			otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
+		}
+		break;
+	case OTG_STATE_A_WAIT_BCON:
+		if (!fsm->a_vbus_vld)
+			otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
+		else if (fsm->b_conn)
+			otg_set_state(fsm, OTG_STATE_A_HOST);
+		else if (fsm->id | fsm->a_bus_drop | fsm->a_wait_bcon_tmout)
+			otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
+		break;
+	case OTG_STATE_A_HOST:
+		if ((!fsm->a_bus_req || fsm->a_suspend_req) &&
+				fsm->transceiver->host->b_hnp_enable)
+			otg_set_state(fsm, OTG_STATE_A_SUSPEND);
+		else if (fsm->id || !fsm->b_conn || fsm->a_bus_drop)
+			otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
+		else if (!fsm->a_vbus_vld)
+			otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
+		break;
+	case OTG_STATE_A_SUSPEND:
+		if (!fsm->b_conn && fsm->transceiver->host->b_hnp_enable)
+			otg_set_state(fsm, OTG_STATE_A_PERIPHERAL);
+		else if (!fsm->b_conn && !fsm->transceiver->host->b_hnp_enable)
+			otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
+		else if (fsm->a_bus_req || fsm->b_bus_resume)
+			otg_set_state(fsm, OTG_STATE_A_HOST);
+		else if (fsm->id || fsm->a_bus_drop || fsm->a_aidl_bdis_tmout)
+			otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
+		else if (!fsm->a_vbus_vld)
+			otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
+		break;
+	case OTG_STATE_A_PERIPHERAL:
+		if (fsm->id || fsm->a_bus_drop)
+			otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
+		else if (fsm->b_bus_suspend)
+			otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
+		else if (!fsm->a_vbus_vld)
+			otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
+		break;
+	case OTG_STATE_A_WAIT_VFALL:
+		if (fsm->id || fsm->a_bus_req || (!fsm->a_sess_vld &&
+					!fsm->b_conn))
+			otg_set_state(fsm, OTG_STATE_A_IDLE);
+		break;
+	case OTG_STATE_A_VBUS_ERR:
+		if (fsm->id || fsm->a_bus_drop || fsm->a_clr_err)
+			otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
+		break;
+	default:
+		break;
+	}
+	spin_unlock_irqrestore(&fsm->lock, flags);
+
+	/*	VDBG("quit statemachine, changed = %d \n", state_changed); */
+	return state_changed;
+}
diff --git a/drivers/usb/otg/otg_fsm.h b/drivers/usb/otg/otg_fsm.h
new file mode 100644
index 000000000000..cd6894c80403
--- /dev/null
+++ b/drivers/usb/otg/otg_fsm.h
@@ -0,0 +1,151 @@
+/* Copyright (C) 2006-2008 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the  GNU General Public License along
+ * with this program; if not, write  to the Free Software Foundation, Inc.,
+ * 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#if 0
+#define DEBUG 	1
+#define VERBOSE	1
+#endif
+
+#ifdef DEBUG
+#define DBG(fmt, args...) printk(KERN_DEBUG "j=%lu [%s] " fmt "\n", jiffies, \
+				 __func__, ## args)
+#else
+#define DBG(fmt, args...)	do {} while (0)
+#endif
+
+#ifdef VERBOSE
+#define VDBG		DBG
+#else
+#define VDBG(stuff...)	do {} while (0)
+#endif
+
+#ifdef VERBOSE
+#define MPC_LOC printk("Current Location [%s]:[%d]\n", __FILE__, __LINE__)
+#else
+#define MPC_LOC do {} while (0)
+#endif
+
+#define PROTO_UNDEF	(0)
+#define PROTO_HOST	(1)
+#define PROTO_GADGET	(2)
+
+/* OTG state machine according to the OTG spec */
+struct otg_fsm {
+	/* Input */
+	int a_bus_resume;
+	int a_bus_suspend;
+	int a_conn;
+	int a_sess_vld;
+	int a_srp_det;
+	int a_vbus_vld;
+	int b_bus_resume;
+	int b_bus_suspend;
+	int b_conn;
+	int b_se0_srp;
+	int b_sess_end;
+	int b_sess_vld;
+	int id;
+
+	/* Internal variables */
+	int a_set_b_hnp_en;
+	int b_srp_done;
+	int b_hnp_enable;
+
+	/* Timeout indicator for timers */
+	int a_wait_vrise_tmout;
+	int a_wait_bcon_tmout;
+	int a_aidl_bdis_tmout;
+	int b_ase0_brst_tmout;
+
+	/* Informative variables */
+	int a_bus_drop;
+	int a_bus_req;
+	int a_clr_err;
+	int a_suspend_req;
+	int b_bus_req;
+
+	/* Output */
+	int drv_vbus;
+	int loc_conn;
+	int loc_sof;
+
+	struct otg_fsm_ops *ops;
+	struct otg_transceiver *transceiver;
+
+	/* Current usb protocol used: 0:undefine; 1:host; 2:client */
+	int protocol;
+	spinlock_t lock;
+};
+
+struct otg_fsm_ops {
+	void	(*chrg_vbus)(int on);
+	void	(*drv_vbus)(int on);
+	void	(*loc_conn)(int on);
+	void	(*loc_sof)(int on);
+	void	(*start_pulse)(void);
+	void	(*add_timer)(void *timer);
+	void	(*del_timer)(void *timer);
+	int	(*start_host)(struct otg_fsm *fsm, int on);
+	int	(*start_gadget)(struct otg_fsm *fsm, int on);
+};
+
+
+static inline void otg_chrg_vbus(struct otg_fsm *fsm, int on)
+{
+	fsm->ops->chrg_vbus(on);
+}
+
+static inline void otg_drv_vbus(struct otg_fsm *fsm, int on)
+{
+	if (fsm->drv_vbus != on) {
+		fsm->drv_vbus = on;
+		fsm->ops->drv_vbus(on);
+	}
+}
+
+static inline void otg_loc_conn(struct otg_fsm *fsm, int on)
+{
+	if (fsm->loc_conn != on) {
+		fsm->loc_conn = on;
+		fsm->ops->loc_conn(on);
+	}
+}
+
+static inline void otg_loc_sof(struct otg_fsm *fsm, int on)
+{
+	if (fsm->loc_sof != on) {
+		fsm->loc_sof = on;
+		fsm->ops->loc_sof(on);
+	}
+}
+
+static inline void otg_start_pulse(struct otg_fsm *fsm)
+{
+	fsm->ops->start_pulse();
+}
+
+static inline void otg_add_timer(struct otg_fsm *fsm, void *timer)
+{
+	fsm->ops->add_timer(timer);
+}
+
+static inline void otg_del_timer(struct otg_fsm *fsm, void *timer)
+{
+	fsm->ops->del_timer(timer);
+}
+
+int otg_statemachine(struct otg_fsm *fsm);
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 3f3ce13fef43..095263ba8bc7 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -381,6 +381,10 @@ config FB_CLPS711X
 	  Say Y to enable the Framebuffer driver for the CLPS7111 and
 	  EP7212 processors.
 
+if ARCH_MXC
+source "drivers/video/mxc/Kconfig"
+endif
+
 config FB_SA1100
 	bool "SA-1100 LCD support"
 	depends on (FB = y) && ARM && ARCH_SA1100
@@ -1940,6 +1944,16 @@ config FB_S3C2410_DEBUG
 	help
 	  Turn on debugging messages. Note that you can set/unset at run time
 	  through sysfs
+	  
+config FB_S3C2443_TFT
+        tristate "S3C2443 TFT LCD framebuffer support"
+        depends on FB && CPU_S3C2443
+        select FB_CFB_FILLRECT
+        select FB_CFB_COPYAREA
+        select FB_CFB_IMAGEBLIT	
+        default y
+        ---help---
+          Add the support for TFT-LCD displays on the Samsung S3C2443 platforms
 
 config FB_SM501
 	tristate "Silicon Motion SM501 framebuffer support"
@@ -1974,6 +1988,16 @@ config FB_PNX4008_DUM_RGB
 	---help---
 	  Say Y here to enable support for PNX4008 RGB Framebuffer
 
+config FB_STMP37XX
+	tristate "STMP 37XX LCD Framebuffer driver"
+	depends on FB && (ARCH_STMP37XX || ARCH_STMP378X)
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	---help---
+	  Say Y here to enable support for the framebuffer driver for the
+	  Sigmatel STMP37XX board.
+
 config FB_IBM_GXT4500
 	tristate "Framebuffer support for IBM GXT4500P adaptor"
 	depends on FB && PPC
@@ -2033,6 +2057,29 @@ config FB_SH7760
          and 8, 15 or 16 bpp color; 90 degrees clockwise display rotation for
          panels <= 320 pixel horizontal resolution.
 
+config FB_NS9360
+	tristate "NS9360 LCD framebuffer support"
+	depends on FB && PROCESSOR_NS9360
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	help
+	  This enables support for the display controller inside the
+	  Digi ns9360 CPUs.
+
+config FB_HX8347
+	tristate "HX8347 LCD framebuffer support"
+	depends on FB
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select FB_SYS_FOPS
+	select FB_DEFERRED_IO
+
+	help
+	  This enables support for the HX8347 display controller.
+	  Only the register interface mode is supported.
+
 config FB_VIRTUAL
 	tristate "Virtual Frame Buffer support (ONLY FOR TESTING!)"
 	depends on FB
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index e39e33e797da..fb28506f761d 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -110,10 +110,13 @@ obj-$(CONFIG_FB_S1D13XXX)	  += s1d13xxxfb.o
 obj-$(CONFIG_FB_SH7760)		  += sh7760fb.o
 obj-$(CONFIG_FB_IMX)              += imxfb.o
 obj-$(CONFIG_FB_S3C2410)	  += s3c2410fb.o
+obj-$(CONFIG_FB_S3C2443_TFT)      += s3c2443fb_tft.o
 obj-$(CONFIG_FB_FSL_DIU)	  += fsl-diu-fb.o
 obj-$(CONFIG_FB_COBALT)           += cobalt_lcdfb.o
 obj-$(CONFIG_FB_PNX4008_DUM)	  += pnx4008/
 obj-$(CONFIG_FB_PNX4008_DUM_RGB)  += pnx4008/
+obj-$(CONFIG_FB_MXC)		  += mxc/
+obj-$(CONFIG_FB_STMP37XX)	  += stmp37xxfb.o
 obj-$(CONFIG_FB_IBM_GXT4500)	  += gxt4500.o
 obj-$(CONFIG_FB_PS3)		  += ps3fb.o
 obj-$(CONFIG_FB_SM501)            += sm501fb.o
@@ -123,6 +126,8 @@ obj-$(CONFIG_FB_OMAP)             += omap/
 obj-$(CONFIG_XEN_FBDEV_FRONTEND)  += xen-fbfront.o
 obj-$(CONFIG_FB_CARMINE)          += carminefb.o
 obj-$(CONFIG_FB_MB862XX)	  += mb862xx/
+obj-$(CONFIG_FB_NS9360)		  += ns9360fb.o
+obj-$(CONFIG_FB_HX8347)		  += hx8347fb.o
 
 # Platform or fallback drivers go here
 obj-$(CONFIG_FB_UVESA)            += uvesafb.o
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index 4a4dd9adc328..c24696c0f7a7 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -214,3 +214,44 @@ config BACKLIGHT_SAHARA
 	help
 	  If you have a Tabletkiosk Sahara Touch-iT, say y to enable the
 	  backlight driver.
+
+menuconfig BACKLIGHT_MXC
+	bool "Freescale MXC/i.MX Backlight Drivers"
+	depends on BACKLIGHT_CLASS_DEVICE && ARCH_MXC
+	default y
+	help
+	  If you have a Freescale MC13783 PMIC, say y to enable the
+	  backlight driver.
+
+config BACKLIGHT_MXC_IPU
+	tristate "IPU PWM Backlight Driver"
+	depends on BACKLIGHT_MXC && MXC_IPU_V1
+	default y
+
+config BACKLIGHT_MXC_LCDC
+	tristate "LCDC PWM Backlight Driver"
+	depends on BACKLIGHT_MXC && (ARCH_MX21 || ARCH_MX27 || ARCH_MX25)
+	default y
+
+config BACKLIGHT_MXC_PMIC
+	tristate "PMIC Backlight Driver"
+	depends on BACKLIGHT_MXC && MXC_MC13783_LIGHT && MXC_MC13783_POWER
+	default y
+
+config BACKLIGHT_MXC_MC13892
+	tristate "Mc13892 Backlight Driver"
+	depends on BACKLIGHT_MXC && MXC_MC13892_LIGHT
+	default y
+	
+config BACKLIGHT_STMP37XX
+	tristate "SigmaTel STMP37xx Backlight Driver"
+	depends on BACKLIGHT_CLASS_DEVICE && (ARCH_STMP37XX || ARCH_STMP378X)
+	default y
+	help
+	  If you have a STMP37xx, say y to enable the
+	  backlight driver.
+
+config BACKLIGHT_WM8350
+    tristate "WM8350 Backlight Driver"
+    depends on BACKLIGHT_MXC && REGULATOR_WM8350
+    default y
diff --git a/drivers/video/backlight/Makefile b/drivers/video/backlight/Makefile
index 103427de6703..4d1f076e49a6 100644
--- a/drivers/video/backlight/Makefile
+++ b/drivers/video/backlight/Makefile
@@ -23,3 +23,9 @@ obj-$(CONFIG_BACKLIGHT_MBP_NVIDIA) += mbp_nvidia_bl.o
 obj-$(CONFIG_BACKLIGHT_TOSA)	+= tosa_bl.o
 obj-$(CONFIG_BACKLIGHT_SAHARA)	+= kb3886_bl.o
 
+obj-$(CONFIG_BACKLIGHT_MXC_LCDC) += mxc_lcdc_bl.o
+obj-$(CONFIG_BACKLIGHT_MXC_IPU)	+= mxc_ipu_bl.o
+obj-$(CONFIG_BACKLIGHT_MXC_PMIC) += mxc_pmic_bl.o
+obj-$(CONFIG_BACKLIGHT_WM8350) += wm8350_bl.o
+obj-$(CONFIG_BACKLIGHT_MXC_MC13892) += mxc_mc13892_bl.o
+obj-$(CONFIG_BACKLIGHT_STMP37XX) += stmp37xx_bl.o
diff --git a/drivers/video/backlight/mxc_ipu_bl.c b/drivers/video/backlight/mxc_ipu_bl.c
new file mode 100644
index 000000000000..a11c2d57e25e
--- /dev/null
+++ b/drivers/video/backlight/mxc_ipu_bl.c
@@ -0,0 +1,155 @@
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/*!
+ * @defgroup IPU_BL MXC IPU Backlight Driver
+ */
+/*!
+ * @file mxc_ipu_bl.c
+ *
+ * @brief Backlight Driver for IPU PWM on Freescale MXC/i.MX platforms.
+ *
+ * This file contains API defined in include/linux/clk.h for setting up and
+ * retrieving clocks.
+ *
+ * Based on Sharp's Corgi Backlight Driver
+ *
+ * @ingroup IPU_BL
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+#include <linux/ipu.h>
+
+#define MXC_MAX_INTENSITY 	255
+#define MXC_DEFAULT_INTENSITY 	127
+#define MXC_INTENSITY_OFF 	0
+
+struct mxcbl_dev_data {
+	int intensity;
+};
+
+static int fb_id;
+
+static int mxcbl_send_intensity(struct backlight_device *bd)
+{
+	int intensity = bd->props.brightness;
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+
+	if (bd->props.power != FB_BLANK_UNBLANK)
+		intensity = 0;
+	if (bd->props.fb_blank != FB_BLANK_UNBLANK)
+		intensity = 0;
+
+	ipu_sdc_set_brightness(intensity);
+
+	devdata->intensity = intensity;
+	return 0;
+}
+
+static int mxcbl_get_intensity(struct backlight_device *bd)
+{
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+	return devdata->intensity;
+}
+
+static int mxcbl_check_fb(struct fb_info *info)
+{
+	int id = info->fix.id[4] - '0';
+	if (id == fb_id) {
+		if ((id == 3) && !strcmp(info->fix.id, "DISP3 FG")) {
+			return 0;
+		}
+		return 1;
+	}
+	return 0;
+}
+
+static struct backlight_ops mxcbl_ops = {
+	.get_brightness = mxcbl_get_intensity,
+	.update_status = mxcbl_send_intensity,
+	.check_fb = mxcbl_check_fb,
+};
+
+static int __init mxcbl_probe(struct platform_device *pdev)
+{
+	struct backlight_device *bd;
+	struct mxcbl_dev_data *devdata;
+	int ret = 0;
+
+	devdata = kzalloc(sizeof(struct mxcbl_dev_data), GFP_KERNEL);
+	if (!devdata)
+		return -ENOMEM;
+	fb_id = (int)pdev->dev.platform_data;
+
+	bd = backlight_device_register(pdev->dev.bus_id, &pdev->dev, devdata,
+				       &mxcbl_ops);
+	if (IS_ERR(bd)) {
+		ret = PTR_ERR(bd);
+		goto err0;
+	}
+	platform_set_drvdata(pdev, bd);
+
+	bd->props.brightness = MXC_DEFAULT_INTENSITY;
+	bd->props.max_brightness = MXC_MAX_INTENSITY;
+	bd->props.power = FB_BLANK_UNBLANK;
+	bd->props.fb_blank = FB_BLANK_UNBLANK;
+	backlight_update_status(bd);
+
+	printk("MXC Backlight Device %s Initialized.\n", pdev->dev.bus_id);
+	return 0;
+      err0:
+	kfree(devdata);
+	return ret;
+}
+
+static int mxcbl_remove(struct platform_device *pdev)
+{
+	struct backlight_device *bd = platform_get_drvdata(pdev);
+
+	bd->props.brightness = MXC_INTENSITY_OFF;
+	backlight_update_status(bd);
+
+	backlight_device_unregister(bd);
+
+	return 0;
+}
+
+static struct platform_driver mxcbl_driver = {
+	.probe = mxcbl_probe,
+	.remove = mxcbl_remove,
+	.driver = {
+		   .name = "mxc_ipu_bl",
+		   },
+};
+
+static int __init mxcbl_init(void)
+{
+	return platform_driver_register(&mxcbl_driver);
+}
+
+static void __exit mxcbl_exit(void)
+{
+	platform_driver_unregister(&mxcbl_driver);
+}
+
+late_initcall(mxcbl_init);
+module_exit(mxcbl_exit);
+
+MODULE_DESCRIPTION("Freescale MXC/i.MX IPU PWM Backlight Driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/backlight/mxc_lcdc_bl.c b/drivers/video/backlight/mxc_lcdc_bl.c
new file mode 100644
index 000000000000..25076e07acbe
--- /dev/null
+++ b/drivers/video/backlight/mxc_lcdc_bl.c
@@ -0,0 +1,160 @@
+/*
+ * Copyright 2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/*!
+ * @defgroup LCDC_BL MXC LCDC Backlight Driver
+ */
+/*!
+ * @file mxc_lcdc_bl.c
+ *
+ * @brief Backlight Driver for LCDC PWM on Freescale MXC/i.MX platforms.
+ *
+ * This file contains API defined in include/linux/clk.h for setting up and
+ * retrieving clocks.
+ *
+ * Based on Sharp's Corgi Backlight Driver
+ *
+ * @ingroup LCDC_BL
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+#include <linux/clk.h>
+
+#define MXC_MAX_INTENSITY 	255
+#define MXC_DEFAULT_INTENSITY 	127
+#define MXC_INTENSITY_OFF 	0
+
+extern void mx2fb_set_brightness(uint8_t);
+
+struct mxcbl_dev_data {
+	struct clk *clk;
+	int intensity;
+};
+
+static int mxcbl_send_intensity(struct backlight_device *bd)
+{
+	int intensity = bd->props.brightness;
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+
+	if (bd->props.power != FB_BLANK_UNBLANK)
+		intensity = 0;
+	if (bd->props.fb_blank != FB_BLANK_UNBLANK)
+		intensity = 0;
+
+	if ((devdata->intensity == 0) && (intensity != 0))
+		clk_enable(devdata->clk);
+
+	/* PWM contrast control register */
+	mx2fb_set_brightness(intensity);
+
+	if ((devdata->intensity != 0) && (intensity == 0))
+		clk_disable(devdata->clk);
+
+	devdata->intensity = intensity;
+	return 0;
+}
+
+static int mxcbl_get_intensity(struct backlight_device *bd)
+{
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+	return devdata->intensity;
+}
+
+static int mxcbl_check_fb(struct fb_info *info)
+{
+	if (strcmp(info->fix.id, "DISP0 BG") == 0) {
+		return 1;
+	}
+	return 0;
+}
+
+static struct backlight_ops mxcbl_ops = {
+	.get_brightness = mxcbl_get_intensity,
+	.update_status = mxcbl_send_intensity,
+	.check_fb = mxcbl_check_fb,
+};
+
+static int __init mxcbl_probe(struct platform_device *pdev)
+{
+	struct backlight_device *bd;
+	struct mxcbl_dev_data *devdata;
+	int ret = 0;
+
+	devdata = kzalloc(sizeof(struct mxcbl_dev_data), GFP_KERNEL);
+	if (!devdata)
+		return -ENOMEM;
+
+	devdata->clk = clk_get(NULL, "lcdc_clk");
+
+	bd = backlight_device_register(pdev->dev.bus_id, &pdev->dev, devdata,
+				       &mxcbl_ops);
+	if (IS_ERR(bd)) {
+		ret = PTR_ERR(bd);
+		goto err0;
+	}
+	platform_set_drvdata(pdev, bd);
+
+	bd->props.brightness = MXC_DEFAULT_INTENSITY;
+	bd->props.max_brightness = MXC_MAX_INTENSITY;
+	bd->props.power = FB_BLANK_UNBLANK;
+	bd->props.fb_blank = FB_BLANK_UNBLANK;
+	mx2fb_set_brightness(MXC_DEFAULT_INTENSITY);
+
+	printk("MXC Backlight Device %s Initialized.\n", pdev->dev.bus_id);
+	return 0;
+      err0:
+	kfree(devdata);
+	return ret;
+}
+
+static int mxcbl_remove(struct platform_device *pdev)
+{
+	struct backlight_device *bd = platform_get_drvdata(pdev);
+
+	bd->props.brightness = MXC_INTENSITY_OFF;
+	backlight_update_status(bd);
+
+	backlight_device_unregister(bd);
+
+	return 0;
+}
+
+static struct platform_driver mxcbl_driver = {
+	.probe = mxcbl_probe,
+	.remove = mxcbl_remove,
+	.driver = {
+		   .name = "mxc_lcdc_bl",
+		   },
+};
+
+static int __init mxcbl_init(void)
+{
+	return platform_driver_register(&mxcbl_driver);
+}
+
+static void __exit mxcbl_exit(void)
+{
+	platform_driver_unregister(&mxcbl_driver);
+}
+
+module_init(mxcbl_init);
+module_exit(mxcbl_exit);
+
+MODULE_DESCRIPTION("Freescale MXC/i.MX LCDC PWM Backlight Driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/backlight/mxc_mc13892_bl.c b/drivers/video/backlight/mxc_mc13892_bl.c
new file mode 100644
index 000000000000..2038859243dc
--- /dev/null
+++ b/drivers/video/backlight/mxc_mc13892_bl.c
@@ -0,0 +1,177 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+
+#include <linux/pmic_light.h>
+#include <linux/pmic_external.h>
+
+/*
+#define MXC_MAX_INTENSITY 	255
+#define MXC_DEFAULT_INTENSITY 	127
+*/
+/* workaround for atlas hot issue */
+#define MXC_MAX_INTENSITY 	128
+#define MXC_DEFAULT_INTENSITY 	64
+
+#define MXC_INTENSITY_OFF 	0
+
+struct mxcbl_dev_data {
+	int intensity;
+	int suspend;
+};
+
+static int mxcbl_set_intensity(struct backlight_device *bd)
+{
+	int brightness = bd->props.brightness;
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+
+	if (bd->props.power != FB_BLANK_UNBLANK)
+		brightness = 0;
+	if (bd->props.fb_blank != FB_BLANK_UNBLANK)
+		brightness = 0;
+	if (devdata->suspend)
+		brightness = 0;
+
+	brightness = brightness / 4;
+	mc13892_bklit_set_dutycycle(LIT_MAIN, brightness);
+	devdata->intensity = brightness;
+
+	return 0;
+}
+
+static int mxcbl_get_intensity(struct backlight_device *bd)
+{
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+	return devdata->intensity;
+}
+
+static int mxcbl_check_fb(struct fb_info *info)
+{
+	char *id = info->fix.id;
+
+	if (!strcmp(id, "DISP3 BG"))
+		return 1;
+	else
+		return 0;
+}
+
+static struct backlight_ops bl_ops;
+
+static int __init mxcbl_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct backlight_device *bd;
+	struct mxcbl_dev_data *devdata;
+	pmic_version_t pmic_version;
+
+	pr_debug("mc13892 backlight start probe\n");
+
+	devdata = kzalloc(sizeof(struct mxcbl_dev_data), GFP_KERNEL);
+	if (!devdata)
+		return -ENOMEM;
+
+	bl_ops.check_fb = mxcbl_check_fb;
+	bl_ops.get_brightness = mxcbl_get_intensity;
+	bl_ops.update_status = mxcbl_set_intensity;
+	bd = backlight_device_register(pdev->dev.bus_id, &pdev->dev, devdata,
+				       &bl_ops);
+	if (IS_ERR(bd)) {
+		ret = PTR_ERR(bd);
+		goto err0;
+	}
+
+	platform_set_drvdata(pdev, bd);
+
+	/* according to LCD spec, current should be 18mA */
+	/* workaround for MC13892 TO1.1 crash issue, set current 6mA */
+	pmic_version = pmic_get_version();
+	if (pmic_version.revision < 20)
+		mc13892_bklit_set_current(LIT_MAIN, LIT_CURR_6);
+	else
+		mc13892_bklit_set_current(LIT_MAIN, LIT_CURR_18);
+	bd->props.brightness = MXC_DEFAULT_INTENSITY;
+	bd->props.max_brightness = MXC_MAX_INTENSITY;
+	bd->props.power = FB_BLANK_UNBLANK;
+	bd->props.fb_blank = FB_BLANK_UNBLANK;
+	backlight_update_status(bd);
+	pr_debug("mc13892 backlight probed successfully\n");
+	return 0;
+
+      err0:
+	kfree(devdata);
+	return ret;
+}
+
+static int mxcbl_remove(struct platform_device *pdev)
+{
+	struct backlight_device *bd = platform_get_drvdata(pdev);
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+
+	kfree(devdata);
+	backlight_device_unregister(bd);
+	return 0;
+}
+
+static int mxcbl_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct backlight_device *bd = platform_get_drvdata(pdev);
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+
+	devdata->suspend = 1;
+	backlight_update_status(bd);
+	return 0;
+}
+
+static int mxcbl_resume(struct platform_device *pdev)
+{
+	struct backlight_device *bd = platform_get_drvdata(pdev);
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+
+	devdata->suspend = 0;
+	backlight_update_status(bd);
+	return 0;
+}
+
+static struct platform_driver mxcbl_driver = {
+	.probe = mxcbl_probe,
+	.remove = mxcbl_remove,
+	.suspend = mxcbl_suspend,
+	.resume = mxcbl_resume,
+	.driver = {
+		   .name = "mxc_mc13892_bl",
+		   },
+};
+
+static int __init mxcbl_init(void)
+{
+	return platform_driver_register(&mxcbl_driver);
+}
+
+static void __exit mxcbl_exit(void)
+{
+	platform_driver_unregister(&mxcbl_driver);
+}
+
+module_init(mxcbl_init);
+module_exit(mxcbl_exit);
+
+MODULE_DESCRIPTION("Freescale MXC/i.MX PMIC Backlight Driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/backlight/mxc_pmic_bl.c b/drivers/video/backlight/mxc_pmic_bl.c
new file mode 100644
index 000000000000..a3b0c7b44a01
--- /dev/null
+++ b/drivers/video/backlight/mxc_pmic_bl.c
@@ -0,0 +1,197 @@
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/*!
+ * @defgroup PMIC_BL MXC PMIC Backlight Driver
+ */
+/*!
+ * @file mxc_pmic_bl.c
+ *
+ * @brief PMIC Backlight Driver for Freescale MXC/i.MX platforms.
+ *
+ * This file contains API defined in include/linux/clk.h for setting up and
+ * retrieving clocks.
+ *
+ * Based on Sharp's Corgi Backlight Driver
+ *
+ * @ingroup PMIC_BL
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+#include <linux/pmic_light.h>
+
+#include <mach/pmic_power.h>
+
+#define MXC_MAX_INTENSITY 	255
+#define MXC_DEFAULT_INTENSITY 	127
+#define MXC_INTENSITY_OFF 	0
+
+struct mxcbl_dev_data {
+	int bl_id;
+	int intensity;
+	struct backlight_ops bl_ops;
+};
+
+static int pmic_bl_use_count;
+static int main_fb_id;
+static int sec_fb_id;
+
+static int mxcbl_send_intensity(struct backlight_device *bd)
+{
+	int intensity = bd->props.brightness;
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+
+	if (bd->props.power != FB_BLANK_UNBLANK)
+		intensity = 0;
+	if (bd->props.fb_blank != FB_BLANK_UNBLANK)
+		intensity = 0;
+
+	intensity = intensity / 16;
+	pmic_bklit_set_dutycycle(devdata->bl_id, intensity);
+
+	devdata->intensity = intensity;
+	return 0;
+}
+
+static int mxcbl_get_intensity(struct backlight_device *bd)
+{
+	struct mxcbl_dev_data *devdata = dev_get_drvdata(&bd->dev);
+	return devdata->intensity;
+}
+
+static int mxcbl_check_main_fb(struct fb_info *info)
+{
+	int id = info->fix.id[4] - '0';
+
+	if (id == main_fb_id) {
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+static int mxcbl_check_sec_fb(struct fb_info *info)
+{
+	int id = info->fix.id[4] - '0';
+
+	if (id == sec_fb_id) {
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+static int __init mxcbl_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct backlight_device *bd;
+	struct mxcbl_dev_data *devdata;
+
+	devdata = kzalloc(sizeof(struct mxcbl_dev_data), GFP_KERNEL);
+	if (!devdata)
+		return -ENOMEM;
+	devdata->bl_id = pdev->id;
+
+	if (pdev->id == 0) {
+		devdata->bl_ops.check_fb = mxcbl_check_main_fb;
+		main_fb_id = (int)pdev->dev.platform_data;
+	} else {
+		devdata->bl_ops.check_fb = mxcbl_check_sec_fb;
+		sec_fb_id = (int)pdev->dev.platform_data;
+	}
+
+	devdata->bl_ops.get_brightness = mxcbl_get_intensity;
+	devdata->bl_ops.update_status = mxcbl_send_intensity,
+	    bd =
+	    backlight_device_register(pdev->dev.bus_id, &pdev->dev, devdata,
+				      &devdata->bl_ops);
+	if (IS_ERR(bd)) {
+		ret = PTR_ERR(bd);
+		goto err0;
+	}
+
+	platform_set_drvdata(pdev, bd);
+
+	if (pmic_bl_use_count++ == 0) {
+		pmic_power_regulator_on(SW_SW3);
+		pmic_power_regulator_set_lp_mode(SW_SW3, LOW_POWER_CTRL_BY_PIN);
+
+		pmic_bklit_tcled_master_enable();
+		pmic_bklit_enable_edge_slow();
+		pmic_bklit_set_cycle_time(0);
+	}
+
+	pmic_bklit_set_current(devdata->bl_id, 7);
+	bd->props.brightness = MXC_DEFAULT_INTENSITY;
+	bd->props.max_brightness = MXC_MAX_INTENSITY;
+	bd->props.power = FB_BLANK_UNBLANK;
+	bd->props.fb_blank = FB_BLANK_UNBLANK;
+	backlight_update_status(bd);
+
+	printk("MXC Backlight Device %s Initialized.\n", pdev->dev.bus_id);
+	return 0;
+      err0:
+	kfree(devdata);
+	return ret;
+}
+
+static int mxcbl_remove(struct platform_device *pdev)
+{
+	struct backlight_device *bd = platform_get_drvdata(pdev);
+
+	bd->props.brightness = MXC_INTENSITY_OFF;
+	backlight_update_status(bd);
+
+	if (--pmic_bl_use_count == 0) {
+		pmic_bklit_tcled_master_disable();
+
+		pmic_power_regulator_off(SW_SW3);
+		pmic_power_regulator_set_lp_mode(SW_SW3, LOW_POWER_CTRL_BY_PIN);
+	}
+
+	backlight_device_unregister(bd);
+
+	printk("MXC Backlight Driver Unloaded\n");
+
+	return 0;
+}
+
+static struct platform_driver mxcbl_driver = {
+	.probe = mxcbl_probe,
+	.remove = mxcbl_remove,
+	.driver = {
+		   .name = "mxc_pmic_bl",
+		   },
+};
+
+static int __init mxcbl_init(void)
+{
+	return platform_driver_register(&mxcbl_driver);
+}
+
+static void __exit mxcbl_exit(void)
+{
+	platform_driver_unregister(&mxcbl_driver);
+}
+
+module_init(mxcbl_init);
+module_exit(mxcbl_exit);
+
+MODULE_DESCRIPTION("Freescale MXC/i.MX PMIC Backlight Driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index ea07258565f0..e641584e212e 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -3,7 +3,7 @@
  *
  * simple PWM based backlight control, board code has to setup
  * 1) pin configuration so PWM waveforms can output
- * 2) platform_data casts to the PWM id (0/1/2/3 on PXA)
+ * 2) platform_data being correctly configured
  *
  * 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
@@ -97,7 +97,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	} else
 		dev_dbg(&pdev->dev, "got pwm for backlight\n");
 
-	bl = backlight_device_register(pdev->name, &pdev->dev,
+	bl = backlight_device_register(dev_name(&pdev->dev), &pdev->dev,
 			pb, &pwm_backlight_ops);
 	if (IS_ERR(bl)) {
 		dev_err(&pdev->dev, "failed to register backlight\n");
diff --git a/drivers/video/backlight/stmp37xx_bl.c b/drivers/video/backlight/stmp37xx_bl.c
new file mode 100644
index 000000000000..6b4d9a0ccbaa
--- /dev/null
+++ b/drivers/video/backlight/stmp37xx_bl.c
@@ -0,0 +1,378 @@
+/*
+ * Backlight Driver for Freescale STMP37XX/STMP378X
+ *
+ * Embedded Alley Solutions, Inc <source@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+
+#include <mach/lcdif.h>
+#include <mach/regulator.h>
+
+struct stmp3xxx_bl_data {
+	struct notifier_block nb;
+	struct notifier_block reg_nb;
+	struct notifier_block reg_init_nb;
+	struct backlight_device *bd;
+	struct stmp3xxx_platform_bl_data *pdata;
+	int current_intensity;
+	int saved_intensity;
+	int stmp3xxxbl_suspended;
+	int stmp3xxxbl_constrained;
+};
+
+static int stmp3xxxbl_do_probe(struct stmp3xxx_bl_data *data,
+		struct stmp3xxx_platform_bl_data *pdata);
+static int stmp3xxxbl_set_intensity(struct backlight_device *bd);
+static inline void bl_register_reg(struct stmp3xxx_platform_bl_data *pdata,
+				   struct stmp3xxx_bl_data *data);
+
+
+/*
+ * If we got here init is done
+ */
+static int bl_init_reg_callback(struct notifier_block *self,
+			unsigned long event, void *data)
+{
+	struct stmp3xxx_bl_data *bdata;
+	struct stmp3xxx_platform_bl_data *pdata;
+	struct regulator *r = regulator_get(NULL, "stmp3xxx-bl-1");
+
+	bdata = container_of(self, struct stmp3xxx_bl_data, reg_init_nb);
+	pdata = bdata->pdata;
+
+	if (r && !IS_ERR(r))
+		regulator_put(r);
+	else
+		goto out;
+
+	bl_register_reg(pdata, bdata);
+
+	if (pdata->regulator) {
+
+		printk(KERN_NOTICE"%s: setting intensity\n", __func__);
+
+		bus_unregister_notifier(&platform_bus_type,
+					&bdata->reg_init_nb);
+		mutex_lock(&bdata->bd->ops_lock);
+		stmp3xxxbl_set_intensity(bdata->bd);
+		mutex_unlock(&bdata->bd->ops_lock);
+	}
+
+out:
+	return 0;
+}
+
+static int bl_reg_callback(struct notifier_block *self,
+			unsigned long event, void *data)
+{
+	struct stmp3xxx_bl_data *bdata;
+	struct stmp3xxx_platform_bl_data *pdata;
+	bdata = container_of(self, struct stmp3xxx_bl_data, reg_nb);
+	pdata = bdata->pdata;
+
+	mutex_lock(&bdata->bd->ops_lock);
+
+	switch (event) {
+	case STMP3XXX_REG5V_IS_USB:
+		bdata->bd->props.max_brightness = pdata->bl_cons_intensity;
+		bdata->bd->props.brightness = pdata->bl_cons_intensity;
+		bdata->saved_intensity = bdata->current_intensity;
+		bdata->stmp3xxxbl_constrained = 1;
+		break;
+	case STMP3XXX_REG5V_NOT_USB:
+		bdata->bd->props.max_brightness = pdata->bl_max_intensity;
+		bdata->bd->props.brightness = bdata->saved_intensity;
+		bdata->stmp3xxxbl_constrained = 0;
+		break;
+	}
+
+	stmp3xxxbl_set_intensity(bdata->bd);
+	mutex_unlock(&bdata->bd->ops_lock);
+	return 0;
+}
+
+static inline void bl_unregister_reg(struct stmp3xxx_platform_bl_data *pdata,
+				  struct stmp3xxx_bl_data *data)
+{
+	if (!pdata)
+		return;
+	if (pdata->regulator)
+		regulator_unregister_notifier(pdata->regulator,
+					    &data->reg_nb);
+	if (pdata->regulator)
+		regulator_put(pdata->regulator);
+	pdata->regulator = NULL;
+}
+
+static inline void bl_register_reg(struct stmp3xxx_platform_bl_data *pdata,
+				   struct stmp3xxx_bl_data *data)
+{
+	pdata->regulator = regulator_get(NULL, "stmp3xxx-bl-1");
+	if (pdata->regulator && !IS_ERR(pdata->regulator)) {
+		regulator_set_mode(pdata->regulator, REGULATOR_MODE_FAST);
+		if (pdata->regulator) {
+			data->reg_nb.notifier_call = bl_reg_callback;
+			regulator_register_notifier(pdata->regulator,
+						    &data->reg_nb);
+		}
+	} else{
+		printk(KERN_ERR "%s: failed to get regulator\n", __func__);
+		pdata->regulator = NULL;
+	}
+
+}
+
+static int bl_callback(struct notifier_block *self,
+		       unsigned long event, void *data)
+{
+	struct stmp3xxx_platform_fb_entry *pentry = data;
+	struct stmp3xxx_bl_data *bdata;
+	struct stmp3xxx_platform_bl_data *pdata;
+
+	switch (event) {
+	case STMP3XXX_LCDIF_PANEL_INIT:
+		bdata = container_of(self, struct stmp3xxx_bl_data, nb);
+		pdata = pentry->bl_data;
+		bdata->pdata = pdata;
+		if (pdata) {
+			bl_register_reg(pdata, bdata);
+			if (!pdata->regulator) {
+				/* wait for regulator to appear */
+				bdata->reg_init_nb.notifier_call =
+						bl_init_reg_callback;
+				bus_register_notifier(&platform_bus_type,
+						      &bdata->reg_init_nb);
+			}
+			return stmp3xxxbl_do_probe(bdata, pdata);
+		}
+		break;
+
+	case STMP3XXX_LCDIF_PANEL_RELEASE:
+		bdata = container_of(self, struct stmp3xxx_bl_data, nb);
+		pdata = pentry->bl_data;
+		if (pdata) {
+			bus_unregister_notifier(&platform_bus_type,
+						&bdata->reg_init_nb);
+			bl_unregister_reg(pdata, bdata);
+			pdata->free_bl(pdata);
+		}
+		bdata->pdata = NULL;
+		break;
+	}
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int stmp3xxxbl_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct stmp3xxx_bl_data *data = platform_get_drvdata(pdev);
+	struct stmp3xxx_platform_bl_data *pdata = data->pdata;
+
+	data->stmp3xxxbl_suspended = 1;
+	if (pdata) {
+		dev_dbg(&pdev->dev, "real suspend\n");
+		stmp3xxxbl_set_intensity(data->bd);
+	}
+	return 0;
+}
+
+static int stmp3xxxbl_resume(struct platform_device *pdev)
+{
+	struct stmp3xxx_bl_data *data = platform_get_drvdata(pdev);
+	struct stmp3xxx_platform_bl_data *pdata = data->pdata;
+	int ret = 0;
+
+	data->stmp3xxxbl_suspended = 0;
+	if (pdata) {
+		dev_dbg(&pdev->dev, "real resume\n");
+		pdata->free_bl(pdata);
+		ret = pdata->init_bl(pdata);
+		if (ret)
+			goto out;
+		stmp3xxxbl_set_intensity(data->bd);
+	}
+out:
+	return ret;
+}
+#else
+#define stmp3xxxbl_suspend	NULL
+#define stmp3xxxbl_resume	NULL
+#endif
+/*
+ *  This function should be called with bd->ops_lock held
+ *  Suspend/resume ?
+ */
+static int stmp3xxxbl_set_intensity(struct backlight_device *bd)
+{
+	struct platform_device *pdev = dev_get_drvdata(&bd->dev);
+	struct stmp3xxx_bl_data *data = platform_get_drvdata(pdev);
+	struct stmp3xxx_platform_bl_data *pdata = data->pdata;
+
+	if (pdata) {
+		int ret;
+
+		ret = pdata->set_bl_intensity(pdata, bd,
+					      data->stmp3xxxbl_suspended);
+		if (ret)
+			bd->props.brightness = data->current_intensity;
+		else
+			data->current_intensity = bd->props.brightness;
+		return ret;
+	} else
+		return -ENODEV;
+}
+
+static int stmp3xxxbl_get_intensity(struct backlight_device *bd)
+{
+	struct platform_device *pdev = dev_get_drvdata(&bd->dev);
+	struct stmp3xxx_bl_data *data = platform_get_drvdata(pdev);
+
+	return data->current_intensity;
+}
+
+static struct backlight_ops stmp3xxxbl_ops = {
+	.get_brightness = stmp3xxxbl_get_intensity,
+	.update_status  = stmp3xxxbl_set_intensity,
+};
+
+static int stmp3xxxbl_do_probe(struct stmp3xxx_bl_data *data,
+		struct stmp3xxx_platform_bl_data *pdata)
+{
+	int ret = pdata->init_bl(pdata);
+
+	if (ret)
+		goto out;
+
+	data->bd->props.power = FB_BLANK_UNBLANK;
+	data->bd->props.fb_blank = FB_BLANK_UNBLANK;
+	if (data->stmp3xxxbl_constrained) {
+		data->bd->props.max_brightness = pdata->bl_cons_intensity;
+		data->bd->props.brightness = pdata->bl_cons_intensity;
+	} else {
+		data->bd->props.max_brightness = pdata->bl_max_intensity;
+		data->bd->props.brightness = pdata->bl_default_intensity;
+	}
+
+	data->pdata = pdata;
+	stmp3xxxbl_set_intensity(data->bd);
+
+out:
+	return ret;
+}
+
+static int __init stmp3xxxbl_probe(struct platform_device *pdev)
+{
+	struct stmp3xxx_bl_data *data;
+	struct stmp3xxx_platform_bl_data *pdata = pdev->dev.platform_data;
+	int ret = 0;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	data->bd = backlight_device_register(pdev->name, &pdev->dev, pdev,
+					&stmp3xxxbl_ops);
+	if (IS_ERR(data->bd)) {
+		ret = PTR_ERR(data->bd);
+		goto out_1;
+	}
+
+	get_device(&pdev->dev);
+
+	data->nb.notifier_call = bl_callback;
+	stmp3xxx_lcdif_register_client(&data->nb);
+	platform_set_drvdata(pdev, data);
+
+	if (pdata) {
+		ret = stmp3xxxbl_do_probe(data, pdata);
+		if (ret)
+			goto out_2;
+	}
+
+	goto out;
+
+out_2:
+	put_device(&pdev->dev);
+out_1:
+	kfree(data);
+out:
+	return ret;
+}
+
+static int stmp3xxxbl_remove(struct platform_device *pdev)
+{
+	struct stmp3xxx_platform_bl_data *pdata = pdev->dev.platform_data;
+	struct stmp3xxx_bl_data *data = platform_get_drvdata(pdev);
+	struct backlight_device *bd = data->bd;
+
+	bd->props.power = FB_BLANK_POWERDOWN;
+	bd->props.fb_blank = FB_BLANK_POWERDOWN;
+	bd->props.brightness = 0;
+	data->current_intensity = bd->props.brightness;
+
+	if (pdata) {
+		pdata->set_bl_intensity(pdata, bd, data->stmp3xxxbl_suspended);
+		if (pdata->free_bl)
+			pdata->free_bl(pdata);
+	}
+	backlight_device_unregister(bd);
+	if (pdata->regulator)
+		regulator_put(pdata->regulator);
+	put_device(&pdev->dev);
+	platform_set_drvdata(pdev, NULL);
+	stmp3xxx_lcdif_unregister_client(&data->nb);
+	kfree(data);
+
+	return 0;
+}
+
+static struct platform_driver stmp3xxxbl_driver = {
+	.probe		= stmp3xxxbl_probe,
+	.remove		= __devexit_p(stmp3xxxbl_remove),
+	.suspend	= stmp3xxxbl_suspend,
+	.resume		= stmp3xxxbl_resume,
+	.driver		= {
+		.name	= "stmp3xxx-bl",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init stmp3xxx_init(void)
+{
+	return platform_driver_register(&stmp3xxxbl_driver);
+}
+
+static void __exit stmp3xxx_exit(void)
+{
+	platform_driver_unregister(&stmp3xxxbl_driver);
+}
+
+module_init(stmp3xxx_init);
+module_exit(stmp3xxx_exit);
+
+MODULE_AUTHOR("Embedded Alley Solutions, Inc <sources@embeddedalley.com>");
+MODULE_DESCRIPTION("STMP3xxx Backlight Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/backlight/wm8350_bl.c b/drivers/video/backlight/wm8350_bl.c
new file mode 100644
index 000000000000..c724025f74dc
--- /dev/null
+++ b/drivers/video/backlight/wm8350_bl.c
@@ -0,0 +1,298 @@
+/*
+ *  Backlight driver for DCDC2  on i.MX32ADS board
+ *
+ *  Copyright(C) 2007 Wolfson Microelectronics PLC.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/mutex.h>
+#include <linux/fb.h>
+#include <linux/platform_device.h>
+#include <linux/backlight.h>
+#include <linux/regulator/consumer.h>
+#include <linux/mfd/wm8350/pmic.h>
+#include <linux/mfd/wm8350/bl.h>
+
+struct wm8350_backlight {
+	struct backlight_properties props;
+	struct backlight_device *device;
+	struct regulator *dcdc;
+	struct regulator *isink;
+	struct notifier_block notifier;
+	struct work_struct work;
+	struct mutex mutex;
+	int intensity;
+	int suspend;
+	int retries;
+};
+
+/* hundredths of uA, 405 = 4.05 uA */
+static const int intensity_huA[] = {
+	405, 482, 573, 681, 810, 963, 1146, 1362, 1620, 1927, 2291, 2725,
+	3240, 3853, 4582, 5449, 6480, 7706, 9164, 10898, 12960, 15412, 18328,
+	21796, 25920, 30824, 36656, 43592, 51840, 61648, 73313, 87184,
+	103680, 123297, 146626, 174368, 207360, 246594, 293251, 348737,
+	414720, 493188, 586503, 697473, 829440, 986376, 1173005, 1394946,
+	1658880, 1972752, 2346011, 2789892, 3317760, 3945504, 4692021,
+	5579785, 6635520, 7891008, 9384042, 11159570, 13271040, 15782015,
+	18768085, 22319140,
+};
+
+static void bl_work(struct work_struct *work)
+{
+	struct wm8350_backlight *bl =
+		container_of(work, struct wm8350_backlight, work);
+	struct regulator *isink = bl->isink;
+
+	mutex_lock(&bl->mutex);
+	if (bl->intensity >= 0 &&
+		bl->intensity < ARRAY_SIZE(intensity_huA)) {
+		bl->retries = 0;
+		regulator_set_current_limit(isink,
+			0, intensity_huA[bl->intensity] / 100);
+	} else
+		printk(KERN_ERR "wm8350: Backlight intensity error\n");
+	mutex_unlock(&bl->mutex);
+}
+
+static int wm8350_bl_notifier(struct notifier_block *self,
+	unsigned long event, void *data)
+{
+	struct wm8350_backlight *bl =
+		container_of(self, struct wm8350_backlight, notifier);
+	struct regulator *isink = bl->isink;
+
+	if (event & REGULATOR_EVENT_UNDER_VOLTAGE)
+		printk(KERN_ERR "wm8350: BL DCDC undervoltage\n");
+	if (event & REGULATOR_EVENT_REGULATION_OUT)
+		printk(KERN_ERR "wm8350: BL ISINK out of regulation\n");
+
+	mutex_lock(&bl->mutex);
+	if (bl->retries) {
+		bl->retries--;
+		regulator_disable(isink);
+		regulator_set_current_limit(isink, 0, bl->intensity);
+		regulator_enable(isink);
+	} else {
+		printk(KERN_ERR
+			"wm8350: BL regulation retry failure - disable\n");
+		bl->intensity = 0;
+		regulator_disable(isink);
+	}
+	mutex_unlock(&bl->mutex);
+	return 0;
+}
+
+static int wm8350_bl_send_intensity(struct backlight_device *bd)
+{
+	struct wm8350_backlight *bl =
+		(struct wm8350_backlight *)dev_get_drvdata(&bd->dev);
+	int intensity = bd->props.brightness;
+
+	if (bd->props.power != FB_BLANK_UNBLANK)
+		intensity = 0;
+	if (bd->props.fb_blank != FB_BLANK_UNBLANK)
+		intensity = 0;
+	if (bl->suspend)
+		intensity = 0;
+
+	mutex_lock(&bl->mutex);
+	bl->intensity = intensity;
+	mutex_unlock(&bl->mutex);
+	schedule_work(&bl->work);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int wm8350_bl_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct wm8350_backlight *bl =
+		(struct wm8350_backlight *)platform_get_drvdata(pdev);
+
+	bl->suspend = 1;
+	backlight_update_status(bl->device);
+	return 0;
+}
+
+static int wm8350_bl_resume(struct platform_device *pdev)
+{
+	struct wm8350_backlight *bl =
+		(struct wm8350_backlight *)platform_get_drvdata(pdev);
+
+	bl->suspend = 0;
+	backlight_update_status(bl->device);
+	return 0;
+}
+#else
+#define wm8350_bl_suspend	NULL
+#define wm8350_bl_resume	NULL
+#endif
+
+static int wm8350_bl_get_intensity(struct backlight_device *bd)
+{
+	struct wm8350_backlight *bl =
+		(struct wm8350_backlight *)dev_get_drvdata(&bd->dev);
+	return bl->intensity;
+}
+
+static struct backlight_ops wm8350_bl_ops = {
+	.get_brightness = wm8350_bl_get_intensity,
+	.update_status  = wm8350_bl_send_intensity,
+};
+
+static int wm8350_bl_probe(struct platform_device *pdev)
+{
+	struct regulator *isink, *dcdc;
+	struct wm8350_backlight *bl;
+	struct wm8350_bl_platform_data *pdata = pdev->dev.platform_data;
+	struct wm8350 *pmic;
+	int ret;
+
+	if (pdata == NULL) {
+		printk(KERN_ERR "%s: no platform data\n", __func__);
+		return -ENODEV;
+	}
+
+	if (pdata->isink != WM8350_ISINK_A && pdata->isink != WM8350_ISINK_B) {
+		printk(KERN_ERR "%s: invalid ISINK\n", __func__);
+		return -EINVAL;
+	}
+	if (pdata->dcdc != WM8350_DCDC_2 && pdata->dcdc != WM8350_DCDC_5) {
+		printk(KERN_ERR "%s: invalid DCDC\n", __func__);
+		return -EINVAL;
+	}
+
+	printk(KERN_INFO "wm8350: backlight using %s and %s\n",
+		pdata->isink == WM8350_ISINK_A ? "ISINKA" : "ISINKB",
+		pdata->dcdc == WM8350_DCDC_2 ? "DCDC2" : "DCDC5");
+
+	isink = regulator_get(&pdev->dev,
+		pdata->isink == WM8350_ISINK_A ? "ISINKA" : "ISINKB");
+	if (IS_ERR(isink) || isink == NULL) {
+		printk(KERN_ERR "%s: cant get ISINK\n", __func__);
+		return PTR_ERR(isink);
+	}
+
+	dcdc = regulator_get(&pdev->dev,
+		pdata->dcdc == WM8350_DCDC_2 ? "DCDC2" : "DCDC5");
+	if (IS_ERR(dcdc) || dcdc == NULL) {
+		printk(KERN_ERR "%s: cant get DCDC\n", __func__);
+		regulator_put(isink);
+		return PTR_ERR(dcdc);
+	}
+
+	bl = kzalloc(sizeof(*bl), GFP_KERNEL);
+	if (bl == NULL) {
+		regulator_put(isink);
+		regulator_put(dcdc);
+		return -ENOMEM;
+	}
+
+	mutex_init(&bl->mutex);
+	INIT_WORK(&bl->work, bl_work);
+	bl->props.max_brightness = pdata->max_brightness;
+	bl->props.power = pdata->power;
+	bl->props.brightness = pdata->brightness;
+	bl->retries = pdata->retries;
+	bl->dcdc = dcdc;
+	bl->isink = isink;
+	platform_set_drvdata(pdev, bl);
+	pmic = regulator_get_drvdata(bl->isink);
+
+	wm8350_bl_ops.check_fb = pdata->check_fb;
+
+	bl->device = backlight_device_register(pdev->dev.bus_id, &pdev->dev,
+		bl, &wm8350_bl_ops);
+	if (IS_ERR(bl->device)) {
+		ret =  PTR_ERR(bl->device);
+		regulator_put(dcdc);
+		regulator_put(isink);
+		kfree(bl);
+		return ret;
+	}
+
+	bl->notifier.notifier_call = wm8350_bl_notifier;
+	regulator_register_notifier(dcdc, &bl->notifier);
+	regulator_register_notifier(isink, &bl->notifier);
+	bl->device->props = bl->props;
+
+	regulator_set_current_limit(isink, 0, 20000);
+
+	wm8350_isink_set_flash(pmic, pdata->isink,
+		WM8350_ISINK_FLASH_DISABLE,
+		WM8350_ISINK_FLASH_TRIG_BIT,
+		WM8350_ISINK_FLASH_DUR_32MS,
+		WM8350_ISINK_FLASH_ON_1_00S,
+		WM8350_ISINK_FLASH_OFF_1_00S,
+		WM8350_ISINK_FLASH_MODE_EN);
+
+	wm8350_dcdc25_set_mode(pmic, pdata->dcdc,
+		WM8350_ISINK_MODE_BOOST, WM8350_ISINK_ILIM_NORMAL,
+		pdata->voltage_ramp, pdata->isink == WM8350_ISINK_A ?
+		WM8350_DC5_FBSRC_ISINKA : WM8350_DC5_FBSRC_ISINKB);
+
+	wm8350_dcdc_set_slot(pmic, pdata->dcdc, 15, 0,
+		pdata->dcdc == WM8350_DCDC_2 ?
+		WM8350_DC2_ERRACT_SHUTDOWN_CONV : WM8350_DC5_ERRACT_NONE);
+
+	regulator_enable(isink);
+	backlight_update_status(bl->device);
+	return 0;
+}
+
+static int wm8350_bl_remove(struct platform_device *pdev)
+{
+	struct wm8350_backlight *bl =
+		(struct wm8350_backlight *)platform_get_drvdata(pdev);
+	struct regulator *isink = bl->isink, *dcdc = bl->dcdc;
+
+	bl->intensity = 0;
+	backlight_update_status(bl->device);
+	schedule_work(&bl->work);
+	flush_scheduled_work();
+	backlight_device_unregister(bl->device);
+
+	regulator_set_current_limit(isink, 0, 0);
+	regulator_disable(isink);
+	regulator_unregister_notifier(isink, &bl->notifier);
+	regulator_unregister_notifier(dcdc, &bl->notifier);
+	regulator_put(isink);
+	regulator_put(dcdc);
+	return 0;
+}
+
+struct platform_driver imx32ads_backlight_driver = {
+	.driver		= {
+		.name	= "wm8350-bl",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= wm8350_bl_probe,
+	.remove		= wm8350_bl_remove,
+	.suspend	= wm8350_bl_suspend,
+	.resume		= wm8350_bl_resume,
+};
+
+static int __devinit imx32ads_backlight_init(void)
+{
+	return platform_driver_register(&imx32ads_backlight_driver);
+}
+
+static void imx32ads_backlight_exit(void)
+{
+	platform_driver_unregister(&imx32ads_backlight_driver);
+}
+
+device_initcall_sync(imx32ads_backlight_init);
+module_exit(imx32ads_backlight_exit);
+
+MODULE_AUTHOR("Liam Girdwood <lg@opensource.wolfsonmicro.com>");
+MODULE_DESCRIPTION("WM8350 Backlight driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/hx8347fb.c b/drivers/video/hx8347fb.c
new file mode 100644
index 000000000000..1b0e66e12921
--- /dev/null
+++ b/drivers/video/hx8347fb.c
@@ -0,0 +1,482 @@
+/*
+ * linux/drivers/video/hx8347fb.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License. See the file COPYING in the main directory of this archive for
+ * more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/list.h>
+#include <linux/firmware.h>
+#include <linux/dma-mapping.h>
+#include <linux/uaccess.h>
+#include <linux/irq.h>
+
+#include <video/hx8347fb.h>
+
+#include <asm/unaligned.h>
+
+#define DRV_NAME	"hx8347"
+
+
+#ifdef CONFIG_PM
+
+/*
+ * Command sequences, as follows:
+ * register, data, delay (ms)
+ * Taken from a Himax application note
+ */
+u8 hxstandby_in[][3] = {
+	{0x26, 0x38, 40},	/* Display off */
+	{0x26, 0x28, 40},
+	{0x26, 0, 40},
+	{0x43, 0, 10},		/* Power off */
+	{0x1b, 0, 10},
+	{0x1b, 0x08, 10},
+	{0x1c, 0, 10},
+	{0x90, 0, 10},
+	{0x1b, 0x09, 10},	/* Into standby mode */
+	{0x19, 0x48, 0}		/* Stop oscillation */
+};
+
+u8 hxstandby_out[][3] = {
+	{0x19, 0x49, 10},	/* Start oscillation */
+	{0x1b, 0x08, 0},	/* Exit standby mode */
+	{0x20, 0x40, 0},	/* Power supply setting */
+	{0x1d, 0x07, 0},
+	{0x1e, 0, 0},
+	{0x1f, 0x03, 0},
+	{0x44, 0x20, 0},
+	{0x45, 0x0e, 10},
+	{0x1c, 0x04, 20},
+	{0x1b, 0x18, 40},
+	{0x1b, 0x10, 40},
+	{0x43, 0x80, 100},
+	{0x90, 0x7f, 40},	/* Display on setting */
+	{0x26, 0x04, 40},
+	{0x26, 0x24, 0},
+	{0x26, 0x2c, 40},
+	{0x26, 0x3c, 0},
+};
+
+static void hx8347_standby(struct hx8347fb_par *par, int standby)
+{
+	int i;
+
+	if (standby) {
+		for (i = 0; i < (sizeof(hxstandby_in) / 3); i++) {
+			par->pdata->wr_reg(par, hxstandby_in[i][0], (u16)hxstandby_in[i][1]);
+			mdelay(hxstandby_in[i][2]);
+		}
+	} else {
+		for (i = 0; i < (sizeof(hxstandby_out) / 3); i++) {
+			par->pdata->wr_reg(par, hxstandby_out[i][0], (u16)hxstandby_out[i][1]);
+			mdelay(hxstandby_out[i][2]);
+		}
+	}
+}
+#endif
+
+static void hx8347_dpy_update(struct hx8347fb_par *par)
+{
+	u16 *buf = (u16 __force *)par->info->screen_base;
+
+	par->pdata->set_idx(par, SRAM_WR_CTRL);
+	par->pdata->wr_data(par, buf, par->info->fix.smem_len);
+}
+
+/* this is called back from the deferred io workqueue */
+static void hx8347_dpy_deferred_io(struct fb_info *info,
+				struct list_head *pagelist)
+{
+	hx8347_dpy_update(info->par);
+}
+
+static void hx8347_fillrect(struct fb_info *info,
+				   const struct fb_fillrect *rect)
+{
+	struct hx8347fb_par *par = info->par;
+
+	sys_fillrect(info, rect);
+	hx8347_dpy_update(par);
+}
+
+static void hx8347_copyarea(struct fb_info *info,
+				   const struct fb_copyarea *area)
+{
+	struct hx8347fb_par *par = info->par;
+
+	sys_copyarea(info, area);
+	hx8347_dpy_update(par);
+}
+
+static void hx8347_imageblit(struct fb_info *info,
+				const struct fb_image *image)
+{
+	struct hx8347fb_par *par = info->par;
+
+	sys_imageblit(info, image);
+	hx8347_dpy_update(par);
+}
+
+/*
+ * this is the slow path from userspace. they can seek and write to
+ * the fb. it is based on fb_sys_write
+ */
+static ssize_t hx8347_write(struct fb_info *info, const char __user *buf,
+				size_t count, loff_t *ppos)
+{
+	struct hx8347fb_par *par = info->par;
+	unsigned long p = *ppos;
+	void *dst;
+	int err = 0;
+	unsigned long total_size;
+
+	if (info->state != FBINFO_STATE_RUNNING)
+		return -EPERM;
+
+	total_size = info->fix.smem_len;
+
+	if (p > total_size)
+		return -EFBIG;
+
+	if (count > total_size) {
+		err = -EFBIG;
+		count = total_size;
+	}
+
+	if (count + p > total_size) {
+		if (!err)
+			err = -ENOSPC;
+
+		count = total_size - p;
+	}
+
+	dst = (void __force *)(info->screen_base + p);
+
+	if (copy_from_user(dst, buf, count))
+		err = -EFAULT;
+
+	if  (!err)
+		*ppos += count;
+
+	hx8347_dpy_update(par);
+
+	return (err) ? err : count;
+}
+
+static int hx8347_set_par(struct fb_info *fb)
+{
+	fb->fix.visual = FB_VISUAL_TRUECOLOR;
+	fb->fix.line_length = fb->var.width * fb->var.bits_per_pixel / 8;
+
+	return 0;
+}
+
+static int hx8347_setcolreg(unsigned regno, unsigned red, unsigned green,
+		unsigned blue, unsigned transp, struct fb_info *fb)
+{
+	struct hx8347fb_par *par = fb->par;
+
+	if (regno > fb->var.bits_per_pixel)
+		return -EINVAL;
+
+	par->pdata->pseudo_pal[regno] = (red & 0xf800) |
+			((green & 0xfc00) >> 5) | ((green & 0xf800) >> 11);
+
+	return 0;
+}
+
+static struct fb_ops hx8347fb_ops = {
+	.owner		= THIS_MODULE,
+	.fb_set_par 	= hx8347_set_par,
+	.fb_setcolreg   = hx8347_setcolreg,
+	.fb_read        = fb_sys_read,
+	.fb_write	= hx8347_write,
+	.fb_fillrect	= hx8347_fillrect,
+	.fb_copyarea	= hx8347_copyarea,
+	.fb_imageblit	= hx8347_imageblit,
+};
+
+static struct fb_deferred_io hx8347fb_defio = {
+	.delay		= HZ/20,
+	.deferred_io	= hx8347_dpy_deferred_io,
+};
+
+static int __devinit hx8347_probe(struct platform_device *pdev)
+{
+	struct fb_info *info;
+	struct hx8347fb_pdata *pdata;
+	struct hx8347fb_par *par;
+	struct resource *cmdregs = NULL;
+	struct resource *dataregs = NULL;
+	unsigned char *videomemory;
+	int ret;
+
+	/* pick up board specific routines */
+	pdata = pdev->dev.platform_data;
+	if (!pdata)
+		return -EINVAL;
+
+	/* try to count device specific driver, if can't, platform recalls */
+	if (!try_module_get(pdata->owner))
+		return -ENODEV;
+
+	cmdregs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!cmdregs) {
+		dev_err(&pdev->dev, "%s: unable to get iomem resources\n", __func__);
+		ret = -ENXIO;
+		goto err_resources;
+	}
+	dataregs = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!dataregs) {
+		dev_err(&pdev->dev, "%s: unable to get iomem resources\n", __func__);
+		ret = -ENXIO;
+		goto err_resources;
+	}
+
+	pdata->irq = platform_get_irq(pdev, 0);
+	if (pdata->irq < 0) {
+		dev_err(&pdev->dev, "%s: unable to get irq\n", __func__);
+		ret = pdata->irq;
+		goto err_resources;
+	}
+
+	if (!request_mem_region(cmdregs[0].start, cmdregs[0].end - cmdregs[0].start + 1, "hx8347fb cmd")) {
+		dev_dbg(&pdev->dev, "%s: request_mem_region failed\n", __func__);
+		ret = -EBUSY;
+		goto err_resources;
+	}
+
+	if (!request_mem_region(dataregs[0].start, dataregs[0].end - dataregs[0].start + 1, "hx8347fb data")) {
+		dev_dbg(&pdev->dev, "%s:request_mem_region failed\n", __func__);
+		ret = -EBUSY;
+		goto err_request_mem;
+	}
+	info = framebuffer_alloc(sizeof(struct hx8347fb_par), &pdev->dev);
+	if (!info) {
+		dev_dbg(&pdev->dev, "%s: unable to allocate frambebuffer device\n", __func__);
+		ret = -ENOMEM;
+		goto err_fballoc;
+	}
+	par = info->par;
+	par->mmio_cmd = (void __iomem *)ioremap_nocache(cmdregs[0].start, cmdregs[0].end - cmdregs[0].start + 1);
+	if (!par->mmio_cmd) {
+		dev_err(&pdev->dev, "%s: error ioremap cmd\n", __func__);
+		ret = -EBUSY;
+		goto err_map_cmd;
+	}
+	par->mmio_data = (void __iomem *)ioremap_nocache(dataregs[0].start, dataregs[0].end - dataregs[0].start + 1);
+	if (!par->mmio_data) {
+		dev_dbg(&pdev->dev, "%s: error ioremap data\n", __func__);
+		ret = -EBUSY;
+		goto err_map_data;
+	}
+
+	info->fix.smem_len = pdata->xres * pdata->yres * pdata->bits_per_pixel / 8;
+	if (!pdata->usedma) {
+		if ((videomemory = vmalloc(info->fix.smem_len)) == NULL) {
+			dev_err(&pdev->dev, "%s: error vmalloc\n", __func__);
+			ret = -ENOMEM;
+	       		goto err_vmalloc;
+		}
+	} else {
+		pdev->dev.coherent_dma_mask = (u32)-1;
+		videomemory = dma_alloc_writecombine(&pdev->dev, info->fix.smem_len,
+			&par->fb_p, GFP_KERNEL);
+		if (!videomemory) {
+			dev_dbg(&pdev->dev, "%s: err_alloc_fb\n", __func__);
+			ret = -ENOMEM;
+			goto err_dma_alloc;
+		}
+	}
+
+	memset(videomemory, 0, info->fix.smem_len);
+	info->screen_base = (char __force __iomem *)videomemory;
+
+	info->fbops = &hx8347fb_ops;
+	info->pseudo_palette = &pdata->pseudo_pal[0];
+	strcpy(info->fix.id, DRV_NAME);
+	info->fix.type = FB_TYPE_PACKED_PIXELS;
+	info->fix.accel = FB_ACCEL_NONE;
+	info->fix.smem_start = dataregs[0].start;
+	info->fix.line_length = pdata->xres * pdata->bits_per_pixel / 8;
+
+	info->var.activate = FB_ACTIVATE_NOW;
+	info->var.height = pdata->yres;
+	info->var.yres = pdata->yres;
+	info->var.yres_virtual = pdata->yres;
+	info->var.width = pdata->xres;
+	info->var.xres = pdata->xres;
+	info->var.xres_virtual = pdata->xres;
+	info->var.vmode = FB_VMODE_NONINTERLACED;
+	info->var.bits_per_pixel = pdata->bits_per_pixel;
+
+	info->var.red.offset = 11;
+	info->var.green.offset = 5;
+	info->var.blue.offset = 0;
+	info->var.red.length = 5;
+	info->var.green.length = 6;
+	info->var.blue.length = 5;
+
+	par->info = info;
+	par->pdata = pdata;
+
+	info->flags = FBINFO_FLAG_DEFAULT;
+	info->fbdefio = &hx8347fb_defio;
+	fb_deferred_io_init(info);
+
+	ret = register_framebuffer(info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "%s: unable to register fb device\n", DRV_NAME);
+		goto err_fbreg;
+	}
+
+	/* Verify that the minimal hooks have been setup */
+	if (!par->pdata->init || !par->pdata->wr_reg ||
+	    !par->pdata->set_idx || !par->pdata->wr_data)
+		goto err_init;
+
+	ret = par->pdata->init(par);
+	if (ret < 0)
+		goto err_init;
+
+	/* Enable backlight */
+	if (par->pdata->bl_enable)
+		par->pdata->bl_enable(par, 1);
+
+	platform_set_drvdata(pdev, info);
+	printk(KERN_INFO
+	       "fb%d: HX8347 frame buffer device, using %dK of video memory\n",
+	       info->node, info->fix.smem_len >> 10);
+
+	return 0;
+
+err_init:
+	unregister_framebuffer(info);
+err_fbreg:
+	if (!pdata->usedma)
+		vfree(videomemory);
+err_vmalloc:
+err_dma_alloc:
+	iounmap(par->mmio_data);
+err_map_data:
+	iounmap(par->mmio_cmd);
+err_map_cmd:
+	framebuffer_release(info);
+err_fballoc:
+	release_mem_region(dataregs[0].start, dataregs[0].end - dataregs[0].start + 1);
+err_request_mem:
+	release_mem_region(cmdregs[0].start, cmdregs[0].end - cmdregs[0].start + 1);
+err_resources:
+	module_put(pdata->owner);
+
+	return ret;
+}
+
+static int __devexit hx8347_remove(struct platform_device *dev)
+{
+	struct fb_info *info = platform_get_drvdata(dev);
+	struct hx8347fb_par *par = info->par;
+
+	if (info) {
+		if (par->pdata->bl_enable)
+			par->pdata->bl_enable(par, 0);
+		unregister_framebuffer(info);
+		fb_deferred_io_cleanup(info);
+		fb_dealloc_cmap(&info->cmap);
+		if (!par->pdata->usedma)
+			vfree((void __force *)info->screen_base);
+		iounmap(par->mmio_data);
+		iounmap(par->mmio_cmd);
+
+		if (par->pdata->cleanup)
+			par->pdata->cleanup(par);
+
+		module_put(par->pdata->owner);
+		dev_dbg(&dev->dev, "calling release\n");
+		framebuffer_release(info);
+	}
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/* suspend and resume support for the lcd controller */
+static int hx8347_suspend(struct platform_device *dev, pm_message_t state)
+{
+	struct fb_info *info = platform_get_drvdata(dev);
+	struct hx8347fb_par *par = info->par;
+
+	if (state.event == PM_EVENT_SUSPEND) {
+		/* Disable the backlight set standby mode */
+		if (par->pdata->bl_enable)
+			par->pdata->bl_enable(par, 0);
+		mdelay(1);
+		hx8347_standby(par, 1);
+	}
+
+	return 0;
+}
+
+static int hx8347_resume(struct platform_device *dev)
+{
+	struct fb_info *info = platform_get_drvdata(dev);
+	struct hx8347fb_par *par = info->par;
+
+	/* Configure the display to resume and enable the backlight */
+	hx8347_standby(par, 0);
+	mdelay(10);
+	if (par->pdata->bl_enable)
+		par->pdata->bl_enable(par, 1);
+
+	return 0;
+}
+#else
+#define hx8347_suspend NULL
+#define hx8347_resume  NULL
+#endif
+
+
+static struct platform_driver hx8347_driver = {
+	.probe	 = hx8347_probe,
+	.remove  = hx8347_remove,
+	.suspend = hx8347_suspend,
+	.resume  = hx8347_resume,
+
+	.driver	= {
+		.owner	= THIS_MODULE,
+		.name	= "hx8347",
+	},
+};
+
+static int __init hx8347_init(void)
+{
+	return platform_driver_register(&hx8347_driver);
+}
+
+static void __exit hx8347_exit(void)
+{
+	platform_driver_unregister(&hx8347_driver);
+}
+
+module_init(hx8347_init);
+module_exit(hx8347_exit);
+
+MODULE_DESCRIPTION("Framebuffer driver for the HX8347 controller");
+MODULE_AUTHOR("Pedro Perez de Heredia");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/modedb.c b/drivers/video/modedb.c
index d3c3af53a290..4fc9bcd1b6e1 100644
--- a/drivers/video/modedb.c
+++ b/drivers/video/modedb.c
@@ -220,6 +220,10 @@ static const struct fb_videomode modedb[] = {
 	NULL, 72, 320, 240, 63492, 16, 16, 16, 4, 48, 2,
 	0, FB_VMODE_DOUBLE
     }, {
+	/* 480x272 @ 60 Hz, 31.5 kHz hsync */
+	NULL, 60, 320, 240, 79440, 16, 16, 16, 5, 48, 1,
+	0, FB_VMODE_DOUBLE
+    }, {
 	/* 400x300 @ 56 Hz, 35.2 kHz hsync, 4:3 aspect ratio */
 	NULL, 56, 400, 300, 55555, 64, 16, 10, 1, 32, 1,
 	0, FB_VMODE_DOUBLE
diff --git a/drivers/video/mxc/Kconfig b/drivers/video/mxc/Kconfig
new file mode 100644
index 000000000000..709cfcfc45f4
--- /dev/null
+++ b/drivers/video/mxc/Kconfig
@@ -0,0 +1,87 @@
+config FB_MXC
+	tristate "MXC Framebuffer support"
+	depends on FB && (MXC_IPU || ARCH_MX21 || ARCH_MX27 || ARCH_MX25)
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	select FB_MODE_HELPERS
+	default y
+	help
+	  This is a framebuffer device for the MXC LCD Controller.
+	  See <http://www.linux-fbdev.org/> for information on framebuffer
+	  devices.
+
+	  If you plan to use the LCD display with your MXC system, say
+	  Y here.
+
+config FB_MXC_SYNC_PANEL
+	depends on FB_MXC
+	tristate "Synchronous Panel Framebuffer"
+	default y
+
+config FB_MXC_EPSON_VGA_SYNC_PANEL
+	depends on FB_MXC_SYNC_PANEL
+	tristate "Epson VGA Panel"
+	default n
+
+config FB_MXC_TVOUT_TVE
+	tristate "MXC TVE TV Out Encoder"
+	depends on FB_MXC_SYNC_PANEL
+	depends on MXC_IPU_V3
+
+config FB_MXC_CLAA_WVGA_SYNC_PANEL
+	depends on FB_MXC_SYNC_PANEL
+	tristate "CLAA WVGA Panel"
+
+config FB_MXC_CH7026
+	depends on FB_MXC_SYNC_PANEL
+	tristate "Chrontel CH7026 VGA Interface Chip"
+
+config FB_MXC_TVOUT
+	bool "FS453 TV Out Encoder"
+	depends on FB_MXC_SYNC_PANEL
+
+config FB_MXC_TVOUT_CH7024
+	tristate "CH7024 TV Out Encoder"
+	depends on FB_MXC_SYNC_PANEL
+
+config FB_MXC_LOW_PWR_DISPLAY
+	bool "Low Power Display Refresh Mode"
+	depends on FB_MXC_SYNC_PANEL && MXC_FB_IRAM
+	default y
+
+config FB_MXC_INTERNAL_MEM
+	bool "Framebuffer in Internal RAM"
+	depends on FB_MXC_SYNC_PANEL && MXC_FB_IRAM
+	default y
+
+config FB_MXC_ASYNC_PANEL
+	depends on FB_MXC
+	bool "Asynchronous Panels"
+	default n
+
+menu "Asynchronous Panel Type"
+	depends on FB_MXC_ASYNC_PANEL && FB_MXC
+
+config FB_MXC_EPSON_PANEL
+    depends on FB_MXC_ASYNC_PANEL
+	default n
+	bool "Epson 176x220 Panel"
+
+endmenu
+
+choice
+	prompt "Async Panel Interface Type"
+	depends on FB_MXC_ASYNC_PANEL && FB_MXC
+	default FB_MXC_ASYNC_PANEL_IFC_16_BIT
+
+config FB_MXC_ASYNC_PANEL_IFC_8_BIT
+	bool "8-bit Parallel Bus Interface"
+
+config FB_MXC_ASYNC_PANEL_IFC_16_BIT
+	bool "16-bit Parallel Bus Interface"
+
+config FB_MXC_ASYNC_PANEL_IFC_SERIAL
+	bool "Serial Bus Interface"
+
+endchoice
diff --git a/drivers/video/mxc/Makefile b/drivers/video/mxc/Makefile
new file mode 100644
index 000000000000..a1bd24b967a5
--- /dev/null
+++ b/drivers/video/mxc/Makefile
@@ -0,0 +1,22 @@
+ifeq ($(CONFIG_ARCH_MX21)$(CONFIG_ARCH_MX27)$(CONFIG_ARCH_MX25),y)
+	obj-$(CONFIG_FB_MXC_TVOUT)              += fs453.o
+	obj-$(CONFIG_FB_MXC_SYNC_PANEL)         += mx2fb.o mxcfb_modedb.o
+	obj-$(CONFIG_FB_MXC_EPSON_PANEL)        += mx2fb_epson.o
+else
+ifeq ($(CONFIG_MXC_IPU_V1),y)
+	obj-$(CONFIG_FB_MXC_SYNC_PANEL)         += mxcfb.o mxcfb_modedb.o
+else
+	obj-$(CONFIG_FB_MXC_SYNC_PANEL)         += mxc_ipuv3_fb.o
+endif
+	obj-$(CONFIG_FB_MXC_TVOUT)              += fs453.o
+	obj-$(CONFIG_FB_MXC_EPSON_PANEL)        += mxcfb_epson.o
+	obj-$(CONFIG_FB_MXC_EPSON_QVGA_PANEL)   += mxcfb_epson_qvga.o
+	obj-$(CONFIG_FB_MXC_TOSHIBA_QVGA_PANEL) += mxcfb_toshiba_qvga.o
+	obj-$(CONFIG_FB_MXC_SHARP_128_PANEL)    += mxcfb_sharp_128x128.o
+endif
+obj-$(CONFIG_FB_MXC_EPSON_VGA_SYNC_PANEL)   += mxcfb_epson_vga.o
+obj-$(CONFIG_FB_MXC_CLAA_WVGA_SYNC_PANEL)   += mxcfb_claa_wvga.o
+obj-$(CONFIG_FB_MXC_TVOUT_CH7024)           += ch7024.o
+obj-$(CONFIG_FB_MXC_TVOUT_TVE)              += tve.o
+obj-$(CONFIG_FB_MXC_CH7026)		    		+= mxcfb_ch7026.o
+obj-$(CONFIG_FB_MODE_HELPERS)				+= mxc_edid.o
diff --git a/drivers/video/mxc/ch7024.c b/drivers/video/mxc/ch7024.c
new file mode 100644
index 000000000000..35ffb7570c2b
--- /dev/null
+++ b/drivers/video/mxc/ch7024.c
@@ -0,0 +1,866 @@
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ch7024.c
+ * @brief Driver for CH7024 TV encoder
+ *
+ * @ingroup Framebuffer
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/spinlock.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/sysfs.h>
+#include <linux/mxcfb.h>
+#include <linux/regulator/consumer.h>
+#include <asm/uaccess.h>
+#include <asm/atomic.h>
+#include <mach/gpio.h>
+#include <mach/hw_events.h>
+
+/*!
+ * CH7024 registers
+ */
+#define CH7024_DEVID		0x00
+#define CH7024_REVID		0x01
+#define CH7024_PG		0x02
+
+#define CH7024_RESET		0x03
+#define CH7024_POWER		0x04
+#define CH7024_TVHUE		0x05
+#define CH7024_TVSAT		0x06
+#define CH7024_TVCTA		0x07
+#define CH7024_TVBRI		0x08
+#define CH7024_TVSHARP		0x09
+#define CH7024_OUT_FMT		0x0A
+#define CH7024_XTAL		0x0B
+#define CH7024_IDF1		0x0C
+#define CH7024_IDF2		0x0D
+#define CH7024_SYNC		0x0E
+#define CH7024_TVFILTER1	0x0F
+#define CH7024_TVFILTER2	0x10
+#define CH7024_IN_TIMING1	0x11
+#define CH7024_IN_TIMING2	0x12
+#define CH7024_IN_TIMING3	0x13
+#define CH7024_IN_TIMING4	0x14
+#define CH7024_IN_TIMING5	0x15
+#define CH7024_IN_TIMING6	0x16
+#define CH7024_IN_TIMING7	0x17
+#define CH7024_IN_TIMING8	0x18
+#define CH7024_IN_TIMING9	0x19
+#define CH7024_IN_TIMING10	0x1A
+#define CH7024_IN_TIMING11	0x1B
+#define CH7024_ACIV		0x1C
+#define CH7024_CLK_TREE		0x1D
+#define CH7024_OUT_TIMING1	0x1E
+#define CH7024_OUT_TIMING2	0x1F
+#define CH7024_V_POS1		0x20
+#define CH7024_V_POS2		0x21
+#define CH7024_H_POS1		0x22
+#define CH7024_H_POS2		0x23
+#define CH7024_PCLK_A1		0x24
+#define CH7024_PCLK_A2		0x25
+#define CH7024_PCLK_A3		0x26
+#define CH7024_PCLK_A4		0x27
+#define CH7024_CLK_P1		0x28
+#define CH7024_CLK_P2		0x29
+#define CH7024_CLK_P3		0x2A
+#define CH7024_CLK_N1		0x2B
+#define CH7024_CLK_N2		0x2C
+#define CH7024_CLK_N3		0x2D
+#define CH7024_CLK_T		0x2E
+#define CH7024_PLL1		0x2F
+#define CH7024_PLL2		0x30
+#define CH7024_PLL3		0x31
+#define CH7024_SC_FREQ1		0x34
+#define CH7024_SC_FREQ2		0x35
+#define CH7024_SC_FREQ3		0x36
+#define CH7024_SC_FREQ4		0x37
+#define CH7024_DAC_TRIM		0x62
+#define CH7024_DATA_IO		0x63
+#define CH7024_ATT_DISP		0x7E
+
+/*!
+ * CH7024 register values
+ */
+/* video output formats */
+#define CH7024_VOS_NTSC_M	0x0
+#define CH7024_VOS_NTSC_J	0x1
+#define CH7024_VOS_NTSC_443	0x2
+#define CH7024_VOS_PAL_BDGHKI	0x3
+#define CH7024_VOS_PAL_M	0x4
+#define CH7024_VOS_PAL_N	0x5
+#define CH7024_VOS_PAL_NC	0x6
+#define CH7024_VOS_PAL_60	0x7
+/* crystal predefined */
+#define CH7024_XTAL_13MHZ	0x4
+#define CH7024_XTAL_26MHZ	0xB
+
+/* chip ID */
+#define CH7024_DEVICE_ID	0x45
+
+/* clock source define */
+#define CLK_HIGH	0
+#define CLK_LOW		1
+
+/* CH7024 presets structs */
+struct ch7024_clock {
+	u32 A;
+	u32 P;
+	u32 N;
+	u32 T;
+	u8 PLLN1;
+	u8 PLLN2;
+	u8 PLLN3;
+};
+
+struct ch7024_input_timing {
+	u32 HTI;
+	u32 VTI;
+	u32 HAI;
+	u32 VAI;
+	u32 HW;
+	u32 HO;
+	u32 VW;
+	u32 VO;
+	u32 VOS;
+};
+
+#define TVOUT_FMT_OFF	0
+#define TVOUT_FMT_NTSC	1
+#define TVOUT_FMT_PAL	2
+
+static int enabled;		/* enable power on or not */
+static int pm_status;		/* status before suspend */
+
+static struct i2c_client *ch7024_client;
+static struct fb_info *ch7024_fbi;
+static int ch7024_cur_mode;
+static u32 detect_gpio;
+static struct regulator *io_reg;
+static struct regulator *core_reg;
+static struct regulator *analog_reg;
+
+static void hp_detect_wq_handler(struct work_struct *);
+DECLARE_DELAYED_WORK(ch7024_wq, hp_detect_wq_handler);
+
+static inline int ch7024_read_reg(u8 reg)
+{
+	return i2c_smbus_read_byte_data(ch7024_client, reg);
+}
+
+static inline int ch7024_write_reg(u8 reg, u8 word)
+{
+	return i2c_smbus_write_byte_data(ch7024_client, reg, word);
+}
+
+/**
+ * PAL B/D/G/H/K/I clock and timting structures
+ */
+static struct ch7024_clock ch7024_clk_pal = {
+	.A = 0x0,
+	.P = 0x36b00,
+	.N = 0x41eb00,
+	.T = 0x3f,
+	.PLLN1 = 0x0,
+	.PLLN2 = 0x1b,
+	.PLLN3 = 0x12,
+};
+
+static struct ch7024_input_timing ch7024_timing_pal = {
+	.HTI = 950,
+	.VTI = 560,
+	.HAI = 640,
+	.VAI = 480,
+	.HW = 60,
+	.HO = 250,
+	.VW = 40,
+	.VO = 40,
+	.VOS = CH7024_VOS_PAL_BDGHKI,
+};
+
+/**
+ * NTSC_M clock and timting structures
+ * TODO: change values to work well.
+ */
+static struct ch7024_clock ch7024_clk_ntsc = {
+	.A = 0x0,
+	.P = 0x2ac90,
+	.N = 0x36fc90,
+	.T = 0x3f,
+	.PLLN1 = 0x0,
+	.PLLN2 = 0x1b,
+	.PLLN3 = 0x12,
+};
+
+static struct ch7024_input_timing ch7024_timing_ntsc = {
+	.HTI = 801,
+	.VTI = 554,
+	.HAI = 640,
+	.VAI = 480,
+	.HW = 60,
+	.HO = 101,
+	.VW = 20,
+	.VO = 54,
+	.VOS = CH7024_VOS_NTSC_M,
+};
+
+static struct fb_videomode video_modes[] = {
+	{
+	 /* NTSC TV output */
+	 "TV-NTSC", 60, 640, 480, 37594,
+	 0, 101,
+	 0, 54,
+	 60, 20,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+	{
+	 /* PAL TV output */
+	 "TV-PAL", 50, 640, 480, 37594,
+	 0, 250,
+	 0, 40,
+	 60, 40,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+};
+
+/**
+ * ch7024_setup
+ * initial the CH7024 chipset by setting register
+ * @param:
+ * 	vos: output video format
+ * @return:
+ * 	0 successful
+ * 	otherwise failed
+ */
+static int ch7024_setup(int vos)
+{
+	struct ch7024_input_timing *ch_timing;
+	struct ch7024_clock *ch_clk;
+#ifdef DEBUG_CH7024
+	int i, val;
+#endif
+
+	/* select output video format */
+	if (vos == TVOUT_FMT_PAL) {
+		ch_timing = &ch7024_timing_pal;
+		ch_clk = &ch7024_clk_pal;
+		pr_debug("CH7024: change to PAL video\n");
+	} else if (vos == TVOUT_FMT_NTSC) {
+		ch_timing = &ch7024_timing_ntsc;
+		ch_clk = &ch7024_clk_ntsc;
+		pr_debug("CH7024: change to NTSC video\n");
+	} else {
+
+		pr_debug("CH7024: no such video format.\n");
+		return -EINVAL;
+	}
+	ch7024_write_reg(CH7024_RESET, 0x0);
+	ch7024_write_reg(CH7024_RESET, 0x3);
+
+	ch7024_write_reg(CH7024_POWER, 0x0C);	/* power on, disable DAC */
+	ch7024_write_reg(CH7024_XTAL, CH7024_XTAL_26MHZ);
+	ch7024_write_reg(CH7024_SYNC, 0x0D);	/* SLAVE mode, and TTL */
+	ch7024_write_reg(CH7024_IDF1, 0x00);
+	ch7024_write_reg(CH7024_TVFILTER1, 0x00);	/* set XCH=0 */
+	ch7024_write_reg(CH7024_CLK_TREE, 0x9E);	/* Invert input clk */
+
+	/* set input clock and divider */
+	/* set PLL */
+	ch7024_write_reg(CH7024_PLL1, ch_clk->PLLN1);
+	ch7024_write_reg(CH7024_PLL2, ch_clk->PLLN2);
+	ch7024_write_reg(CH7024_PLL3, ch_clk->PLLN3);
+	/* set A register */
+	ch7024_write_reg(CH7024_PCLK_A1, (ch_clk->A >> 24) & 0xFF);
+	ch7024_write_reg(CH7024_PCLK_A2, (ch_clk->A >> 16) & 0xFF);
+	ch7024_write_reg(CH7024_PCLK_A3, (ch_clk->A >> 8) & 0xFF);
+	ch7024_write_reg(CH7024_PCLK_A4, ch_clk->A & 0xFF);
+	/* set P register */
+	ch7024_write_reg(CH7024_CLK_P1, (ch_clk->P >> 16) & 0xFF);
+	ch7024_write_reg(CH7024_CLK_P2, (ch_clk->P >> 8) & 0xFF);
+	ch7024_write_reg(CH7024_CLK_P3, ch_clk->P & 0xFF);
+	/* set N register */
+	ch7024_write_reg(CH7024_CLK_N1, (ch_clk->N >> 16) & 0xFF);
+	ch7024_write_reg(CH7024_CLK_N2, (ch_clk->N >> 8) & 0xFF);
+	ch7024_write_reg(CH7024_CLK_N3, ch_clk->N & 0xFF);
+	/* set T register */
+	ch7024_write_reg(CH7024_CLK_T, ch_clk->T & 0xFF);
+
+	/* set sub-carrier frequency generation method */
+	ch7024_write_reg(CH7024_ACIV, 0x00);	/* ACIV = 0, automatical SCF */
+	/* TV out pattern and DAC switch */
+	ch7024_write_reg(CH7024_OUT_FMT, (0x10 | ch_timing->VOS) & 0xFF);
+
+	/* input settings */
+	/* input format, RGB666 */
+	ch7024_write_reg(CH7024_IDF2, 0x02);
+	/* HAI/HTI VAI */
+	ch7024_write_reg(CH7024_IN_TIMING1, ((ch_timing->HTI >> 5) & 0x38) |
+			 ((ch_timing->HAI >> 8) & 0x07));
+	ch7024_write_reg(CH7024_IN_TIMING2, ch_timing->HAI & 0xFF);
+	ch7024_write_reg(CH7024_IN_TIMING8, ch_timing->VAI & 0xFF);
+	/* HTI VTI */
+	ch7024_write_reg(CH7024_IN_TIMING3, ch_timing->HTI & 0xFF);
+	ch7024_write_reg(CH7024_IN_TIMING9, ch_timing->VTI & 0xFF);
+	/* HW/HO(h) VW */
+	ch7024_write_reg(CH7024_IN_TIMING4, ((ch_timing->HW >> 5) & 0x18) |
+			 ((ch_timing->HO >> 8) & 0x7));
+	ch7024_write_reg(CH7024_IN_TIMING6, ch_timing->HW & 0xFF);
+	ch7024_write_reg(CH7024_IN_TIMING11, ch_timing->VW & 0x3F);
+	/* HO(l) VO/VAI/VTI */
+	ch7024_write_reg(CH7024_IN_TIMING5, ch_timing->HO & 0xFF);
+	ch7024_write_reg(CH7024_IN_TIMING7, ((ch_timing->VO >> 4) & 0x30) |
+			 ((ch_timing->VTI >> 6) & 0x0C) |
+			 ((ch_timing->VAI >> 8) & 0x03));
+	ch7024_write_reg(CH7024_IN_TIMING10, ch_timing->VO & 0xFF);
+
+	/* adjust the brightness */
+	ch7024_write_reg(CH7024_TVBRI, 0x90);
+
+	ch7024_write_reg(CH7024_OUT_TIMING1, 0x4);
+	ch7024_write_reg(CH7024_OUT_TIMING2, 0xe0);
+
+	if (vos == TVOUT_FMT_PAL) {
+		ch7024_write_reg(CH7024_V_POS1, 0x03);
+		ch7024_write_reg(CH7024_V_POS2, 0x7d);
+	} else {
+		ch7024_write_reg(CH7024_V_POS1, 0x02);
+		ch7024_write_reg(CH7024_V_POS2, 0x7b);
+	}
+
+	ch7024_write_reg(CH7024_POWER, 0x00);
+
+#ifdef DEBUG_CH7024
+	for (i = 0; i < CH7024_SC_FREQ4; i++) {
+
+		val = ch7024_read_reg(i);
+		pr_debug("CH7024, reg[0x%x] = %x\n", i, val);
+	}
+#endif
+	return 0;
+}
+
+/**
+ * ch7024_enable
+ * Enable the ch7024 Power to begin TV encoder
+ */
+static int ch7024_enable(void)
+{
+	int en = enabled;
+
+	if (!enabled) {
+		regulator_enable(core_reg);
+		regulator_enable(io_reg);
+		regulator_enable(analog_reg);
+		msleep(200);
+		enabled = 1;
+		ch7024_write_reg(CH7024_POWER, 0x00);
+		pr_debug("CH7024 power on.\n");
+	}
+	return en;
+}
+
+/**
+ * ch7024_disable
+ * Disable the ch7024 Power to stop TV encoder
+ */
+static void ch7024_disable(void)
+{
+	if (enabled) {
+		enabled = 0;
+		ch7024_write_reg(CH7024_POWER, 0x0D);
+		regulator_disable(analog_reg);
+		regulator_disable(io_reg);
+		regulator_disable(core_reg);
+		pr_debug("CH7024 power off.\n");
+	}
+}
+
+static int ch7024_detect(void)
+{
+	int en;
+	int detect = 0;
+
+	if (gpio_get_value(detect_gpio) == 1) {
+		set_irq_type(ch7024_client->irq, IRQF_TRIGGER_FALLING);
+
+		en = ch7024_enable();
+
+		ch7024_write_reg(CH7024_DAC_TRIM, 0xB4);
+		msleep(50);
+		detect = ch7024_read_reg(CH7024_ATT_DISP) & 0x3;
+		ch7024_write_reg(CH7024_DAC_TRIM, 0x34);
+
+		if (!en)
+			ch7024_disable();
+	} else {
+		set_irq_type(ch7024_client->irq, IRQF_TRIGGER_RISING);
+	}
+	dev_dbg(&ch7024_client->dev, "detect = %d\n", detect);
+	return (detect);
+}
+
+static irqreturn_t hp_detect_handler(int irq, void *data)
+{
+	disable_irq(irq);
+	schedule_delayed_work(&ch7024_wq, 50);
+
+	return IRQ_HANDLED;
+}
+
+static void hp_detect_wq_handler(struct work_struct *work)
+{
+	int detect;
+	struct mxc_hw_event event = { HWE_PHONEJACK_PLUG, 0 };
+
+	detect = ch7024_detect();
+
+	enable_irq(ch7024_client->irq);
+
+	sysfs_notify(&ch7024_client->dev.kobj, NULL, "headphone");
+
+	/* send hw event by netlink */
+	event.args = detect;
+	hw_event_send(1, &event);
+}
+
+int ch7024_fb_event(struct notifier_block *nb, unsigned long val, void *v)
+{
+	struct fb_event *event = v;
+	struct fb_info *fbi = event->info;
+
+	switch (val) {
+	case FB_EVENT_FB_REGISTERED:
+		if ((ch7024_fbi != NULL) || strcmp(fbi->fix.id, "DISP3 BG"))
+			break;
+
+		ch7024_fbi = fbi;
+		fb_add_videomode(&video_modes[0], &ch7024_fbi->modelist);
+		fb_add_videomode(&video_modes[1], &ch7024_fbi->modelist);
+		break;
+	case FB_EVENT_MODE_CHANGE:
+		if (ch7024_fbi != fbi)
+			break;
+
+		if (!fbi->mode) {
+			ch7024_disable();
+			ch7024_cur_mode = TVOUT_FMT_OFF;
+			return 0;
+		}
+
+		if (fb_mode_is_equal(fbi->mode, &video_modes[0])) {
+			ch7024_cur_mode = TVOUT_FMT_NTSC;
+			ch7024_enable();
+			ch7024_setup(TVOUT_FMT_NTSC);
+		} else if (fb_mode_is_equal(fbi->mode, &video_modes[1])) {
+			ch7024_cur_mode = TVOUT_FMT_PAL;
+			ch7024_enable();
+			ch7024_setup(TVOUT_FMT_PAL);
+		} else {
+			ch7024_disable();
+			ch7024_cur_mode = TVOUT_FMT_OFF;
+			return 0;
+		}
+		break;
+	case FB_EVENT_BLANK:
+		if ((ch7024_fbi != fbi) || (ch7024_cur_mode == TVOUT_FMT_OFF))
+			return 0;
+
+		if (*((int *)event->data) == FB_BLANK_UNBLANK) {
+			ch7024_enable();
+			ch7024_setup(ch7024_cur_mode);
+		} else {
+			ch7024_disable();
+		}
+		break;
+	}
+	return 0;
+}
+
+static struct notifier_block nb = {
+	.notifier_call = ch7024_fb_event,
+};
+
+static ssize_t show_headphone(struct device_driver *dev, char *buf)
+{
+	int detect;
+
+	detect = ch7024_detect();
+
+	if (detect == 0) {
+		strcpy(buf, "none\n");
+	} else if (detect == 1) {
+		strcpy(buf, "cvbs\n");
+	} else {
+		strcpy(buf, "headset\n");
+	}
+
+	return strlen(buf);
+}
+
+DRIVER_ATTR(headphone, 0644, show_headphone, NULL);
+
+static ssize_t show_brightness(struct device_driver *dev, char *buf)
+{
+	u32 reg;
+	reg = ch7024_read_reg(CH7024_TVBRI);
+	return snprintf(buf, PAGE_SIZE, "%u", reg);
+}
+
+static ssize_t store_brightness(struct device_driver *dev, const char *buf,
+				size_t count)
+{
+	char *endp;
+	int brightness = simple_strtoul(buf, &endp, 0);
+	size_t size = endp - buf;
+
+	if (*endp && isspace(*endp))
+		size++;
+	if (size != count)
+		return -EINVAL;
+
+	if (brightness > 255)
+		brightness = 255;
+
+	ch7024_write_reg(CH7024_TVBRI, brightness);
+
+	return count;
+}
+
+DRIVER_ATTR(brightness, 0644, show_brightness, store_brightness);
+
+static ssize_t show_contrast(struct device_driver *dev, char *buf)
+{
+	u32 reg;
+	reg = ch7024_read_reg(CH7024_TVCTA);
+
+	reg *= 2;		/* Scale to 0 - 255 */
+
+	return snprintf(buf, PAGE_SIZE, "%u", reg);
+}
+
+static ssize_t store_contrast(struct device_driver *dev, const char *buf,
+			      size_t count)
+{
+	char *endp;
+	int contrast = simple_strtoul(buf, &endp, 0);
+	size_t size = endp - buf;
+
+	if (*endp && isspace(*endp))
+		size++;
+	if (size != count)
+		return -EINVAL;
+
+	contrast /= 2;
+	if (contrast > 127)
+		contrast = 127;
+
+	ch7024_write_reg(CH7024_TVCTA, contrast);
+
+	return count;
+}
+
+DRIVER_ATTR(contrast, 0644, show_contrast, store_contrast);
+
+static ssize_t show_hue(struct device_driver *dev, char *buf)
+{
+	u32 reg;
+	reg = ch7024_read_reg(CH7024_TVHUE);
+
+	reg *= 2;		/* Scale to 0 - 255 */
+
+	return snprintf(buf, PAGE_SIZE, "%u", reg);
+}
+
+static ssize_t store_hue(struct device_driver *dev, const char *buf,
+			 size_t count)
+{
+	char *endp;
+	int hue = simple_strtoul(buf, &endp, 0);
+	size_t size = endp - buf;
+
+	if (*endp && isspace(*endp))
+		size++;
+	if (size != count)
+		return -EINVAL;
+
+	hue /= 2;
+	if (hue > 127)
+		hue = 127;
+
+	ch7024_write_reg(CH7024_TVHUE, hue);
+
+	return count;
+}
+
+DRIVER_ATTR(hue, 0644, show_hue, store_hue);
+
+static ssize_t show_saturation(struct device_driver *dev, char *buf)
+{
+	u32 reg;
+	reg = ch7024_read_reg(CH7024_TVSAT);
+
+	reg *= 2;		/* Scale to 0 - 255 */
+
+	return snprintf(buf, PAGE_SIZE, "%u", reg);
+}
+
+static ssize_t store_saturation(struct device_driver *dev, const char *buf,
+				size_t count)
+{
+	char *endp;
+	int saturation = simple_strtoul(buf, &endp, 0);
+	size_t size = endp - buf;
+
+	if (*endp && isspace(*endp))
+		size++;
+	if (size != count)
+		return -EINVAL;
+
+	saturation /= 2;
+	if (saturation > 127)
+		saturation = 127;
+
+	ch7024_write_reg(CH7024_TVSAT, saturation);
+
+	return count;
+}
+
+DRIVER_ATTR(saturation, 0644, show_saturation, store_saturation);
+
+static ssize_t show_sharpness(struct device_driver *dev, char *buf)
+{
+	u32 reg;
+	reg = ch7024_read_reg(CH7024_TVSHARP);
+
+	reg *= 32;		/* Scale to 0 - 255 */
+
+	return snprintf(buf, PAGE_SIZE, "%u", reg);
+}
+
+static ssize_t store_sharpness(struct device_driver *dev, const char *buf,
+			       size_t count)
+{
+	char *endp;
+	int sharpness = simple_strtoul(buf, &endp, 0);
+	size_t size = endp - buf;
+
+	if (*endp && isspace(*endp))
+		size++;
+	if (size != count)
+		return -EINVAL;
+
+	sharpness /= 32;	/* Scale to 0 - 7 */
+	if (sharpness > 7)
+		sharpness = 7;
+
+	ch7024_write_reg(CH7024_TVSHARP, sharpness);
+
+	return count;
+}
+
+DRIVER_ATTR(sharpness, 0644, show_sharpness, store_sharpness);
+
+static int ch7024_probe(struct i2c_client *client, const struct i2c_device_id *dev_id)
+{
+	int ret, i;
+	u32 id;
+	u32 irqtype;
+	struct mxc_tvout_platform_data *plat_data = client->dev.platform_data;
+
+	ch7024_client = client;
+
+	io_reg = regulator_get(&client->dev, plat_data->io_reg);
+	core_reg = regulator_get(&client->dev, plat_data->core_reg);
+	analog_reg = regulator_get(&client->dev, plat_data->analog_reg);
+
+	regulator_enable(io_reg);
+	regulator_enable(core_reg);
+	regulator_enable(analog_reg);
+	msleep(200);
+
+	id = ch7024_read_reg(CH7024_DEVID);
+
+	regulator_disable(core_reg);
+	regulator_disable(io_reg);
+	regulator_disable(analog_reg);
+
+	if (id < 0 || id != CH7024_DEVICE_ID) {
+		printk(KERN_ERR
+		       "ch7024: TV encoder not present: id = %x\n", id);
+		return -ENODEV;
+	}
+	printk(KERN_ERR "ch7024: TV encoder present: id = %x\n", id);
+
+	detect_gpio = plat_data->detect_line;
+
+	if (client->irq > 0) {
+		if (ch7024_detect() == 0)
+			irqtype = IRQF_TRIGGER_RISING;
+		else
+			irqtype = IRQF_TRIGGER_FALLING;
+
+		ret = request_irq(client->irq, hp_detect_handler, irqtype,
+				  client->name, client);
+		if (ret < 0)
+			goto err0;
+
+		ret = driver_create_file(&client->driver->driver,
+					 &driver_attr_headphone);
+		if (ret < 0)
+			goto err1;
+	}
+
+	ret = driver_create_file(&client->driver->driver,
+				 &driver_attr_brightness);
+	if (ret)
+		goto err2;
+
+	ret = driver_create_file(&client->driver->driver,
+				 &driver_attr_contrast);
+	if (ret)
+		goto err3;
+	ret = driver_create_file(&client->driver->driver, &driver_attr_hue);
+	if (ret)
+		goto err4;
+	ret = driver_create_file(&client->driver->driver,
+				 &driver_attr_saturation);
+	if (ret)
+		goto err5;
+	ret = driver_create_file(&client->driver->driver,
+				 &driver_attr_sharpness);
+	if (ret)
+		goto err6;
+
+	for (i = 0; i < num_registered_fb; i++) {
+		if (strcmp(registered_fb[i]->fix.id, "DISP3 BG") == 0) {
+			ch7024_fbi = registered_fb[i];
+			break;
+		}
+	}
+	if (ch7024_fbi != NULL) {
+		fb_add_videomode(&video_modes[0], &ch7024_fbi->modelist);
+		fb_add_videomode(&video_modes[1], &ch7024_fbi->modelist);
+	}
+	fb_register_client(&nb);
+
+	return 0;
+      err6:
+	driver_remove_file(&client->driver->driver, &driver_attr_saturation);
+      err5:
+	driver_remove_file(&client->driver->driver, &driver_attr_hue);
+      err4:
+	driver_remove_file(&client->driver->driver, &driver_attr_contrast);
+      err3:
+	driver_remove_file(&client->driver->driver, &driver_attr_brightness);
+      err2:
+	driver_remove_file(&client->driver->driver, &driver_attr_headphone);
+      err1:
+	free_irq(client->irq, client);
+      err0:
+	return ret;
+}
+
+static int ch7024_remove(struct i2c_client *client)
+{
+	free_irq(client->irq, client);
+
+	regulator_put(io_reg);
+	regulator_put(core_reg);
+	regulator_put(analog_reg);
+
+	driver_remove_file(&client->driver->driver, &driver_attr_headphone);
+	driver_remove_file(&client->driver->driver, &driver_attr_brightness);
+	driver_remove_file(&client->driver->driver, &driver_attr_contrast);
+	driver_remove_file(&client->driver->driver, &driver_attr_hue);
+	driver_remove_file(&client->driver->driver, &driver_attr_saturation);
+	driver_remove_file(&client->driver->driver, &driver_attr_sharpness);
+
+	fb_unregister_client(&nb);
+
+	ch7024_client = 0;
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/*!
+ * PM suspend/resume routing
+ */
+static int ch7024_suspend(struct i2c_client *client, pm_message_t state)
+{
+	pr_debug("Ch7024 suspend routing..\n");
+	if (enabled) {
+		ch7024_disable();
+		pm_status = 1;
+	} else {
+		pm_status = 0;
+	}
+	return 0;
+}
+
+static int ch7024_resume(struct i2c_client *client)
+{
+	pr_debug("Ch7024 resume routing..\n");
+	if (pm_status) {
+		ch7024_enable();
+		ch7024_setup(ch7024_cur_mode);
+	}
+	return 0;
+}
+#else
+#define ch7024_suspend	NULL
+#define ch7024_resume	NULL
+#endif
+
+static const struct i2c_device_id ch7024_id[] = {
+	{ "ch7024", 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, ch7024_id);
+
+static struct i2c_driver ch7024_driver = {
+	.driver = {
+		   .name = "ch7024",
+		   },
+	.probe = ch7024_probe,
+	.remove = ch7024_remove,
+	.suspend = ch7024_suspend,
+	.resume = ch7024_resume,
+	.id_table = ch7024_id,
+};
+
+static int __init ch7024_init(void)
+{
+	return i2c_add_driver(&ch7024_driver);
+}
+
+static void __exit ch7024_exit(void)
+{
+	i2c_del_driver(&ch7024_driver);
+}
+
+module_init(ch7024_init);
+module_exit(ch7024_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("CH7024 TV encoder driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/mxc/fs453.c b/drivers/video/mxc/fs453.c
new file mode 100644
index 000000000000..449675e83aef
--- /dev/null
+++ b/drivers/video/mxc/fs453.c
@@ -0,0 +1,494 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup FS453 Focus FS453 TV Encoder Driver
+ */
+/*!
+ * @file fs453.c
+ * @brief Driver for FS453/4 TV encoder
+ *
+ * @ingroup FS453
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/ioctl.h>
+#include <linux/video_encoder.h>
+
+#include "fs453.h"
+
+/*
+ * FIXME: VGA mode is not defined by video_encoder.h
+ * while FS453 supports VGA output.
+ */
+#ifndef VIDEO_ENCODER_VGA
+#define VIDEO_ENCODER_VGA	32
+#endif
+
+/*!
+ * This stucture contains the status of FS453.
+ */
+struct fs453_data {
+	int norm;
+	int input;
+	int output;
+	int enable;
+};
+
+/*!
+ * This structure contains all the register values needed to program the
+ * TV encoder chip.  This structure is instantiated and initialized for
+ * each supported output standard.
+ */
+struct fs453_presets {
+	u32 mode;		/*! Video mode */
+	u16 qpr;		/*! Quick Program Register */
+	u16 pwr_mgmt;		/*! Power Management */
+	u16 iho;		/*! Input Horizontal Offset */
+	u16 ivo;		/*! Input Vertical Offset */
+	u16 ihw;		/*! Input Horizontal Width */
+	u16 vsc;		/*! Vertical Scaling Coefficient */
+	u16 hsc;		/*! Horizontal Scaling Coefficient */
+	u16 bypass;		/*! Bypass */
+	u16 misc;		/*! Miscellaneous Bits Register */
+	u8 misc46;		/*! Miscellaneous Bits Register 46 */
+	u8 misc47;		/*! Miscellaneous Bits Register 47 */
+	u32 ncon;		/*! Numerator of NCO Word */
+	u32 ncod;		/*! Denominator of NCO Word */
+	u16 pllm;		/*! PLL M and Pump Control */
+	u16 plln;		/*! PLL N */
+	u16 pllpd;		/*! PLL Post-Divider */
+	u16 vid_cntl0;		/*! Video Control 0 */
+	u16 dac_cntl;		/*! DAC Control */
+	u16 fifo_lat;		/*! FIFO Latency */
+};
+
+static struct fs453_presets fs453_vga_presets = {
+	.mode = VIDEO_ENCODER_VGA,
+	.qpr = 0x9cb0,
+	.pwr_mgmt = 0x0408,
+	.misc = 0x0103,
+	.ncon = 0x00000000,
+	.ncod = 0x00000000,
+	.misc46 = 0xa9,
+	.misc47 = 0x00,
+	.pllm = 0x317f,
+	.plln = 0x008e,
+	.pllpd = 0x0202,
+	.vid_cntl0 = 0x4006,
+	.dac_cntl = 0x00e4,
+	.fifo_lat = 0x0082,
+};
+
+static struct fs453_presets fs453_ntsc_presets = {
+	.mode = VIDEO_ENCODER_NTSC,
+	.qpr = 0x9c48,
+	.pwr_mgmt = 0x0200,
+	.misc = 0x0103,
+	.ncon = 0x00000001,
+	.ncod = 0x00000001,
+	.misc46 = 0x01,
+	.misc47 = 0x00,
+	.pllm = 0x4000 | (296 - 17),
+	.plln = 30 - 1,
+	.pllpd = ((10 - 1) << 8) | (10 - 1),
+	.iho = 0,
+	.ivo = 40,
+	.ihw = 768,
+	.vsc = 789,
+	.hsc = 0x0000,
+	.bypass = 0x000a,
+	.vid_cntl0 = 0x0340,
+	.dac_cntl = 0x00e4,
+	.fifo_lat = 0x0082,
+};
+
+static struct fs453_presets fs453_pal_presets = {
+	.mode = VIDEO_ENCODER_PAL,
+	.qpr = 0x9c41,
+	.pwr_mgmt = 0x0200,
+	.misc = 0x0103,
+	.ncon = 0x00000001,
+	.ncod = 0x00000001,
+	.misc46 = 0x01,
+	.misc47 = 0x00,
+	.pllm = 0x4000 | (296 - 17),
+	.plln = 30 - 1,
+	.pllpd = ((10 - 1) << 8) | (10 - 1),
+	.iho = 0,
+	.ivo = 19,
+	.ihw = 768,
+	.vsc = 8200,
+	.hsc = 0x0000,
+	.bypass = 0x000a,
+	.vid_cntl0 = 0x0340,
+	.dac_cntl = 0x00e4,
+	.fifo_lat = 0x0082,
+};
+
+static int fs453_preset(struct i2c_client *client,
+			struct fs453_presets *presets);
+static int fs453_enable(struct i2c_client *client, int enable);
+
+static struct i2c_driver fs453_driver;
+/*
+ * FIXME: fs453_client will represent the first FS453 device found by
+ * the I2C subsystem, which means fs453_ioctl() always works on the
+ * first FS453 device.
+ */
+static struct i2c_client *fs453_client = 0;
+
+static int fs453_command(struct i2c_client *client, unsigned int cmd, void *arg)
+{
+	int val;
+	char *smode = 0;
+	struct video_encoder_capability *cap;
+	struct fs453_data *data = i2c_get_clientdata(client);
+	int ret = 0;
+
+	switch (cmd) {
+	case ENCODER_GET_CAPABILITIES:
+		cap = arg;
+		cap->flags =
+		    VIDEO_ENCODER_PAL | VIDEO_ENCODER_NTSC | VIDEO_ENCODER_VGA;
+		cap->inputs = 1;
+		cap->outputs = 1;
+		break;
+	case ENCODER_SET_NORM:
+		val = *(int *)arg;
+		switch (val) {
+		case VIDEO_ENCODER_PAL:
+			ret = fs453_preset(client, &fs453_pal_presets);
+			smode = "PAL";
+			break;
+		case VIDEO_ENCODER_NTSC:
+			ret = fs453_preset(client, &fs453_ntsc_presets);
+			smode = "NTSC";
+			break;
+		case VIDEO_ENCODER_VGA:
+			ret = fs453_preset(client, &fs453_vga_presets);
+			smode = "VGA";
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+		if (!ret) {
+			data->norm = val;
+			data->enable = 1;
+			pr_debug("FS453: switched to %s\n", smode);
+		}
+		break;
+	case ENCODER_SET_INPUT:
+		val = *(int *)arg;
+		/* We have only one input */
+		if (val != 0)
+			return -EINVAL;
+		data->input = val;
+		break;
+	case ENCODER_SET_OUTPUT:
+		val = *(int *)arg;
+		/* We have only one output */
+		if (val != 0)
+			return -EINVAL;
+		data->output = val;
+		break;
+	case ENCODER_ENABLE_OUTPUT:
+		val = *(int *)arg;
+		if ((ret = fs453_enable(client, val)) == 0)
+			data->enable = val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int i2c_fs453_detect_client(struct i2c_adapter *adapter, int address,
+				   int kind)
+{
+	int chip_id;
+	struct i2c_client *client;
+	struct fs453_data *data;
+	const char *client_name = "FS453 I2C dev";
+
+	pr_debug("FS453: i2c-bus: %s; address: 0x%x\n", adapter->name, address);
+
+	/* Let's see whether this adapter can support what we need */
+	if (!i2c_check_functionality(adapter,
+				     I2C_FUNC_SMBUS_WORD_DATA |
+				     I2C_FUNC_SMBUS_BYTE_DATA)) {
+		pr_debug("FS453: SMBUS word/byte operations not permited.\n");
+		return 0;
+	}
+
+	client =
+	    kmalloc(sizeof(struct i2c_client) + sizeof(struct fs453_data),
+		    GFP_KERNEL);
+	if (!client)
+		return -ENOMEM;
+
+	data = (struct fs453_data *)(client + 1);
+	client->addr = address;
+	client->adapter = adapter;
+	client->driver = &fs453_driver;
+	client->flags = 0;
+
+	/*
+	 * The generic detection, that is skipped if any force
+	 * parameter was used.
+	 */
+	if (kind < 0) {
+		chip_id = i2c_smbus_read_word_data(client, FS453_ID);
+		if (chip_id != FS453_CHIP_ID) {
+			pr_info("FS453: TV encoder not present\n");
+			kfree(client);
+			return 0;
+		} else
+			pr_info("FS453: TV encoder present, ID=0x%04X\n",
+				chip_id);
+	}
+	strcpy(client->name, client_name);
+
+	/* FS453 default status */
+	data->input = 0;
+	data->output = 0;
+	data->norm = 0;
+	data->enable = 0;
+	i2c_set_clientdata(client, data);
+
+	if (i2c_attach_client(client)) {
+		pr_debug("FS453: i2c_attach_client() failed.\n");
+		kfree(client);
+	} else if (fs453_client == 0)
+		fs453_client = client;
+
+	return 0;
+}
+
+static unsigned short normal_i2c[] = { FS453_I2C_ADDR, I2C_CLIENT_END };
+
+/* Magic definition of all other variables and things */
+I2C_CLIENT_INSMOD;
+
+static int i2c_fs453_attach(struct i2c_adapter *adap)
+{
+	return i2c_probe(adap, &addr_data, &i2c_fs453_detect_client);
+}
+
+static int i2c_fs453_detach(struct i2c_client *client)
+{
+	int err;
+
+	if ((err = i2c_detach_client(client))) {
+		pr_debug("FS453: i2c_detach_client() failed\n");
+		return err;
+	}
+
+	if (fs453_client == client)
+		fs453_client = 0;
+
+	kfree(client);
+	return 0;
+}
+
+static struct i2c_driver fs453_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "FS453 driver",
+		   },
+	.attach_adapter = &i2c_fs453_attach,
+	.detach_client = &i2c_fs453_detach,
+	.command = fs453_command,
+};
+
+/*!
+ * @brief Function to read TV encoder registers on the i2c bus
+ * @param     client	I2C client structure
+ * @param     reg	The register number
+ * @param     value	Pointer to buffer to receive the read data
+ * @param     len	Number of 16-bit register words to read
+ * @return    0 on success, others on failure
+ */
+static int fs453_read(struct i2c_client *client, u8 reg, u32 * value, u32 len)
+{
+	if (len == 1)
+		*value = i2c_smbus_read_byte_data(client, reg);
+	else if (len == 2)
+		*value = i2c_smbus_read_word_data(client, reg);
+	else if (len == 4) {
+		*(u16 *) value = i2c_smbus_read_word_data(client, reg);
+		*((u16 *) value + 1) =
+		    i2c_smbus_read_word_data(client, reg + 2);
+	} else
+		return -EINVAL;
+
+	return 0;
+}
+
+/*!
+ * @brief Function to write a TV encoder register on the i2c bus
+ * @param     client	I2C client structure
+ * @param     reg	The register number
+ * @param     value	The value to write
+ * @param     len	Number of words to write (must be 1)
+ * @return    0 on success, others on failure
+ */
+static int fs453_write(struct i2c_client *client, u8 reg, u32 value, u32 len)
+{
+	if (len == 1)
+		return i2c_smbus_write_byte_data(client, reg, (u8) value);
+	else if (len == 2)
+		return i2c_smbus_write_word_data(client, reg, (u16) value);
+	else if (len == 4)
+		return i2c_smbus_write_block_data(client, reg, len,
+						  (u8 *) & value);
+	else
+		return -EINVAL;
+}
+
+/*!
+ * @brief Function to initialize the TV encoder
+ * @param     client	I2C client structure
+ * @param     presets	FS453 pre-defined register values
+ * @return    0 on success; ENODEV if the encoder wasn't found
+ */
+static int fs453_preset(struct i2c_client *client,
+			struct fs453_presets *presets)
+{
+	u32 data;
+
+	if (!client)
+		return -ENODEV;
+
+	/* set the clock level */
+	fs453_write(client, FS453_CR, CR_GCC_CK_LVL, 2);
+
+	/* soft reset the encoder */
+	fs453_read(client, FS453_CR, &data, 2);
+	fs453_write(client, FS453_CR, data | CR_SRESET, 2);
+	fs453_write(client, FS453_CR, data & ~CR_SRESET, 2);
+
+	fs453_write(client, FS453_BYPASS, presets->bypass, 2);
+
+	/* Write the QPR (Quick Programming Register). */
+	fs453_write(client, FS453_QPR, presets->qpr, 2);
+
+	if (presets->mode != VIDEO_ENCODER_VGA) {
+		/* set up the NCO and PLL */
+		fs453_write(client, FS453_NCON, presets->ncon, 4);
+		fs453_write(client, FS453_NCOD, presets->ncod, 4);
+		fs453_write(client, FS453_PLL_M_PUMP, presets->pllm, 2);
+		fs453_write(client, FS453_PLL_N, presets->plln, 2);
+		fs453_write(client, FS453_PLL_PDIV, presets->pllpd, 2);
+
+		/* latch the NCO and PLL settings */
+		fs453_read(client, FS453_CR, &data, 2);
+		fs453_write(client, FS453_CR, data | CR_NCO_EN, 2);
+		fs453_write(client, FS453_CR, data & ~CR_NCO_EN, 2);
+	}
+
+	/* customize */
+	fs453_write(client, FS453_PWR_MGNT, presets->pwr_mgmt, 2);
+
+	fs453_write(client, FS453_IHO, presets->iho, 2);
+	fs453_write(client, FS453_IVO, presets->ivo, 2);
+	fs453_write(client, FS453_IHW, presets->ihw, 2);
+	fs453_write(client, FS453_VSC, presets->vsc, 2);
+	fs453_write(client, FS453_HSC, presets->hsc, 2);
+
+	fs453_write(client, FS453_MISC, presets->misc, 2);
+
+	fs453_write(client, FS453_VID_CNTL0, presets->vid_cntl0, 2);
+	fs453_write(client, FS453_MISC_46, presets->misc46, 1);
+	fs453_write(client, FS453_MISC_47, presets->misc47, 1);
+
+	fs453_write(client, FS453_DAC_CNTL, presets->dac_cntl, 2);
+	fs453_write(client, FS453_FIFO_LAT, presets->fifo_lat, 2);
+
+	return 0;
+}
+
+/*!
+ * @brief Function to enable/disable the TV encoder
+ * @param     client	I2C client structure
+ * @param     enable	0 to disable, others to enable
+ * @return    0 on success; ENODEV if the encoder wasn't found
+ */
+static int fs453_enable(struct i2c_client *client, int enable)
+{
+	struct fs453_data *data;
+
+	if (!client)
+		return -ENODEV;
+
+	data = i2c_get_clientdata(client);
+
+	if (enable)
+		return fs453_command(client, ENCODER_SET_NORM, &data->norm);
+	else
+		return fs453_write(client, FS453_PWR_MGNT, 0x3BFF, 2);
+}
+
+/*!
+ * @brief FS453 control routine
+ * @param	cmd	Control command
+ * @param	arg	Control argument
+ * @return	0 on success, others on failure
+ */
+int fs453_ioctl(unsigned int cmd, void *arg)
+{
+	if (!fs453_client)
+		return -ENODEV;
+
+	return fs453_command(fs453_client, cmd, arg);
+}
+
+/*!
+ * @brief Probe for the TV enocder and initialize the driver
+ * @return    0 on success, others on failure
+ */
+static int __init fs453_init(void)
+{
+	int err;
+
+	pr_info("FS453/4 driver, (c) 2005 Freescale Semiconductor, Inc.\n");
+
+	if ((err = i2c_add_driver(&fs453_driver))) {
+		pr_info("FS453: driver registration failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Module exit routine
+ */
+static void __exit fs453_exit(void)
+{
+	i2c_del_driver(&fs453_driver);
+}
+
+module_init(fs453_init);
+module_exit(fs453_exit);
+
+EXPORT_SYMBOL(fs453_ioctl);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("FS453/4 TV encoder driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/mxc/fs453.h b/drivers/video/mxc/fs453.h
new file mode 100644
index 000000000000..1edd11481b76
--- /dev/null
+++ b/drivers/video/mxc/fs453.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+ /*!
+  * @file fs453.h
+  * @brief Driver for FS453/4 TV encoder
+  *
+  * @ingroup FS453
+  */
+
+#ifndef __FS453_H__
+#define __FS453_H__
+
+/* I2C address of the FS453 chip */
+
+#define I2C1_BUS	0
+#define FS453_I2C_ADDR	0x6A
+
+/*!
+ *
+ * FS453 register file
+ *
+ */
+#define FS453_IHO		0x00	/*! Input Horizontal Offset */
+#define FS453_IVO		0x02	/*! Input Vertical Offset */
+#define FS453_IHW		0x04	/*! Input Horizontal Width */
+#define FS453_VSC		0x06	/*! Vertical Scaling Coefficient */
+#define FS453_HSC		0x08	/*! Horizontal Scaling Coefficient */
+#define FS453_BYPASS		0x0A	/*! BYPASS */
+#define FS453_CR		0x0C	/*! Command Register */
+#define FS453_MISC		0x0E	/*! Miscellaneous Bits Register */
+#define FS453_NCON		0x10	/*! Numerator of NCO Word */
+#define FS453_NCOD		0x14	/*! Denominator of NCO Word */
+#define FS453_PLL_M_PUMP	0x18	/*! PLL M and Pump Control */
+#define FS453_PLL_N		0x1A	/*! PLL N */
+#define FS453_PLL_PDIV		0x1C	/*! PLL Post-Divider */
+#define FS453_SHP		0x24	/*! Sharpness Filter */
+#define FS453_FLK		0x26	/*! Filcker Filter Coefficient */
+#define FS453_GPIO		0x28	/*! General Purpose I/O, Output Enab */
+#define FS453_ID		0x32	/*! Part Identification Number */
+#define FS453_STATUS		0x34	/*! Status Port */
+#define FS453_FIFO_SP		0x36	/*! FIFO Status Port Fill/Underrun */
+#define FS453_FIFO_LAT		0x38	/*! FIFO Latency */
+#define FS453_CHR_FREQ		0x40	/*! Chroma Subcarrier Frequency */
+#define FS453_CHR_PHASE		0x44	/*! Chroma Phase */
+#define FS453_MISC_45		0x45	/*! Miscellaneous Bits Register 45 */
+#define FS453_MISC_46		0x46	/*! Miscellaneous Bits Register 46 */
+#define FS453_MISC_47		0x47	/*! Miscellaneous Bits Register 47 */
+#define FS453_HSYNC_WID		0x48	/*! HSync Width */
+#define FS453_BURST_WID		0x49	/*! Burst Width */
+#define FS453_BPORCH		0x4A	/*! Back Porch Width */
+#define FS453_CB_BURST		0x4B	/*! Cb Burst Amplitude */
+#define FS453_CR_BURST		0x4C	/*! Cr Burst Amplitude */
+#define FS453_MISC_4D		0x4D	/*! Miscellaneous Bits Register 4D */
+#define FS453_BLACK_LVL		0x4E	/*! Black Level */
+#define FS453_BLANK_LVL		0x50	/*! Blank Level */
+#define FS453_NUM_LINES		0x57	/*! Number of Lines */
+#define FS453_WHITE_LVL		0x5E	/*! White Level */
+#define FS453_CB_GAIN		0x60	/*! Cb Color Saturation */
+#define FS453_CR_GAIN		0x62	/*! Cr Color Saturation */
+#define FS453_TINT		0x65	/*! Tint */
+#define FS453_BR_WAY		0x69	/*! Width of Breezeway */
+#define FS453_FR_PORCH		0x6C	/*! Front Porch */
+#define FS453_NUM_PIXELS	0x71	/*! Total num. of luma/chroma Pixels */
+#define FS453_1ST_LINE		0x73	/*! First Video Line */
+#define FS453_MISC_74		0x74	/*! Miscellaneous Bits Register 74 */
+#define FS453_SYNC_LVL		0x75	/*! Sync Level */
+#define FS453_VBI_BL_LVL	0x7C	/*! VBI Blank Level */
+#define FS453_SOFT_RST		0x7E	/*! Encoder Soft Reset */
+#define FS453_ENC_VER		0x7F	/*! Encoder Version */
+#define FS453_WSS_CONFIG	0x80	/*! WSS Configuration Register */
+#define FS453_WSS_CLK		0x81	/*! WSS Clock */
+#define FS453_WSS_DATAF1	0x83	/*! WSS Data Field 1 */
+#define FS453_WSS_DATAF0	0x86	/*! WSS Data Field 0 */
+#define FS453_WSS_LNF1		0x89	/*! WSS Line Number Field 1 */
+#define FS453_WSS_LNF0		0x8A	/*! WSS Line Number Field 0 */
+#define FS453_WSS_LVL		0x8B	/*! WSS Level */
+#define FS453_MISC_8D		0x8D	/*! Miscellaneous Bits Register 8D */
+#define FS453_VID_CNTL0		0x92	/*! Video Control 0 */
+#define FS453_HD_FP_SYNC	0x94	/*! Horiz. Front Porch & HSync Width */
+#define FS453_HD_YOFF_BP	0x96	/*! HDTV Lum. Offset & Back Porch */
+#define FS453_SYNC_DL		0x98	/*! Sync Delay Value */
+#define FS453_LD_DET		0x9C	/*! DAC Load Detect */
+#define FS453_DAC_CNTL		0x9E	/*! DAC Control */
+#define FS453_PWR_MGNT		0xA0	/*! Power Management */
+#define FS453_RED_MTX		0xA2	/*! RGB to YCrCb Matrix Red Coeff. */
+#define FS453_GRN_MTX		0xA4	/*! RGB to YCrCb Matrix Green Coeff. */
+#define FS453_BLU_MTX		0xA6	/*! RGB to YCrCb Matrix Blue Coeff. */
+#define FS453_RED_SCL		0xA8	/*! RGB to YCrCb Scaling Red Coeff. */
+#define FS453_GRN_SCL		0xAA	/*! RGB to YCrCb Scaling Green Coeff. */
+#define FS453_BLU_SCL		0xAC	/*! RGB to YCrCb Scaling Blue Coeff. */
+#define FS453_CC_FIELD_1	0xAE	/*! Closed Caption Field 1 Data */
+#define FS453_CC_FIELD_2	0xB0	/*! Closed Caption Field 2 Data */
+#define FS453_CC_CONTROL	0xB2	/*! Closed Caption Control */
+#define FS453_CC_BLANK_VALUE	0xB4	/*! Closed Caption Blanking Value */
+#define FS453_CC_BLANK_SAMPLE	0xB6	/*! Closed Caption Blanking Sample */
+#define FS453_HACT_ST		0xB8	/*! HDTV Horizontal Active Start */
+#define FS453_HACT_WD		0xBA	/*! HDTV Horizontal Active Width */
+#define FS453_VACT_ST		0xBC	/*! HDTV Veritical Active Width */
+#define FS453_VACT_HT		0xBE	/*! HDTV Veritical Active Height */
+#define FS453_PR_PB_SCALING	0xC0	/*! Pr and Pb Relative Scaling */
+#define FS453_LUMA_BANDWIDTH	0xC2	/*! Luminance Frequency Response */
+#define FS453_QPR		0xC4	/*! Quick Program Register */
+
+/*! Command register bits */
+
+#define CR_GCC_CK_LVL		0x2000	/*! Graphics Controller switching lev */
+#define CR_P656_LVL		0x1000	/*! Pixel Port Output switching level */
+#define CR_P656_IN		0x0800	/*! Pixel Port In */
+#define CR_P656_OUT		0x0400	/*! Pixel Port Out */
+#define CR_CBAR_480P		0x0200	/*! 480P Color Bars */
+#define CR_PAL_NTSCIN		0x0100	/*! PAL or NTSC input */
+#define CR_SYNC_MS		0x0080	/*! Sync Master or Slave */
+#define CR_FIFO_CLR		0x0040	/*! FIFO Clear */
+#define CR_CACQ_CLR		0x0020	/*! CACQ Clear */
+#define CR_CDEC_BP		0x0010	/*! Chroma Decimator Bypass */
+#define CR_NCO_EN		0x0002	/*! Enable NCO Latch */
+#define CR_SRESET		0x0001	/*! Soft Reset */
+
+/*! Chip ID register bits */
+
+#define FS453_CHIP_ID		0xFE05	/*! Chip ID register expected value */
+
+#endif				/* __FS453_H__ */
diff --git a/drivers/video/mxc/mx2fb.c b/drivers/video/mxc/mx2fb.c
new file mode 100644
index 000000000000..bdf7d19e19f1
--- /dev/null
+++ b/drivers/video/mxc/mx2fb.c
@@ -0,0 +1,1346 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup Framebuffer_MX27 Framebuffer Driver for MX27.
+ */
+
+/*!
+ * @file mx2fb.c
+ *
+ * @brief Frame buffer driver for MX27 ADS.
+ *
+ * @ingroup Framebuffer_MX27
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/clk.h>
+#include <linux/mxcfb.h>
+#include <asm/uaccess.h>
+
+#include "mx2fb.h"
+
+#define MX2FB_TYPE_BG          0
+#define MX2FB_TYPE_GW          1
+
+extern void gpio_lcdc_active(void);
+extern void gpio_lcdc_inactive(void);
+extern void board_power_lcd(int on);
+
+static char *fb_mode = 0;
+static int fb_enabled = 0;
+static unsigned long default_bpp = 16;
+static ATOMIC_NOTIFIER_HEAD(mx2fb_notifier_list);
+static struct clk *lcdc_clk;
+/*!
+ * @brief Structure containing the MX2 specific framebuffer information.
+ */
+struct mx2fb_info {
+	int type;
+	char *id;
+	int registered;
+	int blank;
+	unsigned long pseudo_palette[16];
+};
+
+/* Framebuffer APIs */
+static int mx2fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info);
+static int mx2fb_set_par(struct fb_info *info);
+static int mx2fb_setcolreg(unsigned regno, unsigned red, unsigned green,
+			   unsigned blue, unsigned transp,
+			   struct fb_info *info);
+static int mx2fb_pan_display(struct fb_var_screeninfo *var,
+			     struct fb_info *info);
+static int mx2fb_blank(int blank_mode, struct fb_info *info);
+static int mx2fb_ioctl(struct fb_info *info, unsigned int cmd,
+		       unsigned long arg);
+
+/* Driver entries */
+int __init mx2fb_init(void);
+void __exit mx2fb_exit(void);
+#ifndef MODULE
+static int __init mx2fb_setup(char *);
+#endif
+
+/* Internal functions */
+static int __init _init_fbinfo(struct fb_info *info,
+			       struct platform_device *pdev);
+static int __init _install_fb(struct fb_info *info,
+			      struct platform_device *pdev);
+static void __exit _uninstall_fb(struct fb_info *info);
+static int _map_video_memory(struct fb_info *info);
+static void _unmap_video_memory(struct fb_info *info);
+static void _set_fix(struct fb_info *info);
+static void _enable_lcdc(struct fb_info *info);
+static void _disable_lcdc(struct fb_info *info);
+static void _enable_graphic_window(struct fb_info *info);
+static void _disable_graphic_window(struct fb_info *info);
+static void _update_lcdc(struct fb_info *info);
+static void _request_irq(void);
+static void _free_irq(void);
+
+#ifdef CONFIG_PM
+static int mx2fb_suspend(struct platform_device *pdev, pm_message_t state);
+static int mx2fb_resume(struct platform_device *pdev);
+#else
+#define mx2fb_suspend	0
+#define mx2fb_resume	0
+#endif
+
+static int mx2fb_probe(struct platform_device *pdev);
+
+#ifdef CONFIG_FB_MXC_TVOUT
+#include <linux/video_encoder.h>
+/*
+ * FIXME: VGA mode is not defined by video_encoder.h
+ * while FS453 supports VGA output.
+ */
+#ifndef VIDEO_ENCODER_VGA
+#define VIDEO_ENCODER_VGA	32
+#endif
+
+#define MODE_PAL		"TV-PAL"
+#define MODE_NTSC		"TV-NTSC"
+#define MODE_VGA		"TV-VGA"
+
+extern int fs453_ioctl(unsigned int cmd, void *arg);
+#endif
+
+struct mx2fb_info mx2fbi_bg = {
+	.type = MX2FB_TYPE_BG,
+	.id = "DISP0 BG",
+	.registered = 0,
+};
+
+static struct mx2fb_info mx2fbi_gw = {
+	.type = MX2FB_TYPE_GW,
+	.id = "DISP0 FG",
+	.registered = 0,
+};
+
+/*! Current graphic window information */
+static struct fb_gwinfo g_gwinfo = {
+	.enabled = 0,
+	.alpha_value = 255,
+	.ck_enabled = 0,
+	.ck_red = 0,
+	.ck_green = 0,
+	.ck_blue = 0,
+	.xpos = 0,
+	.ypos = 0,
+};
+
+/*!
+ * @brief Framebuffer information structures.
+ * There are up to 3 framebuffers: background, TVout, and graphic window.
+ * If graphic window is configured, it must be the last framebuffer.
+ */
+static struct fb_info mx2fb_info[] = {
+	{.par = &mx2fbi_bg},
+	{.par = &mx2fbi_gw},
+};
+
+/*!
+ * @brief This structure contains pointers to the power management
+ * callback functions.
+ */
+static struct platform_driver mx2fb_driver = {
+	.driver = {
+		   .name = "mxc_sdc_fb",
+		   .owner = THIS_MODULE,
+		   .bus = &platform_bus_type,
+		   },
+	.probe = mx2fb_probe,
+	.suspend = mx2fb_suspend,
+	.resume = mx2fb_resume,
+};
+
+/*!
+ * @brief Framebuffer file operations
+ */
+static struct fb_ops mx2fb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = mx2fb_check_var,
+	.fb_set_par = mx2fb_set_par,
+	.fb_setcolreg = mx2fb_setcolreg,
+	.fb_blank = mx2fb_blank,
+	.fb_pan_display = mx2fb_pan_display,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+	//.fb_cursor = soft_cursor,
+	.fb_ioctl = mx2fb_ioctl,
+};
+
+/*!
+ * @brief Validates a var passed in.
+ *
+ * @param var	Frame buffer variable screen structure
+ * @param info	Frame buffer structure that represents a single frame buffer
+ *
+ * @return	Negative errno on error, or zero on success.
+ *
+ * Checks to see if the hardware supports the state requested by var passed
+ * in. This function does not alter the hardware state! If the var passed in
+ * is slightly off by what the hardware can support then we alter the var
+ * PASSED in to what we can do. If the hardware doesn't support mode change
+ * a -EINVAL will be returned by the upper layers.
+ *
+ */
+static int mx2fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	unsigned long htotal, vtotal;
+
+	if (var->xres_virtual < var->xres)
+		var->xres_virtual = var->xres;
+	if (var->yres_virtual < var->yres)
+		var->yres_virtual = var->yres;
+
+	if (var->xoffset < 0)
+		var->xoffset = 0;
+
+	if (var->yoffset < 0)
+		var->yoffset = 0;
+
+	if (var->xoffset + info->var.xres > info->var.xres_virtual)
+		var->xoffset = info->var.xres_virtual - info->var.xres;
+
+	if (var->yoffset + info->var.yres > info->var.yres_virtual)
+		var->yoffset = info->var.yres_virtual - info->var.yres;
+
+	if ((var->bits_per_pixel != 32) && (var->bits_per_pixel != 24) &&
+	    (var->bits_per_pixel != 16)) {
+		var->bits_per_pixel = default_bpp;
+	}
+
+	switch (var->bits_per_pixel) {
+	case 16:
+		var->red.length = 5;
+		var->red.offset = 11;
+		var->red.msb_right = 0;
+
+		var->green.length = 6;
+		var->green.offset = 5;
+		var->green.msb_right = 0;
+
+		var->blue.length = 5;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 24:
+		var->red.length = 8;
+		var->red.offset = 16;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 8;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 32:
+		var->red.length = 8;
+		var->red.offset = 16;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 8;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 8;
+		var->transp.offset = 24;
+		var->transp.msb_right = 0;
+		break;
+	}
+
+	if (var->pixclock < 1000) {
+		htotal = var->xres + var->right_margin + var->hsync_len +
+		    var->left_margin;
+		vtotal = var->yres + var->lower_margin + var->vsync_len +
+		    var->upper_margin;
+		var->pixclock = (vtotal * htotal * 6UL) / 100UL;
+		var->pixclock = KHZ2PICOS(var->pixclock);
+		dev_dbg(info->device,
+			"pixclock set for 60Hz refresh = %u ps\n",
+			var->pixclock);
+	}
+
+	var->height = -1;
+	var->width = -1;
+	var->grayscale = 0;
+
+	/* Copy nonstd field to/from sync for fbset usage */
+	var->sync |= var->nonstd;
+	var->nonstd |= var->sync;
+
+	return 0;
+}
+
+/*!
+ * @brief Alters the hardware state.
+ *
+ * @param info	Frame buffer structure that represents a single frame buffer
+ *
+ * @return Zero on success others on failure
+ *
+ * Using the fb_var_screeninfo in fb_info we set the resolution of this
+ * particular framebuffer. This function alters the fb_fix_screeninfo stored
+ * in fb_info. It doesn't not alter var in fb_info since we are using that
+ * data. This means we depend on the data in var inside fb_info to be
+ * supported by the hardware. mx2fb_check_var is always called before
+ * mx2fb_set_par to ensure this.
+ */
+static int mx2fb_set_par(struct fb_info *info)
+{
+	unsigned long len;
+	struct mx2fb_info *mx2fbi = (struct mx2fb_info *)info->par;
+
+	_set_fix(info);
+
+	len = info->var.yres_virtual * info->fix.line_length;
+	if (len > info->fix.smem_len) {
+		if (info->fix.smem_start)
+			_unmap_video_memory(info);
+
+		/* Memory allocation for framebuffer */
+		if (_map_video_memory(info)) {
+			dev_err(info->device, "Unable to allocate fb memory\n");
+			return -ENOMEM;
+		}
+	}
+
+	_update_lcdc(info);
+	if (info->fbops->fb_blank)
+		info->fbops->fb_blank(mx2fbi->blank, info);
+
+	return 0;
+}
+
+/*!
+ * @brief Sets a color register.
+ *
+ * @param regno		Which register in the CLUT we are programming
+ * @param red		The red value which can be up to 16 bits wide
+ * @param green		The green value which can be up to 16 bits wide
+ * @param blue		The blue value which can be up to 16 bits wide.
+ * @param transp	If supported the alpha value which can be up to
+ *                      16 bits wide.
+ * @param info		Frame buffer info structure
+ *
+ * @return		Negative errno on error, or zero on success.
+ *
+ * Set a single color register. The values supplied have a 16 bit magnitude
+ * which needs to be scaled in this function for the hardware. Things to take
+ * into consideration are how many color registers, if any, are supported with
+ * the current color visual. With truecolor mode no color palettes are
+ * supported. Here a psuedo palette is created which we store the value in
+ * pseudo_palette in struct fb_info. For pseudocolor mode we have a limited
+ * color palette.
+ */
+static int mx2fb_setcolreg(unsigned regno, unsigned red, unsigned green,
+			   unsigned blue, unsigned transp, struct fb_info *info)
+{
+	int ret = 1;
+
+	/*
+	 * If greyscale is true, then we convert the RGB value
+	 * to greyscale no matter what visual we are using.
+	 */
+	if (info->var.grayscale)
+		red = green = blue = (19595 * red + 38470 * green +
+				      7471 * blue) >> 16;
+	switch (info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		/*
+		 * 16-bit True Colour.  We encode the RGB value
+		 * according to the RGB bitfield information.
+		 */
+		if (regno < 16) {
+			u32 *pal = info->pseudo_palette;
+			u32 v;
+
+#define CNVT_TOHW(val,width) ((((val)<<(width))+0x7FFF-(val))>>16)
+			red = CNVT_TOHW(red, info->var.red.length);
+			green = CNVT_TOHW(green, info->var.green.length);
+			blue = CNVT_TOHW(blue, info->var.blue.length);
+			transp = CNVT_TOHW(transp, info->var.transp.length);
+#undef CNVT_TOHW
+
+			v = (red << info->var.red.offset) |
+			    (green << info->var.green.offset) |
+			    (blue << info->var.blue.offset) |
+			    (transp << info->var.transp.offset);
+
+			pal[regno] = v;
+			ret = 0;
+		}
+		break;
+	case FB_VISUAL_STATIC_PSEUDOCOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * @brief Pans the display.
+ *
+ * @param var	Frame buffer variable screen structure
+ * @param info	Frame buffer structure that represents a single frame buffer
+ *
+ * @return	Negative errno on error, or zero on success.
+ *
+ * Pan (or wrap, depending on the `vmode' field) the display using the
+ * 'xoffset' and 'yoffset' fields of the 'var' structure. If the values
+ * don't fit, return -EINVAL.
+ */
+static int mx2fb_pan_display(struct fb_var_screeninfo *var,
+			     struct fb_info *info)
+{
+	if ((info->var.xoffset == var->xoffset) &&
+	    (info->var.yoffset == var->yoffset)) {
+		return 0;	/* No change, do nothing */
+	}
+
+	if (var->xoffset < 0 || var->yoffset < 0
+	    || var->xoffset + info->var.xres > info->var.xres_virtual
+	    || var->yoffset + info->var.yres > info->var.yres_virtual)
+		return -EINVAL;
+
+	info->var.xoffset = var->xoffset;
+	info->var.yoffset = var->yoffset;
+
+	_update_lcdc(info);
+
+	if (var->vmode & FB_VMODE_YWRAP) {
+		info->var.vmode |= FB_VMODE_YWRAP;
+	} else {
+		info->var.vmode &= ~FB_VMODE_YWRAP;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Blanks the display.
+ *
+ * @param blank_mode	The blank mode we want.
+ * @param info		Frame buffer structure that represents a single frame buffer
+ *
+ * @return		Negative errno on error, or zero on success.
+ *
+ * Blank the screen if blank_mode != 0, else unblank. Return 0 if blanking
+ * succeeded, != 0 if un-/blanking failed.
+ * blank_mode == 2: suspend vsync
+ * blank_mode == 3: suspend hsync
+ * blank_mode == 4: powerdown
+ */
+static int mx2fb_blank(int blank_mode, struct fb_info *info)
+{
+	struct mx2fb_info *mx2fbi = (struct mx2fb_info *)info->par;
+
+	dev_dbg(info->device, "blank mode = %d\n", blank_mode);
+
+	mx2fbi->blank = blank_mode;
+
+	switch (blank_mode) {
+	case FB_BLANK_POWERDOWN:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_NORMAL:
+		_disable_lcdc(info);
+		break;
+	case FB_BLANK_UNBLANK:
+		_enable_lcdc(info);
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Ioctl function to support customized ioctl operations.
+ *
+ * @param info	Framebuffer structure that represents a single frame buffer
+ * @param cmd	The command number
+ * @param arg	Argument which depends on cmd
+ *
+ * @return	Negative errno on error, or zero on success.
+ */
+static int mx2fb_ioctl(struct fb_info *info, unsigned int cmd,
+		       unsigned long arg)
+{
+	struct mx2fb_info *mx2fbi = (struct mx2fb_info *)info->par;
+	struct mx2fb_gbl_alpha ga;
+	struct mx2fb_color_key ck;
+
+	switch (cmd) {
+	case MX2FB_SET_GBL_ALPHA:
+		if (mx2fbi->type != MX2FB_TYPE_GW)
+			return -ENODEV;
+
+		if (!arg)
+			return -EINVAL;
+
+		/* set graphic window information */
+		if (copy_from_user((void *)&ga, (void *)arg, sizeof(ga)))
+			return -EFAULT;
+
+		g_gwinfo.alpha_value = ga.alpha;
+
+		if (g_gwinfo.enabled)
+			_enable_graphic_window(info);
+		else
+			_disable_graphic_window(info);
+		break;
+	case MX2FB_SET_CLR_KEY:
+		if (mx2fbi->type != MX2FB_TYPE_GW)
+			return -ENODEV;
+
+		if (!arg)
+			return -EINVAL;
+
+		/* set graphic window information */
+		if (copy_from_user((void *)&ck, (void *)arg, sizeof(ck)))
+			return -EFAULT;
+
+		g_gwinfo.ck_enabled = ck.enable;
+		g_gwinfo.ck_red = (ck.color_key & 0x003F0000) >> 16;
+		g_gwinfo.ck_green = (ck.color_key & 0x00003F00) >> 8;
+		g_gwinfo.ck_blue = ck.color_key & 0x0000003F;
+
+		if (g_gwinfo.enabled)
+			_enable_graphic_window(info);
+		else
+			_disable_graphic_window(info);
+		break;
+	case FBIOGET_GWINFO:
+		if (mx2fbi->type != MX2FB_TYPE_GW)
+			return -ENODEV;
+
+		if (!arg)
+			return -EINVAL;
+
+		/* get graphic window information */
+		if (copy_to_user((void *)arg, (void *)&g_gwinfo,
+				 sizeof(g_gwinfo)))
+			return -EFAULT;
+		break;
+	case FBIOPUT_GWINFO:
+		if (mx2fbi->type != MX2FB_TYPE_GW)
+			return -ENODEV;
+
+		if (!arg)
+			return -EINVAL;
+
+		/* set graphic window information */
+		if (copy_from_user((void *)&g_gwinfo, (void *)arg,
+				   sizeof(g_gwinfo)))
+			return -EFAULT;
+
+		if (g_gwinfo.enabled)
+			_enable_graphic_window(info);
+		else
+			_disable_graphic_window(info);
+		break;
+#ifdef CONFIG_FB_MXC_TVOUT
+	case ENCODER_GET_CAPABILITIES:{
+			int ret;
+			struct video_encoder_capability cap;
+
+			if (mx2fbi->type != MX2FB_TYPE_BG)
+				return -ENODEV;
+
+			ret = fs453_ioctl(cmd, &cap);
+			if (ret)
+				return ret;
+
+			if (copy_to_user((void *)arg, &cap, sizeof(cap)))
+				return -EFAULT;
+			break;
+		}
+	case ENCODER_SET_NORM:{
+			int ret;
+			unsigned long mode;
+			char *smode;
+			struct fb_var_screeninfo var;
+
+			if (mx2fbi->type != MX2FB_TYPE_BG)
+				return -ENODEV;
+
+			if (copy_from_user(&mode, (void *)arg, sizeof(mode)))
+				return -EFAULT;
+			if ((ret = fs453_ioctl(cmd, &mode)))
+				return ret;
+
+			if (mode == VIDEO_ENCODER_PAL)
+				smode = MODE_PAL;
+			else if (mode == VIDEO_ENCODER_NTSC)
+				smode = MODE_NTSC;
+			else
+				smode = MODE_VGA;
+
+			var = info->var;
+			var.nonstd = 0;
+			ret = fb_find_mode(&var, info, smode, mxcfb_modedb,
+					   mxcfb_modedb_sz, NULL, default_bpp);
+			if ((ret != 1) && (ret != 2))	/* specified mode not found */
+				return -ENODEV;
+
+			info->var = var;
+			fb_mode = smode;
+			return mx2fb_set_par(info);
+		}
+	case ENCODER_SET_INPUT:
+	case ENCODER_SET_OUTPUT:
+	case ENCODER_ENABLE_OUTPUT:{
+			unsigned long varg;
+
+			if (mx2fbi->type != MX2FB_TYPE_BG)
+				return -ENODEV;
+
+			if (copy_from_user(&varg, (void *)arg, sizeof(varg)))
+				return -EFAULT;
+			return fs453_ioctl(cmd, &varg);
+		}
+#endif
+	default:
+		dev_dbg(info->device, "Unknown ioctl command (0x%08X)\n", cmd);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Set fixed framebuffer parameters based on variable settings.
+ *
+ * @param info	framebuffer information pointer
+ * @return	Negative errno on error, or zero on success.
+ */
+static void _set_fix(struct fb_info *info)
+{
+	struct fb_fix_screeninfo *fix = &info->fix;
+	struct fb_var_screeninfo *var = &info->var;
+	struct mx2fb_info *mx2fbi = (struct mx2fb_info *)info->par;
+
+	strncpy(fix->id, mx2fbi->id, strlen(mx2fbi->id));
+	fix->line_length = var->xres_virtual * var->bits_per_pixel / 8;
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->accel = FB_ACCEL_NONE;
+	fix->visual = FB_VISUAL_TRUECOLOR;
+	fix->xpanstep = 1;
+	fix->ypanstep = 1;
+}
+
+/*!
+ * @brief Initialize framebuffer information structure.
+ *
+ * @param info	framebuffer information pointer
+ * @param pdev	pointer to struct device
+ * @return	Negative errno on error, or zero on success.
+ */
+static int __init _init_fbinfo(struct fb_info *info,
+			       struct platform_device *pdev)
+{
+	struct mx2fb_info *mx2fbi = (struct mx2fb_info *)info->par;
+
+	info->device = &pdev->dev;
+	info->var.activate = FB_ACTIVATE_NOW;
+	info->fbops = &mx2fb_ops;
+	info->flags = FBINFO_FLAG_DEFAULT;
+	info->pseudo_palette = &mx2fbi->pseudo_palette;
+
+	/* Allocate colormap */
+	fb_alloc_cmap(&info->cmap, 16, 0);
+
+	return 0;
+}
+
+/*!
+ * @brief Install framebuffer into the system.
+ *
+ * @param info	framebuffer information pointer
+ * @param pdev  pointer to struct device
+ * @return	Negative errno on error, or zero on success.
+ */
+static int __init _install_fb(struct fb_info *info,
+			      struct platform_device *pdev)
+{
+	struct mx2fb_info *mx2fbi = (struct mx2fb_info *)info->par;
+
+	if (_init_fbinfo(info, pdev))
+		return -EINVAL;
+
+	if (fb_mode == 0)
+		fb_mode = pdev->dev.platform_data;
+
+	if (!fb_find_mode(&info->var, info, fb_mode, mxcfb_modedb,
+			  mxcfb_modedb_sz, NULL, default_bpp)) {
+		fb_dealloc_cmap(&info->cmap);
+		return -EBUSY;
+	}
+
+	/* Default Y virtual size is 2x panel size */
+	/* info->var.yres_virtual = info->var.yres << 1; */
+
+	if (mx2fbi->type == MX2FB_TYPE_GW)
+		mx2fbi->blank = FB_BLANK_NORMAL;
+	else
+		mx2fbi->blank = FB_BLANK_UNBLANK;
+
+	if (mx2fb_set_par(info)) {
+		fb_dealloc_cmap(&info->cmap);
+		return -EINVAL;
+	}
+
+	if (register_framebuffer(info) < 0) {
+		_unmap_video_memory(info);
+		fb_dealloc_cmap(&info->cmap);
+		return -EINVAL;
+	}
+
+	mx2fbi->registered = 1;
+	dev_info(info->device, "fb%d: %s fb device registered successfully.\n",
+		 info->node, info->fix.id);
+
+	return 0;
+}
+
+/*!
+ * @brief Uninstall framebuffer from the system.
+ *
+ * @param info	framebuffer information pointer
+ */
+static void __exit _uninstall_fb(struct fb_info *info)
+{
+	struct mx2fb_info *mx2fbi = (struct mx2fb_info *)info->par;
+
+	if (!mx2fbi->registered)
+		return;
+
+	unregister_framebuffer(info);
+	_unmap_video_memory(info);
+	if (&info->cmap)
+		fb_dealloc_cmap(&info->cmap);
+
+	mx2fbi->registered = 0;
+}
+
+/*!
+ * @brief Allocate memory for framebuffer.
+ *
+ * @param info	framebuffer information pointer
+ * @return	Negative errno on error, or zero on success.
+ */
+static int _map_video_memory(struct fb_info *info)
+{
+	info->fix.smem_len = info->fix.line_length * info->var.yres_virtual;
+	info->screen_base = dma_alloc_coherent(0,
+					       info->fix.smem_len,
+					       (dma_addr_t *) & info->fix.
+					       smem_start,
+					       GFP_DMA | GFP_KERNEL);
+
+	if (info->screen_base == 0) {
+		dev_err(info->device, "Unable to allocate fb memory\n");
+		return -EBUSY;
+	}
+	dev_dbg(info->device, "Allocated fb @ paddr=0x%08lX, size=%d.\n",
+		info->fix.smem_start, info->fix.smem_len);
+
+	info->screen_size = info->fix.smem_len;
+
+	/* Clear the screen */
+	memset((char *)info->screen_base, 0, info->fix.smem_len);
+
+	return 0;
+}
+
+/*!
+ * @brief Release memory for framebuffer.
+ * @param info	framebuffer information pointer
+ */
+static void _unmap_video_memory(struct fb_info *info)
+{
+	dma_free_coherent(0, info->fix.smem_len, info->screen_base,
+			  (dma_addr_t) info->fix.smem_start);
+
+	info->screen_base = 0;
+	info->fix.smem_start = 0;
+	info->fix.smem_len = 0;
+}
+
+/*!
+ * @brief Enable LCD controller.
+ * @param info	framebuffer information pointer
+ */
+static void _enable_lcdc(struct fb_info *info)
+{
+	static int first_enable = 1;
+	struct mx2fb_info *mx2fbi = (struct mx2fb_info *)info->par;
+
+	/*
+	 * Graphic window can only be enabled while the HCLK to the LCDC
+	 * is disabled. Once enabled it can subsequently be disabled and
+	 * enabled without turning off the HCLK.
+	 * The graphic window is enabled and then disabled here. So next
+	 * time to enable graphic window the HCLK to LCDC does not need
+	 * to be disabled, and the flicker (due to disabling of HCLK to
+	 * LCDC) is avoided.
+	 */
+	if (first_enable) {
+		_enable_graphic_window(info);
+		_disable_graphic_window(info);
+		first_enable = 0;
+	}
+
+	if (mx2fbi->type == MX2FB_TYPE_GW)
+		_enable_graphic_window(info);
+	else if (!fb_enabled) {
+		clk_enable(lcdc_clk);
+		gpio_lcdc_active();
+		board_power_lcd(1);
+		fb_enabled++;
+#ifdef CONFIG_FB_MXC_TVOUT
+		if (fb_mode) {
+			unsigned long mode = 0;
+
+			if (strcmp(fb_mode, MODE_VGA) == 0)
+				mode = VIDEO_ENCODER_VGA;
+			else if (strcmp(fb_mode, MODE_NTSC) == 0)
+				mode = VIDEO_ENCODER_NTSC;
+			else if (strcmp(fb_mode, MODE_PAL) == 0)
+				mode = VIDEO_ENCODER_PAL;
+			if (mode)
+				fs453_ioctl(ENCODER_SET_NORM, &mode);
+		}
+#endif
+	}
+}
+
+/*!
+ * @brief Disable LCD controller.
+ * @param info	framebuffer information pointer
+ */
+static void _disable_lcdc(struct fb_info *info)
+{
+	struct mx2fb_info *mx2fbi = (struct mx2fb_info *)info->par;
+
+	if (mx2fbi->type == MX2FB_TYPE_GW)
+		_disable_graphic_window(info);
+	else {
+		if (fb_enabled) {
+			gpio_lcdc_inactive();
+			board_power_lcd(0);
+			clk_disable(lcdc_clk);
+			fb_enabled = 0;
+		}
+#ifdef CONFIG_FB_MXC_TVOUT
+		if (fb_mode) {
+			int enable = 0;
+
+			if ((strcmp(fb_mode, MODE_VGA) == 0)
+			    || (strcmp(fb_mode, MODE_NTSC) == 0)
+			    || (strcmp(fb_mode, MODE_PAL) == 0))
+				fs453_ioctl(ENCODER_ENABLE_OUTPUT, &enable);
+		}
+#endif
+	}
+}
+
+/*!
+ * @brief Enable graphic window.
+ * @param info	framebuffer information pointer
+ */
+static void _enable_graphic_window(struct fb_info *info)
+{
+	struct fb_var_screeninfo *var = &info->var;
+
+	g_gwinfo.enabled = 1;
+
+	g_gwinfo.base = (var->yoffset * var->xres_virtual + var->xoffset);
+	g_gwinfo.base *= (var->bits_per_pixel) / 8;
+	g_gwinfo.base += info->fix.smem_start;
+
+	g_gwinfo.xres = var->xres;
+	g_gwinfo.yres = var->yres;
+	g_gwinfo.xres_virtual = var->xres_virtual;
+
+	mx2_gw_set(&g_gwinfo);
+}
+
+/*!
+ * @brief Disable graphic window.
+ * @param info	framebuffer information pointer
+ */
+static void _disable_graphic_window(struct fb_info *info)
+{
+	unsigned long i = 0;
+
+	g_gwinfo.enabled = 0;
+
+	/*
+	 * Set alpha value to zero and reduce gw size, otherwise the graphic
+	 * window will not be able to be enabled again.
+	 */
+	__raw_writel(__raw_readl(LCDC_REG(LCDC_LGWCR)) & 0x00FFFFFF,
+		     LCDC_REG(LCDC_LGWCR));
+	__raw_writel(((16 >> 4) << 20) + 16, LCDC_REG(LCDC_LGWSR));
+	while (i < 1000)
+		i++;
+
+	/* Now disable graphic window */
+	__raw_writel(__raw_readl(LCDC_REG(LCDC_LGWCR)) & ~0x00400000,
+		     LCDC_REG(LCDC_LGWCR));
+
+	dev_dbg(info->device, "Graphic window disabled.\n");
+}
+
+/*!
+ * @brief Setup graphic window properties.
+ * @param gwinfo	graphic window information pointer
+ */
+void mx2_gw_set(struct fb_gwinfo *gwinfo)
+{
+	int width, height, xpos, ypos;
+	int width_bg, height_bg;
+	unsigned long lgwcr = 0x00400000;	/* Graphic window control register */
+
+	if (!gwinfo->enabled) {
+		_disable_graphic_window(0);
+		return;
+	}
+
+	/* Graphic window start address register */
+	__raw_writel(gwinfo->base, LCDC_REG(LCDC_LGWSAR));
+
+	/*
+	 * The graphic window width, height, x position and y position
+	 * must be synced up width the background window, otherwise there
+	 * may be flickering.
+	 */
+	width_bg = (__raw_readl(LCDC_REG(LCDC_LSR)) & 0x03F00000) >> 16;
+	height_bg = __raw_readl(LCDC_REG(LCDC_LSR)) & 0x000003FF;
+
+	width = (gwinfo->xres > width_bg) ? width_bg : gwinfo->xres;
+	height = (gwinfo->yres > height_bg) ? height_bg : gwinfo->yres;
+
+	xpos = gwinfo->xpos;
+	ypos = gwinfo->ypos;
+
+	if (xpos + width > width_bg)
+		xpos = width_bg - width;
+	if (ypos + height > height_bg)
+		ypos = height_bg - height;
+
+	/* Graphic window size register */
+	__raw_writel(((width >> 4) << 20) + height, LCDC_REG(LCDC_LGWSR));
+
+	/* Graphic window virtual page width register */
+	__raw_writel(gwinfo->xres_virtual >> 1, LCDC_REG(LCDC_LGWVPWR));
+
+	/* Graphic window position register */
+	__raw_writel(((xpos & 0x000003FF) << 16) | (ypos & 0x000003FF),
+		     LCDC_REG(LCDC_LGWPR));
+
+	/* Graphic window panning offset register */
+	__raw_writel(0, LCDC_REG(LCDC_LGWPOR));
+
+	/* Graphic window DMA control register */
+	if (cpu_is_mx27_rev(CHIP_REV_2_0) > 0)
+		__raw_writel(0x00040060, LCDC_REG(LCDC_LGWDCR));
+	else
+		__raw_writel(0x00020010, LCDC_REG(LCDC_LGWDCR));
+
+	/* Graphic window control register */
+	lgwcr |= (gwinfo->alpha_value & 0x000000FF) << 24;
+	lgwcr |= gwinfo->ck_enabled ? 0x00800000 : 0;
+	lgwcr |= gwinfo->vs_reversed ? 0x00200000 : 0;
+
+	/*
+	 * Color keying value
+	 * Todo: assume always use RGB565
+	 */
+	lgwcr |= (gwinfo->ck_red & 0x0000003F) << 12;
+	lgwcr |= (gwinfo->ck_green & 0x0000003F) << 6;
+	lgwcr |= gwinfo->ck_blue & 0x0000003F;
+
+	__raw_writel(lgwcr, LCDC_REG(LCDC_LGWCR));
+
+	pr_debug("Graphic window enabled.\n");
+}
+
+/*!
+ * @brief Update LCDC registers
+ * @param info	framebuffer information pointer
+ */
+static void _update_lcdc(struct fb_info *info)
+{
+	unsigned long base;
+	unsigned long perclk, pcd, pcr;
+	struct fb_var_screeninfo *var = &info->var;
+	struct mx2fb_info *mx2fbi = (struct mx2fb_info *)info->par;
+
+	if (mx2fbi->type == MX2FB_TYPE_GW) {
+		_enable_graphic_window(info);
+		return;
+	}
+
+	base = (var->yoffset * var->xres_virtual + var->xoffset);
+	base *= (var->bits_per_pixel) / 8;
+	base += info->fix.smem_start;
+
+	/* Screen start address register */
+	__raw_writel(base, LCDC_REG(LCDC_LSSAR));
+
+	/* Size register */
+	dev_dbg(info->device, "xres = %d, yres = %d\n",
+		info->var.xres, info->var.yres);
+	__raw_writel(((info->var.xres >> 4) << 20) + info->var.yres,
+		     LCDC_REG(LCDC_LSR));
+
+	/* Virtual page width register */
+	__raw_writel(info->var.xres_virtual >> 1, LCDC_REG(LCDC_LVPWR));
+
+	/* To setup LCDC pixel clock */
+	perclk = clk_round_rate(lcdc_clk, 134000000);
+	if (clk_set_rate(lcdc_clk, perclk)) {
+		printk(KERN_INFO "mx2fb: Unable to set clock to %lu\n", perclk);
+		perclk = clk_get_rate(lcdc_clk);
+	}
+
+	/* Calculate pixel clock divider, and round to the nearest integer */
+	pcd = (perclk * 8 / (PICOS2KHZ(var->pixclock) * 1000UL) + 4) / 8;
+	if (--pcd > 0x3F)
+		pcd = 0x3F;
+
+	/* Panel configuration register */
+	pcr = 0xFA008B80 | pcd;
+	pcr |= (var->sync & FB_SYNC_CLK_LAT_FALL) ? 0x00200000 : 0;
+	pcr |= (var->sync & FB_SYNC_DATA_INVERT) ? 0x01000000 : 0;
+	pcr |= (var->sync & FB_SYNC_SHARP_MODE) ? 0x00000040 : 0;
+	pcr |= (var->sync & FB_SYNC_OE_LOW_ACT) ? 0x00100000 : 0;
+	__raw_writel(pcr, LCDC_REG(LCDC_LPCR));
+
+	/* Horizontal and vertical configuration register */
+	__raw_writel(((var->hsync_len - 1) << 26)
+		     + ((var->right_margin - 1) << 8)
+		     + (var->left_margin - 3), LCDC_REG(LCDC_LHCR));
+	__raw_writel((var->vsync_len << 26)
+		     + (var->lower_margin << 8)
+		     + var->upper_margin, LCDC_REG(LCDC_LVCR));
+
+	/* Sharp configuration register */
+	__raw_writel(0x00120300, LCDC_REG(LCDC_LSCR));
+
+	/* Refresh mode control reigster */
+	__raw_writel(0x00000000, LCDC_REG(LCDC_LRMCR));
+
+	/* DMA control register */
+	if (cpu_is_mx27_rev(CHIP_REV_2_0) > 0)
+		__raw_writel(0x00040060, LCDC_REG(LCDC_LDCR));
+	else
+		__raw_writel(0x00020010, LCDC_REG(LCDC_LDCR));
+}
+
+/*!
+ * @brief Set LCD brightness
+ * @param level	brightness level
+ */
+void mx2fb_set_brightness(uint8_t level)
+{
+	/* Set LCDC PWM contract control register */
+	__raw_writel(0x00A90300 | level, LCDC_REG(LCDC_LPCCR));
+}
+
+EXPORT_SYMBOL(mx2fb_set_brightness);
+
+/*
+ * @brief LCDC interrupt handler
+ */
+static irqreturn_t mx2fb_isr(int irq, void *dev_id)
+{
+	struct fb_event event;
+	unsigned long status = __raw_readl(LCDC_REG(LCDC_LISR));
+
+	if (status & MX2FB_INT_EOF) {
+		event.info = &mx2fb_info[0];
+		atomic_notifier_call_chain(&mx2fb_notifier_list,
+					   FB_EVENT_MXC_EOF, &event);
+	}
+
+	if (status & MX2FB_INT_GW_EOF) {
+		event.info = &mx2fb_info[1];
+		atomic_notifier_call_chain(&mx2fb_notifier_list,
+					   FB_EVENT_MXC_EOF, &event);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * @brief Config and request LCDC interrupt
+ */
+static void _request_irq(void)
+{
+	unsigned long status;
+	unsigned long flags;
+
+	/* Read to clear the status */
+	status = __raw_readl(LCDC_REG(LCDC_LISR));
+
+	if (request_irq(MXC_INT_LCDC, mx2fb_isr, 0, "LCDC", 0))
+		pr_info("Request LCDC IRQ failed.\n");
+	else {
+		spin_lock_irqsave(&mx2fb_notifier_list.lock, flags);
+
+		/* Enable interrupt in case client has registered */
+		if (mx2fb_notifier_list.head != NULL) {
+			unsigned long status;
+			unsigned long ints = MX2FB_INT_EOF;
+
+			ints |= MX2FB_INT_GW_EOF;
+
+			/* Read to clear the status */
+			status = __raw_readl(LCDC_REG(LCDC_LISR));
+
+			/* Configure interrupt condition for EOF */
+			__raw_writel(0x0, LCDC_REG(LCDC_LICR));
+
+			/* Enable EOF and graphic window EOF interrupt */
+			__raw_writel(ints, LCDC_REG(LCDC_LIER));
+		}
+
+		spin_unlock_irqrestore(&mx2fb_notifier_list.lock, flags);
+	}
+}
+
+/*!
+ * @brief Free LCDC interrupt handler
+ */
+static void _free_irq(void)
+{
+	/* Disable all LCDC interrupt */
+	__raw_writel(0x0, LCDC_REG(LCDC_LIER));
+
+	free_irq(MXC_INT_LCDC, 0);
+}
+
+/*!
+ * @brief Register a client notifier
+ * @param nb	notifier block to callback on events
+ */
+int mx2fb_register_client(struct notifier_block *nb)
+{
+	unsigned long flags;
+	int ret;
+
+	ret = atomic_notifier_chain_register(&mx2fb_notifier_list, nb);
+
+	spin_lock_irqsave(&mx2fb_notifier_list.lock, flags);
+
+	/* Enable interrupt in case client has registered */
+	if (mx2fb_notifier_list.head != NULL) {
+		unsigned long status;
+		unsigned long ints = MX2FB_INT_EOF;
+
+		ints |= MX2FB_INT_GW_EOF;
+
+		/* Read to clear the status */
+		status = __raw_readl(LCDC_REG(LCDC_LISR));
+
+		/* Configure interrupt condition for EOF */
+		__raw_writel(0x0, LCDC_REG(LCDC_LICR));
+
+		/* Enable EOF and graphic window EOF interrupt */
+		__raw_writel(ints, LCDC_REG(LCDC_LIER));
+	}
+
+	spin_unlock_irqrestore(&mx2fb_notifier_list.lock, flags);
+
+	return ret;
+}
+
+/*!
+ * @brief Unregister a client notifier
+ * @param nb	notifier block to callback on events
+ */
+int mx2fb_unregister_client(struct notifier_block *nb)
+{
+	unsigned long flags;
+	int ret;
+
+	ret = atomic_notifier_chain_unregister(&mx2fb_notifier_list, nb);
+
+	spin_lock_irqsave(&mx2fb_notifier_list.lock, flags);
+
+	/* Mask interrupt in case no client registered */
+	if (mx2fb_notifier_list.head == NULL)
+		__raw_writel(0x0, LCDC_REG(LCDC_LIER));
+
+	spin_unlock_irqrestore(&mx2fb_notifier_list.lock, flags);
+
+	return ret;
+}
+
+#ifdef CONFIG_PM
+/*
+ * Power management hooks. Note that we won't be called from IRQ context,
+ * unlike the blank functions above, so we may sleep.
+ */
+
+/*!
+ * @brief Suspends the framebuffer and blanks the screen.
+ * Power management support
+ */
+static int mx2fb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	_disable_lcdc(&mx2fb_info[0]);
+
+	return 0;
+}
+
+/*!
+ * @brief Resumes the framebuffer and unblanks the screen.
+ * Power management support
+ */
+static int mx2fb_resume(struct platform_device *pdev)
+{
+	_enable_lcdc(&mx2fb_info[0]);
+
+	return 0;
+}
+
+#endif				/* CONFIG_PM */
+
+/*!
+ * @brief Probe routine for the framebuffer driver. It is called during the
+ *        driver binding process.
+ *
+ * @return Appropriate error code to the kernel common code
+ */
+static int mx2fb_probe(struct platform_device *pdev)
+{
+	int ret, i;
+
+	lcdc_clk = clk_get(&pdev->dev, "lcdc_clk");
+
+	for (i = 0; i < sizeof(mx2fb_info) / sizeof(struct fb_info); i++) {
+		if ((ret = _install_fb(&mx2fb_info[i], pdev))) {
+			dev_err(&pdev->dev,
+				"Failed to register framebuffer %d\n", i);
+			return ret;
+		}
+	}
+	_request_irq();
+
+	return 0;
+}
+
+/*!
+ * @brief Initialization
+ */
+int __init mx2fb_init(void)
+{
+	/*
+	 * For kernel boot options (in 'video=xxxfb:<options>' format)
+	 */
+#ifndef MODULE
+	{
+		char *option;
+
+		if (fb_get_options("mxcfb", &option))
+			return -ENODEV;
+		mx2fb_setup(option);
+	}
+#endif
+	return platform_driver_register(&mx2fb_driver);
+}
+
+/*!
+ * @brief Cleanup
+ */
+void __exit mx2fb_exit(void)
+{
+	int i;
+
+	_free_irq();
+	for (i = sizeof(mx2fb_info) / sizeof(struct fb_info); i > 0; i--)
+		_uninstall_fb(&mx2fb_info[i - 1]);
+
+	platform_driver_unregister(&mx2fb_driver);
+}
+
+#ifndef MODULE
+/*!
+ * @brief Setup
+ * Parse user specified options
+ * Example: video=mxcfb:240x320,bpp=16,Sharp-QVGA
+ */
+static int __init mx2fb_setup(char *options)
+{
+	char *opt;
+
+	if (!options || !*options)
+		return 0;
+
+	while ((opt = strsep(&options, ",")) != NULL) {
+		if (!*opt)
+			continue;
+
+		if (!strncmp(opt, "bpp=", 4))
+			default_bpp = simple_strtoul(opt + 4, NULL, 0);
+		else
+			fb_mode = opt;
+	}
+
+	return 0;
+}
+#endif
+
+/* Modularization */
+module_init(mx2fb_init);
+module_exit(mx2fb_exit);
+
+EXPORT_SYMBOL(mx2_gw_set);
+EXPORT_SYMBOL(mx2fb_register_client);
+EXPORT_SYMBOL(mx2fb_unregister_client);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MX2 framebuffer driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/mxc/mx2fb.h b/drivers/video/mxc/mx2fb.h
new file mode 100644
index 000000000000..ed20d78289ce
--- /dev/null
+++ b/drivers/video/mxc/mx2fb.h
@@ -0,0 +1,141 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file    mx2fb.h
+ *
+ * @brief Header file for the MX27 Frame buffer
+ *
+ * @ingroup Framebuffer
+ */
+#ifndef __MX2FB_H__
+#define __MX2FB_H__
+
+/*! @brief MX27 LCDC graphic window information */
+struct fb_gwinfo {
+	/*! Non-zero if graphic window is enabled */
+	__u32 enabled;
+
+	/* The fields below are valid only when graphic window is enabled */
+
+	/*! Graphic window alpha value from 0 to 255 */
+	__u32 alpha_value;
+
+	/*! Non-zero if graphic window color keying is enabled. */
+	__u32 ck_enabled;
+
+	/*
+	 * The fields ck_red, ck_green and ck_blue are valid only when
+	 * graphic window and the color keying are enabled. They are the
+	 * color component of graphic window color keying.
+	 */
+
+	/*! Color keying red component */
+	__u32 ck_red;
+
+	/*! Color keying green component */
+	__u32 ck_green;
+
+	/*! Color keying blue component */
+	__u32 ck_blue;
+
+	/*! Graphic window x position */
+	__u32 xpos;
+
+	/*! Graphic window y position */
+	__u32 ypos;
+
+	/*! Non-zero if graphic window vertical scan in reverse direction. */
+	__u32 vs_reversed;
+
+	/*
+	 * The following fields are valid for FBIOGET_GWINFO and
+	 * mx2_gw_set(). FBIOPUT_GWINFO ignores these fields.
+	 */
+	__u32 base;		/* Graphic window start address */
+	__u32 xres;		/* Visible x resolution */
+	__u32 yres;		/* Visible y resolution */
+	__u32 xres_virtual;	/* Virtual x resolution */
+};
+
+/* 0x46E0-0x46FF are reserved for MX27 */
+#define FBIOGET_GWINFO		0x46E0	/*!< Get graphic window information */
+#define FBIOPUT_GWINFO		0x46E1	/*!< Set graphic window information */
+
+struct mx2fb_gbl_alpha {
+	int enable;
+	int alpha;
+};
+
+struct mx2fb_color_key {
+	int enable;
+	__u32 color_key;
+};
+
+#define MX2FB_SET_GBL_ALPHA	_IOW('M', 0, struct mx2fb_gbl_alpha)
+#define MX2FB_SET_CLR_KEY	_IOW('M', 1, struct mx2fb_color_key)
+#define MX2FB_WAIT_FOR_VSYNC	_IOW('F', 0x20, u_int32_t)
+
+#ifdef __KERNEL__
+
+/*
+ * LCDC register definitions
+ */
+#define LCDC_LSSAR		0x00
+#define LCDC_LSR		0x04
+#define LCDC_LVPWR		0x08
+#define LCDC_LCPR		0x0C
+#define LCDC_LCWHBR		0x10
+#define LCDC_LCCMR		0x14
+#define LCDC_LPCR		0x18
+#define LCDC_LHCR		0x1C
+#define LCDC_LVCR		0x20
+#define LCDC_LPOR		0x24
+#define LCDC_LSCR		0x28
+#define LCDC_LPCCR		0x2C
+#define LCDC_LDCR		0x30
+#define LCDC_LRMCR		0x34
+#define LCDC_LICR		0x38
+#define LCDC_LIER		0x3C
+#define LCDC_LISR		0x40
+#define LCDC_LGWSAR		0x50
+#define LCDC_LGWSR		0x54
+#define LCDC_LGWVPWR		0x58
+#define LCDC_LGWPOR		0x5C
+#define LCDC_LGWPR		0x60
+#define LCDC_LGWCR		0x64
+#define LCDC_LGWDCR		0x68
+#define LCDC_LAUSCR		0x80
+#define LCDC_LAUSCCR		0x84
+
+#define LCDC_REG(reg)		(IO_ADDRESS(LCDC_BASE_ADDR) + reg)
+
+#define MX2FB_INT_BOF		0x0001	/* Beginning of Frame */
+#define MX2FB_INT_EOF		0x0002	/* End of Frame */
+#define MX2FB_INT_ERR_RES	0x0004	/* Error Response */
+#define MX2FB_INT_UDR_ERR	0x0008	/* Under Run Error */
+#define MX2FB_INT_GW_BOF	0x0010	/* Graphic Window BOF */
+#define MX2FB_INT_GW_EOF	0x0020	/* Graphic Window EOF */
+#define MX2FB_INT_GW_ERR_RES	0x0040	/* Graphic Window ERR_RES */
+#define MX2FB_INT_GW_UDR_ERR	0x0080	/* Graphic Window UDR_ERR */
+
+#define FB_EVENT_MXC_EOF	0x8001	/* End of Frame event */
+
+int mx2fb_register_client(struct notifier_block *nb);
+int mx2fb_unregister_client(struct notifier_block *nb);
+
+void mx2_gw_set(struct fb_gwinfo *gwinfo);
+
+#endif				/* __KERNEL__ */
+
+#endif				/* __MX2FB_H__ */
diff --git a/drivers/video/mxc/mxc_edid.c b/drivers/video/mxc/mxc_edid.c
new file mode 100644
index 000000000000..a6a87bd86f65
--- /dev/null
+++ b/drivers/video/mxc/mxc_edid.c
@@ -0,0 +1,90 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup Framebuffer Framebuffer Driver for SDC and ADC.
+ */
+
+/*!
+ * @file mxc_edid.c
+ *
+ * @brief MXC EDID tools
+ *
+ * @ingroup Framebuffer
+ */
+
+/*!
+ * Include files
+ */
+#include <linux/fb.h>
+
+#define EDID_LENGTH 128
+
+static u8 edid[EDID_LENGTH];
+
+int read_edid(struct i2c_adapter *adp,
+	      struct fb_var_screeninfo *einfo,
+	      int *dvi)
+{
+	u8 buf0[2] = {0, 0};
+	int dat = 0;
+	u16 addr = 0x50;
+	struct i2c_msg msg[2] = {
+		{
+		.addr	= addr,
+		.flags	= 0,
+		.len	= 1,
+		.buf	= buf0,
+		}, {
+		.addr	= addr,
+		.flags	= I2C_M_RD,
+		.len	= EDID_LENGTH,
+		.buf	= edid,
+		},
+	};
+
+	if (adp == NULL || einfo == NULL)
+		return -EINVAL;
+
+	buf0[0] = 0x00;
+	memset(&edid, 0, sizeof(edid));
+	memset(einfo, 0, sizeof(struct fb_var_screeninfo));
+	dat = i2c_transfer(adp, msg, 2);
+
+	/* If 0x50 fails, try 0x37. */
+	if (edid[1] == 0x00) {
+		msg[0].addr = msg[1].addr = 0x37;
+		dat = i2c_transfer(adp, msg, 2);
+	}
+
+	if (edid[1] == 0x00)
+		return -ENOENT;
+
+	*dvi = 0;
+	if ((edid[20] == 0x80) || (edid[20] == 0x88) || (edid[20] == 0))
+		*dvi = 1;
+
+	dat = fb_parse_edid(edid, einfo);
+	if (dat)
+		return -dat;
+
+	/* This is valid for version 1.3 of the EDID */
+	if ((edid[18] == 1) && (edid[19] == 3)) {
+		einfo->height = edid[21] * 10;
+		einfo->width = edid[22] * 10;
+	}
+
+	return 0;
+}
+
+
diff --git a/drivers/video/mxc/mxc_ipuv3_fb.c b/drivers/video/mxc/mxc_ipuv3_fb.c
new file mode 100644
index 000000000000..279764d9a3a4
--- /dev/null
+++ b/drivers/video/mxc/mxc_ipuv3_fb.c
@@ -0,0 +1,1599 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup Framebuffer Framebuffer Driver for SDC and ADC.
+ */
+
+/*!
+ * @file mxcfb.c
+ *
+ * @brief MXC Frame buffer driver for SDC
+ *
+ * @ingroup Framebuffer
+ */
+
+/*!
+ * Include files
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/dma-mapping.h>
+#include <linux/clk.h>
+#include <linux/console.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+#include <linux/mxcfb.h>
+#include <asm/mach-types.h>
+#include <asm/uaccess.h>
+#include <mach/hardware.h>
+
+/*
+ * Driver name
+ */
+#define MXCFB_NAME      "mxc_sdc_fb"
+/*!
+ * Structure containing the MXC specific framebuffer information.
+ */
+struct mxcfb_info {
+	int blank;
+	ipu_channel_t ipu_ch;
+	int ipu_di;
+	u32 ipu_di_pix_fmt;
+	bool overlay;
+	bool alpha_chan_en;
+	dma_addr_t alpha_phy_addr0;
+	dma_addr_t alpha_phy_addr1;
+	void *alpha_virt_addr0;
+	void *alpha_virt_addr1;
+	uint32_t alpha_mem_len;
+	uint32_t ipu_ch_irq;
+	uint32_t cur_ipu_buf;
+	uint32_t cur_ipu_alpha_buf;
+
+	u32 pseudo_palette[16];
+
+	struct semaphore flip_sem;
+	struct semaphore alpha_flip_sem;
+	struct completion vsync_complete;
+};
+
+struct mxcfb_alloc_list {
+	struct list_head list;
+	dma_addr_t phy_addr;
+	void *cpu_addr;
+	u32 size;
+};
+
+enum {
+	BOTH_ON,
+	SRC_ON,
+	TGT_ON,
+	BOTH_OFF
+};
+
+static char *fb_mode;
+static unsigned long default_bpp = 16;
+static bool g_dp_in_use;
+LIST_HEAD(fb_alloc_list);
+static struct fb_info *mxcfb_info[3];
+
+static uint32_t bpp_to_pixfmt(struct fb_info *fbi)
+{
+	uint32_t pixfmt = 0;
+
+	if (fbi->var.nonstd)
+		return fbi->var.nonstd;
+
+	switch (fbi->var.bits_per_pixel) {
+	case 24:
+		pixfmt = IPU_PIX_FMT_BGR24;
+		break;
+	case 32:
+		pixfmt = IPU_PIX_FMT_BGR32;
+		break;
+	case 16:
+		pixfmt = IPU_PIX_FMT_RGB565;
+		break;
+	}
+	return pixfmt;
+}
+
+static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id);
+static int mxcfb_blank(int blank, struct fb_info *info);
+static int mxcfb_map_video_memory(struct fb_info *fbi);
+static int mxcfb_unmap_video_memory(struct fb_info *fbi);
+
+/*
+ * Set fixed framebuffer parameters based on variable settings.
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_set_fix(struct fb_info *info)
+{
+	struct fb_fix_screeninfo *fix = &info->fix;
+	struct fb_var_screeninfo *var = &info->var;
+
+	fix->line_length = var->xres_virtual * var->bits_per_pixel / 8;
+
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->accel = FB_ACCEL_NONE;
+	fix->visual = FB_VISUAL_TRUECOLOR;
+	fix->xpanstep = 1;
+	fix->ypanstep = 1;
+
+	return 0;
+}
+
+static int _setup_disp_channel1(struct fb_info *fbi)
+{
+	ipu_channel_params_t params;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	memset(&params, 0, sizeof(params));
+	params.mem_dp_bg_sync.di = mxc_fbi->ipu_di;
+
+	/*
+	 * Assuming interlaced means yuv output, below setting also
+	 * valid for mem_dc_sync. FG should have the same vmode as BG.
+	 */
+	if (mxc_fbi->ipu_ch == MEM_FG_SYNC) {
+		struct mxcfb_info *mxc_fbi_tmp;
+		int i;
+
+		for (i = 0; i < num_registered_fb; i++) {
+			mxc_fbi_tmp = (struct mxcfb_info *)
+				(registered_fb[i]->par);
+			if (mxc_fbi_tmp->ipu_ch == MEM_BG_SYNC) {
+				fbi->var.vmode =
+				registered_fb[i]->var.vmode;
+				break;
+			}
+		}
+	}
+	if (fbi->var.vmode & FB_VMODE_INTERLACED) {
+		params.mem_dp_bg_sync.interlaced = true;
+		params.mem_dp_bg_sync.out_pixel_fmt =
+			IPU_PIX_FMT_YUV444;
+	} else {
+		if (mxc_fbi->ipu_di_pix_fmt)
+			params.mem_dp_bg_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
+		else
+			params.mem_dp_bg_sync.out_pixel_fmt = IPU_PIX_FMT_RGB666;
+	}
+	params.mem_dp_bg_sync.in_pixel_fmt = bpp_to_pixfmt(fbi);
+	if (mxc_fbi->alpha_chan_en)
+		params.mem_dp_bg_sync.alpha_chan_en = true;
+
+	ipu_init_channel(mxc_fbi->ipu_ch, &params);
+
+	return 0;
+}
+
+static int _setup_disp_channel2(struct fb_info *fbi)
+{
+	int retval = 0;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	mxc_fbi->cur_ipu_buf = 1;
+	sema_init(&mxc_fbi->flip_sem, 1);
+	if (mxc_fbi->alpha_chan_en) {
+		mxc_fbi->cur_ipu_alpha_buf = 1;
+		sema_init(&mxc_fbi->alpha_flip_sem, 1);
+	}
+	fbi->var.xoffset = fbi->var.yoffset = 0;
+
+	retval = ipu_init_channel_buffer(mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
+					 bpp_to_pixfmt(fbi),
+					 fbi->var.xres, fbi->var.yres,
+					 fbi->fix.line_length,
+					 IPU_ROTATE_NONE,
+					 fbi->fix.smem_start +
+					 (fbi->fix.line_length * fbi->var.yres),
+					 fbi->fix.smem_start,
+					 0, 0);
+	if (retval) {
+		dev_err(fbi->device,
+			"ipu_init_channel_buffer error %d\n", retval);
+	}
+
+	if (mxc_fbi->alpha_chan_en) {
+		retval = ipu_init_channel_buffer(mxc_fbi->ipu_ch,
+						 IPU_ALPHA_IN_BUFFER,
+						 IPU_PIX_FMT_GENERIC,
+						 fbi->var.xres, fbi->var.yres,
+						 fbi->var.xres,
+						 IPU_ROTATE_NONE,
+						 mxc_fbi->alpha_phy_addr0,
+						 mxc_fbi->alpha_phy_addr1,
+						 0, 0);
+		if (retval) {
+			dev_err(fbi->device,
+				"ipu_init_channel_buffer error %d\n", retval);
+			return retval;
+		}
+	}
+
+	return retval;
+}
+
+/*
+ * Set framebuffer parameters and change the operating mode.
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_set_par(struct fb_info *fbi)
+{
+	int retval = 0;
+	u32 mem_len, alpha_mem_len;
+	ipu_di_signal_cfg_t sig_cfg;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	dev_dbg(fbi->device, "Reconfiguring framebuffer\n");
+
+	ipu_disable_irq(mxc_fbi->ipu_ch_irq);
+	ipu_disable_channel(mxc_fbi->ipu_ch, true);
+	ipu_uninit_channel(mxc_fbi->ipu_ch);
+	ipu_clear_irq(mxc_fbi->ipu_ch_irq);
+	mxcfb_set_fix(fbi);
+
+	mem_len = fbi->var.yres_virtual * fbi->fix.line_length;
+	if (!fbi->fix.smem_start || (mem_len > fbi->fix.smem_len)) {
+		if (fbi->fix.smem_start)
+			mxcfb_unmap_video_memory(fbi);
+
+		if (mxcfb_map_video_memory(fbi) < 0)
+			return -ENOMEM;
+	}
+	if (mxc_fbi->alpha_chan_en) {
+		alpha_mem_len = fbi->var.xres * fbi->var.yres;
+		if ((!mxc_fbi->alpha_phy_addr0 && !mxc_fbi->alpha_phy_addr1) ||
+		    (alpha_mem_len > mxc_fbi->alpha_mem_len)) {
+			if (mxc_fbi->alpha_phy_addr0)
+				dma_free_coherent(fbi->device,
+						  mxc_fbi->alpha_mem_len,
+						  mxc_fbi->alpha_virt_addr0,
+						  mxc_fbi->alpha_phy_addr0);
+			if (mxc_fbi->alpha_phy_addr1)
+				dma_free_coherent(fbi->device,
+						  mxc_fbi->alpha_mem_len,
+						  mxc_fbi->alpha_virt_addr1,
+						  mxc_fbi->alpha_phy_addr1);
+
+			mxc_fbi->alpha_virt_addr0 =
+					dma_alloc_coherent(fbi->device,
+						  alpha_mem_len,
+						  &mxc_fbi->alpha_phy_addr0,
+						  GFP_DMA | GFP_KERNEL);
+
+			mxc_fbi->alpha_virt_addr1 =
+					dma_alloc_coherent(fbi->device,
+						  alpha_mem_len,
+						  &mxc_fbi->alpha_phy_addr1,
+						  GFP_DMA | GFP_KERNEL);
+			if (mxc_fbi->alpha_virt_addr0 == NULL ||
+			    mxc_fbi->alpha_virt_addr1 == NULL) {
+				dev_err(fbi->device, "mxcfb: dma alloc for"
+					" alpha buffer failed.\n");
+				if (mxc_fbi->alpha_virt_addr0)
+					dma_free_coherent(fbi->device,
+						  mxc_fbi->alpha_mem_len,
+						  mxc_fbi->alpha_virt_addr0,
+						  mxc_fbi->alpha_phy_addr0);
+				if (mxc_fbi->alpha_virt_addr1)
+					dma_free_coherent(fbi->device,
+						  mxc_fbi->alpha_mem_len,
+						  mxc_fbi->alpha_virt_addr1,
+						  mxc_fbi->alpha_phy_addr1);
+				return -ENOMEM;
+			}
+			mxc_fbi->alpha_mem_len = alpha_mem_len;
+		}
+	}
+
+	_setup_disp_channel1(fbi);
+
+	if (!mxc_fbi->overlay) {
+		uint32_t out_pixel_fmt;
+
+		memset(&sig_cfg, 0, sizeof(sig_cfg));
+		if (fbi->var.vmode & FB_VMODE_INTERLACED) {
+			sig_cfg.interlaced = true;
+			out_pixel_fmt = IPU_PIX_FMT_YUV444;
+		} else {
+			if (mxc_fbi->ipu_di_pix_fmt)
+				out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
+			else
+				out_pixel_fmt = IPU_PIX_FMT_RGB666;
+		}
+		if (fbi->var.vmode & FB_VMODE_ODD_FLD_FIRST) /* PAL */
+			sig_cfg.odd_field_first = true;
+		if (fbi->var.sync & FB_SYNC_EXT)
+			sig_cfg.ext_clk = true;
+		if (fbi->var.sync & FB_SYNC_HOR_HIGH_ACT)
+			sig_cfg.Hsync_pol = true;
+		if (fbi->var.sync & FB_SYNC_VERT_HIGH_ACT)
+			sig_cfg.Vsync_pol = true;
+		if (!(fbi->var.sync & FB_SYNC_CLK_LAT_FALL))
+			sig_cfg.clk_pol = true;
+		if (fbi->var.sync & FB_SYNC_DATA_INVERT)
+			sig_cfg.data_pol = true;
+		if (!(fbi->var.sync & FB_SYNC_OE_LOW_ACT))
+			sig_cfg.enable_pol = true;
+		if (fbi->var.sync & FB_SYNC_CLK_IDLE_EN)
+			sig_cfg.clkidle_en = true;
+
+		dev_dbg(fbi->device, "pixclock = %ul Hz\n",
+			(u32) (PICOS2KHZ(fbi->var.pixclock) * 1000UL));
+
+		if (ipu_init_sync_panel(mxc_fbi->ipu_di,
+					(PICOS2KHZ(fbi->var.pixclock)) * 1000UL,
+					fbi->var.xres, fbi->var.yres,
+					out_pixel_fmt,
+					fbi->var.left_margin,
+					fbi->var.hsync_len,
+					fbi->var.right_margin,
+					fbi->var.upper_margin,
+					fbi->var.vsync_len,
+					fbi->var.lower_margin,
+					0, sig_cfg) != 0) {
+			dev_err(fbi->device,
+				"mxcfb: Error initializing panel.\n");
+			return -EINVAL;
+		}
+
+		fbi->mode =
+		    (struct fb_videomode *)fb_match_mode(&fbi->var,
+							 &fbi->modelist);
+		ipu_disp_set_window_pos(mxc_fbi->ipu_ch, 0, 0);
+	}
+
+	retval = _setup_disp_channel2(fbi);
+	if (retval)
+		return retval;
+
+	if (mxc_fbi->blank == FB_BLANK_UNBLANK) {
+		ipu_enable_channel(mxc_fbi->ipu_ch);
+	}
+
+	return retval;
+}
+
+static int _swap_channels(struct fb_info *fbi,
+			  struct fb_info *fbi_to, bool both_on)
+{
+	int retval, tmp;
+	ipu_channel_t old_ch;
+	struct mxcfb_info *mxc_fbi_from = (struct mxcfb_info *)fbi->par;
+	struct mxcfb_info *mxc_fbi_to = (struct mxcfb_info *)fbi_to->par;
+
+	if (both_on) {
+		ipu_disable_channel(mxc_fbi_to->ipu_ch, true);
+		ipu_uninit_channel(mxc_fbi_to->ipu_ch);
+	}
+
+	/* switch the mxc fbi parameters */
+	old_ch = mxc_fbi_from->ipu_ch;
+	mxc_fbi_from->ipu_ch = mxc_fbi_to->ipu_ch;
+	mxc_fbi_to->ipu_ch = old_ch;
+	tmp = mxc_fbi_from->ipu_ch_irq;
+	mxc_fbi_from->ipu_ch_irq = mxc_fbi_to->ipu_ch_irq;
+	mxc_fbi_to->ipu_ch_irq = tmp;
+
+	_setup_disp_channel1(fbi);
+	retval = _setup_disp_channel2(fbi);
+	if (retval)
+		return retval;
+
+	/* switch between dp and dc, disable old idmac, enable new idmac */
+	retval = ipu_swap_channel(old_ch, mxc_fbi_from->ipu_ch);
+	ipu_uninit_channel(old_ch);
+
+	if (both_on) {
+		_setup_disp_channel1(fbi_to);
+		retval = _setup_disp_channel2(fbi_to);
+		if (retval)
+			return retval;
+		ipu_enable_channel(mxc_fbi_to->ipu_ch);
+	}
+
+	return retval;
+}
+
+static int swap_channels(struct fb_info *fbi)
+{
+	int i;
+	int swap_mode;
+	ipu_channel_t ch_to;
+	struct mxcfb_info *mxc_fbi_from = (struct mxcfb_info *)fbi->par;
+	struct fb_info *fbi_to = NULL;
+	struct mxcfb_info *mxc_fbi_to;
+
+	/* what's the target channel? */
+	if (mxc_fbi_from->ipu_ch == MEM_BG_SYNC)
+		ch_to = MEM_DC_SYNC;
+	else
+		ch_to = MEM_BG_SYNC;
+
+	for (i = 0; i < num_registered_fb; i++) {
+		mxc_fbi_to =
+			(struct mxcfb_info *)mxcfb_info[i]->par;
+		if (mxc_fbi_to->ipu_ch == ch_to) {
+			fbi_to = mxcfb_info[i];
+			break;
+		}
+	}
+	if (fbi_to == NULL)
+		return -1;
+
+	if (mxc_fbi_from->blank == FB_BLANK_UNBLANK) {
+		if (mxc_fbi_to->blank == FB_BLANK_UNBLANK)
+			swap_mode = BOTH_ON;
+		else
+			swap_mode = SRC_ON;
+	} else {
+		if (mxc_fbi_to->blank == FB_BLANK_UNBLANK)
+			swap_mode = TGT_ON;
+		else
+			swap_mode = BOTH_OFF;
+	}
+
+	/* tvout di-1: for DC use UYVY, for DP use RGB */
+	if (mxc_fbi_from->ipu_di == 1 && ch_to == MEM_DC_SYNC) {
+		fbi->var.bits_per_pixel = 16;
+		fbi->var.nonstd = IPU_PIX_FMT_UYVY;
+	} else if (mxc_fbi_from->ipu_di == 1 && ch_to == MEM_BG_SYNC) {
+		fbi->var.nonstd = 0;
+	} else if (mxc_fbi_from->ipu_di == 0 && ch_to == MEM_DC_SYNC) {
+		fbi_to->var.nonstd = 0;
+	} else if (mxc_fbi_from->ipu_di == 0 && ch_to == MEM_BG_SYNC) {
+		fbi->var.bits_per_pixel = 16;
+		fbi->var.nonstd = IPU_PIX_FMT_UYVY;
+	}
+
+	switch (swap_mode) {
+	case BOTH_ON:
+		/* disable target->switch src->enable target */
+		_swap_channels(fbi, fbi_to, true);
+		break;
+	case SRC_ON:
+		/* just switch src */
+		_swap_channels(fbi, fbi_to, false);
+		break;
+	case TGT_ON:
+		/* just switch target */
+		_swap_channels(fbi_to, fbi, false);
+		break;
+	case BOTH_OFF:
+		/* switch directly, no more need to do */
+		mxc_fbi_to->ipu_ch = mxc_fbi_from->ipu_ch;
+		mxc_fbi_from->ipu_ch = ch_to;
+		i = mxc_fbi_from->ipu_ch_irq;
+		mxc_fbi_from->ipu_ch_irq = mxc_fbi_to->ipu_ch_irq;
+		mxc_fbi_to->ipu_ch_irq = i;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Check framebuffer variable parameters and adjust to valid values.
+ *
+ * @param       var      framebuffer variable parameters
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	u32 vtotal;
+	u32 htotal;
+
+	if (var->xres_virtual < var->xres)
+		var->xres_virtual = var->xres;
+	if (var->yres_virtual < var->yres)
+		var->yres_virtual = var->yres;
+
+	if ((var->bits_per_pixel != 32) && (var->bits_per_pixel != 24) &&
+	    (var->bits_per_pixel != 16) && (var->bits_per_pixel != 8))
+		var->bits_per_pixel = default_bpp;
+
+	switch (var->bits_per_pixel) {
+	case 8:
+		var->red.length = 3;
+		var->red.offset = 5;
+		var->red.msb_right = 0;
+
+		var->green.length = 3;
+		var->green.offset = 2;
+		var->green.msb_right = 0;
+
+		var->blue.length = 2;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 16:
+		var->red.length = 5;
+		var->red.offset = 11;
+		var->red.msb_right = 0;
+
+		var->green.length = 6;
+		var->green.offset = 5;
+		var->green.msb_right = 0;
+
+		var->blue.length = 5;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 24:
+		var->red.length = 8;
+		var->red.offset = 16;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 8;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 32:
+		var->red.length = 8;
+		var->red.offset = 16;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 8;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 8;
+		var->transp.offset = 24;
+		var->transp.msb_right = 0;
+		break;
+	}
+
+	if (var->pixclock < 1000) {
+		htotal = var->xres + var->right_margin + var->hsync_len +
+		    var->left_margin;
+		vtotal = var->yres + var->lower_margin + var->vsync_len +
+		    var->upper_margin;
+		var->pixclock = (vtotal * htotal * 6UL) / 100UL;
+		var->pixclock = KHZ2PICOS(var->pixclock);
+		dev_dbg(info->device,
+			"pixclock set for 60Hz refresh = %u ps\n",
+			var->pixclock);
+	}
+
+	var->height = -1;
+	var->width = -1;
+	var->grayscale = 0;
+
+	return 0;
+}
+
+static inline u_int _chan_to_field(u_int chan, struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+static int mxcfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+			   u_int trans, struct fb_info *fbi)
+{
+	unsigned int val;
+	int ret = 1;
+
+	/*
+	 * If greyscale is true, then we convert the RGB value
+	 * to greyscale no matter what visual we are using.
+	 */
+	if (fbi->var.grayscale)
+		red = green = blue = (19595 * red + 38470 * green +
+				      7471 * blue) >> 16;
+	switch (fbi->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		/*
+		 * 16-bit True Colour.  We encode the RGB value
+		 * according to the RGB bitfield information.
+		 */
+		if (regno < 16) {
+			u32 *pal = fbi->pseudo_palette;
+
+			val = _chan_to_field(red, &fbi->var.red);
+			val |= _chan_to_field(green, &fbi->var.green);
+			val |= _chan_to_field(blue, &fbi->var.blue);
+
+			pal[regno] = val;
+			ret = 0;
+		}
+		break;
+
+	case FB_VISUAL_STATIC_PSEUDOCOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		break;
+	}
+
+	return ret;
+}
+
+/*
+ * Function to handle custom ioctls for MXC framebuffer.
+ *
+ * @param       inode   inode struct
+ *
+ * @param       file    file struct
+ *
+ * @param       cmd     Ioctl command to handle
+ *
+ * @param       arg     User pointer to command arguments
+ *
+ * @param       fbi     framebuffer information pointer
+ */
+static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	int __user *argp = (void __user *)arg;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	switch (cmd) {
+	case MXCFB_SET_GBL_ALPHA:
+		{
+			struct mxcfb_gbl_alpha ga;
+
+			if (copy_from_user(&ga, (void *)arg, sizeof(ga))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			if (ipu_disp_set_global_alpha(mxc_fbi->ipu_ch,
+						      (bool)ga.enable,
+						      ga.alpha)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (ga.enable)
+				mxc_fbi->alpha_chan_en = false;
+
+			if (ga.enable)
+				dev_dbg(fbi->device,
+					"Set global alpha of %s to %d\n",
+					fbi->fix.id, ga.alpha);
+			break;
+		}
+	case MXCFB_SET_LOC_ALPHA:
+		{
+			struct mxcfb_loc_alpha la;
+			int i;
+			char *video_plane_idstr = "";
+
+			if (copy_from_user(&la, (void *)arg, sizeof(la))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			if (ipu_disp_set_global_alpha(mxc_fbi->ipu_ch,
+						      !(bool)la.enable, 0)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (la.enable) {
+				mxc_fbi->alpha_chan_en = true;
+
+				if (mxc_fbi->ipu_ch == MEM_FG_SYNC)
+					video_plane_idstr = "DISP3 BG";
+				else if (mxc_fbi->ipu_ch == MEM_BG_SYNC)
+					video_plane_idstr = "DISP3 FG";
+
+				for (i = 0; i < num_registered_fb; i++) {
+					char *idstr = registered_fb[i]->fix.id;
+					if (strcmp(idstr, video_plane_idstr) == 0) {
+						((struct mxcfb_info *)(registered_fb[i]->par))->alpha_chan_en = false;
+						break;
+					}
+				}
+			} else
+				mxc_fbi->alpha_chan_en = false;
+
+			mxcfb_set_par(fbi);
+
+			la.alpha_phy_addr0 = mxc_fbi->alpha_phy_addr0;
+			la.alpha_phy_addr1 = mxc_fbi->alpha_phy_addr1;
+			if (copy_to_user((void *)arg, &la, sizeof(la))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			if (la.enable)
+				dev_dbg(fbi->device,
+					"Enable DP local alpha for %s\n",
+					fbi->fix.id);
+			break;
+		}
+	case MXCFB_SET_LOC_ALP_BUF:
+		{
+			unsigned long base;
+			uint32_t ipu_alp_ch_irq;
+
+			if (!(((mxc_fbi->ipu_ch == MEM_FG_SYNC) ||
+			     (mxc_fbi->ipu_ch == MEM_BG_SYNC)) &&
+			     (mxc_fbi->alpha_chan_en))) {
+				dev_err(fbi->device,
+					"Should use background or overlay "
+					"framebuffer to set the alpha buffer "
+					"number\n");
+				return -EINVAL;
+			}
+
+			if (get_user(base, argp))
+				return -EFAULT;
+
+			if (base != mxc_fbi->alpha_phy_addr0 &&
+			    base != mxc_fbi->alpha_phy_addr1) {
+				dev_err(fbi->device,
+					"Wrong alpha buffer physical address "
+					"%lu\n", base);
+				return -EINVAL;
+			}
+
+			if (mxc_fbi->ipu_ch == MEM_FG_SYNC)
+				ipu_alp_ch_irq = IPU_IRQ_FG_ALPHA_SYNC_EOF;
+			else
+				ipu_alp_ch_irq = IPU_IRQ_BG_ALPHA_SYNC_EOF;
+
+			down(&mxc_fbi->alpha_flip_sem);
+
+			mxc_fbi->cur_ipu_alpha_buf =
+						!mxc_fbi->cur_ipu_alpha_buf;
+			if (ipu_update_channel_buffer(mxc_fbi->ipu_ch,
+						      IPU_ALPHA_IN_BUFFER,
+						      mxc_fbi->
+							cur_ipu_alpha_buf,
+						      base) == 0) {
+				ipu_select_buffer(mxc_fbi->ipu_ch,
+						  IPU_ALPHA_IN_BUFFER,
+						  mxc_fbi->cur_ipu_alpha_buf);
+				ipu_clear_irq(ipu_alp_ch_irq);
+				ipu_enable_irq(ipu_alp_ch_irq);
+			} else {
+				dev_err(fbi->device,
+					"Error updating %s SDC alpha buf %d "
+					"to address=0x%08lX\n",
+					fbi->fix.id,
+					mxc_fbi->cur_ipu_alpha_buf, base);
+			}
+			break;
+		}
+	case MXCFB_SET_CLR_KEY:
+		{
+			struct mxcfb_color_key key;
+			if (copy_from_user(&key, (void *)arg, sizeof(key))) {
+				retval = -EFAULT;
+				break;
+			}
+			retval = ipu_disp_set_color_key(mxc_fbi->ipu_ch,
+							key.enable,
+							key.color_key);
+			dev_dbg(fbi->device, "Set color key to 0x%08X\n",
+				key.color_key);
+			break;
+		}
+	case MXCFB_WAIT_FOR_VSYNC:
+		{
+			if (mxc_fbi->blank != FB_BLANK_UNBLANK)
+				break;
+
+			down(&mxc_fbi->flip_sem);
+			init_completion(&mxc_fbi->vsync_complete);
+
+			ipu_clear_irq(mxc_fbi->ipu_ch_irq);
+			ipu_enable_irq(mxc_fbi->ipu_ch_irq);
+			retval = wait_for_completion_interruptible_timeout(
+				&mxc_fbi->vsync_complete, 1 * HZ);
+			if (retval == 0) {
+				dev_err(fbi->device,
+					"MXCFB_WAIT_FOR_VSYNC: timeout %d\n",
+					retval);
+				retval = -ETIME;
+			} else if (retval > 0) {
+				retval = 0;
+			}
+			break;
+		}
+	case FBIO_ALLOC:
+		{
+			int size;
+			struct mxcfb_alloc_list *mem;
+
+			mem = kzalloc(sizeof(*mem), GFP_KERNEL);
+			if (mem == NULL)
+				return -ENOMEM;
+
+			if (get_user(size, argp))
+				return -EFAULT;
+
+			mem->size = PAGE_ALIGN(size);
+
+			mem->cpu_addr = dma_alloc_coherent(fbi->device, size,
+							   &mem->phy_addr,
+							   GFP_DMA);
+			if (mem->cpu_addr == NULL) {
+				kfree(mem);
+				return -ENOMEM;
+			}
+
+			list_add(&mem->list, &fb_alloc_list);
+
+			dev_dbg(fbi->device, "allocated %d bytes @ 0x%08X\n",
+				mem->size, mem->phy_addr);
+
+			if (put_user(mem->phy_addr, argp))
+				return -EFAULT;
+
+			break;
+		}
+	case FBIO_FREE:
+		{
+			unsigned long offset;
+			struct mxcfb_alloc_list *mem;
+
+			if (get_user(offset, argp))
+				return -EFAULT;
+
+			retval = -EINVAL;
+			list_for_each_entry(mem, &fb_alloc_list, list) {
+				if (mem->phy_addr == offset) {
+					list_del(&mem->list);
+					dma_free_coherent(fbi->device,
+							  mem->size,
+							  mem->cpu_addr,
+							  mem->phy_addr);
+					kfree(mem);
+					retval = 0;
+					break;
+				}
+			}
+
+			break;
+		}
+	case MXCFB_SET_OVERLAY_POS:
+		{
+			struct mxcfb_pos pos;
+			struct fb_info *bg_fbi = NULL;
+			struct mxcfb_info *bg_mxcfbi = NULL;
+			int i;
+
+			if (mxc_fbi->ipu_ch != MEM_FG_SYNC) {
+				dev_err(fbi->device, "Should use the overlay "
+					"framebuffer to set the position of "
+					"the overlay window\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			if (copy_from_user(&pos, (void *)arg, sizeof(pos))) {
+				retval = -EFAULT;
+				break;
+			}
+
+			for (i = 0; i < num_registered_fb; i++) {
+				bg_mxcfbi =
+				((struct mxcfb_info *)(registered_fb[i]->par));
+
+				if (bg_mxcfbi->ipu_ch == MEM_BG_SYNC) {
+					bg_fbi = registered_fb[i];
+					break;
+				}
+			}
+
+			if (bg_fbi == NULL) {
+				dev_err(fbi->device, "Cannot find the "
+					"background framebuffer\n");
+				retval = -ENOENT;
+				break;
+			}
+
+			if (fbi->var.xres + pos.x > bg_fbi->var.xres) {
+				if (bg_fbi->var.xres < fbi->var.xres)
+					pos.x = 0;
+				else
+					pos.x = bg_fbi->var.xres - fbi->var.xres;
+			}
+			if (fbi->var.yres + pos.y > bg_fbi->var.yres) {
+				if (bg_fbi->var.yres < fbi->var.yres)
+					pos.y = 0;
+				else
+					pos.y = bg_fbi->var.yres - fbi->var.yres;
+			}
+
+			retval = ipu_disp_set_window_pos(mxc_fbi->ipu_ch,
+							 pos.x, pos.y);
+
+			if (copy_to_user((void *)arg, &pos, sizeof(pos))) {
+				retval = -EFAULT;
+				break;
+			}
+			break;
+		}
+	case MXCFB_GET_FB_IPU_CHAN:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->ipu_ch, argp))
+				return -EFAULT;
+			break;
+		}
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/*
+ * mxcfb_blank():
+ *      Blank the display.
+ */
+static int mxcfb_blank(int blank, struct fb_info *info)
+{
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
+
+	dev_dbg(info->device, "blank = %d\n", blank);
+
+	if (mxc_fbi->blank == blank)
+		return 0;
+
+	mxc_fbi->blank = blank;
+
+	switch (blank) {
+	case FB_BLANK_POWERDOWN:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_NORMAL:
+		ipu_disable_channel(mxc_fbi->ipu_ch, true);
+		ipu_uninit_channel(mxc_fbi->ipu_ch);
+		break;
+	case FB_BLANK_UNBLANK:
+		mxcfb_set_par(info);
+		break;
+	}
+	return 0;
+}
+
+/*
+ * Pan or Wrap the Display
+ *
+ * This call looks only at xoffset, yoffset and the FB_VMODE_YWRAP flag
+ *
+ * @param               var     Variable screen buffer information
+ * @param               info    Framebuffer information pointer
+ */
+static int
+mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
+	u_int y_bottom;
+	unsigned long base;
+
+	if (var->xoffset > 0) {
+		dev_dbg(info->device, "x panning not supported\n");
+		return -EINVAL;
+	}
+
+	if ((info->var.xoffset == var->xoffset) &&
+	    (info->var.yoffset == var->yoffset))
+		return 0;	/* No change, do nothing */
+
+	y_bottom = var->yoffset;
+
+	if (!(var->vmode & FB_VMODE_YWRAP))
+		y_bottom += var->yres;
+
+	if (y_bottom > info->var.yres_virtual)
+		return -EINVAL;
+
+	base = (var->yoffset * var->xres_virtual + var->xoffset);
+	base *= (var->bits_per_pixel) / 8;
+	base += info->fix.smem_start;
+
+	dev_dbg(info->device, "Updating SDC BG buf %d address=0x%08lX\n",
+		mxc_fbi->cur_ipu_buf, base);
+
+	down(&mxc_fbi->flip_sem);
+	init_completion(&mxc_fbi->vsync_complete);
+
+	mxc_fbi->cur_ipu_buf = !mxc_fbi->cur_ipu_buf;
+	if (ipu_update_channel_buffer(mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
+				      mxc_fbi->cur_ipu_buf, base) == 0) {
+		ipu_select_buffer(mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
+				  mxc_fbi->cur_ipu_buf);
+		ipu_clear_irq(mxc_fbi->ipu_ch_irq);
+		ipu_enable_irq(mxc_fbi->ipu_ch_irq);
+	} else {
+		dev_err(info->device,
+			"Error updating SDC buf %d to address=0x%08lX\n",
+			mxc_fbi->cur_ipu_buf, base);
+	}
+
+	dev_dbg(info->device, "Update complete\n");
+
+	info->var.xoffset = var->xoffset;
+	info->var.yoffset = var->yoffset;
+
+	if (var->vmode & FB_VMODE_YWRAP)
+		info->var.vmode |= FB_VMODE_YWRAP;
+	else
+		info->var.vmode &= ~FB_VMODE_YWRAP;
+
+	return 0;
+}
+
+/*
+ * Function to handle custom mmap for MXC framebuffer.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @param       vma     Pointer to vm_area_struct
+ */
+static int mxcfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
+{
+	bool found = false;
+	u32 len;
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	struct mxcfb_alloc_list *mem;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	if (offset < fbi->fix.smem_len) {
+		/* mapping framebuffer memory */
+		len = fbi->fix.smem_len - offset;
+		vma->vm_pgoff = (fbi->fix.smem_start + offset) >> PAGE_SHIFT;
+	} else if ((vma->vm_pgoff ==
+			(mxc_fbi->alpha_phy_addr0 >> PAGE_SHIFT)) ||
+		   (vma->vm_pgoff ==
+			(mxc_fbi->alpha_phy_addr1 >> PAGE_SHIFT))) {
+		len = mxc_fbi->alpha_mem_len;
+	} else {
+		list_for_each_entry(mem, &fb_alloc_list, list) {
+			if (offset == mem->phy_addr) {
+				found = true;
+				len = mem->size;
+				break;
+			}
+		}
+		if (!found)
+			return -EINVAL;
+	}
+
+	len = PAGE_ALIGN(len);
+	if (vma->vm_end - vma->vm_start > len)
+		return -EINVAL;
+
+	/* make buffers bufferable */
+	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+
+	vma->vm_flags |= VM_IO | VM_RESERVED;
+
+	if (remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
+			    vma->vm_end - vma->vm_start, vma->vm_page_prot)) {
+		dev_dbg(fbi->device, "mmap remap_pfn_range failed\n");
+		return -ENOBUFS;
+	}
+
+	return 0;
+}
+
+/*!
+ * This structure contains the pointers to the control functions that are
+ * invoked by the core framebuffer driver to perform operations like
+ * blitting, rectangle filling, copy regions and cursor definition.
+ */
+static struct fb_ops mxcfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_set_par = mxcfb_set_par,
+	.fb_check_var = mxcfb_check_var,
+	.fb_setcolreg = mxcfb_setcolreg,
+	.fb_pan_display = mxcfb_pan_display,
+	.fb_ioctl = mxcfb_ioctl,
+	.fb_mmap = mxcfb_mmap,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+	.fb_blank = mxcfb_blank,
+};
+
+static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id)
+{
+	struct fb_info *fbi = dev_id;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	complete(&mxc_fbi->vsync_complete);
+	up(&mxc_fbi->flip_sem);
+	ipu_disable_irq(irq);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mxcfb_alpha_irq_handler(int irq, void *dev_id)
+{
+	struct fb_info *fbi = dev_id;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	up(&mxc_fbi->alpha_flip_sem);
+	ipu_disable_irq(irq);
+	return IRQ_HANDLED;
+}
+
+/*
+ * Suspends the framebuffer and blanks the screen. Power management support
+ */
+static int mxcfb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct fb_info *fbi = platform_get_drvdata(pdev);
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+	int saved_blank;
+#ifdef CONFIG_FB_MXC_LOW_PWR_DISPLAY
+	void *fbmem;
+#endif
+
+	acquire_console_sem();
+	fb_set_suspend(fbi, 1);
+	saved_blank = mxc_fbi->blank;
+	mxcfb_blank(FB_BLANK_POWERDOWN, fbi);
+	mxc_fbi->blank = saved_blank;
+	release_console_sem();
+
+	return 0;
+}
+
+/*
+ * Resumes the framebuffer and unblanks the screen. Power management support
+ */
+static int mxcfb_resume(struct platform_device *pdev)
+{
+	struct fb_info *fbi = platform_get_drvdata(pdev);
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+	int saved_blank;
+
+	acquire_console_sem();
+	saved_blank = mxc_fbi->blank;
+	mxc_fbi->blank = FB_BLANK_POWERDOWN;
+	mxcfb_blank(saved_blank, fbi);
+	fb_set_suspend(fbi, 0);
+	release_console_sem();
+
+	return 0;
+}
+
+/*
+ * Main framebuffer functions
+ */
+
+/*!
+ * Allocates the DRAM memory for the frame buffer.      This buffer is remapped
+ * into a non-cached, non-buffered, memory region to allow palette and pixel
+ * writes to occur without flushing the cache.  Once this area is remapped,
+ * all virtual memory access to the video memory should occur at the new region.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @return      Error code indicating success or failure
+ */
+static int mxcfb_map_video_memory(struct fb_info *fbi)
+{
+	if (fbi->fix.smem_len < fbi->var.yres_virtual * fbi->fix.line_length)
+		fbi->fix.smem_len = fbi->var.yres_virtual *
+				    fbi->fix.line_length;
+
+	fbi->screen_base = dma_alloc_writecombine(fbi->device,
+				fbi->fix.smem_len,
+				(dma_addr_t *)&fbi->fix.smem_start,
+				GFP_DMA);
+	if (fbi->screen_base == 0) {
+		dev_err(fbi->device, "Unable to allocate framebuffer memory\n");
+		fbi->fix.smem_len = 0;
+		fbi->fix.smem_start = 0;
+		return -EBUSY;
+	}
+
+	dev_dbg(fbi->device, "allocated fb @ paddr=0x%08X, size=%d.\n",
+		(uint32_t) fbi->fix.smem_start, fbi->fix.smem_len);
+
+	fbi->screen_size = fbi->fix.smem_len;
+
+	/* Clear the screen */
+	memset((char *)fbi->screen_base, 0, fbi->fix.smem_len);
+
+	return 0;
+}
+
+/*!
+ * De-allocates the DRAM memory for the frame buffer.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @return      Error code indicating success or failure
+ */
+static int mxcfb_unmap_video_memory(struct fb_info *fbi)
+{
+	dma_free_writecombine(fbi->device, fbi->fix.smem_len,
+			      fbi->screen_base, fbi->fix.smem_start);
+	fbi->screen_base = 0;
+	fbi->fix.smem_start = 0;
+	fbi->fix.smem_len = 0;
+	return 0;
+}
+
+/*!
+ * Initializes the framebuffer information pointer. After allocating
+ * sufficient memory for the framebuffer structure, the fields are
+ * filled with custom information passed in from the configurable
+ * structures.  This includes information such as bits per pixel,
+ * color maps, screen width/height and RGBA offsets.
+ *
+ * @return      Framebuffer structure initialized with our information
+ */
+static struct fb_info *mxcfb_init_fbinfo(struct device *dev, struct fb_ops *ops)
+{
+	struct fb_info *fbi;
+	struct mxcfb_info *mxcfbi;
+
+	/*
+	 * Allocate sufficient memory for the fb structure
+	 */
+	fbi = framebuffer_alloc(sizeof(struct mxcfb_info), dev);
+	if (!fbi)
+		return NULL;
+
+	mxcfbi = (struct mxcfb_info *)fbi->par;
+
+	fbi->var.activate = FB_ACTIVATE_NOW;
+
+	fbi->fbops = ops;
+	fbi->flags = FBINFO_FLAG_DEFAULT;
+	fbi->pseudo_palette = mxcfbi->pseudo_palette;
+
+	/*
+	 * Allocate colormap
+	 */
+	fb_alloc_cmap(&fbi->cmap, 16, 0);
+
+	return fbi;
+}
+
+static ssize_t show_disp_chan(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct mxcfb_info *mxcfbi = (struct mxcfb_info *)info->par;
+
+	if (mxcfbi->ipu_ch == MEM_BG_SYNC)
+		return sprintf(buf, "2-layer-fb-bg\n");
+	else if (mxcfbi->ipu_ch == MEM_FG_SYNC)
+		return sprintf(buf, "2-layer-fb-fg\n");
+	else if (mxcfbi->ipu_ch == MEM_DC_SYNC)
+		return sprintf(buf, "1-layer-fb\n");
+	else
+		return sprintf(buf, "err: no display chan\n");
+}
+
+static ssize_t swap_disp_chan(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct mxcfb_info *mxcfbi = (struct mxcfb_info *)info->par;
+	struct mxcfb_info *fg_mxcfbi = NULL;
+
+	/* swap only happen between DP-BG and DC, while DP-FG disable */
+	if (((mxcfbi->ipu_ch == MEM_BG_SYNC) &&
+	     (strstr(buf, "1-layer-fb") != NULL)) ||
+	    ((mxcfbi->ipu_ch == MEM_DC_SYNC) &&
+	     (strstr(buf, "2-layer-fb-bg") != NULL))) {
+		int i;
+
+		for (i = 0; i < num_registered_fb; i++) {
+			fg_mxcfbi =
+				(struct mxcfb_info *)mxcfb_info[i]->par;
+			if (fg_mxcfbi->ipu_ch == MEM_FG_SYNC)
+				break;
+			else
+				fg_mxcfbi = NULL;
+		}
+		if (!fg_mxcfbi ||
+			fg_mxcfbi->blank == FB_BLANK_UNBLANK) {
+			dev_err(dev,
+				"Can not switch while fb2(fb-fg) is on.\n");
+			return count;
+		}
+
+		if (swap_channels(info) < 0)
+			dev_err(dev, "Swap display channel failed.\n");
+	}
+
+	return count;
+}
+DEVICE_ATTR(fsl_disp_property, 644, show_disp_chan, swap_disp_chan);
+
+/*!
+ * Probe routine for the framebuffer driver. It is called during the
+ * driver binding process.      The following functions are performed in
+ * this routine: Framebuffer initialization, Memory allocation and
+ * mapping, Framebuffer registration, IPU initialization.
+ *
+ * @return      Appropriate error code to the kernel common code
+ */
+static int mxcfb_probe(struct platform_device *pdev)
+{
+	struct fb_info *fbi;
+	struct mxcfb_info *mxcfbi;
+	struct mxc_fb_platform_data *plat_data = pdev->dev.platform_data;
+	struct resource *res;
+	int ret = 0;
+
+	/*
+	 * Initialize FB structures
+	 */
+	fbi = mxcfb_init_fbinfo(&pdev->dev, &mxcfb_ops);
+	if (!fbi) {
+		ret = -ENOMEM;
+		goto err0;
+	}
+	mxcfbi = (struct mxcfb_info *)fbi->par;
+
+	if (!g_dp_in_use) {
+		mxcfbi->ipu_ch_irq = IPU_IRQ_BG_SYNC_EOF;
+		mxcfbi->ipu_ch = MEM_BG_SYNC;
+		mxcfbi->blank = FB_BLANK_UNBLANK;
+	} else {
+		mxcfbi->ipu_ch_irq = IPU_IRQ_DC_SYNC_EOF;
+		mxcfbi->ipu_ch = MEM_DC_SYNC;
+		mxcfbi->blank = FB_BLANK_POWERDOWN;
+	}
+
+	mxcfbi->ipu_di = pdev->id;
+
+	if (pdev->id == 0) {
+		ipu_disp_set_global_alpha(mxcfbi->ipu_ch, true, 0x80);
+		ipu_disp_set_color_key(mxcfbi->ipu_ch, false, 0);
+		strcpy(fbi->fix.id, "DISP3 BG");
+
+		if (!g_dp_in_use)
+			if (ipu_request_irq(IPU_IRQ_BG_ALPHA_SYNC_EOF,
+					    mxcfb_alpha_irq_handler, 0,
+					    MXCFB_NAME, fbi) != 0) {
+				dev_err(&pdev->dev, "Error registering BG "
+						    "alpha irq handler.\n");
+				ret = -EBUSY;
+				goto err1;
+			}
+		g_dp_in_use = true;
+	} else if (pdev->id == 1) {
+		strcpy(fbi->fix.id, "DISP3 BG - DI1");
+
+		if (!g_dp_in_use)
+			if (ipu_request_irq(IPU_IRQ_BG_ALPHA_SYNC_EOF,
+					    mxcfb_alpha_irq_handler, 0,
+					    MXCFB_NAME, fbi) != 0) {
+				dev_err(&pdev->dev, "Error registering BG "
+						    "alpha irq handler.\n");
+				ret = -EBUSY;
+				goto err1;
+			}
+		g_dp_in_use = true;
+	} else if (pdev->id == 2) {	/* Overlay */
+		mxcfbi->ipu_ch_irq = IPU_IRQ_FG_SYNC_EOF;
+		mxcfbi->ipu_ch = MEM_FG_SYNC;
+		mxcfbi->ipu_di = -1;
+		mxcfbi->overlay = true;
+		mxcfbi->blank = FB_BLANK_POWERDOWN;
+
+		strcpy(fbi->fix.id, "DISP3 FG");
+
+		if (ipu_request_irq(IPU_IRQ_FG_ALPHA_SYNC_EOF,
+				    mxcfb_alpha_irq_handler, 0,
+				    MXCFB_NAME, fbi) != 0) {
+			dev_err(&pdev->dev, "Error registering FG alpha irq "
+					    "handler.\n");
+			ret = -EBUSY;
+			goto err1;
+		}
+	}
+
+	mxcfb_info[pdev->id] = fbi;
+
+	if (ipu_request_irq(mxcfbi->ipu_ch_irq, mxcfb_irq_handler, 0,
+			    MXCFB_NAME, fbi) != 0) {
+		dev_err(&pdev->dev, "Error registering BG irq handler.\n");
+		ret = -EBUSY;
+		goto err1;
+	}
+	ipu_disable_irq(mxcfbi->ipu_ch_irq);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res) {
+		fbi->fix.smem_len = res->end - res->start + 1;
+		fbi->fix.smem_start = res->start;
+		fbi->screen_base = ioremap(fbi->fix.smem_start, fbi->fix.smem_len);
+	}
+
+	/* Need dummy values until real panel is configured */
+	fbi->var.xres = 240;
+	fbi->var.yres = 320;
+
+	if (!fb_mode && plat_data && plat_data->mode_str)
+		fb_find_mode(&fbi->var, fbi, plat_data->mode_str, NULL, 0, NULL,
+			     default_bpp);
+
+	if (fb_mode)
+		fb_find_mode(&fbi->var, fbi, fb_mode, NULL, 0, NULL,
+			     default_bpp);
+
+	if (plat_data) {
+		mxcfbi->ipu_di_pix_fmt = plat_data->interface_pix_fmt;
+		if (!fb_mode && plat_data->mode)
+			fb_videomode_to_var(&fbi->var, plat_data->mode);
+	}
+
+	mxcfb_check_var(&fbi->var, fbi);
+
+	/* Default Y virtual size is 2x panel size */
+	fbi->var.yres_virtual = fbi->var.yres * 2;
+
+	mxcfb_set_fix(fbi);
+
+	/* alocate fb first */
+	if (!res)
+		if (mxcfb_map_video_memory(fbi) < 0)
+			return -ENOMEM;
+
+	ret = register_framebuffer(fbi);
+	if (ret < 0)
+		goto err2;
+
+	platform_set_drvdata(pdev, fbi);
+
+	ret = device_create_file(fbi->dev, &dev_attr_fsl_disp_property);
+	if (ret)
+		dev_err(&pdev->dev, "Error %d on creating file\n", ret);
+
+	return 0;
+
+err2:
+	ipu_free_irq(mxcfbi->ipu_ch_irq, fbi);
+err1:
+	fb_dealloc_cmap(&fbi->cmap);
+	framebuffer_release(fbi);
+err0:
+	return ret;
+}
+
+static int mxcfb_remove(struct platform_device *pdev)
+{
+	struct fb_info *fbi = platform_get_drvdata(pdev);
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	if (!fbi)
+		return 0;
+
+	mxcfb_blank(FB_BLANK_POWERDOWN, fbi);
+	ipu_free_irq(mxc_fbi->ipu_ch_irq, fbi);
+	mxcfb_unmap_video_memory(fbi);
+
+	if (&fbi->cmap)
+		fb_dealloc_cmap(&fbi->cmap);
+
+	unregister_framebuffer(fbi);
+	framebuffer_release(fbi);
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcfb_driver = {
+	.driver = {
+		   .name = MXCFB_NAME,
+		   },
+	.probe = mxcfb_probe,
+	.remove = mxcfb_remove,
+	.suspend = mxcfb_suspend,
+	.resume = mxcfb_resume,
+};
+
+/*
+ * Parse user specified options (`video=trident:')
+ * example:
+ * 	video=trident:800x600,bpp=16,noaccel
+ */
+int mxcfb_setup(char *options)
+{
+	char *opt;
+	if (!options || !*options)
+		return 0;
+	while ((opt = strsep(&options, ",")) != NULL) {
+		if (!*opt)
+			continue;
+		if (!strncmp(opt, "bpp=", 4))
+			default_bpp = simple_strtoul(opt + 4, NULL, 0);
+		else
+			fb_mode = opt;
+	}
+	return 0;
+}
+
+/*!
+ * Main entry function for the framebuffer. The function registers the power
+ * management callback functions with the kernel and also registers the MXCFB
+ * callback functions with the core Linux framebuffer driver \b fbmem.c
+ *
+ * @return      Error code indicating success or failure
+ */
+int __init mxcfb_init(void)
+{
+	int ret = 0;
+#ifndef MODULE
+	char *option = NULL;
+#endif
+
+#ifndef MODULE
+	if (fb_get_options("mxcfb", &option))
+		return -ENODEV;
+	mxcfb_setup(option);
+#endif
+
+	ret = platform_driver_register(&mxcfb_driver);
+	return ret;
+}
+
+void mxcfb_exit(void)
+{
+	platform_driver_unregister(&mxcfb_driver);
+}
+
+module_init(mxcfb_init);
+module_exit(mxcfb_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC framebuffer driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("fb");
diff --git a/drivers/video/mxc/mxcfb.c b/drivers/video/mxc/mxcfb.c
new file mode 100644
index 000000000000..668f4017471f
--- /dev/null
+++ b/drivers/video/mxc/mxcfb.c
@@ -0,0 +1,1502 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup Framebuffer Framebuffer Driver for SDC and ADC.
+ */
+
+/*!
+ * @file mxcfb.c
+ *
+ * @brief MXC Frame buffer driver for SDC
+ *
+ * @ingroup Framebuffer
+ */
+
+/*!
+ * Include files
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/dma-mapping.h>
+#include <linux/clk.h>
+#include <linux/console.h>
+#include <linux/io.h>
+#include <linux/ipu.h>
+#include <linux/mxcfb.h>
+#include <asm/mach-types.h>
+#include <asm/uaccess.h>
+#include <mach/hardware.h>
+
+/*
+ * Driver name
+ */
+#define MXCFB_NAME      "mxc_sdc_fb"
+/*!
+ * Structure containing the MXC specific framebuffer information.
+ */
+struct mxcfb_info {
+	int blank;
+	ipu_channel_t ipu_ch;
+	uint32_t ipu_ch_irq;
+	uint32_t cur_ipu_buf;
+
+	u32 pseudo_palette[16];
+
+	struct semaphore flip_sem;
+	spinlock_t fb_lock;
+};
+
+struct mxcfb_data {
+	struct fb_info *fbi;
+	struct fb_info *fbi_ovl;
+	volatile int32_t vsync_flag;
+	wait_queue_head_t vsync_wq;
+	wait_queue_head_t suspend_wq;
+	bool suspended;
+	int backlight_level;
+};
+
+struct mxcfb_alloc_list {
+	struct list_head list;
+	dma_addr_t phy_addr;
+	void *cpu_addr;
+	u32 size;
+};
+
+static struct mxcfb_data mxcfb_drv_data;
+
+static char *fb_mode = NULL;
+static unsigned long default_bpp = 16;
+#ifdef CONFIG_FB_MXC_INTERNAL_MEM
+static struct clk *iram_clk;
+#endif
+LIST_HEAD(fb_alloc_list);
+
+static uint32_t bpp_to_pixfmt(int bpp)
+{
+	uint32_t pixfmt = 0;
+	switch (bpp) {
+	case 24:
+		pixfmt = IPU_PIX_FMT_BGR24;
+		break;
+	case 32:
+		pixfmt = IPU_PIX_FMT_BGR32;
+		break;
+	case 16:
+		pixfmt = IPU_PIX_FMT_RGB565;
+		break;
+	}
+	return pixfmt;
+}
+
+extern void gpio_lcd_active(void);
+extern void gpio_lcd_inactive(void);
+#ifdef CONFIG_FB_MXC_TVOUT
+#include <linux/video_encoder.h>
+/*
+ * FIXME: VGA mode is not defined by video_encoder.h
+ * while FS453 supports VGA output.
+ */
+#ifndef VIDEO_ENCODER_VGA
+#define VIDEO_ENCODER_VGA	32
+#endif
+
+#define MODE_PAL		"TV-PAL"
+#define MODE_NTSC		"TV-NTSC"
+#define MODE_VGA		"TV-VGA"
+
+extern int fs453_ioctl(unsigned int cmd, void *arg);
+#endif
+static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id);
+static int mxcfb_blank(int blank, struct fb_info *info);
+static int mxcfb_map_video_memory(struct fb_info *fbi, bool use_internal_ram);
+static int mxcfb_unmap_video_memory(struct fb_info *fbi);
+
+/*
+ * Set fixed framebuffer parameters based on variable settings.
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_set_fix(struct fb_info *info)
+{
+	struct fb_fix_screeninfo *fix = &info->fix;
+	struct fb_var_screeninfo *var = &info->var;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
+
+	if (mxc_fbi->ipu_ch == MEM_SDC_FG)
+		strncpy(fix->id, "DISP3 FG", 8);
+	else
+		strncpy(fix->id, "DISP3 BG", 8);
+
+	fix->line_length = var->xres_virtual * var->bits_per_pixel / 8;
+
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->accel = FB_ACCEL_NONE;
+	fix->visual = FB_VISUAL_TRUECOLOR;
+	fix->xpanstep = 1;
+	fix->ypanstep = 1;
+
+	return 0;
+}
+
+/*
+ * Set framebuffer parameters and change the operating mode.
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_set_par(struct fb_info *fbi)
+{
+	int retval;
+	bool use_iram = false;
+	u32 mem_len;
+	ipu_di_signal_cfg_t sig_cfg;
+	ipu_panel_t mode = IPU_PANEL_TFT;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	if ((retval = wait_event_interruptible(mxcfb_drv_data.suspend_wq,
+					       (mxcfb_drv_data.suspended ==
+						false))) < 0) {
+		return retval;
+	}
+
+	ipu_disable_irq(mxc_fbi->ipu_ch_irq);
+	ipu_disable_channel(mxc_fbi->ipu_ch, true);
+	ipu_uninit_channel(mxc_fbi->ipu_ch);
+	ipu_clear_irq(mxc_fbi->ipu_ch_irq);
+	mxcfb_set_fix(fbi);
+
+	mem_len = fbi->var.yres_virtual * fbi->fix.line_length;
+	if (mem_len > fbi->fix.smem_len) {
+		if (fbi->fix.smem_start)
+			mxcfb_unmap_video_memory(fbi);
+
+#ifdef CONFIG_FB_MXC_INTERNAL_MEM
+		if (mxc_fbi->ipu_ch == MEM_SDC_BG) {
+			use_iram = true;
+		}
+#endif
+		if (mxcfb_map_video_memory(fbi, use_iram) < 0)
+			return -ENOMEM;
+	}
+
+	ipu_init_channel(mxc_fbi->ipu_ch, NULL);
+
+	/* Clear the screen */
+	memset((char *)fbi->screen_base, 0, fbi->fix.smem_len);
+
+	if (mxc_fbi->ipu_ch == MEM_SDC_BG) {
+		memset(&sig_cfg, 0, sizeof(sig_cfg));
+		if (fbi->var.sync & FB_SYNC_HOR_HIGH_ACT)
+			sig_cfg.Hsync_pol = true;
+		if (fbi->var.sync & FB_SYNC_VERT_HIGH_ACT)
+			sig_cfg.Vsync_pol = true;
+		if (!(fbi->var.sync & FB_SYNC_CLK_LAT_FALL))
+			sig_cfg.clk_pol = true;
+		if (fbi->var.sync & FB_SYNC_DATA_INVERT)
+			sig_cfg.data_pol = true;
+		if (!(fbi->var.sync & FB_SYNC_OE_LOW_ACT))
+			sig_cfg.enable_pol = true;
+		if (fbi->var.sync & FB_SYNC_CLK_IDLE_EN)
+			sig_cfg.clkidle_en = true;
+		if (fbi->var.sync & FB_SYNC_SHARP_MODE)
+			mode = IPU_PANEL_SHARP_TFT;
+
+		dev_dbg(fbi->device, "pixclock = %ul Hz\n",
+			(u32) (PICOS2KHZ(fbi->var.pixclock) * 1000UL));
+
+		if (ipu_sdc_init_panel(mode,
+				       (PICOS2KHZ(fbi->var.pixclock)) * 1000UL,
+				       fbi->var.xres, fbi->var.yres,
+				       (fbi->var.sync & FB_SYNC_SWAP_RGB) ?
+				       IPU_PIX_FMT_BGR666 : IPU_PIX_FMT_RGB666,
+				       fbi->var.left_margin,
+				       fbi->var.hsync_len,
+				       fbi->var.right_margin,
+				       fbi->var.upper_margin,
+				       fbi->var.vsync_len,
+				       fbi->var.lower_margin, sig_cfg) != 0) {
+			dev_err(fbi->device,
+				"mxcfb: Error initializing panel.\n");
+			return -EINVAL;
+		}
+
+		fbi->mode =
+		    (struct fb_videomode *)fb_match_mode(&fbi->var,
+							 &fbi->modelist);
+	}
+
+	ipu_disp_set_window_pos(mxc_fbi->ipu_ch, 0, 0);
+
+	mxc_fbi->cur_ipu_buf = 1;
+	sema_init(&mxc_fbi->flip_sem, 1);
+	fbi->var.xoffset = fbi->var.yoffset = 0;
+
+	retval = ipu_init_channel_buffer(mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
+					 bpp_to_pixfmt(fbi->var.bits_per_pixel),
+					 fbi->var.xres, fbi->var.yres,
+					 fbi->var.xres_virtual,
+					 IPU_ROTATE_NONE,
+					 fbi->fix.smem_start +
+					 (fbi->fix.line_length * fbi->var.yres),
+					 fbi->fix.smem_start,
+					 0, 0);
+	if (retval) {
+		dev_err(fbi->device,
+			"ipu_init_channel_buffer error %d\n", retval);
+		return retval;
+	}
+
+	if (mxc_fbi->blank == FB_BLANK_UNBLANK) {
+		ipu_enable_channel(mxc_fbi->ipu_ch);
+	}
+
+	return 0;
+}
+
+/*
+ * Check framebuffer variable parameters and adjust to valid values.
+ *
+ * @param       var      framebuffer variable parameters
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	u32 vtotal;
+	u32 htotal;
+
+	if (var->xres_virtual < var->xres)
+		var->xres_virtual = var->xres;
+	if (var->yres_virtual < var->yres)
+		var->yres_virtual = var->yres;
+
+#ifdef CONFIG_FB_MXC_INTERNAL_MEM
+	if ((info->fix.smem_start == FB_RAM_BASE_ADDR) &&
+	    ((var->yres_virtual * var->xres_virtual * var->bits_per_pixel / 8) >
+	     FB_RAM_SIZE)) {
+		return -EINVAL;
+	}
+#endif
+
+	if ((var->bits_per_pixel != 32) && (var->bits_per_pixel != 24) &&
+	    (var->bits_per_pixel != 16)) {
+		var->bits_per_pixel = default_bpp;
+	}
+
+	switch (var->bits_per_pixel) {
+	case 16:
+		var->red.length = 5;
+		var->red.offset = 11;
+		var->red.msb_right = 0;
+
+		var->green.length = 6;
+		var->green.offset = 5;
+		var->green.msb_right = 0;
+
+		var->blue.length = 5;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 24:
+		var->red.length = 8;
+		var->red.offset = 16;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 8;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 32:
+		var->red.length = 8;
+		var->red.offset = 16;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 8;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 8;
+		var->transp.offset = 24;
+		var->transp.msb_right = 0;
+		break;
+	}
+
+	if (var->pixclock < 1000) {
+		htotal = var->xres + var->right_margin + var->hsync_len +
+		    var->left_margin;
+		vtotal = var->yres + var->lower_margin + var->vsync_len +
+		    var->upper_margin;
+		var->pixclock = (vtotal * htotal * 6UL) / 100UL;
+		var->pixclock = KHZ2PICOS(var->pixclock);
+		dev_dbg(info->device,
+			"pixclock set for 60Hz refresh = %u ps\n",
+			var->pixclock);
+	}
+
+	var->height = -1;
+	var->width = -1;
+	var->grayscale = 0;
+
+	/* nonstd used for YUV formats, but only RGB supported */
+	var->nonstd = 0;
+
+	return 0;
+}
+
+static inline u_int _chan_to_field(u_int chan, struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+static int
+mxcfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+		u_int trans, struct fb_info *fbi)
+{
+	unsigned int val;
+	int ret = 1;
+
+	/*
+	 * If greyscale is true, then we convert the RGB value
+	 * to greyscale no matter what visual we are using.
+	 */
+	if (fbi->var.grayscale)
+		red = green = blue = (19595 * red + 38470 * green +
+				      7471 * blue) >> 16;
+	switch (fbi->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		/*
+		 * 16-bit True Colour.  We encode the RGB value
+		 * according to the RGB bitfield information.
+		 */
+		if (regno < 16) {
+			u32 *pal = fbi->pseudo_palette;
+
+			val = _chan_to_field(red, &fbi->var.red);
+			val |= _chan_to_field(green, &fbi->var.green);
+			val |= _chan_to_field(blue, &fbi->var.blue);
+
+			pal[regno] = val;
+			ret = 0;
+		}
+		break;
+
+	case FB_VISUAL_STATIC_PSEUDOCOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		break;
+	}
+
+	return ret;
+}
+
+/*
+ * Function to handle custom ioctls for MXC framebuffer.
+ *
+ * @param       inode   inode struct
+ *
+ * @param       file    file struct
+ *
+ * @param       cmd     Ioctl command to handle
+ *
+ * @param       arg     User pointer to command arguments
+ *
+ * @param       fbi     framebuffer information pointer
+ */
+static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	int __user *argp = (void __user *)arg;
+
+	if ((retval = wait_event_interruptible(mxcfb_drv_data.suspend_wq,
+					       (mxcfb_drv_data.suspended ==
+						false))) < 0) {
+		return retval;
+	}
+
+	switch (cmd) {
+	case MXCFB_SET_GBL_ALPHA:
+		{
+			struct mxcfb_gbl_alpha ga;
+			if (copy_from_user(&ga, (void *)arg, sizeof(ga))) {
+				retval = -EFAULT;
+				break;
+			}
+			retval =
+			    ipu_sdc_set_global_alpha((bool) ga.enable,
+						     ga.alpha);
+			dev_dbg(fbi->device, "Set global alpha to %d\n",
+				ga.alpha);
+			break;
+		}
+	case MXCFB_SET_CLR_KEY:
+		{
+			struct mxcfb_color_key key;
+			if (copy_from_user(&key, (void *)arg, sizeof(key))) {
+				retval = -EFAULT;
+				break;
+			}
+			retval = ipu_sdc_set_color_key(MEM_SDC_BG, key.enable,
+						       key.color_key);
+			dev_dbg(fbi->device, "Set color key to 0x%08X\n",
+				key.color_key);
+			break;
+		}
+	case MXCFB_WAIT_FOR_VSYNC:
+		{
+#ifndef CONFIG_ARCH_MX3
+			mxcfb_drv_data.vsync_flag = 0;
+			ipu_enable_irq(IPU_IRQ_SDC_DISP3_VSYNC);
+			if (!wait_event_interruptible_timeout
+			    (mxcfb_drv_data.vsync_wq,
+			     mxcfb_drv_data.vsync_flag != 0, 1 * HZ)) {
+				dev_err(fbi->device,
+					"MXCFB_WAIT_FOR_VSYNC: timeout\n");
+				retval = -ETIME;
+				break;
+			} else if (signal_pending(current)) {
+				dev_err(fbi->device,
+					"MXCFB_WAIT_FOR_VSYNC: interrupt received\n");
+				retval = -ERESTARTSYS;
+				break;
+			}
+#endif
+			break;
+		}
+	case MXCFB_GET_FB_IPU_CHAN:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->ipu_ch, argp))
+				return -EFAULT;
+
+			break;
+		}
+#ifdef CONFIG_FB_MXC_TVOUT
+	case ENCODER_GET_CAPABILITIES:
+		{
+			struct video_encoder_capability cap;
+
+			if ((retval = fs453_ioctl(cmd, &cap)))
+				break;
+
+			if (copy_to_user((void *)arg, &cap, sizeof(cap)))
+				retval = -EFAULT;
+			break;
+		}
+	case ENCODER_SET_NORM:
+		{
+			unsigned long mode;
+			char *smode;
+			struct fb_var_screeninfo var;
+
+			if (copy_from_user(&mode, (void *)arg, sizeof(mode))) {
+				retval = -EFAULT;
+				break;
+			}
+			if ((retval = fs453_ioctl(cmd, &mode)))
+				break;
+
+			if (mode == VIDEO_ENCODER_PAL)
+				smode = MODE_PAL;
+			else if (mode == VIDEO_ENCODER_NTSC)
+				smode = MODE_NTSC;
+			else
+				smode = MODE_VGA;
+
+			var = fbi->var;
+			var.nonstd = 0;
+			retval = fb_find_mode(&var, fbi, smode, mxcfb_modedb,
+					      mxcfb_modedb_sz, NULL,
+					      default_bpp);
+			if ((retval != 1) && (retval != 2)) {	/* specified mode not found */
+				retval = -ENODEV;
+				break;
+			}
+
+			fbi->var = var;
+			fb_mode = smode;
+			retval = mxcfb_set_par(fbi);
+			break;
+		}
+	case ENCODER_SET_INPUT:
+	case ENCODER_SET_OUTPUT:
+	case ENCODER_ENABLE_OUTPUT:
+		{
+			unsigned long varg;
+
+			if (copy_from_user(&varg, (void *)arg, sizeof(varg))) {
+				retval = -EFAULT;
+				break;
+			}
+			retval = fs453_ioctl(cmd, &varg);
+			break;
+		}
+#endif
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/*
+ * Function to handle custom ioctls for MXC framebuffer.
+ *
+ * @param       inode   inode struct
+ *
+ * @param       file    file struct
+ *
+ * @param       cmd     Ioctl command to handle
+ *
+ * @param       arg     User pointer to command arguments
+ *
+ * @param       fbi     framebuffer information pointer
+ */
+static int mxcfb_ioctl_ovl(struct fb_info *fbi, unsigned int cmd,
+			   unsigned long arg)
+{
+	int retval = 0;
+	int __user *argp = (void __user *)arg;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	if ((retval = wait_event_interruptible(mxcfb_drv_data.suspend_wq,
+					       (mxcfb_drv_data.suspended ==
+						false))) < 0) {
+		return retval;
+	}
+
+	switch (cmd) {
+	case FBIO_ALLOC:
+		{
+			int size;
+			struct mxcfb_alloc_list *mem;
+
+			mem = kzalloc(sizeof(*mem), GFP_KERNEL);
+			if (mem == NULL)
+				return -ENOMEM;
+
+			if (get_user(size, argp))
+				return -EFAULT;
+
+			mem->size = PAGE_ALIGN(size);
+
+			mem->cpu_addr = dma_alloc_coherent(fbi->device, size,
+							   &mem->phy_addr,
+							   GFP_DMA);
+			if (mem->cpu_addr == NULL) {
+				kfree(mem);
+				return -ENOMEM;
+			}
+
+			list_add(&mem->list, &fb_alloc_list);
+
+			dev_dbg(fbi->device, "allocated %d bytes @ 0x%08X\n",
+				mem->size, mem->phy_addr);
+
+			if (put_user(mem->phy_addr, argp))
+				return -EFAULT;
+
+			break;
+		}
+	case FBIO_FREE:
+		{
+			unsigned long offset;
+			struct mxcfb_alloc_list *mem;
+
+			if (get_user(offset, argp))
+				return -EFAULT;
+
+			retval = -EINVAL;
+			list_for_each_entry(mem, &fb_alloc_list, list) {
+				if (mem->phy_addr == offset) {
+					list_del(&mem->list);
+					dma_free_coherent(fbi->device,
+							  mem->size,
+							  mem->cpu_addr,
+							  mem->phy_addr);
+					kfree(mem);
+					retval = 0;
+					break;
+				}
+			}
+
+			break;
+		}
+	case MXCFB_SET_OVERLAY_POS:
+		{
+			struct mxcfb_pos pos;
+			if (copy_from_user(&pos, (void *)arg, sizeof(pos))) {
+				retval = -EFAULT;
+				break;
+			}
+			retval = ipu_disp_set_window_pos(mxc_fbi->ipu_ch,
+							pos.x, pos.y);
+			break;
+		}
+	case MXCFB_GET_FB_IPU_CHAN:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->ipu_ch, argp))
+				return -EFAULT;
+
+			break;
+		}
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/*
+ * mxcfb_blank():
+ *      Blank the display.
+ */
+static int mxcfb_blank(int blank, struct fb_info *info)
+{
+	int retval;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
+
+	dev_dbg(info->device, "blank = %d\n", blank);
+
+	if (mxc_fbi->blank == blank)
+		return 0;
+
+	if ((retval = wait_event_interruptible(mxcfb_drv_data.suspend_wq,
+					       (mxcfb_drv_data.suspended ==
+						false))) < 0) {
+		return retval;
+	}
+
+	mxc_fbi->blank = blank;
+
+	switch (blank) {
+	case FB_BLANK_POWERDOWN:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_NORMAL:
+		ipu_disable_channel(MEM_SDC_BG, true);
+		gpio_lcd_inactive();
+#ifdef CONFIG_FB_MXC_TVOUT
+		if (fb_mode) {
+			int enable = 0;
+
+			if ((strcmp(fb_mode, MODE_VGA) == 0)
+			    || (strcmp(fb_mode, MODE_NTSC) == 0)
+			    || (strcmp(fb_mode, MODE_PAL) == 0))
+				fs453_ioctl(ENCODER_ENABLE_OUTPUT, &enable);
+		}
+#endif
+		break;
+	case FB_BLANK_UNBLANK:
+		gpio_lcd_active();
+		ipu_enable_channel(MEM_SDC_BG);
+#ifdef CONFIG_FB_MXC_TVOUT
+		if (fb_mode) {
+			unsigned long mode = 0;
+
+			if (strcmp(fb_mode, MODE_VGA) == 0)
+				mode = VIDEO_ENCODER_VGA;
+			else if (strcmp(fb_mode, MODE_NTSC) == 0)
+				mode = VIDEO_ENCODER_NTSC;
+			else if (strcmp(fb_mode, MODE_PAL) == 0)
+				mode = VIDEO_ENCODER_PAL;
+			if (mode)
+				fs453_ioctl(ENCODER_SET_NORM, &mode);
+		}
+#endif
+		break;
+	}
+	return 0;
+}
+
+/*
+ * mxcfb_blank_ovl():
+ *      Blank the display.
+ */
+static int mxcfb_blank_ovl(int blank, struct fb_info *info)
+{
+	int retval;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
+
+	dev_dbg(info->device, "ovl blank = %d\n", blank);
+
+	if (mxc_fbi->blank == blank)
+		return 0;
+
+	if ((retval = wait_event_interruptible(mxcfb_drv_data.suspend_wq,
+					       (mxcfb_drv_data.suspended ==
+						false))) < 0) {
+		return retval;
+	}
+
+	mxc_fbi->blank = blank;
+
+	switch (blank) {
+	case FB_BLANK_POWERDOWN:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_NORMAL:
+		ipu_disable_channel(MEM_SDC_FG, true);
+		break;
+	case FB_BLANK_UNBLANK:
+		ipu_enable_channel(MEM_SDC_FG);
+		break;
+	}
+	return 0;
+}
+
+/*
+ * Pan or Wrap the Display
+ *
+ * This call looks only at xoffset, yoffset and the FB_VMODE_YWRAP flag
+ *
+ * @param               var     Variable screen buffer information
+ * @param               info    Framebuffer information pointer
+ */
+static int
+mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
+	unsigned long lock_flags = 0;
+	int retval;
+	u_int y_bottom;
+	unsigned long base;
+
+	if ((retval = wait_event_interruptible(mxcfb_drv_data.suspend_wq,
+					       (mxcfb_drv_data.suspended ==
+						false))) < 0) {
+		return retval;
+	}
+
+	if (var->xoffset > 0) {
+		dev_dbg(info->device, "x panning not supported\n");
+		return -EINVAL;
+	}
+
+	if ((info->var.xoffset == var->xoffset) &&
+	    (info->var.yoffset == var->yoffset)) {
+		return 0;	// No change, do nothing
+	}
+
+	y_bottom = var->yoffset;
+
+	if (!(var->vmode & FB_VMODE_YWRAP)) {
+		y_bottom += var->yres;
+	}
+
+	if (y_bottom > info->var.yres_virtual) {
+		return -EINVAL;
+	}
+
+	base = (var->yoffset * var->xres_virtual + var->xoffset);
+	base *= (var->bits_per_pixel) / 8;
+	base += info->fix.smem_start;
+
+	down(&mxc_fbi->flip_sem);
+
+	spin_lock_irqsave(&mxc_fbi->fb_lock, lock_flags);
+
+	dev_dbg(info->device, "Updating SDC BG buf %d address=0x%08lX\n",
+		mxc_fbi->cur_ipu_buf, base);
+
+	mxc_fbi->cur_ipu_buf = !mxc_fbi->cur_ipu_buf;
+	if (ipu_update_channel_buffer(mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
+				      mxc_fbi->cur_ipu_buf, base) == 0) {
+		ipu_select_buffer(mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
+				  mxc_fbi->cur_ipu_buf);
+		ipu_clear_irq(mxc_fbi->ipu_ch_irq);
+		ipu_enable_irq(mxc_fbi->ipu_ch_irq);
+	} else {
+		dev_err(info->device,
+			"Error updating SDC buf %d to address=0x%08lX\n",
+			mxc_fbi->cur_ipu_buf, base);
+	}
+
+	spin_unlock_irqrestore(&mxc_fbi->fb_lock, lock_flags);
+
+	dev_dbg(info->device, "Update complete\n");
+
+	info->var.xoffset = var->xoffset;
+	info->var.yoffset = var->yoffset;
+
+	if (var->vmode & FB_VMODE_YWRAP) {
+		info->var.vmode |= FB_VMODE_YWRAP;
+	} else {
+		info->var.vmode &= ~FB_VMODE_YWRAP;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to handle custom mmap for MXC framebuffer.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @param       vma     Pointer to vm_area_struct
+ */
+static int mxcfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
+{
+	bool found = false;
+	u32 len;
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	struct mxcfb_alloc_list *mem;
+
+	if (offset < fbi->fix.smem_len) {
+		/* mapping framebuffer memory */
+		len = fbi->fix.smem_len - offset;
+		vma->vm_pgoff = (fbi->fix.smem_start + offset) >> PAGE_SHIFT;
+	} else {
+		list_for_each_entry(mem, &fb_alloc_list, list) {
+			if (offset == mem->phy_addr) {
+				found = true;
+				len = mem->size;
+				break;
+			}
+		}
+		if (!found) {
+			return -EINVAL;
+		}
+	}
+
+	len = PAGE_ALIGN(len);
+	if (vma->vm_end - vma->vm_start > len) {
+		return -EINVAL;
+	}
+
+	/* make buffers write-thru cacheable */
+	vma->vm_page_prot = __pgprot(pgprot_val(vma->vm_page_prot) &
+				     ~L_PTE_BUFFERABLE);
+
+	vma->vm_flags |= VM_IO | VM_RESERVED;
+
+	if (remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
+			    vma->vm_end - vma->vm_start, vma->vm_page_prot)) {
+		dev_dbg(fbi->device, "mmap remap_pfn_range failed\n");
+		return -ENOBUFS;
+
+	}
+
+	return 0;
+}
+
+/*!
+ * This structure contains the pointers to the control functions that are
+ * invoked by the core framebuffer driver to perform operations like
+ * blitting, rectangle filling, copy regions and cursor definition.
+ */
+static struct fb_ops mxcfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_set_par = mxcfb_set_par,
+	.fb_check_var = mxcfb_check_var,
+	.fb_setcolreg = mxcfb_setcolreg,
+	.fb_pan_display = mxcfb_pan_display,
+	.fb_ioctl = mxcfb_ioctl,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+	.fb_blank = mxcfb_blank,
+};
+
+static struct fb_ops mxcfb_ovl_ops = {
+	.owner = THIS_MODULE,
+	.fb_set_par = mxcfb_set_par,
+	.fb_check_var = mxcfb_check_var,
+	.fb_setcolreg = mxcfb_setcolreg,
+	.fb_pan_display = mxcfb_pan_display,
+	.fb_ioctl = mxcfb_ioctl_ovl,
+	.fb_mmap = mxcfb_mmap,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+	.fb_blank = mxcfb_blank_ovl,
+};
+
+static irqreturn_t mxcfb_vsync_irq_handler(int irq, void *dev_id)
+{
+	struct mxcfb_data *fb_data = dev_id;
+
+	ipu_disable_irq(irq);
+
+	fb_data->vsync_flag = 1;
+	wake_up_interruptible(&fb_data->vsync_wq);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id)
+{
+	struct fb_info *fbi = dev_id;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	up(&mxc_fbi->flip_sem);
+	ipu_disable_irq(irq);
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_PM
+/*
+ * Power management hooks.      Note that we won't be called from IRQ context,
+ * unlike the blank functions above, so we may sleep.
+ */
+
+/*
+ * Suspends the framebuffer and blanks the screen. Power management support
+ */
+static int mxcfb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct mxcfb_data *drv_data = platform_get_drvdata(pdev);
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)drv_data->fbi->par;
+	struct mxcfb_info *mxc_fbi_ovl =
+	    (struct mxcfb_info *)drv_data->fbi_ovl->par;
+#ifdef CONFIG_FB_MXC_LOW_PWR_DISPLAY
+	void *fbmem;
+#endif
+
+	drv_data->suspended = true;
+
+	acquire_console_sem();
+	fb_set_suspend(drv_data->fbi, 1);
+	fb_set_suspend(drv_data->fbi_ovl, 1);
+	release_console_sem();
+
+	if (mxc_fbi_ovl->blank == FB_BLANK_UNBLANK) {
+		ipu_disable_channel(MEM_SDC_FG, true);
+	}
+
+	if (mxc_fbi->blank == FB_BLANK_UNBLANK) {
+#ifdef CONFIG_FB_MXC_LOW_PWR_DISPLAY
+		if (drv_data->fbi->fix.smem_start != FB_RAM_BASE_ADDR) {
+			fbmem = ioremap(FB_RAM_BASE_ADDR, FB_RAM_SIZE);
+			memcpy(fbmem, drv_data->fbi->screen_base, FB_RAM_SIZE);
+			iounmap(fbmem);
+			mxc_fbi->cur_ipu_buf = !mxc_fbi->cur_ipu_buf;
+			ipu_update_channel_buffer(MEM_SDC_BG, IPU_INPUT_BUFFER,
+						  mxc_fbi->cur_ipu_buf,
+						  FB_RAM_BASE_ADDR);
+			ipu_select_buffer(MEM_SDC_BG, IPU_INPUT_BUFFER,
+					  mxc_fbi->cur_ipu_buf);
+		}
+		ipu_lowpwr_display_enable();
+#else
+		ipu_disable_channel(MEM_SDC_BG, true);
+		gpio_lcd_inactive();
+#endif
+#ifdef CONFIG_FB_MXC_TVOUT
+		if (fb_mode) {
+			int enable = 0;
+
+			if ((strcmp(fb_mode, MODE_VGA) == 0)
+			    || (strcmp(fb_mode, MODE_NTSC) == 0)
+			    || (strcmp(fb_mode, MODE_PAL) == 0))
+				fs453_ioctl(ENCODER_ENABLE_OUTPUT, &enable);
+		}
+#endif
+	}
+	return 0;
+}
+
+/*
+ * Resumes the framebuffer and unblanks the screen. Power management support
+ */
+static int mxcfb_resume(struct platform_device *pdev)
+{
+	struct mxcfb_data *drv_data = platform_get_drvdata(pdev);
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)drv_data->fbi->par;
+	struct mxcfb_info *mxc_fbi_ovl =
+	    (struct mxcfb_info *)drv_data->fbi_ovl->par;
+
+	drv_data->suspended = false;
+
+	if (mxc_fbi->blank == FB_BLANK_UNBLANK) {
+#ifdef CONFIG_FB_MXC_LOW_PWR_DISPLAY
+		ipu_lowpwr_display_disable();
+		if (drv_data->fbi->fix.smem_start != FB_RAM_BASE_ADDR) {
+			mxc_fbi->cur_ipu_buf = !mxc_fbi->cur_ipu_buf;
+			ipu_update_channel_buffer(MEM_SDC_BG, IPU_INPUT_BUFFER,
+						  mxc_fbi->cur_ipu_buf,
+						  drv_data->fbi->fix.
+						  smem_start);
+			ipu_select_buffer(MEM_SDC_BG, IPU_INPUT_BUFFER,
+					  mxc_fbi->cur_ipu_buf);
+		}
+#else
+		gpio_lcd_active();
+		ipu_enable_channel(MEM_SDC_BG);
+#endif
+#ifdef CONFIG_FB_MXC_TVOUT
+		if (fb_mode) {
+			u32 mode = 0;
+
+			if (strcmp(fb_mode, MODE_VGA) == 0)
+				mode = VIDEO_ENCODER_VGA;
+			else if (strcmp(fb_mode, MODE_NTSC) == 0)
+				mode = VIDEO_ENCODER_NTSC;
+			else if (strcmp(fb_mode, MODE_PAL) == 0)
+				mode = VIDEO_ENCODER_PAL;
+			if (mode)
+				fs453_ioctl(ENCODER_SET_NORM, &mode);
+		}
+#endif
+	}
+
+	if (mxc_fbi_ovl->blank == FB_BLANK_UNBLANK) {
+		ipu_enable_channel(MEM_SDC_FG);
+	}
+
+	acquire_console_sem();
+	fb_set_suspend(drv_data->fbi, 0);
+	fb_set_suspend(drv_data->fbi_ovl, 0);
+	release_console_sem();
+
+	wake_up_interruptible(&drv_data->suspend_wq);
+	return 0;
+}
+#else
+#define mxcfb_suspend   NULL
+#define mxcfb_resume    NULL
+#endif
+
+/*
+ * Main framebuffer functions
+ */
+
+/*!
+ * Allocates the DRAM memory for the frame buffer.      This buffer is remapped
+ * into a non-cached, non-buffered, memory region to allow palette and pixel
+ * writes to occur without flushing the cache.  Once this area is remapped,
+ * all virtual memory access to the video memory should occur at the new region.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @param       use_internal_ram flag on whether to use internal RAM for memory
+ *
+ * @return      Error code indicating success or failure
+ */
+static int mxcfb_map_video_memory(struct fb_info *fbi, bool use_internal_ram)
+{
+	int retval = 0;
+
+#ifdef CONFIG_FB_MXC_INTERNAL_MEM
+	if (use_internal_ram) {
+		fbi->fix.smem_len = FB_RAM_SIZE;
+		fbi->fix.smem_start = FB_RAM_BASE_ADDR;
+		if (fbi->fix.smem_len <
+		    (fbi->var.yres_virtual * fbi->fix.line_length)) {
+			dev_err(fbi->device,
+				"Not enough internal RAM for framebuffer configuration\n");
+			retval = -EINVAL;
+			goto err0;
+		}
+
+		if (request_mem_region(fbi->fix.smem_start, fbi->fix.smem_len,
+				       fbi->device->driver->name) == NULL) {
+			dev_err(fbi->device,
+				"Unable to request internal RAM\n");
+			retval = -ENOMEM;
+			goto err0;
+		}
+
+		if (!(fbi->screen_base = ioremap(fbi->fix.smem_start,
+						 fbi->fix.smem_len))) {
+			dev_err(fbi->device,
+				"Unable to map fb memory to virtual address\n");
+			release_mem_region(fbi->fix.smem_start,
+					   fbi->fix.smem_len);
+			retval = -EIO;
+			goto err0;
+		}
+
+		iram_clk = clk_get(NULL, "iram_clk");
+		clk_enable(iram_clk);
+	} else
+#endif
+	{
+		fbi->fix.smem_len = fbi->var.yres_virtual *
+		    fbi->fix.line_length;
+		fbi->screen_base =
+		    dma_alloc_writecombine(fbi->device,
+					   fbi->fix.smem_len,
+					   (dma_addr_t *) & fbi->fix.smem_start,
+					   GFP_DMA);
+
+		if (fbi->screen_base == 0) {
+			dev_err(fbi->device,
+				"Unable to allocate framebuffer memory\n");
+			retval = -EBUSY;
+			goto err0;
+		}
+	}
+
+	dev_dbg(fbi->device, "allocated fb @ paddr=0x%08X, size=%d.\n",
+		(uint32_t) fbi->fix.smem_start, fbi->fix.smem_len);
+
+	fbi->screen_size = fbi->fix.smem_len;
+
+	/* Clear the screen */
+	memset((char *)fbi->screen_base, 0, fbi->fix.smem_len);
+
+	return 0;
+
+      err0:
+	fbi->fix.smem_len = 0;
+	fbi->fix.smem_start = 0;
+	fbi->screen_base = NULL;
+	return retval;
+}
+
+/*!
+ * De-allocates the DRAM memory for the frame buffer.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @return      Error code indicating success or failure
+ */
+static int mxcfb_unmap_video_memory(struct fb_info *fbi)
+{
+#ifdef CONFIG_FB_MXC_INTERNAL_MEM
+	if (fbi->fix.smem_start == FB_RAM_BASE_ADDR) {
+		iounmap(fbi->screen_base);
+		release_mem_region(fbi->fix.smem_start, fbi->fix.smem_len);
+		fbi->fix.smem_start = 0;
+		fbi->fix.smem_len = 0;
+		clk_disable(iram_clk);
+	} else
+#endif
+	{
+		dma_free_writecombine(fbi->device, fbi->fix.smem_len,
+				      fbi->screen_base, fbi->fix.smem_start);
+	}
+	fbi->screen_base = 0;
+	fbi->fix.smem_start = 0;
+	fbi->fix.smem_len = 0;
+	return 0;
+}
+
+/*!
+ * Initializes the framebuffer information pointer. After allocating
+ * sufficient memory for the framebuffer structure, the fields are
+ * filled with custom information passed in from the configurable
+ * structures.  This includes information such as bits per pixel,
+ * color maps, screen width/height and RGBA offsets.
+ *
+ * @return      Framebuffer structure initialized with our information
+ */
+static struct fb_info *mxcfb_init_fbinfo(struct device *dev, struct fb_ops *ops)
+{
+	struct fb_info *fbi;
+	struct mxcfb_info *mxcfbi;
+
+	/*
+	 * Allocate sufficient memory for the fb structure
+	 */
+	fbi = framebuffer_alloc(sizeof(struct mxcfb_info), dev);
+	if (!fbi)
+		return NULL;
+
+	mxcfbi = (struct mxcfb_info *)fbi->par;
+
+	fbi->var.activate = FB_ACTIVATE_NOW;
+
+	fbi->fbops = ops;
+	fbi->flags = FBINFO_FLAG_DEFAULT;
+	fbi->pseudo_palette = mxcfbi->pseudo_palette;
+
+	spin_lock_init(&mxcfbi->fb_lock);
+
+	/*
+	 * Allocate colormap
+	 */
+	fb_alloc_cmap(&fbi->cmap, 16, 0);
+
+	return fbi;
+}
+
+/*!
+ * Probe routine for the framebuffer driver. It is called during the
+ * driver binding process.      The following functions are performed in
+ * this routine: Framebuffer initialization, Memory allocation and
+ * mapping, Framebuffer registration, IPU initialization.
+ *
+ * @return      Appropriate error code to the kernel common code
+ */
+static int mxcfb_probe(struct platform_device *pdev)
+{
+	char *mode = pdev->dev.platform_data;
+	struct fb_info *fbi;
+	struct mxcfb_info *mxcfbi;
+	struct fb_info *fbi_ovl;
+	int ret = 0;
+
+	/*
+	 * Initialize FB structures
+	 */
+	fbi = mxcfb_init_fbinfo(&pdev->dev, &mxcfb_ops);
+	if (!fbi) {
+		ret = -ENOMEM;
+		goto err0;
+	}
+	mxcfbi = (struct mxcfb_info *)fbi->par;
+
+	mxcfbi->ipu_ch_irq = IPU_IRQ_SDC_BG_EOF;
+	mxcfbi->cur_ipu_buf = 0;
+	mxcfbi->ipu_ch = MEM_SDC_BG;
+
+	ipu_sdc_set_global_alpha(true, 0xFF);
+	ipu_sdc_set_color_key(MEM_SDC_BG, false, 0);
+
+	if (ipu_request_irq(IPU_IRQ_SDC_BG_EOF, mxcfb_irq_handler, 0,
+			    MXCFB_NAME, fbi) != 0) {
+		dev_err(&pdev->dev, "Error registering BG irq handler.\n");
+		ret = -EBUSY;
+		goto err1;
+	}
+	ipu_disable_irq(IPU_IRQ_SDC_BG_EOF);
+
+	if (fb_mode == NULL) {
+		fb_mode = mode;
+	}
+
+	if (!fb_find_mode(&fbi->var, fbi, fb_mode, mxcfb_modedb,
+			  mxcfb_modedb_sz, NULL, default_bpp)) {
+		ret = -EBUSY;
+		goto err2;
+	}
+	fb_videomode_to_modelist(mxcfb_modedb, mxcfb_modedb_sz, &fbi->modelist);
+
+	/* Default Y virtual size is 2x panel size */
+#ifndef CONFIG_FB_MXC_INTERNAL_MEM
+	fbi->var.yres_virtual = fbi->var.yres * 2;
+#endif
+
+	mxcfb_drv_data.fbi = fbi;
+	mxcfb_drv_data.backlight_level = 255;
+	mxcfb_drv_data.suspended = false;
+	init_waitqueue_head(&mxcfb_drv_data.suspend_wq);
+
+	mxcfbi->blank = FB_BLANK_NORMAL;
+	ret = mxcfb_set_par(fbi);
+	if (ret < 0) {
+		goto err2;
+	}
+	mxcfb_blank(FB_BLANK_UNBLANK, fbi);
+
+	/*
+	 * Register framebuffer
+	 */
+	ret = register_framebuffer(fbi);
+	if (ret < 0) {
+		goto err2;
+	}
+
+	/*
+	 * Initialize Overlay FB structures
+	 */
+	fbi_ovl = mxcfb_init_fbinfo(&pdev->dev, &mxcfb_ovl_ops);
+	if (!fbi_ovl) {
+		ret = -ENOMEM;
+		goto err3;
+	}
+	mxcfb_drv_data.fbi_ovl = fbi_ovl;
+	mxcfbi = (struct mxcfb_info *)fbi_ovl->par;
+
+	mxcfbi->ipu_ch_irq = IPU_IRQ_SDC_FG_EOF;
+	mxcfbi->cur_ipu_buf = 0;
+	mxcfbi->ipu_ch = MEM_SDC_FG;
+
+	if (ipu_request_irq(IPU_IRQ_SDC_FG_EOF, mxcfb_irq_handler, 0,
+			    MXCFB_NAME, fbi_ovl) != 0) {
+		dev_err(fbi->device, "Error registering FG irq handler.\n");
+		ret = -EBUSY;
+		goto err4;
+	}
+	ipu_disable_irq(mxcfbi->ipu_ch_irq);
+
+	/* Default Y virtual size is 2x panel size */
+	fbi_ovl->var = fbi->var;
+	fbi_ovl->var.yres_virtual = fbi->var.yres * 2;
+
+	/* Overlay is blanked by default */
+	mxcfbi->blank = FB_BLANK_NORMAL;
+
+	ret = mxcfb_set_par(fbi_ovl);
+	if (ret < 0) {
+		goto err5;
+	}
+
+	/*
+	 * Register overlay framebuffer
+	 */
+	ret = register_framebuffer(fbi_ovl);
+	if (ret < 0) {
+		goto err5;
+	}
+
+	platform_set_drvdata(pdev, &mxcfb_drv_data);
+
+	init_waitqueue_head(&mxcfb_drv_data.vsync_wq);
+	if (!cpu_is_mx31() && !cpu_is_mx32()) {
+		if ((ret = ipu_request_irq(IPU_IRQ_SDC_DISP3_VSYNC,
+					   mxcfb_vsync_irq_handler,
+					   0, MXCFB_NAME,
+					   &mxcfb_drv_data)) < 0) {
+			goto err6;
+		}
+		ipu_disable_irq(IPU_IRQ_SDC_DISP3_VSYNC);
+	}
+
+	printk(KERN_INFO "mxcfb: fb registered, using mode %s\n", fb_mode);
+	return 0;
+
+      err6:
+	unregister_framebuffer(fbi_ovl);
+      err5:
+	ipu_free_irq(IPU_IRQ_SDC_FG_EOF, fbi_ovl);
+      err4:
+	fb_dealloc_cmap(&fbi_ovl->cmap);
+	framebuffer_release(fbi_ovl);
+      err3:
+	unregister_framebuffer(fbi);
+      err2:
+	ipu_free_irq(IPU_IRQ_SDC_BG_EOF, fbi);
+      err1:
+	fb_dealloc_cmap(&fbi->cmap);
+	framebuffer_release(fbi);
+      err0:
+	printk(KERN_ERR "mxcfb: failed to register fb\n");
+	return ret;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcfb_driver = {
+	.driver = {
+		   .name = MXCFB_NAME,
+		   },
+	.probe = mxcfb_probe,
+	.suspend = mxcfb_suspend,
+	.resume = mxcfb_resume,
+};
+
+/*
+ * Parse user specified options (`video=trident:')
+ * example:
+ * 	video=trident:800x600,bpp=16,noaccel
+ */
+int mxcfb_setup(char *options)
+{
+	char *opt;
+	if (!options || !*options)
+		return 0;
+	while ((opt = strsep(&options, ",")) != NULL) {
+		if (!*opt)
+			continue;
+		if (!strncmp(opt, "bpp=", 4))
+			default_bpp = simple_strtoul(opt + 4, NULL, 0);
+		else
+			fb_mode = opt;
+	}
+	return 0;
+}
+
+/*!
+ * Main entry function for the framebuffer. The function registers the power
+ * management callback functions with the kernel and also registers the MXCFB
+ * callback functions with the core Linux framebuffer driver \b fbmem.c
+ *
+ * @return      Error code indicating success or failure
+ */
+int __init mxcfb_init(void)
+{
+	int ret = 0;
+#ifndef MODULE
+	char *option = NULL;
+#endif
+
+#ifndef MODULE
+	if (fb_get_options("mxcfb", &option))
+		return -ENODEV;
+	mxcfb_setup(option);
+#endif
+
+	ret = platform_driver_register(&mxcfb_driver);
+	return ret;
+}
+
+void mxcfb_exit(void)
+{
+	struct fb_info *fbi = mxcfb_drv_data.fbi;
+
+	if (fbi) {
+		mxcfb_unmap_video_memory(fbi);
+
+		if (&fbi->cmap)
+			fb_dealloc_cmap(&fbi->cmap);
+
+		unregister_framebuffer(fbi);
+		framebuffer_release(fbi);
+	}
+
+	fbi = mxcfb_drv_data.fbi_ovl;
+	if (fbi) {
+		mxcfb_unmap_video_memory(fbi);
+
+		if (&fbi->cmap)
+			fb_dealloc_cmap(&fbi->cmap);
+
+		unregister_framebuffer(fbi);
+		framebuffer_release(fbi);
+	}
+#ifndef CONFIG_ARCH_MX3
+	ipu_free_irq(IPU_IRQ_SDC_DISP3_VSYNC, &mxcfb_drv_data);
+#endif
+
+	platform_driver_unregister(&mxcfb_driver);
+}
+
+module_init(mxcfb_init);
+module_exit(mxcfb_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC framebuffer driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("fb");
diff --git a/drivers/video/mxc/mxcfb_ch7026.c b/drivers/video/mxc/mxcfb_ch7026.c
new file mode 100644
index 000000000000..c781e1c26086
--- /dev/null
+++ b/drivers/video/mxc/mxcfb_ch7026.c
@@ -0,0 +1,369 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup Framebuffer Framebuffer Driver for SDC and ADC.
+ */
+
+/*!
+ * @file mxcfb_epson_vga.c
+ *
+ * @brief MXC Frame buffer driver for SDC
+ *
+ * @ingroup Framebuffer
+ */
+
+/*!
+ * Include files
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/i2c.h>
+#include <linux/mxcfb.h>
+#include <linux/ipu.h>
+
+static struct i2c_client *ch7026_client;
+
+static int lcd_init(void);
+static void lcd_poweron(struct fb_info *info);
+static void lcd_poweroff(void);
+
+static void (*lcd_reset) (void);
+static struct regulator *io_reg;
+static struct regulator *core_reg;
+static struct regulator *analog_reg;
+
+	/* 8 800x600-60 VESA */
+static struct fb_videomode mode = {
+	NULL, 60, 800, 600, 25000, 88, 40, 23, 1, 128, 4,
+	FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA
+};
+
+static void lcd_init_fb(struct fb_info *info)
+{
+	struct fb_var_screeninfo var;
+
+	memset(&var, 0, sizeof(var));
+
+	fb_videomode_to_var(&var, &mode);
+
+	var.activate = FB_ACTIVATE_ALL;
+
+	acquire_console_sem();
+	info->flags |= FBINFO_MISC_USEREVENT;
+	fb_set_var(info, &var);
+	fb_blank(info, FB_BLANK_UNBLANK);
+	info->flags &= ~FBINFO_MISC_USEREVENT;
+	release_console_sem();
+}
+
+static int lcd_fb_event(struct notifier_block *nb, unsigned long val, void *v)
+{
+	struct fb_event *event = v;
+
+	if (strcmp(event->info->fix.id, "DISP3 BG - DI1"))
+		return 0;
+
+	switch (val) {
+	case FB_EVENT_FB_REGISTERED:
+		lcd_init_fb(event->info);
+		lcd_poweron(event->info);
+		break;
+	case FB_EVENT_BLANK:
+		if (*((int *)event->data) == FB_BLANK_UNBLANK)
+			lcd_poweron(event->info);
+		else
+			lcd_poweroff();
+		break;
+	}
+	return 0;
+}
+
+static struct notifier_block nb = {
+	.notifier_call = lcd_fb_event,
+};
+
+/*!
+ * This function is called whenever the SPI slave device is detected.
+ *
+ * @param	spi	the SPI slave device
+ *
+ * @return 	Returns 0 on SUCCESS and error on FAILURE.
+ */
+static int __devinit lcd_probe(struct device *dev)
+{
+	int ret = 0;
+	int i;
+	struct mxc_lcd_platform_data *plat = dev->platform_data;
+
+	if (plat) {
+
+		io_reg = regulator_get(dev, plat->io_reg);
+		if (!IS_ERR(io_reg)) {
+			regulator_set_voltage(io_reg, 1800000, 1800000);
+			regulator_enable(io_reg);
+		} else {
+			io_reg = NULL;
+		}
+
+		core_reg = regulator_get(dev, plat->core_reg);
+		if (!IS_ERR(core_reg)) {
+			regulator_set_voltage(core_reg, 2500000, 2500000);
+			regulator_enable(core_reg);
+		} else {
+			core_reg = NULL;
+		}
+		analog_reg = regulator_get(dev, plat->analog_reg);
+		if (!IS_ERR(analog_reg)) {
+			regulator_set_voltage(analog_reg, 2775000, 2775000);
+			regulator_enable(analog_reg);
+		} else {
+			analog_reg = NULL;
+		}
+		msleep(100);
+
+		lcd_reset = plat->reset;
+		if (lcd_reset)
+			lcd_reset();
+	}
+
+	for (i = 0; i < num_registered_fb; i++) {
+		if (strcmp(registered_fb[i]->fix.id, "DISP3 BG - DI1") == 0) {
+			ret = lcd_init();
+			if (ret < 0)
+				goto err;
+
+			lcd_init_fb(registered_fb[i]);
+			fb_show_logo(registered_fb[i], 0);
+			lcd_poweron(registered_fb[i]);
+		}
+	}
+
+	fb_register_client(&nb);
+	return 0;
+err:
+	if (io_reg)
+		regulator_disable(io_reg);
+	if (core_reg)
+		regulator_disable(core_reg);
+	if (analog_reg)
+		regulator_disable(analog_reg);
+
+	return ret;
+}
+
+static int __devinit ch7026_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	int id_reg;
+	ch7026_client = client;
+
+	return lcd_probe(&client->dev);
+}
+
+static int __devexit ch7026_remove(struct i2c_client *client)
+{
+	fb_unregister_client(&nb);
+	lcd_poweroff();
+	regulator_put(io_reg);
+	regulator_put(core_reg);
+	regulator_put(analog_reg);
+
+	return 0;
+}
+
+static int ch7026_suspend(struct i2c_client *client, pm_message_t message)
+{
+	return 0;
+}
+
+static int ch7026_resume(struct i2c_client *client)
+{
+	return 0;
+}
+
+u8 reg_init[][2] = {
+	{ 0x02, 0x01 },
+	{ 0x02, 0x03 },
+	{ 0x03, 0x00 },
+	{ 0x06, 0x6B },
+	{ 0x08, 0x08 },
+	{ 0x09, 0x80 },
+	{ 0x0C, 0x0A },
+	{ 0x0D, 0x89 },
+	{ 0x0F, 0x23 },
+	{ 0x10, 0x20 },
+	{ 0x11, 0x20 },
+	{ 0x12, 0x40 },
+	{ 0x13, 0x28 },
+	{ 0x14, 0x80 },
+	{ 0x15, 0x52 },
+	{ 0x16, 0x58 },
+	{ 0x17, 0x74 },
+	{ 0x19, 0x01 },
+	{ 0x1A, 0x04 },
+	{ 0x1B, 0x23 },
+	{ 0x1C, 0x20 },
+	{ 0x1D, 0x20 },
+	{ 0x1F, 0x28 },
+	{ 0x20, 0x80 },
+	{ 0x21, 0x12 },
+	{ 0x22, 0x58 },
+	{ 0x23, 0x74 },
+	{ 0x25, 0x01 },
+	{ 0x26, 0x04 },
+	{ 0x37, 0x20 },
+	{ 0x39, 0x20 },
+	{ 0x3B, 0x20 },
+	{ 0x41, 0xA2 },
+	{ 0x4D, 0x03 },
+	{ 0x4E, 0x13 },
+	{ 0x4F, 0xB1 },
+	{ 0x50, 0x3B },
+	{ 0x51, 0x54 },
+	{ 0x52, 0x12 },
+	{ 0x53, 0x13 },
+	{ 0x55, 0xE5 },
+	{ 0x5E, 0x80 },
+	{ 0x69, 0x64 },
+	{ 0x7D, 0x62 },
+	{ 0x04, 0x00 },
+	{ 0x06, 0x69 },
+
+	/*
+	NOTE: The following five repeated sentences are used here to wait memory initial complete, please don't remove...(you could refer to Appendix A of programming guide document (CH7025(26)B Programming Guide Rev2.03.pdf) for detailed information about memory initialization!
+	*/
+	{ 0x03, 0x00 },
+	{ 0x03, 0x00 },
+	{ 0x03, 0x00 },
+	{ 0x03, 0x00 },
+	{ 0x03, 0x00 },
+
+	{ 0x06, 0x68 },
+	{ 0x02, 0x02 },
+	{ 0x02, 0x03 },
+};
+
+#define REGMAP_LENGTH (sizeof(reg_init) / (2*sizeof(u8)))
+
+/*
+ * Send init commands to L4F00242T03
+ *
+ */
+static int lcd_init(void)
+{
+	int i;
+	int dat;
+
+	dev_dbg(&ch7026_client->dev, "initializing CH7026\n");
+
+	/* read device ID */
+	msleep(100);
+	dat = i2c_smbus_read_byte_data(ch7026_client, 0x00);
+	dev_dbg(&ch7026_client->dev, "read id = 0x%02X\n", dat);
+	if (dat != 0x54)
+		return -ENODEV;
+
+	for (i = 0; i < REGMAP_LENGTH; ++i) {
+		if (i2c_smbus_write_byte_data
+		    (ch7026_client, reg_init[i][0], reg_init[i][1]) < 0)
+			return -EIO;
+	}
+
+	return 0;
+}
+
+static int lcd_on;
+/*
+ * Send Power On commands to L4F00242T03
+ *
+ */
+static void lcd_poweron(struct fb_info *info)
+{
+	u16 data[4];
+	u32 refresh;
+
+	if (lcd_on)
+		return;
+
+	dev_dbg(&ch7026_client->dev, "turning on LCD\n");
+
+	data[0] = PICOS2KHZ(info->var.pixclock) / 10;
+	data[2] = info->var.hsync_len + info->var.left_margin +
+	    info->var.xres + info->var.right_margin;
+	data[3] = info->var.vsync_len + info->var.upper_margin +
+	    info->var.yres + info->var.lower_margin;
+
+	refresh = data[2] * data[3];
+	refresh = (PICOS2KHZ(info->var.pixclock) * 1000) / refresh;
+	data[1] = refresh * 100;
+
+	lcd_on = 1;
+}
+
+/*
+ * Send Power Off commands to L4F00242T03
+ *
+ */
+static void lcd_poweroff(void)
+{
+	if (!lcd_on)
+		return;
+
+	dev_dbg(&ch7026_client->dev, "turning off LCD\n");
+
+	lcd_on = 0;
+}
+
+static const struct i2c_device_id ch7026_id[] = {
+	{"ch7026", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ch7026_id);
+
+static struct i2c_driver ch7026_driver = {
+	.driver = {
+		   .name = "ch7026",
+		   },
+	.probe = ch7026_probe,
+	.remove = ch7026_remove,
+	.suspend = ch7026_suspend,
+	.resume = ch7026_resume,
+	.id_table = ch7026_id,
+};
+
+static int __init ch7026_init(void)
+{
+	return i2c_add_driver(&ch7026_driver);
+}
+
+static void __exit ch7026_exit(void)
+{
+	i2c_del_driver(&ch7026_driver);
+}
+
+module_init(ch7026_init);
+module_exit(ch7026_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("CH7026 VGA driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/mxc/mxcfb_claa_wvga.c b/drivers/video/mxc/mxcfb_claa_wvga.c
new file mode 100644
index 000000000000..2e032317ab7d
--- /dev/null
+++ b/drivers/video/mxc/mxcfb_claa_wvga.c
@@ -0,0 +1,236 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup Framebuffer Framebuffer Driver for SDC and ADC.
+ */
+
+/*!
+ * @file mxcfb_claa_wvga.c
+ *
+ * @brief MXC Frame buffer driver for SDC
+ *
+ * @ingroup Framebuffer
+ */
+
+/*!
+ * Include files
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/mxcfb.h>
+#include <linux/regulator/consumer.h>
+
+static void lcd_poweron(void);
+static void lcd_poweroff(void);
+
+static struct platform_device *plcd_dev;
+static struct regulator *io_reg;
+static struct regulator *core_reg;
+static int lcd_on;
+
+static struct fb_videomode video_modes[] = {
+	{
+	 /* 800x480 @ 55 Hz , pixel clk @ 25MHz */
+	 "CLAA-WVGA", 55, 800, 480, 40000, 40, 40, 5, 5, 20, 10,
+	 FB_SYNC_CLK_LAT_FALL,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+};
+
+static void lcd_init_fb(struct fb_info *info)
+{
+	struct fb_var_screeninfo var;
+
+	memset(&var, 0, sizeof(var));
+
+	fb_videomode_to_var(&var, &video_modes[0]);
+
+	var.activate = FB_ACTIVATE_ALL;
+	var.yres_virtual = var.yres * 2;
+
+	acquire_console_sem();
+	info->flags |= FBINFO_MISC_USEREVENT;
+	fb_set_var(info, &var);
+	info->flags &= ~FBINFO_MISC_USEREVENT;
+	release_console_sem();
+}
+
+static int lcd_fb_event(struct notifier_block *nb, unsigned long val, void *v)
+{
+	struct fb_event *event = v;
+
+	if (strcmp(event->info->fix.id, "DISP3 BG")) {
+		return 0;
+	}
+
+	switch (val) {
+	case FB_EVENT_FB_REGISTERED:
+		lcd_init_fb(event->info);
+		lcd_poweron();
+		break;
+	case FB_EVENT_BLANK:
+		if ((event->info->var.xres != 800) ||
+		    (event->info->var.yres != 480)) {
+			break;
+		}
+		if (*((int *)event->data) == FB_BLANK_UNBLANK) {
+			lcd_poweron();
+		} else {
+			lcd_poweroff();
+		}
+		break;
+	}
+	return 0;
+}
+
+static struct notifier_block nb = {
+	.notifier_call = lcd_fb_event,
+};
+
+/*!
+ * This function is called whenever the SPI slave device is detected.
+ *
+ * @param	spi	the SPI slave device
+ *
+ * @return 	Returns 0 on SUCCESS and error on FAILURE.
+ */
+static int __devinit lcd_probe(struct platform_device *pdev)
+{
+	int i;
+	struct mxc_lcd_platform_data *plat = pdev->dev.platform_data;
+
+	if (plat) {
+		if (plat->reset)
+			plat->reset();
+
+		io_reg = regulator_get(&pdev->dev, plat->io_reg);
+		if (IS_ERR(io_reg))
+			io_reg = NULL;
+		core_reg = regulator_get(&pdev->dev, plat->core_reg);
+		if (!IS_ERR(core_reg)) {
+			regulator_set_voltage(io_reg, 1800000, 1800000);
+		} else {
+			core_reg = NULL;
+		}
+	}
+
+	for (i = 0; i < num_registered_fb; i++) {
+		if (strcmp(registered_fb[i]->fix.id, "DISP3 BG") == 0) {
+			lcd_init_fb(registered_fb[i]);
+			fb_show_logo(registered_fb[i], 0);
+			lcd_poweron();
+		} else if (strcmp(registered_fb[i]->fix.id, "DISP3 FG") == 0) {
+			lcd_init_fb(registered_fb[i]);
+		}
+	}
+
+	fb_register_client(&nb);
+
+	plcd_dev = pdev;
+
+	return 0;
+}
+
+static int __devexit lcd_remove(struct platform_device *pdev)
+{
+	fb_unregister_client(&nb);
+	lcd_poweroff();
+	if (io_reg)
+		regulator_put(io_reg);
+	if (core_reg)
+		regulator_put(core_reg);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int lcd_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	return 0;
+}
+
+static int lcd_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+#else
+#define lcd_suspend NULL
+#define lcd_resume NULL
+#endif
+
+/*!
+ * platform driver structure for CLAA WVGA
+ */
+static struct platform_driver lcd_driver = {
+	.driver = {
+		   .name = "lcd_claa"},
+	.probe = lcd_probe,
+	.remove = __devexit_p(lcd_remove),
+	.suspend = lcd_suspend,
+	.resume = lcd_resume,
+};
+
+/*
+ * Send Power On commands to L4F00242T03
+ *
+ */
+static void lcd_poweron(void)
+{
+	if (lcd_on)
+		return;
+
+	dev_dbg(&plcd_dev->dev, "turning on LCD\n");
+	if (core_reg)
+		regulator_enable(core_reg);
+	if (io_reg)
+		regulator_enable(io_reg);
+	lcd_on = 1;
+}
+
+/*
+ * Send Power Off commands to L4F00242T03
+ *
+ */
+static void lcd_poweroff(void)
+{
+	lcd_on = 0;
+	dev_dbg(&plcd_dev->dev, "turning off LCD\n");
+	if (io_reg)
+		regulator_disable(io_reg);
+	if (core_reg)
+		regulator_disable(core_reg);
+}
+
+static int __init claa_lcd_init(void)
+{
+	return platform_driver_register(&lcd_driver);
+}
+
+static void __exit claa_lcd_exit(void)
+{
+	platform_driver_unregister(&lcd_driver);
+}
+
+module_init(claa_lcd_init);
+module_exit(claa_lcd_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("CLAA WVGA LCD init driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/mxc/mxcfb_epson.c b/drivers/video/mxc/mxcfb_epson.c
new file mode 100644
index 000000000000..25b05e4b1a0e
--- /dev/null
+++ b/drivers/video/mxc/mxcfb_epson.c
@@ -0,0 +1,1158 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mxcfb_epson.c
+ *
+ * @brief MXC Frame buffer driver for ADC
+ *
+ * @ingroup Framebuffer
+ */
+
+/*!
+ * Include files
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <mach/hardware.h>
+#include <asm/io.h>
+#include <asm/mach-types.h>
+#include <asm/uaccess.h>
+#include <mach/ipu.h>
+#include <mach/mxcfb.h>
+
+#define PARTIAL_REFRESH
+#define MXCFB_REFRESH_DEFAULT MXCFB_REFRESH_PARTIAL
+/*
+ * Driver name
+ */
+#define MXCFB_NAME      "MXCFB_EPSON"
+
+#define MXCFB_SCREEN_TOP_OFFSET         0
+#define MXCFB_SCREEN_LEFT_OFFSET        2
+#define MXCFB_SCREEN_WIDTH              176
+#define MXCFB_SCREEN_HEIGHT             220
+
+/*!
+ * Enum defining Epson panel commands.
+ */
+enum {
+	DISON = 0xAF,
+	DISOFF = 0xAE,
+	DISCTL = 0xCA,
+	SD_CSET = 0x15,
+	SD_PSET = 0x75,
+	DATCTL = 0xBC,
+	SLPIN = 0x95,
+	SLPOUT = 0x94,
+	DISNOR = 0xA6,
+	RAMWR = 0x5C,
+	VOLCTR = 0xC6,
+	GCP16 = 0xCC,
+	GCP64 = 0xCB,
+};
+
+struct mxcfb_info {
+	int open_count;
+	int blank;
+	uint32_t disp_num;
+
+	u32 pseudo_palette[16];
+
+	int32_t cur_update_mode;
+	dma_addr_t alloc_start_paddr;
+	void *alloc_start_vaddr;
+	u32 alloc_size;
+	uint32_t snoop_window_size;
+};
+
+struct mxcfb_data {
+	struct fb_info *fbi;
+	volatile int32_t vsync_flag;
+	wait_queue_head_t vsync_wq;
+	wait_queue_head_t suspend_wq;
+	bool suspended;
+};
+
+static struct mxcfb_data mxcfb_drv_data;
+static unsigned long default_bpp = 16;
+
+void slcd_gpio_config(void);
+extern void gpio_lcd_active(void);
+static int mxcfb_blank(int blank, struct fb_info *fbi);
+
+static uint32_t bpp_to_pixfmt(int bpp)
+{
+	uint32_t pixfmt = 0;
+	switch (bpp) {
+	case 24:
+		pixfmt = IPU_PIX_FMT_BGR24;
+		break;
+	case 32:
+		pixfmt = IPU_PIX_FMT_BGR32;
+		break;
+	case 16:
+		pixfmt = IPU_PIX_FMT_RGB565;
+		break;
+	}
+	return pixfmt;
+}
+
+/*!
+ * This function sets display region in the Epson panel
+ *
+ * @param        disp    display panel to config
+ * @param	 x1	 x-coordinate of one vertex.
+ * @param	 x2	 x-coordinate of second vertex.
+ * @param	 y1	 y-coordinate of one vertex.
+ * @param	 y2	 y-coordinate of second vertex.
+ */
+void set_panel_region(int disp, uint32_t x1, uint32_t x2,
+		      uint32_t y1, uint32_t y2)
+{
+	uint32_t param[8];
+
+	memset(param, 0, sizeof(uint32_t) * 8);
+	param[0] = x1;
+	param[2] = x2;
+	param[4] = y1;
+	param[6] = y2;
+
+	// SD_CSET
+	ipu_adc_write_cmd(disp, CMD, SD_CSET, param, 4);
+	// SD_PSET
+
+	ipu_adc_write_cmd(disp, CMD, SD_PSET, &(param[4]), 4);
+}
+
+/*!
+ * Function to create and initiate template command buffer for ADC. This
+ * template will be written to Panel memory.
+ */
+static void init_channel_template(int disp)
+{
+	/* template command buffer for ADC is 32 */
+	uint32_t tempCmd[TEMPLATE_BUF_SIZE];
+	uint32_t i = 0;
+
+	memset(tempCmd, 0, sizeof(uint32_t) * TEMPLATE_BUF_SIZE);
+	/* setup update display region */
+	/* whole the screen during init */
+	/*WRITE Y COORDINATE CMND */
+	tempCmd[i++] = ipu_adc_template_gen(WR_CMND, 0, SINGLE_STEP, SD_PSET);
+	/*WRITE Y START ADDRESS CMND LSB[22:8] */
+	tempCmd[i++] = ipu_adc_template_gen(WR_YADDR, 1, SINGLE_STEP, 0x01);
+	/*WRITE Y START ADDRESS CMND MSB[22:16] */
+	tempCmd[i++] = ipu_adc_template_gen(WR_YADDR, 1, SINGLE_STEP, 0x09);
+	/*WRITE Y STOP ADDRESS CMND LSB */
+	tempCmd[i++] = ipu_adc_template_gen(WR_CMND, 1, SINGLE_STEP,
+					    MXCFB_SCREEN_HEIGHT - 1);
+	/*WRITE Y STOP ADDRESS CMND MSB */
+	tempCmd[i++] = ipu_adc_template_gen(WR_CMND, 1, SINGLE_STEP, 0);
+	/*WRITE X COORDINATE CMND */
+	tempCmd[i++] = ipu_adc_template_gen(WR_CMND, 0, SINGLE_STEP, SD_CSET);
+	/*WRITE X ADDRESS CMND LSB[7:0] */
+	tempCmd[i++] = ipu_adc_template_gen(WR_XADDR, 1, SINGLE_STEP, 0x01);
+	/*WRITE X ADDRESS CMND MSB[22:8] */
+	tempCmd[i++] = ipu_adc_template_gen(WR_CMND, 1, SINGLE_STEP, 0);
+	/*WRITE X STOP ADDRESS CMND LSB */
+	tempCmd[i++] = ipu_adc_template_gen(WR_CMND, 1, SINGLE_STEP,
+					    MXCFB_SCREEN_WIDTH + 1);
+	/*WRITE X STOP ADDRESS CMND MSB */
+	tempCmd[i++] = ipu_adc_template_gen(WR_CMND, 1, SINGLE_STEP, 0);
+	/*WRITE MEMORY CMND MSB */
+	tempCmd[i++] = ipu_adc_template_gen(WR_CMND, 0, SINGLE_STEP, RAMWR);
+	/*WRITE DATA CMND and STP */
+	tempCmd[i++] = ipu_adc_template_gen(WR_DATA, 1, STOP, 0);
+
+	ipu_adc_write_template(disp, tempCmd, true);
+}
+
+/*!
+ * Function to initialize the panel. First it resets the panel and then
+ * initilizes panel.
+ */
+static void _init_panel(int disp)
+{
+	uint32_t cmd_param;
+	uint32_t i;
+
+	gpio_lcd_active();
+	slcd_gpio_config();
+
+	// DATCTL
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL_IFC_16_BIT
+	// 16-bit 565 mode
+	cmd_param = 0x28;
+#else
+	// 8-bit 666 mode
+	cmd_param = 0x08;
+#endif
+	ipu_adc_write_cmd(disp, CMD, DATCTL, &cmd_param, 1);
+
+	// Sleep OUT
+	ipu_adc_write_cmd(disp, CMD, SLPOUT, 0, 0);
+
+	// Set display to white
+	// Setup page and column addresses
+	set_panel_region(disp, MXCFB_SCREEN_LEFT_OFFSET,
+			 MXCFB_SCREEN_WIDTH + MXCFB_SCREEN_LEFT_OFFSET - 1,
+			 0, MXCFB_SCREEN_HEIGHT - 1);
+	// Do RAM write cmd
+	ipu_adc_write_cmd(disp, CMD, RAMWR, 0, 0);
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL_IFC_16_BIT
+	for (i = 0; i < (MXCFB_SCREEN_WIDTH * MXCFB_SCREEN_HEIGHT); i++)
+#else
+	for (i = 0; i < (MXCFB_SCREEN_WIDTH * MXCFB_SCREEN_HEIGHT * 3); i++)
+#endif
+		ipu_adc_write_cmd(disp, DAT, 0xFFFF, 0, 0);
+
+	// Pause 80 ms
+	mdelay(80);
+
+	// Display ON
+	ipu_adc_write_cmd(disp, CMD, DISON, 0, 0);
+	// Pause 200 ms
+	mdelay(200);
+
+	pr_debug("initialized panel\n");
+}
+
+#ifdef PARTIAL_REFRESH
+static irqreturn_t mxcfb_sys2_eof_irq_handler(int irq, void *dev_id)
+{
+	ipu_channel_params_t params;
+	struct fb_info *fbi = dev_id;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+	uint32_t stat[2], seg_size;
+	uint32_t lsb, msb;
+	uint32_t update_height, start_line, start_addr, end_line, end_addr;
+	uint32_t stride_pixels = (fbi->fix.line_length * 8) /
+	    fbi->var.bits_per_pixel;
+
+	ipu_adc_get_snooping_status(&stat[0], &stat[1]);
+	//DPRINTK("snoop status = 0x%08X%08X\n", stat[1], stat[0]);
+
+	if (!stat[0] && !stat[1]) {
+		dev_err(fbi->device, "error no bus snooping bits set\n");
+		return IRQ_HANDLED;
+	}
+	ipu_disable_irq(IPU_IRQ_ADC_SYS2_EOF);
+
+	lsb = ffs(stat[0]);
+	if (lsb) {
+		lsb--;
+	} else {
+		lsb = ffs(stat[1]);
+		lsb += 32 - 1;
+	}
+	msb = fls(stat[1]);
+	if (msb) {
+		msb += 32;
+	} else {
+		msb = fls(stat[0]);
+	}
+
+	seg_size = mxc_fbi->snoop_window_size / 64;
+
+	start_addr = lsb * seg_size;	// starting address offset
+	start_line = start_addr / fbi->fix.line_length;
+	start_addr = start_line * fbi->fix.line_length;	// Addr aligned to line
+	start_addr += fbi->fix.smem_start;
+
+	end_addr = msb * seg_size;	// ending address offset
+	end_line = end_addr / fbi->fix.line_length;
+	end_line++;
+
+	if (end_line > fbi->var.yres) {
+		end_line = fbi->var.yres;
+	}
+
+	update_height = end_line - start_line;
+	dev_dbg(fbi->device, "updating rows %d to %d, start addr = 0x%08X\n",
+		start_line, end_line, start_addr);
+
+	ipu_uninit_channel(ADC_SYS1);
+	params.adc_sys1.disp = mxc_fbi->disp_num;
+	params.adc_sys1.ch_mode = WriteTemplateNonSeq;
+	params.adc_sys1.out_left = MXCFB_SCREEN_LEFT_OFFSET;
+	params.adc_sys1.out_top = start_line;
+	ipu_init_channel(ADC_SYS1, &params);
+
+	ipu_init_channel_buffer(ADC_SYS1, IPU_INPUT_BUFFER,
+				bpp_to_pixfmt(fbi->var.bits_per_pixel),
+				MXCFB_SCREEN_WIDTH,
+				update_height,
+				stride_pixels,
+				IPU_ROTATE_NONE, (dma_addr_t) start_addr, 0,
+				0, 0);
+	ipu_enable_channel(ADC_SYS1);
+	ipu_select_buffer(ADC_SYS1, IPU_INPUT_BUFFER, 0);
+	ipu_enable_irq(IPU_IRQ_ADC_SYS1_EOF);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mxcfb_sys1_eof_irq_handler(int irq, void *dev_id)
+{
+	ipu_disable_irq(IPU_IRQ_ADC_SYS1_EOF);
+	ipu_disable_channel(ADC_SYS1, false);
+
+	ipu_enable_channel(ADC_SYS2);
+	ipu_enable_irq(IPU_IRQ_ADC_SYS2_EOF);
+
+	return IRQ_HANDLED;
+}
+#endif
+
+/*!
+ * Function to initialize Asynchronous Display Controller. It also initilizes
+ * the ADC System 1 channel. Configure ADC display 0 parallel interface for
+ * the panel.
+ *
+ * @param       fbi     framebuffer information pointer
+ */
+static void mxcfb_init_panel(struct fb_info *fbi)
+{
+	int msb;
+	int panel_stride;
+	ipu_channel_params_t params;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL_IFC_16_BIT
+	uint32_t pix_fmt = IPU_PIX_FMT_RGB565;
+	ipu_adc_sig_cfg_t sig = { 0, 0, 0, 0, 0, 0, 0, 0,
+		IPU_ADC_BURST_WCS,
+		IPU_ADC_IFC_MODE_SYS80_TYPE2,
+		16, 0, 0, IPU_ADC_SER_NO_RW
+	};
+	mxc_fbi->disp_num = DISP0;
+#elif defined(CONFIG_FB_MXC_ASYNC_PANEL_IFC_8_BIT)
+	uint32_t pix_fmt = IPU_PIX_FMT_RGB666;
+	ipu_adc_sig_cfg_t sig = { 0, 0, 0, 0, 0, 0, 0, 0,
+		IPU_ADC_BURST_WCS,
+		IPU_ADC_IFC_MODE_SYS80_TYPE2,
+		8, 0, 0, IPU_ADC_SER_NO_RW
+	};
+	mxc_fbi->disp_num = DISP0;
+#else
+	uint32_t pix_fmt = IPU_PIX_FMT_RGB565;
+	ipu_adc_sig_cfg_t sig = { 0, 1, 0, 0, 0, 0, 0, 0,
+		IPU_ADC_BURST_SERIAL,
+		IPU_ADC_IFC_MODE_5WIRE_SERIAL_CLK,
+		16, 0, 0, IPU_ADC_SER_NO_RW
+	};
+	fbi->disp_num = DISP1;
+#endif
+
+#ifdef PARTIAL_REFRESH
+	if (ipu_request_irq(IPU_IRQ_ADC_SYS2_EOF, mxcfb_sys2_eof_irq_handler, 0,
+			    MXCFB_NAME, fbi) != 0) {
+		dev_err(fbi->device, "Error registering SYS2 irq handler.\n");
+		return;
+	}
+
+	if (ipu_request_irq(IPU_IRQ_ADC_SYS1_EOF, mxcfb_sys1_eof_irq_handler, 0,
+			    MXCFB_NAME, fbi) != 0) {
+		dev_err(fbi->device, "Error registering SYS1 irq handler.\n");
+		return;
+	}
+	ipu_disable_irq(IPU_IRQ_ADC_SYS1_EOF);
+	ipu_disable_irq(IPU_IRQ_ADC_SYS2_EOF);
+#endif
+	// Init DI interface
+	msb = fls(MXCFB_SCREEN_WIDTH);
+	if (!(MXCFB_SCREEN_WIDTH & ((1UL << msb) - 1)))
+		msb--;		// Already aligned to power 2
+	panel_stride = 1UL << msb;
+	ipu_adc_init_panel(mxc_fbi->disp_num,
+			   MXCFB_SCREEN_WIDTH + MXCFB_SCREEN_LEFT_OFFSET,
+			   MXCFB_SCREEN_HEIGHT,
+			   pix_fmt, panel_stride, sig, XY, 0, VsyncInternal);
+
+	ipu_adc_init_ifc_timing(mxc_fbi->disp_num, true,
+				190, 17, 104, 190, 5000000);
+	ipu_adc_init_ifc_timing(mxc_fbi->disp_num, false, 123, 17, 68, 0, 0);
+
+	// Needed to turn on ADC clock for panel init
+	memset(&params, 0, sizeof(params));
+	params.adc_sys1.disp = mxc_fbi->disp_num;
+	params.adc_sys1.ch_mode = WriteTemplateNonSeq;
+	params.adc_sys1.out_left = MXCFB_SCREEN_LEFT_OFFSET;
+	params.adc_sys1.out_top = MXCFB_SCREEN_TOP_OFFSET;
+	ipu_init_channel(ADC_SYS1, &params);
+
+	_init_panel(mxc_fbi->disp_num);
+	init_channel_template(mxc_fbi->disp_num);
+}
+
+int mxcfb_set_refresh_mode(struct fb_info *fbi, int mode,
+			   struct mxcfb_rect *update_region)
+{
+	unsigned long start_addr;
+	int ret_mode;
+	uint32_t dummy;
+	ipu_channel_params_t params;
+	struct mxcfb_rect rect;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+	uint32_t stride_pixels = (fbi->fix.line_length * 8) /
+	    fbi->var.bits_per_pixel;
+	uint32_t memsize = fbi->fix.smem_len;
+
+	if (mxc_fbi->cur_update_mode == mode)
+		return mode;
+
+	ret_mode = mxc_fbi->cur_update_mode;
+
+	ipu_disable_irq(IPU_IRQ_ADC_SYS1_EOF);
+	ipu_adc_set_update_mode(ADC_SYS1, IPU_ADC_REFRESH_NONE, 0, 0, 0);
+#ifdef PARTIAL_REFRESH
+	ipu_disable_irq(IPU_IRQ_ADC_SYS2_EOF);
+	ipu_adc_set_update_mode(ADC_SYS2, IPU_ADC_REFRESH_NONE, 0, 0, 0);
+#endif
+
+	ipu_disable_channel(ADC_SYS1, true);
+	ipu_clear_irq(IPU_IRQ_ADC_SYS1_EOF);
+#ifdef PARTIAL_REFRESH
+	ipu_disable_channel(ADC_SYS2, true);
+	ipu_clear_irq(IPU_IRQ_ADC_SYS2_EOF);
+#endif
+	ipu_adc_get_snooping_status(&dummy, &dummy);
+
+	mxc_fbi->cur_update_mode = mode;
+
+	switch (mode) {
+	case MXCFB_REFRESH_OFF:
+		if (ipu_adc_set_update_mode(ADC_SYS1, IPU_ADC_REFRESH_NONE,
+					    0, 0, 0) < 0)
+			dev_err(fbi->device, "Error enabling auto refesh.\n");
+		if (ipu_adc_set_update_mode(ADC_SYS2, IPU_ADC_REFRESH_NONE,
+					    0, 0, 0) < 0)
+			dev_err(fbi->device, "Error enabling auto refesh.\n");
+#if 0
+		ipu_init_channel_buffer(ADC_SYS2, IPU_INPUT_BUFFER,
+					bpp_to_pixfmt(fbi->var.bits_per_pixel),
+					1, 1, 4,
+					IPU_ROTATE_NONE,
+					fbi->fix.smem_start,
+					fbi->fix.smem_start, 0, 0);
+		ipu_enable_channel(ADC_SYS2);
+		ipu_select_buffer(ADC_SYS2, IPU_INPUT_BUFFER, 0);
+		ipu_select_buffer(ADC_SYS2, IPU_INPUT_BUFFER, 1);
+		msleep(10);
+#endif
+		ipu_uninit_channel(ADC_SYS1);
+#ifdef PARTIAL_REFRESH
+		ipu_uninit_channel(ADC_SYS2);
+#endif
+		break;
+	case MXCFB_REFRESH_PARTIAL:
+#ifdef PARTIAL_REFRESH
+		ipu_adc_get_snooping_status(&dummy, &dummy);
+
+		params.adc_sys2.disp = DISP0;
+		params.adc_sys2.ch_mode = WriteTemplateNonSeq;
+		params.adc_sys2.out_left = 0;
+		params.adc_sys2.out_top = 0;
+		ipu_init_channel(ADC_SYS2, &params);
+
+		if (ipu_adc_set_update_mode(ADC_SYS1, IPU_ADC_REFRESH_NONE,
+					    0, 0, 0) < 0) {
+			dev_err(fbi->device, "Error enabling auto refesh.\n");
+		}
+		if (ipu_adc_set_update_mode
+		    (ADC_SYS2, IPU_ADC_AUTO_REFRESH_SNOOP, 30,
+		     fbi->fix.smem_start, &memsize) < 0) {
+			dev_err(fbi->device, "Error enabling auto refesh.\n");
+		}
+		mxc_fbi->snoop_window_size = memsize;
+
+		ipu_init_channel_buffer(ADC_SYS2, IPU_INPUT_BUFFER,
+					bpp_to_pixfmt(fbi->var.bits_per_pixel),
+					1, 1, 4,
+					IPU_ROTATE_NONE,
+					fbi->fix.smem_start, 0, 0, 0);
+
+		params.adc_sys1.disp = mxc_fbi->disp_num;
+		params.adc_sys1.ch_mode = WriteTemplateNonSeq;
+		params.adc_sys1.out_left = MXCFB_SCREEN_LEFT_OFFSET;
+		params.adc_sys1.out_top = MXCFB_SCREEN_TOP_OFFSET;
+		ipu_init_channel(ADC_SYS1, &params);
+
+		ipu_init_channel_buffer(ADC_SYS1, IPU_INPUT_BUFFER,
+					bpp_to_pixfmt(fbi->var.bits_per_pixel),
+					MXCFB_SCREEN_WIDTH, MXCFB_SCREEN_HEIGHT,
+					stride_pixels, IPU_ROTATE_NONE,
+					fbi->fix.smem_start, 0, 0, 0);
+		ipu_enable_channel(ADC_SYS1);
+		ipu_select_buffer(ADC_SYS1, IPU_INPUT_BUFFER, 0);
+		ipu_enable_irq(IPU_IRQ_ADC_SYS1_EOF);
+		break;
+#endif
+	case MXCFB_REFRESH_AUTO:
+		if (update_region == NULL) {
+			update_region = &rect;
+			rect.top = 0;
+			rect.left = 0;
+			rect.height = MXCFB_SCREEN_HEIGHT;
+			rect.width = MXCFB_SCREEN_WIDTH;
+		}
+		params.adc_sys1.disp = mxc_fbi->disp_num;
+		params.adc_sys1.ch_mode = WriteTemplateNonSeq;
+		params.adc_sys1.out_left = MXCFB_SCREEN_LEFT_OFFSET +
+		    update_region->left;
+		params.adc_sys1.out_top = MXCFB_SCREEN_TOP_OFFSET +
+		    update_region->top;
+		ipu_init_channel(ADC_SYS1, &params);
+
+		// Address aligned to line
+		start_addr = update_region->top * fbi->fix.line_length;
+		start_addr += fbi->fix.smem_start;
+		start_addr += update_region->left * fbi->var.bits_per_pixel / 8;
+
+		ipu_init_channel_buffer(ADC_SYS1, IPU_INPUT_BUFFER,
+					bpp_to_pixfmt(fbi->var.bits_per_pixel),
+					update_region->width,
+					update_region->height, stride_pixels,
+					IPU_ROTATE_NONE, start_addr, 0, 0, 0);
+		ipu_enable_channel(ADC_SYS1);
+		ipu_select_buffer(ADC_SYS1, IPU_INPUT_BUFFER, 0);
+
+		if (ipu_adc_set_update_mode
+		    (ADC_SYS1, IPU_ADC_AUTO_REFRESH_SNOOP, 30,
+		     fbi->fix.smem_start, &memsize) < 0)
+			dev_err(fbi->device, "Error enabling auto refesh.\n");
+
+		mxc_fbi->snoop_window_size = memsize;
+
+		break;
+	}
+	return ret_mode;
+}
+
+/*
+ * Open the main framebuffer.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @param       user    Set if opened by user or clear if opened by kernel
+ */
+static int mxcfb_open(struct fb_info *fbi, int user)
+{
+	int retval = 0;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	if ((retval = wait_event_interruptible(mxcfb_drv_data.suspend_wq,
+					       (mxcfb_drv_data.suspended ==
+						false))) < 0) {
+		return retval;
+	}
+
+	mxc_fbi->open_count++;
+
+	retval = mxcfb_blank(FB_BLANK_UNBLANK, fbi);
+	return retval;
+}
+
+/*
+ * Close the main framebuffer.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @param       user    Set if opened by user or clear if opened by kernel
+ */
+static int mxcfb_release(struct fb_info *fbi, int user)
+{
+	int retval = 0;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	if ((retval = wait_event_interruptible(mxcfb_drv_data.suspend_wq,
+					       (mxcfb_drv_data.suspended ==
+						false))) < 0) {
+		return retval;
+	}
+
+	--mxc_fbi->open_count;
+	if (mxc_fbi->open_count == 0) {
+		retval = mxcfb_blank(FB_BLANK_POWERDOWN, fbi);
+	}
+	return retval;
+}
+
+/*
+ * Set fixed framebuffer parameters based on variable settings.
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_set_fix(struct fb_info *info)
+{
+	struct fb_fix_screeninfo *fix = &info->fix;
+	struct fb_var_screeninfo *var = &info->var;
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
+
+	// Set framebuffer id to IPU display number.
+	strcpy(fix->id, "DISP0 FB");
+	fix->id[4] = '0' + mxc_fbi->disp_num;
+
+	// Init settings based on the panel size
+	fix->line_length = MXCFB_SCREEN_WIDTH * var->bits_per_pixel / 8;
+
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->accel = FB_ACCEL_NONE;
+	fix->visual = FB_VISUAL_TRUECOLOR;
+	fix->xpanstep = 0;
+	fix->ypanstep = 0;
+
+	return 0;
+}
+
+/*
+ * Set framebuffer parameters and change the operating mode.
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_set_par(struct fb_info *fbi)
+{
+	int retval = 0;
+	int mode;
+
+	if ((retval = wait_event_interruptible(mxcfb_drv_data.suspend_wq,
+					       (mxcfb_drv_data.suspended ==
+						false))) < 0) {
+		return retval;
+	}
+
+	mode = mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_OFF, NULL);
+
+	mxcfb_set_fix(fbi);
+
+	if (mode != MXCFB_REFRESH_OFF) {
+#ifdef PARTIAL_REFRESH
+		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_PARTIAL, NULL);
+#else
+		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_AUTO, NULL);
+#endif
+	}
+	return 0;
+}
+
+/*
+ * Check framebuffer variable parameters and adjust to valid values.
+ *
+ * @param       var      framebuffer variable parameters
+ *
+ * @param       info     framebuffer information pointer
+ */
+static int mxcfb_check_var(struct fb_var_screeninfo *var, struct fb_info *fbi)
+{
+	if (var->xres > MXCFB_SCREEN_WIDTH)
+		var->xres = MXCFB_SCREEN_WIDTH;
+	if (var->yres > MXCFB_SCREEN_HEIGHT)
+		var->yres = MXCFB_SCREEN_HEIGHT;
+	if (var->xres_virtual < var->xres)
+		var->xres_virtual = var->xres;
+	if (var->yres_virtual < var->yres)
+		var->yres_virtual = var->yres;
+
+	if ((var->bits_per_pixel != 32) && (var->bits_per_pixel != 24) &&
+	    (var->bits_per_pixel != 16)) {
+		var->bits_per_pixel = default_bpp;
+	}
+
+	switch (var->bits_per_pixel) {
+	case 16:
+		var->red.length = 5;
+		var->red.offset = 11;
+		var->red.msb_right = 0;
+
+		var->green.length = 6;
+		var->green.offset = 5;
+		var->green.msb_right = 0;
+
+		var->blue.length = 5;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 24:
+		var->red.length = 8;
+		var->red.offset = 16;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 8;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 32:
+		var->red.length = 8;
+		var->red.offset = 16;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 8;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 8;
+		var->transp.offset = 24;
+		var->transp.msb_right = 0;
+		break;
+	}
+
+	var->height = -1;
+	var->width = -1;
+	var->grayscale = 0;
+	var->nonstd = 0;
+
+	var->pixclock = -1;
+	var->left_margin = -1;
+	var->right_margin = -1;
+	var->upper_margin = -1;
+	var->lower_margin = -1;
+	var->hsync_len = -1;
+	var->vsync_len = -1;
+
+	var->vmode = FB_VMODE_NONINTERLACED;
+	var->sync = 0;
+
+	return 0;
+}
+
+static inline u_int _chan_to_field(u_int chan, struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+static int
+mxcfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+		u_int trans, struct fb_info *fbi)
+{
+	unsigned int val;
+	int ret = 1;
+
+	/*
+	 * If greyscale is true, then we convert the RGB value
+	 * to greyscale no matter what visual we are using.
+	 */
+	if (fbi->var.grayscale)
+		red = green = blue = (19595 * red + 38470 * green +
+				      7471 * blue) >> 16;
+	switch (fbi->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		/*
+		 * 16-bit True Colour.  We encode the RGB value
+		 * according to the RGB bitfield information.
+		 */
+		if (regno < 16) {
+			u32 *pal = fbi->pseudo_palette;
+
+			val = _chan_to_field(red, &fbi->var.red);
+			val |= _chan_to_field(green, &fbi->var.green);
+			val |= _chan_to_field(blue, &fbi->var.blue);
+
+			pal[regno] = val;
+			ret = 0;
+		}
+		break;
+
+	case FB_VISUAL_STATIC_PSEUDOCOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		break;
+	}
+
+	return ret;
+}
+
+/*
+ * mxcfb_blank():
+ *      Blank the display.
+ */
+static int mxcfb_blank(int blank, struct fb_info *fbi)
+{
+	int retval = 0;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	dev_dbg(fbi->device, "blank = %d\n", blank);
+
+	if ((retval = wait_event_interruptible(mxcfb_drv_data.suspend_wq,
+					       (mxcfb_drv_data.suspended ==
+						false))) < 0) {
+		return retval;
+	}
+
+	mxc_fbi->blank = blank;
+
+	switch (blank) {
+	case FB_BLANK_POWERDOWN:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_NORMAL:
+		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_OFF, NULL);
+		break;
+	case FB_BLANK_UNBLANK:
+		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_DEFAULT, NULL);
+		break;
+	}
+	return 0;
+}
+
+/*!
+ * This structure contains the pointers to the control functions that are
+ * invoked by the core framebuffer driver to perform operations like
+ * blitting, rectangle filling, copy regions and cursor definition.
+ */
+static struct fb_ops mxcfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_open = mxcfb_open,
+	.fb_release = mxcfb_release,
+	.fb_set_par = mxcfb_set_par,
+	.fb_check_var = mxcfb_check_var,
+	.fb_setcolreg = mxcfb_setcolreg,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+	.fb_blank = mxcfb_blank,
+};
+
+/*!
+ * Allocates the DRAM memory for the frame buffer.      This buffer is remapped
+ * into a non-cached, non-buffered, memory region to allow palette and pixel
+ * writes to occur without flushing the cache.  Once this area is remapped,
+ * all virtual memory access to the video memory should occur at the new region.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @return      Error code indicating success or failure
+ */
+static int mxcfb_map_video_memory(struct fb_info *fbi)
+{
+	u32 msb;
+	u32 offset;
+	struct mxcfb_info *mxcfbi = fbi->par;
+
+	fbi->fix.smem_len = fbi->var.xres_virtual * fbi->var.yres_virtual * 4;
+
+	// Set size to power of 2.
+	msb = fls(fbi->fix.smem_len);
+	if (!(fbi->fix.smem_len & ((1UL << msb) - 1)))
+		msb--;		// Already aligned to power 2
+	if (msb < 11)
+		msb = 11;
+	mxcfbi->alloc_size = (1UL << msb) * 2;
+
+	mxcfbi->alloc_start_vaddr = dma_alloc_coherent(fbi->device,
+						       mxcfbi->alloc_size,
+						       &mxcfbi->
+						       alloc_start_paddr,
+						       GFP_KERNEL | GFP_DMA);
+
+	if (mxcfbi->alloc_start_vaddr == 0) {
+		dev_err(fbi->device, "Unable to allocate framebuffer memory\n");
+		return -ENOMEM;
+	}
+	dev_dbg(fbi->device, "allocated fb memory @ paddr=0x%08X, size=%d.\n",
+		(uint32_t) mxcfbi->alloc_start_paddr, mxcfbi->alloc_size);
+
+	offset =
+	    ((mxcfbi->alloc_size / 2) - 1) & ~((mxcfbi->alloc_size / 2) - 1);
+	fbi->fix.smem_start = mxcfbi->alloc_start_paddr + offset;
+	dev_dbg(fbi->device, "aligned fb start @ paddr=0x%08lX, size=%u.\n",
+		fbi->fix.smem_start, fbi->fix.smem_len);
+
+	fbi->screen_base = mxcfbi->alloc_start_vaddr + offset;
+
+	/* Clear the screen */
+	memset(fbi->screen_base, 0, fbi->fix.smem_len);
+	return 0;
+}
+
+/*!
+ * De-allocates the DRAM memory for the frame buffer.
+ *
+ * @param       fbi     framebuffer information pointer
+ *
+ * @return      Error code indicating success or failure
+ */
+static int mxcfb_unmap_video_memory(struct fb_info *fbi)
+{
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
+
+	dma_free_coherent(fbi->device, mxc_fbi->alloc_size,
+			  mxc_fbi->alloc_start_vaddr,
+			  mxc_fbi->alloc_start_paddr);
+	return 0;
+}
+
+/*!
+ * Initializes the framebuffer information pointer. After allocating
+ * sufficient memory for the framebuffer structure, the fields are
+ * filled with custom information passed in from the configurable
+ * structures.  This includes information such as bits per pixel,
+ * color maps, screen width/height and RGBA offsets.
+ *
+ * @return      Framebuffer structure initialized with our information
+ */
+static struct fb_info *mxcfb_init_fbinfo(struct device *dev, struct fb_ops *ops)
+{
+	struct fb_info *fbi;
+	struct mxcfb_info *mxcfbi;
+
+	/*
+	 * Allocate sufficient memory for the fb structure
+	 */
+	fbi = framebuffer_alloc(sizeof(struct mxcfb_info), dev);
+	if (!fbi)
+		return NULL;
+
+	mxcfbi = (struct mxcfb_info *)fbi->par;
+
+	/*
+	 * Fill in fb_info structure information
+	 */
+	fbi->var.xres = fbi->var.xres_virtual = MXCFB_SCREEN_WIDTH;
+	fbi->var.yres = fbi->var.yres_virtual = MXCFB_SCREEN_HEIGHT;
+	fbi->var.activate = FB_ACTIVATE_NOW;
+	mxcfb_check_var(&fbi->var, fbi);
+
+	mxcfb_set_fix(fbi);
+
+	fbi->fbops = ops;
+	fbi->flags = FBINFO_FLAG_DEFAULT;
+	fbi->pseudo_palette = mxcfbi->pseudo_palette;
+
+	/*
+	 * Allocate colormap
+	 */
+	fb_alloc_cmap(&fbi->cmap, 16, 0);
+
+	return fbi;
+}
+
+/*!
+ * Probe routine for the framebuffer driver. It is called during the
+ * driver binding process.      The following functions are performed in
+ * this routine: Framebuffer initialization, Memory allocation and
+ * mapping, Framebuffer registration, IPU initialization.
+ *
+ * @return      Appropriate error code to the kernel common code
+ */
+static int mxcfb_probe(struct platform_device *pdev)
+{
+	struct fb_info *fbi;
+	struct mxcfb_info *mxc_fbi;
+	int ret;
+
+	platform_set_drvdata(pdev, &mxcfb_drv_data);
+
+	/*
+	 * Initialize FB structures
+	 */
+	fbi = mxcfb_init_fbinfo(&pdev->dev, &mxcfb_ops);
+	if (!fbi) {
+		ret = -ENOMEM;
+		goto err0;
+	}
+	mxcfb_drv_data.fbi = fbi;
+	mxc_fbi = fbi->par;
+
+	mxcfb_drv_data.suspended = false;
+	init_waitqueue_head(&mxcfb_drv_data.suspend_wq);
+
+	/*
+	 * Allocate memory
+	 */
+	ret = mxcfb_map_video_memory(fbi);
+	if (ret < 0) {
+		goto err1;
+	}
+
+	mxcfb_init_panel(fbi);
+
+	/*
+	 * Register framebuffer
+	 */
+	ret = register_framebuffer(fbi);
+	if (ret < 0) {
+		goto err2;
+	}
+
+	dev_info(&pdev->dev, "%s registered\n", MXCFB_NAME);
+
+	return 0;
+
+      err2:
+	mxcfb_unmap_video_memory(fbi);
+      err1:
+	if (&fbi->cmap)
+		fb_dealloc_cmap(&fbi->cmap);
+	framebuffer_release(fbi);
+      err0:
+	return ret;
+}
+
+#ifdef CONFIG_PM
+/*!
+ * Power management hooks.      Note that we won't be called from IRQ context,
+ * unlike the blank functions above, so we may sleep.
+ */
+
+/*!
+ * Suspends the framebuffer and blanks the screen. Power management support
+ *
+ * @param	pdev	pointer to device structure.
+ * @param	state	state of the device.
+ *
+ * @return	success
+ */
+static int mxcfb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct mxcfb_data *drv_data = platform_get_drvdata(pdev);
+	struct fb_info *fbi = drv_data->fbi;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	drv_data->suspended = true;
+
+	if (mxc_fbi->blank == FB_BLANK_UNBLANK)
+		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_OFF, NULL);
+	/* Display OFF */
+	ipu_adc_write_cmd(mxc_fbi->disp_num, CMD, DISOFF, 0, 0);
+
+	return 0;
+}
+
+/*!
+ * Resumes the framebuffer and unblanks the screen. Power management support
+ *
+ * @param       pdev     pointer to device structure.
+ *
+ * @return      success
+ */
+static int mxcfb_resume(struct platform_device *pdev)
+{
+	struct mxcfb_data *drv_data = platform_get_drvdata(pdev);
+	struct fb_info *fbi = drv_data->fbi;
+	struct mxcfb_info *mxc_fbi = fbi->par;
+
+	// Display ON
+	ipu_adc_write_cmd(mxc_fbi->disp_num, CMD, DISON, 0, 0);
+	drv_data->suspended = false;
+
+	if (mxc_fbi->blank == FB_BLANK_UNBLANK)
+		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_DEFAULT, NULL);
+	wake_up_interruptible(&drv_data->suspend_wq);
+
+	return 0;
+}
+#else
+#define mxcfb_suspend   NULL
+#define mxcfb_resume    NULL
+#endif
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxcfb_driver = {
+	.driver = {
+		   .name = MXCFB_NAME,
+		   },
+	.probe = mxcfb_probe,
+	.suspend = mxcfb_suspend,
+	.resume = mxcfb_resume,
+};
+
+/*!
+ * Device definition for the Framebuffer
+ */
+static struct platform_device mxcfb_device = {
+	.name = MXCFB_NAME,
+	.id = 0,
+	.dev = {
+		.coherent_dma_mask = 0xFFFFFFFF,
+		}
+};
+
+/*!
+ * Main entry function for the framebuffer. The function registers the power
+ * management callback functions with the kernel and also registers the MXCFB
+ * callback functions with the core Linux framebuffer driver \b fbmem.c
+ *
+ * @return      Error code indicating success or failure
+ */
+static int mxcfb_init(void)
+{
+	int ret = 0;
+
+	ret = platform_driver_register(&mxcfb_driver);
+	if (ret == 0) {
+		ret = platform_device_register(&mxcfb_device);
+		if (ret != 0) {
+			platform_driver_unregister(&mxcfb_driver);
+		}
+	}
+	return ret;
+}
+
+static void mxcfb_exit(void)
+{
+	struct fb_info *fbi = dev_get_drvdata(&mxcfb_device.dev);
+
+	if (fbi) {
+		mxcfb_unmap_video_memory(fbi);
+
+		if (&fbi->cmap)
+			fb_dealloc_cmap(&fbi->cmap);
+
+		unregister_framebuffer(fbi);
+		framebuffer_release(fbi);
+	}
+
+	platform_device_unregister(&mxcfb_device);
+	platform_driver_unregister(&mxcfb_driver);
+}
+
+module_init(mxcfb_init);
+module_exit(mxcfb_exit);
+
+EXPORT_SYMBOL(mxcfb_set_refresh_mode);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC Epson framebuffer driver");
+MODULE_SUPPORTED_DEVICE("fb");
diff --git a/drivers/video/mxc/mxcfb_epson_vga.c b/drivers/video/mxc/mxcfb_epson_vga.c
new file mode 100644
index 000000000000..28856b6158ac
--- /dev/null
+++ b/drivers/video/mxc/mxcfb_epson_vga.c
@@ -0,0 +1,360 @@
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup Framebuffer Framebuffer Driver for SDC and ADC.
+ */
+
+/*!
+ * @file mxcfb_epson_vga.c
+ *
+ * @brief MXC Frame buffer driver for SDC
+ *
+ * @ingroup Framebuffer
+ */
+
+/*!
+ * Include files
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/spi/spi.h>
+#include <linux/mxcfb.h>
+#include <linux/ipu.h>
+#include <asm/mach-types.h>
+
+static struct spi_device *lcd_spi;
+static struct device *lcd_dev;
+
+static void lcd_init(void);
+static void lcd_poweron(void);
+static void lcd_poweroff(void);
+
+static void (*lcd_reset) (void);
+static struct regulator *io_reg;
+static struct regulator *core_reg;
+
+static struct fb_videomode video_modes[] = {
+	{
+	 /* 480x640 @ 60 Hz */
+	 "Epson-VGA", 60, 480, 640, 41701, 60, 41, 10, 5, 20, 10,
+	 0,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+};
+
+static void lcd_init_fb(struct fb_info *info)
+{
+	struct fb_var_screeninfo var;
+
+	memset(&var, 0, sizeof(var));
+
+	fb_videomode_to_var(&var, &video_modes[0]);
+
+	if (machine_is_mx31_3ds()) {
+		var.upper_margin = 0;
+		var.left_margin = 0;
+	}
+
+	var.activate = FB_ACTIVATE_ALL;
+	var.yres_virtual = var.yres * 2;
+
+	acquire_console_sem();
+	info->flags |= FBINFO_MISC_USEREVENT;
+	fb_set_var(info, &var);
+	info->flags &= ~FBINFO_MISC_USEREVENT;
+	release_console_sem();
+}
+
+static int lcd_fb_event(struct notifier_block *nb, unsigned long val, void *v)
+{
+	struct fb_event *event = v;
+
+	if (strcmp(event->info->fix.id, "DISP3 BG")) {
+		return 0;
+	}
+
+	switch (val) {
+	case FB_EVENT_FB_REGISTERED:
+		lcd_init_fb(event->info);
+		lcd_poweron();
+		break;
+	case FB_EVENT_BLANK:
+		if ((event->info->var.xres != 480) ||
+		    (event->info->var.yres != 640)) {
+			break;
+		}
+		if (*((int *)event->data) == FB_BLANK_UNBLANK) {
+			lcd_poweron();
+		} else {
+			lcd_poweroff();
+		}
+		break;
+	}
+	return 0;
+}
+
+static struct notifier_block nb = {
+	.notifier_call = lcd_fb_event,
+};
+
+/*!
+ * This function is called whenever the SPI slave device is detected.
+ *
+ * @param	spi	the SPI slave device
+ *
+ * @return 	Returns 0 on SUCCESS and error on FAILURE.
+ */
+static int __devinit lcd_probe(struct device *dev)
+{
+	int i;
+	struct mxc_lcd_platform_data *plat = dev->platform_data;
+
+	lcd_dev = dev;
+
+	if (plat) {
+		io_reg = regulator_get(dev, plat->io_reg);
+		if (!IS_ERR(io_reg)) {
+			regulator_set_voltage(io_reg, 1800000, 1800000);
+			regulator_enable(io_reg);
+		}
+		core_reg = regulator_get(dev, plat->core_reg);
+		if (!IS_ERR(core_reg)) {
+			regulator_set_voltage(core_reg, 2800000, 2800000);
+			regulator_enable(core_reg);
+		}
+
+		lcd_reset = plat->reset;
+		if (lcd_reset)
+			lcd_reset();
+	}
+
+	lcd_init();
+
+	for (i = 0; i < num_registered_fb; i++) {
+		if (strcmp(registered_fb[i]->fix.id, "DISP3 BG") == 0) {
+			lcd_init_fb(registered_fb[i]);
+			fb_show_logo(registered_fb[i], 0);
+			lcd_poweron();
+		}
+	}
+
+	fb_register_client(&nb);
+
+	return 0;
+}
+
+static int __devinit lcd_plat_probe(struct platform_device *pdev)
+{
+	ipu_adc_sig_cfg_t sig;
+	ipu_channel_params_t param;
+
+	memset(&sig, 0, sizeof(sig));
+	sig.ifc_width = 9;
+	sig.clk_pol = 1;
+	ipu_init_async_panel(0, IPU_PANEL_SERIAL, 90, IPU_PIX_FMT_GENERIC, sig);
+
+	memset(&param, 0, sizeof(param));
+	ipu_init_channel(DIRECT_ASYNC1, &param);
+
+	return lcd_probe(&pdev->dev);
+}
+
+static int __devinit lcd_spi_probe(struct spi_device *spi)
+{
+	lcd_spi = spi;
+
+	spi->bits_per_word = 9;
+	spi_setup(spi);
+
+	return lcd_probe(&spi->dev);
+}
+
+static int __devexit lcd_remove(struct device *dev)
+{
+	fb_unregister_client(&nb);
+	lcd_poweroff();
+	regulator_put(io_reg);
+	regulator_put(core_reg);
+
+	return 0;
+}
+
+static int __devexit lcd_spi_remove(struct spi_device *spi)
+{
+	int ret = lcd_remove(&spi->dev);
+	lcd_spi = NULL;
+	return ret;
+}
+
+static int __devexit lcd_plat_remove(struct platform_device *pdev)
+{
+	return lcd_remove(&pdev->dev);
+}
+
+static int lcd_suspend(struct spi_device *spi, pm_message_t message)
+{
+	lcd_poweroff();
+	return 0;
+}
+
+static int lcd_resume(struct spi_device *spi)
+{
+	if (lcd_reset)
+		lcd_reset();
+
+	lcd_init();
+	lcd_poweron();
+	return 0;
+}
+
+/*!
+ * spi driver structure for LTV350QV
+ */
+static struct spi_driver lcd_spi_dev_driver = {
+
+	.driver = {
+		   .name = "lcd_spi",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = lcd_spi_probe,
+	.remove = __devexit_p(lcd_spi_remove),
+	.suspend = lcd_suspend,
+	.resume = lcd_resume,
+};
+
+static struct platform_driver lcd_plat_driver = {
+	.driver = {
+		   .name = "lcd_spi",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = lcd_plat_probe,
+	.remove = __devexit_p(lcd_plat_remove),
+};
+
+#define param(x) ((x) | 0x100)
+
+/*
+ * Send init commands to L4F00242T03
+ *
+ */
+static void lcd_init(void)
+{
+	const u16 cmd[] = { 0x36, param(0), 0x3A, param(0x60) };
+
+	dev_dbg(lcd_dev, "initializing LCD\n");
+	if (lcd_spi) {
+		spi_write(lcd_spi, (const u8 *)cmd, ARRAY_SIZE(cmd));
+	} else {
+		ipu_disp_direct_write(DIRECT_ASYNC1, 0x36, 0);
+		ipu_disp_direct_write(DIRECT_ASYNC1, 0x100, 0);
+		ipu_disp_direct_write(DIRECT_ASYNC1, 0x3A, 0);
+		ipu_disp_direct_write(DIRECT_ASYNC1, 0x160, 0);
+		msleep(1);
+		ipu_uninit_channel(DIRECT_ASYNC1);
+	}
+}
+
+static int lcd_on;
+/*
+ * Send Power On commands to L4F00242T03
+ *
+ */
+static void lcd_poweron(void)
+{
+	const u16 slpout = 0x11;
+	const u16 dison = 0x29;
+	ipu_channel_params_t param;
+	if (lcd_on)
+		return;
+
+	dev_dbg(lcd_dev, "turning on LCD\n");
+
+	if (lcd_spi) {
+		msleep(60);
+		spi_write(lcd_spi, (const u8 *)&slpout, 1);
+		msleep(60);
+		spi_write(lcd_spi, (const u8 *)&dison, 1);
+	} else {
+		memset(&param, 0, sizeof(param));
+		ipu_init_channel(DIRECT_ASYNC1, &param);
+		ipu_disp_direct_write(DIRECT_ASYNC1, slpout, 0);
+		msleep(60);
+		ipu_disp_direct_write(DIRECT_ASYNC1, dison, 0);
+		msleep(1);
+		ipu_uninit_channel(DIRECT_ASYNC1);
+	}
+	lcd_on = 1;
+}
+
+/*
+ * Send Power Off commands to L4F00242T03
+ *
+ */
+static void lcd_poweroff(void)
+{
+	const u16 slpin = 0x10;
+	const u16 disoff = 0x28;
+	ipu_channel_params_t param;
+	if (!lcd_on)
+		return;
+
+	dev_dbg(lcd_dev, "turning off LCD\n");
+
+	if (lcd_spi) {
+		msleep(60);
+		spi_write(lcd_spi, (const u8 *)&disoff, 1);
+		msleep(60);
+		spi_write(lcd_spi, (const u8 *)&slpin, 1);
+	} else {
+		memset(&param, 0, sizeof(param));
+		ipu_init_channel(DIRECT_ASYNC1, &param);
+		ipu_disp_direct_write(DIRECT_ASYNC1, disoff, 0);
+		msleep(60);
+		ipu_disp_direct_write(DIRECT_ASYNC1, slpin, 0);
+		msleep(1);
+		ipu_uninit_channel(DIRECT_ASYNC1);
+	}
+	lcd_on = 0;
+}
+
+static int __init epson_lcd_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&lcd_plat_driver);
+	if (ret)
+		return ret;
+
+	return spi_register_driver(&lcd_spi_dev_driver);
+
+}
+
+static void __exit epson_lcd_exit(void)
+{
+	spi_unregister_driver(&lcd_spi_dev_driver);
+}
+
+module_init(epson_lcd_init);
+module_exit(epson_lcd_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Epson VGA LCD init driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/mxc/mxcfb_modedb.c b/drivers/video/mxc/mxcfb_modedb.c
new file mode 100644
index 000000000000..ad31c6b4f856
--- /dev/null
+++ b/drivers/video/mxc/mxcfb_modedb.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/kernel.h>
+#include <linux/mxcfb.h>
+
+struct fb_videomode mxcfb_modedb[] = {
+	{
+	 /* 240x320 @ 60 Hz */
+	 "Sharp-QVGA", 60, 240, 320, 185925, 9, 16, 7, 9, 1, 1,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_SHARP_MODE |
+	 FB_SYNC_DATA_INVERT | FB_SYNC_CLK_IDLE_EN,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+	{
+	 /* 240x33 @ 60 Hz */
+	 "Sharp-CLI", 60, 240, 33, 185925, 9, 16, 7, 9 + 287, 1, 1,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_SHARP_MODE |
+	 FB_SYNC_DATA_INVERT | FB_SYNC_CLK_IDLE_EN,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+	{
+	 /* 640x480 @ 60 Hz */
+	 "NEC-VGA", 60, 640, 480, 38255, 144, 0, 34, 40, 1, 1,
+	 FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+	{
+	 /* 640x480 @ 60 Hz */
+	 "CPT-VGA", 60, 640, 480, 39683, 45, 114, 33, 11, 1, 1,
+	 FB_SYNC_CLK_LAT_FALL,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+	{
+	 /* NTSC TV output */
+	 "TV-NTSC", 60, 640, 480, 37538,
+	 38, 858 - 640 - 38 - 3,
+	 36, 518 - 480 - 36 - 1,
+	 3, 1,
+	 0,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+	{
+	 /* PAL TV output */
+	 "TV-PAL", 50, 640, 480, 37538,
+	 38, 960 - 640 - 38 - 32,
+	 32, 555 - 480 - 32 - 3,
+	 32, 3,
+	 0,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+	{
+	 /* TV output VGA mode, 640x480 @ 65 Hz */
+	 "TV-VGA", 60, 640, 480, 40574, 35, 45, 9, 1, 46, 5,
+	 0, FB_VMODE_NONINTERLACED, 0,
+	 },
+};
+
+int mxcfb_modedb_sz = ARRAY_SIZE(mxcfb_modedb);
diff --git a/drivers/video/mxc/tve.c b/drivers/video/mxc/tve.c
new file mode 100644
index 000000000000..6a37ce39592b
--- /dev/null
+++ b/drivers/video/mxc/tve.c
@@ -0,0 +1,803 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file tve.c
+ * @brief Driver for i.MX TV encoder
+ *
+ * @ingroup Framebuffer
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/console.h>
+#include <linux/clk.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/sysfs.h>
+#include <linux/irq.h>
+#include <linux/sysfs.h>
+#include <linux/platform_device.h>
+#include <linux/mxcfb.h>
+#include <linux/regulator/consumer.h>
+#include <asm/uaccess.h>
+#include <asm/atomic.h>
+
+#define TVE_ENABLE			(1UL)
+#define TVE_DAC_FULL_RATE		(0UL<<1)
+#define TVE_DAC_DIV2_RATE		(1UL<<1)
+#define TVE_DAC_DIV4_RATE		(2UL<<1)
+#define TVE_IPU_CLK_ENABLE		(1UL<<3)
+
+#define CD_LM_INT		0x00000001
+#define CD_SM_INT		0x00000002
+#define CD_MON_END_INT		0x00000004
+#define CD_CH_0_LM_ST		0x00000001
+#define CD_CH_0_SM_ST		0x00000010
+#define CD_CH_1_LM_ST		0x00000002
+#define CD_CH_1_SM_ST		0x00000020
+#define CD_CH_2_LM_ST		0x00000004
+#define CD_CH_2_SM_ST		0x00000040
+#define CD_MAN_TRIG		0x00000100
+
+#define TVE_STAND_MASK			(0x0F<<8)
+#define TVE_NTSC_STAND			(0UL<<8)
+#define TVE_PAL_STAND			(3UL<<8)
+#define TVE_HD720P60_STAND		(4UL<<8)
+
+#define TVOUT_FMT_OFF			0
+#define TVOUT_FMT_NTSC			1
+#define TVOUT_FMT_PAL			2
+#define TVOUT_FMT_720P60		3
+
+static int enabled;		/* enable power on or not */
+
+static struct fb_info *tve_fbi;
+
+struct tve_data {
+	struct platform_device *pdev;
+	int revision;
+	int cur_mode;
+	int output_mode;
+	int detect;
+	void *base;
+	int irq;
+	struct clk *clk;
+	struct regulator *dac_reg;
+	struct regulator *dig_reg;
+	struct delayed_work cd_work;
+} tve;
+
+struct tve_reg_mapping {
+	u32 tve_com_conf_reg;
+	u32 tve_cd_cont_reg;
+	u32 tve_int_cont_reg;
+	u32 tve_stat_reg;
+	u32 tve_mv_cont_reg;
+};
+
+struct tve_reg_fields_mapping {
+	u32 cd_en;
+	u32 cd_trig_mode;
+	u32 cd_lm_int;
+	u32 cd_sm_int;
+	u32 cd_mon_end_int;
+	u32 cd_man_trig;
+	u32 sync_ch_mask;
+	u32 tvout_mode_mask;
+	u32 sync_ch_offset;
+	u32 tvout_mode_offset;
+	u32 cd_ch_stat_offset;
+};
+
+static struct tve_reg_mapping tve_regs_v1 = {
+	0, 0x14, 0x28, 0x2C, 0x48
+};
+
+static struct tve_reg_fields_mapping tve_reg_fields_v1 = {
+	1, 2, 1, 2, 4, 0x00010000, 0x7000, 0x70, 12, 4, 8
+};
+
+static struct tve_reg_mapping tve_regs_v2 = {
+	0, 0x34, 0x64, 0x68, 0xDC
+};
+
+static struct tve_reg_fields_mapping tve_reg_fields_v2 = {
+	1, 2, 1, 2, 4, 0x01000000, 0x700000, 0x7000, 20, 12, 16
+};
+
+
+struct tve_reg_mapping *tve_regs;
+struct tve_reg_fields_mapping *tve_reg_fields;
+
+/* For MX37 need modify some fields in tve_probe */
+static struct fb_videomode video_modes[] = {
+	{
+	 /* NTSC TV output */
+	 "TV-NTSC", 60, 720, 480, 74074,
+	 122, 15,
+	 18, 26,
+	 1, 1,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT | FB_SYNC_EXT,
+	 FB_VMODE_INTERLACED,
+	 0,},
+	{
+	 /* PAL TV output */
+	 "TV-PAL", 50, 720, 576, 74074,
+	 132, 11,
+	 22, 26,
+	 1, 1,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT | FB_SYNC_EXT,
+	 FB_VMODE_INTERLACED | FB_VMODE_ODD_FLD_FIRST,
+	 0,},
+	{
+	 /* 720p60 TV output */
+	 "720P60", 60, 1280, 720, 13468,
+	 260, 109,
+	 25, 4,
+	 1, 1,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT |
+			FB_SYNC_EXT,
+	 FB_VMODE_NONINTERLACED,
+	 0,},
+};
+
+enum tvout_mode {
+	TV_OFF,
+	CVBS0,
+	CVBS2,
+	CVBS02,
+	SVIDEO,
+	SVIDEO_CVBS,
+	YPBPR,
+	RGB
+};
+
+static unsigned short tvout_mode_to_channel_map[8] = {
+	0,	/* TV_OFF */
+	1,	/* CVBS0 */
+	4,	/* CVBS2 */
+	5,	/* CVBS02 */
+	1,	/* SVIDEO */
+	5,	/* SVIDEO_CVBS */
+	1,	/* YPBPR */
+	7	/* RGB */
+};
+
+
+static void tve_set_tvout_mode(int mode)
+{
+	u32 conf_reg;
+
+	conf_reg = __raw_readl(tve.base + tve_regs->tve_com_conf_reg);
+	conf_reg &= ~(tve_reg_fields->sync_ch_mask |
+				tve_reg_fields->tvout_mode_mask);
+	/* clear sync_ch and tvout_mode fields */
+	conf_reg |=
+		mode << tve_reg_fields->
+		tvout_mode_offset | tvout_mode_to_channel_map[mode] <<
+		tve_reg_fields->sync_ch_offset;
+	__raw_writel(conf_reg, tve.base + tve_regs->tve_com_conf_reg);
+}
+
+static int _is_tvout_mode_hd_compatible(void)
+{
+	u32 conf_reg, mode;
+
+	conf_reg = __raw_readl(tve.base + tve_regs->tve_com_conf_reg);
+	mode = (conf_reg >> tve_reg_fields->tvout_mode_offset) & 7;
+	if (mode == YPBPR || mode == RGB) {
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+
+/**
+ * tve_setup
+ * initial the CH7024 chipset by setting register
+ * @param:
+ * 	vos: output video format
+ * @return:
+ * 	0 successful
+ * 	otherwise failed
+ */
+static int tve_setup(int mode)
+{
+	u32 reg;
+	struct clk *pll3_clk;
+	unsigned long pll3_clock_rate = 216000000;
+
+	if (tve.cur_mode == mode)
+		return 0;
+
+	tve.cur_mode = mode;
+
+	switch (mode) {
+	case TVOUT_FMT_PAL:
+	case TVOUT_FMT_NTSC:
+		pll3_clock_rate = 216000000;
+		break;
+	case TVOUT_FMT_720P60:
+		pll3_clock_rate = 297000000;
+		break;
+	}
+	if (enabled)
+		clk_disable(tve.clk);
+
+	pll3_clk = clk_get(NULL, "pll3");
+	clk_disable(pll3_clk);
+	clk_set_rate(pll3_clk, pll3_clock_rate);
+	clk_enable(pll3_clk);
+
+	clk_enable(tve.clk);
+
+	/* select output video format */
+	if (mode == TVOUT_FMT_PAL) {
+		reg = __raw_readl(tve.base + tve_regs->tve_com_conf_reg);
+		reg = (reg & ~TVE_STAND_MASK) | TVE_PAL_STAND;
+		__raw_writel(reg, tve.base + tve_regs->tve_com_conf_reg);
+		pr_debug("TVE: change to PAL video\n");
+	} else if (mode == TVOUT_FMT_NTSC) {
+		reg = __raw_readl(tve.base + tve_regs->tve_com_conf_reg);
+		reg = (reg & ~TVE_STAND_MASK) | TVE_NTSC_STAND;
+		__raw_writel(reg, tve.base + tve_regs->tve_com_conf_reg);
+		pr_debug("TVE: change to NTSC video\n");
+	} else if (mode == TVOUT_FMT_720P60) {
+		if (!_is_tvout_mode_hd_compatible()) {
+			tve_set_tvout_mode(YPBPR);
+			pr_debug("The TV out mode is HD incompatible. Setting to YPBPR.");
+		}
+		reg = __raw_readl(tve.base + tve_regs->tve_com_conf_reg);
+		reg = (reg & ~TVE_STAND_MASK) | TVE_HD720P60_STAND;
+		__raw_writel(reg, tve.base + tve_regs->tve_com_conf_reg);
+		pr_debug("TVE: change to 720P60 video\n");
+	} else if (mode == TVOUT_FMT_OFF) {
+		__raw_writel(0x0, tve.base + tve_regs->tve_com_conf_reg);
+		pr_debug("TVE: change to OFF video\n");
+	} else {
+		pr_debug("TVE: no such video format.\n");
+		if (!enabled)
+			clk_disable(tve.clk);
+		return -EINVAL;
+	}
+
+	if (!enabled)
+		clk_disable(tve.clk);
+
+	return 0;
+}
+
+/**
+ * tve_enable
+ * Enable the tve Power to begin TV encoder
+ */
+static void tve_enable(void)
+{
+	u32 reg;
+
+	if (!enabled) {
+		enabled = 1;
+		clk_enable(tve.clk);
+		reg = __raw_readl(tve.base + tve_regs->tve_com_conf_reg);
+		__raw_writel(reg | TVE_IPU_CLK_ENABLE | TVE_ENABLE,
+					tve.base + tve_regs->tve_com_conf_reg);
+		pr_debug("TVE power on.\n");
+	}
+
+	/* enable interrupt */
+	__raw_writel(CD_SM_INT | CD_LM_INT | CD_MON_END_INT,
+				tve.base + tve_regs->tve_stat_reg);
+	__raw_writel(CD_SM_INT | CD_LM_INT | CD_MON_END_INT,
+				tve.base + tve_regs->tve_int_cont_reg);
+}
+
+/**
+ * tve_disable
+ * Disable the tve Power to stop TV encoder
+ */
+static void tve_disable(void)
+{
+	u32 reg;
+
+	if (enabled) {
+		enabled = 0;
+		reg = __raw_readl(tve.base + tve_regs->tve_com_conf_reg);
+		__raw_writel(reg & ~TVE_ENABLE & ~TVE_IPU_CLK_ENABLE,
+				tve.base + tve_regs->tve_com_conf_reg);
+		clk_disable(tve.clk);
+		pr_debug("TVE power off.\n");
+	}
+}
+
+static int tve_update_detect_status(void)
+{
+	int old_detect = tve.detect;
+	u32 stat_lm, stat_sm, stat;
+	u32 int_ctl = __raw_readl(tve.base + tve_regs->tve_int_cont_reg);
+	u32 cd_cont_reg =
+		__raw_readl(tve.base + tve_regs->tve_cd_cont_reg);
+	u32 timeout = 40;
+
+	if ((cd_cont_reg & 0x1) == 0) {
+		pr_warning("Warning: pls enable TVE CD first!\n");
+		return tve.detect;
+	}
+
+	stat = __raw_readl(tve.base + tve_regs->tve_stat_reg);
+	while (((stat & CD_MON_END_INT) == 0) && (timeout > 0)) {
+		msleep(2);
+		timeout -= 2;
+		stat = __raw_readl(tve.base + tve_regs->tve_stat_reg);
+	}
+	if (((stat & CD_MON_END_INT) == 0) && (timeout <= 0)) {
+		pr_warning("Warning: get detect resultwithout CD_MON_END_INT!\n");
+		return tve.detect;
+	}
+
+	stat = stat >> tve_reg_fields->cd_ch_stat_offset;
+	stat_lm = stat & (CD_CH_0_LM_ST | CD_CH_1_LM_ST | CD_CH_2_LM_ST);
+	if ((stat_lm == (CD_CH_0_LM_ST | CD_CH_1_LM_ST | CD_CH_2_LM_ST)) &&
+		((stat & (CD_CH_0_SM_ST | CD_CH_1_SM_ST | CD_CH_2_SM_ST)) == 0)
+		) {
+			tve.detect = 3;
+			tve.output_mode = YPBPR;
+	} else if ((stat_lm == (CD_CH_0_LM_ST | CD_CH_1_LM_ST)) &&
+		((stat & (CD_CH_0_SM_ST | CD_CH_1_SM_ST)) == 0)) {
+			tve.detect = 4;
+			tve.output_mode = SVIDEO;
+	} else if (stat_lm == CD_CH_0_LM_ST) {
+		stat_sm = stat & CD_CH_0_SM_ST;
+		if (stat_sm != 0) {
+			/* headset */
+			tve.detect = 2;
+			tve.output_mode = TV_OFF;
+		} else {
+			tve.detect = 1;
+			tve.output_mode = CVBS0;
+		}
+	} else if (stat_lm == CD_CH_2_LM_ST) {
+		stat_sm = stat & CD_CH_2_SM_ST;
+		if (stat_sm != 0) {
+			/* headset */
+			tve.detect = 2;
+			tve.output_mode = TV_OFF;
+		} else {
+			tve.detect = 1;
+			tve.output_mode = CVBS2;
+		}
+	} else {
+		/* none */
+		tve.detect = 0;
+		tve.output_mode = TV_OFF;
+	}
+
+	tve_set_tvout_mode(tve.output_mode);
+
+	/* clear interrupt */
+	__raw_writel(CD_MON_END_INT | CD_LM_INT | CD_SM_INT,
+			tve.base + tve_regs->tve_stat_reg);
+
+	__raw_writel(int_ctl | CD_SM_INT | CD_LM_INT,
+			tve.base + tve_regs->tve_int_cont_reg);
+
+	if (old_detect != tve.detect)
+		sysfs_notify(&tve.pdev->dev.kobj, NULL, "headphone");
+
+	dev_dbg(&tve.pdev->dev, "detect = %d mode = %d\n",
+			tve.detect, tve.output_mode);
+	return tve.detect;
+}
+
+static void cd_work_func(struct work_struct *work)
+{
+	tve_update_detect_status();
+}
+
+static int tve_man_detect(void)
+{
+	u32 cd_cont;
+	u32 int_cont;
+
+	if (!enabled)
+		return -1;
+
+	int_cont = __raw_readl(tve.base + tve_regs->tve_int_cont_reg);
+	__raw_writel(int_cont &
+				~(tve_reg_fields->cd_sm_int | tve_reg_fields->cd_lm_int),
+				tve.base + tve_regs->tve_int_cont_reg);
+
+	cd_cont = __raw_readl(tve.base + tve_regs->tve_cd_cont_reg);
+	__raw_writel(cd_cont | tve_reg_fields->cd_trig_mode,
+				tve.base + tve_regs->tve_cd_cont_reg);
+
+	__raw_writel(tve_reg_fields->cd_sm_int | tve_reg_fields->
+			cd_lm_int | tve_reg_fields->
+			cd_mon_end_int | tve_reg_fields->cd_man_trig,
+			tve.base + tve_regs->tve_stat_reg);
+
+	while ((__raw_readl(tve.base + tve_regs->tve_stat_reg)
+		& tve_reg_fields->cd_mon_end_int) == 0)
+		msleep(5);
+
+	tve_update_detect_status();
+
+	__raw_writel(cd_cont, tve.base + tve_regs->tve_cd_cont_reg);
+	__raw_writel(int_cont, tve.base + tve_regs->tve_int_cont_reg);
+
+	return tve.detect;
+}
+
+static irqreturn_t tve_detect_handler(int irq, void *data)
+{
+	u32 int_ctl = __raw_readl(tve.base + tve_regs->tve_int_cont_reg);
+
+	/* disable INT first */
+	int_ctl &= ~(CD_SM_INT | CD_LM_INT | CD_MON_END_INT);
+	__raw_writel(int_ctl, tve.base + tve_regs->tve_int_cont_reg);
+
+	__raw_writel(CD_MON_END_INT | CD_LM_INT | CD_SM_INT,
+			tve.base + tve_regs->tve_stat_reg);
+
+	schedule_delayed_work(&tve.cd_work, msecs_to_jiffies(1000));
+
+	return IRQ_HANDLED;
+}
+
+int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
+{
+	struct fb_event *event = v;
+	struct fb_info *fbi = event->info;
+
+	switch (val) {
+	case FB_EVENT_FB_REGISTERED:
+		pr_debug("fb registered event\n");
+		if ((tve_fbi != NULL) || strcmp(fbi->fix.id, "DISP3 BG - DI1"))
+			break;
+
+		tve_fbi = fbi;
+		fb_add_videomode(&video_modes[0], &tve_fbi->modelist);
+		fb_add_videomode(&video_modes[1], &tve_fbi->modelist);
+		fb_add_videomode(&video_modes[2], &tve_fbi->modelist);
+		break;
+	case FB_EVENT_MODE_CHANGE:
+	{
+		struct fb_videomode cur_mode;
+		struct fb_videomode *mode;
+		struct list_head *pos;
+		struct fb_modelist *modelist;
+
+		if (tve_fbi != fbi)
+			break;
+
+		fb_var_to_videomode(&cur_mode, &fbi->var);
+
+		list_for_each(pos, &tve_fbi->modelist) {
+			modelist = list_entry(pos, struct fb_modelist, list);
+			mode = &modelist->mode;
+			if (fb_mode_is_equal(&cur_mode, mode)) {
+				fbi->mode = mode;
+				break;
+			}
+		}
+
+		if (!fbi->mode) {
+			tve_disable();
+			tve.cur_mode = TVOUT_FMT_OFF;
+			return 0;
+		}
+
+		pr_debug("fb mode change event: xres=%d, yres=%d\n",
+			 fbi->mode->xres, fbi->mode->yres);
+
+		tve_disable();
+
+		if (fb_mode_is_equal(fbi->mode, &video_modes[0])) {
+			tve_setup(TVOUT_FMT_NTSC);
+			tve_enable();
+		} else if (fb_mode_is_equal(fbi->mode, &video_modes[1])) {
+			tve_setup(TVOUT_FMT_PAL);
+			tve_enable();
+		} else if (fb_mode_is_equal(fbi->mode, &video_modes[2])) {
+			tve_setup(TVOUT_FMT_720P60);
+			tve_enable();
+		} else {
+			tve_setup(TVOUT_FMT_OFF);
+		}
+		break;
+	}
+	case FB_EVENT_BLANK:
+		if ((tve_fbi != fbi) || (fbi->mode == NULL))
+			return 0;
+
+		if (*((int *)event->data) == FB_BLANK_UNBLANK) {
+			if (fb_mode_is_equal(fbi->mode, &video_modes[0])) {
+				if (tve.cur_mode != TVOUT_FMT_NTSC) {
+					tve_disable();
+					tve_setup(TVOUT_FMT_NTSC);
+				}
+				tve_enable();
+			} else if (fb_mode_is_equal(fbi->mode,
+					&video_modes[1])) {
+				if (tve.cur_mode != TVOUT_FMT_PAL) {
+					tve_disable();
+					tve_setup(TVOUT_FMT_PAL);
+				}
+				tve_enable();
+			} else if (fb_mode_is_equal(fbi->mode,
+					&video_modes[2])) {
+				if (tve.cur_mode != TVOUT_FMT_720P60) {
+					tve_disable();
+					tve_setup(TVOUT_FMT_720P60);
+				}
+				tve_enable();
+			} else {
+				tve_setup(TVOUT_FMT_OFF);
+			}
+		} else
+			tve_disable();
+		break;
+	}
+	return 0;
+}
+
+static struct notifier_block nb = {
+	.notifier_call = tve_fb_event,
+};
+
+static ssize_t show_headphone(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int detect;
+
+	if (!enabled) {
+		strcpy(buf, "tve power off\n");
+		return strlen(buf);
+	}
+
+	detect = tve_update_detect_status();
+
+	if (detect == 0)
+		strcpy(buf, "none\n");
+	else if (detect == 1)
+		strcpy(buf, "cvbs\n");
+	else if (detect == 2)
+		strcpy(buf, "headset\n");
+	else if (detect == 3)
+		strcpy(buf, "component\n");
+	else
+		strcpy(buf, "svideo\n");
+
+	return strlen(buf);
+}
+
+static DEVICE_ATTR(headphone, S_IRUGO | S_IWUSR, show_headphone, NULL);
+
+static int _tve_get_revision(void)
+{
+	u32 conf_reg;
+	u32 rev = 0;
+
+	/* find out TVE rev based on the base addr default value
+	 * can be used at the init/probe ONLY */
+	conf_reg = __raw_readl(tve.base);
+	switch (conf_reg) {
+	case 0x00842000:
+		rev = 1;
+		break;
+	case 0x00100000:
+		rev = 2;
+		break;
+	}
+	return rev;
+}
+
+static int tve_probe(struct platform_device *pdev)
+{
+	int ret, i;
+	struct resource *res;
+	struct tve_platform_data *plat_data = pdev->dev.platform_data;
+	u32 conf_reg;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL)
+		return -ENOMEM;
+
+	tve.pdev = pdev;
+	tve.base = ioremap(res->start, res->end - res->start);
+
+	tve.irq = platform_get_irq(pdev, 0);
+	if (tve.irq < 0) {
+		ret = tve.irq;
+		goto err0;
+	}
+
+	ret = request_irq(tve.irq, tve_detect_handler, 0, pdev->name, pdev);
+	if (ret < 0)
+		goto err0;
+
+	ret = device_create_file(&pdev->dev, &dev_attr_headphone);
+	if (ret < 0)
+		goto err1;
+
+	for (i = 0; i < num_registered_fb; i++) {
+		if (strcmp(registered_fb[i]->fix.id, "DISP3 BG - DI1") == 0) {
+			tve_fbi = registered_fb[i];
+			break;
+		}
+	}
+
+	/* adjust video mode for mx37 */
+	if (cpu_is_mx37()) {
+		video_modes[0].left_margin = 121;
+		video_modes[0].right_margin = 16;
+		video_modes[0].upper_margin = 17;
+		video_modes[0].lower_margin = 5;
+		video_modes[1].left_margin = 131;
+		video_modes[1].right_margin = 12;
+		video_modes[1].upper_margin = 21;
+		video_modes[1].lower_margin = 3;
+	}
+
+	if (tve_fbi != NULL) {
+		fb_add_videomode(&video_modes[0], &tve_fbi->modelist);
+		fb_add_videomode(&video_modes[1], &tve_fbi->modelist);
+		fb_add_videomode(&video_modes[2], &tve_fbi->modelist);
+	}
+
+	tve.dac_reg = regulator_get(&pdev->dev, plat_data->dac_reg);
+	if (!IS_ERR(tve.dac_reg)) {
+		regulator_set_voltage(tve.dac_reg, 2500000, 2500000);
+		regulator_enable(tve.dac_reg);
+	}
+
+	tve.dig_reg = regulator_get(&pdev->dev, plat_data->dig_reg);
+	if (!IS_ERR(tve.dig_reg)) {
+		regulator_set_voltage(tve.dig_reg, 1250000, 1250000);
+		regulator_enable(tve.dig_reg);
+	}
+
+	tve.clk = clk_get(&pdev->dev, "tve_clk");
+	clk_set_rate(tve.clk, 216000000);
+	clk_enable(tve.clk);
+
+	tve.revision = _tve_get_revision();
+	if (tve.revision == 1) {
+		tve_regs = &tve_regs_v1;
+		tve_reg_fields = &tve_reg_fields_v1;
+	} else {
+		tve_regs = &tve_regs_v2;
+		tve_reg_fields = &tve_reg_fields_v2;
+	}
+
+	/* Setup cable detect, for YPrPb mode, default use channel#0 for Y */
+	INIT_DELAYED_WORK(&tve.cd_work, cd_work_func);
+	if (tve.revision == 1)
+		__raw_writel(0x01067701, tve.base + tve_regs->tve_cd_cont_reg);
+	else
+		__raw_writel(0x00770601, tve.base + tve_regs->tve_cd_cont_reg);
+
+	conf_reg = 0;
+	__raw_writel(conf_reg, tve.base + tve_regs->tve_com_conf_reg);
+
+	__raw_writel(0x00000000, tve.base + tve_regs->tve_mv_cont_reg - 4 * 5);
+	__raw_writel(0x00000000, tve.base + tve_regs->tve_mv_cont_reg - 4 * 4);
+	__raw_writel(0x00000000, tve.base + tve_regs->tve_mv_cont_reg - 4 * 3);
+	__raw_writel(0x00000000, tve.base + tve_regs->tve_mv_cont_reg - 4 * 2);
+	__raw_writel(0x00000000, tve.base + tve_regs->tve_mv_cont_reg - 4);
+	__raw_writel(0x00000000, tve.base + tve_regs->tve_mv_cont_reg);
+
+	clk_disable(tve.clk);
+
+	ret = fb_register_client(&nb);
+	if (ret < 0)
+		goto err2;
+
+	return 0;
+err2:
+	device_remove_file(&pdev->dev, &dev_attr_headphone);
+err1:
+	free_irq(tve.irq, pdev);
+err0:
+	iounmap(tve.base);
+	return ret;
+}
+
+static int tve_remove(struct platform_device *pdev)
+{
+	if (enabled) {
+		clk_disable(tve.clk);
+		enabled = 0;
+	}
+	free_irq(tve.irq, pdev);
+	device_remove_file(&pdev->dev, &dev_attr_headphone);
+	fb_unregister_client(&nb);
+	return 0;
+}
+
+/*!
+ * PM suspend/resume routing
+ */
+static int tve_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	if (enabled) {
+		__raw_writel(0, tve.base + tve_regs->tve_int_cont_reg);
+		__raw_writel(0, tve.base + tve_regs->tve_cd_cont_reg);
+		__raw_writel(0, tve.base + tve_regs->tve_com_conf_reg);
+		clk_disable(tve.clk);
+	}
+	return 0;
+}
+
+static int tve_resume(struct platform_device *pdev)
+{
+	if (enabled) {
+		clk_enable(tve.clk);
+
+		/* Setup cable detect */
+		if (tve.revision == 1)
+			__raw_writel(0x01067701,
+				tve.base + tve_regs->tve_cd_cont_reg);
+		else
+			__raw_writel(0x00770601,
+				tve.base + tve_regs->tve_cd_cont_reg);
+
+		if (tve.cur_mode == TVOUT_FMT_NTSC) {
+			tve_disable();
+			tve.cur_mode = TVOUT_FMT_OFF;
+			tve_setup(TVOUT_FMT_NTSC);
+		} else if (tve.cur_mode == TVOUT_FMT_PAL) {
+			tve_disable();
+			tve.cur_mode = TVOUT_FMT_OFF;
+			tve_setup(TVOUT_FMT_PAL);
+		} else if (tve.cur_mode == TVOUT_FMT_720P60) {
+			tve_disable();
+			tve.cur_mode = TVOUT_FMT_OFF;
+			tve_setup(TVOUT_FMT_720P60);
+		}
+		tve_enable();
+	}
+
+	return 0;
+}
+
+static struct platform_driver tve_driver = {
+	.driver = {
+		   .name = "tve",
+		   },
+	.probe = tve_probe,
+	.remove = tve_remove,
+	.suspend = tve_suspend,
+	.resume = tve_resume,
+};
+
+static int __init tve_init(void)
+{
+	return platform_driver_register(&tve_driver);
+}
+
+static void __exit tve_exit(void)
+{
+	platform_driver_unregister(&tve_driver);
+}
+
+module_init(tve_init);
+module_exit(tve_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("i.MX TV encoder driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/ns9360fb.c b/drivers/video/ns9360fb.c
new file mode 100644
index 000000000000..c6c31c458186
--- /dev/null
+++ b/drivers/video/ns9360fb.c
@@ -0,0 +1,336 @@
+/*
+ * drivers/video/ns9360fb.c
+ *
+ * Copyright (C) 2008 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/fb.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include <mach/regs-sys-ns9360.h>
+#include <mach/ns9360fb.h>
+#include <mach/gpio.h>
+
+#define LCD_TIMING(x)	((x) * 4)
+#define LCD_TIMING2_BCD		(1 << 26)
+#define LCD_TIMING2_PCD(x)	(((x) & 0x1F))
+#define LCD_UPBASE	0x10
+#define LCD_CONTROL	0x1c
+
+#define DRIVER_NAME	"ns9360fb"
+
+#define fb2nfb(fbi) container_of(fbi, struct ns9360fb_info, fb)
+struct ns9360fb_info {
+	struct fb_info		fb;
+
+	struct resource		*mem;
+	struct clk		*clk;
+	struct ns9360fb_pdata	*pdata;
+
+	void __iomem		*ioaddr;
+
+	dma_addr_t		fb_p;
+	unsigned char		*fb_v;
+	unsigned int		fb_size;
+
+	u32			pseudo_pal[16];
+};
+
+static void ns9360fb_select_video_clk_src(int source)
+{
+	u32 reset, clock;
+
+	reset = __raw_readl(SYS_RESET) & ~SYS_RESET_LCDC;
+	__raw_writel(reset, SYS_RESET);
+
+	clock = __raw_readl(SYS_CLOCK) & ~SYS_CLOCK_LPCSEXT;
+	if (!source) {
+		/* Use external clock */
+		if (!gpio_request(15, "ns9360fb-extclk")) {
+			gpio_configure_ns9360(15, 0, 0, 2);
+			clock |= SYS_CLOCK_LPCSEXT;
+		} else {
+			printk(KERN_ERR DRIVER_NAME
+			       ": unable to request gpio for external video clock\n");
+		}
+	}
+	__raw_writel(clock, SYS_CLOCK);
+
+	reset = __raw_readl(SYS_RESET) | SYS_RESET_LCDC;
+	__raw_writel(reset, SYS_RESET);
+}
+
+static int ns9360fb_set_par(struct fb_info *fb)
+{
+	fb->fix.visual = FB_VISUAL_TRUECOLOR;
+	fb->fix.line_length = fb->var.width * fb->var.bits_per_pixel / 8;
+
+	return 0;
+}
+
+static int ns9360fb_setcolreg(unsigned regno, unsigned red, unsigned green,
+		unsigned blue, unsigned transp, struct fb_info *fb)
+{
+	struct ns9360fb_info *info = fb2nfb(fb);
+
+	if (regno > info->fb.var.bits_per_pixel)
+		return -EINVAL;
+
+	info->pseudo_pal[regno] = (red & 0xF800) |
+	       ((green & 0xF800) >> 5) | ((green & 0xF800) >> 10);
+
+	return 0;
+}
+
+static struct fb_ops ns9360fb_ops = {
+	.owner = THIS_MODULE,
+	.fb_set_par = ns9360fb_set_par,
+	.fb_setcolreg = ns9360fb_setcolreg,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+};
+
+static int __init ns9360fb_probe(struct platform_device *pdev)
+{
+	struct ns9360fb_info *info;
+	struct ns9360fb_display *display;
+	char *option = NULL;
+	int ret, i;
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_dbg(&pdev->dev, "%s: err_alloc_info\n", __func__);
+		ret = -ENOMEM;
+		goto err_alloc_info;
+	}
+	platform_set_drvdata(pdev, info);
+
+	info->pdata = pdev->dev.platform_data;
+	if (!info->pdata) {
+		dev_dbg(&pdev->dev, "%s: err_pdata\n", __func__);
+		ret = -ENOENT;
+		goto err_pdata;
+	}
+
+	/* Get the display information */
+	if (!info->pdata->num_displays) {
+		dev_err(&pdev->dev, "no display information available\n");
+		ret = -EINVAL;
+		goto err_pdata;
+	}
+
+	if (info->pdata->num_displays > 1) {
+		/* 
+		 * If there are multiple displays, use the one specified
+		 * through the command line parameter vith following 
+		 * format video=displayfb:<display_name>
+		 */
+		if (fb_get_options("displayfb", &option)) {
+			dev_err(&pdev->dev, 
+				"no display information available in commnad line\n");
+			ret = -ENODEV;
+			goto err_pdata;
+		}
+		if (!option) {
+			ret = -ENODEV;
+			goto err_pdata;
+		}
+		dev_info(&pdev->dev, "display options: %s\n", option);
+
+		for (i = 0; i < info->pdata->num_displays; i++) {
+			if (!strcmp(option, info->pdata->displays[i].display_name))
+				info->pdata->display = &info->pdata->displays[i];
+		}
+	} else {
+		/* If there is only one, that is what we use */
+		info->pdata->display = info->pdata->displays;
+	}
+  
+	if ((display = info->pdata->display) == NULL) {
+		dev_err(&pdev->dev, "no display information available\n");
+		ret = -ENODEV;
+		goto err_pdata;
+	}
+
+	info->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!info->mem) {
+		dev_dbg(&pdev->dev, "%s: err_mem\n", __func__);
+		ret = -ENOENT;
+		goto err_mem;
+	}
+
+	if (!request_mem_region(info->mem->start,
+				info->mem->end - info->mem->start + 1,
+				DRIVER_NAME)) {
+		dev_dbg(&pdev->dev, "%s: err_req_mem\n", __func__);
+		ret = -EBUSY;
+		goto err_req_mem;
+	}
+
+	info->ioaddr = ioremap(info->mem->start,
+			info->mem->end - info->mem->start + 1);
+	if (info->ioaddr <= 0) {
+		dev_dbg(&pdev->dev, "%s: err_map_mem\n", __func__);
+		ret = -EBUSY;
+		goto err_map_mem;
+	}
+
+	strcpy(info->fb.fix.id, DRIVER_NAME);
+	info->fb.fix.type = FB_TYPE_PACKED_PIXELS;
+	info->fb.fix.accel = FB_ACCEL_NONE;
+
+	info->fb.flags = FBINFO_FLAG_DEFAULT;
+	info->fb.fbops = &ns9360fb_ops;
+	info->fb.pseudo_palette = &info->pseudo_pal;
+
+	info->fb.var.activate = FB_ACTIVATE_NOW;
+	info->fb.var.height = display->height;
+	info->fb.var.yres = display->height;
+	info->fb.var.yres_virtual = display->height;
+	info->fb.var.width = display->width;
+	info->fb.var.xres = display->width;
+	info->fb.var.xres_virtual = display->width;
+	info->fb.var.vmode = FB_VMODE_NONINTERLACED;
+	info->fb.var.bits_per_pixel = 16;
+
+	info->fb.var.red.offset = 10;
+	info->fb.var.green.offset = 5;
+	info->fb.var.blue.offset = 0;
+	info->fb.var.red.length = 5;
+	info->fb.var.green.length = 5;
+	info->fb.var.blue.length = 5;
+
+	info->fb.fix.smem_len = info->fb.var.xres * info->fb.var.yres *
+		info->fb.var.bits_per_pixel / 8;
+
+	pdev->dev.coherent_dma_mask = (u32)-1;
+
+	info->fb_size = PAGE_ALIGN(info->fb.fix.smem_len + PAGE_SIZE);
+	info->fb_v = dma_alloc_writecombine(&pdev->dev, info->fb_size,
+			&info->fb_p, GFP_KERNEL);
+	if (!info->fb_v) {
+		dev_dbg(&pdev->dev, "%s: err_alloc_fb\n", __func__);
+		ret = -ENOMEM;
+		goto err_alloc_fb;
+	}
+
+	info->fb.screen_base = info->fb_v + PAGE_SIZE;
+	info->fb.fix.smem_start = info->fb_p + PAGE_SIZE;
+
+	info->clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(info->clk)) {
+		dev_dbg(&pdev->dev, "%s: err_clk_get\n", __func__);
+		ret = PTR_ERR(info->clk);
+		goto err_clk_get;
+	}
+
+	memset(info->fb.screen_base, 0, info->fb.fix.smem_len);
+
+	ret = clk_enable(info->clk);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_clk_enable\n", __func__);
+		goto err_clk_enable;
+	}
+
+	/* display->clock = 0 means use external source */
+	ns9360fb_select_video_clk_src(display->clock);
+
+	if (!(display->timing[2] & LCD_TIMING2_BCD)) {
+		display->timing[2] |=
+			LCD_TIMING2_PCD((clk_get_rate(info->clk) /
+					display->clock) - 1);
+	}
+
+	/* write configuration to video controller */
+	for (i = 0; i < 4; i++)
+		iowrite32(display->timing[i],
+				info->ioaddr + LCD_TIMING(i));
+
+	iowrite32(info->fb.fix.smem_start, info->ioaddr + LCD_UPBASE);
+	iowrite32(display->control, info->ioaddr + LCD_CONTROL);
+
+	ret = register_framebuffer(&info->fb);
+	if (ret) {
+		dev_dbg(&pdev->dev, "%s: err_reg_fb\n", __func__);
+		goto err_reg_fb;
+	}
+
+	dev_info(&pdev->dev, "fb mapped to 0x%p (0x%p), display config %s\n",
+			(void *)info->fb.fix.smem_start, info->fb.screen_base,
+			display->display_name);
+
+	return 0;
+
+err_reg_fb:
+	clk_disable(info->clk);
+err_clk_enable:
+	clk_put(info->clk);
+err_clk_get:
+	dma_free_writecombine(&pdev->dev, info->fb_size,
+			info->fb_v, info->fb_p);
+err_alloc_fb:
+	iounmap(info->ioaddr);
+err_map_mem:
+	release_mem_region(info->mem->start,
+			info->mem->end - info->mem->start + 1);
+err_req_mem:
+err_mem:
+err_pdata:
+	kfree(info);
+err_alloc_info:
+
+	return ret;
+}
+
+static int ns9360fb_remove(struct platform_device *pdev)
+{
+	struct ns9360fb_info *info = platform_get_drvdata(pdev);
+
+	clk_disable(info->clk);
+	clk_put(info->clk);
+
+	dma_free_writecombine(&pdev->dev, info->fb_size,
+			info->fb_v, info->fb_p);
+
+	iounmap(info->ioaddr);
+	release_mem_region(info->mem->start,
+			info->mem->end - info->mem->start + 1);
+
+	kfree(info);
+	return 0;
+}
+
+static struct platform_driver ns9360fb_driver = {
+	.probe		= ns9360fb_probe,
+	.remove		= ns9360fb_remove,
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __devinit ns9360fb_init(void)
+{
+	return platform_driver_register(&ns9360fb_driver);
+}
+
+static void __exit ns9360fb_cleanup(void)
+{
+	platform_driver_unregister(&ns9360fb_driver);
+}
+
+module_init(ns9360fb_init);
+module_exit(ns9360fb_cleanup);
+
+MODULE_AUTHOR("Matthias Ludwig");
+MODULE_DESCRIPTION("Driver for Digi ns9360 SoC framebuffer");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/s3c2410fb_tft.c b/drivers/video/s3c2410fb_tft.c
new file mode 100644
index 000000000000..8632beb68a7e
--- /dev/null
+++ b/drivers/video/s3c2410fb_tft.c
@@ -0,0 +1,1008 @@
+/* -*- linux-c -*-
+ * 
+ * linux/drivers/video/s3cfb.c
+ *
+ * $Id: s3cfb.c,v 1.63 2007/07/12 05:26:04 yreom Exp $
+ *
+ * Revision 1.16  2006/09/14 04:45:15  ihlee215
+ * OSD support added
+ *
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file COPYING in the main directory of this archive for
+ * more details.
+ *
+ *	    S3C LCD Controller Frame Buffer Driver
+ *	    based on skeletonfb.c, sa1100fb.c
+ *
+ */
+
+#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/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/wait.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <asm/div64.h>
+#include <linux/math64.h>
+
+#include <asm/mach/map.h>
+/* #include <asm/arch/registers.h> */
+#include <mach/idle.h>
+#include <mach/fb.h>
+#include <mach/regs-gpio.h>
+
+#include "s3c2410fb.h"
+
+
+#ifdef CONFIG_PM
+#include <linux/pm.h>
+#endif
+
+
+
+
+#define printk_err(fmt, args...)                printk(KERN_ERR "[ ERROR ] s3cfb: " fmt, ## args)
+#define printk_info(fmt, args...)               printk(KERN_INFO "[ INFO ] s3cfb: " fmt, ## args)
+
+
+#if 1
+#define S3CFB_DEBUG
+#endif
+
+#ifdef S3CFB_DEBUG
+#  define printk_debug(fmt, args...)            printk(KERN_DEBUG "s3cfb: " fmt, ## args)
+#else
+#  define printk_debug(fmt, args...)
+#endif
+
+
+
+#define S3CFB_DRIVER_NAME			"s3c2410fb-tft"
+
+
+static int s3c2410fb_check_var(struct fb_var_screeninfo *var,
+			       struct fb_info *info)
+{
+	struct s3c2410fb_info *fbi = info->par;
+	struct s3c2410fb_mach_info *mach_info = fbi->dev->platform_data;
+	struct s3c2410fb_display *display = NULL;
+	struct s3c2410fb_display *default_display = mach_info->displays +
+		mach_info->default_display;
+	int type = default_display->type;
+	unsigned i;
+
+	printk_debug("Calling %s: var=%p, info=%p\n", __func__, var, info);
+
+	/* validate x/y resolution */
+	/* choose default mode if possible */
+	if (var->yres == default_display->yres &&
+	    var->xres == default_display->xres &&
+	    var->bits_per_pixel == default_display->bpp)
+		display = default_display;
+	else
+		for (i = 0; i < mach_info->num_displays; i++)
+			if (type == mach_info->displays[i].type &&
+			    var->yres == mach_info->displays[i].yres &&
+			    var->xres == mach_info->displays[i].xres &&
+			    var->bits_per_pixel == mach_info->displays[i].bpp) {
+				display = mach_info->displays + i;
+				break;
+			}
+
+	if (!display) {
+		printk_err("wrong resolution or depth %dx%d at %d bpp\n",
+			   var->xres, var->yres, var->bits_per_pixel);
+		return -EINVAL;
+	}
+
+	/* it is always the size as the display */
+	var->xres_virtual = display->xres;
+	var->yres_virtual = display->yres;
+	var->height = display->height;
+	var->width = display->width;
+
+	/* copy lcd settings */
+	var->pixclock = display->pixclock;
+	var->left_margin = display->left_margin;
+	var->right_margin = display->right_margin;
+	var->upper_margin = display->upper_margin;
+	var->lower_margin = display->lower_margin;
+	var->vsync_len = display->vsync_len;
+	var->hsync_len = display->hsync_len;
+
+	/*
+	 * If using the Power Signal then request the GPIO
+	 * (@XXX: Use the correct request and configuration function for the GPIO)
+	 * Luis Galdos
+	 */
+	if (display->lcdcon5 & S3C2410_LCDCON5_PWREN) {
+		printk_info("Configuring the power LED GPIO\n");
+		s3c2410_gpio_cfgpin(S3C2410_GPG4, S3C2410_GPG4_LCDPWREN);
+	}
+	
+	var->transp.offset = 0;
+	var->transp.length = 0;
+	/* set r/g/b positions */
+	switch (var->bits_per_pixel) {
+	case 1:
+	case 2:
+	case 4:
+		var->red.offset	= 0;
+		var->red.length	= var->bits_per_pixel;
+		var->green	= var->red;
+		var->blue	= var->red;
+		break;
+	case 8:
+		/* 8 bpp 332 */
+		var->red.length		= 3;
+		var->red.offset		= 5;
+		var->green.length	= 3;
+		var->green.offset	= 2;
+		var->blue.length	= 2;
+		var->blue.offset	= 0;
+		break;
+	case 12:
+		/* 12 bpp 444 */
+		var->red.length		= 4;
+		var->red.offset		= 8;
+		var->green.length	= 4;
+		var->green.offset	= 4;
+		var->blue.length	= 4;
+		var->blue.offset	= 0;
+		break;
+	case 16:
+		/* 16 bpp, 565 format */
+		if (S3C24XX_LCD_WINCON_BPP(display->wincon0) ==
+		    S3C24XX_LCD_WINCON_16BPP_565) {
+			/* 16 bpp, 565 format */
+			var->red.offset		= 11;
+			var->green.offset	= 5;
+			var->blue.offset	= 0;
+			var->red.length		= 5;
+			var->green.length	= 6;
+			var->blue.length	= 5;
+		} else {
+			/* 16 bpp, 1555 format */
+			var->red.offset		= 10;
+			var->green.offset	= 5;
+			var->blue.offset	= 0;
+			var->red.length		= 5;
+			var->green.length	= 5;
+			var->blue.length	= 5;
+		}
+		break;
+	case 18:
+		/* 18bpp only supports 666 */
+		var->red.offset		= 12;
+		var->green.offset	= 6;
+		var->blue.offset	= 0;
+		var->red.length		= 6;
+		var->green.length	= 6;
+		var->blue.length	= 6;
+		break;
+	case 32:
+		/* 24 bpp 888 and 8 dummy */
+		var->red.length		= 8;
+		var->red.offset		= 16;
+		var->green.length	= 8;
+		var->green.offset	= 8;
+		var->blue.length	= 8;
+		var->blue.offset	= 0;
+		break;
+	}
+	return 0;
+}
+
+
+#if 0
+static void s3cfb_dummy_values(struct s3c2410fb_info *fbi)
+{
+
+	writel(0x10415, fbi->io + S3C24XX_LCD_WINCON0);
+	writel(0x00, fbi->io + S3C24XX_LCD_WINCON1);
+	
+	writel(0x173, fbi->io + S3C24XX_LCD_VIDCON0);
+	writel(0x195D060, fbi->io + S3C24XX_LCD_VIDCON1);
+	
+	writel(0x1F0203, fbi->io + S3C24XX_LCD_VIDTCON0);
+	writel(0x40618, fbi->io + S3C24XX_LCD_VIDTCON1);
+	writel(0xEFA7F, fbi->io + S3C24XX_LCD_VIDTCON2);
+	
+	writel(0x00, fbi->io + S3C24XX_LCD_VIDOSD0A);
+	writel(0x13F9DF, fbi->io + S3C24XX_LCD_VIDOSD0B);
+}
+#endif /* 0: Only for testing */
+
+
+/* Write into the register the passed display configuration */
+static void s3c2410fb_activate_var(struct fb_info *info)
+{
+	struct s3c2410fb_info *fbi;
+	unsigned short hsync_cnt, vclk_cnt;
+	unsigned char clkval;
+	struct s3c2410fb_display *display;
+	struct s3c2410fb_mach_info *mach_info;
+	unsigned long pixel_clk, lcd_clk;
+	unsigned long vidcon0, vidcon1, vidtcon0, vidtcon1, vidtcon2;
+	unsigned long wincon0, wincon1;
+	unsigned long vidosd0a, vidosd0b;
+	
+	printk_debug("Calling %s\n", __func__);
+	
+	fbi = info->par;
+	mach_info = fbi->dev->platform_data;
+	display = mach_info->displays + mach_info->default_display;
+	
+	/* Write the configuration into the register VIDCON0 */
+	hsync_cnt = display->lower_margin + /* VBPD */
+		display->upper_margin +     /* VFPD */
+		display->vsync_len +        /* VSPW */
+		display->height;            /* LINEVAL */
+
+	vclk_cnt = display->right_margin +  /* HBPD */
+		display->left_margin +      /* HFPD */
+		display->hsync_len +        /* HSPW */
+		display->width;             /* HOZVAL */
+
+	pixel_clk = (display->frame_rate * vclk_cnt *  hsync_cnt);
+	lcd_clk = clk_get_rate(fbi->clk);
+
+	/*
+	 * @FIXME: The U-Boot has another clock calculation. See under:
+	 * cpu/s3c24xx/s3c2443/fb.c
+	 */
+        clkval = (lcd_clk / pixel_clk) - 1;
+	printk_debug("Calculated CLKVAL is 0x%x (pixel clk: %lu | lcd clk %lu)\n",
+		     clkval, pixel_clk, lcd_clk);
+	
+	vidcon1 = readl(fbi->io + S3C24XX_LCD_VIDCON1);
+	vidcon0 = readl(fbi->io + S3C24XX_LCD_VIDCON0);
+	wincon1 = readl(fbi->io + S3C24XX_LCD_WINCON1);
+	
+	/* Configure the clock */
+	vidcon0 |= (display->vidcon0 | S3C24XX_LCD_VIDCON0_CLKVAL(clkval));
+
+	vidcon1 |= display->vidcon1;
+	
+	vidtcon0 = S3C24XX_LCD_VIDTCON0_VSPW(display->vsync_len - 1) |
+		S3C24XX_LCD_VIDTCON0_VFPD(display->upper_margin - 1) |
+		S3C24XX_LCD_VIDTCON0_VBPD(display->lower_margin - 1);
+
+	vidtcon1 = S3C24XX_LCD_VIDTCON1_HSPW(display->hsync_len - 1) |
+		S3C24XX_LCD_VIDTCON1_HFPD(display->left_margin - 1) |
+		S3C24XX_LCD_VIDTCON1_HBPD(display->right_margin - 1);
+
+	
+	vidtcon2 = S3C24XX_LCD_VIDTCON2_HOZVAL(display->width - 1) |
+		S3C24XX_LCD_VIDTCON2_LINEVAL(display->height - 1);
+
+	/* Write the user configuration too */
+	wincon0 = (display->wincon0 |
+		   S3C24XX_LCD_WINCON0_ENWIN_F |
+		   S3C24XX_LCD_WINCON0_HAWSWP |
+		   S3C24XX_LCD_WINCON0_4WBURST);
+
+	
+	vidosd0a = 0x00;
+	vidosd0b = S3C24XX_LCD_VIDOSD0B_RIGHT_X(display->width - 1) |
+		S3C24XX_LCD_VIDOSD0B_RIGHT_Y(display->height - 1);
+
+	
+	/* And now write the configuration into the corresponding registers */
+	writel(vidcon0, fbi->io + S3C24XX_LCD_VIDCON0);
+	writel(vidcon1, fbi->io + S3C24XX_LCD_VIDCON1);
+	writel(vidtcon0, fbi->io + S3C24XX_LCD_VIDTCON0);
+	writel(vidtcon1, fbi->io + S3C24XX_LCD_VIDTCON1);
+	writel(vidtcon2, fbi->io + S3C24XX_LCD_VIDTCON2);
+	writel(wincon0, fbi->io + S3C24XX_LCD_WINCON0);
+	writel(vidosd0a, fbi->io + S3C24XX_LCD_VIDOSD0A);
+	writel(vidosd0b, fbi->io + S3C24XX_LCD_VIDOSD0B);
+
+	/* These are the configuration addresses for the frame buffer */
+	writel(info->fix.smem_start, fbi->io + S3C24XX_LCD_VIDW00ADD0B0);
+	writel(info->fix.smem_start + info->fix.smem_len,
+	       fbi->io + S3C24XX_LCD_VIDW00ADD1B0);
+		
+	printk_debug("vidcon0 0x%08x | vidcon1 0x%08x\n",
+		     (unsigned int)vidcon0,
+		     (unsigned int)vidcon1);
+	printk_debug("vidtcon0 0x%08x | vidtcon1 0x%08x | vidtcon2 0x%08x\n",
+		     (unsigned int)vidtcon0,
+		     (unsigned int)vidtcon1,
+		     (unsigned int)vidtcon2);
+}
+
+
+/* Write the display configuration to the hardware */
+static int s3c2410fb_set_par(struct fb_info *info)
+{
+	struct fb_var_screeninfo *var = &info->var;
+
+	printk_debug("Calling %s\n", __func__);
+
+	switch (var->bits_per_pixel) {
+	case 32:
+	case 16:
+	case 12:
+		info->fix.visual = FB_VISUAL_TRUECOLOR;
+		break;
+	case 1:
+		info->fix.visual = FB_VISUAL_MONO01;
+		break;
+	default:
+		info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
+		break;
+	}
+
+	
+	info->fix.line_length = (var->xres_virtual * var->bits_per_pixel) / 8;
+	printk_debug("Line length is %i\n", info->fix.line_length);
+
+	
+	/* activate this new configuration */
+	s3c2410fb_activate_var(info);
+	return 0;
+}
+
+
+static void s3c2410fb_lcd_enable(struct s3c2410fb_info *fbi, int enable)
+{
+	unsigned long flags;
+	unsigned long vidcon0;
+	
+	local_irq_save(flags);
+
+	printk_debug("%s the TFT LCD\n", enable ? "Enabling" : "Disabling");
+
+	vidcon0 = readl(fbi->io + S3C24XX_LCD_VIDCON0);
+	if (enable)
+		vidcon0 |= (S3C24XX_LCD_VIDCON0_ENVID | S3C24XX_LCD_VIDCON0_ENVID_F);
+	else
+		vidcon0 &= ~(S3C24XX_LCD_VIDCON0_ENVID | S3C24XX_LCD_VIDCON0_ENVID_F);
+
+	writel(vidcon0, fbi->io + S3C24XX_LCD_VIDCON0);
+	local_irq_restore(flags);
+}
+
+
+
+static int s3c2410fb_blank(int blank_mode, struct fb_info *info)
+{
+	struct s3c2410fb_info *fbi = info->par;
+
+	printk_debug("Blank (mode=%d, info=%p, power down=%i)\n",
+		     blank_mode, info, FB_BLANK_POWERDOWN);
+
+	if (blank_mode == FB_BLANK_POWERDOWN) {
+		s3c2410fb_lcd_enable(fbi, 0);
+	} else {
+		s3c2410fb_lcd_enable(fbi, 1);
+	}
+
+	return 0;
+}
+
+static void schedule_palette_update(struct s3c2410fb_info *fbi,
+				    unsigned int regno, unsigned int val)
+{
+	unsigned long flags;
+	unsigned long irqen;
+	void __iomem *irq_base = fbi->irq_base;
+
+	printk_debug("Calling %s\n", __func__);
+	
+	local_irq_save(flags);
+
+	fbi->palette_buffer[regno] = val;
+
+	if (!fbi->palette_ready) {
+		fbi->palette_ready = 1;
+		/* enable IRQ */
+		irqen = readl(irq_base + S3C24XX_LCDINTMSK);
+		irqen &= ~S3C2410_LCDINT_FRSYNC;
+		writel(irqen, irq_base + S3C24XX_LCDINTMSK);
+	}
+
+	local_irq_restore(flags);
+}
+
+
+/* from pxafb.c */
+static inline unsigned int chan_to_field(unsigned int chan,
+					 struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+
+
+static int s3c2410fb_setcolreg(unsigned regno,
+			       unsigned red, unsigned green, unsigned blue,
+			       unsigned transp, struct fb_info *info)
+{
+	struct s3c2410fb_info *fbi = info->par;
+	void __iomem *regs = fbi->io;
+	unsigned int val;
+
+
+	switch (info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		/* true-colour, use pseudo-palette */
+		if (regno < 16) {
+			u32 *pal = info->pseudo_palette;
+
+			val  = chan_to_field(red,   &info->var.red);
+			val |= chan_to_field(green, &info->var.green);
+			val |= chan_to_field(blue,  &info->var.blue);
+
+			pal[regno] = val;
+		}
+		break;
+
+	case FB_VISUAL_PSEUDOCOLOR:
+		if (regno < 256) {
+			/* currently assume RGB 5-6-5 mode */
+
+			val  = (red   >>  0) & 0xf800;
+			val |= (green >>  5) & 0x07e0;
+			val |= (blue  >> 11) & 0x001f;
+
+			writel(val, regs + S3C2410_TFTPAL(regno));
+			schedule_palette_update(fbi, regno, val);
+		}
+
+		break;
+
+	default:
+		return 1;	/* unknown type */
+	}
+
+	return 0;
+}
+
+
+
+/* Function called when the frame buffer device (/dev/fbX) is being opened */
+static int s3c2410fb_tft_open(struct fb_info *info, int user)
+{
+	struct s3c2410fb_info *fbi;
+
+	fbi = info->par;
+	
+	printk_debug("Calling %s\n", __func__);
+	s3c2410fb_lcd_enable(fbi, 1);
+	return 0;
+}
+
+
+static int s3c2410fb_tft_release(struct fb_info *info, int user)
+{
+	struct s3c2410fb_info *fbi;
+
+	fbi = info->par;
+	printk_debug("Calling %s\n", __func__);
+	s3c2410fb_lcd_enable(fbi, 0);
+	return 0;
+}
+
+
+static int s3c2410fb_tft_pan(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+
+
+
+	return 0;
+}
+
+/* These are the available device operations */
+static struct fb_ops s3cfb_ops = {
+	.owner		= THIS_MODULE,
+	.fb_check_var	= s3c2410fb_check_var,
+	.fb_set_par	= s3c2410fb_set_par,
+	.fb_blank	= s3c2410fb_blank,
+	.fb_setcolreg	= s3c2410fb_setcolreg,
+	.fb_fillrect	= cfb_fillrect,
+	.fb_copyarea	= cfb_copyarea,
+	.fb_imageblit	= cfb_imageblit,
+	.fb_open	= s3c2410fb_tft_open,
+	.fb_release	= s3c2410fb_tft_release,
+	.fb_pan_display = s3c2410fb_tft_pan,
+};
+
+
+
+static irqreturn_t s3cfb_irq(int irq, void *dev_id)
+{
+	printk_info("IRQ of the TFT LCD enabled?\n");
+/* 	struct s3c2410fb_info *fbi = dev_id; */
+/* 	void __iomem *irq_base = fbi->irq_base; */
+/* 	unsigned long lcdirq = readl(irq_base + S3C24XX_LCDINTPND); */
+  
+/* 	if (lcdirq & S3C2410_LCDINT_FRSYNC) { */
+/* 		if (fbi->palette_ready) */
+/* 			s3c2410fb_write_palette(fbi); */
+
+/* 		writel(S3C2410_LCDINT_FRSYNC, irq_base + S3C24XX_LCDINTPND); */
+/* 		writel(S3C2410_LCDINT_FRSYNC, irq_base + S3C24XX_LCDSRCPND); */
+/* 	} */
+
+	return IRQ_HANDLED;
+}
+
+
+/*
+ * s3c2410fb_map_video_memory():
+ *	Allocates the DRAM memory for the frame buffer.  This buffer is
+ *	remapped into a non-cached, non-buffered, memory region to
+ *	allow palette and pixel writes to occur without flushing the
+ *	cache.  Once this area is remapped, all virtual memory
+ *	access to the video memory should occur at the new region.
+ */
+static int __init s3c2410fb_map_video_memory(struct fb_info *info)
+{
+	struct s3c2410fb_info *fbi = info->par;
+	dma_addr_t map_dma;
+	unsigned map_size = PAGE_ALIGN(info->fix.smem_len);
+
+	printk_debug("map_video_memory(fbi=%p) map_size %u\n", fbi, map_size);
+
+	info->screen_base = dma_alloc_writecombine(fbi->dev, map_size,
+						   &map_dma, GFP_KERNEL);
+
+	if (info->screen_base) {
+		/* prevent initial garbage on screen */
+		printk_debug("map_video_memory: clear %p:%08x\n",
+			     info->screen_base, map_size);
+		memset(info->screen_base, 0x00, map_size);
+
+		info->fix.smem_start = map_dma;
+		info->fix.smem_len = map_size;
+
+		printk_debug("map_video_memory: dma=%08lx cpu=%p size=%08x\n",
+			     info->fix.smem_start, info->screen_base, map_size);
+	}
+
+	return info->screen_base ? 0 : -ENOMEM;
+}
+
+
+
+static inline void modify_gpio(void __iomem *reg,
+			       unsigned long set, unsigned long mask)
+{
+	unsigned long tmp;
+
+	tmp = readl(reg) & ~mask;
+	writel(tmp | set, reg);
+}
+
+
+
+static int s3c2410fb_init_registers(struct fb_info *info)
+{
+	struct s3c2410fb_info *fbi = info->par;
+	struct s3c2410fb_mach_info *mach_info = fbi->dev->platform_data;
+	unsigned long flags;
+
+	/* Initialise LCD with values from haret */
+
+	local_irq_save(flags);
+
+	/* modify the gpio(s) with interrupts set (bjd) */
+
+	modify_gpio(S3C2410_GPCUP,  mach_info->gpcup,  mach_info->gpcup_mask);
+	modify_gpio(S3C2410_GPCCON, mach_info->gpccon, mach_info->gpccon_mask);
+	modify_gpio(S3C2410_GPDUP,  mach_info->gpdup,  mach_info->gpdup_mask);
+	modify_gpio(S3C2410_GPDCON, mach_info->gpdcon, mach_info->gpdcon_mask);
+
+	local_irq_restore(flags);
+
+/* 	printk_debug("LPCSEL    = 0x%08lx\n", mach_info->lpcsel); */
+/* 	writel(mach_info->lpcsel, lpcsel); */
+
+/* 	printk_debug("replacing TPAL %08x\n", readl(tpal)); */
+
+	/* ensure temporary palette disabled */
+/* 	writel(0x00, tpal); */
+
+	return 0;
+}
+
+
+
+static inline void s3c2410fb_unmap_video_memory(struct fb_info *info)
+{
+	struct s3c2410fb_info *fbi = info->par;
+
+	dma_free_writecombine(fbi->dev, PAGE_ALIGN(info->fix.smem_len),
+			      info->screen_base, info->fix.smem_start);
+}
+
+
+
+int __init s3c_fb_probe(struct platform_device *pdev)
+{
+	struct s3c2410fb_info *info;
+        struct s3c2410fb_display *display;
+	struct fb_info *fbinfo;
+	struct s3c2410fb_mach_info *mach_info;
+	struct resource *res;
+	int ret;
+	int irq;
+        int i;
+        int size;
+        u32 vidcon0;
+
+	/* @XXX: Use a higher probe for setting the type */
+	enum s3c_drv_type drv_type = DRV_S3C2410;
+
+	printk_debug("Probing a new device ID %i\n", pdev->id);
+
+	
+	/* Get the user defined LCD controller configuration */
+	mach_info = pdev->dev.platform_data;
+	if (mach_info == NULL) {
+                printk_err("No platform data for lcd, cannot attach\n");
+                return -EINVAL;
+        }
+
+	if (mach_info->default_display >= mach_info->num_displays) {
+                printk_err("Default is %d but only %d displays\n",
+			   mach_info->default_display, mach_info->num_displays);
+                return -EINVAL;
+        }
+
+	/* Get the user defined display configuration */
+        display = mach_info->displays + mach_info->default_display;
+
+	/* Get the IRQ for the display controller */
+        irq = platform_get_irq(pdev, 0);
+        if (irq < 0) {
+                printk_err("No irq for device.\n");
+                return -ENOENT;
+        }
+
+	/* Allocate the frame buffer for this device */
+	fbinfo = framebuffer_alloc(sizeof(struct s3c2410fb_info), &pdev->dev);
+        if (!fbinfo)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, fbinfo);
+
+	/* Use the private data for our internal settings */
+        info = fbinfo->par;	
+        info->dev = &pdev->dev;
+        info->drv_type = drv_type;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (res == NULL) {
+                dev_err(&pdev->dev, "failed to get memory registers\n");
+                ret = -ENXIO;
+                goto dealloc_fb;
+        }
+
+        size = (res->end - res->start) + 1;
+        info->mem = request_mem_region(res->start, size, pdev->name);
+        if (info->mem == NULL) {
+                dev_err(&pdev->dev, "failed to get memory region\n");
+                ret = -ENOENT;
+                goto dealloc_fb;
+        }
+
+        info->io = ioremap(res->start, size);
+        if (info->io == NULL) {
+                dev_err(&pdev->dev, "ioremap() of registers failed\n");
+                ret = -ENXIO;
+                goto release_mem;
+        }
+
+	info->irq_base = info->io +
+		((drv_type == DRV_S3C2412) ? S3C2412_LCDINTBASE : S3C2410_LCDINTBASE);
+
+	/* @XXX: Do we really need the below code? */
+	strcpy(fbinfo->fix.id, S3CFB_DRIVER_NAME);
+
+        /* Stop the video first */
+        vidcon0 = readl(info->io + S3C24XX_LCD_VIDCON0);
+	vidcon0 &= (S3C24XX_LCD_VIDCON0_ENVID | S3C24XX_LCD_VIDCON0_ENVID_F);
+	writel(vidcon0, info->io + S3C24XX_LCD_VIDCON0);
+
+	/* Start the initial fix configuration */
+	fbinfo->fix.type	    = FB_TYPE_PACKED_PIXELS;
+	fbinfo->fix.type_aux	    = 0;
+	fbinfo->fix.xpanstep	    = 0;
+	fbinfo->fix.ypanstep	    = 0;
+	fbinfo->fix.ywrapstep	    = 0;
+	fbinfo->fix.accel	    = FB_ACCEL_NONE;
+
+	fbinfo->var.nonstd	    = 0;
+	fbinfo->var.activate	    = FB_ACTIVATE_NOW;
+	fbinfo->var.accel_flags     = 0;
+	fbinfo->var.vmode	    = FB_VMODE_NONINTERLACED;
+
+	fbinfo->fbops		    = &s3cfb_ops;
+	fbinfo->flags		    = FBINFO_FLAG_DEFAULT;
+	fbinfo->pseudo_palette      = &info->pseudo_pal;
+
+	for (i = 0; i < 256; i++)
+		info->palette_buffer[i] = PALETTE_BUFF_CLEAR;
+
+	/* Request our IRQ */
+	ret = request_irq(irq, s3cfb_irq, IRQF_DISABLED, pdev->name, info);
+	if (ret) {
+		printk_err("Cannot get irq %d - err %d\n", irq, ret);
+		ret = -EBUSY;
+		goto release_regs;
+	}
+
+	/*
+	 * We are using the HCLK, then the U-Boot is using this clock too
+	 */
+	if (display->vidcon0 & S3C24XX_LCD_VIDCON0_CLKSEL_LCD) {
+		printk_err("Invalid clock (only tested with the HCLK).\n");
+		ret = -EINVAL;
+		goto release_irq;
+	}
+
+	/* @FIXME: Select the clock depending on the passed display configuration */
+	info->clk = clk_get(NULL, "hclk");
+	if (!info->clk || IS_ERR(info->clk)) {
+		printk_err("Failed to get lcd clock source\n");
+		ret = -ENOENT;
+		goto release_irq;
+	}
+
+	clk_enable(info->clk);
+	printk_debug("Got and enabled the HCLK clock\n");
+
+	msleep(1);
+
+	/* find maximum required memory size for display */
+	for (i = 0; i < mach_info->num_displays; i++) {
+		unsigned long smem_len = mach_info->displays[i].xres;
+
+		smem_len *= mach_info->displays[i].yres;
+		smem_len *= mach_info->displays[i].bpp;
+		smem_len >>= 3;
+		if (fbinfo->fix.smem_len < smem_len)
+			fbinfo->fix.smem_len = smem_len;
+	}
+
+	/* Initialize video memory */
+	ret = s3c2410fb_map_video_memory(fbinfo);
+	if (ret) {
+		printk_err("Failed to allocate video RAM: %d\n", ret);
+		ret = -ENOMEM;
+		goto release_clock;
+	}
+
+
+	fbinfo->var.xres = display->xres;
+	fbinfo->var.yres = display->yres;
+	fbinfo->var.bits_per_pixel = display->bpp;
+
+
+	s3c2410fb_init_registers(fbinfo);
+
+
+	s3c2410fb_check_var(&fbinfo->var, fbinfo);
+	
+
+	ret = register_framebuffer(fbinfo);
+	if (ret) {
+		printk_err("Failed to register the frambuffer device, %i\n", ret);
+		goto free_video_memory;
+	}
+
+	
+	printk_info("New frame buffer fb%d: %s frame buffer device\n",
+		    fbinfo->node, fbinfo->fix.id);
+	return 0;
+
+ free_video_memory:
+	s3c2410fb_unmap_video_memory(fbinfo);
+
+ release_clock:
+	clk_disable(info->clk);
+	clk_put(info->clk);
+
+ release_irq:
+	free_irq(irq, info);
+	
+ release_regs:
+	iounmap(info->io);
+
+ release_mem:
+        release_resource(info->mem);
+        kfree(info->mem);
+
+ dealloc_fb:
+        platform_set_drvdata(pdev, NULL);
+        framebuffer_release(fbinfo);
+	
+        return ret;
+}
+
+/* s3c_fb_stop_lcd
+ *
+ * shutdown the lcd controller
+ */
+void s3c_fb_stop_lcd(void)
+{
+/* 	unsigned long flags; */
+/* 	unsigned long tmp; */
+/* 	printk("s3c_fb_stop_lcd() called.\n"); */
+
+/* 	local_irq_save(flags); */
+
+/* #if defined  (CONFIG_FB_LTV350QV ) ||  defined  (CONFIG_FB_LMS430WQ ) || defined(CONFIG_ARCH_S3C64XX) */
+/* 	tmp = __raw_readl(S3C_VIDCON0); */
+/* 	__raw_writel(tmp & ~(ENVID|ENVID_F), S3C_VIDCON0); */
+/* #else */
+/* 	tmp = __raw_readl(S3C_LCDCON1); */
+/* 	__raw_writel(tmp & ~(ENVID|ENVID_F), S3C_LCDCON1); */
+/* #endif */
+/* 	local_irq_restore(flags); */
+}
+
+
+void s3c_fb_start_lcd(void) {
+/* 	unsigned long flags; */
+/* 	unsigned long tmp; */
+/* 	printk("s3c_fb_start_lcd() called.\n"); */
+
+/* 	local_irq_save(flags); */
+
+/* #if defined  (CONFIG_FB_LTV350QV ) || defined  (CONFIG_FB_LMS430WQ ) || defined(CONFIG_ARCH_S3C64XX) */
+/* 	tmp = __raw_readl(S3C_VIDCON0); */
+/* 	__raw_writel(tmp | ENVID | ENVID_F, S3C_VIDCON0); */
+/* #else */
+/* 	tmp = __raw_readl(S3C_LCDCON1); */
+/* 	__raw_writel(tmp | ENVID | ENVID_F, S3C_LCDCON1); */
+/* #endif */
+/* 	local_irq_restore(flags);	 */
+}
+
+/*
+ *  Cleanup
+ */
+static int s3c_fb_remove(struct platform_device *pdev)
+{
+	struct fb_info *fbinfo;
+	struct s3c2410fb_info *info;
+	int irq;
+
+	fbinfo = platform_get_drvdata(pdev);
+	info = fbinfo->par;
+
+	unregister_framebuffer(fbinfo);
+
+	/* Free the requested clock */
+	if (info->clk) {
+		clk_disable(info->clk);
+		clk_put(info->clk);
+		info->clk = NULL;
+	}
+	
+	/* Free the requested IRQ */
+	irq = platform_get_irq(pdev, 0);
+	free_irq(irq, info);
+
+	/* Unamp que requested memory mapped registers */
+	iounmap(info->io);
+
+	
+	release_resource(info->mem);
+	kfree(info->mem);
+
+	platform_set_drvdata(pdev, NULL);
+
+	framebuffer_release(fbinfo);
+	
+/* 	struct s3c_fb_info *info = fbinfo->par; */
+/* 	int irq; */
+/* 	int index=0; */
+
+/* 	s3c_fb_stop_lcd(); */
+/* 	msleep(1); */
+
+/* 	for(index=0; index<S3C_FB_NUM; index++){  */
+/* 		s3c_fb_unmap_video_memory((struct s3c_fb_info *)&info[index]); */
+
+/* 	 	if (lcd_clock) { */
+/* 	 		clk_disable(lcd_clock); */
+/* 	 		clk_put(lcd_clock); */
+/* 	 		lcd_clock = NULL; */
+/* 		} */
+
+/* 		irq = platform_get_irq(pdev, 0); */
+/* 		free_irq(irq,&info[index]); */
+/* 		release_mem_region((unsigned long)S3C_VA_LCD, S3C_SZ_LCD); */
+/* 		unregister_framebuffer(&(info[index].fb)); */
+/* 	} // for(index=0; index<CONFIG_FB_NUM; index++) */
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/* suspend and resume support for the lcd controller */
+static int s3c_fb_suspend(struct platform_device *dev, pm_message_t state)
+{
+/* 	s3c_fb_stop_lcd(); */
+
+/* 	/\* sleep before disabling the clock, we need to ensure */
+/* 	 * the LCD DMA engine is not going to get back on the bus */
+/* 	 * before the clock goes off again (bjd) *\/ */
+
+/* 	msleep(1); */
+/* 	clk_disable(lcd_clock); */
+
+	return 0;
+}
+
+static int s3c_fb_resume(struct platform_device *dev)
+{
+/* 	clk_enable(lcd_clock); */
+/* 	msleep(1); */
+
+/*         Init_LDI(); */
+/* #if 0 */
+/* 	for(int index=0; index<S3C_FB_NUM; index++)  */
+/* 		s3c_fb_init_registers(&info[index]); */
+/* #endif */
+
+	return 0;
+}
+
+#else
+#define s3c_fb_suspend NULL
+#define s3c_fb_resume  NULL
+#endif
+
+static struct platform_driver s3c_fb_driver = {
+	.probe		= s3c_fb_probe,
+	.remove		= s3c_fb_remove,
+	.suspend	= s3c_fb_suspend,
+	.resume		= s3c_fb_resume,
+        .driver		= {
+		.name	= S3CFB_DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+
+int __devinit s3c_fb_init(void)
+{
+	return platform_driver_register(&s3c_fb_driver);
+}
+
+
+static void __exit s3c_fb_cleanup(void)
+{
+	platform_driver_unregister(&s3c_fb_driver);
+}
+
+EXPORT_SYMBOL(s3c_fb_stop_lcd);
+EXPORT_SYMBOL(s3c_fb_start_lcd);
+
+module_init(s3c_fb_init);
+module_exit(s3c_fb_cleanup);
+
+MODULE_AUTHOR("");
+MODULE_DESCRIPTION("Framebuffer driver for the S3C");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/s3c2443fb_tft.c b/drivers/video/s3c2443fb_tft.c
new file mode 100644
index 000000000000..ed81f138966c
--- /dev/null
+++ b/drivers/video/s3c2443fb_tft.c
@@ -0,0 +1,737 @@
+/*
+ * linux/drivers/video/s3c2443fb_tft.c
+ *
+ * Copyright (C) 2009 by Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ * Derived from s3c2410fb.c from Arnaud Patard and Ben Dooks
+ * It could be merged in future
+ *
+ */
+
+#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/dma-mapping.h>
+#include <linux/workqueue.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+
+#include <asm/io.h>
+
+#include <asm/mach/map.h>
+#include <mach/idle.h>
+#include <mach/s3c2443-fb.h>
+#include <mach/regs-gpio.h>
+
+#ifdef CONFIG_PM
+#include <linux/pm.h>
+#endif
+
+#define printk_err(fmt, args...)	printk(KERN_ERR "[ ERROR ] " DRV_NAME ": " fmt, ## args)
+#define printk_info(fmt, args...)	printk(KERN_INFO "[ INFO ] " DRV_NAME ": " fmt, ## args)
+
+#if 0
+#define S3C2443_FB_DEBUG
+#endif
+
+#ifdef S3C2443_FB_DEBUG
+#  define printk_debug(fmt, args...)	printk(KERN_DEBUG DRV_NAME ": " fmt, ## args)
+#else
+#  define printk_debug(fmt, args...)
+#endif
+
+#define DRV_NAME "s3c2443fb-tft"
+#define PALETTE_BUFF_CLEAR (0x80000000)
+
+struct s3c2443fb_info {
+	struct device	*dev;
+	struct clk	*clk;
+	struct resource	*mem;
+	void __iomem	*io;
+	void __iomem	*irq_base;
+	unsigned int	palette_ready;
+	u32		palette_buffer[256];
+	u32		pseudo_pal[16];
+};
+
+static int s3c2443_check_var(struct fb_var_screeninfo *var,
+			       struct fb_info *info)
+{
+	struct s3c2443fb_info *fbi = info->par;
+	struct s3c2443fb_mach_info *mach_info = fbi->dev->platform_data;
+	struct s3c2443fb_display *display = NULL;
+
+	display = mach_info->display;
+	if (!display) {
+		dev_err(fbi->dev, "unable to access display info(null)\n");
+		return -EINVAL;
+	}
+
+	/* it is always the size as the display */
+	var->xres_virtual = display->xres;
+	var->yres_virtual = display->yres;
+	var->height = display->height;
+	var->width = display->width;
+
+	/* copy lcd settings */
+	var->pixclock = display->pixclock;
+	var->left_margin = display->left_margin;
+	var->right_margin = display->right_margin;
+	var->upper_margin = display->upper_margin;
+	var->lower_margin = display->lower_margin;
+	var->vsync_len = display->vsync_len;
+	var->hsync_len = display->hsync_len;
+	var->transp.offset = 0;
+	var->transp.length = 0;
+
+	/* set r/g/b positions */
+	switch (var->bits_per_pixel) {
+	case 1:
+	case 2:
+	case 4:
+		var->red.offset	= 0;
+		var->red.length	= var->bits_per_pixel;
+		var->green	= var->red;
+		var->blue	= var->red;
+		break;
+	case 8:
+		/* 8 bpp 332 */
+		var->red.length		= 3;
+		var->red.offset		= 5;
+		var->green.length	= 3;
+		var->green.offset	= 2;
+		var->blue.length	= 2;
+		var->blue.offset	= 0;
+		break;
+	case 12:
+		/* 12 bpp 444 */
+		var->red.length		= 4;
+		var->red.offset		= 8;
+		var->green.length	= 4;
+		var->green.offset	= 4;
+		var->blue.length	= 4;
+		var->blue.offset	= 0;
+		break;
+	case 16:
+		/* 16 bpp, 565 format */
+		if (display->bpp_mode == S3C24XX_LCD_WINCON_16BPP_565) {
+			/* 16 bpp, 565 format */
+			var->red.offset		= 11;
+			var->green.offset	= 5;
+			var->blue.offset	= 0;
+			var->red.length		= 5;
+			var->green.length	= 6;
+			var->blue.length	= 5;
+		} else {
+			/* 16 bpp, 1555 format */
+			var->red.offset		= 10;
+			var->green.offset	= 5;
+			var->blue.offset	= 0;
+			var->red.length		= 5;
+			var->green.length	= 5;
+			var->blue.length	= 5;
+		}
+		break;
+	case 18:
+		/* 18bpp only supports 666 */
+		var->red.offset		= 12;
+		var->green.offset	= 6;
+		var->blue.offset	= 0;
+		var->red.length		= 6;
+		var->green.length	= 6;
+		var->blue.length	= 6;
+		break;
+	case 32:
+		/* 24 bpp 888 and 8 dummy */
+		var->red.length		= 8;
+		var->red.offset		= 16;
+		var->green.length	= 8;
+		var->green.offset	= 8;
+		var->blue.length	= 8;
+		var->blue.offset	= 0;
+		break;
+	}
+
+	return 0;
+}
+
+/* Write into the register the passed display configuration */
+static void s3c2443fb_activate_var(struct fb_info *info)
+{
+	struct s3c2443fb_info *fbi = info->par;
+	struct s3c2443fb_mach_info *mach_info = fbi->dev->platform_data;
+	struct s3c2443fb_display *display = mach_info->display;
+	unsigned short hsync_cnt, vclk_cnt;
+	unsigned char clkval;
+	unsigned long pixel_clk, lcd_clk;
+	unsigned long vidcon0, vidcon1, vidtcon0, vidtcon1, vidtcon2;
+	unsigned long wincon0, wincon1;
+	unsigned long vidosd0a, vidosd0b;
+	
+	/* Compute number of clocks per frame */
+	hsync_cnt = display->lower_margin + /* VBPD */
+		display->upper_margin +     /* VFPD */
+		display->vsync_len +        /* VSPW */
+		display->height +           /* LINEVAL */
+		4;
+
+	vclk_cnt = display->right_margin +  /* HBPD */
+		display->left_margin +      /* HFPD */
+		display->hsync_len +        /* HSPW */
+		display->width +            /* HOZVAL */
+		4;
+
+	pixel_clk = (display->frame_rate * (vclk_cnt+1) *  hsync_cnt);
+	lcd_clk = clk_get_rate(fbi->clk);
+        clkval = (lcd_clk / pixel_clk) - 1;
+
+	if (abs((lcd_clk / clkval) - pixel_clk) > 
+	    abs((lcd_clk / (clkval + 1)) - pixel_clk))
+		clkval++;
+
+	printk_debug("Calculated CLKVAL is 0x%x (pixel clk: %lu | lcd clk %lu)\n",
+		     clkval, pixel_clk, lcd_clk);
+	
+	vidcon1 = readl(fbi->io + S3C24XX_LCD_VIDCON1);
+	vidcon0 = readl(fbi->io + S3C24XX_LCD_VIDCON0);
+	wincon1 = readl(fbi->io + S3C24XX_LCD_WINCON1);
+	
+	/* Configure the clock */
+	vidcon0 &= ~(S3C24XX_LCD_VIDCON0_CLKSEL_MASK |
+		     S3C24XX_LCD_VIDCON0_CLKVAL_MASK);
+	if (!strcmp(display->clock_source, "display-if"))
+		vidcon0 |= S3C24XX_LCD_VIDCON0_CLKSEL_LCD;
+
+	vidcon0 |=	(display->vidcon0 | S3C24XX_LCD_VIDCON0_CLKVAL(clkval));
+	vidcon1 |=	display->vidcon1;
+	
+	vidtcon0 =	S3C24XX_LCD_VIDTCON0_VSPW(display->vsync_len - 1) |
+			S3C24XX_LCD_VIDTCON0_VFPD(display->upper_margin - 1) |
+			S3C24XX_LCD_VIDTCON0_VBPD(display->lower_margin - 1);
+
+	vidtcon1 =	S3C24XX_LCD_VIDTCON1_HSPW(display->hsync_len - 1) |
+			S3C24XX_LCD_VIDTCON1_HFPD(display->left_margin - 1) |
+			S3C24XX_LCD_VIDTCON1_HBPD(display->right_margin - 1);
+
+	vidtcon2 =	S3C24XX_LCD_VIDTCON2_HOZVAL(display->width - 1) |
+			S3C24XX_LCD_VIDTCON2_LINEVAL(display->height - 1);
+
+	/* Write the user configuration too */
+	wincon0	=	display->bpp_mode |
+			S3C24XX_LCD_WINCON0_ENWIN_F |
+			S3C24XX_LCD_WINCON0_HAWSWP |
+			S3C24XX_LCD_WINCON0_16WBURST;
+
+	vidosd0a =	0x00;
+	vidosd0b =	S3C24XX_LCD_VIDOSD0B_RIGHT_X(display->width - 1) |
+			S3C24XX_LCD_VIDOSD0B_RIGHT_Y(display->height - 1);
+
+	/* And now write the configuration into the corresponding registers */
+	writel(vidcon0, fbi->io + S3C24XX_LCD_VIDCON0);
+	writel(vidcon1, fbi->io + S3C24XX_LCD_VIDCON1);
+	writel(vidtcon0, fbi->io + S3C24XX_LCD_VIDTCON0);
+	writel(vidtcon1, fbi->io + S3C24XX_LCD_VIDTCON1);
+	writel(vidtcon2, fbi->io + S3C24XX_LCD_VIDTCON2);
+	writel(wincon0, fbi->io + S3C24XX_LCD_WINCON0);
+	writel(vidosd0a, fbi->io + S3C24XX_LCD_VIDOSD0A);
+	writel(vidosd0b, fbi->io + S3C24XX_LCD_VIDOSD0B);
+
+	/* These are the configuration addresses for the frame buffer */
+	writel(info->fix.smem_start, fbi->io + S3C24XX_LCD_VIDW00ADD0B0);
+	writel(info->fix.smem_start + info->fix.smem_len,
+	       fbi->io + S3C24XX_LCD_VIDW00ADD1B0);
+		
+	printk_debug("vidcon0 0x%08x | vidcon1 0x%08x\n",
+		     (unsigned int)vidcon0,
+		     (unsigned int)vidcon1);
+	printk_debug("vidtcon0 0x%08x | vidtcon1 0x%08x | vidtcon2 0x%08x\n",
+		     (unsigned int)vidtcon0,
+		     (unsigned int)vidtcon1,
+		     (unsigned int)vidtcon2);
+}
+
+/* Write the display configuration to the hardware */
+static int s3c2443fb_set_par(struct fb_info *info)
+{
+	struct fb_var_screeninfo *var = &info->var;
+
+	switch (var->bits_per_pixel) {
+	case 32:
+	case 16:
+	case 12:
+		info->fix.visual = FB_VISUAL_TRUECOLOR;
+		break;
+	case 1:
+		info->fix.visual = FB_VISUAL_MONO01;
+		break;
+	default:
+		info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
+		break;
+	}
+
+	
+	info->fix.line_length = (var->xres_virtual * var->bits_per_pixel) / 8;
+	
+	/* activate this new configuration */
+	s3c2443fb_activate_var(info);
+	return 0;
+}
+
+static void s3c2443fb_lcd_enable(struct s3c2443fb_info *fbi, int enable)
+{
+	struct s3c2443fb_mach_info *mach_info = fbi->dev->platform_data;
+	struct s3c2443fb_display *display = mach_info->display;
+	unsigned long flags;
+	unsigned long vidcon0;
+
+	local_irq_save(flags);
+	vidcon0 = readl(fbi->io + S3C24XX_LCD_VIDCON0);
+	if (enable)
+		vidcon0 |= S3C24XX_LCD_VIDCON0_ENVID | S3C24XX_LCD_VIDCON0_ENVID_F;
+	else
+		vidcon0 &= ~(S3C24XX_LCD_VIDCON0_ENVID | S3C24XX_LCD_VIDCON0_ENVID_F);
+
+	writel(vidcon0, fbi->io + S3C24XX_LCD_VIDCON0);
+
+	if (display->display_power_enable)
+		display->display_power_enable(enable);
+
+	local_irq_restore(flags);
+}
+
+static int s3c2443fb_blank(int blank_mode, struct fb_info *info)
+{
+	struct s3c2443fb_info *fbi = info->par;
+
+	printk_debug("Blank (mode=%d, info=%p)\n", blank_mode, info);
+
+	if (blank_mode == FB_BLANK_POWERDOWN) {
+		s3c2443fb_lcd_enable(fbi, 0);
+	} else {
+		s3c2443fb_lcd_enable(fbi, 1);
+	}
+
+	return 0;
+}
+
+static void schedule_palette_update(struct s3c2443fb_info *fbi,
+				    unsigned int regno, unsigned int val)
+{
+	unsigned long flags;
+	unsigned long irqen;
+	void __iomem *irq_base = fbi->irq_base;
+
+	printk_debug("Calling %s\n", __func__);
+	
+	local_irq_save(flags);
+
+	fbi->palette_buffer[regno] = val;
+
+	if (!fbi->palette_ready) {
+		fbi->palette_ready = 1;
+		/* enable IRQ */
+		irqen = readl(irq_base + S3C24XX_LCDINTMSK);
+		irqen &= ~S3C2410_LCDINT_FRSYNC;
+		writel(irqen, irq_base + S3C24XX_LCDINTMSK);
+	}
+
+	local_irq_restore(flags);
+}
+
+
+/* from pxafb.c */
+static inline unsigned int chan_to_field(unsigned int chan,
+				         struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+static int s3c2443fb_setcolreg(unsigned regno,
+			       unsigned red, unsigned green, unsigned blue,
+			       unsigned transp, struct fb_info *info)
+{
+	struct s3c2443fb_info *fbi = info->par;
+	void __iomem *regs = fbi->io;
+	unsigned int val;
+
+	switch (info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		/* true-colour, use pseudo-palette */
+		if (regno < 16) {
+			u32 *pal = info->pseudo_palette;
+
+			val  = chan_to_field(red,   &info->var.red);
+			val |= chan_to_field(green, &info->var.green);
+			val |= chan_to_field(blue,  &info->var.blue);
+
+			pal[regno] = val;
+		}
+		break;
+
+	case FB_VISUAL_PSEUDOCOLOR:
+		if (regno < 256) {
+			/* currently assume RGB 5-6-5 mode */
+
+			val  = (red   >>  0) & 0xf800;
+			val |= (green >>  5) & 0x07e0;
+			val |= (blue  >> 11) & 0x001f;
+
+			writel(val, regs + S3C2410_TFTPAL(regno));
+			schedule_palette_update(fbi, regno, val);
+		}
+
+		break;
+
+	default:
+		return 1;	/* unknown type */
+	}
+
+	return 0;
+}
+
+/* These are the available device operations */
+static struct fb_ops s3c2443fb_tft_fbops = {
+	.owner		= THIS_MODULE,
+	.fb_check_var	= s3c2443_check_var,
+	.fb_set_par	= s3c2443fb_set_par,
+	.fb_blank	= s3c2443fb_blank,
+	.fb_setcolreg	= s3c2443fb_setcolreg,
+	.fb_fillrect	= cfb_fillrect,
+	.fb_copyarea	= cfb_copyarea,
+	.fb_imageblit	= cfb_imageblit,
+};
+
+/*
+ * Allocates the DRAM memory for the frame buffer.  This buffer is
+ * remapped into a non-cached, non-buffered, memory region to
+ * allow palette and pixel writes to occur without flushing the
+ * cache.  Once this area is remapped, all virtual memory
+ * access to the video memory should occur at the new region.
+ */
+static int __init s3c2443fb_map_video_memory(struct fb_info *info)
+{
+	struct s3c2443fb_info *fbi = info->par;
+	dma_addr_t map_dma;
+	unsigned map_size = PAGE_ALIGN(info->fix.smem_len);
+
+	info->screen_base = dma_alloc_writecombine(fbi->dev, map_size,
+						   &map_dma, GFP_KERNEL);
+
+	if (info->screen_base) {
+		/* clear screen */
+		memset(info->screen_base, 0x00, map_size);
+		info->fix.smem_start = map_dma;
+	}
+
+	return info->screen_base ? 0 : -ENOMEM;
+}
+
+static inline void modify_gpio(void __iomem *reg,
+			       unsigned long set, unsigned long mask)
+{
+	unsigned long tmp;
+
+	tmp = readl(reg) & ~mask;
+	writel(tmp | set, reg);
+}
+
+static int s3c2443fb_init_registers(struct fb_info *info)
+{
+	struct s3c2443fb_info *fbi = info->par;
+	struct s3c2443fb_mach_info *mach_info = fbi->dev->platform_data;
+	unsigned long flags;
+
+	/* modify the gpio(s) with interrupts set (bjd) */
+	local_irq_save(flags);
+	/* Select display type and config the gpios */  
+	writel(readl(S3C24XX_MISCCR) | (1 << 28), S3C24XX_MISCCR);
+	modify_gpio(S3C2410_GPCUP,  mach_info->gpcup,  mach_info->gpcup_mask);
+	modify_gpio(S3C2410_GPCCON, mach_info->gpccon, mach_info->gpccon_mask);
+	modify_gpio(S3C2410_GPDUP,  mach_info->gpdup,  mach_info->gpdup_mask);
+	modify_gpio(S3C2410_GPDCON, mach_info->gpdcon, mach_info->gpdcon_mask);
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static inline void s3c2443fb_unmap_video_memory(struct fb_info *info)
+{
+	struct s3c2443fb_info *fbi = info->par;
+
+	dma_free_writecombine(fbi->dev, PAGE_ALIGN(info->fix.smem_len),
+			      info->screen_base, info->fix.smem_start);
+}
+
+int __init s3c2443_fb_probe(struct platform_device *pdev)
+{
+	struct s3c2443fb_info *info;
+	struct s3c2443fb_display *display;
+	struct s3c2443fb_mach_info *mach_info;
+	struct fb_info *fbinfo;
+	struct resource *res;
+	int ret, i;
+	char *option = NULL;
+
+	/* Get the user defined LCD controller configuration */
+	mach_info = pdev->dev.platform_data;
+	if (mach_info == NULL) {
+		dev_err(&pdev->dev, "no platform data for lcd driver\n");
+		return -EINVAL;
+	}
+
+	/* Get the display information */
+	if (!mach_info->num_displays) {
+		dev_err(&pdev->dev, "no display information available\n");
+		return -EINVAL;
+	}
+
+	if (mach_info->num_displays > 1) {
+		/* 
+		 * If there are multiple displays, use the one specified
+		 * through the command line parameter vith following 
+		 * format video=displayfb:<display_name>
+		 */
+		if (fb_get_options("displayfb", &option)) {
+			dev_err(&pdev->dev, "no display information available in commnad line\n");
+			return -ENODEV;
+		}
+		if (!option)
+			return -ENODEV;
+
+		printk_debug("display options: %s\n", option);
+		for (i = 0; i < mach_info->num_displays; i++) {
+			if (!strcmp(option, mach_info->displays[i].display_name))
+				mach_info->display = &mach_info->displays[i];
+		}
+	} else {
+		/* If there is only one, that is what we use */
+		mach_info->display = mach_info->displays;
+	}
+  
+	if ((display = mach_info->display) == NULL) {
+		dev_err(&pdev->dev, "no display information available\n");
+		return -EINVAL;
+	}
+	
+	/* Allocate the frame buffer for this device */
+	fbinfo = framebuffer_alloc(sizeof(struct s3c2443fb_info), &pdev->dev);
+	if (!fbinfo)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, fbinfo);
+
+	/* Use the private data for our internal settings */
+	info = fbinfo->par;	
+	info->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "failed to get memory registers\n");
+		ret = -ENXIO;
+		goto dealloc_fb;
+	}
+	info->mem = request_mem_region(res->start,
+					res->end - res->start + 1, pdev->name);
+	if (info->mem == NULL) {
+		dev_err(&pdev->dev, "failed to get memory region\n");
+		ret = -ENOENT;
+		goto dealloc_fb;
+	}
+
+	info->io = ioremap(res->start, res->end - res->start + 1);
+	if (info->io == NULL) {
+		dev_err(&pdev->dev, "ioremap of registers failed\n");
+		ret = -ENXIO;
+		goto release_mem;
+	}
+
+	strcpy(fbinfo->fix.id, DRV_NAME);
+
+	/* Start the initial fix configuration */
+	fbinfo->fix.type	    = FB_TYPE_PACKED_PIXELS;
+	fbinfo->fix.type_aux	    = 0;
+	fbinfo->fix.xpanstep	    = 0;
+	fbinfo->fix.ypanstep	    = 0;
+	fbinfo->fix.ywrapstep	    = 0;
+	fbinfo->fix.accel	    = FB_ACCEL_NONE;
+
+	fbinfo->var.nonstd	    = 0;
+	fbinfo->var.activate	    = FB_ACTIVATE_NOW;
+	fbinfo->var.accel_flags     = 0;
+	fbinfo->var.vmode	    = FB_VMODE_NONINTERLACED;
+
+	fbinfo->fbops		    = &s3c2443fb_tft_fbops;
+	fbinfo->flags		    = FBINFO_FLAG_DEFAULT;
+	fbinfo->pseudo_palette      = &info->pseudo_pal;
+
+	for (i = 0; i < 256; i++)
+		info->palette_buffer[i] = PALETTE_BUFF_CLEAR;
+
+	info->clk = clk_get(NULL, mach_info->display->clock_source);
+	if (!info->clk || IS_ERR(info->clk)) {
+		dev_err(&pdev->dev, "failed to get lcd clock source %s\n", 
+			mach_info->display->clock_source);
+		ret = -ENOENT;
+		goto release_regs;
+	}
+
+	clk_enable(info->clk);
+	msleep(1);
+
+	/* Initialize video memory */
+	fbinfo->var.xres = display->xres;
+	fbinfo->var.yres = display->yres;
+	fbinfo->var.bits_per_pixel = display->bpp;
+	fbinfo->fix.smem_len = display->xres * display->yres * (display->bpp >> 3);
+
+	ret = s3c2443fb_map_video_memory(fbinfo);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to map video memory\n");
+		ret = -ENOMEM;
+		goto release_clock;
+	}
+
+	s3c2443fb_init_registers(fbinfo);
+
+	s3c2443_check_var(&fbinfo->var, fbinfo);
+
+	ret = register_framebuffer(fbinfo);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register framebuffer device\n");
+		goto free_video_memory;
+	}
+
+	s3c2443fb_lcd_enable(info, 1);
+
+	printk(KERN_INFO DRV_NAME ": frame buffer fb%d: %s, display config %s\n",
+	       fbinfo->node, fbinfo->fix.id, mach_info->display->display_name);
+	return 0;
+
+free_video_memory:
+	s3c2443fb_unmap_video_memory(fbinfo);
+
+release_clock:
+	clk_disable(info->clk);
+	clk_put(info->clk);
+
+release_regs:
+	iounmap(info->io);
+
+release_mem:
+	release_resource(info->mem);
+	kfree(info->mem);
+
+dealloc_fb:
+	platform_set_drvdata(pdev, NULL);
+	framebuffer_release(fbinfo);
+
+	return ret;
+}
+
+static int s3c2443_fb_remove(struct platform_device *pdev)
+{
+	struct fb_info *fbinfo = platform_get_drvdata(pdev);
+	struct s3c2443fb_info *info = fbinfo->par;
+
+	fbinfo = platform_get_drvdata(pdev);
+	info = fbinfo->par;
+
+	s3c2443fb_lcd_enable(info, 0);
+	unregister_framebuffer(fbinfo);
+	s3c2443fb_unmap_video_memory(fbinfo);
+
+	if (info->clk) {
+		clk_disable(info->clk);
+		clk_put(info->clk);
+		info->clk = NULL;
+	}
+	
+	iounmap(info->io);
+	release_resource(info->mem);
+	kfree(info->mem);
+	platform_set_drvdata(pdev, NULL);
+	framebuffer_release(fbinfo);
+	
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/* suspend and resume support for the lcd controller */
+static int s3c2443_fb_suspend(struct platform_device *dev, pm_message_t state)
+{
+	struct fb_info *fbinfo = platform_get_drvdata(dev);
+	struct s3c2443fb_info *info = fbinfo->par;
+
+	if (state.event == PM_EVENT_SUSPEND) {
+		s3c2443fb_lcd_enable(info, 0);
+		/* sleep before disabling the clock, we need to ensure
+		* the LCD DMA engine is not going to get back on the bus
+		* before the clock goes off again (bjd) */
+		msleep(1);
+		clk_disable(info->clk);
+	}
+
+	return 0;
+}
+
+static int s3c2443_fb_resume(struct platform_device *dev)
+{
+	struct fb_info	   *fbinfo = platform_get_drvdata(dev);
+	struct s3c2443fb_info *info = fbinfo->par;
+	
+	clk_enable(info->clk);
+	msleep(1);
+	s3c2443fb_init_registers(fbinfo);
+	s3c2443_check_var(&fbinfo->var, fbinfo);
+	s3c2443fb_activate_var(fbinfo);
+	s3c2443fb_lcd_enable(info, 1);
+	
+	return 0;
+}
+#else
+#define s3c2443_fb_suspend NULL
+#define s3c2443_fb_resume  NULL
+#endif
+
+static struct platform_driver s3c2443_fb_driver = {
+	.probe		= s3c2443_fb_probe,
+	.remove		= s3c2443_fb_remove,
+	.suspend	= s3c2443_fb_suspend,
+	.resume		= s3c2443_fb_resume,
+        .driver		= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+int __devinit s3c2443_fb_init(void)
+{
+	return platform_driver_register(&s3c2443_fb_driver);
+}
+
+static void __exit s3c2443_fb_cleanup(void)
+{
+	platform_driver_unregister(&s3c2443_fb_driver);
+}
+
+module_init(s3c2443_fb_init);
+module_exit(s3c2443_fb_cleanup);
+
+MODULE_AUTHOR("Luis Galdos & Pedro Perez de Heredia");
+MODULE_DESCRIPTION("Framebuffer driver for the S3C2443");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/video/stmp37xxfb.c b/drivers/video/stmp37xxfb.c
new file mode 100644
index 000000000000..bf058ae3e097
--- /dev/null
+++ b/drivers/video/stmp37xxfb.c
@@ -0,0 +1,844 @@
+/*
+ * Freescale STMP37XX/STMP378X framebuffer driver
+ *
+ * Author: Vitaly Wool <vital@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/uaccess.h>
+#include <linux/cpufreq.h>
+
+#include <mach/hardware.h>
+#include <mach/regs-pinctrl.h>
+#include <mach/regs-lcdif.h>
+#include <mach/regs-clkctrl.h>
+#include <mach/regs-apbh.h>
+#include <mach/lcdif.h>
+
+#include <mach/stmp3xxx.h>
+
+#define NUM_SCREENS	1
+
+struct stmp3xxx_fb_data {
+	struct fb_info info;
+	struct stmp3xxx_platform_fb_data *pdata;
+	int is_blank;
+	ssize_t mem_size;
+	ssize_t map_size;
+	dma_addr_t phys_start;
+	dma_addr_t cur_phys;
+	int dma_irq;
+	int err_irq;
+	void *virt_start;
+	struct device *dev;
+	wait_queue_head_t vsync_wait_q;
+	u32 vsync_count;
+	void *par;
+};
+
+/* forward declaration */
+static int stmp3xxxfb_blank(int blank, struct fb_info *info);
+static unsigned char *default_panel_name;
+static struct stmp3xxx_fb_data *cdata;
+
+static irqreturn_t lcd_irq_handler(int irq, void *dev_id)
+{
+	struct stmp3xxx_fb_data *data = dev_id;
+	u32 status_lcd = HW_LCDIF_CTRL1_RD();
+	u32 status_apbh = HW_APBH_CTRL1_RD();
+	pr_debug("%s: irq %d\n", __func__, irq);
+
+	if (status_apbh & BM_APBH_CTRL1_CH0_CMDCMPLT_IRQ)
+		HW_APBH_CTRL1_CLR(BM_APBH_CTRL1_CH0_CMDCMPLT_IRQ);
+
+	if (status_lcd & BM_LCDIF_CTRL1_VSYNC_EDGE_IRQ) {
+		pr_debug("%s: VSYNC irq\n", __func__);
+		data->vsync_count++;
+		HW_LCDIF_CTRL1_CLR(BM_LCDIF_CTRL1_VSYNC_EDGE_IRQ);
+		wake_up_interruptible(&data->vsync_wait_q);
+	}
+	if (status_lcd & BM_LCDIF_CTRL1_CUR_FRAME_DONE_IRQ) {
+		pr_debug("%s: frame done irq\n", __func__);
+		HW_LCDIF_CTRL1_CLR(BM_LCDIF_CTRL1_CUR_FRAME_DONE_IRQ);
+		data->vsync_count++;
+	}
+	if (status_lcd & BM_LCDIF_CTRL1_UNDERFLOW_IRQ) {
+		pr_debug("%s: underflow irq\n", __func__);
+		HW_LCDIF_CTRL1_CLR(BM_LCDIF_CTRL1_UNDERFLOW_IRQ);
+	}
+	if (status_lcd & BM_LCDIF_CTRL1_OVERFLOW_IRQ) {
+		pr_debug("%s: overflow irq\n", __func__);
+		HW_LCDIF_CTRL1_CLR(BM_LCDIF_CTRL1_OVERFLOW_IRQ);
+	}
+	return IRQ_HANDLED;
+}
+
+static struct fb_var_screeninfo stmp3xxxfb_default __devinitdata = {
+	.activate =	FB_ACTIVATE_TEST,
+	.height =	-1,
+	.width =	-1,
+	.pixclock =	20000,
+	.left_margin =	64,
+	.right_margin =	64,
+	.upper_margin =	32,
+	.lower_margin =	32,
+	.hsync_len =	64,
+	.vsync_len =	2,
+	.vmode =	FB_VMODE_NONINTERLACED,
+};
+
+static struct fb_fix_screeninfo stmp3xxxfb_fix __devinitdata = {
+	.id =		"stmp3xxxfb",
+	.type =		FB_TYPE_PACKED_PIXELS,
+	.visual =       FB_VISUAL_TRUECOLOR,
+	.xpanstep =	0,
+	.ypanstep =	0,
+	.ywrapstep =	0,
+	.accel =	FB_ACCEL_NONE,
+};
+
+int stmp3xxxfb_get_info(struct fb_var_screeninfo *var,
+			struct fb_fix_screeninfo *fix)
+{
+	if (!cdata)
+		return -ENODEV;
+
+	*var = cdata->info.var;
+	*fix = cdata->info.fix;
+	return 0;
+}
+
+void stmp3xxxfb_cfg_pxp(int enable, dma_addr_t pxp_phys)
+{
+	if (enable)
+		cdata->pdata->cur->pan_display(pxp_phys);
+	else
+		cdata->pdata->cur->pan_display(cdata->cur_phys);
+}
+
+static int stmp3xxxfb_mmap(struct fb_info *info, struct vm_area_struct *vma)
+{
+	struct stmp3xxx_fb_data *data = (struct stmp3xxx_fb_data *)info;
+
+	unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
+
+	if (off < info->fix.smem_len)
+		return dma_mmap_writecombine(data->dev, vma,
+				data->virt_start,
+				data->phys_start,
+				info->fix.smem_len);
+	else
+		return -EINVAL;
+}
+
+static int stmp3xxxfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+			 u_int transp, struct fb_info *info)
+{
+	if (regno >= 256)	/* no. of hw registers */
+		return 1;
+	/*
+	* Program hardware... do anything you want with transp
+	*/
+
+	/* grayscale works only partially under directcolor */
+	if (info->var.grayscale) {
+		/* grayscale = 0.30*R + 0.59*G + 0.11*B */
+		red = green = blue =
+		    (red * 77 + green * 151 + blue * 28) >> 8;
+	}
+
+	/* Directcolor:
+	 *   var->{color}.offset contains start of bitfield
+	 *   var->{color}.length contains length of bitfield
+	 *   {hardwarespecific} contains width of RAMDAC
+	 *   cmap[X] is programmed to
+	 *	(X << red.offset) | (X << green.offset) | (X << blue.offset)
+	 *   RAMDAC[X] is programmed to (red, green, blue)
+	 *
+	 * Pseudocolor:
+	 *    uses offset = 0 && length = RAMDAC register width.
+	 *    var->{color}.offset is 0
+	 *    var->{color}.length contains widht of DAC
+	 *    cmap is not used
+	 *    RAMDAC[X] is programmed to (red, green, blue)
+	 * Truecolor:
+	 *    does not use DAC. Usually 3 are present.
+	 *    var->{color}.offset contains start of bitfield
+	 *    var->{color}.length contains length of bitfield
+	 *    cmap is programmed to
+	 *	(red << red.offset) | (green << green.offset) |
+	 *	(blue << blue.offset) | (transp << transp.offset)
+	 *    RAMDAC does not exist
+	 */
+#define CNVT_TOHW(val, width) ((((val)<<(width))+0x7FFF-(val))>>16)
+	switch (info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		red = CNVT_TOHW(red, info->var.red.length);
+		green = CNVT_TOHW(green, info->var.green.length);
+		blue = CNVT_TOHW(blue, info->var.blue.length);
+		transp = CNVT_TOHW(transp, info->var.transp.length);
+		break;
+	case FB_VISUAL_DIRECTCOLOR:
+		red = CNVT_TOHW(red, 8);	/* expect 8 bit DAC */
+		green = CNVT_TOHW(green, 8);
+		blue = CNVT_TOHW(blue, 8);
+		/* hey, there is bug in transp handling... */
+		transp = CNVT_TOHW(transp, 8);
+		break;
+	}
+#undef CNVT_TOHW
+	/* Truecolor has hardware independent palette */
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR) {
+
+		if (regno >= 16)
+			return 1;
+
+		((u32 *) (info->pseudo_palette))[regno] =
+				(red << info->var.red.offset) |
+				(green << info->var.green.offset) |
+				(blue << info->var.blue.offset) |
+				(transp << info->var.transp.offset);
+	}
+	return 0;
+}
+
+static inline u_long get_line_length(int xres_virtual, int bpp)
+{
+	u_long length;
+
+	length = xres_virtual * bpp;
+	length = (length + 31) & ~31;
+	length >>= 3;
+	return length;
+}
+
+static int get_matching_pentry(struct stmp3xxx_platform_fb_entry *pentry,
+			       void *data, int ret_prev)
+{
+	struct fb_var_screeninfo *info = data;
+	pr_debug("%s: %d:%d:%d vs %d:%d:%d\n", __func__,
+		pentry->x_res, pentry->y_res, pentry->bpp,
+		info->yres, info->xres, info->bits_per_pixel);
+	if (pentry->x_res == info->yres && pentry->y_res == info->xres &&
+	    pentry->bpp == info->bits_per_pixel)
+		ret_prev = (int)pentry;
+	return ret_prev;
+}
+
+static int get_matching_pentry_by_name(
+		struct stmp3xxx_platform_fb_entry *pentry,
+		void *data,
+		int ret_prev)
+{
+	unsigned char *name = data;
+	if (!strcmp(pentry->name, name))
+		ret_prev = (int)pentry;
+	return ret_prev;
+}
+
+/*
+ * This routine actually sets the video mode. It's in here where we
+ * the hardware state info->par and fix which can be affected by the
+ * change in par. For this driver it doesn't do much.
+ *
+ * XXX: REVISIT
+ */
+static int stmp3xxxfb_set_par(struct fb_info *info)
+{
+	struct stmp3xxx_fb_data *data = (struct stmp3xxx_fb_data *)info;
+	struct stmp3xxx_platform_fb_data *pdata = data->pdata;
+	struct stmp3xxx_platform_fb_entry *pentry;
+	pentry = (void *)stmp3xxx_lcd_iterate_pdata(pdata,
+						get_matching_pentry,
+						&info->var);
+
+	dev_dbg(data->dev, "%s: xres %d, yres %d, bpp %d\n",
+		__func__,
+		info->var.xres,
+		info->var.yres,
+		info->var.bits_per_pixel);
+	if (!pentry)
+		return -EINVAL;
+
+	info->fix.line_length = get_line_length(info->var.xres_virtual,
+						info->var.bits_per_pixel);
+
+	if (pentry == pdata->cur || !pdata->cur)
+		return 0;
+
+	/* release prev panel */
+	stmp3xxxfb_blank(FB_BLANK_POWERDOWN, &data->info);
+	if (pdata->cur->stop_panel)
+		pdata->cur->stop_panel();
+	pdata->cur->release_panel(data->dev, pdata->cur);
+
+	info->fix.smem_len = pentry->y_res * pentry->x_res * pentry->bpp / 8;
+	info->screen_size = info->fix.smem_len;
+	memset((void *)info->screen_base, 0, info->screen_size);
+
+	/* init next panel */
+	pdata->cur = pentry;
+	stmp3xxx_init_lcdif();
+	pentry->init_panel(data->dev, data->phys_start, info->fix.smem_len,
+			   pentry);
+	pentry->run_panel();
+	stmp3xxxfb_blank(FB_BLANK_UNBLANK, &data->info);
+
+	return 0;
+}
+
+static int stmp3xxxfb_check_var(struct fb_var_screeninfo *var,
+				struct fb_info *info)
+{
+	u32 line_length;
+	struct stmp3xxx_fb_data *data = (struct stmp3xxx_fb_data *)info;
+	struct stmp3xxx_platform_fb_data *pdata = data->pdata;
+
+	/*
+	 *  FB_VMODE_CONUPDATE and FB_VMODE_SMOOTH_XPAN are equal!
+	 *  as FB_VMODE_SMOOTH_XPAN is only used internally
+	 */
+
+	if (var->vmode & FB_VMODE_CONUPDATE) {
+		var->vmode |= FB_VMODE_YWRAP;
+		var->xoffset = info->var.xoffset;
+		var->yoffset = info->var.yoffset;
+	}
+
+	pr_debug("%s: xres %d, yres %d, bpp %d\n", __func__,
+		var->xres,  var->yres, var->bits_per_pixel);
+	/*
+	 *  Some very basic checks
+	 */
+	if (!var->xres)
+		var->xres = 1;
+	if (!var->yres)
+		var->yres = 1;
+	if (var->xres > var->xres_virtual)
+		var->xres_virtual = var->xres;
+	if (var->yres > var->yres_virtual)
+		var->yres_virtual = var->yres;
+
+	if (var->xres_virtual < var->xoffset + var->xres)
+		var->xres_virtual = var->xoffset + var->xres;
+	if (var->yres_virtual < var->yoffset + var->yres)
+		var->yres_virtual = var->yoffset + var->yres;
+
+	line_length = get_line_length(var->xres_virtual, var->bits_per_pixel);
+	dev_dbg(data->dev,
+		"line_length %d, var->yres_virtual %d, data->mem_size %d\n",
+		line_length, var->yres_virtual, data->mem_size);
+	if (line_length * var->yres_virtual > data->map_size)
+		return -ENOMEM;
+
+	if (!stmp3xxx_lcd_iterate_pdata(pdata, get_matching_pentry, var))
+		return -EINVAL;
+
+	if (var->bits_per_pixel == 16) {
+		/* RGBA 5551 */
+		if (var->transp.length) {
+			var->red.offset = 0;
+			var->red.length = 5;
+			var->green.offset = 5;
+			var->green.length = 5;
+			var->blue.offset = 10;
+			var->blue.length = 5;
+			var->transp.offset = 15;
+			var->transp.length = 1;
+		} else {	/* RGB 565 */
+			var->red.offset = 0;
+			var->red.length = 5;
+			var->green.offset = 5;
+			var->green.length = 6;
+			var->blue.offset = 11;
+			var->blue.length = 5;
+			var->transp.offset = 0;
+			var->transp.length = 0;
+		}
+	} else {
+		var->red.offset = 16;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+	}
+
+	var->red.msb_right = 0;
+	var->green.msb_right = 0;
+	var->blue.msb_right = 0;
+	var->transp.msb_right = 0;
+
+	return 0;
+}
+
+
+static int stmp3xxxfb_wait_for_vsync(u32 channel, struct fb_info *info)
+{
+	struct stmp3xxx_fb_data *data = (struct stmp3xxx_fb_data *)info;
+	u32 count = data->vsync_count;
+	int ret = 0;
+
+	HW_LCDIF_CTRL1_SET(BM_LCDIF_CTRL1_VSYNC_EDGE_IRQ_EN);
+	ret = wait_event_interruptible_timeout(data->vsync_wait_q,
+			count != data->vsync_count, HZ / 10);
+	HW_LCDIF_CTRL1_CLR(BM_LCDIF_CTRL1_VSYNC_EDGE_IRQ_EN);
+	if (!ret) {
+		dev_err(data->dev, "wait for vsync timed out\n");
+		ret = -ETIMEDOUT;
+	}
+	return ret;
+}
+
+static int stmp3xxxfb_ioctl(struct fb_info *info, unsigned int cmd,
+			unsigned long arg)
+{
+	u32 channel = 0;
+	int ret = -EINVAL;
+
+	switch (cmd) {
+	case FBIO_WAITFORVSYNC:
+		if (!get_user(channel, (__u32 __user *) arg))
+			ret = stmp3xxxfb_wait_for_vsync(channel, info);
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+static int stmp3xxxfb_blank(int blank, struct fb_info *info)
+{
+	struct stmp3xxx_fb_data *data = (struct stmp3xxx_fb_data *)info;
+	int ret = data->pdata->cur->blank_panel ?
+		data->pdata->cur->blank_panel(blank) :
+		-ENOTSUPP;
+	if (ret == 0)
+		data->is_blank = (blank != FB_BLANK_UNBLANK);
+	return ret;
+}
+
+static int stmp3xxxfb_pan_display(struct fb_var_screeninfo *var,
+				struct fb_info *info)
+{
+	struct stmp3xxx_fb_data *data = (struct stmp3xxx_fb_data *)info;
+	int ret = 0;
+
+	pr_debug("%s: var->xoffset %d, info->var.xoffset %d\n",
+		__func__, var->xoffset, info->var.xoffset);
+	/* check if var is valid; also, xpan is not supported */
+	if (!var || (var->xoffset != info->var.xoffset) ||
+	    (var->yoffset + var->yres > var->yres_virtual)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (!data->pdata->cur->pan_display) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* update framebuffer visual */
+	data->cur_phys = data->phys_start +
+			info->fix.line_length * var->yoffset;
+	data->pdata->cur->pan_display(data->cur_phys);
+out:
+	return ret;
+}
+
+static struct fb_ops stmp3xxxfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = stmp3xxxfb_check_var,
+	.fb_set_par = stmp3xxxfb_set_par,
+	.fb_mmap = stmp3xxxfb_mmap,
+	.fb_setcolreg = stmp3xxxfb_setcolreg,
+	.fb_ioctl = stmp3xxxfb_ioctl,
+	.fb_blank = stmp3xxxfb_blank,
+	.fb_pan_display = stmp3xxxfb_pan_display,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+};
+
+static void init_timings(struct stmp3xxx_fb_data *data)
+{
+	unsigned phase_time;
+	unsigned timings;
+
+	/* Just use a phase_time of 1. As optimal as it gets, now. */
+	phase_time = 1;
+
+	/* Program all 4 timings the same */
+	timings = phase_time;
+	timings |= timings << 8;
+	timings |= timings << 16;
+	HW_LCDIF_TIMING_WR(timings);
+}
+
+#ifdef CONFIG_CPU_FREQ
+
+struct stmp3xxxfb_notifier_block {
+	struct stmp3xxx_fb_data *fb_data;
+	struct notifier_block nb;
+};
+
+static int stmp3xxxfb_notifier(struct notifier_block *self,
+				unsigned long phase, void *p)
+{
+	struct stmp3xxxfb_notifier_block *block =
+		container_of(self, struct stmp3xxxfb_notifier_block, nb);
+	struct stmp3xxx_fb_data *data = block->fb_data;
+
+	switch (phase) {
+	case CPUFREQ_POSTCHANGE:
+		stmp3xxxfb_blank(FB_BLANK_UNBLANK, &data->info);
+		break;
+
+	case CPUFREQ_PRECHANGE:
+		stmp3xxxfb_blank(FB_BLANK_POWERDOWN, &data->info);
+		break;
+
+	default:
+		dev_dbg(data->dev, "didn't handle notify %ld\n", phase);
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct stmp3xxxfb_notifier_block stmp3xxxfb_nb = {
+	.nb = {
+		.notifier_call = stmp3xxxfb_notifier,
+	},
+};
+#endif /* CONFIG_CPU_FREQ */
+
+static int get_max_memsize(struct stmp3xxx_platform_fb_entry *pentry,
+			   void *data, int ret_prev)
+{
+	struct stmp3xxx_fb_data *fbdata = data;
+	int sz = pentry->x_res * pentry->y_res * pentry->bpp / 8;
+	fbdata->mem_size = sz < ret_prev ? ret_prev : sz;
+	pr_debug("%s: mem_size now %d\n", __func__, fbdata->mem_size);
+	return fbdata->mem_size;
+}
+
+static int __devinit stmp3xxxfb_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct stmp3xxx_fb_data *data;
+	struct resource *res;
+	struct fb_info *info;
+	struct stmp3xxx_platform_fb_data *pdata = pdev->dev.platform_data;
+	struct stmp3xxx_platform_fb_entry *pentry;
+
+	if (pdata == NULL) {
+		ret = -ENODEV;
+		goto out;
+	}
+
+	if (default_panel_name) {
+		pentry = (void *)stmp3xxx_lcd_iterate_pdata(pdata,
+					get_matching_pentry_by_name,
+					default_panel_name);
+		if (pentry) {
+			stmp3xxx_lcd_move_pentry_up(pentry, pdata);
+			pdata->cur = pentry;
+		}
+	}
+	if (!default_panel_name || !pentry)
+		pentry = pdata->cur;
+	if (!pentry || !pentry->init_panel || !pentry->run_panel ||
+	    !pentry->release_panel) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	data = (struct stmp3xxx_fb_data *)framebuffer_alloc(
+				sizeof(struct stmp3xxx_fb_data) +
+				sizeof(u32) * 256 -
+				sizeof(struct fb_info), &pdev->dev);
+	if (data == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	cdata = data;
+	data->dev = &pdev->dev;
+	data->pdata = pdata;
+	platform_set_drvdata(pdev, data);
+	info = &data->info;
+
+	dev_dbg(&pdev->dev, "resolution %dx%d, bpp %d\n", pentry->x_res,
+		pentry->y_res, pentry->bpp);
+
+	stmp3xxx_lcd_iterate_pdata(pdata, get_max_memsize, data);
+
+	data->map_size = PAGE_ALIGN(data->mem_size) * NUM_SCREENS;
+	dev_dbg(&pdev->dev, "memory to allocate: %d\n", data->map_size);
+
+	data->virt_start = dma_alloc_writecombine(&pdev->dev,
+					data->map_size,
+					&data->phys_start,
+					GFP_KERNEL);
+
+	if (data->virt_start == NULL) {
+		ret = -ENOMEM;
+		goto out_dma;
+	}
+	dev_dbg(&pdev->dev, "allocated at %p:0x%x\n", data->virt_start,
+		data->phys_start);
+
+	stmp3xxxfb_default.bits_per_pixel = pentry->bpp;
+	/* NB: rotated */
+	stmp3xxxfb_default.xres = pentry->y_res;
+	stmp3xxxfb_default.yres = pentry->x_res;
+	stmp3xxxfb_default.xres_virtual = pentry->y_res;
+	stmp3xxxfb_default.yres_virtual = data->map_size /
+					(pentry->y_res * pentry->bpp / 8);
+	if (stmp3xxxfb_default.yres_virtual >= stmp3xxxfb_default.yres * 2)
+		stmp3xxxfb_default.yres_virtual = stmp3xxxfb_default.yres * 2;
+	else
+		stmp3xxxfb_default.yres_virtual = stmp3xxxfb_default.yres;
+
+	stmp3xxxfb_fix.smem_start = data->phys_start;
+	stmp3xxxfb_fix.smem_len = pentry->y_res * pentry->x_res * pentry->bpp /
+		8;
+	stmp3xxxfb_fix.ypanstep = 1;
+
+	switch (pentry->bpp) {
+	case 32:
+	case 24:
+		stmp3xxxfb_default.red.offset = 16;
+		stmp3xxxfb_default.red.length = 8;
+		stmp3xxxfb_default.green.offset = 8;
+		stmp3xxxfb_default.green.length = 8;
+		stmp3xxxfb_default.blue.offset = 0;
+		stmp3xxxfb_default.blue.length = 8;
+		break;
+
+	case 16:
+		stmp3xxxfb_default.red.offset = 11;
+		stmp3xxxfb_default.red.length = 5;
+		stmp3xxxfb_default.green.offset = 5;
+		stmp3xxxfb_default.green.length = 6;
+		stmp3xxxfb_default.blue.offset = 0;
+		stmp3xxxfb_default.blue.length = 5;
+		break;
+
+	default:
+		dev_err(&pdev->dev, "unsupported bitwidth %d\n", pentry->bpp);
+		ret = -EINVAL;
+		goto out_dma;
+	}
+
+	info->screen_base = data->virt_start;
+	info->fbops = &stmp3xxxfb_ops;
+
+	ret = fb_find_mode(&info->var, info, NULL, NULL, 0, NULL, 8);
+
+	if (!ret || (ret == 4))
+		info->var = stmp3xxxfb_default;
+	info->fix = stmp3xxxfb_fix;
+	info->pseudo_palette = &data->par;
+	data->par = NULL;
+	info->flags = FBINFO_FLAG_DEFAULT;
+
+	init_waitqueue_head(&data->vsync_wait_q);
+	data->vsync_count = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "cannot get IRQ resource\n");
+		ret = -ENODEV;
+		goto out_dma;
+	}
+	data->dma_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "cannot get IRQ resource\n");
+		ret = -ENODEV;
+		goto out_dma;
+	}
+	data->err_irq = res->start;
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret)
+		goto out_cmap;
+
+	stmp3xxx_init_lcdif();
+	ret = pentry->init_panel(data->dev, data->phys_start,
+				stmp3xxxfb_fix.smem_len, pentry);
+	if (ret) {
+		dev_err(&pdev->dev, "cannot initialize LCD panel\n");
+		goto out_panel;
+	}
+	dev_dbg(&pdev->dev, "LCD panel initialized\n");
+	init_timings(data);
+
+	ret = request_irq(data->dma_irq, lcd_irq_handler, 0, "fb_dma", data);
+	if (ret) {
+		dev_err(&pdev->dev, "request_irq (%d) failed with error %d\n",
+				data->dma_irq, ret);
+		goto out_panel;
+	}
+	ret = request_irq(data->err_irq, lcd_irq_handler, 0, "fb_error", data);
+	if (ret) {
+		dev_err(&pdev->dev, "request_irq (%d) failed with error %d\n",
+				data->err_irq, ret);
+		goto out_irq;
+	}
+	ret = register_framebuffer(info);
+	if (ret)
+		goto out_register;
+
+	pentry->run_panel();
+	dev_dbg(&pdev->dev, "LCD DMA channel has been started\n");
+	data->cur_phys = data->phys_start;
+	dev_dbg(&pdev->dev, "LCD running now\n");
+
+#ifdef CONFIG_CPU_FREQ
+	stmp3xxxfb_nb.fb_data = data;
+	cpufreq_register_notifier(&stmp3xxxfb_nb.nb,
+				CPUFREQ_TRANSITION_NOTIFIER);
+#endif /* CONFIG_CPU_FREQ */
+
+	goto out;
+
+out_register:
+	free_irq(data->err_irq, data);
+out_irq:
+	free_irq(data->dma_irq, data);
+out_panel:
+	fb_dealloc_cmap(&info->cmap);
+out_cmap:
+	dma_free_writecombine(&pdev->dev, data->map_size, data->virt_start,
+			data->phys_start);
+out_dma:
+	kfree(data);
+out:
+	return ret;
+}
+
+static int stmp3xxxfb_remove(struct platform_device *pdev)
+{
+	struct stmp3xxx_fb_data *data = platform_get_drvdata(pdev);
+	struct stmp3xxx_platform_fb_data *pdata = pdev->dev.platform_data;
+	struct stmp3xxx_platform_fb_entry *pentry = pdata->cur;
+
+	stmp3xxxfb_blank(FB_BLANK_POWERDOWN, &data->info);
+	if (pentry->stop_panel)
+		pentry->stop_panel();
+	pentry->release_panel(&pdev->dev, pentry);
+
+	unregister_framebuffer(&data->info);
+	framebuffer_release(&data->info);
+	fb_dealloc_cmap(&data->info.cmap);
+	free_irq(data->dma_irq, data);
+	free_irq(data->err_irq, data);
+	dma_free_writecombine(&pdev->dev, data->map_size, data->virt_start,
+			data->phys_start);
+	kfree(data);
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int stmp3xxxfb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct stmp3xxx_fb_data *data = platform_get_drvdata(pdev);
+	struct stmp3xxx_platform_fb_data *pdata = pdev->dev.platform_data;
+	struct stmp3xxx_platform_fb_entry *pentry = pdata->cur;
+	int ret;
+
+	ret = stmp3xxxfb_blank(FB_BLANK_POWERDOWN, &data->info);
+	if (ret)
+		goto out;
+	if (pentry->stop_panel)
+		pentry->stop_panel();
+	pentry->release_panel(data->dev, pentry);
+
+out:
+	return ret;
+}
+
+static int stmp3xxxfb_resume(struct platform_device *pdev)
+{
+	struct stmp3xxx_fb_data *data = platform_get_drvdata(pdev);
+	struct stmp3xxx_platform_fb_data *pdata = pdev->dev.platform_data;
+	struct stmp3xxx_platform_fb_entry *pentry = pdata->cur;
+
+	stmp3xxx_init_lcdif();
+	init_timings(data);
+	pentry->init_panel(data->dev, data->phys_start, data->info.fix.smem_len,
+			   pentry);
+	pentry->run_panel();
+	stmp3xxxfb_blank(FB_BLANK_UNBLANK, &data->info);
+	return 0;
+}
+#else
+#define stmp3xxxfb_suspend	NULL
+#define	stmp3xxxfb_resume	NULL
+#endif
+
+static struct platform_driver stmp3xxxfb_driver = {
+	.probe		= stmp3xxxfb_probe,
+	.remove		= stmp3xxxfb_remove,
+	.suspend	= stmp3xxxfb_suspend,
+	.resume		= stmp3xxxfb_resume,
+	.driver		= {
+		.name   = "stmp3xxx-fb",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init stmp3xxxfb_init(void)
+{
+	return platform_driver_register(&stmp3xxxfb_driver);
+}
+
+static void __exit stmp3xxxfb_exit(void)
+{
+	platform_driver_unregister(&stmp3xxxfb_driver);
+}
+
+module_init(stmp3xxxfb_init);
+module_exit(stmp3xxxfb_exit);
+
+/*
+ * LCD panel select
+ */
+static int __init default_panel_select(char *str)
+{
+	default_panel_name = str;
+	return 0;
+}
+__setup("lcd_panel=", default_panel_select);
+
+MODULE_AUTHOR("Vitaly Wool <vital@embeddedalley.com>");
+MODULE_DESCRIPTION("STMP3xxx Framebuffer Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/w1/masters/Kconfig b/drivers/w1/masters/Kconfig
index 90616822cd20..f75d0d10c21f 100644
--- a/drivers/w1/masters/Kconfig
+++ b/drivers/w1/masters/Kconfig
@@ -34,6 +34,12 @@ config W1_MASTER_DS2482
 	  This driver can also be built as a module.  If so, the module
 	  will be called ds2482.
 
+config W1_MASTER_MXC
+	tristate "Freescale MXC driver for 1-wire"
+	depends on W1 && ARCH_MXC
+	help
+	  Say Y here to enable MXC 1-wire host
+
 config W1_MASTER_DS1WM
 	tristate "Maxim DS1WM 1-wire busmaster"
 	depends on W1 && ARM && HAVE_CLK
diff --git a/drivers/w1/masters/Makefile b/drivers/w1/masters/Makefile
index bc4714a75f3a..c5a3e96fcbab 100644
--- a/drivers/w1/masters/Makefile
+++ b/drivers/w1/masters/Makefile
@@ -5,6 +5,8 @@
 obj-$(CONFIG_W1_MASTER_MATROX)		+= matrox_w1.o
 obj-$(CONFIG_W1_MASTER_DS2490)		+= ds2490.o
 obj-$(CONFIG_W1_MASTER_DS2482)		+= ds2482.o
+obj-$(CONFIG_W1_MASTER_MXC)		+= mxc_w1.o
+
 obj-$(CONFIG_W1_MASTER_DS1WM)		+= ds1wm.o
 obj-$(CONFIG_W1_MASTER_GPIO)		+= w1-gpio.o
 obj-$(CONFIG_HDQ_MASTER_OMAP)		+= omap_hdq.o
diff --git a/drivers/w1/masters/mxc_w1.c b/drivers/w1/masters/mxc_w1.c
new file mode 100644
index 000000000000..7c0e4a10d23d
--- /dev/null
+++ b/drivers/w1/masters/mxc_w1.c
@@ -0,0 +1,449 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup MXC_OWIRE MXC Driver for owire interface
+ */
+
+/*!
+ * @file mxc_w1.c
+ *
+ * @brief Driver for the Freescale Semiconductor MXC owire interface.
+ *
+ *
+ * @ingroup MXC_OWIRE
+ */
+
+/*!
+ * Include Files
+ */
+
+#include <asm/atomic.h>
+#include <asm/types.h>
+#include <asm/io.h>
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/pci_ids.h>
+#include <linux/pci.h>
+#include <linux/timer.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <mach/hardware.h>
+#include <asm/setup.h>
+
+#include "../w1.h"
+#include "../w1_int.h"
+#include "../w1_log.h"
+
+/*
+ * mxc function declarations
+ */
+
+static int __devinit mxc_w1_probe(struct platform_device *pdev);
+static int __devexit mxc_w1_remove(struct platform_device *pdev);
+static DECLARE_COMPLETION(transmit_done);
+extern void gpio_owire_active(void);
+extern void gpio_owire_inactive(void);
+
+/*
+ * MXC W1 Register offsets
+ */
+#define MXC_W1_CONTROL          0x00
+#define MXC_W1_TIME_DIVIDER     0x02
+#define MXC_W1_RESET            0x04
+#define MXC_W1_COMMAND          0x06
+#define MXC_W1_TXRX             0x08
+#define MXC_W1_INTERRUPT        0x0A
+#define MXC_W1_INTERRUPT_EN     0x0C
+DEFINE_SPINLOCK(w1_lock);
+
+/*!
+ * This structure contains pointers to  callback functions.
+ */
+static struct platform_driver mxc_w1_driver = {
+	.driver = {
+		   .name = "mxc_w1",
+		   },
+	.probe = mxc_w1_probe,
+	.remove = mxc_w1_remove,
+};
+
+/*!
+ * This structure is used to store
+ * information specific to w1 module.
+ */
+
+struct mxc_w1_device {
+	char *base_address;
+	unsigned long found;
+	unsigned int clkdiv;
+	struct clk *clk;
+	struct w1_bus_master *bus_master;
+};
+
+/*
+ * this is the low level routine to
+ * reset the device on the One Wire interface
+ * on the hardware
+ * @param data  the data field of the w1 device structure
+ * @return the function returns 0 when the reset pulse has
+ *  been generated
+ */
+static u8 mxc_w1_ds2_reset_bus(void *data)
+{
+	volatile u8 reg_val;
+	u8 ret;
+	struct mxc_w1_device *dev = (struct mxc_w1_device *)data;
+
+	__raw_writeb(0x80, (dev->base_address + MXC_W1_CONTROL));
+
+	do {
+		reg_val = __raw_readb(dev->base_address + MXC_W1_CONTROL);
+	} while (((reg_val >> 7) & 0x1) != 0);
+	ret = ((reg_val >> 7) & 0x1);
+	return ret;
+}
+
+/*!
+ * this is the low level routine to read/write a bit on the One Wire
+ * interface on the hardware
+ * @param data  the data field of the w1 device structure
+ * @param bit  0 = write-0 cycle, 1 = write-1/read cycle
+ * @return the function returns the bit read (0 or 1)
+ */
+static u8 mxc_w1_ds2_touch_bit(void *data, u8 bit)
+{
+
+	volatile u8 reg_val;
+	struct mxc_w1_device *dev = (struct mxc_w1_device *)data;
+	u8 ret = 0;
+
+	if (0 == bit) {
+		__raw_writeb((1 << 5), (dev->base_address + MXC_W1_CONTROL));
+
+		do {
+			reg_val =
+			    __raw_readb(dev->base_address + MXC_W1_CONTROL);
+		} while (0 != ((reg_val >> 5) & 0x1));
+	}
+
+	else {
+		__raw_writeb((1 << 4), dev->base_address + MXC_W1_CONTROL);
+		do {
+			reg_val =
+			    __raw_readb(dev->base_address + MXC_W1_CONTROL);
+		} while (0 != ((reg_val >> 4) & 0x1));
+
+		reg_val =
+		    (((__raw_readb(dev->base_address + MXC_W1_CONTROL)) >> 3) &
+		     0x1);
+		ret = (u8) (reg_val);
+	}
+
+	return ret;
+}
+
+static void mxc_w1_ds2_write_byte(void *data, u8 byte)
+{
+	struct mxc_w1_device *dev = (struct mxc_w1_device *)data;
+	INIT_COMPLETION(transmit_done);
+	__raw_writeb(byte, (dev->base_address + MXC_W1_TXRX));
+	__raw_writeb(0x10, (dev->base_address + MXC_W1_INTERRUPT_EN));
+	wait_for_completion(&transmit_done);
+}
+static u8 mxc_w1_ds2_read_byte(void *data)
+{
+	volatile u8 reg_val;
+	struct mxc_w1_device *dev = (struct mxc_w1_device *)data;
+	mxc_w1_ds2_write_byte(data, 0xFF);
+	reg_val = __raw_readb((dev->base_address + MXC_W1_TXRX));
+	return reg_val;
+}
+static u8 mxc_w1_read_byte(void *data)
+{
+	volatile u8 reg_val;
+	struct mxc_w1_device *dev = (struct mxc_w1_device *)data;
+	reg_val = __raw_readb((dev->base_address + MXC_W1_TXRX));
+	return reg_val;
+}
+static irqreturn_t w1_interrupt_handler(int irq, void *data)
+{
+	u8 reg_val;
+	irqreturn_t ret = IRQ_NONE;
+	struct mxc_w1_device *dev = (struct mxc_w1_device *)data;
+	reg_val = __raw_readb((dev->base_address + MXC_W1_INTERRUPT));
+	if ((reg_val & 0x10)) {
+		complete(&transmit_done);
+		reg_val = __raw_readb((dev->base_address + MXC_W1_TXRX));
+		ret = IRQ_HANDLED;
+	}
+	return ret;
+}
+void search_ROM_accelerator(void *data, struct w1_master *master, u8 search_type,
+			    w1_slave_found_callback cb)
+{
+	u64 rn[2], last_rn[2], rn2[2];
+	u64 rn1, rom_id, temp, temp1;
+	int i, j, z, w, last_zero, loop;
+	u8 bit, reg_val, bit2;
+	u8 byte, byte1;
+	int disc, prev_disc, last_disc;
+	struct mxc_w1_device *dev = (struct mxc_w1_device *)data;
+	last_rn[0] = 0;
+	last_rn[1] = 0;
+	rom_id = 0;
+	prev_disc = 0;
+	loop = 0;
+	disc = -1;
+	last_disc = 0;
+	last_zero = 0;
+	while (!last_zero) {
+		/*
+		 * Reset bus and all 1-wire device state machines
+		 * so they can respond to our requests.
+		 *
+		 * Return 0 - device(s) present, 1 - no devices present.
+		 */
+		if (mxc_w1_ds2_reset_bus(data)) {
+			pr_debug("No devices present on the wire.\n");
+			break;
+		}
+		rn[0] = 0;
+		rn[1] = 0;
+		__raw_writeb(0x80, (dev->base_address + MXC_W1_CONTROL));
+		mdelay(1);
+		mxc_w1_ds2_write_byte(data, 0xF0);
+		__raw_writeb(0x02, (dev->base_address + MXC_W1_COMMAND));
+		memcpy(rn2, last_rn, 16);
+		z = 0;
+		w = 0;
+		for (i = 0; i < 16; i++) {
+			reg_val = rn2[z] >> (8 * w);
+			mxc_w1_ds2_write_byte(data, reg_val);
+			reg_val = mxc_w1_read_byte(data);
+			if ((reg_val & 0x3) == 0x3) {
+				pr_debug("Device is Not Responding\n");
+				break;
+			}
+			for (j = 0; j < 8; j += 2) {
+				byte = 0xFF;
+				byte1 = 1;
+				byte ^= byte1 << j;
+				bit = (reg_val >> j) & 0x1;
+				bit2 = (reg_val >> j);
+				if (bit) {
+					prev_disc = disc;
+					disc = 8 * i + j;
+					reg_val &= byte;
+				}
+			}
+			rn1 = 0;
+			rn1 = reg_val;
+			rn[z] |= rn1 << (8 * w);
+			w++;
+			if (i == 7) {
+				z++;
+				w = 0;
+			}
+		}
+		if ((disc == -1) || (disc == prev_disc))
+			last_zero = 1;
+		if (disc == last_disc)
+			disc = prev_disc;
+		z = 0;
+		rom_id = 0;
+		for (i = 0, j = 1; i < 64; i++) {
+			temp = 0;
+			temp = (rn[z] >> j) & 0x1;
+			rom_id |= (temp << i);
+			j += 2;
+			if (i == 31) {
+				z++;
+				j = 1;
+			}
+
+		}
+		if (disc > 63) {
+			last_rn[0] = rn[0];
+			temp1 = rn[1];
+			loop = disc % 64;
+			temp = 1;
+			temp1 |= (temp << (loop + 1)) - 1;
+			temp1 |= (temp << (loop + 1));
+			last_rn[1] = temp1;
+
+		} else {
+			last_rn[1] = 0;
+			temp1 = rn[0];
+			temp = 1;
+			temp1 |= (temp << (loop + 1)) - 1;
+			temp1 |= (temp << (loop + 1));
+			last_rn[0] = temp1;
+		}
+		last_disc = disc;
+		cb(master, rom_id);
+	}
+}
+
+/*!
+ * this routine sets the One Wire clock
+ * to a value of 1 Mhz, as required by
+ * hardware.
+ * @param   dev   the device structure for w1
+ * @return  The function returns void
+ */
+static void mxc_w1_hw_init(struct mxc_w1_device *dev)
+{
+	clk_enable(dev->clk);
+
+	/* set the timer divider clock to divide by 65 */
+	/* as the clock to the One Wire is at 66.5MHz */
+	__raw_writeb(dev->clkdiv, dev->base_address + MXC_W1_TIME_DIVIDER);
+
+	return;
+}
+
+/*!
+ * this is the probe routine for the One Wire driver.
+ * It is called during the driver initilaization.
+ * @param   pdev   the platform device structure for w1
+ * @return The function returns 0 on success
+ * and a non-zero value on failure
+ *
+ */
+static int __devinit mxc_w1_probe(struct platform_device *pdev)
+{
+	struct mxc_w1_device *dev;
+	struct mxc_w1_config *data =
+	    (struct mxc_w1_config *)pdev->dev.platform_data;
+	int flag, ret_val, irq = -ENXIO;
+	int err = 0;
+	ret_val = 0;
+	flag = data->search_rom_accelerator;
+	dev = kzalloc(sizeof(struct mxc_w1_device) +
+		      sizeof(struct w1_bus_master), GFP_KERNEL);
+	if (!dev) {
+		return -ENOMEM;
+	}
+	dev->clk = clk_get(&pdev->dev, "owire_clk");
+	dev->bus_master = (struct w1_bus_master *)(dev + 1);
+	dev->found = 1;
+	dev->clkdiv = (clk_get_rate(dev->clk) / 1000000) - 1;
+	dev->base_address = (void *)IO_ADDRESS(OWIRE_BASE_ADDR);
+
+	mxc_w1_hw_init(dev);
+	dev->bus_master->data = dev;
+	dev->bus_master->reset_bus = &mxc_w1_ds2_reset_bus;
+	dev->bus_master->touch_bit = &mxc_w1_ds2_touch_bit;
+	if (flag) {
+		dev->bus_master->write_byte = &mxc_w1_ds2_write_byte;
+		dev->bus_master->read_byte = &mxc_w1_ds2_read_byte;
+		dev->bus_master->search = &search_ROM_accelerator;
+		irq = platform_get_irq(pdev, 0);
+		if (irq < 0) {
+			err = -ENOENT;
+			goto err_out_free_device;
+		}
+		ret_val =
+		    request_irq(irq, w1_interrupt_handler, 0, "mxc_w1", dev);
+		if (ret_val) {
+			pr_debug("OWire:request_irq(%d) returned error %d\n",
+				 irq, ret_val);
+			err = -1;
+			goto err_out_free_device;
+		}
+	}
+	err = w1_add_master_device(dev->bus_master);
+	if (err)
+		goto err_irq;
+
+	platform_set_drvdata(pdev, dev);
+	return 0;
+
+err_irq:
+	if (irq >= 0)
+		free_irq(irq, dev);
+err_out_free_device:
+	kfree(dev);
+	return err;
+}
+
+/*
+ * disassociate the w1 device from the driver
+ * @param   dev   the device structure for w1
+ * @return  The function returns void
+ */
+static int mxc_w1_remove(struct platform_device *pdev)
+{
+	struct mxc_w1_device *dev = platform_get_drvdata(pdev);
+	struct mxc_w1_config *data =
+	    (struct mxc_w1_config *)pdev->dev.platform_data;
+	int irq;
+
+	if (dev->found) {
+		w1_remove_master_device(dev->bus_master);
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if ((irq >= 0) && (data->search_rom_accelerator))
+		free_irq(irq, dev);
+
+	clk_disable(dev->clk);
+	clk_put(dev->clk);
+
+	kfree(dev);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+/*
+ * initialize the driver
+ * @return The function returns 0 on success
+ * and a non-zero value on failure
+ */
+
+static int __init mxc_w1_init(void)
+{
+	int ret;
+
+	printk(KERN_INFO "Serial: MXC OWire driver\n");
+
+	gpio_owire_active();
+
+	ret = platform_driver_register(&mxc_w1_driver);
+
+	return ret;
+}
+
+/*
+ * cleanup before the driver exits
+ */
+static void mxc_w1_exit(void)
+{
+	gpio_owire_inactive();
+	platform_driver_unregister(&mxc_w1_driver);
+}
+
+module_init(mxc_w1_init);
+module_exit(mxc_w1_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Freescale Semiconductors Inc");
+MODULE_DESCRIPTION("Driver for One-Wire on MXC");
diff --git a/drivers/w1/slaves/Kconfig b/drivers/w1/slaves/Kconfig
index 8d0b1fb1e52e..ad77126801f5 100644
--- a/drivers/w1/slaves/Kconfig
+++ b/drivers/w1/slaves/Kconfig
@@ -16,12 +16,40 @@ config W1_SLAVE_SMEM
 	  Say Y here if you want to connect 1-wire
 	  simple 64bit memory rom(ds2401/ds2411/ds1990*) to your wire.
 
+config W1_SLAVE_DS2431
+	tristate "1kb EEPROM family support (DS2431)"
+	help
+	  Say Y here if you want to use a 1-wire
+	  1kb EEPROM family device (DS2431)
+
+config W1_SLAVE_DS2751
+        tristate "Battery Level sensing support (DS2751)"
+        depends on W1
+        help
+          Say Y here if you want to use a 1-wire
+          battery level sensing device (DS2751).
+
+config W1_SLAVE_DS2751_CRC
+        bool "Protect DS2751 data with a CRC16"
+        depends on W1_SLAVE_DS2751
+        select CRC16
+        help
+          Say Y here to protect DS2751 data with a CRC16.
+          Each block has 30 bytes of data and a two byte CRC16.
+          Full block writes are only allowed if the CRC is valid.
+
 config W1_SLAVE_DS2433
 	tristate "4kb EEPROM family support (DS2433)"
 	help
 	  Say Y here if you want to use a 1-wire
 	  4kb EEPROM family device (DS2433).
 
+config W1_SLAVE_DS2438
+	tristate "Smart Battery Monitor (DS2438)"
+	help
+	  Say Y here if you want to use a 1-wire
+	  Smart Battery Monitor family device (DS2438).
+
 config W1_SLAVE_DS2433_CRC
 	bool "Protect DS2433 data with a CRC16"
 	depends on W1_SLAVE_DS2433
diff --git a/drivers/w1/slaves/Makefile b/drivers/w1/slaves/Makefile
index 990f400b6d22..fd53f53d8681 100644
--- a/drivers/w1/slaves/Makefile
+++ b/drivers/w1/slaves/Makefile
@@ -4,6 +4,9 @@
 
 obj-$(CONFIG_W1_SLAVE_THERM)	+= w1_therm.o
 obj-$(CONFIG_W1_SLAVE_SMEM)	+= w1_smem.o
+obj-$(CONFIG_W1_SLAVE_DS2431)	+= w1_ds2431.o
 obj-$(CONFIG_W1_SLAVE_DS2433)	+= w1_ds2433.o
 obj-$(CONFIG_W1_SLAVE_DS2760)	+= w1_ds2760.o
 obj-$(CONFIG_W1_SLAVE_BQ27000)	+= w1_bq27000.o
+obj-$(CONFIG_W1_SLAVE_DS2751)	+= w1_ds2751.o
+obj-$(CONFIG_W1_SLAVE_DS2438)	+= w1_ds2438.o
diff --git a/drivers/w1/slaves/w1_ds2438.c b/drivers/w1/slaves/w1_ds2438.c
new file mode 100644
index 000000000000..a3434ad5994c
--- /dev/null
+++ b/drivers/w1/slaves/w1_ds2438.c
@@ -0,0 +1,585 @@
+/*
+ * Copyright 2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <asm/types.h>
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/device.h>
+#include <linux/jiffies.h>
+#include <linux/workqueue.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/idr.h>
+#include <linux/power_supply.h>
+
+#include "../w1.h"
+#include "../w1_int.h"
+#include "../w1_family.h"
+#include "w1_ds2438.h"
+
+struct ds2438_device_info {
+	/* DS2438 data, valid after calling ds2438_battery_read_status() */
+	unsigned long update_time;	/* jiffies when data read */
+	char raw[DS2438_PAGE_SIZE];	/* raw DS2438 data */
+	int voltage_uV;
+	int current_uA;
+	int accum_current_uAh;
+	int temp_C;
+	int charge_status;
+	u8 init:1;
+	u8 setup:1;
+	u8 calibrate:1;
+	u8 input_src:1;
+	u8 ee_flg:1;
+	u8 resv_bit:3;
+	u8 threshold:8;
+	u16 resv_bytes;
+	u32 senser;
+
+	struct power_supply bat;
+	struct device *w1_dev;
+	struct ds2438_ops ops;
+	struct workqueue_struct *monitor_wqueue;
+	struct delayed_work monitor_work;
+};
+
+#define DS2438_SENSER	25
+#define to_ds2438_device_info(x) container_of((x), struct ds2438_device_info, \
+					      bat);
+
+
+static enum power_supply_property ds2438_battery_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_TEMP,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+};
+
+static char ds2438_sensers_title[] = "DS2438 senserin thousands of resister:";
+static unsigned int cache_time = 1000;
+module_param(cache_time, uint, 0644);
+MODULE_PARM_DESC(cache_time, "cache time in milliseconds");
+
+static ssize_t ds2438_show_input(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+
+	return sprintf(buf, "%s\n", di->input_src ? "1:VDD" : "0:VAD");
+}
+
+static ssize_t ds2438_show_senser(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	int len;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+
+	len = sprintf(buf, "%s\n", ds2438_sensers_title);
+	len += sprintf(buf + len, "%d\n", di->senser);
+	return len;
+}
+
+static ssize_t ds2438_show_ee(struct device *dev, struct device_attribute *attr,
+			      char *buf)
+{
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+
+	return sprintf(buf, "%d\n", di->ee_flg);
+}
+
+static ssize_t ds2438_show_threshold(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+
+	return sprintf(buf, "%d\n", di->threshold);
+}
+
+static ssize_t ds2438_set_input(struct device *dev,
+				struct device_attribute *attr, const char *buf,
+				size_t count)
+{
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+	di->input_src = !!simple_strtoul(buf, NULL, 0);
+	return count;
+}
+
+static ssize_t ds2438_set_senser(struct device *dev,
+				 struct device_attribute *attr, const char *buf,
+				 size_t count)
+{
+	u32 resister;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+	resister = simple_strtoul(buf, NULL, 0);
+	if (resister)
+		di->senser = resister;
+	return count;
+}
+
+static ssize_t ds2438_set_ee(struct device *dev, struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+
+	di->ee_flg = !!simple_strtoul(buf, NULL, 0);
+	di->setup = 1;
+	return count;
+}
+
+static ssize_t ds2438_set_threshold(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	int threshold;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+
+	threshold = simple_strtoul(buf, NULL, 0);
+	if (threshold < 256) {
+		di->threshold = threshold;
+		di->setup = 1;
+		return count;
+	}
+	return -EINVAL;
+}
+
+static ssize_t ds2438_set_calibrate(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+
+	di->calibrate = !!simple_strtoul(buf, NULL, 0);
+	return count;
+}
+
+static struct device_attribute ds2438_dev_attr[] = {
+	__ATTR(input_src, 0664, ds2438_show_input, ds2438_set_input),
+	__ATTR(senser, 0664, ds2438_show_senser, ds2438_set_senser),
+	__ATTR(ee_flg, 0664, ds2438_show_ee, ds2438_set_ee),
+	__ATTR(threshold, 0664, ds2438_show_threshold, ds2438_set_threshold),
+	__ATTR(calibrate, 0220, NULL, ds2438_set_calibrate),
+};
+
+static void ds2438_setup(struct ds2438_device_info *di)
+{
+	di->ops.load_sram(di->w1_dev, PAGE0_CONTROL);
+	di->ops.read_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	if (di->init && di->setup) {
+		if (di->ee_flg)
+			di->raw[PAGE0_STAT_CTRL] |= DS2438_CTRL_EE;
+		else
+			di->raw[PAGE0_STAT_CTRL] &= ~DS2438_CTRL_EE;
+		if (di->input_src)
+			di->raw[PAGE0_STAT_CTRL] |= DS2438_CTRL_AD;
+		else
+			di->raw[PAGE0_STAT_CTRL] &= ~DS2438_CTRL_AD;
+		di->raw[PAGE0_THRESHOLD] = di->threshold;
+	} else {
+		di->ee_flg = !!(di->raw[PAGE0_STAT_CTRL] & DS2438_CTRL_EE);
+		di->input_src = !!(di->raw[PAGE0_STAT_CTRL] & DS2438_CTRL_AD);
+		di->threshold = di->raw[PAGE0_THRESHOLD];
+		di->raw[PAGE0_STAT_CTRL] |= DS2438_CTRL_IAD | DS2438_CTRL_CA;
+	}
+	di->ops.write_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	di->ops.drain_sram(di->w1_dev, PAGE0_CONTROL);
+	if (!di->init) {
+		di->calibrate = 1;
+		di->init = 1;
+	}
+	di->setup = 0;
+}
+
+static void ds2438_calibrate(struct ds2438_device_info *di)
+{
+	int current_raw;
+	/* disable ICA */
+	di->ops.load_sram(di->w1_dev, PAGE0_CONTROL);
+	di->ops.read_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	di->raw[PAGE0_STAT_CTRL] &= ~DS2438_CTRL_IAD;
+	di->ops.write_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	di->ops.drain_sram(di->w1_dev, PAGE0_CONTROL);
+
+	/* Zero offset */
+	di->ops.load_sram(di->w1_dev, PAGE1_ETM);
+	di->ops.read_page(di->w1_dev, PAGE1_ETM, di->raw);
+	ds2438_writew(di->raw + PAGE1_OFFSET_LSB, 0);
+	di->ops.drain_sram(di->w1_dev, PAGE1_ETM_BYTE0);
+
+	/* enable ICA & read current */
+	di->ops.load_sram(di->w1_dev, PAGE0_CONTROL);
+	di->ops.read_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	di->raw[PAGE0_STAT_CTRL] |= DS2438_CTRL_IAD;
+	di->ops.write_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	di->ops.drain_sram(di->w1_dev, PAGE0_CONTROL);
+	/*wait current convert about 36HZ */
+	mdelay(30);
+	/* disable ICA */
+	di->ops.load_sram(di->w1_dev, PAGE0_CONTROL);
+	di->ops.read_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	di->raw[PAGE0_STAT_CTRL] &= ~DS2438_CTRL_IAD;
+	di->ops.write_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	di->ops.drain_sram(di->w1_dev, PAGE0_CONTROL);
+	/* read current value */
+	current_raw = ds2438_readw(di->raw + PAGE0_CURRENT_LSB);
+	/* write offset by current value */
+	di->ops.load_sram(di->w1_dev, PAGE1_ETM);
+	di->ops.read_page(di->w1_dev, PAGE1_ETM, di->raw);
+	ds2438_writew(di->raw + PAGE1_OFFSET_LSB, current_raw << 8);
+	di->ops.write_page(di->w1_dev, PAGE1_ETM, di->raw);
+	di->ops.drain_sram(di->w1_dev, PAGE1_ETM);
+
+	/*enable ICA again */
+	di->ops.load_sram(di->w1_dev, PAGE0_CONTROL);
+	di->ops.read_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	di->raw[PAGE0_STAT_CTRL] |= DS2438_CTRL_IAD;
+	di->ops.write_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	di->ops.drain_sram(di->w1_dev, PAGE0_CONTROL);
+	di->calibrate = 0;
+}
+
+/*
+ * power supply temperture is in tenths of degree.
+ */
+static inline int ds2438_get_temp(u16 raw)
+{
+	int degree, s;
+	s = !!(raw & 0x8000);
+
+	if (s)
+		raw = ((~raw & 0x7FFF) + 1);
+	degree = ((raw >> 8) * 10) + (((raw & 0xFF) * 5) + 63) / 128;
+	return s ? -degree : degree;
+}
+
+/*
+ * power supply current is in uA.
+ */
+static inline int ds2438_get_current(u32 senser, u16 raw)
+{
+	int s, current_uA;
+	s = !!(raw & 0xFC00);
+	/* (x * 1000 * 1000)uA / (4096 * (Rsens / 1000)) */
+	raw &= 0x3FF;
+	current_uA = raw * 125 * 125 * 125;
+	current_uA /= (senser << 3);
+	return s ? -current_uA : current_uA;
+}
+
+/*
+ * power supply current is in uAh.
+ */
+static inline int ds2438_get_ica(u32 senser, u8 raw)
+{
+	int charge_uAh;
+	/* (x * 1000 * 1000)uA / (2048 * (Rsens / 1000)) */
+	charge_uAh = (raw * 125 * 125 * 125) >> 4;
+	charge_uAh /= (senser << 4);
+	return charge_uAh;
+}
+
+static int ds2438_battery_update_page1(struct ds2438_device_info *di)
+{
+	int ica_raw;
+	di->ops.load_sram(di->w1_dev, PAGE1_ETM);
+	di->ops.read_page(di->w1_dev, PAGE1_ETM, di->raw);
+	ica_raw = di->raw[PAGE1_ICA];
+	di->accum_current_uAh = ds2438_get_ica(di->senser, ica_raw);
+	return 0;
+}
+
+static int ds2438_battery_read_status(struct ds2438_device_info *di)
+{
+	u8 status;
+	int temp_raw, voltage_raw, current_raw;
+
+	if (!(di->init) || di->setup)
+		ds2438_setup(di);
+
+	if (di->calibrate)
+		ds2438_calibrate(di);
+
+	if (di->update_time && time_before(jiffies, di->update_time +
+					   msecs_to_jiffies(cache_time)))
+		return 0;
+
+	di->ops.load_sram(di->w1_dev, PAGE0_CONTROL);
+	di->ops.read_page(di->w1_dev, PAGE0_CONTROL, di->raw);
+	status = di->raw[PAGE0_STAT_CTRL];
+	temp_raw = ds2438_readw(di->raw + PAGE0_TEMP_LSB);
+	voltage_raw = ds2438_readw(di->raw + PAGE0_VOLTAGE_LSB);
+	current_raw = ds2438_readw(di->raw + PAGE0_CURRENT_LSB);
+	di->temp_C = ds2438_get_temp(temp_raw);
+	di->voltage_uV = voltage_raw * 10000;
+	di->current_uA = ds2438_get_current(di->senser, current_raw);
+
+	ds2438_battery_update_page1(di);
+
+	if (!(status & DS2438_STAT_TB))
+		di->ops.command(di->w1_dev, DS2438_CONVERT_TEMP, 0);
+	if (!(status & DS2438_STAT_ADB))
+		di->ops.command(di->w1_dev, DS2438_CONVERT_VOLT, 0);
+	di->update_time = jiffies;
+	return 0;
+}
+
+static void ds2438_battery_update_status(struct ds2438_device_info *di)
+{
+	int old_charge_status = di->charge_status;
+
+	ds2438_battery_read_status(di);
+
+	if (di->charge_status != old_charge_status)
+		power_supply_changed(&di->bat);
+}
+
+static void ds2438_battery_work(struct work_struct *work)
+{
+	struct ds2438_device_info *di = container_of(work,
+						     struct ds2438_device_info,
+						     monitor_work.work);
+	const int interval = HZ * 60;
+
+	dev_dbg(di->w1_dev, "%s\n", __func__);
+
+	ds2438_battery_update_status(di);
+	queue_delayed_work(di->monitor_wqueue, &di->monitor_work, interval);
+}
+
+static void ds2438_battery_external_power_changed(struct power_supply *psy)
+{
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+
+	dev_dbg(di->w1_dev, "%s\n", __func__);
+
+	cancel_delayed_work(&di->monitor_work);
+	queue_delayed_work(di->monitor_wqueue, &di->monitor_work, HZ / 10);
+}
+
+static int ds2438_battery_get_property(struct power_supply *psy,
+				       enum power_supply_property psp,
+				       union power_supply_propval *val)
+{
+	struct ds2438_device_info *di = to_ds2438_device_info(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		val->intval = di->charge_status;
+		return 0;
+	default:
+		break;
+	}
+
+	ds2438_battery_read_status(di);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		val->intval = di->voltage_uV;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		val->intval = di->current_uA;
+		break;
+	case POWER_SUPPLY_PROP_TEMP:
+		val->intval = di->temp_C;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		val->intval = di->accum_current_uAh;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* W1 slave DS2438 famliy operations */
+static int ds2438_read_page(struct device *dev, u8 page, u8 *buf)
+{
+	struct w1_slave *slave = container_of(dev, struct w1_slave, dev);
+	if ((page >= DS2438_PAGE_NUM) || (buf == NULL))
+		return -EINVAL;
+
+	mutex_lock(&slave->master->mutex);
+	if (!w1_reset_select_slave(slave)) {
+		w1_write_8(slave->master, W1_READ_SCRATCHPAD);
+		w1_write_8(slave->master, page);
+		w1_read_block(slave->master, buf, DS2438_PAGE_SIZE);
+	}
+	mutex_unlock(&slave->master->mutex);
+	return 0;
+}
+
+static int ds2438_write_page(struct device *dev, u8 page, u8 *buf)
+{
+	struct w1_slave *slave = container_of(dev, struct w1_slave, dev);
+	if ((page >= DS2438_PAGE_NUM) || (buf == NULL))
+		return -EINVAL;
+
+	mutex_lock(&slave->master->mutex);
+	if (!w1_reset_select_slave(slave)) {
+		w1_write_8(slave->master, DS2438_WRITE_SCRATCHPAD);
+		w1_write_8(slave->master, page);
+		w1_write_block(slave->master, buf, DS2438_PAGE_SIZE);
+	}
+	mutex_unlock(&slave->master->mutex);
+	return 0;
+}
+
+static int ds2438_command(struct device *dev, u8 command, u8 data)
+{
+	struct w1_slave *slave = container_of(dev, struct w1_slave, dev);
+
+	mutex_lock(&slave->master->mutex);
+	if (!w1_reset_select_slave(slave)) {
+		w1_write_8(slave->master, command);
+		switch (command) {
+		case DS2438_COPY_SCRATCHPAD:
+		case DS2438_RECALL_MEMORY:
+			w1_write_8(slave->master, data);
+		}
+	}
+	mutex_unlock(&slave->master->mutex);
+	return 0;
+}
+
+static int ds2438_drain_sram(struct device *dev, u8 page)
+{
+	return ds2438_command(dev, DS2438_COPY_SCRATCHPAD, page);
+}
+
+static int ds2438_load_sram(struct device *dev, u8 page)
+{
+	return ds2438_command(dev, DS2438_RECALL_MEMORY, page);
+}
+
+static inline void ds2438_defaut_ops(struct ds2438_ops *ops)
+{
+	ops->read_page = ds2438_read_page;
+	ops->write_page = ds2438_write_page;
+	ops->drain_sram = ds2438_drain_sram;
+	ops->load_sram = ds2438_load_sram;
+	ops->command = ds2438_command;
+}
+
+static int ds2438_add_slave(struct w1_slave *slave)
+{
+	int i, retval = 0;
+	struct ds2438_device_info *di;
+
+	di = kzalloc(sizeof(*di), GFP_KERNEL);
+	if (!di) {
+		retval = -ENOMEM;
+		goto di_alloc_failed;
+	}
+
+	di->w1_dev = &slave->dev;
+	di->bat.name = slave->dev.bus_id;
+	di->bat.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->bat.properties = ds2438_battery_props;
+	di->bat.num_properties = ARRAY_SIZE(ds2438_battery_props);
+	di->bat.get_property = ds2438_battery_get_property;
+	di->bat.external_power_changed = ds2438_battery_external_power_changed;
+	ds2438_defaut_ops(&di->ops);
+	di->senser = DS2438_SENSER;
+	di->charge_status = POWER_SUPPLY_STATUS_UNKNOWN;
+
+	retval = power_supply_register(&slave->dev, &di->bat);
+	if (retval) {
+		dev_err(&slave->dev, "failed to register battery\n");
+		goto batt_failed;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ds2438_dev_attr); i++) {
+		if (device_create_file(di->bat.dev, ds2438_dev_attr + i)) {
+			printk(KERN_ERR "Customize attribute file fail!\n");
+			break;
+		}
+	}
+
+	if (i != ARRAY_SIZE(ds2438_dev_attr)) {
+		for (; i >= 0; i++)
+			device_remove_file(di->bat.dev, ds2438_dev_attr + i);
+		goto workqueue_failed;
+	}
+	INIT_DELAYED_WORK(&di->monitor_work, ds2438_battery_work);
+	di->monitor_wqueue = create_singlethread_workqueue(slave->dev.bus_id);
+	if (!di->monitor_wqueue) {
+		retval = -ESRCH;
+		goto workqueue_failed;
+	}
+	dev_set_drvdata(&slave->dev, di);
+	queue_delayed_work(di->monitor_wqueue, &di->monitor_work, HZ / 2);
+
+	goto success;
+
+      workqueue_failed:
+	power_supply_unregister(&di->bat);
+      batt_failed:
+	kfree(di);
+      di_alloc_failed:
+      success:
+	return retval;
+}
+
+static void ds2438_remove_slave(struct w1_slave *slave)
+{
+	struct ds2438_device_info *di = dev_get_drvdata(&slave->dev);
+
+	cancel_rearming_delayed_workqueue(di->monitor_wqueue,
+					  &di->monitor_work);
+	destroy_workqueue(di->monitor_wqueue);
+	power_supply_unregister(&di->bat);
+}
+
+static struct w1_family_ops w1_ds2438_fops = {
+	.add_slave = ds2438_add_slave,
+	.remove_slave = ds2438_remove_slave,
+};
+
+static struct w1_family w1_family_ds2438 = {
+	.fid = W1_FAMILY_DS2438,
+	.fops = &w1_ds2438_fops,
+};
+
+static int __init w1_ds2438_init(void)
+{
+	pr_info("1-wire driver for the DS2438 smart battery monitor\n");
+	return w1_register_family(&w1_family_ds2438);
+}
+
+static void __exit w1_ds2438_fini(void)
+{
+	w1_unregister_family(&w1_family_ds2438);
+}
+
+module_init(w1_ds2438_init);
+module_exit(w1_ds2438_fini);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Freescale Semiconductors Inc");
+MODULE_DESCRIPTION("1-wire DS2438 family, smart battery monitor.");
diff --git a/drivers/w1/slaves/w1_ds2438.h b/drivers/w1/slaves/w1_ds2438.h
new file mode 100644
index 000000000000..fe22b6ec253b
--- /dev/null
+++ b/drivers/w1/slaves/w1_ds2438.h
@@ -0,0 +1,119 @@
+/*
+ * Copyright 2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef __W1_DS2438_H__
+#define __W1_DS2438_H__
+
+#include <asm/types.h>
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/device.h>
+#include <linux/types.h>
+
+#define DS2438_DEV_NAME	"ds2438-battery"
+
+#define DS2438_PAGE_SIZE	8
+#define DS2438_PAGE_NUM		8
+
+#define DS2438_CONVERT_TEMP	0x44
+#define DS2438_CONVERT_VOLT	0xB4
+#define DS2438_WRITE_SCRATCHPAD	0x4E
+#define DS2438_COPY_SCRATCHPAD	0x48
+#define DS2438_RECALL_MEMORY	0xB8
+
+enum DS2438_PAGE {
+	PAGE0_CONTROL = 0,
+	PAGE1_ETM,
+	PAGE2_STAMP,
+	PAGE3_RESV3,
+	PAGE4_RESV4,
+	PAGE5_RESV5,
+	PAGE6_RESV6,
+	PAGE7_CCA,
+};
+
+enum DS2438_REG {
+	/* PAGE 0 */
+	PAGE0_STAT_CTRL = 0,
+	PAGE0_TEMP_LSB = 1,
+	PAGE0_TEMP_MSB = 2,
+	PAGE0_VOLTAGE_LSB = 3,
+	PAGE0_VOLTAGE_MSB = 4,
+	PAGE0_CURRENT_LSB = 5,
+	PAGE0_CURRENT_MSB = 6,
+	PAGE0_THRESHOLD = 7,
+
+	/* PAGE 1 */
+	PAGE1_ETM_BYTE0 = 0,
+	PAGE1_ETM_BYTE1 = 1,
+	PAGE1_ETM_BYTE2 = 2,
+	PAGE1_ETM_BYTE3 = 3,
+	PAGE1_ICA = 4,
+	PAGE1_OFFSET_LSB = 5,
+	PAGE1_OFFSET_MSB = 6,
+
+	/* PAGE 2 */
+	PAGE2_DISCONNECT_BYTE0 = 0,
+	PAGE2_DISCONNECT_BYTE1 = 1,
+	PAGE2_DISCONNECT_BYTE2 = 2,
+	PAGE2_DISCONNECT_BYTE3 = 3,
+	PAGE2_END_CHARGE_BYTE0 = 4,
+	PAGE2_END_CHARGE_BYTE1 = 5,
+	PAGE2_END_CHARGE_BYTE2 = 6,
+	PAGE2_END_CHARGE_BYTE3 = 7,
+
+	/* PAGE 3 */
+	/* PAGE 4 */
+	/* PAGE 5 */
+	/* PAGE 6 */
+	/* PAGE 7 */
+	PAGE7_CCA_LSB = 4,
+	PAGE7_CCA_MSB = 5,
+	PAGE7_DCA_LSB = 6,
+	PAGE7_DCA_MSB = 7,
+};
+
+#define DS2438_CTRL_IAD		(1 << 0)
+#define DS2438_CTRL_CA		(1 << 1)
+#define DS2438_CTRL_EE		(1 << 2)
+#define DS2438_CTRL_AD		(1 << 3)
+#define DS2438_STAT_TB		(1 << 4)
+#define DS2438_STAT_NVB		(1 << 5)
+#define DS2438_STAT_ADB		(1 << 6)
+
+struct ds2438_ops {
+	int (*read_page) (struct device *, u8, u8 *);
+	int (*read_byte) (struct device *, u8, u8, u8 *);
+	int (*read_halfword) (struct device *, u8, u8, u16 *);
+	int (*read_word) (struct device *, u8, u8, u32 *);
+	int (*write_page) (struct device *, u8, u8 *);
+	int (*write_byte) (struct device *, u8, u8, u8);
+	int (*write_halfword) (struct device *, u8, u8, u16);
+	int (*write_word) (struct device *, u8, u8, u32);
+	int (*drain_sram) (struct device *, u8);
+	int (*load_sram) (struct device *, u8);
+	int (*command) (struct device *, u8, u8);
+};
+
+static inline u16 ds2438_readw(u8 *raw)
+{
+	return ((*(raw + 1)) << 8) | (*raw);
+}
+
+static inline void ds2438_writew(u8 *raw, u16 data)
+{
+	*raw++ = data & 0xFF;
+	*raw = (data >> 8) & 0xFF;
+}
+#endif				/* __W1_DS2438_H__ */
diff --git a/drivers/w1/slaves/w1_ds2751.c b/drivers/w1/slaves/w1_ds2751.c
new file mode 100644
index 000000000000..9346a21bdc70
--- /dev/null
+++ b/drivers/w1/slaves/w1_ds2751.c
@@ -0,0 +1,317 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/*
+ * Implementation based on w1_ds2433.c
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#ifdef CONFIG_W1_F51_CRC
+#include <linux/crc16.h>
+
+#define CRC16_INIT		0
+#define CRC16_VALID		0xb001
+
+#endif
+
+#include "../w1.h"
+#include "../w1_int.h"
+#include "../w1_family.h"
+
+#define W1_EEPROM_SIZE		32
+#define W1_PAGE_SIZE		32
+#define W1_PAGE_BITS		5
+#define W1_PAGE_MASK		0x1F
+
+#define W1_F51_TIME		300
+
+#define W1_F51_READ_EEPROM	0xB8
+#define W1_F51_WRITE_SCRATCH	0x6C
+#define W1_F51_READ_SCRATCH	0x69
+#define W1_F51_COPY_SCRATCH	0x48
+#define W1_STATUS_OFFSET        0x0001
+#define W1_EEPROM_OFFSET        0x0007
+#define W1_SPECIAL_OFFSET       0x0008
+#define W1_EEPROM_BLOCK_0       0x0020
+#define W1_EEPROM_BLOCK_1       0x0030
+#define W1_SRAM                 0x0080
+struct w1_f51_data {
+	u8 memory[W1_EEPROM_SIZE];
+	u32 validcrc;
+};
+
+/**
+ * Check the file size bounds and adjusts count as needed.
+ * This would not be needed if the file size didn't reset to 0 after a write.
+ */
+static inline size_t w1_f51_fix_count(loff_t off, size_t count, size_t size)
+{
+	if (off > size)
+		return 0;
+
+	if ((off + count) > size)
+		return (size - off);
+
+	return count;
+}
+
+#ifdef CONFIG_W1_F51_CRC
+static int w1_f51_refresh_block(struct w1_slave *sl, struct w1_f51_data *data,
+				int block)
+{
+	u8 wrbuf[3];
+	int off = block * W1_PAGE_SIZE;
+	if (data->validcrc & (1 << block))
+		return 0;
+
+	if (w1_reset_select_slave(sl)) {
+		data->validcrc = 0;
+		return -EIO;
+	}
+	wrbuf[0] = W1_F51_READ_EEPROM;
+	wrbuf[1] = off & 0xff;
+	wrbuf[2] = off >> 8;
+	w1_write_block(sl->master, wrbuf, 3);
+	w1_read_block(sl->master, &data->memory[off], W1_PAGE_SIZE);
+
+	/* cache the block if the CRC is valid */
+	if (crc16(CRC16_INIT, &data->memory[off], W1_PAGE_SIZE) == CRC16_VALID)
+		data->validcrc |= (1 << block);
+
+	return 0;
+}
+#endif				/* CONFIG_W1_F51_CRC */
+
+static ssize_t w1_f51_read_bin(struct kobject *kobj, char *buf, loff_t off,
+			       size_t count)
+{
+	struct w1_slave *sl = kobj_to_w1_slave(kobj);
+#ifdef CONFIG_W1_F51_CRC
+	struct w1_f51_data *data = sl->family_data;
+	int i, min_page, max_page;
+#else
+	u8 wrbuf[3];
+#endif
+
+	if ((count = w1_f51_fix_count(off, count, W1_EEPROM_SIZE)) == 0) {
+		return 0;
+	}
+	mutex_lock(&sl->master->mutex);
+#ifdef CONFIG_W1_F51_CRC
+	min_page = (off >> W1_PAGE_BITS);
+	max_page = (off + count - 1) >> W1_PAGE_BITS;
+	for (i = min_page; i <= max_page; i++) {
+		if (w1_f51_refresh_block(sl, data, i)) {
+			count = -EIO;
+			goto out_up;
+		}
+	}
+	memcpy(buf, &data->memory[off], count);
+
+#else				/* CONFIG_W1_F51_CRC */
+
+	/* read directly from the EEPROM */
+	if (w1_reset_select_slave(sl)) {
+		count = -EIO;
+		goto out_up;
+	}
+	off = (loff_t) W1_EEPROM_BLOCK_0;
+	wrbuf[0] = W1_F51_READ_EEPROM;
+	wrbuf[1] = off & 0xff;
+	wrbuf[2] = off >> 8;
+	w1_write_block(sl->master, wrbuf, 3);
+	if (w1_reset_select_slave(sl)) {
+		count = -EIO;
+		goto out_up;
+	}
+
+	wrbuf[0] = W1_F51_READ_SCRATCH;
+	wrbuf[1] = off & 0xff;
+	wrbuf[2] = off >> 8;
+	w1_write_block(sl->master, wrbuf, 3);
+	w1_read_block(sl->master, buf, count);
+
+#endif				/* CONFIG_W1_F51_CRC */
+
+      out_up:
+	mutex_unlock(&sl->master->mutex);
+	return count;
+}
+
+/**
+ * Writes to the scratchpad and reads it back for verification.
+ * Then copies the scratchpad to EEPROM.
+ * The data must be on one page.
+ * The master must be locked.
+ *
+ * @param sl	The slave structure
+ * @param addr	Address for the write
+ * @param len   length must be <= (W1_PAGE_SIZE - (addr & W1_PAGE_MASK))
+ * @param data	The data to write
+ * @return	0=Success -1=failure
+ */
+static int w1_f51_write(struct w1_slave *sl, int addr, int len, const u8 * data)
+{
+	u8 wrbuf[4];
+	u8 rdbuf[W1_EEPROM_SIZE + 3];
+	u8 es = (addr + len - 1) & 0x1f;
+	/* Write the data to the scratchpad */
+	if (w1_reset_select_slave(sl))
+		return -1;
+	wrbuf[0] = W1_F51_WRITE_SCRATCH;
+	wrbuf[1] = addr & 0xff;
+	wrbuf[2] = addr >> 8;
+
+	w1_write_block(sl->master, wrbuf, 3);
+	w1_write_block(sl->master, data, len);
+	/* Read the scratchpad and verify */
+	if (w1_reset_select_slave(sl))
+		return -1;
+	wrbuf[0] = W1_F51_READ_SCRATCH;
+	w1_write_block(sl->master, wrbuf, 3);
+	w1_read_block(sl->master, rdbuf, len + 3);
+	/* Compare what was read against the data written */
+	if (memcmp(data, &rdbuf[0], len) != 0) {
+		printk("Error reading the scratch Pad\n");
+		return -1;
+	}
+	/* Copy the scratchpad to EEPROM */
+	if (w1_reset_select_slave(sl))
+		return -1;
+	wrbuf[0] = W1_F51_COPY_SCRATCH;
+	wrbuf[3] = es;
+	w1_write_block(sl->master, wrbuf, 4);
+	/* Sleep for 5 ms to wait for the write to complete */
+	msleep(5);
+
+	/* Reset the bus to wake up the EEPROM (this may not be needed) */
+	w1_reset_bus(sl->master);
+
+	return 0;
+}
+
+static ssize_t w1_f51_write_bin(struct kobject *kobj, char *buf, loff_t off,
+				size_t count)
+{
+	struct w1_slave *sl = kobj_to_w1_slave(kobj);
+	int addr;
+
+	if ((count = w1_f51_fix_count(off, count, W1_EEPROM_SIZE)) == 0)
+		return 0;
+	off = (loff_t) 0x0020;
+#ifdef CONFIG_W1_F51_CRC
+	/* can only write full blocks in cached mode */
+	if ((off & W1_PAGE_MASK) || (count & W1_PAGE_MASK)) {
+		dev_err(&sl->dev, "invalid offset/count off=%d cnt=%zd\n",
+			(int)off, count);
+		return -EINVAL;
+	}
+
+	/* make sure the block CRCs are valid */
+	for (idx = 0; idx < count; idx += W1_PAGE_SIZE) {
+		if (crc16(CRC16_INIT, &buf[idx], W1_PAGE_SIZE) != CRC16_VALID) {
+			dev_err(&sl->dev, "bad CRC at offset %d\n", (int)off);
+			return -EINVAL;
+		}
+	}
+#endif				/* CONFIG_W1_F51_CRC */
+
+	mutex_lock(&sl->master->mutex);
+
+	/* Can only write data to one page at a time */
+	addr = off;
+	if (w1_f51_write(sl, addr, count, buf) < 0) {
+		count = -EIO;
+		goto out_up;
+	}
+
+      out_up:
+	mutex_unlock(&sl->master->mutex);
+
+	return count;
+}
+
+static struct bin_attribute w1_f51_bin_attr = {
+	.attr = {
+		 .name = "eeprom",
+		 .mode = S_IRUGO | S_IWUSR,
+		 .owner = THIS_MODULE,
+		 },
+	.size = W1_EEPROM_SIZE,
+	.read = w1_f51_read_bin,
+	.write = w1_f51_write_bin,
+};
+
+static int w1_f51_add_slave(struct w1_slave *sl)
+{
+	int err;
+#ifdef CONFIG_W1_F51_CRC
+	struct w1_f51_data *data;
+	data = kmalloc(sizeof(struct w1_f51_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+	memset(data, 0, sizeof(struct w1_f51_data));
+	sl->family_data = data;
+
+#endif				/* CONFIG_W1_F51_CRC */
+
+	err = sysfs_create_bin_file(&sl->dev.kobj, &w1_f51_bin_attr);
+
+#ifdef CONFIG_W1_F51_CRC
+	if (err)
+		kfree(data);
+#endif				/* CONFIG_W1_F51_CRC */
+
+	return err;
+}
+
+static void w1_f51_remove_slave(struct w1_slave *sl)
+{
+#ifdef CONFIG_W1_F51_CRC
+	kfree(sl->family_data);
+	sl->family_data = NULL;
+#endif				/* CONFIG_W1_F51_CRC */
+	sysfs_remove_bin_file(&sl->dev.kobj, &w1_f51_bin_attr);
+}
+
+static struct w1_family_ops w1_f51_fops = {
+	.add_slave = w1_f51_add_slave,
+	.remove_slave = w1_f51_remove_slave,
+};
+
+static struct w1_family w1_family_51 = {
+	.fid = W1_EEPROM_DS2751,
+	.fops = &w1_f51_fops,
+};
+
+static int __init w1_f51_init(void)
+{
+	return w1_register_family(&w1_family_51);
+}
+
+static void __exit w1_f51_fini(void)
+{
+	w1_unregister_family(&w1_family_51);
+}
+
+module_init(w1_f51_init);
+module_exit(w1_f51_fini);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Freescale Semiconductors Inc");
+MODULE_DESCRIPTION
+    ("w1 family 51 driver for DS2751, Battery Level Sensing Device");
diff --git a/drivers/w1/w1_family.h b/drivers/w1/w1_family.h
index 3ca1b9298f21..c541f9215765 100644
--- a/drivers/w1/w1_family.h
+++ b/drivers/w1/w1_family.h
@@ -32,8 +32,10 @@
 #define W1_THERM_DS18S20 	0x10
 #define W1_THERM_DS1822  	0x22
 #define W1_EEPROM_DS2433  	0x23
+#define W1_EEPROM_DS2751        0x51
 #define W1_THERM_DS18B20 	0x28
 #define W1_EEPROM_DS2431	0x2D
+#define W1_FAMILY_DS2438	0x26
 #define W1_FAMILY_DS2760	0x30
 
 #define MAXNAMELEN		32
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 4fd3fa5546b1..edc87ff76d7c 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -196,6 +196,27 @@ config PNX4008_WATCHDOG
 
 	  Say N if you are unsure.
 
+config MXC_WATCHDOG
+	tristate "MXC watchdog"
+	depends on WATCHDOG && WATCHDOG_NOWAYOUT
+	depends on ARCH_MXC
+	help
+	  Watchdog timer embedded into MXC chips. This will
+	  reboot your system when timeout is reached.
+
+	  NOTE: once enabled, this timer cannot be disabled.
+	  To compile this driver as a module, choose M here: the
+	  module will be called mxc_wdt.
+
+config STMP3XXX_WATCHDOG
+	tristate "Sigmatel STMP3XXX watchdog"
+	depends on ARCH_STMP3XXX
+	help
+	  Say Y here if to include support for the watchdog timer
+	  for the Sigmatel STMP37XX/378X SoC.
+	  To compile this driver as a module, choose M here: the
+	  module will be called stmp3xxx_wdt.
+
 config IOP_WATCHDOG
 	tristate "IOP Watchdog"
 	depends on PLAT_IOP
@@ -233,6 +254,15 @@ config ORION5X_WATCHDOG
 	  To compile this driver as a module, choose M here: the
 	  module will be called orion5x_wdt.
 
+config NS9XXX_WATCHDOG
+	tristate "ns9xxx watchdog"
+	depends on PROCESSOR_NS921X || PROCESSOR_NS9360
+	help
+	  Say Y here if to include support for the watchdog timer
+	  in the ns9xxx processor.
+	  This driver can be built as a module by choosing M. The module
+	  will be called ns9xxx_wdt.
+
 # ARM26 Architecture
 
 # AVR32 Architecture
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index e352bbb7630b..181c0b79304a 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -37,10 +37,13 @@ obj-$(CONFIG_S3C2410_WATCHDOG) += s3c2410_wdt.o
 obj-$(CONFIG_SA1100_WATCHDOG) += sa1100_wdt.o
 obj-$(CONFIG_MPCORE_WATCHDOG) += mpcore_wdt.o
 obj-$(CONFIG_EP93XX_WATCHDOG) += ep93xx_wdt.o
+obj-$(CONFIG_MXC_WATCHDOG)	+= mxc_wdt.o
+obj-$(CONFIG_STMP3XXX_WATCHDOG) += stmp3xxx_wdt.o
 obj-$(CONFIG_PNX4008_WATCHDOG) += pnx4008_wdt.o
 obj-$(CONFIG_IOP_WATCHDOG) += iop_wdt.o
 obj-$(CONFIG_DAVINCI_WATCHDOG) += davinci_wdt.o
 obj-$(CONFIG_ORION5X_WATCHDOG) += orion5x_wdt.o
+obj-$(CONFIG_NS9XXX_WATCHDOG) += ns9xxx_wdt.o
 
 # ARM26 Architecture
 
diff --git a/drivers/watchdog/mxc_wdt.c b/drivers/watchdog/mxc_wdt.c
new file mode 100644
index 000000000000..833fa4cff224
--- /dev/null
+++ b/drivers/watchdog/mxc_wdt.c
@@ -0,0 +1,385 @@
+/*
+ * linux/drivers/char/watchdog/mxc_wdt.c
+ *
+ * Watchdog driver for FSL MXC. It is based on omap1610_wdt.c
+ *
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * 2005 (c) MontaVista Software, Inc.  All Rights Reserved.
+
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * History:
+ *
+ * 20051207: <AKuster@mvista.com>
+ *	     	Full rewrite based on
+ *		linux-2.6.15-rc5/drivers/char/watchdog/omap_wdt.c
+ *	     	Add platform resource support
+ *
+ */
+
+/*!
+ * @defgroup WDOG Watchdog Timer (WDOG) Driver
+ */
+/*!
+ * @file mxc_wdt.c
+ *
+ * @brief Watchdog timer driver
+ *
+ * @ingroup WDOG
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/miscdevice.h>
+#include <linux/watchdog.h>
+#include <linux/reboot.h>
+#include <linux/smp_lock.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/moduleparam.h>
+#include <linux/clk.h>
+
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <mach/hardware.h>
+#include <asm/irq.h>
+#include <asm/bitops.h>
+
+#include <mach/hardware.h>
+#include "mxc_wdt.h"
+#define DVR_VER "2.0"
+
+#define WDOG_SEC_TO_COUNT(s)  ((s * 2) << 8)
+#define WDOG_COUNT_TO_SEC(c)  ((c >> 8) / 2)
+
+static u32 wdt_base_reg;
+static int mxc_wdt_users;
+static struct clk *mxc_wdt_clk;
+
+static unsigned timer_margin = TIMER_MARGIN_DEFAULT;
+module_param(timer_margin, uint, 0);
+MODULE_PARM_DESC(timer_margin, "initial watchdog timeout (in seconds)");
+
+static unsigned dev_num = 0;
+
+static void mxc_wdt_ping(u32 base)
+{
+	/* issue the service sequence instructions */
+	__raw_writew(WDT_MAGIC_1, base + MXC_WDT_WSR);
+	__raw_writew(WDT_MAGIC_2, base + MXC_WDT_WSR);
+}
+
+static void mxc_wdt_config(u32 base)
+{
+	u16 val;
+
+	val = __raw_readw(base + MXC_WDT_WCR);
+	val |= 0xFF00 | WCR_WOE_BIT | WCR_WDA_BIT | WCR_SRS_BIT;
+	/* enable suspend WDT */
+	val |= WCR_WDZST_BIT | WCR_WDBG_BIT;
+	/* generate reset if wdog times out */
+	val &= ~WCR_WRE_BIT;
+
+	__raw_writew(val, base + MXC_WDT_WCR);
+}
+
+static void mxc_wdt_enable(u32 base)
+{
+	u16 val;
+
+	val = __raw_readw(base + MXC_WDT_WCR);
+	val |= WCR_WDE_BIT;
+	__raw_writew(val, base + MXC_WDT_WCR);
+}
+
+static void mxc_wdt_disable(u32 base)
+{
+	/* disable not supported by this chip */
+}
+
+static void mxc_wdt_adjust_timeout(unsigned new_timeout)
+{
+	if (new_timeout < TIMER_MARGIN_MIN)
+		new_timeout = TIMER_MARGIN_DEFAULT;
+	if (new_timeout > TIMER_MARGIN_MAX)
+		new_timeout = TIMER_MARGIN_MAX;
+	timer_margin = new_timeout;
+}
+
+static u16 mxc_wdt_get_timeout(u32 base)
+{
+	u16 val;
+
+	val = __raw_readw(base + MXC_WDT_WCR);
+	return WDOG_COUNT_TO_SEC(val);
+}
+
+static u16 mxc_wdt_get_bootreason(u32 base)
+{
+	u16 val;
+
+	val = __raw_readw(base + MXC_WDT_WRSR);
+	return val;
+}
+
+static void mxc_wdt_set_timeout(u32 base)
+{
+	u16 val;
+	val = __raw_readw(base + MXC_WDT_WCR);
+	val = (val & 0x00FF) | WDOG_SEC_TO_COUNT(timer_margin);
+	__raw_writew(val, base + MXC_WDT_WCR);
+	val = __raw_readw(base + MXC_WDT_WCR);
+	timer_margin = WDOG_COUNT_TO_SEC(val);
+}
+
+/*
+ *	Allow only one task to hold it open
+ */
+
+static int mxc_wdt_open(struct inode *inode, struct file *file)
+{
+
+	if (test_and_set_bit(1, (unsigned long *)&mxc_wdt_users))
+		return -EBUSY;
+
+	mxc_wdt_config(wdt_base_reg);
+	mxc_wdt_set_timeout(wdt_base_reg);
+	mxc_wdt_enable(wdt_base_reg);
+	mxc_wdt_ping(wdt_base_reg);
+
+	return 0;
+}
+
+static int mxc_wdt_release(struct inode *inode, struct file *file)
+{
+	/*
+	 *      Shut off the timer unless NOWAYOUT is defined.
+	 */
+#ifndef CONFIG_WATCHDOG_NOWAYOUT
+	mxc_wdt_disable(wdt_base_reg);
+
+#else
+	printk(KERN_CRIT "mxc_wdt: Unexpected close, not stopping!\n");
+#endif
+	mxc_wdt_users = 0;
+	return 0;
+}
+
+static ssize_t
+mxc_wdt_write(struct file *file, const char __user * data,
+	      size_t len, loff_t * ppos)
+{
+	/* Refresh LOAD_TIME. */
+	if (len)
+		mxc_wdt_ping(wdt_base_reg);
+	return len;
+}
+
+static int
+mxc_wdt_ioctl(struct inode *inode, struct file *file,
+	      unsigned int cmd, unsigned long arg)
+{
+	int new_margin;
+	int bootr;
+
+	static struct watchdog_info ident = {
+		.identity = "MXC Watchdog",
+		.options = WDIOF_SETTIMEOUT,
+		.firmware_version = 0,
+	};
+
+	switch (cmd) {
+	default:
+		return -ENOIOCTLCMD;
+	case WDIOC_GETSUPPORT:
+		return copy_to_user((struct watchdog_info __user *)arg, &ident,
+				    sizeof(ident));
+	case WDIOC_GETSTATUS:
+		return put_user(0, (int __user *)arg);
+	case WDIOC_GETBOOTSTATUS:
+		bootr = mxc_wdt_get_bootreason(wdt_base_reg);
+		return put_user(bootr, (int __user *)arg);
+	case WDIOC_KEEPALIVE:
+		mxc_wdt_ping(wdt_base_reg);
+		return 0;
+	case WDIOC_SETTIMEOUT:
+		if (get_user(new_margin, (int __user *)arg))
+			return -EFAULT;
+
+		mxc_wdt_adjust_timeout(new_margin);
+		mxc_wdt_disable(wdt_base_reg);
+		mxc_wdt_set_timeout(wdt_base_reg);
+		mxc_wdt_enable(wdt_base_reg);
+		mxc_wdt_ping(wdt_base_reg);
+		return 0;
+
+	case WDIOC_GETTIMEOUT:
+		mxc_wdt_ping(wdt_base_reg);
+		new_margin = mxc_wdt_get_timeout(wdt_base_reg);
+		return put_user(new_margin, (int __user *)arg);
+	}
+}
+
+static struct file_operations mxc_wdt_fops = {
+	.owner = THIS_MODULE,
+	.write = mxc_wdt_write,
+	.ioctl = mxc_wdt_ioctl,
+	.open = mxc_wdt_open,
+	.release = mxc_wdt_release,
+};
+
+static struct miscdevice mxc_wdt_miscdev = {
+	.minor = WATCHDOG_MINOR,
+	.name = "watchdog",
+	.fops = &mxc_wdt_fops
+};
+
+static int __init mxc_wdt_probe(struct platform_device *pdev)
+{
+	struct resource *res, *mem;
+	int ret;
+
+	/* reserve static register mappings */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, dev_num);
+	if (!res)
+		return -ENOENT;
+
+	mem = request_mem_region(res->start, res->end - res->start + 1,
+				 pdev->name);
+	if (mem == NULL)
+		return -EBUSY;
+
+	platform_set_drvdata(pdev, mem);
+
+	wdt_base_reg = IO_ADDRESS(res->start);
+	mxc_wdt_disable(wdt_base_reg);
+	mxc_wdt_adjust_timeout(timer_margin);
+
+	mxc_wdt_users = 0;
+
+	mxc_wdt_miscdev.this_device = &pdev->dev;
+
+	mxc_wdt_clk = clk_get(NULL, "wdog_clk");
+	clk_enable(mxc_wdt_clk);
+
+	ret = misc_register(&mxc_wdt_miscdev);
+	if (ret)
+		goto fail;
+
+	pr_info("MXC Watchdog # %d Timer: initial timeout %d sec\n", dev_num,
+		timer_margin);
+
+	return 0;
+
+      fail:
+	release_resource(mem);
+	pr_info("MXC Watchdog Probe failed\n");
+	return ret;
+}
+
+static void mxc_wdt_shutdown(struct platform_device *pdev)
+{
+	struct resource *res = platform_get_drvdata(pdev);
+	mxc_wdt_disable(wdt_base_reg);
+	pr_info("MXC Watchdog # %d shutdown\n", dev_num);
+}
+
+static int __exit mxc_wdt_remove(struct platform_device *pdev)
+{
+	struct resource *mem = platform_get_drvdata(pdev);
+	misc_deregister(&mxc_wdt_miscdev);
+	release_resource(mem);
+	pr_info("MXC Watchdog # %d removed\n", dev_num);
+	return 0;
+}
+
+#ifdef	CONFIG_PM
+
+/* REVISIT ... not clear this is the best way to handle system suspend; and
+ * it's very inappropriate for selective device suspend (e.g. suspending this
+ * through sysfs rather than by stopping the watchdog daemon).  Also, this
+ * may not play well enough with NOWAYOUT...
+ */
+
+static int mxc_wdt_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct resource *res = platform_get_drvdata(pdev);
+
+	if (mxc_wdt_users) {
+		mxc_wdt_disable(wdt_base_reg);
+	}
+	return 0;
+}
+
+static int mxc_wdt_resume(struct platform_device *pdev)
+{
+	struct resource *res = platform_get_drvdata(pdev);
+	if (mxc_wdt_users) {
+		mxc_wdt_enable(wdt_base_reg);
+		mxc_wdt_ping(wdt_base_reg);
+	}
+	return 0;
+}
+
+#else
+#define	mxc_wdt_suspend	NULL
+#define	mxc_wdt_resume		NULL
+#endif
+
+static struct platform_driver mxc_wdt_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "mxc_wdt",
+		   },
+	.probe = mxc_wdt_probe,
+	.shutdown = mxc_wdt_shutdown,
+	.remove = __exit_p(mxc_wdt_remove),
+	.suspend = mxc_wdt_suspend,
+	.resume = mxc_wdt_resume,
+};
+
+static int __init mxc_wdt_init(void)
+{
+	pr_info("MXC WatchDog Driver %s\n", DVR_VER);
+
+	if ((timer_margin < TIMER_MARGIN_MIN) ||
+	    (timer_margin > TIMER_MARGIN_MAX)) {
+		pr_info("MXC watchdog error. wrong timer_margin %d\n",
+			timer_margin);
+		pr_info("    Range: %d to %d seconds\n", TIMER_MARGIN_MIN,
+			TIMER_MARGIN_MAX);
+		return -EINVAL;
+	}
+
+	return platform_driver_register(&mxc_wdt_driver);
+}
+
+static void __exit mxc_wdt_exit(void)
+{
+	platform_driver_unregister(&mxc_wdt_driver);
+	pr_info("MXC WatchDog Driver removed\n");
+}
+
+module_init(mxc_wdt_init);
+module_exit(mxc_wdt_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
diff --git a/drivers/watchdog/mxc_wdt.h b/drivers/watchdog/mxc_wdt.h
new file mode 100644
index 000000000000..cd09b9acf99f
--- /dev/null
+++ b/drivers/watchdog/mxc_wdt.h
@@ -0,0 +1,37 @@
+/*
+ *  linux/drivers/char/watchdog/mxc_wdt.h
+ *
+ *  BRIEF MODULE DESCRIPTION
+ *      MXC Watchdog timer register definitions
+ *
+ * Author: MontaVista Software, Inc.
+ *       <AKuster@mvista.com> or <source@mvista.com>
+ *
+ * 2005 (c) MontaVista Software, Inc.
+ * Copyright 2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#ifndef __MXC_WDT_H__
+#define __MXC_WDT_H__
+
+#define MXC_WDT_WCR             0x00
+#define MXC_WDT_WSR             0x02
+#define MXC_WDT_WRSR            0x04
+#define WCR_WOE_BIT             (1 << 6)
+#define WCR_WDA_BIT             (1 << 5)
+#define WCR_SRS_BIT             (1 << 4)
+#define WCR_WRE_BIT             (1 << 3)
+#define WCR_WDE_BIT             (1 << 2)
+#define WCR_WDBG_BIT            (1 << 1)
+#define WCR_WDZST_BIT           (1 << 0)
+#define WDT_MAGIC_1             0x5555
+#define WDT_MAGIC_2             0xAAAA
+
+#define TIMER_MARGIN_MAX    	127
+#define TIMER_MARGIN_DEFAULT	60	/* 60 secs */
+#define TIMER_MARGIN_MIN	1
+
+#endif				/* __MXC_WDT_H__ */
diff --git a/drivers/watchdog/ns9xxx_wdt.c b/drivers/watchdog/ns9xxx_wdt.c
new file mode 100644
index 000000000000..6234fbaa1609
--- /dev/null
+++ b/drivers/watchdog/ns9xxx_wdt.c
@@ -0,0 +1,330 @@
+/*
+ * drivers/watchdog/ns9xxx.c
+ *
+ * based on at91rm9200_wdt.c by Andrew Victor
+ *
+ * Copyright (C) 2009 Digi International Inc.
+ * 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 version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/io.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/watchdog.h>
+
+#define WDT_CONFIG			(0)
+#define WDT_TIMER			(4)
+
+#define WDT_CONFIG_ENABLE		(1 << 7)
+#define WDT_CONFIG_IRQCLEAR		(1 << 5)
+#define WDT_CONFIG_RESPONSE		(1 << 4)
+#define WDT_CONFIG_DIV64		(0x5)
+
+#define DRIVER_NAME			"ns9xxx-wdt"
+#define DEFAULT_TIME			10
+
+#define ns9xxx_wdt_pat()				\
+{							\
+	iowrite32(pdata.multiplier * timeout,		\
+			pdata.ioaddr + WDT_TIMER);	\
+}
+
+struct ns9xxx_wdt_pdata {
+	void __iomem		*ioaddr;
+	struct resource		*mem;
+	struct clk		*clk;
+
+	unsigned int		multiplier;
+	unsigned int		timeout_max;
+
+	unsigned long		busy;
+};
+
+static struct ns9xxx_wdt_pdata pdata;
+
+static unsigned int timeout = DEFAULT_TIME;
+module_param(timeout, int, 0);
+MODULE_PARM_DESC(timeout, "Watchdog Timeout in seconds. "
+		"(default=" __MODULE_STRING(DEFAULT_TIME) ")");
+
+static struct watchdog_info ns9xxx_wdt_info = {
+	.identity	= "ns9xxx watchdog",
+	.options	= WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING,
+};
+
+/* disable watchdog */
+static inline void ns9xxx_wdt_stop(void)
+{
+	unsigned long cfg;
+
+	/* watchdog cannot be disabled,
+	 * but stopped by clearing WDT_CONFIG_IRQCLEAR  */
+	cfg = ioread32(pdata.ioaddr + WDT_CONFIG) & ~WDT_CONFIG_IRQCLEAR;
+	iowrite32(cfg, pdata.ioaddr + WDT_CONFIG);
+}
+
+/* enable/reset watchdog */
+static inline void ns9xxx_wdt_start(void)
+{
+	ns9xxx_wdt_pat();
+
+	/* enable watchdog
+	 * reset interrupt
+	 * action = reset device
+	 * divisor = 64 (slowest)
+	 */
+	iowrite32(WDT_CONFIG_ENABLE | WDT_CONFIG_IRQCLEAR
+			| WDT_CONFIG_RESPONSE | WDT_CONFIG_DIV64,
+			pdata.ioaddr + WDT_CONFIG);
+}
+
+/* device is opened, start watchdog */
+static int ns9xxx_wdt_open(struct inode *inode, struct file *file)
+{
+	if (test_and_set_bit(0, &pdata.busy))
+		return -EBUSY;
+
+	ns9xxx_wdt_start();
+	return nonseekable_open(inode, file);
+}
+
+/* device is closed, watchdog will not(!) be disabled */
+static int ns9xxx_wdt_close(struct inode *inode, struct file *file)
+{
+	clear_bit(0, &pdata.busy);
+	return 0;
+}
+
+/* update the time interval */
+static int ns9xxx_wdt_settimeout(int new_time)
+{
+	/* skip check if we do not have a clock speed yet */
+	if ((new_time <= 0) || (new_time > pdata.timeout_max))
+		return -EINVAL;
+
+	timeout = new_time;
+	return 0;
+}
+
+/* handle ioctl */
+static int ns9xxx_wdt_ioctl(struct inode *inode, struct file *file,
+		unsigned int cmd, unsigned long arg)
+{
+	void __user *argp = (void __user *)arg;
+	int __user *p = argp;
+	int new_value;
+	int ret;
+
+	switch (cmd) {
+	case WDIOC_KEEPALIVE:
+		ns9xxx_wdt_pat();
+		return 0;
+	case WDIOC_GETSUPPORT:
+		return copy_to_user(argp, &ns9xxx_wdt_info,
+				sizeof(ns9xxx_wdt_info)) ? -EFAULT : 0;
+	case WDIOC_SETTIMEOUT:
+		if (get_user(new_value, p))
+			return -EFAULT;
+		ret = ns9xxx_wdt_settimeout(new_value);
+		if (ret)
+			return ret;
+		ns9xxx_wdt_start();
+		return put_user(timeout, p);
+	case WDIOC_GETTIMEOUT:
+		return put_user(timeout, p);
+	case WDIOC_GETSTATUS:
+	case WDIOC_GETBOOTSTATUS:
+		return put_user(0, p);
+	case WDIOC_SETOPTIONS:
+		if (get_user(new_value, p))
+			return -EFAULT;
+		if (new_value & WDIOS_ENABLECARD)
+			ns9xxx_wdt_start();
+		return 0;
+	default:
+		return -ENOTTY;
+	}
+}
+
+/* pat watchdog on every write to the device */
+static ssize_t ns9xxx_wdt_write(struct file *file,
+		const char __user *data, size_t len, loff_t *ppos)
+{
+	ns9xxx_wdt_pat();
+	return len;
+}
+
+static const struct file_operations ns9xxx_wdt_fops = {
+	.owner		= THIS_MODULE,
+	.llseek		= no_llseek,
+	.ioctl		= ns9xxx_wdt_ioctl,
+	.open		= ns9xxx_wdt_open,
+	.release	= ns9xxx_wdt_close,
+	.write		= ns9xxx_wdt_write,
+};
+
+static struct miscdevice ns9xxx_wdt_miscdev = {
+	.minor		= WATCHDOG_MINOR,
+	.name		= DRIVER_NAME,
+	.fops		= &ns9xxx_wdt_fops,
+};
+
+static int __devinit ns9xxx_wdt_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	if (ns9xxx_wdt_miscdev.parent)
+		return -EBUSY;
+
+	ns9xxx_wdt_miscdev.parent = &pdev->dev;
+
+	pdata.mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!pdata.mem) {
+		pr_err(DRIVER_NAME ": memory not available\n");
+		return -ENOENT;
+	}
+
+	if (!request_mem_region(pdata.mem->start,
+				pdata.mem->end - pdata.mem->start + 1,
+				DRIVER_NAME)) {
+		pr_err(DRIVER_NAME ": memory already mapped\n");
+		ret = -EIO;
+		goto err_mem;
+	}
+
+	pdata.ioaddr = ioremap(pdata.mem->start,
+			pdata.mem->end - pdata.mem->start + 1);
+	if (!pdata.ioaddr) {
+		pr_err(DRIVER_NAME ": unable to remap IO memory\n");
+		ret = -EIO;
+		goto err_remap;
+	}
+
+	pdata.clk = clk_get(&pdev->dev, DRIVER_NAME);
+	if (IS_ERR(pdata.clk)) {
+		pr_err(DRIVER_NAME ": clock not available\n");
+		ret = PTR_ERR(pdata.clk);
+		goto err_clk_get;
+	}
+
+	if (!clk_get_rate(pdata.clk)) {
+		pr_err(DRIVER_NAME ": cannot get clock rate\n");
+		ret = -EIO;
+		goto err_clk_rate;
+	}
+
+	/* calculate counter speed and maximum time
+	 * clock speed is cpu speed divided by 64 (slowest mode)
+	 * counter uses full 32bit register
+	 */
+
+	pdata.multiplier = clk_get_rate(pdata.clk) / 64;
+	pdata.timeout_max = 0;
+	pdata.timeout_max = ~pdata.timeout_max / pdata.multiplier;
+
+	/* recheck timeout for overflow */
+	ns9xxx_wdt_settimeout(timeout);
+
+	ret = misc_register(&ns9xxx_wdt_miscdev);
+	if (ret) {
+		pr_err(DRIVER_NAME ": cannot register misc device\n");
+		goto err_register;
+	}
+
+	dev_info(&pdev->dev, "NS9xxx watchdog timer at 0x%p\n",
+			pdata.ioaddr);
+	return 0;
+
+err_register:
+err_clk_rate:
+	clk_put(pdata.clk);
+err_clk_get:
+	iounmap(pdata.ioaddr);
+err_remap:
+	release_mem_region(pdata.mem->start,
+			 pdata.mem->end - pdata.mem->start + 1);
+err_mem:
+	release_resource(pdata.mem);
+
+	return ret;
+}
+
+static int __devexit ns9xxx_wdt_remove(struct platform_device *pdev)
+{
+	int res;
+
+	res = misc_deregister(&ns9xxx_wdt_miscdev);
+	if (!res)
+		ns9xxx_wdt_miscdev.parent = NULL;
+
+	return res;
+}
+
+static void ns9xxx_wdt_shutdown(struct platform_device *pdev)
+{
+	ns9xxx_wdt_stop();
+}
+
+#ifdef CONFIG_PM
+
+static int ns9xxx_wdt_suspend(struct platform_device *pdev,
+		pm_message_t message)
+{
+	ns9xxx_wdt_stop();
+	return 0;
+}
+
+static int ns9xxx_wdt_resume(struct platform_device *pdev)
+{
+	if (pdata.busy)
+		ns9xxx_wdt_start();
+	return 0;
+}
+
+#else
+# define ns9xxx_wdt_suspend NULL
+# define ns9xxx_wdt_resume	NULL
+#endif
+
+
+static struct platform_driver ns9xxx_wdt_driver = {
+	.probe		= ns9xxx_wdt_probe,
+	.remove		= __devexit_p(ns9xxx_wdt_remove),
+	.shutdown	= ns9xxx_wdt_shutdown,
+	.suspend	= ns9xxx_wdt_suspend,
+	.resume		= ns9xxx_wdt_resume,
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init ns9xxx_wdt_init(void)
+{
+	return platform_driver_register(&ns9xxx_wdt_driver);
+}
+
+static void __exit ns9xxx_wdt_exit(void)
+{
+	return platform_driver_unregister(&ns9xxx_wdt_driver);
+}
+
+module_init(ns9xxx_wdt_init);
+module_exit(ns9xxx_wdt_exit);
+
+MODULE_AUTHOR("Digi International Inc.");
+MODULE_DESCRIPTION("Digi NS9xxx Watchdog Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/watchdog/stmp3xxx_wdt.c b/drivers/watchdog/stmp3xxx_wdt.c
new file mode 100644
index 000000000000..72b0e0d01c00
--- /dev/null
+++ b/drivers/watchdog/stmp3xxx_wdt.c
@@ -0,0 +1,301 @@
+/*
+ * Watchdog driver for Freescale STMP37XX/STMP378X
+ *
+ * Author: Vitaly Wool <vital@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/watchdog.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/spinlock.h>
+#include <linux/init.h>
+#include <linux/uaccess.h>
+
+#include <mach/regs-rtc.h>
+
+#define DEFAULT_HEARTBEAT 19
+#define MAX_HEARTBEAT     (0x10000000 >> 6) /* actually 0x100000000 >> 10 */
+
+/* missing bitmask in headers */
+#define BV_RTC_PERSISTENT1_GENERAL__RTC_FORCE_UPDATER     0x80000000
+
+#define WDT_IN_USE        0
+#define WDT_OK_TO_CLOSE   1
+#define WDT_REGION_INITED 2
+#define WDT_DEVICE_INITED 3
+
+#define WDOG_COUNTER_RATE	1000 /* 1 kHz clock */
+
+DEFINE_SPINLOCK(io_lock);
+static unsigned long wdt_status;
+static const int  nowayout = WATCHDOG_NOWAYOUT;
+static int heartbeat = DEFAULT_HEARTBEAT;
+static unsigned long boot_status;
+
+
+static void wdt_enable(u32 value)
+{
+	spin_lock(&io_lock);
+	HW_RTC_WATCHDOG_WR(value);
+	HW_RTC_CTRL_SET(BM_RTC_CTRL_WATCHDOGEN);
+	HW_RTC_PERSISTENT1_SET(BV_RTC_PERSISTENT1_GENERAL__RTC_FORCE_UPDATER);
+	spin_unlock(&io_lock);
+}
+
+static void wdt_disable(void)
+{
+	spin_lock(&io_lock);
+	HW_RTC_PERSISTENT1_CLR(BV_RTC_PERSISTENT1_GENERAL__RTC_FORCE_UPDATER);
+	HW_RTC_CTRL_CLR(BM_RTC_CTRL_WATCHDOGEN);
+	spin_unlock(&io_lock);
+}
+
+static int stmp3xxx_wdt_open(struct inode *inode, struct file *file)
+{
+	if (test_and_set_bit(WDT_IN_USE, &wdt_status))
+		return -EBUSY;
+
+	clear_bit(WDT_OK_TO_CLOSE, &wdt_status);
+
+	wdt_enable(heartbeat * WDOG_COUNTER_RATE);
+
+	return nonseekable_open(inode, file);
+}
+
+static ssize_t
+stmp3xxx_wdt_write(struct file *file, const char *data, size_t len,
+		  loff_t *ppos)
+{
+	if (len) {
+		if (!nowayout) {
+			size_t i;
+
+			clear_bit(WDT_OK_TO_CLOSE, &wdt_status);
+
+			for (i = 0; i != len; i++) {
+				char c;
+
+				if (get_user(c, data + i))
+					return -EFAULT;
+				if (c == 'V')
+					set_bit(WDT_OK_TO_CLOSE, &wdt_status);
+			}
+		}
+		wdt_enable(heartbeat * WDOG_COUNTER_RATE);
+	}
+
+	return len;
+}
+
+static struct watchdog_info ident = {
+	.options	= WDIOF_CARDRESET |
+			  WDIOF_MAGICCLOSE |
+			  WDIOF_SETTIMEOUT |
+			  WDIOF_KEEPALIVEPING,
+	.identity = "STMP37XX Watchdog",
+};
+
+static int
+stmp3xxx_wdt_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+			unsigned long arg)
+{
+	int new_heartbeat, opts;
+	int ret = -ENOTTY;
+
+	switch (cmd) {
+	case WDIOC_GETSUPPORT:
+		ret = copy_to_user((struct watchdog_info __user *)arg, &ident,
+				   sizeof(ident)) ? -EFAULT : 0;
+		break;
+
+	case WDIOC_GETSTATUS:
+		ret = put_user(0, (int __user *)arg);
+		break;
+
+	case WDIOC_GETBOOTSTATUS:
+		ret = put_user(boot_status, (int __user *)arg);
+		break;
+
+	case WDIOC_SETOPTIONS:
+		get_user(opts, (int __user *)arg);
+		if (opts & WDIOS_DISABLECARD)
+			wdt_disable();
+		else if (opts & WDIOS_ENABLECARD)
+			wdt_enable(heartbeat * WDOG_COUNTER_RATE);
+		else {
+			pr_debug("%s: unknown option 0x%x\n", __func__, opts);
+			ret = -EINVAL;
+		}
+		break;
+
+	case WDIOC_SETTIMEOUT:
+		get_user(new_heartbeat, (int __user *)arg);
+		if (new_heartbeat <= 0 || new_heartbeat > MAX_HEARTBEAT) {
+			ret = -EINVAL;
+			break;
+		}
+
+		heartbeat = new_heartbeat;
+		wdt_enable(heartbeat * WDOG_COUNTER_RATE);
+		/* Fall through */
+
+	case WDIOC_GETTIMEOUT:
+		ret = put_user(heartbeat, (int __user *)arg);
+		break;
+
+	case WDIOC_KEEPALIVE:
+		wdt_enable(heartbeat * WDOG_COUNTER_RATE);
+		ret = 0;
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int stmp3xxx_wdt_release(struct inode *inode, struct file *file)
+{
+	int ret = 0;
+
+	if (!nowayout) {
+		if (!test_bit(WDT_OK_TO_CLOSE, &wdt_status)) {
+			pr_debug("%s: Device closed unexpectdly\n", __func__);
+			ret = -EINVAL;
+		} else {
+			wdt_disable();
+			clear_bit(WDT_OK_TO_CLOSE, &wdt_status);
+		}
+	}
+	clear_bit(WDT_IN_USE, &wdt_status);
+
+	return ret;
+}
+
+static const struct file_operations stmp3xxx_wdt_fops = {
+	.owner = THIS_MODULE,
+	.llseek = no_llseek,
+	.write = stmp3xxx_wdt_write,
+	.ioctl = stmp3xxx_wdt_ioctl,
+	.open = stmp3xxx_wdt_open,
+	.release = stmp3xxx_wdt_release,
+};
+
+static struct miscdevice stmp3xxx_wdt_miscdev = {
+	.minor = WATCHDOG_MINOR,
+	.name = "watchdog",
+	.fops = &stmp3xxx_wdt_fops,
+};
+
+static int stmp3xxx_wdt_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+
+	if (heartbeat < 1 || heartbeat > MAX_HEARTBEAT)
+		heartbeat = DEFAULT_HEARTBEAT;
+
+	boot_status = HW_RTC_PERSISTENT1_RD() &
+			BV_RTC_PERSISTENT1_GENERAL__RTC_FORCE_UPDATER;
+	boot_status = !!boot_status;
+	HW_RTC_PERSISTENT1_CLR(BV_RTC_PERSISTENT1_GENERAL__RTC_FORCE_UPDATER);
+
+	dev_dbg(&pdev->dev, "STMP37XX Watchdog Timer: heartbeat %d sec\n",
+		heartbeat);
+
+	ret = misc_register(&stmp3xxx_wdt_miscdev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "cannot register misc device\n");
+	} else {
+		wdt_disable();		/* disable for now */
+		set_bit(WDT_DEVICE_INITED, &wdt_status);
+	}
+
+	return ret;
+}
+
+static int stmp3xxx_wdt_remove(struct platform_device *pdev)
+{
+	misc_deregister(&stmp3xxx_wdt_miscdev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int wdt_suspended;
+static u32 wdt_saved_time;
+
+static int stmp3xxx_wdt_suspend(struct platform_device *pdev,
+				pm_message_t state)
+{
+	if (HW_RTC_CTRL_RD() & BM_RTC_CTRL_WATCHDOGEN) {
+		wdt_suspended = 1;
+		wdt_saved_time = HW_RTC_WATCHDOG_RD();
+		wdt_disable();
+	}
+	return 0;
+}
+
+static int stmp3xxx_wdt_resume(struct platform_device *pdev)
+{
+	if (wdt_suspended) {
+		wdt_enable(wdt_saved_time);
+		wdt_suspended = 0;
+	}
+	return 0;
+}
+#else
+#define stmp3xxx_wdt_suspend	NULL
+#define stmp3xxx_wdt_resume	NULL
+#endif
+
+static struct platform_driver platform_wdt_driver = {
+	.driver = {
+		.name = "stmp3xxx_wdt",
+	},
+	.probe = stmp3xxx_wdt_probe,
+	.remove = __devexit_p(stmp3xxx_wdt_remove),
+	.suspend = stmp3xxx_wdt_suspend,
+	.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_DESCRIPTION("STMP37XX Watchdog Driver");
+MODULE_LICENSE("GPL");
+
+module_param(heartbeat, int, 0);
+MODULE_PARM_DESC(heartbeat,
+		 "Watchdog heartbeat period in seconds from 1 to "
+		 __MODULE_STRING(MAX_HEARTBEAT) ", default "
+		 __MODULE_STRING(DEFAULT_HEARTBEAT));
+
+MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);